Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/physics/src/bullet/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuVoronoiSimplexSolver.cpp @ 1966

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

Let's go for multithreaded physics!
  • 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 "SpuVoronoiSimplexSolver.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    SpuVoronoiSimplexSolver::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    SpuVoronoiSimplexSolver::reduceVertices (const SpuUsageBitfield& 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 SpuVoronoiSimplexSolver::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 SpuVoronoiSimplexSolver::addVertex(const btVector3& w, const btPoint3& p, const btPoint3& 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    SpuVoronoiSimplexSolver::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                                                                m_simplexPointsP[3] * m_cachedBC.m_barycentricCoords[3];
173
174                                m_cachedP2 = m_simplexPointsQ[0] * m_cachedBC.m_barycentricCoords[0] +
175                                        m_simplexPointsQ[1] * m_cachedBC.m_barycentricCoords[1] +
176                                        m_simplexPointsQ[2] * m_cachedBC.m_barycentricCoords[2] +
177                                        m_simplexPointsQ[3] * m_cachedBC.m_barycentricCoords[3];
178
179                                m_cachedV = m_cachedP1-m_cachedP2;
180
181                                reduceVertices (m_cachedBC.m_usedVertices);
182                                m_cachedValidClosest =  m_cachedBC.isValid();
183
184                                break;
185                        }
186                case 4:
187                        {
188
189                               
190                                btVector3 p (btScalar(0.),btScalar(0.),btScalar(0.));
191                               
192                                const btVector3& a = m_simplexVectorW[0];
193                                const btVector3& b = m_simplexVectorW[1];
194                                const btVector3& c = m_simplexVectorW[2];
195                                const btVector3& d = m_simplexVectorW[3];
196
197                                bool hasSeperation = closestPtPointTetrahedron(p,a,b,c,d,m_cachedBC);
198
199                                if (hasSeperation)
200                                {
201
202                                        m_cachedP1 = m_simplexPointsP[0] * m_cachedBC.m_barycentricCoords[0] +
203                                                m_simplexPointsP[1] * m_cachedBC.m_barycentricCoords[1] +
204                                                m_simplexPointsP[2] * m_cachedBC.m_barycentricCoords[2] +
205                                                m_simplexPointsP[3] * m_cachedBC.m_barycentricCoords[3];
206
207                                        m_cachedP2 = m_simplexPointsQ[0] * m_cachedBC.m_barycentricCoords[0] +
208                                                m_simplexPointsQ[1] * m_cachedBC.m_barycentricCoords[1] +
209                                                m_simplexPointsQ[2] * m_cachedBC.m_barycentricCoords[2] +
210                                                m_simplexPointsQ[3] * m_cachedBC.m_barycentricCoords[3];
211
212                                        m_cachedV = m_cachedP1-m_cachedP2;
213                                        reduceVertices (m_cachedBC.m_usedVertices);
214                                } else
215                                {
216//                                      printf("sub distance got penetration\n");
217
218                                        if (m_cachedBC.m_degenerate)
219                                        {
220                                                m_cachedValidClosest = false;
221                                        } else
222                                        {
223                                                m_cachedValidClosest = true;
224                                                //degenerate case == false, penetration = true + zero
225                                                m_cachedV.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
226                                        }
227                                        break;
228                                }
229
230                                m_cachedValidClosest = m_cachedBC.isValid();
231
232                                //closest point origin from tetrahedron
233                                break;
234                        }
235                default:
236                        {
237                                m_cachedValidClosest = false;
238                        }
239                };
240        }
241
242        return m_cachedValidClosest;
243
244}
245
246//return/calculate the closest vertex
247bool SpuVoronoiSimplexSolver::closest(btVector3& v)
248{
249        bool succes = updateClosestVectorAndPoints();
250        v = m_cachedV;
251        return succes;
252}
253
254
255
256btScalar SpuVoronoiSimplexSolver::maxVertex()
257{
258        int i, numverts = numVertices();
259        btScalar maxV = btScalar(0.);
260        for (i=0;i<numverts;i++)
261        {
262                btScalar curLen2 = m_simplexVectorW[i].length2();
263                if (maxV < curLen2)
264                        maxV = curLen2;
265        }
266        return maxV;
267}
268
269
270
271        //return the current simplex
272int SpuVoronoiSimplexSolver::getSimplex(btPoint3 *pBuf, btPoint3 *qBuf, btVector3 *yBuf) const
273{
274        int i;
275        for (i=0;i<numVertices();i++)
276        {
277                yBuf[i] = m_simplexVectorW[i];
278                pBuf[i] = m_simplexPointsP[i];
279                qBuf[i] = m_simplexPointsQ[i];
280        }
281        return numVertices();
282}
283
284
285
286
287bool SpuVoronoiSimplexSolver::inSimplex(const btVector3& w)
288{
289        bool found = false;
290        int i, numverts = numVertices();
291        //btScalar maxV = btScalar(0.);
292       
293        //w is in the current (reduced) simplex
294        for (i=0;i<numverts;i++)
295        {
296                if (m_simplexVectorW[i] == w)
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 SpuVoronoiSimplexSolver::backup_closest(btVector3& v) 
308{
309        v = m_cachedV;
310}
311
312
313bool SpuVoronoiSimplexSolver::emptySimplex() const 
314{
315        return (numVertices() == 0);
316
317}
318
319void SpuVoronoiSimplexSolver::compute_points(btPoint3& p1, btPoint3& p2) 
320{
321        updateClosestVectorAndPoints();
322        p1 = m_cachedP1;
323        p2 = m_cachedP2;
324
325}
326
327
328
329
330bool    SpuVoronoiSimplexSolver::closestPtPointTriangle(const btPoint3& p, const btPoint3& a, const btPoint3& b, const btPoint3& c,SpuSubSimplexClosestResult& 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 SpuVoronoiSimplexSolver::pointOutsideOfPlane(const btPoint3& p, const btPoint3& a, const btPoint3& b, const btPoint3& c, const btPoint3& 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    SpuVoronoiSimplexSolver::closestPtPointTetrahedron(const btPoint3& p, const btPoint3& a, const btPoint3& b, const btPoint3& c, const btPoint3& d, SpuSubSimplexClosestResult& finalResult)
458{
459        SpuSubSimplexClosestResult 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                btPoint3 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                btPoint3 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                        finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexB;
529                        finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexC;
530                        finalResult.setBarycentricCoordinates(
531                                        tempResult.m_barycentricCoords[VERTA],
532                                        0,
533                                        tempResult.m_barycentricCoords[VERTB],
534                                        tempResult.m_barycentricCoords[VERTC]
535                        );
536
537                }
538    }
539    // Repeat test for face adb
540
541       
542        if (pointOutsideADB)
543        {
544                closestPtPointTriangle(p, a, d, b,tempResult);
545                btPoint3 q = tempResult.m_closestPointOnSimplex;
546                //convert result bitmask!
547
548        btScalar sqDist = (q - p).dot( q - p);
549        if (sqDist < bestSqDist) 
550                {
551                        bestSqDist = sqDist;
552                        finalResult.m_closestPointOnSimplex = q;
553                        finalResult.m_usedVertices.reset();
554                        finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA;
555                        finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexB;
556                        finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexC;
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                btPoint3 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                        finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexA;
581                        finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexB;
582                        finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexC;
583
584                        finalResult.setBarycentricCoordinates(
585                                        0,
586                                        tempResult.m_barycentricCoords[VERTA],
587                                        tempResult.m_barycentricCoords[VERTC],
588                                        tempResult.m_barycentricCoords[VERTB]
589                        );
590
591                }
592    }
593
594        //help! we ended up full !
595       
596        if (finalResult.m_usedVertices.usedVertexA &&
597                finalResult.m_usedVertices.usedVertexB &&
598                finalResult.m_usedVertices.usedVertexC &&
599                finalResult.m_usedVertices.usedVertexD) 
600        {
601                return true;
602        }
603
604    return true;
605}
606
Note: See TracBrowser for help on using the repository browser.