Commit 1b8d1d2c authored by VAN TOLL Wouter's avatar VAN TOLL Wouter
Browse files

Merge branch 'master' into acceleration-based

parents 510a0458 90f6e848
......@@ -3,19 +3,28 @@ UMANS (Unified Microscopic Agent Navigation Simulator)
![](doc/UMANS-Logo-Transparent-Medium.png)
UMANS (short for Unified Microscopic Agent Navigation Simulator) is an agent-based
crowd-simulation engine that focuses on the local (a.k.a. "microscopic") aspects of navigation.
[UMANS](https://project.inria.fr/crowdscience/project/ocsr/umans/) (short for Unified Microscopic Agent Navigation Simulator) is an easy-to-use crowd-simulation engine that models each person in the crowd as an intelligent agent (i.e., the simulation is "microscopic"). UMANS currently focuses on the *local* aspects of navigation.
Many algorithms for microscopic crowd simulation have been developed over the past decades.
However, each implementation has its own settings and details that can greatly influence the results.
The purpose of UMANS is to reproduce as many existing algorithms as possible via one principle,
while unifying as many overall settings as possible.
This allows for a more honest and meaningful comparison of simulation algorithms.
Many algorithms for microscopic crowd simulation have been developed over the past decades. However, each implementation has its own settings and details that can greatly influence the results. The purpose of UMANS is to reproduce as many existing algorithms as possible via one principle, while unifying as many overall settings as possible. This allows for a more honest and meaningful comparison of simulation algorithms.
UMANS was previously known as OCSR (Open Crowd Simulation Resources).
Some parts of the project may still use this old name.
Since 2020, the term "OCSR" refers to the collective of open crowd-simulation resources developed at Inria Rennes.
These resources include UMANS (a simulation engine) and ChAOS (a visualization application).
UMANS was previously known as OCSR (Open Crowd Simulation Resources). Some parts of the project may still use this old name. Since 2020, the term "[OCSR](https://project.inria.fr/crowdscience/project/ocsr/)" refers to the collective of open crowd-simulation resources developed at Inria Rennes. These resources include UMANS (a simulation engine) and [ChAOS](https://project.inria.fr/crowdscience/project/ocsr/chaos/) (a visualization application).
UMANS is primarily meant for the scientific community, as a unified tool for experimenting with different navigation algorithms. However, the tool can be used freely by everyone, and it does not require any programming knowledge.
### What can UMANS do?
* **Run a crowd simulation:** Given a scenario file containing agents and their properties, UMANS can output the trajectories of all agents during a period of time. These trajectories can then be loaded into other applications for visualization or analysis.
* **Basic visualization:** You can use the *UMANS-GUI* application for a simple 2D visualization of a scenario.
* **Model many kinds of behavior:** A scenario file should define the so-called *policies* that agents use for local navigation.
Overall, a policy consists of a cost function (possibly with parameters) and an optimization method.
You can mix and match these elements as you wish. No programming knowledge is required for this.
* **Support new behavior:** If you are a programmer, you can clone the UMANS repository and *add your own cost functions* (and/or optimization methods), to add new types of behavior to the system. Adding new cost functions is easy thanks to the software's architecture.
### What can UMANS *not* do?
* **Global path planning:** The UMANS library deliberately focuses on *local navigation only*, i.e. the local interactions between agents.
The simulation environment may contain static obstacles, but the agents in UMANS *do not* plan a global path.
* **Fancy visualization:** UMANS focuses on the simulation itself. For nice 3D animations of a crowd, consider using separate programs such as [ChAOS](https://project.inria.fr/crowdscience/project/ocsr/chaos/).
# Reference
......@@ -31,62 +40,18 @@ The UMANS software has been described in the following scientific publication:
Please cite this publication when referring to UMANS in your work.
# Getting started
UMANS has three main applications that you can use:
- ***UMANS-ConsoleApplication***: A command-line-only program that can run a crowd-simulation scenario and save the results.
If you run this program without any arguments, it will print more detailed usage instructions.
- ***UMANS-GUI***: A demo program with a (Qt) GUI, in which you can view a simulation and edit it with basic mouse and keyboard interaction.
- ***UMANS-Library***: A dynamic library (a DLL on Windows, or an .so file on Linux) with an API that can be integrated into other software.
To obtain these programs, you have two options: download the pre-built binaries, or compile the code yourself.
### Option 1: Download the binaries
For downloadable pre-built binaries of UMANS,
please visit the [Releases](https://gitlab.inria.fr/OCSR/UMANS/-/releases) page of this repository.
### Option 2: Compile the code
To compile the UMANS codebase on your machine, perform the following steps:
1. Clone or download the code from [our GitLab repository](https://gitlab.inria.fr/OCSR/UMANS/).
2. Download and install [Qt](https://www.qt.io/) (optional) and [CMake](https://cmake.org/) (required).
(Qt is only needed if you want to compile the *UMANS-GUI* application.)
3. Use CMake to turn the codebase into a programming project for your IDE/compiler of choice.
(You can also disable the UMANS-GUI project here. If you disable it, you do not need a Qt installation.)
4. Compile the code in your IDE/compiler.
This should build the three applications mentioned before.
### Input and output
The main **input** of a UMANS simulation is an **XML file** that describes agents, their start and goal positions,
their navigation behavior, and optionally obstacles.
Many working examples can be found in the [*examples*](https://gitlab.inria.fr/OCSR/UMANS/-/tree/master/examples) folder.
The main **output** of a UMANS simulation (or at least of the *UMANS-ConsoleApplication* program)
is a **folder with CSV files**, where each file contains the trajectory of one agent in the crowd.
You can plot these trajectories in a tool of choice.
You can also consider using the [ChAOS](https://gitlab.inria.fr/OCSR/ChAOS/) software
to visualize the results with animated 3D characters.
The *UMANS-ConsoleApplication* program simply converts input (XML) to output (CSV).
By contrast, *UMANS-GUI* and *UMANS-Library* are meant for interactive purposes, so they do not produce any output files by default.
# Documentation
### For end users
# Wiki and documentation
For general users of UMANS, we are creating a [Wiki](https://gitlab.inria.fr/OCSR/UMANS/-/wikis/home)
with basic information on how to use the software. We will extend this wiki over time.
The UMANS repository comes with a [Wiki](https://gitlab.inria.fr/OCSR/UMANS/-/wikis/) that helps you install and use the UMANS software. It contains the following pages:
### For programmers
* [Getting started](https://gitlab.inria.fr/OCSR/UMANS/-/wikis/Getting%20started) - This page explains how to download or compile the UMANS binaries, what their main input and output is, and what to do if you run into problems.
* [Configuration files](https://gitlab.inria.fr/OCSR/UMANS/-/wikis/Configuration%20files) - This page explains the XML files that UMANS takes as input. It describes all options for these input files, so that you can create new scenarios yourself.
* [For developers](https://gitlab.inria.fr/OCSR/UMANS/-/wikis/For%20developers) - This page is for those who want to dive into the C++ code itself. For example, it explains how you can write your own cost functions.
For those interested in using/extending the UMANS source code,
most of the codebase has been carefully documented in a style compatible with [Doxygen](http://www.doxygen.nl/).
This facilitates development in an IDE such as Visual Studio.
If you run Doxygen on the UMANS root folder, it wil generate a *html* folder with documentation.
Also, most of the source code of UMANS has been carefully documented in a style compatible with [Doxygen](http://www.doxygen.nl/).
This documentation is mostly meant for developers who intend to use/extend the UMANS codebase itself.
The Doxygen documentation also facilitates development in an IDE such as Visual Studio.
If you run the Doxygen program on the UMANS root folder, it wil generate a *html* folder with all documentation pages.
(This folder is not part of the repository on purpose.)
# Third-party code / licenses
......
......@@ -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,
......
......@@ -34,7 +34,7 @@ float Karamouzas::GetCost(const Vector2D& velocity, Agent* agent, const WorldBas
const auto& neighbors = agent->getNeighbors();
float TTC_preferred = ComputeTimeToFirstCollision(agent->getPosition(), prefVelocity, agent->getRadius(), neighbors, range_, true);
// This collision-avoidance method has a dynamic angular range, so ignore velocities that are outside it
if (angle(velocity, prefVelocity) > getMaxDeviationAngle(agent, TTC_preferred))
return MaxFloat;
......@@ -42,13 +42,13 @@ float Karamouzas::GetCost(const Vector2D& velocity, Agent* agent, const WorldBas
// same for speed
if (speed < getMinSpeed(agent, TTC_preferred) || speed > getMaxSpeed(agent, TTC_preferred))
return MaxFloat;
// compute time to collision at this candidate velocity
float TTC = ComputeTimeToFirstCollision(agent->getPosition(), velocity, agent->getRadius(), neighbors, range_, true);
// the cost is a weighted sum of factors:
float A = alpha * (1 - cosAngle(velocity, prefVelocity)/2.0f);
float A = alpha * (1 - cosAngle(velocity, prefVelocity) / 2.0f);
float B = beta * abs(speed - currentVelocity.magnitude()) / maxSpeed;
float C = gamma * (velocity - prefVelocity).magnitude() / (2 * maxSpeed);
float D = delta * std::max(0.0f, t_max - TTC) / t_max;
......@@ -86,8 +86,8 @@ float Karamouzas::getMaxSpeed(const Agent* agent, const float ttc) const
// no collision: always move at the preferred speed
if (ttc <= 0 || ttc >= tc_max)
return prefSpeed + epsilon;
return prefSpeed + epsilon;
// collision very nearby: allow all speeds
if (ttc > 0 && ttc < tc_min)
return maxSpeed + epsilon;
......@@ -98,12 +98,15 @@ float Karamouzas::getMaxSpeed(const Agent* agent, const float ttc) const
float Karamouzas::getMaxDeviationAngle(const Agent* agent, const float ttc) const
{
if (ttc <= 0 || ttc >= tc_max)
if (ttc >= tc_max)
return d_min;
if (ttc <= 0)
return d_max;
if (ttc < tc_min)
// quadratic decrease: from d_max at TTC = 0, to d_mid at TTC = tc_min
return d_mid + pow((tc_min - ttc)/tc_min, 2) * (d_max - d_mid);
return d_mid + pow((tc_min - ttc) / tc_min, 2) * (d_max - d_mid);
if (ttc < tc_mid)
return d_mid;
......
......@@ -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
......
......@@ -28,7 +28,9 @@ Vector2D ObjectInteractionForces::ComputeForce(Agent* agent, const WorldBase * w
const Vector2D& Position = agent->getPosition();
const float rangeSquared = range_ * range_;
// loop over all neighbors; sum up the forces per neighbor
// loop over all neighbors; sum up the forces for all neighbors that are in range
// - agents
Vector2D AgentForces(0, 0);
const auto& neighbors = agent->getNeighbors();
for (const PhantomAgent& other : neighbors.first)
......@@ -37,10 +39,22 @@ Vector2D ObjectInteractionForces::ComputeForce(Agent* agent, const WorldBase * w
AgentForces += ComputeAgentInteractionForce(agent, other);
}
// - obstacles
Vector2D ObstacleForces(0, 0);
for (const LineSegment2D& obs : neighbors.second)
{
if (distanceToLineSquared(Position, obs.first, obs.second, true) <= rangeSquared)
// We know that obstacles are always closed polygons, whose boundary points are given in counter-clockwise order.
// 1. Ignore an obstacle segment if an agent lies on the wrong side of it. In this case, either the agent is inside the obstacle, or another segment is more relevant.
if (isPointLeftOfLine(Position, obs.first, obs.second))
continue;
// 2. Ignore an obstacle segment if the nearest point is its second endpoint. This prevents double forces at obstacle corners.
const Vector2D& nearest = nearestPointOnLine(Position, obs.first, obs.second, true);
if (nearest == obs.second)
continue;
if (distanceSquared(Position, nearest) <= rangeSquared)
ObstacleForces += ComputeObstacleInteractionForce(agent, obs);
}
......
......@@ -53,7 +53,7 @@ std::pair<float, float> TtcaDca::getMovementCostGradient(const Vector2D& velocit
float ds = (velocity.magnitude() - agent->getPreferredSpeed());
// Gradients of the movement cost C_m (Eq. 14 and 15 in Dutra et al.)
double DC_angle = -alpha / (2*sigAngle_goal_*sigAngle_goal_) * exp(-0.5 * pow(alpha / sigAngle_goal_, 2.0));
double DC_angle = alpha / (2*sigAngle_goal_*sigAngle_goal_) * exp(-0.5 * pow(alpha / sigAngle_goal_, 2.0));
double DC_speed = ds / (2*sigSpeed_goal_*sigSpeed_goal_) * exp(-0.5 * pow(ds / sigSpeed_goal_, 2.0));
return { (float)DC_angle, (float)DC_speed };
......@@ -87,27 +87,34 @@ float TtcaDca::GetCost(const Vector2D& velocity, Agent* agent, const WorldBase *
const Vector2D& relPos = neighborPos - Position;
const Vector2D& relVelocity = neighbor.GetVelocity() - velocity;
// Only see agents in front of the direction of motion
if (relPos.dot(velocity) < 0)
// ignore neighbors that are behind the agent; the original Dutra method uses rendering, and agents always face forward
if (angle(relPos, agent->getVelocity()) > viewingAngleHalf_)
continue;
// there is adaptation only if relative velocity is not zero
if (relVelocity.sqrMagnitude() > eps)
{
// computing ttc and dca
const auto& ttca_dca = ComputeTimeAndDistanceToClosestApproach(
Position, velocity, Radius,
neighborPos, neighbor.GetVelocity(), neighbor.realAgent->getRadius());
// --> disabled because it makes the cost function non-smooth
//if (relVelocity.sqrMagnitude() <= eps)
// continue;
if (ttca_dca.first < 0) continue;
// computing ttc and dca
const auto& ttca_dca = ComputeTimeAndDistanceToClosestApproach(
Position, velocity, Radius,
neighborPos, neighbor.GetVelocity(), neighbor.realAgent->getRadius());
// computing the cost for this neighbor
float scale = 1 / relPos.sqrMagnitude(); //simulate num of pixels of an agent in screen
ObstacleCostScale += scale;
// ignore TTCAs in the past
// --> disabled because it makes the cost function non-smooth
//if (ttca_dca.first < 0)
// continue;
float cost = costForTtcaDca(ttca_dca.first, ttca_dca.second) * scale;
ObstacleCost += cost;
}
// The original method does this per pixel; we do it per obstacle.
// To simulate the "number of pixels" for this obstacle, scale by the distance
float distance = relPos.magnitude() - Radius - neighbor.realAgent->getRadius();
float scale = 1 / (distance*distance); //simulate num of pixels of an agent in screen
ObstacleCostScale += scale;
float cost = costForTtcaDca(ttca_dca.first, ttca_dca.second) * scale;
ObstacleCost += cost;
}
// TODO: check neighboring obstacles
......@@ -123,101 +130,101 @@ float TtcaDca::GetCost(const Vector2D& velocity, Agent* agent, const WorldBase *
Vector2D TtcaDca::GetGradient(const Vector2D& velocity, Agent* agent, const WorldBase * world) const
{
// For now, use the approximated gradient
return CostFunction::GetGradient(velocity, agent, world);
// Below is the code for the analytical gradient. It seems incorrect, although it literally follows the paper by Dutra et al.
const float Radius = agent->getRadius();
const Vector2D& Position = agent->getPosition();
const float VelMagnitude = velocity.magnitude();
const Vector2D& VelNorm = velocity / VelMagnitude;
const Vector2D VelRot(velocity.y, -velocity.x);
const float rangeSquared = range_ * range_;
// --- collision avoidance
float GradTh = 0;
float GradS = 0;
float totalScale = 0;
const auto& neighbors = agent->getNeighbors();
const auto& neighbors = agent->getNeighbors();
// for each agent of the neighbourhood
for (const auto& neighbor : neighbors.first)
{
{
if (neighbor.GetDistanceSquared() > rangeSquared)
continue;
// Compute relative velocity and relative position
const Vector2D& relPos = neighbor.GetPosition() - Position;
const Vector2D& relVelocity = neighbor.GetVelocity() - velocity;
// Only see agents in front of the direction of motion
if (relPos.dot(velocity) < 0)
continue;
// ------------------
// --- The following code is very similar to CostFunction::ComputeTimeAndDistanceToClosestApproach,
// but we need certain components of it later in the calculation.
// there is adaptation only if relative velocity is not zero
float relVelocitySqrMagnitude = relVelocity.sqrMagnitude();
if (relVelocitySqrMagnitude <= eps)
continue;
const Vector2D dp_center(neighbor.GetPosition() - Position);
const Vector2D dv_center(neighbor.GetVelocity() - velocity);
/*// Option 1: TTCA and DCA according to our reusable functions
const auto& ttca_dca = ComputeTimeAndDistanceToClosestApproach(
Position, velocity, Radius,
neighbor.position, neighbor.velocity, neighbor.realAgent->getRadius());
float ttca = ttca_dca.first;
float dca = ttca_dca.second;
const Vector2D& vdca = (relPos + ttca * relVelocity);*/
// Option 2: TTCA and DCA according to Dutra et al.
// (the only difference is that it does not include agents' radii for DCA)
float ttca = -relPos.dot(relVelocity) / relVelocitySqrMagnitude;
const Vector2D& vdca = (relPos + ttca * relVelocity);
float dca = vdca.magnitude();
if (ttca <= eps)
// ignore neighbors that are behind the agent; the original Dutra method uses rendering, and agents always face forward
if (angle(dp_center, agent->getVelocity()) > viewingAngleHalf_)
continue;
// skip extreme TTCAs
if (ttca > 10)
// The standard TTCA definition works with these dp and dv immediately.
// We make some corrections to account for the radii of agents:
float dpMag = dp_center.magnitude();
const Vector2D& dp = dp_center / dpMag * (dpMag - Radius - neighbor.realAgent->getRadius());
Vector2D dpNormal(-dp_center.y, dp_center.x);
const Vector2D dv = dv_center + dpNormal * dpNormal.dot(dv_center);
float dvSqrMagnitude = dv.sqrMagnitude();
// Source: Dutra et al, "Gradient-based steering for vision-based crowd simulation algorithms", 2016.
float ttca = (dvSqrMagnitude == 0 ? 0 : -dp.dot(dv) / dvSqrMagnitude);
// Prevent negative ttca?
// This means that if the agents are moving away from each other, it does not matter how strongly they do this
if (ttca < 0) ttca = 0;
const Vector2D vdca(dp + dv * ttca);
float dca = vdca.magnitude(); // subtracting the radii is not needed anymore, dp and dv already incorporate this
// Prevent negative dca?
// Combined with a non-negative ttca, this can only happen if agents are already colliding now.
if (dca < 0) dca = 0;
// --- End copy from CostFunction::ComputeTimeAndDistanceToClosestApproach
// ------------------
// If the relative velocity is almost zero, then the gradient is very unreliable; ignore it.
// This is dangerous in GetCost(), but acceptable for GetGradient().
if (dvSqrMagnitude <= eps)
continue;
// Partial derivatives of TTCA (Eq. 29 and 30 of Dutra et al.)
const Vector2D& rel = (relPos + 2 * ttca*relVelocity);
float gradTtcaAngle = -rel.dot(VelRot) / relVelocitySqrMagnitude;
float gradTtcaSpeed = rel.dot(VelNorm) / relVelocitySqrMagnitude;
const Vector2D VelRot(dv.y, -dv.x);
const float VelMagnitude = sqrt(dvSqrMagnitude);
const Vector2D& VelNorm = dv / VelMagnitude;
// Partial derivatives of TTCA (Eq. 29 and 30 of Dutra et al.)
float gradTtcaAngle = 0, gradTtcaSpeed = 0;
if (abs(ttca) > eps)
{
const Vector2D& rel = (dp + 2 * ttca*dv);
gradTtcaAngle = -rel.dot(VelRot) / dvSqrMagnitude;
gradTtcaSpeed = rel.dot(VelNorm) / dvSqrMagnitude;
}
// otherwise TTCA is too small, so gradient cannot be properly determined
// Partial derivatives of DCA (Eq. 36 and 37 of Dutra et al.)
float gradDcaAngle, gradDcaSpeed;
float gradDcaAngle = 0, gradDcaSpeed = 0;
if (abs(dca) > eps)
{
gradDcaAngle = vdca.dot(gradTtcaAngle*relVelocity + ttca * VelRot) / dca;
gradDcaSpeed = vdca.dot(gradTtcaSpeed*relVelocity - ttca * VelNorm) / dca;
}
else
{
// DCA is too small, so gradient cannot be properly determined
gradDcaAngle = 0;
gradDcaSpeed = 0;
}
gradDcaAngle = vdca.dot(gradTtcaAngle*dv + ttca * VelRot) / dca;
gradDcaSpeed = vdca.dot(gradTtcaSpeed*dv - ttca * VelNorm) / dca;
}
// otherwise DCA is too small, so gradient cannot be properly determined
// Overall gradients of the obstacle cost function (Eq. 16 and 17 in Dutra et al.)
float cost = costForTtcaDca(ttca, dca);
float cost = costForTtcaDca(ttca, dca);
float ttcaFrac = ttca / (sigTtca_*sigTtca_);
float dcaFrac = dca / (sigDca_*sigDca_);
float gradCSpeed = -cost * (gradTtcaSpeed * ttcaFrac + gradDcaSpeed * dcaFrac);
float gradCAngle = -cost * (gradTtcaAngle * ttcaFrac + gradDcaAngle * dcaFrac);
float gradCSpeed = -cost * (gradTtcaSpeed * ttcaFrac + gradDcaSpeed * dcaFrac);
float gradCAngle = -cost * (gradTtcaAngle * ttcaFrac + gradDcaAngle * dcaFrac);
// The original method does this per pixel; we do it per obstacle.
// To simulate the "number of pixels" for this obstacle, scale by the distance
float scale = 1 / relPos.sqrMagnitude();
float scale = 1 / dp.sqrMagnitude();
totalScale += scale;
GradTh += gradCAngle * scale;
GradS += gradCSpeed * scale;
}
}
// TODO: check neighboring obstacles
// ...
......@@ -235,21 +242,20 @@ Vector2D TtcaDca::GetGradient(const Vector2D& velocity, Agent* agent, const Worl
const auto& GradThS_movement = getMovementCostGradient(velocity, agent);
GradTh += GradThS_movement.first;
GradS += GradThS_movement.second;
// clamp the change in angle
//GradTh = clampVector(GradTh, -1.f, 1.f);
// --- convert to a gradient in Euclidean velocity space
Vector2D Gradient;
if (VelMagnitude < 0.1)
if (velocity.sqrMagnitude() < 0.01)
{
// If the velocity is zero, then the transformation will return (0,0) and the agent will never start moving.
// If the velocity is (almost) zero, then the transformation will return (nearly) (0,0) and the agent will never start moving.
Gradient = RotateGradientToEuclideanCoordinates(GradTh, GradS,
agent->getPreferredVelocity().getnormalized(), agent->getPreferredVelocity().magnitude());
}
else
{
Gradient = RotateGradientToEuclideanCoordinates(GradTh, GradS, VelNorm, VelMagnitude);
Gradient = RotateGradientToEuclideanCoordinates(GradTh, GradS, velocity.getnormalized(), velocity.magnitude());
}
return Gradient;
......
......@@ -64,6 +64,8 @@ private:
float sigTtca_ = 1;
float sigDca_ = 0.3f;
const float viewingAngleHalf_ = (float)(PI / 2.0);
float costForTtcaDca(float ttca, float dca) const;
float getMovementCost(const Vector2D& velocity, const Agent* agent) const;
......
......@@ -211,8 +211,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
};
......
......@@ -329,16 +329,57 @@ float CostFunction::ComputeTimeToCollision_LineSegmentInterior(const Vector2D& p
return std::max(0.0f, timeToCollision - timeToSubtract);
}