gzMatrix.h

Go to the documentation of this file.

//******************************************************************************
// File         : gzMatrix.h
// Module       : gzBase
// Description  : Class definition of Matrix utilites.
// Author       : Anders Modén      
// Product      : GizmoBase 2.1.1
//      
// Copyright © 2003- Saab Training Systems AB, Sweden
//          
// NOTE:    GizmoBase is a platform abstraction utility layer for C++. It contains 
//          design patterns and C++ solutions for the advanced programmer.
//
//
// Revision History...                          
//                                  
// Who  Date    Description                     
//                                  
// AMO  981104  Created file    
//
//******************************************************************************

#ifndef __GZ_MATRIX_H__
#define __GZ_MATRIX_H__
#include "gzBasicTypes.h"
#include "gzBase.h"

#include <math.h>

#define gzSqrt(x) sqrt(x)   // Default implementation

// ******************** class definitions ***************************************

//******************************************************************************
// Class    : -
//                                  
// Purpose  : - 
//                                  
// Notes    : - 
//                                  
// Revision History...                          
//                                  
// Who  Date    Description                     
//                                  
// AMO  981128  Created 
//                                  
//******************************************************************************
class  GZ_BASE_EXPORT gzVec2
{
public:
    gzVec2( ){};
    gzVec2( const gzReal &v1_val , const gzReal &v2_val):v1(v1_val),v2(v2_val){};

    gzVoid set(const gzReal &v1_val, const gzReal &v2_val){ v1=v1_val;v2=v2_val;}

    gzReal length() const
    {
        return (gzReal)gzSqrt(v1*v1+v2*v2);
    }

    
    gzReal magnitude() const
    {
        return v1*v1+v2*v2;
    }


    gzVec2 unit() const
    {
        gzReal len(length());
        if(len)
        {
            gzReal invlen(GZ_REAL_ONE/len);
            return gzVec2(v1*invlen,v2*invlen);
        }
        else 
            return gzVec2(0,0);
    }

    gzVec2 abs() const
    {
        return gzVec2(gzAbs(v1),gzAbs(v2));
    }

    
    gzVoid normalize()
    {
        gzReal len(length());
        if(!len)
            return;

        gzReal invlen(GZ_REAL_ONE/len);
        
        v1=v1*invlen;
        v2=v2*invlen;
    }

    gzBool operator==( const gzVec2 &right) const { return (v1==right.v1) && (v2==right.v2); }
    gzBool operator!=( const gzVec2 &right) const { return (v1!=right.v1) || (v2!=right.v2); }

    gzVec2 & operator+=(const gzVec2 &vector)
    {
        v1+=vector.v1;
        v2+=vector.v2;
        return *this;
    }

    gzVec2 & operator-=(const gzVec2 &vector)
    {
        v1+=vector.v1;
        v2+=vector.v2;
        return *this;
    }

    gzVec2 & operator *= (const gzReal &val)
    {
        v1*=val;
        v2*=val;
        return *this;
    }

    gzVec2  operator * (const gzReal &val) const
    {
        return gzVec2(v1*val,v2*val);
    }

    gzVec2  operator / (const gzReal &val) const
    {
        gzReal inv=GZ_REAL_ONE/val;

        return gzVec2(v1*inv,v2*inv);
    }
    
    gzVec2 operator - (const gzVec2 &vector_b) const
    {
        return gzVec2(v1-vector_b.v1,v2-vector_b.v2);
    }

    gzVec2  operator + (const gzVec2 &vector_b) const
    {
        return gzVec2(v1+vector_b.v1,v2+vector_b.v2);
    }

    gzULong         hash()  const;

public:

    union
    {
        struct
        {
            gzReal v1,v2;
        };
        struct
        {
            gzReal x,y;
        };
        struct
        {
            gzReal s,t;
        };
        struct
        {
            gzReal u,v;
        };
    };
};

inline gzVec2   operator * (const gzReal &val , const gzVec2 &vector)
{
    return gzVec2(val*vector.v1,val*vector.v2);
}

class gzVec4;   // forward decl

//******************************************************************************
// Class    : -
//                                  
// Purpose  : - 
//                                  
// Notes    : - 
//                                  
// Revision History...                          
//                                  
// Who  Date    Description                     
//                                  
// AMO  981104  Created 
//                                  
//******************************************************************************
class  GZ_BASE_EXPORT gzVec3
{
public:

