.. _program_listing_file_src_ompl_base_StateSpace.h:

Program Listing for File StateSpace.h
=====================================

|exhale_lsh| :ref:`Return to documentation for file <file_src_ompl_base_StateSpace.h>` (``src/ompl/base/StateSpace.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_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;
   
               using StateType = ompl::base::State;
   
               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;
   
               virtual void setValidSegmentCountFactor(unsigned int factor);
   
               virtual unsigned int getValidSegmentCountFactor() const;
   
               virtual double getLongestValidSegmentLength() const;
   
               virtual 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, 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, unsigned int index) const;
   
               virtual const double *getValueAddressAtIndex(const State *state, unsigned int index) const;
   
               virtual const std::vector<ValueLocation> &getValueLocations() const;
   
               virtual const std::map<std::string, ValueLocation> &getValueLocationsByName() const;
   
               virtual double *getValueAddressAtLocation(State *state, const ValueLocation &loc) const;
   
               virtual const double *getValueAddressAtLocation(const State *state, const ValueLocation &loc) const;
   
               virtual double *getValueAddressAtName(State *state, const std::string &name) const;
   
               virtual const double *getValueAddressAtName(const State *state, const std::string &name) const;
   
               virtual void copyToReals(std::vector<double> &reals, const State *source) const;
   
               virtual 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 = std::cout) 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:
               using StateType = ompl::base::CompoundState;
   
               CompoundStateSpace();
   
               CompoundStateSpace(const std::vector<StateSpacePtr> &components, const std::vector<double> &weights);
   
               ~CompoundStateSpace() override = default;
   
               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());
               }
   
               bool isCompound() const override;
   
               bool isHybrid() const override;
   
               bool isMetricSpace() const override;
   
               void addSubspace(const StateSpacePtr &component, double weight);
   
               unsigned int getSubspaceCount() const;
   
               const StateSpacePtr &getSubspace(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(unsigned int index) const;
   
               double getSubspaceWeight(const std::string &name) const;
   
               void setSubspaceWeight(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();
               StateSamplerPtr allocSubspaceStateSampler(const StateSpace *subspace) const override;
   
               unsigned int getDimension() const override;
   
               double getMaximumExtent() const override;
   
               double getMeasure() const override;
   
               void enforceBounds(State *state) const override;
   
               bool satisfiesBounds(const State *state) const override;
   
               void copyState(State *destination, const State *source) const override;
   
               unsigned int getSerializationLength() const override;
   
               void serialize(void *serialization, const State *state) const override;
   
               void deserialize(State *state, const void *serialization) const override;
   
               double distance(const State *state1, const State *state2) const override;
   
               void setLongestValidSegmentFraction(double segmentFraction) override;
   
               unsigned int validSegmentCount(const State *state1, const State *state2) const override;
   
               bool equalStates(const State *state1, const State *state2) const override;
   
               void interpolate(const State *from, const State *to, double t, State *state) const override;
   
               StateSamplerPtr allocDefaultStateSampler() const override;
   
               State *allocState() const override;
   
               void freeState(State *state) const override;
   
               double *getValueAddressAtIndex(State *state, unsigned int index) const override;
   
               void printState(const State *state, std::ostream &out) const override;
   
               void printSettings(std::ostream &out) const override;
   
               void computeLocations() override;
   
               void setup() override;
   
           protected:
               void allocStateComponents(CompoundState *state) const;
   
               std::vector<StateSpacePtr> components_;
   
               unsigned int componentCount_{0u};
   
               std::vector<double> weights_;
   
               double weightSum_{0.0};
   
               bool locked_{false};
           };
   
           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