Commit bfd24801 authored by VAN TOLL Wouter's avatar VAN TOLL Wouter
Browse files

Bugfix in CostFunctions/Karamouzas.cpp

parent 84230267
......@@ -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;
......
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