# A set of routines that make use of homogeneous transformation
# matrices to perform transformations in 2D space.
# The transformation matrix is 
#  T = [R2x2 | Q2x1]
#      [01x2 | 1   ]3x3
# where
#   R2x2 is the 2x2 orthogonal rotation matrix.
#   Q2x1 is the 2x1 vector that specifies translation.
#   01x2 is a row vector of 2 zeros
#   1 is constant.  
# The last two (the padding) are used to align the values to allow
# matrix operations while allowing all information to be specified
# within the matrix.  This method is used in manipulator robotics.
#
# These transformations are used in
# [A  ]   A  [B  ]
# [ P ] =  T [ P ]
# [ 1 ]   B  [ 1 ]
#
# (ASCII limitations aren't helping this documentation...)  Which
# means the point in reference frame A (on the left) is determined
# by applying the Transformation matrix (established for
# translation from B -> A) to a (padded) point described in terms
# of reference frame B.
# When using these routines, I usually write transformation
# matrices like this:
#   aTb -> transformation from a coordinate frame, b, to a.
#
# Too much to document here, but other sources may help.
# Note that aTd = aTb bTc cTd (where matrix multiplication
# is involved).  
#
# Although padding of the Transformation matrix is not really
# necessary, keeping the matrices square should allow use of these
# arrays with other linear algebra packages.
#
# I struggled a bit with how parameters could be intuitively and
# efficiently passed and created.  I have settled on this
# convention:
#   Set[Command]  procedures use upvar and perform their
#                 operations in-place.  The transformation
#                 matrix variable is passed by name (like
#                 lassign) and must exist.
#   [Command]     procedures return new transformation
#                 matrices.
# This convention has come about to allow the differentiation
# between calling procedures by name or by value, and to
# reduce run-time debugging.
#
# Lastly, rather than using column vectors for the positions
# (represented outside the transformation matrices) these routines
# make use of (unpadded) row vectors.  This proves more convenient
# to use.

package require Tcl 8.5
package provide Trans2D 0.2

namespace eval Trans2D {
    # Could export everything, but this provides a short list
    # of the procedure names.
    namespace export Empty \
        SetEmpty \
        Rotation \
        SetRotation \
        GetRotation \
        Position \
        SetPosition \
        GetPosition \
        Scale \
        SetScale \
        Reflection \
        SetReflection \
        Shear \
        SetShear \
        Transformation \
        SetTransformation \
        ApplyRotation \
        ApplyTransform \
        CompoundTransforms \
        InverseTransform \
        DumpTransform
}

##################################################################
proc Trans2D::Empty { } {
    # Returns an empty transformation matrix with a position 
    # vector of 0,0 and no rotation.
    
    return { {1 0 0} {0 1 0} {0 0 1} }
}

##################################################################
proc Trans2D::SetEmpty { T } {
    # Clears the named transformation matrix, establishing a
    # position vector of 0,0 and no rotation.
    
    upvar $T Tl
    if {![info exists Tl]} {
        error "Variable $T does not exist."
    }
    
    set T1 { {1 0 0} {0 1 0} {0 0 1} }
}

######################################################################
proc Trans2D::Rotation { angle_rad } {
    # Returns a new transformation matrix with the rotation
    # portion set according to angle_rad, and a position vector of
    # 0,0.
    
    set T [Empty]
    SetRotation T $angle_rad
    return $T
}

##################################################################
proc Trans2D::SetRotation { T angle_rad } {
    # Sets a rotation matrix within a Transform matrix, specified
    # by name in T.  Leaves the position vector unaffected. Since
    # this package is for 2D, these rotations are always about the
    # Z axis!
    
    upvar $T Tl
    if {![info exists Tl]} {
        error "Variable $T does not exist"
    }
    
    lassign [lindex $Tl 0] r00 r01 x
    lassign [lindex $Tl 1] r10 r11 y
    
    set s [expr {sin($angle_rad)}]
    set c [expr {cos($angle_rad)}]
    
    set Tl [list [list $c [expr {-$s}] $x] [list $s $c $y] {0 0 1}]
}

##################################################################
proc Trans2D::GetRotation { T } {
    # Returns the angle (in radians) from a transformation matrix.
    # This is pretty straight-forward since we are dealing only 
    # with 2D: one value with a simple calculation.
    
    lassign [lindex $T 1] s c notneeded
    return [expr {atan2($s,$c)}]
}

