Added collision avoidance to MoveGlobal routine

mg_obstacles/src/static_obstacle.cpp

@@ -0,0 +1,46 @@
+#include "mg_obstacles/static_obstacle.hpp"
+#include "mg_obstacles/sdf.hpp"
+
+namespace mg {
+
+    double StaticObstacle::width  = 0.1;
+    double StaticObstacle::height = 0.4;
+
+    StaticObstacle::StaticObstacle(const Json::Value& json) :
+      pos(json.get("x", 1.0).asDouble(), json.get("y", 1.0).asDouble()),
+      horizontal(json.get("horizontal", true).asBool()) { }
+
+    double StaticObstacle::distance(glm::dvec2 position) const {
+        const double width  = StaticObstacle::width;
+        const double height = StaticObstacle::height;
+        if (!horizontal) {
+            return boxSdf(pos + 0.5 * glm::dvec2{ width, -height }, glm::dvec2{ width, height } * 0.5, position);
+        } else {
+            return boxSdf(pos + 0.5 * glm::dvec2{ height, -width }, glm::dvec2{ height, width } * 0.5, position);
+        }
+    }
+
+    double StaticObstacle::distanceBox(glm::dvec2 position, glm::dvec2 size, glm::dmat2 rot_mat) const {
+        const double width  = StaticObstacle::width;
+        const double height = StaticObstacle::height;
+        if (!horizontal) {
+            return boxToBox(
+              pos + 0.5 * glm::dvec2{ width, -height }, glm::dvec2{ width, height }, position, size, rot_mat);
+        } else {
+            return boxToBox(
+              pos + 0.5 * glm::dvec2{ height, -width }, glm::dvec2{ height, width }, position, size, rot_mat);
+        }
+    }
+
+    bool StaticObstacle::contains(glm::dvec2 position, double radius) const {
+        const double width  = StaticObstacle::width;
+        const double height = StaticObstacle::height;
+        if (!horizontal) {
+            return inBoxSdf(
+              pos + 0.5 * glm::dvec2{ width, -height }, glm::dvec2{ width, height } * 0.5, position, radius);
+        } else {
+            return inBoxSdf(
+              pos + 0.5 * glm::dvec2{ height, -width }, glm::dvec2{ height, width } * 0.5, position, radius);
+        }
+    }
+}