/** * Function to calculate the number of timesteps between the * current agent and some other agent */ private float timeStepsBetween(T4Agent otherAgent, float acceleration) { Vector3 thisAgentPos = this.agent.transform.position; Vector3 otherAgentPos = otherAgent.agent.transform.position; float distance = Vector3.Distance(thisAgentPos, otherAgentPos); //subtract 2*agentSize from the distance to account for the size of the agents distance = distance - 2 * agentSize; if (distance <= 0) { Debug.LogError("Two agents collided"); distance = 0; } //Calculate the summed speed for both agents in the direction directly towards eachother float speed = acceleration; float angle = Vector3.Angle(otherAgentPos - thisAgentPos, this.velocity); //Transform the angle to radians angle = angle * (Mathf.PI / 180.0f); speed = speed + this.velocity.magnitude * Mathf.Cos(angle); //And now for the other agents speed angle = Vector3.Angle(thisAgentPos - otherAgentPos, otherAgent.velocity); angle = angle * (Mathf.PI / 180.0f); speed = speed + otherAgent.velocity.magnitude * Mathf.Cos(angle); float timeSteps = (distance / (speed * Time.deltaTime)); return(timeSteps); }
/* * Function to check if there is a collision risk. If it is * the function will return a direction vector that the agent should use * in the next timestep. */ public Vector3 collisionDetection(List <T4Agent> agents, float acceleration, Vector3 goalPos) { //If we have the last angles between the agent and the other agents if (lastAngles != null) { Vector3 dirVector = new Vector3(0, 0, 0); float closests = float.PositiveInfinity; Vector3 closestVec = new Vector3(); for (int i = 0; i < agents.Count; i++) { T4Agent curAgent = agents[i]; Vector3 thisAgentPos = this.agent.transform.position; Vector3 otherAgentPos = curAgent.agent.transform.position; //If its not the same agent if (!string.Equals(curAgent.id, this.id)) { float timeSteps = this.timeStepsBetween(curAgent, acceleration); if (timeSteps <= timeStepLimit) { float angle = Vector3.Angle(otherAgentPos - thisAgentPos, Vector3.right); float lastAngle = lastAngles[i]; //If the difference between lastAngle and the current angle is small enough if (lastAngle - angle < angleEqLimit) { float distToOther = Vector3.Distance(thisAgentPos, otherAgentPos); float distToTarget = Vector3.Distance(thisAgentPos, goalPos); if (distToTarget > distToOther) { Vector3 temp = otherAgentPos - thisAgentPos; if (timeSteps < closests) { closests = timeSteps; closestVec = temp; } } //dirVector=dirVector+new Vector3((-1f/Mathf.Pow(timeSteps,2))*temp.x,0,(-1f/Mathf.Pow(timeSteps,2))*temp.z); } lastAngles[i] = angle; } } } dirVector = new Vector3(-0.5f * closestVec.x, 0, -closestVec.z); return(dirVector); } //If we dont have any previous angles we just calculate the current and save them else { lastAngles = new List <float>(); for (int i = 0; i < agents.Count; i++) { T4Agent curAgent = agents[i]; //If it's not the same agent if (!string.Equals(curAgent.id, this.id)) { float angle = Vector3.Angle(curAgent.agent.transform.position - this.agent.transform.position, Vector3.right); lastAngles.Add(angle); } else { //We have to add something to the list of angles to keep the same order lastAngles.Add(0.0f); } } return(new Vector3(0, 0, 0)); } }