obstacle_free.H

Go to the documentation of this file.

// Copyright (C) 2007 University of Louisiana at Lafayette
// Authors: Suresh Golconda

#ifndef CAJUN_OBSTACLE_FREE_H
#define CAJUN_OBSTACLE_FREE_H


#include "map_grid.H"
#include "rect_filler.H"
#include <math.h>

#include "rndf.H"


#include <vector>
#include <cmath>


namespace cajun
{

class obstacle_free_t
{
public:
        obstacle_free_t (map_grid_t *map_grid_,
                         double cell_size_, rndf_t const *rndf_,
                         unsigned zone_id_):

                m_cell_size (cell_size_),
                m_filler (map_grid_), m_rndf (rndf_), m_zone_id (zone_id_)

                { }
private:
        class filler_t
        {
        public:
                filler_t (map_grid_t *map_grid_) :
                        m_map_grid (map_grid_), m_blocked (false) {}

                void reset_blocked () { m_blocked = false; }

                void fill (int x_, int y_, unsigned w_)
                {
                        map_grid_t::iterator_t iter =
                                m_map_grid->find (x_, y_);

                        static float max_ob_cost = 0;

                        while (w_--)
                        {
                                max_ob_cost = std::max (max_ob_cost, iter->obstacle);

                                m_blocked = m_blocked ||
                                        //fixme: is not checking boundaries
//                                      iter->boundary == FLT_MAX ||
                                        iter->obstacle == FLT_MAX;

                                iter.neighbor (1, 0);
                        }
                }

                bool is_blocked () { return m_blocked; }

        protected:
                map_grid_t * const m_map_grid;

                bool m_blocked;
//              float m_cost;
        };

        double m_cell_size;

        filler_t m_filler;
        rect_filler_t m_rect_filler;

        rndf_t const *m_rndf;
        unsigned m_zone_id;
public:

        bool obstacle_in_region (double x0_, double y0_,
                                 double x1_, double y1_, double size_)
        {
                double orient = atan2 (y1_ - y0_, x1_ - x0_);
                double dx = size_ * cos (orient);
                double dy = size_ * sin (orient);

                x0_ -= dx;
                y0_ -= dy;
                x1_ += dx;
                y1_ += dy;

                m_filler.reset_blocked ();

                m_rect_filler.fill (m_filler,
                                    x0_ / m_cell_size, y0_ / m_cell_size,
                                    x1_ / m_cell_size, y1_ / m_cell_size,
                                    size_ / m_cell_size);

                return m_filler.is_blocked ();
        }
};


};


#endif

---