/**
* Check if the agent will get to the goal before it collides
*/
private bool goalBeforeCollision(T5Agent otherAgent, Vector3 waypoint)
{
float distToGoal = Vector3.Distance(this.agent.transform.position, waypoint);
float distToOther = Vector3.Distance(this.agent.transform.position, otherAgent.agent.transform.position);
Vector3 otherAgentVel = new Vector3(0, 0, 0);
if (otherAgent.isCar && otherAgent.velSize != 0f)
{
otherAgentVel = otherAgent.getCarVelocity();
}
else
{
otherAgentVel = otherAgent.velocity;
}
if (Vector3.Equals(otherAgentVel, Vector3.zero) && distToGoal < distToOther)
{
//Debug.Log("GoalBeforeCol true");
return(true);
}
return(false);
}
private float findIntersectionPoint(Vector3 newVelocity, T5Agent otherAgent)
{
Vector3 velToCheck3 = new Vector3(0, 0, 0);
if (this.isCar)
{
velToCheck3 = 2 * newVelocity - this.getCarVelocity();
}
else
{
velToCheck3 = 2 * newVelocity - this.velocity;
}
Vector2 velToCheck = new Vector2();
velToCheck.x = velToCheck3.x;
velToCheck.y = velToCheck3.z;
Vector2 curPos = new Vector2();
curPos.x = this.agent.transform.position.x;
curPos.y = this.agent.transform.position.z;
Vector2 otherPos = new Vector2();
otherPos.x = otherAgent.agent.transform.position.x;
otherPos.y = otherAgent.agent.transform.position.z;
Vector2 toCircleCenter = otherPos - curPos;
Vector2 otherAgentVel = new Vector2();
if (otherAgent.isCar)
{
Vector3 otherAgentVel3 = otherAgent.getCarVelocity();
otherAgentVel.x = otherAgentVel3.x;
otherAgentVel.y = otherAgentVel3.z;
}
else
{
otherAgentVel.x = otherAgent.velocity.x;
otherAgentVel.y = otherAgent.velocity.z;
}
Vector2 relativeVel = velToCheck - otherAgentVel;
Vector2 velocityRay = (timeStepLimit * relativeVel) - curPos;
float r = 2 * agentSize;
//Debug.Log ("To circle center:" + toCircleCenter);
//Debug.Log ("relativeVel:" + relativeVel);
//Debug.Log ("velocityRay:" + velocityRay);
float angle = Vector2.Angle(velocityRay, toCircleCenter);
if (angle < 0.1f)
{
angle = 0;
}
angle = angle * (Mathf.PI / 180.0f);
//Debug.Log ("Angle:" + angle);
float distance = Mathf.Abs(Mathf.Sin(angle) * toCircleCenter.magnitude);
//Debug.Log ("Distance:" + distance);
//If the distance is less than the radius the velocity is not ok
if (distance <= r)
{
float distAlongRay = Mathf.Abs(Mathf.Cos(angle) * toCircleCenter.magnitude);
float distInside = Mathf.Pow(r, 2) - Mathf.Pow(distance, 2);
float distToIntersect = distAlongRay - distInside;
float timeToIntersect = distToIntersect / relativeVel.magnitude;
//Debug.Log("Relative Vel magnitude:"+relativeVel.magnitude);
//Debug.Log ("Line cut circle");
return(timeToIntersect);
}
else
{
return(float.PositiveInfinity);
}
}