Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/trunk/src/external/bullet/BulletCollision/Gimpact/gim_tri_collision.cpp @ 8295

Last change on this file since 8295 was 5781, checked in by rgrieder, 15 years ago

Reverted trunk again. We might want to find a way to delete these revisions again (x3n's changes are still available as diff in the commit mails).

  • Property svn:eol-style set to native
File size: 15.6 KB
Line 
1
2/*! \file gim_tri_collision.h
3\author Francisco Len Nßjera
4*/
5/*
6-----------------------------------------------------------------------------
7This source file is part of GIMPACT Library.
8
9For the latest info, see http://gimpact.sourceforge.net/
10
11Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371.
12email: 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
41class GIM_TRIANGLE_CALCULATION_CACHE
42{
43public:
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{
494public:
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
626bool 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
Note: See TracBrowser for help on using the repository browser.