gfx_math_matrix.hh

Go to the documentation of this file.

/*
 * LEGAL:   COPYRIGHT (C) 2004 JIM E. BROOKS
 *          THIS SOURCE CODE IS RELEASED UNDER THE TERMS
 *          OF THE GNU GENERAL PUBLIC LICENSE VERSION 2 (GPL 2).
 *****************************************************************************/

#ifndef GFX_MATH_MATRIX_HH
#define GFX_MATH_MATRIX_HH 1

namespace gfx {


// MATRIX_EXT=1 extended precision.
#define MATRIX_EXT  0

// A gfx matrix has 12 elements to minimize space (W elements aren't needed).
// A 16 element matrix remains a compile option.
#define MATRIX_ELEMS  12

// Row-major or column-major.
// If row-major, to go to row i, multiply i by amount of columns.
#ifndef MATRIX_ROW_MAJOR
#define MATRIX_ROW_MAJOR  0
#endif

// Matrix element offsets.
#if MATRIX_ELEMS == 12
    #if MATRIX_ROW_MAJOR
enum { Xx, Xy, Xz, Ox,
       Yx, Yy, Yz, Oy,
       Zx, Zy, Zz, Oz };
    #else
enum { Xx, Yx, Zx,
       Xy, Yy, Zy,
       Xz, Yz, Zz,
       Ox, Oy, Oz };
    #endif // MATRIX_ROW_MAJOR
#elif MATRIX_ELEMS == 16
    #if MATRIX_ROW_MAJOR
enum { Xx, Xy, Xz, Ox,
       Yx, Yy, Yz, Oy,
       Zx, Zy, Zz, Oz,
       Xw, Yw, Zw, Ow };  // W elements are strongly deprecated
    #else
enum { Xx, Yx, Zx, Xw,
       Xy, Yy, Zy, Yw,
       Xz, Yz, Zz, Zw,
       Ox, Oy, Oz, Ow };
    #endif // MATRIX_ROW_MAJOR
#endif


class Matrix : public Shared
{

public:

    // Matrix fp type.
#if MATRIX_EXT
    typedef fpx FPM;        
#else
    typedef fp FPM;         
#endif
    typedef FPM ElemType;   

    Matrix( void )
    {
        Identity();
    }

    // (default copy methods suffice)

    // Element access.
    FPM&       operator[]( uint i )       { ASSERT2( i < MATRIX_ELEMS ); return m[i]; }
    const FPM& operator[]( uint i ) const { ASSERT2( i < MATRIX_ELEMS ); return m[i]; }

    friend bool operator==( const Matrix& m1, const Matrix& m2 )
    {
        for ( uint i = 0; i < MATRIX_ELEMS; ++i )
            if ( m1[i] != m2[i] ) return false;
        return true;  // each was equal
    }

    friend bool operator!=( const Matrix& m1, const Matrix& m2 )
    {
        return not (m1 == m2);
    }

    friend bool IfRotationEqual( const Matrix& m1, const Matrix& m2 )
    {
        for ( uint i = 0; i < Ox; ++i )
            if ( m1[i] != m2[i] ) return false;
        return true;  // each was equal
    }

    Vector3 GetOrigin( void ) const
    {
        return Vector3( m[Ox], m[Oy], m[Oz] );
    }

    void SetOrigin( const Vector3& origin )
    {
        m[Ox] = origin.x;
        m[Oy] = origin.y;
        m[Oz] = origin.z;
    }

    // Convert Matrix object into an OpenGL 4x4 matrix.
    void Convert( fp out[16] ) const
    {
        out[ 0] = m[Xx]; out[ 1] = m[Xy]; out[ 2] = m[Xz];
        out[ 4] = m[Yx]; out[ 5] = m[Yy]; out[ 6] = m[Yz];
        out[ 8] = m[Zx]; out[ 9] = m[Zy]; out[10] = m[Zz];
        out[12] = m[Ox]; out[13] = m[Oy]; out[14] = m[Oz];
#if MATRIX_ELEMS == 12
        out[ 0+3] = 0.0;
        out[ 4+3] = 0.0;
        out[ 8+3] = 0.0;
        out[12+3] = 1.0;
#else // MATRIX_ELEMS == 16
        out[ 0+3] = m[Xw];
        out[ 4+3] = m[Yw];
        out[ 8+3] = m[Zw];
        out[12+3] = m[Ow];
#endif
    }

