path: root/src/algebra/dhmatrix.spad.pamphlet

\documentclass{article}
\usepackage{open-axiom}
\begin{document}
\title{\$SPAD/src/algebra dhmatrix.spad}
\author{Richard Paul and Timothy Daly}
\maketitle
\begin{abstract}
\end{abstract}
\eject
\tableofcontents
\eject
\mathchardef\bigp="3250
\mathchardef\bigq="3251
\mathchardef\bigslash="232C
\section{Homogeneous Transformations}
The study of robot manipulation is concerned with the relationship between
objects, and between objects and manipulators. In this chapter we will
develop the representation necessary to describe these relationships. Similar
problems of representation have already been solved in the field of computer
graphics, where the relationship between objects must also be described.
Homogeneous transformations are used in this field and in computer vision
[Duda] [Robserts63] [Roberts65]. These transformations were employed by 
Denavit to describe linkages [Denavit] and are now used to describe 
manipulators [Pieper] [Paul72] [Paul77b].

We will first establish notation for vectors and planes and then introduce
transformations on them. These transformations consist primarily of
translation and rotation. We will then show that these transformations
can also be considered as coordinate frames in which to represent
objects, including the manipulator. The inverse transformation will
then be introduced. A later section describes the general rotation
transformation representing a rotation about a vector. An algorithm is
then described to find the equivalent axis and angle of rotations
represented by any given transformation. A brief section on stretching
and scaling transforms is included together with a section on the
perspective transformation. The chapter concludes with a section on
transformation equations.

\section{Notation}

In describing the relationship between objects we will make use of
point vectors, planes, and coordinate frames. Point vectors are
denoted by lower case, bold face characters. Planes are denoted by
script characters, and coordinate frames by upper case, bold face
characters. For example:

\begin{tabular}{ll}
vectors & {\bf v}, {\bf x1}, {\bf x} \\
planes  & $\bigp$, $\bigq$ \\
coordinate frames & {\bf I}, {\bf A}, {\bf CONV}\\
\end{tabular}\\

We will use point vectors, planes, and coordinate frames as variables
which have associated values. For example, a point vector has as value
its three Cartesian coordinate components.

If we wish to describe a point in space, which we will call {\sl p},
with respect to a coordinate frame {\bf E}, we will use a vector which
we will call {\bf v}. We will write this as

$$^E{\bf v}$$

\noindent
The leading superscript describes the defining coordinate frame.

We might also wish to describe this same point, {\sl p}, with respect
to a different coordinate frame, for example {\bf H}, using a vector
{\bf w} as

$$^H{\bf w}$$

\noindent
{\bf v} and {\bf w} are two vectors which probably have different
component values and ${\bf v} \ne {\bf w}$ even though both vectors
describe the same point {\sl p}. The case might also exist of a vector
{\bf a} describing a point 3 inches above any frame

$${^{F^1}}{\bf a}\qquad {^{F^2}}{\bf a}$$

\noindent
In this case the vectors are identical but describe different
points. Frequently, the defining frame will be obvious from the text
and the superscripts will be left off. In many cases the name of the
vector will be the same as the name of the object described, for
example, the tip of a pin might be described by a vector {\bf tip}
with respect to a frame {\bf BASE} as

$${^{BASE}}{\bf tip}$$

\noindent
If it were obvious from the text that we were describing the vector
with respect to {\bf BASE} then we might simply write

$${\bf tip}$$

If we also wish to describe this point with respect to another
coordinate frame say, {\bf HAND}, then we must use another vector to
describe this relationship, for example

$${^{HAND}{\bf tv}}$$

\noindent
${^{HAND}{\bf tv}}$ and {\bf tip} both describe the same feature but
have different values. In order to refer to individual components of
coordinate frames, point vectors, or planes, we add subscripts to
indicate the particular component. For example, the vector
${^{HAND}{\bf tv}}$ has components ${^{HAND}{\bf tv}}_{\bf x}$,
${^{HAND}{\bf tv}}_{\bf y}$, ${^{HAND}{\bf tv}}_{\bf z}$.

\section{Vectors}

The homogeneous coordinate representation of objects in $n$-space
is an $(n + 1)$-space entity such that a particular perspective
projection recreates the $n$-space. This can also be viewed as the
addition of an extra coordinate to each vector, a scale factor, such
that the vector has the same meaning if each component, including the
scale factor, is multiplied by a constant.

A point vector

$${\bf v} = a{\bf i} + b{\bf j} + c{\bf k}\eqno(1.1)$$

\noindent
where {\bf i}, {\bf j}, and {\bf k} are unit vectors along the $x$,
$y$, and $z$ coordinate axes, respectively, is represented in
homogeneous coordinates as a column matrix

$${\bf v} = \left[\matrix{{\bf x}\cr
                          {\bf y}\cr
                          {\bf z}\cr
                          {\bf w}\cr}
            \right]\eqno(1.2)$$

\noindent
where

$${{\bf a} = {\bf x}/{\bf w}}$$
$${{\bf b} = {\bf y}/{\bf w}}\eqno(1.3)$$
$${{\bf c} = {\bf z}/{\bf w}}$$

\noindent
Thus the vector $3{\bf i} + 4{\bf j} + 5{\bf k}$ can be represented as
$[3,4,5,1]^{\rm T}$ or as $[6,8,10,2]^{\rm T}$ or again 
as $[-30,-40,-50,-10]^{\rm T}$,
etc. The superscript $T$ indicates the transpose of the row vector
into a column vector. The vector at the origin, the null vector, is
represented as $[0,0,0,n]^{\rm T}$ where $n$ is any non-zero scale
factor. The vector $[0,0,0,0]^{\rm T}$ is undefined. Vectors of the form
$[a,b,c,0]^{\rm T}$ represent vectors at infinity and are used to represent
directions; the addition of any other finite vector does not change
their value in any way.

We will also make use of the vector dot and cross products. Given two
vectors

$${\bf a} = a_x{\bf i} + a_y{\bf j} + a_z{\bf k}\eqno(1.4)$$
$${\bf b} = b_x{\bf i} + b_y{\bf j} + b_z{\bf k}$$

\noindent
we define the vector dot product, indicated by ``$\cdot$'' as

$${\bf a} \cdot {\bf b} = {a_x}{b_x} + {a_y}{b_y} + {a_z}{b_z}\eqno(1.5)$$

\noindent
The dot product of two vectors is a scalar. The cross product,
indicated by an ``$\times$'', is another vector perpendicular to the
plane formed by the vectors of the product and is defined by

$${\bf a} \times {\bf b} = ({a_y}{b_z} - {a_z}{b_y}){\bf i} +
                         ({a_z}{b_x} - {a_x}{b_z}){\bf j} +
                         ({a_x}{b_y} - {a_y}{b_x}){\bf k}\eqno(1.6)$$

\noindent
This definition is easily remembered as the expansion of the
determinant

$${\bf a} \times {\bf b} = 
  \left|\matrix{{\bf i}&{\bf j}&{\bf k}\cr
                 {a_x}&{a_y}&{a_z}\cr
                 {b_x}&{b_y}&{b_z}\cr}\right|\eqno(1.7)$$

\section{Planes}
A plane is represented as a row matrix

$$\bigp=[a,b,c,d]\eqno(1.8)$$

\noindent
such that if a point {\bf v} lies in a plane $\bigp$ the matrix
product

$$\bigp{\bf v} = 0\eqno(1.9)$$

\noindent
or in expanded form

$$xa + yb + zc + wd = 0\eqno(1.10)$$

\noindent
If we define a constant

$$m = +\sqrt{a^2 + b^2 + c^2}\eqno(1.11)$$

\noindent
and divide Equation 1.10 by $wm$ we obtain

$${x\over w}{a\over m} + {y\over w}{b\over m} + {z\over w}{c\over m} 
   = -{d\over m}\eqno(1.12)$$

\noindent
The left hand side of Equation 1.12 is the vector dot product of two
vectors $(x/w){\bf i} + (y/w){\bf j} + (z/w){\bf k}$ and 
$(a/m){\bf i} + (b/m){\bf j} + (c/m){\bf k}$ and represents the
directed distance of the point 
$(x/w){\bf i} + (y/w){\bf j} + (z/w){\bf k}$ along the vector\\
$(a/m){\bf i} + (b/m){\bf j} + (c/m){\bf k}$. The vector
$(a/m){\bf i} + (b/m){\bf j} + (c/m){\bf k}$ can be interpreted as the
outward pointing normal of a plane situated a distance $-d/m$ from the
origin in the direction of the normal. Thus a plane $\bigp$ parallel
to the $x$,$y$ plane, one unit along the $z$ axis, is represented as

$${\rm {\ \ \ \ \ \ \ \ \ }} \bigp = [0,0,1,-1]\eqno(1.13)$$
$${\rm {or\  as\ \ \ }} \bigp = [0,0,2,-2]\eqno(1.14)$$
$${\rm {\ \ \ \ \ or\  as\ \ \ }} \bigp = [0,0,-100,100]\eqno(1.15)$$

\noindent
A point ${\bf v} = [10,20,1,1]$ should lie in this plane

$$[0,0,-100,100]\left[\matrix{10\cr
                              20\cr
                               1\cr
                               1\cr}
                \right]
     = 0\eqno(1.16)$$

\noindent
or

$$[0,0,1,-1]\left[\matrix{ -5\cr
                          -10\cr
                          -.5\cr
                          -.5\cr}
             \right]
     = 0\eqno(1.17)$$

\noindent
The point ${\bf v} = [0,0,2,1]$ lies above the plane

$$[0,0,2,-2]\left[\matrix{0\cr
                          0\cr
                          2\cr
                          1\cr}
             \right]
     = 2\eqno(1.18)$$

and $\bigp{\bf v}$ is indeed positive, indicating that the point is
outside the plane in the direction of the outward pointing normal. A
point ${\bf v} = [0,0,0,1]$ lies below the plane

$$[0,0,1,-1]\left[\matrix{0\cr
                          0\cr
                          0\cr
                          1\cr}
             \right]
     = -1\eqno(1.19)$$

\noindent
The plane $[0,0,0,0]$ is undefined.

\section{Transformations}

\noindent
A transformation of the space {\bf H} is a 4x4 matrix and can
represent translation, rotation, stretching, and perspective
transformations. Given a point {\bf u}, its transformation {\bf v} is
represented by the matrix product

$${\bf v} = {\bf H}{\bf u}\eqno(1.20)$$

\noindent
The corresponding plane transformation $\bigp$ to $\bigq$ is

$$\bigq = \bigp{\bf H^{-1}}\eqno(1.21)$$

\noindent
as we requre that the condition

$$\bigq{\bf v} = \bigp{\bf u}\eqno(1.22)$$

\noindent
is invariant under all transformations. To verify this we substitute
from Equations 1.20 and 1.21 into the left hand side of 1.22 and we
obtain on the right hand side ${\bf H^{-1}}{\bf H}$ which is the
identity matrix {\bf I}

$$\bigp{\bf H^{-1}}{\bf H}{\bf u} = \bigp{\bf u}\eqno(1.23)$$

\section{Translation Transformation}

\noindent
The transformation {\bf H} corresponding to a translation by a vector
$a{\bf i} + b{\bf j} + c{\bf k}$ is

$${\bf H} = {\bf Trans(a,b,c)} = 
   \left[\matrix{1&0&0&a\cr
                 0&1&0&b\cr
                 0&0&1&c\cr
                 0&0&0&1\cr}
   \right]\eqno(1.24)$$

\noindent
Given a vector ${\bf u} = [x,y,z,w]^{\rm T}$ the transformed vector {\bf v}
is given by

$${\bf H} = {\bf Trans(a,b,c)} = 
   \left[\matrix{1&0&0&a\cr
                 0&1&0&b\cr
                 0&0&1&c\cr
                 0&0&0&1\cr}
   \right]
   \left[\matrix{x\cr
                 y\cr
                 z\cr
                 w\cr}
   \right]\eqno(1.25)$$

$${\bf v} = \left[\matrix{x + aw\cr
                          y + bw\cr
                          z + cw\cr
                          w\cr}
            \right]
          = \left[\matrix{x/w + a\cr
                          y/w + b\cr
                          z/w + c\cr
                          1\cr}
            \right]\eqno(1.26)$$

\noindent
The translation may also be interpreted as the addition of the two
vectors $(x/w){\bf i} + (y/w){\bf j} + (z/w){\bf k}$ and 
$a{\bf i} + b{\bf j} + c{\bf k}$.

Every element of a transformation matrix may be multiplied by a
non-zero constant without changing the transformation, in the same
manner as points and planes. Consider the vector $2{\bf i} + 3{\bf j}
+ 2{\bf k}$ translated by, or added to\\
4{\bf i} - 3{\bf j} + 7{\bf k}

$$\left[\matrix{6\cr
                0\cr
                9\cr
                1\cr}
  \right] =
  \left[\matrix{1 & 0 & 0 & 4\cr
                0 & 1 & 0 & -3\cr
                0 & 0 & 1 & 7\cr
                0 & 0 & 0 & 1\cr}
  \right]
  \left[\matrix{2\cr
                3\cr
                2\cr
                1\cr}
  \right]\eqno(1.27)$$

\noindent
If we multiply the transmation matrix elements by, say, -5, and the
vector elements by 2, we obtain

$$\left[\matrix{-60\cr
                0\cr
                -90\cr
                -10\cr}
  \right] =
  \left[\matrix{-5 & 0 &  0 & -20\cr
                0 & -5 &  0 &  15\cr
                0 &  0 & -5 & -35\cr
                0 &  0 &  0 &  -5\cr}
  \right]
  \left[\matrix{4\cr
                6\cr
                4\cr
                2\cr}
  \right]\eqno(1.28)$$

\noindent
which corresponds to the vector $[6,0,9,1]^{\rm T}$ as before. The point
$[2,3,2,1]$ lies in the plane $[1,0,0,-2]$

$$[1,0,0,-2]\left[\matrix{2\cr
                          3\cr
                          2\cr
                          1\cr}
            \right] = 0\eqno(1.29)$$

\noindent
The transformed point is, as we have already found, $[6,0,9,1]^{\rm T}$. We
will now compute the transformed plane. The inverse of the transform
is 

$$\left[\matrix{1 & 0 & 0 & -4\cr
                0 & 1 & 0 &  3\cr
                0 & 0 & 1 & -7\cr
                0 & 0 & 0 &  1\cr}\right]$$

\noindent
and the transformed plane

$$[1\ 0\ 0\ -6] = [1\ 0\ 0\ -2]\left[\matrix{1 & 0 & 0 & -4\cr
                                        0 & 1 & 0 &  3\cr
                                        0 & 0 & 1 & -7\cr
                                        0 & 0 & 0 &  1\cr}
                         \right]\eqno(1.30)$$

\noindent
Once again the transformed point lies in the transformed plane

$$[1\ 0\ 0\ -6] \left[\matrix{6\cr
                              0\cr
                              9\cr
                              1\cr}\right] = 0\eqno(1.31)$$

The general translation operation can be represented in Axiom as

<<translate>>=
    translate(x,y,z) ==
     matrix(_
      [[1,0,0,x],_
       [0,1,0,y],_
       [0,0,1,z],_
       [0,0,0,1]])
@
\section{Rotation Transformations}

\noindent
The transformations corresponding to rotations about the $x$, $y$, and
$z$ axes by an angle $\theta$ are

$${\bf Rot(x,\theta)} = 
    \left[\matrix{1 &             0 &              0 & 0\cr
                  0 & {cos\ \theta} & {-sin\ \theta} & 0\cr
                  0 & {sin\ \theta} & {cos\ \theta}  & 0\cr
                  0 &             0 &              0 & 1}\right]
   \eqno(1.32)$$

Rotations can be described in Axiom as functions that return
matrices. We can define a function for each of the rotation matrices
that correspond to the rotations about each axis. Note that the
sine and cosine functions in Axiom expect their argument to be in
radians rather than degrees. This conversion is

$$radians = {{degrees * \pi}\over{180}}$$

\noindent
The Axiom code for ${\bf Rot(x,degree)}$ is

<<rotatex>>=
    rotatex(degree) ==
     angle := degree * pi() / 180::R
     cosAngle := cos(angle)
     sinAngle := sin(angle)
     matrix(_
      [[1,     0,           0,      0], _
       [0, cosAngle, -sinAngle, 0], _
       [0, sinAngle,  cosAngle, 0], _
       [0,     0,           0,      1]])
@

