/// <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); }