398 lines
11 KiB
C++
398 lines
11 KiB
C++
// Copyright (C) 2003 Dolphin Project.
|
|
|
|
// This program is free software: you can redistribute it and/or modify
|
|
// it under the terms of the GNU General Public License as published by
|
|
// the Free Software Foundation, version 2.0.
|
|
|
|
// This program is distributed in the hope that it will be useful,
|
|
// but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
// GNU General Public License 2.0 for more details.
|
|
|
|
// A copy of the GPL 2.0 should have been included with the program.
|
|
// If not, see http://www.gnu.org/licenses/
|
|
|
|
// Official SVN repository and contact information can be found at
|
|
// http://code.google.com/p/dolphin-emu/
|
|
|
|
#include <vector>
|
|
#include <string>
|
|
|
|
#include "../../../Core/InputCommon/Src/SDL.h" // Core
|
|
#include "../../../Core/InputCommon/Src/XInput.h"
|
|
|
|
#include "Common.h" // Common
|
|
#include "MathUtil.h"
|
|
#include "StringUtil.h" // for ArrayToString()
|
|
#include "IniFile.h"
|
|
#include "pluginspecs_wiimote.h"
|
|
|
|
#include "EmuDefinitions.h" // Local
|
|
#include "main.h"
|
|
#include "wiimote_hid.h"
|
|
#include "EmuSubroutines.h"
|
|
#include "EmuMain.h"
|
|
#include "Encryption.h" // for extension encryption
|
|
#include "Config.h" // for g_Config
|
|
|
|
|
|
namespace WiiMoteEmu
|
|
{
|
|
|
|
|
|
//******************************************************************************
|
|
// Accelerometer functions
|
|
//******************************************************************************
|
|
|
|
/*
|
|
// Test the calculations
|
|
void TiltTest(u8 x, u8 y, u8 z)
|
|
{
|
|
int Roll, Pitch, RollAdj, PitchAdj;
|
|
PitchAccelerometerToDegree(x, y, z, Roll, Pitch, RollAdj, PitchAdj);
|
|
std::string From = StringFromFormat("From: X:%i Y:%i Z:%i Roll:%s Pitch:%s", x, y, z,
|
|
(Roll >= 0) ? StringFromFormat(" %03i", Roll).c_str() : StringFromFormat("%04i", Roll).c_str(),
|
|
(Pitch >= 0) ? StringFromFormat(" %03i", Pitch).c_str() : StringFromFormat("%04i", Pitch).c_str());
|
|
|
|
float _Roll = (float)Roll, _Pitch = (float)Pitch;
|
|
PitchDegreeToAccelerometer(_Roll, _Pitch, x, y, z);
|
|
std::string To = StringFromFormat("%s\nTo: X:%i Y:%i Z:%i Roll:%s Pitch:%s", From.c_str(), x, y, z,
|
|
(_Roll >= 0) ? StringFromFormat(" %03i", (int)_Roll).c_str() : StringFromFormat("%04i", (int)_Roll).c_str(),
|
|
(_Pitch >= 0) ? StringFromFormat(" %03i", (int)_Pitch).c_str() : StringFromFormat("%04i", (int)_Pitch).c_str());
|
|
NOTICE_LOG(CONSOLE, "\n%s", To.c_str());
|
|
}
|
|
*/
|
|
|
|
/* Angles adjustment for the upside down state when both roll and pitch is
|
|
used. When the absolute values of the angles go over 90 the Wiimote is
|
|
upside down and these adjustments are needed. */
|
|
void AdjustAngles(int &Roll, int &Pitch)
|
|
{
|
|
int OldPitch = Pitch;
|
|
|
|
if (abs(Roll) > 90)
|
|
{
|
|
if (Pitch >= 0)
|
|
Pitch = 180 - Pitch; // 15 to 165
|
|
else
|
|
Pitch = -180 - Pitch; // -15 to -165
|
|
}
|
|
|
|
if (abs(OldPitch) > 90)
|
|
{
|
|
if (Roll >= 0)
|
|
Roll = 180 - Roll; // 15 to 165
|
|
else
|
|
Roll = -180 - Roll; // -15 to -165
|
|
}
|
|
}
|
|
|
|
|
|
// Angles to accelerometer values
|
|
void PitchDegreeToAccelerometer(int _Roll, int _Pitch, int &_x, int &_y, int &_z)
|
|
{
|
|
// We need radiands for the math functions
|
|
float Roll = InputCommon::Deg2Rad((float)_Roll);
|
|
float Pitch = InputCommon::Deg2Rad((float)_Pitch);
|
|
// We need float values
|
|
float x = 0.0f, y = 0.0f, z = 0.0f;
|
|
|
|
// In these cases we can use the simple and accurate formula
|
|
if(g_Config.Tilt.Range.Roll && g_Config.Tilt.Range.Pitch == 0)
|
|
{
|
|
x = sin(Roll);
|
|
z = cos(Roll);
|
|
}
|
|
else if (g_Config.Tilt.Range.Pitch && g_Config.Tilt.Range.Roll == 0)
|
|
{
|
|
y = sin(Pitch);
|
|
z = cos(Pitch);
|
|
}
|
|
else if(g_Config.Tilt.Range.Roll && g_Config.Tilt.Range.Pitch)
|
|
{
|
|
// ====================================================
|
|
/* This seems to always produce the exact same combination of x, y, z
|
|
and Roll and Pitch that the real Wiimote produce. There is an
|
|
unlimited amount of x, y, z combinations for any combination of Roll
|
|
and Pitch. But if we select a Z from the smallest of the absolute
|
|
value of cos(Roll) and cos (Pitch) we get the right values. */
|
|
// ---------
|
|
if (abs(cos(Roll)) < abs(cos(Pitch)))
|
|
z = cos(Roll);
|
|
else
|
|
z = cos(Pitch);
|
|
/* I got these from reversing the calculation in
|
|
PitchAccelerometerToDegree() in a math program. */
|
|
float x_num = 2 * tanf(0.5f * Roll) * z;
|
|
float x_den = pow2f(tanf(0.5f * Roll)) - 1;
|
|
x = - (x_num / x_den);
|
|
float y_num = 2 * tanf(0.5f * Pitch) * z;
|
|
float y_den = pow2f(tanf(0.5f * Pitch)) - 1;
|
|
y = - (y_num / y_den);
|
|
}
|
|
|
|
// Multiply with neutral value and its g
|
|
float xg = g_wm.cal_g.x;
|
|
float yg = g_wm.cal_g.y;
|
|
float zg = g_wm.cal_g.z;
|
|
int ix = g_wm.cal_zero.x + (int)(xg * x);
|
|
int iy = g_wm.cal_zero.y + (int)(yg * y);
|
|
int iz = g_wm.cal_zero.z + (int)(zg * z);
|
|
|
|
if (!g_Config.bUpright)
|
|
{
|
|
if(g_Config.Tilt.Range.Roll != 0) _x = ix;
|
|
if(g_Config.Tilt.Range.Pitch != 0) _y = iy;
|
|
_z = iz;
|
|
}
|
|
else // Upright wiimote
|
|
{
|
|
if(g_Config.Tilt.Range.Roll != 0) _x = ix;
|
|
if(g_Config.Tilt.Range.Pitch != 0) _z = iy;
|
|
_y = 0xFF - iz;
|
|
}
|
|
|
|
// Direct mapping for swing, from analog stick to accelerometer
|
|
if (g_Config.Tilt.Range.Roll == 0)
|
|
{
|
|
_x -= _Roll;
|
|
}
|
|
if (g_Config.Tilt.Range.Pitch == 0)
|
|
{
|
|
if (!g_Config.bUpright)
|
|
_z -= _Pitch;
|
|
else // Upright wiimote
|
|
_y += _Pitch;
|
|
}
|
|
}
|
|
|
|
// Accelerometer to roll and pitch angles
|
|
float AccelerometerToG(float Current, float Neutral, float G)
|
|
{
|
|
float _G = (Current - Neutral) / G;
|
|
return _G;
|
|
}
|
|
|
|
void PitchAccelerometerToDegree(u8 _x, u8 _y, u8 _z, int &_Roll, int &_Pitch, int &_RollAdj, int &_PitchAdj)
|
|
{
|
|
// Definitions
|
|
float Roll = 0, Pitch = 0;
|
|
|
|
// Calculate how many g we are from the neutral
|
|
float x = AccelerometerToG((float)_x, (float)g_wm.cal_zero.x, (float)g_wm.cal_g.x);
|
|
float y = AccelerometerToG((float)_y, (float)g_wm.cal_zero.y, (float)g_wm.cal_g.y);
|
|
float z = AccelerometerToG((float)_z, (float)g_wm.cal_zero.z, (float)g_wm.cal_g.z);
|
|
|
|
if (!g_Config.bUpright)
|
|
{
|
|
// If it is over 1g then it is probably accelerating and may not reliable
|
|
//if (abs(accel->x - ac->cal_zero.x) <= ac->cal_g.x)
|
|
{
|
|
// Calculate the degree
|
|
Roll = InputCommon::Rad2Deg(atan2(x, z));
|
|
}
|
|
|
|
//if (abs(_y - g_wm.cal_zero.y) <= g_wm.cal_g.y)
|
|
{
|
|
// Calculate the degree
|
|
Pitch = InputCommon::Rad2Deg(atan2(y, z));
|
|
}
|
|
}
|
|
else
|
|
{
|
|
//if (abs(accel->z - ac->cal_zero.z) <= ac->cal_g.x)
|
|
{
|
|
// Calculate the degree
|
|
Roll = InputCommon::Rad2Deg(atan2(z, -y));
|
|
}
|
|
|
|
//if (abs(_x - g_wm.cal_zero.x) <= g_wm.cal_g.x)
|
|
{
|
|
// Calculate the degree
|
|
Pitch = InputCommon::Rad2Deg(atan2(-x, -y));
|
|
}
|
|
}
|
|
|
|
_Roll = (int)Roll;
|
|
_Pitch = (int)Pitch;
|
|
|
|
/* Don't allow forces bigger than 1g */
|
|
if (x < -1.0) x = -1.0; else if (x > 1.0) x = 1.0;
|
|
if (y < -1.0) y = -1.0; else if (y > 1.0) y = 1.0;
|
|
if (z < -1.0) z = -1.0; else if (z > 1.0) z = 1.0;
|
|
if (!g_Config.bUpright)
|
|
{
|
|
Roll = InputCommon::Rad2Deg(atan2(x, z));
|
|
Pitch = InputCommon::Rad2Deg(atan2(y, z));
|
|
}
|
|
else
|
|
{
|
|
Roll = InputCommon::Rad2Deg(atan2(z, -y));
|
|
Pitch = InputCommon::Rad2Deg(atan2(-x, -y));
|
|
}
|
|
_RollAdj = (int)Roll;
|
|
_PitchAdj = (int)Pitch;
|
|
}
|
|
|
|
|
|
|
|
//******************************************************************************
|
|
// IR data functions
|
|
//******************************************************************************
|
|
|
|
// Calculate dot positions from the basic 10 byte IR data
|
|
void IRData2DotsBasic(u8 *Data)
|
|
{
|
|
struct SDot* Dot = g_Wiimote_kbd.IR.Dot;
|
|
|
|
Dot[0].Rx = 1023 - (Data[0] | ((Data[2] & 0x30) << 4));
|
|
Dot[0].Ry = Data[1] | ((Data[2] & 0xc0) << 2);
|
|
|
|
Dot[1].Rx = 1023 - (Data[3] | ((Data[2] & 0x03) << 8));
|
|
Dot[1].Ry = Data[4] | ((Data[2] & 0x0c) << 6);
|
|
|
|
Dot[2].Rx = 1023 - (Data[5] | ((Data[7] & 0x30) << 4));
|
|
Dot[2].Ry = Data[6] | ((Data[7] & 0xc0) << 2);
|
|
|
|
Dot[3].Rx = 1023 - (Data[8] | ((Data[7] & 0x03) << 8));
|
|
Dot[3].Ry = Data[9] | ((Data[7] & 0x0c) << 6);
|
|
|
|
/* set each IR spot to visible if spot is in range */
|
|
for (int i = 0; i < 4; ++i)
|
|
{
|
|
if (Dot[i].Ry == 1023)
|
|
{
|
|
Dot[i].Visible = 0;
|
|
}
|
|
else
|
|
{
|
|
Dot[i].Visible = 1;
|
|
Dot[i].Size = 0; /* since we don't know the size, set it as 0 */
|
|
}
|
|
|
|
// For now we let our virtual resolution be the same as the default one
|
|
Dot[i].X = Dot[i].Rx; Dot[i].Y = Dot[i].Ry;
|
|
}
|
|
|
|
// Calculate the other values
|
|
ReorderIRDots();
|
|
IRData2Distance();
|
|
}
|
|
|
|
|
|
// Calculate dot positions from the extented 12 byte IR data
|
|
void IRData2Dots(u8 *Data)
|
|
{
|
|
struct SDot* Dot = g_Wiimote_kbd.IR.Dot;
|
|
|
|
for (int i = 0; i < 4; ++i)
|
|
{
|
|
//Console::Print("Rx: %i\n", Dot[i].Rx);
|
|
|
|
Dot[i].Rx = 1023 - (Data[3*i] | ((Data[(3*i)+2] & 0x30) << 4));
|
|
Dot[i].Ry = Data[(3*i)+1] | ((Data[(3*i)+2] & 0xc0) << 2);
|
|
|
|
Dot[i].Size = Data[(3*i)+2] & 0x0f;
|
|
|
|
/* if in range set to visible */
|
|
if (Dot[i].Ry == 1023)
|
|
Dot[i].Visible = false;
|
|
else
|
|
Dot[i].Visible = true;
|
|
|
|
//Console::Print("Rx: %i\n", Dot[i].Rx);
|
|
|
|
// For now we let our virtual resolution be the same as the default one
|
|
Dot[i].X = Dot[i].Rx; Dot[i].Y = Dot[i].Ry;
|
|
}
|
|
|
|
// Calculate the other values
|
|
ReorderIRDots();
|
|
IRData2Distance();
|
|
}
|
|
|
|
|
|
// Reorder the IR dots according to their x-axis value
|
|
void ReorderIRDots()
|
|
{
|
|
// Create a shortcut
|
|
SDot* Dot = g_Wiimote_kbd.IR.Dot;
|
|
|
|
// Variables
|
|
int i, j, order;
|
|
|
|
// Reset the dot ordering to zero
|
|
for (i = 0; i < 4; ++i)
|
|
Dot[i].Order = 0;
|
|
|
|
// is this just a weird filter+sort?
|
|
for (order = 1; order < 5; ++order)
|
|
{
|
|
i = 0;
|
|
|
|
//
|
|
for (; !Dot[i].Visible || Dot[i].Order; ++i)
|
|
if (i > 4) return;
|
|
|
|
//
|
|
for (j = 0; j < 4; ++j)
|
|
{
|
|
if (Dot[j].Visible && !Dot[j].Order && (Dot[j].X < Dot[i].X))
|
|
i = j;
|
|
}
|
|
|
|
Dot[i].Order = order;
|
|
}
|
|
}
|
|
|
|
|
|
// Calculate dot positions from the extented 12 byte IR data
|
|
void IRData2Distance()
|
|
{
|
|
// Create a shortcut
|
|
struct SDot* Dot = g_Wiimote_kbd.IR.Dot;
|
|
|
|
// Make these ones global
|
|
int i1, i2;
|
|
|
|
for (i1 = 0; i1 < 4; ++i1)
|
|
if (Dot[i1].Visible) break;
|
|
|
|
// Only one dot was visible, we can not calculate the distance
|
|
if (i1 == 4) { g_Wiimote_kbd.IR.Distance = 0; return; }
|
|
|
|
// Look at the next dot
|
|
for (i2 = i1 + 1; i2 < 4; ++i2)
|
|
if (Dot[i2].Visible) break;
|
|
|
|
// Only one dot was visible, we can not calculate the distance
|
|
if (i2 == 4) { g_Wiimote_kbd.IR.Distance = 0; return; }
|
|
|
|
/* For the emulated Wiimote the y distance is always zero so then the distance is the
|
|
simple distance between the x dots, i.e. the sensor bar width */
|
|
int xd = Dot[i2].X - Dot[i1].X;
|
|
int yd = Dot[i2].Y - Dot[i1].Y;
|
|
|
|
// Save the distance
|
|
g_Wiimote_kbd.IR.Distance = (int)sqrt((float)(xd*xd) + (float)(yd*yd));
|
|
}
|
|
|
|
|
|
//******************************************************************************
|
|
// Classic Controller functions
|
|
//******************************************************************************
|
|
|
|
std::string CCData2Values(u8 *Data)
|
|
{
|
|
return StringFromFormat(
|
|
"Tl:%03i Tr:%03i Lx:%03i Ly:%03i Rx:%03i Ry:%03i",
|
|
(((Data[2] & 0x60) >> 2) | ((Data[3] & 0xe0) >> 5)),
|
|
(Data[3] & 0x1f),
|
|
(Data[0] & 0x3f),
|
|
(Data[1] & 0x3f),
|
|
((Data[0] & 0xc0) >> 3) | ((Data[1] & 0xc0) >> 5) | ((Data[2] & 0x80) >> 7),
|
|
(Data[2] & 0x1f));
|
|
}
|
|
|
|
} // WiiMoteEmu
|