/***************************************************************************** * General matrix manipulation routines. * ****************************************************************************** * (C) Gershon Elber, Technion, Israel Institute of Technology * ****************************************************************************** * Written by: Gershon Elber Ver 1.0, June 1988 * *****************************************************************************/ #include #include "irit_sm.h" #include "genmat.h" /***************************************************************************** * DESCRIPTION: M * Routine to generate a 4*4 unit matrix: M * * * PARAMETERS: M * Mat: Matrix to initialize as a unit matrix. M * * * RETURN VALUE: M * void M * * * KEYWORDS: M * MatGenUnitMat, transformations, unit matrix M *****************************************************************************/ void MatGenUnitMat(MatrixType Mat) { int i, j; for (i = 0; i < 4; i++) for (j = 0; j < 4; j++) if (i == j) Mat[i][j] = 1.0; else Mat[i][j] = 0.0; } /***************************************************************************** * DESCRIPTION: M * Routine to generate a 4*4 matrix to translate in Tx, Ty, Tz amounts. M * * * PARAMETERS: M * Tx, Ty, Tz: Translational amounts requested. M * Mat: Matrix to initialize as a translation matrix. M * * * RETURN VALUE: M * void M * * * KEYWORDS: M * MatGenMatTrans, transformations, translation M *****************************************************************************/ void MatGenMatTrans(RealType Tx, RealType Ty, RealType Tz, MatrixType Mat) { MatGenUnitMat(Mat); /* Make it unit matrix. */ Mat[3][0] = Tx; Mat[3][1] = Ty; Mat[3][2] = Tz; } /***************************************************************************** * DESCRIPTION: M * Routine to generate a 4*4 matrix to Scale x, y, z in Sx, Sy, Sz amounts. M * * * PARAMETERS: M * Sx, Sy, Sz: Scaling factors requested. M * Mat: Matrix to initialize as a scaling matrix. M * * * RETURN VALUE: M * void M * * * KEYWORDS: M * MatGenMatScale, transformations, scaling M *****************************************************************************/ void MatGenMatScale(RealType Sx, RealType Sy, RealType Sz, MatrixType Mat) { MatGenUnitMat(Mat); /* Make it unit matrix. */ Mat[0][0] = Sx; Mat[1][1] = Sy; Mat[2][2] = Sz; } /***************************************************************************** * DESCRIPTION: M * Routine to generate a 4*4 matrix to uniformly scale Scale amount. M * * * PARAMETERS: M * Scale: Uniform scaling factor requested. M * Mat: Matrix to initialize as a scaling matrix. M * * * RETURN VALUE: M * void M * * * KEYWORDS: M * MatGenMatUnifScale, transformations, scaling M *****************************************************************************/ void MatGenMatUnifScale(RealType Scale, MatrixType Mat) { MatGenMatScale(Scale, Scale, Scale, Mat); } /***************************************************************************** * DESCRIPTION: M * Routine to generate a 4*4 matrix to Rotate around the X axis by Teta M * radians. M * * * PARAMETERS: M * Teta: Amount of rotation, in radians. M * Mat: Matrix to initialize as a rotation matrix. M * * * RETURN VALUE: M * void M * * * KEYWORDS: M * MatGenMatRotX1, transformations, rotation M *****************************************************************************/ void MatGenMatRotX1(RealType Teta, MatrixType Mat) { RealType CTeta, STeta; CTeta = cos(Teta); STeta = sin(Teta); MatGenMatRotX(CTeta, STeta, Mat); } /***************************************************************************** * DESCRIPTION: M * Routine to generate a 4*4 matrix to Rotate around the X axis by Teta, M * given the sin and cosine of Teta M * * * PARAMETERS: M * SinTeta, CosTeta: Amount of rotation, given as sine and cosine of Teta. M * Mat: Matrix to initialize as a rotation matrix. M * * * RETURN VALUE: M * void M * * * KEYWORDS: M * MatGenMatRotX, transformations, rotation M *****************************************************************************/ void MatGenMatRotX(RealType CosTeta, RealType SinTeta, MatrixType Mat) { MatGenUnitMat(Mat); /* Make it unit matrix. */ Mat[1][1] = CosTeta; Mat[1][2] = SinTeta; Mat[2][1] = -SinTeta; Mat[2][2] = CosTeta; } /***************************************************************************** * DESCRIPTION: M * Routine to generate a 4*4 matrix to Rotate around the Y axis by Teta M * radians. M * * * PARAMETERS: M * Teta: Amount of rotation, in radians. M * Mat: Matrix to initialize as a rotation matrix. M * * * RETURN VALUE: M * void M * * * KEYWORDS: M * MatGenMatRotY1, transformations, rotation M *****************************************************************************/ void MatGenMatRotY1(RealType Teta, MatrixType Mat) { RealType CTeta, STeta; CTeta = cos(Teta); STeta = sin(Teta); MatGenMatRotY(CTeta, STeta, Mat); } /***************************************************************************** * DESCRIPTION: M * Routine to generate a 4*4 matrix to Rotate around the Y axis by Teta, M * given the sin and cosine of Teta M * * * PARAMETERS: M * SinTeta, CosTeta: Amount of rotation, given as sine and cosine of Teta. M * Mat: Matrix to initialize as a rotation matrix. M * * * RETURN VALUE: M * void M * * * KEYWORDS: M * MatGenMatRotY, transformations, rotation M *****************************************************************************/ void MatGenMatRotY(RealType CosTeta, RealType SinTeta, MatrixType Mat) { MatGenUnitMat(Mat); /* Make it unit matrix. */ Mat[0][0] = CosTeta; Mat[0][2] = -SinTeta; Mat[2][0] = SinTeta; Mat[2][2] = CosTeta; } /***************************************************************************** * DESCRIPTION: M * Routine to generate a 4*4 matrix to Rotate around the Z axis by Teta M * radians. M * * * PARAMETERS: M * Teta: Amount of rotation, in radians. M * Mat: Matrix to initialize as a rotation matrix. M * * * RETURN VALUE: M * void M * * * KEYWORDS: M * MatGenMatRotZ1, transformations, rotation M *****************************************************************************/ void MatGenMatRotZ1(RealType Teta, MatrixType Mat) { RealType CTeta, STeta; CTeta = cos(Teta); STeta = sin(Teta); MatGenMatRotZ(CTeta, STeta, Mat); } /***************************************************************************** * DESCRIPTION: M * Routine to generate a 4*4 matrix to Rotate around the Z axis by Teta, M * given the sin and cosine of Teta M * * * PARAMETERS: M * SinTeta, CosTeta: Amount of rotation, given as sine and cosine of Teta. M * Mat: Matrix to initialize as a rotation matrix. M * * * RETURN VALUE: M * void M * * * KEYWORDS: M * MatGenMatRotZ, transformations, rotation M *****************************************************************************/ void MatGenMatRotZ(RealType CosTeta, RealType SinTeta, MatrixType Mat) { MatGenUnitMat(Mat); /* Make it unit matrix. */ Mat[0][0] = CosTeta; Mat[0][1] = SinTeta; Mat[1][0] = -SinTeta; Mat[1][1] = CosTeta; } /***************************************************************************** * DESCRIPTION: M * Routine to multiply 2 4by4 matrices. M * MatRes may be one of Mat1 or Mat2 - it is only updated in the end. M * * * PARAMETERS: M * MatRes: Result of matrix product. M * Mat1, Mat2: The two operand of the matrix product. M * * * RETURN VALUE: M * void M * * * KEYWORDS: M * MatMultTwo4by4, transformations, matrix product M *****************************************************************************/ void MatMultTwo4by4(MatrixType MatRes, MatrixType Mat1, MatrixType Mat2) { int i, j, k; MatrixType MatResTemp; for (i = 0; i < 4; i++) for (j = 0; j < 4; j++) { MatResTemp[i][j] = 0; for (k = 0; k < 4; k++) MatResTemp[i][j] += Mat1[i][k] * Mat2[k][j]; } for (i = 0; i < 4; i++) for (j = 0; j < 4; j++) MatRes[i][j] = MatResTemp[i][j]; } /***************************************************************************** * DESCRIPTION: M * Routine to add 2 4by4 matrices. M * MatRes may be one of Mat1 or Mat2. M * * * PARAMETERS: M * MatRes: Result of matrix addition. M * Mat1, Mat2: The two operand of the matrix addition. M * * * RETURN VALUE: M * void M * * * KEYWORDS: M * MatAddTwo4by4, transformations, matrix addition M *****************************************************************************/ void MatAddTwo4by4(MatrixType MatRes, MatrixType Mat1, MatrixType Mat2) { int i, j; for (i = 0; i < 4; i++) for (j = 0; j < 4; j++) MatRes[i][j] = Mat1[i][j] + Mat2[i][j]; } /***************************************************************************** * DESCRIPTION: M * Routine to subtract 2 4by4 matrices. M * MatRes may be one of Mat1 or Mat2. M * * * PARAMETERS: M * MatRes: Result of matrix subtraction. M * Mat1, Mat2: The two operand of the matrix subtraction. M * * * RETURN VALUE: M * void M * * * KEYWORDS: M * MatSubTwo4by4, transformations, matrix subtraction M *****************************************************************************/ void MatSubTwo4by4(MatrixType MatRes, MatrixType Mat1, MatrixType Mat2) { int i, j; for (i = 0; i < 4; i++) for (j = 0; j < 4; j++) MatRes[i][j] = Mat1[i][j] - Mat2[i][j]; } /***************************************************************************** * DESCRIPTION: M * Routine to scale a 4by4 matrix. M * MatRes may be Mat. M * * * PARAMETERS: M * MatRes: Result of matrix scaling. M * Mat: The two operand of the matrix scaling. M * Scale: Scalar value to multiple matrix with. M * * * RETURN VALUE: M * void M * * * KEYWORDS: M * MatScale4by4, transformations, matrix scaling M *****************************************************************************/ void MatScale4by4(MatrixType MatRes, MatrixType Mat, RealType *Scale) { int i, j; for (i = 0; i < 4; i++) for (j = 0; j < 4; j++) MatRes[i][j] = Mat[i][j] * (*Scale); } /***************************************************************************** * DESCRIPTION: M * Routine to multiply an XYZ Vector by 4by4 matrix: M * The Vector has only 3 components (X, Y, Z) and it is assumed that W = 1 M * VRes may be Vec as it is only updated in the end. M * * * PARAMETERS: M * VRes: Result of vector - matrix product. M * Vec: Vector to transfrom using Matrix. M * Mat: Transformation matrix. M * * * RETURN VALUE: M * void M * * * KEYWORDS: M * MatMultVecby4by4, transformations, vector matrix product M *****************************************************************************/ void MatMultVecby4by4(VectorType VRes, VectorType Vec, MatrixType Mat) { int i, j; RealType CalcW = Mat[3][3]; VectorType VTemp; for (i = 0; i < 3; i++) { VTemp[i] = Mat[3][i]; /* Initiate it with the weight factor. */ for (j = 0; j < 3; j++) VTemp[i] += Vec[j] * Mat[j][i]; } for (i = 0; i < 3; i++) CalcW += Vec[i] * Mat[i][3]; if (CalcW == 0) CalcW = 1 / IRIT_INFNTY; for (i = 0; i < 3; i++) VRes[i] = VTemp[i] / CalcW; } /***************************************************************************** * DESCRIPTION: M * Routine to multiply a WXYZ Vector by 4by4 matrix: M * The Vector has only 4 components (W, X, Y, Z). M * VRes may be Vec as it is only updated in the end. M * * * PARAMETERS: M * VRes: Result of vector - matrix product. M * Vec: Vector to transfrom using Matrix. M * Mat: Transformation matrix. M * * * RETURN VALUE: M * void M * * * KEYWORDS: M * MatMultWVecby4by4, transformations, vector matrix product M *****************************************************************************/ void MatMultWVecby4by4(RealType VRes[4], RealType Vec[4], MatrixType Mat) { int i, j; RealType VTemp[4]; for (i = 0; i < 4; i++) { VTemp[i] = 0.0; for (j = 0; j < 4; j++) VTemp[i] += Vec[j] * Mat[j][i]; } for (i = 0; i < 4; i++) VRes[i] = VTemp[i]; } /***************************************************************************** * DESCRIPTION: M * Routine to compute the INVERSE of a given matrix M which is not modified. M * The matrix is assumed to be 4 by 4 (transformation matrix). M * Return TRUE if inverted matrix (InvM) do exists. M * * * PARAMETERS: M * M: Original matrix to invert. M * InvM: Inverted matrix will be placed here. M * * * RETURN VALUE: M * int: TRUE if inverse exists, FALSE otherwise. M * * * KEYWORDS: M * MatInverseMatrix, transformations, matrix inverse M *****************************************************************************/ int MatInverseMatrix(MatrixType M, MatrixType InvM) { MatrixType A; int i, j, k; RealType V; MAT_COPY(A, M); /* Prepare temporary copy of M in A. */ MatGenUnitMat(InvM); /* Make it unit matrix. */ for (i = 0; i < 4; i++) { V = A[i][i]; /* Find the new pivot. */ k = i; for (j = i + 1; j < 4; j++) if (ABS(A[j][i]) > ABS(V)) { /* Find maximum on col i, row i+1..n */ V = A[j][i]; k = j; } j = k; if (i != j) for (k = 0; k < 4; k++) { SWAP(RealType, A[i][k], A[j][k]); SWAP(RealType, InvM[i][k], InvM[j][k]); } for (j = i + 1; j < 4; j++) { /* Eliminate col i from row i+1..n. */ V = A[j][i] / A[i][i]; for (k = 0; k < 4; k++) { A[j][k] -= V * A[i][k]; InvM[j][k] -= V * InvM[i][k]; } } } for (i = 3; i >= 0; i--) { /* Back Substitution. */ if (A[i][i] == 0) return FALSE; /* Error. */ for (j = 0; j < i; j++) { /* Eliminate col i from row 1..i-1. */ V = A[j][i] / A[i][i]; for (k = 0; k < 4; k++) { /* A[j][k] -= V * A[i][k]; */ InvM[j][k] -= V * InvM[i][k]; } } } for (i = 0; i < 4; i++) /* Normalize the inverse Matrix. */ for (j = 0; j < 4; j++) InvM[i][j] /= A[i][i]; return TRUE; } /***************************************************************************** * DESCRIPTION: M * Routine to estimate the scaling factor in a matrix by computing the M * average of the scale of the X, Y, and Z unit vectors. M * * * PARAMETERS: M * M: Matrix to estimate scaling factors. M * * * RETURN VALUE: M * RealType: Estimated Scaling factor. M * * * KEYWORDS: M * MatScaleFactorMatrix, transformations M *****************************************************************************/ RealType MatScaleFactorMatrix(MatrixType M) { RealType Scale = 0.0; VectorType V, VRes, VRef; V[0] = 0; V[1] = 0; V[2] = 0; MatMultVecby4by4(VRef, V, M); V[0] = 0; V[1] = 0; V[2] = 1; MatMultVecby4by4(VRes, V, M); PT_SUB(VRes, VRes, VRef); Scale += PT_LENGTH(VRes); V[0] = 0; V[1] = 1; V[2] = 0; MatMultVecby4by4(VRes, V, M); PT_SUB(VRes, VRes, VRef); Scale += PT_LENGTH(VRes); V[0] = 1; V[1] = 0; V[2] = 0; MatMultVecby4by4(VRes, V, M); PT_SUB(VRes, VRes, VRef); Scale += PT_LENGTH(VRes); return Scale / 3.0; }