cv/mgflib/vect.c

#ifndef lint
static const char       RCSid[] = "$Id$";
#endif
#include <math.h>
#include <string.h>
#include "vect.h"

#ifndef M_PI
#define M_PI    3.14159265358979323846
#endif
#define PI      ((double)M_PI)

#define EPSILON 1e-6

void   adjoint (Matrix mat);
double det4x4 (Matrix mat);
double det3x3 (double a1, double a2, double a3, double b1, double b2,
               double b3, double c1, double c2, double c3);
double det2x2 (double a, double b, double c, double d);


void vect_init (Vector v, float  x, float  y, float  z)
{
    v[X] = x;
    v[Y] = y;
    v[Z] = z;
}


void vect_copy (Vector v1, Vector v2)
{
    v1[X] = v2[X];
    v1[Y] = v2[Y];
    v1[Z] = v2[Z];
}


int vect_equal (Vector v1, Vector v2)
{
    if (v1[X] == v2[X] && v1[Y] == v2[Y] && v1[Z] == v2[Z])
        return 1;
    else
        return 0;
}


void vect_add (Vector v1, Vector v2, Vector v3)
{
    v1[X] = v2[X] + v3[X];
    v1[Y] = v2[Y] + v3[Y];
    v1[Z] = v2[Z] + v3[Z];
}


void vect_sub (Vector v1, Vector v2, Vector v3)
{
    v1[X] = v2[X] - v3[X];
    v1[Y] = v2[Y] - v3[Y];
    v1[Z] = v2[Z] - v3[Z];
}


void vect_scale (Vector v1, Vector v2, float  k)
{
    v1[X] = k * v2[X];
    v1[Y] = k * v2[Y];
    v1[Z] = k * v2[Z];
}


float vect_mag (Vector v)
{
    float mag = sqrt(v[X]*v[X] + v[Y]*v[Y] + v[Z]*v[Z]);

    return mag;
}


void vect_normalize (Vector v)
{
    float mag = vect_mag (v);

    if (mag > 0.0)
        vect_scale (v, v, 1.0/mag);
}


float vect_dot (Vector v1, Vector v2)
{
    return (v1[X]*v2[X] + v1[Y]*v2[Y] + v1[Z]*v2[Z]);
}


void vect_cross (Vector v1, Vector v2, Vector v3)
{
    v1[X] = (v2[Y] * v3[Z]) - (v2[Z] * v3[Y]);
    v1[Y] = (v2[Z] * v3[X]) - (v2[X] * v3[Z]);
    v1[Z] = (v2[X] * v3[Y]) - (v2[Y] * v3[X]);
}

void vect_min (Vector v1, Vector v2, Vector v3)
{
    v1[X] = (v2[X] < v3[X]) ? v2[X] : v3[X];
    v1[Y] = (v2[Y] < v3[Y]) ? v2[Y] : v3[Y];
    v1[Z] = (v2[Z] < v3[Z]) ? v2[Z] : v3[Z];
}


void vect_max (Vector v1, Vector v2, Vector v3)
{
    v1[X] = (v2[X] > v3[X]) ? v2[X] : v3[X];
    v1[Y] = (v2[Y] > v3[Y]) ? v2[Y] : v3[Y];
    v1[Z] = (v2[Z] > v3[Z]) ? v2[Z] : v3[Z];
}


/* Return the angle between two vectors */
float vect_angle (Vector v1, Vector v2)
{
    float  mag1, mag2, angle, cos_theta;

    mag1 = vect_mag(v1);
    mag2 = vect_mag(v2);

    if (mag1 * mag2 == 0.0)
        angle = 0.0;
    else {
        cos_theta = vect_dot(v1,v2) / (mag1 * mag2);

        if (cos_theta <= -1.0)
            angle = 180.0;
        else if (cos_theta >= +1.0)
            angle = 0.0;
        else
            angle = (180.0/PI) * acos(cos_theta);
    }

    return angle;
}


void vect_print (FILE *f, Vector v, int dec, char sep)
{
    char fstr[] = "%.4f, %.4f, %.4f";

    if (dec < 0) dec = 0;
    if (dec > 9) dec = 9;

    fstr[2]  = '0' + dec;
    fstr[8]  = '0' + dec;
    fstr[14] = '0' + dec;

    fstr[4]  = sep;
    fstr[10] = sep;

    fprintf (f, fstr, v[X], v[Y], v[Z]);
}


/* Rotate a vector about the X, Y or Z axis */
void vect_rotate (Vector v1, Vector v2, int axis, float angle)
{
    float  cosa, sina;

    cosa = cos ((PI/180.0) * angle);
    sina = sin ((PI/180.0) * angle);

    switch (axis) {
        case X:
            v1[X] =  v2[X];
            v1[Y] =  v2[Y] * cosa + v2[Z] * sina;
            v1[Z] =  v2[Z] * cosa - v2[Y] * sina;
            break;

        case Y:
            v1[X] = v2[X] * cosa - v2[Z] * sina;
            v1[Y] = v2[Y];
            v1[Z] = v2[Z] * cosa + v2[X] * sina;
            break;

        case Z:
            v1[X] = v2[X] * cosa + v2[Y] * sina;
            v1[Y] = v2[Y] * cosa - v2[X] * sina;
            v1[Z] = v2[Z];
            break;
    }
}


/* Rotate a vector about a specific axis */
void vect_axis_rotate (Vector v1, Vector v2, Vector axis, float angle)
{
    float  cosa, sina;
    Matrix mat;

    cosa = cos ((PI/180.0) * angle);
    sina = sin ((PI/180.0) * angle);

    mat[0][0] = (axis[X] * axis[X]) + ((1.0 - (axis[X] * axis[X]))*cosa);
    mat[0][1] = (axis[X] * axis[Y] * (1.0 - cosa)) - (axis[Z] * sina);
    mat[0][2] = (axis[X] * axis[Z] * (1.0 - cosa)) + (axis[Y] * sina);
    mat[0][3] = 0.0;

    mat[1][0] = (axis[X] * axis[Y] * (1.0 - cosa)) + (axis[Z] * sina);
    mat[1][1] = (axis[Y] * axis[Y]) + ((1.0 - (axis[Y] * axis[Y])) * cosa);
    mat[1][2] = (axis[Y] * axis[Z] * (1.0 - cosa)) - (axis[X] * sina);
    mat[1][3] = 0.0;

    mat[2][0] = (axis[X] * axis[Z] * (1.0 - cosa)) - (axis[Y] * sina);
    mat[2][1] = (axis[Y] * axis[Z] * (1.0 - cosa)) + (axis[X] * sina);
    mat[2][2] = (axis[Z] * axis[Z]) + ((1.0 - (axis[Z] * axis[Z])) * cosa);
    mat[2][3] = 0.0;

    mat[3][0] = mat[3][1] = mat[3][2] = mat[3][3] = 0.0;

    vect_transform (v1, v2, mat);
}


/* Transform the given vector */
void vect_transform (Vector v1, Vector v2, Matrix mat)
{
    Vector tmp;

    tmp[X] = (v2[X] * mat[0][0]) + (v2[Y] * mat[1][0]) + (v2[Z] * mat[2][0]) + mat[3][0];
    tmp[Y] = (v2[X] * mat[0][1]) + (v2[Y] * mat[1][1]) + (v2[Z] * mat[2][1]) + mat[3][1];
    tmp[Z] = (v2[X] * mat[0][2]) + (v2[Y] * mat[1][2]) + (v2[Z] * mat[2][2]) + mat[3][2];

    vect_copy (v1, tmp);
}


/* Create an identity matrix */
void mat_identity (Matrix mat)
{
    int i, j;

    for (i = 0; i < 4; i++)
        for (j = 0; j < 4; j++)
            mat[i][j] = 0.0;

    for (i = 0; i < 4; i++)
        mat[i][i] = 1.0;
}


void mat_copy (Matrix mat1, Matrix mat2)
{
    int i, j;

    for (i = 0; i < 4; i++)
        for (j = 0; j < 4; j++)
            mat1[i][j] = mat2[i][j];
}


/* Rotate a matrix about the X, Y or Z axis */
void mat_rotate (Matrix mat1, Matrix mat2, int axis, float angle)
{
    Matrix mat;
    float  cosa, sina;

    cosa = cos ((PI/180.0) * angle);
    sina = sin ((PI/180.0) * angle);

    mat_identity (mat);

    switch (axis) {
        case X:
            mat[1][1] = cosa;
            mat[1][2] = sina;
            mat[2][1] = -sina;
            mat[2][2] = cosa;
            break;

        case Y:
            mat[0][0] = cosa;
            mat[0][2] = -sina;
            mat[2][0] = sina;
            mat[2][2] = cosa;
            break;

        case Z:
            mat[0][0] = cosa;
            mat[0][1] = sina;
            mat[1][0] = -sina;
            mat[1][1] = cosa;
            break;
    }

    mat_mult (mat1, mat2, mat);
}


void mat_axis_rotate (Matrix mat1, Matrix mat2, Vector axis, float angle)
{
    float  cosa, sina;
    Matrix mat;

    cosa = cos ((PI/180.0) * angle);
    sina = sin ((PI/180.0) * angle);

    mat[0][0] = (axis[X] * axis[X]) + ((1.0 - (axis[X] * axis[X]))*cosa);
    mat[0][1] = (axis[X] * axis[Y] * (1.0 - cosa)) - (axis[Z] * sina);
    mat[0][2] = (axis[X] * axis[Z] * (1.0 - cosa)) + (axis[Y] * sina);
    mat[0][3] = 0.0;

    mat[1][0] = (axis[X] * axis[Y] * (1.0 - cosa)) + (axis[Z] * sina);
    mat[1][1] = (axis[Y] * axis[Y]) + ((1.0 - (axis[Y] * axis[Y])) * cosa);
    mat[1][2] = (axis[Y] * axis[Z] * (1.0 - cosa)) - (axis[X] * sina);
    mat[1][3] = 0.0;

    mat[2][0] = (axis[X] * axis[Z] * (1.0 - cosa)) - (axis[Y] * sina);
    mat[2][1] = (axis[Y] * axis[Z] * (1.0 - cosa)) + (axis[X] * sina);
    mat[2][2] = (axis[Z] * axis[Z]) + ((1.0 - (axis[Z] * axis[Z])) * cosa);
    mat[2][3] = 0.0;

    mat[3][0] = mat[3][1] = mat[3][2] = mat[3][3] = 0.0;

    mat_mult (mat1, mat2, mat);
}


/*  mat1 <-- mat2 * mat3 */
void mat_mult (Matrix mat1, Matrix mat2, Matrix mat3)
{
    float sum;
    int   i, j, k;
    Matrix result;

    for (i = 0; i < 4; i++) {
        for (j = 0; j < 4; j++) {
            sum = 0.0;

            for (k = 0; k < 4; k++)
                sum = sum + mat2[i][k] * mat3[k][j];

            result[i][j] = sum;
        }
    }

    for (i = 0; i < 4; i++)
        for (j = 0; j < 4; j++)
            mat1[i][j] = result[i][j];
}


/*
   Decodes a 3x4 transformation matrix into separate scale, rotation,
   translation, and shear vectors. Based on a program by Spencer W.
   Thomas (Graphics Gems II)
*/
void mat_decode (Matrix mat, Vector scale,  Vector shear, Vector rotate,
                Vector transl)
{
    int i;
    Vector row[3], temp;

    for (i = 0; i < 3; i++)
        transl[i] = mat[3][i];

    for (i = 0; i < 3; i++) {
        row[i][X] = mat[i][0];
        row[i][Y] = mat[i][1];
        row[i][Z] = mat[i][2];
    }

    scale[X] = vect_mag (row[0]);
    vect_normalize (row[0]);

    shear[X] = vect_dot (row[0], row[1]);
    row[1][X] = row[1][X] - shear[X]*row[0][X];
    row[1][Y] = row[1][Y] - shear[X]*row[0][Y];
    row[1][Z] = row[1][Z] - shear[X]*row[0][Z];

    scale[Y] = vect_mag (row[1]);
    vect_normalize (row[1]);

    if (scale[Y] != 0.0)
        shear[X] /= scale[Y];

    shear[Y] = vect_dot (row[0], row[2]);
    row[2][X] = row[2][X] - shear[Y]*row[0][X];
    row[2][Y] = row[2][Y] - shear[Y]*row[0][Y];
    row[2][Z] = row[2][Z] - shear[Y]*row[0][Z];

    shear[Z] = vect_dot (row[1], row[2]);
    row[2][X] = row[2][X] - shear[Z]*row[1][X];
    row[2][Y] = row[2][Y] - shear[Z]*row[1][Y];
    row[2][Z] = row[2][Z] - shear[Z]*row[1][Z];

    scale[Z] = vect_mag (row[2]);
    vect_normalize (row[2]);

    if (scale[Z] != 0.0) {
        shear[Y] /= scale[Z];
        shear[Z] /= scale[Z];
    }

    vect_cross (temp, row[1], row[2]);
    if (vect_dot (row[0], temp) < 0.0) {
        for (i = 0; i < 3; i++) {
            scale[i]  *= -1.0;
            row[i][X] *= -1.0;
            row[i][Y] *= -1.0;
            row[i][Z] *= -1.0;
        }
    }

    if (row[0][Z] < -1.0) row[0][Z] = -1.0;
    if (row[0][Z] > +1.0) row[0][Z] = +1.0;

    rotate[Y] = asin(-row[0][Z]);

    if (fabs(cos(rotate[Y])) > EPSILON) {
        rotate[X] = atan2 (row[1][Z], row[2][Z]);
        rotate[Z] = atan2 (row[0][Y], row[0][X]);
    }
    else {
        rotate[X] = atan2 (row[1][X], row[1][Y]);
        rotate[Z] = 0.0;
    }

    /* Convert the rotations to degrees */
    rotate[X] = (180.0/PI)*rotate[X];
    rotate[Y] = (180.0/PI)*rotate[Y];
    rotate[Z] = (180.0/PI)*rotate[Z];
}


/* Matrix inversion code from Graphics Gems */

/* mat1 <-- mat2^-1 */
float mat_inv (Matrix mat1, Matrix mat2)
{
    int i, j;
    float det;

    if (mat1 != mat2) {
        for (i = 0; i < 4; i++)
            for (j = 0; j < 4; j++)
                mat1[i][j] = mat2[i][j];
    }

    det = det4x4 (mat1);

    if (fabs (det) < EPSILON)
        return 0.0;

    adjoint (mat1);

    for (i = 0; i < 4; i++)
        for(j = 0; j < 4; j++)
            mat1[i][j] = mat1[i][j] / det;

    return det;
}


void adjoint (Matrix mat)
{
    double a1, a2, a3, a4, b1, b2, b3, b4;
    double c1, c2, c3, c4, d1, d2, d3, d4;

    a1 = mat[0][0]; b1 = mat[0][1];
    c1 = mat[0][2]; d1 = mat[0][3];

    a2 = mat[1][0]; b2 = mat[1][1];
    c2 = mat[1][2]; d2 = mat[1][3];

    a3 = mat[2][0]; b3 = mat[2][1];
    c3 = mat[2][2]; d3 = mat[2][3];

    a4 = mat[3][0]; b4 = mat[3][1];
    c4 = mat[3][2]; d4 = mat[3][3];

    /* row column labeling reversed since we transpose rows & columns */
    mat[0][0]  =  det3x3 (b2, b3, b4, c2, c3, c4, d2, d3, d4);
    mat[1][0]  = -det3x3 (a2, a3, a4, c2, c3, c4, d2, d3, d4);
    mat[2][0]  =  det3x3 (a2, a3, a4, b2, b3, b4, d2, d3, d4);
    mat[3][0]  = -det3x3 (a2, a3, a4, b2, b3, b4, c2, c3, c4);

    mat[0][1]  = -det3x3 (b1, b3, b4, c1, c3, c4, d1, d3, d4);
    mat[1][1]  =  det3x3 (a1, a3, a4, c1, c3, c4, d1, d3, d4);
    mat[2][1]  = -det3x3 (a1, a3, a4, b1, b3, b4, d1, d3, d4);
    mat[3][1]  =  det3x3 (a1, a3, a4, b1, b3, b4, c1, c3, c4);

    mat[0][2]  =  det3x3 (b1, b2, b4, c1, c2, c4, d1, d2, d4);
    mat[1][2]  = -det3x3 (a1, a2, a4, c1, c2, c4, d1, d2, d4);
    mat[2][2]  =  det3x3 (a1, a2, a4, b1, b2, b4, d1, d2, d4);
    mat[3][2]  = -det3x3 (a1, a2, a4, b1, b2, b4, c1, c2, c4);

    mat[0][3]  = -det3x3 (b1, b2, b3, c1, c2, c3, d1, d2, d3);
    mat[1][3]  =  det3x3 (a1, a2, a3, c1, c2, c3, d1, d2, d3);
    mat[2][3]  = -det3x3 (a1, a2, a3, b1, b2, b3, d1, d2, d3);
    mat[3][3]  =  det3x3 (a1, a2, a3, b1, b2, b3, c1, c2, c3);
}


double det4x4 (Matrix mat)
{
    double ans;
    double a1, a2, a3, a4, b1, b2, b3, b4, c1, c2, c3, c4, d1, d2, d3,                  d4;

    a1 = mat[0][0]; b1 = mat[0][1];
    c1 = mat[0][2]; d1 = mat[0][3];

    a2 = mat[1][0]; b2 = mat[1][1];
    c2 = mat[1][2]; d2 = mat[1][3];

    a3 = mat[2][0]; b3 = mat[2][1];
    c3 = mat[2][2]; d3 = mat[2][3];

    a4 = mat[3][0]; b4 = mat[3][1];
    c4 = mat[3][2]; d4 = mat[3][3];

    ans = a1 * det3x3 (b2, b3, b4, c2, c3, c4, d2, d3, d4) -
          b1 * det3x3 (a2, a3, a4, c2, c3, c4, d2, d3, d4) +
          c1 * det3x3 (a2, a3, a4, b2, b3, b4, d2, d3, d4) -
          d1 * det3x3 (a2, a3, a4, b2, b3, b4, c2, c3, c4);

    return ans;
}


double det3x3 (double a1, double a2, double a3, double b1, double b2,
               double b3, double c1, double c2, double c3)
{
    double ans;

    ans = a1 * det2x2 (b2, b3, c2, c3)
        - b1 * det2x2 (a2, a3, c2, c3)
        + c1 * det2x2 (a2, a3, b2, b3);

    return ans;
}


double det2x2 (double a, double b, double c, double d)
{
    double ans;
    ans = a * d - b * c;
    return ans;
}

Revision:	1.3
Committed:	Fri Feb 28 20:11:30 2003 UTC (22 years, 8 months ago) by greg
Content type:	text/plain
Branch:	MAIN
CVS Tags:	rad4R0, rad3R9, rad3R8, rad3R7P2, rad3R7P1, rad3R6P1, rad3R6, rad3R5
Changes since 1.2:	+1 -2 lines
Log Message:	Updates for 3.5 release
#	User	Rev	Content
1	greg	1.1	#ifndef lint
2	greg	1.3	static const char RCSid[] = "$Id$";
3	greg	1.1	#endif
4			#include <math.h>
5			#include <string.h>
6			#include "vect.h"
7
8			#ifndef M_PI
9			#define M_PI 3.14159265358979323846
10			#endif
11	gregl	1.2	#define PI ((double)M_PI)
12	greg	1.1
13			#define EPSILON 1e-6
14
15			void adjoint (Matrix mat);
16			double det4x4 (Matrix mat);
17			double det3x3 (double a1, double a2, double a3, double b1, double b2,
18			double b3, double c1, double c2, double c3);
19			double det2x2 (double a, double b, double c, double d);
20
21
22			void vect_init (Vector v, float x, float y, float z)
23			{
24			v[X] = x;
25			v[Y] = y;
26			v[Z] = z;
27			}
28
29
30			void vect_copy (Vector v1, Vector v2)
31			{
32			v1[X] = v2[X];
33			v1[Y] = v2[Y];
34			v1[Z] = v2[Z];
35			}
36
37
38			int vect_equal (Vector v1, Vector v2)
39			{
40			if (v1[X] == v2[X] && v1[Y] == v2[Y] && v1[Z] == v2[Z])
41			return 1;
42			else
43			return 0;
44			}
45
46
47			void vect_add (Vector v1, Vector v2, Vector v3)
48			{
49			v1[X] = v2[X] + v3[X];
50			v1[Y] = v2[Y] + v3[Y];
51			v1[Z] = v2[Z] + v3[Z];
52			}
53
54
55			void vect_sub (Vector v1, Vector v2, Vector v3)
56			{
57			v1[X] = v2[X] - v3[X];
58			v1[Y] = v2[Y] - v3[Y];
59			v1[Z] = v2[Z] - v3[Z];
60			}
61
62
63			void vect_scale (Vector v1, Vector v2, float k)
64			{
65			v1[X] = k * v2[X];
66			v1[Y] = k * v2[Y];
67			v1[Z] = k * v2[Z];
68			}
69
70
71			float vect_mag (Vector v)
72			{
73			float mag = sqrt(v[X]v[X] + v[Y]v[Y] + v[Z]*v[Z]);
74
75			return mag;
76			}
77
78
79			void vect_normalize (Vector v)
80			{
81			float mag = vect_mag (v);
82
83			if (mag > 0.0)
84			vect_scale (v, v, 1.0/mag);
85			}
86
87
88			float vect_dot (Vector v1, Vector v2)
89			{
90			return (v1[X]v2[X] + v1[Y]v2[Y] + v1[Z]*v2[Z]);
91			}
92
93
94			void vect_cross (Vector v1, Vector v2, Vector v3)
95			{
96			v1[X] = (v2[Y] * v3[Z]) - (v2[Z] * v3[Y]);
97			v1[Y] = (v2[Z] * v3[X]) - (v2[X] * v3[Z]);
98			v1[Z] = (v2[X] * v3[Y]) - (v2[Y] * v3[X]);
99			}
100
101			void vect_min (Vector v1, Vector v2, Vector v3)
102			{
103			v1[X] = (v2[X] < v3[X]) ? v2[X] : v3[X];
104			v1[Y] = (v2[Y] < v3[Y]) ? v2[Y] : v3[Y];
105			v1[Z] = (v2[Z] < v3[Z]) ? v2[Z] : v3[Z];
106			}
107
108
109			void vect_max (Vector v1, Vector v2, Vector v3)
110			{
111			v1[X] = (v2[X] > v3[X]) ? v2[X] : v3[X];
112			v1[Y] = (v2[Y] > v3[Y]) ? v2[Y] : v3[Y];
113			v1[Z] = (v2[Z] > v3[Z]) ? v2[Z] : v3[Z];
114			}
115
116
117			/* Return the angle between two vectors */
118			float vect_angle (Vector v1, Vector v2)
119			{
120			float mag1, mag2, angle, cos_theta;
121
122			mag1 = vect_mag(v1);
123			mag2 = vect_mag(v2);
124
125			if (mag1 * mag2 == 0.0)
126			angle = 0.0;
127			else {
128			cos_theta = vect_dot(v1,v2) / (mag1 * mag2);
129
130			if (cos_theta <= -1.0)
131			angle = 180.0;
132			else if (cos_theta >= +1.0)
133			angle = 0.0;
134			else
135	gregl	1.2	angle = (180.0/PI) * acos(cos_theta);
136	greg	1.1	}
137
138			return angle;
139			}
140
141
142			void vect_print (FILE *f, Vector v, int dec, char sep)
143			{
144			char fstr[] = "%.4f, %.4f, %.4f";
145
146			if (dec < 0) dec = 0;
147			if (dec > 9) dec = 9;
148
149			fstr[2] = '0' + dec;
150			fstr[8] = '0' + dec;
151			fstr[14] = '0' + dec;
152
153			fstr[4] = sep;
154			fstr[10] = sep;
155
156			fprintf (f, fstr, v[X], v[Y], v[Z]);
157			}
158
159
160			/* Rotate a vector about the X, Y or Z axis */
161			void vect_rotate (Vector v1, Vector v2, int axis, float angle)
162			{
163			float cosa, sina;
164
165	gregl	1.2	cosa = cos ((PI/180.0) * angle);
166			sina = sin ((PI/180.0) * angle);
167	greg	1.1
168			switch (axis) {
169			case X:
170			v1[X] = v2[X];
171			v1[Y] = v2[Y] * cosa + v2[Z] * sina;
172			v1[Z] = v2[Z] * cosa - v2[Y] * sina;
173			break;
174
175			case Y:
176			v1[X] = v2[X] * cosa - v2[Z] * sina;
177			v1[Y] = v2[Y];
178			v1[Z] = v2[Z] * cosa + v2[X] * sina;
179			break;
180
181			case Z:
182			v1[X] = v2[X] * cosa + v2[Y] * sina;
183			v1[Y] = v2[Y] * cosa - v2[X] * sina;
184			v1[Z] = v2[Z];
185			break;
186			}
187			}
188
189
190			/* Rotate a vector about a specific axis */
191			void vect_axis_rotate (Vector v1, Vector v2, Vector axis, float angle)
192			{
193			float cosa, sina;
194			Matrix mat;
195
196	gregl	1.2	cosa = cos ((PI/180.0) * angle);
197			sina = sin ((PI/180.0) * angle);
198	greg	1.1
199			mat[0][0] = (axis[X] * axis[X]) + ((1.0 - (axis[X] * axis[X]))*cosa);
200			mat[0][1] = (axis[X] * axis[Y] * (1.0 - cosa)) - (axis[Z] * sina);
201			mat[0][2] = (axis[X] * axis[Z] * (1.0 - cosa)) + (axis[Y] * sina);
202			mat[0][3] = 0.0;
203
204			mat[1][0] = (axis[X] * axis[Y] * (1.0 - cosa)) + (axis[Z] * sina);
205			mat[1][1] = (axis[Y] * axis[Y]) + ((1.0 - (axis[Y] * axis[Y])) * cosa);
206			mat[1][2] = (axis[Y] * axis[Z] * (1.0 - cosa)) - (axis[X] * sina);
207			mat[1][3] = 0.0;
208
209			mat[2][0] = (axis[X] * axis[Z] * (1.0 - cosa)) - (axis[Y] * sina);
210			mat[2][1] = (axis[Y] * axis[Z] * (1.0 - cosa)) + (axis[X] * sina);
211			mat[2][2] = (axis[Z] * axis[Z]) + ((1.0 - (axis[Z] * axis[Z])) * cosa);
212			mat[2][3] = 0.0;
213
214			mat[3][0] = mat[3][1] = mat[3][2] = mat[3][3] = 0.0;
215
216			vect_transform (v1, v2, mat);
217			}
218
219
220			/* Transform the given vector */
221			void vect_transform (Vector v1, Vector v2, Matrix mat)
222			{
223			Vector tmp;
224
225			tmp[X] = (v2[X] * mat[0][0]) + (v2[Y] * mat[1][0]) + (v2[Z] * mat[2][0]) + mat[3][0];
226			tmp[Y] = (v2[X] * mat[0][1]) + (v2[Y] * mat[1][1]) + (v2[Z] * mat[2][1]) + mat[3][1];
227			tmp[Z] = (v2[X] * mat[0][2]) + (v2[Y] * mat[1][2]) + (v2[Z] * mat[2][2]) + mat[3][2];
228
229			vect_copy (v1, tmp);
230			}
231
232
233			/* Create an identity matrix */
234			void mat_identity (Matrix mat)
235			{
236			int i, j;
237
238			for (i = 0; i < 4; i++)
239			for (j = 0; j < 4; j++)
240			mat[i][j] = 0.0;
241
242			for (i = 0; i < 4; i++)
243			mat[i][i] = 1.0;
244			}
245
246
247			void mat_copy (Matrix mat1, Matrix mat2)
248			{
249			int i, j;
250
251			for (i = 0; i < 4; i++)
252			for (j = 0; j < 4; j++)
253			mat1[i][j] = mat2[i][j];
254			}
255
256
257			/* Rotate a matrix about the X, Y or Z axis */
258			void mat_rotate (Matrix mat1, Matrix mat2, int axis, float angle)
259			{
260			Matrix mat;
261			float cosa, sina;
262
263	gregl	1.2	cosa = cos ((PI/180.0) * angle);
264			sina = sin ((PI/180.0) * angle);
265	greg	1.1
266			mat_identity (mat);
267
268			switch (axis) {
269			case X:
270			mat[1][1] = cosa;
271			mat[1][2] = sina;
272			mat[2][1] = -sina;
273			mat[2][2] = cosa;
274			break;
275
276			case Y:
277			mat[0][0] = cosa;
278			mat[0][2] = -sina;
279			mat[2][0] = sina;
280			mat[2][2] = cosa;
281			break;
282
283			case Z:
284			mat[0][0] = cosa;
285			mat[0][1] = sina;
286			mat[1][0] = -sina;
287			mat[1][1] = cosa;
288			break;
289			}
290
291			mat_mult (mat1, mat2, mat);
292			}
293
294
295			void mat_axis_rotate (Matrix mat1, Matrix mat2, Vector axis, float angle)
296			{
297			float cosa, sina;
298			Matrix mat;
299
300	gregl	1.2	cosa = cos ((PI/180.0) * angle);
301			sina = sin ((PI/180.0) * angle);
302	greg	1.1
303			mat[0][0] = (axis[X] * axis[X]) + ((1.0 - (axis[X] * axis[X]))*cosa);
304			mat[0][1] = (axis[X] * axis[Y] * (1.0 - cosa)) - (axis[Z] * sina);
305			mat[0][2] = (axis[X] * axis[Z] * (1.0 - cosa)) + (axis[Y] * sina);
306			mat[0][3] = 0.0;
307
308			mat[1][0] = (axis[X] * axis[Y] * (1.0 - cosa)) + (axis[Z] * sina);
309			mat[1][1] = (axis[Y] * axis[Y]) + ((1.0 - (axis[Y] * axis[Y])) * cosa);
310			mat[1][2] = (axis[Y] * axis[Z] * (1.0 - cosa)) - (axis[X] * sina);
311			mat[1][3] = 0.0;
312
313			mat[2][0] = (axis[X] * axis[Z] * (1.0 - cosa)) - (axis[Y] * sina);
314			mat[2][1] = (axis[Y] * axis[Z] * (1.0 - cosa)) + (axis[X] * sina);
315			mat[2][2] = (axis[Z] * axis[Z]) + ((1.0 - (axis[Z] * axis[Z])) * cosa);
316			mat[2][3] = 0.0;
317
318			mat[3][0] = mat[3][1] = mat[3][2] = mat[3][3] = 0.0;
319
320			mat_mult (mat1, mat2, mat);
321			}
322
323
324			/* mat1 <-- mat2 * mat3 */
325			void mat_mult (Matrix mat1, Matrix mat2, Matrix mat3)
326			{
327			float sum;
328			int i, j, k;
329			Matrix result;
330
331			for (i = 0; i < 4; i++) {
332			for (j = 0; j < 4; j++) {
333			sum = 0.0;
334
335			for (k = 0; k < 4; k++)
336			sum = sum + mat2[i][k] * mat3[k][j];
337
338			result[i][j] = sum;
339			}
340			}
341
342			for (i = 0; i < 4; i++)
343			for (j = 0; j < 4; j++)
344			mat1[i][j] = result[i][j];
345			}
346
347
348			/*
349			Decodes a 3x4 transformation matrix into separate scale, rotation,
350			translation, and shear vectors. Based on a program by Spencer W.
351			Thomas (Graphics Gems II)
352			*/
353			void mat_decode (Matrix mat, Vector scale, Vector shear, Vector rotate,
354			Vector transl)
355			{
356			int i;
357			Vector row[3], temp;
358
359			for (i = 0; i < 3; i++)
360			transl[i] = mat[3][i];
361
362			for (i = 0; i < 3; i++) {
363			row[i][X] = mat[i][0];
364			row[i][Y] = mat[i][1];
365			row[i][Z] = mat[i][2];
366			}
367
368			scale[X] = vect_mag (row[0]);
369			vect_normalize (row[0]);
370
371			shear[X] = vect_dot (row[0], row[1]);
372			row[1][X] = row[1][X] - shear[X]*row[0][X];
373			row[1][Y] = row[1][Y] - shear[X]*row[0][Y];
374			row[1][Z] = row[1][Z] - shear[X]*row[0][Z];
375
376			scale[Y] = vect_mag (row[1]);
377			vect_normalize (row[1]);
378
379			if (scale[Y] != 0.0)
380			shear[X] /= scale[Y];
381
382			shear[Y] = vect_dot (row[0], row[2]);
383			row[2][X] = row[2][X] - shear[Y]*row[0][X];
384			row[2][Y] = row[2][Y] - shear[Y]*row[0][Y];
385			row[2][Z] = row[2][Z] - shear[Y]*row[0][Z];
386
387			shear[Z] = vect_dot (row[1], row[2]);
388			row[2][X] = row[2][X] - shear[Z]*row[1][X];
389			row[2][Y] = row[2][Y] - shear[Z]*row[1][Y];
390			row[2][Z] = row[2][Z] - shear[Z]*row[1][Z];
391
392			scale[Z] = vect_mag (row[2]);
393			vect_normalize (row[2]);
394
395			if (scale[Z] != 0.0) {
396			shear[Y] /= scale[Z];
397			shear[Z] /= scale[Z];
398			}
399
400			vect_cross (temp, row[1], row[2]);
401			if (vect_dot (row[0], temp) < 0.0) {
402			for (i = 0; i < 3; i++) {
403			scale[i] *= -1.0;
404			row[i][X] *= -1.0;
405			row[i][Y] *= -1.0;
406			row[i][Z] *= -1.0;
407			}
408			}
409
410			if (row[0][Z] < -1.0) row[0][Z] = -1.0;
411			if (row[0][Z] > +1.0) row[0][Z] = +1.0;
412
413			rotate[Y] = asin(-row[0][Z]);
414
415			if (fabs(cos(rotate[Y])) > EPSILON) {
416			rotate[X] = atan2 (row[1][Z], row[2][Z]);
417			rotate[Z] = atan2 (row[0][Y], row[0][X]);
418			}
419			else {
420			rotate[X] = atan2 (row[1][X], row[1][Y]);
421			rotate[Z] = 0.0;
422			}
423
424			/* Convert the rotations to degrees */
425	gregl	1.2	rotate[X] = (180.0/PI)*rotate[X];
426			rotate[Y] = (180.0/PI)*rotate[Y];
427			rotate[Z] = (180.0/PI)*rotate[Z];
428	greg	1.1	}
429
430
431			/* Matrix inversion code from Graphics Gems */
432
433			/* mat1 <-- mat2^-1 */
434			float mat_inv (Matrix mat1, Matrix mat2)
435			{
436			int i, j;
437			float det;
438
439			if (mat1 != mat2) {
440			for (i = 0; i < 4; i++)
441			for (j = 0; j < 4; j++)
442			mat1[i][j] = mat2[i][j];
443			}
444
445			det = det4x4 (mat1);
446
447			if (fabs (det) < EPSILON)
448			return 0.0;
449
450			adjoint (mat1);
451
452			for (i = 0; i < 4; i++)
453			for(j = 0; j < 4; j++)
454			mat1[i][j] = mat1[i][j] / det;
455
456			return det;
457			}
458
459
460			void adjoint (Matrix mat)
461			{
462			double a1, a2, a3, a4, b1, b2, b3, b4;
463			double c1, c2, c3, c4, d1, d2, d3, d4;
464
465			a1 = mat[0][0]; b1 = mat[0][1];
466			c1 = mat[0][2]; d1 = mat[0][3];
467
468			a2 = mat[1][0]; b2 = mat[1][1];
469			c2 = mat[1][2]; d2 = mat[1][3];
470
471			a3 = mat[2][0]; b3 = mat[2][1];
472			c3 = mat[2][2]; d3 = mat[2][3];
473
474			a4 = mat[3][0]; b4 = mat[3][1];
475			c4 = mat[3][2]; d4 = mat[3][3];
476
477			/* row column labeling reversed since we transpose rows & columns */
478			mat[0][0] = det3x3 (b2, b3, b4, c2, c3, c4, d2, d3, d4);
479			mat[1][0] = -det3x3 (a2, a3, a4, c2, c3, c4, d2, d3, d4);
480			mat[2][0] = det3x3 (a2, a3, a4, b2, b3, b4, d2, d3, d4);
481			mat[3][0] = -det3x3 (a2, a3, a4, b2, b3, b4, c2, c3, c4);
482
483			mat[0][1] = -det3x3 (b1, b3, b4, c1, c3, c4, d1, d3, d4);
484			mat[1][1] = det3x3 (a1, a3, a4, c1, c3, c4, d1, d3, d4);
485			mat[2][1] = -det3x3 (a1, a3, a4, b1, b3, b4, d1, d3, d4);
486			mat[3][1] = det3x3 (a1, a3, a4, b1, b3, b4, c1, c3, c4);
487
488			mat[0][2] = det3x3 (b1, b2, b4, c1, c2, c4, d1, d2, d4);
489			mat[1][2] = -det3x3 (a1, a2, a4, c1, c2, c4, d1, d2, d4);
490			mat[2][2] = det3x3 (a1, a2, a4, b1, b2, b4, d1, d2, d4);
491			mat[3][2] = -det3x3 (a1, a2, a4, b1, b2, b4, c1, c2, c4);
492
493			mat[0][3] = -det3x3 (b1, b2, b3, c1, c2, c3, d1, d2, d3);
494			mat[1][3] = det3x3 (a1, a2, a3, c1, c2, c3, d1, d2, d3);
495			mat[2][3] = -det3x3 (a1, a2, a3, b1, b2, b3, d1, d2, d3);
496			mat[3][3] = det3x3 (a1, a2, a3, b1, b2, b3, c1, c2, c3);
497			}
498
499
500			double det4x4 (Matrix mat)
501			{
502			double ans;
503			double a1, a2, a3, a4, b1, b2, b3, b4, c1, c2, c3, c4, d1, d2, d3, d4;
504
505			a1 = mat[0][0]; b1 = mat[0][1];
506			c1 = mat[0][2]; d1 = mat[0][3];
507
508			a2 = mat[1][0]; b2 = mat[1][1];
509			c2 = mat[1][2]; d2 = mat[1][3];
510
511			a3 = mat[2][0]; b3 = mat[2][1];
512			c3 = mat[2][2]; d3 = mat[2][3];
513
514			a4 = mat[3][0]; b4 = mat[3][1];
515			c4 = mat[3][2]; d4 = mat[3][3];
516
517			ans = a1 * det3x3 (b2, b3, b4, c2, c3, c4, d2, d3, d4) -
518			b1 * det3x3 (a2, a3, a4, c2, c3, c4, d2, d3, d4) +
519			c1 * det3x3 (a2, a3, a4, b2, b3, b4, d2, d3, d4) -
520			d1 * det3x3 (a2, a3, a4, b2, b3, b4, c2, c3, c4);
521
522			return ans;
523			}
524
525
526			double det3x3 (double a1, double a2, double a3, double b1, double b2,
527			double b3, double c1, double c2, double c3)
528			{
529			double ans;
530
531			ans = a1 * det2x2 (b2, b3, c2, c3)
532			- b1 * det2x2 (a2, a3, c2, c3)
533			+ c1 * det2x2 (a2, a3, b2, b3);
534
535			return ans;
536			}
537
538
539			double det2x2 (double a, double b, double c, double d)
540			{
541			double ans;
542			ans = a * d - b * c;
543			return ans;
544			}
545