ScopedState.h

/*********************************************************************
 * 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