######################################################################
proc Trans2D::Position { x {y {}} } {
    # Returns a transformation matrix with the position vector set
    # according to the parameters.  If parameter y is not passed,
    # x is assumed to be a two-item list, a row vector, containing
    # both x and y.
    
    set T [Empty]
    SetPosition T $x $y
    return $T
}

##################################################################
proc Trans2D::SetPosition { T x {y {}} } {
    # Sets a position (displacement) within a Transform matrix,
    # specified by name in T.  If parameter y is not passed, x is
    # assumed to be a two-item list, a row vector, containing both
    # x and y.
    
    upvar $T Tl
    if {![info exists Tl]} {
        error "Variable $T does not exist"
    }
    
    if {$y eq {}} {
        if {[llength $x] != 2} {
            error "Single position parameter needs to be a two-item list."
        }
        set y [lindex $x 1]
        set x [lindex $x 0]
    }
    
    lassign [lindex $Tl 0] r00 r01 xold
    lassign [lindex $Tl 1] r10 r11 yold
    
    set Tl [list [list $r00 $r01 $x] [list $r10 $r11 $y] {0 0 1}]
}

##################################################################
proc Trans2D::GetPosition { T } {
    # Returns the position (displacement) vector associated with a
    # transformation matrix.  Returns this vector in row form.
    
    return [list [lindex [lindex $T 0] 2] [lindex [lindex $T 1] 2]]
}

######################################################################
proc Trans2D::Scale { scalefactor {scalefactory {}} } {
    # Returns a(n orthogonal) transformation matrix with a scaling factor set
    # according to scalefactor.  If scalefactory is specified,
    # then the x and y scaling differs.  The offset encoded in
    # the transformation matrix is set to 0.
    
    set T [Empty]
    SetScale T $scalefactor $scalefactory
    return $T
}

######################################################################
proc Trans2D::SetScale { T scalefactor {scalefactory {}} } {
    # Establishes the scale factor for a(n orthogonal) transformation matrix.
    # If scalefactory is specified, scaling in x and y differ
    # according to the parameters, otherwise scaling is 
    # performed identically in the x and y directions.
    # Note that the offset in T, if it exists, is left intact.
    
    upvar $T Tl
    if {![info exists Tl]} {
        error "Variable $T does not exist."
    }
    
    if {$scalefactory eq {}} {
        set scalefactory $scalefactor
        set scalefactorx $scalefactor
    } else {
        set scalefactorx $scalefactor
    }

    lassign [lindex $Tl 0] r00 r01 x
    lassign [lindex $Tl 1] r10 r11 y
    
    set Tl [list [list $scalefactorx 0 $x] [list 0 $scalefactory $y] {0 0 1}]
}

######################################################################
proc Trans2D::Reflection { direction } {
    # Returns a(n orthogonal) transformation matrix with the rotation
    # portion set to reflect in the direction specified.  The direction
    # may be "x", "y", or "both".  The offset encoded in the 
    # transformation matrix is set to 0.
    
    set T [Empty]
    SetReflection T $direction
    return $T
}

######################################################################
proc Trans2D::SetReflection { T direction } {
    # Establishes the rotational portion of transformation, T, such
    # that reflection in direction is implemented.  The direction
    # may be "x", "y", or "both".  The offset encoded in the 
    # transformation matrix is left intact.  To achieve reflection
    # about an aribtrary angle, compound the reflection 
    # transformation with a rotation.
    
    upvar $T Tl
    if {![info exists Tl]} {
        error "Variable $T does not exist."
    }

    lassign [lindex $Tl 0] r00 r01 x
    lassign [lindex $Tl 1] r10 r11 y
    
    switch -exact -- $direction {
        x {
            set Tl [list [list -1 0 $x] [list 0 1 $y] {0 0 1}]
        }
        y {
            set Tl [list [list 1 0 $x] [list 0 -1 $y] {0 0 1}]
        }
        both {
            set Tl [list [list -1 0 $x] [list 0 -1 $y] {0 0 1}]
        }
        default {
            error "Illegal direction $direction specified: must be x, y, or both."
        }
    }
}

######################################################################
proc Trans2D::Shear { direction factor } {
    # Returns a transformaton to perform shear mapping parallel
    # to the axis specified in the direction, "x" or "y", with
    # a constant factor specified.
    # The offset encoded in the transformation matrix is set to 0.
    
    set T [Empty]
    SetShear T $direction $factor
    return $T
}

