00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027 #include "btVoronoiSimplexSolver.h"
00028
00029 #define VERTA 0
00030 #define VERTB 1
00031 #define VERTC 2
00032 #define VERTD 3
00033
00034 #define CATCH_DEGENERATE_TETRAHEDRON 1
00035 void btVoronoiSimplexSolver::removeVertex(int index)
00036 {
00037
00038 btAssert(m_numVertices>0);
00039 m_numVertices--;
00040 m_simplexVectorW[index] = m_simplexVectorW[m_numVertices];
00041 m_simplexPointsP[index] = m_simplexPointsP[m_numVertices];
00042 m_simplexPointsQ[index] = m_simplexPointsQ[m_numVertices];
00043 }
00044
00045 void btVoronoiSimplexSolver::reduceVertices (const btUsageBitfield& usedVerts)
00046 {
00047 if ((numVertices() >= 4) && (!usedVerts.usedVertexD))
00048 removeVertex(3);
00049
00050 if ((numVertices() >= 3) && (!usedVerts.usedVertexC))
00051 removeVertex(2);
00052
00053 if ((numVertices() >= 2) && (!usedVerts.usedVertexB))
00054 removeVertex(1);
00055
00056 if ((numVertices() >= 1) && (!usedVerts.usedVertexA))
00057 removeVertex(0);
00058
00059 }
00060
00061
00062
00063
00064
00065
00066 void btVoronoiSimplexSolver::reset()
00067 {
00068 m_cachedValidClosest = false;
00069 m_numVertices = 0;
00070 m_needsUpdate = true;
00071 m_lastW = btVector3(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
00072 m_cachedBC.reset();
00073 }
00074
00075
00076
00077
00078 void btVoronoiSimplexSolver::addVertex(const btVector3& w, const btVector3& p, const btVector3& q)
00079 {
00080 m_lastW = w;
00081 m_needsUpdate = true;
00082
00083 m_simplexVectorW[m_numVertices] = w;
00084 m_simplexPointsP[m_numVertices] = p;
00085 m_simplexPointsQ[m_numVertices] = q;
00086
00087 m_numVertices++;
00088 }
00089
00090 bool btVoronoiSimplexSolver::updateClosestVectorAndPoints()
00091 {
00092
00093 if (m_needsUpdate)
00094 {
00095 m_cachedBC.reset();
00096
00097 m_needsUpdate = false;
00098
00099 switch (numVertices())
00100 {
00101 case 0:
00102 m_cachedValidClosest = false;
00103 break;
00104 case 1:
00105 {
00106 m_cachedP1 = m_simplexPointsP[0];
00107 m_cachedP2 = m_simplexPointsQ[0];
00108 m_cachedV = m_cachedP1-m_cachedP2;
00109 m_cachedBC.reset();
00110 m_cachedBC.setBarycentricCoordinates(btScalar(1.),btScalar(0.),btScalar(0.),btScalar(0.));
00111 m_cachedValidClosest = m_cachedBC.isValid();
00112 break;
00113 };
00114 case 2:
00115 {
00116
00117 const btVector3& from = m_simplexVectorW[0];
00118 const btVector3& to = m_simplexVectorW[1];
00119 btVector3 nearest;
00120
00121 btVector3 p (btScalar(0.),btScalar(0.),btScalar(0.));
00122 btVector3 diff = p - from;
00123 btVector3 v = to - from;
00124 btScalar t = v.dot(diff);
00125
00126 if (t > 0) {
00127 btScalar dotVV = v.dot(v);
00128 if (t < dotVV) {
00129 t /= dotVV;
00130 diff -= t*v;
00131 m_cachedBC.m_usedVertices.usedVertexA = true;
00132 m_cachedBC.m_usedVertices.usedVertexB = true;
00133 } else {
00134 t = 1;
00135 diff -= v;
00136
00137 m_cachedBC.m_usedVertices.usedVertexB = true;
00138 }
00139 } else
00140 {
00141 t = 0;
00142
00143 m_cachedBC.m_usedVertices.usedVertexA = true;
00144 }
00145 m_cachedBC.setBarycentricCoordinates(1-t,t);
00146 nearest = from + t*v;
00147
00148 m_cachedP1 = m_simplexPointsP[0] + t * (m_simplexPointsP[1] - m_simplexPointsP[0]);
00149 m_cachedP2 = m_simplexPointsQ[0] + t * (m_simplexPointsQ[1] - m_simplexPointsQ[0]);
00150 m_cachedV = m_cachedP1 - m_cachedP2;
00151
00152 reduceVertices(m_cachedBC.m_usedVertices);
00153
00154 m_cachedValidClosest = m_cachedBC.isValid();
00155 break;
00156 }
00157 case 3:
00158 {
00159
00160 btVector3 p (btScalar(0.),btScalar(0.),btScalar(0.));
00161
00162 const btVector3& a = m_simplexVectorW[0];
00163 const btVector3& b = m_simplexVectorW[1];
00164 const btVector3& c = m_simplexVectorW[2];
00165
00166 closestPtPointTriangle(p,a,b,c,m_cachedBC);
00167 m_cachedP1 = m_simplexPointsP[0] * m_cachedBC.m_barycentricCoords[0] +
00168 m_simplexPointsP[1] * m_cachedBC.m_barycentricCoords[1] +
00169 m_simplexPointsP[2] * m_cachedBC.m_barycentricCoords[2];
00170
00171 m_cachedP2 = m_simplexPointsQ[0] * m_cachedBC.m_barycentricCoords[0] +
00172 m_simplexPointsQ[1] * m_cachedBC.m_barycentricCoords[1] +
00173 m_simplexPointsQ[2] * m_cachedBC.m_barycentricCoords[2];
00174
00175 m_cachedV = m_cachedP1-m_cachedP2;
00176
00177 reduceVertices (m_cachedBC.m_usedVertices);
00178 m_cachedValidClosest = m_cachedBC.isValid();
00179
00180 break;
00181 }
00182 case 4:
00183 {
00184
00185
00186 btVector3 p (btScalar(0.),btScalar(0.),btScalar(0.));
00187
00188 const btVector3& a = m_simplexVectorW[0];
00189 const btVector3& b = m_simplexVectorW[1];
00190 const btVector3& c = m_simplexVectorW[2];
00191 const btVector3& d = m_simplexVectorW[3];
00192
00193 bool hasSeperation = closestPtPointTetrahedron(p,a,b,c,d,m_cachedBC);
00194
00195 if (hasSeperation)
00196 {
00197
00198 m_cachedP1 = m_simplexPointsP[0] * m_cachedBC.m_barycentricCoords[0] +
00199 m_simplexPointsP[1] * m_cachedBC.m_barycentricCoords[1] +
00200 m_simplexPointsP[2] * m_cachedBC.m_barycentricCoords[2] +
00201 m_simplexPointsP[3] * m_cachedBC.m_barycentricCoords[3];
00202
00203 m_cachedP2 = m_simplexPointsQ[0] * m_cachedBC.m_barycentricCoords[0] +
00204 m_simplexPointsQ[1] * m_cachedBC.m_barycentricCoords[1] +
00205 m_simplexPointsQ[2] * m_cachedBC.m_barycentricCoords[2] +
00206 m_simplexPointsQ[3] * m_cachedBC.m_barycentricCoords[3];
00207
00208 m_cachedV = m_cachedP1-m_cachedP2;
00209 reduceVertices (m_cachedBC.m_usedVertices);
00210 } else
00211 {
00212
00213
00214 if (m_cachedBC.m_degenerate)
00215 {
00216 m_cachedValidClosest = false;
00217 } else
00218 {
00219 m_cachedValidClosest = true;
00220
00221 m_cachedV.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
00222 }
00223 break;
00224 }
00225
00226 m_cachedValidClosest = m_cachedBC.isValid();
00227
00228
00229 break;
00230 }
00231 default:
00232 {
00233 m_cachedValidClosest = false;
00234 }
00235 };
00236 }
00237
00238 return m_cachedValidClosest;
00239
00240 }
00241
00242
00243 bool btVoronoiSimplexSolver::closest(btVector3& v)
00244 {
00245 bool succes = updateClosestVectorAndPoints();
00246 v = m_cachedV;
00247 return succes;
00248 }
00249
00250
00251
00252 btScalar btVoronoiSimplexSolver::maxVertex()
00253 {
00254 int i, numverts = numVertices();
00255 btScalar maxV = btScalar(0.);
00256 for (i=0;i<numverts;i++)
00257 {
00258 btScalar curLen2 = m_simplexVectorW[i].length2();
00259 if (maxV < curLen2)
00260 maxV = curLen2;
00261 }
00262 return maxV;
00263 }
00264
00265
00266
00267
00268 int btVoronoiSimplexSolver::getSimplex(btVector3 *pBuf, btVector3 *qBuf, btVector3 *yBuf) const
00269 {
00270 int i;
00271 for (i=0;i<numVertices();i++)
00272 {
00273 yBuf[i] = m_simplexVectorW[i];
00274 pBuf[i] = m_simplexPointsP[i];
00275 qBuf[i] = m_simplexPointsQ[i];
00276 }
00277 return numVertices();
00278 }
00279
00280
00281
00282
00283 bool btVoronoiSimplexSolver::inSimplex(const btVector3& w)
00284 {
00285 bool found = false;
00286 int i, numverts = numVertices();
00287
00288
00289
00290 for (i=0;i<numverts;i++)
00291 {
00292 #ifdef BT_USE_EQUAL_VERTEX_THRESHOLD
00293 if ( m_simplexVectorW[i].distance2(w) <= m_equalVertexThreshold)
00294 #else
00295 if (m_simplexVectorW[i] == w)
00296 #endif
00297 found = true;
00298 }
00299
00300
00301 if (w == m_lastW)
00302 return true;
00303
00304 return found;
00305 }
00306
00307 void btVoronoiSimplexSolver::backup_closest(btVector3& v)
00308 {
00309 v = m_cachedV;
00310 }
00311
00312
00313 bool btVoronoiSimplexSolver::emptySimplex() const
00314 {
00315 return (numVertices() == 0);
00316
00317 }
00318
00319 void btVoronoiSimplexSolver::compute_points(btVector3& p1, btVector3& p2)
00320 {
00321 updateClosestVectorAndPoints();
00322 p1 = m_cachedP1;
00323 p2 = m_cachedP2;
00324
00325 }
00326
00327
00328
00329
00330 bool btVoronoiSimplexSolver::closestPtPointTriangle(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c,btSubSimplexClosestResult& result)
00331 {
00332 result.m_usedVertices.reset();
00333
00334
00335 btVector3 ab = b - a;
00336 btVector3 ac = c - a;
00337 btVector3 ap = p - a;
00338 btScalar d1 = ab.dot(ap);
00339 btScalar d2 = ac.dot(ap);
00340 if (d1 <= btScalar(0.0) && d2 <= btScalar(0.0))
00341 {
00342 result.m_closestPointOnSimplex = a;
00343 result.m_usedVertices.usedVertexA = true;
00344 result.setBarycentricCoordinates(1,0,0);
00345 return true;
00346 }
00347
00348
00349 btVector3 bp = p - b;
00350 btScalar d3 = ab.dot(bp);
00351 btScalar d4 = ac.dot(bp);
00352 if (d3 >= btScalar(0.0) && d4 <= d3)
00353 {
00354 result.m_closestPointOnSimplex = b;
00355 result.m_usedVertices.usedVertexB = true;
00356 result.setBarycentricCoordinates(0,1,0);
00357
00358 return true;
00359 }
00360
00361 btScalar vc = d1*d4 - d3*d2;
00362 if (vc <= btScalar(0.0) && d1 >= btScalar(0.0) && d3 <= btScalar(0.0)) {
00363 btScalar v = d1 / (d1 - d3);
00364 result.m_closestPointOnSimplex = a + v * ab;
00365 result.m_usedVertices.usedVertexA = true;
00366 result.m_usedVertices.usedVertexB = true;
00367 result.setBarycentricCoordinates(1-v,v,0);
00368 return true;
00369
00370 }
00371
00372
00373 btVector3 cp = p - c;
00374 btScalar d5 = ab.dot(cp);
00375 btScalar d6 = ac.dot(cp);
00376 if (d6 >= btScalar(0.0) && d5 <= d6)
00377 {
00378 result.m_closestPointOnSimplex = c;
00379 result.m_usedVertices.usedVertexC = true;
00380 result.setBarycentricCoordinates(0,0,1);
00381 return true;
00382 }
00383
00384
00385 btScalar vb = d5*d2 - d1*d6;
00386 if (vb <= btScalar(0.0) && d2 >= btScalar(0.0) && d6 <= btScalar(0.0)) {
00387 btScalar w = d2 / (d2 - d6);
00388 result.m_closestPointOnSimplex = a + w * ac;
00389 result.m_usedVertices.usedVertexA = true;
00390 result.m_usedVertices.usedVertexC = true;
00391 result.setBarycentricCoordinates(1-w,0,w);
00392 return true;
00393
00394 }
00395
00396
00397 btScalar va = d3*d6 - d5*d4;
00398 if (va <= btScalar(0.0) && (d4 - d3) >= btScalar(0.0) && (d5 - d6) >= btScalar(0.0)) {
00399 btScalar w = (d4 - d3) / ((d4 - d3) + (d5 - d6));
00400
00401 result.m_closestPointOnSimplex = b + w * (c - b);
00402 result.m_usedVertices.usedVertexB = true;
00403 result.m_usedVertices.usedVertexC = true;
00404 result.setBarycentricCoordinates(0,1-w,w);
00405 return true;
00406
00407 }
00408
00409
00410 btScalar denom = btScalar(1.0) / (va + vb + vc);
00411 btScalar v = vb * denom;
00412 btScalar w = vc * denom;
00413
00414 result.m_closestPointOnSimplex = a + ab * v + ac * w;
00415 result.m_usedVertices.usedVertexA = true;
00416 result.m_usedVertices.usedVertexB = true;
00417 result.m_usedVertices.usedVertexC = true;
00418 result.setBarycentricCoordinates(1-v-w,v,w);
00419
00420 return true;
00421
00422
00423 }
00424
00425
00426
00427
00428
00430 int btVoronoiSimplexSolver::pointOutsideOfPlane(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d)
00431 {
00432 btVector3 normal = (b-a).cross(c-a);
00433
00434 btScalar signp = (p - a).dot(normal);
00435 btScalar signd = (d - a).dot( normal);
00436
00437 #ifdef CATCH_DEGENERATE_TETRAHEDRON
00438 #ifdef BT_USE_DOUBLE_PRECISION
00439 if (signd * signd < (btScalar(1e-8) * btScalar(1e-8)))
00440 {
00441 return -1;
00442 }
00443 #else
00444 if (signd * signd < (btScalar(1e-4) * btScalar(1e-4)))
00445 {
00446
00447 return -1;
00448 }
00449 #endif
00450
00451 #endif
00452
00453 return signp * signd < btScalar(0.);
00454 }
00455
00456
00457 bool btVoronoiSimplexSolver::closestPtPointTetrahedron(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d, btSubSimplexClosestResult& finalResult)
00458 {
00459 btSubSimplexClosestResult tempResult;
00460
00461
00462 finalResult.m_closestPointOnSimplex = p;
00463 finalResult.m_usedVertices.reset();
00464 finalResult.m_usedVertices.usedVertexA = true;
00465 finalResult.m_usedVertices.usedVertexB = true;
00466 finalResult.m_usedVertices.usedVertexC = true;
00467 finalResult.m_usedVertices.usedVertexD = true;
00468
00469 int pointOutsideABC = pointOutsideOfPlane(p, a, b, c, d);
00470 int pointOutsideACD = pointOutsideOfPlane(p, a, c, d, b);
00471 int pointOutsideADB = pointOutsideOfPlane(p, a, d, b, c);
00472 int pointOutsideBDC = pointOutsideOfPlane(p, b, d, c, a);
00473
00474 if (pointOutsideABC < 0 || pointOutsideACD < 0 || pointOutsideADB < 0 || pointOutsideBDC < 0)
00475 {
00476 finalResult.m_degenerate = true;
00477 return false;
00478 }
00479
00480 if (!pointOutsideABC && !pointOutsideACD && !pointOutsideADB && !pointOutsideBDC)
00481 {
00482 return false;
00483 }
00484
00485
00486 btScalar bestSqDist = FLT_MAX;
00487
00488 if (pointOutsideABC)
00489 {
00490 closestPtPointTriangle(p, a, b, c,tempResult);
00491 btVector3 q = tempResult.m_closestPointOnSimplex;
00492
00493 btScalar sqDist = (q - p).dot( q - p);
00494
00495 if (sqDist < bestSqDist) {
00496 bestSqDist = sqDist;
00497 finalResult.m_closestPointOnSimplex = q;
00498
00499 finalResult.m_usedVertices.reset();
00500 finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA;
00501 finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexB;
00502 finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexC;
00503 finalResult.setBarycentricCoordinates(
00504 tempResult.m_barycentricCoords[VERTA],
00505 tempResult.m_barycentricCoords[VERTB],
00506 tempResult.m_barycentricCoords[VERTC],
00507 0
00508 );
00509
00510 }
00511 }
00512
00513
00514
00515 if (pointOutsideACD)
00516 {
00517 closestPtPointTriangle(p, a, c, d,tempResult);
00518 btVector3 q = tempResult.m_closestPointOnSimplex;
00519
00520
00521 btScalar sqDist = (q - p).dot( q - p);
00522 if (sqDist < bestSqDist)
00523 {
00524 bestSqDist = sqDist;
00525 finalResult.m_closestPointOnSimplex = q;
00526 finalResult.m_usedVertices.reset();
00527 finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA;
00528
00529 finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexB;
00530 finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexC;
00531 finalResult.setBarycentricCoordinates(
00532 tempResult.m_barycentricCoords[VERTA],
00533 0,
00534 tempResult.m_barycentricCoords[VERTB],
00535 tempResult.m_barycentricCoords[VERTC]
00536 );
00537
00538 }
00539 }
00540
00541
00542
00543 if (pointOutsideADB)
00544 {
00545 closestPtPointTriangle(p, a, d, b,tempResult);
00546 btVector3 q = tempResult.m_closestPointOnSimplex;
00547
00548
00549 btScalar sqDist = (q - p).dot( q - p);
00550 if (sqDist < bestSqDist)
00551 {
00552 bestSqDist = sqDist;
00553 finalResult.m_closestPointOnSimplex = q;
00554 finalResult.m_usedVertices.reset();
00555 finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA;
00556 finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexC;
00557
00558 finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexB;
00559 finalResult.setBarycentricCoordinates(
00560 tempResult.m_barycentricCoords[VERTA],
00561 tempResult.m_barycentricCoords[VERTC],
00562 0,
00563 tempResult.m_barycentricCoords[VERTB]
00564 );
00565
00566 }
00567 }
00568
00569
00570
00571 if (pointOutsideBDC)
00572 {
00573 closestPtPointTriangle(p, b, d, c,tempResult);
00574 btVector3 q = tempResult.m_closestPointOnSimplex;
00575
00576 btScalar sqDist = (q - p).dot( q - p);
00577 if (sqDist < bestSqDist)
00578 {
00579 bestSqDist = sqDist;
00580 finalResult.m_closestPointOnSimplex = q;
00581 finalResult.m_usedVertices.reset();
00582
00583 finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexA;
00584 finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexC;
00585 finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexB;
00586
00587 finalResult.setBarycentricCoordinates(
00588 0,
00589 tempResult.m_barycentricCoords[VERTA],
00590 tempResult.m_barycentricCoords[VERTC],
00591 tempResult.m_barycentricCoords[VERTB]
00592 );
00593
00594 }
00595 }
00596
00597
00598
00599 if (finalResult.m_usedVertices.usedVertexA &&
00600 finalResult.m_usedVertices.usedVertexB &&
00601 finalResult.m_usedVertices.usedVertexC &&
00602 finalResult.m_usedVertices.usedVertexD)
00603 {
00604 return true;
00605 }
00606
00607 return true;
00608 }
00609