File: SVMatrix2dImpl.mesa
Last edited by Bier on June 1, 1984 4:41:36 pm PDT
Author: Eric Bier on June 26, 1984 11:24:49 am PDT
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.