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

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();
}
else
{
// 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