diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index 86e8676fb7..5bf2ab5ff9 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -531,67 +531,33 @@ void AnnotatedCameraWidget::drawLaneLines(QPainter &painter, const UIState *s) { [&](const QPointF &point) { return point.y() >= 0 && point.y() <= height(); }); qDebug() << "valid_points len:" << valid_points.length() << "right_points len:" << right_points.length(); - float prev_y_pos = -1; - int drawn = 0; - for (int i = 0; i < right_points.length(); i++) { - const auto &acceleration = sm["uiPlan"].getUiPlan().getAccel(); - if (i >= acceleration.size()) {qDebug() << "breaking"; break; }; - -// if (right_points[i].y() - prev_y_pos < 0.1) continue; - - // Some points are out of frame - if (right_points[i].y() < 0 || right_points[i].y() > height()) continue; - - // flip so 0 is bottom of frame - float lin_grad_point = (height() - right_points[i].y()) / height(); - qDebug() << "lin_grad_point:" << lin_grad_point; - -// if (lin_grad_point - prev_y_pos < 0.01) continue; - prev_y_pos = lin_grad_point; - - // speed up: 120, slow down: 0 - float path_hue = fmax(fmin(60 + acceleration[i] * 35, 120), 0); - // FIXME: painter.drawPolygon can be slow if hue is not rounded - path_hue = int(path_hue * 100 + 0.5) / 100; - - float saturation = fmin(fabs(acceleration[i] * 1.5), 1); - float lightness = interp1d(saturation, 0.0, 1.0, 0.95, 0.62); // lighter when grey - float alpha = interp1d(lin_grad_point, 0.75 / 2., 0.75, 0.4, 0.0); // matches previous alpha fade - bg.setColorAt(lin_grad_point, QColor::fromHslF(path_hue / 360., saturation, lightness, alpha)); - drawn += 1; - qDebug() << "i:" << i << "len points:" << valid_points.length(); - -// if (i == valid_points.length() - 1) break; + int final_idx = 0; + if (!valid_points.isEmpty()) { + for (int p = 0; p < MAX_PATH_POINTS; p++) { + int i = (p / (MAX_PATH_POINTS - 1)) * (valid_points.length() - 1); + final_idx = i; + + qDebug() << "i:" << i << "len points:" << valid_points.length(); + const auto &acceleration = sm["uiPlan"].getUiPlan().getAccel(); + if (i >= acceleration.size()) break; + + // flip so 0 is bottom of frame + float lin_grad_point = (height() - valid_points[i].y()) / height(); + qDebug() << "lin_grad_point:" << lin_grad_point; + + // speed up: 120, slow down: 0 + float path_hue = fmax(fmin(60 + acceleration[i] * 35, 120), 0); + // FIXME: painter.drawPolygon can be slow if hue is not rounded + path_hue = int(path_hue * 100 + 0.5) / 100; + + float saturation = fmin(fabs(acceleration[i] * 1.5), 1); + float lightness = interp1d(saturation, 0.0, 1.0, 0.95, 0.62); // lighter when grey + float alpha = interp1d(lin_grad_point, 0.75 / 2., 0.75, 0.4, 0.0); // matches previous alpha fade + bg.setColorAt(lin_grad_point, QColor::fromHslF(path_hue / 360., saturation, lightness, alpha)); + if (i == valid_points.length() - 1) break; + } + assert(final_idx == (valid_points.length() - 1)); } - qDebug() << "drawn:" << drawn; - -// int final_idx = 0; -// if (!valid_points.isEmpty()) { -// for (int p = 0; p < MAX_PATH_POINTS; p++) { -// int i = (p / (MAX_PATH_POINTS - 1)) * (valid_points.length() - 1); -// final_idx = i; -// -// qDebug() << "i:" << i << "len points:" << valid_points.length(); -// const auto &acceleration = sm["uiPlan"].getUiPlan().getAccel(); -// if (i >= acceleration.size()) break; -// -// // flip so 0 is bottom of frame -// float lin_grad_point = (height() - valid_points[i].y()) / height(); -// qDebug() << "lin_grad_point:" << lin_grad_point; -// -// // speed up: 120, slow down: 0 -// float path_hue = fmax(fmin(60 + acceleration[i] * 35, 120), 0); -// // FIXME: painter.drawPolygon can be slow if hue is not rounded -// path_hue = int(path_hue * 100 + 0.5) / 100; -// -// float saturation = fmin(fabs(acceleration[i] * 1.5), 1); -// float lightness = interp1d(saturation, 0.0, 1.0, 0.95, 0.62); // lighter when grey -// float alpha = interp1d(lin_grad_point, 0.75 / 2., 0.75, 0.4, 0.0); // matches previous alpha fade -// bg.setColorAt(lin_grad_point, QColor::fromHslF(path_hue / 360., saturation, lightness, alpha)); -// if (i == valid_points.length() - 1) break; -// } -// assert(final_idx == (valid_points.length() - 1)); -// } } else { bg.setColorAt(0.0, QColor::fromHslF(148 / 360., 0.94, 0.51, 0.4));