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_MANIFOLDS_REAL_VECTOR_STATE_MANIFOLD_
00038 #define OMPL_BASE_MANIFOLDS_REAL_VECTOR_STATE_MANIFOLD_
00039
00040 #include "ompl/base/StateManifold.h"
00041 #include "ompl/base/manifolds/RealVectorBounds.h"
00042 #include <vector>
00043 #include <string>
00044 #include <map>
00045
00046 namespace ompl
00047 {
00048 namespace base
00049 {
00050
00052 class RealVectorStateSampler : public ManifoldStateSampler
00053 {
00054 public:
00055
00057 RealVectorStateSampler(const StateManifold *manifold) : ManifoldStateSampler(manifold)
00058 {
00059 }
00060
00061 virtual void sampleUniform(State *state);
00062 virtual void sampleUniformNear(State *state, const State *near, const double distance);
00063 virtual void sampleGaussian(State *state, const State *mean, const double stdDev);
00064 };
00065
00067 class RealVectorStateManifold : public StateManifold
00068 {
00069 public:
00070
00072 class StateType : public State
00073 {
00074 public:
00075 StateType(void) : State()
00076 {
00077 }
00078
00081 double operator[](unsigned int i) const
00082 {
00083 return values[i];
00084 }
00085
00088 double& operator[](unsigned int i)
00089 {
00090 return values[i];
00091 }
00092
00094 double *values;
00095 };
00096
00099 RealVectorStateManifold(unsigned int dim = 0) : StateManifold(), dimension_(dim), bounds_(dim), stateBytes_(dim * sizeof(double))
00100 {
00101 type_ = STATE_MANIFOLD_REAL_VECTOR;
00102 setName("RealVector" + getName());
00103 dimensionNames_.resize(dim, "");
00104 }
00105
00106 virtual ~RealVectorStateManifold(void)
00107 {
00108 }
00109
00111 void addDimension(double minBound = 0.0, double maxBound = 0.0);
00112
00114 void addDimension(const std::string &name, double minBound = 0.0, double maxBound = 0.0);
00115
00119 void setBounds(const RealVectorBounds &bounds);
00120
00123 void setBounds(double low, double high);
00124
00126 const RealVectorBounds& getBounds(void) const
00127 {
00128 return bounds_;
00129 }
00130
00131 virtual unsigned int getDimension(void) const;
00132
00135 const std::string& getDimensionName(unsigned int index) const;
00136
00138 int getDimensionIndex(const std::string &name) const;
00139
00141 void setDimensionName(unsigned int index, const std::string &name);
00142
00143 virtual double getMaximumExtent(void) const;
00144
00145 virtual void enforceBounds(State *state) const;
00146
00147 virtual bool satisfiesBounds(const State *state) const;
00148
00149 virtual void copyState(State *destination, const State *source) const;
00150
00151 virtual double distance(const State *state1, const State *state2) const;
00152
00153 virtual bool equalStates(const State *state1, const State *state2) const;
00154
00155 virtual void interpolate(const State *from, const State *to, const double t, State *state) const;
00156
00157 virtual ManifoldStateSamplerPtr allocStateSampler(void) const;
00158
00159 virtual State* allocState(void) const;
00160
00161 virtual void freeState(State *state) const;
00162
00163 virtual double* getValueAddressAtIndex(State *state, const unsigned int index) const;
00164
00165 virtual void printState(const State *state, std::ostream &out) const;
00166
00167 virtual void printSettings(std::ostream &out) const;
00168
00169 virtual void registerProjections(void);
00170
00171 virtual void setup(void);
00172
00173 protected:
00174
00176 unsigned int dimension_;
00177
00179 RealVectorBounds bounds_;
00180
00182 std::vector<std::string> dimensionNames_;
00183
00185 std::map<std::string, unsigned int> dimensionIndex_;
00186
00187 private:
00188
00190 std::size_t stateBytes_;
00191
00192 };
00193 }
00194 }
00195
00196 #endif