StateSpace.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_STATE_SPACE_
#define OMPL_BASE_STATE_SPACE_

#include "ompl/base/State.h"
#include "ompl/base/StateSpaceTypes.h"
#include "ompl/base/StateSampler.h"
#include "ompl/base/ProjectionEvaluator.h"
#include "ompl/base/GenericParam.h"
#include "ompl/util/Console.h"
#include "ompl/util/ClassForward.h"
#include <boost/concept_check.hpp>
#include <iostream>
#include <vector>
#include <string>
#include <map>

namespace ompl
{
    namespace base
    {


        OMPL_CLASS_FORWARD(StateSpace);

        class StateSpace
        {
        public:
            // non-copyable
            StateSpace(const StateSpace&) = delete;
            StateSpace& operator=(const StateSpace&) = delete;

            typedef State StateType;

            StateSpace();

            virtual ~StateSpace();

            template<class T>
            T* as()
            {
                BOOST_CONCEPT_ASSERT((boost::Convertible<T*, StateSpace*>));

                return static_cast<T*>(this);
            }

            template<class T>
            const T* as() const
            {
                BOOST_CONCEPT_ASSERT((boost::Convertible<T*, StateSpace*>));

                return static_cast<const T*>(this);
            }

            struct SubstateLocation
            {
                std::vector<std::size_t> chain;

                const StateSpace        *space;
            };

            struct ValueLocation
            {
                SubstateLocation stateLocation;

                std::size_t      index;
            };

            enum SanityChecks
                {

                    STATESPACE_DISTANCE_DIFFERENT_STATES = (1<<1),

                    STATESPACE_DISTANCE_SYMMETRIC        = (1<<2),

                    STATESPACE_INTERPOLATION             = (1<<3),

                    STATESPACE_TRIANGLE_INEQUALITY       = (1<<4),

                    STATESPACE_DISTANCE_BOUND            = (1<<5),

                    STATESPACE_RESPECT_BOUNDS            = (1<<6),

                    STATESPACE_ENFORCE_BOUNDS_NO_OP      = (1<<7),

                    STATESPACE_SERIALIZATION             = (1<<8)
                };

            virtual bool isCompound() const;

            virtual bool isDiscrete() const;

            virtual bool isHybrid() const;

            virtual bool isMetricSpace() const
            {
                return true;
            }

            virtual bool hasSymmetricDistance() const;

            virtual bool hasSymmetricInterpolate() const;

            const std::string& getName() const;

            void setName(const std::string &name);

            int getType() const
            {
                return type_;
            }

            bool includes(const StateSpacePtr &other) const;

            bool includes(const StateSpace *other) const;

            bool covers(const StateSpacePtr &other) const;

            bool covers(const StateSpace *other) const;

            ParamSet& params()
            {
                return params_;
            }

            const ParamSet& params() const
            {
                return params_;
            }

            virtual double getLongestValidSegmentFraction() const;

            virtual void setLongestValidSegmentFraction(double segmentFraction);

            virtual unsigned int validSegmentCount(const State *state1, const State *state2) const;

            void setValidSegmentCountFactor(unsigned int factor);

            unsigned int getValidSegmentCountFactor() const;

            double getLongestValidSegmentLength() const;

            void computeSignature(std::vector<int> &signature) const;

            virtual unsigned int getDimension() const = 0;

            virtual double getMaximumExtent() const = 0;

            virtual double getMeasure() const = 0;

            virtual void enforceBounds(State *state) const = 0;

            virtual bool satisfiesBounds(const State *state) const = 0;

            virtual void copyState(State *destination, const State *source) const = 0;
            
            State* cloneState(const State* source) const;

            virtual double distance(const State *state1, const State *state2) const = 0;

            virtual unsigned int getSerializationLength() const;

            virtual void serialize(void *serialization, const State *state) const;

            virtual void deserialize(State *state, const void *serialization) const;

            virtual bool equalStates(const State *state1, const State *state2) const = 0;

            virtual void interpolate(const State *from, const State *to, const double t, State *state) const = 0;

            virtual StateSamplerPtr allocDefaultStateSampler() const = 0;

            virtual StateSamplerPtr allocStateSampler() const;

            void setStateSamplerAllocator(const StateSamplerAllocator &ssa);

            void clearStateSamplerAllocator();

            virtual State* allocState() const = 0;

            virtual void freeState(State *state) const = 0;

            virtual double* getValueAddressAtIndex(State *state, const unsigned int index) const;

            const double* getValueAddressAtIndex(const State *state, const unsigned int index) const;

            const std::vector<ValueLocation>& getValueLocations() const;

            const std::map<std::string, ValueLocation>& getValueLocationsByName() const;

            double* getValueAddressAtLocation(State *state, const ValueLocation &loc) const;

            const double* getValueAddressAtLocation(const State *state, const ValueLocation &loc) const;

            double* getValueAddressAtName(State *state, const std::string &name) const;

            const double* getValueAddressAtName(const State *state, const std::string &name) const;

            void copyToReals(std::vector<double> &reals, const State *source) const;

            void copyFromReals(State *destination, const std::vector<double> &reals) const;

            void registerProjection(const std::string &name, const ProjectionEvaluatorPtr &projection);

            void registerDefaultProjection(const ProjectionEvaluatorPtr &projection);

            virtual void registerProjections();

            ProjectionEvaluatorPtr getProjection(const std::string &name) const;

            ProjectionEvaluatorPtr getDefaultProjection() const;

            bool hasProjection(const std::string &name) const;

            bool hasDefaultProjection() const;

            const std::map<std::string, ProjectionEvaluatorPtr>& getRegisteredProjections() const;

            virtual void printState(const State *state, std::ostream &out) const;

            virtual void printSettings(std::ostream &out) const;

            virtual void printProjections(std::ostream &out) const;

            virtual void sanityChecks(double zero, double eps, unsigned int flags) const;

            virtual void sanityChecks() const;

            void diagram(std::ostream &out) const;

            void list(std::ostream &out) const;

            static void Diagram(std::ostream &out);

            static void List(std::ostream &out);

            StateSamplerPtr allocSubspaceStateSampler(const StateSpacePtr &subspace) const;

            virtual StateSamplerPtr allocSubspaceStateSampler(const StateSpace *subspace) const;

            State* getSubstateAtLocation(State *state, const SubstateLocation &loc) const;

            const State* getSubstateAtLocation(const State *state, const SubstateLocation &loc) const;

            const std::map<std::string, SubstateLocation>& getSubstateLocationsByName() const;

            void getCommonSubspaces(const StateSpacePtr &other, std::vector<std::string> &subspaces) const;

            void getCommonSubspaces(const StateSpace *other, std::vector<std::string> &subspaces) const;

            virtual void computeLocations();

            virtual void setup();

        protected:

            static const std::string DEFAULT_PROJECTION_NAME;

            int                                           type_;

            StateSamplerAllocator                         ssa_;

            double                                        maxExtent_;

            double                                        longestValidSegmentFraction_;

            double                                        longestValidSegment_;

            unsigned int                                  longestValidSegmentCountFactor_;

            std::map<std::string, ProjectionEvaluatorPtr> projections_;

            ParamSet                                      params_;

            std::vector<ValueLocation>                    valueLocationsInOrder_;

            std::map<std::string, ValueLocation>          valueLocationsByName_;

            std::map<std::string, SubstateLocation>       substateLocationsByName_;

        private:

            std::string                                   name_;
        };

        class CompoundStateSpace : public StateSpace
        {
        public:

            typedef CompoundState StateType;

            CompoundStateSpace();

            CompoundStateSpace(const std::vector<StateSpacePtr> &components, const std::vector<double> &weights);

            virtual ~CompoundStateSpace()
            {
            }

            template<class T>
            T* as(const unsigned int index) const
            {
                BOOST_CONCEPT_ASSERT((boost::Convertible<T*, StateSpace*>));

                return static_cast<T*>(getSubspace(index).get());
            }

            template<class T>
            T* as(const std::string &name) const
            {
                BOOST_CONCEPT_ASSERT((boost::Convertible<T*, StateSpace*>));

                return static_cast<T*>(getSubspace(name).get());
            }

            virtual bool isCompound() const;

            virtual bool isHybrid() const;

            void addSubspace(const StateSpacePtr &component, double weight);

            unsigned int getSubspaceCount() const;

            const StateSpacePtr& getSubspace(const unsigned int index) const;

            const StateSpacePtr& getSubspace(const std::string& name) const;

            unsigned int getSubspaceIndex(const std::string& name) const;

            bool hasSubspace(const std::string &name) const;

            double getSubspaceWeight(const unsigned int index) const;

            double getSubspaceWeight(const std::string &name) const;

            void setSubspaceWeight(const unsigned int index, double weight);

            void setSubspaceWeight(const std::string &name, double weight);

            const std::vector<StateSpacePtr>& getSubspaces() const;

            const std::vector<double>& getSubspaceWeights() const;

            bool isLocked() const;

            void lock();
            virtual StateSamplerPtr allocSubspaceStateSampler(const StateSpace *subspace) const;

            virtual unsigned int getDimension() const;

            virtual double getMaximumExtent() const;

            virtual double getMeasure() const;

            virtual void enforceBounds(State *state) const;

            virtual bool satisfiesBounds(const State *state) const;

            virtual void copyState(State *destination, const State *source) const;

            virtual unsigned int getSerializationLength() const;

            virtual void serialize(void *serialization, const State *state) const;

            virtual void deserialize(State *state, const void *serialization) const;

            virtual double distance(const State *state1, const State *state2) const;

            virtual void setLongestValidSegmentFraction(double segmentFraction);

            virtual unsigned int validSegmentCount(const State *state1, const State *state2) const;

            virtual bool equalStates(const State *state1, const State *state2) const;

            virtual void interpolate(const State *from, const State *to, const double t, State *state) const;

            virtual StateSamplerPtr allocDefaultStateSampler() const;

            virtual State* allocState() const;

            virtual void freeState(State *state) const;

            virtual double* getValueAddressAtIndex(State *state, const unsigned int index) const;

            virtual void printState(const State *state, std::ostream &out) const;

            virtual void printSettings(std::ostream &out) const;

            virtual void computeLocations();

            virtual void setup();

        protected:

            void allocStateComponents(CompoundState *state) const;

            std::vector<StateSpacePtr>    components_;

            unsigned int                  componentCount_;

            std::vector<double>           weights_;

            double                        weightSum_;

            bool                          locked_;

        };

        StateSpacePtr operator+(const StateSpacePtr &a, const StateSpacePtr &b);

        StateSpacePtr operator-(const StateSpacePtr &a, const StateSpacePtr &b);

        StateSpacePtr operator-(const StateSpacePtr &a, const std::string &name);

        StateSpacePtr operator*(const StateSpacePtr &a, const StateSpacePtr &b);
        enum AdvancedStateCopyOperation
            {
                NO_DATA_COPIED = 0,

                SOME_DATA_COPIED = 1,

                ALL_DATA_COPIED  = 2
            };

        AdvancedStateCopyOperation copyStateData(const StateSpacePtr &destS, State *dest,
                                                 const StateSpacePtr &sourceS, const State *source);

        AdvancedStateCopyOperation copyStateData(const StateSpace *destS, State *dest,
                                                 const StateSpace *sourceS, const State *source);

        AdvancedStateCopyOperation copyStateData(const StateSpacePtr &destS, State *dest,
                                                 const StateSpacePtr &sourceS, const State *source,
                                                 const std::vector<std::string> &subspaces);

        AdvancedStateCopyOperation copyStateData(const StateSpace *destS, State *dest,
                                                 const StateSpace *sourceS, const State *source,
                                                 const std::vector<std::string> &subspaces);
    }
}

#endif