Beispiel #1
0
    // 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");
    }
Beispiel #2
0
    /*
     * 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);
    }
Beispiel #3
0
    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);
            }
        }
    }
Beispiel #4
0
    /**
     * 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 ;
         * }*/
    }