Commit 9a3d272a authored by VAN TOLL Wouter's avatar VAN TOLL Wouter

- 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.
parent dae77bdb
......@@ -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_;
......
......@@ -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);
......
......@@ -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);
}
......
......@@ -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));
......
......@@ -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
......
......@@ -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());
......
......@@ -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()
......
......@@ -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>
......
......@@ -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 };
}
......@@ -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
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment