beams.cpp
00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024 #include <core/exception.h>
00025 #include <models/scanlines/beams.h>
00026
00027 #include <cmath>
00028
00029 using fawkes::point_t;
00030
00031 namespace firevision {
00032 #if 0
00033 }
00034 #endif
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056
00057
00058
00059
00060
00061
00062
00063
00064
00065
00066
00067 ScanlineBeams::ScanlineBeams(unsigned int image_width, unsigned int image_height,
00068 unsigned int start_x, unsigned int start_y,
00069 unsigned int stop_y, unsigned int offset_y,
00070 bool distribute_start_x,
00071 float angle_from, float angle_range,
00072 unsigned int num_beams)
00073 {
00074 if ( start_y < stop_y ) throw fawkes::Exception("start_y < stop_y");
00075 if ( (stop_y > image_height) || (start_y > image_height) ) {
00076 throw fawkes::Exception("(stop_y > height) || (start_y > height)");
00077 }
00078
00079 this->start_x = start_x;
00080 this->start_y = start_y;
00081 this->angle_from = angle_from;
00082 this->angle_range = angle_range;
00083 this->num_beams = num_beams;
00084 this->stop_y = stop_y;
00085 this->offset_y = offset_y;
00086 this->image_width = image_width;
00087 this->image_height = image_height;
00088 this->distribute_start_x = distribute_start_x;
00089
00090 reset();
00091 }
00092
00093
00094 point_t
00095 ScanlineBeams::operator*()
00096 {
00097 return coord;
00098 }
00099
00100 point_t*
00101 ScanlineBeams::operator->()
00102 {
00103 return &coord;
00104 }
00105
00106
00107 bool
00108 ScanlineBeams::finished()
00109 {
00110 return _finished;
00111 }
00112
00113
00114 void
00115 ScanlineBeams::advance()
00116 {
00117
00118 while ( ! _finished && (first_beam < last_beam) ) {
00119
00120 unsigned int x_start = beam_current_pos[next_beam].x;
00121 unsigned int y_start = beam_current_pos[next_beam].y;
00122
00123 unsigned int x_end = beam_end_pos[next_beam].x;
00124 unsigned int y_end = beam_end_pos[next_beam].y;
00125
00126 int x, y, dist, xerr, yerr, dx, dy, incx, incy;
00127
00128
00129 dx = x_end - x_start;
00130 dy = y_end - y_start;
00131
00132
00133 if(dx < 0) {
00134 incx = -1;
00135 dx = -dx;
00136 } else {
00137 incx = dx ? 1 : 0;
00138 }
00139
00140 if(dy < 0) {
00141 incy = -1;
00142 dy = -dy;
00143 } else {
00144 incy = dy ? 1 : 0;
00145 }
00146
00147
00148 dist = (dx > dy) ? dx : dy;
00149
00150
00151 x = x_start;
00152 y = y_start;
00153 xerr = dx;
00154 yerr = dy;
00155
00156
00157 unsigned int offset = 0;
00158 while ( (x >= 0) && ((unsigned int )x < image_width) && ((unsigned int)y > stop_y) &&
00159 (offset < offset_y) ) {
00160 ++offset;
00161
00162 xerr += dx;
00163 yerr += dy;
00164
00165 if(xerr > dist) {
00166 xerr -= dist;
00167 x += incx;
00168 }
00169
00170 if(yerr>dist) {
00171 yerr -= dist;
00172 y += incy;
00173 }
00174 }
00175 if ( (y < 0) || (unsigned int)y <= stop_y ) {
00176 _finished = true;
00177 break;
00178 }
00179 if ( x < 0 ) {
00180 first_beam = ++next_beam;
00181 continue;
00182 }
00183 if ( (unsigned int)x > image_width ) {
00184 last_beam = next_beam - 1;
00185 next_beam = first_beam;
00186 continue;
00187 }
00188
00189 coord.x = x;
00190 coord.y = y;
00191
00192 beam_current_pos[next_beam] = coord;
00193
00194 if ( next_beam < last_beam) {
00195 ++next_beam;
00196 } else {
00197 next_beam = first_beam;
00198 }
00199 break;
00200 }
00201
00202 }
00203
00204
00205 point_t *
00206 ScanlineBeams::operator++()
00207 {
00208 advance();
00209 return &coord;
00210 }
00211
00212
00213 point_t *
00214 ScanlineBeams::operator++(int i)
00215 {
00216 tmp_coord.x = coord.x;
00217 tmp_coord.y = coord.y;
00218 advance();
00219 return &tmp_coord;
00220 }
00221
00222
00223 void
00224 ScanlineBeams::reset()
00225 {
00226 _finished = false;
00227
00228 beam_current_pos.clear();
00229 if ( distribute_start_x ) {
00230 unsigned int offset_start_x = image_width / (num_beams - 1);
00231 for (unsigned int i = 0; i < num_beams; ++i) {
00232 coord.x = i * offset_start_x;
00233 coord.y = start_y;
00234 beam_current_pos.push_back(coord);
00235 }
00236 coord.x = beam_current_pos[0].x;
00237 coord.y = beam_current_pos[0].y;
00238 } else {
00239 coord.x = start_x;
00240 coord.y = start_y;
00241 beam_current_pos.resize( num_beams, coord );
00242 }
00243
00244
00245 beam_end_pos.clear();
00246 next_beam = 0;
00247 float angle_between_beams = angle_range / num_beams;
00248 for (unsigned int i = 0; i < num_beams; ++i) {
00249 float diff_y = beam_current_pos[i].y - stop_y;
00250 float diff_x = diff_y * tan( angle_from + (float)i * angle_between_beams );
00251 point_t end_point;
00252 end_point.y = stop_y;
00253 end_point.x = (int)roundf(diff_x) + start_x;
00254 beam_end_pos.push_back(end_point);
00255 }
00256 first_beam = 0;
00257 last_beam = beam_end_pos.size() - 1;
00258 }
00259
00260 const char *
00261 ScanlineBeams::get_name()
00262 {
00263 return "ScanlineModel::Beams";
00264 }
00265
00266
00267 unsigned int
00268 ScanlineBeams::get_margin()
00269 {
00270 return offset_y;
00271 }
00272
00273 }