Commit 33f81828 authored by VAN TOLL Wouter's avatar VAN TOLL Wouter
Browse files

Merge branch 'master' into 89-visualize-cost-functions

parents 788ecbb5 90f6e848
......@@ -34,7 +34,7 @@ float Karamouzas::GetCost(const Vector2D& velocity, Agent* agent, const WorldBas
const auto& neighbors = agent->getNeighbors();
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))
return MaxFloat;
......@@ -42,13 +42,13 @@ float Karamouzas::GetCost(const Vector2D& velocity, Agent* agent, const WorldBas
// same for speed
if (speed < getMinSpeed(agent, TTC_preferred) || speed > getMaxSpeed(agent, TTC_preferred))
return MaxFloat;
// compute time to collision at this candidate velocity
float TTC = ComputeTimeToFirstCollision(agent->getPosition(), velocity, agent->getRadius(), neighbors, range_, true);
// the cost is a weighted sum of factors:
float A = alpha * (1 - cosAngle(velocity, prefVelocity)/2.0f);
float A = alpha * (1 - cosAngle(velocity, prefVelocity) / 2.0f);
float B = beta * abs(speed - currentVelocity.magnitude()) / maxSpeed;
float C = gamma * (velocity - prefVelocity).magnitude() / (2 * maxSpeed);
float D = delta * std::max(0.0f, t_max - TTC) / t_max;
......@@ -86,8 +86,8 @@ float Karamouzas::getMaxSpeed(const Agent* agent, const float ttc) const
// no collision: always move at the preferred speed
if (ttc <= 0 || ttc >= tc_max)
return prefSpeed + epsilon;
return prefSpeed + epsilon;
// collision very nearby: allow all speeds
if (ttc > 0 && ttc < tc_min)
return maxSpeed + epsilon;
......@@ -98,12 +98,15 @@ float Karamouzas::getMaxSpeed(const Agent* agent, const float ttc) const
float Karamouzas::getMaxDeviationAngle(const Agent* agent, const float ttc) const
{
if (ttc <= 0 || ttc >= tc_max)
if (ttc >= tc_max)
return d_min;
if (ttc <= 0)
return d_max;
if (ttc < tc_min)
// quadratic decrease: from d_max at TTC = 0, to d_mid at TTC = tc_min
return d_mid + pow((tc_min - ttc)/tc_min, 2) * (d_max - d_mid);
return d_mid + pow((tc_min - ttc) / tc_min, 2) * (d_max - d_mid);
if (ttc < tc_mid)
return d_mid;
......
......@@ -28,7 +28,9 @@ Vector2D ObjectInteractionForces::ComputeForce(Agent* agent, const WorldBase * w
const Vector2D& Position = agent->getPosition();
const float rangeSquared = range_ * range_;
// loop over all neighbors; sum up the forces per neighbor
// loop over all neighbors; sum up the forces for all neighbors that are in range
// - agents
Vector2D AgentForces(0, 0);
const auto& neighbors = agent->getNeighbors();
for (const PhantomAgent& other : neighbors.first)
......@@ -37,10 +39,22 @@ Vector2D ObjectInteractionForces::ComputeForce(Agent* agent, const WorldBase * w
AgentForces += ComputeAgentInteractionForce(agent, other);
}
// - obstacles
Vector2D ObstacleForces(0, 0);
for (const LineSegment2D& obs : neighbors.second)
{
if (distanceToLineSquared(Position, obs.first, obs.second, true) <= rangeSquared)
// We know that obstacles are always closed polygons, whose boundary points are given in counter-clockwise order.
// 1. Ignore an obstacle segment if an agent lies on the wrong side of it. In this case, either the agent is inside the obstacle, or another segment is more relevant.
if (isPointLeftOfLine(Position, obs.first, obs.second))
continue;
// 2. Ignore an obstacle segment if the nearest point is its second endpoint. This prevents double forces at obstacle corners.
const Vector2D& nearest = nearestPointOnLine(Position, obs.first, obs.second, true);
if (nearest == obs.second)
continue;
if (distanceSquared(Position, nearest) <= rangeSquared)
ObstacleForces += ComputeObstacleInteractionForce(agent, obs);
}
......
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