dali-physics/third-party/bullet3/src/Bullet3OpenCL/NarrowphaseCollision/b3VoronoiSimplexSolver.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 "b3VoronoiSimplexSolver.h"
  27
  28 #define VERTA 0
  29 #define VERTB 1
  30 #define VERTC 2
  31 #define VERTD 3
  32
  33 #define B3_CATCH_DEGENERATE_TETRAHEDRON 1
  34 void b3VoronoiSimplexSolver::removeVertex(int index)
  35 {
  36         b3Assert(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 b3VoronoiSimplexSolver::reduceVertices(const b3UsageBitfield& 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 b3VoronoiSimplexSolver::reset()
  60 {
  61         m_cachedValidClosest = false;
  62         m_numVertices = 0;
  63         m_needsUpdate = true;
  64         m_lastW = b3MakeVector3(b3Scalar(B3_LARGE_FLOAT), b3Scalar(B3_LARGE_FLOAT), b3Scalar(B3_LARGE_FLOAT));
  65         m_cachedBC.reset();
  66 }
  67
  68 //add a vertex
  69 void b3VoronoiSimplexSolver::addVertex(const b3Vector3& w, const b3Vector3& p, const b3Vector3& 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 b3VoronoiSimplexSolver::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(b3Scalar(1.), b3Scalar(0.), b3Scalar(0.), b3Scalar(0.));
 101                                 m_cachedValidClosest = m_cachedBC.isValid();
 102                                 break;
 103                         };
 104                         case 2:
 105                         {
 106                                 //closest point origin from line segment
 107                                 const b3Vector3& from = m_simplexVectorW[0];
 108                                 const b3Vector3& to = m_simplexVectorW[1];
 109                                 b3Vector3 nearest;
 110
 111                                 b3Vector3 p = b3MakeVector3(b3Scalar(0.), b3Scalar(0.), b3Scalar(0.));
 112                                 b3Vector3 diff = p - from;
 113                                 b3Vector3 v = to - from;
 114                                 b3Scalar t = v.dot(diff);
 115
 116                                 if (t > 0)
 117                                 {
 118                                         b3Scalar 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                                 b3Vector3 p = b3MakeVector3(b3Scalar(0.), b3Scalar(0.), b3Scalar(0.));
 156
 157                                 const b3Vector3& a = m_simplexVectorW[0];
 158                                 const b3Vector3& b = m_simplexVectorW[1];
 159                                 const b3Vector3& 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                                 b3Vector3 p = b3MakeVector3(b3Scalar(0.), b3Scalar(0.), b3Scalar(0.));
 180
 181                                 const b3Vector3& a = m_simplexVectorW[0];
 182                                 const b3Vector3& b = m_simplexVectorW[1];
 183                                 const b3Vector3& c = m_simplexVectorW[2];
 184                                 const b3Vector3& 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(b3Scalar(0.), b3Scalar(0.), b3Scalar(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 b3VoronoiSimplexSolver::closest(b3Vector3& v)
 237 {
 238         bool succes = updateClosestVectorAndPoints();
 239         v = m_cachedV;
 240         return succes;
 241 }
 242
 243 b3Scalar b3VoronoiSimplexSolver::maxVertex()
 244 {
 245         int i, numverts = numVertices();
 246         b3Scalar maxV = b3Scalar(0.);
 247         for (i = 0; i < numverts; i++)
 248         {
 249                 b3Scalar 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 b3VoronoiSimplexSolver::getSimplex(b3Vector3* pBuf, b3Vector3* qBuf, b3Vector3* 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 b3VoronoiSimplexSolver::inSimplex(const b3Vector3& w)
 270 {
 271         bool found = false;
 272         int i, numverts = numVertices();
 273         //b3Scalar maxV = b3Scalar(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                         found = true;
 284         }
 285
 286         //check in case lastW is already removed
 287         if (w == m_lastW)
 288                 return true;
 289
 290         return found;
 291 }
 292
 293 void b3VoronoiSimplexSolver::backup_closest(b3Vector3& v)
 294 {
 295         v = m_cachedV;
 296 }
 297
 298 bool b3VoronoiSimplexSolver::emptySimplex() const
 299 {
 300         return (numVertices() == 0);
 301 }
 302
 303 void b3VoronoiSimplexSolver::compute_points(b3Vector3& p1, b3Vector3& p2)
 304 {
 305         updateClosestVectorAndPoints();
 306         p1 = m_cachedP1;
 307         p2 = m_cachedP2;
 308 }
 309
 310 bool b3VoronoiSimplexSolver::closestPtPointTriangle(const b3Vector3& p, const b3Vector3& a, const b3Vector3& b, const b3Vector3& c, b3SubSimplexClosestResult& result)
 311 {
 312         result.m_usedVertices.reset();
 313
 314         // Check if P in vertex region outside A
 315         b3Vector3 ab = b - a;
 316         b3Vector3 ac = c - a;
 317         b3Vector3 ap = p - a;
 318         b3Scalar d1 = ab.dot(ap);
 319         b3Scalar d2 = ac.dot(ap);
 320         if (d1 <= b3Scalar(0.0) && d2 <= b3Scalar(0.0))
 321         {
 322                 result.m_closestPointOnSimplex = a;
 323                 result.m_usedVertices.usedVertexA = true;
 324                 result.setBarycentricCoordinates(1, 0, 0);
 325                 return true;  // a; // barycentric coordinates (1,0,0)
 326         }
 327
 328         // Check if P in vertex region outside B
 329         b3Vector3 bp = p - b;
 330         b3Scalar d3 = ab.dot(bp);
 331         b3Scalar d4 = ac.dot(bp);
 332         if (d3 >= b3Scalar(0.0) && d4 <= d3)
 333         {
 334                 result.m_closestPointOnSimplex = b;
 335                 result.m_usedVertices.usedVertexB = true;
 336                 result.setBarycentricCoordinates(0, 1, 0);
 337
 338                 return true;  // b; // barycentric coordinates (0,1,0)
 339         }
 340         // Check if P in edge region of AB, if so return projection of P onto AB
 341         b3Scalar vc = d1 * d4 - d3 * d2;
 342         if (vc <= b3Scalar(0.0) && d1 >= b3Scalar(0.0) && d3 <= b3Scalar(0.0))
 343         {
 344                 b3Scalar v = d1 / (d1 - d3);
 345                 result.m_closestPointOnSimplex = a + v * ab;
 346                 result.m_usedVertices.usedVertexA = true;
 347                 result.m_usedVertices.usedVertexB = true;
 348                 result.setBarycentricCoordinates(1 - v, v, 0);
 349                 return true;
 350                 //return a + v * ab; // barycentric coordinates (1-v,v,0)
 351         }
 352
 353         // Check if P in vertex region outside C
 354         b3Vector3 cp = p - c;
 355         b3Scalar d5 = ab.dot(cp);
 356         b3Scalar d6 = ac.dot(cp);
 357         if (d6 >= b3Scalar(0.0) && d5 <= d6)
 358         {
 359                 result.m_closestPointOnSimplex = c;
 360                 result.m_usedVertices.usedVertexC = true;
 361                 result.setBarycentricCoordinates(0, 0, 1);
 362                 return true;  //c; // barycentric coordinates (0,0,1)
 363         }
 364
 365         // Check if P in edge region of AC, if so return projection of P onto AC
 366         b3Scalar vb = d5 * d2 - d1 * d6;
 367         if (vb <= b3Scalar(0.0) && d2 >= b3Scalar(0.0) && d6 <= b3Scalar(0.0))
 368         {
 369                 b3Scalar w = d2 / (d2 - d6);
 370                 result.m_closestPointOnSimplex = a + w * ac;
 371                 result.m_usedVertices.usedVertexA = true;
 372                 result.m_usedVertices.usedVertexC = true;
 373                 result.setBarycentricCoordinates(1 - w, 0, w);
 374                 return true;
 375                 //return a + w * ac; // barycentric coordinates (1-w,0,w)
 376         }
 377
 378         // Check if P in edge region of BC, if so return projection of P onto BC
 379         b3Scalar va = d3 * d6 - d5 * d4;
 380         if (va <= b3Scalar(0.0) && (d4 - d3) >= b3Scalar(0.0) && (d5 - d6) >= b3Scalar(0.0))
 381         {
 382                 b3Scalar w = (d4 - d3) / ((d4 - d3) + (d5 - d6));
 383
 384                 result.m_closestPointOnSimplex = b + w * (c - b);
 385                 result.m_usedVertices.usedVertexB = true;
 386                 result.m_usedVertices.usedVertexC = true;
 387                 result.setBarycentricCoordinates(0, 1 - w, w);
 388                 return true;
 389                 // return b + w * (c - b); // barycentric coordinates (0,1-w,w)
 390         }
 391
 392         // P inside face region. Compute Q through its barycentric coordinates (u,v,w)
 393         b3Scalar denom = b3Scalar(1.0) / (va + vb + vc);
 394         b3Scalar v = vb * denom;
 395         b3Scalar w = vc * denom;
 396
 397         result.m_closestPointOnSimplex = a + ab * v + ac * w;
 398         result.m_usedVertices.usedVertexA = true;
 399         result.m_usedVertices.usedVertexB = true;
 400         result.m_usedVertices.usedVertexC = true;
 401         result.setBarycentricCoordinates(1 - v - w, v, w);
 402
 403         return true;
 404         //      return a + ab * v + ac * w; // = u*a + v*b + w*c, u = va * denom = b3Scalar(1.0) - v - w
 405 }
 406
 407 /// Test if point p and d lie on opposite sides of plane through abc
 408 int b3VoronoiSimplexSolver::pointOutsideOfPlane(const b3Vector3& p, const b3Vector3& a, const b3Vector3& b, const b3Vector3& c, const b3Vector3& d)
 409 {
 410         b3Vector3 normal = (b - a).cross(c - a);
 411
 412         b3Scalar signp = (p - a).dot(normal);  // [AP AB AC]
 413         b3Scalar signd = (d - a).dot(normal);  // [AD AB AC]
 414
 415 #ifdef B3_CATCH_DEGENERATE_TETRAHEDRON
 416 #ifdef BT_USE_DOUBLE_PRECISION
 417         if (signd * signd < (b3Scalar(1e-8) * b3Scalar(1e-8)))
 418         {
 419                 return -1;
 420         }
 421 #else
 422         if (signd * signd < (b3Scalar(1e-4) * b3Scalar(1e-4)))
 423         {
 424                 //              printf("affine dependent/degenerate\n");//
 425                 return -1;
 426         }
 427 #endif
 428
 429 #endif
 430         // Points on opposite sides if expression signs are opposite
 431         return signp * signd < b3Scalar(0.);
 432 }
 433
 434 bool b3VoronoiSimplexSolver::closestPtPointTetrahedron(const b3Vector3& p, const b3Vector3& a, const b3Vector3& b, const b3Vector3& c, const b3Vector3& d, b3SubSimplexClosestResult& finalResult)
 435 {
 436         b3SubSimplexClosestResult tempResult;
 437
 438         // Start out assuming point inside all halfspaces, so closest to itself
 439         finalResult.m_closestPointOnSimplex = p;
 440         finalResult.m_usedVertices.reset();
 441         finalResult.m_usedVertices.usedVertexA = true;
 442         finalResult.m_usedVertices.usedVertexB = true;
 443         finalResult.m_usedVertices.usedVertexC = true;
 444         finalResult.m_usedVertices.usedVertexD = true;
 445
 446         int pointOutsideABC = pointOutsideOfPlane(p, a, b, c, d);
 447         int pointOutsideACD = pointOutsideOfPlane(p, a, c, d, b);
 448         int pointOutsideADB = pointOutsideOfPlane(p, a, d, b, c);
 449         int pointOutsideBDC = pointOutsideOfPlane(p, b, d, c, a);
 450
 451         if (pointOutsideABC < 0 || pointOutsideACD < 0 || pointOutsideADB < 0 || pointOutsideBDC < 0)
 452         {
 453                 finalResult.m_degenerate = true;
 454                 return false;
 455         }
 456
 457         if (!pointOutsideABC && !pointOutsideACD && !pointOutsideADB && !pointOutsideBDC)
 458         {
 459                 return false;
 460         }
 461
 462         b3Scalar bestSqDist = FLT_MAX;
 463         // If point outside face abc then compute closest point on abc
 464         if (pointOutsideABC)
 465         {
 466                 closestPtPointTriangle(p, a, b, c, tempResult);
 467                 b3Vector3 q = tempResult.m_closestPointOnSimplex;
 468
 469                 b3Scalar sqDist = (q - p).dot(q - p);
 470                 // Update best closest point if (squared) distance is less than current best
 471                 if (sqDist < bestSqDist)
 472                 {
 473                         bestSqDist = sqDist;
 474                         finalResult.m_closestPointOnSimplex = q;
 475                         //convert result bitmask!
 476                         finalResult.m_usedVertices.reset();
 477                         finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA;
 478                         finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexB;
 479                         finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexC;
 480                         finalResult.setBarycentricCoordinates(
 481                                 tempResult.m_barycentricCoords[VERTA],
 482                                 tempResult.m_barycentricCoords[VERTB],
 483                                 tempResult.m_barycentricCoords[VERTC],
 484                                 0);
 485                 }
 486         }
 487
 488         // Repeat test for face acd
 489         if (pointOutsideACD)
 490         {
 491                 closestPtPointTriangle(p, a, c, d, tempResult);
 492                 b3Vector3 q = tempResult.m_closestPointOnSimplex;
 493                 //convert result bitmask!
 494
 495                 b3Scalar sqDist = (q - p).dot(q - p);
 496                 if (sqDist < bestSqDist)
 497                 {
 498                         bestSqDist = sqDist;
 499                         finalResult.m_closestPointOnSimplex = q;
 500                         finalResult.m_usedVertices.reset();
 501                         finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA;
 502
 503                         finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexB;
 504                         finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexC;
 505                         finalResult.setBarycentricCoordinates(
 506                                 tempResult.m_barycentricCoords[VERTA],
 507                                 0,
 508                                 tempResult.m_barycentricCoords[VERTB],
 509                                 tempResult.m_barycentricCoords[VERTC]);
 510                 }
 511         }
 512         // Repeat test for face adb
 513
 514         if (pointOutsideADB)
 515         {
 516                 closestPtPointTriangle(p, a, d, b, tempResult);
 517                 b3Vector3 q = tempResult.m_closestPointOnSimplex;
 518                 //convert result bitmask!
 519
 520                 b3Scalar sqDist = (q - p).dot(q - p);
 521                 if (sqDist < bestSqDist)
 522                 {
 523                         bestSqDist = sqDist;
 524                         finalResult.m_closestPointOnSimplex = q;
 525                         finalResult.m_usedVertices.reset();
 526                         finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA;
 527                         finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexC;
 528
 529                         finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexB;
 530                         finalResult.setBarycentricCoordinates(
 531                                 tempResult.m_barycentricCoords[VERTA],
 532                                 tempResult.m_barycentricCoords[VERTC],
 533                                 0,
 534                                 tempResult.m_barycentricCoords[VERTB]);
 535                 }
 536         }
 537         // Repeat test for face bdc
 538
 539         if (pointOutsideBDC)
 540         {
 541                 closestPtPointTriangle(p, b, d, c, tempResult);
 542                 b3Vector3 q = tempResult.m_closestPointOnSimplex;
 543                 //convert result bitmask!
 544                 b3Scalar sqDist = (q - p).dot(q - p);
 545                 if (sqDist < bestSqDist)
 546                 {
 547                         bestSqDist = sqDist;
 548                         finalResult.m_closestPointOnSimplex = q;
 549                         finalResult.m_usedVertices.reset();
 550                         //
 551                         finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexA;
 552                         finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexC;
 553                         finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexB;
 554
 555                         finalResult.setBarycentricCoordinates(
 556                                 0,
 557                                 tempResult.m_barycentricCoords[VERTA],
 558                                 tempResult.m_barycentricCoords[VERTC],
 559                                 tempResult.m_barycentricCoords[VERTB]);
 560                 }
 561         }
 562
 563         //help! we ended up full !
 564
 565         if (finalResult.m_usedVertices.usedVertexA &&
 566                 finalResult.m_usedVertices.usedVertexB &&
 567                 finalResult.m_usedVertices.usedVertexC &&
 568                 finalResult.m_usedVertices.usedVertexD)
 569         {
 570                 return true;
 571         }
 572
 573         return true;
 574 }