    void
    Identity( void )
    {
        m[Xx] = 1.0; m[Xy] = 0.0; m[Xz] = 0.0;
        m[Yx] = 0.0; m[Yy] = 1.0; m[Yz] = 0.0;
        m[Zx] = 0.0; m[Zy] = 0.0; m[Zz] = 1.0;
        m[Ox] = 0.0; m[Oy] = 0.0; m[Oz] = 0.0;
#if MATRIX_ELEMS == 16
        m[Xw] = 0.0; m[Yw] = 0.0; m[Zw] = 0.0; m[Ow] = 1.0;
#endif
    }


    FPM
    RotateX( FPM x, FPM y, FPM z ) const
    {
        return x * m[Xx] + y * m[Xy] + z * m[Xz];  // X
    }

    FPM
    RotateY( FPM x, FPM y, FPM z ) const
    {   
        return x * m[Yx] + y * m[Yy] + z * m[Yz];  // Y
    }

    FPM
    RotateZ( FPM x, FPM y, FPM z ) const
    {
        return x * m[Zx] + y * m[Zy] + z * m[Zz];  // Z
    }

    template<typename VECTOR_DEST,typename VECTOR_SRC>
    VECTOR_DEST
    Rotate( const VECTOR_SRC& v ) const
    {
        return VECTOR_DEST( v.x * m[Xx] + v.y * m[Xy] + v.z * m[Xz],    // X
                            v.x * m[Yx] + v.y * m[Yy] + v.z * m[Yz],    // Y
                            v.x * m[Zx] + v.y * m[Zy] + v.z * m[Zz] );  // Z
    }

    FPM
    RotateTranslateX( FPM x, FPM y, FPM z ) const
    {
        return x * m[Xx] + y * m[Xy] + z * m[Xz] + m[Ox];  // X
    }

    FPM
    RotateTranslateY( FPM x, FPM y, FPM z ) const
    {
        return x * m[Yx] + y * m[Yy] + z * m[Yz] + m[Oy];  // Y
    }

    FPM
    RotateTranslateZ( FPM x, FPM y, FPM z ) const
    {
        return x * m[Zx] + y * m[Zy] + z * m[Zz] + m[Oz];  // Z
    }

    #define CODE_RESIZE_DEST_ARRAY(dest,src,vcnt)                       \
        /* Ensure dest array has enough room. */                        \
        const uint vcnt = src.size();                                   \
        ASSERT( vcnt > 0 );                                             \
        ASSERT( vcnt < MAX_VERTEXS );                                   \
        dest.resize( vcnt );

    template<typename DEST,typename SRC>
    DEST
    RotateTranslate( const SRC& v ) const
    {
        return DEST( v.x * m[Xx] + v.y * m[Xy] + v.z * m[Xz] + m[Ox],
                     v.x * m[Yx] + v.y * m[Yy] + v.z * m[Yz] + m[Oy],
                     v.x * m[Zx] + v.y * m[Zy] + v.z * m[Zz] + m[Oz] );
    }

    #define CODE_ROTATE_TRANSLATE_ARRAY(dest,src,vcnt)                                  \
    {                                                                                   \
        for ( uint i = 0; i < vcnt; ++i )                                               \
        {                                                                               \
          /* catch memory corruption */ \
            dest[i].x = src[i].x * m[Xx] + src[i].y * m[Xy] + src[i].z * m[Xz] + m[Ox]; \
            dest[i].y = src[i].x * m[Yx] + src[i].y * m[Yy] + src[i].z * m[Yz] + m[Oy]; \
            dest[i].z = src[i].x * m[Zx] + src[i].y * m[Zy] + src[i].z * m[Zz] + m[Oz]; \
        }                                                                               \
    }

