22 #include <core/exceptions/software.h>
23 #include <fvutils/base/roi.h>
24 #include <fvutils/draw/drawer.h>
25 #include <fvutils/draw/field_drawer.h>
26 #include <fvutils/ipc/shm_image.h>
34 namespace firevision {
56 FieldDrawer::FieldDrawer(
const FieldLines &lines) : lines_(lines)
99 own_position_ = own_position;
110 own_pos_est_ = own_position_estimate;
120 own_position_.
ori = 12345;
121 own_pos_est_.
ori = 12345;
150 points_est_ = points_est;
166 return std::min(img_width / f_width, img_height / f_height);
176 c_background_ = color;
206 c_line_points_ = color;
216 c_line_points_est_ = color;
236 c_own_pos_est_ = color;
251 unsigned int img_width,
252 unsigned int img_height,
253 bool draw_background,
264 if (draw_background) {
265 unsigned int draw_width = static_cast<unsigned int>(f_width * scale);
266 unsigned int draw_height = static_cast<unsigned int>(f_height * scale);
268 unsigned int v_offset = u_offset + u_offset / 2;
271 unsigned int offset = (
_img_height - draw_height) / 2;
298 unsigned int sx = (
_img_width - draw_width) / 2;
302 for (
unsigned int x = 0; x <
_img_width; ++x) {
325 unsigned int center_x =
326 std::max(0, static_cast<int>(
_img_width / 2) + static_cast<int>(f_offs.
x * scale));
327 unsigned int center_y =
328 std::max(0, static_cast<int>(
_img_height / 2) + static_cast<int>(f_offs.
y * scale));
330 if (own_pos_est_.
ori != 12345) {
335 int x = static_cast<int>(own_pos_est_.
x * scale);
336 int y = static_cast<int>(own_pos_est_.
y * scale);
337 int dx = static_cast<int>(r * cosf(own_pos_est_.
ori));
338 int dy = static_cast<int>(r * sinf(own_pos_est_.
ori));
340 if (draw_landscape) {
352 if (head_yaw_ != 12345) {
353 int hx = static_cast<int>(r * cosf(own_pos_est_.
ori + head_yaw_));
354 int hy = static_cast<int>(r * sinf(own_pos_est_.
ori + head_yaw_));
355 int hdx = static_cast<int>((r + 4) * cosf(own_pos_est_.
ori + head_yaw_));
356 int hdy = static_cast<int>((r + 4) * sinf(own_pos_est_.
ori + head_yaw_));
359 d.
draw_line(x + hx, y - hy, x + hdx, y - hdy);
361 d.
draw_line(y + hy, x - hx, y + hdy, x - hdx);
365 if (own_position_.
ori != 12345) {
370 int x = static_cast<int>(own_position_.
x * scale);
371 int y = static_cast<int>(own_position_.
y * scale);
372 int dx = static_cast<int>(r * cosf(own_position_.
ori));
373 int dy = static_cast<int>(r * sinf(own_position_.
ori));
375 if (draw_landscape) {
387 if (head_yaw_ != 12345) {
388 int hx = static_cast<int>(r * cosf(own_position_.
ori + head_yaw_));
389 int hy = static_cast<int>(r * sinf(own_position_.
ori + head_yaw_));
390 int hdx = static_cast<int>((r + 4) * cosf(own_position_.
ori + head_yaw_));
391 int hdy = static_cast<int>((r + 4) * sinf(own_position_.
ori + head_yaw_));
394 d.
draw_line(x + hx, y - hy, x + hdx, y - hdy);
396 d.
draw_line(y + hy, x - hx, y + hdy, x - hdx);
422 unsigned int center_x =
423 std::max(0, static_cast<int>(
_img_width / 2) + static_cast<int>(f_offs.
x * scale));
424 unsigned int center_y =
425 std::max(0, static_cast<int>(
_img_height / 2) + static_cast<int>(f_offs.
y * scale));
432 for (fld_line_points_t::const_iterator it = points_est_->begin(); it != points_est_->end();
435 static_cast<unsigned int>(center_y - (draw_landscape ? it->y : it->x) * scale);
437 static_cast<unsigned int>((draw_landscape ? it->x : it->y) * scale + center_x);
445 for (fld_line_points_t::const_iterator it = points_->begin(); it != points_->end(); ++it) {
447 static_cast<unsigned int>(center_y - (draw_landscape ? it->y : it->x) * scale);
449 static_cast<unsigned int>((draw_landscape ? it->x : it->y) * scale + center_x);
476 int f_off_x = static_cast<int>(f_offs.
x * scale);
477 int f_off_y = static_cast<int>(f_offs.
y * scale);
479 unsigned int off_x = std::max(0, static_cast<int>(
_img_width / 2) + f_off_x);
480 unsigned int off_y = std::max(0, static_cast<int>(
_img_height / 2) + f_off_y);
486 for (FieldLines::const_iterator it = lines_.begin(); it != lines_.end(); ++it) {
488 static_cast<unsigned int>((draw_landscape ? (*it).start.x : (*it).start.y) * scale);
490 static_cast<unsigned int>((draw_landscape ? (*it).start.y : (*it).start.x) * scale);
492 static_cast<unsigned int>((draw_landscape ? (*it).end.x : (*it).end.y) * scale);
494 static_cast<unsigned int>((draw_landscape ? (*it).end.y : (*it).end.x) * scale);
496 d.
draw_line(off_x + sx, off_y + sy, off_x + ex, off_y + ey);
499 for (field_circles_t::const_iterator it = lines_.
get_circles().begin();
503 static_cast<unsigned int>((draw_landscape ? it->center.x : it->center.y) * scale);
505 static_cast<unsigned int>((draw_landscape ? it->center.y : it->center.x) * scale);
506 unsigned int r = static_cast<unsigned int>(it->radius * scale);