diff --git a/3rd-party/ORCA/ORCASolver.cpp b/3rd-party/ORCA/ORCASolver.cpp index 834da1eb367c32e9e8433780f203b60d6ed143e9..5d198547bb137dfcad6c2b55c686dd0015d1d2cb 100644 --- a/3rd-party/ORCA/ORCASolver.cpp +++ b/3rd-party/ORCA/ORCASolver.cpp @@ -75,7 +75,7 @@ namespace ORCALibrary Solution result; - createAgentOrcaLines(agent, result.orcaLines, timeHorizon, simulationTimeStep, neighbors); + createAgentOrcaLines(agent, result.orcaLines, timeHorizon, simulationTimeStep, neighbors.first); // TODO: include obstacle lines // ... diff --git a/include/CostFunctions/Karamouzas.h b/include/CostFunctions/Karamouzas.h index 2de90678937198154a55d9b611ac4267588311d2..e87d1b06a5fb367890b86eaa7e0495d52f85d3a1 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 3aaf7048b0054d7577d2650bbf85aef6915212ca..b7305d0511c174eb4f65a9f8a52a5b4e467c4647 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 36b80dc3bf7db92a5d34d9eb5f0a6c7b94bce262..12d5b50a684e0f6118c53cb4183ad9596c81493e 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/AgentInteractionForces.h b/include/CostFunctions/ObjectInteractionForces.h similarity index 64% rename from include/CostFunctions/AgentInteractionForces.h rename to include/CostFunctions/ObjectInteractionForces.h index 9709f242ba40e0b96fc07129c44e858771124cbf..e0d59ffef502aef69b995aeb92d86ae38eb017b9 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/PLEdestrians.h b/include/CostFunctions/PLEdestrians.h index b78365e24d52aad514635d8378009730634a5cf3..3e9e647c10eadb9fb10649a27e9ba2a11925d7fe 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 825f5d4a5763941a06e3d3c1cceea77e604ddd31..26c3b18bb0f54354034a3e5ade8fdb9142e10847 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 14b38a2c43c80421c1005732dd9db674d1c93910..ad500fe551867cfd30b038110a3b48ba3f15fdcb 100644 --- a/include/CostFunctions/PowerLaw.h +++ b/include/CostFunctions/PowerLaw.h @@ -22,34 +22,43 @@ #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. -/// -/// 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. +/// This paper defines a force per neighboring agent, and thus the implementation inherits from ObjectInteractionForces. /// +/// ### 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. /// 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"; } @@ -57,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/RVO.h b/include/CostFunctions/RVO.h index 9e8c5f4be650273787f8d516d4e37b00caefc047..aa93c5c7c6b5e72559eb8309ed684ebc204dad0d 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 ca390fea538bfc30938ba39d0fb196b700e25edb..0f3809714d9586eb6c9473621e8e4d44e21ffdff 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 da8127c19074615cc2292f1739076097bbd4993e..c6bf8fa8f0b207caa7cd0e5e26278bd5ae215ca7 100644 --- a/include/CostFunctions/SocialForcesAvoidance.h +++ b/include/CostFunctions/SocialForcesAvoidance.h @@ -22,41 +22,53 @@ #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. /// -/// 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. /// /// ### Name in XML files: /// %SocialForcesAvoidance +/// /// ### 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 AgentInteractionForces +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() : 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/include/CostFunctions/TtcaDca.h b/include/CostFunctions/TtcaDca.h index 03621363ac0f7b10628761837fe6c88624c9780b..a45c7abe9f19bb0bef636937febc4051dc156ac3 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 a7e40decce7fbc62aaa243cf9f567b4b8674460d..10eafbba0a96d1eff73d17cee515cfcad7fcec09 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: diff --git a/include/core/costFunction.h b/include/core/costFunction.h index e8109b7ffd9fd40b3f9e90a29360f570be2e6dd8..e824e2cb8abd5a0c8a2571d3f7ca7206df7d9715 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/crowdSimulator.h b/include/core/crowdSimulator.h index 21d476c5b6df0b5ca4111b31d8f98a546c76a66e..882039a988217007fa7dff4460f6fb51810796c8 100644 --- a/include/core/crowdSimulator.h +++ b/include/core/crowdSimulator.h @@ -104,6 +104,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 1f4f9f8d6da8dccb7c4044db93cf1a5f3de60b70..541c91160591b55dc633642c235f0e5c61375ca7 100644 --- a/include/core/worldBase.h +++ b/include/core/worldBase.h @@ -28,8 +28,9 @@ #include #include // std::reference_wrapper -#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_; @@ -193,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(); @@ -208,7 +215,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 e04a6ff964e27da296fe42250a50baca7e8a4f8a..73495d0dcb9667ebaa6c03d02e9f3557af280c39 100644 --- a/include/core/worldInfinite.h +++ b/include/core/worldInfinite.h @@ -23,15 +23,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 144ddd151b7c12ecd2d670514d8e7c44ce1c833b..ba219468f88e501752e6c3041c546bc87d083b84 100644 --- a/include/core/worldToric.h +++ b/include/core/worldToric.h @@ -22,14 +22,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, @@ -57,7 +50,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 61% rename from include/core/obstacle.h rename to include/tools/Polygon2D.h index 3d2b532471bdd5c3b4131b39aa70a9a4fc544b8f..fed2c7717c9686e133ec09a1338942e6fe2a1c21 100644 --- a/include/core/obstacle.h +++ b/include/tools/Polygon2D.h @@ -19,22 +19,30 @@ ** See the file AUTHORS.md for a list of all contributors. */ -#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 f429119bd90d0acda634490c80ffc6d1079af52c..3e50f3810c053f9a4aaaa6afdc7b1af7758f6474 100644 --- a/include/tools/vector2D.h +++ b/include/tools/vector2D.h @@ -23,6 +23,7 @@ #define LIB_VECTOR2D_H #include +#include const double PI = 3.1415926535897; @@ -119,6 +120,8 @@ public: } }; +typedef std::pair LineSegment2D; + #pragma region [Arithmetic operators] inline Vector2D operator-(Vector2D lhs, Vector2D rhs) @@ -193,7 +196,11 @@ inline float angle(const Vector2D& va, const Vector2D& vb) 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) @@ -254,4 +261,33 @@ 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; + + Vector2D line(la - lb); + float k = (pt - lb).dot(line) / line.sqrMagnitude(); + + 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/FOEAvoidance.cpp b/src/CostFunctions/FOEAvoidance.cpp index fb3d7675fbbd2d479def1ce740d8f0c1a2c13b8f..909aa8a6f9e3ca181b32cd167d8286b4349e6ab5 100644 --- a/src/CostFunctions/FOEAvoidance.cpp +++ b/src/CostFunctions/FOEAvoidance.cpp @@ -58,7 +58,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); @@ -78,6 +80,9 @@ float FOEAvoidance::GetCost(const Vector2D& velocity, Agent* agent, const WorldB Cost += localCost; } + // TODO: check neighboring obstacles + // ... + return Cost; } @@ -93,7 +98,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); @@ -115,6 +122,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 638b9c9859a5f2d6da0b29b17f498c932827c133..4f77552ea4db2176b39e37fe160f4f4fa3b705c0 100644 --- a/src/CostFunctions/Karamouzas.cpp +++ b/src/CostFunctions/Karamouzas.cpp @@ -33,7 +33,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)) @@ -44,7 +44,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 489391fa1a41438337a31c2719d45f8d96e04eb8..8af89465c0ee88667e9e909cf6c2de1d6eb72ebe 100644 --- a/src/CostFunctions/Moussaid.cpp +++ b/src/CostFunctions/Moussaid.cpp @@ -31,7 +31,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/AgentInteractionForces.cpp b/src/CostFunctions/ObjectInteractionForces.cpp similarity index 76% rename from src/CostFunctions/AgentInteractionForces.cpp rename to src/CostFunctions/ObjectInteractionForces.cpp index dfcc13a99324e31f540f53ed6aea4f9d161a380c..c5375d0acfdbc6ab9a26d8a0654c56069f82b52b 100644 --- a/src/CostFunctions/AgentInteractionForces.cpp +++ b/src/CostFunctions/ObjectInteractionForces.cpp @@ -19,17 +19,21 @@ ** 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); const auto& neighbors = agent->getNeighborSubsetInRange(range_); - for (const PhantomAgent& other : neighbors) + 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/PLEdestrians.cpp b/src/CostFunctions/PLEdestrians.cpp index d4176aa9cd24f098471031956340d08673295e4d..a9cd6a89d382fa305c246a4ca8c1181ded487618 100644 --- a/src/CostFunctions/PLEdestrians.cpp +++ b/src/CostFunctions/PLEdestrians.cpp @@ -27,7 +27,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 28b06e9be9c7ff702be830cb2fc84bac105e7014..a76365e84ee1bf3ff43d6eb7e93441c7cc60d5f6 100644 --- a/src/CostFunctions/Paris.cpp +++ b/src/CostFunctions/Paris.cpp @@ -49,7 +49,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; @@ -107,7 +107,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) @@ -137,8 +137,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/PowerLaw.cpp b/src/CostFunctions/PowerLaw.cpp index 22ea014c7647d2656b17fca4f06dbff1bfe09b86..33cd20b0f30880eb0712133aed18c46671121b77 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/RVO.cpp b/src/CostFunctions/RVO.cpp index 9c0f3e2eee7580d480cb09bfd6a6415aff634aa0..8af9801e73b4cf31d0b395aa5c0a667cd79c4dea 100644 --- a/src/CostFunctions/RVO.cpp +++ b/src/CostFunctions/RVO.cpp @@ -38,7 +38,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/SocialForcesAvoidance.cpp b/src/CostFunctions/SocialForcesAvoidance.cpp index c2d1c394b918d4ef0bdc503553a351877dd946c0..c337e6ede4bad46e8dccb7e29a81c2914e7b10e3 100644 --- a/src/CostFunctions/SocialForcesAvoidance.cpp +++ b/src/CostFunctions/SocialForcesAvoidance.cpp @@ -42,20 +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 = repulsion_scale * e^(-b / repulsion_sigma) - // Derivative = repulsion_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 +{ + // 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); } diff --git a/src/CostFunctions/TtcaDca.cpp b/src/CostFunctions/TtcaDca.cpp index 08beb3eb967778dff1482c8af884f705a011da32..38a9618a93fe9a84ea32b43a3ce9ad613e95bb7b 100644 --- a/src/CostFunctions/TtcaDca.cpp +++ b/src/CostFunctions/TtcaDca.cpp @@ -73,9 +73,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; @@ -90,8 +92,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; @@ -103,6 +105,9 @@ float TtcaDca::GetCost(const Vector2D& velocity, Agent* agent, const WorldBase * ObstacleCost += cost; } } + + // TODO: check neighboring obstacles + // ... if (ObstacleCostScale > 0) ObstacleCost /= ObstacleCostScale; @@ -132,9 +137,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; @@ -150,8 +156,8 @@ Vector2D TtcaDca::GetGradient(const Vector2D& velocity, Agent* agent, const Worl { // Option 1: TTCA and DCA according to our reusable functions 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()); float ttca = ttca_dca.first; float dca = ttca_dca.second; const Vector2D& vdca = (relPos + ttca * relVelocity); @@ -205,6 +211,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 c84a8e18a839fc65e17236b2c6c298c651da6c36..da7ff8d05f8473f1dcfd034f03f4092a77f70713 100644 --- a/src/CostFunctions/VanToll.cpp +++ b/src/CostFunctions/VanToll.cpp @@ -35,7 +35,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 5a01f76ef015097aa09e20730ce3a95985b55497..6302fc3af5949a673f67eb5508944b64ead7d644 100644 --- a/src/core/agent.cpp +++ b/src/core/agent.cpp @@ -98,10 +98,17 @@ NeighborList Agent::getNeighborSubsetInRange(float range) const // 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) - result.push_back(neighbor); + 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 5d97eff7a1630075e9fa81a448d1383d078d03e1..c89ef91d8a45a67ddf9805877557c6e07aa9e556 100644 --- a/src/core/costFunction.cpp +++ b/src/core/costFunction.cpp @@ -64,13 +64,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) { @@ -107,8 +100,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 @@ -199,8 +192,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; @@ -234,14 +227,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) @@ -251,12 +245,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 std::max(0.0f, 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/crowdSimulator.cpp b/src/core/crowdSimulator.cpp index 3452650d841efd86d3e03c664472a7a18acf7163..f010925ce4dd54da02b0a04df100e0ee4019adc0 100644 --- a/src/core/crowdSimulator.cpp +++ b/src/core/crowdSimulator.cpp @@ -122,6 +122,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) @@ -139,6 +140,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; @@ -205,6 +207,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 @@ -212,12 +236,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; @@ -230,12 +253,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; @@ -410,6 +449,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. @@ -491,7 +550,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; @@ -503,14 +562,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; @@ -524,6 +583,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/obstacle.cpp b/src/core/obstacle.cpp deleted file mode 100644 index 4376e0163b6fb45c8e63c48efbfc054f8d37b36e..0000000000000000000000000000000000000000 --- a/src/core/obstacle.cpp +++ /dev/null @@ -1,49 +0,0 @@ -/* UMANS: Unified Microscopic Agent Navigation Simulator -** Copyright (C) 2018-2020 Inria Rennes Bretagne Atlantique - Rainbow - Julien Pettré -** -** 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 . -** -** Contact: crowd_group@inria.fr -** Website: https://project.inria.fr/crowdscience/ -** See the file AUTHORS.md for a list of all contributors. -*/ - -#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 9930db0d25f25a7fecd6e7c9deb09c18a2d6f90f..7e11c1b0d345a475bdb571d17cf3d7fb22a6711e 100644 --- a/src/core/policy.cpp +++ b/src/core/policy.cpp @@ -107,13 +107,26 @@ Vector2D Policy::ComputeContactForces(Agent* agent, WorldBase * world) const auto& neighbors = agent->getNeighbors(); // check all colliding agents - for (const auto& neighborAgent : agent->getNeighbors()) + for (const auto& neighborAgent : neighbors.first) { const auto& diff = position - neighborAgent.position; const float intersectionDistance = radius + neighborAgent.realAgent->getRadius() - diff.magnitude(); if (intersectionDistance > 0) totalForce += diff.getnormalized() * (contactForceScale_ * intersectionDistance); } + + // 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 float intersectionDistance = radius - diff.magnitude(); + if (intersectionDistance > 0) + { + const auto& F = diff.getnormalized() * (contactForceScale_ * intersectionDistance); + totalForce = totalForce + F; + } + } } return totalForce; diff --git a/src/core/worldBase.cpp b/src/core/worldBase.cpp index a06285756c6e5a5a4753ceaa5e28215906e589d0..e1d33cd6b544cd80f0fa5ea3c2729d50f6f4c0af 100644 --- a/src/core/worldBase.cpp +++ b/src/core/worldBase.cpp @@ -48,7 +48,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); @@ -61,6 +61,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 @@ -76,24 +96,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 @@ -105,7 +127,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); } @@ -219,6 +241,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 diff --git a/src/core/worldInfinite.cpp b/src/core/worldInfinite.cpp index 08554db807027b723e0f126b3ca26ee48f9ca793..841cb8c4b4d0b9ee4bd846950e45d8b5df5ab619 100644 --- a/src/core/worldInfinite.cpp +++ b/src/core/worldInfinite.cpp @@ -28,12 +28,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 1ed8e2f8bf81b23304f78fa2c33c91ec64649b2a..0205da37368e798a1aad1471d8ef2361701d71f5 100644 --- a/src/core/worldToric.cpp +++ b/src/core/worldToric.cpp @@ -30,22 +30,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_)