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 <fvutils/draw/drawer.h>
00025 #include <fvutils/color/yuv.h>
00026
00027 #include <cmath>
00028 #include <algorithm>
00029 #include <unistd.h>
00030
00031 namespace firevision {
00032 #if 0
00033 }
00034 #endif
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044 Drawer::Drawer()
00045 {
00046 __buffer = NULL;
00047 __color = YUV_t::white();
00048 }
00049
00050
00051 Drawer::~Drawer()
00052 {
00053 }
00054
00055
00056
00057
00058
00059
00060
00061 void
00062 Drawer::set_buffer(unsigned char *buffer,
00063 unsigned int width, unsigned int height)
00064 {
00065 this->__buffer = buffer;
00066 this->__width = width;
00067 this->__height = height;
00068 }
00069
00070
00071
00072
00073
00074
00075
00076 void
00077 Drawer::set_color(unsigned char y, unsigned char u, unsigned char v)
00078 {
00079 __color.Y = y;
00080 __color.U = u;
00081 __color.V = v;
00082 }
00083
00084
00085
00086
00087
00088 void
00089 Drawer::set_color(YUV_t color)
00090 {
00091 __color = color;
00092 }
00093
00094
00095
00096
00097
00098
00099
00100
00101 void
00102 Drawer::draw_circle(int center_x, int center_y, unsigned int radius)
00103 {
00104
00105 if (__buffer == NULL) return;
00106
00107 unsigned int x = 0,
00108 y = radius,
00109 r2 = radius * radius;
00110
00111 unsigned char *up = YUV422_PLANAR_U_PLANE(__buffer, __width, __height);
00112 unsigned char *vp = YUV422_PLANAR_V_PLANE(__buffer, __width, __height);
00113
00114 unsigned int x_tmp, y_tmp, ind_tmp;
00115
00116 while (x <= y) {
00117
00118 x_tmp = center_x + x;
00119 y_tmp = center_y + y;
00120 if ( (x_tmp < __width) && (y_tmp < __height) ) {
00121 ind_tmp = y_tmp * __width + x_tmp;
00122 __buffer[ind_tmp] = __color.Y;
00123 ind_tmp /= 2;
00124 up[ind_tmp] = __color.U;
00125 vp[ind_tmp] = __color.V;
00126 }
00127
00128 x_tmp = center_x - x;
00129 y_tmp = center_y + y;
00130 if ( (x_tmp < __width) && (y_tmp < __height) ) {
00131 ind_tmp = y_tmp * __width + x_tmp;
00132 __buffer[ind_tmp] = __color.Y;
00133 ind_tmp /= 2;
00134 up[ind_tmp] = __color.U;
00135 vp[ind_tmp] = __color.V;
00136 }
00137
00138 x_tmp = center_x + y;
00139 y_tmp = center_y + x;
00140 if ( (x_tmp < __width) && (y_tmp < __height) ) {
00141 ind_tmp = y_tmp * __width + x_tmp;
00142 __buffer[ind_tmp] = __color.Y;
00143 ind_tmp /= 2;
00144 up[ind_tmp] = __color.U;
00145 vp[ind_tmp] = __color.V;
00146 }
00147
00148 x_tmp = center_x - y;
00149 y_tmp = center_y + x;
00150 if ( (x_tmp < __width) && (y_tmp < __height) ) {
00151 ind_tmp = y_tmp * __width + x_tmp;
00152 __buffer[ind_tmp] = __color.Y;
00153 ind_tmp /= 2;
00154 up[ind_tmp] = __color.U;
00155 vp[ind_tmp] = __color.V;
00156 }
00157
00158 x_tmp = center_x + x;
00159 y_tmp = center_y - y;
00160 if ( (x_tmp < __width) && (y_tmp < __height) ) {
00161 ind_tmp = y_tmp * __width + x_tmp;
00162 __buffer[ind_tmp] = __color.Y;
00163 ind_tmp /= 2;
00164 up[ind_tmp] = __color.U;
00165 vp[ind_tmp] = __color.V;
00166 }
00167
00168 x_tmp = center_x - x;
00169 y_tmp = center_y - y;
00170 if ( (x_tmp < __width) && (y_tmp < __height)) {
00171 ind_tmp = y_tmp * __width + x_tmp;
00172 __buffer[ind_tmp] = __color.Y;
00173 ind_tmp /= 2;
00174 up[ind_tmp] = __color.U;
00175 vp[ind_tmp] = __color.V;
00176 }
00177
00178 x_tmp = center_x + y;
00179 y_tmp = center_y - x;
00180 if ( (x_tmp < __width) && (y_tmp < __height)) {
00181 ind_tmp = y_tmp * __width + x_tmp;
00182 __buffer[ind_tmp] = __color.Y;
00183 ind_tmp /= 2;
00184 up[ind_tmp] = __color.U;
00185 vp[ind_tmp] = __color.V;
00186 }
00187
00188 x_tmp = center_x - y;
00189 y_tmp = center_y - x;
00190 if ( (x_tmp < __width) && (y_tmp < __height) ) {
00191 ind_tmp = y_tmp * __width + x_tmp;
00192 __buffer[ind_tmp] = __color.Y;
00193 ind_tmp /= 2;
00194 up[ind_tmp] = __color.U;
00195 vp[ind_tmp] = __color.V;
00196 }
00197
00198 ++x;
00199 y=(int)(sqrt((float)(r2 - x * x))+0.5);
00200 }
00201
00202 }
00203
00204
00205
00206
00207
00208
00209
00210
00211 void
00212 Drawer::draw_rectangle(unsigned int x, unsigned int y,
00213 unsigned int w, unsigned int h)
00214 {
00215
00216 unsigned char *up = YUV422_PLANAR_U_PLANE(__buffer, __width, __height);
00217 unsigned char *vp = YUV422_PLANAR_V_PLANE(__buffer, __width, __height);
00218
00219
00220 for (unsigned int i = x; i < x + w; ++i) {
00221 if ( i < __width ) {
00222 __buffer[ y * __width + i ] = __color.Y;
00223 up[ (y * __width + i) / 2 ] = __color.U;
00224 vp[ (y * __width + i) / 2 ] = __color.V;
00225 } else {
00226 break;
00227 }
00228 }
00229
00230
00231 for (unsigned int i = y; i < y + h; ++i) {
00232
00233 __buffer[ i * __width + x ] = __color.Y;
00234 up[ (i * __width + x) / 2 ] = __color.U;
00235 vp[ (i * __width + x) / 2 ] = __color.V;
00236
00237 if ( (x + w) < __width ) {
00238
00239 __buffer[ i * __width + x + w ] = __color.Y;
00240 up[ (i * __width + x + w) / 2 ] = __color.U;
00241 vp[ (i * __width + x + w) / 2 ] = __color.V;
00242 }
00243 }
00244
00245
00246 for (unsigned int i = x; i < x + w; ++i) {
00247 if ( i < __width ) {
00248 __buffer[ (y + h) * __width + i ] = __color.Y;
00249 up[ ((y + h) * __width + i) / 2 ] = __color.U;
00250 vp[ ((y + h) * __width + i) / 2 ] = __color.V;
00251 } else {
00252 break;
00253 }
00254 }
00255
00256 }
00257
00258
00259
00260
00261
00262
00263
00264
00265
00266
00267 void
00268 Drawer::draw_rectangle_inverted(unsigned int x, unsigned int y,
00269 unsigned int w, unsigned int h)
00270 {
00271
00272 unsigned int ind = 0;
00273
00274
00275 for (unsigned int i = x; i < x + w; ++i) {
00276 if ( i < __width ) {
00277 ind = y * __width + i;
00278 __buffer[ind] = 255 - __buffer[ind];
00279 } else {
00280 break;
00281 }
00282 }
00283
00284
00285 for (unsigned int i = y; i < y + h; ++i) {
00286
00287 ind = i * __width + x;
00288 __buffer[ind] = 255 - __buffer[ind];
00289
00290 if ( (x + w) < __width ) {
00291
00292 ind += w;
00293 __buffer[ind] = 255 - __buffer[ind];
00294 }
00295 }
00296
00297
00298 for (unsigned int i = x; i < x + w; ++i) {
00299 if ( i < __width ) {
00300 __buffer[ind] = 255 - __buffer[ind];
00301 } else {
00302 break;
00303 }
00304 }
00305
00306 }
00307
00308
00309
00310
00311
00312
00313 void
00314 Drawer::draw_point(unsigned int x, unsigned int y)
00315 {
00316 if ( x > __width) return;
00317 if ( y > __height) return;
00318
00319 unsigned char *up = YUV422_PLANAR_U_PLANE(__buffer, __width, __height);
00320 unsigned char *vp = YUV422_PLANAR_V_PLANE(__buffer, __width, __height);
00321
00322 __buffer[ y * __width + x ] = __color.Y;
00323 up[ (y * __width + x) / 2 ] = __color.U;
00324 vp[ (y * __width + x) / 2 ] = __color.V;
00325 }
00326
00327
00328
00329
00330
00331
00332
00333
00334
00335 void
00336 Drawer::color_point(unsigned int x, unsigned int y)
00337 {
00338 if ( x > __width) return;
00339 if ( y > __height) return;
00340
00341 unsigned char *up = YUV422_PLANAR_U_PLANE(__buffer, __width, __height);
00342 unsigned char *vp = YUV422_PLANAR_V_PLANE(__buffer, __width, __height);
00343
00344 __buffer[ y * __width + x ] = __color.Y;
00345 up[ (y * __width + x) / 2 ] = __color.U;
00346 vp[ (y * __width + x) / 2 ] = __color.V;
00347 }
00348
00349
00350
00351
00352
00353
00354
00355
00356
00357 void
00358 Drawer::color_point(unsigned int x, unsigned int y, YUV_t color)
00359 {
00360 if ( x > __width) return;
00361 if ( y > __height) return;
00362
00363 unsigned char *up = YUV422_PLANAR_U_PLANE(__buffer, __width, __height);
00364 unsigned char *vp = YUV422_PLANAR_V_PLANE(__buffer, __width, __height);
00365
00366 __buffer[ y * __width + x ] = color.Y;
00367 up[ (y * __width + x) / 2 ] = color.U;
00368 vp[ (y * __width + x) / 2 ] = color.V;
00369 }
00370
00371
00372
00373
00374
00375
00376
00377
00378
00379
00380 void
00381 Drawer::draw_line(unsigned int x_start, unsigned int y_start,
00382 unsigned int x_end, unsigned int y_end)
00383 {
00384
00385
00386
00387
00388
00389
00390 int x, y, dist, xerr, yerr, dx, dy, incx, incy;
00391 bool was_inside_image = false;
00392
00393 unsigned char *up = YUV422_PLANAR_U_PLANE(__buffer, __width, __height);
00394 unsigned char *vp = YUV422_PLANAR_V_PLANE(__buffer, __width, __height);
00395
00396
00397 dx = x_end - x_start;
00398 dy = y_end - y_start;
00399
00400
00401 if(dx < 0) {
00402 incx = -1;
00403 dx = -dx;
00404 } else {
00405 incx = dx ? 1 : 0;
00406 }
00407
00408 if(dy < 0) {
00409 incy = -1;
00410 dy = -dy;
00411 } else {
00412 incy = dy ? 1 : 0;
00413 }
00414
00415
00416 dist = (dx > dy) ? dx : dy;
00417
00418
00419 x = x_start;
00420 y = y_start;
00421 xerr = dx;
00422 yerr = dy;
00423
00424
00425 for(int t = 0; t < dist; ++t) {
00426 if ( ((unsigned int)x < __width) && ((unsigned int)y < __height) ) {
00427 if ( (x >= 0) && (y >= 0) ) {
00428 was_inside_image = true;
00429 __buffer[ y * __width + x ] = __color.Y;
00430 up[ (y * __width + x) / 2 ] = __color.U;
00431 vp[ (y * __width + x) / 2 ] = __color.V;
00432 }
00433 } else {
00434 if ( was_inside_image ) {
00435 break;
00436 }
00437 }
00438
00439 xerr += dx;
00440 yerr += dy;
00441
00442 if(xerr > dist) {
00443 xerr -= dist;
00444 x += incx;
00445 }
00446
00447 if(yerr>dist) {
00448 yerr -= dist;
00449 y += incy;
00450 }
00451 }
00452
00453 if ( (x_end < __width) && (y_end < __height) ) {
00454 __buffer[ y_end * __width + x_end ] = __color.Y;
00455 up[ (y_end * __width + x_end) / 2 ] = __color.U;
00456 vp[ (y_end * __width + x_end) / 2 ] = __color.V;
00457 }
00458
00459 }
00460
00461
00462
00463
00464
00465
00466 void
00467 Drawer::draw_cross(unsigned int x_center, unsigned int y_center, unsigned int width)
00468 {
00469 x_center = std::min(x_center, __width);
00470 y_center = std::min(y_center, __height);
00471
00472 int r = width / 2;
00473 unsigned int a = std::max(0, (int)x_center - r);
00474 unsigned int b = std::min(x_center + r, __width);
00475 draw_line(a, y_center, b, y_center);
00476
00477 a = std::max(0, (int)y_center - r);
00478 b = std::min(y_center + r, __height);
00479 draw_line(x_center, a, x_center, b);
00480 }
00481
00482 }