diff --git a/include/osg/Matrix.new b/include/osg/Matrix.new deleted file mode 100644 index f145e9e04..000000000 --- a/include/osg/Matrix.new +++ /dev/null @@ -1,225 +0,0 @@ - -#ifndef OSG_Matrix -#define OSG_Matrix 1 - -#include -#include -#include -//#include - -#ifdef OSG_USE_IO_DOT_H -#include -#else -#include -using namespace std; -#endif - -namespace osg { - -class Quat; - -class SG_EXPORT Matrix : public Object -{ -// private: - public: - float _mat[4][4]; - bool fully_realized; - - public: -// const char* name() { return "My Matrix "; } - META_Object(Matrix); - - Matrix(); - Matrix( const Matrix& other ); - explicit Matrix( float const * const def ); - 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); - - virtual ~Matrix() {} - - Matrix& operator = (const Matrix& ); - - int compare(const Matrix& m) const { return memcmp(_mat,m._mat,sizeof(_mat)); } - - bool operator < (const Matrix& m) const { return compare(m)<0; } - bool operator == (const Matrix& m) const { return compare(m)==0; } - bool operator != (const Matrix& m) const { return compare(m)!=0; } - - inline float& operator()(int col, int row) { return _mat[col][row]; } - inline float operator()(int col, int row) const { return _mat[col][row]; } - - void set( float const * const ); - void 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); - const float * values() { return (const float *)_mat; } - - void makeIdent(); - void makeScale( const Vec3& ); - void makeScale( float, float, float ); - - void makeTrans( const Vec3& ); - void makeTrans( float, float, float ); - //TODO: original preTrans was optimized (M=Tr*M) - // but also has the assumption that M (this) is an affine transformation Matrix - // can I still do something to optimize the same case now? - - void makeRot( const Vec3& from, const Vec3& to ); - void makeRot( float angle, const Vec3& orientation ); - void makeRot( float angle, float x, float y, float z ); - void makeRot( const Quat& ); - void makeRot( float, float, float ); //Euler angles - - bool invert( const Matrix& ); - bool invertAffine( const Matrix& ); - - //basic utility functions to create new matrices or vectors - static Matrix scale( const Vec3& ); - static Matrix scale( float, float, float ); - static Matrix trans( const Vec3& ); - static Matrix trans( float, float, float ); - static Matrix rotate( const Vec3&, const Vec3& ); - static Matrix rotate( float, float, float, float ); - static Matrix rotate( const Quat& ); - - inline Vec3 preMult( const Vec3& v ) const; - inline Vec3 postMult( const Vec3& v ) const; - inline Vec3 operator* ( const Vec3& v ) const; - inline Vec4 preMult( const Vec4& v ) const; - inline Vec4 postMult( const Vec4& v ) const; - inline Vec4 operator* ( const Vec4& v ) const; - -//start of Deprecated methods - - void copy( const Matrix& ); - void preScale( float sx, float sy, float sz, const Matrix& m ); - void postScale( const Matrix& m, float sx, float sy, float sz ); - void preScale( float sx, float sy, float sz ); - void postScale( float sx, float sy, float sz ); - - void preTrans( float tx, float ty, float tz, const Matrix& m ); - void postTrans( const Matrix& m, float tx, float ty, float tz ); - void preTrans( float tx, float ty, float tz); - void postTrans( float tx, float ty, float tz ); - - void preRot( float deg, float x, float y, float z, const Matrix& m ); - void postRot( const Matrix& m, float deg, float x, float y, float z ); - void preRot( float deg, float x, float y, float z ); - void postRot( float deg, float x, float y, float z ); - - /** apply apply an 3x3 transform of v*M[0..2,0..2] */ - inline static Vec3 transform3x3(const Vec3& v,const Matrix& m); - /** apply apply an 3x3 transform of M[0..2,0..2]*v */ - inline static Vec3 transform3x3(const Matrix& m,const Vec3& v); - -//end of Deprecated methods - - - // basic matrix multiplication, our workhorse methods. - void mult( const Matrix&, const Matrix& ); - void preMult( const Matrix& ); - void postMult( const Matrix& ); - - // Helper class to optimize product expressions somewhat - class MatrixProduct { - public: - const Matrix& A; - const Matrix& B; - - MatrixProduct( const Matrix& lhs, const Matrix& rhs ) : A(lhs), B(rhs) {} - }; - - inline MatrixProduct operator * ( const Matrix& other ) const - { return MatrixProduct(*this, other); } - - inline void operator *= ( const Matrix& other ) - { if( this == &other ) { - Matrix temp(other); - postMult( temp ); - } - else postMult( other ); - } - inline void operator = ( const MatrixProduct& p ) - { - if( this == &(p.A)) postMult(p.B); - else if( this == &(p.B)) preMult(p.A); - else mult( p.A, p.B ); - } - - Matrix( const MatrixProduct& p ) //allows implicit evaluation of the product - { mult( p.A, p.B ); } -}; - -inline Vec3 Matrix::postMult ( const Vec3& v ) const { - float d = 1.0f/(_mat[3][0]*v.x()+_mat[3][1]*v.y()+_mat[3][2]*v.z()+_mat[3][3]) ; - return Vec3( (_mat[0][0]*v.x() + _mat[0][1]*v.y() + _mat[0][2]*v.z() + _mat[0][3])*d, - (_mat[1][0]*v.x() + _mat[1][1]*v.y() + _mat[1][2]*v.z() + _mat[1][3])*d, - (_mat[2][0]*v.x() + _mat[2][1]*v.y() + _mat[2][2]*v.z() + _mat[2][3])*d) ; -} - -inline Vec3 Matrix::preMult (const Vec3& v ) const { - float d = 1.0f/(_mat[0][3]*v.x()+_mat[1][3]*v.y()+_mat[2][3]*v.z()+_mat[3][3]) ; - return Vec3( (_mat[0][0]*v.x() + _mat[1][0]*v.y() + _mat[2][0]*v.z() + _mat[3][0])*d, - (_mat[0][1]*v.x() + _mat[1][1]*v.y() + _mat[2][1]*v.z() + _mat[3][1])*d, - (_mat[0][2]*v.x() + _mat[1][2]*v.y() + _mat[2][2]*v.z() + _mat[3][2])*d); -} -inline Vec3 Matrix::operator* (const Vec3& v) const { - return postMult(v); -} -inline Vec3 operator* (const Vec3& v, const Matrix& m ) { - return m.preMult(v); -} -inline Vec4 Matrix::postMult(const Vec4& v) const { - return Vec4( (_mat[0][0]*v.x() + _mat[0][1]*v.y() + _mat[0][2]*v.z() + _mat[0][3]*v.w()), - (_mat[1][0]*v.x() + _mat[1][1]*v.y() + _mat[1][2]*v.z() + _mat[1][3]*v.w()), - (_mat[2][0]*v.x() + _mat[2][1]*v.y() + _mat[2][2]*v.z() + _mat[2][3]*v.w()), - (_mat[3][0]*v.x() + _mat[3][1]*v.y() + _mat[3][2]*v.z() + _mat[3][3]*v.w())) ; -} -/* -inline Vec4 Matrix::preMult(const Vec4& v,const Matrix& m) { - return Vec4( (m._mat[0][0]*v.x() + m._mat[1][0]*v.y() + m._mat[2][0]*v.z() + m._mat[3][0]*v.w()), - (m._mat[0][1]*v.x() + m._mat[1][1]*v.y() + m._mat[2][1]*v.z() + m._mat[3][1]*v.w()), - (m._mat[0][2]*v.x() + m._mat[1][2]*v.y() + m._mat[2][2]*v.z() + m._mat[3][2]*v.w()), - (m._mat[0][3]*v.x() + m._mat[1][3]*v.y() + m._mat[2][3]*v.z() + m._mat[3][3]*v.w())); -} -*/ -inline Vec4 Matrix::operator* (const Vec4& v) const { - return postMult(v); -} -inline Vec4 operator* (const Vec4& v, const Matrix& m ) { - return m.preMult(v); -} - -inline Vec3 Matrix::transform3x3(const Vec3& v,const Matrix& m) -{ - return Vec3( (m._mat[0][0]*v.x() + m._mat[1][0]*v.y() + m._mat[2][0]*v.z()), - (m._mat[0][1]*v.x() + m._mat[1][1]*v.y() + m._mat[2][1]*v.z()), - (m._mat[0][2]*v.x() + m._mat[1][2]*v.y() + m._mat[2][2]*v.z())); -} - -inline Vec3 Matrix::transform3x3(const Matrix& m,const Vec3& v) -{ - return Vec3( (m._mat[0][0]*v.x() + m._mat[0][1]*v.y() + m._mat[0][2]*v.z()), - (m._mat[1][0]*v.x() + m._mat[1][1]*v.y() + m._mat[1][2]*v.z()), - (m._mat[2][0]*v.x() + m._mat[2][1]*v.y() + m._mat[2][2]*v.z()) ) ; -} - -inline ostream& operator<< (ostream& os, const Matrix& m ) { - os << "{"; - for(int row=0; row<4; ++row) { - os << "\t"; - for(int col=0; col<4; ++col) - os << m(col,row) << " "; - os << endl; - } - os << "}" << endl; - return os; -} - -}; //namespace osg - - -#endif diff --git a/include/osg/Matrix.old b/include/osg/Matrix.old deleted file mode 100644 index 4dd94dd08..000000000 --- a/include/osg/Matrix.old +++ /dev/null @@ -1,190 +0,0 @@ -#ifndef OSG_MATRIX -#define OSG_MATRIX 1 - -#include -#include -#include - -#ifdef OSG_USE_IO_DOT_H -#include -#else -#include -using namespace std; -#endif - -namespace osg { - -/** 4x4 Matrix for storage & manipulation of transformations in scene graph. - Provides basic maths operations, IO and via osg::Object reference counting. - You can directly load the matrix with OpenGL's LoadMatrixf() function via - the public member _mat as the matrix is stored in the OpenGL format. - Caution: The disadvantage of this feature is, that the matrix access is - 'transposed' if you compare it with the standard C/C++ 2d-array-access - convention . I.e. _mat[i][j] accesses the ith column of the jth row in the - 4x4 matrix. -*/ - -class SG_EXPORT Matrix : public Object -{ - public: - Matrix(); - Matrix(const Matrix& 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); - - Matrix& operator = (const Matrix& matrix); - - virtual ~Matrix(); - - virtual Object* clone() const { return new Matrix(); } - virtual bool isSameKindAs(const Object* obj) const { return dynamic_cast(obj)!=NULL; } - virtual const char* className() const { return "Matrix"; } - - int compare(const Matrix& m) const { return memcmp(_mat,m._mat,sizeof(_mat)); } - - bool operator < (const Matrix& m) const { return compare(m)<0; } - bool operator == (const Matrix& m) const { return compare(m)==0; } - bool operator != (const Matrix& m) const { return compare(m)!=0; } - - void makeIdent(); - - void set(const float* m); - - void 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); - - void copy(const Matrix& matrix); - - void makeScale(float sx, float sy, float sz); - void preScale( float sx, float sy, float sz, const Matrix& m ); - void postScale( const Matrix& m, float sx, float sy, float sz ); - - void preScale( float sx, float sy, float sz ); - void postScale( float sx, float sy, float sz ); - - - void makeTrans( float tx, float ty, float tz ); - void preTrans( float tx, float ty, float tz, const Matrix& m ); - void postTrans( const Matrix& m, float tx, float ty, float tz ); - - void preTrans( float tx, float ty, float tz ); - void postTrans( float tx, float ty, float tz ); - - - /** - * Calc the rotation matrix which aligns vector \a old_vec with - * vector \a new_vec. Both \a old_vec and \a new_vec must have - * length 1.0. - */ - void makeRot( const Vec3& old_vec, const Vec3& new_vec ); - - void makeRot( float deg, float x, float y, float z ); - void preRot( float deg, float x, float y, float z, const Matrix& m ); - void postRot( const Matrix& m, float deg, float x, float y, float z ); - - void preRot( float deg, float x, float y, float z ); - void postRot( float deg, float x, float y, float z ); - - void setTrans( float tx, float ty, float tz ); - void setTrans( const Vec3& v ); - Vec3 getTrans() const { return Vec3(_mat[3][0],_mat[3][1],_mat[3][2]); } - - void preMult(const Matrix& m); - void postMult(const Matrix& m); - void mult(const Matrix& lhs,const Matrix& rhs); - - Matrix operator * (const Matrix& m) const; - - /** apply apply an 3x3 transform of v*M[0..2,0..2] */ - inline static Vec3 transform3x3(const Vec3& v,const Matrix& m); - /** apply apply an 3x3 transform of M[0..2,0..2]*v */ - inline static Vec3 transform3x3(const Matrix& m,const Vec3& v); - - /** post multipy v. ie. (m*v) */ - inline Vec3 operator * (const Vec3& v) const; - - /** pre multipy v. ie. (v*m) */ - friend inline Vec3 operator * (const Vec3& v,const Matrix& m); - - /** post multipy v. ie. (m*v) */ - inline Vec4 operator * (const Vec4& v) const; - - /** pre multipy v. ie. (v*m) */ - friend inline Vec4 operator * (const Vec4& v,const Matrix& m); - - friend inline ostream& operator << (ostream& output, const Matrix& matrix); - - bool invert(const Matrix& m); - - public : - float _mat[4][4]; - - protected: -}; - -inline Vec3 Matrix::operator * (const Vec3& v) const -{ - float d = 1.0f/(_mat[3][0]*v.x()+_mat[3][1]*v.y()+_mat[3][2]*v.z()+_mat[3][3]) ; - return Vec3( (_mat[0][0]*v.x() + _mat[0][1]*v.y() + _mat[0][2]*v.z() + _mat[0][3])*d, - (_mat[1][0]*v.x() + _mat[1][1]*v.y() + _mat[1][2]*v.z() + _mat[1][3])*d, - (_mat[2][0]*v.x() + _mat[2][1]*v.y() + _mat[2][2]*v.z() + _mat[2][3])*d) ; -} - - -inline Vec3 operator * (const Vec3& v,const Matrix& m) -{ - float d = 1.0f/(m._mat[0][3]*v.x()+m._mat[1][3]*v.y()+m._mat[2][3]*v.z()+m._mat[3][3]) ; - return Vec3( (m._mat[0][0]*v.x() + m._mat[1][0]*v.y() + m._mat[2][0]*v.z() + m._mat[3][0])*d, - (m._mat[0][1]*v.x() + m._mat[1][1]*v.y() + m._mat[2][1]*v.z() + m._mat[3][1])*d, - (m._mat[0][2]*v.x() + m._mat[1][2]*v.y() + m._mat[2][2]*v.z() + m._mat[3][2])*d); -} - -inline Vec4 Matrix::operator * (const Vec4& v) const -{ - return Vec4( (_mat[0][0]*v.x() + _mat[0][1]*v.y() + _mat[0][2]*v.z() + _mat[0][3]*v.w()), - (_mat[1][0]*v.x() + _mat[1][1]*v.y() + _mat[1][2]*v.z() + _mat[1][3]*v.w()), - (_mat[2][0]*v.x() + _mat[2][1]*v.y() + _mat[2][2]*v.z() + _mat[2][3]*v.w()), - (_mat[3][0]*v.x() + _mat[3][1]*v.y() + _mat[3][2]*v.z() + _mat[3][3]*v.w())) ; -} - - -inline Vec4 operator * (const Vec4& v,const Matrix& m) -{ - return Vec4( (m._mat[0][0]*v.x() + m._mat[1][0]*v.y() + m._mat[2][0]*v.z() + m._mat[3][0]*v.w()), - (m._mat[0][1]*v.x() + m._mat[1][1]*v.y() + m._mat[2][1]*v.z() + m._mat[3][1]*v.w()), - (m._mat[0][2]*v.x() + m._mat[1][2]*v.y() + m._mat[2][2]*v.z() + m._mat[3][2]*v.w()), - (m._mat[0][3]*v.x() + m._mat[1][3]*v.y() + m._mat[2][3]*v.z() + m._mat[3][3]*v.w())); -} - -inline Vec3 Matrix::transform3x3(const Vec3& v,const Matrix& m) -{ - return Vec3( (m._mat[0][0]*v.x() + m._mat[1][0]*v.y() + m._mat[2][0]*v.z()), - (m._mat[0][1]*v.x() + m._mat[1][1]*v.y() + m._mat[2][1]*v.z()), - (m._mat[0][2]*v.x() + m._mat[1][2]*v.y() + m._mat[2][2]*v.z())); -} - -inline Vec3 Matrix::transform3x3(const Matrix& m,const Vec3& v) -{ - return Vec3( (m._mat[0][0]*v.x() + m._mat[0][1]*v.y() + m._mat[0][2]*v.z()), - (m._mat[1][0]*v.x() + m._mat[1][1]*v.y() + m._mat[1][2]*v.z()), - (m._mat[2][0]*v.x() + m._mat[2][1]*v.y() + m._mat[2][2]*v.z()) ) ; -} - -inline ostream& operator << (ostream& output, const Matrix& matrix) -{ - output << "{"< -#include -#include - -#include //memcpy -#include //acos - -using namespace osg; - -#define DEG2RAD(x) ((x)*M_PI/180.0) -#define RAD2DEG(x) ((x)*180.0/M_PI) - - -#define WARN_DEPRECATED -#define ANGLES_IN_DEGREES - -Matrix::Matrix() : Object(), fully_realized(false) {} - -Matrix::Matrix( const Matrix& other ) : Object() { - set( (float const * const) other._mat ); -} - -Matrix::Matrix( float const * 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) -{ - _mat[0][0] = a00; - _mat[0][1] = a01; - _mat[0][2] = a02; - _mat[0][3] = a03; - - _mat[1][0] = a10; - _mat[1][1] = a11; - _mat[1][2] = a12; - _mat[1][3] = a13; - - _mat[2][0] = a20; - _mat[2][1] = a21; - _mat[2][2] = a22; - _mat[2][3] = a23; - - _mat[3][0] = a30; - _mat[3][1] = a31; - _mat[3][2] = a32; - _mat[3][3] = a33; -} - -Matrix& Matrix::operator = (const Matrix& other ) { - if( &other == this ) return *this; - set((const float*)other._mat); - return *this; -} - -void Matrix::set( float const * const def ) { - memcpy( _mat, def, sizeof(_mat) ); - fully_realized = true; -} - -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) -{ - _mat[0][0] = a00; - _mat[0][1] = a01; - _mat[0][2] = a02; - _mat[0][3] = a03; - - _mat[1][0] = a10; - _mat[1][1] = a11; - _mat[1][2] = a12; - _mat[1][3] = a13; - - _mat[2][0] = a20; - _mat[2][1] = a21; - _mat[2][2] = a22; - _mat[2][3] = a23; - - _mat[3][0] = a30; - _mat[3][1] = a31; - _mat[3][2] = a32; - _mat[3][3] = a33; -} - -#define SET_ROW(row, v1, v2, v3, v4 ) \ - _mat[(row)][0] = (v1); \ - _mat[(row)][1] = (v2); \ - _mat[(row)][2] = (v3); \ - _mat[(row)][3] = (v4); - -void Matrix::makeIdent() { - 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 ) - - fully_realized = true; -} - -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 ) - - fully_realized = true; -} - -void Matrix::makeTrans( const Vec3& v ) { - makeTrans( v[0], v[1], v[2] ); -} - -void Matrix::makeTrans( 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 ) - - fully_realized = true; -} - -void Matrix::makeRot( const Vec3& from, const Vec3& to ) { - double d = from * to; // dot product == cos( angle between from & to ) - if( d < 0.9999 ) { - double angle = acos(d); -#ifdef ANGLES_IN_DEGREES - angle = RAD2DEG(angle); -#endif - Vec3 axis = to ^ from; //we know ((to) x (from)) is perpendicular to both - makeRot( angle, axis ); - } - else - makeIdent(); -} - -void Matrix::makeRot( float angle, const Vec3& axis ) -{ - makeRot( angle, axis.x(), axis.y(), axis.z() ); -} - -void Matrix::makeRot( float angle, float x, float y, float z ) { - float d = sqrt( x*x + y*y + z*z ); - if( d == 0 ) - return; - -#ifdef ANGLES_IN_DEGREES - angle = DEG2RAD(angle); -#endif - - float sin_half = sin( angle/2 ); - float cos_half = cos( angle/2 ); - - Quat q( sin_half * (x/d), - sin_half * (y/d), - sin_half * (z/d), - cos_half ); - makeRot( q ); -} - -void Matrix::makeRot( const Quat& q ) { - // taken from Shoemake/ACM SIGGRAPH 89 - Vec4 v = q.asVec4(); - - double xs = 2 * v.x(); //assume q is already normalized? assert? - double ys = 2 * v.y(); // if not, xs = 2 * v.x() / d, ys = 2 * v.y() / d - double zs = 2 * v.z(); // and zs = 2 * v.z() /d where d = v.length2() - - double xx = xs * v.x(); - double xy = ys * v.x(); - double xz = zs * v.x(); - double yy = ys * v.y(); - double yz = zs * v.y(); - double zz = zs * v.z(); - double wx = xs * v.w(); - double wy = ys * v.w(); - double wz = zs * v.w(); - - SET_ROW(0, 1.0-(yy+zz), xy + wz, xz - wy, 0.0 ) - SET_ROW(1, xy - wz, 1.0-(xx+zz),yz + wx, 0.0 ) - SET_ROW(2, xz + wz, yz - wx, 1.0-(xx+yy),0.0 ) - SET_ROW(3, 0.0, 0.0, 0.0, 1.0 ) - - fully_realized = true; -} - -void Matrix::makeRot( float yaw, float pitch, float roll) -{ -#ifdef ANGLES_IN_DEGREES - yaw = DEG2RAD(yaw); - pitch = DEG2RAD(pitch); - roll = DEG2RAD(roll); -#endif - - // lifted straight from SOLID library v1.01 Quaternion.h - // available from http://www.win.tue.nl/~gino/solid/ - // and also distributed under the LGPL - float cosYaw = cos(yaw / 2); - float sinYaw = sin(yaw / 2); - float cosPitch = cos(pitch / 2); - float sinPitch = sin(pitch / 2); - float cosRoll = cos(roll / 2); - float sinRoll = sin(roll / 2); - Quat q(sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw, - cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw, - cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw, - cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw); - makeRot( q ); -} - -#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]) - -void Matrix::mult( const Matrix& lhs, const Matrix& rhs ) { -// 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); - fully_realized = true; -} - -void Matrix::preMult( const Matrix& other ) { - float t1,t2,t3,t4; - if( !fully_realized ) { - //act as if this were an identity Matrix - set((const float*)other._mat); - return; - } - for(int col=0; col<4; ++col) { - t1 = INNER_PRODUCT( other, *this, col, 0 ); - t2 = INNER_PRODUCT( other, *this, col, 1 ); - t3 = INNER_PRODUCT( other, *this, col, 2 ); - t4 = INNER_PRODUCT( other, *this, col, 3 ); - _mat[col][0] = t1; - _mat[col][1] = t2; - _mat[col][2] = t3; - _mat[col][3] = t4; - } -} - -void Matrix::postMult( const Matrix& other ) { - float t[4]; - if( !fully_realized ) { - //act as if this were an identity Matrix - set((const float*)other._mat); - return; - } - for(int row=0; row<4; ++row) { - t[0] = INNER_PRODUCT( *this, other, 0, row ); - t[1] = INNER_PRODUCT( *this, other, 1, row ); - t[2] = INNER_PRODUCT( *this, other, 2, row ); - t[3] = INNER_PRODUCT( *this, other, 3, row ); - SET_ROW(row, t[0], t[1], t[2], t[3] ) - } -} - -#undef SET_ROW -#undef INNER_PRODUCT - -bool Matrix::invert( const Matrix& _m ) { - - if (&_m==this) - { - Matrix tm(_m); - return invert(tm); - } - if ( _m._mat[0][3] == 0.0 - && _m._mat[1][3] == 0.0 - && _m._mat[2][3] == 0.0 - && _m._mat[3][3] == 1.0 ) - { - return invertAffine( _m ); - } - - // code lifted from VR Juggler. - // not cleanly added, but seems to work. RO. - const float* a = reinterpret_cast(_m._mat); - float* b = reinterpret_cast(_mat); - - int n = 4; - int i, j, k; - int r[ 4], c[ 4], row[ 4], col[ 4]; - float m[ 4][ 4*2], pivot, max_m, tmp_m, fac; - - /* Initialization */ - for ( i = 0; i < n; i ++ ) - { - r[ i] = c[ i] = 0; - row[ i] = col[ i] = 0; - } - - /* Set working matrix */ - for ( i = 0; i < n; i++ ) - { - for ( j = 0; j < n; j++ ) - { - m[ i][ j] = a[ i * n + j]; - m[ i][ j + n] = ( i == j ) ? 1.0 : 0.0 ; - } - } - - /* Begin of loop */ - for ( k = 0; k < n; k++ ) - { - /* Choosing the pivot */ - for ( i = 0, max_m = 0; i < n; i++ ) - { - if ( row[ i] ) continue; - for ( j = 0; j < n; j++ ) - { - if ( col[ j] ) continue; - tmp_m = fabs( m[ i][j]); - if ( tmp_m > max_m) - { - max_m = tmp_m; - r[ k] = i; - c[ k] = j; - } - } - } - row[ r[k] ] = col[ c[k] ] = 1; - pivot = m[ r[ k] ][ c[ k] ]; - - if ( fabs( pivot) <= 1e-20) - { - notify(WARN) << "*** pivot = %f in mat_inv. ***\n"; - //exit( 0); - return false; - } - - /* Normalization */ - for ( j = 0; j < 2*n; j++ ) - { - if ( j == c[ k] ) - m[ r[ k]][ j] = 1.0; - else - m[ r[ k]][ j] /=pivot; - } - - /* Reduction */ - for ( i = 0; i < n; i++ ) - { - if ( i == r[ k] ) - continue; - - for ( j=0, fac = m[ i][ c[k]];j < 2*n; j++ ) - { - if ( j == c[ k] ) - m[ i][ j] =0.0; - else - m[ i][ j] -=fac * m[ r[k]][ j]; - } - } - } - - /* Assign invers to a matrix */ - for ( i = 0; i < n; i++ ) - for ( j = 0; j < n; j++ ) - row[ i] = ( c[ j] == i ) ? r[j] : row[ i]; - - for ( i = 0; i < n; i++ ) - for ( j = 0; j < n; j++ ) - b[ i * n + j] = m[ row[ i]][j + n]; - - return true; // It worked -} - -const double PRECISION_LIMIT = 1.0e-15; - -bool Matrix::invertAffine( const Matrix& _m ) { - // adapted from Graphics Gems II. - // - // This method treats the matrix as a block matrix and calculates - // the inverse of one submatrix, improving performance over something - // that inverts any non-singular matrix: - // -1 - // -1 [ A 0 ] -1 [ A 0 ] - // M = [ ] = [ -1 ] - // [ C 1 ] [-CA 1 ] - // - // returns true if _m is nonsingular, and (*this) contains its inverse - // otherwise returns false. (*this unchanged) - - // assert( this->isAffine())? - double det_1, pos, neg, temp; - - pos = neg = 0.0; - -#define ACCUMULATE \ - { \ - if(temp < 0.0) pos += temp; \ - else neg += temp; \ - } - - temp = _m._mat[0][0] * _m._mat[1][1] * _m._mat[2][2]; ACCUMULATE; - temp = _m._mat[0][1] * _m._mat[1][2] * _m._mat[2][0]; ACCUMULATE; - temp = _m._mat[0][2] * _m._mat[1][0] * _m._mat[2][1]; ACCUMULATE; - - temp = - _m._mat[0][2] * _m._mat[1][1] * _m._mat[2][0]; ACCUMULATE; - temp = - _m._mat[0][1] * _m._mat[1][0] * _m._mat[2][2]; ACCUMULATE; - temp = - _m._mat[0][0] * _m._mat[1][2] * _m._mat[2][1]; ACCUMULATE; - - det_1 = pos + neg; - - if( (det_1 == 0.0) || (abs(det_1/(pos-neg)) < PRECISION_LIMIT )) { - // _m has no inverse - notify(WARN) << "Matrix::invert(): Matrix has no inverse." << endl; - return false; - } - - // inverse is adj(A)/det(A) - det_1 = 1.0 / det_1; - - _mat[0][0] = (_m._mat[1][1] * _m._mat[2][2] - _m._mat[1][2] * _m._mat[2][1]) * det_1; - _mat[1][0] = (_m._mat[1][0] * _m._mat[2][2] - _m._mat[1][2] * _m._mat[2][0]) * det_1; - _mat[2][0] = (_m._mat[1][0] * _m._mat[2][1] - _m._mat[1][1] * _m._mat[2][0]) * det_1; - _mat[0][1] = (_m._mat[0][1] * _m._mat[2][2] - _m._mat[0][2] * _m._mat[2][1]) * det_1; - _mat[1][1] = (_m._mat[0][0] * _m._mat[2][2] - _m._mat[0][2] * _m._mat[2][0]) * det_1; - _mat[2][1] = (_m._mat[0][0] * _m._mat[2][1] - _m._mat[0][1] * _m._mat[2][0]) * det_1; - _mat[0][2] = (_m._mat[0][1] * _m._mat[1][2] - _m._mat[0][2] * _m._mat[1][1]) * det_1; - _mat[1][2] = (_m._mat[0][0] * _m._mat[1][2] - _m._mat[0][2] * _m._mat[1][0]) * det_1; - _mat[2][2] = (_m._mat[0][0] * _m._mat[1][1] - _m._mat[0][1] * _m._mat[1][0]) * det_1; - - // calculate -C * inv(A) - _mat[3][0] = -(_m._mat[3][0] * _mat[0][0] + _m._mat[3][1] * _mat[1][0] + _m._mat[3][2] * _mat[2][0] ); - _mat[3][1] = -(_m._mat[3][0] * _mat[0][1] + _m._mat[3][1] * _mat[1][1] + _m._mat[3][2] * _mat[2][1] ); - _mat[3][2] = -(_m._mat[3][0] * _mat[0][2] + _m._mat[3][1] * _mat[1][2] + _m._mat[3][2] * _mat[2][2] ); - - _mat[0][3] = 0.0; - _mat[1][3] = 0.0; - _mat[2][3] = 0.0; - _mat[3][3] = 1.0; - - fully_realized = true; - return true; -} - -//static utility methods -Matrix Matrix::scale(float sx, float sy, float sz) { - Matrix m; - m.makeScale(sx,sy,sz); - return m; -} -Matrix Matrix::scale(const Vec3& v ) { - return scale(v.x(), v.y(), v.z() ); -} -Matrix Matrix::trans(float tx, float ty, float tz) { - Matrix m; - m.makeTrans(tx,ty,tz); - return m; -} -Matrix Matrix::trans(const Vec3& v ) { - return trans(v.x(), v.y(), v.z() ); -} -Matrix Matrix::rotate( const Quat& q ) { - Matrix m; - m.makeRot( q ); - return m; -} -Matrix Matrix::rotate(float angle, float x, float y, float z ) { - Matrix m; - m.makeRot(angle,x,y,z); - return m; -} -Matrix Matrix::rotate(const Vec3& from, const Vec3& to ) { - Matrix m; - m.makeRot(from,to); - return m; -} - -//Deprecated methods -void Matrix::copy( const Matrix& other) { -#ifdef WARN_DEPRECATED - notify(NOTICE) << "Matrix::copy is deprecated. Use = instead."; -#endif - (*this) = other; -} -void Matrix::preScale( float sx, float sy, float sz, const Matrix& m ) { -#ifdef WARN_DEPRECATED - notify(NOTICE) << "Matrix::preScale is deprecated. Use result = (Matrix::scale * m) instead."; -#endif - (*this) = ( scale(sx,sy,sz) * m ); -} -void Matrix::postScale( const Matrix& m, float sx, float sy, float sz ) { -#ifdef WARN_DEPRECATED - notify(NOTICE) << "Matrix::postScale is deprecated. Use result = (m * Matrix::scale()) instead."; -#endif - (*this) = ( m * scale(sx,sy,sz) ); -} -void Matrix::preScale( float sx, float sy, float sz ) { -#ifdef WARN_DEPRECATED - notify(NOTICE) << "Matrix::preScale is deprecated. Use M.preMult( Matrix::scale ) instead."; -#endif - preMult( scale(sx,sy,sz) ); -} -void Matrix::postScale( float sx, float sy, float sz ) { -#ifdef WARN_DEPRECATED - notify(NOTICE) << "Matrix::postScale is deprecated. Use M.postMult( Matrix::scale ) instead."; -#endif - postMult( scale(sx,sy,sz) ); -} -void Matrix::preTrans( float tx, float ty, float tz, const Matrix& m ) { -#ifdef WARN_DEPRECATED - notify(NOTICE) << "Matrix::preTrans is deprecated. Use result = Matrix::trans * m instead."; -#endif - (*this) = trans(tx,ty,tz) * m; -} -void Matrix::postTrans( const Matrix& m, float tx, float ty, float tz ) { -#ifdef WARN_DEPRECATED - notify(NOTICE) << "Matrix::postTrans is deprecated. Use result = m * Matrix::trans instead."; -#endif - (*this) = m * trans(tx,ty,tz); -} -void Matrix::preTrans( float sx, float sy, float sz ) { -#ifdef WARN_DEPRECATED - notify(NOTICE) << "Matrix::preTrans is deprecated. Use result = Matrix::trans * m instead."; -#endif - preMult( trans(sx,sy,sz) ); -} -void Matrix::postTrans( float sx, float sy, float sz ) { -#ifdef WARN_DEPRECATED - notify(NOTICE) << "Matrix::postTrans is deprecated. Use result = m * Matrix::trans instead."; -#endif - postMult( trans(sx,sy,sz) ); -} -void Matrix::preRot( float deg, float x, float y, float z, const Matrix& m ) { -#ifdef WARN_DEPRECATED - notify(NOTICE) << "Matrix::preRot is deprecated. Use result = Matrix::rot * m instead."; -#endif - (*this) = rotate(deg,x,y,z) * m; -} -void Matrix::postRot( const Matrix& m, float deg, float x, float y, float z ) { -#ifdef WARN_DEPRECATED - notify(NOTICE) << "Matrix::postRot is deprecated. Use result = m * Matrix::rotate instead."; -#endif - (*this) = m * rotate(deg,x,y,z); -} -void Matrix::preRot( float deg, float x, float y, float z ) { -#ifdef WARN_DEPRECATED - notify(NOTICE) << "Matrix::preRot is deprecated. Use m.preMult( Matrix::rotate ) instead."; -#endif - preMult( rotate(deg,x,y,z) ); -} -void Matrix::postRot( float deg, float x, float y, float z ) { -#ifdef WARN_DEPRECATED - notify(NOTICE) << "Matrix::postRot is deprecated. Use m.postMult( Matrix::rotate ) instead."; -#endif - postMult( rotate(deg,x,y,z) ); -} diff --git a/src/osg/Matrix.cpp.old b/src/osg/Matrix.cpp.old deleted file mode 100644 index d48060776..000000000 --- a/src/osg/Matrix.cpp.old +++ /dev/null @@ -1,568 +0,0 @@ -#include -#include - -#include -#include -#include -#include - -#define square(x) ((x)*(x)) -#define DEG2RAD(x) ((x)*M_PI/180.0) -#define RAD2DEG(x) ((x)*180.0/M_PI) - -using namespace osg; - -typedef struct quaternion_ -{ - double x ; - double y ; - double z ; - double w ; -} quaternion ; - -/* C = a(row).b(row) */ - -#define matrix_inner_product( a, b, row, col, C ) \ - { \ - (C)[row][col] = (a)[row][0] * (b)[0][col] + \ - (a)[row][1] * (b)[1][col] + \ - (a)[row][2] * (b)[2][col] + \ - (a)[row][3] * (b)[3][col]; \ - } - -/* C = a.b */ - -#define matrix_mult( a, b, C ) \ - { \ - matrix_inner_product( a, b, 0, 0, C ); \ - matrix_inner_product( a, b, 0, 1, C ); \ - matrix_inner_product( a, b, 0, 2, C ); \ - matrix_inner_product( a, b, 0, 3, C ); \ - matrix_inner_product( a, b, 1, 0, C ); \ - matrix_inner_product( a, b, 1, 1, C ); \ - matrix_inner_product( a, b, 1, 2, C ); \ - matrix_inner_product( a, b, 1, 3, C ); \ - matrix_inner_product( a, b, 2, 0, C ); \ - matrix_inner_product( a, b, 2, 1, C ); \ - matrix_inner_product( a, b, 2, 2, C ); \ - matrix_inner_product( a, b, 2, 3, C ); \ - matrix_inner_product( a, b, 3, 0, C ); \ - matrix_inner_product( a, b, 3, 1, C ); \ - matrix_inner_product( a, b, 3, 2, C ); \ - matrix_inner_product( a, b, 3, 3, C ); \ - } - -static void quaternion_matrix( quaternion *q, double mat[4][4] ) -{ - /* copied from Shoemake/ACM SIGGRAPH 89 */ - double xs, ys, zs, wx, wy, wz, xx, xy, xz, yy, yz, zz ; - - xs = q->x + q->x; - ys = q->y + q->y; - zs = q->z + q->z; - - wx = q->w * xs ; wy = q->w * ys ; wz = q->w * zs ; - xx = q->x * xs ; xy = q->x * ys ; xz = q->x * zs ; - yy = q->y * ys ; yz = q->y * zs ; zz = q->z * zs ; - - mat[0][0] = 1.0 - ( yy + zz ) ; - mat[0][1] = xy - wz ; - mat[0][2] = xz + wy ; - mat[1][0] = xy + wz ; - mat[1][1] = 1.0 - ( xx + zz ) ; - mat[1][2] = yz - wx ; - mat[2][0] = xz - wy ; - mat[2][1] = yz + wx ; - mat[2][2] = 1.0 - ( xx + yy ) ; - - mat[0][3] = 0.0; - mat[1][3] = 0.0; - mat[2][3] = 0.0; - - mat[3][0] = 0.0; - mat[3][1] = 0.0; - mat[3][2] = 0.0; - mat[3][3] = 1.0; -} - - -Matrix::Matrix() -{ - makeIdent(); -} - - -Matrix::Matrix(const Matrix& matrix) : Object() -{ - memcpy(_mat,matrix._mat,sizeof(_mat)); -} - - -Matrix& Matrix::operator = (const Matrix& matrix) -{ - if (&matrix==this) return *this; - memcpy(_mat,matrix._mat,sizeof(_mat)); - return *this; -} - - -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) -{ - _mat[0][0] = a00; - _mat[0][1] = a01; - _mat[0][2] = a02; - _mat[0][3] = a03; - - _mat[1][0] = a10; - _mat[1][1] = a11; - _mat[1][2] = a12; - _mat[1][3] = a13; - - _mat[2][0] = a20; - _mat[2][1] = a21; - _mat[2][2] = a22; - _mat[2][3] = a23; - - _mat[3][0] = a30; - _mat[3][1] = a31; - _mat[3][2] = a32; - _mat[3][3] = a33; -} - - -Matrix::~Matrix() -{ -} - - -void Matrix::makeIdent() -{ - _mat[0][0] = 1.0f; - _mat[0][1] = 0.0f; - _mat[0][2] = 0.0f; - _mat[0][3] = 0.0f; - - _mat[1][0] = 0.0f; - _mat[1][1] = 1.0f; - _mat[1][2] = 0.0f; - _mat[1][3] = 0.0f; - - _mat[2][0] = 0.0f; - _mat[2][1] = 0.0f; - _mat[2][2] = 1.0f; - _mat[2][3] = 0.0f; - - _mat[3][0] = 0.0f; - _mat[3][1] = 0.0f; - _mat[3][2] = 0.0f; - _mat[3][3] = 1.0f; -} - -void Matrix::set(const float* m) -{ - _mat[0][0] = m[0]; - _mat[0][1] = m[1]; - _mat[0][2] = m[2]; - _mat[0][3] = m[3]; - - _mat[1][0] = m[4]; - _mat[1][1] = m[5]; - _mat[1][2] = m[6]; - _mat[1][3] = m[7]; - - _mat[2][0] = m[8]; - _mat[2][1] = m[9]; - _mat[2][2] = m[10]; - _mat[2][3] = m[11]; - - _mat[3][0] = m[12]; - _mat[3][1] = m[13]; - _mat[3][2] = m[14]; - _mat[3][3] = m[15]; -} - - -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) -{ - _mat[0][0] = a00; - _mat[0][1] = a01; - _mat[0][2] = a02; - _mat[0][3] = a03; - - _mat[1][0] = a10; - _mat[1][1] = a11; - _mat[1][2] = a12; - _mat[1][3] = a13; - - _mat[2][0] = a20; - _mat[2][1] = a21; - _mat[2][2] = a22; - _mat[2][3] = a23; - - _mat[3][0] = a30; - _mat[3][1] = a31; - _mat[3][2] = a32; - _mat[3][3] = a33; -} - -void Matrix::copy(const Matrix& matrix) -{ - memcpy(_mat,matrix._mat,sizeof(_mat)); -} - - -void Matrix::makeScale(float sx, float sy, float sz) -{ - makeIdent(); - _mat[0][0] = sx; - _mat[1][1] = sy; - _mat[2][2] = sz; -} - - -void Matrix::preScale( float sx, float sy, float sz, const Matrix& m ) -{ - Matrix transMat; - transMat.makeScale(sx, sy, sz); - mult(transMat,m); -} - - -void Matrix::postScale( const Matrix& m, float sx, float sy, float sz ) -{ - Matrix transMat; - transMat.makeScale(sx, sy, sz); - mult(m,transMat); -} - - -void Matrix::preScale( float sx, float sy, float sz ) -{ - Matrix transMat; - transMat.makeScale(sx, sy, sz); - preMult(transMat); -} - - -void Matrix::postScale( float sx, float sy, float sz ) -{ - Matrix transMat; - transMat.makeScale(sx, sy, sz); - postMult(transMat); -} - - -void Matrix::makeTrans( float tx, float ty, float tz ) -{ - makeIdent(); - _mat[3][0] = tx; - _mat[3][1] = ty; - _mat[3][2] = tz; -} - - -void Matrix::preTrans( float tx, float ty, float tz, const Matrix& m ) -{ - Matrix transMat; - transMat.makeTrans(tx, ty, tz); - mult(transMat,m); -} - - -void Matrix::postTrans( const Matrix& m, float tx, float ty, float tz ) -{ - Matrix transMat; - transMat.makeTrans(tx, ty, tz); - mult(m,transMat); -} - - -void Matrix::preTrans( float tx, float ty, float tz ) -{ - _mat[3][0] = (tx * _mat[0][0]) + (ty * _mat[1][0]) + (tz * _mat[2][0]) + _mat[3][0]; - _mat[3][1] = (tx * _mat[0][1]) + (ty * _mat[1][1]) + (tz * _mat[2][1]) + _mat[3][1]; - _mat[3][2] = (tx * _mat[0][2]) + (ty * _mat[1][2]) + (tz * _mat[2][2]) + _mat[3][2]; - _mat[3][3] = (tx * _mat[0][3]) + (ty * _mat[1][3]) + (tz * _mat[2][3]) + _mat[3][3]; -} - - -void Matrix::postTrans( float tx, float ty, float tz ) -{ - Matrix transMat; - transMat.makeTrans(tx, ty, tz); - postMult(transMat); -} - -void Matrix::makeRot( const Vec3& old_vec, const Vec3& new_vec ) -{ - /* dot product == cos(angle old_vec<>new_vec). */ - double d = new_vec * old_vec; - if ( d < 0.9999 ) - { - double angle = acos( d ); - Vec3 rot_axis = new_vec ^ old_vec; - makeRot( RAD2DEG(angle), - rot_axis.x(), rot_axis.y(), rot_axis.z() ); - } - else - makeIdent(); -} - -void Matrix::makeRot( float deg, float x, float y, float z ) -{ - double __mat[4][4]; - quaternion q; - float d = sqrtf( square(x) + square(y) + square(z) ); - - if( d == 0 ) - return; - - float sin_HalfAngle = sinf( DEG2RAD(deg/2) ); - float cos_HalfAngle = cosf( DEG2RAD(deg/2) ); - - q.x = sin_HalfAngle * (x/d); - q.y = sin_HalfAngle * (y/d); - q.z = sin_HalfAngle * (z/d); - q.w = cos_HalfAngle; - - quaternion_matrix( &q, __mat ); - - for(int i=0;i<4;++i) - { - for(int j=0;j<4;++j) - { - _mat[i][j]=__mat[i][j]; - } - } -} - - -void Matrix::preRot( float deg, float x, float y, float z, const Matrix& m ) -{ - Matrix rotMat; - rotMat.makeRot( deg, x, y, z ); - mult(rotMat,m); -} - - -void Matrix::postRot( const Matrix& m, float deg, float x, float y, float z ) -{ - Matrix rotMat; - rotMat.makeRot( deg, x, y, z ); - mult(m,rotMat); -} - - -void Matrix::preRot( float deg, float x, float y, float z ) -{ - quaternion q; - double __mat[4][4]; - float res_mat[4][4]; - - float d = sqrtf( square(x) + square(y) + square(z) ); - - if( d == 0 ) - return; - - float sin_HalfAngle = sinf( DEG2RAD(deg/2) ); - float cos_HalfAngle = cosf( DEG2RAD(deg/2) ); - - q.x = sin_HalfAngle * (x/d); - q.y = sin_HalfAngle * (y/d); - q.z = sin_HalfAngle * (z/d); - q.w = cos_HalfAngle; - - quaternion_matrix( &q, __mat ); - matrix_mult( __mat, _mat, res_mat ); - memcpy( _mat, res_mat, sizeof( _mat ) ); -} - - -void Matrix::postRot( float deg, float x, float y, float z ) -{ - quaternion q; - double __mat[4][4]; - float res_mat[4][4]; - - float d = sqrtf( square(x) + square(y) + square(z) ); - - if( d == 0 ) - return; - - float sin_HalfAngle = sinf( DEG2RAD(deg/2) ); - float cos_HalfAngle = cosf( DEG2RAD(deg/2) ); - - q.x = sin_HalfAngle * (x/d); - q.y = sin_HalfAngle * (y/d); - q.z = sin_HalfAngle * (z/d); - q.w = cos_HalfAngle; - - quaternion_matrix( &q, __mat ); - matrix_mult( _mat, __mat , res_mat ); - memcpy( _mat, res_mat, sizeof( _mat ) ); -} - - -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::preMult(const Matrix& m) -{ - Matrix tm; - matrix_mult( m._mat, _mat, tm._mat ); - *this = tm; -} - - -void Matrix::postMult(const Matrix& m) -{ - Matrix tm; - matrix_mult( _mat, m._mat, tm._mat ); - *this = tm; -} - - -void Matrix::mult(const Matrix& lhs,const Matrix& rhs) -{ - if (&lhs==this || &rhs==this) - { - osg::Matrix tm; - matrix_mult( lhs._mat, rhs._mat, tm._mat ); - *this = tm; - } - else - { - matrix_mult( lhs._mat, rhs._mat, _mat ); - } -} - - -Matrix Matrix::operator * (const Matrix& m) const -{ - Matrix tm; - matrix_mult( _mat,m._mat, tm._mat ); - return tm; -} - - -bool Matrix::invert(const Matrix& invm) -{ - if (&invm==this) { - Matrix tm(invm); - return invert(tm); - } - - // code lifted from VR Juggler. - // not cleanly added, but seems to work. RO. - - const float* a = reinterpret_cast(invm._mat); - float* b = reinterpret_cast(_mat); - - int n = 4; - int i, j, k; - int r[ 4], c[ 4], row[ 4], col[ 4]; - float m[ 4][ 4*2], pivot, max_m, tmp_m, fac; - - /* Initialization */ - for ( i = 0; i < n; i ++ ) - { - r[ i] = c[ i] = 0; - row[ i] = col[ i] = 0; - } - - /* Set working matrix */ - for ( i = 0; i < n; i++ ) - { - for ( j = 0; j < n; j++ ) - { - m[ i][ j] = a[ i * n + j]; - m[ i][ j + n] = ( i == j ) ? 1.0 : 0.0 ; - } - } - - /* Begin of loop */ - for ( k = 0; k < n; k++ ) - { - /* Choosing the pivot */ - for ( i = 0, max_m = 0; i < n; i++ ) - { - if ( row[ i] ) continue; - for ( j = 0; j < n; j++ ) - { - if ( col[ j] ) continue; - tmp_m = fabs( m[ i][j]); - if ( tmp_m > max_m) - { - max_m = tmp_m; - r[ k] = i; - c[ k] = j; - } - } - } - row[ r[k] ] = col[ c[k] ] = 1; - pivot = m[ r[ k] ][ c[ k] ]; - - if ( fabs( pivot) <= 1e-20) - { - notify(WARN) << "*** pivot = %f in mat_inv. ***\n"; - //exit( 0); - return false; - } - - /* Normalization */ - for ( j = 0; j < 2*n; j++ ) - { - if ( j == c[ k] ) - m[ r[ k]][ j] = 1.0; - else - m[ r[ k]][ j] /=pivot; - } - - /* Reduction */ - for ( i = 0; i < n; i++ ) - { - if ( i == r[ k] ) - continue; - - for ( j=0, fac = m[ i][ c[k]];j < 2*n; j++ ) - { - if ( j == c[ k] ) - m[ i][ j] =0.0; - else - m[ i][ j] -=fac * m[ r[k]][ j]; - } - } - } - - /* Assign invers to a matrix */ - for ( i = 0; i < n; i++ ) - for ( j = 0; j < n; j++ ) - row[ i] = ( c[ j] == i ) ? r[j] : row[ i]; - - for ( i = 0; i < n; i++ ) - for ( j = 0; j < n; j++ ) - b[ i * n + j] = m[ row[ i]][j + n]; - - return true; // It worked -}