/// <summary>
/// Physics Update
/// </summary>
void FixedUpdate()
{
foreach (GameObject agent in agents)
{
if (agent != null)
{
AgentPredictiveAvoidanceModel agentOther = agent.GetComponent <AgentPredictiveAvoidanceModel> ();
Rigidbody agentRigidBody = agent.GetComponent <Rigidbody> ();
Transform agentTransform = agent.GetComponent <Transform> ();
float idealWallDistance = agentOther.agentRadius + wallDistance;
float safe = idealWallDistance * idealWallDistance;
wall_normal = calc_wall_normal(agentTransform, box1);
Pair <Vector3, Vector3> line_p = calcWallPointsFromNormal(box1, wall_normal);
Vector3 n_w = agentTransform.position - closestPointLineSegment(line_p.First, line_p.Second, agentTransform.position);
//Debug.DrawLine (line_p.First, line_p.Second, Color.blue, 0.02f);
//Debug.DrawLine (new Vector3(5, 0, box1.zmin), new Vector3(5, 0, box1.zmax), Color.red, 0.02f);
float d_w = n_w.sqrMagnitude;
if (d_w < safe)
{
d_w = Mathf.Sqrt(d_w);
if (d_w > 0)
{
n_w /= d_w;
}
float distanceMinimumRadius = (d_w - agentOther.agentRadius) < 0.001f ? 0.001f : d_w - agentOther.agentRadius;
//Debug.Log("idealWallDistance: " + idealWallDistance);
//Debug.Log("d_w: " + d_w);
//Debug.Log("distanceMinimumRadius: " + distanceMinimumRadius);
//Debug.Log("n_w.magnitude: " + n_w.magnitude);
//Debug.Log("Final Magnitude: " + (idealWallDistance - d_w) / Mathf.Pow(distanceMinimumRadius, wallSteepness));
Vector3 drivingForce = (idealWallDistance - d_w) / Mathf.Pow(distanceMinimumRadius, wallSteepness) * n_w;
//Debug.DrawLine(agentTransform.position, agentTransform.position + drivingForce, Color.blue, 0.02f);
agentRigidBody.AddForce(drivingForce, ForceMode.Force);
// MELISSA check HERE
DistractedAgent distractionScript = agent.GetComponent <DistractedAgent>();
if (distractionScript != null)
{
distractionScript.PayAttention();
}
}
}
}
}
/// <summary>
/// Physics update
/// </summary>
void FixedUpdate()
{
List <KeyValuePair <float, GameObject> > t_pairs = new List <KeyValuePair <float, GameObject> >();
bool collision_ = false;
int count = 0;
Vector3 preferredVelocity = agentSelf.GetCurrentGoal() - transform.position;
float goalDistance = preferredVelocity.sqrMagnitude;
preferredVelocity *= preferredSpeed / Mathf.Sqrt(goalDistance);
// Goal Driven Force (Always added)
Vector3 goalForce = (preferredVelocity - rb.velocity) / ksi;
rb.AddForce(goalForce, ForceMode.Force);
Vector3 drivingForce = Vector3.zero;
Vector3 idealDrivingForce = (preferredVelocity - rb.velocity) / ksi;
Vector3 desiredVelocity = rb.velocity + idealDrivingForce * Time.fixedDeltaTime; ///0.02 is the fixed update physics timestep
float desiredSpeed = desiredVelocity.magnitude;
for (int i = 0; i < neighbor_agents.Length; ++i)
{
GameObject agent = neighbor_agents [i];
if (agent != null)
{
if ((agent.transform.position - transform.position).magnitude < neighborDist)
{
AgentPredictiveAvoidanceModel otherAgent = agent.GetComponent <AgentPredictiveAvoidanceModel> ();
Rigidbody otherAgentRB = agent.GetComponent <Rigidbody> ();
Transform otherAgentTransform = agent.GetComponent <Transform> ();
// ignore own tag and far distance agent
if (this.GetInstanceID() == agent.GetInstanceID())
{
continue;
}
float combinedRadius = agentPersonalSpace + otherAgent.agentRadius;
Vector3 w = otherAgentTransform.position - transform.position;
if (w.sqrMagnitude < combinedRadius * combinedRadius)
{
collision_ = true;
t_pairs.Add(new KeyValuePair <float, GameObject> (0.0f, agent));
}
else
{
Vector3 relDir = w.normalized;
if (Vector3.Dot(relDir, rb.velocity.normalized) < _cosFov)
{
continue;
}
float tc = rayIntersectsDisc(transform.position, otherAgentTransform.position, desiredVelocity - otherAgentRB.velocity, combinedRadius);
if (tc < timeHorizon)
{
if (t_pairs.Count < maxNeighbors)
{
t_pairs.Add(new KeyValuePair <float, GameObject> (tc, agent));
}
else if (tc < t_pairs.ToArray() [0].Key)
{
t_pairs.RemoveAt(t_pairs.Count - 1);
t_pairs.Add(new KeyValuePair <float, GameObject> (tc, agent));
} //What to do if max neighbours is reached THIS NEED TO BE IMPLEMENTED
}
}
}
}
}
//Debug.Log ("Adding Collision Force");
for (int i = 0; i < t_pairs.Count; ++i)
{
float t_ = t_pairs[i].Key;
GameObject agent = t_pairs[i].Value;
Vector3 forceDirection = transform.position + desiredVelocity * t_ - agent.transform.position - agent.GetComponent <Rigidbody>().velocity *t_;
float forceDistance = forceDirection.magnitude;
if (forceDistance > 0f)
{
forceDirection /= forceDistance;
}
float collisionDistance = Mathf.Max(forceDistance - agentRadius - agent.GetComponent <AgentPredictiveAvoidanceModel>().agentRadius, .0f); // distance between their cylindrical bodies at the time of collision
float D = Mathf.Max(desiredSpeed * t_ + collisionDistance, 0.001f); // D = input to evasive force magnitude piecewise function
float forceMagnitude;
if (D < dmin)
{
forceMagnitude = agentStrength * dmin / D;
}
else if (D < dmid)
{
forceMagnitude = agentStrength;
}
else if (D < dmax)
{
forceMagnitude = agentStrength * (dmax - D) / (dmax - dmid);
}
else
{
forceMagnitude = 0f;
continue;
}
forceMagnitude *= Mathf.Pow((collision_ ? 1.0f : wFactor), count++);
drivingForce += forceMagnitude * forceDirection;
}
// Add noise for reducing deadlocks adding naturalness
if (noise)
{
float angle = Random.value * 2.0f * Mathf.PI;
float dist = Random.value * 0.001f;
drivingForce += dist * new Vector3(Mathf.Cos(angle), Mathf.Sin(angle));
}
//Clamp the force to within a reasonable range
drivingForce = Vector3.ClampMagnitude(drivingForce, 40.0f);
rb.AddForce(drivingForce, ForceMode.Force);
}
