Matrix34.h

Go to the documentation of this file.

// BC  12/24/2003  \Bob110\Include\Matrix34.h

#ifndef __MATRIX34_H
#define __MATRIX34_H

#ifndef __MATRIX44_H
#include "Matrix44.h"
#endif
#ifndef __MATRIX33_H
#include "Matrix33.h"
#endif
#ifndef __ROTATE_H
#include "Rotate.h"
#endif

#ifdef _SIMD
#include "smlmath.h"
#include "smlMatrix34.h"
#endif

class MATRIXEXPORT Matrix34 {
protected:
#ifdef _SIMD
   union {
      struct { SMLMatrix34f m_smlmatrix34f; };
      struct { Vector4 v[3]; };
   };
#else
   Vector4 v[3];
#endif

public:
   Matrix34( void ) { SetIdentity(FALSE); }
   Matrix34( const Vector4 &, const Vector4 &, const Vector4 & );
   Matrix34( const Matrix34 &m );
   Matrix34( float fillvalue );
   Matrix34( const RotateVector &rotate );
   Matrix34( const Quaternion & );
   Matrix34( const RotateEuler &re );
   Matrix34( const TSQ &tsq );
   
#ifdef _SIMD
   const SMLMatrix34f &GetSMLMatrix34f() const { return m_smlmatrix34f; }
   void  SetIdentity(BOOL state) { m_smlmatrix34f.SetIdentity( state!=FALSE ); }
   void InitBottomRow() {
      m_smlmatrix34f.Set( 3,0,0 );
      m_smlmatrix34f.Set( 3,1,0 );
      m_smlmatrix34f.Set( 3,2,0 );
      m_smlmatrix34f.Set( 3,3,1 );
   }
#else
   void  SetIdentity(BOOL state) {}
   void InitBottomRow() {}
#endif
   Matrix34 &operator *= ( const Matrix34 &other ) { return (*this = *this * other); }

   const Vector4 &operator[]( int row ) const { return v[row]; }
   Vector4 &operator[]( int row ) { return v[row]; }
   void Set( const Vector4 &a, const Vector4 &b, const Vector4 &c );
   void SetIdentity()
   {
      v[0].Set(1.0f,0.0f,0.0f,0.0f);
      v[1].Set(0.0f,1.0f,0.0f,0.0f);
      v[2].Set(0.0f,0.0f,1.0f,0.0f);
      SetIdentity( TRUE );      
   }
   Matrix34 Transpose( void ) const;
   Matrix34 Inverse( void ) const;
   void SetTranslate( const Vector &t );
   Vector GetTranslate( void ) const;
   Matrix34 NoTranslate( void ) const;
   Matrix34 GetScaleMatrix( void ) const;
   Vector   GetScaleVector( void ) const;
   int operator == ( const Matrix34 &a ) const { return v[0]==a[0] && v[1]==a[1] && v[2]==a[2]; }
   int operator != ( const Matrix34 &a ) const { return !(*this == a); }   
   Vector GetXAxis() const { return Vector( v[0][0], v[1][0], v[2][0] ); }
   Vector GetYAxis() const { return Vector( v[0][1], v[1][1], v[2][1] ); }
   Vector GetZAxis() const { return Vector( v[0][2], v[1][2], v[2][2] ); }
   Matrix34 operator * ( const Matrix34 & ) const;
   Vector operator * ( const Vector & ) const;

   Matrix34 InvertRotateTranslate() const  // THE MATRIX MUST HAVE NO SCALE
   {
      Matrix34 rotate = Transpose();
      rotate.SetTranslate( rotate * -GetTranslate());
      return rotate;
   }
   friend /* inline */ Vector operator * ( const Vector &v, const Matrix34 &m );
   friend MATRIXEXPORT Matrix34 Identity34( void );

