Commit 6841f8a7 authored by Fabien Grzeskowiak's avatar Fabien Grzeskowiak

Merge branch 'API-obstacles' into set_delta_time_live

parents bad087c0 43fc2dbb
<?xml version="1.0" encoding="utf-8"?> <?xml version="1.0" encoding="utf-8"?>
<Simulation delta_time="0.1" end_time="100"> <Simulation delta_time="0.1" end_time="200">
<World type="Infinite" /> <World type="Toric" width="60" height="60" />
<Policies file="policies/Karamouzas.xml" /> <Policies file="policies/Karamouzas.xml" />
<Agents file="agents/OneWayFlow200.xml" /> <Agents file="agents/OneWayFlow200.xml" />
</Simulation> </Simulation>
<?xml version="1.0" encoding="utf-8"?> <?xml version="1.0" encoding="utf-8"?>
<Simulation delta_time="0.1" end_time="100"> <Simulation delta_time="0.1" end_time="200">
<World type="Infinite" /> <World type="Toric" width="60" height="60" />
<Policies file="policies/ORCA.xml" /> <Policies file="policies/ORCA.xml" />
<Agents file="agents/OneWayFlow200.xml" /> <Agents file="agents/OneWayFlow200.xml" />
</Simulation> </Simulation>
<?xml version="1.0" encoding="utf-8"?>
<Simulation delta_time="0.1" end_time="200">
<World type="Toric" width="60" height="60" />
<Policies file="policies/SocialForces.xml" />
<Agents file="agents/OneWayFlow200.xml" />
</Simulation>
<?xml version="1.0" encoding="utf-8"?>
<Simulation delta_time="0.1" end_time="200">
<World type="Toric" width="60" height="60" />
<Policies file="policies/VanToll.xml" />
<Agents file="agents/OneWayFlow200.xml" />
</Simulation>
<?xml version="1.0" encoding="utf-8"?> <?xml version="1.0" encoding="utf-8"?>
<Policies> <Policies>
<Policy id="0" OptimizationMethod="gradient"> <Policy id="0" OptimizationMethod="gradient">
<costfunction coeff="20" range="100" name="TtcaDca" sigmaTtca="1" sigmaDca="0.3" sigmaAngle_goal="2" sigmaSpeed_goal="2" /> <costfunction coeff="20" range="5" name="TtcaDca" sigmaTtca="1" sigmaDca="0.3" sigmaAngle_goal="2" sigmaSpeed_goal="2" />
</Policy> </Policy>
</Policies> </Policies>
\ No newline at end of file
<?xml version="1.0" encoding="utf-8"?> <?xml version="1.0" encoding="utf-8"?>
<Policies> <Policies>
<Policy id="0" OptimizationMethod="gradient"> <Policy id="0" OptimizationMethod="gradient">
<costfunction range="100" name="FOEAvoidance" coeff="3" /> <costfunction range="5" name="FOEAvoidance" coeff="3" />
<costfunction range="100" name="GoalReachingForce" /> <costfunction name="GoalReachingForce" />
</Policy> </Policy>
</Policies> </Policies>
\ No newline at end of file
<?xml version="1.0" encoding="utf-8"?> <?xml version="1.0" encoding="utf-8"?>
<Policies> <Policies>
<Policy id="0" OptimizationMethod="sampling" SamplingType="regular" SamplingBase="zero" SamplingBaseDirection="preferred velocity" SamplingAngle="180" SpeedSamples="15" AngleSamples="19" SamplingRadius="maximum speed" RelaxationTime="0" IncludeBaseAsSample="true"> <Policy id="0" OptimizationMethod="sampling" SamplingType="regular" SamplingBase="zero" SamplingBaseDirection="preferred velocity" SamplingAngle="180" SpeedSamples="15" AngleSamples="19" SamplingRadius="maximum speed" RelaxationTime="0" IncludeBaseAsSample="true">
<costfunction range="100" name="Karamouzas" /> <costfunction range="5" name="Karamouzas" />
</Policy> </Policy>
</Policies> </Policies>
\ No newline at end of file
<?xml version="1.0" encoding="utf-8"?> <?xml version="1.0" encoding="utf-8"?>
<Policies> <Policies>
<Policy id="0" OptimizationMethod="sampling" SamplingType="regular" SamplingBase="zero" SamplingBaseDirection="preferred velocity" SamplingAngle="180" SpeedSamples="5" AngleSamples="19" SamplingRadius="preferred speed" RelaxationTime="0.5"> <Policy id="0" OptimizationMethod="sampling" SamplingType="regular" SamplingBase="zero" SamplingBaseDirection="preferred velocity" SamplingAngle="180" SpeedSamples="5" AngleSamples="19" SamplingRadius="preferred speed" RelaxationTime="0.5">
<costfunction range="100" name="Moussaid" /> <costfunction range="5" name="Moussaid" />
</Policy> </Policy>
</Policies> </Policies>
\ No newline at end of file
<?xml version="1.0" encoding="utf-8"?> <?xml version="1.0" encoding="utf-8"?>
<Policies> <Policies>
<Policy id="0" OptimizationMethod="global" RelaxationTime="0"> <Policy id="0" OptimizationMethod="global" RelaxationTime="0">
<costfunction range="100" name="ORCA" /> <costfunction range="5" name="ORCA" />
</Policy> </Policy>
</Policies> </Policies>
\ No newline at end of file
<?xml version="1.0" encoding="utf-8"?> <?xml version="1.0" encoding="utf-8"?>
<Policies> <Policies>
<Policy id="0" OptimizationMethod="sampling" SamplingType="regular" SamplingBase="zero" SamplingBaseDirection="unit" SamplingAngle="360" SpeedSamples="4" AngleSamples="36" SamplingRadius="maximum speed" RelaxationTime="0.5"> <Policy id="0" OptimizationMethod="sampling" SamplingType="regular" SamplingBase="zero" SamplingBaseDirection="unit" SamplingAngle="360" SpeedSamples="4" AngleSamples="36" SamplingRadius="maximum speed" RelaxationTime="0.5">
<costfunction range="100" name="PLEdestrians" t_min="0.5" t_max="3" /> <costfunction range="5" name="PLEdestrians" t_min="0.5" t_max="3" />
</Policy> </Policy>
</Policies> </Policies>
\ No newline at end of file
<?xml version="1.0" encoding="utf-8"?> <?xml version="1.0" encoding="utf-8"?>
<Policies> <Policies>
<Policy id="0" OptimizationMethod="sampling" SamplingType="regular" SamplingBase="zero" SamplingBaseDirection="unit" SamplingAngle="360" SpeedSamples="4" AngleSamples="36" SamplingRadius="maximum speed" RelaxationTime="0"> <Policy id="0" OptimizationMethod="sampling" SamplingType="regular" SamplingBase="zero" SamplingBaseDirection="unit" SamplingAngle="360" SpeedSamples="4" AngleSamples="36" SamplingRadius="maximum speed" RelaxationTime="0">
<costfunction range="100" name="Paris"/> <costfunction range="5" name="Paris"/>
</Policy> </Policy>
</Policies> </Policies>
\ No newline at end of file
<?xml version="1.0" encoding="utf-8"?> <?xml version="1.0" encoding="utf-8"?>
<Policies> <Policies>
<Policy id="0" OptimizationMethod="gradient" RelaxationTime="0.5"> <Policy id="0" OptimizationMethod="gradient" RelaxationTime="0.5">
<costfunction range="100" name="PowerLaw" /> <costfunction range="5" name="PowerLaw" />
<costfunction range="100" name="GoalReachingForce" /> <costfunction name="GoalReachingForce" />
</Policy> </Policy>
</Policies> </Policies>
\ No newline at end of file
<?xml version="1.0" encoding="utf-8"?> <?xml version="1.0" encoding="utf-8"?>
<Policies> <Policies>
<Policy id="0" OptimizationMethod="sampling" SamplingType="random" SamplingBase="current velocity" SamplingBaseDirection="unit" SamplingAngle="360" RandomSamples="250" SamplingRadius="maximum acceleration" RelaxationTime="0"> <Policy id="0" OptimizationMethod="sampling" SamplingType="random" SamplingBase="current velocity" SamplingBaseDirection="unit" SamplingAngle="360" RandomSamples="250" SamplingRadius="maximum acceleration" RelaxationTime="0">
<costfunction range="100" name="RVO" /> <costfunction range="5" name="RVO" />
</Policy> </Policy>
</Policies> </Policies>
\ No newline at end of file
<?xml version="1.0" encoding="utf-8"?> <?xml version="1.0" encoding="utf-8"?>
<Policies> <Policies>
<Policy id="0" OptimizationMethod="gradient" RelaxationTime="0.5"> <Policy id="0" OptimizationMethod="gradient" RelaxationTime="0.5">
<costfunction range="100" name="SocialForcesAvoidance" /> <costfunction range="5" name="SocialForcesAvoidance" />
<costfunction range="100" name="GoalReachingForce" /> <costfunction name="GoalReachingForce" />
</Policy> </Policy>
</Policies> </Policies>
\ No newline at end of file
<?xml version="1.0" encoding="utf-8"?> <?xml version="1.0" encoding="utf-8"?>
<Policies> <Policies>
<Policy id="0" OptimizationMethod="sampling" SamplingType="regular" SamplingBase="zero" SamplingBaseDirection="preferred velocity" SamplingAngle="180" SpeedSamples="2" AngleSamples="19" SamplingRadius="preferred speed" RelaxationTime="0.25"> <Policy id="0" OptimizationMethod="sampling" SamplingType="regular" SamplingBase="zero" SamplingBaseDirection="preferred velocity" SamplingAngle="180" SpeedSamples="2" AngleSamples="19" SamplingRadius="preferred speed" RelaxationTime="0.25">
<costfunction range="100" name="VanToll" /> <costfunction range="5" name="VanToll" />
</Policy> </Policy>
</Policies> </Policies>
\ No newline at end of file
...@@ -107,11 +107,11 @@ namespace ORCALibrary ...@@ -107,11 +107,11 @@ namespace ORCALibrary
/* Create agent ORCA lines. */ /* Create agent ORCA lines. */
for (const PhantomAgent& other : agentNeighbors_) for (const PhantomAgent& other : agentNeighbors_)
{ {
if (distanceSquared(other.position, position_) > maxDistSq) if (other.GetDistanceSquared() > maxDistSq)
continue; continue;
const Vector2D& relativePosition = other.position - position_; const Vector2D& relativePosition = other.GetPosition() - position_;
const Vector2D& relativeVelocity = velocity_ - other.velocity; const Vector2D& relativeVelocity = velocity_ - other.GetVelocity();
const float distSq = relativePosition.sqrMagnitude(); const float distSq = relativePosition.sqrMagnitude();
const float combinedRadius = radius_ + other.realAgent->getRadius(); const float combinedRadius = radius_ + other.realAgent->getRadius();
const float combinedRadiusSq = combinedRadius * combinedRadius; const float combinedRadiusSq = combinedRadius * combinedRadius;
......
...@@ -103,6 +103,7 @@ int main( int argc, char * argv[] ) ...@@ -103,6 +103,7 @@ int main( int argc, char * argv[] )
CrowdSimulator* cs = CrowdSimulator::FromConfigFile(configFile, true); CrowdSimulator* cs = CrowdSimulator::FromConfigFile(configFile, true);
if (cs != nullptr) if (cs != nullptr)
{ {
cs->GetWorld()->SetNumberOfThreads(nrThreads);
if (outputFolder != "") if (outputFolder != "")
cs->StartCSVOutput(outputFolder, false); // false = don't save any files until the simulation ends cs->StartCSVOutput(outputFolder, false); // false = don't save any files until the simulation ends
cs->RunSimulationUntilEnd(); cs->RunSimulationUntilEnd();
......
...@@ -64,11 +64,11 @@ float FOEAvoidance::GetCost(const Vector2D& velocity, Agent* agent, const WorldB ...@@ -64,11 +64,11 @@ float FOEAvoidance::GetCost(const Vector2D& velocity, Agent* agent, const WorldB
// check neighboring agents // check neighboring agents
for (const PhantomAgent& neighbor : neighbors.first) for (const PhantomAgent& neighbor : neighbors.first)
{ {
if (distanceSquared(Position, neighbor.position) > rangeSquared) if (neighbor.GetDistanceSquared() > rangeSquared)
continue; continue;
Vector2D Vel = RT * (neighbor.velocity - velocity); Vector2D Vel = RT * (neighbor.GetVelocity() - velocity);
Vector2D Pos = RT * (neighbor.position - Position); Vector2D Pos = RT * (neighbor.GetPosition() - Position);
if (Pos.y < MIN_DISTANCE || Vel.y > -MIN_VELOCITY) if (Pos.y < MIN_DISTANCE || Vel.y > -MIN_VELOCITY)
continue; continue;
...@@ -108,11 +108,11 @@ Vector2D FOEAvoidance::GetGradient(const Vector2D& velocity, Agent* agent, const ...@@ -108,11 +108,11 @@ Vector2D FOEAvoidance::GetGradient(const Vector2D& velocity, Agent* agent, const
// check neighboring agents // check neighboring agents
for (const PhantomAgent& neighbor : neighbors.first) for (const PhantomAgent& neighbor : neighbors.first)
{ {
if (distanceSquared(Position, neighbor.position) > rangeSquared) if (neighbor.GetDistanceSquared() > rangeSquared)
continue; continue;
Vector2D Vel = RT * (neighbor.velocity - velocity); Vector2D Vel = RT * (neighbor.GetVelocity() - velocity);
Vector2D Pos = RT * (neighbor.position - Position); Vector2D Pos = RT * (neighbor.GetPosition() - Position);
if (Pos.y < MIN_DISTANCE || Vel.y > -MIN_VELOCITY) if (Pos.y < MIN_DISTANCE || Vel.y > -MIN_VELOCITY)
continue; continue;
......
...@@ -35,7 +35,7 @@ ...@@ -35,7 +35,7 @@
class GoalReachingForce : public ForceBasedFunction class GoalReachingForce : public ForceBasedFunction
{ {
public: public:
GoalReachingForce() : ForceBasedFunction() {} GoalReachingForce() : ForceBasedFunction() { range_ = 0; }
virtual ~GoalReachingForce() {} virtual ~GoalReachingForce() {}
const static std::string GetName() { return "GoalReachingForce"; } const static std::string GetName() { return "GoalReachingForce"; }
......
...@@ -33,7 +33,7 @@ Vector2D ObjectInteractionForces::ComputeForce(Agent* agent, const WorldBase * w ...@@ -33,7 +33,7 @@ Vector2D ObjectInteractionForces::ComputeForce(Agent* agent, const WorldBase * w
const auto& neighbors = agent->getNeighbors(); const auto& neighbors = agent->getNeighbors();
for (const PhantomAgent& other : neighbors.first) for (const PhantomAgent& other : neighbors.first)
{ {
if (distanceSquared(other.position, Position) <= rangeSquared) if (other.GetDistanceSquared() <= rangeSquared)
AgentForces += ComputeAgentInteractionForce(agent, other); AgentForces += ComputeAgentInteractionForce(agent, other);
} }
......
...@@ -89,13 +89,14 @@ std::pair<float, float> Paris::ComputeT1andT2(const Vector2D& origin, const Vect ...@@ -89,13 +89,14 @@ std::pair<float, float> Paris::ComputeT1andT2(const Vector2D& origin, const Vect
Paris::NeighborAvoidanceRange Paris::ComputeNeighborAvoidanceRange(const Vector2D& direction, const Agent* agent, const PhantomAgent& neighbor) const Paris::NeighborAvoidanceRange Paris::ComputeNeighborAvoidanceRange(const Vector2D& direction, const Agent* agent, const PhantomAgent& neighbor) const
{ {
const auto& agentPos = agent->getPosition(); const auto& agentPos = agent->getPosition();
const auto& neighborPos = neighbor.GetPosition();
const float radius1 = agent->getRadius(), radius2 = neighbor.realAgent->getRadius(); const float radius1 = agent->getRadius(), radius2 = neighbor.realAgent->getRadius();
// Find the point where the trajectories of 'agent' and 'neighbor' intersect // Find the point where the trajectories of 'agent' and 'neighbor' intersect
Vector2D X; Vector2D X;
bool intersects = getLineIntersection( bool intersects = getLineIntersection(
agentPos, agentPos + direction, agentPos, agentPos + direction,
neighbor.position, neighbor.position + neighbor.velocity, neighborPos, neighborPos + neighbor.GetVelocity(),
X); X);
// If there is no intersection, two things could be going on: // If there is no intersection, two things could be going on:
...@@ -104,7 +105,7 @@ Paris::NeighborAvoidanceRange Paris::ComputeNeighborAvoidanceRange(const Vector2 ...@@ -104,7 +105,7 @@ Paris::NeighborAvoidanceRange Paris::ComputeNeighborAvoidanceRange(const Vector2
if (!intersects) if (!intersects)
{ {
// check if the agents are on collision course // check if the agents are on collision course
float ttc = ComputeTimeToCollision(agentPos, direction, radius1, neighbor.position, neighbor.velocity, radius2); float ttc = ComputeTimeToCollision(agentPos, direction, radius1, neighborPos, neighbor.GetVelocity(), radius2);
// if not (or if it's too far in the future), this direction is fine // if not (or if it's too far in the future), this direction is fine
if (ttc > t_max) if (ttc > t_max)
...@@ -121,7 +122,7 @@ Paris::NeighborAvoidanceRange Paris::ComputeNeighborAvoidanceRange(const Vector2 ...@@ -121,7 +122,7 @@ Paris::NeighborAvoidanceRange Paris::ComputeNeighborAvoidanceRange(const Vector2
return NeighborAvoidanceRange(t_max, 0, 0, std::numeric_limits<float>::max()); return NeighborAvoidanceRange(t_max, 0, 0, std::numeric_limits<float>::max());
// compute t1 and t2, the times at which 'neighbor' starts and stops touching the point X // compute t1 and t2, the times at which 'neighbor' starts and stops touching the point X
const auto& t12 = ComputeT1andT2(neighbor.position, neighbor.velocity, X, radius1 + radius2); const auto& t12 = ComputeT1andT2(neighborPos, neighbor.GetVelocity(), X, radius1 + radius2);
// compute s1 and s2, the speeds that 'agent' should use to reach X after exactly t1 and t2 seconds // compute s1 and s2, the speeds that 'agent' should use to reach X after exactly t1 and t2 seconds
float distToX = (X - agentPos).magnitude(); float distToX = (X - agentPos).magnitude();
...@@ -141,7 +142,7 @@ Paris::NeighborAvoidanceRangeList Paris::ComputeAllNeighborAvoidanceRanges(const ...@@ -141,7 +142,7 @@ Paris::NeighborAvoidanceRangeList Paris::ComputeAllNeighborAvoidanceRanges(const
// agents // agents
for (const auto& neighbor : neighbors.first) for (const auto& neighbor : neighbors.first)
{ {
if (distanceSquared(Position, neighbor.position) > rangeSquared) if (neighbor.GetDistanceSquared() > rangeSquared)
continue; continue;
result.push_back(ComputeNeighborAvoidanceRange(direction, agent, neighbor)); result.push_back(ComputeNeighborAvoidanceRange(direction, agent, neighbor));
......
...@@ -34,8 +34,8 @@ Vector2D PowerLaw::ComputeAgentInteractionForce(const Agent* agent, const Phanto ...@@ -34,8 +34,8 @@ Vector2D PowerLaw::ComputeAgentInteractionForce(const Agent* agent, const Phanto
// Some constants appear to be wrong, but we have kept this to match the paper as closely as possible. // Some constants appear to be wrong, but we have kept this to match the paper as closely as possible.
float R = other.realAgent->getRadius() + agent->getRadius(); float R = other.realAgent->getRadius() + agent->getRadius();
const Vector2D& x = agent->getPosition() - other.position; const Vector2D& x = agent->getPosition() - other.GetPosition();
const Vector2D& v = agent->getVelocity() - other.velocity; const Vector2D& v = agent->getVelocity() - other.GetVelocity();
float a = v.dot(v); float a = v.dot(v);
float b = -x.dot(v); float b = -x.dot(v);
float c = x.dot(x) - R * R; float c = x.dot(x) - R * R;
......
...@@ -22,8 +22,6 @@ ...@@ -22,8 +22,6 @@
#include <CostFunctions/RandomFunction.h> #include <CostFunctions/RandomFunction.h>
#include <core/agent.h> #include <core/agent.h>
RandomFunction::RandomFunction() : CostFunction() {}
float RandomFunction::GetCost(const Vector2D& velocity, Agent* agent, const WorldBase * world) const float RandomFunction::GetCost(const Vector2D& velocity, Agent* agent, const WorldBase * world) const
{ {
return agent->ComputeRandomNumber(-1, 1); return agent->ComputeRandomNumber(-1, 1);
......
...@@ -35,7 +35,7 @@ ...@@ -35,7 +35,7 @@
class RandomFunction : public CostFunction class RandomFunction : public CostFunction
{ {
public: public:
RandomFunction(); RandomFunction() : CostFunction() { range_ = 0; }
virtual ~RandomFunction() {} virtual ~RandomFunction() {}
const static std::string GetName() { return "RandomFunction"; } const static std::string GetName() { return "RandomFunction"; }
......
...@@ -45,7 +45,7 @@ Vector2D SocialForcesAvoidance::ComputeAgentInteractionForce(const Agent* agent, ...@@ -45,7 +45,7 @@ Vector2D SocialForcesAvoidance::ComputeAgentInteractionForce(const Agent* agent,
// This means that the force always points exactly in the opposite direction of R. // This means that the force always points exactly in the opposite direction of R.
// The complicated part is finding its *magnitude*. // The complicated part is finding its *magnitude*.
const Vector2D& R = agent->getPosition() - other.position; const Vector2D& R = agent->getPosition() - other.GetPosition();
float magR = R.magnitude(); float magR = R.magnitude();
// if the agents are already colliding right now, don't apply any force; we'll do this via contact forces instead // if the agents are already colliding right now, don't apply any force; we'll do this via contact forces instead
...@@ -53,7 +53,7 @@ Vector2D SocialForcesAvoidance::ComputeAgentInteractionForce(const Agent* agent, ...@@ -53,7 +53,7 @@ Vector2D SocialForcesAvoidance::ComputeAgentInteractionForce(const Agent* agent,
return Vector2D(0, 0); return Vector2D(0, 0);
// preferred velocity of the other agent // preferred velocity of the other agent
float vb = other.velocity.magnitude(); float vb = other.GetVelocity().magnitude();
const Vector2D& Eb = other.realAgent->getPreferredVelocity().getnormalized(); const Vector2D& Eb = other.realAgent->getPreferredVelocity().getnormalized();
// B = semi-minor axis of an ellipse, describing the repulsive potential, // B = semi-minor axis of an ellipse, describing the repulsive potential,
......
...@@ -79,12 +79,13 @@ float TtcaDca::GetCost(const Vector2D& velocity, Agent* agent, const WorldBase * ...@@ -79,12 +79,13 @@ float TtcaDca::GetCost(const Vector2D& velocity, Agent* agent, const WorldBase *
// for each agent of the neighbourhood // for each agent of the neighbourhood
for (const auto& neighbor : neighbors.first) for (const auto& neighbor : neighbors.first)
{ {
if (distanceSquared(neighbor.position, Position) >= rangeSquared) const auto& neighborPos = neighbor.GetPosition();
if (neighbor.GetDistanceSquared() >= rangeSquared)
continue; continue;
// Compute relative velocity and relative position // Compute relative velocity and relative position
const Vector2D& relPos = neighbor.position - Position; const Vector2D& relPos = neighborPos - Position;
const Vector2D& relVelocity = neighbor.velocity - velocity; const Vector2D& relVelocity = neighbor.GetVelocity() - velocity;
// Only see agents in front of the direction of motion // Only see agents in front of the direction of motion
if (relPos.dot(velocity) < 0) if (relPos.dot(velocity) < 0)
...@@ -96,7 +97,7 @@ float TtcaDca::GetCost(const Vector2D& velocity, Agent* agent, const WorldBase * ...@@ -96,7 +97,7 @@ float TtcaDca::GetCost(const Vector2D& velocity, Agent* agent, const WorldBase *
// computing ttc and dca // computing ttc and dca
const auto& ttca_dca = ComputeTimeAndDistanceToClosestApproach( const auto& ttca_dca = ComputeTimeAndDistanceToClosestApproach(
Position, velocity, Radius, Position, velocity, Radius,
neighbor.position, neighbor.velocity, neighbor.realAgent->getRadius()); neighborPos, neighbor.GetVelocity(), neighbor.realAgent->getRadius());
if (ttca_dca.first < 0) continue; if (ttca_dca.first < 0) continue;
...@@ -146,12 +147,12 @@ Vector2D TtcaDca::GetGradient(const Vector2D& velocity, Agent* agent, const Worl ...@@ -146,12 +147,12 @@ Vector2D TtcaDca::GetGradient(const Vector2D& velocity, Agent* agent, const Worl
// for each agent of the neighbourhood // for each agent of the neighbourhood
for (const auto& neighbor : neighbors.first) for (const auto& neighbor : neighbors.first)
{ {
if (distanceSquared(Position, neighbor.position) > rangeSquared) if (neighbor.GetDistanceSquared() > rangeSquared)
continue; continue;
// Compute relative velocity and relative position // Compute relative velocity and relative position
const Vector2D& relPos = neighbor.position - Position; const Vector2D& relPos = neighbor.GetPosition() - Position;
const Vector2D& relVelocity = neighbor.velocity - velocity; const Vector2D& relVelocity = neighbor.GetVelocity() - velocity;
// Only see agents in front of the direction of motion // Only see agents in front of the direction of motion
if (relPos.dot(velocity) < 0) if (relPos.dot(velocity) < 0)
......
/* UMANS: Unified Microscopic Agent Navigation Simulator
** Copyright (C) 2018-2020 Inria Rennes Bretagne Atlantique - Rainbow - Julien Pettré
**
** This program is free software: you can redistribute it and/or modify
** it under the terms of the GNU General Public License as published by
** the Free Software Foundation, either version 3 of the License, or
** (at your option) any later version.
**
** This program is distributed in the hope that it will be useful,
** but WITHOUT ANY WARRANTY; without even the implied warranty of
** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
** GNU General Public License for more details.
**
** You should have received a copy of the GNU General Public License
** along with this program. If not, see <https://www.gnu.org/licenses/>.
**
** Contact: crowd_group@inria.fr
** Website: https://project.inria.fr/crowdscience/
** See the file AUTHORS.md for a list of all contributors.
*/
#include "core/costFunctionFactory.h"
#include "CostFunctions/costFunctionRegistration.h"
#include "CostFunctions/FOEAvoidance.h"
#include "CostFunctions/GenericCost.h"
#include "CostFunctions/GoalReachingForce.h"
#include "CostFunctions/Karamouzas.h"
#include "CostFunctions/Moussaid.h"
#include "CostFunctions/ORCA.h"
#include "CostFunctions/Paris.h"
#include "CostFunctions/PLEdestrians.h"
#include "CostFunctions/PowerLaw.h"
#include "CostFunctions/RandomFunction.h"
#include "CostFunctions/RVO.h"
#include "CostFunctions/SocialForcesAvoidance.h"
#include "CostFunctions/TtcaDca.h"
#include "CostFunctions/VanToll.h"
bool registered = false;
void RegisterCostFunctions()
{
if (registered)
return;
registered = true;
REGISTER_COST_FUNCTION(FOEAvoidance);
REGISTER_COST_FUNCTION(GenericCost);
REGISTER_COST_FUNCTION(GoalReachingForce);
REGISTER_COST_FUNCTION(Karamouzas);
REGISTER_COST_FUNCTION(Moussaid);
REGISTER_COST_FUNCTION(ORCA);
REGISTER_COST_FUNCTION(PowerLaw);
REGISTER_COST_FUNCTION(RandomFunction);
REGISTER_COST_FUNCTION(SocialForcesAvoidance);
REGISTER_COST_FUNCTION(TtcaDca);
REGISTER_COST_FUNCTION(RVO);
REGISTER_COST_FUNCTION(Paris);
REGISTER_COST_FUNCTION(PLEdestrians);
REGISTER_COST_FUNCTION(VanToll);
}
\ No newline at end of file
/* UMANS: Unified Microscopic Agent Navigation Simulator
** Copyright (C) 2018-2020 Inria Rennes Bretagne Atlantique - Rainbow - Julien Pettré
**
** This program is free software: you can redistribute it and/or modify
** it under the terms of the GNU General Public License as published by
** the Free Software Foundation, either version 3 of the License, or
** (at your option) any later version.
**
** This program is distributed in the hope that it will be useful,
** but WITHOUT ANY WARRANTY; without even the implied warranty of
** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
** GNU General Public License for more details.
**
** You should have received a copy of the GNU General Public License
** along with this program. If not, see <https://www.gnu.org/licenses/>.
**
** Contact: crowd_group@inria.fr
** Website: https://project.inria.fr/crowdscience/
** See the file AUTHORS.md for a list of all contributors.
*/
#ifndef LIB_COSTFUNCTIONREGISTRATION_H
#define LIB_COSTFUNCTIONREGISTRATION_H
void RegisterCostFunctions();
#endif //LIB_COSTFUNCTIONREGISTRATION_H
...@@ -42,15 +42,18 @@ std::vector<size_t> AgentKDTree::FindAllAgentsInRange(const Vector2D& position, ...@@ -42,15 +42,18 @@ std::vector<size_t> AgentKDTree::FindAllAgentsInRange(const Vector2D& position,
auto nrResults = kdTree->radiusSearch(q, radius*radius, result_indicesAndDistances, params); auto nrResults = kdTree->radiusSearch(q, radius*radius, result_indicesAndDistances, params);
// convert the result to a list of agent IDs, possibly ignoring a certain agent // convert the result to a list of agent IDs, possibly ignoring a certain agent
std::vector<size_t> result; std::vector<size_t> result(nrResults); size_t index = 0;
for (size_t i = 0; i < nrResults; ++i) for (size_t i = 0; i < nrResults; ++i)
{ {
const size_t pointCloudIndex = result_indicesAndDistances[i].first; const size_t pointCloudIndex = result_indicesAndDistances[i].first;
const size_t agentID = pointCloud.agentPositions[pointCloudIndex].first; const size_t agentID = pointCloud.agentPositions[pointCloudIndex].first;
if (agentToIgnore != nullptr && agentID == agentToIgnore->getID()) if (agentToIgnore != nullptr && agentID == agentToIgnore->getID())
continue; continue;
result.push_back(agentID); result[index] = agentID;
++index;
} }
if (index < nrResults)
result.pop_back();