From 4765c8744d6757bff5f8bf4a4e1fad7bfd51026f Mon Sep 17 00:00:00 2001 From: Robert Osfield Date: Mon, 12 Jan 2004 14:22:18 +0000 Subject: [PATCH] Introduce new Matrix::invert() implementation from Ravi Mathur, with tweaks by Robert Osfield. --- include/osg/Matrixd | 9 +- include/osg/Matrixf | 9 +- src/osg/CullStack.cpp | 13 +- src/osg/Matrix_implementation.cpp | 190 +++++++++++++++++++++++++++++- 4 files changed, 209 insertions(+), 12 deletions(-) diff --git a/include/osg/Matrixd b/include/osg/Matrixd index 02bedbbd5..d18925092 100644 --- a/include/osg/Matrixd +++ b/include/osg/Matrixd @@ -168,7 +168,14 @@ class SG_EXPORT Matrixd /** Get to the position and orientation of a modelview matrix, using the same convention as gluLookAt. */ void getLookAt(Vec3& eye,Vec3& center,Vec3& up,value_type lookDistance=1.0f); - bool invert( const Matrixd& ); + /** invert the matrix rhs. */ + bool invert( const Matrixd& rhs); + + /** full 4x4 matrix invert. */ + bool invert_4x4_orig( const Matrixd& ); + + /** full 4x4 matrix invert. */ + bool invert_4x4_new( const Matrixd& ); //basic utility functions to create new matrices inline static Matrixd identity( void ); diff --git a/include/osg/Matrixf b/include/osg/Matrixf index eaa056b48..010e66e20 100644 --- a/include/osg/Matrixf +++ b/include/osg/Matrixf @@ -167,7 +167,14 @@ class SG_EXPORT Matrixf /** Get to the position and orientation of a modelview matrix, using the same convention as gluLookAt. */ void getLookAt(Vec3& eye,Vec3& center,Vec3& up,value_type lookDistance=1.0f); - bool invert( const Matrixf& ); + /** invert the matrix rhs placing result in this matrix.*/ + bool invert( const Matrixf& rhs); + + /** full 4x4 matrix invert. */ + bool invert_4x4_orig( const Matrixf& ); + + /** full 4x4 matrix invert. */ + bool invert_4x4_new( const Matrixf& ); //basic utility functions to create new matrices inline static Matrixf identity( void ); diff --git a/src/osg/CullStack.cpp b/src/osg/CullStack.cpp index ba029b771..693f0f1c0 100644 --- a/src/osg/CullStack.cpp +++ b/src/osg/CullStack.cpp @@ -216,10 +216,11 @@ void CullStack::pushModelViewMatrix(RefMatrix* matrix) pushCullingSet(); - //osg::Timer timer; +#if 1 + osg::Vec3 slow_eyepoint(osg::Matrix::inverse(*matrix).getTrans()); + _eyePointStack.push_back(slow_eyepoint); +#else - //osg::Timer_t tick_1 = timer.tick(); - // fast method for computing the eye point in local coords which doesn't require the inverse matrix. const float x_0 = (*matrix)(0,0); const float x_1 = (*matrix)(1,0); @@ -272,9 +273,6 @@ void CullStack::pushModelViewMatrix(RefMatrix* matrix) x_1*x_scale + y_1*y_scale + z_1*z_scale, x_2*x_scale + y_2*y_scale + z_2*z_scale); - _eyePointStack.push_back(fast_eyepoint); - - // std::cout<<"fast path "<<*matrix<=0?1:0) | diff --git a/src/osg/Matrix_implementation.cpp b/src/osg/Matrix_implementation.cpp index 0883bef36..57360d303 100644 --- a/src/osg/Matrix_implementation.cpp +++ b/src/osg/Matrix_implementation.cpp @@ -14,6 +14,7 @@ #include #include #include +#include #include @@ -327,6 +328,191 @@ void Matrix_implementation::postMult( const Matrix_implementation& other ) #undef INNER_PRODUCT +bool Matrix_implementation::invert( const Matrix_implementation& rhs) +{ +#if 1 + return invert_4x4_new(rhs); +#else + static const osg::Timer& timer = *Timer::instance(); + + Matrix_implementation a; + Matrix_implementation b; + + Timer_t t1 = timer.tick(); + + a.invert_4x4_new(rhs); + + Timer_t t2 = timer.tick(); + + b.invert_4x4_orig(rhs); + + Timer_t t3 = timer.tick(); + + static double new_time = 0.0; + static double orig_time = 0.0; + static double count = 0.0; + + new_time += timer.delta_u(t1,t2); + orig_time += timer.delta_u(t2,t3); + ++count; + + std::cout<<"Average new="< 1.0e-6 ) // Involves perspective, so we must + { // compute the full inverse + + Matrix_implementation TPinv; + _mat[3][0] = _mat[3][1] = _mat[3][2] = 0.0; + +#define px r00 +#define py r01 +#define pz r02 +#define one_over_s one_over_det +#define a r10 +#define b r11 +#define c r12 + + a = mat._mat[0][3]; b = mat._mat[1][3]; c = mat._mat[2][3]; + px = _mat[0][0]*a + _mat[0][1]*b + _mat[0][2]*c; + py = _mat[1][0]*a + _mat[1][1]*b + _mat[1][2]*c; + pz = _mat[2][0]*a + _mat[2][1]*b + _mat[2][2]*c; + +#undef a +#undef a +#undef c +#define tx r10 +#define ty r11 +#define tz r12 + + tx = mat._mat[3][0]; ty = mat._mat[3][1]; tz = mat._mat[3][2]; + one_over_s = 1.0/(d - (tx*px + ty*py + tz*pz)); + + tx *= one_over_s; ty *= one_over_s; tz *= one_over_s; // Reduces number of calculations later on + + // Compute inverse of trans*corr + TPinv._mat[0][0] = tx*px + 1.0; + TPinv._mat[0][1] = ty*px; + TPinv._mat[0][2] = tz*px; + TPinv._mat[0][3] = -px * one_over_s; + TPinv._mat[1][0] = tx*py; + TPinv._mat[1][1] = ty*py + 1.0; + TPinv._mat[1][2] = tz*py; + TPinv._mat[1][3] = -py * one_over_s; + TPinv._mat[2][0] = tx*pz; + TPinv._mat[2][1] = ty*pz; + TPinv._mat[2][2] = tz*pz + 1.0; + TPinv._mat[2][3] = -pz * one_over_s; + TPinv._mat[3][0] = -tx; + TPinv._mat[3][1] = -ty; + TPinv._mat[3][2] = -tz; + TPinv._mat[3][3] = one_over_s; + + preMult(TPinv); // Finish computing full inverse of mat + +#undef px +#undef py +#undef pz +#undef s +#undef d + } + else // Rightmost column is [0; 0; 0; 1] so it can be ignored + { + tx = mat._mat[3][0]; ty = mat._mat[3][1]; tz = mat._mat[3][2]; + + // Compute translation components of mat' + _mat[3][0] = -(tx*_mat[0][0] + ty*_mat[1][0] + tz*_mat[2][0]); + _mat[3][1] = -(tx*_mat[0][1] + ty*_mat[1][1] + tz*_mat[2][1]); + _mat[3][2] = -(tx*_mat[0][2] + ty*_mat[1][2] + tz*_mat[2][2]); + +#undef tx +#undef ty +#undef tz + } + + return true; +} + + template inline T SGL_ABS(T a) { @@ -337,11 +523,11 @@ inline T SGL_ABS(T a) #define SGL_SWAP(a,b,temp) ((temp)=(a),(a)=(b),(b)=(temp)) #endif -bool Matrix_implementation::invert( const Matrix_implementation& mat ) +bool Matrix_implementation::invert_4x4_orig( const Matrix_implementation& mat ) { if (&mat==this) { Matrix_implementation tm(mat); - return invert(tm); + return invert_4x4_orig(tm); } unsigned int indxc[4], indxr[4], ipiv[4];