wiimote ir smoothing ingebouwd

20 files changed, 1224 insertions, 504 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
diff --git a/matchblox/engine/C_3DObject.cpp b/matchblox/engine/C_3DObject.cpp
index 75e7ead..b9186cd 100644
--- a/matchblox/engine/C_3DObject.cpp
+++ b/matchblox/engine/C_3DObject.cpp
@@ -14,7 +14,7 @@ C_3DObject::C_3DObject()
    m_bOwnTexture(false),

    m_bDispListValid(false)

 {

-  std::cout << "C_3DObject:: default constructor\n";

+  //std::cout << "C_3DObject:: default constructor\n";

 }

    

 

@@ -71,11 +71,11 @@ C_3DObject::~C_3DObject()
 

 void C_3DObject::TransRotateScale()

 {

-  glScaled(m_Scale.x, m_Scale.y, m_Scale.z);

   glTranslated(m_Pos.x, m_Pos.y, m_Pos.z);

   glRotated(m_Rot.z, 0.0, 0.0, 1.0);

   glRotated(m_Rot.x, 1.0, 0.0, 0.0);

   glRotated(m_Rot.y, 0.0, 1.0, 0.0);

+  glScaled(m_Scale.x, m_Scale.y, m_Scale.z);

 }

 

 void C_3DObject::Render()

@@ -95,16 +95,6 @@ void C_3DObject::Render()
   glMaterialfv(GL_FRONT, GL_EMISSION, m_Mat.m_fEmi);

   glMaterialf(GL_FRONT, GL_SHININESS, m_Mat.m_fShin);

 

-/*

-  //scale translate and rotate

-  glPushMatrix();

-  

-  glScaled(m_Scale.x, m_Scale.y, m_Scale.z);

-  glTranslated(m_Pos.x, m_Pos.y, m_Pos.z);

-  glRotated(m_Rot.z, 0.0, 0.0, 1.0);

-  glRotated(m_Rot.x, 1.0, 0.0, 0.0);

-  glRotated(m_Rot.y, 0.0, 1.0, 0.0);

-*/

 

   //render by calling the display list

   if(m_bDispListValid)

@@ -115,11 +105,9 @@ void C_3DObject::Render()
   {

     //or render a LARGE cube when we failed to load the

     //geometry

-    glutSolidCube(10.0);

+    glutSolidCube(1.0);

   }

 

-//  glPopMatrix();

-

   glPopAttrib();

 }

 

@@ -140,23 +128,27 @@ bool C_3DObject::CreateObject(const char *f_pFilePath)
   

   l_bParseOk = LoadGeometryData(f_pFilePath, l_Geom);

   

-  //output statistics

-  std::cout << "C_3DObject: file " << f_pFilePath

-            << "\nParse:     " << (l_bParseOk? "SUCCESS" : "FAILURE")

-            << "\nVertices:  " << l_Geom.m_verts.size()-1

-            << "\nNormals:   " << l_Geom.m_norms.size()-1

-            << "\nTexCrds:   " << l_Geom.m_texs.size()-1

-            << "\nTriangles: " << l_Geom.m_triangles.size()

-            << "\nBoundBox:  x(" << m_BBox.m_Min.x << "," << m_BBox.m_Max.x << ")"

-            << "\n           y(" << m_BBox.m_Min.y << "," << m_BBox.m_Max.y << ")"

-            << "\n           z(" << m_BBox.m_Min.z << "," << m_BBox.m_Max.z << ")"

-            << std::endl;

+  ////output statistics

+  //std::cout << "C_3DObject: file " << f_pFilePath

+  //          << "\nParse:     " << (l_bParseOk? "SUCCESS" : "FAILURE")

+  //          << "\nVertices:  " << l_Geom.m_verts.size()-1

+  //          << "\nNormals:   " << l_Geom.m_norms.size()-1

+  //          << "\nTexCrds:   " << l_Geom.m_texs.size()-1

+  //          << "\nTriangles: " << l_Geom.m_triangles.size()

+  //          << "\nBoundBox:  x(" << m_BBox.m_Min.x << "," << m_BBox.m_Max.x << ")"

+  //          << "\n           y(" << m_BBox.m_Min.y << "," << m_BBox.m_Max.y << ")"

+  //          << "\n           z(" << m_BBox.m_Min.z << "," << m_BBox.m_Max.z << ")"

+  //          << std::endl;

 

  if(!l_bParseOk)

   {

     std::cout << "C_3DObject Error: failed to parse file "

               << f_pFilePath << std::endl;

 

+    //set bounding box to the size of a unit cube

+    m_BBox.m_Max = Vect3D_t(0.5, 0.5, 0.5);

+    m_BBox.m_Min = Vect3D_t(-0.5, -0.5, -0.5);

+

     return false;

   }

   

diff --git a/matchblox/engine/C_3DObject.h b/matchblox/engine/C_3DObject.h
index 55ae123..2ad5a88 100644
--- a/matchblox/engine/C_3DObject.h
+++ b/matchblox/engine/C_3DObject.h
@@ -37,7 +37,7 @@ public:
   inline const Vect3D_t& GetRot() { return m_Rot; }

   inline const Vect3D_t& GetScale() { return m_Scale; }

   inline const BoundingBox_t& GetBoundingBox() { return m_BBox; }

-  inline const BoundingBox_t& GetAbsBoundBox() { return m_BBox + m_Pos; } 

+  inline const BoundingBox_t GetAbsBoundBox() { return m_BBox + m_Pos; } 

   inline bool  Initialized() { return m_bDispListValid; }

 

   void TransRotateScale(); //applies the scaling, translation and rotation

diff --git a/matchblox/engine/C_Block.cpp b/matchblox/engine/C_Block.cpp
index 4c092a2..81d3b66 100644
--- a/matchblox/engine/C_Block.cpp
+++ b/matchblox/engine/C_Block.cpp
@@ -1,13 +1,14 @@
 #include <GL/glut.h>

 

 #include <iostream>

-

+#define _USE_MATH_DEFINES

+#include <math.h>

 #include "C_Block.h"

 

 #define FADE_DURATION 500

 

 /*Durations of animations*/

-unsigned int g_BlockAnimDurations[4] = {0, 500, 500, 500};

