9/10-bit accelerometer data interleaved with buttons

some useless IR pointer math... It still doesn't make rotation work
a sign fix when tilting along y axis (twisting the wiimote)

git-svn-id: https://dolphin-emu.googlecode.com/svn/trunk@6121 8ced0084-cf51-0410-be5f-012b33b47a6e
This commit is contained in:
dapetcu21 2010-08-24 08:48:09 +00:00
parent cf5088c37e
commit b7ecd01686
7 changed files with 228 additions and 71 deletions

View File

@ -68,8 +68,8 @@ THREAD_RETURN UDPWiiThread(void* arg)
UDPWiimote::UDPWiimote(const char *_port, const char * name, int _index) :
port(_port), displayName(name),
d(new _d) ,x(0),y(0),z(0),naX(0),naY(0),naZ(0),nunX(0),nunY(0),
pointerX(-0.1),pointerY(-0.1),nunMask(0),mask(0),index(_index), int_port(atoi(_port))
d(new _d) ,x(0),y(0),z(1.0f),naX(0),naY(0),naZ(-1.0f),nunX(0),nunY(0),
pointerX(1001.0f/2),pointerY(0),nunMask(0),mask(0),index(_index), int_port(atoi(_port))
{
static bool sranded=false;

View File

@ -541,6 +541,10 @@
RelativePath=".\Src\WiimoteEmu\Encryption.h"
>
</File>
<File
RelativePath=".\Src\WiimoteEmu\MatrixMath.h"
>
</File>
<File
RelativePath=".\Src\WiimoteEmu\Speaker.cpp"
>

View File

@ -67,13 +67,15 @@ void Nunchuk::GetState(u8* const data, const bool focus)
// stick / not using calibration data for stick, o well
m_stick->GetState(&ncdata->jx, &ncdata->jy, 0x80, focus ? 127 : 0);
AccelData accel;
// tilt
EmulateTilt((wm_accel*)&ncdata->ax, m_tilt, (accel_cal*)&reg[0x20], focus);
EmulateTilt(&accel, m_tilt, focus);
if (focus)
{
// swing
EmulateSwing((wm_accel*)&ncdata->ax, m_swing, (accel_cal*)&reg[0x20]);
EmulateSwing(&accel, m_swing);
// shake
EmulateShake(&ncdata->ax, m_shake, m_shake_step);
// buttons
@ -105,16 +107,20 @@ void Nunchuk::GetState(u8* const data, const bool focus)
}
if (m_udpWrap->updNunAccel)
{
const accel_cal * const calib = (accel_cal*)&reg[0x20];
wm_accel * const accel = (wm_accel*)&ncdata->ax;
float x,y,z;
m_udpWrap->inst->getNunchuckAccel(x,y,z);
accel->x=u8(x*(calib->one_g.x-calib->zero_g.x)+calib->zero_g.x);
accel->y=u8(y*(calib->one_g.y-calib->zero_g.y)+calib->zero_g.y);
accel->z=u8(z*(calib->one_g.z-calib->zero_g.z)+calib->zero_g.z);
accel.x=x;
accel.y=y;
accel.z=z;
}
}
//End UDPNunchuck
wm_accel* dt = (wm_accel*)&ncdata->ax;
accel_cal* calib = (accel_cal*)&reg[0x20];
dt->x=u8(accel.x*(calib->one_g.x-calib->zero_g.x)+calib->zero_g.x);
dt->y=u8(accel.y*(calib->one_g.y-calib->zero_g.y)+calib->zero_g.y);
dt->z=u8(accel.z*(calib->one_g.z-calib->zero_g.z)+calib->zero_g.z);
}

View File