$${\bf Rot(y,\theta)} = 
    \left[\matrix{{cos\ \theta}  & 0 & {sin\ \theta} & 0\cr
                               0 & 1 &             0 & 0\cr
                  {-sin\ \theta} & 0 & {cos\ \theta} & 0\cr
                               0 & 0 &             0 & 1\cr}\right]
   \eqno(1.33)$$

\noindent 
The Axiom code for ${\bf Rot(y,degree)}$ is

<<rotatey>>=
    rotatey(degree) ==
     angle := degree * pi() / 180::R
     cosAngle := cos(angle)
     sinAngle := sin(angle)
     matrix(_
      [[ cosAngle, 0, sinAngle, 0], _
       [    0,       1,     0,      0], _
       [-sinAngle, 0, cosAngle, 0], _
       [    0,       0,     0,      1]])
@

$${\bf Rot(z,\theta)} = 
    \left[\matrix{{cos\ \theta} & {-sin\ \theta} & 0 & 0\cr
                  {sin\ \theta} &  {cos\ \theta} & 0 & 0\cr
                              0 &              0 & 1 & 0\cr
                              0 &              0 & 0 & 1}\right]
   \eqno(1.34)$$

\noindent 
And the Axiom code for ${\bf Rot(z,degree)}$ is

<<rotatez>>=
    rotatez(degree) ==
     angle := degree * pi() / 180::R
     cosAngle := cos(angle)
     sinAngle := sin(angle)
     matrix(_
      [[cosAngle, -sinAngle, 0, 0], _
       [sinAngle,  cosAngle, 0, 0], _
       [   0,           0,       1, 0], _
       [   0,           0,       0, 1]])
@
\noindent
Let us interpret these rotations by means of an example. Given a point
${\bf u} = 7{\bf i} + 3{\bf j} + 2{\bf k}$ what is the effect of
rotating it $90^\circ$ about the ${\bf z}$ axis to ${\bf v}$? The
transform is obtained from Equation 1.34 with $sin\ \theta = 1$ and 
$cos\ \theta = 0$. 

$$\left[\matrix{-3\cr
                 7\cr
                 2\cr
                 1\cr}
  \right] =
  \left[\matrix{0 & -1 &  0 & 0\cr
                1 &  0 &  0 & 0\cr
                0 &  0 &  1 & 0\cr
                0 &  0 &  0 & 1\cr}
  \right]
  \left[\matrix{7\cr
                3\cr
                2\cr
                1\cr}
  \right]\eqno(1.35)$$

\noindent
Let us now rotate {\bf v} $90^\circ$ about the $y$ axis to 
{\bf w}. The transform is obtained from Equation 1.33 and we have

$$\left[\matrix{2\cr
                7\cr
                3\cr
                1\cr}
  \right] =
  \left[\matrix{ 0 &  0 &  1 & 0\cr
                 0 &  1 &  0 & 0\cr
                -1 &  0 &  0 & 0\cr
                 0 &  0 &  0 & 1\cr}
  \right]
  \left[\matrix{-3\cr
                 7\cr
                 2\cr
                 1\cr}
  \right]\eqno(1.36)$$

\noindent
If we combine these two rotations we have

$${\rm \ \ \ \ \ \ \ } {\bf v} = {\bf Rot(z,90)}{\bf u}\eqno(1.37)$$

$${\rm and\ \ \ } {\bf w} = {\bf Rot(y,90)}{\bf v}\eqno(1.38)$$

\noindent
Substituting for {\bf v} from Equation 1.37 into Equation 1.38 we
obtain 

$${\bf w} = {\bf Rot(y,90)}\ {\bf Rot(z,90)}\ {\bf u}\eqno(1.39)$$

$${\bf Rot(y,90)}\ {\bf Rot(z,90)} = 
   \left[\matrix{ 0 & 0 & 1 & 0\cr
                  0 & 1 & 0 & 0\cr
                 -1 & 0 & 0 & 0\cr
                  0 & 0 & 0 & 1}
   \right]
   \left[\matrix{0 & -1 & 0 & 0\cr
                 1 &  0 & 0 & 0\cr
                 0 &  0 & 1 & 0\cr
                 0 &  0 & 0 & 1}
   \right]\eqno(1.40)$$

$${\bf Rot(y,90)}\ {\bf Rot(z,90)} = 
   \left[\matrix{0 &  0 & 1 & 0\cr
                 1 &  0 & 0 & 0\cr
                 0 &  1 & 0 & 0\cr
                 0 &  0 & 0 & 1}
   \right]\eqno(1.41)$$

\noindent
thus

$${\bf w} = \left[\matrix{2\cr
                          7\cr
                          3\cr
                          1}\right]
          = \left[\matrix{0 & 0 & 1 & 0\cr
                          1 & 0 & 0 & 0\cr
                          0 & 1 & 0 & 0\cr
                          0 & 0 & 0 & 1}\right]
            \left[\matrix{7\cr
                          3\cr
                          2\cr
                          1}\right]\eqno(1.42)$$

\noindent
as we obtained before.

If we reverse the order of rotations and first rotate $90^\circ$ about
the $y$ axis and then $90^\circ$ about the $z$ axis, we obtain a
different position

$${\bf Rot(z,90)}{\bf Rot(y,90)} =
    \left[\matrix{0 & -1 & 0 & 0\cr
                  1 &  0 & 0 & 0\cr
                  0 &  0 & 1 & 0\cr
                  0 &  0 & 0 & 1}
    \right]
    \left[\matrix{ 0 & 0 & 1 & 0\cr
                   0 & 1 & 0 & 0\cr
                  -1 & 0 & 0 & 0\cr
                   0 & 0 & 0 & 1}
    \right]
  = \left[\matrix{ 0 & -1 & 0 & 0\cr
                   0 &  0 & 1 & 0\cr
                  -1 &  0 & 0 & 0\cr
                   0 &  0 & 0 & 1}
    \right]\eqno(1.43)$$

\noindent
and the point {\bf u} transforms into {\bf w} as

$$\left[\matrix{-3\cr
                 2\cr
                -7\cr
                 1}
  \right]
 = \left[\matrix{ 0 & -1 & 0 & 0\cr
                  0 &  0 & 1 & 0\cr
                 -1 &  0 & 0 & 0\cr
                  0 &  0 & 0 & 1}
   \right]
   \left[\matrix{7\cr
                 3\cr
                 2\cr
                 1}
   \right]\eqno(1.44)$$

\noindent
We should expect this, as matrix multiplication is noncommutative.

$${\bf A}{\bf B} \ne {\bf B}{\bf A}\eqno(1.45)$$

We will now combine the original rotation with a translation 
$4{\bf i}-3{\bf j}+7{\bf k}$. We obtain the translation from Equation
1.27 and the rotation from Equation 1.41. The matrix expression is

$${\bf Trans(4,-3,7)}{\bf Rot(y,90)}{\bf Rot(z,90)}
   = \left[\matrix{1 & 0 & 0 &  4\cr
                   0 & 1 & 0 & -3\cr
                   0 & 0 & 1 &  7\cr
                   0 & 0 & 0 &  1}
     \right]
     \left[\matrix{0 & 0 & 1 & 0\cr
                   1 & 0 & 0 & 0\cr
                   0 & 1 & 0 & 0\cr
                   0 & 0 & 0 & 1}
     \right]
   = \left[\matrix{0 & 0 & 1 &  4\cr
                   1 & 0 & 0 & -3\cr
                   0 & 1 & 0 &  7\cr
                   0 & 0 & 0 &  1}
     \right]\eqno(1.46)$$

\noindent
and our point ${\bf w} = 7{\bf i}+3{\bf j}+2{\bf k}$ transforms into
{\bf x} as

$$\left[\matrix{ 6\cr
                 4\cr
                10\cr
                 1}
  \right]
 = \left[\matrix{0 & 0 & 1 &  4\cr
                 1 & 0 & 0 & -3\cr
                 0 & 1 & 0 &  7\cr
                 0 & 0 & 0 &  1}
  \right]
  \left[\matrix{7\cr
                3\cr
                2\cr
                1}
  \right]\eqno(1.47)$$

\section{Coordinate Frames}

\noindent
We can interpret the elements of the homogeneous transformation as
four vectors describing a second coordinate frame. The vector
$[0,0,0,1]^{\rm T}$ lies at the origin of the second coordinate frame. Its
transformation corresponds to the right hand column of the
transformation matrix. Consider the transform in Equation 1.47

$$\left[\matrix{ 4\cr
                -3\cr
                 7\cr
                 1}
  \right]
 = \left[\matrix{0 & 0 & 1 &  4\cr
                 1 & 0 & 0 & -3\cr
                 0 & 1 & 0 &  7\cr
                 0 & 0 & 0 &  1}
   \right]
   \left[\matrix{0\cr
                 0\cr
                 0\cr
                 1}
   \right]\eqno(1.48)$$

\noindent
The transform of the null vector is $[4,-3,7,1]^{\rm T}$, the right
hand column. If we transform vectors corresponding to unit vectors
along the $x$, $y$, and $z$ axes, we obtain $[4,-2,7,1]^{\rm T}$,
$[4,-3,8,1]^{\rm T}$, and $[5,-3,7,1]^{\rm T}$, respectively. Those
four vectors form a coordinate frame.

The direction of these unit vectors is formed by subtracting the
vector representing the origin of this coordinate frame and extending
the vectors to infinity by reducing their scale factors to zero. The
direction of the $x$, $y$, and $z$ axes of this frame are
$[0,1,0,0]^{\rm T}$, $[0,0,1,0]^{\rm T}$, and $[1,0,0,0]^{\rm T}$,
respectively. These direction vectors correspond to the first three
columns of the transformation matrix. The transformation matrix thus
describes the three axis directions and the position of the origin of
a coordinate frame rotated and translated away from the reference
coordinate frame. When a vector is transformed, as in Equation 1.47,
the original vector can be considered as a vector described in the
coordinate frame. The transformed vector is the same vector described
with respect to the reference coordinate frame.

\section{Relative Transformations}

\noindent
The rotations and translations we have been describing have all been
made with respect to the fixed reference coordinate frame. Thus, in
the example given, 

$${\bf Trans(4,-3,7)}{\bf Rot(y,90)}{\bf Rot(z,90)}
   = \left[\matrix{0 & 0 & 1 &  4\cr
                   1 & 0 & 0 & -3\cr
                   0 & 1 & 0 &  7\cr
                   0 & 0 & 0 &  1}
     \right]\eqno(1.49)$$

\noindent
the frame is first rotated around the reference $z$ axis by
$90^\circ$, then rotated $90^\circ$ around the reference $y$ axis, and
finally translated by $4{\bf i}-3{\bf j}+7{\bf k}$. We may also
interpret the operation in the reverse order, from left to right, as
follows: the object is first translated by 
$4{\bf i}-3{\bf j}+7{\bf k}$; it is then rotated $90^\circ$ around the
current frames axes, which in this case are the same as the reference
axes; it is then rotated $90^\circ$ about the newly rotated (current)
frames axes.

In general, if we postmultiply a transform representing a frame by a
second transformation describing a rotation and/or translation, we
make that translation and/or rotation with respect to the frame axes
described by the first transformation. If we premultiply the frame
transformation by a transformation representing a translation and/or
rotation, then that translation and/or rotation is made with respect to
the base reference coordinate frame. Thus, given a frame {\bf C} and a
transformation {\bf T}, corresponding to a rotation of $90^\circ$
about the $z$ axis, and a translation of 10 units in the $x$
direction, we obtain a new position {\bf X} when the change is made in
the base coordinates ${\bf X} = {\bf T} {\bf C}$

$$\left[\matrix{0 & 0 & 1 &  0\cr
                1 & 0 & 0 & 20\cr
                0 & 1 & 0 &  0\cr
                0 & 0 & 0 &  1}
  \right]
 = \left[\matrix{0 & -1 & 0 & 10\cr
                 1 &  0 & 0 &  0\cr
                 0 &  0 & 1 &  0\cr
                 0 &  0 & 0 &  1}
  \right]
  \left[\matrix{1 & 0 &  0 & 20\cr
                0 & 0 & -1 & 10\cr
                0 & 1 &  0 &  0\cr
                0 & 0 &  0 &  1}
  \right]\eqno(1.50)$$

\noindent
and a new position {\bf Y} when the change is made relative to the
frame axes as ${\bf Y} = {\bf C} {\bf T}$

$$\left[\matrix{0 & -1 &  0 & 30\cr
                0 &  0 & -1 & 10\cr
                1 &  0 &  0 &  0\cr
                0 &  0 &  0 &  1}
  \right]
 = \left[\matrix{1 &  0 &  0 & 20\cr
                 0 &  0 & -1 & 10\cr
                 0 &  1 &  0 &  0\cr
                 0 &  0 &  0 &  1}
  \right]
  \left[\matrix{0 & -1 &  0 & 10\cr
                1 &  0 &  0 &  0\cr
                0 &  0 &  1 &  0\cr
                0 &  0 &  0 &  1}
  \right]\eqno(1.51)$$

\section{Objects}

\noindent
Transformations are used to describe the position and orientation of
objects. An object is described by six points with respect to a
coordinate frame fixed in the object.

If we rotate the object $90^\circ$ about the $z$ axis and then
$90^\circ$ about the $y$ axis, followed by a translation of four units
in the $x$ direction, we can describe the transformation as

$${\bf Trans(4,0,0)}{\bf Rot(y,90)}{\bf Rot(z,90)} =
   \left[\matrix{0 & 0 & 1 & 4\cr
                 1 & 0 & 0 & 0\cr
                 0 & 1 & 0 & 0\cr
                 0 & 0 & 0 & 1}
   \right]\eqno(1.52)$$

\noindent
The transformation matrix represents the operation of rotation and
translation on a coordinate frame originally aligned with the
reference coordinate frame. We may transform the six points of the
object as

$$\left[\matrix{4 &  4 &  6 & 6 &  4 &  4\cr
                1 & -1 & -1 & 1 &  1 & -1\cr
                0 &  0 &  0 & 0 &  4 &  4\cr
                1 &  1 &  1 & 1 &  1 &  1}
  \right]
 = \left[\matrix{0 & 0 & 1 & 4\cr
                 1 & 0 & 0 & 0\cr
                 0 & 1 & 0 & 0\cr
                 0 & 0 & 0 & 1}
  \right]
  \left[\matrix{1 & -1 & -1 & 1 & 1 & -1\cr
                0 &  0 &  0 & 0 & 4 &  4\cr
                0 &  0 &  2 & 2 & 0 &  0\cr
                1 &  1 &  1 & 1 & 1 &  1}
  \right]\eqno(1.53)$$

It can be seen that the object described bears the same fixed
relationship to its coordinate frame, whose position and orientation
are described by the transformation. Given an object described by a
reference coordinate frame, and a transformation representing the
position and orientation of the object's axes, the object can be
simply reconstructed, without the necessity of transforming all the
points, by noting the direction and orientation of key features with
respect to the describing frame's coordinate axes. By drawing the
transformed coordinate frame, the object can be related to the new
axis directions.

\section{Inverse Transformations}

\noindent
We are now in a position to develop the inverse transformation as the
transform which carries the transformed coordinate frame back to the
original frame. This is simply the description of the reference
coordinate frame with respect to the transformed frame. Suppose the
direction of the reference frame $x$ axis is $[0,0,1,0]^{\rm T}$ with
respect to the transformed frame. The $y$ and $z$ axes are 
$[1,0,0,0]^{\rm T}$ and $[0,1,0,0]^{\rm T}$, respectively. The
location of the origin is $[0,0,-4,1]^{\rm T}$ with respect to the
transformed frame and thus the inverse transformation is

$${\bf T^{-1}} = \left[\matrix{0 & 1 & 0 &  0\cr
                               0 & 0 & 1 &  0\cr
                               1 & 0 & 0 & -4\cr
                               0 & 0 & 0 &  1}
                 \right]\eqno(1.54)$$

\noindent
That this is indeed the tranform inverse is easily verifyed by
multiplying it by the transform {\bf T} to obtain the identity
transform 

$$\left[\matrix{1 & 0 & 0 & 0\cr
                0 & 1 & 0 & 0\cr
                0 & 0 & 1 & 0\cr
                0 & 0 & 0 & 1}
  \right]
 = \left[\matrix{0 & 1 & 0 &  0\cr
                 0 & 0 & 1 &  0\cr
                 1 & 0 & 0 & -4\cr
                 0 & 0 & 0 &  1}
   \right]
   \left[\matrix{0 & 0 & 1 & 4\cr
                 1 & 0 & 0 & 0\cr
                 0 & 1 & 0 & 0\cr
                 0 & 0 & 0 & 1}
   \right]\eqno(1.55)$$ 