+unsigned int g_BlockAnimDurations[4] = {0, 500, 500, 100};

 

 C_Block::C_Block(const char* f_strFileName,

           GLuint f_uiColorTex ,

@@ -26,7 +27,7 @@ C_Block::~C_Block()
 void C_Block::Render(unsigned int f_iElapsedTime)

 {

   unsigned int l_uiDeltaTime = f_iElapsedTime - m_uiAnimStart;

-  double l_Scale = 0.0;

+  double l_Scale, l_Scalexz, l_Frac;

 

 

   //check if the previous animation has ended in between

@@ -56,11 +57,19 @@ void C_Block::Render(unsigned int f_iElapsedTime)
     C_3DObject::Render();

     break;

 

-  case BS_COLLIDE:

+  case BS_COLLIDE: //wobble

+    l_Frac = ((double)l_uiDeltaTime / (double)g_BlockAnimDurations[BS_COLLIDE]);

+    l_Frac *= (M_PI/2.0);

+    l_Scale = 0.8 + cos(l_Frac)*0.2;

+    l_Scalexz = 1.0 - sin(l_Frac)*0.2;

+    SetScale(l_Scalexz, l_Scale, l_Scalexz);

+    TransRotateScale();

+    C_3DObject::Render();

     break;

 

   case BS_IDLE:

   default:

+    SetScale(1.0, 1.0, 1.0);

     TransRotateScale();

     C_3DObject::Render();

     break;

diff --git a/matchblox/engine/C_Block.h b/matchblox/engine/C_Block.h
index f0e03b7..f42c2ba 100644
--- a/matchblox/engine/C_Block.h
+++ b/matchblox/engine/C_Block.h
@@ -22,7 +22,8 @@ public:
   ~C_Block();

 

   void Render(unsigned int f_iElapsedTime);

-  inline void SetState(BlockAnimState f_State, unsigned int f_uiElapsedTime) { m_CurrState = f_State; m_uiAnimStart = f_uiElapsedTime; }

+  inline void SetState(BlockAnimState f_State, unsigned int f_uiElapsedTime) 

+  { if (m_CurrState != f_State) {m_CurrState = f_State; m_uiAnimStart = f_uiElapsedTime; } }

 

 private:

   BlockAnimState m_CurrState;

diff --git a/matchblox/engine/C_Environment.cpp b/matchblox/engine/C_Environment.cpp
index 0c7cbfd..dc5656e 100644
--- a/matchblox/engine/C_Environment.cpp
+++ b/matchblox/engine/C_Environment.cpp
@@ -69,29 +69,34 @@ void C_Environment::Render()
   glDisable(GL_LIGHTING);
   //enable texturing
   glEnable(GL_TEXTURE_2D);
-  glTexEnvf(GL_TEXTURE_2D, GL_TEXTURE_ENV_MODE, GL_REPLACE);
     //neg x (left)
   glBindTexture(GL_TEXTURE_2D, m_uiFaceTex[0]);
+  glTexEnvf(GL_TEXTURE_2D, GL_TEXTURE_ENV_MODE, GL_REPLACE);
   RenderFace(l_Vertices, 3, 0, 4, 7); 
   
   //neg y (down)
   glBindTexture(GL_TEXTURE_2D, m_uiFaceTex[1]);
+  glTexEnvf(GL_TEXTURE_2D, GL_TEXTURE_ENV_MODE, GL_REPLACE);
   RenderFace(l_Vertices, 3, 2, 1, 0);
 
   //nez z (front)
   glBindTexture(GL_TEXTURE_2D, m_uiFaceTex[2]);
+  glTexEnvf(GL_TEXTURE_2D, GL_TEXTURE_ENV_MODE, GL_REPLACE);
   RenderFace(l_Vertices, 2, 3, 7, 6);
 
   //pos x (right)
   glBindTexture(GL_TEXTURE_2D, m_uiFaceTex[3]);
+  glTexEnvf(GL_TEXTURE_2D, GL_TEXTURE_ENV_MODE, GL_REPLACE);
   RenderFace(l_Vertices, 1, 2, 6, 5);
 
   //pos y (top)
   glBindTexture(GL_TEXTURE_2D, m_uiFaceTex[4]);
+  glTexEnvf(GL_TEXTURE_2D, GL_TEXTURE_ENV_MODE, GL_REPLACE);
   RenderFace(l_Vertices, 4, 5, 6, 7);
 
   //pos z (back)
   glBindTexture(GL_TEXTURE_2D, m_uiFaceTex[5]);
+  glTexEnvf(GL_TEXTURE_2D, GL_TEXTURE_ENV_MODE, GL_REPLACE);
   RenderFace(l_Vertices, 0, 1, 5, 4);
 
   glPopAttrib();
diff --git a/matchblox/engine/C_Hand.cpp b/matchblox/engine/C_Hand.cpp
index 8c4d977..796be88 100644
--- a/matchblox/engine/C_Hand.cpp
+++ b/matchblox/engine/C_Hand.cpp
@@ -24,8 +24,19 @@ void C_Hand::Render(unsigned int f_iElapsedTime)
     m_CurrState = HS_IDLE;

   }

 

+  MatProps_t l_m = m_Mat; //backup

+

+  //scale translate and rotate

+  glPushMatrix();

+  

+  TransRotateScale();

+

   switch (m_CurrState)

   {

+  case HS_COLLIDE:

+    m_Mat.setEmi(1.0, 0.0, 0.0, 1.0);

+    C_3DObject::Render();

+    break;

   case HS_GRAB:

     C_3DObject::Render();

     break;

@@ -40,4 +51,8 @@ void C_Hand::Render(unsigned int f_iElapsedTime)
     break;

   }

 

+  //restore original matprops

+  m_Mat = l_m;

+

+  glPopMatrix();

 }

diff --git a/matchblox/engine/C_Hand.h b/matchblox/engine/C_Hand.h
index 3ada63d..09fd716 100644
--- a/matchblox/engine/C_Hand.h
+++ b/matchblox/engine/C_Hand.h
@@ -22,7 +22,8 @@ public:
   ~C_Hand();

 

   void Render(unsigned int f_iElapsedTime);

-  inline void SetState(HandAnimState f_State, unsigned int f_uiElapsedTime) { m_CurrState = f_State; m_uiAnimStart = f_uiElapsedTime; }

+  inline void SetState(HandAnimState f_State, unsigned int f_uiElapsedTime) 

+  { if (m_CurrState != f_State) { m_CurrState = f_State; m_uiAnimStart = f_uiElapsedTime; } }

 

 private:

   HandAnimState m_CurrState;

diff --git a/matchblox/engine/C_MatchBloxEngine.cpp b/matchblox/engine/C_MatchBloxEngine.cpp
index 93a178f..7dca60d 100644
--- a/matchblox/engine/C_MatchBloxEngine.cpp
+++ b/matchblox/engine/C_MatchBloxEngine.cpp
@@ -7,9 +7,6 @@
 #define _USE_MATH_DEFINES

 #include <math.h>

 

-#include "C_MatchBloxEngine.h"

-#include "message_input.h"

-

 #include "C_3DObject.h"

 #include "C_Environment.h"

 #include "C_Hand.h"

@@ -19,7 +16,15 @@
 #include "bitmap.h"

 #include "message_queue.h"

 #include "message_input.h"

+#include "wiimote_utils.h"

+#include "C_Smoother.h"

+#include "C_MatchBloxEngine.h"

+#include "font.h"

 

+extern "C" {

+extern GLuint g_iBase;

+extern struct BitmapStruct g_sFont;

+}

 

 C_MatchBloxEngine::C_MatchBloxEngine(const char *f_strModelPath,

                                      const char *f_strLogFile,

@@ -50,8 +55,12 @@ C_MatchBloxEngine::C_MatchBloxEngine(const char *f_strModelPath,
   m_WorldBox.m_Min = Vect3D_t(-15.0, -5.0, -15.0);

   m_WorldBox.m_Max = Vect3D_t(15.0, 15.0, 15.0);

 

-  //m_DotHist.clear();

-  //m_DotHistSize = 15;

+  //set smoothers

+  m_pWorldZSmoother[0] = new C_Smoother<double>(0.0);

+  m_pWorldZSmoother[1] = new C_Smoother<double>(0.0);

+

+  m_pWorldZSmoother[0]->SetSimpleMovingAverage(25); //world z coordinate smoother probably lots of lag (but only in z dir)

+  m_pWorldZSmoother[1]->SetExponentialMovingAverage(0.8);

 }

 

 C_MatchBloxEngine::~C_MatchBloxEngine()

@@ -60,13 +69,21 @@ C_MatchBloxEngine::~C_MatchBloxEngine()
   

   //delete models

   DeleteModels();

+

+  delete m_pWorldZSmoother[0];

+  delete m_pWorldZSmoother[1];

 }

 

 GameResult C_MatchBloxEngine::ProcessMsgs(void)

 {

   struct messageq_s *message;

-  

-  while (message = messageq_get(MESSAGE_RENDERER))

+  GameResult l_Result = GR_BUSY; //default return value

+

+  //init current time

+  m_uiCurrentTime = glutGet(GLUT_ELAPSED_TIME);

+

+  while ((message = messageq_get(MESSAGE_RENDERER)) &&

+         l_Result == GR_BUSY)

   {

     switch(message->sender)

     {

@@ -75,70 +92,69 @@ GameResult C_MatchBloxEngine::ProcessMsgs(void)
     case MESSAGE_INPUT_MOUSE:

       break;

     case MESSAGE_INPUT_WIIMOTE:

-      //get message payload

-      //??how to interpret the message payload?? 

-      //wiimote ir dots have a range of 1024x768 in xy which can easily be mapped to world xycoords

-      //for the z coordinate: we know the size of the sensorbar, the reach of a human arm is <1m so 

-      //if we map the initial z distance (in mm) -iz- (taken in ES_GET_READY) to be 0 and -z to be iz - 250mm

-      //and +z to be iz + 250 mm we have 3d input!!

-      //process button presses

-

-      //process ir data

       input_payload_wiimote *l_pMsg;

       l_pMsg = (input_payload_wiimote*)message->payload;

-

-      //for(int i=0; i<4; i++)

+      switch (m_State)

       {

-        if (l_pMsg->btns && WIIMOTE_BUTTON_A)

+      case ES_GET_READY:

+        if (l_pMsg->btns & WIIMOTE_BUTTON_A &&

+          l_pMsg->btns & WIIMOTE_BUTTON_B &&

+          l_pMsg->posDataValid)

         {

-          //init depth

-          Vect3D_t l_relPos;

-          if (CalcWiimoteRelativeCursorPos(l_pMsg, &l_relPos))

-          {

-            m_dInitialWiimoteDist = l_relPos.z;

-          }

-          else

+          //the 3d mouse position data is valid and A+B are pressed

+          //init the z depth and start the game

+          m_dInitialWiimoteDist = l_pMsg->Zdist;

+          if (!StartGame())

           {

-            m_dInitialWiimoteDist = 500.0;

+            l_Result = GR_ERROR;

           }

         }

-        else

+        break;

+

+      case ES_FINISHED:

+        if (l_pMsg->btns & WIIMOTE_BUTTON_ALL)

         {

+          //any button is pressed

+          //delete session and return GAME_FINISHED

+          delete m_pCurrentSession;

+          l_Result = GR_FINISHED;

+        }

+        break;

+

+      default:

+        if (l_pMsg->posDataValid)

+        {

+          m_uiLastPosValid = m_uiCurrentTime;

           Vect3D_t l_WorldPos;

+          if (l_pMsg->btns & WIIMOTE_BUTTON_A)

+          {

+            int i = 0; //debug break

+          }

+

           if (ConvertWiimoteToWorld(l_pMsg, &l_WorldPos))

           {

-            m_pBlock[0]->SetPos(l_WorldPos);

+            CursorMove(l_WorldPos);

           }

         }

-        

-          //std::cout << "Dot["<<i<<"] ("<< l_pMsg->Dot[i].RawX <<","<< l_pMsg->Dot[i].RawY <<")\n";

+        break;

       }

-

-      //if (m_State == ES_GET_READY)

-      //{

-      //  //if player has pressed the A button

-      //  //take the initial z distance

-

-      //  //perhaps we should do a countdown 3.. 2.. 1.. GO! but this requires another engine state

-      //}

-      //if (m_State == ES_PLAYING_GRAB_BLOCK || m_State == ES_PLAYING_PUT_BLOCK)

-      //{

-      //  //convert ir data to cursor pos in world coordinates

-

-      //  Vect3D_t tmpPos(l_pMsg->x, l_pMsg->y, l_pMsg->z);

-      //  CursorMove(tmpPos);

-      //}

+    

       break;

     default:

       std::cout << "Undefined message! Sender: " << message->sender << std::endl;

+      //l_Result = GR_ERROR;

     }

   }

 

-  return GR_BUSY;

+  return l_Result;

 }

 

-void C_MatchBloxEngine::Render_Basics(unsigned int f_uiElapsedTime)

+

+void C_MatchBloxEngine::Render(unsigned int f_uiElapsedTime)

 {

+  //glMatrixMode(GL_MODELVIEW);

+  //glLoadIdentity();

+

   glPushMatrix();

 

   //set camera pitch

@@ -151,69 +167,129 @@ void C_MatchBloxEngine::Render_Basics(unsigned int f_uiElapsedTime)
 

   glPushMatrix();

   

-

   //move the camera backwards

   glTranslated(0.0, 0.0, -16.0);

   glRotated(20.0, 1.0, 0.0, 0.0);

-  

 

-  

-

-  //if (m_pCurrentSession)

-  //{

-    m_pBox[(int)m_CurrentBox]->Render();

-    m_pBlock[0]->Render(f_uiElapsedTime);

-   /* for (int i=0; i<4; i++)

-      m_pBlock[i]->Render(f_uiElapsedTime);*/

-  //  m_pHand->Render(f_uiElapsedTime);

-  //}

-

-  glPopMatrix();

-}

+  m_pBox[(int)m_CurrentBox]->Render();

 

-void C_MatchBloxEngine::Render(unsigned int f_uiElapsedTime)

-{

   switch (m_State)

   {

   case ES_INITIALISED:

-    Render_Basics(f_uiElapsedTime);

-  break;

-

   case ES_ERROR:

-    //render a red cube

-    glPushMatrix();

-    glPushAttrib(GL_ENABLE_BIT);

-    glDisable(GL_LIGHTING);

-    glColor3d(1.0, 0.0, 0.0);

-    glTranslated(0.0, 0.0, -5.0);

-    glutSolidCube(5.0);

-    glPopAttrib();

-    glPopMatrix();

+    m_pHand->Render(f_uiElapsedTime);

   break;

 

   case ES_GET_READY:

-    Render_Basics(f_uiElapsedTime);

     //render some GET READY text

+    RenderGetReady();

   break;

 

   case ES_PLAYING_GRAB_BLOCK:

-    Render_Basics(f_uiElapsedTime);

-  break;

-

   case ES_PLAYING_PUT_BLOCK:

-    Render_Basics(f_uiElapsedTime);

+    m_pBlock[(int)m_CurrentBlock]->Render(f_uiElapsedTime);

+    m_pHand->Render(f_uiElapsedTime);

+

+    //render timer and block count

+    RenderHUD(f_uiElapsedTime);

   break;

 

   case ES_PAUSED:

-    Render_Basics(f_uiElapsedTime);

-    //render menu??

   break;

 

   case ES_FINISHED:

+    RenderResults();

     //render results...

   break;

   }

 

+  if (f_uiElapsedTime - m_uiLastPosValid > 5000)

+  {

+    RenderPointHere();

+  }

+

+  glPopMatrix();

+

+}

+

+void C_MatchBloxEngine::RenderGetReady()

+{

+  ColorStruct l_TextCol = {0.0, 0.3, 1.0};

+  int w = glutGet(GLUT_WINDOW_WIDTH),

+      h = glutGet(GLUT_WINDOW_HEIGHT);

+  

+  RenderText((w/2)-(9*32), (h/2)-16, "Press A+B to begin.", l_TextCol);

+}

+

+void C_MatchBloxEngine::RenderHUD(unsigned int f_uiElapsedTime)

+{

+  ColorStruct l_TextCol = {0.0, 0.3, 1.0};

+  std::stringstream l_ss;

+  unsigned l_uiTime = f_uiElapsedTime - m_pCurrentSession->m_uiSessionStart;

+

+  int l_iMSecs = l_uiTime % 1000;

+  l_uiTime /= 1000;

+  int l_iSecs = l_uiTime % 60;

+  l_uiTime /= 60;

+  int l_iMins = l_uiTime % 60;

+

+  l_ss << "Block " << m_pCurrentSession->m_iCurrentTurn << "/" << m_pCurrentSession->m_iTotalTurns

+   << "      Time: " << setw(2) << setfill('0') << l_iMins << ":" << setw(2) << l_iSecs 

+   << "." << l_iMSecs;

+

+  RenderText(32, 16, (char*)l_ss.str().c_str(), l_TextCol);

+}

+

+void C_MatchBloxEngine::RenderResults()

+{

+  ColorStruct l_TextCol = {0.0, 0.3, 1.0};

+  RenderText(32, 64, "YOU SUCK!", l_TextCol);

+}

+

+void C_MatchBloxEngine::RenderPointHere()

+{

+  ColorStruct l_TextCol = {0.0, 0.3, 1.0};

+  int w = glutGet(GLUT_WINDOW_WIDTH),

+      h = glutGet(GLUT_WINDOW_HEIGHT);

+  

+  RenderText((w/2)-(6*32), (h/4), "Point >o< here!", l_TextCol);

+}

+

+void C_MatchBloxEngine::RenderText(GLint x, GLint y, char *string, struct ColorStruct f_sColor)

