ompl/base/spaces/src/TimeStateSpace.cpp
00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2010, 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: Ioan Sucan */ 00036 00037 #include "ompl/base/spaces/TimeStateSpace.h" 00038 #include "ompl/util/Exception.h" 00039 #include "ompl/tools/config/MagicConstants.h" 00040 #include <limits> 00041 00042 void ompl::base::TimeStateSampler::sampleUniform(State *state) 00043 { 00044 if (space_->as<TimeStateSpace>()->isBounded()) 00045 state->as<TimeStateSpace::StateType>()->position = rng_.uniformReal(space_->as<TimeStateSpace>()->getMinTimeBound(), 00046 space_->as<TimeStateSpace>()->getMaxTimeBound()); 00047 else 00048 state->as<TimeStateSpace::StateType>()->position = 0.0; 00049 } 00050 00051 void ompl::base::TimeStateSampler::sampleUniformNear(State *state, const State *near, const double distance) 00052 { 00053 state->as<TimeStateSpace::StateType>()->position = 00054 rng_.uniformReal(near->as<TimeStateSpace::StateType>()->position - distance, 00055 near->as<TimeStateSpace::StateType>()->position + distance); 00056 space_->enforceBounds(state); 00057 } 00058 00059 void ompl::base::TimeStateSampler::sampleGaussian(State *state, const State *mean, const double stdDev) 00060 { 00061 state->as<TimeStateSpace::StateType>()->position = 00062 rng_.gaussian(mean->as<TimeStateSpace::StateType>()->position, stdDev); 00063 space_->enforceBounds(state); 00064 } 00065 00066 unsigned int ompl::base::TimeStateSpace::getDimension() const 00067 { 00068 return 1; 00069 } 00070 00071 void ompl::base::TimeStateSpace::setBounds(double minTime, double maxTime) 00072 { 00073 if (minTime > maxTime) 00074 throw Exception("The maximum position in time cannot be before the minimum position in time"); 00075 00076 minTime_ = minTime; 00077 maxTime_ = maxTime; 00078 bounded_ = true; 00079 } 00080 00081 double ompl::base::TimeStateSpace::getMaximumExtent() const 00082 { 00083 return bounded_ ? maxTime_ - minTime_ : 1.0; 00084 } 00085 00086 double ompl::base::TimeStateSpace::getMeasure() const 00087 { 00088 return getMaximumExtent(); 00089 } 00090 00091 void ompl::base::TimeStateSpace::enforceBounds(State *state) const 00092 { 00093 if (bounded_) 00094 { 00095 if (state->as<StateType>()->position > maxTime_) 00096 state->as<StateType>()->position = maxTime_; 00097 else 00098 if (state->as<StateType>()->position < minTime_) 00099 state->as<StateType>()->position = minTime_; 00100 } 00101 } 00102 00103 bool ompl::base::TimeStateSpace::satisfiesBounds(const State *state) const 00104 { 00105 return !bounded_ || (state->as<StateType>()->position >= minTime_ - std::numeric_limits<double>::epsilon() && 00106 state->as<StateType>()->position <= maxTime_ + std::numeric_limits<double>::epsilon()); 00107 } 00108 00109 void ompl::base::TimeStateSpace::copyState(State *destination, const State *source) const 00110 { 00111 destination->as<StateType>()->position = source->as<StateType>()->position; 00112 } 00113 00114 unsigned int ompl::base::TimeStateSpace::getSerializationLength() const 00115 { 00116 return sizeof(double); 00117 } 00118 00119 void ompl::base::TimeStateSpace::serialize(void *serialization, const State *state) const 00120 { 00121 memcpy(serialization, &state->as<StateType>()->position, sizeof(double)); 00122 } 00123 00124 void ompl::base::TimeStateSpace::deserialize(State *state, const void *serialization) const 00125 { 00126 memcpy(&state->as<StateType>()->position, serialization, sizeof(double)); 00127 } 00128 00129 double ompl::base::TimeStateSpace::distance(const State *state1, const State *state2) const 00130 { 00131 return fabs(state1->as<StateType>()->position - state2->as<StateType>()->position); 00132 } 00133 00134 bool ompl::base::TimeStateSpace::equalStates(const State *state1, const State *state2) const 00135 { 00136 return fabs(state1->as<StateType>()->position - state2->as<StateType>()->position) < std::numeric_limits<double>::epsilon() * 2.0; 00137 } 00138 00139 void ompl::base::TimeStateSpace::interpolate(const State *from, const State *to, const double t, State *state) const 00140 { 00141 state->as<StateType>()->position = from->as<StateType>()->position + 00142 (to->as<StateType>()->position - from->as<StateType>()->position) * t; 00143 } 00144 00145 ompl::base::StateSamplerPtr ompl::base::TimeStateSpace::allocDefaultStateSampler() const 00146 { 00147 return StateSamplerPtr(new TimeStateSampler(this)); 00148 } 00149 00150 ompl::base::State* ompl::base::TimeStateSpace::allocState() const 00151 { 00152 return new StateType(); 00153 } 00154 00155 void ompl::base::TimeStateSpace::freeState(State *state) const 00156 { 00157 delete static_cast<StateType*>(state); 00158 } 00159 00160 void ompl::base::TimeStateSpace::registerProjections() 00161 { 00162 class TimeDefaultProjection : public ProjectionEvaluator 00163 { 00164 public: 00165 00166 TimeDefaultProjection(const StateSpace *space) : ProjectionEvaluator(space) 00167 { 00168 } 00169 00170 virtual unsigned int getDimension() const 00171 { 00172 return 1; 00173 } 00174 00175 virtual void defaultCellSizes() 00176 { 00177 cellSizes_.resize(1); 00178 if (space_->as<TimeStateSpace>()->isBounded()) 00179 { 00180 bounds_.resize(1); 00181 bounds_.low[0] = space_->as<TimeStateSpace>()->getMinTimeBound(); 00182 bounds_.high[0] = space_->as<TimeStateSpace>()->getMaxTimeBound(); 00183 cellSizes_[0] = bounds_.getDifference()[0] / magic::PROJECTION_DIMENSION_SPLITS; 00184 } 00185 else 00186 cellSizes_[0] = 1.0; 00187 } 00188 00189 virtual void project(const State *state, EuclideanProjection &projection) const 00190 { 00191 projection(0) = state->as<TimeStateSpace::StateType>()->position; 00192 } 00193 }; 00194 00195 registerDefaultProjection(ProjectionEvaluatorPtr(dynamic_cast<ProjectionEvaluator*>(new TimeDefaultProjection(this)))); 00196 } 00197 00198 double* ompl::base::TimeStateSpace::getValueAddressAtIndex(State *state, const unsigned int index) const 00199 { 00200 return index == 0 ? &(state->as<StateType>()->position) : NULL; 00201 } 00202 00203 void ompl::base::TimeStateSpace::printState(const State *state, std::ostream &out) const 00204 { 00205 out << "TimeState ["; 00206 if (state) 00207 out << state->as<StateType>()->position; 00208 else 00209 out << "NULL"; 00210 out << ']' << std::endl; 00211 } 00212 00213 void ompl::base::TimeStateSpace::printSettings(std::ostream &out) const 00214 { 00215 out << "Time state space '" << getName() << "'" << std::endl; 00216 }