wiimote ir smoothing ingebouwd

7 files changed, 669 insertions, 42 deletions

diff --git a/matchblox/common/C_Smoother.cpp b/matchblox/common/C_Smoother.cpp
new file mode 100644
index 0000000..010aba1
--- /dev/null
+++ b/matchblox/common/C_Smoother.cpp
@@ -0,0 +1,10 @@
+#include <map>

+#include <iostream>

+#include <iomanip>

+

+#include "C_Smoother.h"

+

+using namespace std;

+

+

+

diff --git a/matchblox/common/C_Smoother.h b/matchblox/common/C_Smoother.h
new file mode 100644
index 0000000..e7d9508
--- /dev/null
+++ b/matchblox/common/C_Smoother.h
@@ -0,0 +1,122 @@
+#ifndef C_3D_POINT_SMOOTHER_H_

+

+#define C_3D_POINT_SMOOTHER_H_

+

+#include <iostream>

+#include <list>

+#include <iostream>

+#include <iomanip>

+

+using namespace std;

+

+typedef enum

+{                 //           k-1

+  SimpMovingAvg,  //S(t) = (1/k)*sum(X(t-n))  =  S(t-1) + (X(t) - X(t-k))/k

+                  //           n=0

+

+  ExpMovingAvg    //S(0) = X(0)

+                  //S(t) = a*X(t) + (1 - a)*S(t-1) = S(t-1) + a*(X(t) - S(t-1))

+

+} SmoothingMethod;

+

+

+template<typename T>

+class C_Smoother

+{

+public:

+  C_Smoother(T f_ZeroVal) : 

+    m_Method(SimpMovingAvg), 

+    m_iWindowSize(2), 

+    m_iSampCount(0),

+    m_ZeroVal(f_ZeroVal),

+    m_dExponent(0.0) {}

+

+  void SetSimpleMovingAverage(int f_iWindowSize);

+  void SetExponentialMovingAverage(double f_dExponent);

+

+  T Smooth(const T &f_Xt);

+

+private:

+  SmoothingMethod m_Method;

+  int             m_iWindowSize, //window size for SimpMovingAvg

+                  m_iSampCount;  //number of processed samples

+  list<T>         m_SampWindow;

+  double          m_dExponent;   //exponent for ExpMovingAvg

+  T               m_ZeroVal,     //the value that is supposed to be zero

+                  m_St;          //last smoothed statistic S(t)

+

+  T SmoothSample_SimpMovingAvg(const T &f_Xt);

+  T SmoothSample_ExpMovingAvg(const T &f_Xt);

+};

+

+template <typename T> 

+void C_Smoother<T>::SetSimpleMovingAverage(int f_iWindowSize)

+{

+  //reset smoothing stuff for Simple moving average smoothing

+  m_Method = SimpMovingAvg;

+  m_iWindowSize = f_iWindowSize;

+  m_iSampCount = 0;

+  m_St = m_ZeroVal; 

+  m_SampWindow.clear();

+}

+

+template <typename T> 

+void C_Smoother<T>::SetExponentialMovingAverage(double f_dExponent)

+{

+  m_Method = ExpMovingAvg;

+  m_dExponent = f_dExponent;

+  m_iSampCount = 0;

+  m_St = m_ZeroVal;

+}

+

+template <typename T> 

+T C_Smoother<T>::SmoothSample_SimpMovingAvg(const T &f_Xt)

+{

+  //add sample to the window

+  m_SampWindow.push_front(f_Xt);

+

+  if (m_iSampCount < m_iWindowSize)

+  {

+    //we don't have enough cmopute the average of the points currently in the window

+    T l_sum = m_ZeroVal;

+    for (std::list<T>::iterator it = m_SampWindow.begin(); it != m_SampWindow.end(); it++)

+      l_sum += *it;

+    return l_sum / (double)m_SampWindow.size();

+  }

+  else

+  {

+    //there are mote than m_iWindowSize samples, incrementally update smoothed sample

+    T l_Xtk = m_SampWindow.back(); //pop the last point from the (window) list

+    m_SampWindow.pop_back(); 

+    return m_St + ((f_Xt - l_Xtk)/(double)m_iWindowSize);

+  }

+}

+

+template <typename T> 

+T C_Smoother<T>::SmoothSample_ExpMovingAvg(const T &f_Xt)

+{

+  if (m_iSampCount == 0)

+    return f_Xt;

+  else

+    return m_St + m_dExponent * (f_Xt - m_St);

+}