   // GL MATRIX CREATION FUNCTIONS
   friend MATRIXEXPORT Matrix34 Translatef( float x, float y, float z );
   friend MATRIXEXPORT Matrix34 TranslateV( const Vector &v );
   friend MATRIXEXPORT Matrix34 Scalef( float x, float y, float z );
   friend MATRIXEXPORT Matrix34 ScaleV( const Vector &v );
   friend MATRIXEXPORT Matrix34 RotateVRadians( float angle, const Vector &axis );
   friend MATRIXEXPORT Matrix34 Rotatef( float angle, float x, float y, float z );
   friend MATRIXEXPORT Matrix34 Rotate2V( const Vector &from, const Vector &to );
   friend MATRIXEXPORT Matrix34 XRotateRadians( float anglerad );
   friend MATRIXEXPORT Matrix34 YRotateRadians( float anglerad );
   friend MATRIXEXPORT Matrix34 ZRotateRadians( float anglerad );
   friend MATRIXEXPORT Matrix34 YXRotateRadians( float yangle, float xangle );
   friend MATRIXEXPORT Matrix34 YXZRotateRadians( float yangle, float xangle, float zangle );
   friend /* inline */ Matrix34 YXZRotate( float yangle, float xangle, float zangle );
   friend MATRIXEXPORT Matrix34 XZYRotate( float xangle, float zangle, float yangle );
   friend MATRIXEXPORT Matrix34 VectorsToStandard( const Vector &becomezdir, const Vector &becomeydir );
   friend MATRIXEXPORT Matrix34 StandardToVectors( const Vector &newzdir, const Vector &newydir );

   friend /* inline */ Matrix34 RotateV( float angle, const Vector &axis );
   friend /* inline */ Matrix34 XRotate( float angle );
   friend /* inline */ Matrix34 YRotate( float angle );
   friend /* inline */ Matrix34 ZRotate( float angle );
   
   // CONVERSION FUNCTIONS
   friend MATRIXEXPORT Matrix44 ToMatrix44( const Matrix33 &m );
   friend MATRIXEXPORT Matrix44 ToMatrix44( const Matrix34 &m );
   friend MATRIXEXPORT Matrix33 ToMatrix33( const Matrix34 &m );
   friend MATRIXEXPORT Matrix33 ToMatrix33( const Matrix44 &m );
   friend MATRIXEXPORT Matrix34 ToMatrix34( const Matrix44 &m );
   friend MATRIXEXPORT Matrix34 ToMatrix34( const Matrix33 &m );

   friend MATRIXEXPORT void MatrixTimesScale( Matrix34 &a, const Vector &s );
   friend MATRIXEXPORT void ScaleTimesMatrix( const Vector &s, Matrix34 &a );
   friend MATRIXEXPORT void TwoVectorsToAxisAngle( const Vector &from, const Vector &to, Vector &axis, float &radangle );
   friend MATRIXEXPORT void TransformVectorNoTranslate( const Matrix34 &matrix, Vector &p );
   friend /* inline */ Matrix34 CreateEulerMatrix( const Rotate &rotate );
   friend /* inline */ void TransformVector( const Matrix34 &matrix, Vector &p );
};

// inline friend functions
inline Vector operator * ( const Vector &v, const Matrix34 &m )
{
    return m * v;
}
inline Matrix34 YXZRotate( float yangle, float xangle, float zangle ) 
{
   return YXZRotateRadians( yangle/RADIANS, xangle/RADIANS, zangle/RADIANS ); 
}
inline Matrix34 RotateV( float angle, const Vector &axis )
{
   return RotateVRadians( angle / RADIANS, axis ); 
}
inline Matrix34 XRotate( float angle )
{
   return XRotateRadians( angle / RADIANS ); 
}
inline Matrix34 YRotate( float angle )
{
   return YRotateRadians( angle / RADIANS ); 
}
inline Matrix34 ZRotate( float angle )
{
   return ZRotateRadians( angle / RADIANS ); 
}
inline Matrix34 CreateEulerMatrix( const Rotate &rotate )
{
   return YXZRotate( -rotate.roll, -rotate.swivel, -rotate.tilt ).Transpose(); 
}
inline void TransformVector( const Matrix34 &matrix, Vector &p ) 
{
   p = matrix * p; 
}