    template<typename DEST,typename SRC>
    void
    RotateTranslate( Array<DEST>& dest, const Array<SRC>& src ) const
    {
        CODE_RESIZE_DEST_ARRAY(dest,src,vcnt)
#if DEBUG
        // Compile slower STL operator[] but handles vertex typesigs in DEBUG builds.
        CODE_ROTATE_TRANSLATE_ARRAY(dest,src,vcnt)
#else
        // Compile optimal code to directly index raw array (incompatible with typesigs).
        Vector3* destRaw = dest.PTR(); const Vector3* srcRaw = src.CONST_PTR();  // -- CAST --
        CODE_ROTATE_TRANSLATE_ARRAY(destRaw,srcRaw,vcnt)
#endif
    }

    #undef CODE_ROTATE_TRANSLATE_ARRAY

    template<typename DEST,typename SRC>
    DEST
    TranslateRotate( const SRC& src ) const
    {
        fp x = src.x + m[Ox];
        fp y = src.y + m[Oy];
        fp z = src.z + m[Oz];
        return DEST( x * m[Xx] + y * m[Xy] + z * m[Xz],
                     x * m[Yx] + y * m[Yy] + z * m[Yz],
                     x * m[Zx] + y * m[Zy] + z * m[Zz] );
    }

    #define CODE_TRANSLATE_ROTATE_ARRAY(dest,src,vcnt)                                  \
    {                                                                                   \
        for ( uint i = 0; i < vcnt; ++i )                                               \
        {                                                                               \
          /* catch memory corruption */ \
            fp x = src[i].x + m[Ox];                                                    \
            fp y = src[i].y + m[Oy];                                                    \
            fp z = src[i].z + m[Oz];                                                    \
            dest[i].x = x * m[Xx] + y * m[Xy] + z * m[Xz];                              \
            dest[i].y = x * m[Yx] + y * m[Yy] + z * m[Yz];                              \
            dest[i].z = x * m[Zx] + y * m[Zy] + z * m[Zz];                              \
        }                                                                               \
    }

    template<typename DEST,typename SRC>
    void
    TranslateRotate( Array<DEST>& dest, const Array<SRC>& src ) const
    {
        CODE_RESIZE_DEST_ARRAY(dest,src,vcnt)
#if DEBUG
        // Compile slower STL operator[] but handles vertex typesigs in DEBUG builds.
        CODE_TRANSLATE_ROTATE_ARRAY(dest,src,vcnt)
#else
        // Compile optimal code to directly index raw array (incompatible with typesigs).
        Vector3* destRaw = dest.PTR(); const Vector3* srcRaw = src.CONST_PTR();  // -- CAST --
        CODE_TRANSLATE_ROTATE_ARRAY(destRaw,srcRaw,vcnt)
#endif
    }

    #undef CODE_TRANSLATE_ROTATE_ARRAY


    WorldVertex
    Transform( const LocalVertex& v ) const
    {
        return RotateTranslate<WorldVertex,LocalVertex>( v );
    }
    void
    Transform( Array<WorldVertex>& dest, const Array<LocalVertex>& src ) const
    {
        RotateTranslate<WorldVertex,LocalVertex>( dest, src );
    }

    EyeVertex
    Transform( const WorldVertex& v ) const
    {
        return TranslateRotate<EyeVertex,WorldVertex>( v );
    }
    void
    Transform( Array<EyeVertex>& dest, const Array<WorldVertex>& src ) const
    {
        TranslateRotate<EyeVertex,WorldVertex>( dest, src );
    }

    void
    Transform( Array<EyeVertex>& dest, const Array<LocalVertex>& src ) const
    {
        RotateTranslate<EyeVertex,LocalVertex>( dest, src );
    }

    template<typename VECTOR>
    VECTOR
    MoveFixed( uint axis, fp inc ) const
    {
    ASSERT( CHECK_AXIS( axis ) );
        return VECTOR( inc * m[MIXF(axis,XX)],
                       inc * m[MIXF(axis,YY)],
                       inc * m[MIXF(axis,ZZ)] );
    }

    template<typename VECTOR>
    VECTOR
    MoveLocal( uint axis, fp inc ) const
    {
    ASSERT( CHECK_AXIS( axis ) );
        return VECTOR( inc * m[MIXL(axis,XX)],
                       inc * m[MIXL(axis,YY)],
                       inc * m[MIXL(axis,ZZ)] );
    }


