DIRECTORY
SVMatrix3d, RealFns, Real, SV2d, SV3d, SVVector3d;

SVMatrix3dImpl: CEDAR PROGRAM
IMPORTS RealFns, Real, SVVector3d
EXPORTS SVMatrix3d =
BEGIN

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] = {
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;
};

UpdateDisplacement: PUBLIC PROC [mat: Matrix4by4, vec: Vector3d] 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;
};

UpdateVectorWithInverse: PUBLIC PROC [inverse: Matrix4by4, vec: Vector3d] RETURNS [newVec: Vector3d] = {
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] = {
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] = {
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] = {
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] = {
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] = {
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] = {
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];
};
	
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] _ -(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;

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] = {
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] = {
det: REAL _ Determinant[mat];
IF det = 0 THEN SIGNAL DegenerateInverse;
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;
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;
};



AInTermsOfB: PUBLIC PROC [aWORLD,bWORLD: Matrix4by4] RETURNS [ab: Matrix4by4] = {
WORLDb: Matrix4by4 _ Inverse[bWORLD];
ab _ Mult[aWORLD,WORLDb];
};

UnitNormalize: PUBLIC PROC [mat: Matrix4by4] RETURNS [unitMat: Matrix4by4] = {
 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] = {
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] = {
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] = {
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] = {
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] = {
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] = {
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] = {
IF r < 0 THEN RETURN[-Real.Round[-r]]
ELSE RETURN[Real.Round[r]];
};

END.

dFile: SVMatrix3dImpl.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

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  |

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 |
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.

Simple multiply mat by vec seen as a column vector, ignoring translation components.
Simple multiply the transpose of "inverse" by vec seen as a column vector.
Simple multiply the transpose of "inverse" by plane seen as a column vector.
Computes bc*ab where * is standard matrix multiplication.

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.
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.
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.
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
Because of the orthogonality of the transform matrices, matrix inversion is relatively easy.

Rotation part just transposes.
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.
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;
};
interchange rows and columns
Inverts any matrix of the given form (even with scaling).  Find the matrix of Cofactors, transpose it and divide by the determinant
Compute the matrix of cofactors and transpose it as you go.
The fourth column of the inverse is -inverse*t, where t is the fourth column of mat.
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
};

Normalizing
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.
 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|
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|
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.

Finds a matrix with the same scaling and rotation as mat, with the axes guaranteed to be orthogonal, removing translational components.
If a component is almost a multiple of .5, make it exactly so.
If a component is almost a multiple of .5, make it exactly so.
RealOps.RoundI doesn't seem to work on negative numbers.
Κ–

"Cedar" style˜Ihead1š
œ™Iprocš
œ1™1Lš
œ'™'Lš
œR™Rš
Οk	˜	Lš
œ3˜3—šœ
˜Lšœ˜!Lšœ
˜—šœ
˜Lš
œω™ωIblockšœΟd
œž
œž
œž
œž
œž
œž
œ
ž
œž
œ
™_Lšœœ˜#Lšœœ˜#Lšœœœ
˜Lšœ
œ˜Lšœ	œ˜Lšœ	œ˜Lšœ
œ˜%Lšœœ
˜L˜
—šΟnœœ
œœ˜>Lš	œœ
œœ
Οc6˜NLšœ
œœ
œ˜Lšœœœ˜'Lšœœ˜ Lšœ
œ
˜	Lšœ
˜Lš
œ˜—šŸœœ#œ˜ULš
œέ
™έ
šœœ
œ˜Lš
œW˜WLšœ
˜—Jš
œ˜JšœW
œ+
œ
œA
œy
œ"
œ 
œ6
œ€

