Commit 9759d20b authored by VAN TOLL Wouter's avatar VAN TOLL Wouter
Browse files

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

Removed Agent::getNeighborSubsetInRange() due to performance issues. Cost functions should now perform their own range checks.
parent aff5699d
......@@ -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;
......
......@@ -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;
};
/**
......
......@@ -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
......@@ -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;
......
......@@ -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;
......
......@@ -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>
......
......@@ -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
......@@ -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);
......
......@@ -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:
......
......@@ -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());
......
......@@ -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_)
);
}
......
......@@ -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;
}
......@@ -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;
......
......@@ -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?
// ...
......
......@@ -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();
}
......
......@@ -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;
......
......@@ -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);
......
......@@ -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();
......
......@@ -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?
......
Supports Markdown
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