Removed Agent::getNeighborSubsetInRange() due to performance issues. Cost...

Removed Agent::getNeighborSubsetInRange() due to performance issues. Cost functions should now perform their own range checks.

3rd-party/ORCA/ORCASolver.cpp

@@ -59,7 +59,7 @@ namespace ORCALibrary
 
 	/* Search for the best new velocity. */
 	Solution Solver::solveOrcaProgram(const Agent& agent, 
-		const float timeHorizon, const float currentTime, const float simulationTimeStep, const NeighborList& neighbors) const
+		const float timeHorizon, const float currentTime, const float simulationTimeStep, const NeighborList& neighbors, const float maxDistance) const
 	{
 		const Vector2D& prefVelocity_ = agent.getPreferredVelocity();
 		const float maxSpeed_ = agent.getMaximumSpeed();
@@ -75,7 +75,7 @@ namespace ORCALibrary
 
 		Solution result;
 
-		createAgentOrcaLines(agent, result.orcaLines, timeHorizon, simulationTimeStep, neighbors.first);
+		createAgentOrcaLines(agent, result.orcaLines, timeHorizon, simulationTimeStep, neighbors.first, maxDistance);
 
 		// TODO: include obstacle lines
 		// ...
@@ -95,17 +95,20 @@ namespace ORCALibrary
 	}
 
 	void Solver::createAgentOrcaLines(const Agent& agent, std::vector<Line>& orcaLines_, const float timeHorizon_, const float simulationTimeStep,
-		const AgentNeighborList& agentNeighbors_) const
+		const AgentNeighborList& agentNeighbors_, const float maxDistance) const
 	{
 		const Vector2D& position_ = agent.getPosition();
 		const Vector2D& velocity_ = agent.getVelocity();
 		const float maxSpeed_ = agent.getMaximumSpeed();
 		const float radius_ = agent.getRadius();
 		const float radiusSq = radius_ * radius_;
+		const float maxDistSq = maxDistance * maxDistance;
 
 		/* Create agent ORCA lines. */
-		for (size_t i = 0; i < agentNeighbors_.size(); ++i) {
-			const PhantomAgent& other = agentNeighbors_[i];
+		for (const PhantomAgent& other : agentNeighbors_)
+		{
+			if (distanceSquared(other.position, position_) > maxDistSq)
+				continue;
 
 			const Vector2D& relativePosition = other.position - position_;
 			const Vector2D& relativeVelocity = velocity_ - other.velocity;

3rd-party/ORCA/ORCASolver.h

@@ -65,13 +65,13 @@ namespace ORCALibrary
 	{
 	public:
 		Solution solveOrcaProgram(const Agent& agent,
-			const float timeHorizon, const float currentTime, const float simulationTimeStep, const NeighborList& neighbors) const;
+			const float timeHorizon, const float currentTime, const float simulationTimeStep, const NeighborList& neighbors, const float maxDistance) const;
 
 	private:
 		/*void createObstacleOrcaLines(const Agent& agent, 
-			std::vector<Line>& orcaLines_, const float timeHorizonObst_, const float simulationTimeStep, const ObstacleNeighborList& obstacleNeighbors_) const;*/
+			std::vector<Line>& orcaLines_, const float timeHorizonObst_, const float simulationTimeStep, const ObstacleNeighborList& obstacleNeighbors_, const float maxDistance) const;*/
 		void createAgentOrcaLines(const Agent& agent, 
-			std::vector<Line>& orcaLines_, const float timeHorizon_, const float simulationTimeStep, const AgentNeighborList& agentNeighbors_) const;
+			std::vector<Line>& orcaLines_, const float timeHorizon_, const float simulationTimeStep, const AgentNeighborList& agentNeighbors_, const float maxDistance) const;
 	};
 
 	/**

include/CostFunctions/Moussaid.h

@@ -56,7 +56,7 @@ public:
 	void parseParameters(const CostFunctionParameters & params) override;
 
 private:
-	float getDistanceToCollisionAtPreferredSpeed(const Vector2D& direction, const Agent* agent, const NeighborList& neighbors) const;
+	float getDistanceToCollisionAtPreferredSpeed(const Vector2D& direction, const Agent* agent) const;
 };
 
 #endif //LIB_MOUSSAID_H

include/CostFunctions/Paris.h

@@ -77,7 +77,7 @@ public:
 private:
 	std::pair<float, float> ComputeT1andT2(const Vector2D& origin, const Vector2D& velocity, const Vector2D& point, const float radius) const;
 	NeighborAvoidanceRange ComputeNeighborAvoidanceRange(const Vector2D& direction, const Agent* agent, const PhantomAgent& neighbor) const;
-	NeighborAvoidanceRangeList ComputeAllNeighborAvoidanceRanges(const Vector2D& direction, const Agent* agent, const NeighborList& neighbors) const;
+	NeighborAvoidanceRangeList ComputeAllNeighborAvoidanceRanges(const Vector2D& direction, const Agent* agent) const;
 
 	float GetDirectionCost(const Vector2D& direction, const Agent* agent, const NeighborAvoidanceRangeList& avoidanceTimes) const;
 	float GetSpeedDeviationCost(const float speed, const Vector2D& direction, const Agent* agent, const NeighborAvoidanceRangeList& avoidanceRanges) const;

include/core/agent.h

@@ -162,13 +162,6 @@ public:
 	/// Methods that compute and return a result based on the agent's internal state.
 	/// @{
 
-	/// <summary>Returns the neighbors that are within a given range of this agent.</summary>
-	/// <remarks>This method uses the pre-computed list of neighbors and filters it by the given distance. 
-	/// The method does *not* perform a new nearest-neighbor query.</remarks>
-	/// <param name="range">A distance in meters; this method will return all neighbors within 'range' meters from the agent.</param>
-	/// <returns>A copy of the agent's NeighborList containing only the neighbors that lie witihn the given distance.</returns>
-	NeighborList getNeighborSubsetInRange(float range) const;
-
 	/// <summary>Checks and returns whether the agent has reached its goal position.</summary>
 	/// <returns>true if the agent's current position is sufficiently close to its goal; false otherwise.</returns>
 	bool hasReachedGoal() const;

include/core/costFunction.h

@@ -220,6 +220,8 @@ protected:
 	/// <param name="velocity">The hypothetical velocity of the querying agent.</param>
 	/// <param name="radius">The radius (in meters) of the querying agent.</param>
 	/// <param name="neighbors">A list of neighboring agents and obstacles.</param>
+	/// <param name="maximumDistance">The maximum distance between the query position and a neighboring object.
+	/// Any objects farther away will be ignored.</param>
 	/// <param name="ignoreCurrentCollisions">Whether or not to ignore any collisions that are already happening now.</param>
 	/// <returns>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 objects.
@@ -228,7 +230,7 @@ protected:
 	/// <seealso cref="ComputeTimeToCollision"/>
 	float ComputeTimeToFirstCollision(
 		const Vector2D& position, const Vector2D& velocity, const float radius,
-		const NeighborList& neighbors, bool ignoreCurrentCollisions = true) const;
+		const NeighborList& neighbors, float maximumDistance, bool ignoreCurrentCollisions) const;
 
 	/// <summary>Computes the expected time at which the distance between two disk-shaped objects is minimal, and the value of this distance.</summary>
 	/// <param name="position1">The current position of object 1.</param>

include/tools/vector2D.h

@@ -149,6 +149,16 @@ inline Vector2D operator-(Vector2D lhs)
 	return Vector2D(-lhs.x, -lhs.y);
 }
 
+inline float distance(const Vector2D& p, const Vector2D& q)
+{
+	return (p - q).magnitude();
+}
+
+inline float distanceSquared(const Vector2D& p, const Vector2D& q)
+{
+	return (p - q).sqrMagnitude();
+}
+
 #pragma endregion
 
 #pragma region [Angle-related functions]
@@ -296,7 +306,12 @@ inline Vector2D nearestPointOnLine(const Vector2D& pt, const Vector2D& la, const
 
 inline float distanceToLine(const Vector2D& pt, const Vector2D& la, const Vector2D& lb, bool onSegment = false)
 {
-	return (pt - nearestPointOnLine(pt, la, lb, onSegment)).magnitude();
+	return distance(pt, nearestPointOnLine(pt, la, lb, onSegment));
+}
+
+inline float distanceToLineSquared(const Vector2D& pt, const Vector2D& la, const Vector2D& lb, bool onSegment = false)
+{
+	return distanceSquared(pt, nearestPointOnLine(pt, la, lb, onSegment));
 }
 
 #endif // LIB_VECTOR2D_H

src/CostFunctions/FOEAvoidance.cpp

@@ -55,13 +55,17 @@ float FOEAvoidance::GetCost(const Vector2D& velocity, Agent* agent, const WorldB
 	const Vector2D& Vnorm = velocity.getnormalized();
 	Matrix R(Vector2D(Vnorm.y, -Vnorm.x), Vnorm);
 	const Matrix& RT = R.GetTransposed();
+	const float rangeSquared = range_ * range_;
+	const auto& neighbors = agent->getNeighbors();
 
 	float Cost = 0;
-	const auto& neighbors = agent->getNeighborSubsetInRange(range_);
 
 	// check neighboring agents
 	for (const PhantomAgent& neighbor : neighbors.first)
 	{
+		if (distanceSquared(Position, neighbor.position) > rangeSquared)
+			continue;
+
 		Vector2D Vel = RT * (neighbor.velocity - velocity);
 		Vector2D Pos = RT * (neighbor.position - Position);
 
@@ -95,13 +99,17 @@ Vector2D FOEAvoidance::GetGradient(const Vector2D& velocity, Agent* agent, const
 	const Vector2D& Vnorm = velocity.getnormalized();
 	Matrix R(Vector2D(Vnorm.y, -Vnorm.x), Vnorm);
 	const Matrix& RT = R.GetTransposed();
+	const float rangeSquared = range_ * range_;
+	const auto& neighbors = agent->getNeighbors();
 
 	float gradtheta = 0, gradv = 0;
-	const auto& neighbors = agent->getNeighborSubsetInRange(range_);
 
 	// check neighboring agents
 	for (const PhantomAgent& neighbor : neighbors.first)
 	{
+		if (distanceSquared(Position, neighbor.position) > rangeSquared)
+			continue;
+
 		Vector2D Vel = RT * (neighbor.velocity - velocity);
 		Vector2D Pos = RT * (neighbor.position - Position);
 

src/CostFunctions/Karamouzas.cpp

@@ -27,13 +27,13 @@ using namespace std;
 
 float Karamouzas::GetCost(const Vector2D& velocity, Agent* agent, const WorldBase * world) const
 {
-	const auto& neighbors = agent->getNeighborSubsetInRange(range_);
 	const auto& prefVelocity = agent->getPreferredVelocity();
 	const auto& currentVelocity = agent->getVelocity();
 	const auto& speed = velocity.magnitude();
 	const float maxSpeed = agent->getMaximumSpeed();
+	const auto& neighbors = agent->getNeighbors();
 
-	float TTC_preferred = ComputeTimeToFirstCollision(agent->getPosition(), prefVelocity, agent->getRadius(), neighbors);
+	float TTC_preferred = ComputeTimeToFirstCollision(agent->getPosition(), prefVelocity, agent->getRadius(), neighbors, range_, true);
 	
 	// 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->getPosition(), velocity, agent->getRadius(), neighbors);
+	float TTC = ComputeTimeToFirstCollision(agent->getPosition(), velocity, agent->getRadius(), neighbors, range_, true);
 
 	// the cost is a weighted sum of factors:
 

src/CostFunctions/Moussaid.cpp

@@ -25,13 +25,13 @@
 
 using namespace std;
 
-float Moussaid::getDistanceToCollisionAtPreferredSpeed(const Vector2D& direction, const Agent* agent, const NeighborList& neighbors) const
+float Moussaid::getDistanceToCollisionAtPreferredSpeed(const Vector2D& direction, const Agent* agent) const
 {
 	const float prefSpeed = agent->getPreferredSpeed();
 	const Vector2D& velocityWithPrefSpeed = direction * prefSpeed;
 
 	// compute the time to collision at this velocity
-	float ttc = ComputeTimeToFirstCollision(agent->getPosition(), velocityWithPrefSpeed, agent->getRadius(), neighbors, false);
+	float ttc = ComputeTimeToFirstCollision(agent->getPosition(), velocityWithPrefSpeed, agent->getRadius(), agent->getNeighbors(), range_, false);
 
 	// convert to the distance to collision, clamped to a maximum distance
 	float distanceToCollision = (ttc == MaxFloat ? MaxFloat : ttc * prefSpeed);
@@ -43,13 +43,12 @@ float Moussaid::getDistanceToCollisionAtPreferredSpeed(const Vector2D& direction
 
 float Moussaid::GetCost(const Vector2D& velocity, Agent* agent, const WorldBase * world) const
 {
-	const auto& neighbors = agent->getNeighborSubsetInRange(range_);
 	const float speed = velocity.magnitude(); 
 	const float prefSpeed = agent->getPreferredSpeed();
 
 	// compute the distance to collision at maximum speed
 	const auto& dir = velocity.getnormalized();
-	float DC = getDistanceToCollisionAtPreferredSpeed(dir, agent, neighbors);
+	float DC = getDistanceToCollisionAtPreferredSpeed(dir, agent);
 
 	// compute the cost for this direction, assuming maximum speed
 	float K = d_max * d_max + DC * DC - 2 * d_max * DC*cosAngle(dir, agent->getPreferredVelocity());

src/CostFunctions/ORCA.cpp

@@ -32,7 +32,7 @@ const ORCALibrary::Solution& ORCA::GetOrcaSolutionForAgent(Agent* agent, const W
 		// run ORCA and store the solution in the agent
 		ORCALibrary::Solver solver;
 		agent->SetOrcaSolution(
-			solver.solveOrcaProgram(*agent, timeHorizon, world->GetCurrentTime(), world->GetDeltaTime(), agent->getNeighborSubsetInRange(range_))
+			solver.solveOrcaProgram(*agent, timeHorizon, world->GetCurrentTime(), world->GetDeltaTime(), agent->getNeighbors(), range_)
 		);
 	}
 

src/CostFunctions/ObjectInteractionForces.cpp

@@ -25,15 +25,24 @@
 
 Vector2D ObjectInteractionForces::ComputeForce(Agent* agent, const WorldBase * world) const
 {
+	const Vector2D& Position = agent->getPosition();
+	const float rangeSquared = range_ * range_;
+
 	// loop over all neighbors; sum up the forces per neighbor
 	Vector2D AgentForces(0, 0);
-	const auto& neighbors = agent->getNeighborSubsetInRange(range_);
+	const auto& neighbors = agent->getNeighbors();
 	for (const PhantomAgent& other : neighbors.first)
-		AgentForces += ComputeAgentInteractionForce(agent, other);
+	{
+		if (distanceSquared(other.position, Position) <= rangeSquared)
+			AgentForces += ComputeAgentInteractionForce(agent, other);
+	}
 
 	Vector2D ObstacleForces(0, 0);
 	for (const LineSegment2D& obs : neighbors.second)
-		ObstacleForces += ComputeObstacleInteractionForce(agent, obs);
+	{
+		if (distanceToLineSquared(Position, obs.first, obs.second, true) <= rangeSquared)
+			ObstacleForces += ComputeObstacleInteractionForce(agent, obs);
+	}
 
 	return AgentForces + ObstacleForces;
 }

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->getPosition(), velocity, agent->getRadius(), agent->getNeighborSubsetInRange(range_));
+	float ttc = ComputeTimeToFirstCollision(agent->getPosition(), velocity, agent->getRadius(), agent->getNeighbors(), range_, true);
 	if (ttc < t_min)
 		return MaxFloat;
 

src/CostFunctions/Paris.cpp

@@ -27,14 +27,11 @@ using namespace std;
 
 float Paris::GetCost(const Vector2D& velocity, Agent* agent, const WorldBase * world) const
 {
-	// get all neighboring agents
-	const auto& neighbors = agent->getNeighborSubsetInRange(range_);
-	
 	const Vector2D& direction = velocity.getnormalized();
 
 	// Compute t1 and t2 for each neighbor, as defined in this paper:
 	// the agent will avoid a neighbor by passing in front of it (before t1) or behind it (after t2).
-	const auto& neighborAvoidanceTimes = ComputeAllNeighborAvoidanceRanges(direction, agent, neighbors);
+	const auto& neighborAvoidanceTimes = ComputeAllNeighborAvoidanceRanges(direction, agent);
 
 	float directionCost = GetDirectionCost(direction, agent, neighborAvoidanceTimes);
 	float speedDeviationCost = GetSpeedDeviationCost(velocity.magnitude(), direction, agent, neighborAvoidanceTimes);
@@ -134,13 +131,21 @@ Paris::NeighborAvoidanceRange Paris::ComputeNeighborAvoidanceRange(const Vector2
 	return NeighborAvoidanceRange(t12.first, t12.second, s1, s2);
 }
 
-Paris::NeighborAvoidanceRangeList Paris::ComputeAllNeighborAvoidanceRanges(const Vector2D& direction, const Agent* agent, const NeighborList& neighbors) const
+Paris::NeighborAvoidanceRangeList Paris::ComputeAllNeighborAvoidanceRanges(const Vector2D& direction, const Agent* agent) const
 {
 	NeighborAvoidanceRangeList result;
+	const Vector2D& Position = agent->getPosition();
+	const auto& neighbors = agent->getNeighbors();
+	const float rangeSquared = range_ * range_;
 
 	// agents
 	for (const auto& neighbor : neighbors.first)
+	{
+		if (distanceSquared(Position, neighbor.position) > rangeSquared)
+			continue;
+
 		result.push_back(ComputeNeighborAvoidanceRange(direction, agent, neighbor));
+	}
 
 	// TODO: static obstacles?
 	// ...

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(position, RVOVelocity, radius, agent->getNeighborSubsetInRange(range_));
+	float minTTC = ComputeTimeToFirstCollision(position, RVOVelocity, radius, agent->getNeighbors(), range_, true);
 
 	return w / minTTC + vDiff.magnitude();
 }

src/CostFunctions/TtcaDca.cpp

@@ -63,6 +63,7 @@ float TtcaDca::GetCost(const Vector2D& velocity, Agent* agent, const WorldBase *
 {
 	const float Radius = agent->getRadius();
 	const Vector2D& Position = agent->getPosition();
+	const float rangeSquared = range_ * range_;
 
 	// --- movement towards the goal
 
@@ -73,12 +74,14 @@ float TtcaDca::GetCost(const Vector2D& velocity, Agent* agent, const WorldBase *
 	float ObstacleCost = 0; 
 	float ObstacleCostScale = 0;
 
-	// get neighboring agents and obstacles
-	const auto& neighbors = agent->getNeighborSubsetInRange(range_);
-	
+	const auto& neighbors = agent->getNeighbors();
+
 	// for each agent of the neighbourhood
 	for (const auto& neighbor : neighbors.first)
 	{
+		if (distanceSquared(neighbor.position, Position) >= rangeSquared)
+			continue;
+
 		// Compute relative velocity and relative position
 		const Vector2D& relPos = neighbor.position - Position;
 		const Vector2D& relVelocity = neighbor.velocity - velocity;
@@ -130,6 +133,7 @@ Vector2D TtcaDca::GetGradient(const Vector2D& velocity, Agent* agent, const Worl
 	const float VelMagnitude = velocity.magnitude();
 	const Vector2D& VelNorm = velocity / VelMagnitude;
 	const Vector2D VelRot(velocity.y, -velocity.x);
+	const float rangeSquared = range_ * range_;
 
 	// --- collision avoidance
     
@@ -137,11 +141,14 @@ Vector2D TtcaDca::GetGradient(const Vector2D& velocity, Agent* agent, const Worl
 	float GradS = 0;
 	float totalScale = 0;
 
-    const auto& neighbors = agent->getNeighborSubsetInRange(range_);
+    const auto& neighbors = agent->getNeighbors();
 	
 	// for each agent of the neighbourhood
 	for (const auto& neighbor : neighbors.first)
     {
+		if (distanceSquared(Position, neighbor.position) > rangeSquared)
+			continue;
+		
 		// Compute relative velocity and relative position
 		const Vector2D& relPos = neighbor.position - Position;
 		const Vector2D& relVelocity = neighbor.velocity - velocity;

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(currentPos, velocity, agent->getRadius(), agent->getNeighborSubsetInRange(range_), false);
+	const float candidateTTC = ComputeTimeToFirstCollision(currentPos, velocity, agent->getRadius(), agent->getNeighbors(), range_, false);
 
 	// compute the distance to collision, bounded by the viewing distance
 	float distanceToCollision = (candidateTTC == MaxFloat ? MaxFloat : candidateTTC * speed);

src/core/agent.cpp

@@ -96,33 +96,6 @@ void Agent::UpdateVelocityAndPosition(float dt)
 
 #pragma region [Advanced getters]
 
-NeighborList Agent::getNeighborSubsetInRange(float range) const
-{
-	// if the requested range is big enough, simply return all neighbors that were already computed
-	if (range >= settings_.policy_->getInteractionRange())
-		return neighbors_;
-	
-	// otherwise, create a new list that contains a subset
-	NeighborList result;
-
-	// add all neighboring agents in range
-	const float sqrRange = range * range;
-	for (const auto& neighbor : neighbors_.first)
-	{
-		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;
-}
-
 bool Agent::hasReachedGoal() const
 {
 	return (goal_ - position_).sqrMagnitude() <= getRadius() * getRadius();

src/core/costFunction.cpp

@@ -228,13 +228,17 @@ float CostFunction::ComputeTimeToCollision(
 }
 
 float CostFunction::ComputeTimeToFirstCollision(const Vector2D& position, const Vector2D& velocity, const float radius,
-	const NeighborList& neighbors, bool ignoreCurrentCollisions) const
+	const NeighborList& neighbors, const float maximumDistance, bool ignoreCurrentCollisions) const
 {
 	float minTTC = MaxFloat;
+	const float maxDistSquared = maximumDistance * maximumDistance;
 
 	// check neighboring agents
 	for (const auto& neighborAgent : neighbors.first)
 	{
+		if (distanceSquared(position, neighborAgent.position) > maxDistSquared)
+			continue;
+
 		float ttc = ComputeTimeToCollision(position, velocity, radius, neighborAgent.position, neighborAgent.velocity, neighborAgent.realAgent->getRadius());
 		
 		// ignore current collisions?
@@ -248,6 +252,9 @@ float CostFunction::ComputeTimeToFirstCollision(const Vector2D& position, const
 	// check neighboring obstacles
 	for (const auto& neighboringObstacle : neighbors.second)
 	{
+		if (distanceToLineSquared(position, neighboringObstacle.first, neighboringObstacle.second, true) > maxDistSquared)
+			continue;
+
 		float ttc = ComputeTimeToCollision_LineSegment(position, velocity, radius, neighboringObstacle);
 
 		// ignore current collisions?