e17261c45f
modelview matrix rathan than an identity matrix, this matrix is then modified locally. Changed the osg::Matrix set methods so they are inline. Added a few useful comments to MemoryManager.cpp to help people understand the assert's better.
361 lines
9.5 KiB
C++
361 lines
9.5 KiB
C++
#include <osg/Matrix>
|
|
#include <osg/Quat>
|
|
#include <osg/Notify>
|
|
#include <osg/Types>
|
|
#include <osg/Math>
|
|
|
|
#include <stdlib.h>
|
|
|
|
using namespace osg;
|
|
|
|
#define SET_ROW(row, v1, v2, v3, v4 ) \
|
|
_mat[(row)][0] = (v1); \
|
|
_mat[(row)][1] = (v2); \
|
|
_mat[(row)][2] = (v3); \
|
|
_mat[(row)][3] = (v4);
|
|
|
|
#define INNER_PRODUCT(a,b,r,c) \
|
|
((a)._mat[r][0] * (b)._mat[0][c]) \
|
|
+((a)._mat[r][1] * (b)._mat[1][c]) \
|
|
+((a)._mat[r][2] * (b)._mat[2][c]) \
|
|
+((a)._mat[r][3] * (b)._mat[3][c])
|
|
|
|
|
|
|
|
|
|
|
|
Matrix::Matrix() : Object()
|
|
{
|
|
makeIdentity();
|
|
}
|
|
|
|
Matrix::Matrix( const Matrix& other) : Object()
|
|
{
|
|
set( (const float *) other._mat );
|
|
}
|
|
|
|
Matrix::Matrix( const float * const def )
|
|
{
|
|
set( def );
|
|
}
|
|
|
|
Matrix::Matrix( float a00, float a01, float a02, float a03,
|
|
float a10, float a11, float a12, float a13,
|
|
float a20, float a21, float a22, float a23,
|
|
float a30, float a31, float a32, float a33)
|
|
{
|
|
SET_ROW(0, a00, a01, a02, a03 )
|
|
SET_ROW(1, a10, a11, a12, a13 )
|
|
SET_ROW(2, a20, a21, a22, a23 )
|
|
SET_ROW(3, a30, a31, a32, a33 )
|
|
}
|
|
|
|
void Matrix::set( float a00, float a01, float a02, float a03,
|
|
float a10, float a11, float a12, float a13,
|
|
float a20, float a21, float a22, float a23,
|
|
float a30, float a31, float a32, float a33)
|
|
{
|
|
SET_ROW(0, a00, a01, a02, a03 )
|
|
SET_ROW(1, a10, a11, a12, a13 )
|
|
SET_ROW(2, a20, a21, a22, a23 )
|
|
SET_ROW(3, a30, a31, a32, a33 )
|
|
}
|
|
|
|
void Matrix::setTrans( float tx, float ty, float tz )
|
|
{
|
|
_mat[3][0] = tx;
|
|
_mat[3][1] = ty;
|
|
_mat[3][2] = tz;
|
|
}
|
|
|
|
|
|
void Matrix::setTrans( const Vec3& v )
|
|
{
|
|
_mat[3][0] = v[0];
|
|
_mat[3][1] = v[1];
|
|
_mat[3][2] = v[2];
|
|
}
|
|
|
|
void Matrix::makeIdentity()
|
|
{
|
|
SET_ROW(0, 1, 0, 0, 0 )
|
|
SET_ROW(1, 0, 1, 0, 0 )
|
|
SET_ROW(2, 0, 0, 1, 0 )
|
|
SET_ROW(3, 0, 0, 0, 1 )
|
|
}
|
|
|
|
void Matrix::makeScale( const Vec3& v )
|
|
{
|
|
makeScale(v[0], v[1], v[2] );
|
|
}
|
|
|
|
void Matrix::makeScale( float x, float y, float z )
|
|
{
|
|
SET_ROW(0, x, 0, 0, 0 )
|
|
SET_ROW(1, 0, y, 0, 0 )
|
|
SET_ROW(2, 0, 0, z, 0 )
|
|
SET_ROW(3, 0, 0, 0, 1 )
|
|
}
|
|
|
|
void Matrix::makeTranslate( const Vec3& v )
|
|
{
|
|
makeTranslate( v[0], v[1], v[2] );
|
|
}
|
|
|
|
void Matrix::makeTranslate( float x, float y, float z )
|
|
{
|
|
SET_ROW(0, 1, 0, 0, 0 )
|
|
SET_ROW(1, 0, 1, 0, 0 )
|
|
SET_ROW(2, 0, 0, 1, 0 )
|
|
SET_ROW(3, x, y, z, 1 )
|
|
}
|
|
|
|
void Matrix::makeRotate( const Vec3& from, const Vec3& to )
|
|
{
|
|
Quat quat;
|
|
quat.makeRotate(from,to);
|
|
quat.get(*this);
|
|
}
|
|
|
|
void Matrix::makeRotate( float angle, const Vec3& axis )
|
|
{
|
|
Quat quat;
|
|
quat.makeRotate( angle, axis);
|
|
quat.get(*this);
|
|
}
|
|
|
|
void Matrix::makeRotate( float angle, float x, float y, float z )
|
|
{
|
|
Quat quat;
|
|
quat.makeRotate( angle, x, y, z);
|
|
quat.get(*this);
|
|
}
|
|
|
|
void Matrix::makeRotate( const Quat& q )
|
|
{
|
|
q.get(*this);
|
|
}
|
|
|
|
void Matrix::makeRotate( float heading, float pitch, float roll)
|
|
{
|
|
Quat quat;
|
|
quat.makeRotate(heading,pitch,roll);
|
|
quat.get(*this);
|
|
}
|
|
|
|
void Matrix::mult( const Matrix& lhs, const Matrix& rhs )
|
|
{
|
|
if (&lhs==this)
|
|
{
|
|
postMult(rhs);
|
|
return;
|
|
}
|
|
if (&rhs==this)
|
|
{
|
|
preMult(lhs);
|
|
}
|
|
|
|
// PRECONDITION: We assume neither &lhs nor &rhs == this
|
|
// if it did, use preMult or postMult instead
|
|
_mat[0][0] = INNER_PRODUCT(lhs, rhs, 0, 0);
|
|
_mat[0][1] = INNER_PRODUCT(lhs, rhs, 0, 1);
|
|
_mat[0][2] = INNER_PRODUCT(lhs, rhs, 0, 2);
|
|
_mat[0][3] = INNER_PRODUCT(lhs, rhs, 0, 3);
|
|
_mat[1][0] = INNER_PRODUCT(lhs, rhs, 1, 0);
|
|
_mat[1][1] = INNER_PRODUCT(lhs, rhs, 1, 1);
|
|
_mat[1][2] = INNER_PRODUCT(lhs, rhs, 1, 2);
|
|
_mat[1][3] = INNER_PRODUCT(lhs, rhs, 1, 3);
|
|
_mat[2][0] = INNER_PRODUCT(lhs, rhs, 2, 0);
|
|
_mat[2][1] = INNER_PRODUCT(lhs, rhs, 2, 1);
|
|
_mat[2][2] = INNER_PRODUCT(lhs, rhs, 2, 2);
|
|
_mat[2][3] = INNER_PRODUCT(lhs, rhs, 2, 3);
|
|
_mat[3][0] = INNER_PRODUCT(lhs, rhs, 3, 0);
|
|
_mat[3][1] = INNER_PRODUCT(lhs, rhs, 3, 1);
|
|
_mat[3][2] = INNER_PRODUCT(lhs, rhs, 3, 2);
|
|
_mat[3][3] = INNER_PRODUCT(lhs, rhs, 3, 3);
|
|
}
|
|
|
|
void Matrix::preMult( const Matrix& other )
|
|
{
|
|
// brute force method requiring a copy
|
|
//Matrix tmp(other* *this);
|
|
// *this = tmp;
|
|
|
|
// more efficient method just use a float[4] for temporary storage.
|
|
float t[4];
|
|
for(int col=0; col<4; ++col) {
|
|
t[0] = INNER_PRODUCT( other, *this, 0, col );
|
|
t[1] = INNER_PRODUCT( other, *this, 1, col );
|
|
t[2] = INNER_PRODUCT( other, *this, 2, col );
|
|
t[3] = INNER_PRODUCT( other, *this, 3, col );
|
|
_mat[0][col] = t[0];
|
|
_mat[1][col] = t[1];
|
|
_mat[2][col] = t[2];
|
|
_mat[3][col] = t[3];
|
|
}
|
|
|
|
}
|
|
|
|
void Matrix::postMult( const Matrix& other )
|
|
{
|
|
// brute force method requiring a copy
|
|
//Matrix tmp(*this * other);
|
|
// *this = tmp;
|
|
|
|
// more efficient method just use a float[4] for temporary storage.
|
|
float t[4];
|
|
for(int row=0; row<4; ++row)
|
|
{
|
|
t[0] = INNER_PRODUCT( *this, other, row, 0 );
|
|
t[1] = INNER_PRODUCT( *this, other, row, 1 );
|
|
t[2] = INNER_PRODUCT( *this, other, row, 2 );
|
|
t[3] = INNER_PRODUCT( *this, other, row, 3 );
|
|
SET_ROW(row, t[0], t[1], t[2], t[3] )
|
|
}
|
|
}
|
|
|
|
#undef INNER_PRODUCT
|
|
|
|
|
|
template <class T>
|
|
inline T SGL_ABS(T a)
|
|
{
|
|
return (a >= 0 ? a : -a);
|
|
}
|
|
|
|
#ifndef SGL_SWAP
|
|
#define SGL_SWAP(a,b,temp) ((temp)=(a),(a)=(b),(b)=(temp))
|
|
#endif
|
|
|
|
bool Matrix::invert( const Matrix& mat )
|
|
{
|
|
if (&mat==this) {
|
|
Matrix tm(mat);
|
|
return invert(tm);
|
|
}
|
|
|
|
unsigned int indxc[4], indxr[4], ipiv[4];
|
|
unsigned int i,j,k,l,ll;
|
|
unsigned int icol = 0;
|
|
unsigned int irow = 0;
|
|
float temp, pivinv, dum, big;
|
|
|
|
// copy in place this may be unnecessary
|
|
*this = mat;
|
|
|
|
for (j=0; j<4; j++) ipiv[j]=0;
|
|
|
|
for(i=0;i<4;i++)
|
|
{
|
|
big=(float)0.0;
|
|
for (j=0; j<4; j++)
|
|
if (ipiv[j] != 1)
|
|
for (k=0; k<4; k++)
|
|
{
|
|
if (ipiv[k] == 0)
|
|
{
|
|
if (SGL_ABS(operator()(j,k)) >= big)
|
|
{
|
|
big = SGL_ABS(operator()(j,k));
|
|
irow=j;
|
|
icol=k;
|
|
}
|
|
}
|
|
else if (ipiv[k] > 1)
|
|
return false;
|
|
}
|
|
++(ipiv[icol]);
|
|
if (irow != icol)
|
|
for (l=0; l<4; l++) SGL_SWAP(operator()(irow,l),
|
|
operator()(icol,l),
|
|
temp);
|
|
|
|
indxr[i]=irow;
|
|
indxc[i]=icol;
|
|
if (operator()(icol,icol) == 0)
|
|
return false;
|
|
|
|
pivinv = 1.0/operator()(icol,icol);
|
|
operator()(icol,icol) = 1;
|
|
for (l=0; l<4; l++) operator()(icol,l) *= pivinv;
|
|
for (ll=0; ll<4; ll++)
|
|
if (ll != icol)
|
|
{
|
|
dum=operator()(ll,icol);
|
|
operator()(ll,icol) = 0;
|
|
for (l=0; l<4; l++) operator()(ll,l) -= operator()(icol,l)*dum;
|
|
}
|
|
}
|
|
for (int lx=4; lx>0; --lx)
|
|
{
|
|
if (indxr[lx-1] != indxc[lx-1])
|
|
for (k=0; k<4; k++) SGL_SWAP(operator()(k,indxr[lx-1]),
|
|
operator()(k,indxc[lx-1]),temp);
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
void Matrix::makeOrtho(const double left, const double right,
|
|
const double bottom, const double top,
|
|
const double zNear, const double zFar)
|
|
{
|
|
// note transpose of Matrix wr.t OpenGL documentation, since the OSG use post multiplication rather than pre.
|
|
double tx = -(right+left)/(right-left);
|
|
double ty = -(top+bottom)/(top-bottom);
|
|
double tz = -(zFar+zNear)/(zFar-zNear);
|
|
SET_ROW(0, 2.0f/(right-left), 0.0f, 0.0f, 0.0f )
|
|
SET_ROW(1, 0.0f, 2.0f/(top-bottom), 0.0f, 0.0f )
|
|
SET_ROW(2, 0.0f, 0.0f, -2.0f/(zFar-zNear), 0.0f )
|
|
SET_ROW(3, tx, ty, tz, 1.0f )
|
|
}
|
|
|
|
void Matrix::makeFrustum(const double left, const double right,
|
|
const double bottom, const double top,
|
|
const double zNear, const double zFar)
|
|
{
|
|
// note transpose of Matrix wr.t OpenGL documentation, since the OSG use post multiplication rather than pre.
|
|
double A = (right+left)/(right-left);
|
|
double B = (top+bottom)/(top-bottom);
|
|
double C = -(zFar+zNear)/(zFar-zNear);
|
|
double D = -2.0*zFar*zNear/(zFar-zNear);
|
|
SET_ROW(0, 2.0f*zNear/(right-left), 0.0f, 0.0f, 0.0f )
|
|
SET_ROW(1, 0.0f, 2.0f*zNear/(top-bottom), 0.0f, 0.0f )
|
|
SET_ROW(2, A, B, C, -1.0f )
|
|
SET_ROW(3, 0.0f, 0.0f, D, 0.0f )
|
|
}
|
|
|
|
|
|
void Matrix::makePerspective(const double fovy,const double aspectRatio,
|
|
const double zNear, const double zFar)
|
|
{
|
|
// calculate the appropriate left, right etc.
|
|
double tan_fovy = tan(DegreesToRadians(fovy*0.5));
|
|
double right = tan_fovy * aspectRatio * zNear;
|
|
double left = -right;
|
|
double top = tan_fovy * zNear;
|
|
double bottom = -top;
|
|
makeFrustum(left,right,bottom,top,zNear,zFar);
|
|
}
|
|
|
|
|
|
void Matrix::makeLookAt(const Vec3& eye,const Vec3& center,const Vec3& up)
|
|
{
|
|
Vec3 f(center-eye);
|
|
f.normalize();
|
|
Vec3 s(f^up);
|
|
s.normalize();
|
|
Vec3 u(s^f);
|
|
u.normalize();
|
|
|
|
set(
|
|
s[0], u[0], -f[0], 0.0f,
|
|
s[1], u[1], -f[1], 0.0f,
|
|
s[2], u[2], -f[2], 0.0f,
|
|
0.0f, 0.0f, 0.0f, 1.0f);
|
|
|
|
preMult(Matrix::translate(-eye));
|
|
}
|
|
|
|
#undef SET_ROW
|