37 #ifndef OMPL_BASE_SCOPED_STATE_
38 #define OMPL_BASE_SCOPED_STATE_
40 #include "ompl/base/SpaceInformation.h"
41 #include <boost/concept_check.hpp>
55 template<
class T = StateSpace>
59 BOOST_CONCEPT_ASSERT((boost::Convertible<T*, StateSpace*>));
62 BOOST_CONCEPT_ASSERT((boost::Convertible<typename T::StateType*, State*>));
75 State *s = space_->allocState();
89 State *s = space_->allocState();
101 State *s = space_->allocState();
103 space_->copyState(s, static_cast<const State*>(other.
get()));
110 BOOST_CONCEPT_ASSERT((boost::Convertible<O*, StateSpace*>));
111 BOOST_CONCEPT_ASSERT((boost::Convertible<typename O::StateType*, State*>));
119 State *s = space_->allocState();
121 space_->copyState(s, static_cast<const State*>(other.
get()));
128 State *s = space_->allocState();
129 space_->copyState(s, state);
141 space_->freeState(state_);
155 space_->freeState(state_);
158 State *s = space_->allocState();
160 space_->copyState(s, static_cast<const State*>(other.
get()));
168 if (other != static_cast<State*>(state_))
176 space_->copyState(static_cast<State*>(state_), other);
184 if (&other != static_cast<State*>(state_))
192 space_->copyState(static_cast<State*>(state_), &other);
201 BOOST_CONCEPT_ASSERT((boost::Convertible<O*, StateSpace*>));
202 BOOST_CONCEPT_ASSERT((boost::Convertible<typename O::StateType*, State*>));
210 if (reinterpret_cast<const void*>(&other) != reinterpret_cast<const void*>(
this))
212 space_->freeState(state_);
215 State *s = space_->allocState();
217 space_->copyState(s, static_cast<const State*>(other.
get()));
225 for (
unsigned int i = 0 ; i < reals.size() ; ++i)
226 if (
double *va = space_->getValueAddressAtIndex(state_, i))
236 unsigned int index = 0;
237 while (
double *va = space_->getValueAddressAtIndex(state_, index++))
246 BOOST_CONCEPT_ASSERT((boost::Convertible<O*, StateSpace*>));
247 BOOST_CONCEPT_ASSERT((boost::Convertible<typename O::StateType*, State*>));
255 return space_->equalStates(static_cast<const State*>(state_), static_cast<const State*>(other.
get()));
262 return !(*
this == other);
275 double *val = space_->getValueAddressAtIndex(state_, index);
284 const double *val = space_->getValueAddressAtIndex(state_, index);
293 const std::map<std::string, StateSpace::ValueLocation> &vm = space_->getValueLocationsByName();
294 std::map<std::string, StateSpace::ValueLocation>::const_iterator it = vm.find(name);
297 double *val = space_->getValueAddressAtLocation(state_, it->second);
301 throw Exception(
"Name '" + name +
"' not known");
307 const std::map<std::string, StateSpace::ValueLocation> &vm = space_->getValueLocationsByName();
308 std::map<std::string, StateSpace::ValueLocation>::const_iterator it = vm.find(name);
311 const double *val = space_->getValueAddressAtLocation(state_, it->second);
315 throw Exception(
"Name '" + name +
"' not known");
322 BOOST_CONCEPT_ASSERT((boost::Convertible<O*, StateSpace*>));
323 BOOST_CONCEPT_ASSERT((boost::Convertible<typename O::StateType*, State*>));
330 return space_->distance(static_cast<const State*>(state_), state);
337 sampler_ = space_->allocStateSampler();
338 sampler_->sampleUniform(state_);
344 space_->enforceBounds(state_);
350 return space_->satisfiesBounds(state_);
356 std::vector<double>
reals(
void)
const
358 std::vector<double> r;
359 unsigned int index = 0;
360 while (
double *va = space_->getValueAddressAtIndex(state_, index++))
366 void print(std::ostream &out = std::cout)
const
368 space_->printState(state_, out);
497 std::ostream& operator<<(std::ostream &out, const ScopedState<T> &state)
511 template<
class T,
class Y>
515 copyStateData(to.getSpace(), to.get(), from.getSpace(), from.get());
527 template<
class T,
class Y>
539 template<
class T,
class Y>