RRT-Star.hpp

/*
 * RRT-Star.hpp
 *
 *  Created on: Nov 04, 2013
 *      Author: ddevaurs
 */

#ifndef _RRT_STAR_HPP_
#define _RRT_STAR_HPP_

#include "planner/Diffusion/RRT.hpp"


class StarExpansion: public RRTExpansion
{
public:
        StarExpansion(Graph * graph);

        virtual ~StarExpansion() {};

private:
        bool isValidConnection(localPathPtr_t path);

        Node * addNode(localPathPtr_t path, Node * fromNode, Node * directionNode);

private:
        unsigned dimension;

        double gamma;

        double gamma_star;
};


class StarRRT: public RRT
{
public:
        StarRRT(Robot * robot, Graph * graph) : RRT(robot, graph) {
                std::cout << "Construct the RRT*" << std::endl;
        }

        virtual ~StarRRT() {};

        unsigned init() {
                expansion = new StarExpansion(_Graph);
                return TreePlanner::init();
        }

        bool checkStopConditions() {
                return TreePlanner::checkStopConditions();
        }
};

#endif