ORCA::GetCost(): Removed unused code.

src/Engine/CostFunctions/ORCA.cpp

@@ -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