File: Matrix3dImpl.mesa
Last Edited by: Bier, July 3, 1983 1:23 pm
Author: Eric Allan Bier on May 27, 1982
Contents: Implementation of a 4 by 4 Homogenous Transform package for 3d graphics
DIRECTORY
Matrix3d,
RealFns,
SVVector3d;
Matrix3dImpl: PROGRAM IMPORTS RealFns, 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 |  | 0 0 0 1 | | 1 |
Matrix4by4: TYPE = ARRAY [1..4] OF ARRAY [1..4] OF REAL;
Matrix3by3: TYPE = ARRAY [1..3] OF ARRAY [1..3] OF REAL;
Point3d: TYPE = ARRAY [1..3] OF REAL;
Point2d: TYPE = ARRAY [1..2] OF REAL;
Vector: TYPE = SVVector3d.Vector;
Identity: PUBLIC PROCEDURE [] RETURNS [identityMat: Matrix4by4] = {
i, j: CARDINAL;
FOR i IN [1..4] DO-- in the first three columns, the elements are copied
FOR j IN [1..4] DO
  IF i = j THEN identityMat[i][j] ← 1.0
   ELSE identityMat[i][j] ← 0.0;
ENDLOOP;
ENDLOOP;
};
Update: PUBLIC PROCEDURE [mat: Matrix4by4, point: Point3d] 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;
};
UpdateVector: PUBLIC PROC [mat: Matrix4by4, vec: Vector] RETURNS [newVec: Vector] = {
 Vectors can be rotated and scale, but not translated
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;
};
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.
UpdateVectorWithInverse: PUBLIC PROC [inverse: Matrix4by4, vec: Vector] RETURNS [newVec: Vector] = {
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 [mat: Matrix4by4, vec: Vector] RETURNS [newVec: Vector] = {
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 [mat: Matrix4by4, vec: Vector] RETURNS [newVec: Vector] = {
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;
};
GetPerspective: 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 PROCEDURE [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;
transMat[4][1] ← transMat[4][2] ← transMat[4][3] ← 0.0;
transMat[4][4] ← 1.0;
};
Translate: PUBLIC PROCEDURE [mat: Matrix4by4, dx, dy, dz: REAL] RETURNS [transMat: Matrix4by4] = {
i, j: CARDINAL;
FOR i IN [1..4] 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;
transMat[4][4] ← 1.0;
};
RotateAboutXAxis: PUBLIC PROCEDURE [mat: Matrix4by4, degrees: REAL] RETURNS [transMat: Matrix4by4] = {
x' = x
y' = y cos(degrees) -z sin(degrees)
z' = y sin(degrees) + z cos(degrees)
sind: REAL ← RealFns.SinDeg[degrees];
cosd: REAL ← RealFns.CosDeg[degrees];
rotMat: Matrix4by4 ← Identity[];
rotMat[2][2] ← rotMat[3][3] ← cosd;
rotMat[2][3] ← -sind; rotMat[3][2] ← sind;
transMat ← MatMult[rotMat,mat];
};

RotateAboutYAxis: PUBLIC PROCEDURE [mat: Matrix4by4, degrees: REAL] RETURNS [transMat: Matrix4by4] = {
x' = x cos(degrees) + z sin(degrees)
y' = y
z' = -x sin(degrees) + z cos(degrees)
sind: REAL ← RealFns.SinDeg[degrees];
cosd: REAL ← RealFns.CosDeg[degrees];
rotMat: Matrix4by4 ← Identity[];
rotMat[1][1] ← rotMat[3][3] ← cosd;
rotMat[3][1] ← -sind; rotMat[1][3] ← sind;
transMat ← MatMult[rotMat,mat];
};

RotateAboutZAxis: PUBLIC PROCEDURE [mat: Matrix4by4, degrees: REAL] RETURNS [transMat: Matrix4by4] = {
x' = x cos(degrees) -y sin(degrees)
y' = x sin(degrees) + y cos(degrees)
z' = z
sind: REAL ← RealFns.SinDeg[degrees];
cosd: REAL ← RealFns.CosDeg[degrees];
rotMat: Matrix4by4 ← Identity[];
rotMat[1][1] ← rotMat[2][2] ← cosd;
rotMat[1][2] ← -sind; rotMat[2][1] ← sind;
transMat ← MatMult[rotMat,mat];
};
LocalScale: PUBLIC PROCEDURE [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;
transMat[4][1] ← transMat[4][2] ← transMat[4][3] ← 0.0;
transMat[4][4] ← 1.0;
};

LocalTranslate: PUBLIC PROCEDURE [mat: Matrix4by4, dx, dy, dz: REAL] RETURNS [transMat: Matrix4by4] = {
i, j: CARDINAL;
FOR i IN [1..4] 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];
transMat[4][4] ← 1.0;
};

LocalRotateAboutXAxis: PUBLIC PROCEDURE [mat: Matrix4by4, degrees: REAL] RETURNS [transMat: Matrix4by4] = {
sind: REAL ← RealFns.SinDeg[degrees];
cosd: REAL ← RealFns.CosDeg[degrees];
rotMat: Matrix4by4 ← Identity[];
rotMat[2][2] ← rotMat[3][3] ← cosd;
rotMat[2][3] ← -sind; rotMat[3][2] ← sind;
transMat ← MatMult[mat,rotMat];
};

LocalRotateAboutYAxis: PUBLIC PROCEDURE [mat: Matrix4by4, degrees: REAL] RETURNS [transMat: Matrix4by4] = {
sind: REAL ← RealFns.SinDeg[degrees];
cosd: REAL ← RealFns.CosDeg[degrees];
rotMat: Matrix4by4 ← Identity[];
rotMat[1][1] ← rotMat[3][3] ← cosd;
rotMat[3][1] ← -sind; rotMat[1][3] ← sind;
transMat ← MatMult[mat,rotMat];
};

LocalRotateAboutZAxis: PUBLIC PROCEDURE [mat: Matrix4by4, degrees: REAL] RETURNS [transMat: Matrix4by4] = {
sind: REAL ← RealFns.SinDeg[degrees];
cosd: REAL ← RealFns.CosDeg[degrees];
rotMat: Matrix4by4 ← Identity[];
rotMat[1][1] ← rotMat[2][2] ← cosd;
rotMat[1][2] ← -sind; rotMat[2][1] ← sind;
transMat ← MatMult[mat,rotMat];
};

MatMult: PUBLIC PROCEDURE [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;
transMat[4][1] ← transMat[4][2] ← transMat[4][3] ← 0; -- Last row
transMat[4][4] ← 1.0;
};

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;
};

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;
};
MakeMatFromZandXAxis: PUBLIC PROC [zAxis, xAxis: Vector, 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: Vector ← SVVector3d.Normalize[zAxis];
normY: Vector ← SVVector3d.Normalize[SVVector3d.CrossProduct[normZ, xAxis]];
normX: Vector ← 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;
mat[4][1] ← mat[4][2] ← mat[4][3] ← 0; mat[4][4] ← 1;
};

