planners/ik_mpep/SMGraph.cpp

Go to the documentation of this file.

#include "SMGraph.h"
#include <assert.h>
#include <LEDA/shortest_path.h>
#include <LEDA/templates/shortest_path.t>


#define DATA_LEN_IN_NODE  1
#define DATA_LEN_IN_EDGE  1

//Construction function,
//  where the ENTRY node will be set with A configuration.
SMGraph::SMGraph(Configuration &entry):
                G(DATA_LEN_IN_NODE, DATA_LEN_IN_EDGE)
{
        G.make_undirected();
        node newNode = NewNode(entry);

        assert(newNode!=NULL);

        entryNode = newNode;
        exitNode = NULL;

        current = NULL;
        currentNode = NULL;
}

SMGraph::~SMGraph()
{
        node v;
        Configuration *config;
        forall_nodes(v, G)
        {
                config = (Configuration *)v->data(0);
                delete config;
        }
        G.clear();
}

//Actually, the purpose letting this function here,
//  is just to make it similiar with "SetExitConfig"
//  maybe never get called.
bool SMGraph::SetEntryConfig(Configuration &entry)
{
        node v=Lookup(entry);

        if (v!=NULL)
        {
                entryNode = v;
                return true;
        }
        else
        {
                return false;
        }
}

//Update the EXIT node for the Self-Motion Graph.
bool SMGraph::SetExitConfig(Configuration &exit)
{
        node v=Lookup(exit);

        if (v!=NULL)
        {
                exitNode = v;
                return true;
        }
        else
        {
                return false;
        }
}

//Call the shortest path algorithm to get the shortest path from
//  the ENTRY node to the EXIT node.
bool SMGraph::GetPath()
{
        if (entryNode==NULL)
                return false;
        if (exitNode==NULL)
                return false;

        edge_array<double>cost(G);
        edge ed;
        forall_edges(ed, G)
        {
                cost[ed] = G.inf(ed);
        }

        node_array<double> dist(G);
        node_array<edge> pred(G);

        bool result = SHORTEST_PATH_T(G, entryNode, cost, dist, pred);

        if (result==false)
                return false;

        //Got the shortest path, retrieve the corresponding configuration from the path.
        path.clear();

        node v;
        v=exitNode;
        path.append(G.inf(v));
        while(pred[v]!=NULL)
        {
                edge e;
                e = pred[v];
                v=G.opposite(v, e);
                path.append(G.inf(v));
        }

        return true;
}

void* SMGraph::FirstNode()
{
        currentNode = G.first_node();
        return currentNode;
}

void* SMGraph::NextNode()
{
        currentNode = G.succ_node(currentNode);
        return currentNode;
}

Configuration* SMGraph::GetConfiguration(void *v)
{
        return (Configuration *)G.inf((node)v);
}

Configuration* SMGraph::FirstConfigInPath()
{
        current = path.first();
        if (current != NULL)
                return (Configuration *)path.inf(current);
        else
                return NULL;
}

Configuration* SMGraph::NextConfigInPath()
{
        current = path.succ(current);
        if (current != NULL)
                return (Configuration *)path.inf(current);
        else
                return NULL;
}

void *SMGraph::AddConnection(void* parent, Configuration &son)
{
        node v=NewNode(son);
        double dist = Distance(*(G.inf((node)parent)), son);
        edge e=G.new_edge((node)parent, v, dist);
        exitNode = v;
        return v;
}

void *SMGraph::AddConnection(Configuration &parent, Configuration &son)
{
        node v=Lookup(parent);

        if (v!=NULL)
        {
                return AddConnection(v, son);
        }
        else
        {
                return NULL;
        }
}

void SMGraph::Clear()
{
        if ( !G.empty() )
        {
                G.clear();
        }
}

bool SMGraph::OptimizeConnection()
{
        //What is expected here is to optimize the structure of Graph, including
        //  - Make the Graph more compact.
        //  - Remove the cyclic connection, if there is
        //  - Remove independent vertex, if there is
        //  - Maybe some other operation
        //However, just forget about it at this moment.
        return true;
}

int SMGraph::Node_Num()
{
        return G.number_of_nodes();
}

GRAPH<Configuration *, double> *SMGraph::GetSMGraph() 
{
        return &G;
}

int SMGraph::Node_Edge()
{
        return G.number_of_edges();
}

void SMGraph::GetConfig(const node& n, Configuration **conf)
{
        *conf = (Configuration *)(G.inf(n));
}

void SMGraph::GetConfig(const edge& e, Configuration **conf1, Configuration **conf2)
{
        node v = G.source(e);
        *conf1 = (Configuration *)(G.inf(v));
        v = G.target(e);
        *conf2 = (Configuration *)(G.inf(v));
}

node SMGraph::NewNode(Configuration &config)
{
        Configuration *data = new Configuration(config);
        node v=G.new_node(data);
        return v;
}

node SMGraph::Lookup(Configuration &config)
{
        node v;
        Configuration *inf;
        forall_nodes(v, G)
        {
                inf = (Configuration *)(G.inf(v));
                if ((*inf)==config)
                        break;
        }
        return v;
}

double SMGraph::Distance (const Configuration& a, const Configuration& b)
{
        //Here, actually need to compute the distance between two configurations.
        //  However, computing this distance required the visibility of Collision Detection(C-D).
        //  So, I'm thinking to hang this in there.
        //    If someday we really need that, we can use callback or exporting C-D to make it happen.
        //    Before that Self-Motion Graph is actually a un-weighted graph.
        return 1;
}

---