File: [Development] / inventor / lib / database / src / sb / SbMatrix.c++ (download)
Revision 1.2, Sat Oct 14 10:46:07 2000 UTC (17 years ago) by jlim
Branch: MAIN
CVS Tags: release-2_1_5-9, release-2_1_5-8, release-2_1_5-10, HEAD
Changes since 1.1: +2 -6
lines
Fixed Bug 22, removed dependence on POSIX_SOURCE and _XOPEN_SOURCE, conform to
ANSI 'for' scoping rules (Bug 7), added proper type casts, replaced bcopy()
with memcpy(), and eliminated warnings about implicit function definitions.
|
/*
*
* Copyright (C) 2000 Silicon Graphics, Inc. All Rights Reserved.
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* This library 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
* Lesser General Public License for more details.
*
* Further, this software is distributed without any warranty that it is
* free of the rightful claim of any third person regarding infringement
* or the like. Any license provided herein, whether implied or
* otherwise, applies only to this software file. Patent licenses, if
* any, provided herein do not apply to combinations of this program with
* other software, or any other product whatsoever.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
* Contact information: Silicon Graphics, Inc., 1600 Amphitheatre Pkwy,
* Mountain View, CA 94043, or:
*
* http://www.sgi.com
*
* For further information regarding this notice, see:
*
* http://oss.sgi.com/projects/GenInfo/NoticeExplan/
*
*/
/*
* Copyright (C) 1990,91 Silicon Graphics, Inc.
*
_______________________________________________________________________
______________ S I L I C O N G R A P H I C S I N C . ____________
|
| $Revision: 1.2 $
|
| Classes:
| SbMatrix
|
| Author(s) : Nick Thompson, David Mott, Howard Look
|
| Notes:
| Matrices are stored in row-major order.
|
______________ S I L I C O N G R A P H I C S I N C . ____________
_______________________________________________________________________
*/
#include <Inventor/SbLinear.h>
// amount squared to figure if two floats are equal
// (used for operator == right now)
#define DELTA 1e-6
//
// Handy absolute value macro:
//
#define ABS(a) ((a) < 0.0 ? -(a) : (a))
//
// Macro for checking is a matrix is idenity.
//
#define IS_IDENTITY(matrix) ( \
(matrix[0][0] == 1.0) && \
(matrix[0][1] == 0.0) && \
(matrix[0][2] == 0.0) && \
(matrix[0][3] == 0.0) && \
(matrix[1][0] == 0.0) && \
(matrix[1][1] == 1.0) && \
(matrix[1][2] == 0.0) && \
(matrix[1][3] == 0.0) && \
(matrix[2][0] == 0.0) && \
(matrix[2][1] == 0.0) && \
(matrix[2][2] == 1.0) && \
(matrix[2][3] == 0.0) && \
(matrix[3][0] == 0.0) && \
(matrix[3][1] == 0.0) && \
(matrix[3][2] == 0.0) && \
(matrix[3][3] == 1.0))
//
// Constructor given all 16 elements in row-major order
//
SbMatrix::SbMatrix(float a11, float a12, float a13, float a14,
float a21, float a22, float a23, float a24,
float a31, float a32, float a33, float a34,
float a41, float a42, float a43, float a44)
{
matrix[0][0] = a11;
matrix[0][1] = a12;
matrix[0][2] = a13;
matrix[0][3] = a14;
matrix[1][0] = a21;
matrix[1][1] = a22;
matrix[1][2] = a23;
matrix[1][3] = a24;
matrix[2][0] = a31;
matrix[2][1] = a32;
matrix[2][2] = a33;
matrix[2][3] = a34;
matrix[3][0] = a41;
matrix[3][1] = a42;
matrix[3][2] = a43;
matrix[3][3] = a44;
}
//
// Constructor from a 4x4 array of elements
//
SbMatrix::SbMatrix(const SbMat &m)
{
matrix[0][0] = m[0][0];
matrix[0][1] = m[0][1];
matrix[0][2] = m[0][2];
matrix[0][3] = m[0][3];
matrix[1][0] = m[1][0];
matrix[1][1] = m[1][1];
matrix[1][2] = m[1][2];
matrix[1][3] = m[1][3];
matrix[2][0] = m[2][0];
matrix[2][1] = m[2][1];
matrix[2][2] = m[2][2];
matrix[2][3] = m[2][3];
matrix[3][0] = m[3][0];
matrix[3][1] = m[3][1];
matrix[3][2] = m[3][2];
matrix[3][3] = m[3][3];
}
////////////////////////////////////////////
//
// Setting matrix values
//
////////////////////////////////////////////
//
// Sets value from 4x4 array of elements
//
void
SbMatrix::setValue(const SbMat &m)
{
matrix[0][0] = m[0][0];
matrix[0][1] = m[0][1];
matrix[0][2] = m[0][2];
matrix[0][3] = m[0][3];
matrix[1][0] = m[1][0];
matrix[1][1] = m[1][1];
matrix[1][2] = m[1][2];
matrix[1][3] = m[1][3];
matrix[2][0] = m[2][0];
matrix[2][1] = m[2][1];
matrix[2][2] = m[2][2];
matrix[2][3] = m[2][3];
matrix[3][0] = m[3][0];
matrix[3][1] = m[3][1];
matrix[3][2] = m[3][2];
matrix[3][3] = m[3][3];
}
//
// Return an identity matrix
//
SbMatrix
SbMatrix::identity()
{
return SbMatrix(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);
}
//
// Sets matrix to be identity
//
void
SbMatrix::makeIdentity()
{
matrix[0][0] = 1.0;
matrix[0][1] = 0.0;
matrix[0][2] = 0.0;
matrix[0][3] = 0.0;
matrix[1][0] = 0.0;
matrix[1][1] = 1.0;
matrix[1][2] = 0.0;
matrix[1][3] = 0.0;
matrix[2][0] = 0.0;
matrix[2][1] = 0.0;
matrix[2][2] = 1.0;
matrix[2][3] = 0.0;
matrix[3][0] = 0.0;
matrix[3][1] = 0.0;
matrix[3][2] = 0.0;
matrix[3][3] = 1.0;
}
//
// Sets matrix to rotate by given rotation
//
void
SbMatrix::setRotate(const SbRotation &rotation)
{
rotation.getValue(*this);
}
//
// Sets matrix to scale by given uniform factor
//
void
SbMatrix::setScale(float s)
{
matrix[0][0] = s;
matrix[0][1] = 0.0;
matrix[0][2] = 0.0;
matrix[0][3] = 0.0;
matrix[1][0] = 0.0;
matrix[1][1] = s;
matrix[1][2] = 0.0;
matrix[1][3] = 0.0;
matrix[2][0] = 0.0;
matrix[2][1] = 0.0;
matrix[2][2] = s;
matrix[2][3] = 0.0;
matrix[3][0] = 0.0;
matrix[3][1] = 0.0;
matrix[3][2] = 0.0;
matrix[3][3] = 1.0;
}
//
// Sets matrix to scale by given vector
//
void
SbMatrix::setScale(const SbVec3f &s)
{
matrix[0][0] = s[0];
matrix[0][1] = 0.0;
matrix[0][2] = 0.0;
matrix[0][3] = 0.0;
matrix[1][0] = 0.0;
matrix[1][1] = s[1];
matrix[1][2] = 0.0;
matrix[1][3] = 0.0;
matrix[2][0] = 0.0;
matrix[2][1] = 0.0;
matrix[2][2] = s[2];
matrix[2][3] = 0.0;
matrix[3][0] = 0.0;
matrix[3][1] = 0.0;
matrix[3][2] = 0.0;
matrix[3][3] = 1.0;
}
//
// Sets matrix to translate by given vector
//
void
SbMatrix::setTranslate(const SbVec3f &t)
{
matrix[0][0] = 1.0;
matrix[0][1] = 0.0;
matrix[0][2] = 0.0;
matrix[0][3] = 0.0;
matrix[1][0] = 0.0;
matrix[1][1] = 1.0;
matrix[1][2] = 0.0;
matrix[1][3] = 0.0;
matrix[2][0] = 0.0;
matrix[2][1] = 0.0;
matrix[2][2] = 1.0;
matrix[2][3] = 0.0;
matrix[3][0] = t[0];
matrix[3][1] = t[1];
matrix[3][2] = t[2];
matrix[3][3] = 1.0;
}
////////////////////////////////////////////
//
// Returning matrix values and other info
//
////////////////////////////////////////////
//
// Returns 4x4 array of elements
//
void
SbMatrix::getValue(SbMat &m) const
{
m[0][0] = matrix[0][0];
m[0][1] = matrix[0][1];
m[0][2] = matrix[0][2];
m[0][3] = matrix[0][3];
m[1][0] = matrix[1][0];
m[1][1] = matrix[1][1];
m[1][2] = matrix[1][2];
m[1][3] = matrix[1][3];
m[2][0] = matrix[2][0];
m[2][1] = matrix[2][1];
m[2][2] = matrix[2][2];
m[2][3] = matrix[2][3];
m[3][0] = matrix[3][0];
m[3][1] = matrix[3][1];
m[3][2] = matrix[3][2];
m[3][3] = matrix[3][3];
}
//
// Returns determinant of 3x3 submatrix composed of given row and
// column indices (0-3 for each).
//
float
SbMatrix::det3(int r1, int r2, int r3, int c1, int c2, int c3) const
{
return ( matrix[r1][c1] * matrix[r2][c2] * matrix[r3][c3]
+ matrix[r1][c2] * matrix[r2][c3] * matrix[r3][c1]
+ matrix[r1][c3] * matrix[r2][c1] * matrix[r3][c2]
- matrix[r1][c1] * matrix[r2][c3] * matrix[r3][c2]
- matrix[r1][c2] * matrix[r2][c1] * matrix[r3][c3]
- matrix[r1][c3] * matrix[r2][c2] * matrix[r3][c1]);
}
//
// Returns determinant of entire matrix
//
float
SbMatrix::det4() const
{
return ( matrix[0][3] * det3(1, 2, 3, 0, 1, 2)
+ matrix[1][3] * det3(0, 2, 3, 0, 1, 2)
+ matrix[2][3] * det3(0, 1, 3, 0, 1, 2)
+ matrix[3][3] * det3(0, 1, 2, 0, 1, 2));
}
//
// Factors a matrix m into 5 pieces: m = r s r^ u t, where r^
// means transpose of r, and r and u are rotations, s is a scale,
// and t is a translation. Any projection information is returned
// in proj.
//
// routines for matrix factorization taken from BAGS code, written by
// John Hughes (?). Original comment follows:
//
/* Copyright 1988, Brown Computer Graphics Group. All Rights Reserved. */
/* --------------------------------------------------------------------------
* This file contains routines to do the MAT3factor operation, which
* factors a matrix m:
* m = r s r^ u t, where r^ means transpose of r, and r and u are
* rotations, s is a scale, and t is a translation.
*
* It is based on the Jacobi method for diagonalizing real symmetric
* matrices, taken from Linear Algebra, Wilkenson and Reinsch, Springer-Verlag
* math series, Volume II, 1971, page 204. Call number QA251W623.
* In ALGOL!
* -------------------------------------------------------------------------*/
/*
* Variable declarations from the original source:
*
* n : order of matrix A
* eivec: true if eigenvectors are desired, false otherwise.
* a : Array [1:n, 1:n] of numbers, assumed symmetric!
*
* a : Superdiagonal elements of the original array a are destroyed.
* Diagonal and subdiagonal elements are untouched.
* d : Array [1:n] of eigenvalues of a.
* v : Array [1:n, 1:n] containing (if eivec = TRUE), the eigenvectors of
* a, with the kth column being the normalized eigenvector with
* eigenvalue d[k].
* rot : The number of jacobi rotations required to perform the operation.
*/
SbBool
SbMatrix::factor(SbMatrix &r, SbVec3f &s, SbMatrix &u, SbVec3f &t,
SbMatrix &proj) const
{
double det; /* Determinant of matrix A */
double det_sign; /* -1 if det < 0, 1 if det > 0 */
double scratch;
int i, j;
int junk;
SbMatrix a, b, si;
float evalues[3];
SbVec3f evectors[3];
a = *this;
proj.makeIdentity();
scratch = 1.0;
for (i = 0; i < 3; i++) {
for (j = 0; j < 3; j++) {
a.matrix[i][j] *= scratch;
}
t[i] = matrix[3][i] * scratch;
a.matrix[3][i] = a.matrix[i][3] = 0.0;
}
a.matrix[3][3] = 1.0;
/* (3) Compute det A. If negative, set sign = -1, else sign = 1 */
det = a.det3();
det_sign = (det < 0.0 ? -1.0 : 1.0);
if (det_sign * det < 1e-12)
return(FALSE); // singular
/* (4) B = A * A^ (here A^ means A transpose) */
b = a * a.transpose();
b.jacobi3(evalues, evectors, junk);
// find min / max eigenvalues and do ratio test to determine singularity
r = SbMatrix(evectors[0][0], evectors[0][1], evectors[0][2], 0.0,
evectors[1][0], evectors[1][1], evectors[1][2], 0.0,
evectors[2][0], evectors[2][1], evectors[2][2], 0.0,
0.0, 0.0, 0.0, 1.0);
/* Compute s = sqrt(evalues), with sign. Set si = s-inverse */
si.makeIdentity();
for (i = 0; i < 3; i++) {
s[i] = det_sign * sqrt(evalues[i]);
si.matrix[i][i] = 1.0 / s[i];
}
/* (5) Compute U = R^ S! R A. */
u = r * si * r.transpose() * a;
return(TRUE);
}
//
// Diagonalizes 3x3 matrix. See comment for factor().
//
#define SB_JACOBI_RANK 3
void
SbMatrix::jacobi3(float evalues[SB_JACOBI_RANK],
SbVec3f evectors[SB_JACOBI_RANK], int &rots) const
{
double sm; // smallest entry
double theta; // angle for Jacobi rotation
double c, s, t; // cosine, sine, tangent of theta
double tau; // sine / (1 + cos)
double h, g; // two scrap values
double thresh; // threshold below which no rotation done
double b[SB_JACOBI_RANK]; // more scratch
double z[SB_JACOBI_RANK]; // more scratch
int p, q, i, j;
double a[SB_JACOBI_RANK][SB_JACOBI_RANK];
// initializations
for (i = 0; i < SB_JACOBI_RANK; i++) {
b[i] = evalues[i] = matrix[i][i];
z[i] = 0.0;
for (j = 0; j < SB_JACOBI_RANK; j++) {
evectors[i][j] = (i == j) ? 1.0 : 0.0;
a[i][j] = matrix[i][j];
}
}
rots = 0;
// Why 50? I don't know--it's the way the folks who wrote the
// algorithm did it:
for (i = 0; i < 50; i++) {
sm = 0.0;
for (p = 0; p < SB_JACOBI_RANK - 1; p++)
for (q = p+1; q < SB_JACOBI_RANK; q++)
sm += ABS(a[p][q]);
if (sm == 0.0)
return;
thresh = (i < 3 ?
(.2 * sm / (SB_JACOBI_RANK * SB_JACOBI_RANK)) :
0.0);
for (p = 0; p < SB_JACOBI_RANK - 1; p++) {
for (q = p+1; q < SB_JACOBI_RANK; q++) {
g = 100.0 * ABS(a[p][q]);
if (i > 3 && (ABS(evalues[p]) + g == ABS(evalues[p])) &&
(ABS(evalues[q]) + g == ABS(evalues[q])))
a[p][q] = 0.0;
else if (ABS(a[p][q]) > thresh) {
h = evalues[q] - evalues[p];
if (ABS(h) + g == ABS(h))
t = a[p][q] / h;
else {
theta = .5 * h / a[p][q];
t = 1.0 / (ABS(theta) + sqrt(1 + theta * theta));
if (theta < 0.0) t = -t;
}
// End of computing tangent of rotation angle
c = 1.0 / sqrt(1.0 + t*t);
s = t * c;
tau = s / (1.0 + c);
h = t * a[p][q];
z[p] -= h;
z[q] += h;
evalues[p] -= h;
evalues[q] += h;
a[p][q] = 0.0;
for (j = 0; j < p; j++) {
g = a[j][p];
h = a[j][q];
a[j][p] = g - s * (h + g * tau);
a[j][q] = h + s * (g - h * tau);
}
for (j = p+1; j < q; j++) {
g = a[p][j];
h = a[j][q];
a[p][j] = g - s * (h + g * tau);
a[j][q] = h + s * (g - h * tau);
}
for (j = q+1; j < SB_JACOBI_RANK; j++) {
g = a[p][j];
h = a[q][j];
a[p][j] = g - s * (h + g * tau);
a[q][j] = h + s * (g - h * tau);
}
for (j = 0; j < SB_JACOBI_RANK; j++) {
g = evectors[j][p];
h = evectors[j][q];
evectors[j][p] = g - s * (h + g * tau);
evectors[j][q] = h + s * (g - h * tau);
}
}
rots++;
}
}
for (p = 0; p < SB_JACOBI_RANK; p++) {
evalues[p] = b[p] += z[p];
z[p] = 0;
}
}
}
#undef SB_JACOBI_RANK
//
// Returns inverse of matrix. Results are undefined for
// singular matrices. Uses LU decomposition, from Numerial Recipies in C,
// pp 43-45. They say "There is no better way."
//
// Oh, yeah! Well, if the matrix is affine, there IS a better way.
// So we call affine_inverse to see if we can get away with it...
//
SbMatrix
SbMatrix::inverse() const
{
// Trivial case
if (IS_IDENTITY(matrix))
return SbMatrix::identity();
// Affine case...
SbMatrix affineAnswer;
if ( affine_inverse( SbMatrix(matrix), affineAnswer ) )
return affineAnswer;
int index[4];
float d, invmat[4][4], temp;
SbMatrix inverse = *this;
#ifdef DEBUGGING
int i, j;
#endif /* DEBUGGING */
if(inverse.LUDecomposition(index, d)) {
#ifdef DEBUGGING
for(j = 0; j < 4; j++) {
for(i = 0; i < 4; i++)
invmat[j][i] = 0.0;
invmat[j][j] = 1.0;
inverse.LUBackSubstitution(index, invmat[j]);
}
#else
invmat[0][0] = 1.0;
invmat[0][1] = 0.0;
invmat[0][2] = 0.0;
invmat[0][3] = 0.0;
inverse.LUBackSubstitution(index, invmat[0]);
invmat[1][0] = 0.0;
invmat[1][1] = 1.0;
invmat[1][2] = 0.0;
invmat[1][3] = 0.0;
inverse.LUBackSubstitution(index, invmat[1]);
invmat[2][0] = 0.0;
invmat[2][1] = 0.0;
invmat[2][2] = 1.0;
invmat[2][3] = 0.0;
inverse.LUBackSubstitution(index, invmat[2]);
invmat[3][0] = 0.0;
invmat[3][1] = 0.0;
invmat[3][2] = 0.0;
invmat[3][3] = 1.0;
inverse.LUBackSubstitution(index, invmat[3]);
#endif /* DEBUGGING */
#ifdef DEBUGGING
// transpose invmat
for(j = 0; j < 4; j++) {
for(i = 0; i < j; i++) {
temp = invmat[i][j];
invmat[i][j] = invmat[j][i];
invmat[j][i] = temp;
}
}
#else
#define SWAP(i,j) \
temp = invmat[i][j]; \
invmat[i][j] = invmat[j][i]; \
invmat[j][i] = temp;
SWAP(1,0);
SWAP(2,0);
SWAP(2,1);
SWAP(3,0);
SWAP(3,1);
SWAP(3,2);
#undef SWAP
#endif /* DEBUGGING */
inverse.setValue(invmat);
}
return inverse;
}
// The following method finds the inverse of an affine matrix.
// The last column MUST be [0 0 0 1] for this to work.
// This is taken from graphics gems 2, page 603
//
// computes the inverse of a 3d affine matrix; i.e. a matrix with a
// dimensionality of 4 where the right column has the entries (0,0,0,1).
//
// This procedure treats the 4 by 4 matrix as a block matrix and calculates
// the inverse of one submatrix for a significant performance
// improvement over a general procedure that can invert any nonsingular matrix.
//
// -1
// -1 | | | -1 |
// M = |A 0| = | A 0|
// | | | |
// | | | -1 |
// |C 1| |-CA 1|
//
// where M is a 4 by 4 matrix,
// A is the 3 by 3 upper left submatrix of M,
// C is the 1 by 3 lower left submatrix of M.
// Input:
// in - 3D affine matrix
// Output:
// out - inverse of 3D affine matrix
// Returned Value:
// TRUE if input matrix is nonsingular and affine
// FALSE otherwise
SbBool
SbMatrix::affine_inverse(const SbMatrix &in, SbMatrix &out) const
{
// Check if matrix is affine...
if ( in[0][3] != 0.0 || in[1][3] != 0.0 || in[2][3] != 0.0 ||
in[3][3] != 1.0 )
return FALSE;
// Calculate the determinant of submatrix A and determine if the matrix
// is singular as limited by the double precision floating
// point data representation
double det_1;
double pos, neg, temp;
#define ACCUMULATE \
if (temp >= 0.0) \
pos += temp; \
else \
neg += temp;
pos = neg = 0.0;
temp = in[0][0] * in[1][1] * in[2][2];
ACCUMULATE
temp = in[0][1] * in[1][2] * in[2][0];
ACCUMULATE
temp = in[0][2] * in[1][0] * in[2][1];
ACCUMULATE
temp = -in[0][2] * in[1][1] * in[2][0];
ACCUMULATE
temp = -in[0][1] * in[1][0] * in[2][2];
ACCUMULATE
temp = -in[0][0] * in[1][2] * in[2][1];
ACCUMULATE
det_1 = pos + neg;
#undef ACCUMULATE
#define PRECISION_LIMIT (1.0e-15)
// Is the submatrix A singular?
temp = det_1 / (pos - neg);
if (ABS(temp) < PRECISION_LIMIT)
return FALSE;
// Calculate inverse(A) = adj(A) / det(A)
det_1 = 1.0 / det_1;
out[0][0] = (in[1][1] * in[2][2] - in[1][2] * in[2][1]) * det_1;
out[1][0] = -(in[1][0] * in[2][2] - in[1][2] * in[2][0]) * det_1;
out[2][0] = (in[1][0] * in[2][1] - in[1][1] * in[2][0]) * det_1;
out[0][1] = -(in[0][1] * in[2][2] - in[0][2] * in[2][1]) * det_1;
out[1][1] = (in[0][0] * in[2][2] - in[0][2] * in[2][0]) * det_1;
out[2][1] = -(in[0][0] * in[2][1] - in[0][1] * in[2][0]) * det_1;
out[0][2] = (in[0][1] * in[1][2] - in[0][2] * in[1][1]) * det_1;
out[1][2] = -(in[0][0] * in[1][2] - in[0][2] * in[1][0]) * det_1;
out[2][2] = (in[0][0] * in[1][1] - in[0][1] * in[1][0]) * det_1;
// Calculate -C * inverse(A)
out[3][0] = -( in[3][0] * out[0][0] +
in[3][1] * out[1][0] +
in[3][2] * out[2][0] );
out[3][1] = -( in[3][0] * out[0][1] +
in[3][1] * out[1][1] +
in[3][2] * out[2][1] );
out[3][2] = -( in[3][0] * out[0][2] +
in[3][1] * out[1][2] +
in[3][2] * out[2][2] );
// Fill in last column
out[0][3] = out[1][3] = out[2][3] = 0.0;
out[3][3] = 1.0;
#undef PRECISION_LIMIT
return TRUE;
}
//
// Perform an LU decomposition of a matrix. From Numerical Recipes in C, pg 43
// Destroys the original matrix. The resulting matrix is as follows
//
// a b c d 1 0 0 0 a b c d
// LU = e f g h L = e 1 0 0 U = 0 f g h
// i j k l i j 1 0 0 0 k l
// m n o p m n o 1 0 0 0 p
//
// For singular matrices, want to calculate something
// that is close to the inverse, so we can pick
// things scaled to zero in some dimension.
//
SbBool
SbMatrix::LUDecomposition(int index[4], float &d)
{
int imax;
float big, dum, sum, temp;
float vv[4];
#ifdef DEBUGGING
int i, j, k;
#endif /* DEBUGGING */
d = 1.0;
#ifdef DEBUGGING
for(i = 0; i < 4; i++) {
big = 0.0;
for(j = 0; j < 4; j++)
if((temp = ABS(matrix[i][j])) > big) big = temp;
if(big == 0.0) {
matrix[i][i] = 1e-6;
big = matrix[i][i];
}
vv[i] = 1.0 / big;
}
#else
#define COMPUTE_VV(i) \
big = 0.0; \
if((temp = ABS(matrix[i][0])) > big) big = temp; \
if((temp = ABS(matrix[i][1])) > big) big = temp; \
if((temp = ABS(matrix[i][2])) > big) big = temp; \
if((temp = ABS(matrix[i][3])) > big) big = temp; \
if(big == 0.0) { \
matrix[i][i] = 1e-6; \
big = matrix[i][i]; \
} \
vv[i] = 1.0 / big;
COMPUTE_VV(0);
COMPUTE_VV(1);
COMPUTE_VV(2);
COMPUTE_VV(3);
#undef COMPUTE_VV
#endif /* DEBUGGING */
#ifdef DEBUGGING
// This is the code as it originally existed.
// Below this is the unrolled, really unreadable version.
for(j = 0; j < 4; j++) {
// BLOCK 1
for(i = 0; i < j; i++) {
sum = matrix[i][j];
for(k = 0; k < i; k++)
sum -= matrix[i][k] * matrix[k][j];
matrix[i][j] = sum;
}
big = 0.0;
// BLOCK 2
for(i = j; i < 4; i++) {
sum = matrix[i][j];
for(k = 0; k < j; k++)
sum -= matrix[i][k] * matrix[k][j];
matrix[i][j] = sum;
if((dum = vv[i] * ABS(sum)) >= big) {
big = dum;
imax = i;
}
}
// BLOCK 3
if(j != imax) {
for(k = 0; k < 4; k++) {
dum = matrix[imax][k];
matrix[imax][k] = matrix[j][k];
matrix[j][k] = dum;
}
d = -d;
vv[imax] = vv[j];
}
// BLOCK 4
index[j] = imax;
if(matrix[j][j] == 0.0) matrix[j][j] = 1e-20;
// BLOCK 5
if(j != 4 - 1) {
dum = 1.0 / (matrix[j][j]);
for(i = j + 1; i < 4; i++)
matrix[i][j] *= dum;
}
}
#else
// This is the completely unreadable, but much faster
// version of the above code. First, some macros that
// never change.
// macro for block 3, inner k loop
#define BLOCK3INNER(j,k) \
dum = matrix[imax][k]; \
matrix[imax][k] = matrix[j][k]; \
matrix[j][k] = dum;
// macro for block 4
#define BLOCK4(j) \
index[j] = imax; \
if(matrix[j][j] == 0.0) matrix[j][j] = 1e-20;
// Now the code...
// *********************************************
// j = 0
// *********************************************
// BLOCK 1, j = 0
// does nothing when j==0
big = 0.0;
// BLOCK 2, j = 0
// macro for block 2 when j == 0
// inner k loop does nothing when j == 0
#define BLOCK2J0(i) \
sum = matrix[i][0]; \
if((dum = vv[i] * ABS(sum)) >= big) { \
big = dum; \
imax = i; \
}
// for(i = j; i < 4; i++)
BLOCK2J0(0)
BLOCK2J0(1)
BLOCK2J0(2)
BLOCK2J0(3)
#undef BLOCK2J0
// BLOCK 3, j = 0
if(0 != imax) {
BLOCK3INNER(0,0);
BLOCK3INNER(0,1);
BLOCK3INNER(0,2);
BLOCK3INNER(0,3);
d = -d;
vv[imax] = vv[0];
}
// BLOCK 4, j = 0
BLOCK4(0);
// BLOCK 5, j = 0
dum = 1.0 / (matrix[0][0]);
// for(i = j + 1; i < 4; i++)
matrix[1][0] *= dum;
matrix[2][0] *= dum;
matrix[3][0] *= dum;
// *********************************************
// j = 1
// *********************************************
// BLOCK 1, j = 1
// for(i = 0; i < j; i++) {
sum = matrix[0][1];
// for(k = 0; k < i; k++)
// nothing
big = 0.0;
// BLOCK 2, j = 1
// macro for block 2 when j == 1
#define BLOCK2J1(i) \
sum = matrix[i][1]; \
sum -= matrix[i][0] * matrix[0][1]; \
matrix[i][1] = sum; \
if((dum = vv[i] * ABS(sum)) >= big) { \
big = dum; \
imax = i; \
}
// for(i = j; i < 4; i++)
BLOCK2J1(1)
BLOCK2J1(2)
BLOCK2J1(3)
#undef BLOCK2J1
// BLOCK 3, j = 1
if(1 != imax) {
BLOCK3INNER(1,0);
BLOCK3INNER(1,1);
BLOCK3INNER(1,2);
BLOCK3INNER(1,3);
d = -d;
vv[imax] = vv[1];
}
// BLOCK 4, j = 1
BLOCK4(1);
// BLOCK 5, j = 1
dum = 1.0 / (matrix[1][1]);
// for(i = j + 1; i < 4; i++)
matrix[2][1] *= dum;
matrix[3][1] *= dum;
// *********************************************
// j = 2
// *********************************************
// BLOCK 1, j = 2
// for(i = 0; i < j; i++) {
// i = 0
sum = matrix[0][2];
// for(k = 0; k < i; k++)
// nothing
// i = 1
sum = matrix[1][2];
// for(k = 0; k < i; k++)
sum -= matrix[1][0] * matrix[0][2];
matrix[1][2] = sum;
big = 0.0;
// BLOCK 2, j = 2
// macro for block 2 when j == 2
#define BLOCK2J2(i) \
sum = matrix[i][2]; \
sum -= matrix[i][0] * matrix[0][2]; \
sum -= matrix[i][1] * matrix[1][2]; \
matrix[i][2] = sum; \
if((dum = vv[i] * ABS(sum)) >= big) { \
big = dum; \
imax = i; \
}
// for(i = j; i < 4; i++)
BLOCK2J2(2)
BLOCK2J2(3)
#undef BLOCK2J2
// BLOCK 3, j = 2
if(2 != imax) {
BLOCK3INNER(2,0);
BLOCK3INNER(2,1);
BLOCK3INNER(2,2);
BLOCK3INNER(2,3);
d = -d;
vv[imax] = vv[2];
}
// BLOCK 4, j = 2
BLOCK4(2);
// BLOCK 5, j = 2
dum = 1.0 / (matrix[2][2]);
// for(i = j + 1; i < 4; i++)
matrix[3][2] *= dum;
// *********************************************
// j = 3
// *********************************************
// BLOCK 1, j = 3
// for(i = 0; i < j; i++) {
// i = 0
sum = matrix[0][3];
// for(k = 0; k < i; k++)
// nothing
// i = 1
sum = matrix[1][3];
// for(k = 0; k < i; k++)
sum -= matrix[1][0] * matrix[0][3];
matrix[1][3] = sum;
// i = 2
sum = matrix[2][3];
// for(k = 0; k < i; k++)
sum -= matrix[2][0] * matrix[0][3];
sum -= matrix[2][1] * matrix[1][3];
matrix[2][3] = sum;
big = 0.0;
// BLOCK 2, j = 3
// macro for block 2 when j == 3
#define BLOCK2J3(i) \
sum = matrix[i][3]; \
sum -= matrix[i][0] * matrix[0][3]; \
sum -= matrix[i][1] * matrix[1][3]; \
sum -= matrix[i][2] * matrix[2][3]; \
matrix[i][3] = sum; \
if((dum = vv[i] * ABS(sum)) >= big) { \
big = dum; \
imax = i; \
}
// for(i = j; i < 4; i++)
BLOCK2J3(3)
#undef BLOCK2J3
// BLOCK 3, j = 3
if(3 != imax) {
BLOCK3INNER(3,0);
BLOCK3INNER(3,1);
BLOCK3INNER(3,2);
BLOCK3INNER(3,3);
d = -d;
vv[imax] = vv[3];
}
// BLOCK 4, j = 3
BLOCK4(3);
// BLOCK 5, j = 3
// does not execute when j == 3
#endif /* DEBUGGING */
return TRUE;
}
//
// Perform back-subtitution on input LU matrix, from Numerial Recipies in C,
// pg 44
//
void
SbMatrix::LUBackSubstitution(int index[4], float b[4]) const
{
int ii = -1, ip, j;
float sum;
#ifdef DEBUGGING
int i;
#endif /* DEBUGGING */
#ifdef DEBUGGING
for(i = 0; i < 4; i++) {
ip = index[i];
sum = b[ip];
b[ip] = b[i];
if(ii >= 0)
for(j = ii; j <= i - 1; j++)
sum -= matrix[i][j] * b[j];
else if(sum) ii = i;
b[i] = sum;
}
#else
#define BACKSUB(i) \
ip = index[i]; \
sum = b[ip]; \
b[ip] = b[i]; \
if(ii >= 0) \
for(j = ii; j <= i - 1; j++) \
sum -= matrix[i][j] * b[j]; \
else if(sum) ii = i; \
b[i] = sum;
BACKSUB(0);
BACKSUB(1);
BACKSUB(2);
BACKSUB(3);
#undef BACKSUB
#endif /* DEBUGGING */
#ifdef DEBUGGING
for(i = 4 - 1; i >= 0; i--) {
sum = b[i];
for(j = i + 1; j < 4; j++)
sum -= matrix[i][j]*b[j];
b[i] = sum / matrix[i][i];
}
#else
sum = b[3];
b[3] = sum / matrix[3][3];
sum = b[2] - matrix[2][3]*b[3];
b[2] = sum / matrix[2][2];
sum = b[1] - matrix[1][2]*b[2] - matrix[1][3]*b[3];
b[1] = sum / matrix[1][1];
sum = b[0] - matrix[0][1]*b[1] - matrix[0][2]*b[2] - matrix[0][3]*b[3];
b[0] = sum / matrix[0][0];
#endif /* DEBUGGING */
}
//
// Returns transpose of matrix
//
SbMatrix
SbMatrix::transpose() const
{
return SbMatrix(matrix[0][0], matrix[1][0], matrix[2][0], matrix[3][0],
matrix[0][1], matrix[1][1], matrix[2][1], matrix[3][1],
matrix[0][2], matrix[1][2], matrix[2][2], matrix[3][2],
matrix[0][3], matrix[1][3], matrix[2][3], matrix[3][3]);
}
//
// Composes the matrix from translation, rotation, scale, etc.
//
void
SbMatrix::setTransform(const SbVec3f &translation,
const SbRotation &rotation,
const SbVec3f &scaleFactor,
const SbRotation &scaleOrientation,
const SbVec3f ¢er)
{
#define TRANSLATE(vec) m.setTranslate(vec), multLeft(m)
#define ROTATE(rot) rot.getValue(m), multLeft(m)
SbMatrix m;
makeIdentity();
if (translation != SbVec3f(0,0,0))
TRANSLATE(translation);
if (center != SbVec3f(0,0,0))
TRANSLATE(center);
if (rotation != SbRotation(0,0,0,1))
ROTATE(rotation);
if (scaleFactor != SbVec3f(1,1,1)) {
SbRotation so = scaleOrientation;
if (so != SbRotation(0,0,0,1))
ROTATE(so);
m.setScale(scaleFactor);
multLeft(m);
if (so != SbRotation(0,0,0,1)) {
so.invert();
ROTATE(so);
}
}
if (center != SbVec3f(0,0,0))
TRANSLATE(-center);
#undef TRANSLATE
#undef ROTATE
}
//
// Decomposes a rotation into translation etc, based on scale
//
void
SbMatrix::getTransform(SbVec3f &translation,
SbRotation &rotation,
SbVec3f &scaleFactor,
SbRotation &scaleOrientation,
const SbVec3f ¢er) const
{
SbMatrix so, rot, proj;
if (center != SbVec3f(0,0,0)) {
// to get fields for a non-0 center, we
// need to decompose a new matrix "m" such
// that [-center][m][center] = [this]
// i.e., [m] = [center][this][-center]
// (this trick stolen from Showcase code)
SbMatrix m,c;
m.setTranslate(-center);
m.multLeft(*this);
c.setTranslate(center);
m.multLeft(c);
m.factor(so,scaleFactor,rot,translation,proj);
}
else
this->factor(so,scaleFactor,rot,translation,proj);
scaleOrientation = so.transpose(); // have to transpose because factor
// gives us transpose of correct answer.
rotation = rot;
}
////////////////////////////////////////////
//
// Matrix/matrix and matrix/vector arithmetic
//
////////////////////////////////////////////
//
// Multiplies matrix by given matrix on right
//
SbMatrix &
SbMatrix::multRight(const SbMatrix &m)
{
// Trivial cases
if (IS_IDENTITY(m))
return *this;
else if (IS_IDENTITY(matrix))
return (*this = m);
SbMat tmp;
#define MULT_RIGHT(i,j) (matrix[i][0]*m.matrix[0][j] + \
matrix[i][1]*m.matrix[1][j] + \
matrix[i][2]*m.matrix[2][j] + \
matrix[i][3]*m.matrix[3][j])
tmp[0][0] = MULT_RIGHT(0,0);
tmp[0][1] = MULT_RIGHT(0,1);
tmp[0][2] = MULT_RIGHT(0,2);
tmp[0][3] = MULT_RIGHT(0,3);
tmp[1][0] = MULT_RIGHT(1,0);
tmp[1][1] = MULT_RIGHT(1,1);
tmp[1][2] = MULT_RIGHT(1,2);
tmp[1][3] = MULT_RIGHT(1,3);
tmp[2][0] = MULT_RIGHT(2,0);
tmp[2][1] = MULT_RIGHT(2,1);
tmp[2][2] = MULT_RIGHT(2,2);
tmp[2][3] = MULT_RIGHT(2,3);
tmp[3][0] = MULT_RIGHT(3,0);
tmp[3][1] = MULT_RIGHT(3,1);
tmp[3][2] = MULT_RIGHT(3,2);
tmp[3][3] = MULT_RIGHT(3,3);
#undef MULT_RIGHT
return (*this = tmp);
}
//
// Multiplies matrix by given matrix on left
//
SbMatrix &
SbMatrix::multLeft(const SbMatrix &m)
{
// Trivial cases
if (IS_IDENTITY(m))
return *this;
else if (IS_IDENTITY(matrix))
return (*this = m);
SbMat tmp;
#define MULT_LEFT(i,j) (m.matrix[i][0]*matrix[0][j] + \
m.matrix[i][1]*matrix[1][j] + \
m.matrix[i][2]*matrix[2][j] + \
m.matrix[i][3]*matrix[3][j])
tmp[0][0] = MULT_LEFT(0,0);
tmp[0][1] = MULT_LEFT(0,1);
tmp[0][2] = MULT_LEFT(0,2);
tmp[0][3] = MULT_LEFT(0,3);
tmp[1][0] = MULT_LEFT(1,0);
tmp[1][1] = MULT_LEFT(1,1);
tmp[1][2] = MULT_LEFT(1,2);
tmp[1][3] = MULT_LEFT(1,3);
tmp[2][0] = MULT_LEFT(2,0);
tmp[2][1] = MULT_LEFT(2,1);
tmp[2][2] = MULT_LEFT(2,2);
tmp[2][3] = MULT_LEFT(2,3);
tmp[3][0] = MULT_LEFT(3,0);
tmp[3][1] = MULT_LEFT(3,1);
tmp[3][2] = MULT_LEFT(3,2);
tmp[3][3] = MULT_LEFT(3,3);
#undef MULT_LEFT
return (*this = tmp);
}
//
// Multiplies matrix by given column vector, giving vector result
//
void
SbMatrix::multMatrixVec(const SbVec3f &src, SbVec3f &dst) const
{
float x,y,z,w;
x = matrix[0][0]*src[0] + matrix[0][1]*src[1] +
matrix[0][2]*src[2] + matrix[0][3];
y = matrix[1][0]*src[0] + matrix[1][1]*src[1] +
matrix[1][2]*src[2] + matrix[1][3];
z = matrix[2][0]*src[0] + matrix[2][1]*src[1] +
matrix[2][2]*src[2] + matrix[2][3];
w = matrix[3][0]*src[0] + matrix[3][1]*src[1] +
matrix[3][2]*src[2] + matrix[3][3];
dst.setValue(x/w, y/w, z/w);
}
//
// Multiplies given row vector by matrix, giving vector result
//
void
SbMatrix::multVecMatrix(const SbVec3f &src, SbVec3f &dst) const
{
float x,y,z,w;
x = src[0]*matrix[0][0] + src[1]*matrix[1][0] +
src[2]*matrix[2][0] + matrix[3][0];
y = src[0]*matrix[0][1] + src[1]*matrix[1][1] +
src[2]*matrix[2][1] + matrix[3][1];
z = src[0]*matrix[0][2] + src[1]*matrix[1][2] +
src[2]*matrix[2][2] + matrix[3][2];
w = src[0]*matrix[0][3] + src[1]*matrix[1][3] +
src[2]*matrix[2][3] + matrix[3][3];
dst.setValue(x/w, y/w, z/w);
}
//
// Multiplies given row vector by matrix, giving vector result
// src is assumed to be a direction vector, so translation part of
// matrix is ignored.
//
void
SbMatrix::multDirMatrix(const SbVec3f &src, SbVec3f &dst) const
{
float x,y,z;
x = src[0]*matrix[0][0] + src[1]*matrix[1][0] + src[2]*matrix[2][0];
y = src[0]*matrix[0][1] + src[1]*matrix[1][1] + src[2]*matrix[2][1];
z = src[0]*matrix[0][2] + src[1]*matrix[1][2] + src[2]*matrix[2][2];
dst.setValue(x, y, z);
}
//
// Multiplies the given line by the matrix resulting in a new
// line. The origin of the line is transformed by the whole matrix.
// The translation part of the matrix is ignored when transforming
// the direction of the line.
//
void
SbMatrix::multLineMatrix(const SbLine &src, SbLine &dst) const
{
SbVec3f pos, dir;
multVecMatrix(src.getPosition(), pos);
multDirMatrix(src.getDirection(), dir);
dst.setValue(pos, pos+dir);
}
////////////////////////////////////////////
//
// Miscellaneous
//
////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////
//
// Description:
// Prints a formatted version of the matrix to the given file
// pointer.
//
// Use: public
void
SbMatrix::print(FILE *fp) const
//
////////////////////////////////////////////////////////////////////////
{
int i, j;
for (i = 0; i < 4; i++)
for (j = 0; j < 4; j++)
fprintf(fp, "%10.5g%c", matrix[i][j], j < 3 ? '\t' : '\n');
}
////////////////////////////////////////////
//
// Operators
//
////////////////////////////////////////////
//
// Sets value from 4x4 array of elements
//
SbMatrix &
SbMatrix::operator =(const SbMat &m)
{
setValue(m);
return *this;
}
//
// ??? why do we need to write this out like this?
// ??? it doesn't seem to work otherwise, but why not?
//
SbMatrix &
SbMatrix::operator =(const SbMatrix &m)
{
matrix[0][0] = m.matrix[0][0];
matrix[0][1] = m.matrix[0][1];
matrix[0][2] = m.matrix[0][2];
matrix[0][3] = m.matrix[0][3];
matrix[1][0] = m.matrix[1][0];
matrix[1][1] = m.matrix[1][1];
matrix[1][2] = m.matrix[1][2];
matrix[1][3] = m.matrix[1][3];
matrix[2][0] = m.matrix[2][0];
matrix[2][1] = m.matrix[2][1];
matrix[2][2] = m.matrix[2][2];
matrix[2][3] = m.matrix[2][3];
matrix[3][0] = m.matrix[3][0];
matrix[3][1] = m.matrix[3][1];
matrix[3][2] = m.matrix[3][2];
matrix[3][3] = m.matrix[3][3];
return *this;
}
//
// Binary multiplication of matrices
//
SbMatrix
operator *(const SbMatrix &l, const SbMatrix &r)
{
SbMatrix m = l;
m *= r;
return m;
}
//
// Equality comparison operator. All componenents must match exactly.
//
int
operator ==(const SbMatrix &m1, const SbMatrix &m2)
{
return (
m1.matrix[0][0] == m2.matrix[0][0] &&
m1.matrix[0][1] == m2.matrix[0][1] &&
m1.matrix[0][2] == m2.matrix[0][2] &&
m1.matrix[0][3] == m2.matrix[0][3] &&
m1.matrix[1][0] == m2.matrix[1][0] &&
m1.matrix[1][1] == m2.matrix[1][1] &&
m1.matrix[1][2] == m2.matrix[1][2] &&
m1.matrix[1][3] == m2.matrix[1][3] &&
m1.matrix[2][0] == m2.matrix[2][0] &&
m1.matrix[2][1] == m2.matrix[2][1] &&
m1.matrix[2][2] == m2.matrix[2][2] &&
m1.matrix[2][3] == m2.matrix[2][3] &&
m1.matrix[3][0] == m2.matrix[3][0] &&
m1.matrix[3][1] == m2.matrix[3][1] &&
m1.matrix[3][2] == m2.matrix[3][2] &&
m1.matrix[3][3] == m2.matrix[3][3]
);
}
//
// Equality comparison operator within given tolerance for each component
//
SbBool
SbMatrix::equals(const SbMatrix &m, float tolerance) const
{
int i, j;
float d;
for (i = 0; i < 4; i++)
for (j = 0; j < 4; j++) {
d = matrix[i][j] - m.matrix[i][j];
if (ABS(d) > tolerance)
return FALSE;
}
return TRUE;
}
![[BACK]](/icons/back.gif){kind=icon}
Return to SbMatrix.c++ CVS log ![[TXT]](/icons/text.gif){kind=icon}
![[DIR]](/icons/dir.gif){kind=icon}
Up to [Development] / inventor / lib / database / src / sb