Bullet Collision Detection & Physics Library
gim_tri_collision.cpp
Go to the documentation of this file.
1 
5 /*
6 -----------------------------------------------------------------------------
7 This source file is part of GIMPACT Library.
8 
9 For the latest info, see http://gimpact.sourceforge.net/
10 
11 Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371.
12 email: projectileman@yahoo.com
13 
14  This library is free software; you can redistribute it and/or
15  modify it under the terms of EITHER:
16  (1) The GNU Lesser General Public License as published by the Free
17  Software Foundation; either version 2.1 of the License, or (at
18  your option) any later version. The text of the GNU Lesser
19  General Public License is included with this library in the
20  file GIMPACT-LICENSE-LGPL.TXT.
21  (2) The BSD-style license that is included with this library in
22  the file GIMPACT-LICENSE-BSD.TXT.
23  (3) The zlib/libpng license that is included with this library in
24  the file GIMPACT-LICENSE-ZLIB.TXT.
25 
26  This library is distributed in the hope that it will be useful,
27  but WITHOUT ANY WARRANTY; without even the implied warranty of
28  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files
29  GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details.
30 
31 -----------------------------------------------------------------------------
32 */
33 
34 #include "gim_tri_collision.h"
35 
36 
37 #define TRI_LOCAL_EPSILON 0.000001f
38 #define MIN_EDGE_EDGE_DIS 0.00001f
39 
40 
41 class GIM_TRIANGLE_CALCULATION_CACHE
42 {
43 public:
44  GREAL margin;
45  btVector3 tu_vertices[3];
46  btVector3 tv_vertices[3];
47  btVector4 tu_plane;
48  btVector4 tv_plane;
49  btVector3 closest_point_u;
50  btVector3 closest_point_v;
51  btVector3 edge_edge_dir;
52  btVector3 distances;
53  GREAL du[4];
54  GREAL du0du1;
55  GREAL du0du2;
56  GREAL dv[4];
57  GREAL dv0dv1;
58  GREAL dv0dv2;
59  btVector3 temp_points[MAX_TRI_CLIPPING];
60  btVector3 temp_points1[MAX_TRI_CLIPPING];
61  btVector3 contact_points[MAX_TRI_CLIPPING];
62 
63 
64 
66  SIMD_FORCE_INLINE bool compute_intervals(
67  const GREAL &D0,
68  const GREAL &D1,
69  const GREAL &D2,
70  const GREAL &D0D1,
71  const GREAL &D0D2,
72  GREAL & scale_edge0,
73  GREAL & scale_edge1,
74  GUINT &edge_index0,
75  GUINT &edge_index1)
76  {
77  if(D0D1>0.0f)
78  {
79  /* here we know that D0D2<=0.0 */
80  /* that is D0, D1 are on the same side, D2 on the other or on the plane */
81  scale_edge0 = -D2/(D0-D2);
82  scale_edge1 = -D1/(D2-D1);
83  edge_index0 = 2;edge_index1 = 1;
84  }
85  else if(D0D2>0.0f)
86  {
87  /* here we know that d0d1<=0.0 */
88  scale_edge0 = -D0/(D1-D0);
89  scale_edge1 = -D1/(D2-D1);
90  edge_index0 = 0;edge_index1 = 1;
91  }
92  else if(D1*D2>0.0f || D0!=0.0f)
93  {
94  /* here we know that d0d1<=0.0 or that D0!=0.0 */
95  scale_edge0 = -D0/(D1-D0);
96  scale_edge1 = -D2/(D0-D2);
97  edge_index0 = 0 ;edge_index1 = 2;
98  }
99  else
100  {
101  return false;
102  }
103  return true;
104  }
105 
106 
108 
110  SIMD_FORCE_INLINE GUINT clip_triangle(
111  const btVector4 & tri_plane,
112  const btVector3 * tripoints,
113  const btVector3 * srcpoints,
114  btVector3 * clip_points)
115  {
116  // edge 0
117 
118  btVector4 edgeplane;
119 
120  EDGE_PLANE(tripoints[0],tripoints[1],tri_plane,edgeplane);
121 
122  GUINT clipped_count = PLANE_CLIP_TRIANGLE3D(
123  edgeplane,srcpoints[0],srcpoints[1],srcpoints[2],temp_points);
124 
125  if(clipped_count == 0) return 0;
126 
127  // edge 1
128 
129  EDGE_PLANE(tripoints[1],tripoints[2],tri_plane,edgeplane);
130 
131  clipped_count = PLANE_CLIP_POLYGON3D(
132  edgeplane,temp_points,clipped_count,temp_points1);
133 
134  if(clipped_count == 0) return 0;
135 
136  // edge 2
137 
138  EDGE_PLANE(tripoints[2],tripoints[0],tri_plane,edgeplane);
139 
140  clipped_count = PLANE_CLIP_POLYGON3D(
141  edgeplane,temp_points1,clipped_count,clip_points);
142 
143  return clipped_count;
144 
145 
146  /*GUINT i0 = (tri_plane.closestAxis()+1)%3;
147  GUINT i1 = (i0+1)%3;
148  // edge 0
149  btVector3 temp_points[MAX_TRI_CLIPPING];
150  btVector3 temp_points1[MAX_TRI_CLIPPING];
151 
152  GUINT clipped_count= PLANE_CLIP_TRIANGLE_GENERIC(
153  0,srcpoints[0],srcpoints[1],srcpoints[2],temp_points,
154  DISTANCE_EDGE(tripoints[0],tripoints[1],i0,i1));
155 
156 
157  if(clipped_count == 0) return 0;
158 
159  // edge 1
160  clipped_count = PLANE_CLIP_POLYGON_GENERIC(
161  0,temp_points,clipped_count,temp_points1,
162  DISTANCE_EDGE(tripoints[1],tripoints[2],i0,i1));
163 
164  if(clipped_count == 0) return 0;
165 
166  // edge 2
167  clipped_count = PLANE_CLIP_POLYGON_GENERIC(
168  0,temp_points1,clipped_count,clipped_points,
169  DISTANCE_EDGE(tripoints[2],tripoints[0],i0,i1));
170 
171  return clipped_count;*/
172  }
173 
174  SIMD_FORCE_INLINE void sort_isect(
175  GREAL & isect0,GREAL & isect1,GUINT &e0,GUINT &e1,btVector3 & vec0,btVector3 & vec1)
176  {
177  if(isect1<isect0)
178  {
179  //swap
180  GIM_SWAP_NUMBERS(isect0,isect1);
181  GIM_SWAP_NUMBERS(e0,e1);
182  btVector3 tmp = vec0;
183  vec0 = vec1;
184  vec1 = tmp;
185  }
186  }
187 
189 
200  SIMD_FORCE_INLINE GUINT cross_line_intersection_test()
201  {
202  // Compute direction of intersection line
203  edge_edge_dir = tu_plane.cross(tv_plane);
204  GREAL Dlen;
205  VEC_LENGTH(edge_edge_dir,Dlen);
206 
207  if(Dlen<0.0001)
208  {
209  return 0; //faces near paralele
210  }
211 
212  edge_edge_dir*= 1/Dlen;//normalize
213 
214 
215  // Compute interval for triangle 1
216  GUINT tu_e0,tu_e1;//edge indices
217  GREAL tu_scale_e0,tu_scale_e1;//edge scale
218  if(!compute_intervals(du[0],du[1],du[2],
219  du0du1,du0du2,tu_scale_e0,tu_scale_e1,tu_e0,tu_e1)) return 0;
220 
221  // Compute interval for triangle 2
222  GUINT tv_e0,tv_e1;//edge indices
223  GREAL tv_scale_e0,tv_scale_e1;//edge scale
224 
225  if(!compute_intervals(dv[0],dv[1],dv[2],
226  dv0dv1,dv0dv2,tv_scale_e0,tv_scale_e1,tv_e0,tv_e1)) return 0;
227 
228  //proyected vertices
229  btVector3 up_e0 = tu_vertices[tu_e0].lerp(tu_vertices[(tu_e0+1)%3],tu_scale_e0);
230  btVector3 up_e1 = tu_vertices[tu_e1].lerp(tu_vertices[(tu_e1+1)%3],tu_scale_e1);
231 
232  btVector3 vp_e0 = tv_vertices[tv_e0].lerp(tv_vertices[(tv_e0+1)%3],tv_scale_e0);
233  btVector3 vp_e1 = tv_vertices[tv_e1].lerp(tv_vertices[(tv_e1+1)%3],tv_scale_e1);
234 
235  //proyected intervals
236  GREAL isect_u[] = {up_e0.dot(edge_edge_dir),up_e1.dot(edge_edge_dir)};
237  GREAL isect_v[] = {vp_e0.dot(edge_edge_dir),vp_e1.dot(edge_edge_dir)};
238 
239  sort_isect(isect_u[0],isect_u[1],tu_e0,tu_e1,up_e0,up_e1);
240  sort_isect(isect_v[0],isect_v[1],tv_e0,tv_e1,vp_e0,vp_e1);
241 
242  const GREAL midpoint_u = 0.5f*(isect_u[0]+isect_u[1]); // midpoint
243  const GREAL midpoint_v = 0.5f*(isect_v[0]+isect_v[1]); // midpoint
244 
245  if(midpoint_u<midpoint_v)
246  {
247  if(isect_u[1]>=isect_v[1]) // face U casts face V
248  {
249  return 1;
250  }
251  else if(isect_v[0]<=isect_u[0]) // face V casts face U
252  {
253  return 2;
254  }
255  // closest points
256  closest_point_u = up_e1;
257  closest_point_v = vp_e0;
258  // calc edges and separation
259 
260  if(isect_u[1]+ MIN_EDGE_EDGE_DIS<isect_v[0]) //calc distance between two lines instead
261  {
262  SEGMENT_COLLISION(
263  tu_vertices[tu_e1],tu_vertices[(tu_e1+1)%3],
264  tv_vertices[tv_e0],tv_vertices[(tv_e0+1)%3],
265  closest_point_u,
266  closest_point_v);
267 
268  edge_edge_dir = closest_point_u-closest_point_v;
269  VEC_LENGTH(edge_edge_dir,distances[2]);
270  edge_edge_dir *= 1.0f/distances[2];// normalize
271  }
272  else
273  {
274  distances[2] = isect_v[0]-isect_u[1];//distance negative
275  //edge_edge_dir *= -1.0f; //normal pointing from V to U
276  }
277 
278  }
279  else
280  {
281  if(isect_v[1]>=isect_u[1]) // face V casts face U
282  {
283  return 2;
284  }
285  else if(isect_u[0]<=isect_v[0]) // face U casts face V
286  {
287  return 1;
288  }
289  // closest points
290  closest_point_u = up_e0;
291  closest_point_v = vp_e1;
292  // calc edges and separation
293 
294  if(isect_v[1]+MIN_EDGE_EDGE_DIS<isect_u[0]) //calc distance between two lines instead
295  {
296  SEGMENT_COLLISION(
297  tu_vertices[tu_e0],tu_vertices[(tu_e0+1)%3],
298  tv_vertices[tv_e1],tv_vertices[(tv_e1+1)%3],
299  closest_point_u,
300  closest_point_v);
301 
302  edge_edge_dir = closest_point_u-closest_point_v;
303  VEC_LENGTH(edge_edge_dir,distances[2]);
304  edge_edge_dir *= 1.0f/distances[2];// normalize
305  }
306  else
307  {
308  distances[2] = isect_u[0]-isect_v[1];//distance negative
309  //edge_edge_dir *= -1.0f; //normal pointing from V to U
310  }
311  }
312  return 3;
313  }
314 
315 
317  SIMD_FORCE_INLINE bool triangle_collision(
318  const btVector3 & u0,
319  const btVector3 & u1,
320  const btVector3 & u2,
321  GREAL margin_u,
322  const btVector3 & v0,
323  const btVector3 & v1,
324  const btVector3 & v2,
325  GREAL margin_v,
326  GIM_TRIANGLE_CONTACT_DATA & contacts)
327  {
328 
329  margin = margin_u + margin_v;
330 
331  tu_vertices[0] = u0;
332  tu_vertices[1] = u1;
333  tu_vertices[2] = u2;
334 
335  tv_vertices[0] = v0;
336  tv_vertices[1] = v1;
337  tv_vertices[2] = v2;
338 
339  //create planes
340  // plane v vs U points
341 
342  TRIANGLE_PLANE(tv_vertices[0],tv_vertices[1],tv_vertices[2],tv_plane);
343 
344  du[0] = DISTANCE_PLANE_POINT(tv_plane,tu_vertices[0]);
345  du[1] = DISTANCE_PLANE_POINT(tv_plane,tu_vertices[1]);
346  du[2] = DISTANCE_PLANE_POINT(tv_plane,tu_vertices[2]);
347 
348 
349  du0du1 = du[0] * du[1];
350  du0du2 = du[0] * du[2];
351 
352 
353  if(du0du1>0.0f && du0du2>0.0f) // same sign on all of them + not equal 0 ?
354  {
355  if(du[0]<0) //we need test behind the triangle plane
356  {
357  distances[0] = GIM_MAX3(du[0],du[1],du[2]);
358  distances[0] = -distances[0];
359  if(distances[0]>margin) return false; //never intersect
360 
361  //reorder triangle v
362  VEC_SWAP(tv_vertices[0],tv_vertices[1]);
363  VEC_SCALE_4(tv_plane,-1.0f,tv_plane);
364  }
365  else
366  {
367  distances[0] = GIM_MIN3(du[0],du[1],du[2]);
368  if(distances[0]>margin) return false; //never intersect
369  }
370  }
371  else
372  {
373  //Look if we need to invert the triangle
374  distances[0] = (du[0]+du[1]+du[2])/3.0f; //centroid
375 
376  if(distances[0]<0.0f)
377  {
378  //reorder triangle v
379  VEC_SWAP(tv_vertices[0],tv_vertices[1]);
380  VEC_SCALE_4(tv_plane,-1.0f,tv_plane);
381 
382  distances[0] = GIM_MAX3(du[0],du[1],du[2]);
383  distances[0] = -distances[0];
384  }
385  else
386  {
387  distances[0] = GIM_MIN3(du[0],du[1],du[2]);
388  }
389  }
390 
391 
392  // plane U vs V points
393 
394  TRIANGLE_PLANE(tu_vertices[0],tu_vertices[1],tu_vertices[2],tu_plane);
395 
396  dv[0] = DISTANCE_PLANE_POINT(tu_plane,tv_vertices[0]);
397  dv[1] = DISTANCE_PLANE_POINT(tu_plane,tv_vertices[1]);
398  dv[2] = DISTANCE_PLANE_POINT(tu_plane,tv_vertices[2]);
399 
400  dv0dv1 = dv[0] * dv[1];
401  dv0dv2 = dv[0] * dv[2];
402 
403 
404  if(dv0dv1>0.0f && dv0dv2>0.0f) // same sign on all of them + not equal 0 ?
405  {
406  if(dv[0]<0) //we need test behind the triangle plane
407  {
408  distances[1] = GIM_MAX3(dv[0],dv[1],dv[2]);
409  distances[1] = -distances[1];
410  if(distances[1]>margin) return false; //never intersect
411 
412  //reorder triangle u
413  VEC_SWAP(tu_vertices[0],tu_vertices[1]);
414  VEC_SCALE_4(tu_plane,-1.0f,tu_plane);
415  }
416  else
417  {
418  distances[1] = GIM_MIN3(dv[0],dv[1],dv[2]);
419  if(distances[1]>margin) return false; //never intersect
420  }
421  }
422  else
423  {
424  //Look if we need to invert the triangle
425  distances[1] = (dv[0]+dv[1]+dv[2])/3.0f; //centroid
426 
427  if(distances[1]<0.0f)
428  {
429  //reorder triangle v
430  VEC_SWAP(tu_vertices[0],tu_vertices[1]);
431  VEC_SCALE_4(tu_plane,-1.0f,tu_plane);
432 
433  distances[1] = GIM_MAX3(dv[0],dv[1],dv[2]);
434  distances[1] = -distances[1];
435  }
436  else
437  {
438  distances[1] = GIM_MIN3(dv[0],dv[1],dv[2]);
439  }
440  }
441 
442  GUINT bl;
443  /* bl = cross_line_intersection_test();
444  if(bl==3)
445  {
446  //take edge direction too
447  bl = distances.maxAxis();
448  }
449  else
450  {*/
451  bl = 0;
452  if(distances[0]<distances[1]) bl = 1;
453  //}
454 
455  if(bl==2) //edge edge separation
456  {
457  if(distances[2]>margin) return false;
458 
459  contacts.m_penetration_depth = -distances[2] + margin;
460  contacts.m_points[0] = closest_point_v;
461  contacts.m_point_count = 1;
462  VEC_COPY(contacts.m_separating_normal,edge_edge_dir);
463 
464  return true;
465  }
466 
467  //clip face against other
468 
469 
470  GUINT point_count;
471  //TODO
472  if(bl == 0) //clip U points against V
473  {
474  point_count = clip_triangle(tv_plane,tv_vertices,tu_vertices,contact_points);
475  if(point_count == 0) return false;
476  contacts.merge_points(tv_plane,margin,contact_points,point_count);
477  }
478  else //clip V points against U
479  {
480  point_count = clip_triangle(tu_plane,tu_vertices,tv_vertices,contact_points);
481  if(point_count == 0) return false;
482  contacts.merge_points(tu_plane,margin,contact_points,point_count);
483  contacts.m_separating_normal *= -1.f;
484  }
485  if(contacts.m_point_count == 0) return false;
486  return true;
487  }
488 
489 };
490 
491 
492 /*class GIM_TRIANGLE_CALCULATION_CACHE
493 {
494 public:
495  GREAL margin;
496  GUINT clipped_count;
497  btVector3 tu_vertices[3];
498  btVector3 tv_vertices[3];
499  btVector3 temp_points[MAX_TRI_CLIPPING];
500  btVector3 temp_points1[MAX_TRI_CLIPPING];
501  btVector3 clipped_points[MAX_TRI_CLIPPING];
502  GIM_TRIANGLE_CONTACT_DATA contacts1;
503  GIM_TRIANGLE_CONTACT_DATA contacts2;
504 
505 
507  GUINT clip_triangle(
508  const btVector4 & tri_plane,
509  const btVector3 * tripoints,
510  const btVector3 * srcpoints,
511  btVector3 * clipped_points)
512  {
513  // edge 0
514 
515  btVector4 edgeplane;
516 
517  EDGE_PLANE(tripoints[0],tripoints[1],tri_plane,edgeplane);
518 
519  GUINT clipped_count = PLANE_CLIP_TRIANGLE3D(
520  edgeplane,srcpoints[0],srcpoints[1],srcpoints[2],temp_points);
521 
522  if(clipped_count == 0) return 0;
523 
524  // edge 1
525 
526  EDGE_PLANE(tripoints[1],tripoints[2],tri_plane,edgeplane);
527 
528  clipped_count = PLANE_CLIP_POLYGON3D(
529  edgeplane,temp_points,clipped_count,temp_points1);
530 
531  if(clipped_count == 0) return 0;
532 
533  // edge 2
534 
535  EDGE_PLANE(tripoints[2],tripoints[0],tri_plane,edgeplane);
536 
537  clipped_count = PLANE_CLIP_POLYGON3D(
538  edgeplane,temp_points1,clipped_count,clipped_points);
539 
540  return clipped_count;
541  }
542 
543 
544 
545 
547  bool triangle_collision(
548  const btVector3 & u0,
549  const btVector3 & u1,
550  const btVector3 & u2,
551  GREAL margin_u,
552  const btVector3 & v0,
553  const btVector3 & v1,
554  const btVector3 & v2,
555  GREAL margin_v,
556  GIM_TRIANGLE_CONTACT_DATA & contacts)
557  {
558 
559  margin = margin_u + margin_v;
560 
561 
562  tu_vertices[0] = u0;
563  tu_vertices[1] = u1;
564  tu_vertices[2] = u2;
565 
566  tv_vertices[0] = v0;
567  tv_vertices[1] = v1;
568  tv_vertices[2] = v2;
569 
570  //create planes
571  // plane v vs U points
572 
573 
574  TRIANGLE_PLANE(tv_vertices[0],tv_vertices[1],tv_vertices[2],contacts1.m_separating_normal);
575 
576  clipped_count = clip_triangle(
577  contacts1.m_separating_normal,tv_vertices,tu_vertices,clipped_points);
578 
579  if(clipped_count == 0 )
580  {
581  return false;//Reject
582  }
583 
584  //find most deep interval face1
585  contacts1.merge_points(contacts1.m_separating_normal,margin,clipped_points,clipped_count);
586  if(contacts1.m_point_count == 0) return false; // too far
587 
588  //Normal pointing to triangle1
589  //contacts1.m_separating_normal *= -1.f;
590 
591  //Clip tri1 by tri2 edges
592 
593  TRIANGLE_PLANE(tu_vertices[0],tu_vertices[1],tu_vertices[2],contacts2.m_separating_normal);
594 
595  clipped_count = clip_triangle(
596  contacts2.m_separating_normal,tu_vertices,tv_vertices,clipped_points);
597 
598  if(clipped_count == 0 )
599  {
600  return false;//Reject
601  }
602 
603  //find most deep interval face1
604  contacts2.merge_points(contacts2.m_separating_normal,margin,clipped_points,clipped_count);
605  if(contacts2.m_point_count == 0) return false; // too far
606 
607  contacts2.m_separating_normal *= -1.f;
608 
610  if(contacts2.m_penetration_depth<contacts1.m_penetration_depth)
611  {
612  contacts.copy_from(contacts2);
613  }
614  else
615  {
616  contacts.copy_from(contacts1);
617  }
618  return true;
619  }
620 
621 
622 };*/
623 
624 
625 
626 bool GIM_TRIANGLE::collide_triangle_hard_test(
627  const GIM_TRIANGLE & other,
628  GIM_TRIANGLE_CONTACT_DATA & contact_data) const
629 {
630  GIM_TRIANGLE_CALCULATION_CACHE calc_cache;
631  return calc_cache.triangle_collision(
632  m_vertices[0],m_vertices[1],m_vertices[2],m_margin,
633  other.m_vertices[0],other.m_vertices[1],other.m_vertices[2],other.m_margin,
634  contact_data);
635 
636 }
637 
638 
639 
640 
Generated on Thu Oct 3 2013 18:25:08 for Bullet Collision Detection & Physics Library by   doxygen 1.8.4