diff --git a/selfdrive/ui/paint.cc b/selfdrive/ui/paint.cc index 1bbbb8379c..147c32b701 100644 --- a/selfdrive/ui/paint.cc +++ b/selfdrive/ui/paint.cc @@ -302,12 +302,10 @@ void ui_resize(UIState *s, int width, int height) { // Apply transformation such that video pixel coordinates match video // 1) Put (0, 0) in the middle of the video - nvgTranslate(s->vg, width / 2, height / 2 + y_offset); // 2) Apply same scaling as video - nvgScale(s->vg, zoom, zoom); // 3) Put (0, 0) in top left corner of video - nvgTranslate(s->vg, -intrinsic_matrix.v[2], -intrinsic_matrix.v[5]); - - nvgCurrentTransform(s->vg, s->car_space_transform); - nvgResetTransform(s->vg); + s->car_space_transform.reset(); + s->car_space_transform.translate(width / 2, height / 2 + y_offset) + .scale(zoom, zoom) + .translate(-intrinsic_matrix.v[2], -intrinsic_matrix.v[5]); } diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 3d151149c4..f38b15cf55 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -19,16 +19,20 @@ // image space. static bool calib_frame_to_full_frame(const UIState *s, float in_x, float in_y, float in_z, vertex_data *out) { const float margin = 500.0f; + const QRectF clip_region{-margin, -margin, s->fb_w + 2 * margin, s->fb_h + 2 * margin}; + const vec3 pt = (vec3){{in_x, in_y, in_z}}; const vec3 Ep = matvecmul3(s->scene.view_from_calib, pt); const vec3 KEp = matvecmul3(s->wide_camera ? ecam_intrinsic_matrix : fcam_intrinsic_matrix, Ep); // Project. - float x = KEp.v[0] / KEp.v[2]; - float y = KEp.v[1] / KEp.v[2]; - - nvgTransformPoint(&out->x, &out->y, s->car_space_transform, x, y); - return out->x >= -margin && out->x <= s->fb_w + margin && out->y >= -margin && out->y <= s->fb_h + margin; + QPointF point = s->car_space_transform.map(QPointF{KEp.v[0] / KEp.v[2], KEp.v[1] / KEp.v[2]}); + if (clip_region.contains(point)) { + out->x = point.x(); + out->y = point.y(); + return true; + } + return false; } static int get_path_length_idx(const cereal::ModelDataV2::XYZTData::Reader &line, const float path_height) { diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index 3f52bf120d..ad754b550b 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -7,7 +7,7 @@ #include #include #include - +#include #include "nanovg.h" #include "cereal/messaging/messaging.h" @@ -146,7 +146,7 @@ typedef struct UIState { bool awake; bool has_prime = false; - float car_space_transform[6]; + QTransform car_space_transform; bool wide_camera; float running_time;