From: curt Date: Fri, 30 May 1997 19:25:54 +0000 (+0000) Subject: The MAT3 routines from SRGP. X-Git-Url: https://git.mxchange.org/?a=commitdiff_plain;h=935d4f3bda736eaa434dc9d405dabe584ad62667;p=simgear.git The MAT3 routines from SRGP. --- 935d4f3bda736eaa434dc9d405dabe584ad62667 diff --git a/Math/MAT3geom.c b/Math/MAT3geom.c new file mode 100644 index 00000000..76ebe2e5 --- /dev/null +++ b/Math/MAT3geom.c @@ -0,0 +1,177 @@ +/* #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; +} + diff --git a/Math/MAT3inv.c b/Math/MAT3inv.c new file mode 100644 index 00000000..f5e72a30 --- /dev/null +++ b/Math/MAT3inv.c @@ -0,0 +1,312 @@ +/* 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); +} diff --git a/Math/MAT3mat.c b/Math/MAT3mat.c new file mode 100644 index 00000000..0e1ea559 --- /dev/null +++ b/Math/MAT3mat.c @@ -0,0 +1,138 @@ +/* #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); + } +} diff --git a/Math/MAT3vec.c b/Math/MAT3vec.c new file mode 100644 index 00000000..9e71042b --- /dev/null +++ b/Math/MAT3vec.c @@ -0,0 +1,159 @@ +/* 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); +} diff --git a/Math/Makefile b/Math/Makefile new file mode 100644 index 00000000..d2e307ae --- /dev/null +++ b/Math/Makefile @@ -0,0 +1,73 @@ +#--------------------------------------------------------------------------- +# 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. +# diff --git a/Math/mat3.h b/Math/mat3.h new file mode 100644 index 00000000..70f7a616 --- /dev/null +++ b/Math/mat3.h @@ -0,0 +1,147 @@ +/* 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 +#endif +#include + + +#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 + diff --git a/Math/mat3defs.h b/Math/mat3defs.h new file mode 100644 index 00000000..34759c0d --- /dev/null +++ b/Math/mat3defs.h @@ -0,0 +1,39 @@ +/* Copyright 1988, Brown Computer Graphics Group. All Rights Reserved. */ + +#include +/* #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; */ diff --git a/Math/mat3err.h b/Math/mat3err.h new file mode 100644 index 00000000..d2c5e4f9 --- /dev/null +++ b/Math/mat3err.h @@ -0,0 +1,26 @@ +#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 + +