double ompl::base::SO3StateSpace::distance(const State *state1, const State *state2) const
{
BOOST_ASSERT_MSG(satisfiesBounds(state1) && satisfiesBounds(state2),
"The states passed to SO3StateSpace::distance are not within bounds. Call "
"SO3StateSpace::enforceBounds() in, e.g., ompl::control::ODESolver::PostPropagationEvent, "
"ompl::control::StatePropagator, or ompl::base::StateValidityChecker");
return arcLength(state1, state2);
}
double ompl::base::SO2StateSpace::distance(const State *state1, const State *state2) const
{
// assuming the states 1 & 2 are within bounds
double d = fabs(state1->as<StateType>()->value - state2->as<StateType>()->value);
BOOST_ASSERT_MSG(satisfiesBounds(state1) && satisfiesBounds(state2),
"The states passed to SO2StateSpace::distance are not within bounds. Call "
"SO2StateSpace::enforceBounds() in, e.g., ompl::control::ODESolver::PostPropagationEvent, "
"ompl::control::StatePropagator, or ompl::base::StateValidityChecker");
return (d > boost::math::constants::pi<double>()) ? 2.0 * boost::math::constants::pi<double>() - d : d;
}
bool ompl::base::SpaceInformation::searchValidNearby(State *state, const State *near, double distance, unsigned int attempts) const
{
if (satisfiesBounds(near) && isValid(near))
{
if (state != near)
copyState(state, near);
return true;
}
else
{
// try to find a valid state nearby
UniformValidStateSampler *uvss = new UniformValidStateSampler(this);
uvss->setNrAttempts(attempts);
return searchValidNearby(ValidStateSamplerPtr(uvss), state, near, distance);
}
}
bool ompl::base::SpaceInformation::searchValidNearby(const ValidStateSamplerPtr &sampler, State *state, const State *near, double distance) const
{
if (state != near)
copyState(state, near);
// fix bounds, if needed
if (!satisfiesBounds(state))
enforceBounds(state);
bool result = isValid(state);
if (!result)
{
// try to find a valid state nearby
State *temp = cloneState(state);
result = sampler->sampleNear(state, temp, distance);
freeState(temp);
}
return result;
}