\noindent
In general, given a transform with elements

$${\bf T} = \left[\matrix{n_x & o_x & a_x & p_x\cr
                          n_y & o_y & a_y & p_y\cr
                          n_z & o_z & a_z & p_z\cr
                            0 &   0 &   0 &   1}
            \right]\eqno(1.56)$$

\noindent
then the inverse is

$${\bf T^{-1}} = \left[\matrix{n_x & n_y & n_z & -{\bf p} \cdot {\bf n}\cr
                               o_x & o_y & o_z & -{\bf p} \cdot {\bf o}\cr
                               a_x & a_y & a_z & -{\bf p} \cdot {\bf a}\cr
                                 0 &   0 &   0 &   1}
                 \right]\eqno(1.57)$$

\noindent
where {\bf p}, {\bf n}, {\bf o}, and {\bf a} are the four column
vectors and ``$\cdot$'' represents the vector dot product. This result
is easily verified by postmultiplying Equation 1.56 by Equation 1.57.

\section{General Rotation Transformation}

\noindent
We state the rotation transformations for rotations about the $x$,
$y$, and $z$ axes (Equations 1.32, 1.33 and 1.34). These
transformations have a simple geometric interpretation. For example,
in the case of a rotation about the $z$ axis, the column representing
the $z$ axis will remain constant, while the column elements
representing the $x$ and $y$ axes will vary.

\noindent
We will now develop the transformation matrix representing a rotation
around an arbitrary vector {\bf k} located at the origin. In order to
do this we will imagine that {\bf k} is the $z$ axis unit vector of a
coordinate frame {\bf C} 

$${\bf C} = \left[\matrix{n_x & o_x & a_x & p_x\cr
                          n_y & o_y & a_y & p_y\cr
                          n_z & o_z & a_z & p_z\cr
                            0 &   0 &   0 &   1}
            \right]\eqno(1.58)$$

$${\bf k} = a_x{\bf i} + a_y{\bf j} + a_z{\bf k}\eqno(1.59)$$

\noindent
Rotating around the vector {\bf k} is then equivalent to rotating
around the $z$ axis of the frame {\bf C}.

$${\bf Rot(k,\theta)} = {\bf Rot(^C{\bf z},\theta)}\eqno(1.60)$$

\noindent
If we are given a frame {\bf T} described with respect to the
reference coordinate frame, we can find a frame {\bf X} which
describes the same frame with respect to frame {\bf C} as

$${\bf T} = {\bf C} {\bf X}\eqno(1.61)$$

\noindent
where {\bf X} describes the position of {\bf T} with respect to frame
{\bf C}. Solving for {\bf X} we obtain

$${\bf X} = {\bf C^{-1}} {\bf T}\eqno(1.62)$$

\noindent
Rotation {\bf T} around {\bf k} is equivalent to rotating {\bf X}
around the $z$ axis of frame {\bf C}

$${\bf Rot(k,\theta)} {\bf T}
    = {\bf C} {\bf Rot(z,\theta)} {\bf X}\eqno(1.63)$$

$${\bf Rot(k,\theta)} {\bf T}
    = {\bf C} {\bf Rot(z,\theta)} {\bf C^{-1}} {\bf T}.\eqno(1.64)$$

\noindent
Thus

$${\bf Rot(k,\theta)} 
    = {\bf C} {\bf Rot(z,\theta)} {\bf C^{-1}}\eqno(1.65)$$

\noindent
However, we have only {\bf k}, the $z$ axis of the frame {\bf C}. By
expanding equation 1.65 we will discover that 
${\bf C} {\bf Rot(z,\theta)} {\bf C^{-1}}$ is a function of {\bf k}
only. 

Multiplying ${\bf Rot(z,\theta)}$ on the right by ${\bf C^{-1}}$ we
obtain 

$${\bf Rot(z,\theta)} {\bf C^{-1}}
   = \left[\matrix{cos \theta & -sin \theta & 0 & 0\cr
                   sin \theta &  cos \theta & 0 & 0\cr
                            0 &           0 & 1 & 0\cr
                            0 &           0 & 0 & 1}
     \right]
     \left[\matrix{n_x & n_y & n_z & 0\cr
                   o_x & o_x & o_z & 0\cr
                   a_x & a_y & a_z & 0\cr
                     0 &   0 &   0 & 1}
      \right]$$

$$ = \left[\matrix{n_x cos \theta - o_x sin \theta & 
                   n_y cos \theta - o_y sin \theta &
                   n_z cos \theta - o_z sin \theta & 0\cr
                   n_x sin \theta + o_x cos \theta &
                   n_y sin \theta + o_y cos \theta &
                   n_z sin \theta + o_z cos \theta & 0\cr
                   a_x & a_y & a_z & 0\cr
                     0 &   0 &   0 & 1}
     \right]\eqno(1.66)$$

\noindent
premultiplying by

$${\bf C} = \left[\matrix{n_x & o_x & a_x & 0\cr
                          n_y & o_y & a_y & 0\cr
                          n_z & o_z & a_z & 0\cr
                            0 &   0 &   0 & 1}
            \right]\eqno(1.67)$$

\noindent
we obtain ${\bf C} {\bf Rot(z,\theta)} {\bf C^{-1}}$

$$\left[\matrix{
n_x n_x cos \theta - n_x o_x sin \theta + n_x o_x sin \theta + o_x o_x
cos \theta + a_x a_x\cr
n_y n_x cos \theta - n_y o_x sin \theta + n_x o_y sin \theta + o_x o_y
cos \theta + a_y a_x\cr
n_z n_x cos \theta - n_z o_x sin \theta + n_x o_z sin \theta + o_x o_z
cos \theta + a_z a_x\cr
0}
\right.$$

$$\matrix{
n_x n_y cos \theta - n_x o_y sin \theta + n_y o_x sin \theta + o_y o_x
cos \theta + a_x a_y\cr
n_y n_y cos \theta - n_y o_y sin \theta + n_y o_y sin \theta + o_y o_y
cos \theta + a_y a_y\cr
n_z n_y cos \theta - n_z o_y sin \theta + n_y o_z sin \theta + o_y o_z
cos \theta + a_z a_y\cr
0}\eqno(1.68)$$

$$\left.\matrix{
n_x n_z cos \theta - n_x o_z sin \theta + n_z o_x sin \theta + o_z o_x
cos \theta + a_x a_x & 0\cr
n_y n_z cos \theta - n_y o_z sin \theta + n_z o_y sin \theta + o_z o_y
cos \theta + a_y a_z & 0\cr
n_z n_z cos \theta - n_z o_z sin \theta + n_z o_z sin \theta + o_z o_z
cos \theta + a_z a_z & 0\cr
0 & 1}
\right]$$

\noindent
Simplifying, using the following relationships:\\
the dot product of any row or column of {\bf C} with any other row or
column is zero, as the vectors are orthogonal;\\
the dot product of any row or column of {\bf C} with itself is {\bf 1}
as the vectors are of unit magnitude;\\
the $z$ unit vector is the vector cross product of the $x$ and $y$
vectors or
$${\bf a} = {\bf n} \times {\bf o}\eqno(1.69)$$

\noindent
which has components

$$a_x = n_y o_z - n_z o_y$$
$$a_y = n_z o_x - n_x o_z$$
$$a_z = n_x o_y - n_y o_x$$

\noindent
the versine, abbreviated ${\bf vers \ \theta}$, is defined as 
${\bf vers \ \theta} = (1 - cos \ \theta)$,
${k_x = a_x}$, ${k_y = a_y}$ and ${k_z = a_z}$. 
We obtain ${\bf Rot(k,\theta)} =$
$$\left[\matrix{
k_x k_x vers \theta + cos \theta & 
k_y k_x vers \theta - k_z sin \theta &
k_z k_x vers \theta + k_y sin \theta & 
0\cr
k_x k_y vers \theta + k_z sin \theta &
k_y k_y vers \theta + cos \theta &
k_z k_y vers \theta - k_x sin \theta & 
0\cr
k_x k_z vers \theta - k_y sin \theta &
k_y k_z vers \theta + k_x sin \theta &
k_z k_z vers \theta + cos \theta &
0\cr
0 & 0 & 0 & 1}
\right]\eqno(1.70)$$

