<> <> <> <> DIRECTORY RealFns, SV2d, SVMatrix2d, SVVector2d; SVMatrix2dImpl: CEDAR PROGRAM IMPORTS RealFns, SVVector2d EXPORTS SVMatrix2d = BEGIN Matrix3by3: TYPE = ARRAY [1..3] OF ARRAY [1..3] OF REAL; Matrix2by2: TYPE = ARRAY [1..2] OF ARRAY [1..2] OF REAL; Point2d: TYPE = SV2d.Point2d; Vector2d: TYPE = SVVector2d.Vector2d; Identity: PUBLIC PROC [] RETURNS [identityMat: Matrix3by3] = { FOR i: NAT IN [1..3] DO FOR j: NAT IN [1..3] DO identityMat[i][j] _ IF i = j THEN 1 ELSE 0; ENDLOOP; ENDLOOP; }; Update: PUBLIC PROC [mat: Matrix3by3, point: Point2d] RETURNS [newPoint: Point2d] = { <<| x' | | (1,1) (1,2) (1,3) | | x |>> <<| y' | = | (2,1) (2,2) (2,3) | | y |>> <<| 1 | | 0 0 1 | | 1 |>> FOR i: NAT IN [1..2] DO newPoint[i] _ mat[i][1]*point[1] + mat[i][2]*point[2] + mat[i][3]; ENDLOOP; }; UpdateVector: PUBLIC PROC [mat: Matrix3by3, vec: Vector2d] RETURNS [newVec: Vector2d] = { <> FOR i: NAT IN [1..2] DO newVec[i] _ mat[i][1]*vec[1] + mat[i][2]*vec[2]; ENDLOOP; }; <> Scale: PUBLIC PROC [mat: Matrix3by3, sx, sy: REAL] RETURNS [transMat: Matrix3by3] = { FOR j: NAT IN[1..3] DO transMat[1][j] _ mat[1][j]*sx; ENDLOOP; FOR j: NAT IN[1..3] DO transMat[2][j] _ mat[2][j]*sy; ENDLOOP; transMat[3][1] _ transMat[3][2] _ 0.0; transMat[3][3] _ 1.0; }; Translate: PUBLIC PROC [mat: Matrix3by3, dx, dy: REAL] RETURNS [transMat: Matrix3by3] = { i, j: CARDINAL; FOR i IN [1..3] DO-- in the first two columns, the elements are copied FOR j IN [1..2] DO transMat[i][j] _ mat[i][j]; ENDLOOP; ENDLOOP; transMat[1][3] _ mat[1][3] + dx;-- add the translation offset to the last row transMat[2][3] _ mat[2][3] + dy; transMat[3][3] _ 1.0; }; RotateCCW: PUBLIC PROC [mat: Matrix3by3, degrees: REAL] RETURNS [transMat: Matrix3by3] = { sind: REAL _ RealFns.SinDeg[degrees]; cosd: REAL _ RealFns.CosDeg[degrees]; rotMat: Matrix3by3 _ Identity[]; rotMat[1][1] _ rotMat[2][2] _ cosd; rotMat[1][2] _ -sind; rotMat[2][1] _ sind; transMat _ MatMult[rotMat, mat]; }; <> LocalScale: PUBLIC PROC [mat: Matrix3by3, sx, sy: REAL] RETURNS [transMat: Matrix3by3] = { FOR i: NAT IN[1..2] DO transMat[i][1] _ mat[i][1]*sx; ENDLOOP; FOR i: NAT IN[1..2] DO transMat[i][2] _ mat[i][2]*sy; ENDLOOP; FOR i: NAT IN[1..2] DO transMat[i][3] _ mat[i][3]; ENDLOOP; transMat[3][1] _ transMat[3][2] _ 0.0; transMat[3][3] _ 1.0; }; LocalTranslate: PUBLIC PROC [mat: Matrix3by3, dx, dy: REAL] RETURNS [transMat: Matrix3by3] = { i, j: CARDINAL; FOR i IN [1..3] DO-- in the first two columns, the elements are copied FOR j IN [1..2] DO transMat[i][j] _ mat[i][j]; ENDLOOP; ENDLOOP; transMat[1][3] _ mat[1][1]*dx + mat[1][2]*dy + mat[1][3]; transMat[2][3] _ mat[2][1]*dx + mat[2][2]*dy + mat[2][3]; transMat[3][3] _ 1.0; }; LocalRotateCCW: PUBLIC PROC [mat: Matrix3by3, degrees: REAL] RETURNS [transMat: Matrix3by3] = { sind: REAL _ RealFns.SinDeg[degrees]; cosd: REAL _ RealFns.CosDeg[degrees]; rotMat: Matrix3by3 _ Identity[]; rotMat[1][1] _ rotMat[2][2] _ cosd; rotMat[1][2] _ -sind; rotMat[2][1] _ sind; transMat _ MatMult[mat, rotMat]; }; MatMult: PUBLIC PROC [left, right: Matrix3by3] RETURNS [transMat: Matrix3by3] = { <> i, j: CARDINAL; FOR i IN [1..2] DO-- The last row is always [0 0 1] FOR j IN [1..2] DO-- In the first two columns, the last element is always zero transMat[i][j] _ left[i][1]*right[1][j] + left[i][2]*right[2][j]; ENDLOOP; ENDLOOP; FOR i IN [1..2] DO-- calculate the last column transMat[i][3] _ left[i][1]*right[1][3] + left[i][2]*right[2][3] + left[i][3]; ENDLOOP; transMat[3][1] _ transMat[3][2] _ 0;-- Last row transMat[3][3] _ 1.0; }; MakeScaleMat: PUBLIC PROC [sx, sy: REAL] RETURNS [scale: Matrix3by3] = { scale _ Identity[]; scale[1][1] _ sx;scale[2][2] _ sy; }; MakeTranslateMat: PUBLIC PROC [dx, dy: REAL] RETURNS [trans: Matrix3by3] = { trans _ Identity[]; trans[1][3] _ dx;trans[2][3] _ dy; }; MakeRotateCCWMat: PUBLIC PROC [degrees: REAL] RETURNS [rot: Matrix3by3] = { sind: REAL _ RealFns.SinDeg[degrees]; cosd: REAL _ RealFns.CosDeg[degrees]; rot _ Identity[]; rot[1][1] _ rot[2][2] _ cosd; rot[1][2] _ -sind; rot[2][1] _ sind; }; MakeMatFromAxes: PUBLIC PROC [xAxis, yAxis: Vector2d, origin: Point2d] RETURNS [mat: Matrix3by3] = { FOR i: NAT IN[1..2] DO mat[i][1] _ xAxis[i]; mat[i][2] _ yAxis[i]; mat[i][3] _ origin[i]; ENDLOOP; mat[3][1] _ mat[3][2] _ 0; mat[3][3] _ 1; }; ScaleFromMatrix: PUBLIC PROC [mat: Matrix3by3] RETURNS [sx, sy: REAL] = { sx _ SVVector2d.Magnitude[[mat[1][1],mat[2][1]]]; sy _ SVVector2d.Magnitude[[mat[1][2],mat[2][2]]]; <> }; OriginOfMatrix: PUBLIC PROC [mat: Matrix3by3] RETURNS [origin: Point2d] = { origin[1] _ mat[1][3]; origin[2] _ mat[2][3]; }; XAxisOfMatrix: PUBLIC PROC [mat: Matrix3by3] RETURNS [xAxis: Vector2d] = { xAxis[1] _ mat[1][1]; xAxis[2] _ mat[2][1]; }; YAxisOfMatrix: PUBLIC PROC [mat: Matrix3by3] RETURNS [yAxis: Vector2d] = { yAxis[1] _ mat[1][2]; yAxis[2] _ mat[2][2]; }; RotationOfMatrix: PUBLIC PROC [mat: Matrix3by3] RETURNS [degrees: REAL] = { <> <> <> cos, sin: REAL; xAxis, yAxis: Vector2d; xAxis _ XAxisOfMatrix[mat]; yAxis _ YAxisOfMatrix[mat]; xAxis _ SVVector2d.Normalize[xAxis]; yAxis _ SVVector2d.Normalize[yAxis]; cos _ SVVector2d.DotProduct[xAxis, [1,0]]; sin _ - SVVector2d.DotProduct[yAxis, [1,0]]; degrees _ RealFns.ArcTanDeg[sin, cos]; }; Determinant: PUBLIC PROC [mat: Matrix3by3] 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]); }; Determinant2by2: PRIVATE PROC [mat: Matrix2by2] RETURNS [det: REAL] = { det _ mat[1][1]*mat[2][2] - mat[2][1]*mat[1][2]; }; Transpose: PUBLIC PROC [mat: Matrix3by3] RETURNS [matT: Matrix3by3] = { <> FOR i: NAT IN[1..3] DO FOR j: NAT IN[1..3] DO matT[i][j] _ mat[j][i]; ENDLOOP; ENDLOOP; }; Cofactor: PUBLIC PROC [mat: Matrix3by3, row, col: NAT] RETURNS [cof: REAL] = { <> rowCount, colCount: NAT; exponent: NAT; m2: Matrix2by2; rowCount _ 0; FOR i: NAT IN[1..3] DO IF i = row THEN LOOP; rowCount _ rowCount + 1; colCount _ 0; FOR j: NAT IN[1..3] DO IF j = col THEN LOOP; colCount _ colCount + 1; m2[rowCount][colCount] _ mat[i][j]; ENDLOOP; ENDLOOP; cof _ Determinant2by2[m2]; exponent _ row+col; IF (exponent/2)*2 # exponent THEN cof _ -cof; }; Inverse: PUBLIC PROC [mat: Matrix3by3] RETURNS [inverse: Matrix3by3] = { <> det: REAL _ Determinant[mat]; IF det = 0 THEN SIGNAL DegenerateInverse; FOR i: NAT IN[1..2] DO FOR j: NAT IN[1..3] DO inverse[i][j] _ Cofactor[mat,j,i]/det; -- transpose as you do it ENDLOOP; ENDLOOP; inverse[3][1] _ inverse[3][2] _ 0.0; inverse[3][3] _ 1.0; -- these would also be the calculated values if we did them in the loop }; DegenerateInverse: PUBLIC SIGNAL = CODE; WorldToLocal: PUBLIC PROC [AinWorld,BinWorld: Matrix3by3] RETURNS [BinTermsOfA: Matrix3by3] = { <(BinTermsOfA) = BinWorld;>> <> worldAInverse: Matrix3by3 _ Inverse[AinWorld]; BinTermsOfA _ MatMult[worldAInverse, BinWorld]; }; END.