Bullet Collision Detection & Physics Library
gim_box_collision.h
Go to the documentation of this file.
1 #ifndef GIM_BOX_COLLISION_H_INCLUDED
2 #define GIM_BOX_COLLISION_H_INCLUDED
3 
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 
135 class GIM_BOX_BOX_TRANSFORM_CACHE
136 {
137 public:
138  btVector3 m_T1to0;
139  btMatrix3x3 m_R1to0;
140  btMatrix3x3 m_AR;
141 
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 
150  GIM_BOX_BOX_TRANSFORM_CACHE()
151  {
152  }
153 
154 
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 
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 
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 
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 
197 class GIM_AABB
198 {
199 public:
200  btVector3 m_min;
201  btVector3 m_max;
202 
203  GIM_AABB()
204  {}
205 
206 
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 
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 
241  GIM_AABB(const GIM_AABB &other):
242  m_min(other.m_min),m_max(other.m_max)
243  {
244  }
245 
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 
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 
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 
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>
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>
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 
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 
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 
354  template<typename CLASS_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 
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 
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 
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 
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 
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 
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 
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 
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 
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 
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 
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
Generated on Thu Oct 3 2013 18:25:07 for Bullet Collision Detection & Physics Library by   doxygen 1.8.4