OpenSceneGraph/include/osg/Matrixf

/* -*-c++-*- OpenSceneGraph - Copyright (C) 1998-2003 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/Vec3>
#include <osg/Vec4>
#include <osg/Quat>

#include <string.h>

#include <ostream>
#include <algorithm>

namespace osg {

class SG_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 ) { set(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 { return memcmp(_mat,m._mat,sizeof(_mat)); }

        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& rhs);

        void set(const Matrixd& rhs);

        inline void set(const Matrixf& rhs) { set(rhs.ptr()); }

        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 Vec3& );
        void makeScale( value_type, value_type, value_type );
        
        void makeTranslate( const Vec3& );
        void makeTranslate( value_type, value_type, value_type );
        
        void makeRotate( const Vec3& from, const Vec3& to );
        void makeRotate( value_type angle, const Vec3& 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 Vec3& axis1, 
                         value_type angle2, const Vec3& axis2,
                         value_type angle3, const Vec3& axis3);

        
        
        /** Set to a orthographic projection. See glOrtho for further details.*/
        void makeOrtho(double left, double right,
                       double bottom, double top,
                       double zNear, double zFar);

        /** Get the othorgraphic 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 setting of a perspective projection matrix.
          * Returns false if matrix is not an orthographic matrix, where parameter values are undefined.*/
        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 setting of a symetric perspective projection matrix.
          * Returns false if matrix is not a perspective matrix, where parameter values are undefined. 
          * Note, if matrix is not a symetric perspective matrix then the shear will be lost.
          * Asymetric metrices 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 to the position and orientation modelview matrix, using the same convention as gluLookAt. */
        void makeLookAt(const Vec3& eye,const Vec3& center,const Vec3& up);
        
        /** Get to the position and orientation of a modelview matrix, using the same convention as gluLookAt. */
        void getLookAt(Vec3& eye,Vec3& center,Vec3& up,value_type lookDistance=1.0f) const;

        /** invert the matrix rhs placing result in this matrix.*/
        bool invert( const Matrixf& rhs);

        /** full 4x4 matrix invert. */
        bool invert_4x4_orig( const Matrixf& );

        /** full 4x4 matrix invert. */
        bool invert_4x4_new( const Matrixf& );

        //basic utility functions to create new matrices
	inline static Matrixf identity( void );
        inline static Matrixf scale( const Vec3& sv);
        inline static Matrixf scale( value_type sx, value_type sy, value_type sz);
        inline static Matrixf translate( const Vec3& dv);
        inline static Matrixf translate( value_type x, value_type y, value_type z);
        inline static Matrixf rotate( const Vec3& from, const Vec3& 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 Vec3& axis);
        inline static Matrixf rotate( value_type angle1, const Vec3& axis1, 
                                     value_type angle2, const Vec3& axis2,
                                     value_type angle3, const Vec3& axis3);
        inline static Matrixf rotate( const Quat& quat);
        inline static Matrixf inverse( const Matrixf& matrix);
        
        /** Create a orthographic projection. 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 Vec3& eye,const Vec3& center,const Vec3& up);




        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;

        void setTrans( value_type tx, value_type ty, value_type tz );
	void setTrans( const Vec3& v );
        inline Vec3 getTrans() const { return Vec3(_mat[3][0],_mat[3][1],_mat[3][2]); } 
        
        inline Vec3 getScale() const { return Vec3(_mat[0][0],_mat[1][1],_mat[2][2]); }
        
    	/** apply apply an 3x3 transform of v*M[0..2,0..2]  */
    	inline static Vec3 transform3x3(const Vec3& v,const Matrixf& m);
    	/** apply apply an 3x3 transform of M[0..2,0..2]*v  */
    	inline static Vec3 transform3x3(const Matrixf& m,const Vec3& v);


        // basic Matrixf multiplication, our workhorse methods.
        void mult( const Matrixf&, const Matrixf& );
        void preMult( const Matrixf& );
        void postMult( const Matrixf& );

        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():Matrixf() {}
        RefMatrixf( const Matrixf& other) : Matrixf(other) {}
        RefMatrixf( const Matrixd& other) : Matrixf(other) {}
        RefMatrixf( const RefMatrixf& other) : Object(other), Matrixf(other) {}
        explicit RefMatrixf( Matrixf::value_type const * const def ):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):
            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 Vec3& 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 Vec3& 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 Vec3& axis )
{
    Matrixf m;
    m.makeRotate(angle,axis);
    return m;
}
inline Matrixf Matrixf::rotate( value_type angle1, const Vec3& axis1, 
                              value_type angle2, const Vec3& axis2,
                              value_type angle3, const Vec3& axis3)
{
    Matrixf m;
    m.makeRotate(angle1,axis1,angle2,axis2,angle3,axis3);
    return m;
}
inline Matrixf Matrixf::rotate(const Vec3& from, const Vec3& 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::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 Vec3& eye,const Vec3& center,const Vec3& up)
{
    Matrixf m;
    m.makeLookAt(eye,center,up);
    return m;
}


inline Vec3 Matrixf::postMult( const Vec3& 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 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 Matrixf::preMult( const Vec3& 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 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 Vec4 Matrixf::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 Matrixf::preMult( const Vec4& v ) const
{
    return Vec4( (_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 Vec3 Matrixf::transform3x3(const Vec3& v,const Matrixf& 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 Matrixf::transform3x3(const Matrixf& 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 Vec3 operator* (const Vec3& v, const Matrixf& m )
{
	return m.preMult(v);
}
inline Vec4 operator* (const Vec4& v, const Matrixf& m )
{
	return m.preMult(v);
}

inline Vec3 Matrixf::operator* (const Vec3& v) const
{
	return postMult(v);
}
inline Vec4 Matrixf::operator* (const Vec4& v) const
{
	return postMult(v);
}

inline std::ostream& operator<< (std::ostream& os, const Matrixf& m )
{
    os << "{"<<std::endl;
    for(int row=0; row<4; ++row) {
        os << "\t";
        for(int col=0; col<4; ++col)
            os << m(row,col) << " ";
        os << std::endl;
    }
    os << "}" << std::endl;
    return os;
}


} //namespace osg


#endif