From af7e23ab119eba7c0579796abd288c027edabfa9 Mon Sep 17 00:00:00 2001
From: Paul Oliver <contact@pauloliver.dev>
Date: Thu, 29 Feb 2024 19:20:22 +0100
Subject: Initial commit

---
 src/NR_Utils.cpp | 88 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 88 insertions(+)
 create mode 100644 src/NR_Utils.cpp

(limited to 'src/NR_Utils.cpp')

diff --git a/src/NR_Utils.cpp b/src/NR_Utils.cpp
new file mode 100644
index 0000000..2768dad
--- /dev/null
+++ b/src/NR_Utils.cpp
@@ -0,0 +1,88 @@
+#include <NR_Utils.hpp>
+#include <Agent.hpp>
+#include <Stage.hpp>
+
+using namespace std;
+
+ssize_t
+pMod(ssize_t n, ssize_t d)
+{
+    return (n % d + d) % d;
+}
+
+float
+dist(const b2Vec2& a, const b2Vec2 b)
+{
+    auto x = a.x - b.x;
+    auto y = a.y - b.y;
+
+    return sqrt(x * x + y * y);
+}
+
+float
+dotP(const b2Vec2& a, const b2Vec2 b)
+{
+    return a.x * b.x + a.y * b.y;
+}
+
+float32
+RayCastCallback::ReportFixture(b2Fixture* fixture, const b2Vec2& point, const b2Vec2& normal, float32 fraction)
+{
+    if (fixture->GetFilterData().groupIndex == -1 || fixture->IsSensor()) {
+        return -1.0f;
+    }
+
+    _fixture  = fixture;
+    _fraction = 1.0f - fraction;
+
+    return fraction;
+}
+
+bool
+QueryCallback::ReportFixture(b2Fixture* fixture)
+{
+    _fixture = fixture;
+
+    return false;
+}
+
+bool
+TrapCallback::ReportFixture(b2Fixture* fixture)
+{
+    if (fixture->GetBody()->GetType() != b2_dynamicBody) {
+        return true;
+    }
+
+    Agent& agent    = *static_cast<Agent*>(fixture->GetUserData());
+    agent._isOnTrap = true;
+    agent._body->SetLinearVelocity({ 0.0f, 0.0f });
+    agent._body->SetAngularVelocity(0.0f);
+
+    return true;
+}
+
+void
+ContactListener::BeginContact(b2Contact* contact)
+{
+    b2Fixture* fixA = contact->GetFixtureA();
+    b2Fixture* fixB = contact->GetFixtureB();
+
+    if (!fixA->IsSensor() && !fixB->IsSensor()) {
+        return;
+    }
+
+    Agent*     agent = nullptr;
+    b2Fixture* check = nullptr;
+
+    if (fixA->IsSensor()) {
+        check = fixA;
+        agent = static_cast<Agent*>(fixB->GetUserData());
+    } else {
+        check = fixB;
+        agent = static_cast<Agent*>(fixA->GetUserData());
+    }
+
+    if (check->GetUserData() == agent->_nextCheckPoint) {
+        agent->incrementCheckPoint(*_stage);
+    }
+}
-- 
cgit v1.2.1