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/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(void) 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(void) const
00082 {
00083 return bounded_ ? maxTime_ - minTime_ : 1.0;
00084 }
00085
00086 void ompl::base::TimeStateSpace::enforceBounds(State *state) const
00087 {
00088 if (bounded_)
00089 {
00090 if (state->as<StateType>()->position > maxTime_)
00091 state->as<StateType>()->position = maxTime_;
00092 else
00093 if (state->as<StateType>()->position < minTime_)
00094 state->as<StateType>()->position = minTime_;
00095 }
00096 }
00097
00098 bool ompl::base::TimeStateSpace::satisfiesBounds(const State *state) const
00099 {
00100 return !bounded_ || (state->as<StateType>()->position >= minTime_ - std::numeric_limits<double>::epsilon() &&
00101 state->as<StateType>()->position <= maxTime_ + std::numeric_limits<double>::epsilon());
00102 }
00103
00104 void ompl::base::TimeStateSpace::copyState(State *destination, const State *source) const
00105 {
00106 destination->as<StateType>()->position = source->as<StateType>()->position;
00107 }
00108
00109 double ompl::base::TimeStateSpace::distance(const State *state1, const State *state2) const
00110 {
00111 return fabs(state1->as<StateType>()->position - state2->as<StateType>()->position);
00112 }
00113
00114 bool ompl::base::TimeStateSpace::equalStates(const State *state1, const State *state2) const
00115 {
00116 return fabs(state1->as<StateType>()->position - state2->as<StateType>()->position) < std::numeric_limits<double>::epsilon() * 2.0;
00117 }
00118
00119 void ompl::base::TimeStateSpace::interpolate(const State *from, const State *to, const double t, State *state) const
00120 {
00121 state->as<StateType>()->position = from->as<StateType>()->position +
00122 (to->as<StateType>()->position - from->as<StateType>()->position) * t;
00123 }
00124
00125 ompl::base::StateSamplerPtr ompl::base::TimeStateSpace::allocStateSampler(void) const
00126 {
00127 return StateSamplerPtr(new TimeStateSampler(this));
00128 }
00129
00130 ompl::base::State* ompl::base::TimeStateSpace::allocState(void) const
00131 {
00132 return new StateType();
00133 }
00134
00135 void ompl::base::TimeStateSpace::freeState(State *state) const
00136 {
00137 delete static_cast<StateType*>(state);
00138 }
00139
00140 void ompl::base::TimeStateSpace::registerProjections(void)
00141 {
00142 class TimeDefaultProjection : public ProjectionEvaluator
00143 {
00144 public:
00145
00146 TimeDefaultProjection(const StateSpace *space) : ProjectionEvaluator(space)
00147 {
00148 }
00149
00150 virtual unsigned int getDimension(void) const
00151 {
00152 return 1;
00153 }
00154
00155 virtual void defaultCellSizes(void)
00156 {
00157 cellSizes_.resize(1);
00158 if (space_->as<TimeStateSpace>()->isBounded())
00159 cellSizes_[0] = (space_->as<TimeStateSpace>()->getMaxTimeBound() - space_->as<TimeStateSpace>()->getMinTimeBound()) / magic::PROJECTION_DIMENSION_SPLITS;
00160 else
00161 cellSizes_[0] = 1.0;
00162 }
00163
00164 virtual void project(const State *state, EuclideanProjection &projection) const
00165 {
00166 projection(0) = state->as<TimeStateSpace::StateType>()->position;
00167 }
00168 };
00169
00170 registerDefaultProjection(ProjectionEvaluatorPtr(dynamic_cast<ProjectionEvaluator*>(new TimeDefaultProjection(this))));
00171 }
00172
00173 double* ompl::base::TimeStateSpace::getValueAddressAtIndex(State *state, const unsigned int index) const
00174 {
00175 return index == 0 ? &(state->as<StateType>()->position) : NULL;
00176 }
00177
00178 void ompl::base::TimeStateSpace::printState(const State *state, std::ostream &out) const
00179 {
00180 out << "TimeState [";
00181 if (state)
00182 out << state->as<StateType>()->position;
00183 else
00184 out << "NULL";
00185 out << ']' << std::endl;
00186 }
00187
00188 void ompl::base::TimeStateSpace::printSettings(std::ostream &out) const
00189 {
00190 out << "Time state space '" << getName() << "'" << std::endl;
00191 }