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_SPACES_SO3_STATE_SPACE_
00038 #define OMPL_BASE_SPACES_SO3_STATE_SPACE_
00039
00040 #include "ompl/base/StateSpace.h"
00041
00042 namespace ompl
00043 {
00044 namespace base
00045 {
00046
00048 class SO3StateSampler : public StateSampler
00049 {
00050 public:
00051
00053 SO3StateSampler(const StateSpace *space) : StateSampler(space)
00054 {
00055 }
00056
00057 virtual void sampleUniform(State *state);
00066 virtual void sampleUniformNear(State *state, const State *near, const double distance);
00074 virtual void sampleGaussian(State *state, const State *mean, const double stdDev);
00075 };
00076
00081 class SO3StateSpace : public StateSpace
00082 {
00083 public:
00084
00085
00091 class StateType : public State
00092 {
00093 public:
00094
00096 void setAxisAngle(double ax, double ay, double az, double angle);
00097
00099 void setIdentity(void);
00100
00102 double x;
00103
00105 double y;
00106
00108 double z;
00109
00111 double w;
00112 };
00113
00114 SO3StateSpace(void) : StateSpace()
00115 {
00116 setName("SO3" + getName());
00117 type_ = STATE_SPACE_SO3;
00118 }
00119
00120 virtual ~SO3StateSpace(void)
00121 {
00122 }
00123
00125 double norm(const StateType *state) const;
00126
00127 virtual unsigned int getDimension(void) const;
00128
00129 virtual double getMaximumExtent(void) const;
00130
00131 virtual void enforceBounds(State *state) const;
00132
00133 virtual bool satisfiesBounds(const State *state) const;
00134
00135 virtual void copyState(State *destination, const State *source) const;
00136
00137 virtual unsigned int getSerializationLength(void) const;
00138
00139 virtual void serialize(void *serialization, const State *state) const;
00140
00141 virtual void deserialize(State *state, const void *serialization) const;
00142
00143 virtual double distance(const State *state1, const State *state2) const;
00144
00145 virtual bool equalStates(const State *state1, const State *state2) const;
00146
00147 virtual void interpolate(const State *from, const State *to, const double t, State *state) const;
00148
00149 virtual StateSamplerPtr allocDefaultStateSampler(void) const;
00150
00151 virtual State* allocState(void) const;
00152
00153 virtual void freeState(State *state) const;
00154
00155 virtual double* getValueAddressAtIndex(State *state, const unsigned int index) const;
00156
00157 virtual void printState(const State *state, std::ostream &out) const;
00158
00159 virtual void printSettings(std::ostream &out) const;
00160
00161 virtual void registerProjections(void);
00162 };
00163 }
00164 }
00165
00166 #endif