37 #include "ompl/base/spaces/SO2StateSpace.h"
41 #include "ompl/tools/config/MagicConstants.h"
42 #include <boost/math/constants/constants.hpp>
47 rng_.
uniformReal(-boost::math::constants::pi<double>(), boost::math::constants::pi<double>());
54 space_->enforceBounds(state);
60 space_->enforceBounds(state);
70 return boost::math::constants::pi<double>();
75 double v = fmod(state->
as<
StateType>()->
value, 2.0 * boost::math::constants::pi<double>());
76 if (v < -boost::math::constants::pi<double>())
77 v += 2.0 * boost::math::constants::pi<double>();
79 if (v > boost::math::constants::pi<double>())
80 v -= 2.0 * boost::math::constants::pi<double>();
86 return (state->
as<
StateType>()->
value < boost::math::constants::pi<double>() + std::numeric_limits<double>::epsilon()) &&
87 (state->
as<
StateType>()->value > -boost::math::constants::pi<double>() - std::numeric_limits<double>::epsilon());
97 return sizeof(double);
114 return (d > boost::math::constants::pi<double>()) ? 2.0 * boost::math::constants::pi<double>() - d : d;
125 if (fabs(diff) <= boost::math::constants::pi<double>())
131 diff = 2.0 * boost::math::constants::pi<double>() - diff;
133 diff = -2.0 * boost::math::constants::pi<double>() - diff;
136 if (v > boost::math::constants::pi<double>())
137 v -= 2.0 * boost::math::constants::pi<double>();
139 if (v < -boost::math::constants::pi<double>())
140 v += 2.0 * boost::math::constants::pi<double>();
169 virtual unsigned int getDimension(
void)
const
174 virtual void defaultCellSizes(
void)
176 cellSizes_.resize(1);
186 registerDefaultProjection(
ProjectionEvaluatorPtr(dynamic_cast<ProjectionEvaluator*>(
new SO2DefaultProjection(
this))));
191 return index == 0 ? &(state->
as<
StateType>()->value) : NULL;
201 out <<
']' << std::endl;
206 out <<
"SO2 state space '" << getName() <<
"'" << std::endl;