+{

+  glPushAttrib(GL_ENABLE_BIT | GL_TEXTURE_BIT | GL_CURRENT_BIT | GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
+  
+  glDisable(GL_DEPTH_TEST);
+  
+  glEnable(GL_TEXTURE_2D);
+  glBindTexture(GL_TEXTURE_2D, g_sFont.m_iImageId);
+  glTexEnvf(GL_TEXTURE_2D, GL_TEXTURE_ENV_MODE, GL_MODULATE);
+
+  glColor3d(f_sColor.m_dRed, f_sColor.m_dGreen, f_sColor.m_dBlue);
+
+  glMatrixMode(GL_MODELVIEW);
+  glPushMatrix(); //push modelview matrix
+
+  glMatrixMode(GL_PROJECTION);
+  glPushMatrix(); //push projection matrix
+
+    glLoadIdentity();
+    glOrtho(0, glutGet(GLUT_WINDOW_WIDTH), glutGet(GLUT_WINDOW_HEIGHT), 0, 0, 1);
+
+    glEnable(GL_BLEND);
+    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
+
+    glTranslated(x, y, 0);

+	  glListBase(g_iBase - 32);

+	  glCallLists((GLsizei)strlen(string), GL_UNSIGNED_BYTE, string);
+    
+  glMatrixMode(GL_PROJECTION);
+  glPopMatrix(); //pop projection matrix
+
+  glMatrixMode(GL_MODELVIEW);
+  glPopMatrix(); //pop modelview matrix
+
+  glPopAttrib();

 }

 

 bool C_MatchBloxEngine::NewGame(int f_iUserID, int f_iGameId, BoxSize f_BS)

@@ -241,13 +317,8 @@ bool C_MatchBloxEngine::StartGame()
 {

   if (m_State == ES_GET_READY)

   {

-    //start the session timer

-    m_CurrentBlock = m_pCurrentSession->StartSession();

-

-    //move a block into the sky

-    m_pBlock[(int)m_CurrentBlock ]->SetPos(0.0, 0.0, 15.0);

-

-    m_State = ES_PLAYING_GRAB_BLOCK;

+    //start the game session

+     NewBlock();

 

     return true;

   }

@@ -353,7 +424,9 @@ bool C_MatchBloxEngine::LoadModels(const char* f_strModelDir)
 

   

   //load the hand???

-  

+  l_Mat.setAmb(1.0, 1.0, 1.0, 1.0);

+  l_Mat.setDif(1.0, 1.0, 1.0, 1.0);

+  m_pHand = new C_Hand("", 0, l_Mat);

   

   //Load the box tiles

   l_Mat.setAmb(1.0, 1.0, 1.0, 1.0);

@@ -437,245 +510,227 @@ void C_MatchBloxEngine::LoadTexture(const char* f_BmpName, GLuint &f_uiTexHandle
   glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_REPEAT);

 }

 

+

 void C_MatchBloxEngine::CursorMove(Vect3D_t &f_NewCursorPos)

 {

   //check the state to see what 3d object currently has to be considered to 

   //be the cursor currently is and which bounding box we need to check for overlap

-  BoundingBox_t l_CursBBox;

-  bool          l_bCollision = false; //indicates a collission has happend

+  bool l_bCollision = false; //indicates a collission has happend

 

   switch (m_State)

   {

   case ES_PLAYING_PUT_BLOCK:

     //cursor is the current block figure (that is being held by the player)

-    //translate the bounding box of the block with the cursor position 

-    l_CursBBox = m_pBlock[m_CurrentBlock]->GetBoundingBox() + f_NewCursorPos;

+    //translate the bounding box of the block with the cursor position

+    l_bCollision = CursorMove_PutBlock(f_NewCursorPos);

     break;

 

   default:

     //cursor is the hand object

-    l_CursBBox = m_pHand->GetBoundingBox() + f_NewCursorPos;

+    l_bCollision = CursorMove_GrabBlock(f_NewCursorPos);

     break;

   }

 

-  //now we have the bounding box of the cursor which we use to do some simple hit tests

-  

-  //make sure the cursor is still completely contained in the world bounding box

-  if (!m_WorldBox.Contains(l_CursBBox))

+  if (l_bCollision)

   {

-    //restore the previous cursor position, because the new one is invalid

-    //... nothing to do actually

-    //maybe generate some vibration event and collision animation???

-    l_bCollision = true;

+    //send a rumble message and set collision animation state

+    //message_send()

   }

-  else

+}

+

+bool C_MatchBloxEngine::CursorMove_GrabBlock(Vect3D_t &f_NewCursorPos)

+{

+  BoundingBox_t l_HandBB = m_pHand->GetBoundingBox() + f_NewCursorPos;

+

+  if (m_WorldBox.Contains(l_HandBB))

   {

-    //check for bounding box overlap with the static elements in the scene 

-    //actually only the block box

-    if (m_pBox[m_CurrentBox]->GetAbsBoundBox().Overlap(l_CursBBox))

+    if (m_State == ES_PLAYING_GRAB_BLOCK)

     {

-      //overlap with the block box, a collision is very likely so set 

-      //collision to true and reset to false when the player is putting

-      //the block in the right hole (if the player is holding a block

-      //that is, if the player isn't then there is certainly a collision)

-      l_bCollision = true;

-      

-      //check if we are holding a block that has to be put in the box

-      if (m_State == ES_PLAYING_PUT_BLOCK)

+      //check for overlap with the current block

+      if (m_pBlock[(int)m_CurrentBlock]->GetAbsBoundBox().Overlap(l_HandBB))

       {

-        

+        //grabbing the block

+        m_State = ES_PLAYING_PUT_BLOCK;

 

-      }

-      else

-      {

-        //we are not holding a block so every contact with the 

-        //block box is a collision

-        l_bCollision = true;

+        m_pHand->SetState(HS_GRAB, m_uiCurrentTime);

+

+        m_pBlock[(int)m_CurrentBlock]->SetPos(f_NewCursorPos);

       }

     }

-  }

+    

+    //set the new position

+    m_pHand->SetPos(f_NewCursorPos);

 

+    return false;

+  }

+  else

+  {

+    //the cursor is leaving the world box, which is a collision

+    m_pHand->SetState(HS_COLLIDE, m_uiCurrentTime);

+    return true; //true == collision

+  }

 }

 

-//bool C_MatchBloxEngine::CursorMove_PutBlock(Vect3D_t &f_CursPos, BoundingBox_t &f_CursBBox)

-//{

-//  //check if the block is being put in the right hole

-//  //by testing whether the position of the cursor is close 

-//  //enough to the center of the correct hole

-//  //note that the hole positions are relative to the position

-//  //of the box

-//  bool     l_bCollision = false;

-//  Vect3D_t l_AbsHolePos = m_pBox[m_CurrentBox]->GetPos() + 

-//                          m_pTiles[m_CurrentBlock]->GetPos();

-//  Vect3D_t l_PosDif = l_AbsHolePos - f_CursPos;

-//  double   l_dXZProximity = l_PosDif.x * l_PosDif.x + 

-//                            l_PosDif.y * l_PosDif.y;

-//

-//  if (l_dXZProximity < m_GameSettings.m_dMinProximity)

-//  {

-//    //the block the player is holding is considered to be in the right hole

-//    //l_bCollision = false;

-//   

-//    //?? would it not be better to check for bounding box intersection with hole?

-//    l_bCollision = !(m_pTiles[m_CurrentBlock]->GetAbsBoundBox() + 

-//                     m_pBox[m_CurrentBox]->GetPos()).Overlap(f_CursBBox);

-//

-//    //now check if the block is far enough in the hole to count as a point

-//    if (l_PosDif.z > m_GameSettings.m_dHoleDepth)

-//    {

-//      //yipee!! we have got a winner!!

-//      m_CurrentBlock = m_pCurrentSession->NewTurn();

-//

-//      //m_GameState

-//    }

-//

-//

-//   

-//  }

-//

-//  return false;

-//}

-

-//bool C_MatchBloxEngine::CursorMove_GrabBlock(Vect3D_t &f_CursPos, BoundingBox_t &f_CursBBox)

-//{

-//  //grabbing a block: just check for bounding box intersection

-//  if (m_pBlock[(int)m_CurrentBlock]->GetAbsBoundBox().Overlap(f_CursBBox))

-//  {

-//    

-//  } 

-//  return true;

-//}

-

-

-bool C_MatchBloxEngine::FindIRDots(input_payload_wiimote *f_pWiimote, ir_dot_t f_Dot[2])

+bool C_MatchBloxEngine::CursorMove_PutBlock(Vect3D_t &f_NewCursorPos)

 {

-  //find the pair of ir dots with the largest squared distance

-  int mdist = 0, //max length

-      dist2,dx,dy,

-      dot0, dot1;

-  

-  //loop through all combinations

-  for(int i = 0; i < 4; i++)

+  bool l_bCollision;

+  BoundingBox_t l_BlockBB = m_pBlock[(int)m_CurrentBlock]->GetBoundingBox() + f_NewCursorPos,

+    l_TileBB = m_pTiles[(int)m_CurrentBlock]->GetAbsBoundBox() + m_pBox[(int)m_CurrentBox]->GetPos();

+

+  if (m_WorldBox.Contains(l_BlockBB))

   {

-    for (int j = i+1; j < 4; j++)

+    //check for overlap with the block box

+    if (m_pBox[(int)m_CurrentBox]->GetAbsBoundBox().Overlap(l_BlockBB))

     {

-      //check if the ir dots are found

-      if (f_pWiimote->ir.dot[i].visible && 

-          f_pWiimote->ir.dot[j].visible)

+      //check for overlap with the target hole tile

+      if (l_TileBB.Overlap(l_BlockBB))

       {

-        //compute the squared distance

-        dx = f_pWiimote->ir.dot[i].rx - f_pWiimote->ir.dot[j].rx;

-        dy = f_pWiimote->ir.dot[i].ry - f_pWiimote->ir.dot[j].ry;

-        dist2 = dx*dx + dy*dy;

-

-        if (dist2 > mdist)

+        Vect3D_t l_AbsHolePos = m_pBox[m_CurrentBox]->GetPos() + 

+                          m_pTiles[m_CurrentBlock]->GetPos();

+        Vect3D_t l_PosDif = l_AbsHolePos - m_pBlock[(int)m_CurrentBlock]->GetPos();

+        double   l_dXZProximity = l_PosDif.x * l_PosDif.x + 

+                                  l_PosDif.y * l_PosDif.y;

+        cout << "Block proximety: " << l_dXZProximity << endl;

+

+        if (l_dXZProximity < m_GameSettings.m_dMinProximity)

+        {

+          //we have a winner

+          l_bCollision = false;

+          NewBlock();

+        }

+        else

         {

-          mdist = dist2;

-          dot0 = i;

-          dot1 = j;

-          //std::cout << "(" << i << "," << j <<") " << mdist << std::endl;

+          l_bCollision = true;

         }

       }

-    }

-  }

-

-  if (mdist > 0)

-  {

-    std::cout << "Winner - (" << dot0 << "," << dot1 <<") " << mdist << " numdots: " << (int)f_pWiimote->ir.num_dots << std::endl;

-    //left down is f_Dot[0]

-    if (f_pWiimote->ir.dot[dot0].rx < f_pWiimote->ir.dot[dot1].ry ||

-        (f_pWiimote->ir.dot[dot0].rx == f_pWiimote->ir.dot[dot1].ry &&

-         f_pWiimote->ir.dot[dot0].rx < f_pWiimote->ir.dot[dot1].ry))

-    {

-      f_Dot[0] = f_pWiimote->ir.dot[dot0];

-      f_Dot[1] = f_pWiimote->ir.dot[dot1];

+      else

+      {

+        //overlap with the block box but not with the correct tile

+        l_bCollision = true;

+      }

     }

     else

     {

-      f_Dot[0] = f_pWiimote->ir.dot[dot1];

-      f_Dot[1] = f_pWiimote->ir.dot[dot0];

+      //inside the world box and not overlapping the block box

+      l_bCollision = false;

     }

-    

-    return true;

   }

   else

   {

-    //std::cout << "nothing...\n";

-    return false;

+    //leaving the world box

+    l_bCollision = true;

   }

-}

 

