ksys/phys: Add ContactPointInfo iterator

It's mostly the same thing as LayerContactPointInfo::Iterator.
With some functions inexplicably marked as virtual.

3 files changed, 92 insertions, 4 deletions

diff --git a/data/uking_functions.csv b/data/uking_functions.csv
index d0b49079..4f65df97 100644
--- a/data/uking_functions.csv
+++ b/data/uking_functions.csv
@@ -84113,10 +84113,10 @@ Address,Quality,Size,Name
 0x0000007100fc76e4,O,000004,_ZN4ksys4phys16ContactPointInfoD0Ev
 0x0000007100fc76e8,O,000092,_ZN4ksys4phys16ContactPointInfo11allocPointsEPN4sead4HeapEi
 0x0000007100fc7744,O,000048,_ZN4ksys4phys16ContactPointInfo10freePointsEv
-0x0000007100fc7774,U,000084,phys::SomeClass::ctor
-0x0000007100fc77c8,U,000044,phys::SomeClass::ctor_0
-0x0000007100fc77f4,U,000260,phys::SomeClass::someCalc
-0x0000007100fc78f8,U,000056,phys::SomeClass::x
+0x0000007100fc7774,O,000084,_ZN4ksys4phys16ContactPointInfo8IteratorC1ERKN4sead6BufferIPNS0_12ContactPointEEEi
+0x0000007100fc77c8,O,000044,_ZN4ksys4phys16ContactPointInfo8IteratorC1ERKN4sead6BufferIPNS0_12ContactPointEEEiNS2_5IsEndE
+0x0000007100fc77f4,O,000260,_ZNK4ksys4phys16ContactPointInfo8Iterator16getPointPositionEPN4sead7Vector3IfEENS2_5PointE
+0x0000007100fc78f8,O,000056,_ZNK4ksys4phys16ContactPointInfo8Iterator16getPointPositionENS2_5PointE
 0x0000007100fc7930,O,000140,_ZN4ksys4phys17SensorGroupFilter4makeENS0_12ContactLayer9ValueTypeEPN4sead4HeapE
 0x0000007100fc79bc,O,000004,_ZN4ksys4phys17SensorGroupFilterD1Ev
 0x0000007100fc79c0,O,000008,_ZThn16_N4ksys4phys17SensorGroupFilterD1Ev
diff --git a/src/KingSystem/Physics/System/physContactPointInfo.cpp b/src/KingSystem/Physics/System/physContactPointInfo.cpp
index 0553e383..e5a9cf31 100644
--- a/src/KingSystem/Physics/System/physContactPointInfo.cpp
+++ b/src/KingSystem/Physics/System/physContactPointInfo.cpp
@@ -1,4 +1,5 @@
 #include "KingSystem/Physics/System/physContactPointInfo.h"
+#include "KingSystem/Physics/System/physContactMgr.h"
 #include "KingSystem/Physics/System/physSystem.h"
 
 namespace ksys::phys {
@@ -25,4 +26,50 @@ void ContactPointInfo::freePoints() {
     mPoints.freeBuffer();
 }
 
+ContactPointInfo::Iterator::Iterator(const Points& points, int count)
+    : mPoints(points.getBufferPtr()), mPointsNum(count), mPointsStart(points.getBufferPtr()) {
+    for (int i = 0; i != count; ++i) {
+        if (!mPoints[i]->flags.isOn(ContactPoint::Flag::_1))
+            break;
+        ++mIdx;
+    }
+}
+
+ContactPointInfo::Iterator::Iterator(const Points& points, int count, IsEnd is_end)
+    : mIdx(count), mPoints(points.getBufferPtr()), mPointsNum(count),
+      mPointsStart(points.getBufferPtr()) {}
+
+void ContactPointInfo::Iterator::getPointPosition(sead::Vector3f* out, Point point) const {
+    const float separating_distance = getPoint()->separating_distance;
+    out->e = getPoint()->position.e;
+
+    switch (point) {
+    case Point::BodyA: {
+        if (getPoint()->flags.isOn(ContactPoint::Flag::Penetrating))
+            return;
+        *out += getPoint()->separating_normal * -separating_distance;
+        break;
+    }
+
+    case Point::BodyB: {
+        if (!getPoint()->flags.isOn(ContactPoint::Flag::Penetrating))
+            return;
+        *out += getPoint()->separating_normal * separating_distance;
+        break;
+    }
+
+    case Point::Midpoint:
+    default: {
+        *out += getPoint()->separating_normal * separating_distance * 0.5f;
+        break;
+    }
+    }
+}
+
+sead::Vector3f ContactPointInfo::Iterator::getPointPosition(Point point) const {
+    sead::Vector3f out;
+    getPointPosition(&out, point);
+    return out;
+}
+
 }  // namespace ksys::phys
diff --git a/src/KingSystem/Physics/System/physContactPointInfo.h b/src/KingSystem/Physics/System/physContactPointInfo.h
index 2dc52fd4..82437534 100644
--- a/src/KingSystem/Physics/System/physContactPointInfo.h
+++ b/src/KingSystem/Physics/System/physContactPointInfo.h
@@ -81,6 +81,44 @@ public:
         const RigidBody::CollisionMasks* collision_masks;
     };
 
+    class Iterator {
+    public:
+        enum class IsEnd : bool { Yes = true };
+
+        enum class Point {
+            BodyA,
+            BodyB,
+            Midpoint,
+        };
+
+        Iterator(const Points& points, int count);
+        Iterator(const Points& points, int count, IsEnd is_end);
+
+        Iterator& operator++() {
+            ++mIdx;
+            return *this;
+        }
+
+        virtual void getPointPosition(sead::Vector3f* out, Point point) const;
+        virtual sead::Vector3f getPointPosition(Point point) const;
+
+        const ContactPoint* getPoint() const { return mPoints[mIdx]; }
+        const ContactPoint* operator*() const { return getPoint(); }
+
+        friend bool operator==(const Iterator& lhs, const Iterator& rhs) {
+            return lhs.mIdx == rhs.mIdx;
+        }
+        friend bool operator!=(const Iterator& lhs, const Iterator& rhs) {
+            return !operator==(lhs, rhs);
+        }
+
+    private:
+        int mIdx = 0;
+        const ContactPoint* const* mPoints = nullptr;
+        int mPointsNum = 0;
+        const ContactPoint* const* mPointsStart = nullptr;
+    };
+
     using ContactCallback = sead::IDelegate2R<ShouldDisableContact*, const Event&, bool>;
 
     static ContactPointInfo* make(sead::Heap* heap, int num, const sead::SafeString& name, int a,
@@ -95,6 +133,9 @@ public:
     ContactCallback* getContactCallback() const { return mContactCallback; }
     void setContactCallback(ContactCallback* cb) { mContactCallback = cb; }
 
+    auto begin() const { return Iterator(mPoints, mNumContactPoints); }
+    auto end() const { return Iterator(mPoints, mNumContactPoints, Iterator::IsEnd::Yes); }
+
 private:
     friend class ContactMgr;