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 unsigned int ompl::base::TimeStateSpace::getSerializationLength(void) const
00110 {
00111 return sizeof(double);
00112 }
00113
00114 void ompl::base::TimeStateSpace::serialize(void *serialization, const State *state) const
00115 {
00116 memcpy(serialization, &state->as<StateType>()->position, sizeof(double));
00117 }
00118
00119 void ompl::base::TimeStateSpace::deserialize(State *state, const void *serialization) const
00120 {
00121 memcpy(&state->as<StateType>()->position, serialization, sizeof(double));
00122 }
00123
00124 double ompl::base::TimeStateSpace::distance(const State *state1, const State *state2) const
00125 {
00126 return fabs(state1->as<StateType>()->position - state2->as<StateType>()->position);
00127 }
00128
00129 bool ompl::base::TimeStateSpace::equalStates(const State *state1, const State *state2) const
00130 {
00131 return fabs(state1->as<StateType>()->position - state2->as<StateType>()->position) < std::numeric_limits<double>::epsilon() * 2.0;
00132 }
00133
00134 void ompl::base::TimeStateSpace::interpolate(const State *from, const State *to, const double t, State *state) const
00135 {
00136 state->as<StateType>()->position = from->as<StateType>()->position +
00137 (to->as<StateType>()->position - from->as<StateType>()->position) * t;
00138 }
00139
00140 ompl::base::StateSamplerPtr ompl::base::TimeStateSpace::allocDefaultStateSampler(void) const
00141 {
00142 return StateSamplerPtr(new TimeStateSampler(this));
00143 }
00144
00145 ompl::base::State* ompl::base::TimeStateSpace::allocState(void) const
00146 {
00147 return new StateType();
00148 }
00149
00150 void ompl::base::TimeStateSpace::freeState(State *state) const
00151 {
00152 delete static_cast<StateType*>(state);
00153 }
00154
00155 void ompl::base::TimeStateSpace::registerProjections(void)
00156 {
00157 class TimeDefaultProjection : public ProjectionEvaluator
00158 {
00159 public:
00160
00161 TimeDefaultProjection(const StateSpace *space) : ProjectionEvaluator(space)
00162 {
00163 }
00164
00165 virtual unsigned int getDimension(void) const
00166 {
00167 return 1;
00168 }
00169
00170 virtual void defaultCellSizes(void)
00171 {
00172 cellSizes_.resize(1);
00173 if (space_->as<TimeStateSpace>()->isBounded())
00174 cellSizes_[0] = (space_->as<TimeStateSpace>()->getMaxTimeBound() - space_->as<TimeStateSpace>()->getMinTimeBound()) / magic::PROJECTION_DIMENSION_SPLITS;
00175 else
00176 cellSizes_[0] = 1.0;
00177 }
00178
00179 virtual void project(const State *state, EuclideanProjection &projection) const
00180 {
00181 projection(0) = state->as<TimeStateSpace::StateType>()->position;
00182 }
00183 };
00184
00185 registerDefaultProjection(ProjectionEvaluatorPtr(dynamic_cast<ProjectionEvaluator*>(new TimeDefaultProjection(this))));
00186 }
00187
00188 double* ompl::base::TimeStateSpace::getValueAddressAtIndex(State *state, const unsigned int index) const
00189 {
00190 return index == 0 ? &(state->as<StateType>()->position) : NULL;
00191 }
00192
00193 void ompl::base::TimeStateSpace::printState(const State *state, std::ostream &out) const
00194 {
00195 out << "TimeState [";
00196 if (state)
00197 out << state->as<StateType>()->position;
00198 else
00199 out << "NULL";
00200 out << ']' << std::endl;
00201 }
00202
00203 void ompl::base::TimeStateSpace::printSettings(std::ostream &out) const
00204 {
00205 out << "Time state space '" << getName() << "'" << std::endl;
00206 }