00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include "ompl/base/spaces/DiscreteStateSpace.h"
00038 #include "ompl/util/Exception.h"
00039 #include <limits>
00040 #include <cstdlib>
00041
00042 void ompl::base::DiscreteStateSampler::sampleUniform(State *state)
00043 {
00044 state->as<DiscreteStateSpace::StateType>()->value =
00045 rng_.uniformInt(space_->as<DiscreteStateSpace>()->getLowerBound(),
00046 space_->as<DiscreteStateSpace>()->getUpperBound());
00047 }
00048
00049 void ompl::base::DiscreteStateSampler::sampleUniformNear(State *state, const State *near, const double distance)
00050 {
00051 const int d = (int)floor(distance + 0.5);
00052 state->as<DiscreteStateSpace::StateType>()->value =
00053 rng_.uniformInt(near->as<DiscreteStateSpace::StateType>()->value - d,
00054 near->as<DiscreteStateSpace::StateType>()->value + d);
00055 space_->enforceBounds(state);
00056 }
00057
00058 void ompl::base::DiscreteStateSampler::sampleGaussian(State *state, const State *mean, const double stdDev)
00059 {
00060 state->as<DiscreteStateSpace::StateType>()->value =
00061 (int)floor(rng_.gaussian(mean->as<DiscreteStateSpace::StateType>()->value, stdDev) + 0.5);
00062 space_->enforceBounds(state);
00063 }
00064
00065 bool ompl::base::DiscreteStateSpace::isDiscrete(void) const
00066 {
00067 return true;
00068 }
00069
00070 unsigned int ompl::base::DiscreteStateSpace::getDimension(void) const
00071 {
00072 return 1;
00073 }
00074
00075 double ompl::base::DiscreteStateSpace::getMaximumExtent(void) const
00076 {
00077 return upperBound_ - lowerBound_;
00078 }
00079
00080 void ompl::base::DiscreteStateSpace::enforceBounds(State *state) const
00081 {
00082 if (state->as<StateType>()->value < lowerBound_)
00083 state->as<StateType>()->value = lowerBound_;
00084 else
00085 if (state->as<StateType>()->value > upperBound_)
00086 state->as<StateType>()->value = upperBound_;
00087 }
00088
00089 bool ompl::base::DiscreteStateSpace::satisfiesBounds(const State *state) const
00090 {
00091 return state->as<StateType>()->value >= lowerBound_ && state->as<StateType>()->value <= upperBound_;
00092 }
00093
00094 void ompl::base::DiscreteStateSpace::copyState(State *destination, const State *source) const
00095 {
00096 destination->as<StateType>()->value = source->as<StateType>()->value;
00097 }
00098
00099 unsigned int ompl::base::DiscreteStateSpace::getSerializationLength(void) const
00100 {
00101 return sizeof(int);
00102 }
00103
00104 void ompl::base::DiscreteStateSpace::serialize(void *serialization, const State *state) const
00105 {
00106 memcpy(serialization, &state->as<StateType>()->value, sizeof(int));
00107 }
00108
00109 void ompl::base::DiscreteStateSpace::deserialize(State *state, const void *serialization) const
00110 {
00111 memcpy(&state->as<StateType>()->value, serialization, sizeof(int));
00112 }
00113
00114 double ompl::base::DiscreteStateSpace::distance(const State *state1, const State *state2) const
00115 {
00116 return abs(state1->as<StateType>()->value - state2->as<StateType>()->value);
00117 }
00118
00119 bool ompl::base::DiscreteStateSpace::equalStates(const State *state1, const State *state2) const
00120 {
00121 return state1->as<StateType>()->value == state2->as<StateType>()->value;
00122 }
00123
00124 void ompl::base::DiscreteStateSpace::interpolate(const State *from, const State *to, const double t, State *state) const
00125 {
00126 state->as<StateType>()->value = (int)floor(from->as<StateType>()->value +
00127 (to->as<StateType>()->value - from->as<StateType>()->value) * t + 0.5);
00128 }
00129
00130 ompl::base::StateSamplerPtr ompl::base::DiscreteStateSpace::allocDefaultStateSampler(void) const
00131 {
00132 return StateSamplerPtr(new DiscreteStateSampler(this));
00133 }
00134
00135 ompl::base::State* ompl::base::DiscreteStateSpace::allocState(void) const
00136 {
00137 return new StateType();
00138 }
00139
00140 void ompl::base::DiscreteStateSpace::freeState(State *state) const
00141 {
00142 delete static_cast<StateType*>(state);
00143 }
00144
00145 void ompl::base::DiscreteStateSpace::registerProjections(void)
00146 {
00147 class DiscreteDefaultProjection : public ProjectionEvaluator
00148 {
00149 public:
00150
00151 DiscreteDefaultProjection(const StateSpace *space) : ProjectionEvaluator(space)
00152 {
00153 }
00154
00155 virtual unsigned int getDimension(void) const
00156 {
00157 return 1;
00158 }
00159
00160 virtual void defaultCellSizes(void)
00161 {
00162 cellSizes_.resize(1);
00163 cellSizes_[0] = 1.0;
00164 }
00165
00166 virtual void project(const State *state, EuclideanProjection &projection) const
00167 {
00168 projection(0) = state->as<DiscreteStateSpace::StateType>()->value;
00169 }
00170 };
00171
00172 registerDefaultProjection(ProjectionEvaluatorPtr(dynamic_cast<ProjectionEvaluator*>(new DiscreteDefaultProjection(this))));
00173 }
00174
00175 void ompl::base::DiscreteStateSpace::setup(void)
00176 {
00177 if (lowerBound_ > upperBound_)
00178 throw Exception("Lower bound cannot be larger than upper bound for a discrete space");
00179 StateSpace::setup();
00180 }
00181
00182 void ompl::base::DiscreteStateSpace::printState(const State *state, std::ostream &out) const
00183 {
00184 out << "DiscreteState [";
00185 if (state)
00186 out << state->as<StateType>()->value;
00187 else
00188 out << "NULL";
00189 out << ']' << std::endl;
00190 }
00191
00192 void ompl::base::DiscreteStateSpace::printSettings(std::ostream &out) const
00193 {
00194 out << "Discrete state space '" << getName() << "' with bounds [" << lowerBound_ << ", " << upperBound_ << "]" << std::endl;
00195 }