# homogeneous-coords.tcl --
 # 
 # Part of: Useless Widgets Package
 # Contents: shows how to move a frame with homogeneous coords
 # Date: Mon Nov 15, 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 psi phi }
 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}
 
 # theta	-> rotation around the world's Z axis
 # psi	-> rotation around the world's X axis
 # phi	-> rotation around the world's Y axis
 # x     -> translation along the world's X axis
 # y     -> translation along the world's Y axis
 # z     -> translation along the world's Z axis
 set frame_coord_names		{ x y z theta psi phi }
 set frame_coord_to_radiant	{ no no no yes yes yes }
 set frame_parameters		{0 0 0 0 0 0}
 set frame_frame			{}
 set frame_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
 }
 
 option add *frame.lab_theta.text	"rot(Z)"
 option add *frame.lab_psi.text		"rot(X)"
 option add *frame.lab_phi.text		"rot(Y)"
 option add *frame.lab_x.text		"X"
 option add *frame.lab_y.text		"Y"
 option add *frame.lab_z.text		"Z"
 
 option add *lab_theta.text		"theta"
 option add *lab_psi.text		"psi"
 option add *lab_phi.text		"phi"
 
 set option_command	{ option add *$widget.$coord.$op $val }
 set angle_options	{ to -180.0 from 180.0 }
 set range_options	{ to -200.0 from 200.0 }
 set foreach_option_command { foreach {op val} $options $option_command }
 
 set widget world
 set options $angle_options
 foreach coord $world_coord_names $foreach_option_command
 set widget frame
 set options $angle_options
 foreach coord [lrange $frame_coord_names 0 2] $foreach_option_command
 set options $range_options
 foreach coord [lrange $frame_coord_names 3 5] $foreach_option_command
 
 #page
 ## ------------------------------------------------------------
 ## Widgets.
 ## ------------------------------------------------------------
 
 proc widget_build {} {
     global	world_coord_names frame_coord_names world_frame frame_frame
 
     wm geometry . +20+20
     wm title . "Playing with homogeneous coordinates"
     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 frame $frame_coord_names "Homogeneous"
 
     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 $frame_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
 	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 Frame_Frame -nooption novalue
     .c itemconfigure Canvas_Frame -fill black
     .c itemconfigure Frame -arrow last
 }
 #page
 interp alias \
     {} widget_initialise_frame_scales \
     {} widget_initialise_scales frame_frame $frame_coord_names
 interp alias \
     {} widget_initialise_world_scales \
     {} widget_initialise_scales world_frame $world_coord_names
 proc widget_initialise_scales { frameVar coordNames coordValues } {
     upvar	\#0 $frameVar frame
     foreach name $coordNames value $coordValues { $frame.$name set $value }
 }
 #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))}]
 }
 proc math_matrix_mul { a b } {
     foreach {
 	a11 a12 a13 a14  a21 a22 a23 a24  a31 a32 a33 a34  a41 a42 a43 a44
     } $a {}
     foreach {
 	b11 b12 b13 b14  b21 b22 b23 b24  b31 b32 b33 b34  b41 b42 b43 b44
     } $b {}
 
     set expr {double([set c$i$j])+double([set a$i$k])*double([set b$k$j])}
     
     for {set i 1} {$i < 5} {incr i} {
 	for {set j 1} {$j < 5} {incr j} {
 	    set c$i$j 0.0
 	    for {set k 1} {$k < 5} {incr k} { set c$i$j [expr $expr] }
 	}
     }
     
     list \
 	$c11 $c12 $c13 $c14  $c21 $c22 $c23 $c24 \
 	$c31 $c32 $c33 $c34  $c41 $c42 $c43 $c44
 }
 proc math_eval_list_of_expressions { expr_list args } {
     set matrix {}
     foreach expr $expr_list { lappend matrix [uplevel expr $expr] }
     eval { lappend matrix } $args
 }
 #page
 proc math_fundamental_rotation_around_z { theta } {
     math_sin_cos $theta t
     math_eval_list_of_expressions {
 	{$cos_t} {-$sin_t} 0 0
 	{$sin_t} {$cos_t} 0 0
     }  0 0 1 0  0 0 0 1
 }
 proc math_fundamental_rotation_around_x { psi } {
     math_sin_cos $psi p
     math_eval_list_of_expressions {
 	1 0 0 0
 	0 {$cos_p} {-$sin_p} 0
 	0 {$sin_p} {$cos_p} 0
     }  0 0 0 1
 }
 proc math_fundamental_rotation_around_y { phi } {
     math_sin_cos $phi f
     math_eval_list_of_expressions {
 	{$cos_f} 0 {$sin_f} 0
 	0 1 0 0
 	{-$sin_f} 0 {$cos_f} 0
     }  0 0 0 1
 }
 proc math_fundamental_translation { x y z } {
     list 1 0 0 $x  0 1 0 $y  0 0 1 $z  0 0 0 1
 }
 #page
 # The "transform_list" argument is a list of the form:
 #
 # {
 #   { premultiplication_boolean { a11 a12 a13 ... a44 }
 #   { premultiplication_boolean { b11 b12 b13 ... b44 }
 #   { premultiplication_boolean { c11 c12 c13 ... c44 }
 #   ...
 # }
 #
 # matrices are applied in the same order in which the appear in the
 # list.
 proc math_transformation { transform_list coords } {
     foreach transform $transform_list {
 	foreach {premultiplication matrix} $transform {
 	    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 {
 		if { $premultiplication } {
 		    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)}]
 		} else {
 		    lappend result \
 			[expr {double($x)*double($a11)+double($y)*double($a21)+
 			       double($z)*double($a31)+double($other)*double($a41)}] \
 			[expr {double($x)*double($a12)+double($y)*double($a22)+
 			       double($z)*double($a32)+double($other)*double($a42)}] \
 			[expr {double($x)*double($a13)+double($y)*double($a23)+
 			       double($z)*double($a33)+double($other)*double($a43)}] \
 			[expr {double($x)*double($a14)+double($y)*double($a24)+
 			       double($z)*double($a34)+double($other)*double($a44)}]
 		}
 	    }
 	    set coords $result
 	}
     }
     set result {}
     foreach { x y z o } $coords { lappend result $x $y }
     return $result
 }
 #page
 proc canvas_parameters_to_transformation_matrix { parameters } {
     global	canvas_coord_names
     foreach $canvas_coord_names $parameters {}
     math_sin_cos $theta t
     list [list yes [math_eval_list_of_expressions {
 	{$cos_t} {-$sin_t} 0 {$x}
 	{-$sin_t} {-$cos_t} 0 {$y}
     }  0 0 1 0  0 0 0 1]]
 }
 proc world_parameters_to_transformation_matrix { parameters } {
     global	world_coord_names
     foreach $world_coord_names $parameters {}
     foreach { axis angleName } { z theta x psi y phi } {
 	lappend result \
 	    [list yes [math_fundamental_rotation_around_$axis [set $angleName]]]
     }
     return $result
 }
 proc frame_parameters_to_trasformation_matrix { parameters } {
     global	frame_coord_names
     foreach $frame_coord_names $parameters {}
     set result [list [list yes [math_fundamental_translation $x $y $z]]]
     foreach { axis angleName } { z theta x psi y phi } {
 	lappend result \
 	    [list yes \
 		 [math_fundamental_rotation_around_$axis [set $angleName]]]
     }
     return $result
 }
 #page
 proc update_frame_matrix { index rad value } {
     upvar	\#0 frame_parameters parameters frame_matrix matrix
 
     if { $rad } { set value [math_deg2rad $value] }
     lset parameters $index $value
     set matrix [frame_parameters_to_trasformation_matrix $parameters]
     after 0 update_drawing
 }
 foreach name $frame_coord_names rad $frame_coord_to_radiant {
     interp alias {} update_frame_matrix_$name	\
 	{} update_frame_matrix [lsearch $frame_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
 }
 foreach name $world_coord_names {
     interp alias {} update_world_matrix_$name \
 	{} update_world_matrix [lsearch $world_coord_names $name]
 }
 #page
 ## ------------------------------------------------------------
 ## Draw frames.
 ## ------------------------------------------------------------
 
 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_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 }
 interp alias {} draw_frame_frame {} draw_frame Frame_Frame \
     {frame_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 }
 proc draw_frame { tag transform_names xaxis yaxis zaxis } {
     foreach T $transform_names { eval {lappend transform} [uplevel \#0 set $T] }
     .c delete $tag
     foreach varname { xaxis yaxis zaxis } {
 	set id [.c create line [math_transformation $transform [set $varname]]]
 	.c addtag $tag withtag $id
 	.c addtag Frame withtag $id
 	.c addtag $varname withtag $id
     }
 }
 proc update_drawing {} {
     draw_world_frame
     draw_frame_frame
     widget_canvas_configure_tags
 }
 #page
 ## ------------------------------------------------------------
 ## Do stuff.
 ## ------------------------------------------------------------
 
 proc main {} {
     global	exit_trigger \
 	canvas_parameters world_parameters frame_parameters \
 	canvas_matrix
     
     widget_build
     set canvas_matrix [canvas_parameters_to_transformation_matrix $canvas_parameters]
     draw_canvas_frame
     widget_initialise_world_scales $world_parameters
     widget_initialise_frame_scales $frame_parameters
 
     vwait exit_trigger
     exit
 }
 #page
 ## ------------------------------------------------------------
 ## Let's go.
 ## ------------------------------------------------------------
 
 main
 
 ### end of file
 # Local Variables:
 # mode: tcl
 # End: