FGFS-4.1/simgear
4
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

The MAT3 routines from SRGP.

Browse Source
This commit is contained in:
curt 1997-05-30 19:25:54 +00:00
commit 935d4f3bda
8 changed files with 1071 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

177
Math/MAT3geom.c Normal file
View File

@ -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;
}

312
Math/MAT3inv.c Normal file
View File

@ -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);
}

138
Math/MAT3mat.c Normal file
View File

@ -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);
}
}

159
Math/MAT3vec.c Normal file
View File

@ -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);
}

73
Math/Makefile Normal file
View File

@ -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.
#

147
Math/mat3.h Normal file
View File

@ -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 <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

39
Math/mat3defs.h Normal file
View File

@ -0,0 +1,39 @@
/* 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; */

26
Math/mat3err.h Normal file
View File

@ -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