== SoC Bullet - Bullet Upgrade to 2.76 ==
[blender.git] / extern / bullet2 / BulletCollision / Gimpact / gim_tri_collision.cpp
1
2 /*! \file gim_tri_collision.h
3 \author Francisco Le\7fn N├čjera
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
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         */
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
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         */
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
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
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