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);
}
// Use this for initialization
void Start()
{
Application.runInBackground = true;
map = new PolyMapLoader("polygMap1/x", "polygMap1/y", "polygMap1/goalPos", "polygMap1/startPos",
"polygMap1/button");
polyData = map.polyData;
graph = new VGraph();
walkableLines = new List <Line> ();
//Create visibility graph
CreateObstacles();
ConstructWalkableLines();
CreateInterObstacleWalk();
agents = new List <T5Agent>();
agentColors = new List <Color>();
agentColors.Add(Color.black);
agentColors.Add(Color.blue);
agentColors.Add(Color.yellow);
agentColors.Add(Color.cyan);
agentColors.Add(Color.green);
paths = new List <Vector3> [map.polyData.start.Count];
agentAtWaypoint = new int[map.polyData.start.Count];
int agentCounter = 0;
for (int i = 0; i < map.polyData.start.Count; i = i + 1)
{
Vector3 startNode = map.polyData.start[i];
Vector3 endNode = map.polyData.end[i];
T5Agent newAgent = new T5Agent("Agent " + agentCounter, startNode, endNode, agentCounter, true);
newAgent.agent.gameObject.renderer.material.color = agentColors[i];
List <PolyNode> ppath = pathFinder.AStarSearch(startNode, endNode, graph);
List <Vector3> temp = new List <Vector3> ();
foreach (PolyNode p in ppath)
{
temp.Add(p.pos);
}
//temp.Add(endNode);
paths[i] = temp;
agentAtWaypoint[i] = 0;
agents.Add(newAgent);
agentCounter++;
}
Debug.Log("Agents size:" + agents.Count);
StartCoroutine("Move");
}
/**
* 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);
}
}
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);
}
}
}