@ -0,0 +1,84 @@
#ifndef MATRIXMATH_H
#define MATRIXMATH_H
#include <math.h>
typedef double Matrix[4][4];
typedef struct
{
double x,y,z;
} Vertex;
inline void MatrixIdentity(Matrix & m)
{
m[0][0]=1; m[0][1]=0; m[0][2]=0; m[0][3]=0;
m[1][0]=0; m[1][1]=1; m[1][2]=0; m[1][3]=0;
m[2][0]=0; m[2][1]=0; m[2][2]=1; m[2][3]=0;
m[3][0]=0; m[3][1]=0; m[3][2]=0; m[3][3]=1;
}
inline void MatrixFrustum(Matrix &m, double l, double r, double b, double t, double n, double f)
{
m[0][0]=2*n/(r-l); m[0][1]=0; m[0][2]=0; m[0][3]=0;
m[1][0]=0; m[1][1]=2*n/(t-b); m[1][2]=0; m[1][3]=0;
m[2][0]=(r+l)/(r-l); m[2][1]=(t+b)/(t-b); m[2][2]=(f+n)/(f-n); m[2][3]=-1;
m[3][0]=0; m[3][1]=0; m[3][2]=2*f*n/(f-n); m[3][3]=0;
}
inline void MatrixPerspective(Matrix & m, double fovy, double aspect, double nplane, double fplane)
{
double xmin,xmax,ymin,ymax;
ymax = nplane * tan(fovy * M_PI / 360.0);
ymin = -ymax;
xmin = ymin * aspect;
xmax = ymax * aspect;
MatrixFrustum(m,xmin,xmax,ymin,ymax,nplane,fplane);
}
inline void MatrixRotationByZ(Matrix &m, double sin, double cos)
{
m[0][0]=cos; m[0][1]=-sin; m[0][2]=0; m[0][3]=0;
m[1][0]=sin; m[1][1]=cos; m[1][2]=0; m[1][3]=0;
m[2][0]=0; m[2][1]=0; m[2][2]=1; m[2][3]=0;
m[3][0]=0; m[3][1]=0; m[3][2]=0; m[3][3]=1;
}
inline void MatrixScale(Matrix &m, double xfact, double yfact, double zfact)
{
m[0][0]=xfact; m[0][1]=0; m[0][2]=0; m[0][3]=0;
m[1][0]=0; m[1][1]=yfact; m[1][2]=0; m[1][3]=0;
m[2][0]=0; m[2][1]=0; m[2][2]=zfact; m[2][3]=0;
m[3][0]=0; m[3][1]=0; m[3][2]=0; m[3][3]=1;
}
inline void MatrixMultiply(Matrix &r, const Matrix &a, const Matrix &b)
{
for (int i=0; i<16; i++)
r[i>>2][i&3]=0.0f;
for (int i=0; i<4; i++)
for (int j=0; j<4; j++)
for (int k=0; k<4; k++)
r[i][j]+=a[i][k]*b[k][j];
}
inline void MatrixTransformVertex(Matrix const & m, Vertex & v)
{
Vertex ov;
double w;
ov.x=v.x;
ov.y=v.y;
ov.z=v.z;
v.x = m[0][0] * ov.x + m[0][1] * ov.y + m[0][2] * ov.z + m[0][3];
v.y = m[1][0] * ov.x + m[1][1] * ov.y + m[1][2] * ov.z + m[1][3];
v.z = m[2][0] * ov.x + m[2][1] * ov.y + m[2][2] * ov.z + m[2][3];
w = m[3][0] * ov.x + m[3][1] * ov.y + m[3][2] * ov.z + m[3][3];
if (w!=0)
{
v.x/=w;
v.y/=w;
v.z/=w;
}
}
#endif

View File

