diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index 5bf2ab5ff9..86e8676fb7 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -531,33 +531,67 @@ 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(); - 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)); + 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; } + 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));