Match and link CRumbleGenerator, nearly match CRumbleVoice

Former-commit-id: bd48696bf3

asm/Kyoto/Input/CRumbleGenerator.s

@@ -2,8 +2,8 @@
 
 .section .text, "ax"
 
-.global SetDisabled__14CRumbleManagerFb
+.global SetDisabled__16CRumbleGeneratorFb
-SetDisabled__14CRumbleManagerFb:
+SetDisabled__16CRumbleGeneratorFb:
 /* 80369D00 00366C60  94 21 FF F0 */	stwu r1, -0x10(r1)
 /* 80369D04 00366C64  7C 08 02 A6 */	mflr r0
 /* 80369D08 00366C68  90 01 00 14 */	stw r0, 0x14(r1)
@@ -52,8 +52,8 @@ lbl_80369D84:
 /* 80369DA0 00366D00  2C 1C 00 04 */	cmpwi r28, 4
 /* 80369DA4 00366D04  3B DE 00 04 */	addi r30, r30, 4
 /* 80369DA8 00366D08  41 80 FF DC */	blt lbl_80369D84
-/* 80369DAC 00366D0C  3C 60 80 3E */	lis r3, lbl_803D8548@ha
+/* 80369DAC 00366D0C  3C 60 80 3E */	lis r3, kStopAll__16CRumbleGenerator@ha
-/* 80369DB0 00366D10  38 63 85 48 */	addi r3, r3, lbl_803D8548@l
+/* 80369DB0 00366D10  38 63 85 48 */	addi r3, r3, kStopAll__16CRumbleGenerator@l
 /* 80369DB4 00366D14  48 01 CD 71 */	bl PADControlAllMotors
 /* 80369DB8 00366D18  E3 E1 00 28 */	psq_l f31, 40(r1), 0, qr0
 /* 80369DBC 00366D1C  80 01 00 34 */	lwz r0, 0x34(r1)
@@ -388,8 +388,8 @@ lbl_805AEB1C:
 
 .section .rodata
 .balign 8
-.global lbl_803D8548
+.global kStopAll__16CRumbleGenerator
-lbl_803D8548:
+kStopAll__16CRumbleGenerator:
 	# ROM: 0x3D5548
 	.4byte 0x00000002
 	.4byte 0x00000002

asm/MetroidPrime/CStateManager.s

@@ -590,7 +590,7 @@ lbl_80043FAC:
 /* 80043FB4 00040F14  54 00 CF FF */	rlwinm. r0, r0, 0x19, 0x1f, 0x1f
 /* 80043FB8 00040F18  41 82 00 34 */	beq lbl_80043FEC
 /* 80043FBC 00040F1C  38 80 00 00 */	li r4, 0
-/* 80043FC0 00040F20  48 32 5D 41 */	bl SetDisabled__14CRumbleManagerFb
+/* 80043FC0 00040F20  48 32 5D 41 */	bl SetDisabled__16CRumbleGeneratorFb
 /* 80043FC4 00040F24  48 00 00 28 */	b lbl_80043FEC
 lbl_80043FC8:
 /* 80043FC8 00040F28  80 7E 08 8C */	lwz r3, 0x88c(r30)
@@ -598,7 +598,7 @@ lbl_80043FC8:
 /* 80043FD0 00040F30  54 00 CF FF */	rlwinm. r0, r0, 0x19, 0x1f, 0x1f
 /* 80043FD4 00040F34  40 82 00 0C */	bne lbl_80043FE0
 /* 80043FD8 00040F38  38 80 00 01 */	li r4, 1
-/* 80043FDC 00040F3C  48 32 5D 25 */	bl SetDisabled__14CRumbleManagerFb
+/* 80043FDC 00040F3C  48 32 5D 25 */	bl SetDisabled__16CRumbleGeneratorFb
 lbl_80043FE0:
 /* 80043FE0 00040F40  80 7E 08 50 */	lwz r3, 0x850(r30)
 /* 80043FE4 00040F44  38 80 00 01 */	li r4, 1

configure.py

@@ -690,7 +690,7 @@ LIBS = [
             "Kyoto/Animation/CPoseAsTransformsVariableSize",
             ["Kyoto/Input/CRumbleVoice", False],
             ["Kyoto/Input/RumbleAdsr", True],
-            "Kyoto/Input/CRumbleGenerator",
+            ["Kyoto/Input/CRumbleGenerator", True],
             "Kyoto/Audio/SDSPStream",
             ["Kyoto/Audio/g721", True],
             "Kyoto/Audio/CStaticAudioPlayer",

include/Kyoto/Input/CRumbleGenerator.hpp

@@ -8,6 +8,7 @@
 
 class CRumbleGenerator {
 private:
+  static const EMotorState kStopAll[4];
   CRumbleVoice x0_voices[4];
   float xc0_periodTime[4];
   float xd0_onTime[4];
@@ -18,7 +19,7 @@ public:
   CRumbleGenerator();
   ~CRumbleGenerator();
 
-  s16 Rumble(const SAdsrData& adsr, float, ERumblePriority prio, EIOPort port);
+  short Rumble(const SAdsrData& adsr, float, ERumblePriority prio, EIOPort port);
   void Stop(short id, EIOPort port) {
     if (id == -1)
       return;
@@ -28,6 +29,7 @@ public:
   void Deactivate(short, bool);
   void Update(float);
   void HardStopAll();
+  void SetDisabled(const bool disabled);
 };
 
 #endif // _CRUMBLEGENERATOR

include/Kyoto/Input/CRumbleVoice.hpp

@@ -88,10 +88,13 @@ public:
   void HardReset();
   bool UpdateChannel(SAdsrDelta& delta, const SAdsrData& data, float dt);
   bool Update(float dt);
-  uint GetFreeChannel() const;
+  ushort GetFreeChannel() const;
   float GetIntensity() const;
   bool OwnsSustained(short id) const;
   short CreateRumbleHandle(ushort idx);
+  ushort GetChannelId(short handle) const { return handle & 0xf; }
+  ushort GetOwnerId(short handle) const { return ((handle >> 8) & 0xFF); }
+  ERumblePriority GetPriority(uint idx) { return x10_deltas[idx].x1c_priority; }
 };
 
 #endif // _CRUMBLEVOICE

src/Kyoto/Input/CRumbleGenerator.cpp

@@ -0,0 +1,81 @@
+#include "Kyoto/Input/CRumbleGenerator.hpp"
+
+const EMotorState CRumbleGenerator::kStopAll[4] = {
+    kMS_StopHard,
+    kMS_StopHard,
+    kMS_StopHard,
+    kMS_StopHard,
+};
+
+CRumbleGenerator::CRumbleGenerator() : xf0_24_disabled(false) { HardStopAll(); }
+
+CRumbleGenerator::~CRumbleGenerator() { HardStopAll(); }
+
+short CRumbleGenerator::Rumble(const SAdsrData& adsr, float gain, ERumblePriority prio,
+                               EIOPort port) {
+  ushort freeChan = x0_voices[port].GetFreeChannel();
+  if (prio >= x0_voices[port].GetPriority(freeChan)) {
+    xc0_periodTime[port] = 0.f;
+    xd0_onTime[port] = 0.f;
+    return x0_voices[port].Activate(adsr, freeChan, gain, prio);
+  }
+  return -1;
+}
+
+void CRumbleGenerator::Update(float dt) {
+  if (!xf0_24_disabled) {
+    bool updated = false;
+    for (int i = 0; i < 4; ++i) {
+      const float intensity = x0_voices[i].GetIntensity();
+      if (!x0_voices[i].Update(dt) || intensity <= 0.f) {
+        xc0_periodTime[i] = 0.f;
+        xd0_onTime[i] = 0.f;
+        if (xe0_commandArray[i] != kMS_Stop) {
+          xe0_commandArray[i] = kMS_Stop;
+          updated = true;
+        }
+      } else {
+        xc0_periodTime[i] += dt;
+        if (xc0_periodTime[i] >= 1.f / (30.f * intensity)) {
+          xc0_periodTime[i] = 0.f;
+          if (xe0_commandArray[i] != kMS_Rumble) {
+            xe0_commandArray[i] = kMS_Rumble;
+            updated = true;
+          }
+        } else {
+          xd0_onTime[i] += dt;
+          if (xd0_onTime[i] >= (1.f / 30.f)) {
+            xd0_onTime[i] = 0.f;
+            if (xe0_commandArray[i] != kMS_Stop) {
+              xe0_commandArray[i] = kMS_Stop;
+              updated = true;
+            }
+          }
+        }
+      }
+    }
+    if (updated) {
+      PADControlAllMotors(reinterpret_cast< const u32* >(xe0_commandArray));
+    }
+  }
+}
+
+void CRumbleGenerator::HardStopAll() {
+
+  for (int i = 0; i < 4; ++i) {
+    xc0_periodTime[i] = 0.f;
+    xd0_onTime[i] = 0.f;
+    xe0_commandArray[i] = kMS_Stop;
+    x0_voices[i].HardReset();
+  }
+
+  PADControlAllMotors(reinterpret_cast< const u32* >(kStopAll));
+}
+
+void CRumbleGenerator::SetDisabled(const bool disabled) {
+  if (disabled) {
+    HardStopAll();
+  }
+
+  xf0_24_disabled = disabled;
+}

src/Kyoto/Input/CRumbleVoice.cpp

@@ -22,14 +22,13 @@ short CRumbleVoice::Activate(const SAdsrData& data, ushort idx, float gain, ERum
 }
 
 void CRumbleVoice::Deactivate(short id, bool b1) {
-  if (id == -1)
+  if (id == -1 || !OwnsSustained(id)) {
-    return;
-  if (OwnsSustained(id)) {
-    return;
-  } else if (x2c_usedChannels & (1 << (id & 0xf))) {
-    x10_deltas[(id & 0xf)].x20_phase = SAdsrDelta::kP_Release;
     return;
   }
+
+  if (x2c_usedChannels & (1 << GetChannelId(id))) {
+    x10_deltas[GetChannelId(id)].x20_phase = SAdsrDelta::kP_Release;
+  }
 }
 
 void CRumbleVoice::HardReset() {
@@ -130,14 +129,15 @@ bool CRumbleVoice::Update(float dt) {
   return false;
 }
 
-uint CRumbleVoice::GetFreeChannel() const {
+ushort CRumbleVoice::GetFreeChannel() const {
   for (ushort i = 0; i < 4; ++i) {
     if ((x2c_usedChannels & (1 << i)) == 0) {
-      return i;
+      return (ushort)i;
     }
   }
   return 0;
 }
+
 float CRumbleVoice::GetIntensity() const {
   float ret = x10_deltas[0].x0_curIntensity;
   if (ret < x10_deltas[1].x0_curIntensity) {
@@ -158,17 +158,20 @@ float CRumbleVoice::GetIntensity() const {
 
   return ret;
 }
+
 bool CRumbleVoice::OwnsSustained(short handle) const {
-  ushort idx = (((ushort)handle >> 8) & 0xff);
+  const ushort i = GetChannelId(handle);
-  if (idx < 4)
+  const uint owner = GetOwnerId(handle);
-    return x20_handleIds[idx] == (idx & 0xf);
+  return i < 4 ? x20_handleIds[i] == owner : false;
-  return false;
 }
 
+/* TODO: Fake matched, find real solution */
 short CRumbleVoice::CreateRumbleHandle(ushort idx) {
   ++x2e_lastId;
   if (x2e_lastId == 0)
     x2e_lastId = 1;
-  x20_handleIds[idx] = x2e_lastId;
+  u16 x = idx;
-  return ((x2e_lastId << 8) | idx) & 0xFFFF;
+  u16* h = &x20_handleIds[x];
+  *h = x2e_lastId;
+  return ((x2e_lastId << 8) | x) & 0xFFFF;
 }