// regular inlined functions
inline void Matrix34::Set( const Vector4 &v0, const Vector4 &v1, const Vector4 &v2 )
{
   v[0] = v0;
   v[1] = v1;
   v[2] = v2;
   InitBottomRow();
   SetIdentity(FALSE);
}

inline Matrix34::Matrix34( const Vector4 &v0, const Vector4 &v1, const Vector4 &v2 )
{
   Set( v0, v1, v2 );
}

inline Matrix34::Matrix34( float fillvalue )
{
   v[0] = Vector4( fillvalue );
   v[1] = Vector4( fillvalue );
   v[2] = Vector4( fillvalue );
   SetIdentity( FALSE );
}

inline Matrix34::Matrix34( const RotateVector &rv )
{
   float y = (float)atan2( rv.end.x, rv.end.z );
   float x = (float)atan2( -rv.end.y, (float)sqrt(rv.end.x*rv.end.x + rv.end.z*rv.end.z));
   if ( rv.roll )
      *this = YXZRotateRadians( y, x, rv.roll / RADIANS );
   else
      *this = YXRotateRadians( y, x );
   SetIdentity( FALSE );
}

inline Matrix34::Matrix34( const RotateEuler &re )
{
   *this = YXZRotate( re.y, re.x, re.z );
   SetIdentity( FALSE );
}

inline Matrix34 Identity34( void )
{
   static Matrix34 identity34(
                    Vector4( 1.f, 0.f, 0.f, 0.f ),
                    Vector4( 0.f, 1.f, 0.f, 0.f ),
                    Vector4( 0.f, 0.f, 1.f, 0.f ));
   identity34.SetIdentity( TRUE );
   identity34.InitBottomRow();
   return identity34;
}                    

inline void Matrix34::SetTranslate( const Vector &t )
{
   v[0][3] = t.x;
   v[1][3] = t.y;
   v[2][3] = t.z;
   SetIdentity( FALSE );
}

inline Vector Matrix34::GetTranslate( void ) const
{
   return Vector( v[0][3], v[1][3], v[2][3] );
}

inline Matrix34 Matrix34::NoTranslate( void ) const
{
   return Matrix34( Vector4( v[0][0], v[0][1], v[0][2], 0.f ),
                    Vector4( v[1][0], v[1][1], v[1][2], 0.f ),
                    Vector4( v[2][0], v[2][1], v[2][2], 0.f ));
}

inline Matrix34 Matrix34::GetScaleMatrix( void ) const
{
   return Matrix34((Quaternion(*this).Inverse())) * NoTranslate();
}

inline Vector Matrix34::GetScaleVector( void ) const
{
   Matrix34 scalem = GetScaleMatrix();
   return Vector(scalem[0][0], scalem[1][1], scalem[2][2]);
}

inline Vector &Vector::operator *= ( const Matrix34 &m )
{
   *this =  m * *this;
   return *this;
}

// ------------ CONVERSION TO AND FROM OTHER TYPES -------------------

inline Matrix44 ToMatrix44( const Matrix34 &m )
{
   return Matrix44( m[0],
                    m[1],
                    m[2],
           Vector4( 0.f, 0.f, 0.f, 1.f ));
}

inline Matrix44 ToMatrix44( const Matrix33 &m )
{
   return Matrix44(
    Vector4( m[0][0], m[0][1], m[0][2], 0.f ),
    Vector4( m[1][0], m[1][1], m[1][2], 0.f ),
    Vector4( m[2][0], m[2][1], m[2][2], 0.f ),
    Vector4( 0.f,     0.f,     0.f,     1.f ));
}

