All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
TimeStateSpace.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/TimeStateSpace.h"
38 #include "ompl/util/Exception.h"
39 #include "ompl/tools/config/MagicConstants.h"
40 #include <limits>
41 
43 {
44  if (space_->as<TimeStateSpace>()->isBounded())
46  space_->as<TimeStateSpace>()->getMaxTimeBound());
47  else
48  state->as<TimeStateSpace::StateType>()->position = 0.0;
49 }
50 
51 void ompl::base::TimeStateSampler::sampleUniformNear(State *state, const State *near, const double distance)
52 {
53  state->as<TimeStateSpace::StateType>()->position =
54  rng_.uniformReal(near->as<TimeStateSpace::StateType>()->position - distance,
55  near->as<TimeStateSpace::StateType>()->position + distance);
56  space_->enforceBounds(state);
57 }
58 
59 void ompl::base::TimeStateSampler::sampleGaussian(State *state, const State *mean, const double stdDev)
60 {
61  state->as<TimeStateSpace::StateType>()->position =
62  rng_.gaussian(mean->as<TimeStateSpace::StateType>()->position, stdDev);
63  space_->enforceBounds(state);
64 }
65 
67 {
68  return 1;
69 }
70 
71 void ompl::base::TimeStateSpace::setBounds(double minTime, double maxTime)
72 {
73  if (minTime > maxTime)
74  throw Exception("The maximum position in time cannot be before the minimum position in time");
75 
76  minTime_ = minTime;
77  maxTime_ = maxTime;
78  bounded_ = true;
79 }
80 
82 {
83  return bounded_ ? maxTime_ - minTime_ : 1.0;
84 }
85 
87 {
88  if (bounded_)
89  {
90  if (state->as<StateType>()->position > maxTime_)
91  state->as<StateType>()->position = maxTime_;
92  else
93  if (state->as<StateType>()->position < minTime_)
94  state->as<StateType>()->position = minTime_;
95  }
96 }
97 
99 {
100  return !bounded_ || (state->as<StateType>()->position >= minTime_ - std::numeric_limits<double>::epsilon() &&
101  state->as<StateType>()->position <= maxTime_ + std::numeric_limits<double>::epsilon());
102 }
103 
104 void ompl::base::TimeStateSpace::copyState(State *destination, const State *source) const
105 {
106  destination->as<StateType>()->position = source->as<StateType>()->position;
107 }
108 
110 {
111  return sizeof(double);
112 }
113 
114 void ompl::base::TimeStateSpace::serialize(void *serialization, const State *state) const
115 {
116  memcpy(serialization, &state->as<StateType>()->position, sizeof(double));
117 }
118 
119 void ompl::base::TimeStateSpace::deserialize(State *state, const void *serialization) const
120 {
121  memcpy(&state->as<StateType>()->position, serialization, sizeof(double));
122 }
123 
124 double ompl::base::TimeStateSpace::distance(const State *state1, const State *state2) const
125 {
126  return fabs(state1->as<StateType>()->position - state2->as<StateType>()->position);
127 }
128 
129 bool ompl::base::TimeStateSpace::equalStates(const State *state1, const State *state2) const
130 {
131  return fabs(state1->as<StateType>()->position - state2->as<StateType>()->position) < std::numeric_limits<double>::epsilon() * 2.0;
132 }
133 
134 void ompl::base::TimeStateSpace::interpolate(const State *from, const State *to, const double t, State *state) const
135 {
136  state->as<StateType>()->position = from->as<StateType>()->position +
137  (to->as<StateType>()->position - from->as<StateType>()->position) * t;
138 }
139 
141 {
142  return StateSamplerPtr(new TimeStateSampler(this));
143 }
144 
146 {
147  return new StateType();
148 }
149 
151 {
152  delete static_cast<StateType*>(state);
153 }
154 
156 {
157  class TimeDefaultProjection : public ProjectionEvaluator
158  {
159  public:
160 
161  TimeDefaultProjection(const StateSpace *space) : ProjectionEvaluator(space)
162  {
163  }
164 
165  virtual unsigned int getDimension(void) const
166  {
167  return 1;
168  }
169 
170  virtual void defaultCellSizes(void)
171  {
172  cellSizes_.resize(1);
173  if (space_->as<TimeStateSpace>()->isBounded())
174  cellSizes_[0] = (space_->as<TimeStateSpace>()->getMaxTimeBound() - space_->as<TimeStateSpace>()->getMinTimeBound()) / magic::PROJECTION_DIMENSION_SPLITS;
175  else
176  cellSizes_[0] = 1.0;
177  }
178 
179  virtual void project(const State *state, EuclideanProjection &projection) const
180  {
181  projection(0) = state->as<TimeStateSpace::StateType>()->position;
182  }
183  };
184 
185  registerDefaultProjection(ProjectionEvaluatorPtr(dynamic_cast<ProjectionEvaluator*>(new TimeDefaultProjection(this))));
186 }
187 
188 double* ompl::base::TimeStateSpace::getValueAddressAtIndex(State *state, const unsigned int index) const
189 {
190  return index == 0 ? &(state->as<StateType>()->position) : NULL;
191 }
192 
193 void ompl::base::TimeStateSpace::printState(const State *state, std::ostream &out) const
194 {
195  out << "TimeState [";
196  if (state)
197  out << state->as<StateType>()->position;
198  else
199  out << "NULL";
200  out << ']' << std::endl;
201 }
202 
203 void ompl::base::TimeStateSpace::printSettings(std::ostream &out) const
204 {
205  out << "Time state space '" << getName() << "'" << std::endl;
206 }