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

- Changed PhantomAgent so that it only stores an offset, and not an actual...

- Changed PhantomAgent so that it only stores an offset, and not an actual translated position. This allows the simulation to re-use a PhantomAgent over multiple frames, so we can now decide to do neighbor queries less often.
- Changed other classes accordingly.
parent 98d63493
......@@ -107,11 +107,11 @@ namespace ORCALibrary
/* Create agent ORCA lines. */
for (const PhantomAgent& other : agentNeighbors_)
{
if (distanceSquared(other.position, position_) > maxDistSq)
if (distanceSquared(other.GetPosition(), position_) > maxDistSq)
continue;
const Vector2D& relativePosition = other.position - position_;
const Vector2D& relativeVelocity = velocity_ - other.velocity;
const Vector2D& relativePosition = other.GetPosition() - position_;
const Vector2D& relativeVelocity = velocity_ - other.GetVelocity();
const float distSq = relativePosition.sqrMagnitude();
const float combinedRadius = radius_ + other.realAgent->getRadius();
const float combinedRadiusSq = combinedRadius * combinedRadius;
......
......@@ -64,11 +64,11 @@ float FOEAvoidance::GetCost(const Vector2D& velocity, Agent* agent, const WorldB
// check neighboring agents
for (const PhantomAgent& neighbor : neighbors.first)
{
if (distanceSquared(Position, neighbor.position) > rangeSquared)
if (distanceSquared(Position, neighbor.GetPosition()) > rangeSquared)
continue;
Vector2D Vel = RT * (neighbor.velocity - velocity);
Vector2D Pos = RT * (neighbor.position - Position);
Vector2D Vel = RT * (neighbor.GetVelocity() - velocity);
Vector2D Pos = RT * (neighbor.GetPosition() - Position);
if (Pos.y < MIN_DISTANCE || Vel.y > -MIN_VELOCITY)
continue;
......@@ -108,11 +108,11 @@ Vector2D FOEAvoidance::GetGradient(const Vector2D& velocity, Agent* agent, const
// check neighboring agents
for (const PhantomAgent& neighbor : neighbors.first)
{
if (distanceSquared(Position, neighbor.position) > rangeSquared)
if (distanceSquared(Position, neighbor.GetPosition()) > rangeSquared)
continue;
Vector2D Vel = RT * (neighbor.velocity - velocity);
Vector2D Pos = RT * (neighbor.position - Position);
Vector2D Vel = RT * (neighbor.GetVelocity() - velocity);
Vector2D Pos = RT * (neighbor.GetPosition() - Position);
if (Pos.y < MIN_DISTANCE || Vel.y > -MIN_VELOCITY)
continue;
......
......@@ -33,7 +33,7 @@ Vector2D ObjectInteractionForces::ComputeForce(Agent* agent, const WorldBase * w
const auto& neighbors = agent->getNeighbors();
for (const PhantomAgent& other : neighbors.first)
{
if (distanceSquared(other.position, Position) <= rangeSquared)
if (distanceSquared(other.GetPosition(), Position) <= rangeSquared)
AgentForces += ComputeAgentInteractionForce(agent, other);
}
......
......@@ -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
{
const auto& agentPos = agent->getPosition();
const auto& neighborPos = neighbor.GetPosition();
const float radius1 = agent->getRadius(), radius2 = neighbor.realAgent->getRadius();
// Find the point where the trajectories of 'agent' and 'neighbor' intersect
Vector2D X;
bool intersects = getLineIntersection(
agentPos, agentPos + direction,
neighbor.position, neighbor.position + neighbor.velocity,
neighborPos, neighborPos + neighbor.GetVelocity(),
X);
// If there is no intersection, two things could be going on:
......@@ -104,7 +105,7 @@ Paris::NeighborAvoidanceRange Paris::ComputeNeighborAvoidanceRange(const Vector2
if (!intersects)
{
// 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 (ttc > t_max)
......@@ -121,7 +122,7 @@ Paris::NeighborAvoidanceRange Paris::ComputeNeighborAvoidanceRange(const Vector2
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
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
float distToX = (X - agentPos).magnitude();
......@@ -141,7 +142,7 @@ Paris::NeighborAvoidanceRangeList Paris::ComputeAllNeighborAvoidanceRanges(const
// agents
for (const auto& neighbor : neighbors.first)
{
if (distanceSquared(Position, neighbor.position) > rangeSquared)
if (distanceSquared(Position, neighbor.GetPosition()) > rangeSquared)
continue;
result.push_back(ComputeNeighborAvoidanceRange(direction, agent, neighbor));
......
......@@ -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.
float R = other.realAgent->getRadius() + agent->getRadius();
const Vector2D& x = agent->getPosition() - other.position;
const Vector2D& v = agent->getVelocity() - other.velocity;
const Vector2D& x = agent->getPosition() - other.GetPosition();
const Vector2D& v = agent->getVelocity() - other.GetVelocity();
float a = v.dot(v);
float b = -x.dot(v);
float c = x.dot(x) - R * R;
......
......@@ -45,7 +45,7 @@ Vector2D SocialForcesAvoidance::ComputeAgentInteractionForce(const Agent* agent,
// This means that the force always points exactly in the opposite direction of R.
// 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();
// 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,
return Vector2D(0, 0);
// preferred velocity of the other agent
float vb = other.velocity.magnitude();
float vb = other.GetVelocity().magnitude();
const Vector2D& Eb = other.realAgent->getPreferredVelocity().getnormalized();
// 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 *
// for each agent of the neighbourhood
for (const auto& neighbor : neighbors.first)
{
if (distanceSquared(neighbor.position, Position) >= rangeSquared)
const auto& neighborPos = neighbor.GetPosition();
if (distanceSquared(neighborPos, Position) >= rangeSquared)
continue;
// Compute relative velocity and relative position
const Vector2D& relPos = neighbor.position - Position;
const Vector2D& relVelocity = neighbor.velocity - velocity;
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)
......@@ -96,7 +97,7 @@ float TtcaDca::GetCost(const Vector2D& velocity, Agent* agent, const WorldBase *
// computing ttc and dca
const auto& ttca_dca = ComputeTimeAndDistanceToClosestApproach(
Position, velocity, Radius,
neighbor.position, neighbor.velocity, neighbor.realAgent->getRadius());
neighborPos, neighbor.GetVelocity(), neighbor.realAgent->getRadius());
if (ttca_dca.first < 0) continue;
......@@ -146,12 +147,12 @@ Vector2D TtcaDca::GetGradient(const Vector2D& velocity, Agent* agent, const Worl
// for each agent of the neighbourhood
for (const auto& neighbor : neighbors.first)
{
if (distanceSquared(Position, neighbor.position) > rangeSquared)
if (distanceSquared(Position, neighbor.GetPosition()) > rangeSquared)
continue;
// Compute relative velocity and relative position
const Vector2D& relPos = neighbor.position - Position;
const Vector2D& relVelocity = neighbor.velocity - velocity;
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)
......
......@@ -235,10 +235,10 @@ float CostFunction::ComputeTimeToFirstCollision(const Vector2D& position, const
// check neighboring agents
for (const auto& neighborAgent : neighbors.first)
{
if (distanceSquared(position, neighborAgent.position) > maxDistSquared)
if (distanceSquared(position, neighborAgent.GetPosition()) > maxDistSquared)
continue;
float ttc = ComputeTimeToCollision(position, velocity, radius, neighborAgent.position, neighborAgent.velocity, neighborAgent.realAgent->getRadius());
float ttc = ComputeTimeToCollision(position, velocity, radius, neighborAgent.GetPosition(), neighborAgent.GetVelocity(), neighborAgent.realAgent->getRadius());
// ignore current collisions?
if (ignoreCurrentCollisions && ttc == 0)
......
......@@ -115,7 +115,7 @@ Vector2D Policy::ComputeContactForces(Agent* agent, WorldBase * world)
// check all colliding agents
for (const auto& neighborAgent : neighbors.first)
{
const auto& diff = position - neighborAgent.position;
const auto& diff = position - neighborAgent.GetPosition();
const float intersectionDistance = radius + neighborAgent.realAgent->getRadius() - diff.magnitude();
if (intersectionDistance > 0)
totalForce += diff.getnormalized() * (contactForceScale_ * intersectionDistance);
......
......@@ -37,23 +37,27 @@
/// Nearest-neighbor queries in WorldBase return results of this type.</summary>
struct PhantomAgent
{
/// <summary>The agent's position, possibly transposed to match the type of world in which the simulation takes place.</summary>
Vector2D position;
/// <summary>The agent's velocity, possibly transposed to match the type of world in which the simulation takes place.</summary>
Vector2D velocity;
private:
/// <summary>The amount by which the neighboring Agent's position should be offset, e.g. to account for wrap-around in a toric world.</summary>
Vector2D positionOffset;
public:
/// <summary>A pointer to the original Agent in the simulation.</summary>
const Agent* realAgent;
/// <summary>Creates a PhantomAgent with the given details.</summary>
/// <param name="pos">The position of the PhantomAgent.</param>
/// <param name="vel">The velocity of the PhantomAgent.</param>
/// <param name="agent">A pointer to the original Agent in the simulation.</param>
PhantomAgent(const Vector2D& pos, const Vector2D& vel, const Agent* agent)
: position(pos), velocity(vel), realAgent(agent){}
/// <param name="posOffset">The amount by which the neighboring Agent's position should be offset, e.g. to account for wrap-around in a toric world.</param>
PhantomAgent(const Agent* agent, const Vector2D& posOffset = Vector2D(0, 0))
: realAgent(agent), positionOffset(posOffset) {}
PhantomAgent() : realAgent(nullptr) {}
/// <summary>Computes and returns this neighboring agent's position, translated by the offset of this PhantomAgent.</summary>
inline Vector2D GetPosition() const { return realAgent->getPosition() + positionOffset; }
/// <summary>Returns the velocity of this neighboring agent.</summary>
inline Vector2D GetVelocity() const { return realAgent->getVelocity(); }
};
/// <summary>An abstract class describing a world in which a simulation can take place.</summary>
......
......@@ -36,7 +36,7 @@ NeighborList WorldInfinite::ComputeNeighbors(const Vector2D& position, float sea
const auto& agents = computeNeighboringAgents_Flat(position, search_radius, queryingAgent);
for (const Agent* agent : agents)
phantoms.push_back(PhantomAgent(agent->getPosition(), agent->getVelocity(), agent));
phantoms.push_back(PhantomAgent(agent));
}
return { phantoms, computeNeighboringObstacles_Flat(position, search_radius) };
......
......@@ -37,7 +37,7 @@ void WorldToric::computeNeighbors_Displaced(const Vector2D& position, const Vect
// agents
const auto& agents = computeNeighboringAgents_Flat(position + displacement, search_radius, queryingAgent);
for (const Agent* agent : agents)
result.first.push_back(PhantomAgent(agent->getPosition() - displacement, agent->getVelocity(), agent));
result.first.push_back(PhantomAgent(agent, -displacement));
}
// obstacles
......
......@@ -621,7 +621,7 @@ void UMANSOpenGLWidget::mouseReleaseEvent(QMouseEvent *event)
if (!nearest.first.empty())
{
// check the pixel distance between the mouse and the agent
double worldDistance = distance(nearest.first[0].position, p);
double worldDistance = distance(nearest.first[0].GetPosition(), p);
double screenDistance = worldDistance / getScreenToWorldScale().width();
// if the agent is close enough, select it
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment