Esempio n. 1
0
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.");
    }
}
Esempio n. 2
0
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;
}