mawei/onepilot
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

ui: Fix path artefact when going over hill (#24285)

Browse Source 
old-commit-hash: 38300474bbf2d956ca2482df7c2f93db43b34d27
This commit is contained in:
Willem Melching
2022-04-21 16:18:48 +02:00
committed by GitHub
parent ebec632ea2
commit c1091a67cd
1 changed files with 24 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

32
selfdrive/ui/ui.cc
View File

@@ -55,17 +55,33 @@ static void update_leads(UIState *s, const cereal::RadarState::Reader &radar_sta
}
static void update_line_data(const UIState *s, const cereal::ModelDataV2::XYZTData::Reader &line,
float y_off, float z_off, line_vertices_data *pvd, int max_idx) {
float y_off, float z_off, line_vertices_data *pvd, int max_idx, bool allow_invert=true) {
const auto line_x = line.getX(), line_y = line.getY(), line_z = line.getZ();
QPointF *v = &pvd->v[0];
std::vector<QPointF> left_points, right_points;
for (int i = 0; i <= max_idx; i++) {
v += calib_frame_to_full_frame(s, line_x[i], line_y[i] - y_off, line_z[i] + z_off, v);
QPointF left, right;
bool l = calib_frame_to_full_frame(s, line_x[i], line_y[i] - y_off, line_z[i] + z_off, &left);
bool r = calib_frame_to_full_frame(s, line_x[i], line_y[i] + y_off, line_z[i] + z_off, &right);
if (l && r) {
// For wider lines the drawn polygon will "invert" when going over a hill and cause artifacts
if (!allow_invert && left_points.size() && left.y() > left_points.back().y()) {
continue;
}
left_points.push_back(left);
right_points.push_back(right);
}
}
for (int i = max_idx; i >= 0; i--) {
v += calib_frame_to_full_frame(s, line_x[i], line_y[i] + y_off, line_z[i] + z_off, v);
}
pvd->cnt = v - pvd->v;
pvd->cnt = 2 * left_points.size();
assert(left_points.size() == right_points.size());
assert(pvd->cnt <= std::size(pvd->v));
for (int left_idx = 0; left_idx < left_points.size(); left_idx++){
int right_idx = 2 * left_points.size() - left_idx - 1;
pvd->v[left_idx] = left_points[left_idx];
pvd->v[right_idx] = right_points[left_idx];
}
}
static void update_model(UIState *s, const cereal::ModelDataV2::Reader &model) {
@@ -98,7 +114,7 @@ static void update_model(UIState *s, const cereal::ModelDataV2::Reader &model) {
max_distance = std::clamp((float)(lead_d - fmin(lead_d * 0.35, 10.)), 0.0f, max_distance);
}
max_idx = get_path_length_idx(model_position, max_distance);
update_line_data(s, model_position, scene.end_to_end ? 0.9 : 0.5, 1.22, &scene.track_vertices, max_idx);
update_line_data(s, model_position, scene.end_to_end ? 0.9 : 0.5, 1.22, &scene.track_vertices, max_idx, false);
}
static void update_sockets(UIState *s) {