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 #ifndef OMPL_BASE_SCOPED_STATE_
00038 #define OMPL_BASE_SCOPED_STATE_
00039
00040 #include "ompl/base/SpaceInformation.h"
00041 #include <boost/concept_check.hpp>
00042 #include <iostream>
00043
00044 namespace ompl
00045 {
00046 namespace base
00047 {
00048
00055 template<class T = StateSpace>
00056 class ScopedState
00057 {
00059 BOOST_CONCEPT_ASSERT((boost::Convertible<T*, StateSpace*>));
00060
00062 BOOST_CONCEPT_ASSERT((boost::Convertible<typename T::StateType*, State*>));
00063
00064 public:
00065
00067 typedef typename T::StateType StateType;
00068
00072 explicit
00073 ScopedState(const SpaceInformationPtr &si) : space_(si->getStateSpace())
00074 {
00075 State *s = space_->allocState();
00076
00077
00078
00079
00080
00081 state_ = static_cast<StateType*>(s);
00082 }
00083
00086 explicit
00087 ScopedState(const StateSpacePtr &space) : space_(space)
00088 {
00089 State *s = space_->allocState();
00090
00091
00092
00093
00094
00095 state_ = static_cast<StateType*>(s);
00096 }
00097
00099 ScopedState(const ScopedState<T> &other) : space_(other.getSpace())
00100 {
00101 State *s = space_->allocState();
00102 state_ = static_cast<StateType*>(s);
00103 space_->copyState(s, static_cast<const State*>(other.get()));
00104 }
00105
00107 template<class O>
00108 ScopedState(const ScopedState<O> &other) : space_(other.getSpace())
00109 {
00110 BOOST_CONCEPT_ASSERT((boost::Convertible<O*, StateSpace*>));
00111 BOOST_CONCEPT_ASSERT((boost::Convertible<typename O::StateType*, State*>));
00112
00113
00114
00115
00116
00117
00118
00119 State *s = space_->allocState();
00120 state_ = static_cast<StateType*>(s);
00121 space_->copyState(s, static_cast<const State*>(other.get()));
00122 }
00123
00126 ScopedState(const StateSpacePtr &space, const State *state) : space_(space)
00127 {
00128 State *s = space_->allocState();
00129 space_->copyState(s, state);
00130
00131
00132
00133
00134
00135 state_ = static_cast<StateType*>(s);
00136 }
00137
00139 ~ScopedState(void)
00140 {
00141 space_->freeState(state_);
00142 }
00143
00145 const StateSpacePtr& getSpace(void) const
00146 {
00147 return space_;
00148 }
00149
00151 ScopedState<T>& operator=(const ScopedState<T> &other)
00152 {
00153 if (&other != this)
00154 {
00155 space_->freeState(state_);
00156 space_ = other.getSpace();
00157
00158 State *s = space_->allocState();
00159 state_ = static_cast<StateType*>(s);
00160 space_->copyState(s, static_cast<const State*>(other.get()));
00161 }
00162 return *this;
00163 }
00164
00166 ScopedState<T>& operator=(const State *other)
00167 {
00168 if (other != static_cast<State*>(state_))
00169 {
00170
00171
00172
00173
00174
00175
00176 space_->copyState(static_cast<State*>(state_), other);
00177 }
00178 return *this;
00179 }
00180
00182 ScopedState<T>& operator=(const State &other)
00183 {
00184 if (&other != static_cast<State*>(state_))
00185 {
00186
00187
00188
00189
00190
00191
00192 space_->copyState(static_cast<State*>(state_), &other);
00193 }
00194 return *this;
00195 }
00196
00198 template<class O>
00199 ScopedState<T>& operator=(const ScopedState<O> &other)
00200 {
00201 BOOST_CONCEPT_ASSERT((boost::Convertible<O*, StateSpace*>));
00202 BOOST_CONCEPT_ASSERT((boost::Convertible<typename O::StateType*, State*>));
00203
00204
00205
00206
00207
00208
00209
00210 if (reinterpret_cast<const void*>(&other) != reinterpret_cast<const void*>(this))
00211 {
00212 space_->freeState(state_);
00213 space_ = other.getSpace();
00214
00215 State *s = space_->allocState();
00216 state_ = static_cast<StateType*>(s);
00217 space_->copyState(s, static_cast<const State*>(other.get()));
00218 }
00219 return *this;
00220 }
00221
00223 ScopedState<T>& operator=(const std::vector<double> &reals)
00224 {
00225 for (unsigned int i = 0 ; i < reals.size() ; ++i)
00226 if (double *va = space_->getValueAddressAtIndex(state_, i))
00227 *va = reals[i];
00228 else
00229 break;
00230 return *this;
00231 }
00232
00234 ScopedState<T>& operator=(const double value)
00235 {
00236 unsigned int index = 0;
00237 while (double *va = space_->getValueAddressAtIndex(state_, index++))
00238 *va = value;
00239 return *this;
00240 }
00241
00243 template<class O>
00244 bool operator==(const ScopedState<O> &other) const
00245 {
00246 BOOST_CONCEPT_ASSERT((boost::Convertible<O*, StateSpace*>));
00247 BOOST_CONCEPT_ASSERT((boost::Convertible<typename O::StateType*, State*>));
00248
00249
00250
00251
00252
00253
00254
00255 return space_->equalStates(static_cast<const State*>(state_), static_cast<const State*>(other.get()));
00256 }
00257
00259 template<class O>
00260 bool operator!=(const ScopedState<O> &other) const
00261 {
00262 return !(*this == other);
00263 }
00264
00270 ScopedState<> operator[](const StateSpacePtr &s) const;
00271
00273 double& operator[](const unsigned int index)
00274 {
00275 double *val = space_->getValueAddressAtIndex(state_, index);
00276 if (!val)
00277 throw Exception("Index out of bounds");
00278 return *val;
00279 }
00280
00282 double operator[](const unsigned int index) const
00283 {
00284 const double *val = space_->getValueAddressAtIndex(state_, index);
00285 if (!val)
00286 throw Exception("Index out of bounds");
00287 return *val;
00288 }
00289
00291 double& operator[](const std::string &name)
00292 {
00293 const std::map<std::string, StateSpace::ValueLocation> &vm = space_->getValueLocationsByName();
00294 std::map<std::string, StateSpace::ValueLocation>::const_iterator it = vm.find(name);
00295 if (it != vm.end())
00296 {
00297 double *val = space_->getValueAddressAtLocation(state_, it->second);
00298 if (val)
00299 return *val;
00300 }
00301 throw Exception("Name '" + name + "' not known");
00302 }
00303
00305 double operator[](const std::string &name) const
00306 {
00307 const std::map<std::string, StateSpace::ValueLocation> &vm = space_->getValueLocationsByName();
00308 std::map<std::string, StateSpace::ValueLocation>::const_iterator it = vm.find(name);
00309 if (it != vm.end())
00310 {
00311 const double *val = space_->getValueAddressAtLocation(state_, it->second);
00312 if (val)
00313 return *val;
00314 }
00315 throw Exception("Name '" + name + "' not known");
00316 }
00317
00319 template<class O>
00320 double distance(const ScopedState<O> &other) const
00321 {
00322 BOOST_CONCEPT_ASSERT((boost::Convertible<O*, StateSpace*>));
00323 BOOST_CONCEPT_ASSERT((boost::Convertible<typename O::StateType*, State*>));
00324 return distance(other.get());
00325 }
00326
00328 double distance(const State *state) const
00329 {
00330 return space_->distance(static_cast<const State*>(state_), state);
00331 }
00332
00334 void random(void)
00335 {
00336 if (!sampler_)
00337 sampler_ = space_->allocStateSampler();
00338 sampler_->sampleUniform(state_);
00339 }
00340
00342 void enforceBounds(void)
00343 {
00344 space_->enforceBounds(state_);
00345 }
00346
00348 bool satisfiesBounds(void) const
00349 {
00350 return space_->satisfiesBounds(state_);
00351 }
00352
00356 std::vector<double> reals(void) const
00357 {
00358 std::vector<double> r;
00359 unsigned int index = 0;
00360 while (double *va = space_->getValueAddressAtIndex(state_, index++))
00361 r.push_back(*va);
00362 return r;
00363 }
00364
00366 void print(std::ostream &out = std::cout) const
00367 {
00368 space_->printState(state_, out);
00369 }
00370
00372 StateType& operator*(void)
00373 {
00374 return *state_;
00375 }
00376
00378 const StateType& operator*(void) const
00379 {
00380 return *state_;
00381 }
00382
00384 StateType* operator->(void)
00385 {
00386 return state_;
00387 }
00388
00390 const StateType* operator->(void) const
00391 {
00392 return state_;
00393 }
00394
00396 StateType* get(void)
00397 {
00398 return state_;
00399 }
00400
00402 const StateType* get(void) const
00403 {
00404 return state_;
00405 }
00406
00408 StateType* operator()(void) const
00409 {
00410 return state_;
00411 }
00412
00413 private:
00414
00415 StateSpacePtr space_;
00416 StateSamplerPtr sampler_;
00417 StateType *state_;
00418 };
00419
00495 template<class T>
00496 inline
00497 std::ostream& operator<<(std::ostream &out, const ScopedState<T> &state)
00498 {
00499 state.print(out);
00500 return out;
00501 }
00502
00511 template<class T, class Y>
00512 inline
00513 ScopedState<T>& operator<<(ScopedState<T> &to, const ScopedState<Y> &from)
00514 {
00515 copyStateData(to.getSpace(), to.get(), from.getSpace(), from.get());
00516 return to;
00517 }
00518
00527 template<class T, class Y>
00528 inline
00529 const ScopedState<T>& operator>>(const ScopedState<T> &from, ScopedState<Y> &to)
00530 {
00531 copyStateData(to.getSpace(), to.get(), from.getSpace(), from.get());
00532 return from;
00533 }
00534
00539 template<class T, class Y>
00540 inline
00541 ScopedState<> operator^(const ScopedState<T> &a, const ScopedState<Y> &b)
00542 {
00543 ScopedState<> r(a.getSpace() + b.getSpace());
00544 return r << a << b;
00545 }
00546
00549 template<class T>
00550 ScopedState<> ScopedState<T>::operator[](const StateSpacePtr &s) const
00551 {
00552 ScopedState<> r(s);
00553 return r << *this;
00554 }
00555
00557 typedef boost::shared_ptr< ScopedState<> > ScopedStatePtr;
00558 }
00559 }
00560
00561 #endif