inline Matrix33 ToMatrix33( const Matrix34 &m )
{
   return Matrix33( Vector( m[0][0], m[0][1], m[0][2] ),
                    Vector( m[1][0], m[1][1], m[1][2] ),
                    Vector( m[2][0], m[2][1], m[2][2] ));
}

inline Matrix33 ToMatrix33( const Matrix44 &m )
{
   return Matrix33( Vector( m[0][0], m[0][1], m[0][2] ),
                    Vector( m[1][0], m[1][1], m[1][2] ),
                    Vector( m[2][0], m[2][1], m[2][2] ));
}

inline Matrix34 ToMatrix34( const Matrix44 &m )
{
   return Matrix34( m[0],
                    m[1],
                    m[2] );
}

inline Matrix34 ToMatrix34( const Matrix33 &m )
{
   return Matrix34( Vector4( m[0][0], m[0][1], m[0][2], 0.f ),
                    Vector4( m[1][0], m[1][1], m[1][2], 0.f ),
                    Vector4( m[2][0], m[2][1], m[2][2], 0.f ));
}

inline Matrix34 VectorsToStandard( const Vector &becomezdir, const Vector &becomeydir )
{
   Matrix34 vectostandard = Rotate2V( becomezdir, Vector( 0.0f, 0.0f, 1.0f ));
   Vector   rollvec = vectostandard * becomeydir;
   float    roll = (float)atan2( -rollvec.x, rollvec.y );
   if ( roll )
      vectostandard = ZRotateRadians( -roll ) * vectostandard;
   return vectostandard;  // may be faster to just compute like below, but the transpose
}

inline Matrix34 StandardToVectors( const Vector &newzdir, const Vector &newydir )
{
   Vector xdir, ydir, zdir;
   ydir = newydir;
   ydir.Normalize();
   zdir = newzdir;
   zdir.Normalize();
   xdir = Cross( ydir, zdir );
   float xlength = xdir.Norm();
   if (xlength!=1.0f) {
      xdir /= xlength;
      ydir = Cross( zdir, xdir );
   }
   return Matrix34( Vector4(xdir[0], ydir[0], zdir[0], 0),
                    Vector4(xdir[0], ydir[0], zdir[0], 0),
                    Vector4(xdir[0], ydir[0], zdir[0], 0));
}

// -------------------- GL STYLE CREATION FUNCTIONS ----------------------

inline Matrix34 Translatef( float x, float y, float z )
{
   return Matrix34( Vector4( 1.f, 0.f, 0.f, x ),
                    Vector4( 0.f, 1.f, 0.f, y ),
                    Vector4( 0.f, 0.f, 1.f, z ));
}

inline Matrix34 TranslateV( const Vector &v )
{
   return Translatef( v[0], v[1], v[2] );
}

inline Matrix34 Scalef( float x, float y, float z )
{
   return Matrix34( Vector4(   x, 0.f, 0.f, 0.f ),
                    Vector4( 0.f,   y, 0.f, 0.f ),
                    Vector4( 0.f, 0.f,   z, 0.f ));
}

inline Matrix34 ScaleV( const Vector &v )
{
   return Scalef( v[0], v[1], v[2] );
}

inline Matrix34 Rotatef( float angle, float x, float y, float z )
{
   return RotateV( angle, Vector( x, y, z ));
}

inline void TransformVectorNoTranslate( const Matrix34 &m, Vector &v )  // CANDIDATE FOR SML
{
   v.Set( m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z, 
          m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z,
          m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z );
}

inline Matrix34 XRotateRadians( float anglerad )
{
   float ca = (float)cos( anglerad ),
         sa = (float)sin( anglerad );
   return Matrix34( Vector4( 1.f, 0.f, 0.f, 0.f ),
                    Vector4( 0.f,  ca, -sa, 0.f ),
                    Vector4( 0.f,  sa,  ca, 0.f ));
}

