00001
00002
00003
00004 #ifndef CAJUN_GRID_SLOPE_NO_Z_H
00005 #define CAJUN_GRID_SLOPE_NO_Z_H
00006
00007 #include "data_type.H"
00008 #include "data_queue.H"
00009 #include "data_logger.H"
00010 #include "grid.H"
00011 #include <cmath>
00012 #include <cstdio>
00013 #include <algorithm>
00014 #include <deque>
00015
00016 # define SCANS_PER_SEC 75
00017
00018 namespace cajun
00019 {
00020
00021 # define assign_slope(curr_beam) assign_slope.end_beam[(curr_beam)]
00022 # define scan_gp_beam(scan_num, beam_num) scan_gp_db[(scan_num)].end_beam[(beam_num)]
00023 # define triangle_vertices(tot_triangles) triangle_vertices.triangle_ver[(tot_triangles)]
00024
00025 class grid_slope_no_z_t
00026 {
00027
00028 scan_analysis_data_t assign_slope;
00029
00030 data_queue_writer_t<scan_analysis_data_t> *m_dq_writer;
00031 data_queue_writer_t<scan_analysis_data_t> *m_dq_b_writer;
00032 data_queue_writer_t<triangle_data_t> *triangle_writer;
00033
00034 scan_gp_data_t *scan_gp_db;
00035
00036 static unsigned const NULL_ENTRY = INT_MAX ;
00037
00038 struct beam_chain_t
00039 {
00040 beam_chain_t() : next(NULL_ENTRY),
00041 previous (NULL_ENTRY) {};
00042 unsigned next ;
00043 unsigned previous ;
00044 };
00045
00046 beam_chain_t *beam_list;
00047
00048 struct data_cell_t
00049 {
00050 data_cell_t() : head(NULL_ENTRY),
00051 tail(NULL_ENTRY) {};
00052 unsigned head;
00053 unsigned tail;
00054 };
00055
00056 struct dist_list_t
00057 {
00058 unsigned index;
00059 double dist_sqr;
00060 };
00061
00062 struct dist_sort_t
00063 {
00064 bool operator () (dist_list_t const &v1,
00065 dist_list_t const &v2)const
00066 {return (v1.dist_sqr < v2.dist_sqr) ;}
00067
00068 };
00069
00070 std::vector<dist_list_t> ver_vec;
00071
00072 typedef grid_t<data_cell_t> data_grid_t;
00073 data_grid_t grid;
00074
00075 double data_cell_size;
00076 double R2D;
00077 int triangles_per_beam;
00078 unsigned head_of_db;
00079 unsigned tail_of_db;
00080 double MIN_LEN_SQR,MAX_LEN_SQR;
00081 unsigned VER_PER_CELL;
00082 unsigned MAX_VEC_LEN;
00083 unsigned MAX_SEARCH_SCANS;
00084 double refresh_time;
00085 unsigned scans_in_repository;
00086
00087 public:
00088 grid_slope_no_z_t ( data_queue_writer_t<scan_analysis_data_t>
00089 *scan_analysis_dq_b_writer,
00090 data_queue_writer_t<triangle_data_t>
00091 * tri_writer_,
00092 int triangle_count,
00093 double min_len, double max_len,
00094 unsigned ver_per_cell, unsigned max_vec_len,
00095 unsigned num_search_scans,double REFRESH_TIME )
00096 {
00097 m_dq_b_writer = scan_analysis_dq_b_writer;
00098 triangle_writer = tri_writer_;
00099 data_cell_size = 0.32;
00100 R2D = 180.0/ M_PI;
00101 triangles_per_beam = triangle_count;
00102 MIN_LEN_SQR = min_len * min_len;
00103 MAX_LEN_SQR = max_len * max_len;
00104 VER_PER_CELL = ver_per_cell;
00105 MAX_VEC_LEN = max_vec_len;
00106 refresh_time = REFRESH_TIME;
00107
00108 scan_gp_db = new scan_gp_data_t[num_search_scans];
00109
00110 for (unsigned i = 0; i < num_search_scans; i++)
00111 scan_gp_db[i].num_beams = 0;
00112
00113 beam_list = new beam_chain_t[num_search_scans *
00114 SCAN_GP_MAX_BEAMS];
00115
00116 MAX_SEARCH_SCANS = num_search_scans;
00117 head_of_db = 0;
00118 tail_of_db = 0;
00119 scans_in_repository = 0;
00120 grid.lru_limit (num_search_scans + 1);
00121 };
00122
00123 scan_gp_data_t &get_free_db_entry ()
00124 { return scan_gp_db[head_of_db]; }
00125
00126 bool is_db_full()
00127 {
00128 if(scans_in_repository == MAX_SEARCH_SCANS)
00129 return true;
00130 return false;
00131 }
00132
00133 void delete_old_data (double curr_time)
00134 {
00135
00136 double diff_time = curr_time - refresh_time;
00137
00138 while(scans_in_repository != 0 &&
00139 scan_gp_db[tail_of_db].tstamp <= diff_time )
00140 {
00141 delete_scan (scan_gp_db[tail_of_db]);
00142 scans_in_repository --;
00143 tail_of_db = (tail_of_db + 1) % MAX_SEARCH_SCANS;
00144 }
00145 }
00146
00147 void refresh_repository ()
00148 {
00149 while ( scans_in_repository != 0)
00150 {
00151 delete_scan (scan_gp_db[tail_of_db]);
00152 scans_in_repository --;
00153 tail_of_db = (tail_of_db + 1) % MAX_SEARCH_SCANS;
00154 }
00155 for ( unsigned i = 0; i < MAX_SEARCH_SCANS; i++ )
00156 {
00157 scan_gp_db[i].num_beams = 0;
00158 }
00159 scans_in_repository = 0;
00160 tail_of_db = 0;
00161 head_of_db = 0;
00162 }
00163
00164 void process_new_db_entry ()
00165 {
00166 assert (scans_in_repository <= MAX_SEARCH_SCANS);
00167 scans_in_repository ++;
00168 fill_grid (scan_gp_db[head_of_db], head_of_db);
00169 detect_slope(head_of_db);
00170 head_of_db = (head_of_db + 1) % MAX_SEARCH_SCANS;
00171 }
00172 void sanity_check ( char * s, unsigned index);
00173
00174 void delete_scan ( scan_gp_data_t const &points);
00175
00176 void fill_grid ( scan_gp_data_t const &points, unsigned head);
00177
00178 void detect_slope (unsigned const &head);
00179
00180 void compute_slope ( double x1, double y1, double z1,
00181 double x2, double y2, double z2,
00182 double x3, double y3, double z3,
00183 scan_analysis_data_t &assign_slope,
00184 triangle_data_t &triangle_vertices);
00185
00186 void push_triangle_details (
00187 triangle_data_t &triangle_vertices,
00188 double x1_, double y1_, double z1_,
00189 double x2_, double y2_, double z2_,
00190 double x3_, double y3_, double z3_,
00191 float abs_max_slope_);
00192
00193
00194 void find_eligible_vertices ( double x1, double y1 );
00195
00196 void make_triangles ( double const & x1, double const & y1,
00197 double const & z1,
00198 scan_analysis_data_t & assign_slope,
00199 triangle_data_t & triangle_vertices);
00200
00201 bool test_sec_ver ( double x1, double x2,
00202 double y1, double y2,
00203 double &dist_sqr );
00204
00205
00206 };
00207 };
00208 #endif
