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
00088
00092 class ProjectionMatrix
00093 {
00094 public:
00095
00097 typedef std::vector< std::valarray<double> > Matrix;
00098
00114 static Matrix ComputeRandom(const unsigned int from, const unsigned int to, const std::vector<double> &scale);
00115
00124 static Matrix ComputeRandom(const unsigned int from, const unsigned int to);
00125
00127 void computeRandom(const unsigned int from, const unsigned int to, const std::vector<double> &scale);
00128
00130 void computeRandom(const unsigned int from, const unsigned int to);
00131
00133 void project(const double *from, double *to) const;
00134
00136 void print(std::ostream &out = std::cout) const;
00137
00139 Matrix mat;
00140 };
00141
00142 ClassForward(StateSpace);
00143
00145 ClassForward(ProjectionEvaluator);
00146
00156 class ProjectionEvaluator : private boost::noncopyable
00157 {
00158 public:
00159
00161 ProjectionEvaluator(const StateSpace *space) : space_(space), defaultCellSizes_(true), cellSizesWereInferred_(false)
00162 {
00163 }
00164
00166 ProjectionEvaluator(const StateSpacePtr &space) : space_(space.get()), defaultCellSizes_(true), cellSizesWereInferred_(false)
00167 {
00168 }
00169
00170 virtual ~ProjectionEvaluator(void)
00171 {
00172 }
00173
00175 virtual unsigned int getDimension(void) const = 0;
00176
00178 virtual void project(const State *state, EuclideanProjection &projection) const = 0;
00179
00187 void setCellSizes(const std::vector<double> &cellSizes);
00188
00190 bool userConfigured(void) const;
00191
00193 const std::vector<double>& getCellSizes(void) const
00194 {
00195 return cellSizes_;
00196 }
00197
00199 void checkCellSizes(void) const;
00200
00206 void inferCellSizes(void);
00207
00212 virtual void defaultCellSizes(void);
00213
00215 virtual void setup(void);
00216
00218 void computeCoordinates(const EuclideanProjection &projection, ProjectionCoordinates &coord) const;
00219
00221 void computeCoordinates(const State *state, ProjectionCoordinates &coord) const
00222 {
00223 EuclideanProjection projection(getDimension());
00224 project(state, projection);
00225 computeCoordinates(projection, coord);
00226 }
00227
00229 virtual void printSettings(std::ostream &out = std::cout) const;
00230
00232 virtual void printProjection(const EuclideanProjection &projection, std::ostream &out = std::cout) const;
00233
00234 protected:
00235
00237 const StateSpace *space_;
00238
00242 std::vector<double> cellSizes_;
00243
00248 bool defaultCellSizes_;
00249
00252 bool cellSizesWereInferred_;
00253
00255 msg::Interface msg_;
00256 };
00257
00258 }
00259
00260 }
00261
00262 #endif