    void
    TranslateFixed( uint axis, FPM inc )
    {
    ASSERT( CHECK_AXIS( axis ) );
        m[Ox] += inc * m[MIXF(axis,XX)];
        m[Oy] += inc * m[MIXF(axis,YY)];
        m[Oz] += inc * m[MIXF(axis,ZZ)];
    }

    void
    TranslateLocal( uint axis, FPM inc )
    {
    ASSERT( CHECK_AXIS( axis ) );
        m[Ox] += inc * m[MIXL(axis,XX)];
        m[Oy] += inc * m[MIXL(axis,YY)];
        m[Oz] += inc * m[MIXL(axis,ZZ)];
    }

    void
    RotateFixed( uint axis, Radian rad )
    {
        pair<FPM,FPM> si_co = SinCos<FPM>( rad );
        FPM& s = si_co.first;
        FPM& c = si_co.second;
        Matrix n = *this;  // dest = src
        switch ( axis )
        {
            // X : Pitch
            case AXIS_X:
            n[Yx] = m[Yx]*c - m[Zx]*s;      // Y = Ycos - Zsin
            n[Yy] = m[Yy]*c - m[Zy]*s;
            n[Yz] = m[Yz]*c - m[Zz]*s;

            n[Zx] = m[Yx]*s + m[Zx]*c;      // Z = Ysin + Zcos
            n[Zy] = m[Yy]*s + m[Zy]*c;
            n[Zz] = m[Yz]*s + m[Zz]*c;
            break;

            // Y : Yaw
            case AXIS_Y:
            n[Xx] = m[Xx]*c - m[Zx]*s;      // X = Xcos - Zsin
            n[Xy] = m[Xy]*c - m[Zy]*s;
            n[Xz] = m[Xz]*c - m[Zz]*s;

            n[Zx] = m[Xx]*s + m[Zx]*c;      // Z = Xsin + Zcos
            n[Zy] = m[Xy]*s + m[Zy]*c;
            n[Zz] = m[Xz]*s + m[Zz]*c;
            break;

            // Z : Roll
            case AXIS_Z:
            n[Xx] = m[Xx]*c - m[Yx]*s;      // X = Xcos - Ysin
            n[Xy] = m[Xy]*c - m[Yy]*s;
            n[Xz] = m[Xz]*c - m[Yz]*s;

            n[Yx] = m[Xx]*s + m[Yx]*c;      // Y = Xsin + Ycos
            n[Yy] = m[Xy]*s + m[Yy]*c;
            n[Yz] = m[Xz]*s + m[Yz]*c;
            break;

            default: ASSERT(0); return;
        }

        *this = n;  // dest = src
    }

    void
    RotateLocal( uint axis, Radian rad )
    {
        // Identity (1:1) matrix r.
        Matrix r;

        // Rotate matrix r.
        r.RotateFixed( axis, rad );

        // Transform r thru m.
        // Ie, thru matrix m, pass r as if it were the rotated coords (1.0, 1.0, 1.0).
        Matrix t;
        t[Xx] = r.RotateTranslateX( m[Xx], m[Xy], m[Xz] );
        t[Xy] = r.RotateTranslateY( m[Xx], m[Xy], m[Xz] );
        t[Xz] = r.RotateTranslateZ( m[Xx], m[Xy], m[Xz] );

        t[Yx] = r.RotateTranslateX( m[Yx], m[Yy], m[Yz] );
        t[Yy] = r.RotateTranslateY( m[Yx], m[Yy], m[Yz] );
        t[Yz] = r.RotateTranslateZ( m[Yx], m[Yy], m[Yz] );

        t[Zx] = r.RotateTranslateX( m[Zx], m[Zy], m[Zz] );
        t[Zy] = r.RotateTranslateY( m[Zx], m[Zy], m[Zz] );
        t[Zz] = r.RotateTranslateZ( m[Zx], m[Zy], m[Zz] );

        // m = r excluding origin elements.
        m[Xx] = t[Xx];
        m[Xy] = t[Xy];
        m[Xz] = t[Xz];

        m[Yx] = t[Yx];
        m[Yy] = t[Yy];
        m[Yz] = t[Yz];

        m[Zx] = t[Zx];
        m[Zy] = t[Zy];
        m[Zz] = t[Zz];
    }

    // Limited use.
    FPM*             PTR( void ) const { return const_cast<FPM*>( m ); }
    const FPM* CONST_PTR( void ) const { return m; }

private:
// Access matrix elements using a (row,col) tuple independent of matrix format.
// MIXF() is used for transformations relative to fixed space.
// MIXL() is used for transformations relative to matrix's local space.
#if MATRIX_ELEMS == 12
    #if MATRIX_ROW_MAJOR
    uint MIXF( uint i, uint j ) const  { return i*4 + j; }
    #else
    uint MIXF( uint i, uint j ) const  { return j*3 + i; }
    #endif
    #if MATRIX_ROW_MAJOR
    uint MIXL( uint i, uint j ) const { return j*4 + i; }
    #else
    uint MIXL( uint i, uint j ) const { return i*3 + j; }
    #endif
#elif MATRIX_ELEMS == 16
    #if MATRIX_ROW_MAJOR
    uint MIXF( uint i, uint j ) const  { return i*4 + j; }
    #else
    uint MIXF( uint i, uint j ) const { return j*4 + i; }
    #endif
    #if MATRIX_ROW_MAJOR
    uint MIXL( uint i, uint j ) const { return j*4 + i; }
    #else
    uint MIXL( uint i, uint j ) const { return i*4 + j; }
    #endif
#endif

