Commit 788ecbb5 authored by VAN TOLL Wouter's avatar VAN TOLL Wouter
Browse files

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

parents 24b2516a 84230267
......@@ -53,32 +53,36 @@ float ORCA::GetCost(const Vector2D& velocity, Agent* agent, const WorldBase * wo
// compute or re-use the ORCA solution for this agent
const auto& orcaSolution = GetOrcaSolutionForAgent(agent, world);
if (orcaSolution.isFeasible)
// In this case, if the query velocity lies on the wrong side of any ORCA line, its cost is infinite.
// Because the ORCA program is feasible, there should be at least one velocity for which this is not the case.
for (const auto& orcaLine : orcaSolution.orcaLines)
if (getSignedDistanceToOrcaLine(velocity, orcaLine) > 0.0f)
return MaxFloat;
// Find the maximum distance by which this velocity exceeds any ORCA plane.
// The "getSignedDistanceToOrcaLine" function returns a positive number if the ORCA constraint is violated.
float maxDistance = -MaxFloat;
for (const auto& orcaLine : orcaSolution.orcaLines)
maxDistance = std::max(maxDistance, getSignedDistanceToOrcaLine(velocity, orcaLine));
// otherwise, this velocity is apparently allowed, and the cost is the difference to vPref
// There are three possible cases:
// a) ORCA has a solution, and this velocity is inside the solution space.
// In this case, maxDistance has to be <= 0, because the velocity is on the correct side of all ORCA lines.
// For such "allowed" velocities, ORCA uses the difference to vPref as the cost.
if (maxDistance <= 0)
// the cost is the difference to vPref
return (velocity - agent->getPreferredVelocity()).magnitude();
// In this case, the ORCA program is infeasible, so any velocity lies on the wrong side of at least one ORCA line.
// We should then look for the "least bad" velocity. The cost of a velocity is the largest of all signed line distances.
float bestDistance = -MaxFloat;
for (const auto& orcaLine : orcaSolution.orcaLines)
float distanceToOrcaLine = getSignedDistanceToOrcaLine(velocity, orcaLine);
bestDistance = std::max(bestDistance, distanceToOrcaLine);
// b) ORCA has a solution, but this velocity is not inside the solution space.
// In this case, according to "the real ORCA method", we should return an infinite cost to prevent this velocity from being chosen.
// However, in the context of sampling and gradients, it is better to return a finite value, to distinguish between "bad" and "even worse" velocities.
// Returning maxDistance is a good option, but we should add a sufficiently large constant, so that velocities inside the ORCA solution space will be preferred.
// c) ORCA does not have a solution, and we need to use ORCA's "backup function", which is just maxDistance.
// In this case, it does not hurt to add the same large constant as in case (b).
// In short, both cases can actually use the same cost function:
return bestDistance;
return maxDistance + 2 * agent->getMaximumSpeed();
Vector2D ORCA::GetGlobalMinimum(Agent* agent, const WorldBase* world) 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