00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include "ompl/base/spaces/RealVectorStateProjections.h"
00038 #include "ompl/util/Exception.h"
00039 #include "ompl/tools/config/MagicConstants.h"
00040 #include <cstring>
00041
00043 namespace ompl
00044 {
00045 namespace base
00046 {
00047 static inline void checkSpaceType(const StateSpace *m)
00048 {
00049 if (!dynamic_cast<const RealVectorStateSpace*>(m))
00050 throw Exception("Expected real vector state space for projection");
00051 }
00052 }
00053 }
00055
00056 ompl::base::RealVectorLinearProjectionEvaluator::RealVectorLinearProjectionEvaluator(const StateSpace *space, const std::vector<double> &cellSizes,
00057 const ProjectionMatrix::Matrix &projection) :
00058 ProjectionEvaluator(space)
00059 {
00060 checkSpaceType(space_);
00061 projection_.mat = projection;
00062 setCellSizes(cellSizes);
00063 }
00064
00065 ompl::base::RealVectorLinearProjectionEvaluator::RealVectorLinearProjectionEvaluator(const StateSpacePtr &space, const std::vector<double> &cellSizes,
00066 const ProjectionMatrix::Matrix &projection) :
00067 ProjectionEvaluator(space)
00068 {
00069 checkSpaceType(space_);
00070 projection_.mat = projection;
00071 setCellSizes(cellSizes);
00072 }
00073
00074 ompl::base::RealVectorLinearProjectionEvaluator::RealVectorLinearProjectionEvaluator(const StateSpace *space,
00075 const ProjectionMatrix::Matrix &projection) :
00076 ProjectionEvaluator(space)
00077 {
00078 checkSpaceType(space_);
00079 projection_.mat = projection;
00080 }
00081
00082 ompl::base::RealVectorLinearProjectionEvaluator::RealVectorLinearProjectionEvaluator(const StateSpacePtr &space,
00083 const ProjectionMatrix::Matrix &projection) :
00084 ProjectionEvaluator(space)
00085 {
00086 checkSpaceType(space_);
00087 projection_.mat = projection;
00088 }
00089
00090 ompl::base::RealVectorOrthogonalProjectionEvaluator::RealVectorOrthogonalProjectionEvaluator(const StateSpace *space, const std::vector<double> &cellSizes,
00091 const std::vector<unsigned int> &components) :
00092 ProjectionEvaluator(space), components_(components)
00093 {
00094 checkSpaceType(space_);
00095 setCellSizes(cellSizes);
00096 }
00097
00098 ompl::base::RealVectorOrthogonalProjectionEvaluator::RealVectorOrthogonalProjectionEvaluator(const StateSpacePtr &space, const std::vector<double> &cellSizes,
00099 const std::vector<unsigned int> &components) :
00100 ProjectionEvaluator(space), components_(components)
00101 {
00102 checkSpaceType(space_);
00103 setCellSizes(cellSizes);
00104 }
00105
00106 ompl::base::RealVectorOrthogonalProjectionEvaluator::RealVectorOrthogonalProjectionEvaluator(const StateSpace *space, const std::vector<unsigned int> &components) :
00107 ProjectionEvaluator(space), components_(components)
00108 {
00109 checkSpaceType(space_);
00110 }
00111
00112 ompl::base::RealVectorOrthogonalProjectionEvaluator::RealVectorOrthogonalProjectionEvaluator(const StateSpacePtr &space, const std::vector<unsigned int> &components) :
00113 ProjectionEvaluator(space), components_(components)
00114 {
00115 checkSpaceType(space_);
00116 }
00117
00118 void ompl::base::RealVectorOrthogonalProjectionEvaluator::defaultCellSizes(void)
00119 {
00120 const RealVectorBounds &bounds = space_->as<RealVectorStateSpace>()->getBounds();
00121 cellSizes_.resize(components_.size());
00122 for (unsigned int i = 0 ; i < cellSizes_.size() ; ++i)
00123 cellSizes_[i] = (bounds.high[components_[i]] - bounds.low[components_[i]]) / magic::PROJECTION_DIMENSION_SPLITS;
00124 }
00125
00126 unsigned int ompl::base::RealVectorLinearProjectionEvaluator::getDimension(void) const
00127 {
00128 return projection_.mat.size1();
00129 }
00130
00131 void ompl::base::RealVectorLinearProjectionEvaluator::project(const State *state, EuclideanProjection &projection) const
00132 {
00133 projection_.project(state->as<RealVectorStateSpace::StateType>()->values, projection);
00134 }
00135
00136 unsigned int ompl::base::RealVectorOrthogonalProjectionEvaluator::getDimension(void) const
00137 {
00138 return components_.size();
00139 }
00140
00141 void ompl::base::RealVectorOrthogonalProjectionEvaluator::project(const State *state, EuclideanProjection &projection) const
00142 {
00143 for (unsigned int i = 0 ; i < components_.size() ; ++i)
00144 projection(i) = state->as<RealVectorStateSpace::StateType>()->values[components_[i]];
00145 }
00146
00147 ompl::base::RealVectorIdentityProjectionEvaluator::RealVectorIdentityProjectionEvaluator(const StateSpace *space, const std::vector<double> &cellSizes) :
00148 ProjectionEvaluator(space)
00149 {
00150 checkSpaceType(space_);
00151 setCellSizes(cellSizes);
00152 }
00153
00154 ompl::base::RealVectorIdentityProjectionEvaluator::RealVectorIdentityProjectionEvaluator(const StateSpace *space) :
00155 ProjectionEvaluator(space)
00156 {
00157 checkSpaceType(space_);
00158 }
00159
00160 ompl::base::RealVectorIdentityProjectionEvaluator::RealVectorIdentityProjectionEvaluator(const StateSpacePtr &space, const std::vector<double> &cellSizes) :
00161 ProjectionEvaluator(space)
00162 {
00163 checkSpaceType(space_);
00164 setCellSizes(cellSizes);
00165 }
00166
00167 ompl::base::RealVectorIdentityProjectionEvaluator::RealVectorIdentityProjectionEvaluator(const StateSpacePtr &space) :
00168 ProjectionEvaluator(space)
00169 {
00170 checkSpaceType(space_);
00171 }
00172
00173 void ompl::base::RealVectorIdentityProjectionEvaluator::defaultCellSizes(void)
00174 {
00175 const RealVectorBounds &bounds = space_->as<RealVectorStateSpace>()->getBounds();
00176 cellSizes_.resize(getDimension());
00177 for (unsigned int i = 0 ; i < cellSizes_.size() ; ++i)
00178 cellSizes_[i] = (bounds.high[i] - bounds.low[i]) / magic::PROJECTION_DIMENSION_SPLITS;
00179 }
00180
00181 void ompl::base::RealVectorIdentityProjectionEvaluator::setup(void)
00182 {
00183 copySize_ = getDimension() * sizeof(double);
00184 ProjectionEvaluator::setup();
00185 }
00186
00187 unsigned int ompl::base::RealVectorIdentityProjectionEvaluator::getDimension(void) const
00188 {
00189 return space_->getDimension();
00190 }
00191
00192 void ompl::base::RealVectorIdentityProjectionEvaluator::project(const State *state, EuclideanProjection &projection) const
00193 {
00194 memcpy(&projection(0), state->as<RealVectorStateSpace::StateType>()->values, copySize_);
00195 }