2 /*! \file gim_tri_collision.h
3 \author Francisco Leon Najera
6 -----------------------------------------------------------------------------
7 This source file is part of GIMPACT Library.
9 For the latest info, see http://gimpact.sourceforge.net/
11 Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371.
12 email: projectileman@yahoo.com
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.
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.
31 -----------------------------------------------------------------------------
34 #include "gim_tri_collision.h"
37 #define TRI_LOCAL_EPSILON 0.000001f
38 #define MIN_EDGE_EDGE_DIS 0.00001f
41 class GIM_TRIANGLE_CALCULATION_CACHE
45 btVector3 tu_vertices[3];
46 btVector3 tv_vertices[3];
49 btVector3 closest_point_u;
50 btVector3 closest_point_v;
51 btVector3 edge_edge_dir;
59 btVector3 temp_points[MAX_TRI_CLIPPING];
60 btVector3 temp_points1[MAX_TRI_CLIPPING];
61 btVector3 contact_points[MAX_TRI_CLIPPING];
65 //! if returns false, the faces are paralele
66 SIMD_FORCE_INLINE bool compute_intervals(
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;
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;
92 else if(D1*D2>0.0f || D0!=0.0f)
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;
110 SIMD_FORCE_INLINE GUINT clip_triangle(
111 const btVector4 & tri_plane,
112 const btVector3 * tripoints,
113 const btVector3 * srcpoints,
114 btVector3 * clip_points)
120 EDGE_PLANE(tripoints[0],tripoints[1],tri_plane,edgeplane);
122 GUINT clipped_count = PLANE_CLIP_TRIANGLE3D(
123 edgeplane,srcpoints[0],srcpoints[1],srcpoints[2],temp_points);
125 if(clipped_count == 0) return 0;
129 EDGE_PLANE(tripoints[1],tripoints[2],tri_plane,edgeplane);
131 clipped_count = PLANE_CLIP_POLYGON3D(
132 edgeplane,temp_points,clipped_count,temp_points1);
134 if(clipped_count == 0) return 0;
138 EDGE_PLANE(tripoints[2],tripoints[0],tri_plane,edgeplane);
140 clipped_count = PLANE_CLIP_POLYGON3D(
141 edgeplane,temp_points1,clipped_count,clip_points);
143 return clipped_count;
146 /*GUINT i0 = (tri_plane.closestAxis()+1)%3;
149 btVector3 temp_points[MAX_TRI_CLIPPING];
150 btVector3 temp_points1[MAX_TRI_CLIPPING];
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));
157 if(clipped_count == 0) return 0;
160 clipped_count = PLANE_CLIP_POLYGON_GENERIC(
161 0,temp_points,clipped_count,temp_points1,
162 DISTANCE_EDGE(tripoints[1],tripoints[2],i0,i1));
164 if(clipped_count == 0) return 0;
167 clipped_count = PLANE_CLIP_POLYGON_GENERIC(
168 0,temp_points1,clipped_count,clipped_points,
169 DISTANCE_EDGE(tripoints[2],tripoints[0],i0,i1));
171 return clipped_count;*/
174 SIMD_FORCE_INLINE void sort_isect(
175 GREAL & isect0,GREAL & isect1,GUINT &e0,GUINT &e1,btVector3 & vec0,btVector3 & vec1)
180 GIM_SWAP_NUMBERS(isect0,isect1);
181 GIM_SWAP_NUMBERS(e0,e1);
182 btVector3 tmp = vec0;
188 //! Test verifying interval intersection with the direction between planes
190 \pre tv_plane and tu_plane must be set
192 distances[2] is set with the distance
193 closest_point_u, closest_point_v, edge_edge_dir are set too
195 - 0: faces are paralele
196 - 1: face U casts face V
197 - 2: face V casts face U
200 SIMD_FORCE_INLINE GUINT cross_line_intersection_test()
202 // Compute direction of intersection line
203 edge_edge_dir = tu_plane.cross(tv_plane);
205 VEC_LENGTH(edge_edge_dir,Dlen);
209 return 0; //faces near paralele
212 edge_edge_dir*= 1/Dlen;//normalize
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;
221 // Compute interval for triangle 2
222 GUINT tv_e0,tv_e1;//edge indices
223 GREAL tv_scale_e0,tv_scale_e1;//edge scale
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;
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);
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);
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)};
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);
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
245 if(midpoint_u<midpoint_v)
247 if(isect_u[1]>=isect_v[1]) // face U casts face V
251 else if(isect_v[0]<=isect_u[0]) // face V casts face U
256 closest_point_u = up_e1;
257 closest_point_v = vp_e0;
258 // calc edges and separation
260 if(isect_u[1]+ MIN_EDGE_EDGE_DIS<isect_v[0]) //calc distance between two lines instead
263 tu_vertices[tu_e1],tu_vertices[(tu_e1+1)%3],
264 tv_vertices[tv_e0],tv_vertices[(tv_e0+1)%3],
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
274 distances[2] = isect_v[0]-isect_u[1];//distance negative
275 //edge_edge_dir *= -1.0f; //normal pointing from V to U
281 if(isect_v[1]>=isect_u[1]) // face V casts face U
285 else if(isect_u[0]<=isect_v[0]) // face U casts face V
290 closest_point_u = up_e0;
291 closest_point_v = vp_e1;
292 // calc edges and separation
294 if(isect_v[1]+MIN_EDGE_EDGE_DIS<isect_u[0]) //calc distance between two lines instead
297 tu_vertices[tu_e0],tu_vertices[(tu_e0+1)%3],
298 tv_vertices[tv_e1],tv_vertices[(tv_e1+1)%3],
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
308 distances[2] = isect_u[0]-isect_v[1];//distance negative
309 //edge_edge_dir *= -1.0f; //normal pointing from V to U
316 //! collides by two sides
317 SIMD_FORCE_INLINE bool triangle_collision(
318 const btVector3 & u0,
319 const btVector3 & u1,
320 const btVector3 & u2,
322 const btVector3 & v0,
323 const btVector3 & v1,
324 const btVector3 & v2,
326 GIM_TRIANGLE_CONTACT_DATA & contacts)
329 margin = margin_u + margin_v;
340 // plane v vs U points
342 TRIANGLE_PLANE(tv_vertices[0],tv_vertices[1],tv_vertices[2],tv_plane);
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]);
349 du0du1 = du[0] * du[1];
350 du0du2 = du[0] * du[2];
353 if(du0du1>0.0f && du0du2>0.0f) // same sign on all of them + not equal 0 ?
355 if(du[0]<0) //we need test behind the triangle plane
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
362 VEC_SWAP(tv_vertices[0],tv_vertices[1]);
363 VEC_SCALE_4(tv_plane,-1.0f,tv_plane);
367 distances[0] = GIM_MIN3(du[0],du[1],du[2]);
368 if(distances[0]>margin) return false; //never intersect
373 //Look if we need to invert the triangle
374 distances[0] = (du[0]+du[1]+du[2])/3.0f; //centroid
376 if(distances[0]<0.0f)
379 VEC_SWAP(tv_vertices[0],tv_vertices[1]);
380 VEC_SCALE_4(tv_plane,-1.0f,tv_plane);
382 distances[0] = GIM_MAX3(du[0],du[1],du[2]);
383 distances[0] = -distances[0];
387 distances[0] = GIM_MIN3(du[0],du[1],du[2]);
392 // plane U vs V points
394 TRIANGLE_PLANE(tu_vertices[0],tu_vertices[1],tu_vertices[2],tu_plane);
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]);
400 dv0dv1 = dv[0] * dv[1];
401 dv0dv2 = dv[0] * dv[2];
404 if(dv0dv1>0.0f && dv0dv2>0.0f) // same sign on all of them + not equal 0 ?
406 if(dv[0]<0) //we need test behind the triangle plane
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
413 VEC_SWAP(tu_vertices[0],tu_vertices[1]);
414 VEC_SCALE_4(tu_plane,-1.0f,tu_plane);
418 distances[1] = GIM_MIN3(dv[0],dv[1],dv[2]);
419 if(distances[1]>margin) return false; //never intersect
424 //Look if we need to invert the triangle
425 distances[1] = (dv[0]+dv[1]+dv[2])/3.0f; //centroid
427 if(distances[1]<0.0f)
430 VEC_SWAP(tu_vertices[0],tu_vertices[1]);
431 VEC_SCALE_4(tu_plane,-1.0f,tu_plane);
433 distances[1] = GIM_MAX3(dv[0],dv[1],dv[2]);
434 distances[1] = -distances[1];
438 distances[1] = GIM_MIN3(dv[0],dv[1],dv[2]);
443 /* bl = cross_line_intersection_test();
446 //take edge direction too
447 bl = distances.maxAxis();
452 if(distances[0]<distances[1]) bl = 1;
455 if(bl==2) //edge edge separation
457 if(distances[2]>margin) return false;
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);
467 //clip face against other
472 if(bl == 0) //clip U points against V
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);
478 else //clip V points against U
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;
485 if(contacts.m_point_count == 0) return false;
492 /*class GIM_TRIANGLE_CALCULATION_CACHE
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;
508 const btVector4 & tri_plane,
509 const btVector3 * tripoints,
510 const btVector3 * srcpoints,
511 btVector3 * clipped_points)
517 EDGE_PLANE(tripoints[0],tripoints[1],tri_plane,edgeplane);
519 GUINT clipped_count = PLANE_CLIP_TRIANGLE3D(
520 edgeplane,srcpoints[0],srcpoints[1],srcpoints[2],temp_points);
522 if(clipped_count == 0) return 0;
526 EDGE_PLANE(tripoints[1],tripoints[2],tri_plane,edgeplane);
528 clipped_count = PLANE_CLIP_POLYGON3D(
529 edgeplane,temp_points,clipped_count,temp_points1);
531 if(clipped_count == 0) return 0;
535 EDGE_PLANE(tripoints[2],tripoints[0],tri_plane,edgeplane);
537 clipped_count = PLANE_CLIP_POLYGON3D(
538 edgeplane,temp_points1,clipped_count,clipped_points);
540 return clipped_count;
546 //! collides only on one side
547 bool triangle_collision(
548 const btVector3 & u0,
549 const btVector3 & u1,
550 const btVector3 & u2,
552 const btVector3 & v0,
553 const btVector3 & v1,
554 const btVector3 & v2,
556 GIM_TRIANGLE_CONTACT_DATA & contacts)
559 margin = margin_u + margin_v;
571 // plane v vs U points
574 TRIANGLE_PLANE(tv_vertices[0],tv_vertices[1],tv_vertices[2],contacts1.m_separating_normal);
576 clipped_count = clip_triangle(
577 contacts1.m_separating_normal,tv_vertices,tu_vertices,clipped_points);
579 if(clipped_count == 0 )
581 return false;//Reject
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
588 //Normal pointing to triangle1
589 //contacts1.m_separating_normal *= -1.f;
591 //Clip tri1 by tri2 edges
593 TRIANGLE_PLANE(tu_vertices[0],tu_vertices[1],tu_vertices[2],contacts2.m_separating_normal);
595 clipped_count = clip_triangle(
596 contacts2.m_separating_normal,tu_vertices,tv_vertices,clipped_points);
598 if(clipped_count == 0 )
600 return false;//Reject
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
607 contacts2.m_separating_normal *= -1.f;
609 ////check most dir for contacts
610 if(contacts2.m_penetration_depth<contacts1.m_penetration_depth)
612 contacts.copy_from(contacts2);
616 contacts.copy_from(contacts1);
626 bool GIM_TRIANGLE::collide_triangle_hard_test(
627 const GIM_TRIANGLE & other,
628 GIM_TRIANGLE_CONTACT_DATA & contact_data) const
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,