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/spaces/SO2StateSpace.h"
00038 #include <algorithm>
00039 #include <limits>
00040 #include <cmath>
00041 #include "ompl/tools/config/MagicConstants.h"
00042 #include <boost/math/constants/constants.hpp>
00043
00044 void ompl::base::SO2StateSampler::sampleUniform(State *state)
00045 {
00046 state->as<SO2StateSpace::StateType>()->value =
00047 rng_.uniformReal(-boost::math::constants::pi<double>(), boost::math::constants::pi<double>());
00048 }
00049
00050 void ompl::base::SO2StateSampler::sampleUniformNear(State *state, const State *near, const double distance)
00051 {
00052 state->as<SO2StateSpace::StateType>()->value = rng_.uniformReal(near->as<SO2StateSpace::StateType>()->value - distance,
00053 near->as<SO2StateSpace::StateType>()->value + distance);
00054 space_->enforceBounds(state);
00055 }
00056
00057 void ompl::base::SO2StateSampler::sampleGaussian(State *state, const State *mean, const double stdDev)
00058 {
00059 state->as<SO2StateSpace::StateType>()->value = rng_.gaussian(mean->as<SO2StateSpace::StateType>()->value, stdDev);
00060 space_->enforceBounds(state);
00061 }
00062
00063 unsigned int ompl::base::SO2StateSpace::getDimension(void) const
00064 {
00065 return 1;
00066 }
00067
00068 double ompl::base::SO2StateSpace::getMaximumExtent(void) const
00069 {
00070 return boost::math::constants::pi<double>();
00071 }
00072
00073 void ompl::base::SO2StateSpace::enforceBounds(State *state) const
00074 {
00075 double v = fmod(state->as<StateType>()->value, 2.0 * boost::math::constants::pi<double>());
00076 if (v < -boost::math::constants::pi<double>())
00077 v += 2.0 * boost::math::constants::pi<double>();
00078 else
00079 if (v > boost::math::constants::pi<double>())
00080 v -= 2.0 * boost::math::constants::pi<double>();
00081 state->as<StateType>()->value = v;
00082 }
00083
00084 bool ompl::base::SO2StateSpace::satisfiesBounds(const State *state) const
00085 {
00086 return (state->as<StateType>()->value < boost::math::constants::pi<double>() + std::numeric_limits<double>::epsilon()) &&
00087 (state->as<StateType>()->value > -boost::math::constants::pi<double>() - std::numeric_limits<double>::epsilon());
00088 }
00089
00090 void ompl::base::SO2StateSpace::copyState(State *destination, const State *source) const
00091 {
00092 destination->as<StateType>()->value = source->as<StateType>()->value;
00093 }
00094
00095 unsigned int ompl::base::SO2StateSpace::getSerializationLength(void) const
00096 {
00097 return sizeof(double);
00098 }
00099
00100 void ompl::base::SO2StateSpace::serialize(void *serialization, const State *state) const
00101 {
00102 memcpy(serialization, &state->as<StateType>()->value, sizeof(double));
00103 }
00104
00105 void ompl::base::SO2StateSpace::deserialize(State *state, const void *serialization) const
00106 {
00107 memcpy(&state->as<StateType>()->value, serialization, sizeof(double));
00108 }
00109
00110 double ompl::base::SO2StateSpace::distance(const State *state1, const State *state2) const
00111 {
00112
00113 double d = fabs(state1->as<StateType>()->value - state2->as<StateType>()->value);
00114 return (d > boost::math::constants::pi<double>()) ? 2.0 * boost::math::constants::pi<double>() - d : d;
00115 }
00116
00117 bool ompl::base::SO2StateSpace::equalStates(const State *state1, const State *state2) const
00118 {
00119 return fabs(state1->as<StateType>()->value - state2->as<StateType>()->value) < std::numeric_limits<double>::epsilon() * 2.0;
00120 }
00121
00122 void ompl::base::SO2StateSpace::interpolate(const State *from, const State *to, const double t, State *state) const
00123 {
00124 double diff = to->as<StateType>()->value - from->as<StateType>()->value;
00125 if (fabs(diff) <= boost::math::constants::pi<double>())
00126 state->as<StateType>()->value = from->as<StateType>()->value + diff * t;
00127 else
00128 {
00129 double &v = state->as<StateType>()->value;
00130 if (diff > 0.0)
00131 diff = 2.0 * boost::math::constants::pi<double>() - diff;
00132 else
00133 diff = -2.0 * boost::math::constants::pi<double>() - diff;
00134 v = from->as<StateType>()->value - diff * t;
00135
00136 if (v > boost::math::constants::pi<double>())
00137 v -= 2.0 * boost::math::constants::pi<double>();
00138 else
00139 if (v < -boost::math::constants::pi<double>())
00140 v += 2.0 * boost::math::constants::pi<double>();
00141 }
00142 }
00143
00144 ompl::base::StateSamplerPtr ompl::base::SO2StateSpace::allocDefaultStateSampler(void) const
00145 {
00146 return StateSamplerPtr(new SO2StateSampler(this));
00147 }
00148
00149 ompl::base::State* ompl::base::SO2StateSpace::allocState(void) const
00150 {
00151 return new StateType();
00152 }
00153
00154 void ompl::base::SO2StateSpace::freeState(State *state) const
00155 {
00156 delete static_cast<StateType*>(state);
00157 }
00158
00159 void ompl::base::SO2StateSpace::registerProjections(void)
00160 {
00161 class SO2DefaultProjection : public ProjectionEvaluator
00162 {
00163 public:
00164
00165 SO2DefaultProjection(const StateSpace *space) : ProjectionEvaluator(space)
00166 {
00167 }
00168
00169 virtual unsigned int getDimension(void) const
00170 {
00171 return 1;
00172 }
00173
00174 virtual void defaultCellSizes(void)
00175 {
00176 cellSizes_.resize(1);
00177 cellSizes_[0] = boost::math::constants::pi<double>() / magic::PROJECTION_DIMENSION_SPLITS;
00178 }
00179
00180 virtual void project(const State *state, EuclideanProjection &projection) const
00181 {
00182 projection(0) = state->as<SO2StateSpace::StateType>()->value;
00183 }
00184 };
00185
00186 registerDefaultProjection(ProjectionEvaluatorPtr(dynamic_cast<ProjectionEvaluator*>(new SO2DefaultProjection(this))));
00187 }
00188
00189 double* ompl::base::SO2StateSpace::getValueAddressAtIndex(State *state, const unsigned int index) const
00190 {
00191 return index == 0 ? &(state->as<StateType>()->value) : NULL;
00192 }
00193
00194 void ompl::base::SO2StateSpace::printState(const State *state, std::ostream &out) const
00195 {
00196 out << "SO2State [";
00197 if (state)
00198 out << state->as<StateType>()->value;
00199 else
00200 out << "NULL";
00201 out << ']' << std::endl;
00202 }
00203
00204 void ompl::base::SO2StateSpace::printSettings(std::ostream &out) const
00205 {
00206 out << "SO2 state space '" << getName() << "'" << std::endl;
00207 }