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
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines