a799cdca2f
matrixd. It was returning the values of the diagonal of the matrix, which only returns the scale if there is not a rotation. I fixed this by returning the length of the vectors that form the basis. I also added a function to orthonormalize the rotation component of the matrix. I seem to always run into situations where non uniform (or even uniform) scale complicate my calculations, and I thought other members of the community could use this function as well."
662 lines
24 KiB
C++
662 lines
24 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_MATRIXD
|
|
#define OSG_MATRIXD 1
|
|
|
|
#include <osg/Object>
|
|
#include <osg/Vec3d>
|
|
#include <osg/Vec4d>
|
|
#include <osg/Quat>
|
|
|
|
namespace osg {
|
|
|
|
class Matrixf;
|
|
|
|
class OSG_EXPORT Matrixd
|
|
{
|
|
public:
|
|
|
|
typedef double value_type;
|
|
|
|
inline Matrixd() { makeIdentity(); }
|
|
inline Matrixd( const Matrixd& mat) { set(mat.ptr()); }
|
|
Matrixd( const Matrixf& mat );
|
|
inline explicit Matrixd( float const * const ptr ) { set(ptr); }
|
|
inline explicit Matrixd( double const * const ptr ) { set(ptr); }
|
|
inline explicit Matrixd( const Quat& quat ) { set(quat); }
|
|
|
|
Matrixd(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);
|
|
|
|
~Matrixd() {}
|
|
|
|
int compare(const Matrixd& m) const;
|
|
|
|
bool operator < (const Matrixd& m) const { return compare(m)<0; }
|
|
bool operator == (const Matrixd& m) const { return compare(m)==0; }
|
|
bool operator != (const Matrixd& 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 Matrixd& operator = (const Matrixd& rhs)
|
|
{
|
|
if( &rhs == this ) return *this;
|
|
set(rhs.ptr());
|
|
return *this;
|
|
}
|
|
|
|
Matrixd& operator = (const Matrixf& other);
|
|
|
|
inline void set(const Matrixd& rhs) { set(rhs.ptr()); }
|
|
|
|
void set(const Matrixf& 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);
|
|
|
|
void set(const Quat& q);
|
|
|
|
void get(Quat& q) const;
|
|
|
|
value_type * ptr() { return (value_type*)_mat; }
|
|
const value_type * ptr() const { return (const value_type *)_mat; }
|
|
|
|
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);
|
|
|
|
|
|
|
|
/** 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 othogrraphic 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. */
|
|
bool invert( const Matrixd& rhs);
|
|
|
|
/** full 4x4 matrix invert. */
|
|
bool invert_4x4_orig( const Matrixd& );
|
|
|
|
/** full 4x4 matrix invert. */
|
|
bool invert_4x4_new( const Matrixd& );
|
|
|
|
/** ortho-normalize the 3x3 rotation & scale matrix */
|
|
void orthoNormalize(const Matrixd& rhs);
|
|
|
|
// basic utility functions to create new matrices
|
|
inline static Matrixd identity( void );
|
|
inline static Matrixd scale( const Vec3f& sv);
|
|
inline static Matrixd scale( const Vec3d& sv);
|
|
inline static Matrixd scale( value_type sx, value_type sy, value_type sz);
|
|
inline static Matrixd translate( const Vec3f& dv);
|
|
inline static Matrixd translate( const Vec3d& dv);
|
|
inline static Matrixd translate( value_type x, value_type y, value_type z);
|
|
inline static Matrixd rotate( const Vec3f& from, const Vec3f& to);
|
|
inline static Matrixd rotate( const Vec3d& from, const Vec3d& to);
|
|
inline static Matrixd rotate( value_type angle, value_type x, value_type y, value_type z);
|
|
inline static Matrixd rotate( value_type angle, const Vec3f& axis);
|
|
inline static Matrixd rotate( value_type angle, const Vec3d& axis);
|
|
inline static Matrixd rotate( value_type angle1, const Vec3f& axis1,
|
|
value_type angle2, const Vec3f& axis2,
|
|
value_type angle3, const Vec3f& axis3);
|
|
inline static Matrixd rotate( value_type angle1, const Vec3d& axis1,
|
|
value_type angle2, const Vec3d& axis2,
|
|
value_type angle3, const Vec3d& axis3);
|
|
inline static Matrixd rotate( const Quat& quat);
|
|
inline static Matrixd inverse( const Matrixd& matrix);
|
|
inline static Matrixd orthoNormal(const Matrixd& matrix);
|
|
/** Create an orthographic projection matrix.
|
|
* See glOrtho for further details.
|
|
*/
|
|
inline static Matrixd 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 Matrixd ortho2D(double left, double right,
|
|
double bottom, double top);
|
|
|
|
/** Create a perspective projection.
|
|
* See glFrustum for further details.
|
|
*/
|
|
inline static Matrixd 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 Matrixd 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 Matrixd 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 Matrixd 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;
|
|
|
|
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 Matrixd& m);
|
|
|
|
/** apply a 3x3 transform of v*M[0..2,0..2]. */
|
|
inline static Vec3d transform3x3(const Vec3d& v,const Matrixd& m);
|
|
|
|
/** apply a 3x3 transform of M[0..2,0..2]*v. */
|
|
inline static Vec3f transform3x3(const Matrixd& m,const Vec3f& v);
|
|
|
|
/** apply a 3x3 transform of M[0..2,0..2]*v. */
|
|
inline static Vec3d transform3x3(const Matrixd& m,const Vec3d& v);
|
|
|
|
// basic Matrixd multiplication, our workhorse methods.
|
|
void mult( const Matrixd&, const Matrixd& );
|
|
void preMult( const Matrixd& );
|
|
void postMult( const Matrixd& );
|
|
|
|
inline void operator *= ( const Matrixd& other )
|
|
{ if( this == &other ) {
|
|
Matrixd temp(other);
|
|
postMult( temp );
|
|
}
|
|
else postMult( other );
|
|
}
|
|
|
|
inline Matrixd operator * ( const Matrixd &m ) const
|
|
{
|
|
osg::Matrixd r;
|
|
r.mult(*this,m);
|
|
return r;
|
|
}
|
|
|
|
protected:
|
|
value_type _mat[4][4];
|
|
|
|
};
|
|
|
|
class RefMatrixd : public Object, public Matrixd
|
|
{
|
|
public:
|
|
|
|
RefMatrixd():Matrixd() {}
|
|
RefMatrixd( const Matrixd& other) : Matrixd(other) {}
|
|
RefMatrixd( const Matrixf& other) : Matrixd(other) {}
|
|
RefMatrixd( const RefMatrixd& other) : Object(other), Matrixd(other) {}
|
|
explicit RefMatrixd( Matrixd::value_type const * const def ):Matrixd(def) {}
|
|
RefMatrixd( Matrixd::value_type a00, Matrixd::value_type a01, Matrixd::value_type a02, Matrixd::value_type a03,
|
|
Matrixd::value_type a10, Matrixd::value_type a11, Matrixd::value_type a12, Matrixd::value_type a13,
|
|
Matrixd::value_type a20, Matrixd::value_type a21, Matrixd::value_type a22, Matrixd::value_type a23,
|
|
Matrixd::value_type a30, Matrixd::value_type a31, Matrixd::value_type a32, Matrixd::value_type a33):
|
|
Matrixd(a00, a01, a02, a03,
|
|
a10, a11, a12, a13,
|
|
a20, a21, a22, a23,
|
|
a30, a31, a32, a33) {}
|
|
|
|
virtual Object* cloneType() const { return new RefMatrixd(); }
|
|
virtual Object* clone(const CopyOp&) const { return new RefMatrixd(*this); }
|
|
virtual bool isSameKindAs(const Object* obj) const { return dynamic_cast<const RefMatrixd*>(obj)!=NULL; }
|
|
virtual const char* libraryName() const { return "osg"; }
|
|
virtual const char* className() const { return "Matrix"; }
|
|
|
|
|
|
protected:
|
|
|
|
virtual ~RefMatrixd() {}
|
|
};
|
|
|
|
|
|
// static utility methods
|
|
inline Matrixd Matrixd::identity(void)
|
|
{
|
|
Matrixd m;
|
|
m.makeIdentity();
|
|
return m;
|
|
}
|
|
|
|
inline Matrixd Matrixd::scale(value_type sx, value_type sy, value_type sz)
|
|
{
|
|
Matrixd m;
|
|
m.makeScale(sx,sy,sz);
|
|
return m;
|
|
}
|
|
|
|
inline Matrixd Matrixd::scale(const Vec3f& v )
|
|
{
|
|
return scale(v.x(), v.y(), v.z() );
|
|
}
|
|
|
|
inline Matrixd Matrixd::scale(const Vec3d& v )
|
|
{
|
|
return scale(v.x(), v.y(), v.z() );
|
|
}
|
|
|
|
inline Matrixd Matrixd::translate(value_type tx, value_type ty, value_type tz)
|
|
{
|
|
Matrixd m;
|
|
m.makeTranslate(tx,ty,tz);
|
|
return m;
|
|
}
|
|
|
|
inline Matrixd Matrixd::translate(const Vec3f& v )
|
|
{
|
|
return translate(v.x(), v.y(), v.z() );
|
|
}
|
|
|
|
inline Matrixd Matrixd::translate(const Vec3d& v )
|
|
{
|
|
return translate(v.x(), v.y(), v.z() );
|
|
}
|
|
|
|
inline Matrixd Matrixd::rotate( const Quat& q )
|
|
{
|
|
return Matrixd(q);
|
|
}
|
|
inline Matrixd Matrixd::rotate(value_type angle, value_type x, value_type y, value_type z )
|
|
{
|
|
Matrixd m;
|
|
m.makeRotate(angle,x,y,z);
|
|
return m;
|
|
}
|
|
inline Matrixd Matrixd::rotate(value_type angle, const Vec3f& axis )
|
|
{
|
|
Matrixd m;
|
|
m.makeRotate(angle,axis);
|
|
return m;
|
|
}
|
|
inline Matrixd Matrixd::rotate(value_type angle, const Vec3d& axis )
|
|
{
|
|
Matrixd m;
|
|
m.makeRotate(angle,axis);
|
|
return m;
|
|
}
|
|
inline Matrixd Matrixd::rotate( value_type angle1, const Vec3f& axis1,
|
|
value_type angle2, const Vec3f& axis2,
|
|
value_type angle3, const Vec3f& axis3)
|
|
{
|
|
Matrixd m;
|
|
m.makeRotate(angle1,axis1,angle2,axis2,angle3,axis3);
|
|
return m;
|
|
}
|
|
inline Matrixd Matrixd::rotate( value_type angle1, const Vec3d& axis1,
|
|
value_type angle2, const Vec3d& axis2,
|
|
value_type angle3, const Vec3d& axis3)
|
|
{
|
|
Matrixd m;
|
|
m.makeRotate(angle1,axis1,angle2,axis2,angle3,axis3);
|
|
return m;
|
|
}
|
|
inline Matrixd Matrixd::rotate(const Vec3f& from, const Vec3f& to )
|
|
{
|
|
Matrixd m;
|
|
m.makeRotate(from,to);
|
|
return m;
|
|
}
|
|
inline Matrixd Matrixd::rotate(const Vec3d& from, const Vec3d& to )
|
|
{
|
|
Matrixd m;
|
|
m.makeRotate(from,to);
|
|
return m;
|
|
}
|
|
|
|
inline Matrixd Matrixd::inverse( const Matrixd& matrix)
|
|
{
|
|
Matrixd m;
|
|
m.invert(matrix);
|
|
return m;
|
|
}
|
|
|
|
inline Matrixd orthoNormal(const Matrixd& matrix)
|
|
{
|
|
Matrixd m;
|
|
m.orthoNormalize(matrix);
|
|
return m;
|
|
}
|
|
|
|
inline Matrixd Matrixd::ortho(double left, double right,
|
|
double bottom, double top,
|
|
double zNear, double zFar)
|
|
{
|
|
Matrixd m;
|
|
m.makeOrtho(left,right,bottom,top,zNear,zFar);
|
|
return m;
|
|
}
|
|
|
|
inline Matrixd Matrixd::ortho2D(double left, double right,
|
|
double bottom, double top)
|
|
{
|
|
Matrixd m;
|
|
m.makeOrtho2D(left,right,bottom,top);
|
|
return m;
|
|
}
|
|
|
|
inline Matrixd Matrixd::frustum(double left, double right,
|
|
double bottom, double top,
|
|
double zNear, double zFar)
|
|
{
|
|
Matrixd m;
|
|
m.makeFrustum(left,right,bottom,top,zNear,zFar);
|
|
return m;
|
|
}
|
|
|
|
inline Matrixd Matrixd::perspective(double fovy, double aspectRatio,
|
|
double zNear, double zFar)
|
|
{
|
|
Matrixd m;
|
|
m.makePerspective(fovy,aspectRatio,zNear,zFar);
|
|
return m;
|
|
}
|
|
|
|
inline Matrixd Matrixd::lookAt(const Vec3f& eye,
|
|
const Vec3f& center,
|
|
const Vec3f& up)
|
|
{
|
|
Matrixd m;
|
|
m.makeLookAt(eye,center,up);
|
|
return m;
|
|
}
|
|
|
|
inline Matrixd Matrixd::lookAt(const Vec3d& eye,
|
|
const Vec3d& center,
|
|
const Vec3d& up)
|
|
{
|
|
Matrixd m;
|
|
m.makeLookAt(eye,center,up);
|
|
return m;
|
|
}
|
|
|
|
inline Vec3f Matrixd::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 Matrixd::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 Matrixd::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 Matrixd::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 Matrixd::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 Matrixd::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 Matrixd::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 Matrixd::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 Matrixd::transform3x3(const Vec3f& v,const Matrixd& 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 Matrixd::transform3x3(const Vec3d& v,const Matrixd& 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 Matrixd::transform3x3(const Matrixd& 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 Matrixd::transform3x3(const Matrixd& 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 Vec3f operator* (const Vec3f& v, const Matrixd& m )
|
|
{
|
|
return m.preMult(v);
|
|
}
|
|
|
|
inline Vec3d operator* (const Vec3d& v, const Matrixd& m )
|
|
{
|
|
return m.preMult(v);
|
|
}
|
|
|
|
inline Vec4f operator* (const Vec4f& v, const Matrixd& m )
|
|
{
|
|
return m.preMult(v);
|
|
}
|
|
|
|
inline Vec4d operator* (const Vec4d& v, const Matrixd& m )
|
|
{
|
|
return m.preMult(v);
|
|
}
|
|
|
|
inline Vec3f Matrixd::operator* (const Vec3f& v) const
|
|
{
|
|
return postMult(v);
|
|
}
|
|
|
|
inline Vec3d Matrixd::operator* (const Vec3d& v) const
|
|
{
|
|
return postMult(v);
|
|
}
|
|
|
|
inline Vec4f Matrixd::operator* (const Vec4f& v) const
|
|
{
|
|
return postMult(v);
|
|
}
|
|
|
|
inline Vec4d Matrixd::operator* (const Vec4d& v) const
|
|
{
|
|
return postMult(v);
|
|
}
|
|
|
|
|
|
} //namespace osg
|
|
|
|
|
|
#endif
|