Skip to content

Commit 44faa81

Browse files
TuxSHTuxSH
committed
Rewrite and fix QTM example, add lenny+qtm example
1 parent 979ab79 commit 44faa81

File tree

9 files changed

+4125
-91
lines changed

9 files changed

+4125
-91
lines changed

‎qtm/README.md‎

Lines changed: 0 additions & 6 deletions
This file was deleted.

qtm/Makefile renamed to qtm/eye_position/Makefile

File renamed without changes.

‎qtm/eye_position/source/main.c‎

Lines changed: 214 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,214 @@
1+
#include <string.h>
2+
#include <stdio.h>
3+
#include <3ds.h>
4+
#include <math.h>
5+
6+
void printTrackingData(const QtmTrackingData *data)
7+
{
8+
printf("Flags: %d, %d, %d, %d \n", data->eyesTracked, data->faceDetected, data->eyesDetected, data->clamped);
9+
printf("Confidence level: %.4f \n\n", data->confidenceLevel);
10+
11+
printf("Left eye X (cam): % .4f \n", data->eyeCameraCoordinates[QTM_EYE_LEFT][0]);
12+
printf("Left eye Y (cam): % .4f \n", data->eyeCameraCoordinates[QTM_EYE_LEFT][1]);
13+
printf("Right eye X (cam): % .4f \n", data->eyeCameraCoordinates[QTM_EYE_RIGHT][0]);
14+
printf("Right eye Y (cam): % .4f \n\n", data->eyeCameraCoordinates[QTM_EYE_RIGHT][1]);
15+
16+
// Was previously using data - previous data dPitch, but these values were changing too fast
17+
// making this print look weird.
18+
printf("Pitch deviation: % .4f \n", data->dPitch);
19+
printf("Yaw deviation: % .4f \n", data->dYaw);
20+
printf("Roll deviation: % .4f \n\n", data->dRoll);
21+
//printf("Tick: %lld \n\n", data->samplingTick);
22+
23+
printf("Distance (approx): %d cm \n", (int)roundf(qtmEstimateEyeToCameraDistance(data) / 10.0f));
24+
printf("Head tilt angle: % .4f deg \n", qtmComputeHeadTiltAngle(data) * 180.0 / M_PI);
25+
26+
}
27+
28+
// Convert natural (x, y) coords into 3DS top screen pixel coordinates, taking screen rotation into account
29+
static inline size_t getPixelOffset(int x, int y, size_t bpp)
30+
{
31+
return bpp * (240*x + (240 - y));
32+
}
33+
34+
static inline void writePixel24(void *fb, u32 color, int x, int y)
35+
{
36+
memcpy((u8 *)fb + getPixelOffset(x, y, 3), &color, 3);
37+
}
38+
39+
static void updateEyePixelMarkers(void *fb, const QtmTrackingData *data, const QtmTrackingData *prevData)
40+
{
41+
// Put origin at center of screen, with camera space being 320x240 as usual
42+
// Y axis goes to the top, hence the minus signs
43+
44+
// Left eye
45+
int x0L = (int)(200 + 160 * prevData->eyeCameraCoordinates[QTM_EYE_LEFT][0]);
46+
int y0L = (int)(120 - 160 * prevData->eyeCameraCoordinates[QTM_EYE_LEFT][1]);
47+
48+
// Right eye
49+
int x0R = (int)(200 + 160 * prevData->eyeCameraCoordinates[QTM_EYE_RIGHT][0]);
50+
int y0R = (int)(120 - 160 * prevData->eyeCameraCoordinates[QTM_EYE_RIGHT][1]);
51+
52+
// Replace old markers by black
53+
writePixel24(fb, 0x000000, x0L, y0L);
54+
writePixel24(fb, 0x000000, x0R, y0R);
55+
56+
if (!data)
57+
return;
58+
59+
// Left eye
60+
int x1L = (int)(200 + 160 * data->eyeCameraCoordinates[QTM_EYE_LEFT][0]);
61+
int y1L = (int)(120 - 160 * data->eyeCameraCoordinates[QTM_EYE_LEFT][1]);
62+
63+
// Right eye
64+
int x1R = (int)(200 + 160 * data->eyeCameraCoordinates[QTM_EYE_RIGHT][0]);
65+
int y1R = (int)(120 - 160 * data->eyeCameraCoordinates[QTM_EYE_RIGHT][1]);
66+
67+
// New left eye pixel marker: blue
68+
writePixel24(fb, 0x0000FF, x1L, y1L);
69+
70+
// New right eye pixel marker: red
71+
writePixel24(fb, 0xFF0000, x1R, y1R);
72+
}
73+
74+
static void drawBorderLine(void *fb, u32 color, int startX, int len)
75+
{
76+
for (int i = 0; i < len; i++)
77+
{
78+
for (int j = 0; j < 240; j++)
79+
writePixel24(fb, color, startX + i, 240 - j);
80+
}
81+
}
82+
83+
static void prepareFb(void *fb)
84+
{
85+
// 320x240 camera resolution, means borders at left+40-1px, right-40+1px
86+
87+
memset(fb, 0, 400*240*3);
88+
drawBorderLine(fb, 0xFFFFFF, 35, 4);
89+
drawBorderLine(fb, 0xFFFFFF, 400 - 40 + 1, 4);
90+
}
91+
92+
int main(void)
93+
{
94+
gfxInitDefault();
95+
consoleInit(GFX_BOTTOM, NULL);
96+
consoleClear();
97+
98+
prepareFb(gfxGetFramebuffer(GFX_TOP, GFX_LEFT, NULL, NULL));
99+
gfxSwapBuffers();
100+
prepareFb(gfxGetFramebuffer(GFX_TOP, GFX_LEFT, NULL, NULL));
101+
102+
gfxFlushBuffers();
103+
104+
Result res = 0;
105+
106+
if (qtmCheckServicesRegistered())
107+
{
108+
// Use "qtm:s" to get camera luminance. Otherwise "qtm:u" would have been fine
109+
res = qtmInit(QTM_SERVICE_SYSTEM);
110+
if (R_FAILED(res))
111+
printf("qtmInit(QTM_SERVICE_SYSTEM) failed with result %08lx\n", res);
112+
}
113+
114+
bool qtmAvailable = true;
115+
if (qtmIsInitialized())
116+
{
117+
bool blacklisted = false;
118+
res = QTMU_IsCurrentAppBlacklisted(&blacklisted);
119+
if (R_FAILED(res))
120+
printf("QTMU_IsCurrentAppBlacklisted failed with result %08lx\n", res);
121+
else if (blacklisted)
122+
printf("QTM has been blacklisted for this application\n");
123+
qtmAvailable = !blacklisted;
124+
}
125+
126+
if (!qtmAvailable)
127+
printf("QTM is not available.\n\nPress START to exit.");
128+
129+
QtmTrackingData prevPrevTrackingData = {0};
130+
QtmTrackingData prevTrackingData = {0};
131+
QtmTrackingData curTrackingData = {0};
132+
133+
float lux = 400.0f;
134+
135+
// Main loop
136+
while (aptMainLoop())
137+
{
138+
gspWaitForVBlank();
139+
hidScanInput();
140+
141+
u8* fb = gfxGetFramebuffer(GFX_TOP, GFX_LEFT, NULL, NULL);
142+
u32 kDown = hidKeysDown();
143+
u32 kHeld = hidKeysHeld();
144+
if (kDown & KEY_START)
145+
break; // break in order to return to hbmenu
146+
147+
if(qtmAvailable)
148+
{
149+
QtmTrackingData tmp;
150+
if (!(kHeld & KEY_B))
151+
{
152+
res = QTMU_GetTrackingData(&tmp);
153+
if (R_SUCCEEDED(res))
154+
{
155+
prevPrevTrackingData = prevTrackingData;
156+
prevTrackingData = curTrackingData;
157+
curTrackingData = tmp;
158+
159+
// Update markers as we have new data
160+
// Clear t-2 instead of t-1 markers because of double buffering
161+
updateEyePixelMarkers(fb, &curTrackingData, &prevPrevTrackingData);
162+
}
163+
else switch (res)
164+
{
165+
case 0xC8A183EFu:
166+
printf("QTM is unavailable\n");
167+
qtmAvailable = false;
168+
break;
169+
case 0xC8A18008u:
170+
printf("QTM is paused because camera is in use by user\n");
171+
qtmAvailable = false;
172+
default:
173+
// Should not happen
174+
printf("QTMU_GetTrackingData returned unknown err code %08lx\n", res);
175+
qtmAvailable = false;
176+
break;
177+
}
178+
}
179+
else
180+
{
181+
prevPrevTrackingData = prevTrackingData;
182+
prevTrackingData = curTrackingData;
183+
updateEyePixelMarkers(fb, &curTrackingData, &prevPrevTrackingData);
184+
}
185+
186+
187+
if (qtmAvailable && !(kHeld & KEY_B))
188+
{
189+
// Also update camera luminance (requires "qtm:s")
190+
QTMS_GetCameraLuminance(&lux);
191+
}
192+
193+
if (qtmAvailable)
194+
{
195+
// Reset cursor
196+
printf("\x1b[H");
197+
printTrackingData(&curTrackingData);
198+
printf("\nCam luminance (lux): %2f \n", lux);
199+
}
200+
}
201+
202+
// Flush and swap framebuffers
203+
gfxFlushBuffers();
204+
gfxSwapBuffers();
205+
}
206+
207+
// Exit services
208+
if (qtmIsInitialized())
209+
qtmExit();
210+
211+
gfxExit();
212+
return 0;
213+
}
214+

0 commit comments

Comments
 (0)