@ -28,22 +28,22 @@ namespace UDPTLayer
*butt|=(mask&UDPWM_BR)?Wiimote::PAD_RIGHT:0;
}
void GetAcceleration(UDPWrapper * m , wm_accel * data, accel_cal * calib)
void GetAcceleration(UDPWrapper * m , WiimoteEmu::AccelData * const data)
{
if (!(m->inst)) return;
if (!(m->updAccel)) return;
float x,y,z;
m->inst->getAccel(x,y,z);
data->x=u8(x*(calib->one_g.x-calib->zero_g.x)+calib->zero_g.x);
data->y=u8(y*(calib->one_g.y-calib->zero_g.y)+calib->zero_g.y);
data->z=u8(z*(calib->one_g.z-calib->zero_g.z)+calib->zero_g.z);
data->x=x;
data->y=y;
data->z=z;
}
void GetIR( UDPWrapper * m, float * x, float * y, float * z)
{
if (!(m->inst)) return;
if (!(m->updIR)) return;
if ((*x>-1)&&(*x<1)&&(*y>-1)&&(*y<1)) return; //the recieved values are used ONLY when the normal pointer is offscreen
if ((*x>=-0.999)&&(*x<=0.999)&&(*y>=-0.999)&&(*y<=0.999)) return; //the recieved values are used ONLY when the normal pointer is offscreen
float _x,_y;
m->inst->getIR(_x,_y);
*x=_x*2-1;

View File

@ -14,6 +14,10 @@
#include "UDPTLayer.h"
inline double round(double x) { return (x-floor(x))>0.5 ? ceil(x) : floor(x); } //because damn MSVSC doesen't comply to C99
#include "MatrixMath.h"
namespace WiimoteEmu
{
@ -38,8 +42,6 @@ static const u8 eeprom_data_16D0[] = {
0x77, 0x88, 0x00, 0x00, 0x2B, 0x01, 0xE8, 0x13
};
#define SWING_INTENSITY 0x40
const ReportFeatures reporting_mode_features[] =
{
//0x30: Core Buttons
@ -58,6 +60,11 @@ const ReportFeatures reporting_mode_features[] =
{ 2, 0, 4, 14, 23 },
//0x37: Core Buttons and Accelerometer with 10 IR bytes and 6 Extension Bytes
{ 2, 4, 7, 17, 23 },
//0x3d: 21 Extension Bytes
{ 0, 0, 0, 2, 23 },
//0x3e / 0x3f: Interleaved Core Buttons and Accelerometer with 36 IR bytes
// UNSUPPORTED
{ 0, 0, 0, 0, 23 },
};
void EmulateShake( u8* const accel
@ -79,21 +86,13 @@ void EmulateShake( u8* const accel
shake_step[i] = 0;
}
void EmulateTilt(wm_accel* const accel
void EmulateTilt(AccelData* const accel
, ControllerEmu::Tilt* const tilt_group
, const accel_cal* const cal
, const bool focus, const bool sideways, const bool upright)
{
float roll, pitch;
tilt_group->GetState( &roll, &pitch, 0, focus ? (PI / 2) : 0 ); // 90 degrees
// this isn't doing anything with those low bits in the calib data, o well
const u8* const zero_g = &cal->zero_g.x;
s8 one_g[3];
for ( unsigned int i=0; i<3; ++i )
one_g[i] = (&cal->one_g.x)[i] - zero_g[i];
unsigned int ud = 0, lr = 0, fb = 0;
// some notes that no one will understand but me :p
@ -107,20 +106,22 @@ void EmulateTilt(wm_accel* const accel
lr = sideways;
fb = upright ? 2 : (sideways ? 0 : 1);
if (sideways && !upright)
one_g[fb] *= -1;
if (!sideways && upright)
one_g[ud] *= -1;
int sgn[3]={-1,1,1}; //sign fix
(&accel->x)[ud] = u8(sin((PI / 2) -
std::max(fabsf(roll), fabsf(pitch))) * one_g[ud] + zero_g[ud]);
(&accel->x)[lr] = u8(sin(roll) * -one_g[lr] + zero_g[lr]);
(&accel->x)[fb] = u8(sin(pitch) * one_g[fb] + zero_g[fb]);
if (sideways && !upright)
sgn[fb] *= -1;
if (!sideways && upright)
sgn[ud] *= -1;
(&accel->x)[ud] = (sin((PI / 2) - std::max(fabsf(roll), fabsf(pitch))))*sgn[ud];
(&accel->x)[lr] = -sin(roll)*sgn[lr];
(&accel->x)[fb] = sin(pitch)*sgn[fb];
}
void EmulateSwing(wm_accel* const accel
#define SWING_INTENSITY 2.5f//-uncalibrated(aprox) 0x40-calibrated
void EmulateSwing(AccelData* const accel
, ControllerEmu::Force* const swing_group
, const accel_cal* const cal
, const bool sideways, const bool upright)
{
float swing[3];
@ -383,27 +384,41 @@ void Wiimote::GetCoreData(u8* const data)
*(wm_core*)data |= m_status.buttons;
}
void Wiimote::GetAccelData(u8* const data)
void Wiimote::GetAccelData(u8* const data, u8* const buttons)
{
const bool has_focus = HAS_FOCUS;
const bool is_sideways = m_options->settings[1]->value != 0;
const bool is_upright = m_options->settings[2]->value != 0;
// ----TILT----
EmulateTilt((wm_accel*)data, m_tilt, (accel_cal*)&m_eeprom[0x16], has_focus, is_sideways, is_upright);
EmulateTilt(&m_accel, m_tilt, has_focus, is_sideways, is_upright);
// ----SWING----
// ----SHAKE----
if (has_focus)
{
EmulateSwing((wm_accel*)data, m_swing, (accel_cal*)&m_eeprom[0x16], is_sideways, is_upright);
EmulateSwing(&m_accel, m_swing, is_sideways, is_upright);
EmulateShake(data, m_shake, m_shake_step);
// UDP Wiimote
UDPTLayer::GetAcceleration(m_udp, (wm_accel*)data, (accel_cal*)&m_eeprom[0x16]);
UDPTLayer::GetAcceleration(m_udp, &m_accel);
}
wm_accel* dt = (wm_accel*)data;
accel_cal* calib = (accel_cal*)&m_eeprom[0x16];
double cx,cy,cz;
cx=m_accel.x*(calib->one_g.x-calib->zero_g.x)+calib->zero_g.x;
cy=m_accel.y*(calib->one_g.y-calib->zero_g.y)+calib->zero_g.y;
cz=m_accel.z*(calib->one_g.z-calib->zero_g.z)+calib->zero_g.z;
dt->x=u8(cx);
dt->y=u8(cy);
dt->z=u8(cz);
if (buttons)
{
buttons[0]|=(u8(cx*4)&3)<<5;
buttons[1]|=((u8(cy*4)&1)<<5)|((u8(cz*4)&1)<<6);
}
}
void Wiimote::GetIRData(u8* const data)
void Wiimote::GetIRData(u8* const data, bool use_accel)
{
const bool has_focus = HAS_FOCUS;
@ -413,40 +428,82 @@ void Wiimote::GetIRData(u8* const data)
if (has_focus)
{
float xx = 10000, yy = 0, zz = 0;
float tx, ty;
double sin,cos;
if (use_accel)
{
double ax,az,len;
ax=m_accel.x;
az=m_accel.z;
len=sqrt(ax*ax+az*az);
if (len)
{
ax/=len;
az/=len; //normalizing the vector
sin=-ax;
cos=az;
} else
{
sin=0;
cos=1;
}
// PanicAlert("%d %d %d\nx:%f\nz:%f\nsin:%f\ncos:%f",accel->x,accel->y,accel->z,ax,az,sin,cos);
//PanicAlert("%d %d %d\n%d %d %d\n%d %d %d",accel->x,accel->y,accel->z,calib->zero_g.x,calib->zero_g.y,calib->zero_g.z,
// calib->one_g.x,calib->one_g.y,calib->one_g.z);
} else
{
sin=0; //m_tilt stuff here (can't figure it out yet....)
cos=1;
}
m_ir->GetState(&xx, &yy, &zz, true);
UDPTLayer::GetIR(m_udp, &xx, &yy, &zz);
m_tilt->GetState(&tx, &ty, 0, PI/2, false);
Vertex v[4];
// disabled cause my math still fails
const float rsin = 0;//sin(tx);
const float rcos = 1;//cos(tx);
static const int camWidth=1024;
static const int camHeight=768;
static const double bndup=-0.315447;
static const double bnddown=0.85;
static const double bndleft=0.443364;
static const double bndright=-0.443364;
static const double dist1=250.0f/camWidth; //this seems the optimal distance for zelda
static const double dist2=1.2f*dist1;
for (int i=0; i<4; i++)
{
const float xxx = (xx * -256 * 0.95f);
const float yyy = (yy * -256 * 0.90f);
xx = 512 + xxx * rcos + (144 + yyy) * rsin;
yy = 384 + (108 + yyy) * rcos - xxx * rsin;
v[i].x=xx*(bndright-bndleft)/2+(bndleft+bndright)/2;
v[i].y=yy*(bndup-bnddown)/2+(bndup+bnddown)/2;
v[i].z=0;
}
// dot distance of 25 is too little
const unsigned int distance = (unsigned int)(150 + 100 * zz);
v[0].x-=(zz*0.5+1)*dist1;
v[1].x+=(zz*0.5+1)*dist1;
v[2].x-=(zz*0.5+1)*dist2;
v[3].x+=(zz*0.5+1)*dist2;
x[0] = (unsigned int)(xx - distance * rcos);
x[1] = (unsigned int)(xx + distance * rcos);
x[2] = (unsigned int)(xx - 1.2f * distance * rcos);
x[3] = (unsigned int)(xx + 1.2f * distance * rcos);
y[0] = (unsigned int)(yy - 0.75 * distance * rsin);
y[1] = (unsigned int)(yy + 0.75 * distance * rsin);
y[2] = (unsigned int)(yy - 0.75 * 1.2f * distance * rsin);
y[3] = (unsigned int)(yy + 0.75 * 1.2f * distance * rsin);
#define printmatrix(m) PanicAlert("%f %f %f %f\n%f %f %f %f\n%f %f %f %f\n%f %f %f %f\n",m[0][0],m[0][1],m[0][2],m[0][3],m[1][0],m[1][1],m[1][2],m[1][3],m[2][0],m[2][1],m[2][2],m[2][3],m[3][0],m[3][1],m[3][2],m[3][3])
Matrix rot,tot;
static Matrix scale;
static bool isscale=false;
if (!isscale)
MatrixScale(scale,1,camWidth/camHeight,1);
//MatrixRotationByZ(rot,sin,cos);
MatrixIdentity(rot);
MatrixMultiply(tot,scale,rot);
for (int i=0; i<4; i++)
{
MatrixTransformVertex(tot,v[i]);
if ((v[i].x<-1)||(v[i].x>1)||(v[i].y<-1)||(v[i].y>1))
continue;
x[i]=(u16)round((v[i].x+1)/2*(camWidth-1));
y[i]=(u16)round((v[i].y+1)/2*(camHeight-1));
}
// PanicAlert("%f %f\n%f %f\n%f %f\n%f %f\n%d %d\n%d %d\n%d %d\n%d %d",
// v[0].x,v[0].y,v[1].x,v[1].y,v[2].x,v[2].y,v[3].x,v[3].y,
// x[0],y[0],x[1],y[1],x[2],y[2],x[3],y[38]);
}
// Fill report with valid data when full handshake was done
if (m_reg_ir->data[0x30])
// ir mode
@ -498,6 +555,7 @@ void Wiimote::GetIRData(u8* const data)
break;
// full
case 5 :
PanicAlert("Full IR report");
// UNSUPPORTED
break;
}
@ -570,11 +628,11 @@ void Wiimote::Update()
// acceleration
if (rptf.accel)
GetAccelData(data + rptf.accel);
GetAccelData(data + rptf.accel, rptf.core?(data+rptf.core):NULL);
// IR
if (rptf.ir)
GetIRData(data + rptf.ir);
GetIRData(data + rptf.ir, (rptf.accel!=NULL));
// extension
if (rptf.ext)

View File

@ -37,20 +37,23 @@ struct ReportFeatures
u8 core, accel, ir, ext, size;
};
struct AccelData
{
double x,y,z;
};
extern const ReportFeatures reporting_mode_features[];
void EmulateShake(u8* const accel_data
, ControllerEmu::Buttons* const buttons_group
, unsigned int* const shake_step);
void EmulateTilt(wm_accel* const accel
void EmulateTilt(AccelData* const accel
, ControllerEmu::Tilt* const tilt_group
, const accel_cal* const cal
, const bool focus, const bool sideways = false, const bool upright = false);
void EmulateSwing(wm_accel* const accel
void EmulateSwing(AccelData* const accel
, ControllerEmu::Force* const tilt_group
, const accel_cal* const cal
, const bool sideways = false, const bool upright = false);
class Wiimote : public ControllerEmu
@ -90,8 +93,8 @@ protected:
void HandleExtensionSwap();
void GetCoreData(u8* const data);
void GetAccelData(u8* const data);
void GetIRData(u8* const data);
void GetAccelData(u8* const data, u8* const buttons);
void GetIRData(u8* const data, bool use_accel);
void GetExtData(u8* const data);
bool HaveExtension() const { return m_extension->active_extension > 0; }
@ -128,6 +131,8 @@ private:
ControlGroup* m_rumble;
Extension* m_extension;
ControlGroup* m_options;
// WiiMote accel data
AccelData m_accel;
//UDPWiimote
UDPWrapper* m_udp;