37 #ifndef DEMOS_KOULES_PROJECTION_
38 #define DEMOS_KOULES_PROJECTION_
40 #include <ompl/base/spaces/RealVectorStateSpace.h>
50 if (numDimensions_ > n)
52 else if (numDimensions_ < 3)
58 return numDimensions_;
70 unsigned int numKoules = (numDimensions_ - 3) / 2;
74 for (
unsigned int i = 0; i < numKoules; ++i)
76 projection[2 * i + 3] = x[4 * i + 5];
77 projection[2 * i + 4] = x[4 * i + 6];
81 unsigned int numDimensions_;
virtual void project(const ompl::base::State *state, ompl::base::EuclideanProjection &projection) const
Compute the projection as an array of double values.
virtual void defaultCellSizes(void)
Set the default cell dimensions for this projection. The default implementation of this function is e...
std::vector< double > cellSizes_
The size of a cell, in every dimension of the projected space, in the implicitly defined integer grid...
virtual unsigned int getDimension(void) const =0
Get the dimension of the space (not the dimension of the surrounding ambient space) ...
const T * as(void) const
Cast this instance to a desired type.
const StateSpace * space_
The state space this projection operates on.
Representation of a space in which planning can be performed. Topology specific sampling, interpolation and distance are defined.
boost::numeric::ublas::vector< double > EuclideanProjection
The datatype for state projections. This class contains a real vector.
Definition of an abstract state.
The definition of a state in Rn
virtual unsigned int getDimension(void) const
Return the dimension of the projection defined by this evaluator.
Abstract definition for a class computing projections to Rn. Implicit integer grids are imposed on th...