All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
SO2StateSpace.cpp
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2010, Rice University
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Rice University nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
35 /* Author: Ioan Sucan */
36 
37 #include "ompl/base/spaces/SO2StateSpace.h"
38 #include <algorithm>
39 #include <limits>
40 #include <cmath>
41 #include "ompl/tools/config/MagicConstants.h"
42 #include <boost/math/constants/constants.hpp>
43 
45 {
46  state->as<SO2StateSpace::StateType>()->value =
47  rng_.uniformReal(-boost::math::constants::pi<double>(), boost::math::constants::pi<double>());
48 }
49 
50 void ompl::base::SO2StateSampler::sampleUniformNear(State *state, const State *near, const double distance)
51 {
52  state->as<SO2StateSpace::StateType>()->value = rng_.uniformReal(near->as<SO2StateSpace::StateType>()->value - distance,
53  near->as<SO2StateSpace::StateType>()->value + distance);
54  space_->enforceBounds(state);
55 }
56 
57 void ompl::base::SO2StateSampler::sampleGaussian(State *state, const State *mean, const double stdDev)
58 {
59  state->as<SO2StateSpace::StateType>()->value = rng_.gaussian(mean->as<SO2StateSpace::StateType>()->value, stdDev);
60  space_->enforceBounds(state);
61 }
62 
64 {
65  return 1;
66 }
67 
69 {
70  return boost::math::constants::pi<double>();
71 }
72 
74 {
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>();
78  else
79  if (v > boost::math::constants::pi<double>())
80  v -= 2.0 * boost::math::constants::pi<double>();
81  state->as<StateType>()->value = v;
82 }
83 
85 {
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());
88 }
89 
90 void ompl::base::SO2StateSpace::copyState(State *destination, const State *source) const
91 {
92  destination->as<StateType>()->value = source->as<StateType>()->value;
93 }
94 
96 {
97  return sizeof(double);
98 }
99 
100 void ompl::base::SO2StateSpace::serialize(void *serialization, const State *state) const
101 {
102  memcpy(serialization, &state->as<StateType>()->value, sizeof(double));
103 }
104 
105 void ompl::base::SO2StateSpace::deserialize(State *state, const void *serialization) const
106 {
107  memcpy(&state->as<StateType>()->value, serialization, sizeof(double));
108 }
109 
110 double ompl::base::SO2StateSpace::distance(const State *state1, const State *state2) const
111 {
112  // assuming the states 1 & 2 are within bounds
113  double d = fabs(state1->as<StateType>()->value - state2->as<StateType>()->value);
114  return (d > boost::math::constants::pi<double>()) ? 2.0 * boost::math::constants::pi<double>() - d : d;
115 }
116 
117 bool ompl::base::SO2StateSpace::equalStates(const State *state1, const State *state2) const
118 {
119  return fabs(state1->as<StateType>()->value - state2->as<StateType>()->value) < std::numeric_limits<double>::epsilon() * 2.0;
120 }
121 
122 void ompl::base::SO2StateSpace::interpolate(const State *from, const State *to, const double t, State *state) const
123 {
124  double diff = to->as<StateType>()->value - from->as<StateType>()->value;
125  if (fabs(diff) <= boost::math::constants::pi<double>())
126  state->as<StateType>()->value = from->as<StateType>()->value + diff * t;
127  else
128  {
129  double &v = state->as<StateType>()->value;
130  if (diff > 0.0)
131  diff = 2.0 * boost::math::constants::pi<double>() - diff;
132  else
133  diff = -2.0 * boost::math::constants::pi<double>() - diff;
134  v = from->as<StateType>()->value - diff * t;
135  // input states are within bounds, so the following check is sufficient
136  if (v > boost::math::constants::pi<double>())
137  v -= 2.0 * boost::math::constants::pi<double>();
138  else
139  if (v < -boost::math::constants::pi<double>())
140  v += 2.0 * boost::math::constants::pi<double>();
141  }
142 }
143 
145 {
146  return StateSamplerPtr(new SO2StateSampler(this));
147 }
148 
150 {
151  return new StateType();
152 }
153 
155 {
156  delete static_cast<StateType*>(state);
157 }
158 
160 {
161  class SO2DefaultProjection : public ProjectionEvaluator
162  {
163  public:
164 
165  SO2DefaultProjection(const StateSpace *space) : ProjectionEvaluator(space)
166  {
167  }
168 
169  virtual unsigned int getDimension(void) const
170  {
171  return 1;
172  }
173 
174  virtual void defaultCellSizes(void)
175  {
176  cellSizes_.resize(1);
177  cellSizes_[0] = boost::math::constants::pi<double>() / magic::PROJECTION_DIMENSION_SPLITS;
178  }
179 
180  virtual void project(const State *state, EuclideanProjection &projection) const
181  {
182  projection(0) = state->as<SO2StateSpace::StateType>()->value;
183  }
184  };
185 
186  registerDefaultProjection(ProjectionEvaluatorPtr(dynamic_cast<ProjectionEvaluator*>(new SO2DefaultProjection(this))));
187 }
188 
189 double* ompl::base::SO2StateSpace::getValueAddressAtIndex(State *state, const unsigned int index) const
190 {
191  return index == 0 ? &(state->as<StateType>()->value) : NULL;
192 }
193 
194 void ompl::base::SO2StateSpace::printState(const State *state, std::ostream &out) const
195 {
196  out << "SO2State [";
197  if (state)
198  out << state->as<StateType>()->value;
199  else
200  out << "NULL";
201  out << ']' << std::endl;
202 }
203 
204 void ompl::base::SO2StateSpace::printSettings(std::ostream &out) const
205 {
206  out << "SO2 state space '" << getName() << "'" << std::endl;
207 }