Added collision avoidance to MoveGlobal routine

mg_obstacles/src/sdf.cpp

@@ -0,0 +1,53 @@
+#include "mg_obstacles/sdf.hpp"
+#include <glm/gtx/norm.hpp>
+
+namespace mg {
+    double boxSdf(glm::dvec2 pos, glm::dvec2 size, glm::dvec2 t) {
+        glm::dvec2 d = glm::abs(t - pos) - size;
+        return glm::length(glm::max(d, 0.0)) + glm::min(glm::max(d.x, d.y), 0.0);
+    }
+
+    double circleSdf(glm::dvec2 pos, glm::dvec2 t) { return glm::length(pos - t); }
+
+    bool inCircleSdf(glm::dvec2 pos, double rad, glm::dvec2 t, double radius) {
+        return glm::length2(pos - t) < glm::pow(rad + radius, 2);
+    }
+
+    bool inBoxSdf(glm::dvec2 pos, glm::dvec2 size, glm::dvec2 t, double radius) {
+        const glm::dvec2 d = glm::abs(t - pos) - size;
+        // return (glm::length2(glm::max(d, 0.0))) < glm::pow(radius - glm::min(glm::max(d.x, d.y), 0.0), 2);
+        return boxSdf(pos, size, t) - radius < 0;
+    }
+
+    double boxToBox(const glm::dvec2 pos,
+                    const glm::dvec2 size,
+                    const glm::dvec2 t,
+                    const glm::dvec2 sizet,
+                    const glm::dmat2 rot_mat) {
+
+        const glm::dmat2 irot_mat = glm::transpose(rot_mat);
+
+        glm::dvec2 pointA1 = rot_mat * (glm::dvec2{ -sizet.x, -sizet.y } * 0.5) + t;
+        glm::dvec2 pointA2 = rot_mat * (glm::dvec2{ sizet.x, -sizet.y } * 0.5) + t;
+        glm::dvec2 pointA3 = rot_mat * (glm::dvec2{ -sizet.x, sizet.y } * 0.5) + t;
+        glm::dvec2 pointA4 = rot_mat * (glm::dvec2{ sizet.x, sizet.y } * 0.5) + t;
+
+        double distance = boxSdf(pos, size * 0.5, pointA1);
+
+        distance = glm::min(boxSdf(pos, size * 0.5, pointA2), distance);
+        distance = glm::min(boxSdf(pos, size * 0.5, pointA3), distance);
+        distance = glm::min(boxSdf(pos, size * 0.5, pointA4), distance);
+
+        glm::dvec2 pointB1 = irot_mat * (glm::dvec2{ -size.x, -size.y } * 0.5 + pos - t);
+        glm::dvec2 pointB2 = irot_mat * (glm::dvec2{ size.x, -size.y } * 0.5 + pos - t);
+        glm::dvec2 pointB3 = irot_mat * (glm::dvec2{ -size.x, size.y } * 0.5 + pos - t);
+        glm::dvec2 pointB4 = irot_mat * (glm::dvec2{ size.x, size.y } * 0.5 + pos - t);
+
+        distance = glm::min(boxSdf({}, sizet * 0.5, pointB1), distance);
+        distance = glm::min(boxSdf({}, sizet * 0.5, pointB2), distance);
+        distance = glm::min(boxSdf({}, sizet * 0.5, pointB3), distance);
+        distance = glm::min(boxSdf({}, sizet * 0.5, pointB4), distance);
+
+        return distance;
+    }
+}