MakeHorizontalMatFromZAxis: PUBLIC PROC [zAxis: Vector, 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: Vector;
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: Vector, 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;
 mat[4][1] ← mat[4][2] ← mat[4][3] ← 0; mat[4][4] ← 1;
 };

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: Vector] = {
 xAxis[1] ← mat[1][1];
 xAxis[2] ← mat[2][1];
 xAxis[3] ← mat[3][1];
 };

YAxisOfMatrix: PUBLIC PROC [mat: Matrix4by4] RETURNS [yAxis: Vector] = {
 yAxis[1] ← mat[1][2];
 yAxis[2] ← mat[2][2];
 yAxis[3] ← mat[3][2];
 };

ZAxisOfMatrix: PUBLIC PROC [mat: Matrix4by4] RETURNS [zAxis: Vector] = {
 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] = {
FOR i: NAT IN[1..3] DO
FOR j: NAT IN[1..3] DO
inverse[i][j] ← mat[j][i];
ENDLOOP;
ENDLOOP;
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]);
inverse[4][1] ← inverse[4][2] ← inverse[4][3] ← 0.0;
inverse[4][4] ← 1.0;
};

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] = {
 Because the bottom row of mat has three zeros and a 1, this reduces to finding the cofactor of (4,4).
 det ← Cofactor[mat,4,4];
 };

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;
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
};
WorldToLocal: PUBLIC PROC [AinWorld,BinWorld: Matrix4by4] RETURNS [BinTermsOfA: Matrix4by4] = {
 clearly (AinWorld)<matmult>(BinTermsOfA) = BinWorld;
 Hence BinTermsOfA = (Inverse[AinWorld])(BinWorld);
 worldAInverse: Matrix4by4 ← Inverse[AinWorld];
 BinTermsOfA ← MatMult[worldAInverse,BinWorld];
 };
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: Vector;
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: Vector;
origin: Point3d;
nCrossO, aCrossN: Vector;
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: Vector;
nCrossO, aCrossN: Vector;
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: Vector;
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;
};

END.
Edited on December 28, 1982 11:29 pm, by Bier
Converted to Tioga formatting. Added UpdateVectorWithInverse, UpdateVectorComputeInverse and UpdateVectorEvenScaling.