File: Matrix3dImpl.mesa
Last Edited by: Bier, May 29, 1987 5:47:59 pm PDT
Author: Eric Allan Bier on May 27, 1982
Contents: Implementation of a 4 by 4 Homogenous Transform package for 3d graphics
DIRECTORY
Matrix3d, RealFns, Real, SV2d, SV3d, SVVector3d;
Matrix3dImpl:
CEDAR PROGRAM
IMPORTS RealFns, Real, SVVector3d
EXPORTS Matrix3d =
BEGIN
There are two standard notations for Homogenous Transforms. The one popular with the computer graphics community (as seen in Newman and Sproul) shows the point (x,y,z) as the row vector [x,y,z,1]. The robotics community (See Paul) prefers (x,y,z) as the column vector [x,y,z,1]T. I will use the later because I am more familiar with it. Hence, translation will be shown as:
| x1 | | 1 0 0 tx | | x0 |
| y1 | = | 0 1 0 ty | | y0 |
| z1 | | 0 0 1 tz | | z0 |
| 1 |
Matrix4by4: TYPE = SV3d.Matrix4by4;
Matrix3by3: TYPE = SV2d.Matrix3by3;
Plane: TYPE = REF PlaneObj;
PlaneObj: TYPE = SV3d.PlaneObj;
Point3d: TYPE = SV3d.Point3d;
Point2d: TYPE = SV2d.Point2d;
Vector3d: TYPE = SVVector3d.Vector3d;
almostZero: REAL ← 1.0e-4;
Identity:
PUBLIC
PROC []
RETURNS [identityMat: Matrix4by4] = {
FOR i: NAT IN [1..3] DO -- in the first three columns, the elements are copied
FOR j: NAT IN [1..4] DO
IF i = j THEN identityMat[i][j] ← 1.0
ELSE identityMat[i][j] ← 0.0;
ENDLOOP;
ENDLOOP;
};
Update:
PUBLIC PROC [point: Point3d, mat: Matrix4by4]
RETURNS [newPoint: Point3d] = {
Since I always get this wrong, let me diagram it:
| x' | | (1,1) (1,2) (1,3) (1,4) | | x |
| y' | = | (2,1) (2,2) (2,3) (2,4) | | y |
| z' | | (3,1) (3,2) (3,3) (3,4) | | z |
| 1 | | (4,1) (4,2) (4,3) (4,4) | | 1 |
FOR i:
NAT
IN [1..3]
DO
newPoint[i] ← mat[i][1]*point[1] + mat[i][2]*point[2] + mat[i][3]*point[3] + mat[i][4];
ENDLOOP;
};
Because of the wonders of mathematics, if a shape is transformed by a matrix transform A, then its vector should be transformed by A transpose inverse. If A involves only even scaling, rotation and translation then using A (minus translational components) will produce a vector with the proper direction (though not scaled as it would be with A transpose inverse). However, if A has differential scaling, then A transpose inverse must be used. If you already have A inverse handy, then don't bother to recompute it. Use UpdateVectorWithInverse. Otherwise use UpdateVectorComputeInverse. If there is no differential scaling in A and the magnitude of the result is not important, use UpdateVectorEvenScaling.
UpdateDisplacement:
PUBLIC
PROC [mat: Matrix4by4, vec: Vector3d]
RETURNS [newVec: Vector3d] = {
Simple multiply mat by vec seen as a column vector, ignoring translation components.
FOR i:
NAT
IN [1..3]
DO
newVec[i] ← mat[i][1]*vec[1] + mat[i][2]*vec[2] + mat[i][3]*vec[3];
ENDLOOP;
};
UpdateVectorWithInverse:
PUBLIC
PROC [inverse: Matrix4by4, vec: Vector3d]
RETURNS [newVec: Vector3d] = {
Simple multiply the transpose of "inverse" by vec seen as a column vector.
FOR i: NAT IN [1..3] DO
newVec[i] ← inverse[1][i]*vec[1] + inverse[2][i]*vec[2] + inverse[3][i]*vec[3];
ENDLOOP;
};
UpdateVectorComputeInverse:
PUBLIC
PROC [vec: Vector3d, mat: Matrix4by4]
RETURNS [newVec: Vector3d] = {
inverse: Matrix4by4 ← Inverse[mat];
FOR i: NAT IN [1..3] DO
newVec[i] ← inverse[1][i]*vec[1] + inverse[2][i]*vec[2] + inverse[3][i]*vec[3];
ENDLOOP;
};
UpdateVectorEvenScaling:
PUBLIC
PROC [vec: Vector3d, mat: Matrix4by4]
RETURNS [newVec: Vector3d] = {
FOR i:
NAT
IN [1..3]
DO
newVec[i] ← mat[i][1]*vec[1] + mat[i][2]*vec[2] + mat[i][3]*vec[3];
ENDLOOP;
};
UpdatePlaneWithInverse:
PUBLIC
PROC [plane: Plane, inverse: Matrix4by4]
RETURNS [newPlane: Plane] = {
Simple multiply the transpose of "inverse" by plane seen as a column vector.
newPlane ← NEW[PlaneObj];
newPlane.A ← inverse[1][1]*plane.A + inverse[2][1]*plane.B + inverse[3][1]*plane.C;
newPlane.B ← inverse[1][2]*plane.A + inverse[2][2]*plane.B + inverse[3][2]*plane.C;
newPlane.C ← inverse[1][3]*plane.A + inverse[2][3]*plane.B + inverse[3][3]*plane.C;
newPlane.D ← inverse[1][4]*plane.A + inverse[2][4]*plane.B + inverse[3][4]*plane.C + plane.D;
};
UpdatePlaneComputeInverse:
PUBLIC
PROC [plane: Plane, mat: Matrix4by4]
RETURNS [newPlane: Plane] = {
inverse: Matrix4by4 ← Inverse[mat];
newPlane ← NEW[PlaneObj];
newPlane.A ← inverse[1][1]*plane.A + inverse[2][1]*plane.B + inverse[3][1]*plane.C;
newPlane.B ← inverse[1][2]*plane.A + inverse[2][2]*plane.B + inverse[3][2]*plane.C;
newPlane.C ← inverse[1][3]*plane.A + inverse[2][3]*plane.B + inverse[3][3]*plane.C;
newPlane.D ← inverse[1][4]*plane.A + inverse[2][4]*plane.B + inverse[3][4]*plane.C + plane.D;
};
PerspectiveTrans:
PUBLIC
PROC [oldPoint: Point3d, focalDistance:
REAL ← 1800]
RETURNS [newPoint: Point2d] = {
IF oldPoint[3] >= focalDistance THEN SIGNAL IllegalDepth;
newPoint[1] ← oldPoint[1]*focalDistance/(focalDistance - oldPoint[3]); -- x' = xf/(f-z)
newPoint[2] ← oldPoint[2]*focalDistance/(focalDistance - oldPoint[3]); -- y' = yf/(f-z)
};
IllegalDepth: PUBLIC SIGNAL = CODE;
Scale:
PUBLIC PROC [mat: Matrix4by4, sx, sy, sz:
REAL]
RETURNS [transMat: Matrix4by4] = {
FOR j: NAT IN[1..4] DO
transMat[1][j] ← mat[1][j]*sx;
ENDLOOP;
FOR j: NAT IN[1..4] DO
transMat[2][j] ← mat[2][j]*sy;
ENDLOOP;
FOR j: NAT IN[1..4] DO
transMat[3][j] ← mat[3][j]*sz;
ENDLOOP;
};
Translate:
PUBLIC PROC [mat: Matrix4by4, dx, dy, dz:
REAL]
RETURNS [transMat: Matrix4by4] = {
i, j: CARDINAL;
FOR i
IN [1..3]
DO
-- in the first three columns, the elements are copied
FOR j
IN [1..3]
DO
transMat[i][j] ← mat[i][j];
ENDLOOP;
ENDLOOP;
transMat[1][4] ← mat[1][4] + dx; -- add the translation offset to the last row
transMat[2][4] ← mat[2][4] + dy;
transMat[3][4] ← mat[3][4] + dz;
};
Rotate:
PUBLIC
PROC [mat: Matrix4by4, axis: [1..3], degrees:
REAL]
RETURNS [transMat: Matrix4by4] = {
SELECT axis
FROM
1 => transMat ← RotateX[mat, degrees];
2 => transMat ← RotateY[mat, degrees];
3 => transMat ← RotateZ[mat, degrees];
ENDCASE => ERROR;
};
RotateX:
PUBLIC PROC [mat: Matrix4by4, degrees:
REAL]
RETURNS [transMat: Matrix4by4] = {
rotMat: Matrix4by4 ← MakeRotateXMat[degrees];
transMat ← MatMult[rotMat,mat];
};
RotateY:
PUBLIC PROC [mat: Matrix4by4, degrees:
REAL]
RETURNS [transMat: Matrix4by4] = {
rotMat: Matrix4by4 ← MakeRotateYMat[degrees];
transMat ← MatMult[rotMat,mat];
};
RotateZ:
PUBLIC PROC [mat: Matrix4by4, degrees:
REAL]
RETURNS [transMat: Matrix4by4] = {
rotMat: Matrix4by4 ← MakeRotateZMat[degrees];
transMat ← MatMult[rotMat,mat];
};
RotateAxis:
PUBLIC
PROC [mat: Matrix4by4, axis: Vector3d, degrees:
REAL]
RETURNS [transMat: Matrix4by4] = {
rotMat: Matrix4by4 ← MakeRotateAxisMat[axis, degrees];
transMat ← MatMult[rotMat,mat];
};
SetOrigin:
PUBLIC
PROC [mat: Matrix4by4, origin: Point3d]
RETURNS [newMat: Matrix4by4] = {
FOR i:
NAT
IN [1..3]
DO
FOR j:
NAT
IN [1..3]
DO
newMat[i][j] ← mat[i][j];
ENDLOOP;
ENDLOOP;
FOR i:
NAT
IN [1..3]
DO
newMat[i][4] ← origin[i];
ENDLOOP;
};
LocalScale:
PUBLIC PROC [mat: Matrix4by4, sx, sy, sz:
REAL]
RETURNS [transMat: Matrix4by4] = {
FOR i:
NAT
IN[1..3]
DO
transMat[i][1] ← mat[i][1]*sx;
ENDLOOP;
FOR i:
NAT
IN[1..3]
DO
transMat[i][2] ← mat[i][2]*sy;
ENDLOOP;
FOR i:
NAT
IN[1..3]
DO
transMat[i][3] ← mat[i][3]*sz;
ENDLOOP;
FOR i:
NAT
IN[1..3]
DO
transMat[i][4] ← mat[i][4];
ENDLOOP;
};
LocalTranslate:
PUBLIC PROC [mat: Matrix4by4, dx, dy, dz:
REAL]
RETURNS [transMat: Matrix4by4] = {
i, j: CARDINAL;
FOR i
IN [1..3]
DO
-- in the first three columns, the elements are copied
FOR j
IN [1..3]
DO
transMat[i][j] ← mat[i][j];
ENDLOOP;
ENDLOOP;
transMat[1][4] ← mat[1][1]*dx + mat[1][2]*dy + mat[1][3]*dz + mat[1][4];
transMat[2][4] ← mat[2][1]*dx + mat[2][2]*dy + mat[2][3]*dz + mat[2][4];
transMat[3][4] ← mat[3][1]*dx + mat[3][2]*dy + mat[3][3]*dz + mat[3][4];
};
LocalRotateX:
PUBLIC PROC [mat: Matrix4by4, degrees:
REAL]
RETURNS [transMat: Matrix4by4] = {
rotMat: Matrix4by4 ← MakeRotateXMat[degrees];
transMat ← MatMult[mat,rotMat];
};
LocalRotateY:
PUBLIC PROC [mat: Matrix4by4, degrees:
REAL]
RETURNS [transMat: Matrix4by4] = {
rotMat: Matrix4by4 ← MakeRotateYMat[degrees];
transMat ← MatMult[mat,rotMat];
};
LocalRotateZ:
PUBLIC PROC [mat: Matrix4by4, degrees:
REAL]
RETURNS [transMat: Matrix4by4] = {
rotMat: Matrix4by4 ← MakeRotateZMat[degrees];
transMat ← MatMult[mat,rotMat];
};
LocalRotateAxis:
PUBLIC PROC [mat: Matrix4by4, axis: Vector3d, degrees:
REAL]
RETURNS [transMat: Matrix4by4] = {
rotMat: Matrix4by4 ← MakeRotateAxisMat[axis, degrees];
transMat ← MatMult[mat,rotMat];
};
MatMult:
PUBLIC PROC [left, right: Matrix4by4]
RETURNS [transMat: Matrix4by4] = {
i, j: CARDINAL;
FOR i
IN [1..3]
DO
-- The last row is always [0 0 0 1]
FOR j
IN [1..3]
DO
-- In the first three columns, the last element is always zero
transMat[i][j] ← left[i][1]*right[1][j] + left[i][2]*right[2][j] + left[i][3]*right[3][j];
ENDLOOP;
ENDLOOP;
FOR i
IN [1..3]
DO
-- calculate the last column
transMat[i][4] ← left[i][1]*right[1][4] + left[i][2]*right[2][4] + left[i][3]*right[3][4] + left[i][4];
ENDLOOP;
};
Mult:
PUBLIC PROC [ab, bc: Matrix4by4]
RETURNS [ac: Matrix4by4] = {
Computes bc*ab where * is standard matrix multiplication.
i, j: CARDINAL;
FOR i
IN [1..3]
DO
-- The last row is always [0 0 0 1]
FOR j
IN [1..3]
DO
-- In the first three columns, the last element is always zero
ac[i][j] ← bc[i][1]*ab[1][j] + bc[i][2]*ab[2][j] + bc[i][3]*ab[3][j];
ENDLOOP;
ENDLOOP;
FOR i
IN [1..3]
DO
-- calculate the last column
ac[i][4] ← bc[i][1]*ab[1][4] + bc[i][2]*ab[2][4] + bc[i][3]*ab[3][4] + bc[i][4];
ENDLOOP;
};
Cat:
PUBLIC
PROC [ab, bc, cd: Matrix4by4]
RETURNS [ad: Matrix4by4] = {
ad ← Mult[Mult[ab, bc], cd];
};
MakeScaleMat:
PUBLIC
PROC [sx, sy, sz:
REAL]
RETURNS [scale: Matrix4by4] = {
scale ← Identity[];
scale[1][1] ← sx; scale[2][2] ← sy; scale[3][3] ← sz;
};
MakeTranslateMat:
PUBLIC
PROC [dx, dy, dz:
REAL]
RETURNS [trans: Matrix4by4] = {
trans ← Identity[];
trans[1][4] ← dx; trans[2][4] ← dy; trans[3][4] ← dz;
};
MakeRotateMat:
PUBLIC
PROC [axis: [1..3], degrees:
REAL]
RETURNS [rotMat: Matrix4by4] = {
SELECT axis
FROM
1 => rotMat ← MakeRotateXMat[degrees];
2 => rotMat ← MakeRotateYMat[degrees];
3 => rotMat ← MakeRotateZMat[degrees];
ENDCASE => ERROR;
};
MakeRotateXMat:
PUBLIC
PROC [degrees:
REAL]
RETURNS [rotx: Matrix4by4] = {
sind: REAL ← RealFns.SinDeg[degrees];
cosd: REAL ← RealFns.CosDeg[degrees];
rotx ← Identity[];
rotx[2][2] ← rotx[3][3] ← cosd;
rotx[2][3] ← -sind; rotx[3][2] ← sind;
};
MakeRotateYMat:
PUBLIC
PROC [degrees:
REAL]
RETURNS [roty: Matrix4by4] = {
sind: REAL ← RealFns.SinDeg[degrees];
cosd: REAL ← RealFns.CosDeg[degrees];
roty ← Identity[];
roty[1][1] ← roty[3][3] ← cosd;
roty[3][1] ← -sind; roty[1][3] ← sind;
};
MakeRotateZMat:
PUBLIC
PROC [degrees:
REAL]
RETURNS [rotz: Matrix4by4] = {
sind: REAL ← RealFns.SinDeg[degrees];
cosd: REAL ← RealFns.CosDeg[degrees];
rotz ← Identity[];
rotz[1][1] ← rotz[2][2] ← cosd;
rotz[1][2] ← -sind; rotz[2][1] ← sind;
};
MakeRotateAxisMat:
PUBLIC
PROC [axis: Vector3d, degrees:
REAL]
RETURNS [rotMat: Matrix4by4] = {
sin, cos, vers, kxkxv, kxkyv, kxkzv, kykyv, kykzv, kzkzv, kxsin, kysin, kzsin: REAL;
axis ← SVVector3d.Normalize[axis];
sin ← RealFns.SinDeg[degrees];
cos ← RealFns.CosDeg[degrees];
vers ← 1.0 - cos;
kxkxv ← axis[1]*axis[1]*vers;
kxkyv ← axis[1]*axis[2]*vers;
kxkzv ← axis[1]*axis[3]*vers;
kykyv ← axis[2]*axis[2]*vers;
kykzv ← axis[2]*axis[3]*vers;
kzkzv ← axis[3]*axis[3]*vers;
kxsin ← axis[1]*sin;
kysin ← axis[2]*sin;
kzsin ← axis[3]*sin;
rotMat ← [
[kxkxv+cos, kxkyv-kzsin, kxkzv+kysin, 0],
[kxkyv+kzsin, kykyv+cos, kykzv-kxsin, 0],
[kxkzv-kysin, kykzv+kxsin, kzkzv+cos, 0]
];
};
MakeMatFromZandXAxis:
PUBLIC
PROC [zAxis, xAxis: Vector3d, origin: Point3d]
RETURNS [mat: Matrix4by4] = {
Uses the normalized version of the zAxis given as it is. Projects xAxis onto the plane of zAxis and normalizes. Finds y by cross product.
normZ: Vector3d ← SVVector3d.Normalize[zAxis];
normY: Vector3d ← SVVector3d.Normalize[SVVector3d.CrossProduct[normZ, xAxis]];
normX: Vector3d ← SVVector3d.CrossProduct[normY, normZ];
FOR i:
NAT
IN[1..3]
DO
mat[i][1] ← normX[i];
mat[i][2] ← normY[i];
mat[i][3] ← normZ[i];
mat[i][4] ← origin[i];
ENDLOOP;
};
MakeMatFromYandXAxis:
PUBLIC PROC [yAxis, xAxis: Vector3d, origin: Point3d]
RETURNS [mat: Matrix4by4] = {
Uses the normalized version of the yAxis given as it is. Projects xAxis onto the plane of yAxis and normalizes. Finds z by cross product.
normY: Vector3d ← SVVector3d.Normalize[yAxis];
normZ: Vector3d ← SVVector3d.Normalize[SVVector3d.CrossProduct[xAxis, normY]];
normX: Vector3d ← SVVector3d.CrossProduct[normY, normZ];
FOR i:
NAT
IN[1..3]
DO
mat[i][1] ← normX[i];
mat[i][2] ← normY[i];
mat[i][3] ← normZ[i];
mat[i][4] ← origin[i];
ENDLOOP;
};
MakeHorizontalMatFromZAxis:
PUBLIC
PROC [zAxis: Vector3d, origin: Point3d]
RETURNS [mat: Matrix4by4] = {
Uses zAxis as it is. Finds a horizontal x axis orthogonal to zAxis. If zAxis is vertical, then there are infinitely many. Chooses [1,0,0] in this case.
xAxis: Vector3d;
IF zAxis[1] = 0 AND zAxis[3] = 0 THEN xAxis ← [1,0,0]
ELSE xAxis ← SVVector3d.CrossProduct[[0,1,0], zAxis];
mat ← MakeMatFromZandXAxis[zAxis, xAxis, origin];
};
MakeMatFromAxes:
PUBLIC
PROC [xAxis, yAxis, zAxis: Vector3d, origin: Point3d]
RETURNS [mat: Matrix4by4] = {
FOR i:
NAT
IN[1..3]
DO
mat[i][1] ← xAxis[i];
mat[i][2] ← yAxis[i];
mat[i][3] ← zAxis[i];
mat[i][4] ← origin[i];
ENDLOOP;
};
ScaleFromMatrix:
PUBLIC
PROC [mat: Matrix4by4]
RETURNS [sx, sy, sz:
REAL] = {
Finds out how much this matrix will scale an object it is applied to
when you apply a scaling matrix to some matrix A =
[a b c d] [Sx 0 0 0] [Sx*a Sy*b Sz*c d]
[e f g h] * [0 Sy 0 0] you get [Sx*e Sy*f Sz*g h]
[i j k l] [0 0 Sz 0] [Sx*i Sy*j Sz*k l]
[0 0 0 1] [0 0 0 1] [0 0 0 1]
Since [a,e,i], [b,f,j], and [c,g,k] are unit vectors to begin with, we can find the scaling factors by taking the magnitudes of these quantities
sx ← SVVector3d.Magnitude[[mat[1][1],mat[2][1],mat[3][1]]];
sy ← SVVector3d.Magnitude[[mat[1][2],mat[2][2],mat[3][2]]];
sz ← SVVector3d.Magnitude[[mat[1][3],mat[2][3],mat[3][3]]];
};
OriginOfMatrix:
PUBLIC
PROC [mat: Matrix4by4]
RETURNS [origin: Point3d] = {
origin[1] ← mat[1][4];
origin[2] ← mat[2][4];
origin[3] ← mat[3][4];
};
XAxisOfMatrix:
PUBLIC
PROC [mat: Matrix4by4]
RETURNS [xAxis: Vector3d] = {
xAxis[1] ← mat[1][1];
xAxis[2] ← mat[2][1];
xAxis[3] ← mat[3][1];
};
YAxisOfMatrix:
PUBLIC
PROC [mat: Matrix4by4]
RETURNS [yAxis: Vector3d] = {
yAxis[1] ← mat[1][2];
yAxis[2] ← mat[2][2];
yAxis[3] ← mat[3][2];
};
ZAxisOfMatrix:
PUBLIC
PROC [mat: Matrix4by4]
RETURNS [zAxis: Vector3d] = {
zAxis[1] ← mat[1][3];
zAxis[2] ← mat[2][3];
zAxis[3] ← mat[3][3];
};
Because of the orthogonality of the transform matrices, matrix inversion is relatively easy.
InverseNoScaling:
PUBLIC
PROC [mat: Matrix4by4]
RETURNS [inverse: Matrix4by4] = {
Rotation part just transposes.
FOR i:
NAT
IN[1..3]
DO
FOR j:
NAT
IN[1..3]
DO
inverse[i][j] ← mat[j][i];
ENDLOOP;
ENDLOOP;
Translation part is computed from scalar products.
inverse[1][4] ← -[px nx + py ny + pz nz]
inverse[2][4] ← -[px ox + py oy + pz oz]
inverse[3][4] ← -[px ax + py ay + pz az]
where p is column 4, n is column 1, o is column 2, and a is column 3 of mat.
inverse[1][4] ← -(mat[1][4]*mat[1][1] + mat[2][4]*mat[2][1] + mat[3][4]*mat[3][1]);
inverse[2][4] ← -(mat[1][4]*mat[1][2] + mat[2][4]*mat[2][2] + mat[3][4]*mat[3][2]);
inverse[3][4] ← -(mat[1][4]*mat[1][3] + mat[2][4]*mat[2][3] + mat[3][4]*mat[3][3]);
};
DegenerateInverse: PUBLIC SIGNAL = CODE;
Cofactor: PUBLIC PROC [mat: Matrix4by4, row, col: NAT] RETURNS [cof: REAL] = {
Returns the determinant of the 3 by 3 matrix which results by crossing off row row and column col in mat.
rowCount, colCount: NAT;
exponent: NAT;
m3: Matrix3by3;
rowCount ← 0;
FOR i: NAT IN[1..4] DO
IF i = row THEN LOOP;
rowCount ← rowCount + 1;
colCount ← 0;
FOR j: NAT IN[1..4] DO
IF j = col THEN LOOP;
colCount ← colCount + 1;
m3[rowCount][colCount] ← mat[i][j];
ENDLOOP;
ENDLOOP;
cof ← Determinant3by3[m3];
exponent ← row+col;
IF (exponent/2)*2 # exponent THEN cof ← -cof;
};
Determinant3by3:
PROC [m3: Matrix3by3]
RETURNS [det:
REAL] = {
det ← m3[1][1]*m3[2][2]*m3[3][3] +
m3[2][1]*m3[3][2]*m3[1][3] +
m3[3][1]*m3[1][2]*m3[2][3] -
(m3[3][1]*m3[2][2]*m3[1][3] +
m3[2][1]*m3[1][2]*m3[3][3] +
m3[1][1]*m3[3][2]*m3[2][3]);
};
Determinant:
PUBLIC
PROC [mat: Matrix4by4]
RETURNS [det:
REAL] = {
det ← mat[1][1]*mat[2][2]*mat[3][3] +
mat[2][1]*mat[3][2]*mat[1][3] +
mat[3][1]*mat[1][2]*mat[2][3] -
(mat[3][1]*mat[2][2]*mat[1][3] +
mat[2][1]*mat[1][2]*mat[3][3] +
mat[1][1]*mat[3][2]*mat[2][3]);
};
Transpose:
PUBLIC
PROC [mat: Matrix4by4]
RETURNS [matT: Matrix4by4] = {
interchange rows and columns
FOR i:
NAT
IN[1..4]
DO
FOR j:
NAT
IN[1..4]
DO
matT[i][j] ← mat[j][i];
ENDLOOP;
ENDLOOP;
};
Inverse:
PUBLIC
PROC [mat: Matrix4by4]
RETURNS [inverse: Matrix4by4] = {
Inverts any matrix of the given form (even with scaling). Find the matrix of Cofactors, transpose it and divide by the determinant
det: REAL ← Determinant[mat];
IF det = 0
THEN
SIGNAL DegenerateInverse;
Compute the matrix of cofactors and transpose it as you go.
inverse[1][1] ← (mat[2][2]*mat[3][3] - mat[3][2]*mat[2][3])/det;
inverse[2][1] ← -(mat[2][1]*mat[3][3] - mat[3][1]*mat[2][3])/det;
inverse[3][1] ← (mat[2][1]*mat[3][2] - mat[3][1]*mat[2][2])/det;
inverse[1][2] ← -(mat[1][2]*mat[3][3] - mat[3][2]*mat[1][3])/det;
inverse[2][2] ← (mat[1][1]*mat[3][3] - mat[3][1]*mat[1][3])/det;
inverse[3][2] ← -(mat[1][1]*mat[3][2] - mat[3][1]*mat[1][2])/det;
inverse[1][3] ← (mat[1][2]*mat[2][3] - mat[2][2]*mat[1][3])/det;
inverse[2][3] ← -(mat[1][1]*mat[2][3] - mat[2][1]*mat[1][3])/det;
inverse[3][3] ← (mat[1][1]*mat[2][2] - mat[2][1]*mat[1][2])/det;
The fourth column of the inverse is -inverse*t, where t is the fourth column of mat.
FOR i:
NAT
IN [1..3]
DO
inverse[i][4] ← (-inverse[i][1]*mat[1][4] - inverse[i][2]*mat[2][4] - inverse[i][3]*mat[3][4]);
ENDLOOP;
};
Inverse: PUBLIC PROC [mat: Matrix4by4] RETURNS [inverse: Matrix4by4] = {
Inverts any matrix of the given form (even with scaling). Find the matrix of Cofactors, transpose it and divide by the determinant
det: REAL ← Determinant[mat];
IF det = 0 THEN SIGNAL DegenerateInverse;
FOR i: NAT IN[1..3] DO
FOR j: NAT IN[1..4] DO
inverse[i][j] ← Cofactor[mat,j,i]/det; -- transpose as you do it
ENDLOOP;
ENDLOOP;
inverse[4][1] ← inverse[4][2] ← inverse[4][3] ← 0.0;
inverse[4][4] ← 1.0; -- these would also be the calculated values if we did them in the loop
};
AInTermsOfB:
PUBLIC
PROC [a
WORLD,b
WORLD: Matrix4by4]
RETURNS [a
b: Matrix4by4] = {
WORLDb: Matrix4by4 ← Inverse[bWORLD];
ab ← Mult[aWORLD,WORLDb];
};
Normalizing
UnitNormalize:
PUBLIC
PROC [mat: Matrix4by4]
RETURNS [unitMat: Matrix4by4] = {
finds a matrix transform which provides the same translation and rotation of mat, but with a scaling of 1. While we're at it, make sure the axes of unitMat are perpendicular.
| nx ox ax px |
mat = | ny oy ay py |
| nz oz az pz |
where n, o, a, and p are the column vectors respectively.
Make n a unit vector. Find a = n x o, which guarantees that a is perpendicular to n. Make a a unit vector. Find o = a x n, assuring o is perpendicular to the other 2 and all are unit.
n, o, a: Vector3d;
FOR i: NAT IN[1..3] DO
n[i] ← mat[i][1];
o[i] ← mat[i][2];
ENDLOOP;
n ← SVVector3d.Normalize[n];
a ← SVVector3d.CrossProduct[n,o];
a ← SVVector3d.Normalize[a];
o ← SVVector3d.CrossProduct[a,n];
FOR i:
NAT
IN[1..3]
DO
unitMat[i][1] ← n[i];
unitMat[i][2] ← o[i];
unitMat[i][3] ← a[i];
unitMat[i][4] ← mat[i][4];
ENDLOOP;
}; -- end of UnitNormalize
Normalize:
PUBLIC
PROC [mat: Matrix4by4]
RETURNS [normMat: Matrix4by4] = {
Finds a matrix with the same translation, rotation and scaling as mat, but with the axes guaranteed to be orthogonal.
take n, o, and a as in "UnitNormalize" above. Start with n. Find magnitude of o, and a.
a ← (n x o)|a| / |n x o|
o ← (a x n)|o| / |a x n|
magA, magO: REAL;
n, o, a: Vector3d;
origin: Point3d;
nCrossO, aCrossN: Vector3d;
n ← XAxisOfMatrix[mat];
o ← YAxisOfMatrix[mat];
a ← ZAxisOfMatrix[mat];
origin ← OriginOfMatrix[mat];
magA ← SVVector3d.Magnitude[a];
magO ← SVVector3d.Magnitude[o];
nCrossO ← SVVector3d.CrossProduct[n,o];
a ← SVVector3d.Scale[nCrossO, magA/SVVector3d.Magnitude[nCrossO]];
aCrossN ← SVVector3d.CrossProduct[a,n];
o ← SVVector3d.Scale[aCrossN, magO/SVVector3d.Magnitude[aCrossN]];
normMat ← MakeMatFromAxes[n,o,a,origin];
}; -- end of Normalize
Max:
PRIVATE
PROC [a, b:
REAL]
RETURNS [
REAL] = {
RETURN[IF a >= b THEN a ELSE b];
};
NormalizeUniformScale:
PUBLIC
PROC [mat: Matrix4by4]
RETURNS [normMat: Matrix4by4] = {
Finds a matrix with the same translation, and rotation as mat, with the axes guaranteed to be orthogonal and with the scaling along all three axes set equal to the maximum scaling of the three axes of mat.
Find max of |n|, |o|, and |a|. Call it s. Scale all vectors with this. Proceed as for Normalize.
take n, o, and a as in "UnitNormalize" above. Start with n. Find magnitude of o, and a.
n ← n*s/|n|
a ← (n x o)*s / |n x o|
o ← (a x o)*s / |a x n|
magA, magO, magN, s: REAL;
n, o, a: Vector3d;
nCrossO, aCrossN: Vector3d;
FOR i:
NAT
IN[1..3]
DO
n[i] ← mat[i][1];
o[i] ← mat[i][2];
ENDLOOP;
magN ← SVVector3d.Magnitude[n];
magA ← SVVector3d.Magnitude[a];
magO ← SVVector3d.Magnitude[o];
s ← Max[magN, magA];
s ← Max[s, magO];
n ← SVVector3d.Scale[n, s/magN];
nCrossO ← SVVector3d.CrossProduct[n,o];
a ← SVVector3d.Scale[nCrossO, s/SVVector3d.Magnitude[nCrossO]];
aCrossN ← SVVector3d.CrossProduct[a,n];
o ← SVVector3d.Scale[aCrossN, s/SVVector3d.Magnitude[aCrossN]];
FOR i:
NAT
IN[1..3]
DO
normMat[i][1] ← n[i];
normMat[i][2] ← o[i];
normMat[i][3] ← a[i];
normMat[i][4] ← mat[i][4];
ENDLOOP;
}; -- end of NormalizeUniformScale
NormalizeNoRotation:
PUBLIC
PROC [mat: Matrix4by4]
RETURNS [normMat: Matrix4by4] = {
Finds a matrix with the same translation and scaling as mat, with the axes guaranteed to be orthogonal, removing rotational components.
Find the magnitude of each of n, o, and a (as in "UnitNormalize" above). Scale the appropriate columns of the identity matrix by these quantities and add the translations of mat to the last column.
transMat: Matrix4by4 ← MakeTranslateMat[mat[1][4], mat[2][4], mat[3][4]];
magA, magO, magN: REAL;
n, o, a: Vector3d;
FOR i:
NAT
IN[1..3]
DO
n[i] ← mat[i][1];
o[i] ← mat[i][2];
a[i] ← mat[i][3];
ENDLOOP;
magN ← SVVector3d.Magnitude[n];
magO ← SVVector3d.Magnitude[o];
magA ← SVVector3d.Magnitude[a];
FOR i:
NAT
IN[1..3]
DO
normMat[i][1] ← transMat[i][1]*magN;
normMat[i][2] ← transMat[i][2]*magO;
normMat[i][3] ← transMat[i][3]*magA;
normMat[i][4] ← mat[i][4];
ENDLOOP;
}; -- end of NormalizeNoRotation
NormalizeNoTranslation:
PUBLIC
PROC [mat: Matrix4by4]
RETURNS [normMat: Matrix4by4] = {
Finds a matrix with the same scaling and rotation as mat, with the axes guaranteed to be orthogonal, removing translational components.
normMat ← Normalize[mat];
FOR i:
NAT
IN[1..3]
DO
normMat[i][4] ← 0;
ENDLOOP;
};
AlmostInt:
PRIVATE
PROC [r:
REAL]
RETURNS [
BOOL] = {
rInt: REAL;
rInt ← RoundI[r];
IF ABS[r-rInt] < almostZero THEN RETURN[TRUE]
ELSE RETURN[FALSE];
};
TidyUpVector:
PUBLIC
PROC [vec: Vector3d]
RETURNS [fixedVec: Vector3d] = {
If a component is almost a multiple of .5, make it exactly so.
vec2: REAL;
FOR i:
NAT
IN [1..3]
DO
vec2 ← vec[i]*2.0;
fixedVec[i] ← IF AlmostInt[vec2] THEN RoundI[vec2]/2.0 ELSE vec[i];
ENDLOOP;
};
TidyUpMatrix:
PUBLIC
PROC [mat: Matrix4by4]
RETURNS [fixedMat: Matrix4by4] = {
If a component is almost a multiple of .5, make it exactly so.
mat2: REAL;
FOR i:
NAT
IN [1..3]
DO
FOR j:
NAT
IN [1..4]
DO
mat2 ← mat[i][j]*2.0;
fixedMat[i][j] ← IF AlmostInt[mat2] THEN RoundI[mat2]/2.0 ELSE mat[i][j];
ENDLOOP
ENDLOOP;
};
RoundI:
PRIVATE
PROC [r:
REAL]
RETURNS [
INTEGER] = {
RealOps.RoundI doesn't seem to work on negative numbers.
IF r < 0 THEN RETURN[-Real.Round[-r]]
ELSE RETURN[Real.Round[r]];
};
END.