void ConvexFeature::testEdge(ConvexFeature* cf,const Point3F& s1, const Point3F& e1, CollisionList* cList, F32 tol)
{
F32 tolSquared = tol*tol;
// Test edges against edges
const Edge* edge = mEdgeList.begin();
const Edge* end = mEdgeList.end();
for (; edge != end; edge++) {
if (cList->getCount() >= CollisionList::MaxCollisions)
return;
const Point3F& s2 = mVertexList[edge->vertex[0]];
const Point3F& e2 = mVertexList[edge->vertex[1]];
// Get the distance and closest points
Point3F i1,i2;
F32 distance = sqrDistanceEdges(s1, e1, s2, e2, &i1, &i2);
if (distance > tolSquared)
continue;
distance = mSqrt(distance);
// Need to figure out how to orient the collision normal.
// The current test involves checking to see whether the collision
// points are contained within the convex volumes, which is slow.
if (inVolume(i1) || cf->inVolume(i2))
distance = -distance;
// Contact normal
VectorF normal = i1 - i2;
if ( mIsZero( distance ) )
normal.zero();
else
normal *= 1 / distance;
// Return a collision
Collision& info = cList->increment();
info.point = i1;
info.normal = normal;
info.distance = distance;
info.material = material;
info.object = object;
}
}
bool ompl::geometric::BallTreeRRTstar::solve(const base::PlannerTerminationCondition &ptc)
{
checkValidity();
base::Goal *goal = pdef_->getGoal().get();
base::GoalSampleableRegion *goal_s = dynamic_cast<base::GoalSampleableRegion*>(goal);
if (!goal)
{
msg_.error("Goal undefined");
return false;
}
while (const base::State *st = pis_.nextStart())
{
Motion *motion = new Motion(si_, rO_);
si_->copyState(motion->state, st);
addMotion(motion);
}
if (nn_->size() == 0)
{
msg_.error("There are no valid initial states!");
return false;
}
if (!sampler_)
sampler_ = si_->allocStateSampler();
msg_.inform("Starting with %u states", nn_->size());
Motion *solution = NULL;
Motion *approximation = NULL;
double approximatedist = std::numeric_limits<double>::infinity();
bool sufficientlyShort = false;
Motion *rmotion = new Motion(si_, rO_);
Motion *toTrim = NULL;
base::State *rstate = rmotion->state;
base::State *xstate = si_->allocState();
base::State *tstate = si_->allocState();
std::vector<Motion*> solCheck;
std::vector<Motion*> nbh;
std::vector<double> dists;
std::vector<int> valid;
long unsigned int rewireTest = 0;
std::pair<base::State*,double> lastValid(tstate, 0.0);
while (ptc() == false)
{
bool rejected = false;
/* sample until a state not within any of the existing volumes is found */
do
{
/* sample random state (with goal biasing) */
if (goal_s && rng_.uniform01() < goalBias_ && goal_s->canSample())
goal_s->sampleGoal(rstate);
else
sampler_->sampleUniform(rstate);
/* check to see if it is inside an existing volume */
if (inVolume(rstate))
{
rejected = true;
/* see if the state is valid */
if(!si_->isValid(rstate))
{
/* if it's not, reduce the size of the nearest volume to the distance
between its center and the rejected state */
toTrim = nn_->nearest(rmotion);
double newRad = si_->distance(toTrim->state, rstate);
if (newRad < toTrim->volRadius)
toTrim->volRadius = newRad;
}
}
else
rejected = false;
}
while (rejected);
/* find closest state in the tree */
Motion *nmotion = nn_->nearest(rmotion);
base::State *dstate = rstate;
/* find state to add */
double d = si_->distance(nmotion->state, rstate);
if (d > maxDistance_)
{
si_->getStateSpace()->interpolate(nmotion->state, rstate, maxDistance_ / d, xstate);
dstate = xstate;
}
if (si_->checkMotion(nmotion->state, dstate, lastValid))
{
/* create a motion */
double distN = si_->distance(dstate, nmotion->state);
Motion *motion = new Motion(si_, rO_);
si_->copyState(motion->state, dstate);
motion->parent = nmotion;
motion->cost = nmotion->cost + distN;
/* find nearby neighbors */
double r = std::min(ballRadiusConst_ * (sqrt(log((double)(1 + nn_->size())) / ((double)(nn_->size())))),
ballRadiusMax_);
nn_->nearestR(motion, r, nbh);
rewireTest += nbh.size();
// cache for distance computations
dists.resize(nbh.size());
// cache for motion validity
valid.resize(nbh.size());
std::fill(valid.begin(), valid.end(), 0);
if (delayCC_)
{
// calculate all costs and distances
for (unsigned int i = 0 ; i < nbh.size() ; ++i)
if (nbh[i] != nmotion)
{
double c = nbh[i]->cost + si_->distance(nbh[i]->state, dstate);
nbh[i]->cost = c;
}
// sort the nodes
std::sort(nbh.begin(), nbh.end(), compareMotion);
for (unsigned int i = 0 ; i < nbh.size() ; ++i)
if (nbh[i] != nmotion)
{
dists[i] = si_->distance(nbh[i]->state, dstate);
nbh[i]->cost -= dists[i];
}
// collision check until a valid motion is found
for (unsigned int i = 0 ; i < nbh.size() ; ++i)
if (nbh[i] != nmotion)
{
dists[i] = si_->distance(nbh[i]->state, dstate);
double c = nbh[i]->cost + dists[i];
if (c < motion->cost)
{
if (si_->checkMotion(nbh[i]->state, dstate, lastValid))
{
motion->cost = c;
motion->parent = nbh[i];
valid[i] = 1;
break;
}
else
{
valid[i] = -1;
/* if a collision is found, trim radius to distance from motion to last valid state */
double nR = si_->distance(nbh[i]->state, lastValid.first);
if (nR < nbh[i]->volRadius)
nbh[i]->volRadius = nR;
}
}
}
else
{
valid[i] = 1;
dists[i] = distN;
break;
}
}
else{
/* find which one we connect the new state to*/
for (unsigned int i = 0 ; i < nbh.size() ; ++i)
if (nbh[i] != nmotion)
{
dists[i] = si_->distance(nbh[i]->state, dstate);
double c = nbh[i]->cost + dists[i];
if (c < motion->cost)
{
if (si_->checkMotion(nbh[i]->state, dstate, lastValid))
{
motion->cost = c;
motion->parent = nbh[i];
valid[i] = 1;
}
else
{
valid[i] = -1;
/* if a collision is found, trim radius to distance from motion to last valid state */
double newR = si_->distance(nbh[i]->state, lastValid.first);
if (newR < nbh[i]->volRadius)
nbh[i]->volRadius = newR;
}
}
}
else
{
valid[i] = 1;
dists[i] = distN;
}
}
/* add motion to tree */
addMotion(motion);
motion->parent->children.push_back(motion);
solCheck.resize(1);
solCheck[0] = motion;
/* rewire tree if needed */
for (unsigned int i = 0 ; i < nbh.size() ; ++i)
if (nbh[i] != motion->parent)
{
double c = motion->cost + dists[i];
if (c < nbh[i]->cost)
{
bool v = false;
if (valid[i] == 0)
{
if(!si_->checkMotion(nbh[i]->state, dstate, lastValid))
{
/* if a collision is found, trim radius to distance from motion to last valid state */
double R = si_->distance(nbh[i]->state, lastValid.first);
if (R < nbh[i]->volRadius)
nbh[i]->volRadius = R;
}
else
{
v = true;
}
}
if (valid[i] == 1)
v = true;
if (v)
{
// Remove this node from its parent list
removeFromParent (nbh[i]);
double delta = c - nbh[i]->cost;
nbh[i]->parent = motion;
nbh[i]->cost = c;
nbh[i]->parent->children.push_back(nbh[i]);
solCheck.push_back(nbh[i]);
// Update the costs of the node's children
updateChildCosts(nbh[i], delta);
}
}
}
// check if we found a solution
for (unsigned int i = 0 ; i < solCheck.size() ; ++i)
{
double dist = 0.0;
bool solved = goal->isSatisfied(solCheck[i]->state, &dist);
sufficientlyShort = solved ? goal->isPathLengthSatisfied(solCheck[i]->cost) : false;
if (solved)
{
if (sufficientlyShort)
{
solution = solCheck[i];
break;
}
else if (!solution || (solCheck[i]->cost < solution->cost))
{
solution = solCheck[i];
}
}
else if (!solution && dist < approximatedist)
{
approximation = solCheck[i];
approximatedist = dist;
}
}
// terminate if a sufficient solution is found
if (solution && sufficientlyShort)
break;
}
else
{
/* if a collision is found, trim radius to distance from motion to last valid state */
toTrim = nn_->nearest(nmotion);
double newRadius = si_->distance(toTrim->state, lastValid.first);
if (newRadius < toTrim->volRadius)
toTrim->volRadius = newRadius;
}
}
double solutionCost;
bool approximate = (solution == NULL);
bool addedSolution = false;
if (approximate)
{
solution = approximation;
solutionCost = approximatedist;
}
else
solutionCost = solution->cost;
if (solution != NULL)
{
// construct the solution path
std::vector<Motion*> mpath;
while (solution != NULL)
{
mpath.push_back(solution);
solution = solution->parent;
}
// set the solution path
PathGeometric *path = new PathGeometric(si_);
for (int i = mpath.size() - 1 ; i >= 0 ; --i)
path->append(mpath[i]->state);
goal->addSolutionPath(base::PathPtr(path), approximate, solutionCost);
addedSolution = true;
}
si_->freeState(xstate);
if (rmotion->state)
si_->freeState(rmotion->state);
delete rmotion;
msg_.inform("Created %u states. Checked %lu rewire options.", nn_->size(), rewireTest);
return addedSolution;
}