######################################################################
proc Trans2D::SetShear { T direction factor } {
    # Establsihes the rotational portion of a transformaton matrix
    # to perform a shear mapping parallel to the axis specified 
    # in the direction, "x" or "y", with a constant factor specified.
    # The offset encoded in the transformation matrix is left
    # untouched.

    upvar $T Tl
    if {![info exists Tl]} {
        error "Variable $T does not exist."
    }

    lassign [lindex $Tl 0] r00 r01 x
    lassign [lindex $Tl 1] r10 r11 y
    
    switch -exact -- $direction {
        x {
            set Tl [list [list 1 $factor $x] [list 0 1 $y] {0 0 1}]
        }
        y {
            set Tl [list [list 1 0 $x] [list $factor 1 $y] {0 0 1}]
        }
        default {
            error "Illegal direction $direction specified: must be x or y."
        }
    }
}

######################################################################
proc Trans2D::Transformation { angle_rad x {y {}} } {
    # Returns a transformation matrix with the rotation portion
    # set according to angle_rad, and the position set according
    # to x, y.
    
    set T [Empty]
    SetTransformation T $angle_rad $x $y
    return $T
}

##################################################################
proc Trans2D::SetTransformation { T angle_rad x {y {}} } {
    # Establishes a transformation matrix with the rotation
    # portion set according to angle_rad and the position portion
    # set according to x, y.  Transformation matrix, T, passed by
    # name, must exist. As with SetPosition, the position
    # (displacement) arguments may be specified as two separate
    # items, or one item that contains a two-element list.
    
    # Although this could call SetRotation and SetPosition
    # in succession, this is simply re-done to avoid the overhead
    # of having to burst and reconstruct the matrix twice.
    
    upvar $T Tl
    if {![info exists Tl]} {
        error "Variable $T does not exist"
    }
    
    if {$y eq {}} {
        if {[llength $x] != 2} {
            error "Single position parameter needs to be a two-item list."
        }
        set y [lindex $x 1]
        set x [lindex $x 0]
    }
    
    set s [expr {sin($angle_rad)}]
    set c [expr {cos($angle_rad)}]
    
    set Tl [list [list $c [expr {-$s}] $x] [list $s  $c $y] {0 0 1}]
}

##################################################################
proc Trans2D::ApplyRotation { T coordlist } {
    # Applies only the rotation described in transformation
    # matrix, T, to a position (or n positions) specified as a
    # flat row vector, coordlist of length 2n.  Returns the
    # transformed (rotated) position as a flat, list of n
    # coordinates.
    
    if {[expr {[llength $coordlist] % 2}]} {
        error "coordlist needs to be specified in pairs: non-even length detected"
    }
    
    lassign [lindex $T 0] r00 r01 xf
    lassign [lindex $T 1] r10 r11 yf
    
    set returnvec {}
    for {set i 0} {$i < [llength $coordlist]} {incr i 2} {
        lassign [lrange $coordlist $i [expr {$i + 1}]] x y        
        lappend returnvec [expr {$r00 * $x + $r01 * $y}]
        lappend returnvec [expr {$r10 * $x + $r11 * $y}]
    }
    return $returnvec
}

##################################################################
proc Trans2D::ApplyTransform { T coordlist } {
    # Applies the transformation to positions specified as
    # sequential pairs in args in a flat manner: i.e. if the
    # transformation is to be applied to two positions, then
    # coordlist is x1 y1 x2 y2 The return value is then a list of
    # four values forming the two returned coordinates with
    # respect to the reference frame defined by T.  The return
    # value is a flat list.  This can be called with a single
    # point passed as a row vector in coordlist.
    
    # The flat nature of the arguments is intended to make calling
    # this routine compatible with the canvas coords function.
    # Application to multiple vectors is intended to reduce
    # overhead of bursting the Transformation matrix.
    
    if {[expr {[llength $coordlist] % 2}]} {
        error "coordlist needs to be specified in pairs: length [llength $coordlist] detected"
    }
    
    lassign [lindex $T 0] r00 r01 xf
    lassign [lindex $T 1] r10 r11 yf
    
    set returnvec {}
    for {set i 0} {$i < [llength $coordlist]} {incr i 2} {
        lassign [lrange $coordlist $i [expr {$i + 1}]] x y
        lappend returnvec [expr {$r00 * $x + $r01 * $y + $xf}]
        lappend returnvec [expr {$r10 * $x + $r11 * $y + $yf}]
    }
    return $returnvec
}

