ompl::base::RealVectorOrthogonalProjectionEvaluator::RealVectorOrthogonalProjectionEvaluator(
const StateSpacePtr &space, const std::vector<double> &cellSizes, std::vector<unsigned int> components)
: ProjectionEvaluator(space), components_(std::move(components))
{
checkSpaceType(space_);
setCellSizes(cellSizes);
copyBounds();
}
ompl::base::RealVectorLinearProjectionEvaluator::RealVectorLinearProjectionEvaluator(
const StateSpacePtr &space, const std::vector<double> &cellSizes, const ProjectionMatrix::Matrix &projection)
: ProjectionEvaluator(space)
{
checkSpaceType(space_);
projection_.mat = projection;
setCellSizes(cellSizes);
}
ompl::base::RealVectorIdentityProjectionEvaluator::RealVectorIdentityProjectionEvaluator(
const StateSpacePtr &space, const std::vector<double> &cellSizes)
: ProjectionEvaluator(space)
{
checkSpaceType(space_);
setCellSizes(cellSizes);
copyBounds();
}
void ompl::base::ProjectionEvaluator::mulCellSizes(double factor)
{
if (cellSizes_.size() == getDimension())
{
std::vector<double> c(cellSizes_.size());
for (std::size_t i = 0; i < cellSizes_.size(); ++i)
c[i] = cellSizes_[i] * factor;
setCellSizes(c);
}
}
void ompl::base::ProjectionEvaluator::setCellSizes(unsigned int dim, double cellSize)
{
if (cellSizes_.size() >= dim)
OMPL_ERROR("Dimension %u is not defined for projection evaluator", dim);
else
{
std::vector<double> c = cellSizes_;
c[dim] = cellSize;
setCellSizes(c);
}
}
void ompl::base::ProjectionEvaluator::setup()
{
if (defaultCellSizes_)
defaultCellSizes();
if ((cellSizes_.size() == 0 && getDimension() > 0) || cellSizesWereInferred_)
inferCellSizes();
checkCellSizes();
checkBounds();
unsigned int dim = getDimension();
for (unsigned int i = 0; i < dim; ++i)
params_.declareParam<double>("cellsize." + std::to_string(i),
[this, i](double cellsize)
{
setCellSizes(i, cellsize);
},
[this, i]
{
return getCellSizes(i);
});
}
