public float distanceBetween(PathFindingNode p)
{
/*Finds the distance between this node and the one passed in*/
return (float)(Math.Sqrt(Math.Pow(this.x - p.x, 2) + Math.Pow(this.z - p.z, 2)));
}
public bool addToLL(PathFindingNode p)
{
//Don't allow node to point to itself
if (string.Compare(p.id, this.id) == 0)
return false;
//We'll want to manually check containment based upon id.
PathFindingNode[] pAry = this.adjacent.ToArray<PathFindingNode>();
for (int i = 0; i < pAry.Length; i++)
{
if (string.Compare(pAry[i].id, p.id) == 0)
return false;
//We also want to avoid nodes that pretty much overlap...
//if (Math.Abs(p.x - pAry[i].x) <= 5 && Math.Abs(p.z - pAry[i].z) <= 5)
// return false;
}
//Else, add to adjacency list
adjacent.AddLast(p);
return true;
}
PathFindingNode checkContains_andAdd_jwf(PathFindingNode p)
{
/*In order to avoid nodes positioned almost directly on top of one-another,
* this function will be a helper to quad-tree. If the node already exists in the
* dictionary, or a node close enough to that node, then this function will return
* a reference to that node, else, it returns passed in value */
//Check for close by nodes, including the percise node
for (int i = -25; i <= 25; i++)
for (int j = -25; j <= 25; j++)
{
if (this.pathNodeCollection.ContainsKey(string.Format("{0}::{1}", p.x + i, p.z + j)))
{
return this.pathNodeCollection[string.Format("{0}::{1}", p.x + i, p.z + j)];
}
}
//Else, we need to add this node to the dictionary
this.pathNodeCollection.Add(p.id, p);
return p;
}
bool jwf_existsCollidable(PathFindingNode node, Int32 dist)
{
//return false;
//Does a collidable object exist within the quadrent
//depicted by 'node' and 'dist'?
foreach (Object3D model in stage.Collidable)
{
if (model.Translation.X > (node.x - dist - model.ObjectBoundingSphereRadius - 164) &&
model.Translation.X < (node.x + dist + model.ObjectBoundingSphereRadius + 164) &&
model.Translation.Z > (node.z - dist - model.ObjectBoundingSphereRadius - 164) &&
model.Translation.Z < (node.z + dist + model.ObjectBoundingSphereRadius + 164))
{
return true;
}
}
return false;
}
public bool jwf_QuadTree(PathFindingNode node, Int32 dist)
{
//Given that 'node' is located in the middle of the quad
//Determine if we should split it into 4 smaller quads based upon
//1. If the distance is greater than the maximum allowed distance
//2. There is a colidable object within the quad
if ((dist > MAX_NODE_SEPARATION) || jwf_existsCollidable(node, dist))
{
//Are the nodes already at the minimum distance?
if (dist <= MIN_NODE_SEPARATION)
{
//We are done.
return true;
}
//Else, recursively call this funciton with 4 new quads
//Upper left quad
jwf_QuadTree(new PathFindingNode(this.stage, node.x - (Int32)(dist / 2), node.z - (Int32)(dist / 2)), dist / 2);
//Upper right quad
jwf_QuadTree(new PathFindingNode(this.stage, node.x + (Int32)(dist / 2), node.z - (Int32)(dist / 2)), dist / 2);
//Lower left quad
jwf_QuadTree(new PathFindingNode(this.stage, node.x - (Int32)(dist / 2), node.z + (Int32)(dist / 2)), dist / 2);
//Lower right quad
jwf_QuadTree(new PathFindingNode(this.stage, node.x + (Int32)(dist / 2), node.z + (Int32)(dist / 2)), dist / 2);
}
else
{
//Set up 9 nodes
PathFindingNode[] ary = new PathFindingNode[]{
new PathFindingNode(this.stage, node.x - dist, node.z - dist),
new PathFindingNode(this.stage, node.x, node.z - dist),
new PathFindingNode(this.stage, node.x + dist, node.z - dist),
new PathFindingNode(this.stage, node.x - dist, node.z),
node,
new PathFindingNode(this.stage, node.x + dist, node.z),
new PathFindingNode(this.stage, node.x - dist, node.z + dist),
new PathFindingNode(this.stage, node.x, node.z + dist),
new PathFindingNode(this.stage, node.x + dist, node.z + dist),
};
//Add nodes to dictionary
for (int i = 0; i < ary.Length; i++)
{
ary[i] = checkContains_andAdd_jwf(ary[i]);
}
//Add links
for (int i = 0; i < ary.Length; i++)
{
//Every node (except for center node) gets linked to center node.
this.pathNodeCollection[ary[i].id].addToLL(this.pathNodeCollection[ary[4].id]);
switch (i)
{
case 0: //upper left
this.pathNodeCollection[ary[i].id].addToLL(this.pathNodeCollection[ary[1].id]);
this.pathNodeCollection[ary[i].id].addToLL(this.pathNodeCollection[ary[3].id]);
break;
case 1: //upper middle
this.pathNodeCollection[ary[i].id].addToLL(this.pathNodeCollection[ary[0].id]);
this.pathNodeCollection[ary[i].id].addToLL(this.pathNodeCollection[ary[2].id]);
break;
case 2: //upper right
this.pathNodeCollection[ary[i].id].addToLL(this.pathNodeCollection[ary[1].id]);
this.pathNodeCollection[ary[i].id].addToLL(this.pathNodeCollection[ary[5].id]);
break;
case 3: //middle left
this.pathNodeCollection[ary[i].id].addToLL(this.pathNodeCollection[ary[0].id]);
this.pathNodeCollection[ary[i].id].addToLL(this.pathNodeCollection[ary[6].id]);
break;
case 4: //middle middle
for (int j = 0; j < ary.Length; j++)
this.pathNodeCollection[node.id].addToLL(this.pathNodeCollection[ary[j].id]);
break;
case 5: //middle right
this.pathNodeCollection[ary[i].id].addToLL(this.pathNodeCollection[ary[2].id]);
this.pathNodeCollection[ary[i].id].addToLL(this.pathNodeCollection[ary[8].id]);
break;
case 6: //lower left
this.pathNodeCollection[ary[i].id].addToLL(this.pathNodeCollection[ary[3].id]);
this.pathNodeCollection[ary[i].id].addToLL(this.pathNodeCollection[ary[7].id]);
break;
case 7: //lower middle
this.pathNodeCollection[ary[i].id].addToLL(this.pathNodeCollection[ary[6].id]);
this.pathNodeCollection[ary[i].id].addToLL(this.pathNodeCollection[ary[8].id]);
break;
case 8: //lower right
this.pathNodeCollection[ary[i].id].addToLL(this.pathNodeCollection[ary[7].id]);
this.pathNodeCollection[ary[i].id].addToLL(this.pathNodeCollection[ary[5].id]);
break;
default:
Console.Write("\nIllegal Node Link Error...\n");
break;
}
}
}
return true;
}