diff options
Diffstat (limited to 'matchblox/common/wiimote_utils.cpp')
| -rw-r--r-- | matchblox/common/wiimote_utils.cpp | 21 |
1 files changed, 13 insertions, 8 deletions
diff --git a/matchblox/common/wiimote_utils.cpp b/matchblox/common/wiimote_utils.cpp index 6d0d7a0..7c39019 100644 --- a/matchblox/common/wiimote_utils.cpp +++ b/matchblox/common/wiimote_utils.cpp @@ -42,11 +42,11 @@ void AbstractWiimote::FillWiimoteMsgPayload(input_payload_wiimote &f_payload, do double AbstractWiimote::CalcZDistInMM(Vect3D_t f_Dot[2], double f_dLedDist)
{
- double l_dX = f_Dot[0].x - f_Dot[1].x, //difference in x coordinates
- l_dY = f_Dot[0].y - f_Dot[1].y, //difference in y coordinates
- l_dDotDist = sqrt(l_dX*l_dX + l_dY*l_dY), //distance between ir dots (in camera pixels)
- l_dDotAngle = g_dWiimoteRadPerPixel * l_dDotDist; //the angle between the lines from the camera through the two
- //ir dots (in radians)
+ double l_dX = f_Dot[0].x - f_Dot[1].x, //difference in x coordinates
+ l_dY = f_Dot[0].y - f_Dot[1].y, //difference in y coordinates
+ l_dDotDist = sqrt(l_dX*l_dX + l_dY*l_dY), //distance between ir dots (in camera pixels)
+ l_dDotAngle = g_dWiimoteRadPerPixel * l_dDotDist; //the angle between the lines from the camera through the two
+ //ir dots (in radians)
return (0.5*f_dLedDist)/tan(0.5*l_dDotAngle); //the distance between the sensorbar and wiimote (in mm)
}
@@ -209,13 +209,18 @@ bool AbstractWiimote::CalcWiimoteRelativeCursorPos(input_payload_wiimote &f_Wiim if (!FindSensorBarDots(f_WiimoteMsg.irdot, f_WiimoteMsg.nrdots, l_Dot))
return false;
+ //smooth the ir dots
+ l_Dot[0] = m_pSensBarDotSmoother[0]->Smooth(l_Dot[0]);
+ l_Dot[1] = m_pSensBarDotSmoother[1]->Smooth(l_Dot[1]);
+
+ //store the raw smoothed sensor bar dots in the message payload (used for head tracking)
+ f_WiimoteMsg.SensorBarDot[0].sx = l_Dot[0].x; f_WiimoteMsg.SensorBarDot[0].sy = l_Dot[0].y;
+ f_WiimoteMsg.SensorBarDot[1].sx = l_Dot[1].x; f_WiimoteMsg.SensorBarDot[1].sy = l_Dot[1].y;
+
//invert the x and y axis to correspond to screen coordinates
l_Dot[0] = l_CameraRes - l_Dot[0];
l_Dot[1] = l_CameraRes - l_Dot[1];
- //smooth the ir dots
- l_Dot[0] = m_pSensBarDotSmoother[0]->Smooth(l_Dot[0]);
- l_Dot[1] = m_pSensBarDotSmoother[1]->Smooth(l_Dot[1]);
//calc the angle of the wiimote with respect to the sensorbar
Vect3D_t l_delta = l_Dot[1] - l_Dot[0];
double theta;
|