//this gets called when you press the button in the unity inspector
public void BuildObject()
{
beenBuilt = true;
//this is where you initialize all of your nodes/grid
// find the ground space to gen the nodes on
Ground ground = GameObject.FindObjectOfType <Ground>();
Vector3 ground_pos = ground.transform.position;
Vector3 ground_scale = ground.transform.localScale;
Debug.Log(ground_scale.z);
nodes = new NodeSquare[(int)(ground_scale.x), (int)(ground_scale.z)];
float start_x = -(ground_scale.x / 2) + (float)(.5);
float start_z = -(ground_scale.z / 2) + (float)(.5);
for (int z = 0; z < (int)ground_scale.z; z += 1)
{
for (int x = 0; x < (int)ground_scale.x; x += 1)
{
nodes [x, z] = new NodeSquare(new Vector3(start_x + x, 0.7F, start_z + z), new Vector3(x, 0, z));
}
}
Debug.Log("Completed BuildObject");
}
public List <NodeSquare> a_star(NodeSquare start, NodeSquare goal)
{
List <NodeSquare> closedSet = new List <NodeSquare> ();
List <NodeSquare> openSet = new List <NodeSquare> ();
openSet.Add(start);
Dictionary <NodeSquare, NodeSquare> cameFrom = new Dictionary <NodeSquare, NodeSquare> ();
Dictionary <NodeSquare, float> gScore = new Dictionary <NodeSquare, float> ();
Dictionary <NodeSquare, float> fScore = new Dictionary <NodeSquare, float> ();
foreach (NodeSquare node in nodes)
{
gScore.Add(node, float.MaxValue);
fScore.Add(node, float.MaxValue);
}
gScore [start] = 0;
fScore [start] = heuristic_cost_estimate(start, goal);
while (openSet.Count > 0)
{
NodeSquare current = getLowest(fScore, openSet);
current.makeRobot();
if (current.Equals(goal))
{
return(reconstruct_path(cameFrom, current));
}
openSet.Remove(current);
closedSet.Add(current);
foreach (NodeSquare neighbor in adjacent(current))
{
if (closedSet.Contains(neighbor))
{
continue;
}
float tentative_gScore = gScore [current] + 1;
if (!openSet.Contains(neighbor))
{
openSet.Add(neighbor);
}
else if (tentative_gScore >= gScore [neighbor])
{
continue;
}
cameFrom.Add(neighbor, current);
gScore [neighbor] = tentative_gScore;
fScore [neighbor] = gScore [neighbor] + heuristic_cost_estimate(neighbor, goal);
}
}
return(null);
}
// Use this for initialization
void Start()
{
att_fields = getFieldofType(1);
follow_me = att_fields [0];
ObjectBuilderScript script = GameObject.FindObjectOfType <ObjectBuilderScript>();
script.BuildObject();
followArrow = GameObject.FindObjectOfType <ObjectFollow> ().gameObject;
nodes = ObjectBuilderScript.nodes;
Vector3 roboLoc = myLocation();
Vector3 goalLoc = GameObject.FindGameObjectWithTag("Goal").transform.position;
float startDist = float.MaxValue;
float goalDist = float.MaxValue;
NodeSquare start = new NodeSquare(new Vector3(0, 0, 0), new Vector3(0, 0, 0));
NodeSquare goal = new NodeSquare(new Vector3(0, 0, 0), new Vector3(0, 0, 0));
foreach (NodeSquare node in nodes)
{
if (Vector3.Distance(node.getRenderLoc(), roboLoc) < startDist)
{
start = node;
startDist = Vector3.Distance(node.getRenderLoc(), roboLoc);
}
if (Vector3.Distance(node.getRenderLoc(), goalLoc) < goalDist)
{
goal = node;
goalDist = Vector3.Distance(node.getRenderLoc(), goalLoc);
}
}
// find the nodes that are impassable
List <Field> obstacles = getFieldofType(3);
foreach (Field block in obstacles)
{
foreach (NodeSquare node in nodes)
{
if (PointInOABB(node.getRenderLoc(), block.gameObject.GetComponent <BoxCollider> ()))
{
node.makeImpassable();
}
}
}
// calculate the path
path = a_star(start, goal);
foreach (NodeSquare node in path)
{
node.makePath();
}
path.Reverse();
}
public NodeSquare getLowest(Dictionary <NodeSquare, float> fScore, List <NodeSquare> openSet)
{
NodeSquare lowest = openSet [0];
foreach (NodeSquare key in openSet)
{
if (fScore [key] < fScore [lowest])
{
lowest = key;
}
}
return(lowest);
}
public void SelectNode(NodeSquare node)
{
if (selectedNode == node)
{
DeselectNode();
return;
}
selectedNode = node;
turretToBuild = null;
nodeUI.SetTarget(node);
}
private List <NodeSquare> buildPath(Dictionary <NodeSquare, NodeSquare> edges, NodeSquare start, NodeSquare goal, NodeSquare nearest)
{
List <NodeSquare> path = new List <NodeSquare> ();
path.Add(goal);
path.Add(nearest);
NodeSquare current = nearest;
while (current != start)
{
current = edges [current];
path.Add(current);
}
path.Reverse();
return(path);
}
public void SetTarget(NodeSquare _target)
{
target = _target;
transform.position = target.GetBuildPosition();
if (target.turretLevel <= 1)
{
upgradeCost.text = "$" + target.turretBluePrint.upgradeCost;
upgradeButton.interactable = true;
}
else
{
upgradeCost.text = "DONE";
upgradeButton.interactable = false;
}
sellAmount.text = "$" + target.turretBluePrint.GetSellAmount();
ui.SetActive(true);
}
private float distance(NodeSquare origin, NodeSquare destination)
{
return(Vector3.Distance(origin.getRenderLoc(), destination.getRenderLoc()));
}
private bool lineOfSight(NodeSquare origin, NodeSquare destination)
{
return(!Physics.Raycast(origin.getRenderLoc(), (destination.getRenderLoc() - origin.getRenderLoc()).normalized));
}
public List <NodeSquare> rrt(NodeSquare start, NodeSquare goal)
{
Debug.Log("Starting RRT");
List <NodeSquare> solution = null;
float solutionDistance = 0;
List <NodeSquare> nodeList = new List <NodeSquare> ();
for (int i = 0; i < nodes.GetLength(0); i++)
{
for (int j = 0; j < nodes.GetLength(1); j++)
{
nodeList.Add(nodes[i, j]);
}
}
int nodeCount = nodeList.Count;
Dictionary <NodeSquare, NodeSquare> startEdges = new Dictionary <NodeSquare, NodeSquare> ();
HashSet <NodeSquare> startTree = new HashSet <NodeSquare> ();
startTree.Add(start);
bool success = false;
while (success == false || nodeList.Count > (nodeCount / 2))
{
int index = Random.Range(0, nodeList.Count);
NodeSquare rnd = nodeList [index];
if (!rnd.isPassable)
{
nodeList.Remove(rnd);
continue;
}
Debug.Log(nodeList.Count.ToString() + rnd.getLoc().ToString());
NodeSquare nearestStart = null;
foreach (NodeSquare node in startTree)
{
if (lineOfSight(node, rnd))
{
if (nearestStart == null || distance(rnd, node) < distance(rnd, nearestStart))
{
nearestStart = node;
}
}
}
if (nearestStart != null)
{
startTree.Add(rnd);
startEdges.Add(rnd, nearestStart);
nodeList.Remove(rnd);
Debug.DrawLine(nearestStart.getRenderLoc(), rnd.getRenderLoc(), Color.red, float.PositiveInfinity, false);
if (lineOfSight(rnd, goal))
{
List <NodeSquare> path = buildPath(startEdges, start, goal, rnd);
float pathDist = pathDistance(path);
if (solutionDistance == 0 || pathDist < solutionDistance)
{
solution = path;
solutionDistance = pathDist;
}
success = true;
Debug.DrawLine(rnd.getRenderLoc(), goal.getRenderLoc(), Color.red, float.PositiveInfinity, false);
}
}
}
return(solution);
}
public float heuristic_cost_estimate(NodeSquare start, NodeSquare goal)
{
float distance = Mathf.Sqrt(Mathf.Pow(goal.getLoc().x - start.getLoc().x, 2) + Mathf.Pow(goal.getLoc().z - start.getLoc().z, 2));
return(distance);
}
// Use this for initialization
void Start()
{
att_fields = getFieldofType(1);
follow_me = att_fields [0];
ObjectBuilderScript script = GameObject.FindObjectOfType <ObjectBuilderScript>();
script.BuildObject();
//followArrow = GameObject.FindObjectOfType<ObjectFollow> ().gameObject;
nodes = ObjectBuilderScript.nodes;
Vector3 roboLoc = myLocation();
Vector3 goalLoc = GameObject.FindGameObjectWithTag("Goal").transform.position;
float startDist = float.MaxValue;
float goalDist = float.MaxValue;
NodeSquare start = new NodeSquare(new Vector3(0, 0, 0), new Vector3(0, 0, 0));
NodeSquare goal = new NodeSquare(new Vector3(0, 0, 0), new Vector3(0, 0, 0));
Debug.Log("Begin Start/Goal Identification");
foreach (NodeSquare node in nodes)
{
if (Vector3.Distance(node.getRenderLoc(), roboLoc) < startDist)
{
start = node;
startDist = Vector3.Distance(node.getRenderLoc(), roboLoc);
}
if (Vector3.Distance(node.getRenderLoc(), goalLoc) < goalDist)
{
goal = node;
goalDist = Vector3.Distance(node.getRenderLoc(), goalLoc);
}
}
Debug.Log("Completed Start/Goal Identification");
// find the nodes that are impassable
Debug.Log("Begin Obstacle Identification");
List <Field> obstacles = getFieldofType(3);
foreach (Field block in obstacles)
{
foreach (NodeSquare node in nodes)
{
if (PointInOABB(node.getRenderLoc(), block.gameObject.GetComponent <BoxCollider> ()))
{
node.makeImpassable();
}
}
}
Debug.Log("Completed Obstacle Identification");
if (runtype.Equals(RunType.one))
{
// calculate the path uisng A*
Debug.Log("Begin A* Calculation");
path = a_star(start, goal);
Debug.Log("Completed A* Calculation");
foreach (NodeSquare node in path)
{
node.makePath();
}
path.Reverse();
}
else if (runtype.Equals(RunType.two))
{
// calculate the path using RRT
Debug.Log("Begin RRT Calculation");
path = rrt(start, goal);
Debug.Log("Completed RRT Calculation");
for (int i = 0; i < path.Count - 1; i++)
{
Debug.DrawLine(path [i].getRenderLoc(), path [i + 1].getRenderLoc(), Color.blue, float.PositiveInfinity, false);
}
foreach (NodeSquare node in path)
{
node.makePath();
}
}
}
public List <NodeSquare> adjacent(NodeSquare current)
{
List <NodeSquare> adj = new List <NodeSquare>();
int x = (int)current.getLoc().x;
int z = (int)current.getLoc().z;
int max_x = nodes.GetLength(0);
int max_z = nodes.GetLength(1);
if (x - 1 >= 0 && z - 1 >= 0)
{
if (nodes[x - 1, z - 1].isPassable)
{
adj.Add(nodes [x - 1, z - 1]);
}
}
if (z - 1 >= 0)
{
if (nodes[x, z - 1].isPassable)
{
adj.Add(nodes [x, z - 1]);
}
}
if (x + 1 < max_x && z - 1 >= 0)
{
if (nodes[x + 1, z - 1].isPassable)
{
adj.Add(nodes [x + 1, z - 1]);
}
}
if (x - 1 >= 0)
{
if (nodes[x - 1, z].isPassable)
{
adj.Add(nodes [x - 1, z]);
}
}
if (x + 1 < max_x)
{
if (nodes[x + 1, z].isPassable)
{
adj.Add(nodes [x + 1, z]);
}
}
if (x - 1 >= 0 && z + 1 < max_z)
{
if (nodes[x - 1, z + 1].isPassable)
{
adj.Add(nodes [x - 1, z + 1]);
}
}
if (z + 1 < max_z)
{
if (nodes[x, z + 1].isPassable)
{
adj.Add(nodes [x, z + 1]);
}
}
if (x + 1 < max_x && z + 1 < max_z)
{
if (nodes[x + 1, z + 1].isPassable)
{
adj.Add(nodes [x + 1, z + 1]);
}
}
return(adj);
}
public List <NodeSquare> reconstruct_path(Dictionary <NodeSquare, NodeSquare> cameFrom, NodeSquare current)
{
List <NodeSquare> total_path = new List <NodeSquare>();
total_path.Add(current);
while (cameFrom.ContainsKey(current))
{
current = cameFrom [current];
total_path.Add(current);
}
return(total_path);
}
public void DeselectNode()
{
selectedNode = null;
nodeUI.Hide();
}