--- /dev/null
+/* #include "HEADERS.h" */
+/* Copyright 1988, Brown Computer Graphics Group. All Rights Reserved. */
+
+/* --------------------------------------------------------------------------
+ * This file contains routines that perform geometry-related operations
+ * on matrices.
+ * -------------------------------------------------------------------------*/
+
+#include "mat3defs.h"
+
+/* -------------------------- Static Routines ---------------------------- */
+
+/* ------------------------- Internal Routines --------------------------- */
+
+/* -------------------------- Public Routines ---------------------------- */
+
+/*
+ * This takes a matrix used to transform points, and returns a corresponding
+ * matrix that can be used to transform direction vectors (between points).
+ */
+
+void
+MAT3direction_matrix(result_mat, mat)
+register MAT3mat result_mat, mat;
+{
+ register int i;
+
+ MAT3copy(result_mat, mat);
+
+ for (i = 0; i < 4; i++) result_mat[i][3] = result_mat[3][i] = 0.0;
+
+ result_mat[3][3] = 1.0;
+}
+
+/*
+ * This takes a matrix used to transform points, and returns a corresponding
+ * matrix that can be used to transform vectors that must remain perpendicular
+ * to planes defined by the points. It is useful when you are transforming
+ * some object that has both points and normals in its definition, and you
+ * only have the transformation matrix for the points. This routine returns
+ * FALSE if the normal matrix is uncomputable. Otherwise, it returns TRUE.
+ *
+ * Spike sez: "This is the adjoint for the non-homogeneous part of the
+ * transformation."
+ */
+
+int
+MAT3normal_matrix(result_mat, mat)
+register MAT3mat result_mat, mat;
+{
+ register int ret;
+ MAT3mat tmp_mat;
+
+ MAT3direction_matrix(result_mat, mat);
+
+ if (ret = MAT3invert(tmp_mat, tmp_mat)) MAT3transpose(result_mat, tmp_mat);
+
+ return(ret);
+}
+
+/*
+ * Sets the given matrix to be a scale matrix for the given vector of
+ * scale values.
+ */
+
+void
+MAT3scale(result_mat, scale)
+MAT3mat result_mat;
+MAT3vec scale;
+{
+ MAT3identity(result_mat);
+
+ result_mat[0][0] = scale[0];
+ result_mat[1][1] = scale[1];
+ result_mat[2][2] = scale[2];
+}
+
+/*
+ * Sets up a matrix for a rotation about an axis given by the line from
+ * (0,0,0) to axis, through an angle (in radians).
+ * Looking along the axis toward the origin, the rotation is counter-clockwise.
+ */
+
+#define SELECT .7071 /* selection constant (roughly .5*sqrt(2) */
+
+void
+MAT3rotate(result_mat, axis, angle_in_radians)
+MAT3mat result_mat;
+MAT3vec axis;
+double angle_in_radians;
+{
+ MAT3vec naxis, /* Axis of rotation, normalized */
+ base2, /* 2nd unit basis vec, perp to axis */
+ base3; /* 3rd unit basis vec, perp to axis & base2 */
+ double dot;
+ MAT3mat base_mat, /* Change-of-basis matrix */
+ base_mat_trans; /* Inverse of c-o-b matrix */
+ register int i;
+
+ /* Step 1: extend { axis } to a basis for 3-space: { axis, base2, base3 }
+ * which is orthonormal (all three have unit length, and all three are
+ * mutually orthogonal). Also should be oriented, i.e. axis cross base2 =
+ * base3, rather than -base3.
+ *
+ * Method: Find a vector linearly independent from axis. For this we
+ * either use the y-axis, or, if that is too close to axis, the
+ * z-axis. 'Too close' means that the dot product is too near to 1.
+ */
+
+ MAT3_COPY_VEC(naxis, axis);
+ MAT3_NORMALIZE_VEC(naxis, dot);
+
+ if (dot == 0.0) {
+ /* ERR_ERROR(MAT3_errid, ERR_SEVERE,
+ (ERR_S, "Zero-length axis vector given to MAT3rotate")); */
+ return;
+ }
+
+ MAT3perp_vec(base2, naxis, TRUE);
+ MAT3cross_product(base3, naxis, base2);
+
+ /* Set up the change-of-basis matrix, and its inverse */
+ MAT3identity(base_mat);
+ MAT3identity(base_mat_trans);
+ MAT3identity(result_mat);
+
+ for (i = 0; i < 3; i++){
+ base_mat_trans[i][0] = base_mat[0][i] = naxis[i];
+ base_mat_trans[i][1] = base_mat[1][i] = base2[i];
+ base_mat_trans[i][2] = base_mat[2][i] = base3[i];
+ }
+
+ /* If T(u) = uR, where R is base_mat, then T(x-axis) = naxis,
+ * T(y-axis) = base2, and T(z-axis) = base3. The inverse of base_mat is
+ * its transpose. OK?
+ */
+
+ result_mat[1][1] = result_mat[2][2] = cos(angle_in_radians);
+ result_mat[2][1] = -(result_mat[1][2] = sin(angle_in_radians));
+
+ MAT3mult(result_mat, base_mat_trans, result_mat);
+ MAT3mult(result_mat, result_mat, base_mat);
+}
+
+/*
+ * Sets the given matrix to be a translation matrix for the given vector of
+ * translation values.
+ */
+
+void
+MAT3translate(result_mat, trans)
+MAT3mat result_mat;
+MAT3vec trans;
+{
+ MAT3identity(result_mat);
+
+ result_mat[3][0] = trans[0];
+ result_mat[3][1] = trans[1];
+ result_mat[3][2] = trans[2];
+}
+
+/*
+ * Sets the given matrix to be a shear matrix for the given x and y shear
+ * values.
+ */
+
+void
+MAT3shear(result_mat, xshear, yshear)
+MAT3mat result_mat;
+double xshear, yshear;
+{
+ MAT3identity(result_mat);
+
+ result_mat[2][0] = xshear;
+ result_mat[2][1] = yshear;
+}
+
--- /dev/null
+/* Copyright 1988, Brown Computer Graphics Group. All Rights Reserved. */
+
+/* --------------------------------------------------------------------------
+ * This file contains routines that operate solely on matrices.
+ * -------------------------------------------------------------------------*/
+
+#include "mat3defs.h"
+
+/* -------------------------- Static Routines ---------------------------- */
+
+#define SMALL 1e-20 /* Small enough to be considered zero */
+
+/*
+ * Shuffles rows in inverse of 3x3. See comment in MAT3_inv3_second_col().
+ */
+
+static void
+MAT3_inv3_swap( register double inv[3][3], int row0, int row1, int row2)
+{
+ register int i, tempi;
+ double temp;
+
+#define SWAP_ROWS(a, b) \
+ for (i = 0; i < 3; i++) SWAP(inv[a][i], inv[b][i], temp); \
+ SWAP(a, b, tempi)
+
+ if (row0 != 0){
+ if (row1 == 0) {
+ SWAP_ROWS(row0, row1);
+ }
+ else {
+ SWAP_ROWS(row0, row2);
+ }
+ }
+
+ if (row1 != 1) {
+ SWAP_ROWS(row1, row2);
+ }
+}
+
+/*
+ * Does Gaussian elimination on second column.
+ */
+
+static int
+MAT3_inv3_second_col (register double source[3][3], register double inv[3][3], int row0)
+{
+ register int row1, row2, i1, i2, i;
+ double temp;
+ double a, b;
+
+ /* Find which row to use */
+ if (row0 == 0) i1 = 1, i2 = 2;
+ else if (row0 == 1) i1 = 0, i2 = 2;
+ else i1 = 0, i2 = 1;
+
+ /* Find which is larger in abs. val.:the entry in [i1][1] or [i2][1] */
+ /* and use that value for pivoting. */
+
+ a = source[i1][1]; if (a < 0) a = -a;
+ b = source[i2][1]; if (b < 0) b = -b;
+ if (a > b) row1 = i1;
+ else row1 = i2;
+ row2 = (row1 == i1 ? i2 : i1);
+
+ /* Scale row1 in source */
+ if ((source[row1][1] < SMALL) && (source[row1][1] > -SMALL)) return(FALSE);
+ temp = 1.0 / source[row1][1];
+ source[row1][1] = 1.0;
+ source[row1][2] *= temp; /* source[row1][0] is zero already */
+
+ /* Scale row1 in inv */
+ inv[row1][row1] = temp; /* it used to be a 1.0 */
+ inv[row1][row0] *= temp;
+
+ /* Clear column one, source, and make corresponding changes in inv */
+
+ for (i = 0; i < 3; i++) if (i != row1) { /* for i = all rows but row1 */
+ temp = -source[i][1];
+ source[i][1] = 0.0;
+ source[i][2] += temp * source[row1][2];
+
+ inv[i][row1] = temp * inv[row1][row1];
+ inv[i][row0] += temp * inv[row1][row0];
+ }
+
+ /* Scale row2 in source */
+ if ((source[row2][2] < SMALL) && (source[row2][2] > -SMALL)) return(FALSE);
+ temp = 1.0 / source[row2][2];
+ source[row2][2] = 1.0; /* source[row2][*] is zero already */
+
+ /* Scale row2 in inv */
+ inv[row2][row2] = temp; /* it used to be a 1.0 */
+ inv[row2][row0] *= temp;
+ inv[row2][row1] *= temp;
+
+ /* Clear column one, source, and make corresponding changes in inv */
+ for (i = 0; i < 3; i++) if (i != row2) { /* for i = all rows but row2 */
+ temp = -source[i][2];
+ source[i][2] = 0.0;
+ inv[i][row0] += temp * inv[row2][row0];
+ inv[i][row1] += temp * inv[row2][row1];
+ inv[i][row2] += temp * inv[row2][row2];
+ }
+
+ /*
+ * Now all is done except that the inverse needs to have its rows shuffled.
+ * row0 needs to be moved to inv[0][*], row1 to inv[1][*], etc.
+ *
+ * We *didn't* do the swapping before the elimination so that we could more
+ * easily keep track of what ops are needed to be done in the inverse.
+ */
+ MAT3_inv3_swap(inv, row0, row1, row2);
+
+ return(TRUE);
+}
+
+/*
+ * Fast inversion routine for 3 x 3 matrices. - Written by jfh.
+ *
+ * This takes 30 multiplies/divides, as opposed to 39 for Cramer's Rule.
+ * The algorithm consists of performing fast gaussian elimination, by never
+ * doing any operations where the result is guaranteed to be zero, or where
+ * one operand is guaranteed to be zero. This is done at the cost of clarity,
+ * alas.
+ *
+ * Returns 1 if the inverse was successful, 0 if it failed.
+ */
+
+static int
+MAT3_invert3 (register double source[3][3], register double inv[3][3])
+{
+ register int i, row0;
+ double temp;
+ double a, b, c;
+
+ inv[0][0] = inv[1][1] = inv[2][2] = 1.0;
+ inv[0][1] = inv[0][2] = inv[1][0] = inv[1][2] = inv[2][0] = inv[2][1] = 0.0;
+
+ /* attempt to find the largest entry in first column to use as pivot */
+ a = source[0][0]; if (a < 0) a = -a;
+ b = source[1][0]; if (b < 0) b = -b;
+ c = source[2][0]; if (c < 0) c = -c;
+
+ if (a > b) {
+ if (a > c) row0 = 0;
+ else row0 = 2;
+ }
+ else {
+ if (b > c) row0 = 1;
+ else row0 = 2;
+ }
+
+ /* Scale row0 of source */
+ if ((source[row0][0] < SMALL) && (source[row0][0] > -SMALL)) return(FALSE);
+ temp = 1.0 / source[row0][0];
+ source[row0][0] = 1.0;
+ source[row0][1] *= temp;
+ source[row0][2] *= temp;
+
+ /* Scale row0 of inverse */
+ inv[row0][row0] = temp; /* other entries are zero -- no effort */
+
+ /* Clear column zero of source, and make corresponding changes in inverse */
+
+ for (i = 0; i < 3; i++) if (i != row0) { /* for i = all rows but row0 */
+ temp = -source[i][0];
+ source[i][0] = 0.0;
+ source[i][1] += temp * source[row0][1];
+ source[i][2] += temp * source[row0][2];
+ inv[i][row0] = temp * inv[row0][row0];
+ }
+
+ /*
+ * We've now done gaussian elimination so that the source and
+ * inverse look like this:
+ *
+ * 1 * * * 0 0
+ * 0 * * * 1 0
+ * 0 * * * 0 1
+ *
+ * We now proceed to do elimination on the second column.
+ */
+ if (! MAT3_inv3_second_col(source, inv, row0)) return(FALSE);
+
+ return(TRUE);
+}
+
+/*
+ * Finds a new pivot for a non-simple 4x4. See comments in MAT3invert().
+ */
+
+static int
+MAT3_inv4_pivot (register MAT3mat src, MAT3vec r, double *s, int *swap)
+{
+ register int i, j;
+ double temp, max;
+
+ *swap = -1;
+
+ if (MAT3_IS_ZERO(src[3][3])) {
+
+ /* Look for a different pivot element: one with largest abs value */
+ max = 0.0;
+
+ for (i = 0; i < 4; i++) {
+ if (src[i][3] > max) max = src[*swap = i][3];
+ else if (src[i][3] < -max) max = -src[*swap = i][3];
+ }
+
+ /* No pivot element available ! */
+ if (*swap < 0) return(FALSE);
+
+ else for (j = 0; j < 4; j++) SWAP(src[*swap][j], src[3][j], temp);
+ }
+
+ MAT3_SET_VEC (r, -src[0][3], -src[1][3], -src[2][3]);
+
+ *s = 1.0 / src[3][3];
+
+ src[0][3] = src[1][3] = src[2][3] = 0.0;
+ src[3][3] = 1.0;
+
+ MAT3_SCALE_VEC(src[3], src[3], *s);
+
+ for (i = 0; i < 3; i++) {
+ src[0][i] += r[0] * src[3][i];
+ src[1][i] += r[1] * src[3][i];
+ src[2][i] += r[2] * src[3][i];
+ }
+
+ return(TRUE);
+}
+
+/* ------------------------- Internal Routines --------------------------- */
+
+/* -------------------------- Public Routines ---------------------------- */
+
+/*
+ * This returns the inverse of the given matrix. The result matrix
+ * may be the same as the one to invert.
+ *
+ * Fast inversion routine for 4 x 4 matrices, written by jfh.
+ *
+ * Returns 1 if the inverse was successful, 0 if it failed.
+ *
+ * This routine has been specially tweaked to notice the following:
+ * If the matrix has the form
+ * * * * 0
+ * * * * 0
+ * * * * 0
+ * * * * 1
+ *
+ * (as do many matrices in graphics), then we compute the inverse of
+ * the upper left 3x3 matrix and use this to find the general inverse.
+ *
+ * In the event that the right column is not 0-0-0-1, we do gaussian
+ * elimination to make it so, then use the 3x3 inverse, and then do
+ * our gaussian elimination.
+ */
+
+int
+MAT3invert(result_mat, mat)
+MAT3mat result_mat, mat;
+{
+ MAT3mat src, inv;
+ register int i, j, simple;
+ double m[3][3], inv3[3][3], s, temp;
+ MAT3vec r, t;
+ int swap;
+
+ MAT3copy(src, mat);
+ MAT3identity(inv);
+
+ /* If last column is not (0,0,0,1), use special code */
+ simple = (mat[0][3] == 0.0 && mat[1][3] == 0.0 &&
+ mat[2][3] == 0.0 && mat[3][3] == 1.0);
+
+ if (! simple && ! MAT3_inv4_pivot(src, r, &s, &swap)) return(FALSE);
+
+ MAT3_COPY_VEC(t, src[3]); /* Translation vector */
+
+ /* Copy upper-left 3x3 matrix */
+ for (i = 0; i < 3; i++) for (j = 0; j < 3; j++) m[i][j] = src[i][j];
+
+ if (! MAT3_invert3(m, inv3)) return(FALSE);
+
+ for (i = 0; i < 3; i++) for (j = 0; j < 3; j++) inv[i][j] = inv3[i][j];
+
+ for (i = 0; i < 3; i++) for (j = 0; j < 3; j++)
+ inv[3][i] -= t[j] * inv3[j][i];
+
+ if (! simple) {
+
+ /* We still have to undo our gaussian elimination from earlier on */
+ /* add r0 * first col to last col */
+ /* add r1 * 2nd col to last col */
+ /* add r2 * 3rd col to last col */
+
+ for (i = 0; i < 4; i++) {
+ inv[i][3] += r[0] * inv[i][0] + r[1] * inv[i][1] + r[2] * inv[i][2];
+ inv[i][3] *= s;
+ }
+
+ if (swap >= 0)
+ for (i = 0; i < 4; i++) SWAP(inv[i][swap], inv[i][3], temp);
+ }
+
+ MAT3copy(result_mat, inv);
+
+ return(TRUE);
+}
--- /dev/null
+/* #include "HEADERS.h" */
+/* Copyright 1988, Brown Computer Graphics Group. All Rights Reserved. */
+
+/* --------------------------------------------------------------------------
+ * This file contains routines that operate solely on matrices.
+ * -------------------------------------------------------------------------*/
+
+#include "mat3defs.h"
+
+/* #include "macros.h" */
+
+/* -------------------------- Static Routines ---------------------------- */
+
+/* ------------------------- Internal Routines --------------------------- */
+
+/* -------------------------- Public Routines ---------------------------- */
+
+
+/*
+ * Sets a matrix to identity.
+ */
+
+void
+MAT3identity (register MAT3mat mat)
+{
+ register int i;
+
+ bzero (mat, sizeof(MAT3mat));
+ for (i = 0; i < 4; i++)
+ mat[i][i] = 1.0;
+}
+
+/*
+ * Sets a matrix to zero.
+ */
+
+void
+MAT3zero (MAT3mat mat)
+{
+ bzero (mat, sizeof(MAT3mat));
+}
+
+
+/*
+ * Copies one matrix to another.
+ */
+
+void
+MAT3copy(MAT3mat to, MAT3mat from)
+{
+ bcopy (from, to, sizeof(MAT3mat));
+}
+
+/*
+ * This multiplies two matrices, producing a third, which may the same as
+ * either of the first two.
+ */
+
+void
+MAT3mult (result_mat, mat1, mat2)
+MAT3mat result_mat;
+register MAT3mat mat1, mat2;
+{
+ register int i, j;
+ MAT3mat tmp_mat;
+
+ for (i = 0; i < 4; i++)
+ for (j = 0; j < 4; j++)
+ tmp_mat[i][j] = (mat1[i][0] * mat2[0][j] +
+ mat1[i][1] * mat2[1][j] +
+ mat1[i][2] * mat2[2][j] +
+ mat1[i][3] * mat2[3][j]);
+ MAT3copy (result_mat, tmp_mat);
+}
+
+/*
+ * This returns the transpose of a matrix. The result matrix may be
+ * the same as the one to transpose.
+ */
+
+void
+MAT3transpose (result_mat, mat)
+MAT3mat result_mat;
+register MAT3mat mat;
+{
+ register int i, j;
+ MAT3mat tmp_mat;
+
+ for (i = 0; i < 4; i++)
+ for (j = 0; j < 4; j++)
+ tmp_mat[i][j] = mat[j][i];
+
+ MAT3copy (result_mat, tmp_mat);
+}
+
+
+/*
+ * This prints the given matrix to the given file pointer.
+ */
+
+void
+MAT3print(mat, fp)
+MAT3mat mat;
+FILE *fp;
+{
+ MAT3print_formatted(mat, fp, CNULL, CNULL, CNULL, CNULL);
+}
+
+/*
+ * This prints the given matrix to the given file pointer.
+ * use the format string to pass to fprintf. head and tail
+ * are printed at the beginning and end of each line.
+ */
+
+void
+MAT3print_formatted(mat, fp, title, head, format, tail)
+MAT3mat mat;
+FILE *fp;
+char *title, *head, *format, *tail;
+{
+ register int i, j;
+
+ /* This is to allow this to be called easily from a debugger */
+ if (fp == NULL) fp = stderr;
+
+ if (title == NULL) title = "MAT3 matrix:\n";
+ if (head == NULL) head = " ";
+ if (format == NULL) format = "%#8.4lf ";
+ if (tail == NULL) tail = "\n";
+
+ (void) fprintf(fp, title);
+
+ for (i = 0; i < 4; i++) {
+ (void) fprintf(fp, head);
+ for (j = 0; j < 4; j++) (void) fprintf(fp, format, mat[i][j]);
+ (void) fprintf(fp, tail);
+ }
+}
--- /dev/null
+/* Copyright 1988, Brown Computer Graphics Group. All Rights Reserved. */
+
+/* --------------------------------------------------------------------------
+ * This file contains routines that operate on matrices and vectors, or
+ * vectors and vectors.
+ * -------------------------------------------------------------------------*/
+
+/* #include "sphigslocal.h" */
+
+/* -------------------------- Static Routines ---------------------------- */
+
+/* ------------------------- Internal Routines --------------------------- */
+
+/* -------------------------- Public Routines ---------------------------- */
+
+/*
+ * Multiplies a vector by a matrix, setting the result vector.
+ * It assumes all homogeneous coordinates are 1.
+ * The two vectors involved may be the same.
+ */
+
+#include "mat3.h"
+
+#ifndef TRUE
+# define TRUE 1
+#endif
+
+#ifndef FALSE
+# define FALSE 0
+#endif
+
+
+void
+MAT3mult_vec(result_vec, vec, mat)
+MAT3vec result_vec;
+register MAT3vec vec;
+register MAT3mat mat;
+{
+ MAT3vec tempvec;
+ register double *temp = tempvec;
+
+ temp[0] = vec[0] * mat[0][0] + vec[1] * mat[1][0] +
+ vec[2] * mat[2][0] + mat[3][0];
+ temp[1] = vec[0] * mat[0][1] + vec[1] * mat[1][1] +
+ vec[2] * mat[2][1] + mat[3][1];
+ temp[2] = vec[0] * mat[0][2] + vec[1] * mat[1][2] +
+ vec[2] * mat[2][2] + mat[3][2];
+
+ MAT3_COPY_VEC(result_vec, temp);
+}
+
+/*
+ * Multiplies a vector of size 4 by a matrix, setting the result vector.
+ * The fourth element of the vector is the homogeneous coordinate, which
+ * may or may not be 1. If the "normalize" parameter is TRUE, then the
+ * result vector will be normalized so that the homogeneous coordinate is 1.
+ * The two vectors involved may be the same.
+ * This returns zero if the vector was to be normalized, but couldn't be.
+ */
+
+int
+MAT3mult_hvec(result_vec, vec, mat, normalize)
+MAT3hvec result_vec;
+register MAT3hvec vec;
+register MAT3mat mat;
+{
+ MAT3hvec tempvec;
+ double norm_fac;
+ register double *temp = tempvec;
+ register int ret = TRUE;
+
+ temp[0] = vec[0] * mat[0][0] + vec[1] * mat[1][0] +
+ vec[2] * mat[2][0] + vec[3] * mat[3][0];
+ temp[1] = vec[0] * mat[0][1] + vec[1] * mat[1][1] +
+ vec[2] * mat[2][1] + vec[3] * mat[3][1];
+ temp[2] = vec[0] * mat[0][2] + vec[1] * mat[1][2] +
+ vec[2] * mat[2][2] + vec[3] * mat[3][2];
+ temp[3] = vec[0] * mat[0][3] + vec[1] * mat[1][3] +
+ vec[2] * mat[2][3] + vec[3] * mat[3][3];
+
+ /* Normalize if asked for, possible, and necessary */
+ if (normalize) {
+ if (MAT3_IS_ZERO(temp[3])) {
+#ifndef THINK_C
+ fprintf (stderr,
+ "Can't normalize vector: homogeneous coordinate is 0");
+#endif
+ ret = FALSE;
+ }
+ else {
+ norm_fac = 1.0 / temp[3];
+ MAT3_SCALE_VEC(result_vec, temp, norm_fac);
+ result_vec[3] = 1.0;
+ }
+ }
+ else MAT3_COPY_HVEC(result_vec, temp);
+
+ return(ret);
+}
+
+/*
+ * Sets the first vector to be the cross-product of the last two vectors.
+ */
+
+void
+MAT3cross_product(result_vec, vec1, vec2)
+MAT3vec result_vec;
+register MAT3vec vec1, vec2;
+{
+ MAT3vec tempvec;
+ register double *temp = tempvec;
+
+ temp[0] = vec1[1] * vec2[2] - vec1[2] * vec2[1];
+ temp[1] = vec1[2] * vec2[0] - vec1[0] * vec2[2];
+ temp[2] = vec1[0] * vec2[1] - vec1[1] * vec2[0];
+
+ MAT3_COPY_VEC(result_vec, temp);
+}
+
+/*
+ * Finds a vector perpendicular to vec and stores it in result_vec.
+ * Method: take any vector (we use <0,1,0>) and subtract the
+ * portion of it pointing in the vec direction. This doesn't
+ * work if vec IS <0,1,0> or is very near it. So if this is
+ * the case, use <0,0,1> instead.
+ * If "is_unit" is TRUE, the given vector is assumed to be unit length.
+ */
+
+#define SELECT .7071 /* selection constant (roughly .5*sqrt(2) */
+
+void
+MAT3perp_vec(result_vec, vec, is_unit)
+MAT3vec result_vec, vec;
+int is_unit;
+{
+ MAT3vec norm;
+ double dot;
+
+ MAT3_SET_VEC(result_vec, 0.0, 1.0, 0.0);
+
+ MAT3_COPY_VEC(norm, vec);
+
+ if (! is_unit) MAT3_NORMALIZE_VEC(norm, dot);
+
+ /* See if vector is too close to <0,1,0>. If so, use <0,0,1> */
+ if ((dot = MAT3_DOT_PRODUCT(norm, result_vec)) > SELECT || dot < -SELECT) {
+ result_vec[1] = 0.0;
+ result_vec[2] = 1.0;
+ dot = MAT3_DOT_PRODUCT(norm, result_vec);
+ }
+
+ /* Subtract off non-perpendicular part */
+ result_vec[0] -= dot * norm[0];
+ result_vec[1] -= dot * norm[1];
+ result_vec[2] -= dot * norm[2];
+
+ /* Make result unit length */
+ MAT3_NORMALIZE_VEC(result_vec, dot);
+}
--- /dev/null
+#---------------------------------------------------------------------------
+# Makefile
+#
+# Written by Curtis Olson, started May 1997.
+#
+# Copyright (C) 1997 Curtis L. Olson - curt@infoplane.com
+#
+# This program is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+#
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+#
+# $Id$
+# (Log is kept at end of this file)
+#---------------------------------------------------------------------------
+
+
+TARGET = libmat3.a
+
+CFILES = MAT3geom.c MAT3inv.c MAT3mat.c MAT3vec.c
+HFILES = mat3.h mat3defs.h mat3err.h
+OFILES = $(CFILES:.c=.o)
+
+CC = gcc
+CFLAGS = -g -Wall
+# CFLAGS = -O2 -Wall
+
+AR = ar
+
+INCLUDES =
+
+LIBS =
+
+
+#---------------------------------------------------------------------------
+# Primary Targets
+#---------------------------------------------------------------------------
+
+$(TARGET): $(OFILES) $(HFILES)
+ $(AR) rv $(TARGET) $(OFILES)
+
+all: $(TARGET)
+
+clean:
+ rm -f *.o $(TARGET) *~ core
+
+
+#---------------------------------------------------------------------------
+# Secondary Targets
+#---------------------------------------------------------------------------
+
+
+
+#---------------------------------------------------------------------------
+# $Log$
+# Revision 1.1 1997/05/30 19:25:56 curt
+# The MAT3 routines from SRGP.
+#
+# Revision 1.2 1997/05/23 15:40:29 curt
+# Added GNU copyright headers.
+#
+# Revision 1.1 1997/05/16 15:58:23 curt
+# Initial revision.
+#
--- /dev/null
+/* Copyright 1988, Brown Computer Graphics Group. All Rights Reserved. */
+
+/* -------------------------------------------------------------------------
+ Public MAT3 include file
+ ------------------------------------------------------------------------- */
+
+#ifndef MAT3_HAS_BEEN_INCLUDED
+#define MAT3_HAS_BEEN_INCLUDED
+
+/* ----------------------------- Constants ------------------------------ */
+
+/*
+ * Make sure the math library .h file is included, in case it wasn't.
+ */
+
+#ifndef HUGE
+#include <math.h>
+#endif
+#include <stdio.h>
+
+
+#define MAT3_DET0 -1 /* Indicates singular mat */
+#define MAT3_EPSILON 1e-12 /* Close enough to zero */
+#define MAT3_PI 3.141592653589793 /* Pi */
+
+/* ------------------------------ Types --------------------------------- */
+
+typedef double MAT3mat[4][4]; /* 4x4 matrix */
+typedef double MAT3vec[3]; /* Vector */
+typedef double MAT3hvec[4]; /* Vector with homogeneous coord */
+
+/* ------------------------------ Macros -------------------------------- */
+
+/* Tests if a number is within EPSILON of zero */
+#define MAT3_IS_ZERO(N) ((N) < MAT3_EPSILON && (N) > -MAT3_EPSILON)
+
+/* Sets a vector to the three given values */
+#define MAT3_SET_VEC(V,X,Y,Z) ((V)[0]=(X), (V)[1]=(Y), (V)[2]=(Z))
+
+/* Tests a vector for all components close to zero */
+#define MAT3_IS_ZERO_VEC(V) (MAT3_IS_ZERO((V)[0]) && \
+ MAT3_IS_ZERO((V)[1]) && \
+ MAT3_IS_ZERO((V)[2]))
+
+/* Dot product of two vectors */
+#define MAT3_DOT_PRODUCT(V1,V2) \
+ ((V1)[0]*(V2)[0] + (V1)[1]*(V2)[1] + (V1)[2]*(V2)[2])
+
+/* Copy one vector to other */
+#define MAT3_COPY_VEC(TO,FROM) ((TO)[0] = (FROM)[0], \
+ (TO)[1] = (FROM)[1], \
+ (TO)[2] = (FROM)[2])
+
+/* Normalize vector to unit length, using TEMP as temporary variable.
+ * TEMP will be zero if vector has zero length */
+#define MAT3_NORMALIZE_VEC(V,TEMP) \
+ if ((TEMP = sqrt(MAT3_DOT_PRODUCT(V,V))) > MAT3_EPSILON) { \
+ TEMP = 1.0 / TEMP; \
+ MAT3_SCALE_VEC(V,V,TEMP); \
+ } else TEMP = 0.0
+
+/* Scale vector by given factor, storing result vector in RESULT_V */
+#define MAT3_SCALE_VEC(RESULT_V,V,SCALE) \
+ MAT3_SET_VEC(RESULT_V, (V)[0]*(SCALE), (V)[1]*(SCALE), (V)[2]*(SCALE))
+
+/* Adds vectors V1 and V2, storing result in RESULT_V */
+#define MAT3_ADD_VEC(RESULT_V,V1,V2) \
+ MAT3_SET_VEC(RESULT_V, (V1)[0]+(V2)[0], (V1)[1]+(V2)[1], \
+ (V1)[2]+(V2)[2])
+
+/* Subtracts vector V2 from V1, storing result in RESULT_V */
+#define MAT3_SUB_VEC(RESULT_V,V1,V2) \
+ MAT3_SET_VEC(RESULT_V, (V1)[0]-(V2)[0], (V1)[1]-(V2)[1], \
+ (V1)[2]-(V2)[2])
+
+/* Multiplies vectors V1 and V2, storing result in RESULT_V */
+#define MAT3_MULT_VEC(RESULT_V,V1,V2) \
+ MAT3_SET_VEC(RESULT_V, (V1)[0]*(V2)[0], (V1)[1]*(V2)[1], \
+ (V1)[2]*(V2)[2])
+
+/* Sets RESULT_V to the linear combination of V1 and V2, scaled by
+ * SCALE1 and SCALE2, respectively */
+#define MAT3_LINEAR_COMB(RESULT_V,SCALE1,V1,SCALE2,V2) \
+ MAT3_SET_VEC(RESULT_V, (SCALE1)*(V1)[0] + (SCALE2)*(V2)[0], \
+ (SCALE1)*(V1)[1] + (SCALE2)*(V2)[1], \
+ (SCALE1)*(V1)[2] + (SCALE2)*(V2)[2])
+
+/* Several of the vector macros are useful for homogeneous-coord vectors */
+#define MAT3_SET_HVEC(V,X,Y,Z,W) ((V)[0]=(X), (V)[1]=(Y), \
+ (V)[2]=(Z), (V)[3]=(W))
+
+#define MAT3_COPY_HVEC(TO,FROM) ((TO)[0] = (FROM)[0], \
+ (TO)[1] = (FROM)[1], \
+ (TO)[2] = (FROM)[2], \
+ (TO)[3] = (FROM)[3])
+
+#define MAT3_SCALE_HVEC(RESULT_V,V,SCALE) \
+ MAT3_SET_HVEC(RESULT_V, (V)[0]*(SCALE), (V)[1]*(SCALE), \
+ (V)[2]*(SCALE), (V)[3]*(SCALE))
+
+#define MAT3_ADD_HVEC(RESULT_V,V1,V2) \
+ MAT3_SET_HVEC(RESULT_V, (V1)[0]+(V2)[0], (V1)[1]+(V2)[1], \
+ (V1)[2]+(V2)[2], (V1)[3]+(V2)[3])
+
+#define MAT3_SUB_HVEC(RESULT_V,V1,V2) \
+ MAT3_SET_HVEC(RESULT_V, (V1)[0]-(V2)[0], (V1)[1]-(V2)[1], \
+ (V1)[2]-(V2)[2], (V1)[3]-(V2)[3])
+
+#define MAT3_MULT_HVEC(RESULT_V,V1,V2) \
+ MAT3_SET_HVEC(RESULT_V, (V1)[0]*(V2)[0], (V1)[1]*(V2)[1], \
+ (V1)[2]*(V2)[2], (V1)[3]*(V2)[3])
+
+/* ------------------------------ Entries ------------------------------- */
+
+
+/* In MAT3geom.c */
+void MAT3direction_matrix (MAT3mat result_mat, MAT3mat mat);
+int MAT3normal_matrix (MAT3mat result_mat, MAT3mat mat);
+void MAT3rotate (MAT3mat result_mat, MAT3vec axis, double angle_in_radians);
+void MAT3translate (MAT3mat result_mat, MAT3vec trans);
+void MAT3scale (MAT3mat result_mat, MAT3vec scale);
+void MAT3shear(MAT3mat result_mat, double xshear, double yshear);
+
+/* In MAT3mat.c */
+void MAT3identity(MAT3mat);
+void MAT3zero(MAT3mat);
+void MAT3copy (MAT3mat to, MAT3mat from);
+void MAT3mult (MAT3mat result, MAT3mat, MAT3mat);
+void MAT3transpose (MAT3mat result, MAT3mat);
+int MAT3invert (MAT3mat result, MAT3mat);
+void MAT3print (MAT3mat, FILE *fp);
+void MAT3print_formatted (MAT3mat, FILE *fp,
+ char *title, char *head, char *format, char *tail);
+extern int MAT3equal();
+extern double MAT3trace();
+extern int MAT3power();
+extern int MAT3column_reduce();
+extern int MAT3kernel_basis();
+
+/* In MAT3vec.c */
+void MAT3mult_vec(MAT3vec result_vec, MAT3vec vec, MAT3mat mat);
+int MAT3mult_hvec (MAT3hvec result_vec, MAT3hvec vec, MAT3mat mat, int normalize);
+void MAT3cross_product(MAT3vec result,MAT3vec,MAT3vec);
+void MAT3perp_vec(MAT3vec result_vec, MAT3vec vec, int is_unit);
+
+#endif MAT3_HAS_BEEN_INCLUDED
+
--- /dev/null
+/* Copyright 1988, Brown Computer Graphics Group. All Rights Reserved. */
+
+#include <stdio.h>
+/* #include "mat3err.h" */
+#include "mat3.h"
+
+/* ----------------------------- Constants ------------------------------ */
+
+#define FALSE 0
+#define TRUE 1
+
+#define CNULL ((char *) NULL)
+
+/* ------------------------------ Macros -------------------------------- */
+
+#define ALLOCN(P,T,N,M) \
+ if ((P = (T *) malloc((unsigned) (N) * sizeof(T))) == NULL) \
+ ERR_ERROR(MAT3_errid, ERR_FATAL, (ERR_ALLOC1, M)); \
+ else
+
+#define FREE(P) free((char *) (P))
+
+#define ABS(A) ((A) > 0 ? (A) : -(A))
+#define MIN(A,B) ((A) < (B) ? (A) : (B))
+#define MAX(A,B) ((A) > (B) ? (A) : (B))
+
+#define SWAP(A,B,T) (T = A, A = B, B = T)
+
+/* Is N within EPS of zero ? */
+#define IS_ZERO(N,EPS) ((N) < EPS && (N) > -EPS)
+
+/* Macros for lu routines */
+#define LU_PERMUTE(p,i,j) { int LU_T; LU_T = p[i]; p[i] = p[j]; p[j] = LU_T; }
+
+/* ------------------------- Internal Entries ---------------------------- */
+
+/* ------------------------- Global Variables ---------------------------- */
+
+/* extern ERRid *MAT3_errid; */
--- /dev/null
+#include "sph_errtypes.h"
+
+#ifdef THINK_C
+/* We hide this from gnu's compiler, which doesn't understand it. */
+void SPH__error (int errtype, ...);
+#endif
+
+
+#define ERR_ERROR(A,B,C) \
+ if (1) {char cstr[256]; sprintf C; SPH__error(ERR_MAT3_PACKAGE, cstr); } else
+
+
+#define ERR_S cstr,"%s\n"
+#define ERR_SI cstr,"%s: %d\n"
+#define ERR_SS cstr,"%s: %s\n"
+
+#define ERR_SEVERE 0
+#define ERR_FATAL 0
+
+#define ERR_ALLOC1 0
+
+typedef int ERRid;
+
+#define ERRregister_package(S) 100
+
+
projects / simgear.git / commitdiff