graph_search.H

Go to the documentation of this file.

// Copyright (C) 2006 University of Louisiana at Lafayette
// Authors: Suresh Golconda, Pablo Mejia

//fixme: delete() all news()

#ifndef CAJUN_GRAPH_SEARCH_H
#define CAJUN_GRAPH_SEARCH_H

#include "graph.H"
#include <algorithm>
#include <deque>
#include <float.h>

namespace cajun
{
template <typename ID_T>
class graph_search_t
{
        class search_node_t
        {
        public:
                typename graph_t<ID_T>::vertex_t const *g_node;
                double m_cost;
                search_node_t *prev_s_node; //point to previous node in search
                bool operator < (search_node_t const &sn_) const
                        {
                                return (m_cost < sn_.m_cost);
                        };
                bool operator == (search_node_t const &sn_) const
                        {
                                typename graph_t<ID_T>::vertex_t g1 = *g_node;
                                typename graph_t<ID_T>::vertex_t g2 = *(sn_.g_node);
                                return (g1 == g2);
                        };
                search_node_t (typename graph_t<ID_T>::vertex_t const *g_node_) :
                        g_node (g_node_), m_cost (FLT_MAX), prev_s_node (NULL){}
                double get_cost () { return m_cost; }
                void set_cost (double new_cost) { m_cost = new_cost; }
        };

        class node_containter_t
        {
                search_node_t *m_node;
        public:
                bool operator < (node_containter_t const &nc_) const
                {
                        return *m_node < *(nc_.node ());
                }
                bool operator == (node_containter_t const &nc_) const
                {
                        return *m_node == nc_->m_node;
                }
                node_containter_t (search_node_t *node_)
                {
                        m_node = node_;
                }
                search_node_t* const node () const
                        { return m_node; }
        };

private:
        graph_t <ID_T> *m_graph;
        // Added this list for temporarily debugging, to print the
        // cost of all nodes visited.
        //todo: delete this variable
        std::vector<search_node_t *> m_all_nodes;

        search_node_t* get_search_node (
                typename graph_t<ID_T>::vertex_t *node_)
        {
                // Search if search node already created
                for (unsigned i=0; i < m_all_nodes.size (); i++)
                        if (*(m_all_nodes [i]->g_node) == *node_)
                                return m_all_nodes[i];

                // Create a new search node
                search_node_t *sn = new search_node_t (node_);
                m_all_nodes.push_back (sn);
                return sn;
        }

        bool update_cost (search_node_t *node_src_, search_node_t *node_dest_,
                          typename graph_t<ID_T>::edge_t *edge_)
        { //todo: add cost based on the distance between nodes
                double edge_cost = edge_->cost ();
                double new_cost = node_src_->get_cost () + edge_cost;
                if (node_dest_->get_cost () <= new_cost)
                        return false;
                node_dest_->set_cost (new_cost);
                node_dest_->prev_s_node = node_src_;
                return true;
        }
        void extract_path (std::deque<ID_T> &path_, search_node_t *last_node_)
        {
                search_node_t *node = last_node_;
                while (node != NULL)
                {
                        path_.push_front ((node->g_node)->id ());
                        node = node->prev_s_node;
                }
        }

        void add_to_cont_list (
                search_node_t *node,
                std::deque<node_containter_t> &node_cont_list)
                {
                        for (unsigned i=0; i < node_cont_list.size (); i++)
                                if (*(node_cont_list[i].node ()) == *node)
                                        return;

                        node_cont_list.push_back (node_containter_t (node));
                }


public:
        graph_search_t (graph_t<ID_T> *graph_) : m_graph (graph_)
        {}
        void print_graph ()
        {
                for (unsigned i =0; i< m_all_nodes.size (); i++)
                {
                        search_node_t *node = m_all_nodes[i];
                        search_node_t *p_node = node->prev_s_node;

                        printf (" cst=%.2lf (%d %d %d)",
                                node->get_cost (),
                                node->g_node->id ()[0],
                                node->g_node->id ()[1],
                                node->g_node->id ()[2]);
                        if (p_node != NULL)
                                printf ("\t (%d %d %d)\n",
                                        p_node->g_node->id ()[0],
                                        p_node->g_node->id ()[1],
                                        p_node->g_node->id ()[2]);
                        else
                                printf ("\n");
                }
        }

        bool search_graph (typename graph_t<ID_T>::vertex_t *start_gn,
                           typename graph_t<ID_T>::vertex_t *end_gn,
                           std::deque<ID_T> &path_)
        {
                path_.clear ();
                search_node_t *start_node = new search_node_t (start_gn);

                start_node->set_cost (0);// start cost zero

                search_node_t *end_node = new search_node_t (end_gn);

                std::deque<node_containter_t> node_cont_list;

                node_containter_t start_nc (start_node);

                node_cont_list.push_back (start_nc);

                while (!node_cont_list.empty ())
                {
                        //sorting the list before picking least cost node
                        sort (node_cont_list.begin (), node_cont_list.end ());

                        search_node_t *node = node_cont_list.front ().node ();
                        node_cont_list.pop_front ();

                        if (*node == *end_node)
                        {
                                extract_path (path_, node);
                                clear_all_nodes ();
                                return true;
                        }
                        typename graph_t<ID_T>::vertex_t const *g_node =
                                node->g_node;
                        for (unsigned i = 0; i < g_node->num_edges (); i++)
                        {
                                search_node_t *next_node =
                                        get_search_node ((g_node->edge (i))->get_vertex ());

                                //update to send present node to compute
                                //relative new cost and update pointers
                                if (update_cost (node, next_node, g_node->edge (i)))
                                        add_to_cont_list (next_node, node_cont_list);
                        }
                }

                print_graph ();
                printf ("FAILED FOR GRAPH SEARCH, here :\n");
                clear_all_nodes ();
//              assert (false);
                return false;
        }

        void clear_all_nodes ()
        {
                while (!m_all_nodes.empty ())
                {
                        search_node_t *n = m_all_nodes.back ();
                        m_all_nodes.pop_back ();
                        delete (n);
                }
        }
};
};

#endif

---