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 #ifndef OMPL_BASE_PROJECTION_EVALUATOR_
00038 #define OMPL_BASE_PROJECTION_EVALUATOR_
00039
00040 #include "ompl/base/State.h"
00041 #include "ompl/util/ClassForward.h"
00042 #include "ompl/util/Console.h"
00043 #include <vector>
00044 #include <valarray>
00045 #include <iostream>
00046 #include <boost/noncopyable.hpp>
00047
00048 namespace ompl
00049 {
00050
00051 namespace base
00052 {
00053
00055 typedef std::vector<int> ProjectionCoordinates;
00056
00058 class EuclideanProjection
00059 {
00060 public:
00061
00063 EuclideanProjection(unsigned int n) : values(new double[n])
00064 {
00065 }
00066
00067 ~EuclideanProjection(void)
00068 {
00069 delete[] values;
00070 }
00071
00073 double operator[](unsigned int i) const
00074 {
00075 return values[i];
00076 }
00077
00079 double& operator[](unsigned int i)
00080 {
00081 return values[i];
00082 }
00083
00085 double *values;
00086 };
00087
00091 class ProjectionMatrix
00092 {
00093 public:
00094
00096 typedef std::vector< std::valarray<double> > Matrix;
00097
00113 static Matrix ComputeRandom(const unsigned int from, const unsigned int to, const std::vector<double> &scale);
00114
00123 static Matrix ComputeRandom(const unsigned int from, const unsigned int to);
00124
00126 void computeRandom(const unsigned int from, const unsigned int to, const std::vector<double> &scale);
00127
00129 void computeRandom(const unsigned int from, const unsigned int to);
00130
00132 void project(const double *from, double *to) const;
00133
00135 void print(std::ostream &out = std::cout) const;
00136
00138 Matrix mat;
00139 };
00140
00141 ClassForward(StateSpace);
00142
00144 ClassForward(ProjectionEvaluator);
00145
00155 class ProjectionEvaluator : private boost::noncopyable
00156 {
00157 public:
00158
00160 ProjectionEvaluator(const StateSpace *space) : space_(space), defaultCellSizes_(true), cellSizesWereInferred_(false)
00161 {
00162 }
00163
00165 ProjectionEvaluator(const StateSpacePtr &space) : space_(space.get()), defaultCellSizes_(true), cellSizesWereInferred_(false)
00166 {
00167 }
00168
00169 virtual ~ProjectionEvaluator(void)
00170 {
00171 }
00172
00174 virtual unsigned int getDimension(void) const = 0;
00175
00177 virtual void project(const State *state, EuclideanProjection &projection) const = 0;
00178
00186 void setCellSizes(const std::vector<double> &cellSizes);
00187
00189 bool userConfigured(void) const;
00190
00192 const std::vector<double>& getCellSizes(void) const
00193 {
00194 return cellSizes_;
00195 }
00196
00198 void checkCellSizes(void) const;
00199
00205 void inferCellSizes(void);
00206
00211 void computeCellSizes(const std::vector<const State*> &states);
00212
00217 virtual void defaultCellSizes(void);
00218
00220 virtual void setup(void);
00221
00223 void computeCoordinates(const EuclideanProjection &projection, ProjectionCoordinates &coord) const;
00224
00226 void computeCoordinates(const State *state, ProjectionCoordinates &coord) const
00227 {
00228 EuclideanProjection projection(getDimension());
00229 project(state, projection);
00230 computeCoordinates(projection, coord);
00231 }
00232
00234 virtual void printSettings(std::ostream &out = std::cout) const;
00235
00237 virtual void printProjection(const EuclideanProjection &projection, std::ostream &out = std::cout) const;
00238
00239 protected:
00240
00242 const StateSpace *space_;
00243
00247 std::vector<double> cellSizes_;
00248
00253 bool defaultCellSizes_;
00254
00257 bool cellSizesWereInferred_;
00258
00260 msg::Interface msg_;
00261 };
00262
00263 }
00264
00265 }
00266
00267 #endif