\noindent
This is an important result and should be thoroughly understood before
proceeding further.

From this general rotation transformation we can obtain each of the
elementary rotation transforms. For example ${\bf Rot(x,\theta)}$ is 
${\bf Rot(k,\theta)}$ where ${k_x = 1}$, ${k_y = 0}$, and 
${k_z = 0}$. Substituting these values of {\bf k} into Equation 1.70
we obtain

$${\bf Rot(x,\theta)} = 
\left[\matrix{1 & 0 & 0 & 0\cr
              0 & cos \theta & -sin \theta & 0\cr
              0 & sin \theta &  cos \theta & 0\cr
              0 &          0 &           0 & 1}
\right]\eqno(1.71)$$

\noindent
as before.

\section{Equivalent Angle and Axis of Rotation}

\noindent
Given any arbitrary rotational transformation, we can use Equation
1.70 to obtain an axis about which an equivalent rotation $\theta$ is
made as follows. Given a rotational transformation {\bf R}

$${\bf R} = 
\left[\matrix{n_x & o_x & a_x & 0\cr
              n_y & o_y & a_y & 0\cr
              n_z & o_z & a_z & 0\cr
                0 &   0 &   0 & 1}
\right]\eqno(1.72)$$

\noindent
we may equate {\bf R} to {\bf Rot(k,$\theta$)}

$$\left[\matrix{n_x & o_x & a_x & 0\cr
                n_y & o_y & a_y & 0\cr
                n_z & o_z & a_z & 0\cr
                  0 &   0 &   0 & 1}
  \right] = $$
$$\left[\matrix{
k_x k_x vers \theta + cos \theta & 
k_y k_x vers \theta - k_z sin \theta &
k_z k_x vers \theta + k_y sin \theta & 
0\cr
k_x k_y vers \theta + k_z sin \theta &
k_y k_y vers \theta + cos \theta &
k_z k_y vers \theta - k_x sin \theta & 
0\cr
k_x k_z vers \theta - k_y sin \theta &
k_y k_z vers \theta + k_x sin \theta &
k_z k_z vers \theta + cos \theta &
0\cr
0 & 0 & 0 & 1}
\right]\eqno(1.73)$$

\noindent
Summing the diagonal terms of Equation 1.73 we obtain

$$n_x+o_y+a_z+1=
k_x^2 vers \theta + cos \theta + 
k_y^2 vers \theta + cos \theta +
k_z^2 vers \theta + cos \theta + 1\eqno(1.74)$$

$$\left.\matrix{ n_x+o_y+a_z & = & 
                   (k_x^2+k_y^2+k_z^2)vers \theta + 3 cos \theta\cr
                             & = & 1 + 2 cos \theta}
  \right.\eqno(1.75)$$

\noindent
and the cosine of the angle of rotation is

$$cos \theta = {1\over 2}(n_x+o_y+a_z-1)\eqno(1.76)$$

\noindent
Differencing pairs of off-diagonal terms in Equation 1.73 we obtain 

$$o_z - a_y = 2 k_x sin \theta\eqno(1.77)$$
$$a_x - n_z = 2 k_y sin \theta\eqno(1.78)$$
$$n_y - o_x = 2 k_z sin \theta\eqno(1.79)$$

\noindent
Squaring and adding Equations 1.77-1.79 we obtain an expression for
$sin \theta$

$$(o_z - a_y)^2 + (a_x - n_z)^2 + (n_y - o_x)^2
    = 4 sin^2 \theta\eqno(1.80)$$ 

\noindent
and the sine of the angle of rotation is

$$sin \ \theta = 
  \pm {1\over 2} \sqrt{(o_z - a_y)^2 + (a_x - n_z)^2 + (n_y - o_x)^2}
  \eqno(1.81)$$

\noindent
We may define the rotation to be positive about the vector {\bf k}
such that $0 \leq \theta \leq 180^\circ$. In this case the $+$ sign
is appropriate in Equation 1.81 and thus the angle of rotation
$\theta$ is uniquely defined as

$$tan \ \theta =
 {\sqrt{(o_z - a_y)^2 + (a_x - n_z)^2 + (n_y - o_x)^2}
  \over
  {(n_x + o_y + a_z -1)}}\eqno(1.82)$$

\noindent
The components of {\bf k} may be obtained from Equations 1.77-1.79 as

$$k_x = {{o_z - a_y}\over{2 sin \theta}}\eqno(1.83)$$
$$k_y = {{a_x - n_z}\over{2 sin \theta}}\eqno(1.84)$$
$$k_z = {{n_y - o_x}\over{2 sin \theta}}\eqno(1.85)$$

When the angle of rotation is very small, the axis of rotation is
physically not well defined due to the small magnitude of both
numerator and denominator in Equations 1.83-1.85. If the resulting
angle is small, the vector {\bf k} should be renormalized to ensure
that $\left|{\bf k}\right| = 1$. When the angle of rotation approaches
$180^\circ$ the vector {\bf k} is once again poorly defined by
Equation 1.83-1.85 as the magnitude of the sine is again
decreasing. The axis of rotation is, however, physically well defined
in this case. When $\theta < 150^\circ$, the denominator of
Equations 1.83-1.85 is less than 1. As the angle increases to
$180^\circ$ the rapidly decreasing magnitude of both numerator and
denominator leads to considerable inaccuracies in the determination of
{\bf k}. At $\theta = 180^\circ$, Equations 1.83-1.85 are of the form
$0/0$, yielding no information at all about a physically well defined
vector {\bf k}. If the angle of rotation is greater than $90^\circ$,
then we must follow a different approach in determining {\bf
k}. Equating the diagonal elements of Equation 1.73 we obtain

$$k_x^2 vers \theta + cos \theta = n_x\eqno(1.86)$$
$$k_y^2 vers \theta + cos \theta = o_y\eqno(1.87)$$
$$k_z^2 vers \theta + cos \theta = a_z\eqno(1.88)$$

Substituting for $cos \theta$ and $vers \theta$ from Equation 1.76 and
solving for the elements of {\bf k} we obtain further

$$k_x = 
 \pm \sqrt{{{n_x - cos \theta}\over{1 - cos \theta}}}\eqno(1.89)$$
$$k_y = 
 \pm \sqrt{{{o_y - cos \theta}\over{1 - cos \theta}}}\eqno(1.90)$$
$$k_z = 
 \pm \sqrt{{{a_z - cos \theta}\over{1 - cos \theta}}}\eqno(1.91)$$

\noindent
The largest component of {\bf k} defined by Equations 1.89-1.91
corresponds to the most positive component of $n_x$, $o_y$, and
$a_z$. For this largest element, the sign of the radical can be
obtained from Equations 1.77-1.79. As the sine of the angle of
rotation $\theta$ must be positive, then the sign of the component of
{\bf k} defined by Equations 1.77-1.79 must be the same as the sign of
the left hand side of these equations. Thus we may combine Equations
1.89-1.91 with the information contained in Equations 1.77-1.79 as
follows 

$$k_x = sgn(o_z-a_y)\sqrt{{{(n_x-cos \theta)}
                          \over
                          {1-cos \theta}}}\eqno(1.92)$$ 

$$k_y = sgn(a_x-n_z)\sqrt{{{(o_y-cos \theta)}
                          \over
                          {1-cos \theta}}}\eqno(1.93)$$

$$k_z = sgn(n_y-o_x)\sqrt{{{(a_z-cos \theta)}
                          \over
                          {1-cos \theta}}}\eqno(1.94)$$

\noindent
where $sgn(e) = +1$ if $e \ge 0$ and $sgn(e) = -1$ if $e \le 0$.

Only the largest element of {\bf k} is determined from Equations
1.92-1.94, corresponding to the most positive element of $n_x$, $o_y$,
and $a_z$. The remaining elements are more accurately determined by
the following equations formed by summing pairs of off-diagonal
elements of Equation 1.73

$$n_y + o_x = 2 k_x k_y vers \theta\eqno(1.95)$$
$$o_z + a_y = 2 k_y k_z vers \theta\eqno(1.96)$$
$$n_z + a_x = 2 k_z k_x vers \theta\eqno(1.97)$$

\noindent
If $k_x$ is largest then

$$k_y = {{n_y + o_x}\over{2 k_x vers \theta}} 
  {\rm \ \ \ \ \ from \ Equation \ 1.95}\eqno(1.98)$$

