-- File: SVMatrix2dImpl.mesa
-- Last edited by Bier on December 18, 1982 1:10 am
-- Author: Eric Bier on November 18, 1982 8:40 pm
-- Contents: My own package for manipulating Homogenous 3 x 3 matrices. Parallels Matrix3d in many places

DIRECTORY
 RealFns,
 SV2d,
 SVMatrix2d,
 SVVector2d;

SVMatrix2dImpl: 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] = {
 -- vectors can be rotated and scaled, but not translated
 FOR i: NAT IN [1..2] DO
  newVec[i] ← mat[i][1]*vec[1] + mat[i][2]*vec[2];
 ENDLOOP;
 };
 
-- The next three procedures perform an efficient matrix multiplication as though mat were being multiplied to its left (a transform in reference coordinates) by a matrix which translates or rotates respectively (See Paul).
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];
 };

-- The next three procedures perform an efficient matrix multiplication as though mat were being multiplied on its right (a transform in local coordinates) by a matrix which translates or rotates respectively (See Paul).
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] = {
-- MatMult is almost a general 3 by 3 matrix operation. However, it assumes that the last row of each matrix is [0 0 1].
 
 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]]];
 -- finds out how much this matrix will scale an object it is applied to
 };
 
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] = {
 -- take the dot product of normalized xAxis with [1,0]. This should be cosine.
 -- take the dot product of normalized yAxis with [1,0]. This should be - sine.
 -- Find the arctan.
 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] = {
 -- interchange rows and columns
 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] = {
 -- returns the determinant of the 2 by 2 matrix which results by crossing off row row and column col in mat.
 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] = {
 -- 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..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] = {
 -- clearly (AinWorld)<matmult>(BinTermsOfA) = BinWorld;
 -- Hence BinTermsOfA = (Inverse[AinWorld])(BinWorld);
 worldAInverse: Matrix3by3 ← Inverse[AinWorld];
 BinTermsOfA ← MatMult[worldAInverse, BinWorld];
 };

END.