- PhantomAgent now pre-computes its position (again) and its squared distance to the query point.

- Many cost functions now use PhantomAgent::GetDistanceSquared() instead of re-computing it. This saves a lot of time overall!
- WorldBase/Infinite/Toric: Faster construction of neighbor lists.

src/3rd-party/ORCA/ORCASolver.cpp

@@ -107,7 +107,7 @@ namespace ORCALibrary
 		/* Create agent ORCA lines. */
 		for (const PhantomAgent& other : agentNeighbors_)
 		{
-			if (distanceSquared(other.GetPosition(), position_) > maxDistSq)
+			if (other.GetDistanceSquared() > maxDistSq)
 				continue;
 
 			const Vector2D& relativePosition = other.GetPosition() - position_;

src/Engine/CostFunctions/FOEAvoidance.cpp

@@ -64,7 +64,7 @@ float FOEAvoidance::GetCost(const Vector2D& velocity, Agent* agent, const WorldB
 	// check neighboring agents
 	for (const PhantomAgent& neighbor : neighbors.first)
 	{
-		if (distanceSquared(Position, neighbor.GetPosition()) > rangeSquared)
+		if (neighbor.GetDistanceSquared() > rangeSquared)
 			continue;
 
 		Vector2D Vel = RT * (neighbor.GetVelocity() - velocity);
@@ -108,7 +108,7 @@ Vector2D FOEAvoidance::GetGradient(const Vector2D& velocity, Agent* agent, const
 	// check neighboring agents
 	for (const PhantomAgent& neighbor : neighbors.first)
 	{
-		if (distanceSquared(Position, neighbor.GetPosition()) > rangeSquared)
+		if (neighbor.GetDistanceSquared() > rangeSquared)
 			continue;
 
 		Vector2D Vel = RT * (neighbor.GetVelocity() - velocity);

src/Engine/CostFunctions/ObjectInteractionForces.cpp

@@ -33,7 +33,7 @@ Vector2D ObjectInteractionForces::ComputeForce(Agent* agent, const WorldBase * w
 	const auto& neighbors = agent->getNeighbors();
 	for (const PhantomAgent& other : neighbors.first)
 	{
-		if (distanceSquared(other.GetPosition(), Position) <= rangeSquared)
+		if (other.GetDistanceSquared() <= rangeSquared)
 			AgentForces += ComputeAgentInteractionForce(agent, other);
 	}
 

src/Engine/CostFunctions/Paris.cpp

@@ -142,7 +142,7 @@ Paris::NeighborAvoidanceRangeList Paris::ComputeAllNeighborAvoidanceRanges(const
 	// agents
 	for (const auto& neighbor : neighbors.first)
 	{
-		if (distanceSquared(Position, neighbor.GetPosition()) > rangeSquared)
+		if (neighbor.GetDistanceSquared() > rangeSquared)
 			continue;
 
 		result.push_back(ComputeNeighborAvoidanceRange(direction, agent, neighbor));

src/Engine/CostFunctions/TtcaDca.cpp

@@ -80,7 +80,7 @@ float TtcaDca::GetCost(const Vector2D& velocity, Agent* agent, const WorldBase *
 	for (const auto& neighbor : neighbors.first)
 	{
 		const auto& neighborPos = neighbor.GetPosition();
-		if (distanceSquared(neighborPos, Position) >= rangeSquared)
+		if (neighbor.GetDistanceSquared() >= rangeSquared)
 			continue;
 
 		// Compute relative velocity and relative position
@@ -147,7 +147,7 @@ Vector2D TtcaDca::GetGradient(const Vector2D& velocity, Agent* agent, const Worl
 	// for each agent of the neighbourhood
 	for (const auto& neighbor : neighbors.first)
     {
-		if (distanceSquared(Position, neighbor.GetPosition()) > rangeSquared)
+		if (neighbor.GetDistanceSquared() > rangeSquared)
 			continue;
 		
 		// Compute relative velocity and relative position

src/Engine/core/costFunction.cpp

@@ -235,7 +235,7 @@ float CostFunction::ComputeTimeToFirstCollision(const Vector2D& position, const
 	// check neighboring agents
 	for (const auto& neighborAgent : neighbors.first)
 	{
-		if (distanceSquared(position, neighborAgent.GetPosition()) > maxDistSquared)
+		if (neighborAgent.GetDistanceSquared() > maxDistSquared)
 			continue;
 
 		float ttc = ComputeTimeToCollision(position, velocity, radius, neighborAgent.GetPosition(), neighborAgent.GetVelocity(), neighborAgent.realAgent->getRadius());

src/Engine/core/worldBase.cpp

@@ -48,24 +48,21 @@ void WorldBase::SetNumberOfThreads(int nrThreads)
 	omp_set_num_threads(nrThreads);
 }
 
-std::vector<const Agent*> WorldBase::computeNeighboringAgents_Flat(const Vector2D& position, float search_radius, const Agent* queryingAgent) const
+void WorldBase::computeNeighboringAgents_Flat(const Vector2D& position, float search_radius, const Agent* queryingAgent, std::vector<const Agent*>& result) const
 {
 	// get the IDs of neighbors
 	const auto& agentIDs = agentKDTree->FindAllAgentsInRange(position, search_radius, queryingAgent);
 
 	// convert them to agent pointers
-	std::vector<const Agent*> result(agentIDs.size());
+	result.resize(agentIDs.size());
 	for (int i = 0; i < agentIDs.size(); ++i)
 		result[i] = GetAgent_const(agentIDs[i]);
-
-	return result;
 }
 
-std::vector<LineSegment2D> WorldBase::computeNeighboringObstacles_Flat(const Vector2D& position, float search_radius) const
+void WorldBase::computeNeighboringObstacles_Flat(const Vector2D& position, float search_radius, std::vector<LineSegment2D>& result) const
 {
 	// TODO: implement more efficient neighbor search for obstacles
 	const float radiusSquared = search_radius * search_radius;
-	std::vector<LineSegment2D> result;
 
 	// loop over all obstacle edges
 	for (const auto& obs : obstacles_)
@@ -77,8 +74,6 @@ std::vector<LineSegment2D> WorldBase::computeNeighboringObstacles_Flat(const Vec
 				result.push_back(edge);
 		}
 	}
-
-	return result;
 }
 
 void WorldBase::DoStep()

src/Engine/core/worldBase.h

@@ -27,7 +27,9 @@
 #include <string>
 #include <queue>
 #include <limits>
+
 #include <unordered_map>
+typedef std::unordered_map<size_t, size_t> AgentIDMap;
 
 #include <tools/Polygon2D.h>
 #include <core/agent.h>
@@ -40,6 +42,8 @@ struct PhantomAgent
 private:
 	/// <summary>The amount by which the neighboring Agent's position should be offset, e.g. to account for wrap-around in a toric world.</summary>
 	Vector2D positionOffset;
+	Vector2D position;
+	float distSqr;
 
 public:
 	/// <summary>A pointer to the original Agent in the simulation.</summary>
@@ -47,16 +51,30 @@ public:
 
 	/// <summary>Creates a PhantomAgent with the given details.</summary>
 	/// <param name="agent">A pointer to the original Agent in the simulation.</param>
+	/// <param name="queryPosition">The query position used for finding neighbors; used for precomputing the distance to this PhantomAgent.</param>
 	/// <param name="posOffset">The amount by which the neighboring Agent's position should be offset, e.g. to account for wrap-around in a toric world.</param>
-	PhantomAgent(const Agent* agent, const Vector2D& posOffset = Vector2D(0, 0))
-		: realAgent(agent), positionOffset(posOffset) {}
+	PhantomAgent(const Agent* agent, const Vector2D& queryPosition, const Vector2D& posOffset)
+		: realAgent(agent), positionOffset(posOffset) 
+	{
+		UpdatePositionAndDistance(queryPosition);
+	}
 
 	PhantomAgent() : realAgent(nullptr) {}
 
-	/// <summary>Computes and returns this neighboring agent's position, translated by the offset of this PhantomAgent.</summary>
-	inline Vector2D GetPosition() const { return realAgent->getPosition() + positionOffset; }
+	/// <summary>Returns the (precomputed) position of this neighboring agent, translated by the offset of this PhantomAgent.</summary>
+	inline Vector2D GetPosition() const { return position; }
 	/// <summary>Returns the velocity of this neighboring agent.</summary>
 	inline Vector2D GetVelocity() const { return realAgent->getVelocity(); }
+	/// <summary>Returns the (precomputed) squared distance from this PhantomAgent to the query position that was used to find it.</summary>
+	inline float GetDistanceSquared() const { return distSqr; }
+
+	/// <summary>Pre-computes (or re-computes) the translated position of this PhantomAgent, as well as its distance to a query point.
+	/// Use this method if you want to correct the PhantomAgent's data in a new frame, without having search the AgentKDTree again.</summary>
+	inline void UpdatePositionAndDistance(const Vector2D& queryPosition)
+	{
+		position = realAgent->getPosition() + positionOffset;
+		distSqr = distanceSquared(queryPosition, position);
+	}
 
 };
 
@@ -89,7 +107,7 @@ private:
 	/// <remarks>Because agents can be removed during the simulation, the ID of an agent is not necessarily the same 
 	/// as its position in the list. This is why we need this extra administration.
 	/// (We could also put all agents directly in a map, but then we could not loop over the agents in parallel with OpenMP.)</remarks>
-	std::unordered_map<size_t, size_t> agentPositionsInVector;
+	AgentIDMap agentPositionsInVector;
 
 	/// <summary>The agent ID that will be used for the next agent that gets added.</summary>
 	size_t nextUnusedAgentID;
@@ -217,16 +235,16 @@ protected:
 	/// <summary>Creates a WorldBase object of the given type.</summary>
 	WorldBase(Type type);
 
-	/// <summary>Computes and returns a list of all agents that lie within a given radius of a given position.</summary>
+	/// <summary>Computes a list of all agents that lie within a given radius of a given position.</summary>
 	/// <param name="position">A query position.</param>
 	/// <param name="search_radius">A query radius.</param>
 	/// <param name="queryingAgent">A pointer to the Agent object performing the query. This agent will be excluded from the results.
 	/// Use nullptr to not exclude any agents.</param>
-	/// <returns>A list of pointers to agents queryingAgent lie within "search_radius" meters of "position", 
+	/// <param name="result">[out] Will store a list of pointers to agents queryingAgent lie within "search_radius" meters of "position", 
 	/// excluding the agent denoted by "queryingAgent" (if it exists).</returns>
-	std::vector<const Agent*> computeNeighboringAgents_Flat(const Vector2D& position, float search_radius, const Agent* queryingAgent) const;
+	void computeNeighboringAgents_Flat(const Vector2D& position, float search_radius, const Agent* queryingAgent, std::vector<const Agent*>& result) const;
 
-	std::vector<LineSegment2D> computeNeighboringObstacles_Flat(const Vector2D& position, float search_radius) const;
+	void computeNeighboringObstacles_Flat(const Vector2D& position, float search_radius, std::vector<LineSegment2D>& result) const;
 
 	/// <summary>Subroutine of DoStep() that moves all agents forward using their last computed "new velocities".</summary>
 	/// <remarks>Subclasses of WorldBase may override this method if they require special behavior (e.g. the wrap-around effect in WorldToric).</remarks>

src/Engine/core/worldInfinite.cpp

@@ -33,12 +33,21 @@ NeighborList WorldInfinite::ComputeNeighbors(const Vector2D& position, float sea
 	vector<PhantomAgent> phantoms; 
 	if (agentKDTree != nullptr)
 	{
-		const auto& agents = computeNeighboringAgents_Flat(position, search_radius, queryingAgent);
-
-		for (const Agent* agent : agents)
-			phantoms.push_back(PhantomAgent(agent));
+		// compute neighboring agents
+		vector<const Agent*> agents;
+		computeNeighboringAgents_Flat(position, search_radius, queryingAgent, agents);
+		
+		// efficiently add them to the result
+		Vector2D offset(0, 0);
+		phantoms.resize(agents.size());
+		for (size_t i = 0; i < agents.size(); ++i)
+			phantoms.push_back(PhantomAgent(agents[i], position, offset));
 	}
 
-	return { phantoms, computeNeighboringObstacles_Flat(position, search_radius) };
+	// compute neighboring obstacles
+	vector<LineSegment2D> obstacles;
+	computeNeighboringObstacles_Flat(position, search_radius, obstacles);
+
+	return { phantoms, obstacles };
 }
 

src/Engine/core/worldToric.cpp

@@ -32,18 +32,30 @@ WorldToric::WorldToric(float width, float height)
 void WorldToric::computeNeighbors_Displaced(const Vector2D& position, const Vector2D& displacement, float search_radius, const Agent* queryingAgent,
 	NeighborList& result) const
 {
+	Vector2D minDisplacement(-displacement);
+	
 	if (agentKDTree != nullptr)
 	{
-		// agents
-		const auto& agents = computeNeighboringAgents_Flat(position + displacement, search_radius, queryingAgent);
-		for (const Agent* agent : agents)
-			result.first.push_back(PhantomAgent(agent, -displacement));
+		// compute neighboring agents
+		vector<const Agent*> agents;
+		computeNeighboringAgents_Flat(position + displacement, search_radius, queryingAgent, agents);
+
+		// efficiently add them to the result
+		size_t oldSize = result.first.size(), extraSize = agents.size();
+		result.first.resize(oldSize + extraSize);
+		for (size_t i = 0; i < extraSize; ++i)
+			result.first[oldSize + i] = PhantomAgent(agents[i], position, minDisplacement);
 	}
 
-	// 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));
+	// compute neighboring obstacles
+	vector<LineSegment2D> obstacles;
+	computeNeighboringObstacles_Flat(position + displacement, search_radius, obstacles);
+
+	// efficiently add them to the result
+	size_t oldSize = result.second.size(), extraSize = obstacles.size();
+	result.second.resize(oldSize + extraSize);
+	for (size_t i = 0; i < extraSize; ++i)
+		result.second[oldSize + i] = LineSegment2D(obstacles[i].first - displacement, obstacles[i].second - displacement);
 }
 
 NeighborList WorldToric::ComputeNeighbors(const Vector2D& position, float search_radius, const Agent* queryingAgent) const