ompl/base/ScopedState.h
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 #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 // ideally, this should be a dynamic_cast and we 00078 // should throw an exception in case of 00079 // failure. However, RTTI may not be available across 00080 // shared library boundaries, so we do not use it 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 // ideally, this should be a dynamic_cast and we 00092 // should throw an exception in case of 00093 // failure. However, RTTI may not be available across 00094 // shared library boundaries, so we do not use it 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 // ideally, we should use a dynamic_cast and throw an 00114 // exception in case other.get() does not cast to 00115 // const StateType*. However, RTTI may not be 00116 // available across shared library boundaries, so we 00117 // do not use it 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 // ideally, this should be a dynamic_cast and we 00132 // should throw an exception in case of 00133 // failure. However, RTTI may not be available across 00134 // shared library boundaries, so we do not use it 00135 state_ = static_cast<StateType*>(s); 00136 } 00137 00139 ~ScopedState() 00140 { 00141 space_->freeState(state_); 00142 } 00143 00145 const StateSpacePtr& getSpace() 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 // ideally, we should use a dynamic_cast and throw an 00171 // exception in case other does not cast to 00172 // const StateType*. However, RTTI may not be 00173 // available across shared library boundaries, so we 00174 // do not use it 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 // ideally, we should use a dynamic_cast and throw an 00187 // exception in case &other does not cast to 00188 // const StateType*. However, RTTI may not be 00189 // available across shared library boundaries, so we 00190 // do not use it 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 // ideally, we should use a dynamic_cast and throw an 00205 // exception in case other.get() does not cast to 00206 // const StateType*. However, RTTI may not be 00207 // available across shared library boundaries, so we 00208 // do not use it 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 // ideally, we should use a dynamic_cast and throw an 00250 // exception in case other.get() does not cast to 00251 // const StateType*. However, RTTI may not be 00252 // available across shared library boundaries, so we 00253 // do not use it 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 const 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() 00335 { 00336 if (!sampler_) 00337 sampler_ = space_->allocStateSampler(); 00338 sampler_->sampleUniform(state_); 00339 } 00340 00342 void enforceBounds() 00343 { 00344 space_->enforceBounds(state_); 00345 } 00346 00348 bool satisfiesBounds() const 00349 { 00350 return space_->satisfiesBounds(state_); 00351 } 00352 00356 std::vector<double> reals() 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*() 00373 { 00374 return *state_; 00375 } 00376 00378 const StateType& operator*() const 00379 { 00380 return *state_; 00381 } 00382 00384 StateType* operator->() 00385 { 00386 return state_; 00387 } 00388 00390 const StateType* operator->() const 00391 { 00392 return state_; 00393 } 00394 00396 StateType* get() 00397 { 00398 return state_; 00399 } 00400 00402 const StateType* get() const 00403 { 00404 return state_; 00405 } 00406 00408 StateType* operator()() 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 const 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 const 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