ompl/base/spaces/src/SO2StateSpace.cpp
00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2010, Rice University 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of the Rice University nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 *********************************************************************/ 00034 00035 /* Author: Ioan Sucan */ 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 // Define for boost version < 1.47 00045 #ifndef BOOST_ASSERT_MSG 00046 #define BOOST_ASSERT_MSG(expr, msg) assert(expr) 00047 #endif 00048 00049 void ompl::base::SO2StateSampler::sampleUniform(State *state) 00050 { 00051 state->as<SO2StateSpace::StateType>()->value = 00052 rng_.uniformReal(-boost::math::constants::pi<double>(), boost::math::constants::pi<double>()); 00053 } 00054 00055 void ompl::base::SO2StateSampler::sampleUniformNear(State *state, const State *near, const double distance) 00056 { 00057 state->as<SO2StateSpace::StateType>()->value = rng_.uniformReal(near->as<SO2StateSpace::StateType>()->value - distance, 00058 near->as<SO2StateSpace::StateType>()->value + distance); 00059 space_->enforceBounds(state); 00060 } 00061 00062 void ompl::base::SO2StateSampler::sampleGaussian(State *state, const State *mean, const double stdDev) 00063 { 00064 state->as<SO2StateSpace::StateType>()->value = rng_.gaussian(mean->as<SO2StateSpace::StateType>()->value, stdDev); 00065 space_->enforceBounds(state); 00066 } 00067 00068 unsigned int ompl::base::SO2StateSpace::getDimension() const 00069 { 00070 return 1; 00071 } 00072 00073 double ompl::base::SO2StateSpace::getMaximumExtent() const 00074 { 00075 return boost::math::constants::pi<double>(); 00076 } 00077 00078 double ompl::base::SO2StateSpace::getMeasure() const 00079 { 00080 return 2.0 * boost::math::constants::pi<double>(); 00081 } 00082 00083 void ompl::base::SO2StateSpace::enforceBounds(State *state) const 00084 { 00085 double v = fmod(state->as<StateType>()->value, 2.0 * boost::math::constants::pi<double>()); 00086 if (v <= -boost::math::constants::pi<double>()) 00087 v += 2.0 * boost::math::constants::pi<double>(); 00088 else 00089 if (v > boost::math::constants::pi<double>()) 00090 v -= 2.0 * boost::math::constants::pi<double>(); 00091 state->as<StateType>()->value = v; 00092 } 00093 00094 bool ompl::base::SO2StateSpace::satisfiesBounds(const State *state) const 00095 { 00096 return (state->as<StateType>()->value <= boost::math::constants::pi<double>()) && 00097 (state->as<StateType>()->value > -boost::math::constants::pi<double>()); 00098 } 00099 00100 void ompl::base::SO2StateSpace::copyState(State *destination, const State *source) const 00101 { 00102 destination->as<StateType>()->value = source->as<StateType>()->value; 00103 } 00104 00105 unsigned int ompl::base::SO2StateSpace::getSerializationLength() const 00106 { 00107 return sizeof(double); 00108 } 00109 00110 void ompl::base::SO2StateSpace::serialize(void *serialization, const State *state) const 00111 { 00112 memcpy(serialization, &state->as<StateType>()->value, sizeof(double)); 00113 } 00114 00115 void ompl::base::SO2StateSpace::deserialize(State *state, const void *serialization) const 00116 { 00117 memcpy(&state->as<StateType>()->value, serialization, sizeof(double)); 00118 } 00119 00120 double ompl::base::SO2StateSpace::distance(const State *state1, const State *state2) const 00121 { 00122 // assuming the states 1 & 2 are within bounds 00123 double d = fabs(state1->as<StateType>()->value - state2->as<StateType>()->value); 00124 BOOST_ASSERT_MSG(satisfiesBounds(state1) && satisfiesBounds(state2), 00125 "The states passed to SO2StateSpace::distance are not within bounds. Call " 00126 "SO2StateSpace::enforceBounds() in, e.g., ompl::control::ODESolver::PostPropagationEvent, " 00127 "ompl::control::StatePropagator, or ompl::base::StateValidityChecker"); 00128 return (d > boost::math::constants::pi<double>()) ? 2.0 * boost::math::constants::pi<double>() - d : d; 00129 } 00130 00131 bool ompl::base::SO2StateSpace::equalStates(const State *state1, const State *state2) const 00132 { 00133 return fabs(state1->as<StateType>()->value - state2->as<StateType>()->value) < std::numeric_limits<double>::epsilon() * 2.0; 00134 } 00135 00136 void ompl::base::SO2StateSpace::interpolate(const State *from, const State *to, const double t, State *state) const 00137 { 00138 double diff = to->as<StateType>()->value - from->as<StateType>()->value; 00139 if (fabs(diff) <= boost::math::constants::pi<double>()) 00140 state->as<StateType>()->value = from->as<StateType>()->value + diff * t; 00141 else 00142 { 00143 double &v = state->as<StateType>()->value; 00144 if (diff > 0.0) 00145 diff = 2.0 * boost::math::constants::pi<double>() - diff; 00146 else 00147 diff = -2.0 * boost::math::constants::pi<double>() - diff; 00148 v = from->as<StateType>()->value - diff * t; 00149 // input states are within bounds, so the following check is sufficient 00150 if (v > boost::math::constants::pi<double>()) 00151 v -= 2.0 * boost::math::constants::pi<double>(); 00152 else 00153 if (v < -boost::math::constants::pi<double>()) 00154 v += 2.0 * boost::math::constants::pi<double>(); 00155 } 00156 } 00157 00158 ompl::base::StateSamplerPtr ompl::base::SO2StateSpace::allocDefaultStateSampler() const 00159 { 00160 return StateSamplerPtr(new SO2StateSampler(this)); 00161 } 00162 00163 ompl::base::State* ompl::base::SO2StateSpace::allocState() const 00164 { 00165 return new StateType(); 00166 } 00167 00168 void ompl::base::SO2StateSpace::freeState(State *state) const 00169 { 00170 delete static_cast<StateType*>(state); 00171 } 00172 00173 void ompl::base::SO2StateSpace::registerProjections() 00174 { 00175 class SO2DefaultProjection : public ProjectionEvaluator 00176 { 00177 public: 00178 00179 SO2DefaultProjection(const StateSpace *space) : ProjectionEvaluator(space) 00180 { 00181 } 00182 00183 virtual unsigned int getDimension() const 00184 { 00185 return 1; 00186 } 00187 00188 virtual void defaultCellSizes() 00189 { 00190 cellSizes_.resize(1); 00191 cellSizes_[0] = boost::math::constants::pi<double>() / magic::PROJECTION_DIMENSION_SPLITS; 00192 bounds_.resize(1); 00193 bounds_.low[0] = -boost::math::constants::pi<double>(); 00194 bounds_.high[0] = boost::math::constants::pi<double>(); 00195 } 00196 00197 virtual void project(const State *state, EuclideanProjection &projection) const 00198 { 00199 projection(0) = state->as<SO2StateSpace::StateType>()->value; 00200 } 00201 }; 00202 00203 registerDefaultProjection(ProjectionEvaluatorPtr(dynamic_cast<ProjectionEvaluator*>(new SO2DefaultProjection(this)))); 00204 } 00205 00206 double* ompl::base::SO2StateSpace::getValueAddressAtIndex(State *state, const unsigned int index) const 00207 { 00208 return index == 0 ? &(state->as<StateType>()->value) : NULL; 00209 } 00210 00211 void ompl::base::SO2StateSpace::printState(const State *state, std::ostream &out) const 00212 { 00213 out << "SO2State ["; 00214 if (state) 00215 out << state->as<StateType>()->value; 00216 else 00217 out << "NULL"; 00218 out << ']' << std::endl; 00219 } 00220 00221 void ompl::base::SO2StateSpace::printSettings(std::ostream &out) const 00222 { 00223 out << "SO2 state space '" << getName() << "'" << std::endl; 00224 }