From f11312d1d84e102bb29a44dedce4c60a5ae25e56 Mon Sep 17 00:00:00 2001 From: Dennis Peeten Date: Sat, 24 May 2008 09:59:48 +0000 Subject: --- MatchBloxEngine/MatchBloxEngine/C_Box.cpp | 2 +- .../MatchBloxEngine/C_MatchBloxEngine.cpp | 19 +- .../MatchBloxEngine/C_MatchBloxEngine.h | 11 +- MatchBloxEngine/MatchBloxEngine/typedefs.h | 16 +- matchblox/common/message_input.h | 12 +- matchblox/common/message_queue.c | 4 +- matchblox/engine/C_MatchBloxEngine.cpp | 359 ++++++++++++++----- matchblox/engine/C_MatchBloxEngine.h | 20 +- matchblox/engine/typedefs.h | 322 ++++++++--------- matchblox/lib/WiiYourself!.lib | Bin 25718 -> 328086 bytes matchblox/main.cpp | 108 ++++-- matchblox/matchblox.vcproj | 8 +- menu_demo/menu_demo.vcproj | 2 +- wiimote_ir_smoothing/C_3DPointSmoother.cpp | 240 +++++++++++++ wiimote_ir_smoothing/C_3DPointSmoother.h | 75 ++++ wiimote_ir_smoothing/Vect3D.h | 48 +++ wiimote_ir_smoothing/WiiYourself!.lib | Bin 0 -> 328086 bytes wiimote_ir_smoothing/main.cpp | 103 ++++++ wiimote_ir_smoothing/wiimote.h | 335 ++++++++++++++++++ wiimote_ir_smoothing/wiimote_ir_smoothing.vcproj | 210 +++++++++++ wiimote_ir_smoothing/wiimote_state.h | 391 +++++++++++++++++++++ 21 files changed, 1969 insertions(+), 316 deletions(-) create mode 100644 wiimote_ir_smoothing/C_3DPointSmoother.cpp create mode 100644 wiimote_ir_smoothing/C_3DPointSmoother.h create mode 100644 wiimote_ir_smoothing/Vect3D.h create mode 100644 wiimote_ir_smoothing/WiiYourself!.lib create mode 100644 wiimote_ir_smoothing/main.cpp create mode 100644 wiimote_ir_smoothing/wiimote.h create mode 100644 wiimote_ir_smoothing/wiimote_ir_smoothing.vcproj create mode 100644 wiimote_ir_smoothing/wiimote_state.h diff --git a/MatchBloxEngine/MatchBloxEngine/C_Box.cpp b/MatchBloxEngine/MatchBloxEngine/C_Box.cpp index 9bde3ff..343ae98 100644 --- a/MatchBloxEngine/MatchBloxEngine/C_Box.cpp +++ b/MatchBloxEngine/MatchBloxEngine/C_Box.cpp @@ -20,7 +20,7 @@ C_Box::C_Box(const char *f_strFileName, //calculate the tile size (this should be the same //for all tile types) BoundingBox_t l_BBox = m_pTiles[0]->GetBoundingBox(); - m_dTileSize = l_BBox.m_dRight - l_BBox.m_dLeft; + m_dTileSize = l_BBox.m_Max.x - l_BBox.m_Min.x; //calculate the lower left tile position (in xz plane) m_LowLeftTilePos = Vect3D_t(-m_dTileSize * (double)(m_iTilesX-1)/2.0, diff --git a/MatchBloxEngine/MatchBloxEngine/C_MatchBloxEngine.cpp b/MatchBloxEngine/MatchBloxEngine/C_MatchBloxEngine.cpp index 80767b8..dafa6a5 100644 --- a/MatchBloxEngine/MatchBloxEngine/C_MatchBloxEngine.cpp +++ b/MatchBloxEngine/MatchBloxEngine/C_MatchBloxEngine.cpp @@ -376,7 +376,7 @@ 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) +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 @@ -436,13 +436,14 @@ void C_MatchBloxEngine::CursorMove(Vect3D_t f_NewCursorPos) } -GameResult_t C_MatchBloxEngine::CursorMove_PutBlock(Vect3D_t &f_CursPos, BoundingBox_t &f_CusrBBox) +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; @@ -456,27 +457,31 @@ GameResult_t C_MatchBloxEngine::CursorMove_PutBlock(Vect3D_t &f_CursPos, Boundin //?? 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); + 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_pGameSession->NewTurn(); + m_CurrentBlock = m_pCurrentSession->NewTurn(); - m_GameState + //m_GameState } + } + + return false; } -GameResult_t C_MatchBloxEngine::CursorMove_GrabBlock(Vect3D_t &f_CursPos, BoundingBox_t &f_CusrBBox) +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)) + if (m_pBlock[(int)m_CurrentBlock]->GetAbsBoundBox().Overlap(f_CursBBox)) { } + return true; } diff --git a/MatchBloxEngine/MatchBloxEngine/C_MatchBloxEngine.h b/MatchBloxEngine/MatchBloxEngine/C_MatchBloxEngine.h index 5ef6c5d..48a17b7 100644 --- a/MatchBloxEngine/MatchBloxEngine/C_MatchBloxEngine.h +++ b/MatchBloxEngine/MatchBloxEngine/C_MatchBloxEngine.h @@ -3,6 +3,7 @@ #define C_MATCHBLOXENGINE_HEADER_FILE #include "MessageQueue.h" +#include "typedefs.h" class C_3DObject; class C_Block; @@ -126,13 +127,13 @@ struct GameSession m_uiSessionStart += l_uiPauseTime; m_uiTurnStart += l_uiPauseTime; } -} +}; class C_MatchBloxEngine { public: C_MatchBloxEngine(const char *f_strModelPath, - const char *f_strLogFile + const char *f_strLogFile, GameSettings f_GameSettings); ~C_MatchBloxEngine(); @@ -160,6 +161,8 @@ private: EngineState m_State, m_SavedState; GameSession *m_pCurrentSession; GameSettings m_GameSettings; + BoxSize m_CurrentBox; + BlockType m_CurrentBlock; BoundingBox_t m_WorldBox; //an invisible box that limits the movement of the //player @@ -175,8 +178,8 @@ private: //event/input handlers void CursorMove(Vect3D_t &f_NewCursorPos); - GameResult_t CursorMove_PutBlock(Vect3D_t &f_CursPos, BoundingBox_t &f_CusrBBox); - GameResult_t CursorMove_GrabBlock(Vect3D_t &f_CursPos, BoundingBox_t &f_CusrBBox); + bool CursorMove_PutBlock(Vect3D_t &f_CursPos, BoundingBox_t &f_CusrBBox); + bool CursorMove_GrabBlock(Vect3D_t &f_CursPos, BoundingBox_t &f_CusrBBox); }; diff --git a/MatchBloxEngine/MatchBloxEngine/typedefs.h b/MatchBloxEngine/MatchBloxEngine/typedefs.h index 0b9190c..7acb776 100644 --- a/MatchBloxEngine/MatchBloxEngine/typedefs.h +++ b/MatchBloxEngine/MatchBloxEngine/typedefs.h @@ -29,13 +29,13 @@ typedef struct vec3d inline vec3d& operator*=(const double &scal) { x*=scal; y*=scal; z*=scal; return *this; } - inline const vec3d& operator+(const vec3d &rhs) + inline const vec3d& operator+(const vec3d &rhs) const { return vec3d(*this) += rhs; } - inline const vec3d& operator-(const vec3d &rhs) + inline const vec3d& operator-(const vec3d &rhs) const { return vec3d(*this) -= rhs; } - inline const vec3d& operator*(const vec3d &rhs) + inline const vec3d& operator*(const vec3d &rhs) const { return vec3d(*this) *= rhs; } - inline const vec3d& operator*(const double &scal) + inline const vec3d& operator*(const double &scal) const { return vec3d(*this) *= scal; } } Vect3D_t; @@ -59,7 +59,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) + inline const bb& operator+(const Vect3D_t &v) const { return (bb(*this) += v); } //update the bounding box min/max coords with vertex v @@ -74,7 +74,7 @@ typedef struct bb return *this; } - inline bool Contains(const Vect3D_t &v) + 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 && @@ -82,12 +82,12 @@ typedef struct bb m_Min.z < v.z && v.z < m_Max.z; } - inline bool Contains(const bb& b) + inline bool Contains(const bb& b) const { return Contains(b.m_Min) && Contains(b.m_Max); } - inline bool Overlap(const bb& rhs) + 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) || diff --git a/matchblox/common/message_input.h b/matchblox/common/message_input.h index f06e610..e234b74 100644 --- a/matchblox/common/message_input.h +++ b/matchblox/common/message_input.h @@ -11,6 +11,7 @@ extern "C" { #endif +#include struct input_payload_keyboard { @@ -30,13 +31,10 @@ struct input_payload_mouse int y; }; -struct input_payload_wiimote -{ - int passive; - int button; - int state; - int x; - int y; +struct input_payload_wiimote +{ + unsigned short btns; + ir_t ir; }; #ifdef __cplusplus diff --git a/matchblox/common/message_queue.c b/matchblox/common/message_queue.c index b294cee..4b89a3b 100644 --- a/matchblox/common/message_queue.c +++ b/matchblox/common/message_queue.c @@ -82,8 +82,8 @@ struct messageq_s *messageq_get(int recipient) i = (i < MAX_MESSAGES -1) ? i +1 : 0; if (messageq[i].recipient & recipient) { message_found++; - printf("message[%d].recp = %d (messageq_index= %d)\n", i, messageq[i].recipient, messageq_index); - printf("Message found! %d\n", message_found); + //printf("message[%d].recp = %d (messageq_index= %d)\n", i, messageq[i].recipient, messageq_index); + //printf("Message found! %d\n", message_found); } } while ((i != (messageq_index)) && (!message_found)); diff --git a/matchblox/engine/C_MatchBloxEngine.cpp b/matchblox/engine/C_MatchBloxEngine.cpp index e9c9473..93a178f 100644 --- a/matchblox/engine/C_MatchBloxEngine.cpp +++ b/matchblox/engine/C_MatchBloxEngine.cpp @@ -4,7 +4,11 @@ #include #include +#define _USE_MATH_DEFINES +#include + #include "C_MatchBloxEngine.h" +#include "message_input.h" #include "C_3DObject.h" #include "C_Environment.h" @@ -13,6 +17,9 @@ #include "C_Box.h" #include "C_Log.h" #include "bitmap.h" +#include "message_queue.h" +#include "message_input.h" + C_MatchBloxEngine::C_MatchBloxEngine(const char *f_strModelPath, const char *f_strLogFile, @@ -40,9 +47,11 @@ C_MatchBloxEngine::C_MatchBloxEngine(const char *f_strModelPath, m_CurrentBox = BS_SMALL; //init the world bounding box - m_WorldBox.m_Min = Vect3D_t(-50.0, -50.0, -50.0); - m_WorldBox.m_Max = Vect3D_t(50.0, 50.0, 0.0); + 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; } C_MatchBloxEngine::~C_MatchBloxEngine() @@ -55,25 +64,76 @@ C_MatchBloxEngine::~C_MatchBloxEngine() GameResult C_MatchBloxEngine::ProcessMsgs(void) { - //msgStruct l_msg; + struct messageq_s *message; + + while (message = messageq_get(MESSAGE_RENDERER)) + { + switch(message->sender) + { + case MESSAGE_INPUT_KEYBOARD: + break; + 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++) + { + if (l_pMsg->btns && WIIMOTE_BUTTON_A) + { + //init depth + Vect3D_t l_relPos; + if (CalcWiimoteRelativeCursorPos(l_pMsg, &l_relPos)) + { + m_dInitialWiimoteDist = l_relPos.z; + } + else + { + m_dInitialWiimoteDist = 500.0; + } + } + else + { + Vect3D_t l_WorldPos; + if (ConvertWiimoteToWorld(l_pMsg, &l_WorldPos)) + { + m_pBlock[0]->SetPos(l_WorldPos); + } + } + + //std::cout << "Dot["<Dot[i].RawX <<","<< l_pMsg->Dot[i].RawY <<")\n"; + } + + //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; + } + } - ////process message queue - //while (!f_Queue.empty()) - //{ - // //get the first msg - // l_msg = f_Queue.front(); - // f_Queue.pop(); - - // switch (l_msg.m_MessageType) - // { - // case WII_CURSOR_MOVE: - // - // break; - - // case WII_BUTTON_PRESS: - // break; - // } - //} return GR_BUSY; } @@ -102,8 +162,9 @@ void C_MatchBloxEngine::Render_Basics(unsigned int f_uiElapsedTime) //if (m_pCurrentSession) //{ m_pBox[(int)m_CurrentBox]->Render(); - //for (int i=0; i<4; i++) - //m_pBlock[i]->Render(f_uiElapsedTime); + m_pBlock[0]->Render(f_uiElapsedTime); + /* for (int i=0; i<4; i++) + m_pBlock[i]->Render(f_uiElapsedTime);*/ // m_pHand->Render(f_uiElapsedTime); //} @@ -153,42 +214,20 @@ void C_MatchBloxEngine::Render(unsigned int f_uiElapsedTime) break; } - //glPushMatrix(); - - //double l_dSeconds = (double)f_uiElapsedTime/1000.0; - - //glRotated(l_dSeconds * 5.0, 0.0, 1.0, 0.0); - ////glRotated(l_dSeconds * 10, 0.0, 0.0, 1.0); - - //m_pEnvMap->Render(); - - //glPopMatrix(); - - //m_pBox[0]->Render(); - - //for(int i=0; i<4; i++) - //{ - // glPushMatrix(); - // m_pBlock[i]->Render(f_uiElapsedTime); - // glPopMatrix(); - //} - } bool C_MatchBloxEngine::NewGame(int f_iUserID, int f_iGameId, BoxSize f_BS) { if(m_State == ES_INITIALISED) { - //log new game + m_CurrentBox = f_BS; //prepare a session struct for administration - m_pCurrentSession = new GameSession(8, f_BS); + m_pCurrentSession = new GameSession(m_GameSettings.m_iNrOfTurns, f_BS); //randomize the box tiles - m_pBox[(int)m_pCurrentSession->m_BoxSize]->RandomizeTiles(); + m_pBox[(int)m_CurrentBox]->RandomizeTiles(); - m_CurrentBox = f_BS; - //set state to GET READY m_State = ES_GET_READY; @@ -202,13 +241,11 @@ bool C_MatchBloxEngine::StartGame() { if (m_State == ES_GET_READY) { - BlockType l_Block; - //start the session timer - l_Block = m_pCurrentSession->StartSession(); + m_CurrentBlock = m_pCurrentSession->StartSession(); //move a block into the sky - m_pBlock[(int)l_Block]->SetPos(0.0, 0.0, 15.0); + m_pBlock[(int)m_CurrentBlock ]->SetPos(0.0, 0.0, 15.0); m_State = ES_PLAYING_GRAB_BLOCK; @@ -226,7 +263,7 @@ bool C_MatchBloxEngine::Pause() //save current state m_SavedState = m_State; - //probably do something with a time variable + m_pCurrentSession->PauseSession(); //set current state to paused m_State = ES_PAUSED; @@ -244,8 +281,8 @@ bool C_MatchBloxEngine::Resume() //restore previous state m_State = m_SavedState; - //restore timers - // + //restore timer + m_pCurrentSession->ResumeSession(); return true; } @@ -260,7 +297,12 @@ bool C_MatchBloxEngine::Abort() //set state to initialised m_State = ES_INITIALISED; - //.. + //delete session (if there is any) + if (m_pCurrentSession) + { + delete m_pCurrentSession; + m_pCurrentSession = NULL; + } return true; } @@ -455,52 +497,185 @@ void C_MatchBloxEngine::CursorMove(Vect3D_t &f_NewCursorPos) } -bool C_MatchBloxEngine::CursorMove_PutBlock(Vect3D_t &f_CursPos, BoundingBox_t &f_CursBBox) +//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]) { - //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) + //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++) { - //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) + for (int j = i+1; j < 4; j++) { - //yipee!! we have got a winner!! - m_CurrentBlock = m_pCurrentSession->NewTurn(); - - //m_GameState + //check if the ir dots are found + if (f_pWiimote->ir.dot[i].visible && + f_pWiimote->ir.dot[j].visible) + { + //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) + { + mdist = dist2; + dot0 = i; + dot1 = j; + //std::cout << "(" << i << "," << j <<") " << mdist << std::endl; + } + } } - - - } - return false; + 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 + { + f_Dot[0] = f_pWiimote->ir.dot[dot1]; + f_Dot[1] = f_pWiimote->ir.dot[dot0]; + } + + return true; + } + else + { + //std::cout << "nothing...\n"; + return false; + } } -bool C_MatchBloxEngine::CursorMove_GrabBlock(Vect3D_t &f_CursPos, BoundingBox_t &f_CursBBox) + +bool C_MatchBloxEngine::CalcWiimoteRelativeCursorPos(input_payload_wiimote *f_pWiimote, Vect3D_t *f_pRelPos) { - //grabbing a block: just check for bounding box intersection - if (m_pBlock[(int)m_CurrentBlock]->GetAbsBoundBox().Overlap(f_CursBBox)) - { - - } + //get two ir dots + ir_dot_t l_Dot[2]; + + if (!FindIRDots(f_pWiimote, l_Dot)) + return false; + + //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::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"; + return true; } +bool C_MatchBloxEngine::ConvertWiimoteToWorld(input_payload_wiimote *f_pWiimote, Vect3D_t *f_pWorldPos) +{ + Vect3D_t l_RelPos; + + if (!CalcWiimoteRelativeCursorPos(f_pWiimote, &l_RelPos)) + return false; + + //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 + //using the initial distance + l_RelPos.z = l_RelPos.z - m_dInitialWiimoteDist + 250; + if (l_RelPos.z < 0.0) l_RelPos.z = 0.0; + else if (l_RelPos.z > 500.0) l_RelPos.z = 500.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; + + 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 + + //std::cout << "(" << f_pWorldPos->x << "," << f_pWorldPos->y << "," << f_pWorldPos->z << ")\n"; + return true; +} \ No newline at end of file diff --git a/matchblox/engine/C_MatchBloxEngine.h b/matchblox/engine/C_MatchBloxEngine.h index 19e0630..9b5bc98 100644 --- a/matchblox/engine/C_MatchBloxEngine.h +++ b/matchblox/engine/C_MatchBloxEngine.h @@ -3,7 +3,12 @@ #define C_MATCHBLOXENGINE_HEADER_FILE #include "message_queue.h" +#include "message_input.h" #include "typedefs.h" +#include +//#include + +using namespace std; class C_3DObject; class C_Block; @@ -164,6 +169,12 @@ private: BoxSize m_CurrentBox; BlockType m_CurrentBlock; + //typedef pair dotpair; + //list m_DotHist; + int m_DotHistSize; + + 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 //player @@ -178,8 +189,13 @@ private: //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_PutBlock(Vect3D_t &f_CursPos, BoundingBox_t &f_CusrBBox); + //bool CursorMove_GrabBlock(Vect3D_t &f_CursPos, BoundingBox_t &f_CusrBBox); + + //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 ConvertWiimoteToWorld(input_payload_wiimote *f_pWiimote, Vect3D_t *f_WorldPos); }; diff --git a/matchblox/engine/typedefs.h b/matchblox/engine/typedefs.h index 3821bc6..a7b0b49 100644 --- a/matchblox/engine/typedefs.h +++ b/matchblox/engine/typedefs.h @@ -1,157 +1,158 @@ -#ifndef TYPEDEFS_H - -#define TYPEDEFS_H - -#include -#include +#ifndef TYPEDEFS_H + +#define TYPEDEFS_H + +//#include +#include +#include #include - -#undef max - -typedef struct vec3d -{ - 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; } -} Vect3D_t; - -typedef struct bb -{ - Vect3D_t m_Min, //minimal corner vertex (-x, -y, -z) - m_Max; //maximal corner vertex (+x, +y, +z) - - //default constructor creates a maximum inverted bounding box - bb() - { - //init a bounding box with the min values set to max double and visa versa - double l_doubleMax = std::numeric_limits::max(); - m_Min = Vect3D_t(l_doubleMax, l_doubleMax, l_doubleMax); - m_Max = Vect3D_t(-l_doubleMax, -l_doubleMax, -l_doubleMax); - } - - //copy constructor - bb(const bb &b) : m_Min(b.m_Min), m_Max(b.m_Max) {} - - //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 - { return (bb(*this) += v); } - - //update the bounding box min/max coords with vertex v - inline bb& operator<<(const Vect3D_t &v) - { - if (v.x < m_Min.x) m_Min.x = v.x; - else if (v.x > m_Max.x) m_Max.x = v.x; - if (v.y < m_Min.y) m_Min.y = v.y; - else if (v.y > m_Max.y) m_Max.y = v.y; - if (v.z < m_Min.z) m_Min.z = v.z; - else if (v.z > m_Max.z) m_Max.z = v.z; - return *this; - } - - 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; - } - - inline bool Contains(const bb& b) const - { - return Contains(b.m_Min) && Contains(b.m_Max); - } - - 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) ); - } - -} BoundingBox_t; - - -typedef struct triangle -{ - int v1, n1, t1, - v2, n2, t2, - v3, n3, t3; -} Triangle_t; - -typedef struct mat -{ - GLfloat m_fAmb[4], - m_fDif[4], - m_fSpec[4], - m_fEmi[4], - m_fShin; - - //default constructor - mat() - { - m_fAmb[0] = m_fAmb[1] = m_fAmb[2] = 0.7f; m_fAmb[3] = 1.0f; - m_fDif[0] = m_fDif[1] = m_fDif[3] = 0.7f; m_fDif[3] = 1.0f; - m_fSpec[0] = m_fSpec[1] = m_fSpec[2] = m_fSpec[3] = 1.0f; - m_fEmi[0] = m_fEmi[1] = m_fEmi[2] = 0.0f; m_fEmi[3] = 1.0f; - m_fShin = 128.0f; - } - //for convenience - inline void setAmb(float r, float g, float b, float a) - { m_fAmb[0] = r; m_fAmb[1] = g; m_fAmb[2] = b; m_fAmb[3] = a; } - inline void setDif(float r, float g, float b, float a) - { m_fDif[0] = r; m_fDif[1] = g; m_fDif[2] = b; m_fDif[3] = a; } - inline void setSpec(float r, float g, float b, float a) - { m_fSpec[0] = r; m_fSpec[1] = g; m_fSpec[2] = b; m_fSpec[3] = a; } - inline void setEmi(float r, float g, float b, float a) - { m_fEmi[0] = r; m_fEmi[1] = g; m_fEmi[2] = b; m_fEmi[3] = a; } - -} MatProps_t; -/* -typedef struct facegroup -{ - MatProps_t m_material; - std::vector m_faces; -} FaceGroup_t; -*/ - -typedef struct geom -{ - std::vector m_verts, - m_norms, - m_texs; - std::vector m_triangles; + +#undef max + +typedef struct vec3d +{ + 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; } +} Vect3D_t; + +typedef struct bb +{ + Vect3D_t m_Min, //minimal corner vertex (-x, -y, -z) + m_Max; //maximal corner vertex (+x, +y, +z) + + //default constructor creates a maximum inverted bounding box + bb() + { + //init a bounding box with the min values set to max double and visa versa + double l_doubleMax = std::numeric_limits::max(); + m_Min = Vect3D_t(l_doubleMax, l_doubleMax, l_doubleMax); + m_Max = Vect3D_t(-l_doubleMax, -l_doubleMax, -l_doubleMax); + } + + //copy constructor + bb(const bb &b) : m_Min(b.m_Min), m_Max(b.m_Max) {} + + //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 + { return (bb(*this) += v); } + + //update the bounding box min/max coords with vertex v + inline bb& operator<<(const Vect3D_t &v) + { + if (v.x < m_Min.x) m_Min.x = v.x; + else if (v.x > m_Max.x) m_Max.x = v.x; + if (v.y < m_Min.y) m_Min.y = v.y; + else if (v.y > m_Max.y) m_Max.y = v.y; + if (v.z < m_Min.z) m_Min.z = v.z; + else if (v.z > m_Max.z) m_Max.z = v.z; + return *this; + } + + 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; + } + + inline bool Contains(const bb& b) const + { + return Contains(b.m_Min) && Contains(b.m_Max); + } + + 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) ); + } + +} BoundingBox_t; + + +typedef struct triangle +{ + int v1, n1, t1, + v2, n2, t2, + v3, n3, t3; +} Triangle_t; + +typedef struct mat +{ + GLfloat m_fAmb[4], + m_fDif[4], + m_fSpec[4], + m_fEmi[4], + m_fShin; + + //default constructor + mat() + { + m_fAmb[0] = m_fAmb[1] = m_fAmb[2] = 0.7f; m_fAmb[3] = 1.0f; + m_fDif[0] = m_fDif[1] = m_fDif[3] = 0.7f; m_fDif[3] = 1.0f; + m_fSpec[0] = m_fSpec[1] = m_fSpec[2] = m_fSpec[3] = 1.0f; + m_fEmi[0] = m_fEmi[1] = m_fEmi[2] = 0.0f; m_fEmi[3] = 1.0f; + m_fShin = 128.0f; + } + //for convenience + inline void setAmb(float r, float g, float b, float a) + { m_fAmb[0] = r; m_fAmb[1] = g; m_fAmb[2] = b; m_fAmb[3] = a; } + inline void setDif(float r, float g, float b, float a) + { m_fDif[0] = r; m_fDif[1] = g; m_fDif[2] = b; m_fDif[3] = a; } + inline void setSpec(float r, float g, float b, float a) + { m_fSpec[0] = r; m_fSpec[1] = g; m_fSpec[2] = b; m_fSpec[3] = a; } + inline void setEmi(float r, float g, float b, float a) + { m_fEmi[0] = r; m_fEmi[1] = g; m_fEmi[2] = b; m_fEmi[3] = a; } + +} MatProps_t; +/* +typedef struct facegroup +{ + MatProps_t m_material; + std::vector m_faces; +} FaceGroup_t; +*/ + +typedef struct geom +{ + std::vector m_verts, + m_norms, + m_texs; + std::vector m_triangles; } Geometry_t; typedef struct FrustumParms @@ -173,18 +174,19 @@ typedef enum EyeOrigin { STEREO_LEFT_EYE = 0, STEREO_RIGHT_EYE = 1, MONO_CENTER = 2 -} EyeOrigin_t; - +} EyeOrigin_t; + typedef struct GameState { FrustumParms_t m_FrustumParms; bool m_bHeadTrackingEnabled; bool m_bStereoEnabled; - wiimote *m_pTrackingWiimote; + //wiimote *m_pTrackingWiimote; + wiimote **m_ppWiimotes; 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) -} GameState_t; - -#endif //TYPEDEFS_H - +} GameState_t; + +#endif //TYPEDEFS_H + diff --git a/matchblox/lib/WiiYourself!.lib b/matchblox/lib/WiiYourself!.lib index 0af7d02..d6aaa00 100644 Binary files a/matchblox/lib/WiiYourself!.lib and b/matchblox/lib/WiiYourself!.lib differ diff --git a/matchblox/main.cpp b/matchblox/main.cpp index 6d89b6a..466c418 100644 --- a/matchblox/main.cpp +++ b/matchblox/main.cpp @@ -7,7 +7,8 @@ #endif -#include +//#include +#include #include #include #include @@ -33,37 +34,62 @@ C_MatchBloxEngine *g_pEngine; bool InitWiiMotes() { - g_GameState.m_pTrackingWiimote = new wiimote(); + //connect to the wiimote(s) + g_GameState.m_ppWiimotes = wiiuse_init(1); - g_GameState.m_pTrackingWiimote->ChangedCallback = NULL; //no callbacks, we just poll... - g_GameState.m_pTrackingWiimote->CallbackTriggerFlags = NO_CHANGE; +#ifdef WIN32 + wiiuse_set_bluetooth_stack(g_GameState.m_ppWiimotes, 1, WIIUSE_STACK_MS); +#endif //WIN32 - printf("connecting:\n"); - int retries = 10; - while(!g_GameState.m_pTrackingWiimote->Connect(wiimote::FIRST_AVAILABLE) && retries > 10) + int found = wiiuse_find(g_GameState.m_ppWiimotes, 1, 30); + int connected = wiiuse_connect(g_GameState.m_ppWiimotes , 1); + if (connected) { - Sleep(100); - printf("."); - retries--; + 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); } - - 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_EXT); // IR dots - g_GameState.m_pTrackingWiimote->SetRumble(true); - Sleep(500); - g_GameState.m_pTrackingWiimote->SetRumble(false); - - return true; - } - else - { - return false; + else + { + printf("Failed to connect to any wiimote.\n"); + return 0; } + // 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; + //} } bool LoadGreyScaleShader() @@ -142,10 +168,32 @@ void init_gl(void) void idle_func(void) { - //call engine idle func - MenuRun(); + //g_GameState.m_pTrackingWiimote->RefreshState(); + + 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_pTrackingWiimote->RefreshState(); + g_pEngine->ProcessMsgs(); + + //call engine idle func + //MenuRun(); glutPostRedisplay(); } diff --git a/matchblox/matchblox.vcproj b/matchblox/matchblox.vcproj index 86601a1..1720047 100644 --- a/matchblox/matchblox.vcproj +++ b/matchblox/matchblox.vcproj @@ -46,7 +46,7 @@ PreprocessorDefinitions="WIN32;_DEBUG;_CONSOLE" MinimalRebuild="true" BasicRuntimeChecks="3" - RuntimeLibrary="1" + RuntimeLibrary="3" UsePrecompiledHeader="0" WarningLevel="3" Detect64BitPortabilityProblems="true" @@ -63,7 +63,7 @@ /> diff --git a/menu_demo/menu_demo.vcproj b/menu_demo/menu_demo.vcproj index 07198ca..d4af14f 100644 --- a/menu_demo/menu_demo.vcproj +++ b/menu_demo/menu_demo.vcproj @@ -41,7 +41,7 @@ +#include +#include + +#include "C_3DPointSmoother.h" + +using namespace std; + +void C_3DPointSmoother::SetSimpleMovingAverage(int f_iWindowSize) +{ + //reset smoothing stuff for Simple moving average smoothing + m_Method = SimpMovingAvg; + m_iWindowSize = f_iWindowSize; + m_iSampCount = 0; + m_St = Vect3D_t(0.0, 0.0, 0.0); + m_SampWindow.clear(); +} + +void C_3DPointSmoother::SetExponentialMovingAverage(double f_dExponent) +{ + m_Method = ExpMovingAvg; + m_dExponent = f_dExponent; + m_iSampCount = 0; + m_St = Vect3D_t(0.0, 0.0, 0.0); +} + +Vect3D_t C_3DPointSmoother::SmoothSample_SimpMovingAvg(const Vect3D_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 + Vect3D_t l_sum; + for (std::list::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 + Vect3D_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); + } +} + +Vect3D_t C_3DPointSmoother::SmoothSample_ExpMovingAvg(const Vect3D_t &f_Xt) +{ + if (m_iSampCount == 0) + return f_Xt; + else + return m_St + m_dExponent * (f_Xt - m_St); +} + +Vect3D_t C_3DPointSmoother::Smooth(const Vect3D_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; +} + + + +#include "wiimote_state.h" + +bool FindSensorBarDots(wiimote_state::ir &f_IR, Vect3D_t f_Dots[2]) +{ + RawDot l_rd[4]; + + int n = 0; + for(int i = 0; i < 4; i++) + { + if (f_IR.Dot[i].bFound) + { + l_rd[n].rx = f_IR.Dot[i].RawX; + l_rd[n].ry = f_IR.Dot[i].RawY; + n++; + + } + } + + ////print raw dots + //cout << "Raw IR dots:" << endl; + //for (int i=0; i<4; i++) + //{ + // if (i < n) + // { + // cout << "\t" << l_rd[i] << SPACES << endl; + // } + // else + // { + // cout << "\t-- -- --" << SPACES << endl; + // } + //} + + return FindSensorBarDots(l_rd, n, f_Dots); +} + + +bool FindSensorBarDots(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 intpair; + map m_Distances; //the squared distances between the pair of dots + + //iterate through all pairs and compute their squared distance + for(int i=0; isecond; + l_GroupId[l_dotpair.first] = 0; + l_GroupId[l_dotpair.second] = 1; + l_iDotsLeft -= 2; + + for(map::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 -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 + 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; +} \ No newline at end of file diff --git a/wiimote_ir_smoothing/C_3DPointSmoother.h b/wiimote_ir_smoothing/C_3DPointSmoother.h new file mode 100644 index 0000000..bc2ab82 --- /dev/null +++ b/wiimote_ir_smoothing/C_3DPointSmoother.h @@ -0,0 +1,75 @@ +#ifndef C_3D_POINTS_SMOOTHER_H_ + +#define C_3D_POINT_SMOOTHER_H_ + +#include +#include + +using namespace std; + +#include "Vect3D.h" +#include "wiimote.h" + +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; + +typedef v3 Vect3D_t; + +class C_3DPointSmoother +{ +public: + C_3DPointSmoother() : + m_Method(SimpMovingAvg), + m_iWindowSize(2), + m_iSampCount(0), + m_dExponent(0.0), + m_St() {} + + void SetSimpleMovingAverage(int f_iWindowSize); + void SetExponentialMovingAverage(double f_dExponent); + + Vect3D_t Smooth(const Vect3D_t &f_Xt); + +private: + SmoothingMethod m_Method; + int m_iWindowSize, //window size for SimpMovingAvg + m_iSampCount; //number of processed samples + list m_SampWindow; + double m_dExponent; //exponent for ExpMovingAvg + Vect3D_t m_St; //last smoothed statistic S(t) + + Vect3D_t SmoothSample_SimpMovingAvg(const Vect3D_t &f_Xt); + Vect3D_t SmoothSample_ExpMovingAvg(const Vect3D_t &f_Xt); +}; + +typedef struct rd +{ + int rx, ry; + + rd() { rx = ry = 0;} + rd(const rd &d) { rx=d.rx; ry=d.ry; } + inline const rd& operator=(const rd &rhs) { rx=rhs.rx; ry=rhs.ry; return *this;} + inline const rd& operator-=(const rd &rhs) { rx-=rhs.rx; ry-=rhs.ry; return *this;} + inline const rd operator-(const rd &rhs) { return rd(*this)-=rhs;} +} RawDot; + +inline ostream& operator<<(ostream& os, const RawDot& r ) +{ + stringstream ss; + ss << setprecision(4) + << "(" << r.rx << ", " << r.ry << ")"; + + return os << ss.str(); +} + +bool FindSensorBarDots(wiimote_state::ir &f_ir, Vect3D_t f_Dots[2]); +bool FindSensorBarDots(RawDot *f_rd, int f_iNumdots, Vect3D_t f_Dots[2]); + +#endif //C_3D_POINT_SMOOTHER_H_ \ No newline at end of file diff --git a/wiimote_ir_smoothing/Vect3D.h b/wiimote_ir_smoothing/Vect3D.h new file mode 100644 index 0000000..82659fc --- /dev/null +++ b/wiimote_ir_smoothing/Vect3D.h @@ -0,0 +1,48 @@ +#ifndef VECT3D_H_ + +#define VECT3D_H_ + +#include +#include + +typedef struct v3 //3d vector struct with overloaded operators +{ + 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;} +} 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(); +} + +#endif //VECT3D_H_ \ No newline at end of file diff --git a/wiimote_ir_smoothing/WiiYourself!.lib b/wiimote_ir_smoothing/WiiYourself!.lib new file mode 100644 index 0000000..d6aaa00 Binary files /dev/null and b/wiimote_ir_smoothing/WiiYourself!.lib differ diff --git a/wiimote_ir_smoothing/main.cpp b/wiimote_ir_smoothing/main.cpp new file mode 100644 index 0000000..e03d22d --- /dev/null +++ b/wiimote_ir_smoothing/main.cpp @@ -0,0 +1,103 @@ +#include +#include + +#include "wiimote.h" +#include "Vect3D.h" + +#define _USE_MATH_DEFINES +#include + +#define USING_WIIYOURSELF +#include "C_3DPointSmoother.h" + +#define SPACES " " +#define BLANKLINE " " + +using namespace std; + +int main (int argc, char **argv) +{ + HANDLE console = GetStdHandle(STD_OUTPUT_HANDLE); + COORD origin = {0, 0}; + + Vect3D_t l_Dot[2]; + C_3DPointSmoother l_smoother0, l_smoother1; + + //l_smoother0.SetExponentialMovingAverage(0.2); + //l_smoother1.SetExponentialMovingAverage(0.2); + l_smoother0.SetSimpleMovingAverage(10); + l_smoother1.SetSimpleMovingAverage(10); + + wiimote l_wm = wiimote(); + l_wm.ChangedCallback = NULL; //no callbacks, we just poll... + l_wm.CallbackTriggerFlags = NO_CHANGE; + + cout << "Connecting"; + while (!l_wm.Connect()) + { + cout << "."; + Sleep(500); + } + cout << "\nConnected!\n"; + + //enable ir + l_wm.SetReportType(wiimote::IN_BUTTONS_ACCEL_IR, true); + + Sleep(100); + + cout.fill(' '); + cout.setf(ios::left, ios::adjustfield); + + do + { + l_wm.RefreshState(); + + SetConsoleCursorPosition(console, origin); + + if (FindSensorBarDots(l_wm.IR, l_Dot)) + { + l_Dot[0] = l_smoother0.Smooth(l_Dot[0]); + l_Dot[1] = l_smoother1.Smooth(l_Dot[1]); + cout << "Smoothed Dots:" << endl; + cout << setw(32) << l_Dot[0] ; + cout << setw(32) << l_Dot[1] << endl << endl; + + cout << setw(32) << "Dot distance:" << endl; + Vect3D_t dif = l_Dot[0] - l_Dot[1]; + double dist2 = dif.x*dif.x + dif.y*dif.y + dif.z*dif.z; + double dist = sqrt(dist2); + cout << setw(32) << dist << endl << endl; + + + double radPerPixel = ((M_PI / 180)*40.0)/1016.0, + angle = dist * radPerPixel, + z = (0.5 * 205.0) / tan(0.5 * angle); + cout << setw(32) << "Relative coordinates:" << endl; + cout << "x (px): " << setw(10) << fixed << setprecision(2) + << (l_Dot[0].x + l_Dot[1].x)/2.0 << endl; + cout << "y (px): " << setw(10) << fixed << setprecision(2) + << (l_Dot[0].y + l_Dot[1].y)/2.0 << endl; + cout << "z (mm): " << setw(10) << fixed << setprecision(2) << z << endl; + + + } + else + { + cout << "no dots..."; + + for (int i=0 ; i<20; i++) + cout << BLANKLINE << endl; + } + + Sleep(10); + + } + while (!l_wm.Button.Home());//!l_wm.Button.Home()); + + cout << "Exiting...\n"; + + //l_wm.Disconnect(); + + + return 0; +} \ No newline at end of file diff --git a/wiimote_ir_smoothing/wiimote.h b/wiimote_ir_smoothing/wiimote.h new file mode 100644 index 0000000..e208111 --- /dev/null +++ b/wiimote_ir_smoothing/wiimote.h @@ -0,0 +1,335 @@ +// _______________________________________________________________________________ +// +// - WiiYourself! - native C++ Wiimote library v1.00 +// (c) gl.tter 2007-8 - http://gl.tter.org +// +// see License.txt for conditions of use. see History.txt for change log. +// _______________________________________________________________________________ +// +// wiimote.h (tab = 4 spaces) + +#ifdef _MSC_VER // VC +# pragma once +#endif + +#ifndef _WIIMOTE_H +# define _WIIMOTE_H + +#define WIN32_LEAN_AND_MEAN +#include +#include // auto Unicode/Ansi support +#include // for HID write method +#include // for state recording + using namespace std; + + +#ifdef _MSC_VER // VC-specific: _DEBUG build only _ASSERT() sanity checks +# include +#else +# define _ASSERT // (add your compiler's implementation if you like) +#endif + +#include "wiimote_state.h" + +// configs: + // we request periodic status report updates to refresh the battery level +// and to detect connection loss (through failed writes) +#define REQUEST_STATUS_EVERY_MS 1000 +// all threads (read/parse, audio streaming, async rumble...) use this priority +#define WORKER_THREAD_PRIORITY THREAD_PRIORITY_HIGHEST + + // internals +#define WIIYOURSELF_VERSION_MAJOR 1 +#define WIIYOURSELF_VERSION_MINOR1 0 +#define WIIYOURSELF_VERSION_MINOR2 0 +#define WIIYOURSELF_VERSION_STR _T("1.00") + +// clarity +typedef HANDLE EVENT; + + +// state data changes can be signalled to the app via a callback. Set the wiimote +// object's 'ChangedCallback' any time to enable them. 'changed' is a combination +// of flags indicating which state has changed since the last callback. +typedef void (*state_changed_callback) (class wiimote &owner, + state_change_flags changed); + +// internals +typedef BOOLEAN (__stdcall *hidwrite_ptr)(HANDLE HidDeviceObject, + PVOID ReportBuffer, + ULONG ReportBufferLength); + +// wiimote class - connects and manages a wiimote and its optional extensions +// (Nunchuk/Classic Controller), and exposes their state +class wiimote : public wiimote_state + { + public: + wiimote (); + ~wiimote (); + + public: + // wiimote data input mode (use with SetReportType()) + // (only enable what you need to save battery power) + enum input_report + { + // combinations if buttons/acceleration/IR/Extension data + IN_BUTTONS = 0x30, + IN_BUTTONS_ACCEL = 0x31, + IN_BUTTONS_ACCEL_IR = 0x33, // reports IR EXTENDED data (dot sizes) + IN_BUTTONS_ACCEL_EXT = 0x35, + IN_BUTTONS_ACCEL_IR_EXT = 0x37, // reports IR BASIC data (no dot sizes) + }; + + + public: // convenience accessors: + inline bool IsConnected () const { return bStatusReceived; } + // if IsConnected() unexpectedly returns false, connection was probably lost + inline bool ConnectionLost () const { return bConnectionLost; } + inline bool NunchukConnected () const { return (Internal.bExtension && (Internal.ExtensionType==wiimote_state::NUNCHUK)); } + inline bool ClassicConnected () const { return (Internal.bExtension && (Internal.ExtensionType==wiimote_state::CLASSIC)); } + inline bool IsPlayingAudio () const { return (Internal.Speaker.Freq && Internal.Speaker.Volume); } + inline bool IsPlayingSample () const { return IsPlayingAudio() && (CurrentSample != NULL); } + inline bool IsUsingHIDwrites () const { return bUseHIDwrite; } + inline bool IsRecordingState () const { return Recording.bEnabled; } + + static inline unsigned TotalConnected() { return _TotalConnected; } + + + public: // data + // optional callbacks - set these to your own fuctions (if required) + state_changed_callback ChangedCallback; + // you can avoid unnecessary callback overhead by specifying a mask + // of which state changes should trigger them (default is any) + state_change_flags CallbackTriggerFlags; + + // get the button name from its bit index (some bits are unused) + static const TCHAR* ButtonNameFromBit [16]; + // same for the Classic Controller + static const TCHAR* ClassicButtonNameFromBit [16]; + // get the frequency from speaker_freq enum + static const unsigned FreqLookup [10]; + + public: // methods + // call Connect() first - returns true if wiimote was found & enabled + // - 'wiimote_index' specifies which *installed* (not necessarily + // *connected*) wiimote should be tried (0 = first). + // if you just want the first *available* wiimote that isn't already + // in use, pass in FIRST_AVAILABLE (default). + // - 'force_hidwrites' forces HID output method (it's auto-selected + // when needed and less efficient, so only force for testing). + static const unsigned FIRST_AVAILABLE = 0xffffffff; + bool Connect (unsigned wiimote_index = FIRST_AVAILABLE, + bool force_hidwrites = false); + // disconnect from the controller and stop reading data from it + void Disconnect (); + // set wiimote reporting mode (call after Connnect()) + // continous = true forces the wiimote to send constant updates, even when + // nothing has changed. + // = false only sends data when something has changed (note that + // acceleration data will cause frequent updates anyway as it + // jitters even when the wiimote is stationary) + void SetReportType (input_report type, bool continuous = false); + + // to read the state via polling (reading the public state data direct from + // the wiimote object) you must call RefreshState() at the top of every pass. + // returns a combination of flags to indicate which state (if any) has + // changed since the last call. + inline state_change_flags RefreshState () { return _RefreshState(false); } + + // reset the wiimote (changes report type to non-continuous buttons-only, + // clears LEDs & rumble, mutes & disables speaker) + void Reset (); + // set/clear the wiimote LEDs + void SetLEDs (BYTE led_bits); // bits 0-3 are valid + // set/clear rumble + void SetRumble (bool on); + // alternative - rumble for a fixed amount of time (asynchronous) + void RumbleForAsync (unsigned milliseconds); + + // *experimental* speaker support: + bool MuteSpeaker (bool on); + bool EnableSpeaker (bool on); + bool PlaySquareWave (speaker_freq freq, BYTE volume = 0x40); + // note: PlaySample currently streams from the passed-in wiimote_sample - + // don't delete it until playback has stopped. + bool PlaySample (const wiimote_sample &sample, + BYTE volume = 0x40, + speaker_freq freq_override = FREQ_NONE); + + // 16bit mono sample loading/conversion to native format: + // .wav sample + static bool Load16bitMonoSampleWAV (const TCHAR* filepath, + wiimote_sample &out); + // raw 16bit mono audio data (can be signed or unsigned) + static bool Load16BitMonoSampleRAW (const TCHAR* filepath, + bool _signed, + speaker_freq freq, + wiimote_sample &out); + // converts a 16bit mono sample array to a wiimote_sample + static bool Convert16bitMonoSamples (const short* samples, + bool _signed, + DWORD length, + speaker_freq freq, + wiimote_sample &out); + + // state recording - records state snapshots to a 'state_history' supplied + // by the caller. states are timestamped and only added + // to the list when the specified state changes. + struct state_event { + DWORD time_ms; // system timestamp in milliseconds + wiimote_state state; + }; + typedef list state_history; + static const unsigned UNTIL_STOP = 0xffffffff; + // - pass in a 'state_history' list, and don't destroy/change it until + // recording is stopped. note the list will be cleared first. + // - you can request a specific duration (and use IsRecordingState() to detect + // the end), or UNTIL_STOP. StopRecording() can be called either way. + // - you can specify specific state changes that will trigger an insert + // into the history (others are ignored). + void RecordState (state_history &events_out, + unsigned max_time_ms = UNTIL_STOP, + state_change_flags change_trigger = CHANGED_ALL); + void StopRecording (); + + + private: // methods + // start reading asynchronously from the controller + bool BeginAsyncRead (); + // request status update (battery level, extension status etc) + void RequestStatusReport (); + // read address or register from Wiimote asynchronously (the result is + // parsed internally whenever it arrives) + bool ReadAddress (int address, short size); + // write a single BYTE to a wiimote address or register + inline void WriteData (int address, BYTE data) { WriteData(address, 1, &data); } + // write a BYTE array to a wiimote address or register + void WriteData (int address, BYTE size, const BYTE* buff); + // callback when data is ready to be processed + void OnReadData (DWORD bytes_read); + // parse individual reports by type + int ParseInput (BYTE* buff); + // initializes an extension when plugged in. + void InitializeExtension (); + // decrypts data sent from the extension + void DecryptBuffer (BYTE* buff, unsigned size); + // parses a status report + int ParseStatus (BYTE* buff); + // parses the buttons + int ParseButtons (BYTE* buff); + // parses accelerometer data + int ParseAccel (BYTE* buff); + bool EstimateOrientationFrom(wiimote_state::acceleration &accel); + void ApplyJoystickDeadZones (wiimote_state::joystick &joy); + // parses IR data from report + int ParseIR (BYTE* buff); + // parses data from an extension. + int ParseExtension (BYTE* buff, unsigned offset); + // parses data returned from a read report + int ParseReadAddress (BYTE* buff); + // reads calibration information stored on Wiimote + void ReadCalibration (); + // turns on the IR sensor (the mode must match the reporting mode caps) + void EnableIR (wiimote_state::ir::mode mode); + // disables the IR sensor + void DisableIR (); + // writes a report to the Wiimote (NULL = use 'WriteBuff') + bool WriteReport (BYTE* buff); + bool StartSampleThread (); + // returns the rumble BYTE that needs to be sent with reports. + inline BYTE GetRumbleBit () const { return Internal.bRumble? 0x01 : 0x00; } + // copy the accumulated parsed state into the public state + state_change_flags _RefreshState (bool for_callbacks); + + // static thread funcs: + static unsigned __stdcall ReadParseThreadfunc (void* param); + static unsigned __stdcall AsyncRumbleThreadfunc (void* param); + static unsigned __stdcall SampleStreamThreadfunc(void* param); + static unsigned __stdcall HIDwriteThreadfunc (void* param); + + private: // data + // wiimote output comands + enum output_report + { + OUT_NONE = 0x00, + OUT_LEDs = 0x11, + OUT_TYPE = 0x12, + OUT_IR = 0x13, + OUT_SPEAKER_ENABLE = 0x14, + OUT_STATUS = 0x15, + OUT_WRITEMEMORY = 0x16, + OUT_READMEMORY = 0x17, + OUT_SPEAKER_DATA = 0x18, + OUT_SPEAKER_MUTE = 0x19, + OUT_IR2 = 0x1a, + }; + // input reports used only internally: + static const int IN_STATUS = 0x20; + static const int IN_READADDRESS = 0x21; + // wiimote device IDs: + static const int VID = 0x057e; // 'Nintendo' + static const int PID = 0x0306; // 'Wiimote' + // we could find this out the hard way using HID, but it's 22 + static const int REPORT_LENGTH = 22; + // wiimote registers + static const int REGISTER_CALIBRATION = 0x0016; + static const int REGISTER_IR = 0x04b00030; + static const int REGISTER_IR_SENSITIVITY_1 = 0x04b00000; + static const int REGISTER_IR_SENSITIVITY_2 = 0x04b0001a; + static const int REGISTER_IR_MODE = 0x04b00033; + static const int REGISTER_EXTENSION_INIT = 0x04a40040; + static const int REGISTER_EXTENSION_TYPE = 0x04a400fe; + static const int REGISTER_EXTENSION_CALIBRATION = 0x04a40020; + + HANDLE Handle; // read/write device handle + OVERLAPPED Overlapped; // for async Read/WriteFile() IO + HANDLE ReadParseThread; // waits for overlapped reads & parses result + EVENT DataRead; // signals overlapped read complete + bool bUseHIDwrite; // alternative write method (less efficient + // but required for some BT stacks (eg. MS') + // HidD_SetOutputReport is only supported from XP onwards, so detect & + // load it dynamically: + static HMODULE HidDLL; + static hidwrite_ptr _HidD_SetOutputReport; + + volatile bool bStatusReceived; // for output method detection + volatile bool bConnectionLost; // auto-Disconnect()s if set + static unsigned _TotalCreated; + static unsigned _TotalConnected; + input_report ReportType; // type of data the wiimote delivers + // read buffer + BYTE ReadBuff [REPORT_LENGTH]; + // for polling: state is updated on a thread internally, and made public + // via _RefreshState() + CRITICAL_SECTION StateLock; + wiimote_state Internal; + state_change_flags InternalChanged; // state changes since last RefreshState() + // periodic status report requests (for battery level and connection loss + // detection) + DWORD NextStatusTime; + // async Hidd_WriteReport() thread + HANDLE HIDwriteThread; + queue HIDwriteQueue; + CRITICAL_SECTION HIDwriteQueueLock; // queue must be locked before being modified + // async rumble + HANDLE AsyncRumbleThread; // automatically disables rumble if requested + volatile DWORD AsyncRumbleTimeout; + // orientation estimation + unsigned WiimoteNearGUpdates; + unsigned NunchukNearGUpdates; + // audio + HANDLE SampleThread; + const wiimote_sample* volatile CurrentSample; // otherwise playing square wave + // state recording + struct recording + { + volatile bool bEnabled; + state_history *StateHistory; + volatile DWORD StartTimeMS; + volatile DWORD EndTimeMS; // can be UNTIL_STOP + unsigned TriggerFlags; // wiimote changes trigger a state event + unsigned ExtTriggerFlags;// extension changes " + } Recording; + }; + +#endif // _WIIMOTE_H \ No newline at end of file diff --git a/wiimote_ir_smoothing/wiimote_ir_smoothing.vcproj b/wiimote_ir_smoothing/wiimote_ir_smoothing.vcproj new file mode 100644 index 0000000..eeda708 --- /dev/null +++ b/wiimote_ir_smoothing/wiimote_ir_smoothing.vcproj @@ -0,0 +1,210 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/wiimote_ir_smoothing/wiimote_state.h b/wiimote_ir_smoothing/wiimote_state.h new file mode 100644 index 0000000..e7add28 --- /dev/null +++ b/wiimote_ir_smoothing/wiimote_state.h @@ -0,0 +1,391 @@ +// _______________________________________________________________________________ +// +// - WiiYourself! - native C++ Wiimote library v1.00 +// (c) gl.tter 2007-8 - http://gl.tter.org +// +// see License.txt for conditions of use. see History.txt for change log. +// _______________________________________________________________________________ +// +// wiimote_state.h (tab = 4 spaces) + +// the 'wiimote_state' struct contains all the Wiimote and Extension state data +// (buttons etc) - the wiimote class inherits from this and the app can poll +// the data there at any time. +#ifdef _MSC_VER // VC +# pragma once +#endif + +#ifndef _WIIMOTE_STATE_H +# define _WIIMOTE_STATE_H + +// speaker support: +enum speaker_freq + { + // (keep in sync with FreqLookup in wiimote.cpp) + FREQ_NONE = 0, + // my PC can't keep up with these using bUseHIDwrite, so I haven't + // been able to tune them yet + FREQ_4200HZ = 1, + FREQ_3920HZ = 2, + FREQ_3640HZ = 3, + FREQ_3360HZ = 4, + // these were tuned until the square-wave was glitch-free on my remote - + // may not be exactly right + FREQ_3130HZ = 5, // +190 + FREQ_2940HZ = 6, // +180 + FREQ_2760HZ = 7, // +150 + FREQ_2610HZ = 8, // +140 + FREQ_2470HZ = 9, + }; + +// wiimote_sample - holds the audio sample in the native wiimote format +struct wiimote_sample + { + wiimote_sample() : samples(NULL), length(0), freq(FREQ_NONE) {} + BYTE* samples; + DWORD length; + speaker_freq freq; + }; + +// flags & masks that indicate which part(s) of the wiimote state have changed +enum state_change_flags + { + // state didn't change at all + NO_CHANGE = 0, + + // Wiimote specific + CONNECTION_LOST = 1<<0, + BATTERY_CHANGED = 1<<1, + LEDS_CHANGED = 1<<2, // (probably redudant as wiimmote never + BUTTONS_CHANGED = 1<<3, // changes them unless requested) + ACCEL_CHANGED = 1<<4, + ORIENTATION_CHANGED = 1<<5, + IR_CHANGED = 1<<6, + // all wiimote flags + WIIMOTE_CHANGED = CONNECTION_LOST|BATTERY_CHANGED|LEDS_CHANGED| + BUTTONS_CHANGED|ACCEL_CHANGED|ORIENTATION_CHANGED| + IR_CHANGED, + // Extensions - general + EXTENSION_DISCONNECTED = 1<<7, + EXTENSION_PARTIALLY_INSERTED = 1<<8, + + // Nunchuk specific + NUNCHUK_CONNECTED = 1<<9, + NUNCHUK_BUTTONS_CHANGED = 1<<10, + NUNCHUK_ACCEL_CHANGED = 1<<11, + NUNCHUK_ORIENTATION_CHANGED = 1<<12, + NUNCHUK_JOYSTICK_CHANGED = 1<<13, + // all flags + NUNCHUK_CHANGED = NUNCHUK_CONNECTED|NUNCHUK_BUTTONS_CHANGED| + NUNCHUK_ACCEL_CHANGED|NUNCHUK_ORIENTATION_CHANGED| + NUNCHUK_JOYSTICK_CHANGED, + // Classic Controller + CLASSIC_CONNECTED = 1<<14, + CLASSIC_BUTTONS_CHANGED = 1<<15, + CLASSIC_JOYSTICK_L_CHANGED = 1<<16, + CLASSIC_JOYSTICK_R_CHANGED = 1<<17, + CLASSIC_TRIGGERS_CHANGED = 1<<18, + // all flags + CLASSIC_CHANGED = CLASSIC_CONNECTED|CLASSIC_BUTTONS_CHANGED| + CLASSIC_JOYSTICK_L_CHANGED| + CLASSIC_JOYSTICK_R_CHANGED|CLASSIC_TRIGGERS_CHANGED, + // ALL extension-related flags + EXTENSION_CHANGED = EXTENSION_DISCONNECTED| + NUNCHUK_CHANGED|CLASSIC_CHANGED, + // ALL flags + CHANGED_ALL = WIIMOTE_CHANGED|EXTENSION_CHANGED, + }; + +// wiimote_state (contains the Wiimote and Extension data and settings) +struct wiimote_state + { + friend class wiimote; // for Clear() + + // calibration information (stored on the Wiimote) + struct calibration_info + { + BYTE X0, Y0, Z0; + BYTE XG, YG, ZG; + } CalibrationInfo; + + // button state: + struct buttons + { + // convenience accessors + inline bool A () const { return (Bits & _A) != 0; } + inline bool B () const { return (Bits & _B) != 0; } + inline bool Plus () const { return (Bits & PLUS) != 0; } + inline bool Home () const { return (Bits & HOME) != 0; } + inline bool Minus () const { return (Bits & MINUS) != 0; } + inline bool One () const { return (Bits & ONE) != 0; } + inline bool Two () const { return (Bits & TWO) != 0; } + inline bool Up () const { return (Bits & UP) != 0; } + inline bool Down () const { return (Bits & DOWN) != 0; } + inline bool Left () const { return (Bits & LEFT) != 0; } + inline bool Right () const { return (Bits & RIGHT) != 0; } + + // all 11 buttons stored as bits (set = pressed) + WORD Bits; + + // button bit masks (little-endian order) + enum mask + { + LEFT = 0x0001, + RIGHT = 0x0002, + DOWN = 0x0004, + UP = 0x0008, + PLUS = 0x0010, + TWO = 0x0100, + ONE = 0x0200, + _B = 0x0400, // ie. trigger + _A = 0x0800, + MINUS = 0x1000, + HOME = 0x8000, + // + ALL = LEFT|RIGHT|DOWN|UP|PLUS|TWO|ONE|_A|_B|MINUS|HOME, + }; + } Button; + + // accelerometers state: + struct acceleration + { + BYTE RawX, RawY, RawZ; + float X, Y, Z; + + // note: experimental! the orientation values can only be safely estimated + // if the controller isn't accelerating (otherwise there is no + // simple way to seperate orientation from acceleration - except + // perhaps using the IR reference and/or some clever assumptions). + // so for now the code only updates orientation if the controller + // appear to be stationary (by checking if the acceleration vector + // length is near 1G for several updates in a row). + // also note that there is no way to detect Yaw from the accelerometer + // alone when it's pointing at the screen (and I'm not curently + // processing IR): + struct orientation + { + float X, Y, Z; + unsigned UpdateAge; // how many acceleration updates ago the last + // orientation estimate was made (if this + // value is high, the values are out-of-date + // and probably shouldn't be used). + // Euler angle support (useful for some things). + // * note that decomposing to Euler angles is complex, not always reliable, + // and also depends on your assumptions about the order each component + // is applied in. you may need to handle this yourself for more + // complex scenarios * + float Pitch; // in degrees (-180 - +180) + float Roll; // " + // float Yaw; + } Orientation; + } Acceleration; + + // IR camera state: + struct ir + { + // in theory the IR imager is 1024x768 and so should report raw coords + // 0-1023 x 0-767. in practice I have never seen them exceed the values + // below, so I'm using them instead to give the full 0-1 float range + // (it's possible that the edge pixels are used for processing, or masked + // out due to being unreliable) + static const unsigned MAX_RAW_X = 1016; + static const unsigned MAX_RAW_Y = 760; + + // data mode reported by the IR sensor + enum mode + { + OFF = 0x00, + BASIC = 0x01, // 10 bytes + EXTENDED = 0x03, // 12 bytes + FULL = 0x05, // 16 bytes * 2 (format unknown) + }; + mode Mode; + + struct dot + { + bool bFound; // wiimote can see it + unsigned RawX; + unsigned RawY; + float X; // 0-1 (left-right) + float Y; // " (top -bottom) + int Size; // (not available in BASIC mode) + } Dot[4]; + } IR; + + struct leds + { + // all LEDs stored in bits 0-3 (1 = lit) + BYTE Bits; + + // convenience accessors: + inline bool Lit (unsigned index) + { _ASSERT(index < 4); + return (index >= 4)? false : ((Bits & (1<