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