Removed mat3.h et. al. plib/sg.h provides a superset of mat3.h, has a
cleaner design, and is something that we are already linking in.
This commit is contained in:
curt
@@ -1,168 +0,0 @@
|
||||
/* #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(register double (*result_mat)[4], register double (*mat)[4])
|
||||
{
|
||||
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(register double (*result_mat)[4], register double (*mat)[4])
|
||||
{
|
||||
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(double (*result_mat)[4], double *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(double (*result_mat)[4], double *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(double (*result_mat)[4], double *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(double (*result_mat)[4], double xshear, double yshear)
|
||||
{
|
||||
MAT3identity(result_mat);
|
||||
|
||||
result_mat[2][0] = xshear;
|
||||
result_mat[2][1] = yshear;
|
||||
}
|
||||
|
||||
@@ -1,311 +0,0 @@
|
||||
/* 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(double (*result_mat)[4], double (*mat)[4])
|
||||
{
|
||||
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);
|
||||
}
|
||||
@@ -1,120 +0,0 @@
|
||||
/* #include "HEADERS.h" */
|
||||
/* Copyright 1988, Brown Computer Graphics Group. All Rights Reserved. */
|
||||
|
||||
/* --------------------------------------------------------------------------
|
||||
* This file contains routines that operate solely on matrices.
|
||||
* -------------------------------------------------------------------------*/
|
||||
|
||||
|
||||
#ifdef HAVE_CONFIG_H
|
||||
# include <config.h>
|
||||
#endif
|
||||
|
||||
#ifdef WIN32
|
||||
# ifndef HAVE_STL_SGI_PORT
|
||||
# ifdef __BORLANDC__
|
||||
# include <mem.h>
|
||||
# else
|
||||
# include <memory.h> /* required for memset() and memcpy() */
|
||||
# endif
|
||||
# endif
|
||||
#endif
|
||||
|
||||
#include <string.h>
|
||||
#include "mat3defs.h"
|
||||
|
||||
MAT3mat identityMatrix = {
|
||||
{ 1.0, 0.0, 0.0, 0.0 },
|
||||
{ 0.0, 1.0, 0.0, 0.0 },
|
||||
{ 0.0, 0.0, 1.0, 0.0 },
|
||||
{ 0.0, 0.0, 0.0, 1.0 }
|
||||
};
|
||||
|
||||
/* #include "macros.h" */
|
||||
|
||||
/* -------------------------- Static Routines ---------------------------- */
|
||||
|
||||
/* ------------------------- Internal Routines --------------------------- */
|
||||
|
||||
/* -------------------------- Public Routines ---------------------------- */
|
||||
|
||||
|
||||
#if !defined( USE_XTRA_MAT3_INLINES )
|
||||
|
||||
/*
|
||||
* This multiplies two matrices, producing a third, which may the same as
|
||||
* either of the first two.
|
||||
*/
|
||||
|
||||
void
|
||||
MAT3mult (double (*result_mat)[4], register double (*mat1)[4], register double (*mat2)[4])
|
||||
{
|
||||
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);
|
||||
}
|
||||
#endif // !defined( USE_XTRA_MAT3_INLINES )
|
||||
|
||||
/*
|
||||
* This returns the transpose of a matrix. The result matrix may be
|
||||
* the same as the one to transpose.
|
||||
*/
|
||||
|
||||
void
|
||||
MAT3transpose (double (*result_mat)[4], register double (*mat)[4])
|
||||
{
|
||||
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(double (*mat)[4], 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(double (*mat)[4], FILE *fp, char *title, char *head, char *format, char *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);
|
||||
}
|
||||
}
|
||||
@@ -1,154 +0,0 @@
|
||||
/* 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
|
||||
|
||||
#if !defined( USE_XTRA_MAT3_INLINES )
|
||||
|
||||
void
|
||||
MAT3mult_vec(double *result_vec, register double *vec, register double (*mat)[4])
|
||||
{
|
||||
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);
|
||||
}
|
||||
#endif // !defined( USE_XTRA_MAT3_INLINES )
|
||||
|
||||
/*
|
||||
* 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(double *result_vec, register double *vec, register double (*mat)[4], int normalize)
|
||||
{
|
||||
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);
|
||||
}
|
||||
|
||||
#if !defined( USE_XTRA_MAT3_INLINES )
|
||||
|
||||
/*
|
||||
* Sets the first vector to be the cross-product of the last two vectors.
|
||||
*/
|
||||
|
||||
void
|
||||
MAT3cross_product(double *result_vec, register double *vec1, register double *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);
|
||||
}
|
||||
#endif // !defined( USE_XTRA_MAT3_INLINES )
|
||||
|
||||
/*
|
||||
* 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(double *result_vec, double *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);
|
||||
}
|
||||
@@ -15,7 +15,6 @@ include_HEADERS = \
|
||||
fg_types.hxx \
|
||||
interpolater.hxx \
|
||||
leastsqs.hxx \
|
||||
mat3.h \
|
||||
point3d.hxx \
|
||||
polar3d.hxx \
|
||||
vector.hxx
|
||||
@@ -23,15 +22,10 @@ include_HEADERS = \
|
||||
EXTRA_DIST = linintp2.h linintp2.inl sphrintp.h sphrintp.inl
|
||||
|
||||
libsgmath_a_SOURCES = \
|
||||
MAT3geom.c \
|
||||
MAT3inv.c \
|
||||
MAT3mat.c \
|
||||
MAT3vec.c \
|
||||
fg_geodesy.cxx \
|
||||
fg_random.c \
|
||||
interpolater.cxx \
|
||||
leastsqs.cxx \
|
||||
mat3defs.h mat3err.h \
|
||||
polar3d.cxx \
|
||||
vector.cxx
|
||||
|
||||
|
||||
@@ -1,233 +0,0 @@
|
||||
/* 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>
|
||||
|
||||
#include <string.h>
|
||||
#include <simgear/math/fg_memory.h>
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
|
||||
#define MAT3_DET0 -1 /* Indicates singular mat */
|
||||
#define MAT3_EPSILON 1e-12 /* Close enough to zero */
|
||||
|
||||
#ifdef M_PI
|
||||
# define MAT3_PI M_PI
|
||||
#else
|
||||
# define MAT3_PI 3.14159265358979323846
|
||||
#endif
|
||||
|
||||
#define USE_XTRA_MAT3_INLINES
|
||||
|
||||
#if defined(i386)
|
||||
#define USE_X86_ASM
|
||||
#endif
|
||||
|
||||
#if defined(USE_X86_ASM)
|
||||
static __inline__ int FloatToInt(float f)
|
||||
{
|
||||
int r;
|
||||
__asm__ ("fistpl %0" : "=m" (r) : "t" (f) : "st");
|
||||
return r;
|
||||
}
|
||||
#elif defined(__MSC__) && defined(__WIN32__)
|
||||
static __inline int FloatToInt(float f)
|
||||
{
|
||||
int r;
|
||||
_asm {
|
||||
fld f
|
||||
fistp r
|
||||
}
|
||||
return r;
|
||||
}
|
||||
#else
|
||||
#define FloatToInt(F) ((int) ((F) < 0.0f ? (F)-0.5f : (F)+0.5f))
|
||||
#endif
|
||||
|
||||
/* ------------------------------ Types --------------------------------- */
|
||||
|
||||
typedef double MAT3mat[4][4]; /* 4x4 matrix */
|
||||
typedef double MAT3vec[3]; /* Vector */
|
||||
typedef double MAT3hvec[4]; /* Vector with homogeneous coord */
|
||||
|
||||
/* ------------------------------ Macros -------------------------------- */
|
||||
|
||||
extern MAT3mat identityMatrix;
|
||||
|
||||
/* 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 ------------------------------- */
|
||||
|
||||
|
||||
#define MAT3identity(mat) fgmemcpy( mat, identityMatrix, sizeof(MAT3mat) )
|
||||
#define MAT3zero(mat) fgmemzero( mat, sizeof(MAT3mat) )
|
||||
#define MAT3copy(to, from) fgmemcpy( to, from, sizeof(MAT3mat) )
|
||||
|
||||
#if defined( USE_XTRA_MAT3_INLINES )
|
||||
|
||||
# define MAT3mult_vec( result_vec, vec, mat) { \
|
||||
MAT3vec tempvec; \
|
||||
tempvec[0]=vec[0]*mat[0][0]+vec[1]*mat[1][0]+vec[2]*mat[2][0]+mat[3][0]; \
|
||||
tempvec[1]=vec[0]*mat[0][1]+vec[1]*mat[1][1]+vec[2]*mat[2][1]+mat[3][1]; \
|
||||
tempvec[2]=vec[0]*mat[0][2]+vec[1]*mat[1][2]+vec[2]*mat[2][2]+mat[3][2]; \
|
||||
result_vec[0] = tempvec[0]; \
|
||||
result_vec[1] = tempvec[1]; \
|
||||
result_vec[2] = tempvec[2]; \
|
||||
}
|
||||
|
||||
# define MAT3cross_product(result_vec, vec1, vec2) { \
|
||||
MAT3vec tempvec; \
|
||||
tempvec[0] = vec1[1] * vec2[2] - vec1[2] * vec2[1]; \
|
||||
tempvec[1] = vec1[2] * vec2[0] - vec1[0] * vec2[2]; \
|
||||
tempvec[2] = vec1[0] * vec2[1] - vec1[1] * vec2[0]; \
|
||||
result_vec[0] = tempvec[0]; \
|
||||
result_vec[1] = tempvec[1]; \
|
||||
result_vec[2] = tempvec[2]; \
|
||||
}
|
||||
|
||||
# define MAT3mult( result_mat, 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]); \
|
||||
fgmemcpy(result_mat, tmp_mat, sizeof(MAT3mat)); \
|
||||
}
|
||||
|
||||
#else // !defined( USE_XTRA_MAT3_INLINES )
|
||||
|
||||
/* In MAT3mat.c */
|
||||
void MAT3mult(MAT3mat result, MAT3mat, MAT3mat);
|
||||
void MAT3mult_vec(MAT3vec result_vec, MAT3vec vec, MAT3mat mat);
|
||||
void MAT3cross_product(MAT3vec result,MAT3vec,MAT3vec);
|
||||
|
||||
#endif // defined( USE_XTRA_MAT3_INLINES )
|
||||
|
||||
/* 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);
|
||||
|
||||
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);
|
||||
int MAT3equal( void );
|
||||
double MAT3trace( void );
|
||||
int MAT3power( void );
|
||||
int MAT3column_reduce( void );
|
||||
int MAT3kernel_basis( void );
|
||||
|
||||
/* In MAT3vec.c */
|
||||
int MAT3mult_hvec (MAT3hvec result_vec, MAT3hvec vec, MAT3mat mat, int normalize);
|
||||
void MAT3perp_vec(MAT3vec result_vec, MAT3vec vec, int is_unit);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
#endif /* MAT3_HAS_BEEN_INCLUDED */
|
||||
|
||||
@@ -1,56 +0,0 @@
|
||||
/* Copyright 1988, Brown Computer Graphics Group. All Rights Reserved. */
|
||||
|
||||
|
||||
#ifndef _MAT3DEFS_H
|
||||
#define _MAT3DEFS_H
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
#include <stdio.h>
|
||||
/* #include <Math/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; */
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
#endif /* _MAT3DEFS_H */
|
||||
@@ -1,41 +0,0 @@
|
||||
#ifndef _MAT3ERR_H
|
||||
#define _MAT3ERR_H
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
|
||||
#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
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
#endif /* _MAT3ERR_H */
|
||||
@@ -28,102 +28,6 @@
|
||||
|
||||
#include "vector.hxx"
|
||||
|
||||
#include "mat3.h"
|
||||
|
||||
|
||||
// Map a vector onto the plane specified by normal
|
||||
void map_vec_onto_cur_surface_plane(MAT3vec normal, MAT3vec v0, MAT3vec vec,
|
||||
MAT3vec result)
|
||||
{
|
||||
MAT3vec u1, v, tmp;
|
||||
|
||||
// calculate a vector "u1" representing the shortest distance from
|
||||
// the plane specified by normal and v0 to a point specified by
|
||||
// "vec". "u1" represents both the direction and magnitude of
|
||||
// this desired distance.
|
||||
|
||||
// u1 = ( (normal <dot> vec) / (normal <dot> normal) ) * normal
|
||||
|
||||
MAT3_SCALE_VEC( u1,
|
||||
normal,
|
||||
( MAT3_DOT_PRODUCT(normal, vec) /
|
||||
MAT3_DOT_PRODUCT(normal, normal)
|
||||
)
|
||||
);
|
||||
|
||||
// printf(" vec = %.2f, %.2f, %.2f\n", vec[0], vec[1], vec[2]);
|
||||
// printf(" v0 = %.2f, %.2f, %.2f\n", v0[0], v0[1], v0[2]);
|
||||
// printf(" u1 = %.2f, %.2f, %.2f\n", u1[0], u1[1], u1[2]);
|
||||
|
||||
// calculate the vector "v" which is the vector "vec" mapped onto
|
||||
// the plane specified by "normal" and "v0".
|
||||
|
||||
// v = v0 + vec - u1
|
||||
|
||||
MAT3_ADD_VEC(tmp, v0, vec);
|
||||
MAT3_SUB_VEC(v, tmp, u1);
|
||||
// printf(" v = %.2f, %.2f, %.2f\n", v[0], v[1], v[2]);
|
||||
|
||||
// Calculate the vector "result" which is "v" - "v0" which is a
|
||||
// directional vector pointing from v0 towards v
|
||||
|
||||
// result = v - v0
|
||||
|
||||
MAT3_SUB_VEC(result, v, v0);
|
||||
// printf(" result = %.2f, %.2f, %.2f\n",
|
||||
// result[0], result[1], result[2]);
|
||||
}
|
||||
|
||||
|
||||
// Given a point p, and a line through p0 with direction vector d,
|
||||
// find the shortest distance from the point to the line
|
||||
double fgPointLine(MAT3vec p, MAT3vec p0, MAT3vec d) {
|
||||
MAT3vec u, u1, v;
|
||||
double ud, dd, tmp;
|
||||
|
||||
// u = p - p0
|
||||
MAT3_SUB_VEC(u, p, p0);
|
||||
|
||||
// calculate the projection, u1, of u along d.
|
||||
// u1 = ( dot_prod(u, d) / dot_prod(d, d) ) * d;
|
||||
ud = MAT3_DOT_PRODUCT(u, d);
|
||||
dd = MAT3_DOT_PRODUCT(d, d);
|
||||
tmp = ud / dd;
|
||||
|
||||
MAT3_SCALE_VEC(u1, d, tmp);;
|
||||
|
||||
// v = u - u1 = vector from closest point on line, p1, to the
|
||||
// original point, p.
|
||||
MAT3_SUB_VEC(v, u, u1);
|
||||
|
||||
return sqrt(MAT3_DOT_PRODUCT(v, v));
|
||||
}
|
||||
|
||||
|
||||
// Given a point p, and a line through p0 with direction vector d,
|
||||
// find the shortest distance (squared) from the point to the line
|
||||
double fgPointLineSquared(MAT3vec p, MAT3vec p0, MAT3vec d) {
|
||||
MAT3vec u, u1, v;
|
||||
double ud, dd, tmp;
|
||||
|
||||
// u = p - p0
|
||||
MAT3_SUB_VEC(u, p, p0);
|
||||
|
||||
// calculate the projection, u1, of u along d.
|
||||
// u1 = ( dot_prod(u, d) / dot_prod(d, d) ) * d;
|
||||
ud = MAT3_DOT_PRODUCT(u, d);
|
||||
dd = MAT3_DOT_PRODUCT(d, d);
|
||||
tmp = ud / dd;
|
||||
|
||||
MAT3_SCALE_VEC(u1, d, tmp);;
|
||||
|
||||
// v = u - u1 = vector from closest point on line, p1, to the
|
||||
// original point, p.
|
||||
MAT3_SUB_VEC(v, u, u1);
|
||||
|
||||
return ( MAT3_DOT_PRODUCT(v, v) );
|
||||
}
|
||||
|
||||
|
||||
// Given a point p, and a line through p0 with direction vector d,
|
||||
// find the shortest distance (squared) from the point to the line
|
||||
|
||||
@@ -37,15 +37,6 @@
|
||||
|
||||
#include <plib/sg.h>
|
||||
|
||||
#include <simgear/math/mat3.h>
|
||||
|
||||
|
||||
// Map a vector onto the plane specified by normal
|
||||
void map_vec_onto_cur_surface_plane( MAT3vec normal,
|
||||
MAT3vec v0,
|
||||
MAT3vec vec,
|
||||
MAT3vec result );
|
||||
|
||||
|
||||
inline void sgmap_vec_onto_cur_surface_plane( sgVec3 normal,
|
||||
sgVec3 v0,
|
||||
@@ -92,16 +83,6 @@ inline void sgmap_vec_onto_cur_surface_plane( sgVec3 normal,
|
||||
}
|
||||
|
||||
|
||||
// Given a point p, and a line through p0 with direction vector d,
|
||||
// find the shortest distance from the point to the line
|
||||
double fgPointLine(MAT3vec p, MAT3vec p0, MAT3vec d);
|
||||
|
||||
|
||||
// Given a point p, and a line through p0 with direction vector d,
|
||||
// find the shortest distance (squared) from the point to the line
|
||||
double fgPointLineSquared(MAT3vec p, MAT3vec p0, MAT3vec d);
|
||||
|
||||
|
||||
// Given a point p, and a line through p0 with direction vector d,
|
||||
// find the shortest distance (squared) from the point to the line
|
||||
double sgPointLineDistSquared( const sgVec3 p, const sgVec3 p0,
|
||||
|
||||
Reference in New Issue
Block a user