$$k_z = {{a_x + n_z}\over{2 k_x vers \theta}} 
  {\rm \ \ \ \ \ from \ Equation \ 1.97}\eqno(1.99)$$

\noindent
If $k_y$ is largest then

$$k_x = {{n_y + o_x}\over{2 k_y vers \theta}}
  {\rm \ \ \ \ \ from \ Equation \ 1.95}\eqno(1.100)$$

$$k_z = {{o_z + a_y}\over{2 k_y vers \theta}}
  {\rm \ \ \ \ \ from \ Equation \ 1.96}\eqno(1.101)$$

\noindent
If $k_z$ is largest then

$$k_x = {{a_x + n_z}\over{2 k_z vers \theta}}
  {\rm \ \ \ \ \ from \ Equation \ 1.97}\eqno(1.102)$$

$$k_y = {{o_z + a_y}\over{2 k_z vers \theta}}
  {\rm \ \ \ \ \ from \ Equation \ 1.96}\eqno(1.103)$$

\section{Example 1.1}

\noindent
Determine the equivalent axis and angle of rotation for the matrix
given in Equations 1.41

$${\bf Rot(y,90)}{\bf Rot(z,90)} 
  = \left[\matrix{0 & 0 & 1 & 0\cr
                  1 & 0 & 0 & 0\cr
                  0 & 1 & 0 & 0\cr
                  0 & 0 & 0 & 1}
    \right]\eqno(1.104)$$

\noindent
We first determine ${\bf cos \ \theta}$ from Equation 1.76

$$cos \theta = {{1}\over{2}}(0 + 0 + 0 - 1) 
             = -{{1}\over{2}}\eqno(1.105)$$

\noindent
and $sin \ \theta$ from Equation 1.81

$$sin \theta = {{1}\over{2}}\sqrt{(1-0)^2+(1-0)^2+(1-0)^2}
             = {{\sqrt3}\over{2}}\eqno(1.106)$$

\noindent
Thus

$$\theta = tan^{-1}\left({{\sqrt3}\over{2}}
           \raise15pt\hbox{$\bigslash$}
           {{-1}\over{2}}\right)
         = 120^\circ\eqno(1.107)$$

\noindent
As $\theta > 90$, we determine the largest component of {\bf k}
corresponding to the largest element on the diagonal. As all diagonal
elements are equal in this example we may pick any one. We will pick
$k_x$ given by Equation 1.92

$$k_x = +\sqrt{(0 + {{1}\over{2}})
               \raise15pt\hbox{$\bigslash$}
               (1 + {{1}\over{2}})}
      = {{1}\over{\sqrt{3}}}\eqno(1.108)$$

\noindent
As we have determined $k_x$ we may now determine $k_y$ and $k_z$ from
Equations 1.98 and 1.99, respectively

$$k_y = {{1+0}\over{\sqrt{3}}} = {{1}\over{\sqrt{3}}}\eqno(1.109)$$

$$k_z = {{1+0}\over{\sqrt{3}}} = {{1}\over{\sqrt{3}}}\eqno(1.110)$$

\noindent
In summary, then

$${\bf Rot(y,90)}{\bf Rot(z,90)} = {\bf Rot(k,120)}\eqno(1.111)$$

\noindent
where

$${\bf k} = {{1}\over{\sqrt{3}}} {\bf i}
          + {{1}\over{\sqrt{3}}} {\bf j}
          + {{1}\over{\sqrt{3}}} {\bf k}\eqno(1.112)$$

Any combination of rotations is always equivalent to a single rotation
about some axis {\bf k} by an angle $\theta$, an important result
that we will make use of later.

\section{Stretching and Scaling}

A transform {\bf T} 

$${\bf T} = \left[\matrix{a & 0 & 0 & 0\cr
                          0 & b & 0 & 0\cr
                          0 & 0 & c & 0\cr
                          0 & 0 & 0 & 1}
            \right]\eqno(1.113)$$

\noindent
will stretch objects uniformly along the $x$ axis by a factor $a$,
along the $y$ axis by a factor $b$, and along the $z$ axis by a factor
$c$. Consider any point on an object $x{\bf i}+y{\bf j}+z{\bf k}$; its
tranform is

$$\left[\matrix{ax\cr
                by\cr
                cz\cr
                 1}
  \right]
  = \left[\matrix{a & 0 & 0 & 0\cr
                  0 & b & 0 & 0\cr
                  0 & 0 & c & 0\cr
                  0 & 0 & 0 & 1}
    \right]
    \left[\matrix{x\cr
                  y\cr
                  z\cr
                  1}
    \right]\eqno(1.114)$$

\noindent
indicating stretching as stated. Thus a cube could be transformed into
a rectangular parallelepiped by such a transform.

The Axiom code to perform this scale change is:

<<scale>>=
    scale(scalex, scaley, scalez) ==
     matrix(_
      [[scalex, 0      ,0     , 0], _
       [0     , scaley ,0     , 0], _
       [0     , 0,      scalez, 0], _
       [0     , 0,      0     , 1]])
@
\noindent
The transform {\bf S} where

$${\bf S} = \left[\matrix{s & 0 & 0 & 0\cr
                          0 & s & 0 & 0\cr
                          0 & 0 & s & 0\cr
                          0 & 0 & 0 & 1}
            \right]\eqno(1.115)$$

\noindent
will scale any object by the factor $s$.

\section{Perspective Transformations}

\noindent
Consider the image formed of an object by a simple lens.

The axis of the lens is along the $y$ axis for convenience. An object
point $x$,$y$,$z$ is imaged at $x^\prime$,$y^\prime$,$z^\prime$ if the
lens has a focal length $f$ ($f$ is considered positive). $y^\prime$
represents the image distance and varies with object distance $y$. If
we plot points on a plane perpendicular to the $y$ axis located at
$y^\prime$ (the film plane in a camera), then a perspective image is
formed. 

We will first obtain values of $x^\prime$, $y^\prime$, and $z^\prime$,
then introduce a perspective transformation and show that the same
values are obtained.

Based on the fact that a ray passing through the center of the lens is
undeviated we may write

$${\rm \ \ \ \ \ }{{z}\over{y}} = {{z^\prime}\over{y^\prime}}\eqno(1.116)$$

$${\rm and\ } {{x}\over{y}} = {{x^\prime}\over{y^\prime}}\eqno(1.117)$$

Based on the additional fact that a ray parallel to the lens axis
passes through the focal point $f$, we may write

$${\rm \ \ \ \ \ }{{z}\over{f}} 
    = {{z^\prime}\over{y^\prime + f}}\eqno(1.118)$$

$${\rm and\ } {{x}\over{f}} 
    = {{x^\prime}\over{y^\prime + f}}\eqno(1.119)$$

\noindent
Notice that $x^\prime$, $y^\prime$, and $z^\prime$ are negative and
that $f$ is positive. Eliminating $y^\prime$ between Equations 1.116
and 1.118 we obtain

$${{z}\over{f}} 
    = {{z^\prime}\over{({{z^\prime y}\over{z}} + f)}}\eqno(1.120)$$

\noindent
and solving for $z^\prime$ we obtain the result

$$z^\prime = {{z}\over{(1 - {{y}\over{f}})}}\eqno(1.121)$$

\noindent
Working with Equations 1.117 and 1.119 we can similarly obtain

$$x^\prime = {{x}\over{(1 - {{y}\over{f}})}}\eqno(1.122)$$

\noindent
In order to obtain the image distance $y^\prime$ we rewrite Equations
1.116 and 1.118 as

$${{z}\over{z^\prime}} = {{y}\over{y^\prime}}\eqno(1.123)$$

\noindent
and

$${{z}\over{z^\prime}} = {{f}\over{y^\prime + f}}\eqno(1.124)$$

\noindent
thus

$${{y}\over{y^\prime}} = {{f}\over{y^\prime + f}}\eqno(1.125)$$

\noindent
and solving for $y^\prime$ we obtain the result

$$y^\prime = {{y}\over{(1-{{y}\over{f}})}}\eqno(1.126)$$

The homogeneous transformation {\bf P} which produces the same result
is 

$${\bf P} = \left[\matrix{1 & 0 & 0 & 0\cr
                          0 & 1 & 0 & 0\cr
                          0 & 0 & 1 & 0\cr
                          0 & -{{1}\over{f}} & 0 & 1}
            \right]\eqno(1.127)$$

