File: SVMatrix2dImpl.mesa
Last edited by Bier on June 1, 1984 4:41:36 pm PDT
Author: Eric Bier on January 28, 1987 2:25:18 pm PST
Contents: My own package for manipulating Homogenous 3 x 3 matrices. Parallels SVMatrix3d in many places
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] = {
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.