math.h
Go to the documentation of this file.
1 // Copyright (C) 2003-2008 Rosen Diankov
2 // Computer Science Department
3 // Carnegie Mellon University, Robotics Institute
4 // 5000 Forbes Ave. Pittsburgh, PA 15213
5 // U.S.A.
6 //
7 // This file is part of OpenRAVE.
8 // OpenRAVE is free software: you can redistribute it and/or modify
9 // it under the terms of the GNU Lesser General Public License as published by
10 // the Free Software Foundation, either version 3 of the License, or
11 // at your option) any later version.
12 //
13 // This program is distributed in the hope that it will be useful,
14 // but WITHOUT ANY WARRANTY; without even the implied warranty of
15 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16 // GNU Lesser General Public License for more details.
17 //
18 // You should have received a copy of the GNU Lesser General Public License
19 // along with this program. If not, see <http://www.gnu.org/licenses/>.
28 #ifndef RAVE_MATH_H
29 #define RAVE_MATH_H
30 
31 #ifndef ODE_API
32 
33 // special defines from ODE in case ODE isn't present
34 
35 #define dSINGLE
36 
37 #if defined(dSINGLE)
38 typedef float dReal;
39 #define dSqrt(x) (sqrtf(x)) // square root
40 #define dRecip(x) ((1.0f/(x))) // reciprocal
41 #define dSin(x) (sinf(x)) // sin
42 #define dCos(x) (cosf(x)) // cos
43 #else
44 typedef double dReal;
45 #define dSqrt(x) (sqrt(x)) // square root
46 #define dRecip(x) ((1.0/(x))) // reciprocal
47 #define dSin(x) (sin(x)) // sin
48 #define dCos(x) (cos(x)) // cos
49 #endif
50 typedef dReal dVector3[4];
51 typedef dReal dVector4[4];
52 typedef dReal dMatrix3[4*3];
53 typedef dReal dMatrix4[4*4];
54 typedef dReal dMatrix6[8*6];
55 typedef dReal dQuaternion[4];
56 
57 #define _R(i,j) R[(i)*4+(j)]
58 
59 
60 inline void dQMultiply0 (dQuaternion qa, const dQuaternion qb, const dQuaternion qc)
61 {
62  assert(qa && qb && qc);
63  qa[0] = qb[0]*qc[0] - qb[1]*qc[1] - qb[2]*qc[2] - qb[3]*qc[3];
64  qa[1] = qb[0]*qc[1] + qb[1]*qc[0] + qb[2]*qc[3] - qb[3]*qc[2];
65  qa[2] = qb[0]*qc[2] + qb[2]*qc[0] + qb[3]*qc[1] - qb[1]*qc[3];
66  qa[3] = qb[0]*qc[3] + qb[3]*qc[0] + qb[1]*qc[2] - qb[2]*qc[1];
67 }
68 
69 inline void dRfromQ (dMatrix3 R, const dQuaternion q)
70 {
71  assert (q && R);
72  // q = (s,vx,vy,vz)
73  dReal qq1 = 2*q[1]*q[1];
74  dReal qq2 = 2*q[2]*q[2];
75  dReal qq3 = 2*q[3]*q[3];
76  _R(0,0) = 1 - qq2 - qq3;
77  _R(0,1) = 2*(q[1]*q[2] - q[0]*q[3]);
78  _R(0,2) = 2*(q[1]*q[3] + q[0]*q[2]);
79  _R(0,3) = (dReal)(0.0);
80  _R(1,0) = 2*(q[1]*q[2] + q[0]*q[3]);
81  _R(1,1) = 1 - qq1 - qq3;
82  _R(1,2) = 2*(q[2]*q[3] - q[0]*q[1]);
83  _R(1,3) = (dReal)(0.0);
84  _R(2,0) = 2*(q[1]*q[3] - q[0]*q[2]);
85  _R(2,1) = 2*(q[2]*q[3] + q[0]*q[1]);
86  _R(2,2) = 1 - qq1 - qq2;
87  _R(2,3) = (dReal)(0.0);
88 }
89 
90 inline void dQfromR (dQuaternion q, const dMatrix3 R)
91 {
92  assert(q && R);
93  dReal tr,s;
94  tr = _R(0,0) + _R(1,1) + _R(2,2);
95  if (tr >= 0) {
96  s = dSqrt (tr + 1);
97  q[0] = (dReal)(0.5) * s;
98  s = (dReal)(0.5) * dRecip(s);
99  q[1] = (_R(2,1) - _R(1,2)) * s;
100  q[2] = (_R(0,2) - _R(2,0)) * s;
101  q[3] = (_R(1,0) - _R(0,1)) * s;
102  }
103  else {
104  // find the largest diagonal element and jump to the appropriate case
105  if (_R(1,1) > _R(0,0)) {
106  if (_R(2,2) > _R(1,1)) goto case_2;
107  goto case_1;
108  }
109  if (_R(2,2) > _R(0,0)) goto case_2;
110  goto case_0;
111 
112  case_0:
113  s = dSqrt((_R(0,0) - (_R(1,1) + _R(2,2))) + 1);
114  q[1] = (dReal)(0.5) * s;
115  s = (dReal)(0.5) * dRecip(s);
116  q[2] = (_R(0,1) + _R(1,0)) * s;
117  q[3] = (_R(2,0) + _R(0,2)) * s;
118  q[0] = (_R(2,1) - _R(1,2)) * s;
119  return;
120 
121  case_1:
122  s = dSqrt((_R(1,1) - (_R(2,2) + _R(0,0))) + 1);
123  q[2] = (dReal)(0.5) * s;
124  s = (dReal)(0.5) * dRecip(s);
125  q[3] = (_R(1,2) + _R(2,1)) * s;
126  q[1] = (_R(0,1) + _R(1,0)) * s;
127  q[0] = (_R(0,2) - _R(2,0)) * s;
128  return;
129 
130  case_2:
131  s = dSqrt((_R(2,2) - (_R(0,0) + _R(1,1))) + 1);
132  q[3] = (dReal)(0.5) * s;
133  s = (dReal)(0.5) * dRecip(s);
134  q[1] = (_R(2,0) + _R(0,2)) * s;
135  q[2] = (_R(1,2) + _R(2,1)) * s;
136  q[0] = (_R(1,0) - _R(0,1)) * s;
137  return;
138  }
139 }
140 
141 #undef _R
142 
143 #endif
144 
145 #ifndef PI
146 #define PI ((dReal)3.141592654)
147 #endif
148 
149 #define rswap(x, y) *(int*)&(x) ^= *(int*)&(y) ^= *(int*)&(x) ^= *(int*)&(y);
150 
151 #define g_fEpsilon 1e-8
152 #define distinctRoots 0 // roots r0 < r1 < r2
153 #define singleRoot 1 // root r0
154 #define floatRoot01 2 // roots r0 = r1 < r2
155 #define floatRoot12 4 // roots r0 < r1 = r2
156 #define tripleRoot 6 // roots r0 = r1 = r2
157 
158 template <class T> inline T RAD_2_DEG(T radians) { return (radians * (T)57.29577951); }
159 
160 template <class T> class RaveTransform;
161 template <class T> class RaveTransformMatrix;
162 
163 // multiplies 4x4 matrices
164 inline float* mult4(float* pfres, const float* pf1, const float* pf2);
165 
166 // pf1^T * pf2
167 inline float* multtrans3(float* pfres, const float* pf1, const float* pf2);
168 inline float* multtrans4(float* pfres, const float* pf1, const float* pf2);
169 inline float* transnorm3(float* pfout, const float* pfmat, const float* pf);
170 
171 inline float* transpose3(const float* pf, float* pfres);
172 inline float* transpose4(const float* pf, float* pfres);
173 
174 inline float dot2(const float* pf1, const float* pf2);
175 inline float dot3(const float* pf1, const float* pf2);
176 inline float dot4(const float* pf1, const float* pf2);
177 
178 inline float lengthsqr2(const float* pf);
179 inline float lengthsqr3(const float* pf);
180 inline float lengthsqr4(const float* pf);
181 
182 inline float* normalize2(float* pfout, const float* pf);
183 inline float* normalize3(float* pfout, const float* pf);
184 inline float* normalize4(float* pfout, const float* pf);
185 
186 inline float* cross3(float* pfout, const float* pf1, const float* pf2);
187 
188 // multiplies 3x3 matrices
189 inline float* mult3_s4(float* pfres, const float* pf1, const float* pf2);
190 inline float* mult3_s3(float* pfres, const float* pf1, const float* pf2);
191 
192 inline float* inv3(const float* pf, float* pfres, float* pfdet, int stride);
193 inline float* inv4(const float* pf, float* pfres);
194 
195 
196 inline double* mult4(double* pfres, const double* pf1, const double* pf2);
197 
198 // pf1^T * pf2
199 inline double* multtrans3(double* pfres, const double* pf1, const double* pf2);
200 inline double* multtrans4(double* pfres, const double* pf1, const double* pf2);
201 inline double* transnorm3(double* pfout, const double* pfmat, const double* pf);
202 
203 inline double* transpose3(const double* pf, double* pfres);
204 inline double* transpose4(const double* pf, double* pfres);
205 
206 inline double dot2(const double* pf1, const double* pf2);
207 inline double dot3(const double* pf1, const double* pf2);
208 inline double dot4(const double* pf1, const double* pf2);
209 
210 inline double lengthsqr2(const double* pf);
211 inline double lengthsqr3(const double* pf);
212 inline double lengthsqr4(const double* pf);
213 
214 inline double* normalize2(double* pfout, const double* pf);
215 inline double* normalize3(double* pfout, const double* pf);
216 inline double* normalize4(double* pfout, const double* pf);
217 
218 inline double* cross3(double* pfout, const double* pf1, const double* pf2);
219 
220 // multiplies 3x3 matrices
221 inline double* mult3_s4(double* pfres, const double* pf1, const double* pf2);
222 inline double* mult3_s3(double* pfres, const double* pf1, const double* pf2);
223 
224 inline double* inv3(const double* pf, double* pfres, double* pfdet, int stride);
225 inline double* inv4(const double* pf, double* pfres);
226 
227 
228 inline float RaveSqrt(float f) { return sqrtf(f); }
229 inline double RaveSqrt(double f) { return sqrt(f); }
230 inline float RaveSin(float f) { return sinf(f); }
231 inline double RaveSin(double f) { return sin(f); }
232 inline float RaveCos(float f) { return cosf(f); }
233 inline double RaveCos(double f) { return cos(f); }
234 inline float RaveFabs(float f) { return fabsf(f); }
235 inline double RaveFabs(double f) { return fabs(f); }
236 inline float RaveAcos(float f) { return acosf(f); }
237 inline double RaveAcos(double f) { return acos(f); }
238 
241 template <class T>
242 class RaveVector
243 {
244 public:
245  T x, y, z, w;
246 
247  RaveVector() : x(0), y(0), z(0), w(0) {}
248 
249  RaveVector(T x, T y, T z) : x(x), y(y), z(z), w(0) {}
250  RaveVector(T x, T y, T z, T w) : x(x), y(y), z(z), w(w) {}
251  template<class U> RaveVector(const RaveVector<U> &vec) : x((T)vec.x), y((T)vec.y), z((T)vec.z), w((T)vec.w) {}
252 
254  template<class U> RaveVector(const U* pf) { assert(pf != NULL); x = (T)pf[0]; y = (T)pf[1]; z = (T)pf[2]; w = 0; }
255 
256  T operator[](int i) const { return (&x)[i]; }
257  T& operator[](int i) { return (&x)[i]; }
258 
259  // casting operators
260  operator T* () { return &x; }
261  operator const T* () const { return (const T*)&x; }
262 
263  template <class U>
264  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; }
265 
266  // SCALAR FUNCTIONS
267  template <class U> inline T dot(const RaveVector<U> &v) const { return x*v.x + y*v.y + z*v.z + w*v.w; }
268  inline RaveVector<T>& normalize() { return normalize4(); }
269 
270  inline RaveVector<T>& normalize4() {
271  T f = x*x+y*y+z*z+w*w;
272  assert( f > 0 );
273  f = 1.0f / RaveSqrt(f);
274  x *= f; y *= f; z *= f; w *= f;
275  return *this;
276  }
277  inline RaveVector<T>& normalize3() {
278  T f = x*x+y*y+z*z;
279  assert( f > 0 );
280  f = 1.0f / RaveSqrt(f);
281  x *= f; y *= f; z *= f;
282  return *this;
283  }
284 
285  inline T lengthsqr2() const { return x*x + y*y; }
286  inline T lengthsqr3() const { return x*x + y*y + z*z; }
287  inline T lengthsqr4() const { return x*x + y*y + z*z + w*w; }
288 
289  inline void Set3(const T* pvals) { x = pvals[0]; y = pvals[1]; z = pvals[2]; }
290  inline void Set3(T val1, T val2, T val3) { x = val1; y = val2; z = val3; }
291  inline void Set4(const T* pvals) { x = pvals[0]; y = pvals[1]; z = pvals[2]; w = pvals[3]; }
292  inline void Set4(T val1, T val2, T val3, T val4) { x = val1; y = val2; z = val3; w = val4;}
295  inline RaveVector<T>& Cross(const RaveVector<T> &v) { Cross(*this, v); return *this; }
296 
298  inline RaveVector<T>& Cross(const RaveVector<T> &u, const RaveVector<T> &v) {
299  RaveVector<T> ucrossv;
300  ucrossv[0] = u[1] * v[2] - u[2] * v[1];
301  ucrossv[1] = u[2] * v[0] - u[0] * v[2];
302  ucrossv[2] = u[0] * v[1] - u[1] * v[0];
303  *this = ucrossv;
304  return *this;
305  }
306 
307  inline RaveVector<T> operator-() const { RaveVector<T> v; v.x = -x; v.y = -y; v.z = -z; v.w = -w; return v; }
308  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; }
309  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; }
310  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; }
311  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; }
312 
313  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; }
314  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; }
315  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; }
316 
317  inline RaveVector<T>& operator *= (const T k) { x *= k; y *= k; z *= k; w *= k; return *this; }
318  inline RaveVector<T>& operator /= (const T _k) { T k=1/_k; x *= k; y *= k; z *= k; w *= k; return *this; }
319 
320  template <class U> friend RaveVector<U> operator* (float f, const RaveVector<U>& v);
321  template <class U> friend RaveVector<U> operator* (double f, const RaveVector<U>& v);
322 
323  template <class S, class U> friend std::basic_ostream<S>& operator<<(std::basic_ostream<S>& O, const RaveVector<U>& v);
324  template <class S, class U> friend std::basic_istream<S>& operator>>(std::basic_istream<S>& I, RaveVector<U>& v);
325 
327  template <class U> inline RaveVector<T> operator^(const RaveVector<U> &v) const {
328  RaveVector<T> ucrossv;
329  ucrossv[0] = y * v[2] - z * v[1];
330  ucrossv[1] = z * v[0] - x * v[2];
331  ucrossv[2] = x * v[1] - y * v[0];
332  return ucrossv;
333  }
334 };
335 
336 typedef RaveVector<dReal> Vector;
337 
338 template <class T>
339 inline RaveVector<T> operator* (float f, const RaveVector<T>& left)
340 {
341  RaveVector<T> v;
342  v.x = (T)f * left.x;
343  v.y = (T)f * left.y;
344  v.z = (T)f * left.z;
345  v.w = (T)f * left.w;
346  return v;
347 }
348 
349 template <class T>
350 inline RaveVector<T> operator* (double f, const RaveVector<T>& left)
351 {
352  RaveVector<T> v;
353  v.x = (T)f * left.x;
354  v.y = (T)f * left.y;
355  v.z = (T)f * left.z;
356  v.w = (T)f * left.w;
357  return v;
358 }
359 
361 template <class T>
362 class RaveTransform
363 {
364 public:
365  RaveTransform() { rot.x = 1; }
366  template <class U> RaveTransform(const RaveTransform<U>& t) {
367  rot = t.rot;
368  trans = t.trans;
369  }
370  template <class U> RaveTransform(const RaveVector<U>& rot, const RaveVector<U>& trans) : rot(rot), trans(trans) {}
371  inline RaveTransform(const RaveTransformMatrix<T>& t);
372 
373  void identity() {
374  rot.x = 1; rot.y = rot.z = rot.w = 0;
375  trans.x = trans.y = trans.z = 0;
376  }
377 
378  template <class U>
379  inline void rotfromaxisangle(const RaveVector<U>& axis, U angle) {
380  U sinang = (U)RaveSin(angle/2);
381  rot.x = (U)RaveCos(angle/2);
382  rot.y = axis.x*sinang;
383  rot.z = axis.y*sinang;
384  rot.w = axis.z*sinang;
385  }
386 
388  inline RaveVector<T> operator* (const RaveVector<T>& r) const {
389  return trans + rotate(r);
390  }
391 
392  inline RaveVector<T> rotate(const RaveVector<T>& r) const {
393  T xx = 2 * rot.y * rot.y;
394  T xy = 2 * rot.y * rot.z;
395  T xz = 2 * rot.y * rot.w;
396  T xw = 2 * rot.y * rot.x;
397  T yy = 2 * rot.z * rot.z;
398  T yz = 2 * rot.z * rot.w;
399  T yw = 2 * rot.z * rot.x;
400  T zz = 2 * rot.w * rot.w;
401  T zw = 2 * rot.w * rot.x;
402 
403  RaveVector<T> v;
404  v.x = (1-yy-zz) * r.x + (xy-zw) * r.y + (xz+yw)*r.z;
405  v.y = (xy+zw) * r.x + (1-xx-zz) * r.y + (yz-xw)*r.z;
406  v.z = (xz-yw) * r.x + (yz+xw) * r.y + (1-xx-yy)*r.z;
407  return v;
408  }
409 
411  inline RaveTransform<T> operator* (const RaveTransform<T>& r) const {
412  RaveTransform<T> t;
413  t.trans = operator*(r.trans);
414  t.rot.x = rot.x*r.rot.x - rot.y*r.rot.y - rot.z*r.rot.z - rot.w*r.rot.w;
415  t.rot.y = rot.x*r.rot.y + rot.y*r.rot.x + rot.z*r.rot.w - rot.w*r.rot.z;
416  t.rot.z = rot.x*r.rot.z + rot.z*r.rot.x + rot.w*r.rot.y - rot.y*r.rot.w;
417  t.rot.w = rot.x*r.rot.w + rot.w*r.rot.x + rot.y*r.rot.z - rot.z*r.rot.y;
418 
419  return t;
420  }
421 
422  inline RaveTransform<T>& operator*= (const RaveTransform<T>& right) {
423  *this = operator*(right);
424  return *this;
425  }
426 
427  inline RaveTransform<T> inverse() const {
428  RaveTransform<T> inv;
429  inv.rot.x = rot.x;
430  inv.rot.y = -rot.y;
431  inv.rot.z = -rot.z;
432  inv.rot.w = -rot.w;
433 
434  inv.trans = -inv.rotate(trans);
435  return inv;
436  }
437 
438  template <class S, class U> friend std::basic_ostream<S>& operator<<(std::basic_ostream<S>& O, const RaveTransform<U>& v);
439  template <class S, class U> friend std::basic_istream<S>& operator>>(std::basic_istream<S>& I, RaveTransform<U>& v);
440 
441  RaveVector<T> rot, trans;
442 };
443 
444 typedef RaveTransform<dReal> Transform;
445 
447 template <class T>
448 class RaveTransformMatrix
449 {
450 public:
451  inline RaveTransformMatrix() { identity(); m[3] = m[7] = m[11] = 0; }
452  template <class U> RaveTransformMatrix(const RaveTransformMatrix<U>& t) {
453  // don't memcpy!
454  m[0] = t.m[0]; m[1] = t.m[1]; m[2] = t.m[2]; m[3] = t.m[3];
455  m[4] = t.m[4]; m[5] = t.m[5]; m[6] = t.m[6]; m[7] = t.m[7];
456  m[8] = t.m[8]; m[9] = t.m[9]; m[10] = t.m[10]; m[11] = t.m[11];
457  trans = t.trans;
458  }
459  inline RaveTransformMatrix(const RaveTransform<T>& t);
460 
461  inline void identity() {
462  m[0] = 1; m[1] = 0; m[2] = 0;
463  m[4] = 0; m[5] = 1; m[6] = 0;
464  m[8] = 0; m[9] = 0; m[10] = 1;
465  trans.x = trans.y = trans.z = 0;
466  }
467 
468  template <class U>
469  inline void rotfromaxisangle(const RaveVector<U>& axis, U angle) {
470  RaveVector<T> quat;
471  U sinang = (U)RaveSin(angle/2);
472  quat.x = (U)RaveCos(angle/2);
473  quat.y = axis.x*sinang;
474  quat.z = axis.y*sinang;
475  quat.w = axis.z*sinang;
476  rotfromquat(quat);
477  }
478 
479  template <class U>
480  inline void rotfromquat(const RaveVector<U>& quat) {
481  // q = (s,vx,vy,vz)
482  T qq1 = 2*quat[1]*quat[1];
483  T qq2 = 2*quat[2]*quat[2];
484  T qq3 = 2*quat[3]*quat[3];
485  m[4*0+0] = 1 - qq2 - qq3;
486  m[4*0+1] = 2*(quat[1]*quat[2] - quat[0]*quat[3]);
487  m[4*0+2] = 2*(quat[1]*quat[3] + quat[0]*quat[2]);
488  m[4*0+3] = 0;
489  m[4*1+0] = 2*(quat[1]*quat[2] + quat[0]*quat[3]);
490  m[4*1+1] = 1 - qq1 - qq3;
491  m[4*1+2] = 2*(quat[2]*quat[3] - quat[0]*quat[1]);
492  m[4*1+3] = 0;
493  m[4*2+0] = 2*(quat[1]*quat[3] - quat[0]*quat[2]);
494  m[4*2+1] = 2*(quat[2]*quat[3] + quat[0]*quat[1]);
495  m[4*2+2] = 1 - qq1 - qq2;
496  m[4*2+3] = 0;
497  }
498 
499  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) {
500  m[0] = m_00; m[1] = m_01; m[2] = m_02; m[3] = 0;
501  m[4] = m_10; m[5] = m_11; m[6] = m_12; m[7] = 0;
502  m[8] = m_20; m[9] = m_21; m[10] = m_22; m[11] = 0;
503  }
504 
505  inline T rot(int i, int j) const {
506  assert( i >= 0 && i < 3 && j >= 0 && j < 3);
507  return m[4*i+j];
508  }
509  inline T& rot(int i, int j) {
510  assert( i >= 0 && i < 3 && j >= 0 && j < 3);
511  return m[4*i+j];
512  }
513 
514  template <class U>
515  inline RaveVector<T> operator* (const RaveVector<U>& r) const {
516  RaveVector<T> v;
517  v[0] = r[0] * m[0] + r[1] * m[1] + r[2] * m[2] + trans.x;
518  v[1] = r[0] * m[4] + r[1] * m[5] + r[2] * m[6] + trans.y;
519  v[2] = r[0] * m[8] + r[1] * m[9] + r[2] * m[10] + trans.z;
520  return v;
521  }
522 
524  inline RaveTransformMatrix<T> operator* (const RaveTransformMatrix<T>& r) const {
525  RaveTransformMatrix<T> t;
526  mult3_s4(&t.m[0], &m[0], &r.m[0]);
527  t.trans[0] = r.trans[0] * m[0] + r.trans[1] * m[1] + r.trans[2] * m[2] + trans.x;
528  t.trans[1] = r.trans[0] * m[4] + r.trans[1] * m[5] + r.trans[2] * m[6] + trans.y;
529  t.trans[2] = r.trans[0] * m[8] + r.trans[1] * m[9] + r.trans[2] * m[10] + trans.z;
530  return t;
531  }
532 
533  inline RaveTransformMatrix<T> operator*= (const RaveTransformMatrix<T>& r) const {
534  *this = *this * r;
535  return *this;
536  }
537 
538  template <class U>
539  inline RaveVector<U> rotate(const RaveVector<U>& r) const {
540  RaveVector<U> v;
541  v.x = r.x * m[0] + r.y * m[1] + r.z * m[2];
542  v.y = r.x * m[4] + r.y * m[5] + r.z * m[6];
543  v.z = r.x * m[8] + r.y * m[9] + r.z * m[10];
544  return v;
545  }
546  inline RaveTransformMatrix<T> inverse() const {
547  RaveTransformMatrix<T> inv;
548  inv3(m, inv.m, NULL, 4);
549  inv.trans = -inv.rotate(trans);
550  return inv;
551  }
552 
553  template <class U>
554  inline void Extract(RaveVector<U>& right, RaveVector<U>& up, RaveVector<U>& dir, RaveVector<U>& pos) const {
555  pos = trans;
556  right.x = m[0]; up.x = m[1]; dir.x = m[2];
557  right.y = m[4]; up.y = m[5]; dir.y = m[6];
558  right.z = m[8]; up.z = m[9]; dir.z = m[10];
559  }
560 
561  template <class S, class U> friend std::basic_ostream<S>& operator<<(std::basic_ostream<S>& O, const RaveTransformMatrix<U>& v);
562  template <class S, class U> friend std::basic_istream<S>& operator>>(std::basic_istream<S>& I, RaveTransformMatrix<U>& v);
563 
566  T m[12];
567  RaveVector<T> trans;
568 };
569 typedef RaveTransformMatrix<dReal> TransformMatrix;
570 
571 template <class T>
572 RaveTransform<T>::RaveTransform(const RaveTransformMatrix<T>& t)
573 {
574  trans = t.trans;
575  dReal tr,s;
576  tr = t.m[4*0+0] + t.m[4*1+1] + t.m[4*2+2];
577  if (tr >= 0) {
578  s = RaveSqrt(tr + 1);
579  rot[0] = (dReal)(0.5) * s;
580  s = (dReal)(0.5) * dRecip(s);
581  rot[1] = (t.m[4*2+1] - t.m[4*1+2]) * s;
582  rot[2] = (t.m[4*0+2] - t.m[4*2+0]) * s;
583  rot[3] = (t.m[4*1+0] - t.m[4*0+1]) * s;
584  }
585  else {
586  // find the largest diagonal element and jump to the appropriate case
587  if (t.m[4*1+1] > t.m[4*0+0]) {
588  if (t.m[4*2+2] > t.m[4*1+1]) goto case_2;
589  goto case_1;
590  }
591  if (t.m[4*2+2] > t.m[4*0+0]) goto case_2;
592  goto case_0;
593 
594  case_0:
595  s = RaveSqrt((t.m[4*0+0] - (t.m[4*1+1] + t.m[4*2+2])) + 1);
596  rot[1] = (dReal)(0.5) * s;
597  s = (dReal)(0.5) * dRecip(s);
598  rot[2] = (t.m[4*0+1] + t.m[4*1+0]) * s;
599  rot[3] = (t.m[4*2+0] + t.m[4*0+2]) * s;
600  rot[0] = (t.m[4*2+1] - t.m[4*1+2]) * s;
601  return;
602 
603  case_1:
604  s = RaveSqrt((t.m[4*1+1] - (t.m[4*2+2] + t.m[4*0+0])) + 1);
605  rot[2] = (dReal)(0.5) * s;
606  s = (dReal)(0.5) * dRecip(s);
607  rot[3] = (t.m[4*1+2] + t.m[4*2+1]) * s;
608  rot[1] = (t.m[4*0+1] + t.m[4*1+0]) * s;
609  rot[0] = (t.m[4*0+2] - t.m[4*2+0]) * s;
610  return;
611 
612  case_2:
613  s = RaveSqrt((t.m[4*2+2] - (t.m[4*0+0] + t.m[4*1+1])) + 1);
614  rot[3] = (dReal)(0.5) * s;
615  s = (dReal)(0.5) * dRecip(s);
616  rot[1] = (t.m[4*2+0] + t.m[4*0+2]) * s;
617  rot[2] = (t.m[4*1+2] + t.m[4*2+1]) * s;
618  rot[0] = (t.m[4*1+0] - t.m[4*0+1]) * s;
619  return;
620  }
621 }
622 
623 template <class T>
624 RaveTransformMatrix<T>::RaveTransformMatrix(const RaveTransform<T>& t)
625 {
626  rotfromquat(t.rot);
627  trans = t.trans;
628 
629 }
630 
631 struct RAY
632 {
633  RAY() {}
634  RAY(const Vector& _pos, const Vector& _dir) : pos(_pos), dir(_dir) {}
635  Vector pos, dir;
636 };
637 
638 struct AABB
639 {
640  AABB() {}
641  AABB(const Vector& vpos, const Vector& vextents) : pos(vpos), extents(vextents) {}
642  Vector pos, extents;
643 };
644 
645 struct OBB
646 {
647  Vector right, up, dir, pos, extents;
648 };
649 
650 struct TRIANGLE
651 {
652  TRIANGLE() {}
653  TRIANGLE(const Vector& v1, const Vector& v2, const Vector& v3) : v1(v1), v2(v2), v3(v3) {}
654  ~TRIANGLE() {}
655 
656  Vector v1, v2, v3;
657 
658  const Vector& operator[](int i) const { return (&v1)[i]; }
659  Vector& operator[](int i) { return (&v1)[i]; }
660 
662  inline Vector ComputeNormal() {
663  Vector normal;
664  cross3(normal, v2-v1, v3-v1);
665  return normal;
666  }
667 };
668 
669 
671 inline dReal* transcoord3(dReal* pfout, const TransformMatrix* pmat, const dReal* pf);
672 
674 inline dReal* transnorm3(dReal* pfout, const TransformMatrix* pmat, const dReal* pf);
675 
676 // Routines made for 3D graphics that deal with 3 or 4 dim algebra structures
677 // Functions with postfix 3 are for 3x3 operations, etc
678 
679 // all fns return pfout on success or NULL on failure
680 // results and arguments can share pointers
681 
682 
684 // More complex ops that deal with arbitrary matrices //
686 
687 // extract eigen values and vectors from a 2x2 matrix and returns true if all values are real
688 // returned eigen vectors are normalized
689 inline bool eig2(const dReal* pfmat, dReal* peigs, dReal& fv1x, dReal& fv1y, dReal& fv2x, dReal& fv2y);
690 
691 // Simple routines for linear algebra algorithms //
692 int CubicRoots (double c0, double c1, double c2, double *r0, double *r1, double *r2);
693 template <class T, class S> void Tridiagonal3 (S* mat, T* diag, T* subd);
694 bool QLAlgorithm3 (float* m_aafEntry, float* afDiag, float* afSubDiag);
695 bool QLAlgorithm3 (double* m_aafEntry, double* afDiag, double* afSubDiag);
696 
697 void EigenSymmetric3(dReal* fCovariance, dReal* eval, dReal* fAxes);
698 
699 void GetCovarBasisVectors(dReal fCovariance[3][3], Vector* vRight, Vector* vUp, Vector* vDir);
700 
704 void svd3(const dReal* A, dReal* U, dReal* D, dReal* V);
705 
706 // first root returned is always >= second, roots are defined if the quadratic doesn't have real solutions
707 void QuadraticSolver(dReal* pfQuadratic, dReal* pfRoots);
708 
709 int insideQuadrilateral(const Vector* p0,const Vector* p1, const Vector* p2,const Vector* p3);
710 int insideTriangle(const Vector* p0, const Vector* p1, const Vector* p2);
711 
712 bool RayOBBTest(const RAY& r, const OBB& obb);
713 dReal DistVertexOBBSq(const Vector& v, const OBB& o);
714 
715 template <class T> int Min(T* pts, int stride, int numPts); // returns the index, stride in units of T
716 template <class T> int Max(T* pts, int stride, int numPts); // returns the index
717 
718 // multiplies a matrix by a scalar
719 template <class T> inline void mult(T* pf, T fa, int r);
720 
721 // multiplies a r1xc1 by c1xc2 matrix into pfres, if badd is true adds the result to pfres
722 // does not handle cases where pfres is equal to pf1 or pf2, use multtox for those cases
723 template <class T, class R, class S>
724 inline S* mult(T* pf1, R* pf2, int r1, int c1, int c2, S* pfres, bool badd = false);
725 
726 // pf1 is transposed before mult
727 // rows of pf2 must equal rows of pf1
728 // pfres will be c1xc2 matrix
729 template <class T, class R, class S>
730 inline S* multtrans(T* pf1, R* pf2, int r1, int c1, int c2, S* pfres, bool badd = false);
731 
732 // pf2 is transposed before mult
733 // the columns of both matrices must be the same and equal to c1
734 // r2 is the number of rows in pf2
735 // pfres must be an r1xr2 matrix
736 template <class T, class R, class S>
737 inline S* multtrans_to2(T* pf1, R* pf2, int r1, int c1, int r2, S* pfres, bool badd = false);
738 
739 // multiplies rxc matrix pf1 and cxc matrix pf2 and stores the result in pf1,
740 // the function needs a temporary buffer the size of c doubles, if pftemp == NULL,
741 // the function will allocate the necessary memory, otherwise pftemp should be big
742 // enough to store all the entries
743 template <class T> inline T* multto1(T* pf1, T* pf2, int r1, int c1, T* pftemp = NULL);
744 
745 // same as multto1 except stores the result in pf2, pf1 has to be an r2xr2 matrix
746 // pftemp must be of size r2 if not NULL
747 template <class T, class S> inline T* multto2(T* pf1, S* pf2, int r2, int c2, S* pftemp = NULL);
748 
749 // add pf1 + pf2 and store in pf1
750 template <class T> inline void sub(T* pf1, T* pf2, int r);
751 template <class T> inline T normsqr(const T* pf1, int r);
752 template <class T> inline T lengthsqr(const T* pf1, const T* pf2, int length);
753 template <class T> inline T dot(T* pf1, T* pf2, int length);
754 
755 template <class T> inline T sum(T* pf, int length);
756 
757 // takes the inverse of the 3x3 matrix pf and stores it into pfres, returns true if matrix is invertible
758 template <class T> inline bool inv2(T* pf, T* pfres);
759 
761 // Function Definitions
763 bool eig2(const dReal* pfmat, dReal* peigs, dReal& fv1x, dReal& fv1y, dReal& fv2x, dReal& fv2y)
764 {
765  // x^2 + bx + c
766  dReal a, b, c, d;
767  b = -(pfmat[0] + pfmat[3]);
768  c = pfmat[0] * pfmat[3] - pfmat[1] * pfmat[2];
769  d = b * b - 4.0f * c + 1e-16f;
770 
771  if( d < 0 ) return false;
772  if( d < 1e-16f ) {
773  a = -0.5f * b;
774  peigs[0] = a; peigs[1] = a;
775  fv1x = pfmat[1]; fv1y = a - pfmat[0];
776  c = 1 / RaveSqrt(fv1x*fv1x + fv1y*fv1y);
777  fv1x *= c; fv1y *= c;
778  fv2x = -fv1y; fv2y = fv1x;
779  return true;
780  }
781 
782  // two roots
783  d = RaveSqrt(d);
784  a = -0.5f * (b + d);
785  peigs[0] = a;
786  fv1x = pfmat[1]; fv1y = a-pfmat[0];
787  c = 1 / RaveSqrt(fv1x*fv1x + fv1y*fv1y);
788  fv1x *= c; fv1y *= c;
789 
790  a += d;
791  peigs[1] = a;
792  fv2x = pfmat[1]; fv2y = a-pfmat[0];
793  c = 1 / RaveSqrt(fv2x*fv2x + fv2y*fv2y);
794  fv2x *= c; fv2y *= c;
795  return true;
796 }
797 
798 // returns the number of real roots, fills r1 and r2 with the answers
799 template <class T>
800 inline int solvequad(T a, T b, T c, T& r1, T& r2)
801 {
802  T d = b * b - (T)4 * c * a + (T)1e-16;
803 
804  if( d < 0 ) return 0;
805 
806  if( d < (T)1e-16 ) {
807  r1 = r2 = (T)-0.5 * b / a;
808  return 1;
809  }
810 
811  // two roots
812  d = RaveSqrt(d);
813  r1 = (T)-0.5 * (b + d) / a;
814  r2 = r1 + d/a;
815  return 2;
816 }
817 
818 //#ifndef TI_USING_IPP
819 
820 #define MULT3(stride) { \
821  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]; \
822  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]; \
823  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]; \
824  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]; \
825  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]; \
826  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]; \
827  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]; \
828  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]; \
829  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]; \
830 }
831 
833 template <class T>
834 inline T* _mult3_s4(T* pfres, const T* pf1, const T* pf2)
835 {
836  assert( pf1 != NULL && pf2 != NULL && pfres != NULL );
837 
838  T* pfres2;
839  if( pfres == pf1 || pfres == pf2 ) pfres2 = (T*)alloca(12 * sizeof(T));
840  else pfres2 = pfres;
841 
842  MULT3(4)
843 
844  if( pfres2 != pfres ) memcpy(pfres, pfres2, 12*sizeof(T));
845 
846  return pfres;
847 }
848 
850 template <class T>
851 inline T* _mult3_s3(T* pfres, const T* pf1, const T* pf2)
852 {
853  assert( pf1 != NULL && pf2 != NULL && pfres != NULL );
854 
855  T* pfres2;
856  if( pfres == pf1 || pfres == pf2 ) pfres2 = (T*)alloca(9 * sizeof(T));
857  else pfres2 = pfres;
858 
859  MULT3(3)
860 
861  if( pfres2 != pfres ) memcpy(pfres, pfres2, 9*sizeof(T));
862 
863  return pfres;
864 }
865 
866 // mult4
867 template <class T>
868 inline T* _mult4(T* pfres, const T* p1, const T* p2)
869 {
870  assert( pfres != NULL && p1 != NULL && p2 != NULL );
871 
872  T* pfres2;
873  if( pfres == p1 || pfres == p2 ) pfres2 = (T*)alloca(16 * sizeof(T));
874  else pfres2 = pfres;
875 
876  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];
877  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];
878  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];
879  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];
880 
881  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];
882  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];
883  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];
884  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];
885 
886  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];
887  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];
888  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];
889  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];
890 
891  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];
892  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];
893  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];
894  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];
895 
896  if( pfres != pfres2 ) memcpy(pfres, pfres2, sizeof(T)*16);
897  return pfres;
898 }
899 
900 template <class T>
901 inline T* _multtrans3(T* pfres, const T* pf1, const T* pf2)
902 {
903  T* pfres2;
904  if( pfres == pf1 ) pfres2 = (T*)alloca(9 * sizeof(T));
905  else pfres2 = pfres;
906 
907  pfres2[0] = pf1[0]*pf2[0]+pf1[3]*pf2[3]+pf1[6]*pf2[6];
908  pfres2[1] = pf1[0]*pf2[1]+pf1[3]*pf2[4]+pf1[6]*pf2[7];
909  pfres2[2] = pf1[0]*pf2[2]+pf1[3]*pf2[5]+pf1[6]*pf2[8];
910 
911  pfres2[3] = pf1[1]*pf2[0]+pf1[4]*pf2[3]+pf1[7]*pf2[6];
912  pfres2[4] = pf1[1]*pf2[1]+pf1[4]*pf2[4]+pf1[7]*pf2[7];
913  pfres2[5] = pf1[1]*pf2[2]+pf1[4]*pf2[5]+pf1[7]*pf2[8];
914 
915  pfres2[6] = pf1[2]*pf2[0]+pf1[5]*pf2[3]+pf1[8]*pf2[6];
916  pfres2[7] = pf1[2]*pf2[1]+pf1[5]*pf2[4]+pf1[8]*pf2[7];
917  pfres2[8] = pf1[2]*pf2[2]+pf1[5]*pf2[5]+pf1[8]*pf2[8];
918 
919  if( pfres2 != pfres ) memcpy(pfres, pfres2, 9*sizeof(T));
920 
921  return pfres;
922 }
923 
924 template <class T>
925 inline T* _multtrans4(T* pfres, const T* pf1, const T* pf2)
926 {
927  T* pfres2;
928  if( pfres == pf1 ) pfres2 = (T*)alloca(16 * sizeof(T));
929  else pfres2 = pfres;
930 
931  for(int i = 0; i < 4; ++i) {
932  for(int j = 0; j < 4; ++j) {
933  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];
934  }
935  }
936 
937  return pfres;
938 }
939 
940 //generate a random quaternion
941 inline Vector GetRandomQuat(void)
942 {
943  Vector q;
944 
945  while(1) {
946  q.x = -1 + 2*(rand()/((dReal)RAND_MAX));
947  q.y = -1 + 2*(rand()/((dReal)RAND_MAX));
948  q.z = -1 + 2*(rand()/((dReal)RAND_MAX));
949  q.w = -1 + 2*(rand()/((dReal)RAND_MAX));
950 
951  dReal norm = q.lengthsqr4();
952  if(norm <= 1) {
953  q = q * (1 / RaveSqrt(norm));
954  break;
955  }
956  }
957 
958  return q;
959 }
960 
961 template <class T>
962 inline RaveVector<T> AxisAngle2Quat(const RaveVector<T>& rotaxis, T angle)
963 {
964  angle *= (T)0.5;
965  T fsin = RaveSin(angle);
966  return RaveVector<T>(RaveCos(angle), rotaxis.x*fsin, rotaxis.y * fsin, rotaxis.z * fsin);
967 }
968 
969 template <class T>
970 inline RaveVector<T> dQSlerp(const RaveVector<T>& qa, const RaveVector<T>& qb, T t)
971 {
972  // quaternion to return
973  RaveVector<T> qm;
974 
975  // Calculate angle between them.
976  T cosHalfTheta = qa.w * qb.w + qa.x * qb.x + qa.y * qb.y + qa.z * qb.z;
977  // if qa=qb or qa=-qb then theta = 0 and we can return qa
978  if (RaveFabs(cosHalfTheta) >= 1.0){
979  qm.w = qa.w;qm.x = qa.x;qm.y = qa.y;qm.z = qa.z;
980  return qm;
981  }
982  // Calculate temporary values.
983  T halfTheta = RaveAcos(cosHalfTheta);
984  T sinHalfTheta = RaveSqrt(1 - cosHalfTheta*cosHalfTheta);
985  // if theta = 180 degrees then result is not fully defined
986  // we could rotate around any axis normal to qa or qb
987  if (RaveFabs(sinHalfTheta) < 0.0001f){ // fabs is floating point absolute
988  qm.w = (qa.w * 0.5f + qb.w * 0.5f);
989  qm.x = (qa.x * 0.5f + qb.x * 0.5f);
990  qm.y = (qa.y * 0.5f + qb.y * 0.5f);
991  qm.z = (qa.z * 0.5f + qb.z * 0.5f);
992  return qm;
993  }
994 
995  T ratioA = RaveSin((1 - t) * halfTheta) / sinHalfTheta;
996  T ratioB = RaveSin(t * halfTheta) / sinHalfTheta;
997  //calculate Quaternion.
998  qm.w = (qa.w * ratioA + qb.w * ratioB);
999  qm.x = (qa.x * ratioA + qb.x * ratioB);
1000  qm.y = (qa.y * ratioA + qb.y * ratioB);
1001  qm.z = (qa.z * ratioA + qb.z * ratioB);
1002  return qm;
1003 }
1004 
1006 template <class T> inline T matrixdet3(const T* pf, int stride)
1007 {
1008  return pf[0*stride+2] * (pf[1*stride + 0] * pf[2*stride + 1] - pf[1*stride + 1] * pf[2*stride + 0]) +
1009  pf[1*stride+2] * (pf[0*stride + 1] * pf[2*stride + 0] - pf[0*stride + 0] * pf[2*stride + 1]) +
1010  pf[2*stride+2] * (pf[0*stride + 0] * pf[1*stride + 1] - pf[0*stride + 1] * pf[1*stride + 0]);
1011 }
1012 
1015 template <class T>
1016 inline T* _inv3(const T* pf, T* pfres, T* pfdet, int stride)
1017 {
1018  T* pfres2;
1019  if( pfres == pf ) pfres2 = (T*)alloca(3 * stride * sizeof(T));
1020  else pfres2 = pfres;
1021 
1022  // inverse = C^t / det(pf) where C is the matrix of coefficients
1023 
1024  // calc C^t
1025  pfres2[0*stride + 0] = pf[1*stride + 1] * pf[2*stride + 2] - pf[1*stride + 2] * pf[2*stride + 1];
1026  pfres2[0*stride + 1] = pf[0*stride + 2] * pf[2*stride + 1] - pf[0*stride + 1] * pf[2*stride + 2];
1027  pfres2[0*stride + 2] = pf[0*stride + 1] * pf[1*stride + 2] - pf[0*stride + 2] * pf[1*stride + 1];
1028  pfres2[1*stride + 0] = pf[1*stride + 2] * pf[2*stride + 0] - pf[1*stride + 0] * pf[2*stride + 2];
1029  pfres2[1*stride + 1] = pf[0*stride + 0] * pf[2*stride + 2] - pf[0*stride + 2] * pf[2*stride + 0];
1030  pfres2[1*stride + 2] = pf[0*stride + 2] * pf[1*stride + 0] - pf[0*stride + 0] * pf[1*stride + 2];
1031  pfres2[2*stride + 0] = pf[1*stride + 0] * pf[2*stride + 1] - pf[1*stride + 1] * pf[2*stride + 0];
1032  pfres2[2*stride + 1] = pf[0*stride + 1] * pf[2*stride + 0] - pf[0*stride + 0] * pf[2*stride + 1];
1033  pfres2[2*stride + 2] = pf[0*stride + 0] * pf[1*stride + 1] - pf[0*stride + 1] * pf[1*stride + 0];
1034 
1035  T fdet = pf[0*stride + 2] * pfres2[2*stride + 0] + pf[1*stride + 2] * pfres2[2*stride + 1] +
1036  pf[2*stride + 2] * pfres2[2*stride + 2];
1037 
1038  if( pfdet != NULL )
1039  pfdet[0] = fdet;
1040 
1041  if( fabs(fdet) < 1e-12 ) {
1042  return NULL;
1043  }
1044 
1045  fdet = 1 / fdet;
1046  //if( pfdet != NULL ) *pfdet = fdet;
1047 
1048  if( pfres != pf ) {
1049  pfres[0*stride+0] *= fdet; pfres[0*stride+1] *= fdet; pfres[0*stride+2] *= fdet;
1050  pfres[1*stride+0] *= fdet; pfres[1*stride+1] *= fdet; pfres[1*stride+2] *= fdet;
1051  pfres[2*stride+0] *= fdet; pfres[2*stride+1] *= fdet; pfres[2*stride+2] *= fdet;
1052  return pfres;
1053  }
1054 
1055  pfres[0*stride+0] = pfres2[0*stride+0] * fdet;
1056  pfres[0*stride+1] = pfres2[0*stride+1] * fdet;
1057  pfres[0*stride+2] = pfres2[0*stride+2] * fdet;
1058  pfres[1*stride+0] = pfres2[1*stride+0] * fdet;
1059  pfres[1*stride+1] = pfres2[1*stride+1] * fdet;
1060  pfres[1*stride+2] = pfres2[1*stride+2] * fdet;
1061  pfres[2*stride+0] = pfres2[2*stride+0] * fdet;
1062  pfres[2*stride+1] = pfres2[2*stride+1] * fdet;
1063  pfres[2*stride+2] = pfres2[2*stride+2] * fdet;
1064  return pfres;
1065 }
1066 
1067 // inverse if 92 mults and 39 adds
1068 template <class T>
1069 inline T* _inv4(const T* pf, T* pfres)
1070 {
1071  T* pfres2;
1072  if( pfres == pf ) pfres2 = (T*)alloca(16 * sizeof(T));
1073  else pfres2 = pfres;
1074 
1075  // inverse = C^t / det(pf) where C is the matrix of coefficients
1076 
1077  // calc C^t
1078 
1079  // determinants of all possibel 2x2 submatrices formed by last two rows
1080  T fd0, fd1, fd2;
1081  T f1, f2, f3;
1082  fd0 = pf[2*4 + 0] * pf[3*4 + 1] - pf[2*4 + 1] * pf[3*4 + 0];
1083  fd1 = pf[2*4 + 1] * pf[3*4 + 2] - pf[2*4 + 2] * pf[3*4 + 1];
1084  fd2 = pf[2*4 + 2] * pf[3*4 + 3] - pf[2*4 + 3] * pf[3*4 + 2];
1085 
1086  f1 = pf[2*4 + 1] * pf[3*4 + 3] - pf[2*4 + 3] * pf[3*4 + 1];
1087  f2 = pf[2*4 + 0] * pf[3*4 + 3] - pf[2*4 + 3] * pf[3*4 + 0];
1088  f3 = pf[2*4 + 0] * pf[3*4 + 2] - pf[2*4 + 2] * pf[3*4 + 0];
1089 
1090  pfres2[0*4 + 0] = pf[1*4 + 1] * fd2 - pf[1*4 + 2] * f1 + pf[1*4 + 3] * fd1;
1091  pfres2[0*4 + 1] = -(pf[0*4 + 1] * fd2 - pf[0*4 + 2] * f1 + pf[0*4 + 3] * fd1);
1092 
1093  pfres2[1*4 + 0] = -(pf[1*4 + 0] * fd2 - pf[1*4 + 2] * f2 + pf[1*4 + 3] * f3);
1094  pfres2[1*4 + 1] = pf[0*4 + 0] * fd2 - pf[0*4 + 2] * f2 + pf[0*4 + 3] * f3;
1095 
1096  pfres2[2*4 + 0] = pf[1*4 + 0] * f1 - pf[1*4 + 1] * f2 + pf[1*4 + 3] * fd0;
1097  pfres2[2*4 + 1] = -(pf[0*4 + 0] * f1 - pf[0*4 + 1] * f2 + pf[0*4 + 3] * fd0);
1098 
1099  pfres2[3*4 + 0] = -(pf[1*4 + 0] * fd1 - pf[1*4 + 1] * f3 + pf[1*4 + 2] * fd0);
1100  pfres2[3*4 + 1] = pf[0*4 + 0] * fd1 - pf[0*4 + 1] * f3 + pf[0*4 + 2] * fd0;
1101 
1102  // determinants of first 2 rows of 4x4 matrix
1103  fd0 = pf[0*4 + 0] * pf[1*4 + 1] - pf[0*4 + 1] * pf[1*4 + 0];
1104  fd1 = pf[0*4 + 1] * pf[1*4 + 2] - pf[0*4 + 2] * pf[1*4 + 1];
1105  fd2 = pf[0*4 + 2] * pf[1*4 + 3] - pf[0*4 + 3] * pf[1*4 + 2];
1106 
1107  f1 = pf[0*4 + 1] * pf[1*4 + 3] - pf[0*4 + 3] * pf[1*4 + 1];
1108  f2 = pf[0*4 + 0] * pf[1*4 + 3] - pf[0*4 + 3] * pf[1*4 + 0];
1109  f3 = pf[0*4 + 0] * pf[1*4 + 2] - pf[0*4 + 2] * pf[1*4 + 0];
1110 
1111  pfres2[0*4 + 2] = pf[3*4 + 1] * fd2 - pf[3*4 + 2] * f1 + pf[3*4 + 3] * fd1;
1112  pfres2[0*4 + 3] = -(pf[2*4 + 1] * fd2 - pf[2*4 + 2] * f1 + pf[2*4 + 3] * fd1);
1113 
1114  pfres2[1*4 + 2] = -(pf[3*4 + 0] * fd2 - pf[3*4 + 2] * f2 + pf[3*4 + 3] * f3);
1115  pfres2[1*4 + 3] = pf[2*4 + 0] * fd2 - pf[2*4 + 2] * f2 + pf[2*4 + 3] * f3;
1116 
1117  pfres2[2*4 + 2] = pf[3*4 + 0] * f1 - pf[3*4 + 1] * f2 + pf[3*4 + 3] * fd0;
1118  pfres2[2*4 + 3] = -(pf[2*4 + 0] * f1 - pf[2*4 + 1] * f2 + pf[2*4 + 3] * fd0);
1119 
1120  pfres2[3*4 + 2] = -(pf[3*4 + 0] * fd1 - pf[3*4 + 1] * f3 + pf[3*4 + 2] * fd0);
1121  pfres2[3*4 + 3] = pf[2*4 + 0] * fd1 - pf[2*4 + 1] * f3 + pf[2*4 + 2] * fd0;
1122 
1123  T fdet = pf[0*4 + 3] * pfres2[3*4 + 0] + pf[1*4 + 3] * pfres2[3*4 + 1] +
1124  pf[2*4 + 3] * pfres2[3*4 + 2] + pf[3*4 + 3] * pfres2[3*4 + 3];
1125 
1126  if( fabs(fdet) < 1e-9) return NULL;
1127 
1128  fdet = 1 / fdet;
1129  //if( pfdet != NULL ) *pfdet = fdet;
1130 
1131  if( pfres2 == pfres ) {
1132  mult(pfres, fdet, 16);
1133  return pfres;
1134  }
1135 
1136  int i = 0;
1137  while(i < 16) {
1138  pfres[i] = pfres2[i] * fdet;
1139  ++i;
1140  }
1141 
1142  return pfres;
1143 }
1144 
1145 template <class T>
1146 inline T* _transpose3(const T* pf, T* pfres)
1147 {
1148  assert( pf != NULL && pfres != NULL );
1149 
1150  if( pf == pfres ) {
1151  rswap(pfres[1], pfres[3]);
1152  rswap(pfres[2], pfres[6]);
1153  rswap(pfres[5], pfres[7]);
1154  return pfres;
1155  }
1156 
1157  pfres[0] = pf[0]; pfres[1] = pf[3]; pfres[2] = pf[6];
1158  pfres[3] = pf[1]; pfres[4] = pf[4]; pfres[5] = pf[7];
1159  pfres[6] = pf[2]; pfres[7] = pf[5]; pfres[8] = pf[8];
1160 
1161  return pfres;
1162 }
1163 
1164 template <class T>
1165 inline T* _transpose4(const T* pf, T* pfres)
1166 {
1167  assert( pf != NULL && pfres != NULL );
1168 
1169  if( pf == pfres ) {
1170  rswap(pfres[1], pfres[4]);
1171  rswap(pfres[2], pfres[8]);
1172  rswap(pfres[3], pfres[12]);
1173  rswap(pfres[6], pfres[9]);
1174  rswap(pfres[7], pfres[13]);
1175  rswap(pfres[11], pfres[15]);
1176  return pfres;
1177  }
1178 
1179  pfres[0] = pf[0]; pfres[1] = pf[4]; pfres[2] = pf[8]; pfres[3] = pf[12];
1180  pfres[4] = pf[1]; pfres[5] = pf[5]; pfres[6] = pf[9]; pfres[7] = pf[13];
1181  pfres[8] = pf[2]; pfres[9] = pf[6]; pfres[10] = pf[10]; pfres[11] = pf[14];
1182  pfres[12] = pf[3]; pfres[13] = pf[7]; pfres[14] = pf[11]; pfres[15] = pf[15];
1183  return pfres;
1184 }
1185 
1186 template <class T>
1187 inline T _dot2(const T* pf1, const T* pf2)
1188 {
1189  assert( pf1 != NULL && pf2 != NULL );
1190  return pf1[0]*pf2[0] + pf1[1]*pf2[1];
1191 }
1192 
1193 template <class T>
1194 inline T _dot3(const T* pf1, const T* pf2)
1195 {
1196  assert( pf1 != NULL && pf2 != NULL );
1197  return pf1[0]*pf2[0] + pf1[1]*pf2[1] + pf1[2]*pf2[2];
1198 }
1199 
1200 template <class T>
1201 inline T _dot4(const T* pf1, const T* pf2)
1202 {
1203  assert( pf1 != NULL && pf2 != NULL );
1204  return pf1[0]*pf2[0] + pf1[1]*pf2[1] + pf1[2]*pf2[2] + pf1[3] * pf2[3];
1205 }
1206 
1207 template <class T>
1208 inline T _lengthsqr2(const T* pf)
1209 {
1210  assert( pf != NULL );
1211  return pf[0] * pf[0] + pf[1] * pf[1];
1212 }
1213 
1214 template <class T>
1215 inline T _lengthsqr3(const T* pf)
1216 {
1217  assert( pf != NULL );
1218  return pf[0] * pf[0] + pf[1] * pf[1] + pf[2] * pf[2];
1219 }
1220 
1221 template <class T>
1222 inline T _lengthsqr4(const T* pf)
1223 {
1224  assert( pf != NULL );
1225  return pf[0] * pf[0] + pf[1] * pf[1] + pf[2] * pf[2] + pf[3] * pf[3];
1226 }
1227 
1228 template <class T>
1229 inline T* _normalize2(T* pfout, const T* pf)
1230 {
1231  assert(pf != NULL);
1232 
1233  T f = pf[0]*pf[0] + pf[1]*pf[1];
1234  f = 1 / RaveSqrt(f);
1235  pfout[0] = pf[0] * f;
1236  pfout[1] = pf[1] * f;
1237 
1238  return pfout;
1239 }
1240 
1241 template <class T>
1242 inline T* _normalize3(T* pfout, const T* pf)
1243 {
1244  assert(pf != NULL);
1245 
1246  T f = pf[0]*pf[0] + pf[1]*pf[1] + pf[2]*pf[2];
1247 
1248  f = 1 / RaveSqrt(f);
1249  pfout[0] = pf[0] * f;
1250  pfout[1] = pf[1] * f;
1251  pfout[2] = pf[2] * f;
1252 
1253  return pfout;
1254 }
1255 
1256 template <class T>
1257 inline T* _normalize4(T* pfout, const T* pf)
1258 {
1259  assert(pf != NULL);
1260 
1261  T f = pf[0]*pf[0] + pf[1]*pf[1] + pf[2]*pf[2] + pf[3]*pf[3];
1262 
1263  f = 1 / RaveSqrt(f);
1264  pfout[0] = pf[0] * f;
1265  pfout[1] = pf[1] * f;
1266  pfout[2] = pf[2] * f;
1267  pfout[3] = pf[3] * f;
1268 
1269  return pfout;
1270 }
1271 
1272 template <class T>
1273 inline T* _cross3(T* pfout, const T* pf1, const T* pf2)
1274 {
1275  assert( pfout != NULL && pf1 != NULL && pf2 != NULL );
1276 
1277  T temp[3];
1278  temp[0] = pf1[1] * pf2[2] - pf1[2] * pf2[1];
1279  temp[1] = pf1[2] * pf2[0] - pf1[0] * pf2[2];
1280  temp[2] = pf1[0] * pf2[1] - pf1[1] * pf2[0];
1281 
1282  pfout[0] = temp[0]; pfout[1] = temp[1]; pfout[2] = temp[2];
1283  return pfout;
1284 }
1285 
1286 template <class T>
1287 inline T* transcoord3(T* pfout, const RaveTransformMatrix<T>* pmat, const T* pf)
1288 {
1289  assert( pfout != NULL && pf != NULL && pmat != NULL );
1290 
1291  T dummy[3];
1292  T* pfreal = (pfout == pf) ? dummy : pfout;
1293 
1294  pfreal[0] = pf[0] * pmat->m[0] + pf[1] * pmat->m[1] + pf[2] * pmat->m[2] + pmat->trans.x;
1295  pfreal[1] = pf[0] * pmat->m[4] + pf[1] * pmat->m[5] + pf[2] * pmat->m[6] + pmat->trans.y;
1296  pfreal[2] = pf[0] * pmat->m[8] + pf[1] * pmat->m[9] + pf[2] * pmat->m[10] + pmat->trans.z;
1297 
1298  if( pfout ==pf ) {
1299  pfout[0] = pfreal[0];
1300  pfout[1] = pfreal[1];
1301  pfout[2] = pfreal[2];
1302  }
1303 
1304  return pfout;
1305 }
1306 
1307 template <class T>
1308 inline T* _transnorm3(T* pfout, const T* pfmat, const T* pf)
1309 {
1310  assert( pfout != NULL && pf != NULL && pfmat != NULL );
1311 
1312  T dummy[3];
1313  T* pfreal = (pfout == pf) ? dummy : pfout;
1314 
1315  pfreal[0] = pf[0] * pfmat[0] + pf[1] * pfmat[1] + pf[2] * pfmat[2];
1316  pfreal[1] = pf[0] * pfmat[3] + pf[1] * pfmat[4] + pf[2] * pfmat[5];
1317  pfreal[2] = pf[0] * pfmat[6] + pf[1] * pfmat[7] + pf[2] * pfmat[8];
1318 
1319  if( pfout ==pf ) {
1320  pfout[0] = pfreal[0];
1321  pfout[1] = pfreal[1];
1322  pfout[2] = pfreal[2];
1323  }
1324 
1325  return pfout;
1326 }
1327 
1328 template <class T>
1329 inline T* transnorm3(T* pfout, const RaveTransformMatrix<T>* pmat, const T* pf)
1330 {
1331  assert( pfout != NULL && pf != NULL && pmat != NULL );
1332 
1333  T dummy[3];
1334  T* pfreal = (pfout == pf) ? dummy : pfout;
1335 
1336  pfreal[0] = pf[0] * pmat->m[0] + pf[1] * pmat->m[1] + pf[2] * pmat->m[2];
1337  pfreal[1] = pf[0] * pmat->m[4] + pf[1] * pmat->m[5] + pf[2] * pmat->m[6];
1338  pfreal[2] = pf[0] * pmat->m[8] + pf[1] * pmat->m[9] + pf[2] * pmat->m[10];
1339 
1340  if( pfreal != pfout ) {
1341  pfout[0] = pfreal[0];
1342  pfout[1] = pfreal[1];
1343  pfout[2] = pfreal[2];
1344  }
1345 
1346  return pfout;
1347 }
1348 
1349 inline float* mult4(float* pfres, const float* pf1, const float* pf2) { return _mult4<float>(pfres, pf1, pf2); }
1350 // pf1^T * pf2
1351 inline float* multtrans3(float* pfres, const float* pf1, const float* pf2) { return _multtrans3<float>(pfres, pf1, pf2); }
1352 inline float* multtrans4(float* pfres, const float* pf1, const float* pf2) { return _multtrans4<float>(pfres, pf1, pf2); }
1353 inline float* transnorm3(float* pfout, const float* pfmat, const float* pf) { return _transnorm3<float>(pfout, pfmat, pf); }
1354 
1355 inline float* transpose3(const float* pf, float* pfres) { return _transpose3<float>(pf, pfres); }
1356 inline float* transpose4(const float* pf, float* pfres) { return _transpose4<float>(pf, pfres); }
1357 
1358 inline float dot2(const float* pf1, const float* pf2) { return _dot2<float>(pf1, pf2); }
1359 inline float dot3(const float* pf1, const float* pf2) { return _dot3<float>(pf1, pf2); }
1360 inline float dot4(const float* pf1, const float* pf2) { return _dot4<float>(pf1, pf2); }
1361 
1362 inline float lengthsqr2(const float* pf) { return _lengthsqr2<float>(pf); }
1363 inline float lengthsqr3(const float* pf) { return _lengthsqr3<float>(pf); }
1364 inline float lengthsqr4(const float* pf) { return _lengthsqr4<float>(pf); }
1365 
1366 inline float* normalize2(float* pfout, const float* pf) { return _normalize2<float>(pfout, pf); }
1367 inline float* normalize3(float* pfout, const float* pf) { return _normalize3<float>(pfout, pf); }
1368 inline float* normalize4(float* pfout, const float* pf) { return _normalize4<float>(pfout, pf); }
1369 
1370 inline float* cross3(float* pfout, const float* pf1, const float* pf2) { return _cross3<float>(pfout, pf1, pf2); }
1371 
1372 // multiplies 3x3 matrices
1373 inline float* mult3_s4(float* pfres, const float* pf1, const float* pf2) { return _mult3_s4<float>(pfres, pf1, pf2); }
1374 inline float* mult3_s3(float* pfres, const float* pf1, const float* pf2) { return _mult3_s3<float>(pfres, pf1, pf2); }
1375 
1376 inline float* inv3(const float* pf, float* pfres, float* pfdet, int stride) { return _inv3<float>(pf, pfres, pfdet, stride); }
1377 inline float* inv4(const float* pf, float* pfres) { return _inv4<float>(pf, pfres); }
1378 
1379 
1380 inline double* mult4(double* pfres, const double* pf1, const double* pf2) { return _mult4<double>(pfres, pf1, pf2); }
1381 // pf1^T * pf2
1382 inline double* multtrans3(double* pfres, const double* pf1, const double* pf2) { return _multtrans3<double>(pfres, pf1, pf2); }
1383 inline double* multtrans4(double* pfres, const double* pf1, const double* pf2) { return _multtrans4<double>(pfres, pf1, pf2); }
1384 inline double* transnorm3(double* pfout, const double* pfmat, const double* pf) { return _transnorm3<double>(pfout, pfmat, pf); }
1385 
1386 inline double* transpose3(const double* pf, double* pfres) { return _transpose3<double>(pf, pfres); }
1387 inline double* transpose4(const double* pf, double* pfres) { return _transpose4<double>(pf, pfres); }
1388 
1389 inline double dot2(const double* pf1, const double* pf2) { return _dot2<double>(pf1, pf2); }
1390 inline double dot3(const double* pf1, const double* pf2) { return _dot3<double>(pf1, pf2); }
1391 inline double dot4(const double* pf1, const double* pf2) { return _dot4<double>(pf1, pf2); }
1392 
1393 inline double lengthsqr2(const double* pf) { return _lengthsqr2<double>(pf); }
1394 inline double lengthsqr3(const double* pf) { return _lengthsqr3<double>(pf); }
1395 inline double lengthsqr4(const double* pf) { return _lengthsqr4<double>(pf); }
1396 
1397 inline double* normalize2(double* pfout, const double* pf) { return _normalize2<double>(pfout, pf); }
1398 inline double* normalize3(double* pfout, const double* pf) { return _normalize3<double>(pfout, pf); }
1399 inline double* normalize4(double* pfout, const double* pf) { return _normalize4<double>(pfout, pf); }
1400 
1401 inline double* cross3(double* pfout, const double* pf1, const double* pf2) { return _cross3<double>(pfout, pf1, pf2); }
1402 
1403 // multiplies 3x3 matrices
1404 inline double* mult3_s4(double* pfres, const double* pf1, const double* pf2) { return _mult3_s4<double>(pfres, pf1, pf2); }
1405 inline double* mult3_s3(double* pfres, const double* pf1, const double* pf2) { return _mult3_s3<double>(pfres, pf1, pf2); }
1406 
1407 inline double* inv3(const double* pf, double* pfres, double* pfdet, int stride) { return _inv3<double>(pf, pfres, pfdet, stride); }
1408 inline double* inv4(const double* pf, double* pfres) { return _inv4<double>(pf, pfres); }
1409 
1410 // if the two triangles collide, returns true and fills contactpos with the intersection point
1411 // assuming triangles are declared counter-clockwise!!
1412 // contactnorm is the normal of the second triangle
1413 bool TriTriCollision(const RaveVector<float>& u1, const RaveVector<float>& u2, const RaveVector<float>& u3,
1414  const RaveVector<float>& v1, const RaveVector<float>& v2, const RaveVector<float>& v3,
1415  RaveVector<float>& contactpos, RaveVector<float>& contactnorm);
1416 bool TriTriCollision(const RaveVector<double>& u1, const RaveVector<double>& u2, const RaveVector<double>& u3,
1417  const RaveVector<double>& v1, const RaveVector<double>& v2, const RaveVector<double>& v3,
1418  RaveVector<double>& contactpos, RaveVector<double>& contactnorm);
1419 
1420 template <class T> inline void mult(T* pf, T fa, int r)
1421 {
1422  assert( pf != NULL );
1423 
1424  while(r > 0) {
1425  --r;
1426  pf[r] *= fa;
1427  }
1428 }
1429 
1430 template <class T, class R, class S>
1431 inline S* mult(T* pf1, R* pf2, int r1, int c1, int c2, S* pfres, bool badd)
1432 {
1433  assert( pf1 != NULL && pf2 != NULL && pfres != NULL);
1434  int j, k;
1435 
1436  if( !badd ) memset(pfres, 0, sizeof(S) * r1 * c2);
1437 
1438  while(r1 > 0) {
1439  --r1;
1440 
1441  j = 0;
1442  while(j < c2) {
1443  k = 0;
1444  while(k < c1) {
1445  pfres[j] += (S)(pf1[k] * pf2[k*c2 + j]);
1446  ++k;
1447  }
1448  ++j;
1449  }
1450 
1451  pf1 += c1;
1452  pfres += c2;
1453  }
1454 
1455  return pfres;
1456 }
1457 
1458 template <class T, class R, class S>
1459 inline S* multtrans(T* pf1, R* pf2, int r1, int c1, int c2, S* pfres, bool badd)
1460 {
1461  assert( pf1 != NULL && pf2 != NULL && pfres != NULL);
1462  int i, j, k;
1463 
1464  if( !badd ) memset(pfres, 0, sizeof(S) * c1 * c2);
1465 
1466  i = 0;
1467  while(i < c1) {
1468 
1469  j = 0;
1470  while(j < c2) {
1471 
1472  k = 0;
1473  while(k < r1) {
1474  pfres[j] += (S)(pf1[k*c1] * pf2[k*c2 + j]);
1475  ++k;
1476  }
1477  ++j;
1478  }
1479 
1480  pfres += c2;
1481  ++pf1;
1482 
1483  ++i;
1484  }
1485 
1486  return pfres;
1487 }
1488 
1489 template <class T, class R, class S>
1490 inline S* multtrans_to2(T* pf1, R* pf2, int r1, int c1, int r2, S* pfres, bool badd)
1491 {
1492  assert( pf1 != NULL && pf2 != NULL && pfres != NULL);
1493  int j, k;
1494 
1495  if( !badd ) memset(pfres, 0, sizeof(S) * r1 * r2);
1496 
1497  while(r1 > 0) {
1498  --r1;
1499 
1500  j = 0;
1501  while(j < r2) {
1502  k = 0;
1503  while(k < c1) {
1504  pfres[j] += (S)(pf1[k] * pf2[j*c1 + k]);
1505  ++k;
1506  }
1507  ++j;
1508  }
1509 
1510  pf1 += c1;
1511  pfres += r2;
1512  }
1513 
1514  return pfres;
1515 }
1516 
1517 template <class T> inline T* multto1(T* pf1, T* pf2, int r, int c, T* pftemp)
1518 {
1519  assert( pf1 != NULL && pf2 != NULL );
1520 
1521  int j, k;
1522  bool bdel = false;
1523 
1524  if( pftemp == NULL ) {
1525  pftemp = new T[c];
1526  bdel = true;
1527  }
1528 
1529  while(r > 0) {
1530  --r;
1531 
1532  j = 0;
1533  while(j < c) {
1534 
1535  pftemp[j] = 0.0;
1536 
1537  k = 0;
1538  while(k < c) {
1539  pftemp[j] += pf1[k] * pf2[k*c + j];
1540  ++k;
1541  }
1542  ++j;
1543  }
1544 
1545  memcpy(pf1, pftemp, c * sizeof(T));
1546  pf1 += c;
1547  }
1548 
1549  if( bdel ) delete[] pftemp;
1550 
1551  return pf1;
1552 }
1553 
1554 template <class T, class S> inline T* multto2(T* pf1, S* pf2, int r2, int c2, S* pftemp)
1555 {
1556  assert( pf1 != NULL && pf2 != NULL );
1557 
1558  int i, j, k;
1559  bool bdel = false;
1560 
1561  if( pftemp == NULL ) {
1562  pftemp = new S[r2];
1563  bdel = true;
1564  }
1565 
1566  // do columns first
1567  j = 0;
1568  while(j < c2) {
1569  i = 0;
1570  while(i < r2) {
1571 
1572  pftemp[i] = 0.0;
1573 
1574  k = 0;
1575  while(k < r2) {
1576  pftemp[i] += pf1[i*r2 + k] * pf2[k*c2 + j];
1577  ++k;
1578  }
1579  ++i;
1580  }
1581 
1582  i = 0;
1583  while(i < r2) {
1584  *(pf2+i*c2+j) = pftemp[i];
1585  ++i;
1586  }
1587 
1588  ++j;
1589  }
1590 
1591  if( bdel ) delete[] pftemp;
1592 
1593  return pf1;
1594 }
1595 
1596 template <class T> inline void add(T* pf1, T* pf2, int r)
1597 {
1598  assert( pf1 != NULL && pf2 != NULL);
1599 
1600  while(r > 0) {
1601  --r;
1602  pf1[r] += pf2[r];
1603  }
1604 }
1605 
1606 template <class T> inline void sub(T* pf1, T* pf2, int r)
1607 {
1608  assert( pf1 != NULL && pf2 != NULL);
1609 
1610  while(r > 0) {
1611  --r;
1612  pf1[r] -= pf2[r];
1613  }
1614 }
1615 
1616 template <class T> inline T normsqr(const T* pf1, int r)
1617 {
1618  assert( pf1 != NULL );
1619 
1620  T d = 0.0;
1621  while(r > 0) {
1622  --r;
1623  d += pf1[r] * pf1[r];
1624  }
1625 
1626  return d;
1627 }
1628 
1629 template <class T> inline T lengthsqr(const T* pf1, const T* pf2, int length)
1630 {
1631  T d = 0;
1632  while(length > 0) {
1633  --length;
1634  T t = pf1[length] - pf2[length];
1635  d += t * t;
1636  }
1637 
1638  return d;
1639 }
1640 
1641 template <class T> inline T dot(T* pf1, T* pf2, int length)
1642 {
1643  T d = 0;
1644  while(length > 0) {
1645  --length;
1646  d += pf1[length] * pf2[length];
1647  }
1648 
1649  return d;
1650 }
1651 
1652 template <class T> inline T sum(T* pf, int length)
1653 {
1654  T d = 0;
1655  while(length > 0) {
1656  --length;
1657  d += pf[length];
1658  }
1659 
1660  return d;
1661 }
1662 
1663 template <class T> inline bool inv2(T* pf, T* pfres)
1664 {
1665  T fdet = pf[0] * pf[3] - pf[1] * pf[2];
1666 
1667  if( fabs(fdet) < 1e-16 ) return false;
1668 
1669  fdet = 1 / fdet;
1670  //if( pfdet != NULL ) *pfdet = fdet;
1671 
1672  if( pfres != pf ) {
1673  pfres[0] = fdet * pf[3]; pfres[1] = -fdet * pf[1];
1674  pfres[2] = -fdet * pf[2]; pfres[3] = fdet * pf[0];
1675  return true;
1676  }
1677 
1678  T ftemp = pf[0];
1679  pfres[0] = pf[3] * fdet;
1680  pfres[1] *= -fdet;
1681  pfres[2] *= -fdet;
1682  pfres[3] = ftemp * pf[0];
1683 
1684  return true;
1685 }
1686 
1687 template <class T, class S>
1688 void Tridiagonal3 (S* mat, T* diag, T* subd)
1689 {
1690  T a, b, c, d, e, f, ell, q;
1691 
1692  a = mat[0*3+0];
1693  b = mat[0*3+1];
1694  c = mat[0*3+2];
1695  d = mat[1*3+1];
1696  e = mat[1*3+2];
1697  f = mat[2*3+2];
1698 
1699  subd[2] = 0.0;
1700  diag[0] = a;
1701  if ( fabs(c) >= g_fEpsilon ) {
1702  ell = (T)RaveSqrt(b*b+c*c);
1703  b /= ell;
1704  c /= ell;
1705  q = 2*b*e+c*(f-d);
1706  diag[1] = d+c*q;
1707  diag[2] = f-c*q;
1708  subd[0] = ell;
1709  subd[1] = e-b*q;
1710  mat[0*3+0] = (S)1; mat[0*3+1] = (S)0; mat[0*3+2] = (T)0;
1711  mat[1*3+0] = (S)0; mat[1*3+1] = b; mat[1*3+2] = c;
1712  mat[2*3+0] = (S)0; mat[2*3+1] = c; mat[2*3+2] = -b;
1713  }
1714  else {
1715  diag[1] = d;
1716  diag[2] = f;
1717  subd[0] = b;
1718  subd[1] = e;
1719  mat[0*3+0] = (S)1; mat[0*3+1] = (S)0; mat[0*3+2] = (S)0;
1720  mat[1*3+0] = (S)0; mat[1*3+1] = (S)1; mat[1*3+2] = (S)0;
1721  mat[2*3+0] = (S)0; mat[2*3+1] = (S)0; mat[2*3+2] = (S)1;
1722  }
1723 }
1724 
1725 template <class T>
1726 int Min(T* pts, int stride, int numPts)
1727 {
1728  assert( pts != NULL && numPts > 0 && stride > 0 );
1729 
1730  int best = pts[0]; pts += stride;
1731  for(int i = 1; i < numPts; ++i, pts += stride) {
1732  if( best > pts[0] )
1733  best = pts[0];
1734  }
1735 
1736  return best;
1737 }
1738 
1739 template <class T>
1740 int Max(T* pts, int stride, int numPts)
1741 {
1742  assert( pts != NULL && numPts > 0 && stride > 0 );
1743 
1744  int best = pts[0]; pts += stride;
1745  for(int i = 1; i < numPts; ++i, pts += stride) {
1746  if( best < pts[0] )
1747  best = pts[0];
1748  }
1749 
1750  return best;
1751 }
1752 
1753 // Don't add new lines to the output << operators. Some applications use it to serialize the data
1754 // to send across the network.
1755 
1756 template <class T, class U>
1757 std::basic_ostream<T>& operator<<(std::basic_ostream<T>& O, const RaveVector<U>& v)
1758 {
1759  return O << v.x << " " << v.y << " " << v.z << " " << v.w << " ";
1760 }
1761 
1762 template <class T, class U>
1763 std::basic_istream<T>& operator>>(std::basic_istream<T>& I, RaveVector<U>& v)
1764 {
1765  return I >> v.x >> v.y >> v.z >> v.w;
1766 }
1767 
1768 template <class T, class U>
1769 std::basic_ostream<T>& operator<<(std::basic_ostream<T>& O, const RaveTransform<U>& v)
1770 {
1771  return O << v.rot.x << " " << v.rot.y << " " << v.rot.z << " " << v.rot.w << " "
1772  << v.trans.x << " " << v.trans.y << " " << v.trans.z << " ";
1773 }
1774 
1775 template <class T, class U>
1776 std::basic_istream<T>& operator>>(std::basic_istream<T>& I, RaveTransform<U>& v)
1777 {
1778  return I >> v.rot.x >> v.rot.y >> v.rot.z >> v.rot.w >> v.trans.x >> v.trans.y >> v.trans.z;
1779 }
1780 
1781 // serial in column order! This is the format transformations are passed across the network
1782 template <class T, class U>
1783 std::basic_ostream<T>& operator<<(std::basic_ostream<T>& O, const RaveTransformMatrix<U>& v)
1784 {
1785  return O << v.m[0] << " " << v.m[4] << " " << v.m[8] << " "
1786  << v.m[1] << " " << v.m[5] << " " << v.m[9] << " "
1787  << v.m[2] << " " << v.m[6] << " " << v.m[10] << " "
1788  << v.trans.x << " " << v.trans.y << " " << v.trans.z << " ";
1789 }
1790 
1791 // read in column order! This is the format transformations are passed across the network
1792 template <class T, class U>
1793 std::basic_istream<T>& operator>>(std::basic_istream<T>& I, RaveTransformMatrix<U>& v)
1794 {
1795  return I >> v.m[0] >> v.m[4] >> v.m[8]
1796  >> v.m[1] >> v.m[5] >> v.m[9]
1797  >> v.m[2] >> v.m[6] >> v.m[10]
1798  >> v.trans.x >> v.trans.y >> v.trans.z;
1799 }
1800 
1801 #endif
_mult3_s4
T * _mult3_s4(T *pfres, const T *pf1, const T *pf2)
mult3 with a 3x3 matrix whose row stride is 16 bytes
Definition: math.h:834
multtrans_to2
S * multtrans_to2(T *pf1, R *pf2, int r1, int c1, int r2, S *pfres, bool badd=false)
Definition: math.h:1490
TRIANGLE::~TRIANGLE
~TRIANGLE()
Definition: math.h:654
RaveVector::operator[]
T & operator[](int i)
Definition: math.h:257
_multtrans3
T * _multtrans3(T *pfres, const T *pf1, const T *pf2)
Definition: math.h:901
RaveVector::normalize
RaveVector< T > & normalize()
Definition: math.h:268
_normalize4
T * _normalize4(T *pfout, const T *pf)
Definition: math.h:1257
dSqrt
#define dSqrt(x)
Definition: math.h:39
RaveFabs
float RaveFabs(float f)
Definition: math.h:234
normsqr
T normsqr(const T *pf1, int r)
Definition: math.h:1616
TransformMatrix
RaveTransformMatrix< dReal > TransformMatrix
Definition: math.h:569
sum
T sum(T *pf, int length)
Definition: math.h:1652
RaveVector::operator^
RaveVector< T > operator^(const RaveVector< U > &v) const
cross product operator
Definition: math.h:327
lengthsqr4
float lengthsqr4(const float *pf)
Definition: math.h:1364
RaveVector::operator[]
T operator[](int i) const
Definition: math.h:256
CubicRoots
int CubicRoots(double c0, double c1, double c2, double *r0, double *r1, double *r2)
RaveTransformMatrix::operator<<
friend std::basic_ostream< S > & operator<<(std::basic_ostream< S > &O, const RaveTransformMatrix< U > &v)
lengthsqr
T lengthsqr(const T *pf1, const T *pf2, int length)
Definition: math.h:1629
operator<<
std::basic_ostream< T > & operator<<(std::basic_ostream< T > &O, const RaveVector< U > &v)
Definition: math.h:1757
MULT3
#define MULT3(stride)
Definition: math.h:820
RaveTransformMatrix
affine transformation parameterized with rotation matrices
Definition: math.h:161
angle
TFSIMD_FORCE_INLINE tfScalar angle(const Quaternion &q1, const Quaternion &q2)
RaveVector::operator+
RaveVector< T > operator+(const RaveVector< U > &r) const
Definition: math.h:308
mult3_s3
float * mult3_s3(float *pfres, const float *pf1, const float *pf2)
Definition: math.h:1374
OBB
Definition: math.h:645
RaveVector::RaveVector
RaveVector(T x, T y, T z)
Definition: math.h:249
RaveTransform::operator>>
friend std::basic_istream< S > & operator>>(std::basic_istream< S > &I, RaveTransform< U > &v)
matrixdet3
T matrixdet3(const T *pf, int stride)
calculates the determinant of a 3x3 matrix whose row stride stride elements
Definition: math.h:1006
_mult3_s3
T * _mult3_s3(T *pfres, const T *pf1, const T *pf2)
mult3 with a 3x3 matrix whose row stride is 12 bytes
Definition: math.h:851
s
XmlRpcServer s
RaveTransformMatrix::RaveTransformMatrix
RaveTransformMatrix(const RaveTransformMatrix< U > &t)
Definition: math.h:452
dot2
float dot2(const float *pf1, const float *pf2)
Definition: math.h:1358
_R
#define _R(i, j)
Definition: math.h:57
RaveTransform
affine transformation parameterized with quaterions
Definition: math.h:160
GetRandomQuat
Vector GetRandomQuat(void)
Definition: math.h:941
multtrans
S * multtrans(T *pf1, R *pf2, int r1, int c1, int c2, S *pfres, bool badd=false)
Definition: math.h:1459
_inv4
T * _inv4(const T *pf, T *pfres)
Definition: math.h:1069
dMatrix4
dReal dMatrix4[4 *4]
Definition: math.h:53
RaveTransform::RaveTransform
RaveTransform()
Definition: math.h:365
_multtrans4
T * _multtrans4(T *pfres, const T *pf1, const T *pf2)
Definition: math.h:925
transpose4
float * transpose4(const float *pf, float *pfres)
Definition: math.h:1356
dot3
float dot3(const float *pf1, const float *pf2)
Definition: math.h:1359
RAY::dir
Vector dir
Definition: math.h:635
RaveTransform::operator<<
friend std::basic_ostream< S > & operator<<(std::basic_ostream< S > &O, const RaveTransform< U > &v)
dQSlerp
RaveVector< T > dQSlerp(const RaveVector< T > &qa, const RaveVector< T > &qb, T t)
Definition: math.h:970
AABB::extents
Vector extents
Definition: math.h:642
RaveVector::lengthsqr2
T lengthsqr2() const
Definition: math.h:285
mult4
float * mult4(float *pfres, const float *pf1, const float *pf2)
Definition: math.h:1349
RaveTransform::identity
void identity()
Definition: math.h:373
RaveVector::Cross
RaveVector< T > & Cross(const RaveVector< T > &v)
Definition: math.h:295
operator>>
std::basic_istream< T > & operator>>(std::basic_istream< T > &I, RaveVector< U > &v)
Definition: math.h:1763
normalize4
float * normalize4(float *pfout, const float *pf)
Definition: math.h:1368
TRIANGLE::v1
Vector v1
Definition: math.h:656
solvequad
int solvequad(T a, T b, T c, T &r1, T &r2)
Definition: math.h:800
RaveTransformMatrix::RaveTransformMatrix
RaveTransformMatrix()
Definition: math.h:451
RaveVector::x
T x
Definition: math.h:245
OBB::right
Vector right
Definition: math.h:647
RaveTransformMatrix::operator*=
RaveTransformMatrix< T > operator*=(const RaveTransformMatrix< T > &r) const
Definition: math.h:533
RaveVector
Definition: math.h:242
_normalize2
T * _normalize2(T *pfout, const T *pf)
Definition: math.h:1229
RaveVector::normalize4
RaveVector< T > & normalize4()
Definition: math.h:270
dRecip
#define dRecip(x)
Definition: math.h:40
RaveVector::operator-=
RaveVector< T > & operator-=(const RaveVector< U > &r)
Definition: math.h:314
RaveVector::Cross
RaveVector< T > & Cross(const RaveVector< T > &u, const RaveVector< T > &v)
this = u x v
Definition: math.h:298
inv4
float * inv4(const float *pf, float *pfres)
Definition: math.h:1377
Transform
RaveTransform< dReal > Transform
Definition: math.h:444
RAY::pos
Vector pos
Definition: math.h:635
TRIANGLE::v3
Vector v3
the vertices of the triangle
Definition: math.h:656
RaveVector::y
T y
Definition: math.h:245
RaveTransformMatrix::rotfromquat
void rotfromquat(const RaveVector< U > &quat)
Definition: math.h:480
mult3_s4
float * mult3_s4(float *pfres, const float *pf1, const float *pf2)
Definition: math.h:1373
transcoord3
dReal * transcoord3(dReal *pfout, const TransformMatrix *pmat, const dReal *pf)
computes (*pmat) * v
_dot3
T _dot3(const T *pf1, const T *pf2)
Definition: math.h:1194
EigenSymmetric3
void EigenSymmetric3(dReal *fCovariance, dReal *eval, dReal *fAxes)
RAY::RAY
RAY(const Vector &_pos, const Vector &_dir)
Definition: math.h:634
multto1
T * multto1(T *pf1, T *pf2, int r1, int c1, T *pftemp=NULL)
Definition: math.h:1517
RaveVector::Set3
void Set3(const T *pvals)
Definition: math.h:289
RaveVector::operator>>
friend std::basic_istream< S > & operator>>(std::basic_istream< S > &I, RaveVector< U > &v)
RaveVector::operator*=
RaveVector< T > & operator*=(const RaveVector< U > &r)
Definition: math.h:315
_mult4
T * _mult4(T *pfres, const T *p1, const T *p2)
Definition: math.h:868
RaveTransformMatrix::Extract
void Extract(RaveVector< U > &right, RaveVector< U > &up, RaveVector< U > &dir, RaveVector< U > &pos) const
Definition: math.h:554
f
f
_dot2
T _dot2(const T *pf1, const T *pf2)
Definition: math.h:1187
operator*
RaveVector< T > operator*(float f, const RaveVector< T > &left)
Definition: math.h:339
dVector3
dReal dVector3[4]
Definition: math.h:50
RaveCos
float RaveCos(float f)
Definition: math.h:232
TRIANGLE::TRIANGLE
TRIANGLE()
Definition: math.h:652
mult
void mult(T *pf, T fa, int r)
Definition: math.h:1420
multtrans3
float * multtrans3(float *pfres, const float *pf1, const float *pf2)
Definition: math.h:1351
RaveSin
float RaveSin(float f)
Definition: math.h:230
_transpose4
T * _transpose4(const T *pf, T *pfres)
Definition: math.h:1165
_inv3
T * _inv3(const T *pf, T *pfres, T *pfdet, int stride)
Definition: math.h:1016
RaveVector::RaveVector
RaveVector(const RaveVector< U > &vec)
Definition: math.h:251
RaveTransformMatrix::rot
T rot(int i, int j) const
Definition: math.h:505
add
void add(T *pf1, T *pf2, int r)
Definition: math.h:1596
RaveSqrt
float RaveSqrt(float f)
Definition: math.h:228
normalize3
float * normalize3(float *pfout, const float *pf)
Definition: math.h:1367
RaveTransform::rot
RaveVector< T > rot
Definition: math.h:441
Tridiagonal3
void Tridiagonal3(S *mat, T *diag, T *subd)
Definition: math.h:1688
_lengthsqr2
T _lengthsqr2(const T *pf)
Definition: math.h:1208
OBB::up
Vector up
Definition: math.h:647
dMatrix6
dReal dMatrix6[8 *6]
Definition: math.h:54
_lengthsqr3
T _lengthsqr3(const T *pf)
Definition: math.h:1215
RaveVector::operator*
RaveVector< T > operator*(T k) const
Definition: math.h:311
insideTriangle
int insideTriangle(const Vector *p0, const Vector *p1, const Vector *p2)
transpose3
float * transpose3(const float *pf, float *pfres)
Definition: math.h:1355
RaveVector::Set4
void Set4(const T *pvals)
Definition: math.h:291
AABB::AABB
AABB(const Vector &vpos, const Vector &vextents)
Definition: math.h:641
RaveVector::lengthsqr4
T lengthsqr4() const
Definition: math.h:287
Vector
RaveVector< dReal > Vector
Definition: math.h:336
rswap
#define rswap(x, y)
Definition: math.h:149
_lengthsqr4
T _lengthsqr4(const T *pf)
Definition: math.h:1222
RaveTransformMatrix::rotfromaxisangle
void rotfromaxisangle(const RaveVector< U > &axis, U angle)
Definition: math.h:469
RAD_2_DEG
T RAD_2_DEG(T radians)
Definition: math.h:158
RaveTransformMatrix::operator*
RaveVector< T > operator*(const RaveVector< U > &r) const
Definition: math.h:515
RaveVector::operator+=
RaveVector< T > & operator+=(const RaveVector< U > &r)
Definition: math.h:313
AABB::AABB
AABB()
Definition: math.h:640
sub
void sub(T *pf1, T *pf2, int r)
Definition: math.h:1606
OBB::pos
Vector pos
Definition: math.h:647
d
d
transnorm3
float * transnorm3(float *pfout, const float *pfmat, const float *pf)
Definition: math.h:1353
insideQuadrilateral
int insideQuadrilateral(const Vector *p0, const Vector *p1, const Vector *p2, const Vector *p3)
RaveTransformMatrix::inverse
RaveTransformMatrix< T > inverse() const
Definition: math.h:546
RaveVector::operator<<
friend std::basic_ostream< S > & operator<<(std::basic_ostream< S > &O, const RaveVector< U > &v)
RaveTransformMatrix::trans
RaveVector< T > trans
translation component
Definition: math.h:567
TRIANGLE
Definition: math.h:650
dQMultiply0
void dQMultiply0(dQuaternion qa, const dQuaternion qb, const dQuaternion qc)
Definition: math.h:60
RaveTransformMatrix::m
T m[12]
Definition: math.h:566
AxisAngle2Quat
RaveVector< T > AxisAngle2Quat(const RaveVector< T > &rotaxis, T angle)
Definition: math.h:962
svd3
void svd3(const dReal *A, dReal *U, dReal *D, dReal *V)
RaveTransform::rotfromaxisangle
void rotfromaxisangle(const RaveVector< U > &axis, U angle)
Definition: math.h:379
dQfromR
void dQfromR(dQuaternion q, const dMatrix3 R)
Definition: math.h:90
_cross3
T * _cross3(T *pfout, const T *pf1, const T *pf2)
Definition: math.h:1273
RAY
Definition: math.h:631
RaveVector::operator*
RaveVector< T > operator*(const RaveVector< U > &r) const
Definition: math.h:310
AABB::pos
Vector pos
Definition: math.h:642
RaveTransformMatrix::operator>>
friend std::basic_istream< S > & operator>>(std::basic_istream< S > &I, RaveTransformMatrix< U > &v)
multto2
T * multto2(T *pf1, S *pf2, int r2, int c2, S *pftemp=NULL)
Definition: math.h:1554
RaveVector::RaveVector
RaveVector()
Definition: math.h:247
RaveVector::normalize3
RaveVector< T > & normalize3()
Definition: math.h:277
dQuaternion
dReal dQuaternion[4]
Definition: math.h:55
AABB
Definition: math.h:638
RayOBBTest
bool RayOBBTest(const RAY &r, const OBB &obb)
dot
T dot(T *pf1, T *pf2, int length)
Definition: math.h:1641
Min
int Min(T *pts, int stride, int numPts)
Definition: math.h:1726
dRfromQ
void dRfromQ(dMatrix3 R, const dQuaternion q)
Definition: math.h:69
RaveTransform::inverse
RaveTransform< T > inverse() const
Definition: math.h:427
RaveVector::Set3
void Set3(T val1, T val2, T val3)
Definition: math.h:290
_dot4
T _dot4(const T *pf1, const T *pf2)
Definition: math.h:1201
_normalize3
T * _normalize3(T *pfout, const T *pf)
Definition: math.h:1242
eig2
bool eig2(const dReal *pfmat, dReal *peigs, dReal &fv1x, dReal &fv1y, dReal &fv2x, dReal &fv2y)
Definition: math.h:763
_transpose3
T * _transpose3(const T *pf, T *pfres)
Definition: math.h:1146
TRIANGLE::operator[]
const Vector & operator[](int i) const
Definition: math.h:658
dVector4
dReal dVector4[4]
Definition: math.h:51
RaveVector::lengthsqr3
T lengthsqr3() const
Definition: math.h:286
multtrans4
float * multtrans4(float *pfres, const float *pf1, const float *pf2)
Definition: math.h:1352
RaveTransformMatrix::rot
T & rot(int i, int j)
Definition: math.h:509
RaveVector::operator-
RaveVector< T > operator-(const RaveVector< U > &r) const
Definition: math.h:309
TRIANGLE::ComputeNormal
Vector ComputeNormal()
assumes CCW ordering of vertices
Definition: math.h:662
length
TFSIMD_FORCE_INLINE tfScalar length(const Quaternion &q)
RaveVector::Set4
void Set4(T val1, T val2, T val3, T val4)
Definition: math.h:292
GetCovarBasisVectors
void GetCovarBasisVectors(dReal fCovariance[3][3], Vector *vRight, Vector *vUp, Vector *vDir)
RaveTransform::operator*
RaveVector< T > operator*(const RaveVector< T > &r) const
transform a 3 dim vector
Definition: math.h:388
inv3
float * inv3(const float *pf, float *pfres, float *pfdet, int stride)
Definition: math.h:1376
RaveTransform::operator*=
RaveTransform< T > & operator*=(const RaveTransform< T > &right)
Definition: math.h:422
RaveVector::operator-
RaveVector< T > operator-() const
Definition: math.h:307
OBB::extents
Vector extents
Definition: math.h:647
RAY::RAY
RAY()
Definition: math.h:633
RaveVector::dot
T dot(const RaveVector< U > &v) const
Definition: math.h:267
lengthsqr3
float lengthsqr3(const float *pf)
Definition: math.h:1363
normalize2
float * normalize2(float *pfout, const float *pf)
Definition: math.h:1366
RaveAcos
float RaveAcos(float f)
Definition: math.h:236
dot4
float dot4(const float *pf1, const float *pf2)
Definition: math.h:1360
cross3
float * cross3(float *pfout, const float *pf1, const float *pf2)
Definition: math.h:1370
OBB::dir
Vector dir
Definition: math.h:647
TRIANGLE::v2
Vector v2
Definition: math.h:656
g_fEpsilon
#define g_fEpsilon
Definition: math.h:151
Max
int Max(T *pts, int stride, int numPts)
Definition: math.h:1740
TriTriCollision
bool TriTriCollision(const RaveVector< float > &u1, const RaveVector< float > &u2, const RaveVector< float > &u3, const RaveVector< float > &v1, const RaveVector< float > &v2, const RaveVector< float > &v3, RaveVector< float > &contactpos, RaveVector< float > &contactnorm)
RaveTransformMatrix::rotfrommat
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)
Definition: math.h:499
RaveVector::RaveVector
RaveVector(const U *pf)
note, it only copes 3 values!
Definition: math.h:254
RaveVector::z
T z
Definition: math.h:245
lengthsqr2
float lengthsqr2(const float *pf)
Definition: math.h:1362
TRIANGLE::operator[]
Vector & operator[](int i)
Definition: math.h:659
RaveVector::operator=
RaveVector< T > & operator=(const RaveVector< U > &r)
Definition: math.h:264
TRIANGLE::TRIANGLE
TRIANGLE(const Vector &v1, const Vector &v2, const Vector &v3)
Definition: math.h:653
QuadraticSolver
void QuadraticSolver(dReal *pfQuadratic, dReal *pfRoots)
DistVertexOBBSq
dReal DistVertexOBBSq(const Vector &v, const OBB &o)
RaveTransform::RaveTransform
RaveTransform(const RaveVector< U > &rot, const RaveVector< U > &trans)
Definition: math.h:370
RaveTransformMatrix::rotate
RaveVector< U > rotate(const RaveVector< U > &r) const
Definition: math.h:539
QLAlgorithm3
bool QLAlgorithm3(float *m_aafEntry, float *afDiag, float *afSubDiag)
RaveVector::operator/=
RaveVector< T > & operator/=(const T _k)
Definition: math.h:318
RaveTransform::rotate
RaveVector< T > rotate(const RaveVector< T > &r) const
Definition: math.h:392
_transnorm3
T * _transnorm3(T *pfout, const T *pfmat, const T *pf)
Definition: math.h:1308
RaveVector::RaveVector
RaveVector(T x, T y, T z, T w)
Definition: math.h:250
RaveVector::w
T w
Definition: math.h:245
RaveTransform::trans
RaveVector< T > trans
rot is a quaternion=(cos(ang/2),axisx*sin(ang/2),axisy*sin(ang/2),axisz*sin(ang/2))
Definition: math.h:441
RaveTransformMatrix::identity
void identity()
Definition: math.h:461
dMatrix3
dReal dMatrix3[4 *3]
Definition: math.h:52
dReal
float dReal
Definition: math.h:38
RaveTransform::RaveTransform
RaveTransform(const RaveTransform< U > &t)
Definition: math.h:366
inv2
bool inv2(T *pf, T *pfres)
Definition: math.h:1663

checkerboard_detector
Author(s): Rosen Diankov (rdiankov@cs.cmu.edu)
autogenerated on Fri May 16 2025 03:10:48