Version 1 of 2D Coordinate Transformations

Updated 2011-01-17 00:30:53 by LWS

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

LWS 16 Jan 2011 - 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.

I am in the process of cleaning up some code to help manage the transformations in a clean fashion... one day I will post that on this wiki.

package require Tcl 8.5

######################################################################
# A set of routines for doing 2D transformations.
######################################################################
namespace eval Trans2D {
    # 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.
    #   Make[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.

    ##################################################################
    proc MakeEmpty { } {
        # 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 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 $Tl does not exist."
        }
        
        set T1 { {1 0 0} {0 1 0} {0 0 1} }
    }

    ######################################################################
    proc MakeRotation { 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 [MakeEmpty]
        SetRotation T $angle_rad
        return $T
    }
    
    ##################################################################
    proc 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 $Tl 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 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 MakePosition { 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 [MakeEmpty]
        SetPosition T $x $y
        return $T
    }
    
    ##################################################################
    proc 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 $Tl 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 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 MakeTransformation { 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 [MakeEmpty]
        SetTransformation T $angle_rad $x $y
        return $T
    }
    
    ##################################################################
    proc 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 $Tl 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 MakeRightHanded { x y } {
        # This routine returns a transformation matrix that is
        # relative to the coordinate specified specified by x,y.  This
        # differs from a a normal translation matrix in that the he
        # Y-axis is flipped, thereby making the coordinate system
        # right-handed (the Z axis is out of the screen) when dealing
        # with the canvas coordinates.
        
        # The reason for doing this is that physics is more intuitive
        # when this coordinate system is used.  This originally had
        # the canvas handle passed in as a parameter, but that was
        # removed in a bid to make this set of routines unrelated to
        # Tk.
        
        return [list [list 1 0 $x] [list 0 -1 $y] {0 0 1}]
    }
    
    ##################################################################
    proc InverseTransform { T } {
        # Returns the inverse of transform matrix, T.  The inverse 
        # allows coordinate transformation in the opposite direction.
        # i.e. Given aTb, determines bTa.
        
        lassign [lindex $T 0] r00 r01 x
        lassign [lindex $T 0] r10 r10 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]
    }
    
    ######################################################################
    proc CompoundTransform { 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 [CompoundTransform {*}$newTs]
        } else {
            # Recursion anchor.
            return [lindex $args 0]
        }
    }
    
    ##################################################################
    proc 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 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 DumpTransform { T stream } {
        # 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 created by the following code. (The gif itself was created using xwd and the technique outlined by our friends at Creating an animated display).

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 it's own 
# rotation.

# Set origin with right-hand coordinates in the centre of the
# canvas.  (cTw = transformation from world coordinates to canvas
# coordinates.
set cTw [::Trans2D::MakeRightHanded 75 75]

# 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::MakeEmpty]
# Initialize the triangle: it is offset from the box.
set bTt [::Trans2D::MakePosition 25 25]
# And the line is offset from the triangle.
set tTl [::Trans2D::MakePosition 20 0]

set angle 0.0
for {set angle 0.0} {$angle < [expr {2 * 3.14159}]} {set angle [expr {$angle + 0.1}]} {
    # 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::CompoundTransform $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.
    ::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::CompoundTransform $cTb $bTt]
    set newcoords [::Trans2D::ApplyTransform $cTt $trianglecoords]
    .c coords $triangle {*}$newcoords
    
    # Update the line's rotation.  (Rotation 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::CompoundTransform $cTt $tTl]
    set newcoords [::Trans2D::ApplyTransform $cTl $linecoords]
    .c coords $line {*}$newcoords
             
    
    # A little delay and an update.
    update
    after 100
}