+  if (l_bCollision)

+  {

+    m_pBlock[(int)m_CurrentBlock]->SetState(BS_COLLIDE, m_uiCurrentTime);

+  }

+  else

+  {

+    if (m_State == ES_PLAYING_PUT_BLOCK)

+    {

+      m_pBlock[(int)m_CurrentBlock]->SetPos(f_NewCursorPos);

+    }

+  }

 

-bool C_MatchBloxEngine::CalcWiimoteRelativeCursorPos(input_payload_wiimote *f_pWiimote, Vect3D_t *f_pRelPos)

-{

-  //get two ir dots

-  ir_dot_t l_Dot[2];

+  //allways set mouse pointer position

+  m_pHand->SetPos(f_NewCursorPos);

 

-  if (!FindIRDots(f_pWiimote, l_Dot))

-    return false;

+  return l_bCollision;

+}

+

+bool C_MatchBloxEngine::Check_PutBlock(Vect3D_t &f_CursPos, BoundingBox_t &f_CursBBox)

+{

+  //check if the block is being put in the right hole

+  //by testing whether the position of the cursor is close 

+  //enough to the center of the correct hole

+  //note that the hole positions are relative to the position

+  //of the box

+  Vect3D_t l_AbsHolePos = m_pBox[m_CurrentBox]->GetPos() + 

+                          m_pTiles[m_CurrentBlock]->GetPos();

+  Vect3D_t l_PosDif = l_AbsHolePos - f_CursPos;

+  double   l_dXZProximity = l_PosDif.x * l_PosDif.x + 

+                            l_PosDif.y * l_PosDif.y;

+  BoundingBox_t l_HoleBBox = m_pTiles[m_CurrentBlock]->GetAbsBoundBox() + m_pBox[m_CurrentBox]->GetPos();

+

+  //first check if the block is being put in the right tile with

+  //the right xz-proximity

+  if (l_HoleBBox.Overlap(f_CursBBox) && 

+      l_dXZProximity < m_GameSettings.m_dMinProximity)

+  {

+    //check if the block is deep enough inside the box to count as a point

+    if (l_PosDif.z > m_GameSettings.m_dHoleDepth)

+    {

+      //yipee!! we have got a winner!!

+      if (m_pCurrentSession->NextTurn(m_CurrentBlock))

+      {

+        NewBlock();

+      }

+      else

+      {

+        GameFinished();

+      }

+      return true;

+    }

 

-  //dotpair l_dp(l_Dot[0], l_Dot[1]);

-  //m_DotHist.push_front(l_dp);

-  //if (m_DotHist.size() > m_DotHistSize)

-  //  m_DotHist.pop_back();

-

-  ////calculate the average dots

-  //l_Dot[0].RawX = l_Dot[0].RawY = l_Dot[1].RawX = l_Dot[1].RawY = 0;

-  //for(list<dotpair>::iterator it = m_DotHist.begin(); it != m_DotHist.end(); it++)

-  //{

-  //  l_Dot[0].RawX += it->first.RawX; l_Dot[0].RawY += it->first.RawY;

-  //  l_Dot[1].RawX += it->second.RawX; l_Dot[1].RawY += it->second.RawY;

-  //}

-  //l_Dot[0].RawX /= (double)m_DotHist.size(); l_Dot[0].RawY /= (double)m_DotHist.size();

-  //l_Dot[1].RawX /= (double)m_DotHist.size(); l_Dot[1].RawY /= (double)m_DotHist.size();

-

-

-  //invert the x and y axis

-  l_Dot[0].rx = 1016 - l_Dot[0].ry;

-  l_Dot[1].rx = 1016 - l_Dot[1].ry;

-  l_Dot[0].rx =  760 - l_Dot[0].ry;

-  l_Dot[1].rx =  760 - l_Dot[1].ry;

-

-  double  l_dX = (double)l_Dot[0].rx - l_Dot[1].rx,            //difference in x coordinates

-          l_dY = (double)l_Dot[0].ry - l_Dot[1].ry,            //difference in y coordinates

-          l_dDotDist = sqrt(l_dX*l_dX + l_dY*l_dY),                //distance between ir dots (in camera pixels)

-          l_dRadPerPixel = ( 41.0 * (M_PI/180.0) ) /1016.0,        //radians per camera pixel: x view angel = 41 deg, 1016 pixels

-          l_dDotAngle = l_dRadPerPixel * l_dDotDist;               //the angle between the lines from the camera through the two 

-                                                                   //ir dots (in radians)

-

-  f_pRelPos->x = (double)(l_Dot[0].rx + l_Dot[1].rx)/2.0; //camera x coordinate [0,1016]

-  f_pRelPos->y = (double)(l_Dot[0].ry + l_Dot[1].ry)/2.0; //camera y coordinate [0,760]

-  f_pRelPos->z = (0.5 * 205.0) / tan(0.5 * l_dDotAngle); //the distance between the sensorbar and wiimote (in mm)

-  //std::cout << "(" << f_pRelPos->x << "," << f_pRelPos->y << "," << f_pRelPos->z << ")\n";

+    //not a win but also no collison -> set the position

+    m_pBlock[(int)m_CurrentBlock]->SetPos(f_CursPos);

+    return true; 

+  }

   

-  return true;

+  return false;

 }

 

-bool C_MatchBloxEngine::ConvertWiimoteToWorld(input_payload_wiimote *f_pWiimote, Vect3D_t *f_pWorldPos)

