Bullet Collision Detection & Physics Library
gim_basic_geometry_operations.h
Go to the documentation of this file.
1 #ifndef GIM_BASIC_GEOMETRY_OPERATIONS_H_INCLUDED
2 #define GIM_BASIC_GEOMETRY_OPERATIONS_H_INCLUDED
3 
9 /*
10 -----------------------------------------------------------------------------
11 This source file is part of GIMPACT Library.
12 
13 For the latest info, see http://gimpact.sourceforge.net/
14 
15 Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371.
16 email: projectileman@yahoo.com
17 
18  This library is free software; you can redistribute it and/or
19  modify it under the terms of EITHER:
20  (1) The GNU Lesser General Public License as published by the Free
21  Software Foundation; either version 2.1 of the License, or (at
22  your option) any later version. The text of the GNU Lesser
23  General Public License is included with this library in the
24  file GIMPACT-LICENSE-LGPL.TXT.
25  (2) The BSD-style license that is included with this library in
26  the file GIMPACT-LICENSE-BSD.TXT.
27  (3) The zlib/libpng license that is included with this library in
28  the file GIMPACT-LICENSE-ZLIB.TXT.
29 
30  This library is distributed in the hope that it will be useful,
31  but WITHOUT ANY WARRANTY; without even the implied warranty of
32  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files
33  GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details.
34 
35 -----------------------------------------------------------------------------
36 */
37 
38 
39 #include "gim_linear_math.h"
40 
41 
42 
43 
44 
45 #define PLANEDIREPSILON 0.0000001f
46 #define PARALELENORMALS 0.000001f
47 
48 
49 #define TRIANGLE_NORMAL(v1,v2,v3,n)\
50 {\
51  vec3f _dif1,_dif2;\
52  VEC_DIFF(_dif1,v2,v1);\
53  VEC_DIFF(_dif2,v3,v1);\
54  VEC_CROSS(n,_dif1,_dif2);\
55  VEC_NORMALIZE(n);\
56 }\
57 
58 #define TRIANGLE_NORMAL_FAST(v1,v2,v3,n){\
59  vec3f _dif1,_dif2; \
60  VEC_DIFF(_dif1,v2,v1); \
61  VEC_DIFF(_dif2,v3,v1); \
62  VEC_CROSS(n,_dif1,_dif2); \
63 }\
64 
65 #define TRIANGLE_PLANE(v1,v2,v3,plane) {\
67  TRIANGLE_NORMAL(v1,v2,v3,plane);\
68  plane[3] = VEC_DOT(v1,plane);\
69 }\
70 
71 #define TRIANGLE_PLANE_FAST(v1,v2,v3,plane) {\
73  TRIANGLE_NORMAL_FAST(v1,v2,v3,plane);\
74  plane[3] = VEC_DOT(v1,plane);\
75 }\
76 
77 #define EDGE_PLANE(e1,e2,n,plane) {\
79  vec3f _dif; \
80  VEC_DIFF(_dif,e2,e1); \
81  VEC_CROSS(plane,_dif,n); \
82  VEC_NORMALIZE(plane); \
83  plane[3] = VEC_DOT(e1,plane);\
84 }\
85 
86 #define DISTANCE_PLANE_POINT(plane,point) (VEC_DOT(plane,point) - plane[3])
87 
88 #define PROJECT_POINT_PLANE(point,plane,projected) {\
89  GREAL _dis;\
90  _dis = DISTANCE_PLANE_POINT(plane,point);\
91  VEC_SCALE(projected,-_dis,plane);\
92  VEC_SUM(projected,projected,point); \
93 }\
94 
95 template<typename CLASS_POINT,typename CLASS_PLANE>
97 SIMD_FORCE_INLINE bool POINT_IN_HULL(
98  const CLASS_POINT& point,const CLASS_PLANE * planes,GUINT plane_count)
99 {
100  GREAL _dis;
101  for (GUINT _i = 0;_i< plane_count;++_i)
102  {
103  _dis = DISTANCE_PLANE_POINT(planes[_i],point);
104  if(_dis>0.0f) return false;
105  }
106  return true;
107 }
108 
109 template<typename CLASS_POINT,typename CLASS_PLANE>
110 SIMD_FORCE_INLINE void PLANE_CLIP_SEGMENT(
111  const CLASS_POINT& s1,
112  const CLASS_POINT &s2,const CLASS_PLANE &plane,CLASS_POINT &clipped)
113 {
114  GREAL _dis1,_dis2;
115  _dis1 = DISTANCE_PLANE_POINT(plane,s1);
116  VEC_DIFF(clipped,s2,s1);
117  _dis2 = VEC_DOT(clipped,plane);
118  VEC_SCALE(clipped,-_dis1/_dis2,clipped);
119  VEC_SUM(clipped,clipped,s1);
120 }
121 
122 enum ePLANE_INTERSECTION_TYPE
123 {
124  G_BACK_PLANE = 0,
125  G_COLLIDE_PLANE,
126  G_FRONT_PLANE
127 };
128 
129 enum eLINE_PLANE_INTERSECTION_TYPE
130 {
131  G_FRONT_PLANE_S1 = 0,
132  G_FRONT_PLANE_S2,
133  G_BACK_PLANE_S1,
134  G_BACK_PLANE_S2,
135  G_COLLIDE_PLANE_S1,
136  G_COLLIDE_PLANE_S2
137 };
138 
140 
152 template<typename CLASS_POINT,typename CLASS_PLANE>
153 SIMD_FORCE_INLINE eLINE_PLANE_INTERSECTION_TYPE PLANE_CLIP_SEGMENT2(
154  const CLASS_POINT& s1,
155  const CLASS_POINT &s2,
156  const CLASS_PLANE &plane,CLASS_POINT &clipped)
157 {
158  GREAL _dis1 = DISTANCE_PLANE_POINT(plane,s1);
159  GREAL _dis2 = DISTANCE_PLANE_POINT(plane,s2);
160  if(_dis1 >-G_EPSILON && _dis2 >-G_EPSILON)
161  {
162  if(_dis1<_dis2) return G_FRONT_PLANE_S1;
163  return G_FRONT_PLANE_S2;
164  }
165  else if(_dis1 <G_EPSILON && _dis2 <G_EPSILON)
166  {
167  if(_dis1>_dis2) return G_BACK_PLANE_S1;
168  return G_BACK_PLANE_S2;
169  }
170 
171  VEC_DIFF(clipped,s2,s1);
172  _dis2 = VEC_DOT(clipped,plane);
173  VEC_SCALE(clipped,-_dis1/_dis2,clipped);
174  VEC_SUM(clipped,clipped,s1);
175  if(_dis1<_dis2) return G_COLLIDE_PLANE_S1;
176  return G_COLLIDE_PLANE_S2;
177 }
178 
180 
194 template<typename CLASS_POINT,typename CLASS_PLANE>
195 SIMD_FORCE_INLINE eLINE_PLANE_INTERSECTION_TYPE PLANE_CLIP_SEGMENT_CLOSEST(
196  const CLASS_POINT& s1,
197  const CLASS_POINT &s2,
198  const CLASS_PLANE &plane,
199  CLASS_POINT &clipped1,CLASS_POINT &clipped2)
200 {
201  eLINE_PLANE_INTERSECTION_TYPE intersection_type = PLANE_CLIP_SEGMENT2(s1,s2,plane,clipped1);
202  switch(intersection_type)
203  {
204  case G_FRONT_PLANE_S1:
205  VEC_COPY(clipped1,s1);
206  VEC_COPY(clipped2,s2);
207  break;
208  case G_FRONT_PLANE_S2:
209  VEC_COPY(clipped1,s2);
210  VEC_COPY(clipped2,s1);
211  break;
212  case G_BACK_PLANE_S1:
213  VEC_COPY(clipped1,s1);
214  VEC_COPY(clipped2,s2);
215  break;
216  case G_BACK_PLANE_S2:
217  VEC_COPY(clipped1,s2);
218  VEC_COPY(clipped2,s1);
219  break;
220  case G_COLLIDE_PLANE_S1:
221  VEC_COPY(clipped2,s1);
222  break;
223  case G_COLLIDE_PLANE_S2:
224  VEC_COPY(clipped2,s2);
225  break;
226  }
227  return intersection_type;
228 }
229 
230 
232 #define PLANE_MINOR_AXES(plane, i0, i1) VEC_MINOR_AXES(plane, i0, i1)
233 
235 
239 template<typename T,typename CLASS_POINT,typename CLASS_PLANE>
240 SIMD_FORCE_INLINE bool RAY_PLANE_COLLISION(
241  const CLASS_PLANE & plane,
242  const CLASS_POINT & vDir,
243  const CLASS_POINT & vPoint,
244  CLASS_POINT & pout,T &tparam)
245 {
246  GREAL _dis,_dotdir;
247  _dotdir = VEC_DOT(plane,vDir);
248  if(_dotdir<PLANEDIREPSILON)
249  {
250  return false;
251  }
252  _dis = DISTANCE_PLANE_POINT(plane,vPoint);
253  tparam = -_dis/_dotdir;
254  VEC_SCALE(pout,tparam,vDir);
255  VEC_SUM(pout,vPoint,pout);
256  return true;
257 }
258 
260 
266 template<typename T,typename CLASS_POINT,typename CLASS_PLANE>
267 SIMD_FORCE_INLINE GUINT LINE_PLANE_COLLISION(
268  const CLASS_PLANE & plane,
269  const CLASS_POINT & vDir,
270  const CLASS_POINT & vPoint,
271  CLASS_POINT & pout,
272  T &tparam,
273  T tmin, T tmax)
274 {
275  GREAL _dis,_dotdir;
276  _dotdir = VEC_DOT(plane,vDir);
277  if(btFabs(_dotdir)<PLANEDIREPSILON)
278  {
279  tparam = tmax;
280  return 0;
281  }
282  _dis = DISTANCE_PLANE_POINT(plane,vPoint);
283  char returnvalue = _dis<0.0f?2:1;
284  tparam = -_dis/_dotdir;
285 
286  if(tparam<tmin)
287  {
288  returnvalue = 0;
289  tparam = tmin;
290  }
291  else if(tparam>tmax)
292  {
293  returnvalue = 0;
294  tparam = tmax;
295  }
296 
297  VEC_SCALE(pout,tparam,vDir);
298  VEC_SUM(pout,vPoint,pout);
299  return returnvalue;
300 }
301 
312 template<typename CLASS_POINT,typename CLASS_PLANE>
313 SIMD_FORCE_INLINE bool INTERSECT_PLANES(
314  const CLASS_PLANE &p1,
315  const CLASS_PLANE &p2,
316  CLASS_POINT &p,
317  CLASS_POINT &d)
318 {
319  VEC_CROSS(d,p1,p2);
320  GREAL denom = VEC_DOT(d, d);
321  if(GIM_IS_ZERO(denom)) return false;
322  vec3f _n;
323  _n[0]=p1[3]*p2[0] - p2[3]*p1[0];
324  _n[1]=p1[3]*p2[1] - p2[3]*p1[1];
325  _n[2]=p1[3]*p2[2] - p2[3]*p1[2];
326  VEC_CROSS(p,_n,d);
327  p[0]/=denom;
328  p[1]/=denom;
329  p[2]/=denom;
330  return true;
331 }
332 
333 //***************** SEGMENT and LINE FUNCTIONS **********************************///
334 
337 template<typename CLASS_POINT>
338 SIMD_FORCE_INLINE void CLOSEST_POINT_ON_SEGMENT(
339  CLASS_POINT & cp, const CLASS_POINT & v,
340  const CLASS_POINT &e1,const CLASS_POINT &e2)
341 {
342  vec3f _n;
343  VEC_DIFF(_n,e2,e1);
344  VEC_DIFF(cp,v,e1);
345  GREAL _scalar = VEC_DOT(cp, _n);
346  _scalar/= VEC_DOT(_n, _n);
347  if(_scalar <0.0f)
348  {
349  VEC_COPY(cp,e1);
350  }
351  else if(_scalar >1.0f)
352  {
353  VEC_COPY(cp,e2);
354  }
355  else
356  {
357  VEC_SCALE(cp,_scalar,_n);
358  VEC_SUM(cp,cp,e1);
359  }
360 }
361 
362 
374 template<typename T,typename CLASS_POINT>
375 SIMD_FORCE_INLINE bool LINE_INTERSECTION_PARAMS(
376  const CLASS_POINT & dir1,
377  CLASS_POINT & point1,
378  const CLASS_POINT & dir2,
379  CLASS_POINT & point2,
380  T& t1,T& t2)
381 {
382  GREAL det;
383  GREAL e1e1 = VEC_DOT(dir1,dir1);
384  GREAL e1e2 = VEC_DOT(dir1,dir2);
385  GREAL e2e2 = VEC_DOT(dir2,dir2);
386  vec3f p1p2;
387  VEC_DIFF(p1p2,point1,point2);
388  GREAL p1p2e1 = VEC_DOT(p1p2,dir1);
389  GREAL p1p2e2 = VEC_DOT(p1p2,dir2);
390  det = e1e2*e1e2 - e1e1*e2e2;
391  if(GIM_IS_ZERO(det)) return false;
392  t1 = (e1e2*p1p2e2 - e2e2*p1p2e1)/det;
393  t2 = (e1e1*p1p2e2 - e1e2*p1p2e1)/det;
394  return true;
395 }
396 
398 template<typename CLASS_POINT>
399 SIMD_FORCE_INLINE void SEGMENT_COLLISION(
400  const CLASS_POINT & vA1,
401  const CLASS_POINT & vA2,
402  const CLASS_POINT & vB1,
403  const CLASS_POINT & vB2,
404  CLASS_POINT & vPointA,
405  CLASS_POINT & vPointB)
406 {
407  CLASS_POINT _AD,_BD,_N;
408  vec4f _M;//plane
409  VEC_DIFF(_AD,vA2,vA1);
410  VEC_DIFF(_BD,vB2,vB1);
411  VEC_CROSS(_N,_AD,_BD);
412  GREAL _tp = VEC_DOT(_N,_N);
413  if(_tp<G_EPSILON)//ARE PARALELE
414  {
415  //project B over A
416  bool invert_b_order = false;
417  _M[0] = VEC_DOT(vB1,_AD);
418  _M[1] = VEC_DOT(vB2,_AD);
419  if(_M[0]>_M[1])
420  {
421  invert_b_order = true;
422  GIM_SWAP_NUMBERS(_M[0],_M[1]);
423  }
424  _M[2] = VEC_DOT(vA1,_AD);
425  _M[3] = VEC_DOT(vA2,_AD);
426  //mid points
427  _N[0] = (_M[0]+_M[1])*0.5f;
428  _N[1] = (_M[2]+_M[3])*0.5f;
429 
430  if(_N[0]<_N[1])
431  {
432  if(_M[1]<_M[2])
433  {
434  vPointB = invert_b_order?vB1:vB2;
435  vPointA = vA1;
436  }
437  else if(_M[1]<_M[3])
438  {
439  vPointB = invert_b_order?vB1:vB2;
440  CLOSEST_POINT_ON_SEGMENT(vPointA,vPointB,vA1,vA2);
441  }
442  else
443  {
444  vPointA = vA2;
445  CLOSEST_POINT_ON_SEGMENT(vPointB,vPointA,vB1,vB2);
446  }
447  }
448  else
449  {
450  if(_M[3]<_M[0])
451  {
452  vPointB = invert_b_order?vB2:vB1;
453  vPointA = vA2;
454  }
455  else if(_M[3]<_M[1])
456  {
457  vPointA = vA2;
458  CLOSEST_POINT_ON_SEGMENT(vPointB,vPointA,vB1,vB2);
459  }
460  else
461  {
462  vPointB = invert_b_order?vB1:vB2;
463  CLOSEST_POINT_ON_SEGMENT(vPointA,vPointB,vA1,vA2);
464  }
465  }
466  return;
467  }
468 
469 
470  VEC_CROSS(_M,_N,_BD);
471  _M[3] = VEC_DOT(_M,vB1);
472 
473  LINE_PLANE_COLLISION(_M,_AD,vA1,vPointA,_tp,btScalar(0), btScalar(1));
474  /*Closest point on segment*/
475  VEC_DIFF(vPointB,vPointA,vB1);
476  _tp = VEC_DOT(vPointB, _BD);
477  _tp/= VEC_DOT(_BD, _BD);
478  _tp = GIM_CLAMP(_tp,0.0f,1.0f);
479  VEC_SCALE(vPointB,_tp,_BD);
480  VEC_SUM(vPointB,vPointB,vB1);
481 }
482 
483 
484 
485 
487 
497 template<typename T>
498 SIMD_FORCE_INLINE bool BOX_AXIS_INTERSECT(T pos, T dir,T bmin, T bmax, T & tfirst, T & tlast)
499 {
500  if(GIM_IS_ZERO(dir))
501  {
502  return !(pos < bmin || pos > bmax);
503  }
504  GREAL a0 = (bmin - pos) / dir;
505  GREAL a1 = (bmax - pos) / dir;
506  if(a0 > a1) GIM_SWAP_NUMBERS(a0, a1);
507  tfirst = GIM_MAX(a0, tfirst);
508  tlast = GIM_MIN(a1, tlast);
509  if (tlast < tfirst) return false;
510  return true;
511 }
512 
513 
515 template<typename T>
516 SIMD_FORCE_INLINE void SORT_3_INDICES(
517  const T * values,
518  GUINT * order_indices)
519 {
520  //get minimum
521  order_indices[0] = values[0] < values[1] ? (values[0] < values[2] ? 0 : 2) : (values[1] < values[2] ? 1 : 2);
522 
523  //get second and third
524  GUINT i0 = (order_indices[0] + 1)%3;
525  GUINT i1 = (i0 + 1)%3;
526 
527  if(values[i0] < values[i1])
528  {
529  order_indices[1] = i0;
530  order_indices[2] = i1;
531  }
532  else
533  {
534  order_indices[1] = i1;
535  order_indices[2] = i0;
536  }
537 }
538 
539 
540 
541 
542 
543 #endif // GIM_VECTOR_H_INCLUDED
Generated on Thu Oct 3 2013 18:25:07 for Bullet Collision Detection & Physics Library by   doxygen 1.8.4