2021-05-08 23:15:17 -06:00
|
|
|
#include "paint.h"
|
|
|
|
|
2021-04-29 12:18:59 -06:00
|
|
|
#include <assert.h>
|
2021-05-08 23:15:17 -06:00
|
|
|
|
2021-04-29 12:18:59 -06:00
|
|
|
#include <algorithm>
|
|
|
|
|
|
|
|
#ifdef __APPLE__
|
|
|
|
#include <OpenGL/gl3.h>
|
|
|
|
#define NANOVG_GL3_IMPLEMENTATION
|
|
|
|
#define nvgCreate nvgCreateGL3
|
|
|
|
#else
|
|
|
|
#include <GLES3/gl3.h>
|
|
|
|
#define NANOVG_GLES3_IMPLEMENTATION
|
|
|
|
#define nvgCreate nvgCreateGLES3
|
|
|
|
#endif
|
2020-12-18 17:40:23 -07:00
|
|
|
|
2020-01-17 12:05:23 -07:00
|
|
|
#define NANOVG_GLES3_IMPLEMENTATION
|
2021-05-08 23:15:17 -06:00
|
|
|
#include <nanovg_gl.h>
|
|
|
|
#include <nanovg_gl_utils.h>
|
|
|
|
|
|
|
|
#include "selfdrive/common/timing.h"
|
|
|
|
#include "selfdrive/common/util.h"
|
2021-05-06 06:39:05 -06:00
|
|
|
#include "selfdrive/hardware/hw.h"
|
2020-08-20 09:16:44 -06:00
|
|
|
|
2021-05-08 23:15:17 -06:00
|
|
|
#include "selfdrive/ui/ui.h"
|
|
|
|
|
2020-01-17 12:05:23 -07:00
|
|
|
// TODO: this is also hardcoded in common/transformations/camera.py
|
2020-09-23 15:16:55 -06:00
|
|
|
// TODO: choose based on frame input size
|
2021-05-06 06:39:05 -06:00
|
|
|
const float y_offset = Hardware::TICI() ? 150.0 : 0.0;
|
|
|
|
const float zoom = Hardware::TICI() ? 2912.8 : 2138.5;
|
2020-01-17 12:05:23 -07:00
|
|
|
|
2021-03-10 21:52:01 -07:00
|
|
|
static void ui_draw_text(const UIState *s, float x, float y, const char *string, float size, NVGcolor color, const char *font_name) {
|
2021-01-16 14:50:06 -07:00
|
|
|
nvgFontFace(s->vg, font_name);
|
|
|
|
nvgFontSize(s->vg, size);
|
|
|
|
nvgFillColor(s->vg, color);
|
|
|
|
nvgText(s->vg, x, y, string, NULL);
|
2020-05-02 12:27:03 -06:00
|
|
|
}
|
|
|
|
|
2021-01-28 07:53:24 -07:00
|
|
|
static void draw_chevron(UIState *s, float x, float y, float sz, NVGcolor fillColor, NVGcolor glowColor) {
|
2020-01-17 12:05:23 -07:00
|
|
|
// glow
|
|
|
|
float g_xo = sz/5;
|
|
|
|
float g_yo = sz/10;
|
2020-04-17 16:59:43 -06:00
|
|
|
nvgBeginPath(s->vg);
|
|
|
|
nvgMoveTo(s->vg, x+(sz*1.35)+g_xo, y+sz+g_yo);
|
|
|
|
nvgLineTo(s->vg, x, y-g_xo);
|
|
|
|
nvgLineTo(s->vg, x-(sz*1.35)-g_xo, y+sz+g_yo);
|
|
|
|
nvgClosePath(s->vg);
|
2020-01-17 12:05:23 -07:00
|
|
|
nvgFillColor(s->vg, glowColor);
|
|
|
|
nvgFill(s->vg);
|
|
|
|
|
|
|
|
// chevron
|
|
|
|
nvgBeginPath(s->vg);
|
2020-04-17 16:59:43 -06:00
|
|
|
nvgMoveTo(s->vg, x+(sz*1.25), y+sz);
|
|
|
|
nvgLineTo(s->vg, x, y);
|
|
|
|
nvgLineTo(s->vg, x-(sz*1.25), y+sz);
|
|
|
|
nvgClosePath(s->vg);
|
2020-01-17 12:05:23 -07:00
|
|
|
nvgFillColor(s->vg, fillColor);
|
|
|
|
nvgFill(s->vg);
|
|
|
|
}
|
|
|
|
|
2021-03-30 05:32:07 -06:00
|
|
|
static void ui_draw_circle_image(const UIState *s, int center_x, int center_y, int radius, const char *image, NVGcolor color, float img_alpha) {
|
2021-01-16 14:50:06 -07:00
|
|
|
nvgBeginPath(s->vg);
|
2021-03-30 05:32:07 -06:00
|
|
|
nvgCircle(s->vg, center_x, center_y, radius);
|
2021-01-16 14:50:06 -07:00
|
|
|
nvgFillColor(s->vg, color);
|
|
|
|
nvgFill(s->vg);
|
2021-03-30 05:32:07 -06:00
|
|
|
const int img_size = radius * 1.5;
|
|
|
|
ui_draw_image(s, {center_x - (img_size / 2), center_y - (img_size / 2), img_size, img_size}, image, img_alpha);
|
2020-05-01 20:16:17 -06:00
|
|
|
}
|
|
|
|
|
2021-03-30 05:32:07 -06:00
|
|
|
static void ui_draw_circle_image(const UIState *s, int center_x, int center_y, int radius, const char *image, bool active) {
|
2020-05-01 20:16:17 -06:00
|
|
|
float bg_alpha = active ? 0.3f : 0.1f;
|
|
|
|
float img_alpha = active ? 1.0f : 0.15f;
|
2021-03-30 05:32:07 -06:00
|
|
|
ui_draw_circle_image(s, center_x, center_y, radius, image, nvgRGBA(0, 0, 0, (255 * bg_alpha)), img_alpha);
|
2020-05-01 20:16:17 -06:00
|
|
|
}
|
|
|
|
|
2021-05-14 23:47:06 -06:00
|
|
|
static void draw_lead(UIState *s, const cereal::RadarState::LeadData::Reader &lead_data, const vertex_data &vd) {
|
2020-04-17 16:59:43 -06:00
|
|
|
// Draw lead car indicator
|
2021-05-14 23:47:06 -06:00
|
|
|
auto [x, y] = vd;
|
2021-01-28 07:53:24 -07:00
|
|
|
|
2020-04-17 16:59:43 -06:00
|
|
|
float fillAlpha = 0;
|
|
|
|
float speedBuff = 10.;
|
|
|
|
float leadBuff = 40.;
|
2021-05-14 23:47:06 -06:00
|
|
|
float d_rel = lead_data.getDRel();
|
|
|
|
float v_rel = lead_data.getVRel();
|
2020-04-17 16:59:43 -06:00
|
|
|
if (d_rel < leadBuff) {
|
|
|
|
fillAlpha = 255*(1.0-(d_rel/leadBuff));
|
|
|
|
if (v_rel < 0) {
|
|
|
|
fillAlpha += 255*(-1*(v_rel/speedBuff));
|
2020-02-20 15:27:11 -07:00
|
|
|
}
|
2020-04-17 16:59:43 -06:00
|
|
|
fillAlpha = (int)(fmin(fillAlpha, 255));
|
|
|
|
}
|
2021-01-28 07:53:24 -07:00
|
|
|
|
2021-04-07 11:12:35 -06:00
|
|
|
float sz = std::clamp((25 * 30) / (d_rel / 3 + 30), 15.0f, 30.0f) * s->zoom;
|
2021-02-01 17:10:15 -07:00
|
|
|
x = std::clamp(x, 0.f, s->viz_rect.right() - sz / 2);
|
2021-03-10 21:52:01 -07:00
|
|
|
y = std::fmin(s->viz_rect.bottom() - sz * .6, y);
|
2021-01-28 07:53:24 -07:00
|
|
|
draw_chevron(s, x, y, sz, nvgRGBA(201, 34, 49, fillAlpha), COLOR_YELLOW);
|
2020-02-20 15:27:11 -07:00
|
|
|
}
|
2020-01-17 12:05:23 -07:00
|
|
|
|
2021-02-08 04:06:14 -07:00
|
|
|
static void ui_draw_line(UIState *s, const line_vertices_data &vd, NVGcolor *color, NVGpaint *paint) {
|
|
|
|
if (vd.cnt == 0) return;
|
2020-07-03 20:42:26 -06:00
|
|
|
|
2021-02-08 04:06:14 -07:00
|
|
|
const vertex_data *v = &vd.v[0];
|
2020-01-17 12:05:23 -07:00
|
|
|
nvgBeginPath(s->vg);
|
2020-11-12 12:30:04 -07:00
|
|
|
nvgMoveTo(s->vg, v[0].x, v[0].y);
|
2021-02-08 04:06:14 -07:00
|
|
|
for (int i = 1; i < vd.cnt; i++) {
|
2020-11-12 12:30:04 -07:00
|
|
|
nvgLineTo(s->vg, v[i].x, v[i].y);
|
2020-01-17 12:05:23 -07:00
|
|
|
}
|
|
|
|
nvgClosePath(s->vg);
|
2020-11-11 21:31:46 -07:00
|
|
|
if (color) {
|
|
|
|
nvgFillColor(s->vg, *color);
|
|
|
|
} else if (paint) {
|
|
|
|
nvgFillPaint(s->vg, *paint);
|
|
|
|
}
|
2020-01-17 12:05:23 -07:00
|
|
|
nvgFill(s->vg);
|
|
|
|
}
|
|
|
|
|
|
|
|
static void draw_frame(UIState *s) {
|
2020-09-03 17:32:55 -06:00
|
|
|
mat4 *out_mat;
|
2021-03-03 03:38:41 -07:00
|
|
|
if (s->scene.driver_view) {
|
2020-01-17 12:05:23 -07:00
|
|
|
glBindVertexArray(s->frame_vao[1]);
|
|
|
|
out_mat = &s->front_frame_mat;
|
|
|
|
} else {
|
2020-09-03 17:32:55 -06:00
|
|
|
glBindVertexArray(s->frame_vao[0]);
|
2020-01-17 12:05:23 -07:00
|
|
|
out_mat = &s->rear_frame_mat;
|
|
|
|
}
|
|
|
|
glActiveTexture(GL_TEXTURE0);
|
2020-09-03 17:32:55 -06:00
|
|
|
|
2021-01-08 06:54:25 -07:00
|
|
|
if (s->last_frame) {
|
2021-01-11 07:52:58 -07:00
|
|
|
glBindTexture(GL_TEXTURE_2D, s->texture[s->last_frame->idx]->frame_tex);
|
2021-05-06 06:39:05 -06:00
|
|
|
if (!Hardware::EON()) {
|
|
|
|
// this is handled in ion on QCOM
|
|
|
|
glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, s->last_frame->width, s->last_frame->height,
|
|
|
|
0, GL_RGB, GL_UNSIGNED_BYTE, s->last_frame->addr);
|
|
|
|
}
|
2020-01-17 12:05:23 -07:00
|
|
|
}
|
|
|
|
|
2021-01-12 07:43:34 -07:00
|
|
|
glUseProgram(s->gl_shader->prog);
|
|
|
|
glUniform1i(s->gl_shader->getUniformLocation("uTexture"), 0);
|
|
|
|
glUniformMatrix4fv(s->gl_shader->getUniformLocation("uTransform"), 1, GL_TRUE, out_mat->v);
|
2020-01-17 12:05:23 -07:00
|
|
|
|
|
|
|
assert(glGetError() == GL_NO_ERROR);
|
|
|
|
glEnableVertexAttribArray(0);
|
2021-03-10 21:52:01 -07:00
|
|
|
glDrawElements(GL_TRIANGLES, 6, GL_UNSIGNED_BYTE, (const void *)0);
|
2020-01-17 12:05:23 -07:00
|
|
|
glDisableVertexAttribArray(0);
|
|
|
|
glBindVertexArray(0);
|
|
|
|
}
|
|
|
|
|
2020-11-11 21:31:46 -07:00
|
|
|
static void ui_draw_vision_lane_lines(UIState *s) {
|
2021-01-06 03:34:52 -07:00
|
|
|
const UIScene &scene = s->scene;
|
2021-03-24 02:10:31 -06:00
|
|
|
NVGpaint track_bg;
|
|
|
|
if (!scene.end_to_end) {
|
|
|
|
// paint lanelines
|
|
|
|
for (int i = 0; i < std::size(scene.lane_line_vertices); i++) {
|
|
|
|
NVGcolor color = nvgRGBAf(1.0, 1.0, 1.0, scene.lane_line_probs[i]);
|
|
|
|
ui_draw_line(s, scene.lane_line_vertices[i], &color, nullptr);
|
|
|
|
}
|
2020-11-27 15:25:52 -07:00
|
|
|
|
2021-03-24 02:10:31 -06:00
|
|
|
// paint road edges
|
|
|
|
for (int i = 0; i < std::size(scene.road_edge_vertices); i++) {
|
|
|
|
NVGcolor color = nvgRGBAf(1.0, 0.0, 0.0, std::clamp<float>(1.0 - scene.road_edge_stds[i], 0.0, 1.0));
|
|
|
|
ui_draw_line(s, scene.road_edge_vertices[i], &color, nullptr);
|
|
|
|
}
|
|
|
|
track_bg = nvgLinearGradient(s->vg, s->fb_w, s->fb_h, s->fb_w, s->fb_h * .4,
|
|
|
|
COLOR_WHITE, COLOR_WHITE_ALPHA(0));
|
|
|
|
} else {
|
|
|
|
track_bg = nvgLinearGradient(s->vg, s->fb_w, s->fb_h, s->fb_w, s->fb_h * .4,
|
|
|
|
COLOR_RED, COLOR_RED_ALPHA(0));
|
2020-01-17 12:05:23 -07:00
|
|
|
}
|
2020-11-11 21:31:46 -07:00
|
|
|
// paint path
|
2021-02-08 04:06:14 -07:00
|
|
|
ui_draw_line(s, scene.track_vertices, nullptr, &track_bg);
|
2020-01-17 12:05:23 -07:00
|
|
|
}
|
|
|
|
|
|
|
|
// Draw all world space objects.
|
|
|
|
static void ui_draw_world(UIState *s) {
|
2020-11-11 03:12:55 -07:00
|
|
|
// Don't draw on top of sidebar
|
2021-02-01 17:10:15 -07:00
|
|
|
nvgScissor(s->vg, s->viz_rect.x, s->viz_rect.y, s->viz_rect.w, s->viz_rect.h);
|
2020-05-12 19:17:04 -06:00
|
|
|
|
2020-01-17 12:05:23 -07:00
|
|
|
// Draw lane edges and vision/mpc tracks
|
2020-11-11 21:31:46 -07:00
|
|
|
ui_draw_vision_lane_lines(s);
|
2020-01-17 12:05:23 -07:00
|
|
|
|
2020-07-23 14:28:58 -06:00
|
|
|
// Draw lead indicators if openpilot is handling longitudinal
|
2021-03-03 09:40:50 -07:00
|
|
|
if (s->scene.longitudinal_control) {
|
2021-05-14 23:47:06 -06:00
|
|
|
auto radar_state = (*s->sm)["radarState"].getRadarState();
|
|
|
|
auto lead_one = radar_state.getLeadOne();
|
|
|
|
auto lead_two = radar_state.getLeadTwo();
|
|
|
|
if (lead_one.getStatus()) {
|
|
|
|
draw_lead(s, lead_one, s->scene.lead_vertices[0]);
|
2020-07-23 14:28:58 -06:00
|
|
|
}
|
2021-05-14 23:47:06 -06:00
|
|
|
if (lead_two.getStatus() && (std::abs(lead_one.getDRel() - lead_two.getDRel()) > 3.0)) {
|
|
|
|
draw_lead(s, lead_two, s->scene.lead_vertices[1]);
|
2020-07-23 14:28:58 -06:00
|
|
|
}
|
2020-01-17 12:05:23 -07:00
|
|
|
}
|
2021-01-15 03:41:20 -07:00
|
|
|
nvgResetScissor(s->vg);
|
2020-01-17 12:05:23 -07:00
|
|
|
}
|
|
|
|
|
|
|
|
static void ui_draw_vision_maxspeed(UIState *s) {
|
2021-02-05 13:12:33 -07:00
|
|
|
const int SET_SPEED_NA = 255;
|
2021-05-14 23:47:06 -06:00
|
|
|
float maxspeed = (*s->sm)["controlsState"].getControlsState().getVCruise();
|
2021-01-04 07:55:04 -07:00
|
|
|
const bool is_cruise_set = maxspeed != 0 && maxspeed != SET_SPEED_NA;
|
2021-03-03 09:40:50 -07:00
|
|
|
if (is_cruise_set && !s->scene.is_metric) { maxspeed *= 0.6225; }
|
2020-01-17 12:05:23 -07:00
|
|
|
|
2021-02-01 17:10:15 -07:00
|
|
|
const Rect rect = {s->viz_rect.x + (bdr_s * 2), int(s->viz_rect.y + (bdr_s * 1.5)), 184, 202};
|
2021-01-15 03:15:48 -07:00
|
|
|
ui_fill_rect(s->vg, rect, COLOR_BLACK_ALPHA(100), 30.);
|
|
|
|
ui_draw_rect(s->vg, rect, COLOR_WHITE_ALPHA(100), 10, 20.);
|
2020-05-02 12:27:03 -06:00
|
|
|
|
2020-01-17 12:05:23 -07:00
|
|
|
nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BASELINE);
|
2021-01-16 14:50:06 -07:00
|
|
|
ui_draw_text(s, rect.centerX(), 148, "MAX", 26 * 2.5, COLOR_WHITE_ALPHA(is_cruise_set ? 200 : 100), "sans-regular");
|
2020-01-17 12:05:23 -07:00
|
|
|
if (is_cruise_set) {
|
2021-01-04 07:55:04 -07:00
|
|
|
const std::string maxspeed_str = std::to_string((int)std::nearbyint(maxspeed));
|
2021-01-16 14:50:06 -07:00
|
|
|
ui_draw_text(s, rect.centerX(), 242, maxspeed_str.c_str(), 48 * 2.5, COLOR_WHITE, "sans-bold");
|
2020-01-17 12:05:23 -07:00
|
|
|
} else {
|
2021-01-16 14:50:06 -07:00
|
|
|
ui_draw_text(s, rect.centerX(), 242, "N/A", 42 * 2.5, COLOR_WHITE_ALPHA(100), "sans-semibold");
|
2020-01-17 12:05:23 -07:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
static void ui_draw_vision_speed(UIState *s) {
|
2021-05-14 23:47:06 -06:00
|
|
|
const float speed = std::max(0.0, (*s->sm)["carState"].getCarState().getVEgo() * (s->scene.is_metric ? 3.6 : 2.2369363));
|
2021-01-04 06:18:38 -07:00
|
|
|
const std::string speed_str = std::to_string((int)std::nearbyint(speed));
|
2020-01-17 12:05:23 -07:00
|
|
|
nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BASELINE);
|
2021-02-01 17:10:15 -07:00
|
|
|
ui_draw_text(s, s->viz_rect.centerX(), 240, speed_str.c_str(), 96 * 2.5, COLOR_WHITE, "sans-bold");
|
2021-03-03 09:40:50 -07:00
|
|
|
ui_draw_text(s, s->viz_rect.centerX(), 320, s->scene.is_metric ? "km/h" : "mph", 36 * 2.5, COLOR_WHITE_ALPHA(200), "sans-regular");
|
2020-01-17 12:05:23 -07:00
|
|
|
}
|
|
|
|
|
|
|
|
static void ui_draw_vision_event(UIState *s) {
|
2021-05-14 23:47:06 -06:00
|
|
|
if ((*s->sm)["controlsState"].getControlsState().getEngageable()) {
|
2020-01-17 12:05:23 -07:00
|
|
|
// draw steering wheel
|
2021-03-30 05:32:07 -06:00
|
|
|
const int radius = 96;
|
|
|
|
const int center_x = s->viz_rect.right() - radius - bdr_s * 2;
|
|
|
|
const int center_y = s->viz_rect.y + radius + (bdr_s * 1.5);
|
|
|
|
ui_draw_circle_image(s, center_x, center_y, radius, "wheel", bg_colors[s->status], 1.0f);
|
2020-01-17 12:05:23 -07:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
static void ui_draw_vision_face(UIState *s) {
|
2021-03-30 05:32:07 -06:00
|
|
|
const int radius = 96;
|
|
|
|
const int center_x = s->viz_rect.x + radius + (bdr_s * 2);
|
|
|
|
const int center_y = s->viz_rect.bottom() - footer_h / 2;
|
2021-05-14 23:47:06 -06:00
|
|
|
bool is_active = (*s->sm)["driverMonitoringState"].getDriverMonitoringState().getIsActiveMode();
|
|
|
|
ui_draw_circle_image(s, center_x, center_y, radius, "driver_face", is_active);
|
2020-01-17 12:05:23 -07:00
|
|
|
}
|
|
|
|
|
2020-04-15 17:48:44 -06:00
|
|
|
static void ui_draw_driver_view(UIState *s) {
|
2021-01-08 12:30:37 -07:00
|
|
|
const bool is_rhd = s->scene.is_rhd;
|
2021-03-02 20:42:45 -07:00
|
|
|
const int width = 4 * s->viz_rect.h / 3;
|
2021-02-01 17:10:15 -07:00
|
|
|
const Rect rect = {s->viz_rect.centerX() - width / 2, s->viz_rect.y, width, s->viz_rect.h}; // x, y, w, h
|
2021-01-08 12:30:37 -07:00
|
|
|
const Rect valid_rect = {is_rhd ? rect.right() - rect.h / 2 : rect.x, rect.y, rect.h / 2, rect.h};
|
2020-04-15 17:48:44 -06:00
|
|
|
|
|
|
|
// blackout
|
2021-03-02 20:42:45 -07:00
|
|
|
const int blackout_x_r = valid_rect.right();
|
2021-05-06 06:39:05 -06:00
|
|
|
const Rect &blackout_rect = Hardware::TICI() ? s->viz_rect : rect;
|
|
|
|
const int blackout_w_r = blackout_rect.right() - valid_rect.right();
|
|
|
|
const int blackout_x_l = blackout_rect.x;
|
2021-03-02 20:42:45 -07:00
|
|
|
const int blackout_w_l = valid_rect.x - blackout_x_l;
|
|
|
|
ui_fill_rect(s->vg, {blackout_x_l, rect.y, blackout_w_l, rect.h}, COLOR_BLACK_ALPHA(144));
|
|
|
|
ui_fill_rect(s->vg, {blackout_x_r, rect.y, blackout_w_r, rect.h}, COLOR_BLACK_ALPHA(144));
|
2021-01-08 12:30:37 -07:00
|
|
|
|
2021-05-14 23:47:06 -06:00
|
|
|
auto driver_state = (*s->sm)["driverState"].getDriverState();
|
|
|
|
const bool face_detected = driver_state.getFaceProb() > 0.4;
|
2021-01-08 12:30:37 -07:00
|
|
|
if (face_detected) {
|
2021-05-14 23:47:06 -06:00
|
|
|
auto fxy_list = driver_state.getFacePosition();
|
2021-01-08 12:30:37 -07:00
|
|
|
float face_x = fxy_list[0];
|
|
|
|
float face_y = fxy_list[1];
|
2021-01-15 03:15:48 -07:00
|
|
|
int fbox_x = valid_rect.centerX() + (is_rhd ? face_x : -face_x) * valid_rect.w;
|
|
|
|
int fbox_y = valid_rect.centerY() + face_y * valid_rect.h;
|
2021-01-08 12:30:37 -07:00
|
|
|
|
|
|
|
float alpha = 0.2;
|
|
|
|
if (face_x = std::abs(face_x), face_y = std::abs(face_y); face_x <= 0.35 && face_y <= 0.4)
|
|
|
|
alpha = 0.8 - (face_x > face_y ? face_x : face_y) * 0.6 / 0.375;
|
|
|
|
|
2021-01-15 03:15:48 -07:00
|
|
|
const int box_size = 0.6 * rect.h / 2;
|
|
|
|
ui_draw_rect(s->vg, {fbox_x - box_size / 2, fbox_y - box_size / 2, box_size, box_size}, nvgRGBAf(1.0, 1.0, 1.0, alpha), 10, 35.);
|
2020-04-15 17:48:44 -06:00
|
|
|
}
|
|
|
|
|
|
|
|
// draw face icon
|
2021-03-30 05:32:07 -06:00
|
|
|
const int face_radius = 85;
|
|
|
|
const int center_x = is_rhd ? rect.right() - face_radius - bdr_s * 2 : rect.x + face_radius + bdr_s * 2;
|
|
|
|
const int center_y = rect.bottom() - face_radius - bdr_s * 2.5;
|
|
|
|
ui_draw_circle_image(s, center_x, center_y, face_radius, "driver_face", face_detected);
|
2020-04-15 17:48:44 -06:00
|
|
|
}
|
|
|
|
|
2020-01-17 12:05:23 -07:00
|
|
|
static void ui_draw_vision_header(UIState *s) {
|
2021-02-01 17:10:15 -07:00
|
|
|
NVGpaint gradient = nvgLinearGradient(s->vg, s->viz_rect.x,
|
|
|
|
s->viz_rect.y+(header_h-(header_h/2.5)),
|
|
|
|
s->viz_rect.x, s->viz_rect.y+header_h,
|
2020-01-17 12:05:23 -07:00
|
|
|
nvgRGBAf(0,0,0,0.45), nvgRGBAf(0,0,0,0));
|
2020-09-03 17:32:55 -06:00
|
|
|
|
2021-02-01 17:10:15 -07:00
|
|
|
ui_fill_rect(s->vg, {s->viz_rect.x, s->viz_rect.y, s->viz_rect.w, header_h}, gradient);
|
2020-01-17 12:05:23 -07:00
|
|
|
|
2021-04-12 14:34:44 -06:00
|
|
|
ui_draw_vision_maxspeed(s);
|
2020-01-17 12:05:23 -07:00
|
|
|
ui_draw_vision_speed(s);
|
|
|
|
ui_draw_vision_event(s);
|
|
|
|
}
|
|
|
|
|
2020-11-27 15:25:52 -07:00
|
|
|
static void ui_draw_vision_frame(UIState *s) {
|
2020-01-17 12:05:23 -07:00
|
|
|
// Draw video frames
|
|
|
|
glEnable(GL_SCISSOR_TEST);
|
2020-10-20 04:14:19 -06:00
|
|
|
glViewport(s->video_rect.x, s->video_rect.y, s->video_rect.w, s->video_rect.h);
|
2021-02-01 17:10:15 -07:00
|
|
|
glScissor(s->viz_rect.x, s->viz_rect.y, s->viz_rect.w, s->viz_rect.h);
|
2020-01-17 12:05:23 -07:00
|
|
|
draw_frame(s);
|
|
|
|
glDisable(GL_SCISSOR_TEST);
|
|
|
|
|
2020-04-30 13:11:35 -06:00
|
|
|
glViewport(0, 0, s->fb_w, s->fb_h);
|
2020-11-27 15:25:52 -07:00
|
|
|
}
|
|
|
|
|
|
|
|
static void ui_draw_vision(UIState *s) {
|
|
|
|
const UIScene *scene = &s->scene;
|
2021-03-03 03:38:41 -07:00
|
|
|
if (!scene->driver_view) {
|
2020-12-15 14:06:22 -07:00
|
|
|
// Draw augmented elements
|
|
|
|
if (scene->world_objects_visible) {
|
|
|
|
ui_draw_world(s);
|
|
|
|
}
|
|
|
|
// Set Speed, Current Speed, Status/Events
|
2020-04-15 17:48:44 -06:00
|
|
|
ui_draw_vision_header(s);
|
2021-05-14 23:47:06 -06:00
|
|
|
if ((*s->sm)["controlsState"].getControlsState().getAlertSize() == cereal::ControlsState::AlertSize::NONE) {
|
2021-05-05 00:23:00 -06:00
|
|
|
ui_draw_vision_face(s);
|
2020-12-15 14:06:22 -07:00
|
|
|
}
|
2020-04-15 17:48:44 -06:00
|
|
|
} else {
|
|
|
|
ui_draw_driver_view(s);
|
|
|
|
}
|
2020-01-17 12:05:23 -07:00
|
|
|
}
|
|
|
|
|
2020-04-01 17:01:05 -06:00
|
|
|
static void ui_draw_background(UIState *s) {
|
2020-10-08 20:29:56 -06:00
|
|
|
const NVGcolor color = bg_colors[s->status];
|
|
|
|
glClearColor(color.r, color.g, color.b, 1.0);
|
2020-04-01 17:01:05 -06:00
|
|
|
glClear(GL_STENCIL_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
|
|
|
|
}
|
|
|
|
|
2021-04-29 16:05:04 -06:00
|
|
|
void ui_draw(UIState *s, int w, int h) {
|
|
|
|
s->viz_rect = Rect{bdr_s, bdr_s, w - 2 * bdr_s, h - 2 * bdr_s};
|
2020-09-03 17:32:55 -06:00
|
|
|
|
2021-05-05 00:23:00 -06:00
|
|
|
const bool draw_vision = s->scene.started && s->vipc_client->connected;
|
2020-11-27 15:25:52 -07:00
|
|
|
|
|
|
|
// GL drawing functions
|
2020-04-01 17:01:05 -06:00
|
|
|
ui_draw_background(s);
|
2021-01-08 06:54:25 -07:00
|
|
|
if (draw_vision) {
|
2020-11-27 15:25:52 -07:00
|
|
|
ui_draw_vision_frame(s);
|
|
|
|
}
|
2020-04-30 13:11:35 -06:00
|
|
|
glEnable(GL_BLEND);
|
|
|
|
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
|
|
|
|
glViewport(0, 0, s->fb_w, s->fb_h);
|
2020-11-27 15:25:52 -07:00
|
|
|
|
|
|
|
// NVG drawing functions - should be no GL inside NVG frame
|
2020-04-30 13:11:35 -06:00
|
|
|
nvgBeginFrame(s->vg, s->fb_w, s->fb_h, 1.0f);
|
2021-04-29 16:05:04 -06:00
|
|
|
|
2021-01-08 06:54:25 -07:00
|
|
|
if (draw_vision) {
|
2020-06-29 14:54:23 -06:00
|
|
|
ui_draw_vision(s);
|
2020-05-12 19:17:04 -06:00
|
|
|
}
|
2020-12-15 14:06:22 -07:00
|
|
|
|
2021-03-30 03:51:18 -06:00
|
|
|
if (s->scene.driver_view && !s->vipc_client->connected) {
|
|
|
|
nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_MIDDLE);
|
|
|
|
ui_draw_text(s, s->viz_rect.centerX(), s->viz_rect.centerY(), "Please wait for camera to start", 40 * 2.5, COLOR_WHITE, "sans-bold");
|
|
|
|
}
|
2020-04-30 13:11:35 -06:00
|
|
|
nvgEndFrame(s->vg);
|
|
|
|
glDisable(GL_BLEND);
|
2020-01-17 12:05:23 -07:00
|
|
|
}
|
|
|
|
|
2021-03-10 21:52:01 -07:00
|
|
|
void ui_draw_image(const UIState *s, const Rect &r, const char *name, float alpha) {
|
2021-01-16 14:50:06 -07:00
|
|
|
nvgBeginPath(s->vg);
|
|
|
|
NVGpaint imgPaint = nvgImagePattern(s->vg, r.x, r.y, r.w, r.h, 0, s->images.at(name), alpha);
|
|
|
|
nvgRect(s->vg, r.x, r.y, r.w, r.h);
|
|
|
|
nvgFillPaint(s->vg, imgPaint);
|
|
|
|
nvgFill(s->vg);
|
2020-04-13 16:38:45 -06:00
|
|
|
}
|
|
|
|
|
2021-01-15 03:15:48 -07:00
|
|
|
void ui_draw_rect(NVGcontext *vg, const Rect &r, NVGcolor color, int width, float radius) {
|
2020-05-02 12:27:03 -06:00
|
|
|
nvgBeginPath(vg);
|
2021-01-15 03:15:48 -07:00
|
|
|
radius > 0 ? nvgRoundedRect(vg, r.x, r.y, r.w, r.h, radius) : nvgRect(vg, r.x, r.y, r.w, r.h);
|
|
|
|
nvgStrokeColor(vg, color);
|
|
|
|
nvgStrokeWidth(vg, width);
|
|
|
|
nvgStroke(vg);
|
2020-05-02 12:27:03 -06:00
|
|
|
}
|
|
|
|
|
2021-01-15 03:15:48 -07:00
|
|
|
static inline void fill_rect(NVGcontext *vg, const Rect &r, const NVGcolor *color, const NVGpaint *paint, float radius) {
|
2020-05-02 12:27:03 -06:00
|
|
|
nvgBeginPath(vg);
|
2021-03-10 21:52:01 -07:00
|
|
|
radius > 0 ? nvgRoundedRect(vg, r.x, r.y, r.w, r.h, radius) : nvgRect(vg, r.x, r.y, r.w, r.h);
|
2021-01-15 03:15:48 -07:00
|
|
|
if (color) nvgFillColor(vg, *color);
|
|
|
|
if (paint) nvgFillPaint(vg, *paint);
|
2020-05-02 12:27:03 -06:00
|
|
|
nvgFill(vg);
|
|
|
|
}
|
2021-01-15 03:15:48 -07:00
|
|
|
void ui_fill_rect(NVGcontext *vg, const Rect &r, const NVGcolor &color, float radius) {
|
|
|
|
fill_rect(vg, r, &color, nullptr, radius);
|
|
|
|
}
|
2021-03-10 21:52:01 -07:00
|
|
|
void ui_fill_rect(NVGcontext *vg, const Rect &r, const NVGpaint &paint, float radius) {
|
2021-01-15 03:15:48 -07:00
|
|
|
fill_rect(vg, r, nullptr, &paint, radius);
|
|
|
|
}
|
2020-05-02 12:27:03 -06:00
|
|
|
|
2020-01-17 12:05:23 -07:00
|
|
|
static const char frame_vertex_shader[] =
|
2020-08-31 07:02:01 -06:00
|
|
|
#ifdef NANOVG_GL3_IMPLEMENTATION
|
2020-02-02 23:19:26 -07:00
|
|
|
"#version 150 core\n"
|
2020-08-31 07:02:01 -06:00
|
|
|
#else
|
|
|
|
"#version 300 es\n"
|
|
|
|
#endif
|
2020-02-02 23:19:26 -07:00
|
|
|
"in vec4 aPosition;\n"
|
|
|
|
"in vec4 aTexCoord;\n"
|
2020-01-17 12:05:23 -07:00
|
|
|
"uniform mat4 uTransform;\n"
|
2020-02-02 23:19:26 -07:00
|
|
|
"out vec4 vTexCoord;\n"
|
2020-01-17 12:05:23 -07:00
|
|
|
"void main() {\n"
|
|
|
|
" gl_Position = uTransform * aPosition;\n"
|
|
|
|
" vTexCoord = aTexCoord;\n"
|
|
|
|
"}\n";
|
|
|
|
|
|
|
|
static const char frame_fragment_shader[] =
|
2020-08-31 07:02:01 -06:00
|
|
|
#ifdef NANOVG_GL3_IMPLEMENTATION
|
2020-02-02 23:19:26 -07:00
|
|
|
"#version 150 core\n"
|
2020-08-31 07:02:01 -06:00
|
|
|
#else
|
|
|
|
"#version 300 es\n"
|
|
|
|
#endif
|
2020-01-17 12:05:23 -07:00
|
|
|
"precision mediump float;\n"
|
|
|
|
"uniform sampler2D uTexture;\n"
|
2020-06-11 19:21:59 -06:00
|
|
|
"in vec4 vTexCoord;\n"
|
|
|
|
"out vec4 colorOut;\n"
|
2020-01-17 12:05:23 -07:00
|
|
|
"void main() {\n"
|
2020-06-11 19:21:59 -06:00
|
|
|
" colorOut = texture(uTexture, vTexCoord.xy);\n"
|
2021-04-27 18:41:59 -06:00
|
|
|
#ifdef QCOM
|
|
|
|
" vec3 dz = vec3(0.0627f, 0.0627f, 0.0627f);\n"
|
|
|
|
" colorOut.rgb = ((vec3(1.0f, 1.0f, 1.0f) - dz) * colorOut.rgb / vec3(1.0f, 1.0f, 1.0f)) + dz;\n"
|
|
|
|
#endif
|
2020-01-17 12:05:23 -07:00
|
|
|
"}\n";
|
|
|
|
|
|
|
|
static const mat4 device_transform = {{
|
|
|
|
1.0, 0.0, 0.0, 0.0,
|
|
|
|
0.0, 1.0, 0.0, 0.0,
|
|
|
|
0.0, 0.0, 1.0, 0.0,
|
|
|
|
0.0, 0.0, 0.0, 1.0,
|
|
|
|
}};
|
|
|
|
|
2021-05-06 06:39:05 -06:00
|
|
|
static mat4 get_driver_view_transform() {
|
|
|
|
const float driver_view_ratio = 1.333;
|
|
|
|
mat4 transform;
|
|
|
|
if (Hardware::TICI()) {
|
|
|
|
// from dmonitoring.cc
|
|
|
|
const int full_width_tici = 1928;
|
|
|
|
const int full_height_tici = 1208;
|
|
|
|
const int adapt_width_tici = 668;
|
|
|
|
const int crop_x_offset = 32;
|
|
|
|
const int crop_y_offset = -196;
|
|
|
|
const float yscale = full_height_tici * driver_view_ratio / adapt_width_tici;
|
|
|
|
const float xscale = yscale*(1080-2*bdr_s)/(2160-2*bdr_s)*full_width_tici/full_height_tici;
|
|
|
|
transform = (mat4){{
|
|
|
|
xscale, 0.0, 0.0, xscale*crop_x_offset/full_width_tici*2,
|
|
|
|
0.0, yscale, 0.0, yscale*crop_y_offset/full_height_tici*2,
|
|
|
|
0.0, 0.0, 1.0, 0.0,
|
|
|
|
0.0, 0.0, 0.0, 1.0,
|
|
|
|
}};
|
|
|
|
|
|
|
|
} else {
|
|
|
|
// frame from 4/3 to 16/9 display
|
|
|
|
transform = (mat4){{
|
|
|
|
driver_view_ratio*(1080-2*bdr_s)/(1920-2*bdr_s), 0.0, 0.0, 0.0,
|
|
|
|
0.0, 1.0, 0.0, 0.0,
|
|
|
|
0.0, 0.0, 1.0, 0.0,
|
|
|
|
0.0, 0.0, 0.0, 1.0,
|
|
|
|
}};
|
|
|
|
}
|
|
|
|
return transform;
|
|
|
|
}
|
2020-01-17 12:05:23 -07:00
|
|
|
|
|
|
|
void ui_nvg_init(UIState *s) {
|
|
|
|
// init drawing
|
2021-05-06 06:39:05 -06:00
|
|
|
|
|
|
|
// on EON, we enable MSAA
|
|
|
|
s->vg = Hardware::EON() ? nvgCreate(0) : nvgCreate(NVG_ANTIALIAS | NVG_STENCIL_STROKES | NVG_DEBUG);
|
2020-01-17 12:05:23 -07:00
|
|
|
assert(s->vg);
|
|
|
|
|
2021-01-16 14:50:06 -07:00
|
|
|
// init fonts
|
|
|
|
std::pair<const char *, const char *> fonts[] = {
|
|
|
|
{"sans-regular", "../assets/fonts/opensans_regular.ttf"},
|
|
|
|
{"sans-semibold", "../assets/fonts/opensans_semibold.ttf"},
|
|
|
|
{"sans-bold", "../assets/fonts/opensans_bold.ttf"},
|
|
|
|
};
|
|
|
|
for (auto [name, file] : fonts) {
|
|
|
|
int font_id = nvgCreateFont(s->vg, name, file);
|
|
|
|
assert(font_id >= 0);
|
|
|
|
}
|
|
|
|
|
|
|
|
// init images
|
|
|
|
std::vector<std::pair<const char *, const char *>> images = {
|
2021-04-29 16:21:40 -06:00
|
|
|
{"wheel", "../assets/img_chffr_wheel.png"},
|
|
|
|
{"driver_face", "../assets/img_driver_face.png"},
|
2021-01-16 14:50:06 -07:00
|
|
|
};
|
|
|
|
for (auto [name, file] : images) {
|
|
|
|
s->images[name] = nvgCreateImage(s->vg, file, 1);
|
|
|
|
assert(s->images[name] != 0);
|
2020-03-09 20:13:32 -06:00
|
|
|
}
|
|
|
|
|
2020-01-17 12:05:23 -07:00
|
|
|
// init gl
|
2021-01-12 07:43:34 -07:00
|
|
|
s->gl_shader = std::make_unique<GLShader>(frame_vertex_shader, frame_fragment_shader);
|
|
|
|
GLint frame_pos_loc = glGetAttribLocation(s->gl_shader->prog, "aPosition");
|
|
|
|
GLint frame_texcoord_loc = glGetAttribLocation(s->gl_shader->prog, "aTexCoord");
|
2020-01-17 12:05:23 -07:00
|
|
|
|
|
|
|
glViewport(0, 0, s->fb_w, s->fb_h);
|
|
|
|
|
|
|
|
glDisable(GL_DEPTH_TEST);
|
|
|
|
|
|
|
|
assert(glGetError() == GL_NO_ERROR);
|
|
|
|
|
2021-03-10 21:52:01 -07:00
|
|
|
for (int i = 0; i < 2; i++) {
|
2020-01-17 12:05:23 -07:00
|
|
|
float x1, x2, y1, y2;
|
|
|
|
if (i == 1) {
|
|
|
|
// flip horizontally so it looks like a mirror
|
|
|
|
x1 = 0.0;
|
|
|
|
x2 = 1.0;
|
|
|
|
y1 = 1.0;
|
|
|
|
y2 = 0.0;
|
|
|
|
} else {
|
|
|
|
x1 = 1.0;
|
|
|
|
x2 = 0.0;
|
|
|
|
y1 = 1.0;
|
|
|
|
y2 = 0.0;
|
|
|
|
}
|
|
|
|
const uint8_t frame_indicies[] = {0, 1, 2, 0, 2, 3};
|
|
|
|
const float frame_coords[4][4] = {
|
|
|
|
{-1.0, -1.0, x2, y1}, //bl
|
|
|
|
{-1.0, 1.0, x2, y2}, //tl
|
|
|
|
{ 1.0, 1.0, x1, y2}, //tr
|
|
|
|
{ 1.0, -1.0, x1, y1}, //br
|
|
|
|
};
|
|
|
|
|
2020-09-03 17:32:55 -06:00
|
|
|
glGenVertexArrays(1, &s->frame_vao[i]);
|
2020-01-17 12:05:23 -07:00
|
|
|
glBindVertexArray(s->frame_vao[i]);
|
|
|
|
glGenBuffers(1, &s->frame_vbo[i]);
|
|
|
|
glBindBuffer(GL_ARRAY_BUFFER, s->frame_vbo[i]);
|
|
|
|
glBufferData(GL_ARRAY_BUFFER, sizeof(frame_coords), frame_coords, GL_STATIC_DRAW);
|
2021-01-12 07:43:34 -07:00
|
|
|
glEnableVertexAttribArray(frame_pos_loc);
|
|
|
|
glVertexAttribPointer(frame_pos_loc, 2, GL_FLOAT, GL_FALSE,
|
2020-01-17 12:05:23 -07:00
|
|
|
sizeof(frame_coords[0]), (const void *)0);
|
2021-01-12 07:43:34 -07:00
|
|
|
glEnableVertexAttribArray(frame_texcoord_loc);
|
|
|
|
glVertexAttribPointer(frame_texcoord_loc, 2, GL_FLOAT, GL_FALSE,
|
2020-01-17 12:05:23 -07:00
|
|
|
sizeof(frame_coords[0]), (const void *)(sizeof(float) * 2));
|
|
|
|
glGenBuffers(1, &s->frame_ibo[i]);
|
|
|
|
glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, s->frame_ibo[i]);
|
|
|
|
glBufferData(GL_ELEMENT_ARRAY_BUFFER, sizeof(frame_indicies), frame_indicies, GL_STATIC_DRAW);
|
2021-03-10 21:52:01 -07:00
|
|
|
glBindBuffer(GL_ARRAY_BUFFER, 0);
|
2020-01-17 12:05:23 -07:00
|
|
|
glBindVertexArray(0);
|
|
|
|
}
|
|
|
|
|
2021-04-07 11:12:35 -06:00
|
|
|
auto intrinsic_matrix = s->wide_camera ? ecam_intrinsic_matrix : fcam_intrinsic_matrix;
|
|
|
|
|
|
|
|
s->zoom = zoom / intrinsic_matrix.v[0];
|
|
|
|
|
|
|
|
if (s->wide_camera) {
|
|
|
|
s->zoom *= 0.5;
|
|
|
|
}
|
|
|
|
|
2020-11-30 17:22:42 -07:00
|
|
|
s->video_rect = Rect{bdr_s, bdr_s, s->fb_w - 2 * bdr_s, s->fb_h - 2 * bdr_s};
|
2021-04-07 11:12:35 -06:00
|
|
|
float zx = s->zoom * 2 * intrinsic_matrix.v[2] / s->video_rect.w;
|
|
|
|
float zy = s->zoom * 2 * intrinsic_matrix.v[5] / s->video_rect.h;
|
2020-10-20 04:14:19 -06:00
|
|
|
|
2020-09-12 19:35:36 -06:00
|
|
|
const mat4 frame_transform = {{
|
2020-10-20 04:14:19 -06:00
|
|
|
zx, 0.0, 0.0, 0.0,
|
2020-12-14 05:37:55 -07:00
|
|
|
0.0, zy, 0.0, -y_offset / s->video_rect.h * 2,
|
2020-09-12 19:35:36 -06:00
|
|
|
0.0, 0.0, 1.0, 0.0,
|
|
|
|
0.0, 0.0, 0.0, 1.0,
|
|
|
|
}};
|
|
|
|
|
2021-05-06 06:39:05 -06:00
|
|
|
s->front_frame_mat = matmul(device_transform, get_driver_view_transform());
|
2020-01-17 12:05:23 -07:00
|
|
|
s->rear_frame_mat = matmul(device_transform, frame_transform);
|
2021-01-15 03:41:20 -07:00
|
|
|
|
|
|
|
// Apply transformation such that video pixel coordinates match video
|
|
|
|
// 1) Put (0, 0) in the middle of the video
|
|
|
|
nvgTranslate(s->vg, s->video_rect.x + s->video_rect.w / 2, s->video_rect.y + s->video_rect.h / 2 + y_offset);
|
|
|
|
|
|
|
|
// 2) Apply same scaling as video
|
2021-04-07 11:12:35 -06:00
|
|
|
nvgScale(s->vg, s->zoom, s->zoom);
|
2021-01-15 03:41:20 -07:00
|
|
|
|
|
|
|
// 3) Put (0, 0) in top left corner of video
|
2021-04-07 11:12:35 -06:00
|
|
|
nvgTranslate(s->vg, -intrinsic_matrix.v[2], -intrinsic_matrix.v[5]);
|
2021-01-15 03:41:20 -07:00
|
|
|
|
|
|
|
nvgCurrentTransform(s->vg, s->car_space_transform);
|
|
|
|
nvgResetTransform(s->vg);
|
2020-01-17 12:05:23 -07:00
|
|
|
}
|