######################################################################
proc Trans2D::CompoundTransforms { args } {
    # Recursively multiplies all transformation matrices, passed
    # by value, from right-hand to left to create a single
    # transformation matrix that translates to frame 0 from frame
    # n, where n is the number of arguments. 
    # i.e. T0 = 0T1 1T2... (n-2)T(n-1) 
    # args is the transformation matrices (by value). Returns
    # a new transformation matrix.
    
    if {[llength $args] > 1} {
        # Get the last two items: matrix multiplication starts
        # at the right.
        set Tn [lindex $args end]
        set Tn1 [lindex $args end-1]
        
        # Brute force multiplication, taking into account the
        # zeros and ones in the homogeneous transformation
        lassign [lindex $Tn 0] rn00 rn01 xn
        lassign [lindex $Tn 1] rn10 rn11 yn
        
        lassign [lindex $Tn1 0] rm00 rm01 xm
        lassign [lindex $Tn1 1] rm10 rm11 ym
        
        set row0 [list [expr {$rn00 * $rm00 + $rn10 * $rm01}] \
                      [expr {$rn01 * $rm00 + $rn11 * $rm01}] \
                      [expr {$xn * $rm00 + $yn * $rm01 + $xm}]]
        
        set row1 [list [expr {$rn00 * $rm10 + $rn10 * $rm11}] \
                      [expr {$rn01 * $rm10 + $rn11 * $rm11}]\
                      [expr {$xn * $rm10 + $yn * $rm11 + $ym}]]
        
        set row2 {0 0 1}
        
        # Replace the last two arguments with this one.
        set newTs [lrange $args 0 end-2]
        lappend newTs [list $row0 $row1 $row2]
        
        # Call recursively.
        return [CompoundTransforms {*}$newTs]
    } else {
        # Recursion anchor.
        return [lindex $args 0]
    }
}

##################################################################
proc Trans2D::InverseTransform { T } {
    # Returns the inverse of transform matrix, T.  The inverse 
    # allows coordinate transformation in the opposite direction.
    # i.e. Given aTb, determines bTa.
    
    # This version, now unused, was sufficient prior to the
    # introduction of scaling as part of the possible transforms.
    # It is left here for good measure.
    #         lassign [lindex $T 0] r00 r01 x
    #         lassign [lindex $T 1] r10 r11 y
    
    #         # The rotation portion is simply transposed.
    #         # The position needs to be multiplied by the negative
    #         # of this new rotational portion.
    
    #         set row0 [list $r00 $r10 [expr -$r00*$x - $r10*$y]]
    #         set row1 [list $r01 $r11 [expr -$r01*$x - $r11*$y]]
    #         set row2 {0 0 1}
    
    #         return [list $row0 $row1 $row2]
    
    # Now a more traditional 3x3 matrix inverse is calculated.
    # Using the notation from the wikipedia page on the topic:
    #  http://en.wikipedia.org/wiki/Invertible_matrix#Inversion_of_3.C3.973_matrices
    
    lassign [lindex $T 0] a b c 
    lassign [lindex $T 1] d e f
    lassign [lindex $T 2] g h k
    
    set detA [expr {$e * $k - $f * $h}]
    set detB [expr {$f * $g - $d * $k}]
    set detC [expr {$d * $h - $e * $g}]
    set detD [expr {$c * $h - $b * $k}]
    set detE [expr {$a * $k - $c * $g}]
    set detF [expr {$g * $b - $a * $h}]
    set detG [expr {$b * $f - $c * $e}]
    set detH [expr {$c * $d - $a * $f}]
    set detK [expr {$a * $e - $b * $d}]
    
    # Typecast to double to have appropriate division operations.
    set DETA [expr {double($a * $detA - $b * $detB + $c * $detC)}]
    
    set A [expr {$detA/$DETA}]
    set B [expr {$detB/$DETA}]
    set C [expr {$detC/$DETA}]
    set D [expr {$detD/$DETA}]
    set E [expr {$detE/$DETA}]
    set F [expr {$detF/$DETA}]
    set G [expr {$detG/$DETA}]
    set H [expr {$detH/$DETA}]
    set K [expr {$detK/$DETA}]
    
    return [list [list $A $D $G] [list $B $E $H] [list $C $F $K]]
}