    gzVec3(){};
    gzVec3(const gzReal &heading , const gzReal &pitch);
    gzVec3( const gzReal &v1_val, const gzReal &v2_val, const gzReal &v3_val):v1(v1_val),v2(v2_val),v3(v3_val){};
    gzVec3(const gzVec2 &right , const gzReal &v3_val=GZ_REAL_ONE):v1(right.v1),v2(right.v2),v3(v3_val){};

    gzVoid set(const gzReal &v1_val, const gzReal &v2_val, const gzReal &v3_val){ v1=v1_val;v2=v2_val;v3=v3_val;}

    gzReal dot(const gzVec3 &vector) const
    {
        return v1*vector.v1+v2*vector.v2+v3*vector.v3;
    }

    gzReal length() const
    {
        return (gzReal)gzSqrt(v1*v1+v2*v2+v3*v3);
    }

    
    gzReal magnitude() const
    {
        return v1*v1+v2*v2+v3*v3;
    }


    gzVec3 unit() const
    {
        gzReal len(length());
        if(len)
        {
            gzReal invlen(GZ_REAL_ONE/len);
            return gzVec3(v1*invlen,v2*invlen,v3*invlen);
        }
        else 
            return gzVec3(0,0,0);
    }

    gzVec3 abs() const
    {
        return gzVec3(gzAbs(v1),gzAbs(v2),gzAbs(v3));
    }

    gzVoid normalize()
    {
        gzReal len(length());
        if(!len)
            return;

        gzReal invlen(GZ_REAL_ONE/len);
        
        v1=v1*invlen;
        v2=v2*invlen;
        v3=v3*invlen;
    }


    gzBool operator==(const gzVec3 &vector) const GZ_NO_THROW { return (v1==vector.v1) && (v2==vector.v2) && (v3==vector.v3); }
    gzBool operator!=(const gzVec3 &vector) const GZ_NO_THROW { return (v1!=vector.v1) || (v2!=vector.v2) || (v3!=vector.v3); }

    gzVec3 operator-() GZ_NO_THROW 
    {
        return gzVec3(-v1,-v2,-v3);
    }

    gzVec3 & operator+=(const gzVec3 &vector) GZ_NO_THROW 
    {
        v1+=vector.v1;
        v2+=vector.v2;
        v3+=vector.v3;
        return *this;
    }

    gzVec3 & operator-=(const gzVec3 &vector) GZ_NO_THROW 
    {
        v1-=vector.v1;
        v2-=vector.v2;
        v3-=vector.v3;
        return *this;
    }

    gzVec3  operator * (const gzReal &val) const GZ_NO_THROW 
    {
        return gzVec3(v1*val,v2*val,v3*val);
    }

    gzVec3 & operator *= (const gzReal &val) GZ_NO_THROW 
    {
        v1*=val;
        v2*=val;
        v3*=val;
        return *this;
    }

    gzVec3  operator / (const gzReal &val) const GZ_NO_THROW 
    {
        gzReal inv=GZ_REAL_ONE/val;
        return gzVec3(v1*inv,v2*inv,v3*inv);
    }
    
    gzVec3 operator - (const gzVec3 &vector_b) const GZ_NO_THROW 
    {
        return gzVec3(v1-vector_b.v1,v2-vector_b.v2,v3-vector_b.v3);
    }

    gzVec3  operator + (const gzVec3 &vector_b) const GZ_NO_THROW 
    {
        return gzVec3(v1+vector_b.v1,v2+vector_b.v2,v3+vector_b.v3);
    }

    gzVec3 cross(const gzVec3 &vector) const GZ_NO_THROW 
    {
        return gzVec3(v2*vector.v3-v3*vector.v2,v3*vector.v1-v1*vector.v3,v1*vector.v2-v2*vector.v1);
    }


    gzVec2      project(const gzVec3 &normal) const;
    
    gzString    asString(const gzString &format="(%lf %lf %lf)") const ;

    gzULong     hash()  const;

public:

    union
    {
        struct
        {
            gzReal v1,v2,v3;
        };
        struct
        {
            gzReal x,y,z;
        };
        struct
        {
            gzReal r,g,b;
        };
        struct
        {
            gzReal heading,pitch,roll;
        };
    };
};



