<> <> <> <> <> <<>> <> <> <> <<>> DIRECTORY Matrix3d, RealFns, Vector3d; Matrix3dImpl: CEDAR MONITOR IMPORTS RealFns, Vector3d EXPORTS Matrix3d ~ BEGIN OPEN Matrix3d; singular: PUBLIC ERROR ~ CODE; <> CopyMatrix: PUBLIC PROC [in: Matrix, out: Matrix _ NIL] RETURNS [Matrix] ~ { IF in = NIL THEN RETURN[NIL]; IF out = NIL THEN out _ NEW[MatrixRep]; out^ _ in^; RETURN[out]; }; CopyMatrixSequence: PUBLIC PROC [in: MatrixSequence] RETURNS [MatrixSequence] ~ { copy: MatrixSequence _ NIL; IF in # NIL THEN { copy _ NEW[MatrixSequenceRep[in.maxLength]]; FOR n: NAT IN [0..in.maxLength) DO copy[n] _ CopyMatrix[in[n]]; ENDLOOP; }; RETURN[copy]; }; Identity: PUBLIC PROC [out: Matrix _ NIL] RETURNS [Matrix] ~ { IF out = NIL THEN out _ NEW[MatrixRep]; out^ _ [[1.0, 0.0, 0.0, 0.0], [0.0, 1.0, 0.0, 0.0], [0.0, 0.0, 1.0, 0.0], [0.0, 0.0, 0.0, 1.0]]; RETURN[out]; }; MakeFromTriad: PUBLIC PROC [v1, v2, v3: Triple, p: Triple _ origin, out: Matrix _ NIL] RETURNS [Matrix] ~ { IF out = NIL THEN out _ NEW[MatrixRep]; v1 _ Vector3d.Normalize[v1]; v2 _ Vector3d.Normalize[v2]; v3 _ Vector3d.Normalize[v3]; out^ _ [[v1.x,v1.y,v1.z,0.0], [v2.x,v2.y,v2.z,0.0], [v3.x,v3.y,v3.z,0.0], [p.x,p.y,p.z,1.0]]; RETURN[out]; }; MakeScale: PUBLIC PROC [s: Triple, out: Matrix _ NIL] RETURNS [Matrix] ~ { IF out = NIL THEN out _ NEW[MatrixRep]; out^ _ [[s.x, 0.0, 0.0, 0.0], [0.0, s.y, 0.0, 0.0], [0.0, 0.0, s.z, 0.0], [0.0, 0.0, 0.0, 1.0]]; RETURN[out]; }; MakeTranslate: PUBLIC PROC [p: Triple, out: Matrix _ NIL] RETURNS [Matrix] ~ { IF out = NIL THEN out _ NEW[MatrixRep]; out^ _ [[1.0, 0.0, 0.0, 0.0], [0.0, 1.0, 0.0, 0.0], [0.0, 0.0, 1.0, 0.0], [p.x, p.y, p.z, 1.0]]; RETURN[out]; }; MakePureRotate: PUBLIC PROC [axis: Triple, theta: REAL, degrees: BOOL _ TRUE, out: Matrix _ NIL] RETURNS [Matrix] ~ { sin: REAL _ IF degrees THEN RealFns.SinDeg[theta] ELSE RealFns.Sin[theta]; cos: REAL _ IF degrees THEN RealFns.CosDeg[theta] ELSE RealFns.Cos[theta]; cos1, sqx, sqy, sqz, xycos1, zxcos1, yzcos1, xsin, ysin, zsin: REAL; axis _ Vector3d.Normalize[axis]; [[sqx, sqy, sqz]] _ Vector3d.MulVectors[axis, axis]; cos1 _ 1.0-cos; xycos1 _ axis.x*axis.y*cos1; xsin _ axis.x*sin; yzcos1 _ axis.y*axis.z*cos1; ysin _ axis.y*sin; zxcos1 _ axis.x*axis.z*cos1; zsin _ axis.z*sin; IF out = NIL THEN out _ NEW[MatrixRep]; out^ _ [ [sqx+(1.0-sqx)*cos, xycos1-zsin, zxcos1+ysin, 0.0], [xycos1+zsin, sqy+(1.0-sqy)*cos, yzcos1-xsin, 0.0], [zxcos1-ysin, yzcos1+xsin, sqz+(1.0-sqz)*cos, 0.0], [0.0, 0.0, 0.0, 1.0]]; RETURN[out]; }; MakeRotate: PUBLIC PROC [axis: Triple, theta: REAL, degrees: BOOL _ TRUE, base: Triple _ origin, out: Matrix _ NIL] RETURNS [Matrix] ~ { rot: Matrix _ ObtainMatrix[]; tran: Matrix _ ObtainMatrix[]; mul: Matrix _ ObtainMatrix[]; out _ Translate[ Mul[ MakeTranslate[[-base.x, -base.y, -base.z], tran], MakePureRotate[axis, theta, degrees, rot], mul], base, out]; ReleaseMatrix[rot]; ReleaseMatrix[tran]; ReleaseMatrix[mul]; RETURN[out]; }; MakePerspective: PUBLIC PROC [dInv, fInv, fov: REAL, out: Matrix _ NIL] RETURNS [Matrix] ~ { tan: REAL _ RealFns.TanDeg[0.5*fov]; out _ Identity[out]; out[2][2] _ tan*(1.0+fInv/MAX[0.0001, ABS[dInv]]); out[2][3] _ tan; RETURN[out]; }; HasPerspective: PUBLIC PROC [mat: Matrix] RETURNS [BOOL] ~ { IF mat = NIL THEN RETURN[FALSE]; RETURN[mat[2][3] # 0.0]; }; <> TransformH: PUBLIC PROC [p: Triple, mat: Matrix] RETURNS [Quad] ~ { RETURN[[ p.x*mat[0][0]+p.y*mat[1][0]+p.z*mat[2][0]+mat[3][0], p.x*mat[0][1]+p.y*mat[1][1]+p.z*mat[2][1]+mat[3][1], p.x*mat[0][2]+p.y*mat[1][2]+p.z*mat[2][2]+mat[3][2], p.x*mat[0][3]+p.y*mat[1][3]+p.z*mat[2][3]+mat[3][3]]]; }; <> <> <> <<>> Transform: PUBLIC PROC [p: Triple, mat: Matrix] RETURNS [Triple] ~ { q: Quad; IF NOT HasPerspective[mat] THEN RETURN[[ p.x*mat[0][0]+p.y*mat[1][0]+p.z*mat[2][0]+mat[3][0], p.x*mat[0][1]+p.y*mat[1][1]+p.z*mat[2][1]+mat[3][1], p.x*mat[0][2]+p.y*mat[1][2]+p.z*mat[2][2]+mat[3][2]]]; q _ TransformH[p, mat]; RETURN[IF q.w = 1.0 THEN [q.x, q.y, q.z] ELSE [q.x/q.w, q.y/q.w, q.z/q.w]]; }; TransformD: PUBLIC PROC [p: Triple, mat: Matrix] RETURNS [Pair] ~ { w: REAL; pp: Pair _ [ p.x*mat[0][0]+p.y*mat[1][0]+p.z*mat[2][0]+mat[3][0], p.x*mat[0][1]+p.y*mat[1][1]+p.z*mat[2][1]+mat[3][1]]; IF NOT HasPerspective[mat] THEN RETURN[pp]; w _ p.x*mat[0][3]+p.y*mat[1][3]+p.z*mat[2][3]+mat[3][3]; RETURN[IF w = 1.0 THEN pp ELSE [pp.x/w, pp.y/w]]; }; TransformPair: PUBLIC PROC [p: Pair, mat: Matrix] RETURNS [Triple] ~ { w: REAL; t: Triple _ [ p.x*mat[0][0]+p.y*mat[1][0]+mat[3][0], p.x*mat[0][1]+p.y*mat[1][1]+mat[3][1], p.x*mat[0][2]+p.y*mat[1][2]+mat[3][2]]; IF NOT HasPerspective[mat] THEN RETURN[t]; w _ p.x*mat[0][3]+p.y*mat[1][3]+mat[3][3]; RETURN[IF w = 1.0 THEN t ELSE [t.x/w, t.y/w, t.z/w]]; }; TransformPairD: PUBLIC PROC [p: Pair, mat: Matrix] RETURNS [Pair] ~ { w: REAL; pp: Pair _ [p.x*mat[0][0]+p.y*mat[1][0]+mat[3][0], p.x*mat[0][1]+p.y*mat[1][1]+mat[3][1]]; IF NOT HasPerspective[mat] THEN RETURN[pp]; w _ p.x*mat[0][3]+p.y*mat[1][3]+mat[3][3]; RETURN[IF w = 1.0 THEN pp ELSE [pp.x/w, pp.y/w]]; }; <> <> TransformVec: PUBLIC PROC [vec: Triple, mat: Matrix] RETURNS [Triple] ~ { RETURN[[ vec.x*mat[0][0]+vec.y*mat[1][0]+vec.z*mat[2][0], vec.x*mat[0][1]+vec.y*mat[1][1]+vec.z*mat[2][1], vec.x*mat[0][2]+vec.y*mat[1][2]+vec.z*mat[2][2]]]; }; TransformVecDiffS: PUBLIC PROC [vec: Triple, mat: Matrix] RETURNS [Triple] ~ { RETURN[Transform[vec, Cofactors[mat]]]; -- or check if sum of squares of rows are all equal. }; <> <> Scale: PUBLIC PROC [in: Matrix, s: REAL, out: Matrix _ NIL] RETURNS [Matrix] ~ { scratch: Matrix _ ObtainMatrix[]; out _ Mul[in, MakeScale[[s, s, s], scratch], out]; ReleaseMatrix[scratch]; RETURN[out]; }; <<>> DiffScale: PUBLIC PROC [in: Matrix, s: Triple, out: Matrix _ NIL] RETURNS [Matrix] ~ { scratch: Matrix _ ObtainMatrix[]; out _ Mul[in, MakeScale[s, scratch], out]; ReleaseMatrix[scratch]; RETURN[out]; }; <<>> Rotate: PUBLIC PROC [in: Matrix, axis: Triple, theta: REAL, degrees: BOOL _ TRUE, base: Triple _ origin, out: Matrix _ NIL] RETURNS [Matrix] ~ { scratch: Matrix _ ObtainMatrix[]; out _ Mul[in, MakeRotate[axis, theta, degrees, base, scratch], out]; ReleaseMatrix[scratch]; RETURN[out]; }; Translate: PUBLIC PROC [in: Matrix, p: Triple, out: Matrix _ NIL] RETURNS [Matrix] ~ { scratch: Matrix _ ObtainMatrix[]; out _ Mul[in, MakeTranslate[p, scratch], out]; ReleaseMatrix[scratch]; RETURN[out]; }; <<>> <> LocalScale: PUBLIC PROC [in: Matrix, s: REAL, out: Matrix _ NIL] RETURNS [Matrix] ~ { scratch: Matrix _ ObtainMatrix[]; out _ Mul[MakeScale[[s, s, s], scratch], in, out]; ReleaseMatrix[scratch]; RETURN[out]; }; LocalDiffScale: PUBLIC PROC [in: Matrix, s: Triple, out: Matrix _ NIL] RETURNS [Matrix] ~ { scratch: Matrix _ ObtainMatrix[]; out _ Mul[MakeScale[s, scratch], in, out]; ReleaseMatrix[scratch]; RETURN[out]; }; LocalRotate: PUBLIC PROC [in: Matrix, axis: Triple, theta: REAL, degrees: BOOL _ TRUE, base: Triple _ origin, out: Matrix _ NIL] RETURNS [Matrix] ~ { scratch: Matrix _ ObtainMatrix[]; out _ Mul[MakeRotate[axis, theta, degrees, base, scratch], in, out]; ReleaseMatrix[scratch]; RETURN[out]; }; LocalTranslate: PUBLIC PROC [in: Matrix, p: Triple, out: Matrix _ NIL] RETURNS [Matrix] ~ { scratch: Matrix _ ObtainMatrix[]; out _ Mul[MakeTranslate[p, scratch], in]; ReleaseMatrix[scratch]; RETURN[out]; }; <> ScalarMul: PROC [in: Matrix, s: REAL, out: Matrix _ NIL] RETURNS [Matrix] ~ { IF out = NIL THEN out _ NEW[MatrixRep]; FOR i: Row IN Row DO FOR j: Col IN Col DO out^[i][j] _ s*in[i][j]; ENDLOOP; ENDLOOP; RETURN[out]; }; <> <> <> <> <> <> <> <> <<};>> <<>> Mul: PUBLIC PROC [left, rite: Matrix, out: Matrix _ NIL] RETURNS [Matrix] ~ { ret: MatrixRep; IF left = NIL OR rite = NIL THEN RETURN[out]; ret[0][0] _ left[0][0]*rite[0][0]+left[0][1]*rite[1][0]+left[0][2]*rite[2][0]+left[0][3]*rite[3][0]; ret[0][1] _ left[0][0]*rite[0][1]+left[0][1]*rite[1][1]+left[0][2]*rite[2][1]+left[0][3]*rite[3][1]; ret[0][2] _ left[0][0]*rite[0][2]+left[0][1]*rite[1][2]+left[0][2]*rite[2][2]+left[0][3]*rite[3][2]; ret[0][3] _ left[0][0]*rite[0][3]+left[0][1]*rite[1][3]+left[0][2]*rite[2][3]+left[0][3]*rite[3][3]; ret[1][0] _ left[1][0]*rite[0][0]+left[1][1]*rite[1][0]+left[1][2]*rite[2][0]+left[1][3]*rite[3][0]; ret[1][1] _ left[1][0]*rite[0][1]+left[1][1]*rite[1][1]+left[1][2]*rite[2][1]+left[1][3]*rite[3][1]; ret[1][2] _ left[1][0]*rite[0][2]+left[1][1]*rite[1][2]+left[1][2]*rite[2][2]+left[1][3]*rite[3][2]; ret[1][3] _ left[1][0]*rite[0][3]+left[1][1]*rite[1][3]+left[1][2]*rite[2][3]+left[1][3]*rite[3][3]; ret[2][0] _ left[2][0]*rite[0][0]+left[2][1]*rite[1][0]+left[2][2]*rite[2][0]+left[2][3]*rite[3][0]; ret[2][1] _ left[2][0]*rite[0][1]+left[2][1]*rite[1][1]+left[2][2]*rite[2][1]+left[2][3]*rite[3][1]; ret[2][2] _ left[2][0]*rite[0][2]+left[2][1]*rite[1][2]+left[2][2]*rite[2][2]+left[2][3]*rite[3][2]; ret[2][3] _ left[2][0]*rite[0][3]+left[2][1]*rite[1][3]+left[2][2]*rite[2][3]+left[2][3]*rite[3][3]; ret[3][0] _ left[3][0]*rite[0][0]+left[3][1]*rite[1][0]+left[3][2]*rite[2][0]+left[3][3]*rite[3][0]; ret[3][1] _ left[3][0]*rite[0][1]+left[3][1]*rite[1][1]+left[3][2]*rite[2][1]+left[3][3]*rite[3][1]; ret[3][2] _ left[3][0]*rite[0][2]+left[3][1]*rite[1][2]+left[3][2]*rite[2][2]+left[3][3]*rite[3][2]; ret[3][3] _ left[3][0]*rite[0][3]+left[3][1]*rite[1][3]+left[3][2]*rite[2][3]+left[3][3]*rite[3][3]; IF out = NIL THEN out _ NEW[MatrixRep]; out^ _ ret; RETURN[out]; }; Cat: PUBLIC PROC [m1, m2: Matrix, m3, m4, m5, m6, out: Matrix _ NIL] RETURNS [Matrix] ~ { IF out = NIL THEN out _ NEW[MatrixRep]; out _ Mul[m1, m2, out]; IF m3 # NIL THEN out _ Mul[out, m3, out]; IF m4 # NIL THEN out _ Mul[out, m4, out]; IF m5 # NIL THEN out _ Mul[out, m5, out]; IF m6 # NIL THEN out _ Mul[out, m6, out]; RETURN[out]; }; Invert: PUBLIC PROC [in: Matrix, out: Matrix _ NIL] RETURNS [Matrix] ~ { det: REAL _ Determinant[in]; scratch: Matrix; IF det = 0.0 THEN ERROR singular; scratch _ ObtainMatrix[]; out _ ScalarMul[Adjoint[in, scratch], 1.0/det, out]; ReleaseMatrix[scratch]; RETURN[out]; }; Adjoint: PUBLIC PROC [in: Matrix, out: Matrix _ NIL] RETURNS [Matrix] ~ { scratch: Matrix _ ObtainMatrix[]; out _ Transpose[Cofactors[in, scratch], out]; ReleaseMatrix[scratch]; RETURN[out]; <> }; Det3x3: PROC [a0, b0, c0, a1, b1, c1, a2, b2, c2: REAL] RETURNS [REAL] ~ { RETURN[a0*(b1*c2-c1*b2)-b0*(a1*c2-c1*a2)+c0*(a1*b2-b1*a2)]; }; Cofactors: PUBLIC PROC [in: Matrix, out: Matrix _ NIL] RETURNS [Matrix] ~ { m: Matrix _ in; IF out = NIL THEN out _ NEW[MatrixRep]; out[0][0] _ Det3x3[m[1][1],m[1][2],m[1][3],m[2][1],m[2][2],m[2][3],m[3][1],m[3][2],m[3][3]]; out[0][1] _ -Det3x3[m[1][0],m[1][2],m[1][3],m[2][0],m[2][2],m[2][3],m[3][0],m[3][2],m[3][3]]; out[0][2] _ Det3x3[m[1][0],m[1][1],m[1][3],m[2][0],m[2][1],m[2][3],m[3][0],m[3][1],m[3][3]]; out[0][3] _ -Det3x3[m[1][0],m[1][1],m[1][2],m[2][0],m[2][1],m[2][2],m[3][0],m[3][1],m[3][2]]; out[1][0] _ -Det3x3[m[0][1],m[0][2],m[0][3],m[2][1],m[2][2],m[2][3],m[3][1],m[3][2],m[3][3]]; out[1][1] _ Det3x3[m[0][0],m[0][2],m[0][3],m[2][0],m[2][2],m[2][3],m[3][0],m[3][2],m[3][3]]; out[1][2] _ -Det3x3[m[0][0],m[0][1],m[0][3],m[2][0],m[2][1],m[2][3],m[3][0],m[3][1],m[3][3]]; out[1][3] _ Det3x3[m[0][0],m[0][1],m[0][2],m[2][0],m[2][1],m[2][2],m[3][0],m[3][1],m[3][2]]; out[2][0] _ Det3x3[m[0][1],m[0][2],m[0][3],m[1][1],m[1][2],m[1][3],m[3][1],m[3][2],m[3][3]]; out[2][1] _ -Det3x3[m[0][0],m[0][2],m[0][3],m[1][0],m[1][2],m[1][3],m[3][0],m[3][2],m[3][3]]; out[2][2] _ Det3x3[m[0][0],m[0][1],m[0][3],m[1][0],m[1][1],m[1][3],m[3][0],m[3][1],m[3][3]]; out[2][3] _ -Det3x3[m[0][0],m[0][1],m[0][2],m[1][0],m[1][1],m[1][2],m[3][0],m[3][1],m[3][2]]; out[3][0] _ -Det3x3[m[0][1],m[0][2],m[0][3],m[1][1],m[1][2],m[1][3],m[2][1],m[2][2],m[2][3]]; out[3][1] _ Det3x3[m[0][0],m[0][2],m[0][3],m[1][0],m[1][2],m[1][3],m[2][0],m[2][2],m[2][3]]; out[3][2] _ -Det3x3[m[0][0],m[0][1],m[0][3],m[1][0],m[1][1],m[1][3],m[2][0],m[2][1],m[2][3]]; out[3][3] _ Det3x3[m[0][0],m[0][1],m[0][2],m[1][0],m[1][1],m[1][2],m[2][0],m[2][1],m[2][2]]; RETURN[out]; }; Determinant: PUBLIC PROC [m: Matrix] RETURNS [REAL] ~ { RETURN[ m[0][0]*Det3x3[m[1][1],m[1][2],m[1][3],m[2][1],m[2][2],m[2][3],m[3][1],m[3][2],m[3][3]] - m[0][1]*Det3x3[m[1][0],m[1][2],m[1][3],m[2][0],m[2][2],m[2][3],m[3][0],m[3][2],m[3][3]] + m[0][2]*Det3x3[m[1][0],m[1][1],m[1][3],m[2][0],m[2][1],m[2][3],m[3][0],m[3][1],m[3][3]] - m[0][3]*Det3x3[m[1][0],m[1][1],m[1][2],m[2][0],m[2][1],m[2][2],m[3][0],m[3][1],m[3][2]]]; }; Transpose: PUBLIC PROC [in: Matrix, out: Matrix _ NIL] RETURNS [Matrix] ~ { IF out = NIL THEN out _ NEW[MatrixRep]; FOR i: Row IN Row DO FOR j: Col IN Col DO out[i][j] _ in[j][i]; ENDLOOP; ENDLOOP; RETURN[out]; }; InTermsOf: PUBLIC PROC [A, B: Matrix, out: Matrix _ NIL] RETURNS [Matrix] ~ { scratch: Matrix _ ObtainMatrix[]; out _ Invert[A, scratch]; out _ Mul[scratch, B, out]; ReleaseMatrix[scratch]; RETURN[out]; }; Invert3x3: PUBLIC PROC [m: Matrix, out: Matrix _ NIL] RETURNS [Matrix] ~ { d: REAL _ Det3x3[m[0][0],m[0][1],m[0][2],m[1][0],m[1][1],m[1][2],m[2][0],m[2][1],m[2][2]]; IF out = NIL THEN out _ NEW[MatrixRep]; out[0][0] _ m[1][1]*m[2][2]-m[1][2]*m[2][1]; out[0][1] _ m[0][2]*m[2][1]-m[0][1]*m[2][2]; out[0][2] _ m[0][1]*m[1][2]-m[0][2]*m[1][1]; out[1][0] _ m[1][2]*m[2][0]-m[1][0]*m[2][2]; out[1][1] _ m[0][0]*m[2][2]-m[0][2]*m[2][0]; out[1][2] _ m[0][2]*m[1][0]-m[0][0]*m[1][2]; out[2][0] _ m[1][0]*m[2][1]-m[1][1]*m[2][0]; out[2][1] _ m[0][1]*m[2][0]-m[0][0]*m[2][1]; out[2][2] _ m[0][0]*m[1][1]-m[0][1]*m[1][0]; IF d # 0.0 THEN { di: REAL _ 1.0/d; FOR i: NAT IN[0..2] DO FOR j: NAT IN[0..2] DO out[i][j] _ di*out[i][j]; ENDLOOP; ENDLOOP; }; RETURN[out]; }; <> nScratchMatrices: NAT ~ 6; scratchMatrices: ARRAY [0..nScratchMatrices) OF Matrix _ ALL[NIL]; ObtainMatrix: PUBLIC ENTRY PROC RETURNS [Matrix] ~ { FOR i: NAT IN [0..nScratchMatrices) DO matrix: Matrix ~ scratchMatrices[i]; IF matrix # NIL THEN { scratchMatrices[i] _ NIL; RETURN [matrix]; }; ENDLOOP; RETURN [NEW[MatrixRep]]; }; ReleaseMatrix: PUBLIC ENTRY PROC [matrix: Matrix] ~ { FOR i: NAT IN [0..nScratchMatrices) DO IF scratchMatrices[i] = NIL THEN { scratchMatrices[i] _ matrix; RETURN; }; ENDLOOP; }; END. .. <> by Crow: /Cyan/Imaging/6.0/ThreeDWorld/Linear3DImpl.mesa by Bier: /Cedar/CedarChest6.0/SolidModeler/Matrix3dImpl.mesa by Wyatt: /Cedar/Cedar6.0/Imager/ImagerTransformationImpl.mesa