Generate.h Source File

00001 /************************************************************** ggt-head beg
00002  *
00003  * GGT: Generic Graphics Toolkit
00004  *
00005  * Original Authors:
00006  *   Allen Bierbaum
00007  *
00008  * -----------------------------------------------------------------
00009  * File:          $RCSfile: Generate.h,v $
00010  * Date modified: $Date: 2003/04/01 00:40:53 $
00011  * Version:       $Revision: 1.70 $
00012  * -----------------------------------------------------------------
00013  *
00014  *********************************************************** ggt-head end */
00015 /*************************************************************** ggt-cpr beg
00016 *
00017 * GGT: The Generic Graphics Toolkit
00018 * Copyright (C) 2001,2002 Allen Bierbaum
00019 *
00020 * This library is free software; you can redistribute it and/or
00021 * modify it under the terms of the GNU Lesser General Public
00022 * License as published by the Free Software Foundation; either
00023 * version 2.1 of the License, or (at your option) any later version.
00024 *
00025 * This library is distributed in the hope that it will be useful,
00026 * but WITHOUT ANY WARRANTY; without even the implied warranty of
00027 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
00028 * Lesser General Public License for more details.
00029 *
00030 * You should have received a copy of the GNU Lesser General Public
00031 * License along with this library; if not, write to the Free Software
00032 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
00033 *
00034  ************************************************************ ggt-cpr end */
00035 #ifndef _GMTL_GENERATE_H_
00036 #define _GMTL_GENERATE_H_
00037 
00038 #include <gmtl/Util/Assert.h>
00039 
00040 #include <gmtl/Vec.h>    // for Vec
00041 #include <gmtl/VecOps.h> // for lengthSquared
00042 #include <gmtl/Quat.h>
00043 #include <gmtl/QuatOps.h>
00044 #include <gmtl/Coord.h>
00045 #include <gmtl/Matrix.h>
00046 #include <gmtl/Util/Meta.h>
00047 #include <gmtl/Math.h>
00048 #include <gmtl/Xforms.h>
00049 
00050 #include <gmtl/EulerAngle.h>
00051 #include <gmtl/AxisAngle.h>
00052 
00053 namespace gmtl
00054 {
00069    template <typename TARGET_TYPE, typename SOURCE_TYPE>
00070    inline TARGET_TYPE make( const SOURCE_TYPE& src,
00071                            Type2Type< TARGET_TYPE > t = Type2Type< TARGET_TYPE >() )
00072    {
00073       gmtl::ignore_unused_variable_warning(t);
00074       TARGET_TYPE target;
00075       return gmtl::set( target, src );
00076    }
00077 
00082    template <typename ROTATION_TYPE, typename SOURCE_TYPE >
00083    inline ROTATION_TYPE makeRot( const SOURCE_TYPE& coord,
00084                                 Type2Type< ROTATION_TYPE > t = Type2Type< ROTATION_TYPE >() )
00085    {
00086       gmtl::ignore_unused_variable_warning(t);
00087       ROTATION_TYPE temporary;
00088       return gmtl::set( temporary, coord );
00089    }
00090 
00102    template< typename ROTATION_TYPE >
00103    inline ROTATION_TYPE makeDirCos( const Vec<typename ROTATION_TYPE::DataType, 3>& xDestAxis,
00104                                   const Vec<typename ROTATION_TYPE::DataType, 3>& yDestAxis,
00105                                   const Vec<typename ROTATION_TYPE::DataType, 3>& zDestAxis,
00106                                   const Vec<typename ROTATION_TYPE::DataType, 3>& xSrcAxis = Vec<typename ROTATION_TYPE::DataType, 3>(1,0,0),
00107                                   const Vec<typename ROTATION_TYPE::DataType, 3>& ySrcAxis = Vec<typename ROTATION_TYPE::DataType, 3>(0,1,0),
00108                                   const Vec<typename ROTATION_TYPE::DataType, 3>& zSrcAxis = Vec<typename ROTATION_TYPE::DataType, 3>(0,0,1),
00109                                Type2Type< ROTATION_TYPE > t = Type2Type< ROTATION_TYPE >() )
00110    {
00111       gmtl::ignore_unused_variable_warning(t);
00112       ROTATION_TYPE temporary;
00113       return setDirCos( temporary, xDestAxis, yDestAxis, zDestAxis, xSrcAxis, ySrcAxis, zSrcAxis );
00114    }
00115 
00131    template<typename TRANS_TYPE, typename SRC_TYPE >
00132    inline TRANS_TYPE makeTrans( const SRC_TYPE& arg,
00133                              Type2Type< TRANS_TYPE > t = Type2Type< TRANS_TYPE >())
00134    {
00135       gmtl::ignore_unused_variable_warning(t);
00136       TRANS_TYPE temporary;
00137       return setTrans( temporary, arg );
00138    }
00139 
00144    template< typename ROTATION_TYPE >
00145    inline ROTATION_TYPE makeRot( const Vec<typename ROTATION_TYPE::DataType, 3>& from,
00146                                  const Vec<typename ROTATION_TYPE::DataType, 3>& to )
00147    {
00148       ROTATION_TYPE temporary;
00149       return setRot( temporary, from, to );
00150    }
00151 
00157    template <typename DEST_TYPE, typename DATA_TYPE>
00158    inline DEST_TYPE& setRot( DEST_TYPE& result, const Vec<DATA_TYPE, 3>& from, const Vec<DATA_TYPE, 3>& to )
00159    {
00160       // @todo should assert that DEST_TYPE::DataType == DATA_TYPE
00161       const DATA_TYPE epsilon = (DATA_TYPE)0.00001;
00162 
00163       gmtlASSERT( gmtl::Math::isEqual( gmtl::length( from ), (DATA_TYPE)1.0, epsilon ) &&
00164                   gmtl::Math::isEqual( gmtl::length( to ), (DATA_TYPE)1.0, epsilon ) &&
00165                   "input params not normalized" );
00166 
00167       DATA_TYPE cosangle = dot( from, to );
00168 
00169       // if cosangle is close to 1, so the vectors are close to being coincident
00170       // Need to generate an angle of zero with any vector we like
00171       // We'll choose identity (no rotation)
00172       if ( Math::isEqual( cosangle, (DATA_TYPE)1.0, epsilon ) )
00173       {
00174          return result = DEST_TYPE();
00175       }
00176 
00177       // vectors are close to being opposite, so rotate one a little...
00178       else if ( Math::isEqual( cosangle, (DATA_TYPE)-1.0, epsilon ) )
00179       {
00180          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;
00181          normalize( cross( axis, from, to_rot ) ); // setRot requires normalized vec
00182          DATA_TYPE angle = Math::aCos( cosangle );
00183          return setRot( result, gmtl::AxisAngle<DATA_TYPE>( angle, axis ) );
00184       }
00185 
00186       // This is the usual situation - take a cross-product of vec1 and vec2
00187       // and that is the axis around which to rotate.
00188       else
00189       {
00190          Vec<DATA_TYPE, 3> axis;
00191          normalize( cross( axis, from, to ) ); // setRot requires normalized vec
00192          DATA_TYPE angle = Math::aCos( cosangle );
00193          return setRot( result, gmtl::AxisAngle<DATA_TYPE>( angle, axis ) );
00194       }
00195    }
00196 
00207    template <typename DATA_TYPE>
00208    inline Vec<DATA_TYPE, 3> makeVec( const Quat<DATA_TYPE>& quat )
00209    {
00210       return Vec<DATA_TYPE, 3>( quat[Xelt], quat[Yelt], quat[Zelt] );
00211    }
00212 
00215    template <typename DATA_TYPE, unsigned SIZE>
00216    inline Vec<DATA_TYPE, SIZE> makeNormal( Vec<DATA_TYPE, SIZE> vec )
00217    {
00218       normalize( vec );
00219       return vec;
00220    }
00221 
00231    template<typename VEC_TYPE, typename DATA_TYPE, unsigned ROWS, unsigned COLS >
00232    inline VEC_TYPE& setTrans( VEC_TYPE& result, const Matrix<DATA_TYPE, ROWS, COLS>& arg )
00233    {
00234       // ASSERT: There are as many
00235 
00236       // if n x n   then (homogeneous case) vecsize == rows-1 or (scale component case) vecsize == rows
00237       // if n x n+1 then (homogeneous case) vecsize == rows   or (scale component case) vecsize == rows+1
00238       gmtlASSERT( ((ROWS == COLS && ( VEC_TYPE::Size == (ROWS-1) ||  VEC_TYPE::Size == ROWS)) ||
00239                (COLS == (ROWS+1) && ( VEC_TYPE::Size == ROWS ||  VEC_TYPE::Size == (ROWS+1)))) &&
00240               "preconditions not met for vector size in call to makeTrans.  Read your documentation." );
00241 
00242       // homogeneous case...
00243       if ((ROWS == COLS &&  VEC_TYPE::Size == ROWS)              // Square matrix and vec so assume homogeneous vector. ex. 4x4 with vec 4
00244           || (COLS == (ROWS+1) &&  VEC_TYPE::Size == (ROWS+1)))  // ex: 3x4 with vec4
00245       {
00246          result[VEC_TYPE::Size-1] = 1.0f;
00247       }
00248 
00249       // non-homogeneous case... (SIZE == ROWS),
00250       //else
00251       //{}
00252 
00253       for (unsigned x = 0; x < COLS - 1; ++x)
00254       {
00255          result[x] = arg( x, COLS - 1 );
00256       }
00257 
00258       return result;
00259    }
00260 
00274    template <typename DATA_TYPE>
00275    inline Quat<DATA_TYPE>& setPure( Quat<DATA_TYPE>& quat, const Vec<DATA_TYPE, 3>& vec )
00276    {
00277       quat.set( vec[0], vec[1], vec[2], 0 );
00278       return quat;
00279    }
00280 
00287    template <typename DATA_TYPE>
00288    inline Quat<DATA_TYPE> makePure( const Vec<DATA_TYPE, 3>& vec )
00289    {
00290       return Quat<DATA_TYPE>( vec[0], vec[1], vec[2], 0 );
00291    }
00292 
00297    template <typename DATA_TYPE>
00298    inline Quat<DATA_TYPE> makeNormal( const Quat<DATA_TYPE>& quat )
00299    {
00300       Quat<DATA_TYPE> temporary( quat );
00301       return normalize( temporary );
00302    }
00303 
00310    template <typename DATA_TYPE>
00311    inline Quat<DATA_TYPE> makeConj( const Quat<DATA_TYPE>& quat )
00312    {
00313       Quat<DATA_TYPE> temporary( quat );
00314       return conj( temporary );
00315    }
00316 
00322    template <typename DATA_TYPE>
00323    inline Quat<DATA_TYPE> makeInvert( const Quat<DATA_TYPE>& quat )
00324    {
00325       Quat<DATA_TYPE> temporary( quat );
00326       return invert( temporary );
00327    }
00328 
00333    template <typename DATA_TYPE>
00334    inline Quat<DATA_TYPE>& set( Quat<DATA_TYPE>& result, const AxisAngle<DATA_TYPE>& axisAngle )
00335    {
00336       gmtlASSERT( (Math::isEqual( lengthSquared( axisAngle.getAxis() ), (DATA_TYPE)1.0, (DATA_TYPE)0.0001 )) &&
00337                    "you must pass in a normalized vector to setRot( quat, rad, vec )" );
00338 
00339       DATA_TYPE half_angle = axisAngle.getAngle() * (DATA_TYPE)0.5;
00340       DATA_TYPE sin_half_angle = Math::sin( half_angle );
00341 
00342       result[Welt] = Math::cos( half_angle );
00343       result[Xelt] = sin_half_angle * axisAngle.getAxis()[0];
00344       result[Yelt] = sin_half_angle * axisAngle.getAxis()[1];
00345       result[Zelt] = sin_half_angle * axisAngle.getAxis()[2];
00346 
00347       // should automagically be normalized (unit magnitude) now...
00348       return result;
00349    }
00350 
00354    template <typename DATA_TYPE>
00355    inline Quat<DATA_TYPE>& setRot( Quat<DATA_TYPE>& result, const AxisAngle<DATA_TYPE>& axisAngle )
00356    {
00357       return gmtl::set( result, axisAngle );
00358    }
00359 
00364    template <typename DATA_TYPE, typename ROT_ORDER>
00365    inline Quat<DATA_TYPE>& set( Quat<DATA_TYPE>& result, const EulerAngle<DATA_TYPE, ROT_ORDER>& euler )
00366    {
00367       // this might be faster if put into the switch statement... (testme)
00368       const int& order = ROT_ORDER::ID;
00369       const DATA_TYPE xRot = (order == XYZ::ID) ? euler[0] : ((order == ZXY::ID) ? euler[1] : euler[2]);
00370       const DATA_TYPE yRot = (order == XYZ::ID) ? euler[1] : ((order == ZXY::ID) ? euler[2] : euler[1]);
00371       const DATA_TYPE zRot = (order == XYZ::ID) ? euler[2] : ((order == ZXY::ID) ? euler[0] : euler[0]);
00372 
00373       // this could be written better for each rotation order, but this is really general...
00374       Quat<DATA_TYPE> qx, qy, qz;
00375 
00376       // precompute half angles
00377       DATA_TYPE xOver2 = xRot * (DATA_TYPE)0.5;
00378       DATA_TYPE yOver2 = yRot * (DATA_TYPE)0.5;
00379       DATA_TYPE zOver2 = zRot * (DATA_TYPE)0.5;
00380 
00381       // set the pitch quat
00382       qx[Xelt] = Math::sin( xOver2 );
00383       qx[Yelt] = (DATA_TYPE)0.0;
00384       qx[Zelt] = (DATA_TYPE)0.0;
00385       qx[Welt] = Math::cos( xOver2 );
00386 
00387       // set the yaw quat
00388       qy[Xelt] = (DATA_TYPE)0.0;
00389       qy[Yelt] = Math::sin( yOver2 );
00390       qy[Zelt] = (DATA_TYPE)0.0;
00391       qy[Welt] = Math::cos( yOver2 );
00392 
00393       // set the roll quat
00394       qz[Xelt] = (DATA_TYPE)0.0;
00395       qz[Yelt] = (DATA_TYPE)0.0;
00396       qz[Zelt] = Math::sin( zOver2 );
00397       qz[Welt] = Math::cos( zOver2 );
00398 
00399       // compose the three in pyr order...
00400       switch (order)
00401       {
00402       case XYZ::ID: result = qx * qy * qz; break;
00403       case ZYX::ID: result = qz * qy * qx; break;
00404       case ZXY::ID: result = qz * qx * qy; break;
00405       default:
00406          gmtlASSERT( false && "unknown rotation order passed to setRot" );
00407          break;
00408       }
00409 
00410       // ensure the quaternion is normalized
00411       normalize( result );
00412       return result;
00413    }
00414 
00418    template <typename DATA_TYPE, typename ROT_ORDER>
00419    inline Quat<DATA_TYPE>& setRot( Quat<DATA_TYPE>& result, const EulerAngle<DATA_TYPE, ROT_ORDER>& euler )
00420    {
00421       return gmtl::set( result, euler );
00422    }
00423 
00428    template <typename DATA_TYPE, unsigned ROWS, unsigned COLS>
00429    inline Quat<DATA_TYPE>& set( Quat<DATA_TYPE>& quat, const Matrix<DATA_TYPE, ROWS, COLS>& mat  )
00430    {
00431       gmtlASSERT( ((ROWS == 3 && COLS == 3) ||
00432                (ROWS == 3 && COLS == 4) ||
00433                (ROWS == 4 && COLS == 3) ||
00434                (ROWS == 4 && COLS == 4)) &&
00435                "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." );
00436 
00437       DATA_TYPE tr( mat( 0, 0 ) + mat( 1, 1 ) + mat( 2, 2 ) ), s( 0.0f );
00438 
00439       // If diagonal is positive
00440       if (tr > (DATA_TYPE)0.0)
00441       {
00442          s = Math::sqrt( tr + (DATA_TYPE)1.0 );
00443          quat[Welt] = s * (DATA_TYPE)0.5;
00444          s = DATA_TYPE(0.5) / s;
00445 
00446          quat[Xelt] = (mat( 2, 1 ) - mat( 1, 2 )) * s;
00447          quat[Yelt] = (mat( 0, 2 ) - mat( 2, 0 )) * s;
00448          quat[Zelt] = (mat( 1, 0 ) - mat( 0, 1 )) * s;
00449       }
00450 
00451       // when Diagonal is negative
00452       else
00453       {
00454          static const unsigned int nxt[3] = { 1, 2, 0 };
00455          unsigned int i( Xelt ), j, k;
00456 
00457          if (mat( 1, 1 ) > mat( 0, 0 ))
00458             i = 1;
00459 
00460          if (mat( 2, 2 ) > mat( i, i ))
00461             i = 2;
00462 
00463          j = nxt[i];
00464          k = nxt[j];
00465 
00466          s = Math::sqrt( (mat( i, i )-(mat( j, j )+mat( k, k ))) + (DATA_TYPE)1.0 );
00467 
00468          DATA_TYPE q[4];
00469          q[i] = s * (DATA_TYPE)0.5;
00470 
00471          if (s != (DATA_TYPE)0.0)
00472             s = DATA_TYPE(0.5) / s;
00473 
00474          q[3] = (mat( k, j ) - mat( j, k )) * s;
00475          q[j] = (mat( j, i ) + mat( i, j )) * s;
00476          q[k] = (mat( k, i ) + mat( i, k )) * s;
00477 
00478          quat[Xelt] = q[0];
00479          quat[Yelt] = q[1];
00480          quat[Zelt] = q[2];
00481          quat[Welt] = q[3];
00482       }
00483 
00484       return quat;
00485    }
00486 
00490    template <typename DATA_TYPE, unsigned ROWS, unsigned COLS>
00491    inline Quat<DATA_TYPE>& setRot( Quat<DATA_TYPE>& result, const Matrix<DATA_TYPE, ROWS, COLS>& mat )
00492    {
00493       return gmtl::set( result, mat );
00494    }
00512    template <typename DATA_TYPE>
00513    inline AxisAngle<DATA_TYPE>& set( AxisAngle<DATA_TYPE>& axisAngle, Quat<DATA_TYPE> quat )
00514    {
00515       // set sure we don't get a NaN result from acos...
00516       if (Math::abs( quat[Welt] ) > (DATA_TYPE)1.0)
00517       {
00518          gmtl::normalize( quat );
00519       }
00520       gmtlASSERT( Math::abs( quat[Welt] ) <= (DATA_TYPE)1.0 && "acos returns NaN when quat[Welt] > 1, try normalizing your quat." );
00521 
00522       // [acos( w ) * 2.0, v / (asin( w ) * 2.0)]
00523 
00524       // set the angle - aCos is mathematically defined to be between 0 and PI
00525       DATA_TYPE rad = Math::aCos( quat[Welt] ) * (DATA_TYPE)2.0;
00526       axisAngle.setAngle( rad );
00527 
00528       // set the axis: (use sin(rad) instead of asin(w))
00529       DATA_TYPE sin_half_angle = Math::sin( rad * (DATA_TYPE)0.5 );
00530       if (sin_half_angle >= (DATA_TYPE)0.0001) // because (PI >= rad >= 0)
00531       {
00532          DATA_TYPE sin_half_angle_inv = DATA_TYPE(1.0) / sin_half_angle;
00533          Vec<DATA_TYPE, 3> axis( quat[Xelt] * sin_half_angle_inv,
00534                                  quat[Yelt] * sin_half_angle_inv,
00535                                  quat[Zelt] * sin_half_angle_inv );
00536          normalize( axis );
00537          axisAngle.setAxis( axis );
00538       }
00539 
00540       // avoid NAN
00541       else
00542       {
00543          // one of the terms should be a 1,
00544          // so we can maintain unit-ness
00545          // in case w is 0 (which here w is 0)
00546          axisAngle.setAxis( gmtl::Vec<DATA_TYPE, 3>(
00547                              DATA_TYPE( 1.0 ) /*- gmtl::Math::abs( quat[Welt] )*/,
00548                             (DATA_TYPE)0.0,
00549                             (DATA_TYPE)0.0 ) );
00550       }
00551       return axisAngle;
00552    }
00553 
00557    template <typename DATA_TYPE>
00558    inline AxisAngle<DATA_TYPE>& setRot( AxisAngle<DATA_TYPE>& result, Quat<DATA_TYPE> quat )
00559    {
00560       return gmtl::set( result, quat );
00561    }
00562 
00564    template <typename DATA_TYPE>
00565    AxisAngle<DATA_TYPE> makeNormal( const AxisAngle<DATA_TYPE>& a )
00566    {
00567       return AxisAngle<DATA_TYPE>( a.getAngle(), makeNormal( a.getAxis() ) );
00568    }
00586    template< typename DATA_TYPE, unsigned ROWS, unsigned COLS, typename ROT_ORDER >
00587    inline EulerAngle<DATA_TYPE, ROT_ORDER>& set( EulerAngle<DATA_TYPE, ROT_ORDER>& euler,
00588                     const Matrix<DATA_TYPE, ROWS, COLS>& mat )
00589    {
00590       // @todo set this a compile time assert...
00591       gmtlASSERT( ROWS >= 3 && COLS >= 3 && ROWS <= 4 && COLS <= 4 &&
00592             "this is undefined for Matrix smaller than 3x3 or bigger than 4x4" );
00593 
00594       DATA_TYPE sx;
00595       DATA_TYPE cz;
00596 
00597       // @todo metaprogram this!
00598       const int& order = ROT_ORDER::ID;
00599       switch (order)
00600       {
00601       case XYZ::ID:
00602          {
00603             euler[2] = Math::aTan2( -mat(0,1), mat(0,0) );       // -(-cy*sz)/(cy*cz) - cy falls out
00604             euler[0] = Math::aTan2( -mat(1,2), mat(2,2) );       // -(sx*cy)/(cx*cy) - cy falls out
00605             cz = Math::cos( euler[2] );
00606             euler[1] = Math::aTan2( mat(0,2), mat(0,0) / cz );   // (sy)/((cy*cz)/cz)
00607          }
00608          break;
00609       case ZYX::ID:
00610          {
00611             euler[0] = Math::aTan2( mat(1,0), mat(0,0) );        // (cy*sz)/(cy*cz) - cy falls out
00612             euler[2] = Math::aTan2( mat(2,1), mat(2,2) );        // (sx*cy)/(cx*cy) - cy falls out
00613             sx = Math::sin( euler[2] );
00614             euler[1] = Math::aTan2( -mat(2,0), mat(2,1) / sx );  // -(-sy)/((sx*cy)/sx)
00615          }
00616          break;
00617       case ZXY::ID:
00618          {
00619             // Extract the rotation directly from the matrix
00620             DATA_TYPE x_angle;
00621             DATA_TYPE y_angle;
00622             DATA_TYPE z_angle;
00623             DATA_TYPE cos_y, sin_y;
00624             DATA_TYPE cos_x, sin_x;
00625             DATA_TYPE cos_z, sin_z;
00626 
00627             sin_x = mat(2,1);
00628             x_angle = Math::aSin( sin_x );     // Get x angle
00629             cos_x = Math::cos( x_angle );
00630 
00631             // Check if cos_x = Zero
00632             if (cos_x != 0.0f)     // ASSERT: cos_x != 0
00633             {
00634                   // Get y Angle
00635                cos_y = mat(2,2) / cos_x;
00636                sin_y = -mat(2,0) / cos_x;
00637                y_angle = Math::aTan2( cos_y, sin_y );
00638 
00639                   // Get z Angle
00640                cos_z = mat(1,1) / cos_x;
00641                sin_z = -mat(0,1) / cos_x;
00642                z_angle = Math::aTan2( cos_z, sin_z );
00643             }
00644             else
00645             {
00646                // Arbitrarily set z_angle = 0
00647                z_angle = 0;
00648 
00649                   // Get y Angle
00650                cos_y = mat(0,0);
00651                sin_y = mat(1,0);
00652                y_angle = Math::aTan2( cos_y, sin_y );
00653             }
00654 
00655             euler[1] = x_angle;
00656             euler[2] = y_angle;
00657             euler[0] = z_angle;
00658          }
00659          break;
00660       default:
00661          gmtlASSERT( false && "unknown rotation order passed to setRot" );
00662          break;
00663       }
00664       return euler;
00665    }
00666 
00670    template< typename DATA_TYPE, unsigned ROWS, unsigned COLS, typename ROT_ORDER >
00671    inline EulerAngle<DATA_TYPE, ROT_ORDER>& setRot( EulerAngle<DATA_TYPE, ROT_ORDER>& result, const Matrix<DATA_TYPE, ROWS, COLS>& mat )
00672    {
00673       return gmtl::set( result, mat );
00674    }
00691    template< typename DATA_TYPE, unsigned ROWS, unsigned COLS, unsigned SIZE >
00692    inline Matrix<DATA_TYPE, ROWS, COLS>& setTrans( Matrix<DATA_TYPE, ROWS, COLS>& result,
00693                                                    const Vec<DATA_TYPE, SIZE>& trans )
00694    {
00695       /* @todo make this a compile time assert... */
00696       // if n x n   then (homogeneous case) vecsize == rows-1 or (scale component case) vecsize == rows
00697       // if n x n+1 then (homogeneous case) vecsize == rows   or (scale component case) vecsize == rows+1
00698       gmtlASSERT( ((ROWS == COLS && (SIZE == (ROWS-1) || SIZE == ROWS)) ||
00699                (COLS == (ROWS+1) && (SIZE == ROWS || SIZE == (ROWS+1)))) &&
00700               "preconditions not met for vector size in call to makeTrans.  Read your documentation." );
00701 
00702       // homogeneous case...
00703       if ((ROWS == COLS && SIZE == ROWS) /* Square matrix and vec so assume homogeneous vector. ex. 4x4 with vec 4 */
00704           || (COLS == (ROWS+1) && SIZE == (ROWS+1)))  /* ex: 3x4 with vec4 */
00705       {
00706          for (unsigned x = 0; x < COLS - 1; ++x)
00707             result( x, COLS - 1 ) = trans[x] / trans[SIZE-1];
00708       }
00709 
00710       // non-homogeneous case...
00711       else
00712       {
00713          for (unsigned x = 0; x < COLS - 1; ++x)
00714             result( x, COLS - 1 ) = trans[x];
00715       }
00716       return result;
00717    }
00718 
00721    template< typename DATA_TYPE, unsigned ROWS, unsigned COLS, unsigned SIZE >
00722    inline Matrix<DATA_TYPE, ROWS, COLS>& setScale( Matrix<DATA_TYPE, ROWS, COLS>& result, const Vec<DATA_TYPE, SIZE>& scale )
00723    {
00724       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." );
00725       for (unsigned x = 0; x < SIZE; ++x)
00726          result( x, x ) = scale[x];
00727       return result;
00728    }
00729 
00732    template <typename MATRIX_TYPE, unsigned SIZE>
00733    inline MATRIX_TYPE makeScale( const Vec<typename MATRIX_TYPE::DataType, SIZE>& scale,
00734                                Type2Type< MATRIX_TYPE > t = Type2Type< MATRIX_TYPE >() )
00735    {
00736       gmtl::ignore_unused_variable_warning(t);
00737       MATRIX_TYPE temporary;
00738       return setScale( temporary, scale );
00739    }
00740 
00741 
00742 
00745    template< typename DATA_TYPE, unsigned ROWS, unsigned COLS >
00746    inline Matrix<DATA_TYPE, ROWS, COLS>& setScale( Matrix<DATA_TYPE, ROWS, COLS>& result, const DATA_TYPE scale )
00747    {
00748       for (unsigned x = 0; x < Math::Min( ROWS, COLS, Math::Max( ROWS, COLS ) - 1 ); ++x) // account for 2x4 or other weird sizes...
00749          result( x, x ) = scale;
00750       return result;
00751    }
00752 
00755    template <typename MATRIX_TYPE>
00756    inline MATRIX_TYPE makeScale( const typename MATRIX_TYPE::DataType scale,
00757                                Type2Type< MATRIX_TYPE > t = Type2Type< MATRIX_TYPE >() )
00758    {
00759       gmtl::ignore_unused_variable_warning(t);
00760       MATRIX_TYPE temporary;
00761       return setScale( temporary, scale );
00762    }
00763 
00764 
00765 
00771    template< typename DATA_TYPE, unsigned ROWS, unsigned COLS >
00772    inline Matrix<DATA_TYPE, ROWS, COLS>& setRot( Matrix<DATA_TYPE, ROWS, COLS>& result, const AxisAngle<DATA_TYPE>& axisAngle )
00773    {
00774       /* @todo set this a compile time assert... */
00775       gmtlASSERT( ROWS >= 3 && COLS >= 3 && ROWS <= 4 && COLS <= 4 &&
00776                      "this func is undefined for Matrix smaller than 3x3 or bigger than 4x4" );
00777       gmtlASSERT( Math::isEqual( lengthSquared( axisAngle.getAxis() ), (DATA_TYPE)1.0, (DATA_TYPE)0.001 ) &&
00778                      "you must pass in a normalized vector to setRot( mat, rad, vec )" );
00779 
00780       // GGI: pg 466
00781       DATA_TYPE s = Math::sin( axisAngle.getAngle() );
00782       DATA_TYPE c = Math::cos( axisAngle.getAngle() );
00783       DATA_TYPE t = DATA_TYPE( 1.0 ) - c;
00784       DATA_TYPE x = axisAngle.getAxis()[0];
00785       DATA_TYPE y = axisAngle.getAxis()[1];
00786       DATA_TYPE z = axisAngle.getAxis()[2];
00787 
00788       /* From: Introduction to robotic.  Craig.  Pg. 52 */
00789       result( 0, 0 ) = (t*x*x)+c;     result( 0, 1 ) = (t*x*y)-(s*z); result( 0, 2 ) = (t*x*z)+(s*y);
00790       result( 1, 0 ) = (t*x*y)+(s*z); result( 1, 1 ) = (t*y*y)+c;     result( 1, 2 ) = (t*y*z)-(s*x);
00791       result( 2, 0 ) = (t*x*z)-(s*y); result( 2, 1 ) = (t*y*z)+(s*x); result( 2, 2 ) = (t*z*z)+c;
00792 
00793       return result;
00794    }
00795 
00796 
00801    template< typename DATA_TYPE, unsigned ROWS, unsigned COLS >
00802    inline Matrix<DATA_TYPE, ROWS, COLS>& set( Matrix<DATA_TYPE, ROWS, COLS>& result, const AxisAngle<DATA_TYPE>& axisAngle )
00803    {
00804       gmtl::identity( result );
00805       return setRot( result, axisAngle );
00806    }
00807 
00812    template< typename DATA_TYPE, unsigned ROWS, unsigned COLS, typename ROT_ORDER >
00813    inline Matrix<DATA_TYPE, ROWS, COLS>& setRot( Matrix<DATA_TYPE, ROWS, COLS>& result, const EulerAngle<DATA_TYPE, ROT_ORDER>& euler )
00814    {
00815       // @todo set this a compile time assert...
00816       gmtlASSERT( ROWS >= 3 && COLS >= 3 && ROWS <= 4 && COLS <= 4 && "this is undefined for Matrix smaller than 3x3 or bigger than 4x4" );
00817 
00818       // this might be faster if put into the switch statement... (testme)
00819       const int& order = ROT_ORDER::ID;
00820       const float xRot = (order == XYZ::ID) ? euler[0] : ((order == ZXY::ID) ? euler[1] : euler[2]);
00821       const float yRot = (order == XYZ::ID) ? euler[1] : ((order == ZXY::ID) ? euler[2] : euler[1]);
00822       const float zRot = (order == XYZ::ID) ? euler[2] : ((order == ZXY::ID) ? euler[0] : euler[0]);
00823 
00824       float sx = Math::sin( xRot );  float cx = Math::cos( xRot );
00825       float sy = Math::sin( yRot );  float cy = Math::cos( yRot );
00826       float sz = Math::sin( zRot );  float cz = Math::cos( zRot );
00827 
00828       // @todo metaprogram this!
00829       switch (order)
00830       {
00831       case XYZ::ID:
00832          // Derived by simply multiplying out the matrices by hand X * Y * Z
00833          result( 0, 0 ) = cy*cz;             result( 0, 1 ) = -cy*sz;            result( 0, 2 ) = sy;
00834          result( 1, 0 ) = sx*sy*cz + cx*sz;  result( 1, 1 ) = -sx*sy*sz + cx*cz; result( 1, 2 ) = -sx*cy;
00835          result( 2, 0 ) = -cx*sy*cz + sx*sz; result( 2, 1 ) = cx*sy*sz + sx*cz;  result( 2, 2 ) = cx*cy;
00836          break;
00837       case ZYX::ID:
00838          // Derived by simply multiplying out the matrices by hand Z * Y * Z
00839          result( 0, 0 ) = cy*cz; result( 0, 1 ) = -cx*sz + sx*sy*cz; result( 0, 2 ) = sx*sz + cx*sy*cz;
00840          result( 1, 0 ) = cy*sz; result( 1, 1 ) = cx*cz + sx*sy*sz;  result( 1, 2 ) = -sx*cz + cx*sy*sz;
00841          result( 2, 0 ) = -sy;   result( 2, 1 ) = sx*cy;             result( 2, 2 ) = cx*cy;
00842          break;
00843       case ZXY::ID:
00844          // Derived by simply multiplying out the matrices by hand Z * X * Y
00845          result( 0, 0 ) = cy*cz - sx*sy*sz; result( 0, 1 ) = -cx*sz; result( 0, 2 ) = sy*cz + sx*cy*sz;
00846          result( 1, 0 ) = cy*sz + sx*sy*cz; result( 1, 1 ) = cx*cz;  result( 1, 2 ) = sy*sz - sx*cy*cz;
00847          result( 2, 0 ) = -cx*sy;           result( 2, 1 ) = sx;     result( 2, 2 ) = cx*cy;
00848          break;
00849       default:
00850          gmtlASSERT( false && "unknown rotation order passed to setRot" );
00851          break;
00852       }
00853 
00854       return result;
00855    }
00856 
00860    template< typename DATA_TYPE, unsigned ROWS, unsigned COLS, typename ROT_ORDER >
00861    inline Matrix<DATA_TYPE, ROWS, COLS>& set( Matrix<DATA_TYPE, ROWS, COLS>& result, const EulerAngle<DATA_TYPE, ROT_ORDER>& euler )
00862    {
00863       gmtl::identity( result );
00864       return setRot( result, euler );
00865    }
00866 
00871    template< typename DATA_TYPE, unsigned ROWS, unsigned COLS >
00872    inline DATA_TYPE makeYRot( const Matrix<DATA_TYPE, ROWS, COLS>& mat )
00873    {
00874       const gmtl::Vec<DATA_TYPE, 3> forward_point(0.0f, 0.0f, -1.0f);   // -Z
00875       const gmtl::Vec<DATA_TYPE, 3> origin_point(0.0f, 0.0f, 0.0f);
00876       gmtl::Vec<DATA_TYPE, 3> end_point, start_point;
00877 
00878       gmtl::xform(end_point, mat, forward_point);
00879       gmtl::xform(start_point, mat, origin_point);
00880       gmtl::Vec<DATA_TYPE, 3> direction_vector = end_point - start_point;
00881 
00882       // Constrain the direction to XZ-plane only.
00883       direction_vector[1] = 0.0f;                  // Eliminate Y value
00884       gmtl::normalize(direction_vector);
00885       DATA_TYPE y_rot = gmtl::Math::aCos(gmtl::dot(direction_vector,
00886                                                    forward_point));
00887 
00888       gmtl::Vec<DATA_TYPE, 3> which_side = gmtl::cross(forward_point,
00889                                                        direction_vector);
00890 
00891       // If direction vector to "right" (negative) side of forward
00892       if ( which_side[1] < 0.0f )
00893       {
00894          y_rot = -y_rot;
00895       }
00896 
00897       return y_rot;
00898    }
00899 
00904    template< typename DATA_TYPE, unsigned ROWS, unsigned COLS >
00905    inline DATA_TYPE makeXRot( const Matrix<DATA_TYPE, ROWS, COLS>& mat )
00906    {
00907       const gmtl::Vec<DATA_TYPE, 3> forward_point(0.0f, 0.0f, -1.0f);   // -Z
00908       const gmtl::Vec<DATA_TYPE, 3> origin_point(0.0f, 0.0f, 0.0f);
00909       gmtl::Vec<DATA_TYPE, 3> end_point, start_point;
00910 
00911       gmtl::xform(end_point, mat, forward_point);
00912       gmtl::xform(start_point, mat, origin_point);
00913       gmtl::Vec<DATA_TYPE, 3> direction_vector = end_point - start_point;
00914 
00915       // Constrain the direction to YZ-plane only.
00916       direction_vector[0] = 0.0f;                  // Eliminate X value
00917       gmtl::normalize(direction_vector);
00918       DATA_TYPE x_rot = gmtl::Math::aCos(gmtl::dot(direction_vector,
00919                                                    forward_point));
00920 
00921       gmtl::Vec<DATA_TYPE, 3> which_side = gmtl::cross(forward_point,
00922                                                        direction_vector);
00923 
00924       // If direction vector to "bottom" (negative) side of forward
00925       if ( which_side[0] < 0.0f )
00926       {
00927          x_rot = -x_rot;
00928       }
00929 
00930       return x_rot;
00931    }
00932 
00937    template< typename DATA_TYPE, unsigned ROWS, unsigned COLS >
00938    inline DATA_TYPE makeZRot( const Matrix<DATA_TYPE, ROWS, COLS>& mat )
00939    {
00940       const gmtl::Vec<DATA_TYPE, 3> forward_point(0.0f, 0.0f, -1.0f);   // -Z
00941       const gmtl::Vec<DATA_TYPE, 3> origin_point(0.0f, 0.0f, 0.0f);
00942       gmtl::Vec<DATA_TYPE, 3> end_point, start_point;
00943 
00944       gmtl::xform(end_point, mat, forward_point);
00945       gmtl::xform(start_point, mat, origin_point);
00946       gmtl::Vec<DATA_TYPE, 3> direction_vector = end_point - start_point;
00947 
00948       // Constrain the direction to XY-plane only.
00949       direction_vector[2] = 0.0f;                  // Eliminate Z value
00950       gmtl::normalize(direction_vector);
00951       DATA_TYPE z_rot = gmtl::Math::aCos(gmtl::dot(direction_vector,
00952                                                    forward_point));
00953 
00954       gmtl::Vec<DATA_TYPE, 3> which_side = gmtl::cross(forward_point,
00955                                                        direction_vector);
00956 
00957       // If direction vector to "right" (negative) side of forward
00958       if ( which_side[2] < 0.0f )
00959       {
00960          z_rot = -z_rot;
00961       }
00962 
00963       return z_rot;
00964    }
00965 
00976    template< typename DATA_TYPE, unsigned ROWS, unsigned COLS >
00977    inline Matrix<DATA_TYPE, ROWS, COLS>& setDirCos( Matrix<DATA_TYPE, ROWS, COLS>& result,
00978                                                      const Vec<DATA_TYPE, 3>& xDestAxis,
00979                                                      const Vec<DATA_TYPE, 3>& yDestAxis, const Vec<DATA_TYPE, 3>& zDestAxis,
00980                                                      const Vec<DATA_TYPE, 3>& xSrcAxis = Vec<DATA_TYPE, 3>(1,0,0),
00981                                                      const Vec<DATA_TYPE, 3>& ySrcAxis = Vec<DATA_TYPE, 3>(0,1,0),
00982                                                      const Vec<DATA_TYPE, 3>& zSrcAxis = Vec<DATA_TYPE, 3>(0,0,1) )
00983    {
00984       // @todo set this a compile time assert...
00985       gmtlASSERT( ROWS >= 3 && COLS >= 3 && ROWS <= 4 && COLS <= 4 && "this is undefined for Matrix smaller than 3x3 or bigger than 4x4" );
00986 
00987       DATA_TYPE Xa, Xb, Xc;    // Direction cosines of the secondary x-axis
00988       DATA_TYPE Ya, Yb, Yc;    // Direction cosines of the secondary y-axis
00989       DATA_TYPE Za, Zb, Zc;    // Direction cosines of the secondary z-axis
00990 
00991       Xa = dot(xDestAxis, xSrcAxis);  Xb = dot(xDestAxis, ySrcAxis);  Xc = dot(xDestAxis, zSrcAxis);
00992       Ya = dot(yDestAxis, xSrcAxis);  Yb = dot(yDestAxis, ySrcAxis);  Yc = dot(yDestAxis, zSrcAxis);
00993       Za = dot(zDestAxis, xSrcAxis);  Zb = dot(zDestAxis, ySrcAxis);  Zc = dot(zDestAxis, zSrcAxis);
00994 
00995       // Set the matrix correctly
00996       result( 0, 0 ) = Xa; result( 0, 1 ) = Ya; result( 0, 2 ) = Za;
00997       result( 1, 0 ) = Xb; result( 1, 1 ) = Yb; result( 1, 2 ) = Zb;
00998       result( 2, 0 ) = Xc; result( 2, 1 ) = Yc; result( 2, 2 ) = Zc;
00999 
01000       return result;
01001    }
01002 
01007    template< typename DATA_TYPE, unsigned ROWS, unsigned COLS >
01008    inline Matrix<DATA_TYPE, ROWS, COLS>& setAxes( Matrix<DATA_TYPE, ROWS, COLS>& result,
01009                                                   const Vec<DATA_TYPE, 3>& xAxis,
01010                                                   const Vec<DATA_TYPE, 3>& yAxis,
01011                                                   const Vec<DATA_TYPE, 3>& zAxis )
01012    {
01013       // @todo set this a compile time assert...
01014       gmtlASSERT( ROWS >= 3 && COLS >= 3 && ROWS <= 4 && COLS <= 4 && "this is undefined for Matrix smaller than 3x3 or bigger than 4x4" );
01015 
01016       result( 0, 0 ) = xAxis[0];
01017       result( 1, 0 ) = xAxis[1];
01018       result( 2, 0 ) = xAxis[2];
01019 
01020       result( 0, 1 ) = yAxis[0];
01021       result( 1, 1 ) = yAxis[1];
01022       result( 2, 1 ) = yAxis[2];
01023 
01024       result( 0, 2 ) = zAxis[0];
01025       result( 1, 2 ) = zAxis[1];
01026       result( 2, 2 ) = zAxis[2];
01027 
01028       return result;
01029    }
01030 
01035    template< typename ROTATION_TYPE >
01036    inline ROTATION_TYPE makeAxes( const Vec<typename ROTATION_TYPE::DataType, 3>& xAxis,
01037                                   const Vec<typename ROTATION_TYPE::DataType, 3>& yAxis,
01038                                   const Vec<typename ROTATION_TYPE::DataType, 3>& zAxis,
01039                                   Type2Type< ROTATION_TYPE > t = Type2Type< ROTATION_TYPE >() )
01040    {
01041       gmtl::ignore_unused_variable_warning(t);
01042       ROTATION_TYPE temporary;
01043       return setAxes( temporary, xAxis, yAxis, zAxis );
01044    }
01045 
01050    template < typename DATA_TYPE, unsigned ROWS, unsigned COLS >
01051    inline Matrix<DATA_TYPE, ROWS, COLS> makeTranspose( const Matrix<DATA_TYPE, ROWS, COLS>& m )
01052    {
01053       Matrix<DATA_TYPE, ROWS, COLS> temporary( m );
01054       return transpose( temporary );
01055    }
01056 
01064    template< typename DATA_TYPE, unsigned ROWS, unsigned COLS >
01065    inline Matrix<DATA_TYPE, ROWS, COLS> makeInverse(const Matrix<DATA_TYPE, ROWS, COLS>& src)
01066    {
01067       Matrix<DATA_TYPE, ROWS, COLS> result;
01068       return invert( result, src );
01069    }
01070 
01075    template <typename DATATYPE, typename POS_TYPE, typename ROT_TYPE, unsigned MATCOLS, unsigned MATROWS >
01076    inline Matrix<DATATYPE, MATROWS, MATCOLS>& set( Matrix<DATATYPE, MATROWS, MATCOLS>& mat,
01077                                                    const Coord<POS_TYPE, ROT_TYPE>& coord )
01078    {
01079       // set to ident first...
01080       gmtl::identity( mat );
01081 
01082       // set translation
01083       // @todo metaprogram this out for 3x3 and 2x2 matrices
01084       if (MATCOLS == 4)
01085       {
01086          setTrans( mat, coord.getPos() );// filtered (only sets the trans part).
01087       }
01088       setRot( mat, coord.getRot() ); // filtered (only sets the rot part).
01089       return mat;
01090    }
01091 
01095    template <typename DATA_TYPE, unsigned ROWS, unsigned COLS>
01096    Matrix<DATA_TYPE, ROWS, COLS>& setRot( Matrix<DATA_TYPE, ROWS, COLS>& mat, const Quat<DATA_TYPE>& q )
01097    {
01098       gmtlASSERT( ((ROWS == 3 && COLS == 3) ||
01099                (ROWS == 3 && COLS == 4) ||
01100                (ROWS == 4 && COLS == 3) ||
01101                (ROWS == 4 && COLS == 4)) &&
01102                "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." );
01103 
01104       // From Watt & Watt
01105       DATA_TYPE wx, wy, wz, xx, yy, yz, xy, xz, zz, xs, ys, zs;
01106 
01107       xs = q[Xelt] + q[Xelt]; ys = q[Yelt] + q[Yelt]; zs = q[Zelt] + q[Zelt];
01108       xx = q[Xelt] * xs;      xy = q[Xelt] * ys;      xz = q[Xelt] * zs;
01109       yy = q[Yelt] * ys;      yz = q[Yelt] * zs;      zz = q[Zelt] * zs;
01110       wx = q[Welt] * xs;      wy = q[Welt] * ys;      wz = q[Welt] * zs;
01111 
01112       mat( 0, 0 ) = DATA_TYPE(1.0) - (yy + zz);
01113       mat( 1, 0 ) = xy + wz;
01114       mat( 2, 0 ) = xz - wy;
01115 
01116       mat( 0, 1 ) = xy - wz;
01117       mat( 1, 1 ) = DATA_TYPE(1.0) - (xx + zz);
01118       mat( 2, 1 ) = yz + wx;
01119 
01120       mat( 0, 2 ) = xz + wy;
01121       mat( 1, 2 ) = yz - wx;
01122       mat( 2, 2 ) = DATA_TYPE(1.0) - (xx + yy);
01123 
01124       return mat;
01125    }
01126 
01132    template <typename DATA_TYPE, unsigned ROWS, unsigned COLS>
01133    Matrix<DATA_TYPE, ROWS, COLS>& set( Matrix<DATA_TYPE, ROWS, COLS>& mat, const Quat<DATA_TYPE>& q )
01134    {
01135       setRot( mat, q );
01136 
01137       if (ROWS == 4)
01138       {
01139          mat( 3, 0 ) = DATA_TYPE(0.0);
01140          mat( 3, 1 ) = DATA_TYPE(0.0);
01141          mat( 3, 2 ) = DATA_TYPE(0.0);
01142       }
01143 
01144       if (COLS == 4)
01145       {
01146          mat( 0, 3 ) = DATA_TYPE(0.0);
01147          mat( 1, 3 ) = DATA_TYPE(0.0);
01148          mat( 2, 3 ) = DATA_TYPE(0.0);
01149       }
01150 
01151       if (ROWS == 4 && COLS == 4)
01152          mat( 3, 3 ) = DATA_TYPE(1.0);
01153 
01154       return mat;
01155    }
01156 
01165    template <typename DATATYPE, typename POS_TYPE, typename ROT_TYPE, unsigned MATCOLS, unsigned MATROWS >
01166    inline Coord<POS_TYPE, ROT_TYPE>& set( Coord<POS_TYPE, ROT_TYPE>& eulercoord, const Matrix<DATATYPE, MATROWS, MATCOLS>& mat )
01167    {
01168       gmtl::setTrans( eulercoord.pos(), mat );
01169       gmtl::set( eulercoord.rot(), mat );
01170       return eulercoord;
01171    }
01172 
01176    template <typename DATATYPE, typename POS_TYPE, typename ROT_TYPE, unsigned MATCOLS, unsigned MATROWS >
01177    inline Coord<POS_TYPE, ROT_TYPE>& setRot( Coord<POS_TYPE, ROT_TYPE>& result, const Matrix<DATATYPE, MATROWS, MATCOLS>& mat )
01178    {
01179       return gmtl::set( result, mat );
01180    }
01181 
01184 } // end gmtl namespace.
01185 
01186 #endif