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.