inline gzVec3   operator * (const gzReal &val , const gzVec3 &vector)
{
    return gzVec3(val*vector.v1,val*vector.v2,val*vector.v3);
}



//******************************************************************************
// Class    : -
//                                  
// Purpose  : - 
//                                  
// Notes    : - 
//                                  
// Revision History...                          
//                                  
// Who  Date    Description                     
//                                  
// AMO  981128  Created 
//                                  
//******************************************************************************
class  GZ_BASE_EXPORT gzVec4
{
public:

    gzVec4(){};
    gzVec4( const gzReal &v1_val , const gzReal &v2_val, const gzReal &v3_val, const gzReal &v4_val):v1(v1_val),v2(v2_val),v3(v3_val),v4(v4_val){};
    gzVec4(const gzVec3 &right , const gzReal &v4_val=GZ_REAL_ONE):v1(right.v1),v2(right.v2),v3(right.v3),v4(v4_val){};

    gzVoid set(const gzReal &v1_val, const gzReal &v2_val, const gzReal &v3_val , const gzReal &v4_val){ v1=v1_val;v2=v2_val;v3=v3_val;v4=v4_val;}

    gzBool operator==(const gzVec4 &vector) const { return (v1==vector.v1) && (v2==vector.v2) && (v3==vector.v3) && (v4==vector.v4); }
    gzBool operator!=(const gzVec4 &vector) const { return (v1!=vector.v1) || (v2!=vector.v2) || (v3!=vector.v3) || (v4!=vector.v4); }


    gzVec3 trunc()
    {
        return gzVec3(v1,v2,v3);
    }

    gzVec4 abs() const
    {
        return gzVec4(gzAbs(v1),gzAbs(v2),gzAbs(v3),gzAbs(v4));
    }


    gzVec4 & operator+=(const gzVec4 &vector)
    {
        v1+=vector.v1;
        v2+=vector.v2;
        v3+=vector.v3;
        v4+=vector.v4;
        return *this;
    }

    gzVec4 & operator-=(const gzVec4 &vector)
    {
        v1-=vector.v1;
        v2-=vector.v2;
        v3-=vector.v3;
        v4-=vector.v4;
        return *this;
    }

    gzVec4 & operator*=(const gzReal &factor)
    {
        v1*=factor;
        v2*=factor;
        v3*=factor;
        v4*=factor;
        return *this;
    }

    gzVec4  operator * (const gzReal &val) const
    {
        return gzVec4(v1*val,v2*val,v3*val,v4*val);
    }

    gzVec4  operator / (const gzReal &val) const
    {
        gzReal inv=GZ_REAL_ONE/val;
        return gzVec4(v1*inv,v2*inv,v3*inv,v4*inv);
    }
    
    gzVec4 operator - (const gzVec4 &vector_b) const
    {
        return gzVec4(v1-vector_b.v1,v2-vector_b.v2,v3-vector_b.v3,v4-vector_b.v4);
    }

    gzVec4 operator + (const gzVec4 &vector_b) const
    {
        return gzVec4(v1+vector_b.v1,v2+vector_b.v2,v3+vector_b.v3,v4+vector_b.v4);
    }

    inline operator gzVec3 () const
    { 
        if(v4==GZ_REAL_ONE)
            return gzVec3(v1,v2,v3);

        gzReal inv4=GZ_REAL_ONE/v4;
        return gzVec3(v1*inv4,v2*inv4,v3*inv4); 
    } 

    gzULong hash()  const;

    gzString    asString(const gzString &format="(%lf %lf %lf %lf)") const ;

    union
    {
        struct
        {
            gzReal v1,v2,v3,v4;
        };
        struct
        {
            gzReal x,y,z,w;
        };
        struct
        {
            gzReal r,g,b,a;
        };
    };

};

//******************************************************************************
// Class    : -
//                                  
// Purpose  : - 
//                                  
// Notes    : - 
//                                  
// Revision History...                          
//                                  
// Who  Date    Description                     
//                                  
// AMO  981128  Created 
//                                  
//******************************************************************************
class  GZ_BASE_EXPORT gzMatrix2
{
public:

