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 <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
00035 }
00036 #endif
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056
00057
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
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
00169 if (sector == 7) {
00170
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
00193 ok = false;
00194
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
00220
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
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
00267 simpleBubbleSort(dists, 4);
00268 max_radius = dists[0] - 1;
00269 }
00270
00271 done = false;
00272
00273 if (radius_increment > max_radius) {
00274
00275 radius_increment = 1;
00276 }
00277
00278 if (dead_radius > max_radius) {
00279
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
00301
00302
00303
00304
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
00316
00317
00318
00319
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 }