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); }); }