All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
DiscreteStateSpace.cpp
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2011, 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: Elizabeth Fudge */
36 
37 #include "ompl/base/spaces/DiscreteStateSpace.h"
38 #include "ompl/util/Exception.h"
39 #include <limits>
40 #include <cstdlib>
41 
43 {
44  state->as<DiscreteStateSpace::StateType>()->value =
46  space_->as<DiscreteStateSpace>()->getUpperBound());
47 }
48 
49 void ompl::base::DiscreteStateSampler::sampleUniformNear(State *state, const State *near, const double distance)
50 {
51  const int d = (int)floor(distance + 0.5);
52  state->as<DiscreteStateSpace::StateType>()->value =
53  rng_.uniformInt(near->as<DiscreteStateSpace::StateType>()->value - d,
54  near->as<DiscreteStateSpace::StateType>()->value + d);
55  space_->enforceBounds(state);
56 }
57 
58 void ompl::base::DiscreteStateSampler::sampleGaussian(State *state, const State *mean, const double stdDev)
59 {
60  state->as<DiscreteStateSpace::StateType>()->value =
61  (int)floor(rng_.gaussian(mean->as<DiscreteStateSpace::StateType>()->value, stdDev) + 0.5);
62  space_->enforceBounds(state);
63 }
64 
66 {
67  return true;
68 }
69 
71 {
72  return 1;
73 }
74 
76 {
77  return upperBound_ - lowerBound_;
78 }
79 
81 {
82  if (state->as<StateType>()->value < lowerBound_)
83  state->as<StateType>()->value = lowerBound_;
84  else
85  if (state->as<StateType>()->value > upperBound_)
86  state->as<StateType>()->value = upperBound_;
87 }
88 
90 {
91  return state->as<StateType>()->value >= lowerBound_ && state->as<StateType>()->value <= upperBound_;
92 }
93 
94 void ompl::base::DiscreteStateSpace::copyState(State *destination, const State *source) const
95 {
96  destination->as<StateType>()->value = source->as<StateType>()->value;
97 }
98 
100 {
101  return sizeof(int);
102 }
103 
104 void ompl::base::DiscreteStateSpace::serialize(void *serialization, const State *state) const
105 {
106  memcpy(serialization, &state->as<StateType>()->value, sizeof(int));
107 }
108 
109 void ompl::base::DiscreteStateSpace::deserialize(State *state, const void *serialization) const
110 {
111  memcpy(&state->as<StateType>()->value, serialization, sizeof(int));
112 }
113 
114 double ompl::base::DiscreteStateSpace::distance(const State *state1, const State *state2) const
115 {
116  return abs(state1->as<StateType>()->value - state2->as<StateType>()->value);
117 }
118 
119 bool ompl::base::DiscreteStateSpace::equalStates(const State *state1, const State *state2) const
120 {
121  return state1->as<StateType>()->value == state2->as<StateType>()->value;
122 }
123 
124 void ompl::base::DiscreteStateSpace::interpolate(const State *from, const State *to, const double t, State *state) const
125 {
126  state->as<StateType>()->value = (int)floor(from->as<StateType>()->value +
127  (to->as<StateType>()->value - from->as<StateType>()->value) * t + 0.5);
128 }
129 
131 {
132  return StateSamplerPtr(new DiscreteStateSampler(this));
133 }
134 
136 {
137  return new StateType();
138 }
139 
141 {
142  delete static_cast<StateType*>(state);
143 }
144 
146 {
147  class DiscreteDefaultProjection : public ProjectionEvaluator
148  {
149  public:
150 
151  DiscreteDefaultProjection(const StateSpace *space) : ProjectionEvaluator(space)
152  {
153  }
154 
155  virtual unsigned int getDimension(void) const
156  {
157  return 1;
158  }
159 
160  virtual void defaultCellSizes(void)
161  {
162  cellSizes_.resize(1);
163  cellSizes_[0] = 1.0;
164  }
165 
166  virtual void project(const State *state, EuclideanProjection &projection) const
167  {
168  projection(0) = state->as<DiscreteStateSpace::StateType>()->value;
169  }
170  };
171 
172  registerDefaultProjection(ProjectionEvaluatorPtr(dynamic_cast<ProjectionEvaluator*>(new DiscreteDefaultProjection(this))));
173 }
174 
176 {
177  if (lowerBound_ > upperBound_)
178  throw Exception("Lower bound cannot be larger than upper bound for a discrete space");
180 }
181 
182 void ompl::base::DiscreteStateSpace::printState(const State *state, std::ostream &out) const
183 {
184  out << "DiscreteState [";
185  if (state)
186  out << state->as<StateType>()->value;
187  else
188  out << "NULL";
189  out << ']' << std::endl;
190 }
191 
193 {
194  out << "Discrete state space '" << getName() << "' with bounds [" << lowerBound_ << ", " << upperBound_ << "]" << std::endl;
195 }