Improved realism of drones

joelbeedle · Feb 22, 2024 · 0e7ab1f · 0e7ab1f
1 parent 92cecb3
commit 0e7ab1f
Show file tree

Hide file tree

Showing 12 changed files with 71 additions and 80 deletions.
diff --git a/include/Drone.h b/include/Drone.h
@@ -16,13 +16,16 @@ class Drone {
   b2Fixture *viewSensor;  // The view range sensor
   SwarmBehaviour *behaviour;
   float viewRange;
+  float obstacleViewRange;
   float maxSpeed;
   float maxForce;
   float radius;
+  float mass;
 
  public:
   Drone(b2World *world, const b2Vec2 &position, SwarmBehaviour *behaviour,
-        float viewRange, float maxSpeed, float maxForce, float radius);
+        float viewRange, float obstacleViewRange, float maxSpeed,
+        float maxForce, float radius, float mass);
   ~Drone();
 
   void update(std::vector<Drone *> &drones);
@@ -41,6 +44,9 @@ class Drone {
   float getViewRange() { return viewRange; }
   void setViewRange(float newRange) { viewRange = newRange; }
 
+  float getObstacleViewRange() { return obstacleViewRange; }
+  void setObstacleViewRange(float newRange) { obstacleViewRange = newRange; }
+
   float getMaxSpeed() { return maxSpeed; }
   void setMaxSpeed(float newSpeed) { maxSpeed = newSpeed; }
 

diff --git a/include/FlockingBehaviour.h b/include/FlockingBehaviour.h
@@ -49,5 +49,4 @@ class FlockingBehaviour : public SwarmBehaviour {
   b2Vec2 align(std::vector<b2Body *> &drones, Drone *currentDrone);
   b2Vec2 separate(std::vector<b2Body *> &drones, Drone *currentDrone);
   b2Vec2 cohere(std::vector<b2Body *> &drones, Drone *currentDrone);
-  void performRayCasting(Drone *currentDrone, RayCastCallback &callback);
 };
diff --git a/include/PheremoneBehaviour.h b/include/PheremoneBehaviour.h
@@ -45,7 +45,6 @@ class PheremoneBehaviour : public SwarmBehaviour {
   }
 
  private:
-  void performRayCasting(Drone *currentDrone, RayCastCallback &callback);
   void updatePheremones();
   void layPheremone(const b2Vec2 &position);
 };
diff --git a/include/SwarmBehaviour.h b/include/SwarmBehaviour.h
@@ -5,6 +5,8 @@
 #include <string>
 #include <unordered_map>
 #include <vector>
