22eae68e48
set. The optimization is based on the observation that matrix matrix multiplication with a dense matrix 4x4 is 4^3 Operations whereas multiplication with a transform, or scale matrix is only 4^2 operations. Which is a gain of a *FACTOR*4* for these special cases. The change implements these special cases, provides a unit test for these implementation and converts uses of the expensiver dense matrix matrix routine with the specialized versions. Depending on the transform nodes in the scenegraph this change gives a noticable improovement. For example the osgforest code using the MatrixTransform is about 20% slower than the same codepath using the PositionAttitudeTransform instead of the MatrixTransform with this patch applied. If I remember right, the sse type optimizations did *not* provide a factor 4 improovement. Also these changes are totally independent of any cpu or instruction set architecture. So I would prefer to have this current kind of change instead of some hand coded and cpu dependent assembly stuff. If we need that hand tuned stuff, these can go on top of this changes which must provide than hand optimized additional variants for the specialized versions to give a even better result in the end. An other change included here is a change to rotation matrix from quaterion code. There is a sqrt call which couold be optimized away. Since we divide in effect by sqrt(length)*sqrt(length) which is just length ... "
805 lines
30 KiB
C++
805 lines
30 KiB
C++
/* -*-c++-*- OpenSceneGraph - Copyright (C) 1998-2004 Robert Osfield
|
|
*
|
|
* This library is open source and may be redistributed and/or modified under
|
|
* the terms of the OpenSceneGraph Public License (OSGPL) version 0.0 or
|
|
* (at your option) any later version. The full license is in LICENSE file
|
|
* included with this distribution, and on the openscenegraph.org website.
|
|
*
|
|
* This library is distributed in the hope that it will be useful,
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
* OpenSceneGraph Public License for more details.
|
|
*/
|
|
|
|
#ifndef OSG_MATRIXF
|
|
#define OSG_MATRIXF 1
|
|
|
|
#include <osg/Object>
|
|
#include <osg/Vec3d>
|
|
#include <osg/Vec4d>
|
|
#include <osg/Quat>
|
|
|
|
namespace osg {
|
|
|
|
class Matrixf;
|
|
|
|
class OSG_EXPORT Matrixf
|
|
{
|
|
public:
|
|
|
|
typedef float value_type;
|
|
|
|
inline Matrixf() { makeIdentity(); }
|
|
inline Matrixf( const Matrixf& mat) { set(mat.ptr()); }
|
|
Matrixf( const Matrixd& mat );
|
|
inline explicit Matrixf( float const * const ptr ) { set(ptr); }
|
|
inline explicit Matrixf( double const * const ptr ) { set(ptr); }
|
|
inline explicit Matrixf( const Quat& quat ) { makeRotate(quat); }
|
|
|
|
Matrixf( value_type a00, value_type a01, value_type a02, value_type a03,
|
|
value_type a10, value_type a11, value_type a12, value_type a13,
|
|
value_type a20, value_type a21, value_type a22, value_type a23,
|
|
value_type a30, value_type a31, value_type a32, value_type a33);
|
|
|
|
~Matrixf() {}
|
|
|
|
int compare(const Matrixf& m) const;
|
|
|
|
bool operator < (const Matrixf& m) const { return compare(m)<0; }
|
|
bool operator == (const Matrixf& m) const { return compare(m)==0; }
|
|
bool operator != (const Matrixf& m) const { return compare(m)!=0; }
|
|
|
|
inline value_type& operator()(int row, int col) { return _mat[row][col]; }
|
|
inline value_type operator()(int row, int col) const { return _mat[row][col]; }
|
|
|
|
inline bool valid() const { return !isNaN(); }
|
|
inline bool isNaN() const { return osg::isNaN(_mat[0][0]) || osg::isNaN(_mat[0][1]) || osg::isNaN(_mat[0][2]) || osg::isNaN(_mat[0][3]) ||
|
|
osg::isNaN(_mat[1][0]) || osg::isNaN(_mat[1][1]) || osg::isNaN(_mat[1][2]) || osg::isNaN(_mat[1][3]) ||
|
|
osg::isNaN(_mat[2][0]) || osg::isNaN(_mat[2][1]) || osg::isNaN(_mat[2][2]) || osg::isNaN(_mat[2][3]) ||
|
|
osg::isNaN(_mat[3][0]) || osg::isNaN(_mat[3][1]) || osg::isNaN(_mat[3][2]) || osg::isNaN(_mat[3][3]); }
|
|
|
|
inline Matrixf& operator = (const Matrixf& rhs)
|
|
{
|
|
if( &rhs == this ) return *this;
|
|
set(rhs.ptr());
|
|
return *this;
|
|
}
|
|
|
|
Matrixf& operator = (const Matrixd& other);
|
|
|
|
inline void set(const Matrixf& rhs) { set(rhs.ptr()); }
|
|
|
|
void set(const Matrixd& rhs);
|
|
|
|
inline void set(float const * const ptr)
|
|
{
|
|
value_type* local_ptr = (value_type*)_mat;
|
|
for(int i=0;i<16;++i) local_ptr[i]=(value_type)ptr[i];
|
|
}
|
|
|
|
inline void set(double const * const ptr)
|
|
{
|
|
value_type* local_ptr = (value_type*)_mat;
|
|
for(int i=0;i<16;++i) local_ptr[i]=(value_type)ptr[i];
|
|
}
|
|
|
|
void set(value_type a00, value_type a01, value_type a02,value_type a03,
|
|
value_type a10, value_type a11, value_type a12,value_type a13,
|
|
value_type a20, value_type a21, value_type a22,value_type a23,
|
|
value_type a30, value_type a31, value_type a32,value_type a33);
|
|
|
|
value_type * ptr() { return (value_type*)_mat; }
|
|
const value_type * ptr() const { return (const value_type *)_mat; }
|
|
|
|
bool isIdentity() const
|
|
{
|
|
return _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 makeIdentity();
|
|
|
|
void makeScale( const Vec3f& );
|
|
void makeScale( const Vec3d& );
|
|
void makeScale( value_type, value_type, value_type );
|
|
|
|
void makeTranslate( const Vec3f& );
|
|
void makeTranslate( const Vec3d& );
|
|
void makeTranslate( value_type, value_type, value_type );
|
|
|
|
void makeRotate( const Vec3f& from, const Vec3f& to );
|
|
void makeRotate( const Vec3d& from, const Vec3d& to );
|
|
void makeRotate( value_type angle, const Vec3f& axis );
|
|
void makeRotate( value_type angle, const Vec3d& axis );
|
|
void makeRotate( value_type angle, value_type x, value_type y, value_type z );
|
|
void makeRotate( const Quat& );
|
|
void makeRotate( value_type angle1, const Vec3f& axis1,
|
|
value_type angle2, const Vec3f& axis2,
|
|
value_type angle3, const Vec3f& axis3);
|
|
void makeRotate( value_type angle1, const Vec3d& axis1,
|
|
value_type angle2, const Vec3d& axis2,
|
|
value_type angle3, const Vec3d& axis3);
|
|
|
|
|
|
/** decompose the matrix into translation, rotation, scale and scale orientation.*/
|
|
void decompose( osg::Vec3f& translation,
|
|
osg::Quat& rotation,
|
|
osg::Vec3f& scale,
|
|
osg::Quat& so ) const;
|
|
|
|
/** decompose the matrix into translation, rotation, scale and scale orientation.*/
|
|
void decompose( osg::Vec3d& translation,
|
|
osg::Quat& rotation,
|
|
osg::Vec3d& scale,
|
|
osg::Quat& so ) const;
|
|
|
|
|
|
/** Set to an orthographic projection.
|
|
* See glOrtho for further details.
|
|
*/
|
|
void makeOrtho(double left, double right,
|
|
double bottom, double top,
|
|
double zNear, double zFar);
|
|
|
|
/** Get the orthographic settings of the orthographic projection matrix.
|
|
* Note, if matrix is not an orthographic matrix then invalid values
|
|
* will be returned.
|
|
*/
|
|
bool getOrtho(double& left, double& right,
|
|
double& bottom, double& top,
|
|
double& zNear, double& zFar) const;
|
|
|
|
/** Set to a 2D orthographic projection.
|
|
* See glOrtho2D for further details.
|
|
*/
|
|
inline void makeOrtho2D(double left, double right,
|
|
double bottom, double top)
|
|
{
|
|
makeOrtho(left,right,bottom,top,-1.0,1.0);
|
|
}
|
|
|
|
|
|
/** Set to a perspective projection.
|
|
* See glFrustum for further details.
|
|
*/
|
|
void makeFrustum(double left, double right,
|
|
double bottom, double top,
|
|
double zNear, double zFar);
|
|
|
|
/** Get the frustum settings of a perspective projection matrix.
|
|
* Note, if matrix is not a perspective matrix then invalid values
|
|
* will be returned.
|
|
*/
|
|
bool getFrustum(double& left, double& right,
|
|
double& bottom, double& top,
|
|
double& zNear, double& zFar) const;
|
|
|
|
/** Set to a symmetrical perspective projection.
|
|
* See gluPerspective for further details.
|
|
* Aspect ratio is defined as width/height.
|
|
*/
|
|
void makePerspective(double fovy, double aspectRatio,
|
|
double zNear, double zFar);
|
|
|
|
/** Get the frustum settings of a symmetric perspective projection
|
|
* matrix.
|
|
* Return false if matrix is not a perspective matrix,
|
|
* where parameter values are undefined.
|
|
* Note, if matrix is not a symmetric perspective matrix then the
|
|
* shear will be lost.
|
|
* Asymmetric matrices occur when stereo, power walls, caves and
|
|
* reality center display are used.
|
|
* In these configuration one should use the AsFrustum method instead.
|
|
*/
|
|
bool getPerspective(double& fovy, double& aspectRatio,
|
|
double& zNear, double& zFar) const;
|
|
|
|
/** Set the position and orientation to be a view matrix,
|
|
* using the same convention as gluLookAt.
|
|
*/
|
|
void makeLookAt(const Vec3d& eye,const Vec3d& center,const Vec3d& up);
|
|
|
|
/** Get to the position and orientation of a modelview matrix,
|
|
* using the same convention as gluLookAt.
|
|
*/
|
|
void getLookAt(Vec3f& eye,Vec3f& center,Vec3f& up,
|
|
value_type lookDistance=1.0f) const;
|
|
|
|
/** Get to the position and orientation of a modelview matrix,
|
|
* using the same convention as gluLookAt.
|
|
*/
|
|
void getLookAt(Vec3d& eye,Vec3d& center,Vec3d& up,
|
|
value_type lookDistance=1.0f) const;
|
|
|
|
/** invert the matrix rhs, automatically select invert_4x3 or invert_4x4. */
|
|
inline bool invert( const Matrixf& rhs)
|
|
{
|
|
bool is_4x3 = (rhs._mat[0][3]==0.0f && rhs._mat[1][3]==0.0f && rhs._mat[2][3]==0.0f && rhs._mat[3][3]==1.0f);
|
|
return is_4x3 ? invert_4x3(rhs) : invert_4x4(rhs);
|
|
}
|
|
|
|
/** 4x3 matrix invert, not right hand column is assumed to be 0,0,0,1. */
|
|
bool invert_4x3( const Matrixf& rhs);
|
|
|
|
/** full 4x4 matrix invert. */
|
|
bool invert_4x4( const Matrixf& rhs);
|
|
|
|
/** ortho-normalize the 3x3 rotation & scale matrix */
|
|
void orthoNormalize(const Matrixf& rhs);
|
|
|
|
//basic utility functions to create new matrices
|
|
inline static Matrixf identity( void );
|
|
inline static Matrixf scale( const Vec3f& sv);
|
|
inline static Matrixf scale( const Vec3d& sv);
|
|
inline static Matrixf scale( value_type sx, value_type sy, value_type sz);
|
|
inline static Matrixf translate( const Vec3f& dv);
|
|
inline static Matrixf translate( const Vec3d& dv);
|
|
inline static Matrixf translate( value_type x, value_type y, value_type z);
|
|
inline static Matrixf rotate( const Vec3f& from, const Vec3f& to);
|
|
inline static Matrixf rotate( const Vec3d& from, const Vec3d& to);
|
|
inline static Matrixf rotate( value_type angle, value_type x, value_type y, value_type z);
|
|
inline static Matrixf rotate( value_type angle, const Vec3f& axis);
|
|
inline static Matrixf rotate( value_type angle, const Vec3d& axis);
|
|
inline static Matrixf rotate( value_type angle1, const Vec3f& axis1,
|
|
value_type angle2, const Vec3f& axis2,
|
|
value_type angle3, const Vec3f& axis3);
|
|
inline static Matrixf rotate( value_type angle1, const Vec3d& axis1,
|
|
value_type angle2, const Vec3d& axis2,
|
|
value_type angle3, const Vec3d& axis3);
|
|
inline static Matrixf rotate( const Quat& quat);
|
|
inline static Matrixf inverse( const Matrixf& matrix);
|
|
inline static Matrixf orthoNormal(const Matrixf& matrix);
|
|
|
|
/** Create an orthographic projection matrix.
|
|
* See glOrtho for further details.
|
|
*/
|
|
inline static Matrixf ortho(double left, double right,
|
|
double bottom, double top,
|
|
double zNear, double zFar);
|
|
|
|
/** Create a 2D orthographic projection.
|
|
* See glOrtho for further details.
|
|
*/
|
|
inline static Matrixf ortho2D(double left, double right,
|
|
double bottom, double top);
|
|
|
|
/** Create a perspective projection.
|
|
* See glFrustum for further details.
|
|
*/
|
|
inline static Matrixf frustum(double left, double right,
|
|
double bottom, double top,
|
|
double zNear, double zFar);
|
|
|
|
/** Create a symmetrical perspective projection.
|
|
* See gluPerspective for further details.
|
|
* Aspect ratio is defined as width/height.
|
|
*/
|
|
inline static Matrixf perspective(double fovy, double aspectRatio,
|
|
double zNear, double zFar);
|
|
|
|
/** Create the position and orientation as per a camera,
|
|
* using the same convention as gluLookAt.
|
|
*/
|
|
inline static Matrixf lookAt(const Vec3f& eye,
|
|
const Vec3f& center,
|
|
const Vec3f& up);
|
|
|
|
/** Create the position and orientation as per a camera,
|
|
* using the same convention as gluLookAt.
|
|
*/
|
|
inline static Matrixf lookAt(const Vec3d& eye,
|
|
const Vec3d& center,
|
|
const Vec3d& up);
|
|
|
|
inline Vec3f preMult( const Vec3f& v ) const;
|
|
inline Vec3d preMult( const Vec3d& v ) const;
|
|
inline Vec3f postMult( const Vec3f& v ) const;
|
|
inline Vec3d postMult( const Vec3d& v ) const;
|
|
inline Vec3f operator* ( const Vec3f& v ) const;
|
|
inline Vec3d operator* ( const Vec3d& v ) const;
|
|
inline Vec4f preMult( const Vec4f& v ) const;
|
|
inline Vec4d preMult( const Vec4d& v ) const;
|
|
inline Vec4f postMult( const Vec4f& v ) const;
|
|
inline Vec4d postMult( const Vec4d& v ) const;
|
|
inline Vec4f operator* ( const Vec4f& v ) const;
|
|
inline Vec4d operator* ( const Vec4d& v ) const;
|
|
|
|
#ifdef USE_DEPRECATED_API
|
|
inline void set(const Quat& q) { makeRotate(q); }
|
|
inline void get(Quat& q) const { q = getRotate(); }
|
|
#endif
|
|
|
|
void setRotate(const Quat& q);
|
|
/** Get the matrix rotation as a Quat. Note that this function
|
|
* assumes a non-scaled matrix and will return incorrect results
|
|
* for scaled matrixces. Consider decompose() instead.
|
|
*/
|
|
Quat getRotate() const;
|
|
|
|
|
|
void setTrans( value_type tx, value_type ty, value_type tz );
|
|
void setTrans( const Vec3f& v );
|
|
void setTrans( const Vec3d& v );
|
|
|
|
inline Vec3d getTrans() const { return Vec3d(_mat[3][0],_mat[3][1],_mat[3][2]); }
|
|
|
|
inline Vec3d getScale() const {
|
|
Vec3d x_vec(_mat[0][0],_mat[1][0],_mat[2][0]);
|
|
Vec3d y_vec(_mat[0][1],_mat[1][1],_mat[2][1]);
|
|
Vec3d z_vec(_mat[0][2],_mat[1][2],_mat[2][2]);
|
|
return Vec3d(x_vec.length(), y_vec.length(), z_vec.length());
|
|
}
|
|
|
|
/** apply a 3x3 transform of v*M[0..2,0..2]. */
|
|
inline static Vec3f transform3x3(const Vec3f& v,const Matrixf& m);
|
|
|
|
/** apply a 3x3 transform of v*M[0..2,0..2]. */
|
|
inline static Vec3d transform3x3(const Vec3d& v,const Matrixf& m);
|
|
|
|
/** apply a 3x3 transform of M[0..2,0..2]*v. */
|
|
inline static Vec3f transform3x3(const Matrixf& m,const Vec3f& v);
|
|
|
|
/** apply a 3x3 transform of M[0..2,0..2]*v. */
|
|
inline static Vec3d transform3x3(const Matrixf& m,const Vec3d& v);
|
|
|
|
// basic Matrixf multiplication, our workhorse methods.
|
|
void mult( const Matrixf&, const Matrixf& );
|
|
void preMult( const Matrixf& );
|
|
void postMult( const Matrixf& );
|
|
|
|
/** Optimized version of preMult(translate(v)); */
|
|
inline void preMultTranslate( const Vec3d& v );
|
|
inline void preMultTranslate( const Vec3f& v );
|
|
/** Optimized version of postMult(translate(v)); */
|
|
inline void postMultTranslate( const Vec3d& v );
|
|
inline void postMultTranslate( const Vec3f& v );
|
|
|
|
/** Optimized version of preMult(scale(v)); */
|
|
inline void preMultScale( const Vec3d& v );
|
|
inline void preMultScale( const Vec3f& v );
|
|
/** Optimized version of postMult(scale(v)); */
|
|
inline void postMultScale( const Vec3d& v );
|
|
inline void postMultScale( const Vec3f& v );
|
|
|
|
/** Optimized version of preMult(rotate(q)); */
|
|
inline void preMultRotate( const Quat& q );
|
|
/** Optimized version of postMult(rotate(q)); */
|
|
inline void postMultRotate( const Quat& q );
|
|
|
|
inline void operator *= ( const Matrixf& other )
|
|
{ if( this == &other ) {
|
|
Matrixf temp(other);
|
|
postMult( temp );
|
|
}
|
|
else postMult( other );
|
|
}
|
|
|
|
inline Matrixf operator * ( const Matrixf &m ) const
|
|
{
|
|
osg::Matrixf r;
|
|
r.mult(*this,m);
|
|
return r;
|
|
}
|
|
|
|
protected:
|
|
value_type _mat[4][4];
|
|
|
|
};
|
|
|
|
class RefMatrixf : public Object, public Matrixf
|
|
{
|
|
public:
|
|
|
|
RefMatrixf():Object(false), Matrixf() {}
|
|
RefMatrixf( const Matrixf& other) : Object(false), Matrixf(other) {}
|
|
RefMatrixf( const Matrixd& other) : Object(false), Matrixf(other) {}
|
|
RefMatrixf( const RefMatrixf& other) : Object(other), Matrixf(other) {}
|
|
explicit RefMatrixf( Matrixf::value_type const * const def ):Object(false), Matrixf(def) {}
|
|
RefMatrixf( Matrixf::value_type a00, Matrixf::value_type a01, Matrixf::value_type a02, Matrixf::value_type a03,
|
|
Matrixf::value_type a10, Matrixf::value_type a11, Matrixf::value_type a12, Matrixf::value_type a13,
|
|
Matrixf::value_type a20, Matrixf::value_type a21, Matrixf::value_type a22, Matrixf::value_type a23,
|
|
Matrixf::value_type a30, Matrixf::value_type a31, Matrixf::value_type a32, Matrixf::value_type a33):
|
|
Object(false),
|
|
Matrixf(a00, a01, a02, a03,
|
|
a10, a11, a12, a13,
|
|
a20, a21, a22, a23,
|
|
a30, a31, a32, a33) {}
|
|
|
|
virtual Object* cloneType() const { return new RefMatrixf(); }
|
|
virtual Object* clone(const CopyOp&) const { return new RefMatrixf(*this); }
|
|
virtual bool isSameKindAs(const Object* obj) const { return dynamic_cast<const RefMatrixf*>(obj)!=NULL; }
|
|
virtual const char* libraryName() const { return "osg"; }
|
|
virtual const char* className() const { return "Matrix"; }
|
|
|
|
|
|
protected:
|
|
|
|
virtual ~RefMatrixf() {}
|
|
};
|
|
|
|
|
|
//static utility methods
|
|
inline Matrixf Matrixf::identity(void)
|
|
{
|
|
Matrixf m;
|
|
m.makeIdentity();
|
|
return m;
|
|
}
|
|
|
|
inline Matrixf Matrixf::scale(value_type sx, value_type sy, value_type sz)
|
|
{
|
|
Matrixf m;
|
|
m.makeScale(sx,sy,sz);
|
|
return m;
|
|
}
|
|
|
|
inline Matrixf Matrixf::scale(const Vec3f& v )
|
|
{
|
|
return scale(v.x(), v.y(), v.z() );
|
|
}
|
|
|
|
inline Matrixf Matrixf::scale(const Vec3d& v )
|
|
{
|
|
return scale(v.x(), v.y(), v.z() );
|
|
}
|
|
|
|
inline Matrixf Matrixf::translate(value_type tx, value_type ty, value_type tz)
|
|
{
|
|
Matrixf m;
|
|
m.makeTranslate(tx,ty,tz);
|
|
return m;
|
|
}
|
|
|
|
inline Matrixf Matrixf::translate(const Vec3f& v )
|
|
{
|
|
return translate(v.x(), v.y(), v.z() );
|
|
}
|
|
|
|
inline Matrixf Matrixf::translate(const Vec3d& v )
|
|
{
|
|
return translate(v.x(), v.y(), v.z() );
|
|
}
|
|
|
|
inline Matrixf Matrixf::rotate( const Quat& q )
|
|
{
|
|
return Matrixf(q);
|
|
}
|
|
inline Matrixf Matrixf::rotate(value_type angle, value_type x, value_type y, value_type z )
|
|
{
|
|
Matrixf m;
|
|
m.makeRotate(angle,x,y,z);
|
|
return m;
|
|
}
|
|
inline Matrixf Matrixf::rotate(value_type angle, const Vec3f& axis )
|
|
{
|
|
Matrixf m;
|
|
m.makeRotate(angle,axis);
|
|
return m;
|
|
}
|
|
inline Matrixf Matrixf::rotate(value_type angle, const Vec3d& axis )
|
|
{
|
|
Matrixf m;
|
|
m.makeRotate(angle,axis);
|
|
return m;
|
|
}
|
|
inline Matrixf Matrixf::rotate( value_type angle1, const Vec3f& axis1,
|
|
value_type angle2, const Vec3f& axis2,
|
|
value_type angle3, const Vec3f& axis3)
|
|
{
|
|
Matrixf m;
|
|
m.makeRotate(angle1,axis1,angle2,axis2,angle3,axis3);
|
|
return m;
|
|
}
|
|
inline Matrixf Matrixf::rotate( value_type angle1, const Vec3d& axis1,
|
|
value_type angle2, const Vec3d& axis2,
|
|
value_type angle3, const Vec3d& axis3)
|
|
{
|
|
Matrixf m;
|
|
m.makeRotate(angle1,axis1,angle2,axis2,angle3,axis3);
|
|
return m;
|
|
}
|
|
inline Matrixf Matrixf::rotate(const Vec3f& from, const Vec3f& to )
|
|
{
|
|
Matrixf m;
|
|
m.makeRotate(from,to);
|
|
return m;
|
|
}
|
|
inline Matrixf Matrixf::rotate(const Vec3d& from, const Vec3d& to )
|
|
{
|
|
Matrixf m;
|
|
m.makeRotate(from,to);
|
|
return m;
|
|
}
|
|
|
|
inline Matrixf Matrixf::inverse( const Matrixf& matrix)
|
|
{
|
|
Matrixf m;
|
|
m.invert(matrix);
|
|
return m;
|
|
}
|
|
|
|
inline Matrixf Matrixf::orthoNormal(const Matrixf& matrix)
|
|
{
|
|
Matrixf m;
|
|
m.orthoNormalize(matrix);
|
|
return m;
|
|
}
|
|
|
|
inline Matrixf Matrixf::ortho(double left, double right,
|
|
double bottom, double top,
|
|
double zNear, double zFar)
|
|
{
|
|
Matrixf m;
|
|
m.makeOrtho(left,right,bottom,top,zNear,zFar);
|
|
return m;
|
|
}
|
|
|
|
inline Matrixf Matrixf::ortho2D(double left, double right,
|
|
double bottom, double top)
|
|
{
|
|
Matrixf m;
|
|
m.makeOrtho2D(left,right,bottom,top);
|
|
return m;
|
|
}
|
|
|
|
inline Matrixf Matrixf::frustum(double left, double right,
|
|
double bottom, double top,
|
|
double zNear, double zFar)
|
|
{
|
|
Matrixf m;
|
|
m.makeFrustum(left,right,bottom,top,zNear,zFar);
|
|
return m;
|
|
}
|
|
|
|
inline Matrixf Matrixf::perspective(double fovy,double aspectRatio,
|
|
double zNear, double zFar)
|
|
{
|
|
Matrixf m;
|
|
m.makePerspective(fovy,aspectRatio,zNear,zFar);
|
|
return m;
|
|
}
|
|
|
|
inline Matrixf Matrixf::lookAt(const Vec3f& eye,const Vec3f& center,const Vec3f& up)
|
|
{
|
|
Matrixf m;
|
|
m.makeLookAt(eye,center,up);
|
|
return m;
|
|
}
|
|
|
|
inline Matrixf Matrixf::lookAt(const Vec3d& eye,const Vec3d& center,const Vec3d& up)
|
|
{
|
|
Matrixf m;
|
|
m.makeLookAt(eye,center,up);
|
|
return m;
|
|
}
|
|
|
|
inline Vec3f Matrixf::postMult( const Vec3f& v ) const
|
|
{
|
|
value_type d = 1.0f/(_mat[3][0]*v.x()+_mat[3][1]*v.y()+_mat[3][2]*v.z()+_mat[3][3]) ;
|
|
return Vec3f( (_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 Vec3d Matrixf::postMult( const Vec3d& v ) const
|
|
{
|
|
value_type d = 1.0f/(_mat[3][0]*v.x()+_mat[3][1]*v.y()+_mat[3][2]*v.z()+_mat[3][3]) ;
|
|
return Vec3d( (_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 Vec3f Matrixf::preMult( const Vec3f& v ) const
|
|
{
|
|
value_type d = 1.0f/(_mat[0][3]*v.x()+_mat[1][3]*v.y()+_mat[2][3]*v.z()+_mat[3][3]) ;
|
|
return Vec3f( (_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 Vec3d Matrixf::preMult( const Vec3d& v ) const
|
|
{
|
|
value_type d = 1.0f/(_mat[0][3]*v.x()+_mat[1][3]*v.y()+_mat[2][3]*v.z()+_mat[3][3]) ;
|
|
return Vec3d( (_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 Vec4f Matrixf::postMult( const Vec4f& v ) const
|
|
{
|
|
return Vec4f( (_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 Vec4d Matrixf::postMult( const Vec4d& v ) const
|
|
{
|
|
return Vec4d( (_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 Vec4f Matrixf::preMult( const Vec4f& v ) const
|
|
{
|
|
return Vec4f( (_mat[0][0]*v.x() + _mat[1][0]*v.y() + _mat[2][0]*v.z() + _mat[3][0]*v.w()),
|
|
(_mat[0][1]*v.x() + _mat[1][1]*v.y() + _mat[2][1]*v.z() + _mat[3][1]*v.w()),
|
|
(_mat[0][2]*v.x() + _mat[1][2]*v.y() + _mat[2][2]*v.z() + _mat[3][2]*v.w()),
|
|
(_mat[0][3]*v.x() + _mat[1][3]*v.y() + _mat[2][3]*v.z() + _mat[3][3]*v.w()));
|
|
}
|
|
inline Vec4d Matrixf::preMult( const Vec4d& v ) const
|
|
{
|
|
return Vec4d( (_mat[0][0]*v.x() + _mat[1][0]*v.y() + _mat[2][0]*v.z() + _mat[3][0]*v.w()),
|
|
(_mat[0][1]*v.x() + _mat[1][1]*v.y() + _mat[2][1]*v.z() + _mat[3][1]*v.w()),
|
|
(_mat[0][2]*v.x() + _mat[1][2]*v.y() + _mat[2][2]*v.z() + _mat[3][2]*v.w()),
|
|
(_mat[0][3]*v.x() + _mat[1][3]*v.y() + _mat[2][3]*v.z() + _mat[3][3]*v.w()));
|
|
}
|
|
inline Vec3f Matrixf::transform3x3(const Vec3f& v,const Matrixf& m)
|
|
{
|
|
return Vec3f( (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 Vec3d Matrixf::transform3x3(const Vec3d& v,const Matrixf& m)
|
|
{
|
|
return Vec3d( (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 Vec3f Matrixf::transform3x3(const Matrixf& m,const Vec3f& v)
|
|
{
|
|
return Vec3f( (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 Vec3d Matrixf::transform3x3(const Matrixf& m,const Vec3d& v)
|
|
{
|
|
return Vec3d( (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 void Matrixf::preMultTranslate( const Vec3d& v )
|
|
{
|
|
for (unsigned i = 0; i < 3; ++i)
|
|
{
|
|
double tmp = v[i];
|
|
if (tmp == 0)
|
|
continue;
|
|
_mat[3][0] += tmp*_mat[i][0];
|
|
_mat[3][1] += tmp*_mat[i][1];
|
|
_mat[3][2] += tmp*_mat[i][2];
|
|
_mat[3][3] += tmp*_mat[i][3];
|
|
}
|
|
}
|
|
|
|
inline void Matrixf::preMultTranslate( const Vec3f& v )
|
|
{
|
|
for (unsigned i = 0; i < 3; ++i)
|
|
{
|
|
float tmp = v[i];
|
|
if (tmp == 0)
|
|
continue;
|
|
_mat[3][0] += tmp*_mat[i][0];
|
|
_mat[3][1] += tmp*_mat[i][1];
|
|
_mat[3][2] += tmp*_mat[i][2];
|
|
_mat[3][3] += tmp*_mat[i][3];
|
|
}
|
|
}
|
|
|
|
inline void Matrixf::postMultTranslate( const Vec3d& v )
|
|
{
|
|
for (unsigned i = 0; i < 3; ++i)
|
|
{
|
|
double tmp = v[i];
|
|
if (tmp == 0)
|
|
continue;
|
|
_mat[0][i] += tmp*_mat[0][3];
|
|
_mat[1][i] += tmp*_mat[1][3];
|
|
_mat[2][i] += tmp*_mat[2][3];
|
|
_mat[3][i] += tmp*_mat[3][3];
|
|
}
|
|
}
|
|
|
|
inline void Matrixf::postMultTranslate( const Vec3f& v )
|
|
{
|
|
for (unsigned i = 0; i < 3; ++i)
|
|
{
|
|
float tmp = v[i];
|
|
if (tmp == 0)
|
|
continue;
|
|
_mat[0][i] += tmp*_mat[0][3];
|
|
_mat[1][i] += tmp*_mat[1][3];
|
|
_mat[2][i] += tmp*_mat[2][3];
|
|
_mat[3][i] += tmp*_mat[3][3];
|
|
}
|
|
}
|
|
|
|
inline void Matrixf::preMultScale( const Vec3d& v )
|
|
{
|
|
_mat[0][0] *= v[0]; _mat[0][1] *= v[0]; _mat[0][2] *= v[0]; _mat[0][3] *= v[0];
|
|
_mat[1][0] *= v[1]; _mat[1][1] *= v[1]; _mat[1][2] *= v[1]; _mat[1][3] *= v[1];
|
|
_mat[2][0] *= v[2]; _mat[2][1] *= v[2]; _mat[2][2] *= v[2]; _mat[2][3] *= v[2];
|
|
}
|
|
|
|
inline void Matrixf::preMultScale( const Vec3f& v )
|
|
{
|
|
_mat[0][0] *= v[0]; _mat[0][1] *= v[0]; _mat[0][2] *= v[0]; _mat[0][3] *= v[0];
|
|
_mat[1][0] *= v[1]; _mat[1][1] *= v[1]; _mat[1][2] *= v[1]; _mat[1][3] *= v[1];
|
|
_mat[2][0] *= v[2]; _mat[2][1] *= v[2]; _mat[2][2] *= v[2]; _mat[2][3] *= v[2];
|
|
}
|
|
|
|
inline void Matrixf::postMultScale( const Vec3d& v )
|
|
{
|
|
_mat[0][0] *= v[0]; _mat[1][0] *= v[0]; _mat[2][0] *= v[0]; _mat[3][0] *= v[0];
|
|
_mat[0][1] *= v[1]; _mat[1][1] *= v[1]; _mat[2][1] *= v[1]; _mat[3][1] *= v[1];
|
|
_mat[0][2] *= v[2]; _mat[1][2] *= v[2]; _mat[2][2] *= v[2]; _mat[3][2] *= v[2];
|
|
}
|
|
|
|
inline void Matrixf::postMultScale( const Vec3f& v )
|
|
{
|
|
_mat[0][0] *= v[0]; _mat[1][0] *= v[0]; _mat[2][0] *= v[0]; _mat[3][0] *= v[0];
|
|
_mat[0][1] *= v[1]; _mat[1][1] *= v[1]; _mat[2][1] *= v[1]; _mat[3][1] *= v[1];
|
|
_mat[0][2] *= v[2]; _mat[1][2] *= v[2]; _mat[2][2] *= v[2]; _mat[3][2] *= v[2];
|
|
}
|
|
|
|
|
|
inline void Matrixf::preMultRotate( const Quat& q )
|
|
{
|
|
if (q.zeroRotation())
|
|
return;
|
|
Matrixf r;
|
|
r.setRotate(q);
|
|
preMult(r);
|
|
}
|
|
|
|
inline void Matrixf::postMultRotate( const Quat& q )
|
|
{
|
|
if (q.zeroRotation())
|
|
return;
|
|
Matrixf r;
|
|
r.setRotate(q);
|
|
postMult(r);
|
|
}
|
|
|
|
inline Vec3f operator* (const Vec3f& v, const Matrixf& m )
|
|
{
|
|
return m.preMult(v);
|
|
}
|
|
inline Vec3d operator* (const Vec3d& v, const Matrixf& m )
|
|
{
|
|
return m.preMult(v);
|
|
}
|
|
inline Vec4f operator* (const Vec4f& v, const Matrixf& m )
|
|
{
|
|
return m.preMult(v);
|
|
}
|
|
inline Vec4d operator* (const Vec4d& v, const Matrixf& m )
|
|
{
|
|
return m.preMult(v);
|
|
}
|
|
|
|
inline Vec3f Matrixf::operator* (const Vec3f& v) const
|
|
{
|
|
return postMult(v);
|
|
}
|
|
inline Vec3d Matrixf::operator* (const Vec3d& v) const
|
|
{
|
|
return postMult(v);
|
|
}
|
|
inline Vec4f Matrixf::operator* (const Vec4f& v) const
|
|
{
|
|
return postMult(v);
|
|
}
|
|
inline Vec4d Matrixf::operator* (const Vec4d& v) const
|
|
{
|
|
return postMult(v);
|
|
}
|
|
|
|
|
|
} //namespace osg
|
|
|
|
|
|
#endif
|