HRICS_rrtExpansion.hpp

#ifndef HRICS_RRTEXPANSION_H
#define HRICS_RRTEXPANSION_H

#include "API/planningAPI.hpp"
#include "Diffusion/Variants/Transition-RRT.hpp"
#include "HRI_costspace/Grid/HRICS_Grid.hpp"

class HRICS_rrtExpansion : public TransitionExpansion
{
public:
    HRICS_rrtExpansion(Graph * ptrGraph, Node * goal);

    void init();

    void setGrid(API::ThreeDGrid* grid) { _3DGrid = dynamic_cast<HRICS::Grid*>(grid); }

    void setCellPath(std::vector<API::ThreeDCell*> cellPath);

    std::tr1::shared_ptr<Configuration> getExpansionDirection(
            Node* expandComp, Node* goalComp, bool samplePassive, Node*& directionNode);

    std::tr1::shared_ptr<Configuration> getConfigurationInNextCell(Node* CompcoNode, Node* goalComp);

    Node* addNode(Node* currentNode, LocalPath& path, double pathDelta,
                  Node* directionNode, unsigned& nbCreatedNodes);

    bool on3DPathAndAfter(API::ThreeDCell* cell);

private:
    HRICS::Grid*             _3DGrid;
    std::vector<API::ThreeDCell*>  _3DCellPath;

    API::ThreeDCell*               _LastForward;
    API::ThreeDCell*               _LastBackward;

    int         mIndexObjectDof;

    bool                     _forward;
    bool                     _biasing;

    double*                  _Box;

};

#endif // HRICS_RRTEXPANSION_H