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