\noindent
as any point $x{\bf i}+y{\bf j}+z{\bf k}$ transforms as

$$\left[\matrix{x\cr
                y\cr
                z\cr
                {1 - {{{y}\over{f}}}}}
   \right]
 = \left[\matrix{1 & 0 & 0 & 0\cr
                 0 & 1 & 0 & 0\cr
                 0 & 0 & 1 & 0\cr
                 0 & -{{1}\over{f}} & 0 & 1}
    \right]
    \left[\matrix{x\cr
                  y\cr
                  z\cr
                  1}
    \right]\eqno(1.128)$$

\noindent
The image point $x^\prime$, $y^\prime$,, $z^\prime$, obtained by
dividing through by the weight factor $(1 - {{y}\over{f}})$, is

$${{x}\over{(1 - {{y}\over{f}})}}{\bf i} +
  {{y}\over{(1 - {{y}\over{f}})}}{\bf j} +
  {{z}\over{(1 - {{y}\over{f}})}}{\bf k} \eqno(1.129)$$

\noindent
This is the same result that we obtained above.

A transform similar to {\bf P} but with $-{{1}\over{f}}$ at the bottom
of the first column produces a perspective transformation along the
$x$ axis. If the $-{{1}\over{f}}$ term is in the third column then the
projection is along the $z$ axis.

\section{Transform Equations}

\noindent We will frequently be required to deal with transform
equations in which a coordinate frame is described in two or more
ways.  A manipulator is positioned with respect to base coordinates by
a transform {\bf Z}. The end of the manipulator is described by a
transform $^Z{\bf T}_6$, and the end effector is described by
$^{T_6}{\bf E}$. An object is positioned with respect to base
coordinates by a transform {\bf B}, and finally the manipulator end
effector is positioned with respect to the object by $^B{\bf G}$. We
have two descriptions of the position of the end effector, one with
respect to the object and one with respect to the manipulator. As both
positions are the same, we may equate the two descriptions

$${\bf Z}{^Z{\bf T}_6}{^{T_6}{\bf E}}
   = {\bf B}{^B{\bf G}}\eqno(1.130)$$

If we wish to solve Equation 1.130 for the manipulator transform 
${\bf T}_6$ we must premultiply Equation 1.130 by ${\bf Z}^{-1}$ and
postmultiply by ${\bf E}^{-1}$ to obtain

$${\bf T}_6 
 = {{\bf Z}^{-1}} {\bf B} {\bf G} {{\bf E}^{-1}}\eqno(1.131)$$

\noindent
As a further example, consider that the position of the object {\bf B}
is unknown, but that the manipulator is moved such that the end
effector is positioned over the object correctly. We may then solve
for {\bf B} from Equation 1.130 by postmultiplying by ${\bf G}^{-1}$.

$${\bf B} = {\bf Z}{{\bf T}_6}{\bf E}{{\bf G}^{-1}}\eqno(1.133)$$

\section{Summary}

\noindent
Homogeneous transformations may be readily used to describe the
positions and orientations of coordinate frames in space. If a
coordinate frame is embedded in an object then the position and
orientation of the object are also readily described.

The description of object A in terms of object B by means of a
homogeneous transformation may be inverted to obtain the description
of object B in terms of object A. This is not a property of a simple
vector description of the relative displacement of one object with
respect to another.

Transformations may be interpreted as a product of rotation and
translation transformations. If they are intrepreted from left to
right, then the rotations and translations are in terms of the
currently defined coordinate frame. If they are interpreted from right
to left, then the rotations and translations are described with
respect to the reference coordinate frame.

Homogeneous transformations describe coordinate frames in terms of
rectangular components, which are the sines and cosines of
angles. This description may be related to rotations in which case the
description is in terms of a vector and angle of rotation.

\section{Denavit-Hartenberg Matrices}
<<domain DHMATRIX DenavitHartenbergMatrix>>=
--Copyright The Numerical Algorithms Group Limited 1991.

++ 4x4 Matrices for coordinate transformations
++ Author: Timothy Daly
++ Date Created: June 26, 1991
++ Date Last Updated: 26 June 1991
++ Description:
++   This package contains functions to create 4x4 matrices
++   useful for rotating and transforming coordinate systems.
++   These matrices are useful for graphics and robotics.
++   (Reference: Robot Manipulators Richard Paul MIT Press 1981) 
 
 
)abbrev domain DHMATRIX DenavitHartenbergMatrix
 
--% DHMatrix

DenavitHartenbergMatrix(R): Exports == Implementation where
  ++ A Denavit-Hartenberg Matrix is a 4x4 Matrix of the form:
  ++  \spad{nx ox ax px}
  ++  \spad{ny oy ay py}
  ++  \spad{nz oz az pz}
  ++   \spad{0  0  0  1}
  ++ (n, o, and a are the direction cosines)
  R : Join(Field,  TranscendentalFunctionCategory)

-- for the implementation of dhmatrix
  minrow ==> 1
  mincolumn ==> 1
--
  nx ==> x(1,1)::R
  ny ==> x(2,1)::R
  nz ==> x(3,1)::R
  ox ==> x(1,2)::R
  oy ==> x(2,2)::R
  oz ==> x(3,2)::R
  ax ==> x(1,3)::R
  ay ==> x(2,3)::R
  az ==> x(3,3)::R
  px ==> x(1,4)::R
  py ==> x(2,4)::R
  pz ==> x(3,4)::R
  row ==> Vector(R)
  col ==> Vector(R)
  radians ==> pi()/180

  Exports ==> MatrixCategory(R,row,col) with
   *: (%, Point R) -> Point R
     ++ t*p applies the dhmatrix t to point p
   identity: () -> %
     ++ identity() create the identity dhmatrix
   rotatex: R -> %
     ++ rotatex(r) returns a dhmatrix for rotation about axis X for r degrees
   rotatey: R -> %
     ++ rotatey(r) returns a dhmatrix for rotation about axis Y for r degrees
   rotatez: R -> %
     ++ rotatez(r) returns a dhmatrix for rotation about axis Z for r degrees
   scale: (R,R,R) -> %
     ++ scale(sx,sy,sz) returns a dhmatrix for scaling in the X, Y and Z
     ++ directions
   translate: (R,R,R) -> %
     ++ translate(X,Y,Z) returns a dhmatrix for translation by X, Y, and Z
 
  Implementation ==> Matrix(R) add

    identity() == matrix([[1,0,0,0],[0,1,0,0],[0,0,1,0],[0,0,0,1]])

--    inverse(x) == (inverse(x pretend (Matrix R))$Matrix(R)) pretend %
--    dhinverse(x) == matrix( _
--        [[nx,ny,nz,-(px*nx+py*ny+pz*nz)],_
--         [ox,oy,oz,-(px*ox+py*oy+pz*oz)],_
--         [ax,ay,az,-(px*ax+py*ay+pz*az)],_
--         [ 0, 0, 0, 1]])

    d:% * p: Point(R) ==
       v := p pretend Vector R
       v := concat(v, 1$R)
       v := d * v
       point ([v.1, v.2, v.3]$List(R))

<<rotatex>>

<<rotatey>>

<<rotatez>>

<<scale>>

<<translate>>
 
@
\section{License}
<<license>>=
--Portions Copyright (c) Richard Paul
--Copyright (c) 1991-2002, The Numerical ALgorithms Group Ltd.
--All rights reserved.
--
--Redistribution and use in source and binary forms, with or without
--modification, are permitted provided that the following conditions are
--met:
--
--    - Redistributions of source code must retain the above copyright
--      notice, this list of conditions and the following disclaimer.
--
--    - Redistributions in binary form must reproduce the above copyright
--      notice, this list of conditions and the following disclaimer in
--      the documentation and/or other materials provided with the
--      distribution.
--
--    - Neither the name of The Numerical ALgorithms Group Ltd. nor the
--      names of its contributors may be used to endorse or promote products
--      derived from this software without specific prior written permission.
--
--THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
--IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
--TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
--PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER
--OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
--EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
--PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
--PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
--LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
--NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
--SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
@
<<*>>=
<<license>>

<<domain DHMATRIX DenavitHartenbergMatrix>>
@ 
\eject
\begin{thebibliography}{99}
\bibitem{1} Paul, Richard,
{\sl Robot Manipulators},
MIT Press, Cambridge, Mass.,
(1981)
\end{thebibliography}
\end{document}