ksys/phys: Add ContactPointInfo iterator

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

1 files changed, 47 insertions, 0 deletions

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