openrave.org

 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
geometry.h
Go to the documentation of this file.
1 // -*- coding: utf-8 -*-
2 // Copyright (C) 2006-2012 Rosen Diankov <rosen.diankov@gmail.com>
3 //
4 // This file is part of OpenRAVE.
5 // OpenRAVE is free software: you can redistribute it and/or modify
6 // it under the terms of the GNU Lesser General Public License as published by
7 // the Free Software Foundation, either version 3 of the License, or
8 // at your option) any later version.
9 //
10 // This program is distributed in the hope that it will be useful,
11 // but WITHOUT ANY WARRANTY; without even the implied warranty of
12 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13 // GNU Lesser General Public License for more details.
14 //
15 // You should have received a copy of the GNU Lesser General Public License
16 // along with this program. If not, see <http://www.gnu.org/licenses/>.
22 #ifndef OPENRAVE_GEOMETRY_H
23 #define OPENRAVE_GEOMETRY_H
24 
25 #include <cmath>
26 #include <vector>
27 #include <string>
28 #include <limits>
29 #include <utility> // for std::pair
30 #include <cstdlib>
31 
32 #ifndef RAVE_DEPRECATED
33 #define RAVE_DEPRECATED
34 #endif
35 #ifdef BOOST_ASSERT
36 #define MATH_ASSERT BOOST_ASSERT
37 #else
38 #include <cassert>
39 #define MATH_ASSERT assert
40 #endif
41 
42 namespace OpenRAVE {
43 
45 namespace geometry {
46 
47 template <typename T> class RaveTransform;
48 template <typename T> class RaveTransformMatrix;
49 
50 #ifdef OPENRAVE_MATH_SQRT_FLOAT
51 inline float MATH_SQRT(float f) {
52  return OPENRAVE_MATH_SQRT_FLOAT(f);
53 }
54 #else
55 inline float MATH_SQRT(float f) {
56  return sqrtf(f);
57 }
58 #endif
59 #ifdef OPENRAVE_MATH_SQRT_DOUBLE
60 inline double MATH_SQRT(double f) {
61  return OPENRAVE_MATH_SQRT_DOUBLE(f);
62 }
63 #else
64 inline double MATH_SQRT(double f) {
65  return sqrt(f);
66 }
67 #endif
68 
69 #ifdef OPENRAVE_MATH_SIN_FLOAT
70 inline float MATH_SIN(float f) {
71  return OPENRAVE_MATH_SIN_FLOAT(f);
72 }
73 #else
74 inline float MATH_SIN(float f) {
75  return sinf(f);
76 }
77 #endif
78 
79 #ifdef OPENRAVE_MATH_SIN_DOUBLE
80 inline double MATH_SIN(double f) {
81  return OPENRAVE_MATH_SIN_DOUBLE(f);
82 }
83 #else
84 inline double MATH_SIN(double f) {
85  return sin(f);
86 }
87 #endif
88 
89 #ifdef OPENRAVE_MATH_COS_FLOAT
90 inline float MATH_COS(float f) {
91  return OPENRAVE_MATH_COS_FLOAT(f);
92 }
93 #else
94 inline float MATH_COS(float f) {
95  return cosf(f);
96 }
97 #endif
98 
99 #ifdef OPENRAVE_MATH_COS_DOUBLE
100 inline double MATH_COS(double f) {
101  return OPENRAVE_MATH_COS_DOUBLE(f);
102 }
103 #else
104 inline double MATH_COS(double f) {
105  return cos(f);
106 }
107 #endif
108 
109 #ifdef OPENRAVE_MATH_FABS_FLOAT
110 inline float MATH_FABS(float f) {
111  return OPENRAVE_MATH_FABS_FLOAT(f);
112 }
113 #else
114 inline float MATH_FABS(float f) {
115  return fabsf(f);
116 }
117 #endif
118 
119 #ifdef OPENRAVE_MATH_FABS_DOUBLE
120 inline double MATH_FABS(double f) {
121  return OPENRAVE_MATH_FABS_DOUBLE(f);
122 }
123 #else
124 inline double MATH_FABS(double f) {
125  return fabs(f);
126 }
127 #endif
128 
129 #ifdef OPENRAVE_MATH_ACOS_FLOAT
130 inline float MATH_ACOS(float f) {
131  return OPENRAVE_MATH_ACOS_FLOAT(f);
132 }
133 #else
134 inline float MATH_ACOS(float f) {
135  return acosf(f);
136 }
137 #endif
138 
139 #ifdef OPENRAVE_MATH_ACOS_DOUBLE
140 inline double MATH_ACOS(double f) {
141  return OPENRAVE_MATH_ACOS_DOUBLE(f);
142 }
143 #else
144 inline double MATH_ACOS(double f) {
145  return acos(f);
146 }
147 #endif
148 
149 #ifdef OPENRAVE_MATH_ASIN_FLOAT
150 inline float MATH_ASIN(float f) {
151  return OPENRAVE_MATH_ASIN_FLOAT(f);
152 }
153 #else
154 inline float MATH_ASIN(float f) {
155  return asinf(f);
156 }
157 #endif
158 
159 #ifdef OPENRAVE_MATH_ASIN_DOUBLE
160 inline double MATH_ASIN(double f) {
161  return OPENRAVE_MATH_ASIN_DOUBLE(f);
162 }
163 #else
164 inline double MATH_ASIN(double f) {
165  return asin(f);
166 }
167 #endif
168 
169 #ifdef OPENRAVE_MATH_ATAN2_FLOAT
170 inline float MATH_ATAN2(float fy, float fx) {
171  return OPENRAVE_MATH_ATAN2_FLOAT(fy,fx);
172 }
173 #else
174 inline float MATH_ATAN2(float fy, float fx) {
175  return atan2f(fy,fx);
176 }
177 #endif
178 
179 #ifdef OPENRAVE_MATH_ATAN2_DOUBLE
180 inline double MATH_ATAN2(double fy, double fx) {
181  return OPENRAVE_MATH_ATAN2_DOUBLE(fy,fx);
182 }
183 #else
184 inline double MATH_ATAN2(double fy, double fx) {
185  return atan2(fy,fx);
186 }
187 #endif
188 
194 template <typename T>
196 {
197 public:
198  T x, y, z, w;
199 
200  RaveVector() : x(0), y(0), z(0), w(0) {
201  }
202 
203  RaveVector(T x, T y, T z) : x(x), y(y), z(z), w(0) {
204  }
205  RaveVector(T x, T y, T z, T w) : x(x), y(y), z(z), w(w) {
206  }
207  template<typename U> RaveVector(const RaveVector<U> &vec) : x((T)vec.x), y((T)vec.y), z((T)vec.z), w((T)vec.w) {
208  }
209 
211  template<typename U> RaveVector(const U* pf) {
212  MATH_ASSERT(pf != NULL); x = (T)pf[0]; y = (T)pf[1]; z = (T)pf[2]; w = 0;
213  }
214 
215  T operator[] (int i) const {
216  return (&x)[i];
217  }
218  T& operator[] (int i) {
219  return (&x)[i];
220  }
221 
222  template <typename U>
224  x = (T)r.x; y = (T)r.y; z = (T)r.z; w = (T)r.w; return *this;
225  }
226 
227  // SCALAR FUNCTIONS
228  template <typename U> inline T dot(const RaveVector<U> &v) const {
229  return x*v.x + y*v.y + z*v.z + w*v.w;
230  }
231  template <typename U> inline T dot3(const RaveVector<U> &v) const {
232  return x*v.x + y*v.y + z*v.z;
233  }
235  return normalize4();
236  }
238  T f = x*x+y*y+z*z+w*w;
239  if(( f < T(1)-std::numeric_limits<T>::epsilon()) ||( f > T(1)+std::numeric_limits<T>::epsilon()) ) {
240  MATH_ASSERT( f > 0 );
241  // yes it is faster to multiply by (1/f), but with 4 divides we gain precision (which is more important in robotics)
242  f = MATH_SQRT(f);
243  x /= f; y /= f; z /= f; w /= f;
244  }
245  return *this;
246  }
248  T f = x*x+y*y+z*z;
249  if(( f < T(1)-std::numeric_limits<T>::epsilon()) ||( f > T(1)+std::numeric_limits<T>::epsilon()) ) {
250  MATH_ASSERT( f > 0 );
251  f = MATH_SQRT(f);
252  x /= f; y /= f; z /= f;
253  }
254  return *this;
255  }
256 
257  inline T lengthsqr2() const {
258  return x*x + y*y;
259  }
260  inline T lengthsqr3() const {
261  return x*x + y*y + z*z;
262  }
263  inline T lengthsqr4() const {
264  return x*x + y*y + z*z + w*w;
265  }
266 
267  inline void Set3(const T* pvals) {
268  x = pvals[0]; y = pvals[1]; z = pvals[2];
269  }
270  inline void Set3(T val1, T val2, T val3) {
271  x = val1; y = val2; z = val3;
272  }
273  inline void Set4(const T* pvals) {
274  x = pvals[0]; y = pvals[1]; z = pvals[2]; w = pvals[3];
275  }
276  inline void Set4(T val1, T val2, T val3, T val4) {
277  x = val1; y = val2; z = val3; w = val4;
278  }
280  inline RaveVector<T> cross(const RaveVector<T> &v) const {
281  RaveVector<T> ucrossv;
282  ucrossv[0] = y * v[2] - z * v[1];
283  ucrossv[1] = z * v[0] - x * v[2];
284  ucrossv[2] = x * v[1] - y * v[0];
285  return ucrossv;
286  }
287 
289  Cross(*this, v); return *this;
290  }
292  {
293  RaveVector<T> ucrossv;
294  ucrossv[0] = u[1] * v[2] - u[2] * v[1];
295  ucrossv[1] = u[2] * v[0] - u[0] * v[2];
296  ucrossv[2] = u[0] * v[1] - u[1] * v[0];
297  *this = ucrossv;
298  return *this;
299  }
300 
301  inline RaveVector<T> operator-() const {
302  RaveVector<T> v; v.x = -x; v.y = -y; v.z = -z; v.w = -w; return v;
303  }
304  template <typename U> inline RaveVector<T> operator+(const RaveVector<U> &r) const {
305  RaveVector<T> v; v.x = x+T(r.x); v.y = y+T(r.y); v.z = z+T(r.z); v.w = w+T(r.w); return v;
306  }
307  template <typename U> inline RaveVector<T> operator-(const RaveVector<U> &r) const {
308  RaveVector<T> v; v.x = x-T(r.x); v.y = y-T(r.y); v.z = z-T(r.z); v.w = w-T(r.w); return v;
309  }
310  template <typename U> inline RaveVector<T> operator*(const RaveVector<U> &r) const {
311  RaveVector<T> v; v.x = T(r.x)*x; v.y = T(r.y)*y; v.z = T(r.z)*z; v.w = T(r.w)*w; return v;
312  }
313  inline RaveVector<T> operator*(T k) const {
314  RaveVector<T> v; v.x = k*x; v.y = k*y; v.z = k*z; v.w = k*w; return v;
315  }
316 
317  template <typename U> inline RaveVector<T>& operator += (const RaveVector<U>&r) {
318  x += T(r.x); y += T(r.y); z += T(r.z); w += T(r.w); return *this;
319  }
320  template <typename U> inline RaveVector<T>& operator -= (const RaveVector<U>&r) {
321  x -= T(r.x); y -= T(r.y); z -= T(r.z); w -= T(r.w); return *this;
322  }
323  template <typename U> inline RaveVector<T>& operator *= (const RaveVector<U>&r) {
324  x *= T(r.x); y *= T(r.y); z *= T(r.z); w *= T(r.w); return *this;
325  }
326 
327  inline RaveVector<T>& operator *= (const T k) {
328  x *= k; y *= k; z *= k; w *= k; return *this;
329  }
330  inline RaveVector<T>& operator /= (const T _k) {
331  T k=1/_k; x *= k; y *= k; z *= k; w *= k; return *this;
332  }
333 
334  template <typename U> friend RaveVector<U> operator* (float f, const RaveVector<U>&v);
335  template <typename U> friend RaveVector<U> operator* (double f, const RaveVector<U>&v);
336 
337  template <typename U> friend std::ostream& operator<<(std::ostream& O, const RaveVector<U>&v);
338  template <typename U> friend std::istream& operator>>(std::istream& I, RaveVector<U>&v);
339 
341  template <typename U> inline RaveVector<T> operator^(const RaveVector<U> &v) const {
342  RaveVector<T> ucrossv;
343  ucrossv[0] = y * v[2] - z * v[1];
344  ucrossv[1] = z * v[0] - x * v[2];
345  ucrossv[2] = x * v[1] - y * v[0];
346  return ucrossv;
347  }
348 };
349 
350 template <typename T>
351 inline RaveVector<T> operator* (float f, const RaveVector<T>&left)
352 {
353  RaveVector<T> v;
354  v.x = (T)f * left.x;
355  v.y = (T)f * left.y;
356  v.z = (T)f * left.z;
357  v.w = (T)f * left.w;
358  return v;
359 }
360 
361 template <typename T>
362 inline RaveVector<T> operator* (double f, const RaveVector<T>&left)
363 {
364  RaveVector<T> v;
365  v.x = (T)f * left.x;
366  v.y = (T)f * left.y;
367  v.z = (T)f * left.z;
368  v.w = (T)f * left.w;
369  return v;
370 }
371 
376 template <typename T>
378 {
379 public:
381  rot.x = 1;
382  }
383  template <typename U> RaveTransform(const RaveTransform<U>& t) {
384  rot = t.rot;
385  trans = t.trans;
386  T fnorm = rot.lengthsqr4();
387  MATH_ASSERT( fnorm > 0.99f && fnorm < 1.01f );
388  }
389  template <typename U> RaveTransform(const RaveVector<U>& rot, const RaveVector<U>& trans) : rot(rot), trans(trans) {
390  T fnorm = rot.lengthsqr4();
391  MATH_ASSERT( fnorm > 0.99f && fnorm < 1.01f );
392  }
393  inline RaveTransform(const RaveTransformMatrix<T>&t);
394 
395  void identity() {
396  rot.x = 1; rot.y = rot.z = rot.w = 0;
397  trans.x = trans.y = trans.z = 0;
398  }
399 
401  inline RaveVector<T> operator* (const RaveVector<T>&r) const {
402  return trans + rotate(r);
403  }
404 
406  inline RaveVector<T> rotate(const RaveVector<T>& r) const {
407  T xx = 2 * rot.y * rot.y;
408  T xy = 2 * rot.y * rot.z;
409  T xz = 2 * rot.y * rot.w;
410  T xw = 2 * rot.y * rot.x;
411  T yy = 2 * rot.z * rot.z;
412  T yz = 2 * rot.z * rot.w;
413  T yw = 2 * rot.z * rot.x;
414  T zz = 2 * rot.w * rot.w;
415  T zw = 2 * rot.w * rot.x;
416 
417  RaveVector<T> v;
418  v.x = (1-yy-zz) * r.x + (xy-zw) * r.y + (xz+yw)*r.z;
419  v.y = (xy+zw) * r.x + (1-xx-zz) * r.y + (yz-xw)*r.z;
420  v.z = (xz-yw) * r.x + (yz+xw) * r.y + (1-xx-yy)*r.z;
421  return v;
422  }
423 
425  inline RaveTransform<T> rotate(const RaveTransform<T>& r) const {
427  t.trans = rotate(r.trans);
428  t.rot.x = rot.x*r.rot.x - rot.y*r.rot.y - rot.z*r.rot.z - rot.w*r.rot.w;
429  t.rot.y = rot.x*r.rot.y + rot.y*r.rot.x + rot.z*r.rot.w - rot.w*r.rot.z;
430  t.rot.z = rot.x*r.rot.z + rot.z*r.rot.x + rot.w*r.rot.y - rot.y*r.rot.w;
431  t.rot.w = rot.x*r.rot.w + rot.w*r.rot.x + rot.y*r.rot.z - rot.z*r.rot.y;
432  // normalize the transformation
433  T fnorm = t.rot.lengthsqr4();
434  MATH_ASSERT( fnorm > 0.99f && fnorm < 1.01f );
435  t.rot.normalize4();
436  return t;
437  }
438 
442  t.trans = operator*(r.trans);
443  t.rot.x = rot.x*r.rot.x - rot.y*r.rot.y - rot.z*r.rot.z - rot.w*r.rot.w;
444  t.rot.y = rot.x*r.rot.y + rot.y*r.rot.x + rot.z*r.rot.w - rot.w*r.rot.z;
445  t.rot.z = rot.x*r.rot.z + rot.z*r.rot.x + rot.w*r.rot.y - rot.y*r.rot.w;
446  t.rot.w = rot.x*r.rot.w + rot.w*r.rot.x + rot.y*r.rot.z - rot.z*r.rot.y;
447  // normalize the transformation
448  T fnorm = t.rot.lengthsqr4();
449  MATH_ASSERT( fnorm > 0.99f && fnorm < 1.01f );
450  t.rot.normalize4();
451  return t;
452  }
453 
455  *this = operator*(right);
456  return *this;
457  }
458 
459  inline RaveTransform<T> inverse() const {
460  RaveTransform<T> inv;
461  inv.rot.x = rot.x;
462  inv.rot.y = -rot.y;
463  inv.rot.z = -rot.z;
464  inv.rot.w = -rot.w;
465 
466  inv.trans = -inv.rotate(trans);
467  return inv;
468  }
469 
470  template <typename U> inline RaveTransform<T>& operator= (const RaveTransform<U>&r) {
471  trans = r.trans;
472  rot = r.rot;
473  T fnorm = rot.lengthsqr4();
474  MATH_ASSERT( fnorm > 0.99f && fnorm < 1.01f );
475  return *this;
476  }
477 
478  template <typename U> friend std::ostream& operator<<(std::ostream& O, const RaveTransform<U>&v);
479  template <typename U> friend std::istream& operator>>(std::istream& I, RaveTransform<U>&v);
480 
482 };
483 
488 template <typename T>
490 {
491 public:
493  identity(); m[3] = m[7] = m[11] = 0;
494  }
495  template <typename U> RaveTransformMatrix(const RaveTransformMatrix<U>& t) {
496  // don't memcpy!
497  m[0] = T(t.m[0]); m[1] = T(t.m[1]); m[2] = T(t.m[2]); m[3] = T(t.m[3]);
498  m[4] = T(t.m[4]); m[5] = T(t.m[5]); m[6] = T(t.m[6]); m[7] = T(t.m[7]);
499  m[8] = T(t.m[8]); m[9] = T(t.m[9]); m[10] = T(t.m[10]); m[11] = T(t.m[11]);
500  trans = t.trans;
501  }
502  inline RaveTransformMatrix(const RaveTransform<T>&t);
503 
504  inline void identity() {
505  m[0] = 1; m[1] = 0; m[2] = 0;
506  m[4] = 0; m[5] = 1; m[6] = 0;
507  m[8] = 0; m[9] = 0; m[10] = 1;
508  trans.x = trans.y = trans.z = 0;
509  }
510 
511  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) {
512  m[0] = m_00; m[1] = m_01; m[2] = m_02; m[3] = 0;
513  m[4] = m_10; m[5] = m_11; m[6] = m_12; m[7] = 0;
514  m[8] = m_20; m[9] = m_21; m[10] = m_22; m[11] = 0;
515  }
516 
517  inline T rot(int i, int j) const {
518  MATH_ASSERT( i >= 0 && j >= 0 && i < 3 && j < 3);
519  return m[4*i+j];
520  }
521  inline T& rot(int i, int j) {
522  MATH_ASSERT( i >= 0 && j >= 0 && i < 3 && j < 3);
523  return m[4*i+j];
524  }
525 
526  template <typename U>
527  inline RaveVector<T> operator* (const RaveVector<U>&r) const {
528  RaveVector<T> v;
529  v[0] = r[0] * m[0] + r[1] * m[1] + r[2] * m[2] + trans.x;
530  v[1] = r[0] * m[4] + r[1] * m[5] + r[2] * m[6] + trans.y;
531  v[2] = r[0] * m[8] + r[1] * m[9] + r[2] * m[10] + trans.z;
532  return v;
533  }
534 
538  t.m[0*4+0] = m[0*4+0]*r.m[0*4+0]+m[0*4+1]*r.m[1*4+0]+m[0*4+2]*r.m[2*4+0];
539  t.m[0*4+1] = m[0*4+0]*r.m[0*4+1]+m[0*4+1]*r.m[1*4+1]+m[0*4+2]*r.m[2*4+1];
540  t.m[0*4+2] = m[0*4+0]*r.m[0*4+2]+m[0*4+1]*r.m[1*4+2]+m[0*4+2]*r.m[2*4+2];
541  t.m[1*4+0] = m[1*4+0]*r.m[0*4+0]+m[1*4+1]*r.m[1*4+0]+m[1*4+2]*r.m[2*4+0];
542  t.m[1*4+1] = m[1*4+0]*r.m[0*4+1]+m[1*4+1]*r.m[1*4+1]+m[1*4+2]*r.m[2*4+1];
543  t.m[1*4+2] = m[1*4+0]*r.m[0*4+2]+m[1*4+1]*r.m[1*4+2]+m[1*4+2]*r.m[2*4+2];
544  t.m[2*4+0] = m[2*4+0]*r.m[0*4+0]+m[2*4+1]*r.m[1*4+0]+m[2*4+2]*r.m[2*4+0];
545  t.m[2*4+1] = m[2*4+0]*r.m[0*4+1]+m[2*4+1]*r.m[1*4+1]+m[2*4+2]*r.m[2*4+1];
546  t.m[2*4+2] = m[2*4+0]*r.m[0*4+2]+m[2*4+1]*r.m[1*4+2]+m[2*4+2]*r.m[2*4+2];
547  t.trans[0] = r.trans[0] * m[0] + r.trans[1] * m[1] + r.trans[2] * m[2] + trans.x;
548  t.trans[1] = r.trans[0] * m[4] + r.trans[1] * m[5] + r.trans[2] * m[6] + trans.y;
549  t.trans[2] = r.trans[0] * m[8] + r.trans[1] * m[9] + r.trans[2] * m[10] + trans.z;
550  return t;
551  }
552 
554  *this = *this * r;
555  return *this;
556  }
557 
558  template <typename U>
559  inline RaveVector<U> rotate(const RaveVector<U>& r) const {
560  RaveVector<U> v;
561  v.x = r.x * m[0] + r.y * m[1] + r.z * m[2];
562  v.y = r.x * m[4] + r.y * m[5] + r.z * m[6];
563  v.z = r.x * m[8] + r.y * m[9] + r.z * m[10];
564  return v;
565  }
566 
569  t.m[0*4+0] = m[0*4+0]*r.m[0*4+0]+m[0*4+1]*r.m[1*4+0]+m[0*4+2]*r.m[2*4+0];
570  t.m[0*4+1] = m[0*4+0]*r.m[0*4+1]+m[0*4+1]*r.m[1*4+1]+m[0*4+2]*r.m[2*4+1];
571  t.m[0*4+2] = m[0*4+0]*r.m[0*4+2]+m[0*4+1]*r.m[1*4+2]+m[0*4+2]*r.m[2*4+2];
572  t.m[1*4+0] = m[1*4+0]*r.m[0*4+0]+m[1*4+1]*r.m[1*4+0]+m[1*4+2]*r.m[2*4+0];
573  t.m[1*4+1] = m[1*4+0]*r.m[0*4+1]+m[1*4+1]*r.m[1*4+1]+m[1*4+2]*r.m[2*4+1];
574  t.m[1*4+2] = m[1*4+0]*r.m[0*4+2]+m[1*4+1]*r.m[1*4+2]+m[1*4+2]*r.m[2*4+2];
575  t.m[2*4+0] = m[2*4+0]*r.m[0*4+0]+m[2*4+1]*r.m[1*4+0]+m[2*4+2]*r.m[2*4+0];
576  t.m[2*4+1] = m[2*4+0]*r.m[0*4+1]+m[2*4+1]*r.m[1*4+1]+m[2*4+2]*r.m[2*4+1];
577  t.m[2*4+2] = m[2*4+0]*r.m[0*4+2]+m[2*4+1]*r.m[1*4+2]+m[2*4+2]*r.m[2*4+2];
578  t.trans[0] = r.trans[0] * m[0] + r.trans[1] * m[1] + r.trans[2] * m[2];
579  t.trans[1] = r.trans[0] * m[4] + r.trans[1] * m[5] + r.trans[2] * m[6];
580  t.trans[2] = r.trans[0] * m[8] + r.trans[1] * m[9] + r.trans[2] * m[10];
581  return t;
582  }
583 
586  // inverse = C^t / det(pf) where C is the matrix of coefficients
587  // calc C^t
589  inv.m[0*4+0] = m[1*4 + 1] * m[2*4 + 2] - m[1*4 + 2] * m[2*4 + 1];
590  inv.m[0*4+1] = m[0*4 + 2] * m[2*4 + 1] - m[0*4 + 1] * m[2*4 + 2];
591  inv.m[0*4+2] = m[0*4 + 1] * m[1*4 + 2] - m[0*4 + 2] * m[1*4 + 1];
592  inv.m[1*4+0] = m[1*4 + 2] * m[2*4 + 0] - m[1*4 + 0] * m[2*4 + 2];
593  inv.m[1*4+1] = m[0*4 + 0] * m[2*4 + 2] - m[0*4 + 2] * m[2*4 + 0];
594  inv.m[1*4+2] = m[0*4 + 2] * m[1*4 + 0] - m[0*4 + 0] * m[1*4 + 2];
595  inv.m[2*4+0] = m[1*4 + 0] * m[2*4 + 1] - m[1*4 + 1] * m[2*4 + 0];
596  inv.m[2*4+1] = m[0*4 + 1] * m[2*4 + 0] - m[0*4 + 0] * m[2*4 + 1];
597  inv.m[2*4+2] = m[0*4 + 0] * m[1*4 + 1] - m[0*4 + 1] * m[1*4 + 0];
598  T fdet = m[0*4 + 2] * inv.m[2*4+0] + m[1*4 + 2] * inv.m[2*4+1] + m[2*4 + 2] * inv.m[2*4+2];
599  MATH_ASSERT(fdet>=0);
600  fdet = 1 / fdet;
601  inv.m[0*4+0] *= fdet; inv.m[0*4+1] *= fdet; inv.m[0*4+2] *= fdet;
602  inv.m[1*4+0] *= fdet; inv.m[1*4+1] *= fdet; inv.m[1*4+2] *= fdet;
603  inv.m[2*4+0] *= fdet; inv.m[2*4+1] *= fdet; inv.m[2*4+2] *= fdet;
604  inv.trans = -inv.rotate(trans);
605  return inv;
606  }
607 
608  template <typename U>
609  inline void Extract(RaveVector<U>& right, RaveVector<U>& up, RaveVector<U>& dir, RaveVector<U>& pos) const {
610  pos = trans;
611  right.x = m[0]; up.x = m[1]; dir.x = m[2];
612  right.y = m[4]; up.y = m[5]; dir.y = m[6];
613  right.z = m[8]; up.z = m[9]; dir.z = m[10];
614  }
615 
616  template <typename U> friend std::ostream& operator<<(std::ostream& O, const RaveTransformMatrix<U>&v);
617  template <typename U> friend std::istream& operator>>(std::istream& I, RaveTransformMatrix<U>&v);
618 
621  T m[12];
623 };
624 
627 template <typename T>
628 class ray
629 {
630 public:
631  ray() {
632  }
633  ray(const RaveVector<T>&_pos, const RaveVector<T>&_dir) : pos(_pos), dir(_dir) {
634  }
636 };
637 
640 template <typename T>
641 class aabb
642 {
643 public:
644  aabb() {
645  }
646  aabb(const RaveVector<T>&vpos, const RaveVector<T>&vextents) : pos(vpos), extents(vextents) {
647  }
649 };
650 
653 template <typename T>
655 {
656 public:
659 };
660 
663 template <typename T>
664 class obb
665 {
666 public:
668 };
669 
672 template <typename T>
673 class triangle
674 {
675 public:
677  }
678  triangle(const RaveVector<T>&v1, const RaveVector<T>&v2, const RaveVector<T>&v3) : v1(v1), v2(v2), v3(v3) {
679  }
681  }
682 
684 
685  const RaveVector<T>& operator[] (int i) const {
686  return (&v1)[i];
687  }
689  return (&v1)[i];
690  }
691 
694  return (v2-v1).cross(v3-v1);
695  }
696 };
697 
700 template <typename T>
701 class frustum
702 {
703 public:
708 };
709 
711 template <typename T>
713 {
714 public:
715  RaveCameraIntrinsics() : fx(0),fy(0),cx(0),cy(0), focal_length(0.01) {
716  }
717  RaveCameraIntrinsics(T fx, T fy, T cx, T cy) : fx(fx), fy(fy), cx(cx), cy(cy), focal_length(0.01) {
718  }
719 
720  template <typename U>
722  {
724  distortion_coeffs.resize(r.distortion_coeffs.size());
725  std::copy(r.distortion_coeffs.begin(),r.distortion_coeffs.end(),distortion_coeffs.begin());
727  fx = r.fx;
728  fy = r.fy;
729  cx = r.cx;
730  cy = r.cy;
731  }
732 
733  T fx,fy, cx,cy;
734 
740  std::string distortion_model;
741  std::vector<T> distortion_coeffs;
743 };
744 
748 
749 template <typename U>
750 std::ostream& operator<<(std::ostream& O, const RaveVector<U>&v)
751 {
752  return O << v.x << " " << v.y << " " << v.z << " " << v.w << " ";
753 }
754 
755 template <typename U>
756 std::istream& operator>>(std::istream& I, RaveVector<U>&v)
757 {
758  return I >> v.x >> v.y >> v.z >> v.w;
759 }
760 
761 template <typename U>
762 std::ostream& operator<<(std::ostream& O, const RaveTransform<U>&v)
763 {
764  return O << v.rot.x << " " << v.rot.y << " " << v.rot.z << " " << v.rot.w << " " << v.trans.x << " " << v.trans.y << " " << v.trans.z << " ";
765 }
766 
767 template <typename U>
768 std::istream& operator>>(std::istream& I, RaveTransform<U>&v)
769 {
770  return I >> v.rot.x >> v.rot.y >> v.rot.z >> v.rot.w >> v.trans.x >> v.trans.y >> v.trans.z;
771 }
772 
773 template <typename U>
774 std::ostream& operator<<(std::ostream& O, const ray<U>&r)
775 {
776  return O << r.pos.x << " " << r.pos.y << " " << r.pos.z << " " << r.dir.x << " " << r.dir.y << " " << r.dir.z << " ";
777 }
778 
779 template <typename U>
780 std::istream& operator>>(std::istream& I, ray<U>&r)
781 {
782  return I >> r.pos.x >> r.pos.y >> r.pos.z >> r.dir.x >> r.dir.y >> r.dir.z;
783 }
784 
785 
787 template <typename U>
788 std::ostream& operator<<(std::ostream& O, const RaveTransformMatrix<U>&v)
789 {
790  return O << v.m[0] << " " << v.m[4] << " " << v.m[8] << " " << v.m[1] << " " << v.m[5] << " " << v.m[9] << " " << v.m[2] << " " << v.m[6] << " " << v.m[10] << " " << v.trans.x << " " << v.trans.y << " " << v.trans.z << " ";
791 }
792 
794 template <typename U>
795 std::istream& operator>>(std::istream& I, RaveTransformMatrix<U>&v)
796 {
797  return I >> v.m[0] >> v.m[4] >> v.m[8] >> v.m[1] >> v.m[5] >> v.m[9] >> v.m[2] >> v.m[6] >> v.m[10] >> v.trans.x >> v.trans.y >> v.trans.z;
798 }
799 
801 
807 template <typename T> inline RaveVector<T> quatFromAxisAngle(const RaveVector<T>& axis, T angle)
808 {
809  T axislen = MATH_SQRT(axis.lengthsqr3());
810  if( axislen == 0 ) {
811  return RaveVector<T>(T(1),T(0),T(0),T(0));
812  }
813  angle *= T(0.5);
814  T sang = MATH_SIN(angle)/axislen;
815  return RaveVector<T>(MATH_COS(angle),axis.x*sang,axis.y*sang,axis.z*sang);
816 }
817 
822 template <typename T> inline RaveVector<T> quatFromAxisAngle(const RaveVector<T>& axisangle)
823 {
824  T axislen = MATH_SQRT(axisangle.lengthsqr3());
825  if( axislen == 0 ) {
826  return RaveVector<T>(T(1),T(0),T(0),T(0));
827  }
828  T sang = MATH_SIN(axislen*T(0.5))/axislen;
829  return RaveVector<T>(MATH_COS(axislen*T(0.5)),axisangle.x*sang,axisangle.y*sang,axisangle.z*sang);
830 }
831 
836 template <typename T> inline RaveVector<T> quatFromMatrix(const RaveTransformMatrix<T>& rotation)
837 {
838  RaveVector<T> rot;
839  T tr = rotation.m[4*0+0] + rotation.m[4*1+1] + rotation.m[4*2+2];
840  if (tr >= 0) {
841  rot[0] = tr + 1;
842  rot[1] = (rotation.m[4*2+1] - rotation.m[4*1+2]);
843  rot[2] = (rotation.m[4*0+2] - rotation.m[4*2+0]);
844  rot[3] = (rotation.m[4*1+0] - rotation.m[4*0+1]);
845  }
846  else {
847  // find the largest diagonal element and jump to the appropriate case
848  if (rotation.m[4*1+1] > rotation.m[4*0+0]) {
849  if (rotation.m[4*2+2] > rotation.m[4*1+1]) {
850  rot[3] = (rotation.m[4*2+2] - (rotation.m[4*0+0] + rotation.m[4*1+1])) + 1;
851  rot[1] = (rotation.m[4*2+0] + rotation.m[4*0+2]);
852  rot[2] = (rotation.m[4*1+2] + rotation.m[4*2+1]);
853  rot[0] = (rotation.m[4*1+0] - rotation.m[4*0+1]);
854  }
855  else {
856  rot[2] = (rotation.m[4*1+1] - (rotation.m[4*2+2] + rotation.m[4*0+0])) + 1;
857  rot[3] = (rotation.m[4*1+2] + rotation.m[4*2+1]);
858  rot[1] = (rotation.m[4*0+1] + rotation.m[4*1+0]);
859  rot[0] = (rotation.m[4*0+2] - rotation.m[4*2+0]);
860  }
861  }
862  else if (rotation.m[4*2+2] > rotation.m[4*0+0]) {
863  rot[3] = (rotation.m[4*2+2] - (rotation.m[4*0+0] + rotation.m[4*1+1])) + 1;
864  rot[1] = (rotation.m[4*2+0] + rotation.m[4*0+2]);
865  rot[2] = (rotation.m[4*1+2] + rotation.m[4*2+1]);
866  rot[0] = (rotation.m[4*1+0] - rotation.m[4*0+1]);
867  }
868  else {
869  rot[1] = (rotation.m[4*0+0] - (rotation.m[4*1+1] + rotation.m[4*2+2])) + 1;
870  rot[2] = (rotation.m[4*0+1] + rotation.m[4*1+0]);
871  rot[3] = (rotation.m[4*2+0] + rotation.m[4*0+2]);
872  rot[0] = (rotation.m[4*2+1] - rotation.m[4*1+2]);
873  }
874  }
875  rot.normalize4();
876  return rot;
877 }
878 
883 template <typename T> inline RaveTransformMatrix<T> matrixFromQuat(const RaveVector<T>& quat)
884 {
885  // should normalize the quaternion first
886  T length2 = quat.lengthsqr4();
887  MATH_ASSERT(length2 > 0.99 && length2 < 1.01); // make sure it is at least close
888  T ilength2 = 2/length2;
890  T qq1 = ilength2*quat[1]*quat[1];
891  T qq2 = ilength2*quat[2]*quat[2];
892  T qq3 = ilength2*quat[3]*quat[3];
893  t.m[4*0+0] = 1 - qq2 - qq3;
894  t.m[4*0+1] = ilength2*(quat[1]*quat[2] - quat[0]*quat[3]);
895  t.m[4*0+2] = ilength2*(quat[1]*quat[3] + quat[0]*quat[2]);
896  t.m[4*0+3] = 0;
897  t.m[4*1+0] = ilength2*(quat[1]*quat[2] + quat[0]*quat[3]);
898  t.m[4*1+1] = 1 - qq1 - qq3;
899  t.m[4*1+2] = ilength2*(quat[2]*quat[3] - quat[0]*quat[1]);
900  t.m[4*1+3] = 0;
901  t.m[4*2+0] = ilength2*(quat[1]*quat[3] - quat[0]*quat[2]);
902  t.m[4*2+1] = ilength2*(quat[2]*quat[3] + quat[0]*quat[1]);
903  t.m[4*2+2] = 1 - qq1 - qq2;
904  t.m[4*2+3] = 0;
905  return t;
906 }
907 
913 template <typename T> void matrixFromQuat(RaveTransformMatrix<T>& rotation, const RaveVector<T>& quat)
914 {
915  // should normalize the quaternion first
916  T length2 = quat.lengthsqr4();
917  MATH_ASSERT(length2 > 0.99 && length2 < 1.01); // make sure it is at least close
918  T ilength2 = 2/length2;
919  T qq1 = ilength2*quat[1]*quat[1];
920  T qq2 = ilength2*quat[2]*quat[2];
921  T qq3 = ilength2*quat[3]*quat[3];
922  rotation.m[4*0+0] = 1 - qq2 - qq3;
923  rotation.m[4*0+1] = ilength2*(quat[1]*quat[2] - quat[0]*quat[3]);
924  rotation.m[4*0+2] = ilength2*(quat[1]*quat[3] + quat[0]*quat[2]);
925  rotation.m[4*0+3] = 0;
926  rotation.m[4*1+0] = ilength2*(quat[1]*quat[2] + quat[0]*quat[3]);
927  rotation.m[4*1+1] = 1 - qq1 - qq3;
928  rotation.m[4*1+2] = ilength2*(quat[2]*quat[3] - quat[0]*quat[1]);
929  rotation.m[4*1+3] = 0;
930  rotation.m[4*2+0] = ilength2*(quat[1]*quat[3] - quat[0]*quat[2]);
931  rotation.m[4*2+1] = ilength2*(quat[2]*quat[3] + quat[0]*quat[1]);
932  rotation.m[4*2+2] = 1 - qq1 - qq2;
933  rotation.m[4*2+3] = 0;
934 }
935 
941 template <typename T> inline RaveTransformMatrix<T> matrixFromAxisAngle(const RaveVector<T>& axis, T angle)
942 {
943  return matrixFromQuat(quatFromAxisAngle(axis,angle));
944 }
945 
950 template <typename T> inline RaveTransformMatrix<T> matrixFromAxisAngle(const RaveVector<T>& axisangle)
951 {
952  return matrixFromQuat(quatFromAxisAngle(axisangle));
953 }
954 
960 template <typename T>
961 inline RaveVector<T> quatMultiply(const RaveVector<T>& quat0, const RaveVector<T>& quat1)
962 {
963  RaveVector<T> q(quat0.x*quat1.x - quat0.y*quat1.y - quat0.z*quat1.z - quat0.w*quat1.w,
964  quat0.x*quat1.y + quat0.y*quat1.x + quat0.z*quat1.w - quat0.w*quat1.z,
965  quat0.x*quat1.z + quat0.z*quat1.x + quat0.w*quat1.y - quat0.y*quat1.w,
966  quat0.x*quat1.w + quat0.w*quat1.x + quat0.y*quat1.z - quat0.z*quat1.y);
967  // do not normalize since some quaternion math (like derivatives) do not correspond to unit quaternions
968  return q;
969 }
970 
975 template <typename T>
977 {
978  return RaveVector<T>(quat.x,-quat.y,-quat.z,-quat.w);
979 }
980 
987 template <typename T>
988 inline RaveVector<T> quatSlerp(const RaveVector<T>& quat0, const RaveVector<T>& quat1, T t)
989 {
990  // quaternion to return
991  RaveVector<T> qb, qm;
992  if( quat0.dot(quat1) < 0 ) {
993  qb = -quat1;
994  }
995  else {
996  qb = quat1;
997  }
998  // Calculate angle between them.
999  T cosHalfTheta = quat0.w * qb.w + quat0.x * qb.x + quat0.y * qb.y + quat0.z * qb.z;
1000  // if quat0=qb or quat0=-qb then theta = 0 and we can return quat0
1001  if (MATH_FABS(cosHalfTheta) >= 1.0) {
1002  qm.w = quat0.w; qm.x = quat0.x; qm.y = quat0.y; qm.z = quat0.z;
1003  return qm;
1004  }
1005  // Calculate temporary values.
1006  T halfTheta = MATH_ACOS(cosHalfTheta);
1007  T sinHalfTheta = MATH_SQRT(1 - cosHalfTheta*cosHalfTheta);
1008  // if theta = 180 degrees then result is not fully defined
1009  // we could rotate around any axis normal to quat0 or qb
1010  if (MATH_FABS(sinHalfTheta) < 1e-7f) { // fabs is floating point absolute
1011  qm.w = (quat0.w * 0.5f + qb.w * 0.5f);
1012  qm.x = (quat0.x * 0.5f + qb.x * 0.5f);
1013  qm.y = (quat0.y * 0.5f + qb.y * 0.5f);
1014  qm.z = (quat0.z * 0.5f + qb.z * 0.5f);
1015  return qm;
1016  }
1017  T ratioA = MATH_SIN((1 - t) * halfTheta) / sinHalfTheta;
1018  T ratioB = MATH_SIN(t * halfTheta) / sinHalfTheta;
1019  //calculate Quaternion.
1020  qm.w = (quat0.w * ratioA + qb.w * ratioB);
1021  qm.x = (quat0.x * ratioA + qb.x * ratioB);
1022  qm.y = (quat0.y * ratioA + qb.y * ratioB);
1023  qm.z = (quat0.z * ratioA + qb.z * ratioB);
1024  return qm;
1025 }
1026 
1027 template <typename T>
1028 inline RaveVector<T> dQSlerp(const RaveVector<T>& qa, const RaveVector<T>& _qb, T t)
1029 {
1030  return quatSlerp<T>(qa,_qb,t);
1031 }
1032 
1037 template <typename T>
1039 {
1040  T xx = 2 * q.y * q.y;
1041  T xy = 2 * q.y * q.z;
1042  T xz = 2 * q.y * q.w;
1043  T xw = 2 * q.y * q.x;
1044  T yy = 2 * q.z * q.z;
1045  T yz = 2 * q.z * q.w;
1046  T yw = 2 * q.z * q.x;
1047  T zz = 2 * q.w * q.w;
1048  T zw = 2 * q.w * q.x;
1049  RaveVector<T> v;
1050  v.x = (1-yy-zz) * t.x + (xy-zw) * t.y + (xz+yw)*t.z;
1051  v.y = (xy+zw) * t.x + (1-xx-zz) * t.y + (yz-xw)*t.z;
1052  v.z = (xz-yw) * t.x + (yz+xw) * t.y + (1-xx-yy)*t.z;
1053  return v;
1054 }
1055 
1056 
1062 template<typename T>
1064 {
1065  RaveVector<T> rottodirection = sourcedir.cross(targetdir);
1066  T fsin = MATH_SQRT(rottodirection.lengthsqr3());
1067  T fcos = sourcedir.dot3(targetdir);
1068  RaveTransform<T> torient;
1069  if( fsin > 0 ) {
1070  return quatFromAxisAngle(rottodirection*(1/fsin), MATH_ATAN2(fsin, fcos));
1071  }
1072  if( fcos < 0 ) {
1073  // hand is flipped 180, rotate around x axis
1074  rottodirection = RaveVector<T>(1,0,0);
1075  rottodirection -= sourcedir * sourcedir.dot3(rottodirection);
1076  if( rottodirection.lengthsqr3()<1e-8 ) {
1077  rottodirection = RaveVector<T>(0,0,1);
1078  rottodirection -= sourcedir * sourcedir.dot3(rottodirection);
1079  }
1080  rottodirection.normalize3();
1081  return quatFromAxisAngle(rottodirection, MATH_ATAN2(fsin, fcos));
1082  }
1083  return RaveVector<T>(T(1),T(0),T(0),T(0));
1084 }
1085 
1092 template<typename T>
1093 std::pair<T, RaveVector<T> > normalizeAxisRotation(const RaveVector<T>& axis, const RaveVector<T>& quat)
1094 {
1095  T axislen = MATH_SQRT(axis.lengthsqr3());
1096  T angle = MATH_ATAN2(-quat.w*axis.z-quat.z*axis.y-quat.y*axis.x,quat.x*axislen);
1097  T sinangle2 = MATH_SIN(angle)/axislen, cosangle2 = MATH_COS(angle);
1098  RaveVector<T> normalizingquat = RaveVector<T>(cosangle2,axis.x*sinangle2,axis.y*sinangle2,axis.z*sinangle2);
1099  return std::make_pair(2*angle,quatMultiply(normalizingquat,quat));
1100 }
1101 
1106 template<typename T>
1108 {
1109  T sinang = quat.y*quat.y+quat.z*quat.z+quat.w*quat.w;
1110  if( sinang == 0 ) {
1111  return RaveVector<T>(0,0,0);
1112  }
1113  RaveVector<T> _quat;
1114  if( quat.x < 0 ) {
1115  _quat = -quat;
1116  }
1117  else {
1118  _quat = quat;
1119  }
1120  sinang = MATH_SQRT(sinang);
1121  T f = 2.0*MATH_ATAN2(sinang,_quat.x)/sinang;
1122  return RaveVector<T>(_quat.y*f,_quat.z*f,_quat.w*f);
1123 }
1124 
1129 template<typename T>
1131 {
1132  return axisAngleFromQuat(quatFromMatrix(rotation));
1133 }
1134 
1135 template <typename T>
1137 {
1138  trans = t.trans;
1139  rot = quatFromMatrix(t);
1140 }
1141 
1142 template <typename T>
1144 {
1145  matrixFromQuat(*this, t.rot);
1146  trans = t.trans;
1147 
1148 }
1149 
1156 template<typename T>
1157 RaveTransformMatrix<T> transformLookat(const RaveVector<T>& vlookat, const RaveVector<T>& vcamerapos, const RaveVector<T>& vcameraup)
1158 {
1159  RaveVector<T> dir = vlookat - vcamerapos;
1160  T len = MATH_SQRT(dir.lengthsqr3());
1161  if( len > 1e-6 ) {
1162  dir *= 1/len;
1163  }
1164  else {
1165  dir = RaveVector<T>(0,0,1);
1166  }
1167  RaveVector<T> up = vcameraup - dir * dir.dot3(vcameraup);
1168  len = up.lengthsqr3();
1169  if( len < 1e-8 ) {
1170  up = RaveVector<T>(0,1,0);
1171  up -= dir * dir.dot3(up);
1172  len = up.lengthsqr3();
1173  if( len < 1e-8 ) {
1174  up = RaveVector<T>(1,0,0);
1175  up -= dir * dir.dot3(up);
1176  len = up.lengthsqr3();
1177  }
1178  }
1179  up *= 1/MATH_SQRT(len);
1180  RaveVector<T> right = up.cross(dir);
1182  t.m[0] = right.x; t.m[1] = up.x; t.m[2] = dir.x;
1183  t.m[4] = right.y; t.m[5] = up.y; t.m[6] = dir.y;
1184  t.m[8] = right.z; t.m[9] = up.z; t.m[10] = dir.z;
1185  t.trans = vcamerapos;
1186  return t;
1187 }
1188 
1191 template <typename T>
1192 inline int insideQuadrilateral(const RaveVector<T>& v, const RaveVector<T>& verts)
1193 {
1194  RaveVector<T> v4,v5;
1195  T m1,m2;
1196  T anglesum=0,costheta;
1197  for (int i=0; i<4; i++) {
1198  v4.x = verts[i].x - v->x;
1199  v4.y = verts[i].y - v->y;
1200  v4.z = verts[i].z - v->z;
1201  v5.x = verts[(i+1)%4].x - v->x;
1202  v5.y = verts[(i+1)%4].y - v->y;
1203  v5.z = verts[(i+1)%4].z - v->z;
1204  m1 = v4.lengthsqr3();
1205  m2 = v5.lengthsqr3();
1206  if (m1*m2 <= std::numeric_limits<T>::epsilon()*std::numeric_limits<T>::epsilon()) {
1207  return(1); // on a vertex, consider this inside
1208  }
1209  else {
1210  costheta = v4.dot3(v5)/MATH_SQRT(m1*m2);
1211  }
1212  anglesum += MATH_ACOS(costheta);
1213  }
1214  T diff = anglesum - (T)2.0 * M_PI;
1215  return RaveFabs(diff) <= std::numeric_limits<T>::epsilon();
1216 }
1217 
1220 template <typename T>
1221 inline int insideTriangle(const RaveVector<T> v, const triangle<T>& tri)
1222 {
1223  RaveVector<T> v4,v5;
1224  T m1,m2;
1225  T anglesum=0.0;
1226  T costheta;
1227  for (int i=0; i<3; i++) {
1228  v4.x = tri[i].x - v->x;
1229  v4.y = tri[i].y - v->y;
1230  v4.z = tri[i].z - v->z;
1231  v5.x = tri[(i+1)%3].x - v->x;
1232  v5.y = tri[(i+1)%3].y - v->y;
1233  v5.z = tri[(i+1)%3].z - v->z;
1234  m1 = v4.lengthsqr3();
1235  m2 = v5.lengthsqr3();
1236  if (m1*m2 <= std::numeric_limits<T>::epsilon()*std::numeric_limits<T>::epsilon()) {
1237  return(1); // on a vertex, consider this inside
1238  }
1239  else {
1240  costheta = v4.dot3(v5)/MATH_SQRT(m1*m2);
1241  }
1242  anglesum += acos(costheta);
1243  }
1244  T diff = anglesum - (T)2.0 * M_PI;
1245  return RaveFabs(diff) <= std::numeric_limits<T>::epsilon();
1246 }
1247 
1250 template <typename T>
1251 inline bool RayAABBTest(const ray<T>& r, const aabb<T>& ab)
1252 {
1253  RaveVector<T> vd, vpos = r.pos - ab.pos;
1254  if((MATH_FABS(vpos.x) > ab.extents.x)&&(r.dir.x* vpos.x > 0.0f))
1255  return false;
1256  if((MATH_FABS(vpos.y) > ab.extents.y)&&(r.dir.y * vpos.y > 0.0f))
1257  return false;
1258  if((MATH_FABS(vpos.z) > ab.extents.z)&&(r.dir.z * vpos.z > 0.0f))
1259  return false;
1260  vd = r.dir.cross(vpos);
1261  if( MATH_FABS(vd.x) > ab.extents.y * MATH_FABS(r.dir.z) + ab.extents.z * MATH_FABS(r.dir.y) )
1262  return false;
1263  if( MATH_FABS(vd.y) > ab.extents.x * MATH_FABS(r.dir.z) + ab.extents.z * MATH_FABS(r.dir.x) )
1264  return false;
1265  if( MATH_FABS(vd.z) > ab.extents.x * MATH_FABS(r.dir.y) + ab.extents.y * MATH_FABS(r.dir.x) )
1266  return false;
1267  return true;
1268 }
1269 
1272 template <typename T>
1273 inline bool RayOBBTest(const ray<T>& r, const obb<T>& o)
1274 {
1275  RaveVector<T> vpos, vdir, vd;
1276  vd = r.pos - o.pos;
1277  vpos.x = vd.dot3(o.right);
1278  vdir.x = r.dir.dot3(o.right);
1279  if((MATH_FABS(vpos.x) > o.extents.x)&&(vdir.x* vpos.x > 0.0f)) {
1280  return false;
1281  }
1282  vpos.y = vd.dot3(o.up);
1283  vdir.y = r.dir.dot3(o.up);
1284  if((MATH_FABS(vpos.y) > o.extents.y)&&(vdir.y * vpos.y > 0.0f)) {
1285  return false;
1286  }
1287  vpos.z = vd.dot3(o.dir);
1288  vdir.z = r.dir.dot3(o.dir);
1289  if((MATH_FABS(vpos.z) > o.extents.z)&&(vdir.z * vpos.z > 0.0f)) {
1290  return false;
1291  }
1292  cross3(vd, vdir, vpos);
1293  if((MATH_FABS(vd.x) > o.extents.y * MATH_FABS(vdir.z) + o.extents.z * MATH_FABS(vdir.y))||
1294  ( MATH_FABS(vd.y) > o.extents.x * MATH_FABS(vdir.z) + o.extents.z * MATH_FABS(vdir.x)) ||
1295  ( MATH_FABS(vd.z) > o.extents.x * MATH_FABS(vdir.y) + o.extents.y * MATH_FABS(vdir.x)) ) {
1296  return false;
1297  }
1298  return true;
1299 }
1300 
1301 //bool RayFaceTest(const RAY& r, const FACE& f)
1302 //{
1303 // float t = D3DXVec3Dot(&f.plane.vNorm, &(f.v1 - r.vPos) ) / D3DXVec3Dot(&f.plane.vNorm, &r.vDir);
1304 //
1305 // if(t <= 0.0f) return false;
1306 //
1307 // // if e0 = (v2-v1) and e1 = (v3-v1) are the axes, then the point where
1308 // // the ray intersects that plane can be composed from the two
1309 // // if the coefficients are s0 and s1 then s0 & s1 >=0 and s0+s1<=1.0f
1310 // DXVEC3 v = r.vPos + r.vDir * t - f.v1;
1311 //
1312 // float e00, e01, e11, q0, q1, s0, s1;
1313 // e00 = D3DXVec3LengthSq(&(f.v2 - f.v1));
1314 // e01 = D3DXVec3Dot(&(f.v2 - f.v1), &(f.v3 - f.v1));
1315 // e11 = D3DXVec3LengthSq(&(f.v3 - f.v1));
1316 //
1317 // q0 = D3DXVec3Dot(&v, &(f.v2 - f.v1));
1318 // q1 = D3DXVec3Dot(&v, &(f.v3 - f.v1));
1319 //
1320 // s0 = e11 * q0 - e01 * q1;
1321 // s1 = e00 * q1 - e01 * q0;
1322 //
1323 // t = e00 * e11 - e01 * e01;
1324 //
1325 // if( t < -1e-4 ) return s0 < 0.0f && s1 < 0.0f && s0+s1 >= t;
1326 // if( t < 1e-4 ) return false;
1327 //
1328 // return s0 > 0.0f && s1 > 0.0f && s0+s1 <= t;
1329 //}
1330 
1331 //bool RaySphereTest(const RAY& r, const SPHERE& s)
1332 //{
1333 // DXVEC3 v;
1334 // float fLength;
1335 //
1336 // v = s.vPos - r.vPos;
1337 // fLength = D3DXVec3Dot(&v, &r.vDir);
1338 //
1339 // if( D3DXVec3LengthSq(&v) > Sqr(s.fRadius) && fLength < 0.0f )
1340 // return false;
1341 //
1342 // return D3DXVec3LengthSq(&(v - r.vDir * fLength)) < s.fRadius * s.fRadius;
1343 //}
1344 
1345 // 光線と球との交点から球の中心までの距離が半径と等しい
1346 //bool RaySphereTest(const RAY& r, const SPHERE& s, DXVEC3& v)
1347 //{
1348 // // (dir,dir)*t*t + 2(vp,dir)*t + (vp,vp) = r*r
1349 // float fd, f;
1350 //
1351 // v = r.vPos - s.vPos;
1352 // f = D3DXVec3Dot(&v, &r.vDir);
1353 //
1354 // fd = 4.0f * (f * f - D3DXVec3LengthSq(&v) + s.fRadius * s.fRadius);
1355 //
1356 // if( fd < 0.0f ) return false;
1357 //
1358 // fd = sqrtf(fd);
1359 //
1360 // // 最小の数が0より小さいの場合、最大の数を選ぶことにする
1361 //
1362 // if( -2.0f * f - fd > 0.0f ) {
1363 // fd = 0.5f * (-2.0f * f - fd) / D3DXVec3LengthSq(&r.vDir);
1364 // v = r.vPos + fd * r.vDir;
1365 // return true;
1366 // }
1367 //
1368 // if( -2.0f * f + fd > 0.0f ) {
1369 // fd = 0.5f * (-2.0f * f + fd) / D3DXVec3LengthSq(&r.vDir);
1370 // v = r.vPos + fd * r.vDir;
1371 // return true;
1372 // }
1373 //
1374 // return false;
1375 //}
1376 
1377 //bool VertexAABBTest(const DXVEC3& v, const AABB& a)
1378 //{
1379 // return (fabsf(v.x - a.vPos.x) <= a.vExtents.x) &&
1380 // (fabsf(v.y - a.vPos.y) <= a.vExtents.y) &&
1381 // (fabsf(v.z - a.vPos.z) <= a.vExtents.z);
1382 //}
1383 //
1384 //bool VertexOBBTest(const DXVEC3& v, const OBB& o)
1385 //{
1386 // DXVEC3 nv;
1387 // nv = v - o.vPos;
1388 //
1389 // return (fabsf(D3DXVec3Dot(&nv, &o.vRight)) <= o.vExtents.x) &&
1390 // (fabsf(D3DXVec3Dot(&nv, &o.vUp)) <= o.vExtents.y) &&
1391 // (fabsf(D3DXVec3Dot(&nv, &o.vDir)) <= o.vExtents.z);
1392 //}
1393 //
1394 //bool VertexSphereTest(const DXVEC3& v, const SPHERE& s)
1395 //{
1396 // return D3DXVec3LengthSq(&DXVEC3(v-s.vPos)) <= s.fRadius * s.fRadius;
1397 //}
1398 //
1399 //bool VertexConeTest(const DXVEC3& v, CONE& c)
1400 //{
1401 // const DXVEC3& vDir = v - c.vVertex;
1402 // float fDot = D3DXVec3Dot(&vDir, &c.vDir);
1403 //
1404 // return (fDot > 0.0f) && (fDot <= c.fLength) &&
1405 // (fDot * fDot >= c.fCosAng * c.fCosAng * D3DXVec3LengthSq(&vDir));
1406 //}
1407 //
1408 //bool VertexFrustumTest(const DXVEC3& v, const FRUSTUM& fr)
1409 //{
1410 // DXVEC3 vDist = v - fr.vPos;
1411 //
1412 // if(D3DXVec3Dot(&vDist, &(fr.fCosFOVX * fr.vRight - fr.fSinFOVX * fr.vDir)) > 0.0f) return false;
1413 // if(D3DXVec3Dot(&vDist, &(-fr.fCosFOVX * fr.vRight - fr.fSinFOVX * fr.vDir)) > 0.0f) return false;
1414 //
1415 // // top and bottom
1416 // if(D3DXVec3Dot(&vDist, &(fr.fCosFOVY * fr.vUp - fr.fSinFOVY * fr.vDir)) > 0.0f) return false;
1417 // if(D3DXVec3Dot(&vDist, &(-fr.fCosFOVY * fr.vUp - fr.fSinFOVY * fr.vDir)) > 0.0f) return false;
1418 //
1419 // float fDot = vDist.x * fr.vDir.x + vDist.y * fr.vDir.y + vDist.z * fr.vDir.z;
1420 // if( (fDot < fr.fNear) || (fDot > fr.fFar) ) return false;
1421 //
1422 // return true;
1423 //}
1424 
1425 //bool OBBFrustumTest(const OBB& o, const FRUSTUM& fr)
1426 //{
1427 // // check OBB against all 6 planes
1428 // DXVEC3 v = o.vPos - fr.vPos;
1429 //
1430 // // if v lies on the left or bottom sides of the frustrum
1431 // // then freflect about the planes to get it on the right and
1432 // // top sides
1433 //
1434 // // side planes
1435 // DXVEC3 vNorm = fr.fCosFOVX * fr.vRight - fr.fSinFOVX * fr.vDir;
1436 // if(D3DXVec3Dot(&v, &vNorm) > o.vExtents.x * fabsf(D3DXVec3Dot(&vNorm, &o.vRight)) +
1437 // o.vExtents.y * fabsf(D3DXVec3Dot(&vNorm, &o.vUp)) +
1438 // o.vExtents.z * fabsf(D3DXVec3Dot(&vNorm, &o.vDir))) return false;
1439 //
1440 // vNorm = -fr.fCosFOVX * fr.vRight - fr.fSinFOVX * fr.vDir;
1441 // if(D3DXVec3Dot(&v, &vNorm) > o.vExtents.x * fabsf(D3DXVec3Dot(&vNorm, &o.vRight)) +
1442 // o.vExtents.y * fabsf(D3DXVec3Dot(&vNorm, &o.vUp)) +
1443 // o.vExtents.z * fabsf(D3DXVec3Dot(&vNorm, &o.vDir))) return false;
1444 //
1445 // vNorm = fr.fCosFOVY * fr.vUp - fr.fSinFOVY * fr.vDir;
1446 // if(D3DXVec3Dot(&v, &vNorm) > o.vExtents.x * fabsf(D3DXVec3Dot(&vNorm, &o.vRight)) +
1447 // o.vExtents.y * fabsf(D3DXVec3Dot(&vNorm, &o.vUp)) +
1448 // o.vExtents.z * fabsf(D3DXVec3Dot(&vNorm, &o.vDir))) return false;
1449 //
1450 // vNorm = -fr.fCosFOVY * fr.vUp - fr.fSinFOVY * fr.vDir;
1451 // if(D3DXVec3Dot(&v, &vNorm) > o.vExtents.x * fabsf(D3DXVec3Dot(&vNorm, &o.vRight)) +
1452 // o.vExtents.y * fabsf(D3DXVec3Dot(&vNorm, &o.vUp)) +
1453 // o.vExtents.z * fabsf(D3DXVec3Dot(&vNorm, &o.vDir))) return false;
1454 //
1455 // vNorm.x = D3DXVec3Dot(&v, &fr.vDir);
1456 // vNorm.y = o.vExtents.x * fabsf(D3DXVec3Dot(&fr.vDir, &o.vRight)) +
1457 // o.vExtents.y * fabsf(D3DXVec3Dot(&fr.vDir, &o.vUp)) +
1458 // o.vExtents.z * fabsf(D3DXVec3Dot(&fr.vDir, &o.vDir));
1459 //
1460 // if( (vNorm.x < fr.fNear - vNorm.y) || (vNorm.x > fr.fFar + vNorm.y) ) return false;
1461 //
1462 // return true;
1463 //}
1464 
1467 template <typename T>
1468 inline bool IsOBBinFrustum(const obb<T>& o, const frustum<T>& fr)
1469 {
1470  // check OBB against all 6 planes
1471  RaveVector<T> v = o.pos - fr.pos;
1472  // if v lies on the left or bottom sides of the frustrum
1473  // then freflect about the planes to get it on the right and
1474  // top sides
1475  // side planes
1476  RaveVector<T> vNorm = fr.fcosfovx * fr.right - fr.fsinfovx * fr.dir;
1477  if( v.dot3(vNorm) > -o.extents.x * MATH_FABS(vNorm.dot3(o.right)) - o.extents.y * MATH_FABS(vNorm.dot3(o.up)) - o.extents.z * MATH_FABS(vNorm.dot3(o.dir))) {
1478  return false;
1479  }
1480  vNorm = -fr.fcosfovx * fr.right - fr.fsinfovx * fr.dir;
1481  if(dot3(v, vNorm) > -o.extents.x * MATH_FABS(vNorm.dot3(o.right)) - o.extents.y * MATH_FABS(vNorm.dot3(o.up)) - o.extents.z * MATH_FABS(vNorm.dot3(o.dir))) {
1482  return false;
1483  }
1484  vNorm = fr.fcosfovy * fr.up - fr.fsinfovy * fr.dir;
1485  if(dot3(v, vNorm) > -o.extents.x * MATH_FABS(vNorm.dot3(o.right)) - o.extents.y * MATH_FABS(vNorm.dot3(o.up)) - o.extents.z * MATH_FABS(vNorm.dot3(o.dir))) {
1486  return false;
1487  }
1488  vNorm = -fr.fcosfovy * fr.up - fr.fsinfovy * fr.dir;
1489  if(dot3(v, vNorm) > -o.extents.x * MATH_FABS(vNorm.dot3(o.right)) - o.extents.y * MATH_FABS(vNorm.dot3(o.up)) - o.extents.z * MATH_FABS(vNorm.dot3(o.dir))) {
1490  return false;
1491  }
1492  vNorm.x = v.dot3(fr.dir);
1493  vNorm.y = o.extents.x * MATH_FABS(fr.dir.dot3(o.right)) + o.extents.y * MATH_FABS(fr.dir.dot3(o.up)) + o.extents.z * MATH_FABS(fr.dir.dot3(o.dir));
1494  if( (vNorm.x < fr.fnear + vNorm.y) || (vNorm.x > fr.ffar - vNorm.y) ) {
1495  return false;
1496  }
1497  return true;
1498 }
1499 
1500 //bool AABBFrustumTest(const AABB& a, const FRUSTUM& fr)
1501 //{
1502 // // check AABB against all 6 planes
1503 // DXVEC3 v = a.vPos - fr.vPos;
1504 //
1505 // // if v lies on the left or bottom sides of the frustrum
1506 // // then freflect about the planes to get it on the right and
1507 // // top sides
1508 //
1509 // // side planes
1510 // DXVEC3 vNorm = fr.fCosFOVX * fr.vRight - fr.fSinFOVX * fr.vDir;
1511 // if(D3DXVec3Dot(&v, &vNorm) > a.vExtents.x * fabsf(vNorm.x) +
1512 // a.vExtents.y * fabsf(vNorm.y) + a.vExtents.z * fabsf(vNorm.z)) return false;
1513 //
1514 // vNorm = -fr.fCosFOVX * fr.vRight - fr.fSinFOVX * fr.vDir;
1515 // if(D3DXVec3Dot(&v, &vNorm) > a.vExtents.x * fabsf(vNorm.x) +
1516 // a.vExtents.y * fabsf(vNorm.y) + a.vExtents.z * fabsf(vNorm.z)) return false;
1517 //
1518 // vNorm = fr.fCosFOVY * fr.vUp - fr.fSinFOVY * fr.vDir;
1519 // if(D3DXVec3Dot(&v, &vNorm) > a.vExtents.x * fabsf(vNorm.x) +
1520 // a.vExtents.y * fabsf(vNorm.y) + a.vExtents.z * fabsf(vNorm.z)) return false;
1521 //
1522 // vNorm = -fr.fCosFOVY * fr.vUp - fr.fSinFOVY * fr.vDir;
1523 // if(D3DXVec3Dot(&v, &vNorm) > a.vExtents.x * fabsf(vNorm.x) +
1524 // a.vExtents.y * fabsf(vNorm.y) + a.vExtents.z * fabsf(vNorm.z)) return false;
1525 //
1526 // vNorm.x = D3DXVec3Dot(&v, &fr.vDir);
1527 // vNorm.y = a.vExtents.x * fabsf(fr.vDir.x) + a.vExtents.y * fabsf(fr.vDir.y) +
1528 // a.vExtents.z * fabsf(fr.vDir.z);
1529 //
1530 // if( (vNorm.x < fr.fNear - vNorm.y) || (vNorm.x > fr.fFar + vNorm.y) ) return false;
1531 //
1532 // return true;
1533 //}
1534 
1539 template <typename T, typename U>
1540 inline bool IsOBBinConvexHull(const obb<T>& o, const U& vplanes)
1541 {
1542  for(size_t i = 0; i < vplanes.size(); ++i) {
1543  RaveVector<T> vplane = vplanes[i];
1544  if( o.pos.dot3(vplane)+vplane.w < o.extents.x * MATH_FABS(vplane.dot3(o.right)) + o.extents.y * MATH_FABS(vplane.dot3(o.up)) + o.extents.z * MATH_FABS(vplane.dot3(o.dir))) {
1545  return false;
1546  }
1547  }
1548  return true;
1549 }
1550 
1551 //bool SphereSphereTest(const SPHERE& s1, const SPHERE& s2)
1552 //{
1553 // return ( (s1.fRadius + s2.fRadius) * (s1.fRadius + s2.fRadius) >
1554 // D3DXVec3LengthSq(&DXVEC3(s1.vPos - s2.vPos)) );
1555 //}
1556 //
1557 //bool SphereConeTest(const SPHERE& s, const CONE& c)
1558 //{
1559 // // first check if SPHERE is anywhere near the vertes
1560 // DXVEC3 v = s.vPos - c.vVertex;
1561 // float fLengthSq = D3DXVec3LengthSq(&v);
1562 //
1563 // if( fLengthSq < s.fRadius * s.fRadius ) return true;
1564 //
1565 // // check if SPHERE center inside CONE
1566 // float fDot = D3DXVec3Dot(&v, &c.vDir);
1567 // if(fDot < 0.0f || fDot > c.fLength + s.fRadius) return false;
1568 //
1569 // float fDotSq = fDot * fDot;
1570 // if(fDotSq > c.fCosAng * c.fCosAng * fLengthSq) return true;
1571 //
1572 // // SPHERE can touch edge of CONE
1573 // // take the PLANE with the SPHERE center and vDir. From there
1574 // // a right triangle can form, with fLength as the hypotenuse,
1575 // // the CONE's edge as one of the sides, and a perpendicular line
1576 // // from the CONE's edge to the SPHERE's center. If we can figure out
1577 // // the distance along the CONE's edge, then we can measure if a valid
1578 // // triangle forms
1579 //
1580 // float fTest = c.fSinAng * sqrtf(fLengthSq - fDotSq) + c.fCosAng * fDot;
1581 //
1582 // float fSide = c.fLength / c.fCosAng;
1583 // if( fTest > fSide ) {
1584 // // beyond the base of the cone, so check edge
1585 // fLengthSq -= fTest * fTest;
1586 // fTest -= fSide;
1587 // return fLengthSq + fTest*fTest < s.fRadius * s.fRadius;
1588 // }
1589 //
1590 // return (fTest * fTest + s.fRadius * s.fRadius - fLengthSq) >= 0.0f;
1591 //}
1592 //
1593 //bool SphereFrustumTest(const SPHERE& s, const FRUSTUM& fr)
1594 //{
1595 // DXVEC3 v = s.vPos - fr.vPos;
1596 // DXVEC3 vproj;
1597 // vproj.x = D3DXVec3Dot(&v, &fr.vRight);
1598 // vproj.y = D3DXVec3Dot(&v, &fr.vUp);
1599 // vproj.z = D3DXVec3Dot(&v, &fr.vDir);
1600 //
1601 // if( vproj.x * fr.fCosFOVX - vproj.z * fr.fSinFOVX > s.fRadius ||
1602 // -vproj.x * fr.fCosFOVX - vproj.z * fr.fSinFOVX > s.fRadius ||
1603 // vproj.y * fr.fCosFOVY - vproj.z * fr.fSinFOVY > s.fRadius ||
1604 // vproj.y * fr.fCosFOVY - vproj.z * fr.fSinFOVY > s.fRadius ||
1605 // vproj.z < fr.fNear - s.fRadius || vproj.z > fr.fFar + s.fRadius ) return false;
1606 //
1607 // return true;
1608 //}
1609 
1616 template <typename T>
1617 inline bool TriTriCollision(const RaveVector<T>& u1, const RaveVector<T>& u2, const RaveVector<T>& u3, const RaveVector<T>& v1, const RaveVector<T>& v2, const RaveVector<T>& v3, RaveVector<T>& contactpos, RaveVector<T>& contactnorm)
1618 {
1619  // triangle triangle collision test - by Rosen Diankov
1620 
1621  // first see if the faces intersect the planes
1622  // for the face to be intersecting the plane, one of its
1623  // vertices must be on the opposite side of the plane
1624  char b = 0;
1625  RaveVector<T> u12 = u2 - u1, u31 = u1 - u3;
1626  RaveVector<T> v12 = v2 - v1, v23 = v3 - v2, v31 = v1 - v3;
1627  RaveVector<T> vedges[3] = { v12, v23, v31};
1628  RaveVector<T> unorm, vnorm;
1629  unorm = u31.cross(u12);
1630  unorm.w = -unorm.dot3(u1);
1631  vnorm = v31.cross(v12);
1632  vnorm.w = -vnorm.dot3(v1);
1633  if( vnorm.dot3(u1) + vnorm.w > 0 ) {
1634  b |= 1;
1635  }
1636  if( vnorm.dot3(u2) + vnorm.w > 0 ) {
1637  b |= 2;
1638  }
1639  if( vnorm.dot3(u3) + vnorm.w > 0 ) {
1640  b |= 4;
1641  }
1642  if((b == 7)||(b == 0)) {
1643  return false;
1644  }
1645  // now get segment from f1 when it crosses f2's plane
1646  // note that b gives us information on which edges actually intersected
1647  // so figure out the point that is alone on one side of the plane
1648  // then get the segment
1649  RaveVector<T> p1, p2;
1650  const RaveVector<T>* pu=NULL;
1651  switch(b) {
1652  case 1:
1653  case 6:
1654  pu = &u1;
1655  p1 = u2 - u1;
1656  p2 = u3 - u1;
1657  break;
1658  case 2:
1659  case 5:
1660  pu = &u2;
1661  p1 = u1 - u2;
1662  p2 = u3 - u2;
1663  break;
1664  case 4:
1665  case 3:
1666  pu = &u3;
1667  p1 = u1 - u3;
1668  p2 = u2 - u3;
1669  break;
1670  }
1671 
1672  T t = vnorm.dot3(*pu)+vnorm.w;
1673  p1 = *pu - p1 * (t / vnorm.dot3(p1));
1674  p2 = *pu - p2 * (t / vnorm.dot3(p2));
1675 
1676  // go through each of the segments in v2 and clip
1677  RaveVector<T> vcross;
1678  const RaveVector<T>* pv[] = { &v1, &v2, &v3, &v1};
1679 
1680  for(int i = 0; i < 3; ++i) {
1681  const RaveVector<T>* pprev = pv[i];
1682  RaveVector<T> q1 = p1 - *pprev;
1683  RaveVector<T> q2 = p2 - *pprev;
1684  vcross = vedges[i].cross(vnorm);
1685  T t1 = q1.dot3(vcross);
1686  T t2 = q2.dot3(vcross);
1687 
1688  // line segment is out of face
1689  if((t1 >= 0)&&(t2 >= 0)) {
1690  return false;
1691  }
1692  if((t1 > 0)&&(t2 < 0)) {
1693  // keep second point, clip first
1694  RaveVector<T> dq = q2-q1;
1695  p1 -= dq*(t1/dq.dot3(vcross));
1696  }
1697  else if((t1 < 0)&&(t2 > 0)) {
1698  // keep first point, clip second
1699  RaveVector<T> dq = q1-q2;
1700  p2 -= dq*(t2/dq.dot3(vcross));
1701  }
1702  }
1703 
1704  contactpos = 0.5f * (p1 + p2);
1705  contactnorm = vnorm.normalize3();
1706  return true;
1707 }
1708 
1713 template <typename T>
1715 {
1716  obb<T> o;
1717  o.right = RaveVector<T>(t.m[0],t.m[4],t.m[8]);
1718  o.up = RaveVector<T>(t.m[1],t.m[5],t.m[9]);
1719  o.dir = RaveVector<T>(t.m[2],t.m[6],t.m[10]);
1720  o.pos = t*ab.pos;
1721  o.extents = ab.extents;
1722  return o;
1723 }
1724 
1729 template <typename T>
1730 inline obb<T> OBBFromAABB(const aabb<T>& ab, const RaveTransform<T>& t)
1731 {
1732  return OBBFromAABB(ab,RaveTransformMatrix<T>(t));
1733 }
1734 
1739 template <typename T>
1740 inline obb<T> TransformOBB(const RaveTransform<T>& t, const obb<T>& o)
1741 {
1742  obb<T> newobb;
1743  newobb.extents = o.extents;
1744  newobb.pos = t*o.pos;
1745  newobb.right = t.rotate(o.right);
1746  newobb.up = t.rotate(o.up);
1747  newobb.dir = t.rotate(o.dir);
1748  return newobb;
1749 }
1750 
1755 template <typename T>
1757 {
1758  obb<T> newobb;
1759  newobb.extents = o.extents;
1760  newobb.pos = t*o.pos;
1761  newobb.right = t.rotate(o.right);
1762  newobb.up = t.rotate(o.up);
1763  newobb.dir = t.rotate(o.dir);
1764  return newobb;
1765 }
1766 
1768 //void AABBFromOBB(const OBB& obb, DXVEC3& vMin, DXVEC3& vMax)
1769 //{
1770 // vMax.x = fabsf(obb.vRight.x) * obb.vExtents.x + fabsf(obb.vUp.x) * obb.vExtents.y + fabsf(obb.vDir.x) * obb.vExtents.z;
1771 // vMax.y = fabsf(obb.vRight.y) * obb.vExtents.x + fabsf(obb.vUp.y) * obb.vExtents.y + fabsf(obb.vDir.y) * obb.vExtents.z;
1772 // vMax.z = fabsf(obb.vRight.z) * obb.vExtents.x + fabsf(obb.vUp.z) * obb.vExtents.y + fabsf(obb.vDir.z) * obb.vExtents.z;
1773 //
1774 // vMin = obb.vPos - vMax;
1775 // vMax += obb.vPos;
1776 //}
1777 
1781 template <typename T>
1782 inline bool AABBCollision(const aabb<T>& ab1, const aabb<T>& ab2)
1783 {
1784  RaveVector<T> v = ab1.pos-ab2.pos;
1785  return MATH_FABS(v.x) <= ab1.extents.x+ab2.extents.x && MATH_FABS(v.y) <= ab1.extents.y+ab2.extents.y && MATH_FABS(v.z) <= ab1.extents.z+ab2.extents.z;
1786 }
1787 
1788 //bool AABBOBBTest(const AABB& a, const OBB& o)
1789 //{
1790 // DXVEC3 vd = o.vPos - a.vPos;
1791 // float r01, r;
1792 //
1793 // // test the 3 axes of the AABB
1794 //
1795 // // A0
1796 // if(a.vExtents.x + o.vExtents.x * fabsf(o.vRight.x) + o.vExtents.y * fabsf(o.vUp.x) +
1797 // o.vExtents.z * fabsf(o.vDir.x) < fabsf(vd.x)) return false;
1798 // // A1
1799 // if(a.vExtents.y + o.vExtents.x * fabsf(o.vRight.y) + o.vExtents.y * fabsf(o.vUp.y) +
1800 // o.vExtents.z * fabsf(o.vDir.y) < fabsf(vd.y)) return false;
1801 // // A2
1802 // if(a.vExtents.z + o.vExtents.x * fabsf(o.vRight.z) + o.vExtents.y * fabsf(o.vUp.z) +
1803 // o.vExtents.z * fabsf(o.vDir.z) < fabsf(vd.z)) return false;
1804 //
1805 // // test the 3 axes of the OBB
1806 //
1807 // // B0
1808 // if(a.vExtents.x * fabsf(o.vRight.x) + a.vExtents.y * fabsf(o.vRight.y) +
1809 // a.vExtents.z * fabsf(o.vRight.z) + o.vExtents.x <
1810 // fabsf(D3DXVec3Dot(&o.vRight, &vd)) ) return false;
1811 // // B1
1812 // if(a.vExtents.x * fabsf(o.vUp.x) + a.vExtents.y * fabsf(o.vUp.y) +
1813 // a.vExtents.z * fabsf(o.vUp.z) + o.vExtents.y <
1814 // fabsf(D3DXVec3Dot(&o.vUp, &vd)) ) return false;
1815 // // B2
1816 // if(a.vExtents.x * fabsf(o.vDir.x) + a.vExtents.y * fabsf(o.vDir.y) +
1817 // a.vExtents.z * fabsf(o.vDir.z) + o.vExtents.z <
1818 // fabsf(D3DXVec3Dot(&o.vDir, &vd)) ) return false;
1819 //
1820 // // test the 9 different cross products (from different combinations of the axes)
1821 // // A0 x B0 (0, -b0.z, b0.y)
1822 // r01 = a.vExtents.y * fabsf(o.vRight.z) + a.vExtents.z * fabsf(o.vRight.y) +
1823 // o.vExtents.y * fabsf(o.vDir.x) + o.vExtents.z * fabsf(o.vUp.x);
1824 // r = fabsf( o.vRight.y * vd.z - o.vRight.z * vd.y);
1825 // if(r01 < r) return false;
1826 //
1827 // // A0 x B1 (0, -b1.z, b1.y)
1828 // r01 = a.vExtents.y * fabsf(o.vUp.z) + a.vExtents.z * fabsf(o.vUp.y) +
1829 // o.vExtents.x * fabsf(o.vDir.x) + o.vExtents.z * fabsf(o.vRight.x);
1830 // r = fabsf( o.vUp.y * vd.z - o.vUp.z * vd.y);
1831 // if(r01 < r) return false;
1832 //
1833 // // A0 x B2 (0, -b2.z, b2.y)
1834 // r01 = a.vExtents.y * fabsf(o.vDir.z) + a.vExtents.z * fabsf(o.vDir.y) +
1835 // o.vExtents.x * fabsf(o.vUp.x) + o.vExtents.y * fabsf(o.vRight.x);
1836 // r = fabsf( o.vDir.y * vd.z - o.vDir.z * vd.y);
1837 // if(r01 < r) return false;
1838 //
1839 // // A1 x B0 (b0.z, 0, -b0.x)
1840 // r01 = a.vExtents.x * fabsf(o.vRight.z) + a.vExtents.z * fabsf(o.vRight.x) +
1841 // o.vExtents.y * fabsf(o.vDir.y) + o.vExtents.z * fabsf(o.vUp.y);
1842 // r = fabsf( o.vRight.z * vd.x - o.vRight.x * vd.z);
1843 // if(r01 < r) return false;
1844 //
1845 // // A1 x B1 (b1.z, 0, -b1.x)
1846 // r01 = a.vExtents.x * fabsf(o.vUp.z) + a.vExtents.z * fabsf(o.vUp.x) +
1847 // o.vExtents.x * fabsf(o.vDir.y) + o.vExtents.z * fabsf(o.vRight.y);
1848 // r = fabsf( o.vUp.z * vd.x - o.vUp.x * vd.z);
1849 // if(r01 < r) return false;
1850 //
1851 // // A1 x B2 (b2.z, 0, -b2.x)
1852 // r01 = a.vExtents.x * fabsf(o.vDir.z) + a.vExtents.z * fabsf(o.vDir.x) +
1853 // o.vExtents.x * fabsf(o.vUp.y) + o.vExtents.y * fabsf(o.vRight.y);
1854 // r = fabsf( o.vDir.z * vd.x - o.vDir.x * vd.z);
1855 // if(r01 < r) return false;
1856 //
1857 // // A2 x B0 (-b0.y, b0.x, 0)
1858 // r01 = a.vExtents.x * fabsf(o.vRight.y) + a.vExtents.y * fabsf(o.vRight.x) +
1859 // o.vExtents.y * fabsf(o.vDir.z) + o.vExtents.z * fabsf(o.vUp.z);
1860 // r = fabsf( o.vRight.x * vd.y - o.vRight.y * vd.x);
1861 // if(r01 < r) return false;
1862 //
1863 // // A2 x B1 (-b1.y, b1.x, 0)
1864 // r01 = a.vExtents.x * fabsf(o.vUp.y) + a.vExtents.y * fabsf(o.vUp.x) +
1865 // o.vExtents.x * fabsf(o.vDir.z) + o.vExtents.z * fabsf(o.vRight.z);
1866 // r = fabsf( o.vUp.x * vd.y - o.vUp.y * vd.x);
1867 // if(r01 < r) return false;
1868 //
1869 // // A2 x B2 (-b2.y, b2.x, 0)
1870 // r01 = a.vExtents.x * fabsf(o.vDir.y) + a.vExtents.y * fabsf(o.vDir.x) +
1871 // o.vExtents.x * fabsf(o.vUp.z) + o.vExtents.y * fabsf(o.vRight.z);
1872 // r = fabsf( o.vDir.x * vd.y - o.vDir.y * vd.x);
1873 // if(r01 < r) return false;
1874 //
1875 // return true;
1876 //}
1877 //
1878 //bool OBBOBBTest(const OBB& o1, const OBB& o2)
1879 //{
1880 // // convert to AABB and OBB
1881 // AABB ab(DXVEC3(0.0f, 0.0f, 0.0f), o1.vExtents);
1882 // OBB obb;
1883 //
1884 // // position
1885 // ab.vPos = o2.vPos - o1.vPos;
1886 // obb.vPos.x = D3DXVec3Dot(&o1.vRight, &ab.vPos);
1887 // obb.vPos.y = D3DXVec3Dot(&o1.vUp, &ab.vPos);
1888 // obb.vPos.z = D3DXVec3Dot(&o1.vDir, &ab.vPos);
1889 // ab.vPos.x = ab.vPos.y = ab.vPos.z = 0.0f;
1890 //
1891 // obb.vRight.x = D3DXVec3Dot(&o1.vRight, &o2.vRight);
1892 // obb.vRight.y = D3DXVec3Dot(&o1.vUp, &o2.vRight);
1893 // obb.vRight.z = D3DXVec3Dot(&o1.vDir, &o2.vRight);
1894 //
1895 // obb.vUp.x = D3DXVec3Dot(&o1.vRight, &o2.vUp);
1896 // obb.vUp.y = D3DXVec3Dot(&o1.vUp, &o2.vUp);
1897 // obb.vUp.z = D3DXVec3Dot(&o1.vDir, &o2.vUp);
1898 //
1899 // obb.vDir.x = D3DXVec3Dot(&o1.vRight, &o2.vDir);
1900 // obb.vDir.y = D3DXVec3Dot(&o1.vUp, &o2.vDir);
1901 // obb.vDir.z = D3DXVec3Dot(&o1.vDir, &o2.vDir);
1902 //
1903 // obb.vExtents = o2.vExtents;
1904 //
1905 // return AABBOBBTest(ab, obb);
1906 //}
1907 
1908 //AABB AABBUnion(const AABB& ab1, AABB& ab2)
1909 //{
1910 // return AABB( 0.5f * (ab1.vPos + ab2.vPos), 0.5f * (ab1.vExtents + ab2.vExtents +
1911 // DXVEC3(fabsf(ab2.vPos.x-ab1.vPos.x), fabsf(ab2.vPos.y-ab1.vPos.y), fabsf(ab2.vPos.z-ab1.vPos.z))) );
1912 //}
1913 
1914 //bool PlaneFaceTest(const PLANE& p, const FACE& f)
1915 //{
1916 // // for the face to be intersecting the plane, one of its
1917 // // vertices must be on the opposite side of the plane
1918 // BYTE b = 0;
1919 //
1920 // if(D3DXVec3Dot(&p.vNorm, &f.v1) + p.fD > 0.0f) b |= 1;
1921 // if(D3DXVec3Dot(&p.vNorm, &f.v2) + p.fD > 0.0f) b |= 2;
1922 // if(D3DXVec3Dot(&p.vNorm, &f.v3) + p.fD > 0.0f) b |= 4;
1923 //
1924 // if(b == 7 || b == 0) return false;
1925 //
1926 // return true;
1927 //}
1928 //
1929 //bool PlaneAABBTest(const PLANE& p, const AABB& a)
1930 //{
1931 // return fabsf(D3DXVec3Dot(&p.vNorm, &a.vPos) + p.fD) <
1932 // fabsf(D3DXVec3Dot(&p.vNorm, &a.vExtents));
1933 //}
1934 //
1935 //bool PlaneOBBTest(const PLANE& p, const OBB& o)
1936 //{
1937 // return fabsf(D3DXVec3Dot(&p.vNorm, &o.vPos) + p.fD) <
1938 // fabsf(o.vExtents.x * D3DXVec3Dot(&p.vNorm, &o.vRight)) +
1939 // fabsf(o.vExtents.y * D3DXVec3Dot(&p.vNorm, &o.vUp)) +
1940 // fabsf(o.vExtents.z * D3DXVec3Dot(&p.vNorm, &o.vDir));
1941 //}
1942 //
1943 //bool PlaneSphereTest(const PLANE& p, const SPHERE& s)
1944 //{
1945 // return fabsf(D3DXVec3Dot(&p.vNorm, &s.vPos) + p.fD) < s.fRadius;
1946 //}
1947 //
1948 //bool FaceFaceTest(const FACE& f1, const FACE& f2)
1949 //{
1950 // // first see if the faces intersect the planes
1951 // // for the face to be intersecting the plane, one of its
1952 // // vertices must be on the opposite side of the plane
1953 // BYTE b = 0;
1954 //
1955 // if(D3DXVec3Dot(&f2.plane.vNorm, &f1.v1) + f2.plane.fD > 0.0f) b |= 1;
1956 // if(D3DXVec3Dot(&f2.plane.vNorm, &f1.v2) + f2.plane.fD > 0.0f) b |= 2;
1957 // if(D3DXVec3Dot(&f2.plane.vNorm, &f1.v3) + f2.plane.fD > 0.0f) b |= 4;
1958 //
1959 // if(b == 7 || b == 0) return false;
1960 //
1961 // // now get segment from f1 when it crosses f2's plane
1962 // // note that b gives us information on which edges actually intersected
1963 // // so figure out the point that is alone on one side of the plane
1964 // // then get the segment
1965 // DXVEC3 p1, p2;
1966 // const DXVEC3* v;
1967 //
1968 // switch(b) {
1969 // case 1:
1970 // case 6:
1971 // v = &f1.v1;
1972 // p1 = f1.v2 - f1.v1;
1973 // p2 = f1.v3 - f1.v1;
1974 // break;
1975 // case 2:
1976 // case 5:
1977 // v = &f1.v2;
1978 // p1 = f1.v1 - f1.v2;
1979 // p2 = f1.v3 - f1.v2;
1980 // break;
1981 // case 4:
1982 // case 3:
1983 // v = &f1.v3;
1984 // p1 = f1.v1 - f1.v3;
1985 // p2 = f1.v2 - f1.v3;
1986 // break;
1987 // }
1988 //
1989 // float t = D3DXVec3Dot(&f2.plane.vNorm, &(f2.v1 - *v) );
1990 // p1 = *v + p1 * t / D3DXVec3Dot(&f2.plane.vNorm, &p1);
1991 // p2 = *v + p2 * t / D3DXVec3Dot(&f2.plane.vNorm, &p2);
1992 //
1993 // // now p1 and p2 are on the plane
1994 // // let p1 be the midpoint and p2 be the extents
1995 // // then test the line segment with the triangle against
1996 // // the 4 separating axes
1997 // p1 = (p2 + p1) * 0.5f;
1998 // p2 -= p1;
1999 //
2000 // DXVEC3 vCross;
2001 //
2002 // // N x U
2003 // D3DXVec3Cross(&vCross, &f2.plane.vNorm, &p2);
2004 // t = D3DXVec3Dot(&vCross, &p1);
2005 //
2006 // b = D3DXVec3Dot(&vCross, &f2.v2) < t;
2007 //
2008 // // check if all 3 verts are on one side and don't intersect the edge
2009 // if( ( (D3DXVec3Dot(&vCross, &f2.v1) > t) ^ b ) &&
2010 // ( (D3DXVec3Dot(&vCross, &f2.v3) > t) ^ b ) ) return false;
2011 //
2012 // // N x V2-V1
2013 //
2014 // // test: check if projection of segment onto vector
2015 // // is not in the interval of the projection of f2
2016 // float fd1, fd2, ftemp;
2017 //
2018 // D3DXVec3Cross(&vCross, &f2.plane.vNorm, &(f2.v2 - f2.v1));
2019 // t = D3DXVec3Dot(&vCross, &f2.v3);
2020 //
2021 // ftemp = D3DXVec3Dot(&vCross, &p2);
2022 // fd1 = D3DXVec3Dot(&vCross, &p1) - ftemp;
2023 // fd2 = fd1 + 2.0f * ftemp;
2024 //
2025 // ftemp = D3DXVec3Dot(&vCross, &f2.v1);
2026 //
2027 // if( (fd1 < ftemp && fd2 < ftemp) || (fd1 > t && fd2 > t) ) return false;
2028 //
2029 // // N x V3-V1
2030 // D3DXVec3Cross(&vCross, &f2.plane.vNorm, &(f2.v1 - f2.v3));
2031 // t = fabsf(D3DXVec3Dot(&vCross, &f2.v2));
2032 //
2033 // ftemp = D3DXVec3Dot(&vCross, &p2);
2034 // fd1 = D3DXVec3Dot(&vCross, &p1) - ftemp;
2035 // fd2 = fd1 + 2.0f * ftemp;
2036 //
2037 // ftemp = D3DXVec3Dot(&vCross, &f2.v3);
2038 //
2039 // if( (fd1 < ftemp && fd2 < ftemp) || (fd1 > t && fd2 > t) ) return false;
2040 //
2041 // // N x V3-V2
2042 // D3DXVec3Cross(&vCross, &f2.plane.vNorm, &(f2.v3 - f2.v2));
2043 // t = fabsf(D3DXVec3Dot(&vCross, &f2.v1));
2044 //
2045 // ftemp = D3DXVec3Dot(&vCross, &p2);
2046 // fd1 = D3DXVec3Dot(&vCross, &p1) - ftemp;
2047 // fd2 = fd1 + 2.0f * ftemp;
2048 //
2049 // ftemp = D3DXVec3Dot(&vCross, &f2.v2);
2050 //
2051 // if( (fd1 < ftemp && fd2 < ftemp) || (fd1 > t && fd2 > t) ) return false;
2052 //
2053 // return true;
2054 //}
2055 
2056 //bool FaceAABBTest(const FACE& f, const AABB& a)
2057 //{
2058 // // use 13 separating axes to determine collision
2059 // DXVEC3 vD;
2060 // DXVEC3 ve0, ve1, ve2;
2061 // DXVEC3 vNorm; // the cross product between ve0 and ve1, but preserve length!
2062 //
2063 // float p0, p1, p2, r;
2064 //
2065 // // N
2066 // vD = f.v1 - a.vPos;
2067 // if( fabsf(D3DXVec3Dot(&vD, &f.plane.vNorm)) >
2068 // a.vExtents.x * fabsf(f.plane.vNorm.x) + a.vExtents.y * fabsf(f.plane.vNorm.y) +
2069 // a.vExtents.z * fabsf(f.plane.vNorm.z) ) return false;
2070 //
2071 // ve0 = f.v2 - f.v1;
2072 // ve1 = f.v3 - f.v1;
2073 // ve2 = f.v3 - f.v2;
2074 //
2075 // // box's axes
2076 // p0 = vD.x;
2077 // p1 = vD.x + ve0.x;
2078 // p2 = vD.x + ve1.x;
2079 // if( p0 > a.vExtents.x) {
2080 // if(p1 > a.vExtents.x && p2 > a.vExtents.x) return false;
2081 // }
2082 // else if (p0 < -a.vExtents.x) {
2083 // if(p1 < -a.vExtents.x && p2 < -a.vExtents.x) return false;
2084 // }
2085 //
2086 // p0 = vD.y;
2087 // p1 = vD.y + ve0.y;
2088 // p2 = vD.y + ve1.y;
2089 // if( p0 > a.vExtents.y) {
2090 // if(p1 > a.vExtents.y && p2 > a.vExtents.y) return false;
2091 // }
2092 // else if (p0 < -a.vExtents.y) {
2093 // if(p1 < -a.vExtents.y && p2 < -a.vExtents.y) return false;
2094 // }
2095 //
2096 // p0 = vD.z;
2097 // p1 = vD.z + ve0.z;
2098 // p2 = vD.z + ve1.z;
2099 // if( p0 > a.vExtents.z) {
2100 // if(p1 > a.vExtents.z && p2 > a.vExtents.z) return false;
2101 // }
2102 // else if (p0 < -a.vExtents.z) {
2103 // if(p1 < -a.vExtents.z && p2 < -a.vExtents.z) return false;
2104 // }
2105 //
2106 // D3DXVec3Cross(&vNorm, &ve0, &ve1);
2107 //
2108 // // all the cross products between
2109 // // ve0, ve1, ve2 and A0(1,0,0), A1(0, 1, 0), A2(0, 0, 1)
2110 //
2111 // // for all axes L = Ai x vei, compute vD dot L and
2112 // // the projections of the triangle vertices
2113 //
2114 // // A0 x ve0
2115 // // (0, -ve.z, ve.y)
2116 //
2117 // r = a.vExtents.y * fabsf(ve0.z) + a.vExtents.z * fabsf(ve0.y);
2118 // p0 = -ve0.z * vD.y + ve0.y * vD.z;
2119 // p1 = p0 + vNorm.x;
2120 // if( (p0 > r && p1 > r) || (p0 < -r && p1 < -r) ) return false;
2121 //
2122 // // A0 x ve1
2123 //
2124 // r = a.vExtents.y * fabsf(ve1.z) + a.vExtents.z * fabsf(ve1.y);
2125 // p0 = -ve1.z * vD.y + ve1.y * vD.z;
2126 // p1 = p0 - vNorm.x;
2127 // if( (p0 > r && p1 > r) || (p0 < -r && p1 < -r) ) return false;
2128 //
2129 // // A0 x ve2
2130 //
2131 // r = a.vExtents.y * fabsf(ve2.z) + a.vExtents.z * fabsf(ve2.y);
2132 // p0 = -ve2.z * vD.y + ve2.y * vD.z;
2133 // p1 = p0 - vNorm.x;
2134 // if( (p0 > r && p1 > r) || (p0 < -r && p1 < -r) ) return false;
2135 //
2136 // // A1 x ve0
2137 // // (ve.z, 0, -ve.x)
2138 //
2139 // r = a.vExtents.x * fabsf(ve0.z) + a.vExtents.z * fabsf(ve0.x);
2140 // p0 = ve0.z * vD.x - ve0.x * vD.z;
2141 // p1 = p0 + vNorm.y;
2142 // if( (p0 > r && p1 > r) || (p0 < -r && p1 < -r) ) return false;
2143 //
2144 // // A1 x ve1
2145 // r = a.vExtents.x * fabsf(ve1.z) + a.vExtents.z * fabsf(ve1.x);
2146 // p0 = ve1.z * vD.x - ve1.x * vD.z;
2147 // p1 = p0 - vNorm.y;
2148 // if( (p0 > r && p1 > r) || (p0 < -r && p1 < -r) ) return false;
2149 //
2150 // // A1 x ve2
2151 //
2152 // r = a.vExtents.x * fabsf(ve2.z) + a.vExtents.z * fabsf(ve2.x);
2153 // p0 = ve2.z * vD.x - ve2.x * vD.z;
2154 // p1 = p0 - vNorm.y;
2155 // if( (p0 > r && p1 > r) || (p0 < -r && p1 < -r) ) return false;
2156 //
2157 // // A2 x ve0
2158 // // (-ve.y, ve.x, 0)
2159 //
2160 // r = a.vExtents.x * fabsf(ve0.y) + a.vExtents.y * fabsf(ve0.x);
2161 // p0 = - ve0.y * vD.x + ve0.x * vD.y;
2162 // p1 = p0 + vNorm.z;
2163 // if( (p0 > r && p1 > r) || (p0 < -r && p1 < -r) ) return false;
2164 //
2165 // // A2 x ve1
2166 // r = a.vExtents.x * fabsf(ve1.y) + a.vExtents.y * fabsf(ve1.x);
2167 // p0 = - ve1.y * vD.x + ve1.x * vD.y;
2168 // p1 = p0 - vNorm.z;
2169 // if( (p0 > r && p1 > r) || (p0 < -r && p1 < -r) ) return false;
2170 //
2171 // // A2 x ve2
2172 // r = a.vExtents.x * fabsf(ve2.y) + a.vExtents.y * fabsf(ve2.x);
2173 // p0 = - ve2.y * vD.x + ve2.x * vD.y;
2174 // p1 = p0 - vNorm.z;
2175 // if( (p0 > r && p1 > r) || (p0 < -r && p1 < -r) ) return false;
2176 //
2177 // return true;
2178 //}
2179 //
2180 //bool FaceOBBTest(const FACE& f, const OBB& o)
2181 //{
2182 // // convert face into OBB's coordinate system and use
2183 // // FaceAABBTest
2184 //
2185 // AABB ab(DXVEC3(0.0f, 0.0f, 0.0f), o.vExtents);
2186 // FACE newface;
2187 //
2188 // // use ab.vPos as temporary param
2189 // ab.vPos = f.v1 - o.vPos;
2190 // newface.v1.x = D3DXVec3Dot(&ab.vPos, &o.vRight);
2191 // newface.v1.y = D3DXVec3Dot(&ab.vPos, &o.vUp);
2192 // newface.v1.z = D3DXVec3Dot(&ab.vPos, &o.vDir);
2193 // ab.vPos = f.v2 - o.vPos;
2194 // newface.v2.x = D3DXVec3Dot(&ab.vPos, &o.vRight);
2195 // newface.v2.y = D3DXVec3Dot(&ab.vPos, &o.vUp);
2196 // newface.v2.z = D3DXVec3Dot(&ab.vPos, &o.vDir);
2197 // ab.vPos = f.v3 - o.vPos;
2198 // newface.v3.x = D3DXVec3Dot(&ab.vPos, &o.vRight);
2199 // newface.v3.y = D3DXVec3Dot(&ab.vPos, &o.vUp);
2200 // newface.v3.z = D3DXVec3Dot(&ab.vPos, &o.vDir);
2201 //
2202 // ab.vPos.x = ab.vPos.y = ab.vPos.z = 0.0f;
2203 //
2204 // newface.plane.vNorm.x = D3DXVec3Dot(&f.plane.vNorm, &o.vRight);
2205 // newface.plane.vNorm.y = D3DXVec3Dot(&f.plane.vNorm, &o.vUp);
2206 // newface.plane.vNorm.z = D3DXVec3Dot(&f.plane.vNorm, &o.vDir);
2207 // // we don't need the fD param since its not used by the function
2208 //
2209 // return FaceAABBTest(newface, ab);
2210 //}
2211 //
2212 //bool FaceSphereTest(const FACE& f, const SPHERE& s)
2213 //{
2214 // DXVEC3 ve;
2215 // float fLengthSq, fDot;
2216 //
2217 // // check if the sphere actually intersects the face's plane
2218 // fDot = D3DXVec3Dot(&f.plane.vNorm, &s.vPos) + f.plane.fD;
2219 // if(fDot * fDot > s.fRadius * s.fRadius) return false;
2220 //
2221 // // test the sphere against each of the edges by computing the
2222 // // distance from each edge to the center of the circle
2223 //
2224 // // check every point for faster intersection
2225 // if(VertexSphereTest(f.v1, s)) return true;
2226 // if(VertexSphereTest(f.v2, s)) return true;
2227 // if(VertexSphereTest(f.v3, s)) return true;
2228 //
2229 // // edge intersect sphere if the distance from the center of the sphere to the
2230 // // edge is less than the radius and if that's true then check if the
2231 // // sphere actually intersects the edge
2232 //
2233 // ve = f.v2 - f.v1;
2234 // fLengthSq = D3DXVec3LengthSq(&ve);
2235 // fDot = D3DXVec3Dot(&(s.vPos - f.v1), &ve);
2236 //
2237 // if(fDot > 0.0f && fDot < fLengthSq &&
2238 // D3DXVec3LengthSq(&(s.vPos - f.v1)) * fLengthSq - fDot * fDot < fLengthSq * s.fRadius * s.fRadius ) return true;
2239 //
2240 //
2241 // ve = f.v3 - f.v1;
2242 // fLengthSq = D3DXVec3LengthSq(&ve);
2243 // fDot = D3DXVec3Dot(&(s.vPos - f.v1), &ve);
2244 //
2245 // if(fDot > 0.0f && fDot < fLengthSq &&
2246 // D3DXVec3LengthSq(&(s.vPos - f.v1)) * fLengthSq - fDot * fDot < fLengthSq * s.fRadius * s.fRadius ) return true;
2247 //
2248 //
2249 // ve = f.v3 - f.v2;
2250 // fLengthSq = D3DXVec3LengthSq(&ve);
2251 // fDot = D3DXVec3Dot(&(s.vPos - f.v2), &ve);
2252 //
2253 // if(fDot > 0.0f && fDot < fLengthSq &&
2254 // D3DXVec3LengthSq(&(s.vPos - f.v2)) * fLengthSq - fDot * fDot < fLengthSq * s.fRadius * s.fRadius ) return true;
2255 //
2256 //
2257 // // sphere might still be inside the face but not touching any of the edges
2258 // ve = s.vPos - f.plane.vNorm * (D3DXVec3Dot(&f.plane.vNorm, &s.vPos) + f.plane.fD) - f.v1;
2259 //
2260 // float e00, e01, e11, q0, q1, s0, s1, t;
2261 // e00 = D3DXVec3LengthSq(&(f.v2 - f.v1));
2262 // e01 = D3DXVec3Dot(&(f.v2 - f.v1), &(f.v3 - f.v1));
2263 // e11 = D3DXVec3LengthSq(&(f.v3 - f.v1));
2264 //
2265 // q0 = D3DXVec3Dot(&ve, &(f.v2 - f.v1));
2266 // q1 = D3DXVec3Dot(&ve, &(f.v3 - f.v1));
2267 //
2268 // s0 = e11 * q0 - e01 * q1;
2269 // s1 = e00 * q1 - e01 * q0;
2270 //
2271 // t = e00 * e11 - e01 * e01;
2272 //
2273 // if(s0 >= 0.0f && s1 >= 0.0f && s0+s1 <= t) return true;
2274 //
2275 // return false;
2276 //}
2277 //
2278 //bool AABBSphereTest(const AABB& a, const SPHERE& s)
2279 //{
2280 // // algorithm can be found in A Simple Method for Box-Sphere Intersection Testing,
2281 // // Graphics Gems, pp. 247-250 by Jim Arvo
2282 // float side, d = 0.0f;
2283 //
2284 // side = fabsf(s.vPos.x - a.vPos.x);
2285 // if(side > a.vExtents.x) {
2286 // side -= a.vExtents.x;
2287 // d += side * side;
2288 // }
2289 // side = fabsf(s.vPos.y - a.vPos.y);
2290 // if(side > a.vExtents.y) {
2291 // side -= a.vExtents.y;
2292 // d += side * side;
2293 // }
2294 // side = fabsf(s.vPos.z - a.vPos.z);
2295 // if(side > a.vExtents.z) {
2296 // side -= a.vExtents.z;
2297 // d += side * side;
2298 // }
2299 //
2300 // return d < s.fRadius * s.fRadius;
2301 //}
2302 
2303 //bool OBBSphereTest(const OBB& o, const SPHERE& s)
2304 //{
2305 // // algorithm can be found in A Simple Method for Box-Sphere Intersection Testing,
2306 // // Graphics Gems, pp. 247-250 by Jim Arvo
2307 // float side, d = 0.0f;
2308 // DXVEC3 v = s.vPos - o.vPos;
2309 //
2310 // side = fabsf(D3DXVec3Dot(&v, &o.vRight));
2311 // if(side > o.vExtents.x) {
2312 // side -= o.vExtents.x;
2313 // d += side * side;
2314 // }
2315 // side = fabsf(D3DXVec3Dot(&v, &o.vUp));
2316 // if(side > o.vExtents.y) {
2317 // side -= o.vExtents.y;
2318 // d += side * side;
2319 // }
2320 // side = fabsf(D3DXVec3Dot(&v, &o.vDir));
2321 // if(side > o.vExtents.z) {
2322 // side -= o.vExtents.z;
2323 // d += side * side;
2324 // }
2325 //
2326 // return d < s.fRadius * s.fRadius;
2327 //}
2328 
2330 
2331 
2332 //float DistVertexAABBSq(const DXVEC3& v, const AABB& ab)
2333 //{
2334 // float fDist = 0.0f;
2335 // DXVEC3 vn;
2336 // vn.x = fabsf(v.x - ab.vPos.x) - ab.vExtents.x;
2337 // vn.y = fabsf(v.y - ab.vPos.y) - ab.vExtents.y;
2338 // vn.z = fabsf(v.z - ab.vPos.z) - ab.vExtents.z;
2339 //
2340 // if( vn.x > 0.0f ) fDist += vn.x * vn.x;
2341 // if( vn.y > 0.0f ) fDist += vn.x * vn.y;
2342 // if( vn.z > 0.0f ) fDist += vn.x * vn.z;
2343 //
2344 // return fDist;
2345 //}
2346 
2349 template <typename T>
2350 T DistVertexOBBSq(const RaveVector<T>& v, const obb<T>& o)
2351 {
2352  RaveVector<T> vn = v - o.pos;
2353  vn.x = MATH_FABS(vn.dot3(o.right)) - o.extents.x;
2354  vn.y = MATH_FABS(vn.dot3(o.up)) - o.extents.y;
2355  vn.z = MATH_FABS(vn.dot3(o.dir)) - o.extents.z;
2356  // now we have the vertex in OBB's frame
2357  T fDist = 0;
2358  if( vn.x > 0.0f ) {
2359  fDist += vn.x * vn.x;
2360  }
2361  if( vn.y > 0.0f ) {
2362  fDist += vn.y * vn.y;
2363  }
2364  if( vn.z > 0.0f ) {
2365  fDist += vn.z * vn.z;
2366  }
2367  return fDist;
2368 }
2369 
2370 //float DistVertexFaceSq(const DXVEC3& v, const FACE& f)
2371 //{
2372 // DXVEC3 ve, vd;
2373 // float fLengthSq, fDot;
2374 //
2375 // // get the 3 distances from the sphere to the line segments of the face
2376 // // also take the closest distance to the face's plane, and make sure
2377 // // that the point in the plane that is closest is in the face
2378 //
2379 // fDot = D3DXVec3Dot(&f.plane.vNorm, &v) + f.plane.fD;
2380 //
2381 // ve = v - f.plane.vNorm * fDot - f.v1;
2382 //
2383 // // figure out the barycentric coordinates
2384 // float e00, e01, e11, q0, q1, s0, s1, t;
2385 // vd = f.v2 - f.v1;
2386 // e00 = D3DXVec3LengthSq(&vd);
2387 // q0 = D3DXVec3Dot(&ve, &vd);
2388 //
2389 // vd = f.v3 - f.v1;
2390 // e11 = D3DXVec3LengthSq(&vd);
2391 // q1 = D3DXVec3Dot(&ve, &vd);
2392 //
2393 // e01 = D3DXVec3Dot(&(f.v2 - f.v1), &vd);
2394 //
2395 // s0 = e11 * q0 - e01 * q1;
2396 // s1 = e00 * q1 - e01 * q0;
2397 //
2398 // t = e00 * e11 - e01 * e01;
2399 //
2400 // if(s0 >= 0.0f && s1 >= 0.0f && s0+s1 <= t) return fDot * fDot;
2401 //
2402 // // check which edge the vertex is closest to
2403 //
2404 // ve = v - f.v1;
2405 // D3DXVec3Cross(&vd, &(f.v2-f.v1), &f.plane.vNorm);
2406 //
2407 // if( D3DXVec3Dot(&vd, &ve) > 0.0f ) {
2408 // vd = f.v2-f.v1;
2409 // fDot = D3DXVec3Dot(&ve, &vd);
2410 //
2411 // fLengthSq = D3DXVec3LengthSq(&vd);
2412 //
2413 // if( fDot < 0.0f ) return D3DXVec3LengthSq(&ve);
2414 // if( fDot > fLengthSq ) return D3DXVec3LengthSq(&(v-f.v2));
2415 //
2416 // return D3DXVec3LengthSq(&ve) - fDot * fDot / fLengthSq;
2417 // }
2418 //
2419 // D3DXVec3Cross(&vd, &(f.v1-f.v3), &f.plane.vNorm);
2420 //
2421 // if( D3DXVec3Dot(&vd, &ve) > 0.0f ) {
2422 // vd = f.v3-f.v1;
2423 // fDot = D3DXVec3Dot(&ve, &vd);
2424 //
2425 // fLengthSq = D3DXVec3LengthSq(&vd);
2426 //
2427 // if( fDot < 0.0f ) return D3DXVec3LengthSq(&ve);
2428 // if( fDot > fLengthSq ) return D3DXVec3LengthSq(&(v-f.v3));
2429 //
2430 // return D3DXVec3LengthSq(&ve) - fDot * fDot / fLengthSq;
2431 // }
2432 //
2433 // ve = v - f.v2;
2434 // vd = f.v3 - f.v2;
2435 //
2436 // // we know that fDot cannot be < 0 or > lengthsq(vd)
2437 // return D3DXVec3LengthSq(&ve) - Sqr(D3DXVec3Dot(&ve, &vd)) / D3DXVec3LengthSq(&vd);
2438 //}
2439 
2440 //float DistVertexCone(const DXVEC3& vObj, CONE& c)
2441 //{
2442 // DXVEC3 v = vObj-c.vVertex;
2443 // float fDot = D3DXVec3Dot(&c.vDir, &v);
2444 // float fLengthSq = D3DXVec3LengthSq(&v);
2445 //
2446 // // the closest point might be the vertex
2447 // float fDotSq = fDot * fDot;
2448 // if( fDot <= 0.0f && fDotSq > c.fSinAng * c.fSinAng * fLengthSq )
2449 // return -D3DXVec3Length(&v);
2450 //
2451 // // if beyond cone base, then shortest distance can either
2452 // // be the base or the edge of the circle
2453 // if( fDot > c.fLength ) {
2454 // fLengthSq -= fDotSq;
2455 // float fBaseRadius = c.fLength * c.fSinAng / c.fCosAng;
2456 //
2457 // if( fLengthSq < fBaseRadius ) return c.fLength - fDot;
2458 //
2459 // // its the edge
2460 // fDot -= c.fLength;
2461 // return -sqrtf( Sqr(sqrtf(fLengthSq)-fBaseRadius) + fDot*fDot );
2462 // }
2463 //
2464 // // orthogonal project to cone's surface
2465 // float fTest = c.fSinAng * sqrtf(fLengthSq - fDotSq) + c.fCosAng * fDot;
2466 // float fSide = c.fLength / c.fCosAng;
2467 //
2468 // if( fTest > fSide ) {
2469 // // outside and along edge
2470 // fLengthSq -= fTest*fTest;
2471 // fTest -= fSide;
2472 // return -sqrtf(fLengthSq + fTest*fTest);
2473 // }
2474 //
2475 // if( fLengthSq > c.fCosAng * c.fCosAng * fDotSq ) return -sqrtf(fLengthSq-fTest*fTest);
2476 //
2477 // // the vertex is inside the cone, but shortest distance can either be
2478 // // from base of cone or surface of cone, so compute both and compare
2479 // fLengthSq -= fTest*fTest;
2480 //
2481 // fTest = c.fLength - fDot;
2482 //
2483 // if( fTest*fTest < fLengthSq ) return fTest;
2484 //
2485 // return sqrtf(fLengthSq);
2486 //}
2487 //
2488 //float DistVertexFrustum(const DXVEC3& vObjPos, const FRUSTUM& fr)
2489 //{
2490 // DXVEC3 v = vObjPos - fr.vPos, vproj;
2491 // float f, fx, fy, fplane;
2492 // vproj.x = fabsf(D3DXVec3Dot(&v, &fr.vRight));
2493 // vproj.y = fabsf(D3DXVec3Dot(&v, &fr.vUp));
2494 // vproj.z = fabsf(D3DXVec3Dot(&v, &fr.vDir));
2495 //
2496 // // v.x - dot product along the right face
2497 // // v.y - dot product along the up face
2498 //
2499 // fx = vproj.x * fr.fCosFOVX - vproj.z * fr.fSinFOVX;
2500 // fy = vproj.y * fr.fCosFOVY - vproj.z * fr.fSinFOVY;
2501 //
2502 // if( fx > 0.0f ) {
2503 // if( fy > 0.0f ) {
2504 // // vector along upper right edge is: sinx, siny, cosx*cosy
2505 //
2506 // // projected length along this vector
2507 // float fdot = fr.fSinFOVX * vproj.x + fr.fSinFOVY * vproj.y + fr.fCosFOVX * fr.fCosFOVY * vproj.z;
2508 // f = 1.0f + Sqr(fr.fSinFOVX * fr.fSinFOVY);
2509 // fplane = fr.fSinFOVX * fr.fSinFOVX / fr.fCosFOVX + fr.fSinFOVY * fr.fSinFOVY / fr.fCosFOVY +
2510 // fr.fCosFOVX * fr.fCosFOVY;
2511 //
2512 // if( fdot < f * fplane * fr.fNear ) {
2513 // return -sqrtf( Sqr(vproj.x - fr.fNear*fr.fSinFOVX/fr.fCosFOVX) +
2514 // Sqr(vproj.y - fr.fNear*fr.fSinFOVY/fr.fCosFOVY) +
2515 // Sqr(vproj.z - fr.fNear) );
2516 // }
2517 // else if( fdot < f * fplane * fr.fFar )
2518 // return -sqrtf(D3DXVec3LengthSq(&v) - Sqr(fdot / f));
2519 // else
2520 // return -sqrtf( Sqr(vproj.x - fr.fFar*fr.fSinFOVX/fr.fCosFOVX) +
2521 // Sqr(vproj.y - fr.fFar*fr.fSinFOVY/fr.fCosFOVY) +
2522 // Sqr(vproj.z - fr.fFar) );
2523 // }
2524 // else {
2525 // v.x = vproj.x * fr.fSinFOVX + vproj.z * fr.fCosFOVX;
2526 //
2527 // if( v.x < 2.0f * fr.fNear * fr.fSinFOVX )
2528 // return -sqrtf( Sqr(vproj.x - fr.fNear*fr.fSinFOVX/fr.fCosFOVX) + Sqr(vproj.z-fr.fNear) );
2529 // else if( v.x < 2.0f * fr.fFar * fr.fSinFOVX )
2530 // return -fx;
2531 // else
2532 // return -sqrtf( Sqr(vproj.x - fr.fFar*fr.fSinFOVX/fr.fCosFOVX) + Sqr(vproj.z-fr.fFar) );
2533 // }
2534 // }
2535 // else {
2536 // if( fy > 0.0f ) {
2537 // v.y = vproj.y * fr.fSinFOVY + vproj.z * fr.fCosFOVY;
2538 //
2539 // if( v.y < 2.0f * fr.fNear * fr.fSinFOVY )
2540 // return -sqrtf( Sqr(vproj.y - fr.fNear*fr.fSinFOVY/fr.fCosFOVY) + Sqr(vproj.z-fr.fNear) );
2541 // else if( v.x < 2.0f * fr.fFar * fr.fSinFOVY )
2542 // return -fy;
2543 // else
2544 // return -sqrtf( Sqr(vproj.y - fr.fFar*fr.fSinFOVY/fr.fCosFOVY) + Sqr(vproj.z-fr.fFar) );
2545 // }
2546 // else {
2547 // if( vproj.z < fr.fNear ) return vproj.z - fr.fNear;
2548 // else if( vproj.z < fr.fFar ) {
2549 // v.x = fx < fy ? -fx : fy;
2550 // if( v.x > vproj.z-fr.fNear ) v.x = vproj.z - fr.fNear;
2551 // if( v.x > fr.fFar-vproj.z ) v.x = fr.fFar - vproj.z;
2552 //
2553 // return v.x;
2554 // }
2555 // else return fr.fFar - vproj.z;
2556 // }
2557 // }
2558 //
2559 // return 0.0f;
2560 //}
2561 
2563 
2564 //DXVEC3 ClosestPointToPlane(const SPHERE& s, const PLANE& p)
2565 //{
2566 // return s.vPos - (D3DXVec3Dot(&s.vPos, &p.vNorm) + p.fD) * p.vNorm;
2567 //}
2568 //
2569 //DXVEC3 ClosestPointToPlane(const OBB& obb, const PLANE& p)
2570 //{
2571 // DXVEC3 vPt = obb.vPos;
2572 //
2573 // if( D3DXVec3Dot(&p.vNorm, &obb.vRight) < 0.0f)
2574 // vPt += obb.vRight * obb.vExtents.x;
2575 // else vPt -= obb.vRight * obb.vExtents.x;
2576 //
2577 // if( D3DXVec3Dot(&p.vNorm, &obb.vUp) < 0.0f)
2578 // vPt += obb.vUp * obb.vExtents.y;
2579 // else vPt -= obb.vUp * obb.vExtents.y;
2580 //
2581 // if( D3DXVec3Dot(&p.vNorm, &obb.vDir) < 0.0f)
2582 // vPt += obb.vDir * obb.vExtents.z;
2583 // else vPt -= obb.vDir * obb.vExtents.z;
2584 //
2585 // return vPt;
2586 //}
2587 //
2588 //DXVEC3 ClosestPointToPlane(const FACE& f, const PLANE& p)
2589 //{
2590 // // it can be one of 3 points
2591 // float fDist1 = D3DXVec3Dot(&f.v1, &p.vNorm);
2592 // float fDist2 = D3DXVec3Dot(&f.v2, &p.vNorm);
2593 //
2594 // float fDist3 = D3DXVec3Dot(&f.v3, &p.vNorm);
2595 //
2596 // if( fDist1 < fDist2 ) {
2597 // if(fDist1 < fDist3) return f.v1;
2598 // else return f.v3;
2599 // }
2600 // else {
2601 // if(fDist2 < fDist3) return f.v2;
2602 // else return f.v3;
2603 // }
2604 //
2605 // return f.v1;
2606 //}
2607 //
2608 //DXVEC3 ClosestPointToFace(const DXVEC3& v, const FACE& f)
2609 //{
2610 // DXVEC3 ve, vd;
2611 // float fLengthSq, fDot;
2612 //
2613 // // get the 3 distances from the sphere to the line segments of the face
2614 // // also take the closest distance to the face's plane, and make sure
2615 // // that the point in the plane that is closest is in the face
2616 //
2617 // fDot = D3DXVec3Dot(&f.plane.vNorm, &v) + f.plane.fD;
2618 //
2619 // ve = v - f.plane.vNorm * fDot - f.v1;
2620 //
2621 // // figure out the barycentric coordinates
2622 // float e00, e01, e11, q0, q1, s0, s1, t;
2623 // vd = f.v2 - f.v1;
2624 // e00 = D3DXVec3LengthSq(&vd);
2625 // q0 = D3DXVec3Dot(&ve, &vd);
2626 //
2627 // vd = f.v3 - f.v1;
2628 // e11 = D3DXVec3LengthSq(&vd);
2629 // q1 = D3DXVec3Dot(&ve, &vd);
2630 //
2631 // e01 = D3DXVec3Dot(&(f.v2 - f.v1), &vd);
2632 //
2633 // s0 = e11 * q0 - e01 * q1;
2634 // s1 = e00 * q1 - e01 * q0;
2635 //
2636 // t = e00 * e11 - e01 * e01;
2637 //
2638 // assert( t >= -1e5);
2639 // if(s0 >= 0.0f && s1 >= 0.0f && s0+s1 <= t) return ve + f.v1;
2640 //
2641 // // check which edge the vertex is closest to
2642 //
2643 // ve = v - f.v1;
2644 // D3DXVec3Cross(&vd, &(f.v2-f.v1), &f.plane.vNorm);
2645 //
2646 // if( D3DXVec3Dot(&vd, &ve) > 0.0f ) {
2647 // vd = f.v2-f.v1;
2648 // fDot = D3DXVec3Dot(&ve, &vd);
2649 //
2650 // fLengthSq = D3DXVec3LengthSq(&vd);
2651 //
2652 // if( fDot < 0.0f ) return f.v1;
2653 // if( fDot > fLengthSq ) return f.v2;
2654 //
2655 // return f.v1 + fDot * vd / fLengthSq;
2656 // }
2657 //
2658 // D3DXVec3Cross(&vd, &(f.v1-f.v3), &f.plane.vNorm);
2659 //
2660 // if( D3DXVec3Dot(&vd, &ve) > 0.0f ) {
2661 // vd = f.v3-f.v1;
2662 // fDot = D3DXVec3Dot(&ve, &vd);
2663 //
2664 // fLengthSq = D3DXVec3LengthSq(&vd);
2665 //
2666 // if( fDot < 0.0f ) return f.v1;
2667 // if( fDot > fLengthSq ) return f.v3;
2668 //
2669 // return f.v1 + fDot * vd / fLengthSq;
2670 // }
2671 //
2672 // vd = f.v3 - f.v2;
2673 //
2674 // // we know that fDot cannot be < 0 or > lengthsq(vd)
2675 // return f.v2 + D3DXVec3Dot(&(v - f.v2), &vd) * vd / D3DXVec3LengthSq(&vd);
2676 //}
2677 //
2678 //
2682 //PLANE ClosestPlaneFromVertexOBB(const DXVEC3& v, const OBB& o)
2683 //{
2684 // DXVEC3 vn = v - o.vPos;
2685 // DXVEC3 vsign = DXVEC3(1.0f, 1.0f, 1.0f);
2686 // vn = DXVEC3( D3DXVec3Dot(&vn, &o.vRight),
2687 // D3DXVec3Dot(&vn, &o.vUp),
2688 // D3DXVec3Dot(&vn, &o.vDir));
2689 //
2690 // if(vn.x < 0.0f) {
2691 // vsign.x = -1.0f;
2692 // vn.x = -vn.x;
2693 // }
2694 // if(vn.y < 0.0f) {
2695 // vsign.y = -1.0f;
2696 // vn.y = -vn.y;
2697 // }
2698 // if(vn.z < 0.0f) {
2699 // vsign.z = -1.0f;
2700 // vn.z = -vn.z;
2701 // }
2702 //
2703 // vn -= o.vExtents;
2704 //
2705 // if(vn.x > 0.0f) {
2706 //
2707 // if(vn.y > 0.0f) {
2708 //
2709 // if(vn.z > 0.0f) {
2710 // // the closest point from the obb is one of the obb's 8 vertices
2711 //
2712 // // the point
2713 // vsign = o.vPos + DXVEC3(vsign.x * o.vExtents.x, vsign.y * o.vExtents.y, vsign.z * o.vExtents.z);
2714 //
2715 // D3DXVec3Normalize(&vn, &(v-vsign));
2716 // }
2717 // else {
2718 // // an edge; the plane has components in the x and y directions
2719 // vsign = o.vPos + DXVEC3(vsign.x * o.vExtents.x, vsign.y * o.vExtents.y, 0.0f);
2720 //
2721 // // we want only the components along the obb's right and up vectors
2722 // D3DXVec3Normalize(&vn, &(v - o.vDir * D3DXVec3Dot(&o.vDir, &(v-o.vPos)) - vsign) );
2723 // }
2724 // }
2725 // else {
2726 // if(vn.z > 0.0f) {
2727 // // an edge; the plane has components in the x and z directions
2728 // vsign = o.vPos + DXVEC3(vsign.x * o.vExtents.x, 0.0f, vsign.z * o.vExtents.z);
2729 //
2730 // // we want only the components along the obb's right and dir vectors
2731 // D3DXVec3Normalize( &vn, &(v - o.vUp * D3DXVec3Dot(&o.vUp, &(v-o.vPos)) - vsign) );
2732 // }
2733 // else {
2734 // // the right planes
2735 //
2736 // vn = vsign.x * o.vRight; // since we need the sign, compute normal first
2737 //
2738 // vsign = o.vPos + DXVEC3(vsign.x * o.vExtents.x, 0.0f, 0.0f);
2739 // }
2740 // }
2741 // }
2742 // else {
2743 // if(vn.y > 0.0f) {
2744 //
2745 // if(vn.z > 0.0f) {
2746 // // an edge; the plane has components in the y and z directions
2747 // vsign = o.vPos + DXVEC3(0.0f, vsign.y * o.vExtents.y, vsign.z * o.vExtents.z);
2748 //
2749 // // we want only the components along the obb's up and dir vectors
2750 // D3DXVec3Normalize(&vn, &(v - o.vRight * D3DXVec3Dot(&o.vRight, &(v-o.vPos)) - vsign) );
2751 // }
2752 // else {
2753 // // the up planes
2754 //
2755 // vn = vsign.y * o.vUp; // since we need the sign, compute normal first
2756 //
2757 // vsign = o.vPos + DXVEC3(0.0f, vsign.y * o.vExtents.y, 0.0f);
2758 // }
2759 // }
2760 // else {
2761 // if(vn.z > 0.0f) {
2762 // // the dir planes
2763 //
2764 // vn = vsign.z * o.vDir; // since we need the sign, compute normal first
2765 //
2766 // vsign = o.vPos + DXVEC3(0.0f, 0.0f, vsign.z * o.vExtents.z);
2767 // }
2768 // else {
2769 // // if we are here then that means we are doing something wrong
2770 // // but anyway, find the plane that the point is closest to
2771 //
2772 // if(vn.x > vn.y) {
2773 // if(vn.x > vn.z) {
2774 // vn = vsign.x * o.vRight;
2775 // vsign = o.vPos + DXVEC3(vsign.x * o.vExtents.x, 0.0f, 0.0f);
2776 // }
2777 // else {
2778 // vn = vsign.z * o.vDir;
2779 // vsign = o.vPos + DXVEC3(0.0f, 0.0f, vsign.z * o.vExtents.z);
2780 // }
2781 // }
2782 // else {
2783 // if(vn.y > vn.z) {
2784 // vn = vsign.y * o.vUp; // since we need the sign, compute normal first
2785 // vsign = o.vPos + DXVEC3(0.0f, vsign.y * o.vExtents.y, 0.0f);
2786 // }
2787 // else {
2788 // vn = vsign.z * o.vDir;
2789 // vsign = o.vPos + DXVEC3(0.0f, 0.0f, vsign.z * o.vExtents.z);
2790 // }
2791 // }
2792 // }
2793 // }
2794 // }
2795 //
2796 // return PLANE( vn, vsign );
2797 //}
2798 
2799 } // end namespace geometry
2800 } // end namespace OpenRAVE
2801 
2802 #endif