Skip to content
GitLab
Commit aafd9369 authored by VAN TOLL Wouter's avatar VAN TOLL Wouter
Browse files

Merge branch 'profiling' into 'master' 

Performance improvements

See merge request !101
parents 67e46498 4fcca4f6
Hide whitespace changes
Inline Side-by-side
3rd-party/ORCA/ORCASolver.cpp
View file @ aafd9369
......@@ -54,12 +54,12 @@ namespace ORCALibrary
{
inline float det(const Vector2D &vector1, const Vector2D &vector2)
{
return (float)(vector1.x() * vector2.y() - vector1.y() * vector2.x());
return (float)(vector1.x * vector2.y - vector1.y * vector2.x);
}
/* 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
const float timeHorizon, const float currentTime, const float simulationTimeStep, const NeighborList& neighbors, const float maxDistance) const
{
const Vector2D& prefVelocity_ = agent.getPreferredVelocity();
const float maxSpeed_ = agent.getMaximumSpeed();
......@@ -75,7 +75,7 @@ namespace ORCALibrary
Solution result;
createAgentOrcaLines(agent, result.orcaLines, timeHorizon, simulationTimeStep, neighbors.first);
createAgentOrcaLines(agent, result.orcaLines, timeHorizon, simulationTimeStep, neighbors.first, maxDistance);
// TODO: include obstacle lines
// ...
......@@ -95,17 +95,20 @@ namespace ORCALibrary
}
void Solver::createAgentOrcaLines(const Agent& agent, std::vector<Line>& orcaLines_, const float timeHorizon_, const float simulationTimeStep,
const AgentNeighborList& agentNeighbors_) const
const AgentNeighborList& agentNeighbors_, const float maxDistance) const
{
const Vector2D& position_ = agent.getPosition();
const Vector2D& velocity_ = agent.getVelocity();
const float maxSpeed_ = agent.getMaximumSpeed();
const float radius_ = agent.getRadius();
const float radiusSq = radius_ * radius_;
const float maxDistSq = maxDistance * maxDistance;
/* Create agent ORCA lines. */
for (size_t i = 0; i < agentNeighbors_.size(); ++i) {
const PhantomAgent& other = agentNeighbors_[i];
for (const PhantomAgent& other : agentNeighbors_)
{
if (distanceSquared(other.position, position_) > maxDistSq)
continue;
const Vector2D& relativePosition = other.position - position_;
const Vector2D& relativeVelocity = velocity_ - other.velocity;
......@@ -129,7 +132,7 @@ namespace ORCALibrary
const float wLength = std::sqrt(wLengthSq);
const Vector2D& unitW = w / wLength;
line.direction = Vector2D(unitW.y(), -unitW.x());
line.direction = Vector2D(unitW.y, -unitW.x);
u = unitW * (combinedRadius / timeHorizon_ - wLength);
}
else {
......@@ -138,11 +141,11 @@ namespace ORCALibrary
if (det(relativePosition, w) > 0.0f) {
/* Project on left leg. */
line.direction = Vector2D(relativePosition.x() * leg - relativePosition.y() * combinedRadius, relativePosition.x() * combinedRadius + relativePosition.y() * leg) / distSq;
line.direction = Vector2D(relativePosition.x * leg - relativePosition.y * combinedRadius, relativePosition.x * combinedRadius + relativePosition.y * leg) / distSq;
}
else {
/* Project on right leg. */
line.direction = Vector2D(relativePosition.x() * leg + relativePosition.y() * combinedRadius, -relativePosition.x() * combinedRadius + relativePosition.y() * leg) * -1 / distSq;
line.direction = Vector2D(relativePosition.x * leg + relativePosition.y * combinedRadius, -relativePosition.x * combinedRadius + relativePosition.y * leg) * -1 / distSq;
}
const float dotProduct2 = relativeVelocity.dot(line.direction);
......@@ -159,7 +162,7 @@ namespace ORCALibrary
const float wLength = w.sqrMagnitude();
const Vector2D unitW = w / wLength;
line.direction = Vector2D(unitW.y(), -unitW.x());
line.direction = Vector2D(unitW.y, -unitW.x);
u = unitW * (combinedRadius / simulationTimeStep - wLength);
}
......@@ -309,7 +312,7 @@ namespace ORCALibrary
const Vector2D tempResult = result;
if (linearProgram2(projLines, radius, Vector2D(-lines[i].direction.y(), lines[i].direction.x()), true, result) < projLines.size()) {
if (linearProgram2(projLines, radius, Vector2D(-lines[i].direction.y, lines[i].direction.x), true, result) < projLines.size()) {
/* This should in principle not happen. The result is by definition
* already in the feasible region of this linear program. If it fails,
* it is due to small floating point error, and the current result is
......
3rd-party/ORCA/ORCASolver.h
View file @ aafd9369
......@@ -65,13 +65,13 @@ namespace ORCALibrary
{
public:
Solution solveOrcaProgram(const Agent& agent,
const float timeHorizon, const float currentTime, const float simulationTimeStep, const NeighborList& neighbors) const;
const float timeHorizon, const float currentTime, const float simulationTimeStep, const NeighborList& neighbors, const float maxDistance) const;
private:
/*void createObstacleOrcaLines(const Agent& agent,
std::vector<Line>& orcaLines_, const float timeHorizonObst_, const float simulationTimeStep, const ObstacleNeighborList& obstacleNeighbors_) const;*/
std::vector<Line>& orcaLines_, const float timeHorizonObst_, const float simulationTimeStep, const ObstacleNeighborList& obstacleNeighbors_, const float maxDistance) const;*/
void createAgentOrcaLines(const Agent& agent,
std::vector<Line>& orcaLines_, const float timeHorizon_, const float simulationTimeStep, const AgentNeighborList& agentNeighbors_) const;
std::vector<Line>& orcaLines_, const float timeHorizon_, const float simulationTimeStep, const AgentNeighborList& agentNeighbors_, const float maxDistance) const;
};
/**
......
APIFunctions.cpp
View file @ aafd9369
......@@ -92,12 +92,12 @@ extern "C" {
for (int i = 0; i < (int)agents.size(); ++i)
{
agentData[i].id = (int)agents[i]->getID();
agentData[i].position_x = agents[i]->getPosition().x();
agentData[i].position_y = agents[i]->getPosition().y();
agentData[i].velocity_x = agents[i]->getVelocity().x();
agentData[i].velocity_y = agents[i]->getVelocity().y();
agentData[i].viewingDirection_x = agents[i]->getViewingDirection().x();
agentData[i].viewingDirection_y = agents[i]->getViewingDirection().y();
agentData[i].position_x = agents[i]->getPosition().x;
agentData[i].position_y = agents[i]->getPosition().y;
agentData[i].velocity_x = agents[i]->getVelocity().x;
agentData[i].velocity_y = agents[i]->getVelocity().y;
agentData[i].viewingDirection_x = agents[i]->getViewingDirection().x;
agentData[i].viewingDirection_y = agents[i]->getViewingDirection().y;
}
// store references to these results, for the client program to use
......
include/CostFunctions/Moussaid.h
View file @ aafd9369
......@@ -56,7 +56,7 @@ public:
void parseParameters(const CostFunctionParameters & params) override;
private:
float getDistanceToCollisionAtPreferredSpeed(const Vector2D& direction, const Agent* agent, const NeighborList& neighbors) const;
float getDistanceToCollisionAtPreferredSpeed(const Vector2D& direction, const Agent* agent) const;
};
#endif //LIB_MOUSSAID_H
include/CostFunctions/Paris.h
View file @ aafd9369
......@@ -77,7 +77,7 @@ public:
private:
std::pair<float, float> ComputeT1andT2(const Vector2D& origin, const Vector2D& velocity, const Vector2D& point, const float radius) const;
NeighborAvoidanceRange ComputeNeighborAvoidanceRange(const Vector2D& direction, const Agent* agent, const PhantomAgent& neighbor) const;
NeighborAvoidanceRangeList ComputeAllNeighborAvoidanceRanges(const Vector2D& direction, const Agent* agent, const NeighborList& neighbors) const;
NeighborAvoidanceRangeList ComputeAllNeighborAvoidanceRanges(const Vector2D& direction, const Agent* agent) const;
float GetDirectionCost(const Vector2D& direction, const Agent* agent, const NeighborAvoidanceRangeList& avoidanceTimes) const;
float GetSpeedDeviationCost(const float speed, const Vector2D& direction, const Agent* agent, const NeighborAvoidanceRangeList& avoidanceRanges) const;
......
include/core/AgentKDTree.h
View file @ aafd9369
......@@ -53,8 +53,8 @@ private:
// "if/else's" are actually solved at compile time.
inline double kdtree_get_pt(const size_t idx, const size_t dim) const
{
if (dim == 0) return agentPositions[idx].second.x();
else return agentPositions[idx].second.y();
if (dim == 0) return agentPositions[idx].second.x;
else return agentPositions[idx].second.y;
}
// Optional bounding-box computation: return false to default to a standard bbox computation loop.
......
include/core/agent.h
View file @ aafd9369
......@@ -162,13 +162,6 @@ public:
/// Methods that compute and return a result based on the agent's internal state.
/// @{
/// <summary>Returns the neighbors that are within a given range of this agent.</summary>
/// <remarks>This method uses the pre-computed list of neighbors and filters it by the given distance.
/// The method does *not* perform a new nearest-neighbor query.</remarks>
/// <param name="range">A distance in meters; this method will return all neighbors within 'range' meters from the agent.</param>
/// <returns>A copy of the agent's NeighborList containing only the neighbors that lie witihn the given distance.</returns>
NeighborList getNeighborSubsetInRange(float range) const;
/// <summary>Checks and returns whether the agent has reached its goal position.</summary>
/// <returns>true if the agent's current position is sufficiently close to its goal; false otherwise.</returns>
bool hasReachedGoal() const;
......
include/core/costFunction.h
View file @ aafd9369
......@@ -220,6 +220,8 @@ protected:
/// <param name="velocity">The hypothetical velocity of the querying agent.</param>
/// <param name="radius">The radius (in meters) of the querying agent.</param>
/// <param name="neighbors">A list of neighboring agents and obstacles.</param>
/// <param name="maximumDistance">The maximum distance between the query position and a neighboring object.
/// Any objects farther away will be ignored.</param>
/// <param name="ignoreCurrentCollisions">Whether or not to ignore any collisions that are already happening now.</param>
/// <returns>The smallest time to collision (in seconds) of the querying agent with all neighbors in the list.
/// This value may be 0 if there is already a collision with any of the neighboring objects.
......@@ -228,7 +230,7 @@ protected:
/// <seealso cref="ComputeTimeToCollision"/>
float ComputeTimeToFirstCollision(
const Vector2D& position, const Vector2D& velocity, const float radius,
const NeighborList& neighbors, bool ignoreCurrentCollisions = true) const;
const NeighborList& neighbors, float maximumDistance, bool ignoreCurrentCollisions) const;
/// <summary>Computes the expected time at which the distance between two disk-shaped objects is minimal, and the value of this distance.</summary>
/// <param name="position1">The current position of object 1.</param>
......
include/core/worldBase.h
View file @ aafd9369
......@@ -26,7 +26,8 @@
#include <memory>
#include <string>
#include <queue>
#include <limits> // std::reference_wrapper
#include <limits>
#include <unordered_map>
#include <tools/Polygon2D.h>
#include <core/agent.h>
......@@ -87,7 +88,7 @@ private:
/// <remarks>Because agents can be removed during the simulation, the ID of an agent is not necessarily the same
/// as its position in the list. This is why we need this extra administration.
/// (We could also put all agents directly in a map, but then we could not loop over the agents in parallel with OpenMP.)</remarks>
std::map<size_t, size_t> agentPositionsInVector;
std::unordered_map<size_t, size_t> agentPositionsInVector;
/// <summary>The agent ID that will be used for the next agent that gets added.</summary>
size_t nextUnusedAgentID;
......
include/tools/Matrix.h
View file @ aafd9369
......@@ -51,7 +51,7 @@ public:
/// <param name="v1">A 2D vector containing the desired left column of the matrix.</param>
/// <param name="v2">A 2D vector containing the desired right column of the matrix.</param>
Matrix(const Vector2D& v1, const Vector2D& v2)
: a11_(v1.x()), a12_(v2.x()), a21_(v1.y()), a22_(v2.y()) {}
: a11_(v1.x), a12_(v2.x), a21_(v1.y), a22_(v2.y) {}
/// <summary>Computes and returns the transposed version of this Matrix.</summary>
/// <returns>A Matrix object similar to the current one, but with a21 and a12 swapped.</return>
......@@ -113,12 +113,12 @@ inline Matrix operator*(const Matrix& lhs, const float &rhs)
inline Vector2D operator*(const Matrix& lhs, const Vector2D& rhs)
{
return Vector2D(lhs.a11()*rhs.x() + lhs.a12()*rhs.y(), lhs.a21()*rhs.x() + lhs.a22()*rhs.y());
return Vector2D(lhs.a11()*rhs.x + lhs.a12()*rhs.y, lhs.a21()*rhs.x + lhs.a22()*rhs.y);
}
inline Matrix outerProduct(const Vector2D& lhs, const Vector2D& rhs)
{
return Matrix(lhs.x()*rhs.x(), lhs.x()*rhs.y(), lhs.y()*rhs.x(), lhs.y()*rhs.y());
return Matrix(lhs.x*rhs.x, lhs.x*rhs.y, lhs.y*rhs.x, lhs.y*rhs.y);
}
#endif // LIB_MATRIX_H
include/tools/vector2D.h
View file @ aafd9369
......@@ -30,32 +30,26 @@ const double PI = 3.1415926535897;
/// <summary>A 2D vector, used for representing positions, velocities, etc.</summary>
class Vector2D
{
private:
public:
/// <summary>The x component of this vector.</summary>
float x_;
float x;
/// <summary>The y component of this vector.</summary>
float y_;
float y;
public:
/// <summary>Creates a Vector2D with both components set to zero.</summary>
Vector2D() : x_(0.0f), y_(0.0f) {}
Vector2D() : x(0.0f), y(0.0f) {}
/// <summary>Creates a Vector2D with the given x and y components.</summary>
/// <param name="x">The desired x component of the vector.</param>
/// <param name="y">The desired y component of the vector.</param>
Vector2D(float x, float y) : x_(x), y_(y) { }
/// <summary>Retrieves the x component of this Vector2D.</summary>
inline float x() const { return x_; }
/// <summary>Retrieves the y component of this Vector2D.</summary>
inline float y() const { return y_; }
Vector2D(float x, float y) : x(x), y(y) { }
/// <summary>Computes and returns the magnitude of this Vector2D.</summary>
/// <returns>The vector's magnitude, i.e. sqrt(x*x + y*y).</returns>
inline float magnitude() const { return sqrtf(x_ * x_ + y_ * y_); }
inline float magnitude() const { return sqrtf(x * x + y * y); }
/// <summary>Computes and returns the squared magnitude of this Vector2D.</summary>
/// <returns>The vector's squared magnitude, i.e. x*x + y*y.</returns>
inline float sqrMagnitude() const { return x_ * x_ + y_ * y_; }
inline float sqrMagnitude() const { return x * x + y * y; }
/// <summary>Normalizes this Vector2D to unit length, by dividing both x and y by the magnitude.</summary>
inline void normalize()
......@@ -63,8 +57,8 @@ public:
float mag = magnitude();
if (mag > 0)
{
x_ /= mag;
y_ /= mag;
x /= mag;
y /= mag;
}
}
......@@ -72,7 +66,7 @@ public:
/// <returns>A Vector2D with the same direction as the current vector, but with unit length.</returns>
inline Vector2D getnormalized() const
{
Vector2D Result(x_, y_);
Vector2D Result(x, y);
Result.normalize();
return Result;
}
......@@ -82,16 +76,7 @@ public:
/// <returns>The dot product of the current Vector2D and 'other', i.e. x*other.x + y*other.y.</returns>
inline float dot(const Vector2D& other) const
{
return x_ * other.x_ + y_ * other.y_;
}
/// <summary>Sets the x and y components of this Vector2D to the given values.</summary>
/// <param name="x">The desired new x component of this vector.</param>
/// <param name="y">The desired new y component of this vector.</param>
void set(float x, float y)
{
x_ = x;
y_ = y;
return x * other.x + y * other.y;
}
/// <summary>Adds another Vector2D to the current Vector2D.</summary>
......@@ -99,7 +84,7 @@ public:
/// <returns>A reference to the current Vector2D after the operation has been performed.</returns>
inline Vector2D& operator+=(const Vector2D& rhs)
{
x_ += rhs.x(); y_ += rhs.y();
x += rhs.x; y += rhs.y;
return *this;
}
......@@ -108,7 +93,7 @@ public:
/// <returns>A reference to the current Vector2D after the operation has been performed.</returns>
inline Vector2D& operator-=(const Vector2D& rhs)
{
x_ -= rhs.x(); y_ -= rhs.y();
x -= rhs.x; y -= rhs.y;
return *this;
}
......@@ -116,7 +101,7 @@ public:
/// <returns>true if both x and y are 0; false otherwise.</returns>
inline bool isZero() const
{
return x_ == 0 && y_ == 0;
return x == 0 && y == 0;
}
};
......@@ -124,44 +109,54 @@ typedef std::pair<Vector2D, Vector2D> LineSegment2D;
#pragma region [Arithmetic operators]
inline Vector2D operator-(Vector2D lhs, Vector2D rhs)
inline Vector2D operator-(const Vector2D& lhs, const Vector2D& rhs)
{
return Vector2D(lhs.x() - rhs.x(), lhs.y() - rhs.y());
return Vector2D(lhs.x - rhs.x, lhs.y - rhs.y);
}
inline Vector2D operator+(Vector2D lhs, Vector2D rhs)
inline Vector2D operator+(const Vector2D& lhs, const Vector2D& rhs)
{
return Vector2D(lhs.x() + rhs.x(), lhs.y() + rhs.y());
return Vector2D(lhs.x + rhs.x, lhs.y + rhs.y);
}
inline Vector2D operator*(float lhs, Vector2D rhs)
inline Vector2D operator*(float lhs, const Vector2D& rhs)
{
return Vector2D(rhs.x() * lhs, rhs.y() * lhs);
return Vector2D(rhs.x * lhs, rhs.y * lhs);
}
inline Vector2D operator*(Vector2D lhs, float rhs)
inline Vector2D operator*(const Vector2D& lhs, float rhs)
{
return rhs * lhs;
}
inline Vector2D operator/(Vector2D lhs, float rhs)
inline Vector2D operator/(const Vector2D& lhs, float rhs)
{
return (1.0f / rhs) * lhs;
}
inline bool operator==(const Vector2D& p, const Vector2D& q)
{
return p.x() == q.x() && p.y() == q.y();
return p.x == q.x && p.y == q.y;
}
inline bool operator!=(const Vector2D& p, const Vector2D& q)
{
return p.x() != q.x() || p.y() != q.y();
return p.x != q.x || p.y != q.y;
}
inline Vector2D operator-(Vector2D lhs)
inline Vector2D operator-(const Vector2D& lhs)
{
return Vector2D(-lhs.x(), -lhs.y());
return Vector2D(-lhs.x, -lhs.y);
}
inline float distance(const Vector2D& p, const Vector2D& q)
{
return (p - q).magnitude();
}
inline float distanceSquared(const Vector2D& p, const Vector2D& q)
{
return (p - q).sqrMagnitude();
}
#pragma endregion
......@@ -171,7 +166,7 @@ inline Vector2D operator-(Vector2D lhs)
inline Vector2D rotateCounterClockwise(const Vector2D& v, float radians)
{
const double Cos = cos(radians), Sin = sin(radians);
return Vector2D((float)(Cos*v.x() - Sin * v.y()), (float)(Sin*v.x() + Cos * v.y()));
return Vector2D((float)(Cos*v.x - Sin * v.y), (float)(Sin*v.x + Cos * v.y));
}
inline Vector2D rotateCounterClockwiseAroundPoint(const Vector2D& v, const Vector2D& pivot, float radians)
......@@ -205,13 +200,13 @@ inline float cosAngle(const Vector2D& va, const Vector2D& vb)
inline bool isClockwise(const Vector2D &vector1, const Vector2D &vector2)
{
float cross = vector1.x() * vector2.y() - vector1.y() * vector2.x();
float cross = vector1.x * vector2.y - vector1.y * vector2.x;
return cross < 0;
}
inline bool isCounterClockwise(const Vector2D &vector1, const Vector2D &vector2)
{
float cross = vector1.x() * vector2.y() - vector1.y() * vector2.x();
float cross = vector1.x * vector2.y - vector1.y * vector2.x;
return cross > 0;
}
......@@ -263,14 +258,14 @@ inline Vector2D clamp(const Vector2D& v, float maxLength)
inline bool getLineIntersection(const Vector2D& a, const Vector2D& b, const Vector2D& c, const Vector2D& d, Vector2D& result)
{
// Line AB represented as a1x + b1y = c1
double a1 = b.y() - a.y();
double b1 = a.x() - b.x();
double c1 = a1 * a.x() + b1 * a.y();
double a1 = b.y - a.y;
double b1 = a.x - b.x;
double c1 = a1 * a.x + b1 * a.y;
// Line CD represented as a2x + b2y = c2
double a2 = d.y() - c.y();
double b2 = c.x() - d.x();
double c2 = a2 * c.x() + b2 * c.y();
double a2 = d.y - c.y;
double b2 = c.x - d.x;
double c2 = a2 * c.x + b2 * c.y;
double determinant = a1 * b2 - a2 * b1;
......@@ -311,7 +306,12 @@ inline Vector2D nearestPointOnLine(const Vector2D& pt, const Vector2D& la, const
inline float distanceToLine(const Vector2D& pt, const Vector2D& la, const Vector2D& lb, bool onSegment = false)
{
return (pt - nearestPointOnLine(pt, la, lb, onSegment)).magnitude();
return distance(pt, nearestPointOnLine(pt, la, lb, onSegment));
}
inline float distanceToLineSquared(const Vector2D& pt, const Vector2D& la, const Vector2D& lb, bool onSegment = false)
{
return distanceSquared(pt, nearestPointOnLine(pt, la, lb, onSegment));
}
#endif // LIB_VECTOR2D_H
main.cpp
View file @ aafd9369
......@@ -39,11 +39,12 @@ void printBasicInfo()
void printUsageInfo(const std::string& programName)
{
std::cout
<< "Usage: " << programName << " -i -o [-t]" << std::endl
<< "Usage: " << programName << " -i [-o] [-t]" << std::endl
<< " -i (or -input) = An XML file describing the simulation scenario to run." << std::endl
<< " For help on creating scenario files, please see the UMANS documentation." << std::endl
<< " -o (or -output) = Name of a folder to which the simulation output will be written." << std::endl
<< " -o (or -output) = (optional) Name of a folder to which the simulation output will be written." << std::endl
<< " The program will write a CSV file for each agent's trajectory." << std::endl
<< " If you omit this, the program will run faster, but no results will be saved." << std::endl
<< " -t (or -nrThreads) = (optional, default=1) The number of parallel threads to use." << std::endl << std::endl;
}
......@@ -75,26 +76,35 @@ int main( int argc, char * argv[] )
nrThreads = atoi(paramValue.c_str());
}
// if any argument is incorrect, show an error
if (configFile == "" || outputFolder == "")
// input file is mandatory
if (configFile == "")
{
std::cerr << "Input error: Please specify all required arguments." << std::endl;
std::cerr << "Input error: Please specify an input scenario." << std::endl;
printUsageInfo(argv[0]);
return 0;
}
// number of threads must be at least 1
if (nrThreads < 1)
{
std::cerr << "Input error: Please specify a positive number of threads." << std::endl;
printUsageInfo(argv[0]);
return 0;
}
// output folder may be empty; if so, show a warning
if (outputFolder == "")
{
std::cout << "Input warning: No output folder specified." << std::endl
<< "The program will not write any simulation results to CSV files." << std::endl;
}
// run the simulation
CrowdSimulator* cs = CrowdSimulator::FromConfigFile(configFile, true);
if (cs != nullptr)
{
cs->SetCSVOutputDirectory(outputFolder);
if (outputFolder != "")
cs->SetCSVOutputDirectory(outputFolder);
cs->RunSimulationUntilEnd();
}
......
src/CostFunctions/FOEAvoidance.cpp
View file @ aafd9369
......@@ -53,27 +53,31 @@ float FOEAvoidance::GetCost(const Vector2D& velocity, Agent* agent, const WorldB
const Vector2D& Position = agent->getPosition();
const Vector2D& Vnorm = velocity.getnormalized();
Matrix R(Vector2D(Vnorm.y(), -Vnorm.x()), Vnorm);
Matrix R(Vector2D(Vnorm.y, -Vnorm.x), Vnorm);
const Matrix& RT = R.GetTransposed();
const float rangeSquared = range_ * range_;
const auto& neighbors = agent->getNeighbors();
float Cost = 0;
const auto& neighbors = agent->getNeighborSubsetInRange(range_);
// check neighboring agents
for (const PhantomAgent& neighbor : neighbors.first)
{
if (distanceSquared(Position, neighbor.position) > rangeSquared)
continue;
Vector2D Vel = RT * (neighbor.velocity - velocity);
Vector2D Pos = RT * (neighbor.position - Position);
if (Pos.y() < MIN_DISTANCE || Vel.y() > -MIN_VELOCITY)
if (Pos.y < MIN_DISTANCE || Vel.y > -MIN_VELOCITY)
continue;
float xf = Vel.x() / Vel.y();
float xg = Pos.x() / Pos.y();
float xf = Vel.x / Vel.y;
float xg = Pos.x / Pos.y;
float dx = xg - xf;
float ttc = -Pos.y() / Vel.y();
float ttc = -Pos.y / Vel.y;
float sigma = (float)(((agent->getRadius() + neighbor.realAgent->getRadius()) / Pos.y()) / log(10));
float sigma = (float)(((agent->getRadius() + neighbor.realAgent->getRadius()) / Pos.y) / log(10));
float localCost = Importance(ttc)*exp(-abs(dx) / sigma);
......@@ -93,30 +97,34 @@ Vector2D FOEAvoidance::GetGradient(const Vector2D& velocity, Agent* agent, const
const Vector2D& Position = agent->getPosition();
const Vector2D& Vnorm = velocity.getnormalized();
Matrix R(Vector2D(Vnorm.y(), -Vnorm.x()), Vnorm);
Matrix R(Vector2D(Vnorm.y, -Vnorm.x), Vnorm);
const Matrix& RT = R.GetTransposed();
const float rangeSquared = range_ * range_;
const auto& neighbors = agent->getNeighbors();
float gradtheta = 0, gradv = 0;
const auto& neighbors = agent->getNeighborSubsetInRange(range_);
// check neighboring agents
for (const PhantomAgent& neighbor : neighbors.first)
{
if (distanceSquared(Position, neighbor.position) > rangeSquared)
continue;
Vector2D Vel = RT * (neighbor.velocity - velocity);
Vector2D Pos = RT * (neighbor.position - Position);
if (Pos.y() < MIN_DISTANCE || Vel.y() > -MIN_VELOCITY)
if (Pos.y < MIN_DISTANCE || Vel.y > -MIN_VELOCITY)
continue;
float xf = Vel.x() / Vel.y();
float xg = Pos.x() / Pos.y();
float xf = Vel.x / Vel.y;
float xg = Pos.x / Pos.y;
float dx = xg - xf;
float ttc = -Pos.y() / Vel.y();
float ttc = -Pos.y / Vel.y;
float sigma = (float)(((agent->getRadius() + neighbor.realAgent->getRadius()) / Pos.y()) / log(10));
float sigma = (float)(((agent->getRadius() + neighbor.realAgent->getRadius()) / Pos.y) / log(10));