From 502aa461142ea5976c0ea956479a3d7d411a7bc6 Mon Sep 17 00:00:00 2001 From: PositiveBeat Date: Fri, 24 Feb 2023 13:34:35 +0100 Subject: [PATCH 01/12] Added function getSimulator() --- swarmsim/include/application/SwarmSimulator.h | 2 ++ swarmsim/src/application/SwarmSimulator.cpp | 5 +++++ 2 files changed, 7 insertions(+) diff --git a/swarmsim/include/application/SwarmSimulator.h b/swarmsim/include/application/SwarmSimulator.h index 9ac5d59..197c877 100644 --- a/swarmsim/include/application/SwarmSimulator.h +++ b/swarmsim/include/application/SwarmSimulator.h @@ -26,6 +26,8 @@ namespace SwarmSim void loopOnce(); void setRobotPosition(std::string, glm::dvec3); + std::shared_ptr getSimulator(); + private: bool mHeadless; std::unique_ptr mSimulatorView; diff --git a/swarmsim/src/application/SwarmSimulator.cpp b/swarmsim/src/application/SwarmSimulator.cpp index 667a412..ed84e47 100644 --- a/swarmsim/src/application/SwarmSimulator.cpp +++ b/swarmsim/src/application/SwarmSimulator.cpp @@ -89,4 +89,9 @@ namespace SwarmSim } } + std::shared_ptr SwarmSimulator::getSimulator() + { + return mSimulator; + } + } // namespace SwarmSim From 652ea355c101a5e9aee0582a48956745d3ab5491 Mon Sep 17 00:00:00 2001 From: PositiveBeat Date: Fri, 24 Feb 2023 13:34:55 +0100 Subject: [PATCH 02/12] Added function getNeighbors() --- swarmsim/include/application/SwarmSimulator.h | 1 + swarmsim/src/application/SwarmSimulator.cpp | 18 ++++++++++++++++++ 2 files changed, 19 insertions(+) diff --git a/swarmsim/include/application/SwarmSimulator.h b/swarmsim/include/application/SwarmSimulator.h index 197c877..5eff639 100644 --- a/swarmsim/include/application/SwarmSimulator.h +++ b/swarmsim/include/application/SwarmSimulator.h @@ -27,6 +27,7 @@ namespace SwarmSim void setRobotPosition(std::string, glm::dvec3); std::shared_ptr getSimulator(); + std::map getNeighbors(std::string id, glm::dvec3 position, float radius); private: bool mHeadless; diff --git a/swarmsim/src/application/SwarmSimulator.cpp b/swarmsim/src/application/SwarmSimulator.cpp index ed84e47..5b55160 100644 --- a/swarmsim/src/application/SwarmSimulator.cpp +++ b/swarmsim/src/application/SwarmSimulator.cpp @@ -94,4 +94,22 @@ namespace SwarmSim return mSimulator; } + std::map SwarmSimulator::getNeighbors(std::string id, glm::dvec3 position, float radius) + { + std::map robots = mSimulator->getState()->getRobots(); + std::map neighbors; + + std::copy_if(robots.begin(), robots.end(), std::inserter(neighbors, neighbors.end()), + [&id, &position, &radius](std::pair map_robot) + { + if (id != std::get<0>(map_robot)) + { + return glm::length(std::get<1>(map_robot)->getPosition() - position) < radius; + } + return false; + }); + + return neighbors; + } + } // namespace SwarmSim From 6e7bfa41a52af2ee51722bc649ca2c350b7ff6af Mon Sep 17 00:00:00 2001 From: PositiveBeat Date: Fri, 24 Feb 2023 13:43:42 +0100 Subject: [PATCH 03/12] Added comment and changed argument name --- swarmsim/include/application/SwarmSimulator.h | 2 +- swarmsim/src/application/SwarmSimulator.cpp | 7 ++++--- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/swarmsim/include/application/SwarmSimulator.h b/swarmsim/include/application/SwarmSimulator.h index 5eff639..9446d50 100644 --- a/swarmsim/include/application/SwarmSimulator.h +++ b/swarmsim/include/application/SwarmSimulator.h @@ -27,7 +27,7 @@ namespace SwarmSim void setRobotPosition(std::string, glm::dvec3); std::shared_ptr getSimulator(); - std::map getNeighbors(std::string id, glm::dvec3 position, float radius); + std::map getNeighbors(std::string id, glm::dvec3 position, float range); private: bool mHeadless; diff --git a/swarmsim/src/application/SwarmSimulator.cpp b/swarmsim/src/application/SwarmSimulator.cpp index 5b55160..5efd70a 100644 --- a/swarmsim/src/application/SwarmSimulator.cpp +++ b/swarmsim/src/application/SwarmSimulator.cpp @@ -94,17 +94,18 @@ namespace SwarmSim return mSimulator; } - std::map SwarmSimulator::getNeighbors(std::string id, glm::dvec3 position, float radius) + std::map SwarmSimulator::getNeighbors(std::string id, glm::dvec3 position, float range) { std::map robots = mSimulator->getState()->getRobots(); std::map neighbors; + // Add robot tp list if neighbors, if within range std::copy_if(robots.begin(), robots.end(), std::inserter(neighbors, neighbors.end()), - [&id, &position, &radius](std::pair map_robot) + [&id, &position, &range](std::pair map_robot) { if (id != std::get<0>(map_robot)) { - return glm::length(std::get<1>(map_robot)->getPosition() - position) < radius; + return glm::length(std::get<1>(map_robot)->getPosition() - position) < range; } return false; }); From f214be0eeddd2685ab893879be651ad77970b45f Mon Sep 17 00:00:00 2001 From: PositiveBeat Date: Thu, 27 Apr 2023 18:26:31 +0200 Subject: [PATCH 04/12] Changed camera orientation --- swarmsim/src/ui/SimulatorView.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/swarmsim/src/ui/SimulatorView.cpp b/swarmsim/src/ui/SimulatorView.cpp index 4ced7fc..2b5e02f 100644 --- a/swarmsim/src/ui/SimulatorView.cpp +++ b/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, 150.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 From 5a265e27903cb2d4824abf21a7dad4a84e89d18e Mon Sep 17 00:00:00 2001 From: PositiveBeat Date: Thu, 27 Apr 2023 18:27:27 +0200 Subject: [PATCH 05/12] Added member variables to pass data about scene abstraction --- swarmsim/include/sim/Environment.h | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/swarmsim/include/sim/Environment.h b/swarmsim/include/sim/Environment.h index 6f8fd3e..23587da 100644 --- a/swarmsim/include/sim/Environment.h +++ b/swarmsim/include/sim/Environment.h @@ -18,7 +18,18 @@ namespace SwarmSim Environment(); ~Environment(); + std::vector> scene_; + std::vector scene_types_; + private: + enum AreaTypes + { + OUT_OF_BOUNDS, + AREA_OF_OPERATION, + PLAIN, + FOREST, + LAKE + }; }; } // namespace SwarmSim From fdeccd08e2a8f14bd05fb5d33ef899529029944b Mon Sep 17 00:00:00 2001 From: PositiveBeat Date: Thu, 27 Apr 2023 18:28:22 +0200 Subject: [PATCH 06/12] Added function to draw scene polygons with lines --- swarmsim/include/sim/Environment.h | 4 +++ swarmsim/src/sim/Environment.cpp | 43 +++++++++++++++++++++++++++++- 2 files changed, 46 insertions(+), 1 deletion(-) diff --git a/swarmsim/include/sim/Environment.h b/swarmsim/include/sim/Environment.h index 23587da..2580a76 100644 --- a/swarmsim/include/sim/Environment.h +++ b/swarmsim/include/sim/Environment.h @@ -3,7 +3,9 @@ #pragma once #include +#include +#include "glm/glm.hpp" #include "raylib.h" namespace SwarmSim { @@ -15,6 +17,8 @@ namespace SwarmSim DrawGrid(1000, 1.0f); // Default environment is a simple grid plane }; + void drawScene(); + Environment(); ~Environment(); diff --git a/swarmsim/src/sim/Environment.cpp b/swarmsim/src/sim/Environment.cpp index 711e747..5755760 100644 --- a/swarmsim/src/sim/Environment.cpp +++ b/swarmsim/src/sim/Environment.cpp @@ -7,4 +7,45 @@ Environment::Environment() { Environment::~Environment() { } -} // namespace SwarmSim \ No newline at end of file +} // namespace SwarmSim void Environment::drawScene() + void Environment::drawScene() + { + 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 = YELLOW; + 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++; + } + } + From bb07c5e91c9540a35b22a781587acc233797dbc7 Mon Sep 17 00:00:00 2001 From: PositiveBeat Date: Thu, 27 Apr 2023 18:28:51 +0200 Subject: [PATCH 07/12] Called new drawScene() function from draw() --- swarmsim/include/sim/Environment.h | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/swarmsim/include/sim/Environment.h b/swarmsim/include/sim/Environment.h index 2580a76..c1375f2 100644 --- a/swarmsim/include/sim/Environment.h +++ b/swarmsim/include/sim/Environment.h @@ -14,8 +14,9 @@ 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(); From b45a0d5f3f736f77a9e4631d404fec5536ecc79c Mon Sep 17 00:00:00 2001 From: PositiveBeat Date: Thu, 27 Apr 2023 18:28:58 +0200 Subject: [PATCH 08/12] Formatting --- swarmsim/src/application/SwarmSimulator.cpp | 2 +- swarmsim/src/sim/Environment.cpp | 15 +++++++++------ 2 files changed, 10 insertions(+), 7 deletions(-) diff --git a/swarmsim/src/application/SwarmSimulator.cpp b/swarmsim/src/application/SwarmSimulator.cpp index 5efd70a..7cfe595 100644 --- a/swarmsim/src/application/SwarmSimulator.cpp +++ b/swarmsim/src/application/SwarmSimulator.cpp @@ -99,7 +99,7 @@ namespace SwarmSim std::map robots = mSimulator->getState()->getRobots(); std::map neighbors; - // Add robot tp list if neighbors, if within range + // 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 map_robot) { diff --git a/swarmsim/src/sim/Environment.cpp b/swarmsim/src/sim/Environment.cpp index 5755760..1b6d090 100644 --- a/swarmsim/src/sim/Environment.cpp +++ b/swarmsim/src/sim/Environment.cpp @@ -1,13 +1,15 @@ #include "Environment.h" -namespace SwarmSim { -Environment::Environment() { -} +namespace SwarmSim +{ + Environment::Environment() + { + } -Environment::~Environment() { -} + Environment::~Environment() + { + } -} // namespace SwarmSim void Environment::drawScene() void Environment::drawScene() { size_t poly_id = 0; @@ -49,3 +51,4 @@ Environment::~Environment() { } } +} // namespace SwarmSim \ No newline at end of file From 7845ae5c45b8a890b20fbe4869a5d055c8fa21b4 Mon Sep 17 00:00:00 2001 From: PositiveBeat Date: Fri, 28 Apr 2023 01:37:27 +0200 Subject: [PATCH 09/12] Added heading visualisation to robots --- swarmsim/include/application/SwarmSimulator.h | 1 + swarmsim/include/sim/Robot.h | 3 +++ swarmsim/src/application/SwarmSimulator.cpp | 13 +++++++++++++ swarmsim/src/sim/Robot.cpp | 17 ++++++++++++++++- 4 files changed, 33 insertions(+), 1 deletion(-) diff --git a/swarmsim/include/application/SwarmSimulator.h b/swarmsim/include/application/SwarmSimulator.h index 9446d50..2f0cb6d 100644 --- a/swarmsim/include/application/SwarmSimulator.h +++ b/swarmsim/include/application/SwarmSimulator.h @@ -25,6 +25,7 @@ namespace SwarmSim void loop(); void loopOnce(); void setRobotPosition(std::string, glm::dvec3); + void setRobotHeading(std::string, glm::dvec3); std::shared_ptr getSimulator(); std::map getNeighbors(std::string id, glm::dvec3 position, float range); diff --git a/swarmsim/include/sim/Robot.h b/swarmsim/include/sim/Robot.h index 5a36d47..29bc7b6 100644 --- a/swarmsim/include/sim/Robot.h +++ b/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: diff --git a/swarmsim/src/application/SwarmSimulator.cpp b/swarmsim/src/application/SwarmSimulator.cpp index 7cfe595..aa9e36c 100644 --- a/swarmsim/src/application/SwarmSimulator.cpp +++ b/swarmsim/src/application/SwarmSimulator.cpp @@ -89,6 +89,19 @@ 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 SwarmSimulator::getSimulator() { return mSimulator; diff --git a/swarmsim/src/sim/Robot.cpp b/swarmsim/src/sim/Robot.cpp index 791cc56..63d9b5b 100644 --- a/swarmsim/src/sim/Robot.cpp +++ b/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; From dc2bc061889d81d758d60fcc16e1096ed73af9f0 Mon Sep 17 00:00:00 2001 From: PositiveBeat Date: Fri, 28 Apr 2023 15:08:12 +0200 Subject: [PATCH 10/12] Draw ground station in scene --- swarmsim/include/sim/Environment.h | 1 + swarmsim/src/sim/Environment.cpp | 5 +++++ 2 files changed, 6 insertions(+) diff --git a/swarmsim/include/sim/Environment.h b/swarmsim/include/sim/Environment.h index c1375f2..68c8f7a 100644 --- a/swarmsim/include/sim/Environment.h +++ b/swarmsim/include/sim/Environment.h @@ -25,6 +25,7 @@ namespace SwarmSim std::vector> scene_; std::vector scene_types_; + glm::dvec2 ground_station_position_; private: enum AreaTypes diff --git a/swarmsim/src/sim/Environment.cpp b/swarmsim/src/sim/Environment.cpp index 1b6d090..b227cc1 100644 --- a/swarmsim/src/sim/Environment.cpp +++ b/swarmsim/src/sim/Environment.cpp @@ -12,6 +12,11 @@ namespace SwarmSim void Environment::drawScene() { + + // Ground station + DrawCube({float(ground_station_position_.x), float(ground_station_position_.y), -1}, 2.0, 2.0, 1.0, GREEN); + + // Polygons size_t poly_id = 0; Color colour; From a787acb5fcc5294fb17dd2904b59988cd9fda874 Mon Sep 17 00:00:00 2001 From: PositiveBeat Date: Sun, 30 Apr 2023 22:24:11 +0200 Subject: [PATCH 11/12] Set marked ground station size to be dependent on ground station range --- swarmsim/include/sim/Environment.h | 1 + swarmsim/src/sim/Environment.cpp | 4 ++-- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/swarmsim/include/sim/Environment.h b/swarmsim/include/sim/Environment.h index 68c8f7a..0605b3e 100644 --- a/swarmsim/include/sim/Environment.h +++ b/swarmsim/include/sim/Environment.h @@ -26,6 +26,7 @@ namespace SwarmSim std::vector> scene_; std::vector scene_types_; glm::dvec2 ground_station_position_; + double ground_station_range_; private: enum AreaTypes diff --git a/swarmsim/src/sim/Environment.cpp b/swarmsim/src/sim/Environment.cpp index b227cc1..d701ec9 100644 --- a/swarmsim/src/sim/Environment.cpp +++ b/swarmsim/src/sim/Environment.cpp @@ -14,7 +14,7 @@ namespace SwarmSim { // Ground station - DrawCube({float(ground_station_position_.x), float(ground_station_position_.y), -1}, 2.0, 2.0, 1.0, GREEN); + 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; @@ -31,7 +31,7 @@ namespace SwarmSim colour = RED; break; case PLAIN: - colour = YELLOW; + colour = ORANGE; break; case FOREST: colour = GREEN; From 8b32432c7b067e7ceb4b0ae6f36db74758939fde Mon Sep 17 00:00:00 2001 From: PositiveBeat Date: Tue, 16 May 2023 22:29:05 +0200 Subject: [PATCH 12/12] Zoomed out camera --- swarmsim/src/ui/SimulatorView.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/swarmsim/src/ui/SimulatorView.cpp b/swarmsim/src/ui/SimulatorView.cpp index 2b5e02f..a463cee 100644 --- a/swarmsim/src/ui/SimulatorView.cpp +++ b/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, 0.0f, 150.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