math.h
Go to the documentation of this file.
00001 // Copyright (C) 2003-2008 Rosen Diankov
00002 // Computer Science Department
00003 // Carnegie Mellon University, Robotics Institute
00004 // 5000 Forbes Ave. Pittsburgh, PA  15213
00005 // U.S.A.
00006 //
00007 // This file is part of OpenRAVE.
00008 // OpenRAVE is free software: you can redistribute it and/or modify
00009 // it under the terms of the GNU Lesser General Public License as published by
00010 // the Free Software Foundation, either version 3 of the License, or
00011 // at your option) any later version.
00012 //
00013 // This program is distributed in the hope that it will be useful,
00014 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00015 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00016 // GNU Lesser General Public License for more details.
00017 //
00018 // You should have received a copy of the GNU Lesser General Public License
00019 // along with this program.  If not, see <http://www.gnu.org/licenses/>.
00028 #ifndef RAVE_MATH_H
00029 #define RAVE_MATH_H
00030 
00031 #ifndef ODE_API
00032 
00033 // special defines from ODE in case ODE isn't present
00034 
00035 #define dSINGLE
00036 
00037 #if defined(dSINGLE)
00038 typedef float dReal;
00039 #define dSqrt(x) (sqrtf(x))                     // square root 
00040 #define dRecip(x) ((1.0f/(x)))                          // reciprocal 
00041 #define dSin(x) (sinf(x))                               // sin
00042 #define dCos(x) (cosf(x))                               // cos
00043 #else
00044 typedef double dReal;
00045 #define dSqrt(x) (sqrt(x))                      // square root
00046 #define dRecip(x) ((1.0/(x)))                           // reciprocal
00047 #define dSin(x) (sin(x))                                // sin
00048 #define dCos(x) (cos(x))                                // cos
00049 #endif
00050 typedef dReal dVector3[4];
00051 typedef dReal dVector4[4];
00052 typedef dReal dMatrix3[4*3];
00053 typedef dReal dMatrix4[4*4];
00054 typedef dReal dMatrix6[8*6];
00055 typedef dReal dQuaternion[4];
00056 
00057 #define _R(i,j) R[(i)*4+(j)]
00058 
00059 
00060 inline void dQMultiply0 (dQuaternion qa, const dQuaternion qb, const dQuaternion qc)
00061 {
00062     assert(qa && qb && qc);
00063     qa[0] = qb[0]*qc[0] - qb[1]*qc[1] - qb[2]*qc[2] - qb[3]*qc[3];
00064     qa[1] = qb[0]*qc[1] + qb[1]*qc[0] + qb[2]*qc[3] - qb[3]*qc[2];
00065     qa[2] = qb[0]*qc[2] + qb[2]*qc[0] + qb[3]*qc[1] - qb[1]*qc[3];
00066     qa[3] = qb[0]*qc[3] + qb[3]*qc[0] + qb[1]*qc[2] - qb[2]*qc[1];
00067 }
00068 
00069 inline void dRfromQ (dMatrix3 R, const dQuaternion q)
00070 {
00071     assert (q && R);
00072     // q = (s,vx,vy,vz)
00073     dReal qq1 = 2*q[1]*q[1];
00074     dReal qq2 = 2*q[2]*q[2];
00075     dReal qq3 = 2*q[3]*q[3];
00076     _R(0,0) = 1 - qq2 - qq3;
00077     _R(0,1) = 2*(q[1]*q[2] - q[0]*q[3]);
00078     _R(0,2) = 2*(q[1]*q[3] + q[0]*q[2]);
00079     _R(0,3) = (dReal)(0.0);
00080     _R(1,0) = 2*(q[1]*q[2] + q[0]*q[3]);
00081     _R(1,1) = 1 - qq1 - qq3;
00082     _R(1,2) = 2*(q[2]*q[3] - q[0]*q[1]);
00083     _R(1,3) = (dReal)(0.0);
00084     _R(2,0) = 2*(q[1]*q[3] - q[0]*q[2]);
00085     _R(2,1) = 2*(q[2]*q[3] + q[0]*q[1]);
00086     _R(2,2) = 1 - qq1 - qq2;
00087     _R(2,3) = (dReal)(0.0);
00088 }
00089 
00090 inline void dQfromR (dQuaternion q, const dMatrix3 R)
00091 {
00092     assert(q && R);
00093     dReal tr,s;
00094     tr = _R(0,0) + _R(1,1) + _R(2,2);
00095     if (tr >= 0) {
00096         s = dSqrt (tr + 1);
00097         q[0] = (dReal)(0.5) * s;
00098         s = (dReal)(0.5) * dRecip(s);
00099         q[1] = (_R(2,1) - _R(1,2)) * s;
00100         q[2] = (_R(0,2) - _R(2,0)) * s;
00101         q[3] = (_R(1,0) - _R(0,1)) * s;
00102     }
00103     else {
00104         // find the largest diagonal element and jump to the appropriate case
00105         if (_R(1,1) > _R(0,0)) {
00106             if (_R(2,2) > _R(1,1)) goto case_2;
00107             goto case_1;
00108         }
00109         if (_R(2,2) > _R(0,0)) goto case_2;
00110         goto case_0;
00111         
00112     case_0:
00113         s = dSqrt((_R(0,0) - (_R(1,1) + _R(2,2))) + 1);
00114         q[1] = (dReal)(0.5) * s;
00115         s = (dReal)(0.5) * dRecip(s);
00116         q[2] = (_R(0,1) + _R(1,0)) * s;
00117         q[3] = (_R(2,0) + _R(0,2)) * s;
00118         q[0] = (_R(2,1) - _R(1,2)) * s;
00119         return;
00120         
00121     case_1:
00122         s = dSqrt((_R(1,1) - (_R(2,2) + _R(0,0))) + 1);
00123         q[2] = (dReal)(0.5) * s;
00124         s = (dReal)(0.5) * dRecip(s);
00125         q[3] = (_R(1,2) + _R(2,1)) * s;
00126         q[1] = (_R(0,1) + _R(1,0)) * s;
00127         q[0] = (_R(0,2) - _R(2,0)) * s;
00128         return;
00129         
00130     case_2:
00131         s = dSqrt((_R(2,2) - (_R(0,0) + _R(1,1))) + 1);
00132         q[3] = (dReal)(0.5) * s;
00133         s = (dReal)(0.5) * dRecip(s);
00134         q[1] = (_R(2,0) + _R(0,2)) * s;
00135         q[2] = (_R(1,2) + _R(2,1)) * s;
00136         q[0] = (_R(1,0) - _R(0,1)) * s;
00137         return;
00138     }
00139 }
00140 
00141 #undef _R
00142 
00143 #endif
00144 
00145 #ifndef PI
00146 #define PI ((dReal)3.141592654)
00147 #endif
00148 
00149 #define rswap(x, y) *(int*)&(x) ^= *(int*)&(y) ^= *(int*)&(x) ^= *(int*)&(y);
00150 
00151 #define g_fEpsilon 1e-8
00152 #define distinctRoots   0                       // roots r0 < r1 < r2
00153 #define singleRoot              1                       // root r0
00154 #define floatRoot01             2                       // roots r0 = r1 < r2
00155 #define floatRoot12             4                       // roots r0 < r1 = r2
00156 #define tripleRoot              6                       // roots r0 = r1 = r2
00157 
00158 template <class T> inline T RAD_2_DEG(T radians)  { return (radians * (T)57.29577951); }
00159 
00160 template <class T> class RaveTransform;
00161 template <class T> class RaveTransformMatrix;
00162 
00163 // multiplies 4x4 matrices
00164 inline float* mult4(float* pfres, const float* pf1, const float* pf2);
00165 
00166 // pf1^T * pf2
00167 inline float* multtrans3(float* pfres, const float* pf1, const float* pf2);
00168 inline float* multtrans4(float* pfres, const float* pf1, const float* pf2);
00169 inline float* transnorm3(float* pfout, const float* pfmat, const float* pf);
00170 
00171 inline float* transpose3(const float* pf, float* pfres);
00172 inline float* transpose4(const float* pf, float* pfres);
00173 
00174 inline float dot2(const float* pf1, const float* pf2);
00175 inline float dot3(const float* pf1, const float* pf2);
00176 inline float dot4(const float* pf1, const float* pf2);
00177 
00178 inline float lengthsqr2(const float* pf);
00179 inline float lengthsqr3(const float* pf);
00180 inline float lengthsqr4(const float* pf);
00181 
00182 inline float* normalize2(float* pfout, const float* pf);
00183 inline float* normalize3(float* pfout, const float* pf);
00184 inline float* normalize4(float* pfout, const float* pf);
00185 
00186 inline float* cross3(float* pfout, const float* pf1, const float* pf2);
00187 
00188 // multiplies 3x3 matrices
00189 inline float* mult3_s4(float* pfres, const float* pf1, const float* pf2);
00190 inline float* mult3_s3(float* pfres, const float* pf1, const float* pf2);
00191 
00192 inline float* inv3(const float* pf, float* pfres, float* pfdet, int stride);
00193 inline float* inv4(const float* pf, float* pfres);
00194 
00195 
00196 inline double* mult4(double* pfres, const double* pf1, const double* pf2);
00197 
00198 // pf1^T * pf2
00199 inline double* multtrans3(double* pfres, const double* pf1, const double* pf2);
00200 inline double* multtrans4(double* pfres, const double* pf1, const double* pf2);
00201 inline double* transnorm3(double* pfout, const double* pfmat, const double* pf);
00202 
00203 inline double* transpose3(const double* pf, double* pfres);
00204 inline double* transpose4(const double* pf, double* pfres);
00205 
00206 inline double dot2(const double* pf1, const double* pf2);
00207 inline double dot3(const double* pf1, const double* pf2);
00208 inline double dot4(const double* pf1, const double* pf2);
00209 
00210 inline double lengthsqr2(const double* pf);
00211 inline double lengthsqr3(const double* pf);
00212 inline double lengthsqr4(const double* pf);
00213 
00214 inline double* normalize2(double* pfout, const double* pf);
00215 inline double* normalize3(double* pfout, const double* pf);
00216 inline double* normalize4(double* pfout, const double* pf);
00217 
00218 inline double* cross3(double* pfout, const double* pf1, const double* pf2);
00219 
00220 // multiplies 3x3 matrices
00221 inline double* mult3_s4(double* pfres, const double* pf1, const double* pf2);
00222 inline double* mult3_s3(double* pfres, const double* pf1, const double* pf2);
00223 
00224 inline double* inv3(const double* pf, double* pfres, double* pfdet, int stride);
00225 inline double* inv4(const double* pf, double* pfres);
00226 
00227 
00228 inline float RaveSqrt(float f) { return sqrtf(f); }
00229 inline double RaveSqrt(double f) { return sqrt(f); }
00230 inline float RaveSin(float f) { return sinf(f); }
00231 inline double RaveSin(double f) { return sin(f); }
00232 inline float RaveCos(float f) { return cosf(f); }
00233 inline double RaveCos(double f) { return cos(f); }
00234 inline float RaveFabs(float f) { return fabsf(f); }
00235 inline double RaveFabs(double f) { return fabs(f); }
00236 inline float RaveAcos(float f) { return acosf(f); }
00237 inline double RaveAcos(double f) { return acos(f); }
00238 
00241 template <class T>
00242 class RaveVector
00243 {
00244 public:
00245     T x, y, z, w;
00246     
00247     RaveVector() : x(0), y(0), z(0), w(0) {}
00248         
00249     RaveVector(T x, T y, T z) : x(x), y(y), z(z), w(0) {}
00250     RaveVector(T x, T y, T z, T w) : x(x), y(y), z(z), w(w) {}
00251     template<class U> RaveVector(const RaveVector<U> &vec) : x((T)vec.x), y((T)vec.y), z((T)vec.z), w((T)vec.w) {}
00252 
00254     template<class U> RaveVector(const U* pf) { assert(pf != NULL); x = (T)pf[0]; y = (T)pf[1]; z = (T)pf[2]; w = 0; }
00255     
00256     T  operator[](int i) const       { return (&x)[i]; }
00257     T& operator[](int i)             { return (&x)[i]; }
00258     
00259     // casting operators
00260     operator T* () { return &x; }
00261     operator const T* () const { return (const T*)&x; }
00262 
00263     template <class U>
00264     RaveVector<T>& operator=(const RaveVector<U>& r) { x = (T)r.x; y = (T)r.y; z = (T)r.z; w = (T)r.w; return *this; }
00265     
00266     // SCALAR FUNCTIONS
00267     template <class U> inline T dot(const RaveVector<U> &v) const { return x*v.x + y*v.y + z*v.z + w*v.w; }
00268     inline RaveVector<T>& normalize() { return normalize4(); }
00269 
00270     inline RaveVector<T>& normalize4() {
00271         T f = x*x+y*y+z*z+w*w;
00272         assert( f > 0 );
00273         f = 1.0f / RaveSqrt(f);
00274         x *= f; y *= f; z *= f; w *= f;
00275         return *this;
00276     }
00277     inline RaveVector<T>& normalize3() {
00278         T f = x*x+y*y+z*z;
00279         assert( f > 0 );
00280         f = 1.0f / RaveSqrt(f);
00281         x *= f; y *= f; z *= f;
00282         return *this;
00283     }
00284 
00285     inline T lengthsqr2() const { return x*x + y*y; }
00286     inline T lengthsqr3() const { return x*x + y*y + z*z; }
00287     inline T lengthsqr4() const { return x*x + y*y + z*z + w*w; }
00288 
00289     inline void Set3(const T* pvals) { x = pvals[0]; y = pvals[1]; z = pvals[2]; }
00290     inline void Set3(T val1, T val2, T val3) { x = val1; y = val2; z = val3; }
00291     inline void Set4(const T* pvals) { x = pvals[0]; y = pvals[1]; z = pvals[2]; w = pvals[3]; }
00292     inline void Set4(T val1, T val2, T val3, T val4) { x = val1; y = val2; z = val3; w = val4;}
00295     inline RaveVector<T>& Cross(const RaveVector<T> &v) { Cross(*this, v); return *this; }
00296 
00298     inline RaveVector<T>& Cross(const RaveVector<T> &u, const RaveVector<T> &v) {
00299         RaveVector<T> ucrossv;
00300         ucrossv[0] = u[1] * v[2] - u[2] * v[1];
00301         ucrossv[1] = u[2] * v[0] - u[0] * v[2];
00302         ucrossv[2] = u[0] * v[1] - u[1] * v[0];
00303         *this = ucrossv;
00304         return *this;
00305     }
00306 
00307     inline RaveVector<T> operator-() const { RaveVector<T> v; v.x = -x; v.y = -y; v.z = -z; v.w = -w; return v; }
00308     template <class U> inline RaveVector<T> operator+(const RaveVector<U> &r) const { RaveVector<T> v; v.x = x+r.x; v.y = y+r.y; v.z = z+r.z; v.w = w+r.w; return v; }
00309     template <class U> inline RaveVector<T> operator-(const RaveVector<U> &r) const { RaveVector<T> v; v.x = x-r.x; v.y = y-r.y; v.z = z-r.z; v.w = w-r.w; return v; }
00310     template <class U> inline RaveVector<T> operator*(const RaveVector<U> &r) const { RaveVector<T> v; v.x = r.x*x; v.y = r.y*y; v.z = r.z*z; v.w = r.w*w; return v; }
00311     inline RaveVector<T> operator*(T k) const { RaveVector<T> v; v.x = k*x; v.y = k*y; v.z = k*z; v.w = k*w; return v; }
00312 
00313     template <class U> inline RaveVector<T>& operator += (const RaveVector<U>& r) { x += r.x; y += r.y; z += r.z; w += r.w; return *this; }
00314     template <class U> inline RaveVector<T>& operator -= (const RaveVector<U>& r) { x -= r.x; y -= r.y; z -= r.z; w -= r.w; return *this; }
00315     template <class U> inline RaveVector<T>& operator *= (const RaveVector<U>& r) { x *= r.x; y *= r.y; z *= r.z; w *= r.w; return *this; }
00316     
00317     inline RaveVector<T>& operator *= (const T k) { x *= k; y *= k; z *= k; w *= k; return *this; }
00318     inline RaveVector<T>& operator /= (const T _k) { T k=1/_k; x *= k; y *= k; z *= k; w *= k; return *this; }
00319 
00320     template <class U> friend RaveVector<U> operator* (float f, const RaveVector<U>& v);
00321     template <class U> friend RaveVector<U> operator* (double f, const RaveVector<U>& v);
00322     
00323     template <class S, class U> friend std::basic_ostream<S>& operator<<(std::basic_ostream<S>& O, const RaveVector<U>& v);
00324     template <class S, class U> friend std::basic_istream<S>& operator>>(std::basic_istream<S>& I, RaveVector<U>& v);
00325 
00327     template <class U> inline RaveVector<T> operator^(const RaveVector<U> &v) const { 
00328         RaveVector<T> ucrossv;
00329         ucrossv[0] = y * v[2] - z * v[1];
00330         ucrossv[1] = z * v[0] - x * v[2];
00331         ucrossv[2] = x * v[1] - y * v[0];
00332         return ucrossv;
00333     }
00334 };
00335 
00336 typedef RaveVector<dReal> Vector;
00337 
00338 template <class T>
00339 inline RaveVector<T> operator* (float f, const RaveVector<T>& left)
00340 {
00341     RaveVector<T> v;
00342     v.x = (T)f * left.x;
00343     v.y = (T)f * left.y;
00344     v.z = (T)f * left.z;
00345     v.w = (T)f * left.w;
00346     return v;
00347 }
00348 
00349 template <class T>
00350 inline RaveVector<T> operator* (double f, const RaveVector<T>& left)
00351 {
00352     RaveVector<T> v;
00353     v.x = (T)f * left.x;
00354     v.y = (T)f * left.y;
00355     v.z = (T)f * left.z;
00356     v.w = (T)f * left.w;
00357     return v;
00358 }
00359 
00361 template <class T>
00362 class RaveTransform
00363 {
00364 public:
00365     RaveTransform() { rot.x = 1; }
00366     template <class U> RaveTransform(const RaveTransform<U>& t) {
00367         rot = t.rot;
00368         trans = t.trans;
00369     }
00370     template <class U> RaveTransform(const RaveVector<U>& rot, const RaveVector<U>& trans) : rot(rot), trans(trans) {}
00371     inline RaveTransform(const RaveTransformMatrix<T>& t);
00372 
00373     void identity() {
00374         rot.x = 1; rot.y = rot.z = rot.w = 0;
00375         trans.x = trans.y = trans.z = 0;
00376     }
00377     
00378     template <class U>
00379     inline void rotfromaxisangle(const RaveVector<U>& axis, U angle) {
00380         U sinang = (U)RaveSin(angle/2);
00381         rot.x = (U)RaveCos(angle/2);
00382         rot.y = axis.x*sinang;
00383         rot.z = axis.y*sinang;
00384         rot.w = axis.z*sinang;
00385     }
00386 
00388     inline RaveVector<T> operator* (const RaveVector<T>& r) const {
00389         return trans + rotate(r);
00390     }
00391 
00392     inline RaveVector<T> rotate(const RaveVector<T>& r) const {
00393         T xx = 2 * rot.y * rot.y;
00394         T xy = 2 * rot.y * rot.z;
00395         T xz = 2 * rot.y * rot.w;
00396         T xw = 2 * rot.y * rot.x;
00397         T yy = 2 * rot.z * rot.z;
00398         T yz = 2 * rot.z * rot.w;
00399         T yw = 2 * rot.z * rot.x;
00400         T zz = 2 * rot.w * rot.w;
00401         T zw = 2 * rot.w * rot.x;
00402 
00403         RaveVector<T> v;
00404         v.x = (1-yy-zz) * r.x + (xy-zw) * r.y + (xz+yw)*r.z;
00405         v.y = (xy+zw) * r.x + (1-xx-zz) * r.y + (yz-xw)*r.z;
00406         v.z = (xz-yw) * r.x + (yz+xw) * r.y + (1-xx-yy)*r.z;
00407         return v;
00408     }
00409 
00411     inline RaveTransform<T> operator* (const RaveTransform<T>& r) const {
00412         RaveTransform<T> t;
00413         t.trans = operator*(r.trans);
00414         t.rot.x = rot.x*r.rot.x - rot.y*r.rot.y - rot.z*r.rot.z - rot.w*r.rot.w;
00415         t.rot.y = rot.x*r.rot.y + rot.y*r.rot.x + rot.z*r.rot.w - rot.w*r.rot.z;
00416         t.rot.z = rot.x*r.rot.z + rot.z*r.rot.x + rot.w*r.rot.y - rot.y*r.rot.w;
00417         t.rot.w = rot.x*r.rot.w + rot.w*r.rot.x + rot.y*r.rot.z - rot.z*r.rot.y;
00418 
00419         return t;
00420     }
00421     
00422     inline RaveTransform<T>& operator*= (const RaveTransform<T>& right) {
00423         *this = operator*(right);
00424         return *this;
00425     }
00426 
00427     inline RaveTransform<T> inverse() const {
00428         RaveTransform<T> inv;
00429         inv.rot.x = rot.x;
00430         inv.rot.y = -rot.y;
00431         inv.rot.z = -rot.z;
00432         inv.rot.w = -rot.w;
00433         
00434         inv.trans = -inv.rotate(trans);
00435         return inv;
00436     }
00437 
00438     template <class S, class U> friend std::basic_ostream<S>& operator<<(std::basic_ostream<S>& O, const RaveTransform<U>& v);
00439     template <class S, class U> friend std::basic_istream<S>& operator>>(std::basic_istream<S>& I, RaveTransform<U>& v);
00440 
00441     RaveVector<T> rot, trans; 
00442 };
00443 
00444 typedef RaveTransform<dReal> Transform;
00445 
00447 template <class T>
00448 class RaveTransformMatrix
00449 {
00450 public:
00451     inline RaveTransformMatrix() { identity(); m[3] = m[7] = m[11] = 0; }
00452     template <class U> RaveTransformMatrix(const RaveTransformMatrix<U>& t) {
00453         // don't memcpy!
00454         m[0] = t.m[0]; m[1] = t.m[1]; m[2] = t.m[2]; m[3] = t.m[3];
00455         m[4] = t.m[4]; m[5] = t.m[5]; m[6] = t.m[6]; m[7] = t.m[7];
00456         m[8] = t.m[8]; m[9] = t.m[9]; m[10] = t.m[10]; m[11] = t.m[11];
00457         trans = t.trans;
00458     }
00459     inline RaveTransformMatrix(const RaveTransform<T>& t);
00460 
00461     inline void identity() {
00462         m[0] = 1; m[1] = 0; m[2] = 0;
00463         m[4] = 0; m[5] = 1; m[6] = 0;
00464         m[8] = 0; m[9] = 0; m[10] = 1;
00465         trans.x = trans.y = trans.z = 0;
00466     }
00467     
00468     template <class U>
00469     inline void rotfromaxisangle(const RaveVector<U>& axis, U angle) {
00470         RaveVector<T> quat;
00471         U sinang = (U)RaveSin(angle/2);
00472         quat.x = (U)RaveCos(angle/2);
00473         quat.y = axis.x*sinang;
00474         quat.z = axis.y*sinang;
00475         quat.w = axis.z*sinang;
00476         rotfromquat(quat);
00477     }
00478 
00479     template <class U>
00480     inline void rotfromquat(const RaveVector<U>& quat) {
00481             // q = (s,vx,vy,vz)
00482         T qq1 = 2*quat[1]*quat[1];
00483         T qq2 = 2*quat[2]*quat[2];
00484         T qq3 = 2*quat[3]*quat[3];
00485         m[4*0+0] = 1 - qq2 - qq3;
00486         m[4*0+1] = 2*(quat[1]*quat[2] - quat[0]*quat[3]);
00487         m[4*0+2] = 2*(quat[1]*quat[3] + quat[0]*quat[2]);
00488         m[4*0+3] = 0;
00489         m[4*1+0] = 2*(quat[1]*quat[2] + quat[0]*quat[3]);
00490         m[4*1+1] = 1 - qq1 - qq3;
00491         m[4*1+2] = 2*(quat[2]*quat[3] - quat[0]*quat[1]);
00492         m[4*1+3] = 0;
00493         m[4*2+0] = 2*(quat[1]*quat[3] - quat[0]*quat[2]);
00494         m[4*2+1] = 2*(quat[2]*quat[3] + quat[0]*quat[1]);
00495         m[4*2+2] = 1 - qq1 - qq2;
00496         m[4*2+3] = 0;
00497     }
00498     
00499     inline void rotfrommat(T m_00, T m_01, T m_02, T m_10, T m_11, T m_12, T m_20, T m_21, T m_22) {
00500         m[0] = m_00; m[1] = m_01; m[2] = m_02; m[3] = 0;
00501         m[4] = m_10; m[5] = m_11; m[6] = m_12; m[7] = 0;
00502         m[8] = m_20; m[9] = m_21; m[10] = m_22; m[11] = 0;
00503     }
00504 
00505     inline T rot(int i, int j) const {
00506         assert( i >= 0 && i < 3 && j >= 0 && j < 3);
00507         return m[4*i+j];
00508     }
00509     inline T& rot(int i, int j) {
00510         assert( i >= 0 && i < 3 && j >= 0 && j < 3);
00511         return m[4*i+j];
00512     }
00513     
00514     template <class U>
00515     inline RaveVector<T> operator* (const RaveVector<U>& r) const {
00516         RaveVector<T> v;
00517         v[0] = r[0] * m[0] + r[1] * m[1] + r[2] * m[2] + trans.x;
00518         v[1] = r[0] * m[4] + r[1] * m[5] + r[2] * m[6] + trans.y;
00519         v[2] = r[0] * m[8] + r[1] * m[9] + r[2] * m[10] + trans.z;
00520         return v;
00521     }
00522 
00524     inline RaveTransformMatrix<T> operator* (const RaveTransformMatrix<T>& r) const {
00525         RaveTransformMatrix<T> t;
00526         mult3_s4(&t.m[0], &m[0], &r.m[0]);
00527         t.trans[0] = r.trans[0] * m[0] + r.trans[1] * m[1] + r.trans[2] * m[2] + trans.x;
00528         t.trans[1] = r.trans[0] * m[4] + r.trans[1] * m[5] + r.trans[2] * m[6] + trans.y;
00529         t.trans[2] = r.trans[0] * m[8] + r.trans[1] * m[9] + r.trans[2] * m[10] + trans.z;
00530         return t;
00531     }
00532 
00533     inline RaveTransformMatrix<T> operator*= (const RaveTransformMatrix<T>& r) const {
00534         *this = *this * r;
00535         return *this;
00536     }
00537 
00538     template <class U>
00539     inline RaveVector<U> rotate(const RaveVector<U>& r) const {
00540         RaveVector<U> v;
00541         v.x = r.x * m[0] + r.y * m[1] + r.z * m[2];
00542         v.y = r.x * m[4] + r.y * m[5] + r.z * m[6];
00543         v.z = r.x * m[8] + r.y * m[9] + r.z * m[10];
00544         return v;
00545     }
00546     inline RaveTransformMatrix<T> inverse() const {
00547         RaveTransformMatrix<T> inv;
00548         inv3(m, inv.m, NULL, 4);
00549         inv.trans = -inv.rotate(trans);
00550         return inv;
00551     }
00552 
00553     template <class U>
00554     inline void Extract(RaveVector<U>& right, RaveVector<U>& up, RaveVector<U>& dir, RaveVector<U>& pos) const {
00555         pos = trans;
00556         right.x = m[0]; up.x = m[1]; dir.x = m[2];
00557         right.y = m[4]; up.y = m[5]; dir.y = m[6];
00558         right.z = m[8]; up.z = m[9]; dir.z = m[10];
00559     }
00560 
00561     template <class S, class U> friend std::basic_ostream<S>& operator<<(std::basic_ostream<S>& O, const RaveTransformMatrix<U>& v);
00562     template <class S, class U> friend std::basic_istream<S>& operator>>(std::basic_istream<S>& I, RaveTransformMatrix<U>& v);
00563 
00566     T m[12];
00567     RaveVector<T> trans; 
00568 };
00569 typedef RaveTransformMatrix<dReal> TransformMatrix;
00570 
00571 template <class T>
00572 RaveTransform<T>::RaveTransform(const RaveTransformMatrix<T>& t)
00573 {
00574     trans = t.trans;
00575     dReal tr,s;
00576     tr = t.m[4*0+0] + t.m[4*1+1] + t.m[4*2+2];
00577     if (tr >= 0) {
00578         s = RaveSqrt(tr + 1);
00579         rot[0] = (dReal)(0.5) * s;
00580         s = (dReal)(0.5) * dRecip(s);
00581         rot[1] = (t.m[4*2+1] - t.m[4*1+2]) * s;
00582         rot[2] = (t.m[4*0+2] - t.m[4*2+0]) * s;
00583         rot[3] = (t.m[4*1+0] - t.m[4*0+1]) * s;
00584     }
00585     else {
00586         // find the largest diagonal element and jump to the appropriate case
00587         if (t.m[4*1+1] > t.m[4*0+0]) {
00588             if (t.m[4*2+2] > t.m[4*1+1]) goto case_2;
00589             goto case_1;
00590         }
00591         if (t.m[4*2+2] > t.m[4*0+0]) goto case_2;
00592         goto case_0;
00593         
00594     case_0:
00595         s = RaveSqrt((t.m[4*0+0] - (t.m[4*1+1] + t.m[4*2+2])) + 1);
00596         rot[1] = (dReal)(0.5) * s;
00597         s = (dReal)(0.5) * dRecip(s);
00598         rot[2] = (t.m[4*0+1] + t.m[4*1+0]) * s;
00599         rot[3] = (t.m[4*2+0] + t.m[4*0+2]) * s;
00600         rot[0] = (t.m[4*2+1] - t.m[4*1+2]) * s;
00601         return;
00602         
00603     case_1:
00604         s = RaveSqrt((t.m[4*1+1] - (t.m[4*2+2] + t.m[4*0+0])) + 1);
00605         rot[2] = (dReal)(0.5) * s;
00606         s = (dReal)(0.5) * dRecip(s);
00607         rot[3] = (t.m[4*1+2] + t.m[4*2+1]) * s;
00608         rot[1] = (t.m[4*0+1] + t.m[4*1+0]) * s;
00609         rot[0] = (t.m[4*0+2] - t.m[4*2+0]) * s;
00610         return;
00611         
00612     case_2:
00613         s = RaveSqrt((t.m[4*2+2] - (t.m[4*0+0] + t.m[4*1+1])) + 1);
00614         rot[3] = (dReal)(0.5) * s;
00615         s = (dReal)(0.5) * dRecip(s);
00616         rot[1] = (t.m[4*2+0] + t.m[4*0+2]) * s;
00617         rot[2] = (t.m[4*1+2] + t.m[4*2+1]) * s;
00618         rot[0] = (t.m[4*1+0] - t.m[4*0+1]) * s;
00619         return;
00620     }
00621 }
00622 
00623 template <class T>
00624 RaveTransformMatrix<T>::RaveTransformMatrix(const RaveTransform<T>& t)
00625 {
00626     rotfromquat(t.rot);
00627     trans = t.trans;
00628 
00629 }
00630 
00631 struct RAY
00632 {
00633     RAY() {}
00634     RAY(const Vector& _pos, const Vector& _dir) : pos(_pos), dir(_dir) {}
00635     Vector pos, dir;
00636 };
00637 
00638 struct AABB
00639 {
00640     AABB() {}
00641     AABB(const Vector& vpos, const Vector& vextents) : pos(vpos), extents(vextents) {}
00642     Vector pos, extents;
00643 };
00644 
00645 struct OBB
00646 {
00647     Vector right, up, dir, pos, extents;
00648 };
00649 
00650 struct TRIANGLE
00651 {
00652     TRIANGLE() {}
00653     TRIANGLE(const Vector& v1, const Vector& v2, const Vector& v3) : v1(v1), v2(v2), v3(v3) {}
00654     ~TRIANGLE() {}
00655 
00656     Vector v1, v2, v3;      
00657 
00658     const Vector& operator[](int i) const { return (&v1)[i]; }
00659     Vector&       operator[](int i)       { return (&v1)[i]; }
00660 
00662     inline Vector ComputeNormal() {
00663         Vector normal;
00664         cross3(normal, v2-v1, v3-v1);
00665         return normal;
00666     }
00667 };
00668 
00669 
00671 inline dReal* transcoord3(dReal* pfout, const TransformMatrix* pmat, const dReal* pf);
00672 
00674 inline dReal* transnorm3(dReal* pfout, const TransformMatrix* pmat, const dReal* pf);
00675 
00676 // Routines made for 3D graphics that deal with 3 or 4 dim algebra structures
00677 // Functions with postfix 3 are for 3x3 operations, etc
00678 
00679 // all fns return pfout on success or NULL on failure
00680 // results and arguments can share pointers
00681 
00682 
00684 // More complex ops that deal with arbitrary matrices //
00686 
00687 // extract eigen values and vectors from a 2x2 matrix and returns true if all values are real
00688 // returned eigen vectors are normalized
00689 inline bool eig2(const dReal* pfmat, dReal* peigs, dReal& fv1x, dReal& fv1y, dReal& fv2x, dReal& fv2y);
00690 
00691 // Simple routines for linear algebra algorithms //
00692 int CubicRoots (double c0, double c1, double c2, double *r0, double *r1, double *r2);
00693 template <class T, class S> void Tridiagonal3 (S* mat, T* diag, T* subd);
00694 bool QLAlgorithm3 (float* m_aafEntry, float* afDiag, float* afSubDiag);
00695 bool QLAlgorithm3 (double* m_aafEntry, double* afDiag, double* afSubDiag);
00696 
00697 void EigenSymmetric3(dReal* fCovariance, dReal* eval, dReal* fAxes);
00698 
00699 void GetCovarBasisVectors(dReal fCovariance[3][3], Vector* vRight, Vector* vUp, Vector* vDir);
00700 
00704 void svd3(const dReal* A, dReal* U, dReal* D, dReal* V);
00705 
00706 // first root returned is always >= second, roots are defined if the quadratic doesn't have real solutions
00707 void QuadraticSolver(dReal* pfQuadratic, dReal* pfRoots);
00708 
00709 int insideQuadrilateral(const Vector* p0,const Vector* p1, const Vector* p2,const Vector* p3);
00710 int insideTriangle(const Vector* p0, const Vector* p1, const Vector* p2);
00711 
00712 bool RayOBBTest(const RAY& r, const OBB& obb);
00713 dReal DistVertexOBBSq(const Vector& v, const OBB& o);
00714 
00715 template <class T> int Min(T* pts, int stride, int numPts); // returns the index, stride in units of T
00716 template <class T> int Max(T* pts, int stride, int numPts); // returns the index
00717 
00718 // multiplies a matrix by a scalar
00719 template <class T> inline void mult(T* pf, T fa, int r);
00720 
00721 // multiplies a r1xc1 by c1xc2 matrix into pfres, if badd is true adds the result to pfres
00722 // does not handle cases where pfres is equal to pf1 or pf2, use multtox for those cases
00723 template <class T, class R, class S>
00724 inline S* mult(T* pf1, R* pf2, int r1, int c1, int c2, S* pfres, bool badd = false);
00725 
00726 // pf1 is transposed before mult
00727 // rows of pf2 must equal rows of pf1
00728 // pfres will be c1xc2 matrix
00729 template <class T, class R, class S>
00730 inline S* multtrans(T* pf1, R* pf2, int r1, int c1, int c2, S* pfres, bool badd = false);
00731 
00732 // pf2 is transposed before mult
00733 // the columns of both matrices must be the same and equal to c1
00734 // r2 is the number of rows in pf2
00735 // pfres must be an r1xr2 matrix
00736 template <class T, class R, class S>
00737 inline S* multtrans_to2(T* pf1, R* pf2, int r1, int c1, int r2, S* pfres, bool badd = false);
00738 
00739 // multiplies rxc matrix pf1 and cxc matrix pf2 and stores the result in pf1, 
00740 // the function needs a temporary buffer the size of c doubles, if pftemp == NULL,
00741 // the function will allocate the necessary memory, otherwise pftemp should be big
00742 // enough to store all the entries
00743 template <class T> inline T* multto1(T* pf1, T* pf2, int r1, int c1, T* pftemp = NULL);
00744 
00745 // same as multto1 except stores the result in pf2, pf1 has to be an r2xr2 matrix
00746 // pftemp must be of size r2 if not NULL
00747 template <class T, class S> inline T* multto2(T* pf1, S* pf2, int r2, int c2, S* pftemp = NULL);
00748 
00749 // add pf1 + pf2 and store in pf1
00750 template <class T> inline void sub(T* pf1, T* pf2, int r);
00751 template <class T> inline T normsqr(const T* pf1, int r);
00752 template <class T> inline T lengthsqr(const T* pf1, const T* pf2, int length);
00753 template <class T> inline T dot(T* pf1, T* pf2, int length);
00754 
00755 template <class T> inline T sum(T* pf, int length);
00756 
00757 // takes the inverse of the 3x3 matrix pf and stores it into pfres, returns true if matrix is invertible
00758 template <class T> inline bool inv2(T* pf, T* pfres);
00759 
00761 // Function Definitions
00763 bool eig2(const dReal* pfmat, dReal* peigs, dReal& fv1x, dReal& fv1y, dReal& fv2x, dReal& fv2y)
00764 {
00765         // x^2 + bx + c
00766         dReal a, b, c, d;
00767         b = -(pfmat[0] + pfmat[3]);
00768         c = pfmat[0] * pfmat[3] - pfmat[1] * pfmat[2];
00769         d = b * b - 4.0f * c + 1e-16f;
00770 
00771         if( d < 0 ) return false;
00772         if( d < 1e-16f ) {
00773                 a = -0.5f * b;
00774                 peigs[0] = a;   peigs[1] = a;
00775                 fv1x = pfmat[1];                fv1y = a - pfmat[0];
00776                 c = 1 / RaveSqrt(fv1x*fv1x + fv1y*fv1y);
00777                 fv1x *= c;              fv1y *= c;
00778                 fv2x = -fv1y;           fv2y = fv1x;
00779                 return true;
00780         }
00781         
00782         // two roots
00783         d = RaveSqrt(d);
00784         a = -0.5f * (b + d);
00785         peigs[0] = a;
00786         fv1x = pfmat[1];                fv1y = a-pfmat[0];
00787         c = 1 / RaveSqrt(fv1x*fv1x + fv1y*fv1y);
00788         fv1x *= c;              fv1y *= c;
00789 
00790         a += d;
00791         peigs[1] = a;
00792         fv2x = pfmat[1];                fv2y = a-pfmat[0];
00793         c = 1 / RaveSqrt(fv2x*fv2x + fv2y*fv2y);
00794         fv2x *= c;              fv2y *= c;
00795         return true;
00796 }
00797 
00798 // returns the number of real roots, fills r1 and r2 with the answers
00799 template <class T>
00800 inline int solvequad(T a, T b, T c, T& r1, T& r2)
00801 {
00802     T d = b * b - (T)4 * c * a + (T)1e-16;
00803 
00804         if( d < 0 ) return 0;
00805 
00806         if( d < (T)1e-16 ) {
00807                 r1 = r2 = (T)-0.5 * b / a;
00808                 return 1;
00809         }
00810         
00811         // two roots
00812         d = RaveSqrt(d);
00813         r1 = (T)-0.5 * (b + d) / a;
00814     r2 = r1 + d/a;
00815     return 2;
00816 }
00817 
00818 //#ifndef TI_USING_IPP
00819 
00820 #define MULT3(stride) { \
00821         pfres2[0*stride+0] = pf1[0*stride+0]*pf2[0*stride+0]+pf1[0*stride+1]*pf2[1*stride+0]+pf1[0*stride+2]*pf2[2*stride+0]; \
00822         pfres2[0*stride+1] = pf1[0*stride+0]*pf2[0*stride+1]+pf1[0*stride+1]*pf2[1*stride+1]+pf1[0*stride+2]*pf2[2*stride+1]; \
00823         pfres2[0*stride+2] = pf1[0*stride+0]*pf2[0*stride+2]+pf1[0*stride+1]*pf2[1*stride+2]+pf1[0*stride+2]*pf2[2*stride+2]; \
00824         pfres2[1*stride+0] = pf1[1*stride+0]*pf2[0*stride+0]+pf1[1*stride+1]*pf2[1*stride+0]+pf1[1*stride+2]*pf2[2*stride+0]; \
00825         pfres2[1*stride+1] = pf1[1*stride+0]*pf2[0*stride+1]+pf1[1*stride+1]*pf2[1*stride+1]+pf1[1*stride+2]*pf2[2*stride+1]; \
00826         pfres2[1*stride+2] = pf1[1*stride+0]*pf2[0*stride+2]+pf1[1*stride+1]*pf2[1*stride+2]+pf1[1*stride+2]*pf2[2*stride+2]; \
00827         pfres2[2*stride+0] = pf1[2*stride+0]*pf2[0*stride+0]+pf1[2*stride+1]*pf2[1*stride+0]+pf1[2*stride+2]*pf2[2*stride+0]; \
00828         pfres2[2*stride+1] = pf1[2*stride+0]*pf2[0*stride+1]+pf1[2*stride+1]*pf2[1*stride+1]+pf1[2*stride+2]*pf2[2*stride+1]; \
00829         pfres2[2*stride+2] = pf1[2*stride+0]*pf2[0*stride+2]+pf1[2*stride+1]*pf2[1*stride+2]+pf1[2*stride+2]*pf2[2*stride+2]; \
00830 }
00831 
00833 template <class T>
00834 inline T* _mult3_s4(T* pfres, const T* pf1, const T* pf2)
00835 {
00836         assert( pf1 != NULL && pf2 != NULL && pfres != NULL );
00837 
00838         T* pfres2;
00839         if( pfres == pf1 || pfres == pf2 ) pfres2 = (T*)alloca(12 * sizeof(T));
00840         else pfres2 = pfres;
00841 
00842     MULT3(4)
00843 
00844         if( pfres2 != pfres ) memcpy(pfres, pfres2, 12*sizeof(T));
00845 
00846         return pfres;
00847 }
00848 
00850 template <class T>
00851 inline T* _mult3_s3(T* pfres, const T* pf1, const T* pf2)
00852 {
00853         assert( pf1 != NULL && pf2 != NULL && pfres != NULL );
00854 
00855         T* pfres2;
00856         if( pfres == pf1 || pfres == pf2 ) pfres2 = (T*)alloca(9 * sizeof(T));
00857         else pfres2 = pfres;
00858 
00859     MULT3(3)
00860 
00861         if( pfres2 != pfres ) memcpy(pfres, pfres2, 9*sizeof(T));
00862 
00863         return pfres;
00864 }
00865 
00866 // mult4
00867 template <class T> 
00868 inline T* _mult4(T* pfres, const T* p1, const T* p2)
00869 {
00870         assert( pfres != NULL && p1 != NULL && p2 != NULL );
00871 
00872         T* pfres2;
00873         if( pfres == p1 || pfres == p2 ) pfres2 = (T*)alloca(16 * sizeof(T));
00874         else pfres2 = pfres;
00875 
00876         pfres2[0*4+0] = p1[0*4+0]*p2[0*4+0] + p1[0*4+1]*p2[1*4+0] + p1[0*4+2]*p2[2*4+0] + p1[0*4+3]*p2[3*4+0];
00877         pfres2[0*4+1] = p1[0*4+0]*p2[0*4+1] + p1[0*4+1]*p2[1*4+1] + p1[0*4+2]*p2[2*4+1] + p1[0*4+3]*p2[3*4+1];
00878         pfres2[0*4+2] = p1[0*4+0]*p2[0*4+2] + p1[0*4+1]*p2[1*4+2] + p1[0*4+2]*p2[2*4+2] + p1[0*4+3]*p2[3*4+2];
00879         pfres2[0*4+3] = p1[0*4+0]*p2[0*4+3] + p1[0*4+1]*p2[1*4+3] + p1[0*4+2]*p2[2*4+3] + p1[0*4+3]*p2[3*4+3];
00880 
00881         pfres2[1*4+0] = p1[1*4+0]*p2[0*4+0] + p1[1*4+1]*p2[1*4+0] + p1[1*4+2]*p2[2*4+0] + p1[1*4+3]*p2[3*4+0];
00882         pfres2[1*4+1] = p1[1*4+0]*p2[0*4+1] + p1[1*4+1]*p2[1*4+1] + p1[1*4+2]*p2[2*4+1] + p1[1*4+3]*p2[3*4+1];
00883         pfres2[1*4+2] = p1[1*4+0]*p2[0*4+2] + p1[1*4+1]*p2[1*4+2] + p1[1*4+2]*p2[2*4+2] + p1[1*4+3]*p2[3*4+2];
00884         pfres2[1*4+3] = p1[1*4+0]*p2[0*4+3] + p1[1*4+1]*p2[1*4+3] + p1[1*4+2]*p2[2*4+3] + p1[1*4+3]*p2[3*4+3];
00885 
00886         pfres2[2*4+0] = p1[2*4+0]*p2[0*4+0] + p1[2*4+1]*p2[1*4+0] + p1[2*4+2]*p2[2*4+0] + p1[2*4+3]*p2[3*4+0];
00887         pfres2[2*4+1] = p1[2*4+0]*p2[0*4+1] + p1[2*4+1]*p2[1*4+1] + p1[2*4+2]*p2[2*4+1] + p1[2*4+3]*p2[3*4+1];
00888         pfres2[2*4+2] = p1[2*4+0]*p2[0*4+2] + p1[2*4+1]*p2[1*4+2] + p1[2*4+2]*p2[2*4+2] + p1[2*4+3]*p2[3*4+2];
00889         pfres2[2*4+3] = p1[2*4+0]*p2[0*4+3] + p1[2*4+1]*p2[1*4+3] + p1[2*4+2]*p2[2*4+3] + p1[2*4+3]*p2[3*4+3];
00890 
00891         pfres2[3*4+0] = p1[3*4+0]*p2[0*4+0] + p1[3*4+1]*p2[1*4+0] + p1[3*4+2]*p2[2*4+0] + p1[3*4+3]*p2[3*4+0];
00892         pfres2[3*4+1] = p1[3*4+0]*p2[0*4+1] + p1[3*4+1]*p2[1*4+1] + p1[3*4+2]*p2[2*4+1] + p1[3*4+3]*p2[3*4+1];
00893         pfres2[3*4+2] = p1[3*4+0]*p2[0*4+2] + p1[3*4+1]*p2[1*4+2] + p1[3*4+2]*p2[2*4+2] + p1[3*4+3]*p2[3*4+2];
00894         pfres2[3*4+3] = p1[3*4+0]*p2[0*4+3] + p1[3*4+1]*p2[1*4+3] + p1[3*4+2]*p2[2*4+3] + p1[3*4+3]*p2[3*4+3];
00895 
00896         if( pfres != pfres2 ) memcpy(pfres, pfres2, sizeof(T)*16);
00897         return pfres;
00898 }
00899 
00900 template <class T> 
00901 inline T* _multtrans3(T* pfres, const T* pf1, const T* pf2)
00902 {
00903         T* pfres2;
00904         if( pfres == pf1 ) pfres2 = (T*)alloca(9 * sizeof(T));
00905         else pfres2 = pfres;
00906 
00907         pfres2[0] = pf1[0]*pf2[0]+pf1[3]*pf2[3]+pf1[6]*pf2[6];
00908         pfres2[1] = pf1[0]*pf2[1]+pf1[3]*pf2[4]+pf1[6]*pf2[7];
00909         pfres2[2] = pf1[0]*pf2[2]+pf1[3]*pf2[5]+pf1[6]*pf2[8];
00910 
00911         pfres2[3] = pf1[1]*pf2[0]+pf1[4]*pf2[3]+pf1[7]*pf2[6];
00912         pfres2[4] = pf1[1]*pf2[1]+pf1[4]*pf2[4]+pf1[7]*pf2[7];
00913         pfres2[5] = pf1[1]*pf2[2]+pf1[4]*pf2[5]+pf1[7]*pf2[8];
00914 
00915         pfres2[6] = pf1[2]*pf2[0]+pf1[5]*pf2[3]+pf1[8]*pf2[6];
00916         pfres2[7] = pf1[2]*pf2[1]+pf1[5]*pf2[4]+pf1[8]*pf2[7];
00917         pfres2[8] = pf1[2]*pf2[2]+pf1[5]*pf2[5]+pf1[8]*pf2[8];
00918 
00919         if( pfres2 != pfres ) memcpy(pfres, pfres2, 9*sizeof(T));
00920 
00921         return pfres;
00922 }
00923 
00924 template <class T> 
00925 inline T* _multtrans4(T* pfres, const T* pf1, const T* pf2)
00926 {
00927         T* pfres2;
00928         if( pfres == pf1 ) pfres2 = (T*)alloca(16 * sizeof(T));
00929         else pfres2 = pfres;
00930 
00931         for(int i = 0; i < 4; ++i) {
00932                 for(int j = 0; j < 4; ++j) {
00933                         pfres[4*i+j] = pf1[i] * pf2[j] + pf1[i+4] * pf2[j+4] + pf1[i+8] * pf2[j+8] + pf1[i+12] * pf2[j+12];
00934                 }
00935         }
00936 
00937         return pfres;
00938 }
00939 
00940 //generate a random quaternion
00941 inline Vector GetRandomQuat(void)
00942 {
00943     Vector q;
00944 
00945     while(1) {
00946         q.x = -1 + 2*(rand()/((dReal)RAND_MAX));
00947         q.y = -1 + 2*(rand()/((dReal)RAND_MAX));
00948         q.z = -1 + 2*(rand()/((dReal)RAND_MAX));
00949         q.w = -1 + 2*(rand()/((dReal)RAND_MAX));
00950 
00951         dReal norm = q.lengthsqr4();
00952         if(norm <= 1) {
00953             q = q * (1 / RaveSqrt(norm));
00954             break;
00955         }
00956     }
00957     
00958     return q;
00959 }
00960 
00961 template <class T>
00962 inline RaveVector<T> AxisAngle2Quat(const RaveVector<T>& rotaxis, T angle)
00963 {
00964     angle *= (T)0.5;
00965     T fsin = RaveSin(angle);
00966     return RaveVector<T>(RaveCos(angle), rotaxis.x*fsin, rotaxis.y * fsin, rotaxis.z * fsin);
00967 }
00968 
00969 template <class T>
00970 inline RaveVector<T> dQSlerp(const RaveVector<T>& qa, const RaveVector<T>& qb, T t)
00971 {
00972         // quaternion to return
00973         RaveVector<T> qm;
00974 
00975         // Calculate angle between them.
00976         T cosHalfTheta = qa.w * qb.w + qa.x * qb.x + qa.y * qb.y + qa.z * qb.z;
00977         // if qa=qb or qa=-qb then theta = 0 and we can return qa
00978         if (RaveFabs(cosHalfTheta) >= 1.0){
00979                 qm.w = qa.w;qm.x = qa.x;qm.y = qa.y;qm.z = qa.z;
00980                 return qm;
00981         }
00982         // Calculate temporary values.
00983         T halfTheta = RaveAcos(cosHalfTheta);
00984         T sinHalfTheta = RaveSqrt(1 - cosHalfTheta*cosHalfTheta);
00985         // if theta = 180 degrees then result is not fully defined
00986         // we could rotate around any axis normal to qa or qb
00987         if (RaveFabs(sinHalfTheta) < 0.0001f){ // fabs is floating point absolute
00988                 qm.w = (qa.w * 0.5f + qb.w * 0.5f);
00989                 qm.x = (qa.x * 0.5f + qb.x * 0.5f);
00990                 qm.y = (qa.y * 0.5f + qb.y * 0.5f);
00991                 qm.z = (qa.z * 0.5f + qb.z * 0.5f);
00992                 return qm;
00993         }
00994 
00995         T ratioA = RaveSin((1 - t) * halfTheta) / sinHalfTheta;
00996         T ratioB = RaveSin(t * halfTheta) / sinHalfTheta; 
00997         //calculate Quaternion.
00998         qm.w = (qa.w * ratioA + qb.w * ratioB);
00999         qm.x = (qa.x * ratioA + qb.x * ratioB);
01000         qm.y = (qa.y * ratioA + qb.y * ratioB);
01001         qm.z = (qa.z * ratioA + qb.z * ratioB);
01002         return qm;
01003 }
01004 
01006 template <class T> inline T matrixdet3(const T* pf, int stride)
01007 {
01008     return pf[0*stride+2] * (pf[1*stride + 0] * pf[2*stride + 1] - pf[1*stride + 1] * pf[2*stride + 0]) +
01009         pf[1*stride+2] * (pf[0*stride + 1] * pf[2*stride + 0] - pf[0*stride + 0] * pf[2*stride + 1]) +
01010         pf[2*stride+2] * (pf[0*stride + 0] * pf[1*stride + 1] - pf[0*stride + 1] * pf[1*stride + 0]);
01011 }
01012 
01015 template <class T>
01016 inline T* _inv3(const T* pf, T* pfres, T* pfdet, int stride)
01017 {
01018         T* pfres2;
01019         if( pfres == pf ) pfres2 = (T*)alloca(3 * stride * sizeof(T));
01020         else pfres2 = pfres;
01021 
01022         // inverse = C^t / det(pf) where C is the matrix of coefficients
01023 
01024         // calc C^t
01025         pfres2[0*stride + 0] = pf[1*stride + 1] * pf[2*stride + 2] - pf[1*stride + 2] * pf[2*stride + 1];
01026         pfres2[0*stride + 1] = pf[0*stride + 2] * pf[2*stride + 1] - pf[0*stride + 1] * pf[2*stride + 2];
01027         pfres2[0*stride + 2] = pf[0*stride + 1] * pf[1*stride + 2] - pf[0*stride + 2] * pf[1*stride + 1];
01028         pfres2[1*stride + 0] = pf[1*stride + 2] * pf[2*stride + 0] - pf[1*stride + 0] * pf[2*stride + 2];
01029         pfres2[1*stride + 1] = pf[0*stride + 0] * pf[2*stride + 2] - pf[0*stride + 2] * pf[2*stride + 0];
01030         pfres2[1*stride + 2] = pf[0*stride + 2] * pf[1*stride + 0] - pf[0*stride + 0] * pf[1*stride + 2];
01031         pfres2[2*stride + 0] = pf[1*stride + 0] * pf[2*stride + 1] - pf[1*stride + 1] * pf[2*stride + 0];
01032         pfres2[2*stride + 1] = pf[0*stride + 1] * pf[2*stride + 0] - pf[0*stride + 0] * pf[2*stride + 1];
01033         pfres2[2*stride + 2] = pf[0*stride + 0] * pf[1*stride + 1] - pf[0*stride + 1] * pf[1*stride + 0];
01034 
01035         T fdet = pf[0*stride + 2] * pfres2[2*stride + 0] + pf[1*stride + 2] * pfres2[2*stride + 1] +
01036                 pf[2*stride + 2] * pfres2[2*stride + 2];
01037         
01038         if( pfdet != NULL )
01039                 pfdet[0] = fdet;
01040 
01041     if( fabs(fdet) < 1e-12 ) {
01042         return NULL;
01043     }
01044 
01045         fdet = 1 / fdet;
01046         //if( pfdet != NULL ) *pfdet = fdet;
01047 
01048         if( pfres != pf ) {
01049                 pfres[0*stride+0] *= fdet;              pfres[0*stride+1] *= fdet;              pfres[0*stride+2] *= fdet;
01050                 pfres[1*stride+0] *= fdet;              pfres[1*stride+1] *= fdet;              pfres[1*stride+2] *= fdet;
01051                 pfres[2*stride+0] *= fdet;              pfres[2*stride+1] *= fdet;              pfres[2*stride+2] *= fdet;
01052                 return pfres;
01053         }
01054 
01055         pfres[0*stride+0] = pfres2[0*stride+0] * fdet;
01056         pfres[0*stride+1] = pfres2[0*stride+1] * fdet;
01057         pfres[0*stride+2] = pfres2[0*stride+2] * fdet;
01058         pfres[1*stride+0] = pfres2[1*stride+0] * fdet;
01059         pfres[1*stride+1] = pfres2[1*stride+1] * fdet;
01060         pfres[1*stride+2] = pfres2[1*stride+2] * fdet;
01061         pfres[2*stride+0] = pfres2[2*stride+0] * fdet;
01062         pfres[2*stride+1] = pfres2[2*stride+1] * fdet;
01063         pfres[2*stride+2] = pfres2[2*stride+2] * fdet;
01064         return pfres;
01065 }
01066 
01067 // inverse if 92 mults and 39 adds
01068 template <class T>
01069 inline T* _inv4(const T* pf, T* pfres)
01070 {
01071         T* pfres2;
01072         if( pfres == pf ) pfres2 = (T*)alloca(16 * sizeof(T));
01073         else pfres2 = pfres;
01074 
01075         // inverse = C^t / det(pf) where C is the matrix of coefficients
01076 
01077         // calc C^t
01078 
01079         // determinants of all possibel 2x2 submatrices formed by last two rows
01080         T fd0, fd1, fd2;
01081         T f1, f2, f3;
01082         fd0 = pf[2*4 + 0] * pf[3*4 + 1] - pf[2*4 + 1] * pf[3*4 + 0];
01083         fd1 = pf[2*4 + 1] * pf[3*4 + 2] - pf[2*4 + 2] * pf[3*4 + 1];
01084         fd2 = pf[2*4 + 2] * pf[3*4 + 3] - pf[2*4 + 3] * pf[3*4 + 2];
01085 
01086         f1 = pf[2*4 + 1] * pf[3*4 + 3] - pf[2*4 + 3] * pf[3*4 + 1];
01087         f2 = pf[2*4 + 0] * pf[3*4 + 3] - pf[2*4 + 3] * pf[3*4 + 0];
01088         f3 = pf[2*4 + 0] * pf[3*4 + 2] - pf[2*4 + 2] * pf[3*4 + 0];
01089 
01090         pfres2[0*4 + 0] =   pf[1*4 + 1] * fd2 - pf[1*4 + 2] * f1 + pf[1*4 + 3] * fd1;
01091         pfres2[0*4 + 1] = -(pf[0*4 + 1] * fd2 - pf[0*4 + 2] * f1 + pf[0*4 + 3] * fd1);
01092 
01093         pfres2[1*4 + 0] = -(pf[1*4 + 0] * fd2 - pf[1*4 + 2] * f2 + pf[1*4 + 3] * f3);
01094         pfres2[1*4 + 1] =   pf[0*4 + 0] * fd2 - pf[0*4 + 2] * f2 + pf[0*4 + 3] * f3;
01095 
01096         pfres2[2*4 + 0] =   pf[1*4 + 0] * f1 -  pf[1*4 + 1] * f2 + pf[1*4 + 3] * fd0;
01097         pfres2[2*4 + 1] = -(pf[0*4 + 0] * f1 -  pf[0*4 + 1] * f2 + pf[0*4 + 3] * fd0);
01098 
01099         pfres2[3*4 + 0] = -(pf[1*4 + 0] * fd1 - pf[1*4 + 1] * f3 + pf[1*4 + 2] * fd0);
01100         pfres2[3*4 + 1] =   pf[0*4 + 0] * fd1 - pf[0*4 + 1] * f3 + pf[0*4 + 2] * fd0;
01101 
01102         // determinants of first 2 rows of 4x4 matrix
01103         fd0 = pf[0*4 + 0] * pf[1*4 + 1] - pf[0*4 + 1] * pf[1*4 + 0];
01104         fd1 = pf[0*4 + 1] * pf[1*4 + 2] - pf[0*4 + 2] * pf[1*4 + 1];
01105         fd2 = pf[0*4 + 2] * pf[1*4 + 3] - pf[0*4 + 3] * pf[1*4 + 2];
01106 
01107         f1 = pf[0*4 + 1] * pf[1*4 + 3] - pf[0*4 + 3] * pf[1*4 + 1];
01108         f2 = pf[0*4 + 0] * pf[1*4 + 3] - pf[0*4 + 3] * pf[1*4 + 0];
01109         f3 = pf[0*4 + 0] * pf[1*4 + 2] - pf[0*4 + 2] * pf[1*4 + 0];
01110 
01111         pfres2[0*4 + 2] =   pf[3*4 + 1] * fd2 - pf[3*4 + 2] * f1 + pf[3*4 + 3] * fd1;
01112         pfres2[0*4 + 3] = -(pf[2*4 + 1] * fd2 - pf[2*4 + 2] * f1 + pf[2*4 + 3] * fd1);
01113 
01114         pfres2[1*4 + 2] = -(pf[3*4 + 0] * fd2 - pf[3*4 + 2] * f2 + pf[3*4 + 3] * f3);
01115         pfres2[1*4 + 3] =   pf[2*4 + 0] * fd2 - pf[2*4 + 2] * f2 + pf[2*4 + 3] * f3;
01116 
01117         pfres2[2*4 + 2] =   pf[3*4 + 0] * f1 -  pf[3*4 + 1] * f2 + pf[3*4 + 3] * fd0;
01118         pfres2[2*4 + 3] = -(pf[2*4 + 0] * f1 -  pf[2*4 + 1] * f2 + pf[2*4 + 3] * fd0);
01119 
01120         pfres2[3*4 + 2] = -(pf[3*4 + 0] * fd1 - pf[3*4 + 1] * f3 + pf[3*4 + 2] * fd0);
01121         pfres2[3*4 + 3] =   pf[2*4 + 0] * fd1 - pf[2*4 + 1] * f3 + pf[2*4 + 2] * fd0;
01122 
01123         T fdet = pf[0*4 + 3] * pfres2[3*4 + 0] + pf[1*4 + 3] * pfres2[3*4 + 1] +
01124                         pf[2*4 + 3] * pfres2[3*4 + 2] + pf[3*4 + 3] * pfres2[3*4 + 3];
01125 
01126         if( fabs(fdet) < 1e-9) return NULL;
01127 
01128         fdet = 1 / fdet;
01129         //if( pfdet != NULL ) *pfdet = fdet;
01130 
01131         if( pfres2 == pfres ) {
01132                 mult(pfres, fdet, 16);
01133                 return pfres;
01134         }
01135 
01136         int i = 0;
01137         while(i < 16) {
01138                 pfres[i] = pfres2[i] * fdet;
01139                 ++i;
01140         }
01141 
01142         return pfres;
01143 }
01144 
01145 template <class T>
01146 inline T* _transpose3(const T* pf, T* pfres)
01147 {
01148         assert( pf != NULL && pfres != NULL );
01149 
01150         if( pf == pfres ) {
01151                 rswap(pfres[1], pfres[3]);
01152                 rswap(pfres[2], pfres[6]);
01153                 rswap(pfres[5], pfres[7]);
01154                 return pfres;
01155         }
01156 
01157         pfres[0] = pf[0];       pfres[1] = pf[3];       pfres[2] = pf[6];
01158         pfres[3] = pf[1];       pfres[4] = pf[4];       pfres[5] = pf[7];
01159         pfres[6] = pf[2];       pfres[7] = pf[5];       pfres[8] = pf[8];
01160 
01161         return pfres;
01162 }
01163 
01164 template <class T>
01165 inline T* _transpose4(const T* pf, T* pfres)
01166 {
01167         assert( pf != NULL && pfres != NULL );
01168 
01169         if( pf == pfres ) {
01170                 rswap(pfres[1], pfres[4]);
01171                 rswap(pfres[2], pfres[8]);
01172                 rswap(pfres[3], pfres[12]);
01173                 rswap(pfres[6], pfres[9]);
01174                 rswap(pfres[7], pfres[13]);
01175                 rswap(pfres[11], pfres[15]);
01176                 return pfres;
01177         }
01178 
01179         pfres[0] = pf[0];       pfres[1] = pf[4];       pfres[2] = pf[8];               pfres[3] = pf[12];
01180         pfres[4] = pf[1];       pfres[5] = pf[5];       pfres[6] = pf[9];               pfres[7] = pf[13];
01181         pfres[8] = pf[2];       pfres[9] = pf[6];       pfres[10] = pf[10];             pfres[11] = pf[14];
01182         pfres[12] = pf[3];      pfres[13] = pf[7];      pfres[14] = pf[11];             pfres[15] = pf[15];
01183         return pfres;
01184 }
01185 
01186 template <class T>
01187 inline T _dot2(const T* pf1, const T* pf2)
01188 {
01189         assert( pf1 != NULL && pf2 != NULL );
01190         return pf1[0]*pf2[0] + pf1[1]*pf2[1];
01191 }
01192 
01193 template <class T>
01194 inline T _dot3(const T* pf1, const T* pf2)
01195 {
01196         assert( pf1 != NULL && pf2 != NULL );
01197         return pf1[0]*pf2[0] + pf1[1]*pf2[1] + pf1[2]*pf2[2];
01198 }
01199 
01200 template <class T>
01201 inline T _dot4(const T* pf1, const T* pf2)
01202 {
01203         assert( pf1 != NULL && pf2 != NULL );
01204         return pf1[0]*pf2[0] + pf1[1]*pf2[1] + pf1[2]*pf2[2] + pf1[3] * pf2[3];
01205 }
01206 
01207 template <class T>
01208 inline T _lengthsqr2(const T* pf)
01209 {
01210         assert( pf != NULL );
01211         return pf[0] * pf[0] + pf[1] * pf[1];
01212 }
01213 
01214 template <class T>
01215 inline T _lengthsqr3(const T* pf)
01216 {
01217         assert( pf != NULL );
01218         return pf[0] * pf[0] + pf[1] * pf[1] + pf[2] * pf[2];
01219 }
01220 
01221 template <class T>
01222 inline T _lengthsqr4(const T* pf)
01223 {
01224         assert( pf != NULL );
01225         return pf[0] * pf[0] + pf[1] * pf[1] + pf[2] * pf[2] + pf[3] * pf[3];
01226 }
01227 
01228 template <class T>
01229 inline T* _normalize2(T* pfout, const T* pf)
01230 {
01231         assert(pf != NULL);
01232 
01233         T f = pf[0]*pf[0] + pf[1]*pf[1];
01234         f = 1 / RaveSqrt(f);
01235         pfout[0] = pf[0] * f;
01236         pfout[1] = pf[1] * f;
01237 
01238         return pfout;
01239 }
01240 
01241 template <class T>
01242 inline T* _normalize3(T* pfout, const T* pf)
01243 {
01244         assert(pf != NULL);
01245 
01246         T f = pf[0]*pf[0] + pf[1]*pf[1] + pf[2]*pf[2];
01247 
01248         f = 1 / RaveSqrt(f);
01249         pfout[0] = pf[0] * f;
01250         pfout[1] = pf[1] * f;
01251         pfout[2] = pf[2] * f;
01252 
01253         return pfout;
01254 }
01255 
01256 template <class T>
01257 inline T* _normalize4(T* pfout, const T* pf)
01258 {
01259         assert(pf != NULL);
01260 
01261         T f = pf[0]*pf[0] + pf[1]*pf[1] + pf[2]*pf[2] + pf[3]*pf[3];
01262 
01263         f = 1 / RaveSqrt(f);
01264         pfout[0] = pf[0] * f;
01265         pfout[1] = pf[1] * f;
01266         pfout[2] = pf[2] * f;
01267         pfout[3] = pf[3] * f;
01268 
01269         return pfout;
01270 }
01271 
01272 template <class T>
01273 inline T* _cross3(T* pfout, const T* pf1, const T* pf2)
01274 {
01275         assert( pfout != NULL && pf1 != NULL && pf2 != NULL );
01276 
01277     T temp[3];
01278         temp[0] = pf1[1] * pf2[2] - pf1[2] * pf2[1];
01279         temp[1] = pf1[2] * pf2[0] - pf1[0] * pf2[2];
01280         temp[2] = pf1[0] * pf2[1] - pf1[1] * pf2[0];
01281 
01282         pfout[0] = temp[0]; pfout[1] = temp[1]; pfout[2] = temp[2];
01283     return pfout;
01284 }
01285 
01286 template <class T>
01287 inline T* transcoord3(T* pfout, const RaveTransformMatrix<T>* pmat, const T* pf)
01288 {
01289         assert( pfout != NULL && pf != NULL && pmat != NULL );
01290     
01291     T dummy[3];
01292         T* pfreal = (pfout == pf) ? dummy : pfout;
01293 
01294         pfreal[0] = pf[0] * pmat->m[0] + pf[1] * pmat->m[1] + pf[2] * pmat->m[2] + pmat->trans.x;
01295     pfreal[1] = pf[0] * pmat->m[4] + pf[1] * pmat->m[5] + pf[2] * pmat->m[6] + pmat->trans.y;
01296     pfreal[2] = pf[0] * pmat->m[8] + pf[1] * pmat->m[9] + pf[2] * pmat->m[10] + pmat->trans.z;
01297 
01298     if( pfout ==pf ) {
01299         pfout[0] = pfreal[0];
01300         pfout[1] = pfreal[1];
01301         pfout[2] = pfreal[2];
01302     }
01303 
01304         return pfout;
01305 }
01306 
01307 template <class T>
01308 inline T* _transnorm3(T* pfout, const T* pfmat, const T* pf)
01309 {
01310         assert( pfout != NULL && pf != NULL && pfmat != NULL );
01311 
01312     T dummy[3];
01313         T* pfreal = (pfout == pf) ? dummy : pfout;
01314 
01315         pfreal[0] = pf[0] * pfmat[0] + pf[1] * pfmat[1] + pf[2] * pfmat[2];
01316     pfreal[1] = pf[0] * pfmat[3] + pf[1] * pfmat[4] + pf[2] * pfmat[5];
01317     pfreal[2] = pf[0] * pfmat[6] + pf[1] * pfmat[7] + pf[2] * pfmat[8];
01318 
01319     if( pfout ==pf ) {
01320         pfout[0] = pfreal[0];
01321         pfout[1] = pfreal[1];
01322         pfout[2] = pfreal[2];
01323     }
01324 
01325         return pfout;
01326 }
01327 
01328 template <class T>
01329 inline T* transnorm3(T* pfout, const RaveTransformMatrix<T>* pmat, const T* pf)
01330 {
01331         assert( pfout != NULL && pf != NULL && pmat != NULL );
01332 
01333         T dummy[3];
01334     T* pfreal = (pfout == pf) ? dummy : pfout;
01335 
01336         pfreal[0] = pf[0] * pmat->m[0] + pf[1] * pmat->m[1] + pf[2] * pmat->m[2];
01337     pfreal[1] = pf[0] * pmat->m[4] + pf[1] * pmat->m[5] + pf[2] * pmat->m[6];
01338     pfreal[2] = pf[0] * pmat->m[8] + pf[1] * pmat->m[9] + pf[2] * pmat->m[10];
01339 
01340         if( pfreal != pfout ) {
01341         pfout[0] = pfreal[0];
01342         pfout[1] = pfreal[1];
01343         pfout[2] = pfreal[2];
01344     }
01345 
01346         return pfout;
01347 }
01348 
01349 inline float* mult4(float* pfres, const float* pf1, const float* pf2) { return _mult4<float>(pfres, pf1, pf2); }
01350 // pf1^T * pf2
01351 inline float* multtrans3(float* pfres, const float* pf1, const float* pf2) { return _multtrans3<float>(pfres, pf1, pf2); }
01352 inline float* multtrans4(float* pfres, const float* pf1, const float* pf2) { return _multtrans4<float>(pfres, pf1, pf2); }
01353 inline float* transnorm3(float* pfout, const float* pfmat, const float* pf) { return _transnorm3<float>(pfout, pfmat, pf); }
01354 
01355 inline float* transpose3(const float* pf, float* pfres) { return _transpose3<float>(pf, pfres); }
01356 inline float* transpose4(const float* pf, float* pfres) { return _transpose4<float>(pf, pfres); }
01357 
01358 inline float dot2(const float* pf1, const float* pf2) { return _dot2<float>(pf1, pf2); }
01359 inline float dot3(const float* pf1, const float* pf2) { return _dot3<float>(pf1, pf2); }
01360 inline float dot4(const float* pf1, const float* pf2) { return _dot4<float>(pf1, pf2); }
01361 
01362 inline float lengthsqr2(const float* pf) { return _lengthsqr2<float>(pf); }
01363 inline float lengthsqr3(const float* pf) { return _lengthsqr3<float>(pf); }
01364 inline float lengthsqr4(const float* pf) { return _lengthsqr4<float>(pf); }
01365 
01366 inline float* normalize2(float* pfout, const float* pf) { return _normalize2<float>(pfout, pf); }
01367 inline float* normalize3(float* pfout, const float* pf) { return _normalize3<float>(pfout, pf); }
01368 inline float* normalize4(float* pfout, const float* pf) { return _normalize4<float>(pfout, pf); }
01369 
01370 inline float* cross3(float* pfout, const float* pf1, const float* pf2) { return _cross3<float>(pfout, pf1, pf2); }
01371 
01372 // multiplies 3x3 matrices
01373 inline float* mult3_s4(float* pfres, const float* pf1, const float* pf2) { return _mult3_s4<float>(pfres, pf1, pf2); }
01374 inline float* mult3_s3(float* pfres, const float* pf1, const float* pf2) { return _mult3_s3<float>(pfres, pf1, pf2); }
01375 
01376 inline float* inv3(const float* pf, float* pfres, float* pfdet, int stride) { return _inv3<float>(pf, pfres, pfdet, stride); }
01377 inline float* inv4(const float* pf, float* pfres) { return _inv4<float>(pf, pfres); }
01378 
01379 
01380 inline double* mult4(double* pfres, const double* pf1, const double* pf2) { return _mult4<double>(pfres, pf1, pf2); }
01381 // pf1^T * pf2
01382 inline double* multtrans3(double* pfres, const double* pf1, const double* pf2) { return _multtrans3<double>(pfres, pf1, pf2); }
01383 inline double* multtrans4(double* pfres, const double* pf1, const double* pf2) { return _multtrans4<double>(pfres, pf1, pf2); }
01384 inline double* transnorm3(double* pfout, const double* pfmat, const double* pf) { return _transnorm3<double>(pfout, pfmat, pf); }
01385 
01386 inline double* transpose3(const double* pf, double* pfres) { return _transpose3<double>(pf, pfres); }
01387 inline double* transpose4(const double* pf, double* pfres) { return _transpose4<double>(pf, pfres); }
01388 
01389 inline double dot2(const double* pf1, const double* pf2) { return _dot2<double>(pf1, pf2); }
01390 inline double dot3(const double* pf1, const double* pf2) { return _dot3<double>(pf1, pf2); }
01391 inline double dot4(const double* pf1, const double* pf2) { return _dot4<double>(pf1, pf2); }
01392 
01393 inline double lengthsqr2(const double* pf) { return _lengthsqr2<double>(pf); }
01394 inline double lengthsqr3(const double* pf) { return _lengthsqr3<double>(pf); }
01395 inline double lengthsqr4(const double* pf) { return _lengthsqr4<double>(pf); }
01396 
01397 inline double* normalize2(double* pfout, const double* pf) { return _normalize2<double>(pfout, pf); }
01398 inline double* normalize3(double* pfout, const double* pf) { return _normalize3<double>(pfout, pf); }
01399 inline double* normalize4(double* pfout, const double* pf) { return _normalize4<double>(pfout, pf); }
01400 
01401 inline double* cross3(double* pfout, const double* pf1, const double* pf2) { return _cross3<double>(pfout, pf1, pf2); }
01402 
01403 // multiplies 3x3 matrices
01404 inline double* mult3_s4(double* pfres, const double* pf1, const double* pf2) { return _mult3_s4<double>(pfres, pf1, pf2); }
01405 inline double* mult3_s3(double* pfres, const double* pf1, const double* pf2) { return _mult3_s3<double>(pfres, pf1, pf2); }
01406 
01407 inline double* inv3(const double* pf, double* pfres, double* pfdet, int stride) { return _inv3<double>(pf, pfres, pfdet, stride); }
01408 inline double* inv4(const double* pf, double* pfres) { return _inv4<double>(pf, pfres); }
01409 
01410 // if the two triangles collide, returns true and fills contactpos with the intersection point
01411 // assuming triangles are declared counter-clockwise!!
01412 // contactnorm is the normal of the second triangle
01413 bool TriTriCollision(const RaveVector<float>& u1, const RaveVector<float>& u2, const RaveVector<float>& u3,
01414                    const RaveVector<float>& v1, const RaveVector<float>& v2, const RaveVector<float>& v3,
01415                      RaveVector<float>& contactpos, RaveVector<float>& contactnorm);
01416 bool TriTriCollision(const RaveVector<double>& u1, const RaveVector<double>& u2, const RaveVector<double>& u3,
01417                    const RaveVector<double>& v1, const RaveVector<double>& v2, const RaveVector<double>& v3,
01418                      RaveVector<double>& contactpos, RaveVector<double>& contactnorm);
01419 
01420 template <class T> inline void mult(T* pf, T fa, int r)
01421 {
01422         assert( pf != NULL );
01423 
01424         while(r > 0) {
01425                 --r;
01426                 pf[r] *= fa;
01427         }
01428 }
01429 
01430 template <class T, class R, class S>
01431 inline S* mult(T* pf1, R* pf2, int r1, int c1, int c2, S* pfres, bool badd)
01432 {
01433         assert( pf1 != NULL && pf2 != NULL && pfres != NULL);
01434         int j, k;
01435 
01436         if( !badd ) memset(pfres, 0, sizeof(S) * r1 * c2);
01437 
01438         while(r1 > 0) {
01439                 --r1;
01440 
01441                 j = 0;
01442                 while(j < c2) {
01443                         k = 0;
01444                         while(k < c1) {
01445                                 pfres[j] += (S)(pf1[k] * pf2[k*c2 + j]);
01446                                 ++k;
01447                         }
01448                         ++j;
01449                 }
01450 
01451                 pf1 += c1;
01452                 pfres += c2;
01453         }
01454 
01455         return pfres;
01456 }
01457 
01458 template <class T, class R, class S>
01459 inline S* multtrans(T* pf1, R* pf2, int r1, int c1, int c2, S* pfres, bool badd)
01460 {
01461         assert( pf1 != NULL && pf2 != NULL && pfres != NULL);
01462         int i, j, k;
01463 
01464         if( !badd ) memset(pfres, 0, sizeof(S) * c1 * c2);
01465 
01466         i = 0;
01467         while(i < c1) {
01468 
01469                 j = 0;
01470                 while(j < c2) {
01471 
01472                         k = 0;
01473                         while(k < r1) {
01474                                 pfres[j] += (S)(pf1[k*c1] * pf2[k*c2 + j]);
01475                                 ++k;
01476                         }
01477                         ++j;
01478                 }
01479 
01480                 pfres += c2;
01481                 ++pf1;
01482 
01483                 ++i;
01484         }
01485 
01486         return pfres;
01487 }
01488 
01489 template <class T, class R, class S>
01490 inline S* multtrans_to2(T* pf1, R* pf2, int r1, int c1, int r2, S* pfres, bool badd)
01491 {
01492         assert( pf1 != NULL && pf2 != NULL && pfres != NULL);
01493         int j, k;
01494 
01495         if( !badd ) memset(pfres, 0, sizeof(S) * r1 * r2);
01496 
01497         while(r1 > 0) {
01498                 --r1;
01499 
01500                 j = 0;
01501                 while(j < r2) {
01502                         k = 0;
01503                         while(k < c1) {
01504                                 pfres[j] += (S)(pf1[k] * pf2[j*c1 + k]);
01505                                 ++k;
01506                         }
01507                         ++j;
01508                 }
01509 
01510                 pf1 += c1;
01511         pfres += r2;
01512         }
01513 
01514         return pfres;
01515 }
01516 
01517 template <class T> inline T* multto1(T* pf1, T* pf2, int r, int c, T* pftemp)
01518 {
01519         assert( pf1 != NULL && pf2 != NULL );
01520 
01521         int j, k;
01522         bool bdel = false;
01523         
01524         if( pftemp == NULL ) {
01525                 pftemp = new T[c];
01526                 bdel = true;
01527         }
01528 
01529         while(r > 0) {
01530                 --r;
01531 
01532                 j = 0;
01533                 while(j < c) {
01534                         
01535                         pftemp[j] = 0.0;
01536 
01537                         k = 0;
01538                         while(k < c) {
01539                                 pftemp[j] += pf1[k] * pf2[k*c + j];
01540                                 ++k;
01541                         }
01542                         ++j;
01543                 }
01544 
01545                 memcpy(pf1, pftemp, c * sizeof(T));
01546                 pf1 += c;
01547         }
01548 
01549         if( bdel ) delete[] pftemp;
01550 
01551         return pf1;
01552 }
01553 
01554 template <class T, class S> inline T* multto2(T* pf1, S* pf2, int r2, int c2, S* pftemp)
01555 {
01556         assert( pf1 != NULL && pf2 != NULL );
01557 
01558         int i, j, k;
01559         bool bdel = false;
01560         
01561         if( pftemp == NULL ) {
01562                 pftemp = new S[r2];
01563                 bdel = true;
01564         }
01565 
01566         // do columns first
01567         j = 0;
01568         while(j < c2) {
01569                 i = 0;
01570                 while(i < r2) {
01571                         
01572                         pftemp[i] = 0.0;
01573 
01574                         k = 0;
01575                         while(k < r2) {
01576                                 pftemp[i] += pf1[i*r2 + k] * pf2[k*c2 + j];
01577                                 ++k;
01578                         }
01579                         ++i;
01580                 }
01581 
01582                 i = 0;
01583                 while(i < r2) {
01584                         *(pf2+i*c2+j) = pftemp[i];
01585                         ++i;
01586                 }
01587 
01588                 ++j;
01589         }
01590 
01591         if( bdel ) delete[] pftemp;
01592 
01593         return pf1;
01594 }
01595 
01596 template <class T> inline void add(T* pf1, T* pf2, int r)
01597 {
01598         assert( pf1 != NULL && pf2 != NULL);
01599 
01600         while(r > 0) {
01601                 --r;
01602                 pf1[r] += pf2[r];
01603         }
01604 }
01605 
01606 template <class T> inline void sub(T* pf1, T* pf2, int r)
01607 {
01608         assert( pf1 != NULL && pf2 != NULL);
01609 
01610         while(r > 0) {
01611                 --r;
01612                 pf1[r] -= pf2[r];
01613         }
01614 }
01615 
01616 template <class T> inline T normsqr(const T* pf1, int r)
01617 {
01618         assert( pf1 != NULL );
01619 
01620         T d = 0.0;
01621         while(r > 0) {
01622                 --r;
01623                 d += pf1[r] * pf1[r];
01624         }
01625 
01626         return d;
01627 }
01628 
01629 template <class T> inline T lengthsqr(const T* pf1, const T* pf2, int length)
01630 {
01631         T d = 0;
01632         while(length > 0) {
01633                 --length;
01634         T t = pf1[length] - pf2[length];
01635                 d += t * t;
01636         }
01637 
01638         return d;
01639 }
01640 
01641 template <class T> inline T dot(T* pf1, T* pf2, int length)
01642 {
01643         T d = 0;
01644         while(length > 0) {
01645                 --length;
01646                 d += pf1[length] * pf2[length];
01647         }
01648 
01649         return d;
01650 }
01651 
01652 template <class T> inline T sum(T* pf, int length)
01653 {
01654         T d = 0;
01655         while(length > 0) {
01656                 --length;
01657                 d += pf[length];
01658         }
01659 
01660         return d;
01661 }
01662 
01663 template <class T> inline bool inv2(T* pf, T* pfres)
01664 {
01665         T fdet = pf[0] * pf[3] - pf[1] * pf[2];
01666 
01667         if( fabs(fdet) < 1e-16 ) return false;
01668 
01669         fdet = 1 / fdet;
01670         //if( pfdet != NULL ) *pfdet = fdet;
01671 
01672         if( pfres != pf ) {
01673                 pfres[0] = fdet * pf[3];                pfres[1] = -fdet * pf[1];
01674                 pfres[2] = -fdet * pf[2];               pfres[3] = fdet * pf[0];
01675                 return true;
01676         }
01677 
01678         T ftemp = pf[0];
01679         pfres[0] = pf[3] * fdet;
01680         pfres[1] *= -fdet;
01681         pfres[2] *= -fdet;
01682         pfres[3] = ftemp * pf[0];
01683 
01684         return true;
01685 }
01686 
01687 template <class T, class S>
01688 void Tridiagonal3 (S* mat, T* diag, T* subd)
01689 {
01690     T a, b, c, d, e, f, ell, q;
01691 
01692     a = mat[0*3+0];
01693     b = mat[0*3+1];
01694     c = mat[0*3+2];
01695     d = mat[1*3+1];
01696     e = mat[1*3+2];
01697     f = mat[2*3+2];
01698 
01699         subd[2] = 0.0;
01700     diag[0] = a;
01701     if ( fabs(c) >= g_fEpsilon ) {
01702         ell = (T)RaveSqrt(b*b+c*c);
01703         b /= ell;
01704         c /= ell;
01705         q = 2*b*e+c*(f-d);
01706         diag[1] = d+c*q;
01707         diag[2] = f-c*q;
01708         subd[0] = ell;
01709         subd[1] = e-b*q;
01710         mat[0*3+0] = (S)1; mat[0*3+1] = (S)0; mat[0*3+2] = (T)0;
01711         mat[1*3+0] = (S)0; mat[1*3+1] = b; mat[1*3+2] = c;
01712         mat[2*3+0] = (S)0; mat[2*3+1] = c; mat[2*3+2] = -b;
01713     }
01714     else {
01715         diag[1] = d;
01716         diag[2] = f;
01717         subd[0] = b;
01718         subd[1] = e;
01719         mat[0*3+0] = (S)1; mat[0*3+1] = (S)0; mat[0*3+2] = (S)0;
01720         mat[1*3+0] = (S)0; mat[1*3+1] = (S)1; mat[1*3+2] = (S)0;
01721         mat[2*3+0] = (S)0; mat[2*3+1] = (S)0; mat[2*3+2] = (S)1;
01722     }
01723 }
01724 
01725 template <class T>
01726 int Min(T* pts, int stride, int numPts)
01727 {
01728     assert( pts != NULL && numPts > 0 && stride > 0 );
01729 
01730     int best = pts[0]; pts += stride;
01731     for(int i = 1; i < numPts; ++i, pts += stride) {
01732         if( best > pts[0] )
01733             best = pts[0];
01734     }
01735 
01736     return best;
01737 }
01738 
01739 template <class T>
01740 int Max(T* pts, int stride, int numPts)
01741 {
01742     assert( pts != NULL && numPts > 0 && stride > 0 );
01743 
01744     int best = pts[0]; pts += stride;
01745     for(int i = 1; i < numPts; ++i, pts += stride) {
01746         if( best < pts[0] )
01747             best = pts[0];
01748     }
01749 
01750     return best;
01751 }
01752 
01753 // Don't add new lines to the output << operators. Some applications use it to serialize the data
01754 // to send across the network.
01755 
01756 template <class T, class U>
01757 std::basic_ostream<T>& operator<<(std::basic_ostream<T>& O, const RaveVector<U>& v)
01758 {
01759     return O << v.x << " " << v.y << " " << v.z << " " << v.w << " ";
01760 }
01761 
01762 template <class T, class U>
01763 std::basic_istream<T>& operator>>(std::basic_istream<T>& I, RaveVector<U>& v)
01764 {
01765     return I >> v.x >> v.y >> v.z >> v.w;
01766 }
01767 
01768 template <class T, class U>
01769 std::basic_ostream<T>& operator<<(std::basic_ostream<T>& O, const RaveTransform<U>& v)
01770 {
01771     return O << v.rot.x << " " << v.rot.y << " " << v.rot.z << " " << v.rot.w << " "
01772              << v.trans.x << " " << v.trans.y << " " << v.trans.z << " ";
01773 }
01774 
01775 template <class T, class U>
01776 std::basic_istream<T>& operator>>(std::basic_istream<T>& I, RaveTransform<U>& v)
01777 {
01778     return I >> v.rot.x >> v.rot.y >> v.rot.z >> v.rot.w >> v.trans.x >> v.trans.y >> v.trans.z;
01779 }
01780 
01781 // serial in column order! This is the format transformations are passed across the network
01782 template <class T, class U>
01783 std::basic_ostream<T>& operator<<(std::basic_ostream<T>& O, const RaveTransformMatrix<U>& v)
01784 {
01785     return O << v.m[0] << " " << v.m[4] << " " << v.m[8] << " "
01786              << v.m[1] << " " << v.m[5] << " " << v.m[9] << " "
01787              << v.m[2] << " " << v.m[6] << " " << v.m[10] << " "
01788              << v.trans.x << " " << v.trans.y << " " << v.trans.z << " ";
01789 }
01790 
01791 // read in column order! This is the format transformations are passed across the network
01792 template <class T, class U>
01793 std::basic_istream<T>& operator>>(std::basic_istream<T>& I, RaveTransformMatrix<U>& v)
01794 {
01795     return I >> v.m[0] >> v.m[4] >> v.m[8]
01796              >> v.m[1] >> v.m[5] >> v.m[9]
01797              >> v.m[2] >> v.m[6] >> v.m[10]
01798              >> v.trans.x >> v.trans.y >> v.trans.z;
01799 }
01800 
01801 #endif
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends Defines


checkerboard_detector
Author(s): Rosen Diankov (rdiankov@cs.cmu.edu)
autogenerated on Sat Mar 23 2013 18:37:21