bullet: Update to current svn, r2636
[blender.git] / extern / bullet2 / src / BulletCollision / Gimpact / btBoxCollision.h
1 #ifndef BT_BOX_COLLISION_H_INCLUDED
2 #define BT_BOX_COLLISION_H_INCLUDED
3
4 /*! \file gim_box_collision.h
5 \author Francisco Leon Najera
6 */
7 /*
8 This source file is part of GIMPACT Library.
9
10 For the latest info, see http://gimpact.sourceforge.net/
11
12 Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371.
13 email: projectileman@yahoo.com
14
15
16 This software is provided 'as-is', without any express or implied warranty.
17 In no event will the authors be held liable for any damages arising from the use of this software.
18 Permission is granted to anyone to use this software for any purpose,
19 including commercial applications, and to alter it and redistribute it freely,
20 subject to the following restrictions:
21
22 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
23 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
24 3. This notice may not be removed or altered from any source distribution.
25 */
26
27 #include "LinearMath/btTransform.h"
28
29
30 ///Swap numbers
31 #define BT_SWAP_NUMBERS(a,b){ \
32     a = a+b; \
33     b = a-b; \
34     a = a-b; \
35 }\
36
37
38 #define BT_MAX(a,b) (a<b?b:a)
39 #define BT_MIN(a,b) (a>b?b:a)
40
41 #define BT_GREATER(x, y)        btFabs(x) > (y)
42
43 #define BT_MAX3(a,b,c) BT_MAX(a,BT_MAX(b,c))
44 #define BT_MIN3(a,b,c) BT_MIN(a,BT_MIN(b,c))
45
46
47
48
49
50
51 enum eBT_PLANE_INTERSECTION_TYPE
52 {
53         BT_CONST_BACK_PLANE = 0,
54         BT_CONST_COLLIDE_PLANE,
55         BT_CONST_FRONT_PLANE
56 };
57
58 //SIMD_FORCE_INLINE bool test_cross_edge_box(
59 //      const btVector3 & edge,
60 //      const btVector3 & absolute_edge,
61 //      const btVector3 & pointa,
62 //      const btVector3 & pointb, const btVector3 & extend,
63 //      int dir_index0,
64 //      int dir_index1
65 //      int component_index0,
66 //      int component_index1)
67 //{
68 //      // dir coords are -z and y
69 //
70 //      const btScalar dir0 = -edge[dir_index0];
71 //      const btScalar dir1 = edge[dir_index1];
72 //      btScalar pmin = pointa[component_index0]*dir0 + pointa[component_index1]*dir1;
73 //      btScalar pmax = pointb[component_index0]*dir0 + pointb[component_index1]*dir1;
74 //      //find minmax
75 //      if(pmin>pmax)
76 //      {
77 //              BT_SWAP_NUMBERS(pmin,pmax);
78 //      }
79 //      //find extends
80 //      const btScalar rad = extend[component_index0] * absolute_edge[dir_index0] +
81 //                                      extend[component_index1] * absolute_edge[dir_index1];
82 //
83 //      if(pmin>rad || -rad>pmax) return false;
84 //      return true;
85 //}
86 //
87 //SIMD_FORCE_INLINE bool test_cross_edge_box_X_axis(
88 //      const btVector3 & edge,
89 //      const btVector3 & absolute_edge,
90 //      const btVector3 & pointa,
91 //      const btVector3 & pointb, btVector3 & extend)
92 //{
93 //
94 //      return test_cross_edge_box(edge,absolute_edge,pointa,pointb,extend,2,1,1,2);
95 //}
96 //
97 //
98 //SIMD_FORCE_INLINE bool test_cross_edge_box_Y_axis(
99 //      const btVector3 & edge,
100 //      const btVector3 & absolute_edge,
101 //      const btVector3 & pointa,
102 //      const btVector3 & pointb, btVector3 & extend)
103 //{
104 //
105 //      return test_cross_edge_box(edge,absolute_edge,pointa,pointb,extend,0,2,2,0);
106 //}
107 //
108 //SIMD_FORCE_INLINE bool test_cross_edge_box_Z_axis(
109 //      const btVector3 & edge,
110 //      const btVector3 & absolute_edge,
111 //      const btVector3 & pointa,
112 //      const btVector3 & pointb, btVector3 & extend)
113 //{
114 //
115 //      return test_cross_edge_box(edge,absolute_edge,pointa,pointb,extend,1,0,0,1);
116 //}
117
118
119 #define TEST_CROSS_EDGE_BOX_MCR(edge,absolute_edge,pointa,pointb,_extend,i_dir_0,i_dir_1,i_comp_0,i_comp_1)\
120 {\
121         const btScalar dir0 = -edge[i_dir_0];\
122         const btScalar dir1 = edge[i_dir_1];\
123         btScalar pmin = pointa[i_comp_0]*dir0 + pointa[i_comp_1]*dir1;\
124         btScalar pmax = pointb[i_comp_0]*dir0 + pointb[i_comp_1]*dir1;\
125         if(pmin>pmax)\
126         {\
127                 BT_SWAP_NUMBERS(pmin,pmax); \
128         }\
129         const btScalar abs_dir0 = absolute_edge[i_dir_0];\
130         const btScalar abs_dir1 = absolute_edge[i_dir_1];\
131         const btScalar rad = _extend[i_comp_0] * abs_dir0 + _extend[i_comp_1] * abs_dir1;\
132         if(pmin>rad || -rad>pmax) return false;\
133 }\
134
135
136 #define TEST_CROSS_EDGE_BOX_X_AXIS_MCR(edge,absolute_edge,pointa,pointb,_extend)\
137 {\
138         TEST_CROSS_EDGE_BOX_MCR(edge,absolute_edge,pointa,pointb,_extend,2,1,1,2);\
139 }\
140
141 #define TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(edge,absolute_edge,pointa,pointb,_extend)\
142 {\
143         TEST_CROSS_EDGE_BOX_MCR(edge,absolute_edge,pointa,pointb,_extend,0,2,2,0);\
144 }\
145
146 #define TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(edge,absolute_edge,pointa,pointb,_extend)\
147 {\
148         TEST_CROSS_EDGE_BOX_MCR(edge,absolute_edge,pointa,pointb,_extend,1,0,0,1);\
149 }\
150
151
152 //! Returns the dot product between a vec3f and the col of a matrix
153 SIMD_FORCE_INLINE btScalar bt_mat3_dot_col(
154 const btMatrix3x3 & mat, const btVector3 & vec3, int colindex)
155 {
156         return vec3[0]*mat[0][colindex] + vec3[1]*mat[1][colindex] + vec3[2]*mat[2][colindex];
157 }
158
159
160 //!  Class for transforming a model1 to the space of model0
161 ATTRIBUTE_ALIGNED16     (class) BT_BOX_BOX_TRANSFORM_CACHE
162 {
163 public:
164     btVector3  m_T1to0;//!< Transforms translation of model1 to model 0
165         btMatrix3x3 m_R1to0;//!< Transforms Rotation of model1 to model 0, equal  to R0' * R1
166         btMatrix3x3 m_AR;//!< Absolute value of m_R1to0
167
168         SIMD_FORCE_INLINE void calc_absolute_matrix()
169         {
170 //              static const btVector3 vepsi(1e-6f,1e-6f,1e-6f);
171 //              m_AR[0] = vepsi + m_R1to0[0].absolute();
172 //              m_AR[1] = vepsi + m_R1to0[1].absolute();
173 //              m_AR[2] = vepsi + m_R1to0[2].absolute();
174
175                 int i,j;
176
177         for(i=0;i<3;i++)
178         {
179             for(j=0;j<3;j++ )
180             {
181                 m_AR[i][j] = 1e-6f + btFabs(m_R1to0[i][j]);
182             }
183         }
184
185         }
186
187         BT_BOX_BOX_TRANSFORM_CACHE()
188         {
189         }
190
191
192
193         //! Calc the transformation relative  1 to 0. Inverts matrics by transposing
194         SIMD_FORCE_INLINE void calc_from_homogenic(const btTransform & trans0,const btTransform & trans1)
195         {
196
197                 btTransform temp_trans = trans0.inverse();
198                 temp_trans = temp_trans * trans1;
199
200                 m_T1to0 = temp_trans.getOrigin();
201                 m_R1to0 = temp_trans.getBasis();
202
203
204                 calc_absolute_matrix();
205         }
206
207         //! Calcs the full invertion of the matrices. Useful for scaling matrices
208         SIMD_FORCE_INLINE void calc_from_full_invert(const btTransform & trans0,const btTransform & trans1)
209         {
210                 m_R1to0 = trans0.getBasis().inverse();
211                 m_T1to0 = m_R1to0 * (-trans0.getOrigin());
212
213                 m_T1to0 += m_R1to0*trans1.getOrigin();
214                 m_R1to0 *= trans1.getBasis();
215
216                 calc_absolute_matrix();
217         }
218
219         SIMD_FORCE_INLINE btVector3 transform(const btVector3 & point) const
220         {
221         return point.dot3( m_R1to0[0], m_R1to0[1], m_R1to0[2] ) + m_T1to0;
222         }
223 };
224
225
226 #define BOX_PLANE_EPSILON 0.000001f
227
228 //! Axis aligned box
229 ATTRIBUTE_ALIGNED16     (class) btAABB
230 {
231 public:
232         btVector3 m_min;
233         btVector3 m_max;
234
235         btAABB()
236         {}
237
238
239         btAABB(const btVector3 & V1,
240                          const btVector3 & V2,
241                          const btVector3 & V3)
242         {
243                 m_min[0] = BT_MIN3(V1[0],V2[0],V3[0]);
244                 m_min[1] = BT_MIN3(V1[1],V2[1],V3[1]);
245                 m_min[2] = BT_MIN3(V1[2],V2[2],V3[2]);
246
247                 m_max[0] = BT_MAX3(V1[0],V2[0],V3[0]);
248                 m_max[1] = BT_MAX3(V1[1],V2[1],V3[1]);
249                 m_max[2] = BT_MAX3(V1[2],V2[2],V3[2]);
250         }
251
252         btAABB(const btVector3 & V1,
253                          const btVector3 & V2,
254                          const btVector3 & V3,
255                          btScalar margin)
256         {
257                 m_min[0] = BT_MIN3(V1[0],V2[0],V3[0]);
258                 m_min[1] = BT_MIN3(V1[1],V2[1],V3[1]);
259                 m_min[2] = BT_MIN3(V1[2],V2[2],V3[2]);
260
261                 m_max[0] = BT_MAX3(V1[0],V2[0],V3[0]);
262                 m_max[1] = BT_MAX3(V1[1],V2[1],V3[1]);
263                 m_max[2] = BT_MAX3(V1[2],V2[2],V3[2]);
264
265                 m_min[0] -= margin;
266                 m_min[1] -= margin;
267                 m_min[2] -= margin;
268                 m_max[0] += margin;
269                 m_max[1] += margin;
270                 m_max[2] += margin;
271         }
272
273         btAABB(const btAABB &other):
274                 m_min(other.m_min),m_max(other.m_max)
275         {
276         }
277
278         btAABB(const btAABB &other,btScalar margin ):
279                 m_min(other.m_min),m_max(other.m_max)
280         {
281                 m_min[0] -= margin;
282                 m_min[1] -= margin;
283                 m_min[2] -= margin;
284                 m_max[0] += margin;
285                 m_max[1] += margin;
286                 m_max[2] += margin;
287         }
288
289         SIMD_FORCE_INLINE void invalidate()
290         {
291                 m_min[0] = SIMD_INFINITY;
292                 m_min[1] = SIMD_INFINITY;
293                 m_min[2] = SIMD_INFINITY;
294                 m_max[0] = -SIMD_INFINITY;
295                 m_max[1] = -SIMD_INFINITY;
296                 m_max[2] = -SIMD_INFINITY;
297         }
298
299         SIMD_FORCE_INLINE void increment_margin(btScalar margin)
300         {
301                 m_min[0] -= margin;
302                 m_min[1] -= margin;
303                 m_min[2] -= margin;
304                 m_max[0] += margin;
305                 m_max[1] += margin;
306                 m_max[2] += margin;
307         }
308
309         SIMD_FORCE_INLINE void copy_with_margin(const btAABB &other, btScalar margin)
310         {
311                 m_min[0] = other.m_min[0] - margin;
312                 m_min[1] = other.m_min[1] - margin;
313                 m_min[2] = other.m_min[2] - margin;
314
315                 m_max[0] = other.m_max[0] + margin;
316                 m_max[1] = other.m_max[1] + margin;
317                 m_max[2] = other.m_max[2] + margin;
318         }
319
320         template<typename CLASS_POINT>
321         SIMD_FORCE_INLINE void calc_from_triangle(
322                                                         const CLASS_POINT & V1,
323                                                         const CLASS_POINT & V2,
324                                                         const CLASS_POINT & V3)
325         {
326                 m_min[0] = BT_MIN3(V1[0],V2[0],V3[0]);
327                 m_min[1] = BT_MIN3(V1[1],V2[1],V3[1]);
328                 m_min[2] = BT_MIN3(V1[2],V2[2],V3[2]);
329
330                 m_max[0] = BT_MAX3(V1[0],V2[0],V3[0]);
331                 m_max[1] = BT_MAX3(V1[1],V2[1],V3[1]);
332                 m_max[2] = BT_MAX3(V1[2],V2[2],V3[2]);
333         }
334
335         template<typename CLASS_POINT>
336         SIMD_FORCE_INLINE void calc_from_triangle_margin(
337                                                         const CLASS_POINT & V1,
338                                                         const CLASS_POINT & V2,
339                                                         const CLASS_POINT & V3, btScalar margin)
340         {
341                 m_min[0] = BT_MIN3(V1[0],V2[0],V3[0]);
342                 m_min[1] = BT_MIN3(V1[1],V2[1],V3[1]);
343                 m_min[2] = BT_MIN3(V1[2],V2[2],V3[2]);
344
345                 m_max[0] = BT_MAX3(V1[0],V2[0],V3[0]);
346                 m_max[1] = BT_MAX3(V1[1],V2[1],V3[1]);
347                 m_max[2] = BT_MAX3(V1[2],V2[2],V3[2]);
348
349                 m_min[0] -= margin;
350                 m_min[1] -= margin;
351                 m_min[2] -= margin;
352                 m_max[0] += margin;
353                 m_max[1] += margin;
354                 m_max[2] += margin;
355         }
356
357         //! Apply a transform to an AABB
358         SIMD_FORCE_INLINE void appy_transform(const btTransform & trans)
359         {
360                 btVector3 center = (m_max+m_min)*0.5f;
361                 btVector3 extends = m_max - center;
362                 // Compute new center
363                 center = trans(center);
364
365         btVector3 textends = extends.dot3(trans.getBasis().getRow(0).absolute(), 
366                                           trans.getBasis().getRow(1).absolute(), 
367                                           trans.getBasis().getRow(2).absolute());
368
369                 m_min = center - textends;
370                 m_max = center + textends;
371         }
372
373
374         //! Apply a transform to an AABB
375         SIMD_FORCE_INLINE void appy_transform_trans_cache(const BT_BOX_BOX_TRANSFORM_CACHE & trans)
376         {
377                 btVector3 center = (m_max+m_min)*0.5f;
378                 btVector3 extends = m_max - center;
379                 // Compute new center
380                 center = trans.transform(center);
381
382         btVector3 textends = extends.dot3(trans.m_R1to0.getRow(0).absolute(), 
383                                           trans.m_R1to0.getRow(1).absolute(), 
384                                           trans.m_R1to0.getRow(2).absolute());
385         
386                 m_min = center - textends;
387                 m_max = center + textends;
388         }
389
390         //! Merges a Box
391         SIMD_FORCE_INLINE void merge(const btAABB & box)
392         {
393                 m_min[0] = BT_MIN(m_min[0],box.m_min[0]);
394                 m_min[1] = BT_MIN(m_min[1],box.m_min[1]);
395                 m_min[2] = BT_MIN(m_min[2],box.m_min[2]);
396
397                 m_max[0] = BT_MAX(m_max[0],box.m_max[0]);
398                 m_max[1] = BT_MAX(m_max[1],box.m_max[1]);
399                 m_max[2] = BT_MAX(m_max[2],box.m_max[2]);
400         }
401
402         //! Merges a point
403         template<typename CLASS_POINT>
404         SIMD_FORCE_INLINE void merge_point(const CLASS_POINT & point)
405         {
406                 m_min[0] = BT_MIN(m_min[0],point[0]);
407                 m_min[1] = BT_MIN(m_min[1],point[1]);
408                 m_min[2] = BT_MIN(m_min[2],point[2]);
409
410                 m_max[0] = BT_MAX(m_max[0],point[0]);
411                 m_max[1] = BT_MAX(m_max[1],point[1]);
412                 m_max[2] = BT_MAX(m_max[2],point[2]);
413         }
414
415         //! Gets the extend and center
416         SIMD_FORCE_INLINE void get_center_extend(btVector3 & center,btVector3 & extend)  const
417         {
418                 center = (m_max+m_min)*0.5f;
419                 extend = m_max - center;
420         }
421
422         //! Finds the intersecting box between this box and the other.
423         SIMD_FORCE_INLINE void find_intersection(const btAABB & other, btAABB & intersection)  const
424         {
425                 intersection.m_min[0] = BT_MAX(other.m_min[0],m_min[0]);
426                 intersection.m_min[1] = BT_MAX(other.m_min[1],m_min[1]);
427                 intersection.m_min[2] = BT_MAX(other.m_min[2],m_min[2]);
428
429                 intersection.m_max[0] = BT_MIN(other.m_max[0],m_max[0]);
430                 intersection.m_max[1] = BT_MIN(other.m_max[1],m_max[1]);
431                 intersection.m_max[2] = BT_MIN(other.m_max[2],m_max[2]);
432         }
433
434
435         SIMD_FORCE_INLINE bool has_collision(const btAABB & other) const
436         {
437                 if(m_min[0] > other.m_max[0] ||
438                    m_max[0] < other.m_min[0] ||
439                    m_min[1] > other.m_max[1] ||
440                    m_max[1] < other.m_min[1] ||
441                    m_min[2] > other.m_max[2] ||
442                    m_max[2] < other.m_min[2])
443                 {
444                         return false;
445                 }
446                 return true;
447         }
448
449         /*! \brief Finds the Ray intersection parameter.
450         \param aabb Aligned box
451         \param vorigin A vec3f with the origin of the ray
452         \param vdir A vec3f with the direction of the ray
453         */
454         SIMD_FORCE_INLINE bool collide_ray(const btVector3 & vorigin,const btVector3 & vdir)  const
455         {
456                 btVector3 extents,center;
457                 this->get_center_extend(center,extents);;
458
459                 btScalar Dx = vorigin[0] - center[0];
460                 if(BT_GREATER(Dx, extents[0]) && Dx*vdir[0]>=0.0f)      return false;
461                 btScalar Dy = vorigin[1] - center[1];
462                 if(BT_GREATER(Dy, extents[1]) && Dy*vdir[1]>=0.0f)      return false;
463                 btScalar Dz = vorigin[2] - center[2];
464                 if(BT_GREATER(Dz, extents[2]) && Dz*vdir[2]>=0.0f)      return false;
465
466
467                 btScalar f = vdir[1] * Dz - vdir[2] * Dy;
468                 if(btFabs(f) > extents[1]*btFabs(vdir[2]) + extents[2]*btFabs(vdir[1])) return false;
469                 f = vdir[2] * Dx - vdir[0] * Dz;
470                 if(btFabs(f) > extents[0]*btFabs(vdir[2]) + extents[2]*btFabs(vdir[0]))return false;
471                 f = vdir[0] * Dy - vdir[1] * Dx;
472                 if(btFabs(f) > extents[0]*btFabs(vdir[1]) + extents[1]*btFabs(vdir[0]))return false;
473                 return true;
474         }
475
476
477         SIMD_FORCE_INLINE void projection_interval(const btVector3 & direction, btScalar &vmin, btScalar &vmax) const
478         {
479                 btVector3 center = (m_max+m_min)*0.5f;
480                 btVector3 extend = m_max-center;
481
482                 btScalar _fOrigin =  direction.dot(center);
483                 btScalar _fMaximumExtent = extend.dot(direction.absolute());
484                 vmin = _fOrigin - _fMaximumExtent;
485                 vmax = _fOrigin + _fMaximumExtent;
486         }
487
488         SIMD_FORCE_INLINE eBT_PLANE_INTERSECTION_TYPE plane_classify(const btVector4 &plane) const
489         {
490                 btScalar _fmin,_fmax;
491                 this->projection_interval(plane,_fmin,_fmax);
492
493                 if(plane[3] > _fmax + BOX_PLANE_EPSILON)
494                 {
495                         return BT_CONST_BACK_PLANE; // 0
496                 }
497
498                 if(plane[3]+BOX_PLANE_EPSILON >=_fmin)
499                 {
500                         return BT_CONST_COLLIDE_PLANE; //1
501                 }
502                 return BT_CONST_FRONT_PLANE;//2
503         }
504
505         SIMD_FORCE_INLINE bool overlapping_trans_conservative(const btAABB & box, btTransform & trans1_to_0) const
506         {
507                 btAABB tbox = box;
508                 tbox.appy_transform(trans1_to_0);
509                 return has_collision(tbox);
510         }
511
512         SIMD_FORCE_INLINE bool overlapping_trans_conservative2(const btAABB & box,
513                 const BT_BOX_BOX_TRANSFORM_CACHE & trans1_to_0) const
514         {
515                 btAABB tbox = box;
516                 tbox.appy_transform_trans_cache(trans1_to_0);
517                 return has_collision(tbox);
518         }
519
520         //! transcache is the transformation cache from box to this AABB
521         SIMD_FORCE_INLINE bool overlapping_trans_cache(
522                 const btAABB & box,const BT_BOX_BOX_TRANSFORM_CACHE & transcache, bool fulltest) const
523         {
524
525                 //Taken from OPCODE
526                 btVector3 ea,eb;//extends
527                 btVector3 ca,cb;//extends
528                 get_center_extend(ca,ea);
529                 box.get_center_extend(cb,eb);
530
531
532                 btVector3 T;
533                 btScalar t,t2;
534                 int i;
535
536                 // Class I : A's basis vectors
537                 for(i=0;i<3;i++)
538                 {
539                         T[i] =  transcache.m_R1to0[i].dot(cb) + transcache.m_T1to0[i] - ca[i];
540                         t = transcache.m_AR[i].dot(eb) + ea[i];
541                         if(BT_GREATER(T[i], t)) return false;
542                 }
543                 // Class II : B's basis vectors
544                 for(i=0;i<3;i++)
545                 {
546                         t = bt_mat3_dot_col(transcache.m_R1to0,T,i);
547                         t2 = bt_mat3_dot_col(transcache.m_AR,ea,i) + eb[i];
548                         if(BT_GREATER(t,t2))    return false;
549                 }
550                 // Class III : 9 cross products
551                 if(fulltest)
552                 {
553                         int j,m,n,o,p,q,r;
554                         for(i=0;i<3;i++)
555                         {
556                                 m = (i+1)%3;
557                                 n = (i+2)%3;
558                                 o = i==0?1:0;
559                                 p = i==2?1:2;
560                                 for(j=0;j<3;j++)
561                                 {
562                                         q = j==2?1:2;
563                                         r = j==0?1:0;
564                                         t = T[n]*transcache.m_R1to0[m][j] - T[m]*transcache.m_R1to0[n][j];
565                                         t2 = ea[o]*transcache.m_AR[p][j] + ea[p]*transcache.m_AR[o][j] +
566                                                 eb[r]*transcache.m_AR[i][q] + eb[q]*transcache.m_AR[i][r];
567                                         if(BT_GREATER(t,t2))    return false;
568                                 }
569                         }
570                 }
571                 return true;
572         }
573
574         //! Simple test for planes.
575         SIMD_FORCE_INLINE bool collide_plane(
576                 const btVector4 & plane) const
577         {
578                 eBT_PLANE_INTERSECTION_TYPE classify = plane_classify(plane);
579                 return (classify == BT_CONST_COLLIDE_PLANE);
580         }
581
582         //! test for a triangle, with edges
583         SIMD_FORCE_INLINE bool collide_triangle_exact(
584                 const btVector3 & p1,
585                 const btVector3 & p2,
586                 const btVector3 & p3,
587                 const btVector4 & triangle_plane) const
588         {
589                 if(!collide_plane(triangle_plane)) return false;
590
591                 btVector3 center,extends;
592                 this->get_center_extend(center,extends);
593
594                 const btVector3 v1(p1 - center);
595                 const btVector3 v2(p2 - center);
596                 const btVector3 v3(p3 - center);
597
598                 //First axis
599                 btVector3 diff(v2 - v1);
600                 btVector3 abs_diff = diff.absolute();
601                 //Test With X axis
602                 TEST_CROSS_EDGE_BOX_X_AXIS_MCR(diff,abs_diff,v1,v3,extends);
603                 //Test With Y axis
604                 TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(diff,abs_diff,v1,v3,extends);
605                 //Test With Z axis
606                 TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(diff,abs_diff,v1,v3,extends);
607
608
609                 diff = v3 - v2;
610                 abs_diff = diff.absolute();
611                 //Test With X axis
612                 TEST_CROSS_EDGE_BOX_X_AXIS_MCR(diff,abs_diff,v2,v1,extends);
613                 //Test With Y axis
614                 TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(diff,abs_diff,v2,v1,extends);
615                 //Test With Z axis
616                 TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(diff,abs_diff,v2,v1,extends);
617
618                 diff = v1 - v3;
619                 abs_diff = diff.absolute();
620                 //Test With X axis
621                 TEST_CROSS_EDGE_BOX_X_AXIS_MCR(diff,abs_diff,v3,v2,extends);
622                 //Test With Y axis
623                 TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(diff,abs_diff,v3,v2,extends);
624                 //Test With Z axis
625                 TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(diff,abs_diff,v3,v2,extends);
626
627                 return true;
628         }
629 };
630
631
632 //! Compairison of transformation objects
633 SIMD_FORCE_INLINE bool btCompareTransformsEqual(const btTransform & t1,const btTransform & t2)
634 {
635         if(!(t1.getOrigin() == t2.getOrigin()) ) return false;
636
637         if(!(t1.getBasis().getRow(0) == t2.getBasis().getRow(0)) ) return false;
638         if(!(t1.getBasis().getRow(1) == t2.getBasis().getRow(1)) ) return false;
639         if(!(t1.getBasis().getRow(2) == t2.getBasis().getRow(2)) ) return false;
640         return true;
641 }
642
643
644
645 #endif // GIM_BOX_COLLISION_H_INCLUDED