######################################################################
proc Trans2D::DumpTransform { stream T } {
    # Prints the transform to the stream specified.  Useful for
    # debugging.
    foreach row $T {
        puts $stream "| " nonewline
        foreach col $row {
            puts $stream "[format %9.2f $col] " nonewline
        }
        puts $stream "|  "
    }
    set angle_rad [GetRotation $T]
    set pos [GetPosition $T]
    lassign $pos x y
    set distance [format %9.2f [expr sqrt($x * $x + $y * $y)]]
    set angle_deg [format %9.2f [expr $angle_rad * 180.0 / 3.14159]]
    puts $stream "  = $distance < $angle_deg deg"
}

#!/bin/sh
# the next line restarts using tclsh \
    exec wish "$0" ${1+"$@"}

package require Trans2D 

canvas .c -width 150 -height 150 -background black
pack .c

# Put a few line items on the canvas to play with.  Note that the
# starting coordinates will be updated in the loop, below.
# The zero point, though, defines the origin of the object.
# - a centred green box
set box [.c create line -10 -10 10 -10 10 10 -10 10 -10 -10 -width 2 -fill green]
# - a purple triangle centred on one edge
set triangle [.c create line 0 10 5 0 -5 0 0 10 -width 2 -fill purple]
# - an orange line, centred on one end.
set line [.c create line 0 0 10 5 -width 1 -fill orange]

# For the animation, we'll translate the box across the screen and
# rotate it slowly.  The triangle will be attached to the box, with a
# fixed translation and another rotation.  The line will be attached
# to the spinning triangle, again with a fixed offset and its own 
# rotation.

# Set origin with right-hand coordinates in the centre of the
# canvas.  (cTw = transformation from world coordinates to canvas
# coordinates).
set Offset  [::Trans2D::Position 75 75]
set Reflect [::Trans2D::Reflection y]
set cTw [::Trans2D::CompoundTransforms $Offset $Reflect]

# Make a pristine copy of the items' coordinates.
set boxcoords [.c coords $box]
set trianglecoords [.c coords $triangle]
set linecoords [.c coords $line]

# Initialize the box -> world coordinate transformation.
set wTb [::Trans2D::Empty]
# Initialize the triangle: it is offset from the box.
set bTt [::Trans2D::Position 25 25]
# And the line is offset from the triangle.
set tTl [::Trans2D::Position 20 0]

set angle 0.0

proc action {} {
    variable box
    variable triangle
    variable line
    variable cTw
    variable boxcoords
    variable trianglecoords
    variable linecoords
    variable wTb
    variable bTt
    variable tTl
    variable angle
    if {$angle >= [expr {2 * 3.14159}]} {
        return
    }
    
    # Set the new box position by translating according to the current angle.
    ::Trans2D::SetTransformation wTb $angle [expr {20.0 * sin($angle)}] [expr {20.0 * cos($angle)}]
    
    # Update the box's position and rotation.
    # a) determine new transformation matrix
    set cTb [::Trans2D::CompoundTransforms $cTw $wTb]
    # b) get the new coords from the original.
    set newcoords [::Trans2D::ApplyTransform $cTb $boxcoords]
    # c) set the new coords.
    .c coords $box {*}$newcoords

    # Update the triangle's position and rotation.
    ::Trans2D::SetRotation bTt [expr {-$angle * 2.0}]
    # No need to recalculate the cTb portion of the transformation:
    # just use it again!  cTt = cTw wTb bTt
    set cTt [::Trans2D::CompoundTransforms $cTb $bTt]
    set newcoords [::Trans2D::ApplyTransform $cTt $trianglecoords]
    .c coords $triangle {*}$newcoords
    
    # Update the line's position and rotation.  
    # Rotation here, as with the triangle, is just based on the
    # box's spin, for no reason other than I don't have to create a
    # new variable.
    ::Trans2D::SetRotation tTl [expr {$angle * 3.0}]
    set cTl [::Trans2D::CompoundTransforms $cTt $tTl]
    set newcoords [::Trans2D::ApplyTransform $cTl $linecoords]
    .c coords $line {*}$newcoords
             
    
    set angle [expr {$angle + 0.1}]
    # A little delay and an update.
    after 100 action

}

bind . <Map> action