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