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/manifolds/RealVectorStateProjections.h"
00038 #include "ompl/util/Exception.h"
00039 #include "ompl/util/MagicConstants.h"
00040 #include <cstring>
00041
00043 namespace ompl
00044 {
00045 namespace base
00046 {
00047 static inline void checkManifoldType(const StateManifold *m)
00048 {
00049 if (!dynamic_cast<const RealVectorStateManifold*>(m))
00050 throw Exception("Expected real vector manifold for projection");
00051 }
00052 }
00053 }
00055
00056 ompl::base::RealVectorLinearProjectionEvaluator::RealVectorLinearProjectionEvaluator(const StateManifold *manifold, const std::vector<double> &cellSizes,
00057 const ProjectionMatrix::Matrix &projection) :
00058 ProjectionEvaluator(manifold)
00059 {
00060 checkManifoldType(manifold_);
00061 projection_.mat = projection;
00062 setCellSizes(cellSizes);
00063 }
00064
00065 ompl::base::RealVectorLinearProjectionEvaluator::RealVectorLinearProjectionEvaluator(const StateManifoldPtr &manifold, const std::vector<double> &cellSizes,
00066 const ProjectionMatrix::Matrix &projection) :
00067 ProjectionEvaluator(manifold)
00068 {
00069 checkManifoldType(manifold_);
00070 projection_.mat = projection;
00071 setCellSizes(cellSizes);
00072 }
00073
00074 ompl::base::RealVectorLinearProjectionEvaluator::RealVectorLinearProjectionEvaluator(const StateManifold *manifold,
00075 const ProjectionMatrix::Matrix &projection) :
00076 ProjectionEvaluator(manifold)
00077 {
00078 checkManifoldType(manifold_);
00079 projection_.mat = projection;
00080 }
00081
00082 ompl::base::RealVectorLinearProjectionEvaluator::RealVectorLinearProjectionEvaluator(const StateManifoldPtr &manifold,
00083 const ProjectionMatrix::Matrix &projection) :
00084 ProjectionEvaluator(manifold)
00085 {
00086 checkManifoldType(manifold_);
00087 projection_.mat = projection;
00088 }
00089
00090 ompl::base::RealVectorOrthogonalProjectionEvaluator::RealVectorOrthogonalProjectionEvaluator(const StateManifold *manifold, const std::vector<double> &cellSizes,
00091 const std::vector<unsigned int> &components) :
00092 ProjectionEvaluator(manifold), components_(components)
00093 {
00094 checkManifoldType(manifold_);
00095 setCellSizes(cellSizes);
00096 }
00097
00098 ompl::base::RealVectorOrthogonalProjectionEvaluator::RealVectorOrthogonalProjectionEvaluator(const StateManifoldPtr &manifold, const std::vector<double> &cellSizes,
00099 const std::vector<unsigned int> &components) :
00100 ProjectionEvaluator(manifold), components_(components)
00101 {
00102 checkManifoldType(manifold_);
00103 setCellSizes(cellSizes);
00104 }
00105
00106 ompl::base::RealVectorOrthogonalProjectionEvaluator::RealVectorOrthogonalProjectionEvaluator(const StateManifold *manifold, const std::vector<unsigned int> &components) :
00107 ProjectionEvaluator(manifold), components_(components)
00108 {
00109 checkManifoldType(manifold_);
00110 }
00111
00112 ompl::base::RealVectorOrthogonalProjectionEvaluator::RealVectorOrthogonalProjectionEvaluator(const StateManifoldPtr &manifold, const std::vector<unsigned int> &components) :
00113 ProjectionEvaluator(manifold), components_(components)
00114 {
00115 checkManifoldType(manifold_);
00116 }
00117
00118 void ompl::base::RealVectorOrthogonalProjectionEvaluator::defaultCellSizes(void)
00119 {
00120 const RealVectorBounds &bounds = manifold_->as<RealVectorStateManifold>()->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.size();
00129 }
00130
00131 void ompl::base::RealVectorLinearProjectionEvaluator::project(const State *state, EuclideanProjection &projection) const
00132 {
00133 projection_.project(state->as<RealVectorStateManifold::StateType>()->values, projection.values);
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.values[i] = state->as<RealVectorStateManifold::StateType>()->values[components_[i]];
00145 }
00146
00147 ompl::base::RealVectorIdentityProjectionEvaluator::RealVectorIdentityProjectionEvaluator(const StateManifold *manifold, const std::vector<double> &cellSizes) :
00148 ProjectionEvaluator(manifold)
00149 {
00150 checkManifoldType(manifold_);
00151 setCellSizes(cellSizes);
00152 }
00153
00154 ompl::base::RealVectorIdentityProjectionEvaluator::RealVectorIdentityProjectionEvaluator(const StateManifold *manifold) :
00155 ProjectionEvaluator(manifold)
00156 {
00157 checkManifoldType(manifold_);
00158 }
00159
00160 ompl::base::RealVectorIdentityProjectionEvaluator::RealVectorIdentityProjectionEvaluator(const StateManifoldPtr &manifold, const std::vector<double> &cellSizes) :
00161 ProjectionEvaluator(manifold)
00162 {
00163 checkManifoldType(manifold_);
00164 setCellSizes(cellSizes);
00165 }
00166
00167 ompl::base::RealVectorIdentityProjectionEvaluator::RealVectorIdentityProjectionEvaluator(const StateManifoldPtr &manifold) :
00168 ProjectionEvaluator(manifold)
00169 {
00170 checkManifoldType(manifold_);
00171 }
00172
00173 void ompl::base::RealVectorIdentityProjectionEvaluator::defaultCellSizes(void)
00174 {
00175 const RealVectorBounds &bounds = manifold_->as<RealVectorStateManifold>()->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 manifold_->getDimension();
00190 }
00191
00192 void ompl::base::RealVectorIdentityProjectionEvaluator::project(const State *state, EuclideanProjection &projection) const
00193 {
00194 memcpy(projection.values, state->as<RealVectorStateManifold::StateType>()->values, copySize_);
00195 }