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
......@@ -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
......
......@@ -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;
};
/**
......
......@@ -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
......
......@@ -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
......@@ -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;
......
......@@ -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.
......
......@@ -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;
......
......@@ -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>
......
......@@ -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;
......
......@@ -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
......@@ -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
......@@ -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();
}
......
......@@ -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));