.. _program_listing_file_src_ompl_base_ScopedState.h:

Program Listing for File ScopedState.h
======================================

|exhale_lsh| :ref:`Return to documentation for file <file_src_ompl_base_ScopedState.h>` (``src/ompl/base/ScopedState.h``)

.. |exhale_lsh| unicode:: U+021B0 .. UPWARDS ARROW WITH TIP LEFTWARDS

.. code-block:: cpp

   /*********************************************************************
    * Software License Agreement (BSD License)
    *
    *  Copyright (c) 2010, Rice University
    *  All rights reserved.
    *
    *  Redistribution and use in source and binary forms, with or without
    *  modification, are permitted provided that the following conditions
    *  are met:
    *
    *   * Redistributions of source code must retain the above copyright
    *     notice, this list of conditions and the following disclaimer.
    *   * Redistributions in binary form must reproduce the above
    *     copyright notice, this list of conditions and the following
    *     disclaimer in the documentation and/or other materials provided
    *     with the distribution.
    *   * Neither the name of the Rice University nor the names of its
    *     contributors may be used to endorse or promote products derived
    *     from this software without specific prior written permission.
    *
    *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
    *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
    *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
    *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
    *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
    *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
    *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
    *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
    *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
    *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
    *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
    *  POSSIBILITY OF SUCH DAMAGE.
    *********************************************************************/
   
   /* Author: Ioan Sucan */
   
   #ifndef OMPL_BASE_SCOPED_STATE_
   #define OMPL_BASE_SCOPED_STATE_
   
   #include "ompl/base/SpaceInformation.h"
   #include <boost/concept_check.hpp>
   #include <iostream>
   #include <utility>
   
   namespace ompl
   {
       namespace base
       {
           template <class T = StateSpace>
           class ScopedState
           {
               BOOST_CONCEPT_ASSERT((boost::Convertible<T *, StateSpace *>));
   
               BOOST_CONCEPT_ASSERT((boost::Convertible<typename T::StateType *, State *>));
   
           public:
               using StateType = typename T::StateType;
   
               explicit ScopedState(const SpaceInformationPtr &si) : space_(si->getStateSpace())
               {
                   State *s = space_->allocState();
   
                   // ideally, this should be a dynamic_cast and we
                   // should throw an exception in case of
                   // failure. However, RTTI may not be available across
                   // shared library boundaries, so we do not use it
                   state_ = static_cast<StateType *>(s);
               }
   
               explicit ScopedState(StateSpacePtr space) : space_(std::move(space))
               {
                   State *s = space_->allocState();
   
                   // ideally, this should be a dynamic_cast and we
                   // should throw an exception in case of
                   // failure. However, RTTI may not be available across
                   // shared library boundaries, so we do not use it
                   state_ = static_cast<StateType *>(s);
               }
   
               ScopedState(const ScopedState<T> &other) : space_(other.getSpace())
               {
                   State *s = space_->allocState();
                   state_ = static_cast<StateType *>(s);
                   space_->copyState(s, static_cast<const State *>(other.get()));
               }
   
               template <class O>
               ScopedState(const ScopedState<O> &other) : space_(other.getSpace())
               {
                   BOOST_CONCEPT_ASSERT((boost::Convertible<O *, StateSpace *>));
                   BOOST_CONCEPT_ASSERT((boost::Convertible<typename O::StateType *, State *>));
   
                   // ideally, we should use a dynamic_cast and throw an
                   // exception in case other.get() does not cast to
                   // const StateType*. However, RTTI may not be
                   // available across shared library boundaries, so we
                   // do not use it
   
                   State *s = space_->allocState();
                   state_ = static_cast<StateType *>(s);
                   space_->copyState(s, static_cast<const State *>(other.get()));
               }
   
               ScopedState(StateSpacePtr space, const State *state) : space_(std::move(space))
               {
                   State *s = space_->allocState();
                   space_->copyState(s, state);
   
                   // ideally, this should be a dynamic_cast and we
                   // should throw an exception in case of
                   // failure. However, RTTI may not be available across
                   // shared library boundaries, so we do not use it
                   state_ = static_cast<StateType *>(s);
               }
   
               ~ScopedState()
               {
                   space_->freeState(state_);
               }
   
               const StateSpacePtr &getSpace() const
               {
                   return space_;
               }
   
               ScopedState<T> &operator=(const ScopedState<T> &other)
               {
                   if (&other != this)
                   {
                       space_->freeState(state_);
                       space_ = other.getSpace();
   
                       State *s = space_->allocState();
                       state_ = static_cast<StateType *>(s);
                       space_->copyState(s, static_cast<const State *>(other.get()));
                   }
                   return *this;
               }
   
               ScopedState<T> &operator=(const State *other)
               {
                   if (other != static_cast<State *>(state_))
                   {
                       // ideally, we should use a dynamic_cast and throw an
                       // exception in case other does not cast to
                       // const StateType*. However, RTTI may not be
                       // available across shared library boundaries, so we
                       // do not use it
   
                       space_->copyState(static_cast<State *>(state_), other);
                   }
                   return *this;
               }
   
               ScopedState<T> &operator=(const State &other)
               {
                   if (&other != static_cast<State *>(state_))
                   {
                       // ideally, we should use a dynamic_cast and throw an
                       // exception in case &other does not cast to
                       // const StateType*. However, RTTI may not be
                       // available across shared library boundaries, so we
                       // do not use it
   
                       space_->copyState(static_cast<State *>(state_), &other);
                   }
                   return *this;
               }
   
               template <class O>
               ScopedState<T> &operator=(const ScopedState<O> &other)
               {
                   BOOST_CONCEPT_ASSERT((boost::Convertible<O *, StateSpace *>));
                   BOOST_CONCEPT_ASSERT((boost::Convertible<typename O::StateType *, State *>));
   
                   // ideally, we should use a dynamic_cast and throw an
                   // exception in case other.get() does not cast to
                   // const StateType*. However, RTTI may not be
                   // available across shared library boundaries, so we
                   // do not use it
   
                   if (reinterpret_cast<const void *>(&other) != reinterpret_cast<const void *>(this))
                   {
                       space_->freeState(state_);
                       space_ = other.getSpace();
   
                       State *s = space_->allocState();
                       state_ = static_cast<StateType *>(s);
                       space_->copyState(s, static_cast<const State *>(other.get()));
                   }
                   return *this;
               }
   
               ScopedState<T> &operator=(const std::vector<double> &reals)
               {
                   for (unsigned int i = 0; i < reals.size(); ++i)
                       if (double *va = space_->getValueAddressAtIndex(state_, i))
                           *va = reals[i];
                       else
                           break;
                   return *this;
               }
   
               ScopedState<T> &operator=(const double value)
               {
                   unsigned int index = 0;
                   while (double *va = space_->getValueAddressAtIndex(state_, index++))
                       *va = value;
                   return *this;
               }
   
               template <class O>
               bool operator==(const ScopedState<O> &other) const
               {
                   BOOST_CONCEPT_ASSERT((boost::Convertible<O *, StateSpace *>));
                   BOOST_CONCEPT_ASSERT((boost::Convertible<typename O::StateType *, State *>));
   
                   // ideally, we should use a dynamic_cast and throw an
                   // exception in case other.get() does not cast to
                   // const StateType*. However, RTTI may not be
                   // available across shared library boundaries, so we
                   // do not use it
   
                   return space_->equalStates(static_cast<const State *>(state_), static_cast<const State *>(other.get()));
               }
   
               template <class O>
               bool operator!=(const ScopedState<O> &other) const
               {
                   return !(*this == other);
               }
   
               const ScopedState<> operator[](const StateSpacePtr &s) const;
   
               double &operator[](const unsigned int index)
               {
                   double *val = space_->getValueAddressAtIndex(state_, index);
                   if (val == nullptr)
                       throw Exception("Index out of bounds");
                   return *val;
               }
   
               double operator[](const unsigned int index) const
               {
                   const double *val = space_->getValueAddressAtIndex(state_, index);
                   if (val == nullptr)
                       throw Exception("Index out of bounds");
                   return *val;
               }
   
               double &operator[](const std::string &name)
               {
                   const std::map<std::string, StateSpace::ValueLocation> &vm = space_->getValueLocationsByName();
                   auto it = vm.find(name);
                   if (it != vm.end())
                   {
                       double *val = space_->getValueAddressAtLocation(state_, it->second);
                       if (val != nullptr)
                           return *val;
                   }
                   throw Exception("Name '" + name + "' not known");
               }
   
               double operator[](const std::string &name) const
               {
                   const std::map<std::string, StateSpace::ValueLocation> &vm = space_->getValueLocationsByName();
                   auto it = vm.find(name);
                   if (it != vm.end())
                   {
                       const double *val = space_->getValueAddressAtLocation(state_, it->second);
                       if (val != nullptr)
                           return *val;
                   }
                   throw Exception("Name '" + name + "' not known");
               }
   
               template <class O>
               double distance(const ScopedState<O> &other) const
               {
                   BOOST_CONCEPT_ASSERT((boost::Convertible<O *, StateSpace *>));
                   BOOST_CONCEPT_ASSERT((boost::Convertible<typename O::StateType *, State *>));
                   return distance(other.get());
               }
   
               double distance(const State *state) const
               {
                   return space_->distance(static_cast<const State *>(state_), state);
               }
   
               void random()
               {
                   if (!sampler_)
                       sampler_ = space_->allocStateSampler();
                   sampler_->sampleUniform(state_);
               }
   
               void enforceBounds()
               {
                   space_->enforceBounds(state_);
               }
   
               bool satisfiesBounds() const
               {
                   return space_->satisfiesBounds(state_);
               }
   
               std::vector<double> reals() const
               {
                   std::vector<double> r;
                   unsigned int index = 0;
                   while (double *va = space_->getValueAddressAtIndex(state_, index++))
                       r.push_back(*va);
                   return r;
               }
   
               void print(std::ostream &out = std::cout) const
               {
                   space_->printState(state_, out);
               }
   
               StateType &operator*()
               {
                   return *state_;
               }
   
               const StateType &operator*() const
               {
                   return *state_;
               }
   
               StateType *operator->()
               {
                   return state_;
               }
   
               const StateType *operator->() const
               {
                   return state_;
               }
   
               StateType *get()
               {
                   return state_;
               }
   
               const StateType *get() const
               {
                   return state_;
               }
   
               StateType *operator()() const
               {
                   return state_;
               }
   
           private:
               StateSpacePtr space_;
               StateSamplerPtr sampler_;
               StateType *state_;
           };
   
           template <class T>
           inline std::ostream &operator<<(std::ostream &out, const ScopedState<T> &state)
           {
               state.print(out);
               return out;
           }
   
           template <class T, class Y>
           inline ScopedState<T> &operator<<(ScopedState<T> &to, const ScopedState<Y> &from)
           {
               copyStateData(to.getSpace(), to.get(), from.getSpace(), from.get());
               return to;
           }
   
           template <class T, class Y>
           inline const ScopedState<T> &operator>>(const ScopedState<T> &from, ScopedState<Y> &to)
           {
               copyStateData(to.getSpace(), to.get(), from.getSpace(), from.get());
               return from;
           }
   
           template <class T, class Y>
           inline const ScopedState<> operator^(const ScopedState<T> &a, const ScopedState<Y> &b)
           {
               ScopedState<> r(a.getSpace() + b.getSpace());
               return r << a << b;
           }
   
           template <class T>
           const ScopedState<> ScopedState<T>::operator[](const StateSpacePtr &s) const
           {
               ScopedState<> r(s);
               return r << *this;
           }
   
           using ScopedStatePtr = std::shared_ptr<ScopedState<>>;
       }  // namespace base
   }  // namespace ompl
   
   #endif