home
***
CD-ROM
|
disk
|
FTP
|
other
***
search
/
Shareware 1 2 the Maxx
/
sw_1.zip
/
sw_1
/
CAD
/
IRITSM3S.ZIP
/
MISC_LIB
/
GENMAT.C
< prev
next >
Wrap
C/C++ Source or Header
|
1992-01-20
|
6KB
|
160 lines
/*****************************************************************************
* General matrix manipulation routines. *
* *
* Written by: Gershon Elber Ver 1.0, June 1988 *
*****************************************************************************/
#include <math.h>
#include "irit_sm.h"
#include "genmat.h"
/*****************************************************************************
* Routine to generate a 4*4 unit matrix: *
*****************************************************************************/
void GenUnitMat(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;
}
/*****************************************************************************
* Routine to generate a 4*4 matrix to translate in Tx, Ty, Tz amounts. *
*****************************************************************************/
void GenMatTrans(RealType Tx, RealType Ty, RealType Tz, MatrixType Mat)
{
GenUnitMat(Mat); /* Make it unit matrix. */
Mat[3][0] = Tx;
Mat[3][1] = Ty;
Mat[3][2] = Tz;
}
/*****************************************************************************
* Routine to generate a 4*4 matrix to Scale x, y, z in Sx, Sy, Sz amounts. *
*****************************************************************************/
void GenMatScale(RealType Sx, RealType Sy, RealType Sz, MatrixType Mat)
{
GenUnitMat(Mat); /* Make it unit matrix. */
Mat[0][0] = Sx;
Mat[1][1] = Sy;
Mat[2][2] = Sz;
}
/*****************************************************************************
* Routine to generate a 4*4 matrix to Rotate around X with angle of Teta. *
* Note Teta must be given in radians. *
*****************************************************************************/
void GenMatRotX1(RealType Teta, MatrixType Mat)
{
RealType CTeta, STeta;
CTeta = cos(Teta);
STeta = sin(Teta);
GenMatRotX(CTeta, STeta, Mat);
}
/*****************************************************************************
* Routine to generate a 4*4 matrix to Rotate around X with angle of Teta. *
*****************************************************************************/
void GenMatRotX(RealType CosTeta, RealType SinTeta, MatrixType Mat)
{
GenUnitMat(Mat); /* Make it unit matrix. */
Mat[1][1] = CosTeta;
Mat[1][2] = SinTeta;
Mat[2][1] = -SinTeta;
Mat[2][2] = CosTeta;
}
/*****************************************************************************
* Routine to generate a 4*4 matrix to Rotate around Y with angle of Teta. *
* Note Teta must be given in radians. *
*****************************************************************************/
void GenMatRotY1(RealType Teta, MatrixType Mat)
{
RealType CTeta, STeta;
CTeta = cos(Teta);
STeta = sin(Teta);
GenMatRotY(CTeta, STeta, Mat);
}
/*****************************************************************************
* Routine to generate a 4*4 matrix to Rotate around Y with angle of Teta. *
*****************************************************************************/
void GenMatRotY(RealType CosTeta, RealType SinTeta, MatrixType Mat)
{
GenUnitMat(Mat); /* Make it unit matrix. */
Mat[0][0] = CosTeta;
Mat[0][2] = -SinTeta;
Mat[2][0] = SinTeta;
Mat[2][2] = CosTeta;
}
/*****************************************************************************
* Routine to generate a 4*4 matrix to Rotate around Z with angle of Teta. *
* Note Teta must be given in radians. *
*****************************************************************************/
void GenMatRotZ1(RealType Teta, MatrixType Mat)
{
RealType CTeta, STeta;
CTeta = cos(Teta);
STeta = sin(Teta);
GenMatRotZ(CTeta, STeta, Mat);
}
/*****************************************************************************
* Routine to generate a 4*4 matrix to Rotate around Z with angle of Teta. *
*****************************************************************************/
void GenMatRotZ(RealType CosTeta, RealType SinTeta, MatrixType Mat)
{
GenUnitMat(Mat); /* Make it unit matrix. */
Mat[0][0] = CosTeta;
Mat[0][1] = SinTeta;
Mat[1][0] = -SinTeta;
Mat[1][1] = CosTeta;
}
/*****************************************************************************
* Routine to multiply 2 4by4 matrices: *
* Note MatRes might be one of Mat1 or Mat2 - it is only updated in the end! *
*****************************************************************************/
void MultTwo4by4(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];
}
/*****************************************************************************
* Routine to multiply Vector by 4by4 matrix: *
* The Vector has only 3 components (X, Y, Z) and it is assumed that W = 1 *
* Note VRes might be Vec as it is only updated in the end. *
*****************************************************************************/
void MultVecby4by4(RealType VRes[3], RealType Vec[3], MatrixType Mat)
{
int i, j;
RealType CalcW = Mat[3][3];
RealType VTemp[3];
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 / INFINITY;
for (i = 0; i < 3; i++) VRes[i] = VTemp[i]/CalcW;
}