œP™Ι—šŸœœ
œ"œ˜_J™Tšœœ
œ˜Lš
œC˜CLšœ
˜—Jš
œ˜—šŸœœ
œ&œ˜hJ™JJšœœ
œ˜Jš
œP˜PJšœ
˜Jš
œ˜—šŸœœ
œ"œ˜gJš
œ#˜#Jšœœ
œ˜Jš
œP˜PJšœ
˜Jš
œ˜—šŸœœ
œ"œ˜dšœœ
œ˜Jš
œC˜C—Jšœ
˜Jš
œ˜—šŸœœ
œ%œ˜eJš
œL™LJšœœ˜Jš	œ	
œ
œ
œ
œ
˜SJš	œ	
œ
œ
œ
œ
˜SJš	œ	
œ
œ
œ
œ
˜SJšœ	
œ
œ
œ
œ	
œ
˜]Jš
œ˜—šŸœœ
œ!œ˜dLš
œ#˜#Jšœœ˜Lš	œ	
œ
œ
œ
œ
˜SJš	œ	
œ
œ
œ
œ
˜SJš	œ	
œ
œ
œ
œ
˜SJšœ	
œ
œ
œ
œ	
œ
˜]Lš
œ˜—š
Ÿœœ
œ$œ	œ˜mJšœœ
œ˜9JšœH ˜XJšœG ˜WJš
œ˜—Lšœœ
œœ˜$šŸœœœœ˜YJšœœ
œ˜Jš
œ˜Jšœ
˜Jšœœ
œ˜Jš
œ˜Jšœ
˜Jšœœ
œ˜Jš
œ˜Jšœ
˜Jš
œ˜—šŸ	œœœœ˜]Jšœœ
˜šœœœ
 6˜Išœœ˜Jš
œ˜—Jšœ
˜—Jšœ
˜Jšœ! -˜NJš
œ ˜ Jš
œ ˜ J˜—L˜š
Ÿœœ
œ*œœ˜ešœ˜Lš
œ&˜&Lš
œ&˜&Lš
œ&˜&Lšœœ
˜—L˜—šŸœœœœ˜XLš
œ-˜-Lš
œ˜Lš
œ˜—šŸœœœœ˜XLš
œ-˜-Lš
œ˜Lš
œ˜—šŸœœœœ˜XLš
œ-˜-Lš
œ˜Lš
œ˜—š
Ÿ
œœ
œ,œœ˜kLš
œ6˜6Lš
œ˜L˜—L˜šŸ	œœ
œ$œ˜Zšœœ
œ˜šœœ
œ˜Lš
œ˜Lšœ
˜—Lšœ
˜—šœœ
œ˜Lš
œ˜Lšœ
˜—L˜L˜—šŸ
œœœœ˜^šœœ
œ˜Jš
œ˜—Jšœ
˜šœœ
œ˜Jš
œ˜—Jšœ
˜šœœ
œ˜Jš
œ˜—Jšœ
˜šœœ
œ˜Jš
œ˜—Jšœ
˜J˜—šŸœœœœ˜bJšœœ
˜šœœœ
 6˜Išœœ˜Jš
œ˜—Jšœ
˜—Jšœ
˜Jš
œH˜HJš
œH˜HJš
œH˜HJ˜—šŸœœœœ˜]Lš
œ-˜-Lš
œ˜Lš
œ˜—šŸœœœœ˜]Lš
œ-˜-Lš
œ˜Lš
œ˜—šŸœœœœ˜]Lš
œ-˜-Lš
œ˜Lš
œ˜—šŸœœ,œœ˜pLš
œ6˜6Lš
œ˜Lš
œ˜—L˜šŸœœœ˜QJšœœ
˜šœœœ #˜7šœœœ
 >˜QJš
œZ˜Z—Jšœ
˜—Jšœ
˜šœœœ
 ˜/Jš
œh˜h—Jšœ
˜J˜J˜—šŸœœœ˜CL™9Jšœœ
˜šœœœ #˜7šœœœ
 >˜QJš
œE˜E—Jšœ
˜—Jšœ
˜šœœœ
 ˜/Jš
