89 lines
2.4 KiB
C++
89 lines
2.4 KiB
C++
#include "draw.hpp"
|
|
#include <algorithm>
|
|
#include <cmath>
|
|
|
|
DebugOverlay g_dbg;
|
|
|
|
static uint16* g_buf = nullptr;
|
|
static int g_w = 0;
|
|
static int g_h = 0;
|
|
|
|
static inline uint16 rgb565(uint8 r, uint8 g, uint8 b)
|
|
{
|
|
return ((r >> 3) << 11) | ((g >> 2) << 5) | (b >> 3);
|
|
}
|
|
|
|
static inline bool in_screen(int x, int y)
|
|
{
|
|
return x >= 0 && x < g_w && y >= 0 && y < g_h;
|
|
}
|
|
|
|
void dbg_draw_init(uint16* fb_buffer, int w, int h)
|
|
{
|
|
g_buf = fb_buffer;
|
|
g_w = w; g_h = h;
|
|
}
|
|
|
|
void dbg_draw_line(int x0, int y0, int x1, int y1, uint8 r, uint8 g, uint8 b)
|
|
{
|
|
if (!g_buf) return;
|
|
uint16 color = rgb565(r, g, b);
|
|
int dx = std::abs(x1 - x0), sx = x0 < x1 ? 1 : -1;
|
|
int dy = -std::abs(y1 - y0), sy = y0 < y1 ? 1 : -1;
|
|
int err = dx + dy;
|
|
|
|
while (true)
|
|
{
|
|
if (in_screen(x0, y0)) g_buf[y0 * g_w + x0] = color;
|
|
if (x0 == x1 && y0 == y1) break;
|
|
int e2 = 2 * err;
|
|
if (e2 >= dy) { err += dy; x0 += sx; }
|
|
if (e2 <= dx) { err += dx; y0 += sy; }
|
|
}
|
|
}
|
|
|
|
void dbg_draw_cross(int cx, int cy, int size, uint8 r, uint8 g, uint8 b)
|
|
{
|
|
dbg_draw_line(cx - size, cy, cx + size, cy, r, g, b);
|
|
dbg_draw_line(cx, cy - size, cx, cy + size, r, g, b);
|
|
}
|
|
|
|
void dbg_draw_rect(int x, int y, int w, int h, uint8 r, uint8 g, uint8 b)
|
|
{
|
|
dbg_draw_line(x, y, x + w, y, r, g, b);
|
|
dbg_draw_line(x + w, y, x + w, y + h, r, g, b);
|
|
dbg_draw_line(x + w, y + h, x, y + h, r, g, b);
|
|
dbg_draw_line(x, y + h, x, y, r, g, b);
|
|
}
|
|
|
|
void dbg_draw_points(const int points[][2], int count, uint8 r, uint8 g, uint8 b)
|
|
{
|
|
if (!g_buf) return;
|
|
uint16 color = rgb565(r, g, b);
|
|
for (int i = 0; i < count; ++i)
|
|
{
|
|
int x = points[i][0], y = points[i][1];
|
|
if (in_screen(x, y)) g_buf[y * g_w + x] = color;
|
|
}
|
|
}
|
|
|
|
void dbg_draw_edge_zoom(const EdgeLines& lines, uint8 r, uint8 g, uint8 b)
|
|
{
|
|
const int thick = 7; // 车道线宽度 (像素)
|
|
|
|
for (int y = 0; y < IPM_ROW_COUNT; ++y)
|
|
{
|
|
if (lines.left[y] > 0)
|
|
for (int dx = -thick; dx <= thick; ++dx)
|
|
dbg_draw_line(lines.left[y] + dx, y, lines.left[y] + dx, y, r, g, b);
|
|
|
|
if (lines.right[y] > 0)
|
|
for (int dx = -thick; dx <= thick; ++dx)
|
|
dbg_draw_line(lines.right[y] + dx, y, lines.right[y] + dx, y, 0, 255, 0);
|
|
|
|
if (lines.mid[y] > 0)
|
|
for (int dx = -thick; dx <= thick; ++dx)
|
|
dbg_draw_line(lines.mid[y] + dx, y, lines.mid[y] + dx, y, 255, 255, 0);
|
|
}
|
|
}
|