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 double ompl::base::SO2StateSpace::distance(const State *state1, const State *state2) const
00096 {
00097
00098 double d = fabs(state1->as<StateType>()->value - state2->as<StateType>()->value);
00099 return (d > boost::math::constants::pi<double>()) ? 2.0 * boost::math::constants::pi<double>() - d : d;
00100 }
00101
00102 bool ompl::base::SO2StateSpace::equalStates(const State *state1, const State *state2) const
00103 {
00104 return fabs(state1->as<StateType>()->value - state2->as<StateType>()->value) < std::numeric_limits<double>::epsilon() * 2.0;
00105 }
00106
00107 void ompl::base::SO2StateSpace::interpolate(const State *from, const State *to, const double t, State *state) const
00108 {
00109 double diff = to->as<StateType>()->value - from->as<StateType>()->value;
00110 if (fabs(diff) <= boost::math::constants::pi<double>())
00111 state->as<StateType>()->value = from->as<StateType>()->value + diff * t;
00112 else
00113 {
00114 double &v = state->as<StateType>()->value;
00115 if (diff > 0.0)
00116 diff = 2.0 * boost::math::constants::pi<double>() - diff;
00117 else
00118 diff = -2.0 * boost::math::constants::pi<double>() - diff;
00119 v = from->as<StateType>()->value - diff * t;
00120
00121 if (v > boost::math::constants::pi<double>())
00122 v -= 2.0 * boost::math::constants::pi<double>();
00123 else
00124 if (v < -boost::math::constants::pi<double>())
00125 v += 2.0 * boost::math::constants::pi<double>();
00126 }
00127 }
00128
00129 ompl::base::StateSamplerPtr ompl::base::SO2StateSpace::allocStateSampler(void) const
00130 {
00131 return StateSamplerPtr(new SO2StateSampler(this));
00132 }
00133
00134 ompl::base::State* ompl::base::SO2StateSpace::allocState(void) const
00135 {
00136 return new StateType();
00137 }
00138
00139 void ompl::base::SO2StateSpace::freeState(State *state) const
00140 {
00141 delete static_cast<StateType*>(state);
00142 }
00143
00144 void ompl::base::SO2StateSpace::registerProjections(void)
00145 {
00146 class SO2DefaultProjection : public ProjectionEvaluator
00147 {
00148 public:
00149
00150 SO2DefaultProjection(const StateSpace *space) : ProjectionEvaluator(space)
00151 {
00152 }
00153
00154 virtual unsigned int getDimension(void) const
00155 {
00156 return 1;
00157 }
00158
00159 virtual void defaultCellSizes(void)
00160 {
00161 cellSizes_.resize(1);
00162 cellSizes_[0] = boost::math::constants::pi<double>() / magic::PROJECTION_DIMENSION_SPLITS;
00163 }
00164
00165 virtual void project(const State *state, EuclideanProjection &projection) const
00166 {
00167 projection(0) = state->as<SO2StateSpace::StateType>()->value;
00168 }
00169 };
00170
00171 registerDefaultProjection(ProjectionEvaluatorPtr(dynamic_cast<ProjectionEvaluator*>(new SO2DefaultProjection(this))));
00172 }
00173
00174 double* ompl::base::SO2StateSpace::getValueAddressAtIndex(State *state, const unsigned int index) const
00175 {
00176 return index == 0 ? &(state->as<StateType>()->value) : NULL;
00177 }
00178
00179 void ompl::base::SO2StateSpace::printState(const State *state, std::ostream &out) const
00180 {
00181 out << "SO2State [";
00182 if (state)
00183 out << state->as<StateType>()->value;
00184 else
00185 out << "NULL";
00186 out << ']' << std::endl;
00187 }
00188
00189 void ompl::base::SO2StateSpace::printSettings(std::ostream &out) const
00190 {
00191 out << "SO2 state space '" << getName() << "'" << std::endl;
00192 }