diff options
Diffstat (limited to 'matchblox/common/wiimote_utils.cpp')
| -rw-r--r-- | matchblox/common/wiimote_utils.cpp | 326 |
1 files changed, 326 insertions, 0 deletions
diff --git a/matchblox/common/wiimote_utils.cpp b/matchblox/common/wiimote_utils.cpp new file mode 100644 index 0000000..e47646b --- /dev/null +++ b/matchblox/common/wiimote_utils.cpp @@ -0,0 +1,326 @@ +#include <map>
+#include <iostream>
+#include <iomanip>
+
+#include "typedefs.h"
+#include "message_input.h"
+#include "wiimote_utils.h"
+#include "C_Smoother.h"
+
+
+AbstractWiimote::AbstractWiimote()
+{
+ m_pSensBarDotSmoother[0] = new C_Smoother<Vect3D_t>(Vect3D_t(0.0, 0.0, 0.0));
+ m_pSensBarDotSmoother[0]->SetExponentialMovingAverage(0.2);
+ m_pSensBarDotSmoother[1] = new C_Smoother<Vect3D_t>(Vect3D_t(0.0, 0.0, 0.0));
+ m_pSensBarDotSmoother[1]->SetExponentialMovingAverage(0.2);
+}
+
+AbstractWiimote::~AbstractWiimote()
+{
+ delete m_pSensBarDotSmoother[0];
+ delete m_pSensBarDotSmoother[1];
+}
+
+void AbstractWiimote::FillWiimoteMsgPayload(input_payload_wiimote &f_payload, double f_dSensBarLedDist)
+{
+ Vect3D_t l_Dot[2];
+
+ ParseWiimote(f_payload);
+
+ f_payload.relX = f_payload.relY = f_payload.Zdist = 0.0;
+ f_payload.posDataValid = CalcWiimoteRelativeCursorPos(f_payload, f_dSensBarLedDist);
+}
+
+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)
+
+ return (0.5*f_dLedDist)/tan(0.5*l_dDotAngle); //the distance between the sensorbar and wiimote (in mm)
+}
+
+
+//define some struct rawdot operators
+//inline const struct rawdot &operator=(struct rawdot &lhs, const struct rawdot &rhs)
+//{ lhs.rx=rhs.rx; lhs.ry=rhs.ry; return lhs;}
+inline const struct rawdot &operator-=(struct rawdot &lhs, const struct rawdot &rhs)
+{ lhs.rx-=rhs.rx; lhs.ry-=rhs.ry; return lhs; }
+inline const struct rawdot operator-(const struct rawdot &lhs, const struct rawdot &rhs)
+{ struct rawdot tmp = lhs; return tmp-=rhs;}
+
+inline ostream& operator<<(ostream& os, const struct rawdot& r )
+{
+ stringstream ss;
+ ss << setprecision(4)
+ << "(" << r.rx << ", " << r.ry << ")";
+
+ return os << ss.str();
+}
+
+bool AbstractWiimote::FindSensorBarDots(struct rawdot *f_rd, int f_iNumdots, Vect3D_t f_Dot[2])
+{
+ //the ends of the sensorbar contain more 3 to 5 ir leds each. the wiimote
+ //occaisonally detects individual ir leds at the same sensorbar end
+ //the ir dots have to be mapped to the ends of the sensor bar, so when we
+ //have more than 2 dots we have to group them into two groups
+ int l_GroupId[4] = {-1, -1, -1, -1};
+
+ if (f_iNumdots < 2)
+ {
+ //not enough dots
+ return false;
+ }
+ else if (f_iNumdots == 2)
+ {
+ //two dots (easy case)
+ l_GroupId[0] = 0;
+ l_GroupId[1] = 1;
+ //sort the dots below
+ }
+ else //more than 2 dots...
+ {
+ //group dots that are close to each other and compute their average position
+ //first compute the squared distance of all dot pairs and then groupe them
+ //into two groups based on their proximities
+ typedef pair<int,int> intpair;
+ map<int, intpair> m_Distances; //the squared distances between the pair of dots
+
+ //iterate through all pairs and compute their squared distance
+ for(int i=0; i<f_iNumdots; i++)
+ {
+ for(int j=i+1; j<f_iNumdots; j++)
+ {
+ rawdot d = f_rd[i] - f_rd[j];
+ int dist2 = d.rx*d.rx + d.ry*d.ry;
+ m_Distances[dist2] = intpair(i,j);
+ }
+ }
+
+ //first assign the two dots that are furthest apart into two seperate groups then
+ //iterate through the distances from smallest to largest and assign pairs of
+ //dots closest to each other to a group. for 4 dots there are 6 pairs, for 3 dots 3 pairs
+ //continue until all points are grouped into two groups
+ int l_iDotsLeft = f_iNumdots; //number of dots not assigned to a group
+ //get the pair of dots that are furthest away
+ intpair l_dotpair = (--m_Distances.end())->second;
+ l_GroupId[l_dotpair.first] = 0;
+ l_GroupId[l_dotpair.second] = 1;
+ l_iDotsLeft -= 2;
+
+ for(map<int, intpair>::iterator it = m_Distances.begin();
+ it != m_Distances.end() && l_iDotsLeft > 0; it++)
+ {
+ //check if the dots in the pair are assigned a group id
+ intpair dots = it->second;
+
+ //we can only add a dot to a group of the other dot in the pair is already
+ //assigned a group id. If we find a pair of dots of which no dot is assigned
+ //a group id, then we ignore them. If the latter case occurs it means that
+ //the pair is closer to each other then the two initial dots, which means
+ //that we are probably not processing ir dots captured from the sensor bar
+ if (l_GroupId[dots.first] != -1 && l_GroupId[dots.second] == -1)
+ {
+ //second belongs to the same group assigned to first
+ l_GroupId[dots.second] = l_GroupId[dots.first];
+ l_iDotsLeft--;
+ }
+ else if (l_GroupId[dots.first] == -1 && l_GroupId[dots.second] != -1)
+ {
+ //first belongs to the same group assigned to second
+ l_GroupId[dots.first] = l_GroupId[dots.second];
+ l_iDotsLeft--;
+ }
+ }
+ }
+
+ //calculate the average dots
+ int l_iDotsPerGroup[2] = {0,0};
+
+ f_Dot[0] = f_Dot[1] = Vect3D_t(0.0, 0.0, 0.0);
+ for (int i=0; i<f_iNumdots; i++)
+ {
+ if (l_GroupId[i] > -1)
+ {
+ f_Dot[l_GroupId[i]] += Vect3D_t((double)f_rd[i].rx, (double)f_rd[i].ry, 0.0);
+ l_iDotsPerGroup[l_GroupId[i]]++;
+ }
+ }
+ f_Dot[0] /= (double)l_iDotsPerGroup[0];
+ f_Dot[1] /= (double)l_iDotsPerGroup[1];
+
+ bool swapped = false;
+
+ //sort the dots such that f_Dot[0] is the leftmost
+ if (f_Dot[0].x > f_Dot[1].x || (f_Dot[0].x == f_Dot[1].x && f_Dot[0].y > f_Dot[1].y))
+ {
+ //swap
+ Vect3D_t tmp = f_Dot[0];
+ f_Dot[0] = f_Dot[1];
+ f_Dot[1] = tmp;
+ swapped = true;
+ }
+
+ ////print debug stuff
+ /*if (f_iNumdots > 2)
+ {
+ int gid = swapped ? 1 : 0;
+
+ cout.fill(' ');
+
+ cout << setw(32) << "Group 0:" << "Group 1:" << endl;
+ for (int i = 0; i < 4; i++)
+ {
+ if (l_GroupId[i] == gid && i < f_iNumdots)
+ {
+ cout << setw(32) << f_rd[i] << setw(32) << "------" << endl;
+ }
+ else if (i < f_iNumdots)
+ {
+ cout << setw(32) << "------" << setw(32) << f_rd[i] << endl;
+ }
+ else
+ {
+ cout << setw(32) << "------" << setw(32) << "------" << endl;
+ }
+ }
+ cout << endl << setw(32) << "Average:" << endl;
+ cout << setw(32) << f_Dot[0] << setw(32) << f_Dot[1] << endl << endl;
+ }*/
+ return true;
+}
+
+bool AbstractWiimote::CalcWiimoteRelativeCursorPos(input_payload_wiimote &f_WiimoteMsg, double f_dSensBarLedDist)
+{
+ Vect3D_t l_Dot[2], l_RelPos,
+ l_CameraRes(g_dWiimoteXCamResolution, g_dWiimoteYCamResolution, 1.0);
+
+ if (!FindSensorBarDots(f_WiimoteMsg.irdot, f_WiimoteMsg.nrdots, l_Dot))
+ return false;
+
+ //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;
+ if (l_delta.x != 0.0)
+ {
+ theta = -atan(l_delta.y/l_delta.x);
+ }
+ else
+ {
+ theta = l_delta.y > 0.0? -M_PI/2.0: M_PI/2.0;
+ }
+
+ //compute the xy coordinates relative to the center of the wiimote camera
+ Vect3D_t l_CameraCenter = l_CameraRes / 2.0,
+ l_RelFromCenter = ((l_Dot[0] + l_Dot[1])/2.0) - l_CameraCenter;
+
+ //rotate the position around the camera center position to compensate for wiimote rotation
+ f_WiimoteMsg.relX = l_CameraCenter.x + cos(theta)*l_RelFromCenter.x - sin(theta)*l_RelFromCenter.y;
+ f_WiimoteMsg.relY = l_CameraCenter.y + sin(theta)*l_RelFromCenter.x + cos(theta)*l_RelFromCenter.y;
+
+ //now devide the rotated pixel coordinates by the camera resolution to get values in the range [0,1]?? <- not true
+ f_WiimoteMsg.relX /= l_CameraRes.x;
+ f_WiimoteMsg.relY /= l_CameraRes.y;
+
+ f_WiimoteMsg.Zdist = CalcZDistInMM(l_Dot, f_dSensBarLedDist);
+
+ //cout << "Z_mm: " << std::fixed << std::setprecision(2) << setw(10) << left << f_pRelPos->z ;
+ return true;
+}
+
+#ifdef USE_WIIYOURSELF
+
+#include <wiimote.h>
+
+WiiYourselfWiimote::WiiYourselfWiimote()
+{
+ m_pWm = new wiimote();
+ m_pWm->ChangedCallback = NULL; //no callbacks, we just poll...
+ m_pWm->CallbackTriggerFlags = NO_CHANGE;
+}
+
+WiiYourselfWiimote::~WiiYourselfWiimote()
+{
+ if (m_pWm->IsConnected())
+ {
+ m_pWm->Disconnect();
+ }
+
+ delete m_pWm;
+}
+
+bool WiiYourselfWiimote::Connect()
+{
+ if (m_pWm->Connect())
+ {
+ //connected set ir report mode
+ m_pWm->SetReportType(wiimote::IN_BUTTONS_ACCEL_IR, true);
+ return true;
+ }
+ else
+ {
+ return false;
+ }
+}
+
+void WiiYourselfWiimote::StartRumble()
+{
+ m_pWm->SetRumble(true);
+}
+
+void WiiYourselfWiimote::StopRumble()
+{
+ m_pWm->SetRumble(false);
+}
+
+void WiiYourselfWiimote::SetLeds(unsigned char bitmask)
+{
+ m_pWm->SetLEDs(bitmask);
+}
+
+bool WiiYourselfWiimote::IsConnected()
+{
+ return m_pWm->IsConnected();
+}
+
+void WiiYourselfWiimote::ParseWiimote(input_payload_wiimote &f_payload)
+{
+ m_pWm->RefreshState();
+
+ f_payload.nrdots = 0;
+
+ for (int i=0; i<4; i++)
+ {
+ if (m_pWm->IR.Dot[i].bFound)
+ {
+ f_payload.irdot[f_payload.nrdots].rx = m_pWm->IR.Dot[i].RawX;
+ f_payload.irdot[f_payload.nrdots].ry = m_pWm->IR.Dot[i].RawY;
+ f_payload.nrdots++;
+ }
+ }
+
+ f_payload.btns = 0;
+ f_payload.btns |= (m_pWm->Button.A()? WIIMOTE_BUTTON_A : 0) |
+ (m_pWm->Button.B()? WIIMOTE_BUTTON_B : 0) |
+ (m_pWm->Button.Up() ? WIIMOTE_BUTTON_UP : 0) |
+ (m_pWm->Button.Down()? WIIMOTE_BUTTON_DOWN : 0) |
+ (m_pWm->Button.Left()? WIIMOTE_BUTTON_LEFT : 0) |
+ (m_pWm->Button.Right()? WIIMOTE_BUTTON_RIGHT : 0) |
+ (m_pWm->Button.One()? WIIMOTE_BUTTON_ONE : 0) |
+ (m_pWm->Button.Two()? WIIMOTE_BUTTON_TWO : 0) |
+ (m_pWm->Button.Plus()? WIIMOTE_BUTTON_PLUS : 0) |
+ (m_pWm->Button.Minus()? WIIMOTE_BUTTON_MINUS : 0) |
+ (m_pWm->Button.Home()? WIIMOTE_BUTTON_HOME : 0);
+}
+
+#endif //USE_WIIYOURSELF
\ No newline at end of file |