+

+template <typename T>

+T C_Smoother<T>::Smooth(const T &f_Xt)

+{

+  switch(m_Method)

+  {

+  case SimpMovingAvg:

+      m_St = SmoothSample_SimpMovingAvg(f_Xt);

+    break;

+

+  case ExpMovingAvg:

+      m_St = SmoothSample_ExpMovingAvg(f_Xt);

+    break;

+  }

+

+  m_iSampCount++;

+  return m_St;

+}

+

+#endif //C_3D_POINT_SMOOTHER_H_
\ No newline at end of file
diff --git a/matchblox/common/message_input.h b/matchblox/common/message_input.h
index e234b74..093d28e 100644
--- a/matchblox/common/message_input.h
+++ b/matchblox/common/message_input.h
@@ -1,44 +1,66 @@
-/* -*- Mode: C; indent-tabs-mode: t; c-basic-offset: 2; tab-width: 2 -*- */
-#ifndef _CMESSAGE_INPUT_H
-#define _CMESSAGE_INPUT_H
-
-
-#define MESSAGE_INPUT_KEYBOARD  0x00001100
-#define MESSAGE_INPUT_MOUSE     0x00001200
-#define MESSAGE_INPUT_WIIMOTE   0x00001400
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-#include <wiiuse.h>
-
-struct input_payload_keyboard
-{
-  int specialkey;
-  int key;
-  int modifier;
-  int x;
-  int y;
-};
-
-struct input_payload_mouse
-{
-  int passive;
-  int button;
-  int state;
-  int x;
-  int y;
-};
-
+/* -*- Mode: C; indent-tabs-mode: t; c-basic-offset: 2; tab-width: 2 -*- */

+#ifndef _CMESSAGE_INPUT_H

+#define _CMESSAGE_INPUT_H

+

+

+#define MESSAGE_INPUT_KEYBOARD  0x00001100

+#define MESSAGE_INPUT_MOUSE     0x00001200

+#define MESSAGE_INPUT_WIIMOTE   0x00001400

+

+#ifdef __cplusplus

+extern "C" {

+#endif

+

+struct input_payload_keyboard

+{

+  int specialkey;

+  int key;

+  int modifier;

+  int x;

+  int y;

+};

+

+struct input_payload_mouse

+{

+  int passive;

+  int button;

+  int state;

+  int x;

+  int y;

+};

+

+

+enum wiimote_btns

+{

+  WIIMOTE_BUTTON_TWO	 = 0x0001,

+  WIIMOTE_BUTTON_ONE   = 0x0002,

+  WIIMOTE_BUTTON_B		 = 0x0004,

+  WIIMOTE_BUTTON_A		 = 0x0008,

+  WIIMOTE_BUTTON_MINUS = 0x0010,

+  WIIMOTE_BUTTON_HOME  = 0x0080,

+  WIIMOTE_BUTTON_LEFT  = 0x0100,

+  WIIMOTE_BUTTON_RIGHT = 0x0200,

+  WIIMOTE_BUTTON_DOWN	 = 0x0400,

+  WIIMOTE_BUTTON_UP		 = 0x0800,

+  WIIMOTE_BUTTON_PLUS	 = 0x1000,

+  WIIMOTE_BUTTON_ALL	 = 0x1F9F

+};

+

+struct rawdot { int rx, ry; };

+

 struct input_payload_wiimote

 {

-  unsigned short btns;

-  ir_t ir;

-};
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif
+  int    btns,     //button bitmask

+         nrdots;

+  double relX,  //relative x coordinate of mouse cursor [0,1]

+         relY,  //relative y coordinate of mouse cursor [0,1]

+         Zdist; //distance from sensorbar in mm

+  struct rawdot irdot[4];

+  char   posDataValid; //is true when the position data is valid (nrdots > 1)

+};

+

+#ifdef __cplusplus

+}

+#endif

+

+#endif

diff --git a/matchblox/common/textfile.cpp b/matchblox/common/textfile.cpp
new file mode 100644
index 0000000..0b74282
--- /dev/null
+++ b/matchblox/common/textfile.cpp
@@ -0,0 +1,69 @@
+// textfile.cpp

+//

+// simple reading and writing for text files

+//

+// www.lighthouse3d.com

+//

+// You may use these functions freely.

+// they are provided as is, and no warranties, either implicit,

+// or explicit are given

+//////////////////////////////////////////////////////////////////////

+

+