inline Matrix34 YRotateRadians( float anglerad )
{
   float ca = (float)cos( anglerad ),
         sa = (float)sin( anglerad );
   return Matrix34( Vector4(  ca, 0.f,  sa, 0.f ),
                    Vector4( 0.f, 1.f, 0.f, 0.f ),
                    Vector4( -sa, 0.f,  ca, 0.f ));
}

inline Matrix34 ZRotateRadians( float anglerad )
{
   float ca = (float)cos( anglerad ),
         sa = (float)sin( anglerad );
   return Matrix34( Vector4(  ca, -sa, 0.f, 0.f ),
                    Vector4(  sa,  ca, 0.f, 0.f ),
                    Vector4( 0.f, 0.f, 1.f, 0.f ));
}

// ROTATION CONVERSIONS THAT REQUIRE MATRIX34

inline RotateVector::RotateVector( const Matrix34 &matrix )
{
   end = matrix.GetZAxis();
   float y = (float)atan2( end.x, end.z );
   float x = (float)atan2( -end.y, (float)sqrt(end.x*end.x + end.z*end.z));
   Vector rolldir = matrix.GetYAxis();
   rolldir *= YXRotateRadians( y, x ).Transpose();
   roll = (float)atan2( -rolldir.x, rolldir.y ) * RADIANS;
}

inline RotateEuler::RotateEuler( const Matrix34 &matrix )
{
   Vector end = matrix.GetZAxis();
   y = (float)atan2( end.x, end.z );
   x = (float)atan2( -end.y, (float)sqrt(end.x*end.x + end.z*end.z));
   Vector rolldir = matrix.GetYAxis();
   rolldir *= YXRotateRadians( y, x ).Transpose();
   z = (float)atan2( -rolldir.x, rolldir.y ) * RADIANS;
   x *= RADIANS;
   y *= RADIANS;
}

#ifdef _SIMD

inline Matrix34::Matrix34( const Matrix34 &m ) : m_smlmatrix34f( m.m_smlmatrix34f )
{
}

inline Matrix34 Matrix34::operator * ( const Matrix34 &b ) const 
{
   Matrix34 result;
   result.m_smlmatrix34f.Multiply(m_smlmatrix34f, b.m_smlmatrix34f);
   return result;
}

inline Vector Matrix34::operator * ( const Vector &v ) const
{
   Vector result;
   m_smlmatrix34f.TransformPoint( v.m_smlvec3f, result.m_smlvec3f );
   return result;
}

inline Matrix34 Matrix34::Transpose( void ) const
{
   Matrix34 temp( *this );
   temp.m_smlmatrix34f.Transpose();
   temp.InitBottomRow();
   return temp;
}

inline Matrix34 Matrix34::Inverse( void ) const
{
   Matrix34 result, source=*this;
   result.m_smlmatrix34f.Invert(source.m_smlmatrix34f);  // BUG IN SML, SOURCE SHOULD BE DECLARED CONST
   result.InitBottomRow();
   return result;
}      

#else   // not SIMD

inline Matrix34::Matrix34( const Matrix34 &m )
{
   *this = m;
}

inline Vector Matrix34::operator * ( const Vector &vec ) const
{
   return Vector( v[0]|vec, v[1]|vec, v[2]|vec );
}

inline Matrix34 Matrix34::Transpose( void ) const
{
   return Matrix34( Vector4( v[0][0], v[1][0], v[2][0], 0.f ),
                    Vector4( v[0][1], v[1][1], v[2][1], 0.f ),
                    Vector4( v[0][2], v[1][2], v[2][2], 0.f ));
}

inline Matrix34 Matrix34::Inverse( void ) const
{
   Matrix44 a = ToMatrix44( *this );
   return ToMatrix34( a.Inverse() );
}      

#endif  // not SIMD

#endif  // __MATRIX34_H

---