dev

swarmsim/include/application/SwarmSimulator.h

@@ -25,6 +25,10 @@ namespace SwarmSim
         void loop();
         void loopOnce();
         void setRobotPosition(std::string, glm::dvec3);
+        void setRobotHeading(std::string, glm::dvec3);
+
+        std::shared_ptr<Simulator> getSimulator();
+        std::map<std::string, SwarmSim::Robot *> getNeighbors(std::string id, glm::dvec3 position, float range);
 
     private:
         bool mHeadless;

swarmsim/include/sim/Environment.h

@@ -3,7 +3,9 @@
 
 #pragma once
 #include <memory>
+#include <vector>
 
+#include "glm/glm.hpp"
 #include "raylib.h"
 namespace SwarmSim
 {
@@ -12,13 +14,29 @@ namespace SwarmSim
     public:
         virtual void draw()
         {
-            DrawGrid(1000, 1.0f); // Default environment is a simple grid plane
-        };
+            // DrawGrid(1000, 1.0f); // Default environment is a simple grid plane
+            drawScene();
+        }
+
+        void drawScene();
 
         Environment();
         ~Environment();
 
+        std::vector<std::vector<glm::dvec2>> scene_;
+        std::vector<int> scene_types_;
+        glm::dvec2 ground_station_position_;
+        double ground_station_range_;
+
     private:
+        enum AreaTypes
+        {
+            OUT_OF_BOUNDS,
+            AREA_OF_OPERATION,
+            PLAIN,
+            FOREST,
+            LAKE
+        };
     };
 
 } // namespace SwarmSim

swarmsim/include/sim/Robot.h

@@ -15,6 +15,7 @@ namespace SwarmSim
         glm::dvec3 taget;
         glm::dvec3 position;
         glm::dvec3 velocity;
+        glm::dvec3 heading;
     };
     class Robot
     {
@@ -31,11 +32,13 @@ namespace SwarmSim
         // Utility
         glm::dvec3 getPosition();
         glm::dvec3 getVelocity();
+        glm::dvec3 getHeading();
         RobotAttributes getAttributes();
         virtual void reset() = 0;
 
         void setPosition(glm::dvec3 pos);
         void setVelocity(glm::dvec3 vel);
+        void setHeading(glm::dvec3 head);
         std::string getId();
 
     protected:

swarmsim/src/application/SwarmSimulator.cpp

@@ -89,4 +89,41 @@ namespace SwarmSim
         }
     }
 
+    void SwarmSimulator::setRobotHeading(std::string id, glm::dvec3 head)
+    {
+
+        if (mSimulator->getState()->hasRobot(id))
+        {
+            mSimulator->getRobot(id)->setHeading(head);
+        }
+        else
+        {
+            SWARMSIM_CORE_ERROR("Did not find robot with id: %s", id);
+        }
+    }
+
+    std::shared_ptr<Simulator> SwarmSimulator::getSimulator()
+    {
+        return mSimulator;
+    }
+
+    std::map<std::string, SwarmSim::Robot *> SwarmSimulator::getNeighbors(std::string id, glm::dvec3 position, float range)
+    {
+        std::map<std::string, SwarmSim::Robot *> robots = mSimulator->getState()->getRobots();
+        std::map<std::string, SwarmSim::Robot *> neighbors;
+
+        // Add robot to list of neighbors, if within range
+        std::copy_if(robots.begin(), robots.end(), std::inserter(neighbors, neighbors.end()),
+                     [&id, &position, &range](std::pair<std::string, SwarmSim::Robot *> map_robot)
+                     {
+                         if (id != std::get<0>(map_robot))
+                         {
+                             return glm::length(std::get<1>(map_robot)->getPosition() - position) < range;
+                         }
+                         return false;
+                     });
+
+        return neighbors;
+    }
+
 } // namespace SwarmSim

swarmsim/src/sim/Environment.cpp

