Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/trunk/src/bullet/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp @ 2875

Last change on this file since 2875 was 2662, checked in by rgrieder, 16 years ago

Merged presentation branch back to trunk.
  • Property svn:eol-style set to native
File size: 17.0 KB
Line 
1
2/*
3Bullet Continuous Collision Detection and Physics Library
4Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
5
6This software is provided 'as-is', without any express or implied warranty.
7In no event will the authors be held liable for any damages arising from the use of this software.
8Permission is granted to anyone to use this software for any purpose,
9including commercial applications, and to alter it and redistribute it freely,
10subject to the following restrictions:
11
121. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
132. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
143. This notice may not be removed or altered from any source distribution.
15       
16        Elsevier CDROM license agreements grants nonexclusive license to use the software
17        for any purpose, commercial or non-commercial as long as the following credit is included
18        identifying the original source of the software:
19
20        Parts of the source are "from the book Real-Time Collision Detection by
21        Christer Ericson, published by Morgan Kaufmann Publishers,
22        (c) 2005 Elsevier Inc."
23               
24*/
25
26
27#include "btVoronoiSimplexSolver.h"
28#include <assert.h>
29//#include <stdio.h>
30
31#define VERTA  0
32#define VERTB  1
33#define VERTC  2
34#define VERTD  3
35
36#define CATCH_DEGENERATE_TETRAHEDRON 1
37void    btVoronoiSimplexSolver::removeVertex(int index)
38{
39       
40        assert(m_numVertices>0);
41        m_numVertices--;
42        m_simplexVectorW[index] = m_simplexVectorW[m_numVertices];
43        m_simplexPointsP[index] = m_simplexPointsP[m_numVertices];
44        m_simplexPointsQ[index] = m_simplexPointsQ[m_numVertices];
45}
46
47void    btVoronoiSimplexSolver::reduceVertices (const btUsageBitfield& usedVerts)
48{
49        if ((numVertices() >= 4) && (!usedVerts.usedVertexD))
50                removeVertex(3);
51
52        if ((numVertices() >= 3) && (!usedVerts.usedVertexC))
53                removeVertex(2);
54
55        if ((numVertices() >= 2) && (!usedVerts.usedVertexB))
56                removeVertex(1);
57       
58        if ((numVertices() >= 1) && (!usedVerts.usedVertexA))
59                removeVertex(0);
60
61}
62
63
64
65
66
67//clear the simplex, remove all the vertices
68void btVoronoiSimplexSolver::reset()
69{
70        m_cachedValidClosest = false;
71        m_numVertices = 0;
72        m_needsUpdate = true;
73        m_lastW = btVector3(btScalar(1e30),btScalar(1e30),btScalar(1e30));
74        m_cachedBC.reset();
75}
76
77
78
79        //add a vertex
80void btVoronoiSimplexSolver::addVertex(const btVector3& w, const btVector3& p, const btVector3& q)
81{
82        m_lastW = w;
83        m_needsUpdate = true;
84
85        m_simplexVectorW[m_numVertices] = w;
86        m_simplexPointsP[m_numVertices] = p;
87        m_simplexPointsQ[m_numVertices] = q;
88
89        m_numVertices++;
90}
91
92bool    btVoronoiSimplexSolver::updateClosestVectorAndPoints()
93{
94       
95        if (m_needsUpdate)
96        {
97                m_cachedBC.reset();
98
99                m_needsUpdate = false;
100
101                switch (numVertices())
102                {
103                case 0:
104                                m_cachedValidClosest = false;
105                                break;
106                case 1:
107                        {
108                                m_cachedP1 = m_simplexPointsP[0];
109                                m_cachedP2 = m_simplexPointsQ[0];
110                                m_cachedV = m_cachedP1-m_cachedP2; //== m_simplexVectorW[0]
111                                m_cachedBC.reset();
112                                m_cachedBC.setBarycentricCoordinates(btScalar(1.),btScalar(0.),btScalar(0.),btScalar(0.));
113                                m_cachedValidClosest = m_cachedBC.isValid();
114                                break;
115                        };
116                case 2:
117                        {
118                        //closest point origin from line segment
119                                        const btVector3& from = m_simplexVectorW[0];
120                                        const btVector3& to = m_simplexVectorW[1];
121                                        btVector3 nearest;
122
123                                        btVector3 p (btScalar(0.),btScalar(0.),btScalar(0.));
124                                        btVector3 diff = p - from;
125                                        btVector3 v = to - from;
126                                        btScalar t = v.dot(diff);
127                                       
128                                        if (t > 0) {
129                                                btScalar dotVV = v.dot(v);
130                                                if (t < dotVV) {
131                                                        t /= dotVV;
132                                                        diff -= t*v;
133                                                        m_cachedBC.m_usedVertices.usedVertexA = true;
134                                                        m_cachedBC.m_usedVertices.usedVertexB = true;
135                                                } else {
136                                                        t = 1;
137                                                        diff -= v;
138                                                        //reduce to 1 point
139                                                        m_cachedBC.m_usedVertices.usedVertexB = true;
140                                                }
141                                        } else
142                                        {
143                                                t = 0;
144                                                //reduce to 1 point
145                                                m_cachedBC.m_usedVertices.usedVertexA = true;
146                                        }
147                                        m_cachedBC.setBarycentricCoordinates(1-t,t);
148                                        nearest = from + t*v;
149
150                                        m_cachedP1 = m_simplexPointsP[0] + t * (m_simplexPointsP[1] - m_simplexPointsP[0]);
151                                        m_cachedP2 = m_simplexPointsQ[0] + t * (m_simplexPointsQ[1] - m_simplexPointsQ[0]);
152                                        m_cachedV = m_cachedP1 - m_cachedP2;
153                                       
154                                        reduceVertices(m_cachedBC.m_usedVertices);
155
156                                        m_cachedValidClosest = m_cachedBC.isValid();
157                                        break;
158                        }
159                case 3: 
160                        { 
161                                //closest point origin from triangle
162                                btVector3 p (btScalar(0.),btScalar(0.),btScalar(0.)); 
163
164                                const btVector3& a = m_simplexVectorW[0]; 
165                                const btVector3& b = m_simplexVectorW[1]; 
166                                const btVector3& c = m_simplexVectorW[2]; 
167
168                                closestPtPointTriangle(p,a,b,c,m_cachedBC); 
169                                m_cachedP1 = m_simplexPointsP[0] * m_cachedBC.m_barycentricCoords[0] + 
170                                m_simplexPointsP[1] * m_cachedBC.m_barycentricCoords[1] + 
171                                m_simplexPointsP[2] * m_cachedBC.m_barycentricCoords[2]; 
172
173                                m_cachedP2 = m_simplexPointsQ[0] * m_cachedBC.m_barycentricCoords[0] + 
174                                m_simplexPointsQ[1] * m_cachedBC.m_barycentricCoords[1] + 
175                                m_simplexPointsQ[2] * m_cachedBC.m_barycentricCoords[2]; 
176
177                                m_cachedV = m_cachedP1-m_cachedP2; 
178
179                                reduceVertices (m_cachedBC.m_usedVertices); 
180                                m_cachedValidClosest = m_cachedBC.isValid(); 
181
182                                break; 
183                        }
184                case 4:
185                        {
186
187                               
188                                btVector3 p (btScalar(0.),btScalar(0.),btScalar(0.));
189                               
190                                const btVector3& a = m_simplexVectorW[0];
191                                const btVector3& b = m_simplexVectorW[1];
192                                const btVector3& c = m_simplexVectorW[2];
193                                const btVector3& d = m_simplexVectorW[3];
194
195                                bool hasSeperation = closestPtPointTetrahedron(p,a,b,c,d,m_cachedBC);
196
197                                if (hasSeperation)
198                                {
199
200                                        m_cachedP1 = m_simplexPointsP[0] * m_cachedBC.m_barycentricCoords[0] +
201                                                m_simplexPointsP[1] * m_cachedBC.m_barycentricCoords[1] +
202                                                m_simplexPointsP[2] * m_cachedBC.m_barycentricCoords[2] +
203                                                m_simplexPointsP[3] * m_cachedBC.m_barycentricCoords[3];
204
205                                        m_cachedP2 = m_simplexPointsQ[0] * m_cachedBC.m_barycentricCoords[0] +
206                                                m_simplexPointsQ[1] * m_cachedBC.m_barycentricCoords[1] +
207                                                m_simplexPointsQ[2] * m_cachedBC.m_barycentricCoords[2] +
208                                                m_simplexPointsQ[3] * m_cachedBC.m_barycentricCoords[3];
209
210                                        m_cachedV = m_cachedP1-m_cachedP2;
211                                        reduceVertices (m_cachedBC.m_usedVertices);
212                                } else
213                                {
214//                                      printf("sub distance got penetration\n");
215
216                                        if (m_cachedBC.m_degenerate)
217                                        {
218                                                m_cachedValidClosest = false;
219                                        } else
220                                        {
221                                                m_cachedValidClosest = true;
222                                                //degenerate case == false, penetration = true + zero
223                                                m_cachedV.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
224                                        }
225                                        break;
226                                }
227
228                                m_cachedValidClosest = m_cachedBC.isValid();
229
230                                //closest point origin from tetrahedron
231                                break;
232                        }
233                default:
234                        {
235                                m_cachedValidClosest = false;
236                        }
237                };
238        }
239
240        return m_cachedValidClosest;
241
242}
243
244//return/calculate the closest vertex
245bool btVoronoiSimplexSolver::closest(btVector3& v)
246{
247        bool succes = updateClosestVectorAndPoints();
248        v = m_cachedV;
249        return succes;
250}
251
252
253
254btScalar btVoronoiSimplexSolver::maxVertex()
255{
256        int i, numverts = numVertices();
257        btScalar maxV = btScalar(0.);
258        for (i=0;i<numverts;i++)
259        {
260                btScalar curLen2 = m_simplexVectorW[i].length2();
261                if (maxV < curLen2)
262                        maxV = curLen2;
263        }
264        return maxV;
265}
266
267
268
269        //return the current simplex
270int btVoronoiSimplexSolver::getSimplex(btVector3 *pBuf, btVector3 *qBuf, btVector3 *yBuf) const
271{
272        int i;
273        for (i=0;i<numVertices();i++)
274        {
275                yBuf[i] = m_simplexVectorW[i];
276                pBuf[i] = m_simplexPointsP[i];
277                qBuf[i] = m_simplexPointsQ[i];
278        }
279        return numVertices();
280}
281
282
283
284
285bool btVoronoiSimplexSolver::inSimplex(const btVector3& w)
286{
287        bool found = false;
288        int i, numverts = numVertices();
289        //btScalar maxV = btScalar(0.);
290       
291        //w is in the current (reduced) simplex
292        for (i=0;i<numverts;i++)
293        {
294                if (m_simplexVectorW[i] == w)
295                        found = true;
296        }
297
298        //check in case lastW is already removed
299        if (w == m_lastW)
300                return true;
301       
302        return found;
303}
304
305void btVoronoiSimplexSolver::backup_closest(btVector3& v) 
306{
307        v = m_cachedV;
308}
309
310
311bool btVoronoiSimplexSolver::emptySimplex() const 
312{
313        return (numVertices() == 0);
314
315}
316
317void btVoronoiSimplexSolver::compute_points(btVector3& p1, btVector3& p2) 
318{
319        updateClosestVectorAndPoints();
320        p1 = m_cachedP1;
321        p2 = m_cachedP2;
322
323}
324
325
326
327
328bool    btVoronoiSimplexSolver::closestPtPointTriangle(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c,btSubSimplexClosestResult& result)
329{
330        result.m_usedVertices.reset();
331
332    // Check if P in vertex region outside A
333    btVector3 ab = b - a;
334    btVector3 ac = c - a;
335    btVector3 ap = p - a;
336    btScalar d1 = ab.dot(ap);
337    btScalar d2 = ac.dot(ap);
338    if (d1 <= btScalar(0.0) && d2 <= btScalar(0.0)) 
339        {
340                result.m_closestPointOnSimplex = a;
341                result.m_usedVertices.usedVertexA = true;
342                result.setBarycentricCoordinates(1,0,0);
343                return true;// a; // barycentric coordinates (1,0,0)
344        }
345
346    // Check if P in vertex region outside B
347    btVector3 bp = p - b;
348    btScalar d3 = ab.dot(bp);
349    btScalar d4 = ac.dot(bp);
350    if (d3 >= btScalar(0.0) && d4 <= d3) 
351        {
352                result.m_closestPointOnSimplex = b;
353                result.m_usedVertices.usedVertexB = true;
354                result.setBarycentricCoordinates(0,1,0);
355
356                return true; // b; // barycentric coordinates (0,1,0)
357        }
358    // Check if P in edge region of AB, if so return projection of P onto AB
359    btScalar vc = d1*d4 - d3*d2;
360    if (vc <= btScalar(0.0) && d1 >= btScalar(0.0) && d3 <= btScalar(0.0)) {
361        btScalar v = d1 / (d1 - d3);
362                result.m_closestPointOnSimplex = a + v * ab;
363                result.m_usedVertices.usedVertexA = true;
364                result.m_usedVertices.usedVertexB = true;
365                result.setBarycentricCoordinates(1-v,v,0);
366                return true;
367        //return a + v * ab; // barycentric coordinates (1-v,v,0)
368    }
369
370    // Check if P in vertex region outside C
371    btVector3 cp = p - c;
372    btScalar d5 = ab.dot(cp);
373    btScalar d6 = ac.dot(cp);
374    if (d6 >= btScalar(0.0) && d5 <= d6) 
375        {
376                result.m_closestPointOnSimplex = c;
377                result.m_usedVertices.usedVertexC = true;
378                result.setBarycentricCoordinates(0,0,1);
379                return true;//c; // barycentric coordinates (0,0,1)
380        }
381
382    // Check if P in edge region of AC, if so return projection of P onto AC
383    btScalar vb = d5*d2 - d1*d6;
384    if (vb <= btScalar(0.0) && d2 >= btScalar(0.0) && d6 <= btScalar(0.0)) {
385        btScalar w = d2 / (d2 - d6);
386                result.m_closestPointOnSimplex = a + w * ac;
387                result.m_usedVertices.usedVertexA = true;
388                result.m_usedVertices.usedVertexC = true;
389                result.setBarycentricCoordinates(1-w,0,w);
390                return true;
391        //return a + w * ac; // barycentric coordinates (1-w,0,w)
392    }
393
394    // Check if P in edge region of BC, if so return projection of P onto BC
395    btScalar va = d3*d6 - d5*d4;
396    if (va <= btScalar(0.0) && (d4 - d3) >= btScalar(0.0) && (d5 - d6) >= btScalar(0.0)) {
397        btScalar w = (d4 - d3) / ((d4 - d3) + (d5 - d6));
398               
399                result.m_closestPointOnSimplex = b + w * (c - b);
400                result.m_usedVertices.usedVertexB = true;
401                result.m_usedVertices.usedVertexC = true;
402                result.setBarycentricCoordinates(0,1-w,w);
403                return true;           
404       // return b + w * (c - b); // barycentric coordinates (0,1-w,w)
405    }
406
407    // P inside face region. Compute Q through its barycentric coordinates (u,v,w)
408    btScalar denom = btScalar(1.0) / (va + vb + vc);
409    btScalar v = vb * denom;
410    btScalar w = vc * denom;
411   
412        result.m_closestPointOnSimplex = a + ab * v + ac * w;
413        result.m_usedVertices.usedVertexA = true;
414        result.m_usedVertices.usedVertexB = true;
415        result.m_usedVertices.usedVertexC = true;
416        result.setBarycentricCoordinates(1-v-w,v,w);
417       
418        return true;
419//      return a + ab * v + ac * w; // = u*a + v*b + w*c, u = va * denom = btScalar(1.0) - v - w
420
421}
422
423
424
425
426
427/// Test if point p and d lie on opposite sides of plane through abc
428int btVoronoiSimplexSolver::pointOutsideOfPlane(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d)
429{
430        btVector3 normal = (b-a).cross(c-a);
431
432    btScalar signp = (p - a).dot(normal); // [AP AB AC]
433    btScalar signd = (d - a).dot( normal); // [AD AB AC]
434
435#ifdef CATCH_DEGENERATE_TETRAHEDRON
436#ifdef BT_USE_DOUBLE_PRECISION
437if (signd * signd < (btScalar(1e-8) * btScalar(1e-8)))
438        {
439                return -1;
440        }
441#else
442        if (signd * signd < (btScalar(1e-4) * btScalar(1e-4)))
443        {
444//              printf("affine dependent/degenerate\n");//
445                return -1;
446        }
447#endif
448
449#endif
450        // Points on opposite sides if expression signs are opposite
451    return signp * signd < btScalar(0.);
452}
453
454
455bool    btVoronoiSimplexSolver::closestPtPointTetrahedron(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d, btSubSimplexClosestResult& finalResult)
456{
457        btSubSimplexClosestResult tempResult;
458
459    // Start out assuming point inside all halfspaces, so closest to itself
460        finalResult.m_closestPointOnSimplex = p;
461        finalResult.m_usedVertices.reset();
462    finalResult.m_usedVertices.usedVertexA = true;
463        finalResult.m_usedVertices.usedVertexB = true;
464        finalResult.m_usedVertices.usedVertexC = true;
465        finalResult.m_usedVertices.usedVertexD = true;
466
467    int pointOutsideABC = pointOutsideOfPlane(p, a, b, c, d);
468        int pointOutsideACD = pointOutsideOfPlane(p, a, c, d, b);
469        int     pointOutsideADB = pointOutsideOfPlane(p, a, d, b, c);
470        int     pointOutsideBDC = pointOutsideOfPlane(p, b, d, c, a);
471
472   if (pointOutsideABC < 0 || pointOutsideACD < 0 || pointOutsideADB < 0 || pointOutsideBDC < 0)
473   {
474           finalResult.m_degenerate = true;
475           return false;
476   }
477
478   if (!pointOutsideABC  && !pointOutsideACD && !pointOutsideADB && !pointOutsideBDC)
479         {
480                 return false;
481         }
482
483
484    btScalar bestSqDist = FLT_MAX;
485    // If point outside face abc then compute closest point on abc
486        if (pointOutsideABC) 
487        {
488        closestPtPointTriangle(p, a, b, c,tempResult);
489                btVector3 q = tempResult.m_closestPointOnSimplex;
490               
491        btScalar sqDist = (q - p).dot( q - p);
492        // Update best closest point if (squared) distance is less than current best
493        if (sqDist < bestSqDist) {
494                        bestSqDist = sqDist;
495                        finalResult.m_closestPointOnSimplex = q;
496                        //convert result bitmask!
497                        finalResult.m_usedVertices.reset();
498                        finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA;
499                        finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexB;
500                        finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexC;
501                        finalResult.setBarycentricCoordinates(
502                                        tempResult.m_barycentricCoords[VERTA],
503                                        tempResult.m_barycentricCoords[VERTB],
504                                        tempResult.m_barycentricCoords[VERTC],
505                                        0
506                        );
507
508                }
509    }
510 
511
512        // Repeat test for face acd
513        if (pointOutsideACD) 
514        {
515        closestPtPointTriangle(p, a, c, d,tempResult);
516                btVector3 q = tempResult.m_closestPointOnSimplex;
517                //convert result bitmask!
518
519        btScalar sqDist = (q - p).dot( q - p);
520        if (sqDist < bestSqDist) 
521                {
522                        bestSqDist = sqDist;
523                        finalResult.m_closestPointOnSimplex = q;
524                        finalResult.m_usedVertices.reset();
525                        finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA;
526
527                        finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexB;
528                        finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexC;
529                        finalResult.setBarycentricCoordinates(
530                                        tempResult.m_barycentricCoords[VERTA],
531                                        0,
532                                        tempResult.m_barycentricCoords[VERTB],
533                                        tempResult.m_barycentricCoords[VERTC]
534                        );
535
536                }
537    }
538    // Repeat test for face adb
539
540       
541        if (pointOutsideADB)
542        {
543                closestPtPointTriangle(p, a, d, b,tempResult);
544                btVector3 q = tempResult.m_closestPointOnSimplex;
545                //convert result bitmask!
546
547        btScalar sqDist = (q - p).dot( q - p);
548        if (sqDist < bestSqDist) 
549                {
550                        bestSqDist = sqDist;
551                        finalResult.m_closestPointOnSimplex = q;
552                        finalResult.m_usedVertices.reset();
553                        finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA;
554                        finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexC;
555                       
556                        finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexB;
557                        finalResult.setBarycentricCoordinates(
558                                        tempResult.m_barycentricCoords[VERTA],
559                                        tempResult.m_barycentricCoords[VERTC],
560                                        0,
561                                        tempResult.m_barycentricCoords[VERTB]
562                        );
563
564                }
565    }
566    // Repeat test for face bdc
567   
568
569        if (pointOutsideBDC)
570        {
571        closestPtPointTriangle(p, b, d, c,tempResult);
572                btVector3 q = tempResult.m_closestPointOnSimplex;
573                //convert result bitmask!
574        btScalar sqDist = (q - p).dot( q - p);
575        if (sqDist < bestSqDist) 
576                {
577                        bestSqDist = sqDist;
578                        finalResult.m_closestPointOnSimplex = q;
579                        finalResult.m_usedVertices.reset();
580                        //
581                        finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexA;
582                        finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexC;
583                        finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexB;
584
585                        finalResult.setBarycentricCoordinates(
586                                        0,
587                                        tempResult.m_barycentricCoords[VERTA],
588                                        tempResult.m_barycentricCoords[VERTC],
589                                        tempResult.m_barycentricCoords[VERTB]
590                        );
591
592                }
593    }
594
595        //help! we ended up full !
596       
597        if (finalResult.m_usedVertices.usedVertexA &&
598                finalResult.m_usedVertices.usedVertexB &&
599                finalResult.m_usedVertices.usedVertexC &&
600                finalResult.m_usedVertices.usedVertexD) 
601        {
602                return true;
603        }
604
605    return true;
606}
607
Note: See TracBrowser for help on using the repository browser.