    gzMatrix2(){};

    gzMatrix2(const gzVec2 &column_1,const gzVec2 &column_2);

    gzReal v11,v21;
    gzReal v12,v22;
};

//******************************************************************************
// Class    : -
//                                  
// Purpose  : - 
//                                  
// Notes    : - 
//                                  
// Revision History...                          
//                                  
// Who  Date    Description                     
//                                  
// AMO  981128  Created 
//                                  
//******************************************************************************
class  GZ_BASE_EXPORT gzMatrix3
{
public:

    gzMatrix3(){};

    gzMatrix3(const gzVec3 &column_1,const gzVec3 &column_2,const gzVec3 &column_3);

    gzReal v11,v21,v31;
    gzReal v12,v22,v32;
    gzReal v13,v23,v33;
};

// ------- Vector and Matrix Constants -----------------------------------------------------------

enum GZ_MATRIX3_IND { GZ3_V11,GZ3_V21,GZ3_V31,GZ3_V12,GZ3_V22,GZ3_V32,GZ3_V13,GZ3_V23,GZ3_V33};
enum GZ_MATRIX4_IND { GZ4_V11,GZ4_V21,GZ4_V31,GZ4_V41,GZ4_V12,GZ4_V22,GZ4_V32,GZ4_V42,GZ4_V13,GZ4_V23,GZ4_V33,GZ4_V43,GZ4_V14,GZ4_V24,GZ4_V34,GZ4_V44 };

const gzVec2 GZ_ZERO_VEC2       =gzVec2(0,0);
const gzVec3 GZ_ZERO_VEC3       =gzVec3(0,0,0);
const gzVec4 GZ_ZERO_VEC4       =gzVec4(0,0,0,0);

const gzVec2 GZ_ONE_VEC2        =gzVec2(1,1);
const gzVec3 GZ_ONE_VEC3        =gzVec3(1,1,1);
const gzVec4 GZ_ONE_VEC4        =gzVec4(1,1,1,1);

const gzVec3 GZ_X_VEC3      =gzVec3(1,0,0);
const gzVec3 GZ_Y_VEC3      =gzVec3(0,1,0);
const gzVec3 GZ_Z_VEC3      =gzVec3(0,0,1);

const gzVec4 GZ_X_VEC4      =gzVec4(1,0,0,0);
const gzVec4 GZ_Y_VEC4      =gzVec4(0,1,0,0);
const gzVec4 GZ_Z_VEC4      =gzVec4(0,0,1,0);
const gzVec4 GZ_W_VEC4      =gzVec4(0,0,0,1);

//******************************************************************************
// Class    : -
//                                  
// Purpose  : - 
//                                  
// Notes    : - 
//                                  
// Revision History...                          
//                                  
// Who  Date    Description                     
//                                  
// AMO  981128  Created 
//                                  
//******************************************************************************
class  GZ_BASE_EXPORT gzMatrix4
{
public:


    gzMatrix4():isDirty(TRUE){};

    gzMatrix4(const gzVec4 &column_1,const gzVec4 &column_2,const gzVec4 &column_3,const gzVec4 &column_4);

    /* The default faceMatrixMode is GZ_CCW which is counter clock wise rotation */
    gzMatrix4(const gzVec3 &column_1,const gzVec3 &column_2,const gzVec3 &column_3,gzEnum faceMatrixMode=GZ_CCW);

    gzMatrix4(const gzMatrix3 &matrix,gzEnum faceMatrixMode=GZ_CCW);

    gzMatrix4 & operator+=(const gzMatrix4 &matrix);
    gzMatrix4 & operator*=(const gzReal &value);
    gzMatrix4 & operator/=(const gzReal &value);
    gzMatrix4 & operator-=(const gzMatrix4 &matrix);

    gzVoid  identity();

    gzReal  determinant() const;


    gzBool  getInvertedMatrix( gzMatrix4 &to ) const;


    gzVoid  getTransposedMatrix( gzMatrix4 &to ) const;

    gzVoid  getVectorRotation( gzVec3 &vec , gzReal &angle , const gzVec3 &base=GZ_ONE_VEC3) const;

    gzBool operator==(const gzMatrix4 &matrix) const ;
    gzBool operator!=(const gzMatrix4 &matrix) const { return !operator==(matrix); }

