| /*************************************************************************/ |
| /* */ |
| /* Copyright (c) 1994 Stanford University */ |
| /* */ |
| /* All rights reserved. */ |
| /* */ |
| /* Permission is given to use, copy, and modify this software for any */ |
| /* non-commercial purpose as long as this copyright notice is not */ |
| /* removed. All other uses, including redistribution in whole or in */ |
| /* part, are forbidden without prior written permission. */ |
| /* */ |
| /* This software is provided with absolutely no warranty and no */ |
| /* support. */ |
| /* */ |
| /*************************************************************************/ |
| |
| |
| /* |
| * NAME |
| * matrix.c |
| * |
| * DESCRIPTION |
| * This file contains routines for matrix math, including common graphics |
| * related matrix operations, and also some vector operations. |
| */ |
| |
| |
| #include <stdio.h> |
| #include <math.h> |
| #include "rt.h" |
| |
| |
| |
| typedef REAL GJMATRIX[4][8]; /* Matrix for Gauss-Jordan inversion.*/ |
| |
| |
| |
| /* |
| * NAME |
| * VecNorm - normalize vector to unity length |
| * |
| * SYNOPSIS |
| * VOID VecNorm(V) |
| * Point V; // Vector to normalize. |
| * |
| * RETURNS |
| * Nothing. |
| */ |
| |
| VOID VecNorm(V) |
| POINT V; |
| { |
| REAL l; |
| |
| l = VecLen(V); |
| if (l > 0.0000001) |
| { |
| V[0] /= l; |
| V[1] /= l; |
| V[2] /= l; |
| } |
| } |
| |
| |
| |
| /* |
| * NAME |
| * VecMatMult - multiply a vector by a matrix |
| * |
| * SYNOPSIS |
| * VOID VecMatMult(Vt, M, V) |
| * POINT Vt; // Transformed vector. |
| * MATRIX M; // Transformation matrix. |
| * POINT V; // Input vector. |
| * |
| * RETURNS |
| * Nothing. |
| */ |
| |
| VOID VecMatMult(Vt, M, V) |
| POINT Vt; |
| MATRIX M; |
| POINT V; |
| { |
| INT i, j; |
| POINT tvec; |
| |
| /* tvec = M * V */ |
| |
| for (i = 0; i < 4; i++) |
| { |
| tvec[i] = 0.0; |
| for (j = 0; j < 4; j++) |
| tvec[i] += V[j] * M[j][i]; |
| } |
| |
| /* copy tvec to Vt */ |
| |
| for (i = 0; i < 4; i++) |
| Vt[i] = tvec[i]; |
| } |
| |
| |
| |
| /* |
| * NAME |
| * PrintMatrix - print values from matrix to stdout |
| * |
| * SYNOPSIS |
| * VOID PrintMatrix(M, s) |
| * MATRIX M; // Matrix to print. |
| * CHAR *s; // Title string. |
| * |
| * RETURNS |
| * Nothing. |
| */ |
| |
| VOID PrintMatrix(M, s) |
| MATRIX M; |
| CHAR *s; |
| { |
| INT i, j; |
| |
| printf("\n%s\n", s); |
| |
| for (i = 0; i < 4; i++) |
| { |
| printf("\t"); |
| |
| for (j = 0; j < 4; j++) |
| printf("%f ", M[i][j]); |
| |
| printf("\n"); |
| } |
| } |
| |
| |
| |
| /* |
| * NAME |
| * MatrixIdentity - set a matrix to the identity matrix |
| * |
| * SYNOPSIS |
| * VOID MatrixIdentity(M) |
| * MATRIX M; |
| * |
| * RETURNS |
| * Nothing. |
| */ |
| |
| VOID MatrixIdentity(M) |
| MATRIX M; |
| { |
| INT i, j; |
| |
| for (i = 0; i < 4; i++) |
| for (j = 0; j < 4; j++) |
| M[i][j] = 0.0; |
| |
| M[0][0] = 1.0; |
| M[1][1] = 1.0; |
| M[2][2] = 1.0; |
| M[3][3] = 1.0; |
| } |
| |
| |
| |
| /* |
| * NAME |
| * MatrixCopy - copy one matrix to another |
| * |
| * SYNOPSIS |
| * VOID MatrixCopy(A, B) |
| * MATRIX A; // Destination matrix. |
| * MATRIX B; // Source matrix. |
| * |
| * RETURNS |
| * Nothing. |
| */ |
| |
| VOID MatrixCopy(A, B) |
| MATRIX A, B; |
| { |
| INT i, j; |
| |
| for (i = 0; i < 4; i++) |
| for (j = 0; j < 4; j++) |
| A[i][j] = B[i][j]; |
| } |
| |
| |
| |
| /* |
| * NAME |
| * MatrixTranspose - transpose the elements of a matrix |
| * |
| * SYNOPSIS |
| * VOID MatrixTranspose(MT, M) |
| * MATRIX MT; // Transposed matrix. |
| * MATRIX M; // Original matrix. |
| * |
| * RETURNS |
| * Nothing. |
| */ |
| |
| VOID MatrixTranspose(MT, M) |
| MATRIX MT; |
| MATRIX M; |
| { |
| INT i, j; |
| MATRIX tmp; |
| |
| for (i = 0; i < 4; i++) |
| for (j = 0; j < 4; j++) |
| tmp[j][i] = M[i][j]; |
| |
| MatrixCopy(MT, tmp); |
| } |
| |
| |
| |
| /* |
| * NAME |
| * MatrixMult - multiply two matrices |
| * |
| * SYNOPSIS |
| * VOID MatrixMult(C, A, B) |
| * MATRIX C, A, B; // C = A*B |
| * |
| * RETURNS |
| * Nothing. |
| */ |
| |
| VOID MatrixMult(C, A, B) |
| MATRIX C, A, B; |
| { |
| INT i, j, k; |
| MATRIX T; /* Temporary matrix. */ |
| |
| /* T = A*B */ |
| |
| for (i = 0; i < 4; i++) |
| for (j = 0; j < 4; j++) |
| { |
| T[i][j] = 0.0; |
| for (k = 0; k < 4; k++) |
| T[i][j] += A[i][k] * B[k][j]; |
| } |
| |
| /* copy T to C */ |
| |
| for (i = 0; i < 4; i++) |
| for (j = 0; j < 4; j++) |
| C[i][j] = T[i][j]; |
| } |
| |
| |
| |
| /* |
| * NAME |
| * MatrixInverse - invert matrix using Gaussian elimination with partial pivoting |
| * |
| * SYNOPSIS |
| * VOID MatrixInverse(Minv, Mat) |
| * MATRIX Minv; // Inverted matrix. |
| * MATRIX Mat; // Original matrix. |
| * |
| * RETURNS |
| * Nothing. |
| */ |
| |
| VOID MatrixInverse(Minv, Mat) |
| MATRIX Minv; |
| MATRIX Mat; |
| { |
| INT i, j, k; /* Indices. */ |
| GJMATRIX gjmat; /* Inverse calculator. */ |
| REAL tbuf[8]; /* Row holder. */ |
| REAL pval, aval; /* Pivot candidates. */ |
| INT prow; /* Pivot row number. */ |
| REAL c; /* Pivot scale factor. */ |
| MATRIX tmp; |
| |
| for (i = 0; i < 4; i++) |
| for (j = 0; j < 4; j++) |
| gjmat[i][j] = Mat[i][j]; |
| |
| |
| k = 0; |
| for (i = 4; i < 8; i++) |
| { |
| for (j = 4; j < 8; j++) |
| if (i == j) |
| gjmat[k][j] = 1.0; |
| else |
| gjmat[k][j] = 0.0; |
| k++ ; |
| } |
| |
| |
| /* Gaussian elimination. */ |
| |
| for (i = 0; i < 3; i++) |
| { |
| pval = ABS(gjmat[i][i]); |
| prow = i; |
| |
| for (j = i + 1; j < 4; j++) |
| { |
| aval = ABS(gjmat[j][i]); |
| if (aval > pval) |
| { |
| pval = aval; |
| prow = j; |
| } |
| } |
| |
| if (i != prow) |
| { |
| for (k = 0; k < 8; k++) |
| tbuf[k] = gjmat[i][k]; |
| |
| for (k = 0; k < 8; k++) |
| gjmat[i][k] = gjmat[prow][k]; |
| |
| for (k = 0; k < 8; k++) |
| gjmat[prow][k] = tbuf[k]; |
| } |
| |
| for (j = i + 1; j < 4; j++) |
| { |
| c = gjmat[j][i] / gjmat[i][i]; |
| gjmat[j][i] = 0.0; |
| |
| for (k = i + 1; k < 8; k++) |
| gjmat[j][k] = gjmat[j][k] - c * gjmat[i][k]; |
| } |
| } |
| |
| |
| /* Zero columns */ |
| |
| for (i = 0; i < 3; i++) |
| for (j = i + 1; j < 4; j++) |
| { |
| c = gjmat[i][j] / gjmat[j][j]; |
| gjmat[i][j] = 0.0; |
| |
| for (k = j + 1; k < 8; k++) |
| gjmat[i][k] = gjmat[i][k] - c * gjmat[j][k]; |
| } |
| |
| |
| for (i = 0; i < 4; i++) |
| for (k = 4; k < 8; k++) /* normalize row */ |
| gjmat[i][k] /= gjmat[i][i]; |
| |
| |
| /* Generate inverse matrix. */ |
| |
| for (i = 0; i < 4; i++) |
| for (j = 4; j < 8; j++) |
| Minv[i][j - 4] = gjmat[i][j]; |
| |
| MatrixMult(tmp, Mat, Minv); |
| } |
| |
| |
| |
| /* |
| * NAME |
| * Translate - build translation matrix |
| * |
| * SYNOPSIS |
| * VOID Translate(M, dx, dy, dz) |
| * MATRIX M; // New matrix. |
| * REAL dx, dy, dz; // Translation values. |
| * |
| * RETURNS |
| * Nothing. |
| */ |
| |
| VOID Translate(M, dx, dy, dz) |
| MATRIX M; |
| REAL dx, dy, dz; |
| { |
| MatrixIdentity(M); |
| |
| M[3][0] = dx; |
| M[3][1] = dy; |
| M[3][2] = dz; |
| } |
| |
| |
| |
| /* |
| * NAME |
| * Scale - build scaling matrix |
| * |
| * SYNOPSIS |
| * VOID Scale(M, sx, sy, sz) |
| * MATRIX M; // New matrix. |
| * REAL sx, sy, sz; // Scaling factors. |
| * |
| * RETURNS |
| * Nothing. |
| */ |
| |
| VOID Scale(M, sx, sy, sz) |
| MATRIX M; |
| REAL sx, sy, sz; |
| { |
| MatrixIdentity(M); |
| |
| M[0][0] = sx; |
| M[1][1] = sy; |
| M[2][2] = sz; |
| } |
| |
| |
| |
| /* |
| * NAME |
| * Rotate - build rotation matrix |
| * |
| * SYNOPSIS |
| * VOID Rotate(axis, M, angle) |
| * INT axis; // Axis of rotation. |
| * MATRIX M; // New matrix. |
| * REAL angle; // Angle of rotation. |
| * |
| * RETURNS |
| * Nothing. |
| */ |
| |
| VOID Rotate(axis, M, angle) |
| INT axis; |
| MATRIX M; |
| REAL angle; |
| { |
| REAL cosangle; |
| REAL sinangle; |
| |
| MatrixIdentity(M); |
| |
| cosangle = cos(angle); |
| sinangle = sin(angle); |
| |
| switch (axis) |
| { |
| case X_AXIS: |
| M[1][1] = cosangle; |
| M[1][2] = sinangle; |
| M[2][1] = -sinangle; |
| M[2][2] = cosangle; |
| break; |
| |
| case Y_AXIS: |
| M[0][0] = cosangle; |
| M[0][2] = -sinangle; |
| M[2][0] = sinangle; |
| M[2][2] = cosangle; |
| break; |
| |
| case Z_AXIS: |
| M[0][0] = cosangle; |
| M[0][1] = sinangle; |
| M[1][0] = -sinangle; |
| M[1][1] = cosangle; |
| break; |
| |
| default: |
| printf("Unknown rotation axis %ld.\n", axis); |
| exit(5); |
| break; |
| } |
| } |