35 #include "KoulesConfig.h" 36 #include "KoulesStateSpace.h" 37 #include "KoulesGoal.h" 42 const double* v = st->
as<KoulesStateSpace::StateType>()->values;
43 auto space =
si_->getStateSpace()->as<KoulesStateSpace>();
44 std::size_t numKoules = (space->getDimension() - 5) / 4, liveKoules = numKoules;
45 double minDist = sideLength;
47 for (std::size_t i = 1, j = 5; i <= numKoules; ++i, j += 4)
49 if (space->isDead(st, i))
53 minX = std::min(v[j ], sideLength - v[j ]);
54 minY = std::min(v[j + 1], sideLength - v[j + 1]);
55 minDist = std::min(minDist, std::min(minX, minY) - kouleRadius +
threshold_);
58 if (minDist < 0 || liveKoules == 0)
60 return .5 * sideLength * (double) liveKoules + minDist;
65 double* v = st->
as<KoulesStateSpace::StateType>()->values;
66 std::size_t dim =
si_->getStateDimension();
67 stateSampler_->sampleUniform(st);
68 for (std::size_t i = 5; i < dim; i += 4)
virtual void sampleGoal(ompl::base::State *st) const
Sample a state in the goal region.
bool uniformBool()
Generate a random boolean.
const T * as() const
Cast this instance to a desired type.
double uniformReal(double lower_bound, double upper_bound)
Generate a random real within given bounds: [lower_bound, upper_bound)
Definition of an abstract state.
SpaceInformationPtr si_
The space information for this goal.
virtual double distanceGoal(const ompl::base::State *st) const
Compute the distance to the goal (heuristic). This function is the one used in computing the distance...
double threshold_
The maximum distance that is allowed to the goal. By default, this is initialized to the minimum epsi...