Merge branch 'phase2' into 'main'

Phase2 Code Review

See merge request sami.hatna/crowd-simulation!3

Author: Sami Hatna

CMakeLists.txt

@@ -12,7 +12,7 @@ find_package(SDL2 REQUIRED)
 include_directories(${SDL2_INCLUDE_DIRS})
 
 add_executable(crowdsim src/main.cpp)
-SET(CMAKE_CXX_FLAGS  "${CMAKE_CXX_FLAGS} -fsycl")
+SET(CMAKE_CXX_FLAGS  "${CMAKE_CXX_FLAGS} -fsycl -fsycl-targets=spir64-gen")
 
 add_subdirectory(external)
 

external/Actor.cpp

@@ -1,15 +1,12 @@
 #include "Actor.hpp"
 
-Actor::Actor(vecType pPos, vecType pVelocity, vecType pDesiredVelocity, vecType pDestination, float pMass, float pRadius, bool pAtDestination, std::array<int, 3> pColor) {
-    pos = pPos;
-    velocity = pVelocity;
-    desiredVelocity = pDesiredVelocity;
-    destination = pDestination;
-    mass = pMass;
-    radius = pRadius;
-    atDestination = pAtDestination;
-    color = pColor;
-}
+Actor::Actor(vecType pPos, vecType pVelocity, float pDesiredSpeed, int pPathId, float pMass, float pRadius, bool pAtDestination, std::array<int, 3> pColor, bool pHeatmapEnabled):
+    pos(pPos), velocity(pVelocity), desiredSpeed(pDesiredSpeed),
+    pathId(pPathId), mass(pMass), radius(pRadius), 
+    atDestination(pAtDestination), color(pColor), heatmapEnabled(pHeatmapEnabled) {
+        variation = {0, 0};
+        destinationIndex = 0;
+    }
 
 SYCL_EXTERNAL vecType Actor::getPos() const {
     return pos;
@@ -19,12 +16,20 @@ SYCL_EXTERNAL vecType Actor::getVelocity() const {
     return velocity;
 }
 
-SYCL_EXTERNAL vecType Actor::getDesiredVelocity() const {
-    return desiredVelocity;
+SYCL_EXTERNAL float Actor::getDesiredSpeed() const {
+    return desiredSpeed;
+}
+
+SYCL_EXTERNAL int Actor::getPathId() const {
+    return pathId;
 }
 
-SYCL_EXTERNAL vecType Actor::getDestination() const {
-    return destination;
+SYCL_EXTERNAL int Actor::getDestinationIndex() const {
+    return destinationIndex;
+}
+
+SYCL_EXTERNAL vecType Actor::getVariation() const {
+    return variation;
 }
 
 SYCL_EXTERNAL float Actor::getMass() const {
@@ -43,6 +48,10 @@ SYCL_EXTERNAL std::array<int, 3> Actor::getColor() const {
     return color;
 }
 
+SYCL_EXTERNAL bool Actor::getHeatmapEnabled() const {
+    return heatmapEnabled;
+}
+
 SYCL_EXTERNAL void Actor::setPos(vecType newPos) {
     pos = newPos;
 }
@@ -51,14 +60,41 @@ SYCL_EXTERNAL void Actor::setVelocity(vecType newVelocity) {
     velocity = newVelocity;
 }
 
-SYCL_EXTERNAL void Actor::setDesiredVelocity(vecType newDesiredVelocity) {
-    desiredVelocity = newDesiredVelocity;
+SYCL_EXTERNAL void Actor::setDesiredSpeed(float newDesiredSpeed) {
+    desiredSpeed = newDesiredSpeed;
 }
 
-SYCL_EXTERNAL void Actor::setDestination(vecType newDestination) {
-    destination = newDestination;
+SYCL_EXTERNAL void Actor::setVariation(vecType newVariation) {
+    variation = newVariation;
 }
 
 SYCL_EXTERNAL void Actor::setAtDestination(bool param) {
     atDestination = param;
 }
+
+SYCL_EXTERNAL void Actor::setColor(std::array<int, 3> newColor) {
+    color = newColor;
+}
+
+SYCL_EXTERNAL void Actor::checkAtDestination(vecType destination, int pathSize) {
+    std::array<float, 4> destinationBoundingBox = {destination[0] + 0.2f, 
+                                                   destination[0] - 0.2f,
+                                                   destination[1] + 0.2f, 
+                                                   destination[1] - 0.2f};
+    if (pos[0] <= destinationBoundingBox[0] && pos[0] >= destinationBoundingBox[1]
+        && pos[1] <= destinationBoundingBox[2] && pos[1] >= destinationBoundingBox[3]) {
+            if (destinationIndex >= PATHALLOCATIONSIZE - 1 || destinationIndex >= pathSize - 1) {
+                this->setAtDestination(true);
+            }
+            else {
+                destinationIndex++;
+            }
+        }
+}
+
+void Actor::refreshVariation() {
+    std::random_device rd;
+    std::mt19937 gen(rd());
+    std::normal_distribution<float> distr{0.0f, 2.0f};
+    this->setVariation({distr(gen), distr(gen)});
+}

external/Actor.hpp

@@ -3,39 +3,50 @@
 
 #include <iostream>
 #include <array>
+#include <random>
+#include <vector>
 #include <sycl/sycl.hpp>
 #include "VectorMaths.hpp"
-
-using vecType = std::array<float, 2>;
+#include "Path.hpp"
 
 class Actor {
 private:
     vecType pos;
     vecType velocity;
-    vecType desiredVelocity;
-    vecType destination;
+    float desiredSpeed;
+    int pathId;
+    int destinationIndex;
+    vecType variation;
     float mass;
     float radius;
     bool atDestination;
     std::array<int, 3> color;
+    bool heatmapEnabled;
 
-public:    
-    Actor(vecType pPos, vecType pVelocity, vecType pDesiredVelocity, vecType pDestination, float pMass, float pRadius, bool pAtDestination, std::array<int, 3> pColor);
+public:
+    Actor(vecType pPos, vecType pVelocity, float pdesiredSpeed, int pPathId, float pMass, float pRadius, bool pAtDestination, std::array<int, 3> pColor, bool pHeatmapEnabled);
 
     SYCL_EXTERNAL vecType getPos() const;
     SYCL_EXTERNAL vecType getVelocity() const;
-    SYCL_EXTERNAL vecType getDesiredVelocity() const;
-    SYCL_EXTERNAL vecType getDestination() const;
+    SYCL_EXTERNAL float getDesiredSpeed() const;
+    SYCL_EXTERNAL int getPathId() const;
+    SYCL_EXTERNAL int getDestinationIndex() const;
+    SYCL_EXTERNAL vecType getVariation() const;
     SYCL_EXTERNAL float getMass() const;
     SYCL_EXTERNAL float getRadius() const;
     SYCL_EXTERNAL bool getAtDestination() const;
     SYCL_EXTERNAL std::array<int, 3> getColor() const;
+    SYCL_EXTERNAL bool getHeatmapEnabled() const;
 
     SYCL_EXTERNAL void setPos(vecType newPos);
     SYCL_EXTERNAL void setVelocity(vecType newVelocity);
-    SYCL_EXTERNAL void setDesiredVelocity(vecType newDesiredVelocity);
-    SYCL_EXTERNAL void setDestination(vecType newDestination);
+    SYCL_EXTERNAL void setDesiredSpeed(float newDesiredSpeed);
+    SYCL_EXTERNAL void setVariation(vecType newVariation);
     SYCL_EXTERNAL void setAtDestination(bool param);
+    SYCL_EXTERNAL void setColor(std::array<int, 3> newColor);
+
+    SYCL_EXTERNAL void checkAtDestination(vecType destination, int pathSize);
+    void refreshVariation();
 };
 
 #endif

external/CMakeLists.txt

@@ -1 +1 @@
-add_library(external Actor.cpp Room.cpp MathHelper.cpp DifferentialEq.cpp VectorMaths.cpp)
+add_library(external Actor.cpp Room.cpp Path.cpp MathHelper.cpp DifferentialEq.cpp VectorMaths.cpp Heatmap.cpp ParseInputFile.cpp)

external/DifferentialEq.cpp

@@ -1,62 +1,71 @@
 #include "DifferentialEq.hpp"
 
-SYCL_EXTERNAL void differentialEq(int z, sycl::accessor<Actor, 1, sycl::access::mode::read_write> actors, sycl::accessor<std::array<vecType, 2>, 1, sycl::access::mode::read> walls, sycl::stream out) {
-    Actor* i = &actors[z];
+SYCL_EXTERNAL void differentialEq(int currentIndex, sycl::accessor<Actor, 1, sycl::access::mode::read_write> actors, sycl::accessor<std::array<vecType, 2>, 1, sycl::access::mode::read> walls, sycl::accessor<Path, 1, sycl::access::mode::read> paths, sycl::stream out) {
+    Actor* currentActor = &actors[currentIndex];
 
-    vecType pos = i->getPos();
+    vecType pos = currentActor->getPos();
 
-    float mi = i->getMass();
-    float v0i = 2.0f;
-    vecType e0i = normalize(getDirectionVector(i->getPos(), i->getDestination()));
-    vecType vi = i->getVelocity();
+    // Calculate personal impulse
+    float mi = currentActor->getMass();
+    float v0i = currentActor->getDesiredSpeed();
+    vecType destination = paths[currentActor->getPathId()].getCheckpoints()[currentActor->getDestinationIndex()];
+    vecType e0i = normalize(getDirectionVector(currentActor->getPos(), destination));
+    vecType vi = currentActor->getVelocity();
 
     vecType personalImpulse = mi * (((v0i * e0i) - vi) / Ti);
-
+    
+    // Calculate forces applied by neighbouring actors
     vecType peopleForces = {0, 0};
     for (int x = 0; x < actors.size(); x++) {
-        Actor j = actors[x];
-        if (z != x && !j.getAtDestination()) {
-            float rij = j.getRadius() + i->getRadius();
-            float dij = magnitude(pos - j.getPos());
-            vecType nij = (pos - j.getPos()) / dij;
-            vecType tij = {-nij[1], nij[0]};
+        Actor neighbour = actors[x];
+        if (currentIndex != x && !neighbour.getAtDestination()) {
+            float rij = neighbour.getRadius() + currentActor->getRadius();
+            vecType currentToNeighbour = pos - neighbour.getPos();
+            float dij = magnitude(currentToNeighbour);
+            vecType nij = (currentToNeighbour) / dij;
+            vecType tij = getTangentialVector(nij);
             float g = dij > rij ? 0 : rij - dij;
-            float deltavtij = dotProduct((j.getVelocity() - i->getVelocity()), tij);
+            float deltavtij = dotProduct((neighbour.getVelocity() - currentActor->getVelocity()), tij);
 
-            peopleForces += (Ai * exp((rij - dij) / Bi) + K1 * g) * nij + (K2 * g * deltavtij * tij);
+            peopleForces += (Ai * sycl::exp((rij - dij) / Bi) + K1 * g) * nij + (K2 * g * deltavtij * tij);
         }
     }
 
+    // Calculate forces applied by walls
     vecType wallForces = {0, 0};
     for (int x = 0; x < walls.size(); x++) {
-        std::array<vecType, 2> w = walls[x];
-        float ri = i->getRadius();
-        std::pair<float, vecType> dAndn = getDistanceAndNiw(pos, w);
+        std::array<vecType, 2> currentWall = walls[x];
+        float ri = currentActor->getRadius();
+        std::pair<float, vecType> dAndn = getDistanceAndNiw(pos, currentWall);
         float diw = dAndn.first;
         float g = diw > ri ? 0 : ri - diw;
         vecType niw = dAndn.second;
-        vecType tiw = {-niw[1], niw[0]};
+        vecType tiw = getTangentialVector(niw);
 
-        wallForces += (Ai * exp((ri - diw) / Bi) + K1 * g) * niw - (K2 * g * dotProduct(vi, tiw) * tiw);
+        wallForces += (Ai * sycl::exp((ri - diw) / Bi) + K1 * g) * niw - (K2 * g * dotProduct(vi, tiw) * tiw);
     }
 
     vecType forceSum = personalImpulse + peopleForces + wallForces;
-    vecType acceleration = forceSum / mi;
 
-    //out << "People Forces: (" << peopleForces[0] << ", " << peopleForces[1] << ")    " << z << sycl::endl;
-    //out << "Wall Forces: (" << wallForces[0] << ", " << wallForces[1] << ")    " << z << sycl::endl;
-    //out << "Acceleration: (" << acceleration[0] << ", " << acceleration[1] << ")    " << z << sycl::endl;
-    //out << "-----------------------" << sycl::endl;
+    // Apply random force variations
+    forceSum += currentActor->getVariation();
+
+    // Color actor according to heatmap
+    if (currentActor->getHeatmapEnabled()) {
+        auto colorVal = sycl::fabs((forceSum[0] + forceSum[1]) / 700.0f);
+        if (colorVal > 1) { colorVal = 1.0f; }
+        auto color = findColor(colorVal);
+        currentActor->setColor({int(color[0]), int(color[1]), int(color[2])});
+    }
 
-    i->setVelocity(vi + forceSum * 0.001);
-    i->setPos(pos + i->getVelocity() * 0.001);
+    // out << "People Forces: (" << peopleForces[0] << ", " << peopleForces[1] << ")    " << z << sycl::endl;
+    // out << "Wall Forces: (" << wallForces[0] << ", " << wallForces[1] << ")    " << z << sycl::endl;
+    // out << "Acceleration: (" << acceleration[0] << ", " << acceleration[1] << ")    " << z << sycl::endl;
+    // out << "-----------------------" << sycl::endl;
 
-    vecType newPos = i->getPos();
-    vecType destination = i->getDestination();
+    vecType acceleration = forceSum / mi;
+    currentActor->setVelocity(vi + acceleration * TIMESTEP);
+    currentActor->setPos(pos + currentActor->getVelocity() * TIMESTEP);
 
-    std::array<float, 4> destinationBoundingBox = {destination[0] + 0.01f, destination[0] - 0.01f, destination[1] + 0.01f, destination[1] - 0.01f};
-    if (newPos[0] <= destinationBoundingBox[0] && newPos[0] >= destinationBoundingBox[1]
-        && pos[1] <= destinationBoundingBox[2] && pos[1] >= destinationBoundingBox[3]) {
-            i->setAtDestination(true);
-        }
+    currentActor->checkAtDestination(destination, paths[currentActor->getPathId()].getPathSize());
 }

external/DifferentialEq.hpp

@@ -7,15 +7,17 @@
 #include "Actor.hpp"
 #include "MathHelper.hpp"
 #include "VectorMaths.hpp"
+#include "Heatmap.hpp"
+#include "Path.hpp"
 
 constexpr float Ai = 200;
 constexpr float Bi = 0.08;
 constexpr float K1 = 125000;
 constexpr float K2 = 240000;
 constexpr float Ti = 0.5;
 
-using vecType = std::array<float, 2>;
+constexpr float TIMESTEP = 0.001;
 
-SYCL_EXTERNAL void differentialEq(int x, sycl::accessor<Actor, 1, sycl::access::mode::read_write> actors, sycl::accessor<std::array<vecType, 2>, 1, sycl::access::mode::read> walls, sycl::stream out);
+SYCL_EXTERNAL void differentialEq(int currentIndex, sycl::accessor<Actor, 1, sycl::access::mode::read_write> actors, sycl::accessor<std::array<vecType, 2>, 1, sycl::access::mode::read> walls, sycl::accessor<Path, 1, sycl::access::mode::read> paths, sycl::stream out);
 
 #endif

external/Heatmap.cpp

@@ -0,0 +1,25 @@
+#include "Heatmap.hpp"
+
+SYCL_EXTERNAL std::array<float, 3> HSVtoRGB(float h, float s, float v) {
+    float c = v * s;
+    double scaledH = sycl::fmod(h / 60.0, 6.0);
+    float x = c * (1 - sycl::fabs(sycl::fmod(scaledH, 2.0) - 1));
+    float m = v - c;
+
+    std::array<float, 3> rgb = {};
+    
+    if (0 <= scaledH && scaledH < 1) { rgb[0] = c; rgb[1] = x; rgb[2] = 0; }
+    else if (1 <= scaledH && scaledH < 2) { rgb[0] = x; rgb[1] = c; rgb[2] = 0; }
+    else if (2 <= scaledH && scaledH < 3) { rgb[0] = 0; rgb[1] = c; rgb[2] = x; }
+    else if (3 <= scaledH && scaledH < 4) { rgb[0] = 0; rgb[1] = x; rgb[2] = c; }
+    else if (4 <= scaledH && scaledH < 5) { rgb[0] = x; rgb[1] = 0; rgb[2] = c; }
+    else if (5 <= scaledH && scaledH < 6) { rgb[0] = c; rgb[1] = 0; rgb[2] = x; }
+    else { rgb[0] = 0; rgb[1] = 0; rgb[2] = 0; }
+    
+    return rgb;
+}
+
+SYCL_EXTERNAL std::array<float, 3> findColor(float val) {
+    float h = (1.0f - val) * 120.0f;
+    return HSVtoRGB(h, 100, 50);
+}

external/Heatmap.hpp

@@ -0,0 +1,13 @@
+#ifndef Heatmap_hpp
+#define Heatmap_hpp
+
+#include <iostream>
+#include <array>
+#include <vector>
+#include <sycl/sycl.hpp>
+
+SYCL_EXTERNAL std::array<float, 3> HSVtoRGB(float h, float s, float v);
+
+SYCL_EXTERNAL std::array<float, 3> findColor(float val);
+
+#endif

external/MathHelper.cpp

@@ -13,17 +13,11 @@ SYCL_EXTERNAL vecType velToPoint(float speed, vecType pos, vecType destination)
 }
 
 SYCL_EXTERNAL float magnitude(vecType inp) {
-    return std::sqrt((inp[0] * inp[0]) + (inp[1] * inp[1]));
+    return sycl::sqrt((inp[0] * inp[0]) + (inp[1] * inp[1]));
 }
 
 SYCL_EXTERNAL float inverseMagnitude(vecType inp) {
-    float x = (inp[0] * inp[0]) + (inp[1] * inp[1]);
-    float y = x;
-    uint32_t *i = reinterpret_cast<uint32_t *>(&y);
-    const uint32_t expMask = 0x7F800000;
-    const uint32_t magicNumber = 0x5F000000;
-    *i = magicNumber - ((*i >> 1) & expMask);
-    return (x * y * y + 1) / (2 * x * y);
+    return sycl::rsqrt((inp[0] * inp[0]) + (inp[1] * inp[1]));
 }
 
 SYCL_EXTERNAL float dotProduct(vecType a, vecType b) {
@@ -57,10 +51,14 @@ SYCL_EXTERNAL std::pair<float, vecType> getDistanceAndNiw(vecType point, std::ar
         float lSquared = dotProduct(AB, AB);
         if (lSquared == 0.0) {
             return {0, {0, 0}};
-        }
-        float t = std::max(0.0f, std::min(1.0f, dotProduct(AP, AB) / lSquared));
+        } 
+        float t = sycl::max(0.0f, sycl::min(1.0f, dotProduct(AP, AB) / lSquared));
         auto projection = t * AB;
 
         return {distance(AP, projection), normalize(AP - projection)};
     }
 }
+
+SYCL_EXTERNAL vecType getTangentialVector(vecType normal) {
+    return {-normal[1], normal[0]};
+}

external/MathHelper.hpp

@@ -6,7 +6,6 @@
 #include <sycl/sycl.hpp>
 #include "VectorMaths.hpp"
 
-using vecType = std::array<float, 2>;
 
 SYCL_EXTERNAL vecType getDirectionVector(vecType a, vecType b);
 
@@ -26,4 +25,6 @@ SYCL_EXTERNAL vecType normalize(vecType inp);
 
 SYCL_EXTERNAL std::pair<float, vecType> getDistanceAndNiw(vecType point, std::array<vecType, 2> wall);
 
+SYCL_EXTERNAL vecType getTangentialVector(vecType normal);
+
 #endif