From e5771f4230db528494c658693cebeedf085237ad Mon Sep 17 00:00:00 2001 From: Wouter van Toll Date: Wed, 12 Feb 2020 17:51:08 +0100 Subject: [PATCH 1/8] Started adding static obstacles: - New class Polygon2D. - New geometry functions for Vector2D. - New time-to-collision (TTC) functions in CostFunction. - CostFunction::ComputeTimeToFirstCollision() now includes obstacles as well. - Reordered parameters in existing TTC functions; this affects several cost functions. TODO: - Read obstacles from XML. - Cost functions that use something more complicated than "first TTC" should define what to do with obstacles. - Maybe implement more efficient neighbor search for obstacles. --- include/core/costFunction.h | 68 ++++++++----- include/core/worldBase.h | 11 ++- include/core/worldInfinite.h | 3 - include/core/worldToric.h | 11 +-- .../{core/obstacle.h => tools/Polygon2D.h} | 30 +++--- include/tools/vector2D.h | 39 +++++++- src/CostFunctions/AgentInteractionForces.cpp | 2 +- src/CostFunctions/FOEAvoidance.cpp | 14 ++- src/CostFunctions/Karamouzas.cpp | 4 +- src/CostFunctions/Moussaid.cpp | 2 +- src/CostFunctions/PLEdestrians.cpp | 2 +- src/CostFunctions/Paris.cpp | 12 ++- src/CostFunctions/RVO.cpp | 2 +- src/CostFunctions/TtcaDca.cpp | 23 +++-- src/CostFunctions/VanToll.cpp | 2 +- src/core/agent.cpp | 18 ++-- src/core/costFunction.cpp | 96 +++++++++++++++---- src/core/obstacle.cpp | 51 ---------- src/core/policy.cpp | 14 ++- src/core/worldBase.cpp | 34 +++++-- src/core/worldInfinite.cpp | 7 +- src/core/worldToric.cpp | 22 ++--- 22 files changed, 298 insertions(+), 169 deletions(-) rename include/{core/obstacle.h => tools/Polygon2D.h} (62%) delete mode 100644 src/core/obstacle.cpp diff --git a/include/core/costFunction.h b/include/core/costFunction.h index 4c87f05..2c298ee 100644 --- a/include/core/costFunction.h +++ b/include/core/costFunction.h @@ -37,7 +37,10 @@ class CostFunction; struct SamplingParameters; struct PhantomAgent; -typedef std::vector NeighborList; +typedef std::vector AgentNeighborList; +typedef std::vector ObstacleNeighborList; +typedef std::pair NeighborList; + typedef std::vector, float>> CostFunctionList; const float MaxFloat = std::numeric_limits::max(); @@ -198,46 +201,46 @@ protected: /// Useful methods that can come in handy for many CostFunction implementations. /// @{ - /// Computes the expected time to collision of two disk-shaped agents with two hypothetical velocities. - /// The radius (in meters) of agent 1. - /// The current position of agent 1. - /// The hypothetical velocity of agent 1. - /// The radius (in meters) of agent 2. - /// The current position of agent 2. - /// The hypothetical velocity of agent 2. - /// The time (in seconds) after which the two agents will collide if they move at the given velocities. - /// This value is 0 if the agents are already colliding, and it is MaxFloat if the agents will never collide. + /// Computes the expected time to collision of two disk-shaped objects with two hypothetical velocities. + /// The current position of object 1. + /// The hypothetical velocity of object 1. + /// The radius (in meters) of object 1. + /// The current position of object 2. + /// The hypothetical velocity of object 2. + /// The radius (in meters) of object 2. + /// The time (in seconds) after which the two objects will collide if they move at the given velocities. + /// This value is 0 if the objects are already colliding, and it is MaxFloat if the objects will never collide. float ComputeTimeToCollision( - const float radius1, const Vector2D& position1, const Vector2D& velocity1, - const float radius2, const Vector2D& position2, const Vector2D& velocity2) const; + const Vector2D& position1, const Vector2D& velocity1, const float radius1, + const Vector2D& position2, const Vector2D& velocity2, const float radius2) const; - /// Computes the expected time to the first collision with a set of neighboring agents. - /// The radius (in meters) of the querying agent. + /// Computes the expected time to the first collision with a set of neighboring agents and obstacles. /// The current position of the querying agent. /// The hypothetical velocity of the querying agent. - /// A list of neighboring agents. + /// The radius (in meters) of the querying agent. + /// A list of neighboring agents and obstacles. /// Whether or not to ignore any collisions that are already happening now. /// The smallest time to collision (in seconds) of the querying agent with all neighbors in the list. - /// This value may be 0 if there is already a collision with any of the neighboring agents. + /// This value may be 0 if there is already a collision with any of the neighboring objects. /// However, if ignoreCurrentCollisions is set to true, any neighbors that are already colliding will be ignored, /// and the result will be larger than zero. /// float ComputeTimeToFirstCollision( - const float radius, const Vector2D& position, const Vector2D& velocity, + const Vector2D& position, const Vector2D& velocity, const float radius, const NeighborList& neighbors, bool ignoreCurrentCollisions = true) const; - /// Computes the expected time at which the distance between two disk-shaped agents is minimal, and the value of this distance. - /// The radius (in meters) of agent 1. - /// The current position of agent 1. - /// The hypothetical velocity of agent 1. - /// The radius (in meters) of agent 2. - /// The current position of agent 2. - /// The hypothetical velocity of agent 2. + /// Computes the expected time at which the distance between two disk-shaped objects is minimal, and the value of this distance. + /// The current position of object 1. + /// The hypothetical velocity of object 1. + /// The radius (in meters) of object 1. + /// The current position of object 2. + /// The hypothetical velocity of object 2. + /// The radius (in meters) of object 2. /// A pair of two floating-point numbers, where the first is the time to closest approach (ttca) /// and the second is the distance of closest approach (dca). std::pair ComputeTimeAndDistanceToClosestApproach( - const float radius1, const Vector2D& position1, const Vector2D& velocity1, - const float radius2, const Vector2D& position2, const Vector2D& velocity2) const; + const Vector2D& position1, const Vector2D& velocity1, const float radius1, + const Vector2D& position2, const Vector2D& velocity2, const float radius2) const; /// Converts a gradient in the "(angle,speed) domain" to a gradient in the Euclidean (vx,vy) domain. /// The angular component of the gradient to convert. @@ -262,6 +265,19 @@ private: /// This will only receive a value if there are 2 solutions. /// The number of solutions to the equation, i.e. 0, 1, or 2. int SolveQuadraticEquation(float a, float b, float c, float& answer1, float& answer2) const; + + /// Computes the expected time to collision of a moving disk-shaped object and a static line segment. + /// The current position of the moving object. + /// The hypothetical velocity of the moving object. + /// The radius (in meters) of the moving object. + /// The static line-segment object. + /// The time (in seconds) after which the two objects will collide if the disk moves at the given velocity. + /// This value is 0 if the objects are already colliding, and it is MaxFloat if the objects will never collide. + float ComputeTimeToCollision_LineSegment(const Vector2D& position, const Vector2D& velocity, const float radius, + const LineSegment2D& lineSegment) const; + + float ComputeTimeToCollision_LineSegmentInterior(const Vector2D& position, const Vector2D& velocity, const float radius, + const LineSegment2D& lineSegment) const; }; #endif //LIB_COST_FUNCTION_H diff --git a/include/core/worldBase.h b/include/core/worldBase.h index ed549e3..dac642b 100644 --- a/include/core/worldBase.h +++ b/include/core/worldBase.h @@ -28,8 +28,9 @@ #include #include -#include "core/agent.h" -#include "core/AgentKDTree.h" +#include +#include +#include /// A reference to an agent in the simulation, possibly with a transposed position. /// Nearest-neighbor queries in WorldBase return results of this type. @@ -96,6 +97,8 @@ protected: /// The type of this world, e.g. infinite or toric. const Type type_; + std::vector obstacles_; + /// A list containing all agents that are currently in the crowd. std::vector agents_; @@ -208,7 +211,9 @@ protected: /// Use nullptr to not exclude any agents. /// A list of pointers to agents queryingAgent lie within "search_radius" meters of "position", /// excluding the agent denoted by "queryingAgent" (if it exists). - std::vector computeNeighbors_Flat(const Vector2D& position, float search_radius, const Agent* queryingAgent) const; + std::vector computeNeighboringAgents_Flat(const Vector2D& position, float search_radius, const Agent* queryingAgent) const; + + std::vector computeNeighboringObstacles_Flat(const Vector2D& position, float search_radius) const; /// Subroutine of DoStep() that moves all agents forward using their last computed "new velocities". /// Subclasses of WorldBase may override this method if they require special behavior (e.g. the wrap-around effect in WorldToric). diff --git a/include/core/worldInfinite.h b/include/core/worldInfinite.h index 1299516..ce2664a 100644 --- a/include/core/worldInfinite.h +++ b/include/core/worldInfinite.h @@ -25,15 +25,12 @@ #define LIB_WORLD_INFINITE_H #include -#include -#include /// The simplest implementation of WorldBase: an unbounded plane with no wrap-around effects. class WorldInfinite : public WorldBase { public: WorldInfinite(); - NeighborList ComputeNeighbors(const Vector2D& position, float search_radius, const Agent* queryingAgent) const override; }; diff --git a/include/core/worldToric.h b/include/core/worldToric.h index 80b79a5..05660ab 100644 --- a/include/core/worldToric.h +++ b/include/core/worldToric.h @@ -24,14 +24,7 @@ #ifndef LIB_WORLD_TORIC_H #define LIB_WORLD_TORIC_H -#include -#include -#include "core/agent.h" -#include "core/obstacle.h" -#include "core/worldBase.h" - -#include "tools/vector2D.h" -#include +#include /// An implementation of WorldBase that treats the world as a torus, i.e. with horizontal and vertical wrap-around. /// The world can be interpreted as a rectangle with a certain width and height, @@ -59,7 +52,7 @@ public: private: void computeNeighbors_Displaced(const Vector2D& position, const Vector2D& displacement, float search_radius, const Agent* queryingAgent, - std::vector& result) const; + NeighborList& result) const; }; #endif //LIB_WORLD_TORIC_H diff --git a/include/core/obstacle.h b/include/tools/Polygon2D.h similarity index 62% rename from include/core/obstacle.h rename to include/tools/Polygon2D.h index ffed2f3..35cf222 100644 --- a/include/core/obstacle.h +++ b/include/tools/Polygon2D.h @@ -21,22 +21,30 @@ ** Contact : crowd_group@inria.fr */ -#ifndef LIB_OBSTACLE_H -#define LIB_OBSTACLE_H +#ifndef LIB_POLYGON2D_H +#define LIB_POLYGON2D_H #include -#include "tools/vector2D.h" +#include -class Obstacle { - std::vector vertices_; +class Polygon2D +{ +private: + std::vector vertices_; + std::vector edges_; public: - std::vector vertices() const; - Vector2D vertex(int i); - int getNumberOfVertices() const; + Polygon2D(const std::vector & vertices) : vertices_(vertices) + { + // compute the list of edges + const size_t n = vertices.size(); + edges_.resize(n); + for (size_t i = 0; i < vertices.size(); ++i) + edges_[i] = { vertices[i], vertices[(i + 1) % n] }; + } - void setVertices(const std::vector &vertices); - void appendVertex(const Vector2D& vertex); + inline const std::vector& GetVertices() const { return vertices_; } + inline const std::vector& GetEdges() const { return edges_; } }; -#endif //LIB_OBSTACLE_H +#endif //LIB_POLYGON2D_H diff --git a/include/tools/vector2D.h b/include/tools/vector2D.h index afa2b49..3ae1bfd 100644 --- a/include/tools/vector2D.h +++ b/include/tools/vector2D.h @@ -25,6 +25,7 @@ #define LIB_VECTOR2D_H #include +#include const double PI = 3.1415926535897; @@ -121,6 +122,8 @@ public: } }; +typedef std::pair LineSegment2D; + #pragma region [Arithmetic operators] inline Vector2D operator-(Vector2D lhs, Vector2D rhs) @@ -182,14 +185,18 @@ inline float angle(const Vector2D& va, const Vector2D& vb) { const double lengths = va.magnitude() * vb.magnitude(); if (lengths == 0) - return 0; + return 0.0f; return (float)acos(va.dot(vb) / lengths); } inline float cosAngle(const Vector2D& va, const Vector2D& vb) { - return cosf(angle(va, vb)); + const double lengths = va.magnitude() * vb.magnitude(); + if (lengths == 0) + return 0.0f; + + return (float)(va.dot(vb) / lengths); } inline bool isClockwise(const Vector2D &vector1, const Vector2D &vector2) @@ -250,4 +257,32 @@ inline bool getLineIntersection(const Vector2D& a, const Vector2D& b, const Vect return true; } +inline Vector2D nearestPointOnLine(const Vector2D& pt, const Vector2D& la, const Vector2D& lb, + bool onSegment = false, bool* resultIsOnSegment = nullptr, float* resultFrac = nullptr) +{ + if (la == lb) + return la; + + float k = (pt - lb).dot(la - lb) / la.dot(lb); + + if (onSegment) + { + if (k > 1) k = 1; + else if (k < 0) k = 0; + } + + if (resultIsOnSegment != nullptr) + *resultIsOnSegment = (k >= 0 && k <= 1); + + if (resultFrac != nullptr) + *resultFrac = 1 - k; + + return (la * k) + (lb * (1 - k)); +} + +inline float distanceToLine(const Vector2D& pt, const Vector2D& la, const Vector2D& lb, bool onSegment = false) +{ + return (pt - nearestPointOnLine(pt, la, lb, onSegment)).magnitude(); +} + #endif // LIB_VECTOR2D_H diff --git a/src/CostFunctions/AgentInteractionForces.cpp b/src/CostFunctions/AgentInteractionForces.cpp index b3f9cf8..1938284 100644 --- a/src/CostFunctions/AgentInteractionForces.cpp +++ b/src/CostFunctions/AgentInteractionForces.cpp @@ -30,7 +30,7 @@ Vector2D AgentInteractionForces::ComputeForce(Agent* agent, const WorldBase * wo // loop over all neighbors; sum up the forces per neighbor Vector2D AgentForces(0, 0); const auto& neighbors = agent->getNeighborSubsetInRange(range_); - for (const PhantomAgent& other : neighbors) + for (const PhantomAgent& other : neighbors.first) AgentForces += ComputeAgentInteractionForce(agent, other); return AgentForces; diff --git a/src/CostFunctions/FOEAvoidance.cpp b/src/CostFunctions/FOEAvoidance.cpp index 5ed4cb1..899b14e 100644 --- a/src/CostFunctions/FOEAvoidance.cpp +++ b/src/CostFunctions/FOEAvoidance.cpp @@ -60,7 +60,9 @@ float FOEAvoidance::GetCost(const Vector2D& velocity, Agent* agent, const WorldB float Cost = 0; const auto& neighbors = agent->getNeighborSubsetInRange(range_); - for (const PhantomAgent& neighbor : neighbors) + + // check neighboring agents + for (const PhantomAgent& neighbor : neighbors.first) { Vector2D Vel = RT * (neighbor.velocity - velocity); Vector2D Pos = RT * (neighbor.position - Position); @@ -80,6 +82,9 @@ float FOEAvoidance::GetCost(const Vector2D& velocity, Agent* agent, const WorldB Cost += localCost; } + // TODO: check neighboring obstacles + // ... + return Cost; } @@ -95,7 +100,9 @@ Vector2D FOEAvoidance::GetGradient(const Vector2D& velocity, Agent* agent, const float gradtheta = 0, gradv = 0; const auto& neighbors = agent->getNeighborSubsetInRange(range_); - for (const PhantomAgent& neighbor : neighbors) + + // check neighboring agents + for (const PhantomAgent& neighbor : neighbors.first) { Vector2D Vel = RT * (neighbor.velocity - velocity); Vector2D Pos = RT * (neighbor.position - Position); @@ -117,6 +124,9 @@ Vector2D FOEAvoidance::GetGradient(const Vector2D& velocity, Agent* agent, const gradv += localgradv; } + // TODO: check neighboring obstacles + // ... + gradtheta = clamp(gradtheta, -0.1f, 0.1f); //gradtheta = -gradtheta; //gradv = clamp(gradv, -1.f, 1.f); diff --git a/src/CostFunctions/Karamouzas.cpp b/src/CostFunctions/Karamouzas.cpp index 6489099..f183ee0 100644 --- a/src/CostFunctions/Karamouzas.cpp +++ b/src/CostFunctions/Karamouzas.cpp @@ -35,7 +35,7 @@ float Karamouzas::GetCost(const Vector2D& velocity, Agent* agent, const WorldBas const auto& speed = velocity.magnitude(); const float maxSpeed = agent->getMaximumSpeed(); - float TTC_preferred = ComputeTimeToFirstCollision(agent->getRadius(), agent->getPosition(), prefVelocity, neighbors); + float TTC_preferred = ComputeTimeToFirstCollision(agent->getPosition(), prefVelocity, agent->getRadius(), neighbors); // This collision-avoidance method has a dynamic angular range, so ignore velocities that are outside it if (angle(velocity, prefVelocity) > getMaxDeviationAngle(agent, TTC_preferred)) @@ -46,7 +46,7 @@ float Karamouzas::GetCost(const Vector2D& velocity, Agent* agent, const WorldBas return MaxFloat; // compute time to collision at this candidate velocity - float TTC = ComputeTimeToFirstCollision(agent->getRadius(), agent->getPosition(), velocity, neighbors); + float TTC = ComputeTimeToFirstCollision(agent->getPosition(), velocity, agent->getRadius(), neighbors); // the cost is a weighted sum of factors: diff --git a/src/CostFunctions/Moussaid.cpp b/src/CostFunctions/Moussaid.cpp index d512dc8..6d2073b 100644 --- a/src/CostFunctions/Moussaid.cpp +++ b/src/CostFunctions/Moussaid.cpp @@ -33,7 +33,7 @@ float Moussaid::getDistanceToCollisionAtPreferredSpeed(const Vector2D& direction const Vector2D& velocityWithPrefSpeed = direction * prefSpeed; // compute the time to collision at this velocity - float ttc = ComputeTimeToFirstCollision(agent->getRadius(), agent->getPosition(), velocityWithPrefSpeed, neighbors, false); + float ttc = ComputeTimeToFirstCollision(agent->getPosition(), velocityWithPrefSpeed, agent->getRadius(), neighbors, false); // convert to the distance to collision, clamped to a maximum distance float distanceToCollision = (ttc == MaxFloat ? MaxFloat : ttc * prefSpeed); diff --git a/src/CostFunctions/PLEdestrians.cpp b/src/CostFunctions/PLEdestrians.cpp index f146954..1a6a473 100644 --- a/src/CostFunctions/PLEdestrians.cpp +++ b/src/CostFunctions/PLEdestrians.cpp @@ -29,7 +29,7 @@ using namespace std; float PLEdestrians::GetCost(const Vector2D& velocity, Agent* agent, const WorldBase * world) const { - float ttc = ComputeTimeToFirstCollision(agent->getRadius(), agent->getPosition(), velocity, agent->getNeighborSubsetInRange(range_)); + float ttc = ComputeTimeToFirstCollision(agent->getPosition(), velocity, agent->getRadius(), agent->getNeighborSubsetInRange(range_)); if (ttc < t_min) return MaxFloat; diff --git a/src/CostFunctions/Paris.cpp b/src/CostFunctions/Paris.cpp index 52e8acd..6dd7c66 100644 --- a/src/CostFunctions/Paris.cpp +++ b/src/CostFunctions/Paris.cpp @@ -51,7 +51,7 @@ std::pair Paris::ComputeT1andT2(const Vector2D& origin, const Vect // To find them, we must solve the following equation for t: // || origin + velocity*t - point || = radius // We can re-use the time-to-collision solution to find t1: - float t1 = ComputeTimeToCollision(radius, origin, velocity, 0, point, Vector2D(0, 0)); + float t1 = ComputeTimeToCollision(origin, velocity, radius, point, Vector2D(0, 0), 0); if (t1 > t_max) t1 = t_max; @@ -109,7 +109,7 @@ Paris::NeighborAvoidanceRange Paris::ComputeNeighborAvoidanceRange(const Vector2 if (!intersects) { // check if the agents are on collision course - float ttc = ComputeTimeToCollision(radius1, agentPos, direction, radius2, neighbor.position, neighbor.velocity); + float ttc = ComputeTimeToCollision(agentPos, direction, radius1, neighbor.position, neighbor.velocity, radius2); // if not (or if it's too far in the future), this direction is fine if (ttc > t_max) @@ -139,8 +139,14 @@ Paris::NeighborAvoidanceRange Paris::ComputeNeighborAvoidanceRange(const Vector2 Paris::NeighborAvoidanceRangeList Paris::ComputeAllNeighborAvoidanceRanges(const Vector2D& direction, const Agent* agent, const NeighborList& neighbors) const { NeighborAvoidanceRangeList result; - for (const auto& neighbor : neighbors) + + // agents + for (const auto& neighbor : neighbors.first) result.push_back(ComputeNeighborAvoidanceRange(direction, agent, neighbor)); + + // TODO: static obstacles? + // ... + return result; } diff --git a/src/CostFunctions/RVO.cpp b/src/CostFunctions/RVO.cpp index 7349dd2..8bbd414 100644 --- a/src/CostFunctions/RVO.cpp +++ b/src/CostFunctions/RVO.cpp @@ -40,7 +40,7 @@ float RVO::GetCost(const Vector2D& velocity, Agent* agent, const WorldBase * wor const Vector2D& vDiff = agent->getPreferredVelocity() - velocity; // compute the smallest time to collision among all neighboring agents - float minTTC = ComputeTimeToFirstCollision(radius, position, RVOVelocity, agent->getNeighborSubsetInRange(range_)); + float minTTC = ComputeTimeToFirstCollision(position, RVOVelocity, radius, agent->getNeighborSubsetInRange(range_)); return w / minTTC + vDiff.magnitude(); } diff --git a/src/CostFunctions/TtcaDca.cpp b/src/CostFunctions/TtcaDca.cpp index 8cc7e40..340c3ac 100644 --- a/src/CostFunctions/TtcaDca.cpp +++ b/src/CostFunctions/TtcaDca.cpp @@ -75,9 +75,11 @@ float TtcaDca::GetCost(const Vector2D& velocity, Agent* agent, const WorldBase * float ObstacleCost = 0; float ObstacleCostScale = 0; - // for each agent of the neighbourhood + // get neighboring agents and obstacles const auto& neighbors = agent->getNeighborSubsetInRange(range_); - for (const auto& neighbor : neighbors) + + // for each agent of the neighbourhood + for (const auto& neighbor : neighbors.first) { // Compute relative velocity and relative position const Vector2D& relPos = neighbor.position - Position; @@ -92,8 +94,8 @@ float TtcaDca::GetCost(const Vector2D& velocity, Agent* agent, const WorldBase * { // computing ttc and dca const auto& ttca_dca = ComputeTimeAndDistanceToClosestApproach( - Radius, Position, velocity, - neighbor.realAgent->getRadius(), neighbor.position, neighbor.velocity); + Position, velocity, Radius, + neighbor.position, neighbor.velocity, neighbor.realAgent->getRadius()); if (ttca_dca.first < 0) continue; @@ -105,6 +107,9 @@ float TtcaDca::GetCost(const Vector2D& velocity, Agent* agent, const WorldBase * ObstacleCost += cost; } } + + // TODO: check neighboring obstacles + // ... if (ObstacleCostScale > 0) ObstacleCost /= ObstacleCostScale; @@ -128,9 +133,10 @@ Vector2D TtcaDca::GetGradient(const Vector2D& velocity, Agent* agent, const Worl float GradS = 0; float totalScale = 0; - // for each agent of the neighbourhood - const auto& neighbors = agent->getNeighborSubsetInRange(range_); - for (const auto& neighbor : neighbors) + const auto& neighbors = agent->getNeighborSubsetInRange(range_); + + // for each agent of the neighbourhood + for (const auto& neighbor : neighbors.first) { // Compute relative velocity and relative position const Vector2D& relPos = neighbor.position - Position; @@ -200,6 +206,9 @@ Vector2D TtcaDca::GetGradient(const Vector2D& velocity, Agent* agent, const Worl } } + // TODO: check neighboring obstacles + // ... + // The original method averages over all pixels. // To simulate this, we should divide by the total scale we've applied per obstacle. if (totalScale > 0) diff --git a/src/CostFunctions/VanToll.cpp b/src/CostFunctions/VanToll.cpp index 691bc9a..aab403e 100644 --- a/src/CostFunctions/VanToll.cpp +++ b/src/CostFunctions/VanToll.cpp @@ -37,7 +37,7 @@ float VanToll::GetCost(const Vector2D& velocity, Agent* agent, const WorldBase * const float speed = velocity.magnitude(); // compute the time to collision - const float candidateTTC = ComputeTimeToFirstCollision(agent->getRadius(), currentPos, velocity, agent->getNeighborSubsetInRange(range_), false); + const float candidateTTC = ComputeTimeToFirstCollision(currentPos, velocity, agent->getRadius(), agent->getNeighborSubsetInRange(range_), false); // compute the distance to collision, bounded by the viewing distance float distanceToCollision = (candidateTTC == MaxFloat ? MaxFloat : candidateTTC * speed); diff --git a/src/core/agent.cpp b/src/core/agent.cpp index c67e5cf..5995fa3 100644 --- a/src/core/agent.cpp +++ b/src/core/agent.cpp @@ -97,15 +97,21 @@ NeighborList Agent::getNeighborSubsetInRange(float range) const { NeighborList result; - // Add neighbors as long as they are close enough to the agent. - // The "neighbors" list is sorted by distance, so filtering is easy. + // add all neighboring agents in range const float sqrRange = range * range; - for (const auto& neighbor : neighbors_) + for (const auto& neighbor : neighbors_.first) { - if ((neighbor.position - position_).sqrMagnitude() > sqrRange) - break; - result.push_back(neighbor); + if ((neighbor.position - position_).sqrMagnitude() <= sqrRange) + result.first.push_back(neighbor); } + + // add all neighboring obstacles in range + for (const auto& neighbor : neighbors_.second) + { + if (distanceToLine(position_, neighbor.first, neighbor.second, true) <= range) + result.second.push_back(neighbor); + } + return result; } diff --git a/src/core/costFunction.cpp b/src/core/costFunction.cpp index 4b89119..069494c 100644 --- a/src/core/costFunction.cpp +++ b/src/core/costFunction.cpp @@ -72,13 +72,6 @@ Vector2D CostFunction::GetGlobalMinimum(Agent* agent, const WorldBase* world) co ); } -float GetRandomNumber(float minValue, float maxValue) -{ - int range = (int)((maxValue - minValue) * 1000); - float val = (std::rand() % range) / 1000.0f; - return minValue + val; -} - Vector2D CostFunction::ApproximateGlobalMinimumBySampling(Agent* agent, const WorldBase* world, const SamplingParameters& params, const CostFunctionList& costFunctions) { @@ -115,8 +108,8 @@ Vector2D CostFunction::ApproximateGlobalMinimumBySampling(Agent* agent, const Wo for (int i = 0; i < params.randomSamples; ++i) { // create a random velocity in the cone/circle - float randomAngle = GetRandomNumber(-maxAngle, maxAngle); - float randomLength = GetRandomNumber(0, radius); + float randomAngle = agent->ComputeRandomNumber(-maxAngle, maxAngle); + float randomLength = agent->ComputeRandomNumber(0, radius); const Vector2D& velocity = base + rotateCounterClockwise(baseDirection, randomAngle) * randomLength; // compute the cost for this velocity @@ -203,8 +196,8 @@ int CostFunction::SolveQuadraticEquation(float a, float b, float c, float& answe } float CostFunction::ComputeTimeToCollision( - const float radius1, const Vector2D& position1, const Vector2D& velocity1, - const float radius2, const Vector2D& position2, const Vector2D& velocity2) const + const Vector2D& position1, const Vector2D& velocity1, const float radius1, + const Vector2D& position2, const Vector2D& velocity2, const float radius2) const { const Vector2D PDiff(position1 - position2); const float Radii = radius1 + radius2; @@ -238,14 +231,15 @@ float CostFunction::ComputeTimeToCollision( return std::min(t1, t2); } -float CostFunction::ComputeTimeToFirstCollision(const float radius, const Vector2D& position, const Vector2D& velocity, +float CostFunction::ComputeTimeToFirstCollision(const Vector2D& position, const Vector2D& velocity, const float radius, const NeighborList& neighbors, bool ignoreCurrentCollisions) const { float minTTC = MaxFloat; - for (const auto& neighbor : neighbors) + // check neighboring agents + for (const auto& neighborAgent : neighbors.first) { - float ttc = ComputeTimeToCollision(radius, position, velocity, neighbor.realAgent->getRadius(), neighbor.position, neighbor.velocity); + float ttc = ComputeTimeToCollision(position, velocity, radius, neighborAgent.position, neighborAgent.velocity, neighborAgent.realAgent->getRadius()); // ignore current collisions? if (ignoreCurrentCollisions && ttc == 0) @@ -255,12 +249,82 @@ float CostFunction::ComputeTimeToFirstCollision(const float radius, const Vector minTTC = ttc; } + // check neighboring obstacles + for (const auto& neighboringObstacle : neighbors.second) + { + float ttc = ComputeTimeToCollision_LineSegment(position, velocity, radius, neighboringObstacle); + + // ignore current collisions? + if (ignoreCurrentCollisions && ttc == 0) + continue; + + if (ttc < minTTC) + minTTC = ttc; + } + return minTTC; } +float CostFunction::ComputeTimeToCollision_LineSegment(const Vector2D& position, const Vector2D& velocity, const float radius, const LineSegment2D& lineSegment) const +{ + // compute TTC for the first endpoint + float timeToEndpoint1 = ComputeTimeToCollision(position, velocity, radius, lineSegment.first, Vector2D(0,0), 0); + if (timeToEndpoint1 == 0) + return 0; + + // if the line segment is actually a point, return the only TTC we've computed + if (lineSegment.first == lineSegment.second) + return timeToEndpoint1; + + // compute TTC for the second endpoint + float timeToEndpoint2 = ComputeTimeToCollision(position, velocity, radius, lineSegment.second, Vector2D(0, 0), 0); + if (timeToEndpoint2 == 0) + return 0; + + // compute TTC for the interior of the segment + float timeToInterior = ComputeTimeToCollision_LineSegmentInterior(position, velocity, radius, lineSegment); + + // return the smallest of those three + return std::min(std::min(timeToEndpoint1, timeToEndpoint2), timeToInterior); +} + +float CostFunction::ComputeTimeToCollision_LineSegmentInterior(const Vector2D& position, const Vector2D& velocity, const float radius, const LineSegment2D& lineSegment) const +{ + // Check if the disk is already colliding with the line segment right now + if (distanceToLine(position, lineSegment.first, lineSegment.second, true) <= radius) + return 0; + + // Compute the intersection of the disk's future trajectory and the line segment, ignoring the disk's radius for now (we will correct this later) + Vector2D intersection; + bool intersects = getLineIntersection(position, position + velocity, lineSegment.first, lineSegment.second, intersection); + // - check if there is no intersection at all + if (!intersects) + return MaxFloat; + // - check if the intersection point lies behind the agent + if (velocity.dot(intersection - position) < 0) + return MaxFloat; + // - check if the intersection point actually lies on the segment + bool pointIsOnSegment; + nearestPointOnLine(intersection, lineSegment.first, lineSegment.second, false, &pointIsOnSegment); + if (!pointIsOnSegment) + return MaxFloat; + + // Compute the time after which this intersection will occur + float speed = velocity.magnitude(); + float timeToCollision = (intersection - position).magnitude() / speed; + + // We now know when a *point* agent would hit the line segment, but a *disk* agent will hit it sooner. + // Apply a correction for this: + float lineAngle = angle(velocity, lineSegment.second - lineSegment.first); + float distanceToSubtract = radius / sinf(lineAngle); + float timeToSubtract = distanceToSubtract / speed; + + return timeToCollision - timeToSubtract; +} + std::pair CostFunction::ComputeTimeAndDistanceToClosestApproach( - const float radius1, const Vector2D& position1, const Vector2D& velocity1, - const float radius2, const Vector2D& position2, const Vector2D& velocity2) const + const Vector2D& position1, const Vector2D& velocity1, const float radius1, + const Vector2D& position2, const Vector2D& velocity2, const float radius2) const { const Vector2D dp(position2 - position1); const Vector2D dv(velocity2 - velocity1); diff --git a/src/core/obstacle.cpp b/src/core/obstacle.cpp deleted file mode 100644 index 0c6f18f..0000000 --- a/src/core/obstacle.cpp +++ /dev/null @@ -1,51 +0,0 @@ -/* Crowd Simulator Engine -** Copyright(C) 2018 - Inria Rennes - Rainbow - Julien Pettre -** -** This program is free software; you can redistribute it and/or -** modify it under the terms of the GNU General Public License -** as published by the Free Software Foundation; either version 3 -** of the License, or (at your option) any later version. -** -** This program is distributed in the hope that it will be useful, -** but WITHOUT ANY WARRANTY; without even the implied warranty of -** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.See the -** GNU General Public License for more details. -** -** You should have received a copy of the GNU General Public License -** along with this program.If not, see < https://www.gnu.org/licenses/>. -** -** Authors: Axel Lopez Gandia, Javad Amirian, Florian Berton, -** Julien Legros, Lucas Pelerin, Beatriz Cabrero Daniel, Fabien Grzeskowiak, -** Alicia Nicás Miquel, Alexandre Bonneau, Wouter Van toll, Benjamin Niay. -** -** Contact : crowd_group@inria.fr -*/ -#include "core/obstacle.h" -#include - - -std::vector Obstacle::vertices() const -{ - return vertices_; -} - -Vector2D Obstacle::vertex(int i) -{ - assert(i<(int)vertices_.size()); - return vertices_[i]; -} - -void Obstacle::setVertices(const std::vector &vertices) -{ - vertices_ = vertices; -} - -void Obstacle::appendVertex(const Vector2D &vertex) -{ - vertices_.push_back(vertex); -} - -int Obstacle::getNumberOfVertices() const -{ - return (int)vertices_.size(); -} diff --git a/src/core/policy.cpp b/src/core/policy.cpp index 56ee958..a44367c 100644 --- a/src/core/policy.cpp +++ b/src/core/policy.cpp @@ -104,17 +104,25 @@ Vector2D Policy::ComputeContactForces(Agent* agent, WorldBase * world) if (contactForceScale_ > 0) { + const auto& neighbors = agent->getNeighbors(); + // check all colliding agents - for (const auto& neighbor : agent->getNeighbors()) + for (const auto& neighborAgent : neighbors.first) { - const auto& diff = agent->getPosition() - neighbor.position; - const float intersectionDistance = agent->getRadius() + neighbor.realAgent->getRadius() - diff.magnitude(); + const auto& diff = agent->getPosition() - neighborAgent.position; + const float intersectionDistance = agent->getRadius() + neighborAgent.realAgent->getRadius() - diff.magnitude(); if (intersectionDistance > 0) { const auto& F = diff.getnormalized() * (contactForceScale_ * intersectionDistance); totalForce += F; } } + + // TODO: check all colliding obstacles + for (const auto& neighborObstacle : neighbors.second) + { + + } } return totalForce; diff --git a/src/core/worldBase.cpp b/src/core/worldBase.cpp index ed3574e..6cebe4d 100644 --- a/src/core/worldBase.cpp +++ b/src/core/worldBase.cpp @@ -50,7 +50,7 @@ void WorldBase::SetNumberOfThreads(int nrThreads) omp_set_num_threads(nrThreads); } -std::vector WorldBase::computeNeighbors_Flat(const Vector2D& position, float search_radius, const Agent* queryingAgent) const +std::vector WorldBase::computeNeighboringAgents_Flat(const Vector2D& position, float search_radius, const Agent* queryingAgent) const { // get the IDs of neighbors const auto& agentIDs = agentKDTree->FindAllAgentsInRange(position, search_radius, queryingAgent); @@ -63,6 +63,26 @@ std::vector WorldBase::computeNeighbors_Flat(const Vector2D& posit return result; } +std::vector WorldBase::computeNeighboringObstacles_Flat(const Vector2D& position, float search_radius) const +{ + // TODO: implement more efficient neighbor search for obstacles + + std::vector result; + + // loop over all obstacle edges + for (const auto& obs : obstacles_) + { + for (const auto& edge : obs.GetEdges()) + { + // if this line segment is clsoe enough, add it to the result + if (distanceToLine(position, edge.first, edge.second, true) <= search_radius) + result.push_back(edge); + } + } + + return result; +} + void WorldBase::DoStep() { // Before the simulation frame begins, add agents that need to be added now @@ -78,24 +98,26 @@ void WorldBase::DoStep() delete agentKDTree; agentKDTree = new AgentKDTree(agents_); + int n = (int)agents_.size(); + // 2. compute nearest neighbors for each agent #pragma omp parallel for - for (int i = 0; i < (int)agents_.size(); ++i) + for (int i = 0; i < n; ++i) agents_[i]->ComputeNeighbors(this); // 3. compute a preferred velocity for each agent #pragma omp parallel for - for (int i = 0; i < (int)agents_.size(); ++i) + for (int i = 0; i < n; ++i) agents_[i]->ComputePreferredVelocity(); // 4. perform local navigation for each agent, to compute a new velocity for them #pragma omp parallel for - for (int i = 0; i < (int)agents_.size(); ++i) + for (int i = 0; i < n; ++i) agents_[i]->ComputeNextVelocity(this); // 5. compute contact forces for all agents #pragma omp parallel for - for (int i = 0; i < (int)agents_.size(); ++i) + for (int i = 0; i < n; ++i) agents_[i]->ComputeContactForces(this); // 6. move all agents to their new positions @@ -107,7 +129,7 @@ void WorldBase::DoStep() time_ += delta_time_; // remove agents who have reached their goal - for (int i = (int)agents_.size()-1; i >= 0; --i) + for (int i = n-1; i >= 0; --i) if (agents_[i]->getRemoveAtGoal() && agents_[i]->hasReachedGoal()) removeAgentAtListIndex(i); } diff --git a/src/core/worldInfinite.cpp b/src/core/worldInfinite.cpp index 9e8bc3e..e86c1a8 100644 --- a/src/core/worldInfinite.cpp +++ b/src/core/worldInfinite.cpp @@ -30,12 +30,13 @@ WorldInfinite::WorldInfinite() : WorldBase(INFINITE) { } -std::vector WorldInfinite::ComputeNeighbors(const Vector2D& position, float search_radius, const Agent* queryingAgent) const +NeighborList WorldInfinite::ComputeNeighbors(const Vector2D& position, float search_radius, const Agent* queryingAgent) const { - const auto& agents = computeNeighbors_Flat(position, search_radius, queryingAgent); + const auto& agents = computeNeighboringAgents_Flat(position, search_radius, queryingAgent); vector phantoms; for (const Agent* agent : agents) phantoms.push_back(PhantomAgent(agent->getPosition(), agent->getVelocity(), agent)); - return phantoms; + + return { phantoms, computeNeighboringObstacles_Flat(position, search_radius) }; } diff --git a/src/core/worldToric.cpp b/src/core/worldToric.cpp index d4ae5ec..78924e1 100644 --- a/src/core/worldToric.cpp +++ b/src/core/worldToric.cpp @@ -32,22 +32,22 @@ WorldToric::WorldToric(float width, float height) } void WorldToric::computeNeighbors_Displaced(const Vector2D& position, const Vector2D& displacement, float search_radius, const Agent* queryingAgent, - std::vector& result) const + NeighborList& result) const { - const auto& agents = computeNeighbors_Flat(position + displacement, search_radius, queryingAgent); + // agents + const auto& agents = computeNeighboringAgents_Flat(position + displacement, search_radius, queryingAgent); for (const Agent* agent : agents) - result.push_back(PhantomAgent(agent->getPosition() - displacement, agent->getVelocity(), agent)); + result.first.push_back(PhantomAgent(agent->getPosition() - displacement, agent->getVelocity(), agent)); + + // obstacles + const auto& obstacles = computeNeighboringObstacles_Flat(position + displacement, search_radius); + for (const LineSegment2D& obs : obstacles) + result.second.push_back(LineSegment2D(obs.first - displacement, obs.second - displacement)); } -/* -* Searches neighbors in a toric world. -*/ -std::vector WorldToric::ComputeNeighbors(const Vector2D& position, float search_radius, const Agent* queryingAgent) const +NeighborList WorldToric::ComputeNeighbors(const Vector2D& position, float search_radius, const Agent* queryingAgent) const { - // TODO: This function may be incorrect if the agent is close to a *corner*. - // I have the feeling there should be up to 9 queries here, instead of 5. - - vector result; + NeighborList result; computeNeighbors_Displaced(position, Vector2D(0, 0), search_radius, queryingAgent, result); if (position.x() - search_radius < -0.5*width_) -- GitLab From 2c073f9998ea2f1f3680de7343a7a58f76d14a01 Mon Sep 17 00:00:00 2001 From: Wouter van Toll Date: Thu, 13 Feb 2020 14:42:11 +0100 Subject: [PATCH 2/8] - Obstacles can now be read from XML files. - Fixed a bug in nearestPointOnLine(). --- include/core/crowdSimulator.h | 4 ++ include/core/worldBase.h | 4 ++ include/tools/vector2D.h | 3 +- src/core/crowdSimulator.cpp | 88 +++++++++++++++++++++++++++++++---- src/core/worldBase.cpp | 5 ++ 5 files changed, 93 insertions(+), 11 deletions(-) diff --git a/include/core/crowdSimulator.h b/include/core/crowdSimulator.h index 255f424..e498f22 100644 --- a/include/core/crowdSimulator.h +++ b/include/core/crowdSimulator.h @@ -106,6 +106,10 @@ private: bool FromConfigFile_loadAgentsBlock_ExternallyOrNot(const tinyxml2::XMLElement* agentsBlock, const std::string& fileFolder); bool FromConfigFile_loadAgentsBlock(const tinyxml2::XMLElement* agentsBlock); bool FromConfigFile_loadSingleAgent(const tinyxml2::XMLElement* agentElement); + + bool FromConfigFile_loadObstaclesBlock_ExternallyOrNot(const tinyxml2::XMLElement* obstaclesBlock, const std::string& fileFolder); + bool FromConfigFile_loadObstaclesBlock(const tinyxml2::XMLElement* obstaclesBlock); + bool FromConfigFile_loadSingleObstacle(const tinyxml2::XMLElement* obstacleElement); }; #endif diff --git a/include/core/worldBase.h b/include/core/worldBase.h index dac642b..8f49f1e 100644 --- a/include/core/worldBase.h +++ b/include/core/worldBase.h @@ -196,6 +196,10 @@ public: /// @} #pragma endregion + /// Adds an obstacle with the given vertices to the world. + /// A sequence of 2D points defining the obstacle's boundary vertices. + virtual void AddObstacle(const std::vector& points); + /// Cleans up this WorldBase object for removal. virtual ~WorldBase(); diff --git a/include/tools/vector2D.h b/include/tools/vector2D.h index 3ae1bfd..26f4046 100644 --- a/include/tools/vector2D.h +++ b/include/tools/vector2D.h @@ -263,7 +263,8 @@ inline Vector2D nearestPointOnLine(const Vector2D& pt, const Vector2D& la, const if (la == lb) return la; - float k = (pt - lb).dot(la - lb) / la.dot(lb); + Vector2D line(la - lb); + float k = (pt - lb).dot(line) / line.sqrMagnitude(); if (onSegment) { diff --git a/src/core/crowdSimulator.cpp b/src/core/crowdSimulator.cpp index 30817f3..554b0c1 100644 --- a/src/core/crowdSimulator.cpp +++ b/src/core/crowdSimulator.cpp @@ -124,6 +124,7 @@ std::string getFolder(const std::string& filename) bool CrowdSimulator::FromConfigFile_loadWorld(const tinyxml2::XMLElement* worldElement) { + // load the world type WorldBase::Type worldType = WorldBase::Type::UNKNOWN; const char* type = worldElement->Attribute("type"); if (type != nullptr) @@ -141,6 +142,7 @@ bool CrowdSimulator::FromConfigFile_loadWorld(const tinyxml2::XMLElement* worldE std::cout << "Created Infinite world." << std::endl; } + // for toric worlds, load the width and height else if (worldType == WorldBase::Type::TORIC) { float width = -1, height = -1; @@ -207,6 +209,28 @@ bool CrowdSimulator::FromConfigFile_loadAgentsBlock_ExternallyOrNot(const tinyxm return FromConfigFile_loadAgentsBlock(xmlBlock); } +bool CrowdSimulator::FromConfigFile_loadObstaclesBlock_ExternallyOrNot(const tinyxml2::XMLElement* xmlBlock, const std::string& fileFolder) +{ + // - if this block refers to another file, read it + const char* externalFilename = xmlBlock->Attribute("file"); + if (externalFilename != nullptr) + { + tinyxml2::XMLDocument externalDoc; + externalDoc.LoadFile((fileFolder + externalFilename).data()); + if (externalDoc.ErrorID() != 0) + { + std::cerr << "Could not load or parse Obstacles XML file at " << (fileFolder + externalFilename) << "." << std::endl + << "Please check this file location, or place your Obstacles in the main XML file itself." << std::endl; + return false; + } + + return FromConfigFile_loadObstaclesBlock(externalDoc.FirstChildElement("Obstacles")); + } + + // - otherwise, read the agents straight from the file itself + return FromConfigFile_loadObstaclesBlock(xmlBlock); +} + bool CrowdSimulator::FromConfigFile_loadPoliciesBlock(const tinyxml2::XMLElement* xmlBlock) { // load the elements one by one @@ -214,12 +238,11 @@ bool CrowdSimulator::FromConfigFile_loadPoliciesBlock(const tinyxml2::XMLElement while (element != nullptr) { // load a single element - bool success = FromConfigFile_loadSinglePolicy(element); - if (!success) + if (!FromConfigFile_loadSinglePolicy(element)) return false; // go to the next element - element = element->NextSiblingElement(); + element = element->NextSiblingElement("Policy"); } return true; @@ -232,12 +255,28 @@ bool CrowdSimulator::FromConfigFile_loadAgentsBlock(const tinyxml2::XMLElement* while (element != nullptr) { // load a single element - bool success = FromConfigFile_loadSingleAgent(element); - if (!success) + if (!FromConfigFile_loadSingleAgent(element)) + return false; + + // go to the next element + element = element->NextSiblingElement("Agent"); + } + + return true; +} + +bool CrowdSimulator::FromConfigFile_loadObstaclesBlock(const tinyxml2::XMLElement* xmlBlock) +{ + // load the elements one by one + const tinyxml2::XMLElement* element = xmlBlock->FirstChildElement(); + while (element != nullptr) + { + // load a single element + if (!FromConfigFile_loadSingleObstacle(element)) return false; // go to the next element - element = element->NextSiblingElement(); + element = element->NextSiblingElement("Obstacle"); } return true; @@ -411,6 +450,26 @@ bool CrowdSimulator::FromConfigFile_loadSingleAgent(const tinyxml2::XMLElement* return true; } +bool CrowdSimulator::FromConfigFile_loadSingleObstacle(const tinyxml2::XMLElement* obstacleElement) +{ + std::vector points; + + // load coordinates + auto* pointElement = obstacleElement->FirstChildElement("Point"); + while (pointElement != nullptr) + { + float x, y; + pointElement->QueryFloatAttribute("x", &x); + pointElement->QueryFloatAttribute("y", &y); + pointElement = pointElement->NextSiblingElement("Point"); + points.push_back(Vector2D(x, y)); + } + + // add obstacle to world + world_->AddObstacle(points); + return true; +} + CrowdSimulator* CrowdSimulator::FromConfigFile(const std::string& filename, bool isConsoleApplication) { // Parse the XML into the property tree. @@ -489,7 +548,7 @@ CrowdSimulator* CrowdSimulator::FromConfigFile(const std::string& filename, bool // read the block with all policies auto* policiesBlock = worldHandle.FirstChildElement("Policies").ToElement(); - if (!crowdsimulator->FromConfigFile_loadPoliciesBlock_ExternallyOrNot(policiesBlock, fileFolder)) + if (policiesBlock != nullptr && !crowdsimulator->FromConfigFile_loadPoliciesBlock_ExternallyOrNot(policiesBlock, fileFolder)) { std::cerr << "Error while loading policies. The simulation cannot be loaded." << std::endl; delete crowdsimulator; @@ -501,14 +560,14 @@ CrowdSimulator* CrowdSimulator::FromConfigFile(const std::string& filename, bool { std::cerr << "Error: Failed to load any policies for the simulation." << std::endl - << "A simulation needs at least one valid Policy element." << std::endl; + << "A simulation needs a Policy block with at least one valid Policy element." << std::endl; delete crowdsimulator; return nullptr; } // read the block with all agents - auto* agentsElement = docHandle.FirstChildElement("World").FirstChildElement("Agents").ToElement(); - if (!crowdsimulator->FromConfigFile_loadAgentsBlock_ExternallyOrNot(agentsElement, fileFolder)) + auto* agentsElement = worldHandle.FirstChildElement("Agents").ToElement(); + if (agentsElement != nullptr && !crowdsimulator->FromConfigFile_loadAgentsBlock_ExternallyOrNot(agentsElement, fileFolder)) { std::cerr << "Error while loading agents. The simulation cannot be loaded." << std::endl; delete crowdsimulator; @@ -522,6 +581,15 @@ CrowdSimulator* CrowdSimulator::FromConfigFile(const std::string& filename, bool << "The simulation will start without agents." << std::endl; } + // read the block with all obstacles + auto* obstaclesElement = worldHandle.FirstChildElement("Obstacles").ToElement(); + if (obstaclesElement != nullptr && !crowdsimulator->FromConfigFile_loadObstaclesBlock_ExternallyOrNot(obstaclesElement, fileFolder)) + { + std::cerr << "Error while loading obstacles. The simulation cannot be loaded." << std::endl; + delete crowdsimulator; + return nullptr; + } + return crowdsimulator; } diff --git a/src/core/worldBase.cpp b/src/core/worldBase.cpp index 6cebe4d..84b3adb 100644 --- a/src/core/worldBase.cpp +++ b/src/core/worldBase.cpp @@ -243,6 +243,11 @@ void WorldBase::removeAgentAtListIndex(size_t index) #pragma endregion +void WorldBase::AddObstacle(const std::vector& points) +{ + obstacles_.push_back(Polygon2D(points)); +} + WorldBase::~WorldBase() { // delete the KD tree -- GitLab From b4afbbd271df9c405e4d10f631da8b2d3f86ed92 Mon Sep 17 00:00:00 2001 From: Wouter van Toll Date: Thu, 13 Feb 2020 16:30:10 +0100 Subject: [PATCH 3/8] - Policy::ComputeContactForces(): Added obstacle forces. - CostFunction: Bugfix in time-to-collision with line segments. --- src/core/costFunction.cpp | 2 +- src/core/policy.cpp | 17 +++++++++++++---- 2 files changed, 14 insertions(+), 5 deletions(-) diff --git a/src/core/costFunction.cpp b/src/core/costFunction.cpp index 069494c..da6b8a3 100644 --- a/src/core/costFunction.cpp +++ b/src/core/costFunction.cpp @@ -319,7 +319,7 @@ float CostFunction::ComputeTimeToCollision_LineSegmentInterior(const Vector2D& p float distanceToSubtract = radius / sinf(lineAngle); float timeToSubtract = distanceToSubtract / speed; - return timeToCollision - timeToSubtract; + return std::max(0.0f, timeToCollision - timeToSubtract); } std::pair CostFunction::ComputeTimeAndDistanceToClosestApproach( diff --git a/src/core/policy.cpp b/src/core/policy.cpp index a44367c..5764793 100644 --- a/src/core/policy.cpp +++ b/src/core/policy.cpp @@ -101,6 +101,8 @@ Vector2D Policy::getNewVelocitySampling(Agent* agent, WorldBase * world) Vector2D Policy::ComputeContactForces(Agent* agent, WorldBase * world) { Vector2D totalForce(0, 0); + const Vector2D& position = agent->getPosition(); + const float radius = agent->getRadius(); if (contactForceScale_ > 0) { @@ -109,8 +111,8 @@ Vector2D Policy::ComputeContactForces(Agent* agent, WorldBase * world) // check all colliding agents for (const auto& neighborAgent : neighbors.first) { - const auto& diff = agent->getPosition() - neighborAgent.position; - const float intersectionDistance = agent->getRadius() + neighborAgent.realAgent->getRadius() - diff.magnitude(); + const auto& diff = position - neighborAgent.position; + const float intersectionDistance = radius + neighborAgent.realAgent->getRadius() - diff.magnitude(); if (intersectionDistance > 0) { const auto& F = diff.getnormalized() * (contactForceScale_ * intersectionDistance); @@ -118,10 +120,17 @@ Vector2D Policy::ComputeContactForces(Agent* agent, WorldBase * world) } } - // TODO: check all colliding obstacles + // check all colliding obstacles for (const auto& neighborObstacle : neighbors.second) { - + const auto& nearest = nearestPointOnLine(position, neighborObstacle.first, neighborObstacle.second, true); + const auto& diff = position - nearest; + const double intersectionDistance = radius - diff.magnitude(); + if (intersectionDistance > 0) + { + const auto& F = diff.getnormalized() * (contactForceScale_ * intersectionDistance); + totalForce = totalForce + F; + } } } -- GitLab From 6724d0e9fc08a655b28625c6c4a7981ac81af989 Mon Sep 17 00:00:00 2001 From: Wouter van Toll Date: Fri, 14 Feb 2020 15:28:51 +0100 Subject: [PATCH 4/8] Fixed a double/float mix-up --- src/core/policy.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/core/policy.cpp b/src/core/policy.cpp index 5764793..0f0c073 100644 --- a/src/core/policy.cpp +++ b/src/core/policy.cpp @@ -125,7 +125,7 @@ Vector2D Policy::ComputeContactForces(Agent* agent, WorldBase * world) { const auto& nearest = nearestPointOnLine(position, neighborObstacle.first, neighborObstacle.second, true); const auto& diff = position - nearest; - const double intersectionDistance = radius - diff.magnitude(); + const float intersectionDistance = radius - diff.magnitude(); if (intersectionDistance > 0) { const auto& F = diff.getnormalized() * (contactForceScale_ * intersectionDistance); -- GitLab From d5c67dc34454f3ad8401a0b1adccf9641a52d32e Mon Sep 17 00:00:00 2001 From: Wouter van Toll Date: Thu, 20 Feb 2020 16:11:49 +0100 Subject: [PATCH 5/8] Updated Doxygen comments for cost functions. Also added a warning whenever a cost function does not yet support obstacles. --- include/CostFunctions/Karamouzas.h | 12 +++++++--- include/CostFunctions/Moussaid.h | 11 +++++++-- include/CostFunctions/ORCA.h | 19 ++++++++++----- include/CostFunctions/PLEdestrians.h | 15 ++++++++---- include/CostFunctions/Paris.h | 16 +++++++++---- include/CostFunctions/PowerLaw.h | 15 +++++++++--- include/CostFunctions/RVO.h | 23 +++++++++++-------- include/CostFunctions/RandomFunction.h | 2 +- include/CostFunctions/SocialForcesAvoidance.h | 11 +++++++-- include/CostFunctions/TtcaDca.h | 20 +++++++++++----- include/CostFunctions/VanToll.h | 13 +++++++---- 11 files changed, 111 insertions(+), 46 deletions(-) diff --git a/include/CostFunctions/Karamouzas.h b/include/CostFunctions/Karamouzas.h index 2de9067..e87d1b0 100644 --- a/include/CostFunctions/Karamouzas.h +++ b/include/CostFunctions/Karamouzas.h @@ -28,15 +28,21 @@ /// An implementation of the collision-avoidance cost function proposed by %Karamouzas & Overmars, /// in the paper "A velocity-based approach for simulating human collision avoidance" (2010). /// -/// Note: This cost function already includes a goal-reaching component. -/// To reproduce the original method, use this cost function in a sampling-based policy. +/// ### Notes: +/// +/// This cost function already includes a goal-reaching component. +/// The %Karamouzas.xml policy in our example files reproduces the original algorithm. +/// +/// /// ### Name in XML files: /// %Karamouzas +/// /// ### Parameters: /// /// alpha, beta, gamma, delta (float): Scalars for the four main components of the cost function. /// t_max (float): The maximum time-to-collision to consider. -/// +/// +/// class Karamouzas : public CostFunction { private: diff --git a/include/CostFunctions/Moussaid.h b/include/CostFunctions/Moussaid.h index 3aaf704..b7305d0 100644 --- a/include/CostFunctions/Moussaid.h +++ b/include/CostFunctions/Moussaid.h @@ -28,13 +28,20 @@ /// An implementation of the full navigation function proposed by %Moussaid et al., /// in the paper "How simple rules determine pedestrian behavior and crowd disasters" (2011). /// -/// Note: This cost function already includes a goal-reaching component. +/// ### Notes: +/// +/// This cost function already includes a goal-reaching component. +/// The %Moussaid.xml policy in our example files reproduces the original algorithm. +/// +/// /// ### Name in XML files: /// %Moussaid +/// /// ### Parameters: /// /// d_max: The maximum distance-to-collision to consider. -/// +/// +/// class Moussaid : public CostFunction { private: diff --git a/include/CostFunctions/ORCA.h b/include/CostFunctions/ORCA.h index 36b80dc..12d5b50 100644 --- a/include/CostFunctions/ORCA.h +++ b/include/CostFunctions/ORCA.h @@ -29,18 +29,25 @@ /// The cost function of the %ORCA collision-avoidance model proposed by van den Berg et al., /// in the paper "Reciprocal n-body collision avoidance" (2011). /// -/// Note: This cost function already includes a goal-reaching component. -/// -/// The original paper describes a cost function, but also a closed-form solution for finding the optimal velocity. -/// Our implementation contains both options, using the original source code provided by the paper's authors. -/// To reproduce the original method, use this cost function in a global-optimization-based policy. +/// ### Notes: +/// +/// This cost function already includes a goal-reaching component. +/// The original paper describes a cost function, but also a closed-form solution for finding the optimal velocity. +/// Our implementation contains both options, using the original source code provided by the paper's authors. +/// The %ORCA.xml policy in our example files reproduces the original algorithm. +/// WARNING: This cost function does not yet support static obstacles, so any obstacles will be ignored for now. +/// This feature should be added in the future. The original ORCA code uses a different obstacle representation than we do, +/// which makes this integration a bit difficult. +/// /// /// ### Name in XML files: /// %ORCA +/// /// ### Parameters: /// /// timeHorizon (float): The time (in seconds) over which velocity obstacles are defined. -/// +/// +/// class ORCA : public CostFunction { private: diff --git a/include/CostFunctions/PLEdestrians.h b/include/CostFunctions/PLEdestrians.h index b78365e..3e9e647 100644 --- a/include/CostFunctions/PLEdestrians.h +++ b/include/CostFunctions/PLEdestrians.h @@ -28,19 +28,24 @@ /// An implementation of the full navigation fuction proposed by Guy et al., /// in the paper "PLEdestrians: A least-effort approach to crowd simulation" (2010). /// -/// Note: This cost function already includes a goal-reaching component. -/// -/// The original paper presents a closed-form solution for finding the optimal velocity. +/// ### Notes: +/// +/// This cost function already includes a goal-reaching component. +/// The original paper presents a closed-form solution for finding the optimal velocity. /// Our implementation (currently) does not contain this closed-form solution. -/// To obtain an approximation of the original method, use this cost function in a sampling-based policy. +/// To obtain an approximation of the original method, use this cost function in a sampling-based policy. +/// The %PLEdestrians.xml policy in our example files gives a good approximation of the original algorithm. +/// /// /// ### Name in XML files: /// %PLEdestrians +/// /// ### Parameters: /// /// w_a, w_b (float): two weights for the main components of the cost function. /// t_min, t_max (float): two time thresholds. -/// +/// +/// class PLEdestrians : public CostFunction { private: diff --git a/include/CostFunctions/Paris.h b/include/CostFunctions/Paris.h index 825f5d4..26c3b18 100644 --- a/include/CostFunctions/Paris.h +++ b/include/CostFunctions/Paris.h @@ -28,20 +28,28 @@ /// (Work in progress) A re-interpretation of the collision-avoidance model proposed by %Paris et al., /// in the paper "Pedestrian reactive navigation for crowd simulation - A predictive approach" (2007). /// -/// The original description of this navigation model is not entirely clear, and its source code cannot be retrieved. +/// ### Notes: +/// +/// This cost function already includes a goal-reaching component. +/// The original description of this navigation model is not entirely clear, and its source code cannot be retrieved. /// Therefore, this class is merely *our* interpretation of it, translated to the domain of cost functions. -/// There is no guarantee that this interpretation perfectly matches the authors' original intentions. +/// There is no guarantee that this interpretation perfectly matches the authors' original intentions. +/// The %Paris.xml policy in our example files attempts to reproduce the original algorithm, but (again) without any guarantees. +/// WARNING: This cost function does not yet support static obstacles, so any obstacles will be ignored for now. +/// This feature should be added in the future. Unfortunately, the original paper does not clearly define what to do with obstacles. +/// /// -/// Note: This cost function already includes a goal-reaching component. /// ### Name in XML files: /// %Paris +/// /// ### Parameters: /// /// w_a (float): A weight for the "collision avoidance" component of the cost function. /// Determines the ratio between the "collision avoidance" and "preferred motion" components. /// w_b (float): A scalar that determines how the "collision avoidance" importance of a neighbor decreases as time-to-collision increases. /// t_max (float): The maximum time-to-collision to consider. -/// +/// +/// class Paris : public CostFunction { private: diff --git a/include/CostFunctions/PowerLaw.h b/include/CostFunctions/PowerLaw.h index 14b38a2..e87ed70 100644 --- a/include/CostFunctions/PowerLaw.h +++ b/include/CostFunctions/PowerLaw.h @@ -31,11 +31,20 @@ /// /// This paper defines a force per neighboring agent, and thus the implementation inherits from AgentInteractionForces. /// -/// Note: This cost function does *not* include a goal-reaching component. -/// To reproduce the full navigation method, use a gradient-based policy that combines this force with GoalReachingForce. -/// +/// ### Notes: +/// +/// This cost function does *not* include a goal-reaching component. +/// Thus, to bring agents to their goals, you need to combine this cost function with (for example) GoalReachingForce. +/// The %PowerLaw.xml policy in our example files reproduces the original algorithm. +/// using two cost functions: PowerLaw and GoalReachingForce. +/// WARNING: This cost function does not yet support static obstacles, so any obstacles will be ignored for now. +/// This feature should be added in the future. Unfortunately, the original paper does not describe how obstacles are handled, +/// and its original implementation cannot be freely used. +/// +/// /// ### Name in XML files: /// %PowerLaw +/// /// ### Parameters: /// /// k (float): A scaling factor for the overall force. diff --git a/include/CostFunctions/RVO.h b/include/CostFunctions/RVO.h index 9e8c5f4..aa93c5c 100644 --- a/include/CostFunctions/RVO.h +++ b/include/CostFunctions/RVO.h @@ -27,23 +27,26 @@ /// @ingroup costfunctions /// An implementation of the full navigation function proposed by van den Berg et al., /// in the paper "Reciprocal velocity obstacles for real-time multi-agent navigation" (2008). -/// Although the %RVO paper describes the navigation problem in a closed mathematical form, +/// +/// ### Notes: +/// +/// This cost function already includes a goal-reaching component. +/// Although the %RVO paper describes the navigation problem in a closed mathematical form, /// the authors eventually propose a sampling-based implementation. -/// Thus, our implementation does not contain a closed-form solution for finding the optimal velocity. -/// -/// Note: This is *not* the %ORCA method (sometimes referred to as "RVO2") that uses linear programming. +/// Thus, our implementation does not contain a closed-form solution for finding the optimal velocity. +/// This is **not** the %ORCA method (sometimes referred to as "RVO2") that uses linear programming. +/// If you want to use that, use the ORCA cost function instead. +/// The %RVO.xml policy in our example files reproduces the original algorithm. +/// /// -/// Note: This cost function already includes a goal-reaching component. -/// To reproduce the original method, use this cost function in a sampling-based policy. -/// -/// ### Definition: -/// cost(v') = w / TimeToCollision(v') + || vPref - v' || /// ### Name in XML files: /// %RVO +/// /// ### Parameters: /// /// w: A weight for one of the two main components of the cost function. -/// +/// +/// class RVO : public CostFunction { private: diff --git a/include/CostFunctions/RandomFunction.h b/include/CostFunctions/RandomFunction.h index ca390fe..0f38097 100644 --- a/include/CostFunctions/RandomFunction.h +++ b/include/CostFunctions/RandomFunction.h @@ -30,7 +30,7 @@ /// ### Name in XML files: /// %RandomFunction /// ### Parameters: -/// None +/// None. You can use the common coeff parameter to scale the effect of this cost function. /// class RandomFunction : public CostFunction { diff --git a/include/CostFunctions/SocialForcesAvoidance.h b/include/CostFunctions/SocialForcesAvoidance.h index da8127c..c00b4df 100644 --- a/include/CostFunctions/SocialForcesAvoidance.h +++ b/include/CostFunctions/SocialForcesAvoidance.h @@ -30,11 +30,18 @@ /// /// This paper defines a force per neighboring agent, and thus the implementation inherits from AgentInteractionForces. /// -/// Note: This cost function does *not* include a goal-reaching component. -/// To reproduce the full navigation method, use a gradient-based policy that combines this force with GoalReachingForce. +/// ### Notes: +/// +/// This cost function does *not* include a goal-reaching component. +/// Thus, to bring agents to their goals, you need to combine this cost function with (for example) GoalReachingForce. +/// The %SocialForces.xml policy in our example files reproduces the original algorithm, +/// using two cost functions: SocialForces and GoalReachingForce. +/// WARNING: This cost function does not yet support static obstacles, so any obstacles will be ignored for now. +/// This feature should be added in the future. /// /// ### Name in XML files: /// %SocialForcesAvoidance +/// /// ### Parameters: /// /// dt (float): A time window parameter used in force calculations. It does not have a very intuitive meaning. diff --git a/include/CostFunctions/TtcaDca.h b/include/CostFunctions/TtcaDca.h index 0362136..a45c7ab 100644 --- a/include/CostFunctions/TtcaDca.h +++ b/include/CostFunctions/TtcaDca.h @@ -28,17 +28,25 @@ /// An implementation of the full navigation function proposed by Dutra et al., /// in the paper "Gradient-based steering for vision-based crowd simulation algorithms" (2017). /// This cost function is based on the the time and distance to closest approach (ttca, dca). -/// The gradient of this function is well-defined, so it is suitable for gradient-based navigation. -/// -/// In fact, the original algorithm by Dutra et al. uses only the gradient and not the cost itself. +/// The original algorithm by Dutra et al. uses the gradient of this function (which has a closed form), and not the cost itself. /// Thus, to reproduce the full navigation method, use a policy with gradient-based optimization. /// -/// Note: This cost function already includes a goal-reaching component. -/// To reproduce the original method, use this cost function in a gradient-based policy. +/// ### Notes: +/// +/// This cost function already includes a goal-reaching component. +/// The %Dutra.xml policy in our example files gives a good approximation of the original algorithm. +/// This algorithm originally simulates the agent's vision via rendering. We use a simple approximation of this. +/// WARNING: Currently, TtcaDca::GetGradient() actually returns an approximation of the gradient (the standard one of CostFunction), +/// and not the analytical one from the original paper. This is because the analytical gradient seems to yield incorrect behavior. +/// At the moment, it is unclear whether this is a problem in our implementation or in the original equations. +/// WARNING: This cost function does not yet support static obstacles, so any obstacles will be ignored for now. +/// This feature should be added in the future. To do this, we need to implement the concept of ttca/dca for line-segment obstacles, +/// think of a good way to scale the cost by distance, and determine the gradient of this component. +/// /// -/// This algorithm originally simulates the agent's vision via rendering. We use a simple approximation of this. /// ### Name in XML files: /// %TtcaDca +/// /// ### Parameters: /// /// sigmaTtca, sigmaDca (float): Scalars that define the steepness of two exponential functions in the "collision avoidance" part. diff --git a/include/CostFunctions/VanToll.h b/include/CostFunctions/VanToll.h index a7e40de..10eafbb 100644 --- a/include/CostFunctions/VanToll.h +++ b/include/CostFunctions/VanToll.h @@ -28,15 +28,20 @@ /// A custom navigation function by Wouter van Toll, /// also used in the paper "Connecting global and local agent navigation via topology" (2019). /// -/// Note: This cost function already includes a goal-reaching component. -/// To reproduce the original method, use this cost function in a sampling-based policy. -/// +/// ### Notes: +/// +/// This cost function already includes a goal-reaching component. +/// The %VanToll.xml policy in our example files reproduces the original algorithm. +/// +/// /// ### Name in XML files: /// %VanToll +/// /// ### Parameters: /// /// max_distance: The maximum distance-to-collision to consider. -/// +/// +/// class VanToll : public CostFunction { private: -- GitLab From 91d0ad6a9290a893374db79744f4d213ae45a1c4 Mon Sep 17 00:00:00 2001 From: Wouter van Toll Date: Thu, 20 Feb 2020 16:31:24 +0100 Subject: [PATCH 6/8] - Renamed AgentInteractionForces to ObjectInteractionForces. - Added the method ComputeObstacleInteractionForce() to the two subclasses, PowerLaw and SocialForcesAvoidance. Currently, these methods return the zero vector. --- ...tionForces.h => ObjectInteractionForces.h} | 26 ++++++++++++------- include/CostFunctions/PowerLaw.h | 9 ++++--- include/CostFunctions/SocialForcesAvoidance.h | 11 ++++---- ...Forces.cpp => ObjectInteractionForces.cpp} | 10 ++++--- src/CostFunctions/PowerLaw.cpp | 6 +++++ src/CostFunctions/SocialForcesAvoidance.cpp | 10 +++++-- 6 files changed, 49 insertions(+), 23 deletions(-) rename include/CostFunctions/{AgentInteractionForces.h => ObjectInteractionForces.h} (64%) rename src/CostFunctions/{AgentInteractionForces.cpp => ObjectInteractionForces.cpp} (79%) diff --git a/include/CostFunctions/AgentInteractionForces.h b/include/CostFunctions/ObjectInteractionForces.h similarity index 64% rename from include/CostFunctions/AgentInteractionForces.h rename to include/CostFunctions/ObjectInteractionForces.h index 9709f24..e0d59ff 100644 --- a/include/CostFunctions/AgentInteractionForces.h +++ b/include/CostFunctions/ObjectInteractionForces.h @@ -19,22 +19,23 @@ ** See the file AUTHORS.md for a list of all contributors. */ -#ifndef LIB_AGENT_INTERACTION_FORCES_H -#define LIB_AGENT_INTERACTION_FORCES_H +#ifndef LIB_OBJECT_INTERACTION_FORCES_H +#define LIB_OBJECT_INTERACTION_FORCES_H #include -/// An abstract class for a force-based function that computes a force per neighboring agent and sums them up. +/// An abstract class for a force-based function that computes a force per neighboring agent/obstacle and sums them up. /// The specific force per neighbor needs to be filled in by a child class, in the method ComputeAgentInteractionForce(). -class AgentInteractionForces : public ForceBasedFunction +class ObjectInteractionForces : public ForceBasedFunction { protected: - AgentInteractionForces() : ForceBasedFunction() {} - virtual ~AgentInteractionForces() {} + ObjectInteractionForces() : ForceBasedFunction() {} + virtual ~ObjectInteractionForces() {} protected: /// Computes and returns a 2D force vector that the agent experiences. - /// In the case of AgentInteractionForces, this method calls ComputeAgentInteractionForce() for each neighbor of the querying agent, + /// In the case of ObjectInteractionForces, + /// this method calls ComputeAgentInteractionForce() and ComputeObstacleInteractionForce() for each neighbor of the querying agent, /// and returns the sum of all these interaction forces. /// The agent for which a force is requested. /// The world in which the simulation takes place. @@ -42,11 +43,18 @@ protected: virtual Vector2D ComputeForce(Agent* agent, const WorldBase* world) const override; /// Computes a 2D force vector that a given agent experiences due to a neighboring agent. - /// Subclasses of AgentInteractionForces must implement this function. + /// Subclasses of ObjectInteractionForces must implement this function. /// The agent for which a force is requested. /// The neighboring agent. /// A 2D vector describing the force that 'other' applies to 'agent', according to a particular force model. virtual Vector2D ComputeAgentInteractionForce(const Agent* agent, const PhantomAgent& other) const = 0; + + /// Computes a 2D force vector that a given agent experiences due to a neighboring obstacle segment. + /// Subclasses of ObjectInteractionForces must implement this function. + /// The agent for which a force is requested. + /// The neighboring obstacle segment. + /// A 2D vector describing the force that 'obstacle' applies to 'agent', according to a particular force model. + virtual Vector2D ComputeObstacleInteractionForce(const Agent* agent, const LineSegment2D& obstacle) const = 0; }; -#endif //LIB_AGENT_INTERACTION_FORCE_H +#endif //LIB_OBJECT_INTERACTION_FORCES_H diff --git a/include/CostFunctions/PowerLaw.h b/include/CostFunctions/PowerLaw.h index e87ed70..ad500fe 100644 --- a/include/CostFunctions/PowerLaw.h +++ b/include/CostFunctions/PowerLaw.h @@ -22,14 +22,14 @@ #ifndef LIB_POWER_LAW_H #define LIB_POWER_LAW_H -#include +#include #include /// @ingroup costfunctions /// An implementation of the collision-avoidance force proposed by %Karamouzas et al., /// in the paper "Universal Power Law Governing Pedestrian Interactions" (2014). /// -/// This paper defines a force per neighboring agent, and thus the implementation inherits from AgentInteractionForces. +/// This paper defines a force per neighboring agent, and thus the implementation inherits from ObjectInteractionForces. /// /// ### Notes: /// @@ -51,14 +51,14 @@ /// tau0 (float): A time threshold, which could be interpreted as the "maximum time-to-collision of interest". /// /// -class PowerLaw : public AgentInteractionForces +class PowerLaw : public ObjectInteractionForces { private: float k = 1.5f; float tau0 = 3; public: - PowerLaw() : AgentInteractionForces() {} + PowerLaw() : ObjectInteractionForces() {} virtual ~PowerLaw() {} const static std::string GetName() { return "PowerLaw"; } @@ -66,6 +66,7 @@ public: protected: virtual Vector2D ComputeAgentInteractionForce(const Agent* agent, const PhantomAgent& other) const override; + virtual Vector2D ComputeObstacleInteractionForce(const Agent* agent, const LineSegment2D& obstacle) const override; }; #endif //LIB_POWER_LAW_H diff --git a/include/CostFunctions/SocialForcesAvoidance.h b/include/CostFunctions/SocialForcesAvoidance.h index c00b4df..46b98ef 100644 --- a/include/CostFunctions/SocialForcesAvoidance.h +++ b/include/CostFunctions/SocialForcesAvoidance.h @@ -22,13 +22,13 @@ #ifndef LIB_SOCIAL_FORCES_H #define LIB_SOCIAL_FORCES_H -#include +#include /// @ingroup costfunctions /// An implementation of the collision-avoidance force proposed by Helbing and Molnar, /// in the paper "Social force model for pedestrian dynamics" (1995). /// -/// This paper defines a force per neighboring agent, and thus the implementation inherits from AgentInteractionForces. +/// This paper defines a force per neighboring agent, and thus the implementation inherits from ObjectInteractionForces. /// /// ### Notes: /// @@ -49,7 +49,7 @@ /// sigma (float): Determines the steepness of the exponential function defining the inter-agent force. /// /// -class SocialForcesAvoidance : public AgentInteractionForces +class SocialForcesAvoidance : public ObjectInteractionForces { private: float dt = 1; @@ -57,13 +57,14 @@ private: float sigma = 0.3f; public: - SocialForcesAvoidance() : AgentInteractionForces() {} + SocialForcesAvoidance() : ObjectInteractionForces() {} virtual ~SocialForcesAvoidance() {} const static std::string GetName() { return "SocialForcesAvoidance"; } + void parseParameters(const CostFunctionParameters & params) override; protected: virtual Vector2D ComputeAgentInteractionForce(const Agent* agent, const PhantomAgent& other) const override; - void parseParameters(const CostFunctionParameters & params) override; + virtual Vector2D ComputeObstacleInteractionForce(const Agent* agent, const LineSegment2D& obstacle) const override; }; #endif //LIB_SOCIAL_FORCES_H diff --git a/src/CostFunctions/AgentInteractionForces.cpp b/src/CostFunctions/ObjectInteractionForces.cpp similarity index 79% rename from src/CostFunctions/AgentInteractionForces.cpp rename to src/CostFunctions/ObjectInteractionForces.cpp index 6c87fc9..c5375d0 100644 --- a/src/CostFunctions/AgentInteractionForces.cpp +++ b/src/CostFunctions/ObjectInteractionForces.cpp @@ -19,11 +19,11 @@ ** See the file AUTHORS.md for a list of all contributors. */ -#include +#include #include #include -Vector2D AgentInteractionForces::ComputeForce(Agent* agent, const WorldBase * world) const +Vector2D ObjectInteractionForces::ComputeForce(Agent* agent, const WorldBase * world) const { // loop over all neighbors; sum up the forces per neighbor Vector2D AgentForces(0, 0); @@ -31,5 +31,9 @@ Vector2D AgentInteractionForces::ComputeForce(Agent* agent, const WorldBase * wo for (const PhantomAgent& other : neighbors.first) AgentForces += ComputeAgentInteractionForce(agent, other); - return AgentForces; + Vector2D ObstacleForces(0, 0); + for (const LineSegment2D& obs : neighbors.second) + ObstacleForces += ComputeObstacleInteractionForce(agent, obs); + + return AgentForces + ObstacleForces; } diff --git a/src/CostFunctions/PowerLaw.cpp b/src/CostFunctions/PowerLaw.cpp index 22ea014..33cd20b 100644 --- a/src/CostFunctions/PowerLaw.cpp +++ b/src/CostFunctions/PowerLaw.cpp @@ -61,6 +61,12 @@ Vector2D PowerLaw::ComputeAgentInteractionForce(const Agent* agent, const Phanto return component1 * component2; } +Vector2D PowerLaw::ComputeObstacleInteractionForce(const Agent* agent, const LineSegment2D& obstacle) const +{ + // TODO: implement obstacle force + return Vector2D(0, 0); +} + void PowerLaw::parseParameters(const CostFunctionParameters & params) { ForceBasedFunction::parseParameters(params); diff --git a/src/CostFunctions/SocialForcesAvoidance.cpp b/src/CostFunctions/SocialForcesAvoidance.cpp index c2d1c39..a68890d 100644 --- a/src/CostFunctions/SocialForcesAvoidance.cpp +++ b/src/CostFunctions/SocialForcesAvoidance.cpp @@ -45,13 +45,19 @@ Vector2D SocialForcesAvoidance::ComputeAgentInteractionForce(const Agent* agent, if (B < 0.001) return Vector2D(0,0); - // Potential = repulsion_scale * e^(-b / repulsion_sigma) - // Derivative = repulsion_scale * e^(-b / sigma) * DB, where DB = the following: + // Potential = scale * e^(-B / sigma) + // Derivative = scale * e^(-B / sigma) * DB, where DB = the following: const Vector2D& DB = (1 / (4 * B))*(pow(magR + magR2, 2) / (magR * magR2))*R; return scale * exp(-B / sigma) * DB; } +Vector2D SocialForcesAvoidance::ComputeObstacleInteractionForce(const Agent* agent, const LineSegment2D& obstacle) const +{ + // TODO: implement obstacle force + return Vector2D(0, 0); +} + void SocialForcesAvoidance::parseParameters(const CostFunctionParameters & params) { ForceBasedFunction::parseParameters(params); -- GitLab From d0778883fcc1a93d4cd48180894e5f8538cb81e1 Mon Sep 17 00:00:00 2001 From: Wouter van Toll Date: Thu, 20 Feb 2020 17:59:07 +0100 Subject: [PATCH 7/8] SocialForcesAvoidance: - Implemented obstacle avoidance force. - Renamed parameters to match the Helbing/Molnar paper, and explained them a bit better in the documentation. --- include/CostFunctions/SocialForcesAvoidance.h | 12 ++++++-- src/CostFunctions/SocialForcesAvoidance.cpp | 28 ++++++++++++++----- 2 files changed, 30 insertions(+), 10 deletions(-) diff --git a/include/CostFunctions/SocialForcesAvoidance.h b/include/CostFunctions/SocialForcesAvoidance.h index 46b98ef..6edb16b 100644 --- a/include/CostFunctions/SocialForcesAvoidance.h +++ b/include/CostFunctions/SocialForcesAvoidance.h @@ -45,16 +45,22 @@ /// ### Parameters: /// /// dt (float): A time window parameter used in force calculations. It does not have a very intuitive meaning. -/// scale (float): A scaling factor for the overall force. -/// sigma (float): Determines the steepness of the exponential function defining the inter-agent force. +/// V0 (float): A scaling factor for all agent-avoidance forces. +/// sigma (float): Determines the steepness of the exponential function defining an agent-avoidance force. +/// Could be interpreted as the distance (in meters) that an agent wants to keep from another agent. +/// U0 (float): A scaling factor for all obstacle-avoidance forces. +/// R (float): Determines the steepness of the exponential function defining an obstacle-avoidance force. +/// Could be interpreted as the distance (in meters) that an agent wants to keep from obstacles. /// /// class SocialForcesAvoidance : public ObjectInteractionForces { private: float dt = 1; - float scale = 2.1f; + float V0 = 2.1f; float sigma = 0.3f; + float U0 = 10; + float R = 0.2f; public: SocialForcesAvoidance() : ObjectInteractionForces() {} diff --git a/src/CostFunctions/SocialForcesAvoidance.cpp b/src/CostFunctions/SocialForcesAvoidance.cpp index a68890d..c337e6e 100644 --- a/src/CostFunctions/SocialForcesAvoidance.cpp +++ b/src/CostFunctions/SocialForcesAvoidance.cpp @@ -42,26 +42,40 @@ Vector2D SocialForcesAvoidance::ComputeAgentInteractionForce(const Agent* agent, return Vector2D(0, 0); float B = 0.5f*sqrtf(Bsquared2); - if (B < 0.001) + if (B < 0.001f) return Vector2D(0,0); - // Potential = scale * e^(-B / sigma) - // Derivative = scale * e^(-B / sigma) * DB, where DB = the following: + // Potential = V0 * e^(-B / sigma) + // Derivative = V0 * e^(-B / sigma) * DB / sigma, where DB = the following: const Vector2D& DB = (1 / (4 * B))*(pow(magR + magR2, 2) / (magR * magR2))*R; - return scale * exp(-B / sigma) * DB; + return V0 * exp(-B / sigma) * DB; } Vector2D SocialForcesAvoidance::ComputeObstacleInteractionForce(const Agent* agent, const LineSegment2D& obstacle) const { - // TODO: implement obstacle force - return Vector2D(0, 0); + // find the nearest point on the obstacle + const Vector2D& nearest = nearestPointOnLine(agent->getPosition(), obstacle.first, obstacle.second, true); + const Vector2D& diff = agent->getPosition() - nearest; + + // if the distance is extremely small, ignore it + float dist = diff.magnitude(); + if (dist < 0.001f) + return Vector2D(0, 0); + + // Potential = U0 * e^(dist / R) + // Derivative = U0 * e^(dist / R) * 1/R, because dist can be treated as a constant here + // (unlike B in the agent-interaction force, which depends on the difference between agent positions) + float derivateOfPotential = U0 * exp(dist / R) / R; + return -diff.getnormalized() * derivateOfPotential; } void SocialForcesAvoidance::parseParameters(const CostFunctionParameters & params) { ForceBasedFunction::parseParameters(params); params.ReadFloat("dt", dt); - params.ReadFloat("scale", scale); + params.ReadFloat("V0", V0); params.ReadFloat("sigma", sigma); + params.ReadFloat("U0", V0); + params.ReadFloat("R", R); } -- GitLab From 42512dc2565dc8c129638c66fc1220532363bd94 Mon Sep 17 00:00:00 2001 From: Wouter van Toll Date: Thu, 20 Feb 2020 17:59:49 +0100 Subject: [PATCH 8/8] SocialForcesAvoidance: Removed the notice that obstacles are not supported. --- include/CostFunctions/SocialForcesAvoidance.h | 2 -- 1 file changed, 2 deletions(-) diff --git a/include/CostFunctions/SocialForcesAvoidance.h b/include/CostFunctions/SocialForcesAvoidance.h index 6edb16b..c6bf8fa 100644 --- a/include/CostFunctions/SocialForcesAvoidance.h +++ b/include/CostFunctions/SocialForcesAvoidance.h @@ -36,8 +36,6 @@ /// Thus, to bring agents to their goals, you need to combine this cost function with (for example) GoalReachingForce. /// The %SocialForces.xml policy in our example files reproduces the original algorithm, /// using two cost functions: SocialForces and GoalReachingForce. -/// WARNING: This cost function does not yet support static obstacles, so any obstacles will be ignored for now. -/// This feature should be added in the future. /// /// ### Name in XML files: /// %SocialForcesAvoidance -- GitLab