Skip to content

Commit f781af9

Browse files
committed
qtm: fix math error in qtmEstimateEyeToCameraDistance
1 parent 5290d64 commit f781af9

1 file changed

Lines changed: 34 additions & 4 deletions

File tree

  • libctru/source/services

‎libctru/source/services/qtm.c‎

Lines changed: 34 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -126,13 +126,43 @@ float qtmComputeHeadTiltAngle(const QtmTrackingData *data)
126126

127127
float qtmEstimateEyeToCameraDistance(const QtmTrackingData *data)
128128
{
129-
float fovX = qtmComputeFovX(data);
130-
float pxDistNorm = hypotf(
129+
/*
130+
X_L ●────────── IOD = 62 mm ──────────● X_R
131+
\ /
132+
\ /
133+
\ /
134+
\ /
135+
x_l x_r
136+
────────────────────────●─────────────────────●──────────────────────── plane at depth Z
137+
\ / (world coordinates live here)
138+
\ /
139+
\ /
140+
\ /
141+
\ /
142+
\ /
143+
\ /
144+
\ /
145+
\ /
146+
\ /
147+
◆ camera
148+
149+
The triangles are similar, thus:
150+
Δx = x_r - x_l = (X_R - X_L) / Z
151+
Δy = y_r - y_l = (Y_R - Y_L) / Z
152+
Thus:
153+
hypot(Δx, Δy) = sqrt(ΔX² + ΔY²) / Z = IOD / Z
154+
Therefore:
155+
Z = IOD / hypot(Δx, Δy)
156+
157+
Note: QTM's world Y is 0.75x too small (QTM applies the 4:3 aspect ratio twice, bug?), so we undo it here.
158+
*/
159+
160+
float h = hypotf(
131161
data->eyeWorldCoordinates[QTM_EYE_RIGHT][0] - data->eyeWorldCoordinates[QTM_EYE_LEFT][0],
132-
data->eyeWorldCoordinates[QTM_EYE_RIGHT][1] - data->eyeWorldCoordinates[QTM_EYE_LEFT][1]
162+
(data->eyeWorldCoordinates[QTM_EYE_RIGHT][1] - data->eyeWorldCoordinates[QTM_EYE_LEFT][1]) / 0.75f
133163
);
134164

135-
return 31.0f / tanf(0.5f * (pxDistNorm * fovX));
165+
return 62.0f / h;
136166
}
137167

138168
Result QTMU_EnableManualIrLedControl(void)

0 commit comments

Comments
 (0)