grid_slope.H

Go to the documentation of this file.

// Copyright (C) 2005 University of Louisiana at Lafayette
// Authors:  Amit Puntambekar

#ifndef CAJUN_GRID_SLOPE_H
#define CAJUN_GRID_SLOPE_H

#include "data_type.H"
#include "data_queue.H"
#include "data_logger.H"
#include "grid.H"
#include <cmath>
#include <cstdio>
#include <algorithm>
#include <deque>

# define SCANS_PER_SEC 75

namespace cajun
{

# define assign_slope(curr_beam) assign_slope.end_beam[(curr_beam)]
# define scan_gp_beam(scan_num, beam_num) scan_gp_db[(scan_num)].end_beam[(beam_num)]
        class grid_slope_t
        {

                scan_analysis_data_t assign_slope;
                data_queue_writer_t<scan_analysis_data_t> *m_dq_writer;
                data_queue_writer_t<scan_analysis_data_t> *m_dq_b_writer;

                scan_gp_data_t *scan_gp_db;

                static unsigned const NULL_ENTRY = INT_MAX ;

                struct beam_chain_t
                {
                        beam_chain_t() : next(NULL_ENTRY),
                                         previous (NULL_ENTRY) {};
                        unsigned next ;
                        unsigned previous ;
                };

                beam_chain_t  *beam_list;

                struct data_cell_t
                {
                        data_cell_t() : head(NULL_ENTRY),
                                        tail(NULL_ENTRY) {};
                        unsigned head;
                        unsigned tail;
                };

                struct dist_list_t
                {
                        unsigned index;
                        double dist_sqr;
                };

                struct dist_sort_t
                {
                        bool operator () (dist_list_t const &v1,
                                          dist_list_t const &v2)const
                                {return (v1.dist_sqr < v2.dist_sqr) ;}

                };

                std::vector<dist_list_t> ver_vec;

                typedef grid_t<data_cell_t> data_grid_t;
                data_grid_t grid;

                double data_cell_size;
                double R2D;
                int triangles_per_beam;
                unsigned head_of_db;
                unsigned tail_of_db;
                double MIN_LEN_SQR,MAX_LEN_SQR;
                unsigned VER_PER_CELL;
                unsigned MAX_VEC_LEN;
                unsigned MAX_SEARCH_SCANS;
                double refresh_time;
                unsigned scans_in_repository;

        public:
                grid_slope_t ( data_queue_writer_t<scan_analysis_data_t>
                               *scan_analysis_dq_b_writer,
                               int triangle_count,
                               double min_len, double max_len,
                               unsigned ver_per_cell, unsigned max_vec_len,
                               unsigned num_search_scans,double REFRESH_TIME )
                        {
                                m_dq_b_writer = scan_analysis_dq_b_writer;
                                data_cell_size = 0.32;
                                R2D = 180.0/ M_PI;
                                triangles_per_beam = triangle_count;
                                MIN_LEN_SQR = min_len * min_len;
                                MAX_LEN_SQR = max_len * max_len;
                                VER_PER_CELL = ver_per_cell;
                                MAX_VEC_LEN = max_vec_len;
                                refresh_time = REFRESH_TIME;

                                scan_gp_db = new scan_gp_data_t[num_search_scans];

                                for (unsigned i = 0; i < num_search_scans; i++)
                                        scan_gp_db[i].num_beams = 0;

                                beam_list = new beam_chain_t[num_search_scans *
                                                             SCAN_GP_MAX_BEAMS];

                                MAX_SEARCH_SCANS = num_search_scans;
                                head_of_db = 0;
                                tail_of_db = 0;
                                scans_in_repository = 0;
                                grid.lru_limit (num_search_scans + 1);
                        };

                scan_gp_data_t &get_free_db_entry ()
                        { return scan_gp_db[head_of_db]; }

                bool is_db_full()
                        {
                        if(scans_in_repository == MAX_SEARCH_SCANS)
                                return true;
                        return false;
                        }

                void delete_old_data (double curr_time)
                {

                        double diff_time = curr_time - refresh_time;

                        while(scans_in_repository != 0 &&
                              scan_gp_db[tail_of_db].tstamp <= diff_time )
                        {
                                delete_scan (scan_gp_db[tail_of_db]);
                                scans_in_repository --;
                                tail_of_db = (tail_of_db + 1) % MAX_SEARCH_SCANS;
                        }
                }

                void process_new_db_entry ()
                {
                        assert (scans_in_repository <= MAX_SEARCH_SCANS);
                        scans_in_repository ++;
                        fill_grid (scan_gp_db[head_of_db], head_of_db);
                        detect_slope(head_of_db);
                        head_of_db = (head_of_db + 1) % MAX_SEARCH_SCANS;
                }

                void sanity_check ( char * s, unsigned index);

                void delete_scan ( scan_gp_data_t const &points);

                void fill_grid ( scan_gp_data_t const &points, unsigned head);

                void detect_slope (unsigned const &head);

                void compute_slope (  double x1, double y1, double z1,
                                      double x2, double y2, double z2,
                                      double x3, double y3, double z3,
                                      scan_analysis_data_t &assign_slope);

                void find_eligible_vertices ( double x1, double y1, double z1 );

                void make_triangles (     double const & x1, double const & y1,
                                          double const & z1,
                                          scan_analysis_data_t & assign_slope);

                bool  test_sec_ver   ( double x1, double y1,
                                       double x2, double y2,
                                       double z1, double z2,
                                       double &dist_sqr );


        };
};
#endif

---