// Use this for initialization
void Start()
{
map = new PolyMapLoader("T4Map/x", "T4Map/y", "T4Map/goalPosT4", "T4Map/startPosT4", "T4Map/Button");
int agentCounter = 0;
agents = new List <T4Agent2>();
for (int i = 0; i < map.polyData.start.Count; i = i + 1)
{
Vector3 startNode = map.polyData.start[i];
Vector3 endNode = map.polyData.end[i];
T4Agent2 newAgent = new T4Agent2("Agent " + agentCounter, startNode, endNode);
agents.Add(newAgent);
agentCounter++;
}
Debug.Log("Agents size:" + agents.Count);
/*
* //Test
* T4Agent2 temp = new T4Agent2 ("Test", new Vector3 (2, 1, 2), new Vector3(1,1,1));
* T4Agent2 temp2 = new T4Agent2 ("Test2", new Vector3 (0, 1, 0), new Vector3 (2, 1, 2));
* temp2.velocity = new Vector3 (1, 0, 1);
* List<T4Agent2> tempList = new List<T4Agent2> ();
* tempList.Add (temp);
* tempList.Add (temp2);
* float tempTime = temp2.calculateTimeToCollision (new Vector3 (1, 0, 1), tempList);
* Debug.Log ("TempTime:" + tempTime);*/
StartCoroutine("Move");
}
/*
* private bool isInReciprocalVO(Vector3 newVelocity, List<T4Agent2> agents){
*
* Vector3 velToCheck = 2 * newVelocity - this.velocity;
*
* for (int i=0; i<agents.Count; i++) {
* T4Agent2 curAgent=agents[i];
* if(!string.Equals(this.id,curAgent.id)){
* if(!this.checkOkVelocity(velToCheck,curAgent)){
* //if the velocity is not OK
* return true;
* }
* }
*
* }
*
* return false;
*
* }*/
public float calculateTimeToCollision(Vector3 newVelocity, List <T4Agent2> agents)
{
float minTimeToCollision = float.PositiveInfinity;
for (int i = 0; i < agents.Count; i++)
{
T4Agent2 curAgent = agents[i];
if (!string.Equals(this.id, curAgent.id))
{
float timeToCollision = this.findIntersectionPoint(newVelocity, curAgent);
if (timeToCollision < minTimeToCollision)
{
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;
Vector3[] moves = new Vector3[agents.Count];
for (int i = 0; i < agents.Count; i++)
{
moves[i] = new Vector3(0, 0, 0);
}
while (true)
{
//Check if all agents at goal
int agentsAtGoal = 0;
for (int i = 0; i < atGoal.Length; i++)
{
if (atGoal[i] == true)
{
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++)
{
/*
* if(atGoal[i]==true){
* continue;
* }*/
T4Agent2 curAgent = agents[i];
Vector3 current = curAgent.goalPos;
Vector3 dynPVel = curAgent.velocity;
float distance = Vector3.Distance(curAgent.agent.transform.position, current);
float curVel = Vector3.Magnitude(dynPVel);
if (distance <= goalInterval * curVel)
{
//Reached goal
atGoal[i] = true;
//continue;
}
//The code below is used for the 2nd solution of T4
Vector3 newVel = curAgent.findMinimumPenaltyVel(agents, accMax, current, goalInterval);
//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;
/*
* // BELOW IS USED FOR THE FIRST SOLUTION OF T4
*
* //Check collision
* Vector3 collisionVec=curAgent.collisionDetection(agents,accMax,current);
* //If the collisionVector is all zeros we just move along as before
* if(collisionVec.Equals(new Vector3(0,0,0))){
* //Debug.Log("CollVec zeros");
* Vector3 dir;
*
* dir = Vector3.Normalize (current - curAgent.agent.transform.position);
*
* //Debug.Log("dir="+dir);
* //Debug.Log("DynPVel="+dynPVel);
*
* Vector3 normVel = Vector3.Normalize (dynPVel);
*
* //The change is the difference between the direction and the velocity vector
* Vector3 change = dir;
* if(!normVel.Equals(dir)){
* change = Vector3.Normalize (dir - normVel);
* }
* if(change.Equals(new Vector3(0,0,0)))
* change=dir;
*
* //Debug.Log("change"+change);
*
* float accInX = change.x / (Mathf.Abs (change.x) + Mathf.Abs (change.z));
* float accInZ = change.z / (Mathf.Abs (change.x) + Mathf.Abs (change.z));
*
* if (float.IsNaN (accInX)) {
* accInX = 0;
* }
* if (float.IsNaN (accInZ)) {
* accInZ = 0;
* }
*
* float distanceToTarget=Vector3.Distance (current, curAgent.agent.transform.position);
*
* float neededDistToStop=(Mathf.Pow(dynPVel.magnitude,2)/2*accMax);
* //Accelerate
* if(distanceToTarget>neededDistToStop){
* dynPVel.x = dynPVel.x + accMax * accInX * Time.deltaTime;
* dynPVel.z = dynPVel.z + accMax * accInZ * Time.deltaTime;
* }
* //Decelerate
* else{
* //Debug.Log("Decelerate");
* dynPVel.x = dynPVel.x - accMax * accInX * Time.deltaTime;
* dynPVel.z = dynPVel.z - accMax * accInZ * Time.deltaTime;
* }
*
*
*
*
* //Debug.Log("DynPVel="+dynPVel);
*
* //Update the velocity vector
* curAgent.velocity=dynPVel;
*
* curAgent.agent.transform.position = curAgent.agent.transform.position + dynPVel;
*
* }
* //If the collision vector is not all zeros we should steer in that direction
* else{
* //Debug.Log("Collision course");
* //Debug.Log("CollVec:"+collisionVec);
* Vector3 dir;
*
* Vector3 goalDir=Vector3.Normalize (current - curAgent.agent.transform.position);
*
* float scaleColVec=0.7f;
*
* if(collisionVec.magnitude>1){
* collisionVec=Vector3.Normalize(collisionVec);
* }
*
* Vector3 scaledVec=new Vector3();
* scaledVec.x=scaleColVec*collisionVec.x+(1-scaleColVec)*goalDir.x;
* scaledVec.y=0;
* scaledVec.z=scaleColVec*collisionVec.z+(1-scaleColVec)*goalDir.z;
*
* //dir = Vector3.Normalize (collisionVec);
* dir=Vector3.Normalize(scaledVec);
*
* //Debug.Log("CollVec norm:"+dir);
*
* Vector3 normVel = Vector3.Normalize (dynPVel);
*
* //If we are on a collision course we just want to move the other way
* Vector3 change = dir;
*
*
* float accInX = change.x / (Mathf.Abs (change.x) + Mathf.Abs (change.z));
* float accInZ = change.z / (Mathf.Abs (change.x) + Mathf.Abs (change.z));
*
* if (float.IsNaN (accInX)) {
* accInX = 0;
* }
* if (float.IsNaN (accInZ)) {
* accInZ = 0;
* }
*
* float distanceToTarget=Vector3.Distance (current, curAgent.agent.transform.position);
*
* float neededDistToStop=(Mathf.Pow(dynPVel.magnitude,2)/2*accMax);
* //Accelerate
* if(distanceToTarget>neededDistToStop){
* dynPVel.x = dynPVel.x + accMax * accInX * Time.deltaTime;
* dynPVel.z = dynPVel.z + accMax * accInZ * Time.deltaTime;
* }
* //Decelerate
* else{
* //Debug.Log("Decelerate");
* dynPVel.x = dynPVel.x - accMax * accInX * Time.deltaTime;
* dynPVel.z = dynPVel.z - accMax * accInZ * Time.deltaTime;
* }
*
* //Update the velocity vector
* curAgent.velocity=dynPVel;
*
* curAgent.agent.transform.position = curAgent.agent.transform.position + dynPVel;
*
* }*/
yield return(null);
}
}
}
/**
* Function to check if the proposed velocity is OK, i.e. outside the vector obstacle
* given the other agent.
*
*/
/*private bool checkOkVelocity(Vector3 newVel, T4Agent2 otherAgent){
*
* Vector3 curPos = this.agent.transform.position;
* Vector3 otherPos = otherAgent.agent.transform.position;
* float radius = 2 * agentSize;
* Vector3 toCircleCenter = otherPos - curPos;
* Vector3 relativeVel = Vector3.Normalize(newVel - otherAgent.velocity);
* //Debug.Log ("Relative Vel:" + relativeVel);
* Vector3 velocityRay = curPos + timeStepLimit * relativeVel;
*
* //Debug.Log ("VelocityRay:" + velocityRay);
*
* Vector2 toCircle = new Vector2 (toCircleCenter.x, toCircleCenter.z);
* Vector2 velRay = new Vector2 (velocityRay.x, velocityRay.z);
*
* float angle = Vector2.Angle (velRay, toCircle);
* angle = angle * (Mathf.PI / 180.0f);
* float distance = Mathf.Abs(Mathf.Cos (angle) * toCircle.magnitude);
*
* //Vector3 projection = Vector3.Project (toCircleCenter, Vector3.Normalize(velocityRay));
* //Debug.Log ("Projection:" + projection);
*
* //float distance = Vector3.Distance (projection, otherPos);
* //Debug.Log ("Distance:" + distance);
* //If the distance is less than the radius the velocity is not ok
* if (distance < radius) {
* return false;
* }
*
* return true;
* }*/
private float findIntersectionPoint(Vector3 newVelocity, T4Agent2 otherAgent)
{
Vector3 velToCheck3 = 2 * newVelocity - this.velocity;
Vector2 velToCheck = new Vector2();
velToCheck.x = velToCheck3.x;
velToCheck.y = velToCheck3.z;
//Debug.Log ("VelTOCheck3:" + velToCheck3);
//Debug.Log("VelToCheck:"+velToCheck);
//Vector3 velToCheck = newVelocity ;
/*
* Vector3 curPos = this.agent.transform.position;
* Vector3 otherPos = otherAgent.agent.transform.position;
* float r = 2 * agentSize;
* Vector3 toCircleCenter = otherPos-curPos;
* Vector3 relativeVel = Vector3.Normalize(velToCheck - otherAgent.velocity);
* //Debug.Log ("Relative Vel:" + relativeVel);
* Vector3 velocityRay = (timeStepLimit * relativeVel) - curPos;*/
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();
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 ("Line cut circle");
return(timeToIntersect);
}
else
{
return(float.PositiveInfinity);
}
/*
* float a = Vector2.Dot(velocityRay, velocityRay ) ;
* float b = 2*Vector2.Dot(toCircleCenter, velocityRay ) ;
* float c = Vector2.Dot(toCircleCenter,toCircleCenter ) - r*r ;
*
* Debug.Log ("a:" + a);
* Debug.Log ("b:" + b);
* Debug.Log ("c:" + c);
*
* float discriminant = b*b-4*a*c;
* Debug.Log ("Discriminant:" + discriminant);
* if( discriminant < 0 )
* {
* //Debug.Log("RayTotalyMissed");
* return float.PositiveInfinity;
* }
* else
* {
* // ray didn't totally miss sphere,
* // so there is a solution to
* // the equation.
*
* discriminant = Mathf.Sqrt( discriminant );
*
* // either solution may be on or off the ray so need to test both
* // t1 is always the smaller value, because BOTH discriminant and
* // a are nonnegative.
* float t1 = (-b - discriminant)/(2*a);
* float t2 = (-b + discriminant)/(2*a);
*
* Debug.Log("t1:"+t1);
* Debug.Log ("t2:"+t2);
*
*
* if( t1 >= 0 && t1 <= 1 )
* {
* // t1 is the intersection, and it's closer than t2
* // (since t1 uses -b - discriminant)
* // Impale, Poke
*
* //Since the ray is relativeVel*timeStepLimit long
* float time=t1*timeStepLimit;
*
* //Debug.Log("Time to collision found:"+t1);
*
* return time ;
* }
*
* if( t2 >= 0 && t2 <= 1 )
* {
* // t1 is the intersection, and it's closer than t2
* // (since t1 uses -b - discriminant)
* // Impale, Poke
*
* //Since the ray is relativeVel*timeStepLimit long
* float time=t2*timeStepLimit;
*
* //Debug.Log("Time to collision found:"+t1);
*
* return time ;
* }
*
* // no intn: FallShort, Past, CompletelyInside
* return float.PositiveInfinity ;
* }*/
}