+#include <stdio.h>

+#include <stdlib.h>

+#include <string.h>

+

+

+char *textFileRead(char *fn) {

+

+

+	FILE *fp;

+	char *content = NULL;

+

+	int count=0;

+

+	if (fn != NULL) {

+		fp = fopen(fn,"rt");

+

+		if (fp != NULL) {

+      

+      fseek(fp, 0, SEEK_END);

+      count = ftell(fp);

+      rewind(fp);

+

+			if (count > 0) {

+				content = (char *)malloc(sizeof(char) * (count+1));

+				count = fread(content,sizeof(char),count,fp);

+				content[count] = '\0';

+			}

+			fclose(fp);

+		}

+	}

+	return content;

+}

+

+int textFileWrite(char *fn, char *s) {

+

+	FILE *fp;

+	int status = 0;

+

+	if (fn != NULL) {

+		fp = fopen(fn,"w");

+

+		if (fp != NULL) {

+			

+			if (fwrite(s,sizeof(char),strlen(s),fp) == strlen(s))

+				status = 1;

+			fclose(fp);

+		}

+	}

+	return(status);

+}

+

+

+

+

+

+

+

diff --git a/matchblox/common/textfile.h b/matchblox/common/textfile.h
new file mode 100644
index 0000000..1c30e53
--- /dev/null
+++ b/matchblox/common/textfile.h
@@ -0,0 +1,10 @@
+// textfile.h: interface for reading and writing text files

+// www.lighthouse3d.com

+//

+// You may use these functions freely.

+// they are provided as is, and no warranties, either implicit,

+// or explicit are given

+//////////////////////////////////////////////////////////////////////

+

+char *textFileRead(char *fn);

+int textFileWrite(char *fn, char *s);

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
diff --git a/matchblox/common/wiimote_utils.h b/matchblox/common/wiimote_utils.h
new file mode 100644
index 0000000..edb59ce
--- /dev/null
+++ b/matchblox/common/wiimote_utils.h
@@ -0,0 +1,68 @@
+#ifndef WIIMOTE_UTILS_H

+

+#define WIIMOTE_UTILS_H

+

+#include "message_input.h"

+#include "typedefs.h"

+#include "C_Smoother.h"

+

+#define _USE_MATH_DEFINES

+#include <math.h>

+

+static double g_dWiimoteXCamResolution = 1016.0;

+static double g_dWiimoteYCamResolution =  760.0;

+static double g_dWiimoteRadPerPixel    = (41.0 * (M_PI/180.0))/g_dWiimoteXCamResolution;

+

+//abstract wiimote needs to be inherited and the virtual functions implemented to communicate

+//with a wiimote lib

+class AbstractWiimote

+{

+public:

+  AbstractWiimote();

+  ~AbstractWiimote();

+

+  virtual bool Connect() = 0;

+  virtual void StartRumble() = 0;

+  virtual void StopRumble() = 0;

+  virtual void SetLeds(unsigned char bitmask) = 0;

+  virtual bool IsConnected() = 0;

+

+  void FillWiimoteMsgPayload(input_payload_wiimote &f_payload, double f_dSensBarLedDist);

+

+protected:

+  virtual void ParseWiimote(input_payload_wiimote &f_payload) = 0;

+

+  bool   FindSensorBarDots(rawdot *f_rd, int f_iNumdots, Vect3D_t f_Dots[2]);

+  bool   CalcWiimoteRelativeCursorPos(input_payload_wiimote &f_pWiimoteMsg, double f_dSensBarLedDist);

+  double CalcZDistInMM(Vect3D_t f_Dots[2], double f_dSensBarLedDist);

+

+  C_Smoother<Vect3D_t> *m_pSensBarDotSmoother[2];

+};

+

+#ifdef USE_WIIYOURSELF

+

+#include <wiimote.h>

+

+//implementation of an abstract wiimote using the wiiyourself! lib

+class WiiYourselfWiimote : public AbstractWiimote

+{

+public:

+  WiiYourselfWiimote();

+  ~WiiYourselfWiimote();

+

+  bool Connect();

+  void StartRumble();

+  void StopRumble();

+  void SetLeds(unsigned char bitmask);

+  bool IsConnected();

+

+  void ParseWiimote(input_payload_wiimote &f_payload);

+

+private:

+  wiimote *m_pWm;

+};

+

+#endif //USE_WIIYOURSELF

+

+

+#endif //WIIMOTE_UTILS_H
\ No newline at end of file