ScopedState.h

Go to the documentation of this file.

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

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:

            typedef typename T::StateType 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(const StateSpacePtr &space) : space_(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(const StateSpacePtr &space, const State *state) : space_(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(void)
            {
                space_->freeState(state_);
            }

            const StateSpacePtr& getSpace(void) 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);
            }

            ScopedState<> operator[](const StateSpacePtr &s) const;

            double& operator[](const unsigned int index)
            {
                double *val = space_->getValueAddressAtIndex(state_, index);
                if (!val)
                    throw Exception("Index out of bounds");
                return *val;
            }

            double operator[](const unsigned int index) const
            {
                const double *val = space_->getValueAddressAtIndex(state_, index);
                if (!val)
                    throw Exception("Index out of bounds");
                return *val;
            }

            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(void)
            {
                if (!sampler_)
                    sampler_ = space_->allocStateSampler();
                sampler_->sampleUniform(state_);
            }

            void enforceBounds(void)
            {
                space_->enforceBounds(state_);
            }

            bool satisfiesBounds(void) const
            {
                return space_->satisfiesBounds(state_);
            }

            std::vector<double> reals(void) 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*(void)
            {
                return *state_;
            }

            const StateType& operator*(void) const
            {
                return *state_;
            }

            StateType* operator->(void)
            {
                return state_;
            }

            const StateType* operator->(void) const
            {
                return state_;
            }

            StateType* get(void)
            {
                return state_;
            }

            const StateType* get(void) const
            {
                return state_;
            }

            StateType* operator()(void) 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
        ScopedState<> operator^(const ScopedState<T> &a, const ScopedState<Y> &b)
        {
            ScopedState<> r(a.getSpace() + b.getSpace());
            return r << a << b;
        }

        template<class T>
        ScopedState<> ScopedState<T>::operator[](const StateSpacePtr &s) const
        {
            ScopedState<> r(s);
            return r << *this;
        }

        typedef boost::shared_ptr< ScopedState<> > ScopedStatePtr;
    }
}

#endif

---

ompl
Author(s): Ioan Sucan/isucan@rice.edu, Mark Moll/mmoll@rice.edu, Lydia Kavraki/kavraki@rice.edu
autogenerated on Fri Jan 11 09:33:20 2013