/// <summary>
/// Computes the forces exerted on the agent at each simulation step
/// Our simulation model includes the following three forces:
/// A driving force F_i indicating the preference of the agent i
/// to walk in a certain direction at a certain speed, as defined in
/// [D. Helbing, I. Farkas, and T. Vicsek, Nature 407, 487 (2000)],
///
/// F_i = (V_iPref - V_i) / Epsilon
///
/// This force can be replaced by a self-propelled force to simulate agents with
/// no preferred direction of motion following the approach of[D.Grossman,
/// I.S.Aranson, and E.B.Jacob, New J.Phys. 10, 023036(2008).]
///
/// The agent-agent interaction force F_ij derived in Eq. (S2)
/// of the Supplemental material. A similar force F_iO acting
/// on agent i as a result of interaction with each static obstacle O
/// present in the environment.
///
/// In our simulations, we generally assume that
/// obstacles are modeled as a collection of line segments. Then,
///
/// F_iO = -Delta_r * (K*T^(-2)*e^(-T/T_0))
///
/// but now T denotes the minimal intersection time between the ray
/// x_i + t*v_i, t>0 and the 2D capsule resulting after
/// sweeping O with the disc of the agent.
/// </summary>
protected void ComputeForces()
{
//driving force
F = (vPref - velocity) / ksi;
// Compute new neighbors of agent;
proximityNeighbors.Clear();
proximityToken.FindNeighbors(ExtensionMethods.Vector2ToVector3(position), neighborDist, ref proximityNeighbors);
// compute the anticipatory force from each neighbor
for (int i = 0; i < proximityNeighbors.Count; i++)
{
Agent other = (Agent)proximityNeighbors[i];
if (id == other.id)
{
continue;
}
float distanceSq = (other.position - position).sqrMagnitude;
float radiusSq = Mathf.Sqrt(other.radius + radius);
if (System.Math.Abs(distanceSq - radiusSq) > EPSILON)
{
// if agents are actually colliding use their separation distance
if (distanceSq < radiusSq)
{
radiusSq = Mathf.Sqrt(other.radius + radius - Mathf.Sqrt(distanceSq));
}
Vector2 w = other.position - position;
Vector2 v = velocity - other.velocity;
float a = Vector2.Dot(v, v);
float b = Vector2.Dot(w, v);
float c = Vector2.Dot(w, w) - radiusSq;
float discr = b * b - a * c;
if (discr > 0.0f && (a < -EPSILON || a > EPSILON))
{
discr = Mathf.Sqrt(discr);
float t = (b - discr) / a;
if (t > 0)
{
F += -k *Mathf.Exp(-t / t0) * (v - (b * v - a * w) / discr) / (a * Mathf.Pow(t, m)) * (m / t + 1 / t0);
}
}
}
}
//anticipatory forces from static obstacles
for (int i = 0; i < Engine.Instance.GetObstacles.Count; ++i)
{
LineObstacle obstacle = Engine.Instance.GetObstacle(i);
Vector2 n_w = Global.ClosestPointLineSegment(obstacle.P1, obstacle.P2, position) - position;
float d_w = n_w.sqrMagnitude;
// Agent is moving away from obstacle, already colliding or obstacle too far away
if (Vector2.Dot(velocity, n_w) < 0 || System.Math.Abs(d_w - (Mathf.Sqrt(radius))) < EPSILON || d_w > Mathf.Sqrt(neighborDist))
{
continue;
}
// correct the radius, if the agent is already colliding
float r = d_w < Mathf.Sqrt(radius) ? Mathf.Sqrt(d_w) : radius;
float a = Vector2.Dot(velocity, velocity);
bool discCollision = false, segmentCollision = false;
float t_min = Mathf.Infinity;
float b = 1, discr = 1;
float b_temp, discr_temp, c_temp, D_temp;
Vector2 w_temp, w = Vector2.zero, o1_temp, o2_temp, o_temp, o = Vector2.zero;
// time-to-collision with disc_1 of the capped rectangle (capsule)
w_temp = obstacle.P1 - position;
b_temp = Vector2.Dot(w_temp, velocity);
c_temp = Vector2.Dot(w_temp, w_temp) - (r * r);
discr_temp = b_temp * b_temp - a * c_temp;
if (discr_temp > .0f && (a < -EPSILON || a > EPSILON))
{
discr_temp = Mathf.Sqrt(discr_temp);
float t = (b_temp - discr_temp) / a;
if (t > 0)
{
t_min = t;
b = b_temp;
discr = discr_temp;
w = w_temp;
discCollision = true;
}
}
// time-to-collision with disc_2 of the capsule
w_temp = obstacle.P2 - position;
b_temp = Vector2.Dot(w_temp, velocity);
c_temp = Vector2.Dot(w_temp, w_temp) - (r * r);
discr_temp = b_temp * b_temp - a * c_temp;
if (discr_temp > 0 && (a < -EPSILON || a > EPSILON))
{
discr_temp = Mathf.Sqrt(discr_temp);
float t = (b_temp - discr_temp) / a;
if (t > 0 && t < t_min)
{
t_min = t;
b = b_temp;
discr = discr_temp;
w = w_temp;
discCollision = true;
}
}
// time-to-collision with segment_1 of the capsule
o1_temp = obstacle.P1 + r * obstacle.Normal;
o2_temp = obstacle.P2 + r * obstacle.Normal;
o_temp = o2_temp - o1_temp;
D_temp = Global.Det(velocity, o_temp);
if (System.Math.Abs(D_temp) > EPSILON)
{
float inverseDet = 1.0f / D_temp;
float t = Global.Det(o_temp, position - o1_temp) * inverseDet;
float s = Global.Det(velocity, position - o1_temp) * inverseDet;
if (t > 0 && s >= 0 && s <= 1 && t < t_min)
{
t_min = t;
o = o_temp;
discCollision = false;
segmentCollision = true;
}
}
// time-to-collision with segment_2 of the capsule
o1_temp = obstacle.P1 - r * obstacle.Normal;
o2_temp = obstacle.P2 - r * obstacle.Normal;
o_temp = o2_temp - o1_temp;
D_temp = Global.Det(velocity, o_temp);
if (System.Math.Abs(D_temp) > EPSILON)
{
float inverseDet = 1.0f / D_temp;
float t = Global.Det(o_temp, position - o1_temp) * inverseDet;
float s = Global.Det(velocity, position - o1_temp) * inverseDet;
if (t > 0 && s >= 0 && s <= 1 && t < t_min)
{
t_min = t;
o = o_temp;
discCollision = false;
segmentCollision = true;
}
}
if (discCollision)
{
F += -k *Mathf.Exp(-t_min / t0) * (velocity - (b * velocity - a * w) / discr) / (a * Mathf.Pow(t_min, m)) * (m / t_min + 1 / t0);
}
else if (segmentCollision)
{
F += k * Mathf.Exp(-t_min / t0) / (Mathf.Pow(t_min, m) * Global.Det(velocity, o)) * (m / t_min + 1 / t0) * new Vector2(-o.y, o.x);
}
}
}
/// <summary>
/// Add a new line obstacle to the simulation.
/// </summary>
/// <param name="start">The start point of the line segment</param>
/// <param name="end">The end point of the line segment</param>
public void AddObstacle(Vector2 start, Vector2 end)
{
LineObstacle l = new LineObstacle(start, end);
obstacles.Add(l);
}
