Initial commitHEADmaster

1 files changed, 88 insertions, 0 deletions

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);
+    }
+}