2D Coordinate Transformations

http://www.ece.ualberta.ca/~wyard/wiki.tcl.tk/trans2D_animated.gif

LWS 2011-01-16: The following set of namespaced routines can be used to perform 2-dimensional coordinate transformations. Lots of others have done the same thing: see, for instance, Affine transforms on a canvas, and Code to performs 2D graphics transforms. There are even some 3D versions out there.

2015-03-24: Uploaded a newer version with some changes to the naming convention and some enhancements.

# 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"
}

The image up at the top was created from the animation generated by the following code. (The gif itself was created using xwd and the technique outlined by our friends at Creating an animated display).

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

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

LWS Had a few errors in InverseTransform: fixed 07 Jan 2012. LWS Updated version posted 24 Mar 2015. Change log not included.