@@ -1,10 +1,59 @@
 #include "Environment.h"
 
-namespace SwarmSim {
-Environment::Environment() {
-}
+namespace SwarmSim
+{
+    Environment::Environment()
+    {
+    }
 
-Environment::~Environment() {
-}
+    Environment::~Environment()
+    {
+    }
 
-}  // namespace SwarmSim
+    void Environment::drawScene()
+    {
+
+        // Ground station
+        DrawCube({float(ground_station_position_.x), float(ground_station_position_.y), -1}, 2 * ground_station_range_, 2 * ground_station_range_, 1.0, GREEN);
+
+        // Polygons
+        size_t poly_id = 0;
+        Color colour;
+
+        for (auto polygon : scene_)
+        {
+            switch (scene_types_[poly_id])
+            {
+            case OUT_OF_BOUNDS:
+                colour = RED;
+                break;
+            case AREA_OF_OPERATION:
+                colour = RED;
+                break;
+            case PLAIN:
+                colour = ORANGE;
+                break;
+            case FOREST:
+                colour = GREEN;
+                break;
+            case LAKE:
+                colour = BLUE;
+                break;
+            }
+
+            for (size_t i = 1; i < polygon.size(); i++)
+            {
+                glm::dvec2 start = polygon[i - 1];
+                glm::dvec2 end = polygon[i];
+                DrawLine(start[0], start[1], end[0], end[1], colour);
+
+                if (i + 1 == polygon.size())
+                {
+                    DrawLine(end[0], end[1], polygon[0][0], polygon[0][1], colour);
+                }
+            }
+            poly_id++;
+        }
+    }
+
+} // namespace SwarmSim

swarmsim/src/sim/Robot.cpp

@@ -26,12 +26,17 @@ namespace SwarmSim
     void Robot::draw()
     {
         auto pos = getPosition();
+        auto head = getHeading();
+        auto headPoint = pos + head * 2.0; // 2 meters long
+
         Vector3 rlPos = {(float)pos.x, (float)pos.y, (float)pos.z};
+        Vector3 rlHead = {(float)headPoint.x, (float)headPoint.y, (float)headPoint.z};
 
         // TODO draw a triangle instead
         // auto direction = glm::normalize(getVelocity());
-        DrawCube(rlPos, 0.5f, .5f, .5f, RED);
+        DrawCube(rlPos, 0.5f, 0.5f, 0.5f, RED);
         DrawCubeWires(rlPos, 0.5f, 0.5f, 0.5f, MAROON);
+        DrawLine3D(rlPos, rlHead, MAROON);
     }
 
     glm::dvec3 Robot::getPosition()
@@ -44,6 +49,11 @@ namespace SwarmSim
         return this->mAttributes.velocity;
     }
 
+    glm::dvec3 Robot::getHeading()
+    {
+        return this->mAttributes.heading;
+    }
+
     RobotAttributes Robot::getAttributes()
     {
         return mAttributes;
@@ -59,6 +69,11 @@ namespace SwarmSim
         mAttributes.velocity = std::move(vel);
     }
 
+    void Robot::setHeading(glm::dvec3 head)
+    {
+        mAttributes.heading = std::move(head);
+    }
+
     std::string Robot::getId()
     {
         return uuid;

swarmsim/src/ui/SimulatorView.cpp

@@ -28,7 +28,7 @@ namespace SwarmSim
 
         // Define the camera to look into our 3d world
         mCamera = {0};
-        mCamera.position = (Vector3){0.0f, 50.0f, -0.01f}; // Camera position
+        mCamera.position = (Vector3){0.0f, 0.0f, 350.01f}; // Camera position
         mCamera.target = (Vector3){0.0f, 0.0f, 0.0f};      // Camera looking at point
         mCamera.up = (Vector3){0.0f, 1.0f, 0.0f};          // Camera up vector (rotation towards target)
         mCamera.fovy = 45.0f;                              // Camera field-of-view Y