diff --git a/asm/MetroidPrime/Enemies/CElitePirate.s b/asm/MetroidPrime/Enemies/CElitePirate.s index 9a7e643e..10712fa2 100644 --- a/asm/MetroidPrime/Enemies/CElitePirate.s +++ b/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 diff --git a/asm/MetroidPrime/Enemies/SPositionHistory.s b/asm/MetroidPrime/Enemies/SPositionHistory.s index 2d34825c..5fcdfcb9 100644 --- a/asm/MetroidPrime/Enemies/SPositionHistory.s +++ b/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 - diff --git a/include/MetroidPrime/Enemies/SPositionHistory.hpp b/include/MetroidPrime/Enemies/SPositionHistory.hpp new file mode 100644 index 00000000..e1d689ca --- /dev/null +++ b/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 diff --git a/include/rstl/construct.hpp b/include/rstl/construct.hpp index 7e2e4760..0a8fa6ed 100644 --- a/include/rstl/construct.hpp +++ b/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) { diff --git a/include/rstl/reserved_vector.hpp b/include/rstl/reserved_vector.hpp index 389e2840..73ce84fc 100644 --- a/include/rstl/reserved_vector.hpp +++ b/include/rstl/reserved_vector.hpp @@ -42,15 +42,21 @@ public: } ~reserved_vector() { clear(); } - + void push_back(const T& in) { iterator out = begin() + x0_count; out = in; ++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]; } diff --git a/src/MetroidPrime/Enemies/SPositionHistory.cpp b/src/MetroidPrime/Enemies/SPositionHistory.cpp new file mode 100644 index 00000000..3597e21b --- /dev/null +++ b/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; +}