SpaceInformation.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_SPACE_INFORMATION_
#define OMPL_BASE_SPACE_INFORMATION_

#include "ompl/base/State.h"
#include "ompl/base/StateValidityChecker.h"
#include "ompl/base/MotionValidator.h"
#include "ompl/base/StateSpace.h"
#include "ompl/base/ValidStateSampler.h"

#include "ompl/util/ClassForward.h"
#include "ompl/util/Console.h"
#include "ompl/util/Exception.h"

#include <functional>
#include <utility>
#include <cstdlib>
#include <vector>
#include <iostream>

namespace ompl
{

    namespace base
    {

        OMPL_CLASS_FORWARD(SpaceInformation);

        typedef std::function<bool(const State*)> StateValidityCheckerFn;


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

            SpaceInformation(const StateSpacePtr &space);

            virtual ~SpaceInformation()
            {
            }

            bool isValid(const State *state) const
            {
                return stateValidityChecker_->isValid(state);
            }

            const StateSpacePtr& getStateSpace() const
            {
                return stateSpace_;
            }

            bool equalStates(const State *state1, const State *state2) const
            {
                return stateSpace_->equalStates(state1, state2);
            }

            bool satisfiesBounds(const State *state) const
            {
                return stateSpace_->satisfiesBounds(state);
            }

            double distance(const State *state1, const State *state2) const
            {
                return stateSpace_->distance(state1, state2);
            }

            void enforceBounds(State *state) const
            {
                stateSpace_->enforceBounds(state);
            }

            void printState(const State *state, std::ostream &out = std::cout) const
            {
                stateSpace_->printState(state, out);
            }

            void setStateValidityChecker(const StateValidityCheckerPtr &svc)
            {
                stateValidityChecker_ = svc;
                setup_ = false;
            }

            void setStateValidityChecker(const StateValidityCheckerFn &svc);

            const StateValidityCheckerPtr& getStateValidityChecker() const
            {
                return stateValidityChecker_;
            }

            void setMotionValidator(const MotionValidatorPtr &mv)
            {
                motionValidator_ = mv;
                setup_ = false;
            }

            const MotionValidatorPtr& getMotionValidator() const
            {
                return motionValidator_;
            }

            void setStateValidityCheckingResolution(double resolution)
            {
                stateSpace_->setLongestValidSegmentFraction(resolution);
                setup_ = false;
            }

            double getStateValidityCheckingResolution() const
            {
                return stateSpace_->getLongestValidSegmentFraction();
            }


            unsigned int getStateDimension() const
            {
                return stateSpace_->getDimension();
            }

            double getSpaceMeasure() const
            {
                return stateSpace_->getMeasure();
            }

            State* allocState() const
            {
                return stateSpace_->allocState();
            }

            void allocStates(std::vector<State*> &states) const
            {
                for (unsigned int i = 0 ; i < states.size() ; ++i)
                    states[i] = stateSpace_->allocState();
            }

            void freeState(State *state) const
            {
                stateSpace_->freeState(state);
            }

            void freeStates(std::vector<State*> &states) const
            {
                for (unsigned int i = 0 ; i < states.size() ; ++i)
                    stateSpace_->freeState(states[i]);
            }

            void copyState(State *destination, const State *source) const
            {
                stateSpace_->copyState(destination, source);
            }

            State* cloneState(const State *source) const
            {
                return stateSpace_->cloneState(source);
            }

            StateSamplerPtr allocStateSampler() const
            {
                return stateSpace_->allocStateSampler();
            }

            ValidStateSamplerPtr allocValidStateSampler() const;

            void setValidStateSamplerAllocator(const ValidStateSamplerAllocator &vssa);

            void clearValidStateSamplerAllocator();

            double getMaximumExtent() const
            {
                return stateSpace_->getMaximumExtent();
            }

            bool searchValidNearby(State *state, const State *near, double distance, unsigned int attempts) const;

            bool searchValidNearby(const ValidStateSamplerPtr &sampler, State *state, const State *near, double distance) const;

            unsigned int randomBounceMotion(const StateSamplerPtr &sss, const State *start, unsigned int steps, std::vector<State*> &states, bool alloc) const;

            bool checkMotion(const State *s1, const State *s2, std::pair<State*, double> &lastValid) const
            {
                return motionValidator_->checkMotion(s1, s2, lastValid);
            }


            bool checkMotion(const State *s1, const State *s2) const
            {
                return motionValidator_->checkMotion(s1, s2);
            }

            bool checkMotion(const std::vector<State*> &states, unsigned int count, unsigned int &firstInvalidStateIndex) const;

            bool checkMotion(const std::vector<State*> &states, unsigned int count) const;

            unsigned int getMotionStates(const State *s1, const State *s2, std::vector<State*> &states, unsigned int count, bool endpoints, bool alloc) const;

            unsigned int getCheckedMotionCount() const
            {
                return motionValidator_->getCheckedMotionCount();
            }

            double probabilityOfValidState(unsigned int attempts) const;

            double averageValidMotionLength(unsigned int attempts) const;

            void samplesPerSecond(double &uniform, double &near, double &gaussian, unsigned int attempts) const;

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

            virtual void printProperties(std::ostream &out = std::cout) const;

            ParamSet& params()
            {
                return params_;
            }

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

            virtual void setup();

            bool isSetup() const;

        protected:
            void setDefaultMotionValidator();

            StateSpacePtr              stateSpace_;

            StateValidityCheckerPtr    stateValidityChecker_;

            MotionValidatorPtr         motionValidator_;

            bool                       setup_;

            ValidStateSamplerAllocator vssa_;

            ParamSet                   params_;
        };

    }

}

#endif