    friend ostream& operator<<( ostream& strm, const Matrix& m )
    {
        ios::fmtflags savedFlags = strm.flags();
        strm.setf( ios::fixed, ios::floatfield );
        strm.precision( 6 );
        strm << "Xxyzw:  " << m[Xx] << "  " << m[Xy] << "  " << m[Xz] << endl
             << "Yxyzw:  " << m[Yx] << "  " << m[Yy] << "  " << m[Yz] << endl
             << "Zxyzw:  " << m[Zx] << "  " << m[Zy] << "  " << m[Zz] << endl
             << "Oxyzw:  " << m[Ox] << "  " << m[Oy] << "  " << m[Oz]; // << endl;
        strm.flags(savedFlags);
        return strm;
    }

private:
    FPM     m[MATRIX_ELEMS];
};


INLINE Matrix
operator*( const Matrix& m, const Matrix& n )
{
    // Optimizations:
    // Calculating Xw,Yw,Zw,Ow (0,0,0,1) is bypassed as they never change.
    // Xw,Yw,Zw are always 0 so multiplying one factor is bypassed.
    // Ow is always 1 so Oxyz coefficients are added directly without multiplying it by 1.
    // Inefficiencies:
    // Some factors are re-multiplied.

    Matrix prod;

#define MULTIPLY_MATRIX_ELEM3( i, a0,a1, b0,b1, c0,c1 )         \
    prod[i] =   m[a0] * n[a1]                                   \
              + m[b0] * n[b1]                                   \
              + m[c0] * n[c1];

#define MULTIPLY_MATRIX_ELEM4( i, a0,a1, b0,b1, c0,c1, oi )     \
    prod[i] =   m[a0] * n[a1]                                   \
              + m[b0] * n[b1]                                   \
              + m[c0] * n[c1]                                   \
              +         n[oi];

#if MATRIX_ROW_MAJOR
    MULTIPLY_MATRIX_ELEM3( Xx, Xx,Xx, Yx,Xy, Zx,Xz /*, Xw,Ox */      ); 
    MULTIPLY_MATRIX_ELEM3( Xy, Xy,Xx, Yy,Xy, Zy,Xz /*, Yw,Ox */      ); 
    MULTIPLY_MATRIX_ELEM3( Xz, Xz,Xx, Yz,Xy, Zz,Xz /*, Zw,Ox */      ); 
    MULTIPLY_MATRIX_ELEM4( Ox, Ox,Xx, Oy,Xy, Oz,Xz /*, Ow,Ox */ , Ox ); 
    MULTIPLY_MATRIX_ELEM3( Yx, Xx,Yx, Yx,Yy, Zx,Yz /*, Xw,Oy */      ); 
    MULTIPLY_MATRIX_ELEM3( Yy, Xy,Yx, Yy,Yy, Zy,Yz /*, Yw,Oy */      ); 
    MULTIPLY_MATRIX_ELEM3( Yz, Xz,Yx, Yz,Yy, Zz,Yz /*, Zw,Oy */      ); 
    MULTIPLY_MATRIX_ELEM4( Oy, Ox,Yx, Oy,Yy, Oz,Yz /*, Ow,Oy */ , Oy ); 
    MULTIPLY_MATRIX_ELEM3( Zx, Xx,Zx, Yx,Zy, Zx,Zz /*, Xw,Oz */      ); 
    MULTIPLY_MATRIX_ELEM3( Zy, Xy,Zx, Yy,Zy, Zy,Zz /*, Yw,Oz */      ); 
    MULTIPLY_MATRIX_ELEM3( Zz, Xz,Zx, Yz,Zy, Zz,Zz /*, Zw,Oz */      ); 
    MULTIPLY_MATRIX_ELEM4( Oz, Ox,Zx, Oy,Zy, Oz,Zz /*, Ow,Oz */ , Oz ); 
#else
    MULTIPLY_MATRIX_ELEM3( Xx, Xx,Xx, Yx,Xy, Zx,Xz /*, Xw,Ox */      ); 
    MULTIPLY_MATRIX_ELEM3( Yx, Xx,Yx, Yx,Yy, Zx,Yz /*, Xw,Oy */      ); 
    MULTIPLY_MATRIX_ELEM3( Zx, Xx,Zx, Yx,Zy, Zx,Zz /*, Xw,Oz */      ); 
    MULTIPLY_MATRIX_ELEM3( Xy, Xy,Xx, Yy,Xy, Zy,Xz /*, Yw,Ox */      ); 
    MULTIPLY_MATRIX_ELEM3( Yy, Xy,Yx, Yy,Yy, Zy,Yz /*, Yw,Oy */      ); 
    MULTIPLY_MATRIX_ELEM3( Zy, Xy,Zx, Yy,Zy, Zy,Zz /*, Yw,Oz */      ); 
    MULTIPLY_MATRIX_ELEM3( Xz, Xz,Xx, Yz,Xy, Zz,Xz /*, Zw,Ox */      ); 
    MULTIPLY_MATRIX_ELEM3( Yz, Xz,Yx, Yz,Yy, Zz,Yz /*, Zw,Oy */      ); 
    MULTIPLY_MATRIX_ELEM3( Zz, Xz,Zx, Yz,Zy, Zz,Zz /*, Zw,Oz */      ); 
    MULTIPLY_MATRIX_ELEM4( Ox, Ox,Xx, Oy,Xy, Oz,Xz /*, Ow,Ox */ , Ox ); 
    MULTIPLY_MATRIX_ELEM4( Oy, Ox,Yx, Oy,Yy, Oz,Yz /*, Ow,Oy */ , Oy ); 
    MULTIPLY_MATRIX_ELEM4( Oz, Ox,Zx, Oy,Zy, Oz,Zz /*, Ow,Oz */ , Oz ); 
#endif

    return prod;
}

INLINE void
TransposeMatrix( Matrix& dest, const Matrix& src )
{
    // Transpose rotation sub-matrix.
    // X = X'
    dest[Xx] = src[Xx];
    dest[Xy] = src[Yx];
    dest[Xz] = src[Zx];
    // Y = Y'
    dest[Yx] = src[Xy];
    dest[Yy] = src[Yy];
    dest[Yz] = src[Zy];
    // Z = Z'
    dest[Zx] = src[Xz];
    dest[Zy] = src[Yz];
    dest[Zz] = src[Zz];

    // Negate origin sub-matrix.
    dest[Ox] = -src[Ox];
    dest[Oy] = -src[Oy];
    dest[Oz] = -src[Oz];
}

// Served their purpose.
#undef MATRIX_ELEMS
#undef CODE_RESIZE_DEST_ARRAY

} // namespace gfx

#endif // GFX_MATH_MATRIX_HH

Palomino 3D Engine documents generated by doxygen 1.5.3 on Fri Nov 23 11:26:10 2007