void
WiimoteManager::handle_event(CWiimote& wm)
{
std::lock_guard<std::mutex> lock(m_mutex);
char prefixString[64];
sprintf(prefixString, "Controller [%i]: ", wm.GetID());
if(wm.Buttons.isJustPressed(CButtons::BUTTON_UP))
{
//g_orientation = glm::vec3();
}
if(wm.Buttons.isJustPressed(CButtons::BUTTON_RIGHT))
{
log_info("activating gyro");
wm.EnableMotionPlus(CWiimote::ON);
wm.ExpansionDevice.MotionPlus.Gyroscope.SetGyroThreshold(1);
}
if(wm.Buttons.isJustPressed(CButtons::BUTTON_LEFT))
{
wm.EnableMotionPlus(CWiimote::OFF);
}
if(wm.Buttons.isJustPressed(CButtons::BUTTON_A))
{
m_accumulated = glm::vec3(0.0f, 0.0f, 0.0f);
m_gyro_orientation = glm::quat();
m_gyro_pitch = 0.0f;
m_gyro_yaw = 0.0f;
m_gyro_roll = 0.0f;
}
if(wm.isUsingACC())
{
// We are not using WiiCs orientation calculation as it doesn't
// work properly when roll and ptich are in effect at the same
// time
glm::vec3 gravity;
wm.Accelerometer.GetGravityVector(gravity.x, gravity.y, gravity.z);
// convert WiiC gravity vector into OpenGL coordinate system
std::swap(gravity.y, gravity.z);
gravity.z = -gravity.z;
float alpha = 0.05f;
m_smoothed_gravity = alpha * gravity + (1.0f - alpha) * m_smoothed_gravity;
gravity = m_smoothed_gravity;
// calculate the roll
float roll = atan2f(gravity.y, gravity.x) - glm::half_pi<float>();
glm::quat roll_rot = glm::quat(glm::vec3(0.0f, 0.0f, roll));
// remove the roll from the gravity vector, so that pitch can be calculated properly
glm::quat rot_undo = glm::normalize(glm::quat(glm::vec3(0.0f, 0.0f, -roll)));
gravity = rot_undo * gravity;
// calculate pitch
float pitch = atan2f(gravity.z, gravity.y);
glm::quat pitch_rot = glm::quat(glm::vec3(pitch, 0.0f, 0.0f));
m_accel_orientation = pitch_rot * roll_rot;
m_accel_roll = roll;
m_accel_pitch = pitch;
m_yaw = glm::yaw(m_gyro_orientation);
m_pitch = glm::pitch(m_gyro_orientation);
m_roll = glm::roll(m_gyro_orientation);
//m_accel_orientation = glm::quat(glm::vec3(0.0f, glm::radians(glm::yaw(m_gyro_orientation)), 0.0f)) * m_accel_orientation;
//std::cout << m_yaw << std::endl;
//printf("%8.2f %8.2f %8.2f %8.2f %8.2f\n", glm::degrees(pitch), glm::degrees(roll), gravity.x, gravity.y, gravity.z);
}
// if the Motion Plus is turned on then print angles
if(wm.isUsingMotionPlus())
{
float pitch, yaw, roll;
wm.ExpansionDevice.MotionPlus.Gyroscope.GetRates(roll, pitch, yaw);
// convert into radians
roll = glm::radians(roll * 0.01f);
pitch = glm::radians(pitch * -0.01f);
yaw = glm::radians(yaw * 0.01f);
//glm::vec3 rot_x = m_gyro_orientation * glm::vec3(1.0f, 0.0f, 0.0f);
glm::vec3 rot = m_gyro_orientation * glm::vec3(0.0f, 0.0f, 1.0f);
//glm::vec3 rot_z = m_gyro_orientation * glm::vec3(0.0f, 0.0f, 1.0f);
m_gyro_pitch = m_accel_pitch;
m_gyro_yaw = atan2(rot.x, rot.z);
m_gyro_roll = m_accel_roll;
//glm::quat pitch(glm::vec3(m_gyro_pitch, 0.0f, 0.0f));
//glm::quat yaw(glm::vec3(0.0f, m_gyro_yaw, 0.0f));
//glm::quat roll(glm::vec3(0.0f, 0.0f, m_gyro_roll));
//return yaw * pitch * roll;
m_gyro_orientation = m_gyro_orientation * glm::quat(glm::vec3(pitch, yaw, roll));
//glm::quat qpitch(glm::vec3(m_accel_pitch, 0.0f, 0.0f));
//glm::quat qyaw(glm::vec3(0.0f, m_gyro_yaw, 0.0f));
//glm::quat qroll(glm::vec3(0.0f, 0.0f, m_accel_roll));
//m_orientation = /*qyaw */ qpitch * qroll;
//glm::vec3 angles = glm::eulerAngles(g_wiimote.get_orientation());
//glm::vec3 accum = g_wiimote.get_accumulated();
//printf("wiimote: %8.2f %8.2f %8.2f -- ", glm::degrees(angles.x), glm::degrees(angles.y), glm::degrees(angles.z));
//printf("%8.2f %8.2f %8.2f\n", glm::degrees(accum.x), glm::degrees(accum.y), glm::degrees(accum.z));
}
if(wm.isUsingIR())
{
std::vector<CIRDot>::iterator i;
int x, y;
int index;
printf("%s Num IR Dots: %i\n", prefixString, wm.IR.GetNumDots());
printf("%s IR State: %u\n", prefixString, wm.IR.GetState());
std::vector<CIRDot>& dots = wm.IR.GetDots();
for(index = 0, i = dots.begin(); i != dots.end(); ++index, ++i)
{
if((*i).isVisible())
{
(*i).GetCoordinate(x, y);
printf("%s IR source %i: (%i, %i)\n", prefixString, index, x, y);
wm.IR.GetCursorPosition(x, y);
printf("%s IR cursor: (%i, %i)\n", prefixString, x, y);
printf("%s IR z distance: %f\n", prefixString, wm.IR.GetDistance());
}
}
}
if (false)
{
printf("a.p: %8.2f a.r: %8.2f -- g.p: %8.2f g.y: %8.2f g.r: %8.2f\n",
glm::degrees(m_accel_pitch), glm::degrees(m_accel_roll),
glm::degrees(m_gyro_pitch), glm::degrees(m_gyro_yaw), glm::degrees(m_gyro_roll));
}
}
void HandleEvent(CWiimote &wm)
{
char prefixString[64];
if(wm.Buttons.isJustPressed(CButtons::BUTTON_MINUS))
{
wm.SetMotionSensingMode(CWiimote::OFF);
}
if(wm.Buttons.isJustPressed(CButtons::BUTTON_PLUS))
{
wm.SetMotionSensingMode(CWiimote::ON);
}
if(wm.Buttons.isJustPressed(CButtons::BUTTON_DOWN))
{
wm.IR.SetMode(CIR::OFF);
}
if(wm.Buttons.isJustPressed(CButtons::BUTTON_UP))
{
wm.IR.SetMode(CIR::ON);
}
if(wm.Buttons.isJustPressed(CButtons::BUTTON_RIGHT))
{
wm.EnableMotionPlus(CWiimote::ON);
}
if(wm.Buttons.isJustPressed(CButtons::BUTTON_LEFT))
{
wm.EnableMotionPlus(CWiimote::OFF);
}
if(wm.Buttons.isJustPressed(CButtons::BUTTON_B))
{
wm.ToggleRumble();
}
sprintf(prefixString, "Controller [%i]: ", wm.GetID());
if(wm.Buttons.isPressed(CButtons::BUTTON_A))
{
printf("%s A pressed\n", prefixString);
}
if(wm.Buttons.isPressed(CButtons::BUTTON_B))
{
printf("%s B pressed\n", prefixString);
}
if(wm.Buttons.isPressed(CButtons::BUTTON_UP))
{
printf("%s Up pressed\n", prefixString);
}
if(wm.Buttons.isPressed(CButtons::BUTTON_DOWN))
{
printf("%s Down pressed\n", prefixString);
}
if(wm.Buttons.isPressed(CButtons::BUTTON_LEFT))
{
printf("%s Left pressed\n", prefixString);
}
if(wm.Buttons.isPressed(CButtons::BUTTON_RIGHT))
{
printf("%s Right pressed\n", prefixString);
}
if(wm.Buttons.isPressed(CButtons::BUTTON_MINUS))
{
printf("%s Minus pressed\n", prefixString);
}
if(wm.Buttons.isPressed(CButtons::BUTTON_PLUS))
{
printf("%s Plus pressed\n", prefixString);
}
if(wm.Buttons.isPressed(CButtons::BUTTON_ONE))
{
printf("%s One pressed\n", prefixString);
}
if(wm.Buttons.isPressed(CButtons::BUTTON_TWO))
{
printf("%s Two pressed\n", prefixString);
}
if(wm.Buttons.isPressed(CButtons::BUTTON_HOME))
{
printf("%s Home pressed\n", prefixString);
}
// if the accelerometer is turned on then print angles
if(wm.isUsingACC())
{
float pitch, roll, yaw;
wm.Accelerometer.GetOrientation(pitch, roll, yaw);
printf("%s wiimote roll = %f\n", prefixString, roll);
printf("%s wiimote pitch = %f\n", prefixString, pitch);
printf("%s wiimote yaw = %f\n", prefixString, yaw);
}
// if the Motion Plus is turned on then print angles
if(wm.isUsingMotionPlus()) {
float roll_rate, pitch_rate, yaw_rate;
wm.ExpansionDevice.MotionPlus.Gyroscope.GetRates(roll_rate,pitch_rate,yaw_rate);
printf("%s motion plus roll rate = %f\n", prefixString,roll_rate);
printf("%s motion plus pitch rate = %f\n", prefixString,pitch_rate);
printf("%s motion plus yaw rate = %f\n", prefixString,yaw_rate);
}
// if(IR tracking is on then print the coordinates
if(wm.isUsingIR())
{
std::vector<CIRDot>::iterator i;
int x, y;
int index;
printf("%s Num IR Dots: %i\n", prefixString, wm.IR.GetNumDots());
printf("%s IR State: %u\n", prefixString, wm.IR.GetState());
std::vector<CIRDot>& dots = wm.IR.GetDots();
for(index = 0, i = dots.begin(); i != dots.end(); ++index, ++i)
{
if((*i).isVisible())
{
(*i).GetCoordinate(x, y);
printf("%s IR source %i: (%i, %i)\n", prefixString, index, x, y);
wm.IR.GetCursorPosition(x, y);
printf("%s IR cursor: (%i, %i)\n", prefixString, x, y);
printf("%s IR z distance: %f\n", prefixString, wm.IR.GetDistance());
}
}
}
int exType = wm.ExpansionDevice.GetType();
if(exType == wm.ExpansionDevice.TYPE_NUNCHUK)
{
float pitch, roll, yaw, a_pitch, a_roll;
float angle, magnitude;
CNunchuk &nc = wm.ExpansionDevice.Nunchuk;
sprintf(prefixString, "Nunchuk [%i]: ", wm.GetID());
if(nc.Buttons.isPressed(CNunchukButtons::BUTTON_C))
{
printf("%s C pressed\n", prefixString);
}
if(nc.Buttons.isPressed(CNunchukButtons::BUTTON_Z))
{
printf("%s Z pressed\n", prefixString);
}
nc.Accelerometer.GetOrientation(pitch, roll, yaw);
printf("%s roll = %f\n", prefixString, roll);
printf("%s pitch = %f\n", prefixString, pitch);
printf("%s yaw = %f\n", prefixString, yaw);
nc.Joystick.GetPosition(angle, magnitude);
printf("%s joystick angle = %f\n", prefixString, angle);
printf("%s joystick magnitude = %f\n", prefixString, magnitude);
}
else if(exType == wm.ExpansionDevice.TYPE_CLASSIC)
{
float angle, magnitude;
CClassic &cc = wm.ExpansionDevice.Classic;
sprintf(prefixString, "Classic [%i]: ", wm.GetID());
if(cc.Buttons.isPressed(CClassicButtons::BUTTON_A))
{
printf("%s A pressed\n", prefixString);
}
if(cc.Buttons.isPressed(CClassicButtons::BUTTON_B))
{
printf("%s B pressed\n", prefixString);
}
if(cc.Buttons.isPressed(CClassicButtons::BUTTON_X))
{
printf("%s X pressed\n", prefixString);
}
if(cc.Buttons.isPressed(CClassicButtons::BUTTON_Y))
{
printf("%s Y pressed\n", prefixString);
}
if(cc.Buttons.isPressed(CClassicButtons::BUTTON_LEFT))
{
printf("%s Left pressed\n", prefixString);
}
if(cc.Buttons.isPressed(CClassicButtons::BUTTON_UP))
{
printf("%s Up pressed\n", prefixString);
}
if(cc.Buttons.isPressed(CClassicButtons::BUTTON_RIGHT))
{
printf("%s Right pressed\n", prefixString);
}
if(cc.Buttons.isPressed(CClassicButtons::BUTTON_DOWN))
{
printf("%s Down pressed\n", prefixString);
}
if(cc.Buttons.isPressed(CClassicButtons::BUTTON_PLUS))
{
printf("%s Plus pressed\n", prefixString);
}
if(cc.Buttons.isPressed(CClassicButtons::BUTTON_MINUS))
{
printf("%s Minus pressed\n", prefixString);
}
if(cc.Buttons.isPressed(CClassicButtons::BUTTON_HOME))
{
printf("%s Home pressed\n", prefixString);
}
if(cc.Buttons.isPressed(CClassicButtons::BUTTON_ZL))
{
printf("%s ZL pressed\n", prefixString);
}
if(cc.Buttons.isPressed(CClassicButtons::BUTTON_FULL_L))
{
printf("%s ZR pressed\n", prefixString);
}
if(cc.Buttons.isPressed(CClassicButtons::BUTTON_FULL_R))
{
printf("%s ZR pressed\n", prefixString);
}
printf("%s L button pressed = %f\n", prefixString, cc.GetLShoulderButton());
printf("%s R button pressed = %f\n", prefixString, cc.GetRShoulderButton());
cc.LeftJoystick.GetPosition(angle, magnitude);
printf("%s left joystick angle = %f\n", prefixString, angle);
printf("%s left joystick magnitude = %f\n", prefixString, magnitude);
cc.RightJoystick.GetPosition(angle, magnitude);
printf("%s right joystick angle = %f\n", prefixString, angle);
printf("%s right joystick magnitude = %f\n", prefixString, magnitude);
}
else if(exType == wm.ExpansionDevice.TYPE_GUITAR_HERO_3)
{
float angle, magnitude;
CGuitarHero3 &gh = wm.ExpansionDevice.GuitarHero3;
sprintf(prefixString, "Guitar [%i]: ", wm.GetID());
if(gh.Buttons.isPressed(CGH3Buttons::BUTTON_STRUM_UP))
{
printf("%s Strum Up pressed\n", prefixString);
}
if(gh.Buttons.isPressed(CGH3Buttons::BUTTON_STRUM_DOWN))
{
printf("%s Strum Down pressed\n", prefixString);
}
if(gh.Buttons.isPressed(CGH3Buttons::BUTTON_YELLOW))
{
printf("%s Yellow pressed\n", prefixString);
}
if(gh.Buttons.isPressed(CGH3Buttons::BUTTON_GREEN))
{
printf("%s Green pressed\n", prefixString);
}
if(gh.Buttons.isPressed(CGH3Buttons::BUTTON_BLUE))
{
printf("%s Blue pressed\n", prefixString);
}
if(gh.Buttons.isPressed(CGH3Buttons::BUTTON_RED))
{
printf("%s Red pressed\n", prefixString);
}
if(gh.Buttons.isPressed(CGH3Buttons::BUTTON_ORANGE))
{
printf("%s Orange pressed\n", prefixString);
}
if(gh.Buttons.isPressed(CGH3Buttons::BUTTON_PLUS))
{
printf("%s Plus pressed\n", prefixString);
}
if(gh.Buttons.isPressed(CGH3Buttons::BUTTON_MINUS))
{
printf("%s Minus pressed\n", prefixString);
}
printf("%s whammy bar = %f\n", prefixString, gh.GetWhammyBar());
gh.Joystick.GetPosition(angle, magnitude);
printf("%s joystick angle = %f\n", prefixString, angle);
printf("%s joystick magnitude = %f\n", prefixString, magnitude);
}
else if(exType == wm.ExpansionDevice.TYPE_BALANCE_BOARD)
{
CBalanceBoard &bb = wm.ExpansionDevice.BalanceBoard;
float total, topLeft, topRight, bottomLeft, bottomRight;
bb.WeightSensor.GetWeight(total, topLeft, topRight, bottomLeft, bottomRight);
printf("balance board top left weight: %f\n", topLeft);
printf("balance board top right weight: %f\n", topRight);
printf("balance board bottom left weight: %f\n", bottomLeft);
printf("balance board bottom right weight: %f\n", bottomRight);
printf("balance board total weight: %f\n", total);
}
}