1
2 /*! \file gim_tri_collision.h
3 \author Francisco Leon Najera
4 */
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
65 //! if returns false, the faces are paralele
compute_intervals(const GREAL & D0,const GREAL & D1,const GREAL & D2,const GREAL & D0D1,const GREAL & D0D2,GREAL & scale_edge0,GREAL & scale_edge1,GUINT & edge_index0,GUINT & edge_index1)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
107 //! clip triangle
108 /*!
109 */
clip_triangle(const btVector4 & tri_plane,const btVector3 * tripoints,const btVector3 * srcpoints,btVector3 * clip_points)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
sort_isect(GREAL & isect0,GREAL & isect1,GUINT & e0,GUINT & e1,btVector3 & vec0,btVector3 & vec1)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
188 //! Test verifying interval intersection with the direction between planes
189 /*!
190 \pre tv_plane and tu_plane must be set
191 \post
192 distances[2] is set with the distance
193 closest_point_u, closest_point_v, edge_edge_dir are set too
194 \return
195 - 0: faces are paralele
196 - 1: face U casts face V
197 - 2: face V casts face U
198 - 3: nearest edges
199 */
cross_line_intersection_test()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
316 //! collides by two sides
triangle_collision(const btVector3 & u0,const btVector3 & u1,const btVector3 & u2,GREAL margin_u,const btVector3 & v0,const btVector3 & v1,const btVector3 & v2,GREAL margin_v,GIM_TRIANGLE_CONTACT_DATA & contacts)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
506 //! clip triangle
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
546 //! collides only on one side
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
609 ////check most dir for contacts
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
collide_triangle_hard_test(const GIM_TRIANGLE & other,GIM_TRIANGLE_CONTACT_DATA & contact_data) const626 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
641