GenericMathTemplateLibrary: Generate.h Source File

Go to the documentation of this file.
00001 // GMTL is (C) Copyright 2001-2010 by Allen Bierbaum
00002 // Distributed under the GNU Lesser General Public License 2.1 with an
00003 // addendum covering inlined code. (See accompanying files LICENSE and
00004 // LICENSE.addendum or http://www.gnu.org/copyleft/lesser.txt)
00005 
00006 #ifndef _GMTL_GENERATE_H_
00007 #define _GMTL_GENERATE_H_
00008 
00009 #include <gmtl/Defines.h>
00010 #include <gmtl/Util/Assert.h>
00011 #include <gmtl/Util/StaticAssert.h>
00012 
00013 #include <gmtl/Vec.h>    // for Vec
00014 #include <gmtl/VecOps.h> // for lengthSquared
00015 #include <gmtl/Quat.h>
00016 #include <gmtl/QuatOps.h>
00017 #include <gmtl/Coord.h>
00018 #include <gmtl/Matrix.h>
00019 #include <gmtl/Util/Meta.h>
00020 #include <gmtl/Math.h>
00021 #include <gmtl/Xforms.h>
00022 
00023 #include <gmtl/EulerAngle.h>
00024 #include <gmtl/AxisAngle.h>
00025 
00026 namespace gmtl
00027 {
00036    template <typename DATA_TYPE>
00037    inline Vec<DATA_TYPE, 3> makeVec( const Quat<DATA_TYPE>& quat )
00038    {
00039       return Vec<DATA_TYPE, 3>( quat[Xelt], quat[Yelt], quat[Zelt] );
00040    }
00041 
00044    template <typename DATA_TYPE, unsigned SIZE>
00045    inline Vec<DATA_TYPE, SIZE> makeNormal( Vec<DATA_TYPE, SIZE> vec )
00046    {
00047       normalize( vec );
00048       return vec;
00049    }
00050 
00063    template<class DATA_TYPE>
00064    Vec<DATA_TYPE,3> makeCross(const Vec<DATA_TYPE, 3>& v1,
00065                               const Vec<DATA_TYPE, 3>& v2)
00066    {
00067       return Vec<DATA_TYPE,3>( ((v1[Yelt]*v2[Zelt]) - (v1[Zelt]*v2[Yelt])),
00068                                ((v1[Zelt]*v2[Xelt]) - (v1[Xelt]*v2[Zelt])),
00069                                ((v1[Xelt]*v2[Yelt]) - (v1[Yelt]*v2[Xelt])) );
00070    }
00071 
00081    template<typename VEC_TYPE, typename DATA_TYPE, unsigned ROWS, unsigned COLS >
00082    inline VEC_TYPE& setTrans( VEC_TYPE& result, const Matrix<DATA_TYPE, ROWS, COLS>& arg )
00083    {
00084       // ASSERT: There are as many
00085 
00086       // if n x n   then (homogeneous case) vecsize == rows-1 or (scale component case) vecsize == rows
00087       // if n x n+1 then (homogeneous case) vecsize == rows   or (scale component case) vecsize == rows+1
00088       gmtlASSERT( ((ROWS == COLS && ( VEC_TYPE::Size == (ROWS-1) ||  VEC_TYPE::Size == ROWS)) ||
00089                (COLS == (ROWS+1) && ( VEC_TYPE::Size == ROWS ||  VEC_TYPE::Size == (ROWS+1)))) &&
00090               "preconditions not met for vector size in call to makeTrans.  Read your documentation." );
00091 
00092       // homogeneous case...
00093       if ((ROWS == COLS &&  VEC_TYPE::Size == ROWS)              // Square matrix and vec so assume homogeneous vector. ex. 4x4 with vec 4
00094           || (COLS == (ROWS+1) &&  VEC_TYPE::Size == (ROWS+1)))  // ex: 3x4 with vec4
00095       {
00096          result[VEC_TYPE::Size-1] = 1.0f;
00097       }
00098 
00099       // non-homogeneous case... (SIZE == ROWS),
00100       //else
00101       //{}
00102 
00103       for (unsigned x = 0; x < COLS - 1; ++x)
00104       {
00105          result[x] = arg( x, COLS - 1 );
00106       }
00107 
00108       return result;
00109    }
00110 
00124    template <typename DATA_TYPE>
00125    inline Quat<DATA_TYPE>& setPure( Quat<DATA_TYPE>& quat, const Vec<DATA_TYPE, 3>& vec )
00126    {
00127       quat.set( vec[0], vec[1], vec[2], 0 );
00128       return quat;
00129    }
00130 
00137    template <typename DATA_TYPE>
00138    inline Quat<DATA_TYPE> makePure( const Vec<DATA_TYPE, 3>& vec )
00139    {
00140       return Quat<DATA_TYPE>( vec[0], vec[1], vec[2], 0 );
00141    }
00142 
00147    template <typename DATA_TYPE>
00148    inline Quat<DATA_TYPE> makeNormal( const Quat<DATA_TYPE>& quat )
00149    {
00150       Quat<DATA_TYPE> temporary( quat );
00151       return normalize( temporary );
00152    }
00153 
00160    template <typename DATA_TYPE>
00161    inline Quat<DATA_TYPE> makeConj( const Quat<DATA_TYPE>& quat )
00162    {
00163       Quat<DATA_TYPE> temporary( quat );
00164       return conj( temporary );
00165    }
00166 
00172    template <typename DATA_TYPE>
00173    inline Quat<DATA_TYPE> makeInvert( const Quat<DATA_TYPE>& quat )
00174    {
00175       Quat<DATA_TYPE> temporary( quat );
00176       return invert( temporary );
00177    }
00178 
00183    template <typename DATA_TYPE>
00184    inline Quat<DATA_TYPE>& set( Quat<DATA_TYPE>& result, const AxisAngle<DATA_TYPE>& axisAngle )
00185    {
00186       gmtlASSERT( (Math::isEqual( lengthSquared( axisAngle.getAxis() ), (DATA_TYPE)1.0, (DATA_TYPE)0.0001 )) &&
00187                    "you must pass in a normalized vector to setRot( quat, rad, vec )" );
00188 
00189       DATA_TYPE half_angle = axisAngle.getAngle() * (DATA_TYPE)0.5;
00190       DATA_TYPE sin_half_angle = Math::sin( half_angle );
00191 
00192       result[Welt] = Math::cos( half_angle );
00193       result[Xelt] = sin_half_angle * axisAngle.getAxis()[0];
00194       result[Yelt] = sin_half_angle * axisAngle.getAxis()[1];
00195       result[Zelt] = sin_half_angle * axisAngle.getAxis()[2];
00196 
00197       // should automagically be normalized (unit magnitude) now...
00198       return result;
00199    }
00200 
00204    template <typename DATA_TYPE>
00205    inline Quat<DATA_TYPE>& setRot( Quat<DATA_TYPE>& result, const AxisAngle<DATA_TYPE>& axisAngle )
00206    {
00207       return gmtl::set( result, axisAngle );
00208    }
00209 
00214    template <typename DATA_TYPE, typename ROT_ORDER>
00215    inline Quat<DATA_TYPE>& set( Quat<DATA_TYPE>& result, const EulerAngle<DATA_TYPE, ROT_ORDER>& euler )
00216    {
00217       // this might be faster if put into the switch statement... (testme)
00218       const int& order = ROT_ORDER::ID;
00219       const DATA_TYPE xRot = (order == XYZ::ID) ? euler[0] : ((order == ZXY::ID) ? euler[1] : euler[2]);
00220       const DATA_TYPE yRot = (order == XYZ::ID) ? euler[1] : ((order == ZXY::ID) ? euler[2] : euler[1]);
00221       const DATA_TYPE zRot = (order == XYZ::ID) ? euler[2] : ((order == ZXY::ID) ? euler[0] : euler[0]);
00222 
00223       // this could be written better for each rotation order, but this is really general...
00224       Quat<DATA_TYPE> qx, qy, qz;
00225 
00226       // precompute half angles
00227       DATA_TYPE xOver2 = xRot * (DATA_TYPE)0.5;
00228       DATA_TYPE yOver2 = yRot * (DATA_TYPE)0.5;
00229       DATA_TYPE zOver2 = zRot * (DATA_TYPE)0.5;
00230 
00231       // set the pitch quat
00232       qx[Xelt] = Math::sin( xOver2 );
00233       qx[Yelt] = (DATA_TYPE)0.0;
00234       qx[Zelt] = (DATA_TYPE)0.0;
00235       qx[Welt] = Math::cos( xOver2 );
00236 
00237       // set the yaw quat
00238       qy[Xelt] = (DATA_TYPE)0.0;
00239       qy[Yelt] = Math::sin( yOver2 );
00240       qy[Zelt] = (DATA_TYPE)0.0;
00241       qy[Welt] = Math::cos( yOver2 );
00242 
00243       // set the roll quat
00244       qz[Xelt] = (DATA_TYPE)0.0;
00245       qz[Yelt] = (DATA_TYPE)0.0;
00246       qz[Zelt] = Math::sin( zOver2 );
00247       qz[Welt] = Math::cos( zOver2 );
00248 
00249       // compose the three in pyr order...
00250       switch (order)
00251       {
00252       case XYZ::ID: result = qx * qy * qz; break;
00253       case ZYX::ID: result = qz * qy * qx; break;
00254       case ZXY::ID: result = qz * qx * qy; break;
00255       default:
00256          gmtlASSERT( false && "unknown rotation order passed to setRot" );
00257          break;
00258       }
00259 
00260       // ensure the quaternion is normalized
00261       normalize( result );
00262       return result;
00263    }
00264 
00268    template <typename DATA_TYPE, typename ROT_ORDER>
00269    inline Quat<DATA_TYPE>& setRot( Quat<DATA_TYPE>& result, const EulerAngle<DATA_TYPE, ROT_ORDER>& euler )
00270    {
00271       return gmtl::set( result, euler );
00272    }
00273 
00278    template <typename DATA_TYPE, unsigned ROWS, unsigned COLS>
00279    inline Quat<DATA_TYPE>& set( Quat<DATA_TYPE>& quat, const Matrix<DATA_TYPE, ROWS, COLS>& mat  )
00280    {
00281       gmtlASSERT( ((ROWS == 3 && COLS == 3) ||
00282                (ROWS == 3 && COLS == 4) ||
00283                (ROWS == 4 && COLS == 3) ||
00284                (ROWS == 4 && COLS == 4)) &&
00285                "pre conditions not met on set( quat, pos, mat ) which only sets a quaternion to the rotation part of a 3x3, 3x4, 4x3, or 4x4 matrix." );
00286 
00287       DATA_TYPE tr( mat( 0, 0 ) + mat( 1, 1 ) + mat( 2, 2 ) ), s( 0.0f );
00288 
00289       // If diagonal is positive
00290       if (tr > (DATA_TYPE)0.0)
00291       {
00292          s = Math::sqrt( tr + (DATA_TYPE)1.0 );
00293          quat[Welt] = s * (DATA_TYPE)0.5;
00294          s = DATA_TYPE(0.5) / s;
00295 
00296          quat[Xelt] = (mat( 2, 1 ) - mat( 1, 2 )) * s;
00297          quat[Yelt] = (mat( 0, 2 ) - mat( 2, 0 )) * s;
00298          quat[Zelt] = (mat( 1, 0 ) - mat( 0, 1 )) * s;
00299       }
00300 
00301       // when Diagonal is negative
00302       else
00303       {
00304          static const unsigned int nxt[3] = { 1, 2, 0 };
00305          unsigned int i( Xelt ), j, k;
00306 
00307          if (mat( 1, 1 ) > mat( 0, 0 ))
00308             i = 1;
00309 
00310          if (mat( 2, 2 ) > mat( i, i ))
00311             i = 2;
00312 
00313          j = nxt[i];
00314          k = nxt[j];
00315 
00316          s = Math::sqrt( (mat( i, i )-(mat( j, j )+mat( k, k ))) + (DATA_TYPE)1.0 );
00317 
00318          DATA_TYPE q[4];
00319          q[i] = s * (DATA_TYPE)0.5;
00320 
00321          if (s != (DATA_TYPE)0.0)
00322             s = DATA_TYPE(0.5) / s;
00323 
00324          q[3] = (mat( k, j ) - mat( j, k )) * s;
00325          q[j] = (mat( j, i ) + mat( i, j )) * s;
00326          q[k] = (mat( k, i ) + mat( i, k )) * s;
00327 
00328          quat[Xelt] = q[0];
00329          quat[Yelt] = q[1];
00330          quat[Zelt] = q[2];
00331          quat[Welt] = q[3];
00332       }
00333 
00334       return quat;
00335    }
00336 
00340    template <typename DATA_TYPE, unsigned ROWS, unsigned COLS>
00341    inline Quat<DATA_TYPE>& setRot( Quat<DATA_TYPE>& result, const Matrix<DATA_TYPE, ROWS, COLS>& mat )
00342    {
00343       return gmtl::set( result, mat );
00344    }
00362    template <typename DATA_TYPE>
00363    inline AxisAngle<DATA_TYPE>& set( AxisAngle<DATA_TYPE>& axisAngle, Quat<DATA_TYPE> quat )
00364    {
00365       // set sure we don't get a NaN result from acos...
00366       if (Math::abs( quat[Welt] ) > (DATA_TYPE)1.0)
00367       {
00368          gmtl::normalize( quat );
00369       }
00370       gmtlASSERT( Math::abs( quat[Welt] ) <= (DATA_TYPE)1.0 && "acos returns NaN when quat[Welt] > 1, try normalizing your quat." );
00371 
00372       // [acos( w ) * 2.0, v / (asin( w ) * 2.0)]
00373 
00374       // set the angle - aCos is mathematically defined to be between 0 and PI
00375       DATA_TYPE rad = Math::aCos( quat[Welt] ) * (DATA_TYPE)2.0;
00376       axisAngle.setAngle( rad );
00377 
00378       // set the axis: (use sin(rad) instead of asin(w))
00379       DATA_TYPE sin_half_angle = Math::sin( rad * (DATA_TYPE)0.5 );
00380       if (sin_half_angle >= (DATA_TYPE)0.0001) // because (PI >= rad >= 0)
00381       {
00382          DATA_TYPE sin_half_angle_inv = DATA_TYPE(1.0) / sin_half_angle;
00383          Vec<DATA_TYPE, 3> axis( quat[Xelt] * sin_half_angle_inv,
00384                                  quat[Yelt] * sin_half_angle_inv,
00385                                  quat[Zelt] * sin_half_angle_inv );
00386          normalize( axis );
00387          axisAngle.setAxis( axis );
00388       }
00389 
00390       // avoid NAN
00391       else
00392       {
00393          // one of the terms should be a 1,
00394          // so we can maintain unit-ness
00395          // in case w is 0 (which here w is 0)
00396          axisAngle.setAxis( gmtl::Vec<DATA_TYPE, 3>(
00397                              DATA_TYPE( 1.0 ) /*- gmtl::Math::abs( quat[Welt] )*/,
00398                             (DATA_TYPE)0.0,
00399                             (DATA_TYPE)0.0 ) );
00400       }
00401       return axisAngle;
00402    }
00403 
00407    template <typename DATA_TYPE>
00408    inline AxisAngle<DATA_TYPE>& setRot( AxisAngle<DATA_TYPE>& result, Quat<DATA_TYPE> quat )
00409    {
00410       return gmtl::set( result, quat );
00411    }
00412 
00414    template <typename DATA_TYPE>
00415    AxisAngle<DATA_TYPE> makeNormal( const AxisAngle<DATA_TYPE>& a )
00416    {
00417       return AxisAngle<DATA_TYPE>( a.getAngle(), makeNormal( a.getAxis() ) );
00418    }
00436    template< typename DATA_TYPE, unsigned ROWS, unsigned COLS, typename ROT_ORDER >
00437    inline EulerAngle<DATA_TYPE, ROT_ORDER>& set( EulerAngle<DATA_TYPE, ROT_ORDER>& euler,
00438                     const Matrix<DATA_TYPE, ROWS, COLS>& mat )
00439    {
00440       // @todo set this a compile time assert...
00441       gmtlASSERT( ROWS >= 3 && COLS >= 3 && ROWS <= 4 && COLS <= 4 &&
00442             "this is undefined for Matrix smaller than 3x3 or bigger than 4x4" );
00443 
00444       // @todo metaprogram this!
00445       const int& order = ROT_ORDER::ID;
00446       switch (order)
00447       {
00448       case XYZ::ID:
00449          {
00450 #if 0
00451             euler[2] = Math::aTan2( -mat(0,1), mat(0,0) );       // -(-cy*sz)/(cy*cz) - cy falls out
00452             euler[0] = Math::aTan2( -mat(1,2), mat(2,2) );       // -(sx*cy)/(cx*cy) - cy falls out
00453             DATA_TYPE cz = Math::cos( euler[2] );
00454             euler[1] = Math::aTan2( mat(0,2), mat(0,0) / cz );   // (sy)/((cy*cz)/cz)
00455 #else
00456             DATA_TYPE x(0), y(0), z(0);
00457             y = Math::aSin( mat(0,2));
00458             if (y < gmtl::Math::PI_OVER_2)
00459             {
00460                if(y > -gmtl::Math::PI_OVER_2)
00461                {
00462                   x = Math::aTan2(-mat(1,2),mat(2,2));
00463                   z = Math::aTan2(-mat(0,1),mat(0,0));
00464                }
00465                else  // Non-unique (x - z constant)
00466                {
00467                   x = -Math::aTan2(mat(1,0), mat(1,1));
00468                   z = 0;
00469                }
00470             }
00471             else
00472             {
00473                // not unique (x + z constant)
00474                x = Math::aTan2(mat(1,0), mat(1,1));
00475                z = 0;
00476             }
00477             euler[0] = x;
00478             euler[1] = y;
00479             euler[2] = z;
00480 
00481 #endif
00482          }
00483          break;
00484       case ZYX::ID:
00485          {
00486 #if 0
00487             euler[0] = Math::aTan2( mat(1,0), mat(0,0) );        // (cy*sz)/(cy*cz) - cy falls out
00488             euler[2] = Math::aTan2( mat(2,1), mat(2,2) );        // (sx*cy)/(cx*cy) - cy falls out
00489             DATA_TYPE sx = Math::sin( euler[2] );
00490             euler[1] = Math::aTan2( -mat(2,0), mat(2,1) / sx );  // -(-sy)/((sx*cy)/sx)
00491 #else
00492             DATA_TYPE x(0), y(0), z(0);
00493 
00494             y = Math::aSin(-mat(2,0));
00495             if(y < Math::PI_OVER_2)
00496             {
00497                if(y>-Math::PI_OVER_2)
00498                {
00499                   z = Math::aTan2(mat(1,0), mat(0,0));
00500                   x = Math::aTan2(mat(2,1), mat(2,2));
00501                }
00502                else  // not unique (x + z constant)
00503                {
00504                   z = Math::aTan2(-mat(0,1),-mat(0,2));
00505                   x = 0;
00506                }
00507             }
00508             else  // not unique (x - z constant)
00509             {
00510                z = -Math::aTan2(mat(0,1), mat(0,2));
00511                x = 0;
00512             }
00513             euler[0] = z;
00514             euler[1] = y;
00515             euler[2] = x;
00516 #endif
00517          }
00518          break;
00519       case ZXY::ID:
00520          {
00521 #if 0
00522          // Extract the rotation directly from the matrix
00523             DATA_TYPE x_angle;
00524             DATA_TYPE y_angle;
00525             DATA_TYPE z_angle;
00526             DATA_TYPE cos_y, sin_y;
00527             DATA_TYPE cos_x, sin_x;
00528             DATA_TYPE cos_z, sin_z;
00529 
00530             sin_x = mat(2,1);
00531             x_angle = Math::aSin( sin_x );     // Get x angle
00532             cos_x = Math::cos( x_angle );
00533 
00534             // Check if cos_x = Zero
00535             if (cos_x != 0.0f)     // ASSERT: cos_x != 0
00536             {
00537                   // Get y Angle
00538                cos_y = mat(2,2) / cos_x;
00539                sin_y = -mat(2,0) / cos_x;
00540                y_angle = Math::aTan2( cos_y, sin_y );
00541 
00542                   // Get z Angle
00543                cos_z = mat(1,1) / cos_x;
00544                sin_z = -mat(0,1) / cos_x;
00545                z_angle = Math::aTan2( cos_z, sin_z );
00546             }
00547             else
00548             {
00549                // Arbitrarily set z_angle = 0
00550                z_angle = 0;
00551 
00552                   // Get y Angle
00553                cos_y = mat(0,0);
00554                sin_y = mat(1,0);
00555                y_angle = Math::aTan2( cos_y, sin_y );
00556             }
00557 
00558             euler[1] = x_angle;
00559             euler[2] = y_angle;
00560             euler[0] = z_angle;
00561 #else
00562             DATA_TYPE x,y,z;
00563 
00564             x = Math::aSin(mat(2,1));
00565             if(x < Math::PI_OVER_2)
00566             {
00567                if(x > -Math::PI_OVER_2)
00568                {
00569                   z = Math::aTan2(-mat(0,1), mat(1,1));
00570                   y = Math::aTan2(-mat(2,0), mat(2,2));
00571                }
00572                else  // not unique (y - z constant)
00573                {
00574                   z = -Math::aTan2(mat(0,2), mat(0,0));
00575                   y = 0;
00576                }
00577             }
00578             else  // not unique (y + z constant)
00579             {
00580                z = Math::aTan2(mat(0,2), mat(0,0));
00581                y = 0;
00582             }
00583             euler[0] = z;
00584             euler[1] = x;
00585             euler[2] = y;
00586 #endif
00587          }
00588          break;
00589       default:
00590          gmtlASSERT( false && "unknown rotation order passed to setRot" );
00591          break;
00592       }
00593       return euler;
00594    }
00595 
00599    template< typename DATA_TYPE, unsigned ROWS, unsigned COLS, typename ROT_ORDER >
00600    inline EulerAngle<DATA_TYPE, ROT_ORDER>& setRot( EulerAngle<DATA_TYPE, ROT_ORDER>& result, const Matrix<DATA_TYPE, ROWS, COLS>& mat )
00601    {
00602       return gmtl::set( result, mat );
00603    }
00614    template <typename T >
00615    inline Matrix<T, 4,4>& setFrustum( Matrix<T, 4, 4>& result,
00616                                                    T left, T top, T right,
00617                                                    T bottom, T nr, T fr )
00618    {
00619       result.mData[0] = ( T( 2.0 ) * nr ) / ( right - left );
00620       result.mData[1] = T( 0.0 );
00621       result.mData[2] = T( 0.0 );
00622       result.mData[3] = T( 0.0 );
00623 
00624       result.mData[4] = T( 0.0 );
00625       result.mData[5] = ( T( 2.0 ) * nr ) / ( top - bottom );
00626       result.mData[6] = T( 0.0 );
00627       result.mData[7] = T( 0.0 );
00628 
00629       result.mData[8] = ( right + left ) / ( right - left );
00630       result.mData[9] = ( top + bottom ) / ( top - bottom );
00631       result.mData[10] = -( fr + nr ) / ( fr - nr );
00632       result.mData[11] = T( -1.0 );
00633 
00634       result.mData[12] = T( 0.0 );
00635       result.mData[13] = T( 0.0 );
00636       result.mData[14] = -( T( 2.0 ) * fr * nr ) / ( fr - nr );
00637       result.mData[15] = T( 0.0 );
00638 
00639       result.mState = Matrix<T, 4, 4>::FULL; // track state
00640 
00641       return result;
00642    }
00643 
00650    template <typename T >
00651    inline Matrix<T, 4,4>& setOrtho( Matrix<T,4,4>& result,
00652                                           T left, T top, T right,
00653                                           T bottom, T nr, T fr )
00654    {
00655       result.mData[1] = result.mData[2] = result.mData[3] =
00656       result.mData[4] = result.mData[6] = result.mData[7] =
00657       result.mData[8] = result.mData[9] = result.mData[11] = T(0);
00658 
00659       T rl_inv = T(1) / (right - left);
00660       T tb_inv = T(1) / (top - bottom);
00661       T nf_inv = T(1) / (fr - nr);
00662 
00663       result.mData[0] =  T(2) * rl_inv;
00664       result.mData[5] =  T(2) * tb_inv;
00665       result.mData[10] = -T(2) * nf_inv;
00666 
00667       result.mData[12] = -(right + left) * rl_inv;
00668       result.mData[13] = -(top + bottom) * tb_inv;
00669       result.mData[14] = -(fr + nr) * nf_inv;
00670       result.mData[15] = T(1);
00671 
00672       return result;
00673    }
00674 
00687    template <typename T>
00688    inline Matrix<T, 4,4>& setPerspective( Matrix<T, 4, 4>& result,
00689                                                  T fovy, T aspect, T nr, T fr )
00690    {
00691       gmtlASSERT( nr > 0 && fr > nr && "invalid near and far values" );
00692       T theta = Math::deg2Rad( fovy * T( 0.5 ) );
00693       T tangentTheta = Math::tan( theta );
00694 
00695       // tan(theta) = right / nr
00696       // top = tan(theta) * nr
00697       // right = tan(theta) * nr * aspect
00698 
00699       T top = tangentTheta * nr;
00700       T right = top * aspect; // aspect determines the fieald of view in the x-axis
00701 
00702       // TODO: args need to match...
00703       return setFrustum( result, -right, top, right, -top, nr, fr );
00704    }
00705 
00706 
00707    /*
00708    template< typename DATA_TYPE, unsigned ROWS, unsigned COLS, unsigned SIZE, typename REP >
00709    inline Matrix<DATA_TYPE, ROWS, COLS>& setTrans( Matrix<DATA_TYPE, ROWS, COLS>& result,
00710                                                    const VecBase<DATA_TYPE, SIZE, REP>& trans )
00711    {
00712       const Vec<DATA_TYPE,SIZE,meta::DefaultVecTag> temp_vec(trans);
00713       return setTrans(result,trans);
00714    }
00715    */
00716 
00726 #ifdef GMTL_NO_METAPROG
00727    template< typename DATA_TYPE, unsigned ROWS, unsigned COLS, unsigned SIZE >
00728    inline Matrix<DATA_TYPE, ROWS, COLS>& setTrans( Matrix<DATA_TYPE, ROWS, COLS>& result,
00729                                                    const VecBase<DATA_TYPE, SIZE>& trans )
00730 #else
00731    template< typename DATA_TYPE, unsigned ROWS, unsigned COLS, unsigned SIZE, typename REP >
00732    inline Matrix<DATA_TYPE, ROWS, COLS>& setTrans( Matrix<DATA_TYPE, ROWS, COLS>& result,
00733                                                    const VecBase<DATA_TYPE, SIZE, REP>& trans )
00734 #endif
00735    {
00736       /* @todo make this a compile time assert... */
00737       // if n x n   then (homogeneous case) vecsize == rows-1 or (scale component case) vecsize == rows
00738       // if n x n+1 then (homogeneous case) vecsize == rows   or (scale component case) vecsize == rows+1
00739       gmtlASSERT( ((ROWS == COLS && (SIZE == (ROWS-1) || SIZE == ROWS)) ||
00740                (COLS == (ROWS+1) && (SIZE == ROWS || SIZE == (ROWS+1)))) &&
00741               "preconditions not met for vector size in call to makeTrans.  Read your documentation." );
00742 
00743       // homogeneous case...
00744       if ((ROWS == COLS && SIZE == ROWS) /* Square matrix and vec so assume homogeneous vector. ex. 4x4 with vec 4 */
00745           || (COLS == (ROWS+1) && SIZE == (ROWS+1)))  /* ex: 3x4 with vec4 */
00746       {
00747          DATA_TYPE homog_val = trans[SIZE-1];
00748          for (unsigned x = 0; x < COLS - 1; ++x)
00749             result( x, COLS - 1 ) = trans[x] / homog_val;
00750       }
00751 
00752       // non-homogeneous case...
00753       else
00754       {
00755          for (unsigned x = 0; x < COLS - 1; ++x)
00756             result( x, COLS - 1 ) = trans[x];
00757       }
00758       // track state, only override identity
00759       switch (result.mState)
00760       {
00761       case Matrix<DATA_TYPE, ROWS, COLS>::ORTHOGONAL: result.mState = Matrix<DATA_TYPE, ROWS, COLS>::AFFINE; break;
00762       case Matrix<DATA_TYPE, ROWS, COLS>::IDENTITY:   result.mState = Matrix<DATA_TYPE, ROWS, COLS>::TRANS; break;
00763       }
00764       return result;
00765    }
00766 
00769    template< typename DATA_TYPE, unsigned ROWS, unsigned COLS, unsigned SIZE >
00770    inline Matrix<DATA_TYPE, ROWS, COLS>& setScale( Matrix<DATA_TYPE, ROWS, COLS>& result, const Vec<DATA_TYPE, SIZE>& scale )
00771    {
00772       gmtlASSERT( ((SIZE == (ROWS-1) && SIZE == (COLS-1)) || (SIZE == (ROWS-1) && SIZE == COLS) || (SIZE == (COLS-1) && SIZE == ROWS)) && "the scale params must fit within the matrix, check your sizes." );
00773       for (unsigned x = 0; x < SIZE; ++x)
00774       {
00775          result( x, x ) = scale[x];
00776       }
00777       // track state: affine matrix with non-uniform scale now.
00778       result.mState = Matrix<DATA_TYPE, ROWS, COLS>::AFFINE;
00779       result.mState |= Matrix<DATA_TYPE, ROWS, COLS>::NON_UNISCALE;
00780       return result;
00781    }
00782 
00785    template< typename DATA_TYPE, unsigned ROWS, unsigned COLS >
00786    inline Matrix<DATA_TYPE, ROWS, COLS>& setScale( Matrix<DATA_TYPE, ROWS, COLS>& result, const DATA_TYPE scale )
00787    {
00788       for (unsigned x = 0; x < Math::Min( ROWS, COLS, Math::Max( ROWS, COLS ) - 1 ); ++x) // account for 2x4 or other weird sizes...
00789       {
00790          result( x, x ) = scale;
00791       }
00792       // track state: affine matrix with non-uniform scale now.
00793       result.mState = Matrix<DATA_TYPE, ROWS, COLS>::AFFINE;
00794       result.mState |= Matrix<DATA_TYPE, ROWS, COLS>::NON_UNISCALE;
00795       return result;
00796    }
00797 
00802    template <typename MATRIX_TYPE, typename INPUT_TYPE>
00803    inline MATRIX_TYPE makeScale( const INPUT_TYPE& scale,
00804                                Type2Type< MATRIX_TYPE > t = Type2Type< MATRIX_TYPE >() )
00805    {
00806       gmtl::ignore_unused_variable_warning(t);
00807       MATRIX_TYPE temporary;
00808       return setScale( temporary, scale );
00809    }
00810 
00816    template< typename DATA_TYPE, unsigned ROWS, unsigned COLS >
00817    inline Matrix<DATA_TYPE, ROWS, COLS>& setRot( Matrix<DATA_TYPE, ROWS, COLS>& result, const AxisAngle<DATA_TYPE>& axisAngle )
00818    {
00819       /* @todo set this a compile time assert... */
00820       gmtlASSERT( ROWS >= 3 && COLS >= 3 && ROWS <= 4 && COLS <= 4 &&
00821                      "this func is undefined for Matrix smaller than 3x3 or bigger than 4x4" );
00822       gmtlASSERT( Math::isEqual( lengthSquared( axisAngle.getAxis() ), (DATA_TYPE)1.0, (DATA_TYPE)0.001 ) /* &&
00823                      "you must pass in a normalized vector to setRot( mat, rad, vec )" */ );
00824 
00825       // GGI: pg 466
00826       DATA_TYPE s = Math::sin( axisAngle.getAngle() );
00827       DATA_TYPE c = Math::cos( axisAngle.getAngle() );
00828       DATA_TYPE t = DATA_TYPE( 1.0 ) - c;
00829       DATA_TYPE x = axisAngle.getAxis()[0];
00830       DATA_TYPE y = axisAngle.getAxis()[1];
00831       DATA_TYPE z = axisAngle.getAxis()[2];
00832 
00833       /* From: Introduction to robotic.  Craig.  Pg. 52 */
00834       result( 0, 0 ) = (t*x*x)+c;     result( 0, 1 ) = (t*x*y)-(s*z); result( 0, 2 ) = (t*x*z)+(s*y);
00835       result( 1, 0 ) = (t*x*y)+(s*z); result( 1, 1 ) = (t*y*y)+c;     result( 1, 2 ) = (t*y*z)-(s*x);
00836       result( 2, 0 ) = (t*x*z)-(s*y); result( 2, 1 ) = (t*y*z)+(s*x); result( 2, 2 ) = (t*z*z)+c;
00837 
00838       // track state
00839       switch (result.mState)
00840       {
00841       case Matrix<DATA_TYPE, ROWS, COLS>::TRANS:    result.mState = Matrix<DATA_TYPE, ROWS, COLS>::AFFINE; break;
00842       case Matrix<DATA_TYPE, ROWS, COLS>::IDENTITY: result.mState = Matrix<DATA_TYPE, ROWS, COLS>::ORTHOGONAL; break;
00843       }
00844       return result;
00845    }
00846 
00851    template< typename DATA_TYPE, unsigned ROWS, unsigned COLS, typename ROT_ORDER >
00852    inline Matrix<DATA_TYPE, ROWS, COLS>& setRot( Matrix<DATA_TYPE, ROWS, COLS>& result, const EulerAngle<DATA_TYPE, ROT_ORDER>& euler )
00853    {
00854       // @todo set this a compile time assert...
00855       gmtlASSERT( ROWS >= 3 && COLS >= 3 && ROWS <= 4 && COLS <= 4 && "this is undefined for Matrix smaller than 3x3 or bigger than 4x4" );
00856 
00857       // this might be faster if put into the switch statement... (testme)
00858       const int& order = ROT_ORDER::ID;
00859       const DATA_TYPE xRot = (order == XYZ::ID) ? euler[0] : ((order == ZXY::ID) ? euler[1] : euler[2]);
00860       const DATA_TYPE yRot = (order == XYZ::ID) ? euler[1] : ((order == ZXY::ID) ? euler[2] : euler[1]);
00861       const DATA_TYPE zRot = (order == XYZ::ID) ? euler[2] : ((order == ZXY::ID) ? euler[0] : euler[0]);
00862 
00863       DATA_TYPE sx = Math::sin( xRot );  DATA_TYPE cx = Math::cos( xRot );
00864       DATA_TYPE sy = Math::sin( yRot );  DATA_TYPE cy = Math::cos( yRot );
00865       DATA_TYPE sz = Math::sin( zRot );  DATA_TYPE cz = Math::cos( zRot );
00866 
00867       // @todo metaprogram this!
00868       switch (order)
00869       {
00870       case XYZ::ID:
00871          // Derived by simply multiplying out the matrices by hand X * Y * Z
00872          result( 0, 0 ) = cy*cz;             result( 0, 1 ) = -cy*sz;            result( 0, 2 ) = sy;
00873          result( 1, 0 ) = sx*sy*cz + cx*sz;  result( 1, 1 ) = -sx*sy*sz + cx*cz; result( 1, 2 ) = -sx*cy;
00874          result( 2, 0 ) = -cx*sy*cz + sx*sz; result( 2, 1 ) = cx*sy*sz + sx*cz;  result( 2, 2 ) = cx*cy;
00875          break;
00876       case ZYX::ID:
00877          // Derived by simply multiplying out the matrices by hand Z * Y * Z
00878          result( 0, 0 ) = cy*cz; result( 0, 1 ) = -cx*sz + sx*sy*cz; result( 0, 2 ) = sx*sz + cx*sy*cz;
00879          result( 1, 0 ) = cy*sz; result( 1, 1 ) = cx*cz + sx*sy*sz;  result( 1, 2 ) = -sx*cz + cx*sy*sz;
00880          result( 2, 0 ) = -sy;   result( 2, 1 ) = sx*cy;             result( 2, 2 ) = cx*cy;
00881          break;
00882       case ZXY::ID:
00883          // Derived by simply multiplying out the matrices by hand Z * X * Y
00884          result( 0, 0 ) = cy*cz - sx*sy*sz; result( 0, 1 ) = -cx*sz; result( 0, 2 ) = sy*cz + sx*cy*sz;
00885          result( 1, 0 ) = cy*sz + sx*sy*cz; result( 1, 1 ) = cx*cz;  result( 1, 2 ) = sy*sz - sx*cy*cz;
00886          result( 2, 0 ) = -cx*sy;           result( 2, 1 ) = sx;     result( 2, 2 ) = cx*cy;
00887          break;
00888       default:
00889          gmtlASSERT( false && "unknown rotation order passed to setRot" );
00890          break;
00891       }
00892 
00893       // track state
00894       switch (result.mState)
00895       {
00896       case Matrix<DATA_TYPE, ROWS, COLS>::TRANS:    result.mState = Matrix<DATA_TYPE, ROWS, COLS>::AFFINE; break;
00897       case Matrix<DATA_TYPE, ROWS, COLS>::IDENTITY: result.mState = Matrix<DATA_TYPE, ROWS, COLS>::ORTHOGONAL; break;
00898       }
00899       return result;
00900    }
00901 
00906    template< typename DATA_TYPE, unsigned ROWS, unsigned COLS >
00907    inline Matrix<DATA_TYPE, ROWS, COLS>& set( Matrix<DATA_TYPE, ROWS, COLS>& result, const AxisAngle<DATA_TYPE>& axisAngle )
00908    {
00909       gmtl::identity( result );
00910       return setRot( result, axisAngle );
00911    }
00912 
00916    template< typename DATA_TYPE, unsigned ROWS, unsigned COLS, typename ROT_ORDER >
00917    inline Matrix<DATA_TYPE, ROWS, COLS>& set( Matrix<DATA_TYPE, ROWS, COLS>& result, const EulerAngle<DATA_TYPE, ROT_ORDER>& euler )
00918    {
00919       gmtl::identity( result );
00920       return setRot( result, euler );
00921    }
00922 
00927    template< typename DATA_TYPE, unsigned ROWS, unsigned COLS >
00928    inline DATA_TYPE makeYRot( const Matrix<DATA_TYPE, ROWS, COLS>& mat )
00929    {
00930       const gmtl::Vec<DATA_TYPE, 3> forward_point(0.0f, 0.0f, -1.0f);   // -Z
00931       const gmtl::Vec<DATA_TYPE, 3> origin_point(0.0f, 0.0f, 0.0f);
00932       gmtl::Vec<DATA_TYPE, 3> end_point, start_point;
00933 
00934       gmtl::xform(end_point, mat, forward_point);
00935       gmtl::xform(start_point, mat, origin_point);
00936       gmtl::Vec<DATA_TYPE, 3> direction_vector = end_point - start_point;
00937 
00938       // Constrain the direction to XZ-plane only.
00939       direction_vector[1] = 0.0f;                  // Eliminate Y value
00940       gmtl::normalize(direction_vector);
00941       DATA_TYPE y_rot = gmtl::Math::aCos(gmtl::dot(direction_vector,
00942                                                    forward_point));
00943 
00944       gmtl::Vec<DATA_TYPE, 3> which_side = gmtl::makeCross(forward_point,
00945                                                            direction_vector);
00946 
00947       // If direction vector to "right" (negative) side of forward
00948       if ( which_side[1] < 0.0f )
00949       {
00950          y_rot = -y_rot;
00951       }
00952 
00953       return y_rot;
00954    }
00955 
00960    template< typename DATA_TYPE, unsigned ROWS, unsigned COLS >
00961    inline DATA_TYPE makeXRot( const Matrix<DATA_TYPE, ROWS, COLS>& mat )
00962    {
00963       const gmtl::Vec<DATA_TYPE, 3> forward_point(0.0f, 0.0f, -1.0f);   // -Z
00964       const gmtl::Vec<DATA_TYPE, 3> origin_point(0.0f, 0.0f, 0.0f);
00965       gmtl::Vec<DATA_TYPE, 3> end_point, start_point;
00966 
00967       gmtl::xform(end_point, mat, forward_point);
00968       gmtl::xform(start_point, mat, origin_point);
00969       gmtl::Vec<DATA_TYPE, 3> direction_vector = end_point - start_point;
00970 
00971       // Constrain the direction to YZ-plane only.
00972       direction_vector[0] = 0.0f;                  // Eliminate X value
00973       gmtl::normalize(direction_vector);
00974       DATA_TYPE x_rot = gmtl::Math::aCos(gmtl::dot(direction_vector,
00975                                                    forward_point));
00976 
00977       gmtl::Vec<DATA_TYPE, 3> which_side = gmtl::makeCross(forward_point,
00978                                                            direction_vector);
00979 
00980       // If direction vector to "bottom" (negative) side of forward
00981       if ( which_side[0] < 0.0f )
00982       {
00983          x_rot = -x_rot;
00984       }
00985 
00986       return x_rot;
00987    }
00988 
00993    template< typename DATA_TYPE, unsigned ROWS, unsigned COLS >
00994    inline DATA_TYPE makeZRot( const Matrix<DATA_TYPE, ROWS, COLS>& mat )
00995    {
00996       const gmtl::Vec<DATA_TYPE, 3> forward_point(1.0f, 0.0f, 0.0f);   // +x
00997       const gmtl::Vec<DATA_TYPE, 3> origin_point(0.0f, 0.0f, 0.0f);
00998       gmtl::Vec<DATA_TYPE, 3> end_point, start_point;
00999 
01000       gmtl::xform(end_point, mat, forward_point);
01001       gmtl::xform(start_point, mat, origin_point);
01002       gmtl::Vec<DATA_TYPE, 3> direction_vector = end_point - start_point;
01003 
01004       // Constrain the direction to XY-plane only.
01005       direction_vector[2] = 0.0f;                  // Eliminate Z value
01006       gmtl::normalize(direction_vector);
01007       DATA_TYPE z_rot = gmtl::Math::aCos(gmtl::dot(direction_vector,
01008                                                    forward_point));
01009 
01010       gmtl::Vec<DATA_TYPE, 3> which_side = gmtl::makeCross(forward_point,
01011                                                            direction_vector);
01012 
01013       // If direction vector to "right" (negative) side of forward
01014       if ( which_side[2] < 0.0f )
01015       {
01016          z_rot = -z_rot;
01017       }
01018 
01019       return z_rot;
01020    }
01021 
01032    template< typename DATA_TYPE, unsigned ROWS, unsigned COLS >
01033    inline Matrix<DATA_TYPE, ROWS, COLS>& setDirCos( Matrix<DATA_TYPE, ROWS, COLS>& result,
01034                                                      const Vec<DATA_TYPE, 3>& xDestAxis,
01035                                                      const Vec<DATA_TYPE, 3>& yDestAxis, const Vec<DATA_TYPE, 3>& zDestAxis,
01036                                                      const Vec<DATA_TYPE, 3>& xSrcAxis = Vec<DATA_TYPE, 3>(1,0,0),
01037                                                      const Vec<DATA_TYPE, 3>& ySrcAxis = Vec<DATA_TYPE, 3>(0,1,0),
01038                                                      const Vec<DATA_TYPE, 3>& zSrcAxis = Vec<DATA_TYPE, 3>(0,0,1) )
01039    {
01040       // @todo set this a compile time assert...
01041       gmtlASSERT( ROWS >= 3 && COLS >= 3 && ROWS <= 4 && COLS <= 4 && "this is undefined for Matrix smaller than 3x3 or bigger than 4x4" );
01042 
01043       DATA_TYPE Xa, Xb, Xc;    // Direction cosines of the secondary x-axis
01044       DATA_TYPE Ya, Yb, Yc;    // Direction cosines of the secondary y-axis
01045       DATA_TYPE Za, Zb, Zc;    // Direction cosines of the secondary z-axis
01046 
01047       Xa = dot(xDestAxis, xSrcAxis);  Xb = dot(xDestAxis, ySrcAxis);  Xc = dot(xDestAxis, zSrcAxis);
01048       Ya = dot(yDestAxis, xSrcAxis);  Yb = dot(yDestAxis, ySrcAxis);  Yc = dot(yDestAxis, zSrcAxis);
01049       Za = dot(zDestAxis, xSrcAxis);  Zb = dot(zDestAxis, ySrcAxis);  Zc = dot(zDestAxis, zSrcAxis);
01050 
01051       // Set the matrix correctly
01052       result( 0, 0 ) = Xa; result( 0, 1 ) = Ya; result( 0, 2 ) = Za;
01053       result( 1, 0 ) = Xb; result( 1, 1 ) = Yb; result( 1, 2 ) = Zb;
01054       result( 2, 0 ) = Xc; result( 2, 1 ) = Yc; result( 2, 2 ) = Zc;
01055 
01056       // track state
01057       switch (result.mState)
01058       {
01059       case Matrix<DATA_TYPE, ROWS, COLS>::TRANS:    result.mState = Matrix<DATA_TYPE, ROWS, COLS>::AFFINE; break;
01060       case Matrix<DATA_TYPE, ROWS, COLS>::IDENTITY: result.mState = Matrix<DATA_TYPE, ROWS, COLS>::ORTHOGONAL; break;
01061       }
01062 
01063       return result;
01064    }
01065 
01070    template< typename DATA_TYPE, unsigned ROWS, unsigned COLS >
01071    inline Matrix<DATA_TYPE, ROWS, COLS>& setAxes( Matrix<DATA_TYPE, ROWS, COLS>& result,
01072                                                   const Vec<DATA_TYPE, 3>& xAxis,
01073                                                   const Vec<DATA_TYPE, 3>& yAxis,
01074                                                   const Vec<DATA_TYPE, 3>& zAxis )
01075    {
01076       // @todo set this a compile time assert...
01077       gmtlASSERT( ROWS >= 3 && COLS >= 3 && ROWS <= 4 && COLS <= 4 && "this is undefined for Matrix smaller than 3x3 or bigger than 4x4" );
01078 
01079       result( 0, 0 ) = xAxis[0];
01080       result( 1, 0 ) = xAxis[1];
01081       result( 2, 0 ) = xAxis[2];
01082 
01083       result( 0, 1 ) = yAxis[0];
01084       result( 1, 1 ) = yAxis[1];
01085       result( 2, 1 ) = yAxis[2];
01086 
01087       result( 0, 2 ) = zAxis[0];
01088       result( 1, 2 ) = zAxis[1];
01089       result( 2, 2 ) = zAxis[2];
01090 
01091       // track state
01092       switch (result.mState)
01093       {
01094       case Matrix<DATA_TYPE, ROWS, COLS>::TRANS:    result.mState = Matrix<DATA_TYPE, ROWS, COLS>::AFFINE; break;
01095       case Matrix<DATA_TYPE, ROWS, COLS>::IDENTITY: result.mState = Matrix<DATA_TYPE, ROWS, COLS>::ORTHOGONAL; break;
01096       }
01097       return result;
01098    }
01099 
01104    template< typename ROTATION_TYPE >
01105    inline ROTATION_TYPE makeAxes( const Vec<typename ROTATION_TYPE::DataType, 3>& xAxis,
01106                                   const Vec<typename ROTATION_TYPE::DataType, 3>& yAxis,
01107                                   const Vec<typename ROTATION_TYPE::DataType, 3>& zAxis,
01108                                   Type2Type< ROTATION_TYPE > t = Type2Type< ROTATION_TYPE >() )
01109    {
01110       gmtl::ignore_unused_variable_warning(t);
01111       ROTATION_TYPE temporary;
01112       return setAxes( temporary, xAxis, yAxis, zAxis );
01113    }
01114 
01119    template < typename DATA_TYPE, unsigned ROWS, unsigned COLS >
01120    inline Matrix<DATA_TYPE, ROWS, COLS> makeTranspose( const Matrix<DATA_TYPE, ROWS, COLS>& m )
01121    {
01122       Matrix<DATA_TYPE, ROWS, COLS> temporary( m );
01123       return transpose( temporary );
01124    }
01125 
01133    template< typename DATA_TYPE, unsigned ROWS, unsigned COLS >
01134    inline Matrix<DATA_TYPE, ROWS, COLS> makeInvert(const Matrix<DATA_TYPE, ROWS, COLS>& src)
01135    {
01136       Matrix<DATA_TYPE, ROWS, COLS> result;
01137       return invert( result, src );
01138    }
01139 
01143    template <typename DATA_TYPE, unsigned ROWS, unsigned COLS>
01144    Matrix<DATA_TYPE, ROWS, COLS>& setRot( Matrix<DATA_TYPE, ROWS, COLS>& mat, const Quat<DATA_TYPE>& q )
01145    {
01146       gmtlASSERT( ((ROWS == 3 && COLS == 3) ||
01147                (ROWS == 3 && COLS == 4) ||
01148                (ROWS == 4 && COLS == 3) ||
01149                (ROWS == 4 && COLS == 4)) &&
01150                "pre conditions not met on set( mat, quat ) which only sets a quaternion to the rotation part of a 3x3, 3x4, 4x3, or 4x4 matrix." );
01151 
01152       // From Watt & Watt
01153       DATA_TYPE wx, wy, wz, xx, yy, yz, xy, xz, zz, xs, ys, zs;
01154 
01155       xs = q[Xelt] + q[Xelt]; ys = q[Yelt] + q[Yelt]; zs = q[Zelt] + q[Zelt];
01156       xx = q[Xelt] * xs;      xy = q[Xelt] * ys;      xz = q[Xelt] * zs;
01157       yy = q[Yelt] * ys;      yz = q[Yelt] * zs;      zz = q[Zelt] * zs;
01158       wx = q[Welt] * xs;      wy = q[Welt] * ys;      wz = q[Welt] * zs;
01159 
01160       mat( 0, 0 ) = DATA_TYPE(1.0) - (yy + zz);
01161       mat( 1, 0 ) = xy + wz;
01162       mat( 2, 0 ) = xz - wy;
01163 
01164       mat( 0, 1 ) = xy - wz;
01165       mat( 1, 1 ) = DATA_TYPE(1.0) - (xx + zz);
01166       mat( 2, 1 ) = yz + wx;
01167 
01168       mat( 0, 2 ) = xz + wy;
01169       mat( 1, 2 ) = yz - wx;
01170       mat( 2, 2 ) = DATA_TYPE(1.0) - (xx + yy);
01171 
01172       // track state
01173       switch (mat.mState)
01174       {
01175       case Matrix<DATA_TYPE, ROWS, COLS>::TRANS:    mat.mState = Matrix<DATA_TYPE, ROWS, COLS>::AFFINE; break;
01176       case Matrix<DATA_TYPE, ROWS, COLS>::IDENTITY: mat.mState = Matrix<DATA_TYPE, ROWS, COLS>::ORTHOGONAL; break;
01177       }
01178       return mat;
01179    }
01180 
01186    template <typename DATATYPE, typename POS_TYPE, typename ROT_TYPE, unsigned MATCOLS, unsigned MATROWS >
01187    inline Matrix<DATATYPE, MATROWS, MATCOLS>& set( Matrix<DATATYPE, MATROWS, MATCOLS>& mat,
01188                                                    const Coord<POS_TYPE, ROT_TYPE>& coord )
01189    {
01190       // set to ident first...
01191       gmtl::identity( mat );
01192 
01193       // set translation
01194       // @todo metaprogram this out for 3x3 and 2x2 matrices
01195       if (MATCOLS == 4)
01196       {
01197          setTrans( mat, coord.getPos() );// filtered (only sets the trans part).
01198       }
01199       setRot( mat, coord.getRot() ); // filtered (only sets the rot part).
01200       return mat;
01201    }
01202 
01208    template <typename DATA_TYPE, unsigned ROWS, unsigned COLS>
01209    Matrix<DATA_TYPE, ROWS, COLS>& set( Matrix<DATA_TYPE, ROWS, COLS>& mat, const Quat<DATA_TYPE>& q )
01210    {
01211       if (ROWS == 4)
01212       {
01213          mat( 3, 0 ) = DATA_TYPE(0.0);
01214          mat( 3, 1 ) = DATA_TYPE(0.0);
01215          mat( 3, 2 ) = DATA_TYPE(0.0);
01216       }
01217 
01218       if (COLS == 4)
01219       {
01220          mat( 0, 3 ) = DATA_TYPE(0.0);
01221          mat( 1, 3 ) = DATA_TYPE(0.0);
01222          mat( 2, 3 ) = DATA_TYPE(0.0);
01223       }
01224 
01225       if (ROWS == 4 && COLS == 4)
01226          mat( 3, 3 ) = DATA_TYPE(1.0);
01227 
01228       // track state
01229       mat.mState = Matrix<DATA_TYPE, ROWS, COLS>::IDENTITY;
01230 
01231       return setRot( mat, q );
01232    }
01233 
01242    template <typename DATATYPE, typename POS_TYPE, typename ROT_TYPE, unsigned MATCOLS, unsigned MATROWS >
01243    inline Coord<POS_TYPE, ROT_TYPE>& set( Coord<POS_TYPE, ROT_TYPE>& eulercoord, const Matrix<DATATYPE, MATROWS, MATCOLS>& mat )
01244    {
01245       gmtl::setTrans( eulercoord.pos(), mat );
01246       gmtl::set( eulercoord.rot(), mat );
01247       return eulercoord;
01248    }
01249 
01253    template <typename DATATYPE, typename POS_TYPE, typename ROT_TYPE, unsigned MATCOLS, unsigned MATROWS >
01254    inline Coord<POS_TYPE, ROT_TYPE>& setRot( Coord<POS_TYPE, ROT_TYPE>& result, const Matrix<DATATYPE, MATROWS, MATCOLS>& mat )
01255    {
01256       return gmtl::set( result, mat );
01257    }
01258 
01275    template <typename TARGET_TYPE, typename SOURCE_TYPE>
01276    inline TARGET_TYPE make( const SOURCE_TYPE& src,
01277                            Type2Type< TARGET_TYPE > t = Type2Type< TARGET_TYPE >() )
01278    {
01279       gmtl::ignore_unused_variable_warning(t);
01280       TARGET_TYPE target;
01281       return gmtl::set( target, src );
01282    }
01283 
01288    template <typename ROTATION_TYPE, typename SOURCE_TYPE >
01289    inline ROTATION_TYPE makeRot( const SOURCE_TYPE& coord,
01290                                 Type2Type< ROTATION_TYPE > t = Type2Type< ROTATION_TYPE >() )
01291    {
01292       gmtl::ignore_unused_variable_warning(t);
01293       ROTATION_TYPE temporary;
01294       return gmtl::set( temporary, coord );
01295    }
01296 
01308    template< typename ROTATION_TYPE >
01309    inline ROTATION_TYPE makeDirCos( const Vec<typename ROTATION_TYPE::DataType, 3>& xDestAxis,
01310                                   const Vec<typename ROTATION_TYPE::DataType, 3>& yDestAxis,
01311                                   const Vec<typename ROTATION_TYPE::DataType, 3>& zDestAxis,
01312                                   const Vec<typename ROTATION_TYPE::DataType, 3>& xSrcAxis = Vec<typename ROTATION_TYPE::DataType, 3>(1,0,0),
01313                                   const Vec<typename ROTATION_TYPE::DataType, 3>& ySrcAxis = Vec<typename ROTATION_TYPE::DataType, 3>(0,1,0),
01314                                   const Vec<typename ROTATION_TYPE::DataType, 3>& zSrcAxis = Vec<typename ROTATION_TYPE::DataType, 3>(0,0,1),
01315                                Type2Type< ROTATION_TYPE > t = Type2Type< ROTATION_TYPE >() )
01316    {
01317       gmtl::ignore_unused_variable_warning(t);
01318       ROTATION_TYPE temporary;
01319       return setDirCos( temporary, xDestAxis, yDestAxis, zDestAxis, xSrcAxis, ySrcAxis, zSrcAxis );
01320    }
01321 
01337    template<typename TRANS_TYPE, typename SRC_TYPE >
01338    inline TRANS_TYPE makeTrans( const SRC_TYPE& arg,
01339                              Type2Type< TRANS_TYPE > t = Type2Type< TRANS_TYPE >())
01340    {
01341       gmtl::ignore_unused_variable_warning(t);
01342       TRANS_TYPE temporary;
01343       return setTrans( temporary, arg );
01344    }
01345 
01350    template< typename ROTATION_TYPE >
01351    inline ROTATION_TYPE makeRot( const Vec<typename ROTATION_TYPE::DataType, 3>& from,
01352                                  const Vec<typename ROTATION_TYPE::DataType, 3>& to )
01353    {
01354       ROTATION_TYPE temporary;
01355       return setRot( temporary, from, to );
01356    }
01357 
01363    template <typename DEST_TYPE, typename DATA_TYPE>
01364    inline DEST_TYPE& setRot( DEST_TYPE& result, const Vec<DATA_TYPE, 3>& from, const Vec<DATA_TYPE, 3>& to )
01365    {
01366       // @todo should assert that DEST_TYPE::DataType == DATA_TYPE
01367       const DATA_TYPE epsilon = (DATA_TYPE)0.00001;
01368 
01369       gmtlASSERT( gmtl::Math::isEqual( gmtl::length( from ), (DATA_TYPE)1.0, epsilon ) &&
01370                   gmtl::Math::isEqual( gmtl::length( to ), (DATA_TYPE)1.0, epsilon ) /* &&
01371                   "input params not normalized" */);
01372 
01373       DATA_TYPE cosangle = dot( from, to );
01374 
01375       // if cosangle is close to 1, so the vectors are close to being coincident
01376       // Need to generate an angle of zero with any vector we like
01377       // We'll choose identity (no rotation)
01378       if ( Math::isEqual( cosangle, (DATA_TYPE)1.0, epsilon ) )
01379       {
01380          return result = DEST_TYPE();
01381       }
01382 
01383       // vectors are close to being opposite, so rotate one a little...
01384       else if ( Math::isEqual( cosangle, (DATA_TYPE)-1.0, epsilon ) )
01385       {
01386          Vec<DATA_TYPE, 3> to_rot( to[0] + (DATA_TYPE)0.3, to[1] - (DATA_TYPE)0.15, to[2] - (DATA_TYPE)0.15 ), axis;
01387          normalize( cross( axis, from, to_rot ) ); // setRot requires normalized vec
01388          DATA_TYPE angle = Math::aCos( cosangle );
01389          return setRot( result, gmtl::AxisAngle<DATA_TYPE>( angle, axis ) );
01390       }
01391 
01392       // This is the usual situation - take a cross-product of vec1 and vec2
01393       // and that is the axis around which to rotate.
01394       else
01395       {
01396          Vec<DATA_TYPE, 3> axis;
01397          normalize( cross( axis, from, to ) ); // setRot requires normalized vec
01398          DATA_TYPE angle = Math::aCos( cosangle );
01399          return setRot( result, gmtl::AxisAngle<DATA_TYPE>( angle, axis ) );
01400       }
01401    }
01402 
01412    template< typename DATA_TYPE, unsigned ROWS, unsigned COLS >
01413    void setRow(Vec<DATA_TYPE, COLS>& dest, const Matrix<DATA_TYPE, ROWS, COLS>& src, unsigned row)
01414    {
01415       for (unsigned i=0; i<COLS; ++i)
01416       {
01417          dest[i] = src[row][i];
01418       }
01419    }
01420 
01429    template< typename DATA_TYPE, unsigned ROWS, unsigned COLS >
01430    Vec<DATA_TYPE, COLS> makeRow(const Matrix<DATA_TYPE, ROWS, COLS>& src, unsigned row)
01431    {
01432       Vec<DATA_TYPE, COLS> result;
01433       setRow(result, src, row);
01434       return result;
01435    }
01436 
01444    template< typename DATA_TYPE, unsigned ROWS, unsigned COLS >
01445    void setColumn(Vec<DATA_TYPE, ROWS>& dest, const Matrix<DATA_TYPE, ROWS, COLS>& src, unsigned col)
01446    {
01447       for (unsigned i=0; i<ROWS; ++i)
01448       {
01449          dest[i] = src[i][col];
01450       }
01451    }
01452 
01461    template< typename DATA_TYPE, unsigned ROWS, unsigned COLS >
01462    Vec<DATA_TYPE, ROWS> makeColumn(const Matrix<DATA_TYPE, ROWS, COLS>& src, unsigned col)
01463    {
01464       Vec<DATA_TYPE, ROWS> result;
01465       setColumn(result, src, col);
01466       return result;
01467    }
01468 
01469 } // end gmtl namespace.
01470 
01471 #endif