diff --git a/src/3rd-party/ORCA/ORCASolver.cpp b/src/3rd-party/ORCA/ORCASolver.cpp index 09ac67460d46f9ee92de4213ba092beba5ed21e7..4ba9afb7499f6bbcfd1fb8027653b4f78b38154f 100644 --- a/src/3rd-party/ORCA/ORCASolver.cpp +++ b/src/3rd-party/ORCA/ORCASolver.cpp @@ -58,8 +58,9 @@ namespace ORCALibrary } /* Search for the best new velocity. */ - Solution Solver::solveOrcaProgram(const Agent& agent, - const float timeHorizon, const float currentTime, const float simulationTimeStep, const NeighborList& neighbors, const float maxDistance) const + void Solver::solveOrcaProgram(const Agent& agent, + const float timeHorizon, const float currentTime, const float simulationTimeStep, const NeighborList& neighbors, const float maxDistance, + Solution& result) const { const Vector2D& prefVelocity_ = agent.getPreferredVelocity(); const float maxSpeed_ = agent.getMaximumSpeed(); @@ -73,7 +74,7 @@ namespace ORCALibrary // radius = 2.0f; // maxSpeed = 2.0f; - Solution result; + result = Solution(); createAgentOrcaLines(agent, result.orcaLines, timeHorizon, simulationTimeStep, neighbors.first, maxDistance); @@ -90,8 +91,6 @@ namespace ORCALibrary // store the simulation time result.currentSimulationTime = currentTime; - - return result; } void Solver::createAgentOrcaLines(const Agent& agent, std::vector& orcaLines_, const float timeHorizon_, const float simulationTimeStep, diff --git a/src/3rd-party/ORCA/ORCASolver.h b/src/3rd-party/ORCA/ORCASolver.h index 6f09098b44fcd7f459ef11f14e9980d133b39432..720983fa04631c266ed52e65bf5a0281fd292690 100644 --- a/src/3rd-party/ORCA/ORCASolver.h +++ b/src/3rd-party/ORCA/ORCASolver.h @@ -64,8 +64,8 @@ namespace ORCALibrary class Solver { public: - Solution solveOrcaProgram(const Agent& agent, - const float timeHorizon, const float currentTime, const float simulationTimeStep, const NeighborList& neighbors, const float maxDistance) const; + void solveOrcaProgram(const Agent& agent, + const float timeHorizon, const float currentTime, const float simulationTimeStep, const NeighborList& neighbors, const float maxDistance, Solution& result) const; private: /*void createObstacleOrcaLines(const Agent& agent, diff --git a/src/Engine/CostFunctions/ORCA.cpp b/src/Engine/CostFunctions/ORCA.cpp index 871f9cc52f2e51fb15284fdd6197f7e00f7aba4f..3998ef055bd4c24c03eb998c10f678648df039c4 100644 --- a/src/Engine/CostFunctions/ORCA.cpp +++ b/src/Engine/CostFunctions/ORCA.cpp @@ -31,15 +31,21 @@ const ORCALibrary::Solution& ORCA::GetOrcaSolutionForAgent(Agent* agent, const W { // run ORCA and store the solution in the agent ORCALibrary::Solver solver; - agent->SetOrcaSolution( - solver.solveOrcaProgram(*agent, timeHorizon, (float)world->GetCurrentTime(), world->GetDeltaTime(), agent->getNeighbors(), range_) - ); + solver.solveOrcaProgram(*agent, timeHorizon, (float)world->GetCurrentTime(), world->GetDeltaTime(), agent->getNeighbors(), range_, agent->GetOrcaSolution()); } // return the result return agent->GetOrcaSolution(); } +inline float getSignedDistanceToOrcaLine(const Vector2D& velocity, const ORCALibrary::Line& line) +{ + //return (line.point - velocity).dot(Vector2D(line.direction.y, -line.direction.x)); + + const Vector2D& pMinV = line.point - velocity; + return line.direction.x * pMinV.y - line.direction.y * pMinV.x; +} + float ORCA::GetCost(const Vector2D& velocity, Agent* agent, const WorldBase * world) const { const Vector2D& currentVelocity = agent->getVelocity(); @@ -47,33 +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) - { - float distanceToOrcaLine = (velocity - currentVelocity - 0.5f*orcaLine.point).dot(orcaLine.direction); - if (distanceToOrcaLine < 0) - 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; - // otherwise, this velocity is apparently allowed, and the cost is the difference to vPref + for (const auto& orcaLine : orcaSolution.orcaLines) + maxDistance = std::max(maxDistance, getSignedDistanceToOrcaLine(velocity, orcaLine)); + + // 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(); } - 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 line distances (= the max of all negative numbers). - float bestDistance = -MaxFloat; - for (const auto& orcaLine : orcaSolution.orcaLines) - { - float distanceToOrcaLine = (velocity - currentVelocity - 0.5f*orcaLine.point).dot(orcaLine.direction); - 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 diff --git a/src/Engine/core/agent.h b/src/Engine/core/agent.h index 02632068ce60fc2045128c998619ee915d6908cf..b573a83746dbaf957249e08752cac976c46f1cfb 100644 --- a/src/Engine/core/agent.h +++ b/src/Engine/core/agent.h @@ -201,8 +201,10 @@ public: float ComputeRandomNumber(float min, float max); #pragma region [ORCA] + inline const ORCALibrary::Solution& GetOrcaSolution() const { return orcaSolution_; } - inline void SetOrcaSolution(const ORCALibrary::Solution& solution) { orcaSolution_ = solution; } + inline ORCALibrary::Solution& GetOrcaSolution() { return orcaSolution_; } + #pragma endregion };