Almost match SPositionHistory

asm/MetroidPrime/Enemies/CElitePirate.s

@@ -1809,7 +1809,7 @@ lbl_8022882C:
 /* 80228840 002257A0  90 C1 00 08 */	stw r6, 8(r1)
 /* 80228844 002257A4  90 A1 00 0C */	stw r5, 0xc(r1)
 /* 80228848 002257A8  90 01 00 10 */	stw r0, 0x10(r1)
-/* 8022884C 002257AC  48 07 80 E5 */	bl sub_802a0930
+/* 8022884C 002257AC  48 07 80 E5 */	bl AddValue__16SPositionHistoryFRC9CVector3f
 /* 80228850 002257B0  80 01 00 34 */	lwz r0, 0x34(r1)
 /* 80228854 002257B4  83 E1 00 2C */	lwz r31, 0x2c(r1)
 /* 80228858 002257B8  7C 08 03 A6 */	mtlr r0
@@ -3758,7 +3758,7 @@ lbl_8022A3EC:
 /* 8022A474 002273D4  91 01 00 64 */	stw r8, 0x64(r1)
 /* 8022A478 002273D8  90 E1 00 68 */	stw r7, 0x68(r1)
 /* 8022A47C 002273DC  90 01 00 6C */	stw r0, 0x6c(r1)
-/* 8022A480 002273E0  48 07 63 71 */	bl sub_802a07f0
+/* 8022A480 002273E0  48 07 63 71 */	bl GetValue__16SPositionHistoryFRC9CVector3fRC9CVector3f
 /* 8022A484 002273E4  C0 21 00 70 */	lfs f1, 0x70(r1)
 /* 8022A488 002273E8  38 00 00 00 */	li r0, 0
 /* 8022A48C 002273EC  C0 02 B5 B8 */	lfs f0, lbl_805AD2D8@sda21(r2)
@@ -4974,7 +4974,7 @@ lbl_8022B564:
 /* 8022B5EC 0022854C  91 01 00 4C */	stw r8, 0x4c(r1)
 /* 8022B5F0 00228550  90 E1 00 50 */	stw r7, 0x50(r1)
 /* 8022B5F4 00228554  90 01 00 54 */	stw r0, 0x54(r1)
-/* 8022B5F8 00228558  48 07 51 F9 */	bl sub_802a07f0
+/* 8022B5F8 00228558  48 07 51 F9 */	bl GetValue__16SPositionHistoryFRC9CVector3fRC9CVector3f
 /* 8022B5FC 0022855C  C0 21 00 58 */	lfs f1, 0x58(r1)
 /* 8022B600 00228560  38 00 00 00 */	li r0, 0
 /* 8022B604 00228564  C0 02 B5 B8 */	lfs f0, lbl_805AD2D8@sda21(r2)
@@ -5314,7 +5314,7 @@ lbl_8022BA8C:
 /* 8022BAF0 00228A50  91 01 00 4C */	stw r8, 0x4c(r1)
 /* 8022BAF4 00228A54  90 E1 00 50 */	stw r7, 0x50(r1)
 /* 8022BAF8 00228A58  90 01 00 54 */	stw r0, 0x54(r1)
-/* 8022BAFC 00228A5C  48 07 4C F5 */	bl sub_802a07f0
+/* 8022BAFC 00228A5C  48 07 4C F5 */	bl GetValue__16SPositionHistoryFRC9CVector3fRC9CVector3f
 /* 8022BB00 00228A60  C0 21 00 58 */	lfs f1, 0x58(r1)
 /* 8022BB04 00228A64  38 00 00 00 */	li r0, 0
 /* 8022BB08 00228A68  C0 02 B5 B8 */	lfs f0, lbl_805AD2D8@sda21(r2)
@@ -7627,7 +7627,7 @@ lbl_8022DB1C:
 /* 8022DBB4 0022AB14  D0 1D 08 B8 */	stfs f0, 0x8b8(r29)
 /* 8022DBB8 0022AB18  C0 04 00 08 */	lfs f0, 8(r4)
 /* 8022DBBC 0022AB1C  D0 1D 08 BC */	stfs f0, 0x8bc(r29)
-/* 8022DBC0 0022AB20  48 07 2E 39 */	bl sub_802a09f8
+/* 8022DBC0 0022AB20  48 07 2E 39 */	bl __ct__16SPositionHistoryFf
 /* 8022DBC4 0022AB24  88 1D 09 88 */	lbz r0, 0x988(r29)
 /* 8022DBC8 0022AB28  38 80 00 00 */	li r4, 0
 /* 8022DBCC 0022AB2C  50 80 3E 30 */	rlwimi r0, r4, 7, 0x18, 0x18

asm/MetroidPrime/Enemies/SPositionHistory.s

@@ -2,8 +2,8 @@
 
 .section .text, "ax"
 
-.global sub_802a07f0
-sub_802a07f0:
+.global GetValue__16SPositionHistoryFRC9CVector3fRC9CVector3f
+GetValue__16SPositionHistoryFRC9CVector3fRC9CVector3f:
 /* 802A07F0 0029D750  94 21 FF 80 */	stwu r1, -0x80(r1)
 /* 802A07F4 0029D754  7C 08 02 A6 */	mflr r0
 /* 802A07F8 0029D758  90 01 00 84 */	stw r0, 0x84(r1)
@@ -89,8 +89,8 @@ lbl_802A08F0:
 /* 802A0928 0029D888  38 21 00 80 */	addi r1, r1, 0x80
 /* 802A092C 0029D88C  4E 80 00 20 */	blr
 
-.global sub_802a0930
-sub_802a0930:
+.global AddValue__16SPositionHistoryFRC9CVector3f
+AddValue__16SPositionHistoryFRC9CVector3f:
 /* 802A0930 0029D890  80 C3 00 04 */	lwz r6, 4(r3)
 /* 802A0934 0029D894  2C 06 00 10 */	cmpwi r6, 0x10
 /* 802A0938 0029D898  4C 80 00 20 */	bgelr
@@ -145,8 +145,8 @@ lbl_802A09E8:
 /* 802A09F0 0029D950  90 03 00 04 */	stw r0, 4(r3)
 /* 802A09F4 0029D954  4E 80 00 20 */	blr
 
-.global sub_802a09f8
-sub_802a09f8:
+.global __ct__16SPositionHistoryFf
+__ct__16SPositionHistoryFf:
 /* 802A09F8 0029D958  EC 01 00 72 */	fmuls f0, f1, f1
 /* 802A09FC 0029D95C  38 00 00 00 */	li r0, 0
 /* 802A0A00 0029D960  D0 03 00 00 */	stfs f0, 0(r3)
@@ -160,4 +160,3 @@ lbl_805ADE38:
 	# ROM: 0x3FA6D8
 	.4byte 0
 	.4byte 0
-

include/MetroidPrime/Enemies/SPositionHistory.hpp

@@ -0,0 +1,20 @@
+#ifndef _SPOSITIONHISTORY
+#define _SPOSITIONHISTORY
+
+#include "Kyoto/Math/CVector3f.hpp"
+
+#include "rstl/reserved_vector.hpp"
+
+struct SPositionHistory {
+private:
+  float x0_magSquared;
+  rstl::reserved_vector< CVector3f, 16 > x4_values;
+
+public:
+  explicit SPositionHistory(float mag);
+  CVector3f GetValue(const CVector3f& pos, const CVector3f& face);
+  void AddValue(const CVector3f& pos);
+  void Clear() { x4_values.clear(); }
+};
+
+#endif // _SPOSITIONHISTORY

include/rstl/construct.hpp

@@ -3,6 +3,8 @@
 
 #include "types.h"
 
+#include "Kyoto/Alloc/CMemory.hpp"
+
 namespace rstl {
 template < typename T >
 inline void construct(void* dest, const T& src) {

include/rstl/reserved_vector.hpp

@@ -49,8 +49,14 @@ public:
     ++x0_count;
   }
 
+  void pop_back() {
+    destroy(&data()[x0_count]);
+    --x0_count;
+  }
+
   inline T* data() { return reinterpret_cast< T* >(x4_data); }
   inline const T* data() const { return reinterpret_cast< const T* >(x4_data); }
+  inline bool empty() const { return size() == 0; }
   inline int size() const { return x0_count; }
   inline int capacity() const { return N; }
   inline T& front() { return data()[0]; }

src/MetroidPrime/Enemies/SPositionHistory.cpp

@@ -0,0 +1,32 @@
+#include "MetroidPrime/Enemies/SPositionHistory.hpp"
+
+SPositionHistory::SPositionHistory(float mag) : x0_magSquared(mag * mag) {}
+
+void SPositionHistory::AddValue(const CVector3f& pos) {
+  if (x4_values.size() >= 16) {
+    return;
+  }
+  if (x4_values.size() == 0) {
+    x4_values.push_back(pos);
+    return;
+  }
+  CVector3f diff = pos - x4_values.back();
+  if (diff.MagSquared() > x0_magSquared) {
+    x4_values.push_back(pos);
+  }
+}
+
+CVector3f SPositionHistory::GetValue(const CVector3f& pos, const CVector3f& face) {
+  CVector3f result = CVector3f::Zero();
+
+  while (!x4_values.empty()) {
+    const CVector3f v = x4_values.back() - pos;
+    if (CVector3f::Dot(v, face) > 0.f && v.IsMagnitudeSafe()) {
+      result = v.AsNormalized();
+      break;
+    }
+    x4_values.pop_back();
+  }
+
+  return result;
+}