    gzVoid  setRow(gzInt row , const gzVec4 & vec); // row 0-3
    gzVoid  setCol(gzInt col , const gzVec4 & vec); // col 0-3

    gzVoid  setRow(gzInt row , const gzVec3 & vec); // row 0-3
    gzVoid  setCol(gzInt col , const gzVec3 & vec); // col 0-3

    gzVoid  updateFaceMatrixMode() GZ_NO_THROW ;

    // The items 
    gzReal  v11,v21,v31,v41,v12,v22,v32,v42,v13,v23,v33,v43,v14,v24,v34,v44;


    gzBool  isDirty;

    gzEnum  faceMatrixMode;
    
};

// Operators -----------------------------------------------------------

GZ_BASE_EXPORT gzMatrix4 operator * (const gzMatrix4 &mat1 , const gzMatrix4 &mat2);
GZ_BASE_EXPORT gzMatrix4 operator + (const gzMatrix4 &mat1 , const gzMatrix4 &mat2);
GZ_BASE_EXPORT gzMatrix4 operator - (const gzMatrix4 &mat1 , const gzMatrix4 &mat2);

GZ_BASE_EXPORT gzMatrix4 operator * ( const gzMatrix4 &mat1 ,  const gzReal &value);
GZ_BASE_EXPORT gzMatrix4 operator / ( const gzMatrix4 &mat1 ,  const gzReal &value);

GZ_BASE_EXPORT gzVec4 operator * (const gzMatrix4 &mat , const gzVec4 &vec );
GZ_BASE_EXPORT gzVec4 operator * (const gzVec4 &vec , const gzMatrix4 &mat );

GZ_BASE_EXPORT gzVec3 multiply   (const gzMatrix4 &mat , const gzVec3 &vec ); 
GZ_BASE_EXPORT gzVec3 multiply   (const gzMatrix4 &mat , const gzVec3 &vec ,const gzReal &w ); 
GZ_BASE_EXPORT gzVec3 normweight (const gzMatrix4 &mat , const gzVec3 &vec ,const gzReal &w=GZ_REAL_ONE ); 
GZ_BASE_EXPORT gzVec4 operator * (const gzMatrix4 &mat , const gzVec3 &vec ); 
GZ_BASE_EXPORT gzVec3 operator * (const gzMatrix3 &mat , const gzVec3 &vec );

// Utilities -----------------------------------------------------------

GZ_BASE_EXPORT const gzMatrix4 &gzGetUnitMatrix() GZ_NO_THROW ;

GZ_BASE_EXPORT gzVoid gzGetTranslationMatrix( gzMatrix4 &matrix,gzReal dx , gzReal dy , gzReal dz ) GZ_NO_THROW ;
GZ_BASE_EXPORT gzVoid gzGetTranslationMatrix( gzMatrix4 &matrix,const gzVec3 &translation ) GZ_NO_THROW ;

GZ_BASE_EXPORT gzVoid gzGetScaleMatrix( gzMatrix4 &matrix,gzReal dx , gzReal dy , gzReal dz ) GZ_NO_THROW ;
GZ_BASE_EXPORT gzVoid gzGetScaleMatrix( gzMatrix4 &matrix, const gzVec3 &scale ) GZ_NO_THROW ;

GZ_BASE_EXPORT gzVoid gzGetBaseMatrix( gzMatrix4 &matrix , const gzVec3 &base ) GZ_NO_THROW ;
GZ_BASE_EXPORT gzVoid gzGetBaseMatrix( gzMatrix4 &matrix , const gzVec3 &x_base,const gzVec3 &y_base) GZ_NO_THROW;

GZ_BASE_EXPORT gzVoid gzGetRotationMatrix( gzMatrix4 &matrix , const gzVec3 &base , gzReal angle)GZ_NO_THROW ;
GZ_BASE_EXPORT gzVoid gzGetRotationMatrix( gzMatrix4 &matrix , gzReal heading , gzReal pitch , gzReal roll) GZ_NO_THROW ;
GZ_BASE_EXPORT gzVoid gzGetRotationMatrix( gzMatrix4 &matrix , const gzVec3 &orientation) GZ_NO_THROW ;

GZ_BASE_EXPORT gzVoid gzGetBiasMatrix( gzMatrix4 &matrix ,  gzReal x_factor=1.0f ,gzReal x_offset=0.0f , 
                                                        gzReal y_factor=1.0f ,gzReal y_offset=0.0f , 
                                                        gzReal z_factor=1.0f ,gzReal z_offset=0.0f  );

GZ_BASE_EXPORT gzBool gzIsPureTranslation( const gzMatrix4 &matrix , gzVec3 &translation);

GZ_BASE_EXPORT inline gzReal gzGetNormDot( const gzVec3 & a , const gzVec3 & b, const gzVec3 &c , const gzVec3 &dir) GZ_NO_THROW
{
    gzReal x1(b.v1-a.v1);
    gzReal y1(c.v1-a.v1);

    gzReal x2(b.v2-a.v2);
    gzReal y2(c.v2-a.v2);

    gzReal x3(b.v3-a.v3);
    gzReal y3(c.v3-a.v3);

    return (x2*y3-x3*y2)*dir.v1+(x3*y1-x1*y3)*dir.v2+(x1*y2-x2*y1)*dir.v3;
}


GZ_BASE_EXPORT inline gzVoid getExtendedVector(gzVec3 &dest, const gzVec3 &from, const gzVec3 &to , const gzReal &magnitude) GZ_NO_THROW
{
    dest.v1=(to.v1-from.v1)*magnitude+to.v1;
    dest.v2=(to.v2-from.v2)*magnitude+to.v2;
    dest.v3=(to.v3-from.v3)*magnitude+to.v3;
}


//---------------------------- Template Specifics -----------------------------

// -------- gzVec2 --------

template <> inline gzArray<gzVec2> & gzArray<gzVec2>::operator=(const gzArray &copy)
{
    if(&copy==this)
        return *this;

    if(m_size<copy.m_size)  // Allow shrink
    {
        if(m_data)
            delete [] m_data;

        m_size=copy.m_size;

        if(copy.m_size)
        {
            m_data = new gzVec2[copy.m_size];
        }
        else
            m_data=NULL;
    }
    else
        m_size=copy.m_size;

    memcpy(m_data,copy.m_data,m_size*sizeof(gzVec2));

    return *this;
}

template <> inline gzVoid gzDynamicArray<gzVec2>::transferToArray(gzArray<gzVec2> &array) const
{
    array.setSize(m_currentSize);
    memcpy(array.getAddress(),m_data,m_currentSize*sizeof(gzVec2));
}


// -------- gzVec3 --------

template <> inline gzArray<gzVec3> & gzArray<gzVec3>::operator=(const gzArray &copy)
{
    if(&copy==this)
        return *this;

    if(m_size<copy.m_size)  // Allow shrink
    {
        if(m_data)
            delete [] m_data;

        m_size=copy.m_size;

        if(copy.m_size)
        {
            m_data = new gzVec3[copy.m_size];
        }
        else
            m_data=NULL;
    }
    else
        m_size=copy.m_size;

    memcpy(m_data,copy.m_data,m_size*sizeof(gzVec3));

    return *this;
}

template <> inline gzVoid gzDynamicArray<gzVec3>::transferToArray(gzArray<gzVec3> &array) const
{
    array.setSize(m_currentSize);
    memcpy(array.getAddress(),m_data,m_currentSize*sizeof(gzVec3));
}

// -------- gzVec4 --------

template <> inline gzArray<gzVec4> & gzArray<gzVec4>::operator=(const gzArray &copy)
{
    if(&copy==this)
        return *this;

    if(m_size<copy.m_size)  // Allow shrink
    {
        if(m_data)
            delete [] m_data;

        m_size=copy.m_size;

        if(copy.m_size)
        {
            m_data = new gzVec4[copy.m_size];
        }
        else
            m_data=NULL;
    }
    else
        m_size=copy.m_size;

    memcpy(m_data,copy.m_data,m_size*sizeof(gzVec4));

    return *this;
}

template <> inline gzVoid gzDynamicArray<gzVec4>::transferToArray(gzArray<gzVec4> &array) const
{
    array.setSize(m_currentSize);
    memcpy(array.getAddress(),m_data,m_currentSize*sizeof(gzVec4));
}

// ----------------- End of template specifics -------------------

#endif

---