All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator
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(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                     // 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             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             template<class O>
00292             double distance(const ScopedState<O> &other) const
00293             {
00294                 BOOST_CONCEPT_ASSERT((boost::Convertible<O*, StateSpace*>));
00295                 BOOST_CONCEPT_ASSERT((boost::Convertible<typename O::StateType*, State*>));
00296                 return distance(other.get());
00297             }
00298 
00300             double distance(const State *state) const
00301             {
00302                 return space_->distance(static_cast<const State*>(state_), state);
00303             }
00304 
00306             void random(void)
00307             {
00308                 if (!sampler_)
00309                     sampler_ = space_->allocStateSampler();
00310                 sampler_->sampleUniform(state_);
00311             }
00312 
00314             void enforceBounds(void)
00315             {
00316                 space_->enforceBounds(state_);
00317             }
00318 
00320             bool satisfiesBounds(void) const
00321             {
00322                 return space_->satisfiesBounds(state_);
00323             }
00324 
00328             std::vector<double> reals(void) const
00329             {
00330                 std::vector<double> r;
00331                 unsigned int index = 0;
00332                 while (double *va = space_->getValueAddressAtIndex(state_, index++))
00333                     r.push_back(*va);
00334                 return r;
00335             }
00336 
00338             void print(std::ostream &out = std::cout) const
00339             {
00340                 space_->printState(state_, out);
00341             }
00342 
00344             StateType& operator*(void)
00345             {
00346                 return *state_;
00347             }
00348 
00350             const StateType& operator*(void) const
00351             {
00352                 return *state_;
00353             }
00354 
00356             StateType* operator->(void)
00357             {
00358                 return state_;
00359             }
00360 
00362             const StateType* operator->(void) const
00363             {
00364                 return state_;
00365             }
00366 
00368             StateType* get(void)
00369             {
00370                 return state_;
00371             }
00372 
00374             const StateType* get(void) const
00375             {
00376                 return state_;
00377             }
00378 
00380             StateType* operator()(void) const
00381             {
00382                 return state_;
00383             }
00384 
00385         private:
00386 
00387             StateSpacePtr         space_;
00388             StateSamplerPtr  sampler_;
00389             StateType               *state_;
00390         };
00391 
00467         template<class T>
00468         inline
00469         std::ostream& operator<<(std::ostream &out, const ScopedState<T> &state)
00470         {
00471             state.print(out);
00472             return out;
00473         }
00474 
00483         template<class T, class Y>
00484         inline
00485         ScopedState<T>& operator<<(ScopedState<T> &to, const ScopedState<Y> &from)
00486         {
00487             copyStateData(to.getSpace(), to.get(), from.getSpace(), from.get());
00488             return to;
00489         }
00490 
00499         template<class T, class Y>
00500         inline
00501         const ScopedState<T>& operator>>(const ScopedState<T> &from, ScopedState<Y> &to)
00502         {
00503             copyStateData(to.getSpace(), to.get(), from.getSpace(), from.get());
00504             return from;
00505         }
00506 
00511         template<class T, class Y>
00512         inline
00513         ScopedState<> operator^(const ScopedState<T> &a, const ScopedState<Y> &b)
00514         {
00515             ScopedState<> r(a.getSpace() + b.getSpace());
00516             return r << a << b;
00517         }
00518 
00521         template<class T>
00522         ScopedState<> ScopedState<T>::operator[](const StateSpacePtr &s) const
00523         {
00524             ScopedState<> r(s);
00525             return r << *this;
00526         }
00527 
00529         typedef boost::shared_ptr< ScopedState<> > ScopedStatePtr;
00530     }
00531 }
00532 
00533 #endif