Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/kicklib2/src/external/bullet/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp @ 8740

Last change on this file since 8740 was 8284, checked in by rgrieder, 14 years ago

Merged revisions 7978 - 8096 from kicklib to kicklib2.
  • Property svn:eol-style set to native
File size: 17.1 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(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
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#ifdef BT_USE_EQUAL_VERTEX_THRESHOLD
293                if ( m_simplexVectorW[i].distance2(w) <= m_equalVertexThreshold)
294#else
295                if (m_simplexVectorW[i] == w)
296#endif
297                        found = true;
298        }
299
300        //check in case lastW is already removed
301        if (w == m_lastW)
302                return true;
303       
304        return found;
305}
306
307void btVoronoiSimplexSolver::backup_closest(btVector3& v) 
308{
309        v = m_cachedV;
310}
311
312
313bool btVoronoiSimplexSolver::emptySimplex() const 
314{
315        return (numVertices() == 0);
316
317}
318
319void btVoronoiSimplexSolver::compute_points(btVector3& p1, btVector3& p2) 
320{
321        updateClosestVectorAndPoints();
322        p1 = m_cachedP1;
323        p2 = m_cachedP2;
324
325}
326
327
328
329
330bool    btVoronoiSimplexSolver::closestPtPointTriangle(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c,btSubSimplexClosestResult& result)
331{
332        result.m_usedVertices.reset();
333
334    // Check if P in vertex region outside A
335    btVector3 ab = b - a;
336    btVector3 ac = c - a;
337    btVector3 ap = p - a;
338    btScalar d1 = ab.dot(ap);
339    btScalar d2 = ac.dot(ap);
340    if (d1 <= btScalar(0.0) && d2 <= btScalar(0.0)) 
341        {
342                result.m_closestPointOnSimplex = a;
343                result.m_usedVertices.usedVertexA = true;
344                result.setBarycentricCoordinates(1,0,0);
345                return true;// a; // barycentric coordinates (1,0,0)
346        }
347
348    // Check if P in vertex region outside B
349    btVector3 bp = p - b;
350    btScalar d3 = ab.dot(bp);
351    btScalar d4 = ac.dot(bp);
352    if (d3 >= btScalar(0.0) && d4 <= d3) 
353        {
354                result.m_closestPointOnSimplex = b;
355                result.m_usedVertices.usedVertexB = true;
356                result.setBarycentricCoordinates(0,1,0);
357
358                return true; // b; // barycentric coordinates (0,1,0)
359        }
360    // Check if P in edge region of AB, if so return projection of P onto AB
361    btScalar vc = d1*d4 - d3*d2;
362    if (vc <= btScalar(0.0) && d1 >= btScalar(0.0) && d3 <= btScalar(0.0)) {
363        btScalar v = d1 / (d1 - d3);
364                result.m_closestPointOnSimplex = a + v * ab;
365                result.m_usedVertices.usedVertexA = true;
366                result.m_usedVertices.usedVertexB = true;
367                result.setBarycentricCoordinates(1-v,v,0);
368                return true;
369        //return a + v * ab; // barycentric coordinates (1-v,v,0)
370    }
371
372    // Check if P in vertex region outside C
373    btVector3 cp = p - c;
374    btScalar d5 = ab.dot(cp);
375    btScalar d6 = ac.dot(cp);
376    if (d6 >= btScalar(0.0) && d5 <= d6) 
377        {
378                result.m_closestPointOnSimplex = c;
379                result.m_usedVertices.usedVertexC = true;
380                result.setBarycentricCoordinates(0,0,1);
381                return true;//c; // barycentric coordinates (0,0,1)
382        }
383
384    // Check if P in edge region of AC, if so return projection of P onto AC
385    btScalar vb = d5*d2 - d1*d6;
386    if (vb <= btScalar(0.0) && d2 >= btScalar(0.0) && d6 <= btScalar(0.0)) {
387        btScalar w = d2 / (d2 - d6);
388                result.m_closestPointOnSimplex = a + w * ac;
389                result.m_usedVertices.usedVertexA = true;
390                result.m_usedVertices.usedVertexC = true;
391                result.setBarycentricCoordinates(1-w,0,w);
392                return true;
393        //return a + w * ac; // barycentric coordinates (1-w,0,w)
394    }
395
396    // Check if P in edge region of BC, if so return projection of P onto BC
397    btScalar va = d3*d6 - d5*d4;
398    if (va <= btScalar(0.0) && (d4 - d3) >= btScalar(0.0) && (d5 - d6) >= btScalar(0.0)) {
399        btScalar w = (d4 - d3) / ((d4 - d3) + (d5 - d6));
400               
401                result.m_closestPointOnSimplex = b + w * (c - b);
402                result.m_usedVertices.usedVertexB = true;
403                result.m_usedVertices.usedVertexC = true;
404                result.setBarycentricCoordinates(0,1-w,w);
405                return true;           
406       // return b + w * (c - b); // barycentric coordinates (0,1-w,w)
407    }
408
409    // P inside face region. Compute Q through its barycentric coordinates (u,v,w)
410    btScalar denom = btScalar(1.0) / (va + vb + vc);
411    btScalar v = vb * denom;
412    btScalar w = vc * denom;
413   
414        result.m_closestPointOnSimplex = a + ab * v + ac * w;
415        result.m_usedVertices.usedVertexA = true;
416        result.m_usedVertices.usedVertexB = true;
417        result.m_usedVertices.usedVertexC = true;
418        result.setBarycentricCoordinates(1-v-w,v,w);
419       
420        return true;
421//      return a + ab * v + ac * w; // = u*a + v*b + w*c, u = va * denom = btScalar(1.0) - v - w
422
423}
424
425
426
427
428
429/// Test if point p and d lie on opposite sides of plane through abc
430int btVoronoiSimplexSolver::pointOutsideOfPlane(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d)
431{
432        btVector3 normal = (b-a).cross(c-a);
433
434    btScalar signp = (p - a).dot(normal); // [AP AB AC]
435    btScalar signd = (d - a).dot( normal); // [AD AB AC]
436
437#ifdef CATCH_DEGENERATE_TETRAHEDRON
438#ifdef BT_USE_DOUBLE_PRECISION
439if (signd * signd < (btScalar(1e-8) * btScalar(1e-8)))
440        {
441                return -1;
442        }
443#else
444        if (signd * signd < (btScalar(1e-4) * btScalar(1e-4)))
445        {
446//              printf("affine dependent/degenerate\n");//
447                return -1;
448        }
449#endif
450
451#endif
452        // Points on opposite sides if expression signs are opposite
453    return signp * signd < btScalar(0.);
454}
455
456
457bool    btVoronoiSimplexSolver::closestPtPointTetrahedron(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d, btSubSimplexClosestResult& finalResult)
458{
459        btSubSimplexClosestResult tempResult;
460
461    // Start out assuming point inside all halfspaces, so closest to itself
462        finalResult.m_closestPointOnSimplex = p;
463        finalResult.m_usedVertices.reset();
464    finalResult.m_usedVertices.usedVertexA = true;
465        finalResult.m_usedVertices.usedVertexB = true;
466        finalResult.m_usedVertices.usedVertexC = true;
467        finalResult.m_usedVertices.usedVertexD = true;
468
469    int pointOutsideABC = pointOutsideOfPlane(p, a, b, c, d);
470        int pointOutsideACD = pointOutsideOfPlane(p, a, c, d, b);
471        int     pointOutsideADB = pointOutsideOfPlane(p, a, d, b, c);
472        int     pointOutsideBDC = pointOutsideOfPlane(p, b, d, c, a);
473
474   if (pointOutsideABC < 0 || pointOutsideACD < 0 || pointOutsideADB < 0 || pointOutsideBDC < 0)
475   {
476           finalResult.m_degenerate = true;
477           return false;
478   }
479
480   if (!pointOutsideABC  && !pointOutsideACD && !pointOutsideADB && !pointOutsideBDC)
481         {
482                 return false;
483         }
484
485
486    btScalar bestSqDist = FLT_MAX;
487    // If point outside face abc then compute closest point on abc
488        if (pointOutsideABC) 
489        {
490        closestPtPointTriangle(p, a, b, c,tempResult);
491                btVector3 q = tempResult.m_closestPointOnSimplex;
492               
493        btScalar sqDist = (q - p).dot( q - p);
494        // Update best closest point if (squared) distance is less than current best
495        if (sqDist < bestSqDist) {
496                        bestSqDist = sqDist;
497                        finalResult.m_closestPointOnSimplex = q;
498                        //convert result bitmask!
499                        finalResult.m_usedVertices.reset();
500                        finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA;
501                        finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexB;
502                        finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexC;
503                        finalResult.setBarycentricCoordinates(
504                                        tempResult.m_barycentricCoords[VERTA],
505                                        tempResult.m_barycentricCoords[VERTB],
506                                        tempResult.m_barycentricCoords[VERTC],
507                                        0
508                        );
509
510                }
511    }
512 
513
514        // Repeat test for face acd
515        if (pointOutsideACD) 
516        {
517        closestPtPointTriangle(p, a, c, d,tempResult);
518                btVector3 q = tempResult.m_closestPointOnSimplex;
519                //convert result bitmask!
520
521        btScalar sqDist = (q - p).dot( q - p);
522        if (sqDist < bestSqDist) 
523                {
524                        bestSqDist = sqDist;
525                        finalResult.m_closestPointOnSimplex = q;
526                        finalResult.m_usedVertices.reset();
527                        finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA;
528
529                        finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexB;
530                        finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexC;
531                        finalResult.setBarycentricCoordinates(
532                                        tempResult.m_barycentricCoords[VERTA],
533                                        0,
534                                        tempResult.m_barycentricCoords[VERTB],
535                                        tempResult.m_barycentricCoords[VERTC]
536                        );
537
538                }
539    }
540    // Repeat test for face adb
541
542       
543        if (pointOutsideADB)
544        {
545                closestPtPointTriangle(p, a, d, b,tempResult);
546                btVector3 q = tempResult.m_closestPointOnSimplex;
547                //convert result bitmask!
548
549        btScalar sqDist = (q - p).dot( q - p);
550        if (sqDist < bestSqDist) 
551                {
552                        bestSqDist = sqDist;
553                        finalResult.m_closestPointOnSimplex = q;
554                        finalResult.m_usedVertices.reset();
555                        finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA;
556                        finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexC;
557                       
558                        finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexB;
559                        finalResult.setBarycentricCoordinates(
560                                        tempResult.m_barycentricCoords[VERTA],
561                                        tempResult.m_barycentricCoords[VERTC],
562                                        0,
563                                        tempResult.m_barycentricCoords[VERTB]
564                        );
565
566                }
567    }
568    // Repeat test for face bdc
569   
570
571        if (pointOutsideBDC)
572        {
573        closestPtPointTriangle(p, b, d, c,tempResult);
574                btVector3 q = tempResult.m_closestPointOnSimplex;
575                //convert result bitmask!
576        btScalar sqDist = (q - p).dot( q - p);
577        if (sqDist < bestSqDist) 
578                {
579                        bestSqDist = sqDist;
580                        finalResult.m_closestPointOnSimplex = q;
581                        finalResult.m_usedVertices.reset();
582                        //
583                        finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexA;
584                        finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexC;
585                        finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexB;
586
587                        finalResult.setBarycentricCoordinates(
588                                        0,
589                                        tempResult.m_barycentricCoords[VERTA],
590                                        tempResult.m_barycentricCoords[VERTC],
591                                        tempResult.m_barycentricCoords[VERTB]
592                        );
593
594                }
595    }
596
597        //help! we ended up full !
598       
599        if (finalResult.m_usedVertices.usedVertexA &&
600                finalResult.m_usedVertices.usedVertexB &&
601                finalResult.m_usedVertices.usedVertexC &&
602                finalResult.m_usedVertices.usedVertexD) 
603        {
604                return true;
605        }
606
607    return true;
608}
609
Note: See TracBrowser for help on using the repository browser.