Commit c8bff383 authored by VAN TOLL Wouter's avatar VAN TOLL Wouter

Merge branch '71-fix-ORCA-cost-function' into 89-visualize-cost-functions

parents e6a2a79c a0312667
......@@ -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<Line>& orcaLines_, const float timeHorizon_, const float simulationTimeStep,
......
......@@ -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,
......
......@@ -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();
......@@ -53,8 +59,7 @@ float ORCA::GetCost(const Vector2D& velocity, Agent* agent, const WorldBase * wo
// 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)
if (getSignedDistanceToOrcaLine(velocity, orcaLine) > 0.0f)
return MaxFloat;
}
......@@ -64,11 +69,11 @@ float ORCA::GetCost(const Vector2D& velocity, Agent* agent, const WorldBase * wo
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).
// 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 = (velocity - currentVelocity - 0.5f*orcaLine.point).dot(orcaLine.direction);
float distanceToOrcaLine = getSignedDistanceToOrcaLine(velocity, orcaLine);
bestDistance = std::max(bestDistance, distanceToOrcaLine);
}
......
......@@ -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
};
......
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