No Subject
dwhitney@localhost.localdomain
dwhitney@localhost.localdomain
Sat Sep 12 02:25:00 GMT 1998
#include <math.h>
#include <stdlib.h>
#include <iostream.h>
#include "LinAlgebra.h"
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
template <class T1,class T2>
void CrossProduct3(const T1 *v1,const T2 *v2,T1 *r)
{
r[0] = v1[1] * v2[2] - v1[2] * v2[1];
r[1] = v1[2] * v2[0] - v1[0] * v2[2];
r[2] = v1[0] * v2[1] - v1[1] * v2[0];
}
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/* Vector3t */
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/* Vector4t */
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/* Matrix33t */
template<class T>
const int Matrix33t<T>::MATSIZE = 9*sizeof(T);
template <class T1, class T2>
Matrix33t<T1> operator*(const Matrix33t<T1>& m1,
const Matrix33t<T2>& m2)
{
return Matrix33t<T1>(m1[0][0] * m2[0][0] +
m1[0][1] * m2[1][0] +
m1[0][2] * m2[2][0],
m1[0][0] * m2[0][1] +
m1[0][1] * m2[1][1] +
m1[0][2] * m2[2][1],
m1[0][0] * m2[0][2] +
m1[0][1] * m2[1][2] +
m1[0][2] * m2[2][2],
m1[1][0] * m2[0][0] +
m1[1][1] * m2[1][0] +
m1[1][2] * m2[2][0],
m1[1][0] * m2[0][1] +
m1[1][1] * m2[1][1] +
m1[1][2] * m2[2][1],
m1[1][0] * m2[0][2] +
m1[1][1] * m2[1][2] +
m1[1][2] * m2[2][2],
m1[2][0] * m2[0][0] +
m1[2][1] * m2[1][0] +
m1[2][2] * m2[2][0],
m1[2][0] * m2[0][1] +
m1[2][1] * m2[1][1] +
m1[2][2] * m2[2][1],
m1[2][0] * m2[0][2] +
m1[2][1] * m2[1][2] +
m1[2][2] * m2[2][2]);
}
template <class T>
T Determinant(const Matrix33t<T>& m)
{
return
m.mat[0][0] * (m.mat[1][1] * m.mat[2][2] - m.mat[2][1] * m.mat[1][2]) -
m.mat[1][0] * (m.mat[0][1] * m.mat[2][2] - m.mat[2][1] * m.mat[0][2]) +
m.mat[2][0] * (m.mat[0][1] * m.mat[1][2] - m.mat[1][1] * m.mat[0][2]);
}
template <class T>
//
// Invert the matrix m, but return the determinant separately rather than
// dividing through, i.e. return the numerators in the result matrix res,
// and the single common denominator, det, separately.
//
void Invert_rational_terms(const Matrix33t<T>& m,
Matrix33t<T>* res, T* det)
{
// 9 floating-point ops
T d01d12md11d02 = m.mat[0][1] * m.mat[1][2] - m.mat[1][1] * m.mat[0][2];
T d01d22md21d02 = m.mat[0][1] * m.mat[2][2] - m.mat[2][1] * m.mat[0][2];
T d11d22md21d12 = m.mat[1][1] * m.mat[2][2] - m.mat[2][1] * m.mat[1][2];
// 5 floating-point ops
*det =
m.mat[0][0] * d11d22md21d12 -
m.mat[1][0] * d01d22md21d02 +
m.mat[2][0] * d01d12md11d02;
// 19 floating-point ops
res->set(d11d22md21d12, -d01d22md21d02, d01d12md11d02,
m.mat[2][0] * m.mat[1][2] - m.mat[1][0] * m.mat[2][2],
m.mat[0][0] * m.mat[2][2] - m.mat[2][0] * m.mat[0][2],
m.mat[1][0] * m.mat[0][2] - m.mat[0][0] * m.mat[1][2],
m.mat[1][0] * m.mat[2][1] - m.mat[2][0] * m.mat[1][1],
m.mat[2][0] * m.mat[0][1] - m.mat[0][0] * m.mat[2][1],
m.mat[0][0] * m.mat[1][1] - m.mat[1][0] * m.mat[0][1]);
// 33 total floating-point ops
}
template <class T>
int Invert(const Matrix33t<T>& m, Matrix33t<T>* res)
{
T det;
// 33 floating-point ops
Invert_rational_terms(m, res, &det);
if (0.0 == det) return 0;
// 1 floating-point op
T det_inv = 1.0 / det;
// 9 floating-point ops
*res *= det_inv;
// 43 total floating-point ops
return 1;
}
template <class T>
void Transpose(const Matrix33t<T>& m, Matrix33t<T>* res)
{
for (int i=0; i<3; ++i) {
(*res)[i][0] = m[0][i];
(*res)[i][1] = m[1][i];
(*res)[i][2] = m[2][i];
}
}
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/* Matrix44t */
template<class T>
const int Matrix44t<T>::MATSIZE = 16*sizeof(T);
template <class T1, class T2>
Vector4t<T2> operator*(const Matrix44t<T1>&m, const Vector4t<T2>&v)
{
return Vector4t<T2>(m[0][0] * v[0] +
m[0][1] * v[1] +
m[0][2] * v[2] +
m[0][3] * v[3],
m[1][0] * v[0] +
m[1][1] * v[1] +
m[1][2] * v[2] +
m[1][3] * v[3],
m[2][0] * v[0] +
m[2][1] * v[1] +
m[2][2] * v[2] +
m[2][3] * v[3],
m[3][0] * v[0] +
m[3][1] * v[1] +
m[3][2] * v[2] +
m[3][3] * v[3]);
}
template <class T1, class T2>
Vector4t<T2> operator*(const Vector4t<T2>&v, const Matrix44t<T1>&m)
{
return Vector4t<T2>(m[0][0] * v[0] +
m[1][0] * v[1] +
m[2][0] * v[2] +
m[3][0] * v[3],
m[0][1] * v[0] +
m[1][1] * v[1] +
m[2][1] * v[2] +
m[3][1] * v[3],
m[0][2] * v[0] +
m[1][2] * v[1] +
m[2][2] * v[2] +
m[3][2] * v[3],
m[0][3] * v[0] +
m[1][3] * v[1] +
m[2][3] * v[2] +
m[3][3] * v[3]);
}
template <class T1, class T2>
Vector4t<T2> Transform(const Matrix44t<T1>&m, const Vector3t<T2>&v)
{
return Vector4t<T2>(m[0][0] * v[0] +
m[0][1] * v[1] +
m[0][2] * v[2] +
m[0][3],
m[1][0] * v[0] +
m[1][1] * v[1] +
m[1][2] * v[2] +
m[1][3],
m[2][0] * v[0] +
m[2][1] * v[1] +
m[2][2] * v[2] +
m[2][3],
m[3][0] * v[0] +
m[3][1] * v[1] +
m[3][2] * v[2] +
m[3][3]);
}
template <class T1, class T2>
Vector4t<T2> Transform(const Vector3t<T2>&v, const Matrix44t<T1>&m)
{
return Vector4t<T2>(m[0][0] * v[0] +
m[1][0] * v[1] +
m[2][0] * v[2] +
m[3][0],
m[0][1] * v[0] +
m[1][1] * v[1] +
m[2][1] * v[2] +
m[3][1],
m[0][2] * v[0] +
m[1][2] * v[1] +
m[2][2] * v[2] +
m[3][2],
m[0][3] * v[0] +
m[1][3] * v[1] +
m[2][3] * v[2] +
m[3][3]);
}
template <class T1, class T2>
Matrix44t<T1> operator*(const Matrix44t<T1>& m1,
const Matrix44t<T2>& m2)
{
return Matrix44t<T1>(m1[0][0] * m2[0][0] +
m1[0][1] * m2[1][0] +
m1[0][2] * m2[2][0] +
m1[0][3] * m2[3][0],
m1[0][0] * m2[0][1] +
m1[0][1] * m2[1][1] +
m1[0][2] * m2[2][1] +
m1[0][3] * m2[3][1],
m1[0][0] * m2[0][2] +
m1[0][1] * m2[1][2] +
m1[0][2] * m2[2][2] +
m1[0][3] * m2[3][2],
m1[0][0] * m2[0][3] +
m1[0][1] * m2[1][3] +
m1[0][2] * m2[2][3] +
m1[0][3] * m2[3][3],
m1[1][0] * m2[0][0] +
m1[1][1] * m2[1][0] +
m1[1][2] * m2[2][0] +
m1[1][3] * m2[3][0],
m1[1][0] * m2[0][1] +
m1[1][1] * m2[1][1] +
m1[1][2] * m2[2][1] +
m1[1][3] * m2[3][1],
m1[1][0] * m2[0][2] +
m1[1][1] * m2[1][2] +
m1[1][2] * m2[2][2] +
m1[1][3] * m2[3][2],
m1[1][0] * m2[0][3] +
m1[1][1] * m2[1][3] +
m1[1][2] * m2[2][3] +
m1[1][3] * m2[3][3],
m1[2][0] * m2[0][0] +
m1[2][1] * m2[1][0] +
m1[2][2] * m2[2][0] +
m1[2][3] * m2[3][0],
m1[2][0] * m2[0][1] +
m1[2][1] * m2[1][1] +
m1[2][2] * m2[2][1] +
m1[2][3] * m2[3][1],
m1[2][0] * m2[0][2] +
m1[2][1] * m2[1][2] +
m1[2][2] * m2[2][2] +
m1[2][3] * m2[3][2],
m1[2][0] * m2[0][3] +
m1[2][1] * m2[1][3] +
m1[2][2] * m2[2][3] +
m1[2][3] * m2[3][3],
m1[3][0] * m2[0][0] +
m1[3][1] * m2[1][0] +
m1[3][2] * m2[2][0] +
m1[3][3] * m2[3][0],
m1[3][0] * m2[0][1] +
m1[3][1] * m2[1][1] +
m1[3][2] * m2[2][1] +
m1[3][3] * m2[3][1],
m1[3][0] * m2[0][2] +
m1[3][1] * m2[1][2] +
m1[3][2] * m2[2][2] +
m1[3][3] * m2[3][2],
m1[3][0] * m2[0][3] +
m1[3][1] * m2[1][3] +
m1[3][2] * m2[2][3] +
m1[3][3] * m2[3][3]);
}
template<class T>
int Invert(const Matrix44t<T>& m, Matrix44t<T>* mo)
{
int i,r,c; // Inverts matrix of arbitrary size N using
int bigrow; // Gauss-Jordan elimination with partial pivoting,
T big,d; // inverse returned in mo.
T *mi[4]; // Beware : nearly singular matrices are more likely
T *tmp; // to fail in single precision than in double precision,
const int N=4; // and double precision isn't bombproof either....
Matrix44t<T> &Im=*mo;
Matrix44t<T> mt(m);
Im.setIdentity();
for (i=0;i<N;i++) // set row pointers for swapping
mi[i] = mt[i];
for (c=0;c<N;c++) // start column sweep
{
big=0.0;
for (r=c;r<N;r++) // find candidate partial pivot element
{
if (mi[r][c]>big)
{
big=fabs(mi[r][c]);
bigrow=r;
}
}
if (bigrow>c) // pivot element not on diagonal: swap rows
{
tmp=mi[c];
mi[c]=mi[bigrow];
mi[bigrow]=tmp;
tmp=Im[c];
Im[c]=Im[bigrow];
Im[bigrow]=tmp;
}
if (0.0==mi[c][c]) // problems?
{
cerr << "singular matrix: bailing!"; // something more graceful, perhaps?
return 0;
}
d = 1.0/mi[c][c]; // normalize row[c] so diagonal element is unity
for (i=0;i<N;i++)
{
mi[c][i]*=d;
Im[c][i]*=d;
}
for (r=0;r<N;r++) // zero the off-diagonal members of col[c]
if (r!=c)
{
d=mi[r][c];
for (i=0;i<N;i++)
{
mi[r][i] -= d*mi[c][i];
Im[r][i] -= d*Im[c][i];
}
}
} // end column sweep
return 1;
}
/*
* Invert a resticted 4x4 matrix. 4th column must be 0, 0, 0, 1
* Based on code by Doug Kerr.
* m and res may or may not be the same matrix.
*/
template <class T>
void restrictedInvert(const Matrix44t<T>&m, Matrix44t<T>*res)
{
Matrix44t<T> &r=*res;
Matrix33t<T> c; /* matrix of 3x3 cofactors */
T d; /* determinant of the 3x3 */
register int i, j;
/*
* compute the 3x3 cofactors
*/
c[0][0] = (m[1][1]*m[2][2] - m[2][1]*m[1][2]);
c[0][1] = -(m[1][0]*m[2][2] - m[2][0]*m[1][2]);
c[0][2] = (m[1][0]*m[2][1] - m[2][0]*m[1][1]);
c[1][0] = -(m[0][1]*m[2][2] - m[2][1]*m[0][2]);
c[1][1] = (m[0][0]*m[2][2] - m[2][0]*m[0][2]);
c[1][2] = -(m[0][0]*m[2][1] - m[2][0]*m[0][1]);
c[2][0] = (m[0][1]*m[1][2] - m[1][1]*m[0][2]);
c[2][1] = -(m[0][0]*m[1][2] - m[1][0]*m[0][2]);
c[2][2] = (m[0][0]*m[1][1] - m[1][0]*m[0][1]);
/*
* compute the 3x3 determinant
*/
d = 0.;
for( j=0; j<3; j++ )
d += m[0][j] * c[0][j];
/*
* compute the 3x3 inverse
*/
T mp[3];
for( i=0; i<3; i++ ) {
for( j=0; j<3; j++ )
r[i][j] = c[j][i] / d;
mp[i] = -m[3][i];
r[i][3] = 0;
}
r[3][0] = mp[0]*r[0][0] + mp[1]*r[1][0] + mp[2]*r[2][0];
r[3][1] = mp[0]*r[0][1] + mp[1]*r[1][1] + mp[2]*r[2][1];
r[3][2] = mp[0]*r[0][2] + mp[1]*r[1][2] + mp[2]*r[2][2];
r[3][3] = 1;
}
template <class T>
void Transpose(const Matrix44t<T>& m, Matrix44t<T>* res)
{
for (int i=0; i<4; ++i) {
res->mat[i][0] = m[0][i];
res->mat[i][1] = m[1][i];
res->mat[i][2] = m[2][i];
res->mat[i][3] = m[3][i];
}
}
template<class T>
void Matrix44t<T>::translate(const T &x,const T &y,const T &z)
{
float (*m)[4] = this->mat;
m[3][0] = x*m[0][0] + y*m[1][0] + z*m[2][0] + m[3][0];
m[3][1] = x*m[0][1] + y*m[1][1] + z*m[2][1] + m[3][1];
m[3][2] = x*m[0][2] + y*m[1][2] + z*m[2][2] + m[3][2];
m[3][3] = x*m[0][3] + y*m[1][3] + z*m[2][3] + m[3][3];
}
template<class T>
Matrix44t<T> Matrix44t<T>::rotated(char axis,const T &angle) const
{
Matrix44t<T> rotmat(Matrix44t<T>::identity);
int left, right, top, bottom;
float tangle;
left = (axis == 'x' ? 1 : 0);
right = (axis == 'z' ? 1 : 2);
top = (axis == 'x' ? 1 : 0);
bottom = (axis == 'z' ? 1 : 2);
rotmat[top][left] = cos((double)angle);
rotmat[top][right] = sin((double)angle);
if (axis == 'y')
rotmat[top][right] = -rotmat[top][right];
rotmat[bottom][left] = -rotmat[top][right];
rotmat[bottom][right] = rotmat[top][left];
return (rotmat * (*this));
}
template<class T>
void Matrix44t<T>::scale(const T &x,const T &y,const T &z)
{
for (int i=0; i<4; ++i) {
mat[0][i] = x * mat[0][i];
mat[1][i] = y * mat[1][i];
mat[2][i] = z * mat[2][i];
}
}
template<class T>
Matrix44t<T> Matrix44t<T>::scaled(const T &x,const T &y,const T &z) const
{
Matrix44t<T> m;
for (int i=0; i<4; ++i) {
m[0][i] = x * mat[0][i];
m[1][i] = y * mat[1][i];
m[2][i] = z * mat[2][i];
m[3][i] = mat[3][i];
}
return m;
}
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
static void _LinAlgebra_force_instantiation_hacks()
{
abort();
Vector3f v3f1(Xaxis3f), v3f2(Yaxis3f), v3f3;
v3f3 = v3f1 * v3f2;
Vector4f v4f1(Xaxis4f), v4f2(Yaxis4f), v4f3;
v4f3.cross3(v4f1,v4f2);
Vector3d v3d1(Xaxis3d), v3d2(Yaxis3d), v3d3;
v3d3 = v3d1 * v3d2;
Vector4d v4d1(Xaxis4d), v4d2(Yaxis4d), v4d3;
v4d3.cross3(v4d1,v4d2);
Matrix33f m33f(IdentMat33f);
Matrix33f m33f2 = m33f.inverse();
m33f2 = Matrix33f::identity;
Matrix44f m44f(IdentMat44f);
Matrix44f m44f2 = m44f.inverse();
m44f2 = Matrix44f::identity;
m44f.restrictedInvert();
Matrix33d m33d(IdentMat33d);
Matrix33d m33d2 = m33d.inverse();
m33d2 = Matrix33d::identity;
Matrix44d m44d(IdentMat44d);
Matrix44d m44d2 = m44d.inverse();
m44d2 = Matrix44d::identity;
m44d.restrictedInvert();
_LinAlgebra_force_instantiation_hacks();
}
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
More information about the Gcc-bugs
mailing list