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 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines