Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

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

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