radial.cpp

00001 
00002 /***************************************************************************
00003  *  radial.cpp - Implementation of the radial scanline model
00004  *
00005  *  Created: Tue Jul 19 12:46:52 2005
00006  *  Copyright  2005  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 <models/scanlines/radial.h>
00025 
00026 #include <utils/system/console_colors.h>
00027 
00028 #include <cmath>
00029 #include <cstring>
00030 
00031 using fawkes::point_t;
00032 
00033 namespace firevision {
00034 #if 0 /* just to make Emacs auto-indent happy */
00035 }
00036 #endif
00037 
00038 /** @class ScanlineRadial <models/scanlines/radial.h>
00039  * Radial scanlines.
00040  * Uses circles to generate scanline points. A dead radius is ignored in the
00041  * center of the image (for example for the camera itself in an omni-vision system).
00042  * From there circles are used in radius_increment distances. On each circle points are
00043  * generated in a distance of about step pixels. This is done up to a given maximum
00044  * radius. If no maximum radius is supplied (max_radius=0) it is automatically
00045  * calculated depending on the image size.
00046  */
00047 
00048 /** Constructor.
00049  * @param width image width
00050  * @param height image height
00051  * @param center_x radial center center x
00052  * @param center_y radial center center y
00053  * @param radius_increment radius increment
00054  * @param step step
00055  * @param max_radius maximum radius, if set to 0 will be calculated
00056  * automatically depending on the image dimensions.
00057  * @param dead_radius inner radius to ignore
00058  */
00059 ScanlineRadial::ScanlineRadial(unsigned int width, unsigned int height,
00060                                unsigned int center_x, unsigned int center_y,
00061                                unsigned int radius_increment,
00062                                unsigned int step,
00063                                unsigned int max_radius, unsigned int dead_radius
00064                                )
00065 {
00066   this->width            = width;
00067   this->height           = height;
00068   this->center_x         = center_x;
00069   this->center_y         = center_y;
00070   this->radius_increment = radius_increment;
00071   this->step             = step;
00072   this->dead_radius      = dead_radius;
00073   this->max_radius       = max_radius;
00074   this->auto_max_radius  = (max_radius == 0);
00075 
00076   reset();
00077 }
00078 
00079 point_t
00080 ScanlineRadial::operator*()
00081 {
00082   return coord;
00083 }
00084 
00085 point_t*
00086 ScanlineRadial::operator->()
00087 {
00088   return &coord;
00089 }
00090 
00091 point_t *
00092 ScanlineRadial::operator++()
00093 {
00094 
00095   if ( done ) return &coord;
00096 
00097   bool ok = false;
00098 
00099   do {
00100 
00101     tmp_x = 0;
00102     tmp_y = 0;
00103 
00104     if ( current_radius == 0 ) {
00105       // Special case, after first reset
00106       current_radius += radius_increment;
00107       x = 0;
00108       y = current_radius;
00109       ok = true;
00110     } else {
00111 
00112       if ( x < y ) {
00113 
00114         switch (sector) {
00115         case 0:
00116           tmp_x = x;
00117           tmp_y = -y;
00118           break;
00119 
00120         case 1:
00121           tmp_x = y;
00122           tmp_y = -x;
00123           break;
00124 
00125         case 2:
00126           tmp_x = y;
00127           tmp_y = x;
00128           break;
00129 
00130         case 3:
00131           tmp_x = x;
00132           tmp_y = y;
00133           break;
00134 
00135         case 4:
00136           tmp_x = -x;
00137           tmp_y = y;
00138           break;
00139 
00140         case 5:
00141           tmp_x = -y;
00142           tmp_y = x;
00143           break;
00144 
00145         case 6:
00146           tmp_x = -y;
00147           tmp_y = -x;
00148           break;
00149 
00150         case 7:
00151           tmp_x = -x;
00152           tmp_y = -y;
00153           break;
00154 
00155         default:
00156           tmp_x = 0;
00157           tmp_y = 0;
00158           break;
00159 
00160         }
00161 
00162         x += step;
00163         y = (int)(sqrt( (float(current_radius * current_radius) - float(x * x)) ) + 0.5);
00164 
00165         ok = true;
00166 
00167       } else {
00168         //      cout << "x !< y" << endl;
00169         if (sector == 7) {
00170           // Need to go to next circle
00171           current_radius += radius_increment;
00172           x = 0;
00173           y = current_radius;
00174           sector = 0;
00175           if (current_radius >= max_radius) { done = true; ok = true; }
00176         } else {
00177           sector += 1;
00178           x = 0;
00179           y = current_radius;
00180         }
00181       }
00182 
00183     }
00184 
00185     if ( (tmp_x < -(int)center_x) ||
00186          (tmp_x > (int)(width - center_x)) ||
00187          (tmp_y < -(int)center_y) ||
00188          (tmp_y > (int)(height - center_y))
00189          ) {
00190       coord.x = 0;
00191       coord.y = 0;
00192       // out of image, not ok
00193       ok = false;
00194       //done = true;
00195     } else {
00196       coord.x = center_x + tmp_x;
00197       coord.y = center_y + tmp_y;
00198     }
00199 
00200   } while (! ok);
00201 
00202   return &coord;
00203 }
00204 
00205 point_t *
00206 ScanlineRadial::operator++(int)
00207 {
00208   memcpy(&tmp_coord, &coord, sizeof(point_t));
00209   return &tmp_coord;
00210 }
00211 
00212 bool
00213 ScanlineRadial::finished()
00214 {
00215   return done;
00216 }
00217 
00218 
00219 /** Do a simple sort of the given array, sorted descending, biggest first
00220  * this sort is stable
00221  */
00222 void
00223 ScanlineRadial::simpleBubbleSort(unsigned int array[], unsigned int num_elements)
00224 {
00225   bool modified = false;
00226   unsigned int end = num_elements;
00227   unsigned int tmp;
00228   do {
00229     modified = false;
00230 
00231     for (unsigned int i = 0; i < end-1; ++i) {
00232       if ( array[i] < array[i+1] ) {
00233         tmp        = array[i];
00234         array[i]   = array[i+1];
00235         array[i+1] = tmp;
00236         end -= 1;
00237         modified = true;
00238       }
00239     }
00240 
00241   } while ( modified );
00242 }
00243 
00244 void
00245 ScanlineRadial::reset()
00246 {
00247   current_radius = radius_increment;
00248   while (current_radius < dead_radius) {
00249     current_radius += radius_increment;
00250   }
00251   x = 0;
00252   y = current_radius;
00253   sector = 0;
00254 
00255   coord.x = center_x;
00256   coord.y = center_y;
00257 
00258   if ( auto_max_radius ) {
00259     // Calculate distances to corners of image
00260     unsigned int dists[4];
00261     dists[0] = (unsigned int)sqrt( float(center_x * center_x) + float(center_y * center_y) );
00262     dists[1] = (unsigned int)sqrt( float((width - center_x) * (width - center_x)) + float(center_y * center_y) );
00263     dists[2] = (unsigned int)sqrt( float((width - center_x) * (width - center_x)) + float((height - center_y) * (height - center_y)) );
00264     dists[3] = (unsigned int)sqrt( float(center_x * center_x) + float((height - center_y) * (height - center_y)) );
00265 
00266     // now the maximum corner distance is the maximum radius
00267     simpleBubbleSort(dists, 4);
00268     max_radius = dists[0] - 1;
00269   }
00270 
00271   done = false;
00272 
00273   if (radius_increment > max_radius) {
00274     // cout << msg_prefix << cred << "radius_increment > max_radius, resetting radius_increment to one!" << cnormal << endl;
00275     radius_increment = 1;
00276   }
00277 
00278   if (dead_radius > max_radius) {
00279     // cout << msg_prefix << cred << "dead_radius > max_radius, resetting dead_radius to zero!" << cnormal << endl;
00280     dead_radius = 0;
00281     current_radius = radius_increment;
00282   }
00283 
00284 }
00285 
00286 const char *
00287 ScanlineRadial::get_name()
00288 {
00289   return "ScanlineModel::Radial";
00290 }
00291 
00292 
00293 unsigned int
00294 ScanlineRadial::get_margin()
00295 {
00296   return radius_increment;
00297 }
00298 
00299 
00300 /** Set new center point.
00301  * Sets new center point to move around the scanlines in the image.
00302  * Does an implicit reset().
00303  * @param center_x x coordinate of the new center
00304  * @param center_y y coordinate of the new center
00305  */
00306 void
00307 ScanlineRadial::set_center(unsigned int center_x, unsigned int center_y)
00308 {
00309   this->center_x = center_x;
00310   this->center_y = center_y;
00311   reset();
00312 }
00313 
00314 
00315 /** Set new radius.
00316  * Sets the new maximum and dead radius. Does an implicit reset().
00317  * @param dead_radius new dead radius
00318  * @param max_radius new maximum radius, if set to 0 this is automatically
00319  * calculated depending on the image size.
00320  */
00321 void
00322 ScanlineRadial::set_radius(unsigned int dead_radius, unsigned int max_radius)
00323 {
00324   this->max_radius      = 0;
00325   this->dead_radius     = dead_radius;
00326   this->auto_max_radius = (max_radius == 0);
00327 
00328   reset();
00329 }
00330 
00331 } // end namespace firevision

Generated on Tue Feb 22 13:31:17 2011 for Fawkes API by  doxygen 1.4.7