From 752afe178be897525dbd7f17231177aa4ec58ed4 Mon Sep 17 00:00:00 2001
From: dapetcu21 <dapetcu21@gmail.com>
Date: Wed, 25 Aug 2010 11:44:01 +0000
Subject: [PATCH] Fixed wiimote shaking

git-svn-id: https://dolphin-emu.googlecode.com/svn/trunk@6126 8ced0084-cf51-0410-be5f-012b33b47a6e
---
 .../Src/WiimoteEmu/Attachment/Nunchuk.cpp        |  8 ++++----
 .../Src/WiimoteEmu/WiimoteEmu.cpp                | 16 ++++++++--------
 .../Src/WiimoteEmu/WiimoteEmu.h                  |  9 ++++++++-
 3 files changed, 20 insertions(+), 13 deletions(-)

diff --git a/Source/Plugins/Plugin_WiimoteNew/Src/WiimoteEmu/Attachment/Nunchuk.cpp b/Source/Plugins/Plugin_WiimoteNew/Src/WiimoteEmu/Attachment/Nunchuk.cpp
index 6846c00324..446fdecb86 100644
--- a/Source/Plugins/Plugin_WiimoteNew/Src/WiimoteEmu/Attachment/Nunchuk.cpp
+++ b/Source/Plugins/Plugin_WiimoteNew/Src/WiimoteEmu/Attachment/Nunchuk.cpp
@@ -77,7 +77,7 @@ void Nunchuk::GetState(u8* const data, const bool focus)
 		// swing
 		EmulateSwing(&accel, m_swing);
 		// shake
-		EmulateShake(&ncdata->ax, m_shake, m_shake_step);
+		EmulateShake(&accel, m_shake, m_shake_step);
 		// buttons
 		m_buttons->GetState(&ncdata->bt, nunchuk_button_bitmasks);
 	}
@@ -118,9 +118,9 @@ void Nunchuk::GetState(u8* const data, const bool focus)
 
 	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);
+	dt->x=u8(trim(accel.x*(calib->one_g.x-calib->zero_g.x)+calib->zero_g.x));
+	dt->y=u8(trim(accel.y*(calib->one_g.y-calib->zero_g.y)+calib->zero_g.y));
+	dt->z=u8(trim(accel.z*(calib->one_g.z-calib->zero_g.z)+calib->zero_g.z));
 }
 
 
diff --git a/Source/Plugins/Plugin_WiimoteNew/Src/WiimoteEmu/WiimoteEmu.cpp b/Source/Plugins/Plugin_WiimoteNew/Src/WiimoteEmu/WiimoteEmu.cpp
index c7388f2f1d..6f019d77dd 100644
--- a/Source/Plugins/Plugin_WiimoteNew/Src/WiimoteEmu/WiimoteEmu.cpp
+++ b/Source/Plugins/Plugin_WiimoteNew/Src/WiimoteEmu/WiimoteEmu.cpp
@@ -67,11 +67,11 @@ const ReportFeatures reporting_mode_features[] =
 	{ 0, 0, 0, 0, 23 },
 };
 
-void EmulateShake( u8* const accel
+void EmulateShake( AccelData* const accel
 	  , ControllerEmu::Buttons* const buttons_group
 	  , unsigned int* const shake_step )
 {
-	static const u8 shake_data[] = { 0x40, 0x01, 0x40, 0x80, 0xC0, 0xFF, 0xC0, 0x80 };
+	static const double shake_data[] = { -2.5f, -5.0f, -2.5f, 0.0f, 2.5f, 5.0f, 2.5f, 0.0f };
 	static const unsigned int btns[] = { 0x01, 0x02, 0x04 };
 	unsigned int shake = 0;
 
@@ -79,8 +79,8 @@ void EmulateShake( u8* const accel
 	for ( unsigned int i=0; i<3; ++i )
 		if (shake & (1 << i))
 		{
-			accel[i] = shake_data[shake_step[i]++];
-			shake_step[i] %= sizeof(shake_data);
+			(&(accel->x))[i] = shake_data[shake_step[i]++];
+			shake_step[i] %= sizeof(shake_data)/sizeof(double);
 		}
 		else
 			shake_step[i] = 0;
@@ -398,16 +398,16 @@ void Wiimote::GetAccelData(u8* const data, u8* const buttons)
 	if (has_focus)
 	{
 		EmulateSwing(&m_accel, m_swing, is_sideways, is_upright);
-		EmulateShake(data, m_shake, m_shake_step);
+		EmulateShake(&m_accel, m_shake, m_shake_step);
 		// UDP Wiimote
 		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;
+	cx=trim(m_accel.x*(calib->one_g.x-calib->zero_g.x)+calib->zero_g.x);
+	cy=trim(m_accel.y*(calib->one_g.y-calib->zero_g.y)+calib->zero_g.y);
+	cz=trim(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);
diff --git a/Source/Plugins/Plugin_WiimoteNew/Src/WiimoteEmu/WiimoteEmu.h b/Source/Plugins/Plugin_WiimoteNew/Src/WiimoteEmu/WiimoteEmu.h
index a4e282148f..5cc96d7944 100644
--- a/Source/Plugins/Plugin_WiimoteNew/Src/WiimoteEmu/WiimoteEmu.h
+++ b/Source/Plugins/Plugin_WiimoteNew/Src/WiimoteEmu/WiimoteEmu.h
@@ -44,7 +44,7 @@ struct AccelData
 
 extern const ReportFeatures reporting_mode_features[];
 
-void EmulateShake(u8* const accel_data
+void EmulateShake(AccelData* const accel_data
 	  , ControllerEmu::Buttons* const buttons_group
 	  , unsigned int* const shake_step);
 
@@ -56,6 +56,13 @@ void EmulateSwing(AccelData* const accel
 	 , ControllerEmu::Force* const tilt_group
 	 , const bool sideways = false, const bool upright = false);
 
+inline double trim(double a)
+{
+	if (a<=0) return 0;
+	if (a>=255) return 255;
+	return a;
+}
+
 class Wiimote : public ControllerEmu
 {
 public: