1
2 #include <cmath>
3
4 #include <kms++/kms++.h>
5 #include <kms++util/kms++util.h>
6
7 using namespace std;
8
9 namespace kms
10 {
draw_rgb_pixel(IFramebuffer & buf,unsigned x,unsigned y,RGB color)11 void draw_rgb_pixel(IFramebuffer& buf, unsigned x, unsigned y, RGB color)
12 {
13 if (x >= buf.width() || y >= buf.height())
14 throw runtime_error("attempt to draw outside the buffer");
15
16 switch (buf.format()) {
17 case PixelFormat::XRGB8888:
18 case PixelFormat::ARGB8888: {
19 uint32_t* p = (uint32_t*)(buf.map(0) + buf.stride(0) * y + x * 4);
20 *p = color.argb8888();
21 break;
22 }
23 case PixelFormat::XBGR8888:
24 case PixelFormat::ABGR8888: {
25 uint32_t* p = (uint32_t*)(buf.map(0) + buf.stride(0) * y + x * 4);
26 *p = color.abgr8888();
27 break;
28 }
29 case PixelFormat::RGBX8888:
30 case PixelFormat::RGBA8888: {
31 uint32_t* p = (uint32_t*)(buf.map(0) + buf.stride(0) * y + x * 4);
32 *p = color.rgba8888();
33 break;
34 }
35 case PixelFormat::BGRX8888:
36 case PixelFormat::BGRA8888: {
37 uint32_t* p = (uint32_t*)(buf.map(0) + buf.stride(0) * y + x * 4);
38 *p = color.bgra8888();
39 break;
40 }
41 case PixelFormat::XRGB2101010:
42 case PixelFormat::ARGB2101010: {
43 uint32_t* p = (uint32_t*)(buf.map(0) + buf.stride(0) * y + x * 4);
44 *p = color.argb2101010();
45 break;
46 }
47 case PixelFormat::XBGR2101010:
48 case PixelFormat::ABGR2101010: {
49 uint32_t* p = (uint32_t*)(buf.map(0) + buf.stride(0) * y + x * 4);
50 *p = color.abgr2101010();
51 break;
52 }
53 case PixelFormat::RGBX1010102:
54 case PixelFormat::RGBA1010102: {
55 uint32_t* p = (uint32_t*)(buf.map(0) + buf.stride(0) * y + x * 4);
56 *p = color.rgba1010102();
57 break;
58 }
59 case PixelFormat::BGRX1010102:
60 case PixelFormat::BGRA1010102: {
61 uint32_t* p = (uint32_t*)(buf.map(0) + buf.stride(0) * y + x * 4);
62 *p = color.bgra1010102();
63 break;
64 }
65 case PixelFormat::RGB888: {
66 uint8_t* p = buf.map(0) + buf.stride(0) * y + x * 3;
67 p[0] = color.b;
68 p[1] = color.g;
69 p[2] = color.r;
70 break;
71 }
72 case PixelFormat::BGR888: {
73 uint8_t* p = buf.map(0) + buf.stride(0) * y + x * 3;
74 p[0] = color.r;
75 p[1] = color.g;
76 p[2] = color.b;
77 break;
78 }
79 case PixelFormat::RGB332: {
80 uint8_t* p = (uint8_t*)(buf.map(0) + buf.stride(0) * y + x);
81 *p = color.rgb332();
82 break;
83 }
84 case PixelFormat::RGB565: {
85 uint16_t* p = (uint16_t*)(buf.map(0) + buf.stride(0) * y + x * 2);
86 *p = color.rgb565();
87 break;
88 }
89 case PixelFormat::BGR565: {
90 uint16_t* p = (uint16_t*)(buf.map(0) + buf.stride(0) * y + x * 2);
91 *p = color.bgr565();
92 break;
93 }
94 case PixelFormat::XRGB4444:
95 case PixelFormat::ARGB4444: {
96 uint16_t* p = (uint16_t*)(buf.map(0) + buf.stride(0) * y + x * 2);
97 *p = color.argb4444();
98 break;
99 }
100 case PixelFormat::XRGB1555:
101 case PixelFormat::ARGB1555: {
102 uint16_t* p = (uint16_t*)(buf.map(0) + buf.stride(0) * y + x * 2);
103 *p = color.argb1555();
104 break;
105 }
106 default:
107 throw std::invalid_argument("invalid pixelformat");
108 }
109 }
110
draw_yuv444_pixel(IFramebuffer & buf,unsigned x,unsigned y,YUV yuv)111 void draw_yuv444_pixel(IFramebuffer& buf, unsigned x, unsigned y, YUV yuv)
112 {
113 if (x >= buf.width() || y >= buf.height())
114 throw runtime_error("attempt to draw outside the buffer");
115
116 uint8_t* py = (uint8_t*)(buf.map(0) + buf.stride(0) * y + x);
117 uint8_t* pu = (uint8_t*)(buf.map(1) + buf.stride(1) * y + x);
118 uint8_t* pv = (uint8_t*)(buf.map(2) + buf.stride(2) * y + x);
119
120 switch (buf.format()) {
121 case PixelFormat::YUV444:
122 py[0] = yuv.y;
123 pu[0] = yuv.u;
124 pv[0] = yuv.v;
125 break;
126
127 case PixelFormat::YVU444:
128 py[0] = yuv.y;
129 pu[0] = yuv.v;
130 pv[0] = yuv.u;
131 break;
132
133 default:
134 throw std::invalid_argument("invalid pixelformat");
135 }
136 }
137
draw_yuv422_packed_macropixel(IFramebuffer & buf,unsigned x,unsigned y,YUV yuv1,YUV yuv2)138 static void draw_yuv422_packed_macropixel(IFramebuffer& buf, unsigned x, unsigned y,
139 YUV yuv1, YUV yuv2)
140 {
141 uint8_t* p = (uint8_t*)(buf.map(0) + buf.stride(0) * y + x * 2);
142
143 uint8_t y0 = yuv1.y;
144 uint8_t y1 = yuv2.y;
145 uint8_t u = (yuv1.u + yuv2.u) / 2;
146 uint8_t v = (yuv1.v + yuv2.v) / 2;
147
148 switch (buf.format()) {
149 case PixelFormat::UYVY:
150 p[0] = u;
151 p[1] = y0;
152 p[2] = v;
153 p[3] = y1;
154 break;
155
156 case PixelFormat::YUYV:
157 p[0] = y0;
158 p[1] = u;
159 p[2] = y1;
160 p[3] = v;
161 break;
162
163 case PixelFormat::YVYU:
164 p[0] = y0;
165 p[1] = v;
166 p[2] = y1;
167 p[3] = u;
168 break;
169
170 case PixelFormat::VYUY:
171 p[0] = v;
172 p[1] = y0;
173 p[2] = u;
174 p[3] = y1;
175 break;
176
177 default:
178 throw std::invalid_argument("invalid pixelformat");
179 }
180 }
181
draw_yuv422_semiplanar_macropixel(IFramebuffer & buf,unsigned x,unsigned y,YUV yuv1,YUV yuv2)182 static void draw_yuv422_semiplanar_macropixel(IFramebuffer& buf, unsigned x, unsigned y,
183 YUV yuv1, YUV yuv2)
184 {
185 uint8_t* py = (uint8_t*)(buf.map(0) + buf.stride(0) * y + x);
186 uint8_t* puv = (uint8_t*)(buf.map(1) + buf.stride(1) * y + x);
187
188 uint8_t y0 = yuv1.y;
189 uint8_t y1 = yuv2.y;
190 uint8_t u = (yuv1.u + yuv2.u) / 2;
191 uint8_t v = (yuv1.v + yuv2.v) / 2;
192
193 switch (buf.format()) {
194 case PixelFormat::NV16:
195 py[0] = y0;
196 py[1] = y1;
197 puv[0] = u;
198 puv[1] = v;
199 break;
200
201 case PixelFormat::NV61:
202 py[0] = y0;
203 py[1] = y1;
204 puv[0] = v;
205 puv[1] = u;
206 break;
207
208 default:
209 throw std::invalid_argument("invalid pixelformat");
210 }
211 }
212
draw_yuv422_planar_macropixel(IFramebuffer & buf,unsigned x,unsigned y,YUV yuv1,YUV yuv2)213 static void draw_yuv422_planar_macropixel(IFramebuffer& buf, unsigned x, unsigned y,
214 YUV yuv1, YUV yuv2)
215 {
216 uint8_t* py = (uint8_t*)(buf.map(0) + buf.stride(0) * y + x);
217 uint8_t* pu = (uint8_t*)(buf.map(1) + buf.stride(1) * y + x / 2);
218 uint8_t* pv = (uint8_t*)(buf.map(2) + buf.stride(2) * y + x / 2);
219
220 uint8_t y0 = yuv1.y;
221 uint8_t y1 = yuv2.y;
222 uint8_t u = (yuv1.u + yuv2.u) / 2;
223 uint8_t v = (yuv1.v + yuv2.v) / 2;
224
225 switch (buf.format()) {
226 case PixelFormat::YUV422:
227 py[0] = y0;
228 py[1] = y1;
229 pu[0] = u;
230 pv[0] = v;
231 break;
232
233 case PixelFormat::YVU422:
234 py[0] = y0;
235 py[1] = y1;
236 pu[0] = v;
237 pv[0] = u;
238 break;
239
240 default:
241 throw std::invalid_argument("invalid pixelformat");
242 }
243 }
244
draw_yuv422_macropixel(IFramebuffer & buf,unsigned x,unsigned y,YUV yuv1,YUV yuv2)245 void draw_yuv422_macropixel(IFramebuffer& buf, unsigned x, unsigned y, YUV yuv1, YUV yuv2)
246 {
247 if ((x + 1) >= buf.width() || y >= buf.height())
248 throw runtime_error("attempt to draw outside the buffer");
249
250 ASSERT((x & 1) == 0);
251
252 switch (buf.format()) {
253 case PixelFormat::UYVY:
254 case PixelFormat::YUYV:
255 case PixelFormat::YVYU:
256 case PixelFormat::VYUY:
257 draw_yuv422_packed_macropixel(buf, x, y, yuv1, yuv2);
258 break;
259
260 case PixelFormat::NV16:
261 case PixelFormat::NV61:
262 draw_yuv422_semiplanar_macropixel(buf, x, y, yuv1, yuv2);
263 break;
264
265 case PixelFormat::YUV422:
266 case PixelFormat::YVU422:
267 draw_yuv422_planar_macropixel(buf, x, y, yuv1, yuv2);
268 break;
269
270 default:
271 throw std::invalid_argument("invalid pixelformat");
272 }
273 }
274
draw_yuv420_semiplanar_macropixel(IFramebuffer & buf,unsigned x,unsigned y,YUV yuv1,YUV yuv2,YUV yuv3,YUV yuv4)275 static void draw_yuv420_semiplanar_macropixel(IFramebuffer& buf, unsigned x, unsigned y,
276 YUV yuv1, YUV yuv2, YUV yuv3, YUV yuv4)
277 {
278 uint8_t* py1 = (uint8_t*)(buf.map(0) + buf.stride(0) * (y + 0) + x);
279 uint8_t* py2 = (uint8_t*)(buf.map(0) + buf.stride(0) * (y + 1) + x);
280
281 uint8_t* puv = (uint8_t*)(buf.map(1) + buf.stride(1) * (y / 2) + x);
282
283 uint8_t y0 = yuv1.y;
284 uint8_t y1 = yuv2.y;
285 uint8_t y2 = yuv3.y;
286 uint8_t y3 = yuv4.y;
287 uint8_t u = (yuv1.u + yuv2.u + yuv3.u + yuv4.u) / 4;
288 uint8_t v = (yuv1.v + yuv2.v + yuv3.v + yuv4.v) / 4;
289
290 switch (buf.format()) {
291 case PixelFormat::NV12:
292 py1[0] = y0;
293 py1[1] = y1;
294 py2[0] = y2;
295 py2[1] = y3;
296 puv[0] = u;
297 puv[1] = v;
298 break;
299
300 case PixelFormat::NV21:
301 py1[0] = y0;
302 py1[1] = y1;
303 py2[0] = y2;
304 py2[1] = y3;
305 puv[0] = v;
306 puv[1] = u;
307 break;
308
309 default:
310 throw std::invalid_argument("invalid pixelformat");
311 }
312 }
313
draw_yuv420_planar_macropixel(IFramebuffer & buf,unsigned x,unsigned y,YUV yuv1,YUV yuv2,YUV yuv3,YUV yuv4)314 static void draw_yuv420_planar_macropixel(IFramebuffer& buf, unsigned x, unsigned y,
315 YUV yuv1, YUV yuv2, YUV yuv3, YUV yuv4)
316 {
317 uint8_t* py1 = (uint8_t*)(buf.map(0) + buf.stride(0) * (y + 0) + x);
318 uint8_t* py2 = (uint8_t*)(buf.map(0) + buf.stride(0) * (y + 1) + x);
319
320 uint8_t* pu = (uint8_t*)(buf.map(1) + buf.stride(1) * (y / 2) + x / 2);
321 uint8_t* pv = (uint8_t*)(buf.map(2) + buf.stride(2) * (y / 2) + x / 2);
322
323 uint8_t y0 = yuv1.y;
324 uint8_t y1 = yuv2.y;
325 uint8_t y2 = yuv3.y;
326 uint8_t y3 = yuv4.y;
327 uint8_t u = (yuv1.u + yuv2.u + yuv3.u + yuv4.u) / 4;
328 uint8_t v = (yuv1.v + yuv2.v + yuv3.v + yuv4.v) / 4;
329
330 switch (buf.format()) {
331 case PixelFormat::YUV420:
332 py1[0] = y0;
333 py1[1] = y1;
334 py2[0] = y2;
335 py2[1] = y3;
336 pu[0] = u;
337 pv[0] = v;
338 break;
339
340 case PixelFormat::YVU420:
341 py1[0] = y0;
342 py1[1] = y1;
343 py2[0] = y2;
344 py2[1] = y3;
345 pu[0] = v;
346 pv[0] = u;
347 break;
348
349 default:
350 throw std::invalid_argument("invalid pixelformat");
351 }
352 }
353
draw_yuv420_macropixel(IFramebuffer & buf,unsigned x,unsigned y,YUV yuv1,YUV yuv2,YUV yuv3,YUV yuv4)354 void draw_yuv420_macropixel(IFramebuffer& buf, unsigned x, unsigned y,
355 YUV yuv1, YUV yuv2, YUV yuv3, YUV yuv4)
356 {
357 if ((x + 1) >= buf.width() || (y + 1) >= buf.height())
358 throw runtime_error("attempt to draw outside the buffer");
359
360 ASSERT((x & 1) == 0);
361 ASSERT((y & 1) == 0);
362
363 switch (buf.format()) {
364 case PixelFormat::NV12:
365 case PixelFormat::NV21:
366 draw_yuv420_semiplanar_macropixel(buf, x, y, yuv1, yuv2, yuv3, yuv4);
367 break;
368
369 case PixelFormat::YUV420:
370 case PixelFormat::YVU420:
371 draw_yuv420_planar_macropixel(buf, x, y, yuv1, yuv2, yuv3, yuv4);
372 break;
373
374 default:
375 throw std::invalid_argument("invalid pixelformat");
376 }
377 }
378
draw_rect(IFramebuffer & fb,uint32_t x,uint32_t y,uint32_t w,uint32_t h,RGB color)379 void draw_rect(IFramebuffer& fb, uint32_t x, uint32_t y, uint32_t w, uint32_t h, RGB color)
380 {
381 unsigned i, j;
382 YUV yuvcolor = color.yuv();
383
384 switch (fb.format()) {
385 case PixelFormat::XRGB8888:
386 case PixelFormat::XBGR8888:
387 case PixelFormat::ARGB8888:
388 case PixelFormat::ABGR8888:
389 case PixelFormat::RGB888:
390 case PixelFormat::BGR888:
391 case PixelFormat::RGB565:
392 case PixelFormat::BGR565:
393 case PixelFormat::XRGB4444:
394 case PixelFormat::XRGB1555:
395 case PixelFormat::ARGB4444:
396 case PixelFormat::ARGB1555:
397 case PixelFormat::RGB332:
398 for (j = 0; j < h; j++) {
399 for (i = 0; i < w; i++) {
400 draw_rgb_pixel(fb, x + i, y + j, color);
401 }
402 }
403 break;
404
405 case PixelFormat::YUV444:
406 case PixelFormat::YVU444:
407 for (j = 0; j < h; j++) {
408 for (i = 0; i < w; i++) {
409 draw_yuv444_pixel(fb, x + i, y + j, yuvcolor);
410 }
411 }
412 break;
413
414 case PixelFormat::UYVY:
415 case PixelFormat::YUYV:
416 case PixelFormat::YVYU:
417 case PixelFormat::VYUY:
418 case PixelFormat::NV16:
419 case PixelFormat::NV61:
420 case PixelFormat::YUV422:
421 case PixelFormat::YVU422:
422 for (j = 0; j < h; j++) {
423 for (i = 0; i < w; i += 2) {
424 draw_yuv422_macropixel(fb, x + i, y + j, yuvcolor, yuvcolor);
425 }
426 }
427 break;
428
429 case PixelFormat::NV12:
430 case PixelFormat::NV21:
431 case PixelFormat::YUV420:
432 case PixelFormat::YVU420:
433 for (j = 0; j < h; j += 2) {
434 for (i = 0; i < w; i += 2) {
435 draw_yuv420_macropixel(fb, x + i, y + j,
436 yuvcolor, yuvcolor, yuvcolor, yuvcolor);
437 }
438 }
439 break;
440 default:
441 throw std::invalid_argument("draw_rect: unknown pixelformat");
442 }
443 }
444
draw_horiz_line(IFramebuffer & fb,uint32_t x1,uint32_t x2,uint32_t y,RGB color)445 void draw_horiz_line(IFramebuffer& fb, uint32_t x1, uint32_t x2, uint32_t y, RGB color)
446 {
447 for (uint32_t x = x1; x <= x2; ++x)
448 draw_rgb_pixel(fb, x, y, color);
449 }
450
draw_circle(IFramebuffer & fb,int32_t xCenter,int32_t yCenter,int32_t radius,RGB color)451 void draw_circle(IFramebuffer& fb, int32_t xCenter, int32_t yCenter, int32_t radius, RGB color)
452 {
453 int32_t r2 = radius * radius;
454
455 for (int y = -radius; y <= radius; y++) {
456 int32_t x = (int)(sqrt(r2 - y * y) + 0.5);
457 draw_horiz_line(fb, xCenter - x, xCenter + x, yCenter - y, color);
458 }
459 }
460
get_char_pixel(char c,uint32_t x,uint32_t y)461 static bool get_char_pixel(char c, uint32_t x, uint32_t y)
462 {
463 #include "font_8x8.h"
464
465 uint8_t bits = fontdata_8x8[8 * c + y];
466 bool bit = (bits >> (7 - x)) & 1;
467
468 return bit;
469 }
470
draw_char(IFramebuffer & buf,uint32_t xpos,uint32_t ypos,char c,RGB color)471 static void draw_char(IFramebuffer& buf, uint32_t xpos, uint32_t ypos, char c, RGB color)
472 {
473 unsigned x, y;
474 YUV yuvcolor = color.yuv();
475
476 switch (buf.format()) {
477 case PixelFormat::XRGB8888:
478 case PixelFormat::XBGR8888:
479 case PixelFormat::ARGB8888:
480 case PixelFormat::ABGR8888:
481 case PixelFormat::RGB888:
482 case PixelFormat::BGR888:
483 case PixelFormat::RGB565:
484 case PixelFormat::BGR565:
485 case PixelFormat::XRGB4444:
486 case PixelFormat::XRGB1555:
487 case PixelFormat::ARGB4444:
488 case PixelFormat::ARGB1555:
489 case PixelFormat::RGB332:
490 for (y = 0; y < 8; y++) {
491 for (x = 0; x < 8; x++) {
492 bool b = get_char_pixel(c, x, y);
493
494 draw_rgb_pixel(buf, xpos + x, ypos + y, b ? color : RGB());
495 }
496 }
497 break;
498
499 case PixelFormat::YUV444:
500 case PixelFormat::YVU444:
501 for (y = 0; y < 8; y++) {
502 for (x = 0; x < 8; x++) {
503 bool b = get_char_pixel(c, x, y);
504
505 draw_yuv444_pixel(buf, xpos + x, ypos + y, b ? yuvcolor : YUV(RGB()));
506 }
507 }
508 break;
509
510 case PixelFormat::UYVY:
511 case PixelFormat::YUYV:
512 case PixelFormat::YVYU:
513 case PixelFormat::VYUY:
514 case PixelFormat::NV16:
515 case PixelFormat::NV61:
516 case PixelFormat::YUV422:
517 case PixelFormat::YVU422:
518 for (y = 0; y < 8; y++) {
519 for (x = 0; x < 8; x += 2) {
520 bool b0 = get_char_pixel(c, x, y);
521 bool b1 = get_char_pixel(c, x + 1, y);
522
523 draw_yuv422_macropixel(buf, xpos + x, ypos + y,
524 b0 ? yuvcolor : YUV(RGB()), b1 ? yuvcolor : YUV(RGB()));
525 }
526 }
527 break;
528
529 case PixelFormat::NV12:
530 case PixelFormat::NV21:
531 case PixelFormat::YUV420:
532 case PixelFormat::YVU420:
533 for (y = 0; y < 8; y += 2) {
534 for (x = 0; x < 8; x += 2) {
535 bool b00 = get_char_pixel(c, x, y);
536 bool b10 = get_char_pixel(c, x + 1, y);
537 bool b01 = get_char_pixel(c, x, y + 1);
538 bool b11 = get_char_pixel(c, x + 1, y + 1);
539
540 draw_yuv420_macropixel(buf, xpos + x, ypos + y,
541 b00 ? yuvcolor : YUV(RGB()), b10 ? yuvcolor : YUV(RGB()),
542 b01 ? yuvcolor : YUV(RGB()), b11 ? yuvcolor : YUV(RGB()));
543 }
544 }
545 break;
546 default:
547 throw std::invalid_argument("draw_char: unknown pixelformat");
548 }
549 }
550
draw_text(IFramebuffer & buf,uint32_t x,uint32_t y,const string & str,RGB color)551 void draw_text(IFramebuffer& buf, uint32_t x, uint32_t y, const string& str, RGB color)
552 {
553 for (unsigned i = 0; i < str.size(); i++)
554 draw_char(buf, (x + 8 * i), y, str[i], color);
555 }
556
557 } // namespace kms
558