-{

-  Vect3D_t l_RelPos;

 

-  if (!CalcWiimoteRelativeCursorPos(f_pWiimote, &l_RelPos))

+bool C_MatchBloxEngine::ConvertWiimoteToWorld(input_payload_wiimote *f_pWiimoteMsg, Vect3D_t *f_pWorldPos)

+{

+  if (!f_pWiimoteMsg->posDataValid)

+  {

     return false;

+  }

+

+  Vect3D_t l_RelPos(f_pWiimoteMsg->relX, f_pWiimoteMsg->relY, f_pWiimoteMsg->Zdist);

 

   //use the world bounding box dimensions to convert the relative position to world coordinates

 

-  //z is in mm, cap it to 250mm from the initial wiimote distance

+  //z is in mm, cap it to 500mm from the initial wiimote distance

   //using the initial distance

-  l_RelPos.z = l_RelPos.z - m_dInitialWiimoteDist + 250;

+  l_RelPos.z = (l_RelPos.z - m_dInitialWiimoteDist + 500.0) / 1000.0;

   if (l_RelPos.z < 0.0) l_RelPos.z = 0.0;

-  else if (l_RelPos.z > 500.0) l_RelPos.z = 500.0;

+  else if (l_RelPos.z > 1.0) l_RelPos.z = 1.0;

 

-  //Vect3D_t l_WorldSize(m_WorldBox.m_Max - m_WorldBox.m_Min);

-  Vect3D_t l_WorldSize;

-  l_WorldSize.x = m_WorldBox.m_Max.x - m_WorldBox.m_Min.x;

-  l_WorldSize.y = m_WorldBox.m_Max.y - m_WorldBox.m_Min.y;

-  l_WorldSize.z = m_WorldBox.m_Max.z - m_WorldBox.m_Min.z;

+  Vect3D_t l_WorldSize  = m_WorldBox.m_Max - m_WorldBox.m_Min;

+  

+  *f_pWorldPos = m_WorldBox.m_Min + (l_RelPos * l_WorldSize);

 

-  f_pWorldPos->x = m_WorldBox.m_Min.x + ((l_RelPos.x * l_WorldSize.x)/1016.0); //1016 pixels in x

-  f_pWorldPos->y = m_WorldBox.m_Min.y + ((l_RelPos.y * l_WorldSize.y)/ 760.0); // 769 pixels in y

-  f_pWorldPos->z = m_WorldBox.m_Min.z + ((l_RelPos.z * l_WorldSize.z)/ 500.0);//  500 mm in z

+  //cout <<  "Z_world: " << std::fixed << std::setprecision(2) << setw(10) << left << f_pWorldPos->z;

 

-  //std::cout << "(" << f_pWorldPos->x << "," << f_pWorldPos->y << "," << f_pWorldPos->z << ")\n";

+  //smooth the world z coordinate in two passes (first average the last 25 samples)

+  //and then smooth exponentially over the averages

+  f_pWorldPos->z = m_pWorldZSmoother[0]->Smooth(f_pWorldPos->z);

+  f_pWorldPos->z = m_pWorldZSmoother[1]->Smooth(f_pWorldPos->z);

+  //cout <<  "Z_world_smoof: " << std::fixed << std::setprecision(2) << left << f_pWorldPos->z << endl;

   return true;

+}

+

+void C_MatchBloxEngine::NewBlock()

+{

+  if (m_pCurrentSession->NextTurn(m_CurrentBlock))

+  {

+    //move a block into the sky

+    m_pBlock[(int)m_CurrentBlock ]->SetPos(0.0, 10.0, 0.0);

+

+    m_State = ES_PLAYING_GRAB_BLOCK;

+  }

+  else

+  {

+    m_State = ES_FINISHED;

+  }

+}

+

+void C_MatchBloxEngine::GameFinished()

+{

+ 

+

+  //save session results to database

 }
\ No newline at end of file
diff --git a/matchblox/engine/C_MatchBloxEngine.h b/matchblox/engine/C_MatchBloxEngine.h
index 9b5bc98..2e38526 100644
--- a/matchblox/engine/C_MatchBloxEngine.h
+++ b/matchblox/engine/C_MatchBloxEngine.h
@@ -6,6 +6,7 @@
 #include "message_input.h"

 #include "typedefs.h"

 #include <list>

+#include "C_Smoother.h"

 //#include <pair>

 

 using namespace std;

@@ -17,6 +18,7 @@ class C_Log;
 class C_Environment;

 class C_Box;

 

+

 typedef enum GameResult

 {

   GR_FINISHED,

@@ -71,11 +73,9 @@ struct GameSession
                 *m_puiTurnResult;

   int           m_iBlocksLeft[4]; //number of blocks per type

   BlockType     m_CurrentBlockType;

-  BoxSize       m_BoxSize;

   

   GameSession(int f_iNrOfBlocks, BoxSize f_BS) 

-    : m_iTotalTurns(f_iNrOfBlocks), m_iCurrentTurn(0),

-      m_BoxSize(f_BS)

+    : m_iTotalTurns(f_iNrOfBlocks), m_iCurrentTurn(0)

   {

     m_puiTurnResult = new unsigned int[f_iNrOfBlocks];

 

@@ -95,22 +95,36 @@ struct GameSession
   {

     delete [] m_puiTurnResult;

   }

-  BlockType StartSession()

-  {

-    m_uiSessionStart = m_uiTurnStart = glutGet(GLUT_ELAPSED_TIME);

-    return GetRandomBlockType();

-  }

-  BlockType NewTurn()

+  

+  bool NextTurn(BlockType &f_NewBlock)

   {

     unsigned int l_uiTime = glutGet(GLUT_ELAPSED_TIME);

 

-    //save turn result

-    m_puiTurnResult[m_iCurrentTurn] = l_uiTime - m_uiTurnStart;

-    m_iCurrentTurn++;

-    m_uiTurnStart = l_uiTime;

+    if (m_iCurrentTurn == 0)

+    {

+      //session start

+      m_uiSessionStart = m_uiTurnStart = l_uiTime;

+      m_iCurrentTurn++;

+    }

+    else

+    {

+      //next turn -> save turn result

+      m_puiTurnResult[m_iCurrentTurn] = l_uiTime - m_uiTurnStart;

+      m_iCurrentTurn++;

+      m_uiTurnStart = l_uiTime;

+    }

 

-    return GetRandomBlockType();

+    if (m_iCurrentTurn < m_iTotalTurns)

+    {

+      f_NewBlock = GetRandomBlockType();

+      return true;

+    }

+    else 

+    {

+      return false;

+    }

   }

+

   BlockType GetRandomBlockType()

   {

     int l_iBlockType = rand()%4;

@@ -168,11 +182,9 @@ private:
   GameSettings m_GameSettings;

   BoxSize      m_CurrentBox;

   BlockType    m_CurrentBlock;

-

-  //typedef         pair<input_payload_wiimote::dot, input_payload_wiimote::dot> dotpair;

-  //list<dotpair>   m_DotHist;

-  int             m_DotHistSize;

-

+  unsigned int m_uiCurrentTime,

+               m_uiLastPosValid;

+  C_Smoother<double>   *m_pWorldZSmoother[2];  //2 to smooth the computed world z coordinate

   double        m_dInitialWiimoteDist; //initial distance of wiimote in mm, this distance is

                                        //mapped to the z=0.0 in world coordinates

   BoundingBox_t m_WorldBox; //an invisible box that limits the movement of the 

@@ -185,16 +197,24 @@ private:
   void LoadTexture(const char* f_BmpName, GLuint &f_uiTexHandle);

 

   //render routines

-  void Render_Basics(unsigned int f_uiElapsedTime);

+  void RenderGetReady();

+  void RenderHUD(unsigned int f_uiElapsedTime);

+  void RenderResults();

+  void RenderPointHere();

+  void RenderText(GLint x, GLint y, char *string, struct ColorStruct f_sColor);

 

   //event/input handlers

   void CursorMove(Vect3D_t &f_NewCursorPos);

-  //bool CursorMove_PutBlock(Vect3D_t &f_CursPos, BoundingBox_t &f_CusrBBox);

-  //bool CursorMove_GrabBlock(Vect3D_t &f_CursPos, BoundingBox_t &f_CusrBBox);

+  bool CursorMove_GrabBlock(Vect3D_t &f_NewCursorPos);

+  bool CursorMove_PutBlock(Vect3D_t &f_NewCursorPos);

+  bool Check_PutBlock(Vect3D_t &f_CursPos, BoundingBox_t &f_CusrBBox);

+  //bool Check_GrabBlock(Vect3D_t &f_CursPos, BoundingBox_t &f_CusrBBox);

+  void NewBlock();

+  void GameFinished();

 

   //wiimote functions

-  bool FindIRDots(input_payload_wiimote *f_pWiimote, ir_dot_t f_Dot[2]);

-  bool CalcWiimoteRelativeCursorPos(input_payload_wiimote *f_pWiimote, Vect3D_t *f_pRelPos);

+  //bool FindIRDots(input_payload_wiimote *f_pWiimote, ir_dot_t f_Dot[2]);

+  //bool CalcWiimoteRelativeCursorPos(input_payload_wiimote *f_pWiimote, Vect3D_t *f_pRelPos);

   bool ConvertWiimoteToWorld(input_payload_wiimote *f_pWiimote, Vect3D_t *f_WorldPos);

 

 };

diff --git a/matchblox/engine/typedefs.h b/matchblox/engine/typedefs.h
index a7b0b49..867b85a 100644
--- a/matchblox/engine/typedefs.h
+++ b/matchblox/engine/typedefs.h
@@ -2,46 +2,60 @@
 

 #define TYPEDEFS_H

 

-//#include <wiimote.h>

-#include <wiiuse.h>

+#include <iostream>

+#include <iomanip>

+#include <sstream>

 #include <vector>

 #include <limits>

+#include <GL/glut.h>

+

+using namespace std;

 

 #undef max

 

-typedef struct vec3d

+typedef struct v3 //3d vector struct with overloaded operators

 {

-  double x, y, z;

-

-  //constructors

-  vec3d() 

-    { x=0.0, y=0.0, z=0.0; }

-  vec3d(double fx, double fy, double fz) 

-    { x=fx, y=fy, z=fz; }

-  vec3d(const vec3d &clone)

-    { x=clone.x; y=clone.y; z=clone.z; }

-

-  inline vec3d& operator=(const vec3d &rhs)

-    { x=rhs.x; y=rhs.y; z=rhs.z; return *this; }

-  inline vec3d& operator+=(const vec3d &rhs)

-    { x+=rhs.x; y+=rhs.y; z+=rhs.z; return *this; }

-  inline vec3d& operator-=(const vec3d &rhs)

-    { x-=rhs.x; y-=rhs.y; z-=rhs.z; return *this; }

-  inline vec3d& operator*=(const vec3d &rhs)

-    { x*=rhs.x; y*=rhs.y; z*=rhs.z; return *this; }

-  inline vec3d& operator*=(const double &scal)

-    { x*=scal; y*=scal; z*=scal; return *this; }

-

-  inline const vec3d& operator+(const vec3d &rhs) const

-    { return vec3d(*this) += rhs; }

-  inline const vec3d& operator-(const vec3d &rhs) const

-    { return vec3d(*this) -= rhs; }

-  inline const vec3d& operator*(const vec3d &rhs) const

-    { return vec3d(*this) *= rhs; }

-  inline const vec3d& operator*(const double &scal) const

-    { return vec3d(*this) *= scal; }

+  double x,y,z;

+

+  inline v3()                                 {x=y=z= 0.0;}

+  inline v3(double vx, double vy, double vz)  { x=vx; y=vy; z=vz; }

+  inline v3(const v3 &v)                      {x=v.x; y=v.y; z=v.z;}

+

+  v3& operator=(const v3 &rhs) {x=rhs.x; y=rhs.y; z=rhs.z; return *this;}

+

+  inline v3& operator+=(const v3 &rhs) {x+=rhs.x; y+=rhs.y; z+=rhs.z; return *this;}

+  inline v3& operator-=(const v3 &rhs) {x-=rhs.x; y-=rhs.y; z-=rhs.z; return *this;}

+  inline v3& operator*=(const v3 &rhs) {x*=rhs.x; y*=rhs.y; z*=rhs.z; return *this;}

+  inline v3& operator/=(const v3 &rhs) {x/=rhs.x; y/=rhs.y; z/=rhs.z; return *this;}

+

+  //scalar mult/div

+  inline v3& operator*=(const double s) {x*=s; y*=s; z*=s; return *this;}

+  inline v3& operator/=(const double s) {x/=s; y/=s; z/=s; return *this;}

+

+  inline const v3 operator+(const v3 &rhs) const {v3 v(*this); return v+=rhs;} 

+  inline const v3 operator-(const v3 &rhs) const {v3 v(*this); return v-=rhs;} 

+  inline const v3 operator*(const v3 &rhs) const {v3 v(*this); return v*=rhs;}

+  inline const v3 operator/(const v3 &rhs) const {v3 v(*this); return v/=rhs;}  

+

+  inline const v3 operator*(const double s) const {v3 v(*this); return v*=s;}

+  inline const v3 operator/(const double s) const {v3 v(*this); return v/=s;} 

+

+  inline bool operator>(const v3 &rhs) const { return x>rhs.x && y>rhs.y && z>rhs.z; }

+  inline bool operator<(const v3 &rhs) const { return x<rhs.x && y<rhs.y && z<rhs.z; }

 } Vect3D_t;

 

+//scalar vector multiplication s * v

+inline const v3 operator*(const double s, const v3 &rhs) {return rhs*s;}

+

+//output stream operator for Vect3D_t (handy for printing)

+inline std::ostream& operator<<(std::ostream& os, const Vect3D_t& v ) 

+{

+  stringstream ss;

+  ss << fixed << setprecision(2) << "(" << v.x << ", " << v.y << ", " << v.z << ")";

+

+  return os << ss.str();

+}

+

 typedef struct bb

 {

   Vect3D_t m_Min, //minimal corner vertex (-x, -y, -z)

@@ -62,7 +76,7 @@ typedef struct bb
   //translating a bounding box

   inline bb& operator+=(const Vect3D_t &v)

   { m_Min += v; m_Max += v; return *this; }

-  inline const bb& operator+(const Vect3D_t &v) const

+  inline const bb operator+(const Vect3D_t &v) const

   { return (bb(*this) += v); }

 

   //update the bounding box min/max coords with vertex v

@@ -80,9 +94,7 @@ typedef struct bb
   inline bool Contains(const Vect3D_t &v) const

   {

     //check if the point is contained in the box

-    return m_Min.x < v.x && v.x < m_Max.x &&

-           m_Min.y < v.y && v.y < m_Max.y &&

-           m_Min.z < v.z && v.z < m_Max.z;

+    return m_Min < v && v < m_Max;

   }

 

   inline bool Contains(const bb& b) const

@@ -92,13 +104,8 @@ typedef struct bb
 

   inline bool Overlap(const bb& rhs) const

   {

-    //check if the boxes overlap in all three axis

-    return ( ( m_Min.x < rhs.m_Min.x && rhs.m_Min.x < rhs.m_Max.x) ||

-             ( m_Min.x < rhs.m_Max.x && rhs.m_Max.x < rhs.m_Max.x) )  &&

-           ( ( m_Min.y < rhs.m_Min.y && rhs.m_Min.y < rhs.m_Max.y) ||

-             ( m_Min.y < rhs.m_Max.y && rhs.m_Max.y < rhs.m_Max.y) )  &&

-           ( ( m_Min.z < rhs.m_Min.z && rhs.m_Min.z < rhs.m_Max.z) ||

-             ( m_Min.z < rhs.m_Max.z && rhs.m_Max.z < rhs.m_Max.z) );

+

+    return m_Min < rhs.m_Max && m_Max > rhs.m_Min;       

   }

 

 } BoundingBox_t;

@@ -176,13 +183,17 @@ typedef enum EyeOrigin {
     MONO_CENTER      = 2

 } EyeOrigin_t;

 

+

+//forward class declaration

+class AbstractWiimote;

+

 typedef struct GameState

 {

   FrustumParms_t  m_FrustumParms;

   bool            m_bHeadTrackingEnabled;

   bool            m_bStereoEnabled;

   //wiimote         *m_pTrackingWiimote;

-  wiimote         **m_ppWiimotes;

+  AbstractWiimote *m_pWiimote[2];

   GLuint          m_GreyScaleShaderProgram;   //handle to the grayscale shader program

   GLint           m_GSConvScaleLoc;           //handle to the g_ConversionScale variable in the shader

   GLfloat         m_GSConvScale[3];           //grayscale conversion scale (default 0.299, 0.587, 0.114)

diff --git a/matchblox/main.cpp b/matchblox/main.cpp
index 466c418..cedaa0c 100644
--- a/matchblox/main.cpp
+++ b/matchblox/main.cpp
@@ -6,15 +6,11 @@
   #define TRUE !FALSE

 #endif

 

-

-//#include <wiimote.h>

-#include <wiiuse.h>

 #include <GL/glew.h>

 #include <GL/glut.h>

 #include <iostream>

 

 #include <math.h>

-#include <GL/glut.h>

 

 #include "message_queue.h"

 #include "message_input.h"

@@ -23,6 +19,7 @@
 #include "textfile.h"

 

 #include "C_MatchBloxEngine.h"

+#include "wiimote_utils.h"

 

 #define SCREEN_WIDTH  640

 #define SCREEN_HEIGHT 480

@@ -35,61 +32,37 @@ C_MatchBloxEngine *g_pEngine;
 bool InitWiiMotes()

 {

   //connect to the wiimote(s)

-  g_GameState.m_ppWiimotes = wiiuse_init(1);

 

-#ifdef WIN32

-  wiiuse_set_bluetooth_stack(g_GameState.m_ppWiimotes, 1, WIIUSE_STACK_MS);

-#endif //WIN32

+  //create an abstract wiimote interface 

+#ifdef USE_WIIYOURSELF

+  g_GameState.m_pWiimote[0] = new WiiYourselfWiimote();

+#endif //USE_WIIYOURSELF

 

-  int found = wiiuse_find(g_GameState.m_ppWiimotes, 1, 30);

-  int connected = wiiuse_connect(g_GameState.m_ppWiimotes , 1); 

-  if (connected) 

+  printf("connecting:\n");

+  int retries = 10;

+  while(!g_GameState.m_pWiimote[0]->Connect() && retries > 0)

   {

-    printf("Connected to %i wiimotes (of %i found).\n", connected, found); 

-

-    wiiuse_set_leds(g_GameState.m_ppWiimotes[0], WIIMOTE_LED_1 | WIIMOTE_LED_2 | WIIMOTE_LED_3 | WIIMOTE_LED_4);

-    wiiuse_rumble(g_GameState.m_ppWiimotes[0], 1);

-    //wiiuse_motion_sensing(g_GameState.m_ppWiimotes[0], 1);

-    wiiuse_set_ir(g_GameState.m_ppWiimotes[0], 1);

-    Sleep(200);

-    wiiuse_rumble(g_GameState.m_ppWiimotes[0], 0);

-  }

-  else 

-  { 

-    printf("Failed to connect to any wiimote.\n"); 

-    return 0; 

+    Sleep(100);

+    printf(".");

+    //retries--;

   }

- // g_GameState.m_pTrackingWiimote = new wiimote();

-

- // g_GameState.m_pTrackingWiimote->ChangedCallback = NULL; //no callbacks, we just poll...

-	//g_GameState.m_pTrackingWiimote->CallbackTriggerFlags = NO_CHANGE;

-

- // printf("connecting:\n");

- // int retries = 10;

- // while(!g_GameState.m_pTrackingWiimote->Connect(wiimote::FIRST_AVAILABLE) && retries > 10)

- // {

- //   Sleep(100);

- //   printf(".");

- //   retries--;

- // }

   

-  //if (g_GameState.m_pTrackingWiimote->IsConnected())

-  //{

-  //  printf("connected\n");

-

-  //  g_GameState.m_pTrackingWiimote->SetLEDs(0x0f);

-  //  

-  //  g_GameState.m_pTrackingWiimote->SetReportType(wiimote::IN_BUTTONS_ACCEL_IR, true);	 //    IR dots

-  //  g_GameState.m_pTrackingWiimote->SetRumble(true);  

-  //  Sleep(500);

-  //  g_GameState.m_pTrackingWiimote->SetRumble(false);

-

-  //  return true;

-  //}

-  //else

-  //{

-  //  return false;

-  //}

+  if (g_GameState.m_pWiimote[0]->IsConnected())

+  {

+    printf("connected\n");

+

+    g_GameState.m_pWiimote[0]->SetLeds(0x0f);

+    

+    g_GameState.m_pWiimote[0]->StartRumble();  

+    Sleep(500);

+    g_GameState.m_pWiimote[0]->StopRumble();

+

+    return true;

+  }

+  else

+  {

+    return false;

+  }

 }

 

 bool LoadGreyScaleShader()

@@ -168,32 +141,21 @@ void init_gl(void)
 

 void idle_func(void)

 {

-  //g_GameState.m_pTrackingWiimote->RefreshState();

+  struct input_payload_wiimote wiimote_payload;

 

-  if (wiiuse_poll(g_GameState.m_ppWiimotes, 1))

-  {

-    

-  ////put wiimote data in message queue

-  input_payload_wiimote payload;

-  payload.btns = g_GameState.m_ppWiimotes[0]->btns;

-  payload.ir   = g_GameState.m_ppWiimotes[0]->ir;

-  ////copy buttons bitmask

-  //payload.Button.Bits = g_GameState.m_pTrackingWiimote->Button.Bits;

-  ////copy the 4 ir dots

-  //memcpy((void*)payload.Dot, g_GameState.m_pTrackingWiimote->IR.Dot, 4*sizeof(wiimote::ir::dot));

-

-    struct messageq_s message;

-    message.recipient = MESSAGE_MENU | MESSAGE_RENDERER;

-    message.sender = MESSAGE_INPUT_WIIMOTE;

-    message.payload = &payload;

-    message.payload_size = sizeof(input_payload_wiimote);

-    messageq_send(&message);

-  }

+  g_GameState.m_pWiimote[0]->FillWiimoteMsgPayload(wiimote_payload, 205.0); // << should be a sensor bar led distance

+  

+  struct messageq_s message;

+  message.recipient = MESSAGE_MENU | MESSAGE_RENDERER;

+  message.sender = MESSAGE_INPUT_WIIMOTE;

+  message.payload = &wiimote_payload;

+  message.payload_size = sizeof(input_payload_wiimote);

+  messageq_send(&message);

 

   g_pEngine->ProcessMsgs();

   

   //call engine idle func

-  //MenuRun();

+  MenuRun();

 

   glutPostRedisplay();

 }

@@ -205,7 +167,9 @@ void render_scene(void)
   g_pEngine->Render(glutGet(GLUT_ELAPSED_TIME));

 

   glDisable(GL_LIGHTING);

+  glPushMatrix();

   MenuRender();

+  glPopMatrix();

   glEnable(GL_LIGHTING);

 

   glutSwapBuffers();

@@ -239,8 +203,14 @@ void process_normal_keys(unsigned char key, int x, int y)
     break;

     case 'n':

       g_pEngine->Abort();

-      g_pEngine->NewGame(0, 0, BS_LARGE);

+      g_pEngine->NewGame(0, 0, BS_SMALL);

     break;

+    case 'p':

+      if (!g_pEngine->Pause())

+      {

+        g_pEngine->Resume();

+      }

+      break;

   }

 

   payload.specialkey = FALSE;

@@ -307,9 +277,11 @@ int main(int argc, char **argv)
   MenuInit(SCREEN_WIDTH, SCREEN_HEIGHT);

   InitWiiMotes();

 

-  GameSettings l_set = {10, 2, 2};

+  GameSettings l_set = {10, 1.1, 2};

   g_pEngine = new C_MatchBloxEngine("models", "", l_set);

   

+  //glutFullScreen();

+

   glutMainLoop();

 

   return 0;

diff --git a/matchblox/matchblox.vcproj b/matchblox/matchblox.vcproj
index 1720047..c72b1f3 100644
--- a/matchblox/matchblox.vcproj
+++ b/matchblox/matchblox.vcproj
@@ -43,7 +43,7 @@
 				Name="VCCLCompilerTool"

 				Optimization="0"

 				AdditionalIncludeDirectories="include;common;menu;engine;lib"

-				PreprocessorDefinitions="WIN32;_DEBUG;_CONSOLE"

+				PreprocessorDefinitions="WIN32;_DEBUG;_CONSOLE;USE_WIIYOURSELF"

 				MinimalRebuild="true"

 				BasicRuntimeChecks="3"

 				RuntimeLibrary="3"

@@ -63,7 +63,7 @@
 			/>

 			<Tool

 				Name="VCLinkerTool"

-				AdditionalDependencies="glut32.lib glew32.lib wiiuse.lib"

+				AdditionalDependencies="glut32.lib glew32.lib wiiyourself!.lib"

 				LinkIncremental="2"

 				AdditionalLibraryDirectories="lib"

 				GenerateDebugInformation="true"

@@ -122,7 +122,7 @@
 			<Tool

 				Name="VCCLCompilerTool"

 				AdditionalIncludeDirectories="include;common;menu;engine;lib"

-				PreprocessorDefinitions="WIN32;NDEBUG;_CONSOLE"

+				PreprocessorDefinitions="WIN32;NDEBUG;_CONSOLE;E;USE_WIIYOURSELF"

 				RuntimeLibrary="2"

 				UsePrecompiledHeader="0"

 				WarningLevel="3"

@@ -140,7 +140,7 @@
 			/>

 			<Tool

 				Name="VCLinkerTool"

-				AdditionalDependencies="glut32.lib glew32.lib wiiuse.lib"

+				AdditionalDependencies="glut32.lib glew32.lib wiiyourself!.lib"

 				LinkIncremental="1"

 				GenerateDebugInformation="true"

 				SubSystem="1"

@@ -188,10 +188,6 @@
 				>

 			</File>

 			<File

-				RelativePath=".\menu\button.c"

-				>

-			</File>

-			<File

 				RelativePath=".\engine\C_3DObject.cpp"

 				>

 			</File>

@@ -239,6 +235,10 @@
 				RelativePath=".\common\textfile.cpp"

 				>

 			</File>

+			<File

+				RelativePath=".\common\wiimote_utils.cpp"

+				>

+			</File>

 		</Filter>

 		<Filter

 			Name="Header Files"

@@ -250,6 +250,10 @@
 				>

 			</File>

 			<File

+				RelativePath=".\menu\button.c"

+				>

+			</File>

+			<File

 				RelativePath=".\menu\button.h"

 				>

 			</File>

@@ -282,6 +286,10 @@
 				>

 			</File>

 			<File

+				RelativePath=".\common\C_Smoother.h"

+				>

+			</File>

+			<File

 				RelativePath=".\common\font.h"

 				>

 			</File>

@@ -305,6 +313,10 @@
 				RelativePath=".\engine\typedefs.h"

 				>

 			</File>

+			<File

+				RelativePath=".\common\wiimote_utils.h"

+				>

+			</File>

 		</Filter>

 		<Filter

 			Name="Resource Files"

diff --git a/matchblox/menu/menu.c b/matchblox/menu/menu.c
index 02376d1..cb9573c 100644
--- a/matchblox/menu/menu.c
+++ b/matchblox/menu/menu.c
@@ -263,9 +263,9 @@ void MenuRun(void)
   message = messageq_get(MESSAGE_MENU);
   if (message) {
     payload = message->payload;
-    printf("got a message! %c\n", (unsigned char)payload->key);
+    //printf("got a message! %c\n", (unsigned char)payload->key);
     switch (payload->key) {
-      default:
+      //default:
       case ' ':
         MenuNext();
       break;