Commit 5cc9ddeb authored by VAN TOLL Wouter's avatar VAN TOLL Wouter
Browse files

ORCA::GetCost(): Removed unused code.

parent faa35e00
......@@ -83,33 +83,6 @@ float ORCA::GetCost(const Vector2D& velocity, Agent* agent, const WorldBase * wo
// In short, both cases can actually use the same cost function:
return maxDistance + 2 * agent->getMaximumSpeed();
/*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;
// otherwise, this velocity is apparently allowed, and 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 maxDistance = -MaxFloat;
for (const auto& orcaLine : orcaSolution.orcaLines)
float distanceToOrcaLine = getSignedDistanceToOrcaLine(velocity, orcaLine);
maxDistance = std::max(maxDistance, distanceToOrcaLine);
return maxDistance;
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