Komak57

Pathfinding Tree

May 4th, 2013
117
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C# 8.57 KB | None | 0 0
  1. using UnityEngine;
  2. using System.Threading;
  3. using System.Collections;
  4. using System.Collections.Generic;
  5.  
  6. public class AI : MonoBehaviour {
  7.     const byte _TOP = 0x1;
  8.     const byte _BOT = 0x2;
  9.     const byte _LEFT = 0x4;
  10.     const byte _RIGHT = 0x8;
  11.     const byte _STEP = 0x16;
  12.     // list of coordinates in current path
  13.     public List<Vector3> path;// = new ArrayList<Vector3>();
  14.    
  15.     public float offset = -14.546f;
  16.     public float Distancefrom = 0;
  17.     public Vector3 nextTarget = Vector3.zero;
  18.    
  19.     public float Speed = 50;
  20.     public GameObject Cube;
  21.     float vertVelocity = 0;
  22.    
  23.     public bool hastarget = false;
  24.     public int StepLimit = 10;
  25.     public float StepInc = 1;
  26.    
  27.     GameObject plyr;
  28.     GameObject trgt;
  29.     Vector3 lastseen;
  30.     CharacterController me;
  31.     Animator anim;
  32.     // Use this for initialization
  33.     void Start () {
  34.         //if (!plyr)
  35.         me = GetComponent<CharacterController>();
  36.         anim = GetComponent<Animator>();
  37.         plyr = GameObject.Find("Player");
  38.         if (!plyr)
  39.             Debug.Log("Player not found!");
  40.         trgt = GameObject.Find("AITarget");
  41.         //CreatePath();
  42.         lastseen= transform.position;
  43.         path = new List<Vector3>();
  44.         CreatePath();
  45.         hastarget = false;
  46.     }
  47.    
  48.     // Update is called once per frame
  49.     bool hadtarget = false;
  50.     void Update () {
  51.         //if (!hastarget)
  52.         hastarget = Check(plyr);
  53.         if (hastarget != hadtarget) {
  54.             if (hastarget) {
  55.                 path.Add(plyr.transform.position);
  56.                 Debug.Log("Player is in view!");
  57.             } else {
  58.                 lastseen = plyr.transform.position;
  59.                 Debug.Log("Player was last seen "+lastseen);
  60.             }
  61.             hadtarget = hastarget;
  62.         } else {
  63.             if (hastarget) {
  64.                 // ignore pathing and go straight for the player
  65.                 //path.Clear();
  66.                 //path.Add(new Vector3(plyr.transform.position.x, plyr.transform.position.y-14.546f,plyr.transform.position.z));
  67.             }
  68.         }
  69.         vertVelocity += (Physics.gravity.y/2) * Time.deltaTime;
  70.        
  71.         if (path.Count > 0) {
  72.             //Vector3 dir = (path[0] - transform.position);
  73.            
  74.             nextTarget = path[0];
  75.             trgt.transform.position = new Vector3(path[0].x, trgt.transform.position.y, path[0].z);
  76.             Distancefrom = (new Vector3(transform.position.x, 0, transform.position.z) - new Vector3(path[0].x, 0, path[0].z)).magnitude;
  77.            
  78.             // Have we reached target? Is path blocked?
  79.             if (Distancefrom < 25 || !Check(new Vector3(path[0].x, path[0].y+offset, path[0].z))) {
  80.                 path.Remove(path[0]);
  81.                 anim.SetFloat("Speed", 0);
  82.             }
  83.             if (path.Count == 0)
  84.                 return;
  85.            
  86.            
  87.             Vector3 dest = Vector3.MoveTowards(transform.position, new Vector3(path[0].x, path[0].y+offset, path[0].z), Speed*Time.deltaTime)-transform.position;
  88.            
  89.             vertVelocity += Physics.gravity.y;
  90.             dest.y += vertVelocity;
  91.             transform.rotation = Quaternion.Lerp(transform.rotation, Quaternion.LookRotation(new Vector3(path[0].x-transform.position.x, 0, path[0].z-transform.position.z)), 5*Time.deltaTime);
  92.            
  93.             anim.SetFloat("Speed", dest.magnitude);
  94.             me.Move(dest);
  95.             //gameObject.transform.position = dest;
  96.         } else {
  97.             anim.SetFloat("Speed", 0);
  98.         }
  99.     }
  100.     void LateUpdate() {
  101.         if (hastarget) {
  102.             // Add target location to pathing if he's a distance away
  103.             //   and only if he's moved from the location
  104.             if ((new Vector3(transform.position.x, 0, transform.position.z) - new Vector3(plyr.transform.position.x, 0, plyr.transform.position.z)).magnitude > 15) {
  105.                 // Not at target
  106.                 //Debug.Log(path.Count+" coordinates to path.");
  107.                 if (path.Count > 0) {
  108.                     if ((plyr.transform.position - path[path.Count-1]).magnitude > 4) {
  109.                         // player has moved from path
  110.                         //Debug.Log("Player in a new location!");
  111.                         path.Clear();
  112.                         path.Add(plyr.transform.position);
  113.                     } else {
  114.                         // update last added position
  115.                         //Debug.Log("Updating last path.");
  116.                         path[path.Count-1] = plyr.transform.position;
  117.                     }
  118.                 } else {
  119.                     // player has moved
  120.                     //Debug.Log("New path added.");
  121.                     path.Add(plyr.transform.position);
  122.                 }
  123.             }
  124.             else {
  125.                 //Debug.Log("Reached Player!");
  126.                 //path.Remove(path[0]);
  127.                 path.Clear();
  128.             }
  129.         }
  130.     }
  131.    
  132.     // store map containing pathing to target
  133.     List<List<Vector3>> routes = new List<List<Vector3>>();
  134.     List<Vector3> route = new List<Vector3>();
  135.     Dictionary<Vector3, int> map = new Dictionary<Vector3, int>();
  136.     public bool pathing = false;
  137.     Vector3 ppos = Vector3.zero;
  138.     double cycles = 0;
  139.     void CreatePath() {
  140.         if (hastarget) {
  141.             Debug.Log("Creating table...");
  142.             pathing = true;
  143.             map.Clear();
  144.             ppos = new Vector3(plyr.transform.position.x, plyr.transform.position.y+offset, plyr.transform.position.z);
  145.             // start a pathfinding tree
  146.        
  147.             //Thread t = new Thread(() => {
  148.             System.Diagnostics.Stopwatch stopwatch = new System.Diagnostics.Stopwatch();
  149.             cycles = 0;
  150.             stopwatch.Start();
  151.             MapCoord(new Vector3(((transform.position.x)), ((transform.position.y))+10, ((transform.position.z))), 1, 0);
  152.             stopwatch.Stop();
  153.             Debug.Log(cycles + " trees cycled in "+(stopwatch.Elapsed)+" seconds! "+map.Count+" steps marked, "+((pathing)? "Player not found?":"Player found!"));
  154.            
  155.             // did we find our player?
  156.             if (!pathing) {
  157.                 routes.Clear(); route.Clear();
  158.                 Debug.Log(routes.Count+" possible routes, fastest in "+path.Count+" gridblocks");
  159.                 pathing = false;
  160.             }
  161.            
  162.             foreach (var key in map.Keys) {
  163.                 if (map[key] != -1) {
  164.                     float step = ((float)map[key]-1) / StepLimit; float R = (step); float G = (1-R);
  165.    
  166.                     //Debug.Log("Step = "+map[key]+",R: "+R+", G: "+G);
  167.                     GameObject cube = (GameObject)Instantiate(Cube, key, transform.rotation);// (map[key] | _STEP);
  168.                     cube.renderer.material.color = new Color(R, G, 0, 10.0f);
  169.                     cube.transform.localScale = new Vector3(StepInc*2/3, 0, StepInc*2/3);
  170.                     //Debug.Log(msg);
  171.                 } else {
  172.                     GameObject cube = (GameObject)Instantiate(Cube, key, transform.rotation);// (map[key] | _STEP);
  173.                     cube.renderer.material.color = new Color(0, 0, 0, 10.0f);
  174.                     cube.transform.localScale = new Vector3(StepInc, 0, StepInc);
  175.                 }
  176.             }
  177.             //});
  178.             //t.Start();
  179.         }
  180.     }
  181.     // tree based scan
  182.     void MapCoord(Vector3 loc, int step, int back) {
  183.         cycles++;
  184.         //int top = 0, bot = 0, left = 0, right = 0;
  185.  
  186.         // if we have reached player, stop looking
  187.         if ((loc-ppos).magnitude < StepInc) {
  188.             Debug.Log("Player found!");
  189.             //map.Add (coord, (byte)(top | bot<<1 | left<<2 | right<<3 | step<<4));
  190.             //path.Add(loc);
  191.             if (map.ContainsKey(loc)) {
  192.                 map[loc] = -1;
  193.             } else {
  194.                 map.Add(loc, -1);
  195.             }
  196.             //path.Add(loc);
  197.             pathing = false;
  198.             return;
  199.         }
  200.         // leave a limiter to prevent infinite loops
  201.         if (step >= StepLimit) {
  202.             //Debug.Log("Step limit reached.");
  203.             return;
  204.         }
  205.         // ignore retracing steps
  206.         if (map.ContainsKey(loc)) {
  207.             if ((map[loc]) <= step)
  208.                 return;
  209.             map[loc] = step;
  210.         } else {
  211.             map.Add(loc, step);
  212.         }
  213.         // vertical path limiter, stop mapping this tree
  214.         if (Check (loc,new Vector3(loc.x, loc.y+10, loc.z))) {
  215.             Debug.Log("Steep drop D:");
  216.             //return;
  217.         }
  218.         string dbg = "Found:";
  219.         if (back != 1) {
  220.             if (!Check (loc,new Vector3(loc.x, loc.y, loc.z+StepInc))) {
  221.                 //top = 1;
  222.                 dbg += " Top";
  223.                 MapCoord(new Vector3(loc.x, loc.y, loc.z+StepInc), step+1, 2);
  224.             }
  225.         }
  226.         if (back != 2) {
  227.             if (!Check (loc,new Vector3(loc.x, loc.y, loc.z-StepInc))) {
  228.                 //bot = 1;
  229.                 dbg += " Bot";
  230.                 MapCoord(new Vector3(loc.x, loc.y, loc.z-StepInc), step+1, 1);
  231.             }
  232.         }
  233.         if (back != 3) {
  234.             if (!Check (loc,new Vector3(loc.x-StepInc, loc.y, loc.z))) {
  235.                 //left = 1;
  236.                 dbg += " Left";
  237.                 MapCoord(new Vector3(loc.x-StepInc, loc.y, loc.z), step+1, 4);
  238.             }
  239.         }
  240.         if (back != 4) {
  241.             if (!Check (loc,new Vector3(loc.x+StepInc, loc.y, loc.z))) {
  242.                 //right = 1;
  243.                 dbg += " Right";
  244.                 MapCoord(new Vector3(loc.x+StepInc, loc.y, loc.z), step+1, 3);
  245.             }
  246.         }
  247.         //Debug.Log("Step "+step+" "+dbg);
  248.        
  249.         return;
  250.     }
  251.     // returns true if collision with map is detected
  252.     bool Check(Vector3 begin, Vector3 target) {
  253.         RaycastHit hit;
  254.         if (Physics.Raycast(begin, (target-begin), out hit, (target-begin).magnitude))
  255.             if (hit.collider.gameObject.tag == "Field")
  256.                 return true;
  257.         return false;
  258.     }
  259.     // returns false if this object can see target object
  260.     bool Check(GameObject obj) {
  261.         RaycastHit hit;
  262.         if (Physics.Raycast(transform.position, obj.transform.position-transform.position, out hit))
  263.             if (hit.collider.gameObject.name == obj.name)
  264.                 return true;
  265.         return false;
  266.     }
  267.     // returns false if this object can move to destination coordinate
  268.     bool Check(Vector3 target) {
  269.         RaycastHit hit;
  270.         if (Physics.Raycast(transform.position, target-transform.position, out hit))
  271.             if (hit.collider.gameObject.tag == "Field")
  272.                 return true;
  273.         return false;
  274.     }
  275. }
Advertisement
Add Comment
Please, Sign In to add comment