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

Source/Core/InputCommon/Src/UDPWiimote.cpp

@@ -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),
+	d(new _d) ,x(0),y(0),z(1.0f),naX(0),naY(0),naZ(-1.0f),nunX(0),nunY(0),
-	pointerX(-0.1),pointerY(-0.1),nunMask(0),mask(0),index(_index), int_port(atoi(_port))
+	pointerX(1001.0f/2),pointerY(0),nunMask(0),mask(0),index(_index), int_port(atoi(_port))
 {
 		
 	static bool sranded=false;

Source/Plugins/Plugin_WiimoteNew/Plugin_WiimoteNew.vcproj

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

Source/Plugins/Plugin_WiimoteNew/Src/WiimoteEmu/Attachment/Nunchuk.cpp

@@ -66,14 +66,16 @@ 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.x=x;
-			accel->y=u8(y*(calib->one_g.y-calib->zero_g.y)+calib->zero_g.y);
+			accel.y=y;
-			accel->z=u8(z*(calib->one_g.z-calib->zero_g.z)+calib->zero_g.z);
+			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);
 }
 
 

Source/Plugins/Plugin_WiimoteNew/Src/WiimoteEmu/MatrixMath.h

@@ -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

Source/Plugins/Plugin_WiimoteNew/Src/WiimoteEmu/UDPTLayer.h

@@ -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->x=x;
-		data->y=u8(y*(calib->one_g.y-calib->zero_g.y)+calib->zero_g.y);
+		data->y=y;
-		data->z=u8(z*(calib->one_g.z-calib->zero_g.z)+calib->zero_g.z);
+		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;

Source/Plugins/Plugin_WiimoteNew/Src/WiimoteEmu/WiimoteEmu.cpp

@@ -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)
+	int sgn[3]={-1,1,1}; //sign fix
-		one_g[fb] *= -1;
-	if (!sideways && upright)
-		one_g[ud] *= -1;
 
-	(&accel->x)[ud] = u8(sin((PI / 2) -
+	if (sideways && !upright)
-		std::max(fabsf(roll), fabsf(pitch))) * one_g[ud] + zero_g[ud]);
+		sgn[fb] *= -1;
-	(&accel->x)[lr] = u8(sin(roll) * -one_g[lr] + zero_g[lr]);
+	if (!sideways && upright)
-	(&accel->x)[fb] = u8(sin(pitch) * one_g[fb] + zero_g[fb]);
+		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
+		static const int camWidth=1024;
-		const float rsin = 0;//sin(tx);
+		static const int camHeight=768;
-		const float rcos = 1;//cos(tx);
+		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);
+			v[i].x=xx*(bndright-bndleft)/2+(bndleft+bndright)/2;
-			const float yyy = (yy * -256 * 0.90f);
+			v[i].y=yy*(bndup-bnddown)/2+(bndup+bnddown)/2;
-
+			v[i].z=0;
-			xx = 512 + xxx * rcos + (144 + yyy) * rsin;
-			yy = 384 + (108 + yyy) * rcos - xxx * rsin;
 		}
 
-		// dot distance of 25 is too little
+		v[0].x-=(zz*0.5+1)*dist1;
-		const unsigned int distance = (unsigned int)(150 + 100 * zz);
+		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);
+#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])
-		x[1] = (unsigned int)(xx + distance * rcos);
+		Matrix rot,tot;
-		x[2] = (unsigned int)(xx - 1.2f * distance * rcos);
+		static Matrix scale;
-		x[3] = (unsigned int)(xx + 1.2f * distance * rcos);
+		static bool isscale=false;
-
+		if (!isscale)
-		y[0] = (unsigned int)(yy - 0.75 * distance * rsin);
+			MatrixScale(scale,1,camWidth/camHeight,1);
-		y[1] = (unsigned int)(yy + 0.75 * distance * rsin);
+		//MatrixRotationByZ(rot,sin,cos);
-		y[2] = (unsigned int)(yy - 0.75 * 1.2f * distance * rsin);
+		MatrixIdentity(rot);
-		y[3] = (unsigned int)(yy + 0.75 * 1.2f * distance * rsin);
+		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)

Source/Plugins/Plugin_WiimoteNew/Src/WiimoteEmu/WiimoteEmu.h

@@ -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 GetAccelData(u8* const data, u8* const buttons);
-	void GetIRData(u8* const data);
+	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;