+
+#include "RayCastCallback.h"
 class Drone;
 
 struct ParameterDefinition {
@@ -33,4 +35,5 @@ class SwarmBehaviour {
   b2Vec2 avoidDrones(std::vector<b2Body *> &neighbours, Drone *currentDrone);
   b2Vec2 avoidObstacles(std::vector<b2Vec2> &obstaclePoints,
                         Drone *currentDrone);
+  void performRayCasting(Drone *currentDrone, RayCastCallback &callback);
 };
diff --git a/include/UniformRandomWalkBehaviour.h b/include/UniformRandomWalkBehaviour.h
@@ -54,8 +54,6 @@ class UniformRandomWalkBehaviour : public SwarmBehaviour {
   void execute(std::vector<Drone *> &drones, Drone *currentDrone) override;
 
  private:
-  void performRayCasting(Drone *currentDrone, RayCastCallback &callback);
-
   // Function to generate a random time interval between 0 and 10 seconds
   static float generateRandomTimeInterval() {
     return static_cast<float>(std::rand()) / RAND_MAX * 5.0f;

diff --git a/src/Drone.cpp b/src/Drone.cpp
@@ -1,6 +1,7 @@
 // Drone.cpp
 #include "Drone.h"
 
+#include <cmath>
 #include <valarray>
 
 #include "ObjectTypes.h"
@@ -9,12 +10,15 @@
 #define SCREEN_HEIGHT 600
 
 Drone::Drone(b2World *world, const b2Vec2 &position, SwarmBehaviour *behaviour,
-             float viewRange, float maxSpeed, float maxForce, float radius)
+             float viewRange, float obstacleViewRange, float maxSpeed,
+             float maxForce, float radius, float mass)
     : behaviour(behaviour),
       viewRange(viewRange),
+      obstacleViewRange(obstacleViewRange),
       maxSpeed(maxSpeed),
       maxForce(maxForce),
-      radius(radius) {
+      radius(radius),
+      mass(mass) {
   // Create Box2D body
   b2BodyDef bodyDef;
   bodyDef.type = b2_dynamicBody;
@@ -27,14 +31,20 @@ Drone::Drone(b2World *world, const b2Vec2 &position, SwarmBehaviour *behaviour,
 
   b2FixtureDef fixtureDef;
   fixtureDef.shape = &circleShape;
-  fixtureDef.density = 1.0f;
+  float area_m2 = M_PI * pow(radius, 2);
+
+  // Calculating the required density for Box2D
+  float density_box2d = mass / area_m2;
+
+  fixtureDef.density = density_box2d;
   UserData *userData = new UserData();
   userData->type = ObjectType::Drone;  // or ObjectType::Tree for a tree
   userData->drone = this;              // or userData->tree = this for a tree
 
   fixtureDef.userData.pointer = reinterpret_cast<uintptr_t>(userData);
   body->CreateFixture(&fixtureDef);
 
+  // Create tree detecting sensor (downwards camera)
   b2CircleShape sCircleShape;
   sCircleShape.m_radius = viewRange;
 

diff --git a/src/FlockingBehaviour.cpp b/src/FlockingBehaviour.cpp
@@ -46,22 +46,6 @@ void FlockingBehaviour::execute(std::vector<Drone *> &drones,
                            // being transient
 }
 
-void FlockingBehaviour::performRayCasting(Drone *currentDrone,
-                                          RayCastCallback &callback) {
-  // Define the ray casting range and angle
-  float rayRange = currentDrone->getViewRange();
-  ;
-  float deltaAngle = 15.0f;  // dividing the circle into segments
-
-  for (float angle = 0; angle < 360; angle += deltaAngle) {
-    b2Vec2 start = currentDrone->getPosition();
-    b2Vec2 end = start + rayRange * b2Vec2(cosf(angle * (b2_pi / 180.0f)),
-                                           sinf(angle * (b2_pi / 180.0f)));
-
-    currentDrone->getBody()->GetWorld()->RayCast(&callback, start, end);
-  }
-}
-
 b2Vec2 FlockingBehaviour::align(std::vector<b2Body *> &drones,
                                 Drone *currentDrone) {
   b2Vec2 steering(0, 0);

diff --git a/src/PheremoneBehaviour.cpp b/src/PheremoneBehaviour.cpp
@@ -81,22 +81,6 @@ void PheremoneBehaviour::execute(std::vector<Drone *> &drones,
                            // being transient
 }
 
-void PheremoneBehaviour::performRayCasting(Drone *currentDrone,
-                                           RayCastCallback &callback) {
-  // Define the ray casting range and angle
-  float rayRange = currentDrone->getViewRange();
-  ;
-  float deltaAngle = 15.0f;  // dividing the circle into segments
-
-  for (float angle = 0; angle < 360; angle += deltaAngle) {
-    b2Vec2 start = currentDrone->getPosition();
-    b2Vec2 end = start + rayRange * b2Vec2(cosf(angle * (b2_pi / 180.0f)),
-                                           sinf(angle * (b2_pi / 180.0f)));
-
-    currentDrone->getBody()->GetWorld()->RayCast(&callback, start, end);
-  }
-}
-
 void PheremoneBehaviour::layPheremone(const b2Vec2 &position) {
   Pheremone pheremone = {position, 1.0f};
   pheremones[pheremoneCount++] = pheremone;

diff --git a/src/SwarmBehaviour.cpp b/src/SwarmBehaviour.cpp
@@ -66,4 +66,19 @@ b2Vec2 SwarmBehaviour::avoidObstacles(std::vector<b2Vec2> &obstaclePoints,
   }
 
   return steering;
+}
+
+void SwarmBehaviour::performRayCasting(Drone *currentDrone,
+                                       RayCastCallback &callback) {
+  // Define the ray casting range and angle
+  float rayRange = currentDrone->getObstacleViewRange();
+  float deltaAngle = 15.0f;  // dividing the circle into segments
+
+  for (float angle = 0; angle < 360; angle += deltaAngle) {
+    b2Vec2 start = currentDrone->getPosition();
+    b2Vec2 end = start + rayRange * b2Vec2(cosf(angle * (b2_pi / 180.0f)),
+                                           sinf(angle * (b2_pi / 180.0f)));
+
+    currentDrone->getBody()->GetWorld()->RayCast(&callback, start, end);
+  }
 }
diff --git a/src/UniformRandomWalkBehaviour.cpp b/src/UniformRandomWalkBehaviour.cpp
@@ -11,22 +11,6 @@ UniformRandomWalkBehaviour::UniformRandomWalkBehaviour(
   std::srand(std::time(nullptr));  // Initialize random seed
 }
 
-void UniformRandomWalkBehaviour::performRayCasting(Drone *currentDrone,
-                                                   RayCastCallback &callback) {
-  // Define the ray casting range and angle
-  float rayRange = currentDrone->getViewRange();
-  ;
-  float deltaAngle = 15.0f;  // dividing the circle into segments
-
-  for (float angle = 0; angle < 360; angle += deltaAngle) {
-    b2Vec2 start = currentDrone->getPosition();
-    b2Vec2 end = start + rayRange * b2Vec2(cosf(angle * (b2_pi / 180.0f)),
-                                           sinf(angle * (b2_pi / 180.0f)));
-
-    currentDrone->getBody()->GetWorld()->RayCast(&callback, start, end);
-  }
-}
-
 void UniformRandomWalkBehaviour::execute(std::vector<Drone *> &drones,
                                          Drone *currentDrone) {
   // Ensure there is a timer info for the current drone

diff --git a/src/main.cpp b/src/main.cpp
@@ -137,7 +137,7 @@ int main() {
   for (int i = 0; i < 50; i++) {
     drones.push_back(
         new Drone(&world, b2Vec2(rand() % SCREEN_WIDTH, rand() % SCREEN_HEIGHT),
-                  &flockingBehaviour, 5.0f, 10.0f, 0.3f, 1.0f));
+                  &flockingBehaviour, 5.0f, 5.0f, 10.0f, 0.3f, 1.0f, 3.6f));
   }
   int32 counter = 0;
   while (window.isOpen()) {

diff --git a/testbed/tests/DroneSwarmTest.cpp b/testbed/tests/DroneSwarmTest.cpp
@@ -52,16 +52,18 @@ class DroneSwarmTest : public Test {
   // Drone settings
   std::vector<Drone *> drones;
 
-  float viewRange = 10.0f;
-  float maxSpeed = 10.0f;
-  float maxForce = 0.3f;
+  float obstacleViewRange;
+  float viewRange;
+  float maxSpeed;
+  float maxForce;
 
   // Tree settings
   std::vector<Tree *> trees;
   std::vector<Tree *> mappedTrees;
 
   // Visual settings
   bool drawDroneVisualRange = false;
+  bool drawTrees = true;
 
  public:
   DroneSwarmTest() {
@@ -113,8 +115,9 @@ class DroneSwarmTest : public Test {
   }
 
   void initDefaultParameters() {
-    viewRange = 5.0f;
-    maxSpeed = 10.0f;
+    viewRange = 8.0f;
+    obstacleViewRange = 40.0f;
+    maxSpeed = 17.0f;
     maxForce = 0.3f;
 
     flockingParams = {50.0f, 1.6f, 0.8f, 1.6f, 1.2f};
@@ -156,12 +159,14 @@ class DroneSwarmTest : public Test {
   void createDrones(SwarmBehaviour *b) {
     const float margin = 2.0f;  // Define a margin to prevent spawning exactly
                                 // at the border or outside
+    // Drone currently modelled on DJI Matrice 300 RTK
     for (int i = 0; i < DRONE_COUNT; i++) {
       float x = (rand() % static_cast<int>(BORDER_WIDTH - 2 * margin)) + margin;
       float y =
           (rand() % static_cast<int>(BORDER_HEIGHT - 2 * margin)) + margin;
       drones.push_back(new Drone(m_world, b2Vec2(x, y), behaviour, viewRange,
-                                 maxSpeed, maxForce, 1.0f));
+                                 obstacleViewRange, maxSpeed, maxForce, 0.45f,
+                                 6.3f));
     }
   }
 
@@ -242,6 +247,7 @@ class DroneSwarmTest : public Test {
       drone->setMaxForce(maxForce);
       drone->setMaxSpeed(maxSpeed);
       drone->setViewRange(viewRange);
+      drone->setObstacleViewRange(obstacleViewRange);
       drone->updateSensorRange();
     }
   }
@@ -301,6 +307,7 @@ class DroneSwarmTest : public Test {
     // Visual settings
     ImGui::Text("Visual Settings");
     ImGui::Checkbox("Draw Drone visual range", &drawDroneVisualRange);
+    ImGui::Checkbox("Draw Trees", &drawTrees);
 
     if (ImGui::Button("Reset Simulation")) {
       DestroyDrones();
@@ -362,23 +369,25 @@ class DroneSwarmTest : public Test {
               break;
             }
             case ObjectType::Tree: {
-              Tree *tree = userData->tree;
-              b2Vec2 position = body->GetPosition();
-              const b2CircleShape *circleShape =
-                  static_cast<const b2CircleShape *>(fixture->GetShape());
-
-              if (tree->isMapped()) {
-                b2Color customColor =
-                    b2Color(0.0f, 1.0f, 0.0f);  // Custom color for mapped trees
-                g_debugDraw.DrawSolidCircle(position, circleShape->m_radius,
-                                            transform.q.GetXAxis(),
-                                            customColor);
-              } else {
-                b2Color customColor =
-                    b2Color(1.0f, 0.0f, 0.0f);  // Custom color for mapped trees
-                g_debugDraw.DrawSolidCircle(position, circleShape->m_radius,
-                                            transform.q.GetXAxis(),
-                                            customColor);
+              if (drawTrees) {
+                Tree *tree = userData->tree;
+                b2Vec2 position = body->GetPosition();
+                const b2CircleShape *circleShape =
+                    static_cast<const b2CircleShape *>(fixture->GetShape());
+
+                if (tree->isMapped()) {
+                  b2Color customColor = b2Color(
+                      0.0f, 1.0f, 0.0f);  // Custom color for mapped trees
+                  g_debugDraw.DrawSolidCircle(position, circleShape->m_radius,
+                                              transform.q.GetXAxis(),
+                                              customColor);
+                } else {
+                  b2Color customColor = b2Color(
+                      1.0f, 0.0f, 0.0f);  // Custom color for mapped trees
+                  g_debugDraw.DrawSolidCircle(position, circleShape->m_radius,
+                                              transform.q.GetXAxis(),
+                                              customColor);
+                }
               }
               break;
             }