Code-style consistency improvement:
Apply clang-format-all.sh using the _clang-format file through all the cpp/.h files. make sure not to apply it to certain serialization structures, since some parser expects the * as part of the name, instead of type. This commit contains no other changes aside from adding and applying clang-format-all.sh
This commit is contained in:
erwincoumans
@@ -23,26 +23,24 @@ subject to the following restrictions:
|
||||
|
||||
*/
|
||||
|
||||
|
||||
#include "b3VoronoiSimplexSolver.h"
|
||||
|
||||
#define VERTA 0
|
||||
#define VERTB 1
|
||||
#define VERTC 2
|
||||
#define VERTD 3
|
||||
#define VERTA 0
|
||||
#define VERTB 1
|
||||
#define VERTC 2
|
||||
#define VERTD 3
|
||||
|
||||
#define B3_CATCH_DEGENERATE_TETRAHEDRON 1
|
||||
void b3VoronoiSimplexSolver::removeVertex(int index)
|
||||
void b3VoronoiSimplexSolver::removeVertex(int index)
|
||||
{
|
||||
|
||||
b3Assert(m_numVertices>0);
|
||||
b3Assert(m_numVertices > 0);
|
||||
m_numVertices--;
|
||||
m_simplexVectorW[index] = m_simplexVectorW[m_numVertices];
|
||||
m_simplexPointsP[index] = m_simplexPointsP[m_numVertices];
|
||||
m_simplexPointsQ[index] = m_simplexPointsQ[m_numVertices];
|
||||
}
|
||||
|
||||
void b3VoronoiSimplexSolver::reduceVertices (const b3UsageBitfield& usedVerts)
|
||||
void b3VoronoiSimplexSolver::reduceVertices(const b3UsageBitfield& usedVerts)
|
||||
{
|
||||
if ((numVertices() >= 4) && (!usedVerts.usedVertexD))
|
||||
removeVertex(3);
|
||||
@@ -52,29 +50,22 @@ void b3VoronoiSimplexSolver::reduceVertices (const b3UsageBitfield& usedVerts)
|
||||
|
||||
if ((numVertices() >= 2) && (!usedVerts.usedVertexB))
|
||||
removeVertex(1);
|
||||
|
||||
|
||||
if ((numVertices() >= 1) && (!usedVerts.usedVertexA))
|
||||
removeVertex(0);
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
//clear the simplex, remove all the vertices
|
||||
void b3VoronoiSimplexSolver::reset()
|
||||
{
|
||||
m_cachedValidClosest = false;
|
||||
m_numVertices = 0;
|
||||
m_needsUpdate = true;
|
||||
m_lastW = b3MakeVector3(b3Scalar(B3_LARGE_FLOAT),b3Scalar(B3_LARGE_FLOAT),b3Scalar(B3_LARGE_FLOAT));
|
||||
m_lastW = b3MakeVector3(b3Scalar(B3_LARGE_FLOAT), b3Scalar(B3_LARGE_FLOAT), b3Scalar(B3_LARGE_FLOAT));
|
||||
m_cachedBC.reset();
|
||||
}
|
||||
|
||||
|
||||
|
||||
//add a vertex
|
||||
//add a vertex
|
||||
void b3VoronoiSimplexSolver::addVertex(const b3Vector3& w, const b3Vector3& p, const b3Vector3& q)
|
||||
{
|
||||
m_lastW = w;
|
||||
@@ -87,9 +78,8 @@ void b3VoronoiSimplexSolver::addVertex(const b3Vector3& w, const b3Vector3& p, c
|
||||
m_numVertices++;
|
||||
}
|
||||
|
||||
bool b3VoronoiSimplexSolver::updateClosestVectorAndPoints()
|
||||
bool b3VoronoiSimplexSolver::updateClosestVectorAndPoints()
|
||||
{
|
||||
|
||||
if (m_needsUpdate)
|
||||
{
|
||||
m_cachedBC.reset();
|
||||
@@ -98,127 +88,131 @@ bool b3VoronoiSimplexSolver::updateClosestVectorAndPoints()
|
||||
|
||||
switch (numVertices())
|
||||
{
|
||||
case 0:
|
||||
case 0:
|
||||
m_cachedValidClosest = false;
|
||||
break;
|
||||
case 1:
|
||||
case 1:
|
||||
{
|
||||
m_cachedP1 = m_simplexPointsP[0];
|
||||
m_cachedP2 = m_simplexPointsQ[0];
|
||||
m_cachedV = m_cachedP1-m_cachedP2; //== m_simplexVectorW[0]
|
||||
m_cachedV = m_cachedP1 - m_cachedP2; //== m_simplexVectorW[0]
|
||||
m_cachedBC.reset();
|
||||
m_cachedBC.setBarycentricCoordinates(b3Scalar(1.),b3Scalar(0.),b3Scalar(0.),b3Scalar(0.));
|
||||
m_cachedBC.setBarycentricCoordinates(b3Scalar(1.), b3Scalar(0.), b3Scalar(0.), b3Scalar(0.));
|
||||
m_cachedValidClosest = m_cachedBC.isValid();
|
||||
break;
|
||||
};
|
||||
case 2:
|
||||
case 2:
|
||||
{
|
||||
//closest point origin from line segment
|
||||
const b3Vector3& from = m_simplexVectorW[0];
|
||||
const b3Vector3& to = m_simplexVectorW[1];
|
||||
b3Vector3 nearest;
|
||||
//closest point origin from line segment
|
||||
const b3Vector3& from = m_simplexVectorW[0];
|
||||
const b3Vector3& to = m_simplexVectorW[1];
|
||||
b3Vector3 nearest;
|
||||
|
||||
b3Vector3 p =b3MakeVector3(b3Scalar(0.),b3Scalar(0.),b3Scalar(0.));
|
||||
b3Vector3 diff = p - from;
|
||||
b3Vector3 v = to - from;
|
||||
b3Scalar t = v.dot(diff);
|
||||
|
||||
if (t > 0) {
|
||||
b3Scalar dotVV = v.dot(v);
|
||||
if (t < dotVV) {
|
||||
t /= dotVV;
|
||||
diff -= t*v;
|
||||
m_cachedBC.m_usedVertices.usedVertexA = true;
|
||||
m_cachedBC.m_usedVertices.usedVertexB = true;
|
||||
} else {
|
||||
t = 1;
|
||||
diff -= v;
|
||||
//reduce to 1 point
|
||||
m_cachedBC.m_usedVertices.usedVertexB = true;
|
||||
}
|
||||
} else
|
||||
b3Vector3 p = b3MakeVector3(b3Scalar(0.), b3Scalar(0.), b3Scalar(0.));
|
||||
b3Vector3 diff = p - from;
|
||||
b3Vector3 v = to - from;
|
||||
b3Scalar t = v.dot(diff);
|
||||
|
||||
if (t > 0)
|
||||
{
|
||||
b3Scalar dotVV = v.dot(v);
|
||||
if (t < dotVV)
|
||||
{
|
||||
t = 0;
|
||||
//reduce to 1 point
|
||||
t /= dotVV;
|
||||
diff -= t * v;
|
||||
m_cachedBC.m_usedVertices.usedVertexA = true;
|
||||
m_cachedBC.m_usedVertices.usedVertexB = true;
|
||||
}
|
||||
m_cachedBC.setBarycentricCoordinates(1-t,t);
|
||||
nearest = from + t*v;
|
||||
else
|
||||
{
|
||||
t = 1;
|
||||
diff -= v;
|
||||
//reduce to 1 point
|
||||
m_cachedBC.m_usedVertices.usedVertexB = true;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
t = 0;
|
||||
//reduce to 1 point
|
||||
m_cachedBC.m_usedVertices.usedVertexA = true;
|
||||
}
|
||||
m_cachedBC.setBarycentricCoordinates(1 - t, t);
|
||||
nearest = from + t * v;
|
||||
|
||||
m_cachedP1 = m_simplexPointsP[0] + t * (m_simplexPointsP[1] - m_simplexPointsP[0]);
|
||||
m_cachedP2 = m_simplexPointsQ[0] + t * (m_simplexPointsQ[1] - m_simplexPointsQ[0]);
|
||||
m_cachedV = m_cachedP1 - m_cachedP2;
|
||||
|
||||
reduceVertices(m_cachedBC.m_usedVertices);
|
||||
m_cachedP1 = m_simplexPointsP[0] + t * (m_simplexPointsP[1] - m_simplexPointsP[0]);
|
||||
m_cachedP2 = m_simplexPointsQ[0] + t * (m_simplexPointsQ[1] - m_simplexPointsQ[0]);
|
||||
m_cachedV = m_cachedP1 - m_cachedP2;
|
||||
|
||||
m_cachedValidClosest = m_cachedBC.isValid();
|
||||
break;
|
||||
reduceVertices(m_cachedBC.m_usedVertices);
|
||||
|
||||
m_cachedValidClosest = m_cachedBC.isValid();
|
||||
break;
|
||||
}
|
||||
case 3:
|
||||
{
|
||||
//closest point origin from triangle
|
||||
b3Vector3 p =b3MakeVector3(b3Scalar(0.),b3Scalar(0.),b3Scalar(0.));
|
||||
|
||||
const b3Vector3& a = m_simplexVectorW[0];
|
||||
const b3Vector3& b = m_simplexVectorW[1];
|
||||
const b3Vector3& c = m_simplexVectorW[2];
|
||||
|
||||
closestPtPointTriangle(p,a,b,c,m_cachedBC);
|
||||
m_cachedP1 = m_simplexPointsP[0] * m_cachedBC.m_barycentricCoords[0] +
|
||||
m_simplexPointsP[1] * m_cachedBC.m_barycentricCoords[1] +
|
||||
m_simplexPointsP[2] * m_cachedBC.m_barycentricCoords[2];
|
||||
|
||||
m_cachedP2 = m_simplexPointsQ[0] * m_cachedBC.m_barycentricCoords[0] +
|
||||
m_simplexPointsQ[1] * m_cachedBC.m_barycentricCoords[1] +
|
||||
m_simplexPointsQ[2] * m_cachedBC.m_barycentricCoords[2];
|
||||
|
||||
m_cachedV = m_cachedP1-m_cachedP2;
|
||||
|
||||
reduceVertices (m_cachedBC.m_usedVertices);
|
||||
m_cachedValidClosest = m_cachedBC.isValid();
|
||||
|
||||
break;
|
||||
}
|
||||
case 4:
|
||||
case 3:
|
||||
{
|
||||
//closest point origin from triangle
|
||||
b3Vector3 p = b3MakeVector3(b3Scalar(0.), b3Scalar(0.), b3Scalar(0.));
|
||||
|
||||
const b3Vector3& a = m_simplexVectorW[0];
|
||||
const b3Vector3& b = m_simplexVectorW[1];
|
||||
const b3Vector3& c = m_simplexVectorW[2];
|
||||
|
||||
closestPtPointTriangle(p, a, b, c, m_cachedBC);
|
||||
m_cachedP1 = m_simplexPointsP[0] * m_cachedBC.m_barycentricCoords[0] +
|
||||
m_simplexPointsP[1] * m_cachedBC.m_barycentricCoords[1] +
|
||||
m_simplexPointsP[2] * m_cachedBC.m_barycentricCoords[2];
|
||||
|
||||
m_cachedP2 = m_simplexPointsQ[0] * m_cachedBC.m_barycentricCoords[0] +
|
||||
m_simplexPointsQ[1] * m_cachedBC.m_barycentricCoords[1] +
|
||||
m_simplexPointsQ[2] * m_cachedBC.m_barycentricCoords[2];
|
||||
|
||||
m_cachedV = m_cachedP1 - m_cachedP2;
|
||||
|
||||
reduceVertices(m_cachedBC.m_usedVertices);
|
||||
m_cachedValidClosest = m_cachedBC.isValid();
|
||||
|
||||
break;
|
||||
}
|
||||
case 4:
|
||||
{
|
||||
b3Vector3 p = b3MakeVector3(b3Scalar(0.), b3Scalar(0.), b3Scalar(0.));
|
||||
|
||||
|
||||
b3Vector3 p =b3MakeVector3(b3Scalar(0.),b3Scalar(0.),b3Scalar(0.));
|
||||
|
||||
const b3Vector3& a = m_simplexVectorW[0];
|
||||
const b3Vector3& b = m_simplexVectorW[1];
|
||||
const b3Vector3& c = m_simplexVectorW[2];
|
||||
const b3Vector3& d = m_simplexVectorW[3];
|
||||
|
||||
bool hasSeperation = closestPtPointTetrahedron(p,a,b,c,d,m_cachedBC);
|
||||
bool hasSeperation = closestPtPointTetrahedron(p, a, b, c, d, m_cachedBC);
|
||||
|
||||
if (hasSeperation)
|
||||
{
|
||||
|
||||
m_cachedP1 = m_simplexPointsP[0] * m_cachedBC.m_barycentricCoords[0] +
|
||||
m_simplexPointsP[1] * m_cachedBC.m_barycentricCoords[1] +
|
||||
m_simplexPointsP[2] * m_cachedBC.m_barycentricCoords[2] +
|
||||
m_simplexPointsP[3] * m_cachedBC.m_barycentricCoords[3];
|
||||
m_simplexPointsP[1] * m_cachedBC.m_barycentricCoords[1] +
|
||||
m_simplexPointsP[2] * m_cachedBC.m_barycentricCoords[2] +
|
||||
m_simplexPointsP[3] * m_cachedBC.m_barycentricCoords[3];
|
||||
|
||||
m_cachedP2 = m_simplexPointsQ[0] * m_cachedBC.m_barycentricCoords[0] +
|
||||
m_simplexPointsQ[1] * m_cachedBC.m_barycentricCoords[1] +
|
||||
m_simplexPointsQ[2] * m_cachedBC.m_barycentricCoords[2] +
|
||||
m_simplexPointsQ[3] * m_cachedBC.m_barycentricCoords[3];
|
||||
m_simplexPointsQ[1] * m_cachedBC.m_barycentricCoords[1] +
|
||||
m_simplexPointsQ[2] * m_cachedBC.m_barycentricCoords[2] +
|
||||
m_simplexPointsQ[3] * m_cachedBC.m_barycentricCoords[3];
|
||||
|
||||
m_cachedV = m_cachedP1-m_cachedP2;
|
||||
reduceVertices (m_cachedBC.m_usedVertices);
|
||||
} else
|
||||
m_cachedV = m_cachedP1 - m_cachedP2;
|
||||
reduceVertices(m_cachedBC.m_usedVertices);
|
||||
}
|
||||
else
|
||||
{
|
||||
// printf("sub distance got penetration\n");
|
||||
// printf("sub distance got penetration\n");
|
||||
|
||||
if (m_cachedBC.m_degenerate)
|
||||
{
|
||||
m_cachedValidClosest = false;
|
||||
} else
|
||||
}
|
||||
else
|
||||
{
|
||||
m_cachedValidClosest = true;
|
||||
//degenerate case == false, penetration = true + zero
|
||||
m_cachedV.setValue(b3Scalar(0.),b3Scalar(0.),b3Scalar(0.));
|
||||
m_cachedV.setValue(b3Scalar(0.), b3Scalar(0.), b3Scalar(0.));
|
||||
}
|
||||
break;
|
||||
}
|
||||
@@ -228,7 +222,7 @@ bool b3VoronoiSimplexSolver::updateClosestVectorAndPoints()
|
||||
//closest point origin from tetrahedron
|
||||
break;
|
||||
}
|
||||
default:
|
||||
default:
|
||||
{
|
||||
m_cachedValidClosest = false;
|
||||
}
|
||||
@@ -236,7 +230,6 @@ bool b3VoronoiSimplexSolver::updateClosestVectorAndPoints()
|
||||
}
|
||||
|
||||
return m_cachedValidClosest;
|
||||
|
||||
}
|
||||
|
||||
//return/calculate the closest vertex
|
||||
@@ -247,13 +240,11 @@ bool b3VoronoiSimplexSolver::closest(b3Vector3& v)
|
||||
return succes;
|
||||
}
|
||||
|
||||
|
||||
|
||||
b3Scalar b3VoronoiSimplexSolver::maxVertex()
|
||||
{
|
||||
int i, numverts = numVertices();
|
||||
b3Scalar maxV = b3Scalar(0.);
|
||||
for (i=0;i<numverts;i++)
|
||||
for (i = 0; i < numverts; i++)
|
||||
{
|
||||
b3Scalar curLen2 = m_simplexVectorW[i].length2();
|
||||
if (maxV < curLen2)
|
||||
@@ -262,13 +253,11 @@ b3Scalar b3VoronoiSimplexSolver::maxVertex()
|
||||
return maxV;
|
||||
}
|
||||
|
||||
|
||||
|
||||
//return the current simplex
|
||||
int b3VoronoiSimplexSolver::getSimplex(b3Vector3 *pBuf, b3Vector3 *qBuf, b3Vector3 *yBuf) const
|
||||
//return the current simplex
|
||||
int b3VoronoiSimplexSolver::getSimplex(b3Vector3* pBuf, b3Vector3* qBuf, b3Vector3* yBuf) const
|
||||
{
|
||||
int i;
|
||||
for (i=0;i<numVertices();i++)
|
||||
for (i = 0; i < numVertices(); i++)
|
||||
{
|
||||
yBuf[i] = m_simplexVectorW[i];
|
||||
pBuf[i] = m_simplexPointsP[i];
|
||||
@@ -277,20 +266,17 @@ int b3VoronoiSimplexSolver::getSimplex(b3Vector3 *pBuf, b3Vector3 *qBuf, b3Vecto
|
||||
return numVertices();
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
bool b3VoronoiSimplexSolver::inSimplex(const b3Vector3& w)
|
||||
{
|
||||
bool found = false;
|
||||
int i, numverts = numVertices();
|
||||
//b3Scalar maxV = b3Scalar(0.);
|
||||
|
||||
|
||||
//w is in the current (reduced) simplex
|
||||
for (i=0;i<numverts;i++)
|
||||
for (i = 0; i < numverts; i++)
|
||||
{
|
||||
#ifdef BT_USE_EQUAL_VERTEX_THRESHOLD
|
||||
if ( m_simplexVectorW[i].distance2(w) <= m_equalVertexThreshold)
|
||||
if (m_simplexVectorW[i].distance2(w) <= m_equalVertexThreshold)
|
||||
#else
|
||||
if (m_simplexVectorW[i] == w)
|
||||
#endif
|
||||
@@ -300,199 +286,190 @@ bool b3VoronoiSimplexSolver::inSimplex(const b3Vector3& w)
|
||||
//check in case lastW is already removed
|
||||
if (w == m_lastW)
|
||||
return true;
|
||||
|
||||
|
||||
return found;
|
||||
}
|
||||
|
||||
void b3VoronoiSimplexSolver::backup_closest(b3Vector3& v)
|
||||
void b3VoronoiSimplexSolver::backup_closest(b3Vector3& v)
|
||||
{
|
||||
v = m_cachedV;
|
||||
}
|
||||
|
||||
|
||||
bool b3VoronoiSimplexSolver::emptySimplex() const
|
||||
bool b3VoronoiSimplexSolver::emptySimplex() const
|
||||
{
|
||||
return (numVertices() == 0);
|
||||
|
||||
}
|
||||
|
||||
void b3VoronoiSimplexSolver::compute_points(b3Vector3& p1, b3Vector3& p2)
|
||||
void b3VoronoiSimplexSolver::compute_points(b3Vector3& p1, b3Vector3& p2)
|
||||
{
|
||||
updateClosestVectorAndPoints();
|
||||
p1 = m_cachedP1;
|
||||
p2 = m_cachedP2;
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
bool b3VoronoiSimplexSolver::closestPtPointTriangle(const b3Vector3& p, const b3Vector3& a, const b3Vector3& b, const b3Vector3& c,b3SubSimplexClosestResult& result)
|
||||
bool b3VoronoiSimplexSolver::closestPtPointTriangle(const b3Vector3& p, const b3Vector3& a, const b3Vector3& b, const b3Vector3& c, b3SubSimplexClosestResult& result)
|
||||
{
|
||||
result.m_usedVertices.reset();
|
||||
|
||||
// Check if P in vertex region outside A
|
||||
b3Vector3 ab = b - a;
|
||||
b3Vector3 ac = c - a;
|
||||
b3Vector3 ap = p - a;
|
||||
b3Scalar d1 = ab.dot(ap);
|
||||
b3Scalar d2 = ac.dot(ap);
|
||||
if (d1 <= b3Scalar(0.0) && d2 <= b3Scalar(0.0))
|
||||
// Check if P in vertex region outside A
|
||||
b3Vector3 ab = b - a;
|
||||
b3Vector3 ac = c - a;
|
||||
b3Vector3 ap = p - a;
|
||||
b3Scalar d1 = ab.dot(ap);
|
||||
b3Scalar d2 = ac.dot(ap);
|
||||
if (d1 <= b3Scalar(0.0) && d2 <= b3Scalar(0.0))
|
||||
{
|
||||
result.m_closestPointOnSimplex = a;
|
||||
result.m_usedVertices.usedVertexA = true;
|
||||
result.setBarycentricCoordinates(1,0,0);
|
||||
return true;// a; // barycentric coordinates (1,0,0)
|
||||
result.setBarycentricCoordinates(1, 0, 0);
|
||||
return true; // a; // barycentric coordinates (1,0,0)
|
||||
}
|
||||
|
||||
// Check if P in vertex region outside B
|
||||
b3Vector3 bp = p - b;
|
||||
b3Scalar d3 = ab.dot(bp);
|
||||
b3Scalar d4 = ac.dot(bp);
|
||||
if (d3 >= b3Scalar(0.0) && d4 <= d3)
|
||||
// Check if P in vertex region outside B
|
||||
b3Vector3 bp = p - b;
|
||||
b3Scalar d3 = ab.dot(bp);
|
||||
b3Scalar d4 = ac.dot(bp);
|
||||
if (d3 >= b3Scalar(0.0) && d4 <= d3)
|
||||
{
|
||||
result.m_closestPointOnSimplex = b;
|
||||
result.m_usedVertices.usedVertexB = true;
|
||||
result.setBarycentricCoordinates(0,1,0);
|
||||
result.setBarycentricCoordinates(0, 1, 0);
|
||||
|
||||
return true; // b; // barycentric coordinates (0,1,0)
|
||||
return true; // b; // barycentric coordinates (0,1,0)
|
||||
}
|
||||
// Check if P in edge region of AB, if so return projection of P onto AB
|
||||
b3Scalar vc = d1*d4 - d3*d2;
|
||||
if (vc <= b3Scalar(0.0) && d1 >= b3Scalar(0.0) && d3 <= b3Scalar(0.0)) {
|
||||
b3Scalar v = d1 / (d1 - d3);
|
||||
// Check if P in edge region of AB, if so return projection of P onto AB
|
||||
b3Scalar vc = d1 * d4 - d3 * d2;
|
||||
if (vc <= b3Scalar(0.0) && d1 >= b3Scalar(0.0) && d3 <= b3Scalar(0.0))
|
||||
{
|
||||
b3Scalar v = d1 / (d1 - d3);
|
||||
result.m_closestPointOnSimplex = a + v * ab;
|
||||
result.m_usedVertices.usedVertexA = true;
|
||||
result.m_usedVertices.usedVertexB = true;
|
||||
result.setBarycentricCoordinates(1-v,v,0);
|
||||
result.setBarycentricCoordinates(1 - v, v, 0);
|
||||
return true;
|
||||
//return a + v * ab; // barycentric coordinates (1-v,v,0)
|
||||
}
|
||||
//return a + v * ab; // barycentric coordinates (1-v,v,0)
|
||||
}
|
||||
|
||||
// Check if P in vertex region outside C
|
||||
b3Vector3 cp = p - c;
|
||||
b3Scalar d5 = ab.dot(cp);
|
||||
b3Scalar d6 = ac.dot(cp);
|
||||
if (d6 >= b3Scalar(0.0) && d5 <= d6)
|
||||
// Check if P in vertex region outside C
|
||||
b3Vector3 cp = p - c;
|
||||
b3Scalar d5 = ab.dot(cp);
|
||||
b3Scalar d6 = ac.dot(cp);
|
||||
if (d6 >= b3Scalar(0.0) && d5 <= d6)
|
||||
{
|
||||
result.m_closestPointOnSimplex = c;
|
||||
result.m_usedVertices.usedVertexC = true;
|
||||
result.setBarycentricCoordinates(0,0,1);
|
||||
return true;//c; // barycentric coordinates (0,0,1)
|
||||
result.setBarycentricCoordinates(0, 0, 1);
|
||||
return true; //c; // barycentric coordinates (0,0,1)
|
||||
}
|
||||
|
||||
// Check if P in edge region of AC, if so return projection of P onto AC
|
||||
b3Scalar vb = d5*d2 - d1*d6;
|
||||
if (vb <= b3Scalar(0.0) && d2 >= b3Scalar(0.0) && d6 <= b3Scalar(0.0)) {
|
||||
b3Scalar w = d2 / (d2 - d6);
|
||||
// Check if P in edge region of AC, if so return projection of P onto AC
|
||||
b3Scalar vb = d5 * d2 - d1 * d6;
|
||||
if (vb <= b3Scalar(0.0) && d2 >= b3Scalar(0.0) && d6 <= b3Scalar(0.0))
|
||||
{
|
||||
b3Scalar w = d2 / (d2 - d6);
|
||||
result.m_closestPointOnSimplex = a + w * ac;
|
||||
result.m_usedVertices.usedVertexA = true;
|
||||
result.m_usedVertices.usedVertexC = true;
|
||||
result.setBarycentricCoordinates(1-w,0,w);
|
||||
result.setBarycentricCoordinates(1 - w, 0, w);
|
||||
return true;
|
||||
//return a + w * ac; // barycentric coordinates (1-w,0,w)
|
||||
}
|
||||
//return a + w * ac; // barycentric coordinates (1-w,0,w)
|
||||
}
|
||||
|
||||
// Check if P in edge region of BC, if so return projection of P onto BC
|
||||
b3Scalar va = d3 * d6 - d5 * d4;
|
||||
if (va <= b3Scalar(0.0) && (d4 - d3) >= b3Scalar(0.0) && (d5 - d6) >= b3Scalar(0.0))
|
||||
{
|
||||
b3Scalar w = (d4 - d3) / ((d4 - d3) + (d5 - d6));
|
||||
|
||||
// Check if P in edge region of BC, if so return projection of P onto BC
|
||||
b3Scalar va = d3*d6 - d5*d4;
|
||||
if (va <= b3Scalar(0.0) && (d4 - d3) >= b3Scalar(0.0) && (d5 - d6) >= b3Scalar(0.0)) {
|
||||
b3Scalar w = (d4 - d3) / ((d4 - d3) + (d5 - d6));
|
||||
|
||||
result.m_closestPointOnSimplex = b + w * (c - b);
|
||||
result.m_usedVertices.usedVertexB = true;
|
||||
result.m_usedVertices.usedVertexC = true;
|
||||
result.setBarycentricCoordinates(0,1-w,w);
|
||||
return true;
|
||||
// return b + w * (c - b); // barycentric coordinates (0,1-w,w)
|
||||
}
|
||||
result.setBarycentricCoordinates(0, 1 - w, w);
|
||||
return true;
|
||||
// return b + w * (c - b); // barycentric coordinates (0,1-w,w)
|
||||
}
|
||||
|
||||
// P inside face region. Compute Q through its barycentric coordinates (u,v,w)
|
||||
b3Scalar denom = b3Scalar(1.0) / (va + vb + vc);
|
||||
b3Scalar v = vb * denom;
|
||||
b3Scalar w = vc * denom;
|
||||
|
||||
// P inside face region. Compute Q through its barycentric coordinates (u,v,w)
|
||||
b3Scalar denom = b3Scalar(1.0) / (va + vb + vc);
|
||||
b3Scalar v = vb * denom;
|
||||
b3Scalar w = vc * denom;
|
||||
|
||||
result.m_closestPointOnSimplex = a + ab * v + ac * w;
|
||||
result.m_usedVertices.usedVertexA = true;
|
||||
result.m_usedVertices.usedVertexB = true;
|
||||
result.m_usedVertices.usedVertexC = true;
|
||||
result.setBarycentricCoordinates(1-v-w,v,w);
|
||||
|
||||
result.setBarycentricCoordinates(1 - v - w, v, w);
|
||||
|
||||
return true;
|
||||
// return a + ab * v + ac * w; // = u*a + v*b + w*c, u = va * denom = b3Scalar(1.0) - v - w
|
||||
|
||||
// return a + ab * v + ac * w; // = u*a + v*b + w*c, u = va * denom = b3Scalar(1.0) - v - w
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
/// Test if point p and d lie on opposite sides of plane through abc
|
||||
int b3VoronoiSimplexSolver::pointOutsideOfPlane(const b3Vector3& p, const b3Vector3& a, const b3Vector3& b, const b3Vector3& c, const b3Vector3& d)
|
||||
{
|
||||
b3Vector3 normal = (b-a).cross(c-a);
|
||||
b3Vector3 normal = (b - a).cross(c - a);
|
||||
|
||||
b3Scalar signp = (p - a).dot(normal); // [AP AB AC]
|
||||
b3Scalar signd = (d - a).dot( normal); // [AD AB AC]
|
||||
b3Scalar signp = (p - a).dot(normal); // [AP AB AC]
|
||||
b3Scalar signd = (d - a).dot(normal); // [AD AB AC]
|
||||
|
||||
#ifdef B3_CATCH_DEGENERATE_TETRAHEDRON
|
||||
#ifdef BT_USE_DOUBLE_PRECISION
|
||||
if (signd * signd < (b3Scalar(1e-8) * b3Scalar(1e-8)))
|
||||
if (signd * signd < (b3Scalar(1e-8) * b3Scalar(1e-8)))
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
#else
|
||||
if (signd * signd < (b3Scalar(1e-4) * b3Scalar(1e-4)))
|
||||
{
|
||||
// printf("affine dependent/degenerate\n");//
|
||||
// printf("affine dependent/degenerate\n");//
|
||||
return -1;
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif
|
||||
// Points on opposite sides if expression signs are opposite
|
||||
return signp * signd < b3Scalar(0.);
|
||||
return signp * signd < b3Scalar(0.);
|
||||
}
|
||||
|
||||
|
||||
bool b3VoronoiSimplexSolver::closestPtPointTetrahedron(const b3Vector3& p, const b3Vector3& a, const b3Vector3& b, const b3Vector3& c, const b3Vector3& d, b3SubSimplexClosestResult& finalResult)
|
||||
bool b3VoronoiSimplexSolver::closestPtPointTetrahedron(const b3Vector3& p, const b3Vector3& a, const b3Vector3& b, const b3Vector3& c, const b3Vector3& d, b3SubSimplexClosestResult& finalResult)
|
||||
{
|
||||
b3SubSimplexClosestResult tempResult;
|
||||
|
||||
// Start out assuming point inside all halfspaces, so closest to itself
|
||||
// Start out assuming point inside all halfspaces, so closest to itself
|
||||
finalResult.m_closestPointOnSimplex = p;
|
||||
finalResult.m_usedVertices.reset();
|
||||
finalResult.m_usedVertices.usedVertexA = true;
|
||||
finalResult.m_usedVertices.usedVertexA = true;
|
||||
finalResult.m_usedVertices.usedVertexB = true;
|
||||
finalResult.m_usedVertices.usedVertexC = true;
|
||||
finalResult.m_usedVertices.usedVertexD = true;
|
||||
|
||||
int pointOutsideABC = pointOutsideOfPlane(p, a, b, c, d);
|
||||
int pointOutsideABC = pointOutsideOfPlane(p, a, b, c, d);
|
||||
int pointOutsideACD = pointOutsideOfPlane(p, a, c, d, b);
|
||||
int pointOutsideADB = pointOutsideOfPlane(p, a, d, b, c);
|
||||
int pointOutsideBDC = pointOutsideOfPlane(p, b, d, c, a);
|
||||
int pointOutsideADB = pointOutsideOfPlane(p, a, d, b, c);
|
||||
int pointOutsideBDC = pointOutsideOfPlane(p, b, d, c, a);
|
||||
|
||||
if (pointOutsideABC < 0 || pointOutsideACD < 0 || pointOutsideADB < 0 || pointOutsideBDC < 0)
|
||||
{
|
||||
finalResult.m_degenerate = true;
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!pointOutsideABC && !pointOutsideACD && !pointOutsideADB && !pointOutsideBDC)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
b3Scalar bestSqDist = FLT_MAX;
|
||||
// If point outside face abc then compute closest point on abc
|
||||
if (pointOutsideABC)
|
||||
if (pointOutsideABC < 0 || pointOutsideACD < 0 || pointOutsideADB < 0 || pointOutsideBDC < 0)
|
||||
{
|
||||
closestPtPointTriangle(p, a, b, c,tempResult);
|
||||
finalResult.m_degenerate = true;
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!pointOutsideABC && !pointOutsideACD && !pointOutsideADB && !pointOutsideBDC)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
b3Scalar bestSqDist = FLT_MAX;
|
||||
// If point outside face abc then compute closest point on abc
|
||||
if (pointOutsideABC)
|
||||
{
|
||||
closestPtPointTriangle(p, a, b, c, tempResult);
|
||||
b3Vector3 q = tempResult.m_closestPointOnSimplex;
|
||||
|
||||
b3Scalar sqDist = (q - p).dot( q - p);
|
||||
// Update best closest point if (squared) distance is less than current best
|
||||
if (sqDist < bestSqDist) {
|
||||
|
||||
b3Scalar sqDist = (q - p).dot(q - p);
|
||||
// Update best closest point if (squared) distance is less than current best
|
||||
if (sqDist < bestSqDist)
|
||||
{
|
||||
bestSqDist = sqDist;
|
||||
finalResult.m_closestPointOnSimplex = q;
|
||||
//convert result bitmask!
|
||||
@@ -501,25 +478,22 @@ bool b3VoronoiSimplexSolver::closestPtPointTetrahedron(const b3Vector3& p, const
|
||||
finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexB;
|
||||
finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexC;
|
||||
finalResult.setBarycentricCoordinates(
|
||||
tempResult.m_barycentricCoords[VERTA],
|
||||
tempResult.m_barycentricCoords[VERTB],
|
||||
tempResult.m_barycentricCoords[VERTC],
|
||||
0
|
||||
);
|
||||
|
||||
tempResult.m_barycentricCoords[VERTA],
|
||||
tempResult.m_barycentricCoords[VERTB],
|
||||
tempResult.m_barycentricCoords[VERTC],
|
||||
0);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
// Repeat test for face acd
|
||||
if (pointOutsideACD)
|
||||
if (pointOutsideACD)
|
||||
{
|
||||
closestPtPointTriangle(p, a, c, d,tempResult);
|
||||
closestPtPointTriangle(p, a, c, d, tempResult);
|
||||
b3Vector3 q = tempResult.m_closestPointOnSimplex;
|
||||
//convert result bitmask!
|
||||
|
||||
b3Scalar sqDist = (q - p).dot( q - p);
|
||||
if (sqDist < bestSqDist)
|
||||
b3Scalar sqDist = (q - p).dot(q - p);
|
||||
if (sqDist < bestSqDist)
|
||||
{
|
||||
bestSqDist = sqDist;
|
||||
finalResult.m_closestPointOnSimplex = q;
|
||||
@@ -529,52 +503,46 @@ bool b3VoronoiSimplexSolver::closestPtPointTetrahedron(const b3Vector3& p, const
|
||||
finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexB;
|
||||
finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexC;
|
||||
finalResult.setBarycentricCoordinates(
|
||||
tempResult.m_barycentricCoords[VERTA],
|
||||
0,
|
||||
tempResult.m_barycentricCoords[VERTB],
|
||||
tempResult.m_barycentricCoords[VERTC]
|
||||
);
|
||||
|
||||
tempResult.m_barycentricCoords[VERTA],
|
||||
0,
|
||||
tempResult.m_barycentricCoords[VERTB],
|
||||
tempResult.m_barycentricCoords[VERTC]);
|
||||
}
|
||||
}
|
||||
// Repeat test for face adb
|
||||
}
|
||||
// Repeat test for face adb
|
||||
|
||||
|
||||
if (pointOutsideADB)
|
||||
{
|
||||
closestPtPointTriangle(p, a, d, b,tempResult);
|
||||
closestPtPointTriangle(p, a, d, b, tempResult);
|
||||
b3Vector3 q = tempResult.m_closestPointOnSimplex;
|
||||
//convert result bitmask!
|
||||
|
||||
b3Scalar sqDist = (q - p).dot( q - p);
|
||||
if (sqDist < bestSqDist)
|
||||
b3Scalar sqDist = (q - p).dot(q - p);
|
||||
if (sqDist < bestSqDist)
|
||||
{
|
||||
bestSqDist = sqDist;
|
||||
finalResult.m_closestPointOnSimplex = q;
|
||||
finalResult.m_usedVertices.reset();
|
||||
finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA;
|
||||
finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexC;
|
||||
|
||||
|
||||
finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexB;
|
||||
finalResult.setBarycentricCoordinates(
|
||||
tempResult.m_barycentricCoords[VERTA],
|
||||
tempResult.m_barycentricCoords[VERTC],
|
||||
0,
|
||||
tempResult.m_barycentricCoords[VERTB]
|
||||
);
|
||||
|
||||
tempResult.m_barycentricCoords[VERTA],
|
||||
tempResult.m_barycentricCoords[VERTC],
|
||||
0,
|
||||
tempResult.m_barycentricCoords[VERTB]);
|
||||
}
|
||||
}
|
||||
// Repeat test for face bdc
|
||||
|
||||
}
|
||||
// Repeat test for face bdc
|
||||
|
||||
if (pointOutsideBDC)
|
||||
{
|
||||
closestPtPointTriangle(p, b, d, c,tempResult);
|
||||
closestPtPointTriangle(p, b, d, c, tempResult);
|
||||
b3Vector3 q = tempResult.m_closestPointOnSimplex;
|
||||
//convert result bitmask!
|
||||
b3Scalar sqDist = (q - p).dot( q - p);
|
||||
if (sqDist < bestSqDist)
|
||||
b3Scalar sqDist = (q - p).dot(q - p);
|
||||
if (sqDist < bestSqDist)
|
||||
{
|
||||
bestSqDist = sqDist;
|
||||
finalResult.m_closestPointOnSimplex = q;
|
||||
@@ -585,25 +553,22 @@ bool b3VoronoiSimplexSolver::closestPtPointTetrahedron(const b3Vector3& p, const
|
||||
finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexB;
|
||||
|
||||
finalResult.setBarycentricCoordinates(
|
||||
0,
|
||||
tempResult.m_barycentricCoords[VERTA],
|
||||
tempResult.m_barycentricCoords[VERTC],
|
||||
tempResult.m_barycentricCoords[VERTB]
|
||||
);
|
||||
|
||||
0,
|
||||
tempResult.m_barycentricCoords[VERTA],
|
||||
tempResult.m_barycentricCoords[VERTC],
|
||||
tempResult.m_barycentricCoords[VERTB]);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//help! we ended up full !
|
||||
|
||||
|
||||
if (finalResult.m_usedVertices.usedVertexA &&
|
||||
finalResult.m_usedVertices.usedVertexB &&
|
||||
finalResult.m_usedVertices.usedVertexC &&
|
||||
finalResult.m_usedVertices.usedVertexD)
|
||||
finalResult.m_usedVertices.usedVertexD)
|
||||
{
|
||||
return true;
|
||||
}
|
||||
|
||||
return true;
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user