# denavit-hartenberg.tcl --
 # 
 # Part of: Useless Widgets Package
 # Contents: shows how to move a frame along Denavit-Hartenberg coords
 # Date: Tue Nov  2, 2004
 # 
 # Abstract
 # 
 #	This script is just a proof.
 # 
 # Copyright (c) 2004 Marco Maggi
 # 
 # The author  hereby grant permission to use,  copy, modify, distribute,
 # and  license this  software  and its  documentation  for any  purpose,
 # provided that  existing copyright notices  are retained in  all copies
 # and that  this notice  is included verbatim  in any  distributions. No
 # written agreement, license, or royalty  fee is required for any of the
 # authorized uses.  Modifications to this software may be copyrighted by
 # their authors and need not  follow the licensing terms described here,
 # provided that the new terms are clearly indicated on the first page of
 # each file where they apply.
 # 
 # IN NO  EVENT SHALL THE AUTHOR  OR DISTRIBUTORS BE LIABLE  TO ANY PARTY
 # FOR  DIRECT, INDIRECT, SPECIAL,  INCIDENTAL, OR  CONSEQUENTIAL DAMAGES
 # ARISING OUT  OF THE  USE OF THIS  SOFTWARE, ITS DOCUMENTATION,  OR ANY
 # DERIVATIVES  THEREOF, EVEN  IF THE  AUTHOR  HAVE BEEN  ADVISED OF  THE
 # POSSIBILITY OF SUCH DAMAGE.
 # 
 # THE  AUTHOR  AND DISTRIBUTORS  SPECIFICALLY  DISCLAIM ANY  WARRANTIES,
 # INCLUDING,   BUT   NOT  LIMITED   TO,   THE   IMPLIED  WARRANTIES   OF
 # MERCHANTABILITY,    FITNESS   FOR    A    PARTICULAR   PURPOSE,    AND
 # NON-INFRINGEMENT.  THIS  SOFTWARE IS PROVIDED  ON AN "AS  IS" BASIS,
 # AND  THE  AUTHOR  AND  DISTRIBUTORS  HAVE  NO  OBLIGATION  TO  PROVIDE
 # MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS.
 #
 
 #page
 ## ------------------------------------------------------------
 ## Setup.
 ## ------------------------------------------------------------
 
 package require Tcl 8.4
 package require Tk  8.4
 
 #page
 ## ------------------------------------------------------------
 ## Global variables.
 ## ------------------------------------------------------------
 
 # theta -> counterclockwise rotation of the frame center,
 #	   origin is the horizontal axis left->right
 # x	-> horizontal coordinate of the frame center
 # y	-> vertical of the frame center
 #
 # (0,0)     --> TK x coordinate
 #        -------------------------------    -
 #  TK   |               ^ /             |   : y parameter
 #  y    |               |/ \theta param |   :
 # coord |          -----+----->         |   -
 #     | |               |               |
 #     v |               |               |
 #        -------------------------------
 #
 #       |...............| x parameter
 #
 set canvas_coord_names	{theta x y}
 set canvas_parameters	{0 250 250}
 set canvas_matrix	{1 0 0 0  0 1 0 0  0 0 1 0  0 0 0 1}
 
 # theta	-> rotation around the world's Z axis
 # psi	-> rotation around the world's X axis
 # phi	-> rotation around the world's Y axis
 set world_coord_names	{ theta phi psi }
 set world_parameters	{0 0 0}
 set world_frame		{}
 set world_matrix	{1 0 0 0  0 1 0 0  0 0 1 0  0 0 0 1}
 
 # d	-> translation along the world Z axis
 # theta -> rotation around the world Z axis (in degrees)
 # a	-> translation along the D-H X axis
 # alpha -> rotation around the D-H X axis (in degrees)
 set dh_coord_names	{ d theta a alpha }
 set dh_coord_to_radiant	{ no yes no yes }
 set dh_parameters	{0 0 0 0}
 set dh_frame		{}
 set dh_matrix		{1 0 0 0  0 1 0 0  0 0 1 0  0 0 0 1}
 
 set pi			3.14159265359
 set pi_over_180		[expr {$pi/180.0}]
 set 180_over_pi		[expr {180.0/$pi}]
 
 #page
 ## ------------------------------------------------------------
 ## TK options.
 ## ------------------------------------------------------------
 
 option add *borderWidth 1
 option add *Labelframe.borderWidth 2
 
 foreach { option value } {
     background \#f8f8f8 width 500 height 500 relief sunken borderwidth 2
 } {
     option add *c.$option $value
 }
 
 set delta 200.0
 option add *dh.d.to		-200.0
 option add *dh.d.from		200.0
 
 option add *dh.theta.to		-180.0
 option add *dh.theta.from	 180.0
 
 option add *dh.a.to		-200.0
 option add *dh.a.from		 200.0
 
 option add *dh.alpha.to		-180.0
 option add *dh.alpha.from	 180.0
 
 
 foreach coord $world_coord_names {
     option add *world.$coord.to		-180.0
     option add *world.$coord.from	180.0
 }
 
 #page
 ## ------------------------------------------------------------
 ## Widgets.
 ## ------------------------------------------------------------
 
 proc widget_build {} {
     global	world_coord_names dh_coord_names world_frame dh_frame
 
     wm geometry . +20+20
     wm title . "Playing with Denavit and Hartenberg"
     bind . <Return> {set exit_trigger 1}
     bind . <Escape> {set exit_trigger 1}
 
     set s [frame .scales]
     button $s.exit -text Exit -command {set exit_trigger 1}
     widget_build_scale_frame $s world $world_coord_names "World Frame"
     widget_build_scale_frame $s dh $dh_coord_names "Denavit-Hartenberg"
 
     set f [frame $s.explain_colors]
     foreach {ax color} { xaxis red yaxis blue zaxis green} {
 	label $f.$ax -text $ax -foreground $color
     }
     grid $f.xaxis $f.yaxis $f.zaxis -sticky news
     
     grid $s.exit
     grid $f
     grid $world_frame -sticky news
     grid $dh_frame -sticky news
     grid columnconfigure . 0 -weight 1
     grid rowconfigure . 0 -weight 1
     grid [canvas .c] $s
     focus $s.exit
 }
 proc widget_build_scale_frame { master name coord_names label } {
     set var_name [format "%s_frame" $name]
     upvar	\#0 $var_name frame
     set frame [labelframe $master.$name -text $label]
     widget_build_scales $frame $coord_names [format "update_%s_matrix_" $name]
 }
 proc widget_build_scales { master coord_names update_command_prefix } {
     set column_index 0
     foreach coord $coord_names {
 	set labw $master.lab_$coord
 	set scaw $master.$coord
 	label $labw -text $coord
 	scale $scaw -command [format "%s%s" $update_command_prefix $coord]
 	grid $labw -column $column_index -row 0 -sticky news
 	grid $scaw -column $column_index -row 1 -sticky news
 	incr column_index
     }
 }
 proc widget_canvas_configure_tags {} {
     # Axis settings are overridden by frame settings.
     foreach {tag color} {xaxis red yaxis blue zaxis green} {
 	.c itemconfigure $tag -fill $color
     }
     .c itemconfigure World_Frame -dash ,
     #.c itemconfigure Dh_Frame -nooption novalue
     .c itemconfigure Canvas_Frame -fill black
     .c itemconfigure Frame -arrow last
 }
 #page
 proc math_deg2rad { angle } {
     global	pi_over_180
     expr {double($angle)*$pi_over_180}
 }
 proc math_rad2deg { angle } {
     global	180_over_pi
     expr {double($angle)*$180_over_pi}
 }
 proc math_sin_cos { angle nickname } {
     upvar	cos_$nickname cos_t sin_$nickname sin_t
     set cos_t [expr {cos(double($angle))}]
     set sin_t [expr {sin(double($angle))}]
 }
 #page
 proc dh_parameters_to_trasformation_matrix { parameters } {
     global	dh_coord_names
     foreach $dh_coord_names $parameters {}
     math_sin_cos $theta t
     math_sin_cos $alpha a
     math_compute_transformation_matrix {
 	{$cos_t}  {-1.0*($cos_a*$sin_t)}  {$sin_a*$sin_t}        {double($a)*$cos_t}
 	{$sin_t}  {$cos_a*$cos_t}	  {-1.0*($sin_a*$cos_t)} {double($a)*$sin_t}
 	0 {$sin_a} {$cos_a} {double($d)}
     } 0 0 0 1
 }
 proc world_parameters_to_transformation_matrix { parameters } {
     global	world_coord_names
     foreach $world_coord_names $parameters {}
     math_sin_cos $theta t
     math_sin_cos $phi f
     math_sin_cos $psi p
     math_compute_transformation_matrix {
 	{$cos_t*$cos_f-($sin_f*$sin_t*$sin_p)} {-($sin_t*$cos_p)}
 	{$cos_t*$sin_f+$sin_t*$cos_f*$sin_p} 0
 
 	{$sin_t*$cos_f+$cos_t*$sin_p*$sin_f} {$cos_t*$cos_p}
 	{$sin_t*$sin_f-($cos_t*$cos_f*$sin_p)} 0
 
 	{-($cos_p*$cos_f)} {$sin_p} {$cos_p*$cos_f} 0
     } 0 0 0 1
 }
 proc canvas_parameters_to_transformation_matrix { parameters } {
     global	canvas_coord_names
     foreach $canvas_coord_names $parameters {}
     math_sin_cos $theta t
     math_compute_transformation_matrix {
 	{$cos_t} {-$sin_t} 0 {$x}
 	{-$sin_t} {-$cos_t} 0 {$y}
     }  0 0 1 0  0 0 0 1
 }
 proc math_compute_transformation_matrix { expr_list args } {
     set matrix {}
     foreach expr $expr_list { lappend matrix [uplevel expr $expr] }
     if { [llength $args] } {
 	eval { lappend matrix } $args
     } else {
 	set matrix
     }
 }
 #page
 proc initialise_canvas_matrix { parameters } {
     upvar	\#0 canvas_matrix matrix
     set matrix [canvas_parameters_to_transformation_matrix $parameters]
 }
 
 proc update_dh_matrix { index rad value } {
     upvar	\#0 dh_parameters parameters dh_matrix matrix
 
     if { $rad } { set value [math_deg2rad $value] }
     lset parameters $index $value
     set matrix [dh_parameters_to_trasformation_matrix $parameters]
     after 0 update_drawing
 }
 foreach name $dh_coord_names rad $dh_coord_to_radiant {
     interp alias {} update_dh_matrix_$name	\
 	{} update_dh_matrix [lsearch $dh_coord_names $name] $rad
 }
 proc update_world_matrix { index value } {
     upvar	\#0 world_parameters parameters world_matrix matrix
 
     lset parameters $index [math_deg2rad $value]
     set matrix [world_parameters_to_transformation_matrix $parameters]
     after 0 update_drawing
 }
 interp alias {} update_world_matrix_theta	{} update_world_matrix 0
 interp alias {} update_world_matrix_phi		{} update_world_matrix 1
 interp alias {} update_world_matrix_psi		{} update_world_matrix 2
 #page
 proc transformation { matrices coords } {
     foreach matrix $matrices {
 	foreach {
 	    a11 a12 a13 a14  a21 a22 a23 a24  a31 a32 a33 a34  a41 a42 a43 a44
 	} $matrix {}
 	set result {}
 	foreach {x y z other} $coords {
 	    lappend result \
 		[expr {double($x)*double($a11)+
 		       double($y)*double($a12)+
 		       double($z)*double($a13)+
 		       double($other)*double($a14)}] \
 		[expr {double($x)*double($a21)+
 		       double($y)*double($a22)+
 		       double($z)*double($a23)+
 		       double($other)*double($a24)}] \
 		[expr {double($x)*double($a31)+
 		       double($y)*double($a32)+
 		       double($z)*double($a33)+
 		       double($other)*double($a34)}] \
 		[expr {double($x)*double($a41)+
 		       double($y)*double($a42)+
 		       double($z)*double($a43)+
 		       double($other)*double($a44)}]
 	}
 	set coords $result
     }
     set result {}
     foreach { x y z o } $coords { lappend result $x $y }
     return $result
 }
 #page
 proc update_drawing {} {
     draw_world_frame
     draw_dh_frame
     widget_canvas_configure_tags
 }
 interp alias {} update_dh_scales {} update_scales dh_frame $dh_coord_names
 interp alias {} update_world_scales {} update_scales world_frame $world_coord_names
 proc update_scales { frameVar coordNames coordValues } {
     upvar	\#0 $frameVar frame
     foreach name $coordNames value $coordValues { $frame.$name set $value }
 }
 #page
 interp alias {} draw_canvas_frame {} draw_frame Canvas_Frame \
     {canvas_matrix} \
     { -100 0 0 1  100 0 0 1 } \
     { 0 -100 0 1  0 100 0 1 } \
     { 0 0 0 1     0 0 0 1 }
 interp alias {} draw_dh_frame {} draw_frame Dh_Frame \
     {dh_matrix world_matrix canvas_matrix} \
     { -100 0 0 1   100 0 0 1 } \
     { 0 -100 0 1   0 100 0 1 } \
     { 0 0 -100 1   0 0 100 1 }
 interp alias {} draw_world_frame {} draw_frame World_Frame \
     {world_matrix canvas_matrix} \
     { 0 0 0 1   100 0 0 1 } \
     { 0 0 0 1   0 100 0 1 } \
     { 0 0 0 1   0 0 100 1 }
 proc draw_frame { tag matrix_names xaxis yaxis zaxis } {
     set matrices [lmap {uplevel \#0 set} $matrix_names]
     .c delete $tag
     foreach varname { xaxis yaxis zaxis } {
 	set id [.c create line [transformation $matrices [set $varname]]]
 	.c addtag $tag withtag $id
 	.c addtag Frame withtag $id
 	.c addtag $varname withtag $id
     }
 }
 proc lmap { cmd lst } {
     foreach item $lst {	lappend result [eval $cmd { $item }] }
     set result
 }
 #page
 ## ------------------------------------------------------------
 ## Do stuff.
 ## ------------------------------------------------------------
 
 proc main {} {
     global	exit_trigger \
 	canvas_parameters world_parameters dh_parameters
     
     widget_build
     initialise_canvas_matrix $canvas_parameters
     draw_canvas_frame
     update_world_scales $world_parameters
     update_dh_scales $dh_parameters
 
     vwait exit_trigger
     exit
 }
 #page
 ## ------------------------------------------------------------
 ## Let's go.
 ## ------------------------------------------------------------
 
 main
 
 ### end of file
 # Local Variables:
 # mode: tcl
 # End: