1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
|
/*
* Copyright (c) 2017-2018 Wei Shuai <cpuwolf@gmail.com>
*
* Permission to use, copy, modify, and/or distribute this software for any
* purpose with or without fee is hereby granted, provided that the above
* copyright notice and this permission notice appear in all copies.
*/
#include "wii_point_extractor.h"
#include "point_tracker.h"
#include "wii_frame.hpp"
#include "cv/numeric.hpp"
#include "compat/math.hpp"
#include <cmath>
#include <algorithm>
#include <cinttypes>
#include <memory>
#include <QDebug>
using namespace numeric_types;
using namespace pt_module;
WIIPointExtractor::WIIPointExtractor(const QString& module_name) : s(module_name)
{
}
//define a temp draw function
void WIIPointExtractor::draw_point(cv::Mat& preview_frame, const vec2& p, const cv::Scalar& color, int thickness)
{
static constexpr int len = 9;
cv::Point p2(iround(p[0] * preview_frame.cols + preview_frame.cols / 2),
iround(-p[1] * preview_frame.cols + preview_frame.rows / 2));
cv::line(preview_frame,
cv::Point(p2.x - len, p2.y),
cv::Point(p2.x + len, p2.y),
color,
thickness);
cv::line(preview_frame,
cv::Point(p2.x, p2.y - len),
cv::Point(p2.x, p2.y + len),
color,
thickness);
};
bool WIIPointExtractor::draw_points(cv::Mat& preview_frame, const struct wii_info& wii, std::vector<vec2>& points)
{
const float W = 1024.0f;
const float H = 768.0f;
points.reserve(4);
points.clear();
for (unsigned index = 0; index < 4; index++)
{
const struct wii_info_points &dot = wii.Points[index];
if (dot.bvis) {
//qDebug() << "wii:" << dot.RawX << "+" << dot.RawY;
//anti-clockwise rotate the 2D point
const float RX = W - dot.ux;
const float RY = H - dot.uy;
//vec2 dt((dot.RawX - W / 2.0f) / W, -(dot.RawY - H / 2.0f) / W);
//vec2 dt((RX - W / 2.0f) / W, -(RY - H / 2.0f) / W);
//vec2 dt((2.0f*RX - W) / W, -(2.0f*RY - H ) / W);
vec2 dt;
std::tie(dt[0], dt[1]) = to_screen_pos(RX, RY, W, H);
points.push_back(dt);
draw_point(preview_frame, dt, cv::Scalar(0, 255, 0), clamp(dot.isize, 1, 32));
}
}
const bool success = points.size() >= PointModel::N_POINTS;
return success;
}
void WIIPointExtractor::_draw_bg(cv::Mat& preview_frame, const struct wii_info &wii)
{
//draw battery status
cv::line(preview_frame,
cv::Point(0, 0),
cv::Point(preview_frame.cols*wii.BatteryPercent / 100, 0),
(wii.bBatteryDrained ? cv::Scalar(255, 0, 0) : cv::Scalar(0, 140, 0)),
2);
//draw horizon
int pdelta = iround((preview_frame.rows / 4) * tan((wii.Pitch)* M_PI / 180.0f));
int rdelta = iround((preview_frame.cols / 4) * tan((wii.Roll)* M_PI / 180.0f));
cv::line(preview_frame,
cv::Point(0, preview_frame.rows / 2 + rdelta - pdelta),
cv::Point(preview_frame.cols, preview_frame.rows / 2 - rdelta - pdelta),
cv::Scalar(80, 80, 80),
1);
}
void WIIPointExtractor::extract_points(const pt_frame& frame_, pt_preview& preview_frame_, std::vector<vec2>& points)
{
const struct wii_info& wii = frame_.as_const<WIIFrame>()->wii;
cv::Mat& preview_frame = *preview_frame_.as<WIIPreview>();
if (wii.status == wii_cam_data_change)
{
draw_bg(preview_frame, wii);
draw_points(preview_frame, wii, points);
}
}
|