œQ˜Q—Jšœ
˜J˜—L˜šŸœœ
œœ˜FLš
œ˜L˜L™—š
Ÿœœ
œœœ˜LLš
œ˜Lš
œ5˜5Lš
œ˜—š
Ÿœœ
œœœ˜PLš
œ˜Lš
œ5˜5Lš
œ˜—š
Ÿ
œœ
œœœ˜Yšœ˜Lš
œ&˜&Lš
œ&˜&Lš
œ&˜&Lšœœ
˜—L˜—š
Ÿœœ
œœœ˜JLšœœ˜%Lšœœ˜%Lš
œ˜Lš
œ˜Lš
œ&˜&Lš
œ˜—š
Ÿœœ
œœœ˜JLšœœ˜&Lšœœ˜%Lš
œ˜Lš
œ˜Lš
œ&˜&Lš
œ˜—š
Ÿœœ
œœœ˜JLšœœ˜&Lšœœ˜%Lš
œ˜Lš
œ˜Lš
œ&˜&Lš
œ˜—š
Ÿœœ
œœœ˜_LšœOœ
˜TLš
œ"˜"Lš
œ˜Lš
œ˜Lš
œ˜Lš
œ˜Lš
œ˜Lš
œ˜Lš
œ˜Lš
œ˜Lš
œ˜Lš
œ˜Lš
œ˜Lš
œ˜š
œ
˜
Lš
œ)˜)Lš
œ)˜)Lš
œ(˜(Lš
œ˜—L˜L˜—šŸœœ
œ+œ˜iLš
œ‹
™‹
Lš
œ.˜.Lš
œN˜NLš
œ8˜8šœœ
œ˜Lš
œ˜Lš
œ˜Lš
œ˜Lš
œ˜—Lšœ
˜Lš
œ˜L˜—šŸœœ+œ˜iLš
œ‹
™‹
Lš
œ.˜.Lš
œN˜NLš
œ8˜8šœœ
œ˜Lš
œ˜Lš
œ˜Lš
œ˜Lš
œ˜—Lšœ
˜Lš
œ˜L˜—šŸœœ
œ$œ˜hLš
œš
™š
Lš
œ˜Lšœœœ˜5Lšœ1˜5Lš
œ1˜1Lš
œ˜L˜—šŸœœ
œ2œ˜kšœœ
œ˜Lš
œ˜Lš
œ˜Lš
œ˜Lš
œ˜—Lšœ
˜Lš
œ˜L˜—š
Ÿœœ
œœœ˜Mšœt
œδ™ΩLš
œ;˜;Lš
œ;˜;Lš
œ;˜;Lš
œ˜L˜——šŸœœ
œœ˜KLš
œ˜Lš
œ˜Lš
œ˜Lš
œ˜L˜—šŸ
œœ
œœ˜JLš
œ˜Lš
œ˜Lš
œ˜Lš
œ˜—L˜šŸ
œœ
œœ˜JLš
œ˜Lš
œ˜Lš
œ˜Lš
œ˜—L˜šŸ
œœ
œœ˜JLš
œ˜Lš
œ˜Lš
œ˜Lš
œ˜Lš
œ]™]—šŸœœ
œœ˜QL™šœœ
œ˜šœœ
œ˜Lš
œ˜—Lšœ
˜—Lšœ
˜L™2Lš
œ(™(Lš
œ(™(Lš
œ(™(Lš
œL™LLš
œS˜SLš
œS˜SLš
œS˜SLš
œ˜—Lšœœ
œœ
˜)L˜šŸœœ
œœœœ™NLš
œi™iLšœœ
™Lšœ
œ
™Lš
œ™Lš
œ
™
šœœ
œ™Lšœ	œ
œ
™Lš
œ™Lš
œ
™
šœœ
œ™Lšœ	œ
œ
™Lš
œ™Lš
œ#™#—Lšœ
™—Lšœ
™Lš
œ™Lš
œ™Lšœœ™-Lš
œ™—šŸœœœœ˜>Lš
œΎ
˜Ύ
Lš
œ˜L˜—š
Ÿœœ
œœœ˜BLš
œΠ
˜Π
Lš
œ˜—šŸ	œœ
œœ˜GLš
œ™šœœ
œ˜šœœ
œ˜Lš
œ˜—Lšœ
˜—Lšœ
˜Lš
œ˜—šŸœœ
œœ˜HLš
œƒ
™ƒ
Lšœœ˜šœ	œ
œ˜)L™;—Lš
œ@˜@Lš
œA˜ALš
œ@˜@L˜Lš
œA˜ALš
œ@˜@Lš
œA˜AL˜Lš
œ@˜@Lš
œA˜Aš
œ@˜@L™T—šœœ
œ˜Lš
œ_˜_—Lšœ
˜Lš
œ˜—L˜šŸœœ
œœ™HLš
œƒ
™ƒ
Lšœœ™Lšœ	œ
œ™)šœœ
œ™šœœ
œ™Lšœ' ™@—Lšœ
™—Lšœ
™Lš
œ4™4Lšœ G™\Lš
œ™—L˜šŸœœ
œΟuœ‘œœ‘
œ˜QLšΟs‘
œ‘œ˜%Lšœ
‘
œ	‘œ
’‘
œ˜Lš
œ˜—Iheadš
œ™L˜šŸ
œœ
œœ˜NLš
œς™ςLš
œ˜Lšœ
œœ
œ˜Lš
œ˜Lš
œ˜Lšœ
œ
˜	Lš
œ˜Lš
œ"˜"Lš
œ˜Lš
œ"˜"šœ
œœ
œ˜Lš
œ˜Lš
œ˜Lš
œ˜Lš
œ˜—Lšœ
œ
˜	Lšœ ˜Lš
œ
˜
—šŸ	œœ
œœ˜JLš
œ„™„Lšœœ
˜Lš
œ˜Lš
œ˜Lš
œ˜Lš
œ˜Lš
œ˜Lš
œ˜Lš
œ˜Lš
œ˜Lš
œ˜Lš
œ'˜'Lš
œB˜BLš
œ'˜'Lš
œB˜BLš
œ(˜(Lšœ ˜—L˜šŸœœ
œœœœ˜1Lšœ
œœœ˜ Lš
œ˜—L˜šŸœœ
œœ˜Vš
œΖ™ΖLšœœ
˜Lš
œ˜Lš
œ˜šœœ
œ˜Lš
œ˜Lš
œ˜—Lšœ
˜Lš
œ˜Lš
œ˜Lš
œ˜Lš
œ˜Lš
œ˜Lš
œ!˜!Lš
œ'˜'Lš
œ?˜?Lš
œ
˜
Lš
œ'˜'Lš
œ@˜@šœœ
œ˜Lš
œ˜Lš
œ˜Lš
œ˜Lš
œ˜—Lšœ
˜Lšœ ˜"——šŸœœ
œœ˜TLš
œΟ™ΟLš
œI˜ILšœœ
˜Lš
œ˜šœœ
œ˜Lš
œ˜Lš
œ˜Lš
œ˜—Lšœ
˜Lš
œ˜Lš
œ˜Lš
œ˜šœœ
œ˜Lš
œ$˜$Lš
œ$˜$Lš
œ$˜$Lš
œ˜—Lšœ
˜Lšœ ˜ —šŸœœ
œœ˜WLš
œ‡
™‡
Lš
œ˜šœœ
œ˜Lš
œ˜—Lšœ
˜Lš
œ˜L˜—šŸ	œœ
œœœœ˜4Lšœœ
˜Lš
œ˜Lš
œ
œœ
œ
œ
˜-Lšœ
œ
œ˜L˜L˜—šŸœœ
œœ˜JJ™>Jšœœ
˜šœœ
œ˜Jš
œ˜Jšœœœœ˜C—Jšœ
˜J˜J˜—šŸœœ
œœ˜NJ™>Jšœœ
˜šœœ
œ˜šœœ
œ˜Jš
œ˜Jšœœœœ˜I—Jš
˜—Jšœ
˜J˜J˜—šŸœœ
œœœœ˜4Jš
œ8™8Jšœœ
œ˜%Jšœ
œ˜J˜—L˜Lšœ˜—…—D.u§