<> <> <> <> DIRECTORY Matrix3d, RealFns, Real, SV2d, SV3d, SVVector3d; Matrix3dImpl: CEDAR PROGRAM IMPORTS RealFns, Real, SVVector3d EXPORTS Matrix3d = BEGIN <> <<| 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] = { <> 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] = { << 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] = { <> 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.