ompl/base/spaces/src/DiscreteStateSpace.cpp
00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2011, 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: Elizabeth Fudge */ 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() const 00066 { 00067 return true; 00068 } 00069 00070 unsigned int ompl::base::DiscreteStateSpace::getDimension() const 00071 { 00072 return 1; 00073 } 00074 00075 double ompl::base::DiscreteStateSpace::getMaximumExtent() const 00076 { 00077 return upperBound_ - lowerBound_; 00078 } 00079 00080 double ompl::base::DiscreteStateSpace::getMeasure() const 00081 { 00082 return upperBound_ - lowerBound_ + 1.0; 00083 } 00084 00085 void ompl::base::DiscreteStateSpace::enforceBounds(State *state) const 00086 { 00087 if (state->as<StateType>()->value < lowerBound_) 00088 state->as<StateType>()->value = lowerBound_; 00089 else 00090 if (state->as<StateType>()->value > upperBound_) 00091 state->as<StateType>()->value = upperBound_; 00092 } 00093 00094 bool ompl::base::DiscreteStateSpace::satisfiesBounds(const State *state) const 00095 { 00096 return state->as<StateType>()->value >= lowerBound_ && state->as<StateType>()->value <= upperBound_; 00097 } 00098 00099 void ompl::base::DiscreteStateSpace::copyState(State *destination, const State *source) const 00100 { 00101 destination->as<StateType>()->value = source->as<StateType>()->value; 00102 } 00103 00104 unsigned int ompl::base::DiscreteStateSpace::getSerializationLength() const 00105 { 00106 return sizeof(int); 00107 } 00108 00109 void ompl::base::DiscreteStateSpace::serialize(void *serialization, const State *state) const 00110 { 00111 memcpy(serialization, &state->as<StateType>()->value, sizeof(int)); 00112 } 00113 00114 void ompl::base::DiscreteStateSpace::deserialize(State *state, const void *serialization) const 00115 { 00116 memcpy(&state->as<StateType>()->value, serialization, sizeof(int)); 00117 } 00118 00119 double ompl::base::DiscreteStateSpace::distance(const State *state1, const State *state2) const 00120 { 00121 return abs(state1->as<StateType>()->value - state2->as<StateType>()->value); 00122 } 00123 00124 bool ompl::base::DiscreteStateSpace::equalStates(const State *state1, const State *state2) const 00125 { 00126 return state1->as<StateType>()->value == state2->as<StateType>()->value; 00127 } 00128 00129 void ompl::base::DiscreteStateSpace::interpolate(const State *from, const State *to, const double t, State *state) const 00130 { 00131 state->as<StateType>()->value = (int)floor(from->as<StateType>()->value + 00132 (to->as<StateType>()->value - from->as<StateType>()->value) * t + 0.5); 00133 } 00134 00135 ompl::base::StateSamplerPtr ompl::base::DiscreteStateSpace::allocDefaultStateSampler() const 00136 { 00137 return StateSamplerPtr(new DiscreteStateSampler(this)); 00138 } 00139 00140 ompl::base::State* ompl::base::DiscreteStateSpace::allocState() const 00141 { 00142 return new StateType(); 00143 } 00144 00145 void ompl::base::DiscreteStateSpace::freeState(State *state) const 00146 { 00147 delete static_cast<StateType*>(state); 00148 } 00149 00150 void ompl::base::DiscreteStateSpace::registerProjections() 00151 { 00152 class DiscreteDefaultProjection : public ProjectionEvaluator 00153 { 00154 public: 00155 00156 DiscreteDefaultProjection(const StateSpace *space) : ProjectionEvaluator(space) 00157 { 00158 } 00159 00160 virtual unsigned int getDimension() const 00161 { 00162 return 1; 00163 } 00164 00165 virtual void defaultCellSizes() 00166 { 00167 bounds_.resize(1); 00168 bounds_.low[0] = space_->as<DiscreteStateSpace>()->lowerBound_; 00169 bounds_.high[0] = space_->as<DiscreteStateSpace>()->upperBound_; 00170 cellSizes_.resize(1); 00171 cellSizes_[0] = 1.0; 00172 } 00173 00174 virtual void project(const State *state, EuclideanProjection &projection) const 00175 { 00176 projection(0) = state->as<DiscreteStateSpace::StateType>()->value; 00177 } 00178 }; 00179 00180 registerDefaultProjection(ProjectionEvaluatorPtr(dynamic_cast<ProjectionEvaluator*>(new DiscreteDefaultProjection(this)))); 00181 } 00182 00183 void ompl::base::DiscreteStateSpace::setup() 00184 { 00185 if (lowerBound_ > upperBound_) 00186 throw Exception("Lower bound cannot be larger than upper bound for a discrete space"); 00187 StateSpace::setup(); 00188 } 00189 00190 void ompl::base::DiscreteStateSpace::printState(const State *state, std::ostream &out) const 00191 { 00192 out << "DiscreteState ["; 00193 if (state) 00194 out << state->as<StateType>()->value; 00195 else 00196 out << "NULL"; 00197 out << ']' << std::endl; 00198 } 00199 00200 void ompl::base::DiscreteStateSpace::printSettings(std::ostream &out) const 00201 { 00202 out << "Discrete state space '" << getName() << "' with bounds [" << lowerBound_ << ", " << upperBound_ << "]" << std::endl; 00203 }