*** empty log message ***
This commit is contained in:
Robert Osfield
parent
7ae58df42a
commit
9fd1706e3c
@ -1,225 +0,0 @@
|
||||
|
||||
#ifndef OSG_Matrix
|
||||
#define OSG_Matrix 1
|
||||
|
||||
#include <osg/Object>
|
||||
#include <osg/Vec3>
|
||||
#include <osg/Vec4>
|
||||
//#include <osg/Quat>
|
||||
|
||||
#ifdef OSG_USE_IO_DOT_H
|
||||
#include <iostream.h>
|
||||
#else
|
||||
#include <iostream>
|
||||
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
|
||||
@ -1,190 +0,0 @@
|
||||
#ifndef OSG_MATRIX
|
||||
#define OSG_MATRIX 1
|
||||
|
||||
#include <osg/Object>
|
||||
#include <osg/Vec3>
|
||||
#include <osg/Vec4>
|
||||
|
||||
#ifdef OSG_USE_IO_DOT_H
|
||||
#include <iostream.h>
|
||||
#else
|
||||
#include <iostream>
|
||||
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<const Matrix*>(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 << "{"<<endl
|
||||
<< " " << matrix._mat[0][0] << " " << matrix._mat[0][1] << " " << matrix._mat[0][2] << " " << matrix._mat[0][3] << endl
|
||||
<< " " << matrix._mat[1][0] << " " << matrix._mat[1][1] << " " << matrix._mat[1][2] << " " << matrix._mat[1][3] << endl
|
||||
<< " " << matrix._mat[2][0] << " " << matrix._mat[2][1] << " " << matrix._mat[2][2] << " " << matrix._mat[2][3] << endl
|
||||
<< " " << matrix._mat[3][0] << " " << matrix._mat[3][1] << " " << matrix._mat[3][2] << " " << matrix._mat[3][3] << endl
|
||||
<< "}" << endl;
|
||||
return output; // to enable cascading
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
#endif
|
||||
@ -1,577 +0,0 @@
|
||||
|
||||
#include <osg/Matrix>
|
||||
#include <osg/Quat>
|
||||
#include <osg/Notify>
|
||||
|
||||
#include <cstdlib> //memcpy
|
||||
#include <cmath> //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<const float*>(_m._mat);
|
||||
float* b = reinterpret_cast<float*>(_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) );
|
||||
}
|
||||
@ -1,568 +0,0 @@
|
||||
#include <math.h>
|
||||
#include <string.h>
|
||||
|
||||
#include <osg/Types>
|
||||
#include <osg/Matrix>
|
||||
#include <osg/Notify>
|
||||
#include <osg/ref_ptr>
|
||||
|
||||
#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<const float*>(invm._mat);
|
||||
float* b = reinterpret_cast<float*>(_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
|
||||
}
|
||||
Loading…
Reference in New Issue
Block a user