Elementary rotation and transform definitions

Definition of elementary rotation matrices

rX[a_] := {{1, 0, 0}, {0, Cos[a], -Sin[a]}, {0, Sin[a], Cos[a]}} ; rY[a_] := {{Cos[a], 0, Sin[ ... 1, 0}, {-Sin[a], 0, Cos[a]}} ; rZ[a_] := {{Cos[a], -Sin[a], 0}, {Sin[a], Cos[a], 0}, {0, 0, 1}} ;

Convert a 3x3 rotation matrix and a 3x1 position vector to a 4x4 homogeneous operator

RAndPToT[r_, p_] := Join[Transpose[Join[Transpose[r], {p}]], {{0, 0, 0, 1}}] ;

Convert a 3x3 rotation matrix to 4x4 homogeneous rotational operator

RToT[r_] := RAndPToT[r, {0, 0, 0}] ;

Elementary homogeneous rotational operators

RX[a_] := RToT[rX[a]] ; RY[a_] := RToT[rY[a]] ; RZ[a_] := RToT[rZ[a]] ;

Angle-axis rotation [R_k(θ)]

RK[θ_, k_] := Module[{kx, ky, kz, v, c, s},  {kx, ky, kz} = k/Sqrt[k . k] ... y kz v + kx s, kz^2 v + c}} /. {v-> (1 - Cos[θ]), c->Cos[θ], s->Sin[θ]}] ;

Convert a 3x1 position vector to a 4x4 homogeneous translational operator

PToT[p_] := RAndPToT[IdentityMatrix[3], p] ;

Elementary homogeneous translational operators

DX[x_] := PToT[{x, 0, 0}] ; DY[y_] := PToT[{0, y, 0}] ; DZ[z_] := PToT[{0, 0, z}] ;

Extract 3x3 rotation matrix from 4x4 homogeneous transform

RFromT[t_] := TakeMatrix[t, {1, 1}, {3, 3}] ;

Extract 3x1 position vector from 4x4 homogeneous transform

PFromT[t_] := TakeMatrix[t, {1, 4}, {3, 4}] // Flatten ;

Compute inverse of a 4x4 homogeneous transform

InverseT[t_] := Module[{r, p}, r = RFromT[t] ; p = PFromT[t] ; RAndPToT[Transpose[r], -Transpose[r] . p] ] ;

Created by Mathematica  (September 7, 2003)