void ompl::geometric::PathSimplifier::simplify(PathGeometric &path, const base::PlannerTerminationCondition &ptc)
{
if (path.getStateCount() < 3)
return;
// try a randomized step of connecting vertices
bool tryMore = false;
if (ptc == false)
tryMore = reduceVertices(path);
// try to collapse close-by vertices
if (ptc == false)
collapseCloseVertices(path);
// try to reduce verices some more, if there is any point in doing so
int times = 0;
while (tryMore && ptc == false && ++times <= 5)
tryMore = reduceVertices(path);
// if the space is metric, we can do some additional smoothing
if(si_->getStateSpace()->isMetricSpace())
{
bool tryMore = true;
unsigned int times = 0;
do
{
bool shortcut = shortcutPath(path); // split path segments, not just vertices
bool better_goal = gsr_ ? findBetterGoal(path, ptc) : false; // Try to connect the path to a closer goal
tryMore = shortcut || better_goal;
} while(ptc == false && tryMore && ++times <= 5);
// smooth the path with BSpline interpolation
if(ptc == false)
smoothBSpline(path, 3, path.length()/100.0);
// we always run this if the metric-space algorithms were run. In non-metric spaces this does not work.
const std::pair<bool, bool> &p = path.checkAndRepair(magic::MAX_VALID_SAMPLE_ATTEMPTS);
if (!p.second)
OMPL_WARN("Solution path may slightly touch on an invalid region of the state space");
else
if (!p.first)
OMPL_DEBUG("The solution path was slightly touching on an invalid region of the state space, but it was successfully fixed.");
}
}
bool SpuVoronoiSimplexSolver::updateClosestVectorAndPoints()
{
if (m_needsUpdate)
{
m_cachedBC.reset();
m_needsUpdate = false;
switch (numVertices())
{
case 0:
m_cachedValidClosest = false;
break;
case 1:
{
m_cachedP1 = m_simplexPointsP[0];
m_cachedP2 = m_simplexPointsQ[0];
m_cachedV = m_cachedP1-m_cachedP2; //== m_simplexVectorW[0]
m_cachedBC.reset();
m_cachedBC.setBarycentricCoordinates(btScalar(1.),btScalar(0.),btScalar(0.),btScalar(0.));
m_cachedValidClosest = m_cachedBC.isValid();
break;
};
case 2:
{
//closest point origin from line segment
const btVector3& from = m_simplexVectorW[0];
const btVector3& to = m_simplexVectorW[1];
btVector3 nearest;
btVector3 p (btScalar(0.),btScalar(0.),btScalar(0.));
btVector3 diff = p - from;
btVector3 v = to - from;
btScalar t = v.dot(diff);
if (t > 0) {
btScalar 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
{
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_cachedValidClosest = m_cachedBC.isValid();
break;
}
case 3:
{
//closest point origin from triangle
btVector3 p (btScalar(0.),btScalar(0.),btScalar(0.));
const btVector3& a = m_simplexVectorW[0];
const btVector3& b = m_simplexVectorW[1];
const btVector3& 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_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_cachedV = m_cachedP1-m_cachedP2;
reduceVertices (m_cachedBC.m_usedVertices);
m_cachedValidClosest = m_cachedBC.isValid();
break;
}
case 4:
{
btVector3 p (btScalar(0.),btScalar(0.),btScalar(0.));
const btVector3& a = m_simplexVectorW[0];
const btVector3& b = m_simplexVectorW[1];
const btVector3& c = m_simplexVectorW[2];
const btVector3& d = m_simplexVectorW[3];
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_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_cachedV = m_cachedP1-m_cachedP2;
reduceVertices (m_cachedBC.m_usedVertices);
} else
{
// printf("sub distance got penetration\n");
if (m_cachedBC.m_degenerate)
{
m_cachedValidClosest = false;
} else
{
m_cachedValidClosest = true;
//degenerate case == false, penetration = true + zero
m_cachedV.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
}
break;
}
m_cachedValidClosest = m_cachedBC.isValid();
//closest point origin from tetrahedron
break;
}
default:
{
m_cachedValidClosest = false;
}
};
}
return m_cachedValidClosest;
}
bool PfxSimplexSolver::closest(PfxVector3& v)
{
bool ret = false;
bc.reset();
switch(numVertices) {
case 0:
ret = false;
break;
case 1:
{
PfxVector3 tmpP = P[0];
PfxVector3 tmpQ = Q[0];
v = tmpP-tmpQ;
bc.reset();
bc.setBarycentricCoordinates(1.0f,0.0f,0.0f,0.0f);
ret = bc.isValid();
}
break;
case 2:
{
PfxVector3 dir = W[1] - W[0];
PfxFloat t = dot(-W[0],dir) / dot(dir,dir);
if(t < 0.0f) t = 0.0f;
if(t > 1.0f) t = 1.0f;
bc.setBarycentricCoordinates(1-t,t,0.0f,0.0f);
PfxVector3 tmpP = P[0] + t * (P[1] - P[0]);
PfxVector3 tmpQ = Q[0] + t * (Q[1] - Q[0]);
v = tmpP - tmpQ;
reduceVertices();
ret = bc.isValid();
break;
}
case 3:
{
const PfxVector3& a = W[0];
const PfxVector3& b = W[1];
const PfxVector3& c = W[2];
closestPointTriangleFromOrigin(a,b,c,bc);
PfxVector3 tmpP = P[0] * bc.barycentricCoords[0] +
P[1] * bc.barycentricCoords[1] +
P[2] * bc.barycentricCoords[2];
PfxVector3 tmpQ = Q[0] * bc.barycentricCoords[0] +
Q[1] * bc.barycentricCoords[1] +
Q[2] * bc.barycentricCoords[2];
v = tmpP-tmpQ;
reduceVertices();
ret = bc.isValid();
break;
}
case 4:
{
const PfxVector3& a = W[0];
const PfxVector3& b = W[1];
const PfxVector3& c = W[2];
const PfxVector3& d = W[3];
if(closestPointTetrahedronFromOrigin(a,b,c,d,bc)) {
PfxVector3 tmpP = P[0] * bc.barycentricCoords[0] +
P[1] * bc.barycentricCoords[1] +
P[2] * bc.barycentricCoords[2] +
P[3] * bc.barycentricCoords[3];
PfxVector3 tmpQ = Q[0] * bc.barycentricCoords[0] +
Q[1] * bc.barycentricCoords[1] +
Q[2] * bc.barycentricCoords[2] +
Q[3] * bc.barycentricCoords[3];
v = tmpP-tmpQ;
reduceVertices();
ret = bc.isValid();
} else {
// 原点が内部に存在→交差している
ret = true;
v = PfxVector3(0.0f);
}
break;
}
};
return ret;
}