public float calculateTimeToCollision(Vector3 newVelocity, List <T5Agent> agents, float goalInterval, Vector3 waypoint)
{
float minTimeToCollision = float.PositiveInfinity;
for (int i = 0; i < agents.Count; i++)
{
T5Agent curAgent = agents [i];
if (!string.Equals(this.id, curAgent.id) && !this.goalBeforeCollision(curAgent, waypoint))
{
if (!curAgent.isAtGoal(goalInterval))
{
if (this.priority <= curAgent.priority)
{
float timeToCollision = this.findIntersectionPoint(newVelocity, curAgent);
if (timeToCollision < minTimeToCollision && timeToCollision < neighborTimeLimit)
{
minTimeToCollision = timeToCollision;
}
}
}
else
{
float timeToCollision = this.findIntersectionPoint(newVelocity, curAgent);
if (timeToCollision < minTimeToCollision && timeToCollision < neighborTimeLimit)
{
minTimeToCollision = timeToCollision;
}
}
}
}
return(minTimeToCollision);
}
IEnumerator Move()
{
bool[] atGoal = new bool[agents.Count];
for (int i = 0; i < atGoal.Length; i++)
{
atGoal [i] = false;
}
float timeBefore = Time.time;
while (true)
{
//Check if all agents at goal
int agentsAtGoal = 0;
for (int i = 0; i < agents.Count; i++)
{
if (agents[i].isAtGoal(goalInterval))
{
agentsAtGoal++;
}
}
if (agentsAtGoal == agents.Count)
{
Debug.Log("Done");
float timeAfter = Time.time;
Debug.Log("Time:" + (timeAfter - timeBefore));
yield break;
}
//Iterate all agents
for (int i = 0; i < agents.Count; i++)
{
List <Vector3> curPath = paths[i];
int curAgentAtWaypoint = agentAtWaypoint[i];
T5Agent curAgent = agents[i];
Vector3 current = curPath[curAgentAtWaypoint];
//If the current agent is at it's goal it should not move anymore
if (curAgent.isAtGoal(goalInterval))
{
// print (agents[i].id + " done ");
curAgent.velocity = Vector3.zero;
continue;
}
if (Vector3.Distance(curAgent.agent.transform.position, current) < goalInterval)
{
curAgentAtWaypoint++;
agentAtWaypoint[i] = curAgentAtWaypoint;
if (curAgentAtWaypoint >= curPath.Count)
{
curAgent.velocity = Vector3.zero;
continue;
}
current = curPath[curAgentAtWaypoint];
}
bool straightToGoal = curAgent.checkStraightWayToGoal(obstacles);
if (straightToGoal)
{
current = curAgent.goalPos;
curAgentAtWaypoint = curPath.Count - 1;
agentAtWaypoint[i] = curAgentAtWaypoint;
}
bool stuck = curAgent.checkStuck(obstacles, current);
//stuck = false;
newPath:
if (stuck)
{
Vector3 pos = curAgent.agent.transform.position;
Vector3 towardsWaypoint = Vector3.Normalize(current - pos);
Vector3 edgeOfAgent = pos + towardsWaypoint * curAgent.agentSize;
this.addPointToGraph(edgeOfAgent);
List <PolyNode> ppath = pathFinder.AStarSearch(edgeOfAgent,
curAgent.goalPos, graph);
List <Vector3> temp = new List <Vector3> ();
foreach (PolyNode p in ppath)
{
temp.Add(p.pos);
}
curPath = temp;
paths[i] = curPath;
curAgentAtWaypoint = 0;
agentAtWaypoint[i] = 0;
current = curPath[curAgentAtWaypoint];
print(curAgent.id + " ASTAR");
}
if (curAgent.id == "Agent 1")
{
blackGoal = current;
}
//Vector3 current=curAgent.goalPos;
Vector3 dynPVel = curAgent.velocity;
//The code below is used for the 2nd solution of T4
curAgent.findMinimumPenaltyVelCar(ref agents, accMax, current, goalInterval, obstacles, maxWheelAngle
, carLength);
Vector3 newRot = curAgent.velocity;
// Vector3 newVel=curAgent.getCarVelocity();
float curVel = curAgent.velSize;
// print (curAgent.id + " : " + curAgent.getCarVelocity().magnitude + "\n"
// + "curVel: " + curVel + " newVel " + newVel);
bool care = false;
bool flag = true;
foreach (T5Agent agent in agents)
{
if (curAgent != agent)
{
if (Vector3.Distance(curAgent.agent.transform.position, agent.agent.transform.position) < 33)
{
care = true;
break;
}
}
}
if (!care)
{
print(curAgent.id + " I DON'T CARE !!");
if (curVel == 0)
{
curVel = 1;
}
}
else
{
print(curAgent.id + " I CARE !!");
}
Vector3 curPos = curAgent.agent.transform.position;
// Vector3 moveTowards=curPos+newVel;
// float step=newVel.magnitude*Time.deltaTime;
//Debug.Log("Step:"+step);
float wheelAngleRad = maxWheelAngle * (Mathf.PI / 180);
float dTheta = (curAgent.velSize / carLength) * Mathf.Tan(wheelAngleRad);
Quaternion newLookRot;
if (care)
{
newLookRot = Quaternion.Euler(newRot);
}
else
{
newLookRot = Quaternion.LookRotation(current - curAgent.agent.transform.position);
}
if (curAgent.agent.transform.rotation != newLookRot)
{
curAgent.agent.transform.rotation = Quaternion.RotateTowards(curAgent.agent.transform.rotation, newLookRot, dTheta);
}
curAgent.velocity = curAgent.agent.transform.rotation.eulerAngles;
Vector3 curDir = curAgent.agent.transform.eulerAngles;
Vector3 newPos = curAgent.agent.transform.position;
float angleRad = curDir.y * (Mathf.PI / 180);
newPos.x = newPos.x + (curVel * Mathf.Sin(angleRad) * Time.deltaTime);
newPos.z = newPos.z + (curVel * Mathf.Cos(angleRad) * Time.deltaTime);
curAgent.agent.transform.position = newPos;
yield return(null);
}
}
}
IEnumerator Move()
{
bool[] atGoal = new bool[agents.Count];
for (int i = 0; i < atGoal.Length; i++)
{
atGoal [i] = false;
}
float timeBefore = Time.time;
while (true)
{
//Check if all agents at goal
int agentsAtGoal = 0;
for (int i = 0; i < agents.Count; i++)
{
if (agents[i].isAtGoal(goalInterval))
{
agentsAtGoal++;
}
}
if (agentsAtGoal == agents.Count)
{
Debug.Log("Done");
float timeAfter = Time.time;
Debug.Log("Time:" + (timeAfter - timeBefore));
yield break;
}
//Iterate all agents
for (int i = 0; i < agents.Count; i++)
{
List <Vector3> curPath = paths[i];
int curAgentAtWaypoint = agentAtWaypoint[i];
T5Agent curAgent = agents[i];
Vector3 current = curPath[curAgentAtWaypoint];
//If the current agent is at it's goal it should not move anymore
if (curAgent.isAtGoal(goalInterval))
{
curAgent.velocity = Vector3.zero;
continue;
}
if (Vector3.Distance(curAgent.agent.transform.position, current) < goalInterval)
{
curAgentAtWaypoint++;
agentAtWaypoint[i] = curAgentAtWaypoint;
if (curAgentAtWaypoint >= curPath.Count)
{
curAgent.velocity = Vector3.zero;
continue;
}
current = curPath[curAgentAtWaypoint];
}
bool straightToGoal = curAgent.checkStraightWayToGoal(obstacles);
if (straightToGoal)
{
current = curAgent.goalPos;
curAgentAtWaypoint = curPath.Count - 1;
agentAtWaypoint[i] = curAgentAtWaypoint;
}
bool stuck = curAgent.checkStuck(obstacles, current);
if (stuck)
{
Vector3 pos = curAgent.agent.transform.position;
Vector3 towardsWaypoint = Vector3.Normalize(current - pos);
Vector3 edgeOfAgent = pos + towardsWaypoint * curAgent.agentSize;
this.addPointToGraph(edgeOfAgent);
List <PolyNode> ppath = pathFinder.AStarSearch(edgeOfAgent,
curAgent.goalPos, graph);
List <Vector3> temp = new List <Vector3> ();
foreach (PolyNode p in ppath)
{
temp.Add(p.pos);
}
curPath = temp;
paths[i] = curPath;
curAgentAtWaypoint = 0;
agentAtWaypoint[i] = 0;
current = curPath[curAgentAtWaypoint];
}
//Vector3 current=curAgent.goalPos;
Vector3 dynPVel = curAgent.velocity;
//The code below is used for the 2nd solution of T4
Vector3 newVel = curAgent.findMinimumPenaltyVel(agents, accMax, current, goalInterval, obstacles);
//Update the velocity vector
curAgent.velocity = newVel;
Vector3 curPos = curAgent.agent.transform.position;
Vector3 moveTowards = curPos + newVel;
float step = newVel.magnitude * Time.deltaTime;
//Debug.Log("Step:"+step);
curAgent.agent.transform.position = Vector3.MoveTowards(curPos, moveTowards, step);
//curAgent.agent.transform.position = curAgent.agent.transform.position + newVel*Time.deltaTime;
yield return(null);
}
}
}