Fawkes API  Fawkes Development Version
beams.cpp
00001 
00002 /***************************************************************************
00003  *  beams.cpp - Scanline model implementation: beams
00004  *
00005  *  Created: Tue Apr 17 21:09:46 2007
00006  *  Copyright  2005-2007  Tim Niemueller [www.niemueller.de]
00007  *
00008  ****************************************************************************/
00009 
00010 /*  This program is free software; you can redistribute it and/or modify
00011  *  it under the terms of the GNU General Public License as published by
00012  *  the Free Software Foundation; either version 2 of the License, or
00013  *  (at your option) any later version. A runtime exception applies to
00014  *  this software (see LICENSE.GPL_WRE file mentioned below for details).
00015  *
00016  *  This program is distributed in the hope that it will be useful,
00017  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00018  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00019  *  GNU Library General Public License for more details.
00020  *
00021  *  Read the full text in the LICENSE.GPL_WRE file in the doc directory.
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 /* just to make Emacs auto-indent happy */
00033 }
00034 #endif
00035 
00036 /** @class ScanlineBeams <models/scanlines/beams.h>
00037  * Raytraced beams scanline model.
00038  * This model uses a defined number of beams shot from the bottom of the image
00039  * towards the top using Bresenham. With this you can have kind of a radar-like
00040  * scanline model. Additionally the starting points at the bottom can be
00041  * distributed over the full width of the image which alles for a scan aligned
00042  * to the image.
00043  *
00044  * To ease the calculation of the finished state the very last point is traversed
00045  * twice.
00046  *
00047  * @author Tim Niemueller
00048  */
00049 
00050 /** Construtor.
00051  * @param image_width image width
00052  * @param image_height image height
00053  * @param start_x x coordinate of the starting point, ignored if distributed (see below)
00054  * @param start_y y coordinate of the starting point, this is the lowest points of the
00055  * the lines and should thus be close to the bottom of the image
00056  * @param stop_y Y coordinate for stopping the traversal
00057  * @param offset_y number of pixel to advance in Y-direction per iteration
00058  * @param distribute_start_x set to true, to distribute the start x coordinates
00059  * equidistant over the whole width of the image.
00060  * @param angle_from angle to start the scan at, a straight vertical line means
00061  * zero rad, clock-wise positive, in radians
00062  * @param angle_range the range to use to distribute the beams, clockwise positive,
00063  * in radians
00064  * @param num_beams number of beams to use
00065  * @exception Exception thrown if parameters are out of bounds
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     // calculate distance in both directions
00129     dx = x_end - x_start;
00130     dy = y_end - y_start;
00131  
00132     // Calculate sign of the increment
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     // check which distance is larger
00148     dist = (dx > dy) ? dx : dy;
00149     
00150     // Initialize for loops
00151     x = x_start;
00152     y = y_start;
00153     xerr = dx;
00154     yerr = dy;
00155      
00156     /* Calculate and draw pixels */
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 } // end namespace firevision