00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00028 #ifndef RAVE_MATH_H
00029 #define RAVE_MATH_H
00030
00031 #ifndef ODE_API
00032
00033
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
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
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
00164 inline float* mult4(float* pfres, const float* pf1, const float* pf2);
00165
00166
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
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
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
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
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
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
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
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
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
00677
00678
00679
00680
00681
00682
00684
00686
00687
00688
00689 inline bool eig2(const dReal* pfmat, dReal* peigs, dReal& fv1x, dReal& fv1y, dReal& fv2x, dReal& fv2y);
00690
00691
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
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);
00716 template <class T> int Max(T* pts, int stride, int numPts);
00717
00718
00719 template <class T> inline void mult(T* pf, T fa, int r);
00720
00721
00722
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
00727
00728
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
00733
00734
00735
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
00740
00741
00742
00743 template <class T> inline T* multto1(T* pf1, T* pf2, int r1, int c1, T* pftemp = NULL);
00744
00745
00746
00747 template <class T, class S> inline T* multto2(T* pf1, S* pf2, int r2, int c2, S* pftemp = NULL);
00748
00749
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
00758 template <class T> inline bool inv2(T* pf, T* pfres);
00759
00761
00763 bool eig2(const dReal* pfmat, dReal* peigs, dReal& fv1x, dReal& fv1y, dReal& fv2x, dReal& fv2y)
00764 {
00765
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
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
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
00812 d = RaveSqrt(d);
00813 r1 = (T)-0.5 * (b + d) / a;
00814 r2 = r1 + d/a;
00815 return 2;
00816 }
00817
00818
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
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
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
00973 RaveVector<T> qm;
00974
00975
00976 T cosHalfTheta = qa.w * qb.w + qa.x * qb.x + qa.y * qb.y + qa.z * qb.z;
00977
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
00983 T halfTheta = RaveAcos(cosHalfTheta);
00984 T sinHalfTheta = RaveSqrt(1 - cosHalfTheta*cosHalfTheta);
00985
00986
00987 if (RaveFabs(sinHalfTheta) < 0.0001f){
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
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
01023
01024
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
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
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
01076
01077
01078
01079
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
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
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
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
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
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
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
01411
01412
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
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
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
01754
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
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
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