public override Node[] CreateNodes (int number) {
MeshNode[] tmp = new MeshNode[number];
for (int i=0;i<number;i++) {
tmp[i] = new MeshNode ();
}
return tmp as Node[];
}
int GetBox ( MeshNode node ) {
if ( count >= arr.Length ) EnsureCapacity ( count+1 );
arr[count] = new BBTreeBox ( node );
count++;
return count-1;
}
/** Rebuilds the tree using the specified nodes.
* This is faster and gives better quality results compared to calling Insert with all nodes
*/
public void RebuildFrom (MeshNode[] nodes) {
Clear();
if (nodes.Length == 0) {
return;
}
if (nodes.Length == 1) {
GetBox(nodes[0]);
return;
}
// We will use approximately 2N tree nodes
EnsureCapacity(Mathf.CeilToInt (nodes.Length * 2.1f));
// Make a copy of the nodes array since we will be modifying it
var nodeCopies = new MeshNode[nodes.Length];
for (int i = 0; i < nodes.Length; i++) nodeCopies[i] = nodes[i];
RebuildFromInternal(nodeCopies, 0, nodes.Length, false);
}
public void Insert(MeshNode node)
{
int box = this.GetBox(node);
if (box == 0)
{
return;
}
BBTree.BBTreeBox bBTreeBox = this.arr[box];
int num = 0;
BBTree.BBTreeBox bBTreeBox2;
while (true)
{
bBTreeBox2 = this.arr[num];
bBTreeBox2.rect = BBTree.ExpandToContain(bBTreeBox2.rect, bBTreeBox.rect);
if (bBTreeBox2.node != null)
{
break;
}
this.arr[num] = bBTreeBox2;
int num2 = BBTree.ExpansionRequired(this.arr[bBTreeBox2.left].rect, bBTreeBox.rect);
int num3 = BBTree.ExpansionRequired(this.arr[bBTreeBox2.right].rect, bBTreeBox.rect);
if (num2 < num3)
{
num = bBTreeBox2.left;
}
else if (num3 < num2)
{
num = bBTreeBox2.right;
}
else
{
num = ((BBTree.RectArea(this.arr[bBTreeBox2.left].rect) >= BBTree.RectArea(this.arr[bBTreeBox2.right].rect)) ? bBTreeBox2.right : bBTreeBox2.left);
}
}
bBTreeBox2.left = box;
int box2 = this.GetBox(bBTreeBox2.node);
bBTreeBox2.right = box2;
bBTreeBox2.node = null;
this.arr[num] = bBTreeBox2;
}
public void Insert(MeshNode node)
{
BBTreeBox box = new BBTreeBox (this,node);
if (root == null) {
root = box;
return;
}
BBTreeBox c = root;
while (true) {
c.rect = ExpandToContain (c.rect,box.rect);
if (c.node != null) {
//Is Leaf
c.c1 = box;
BBTreeBox box2 = new BBTreeBox (this,c.node);
//Console.WriteLine ("Inserted "+box.node+", rect "+box.rect.ToString ());
c.c2 = box2;
c.node = null;
//c.rect = c.rect.
return;
} else {
float e1 = ExpansionRequired (c.c1.rect,box.rect);
float e2 = ExpansionRequired (c.c2.rect,box.rect);
//Choose the rect requiring the least expansion to contain box.rect
if (e1 < e2) {
c = c.c1;
} else if (e2 < e1) {
c = c.c2;
} else {
//Equal, Choose the one with the smallest area
c = RectArea (c.c1.rect) < RectArea (c.c2.rect) ? c.c1 : c.c2;
}
}
}
}
public void Apply(bool forceNewCheck)
{
//TODO
//This function assumes that connections from the n1,n2 nodes never need to be removed in the future (e.g because the nodes move or something)
var nn = NNConstraint.None;
nn.distanceXZ = true;
var graph = (int)startNode.GraphIndex;
//Search all graphs but the one which start and end nodes are on
nn.graphMask = ~(1 << graph);
var same = true;
{
var info = AstarPath.active.GetNearest(StartTransform.position, nn);
same &= info.node == connectedNode1 && info.node != null;
connectedNode1 = info.node as MeshNode;
clamped1 = info.position;
if (connectedNode1 != null)
{
Debug.DrawRay((Vector3)connectedNode1.position, Vector3.up * 5, Color.red);
}
}
{
var info = AstarPath.active.GetNearest(EndTransform.position, nn);
same &= info.node == connectedNode2 && info.node != null;
connectedNode2 = info.node as MeshNode;
clamped2 = info.position;
if (connectedNode2 != null)
{
Debug.DrawRay((Vector3)connectedNode2.position, Vector3.up * 5, Color.cyan);
}
}
if (connectedNode2 == null || connectedNode1 == null)
{
return;
}
startNode.SetPosition((Int3)StartTransform.position);
endNode.SetPosition((Int3)EndTransform.position);
if (same && !forceNewCheck)
{
return;
}
RemoveConnections(startNode);
RemoveConnections(endNode);
var cost = (uint)Mathf.RoundToInt(
((Int3)(StartTransform.position - EndTransform.position)).costMagnitude * costFactor);
startNode.AddConnection(endNode, cost);
endNode.AddConnection(startNode, cost);
var dir = connectedNode2.position - connectedNode1.position;
for (var a = 0; a < connectedNode1.GetVertexCount(); a++)
{
var va1 = connectedNode1.GetVertex(a);
var va2 = connectedNode1.GetVertex((a + 1) % connectedNode1.GetVertexCount());
if (Int3.DotLong((va2 - va1).Normal2D(), dir) > 0)
{
continue;
}
for (var b = 0; b < connectedNode2.GetVertexCount(); b++)
{
var vb1 = connectedNode2.GetVertex(b);
var vb2 = connectedNode2.GetVertex((b + 1) % connectedNode2.GetVertexCount());
if (Int3.DotLong((vb2 - vb1).Normal2D(), dir) < 0)
{
continue;
}
if (Int3.Angle(vb2 - vb1, va2 - va1) > 170.0 / 360.0f * Mathf.PI * 2)
{
float t1 = 0;
float t2 = 1;
t2 = System.Math.Min(t2, VectorMath.ClosestPointOnLineFactor(va1, va2, vb1));
t1 = System.Math.Max(t1, VectorMath.ClosestPointOnLineFactor(va1, va2, vb2));
if (t2 < t1)
{
Debug.LogError("Something went wrong! " + t1 + " " + t2 + " " + va1 + " " + va2 + " " +
vb1 + " " + vb2 + "\nTODO, how can this happen?");
}
else
{
var pa = (Vector3)(va2 - va1) * t1 + (Vector3)va1;
var pb = (Vector3)(va2 - va1) * t2 + (Vector3)va1;
startNode.portalA = pa;
startNode.portalB = pb;
endNode.portalA = pb;
endNode.portalB = pa;
//Add connections between nodes, or replace old connections if existing
connectedNode1.AddConnection(startNode,
(uint)Mathf.RoundToInt(((Int3)(clamped1 - StartTransform.position)).costMagnitude *
costFactor));
connectedNode2.AddConnection(endNode,
(uint)Mathf.RoundToInt(((Int3)(clamped2 - EndTransform.position)).costMagnitude *
costFactor));
startNode.AddConnection(connectedNode1,
(uint)Mathf.RoundToInt(((Int3)(clamped1 - StartTransform.position)).costMagnitude *
costFactor));
endNode.AddConnection(connectedNode2,
(uint)Mathf.RoundToInt(((Int3)(clamped2 - EndTransform.position)).costMagnitude *
costFactor));
return;
}
}
}
}
}
static bool NodeIntersectsCircle (MeshNode node, Vector3 p, float radius) {
if (float.IsPositiveInfinity(radius)) return true;
/** \bug Is not correct on the Y axis */
return (p - node.ClosestPointOnNode (p)).sqrMagnitude < radius*radius;
}
public bool NodeIntersectsCircle(MeshNode node, Vector3 p, float radius)
{
if (NavMeshGraph.ContainsPoint (node,p,graph.vertices)) {
return true;
}
Int3[] vertices = graph.vertices;
float r2 = radius*radius;
Vector3 p1 = (Vector3)vertices[node[0]], p2 = (Vector3)vertices[node[1]], p3 = (Vector3)vertices[node[2]];
p1.y = p.y;
p2.y = p.y;
p3.y = p.y;
return Mathfx.DistancePointSegmentStrict (p1,p2,p) < r2 ||
Mathfx.DistancePointSegmentStrict (p2,p3,p) < r2 ||
Mathfx.DistancePointSegmentStrict (p3,p1,p) < r2;
}
/** Inserts a mesh node in the tree */
public void Insert(MeshNode node)
{
int boxi = GetBox(node);
// Was set to root
if (boxi == 0)
{
return;
}
BBTreeBox box = arr[boxi];
//int depth = 0;
int c = 0;
while (true)
{
BBTreeBox cb = arr[c];
cb.rect = ExpandToContain(cb.rect, box.rect);
if (cb.node != null)
{
//Is Leaf
cb.left = boxi;
int box2 = GetBox(cb.node);
//BBTreeBox box2 = new BBTreeBox (this,c.node);
//Console.WriteLine ("Inserted "+box.node+", rect "+box.rect.ToString ());
cb.right = box2;
cb.node = null;
//cb.depth++;
//c.rect = c.rect.
arr[c] = cb;
//Debug.Log (depth);
return;
}
else
{
//depth++;
//cb.depth++;
arr[c] = cb;
int e1 = ExpansionRequired(arr[cb.left].rect, box.rect); // * arr[cb.left].depth;
int e2 = ExpansionRequired(arr[cb.right].rect, box.rect); // * arr[cb.left].depth;
//Choose the rect requiring the least expansion to contain box.rect
if (e1 < e2)
{
c = cb.left;
}
else if (e2 < e1)
{
c = cb.right;
}
else
{
//Equal, Choose the one with the smallest area
c = RectArea(arr[cb.left].rect) < RectArea(arr[cb.right].rect) ? cb.left : cb.right;
}
}
}
}
public BBTreeBox (MeshNode node) {
this.node = node;
var first = node.GetVertex(0);
var min = new Int2(first.x,first.z);
Int2 max = min;
for (int i=1;i<node.GetVertexCount();i++) {
var p = node.GetVertex(i);
min.x = Math.Min (min.x,p.x);
min.y = Math.Min (min.y,p.z);
max.x = Math.Max (max.x,p.x);
max.y = Math.Max (max.y,p.z);
}
rect = new IntRect (min.x,min.y,max.x,max.y);
left = right = -1;
}
public void Apply ( bool forceNewCheck ) {
//TODO
//This function assumes that connections from the n1,n2 nodes never need to be removed in the future (e.g because the nodes move or something)
NNConstraint nn = NNConstraint.None;
nn.distanceXZ = true;
int graph = (int)startNode.GraphIndex;
//Search all graphs but the one which start and end nodes are on
nn.graphMask = ~(1 << graph);
bool same = true;
if (true) {
NNInfo n1 = AstarPath.active.GetNearest(StartTransform.position, nn);
same &= n1.node == connectedNode1 && n1.node != null;
connectedNode1 = n1.node as MeshNode;
clamped1 = n1.clampedPosition;
if ( connectedNode1 != null ) Debug.DrawRay ( (Vector3)connectedNode1.position, Vector3.up*5,Color.red);
}
if (true) {
NNInfo n2 = AstarPath.active.GetNearest(EndTransform.position, nn);
same &= n2.node == connectedNode2 && n2.node != null;
connectedNode2 = n2.node as MeshNode;
clamped2 = n2.clampedPosition;
if ( connectedNode2 != null ) Debug.DrawRay ( (Vector3)connectedNode2.position, Vector3.up*5,Color.cyan);
}
if (connectedNode2 == null || connectedNode1 == null) return;
startNode.SetPosition ( (Int3)StartTransform.position );
endNode.SetPosition ( (Int3)EndTransform.position );
if ( same && !forceNewCheck ) return;
RemoveConnections(startNode);
RemoveConnections(endNode);
uint cost = (uint)Mathf.RoundToInt(((Int3)(StartTransform.position-EndTransform.position)).costMagnitude*costFactor);
startNode.AddConnection (endNode, cost);
endNode.AddConnection(startNode, cost);
Int3 dir = connectedNode2.position - connectedNode1.position;
for ( int a=0;a<connectedNode1.GetVertexCount();a++) {
Int3 va1 = connectedNode1.GetVertex ( a );
Int3 va2 = connectedNode1.GetVertex ( (a+1) % connectedNode1.GetVertexCount() );
if ( Int3.DotLong ( (va2-va1).Normal2D (), dir ) > 0 ) continue;
for ( int b=0;b<connectedNode2.GetVertexCount();b++) {
Int3 vb1 = connectedNode2.GetVertex ( b );
Int3 vb2 = connectedNode2.GetVertex ( (b+1) % connectedNode2.GetVertexCount() );
if ( Int3.DotLong ( (vb2-vb1).Normal2D (), dir ) < 0 ) continue;
//Debug.DrawLine ((Vector3)va1, (Vector3)va2, Color.magenta);
//Debug.DrawLine ((Vector3)vb1, (Vector3)vb2, Color.cyan);
//Debug.Break ();
if ( Int3.Angle ( (vb2-vb1), (va2-va1) ) > (170.0/360.0f)*Mathf.PI*2 ) {
float t1 = 0;
float t2 = 1;
t2 = System.Math.Min ( t2, AstarMath.NearestPointFactor ( va1, va2, vb1 ) );
t1 = System.Math.Max ( t1, AstarMath.NearestPointFactor ( va1, va2, vb2 ) );
if ( t2 < t1 ) {
Debug.LogError ("Wait wut!? " + t1 + " " + t2 + " " + va1 + " " + va2 + " " + vb1 + " " + vb2+"\nTODO, fix this error" );
} else {
Vector3 pa = (Vector3)(va2-va1)*t1 + (Vector3)va1;
Vector3 pb = (Vector3)(va2-va1)*t2 + (Vector3)va1;
startNode.portalA = pa;
startNode.portalB = pb;
endNode.portalA = pb;
endNode.portalB = pa;
//Add connections between nodes, or replace old connections if existing
connectedNode1.AddConnection(startNode, (uint)Mathf.RoundToInt (((Int3)(clamped1 - StartTransform.position)).costMagnitude*costFactor));
connectedNode2.AddConnection(endNode, (uint)Mathf.RoundToInt (((Int3)(clamped2 - EndTransform.position)).costMagnitude*costFactor));
startNode.AddConnection(connectedNode1, (uint)Mathf.RoundToInt (((Int3)(clamped1 - StartTransform.position)).costMagnitude*costFactor));
endNode.AddConnection(connectedNode2, (uint)Mathf.RoundToInt (((Int3)(clamped2 - EndTransform.position)).costMagnitude*costFactor));
return;
}
}
}
}
}
/** Calculates the bounding box in XZ space of all nodes between \a from (inclusive) and \a to (exclusive) */
static IntRect NodeBounds (MeshNode[] nodes, int from, int to) {
if (to - from <= 0) throw new ArgumentException();
var first = nodes[from].GetVertex(0);
var min = new Int2(first.x,first.z);
Int2 max = min;
for (int j = from; j < to; j++) {
var node = nodes[j];
var nverts = node.GetVertexCount();
for (int i = 0; i < nverts; i++) {
var p = node.GetVertex(i);
min.x = Math.Min (min.x, p.x);
min.y = Math.Min (min.y, p.z);
max.x = Math.Max (max.x, p.x);
max.y = Math.Max (max.y, p.z);
}
}
return new IntRect (min.x, min.y, max.x, max.y);
}
public BBTreeBox(IntRect rect)
{
node = null;
this.rect = rect;
left = right = -1;
}
public BBTreeBox(MeshNode node)
{
this.node = node;
Int3 vertex = node.GetVertex(0);
Int2 @int = new Int2(vertex.x, vertex.z);
Int2 int2 = @int;
for (int i = 1; i < node.GetVertexCount(); i++)
{
Int3 vertex2 = node.GetVertex(i);
@int.x = Math.Min(@int.x, vertex2.x);
@int.y = Math.Min(@int.y, vertex2.z);
int2.x = Math.Max(int2.x, vertex2.x);
int2.y = Math.Max(int2.y, vertex2.z);
}
this.rect = new IntRect(@int.x, @int.y, int2.x, int2.y);
this.left = (this.right = -1);
}
public void Apply(bool forceNewCheck)
{
NNConstraint none = NNConstraint.None;
none.distanceXZ = true;
int graphIndex = (int)this.startNode.GraphIndex;
none.graphMask = ~(1 << graphIndex);
bool flag = true;
NNInfo nearest = AstarPath.active.GetNearest(this.StartTransform.position, none);
flag &= (nearest.node == this.connectedNode1 && nearest.node != null);
this.connectedNode1 = (nearest.node as MeshNode);
this.clamped1 = nearest.clampedPosition;
if (this.connectedNode1 != null)
{
Debug.DrawRay((Vector3)this.connectedNode1.position, Vector3.up * 5f, Color.red);
}
NNInfo nearest2 = AstarPath.active.GetNearest(this.EndTransform.position, none);
flag &= (nearest2.node == this.connectedNode2 && nearest2.node != null);
this.connectedNode2 = (nearest2.node as MeshNode);
this.clamped2 = nearest2.clampedPosition;
if (this.connectedNode2 != null)
{
Debug.DrawRay((Vector3)this.connectedNode2.position, Vector3.up * 5f, Color.cyan);
}
if (this.connectedNode2 == null || this.connectedNode1 == null)
{
return;
}
this.startNode.SetPosition((Int3)this.StartTransform.position);
this.endNode.SetPosition((Int3)this.EndTransform.position);
if (flag && !forceNewCheck)
{
return;
}
this.RemoveConnections(this.startNode);
this.RemoveConnections(this.endNode);
uint cost = (uint)Mathf.RoundToInt((float)((Int3)(this.StartTransform.position - this.EndTransform.position)).costMagnitude * this.costFactor);
this.startNode.AddConnection(this.endNode, cost);
this.endNode.AddConnection(this.startNode, cost);
Int3 rhs = this.connectedNode2.position - this.connectedNode1.position;
for (int i = 0; i < this.connectedNode1.GetVertexCount(); i++)
{
Int3 vertex = this.connectedNode1.GetVertex(i);
Int3 vertex2 = this.connectedNode1.GetVertex((i + 1) % this.connectedNode1.GetVertexCount());
if (Int3.DotLong((vertex2 - vertex).Normal2D(), rhs) <= 0L)
{
for (int j = 0; j < this.connectedNode2.GetVertexCount(); j++)
{
Int3 vertex3 = this.connectedNode2.GetVertex(j);
Int3 vertex4 = this.connectedNode2.GetVertex((j + 1) % this.connectedNode2.GetVertexCount());
if (Int3.DotLong((vertex4 - vertex3).Normal2D(), rhs) >= 0L)
{
if ((double)Int3.Angle(vertex4 - vertex3, vertex2 - vertex) > 2.9670598109563189)
{
float num = 0f;
float num2 = 1f;
num2 = Math.Min(num2, AstarMath.NearestPointFactor(vertex, vertex2, vertex3));
num = Math.Max(num, AstarMath.NearestPointFactor(vertex, vertex2, vertex4));
if (num2 >= num)
{
Vector3 vector = (Vector3)(vertex2 - vertex) * num + (Vector3)vertex;
Vector3 vector2 = (Vector3)(vertex2 - vertex) * num2 + (Vector3)vertex;
this.startNode.portalA = vector;
this.startNode.portalB = vector2;
this.endNode.portalA = vector2;
this.endNode.portalB = vector;
this.connectedNode1.AddConnection(this.startNode, (uint)Mathf.RoundToInt((float)((Int3)(this.clamped1 - this.StartTransform.position)).costMagnitude * this.costFactor));
this.connectedNode2.AddConnection(this.endNode, (uint)Mathf.RoundToInt((float)((Int3)(this.clamped2 - this.EndTransform.position)).costMagnitude * this.costFactor));
this.startNode.AddConnection(this.connectedNode1, (uint)Mathf.RoundToInt((float)((Int3)(this.clamped1 - this.StartTransform.position)).costMagnitude * this.costFactor));
this.endNode.AddConnection(this.connectedNode2, (uint)Mathf.RoundToInt((float)((Int3)(this.clamped2 - this.EndTransform.position)).costMagnitude * this.costFactor));
return;
}
Debug.LogError(string.Concat(new object[]
{
"Wait wut!? ",
num,
" ",
num2,
" ",
vertex,
" ",
vertex2,
" ",
vertex3,
" ",
vertex4,
"\nTODO, fix this error"
}));
}
}
}
}
}
}
public void Apply ( bool forceNewCheck ) {
//TODO
//This function assumes that connections from the n1,n2 nodes never need to be removed in the future (e.g because the nodes move or something)
var nn = NNConstraint.None;
var graph = (int)startNode.GraphIndex;
//Search all graphs but the one which start and end nodes are on
nn.graphMask = ~(1 << graph);
startNode.SetPosition ( (Int3)StartTransform.position );
endNode.SetPosition ( (Int3)EndTransform.position );
RemoveConnections(startNode);
RemoveConnections(endNode);
var cost = (uint)Mathf.RoundToInt(((Int3)(StartTransform.position-EndTransform.position)).costMagnitude*costFactor);
startNode.AddConnection (endNode, cost);
endNode.AddConnection(startNode, cost);
if (connectedNode1 == null || forceNewCheck) {
var n1 = AstarPath.active.GetNearest(StartTransform.position, nn);
connectedNode1 = n1.node as MeshNode;
clamped1 = n1.clampedPosition;
}
if (connectedNode2 == null || forceNewCheck) {
var n2 = AstarPath.active.GetNearest(EndTransform.position, nn);
connectedNode2 = n2.node as MeshNode;
clamped2 = n2.clampedPosition;
}
if (connectedNode2 == null || connectedNode1 == null) return;
//Add connections between nodes, or replace old connections if existing
connectedNode1.AddConnection(startNode, (uint)Mathf.RoundToInt (((Int3)(clamped1 - StartTransform.position)).costMagnitude*costFactor));
if ( !oneWay ) connectedNode2.AddConnection(endNode, (uint)Mathf.RoundToInt (((Int3)(clamped2 - EndTransform.position)).costMagnitude*costFactor));
if ( !oneWay ) startNode.AddConnection(connectedNode1, (uint)Mathf.RoundToInt (((Int3)(clamped1 - StartTransform.position)).costMagnitude*costFactor));
endNode.AddConnection(connectedNode2, (uint)Mathf.RoundToInt (((Int3)(clamped2 - EndTransform.position)).costMagnitude*costFactor));
}
public void RecalculateCosts()
{
if (this.pivots == null)
{
this.RecalculatePivots();
}
if (this.mode == HeuristicOptimizationMode.None)
{
return;
}
this.pivotCount = 0;
for (int i = 0; i < this.pivots.Length; i++)
{
if (this.pivots[i] != null && (this.pivots[i].Destroyed || !this.pivots[i].Walkable))
{
throw new Exception("Invalid pivot nodes (destroyed or unwalkable)");
}
}
if (this.mode != HeuristicOptimizationMode.RandomSpreadOut)
{
for (int j = 0; j < this.pivots.Length; j++)
{
if (this.pivots[j] == null)
{
throw new Exception("Invalid pivot nodes (null)");
}
}
}
Debug.Log("Recalculating costs...");
this.pivotCount = this.pivots.Length;
Action <int> startCostCalculation = null;
startCostCalculation = delegate(int k)
{
GraphNode pivot = this.pivots[k];
FloodPath fp = null;
fp = FloodPath.Construct(pivot, null);
fp.immediateCallback = delegate(Path _p)
{
_p.Claim(this);
MeshNode meshNode = pivot as MeshNode;
uint costOffset = 0u;
int k;
if (meshNode != null && meshNode.connectionCosts != null)
{
for (k = 0; k < meshNode.connectionCosts.Length; k++)
{
costOffset = Math.Max(costOffset, meshNode.connectionCosts[k]);
}
}
NavGraph[] graphs = AstarPath.active.graphs;
for (int m = graphs.Length - 1; m >= 0; m--)
{
graphs[m].GetNodes(delegate(GraphNode node)
{
int num6 = node.NodeIndex * this.pivotCount + k;
this.EnsureCapacity(num6);
PathNode pathNode = fp.pathHandler.GetPathNode(node);
if (costOffset > 0u)
{
this.costs[num6] = ((pathNode.pathID != fp.pathID || pathNode.parent == null) ? 0u : Math.Max(pathNode.parent.G - costOffset, 0u));
}
else
{
this.costs[num6] = ((pathNode.pathID != fp.pathID) ? 0u : pathNode.G);
}
return(true);
});
}
if (this.mode == HeuristicOptimizationMode.RandomSpreadOut && k < this.pivots.Length - 1)
{
int num = -1;
uint num2 = 0u;
int num3 = this.maxNodeIndex / this.pivotCount;
for (int n = 1; n < num3; n++)
{
uint num4 = 1073741824u;
for (int num5 = 0; num5 <= k; num5++)
{
num4 = Math.Min(num4, this.costs[n * this.pivotCount + num5]);
}
GraphNode node2 = fp.pathHandler.GetPathNode(n).node;
if ((num4 > num2 || num == -1) && node2 != null && !node2.Destroyed && node2.Walkable)
{
num = n;
num2 = num4;
}
}
if (num == -1)
{
Debug.LogError("Failed generating random pivot points for heuristic optimizations");
return;
}
this.pivots[k + 1] = fp.pathHandler.GetPathNode(num).node;
Debug.Log(string.Concat(new object[]
{
"Found node at ",
this.pivots[k + 1].position,
" with score ",
num2
}));
startCostCalculation(k + 1);
}
_p.Release(this);
};
AstarPath.StartPath(fp, true);
};
if (this.mode != HeuristicOptimizationMode.RandomSpreadOut)
{
for (int l = 0; l < this.pivots.Length; l++)
{
startCostCalculation(l);
}
}
else
{
startCostCalculation(0);
}
this.dirty = false;
}
public override void OnDrawGizmos(bool drawNodes)
{
if (!drawNodes)
{
return;
}
if (bbTree != null)
{
bbTree.OnDrawGizmos();
}
Gizmos.DrawWireCube(forcedBounds.center, forcedBounds.size);
//base.OnDrawGizmos (drawNodes);
if (nodes == null)
{
//Scan (AstarPath.active.GetGraphIndex (this));
}
if (nodes == null)
{
return;
}
for (int i = 0; i < nodes.Length; i++)
{
//AstarColor.NodeConnection;
MeshNode node = (MeshNode)nodes[i];
if (AstarPath.active.debugPathData != null && AstarPath.active.showSearchTree && node.GetNodeRun(AstarPath.active.debugPathData).parent != null)
{
//Gizmos.color = new Color (0,1,0,0.7F);
Gizmos.color = NodeColor(node, AstarPath.active.debugPathData);
Gizmos.DrawLine((Vector3)node.position, (Vector3)node.GetNodeRun(AstarPath.active.debugPathData).parent.node.position);
}
else
{
//for (int q=0;q<node.connections.Length;q++) {
// Gizmos.DrawLine (node.position,node.connections[q].position);
//}
}
/*Gizmos.color = AstarColor.MeshEdgeColor;
* for (int q=0;q<node.connections.Length;q++) {
* //Gizmos.color = Color.Lerp (Color.green,Color.red,node.connectionCosts[q]/8000F);
* Gizmos.DrawLine (node.position,node.connections[q].position);
* }*/
//Gizmos.color = NodeColor (node);
//Gizmos.color.a = 0.2F;
if (showMeshOutline)
{
Gizmos.color = NodeColor(node, AstarPath.active.debugPathData);
Gizmos.DrawLine((Vector3)vertices[node.v1], (Vector3)vertices[node.v2]);
Gizmos.DrawLine((Vector3)vertices[node.v2], (Vector3)vertices[node.v3]);
Gizmos.DrawLine((Vector3)vertices[node.v3], (Vector3)vertices[node.v1]);
}
}
}
// Token: 0x060005A1 RID: 1441 RVA: 0x000339EC File Offset: 0x00031DEC
private static bool NodeIntersectsCircle(MeshNode node, Vector3 p, float radius)
{
return(float.IsPositiveInfinity(radius) || (p - node.ClosestPointOnNode(p)).sqrMagnitude < radius * radius);
}
private static void DrawDebugNode(MeshNode node, float yoffset, Color color)
{
UnityEngine.Debug.DrawLine((Vector3)node.GetVertex(1) + Vector3.up * yoffset, (Vector3)node.GetVertex(2) + Vector3.up * yoffset, color);
UnityEngine.Debug.DrawLine((Vector3)node.GetVertex(0) + Vector3.up * yoffset, (Vector3)node.GetVertex(1) + Vector3.up * yoffset, color);
UnityEngine.Debug.DrawLine((Vector3)node.GetVertex(2) + Vector3.up * yoffset, (Vector3)node.GetVertex(0) + Vector3.up * yoffset, color);
}
public void Apply(bool forceNewCheck)
{
//TODO
//This function assumes that connections from the n1,n2 nodes never need to be removed in the future (e.g because the nodes move or something)
NNConstraint nn = NNConstraint.None;
nn.distanceXZ = true;
int graph = (int)startNode.GraphIndex;
//Search all graphs but the one which start and end nodes are on
nn.graphMask = ~(1 << graph);
bool same = true;
if (true)
{
NNInfo n1 = AstarPath.active.GetNearest(StartTransform.position, nn);
same &= n1.node == connectedNode1 && n1.node != null;
connectedNode1 = n1.node as MeshNode;
clamped1 = n1.clampedPosition;
if (connectedNode1 != null)
{
Debug.DrawRay((Vector3)connectedNode1.position, Vector3.up * 5, Color.red);
}
}
if (true)
{
NNInfo n2 = AstarPath.active.GetNearest(EndTransform.position, nn);
same &= n2.node == connectedNode2 && n2.node != null;
connectedNode2 = n2.node as MeshNode;
clamped2 = n2.clampedPosition;
if (connectedNode2 != null)
{
Debug.DrawRay((Vector3)connectedNode2.position, Vector3.up * 5, Color.cyan);
}
}
if (connectedNode2 == null || connectedNode1 == null)
{
return;
}
startNode.SetPosition((Int3)StartTransform.position);
endNode.SetPosition((Int3)EndTransform.position);
if (same && !forceNewCheck)
{
return;
}
RemoveConnections(startNode);
RemoveConnections(endNode);
uint cost = (uint)Mathf.RoundToInt(((Int3)(StartTransform.position - EndTransform.position)).costMagnitude * costFactor);
startNode.AddConnection(endNode, cost);
endNode.AddConnection(startNode, cost);
Int3 dir = connectedNode2.position - connectedNode1.position;
for (int a = 0; a < connectedNode1.GetVertexCount(); a++)
{
Int3 va1 = connectedNode1.GetVertex(a);
Int3 va2 = connectedNode1.GetVertex((a + 1) % connectedNode1.GetVertexCount());
if (Int3.DotLong((va2 - va1).Normal2D(), dir) > 0)
{
continue;
}
for (int b = 0; b < connectedNode2.GetVertexCount(); b++)
{
Int3 vb1 = connectedNode2.GetVertex(b);
Int3 vb2 = connectedNode2.GetVertex((b + 1) % connectedNode2.GetVertexCount());
if (Int3.DotLong((vb2 - vb1).Normal2D(), dir) < 0)
{
continue;
}
//Debug.DrawLine ((Vector3)va1, (Vector3)va2, Color.magenta);
//Debug.DrawLine ((Vector3)vb1, (Vector3)vb2, Color.cyan);
//Debug.Break ();
if (Int3.Angle((vb2 - vb1), (va2 - va1)) > (170.0 / 360.0f) * Mathf.PI * 2)
{
float t1 = 0;
float t2 = 1;
t2 = System.Math.Min(t2, AstarMath.NearestPointFactor(va1, va2, vb1));
t1 = System.Math.Max(t1, AstarMath.NearestPointFactor(va1, va2, vb2));
if (t2 < t1)
{
Debug.LogError("Wait wut!? " + t1 + " " + t2 + " " + va1 + " " + va2 + " " + vb1 + " " + vb2 + "\nTODO, fix this error");
}
else
{
Vector3 pa = (Vector3)(va2 - va1) * t1 + (Vector3)va1;
Vector3 pb = (Vector3)(va2 - va1) * t2 + (Vector3)va1;
startNode.portalA = pa;
startNode.portalB = pb;
endNode.portalA = pb;
endNode.portalB = pa;
//Add connections between nodes, or replace old connections if existing
connectedNode1.AddConnection(startNode, (uint)Mathf.RoundToInt(((Int3)(clamped1 - StartTransform.position)).costMagnitude * costFactor));
connectedNode2.AddConnection(endNode, (uint)Mathf.RoundToInt(((Int3)(clamped2 - EndTransform.position)).costMagnitude * costFactor));
startNode.AddConnection(connectedNode1, (uint)Mathf.RoundToInt(((Int3)(clamped1 - StartTransform.position)).costMagnitude * costFactor));
endNode.AddConnection(connectedNode2, (uint)Mathf.RoundToInt(((Int3)(clamped2 - EndTransform.position)).costMagnitude * costFactor));
return;
}
}
}
}
}
/** Returns if the point is inside the node in XZ space */
public static bool ContainsPoint (MeshNode node, Vector3 pos, Int3[] vertices) {
if (Polygon.IsClockwiseMargin ((Vector3)vertices[node.v1],(Vector3)vertices[node.v2], pos) && Polygon.IsClockwiseMargin ((Vector3)vertices[node.v2],(Vector3)vertices[node.v3], pos) && Polygon.IsClockwiseMargin ((Vector3)vertices[node.v3],(Vector3)vertices[node.v1], pos)) {
return true;
}
return false;
}
/** This performs a linear search through all polygons returning the closest one.
* This will fill the NNInfo with .node for the closest node not necessarily complying with the NNConstraint, and .constrainedNode with the closest node
* complying with the NNConstraint.
* \see GetNearestForce(Node[],Int3[],Vector3,NNConstraint,bool)
*/
public static NNInfo GetNearestForceBoth(Node[] nodes, Int3[] vertices, Vector3 position, NNConstraint constraint, bool accurateNearestNode)
{
Int3 pos = (Int3)position;
float minDist = -1;
Node minNode = null;
float minConstDist = -1;
Node minConstNode = null;
float maxDistSqr = constraint.constrainDistance ? AstarPath.active.maxNearestNodeDistanceSqr : float.PositiveInfinity;
if (nodes == null || nodes.Length == 0)
{
return(new NNInfo());
}
for (int i = 0; i < nodes.Length; i++)
{
MeshNode node = nodes[i] as MeshNode;
if (accurateNearestNode)
{
Vector3 closest = Polygon.ClosesPointOnTriangle((Vector3)vertices[node.v1], (Vector3)vertices[node.v2], (Vector3)vertices[node.v3], position);
float dist = ((Vector3)pos - closest).sqrMagnitude;
if (minNode == null || dist < minDist)
{
minDist = dist;
minNode = node;
}
if (dist < maxDistSqr && constraint.Suitable(node))
{
if (minConstNode == null || dist < minConstDist)
{
minConstDist = dist;
minConstNode = node;
}
}
}
else
{
if (!Polygon.IsClockwise(vertices[node.v1], vertices[node.v2], pos) || !Polygon.IsClockwise(vertices[node.v2], vertices[node.v3], pos) || !Polygon.IsClockwise(vertices[node.v3], vertices[node.v1], pos))
{
float dist = (node.position - pos).sqrMagnitude;
if (minNode == null || dist < minDist)
{
minDist = dist;
minNode = node;
}
if (dist < maxDistSqr && constraint.Suitable(node))
{
if (minConstNode == null || dist < minConstDist)
{
minConstDist = dist;
minConstNode = node;
}
}
}
else
{
int dist = Mathfx.Abs(node.position.y - pos.y);
if (minNode == null || dist < minDist)
{
minDist = dist;
minNode = node;
}
if (dist < maxDistSqr && constraint.Suitable(node))
{
if (minConstNode == null || dist < minConstDist)
{
minConstDist = dist;
minConstNode = node;
}
}
}
}
}
NNInfo nninfo = new NNInfo(minNode);
//Find the point closest to the nearest triangle
if (nninfo.node != null)
{
MeshNode node = nninfo.node as MeshNode; //minNode2 as MeshNode;
Vector3 clP = Polygon.ClosesPointOnTriangle((Vector3)vertices[node.v1], (Vector3)vertices[node.v2], (Vector3)vertices[node.v3], position);
nninfo.clampedPosition = clP;
}
nninfo.constrainedNode = minConstNode;
if (nninfo.constrainedNode != null)
{
MeshNode node = nninfo.constrainedNode as MeshNode; //minNode2 as MeshNode;
Vector3 clP = Polygon.ClosesPointOnTriangle((Vector3)vertices[node.v1], (Vector3)vertices[node.v2], (Vector3)vertices[node.v3], position);
nninfo.constClampedPosition = clP;
}
return(nninfo);
}
public void RecalculateCosts()
{
if (pivots == null)
{
RecalculatePivots();
}
if (mode == HeuristicOptimizationMode.None)
{
return;
}
pivotCount = 0;
for (int i = 0; i < pivots.Length; i++)
{
if (pivots[i] != null && (pivots[i].Destroyed || !pivots[i].Walkable))
{
throw new System.Exception("Invalid pivot nodes (destroyed or unwalkable)");
}
}
if (mode != HeuristicOptimizationMode.RandomSpreadOut)
{
for (int i = 0; i < pivots.Length; i++)
{
if (pivots[i] == null)
{
throw new System.Exception("Invalid pivot nodes (null)");
}
}
}
UnityEngine.Debug.Log("Recalculating costs...");
pivotCount = pivots.Length;
System.Action <int> startCostCalculation = null;
startCostCalculation = delegate(int k) {
GraphNode pivot = pivots[k];
FloodPath fp = null;
fp = FloodPath.Construct(pivot);
fp.immediateCallback = delegate(Path _p) {
// Handle path pooling
_p.Claim(this);
// When paths are calculated on navmesh based graphs
// the costs are slightly modified to match the actual target and start points
// instead of the node centers
// so we have to remove the cost for the first and last connection
// in each path
MeshNode mn = pivot as MeshNode;
uint costOffset = 0;
if (mn != null && mn.connectionCosts != null)
{
for (int i = 0; i < mn.connectionCosts.Length; i++)
{
costOffset = System.Math.Max(costOffset, mn.connectionCosts[i]);
}
}
var graphs = AstarPath.active.graphs;
// Process graphs in reverse order to raise probability that we encounter large NodeIndex values quicker
// to avoid resizing the internal array too often
for (int j = graphs.Length - 1; j >= 0; j--)
{
graphs[j].GetNodes(delegate(GraphNode node) {
int idx = node.NodeIndex * pivotCount + k;
EnsureCapacity(idx);
PathNode pn = fp.pathHandler.GetPathNode(node);
if (costOffset > 0)
{
costs[idx] = pn.pathID == fp.pathID && pn.parent != null ? System.Math.Max(pn.parent.G - costOffset, 0) : 0;
}
else
{
costs[idx] = pn.pathID == fp.pathID ? pn.G : 0;
}
return(true);
});
}
if (mode == HeuristicOptimizationMode.RandomSpreadOut && k < pivots.Length - 1)
{
int best = -1;
uint bestScore = 0;
// Actual number of nodes
int totCount = maxNodeIndex / pivotCount;
// Loop through all nodes
for (int j = 1; j < totCount; j++)
{
// Find the minimum distance from the node to all existing pivot points
uint mx = 1 << 30;
for (int p = 0; p <= k; p++)
{
mx = System.Math.Min(mx, costs[j * pivotCount + p]);
}
// Pick the node which has the largest minimum distance to the existing pivot points
// (i.e pick the one furthest away from the existing ones)
GraphNode node = fp.pathHandler.GetPathNode(j).node;
if ((mx > bestScore || best == -1) && node != null && !node.Destroyed && node.Walkable)
{
best = j;
bestScore = mx;
}
}
if (best == -1)
{
Debug.LogError("Failed generating random pivot points for heuristic optimizations");
return;
}
pivots[k + 1] = fp.pathHandler.GetPathNode(best).node;
Debug.Log("Found node at " + pivots[k + 1].position + " with score " + bestScore);
startCostCalculation(k + 1);
}
// Handle path pooling
_p.Release(this);
};
AstarPath.StartPath(fp, true);
};
if (mode != HeuristicOptimizationMode.RandomSpreadOut)
{
// All calculated in paralell
for (int i = 0; i < pivots.Length; i++)
{
startCostCalculation(i);
}
}
else
{
// Recursive and serial
startCostCalculation(0);
}
dirty = false;
}
public static void BuildFunnelCorridor(INavmesh graph, Node[] path, int startIndex, int endIndex, List <Vector3> left, List <Vector3> right)
{
if (graph == null)
{
Debug.LogError("Couldn't cast graph to the appropriate type (graph isn't a Navmesh type graph, it doesn't implement the INavmesh interface)");
return;
}
Int3[] vertices = graph.vertices;
int lastLeftIndex = -1;
int lastRightIndex = -1;
for (int i = startIndex; i < endIndex; i++)
{
//Find the connection between the nodes
MeshNode n1 = path[i] as MeshNode;
MeshNode n2 = path[i + 1] as MeshNode;
bool foundFirst = false;
int first = -1;
int second = -1;
for (int x = 0; x < 3; x++)
{
//Vector3 vertice1 = vertices[n1.vertices[x]];
int vertice1 = n1.GetVertexIndex(x);
for (int y = 0; y < 3; y++)
{
//Vector3 vertice2 = vertices[n2.vertices[y]];
int vertice2 = n2.GetVertexIndex(y);
if (vertice1 == vertice2)
{
if (foundFirst)
{
second = vertice2;
break;
}
else
{
first = vertice2;
foundFirst = true;
}
}
}
}
if (first == -1 || second == -1)
{
left.Add((Vector3)n1.position);
right.Add((Vector3)n1.position);
left.Add((Vector3)n2.position);
right.Add((Vector3)n2.position);
lastLeftIndex = first;
lastRightIndex = second;
}
else
{
//Debug.DrawLine ((Vector3)vertices[first]+Vector3.up*0.1F,(Vector3)vertices[second]+Vector3.up*0.1F,Color.cyan);
//Debug.Log (first+" "+second);
if (first == lastLeftIndex)
{
left.Add((Vector3)vertices[first]);
right.Add((Vector3)vertices[second]);
lastLeftIndex = first;
lastRightIndex = second;
}
else if (first == lastRightIndex)
{
left.Add((Vector3)vertices[second]);
right.Add((Vector3)vertices[first]);
lastLeftIndex = second;
lastRightIndex = first;
}
else if (second == lastLeftIndex)
{
left.Add((Vector3)vertices[second]);
right.Add((Vector3)vertices[first]);
lastLeftIndex = second;
lastRightIndex = first;
}
else
{
left.Add((Vector3)vertices[first]);
right.Add((Vector3)vertices[second]);
lastLeftIndex = first;
lastRightIndex = second;
}
}
}
}
public BBTreeBox(IntRect rect)
{
this.node = null;
this.rect = rect;
this.left = (this.right = -1);
}
public static void UpdateArea(GraphUpdateObject o, NavGraph graph)
{
INavmesh navgraph = graph as INavmesh;
if (navgraph == null)
{
Debug.LogError("Update Area on NavMesh must be called with a graph implementing INavmesh"); return;
}
if (graph.nodes == null || graph.nodes.Length == 0)
{
Debug.LogError("NavGraph hasn't been generated yet or does not contain any nodes");
return; // new NNInfo ();
}
//System.DateTime startTime = System.DateTime.Now;
Bounds bounds = o.bounds;
Rect r = Rect.MinMaxRect(bounds.min.x, bounds.min.z, bounds.max.x, bounds.max.z);
Vector3 a = new Vector3(r.xMin, 0, r.yMin); // -1 -1
Vector3 b = new Vector3(r.xMin, 0, r.yMax); // -1 1
Vector3 c = new Vector3(r.xMax, 0, r.yMin); // 1 -1
Vector3 d = new Vector3(r.xMax, 0, r.yMax); // 1 1
for (int i = 0; i < graph.nodes.Length; i++)
{
MeshNode node = graph.nodes[i] as MeshNode;
bool inside = false;
int allLeft = 0;
int allRight = 0;
int allTop = 0;
int allBottom = 0;
for (int v = 0; v < 3; v++)
{
Vector3 vert = (Vector3)navgraph.vertices[node[v]];
Vector2 vert2D = new Vector2(vert.x, vert.z);
if (r.Contains(vert2D))
{
//Debug.DrawRay (vert,Vector3.up,Color.yellow);
inside = true;
break;
}
if (vert.x < r.xMin)
{
allLeft++;
}
if (vert.x > r.xMax)
{
allRight++;
}
if (vert.z < r.yMin)
{
allTop++;
}
if (vert.z > r.yMax)
{
allBottom++;
}
//if (!bounds.Contains (node[v]) {
// inside = false;
// break;
//}
}
if (!inside)
{
if (allLeft == 3 || allRight == 3 || allTop == 3 || allBottom == 3)
{
continue;
}
}
for (int v = 0; v < 3; v++)
{
int v2 = v > 1 ? 0 : v + 1;
Vector3 vert1 = (Vector3)navgraph.vertices[node[v]];
Vector3 vert2 = (Vector3)navgraph.vertices[node[v2]];
if (Polygon.Intersects(a, b, vert1, vert2))
{
inside = true; break;
}
if (Polygon.Intersects(a, c, vert1, vert2))
{
inside = true; break;
}
if (Polygon.Intersects(c, d, vert1, vert2))
{
inside = true; break;
}
if (Polygon.Intersects(d, b, vert1, vert2))
{
inside = true; break;
}
}
if (!inside && ContainsPoint(node, a, navgraph.vertices))
{
inside = true;
} //Debug.DrawRay (a+Vector3.right*0.01F*i,Vector3.up,Color.red); }
if (!inside && ContainsPoint(node, b, navgraph.vertices))
{
inside = true;
} //Debug.DrawRay (b+Vector3.right*0.01F*i,Vector3.up,Color.red); }
if (!inside && ContainsPoint(node, c, navgraph.vertices))
{
inside = true;
} //Debug.DrawRay (c+Vector3.right*0.01F*i,Vector3.up,Color.red); }
if (!inside && ContainsPoint(node, d, navgraph.vertices))
{
inside = true;
} //Debug.DrawRay (d+Vector3.right*0.01F*i,Vector3.up,Color.red); }
if (!inside)
{
continue;
}
o.WillUpdateNode(node);
o.Apply(node);
//Debug.DrawLine (vertices[node.v1],vertices[node.v2],Color.blue);
//Debug.DrawLine (vertices[node.v2],vertices[node.v3],Color.blue);
//Debug.DrawLine (vertices[node.v3],vertices[node.v1],Color.blue);
//Debug.Break ();
}
//System.DateTime endTime = System.DateTime.Now;
//float theTime = (endTime-startTime).Ticks*0.0001F;
//Debug.Log ("Intersecting bounds with navmesh took "+theTime.ToString ("0.000")+" ms");
}
public void InternalOnPostScan () {
if ( AstarPath.active.astarData.pointGraph == null ) {
AstarPath.active.astarData.AddGraph ( new PointGraph () );
}
//Get nearest nodes from the first point graph, assuming both start and end transforms are nodes
startNode = AstarPath.active.astarData.pointGraph.AddNode ( new NodeLink3Node(AstarPath.active), (Int3)StartTransform.position );//AstarPath.active.astarData.pointGraph.GetNearest(StartTransform.position).node as PointNode;
startNode.link = this;
endNode = AstarPath.active.astarData.pointGraph.AddNode ( new NodeLink3Node(AstarPath.active), (Int3)EndTransform.position ); //AstarPath.active.astarData.pointGraph.GetNearest(EndTransform.position).node as PointNode;
endNode.link = this;
connectedNode1 = null;
connectedNode2 = null;
if (startNode == null || endNode == null) {
startNode = null;
endNode = null;
return;
}
postScanCalled = true;
reference[startNode] = this;
reference[endNode] = this;
Apply( true );
}
/** Returns the closest point of the node */
public static Vector3 ClosestPointOnNode(MeshNode node, Int3[] vertices, Vector3 pos)
{
return(Polygon.ClosesPointOnTriangle((Vector3)vertices[node[0]], (Vector3)vertices[node[1]], (Vector3)vertices[node[2]], pos));
}
int RebuildFromInternal (MeshNode[] nodes, int from, int to, bool odd) {
if (to - from <= 0) throw new ArgumentException();
if (to - from == 1) {
return GetBox(nodes[from]);
}
var rect = NodeBounds(nodes, from, to);
int box = GetBox(rect);
// Performance optimization for a common case
if (to - from == 2) {
arr[box].left = GetBox(nodes[from]);
arr[box].right = GetBox(nodes[from+1]);
return box;
}
int mx;
if (odd) {
// X
int divider = (rect.xmin + rect.xmax)/2;
mx = SplitByX (nodes, from, to, divider);
} else {
// Y/Z
int divider = (rect.ymin + rect.ymax)/2;
mx = SplitByZ (nodes, from, to, divider);
}
if (mx == from || mx == to) {
// All nodes were on one side of the divider
// Try to split along the other axis
if (!odd) {
// X
int divider = (rect.xmin + rect.xmax)/2;
mx = SplitByX (nodes, from, to, divider);
} else {
// Y/Z
int divider = (rect.ymin + rect.ymax)/2;
mx = SplitByZ (nodes, from, to, divider);
}
if (mx == from || mx == to) {
// All nodes were on one side of the divider
// Just pick one half
mx = (from+to)/2;
}
}
arr[box].left = RebuildFromInternal(nodes, from, mx, !odd);
arr[box].right = RebuildFromInternal(nodes, mx, to, !odd);
return box;
}
/** Generates a navmesh. Based on the supplied vertices and triangles. Memory usage is about O(n) */
public static void GenerateNodes(NavGraph graph, Vector3[] vectorVertices, int[] triangles, out Vector3[] originalVertices, out Int3[] vertices)
{
if (!(graph is INavmesh))
{
Debug.LogError("The specified graph does not implement interface 'INavmesh'");
originalVertices = vectorVertices;
vertices = new Int3[0];
graph.nodes = graph.CreateNodes(0);
return;
}
if (vectorVertices.Length == 0 || triangles.Length == 0)
{
originalVertices = vectorVertices;
vertices = new Int3[0];
graph.nodes = graph.CreateNodes(0);
return;
}
vertices = new Int3[vectorVertices.Length];
//Backup the original vertices
//for (int i=0;i<vectorVertices.Length;i++) {
// vectorVertices[i] = graph.matrix.MultiplyPoint (vectorVertices[i]);
//}
int c = 0;
/*int maxX = 0;
* int maxZ = 0;
*
* //Almost infinity
* int minX = 0xFFFFFFF;
* int minZ = 0xFFFFFFF;*/
for (int i = 0; i < vertices.Length; i++)
{
vertices[i] = (Int3)graph.matrix.MultiplyPoint(vectorVertices[i]);
/*maxX = Mathfx.Max (vertices[i].x, maxX);
* maxZ = Mathfx.Max (vertices[i].z, maxZ);
* minX = Mathfx.Min (vertices[i].x, minX);
* minZ = Mathfx.Min (vertices[i].z, minZ);*/
}
//maxX = maxX-minX;
//maxZ = maxZ-minZ;
Dictionary <Int3, int> hashedVerts = new Dictionary <Int3, int> ();
int[] newVertices = new int[vertices.Length];
for (int i = 0; i < vertices.Length - 1; i++)
{
//int hash = Mathfx.ComputeVertexHash (vertices[i].x,vertices[i].y,vertices[i].z);
//(vertices[i].x-minX)+(vertices[i].z-minX)*maxX+vertices[i].y*maxX*maxZ;
//if (sortedVertices[i] != sortedVertices[i+1]) {
if (!hashedVerts.ContainsKey(vertices[i]))
{
newVertices[c] = i;
hashedVerts.Add(vertices[i], c);
c++;
} // else {
//Debug.Log ("Hash Duplicate "+hash+" "+vertices[i].ToString ());
//}
}
newVertices[c] = vertices.Length - 1;
//int hash2 = (newVertices[c].x-minX)+(newVertices[c].z-minX)*maxX+newVertices[c].y*maxX*maxZ;
//int hash2 = Mathfx.ComputeVertexHash (newVertices[c].x,newVertices[c].y,newVertices[c].z);
if (!hashedVerts.ContainsKey(vertices[newVertices[c]]))
{
hashedVerts.Add(vertices[newVertices[c]], c);
c++;
}
for (int x = 0; x < triangles.Length; x++)
{
Int3 vertex = vertices[triangles[x]];
//int hash3 = (vertex.x-minX)+(vertex.z-minX)*maxX+vertex.y*maxX*maxZ;
//int hash3 = Mathfx.ComputeVertexHash (vertex.x,vertex.y,vertex.z);
//for (int y=0;y<newVertices.Length;y++) {
triangles[x] = hashedVerts[vertex];
}
/*for (int i=0;i<triangles.Length;i += 3) {
*
* Vector3 offset = Vector3.forward*i*0.01F;
* Debug.DrawLine (newVertices[triangles[i]]+offset,newVertices[triangles[i+1]]+offset,Color.blue);
* Debug.DrawLine (newVertices[triangles[i+1]]+offset,newVertices[triangles[i+2]]+offset,Color.blue);
* Debug.DrawLine (newVertices[triangles[i+2]]+offset,newVertices[triangles[i]]+offset,Color.blue);
* }*/
//Debug.Log ("NavMesh - Old vertice count "+vertices.Length+", new vertice count "+c+" "+maxX+" "+maxZ+" "+maxX*maxZ);
Int3[] totalIntVertices = vertices;
vertices = new Int3[c];
originalVertices = new Vector3[c];
for (int i = 0; i < c; i++)
{
vertices[i] = totalIntVertices[newVertices[i]]; //(Int3)graph.matrix.MultiplyPoint (vectorVertices[i]);
originalVertices[i] = (Vector3)vertices[i]; //vectorVertices[newVertices[i]];
}
Node[] nodes = graph.CreateNodes(triangles.Length / 3); //new Node[triangles.Length/3];
graph.nodes = nodes;
for (int i = 0; i < nodes.Length; i++)
{
MeshNode node = (MeshNode)nodes[i]; //new MeshNode ();
node.walkable = true;
node.position = (vertices[triangles[i * 3]] + vertices[triangles[i * 3 + 1]] + vertices[triangles[i * 3 + 2]]) / 3F;
node.v1 = triangles[i * 3];
node.v2 = triangles[i * 3 + 1];
node.v3 = triangles[i * 3 + 2];
if (!Polygon.IsClockwise(vertices[node.v1], vertices[node.v2], vertices[node.v3]))
{
//Debug.DrawLine (vertices[node.v1],vertices[node.v2],Color.red);
//Debug.DrawLine (vertices[node.v2],vertices[node.v3],Color.red);
//Debug.DrawLine (vertices[node.v3],vertices[node.v1],Color.red);
int tmp = node.v1;
node.v1 = node.v3;
node.v3 = tmp;
}
if (Polygon.IsColinear(vertices[node.v1], vertices[node.v2], vertices[node.v3]))
{
Debug.DrawLine((Vector3)vertices[node.v1], (Vector3)vertices[node.v2], Color.red);
Debug.DrawLine((Vector3)vertices[node.v2], (Vector3)vertices[node.v3], Color.red);
Debug.DrawLine((Vector3)vertices[node.v3], (Vector3)vertices[node.v1], Color.red);
}
nodes[i] = node;
}
List <Node> connections = new List <Node> ();
List <int> connectionCosts = new List <int> ();
int identicalError = 0;
for (int i = 0; i < triangles.Length; i += 3)
{
connections.Clear();
connectionCosts.Clear();
//Int3 indices = new Int3(triangles[i],triangles[i+1],triangles[i+2]);
Node node = nodes[i / 3];
for (int x = 0; x < triangles.Length; x += 3)
{
if (x == i)
{
continue;
}
int count = 0;
if (triangles[x] == triangles[i])
{
count++;
}
if (triangles[x + 1] == triangles[i])
{
count++;
}
if (triangles[x + 2] == triangles[i])
{
count++;
}
if (triangles[x] == triangles[i + 1])
{
count++;
}
if (triangles[x + 1] == triangles[i + 1])
{
count++;
}
if (triangles[x + 2] == triangles[i + 1])
{
count++;
}
if (triangles[x] == triangles[i + 2])
{
count++;
}
if (triangles[x + 1] == triangles[i + 2])
{
count++;
}
if (triangles[x + 2] == triangles[i + 2])
{
count++;
}
if (count >= 3)
{
identicalError++;
Debug.DrawLine((Vector3)vertices[triangles[x]], (Vector3)vertices[triangles[x + 1]], Color.red);
Debug.DrawLine((Vector3)vertices[triangles[x]], (Vector3)vertices[triangles[x + 2]], Color.red);
Debug.DrawLine((Vector3)vertices[triangles[x + 2]], (Vector3)vertices[triangles[x + 1]], Color.red);
}
if (count == 2)
{
Node other = nodes[x / 3];
connections.Add(other);
connectionCosts.Add(Mathf.RoundToInt((node.position - other.position).costMagnitude));
}
}
node.connections = connections.ToArray();
node.connectionCosts = connectionCosts.ToArray();
}
if (identicalError > 0)
{
Debug.LogError("One or more triangles are identical to other triangles, this is not a good thing to have in a navmesh\nIncreasing the scale of the mesh might help\nNumber of triangles with error: " + identicalError + "\n");
}
RebuildBBTree(graph);
//Debug.Log ("Graph Generation - NavMesh - Time to compute graph "+((Time.realtimeSinceStartup-startTime)*1000F).ToString ("0")+"ms");
}
/** Inserts a mesh node in the tree */
public void Insert (MeshNode node) {
int boxi = GetBox (node);
// Was set to root
if (boxi == 0) {
return;
}
BBTreeBox box = arr[boxi];
//int depth = 0;
int c = 0;
while (true) {
BBTreeBox cb = arr[c];
cb.rect = ExpandToContain (cb.rect,box.rect);
if (cb.node != null) {
//Is Leaf
cb.left = boxi;
int box2 = GetBox (cb.node);
//BBTreeBox box2 = new BBTreeBox (this,c.node);
//Console.WriteLine ("Inserted "+box.node+", rect "+box.rect.ToString ());
cb.right = box2;
cb.node = null;
//cb.depth++;
//c.rect = c.rect.
arr[c] = cb;
//Debug.Log (depth);
return;
} else {
//depth++;
//cb.depth++;
arr[c] = cb;
int e1 = ExpansionRequired (arr[cb.left].rect,box.rect);// * arr[cb.left].depth;
int e2 = ExpansionRequired (arr[cb.right].rect,box.rect);// * arr[cb.left].depth;
//Choose the rect requiring the least expansion to contain box.rect
if (e1 < e2) {
c = cb.left;
} else if (e2 < e1) {
c = cb.right;
} else {
//Equal, Choose the one with the smallest area
c = RectArea (arr[cb.left].rect) < RectArea (arr[cb.right].rect) ? cb.left : cb.right;
}
}
}
}
// Token: 0x06000041 RID: 65 RVA: 0x00004F38 File Offset: 0x00003338
public Vector3 Update(Vector3 position, List <Vector3> buffer, int numCorners, out bool lastCorner, out bool requiresRepath)
{
lastCorner = false;
requiresRepath = false;
Int3 @int = (Int3)position;
if (this.nodes[this.currentNode].Destroyed)
{
requiresRepath = true;
lastCorner = false;
buffer.Add(position);
return(position);
}
if (this.nodes[this.currentNode].ContainsPoint(@int))
{
if (this.tmpCounter >= 10)
{
this.tmpCounter = 0;
int i = 0;
int count = this.nodes.Count;
while (i < count)
{
if (this.nodes[i].Destroyed)
{
requiresRepath = true;
break;
}
i++;
}
}
else
{
this.tmpCounter++;
}
}
else
{
bool flag = false;
int num = this.currentNode + 1;
int num2 = Math.Min(this.currentNode + 3, this.nodes.Count);
while (num < num2 && !flag)
{
if (this.nodes[num].Destroyed)
{
requiresRepath = true;
lastCorner = false;
buffer.Add(position);
return(position);
}
if (this.nodes[num].ContainsPoint(@int))
{
this.currentNode = num;
flag = true;
}
num++;
}
int num3 = this.currentNode - 1;
int num4 = Math.Max(this.currentNode - 3, 0);
while (num3 > num4 && !flag)
{
if (this.nodes[num3].Destroyed)
{
requiresRepath = true;
lastCorner = false;
buffer.Add(position);
return(position);
}
if (this.nodes[num3].ContainsPoint(@int))
{
this.currentNode = num3;
flag = true;
}
num3--;
}
int num5 = 0;
float num6 = float.PositiveInfinity;
Vector3 vector = Vector3.zero;
int num7 = 0;
int count2 = this.nodes.Count;
while (num7 < count2 && !flag)
{
if (this.nodes[num7].Destroyed)
{
requiresRepath = true;
lastCorner = false;
buffer.Add(position);
return(position);
}
if (this.nodes[num7].ContainsPoint(@int))
{
this.currentNode = num7;
flag = true;
vector = position;
}
else
{
Vector3 vector2 = this.nodes[num7].ClosestPointOnNodeXZ(position);
float sqrMagnitude = (vector2 - position).sqrMagnitude;
if (sqrMagnitude < num6)
{
num6 = sqrMagnitude;
num5 = num7;
vector = vector2;
}
}
num7++;
}
this.tmpCounter = 0;
int j = 0;
int count3 = this.nodes.Count;
while (j < count3)
{
if (this.nodes[j].Destroyed)
{
requiresRepath = true;
break;
}
j++;
}
if (!flag)
{
vector.y = position.y;
MeshNode containingPoint = null;
int containingIndex = this.nodes.Count - 1;
Int3 i3Copy = @int;
GraphNodeDelegate del = delegate(GraphNode node)
{
if ((containingIndex <= 0 || node != this.nodes[containingIndex - 1]) && (containingIndex >= this.nodes.Count - 1 || node != this.nodes[containingIndex + 1]))
{
MeshNode meshNode2 = node as MeshNode;
if (meshNode2 != null && meshNode2.ContainsPoint(i3Copy))
{
containingPoint = meshNode2;
}
}
};
while (containingIndex >= 0 && containingPoint == null)
{
MeshNode meshNode = this.nodes[containingIndex];
meshNode.GetConnections(del);
containingIndex--;
}
if (containingPoint != null)
{
containingIndex++;
this.exactStart = position;
this.UpdateFunnelCorridor(containingIndex, containingPoint as TriangleMeshNode);
this.currentNode = 0;
}
else
{
position = vector;
this.currentNode = num5;
}
}
}
this.currentPosition = position;
if (!this.FindNextCorners(position, this.currentNode, buffer, numCorners, out lastCorner))
{
Debug.LogError("Oh oh");
buffer.Add(position);
return(position);
}
return(position);
}
public BBTreeBox (IntRect rect) {
node = null;
this.rect = rect;
left = right = -1;
}
public Vector3 Update(Vector3 position, List <Vector3> buffer, int numCorners, out bool lastCorner, out bool requiresRepath)
{
lastCorner = false;
requiresRepath = false;
Int3 i3Pos = (Int3)position;
if (nodes[currentNode].Destroyed)
{
requiresRepath = true;
lastCorner = false;
buffer.Add(position);
return(position);
}
if (nodes[currentNode].ContainsPoint(i3Pos))
{
// Only check for destroyed nodes every 10 frames
if (tmpCounter >= 10)
{
tmpCounter = 0;
for (int i = 0, t = nodes.Count; i < t; i++)
{
if (nodes[i].Destroyed)
{
requiresRepath = true;
break;
}
}
}
else
{
tmpCounter++;
}
}
else
{
bool found = false;
for (int i = currentNode + 1, t = System.Math.Min(currentNode + 3, nodes.Count); i < t && !found; i++)
{
if (nodes[i].Destroyed)
{
requiresRepath = true;
lastCorner = false;
buffer.Add(position);
return(position);
}
if (nodes[i].ContainsPoint(i3Pos))
{
currentNode = i;
found = true;
}
}
for (int i = currentNode - 1, t = System.Math.Max(currentNode - 3, 0); i > t && !found; i--)
{
if (nodes[i].Destroyed)
{
requiresRepath = true;
lastCorner = false;
buffer.Add(position);
return(position);
}
if (nodes[i].ContainsPoint(i3Pos))
{
currentNode = i;
found = true;
}
}
int closest = 0;
float closestDist = float.PositiveInfinity;
Vector3 closestPoint = Vector3.zero;
for (int i = 0, t = nodes.Count; i < t && !found; i++)
{
if (nodes[i].Destroyed)
{
requiresRepath = true;
lastCorner = false;
buffer.Add(position);
return(position);
}
if (nodes[i].ContainsPoint(i3Pos))
{
currentNode = i;
found = true;
closestPoint = position;
}
else
{
Vector3 close = nodes[i].ClosestPointOnNodeXZ(position);
float d = (close - position).sqrMagnitude;
if (d < closestDist)
{
closestDist = d;
closest = i;
closestPoint = close;
}
}
}
tmpCounter = 0;
for (int i = 0, t = nodes.Count; i < t; i++)
{
if (nodes[i].Destroyed)
{
requiresRepath = true;
break;
}
}
if (!found)
{
//Debug.DrawLine (position,closestPoint,Color.cyan);
//Debug.DrawRay (closestPoint,Vector3.up,Color.cyan);
//Debug.Break();
closestPoint.y = position.y;
MeshNode nd;
MeshNode containingPoint = null;
int containingIndex = nodes.Count - 1;
// Need to make a copy here, the JIT will move this variable to the heap
// because it is used inside a delegate, if we didn't make a copy here
// we would /always/ allocate 24 bytes (sizeof(i3Pos)) on the heap every time
// this method was called
// now we only do it when this IF statement is executed
Int3 i3Copy = i3Pos;
GraphNodeDelegate del = delegate(GraphNode node) {
if (!(containingIndex > 0 && node == nodes[containingIndex - 1]) && !(containingIndex < nodes.Count - 1 && node == nodes[containingIndex + 1]))
{
MeshNode mn = node as MeshNode;
if (mn != null && mn.ContainsPoint(i3Copy))
{
containingPoint = mn;
}
}
};
for (; containingIndex >= 0 && containingPoint == null; containingIndex--)
{
nd = nodes[containingIndex];
nd.GetConnections(del);
}
if (containingPoint != null)
{
// It will have been decremented once after containingPoint was null, revert that
containingIndex++;
// We have found a node containing the position, but it is outside the funnel
// Recalculate the funnel to include this node
exactStart = position;
UpdateFunnelCorridor(containingIndex, containingPoint as TriangleMeshNode);
currentNode = 0;
found = true;
}
else
{
position = closestPoint;
found = true;
currentNode = closest;
}
}
}
currentPosition = position;
//Debug.DrawLine ((Vector3)graph.vertices[nodes[currentNode].v1] + Vector3.up*0.1f,(Vector3)graph.vertices[nodes[currentNode].v2] + Vector3.up*0.1f,Color.red);
//Debug.DrawLine ((Vector3)graph.vertices[nodes[currentNode].v2] + Vector3.up*0.1f,(Vector3)graph.vertices[nodes[currentNode].v3] + Vector3.up*0.1f,Color.red);
//Debug.DrawLine ((Vector3)graph.vertices[nodes[currentNode].v3] + Vector3.up*0.1f,(Vector3)graph.vertices[nodes[currentNode].v1] + Vector3.up*0.1f,Color.red);
if (!FindNextCorners(position, currentNode, buffer, numCorners, out lastCorner))
{
Debug.LogError("Oh oh");
buffer.Add(position);
return(position);
}
return(position);
//Debug.Log("Nearest " + w1.Elapsed.TotalMilliseconds*1000);
//Debug.Log("Funnel " + w2.Elapsed.TotalMilliseconds*1000);
}
public void RecalculateCosts()
{
if (this.pivots == null)
{
this.RecalculatePivots();
}
if (this.mode == HeuristicOptimizationMode.None)
{
return;
}
this.pivotCount = 0;
for (int i = 0; i < this.pivots.Length; i++)
{
if (this.pivots[i] != null && (this.pivots[i].Destroyed || !this.pivots[i].Walkable))
{
throw new Exception("Invalid pivot nodes (destroyed or unwalkable)");
}
}
if (this.mode != HeuristicOptimizationMode.RandomSpreadOut)
{
for (int j = 0; j < this.pivots.Length; j++)
{
if (this.pivots[j] == null)
{
throw new Exception("Invalid pivot nodes (null)");
}
}
}
Debug.Log("Recalculating costs...");
this.pivotCount = this.pivots.Length;
Action <int> startCostCalculation = null;
int numComplete = 0;
OnPathDelegate onComplete = delegate(Path path)
{
int numComplete = numComplete;
numComplete++;
if (numComplete == this.pivotCount)
{
Debug.Log("Grid graph special case!");
this.ApplyGridGraphEndpointSpecialCase();
}
};
startCostCalculation = delegate(int pivotIndex)
{
GraphNode pivot = this.pivots[pivotIndex];
FloodPath floodPath = null;
floodPath = FloodPath.Construct(pivot, onComplete);
floodPath.immediateCallback = delegate(Path _p)
{
_p.Claim(this);
MeshNode meshNode = pivot as MeshNode;
uint costOffset = 0u;
if (meshNode != null && meshNode.connections != null)
{
for (int l = 0; l < meshNode.connections.Length; l++)
{
costOffset = Math.Max(costOffset, meshNode.connections[l].cost);
}
}
NavGraph[] graphs = AstarPath.active.graphs;
Action <GraphNode> < > 9__3;
for (int m = graphs.Length - 1; m >= 0; m--)
{
NavGraph navGraph = graphs[m];
Action <GraphNode> action;
if ((action = < > 9__3) == null)
{
action = (< > 9__3 = delegate(GraphNode node)
{
int num6 = node.NodeIndex * this.pivotCount + pivotIndex;
this.EnsureCapacity(num6);
PathNode pathNode = ((IPathInternals)floodPath).PathHandler.GetPathNode(node);
if (costOffset > 0u)
{
this.costs[num6] = ((pathNode.pathID == floodPath.pathID && pathNode.parent != null) ? Math.Max(pathNode.parent.G - costOffset, 0u) : 0u);
return;
}
this.costs[num6] = ((pathNode.pathID == floodPath.pathID) ? pathNode.G : 0u);
});
}
navGraph.GetNodes(action);
}
if (this.mode == HeuristicOptimizationMode.RandomSpreadOut && pivotIndex < this.pivots.Length - 1)
{
if (this.pivots[pivotIndex + 1] == null)
{
int num = -1;
uint num2 = 0u;
int num3 = this.maxNodeIndex / this.pivotCount;
for (int n = 1; n < num3; n++)
{
uint num4 = 1073741824u;
for (int num5 = 0; num5 <= pivotIndex; num5++)
{
num4 = Math.Min(num4, this.costs[n * this.pivotCount + num5]);
}
GraphNode node2 = ((IPathInternals)floodPath).PathHandler.GetPathNode(n).node;
if ((num4 > num2 || num == -1) && node2 != null && !node2.Destroyed && node2.Walkable)
{
num = n;
num2 = num4;
}
}
if (num == -1)
{
Debug.LogError("Failed generating random pivot points for heuristic optimizations");
return;
}
this.pivots[pivotIndex + 1] = ((IPathInternals)floodPath).PathHandler.GetPathNode(num).node;
}
startCostCalculation(pivotIndex + 1);
}
_p.Release(this, false);
};
AstarPath.StartPath(floodPath, true);
};
if (this.mode != HeuristicOptimizationMode.RandomSpreadOut)
{
for (int k = 0; k < this.pivots.Length; k++)
{
startCostCalculation(k);
}
}
else
{
startCostCalculation(0);
}
this.dirty = false;
}
public void RebuildFrom(MeshNode[] nodes)
{
this.Clear();
if (nodes.Length == 0)
{
return;
}
if (nodes.Length == 1)
{
this.GetBox(nodes[0]);
return;
}
this.EnsureCapacity(Mathf.CeilToInt((float)nodes.Length * 2.1f));
MeshNode[] array = new MeshNode[nodes.Length];
for (int i = 0; i < nodes.Length; i++)
{
array[i] = nodes[i];
}
this.RebuildFromInternal(array, 0, nodes.Length, false);
}
static int SplitByX (MeshNode[] nodes, int from, int to, int divider) {
int mx = to;
for (int i = from; i < mx; i++) {
if (nodes[i].position.x > divider) {
// swap with mx
mx--;
var tmp = nodes[mx];
nodes[mx] = nodes[i];
nodes[i] = tmp;
i--;
}
}
return mx;
}
private static IntRect NodeBounds(MeshNode[] nodes, int from, int to)
{
if (to - from <= 0)
{
throw new ArgumentException();
}
Int3 vertex = nodes[from].GetVertex(0);
Int2 @int = new Int2(vertex.x, vertex.z);
Int2 int2 = @int;
for (int i = from; i < to; i++)
{
MeshNode meshNode = nodes[i];
for (int j = 1; j < meshNode.GetVertexCount(); j++)
{
Int3 vertex2 = meshNode.GetVertex(j);
@int.x = Math.Min(@int.x, vertex2.x);
@int.y = Math.Min(@int.y, vertex2.z);
int2.x = Math.Max(int2.x, vertex2.x);
int2.y = Math.Max(int2.y, vertex2.z);
}
}
return new IntRect(@int.x, @int.y, int2.x, int2.y);
}
public BBTreeBox(BBTree tree, MeshNode node)
{
this.node = node;
Vector3 first = (Vector3)tree.graph.vertices[node[0]];
Vector2 min = new Vector2(first.x,first.z);
Vector2 max = min;
for (int i=1;i<3;i++) {
Vector3 p = (Vector3)tree.graph.vertices[node[i]];
min.x = Mathf.Min (min.x,p.x);
min.y = Mathf.Min (min.y,p.z);
max.x = Mathf.Max (max.x,p.x);
max.y = Mathf.Max (max.y,p.z);
}
rect = Rect.MinMaxRect (min.x,min.y,max.x,max.y);
}
private static bool NodeIntersectsCircle(MeshNode node, Vector3 p, float radius)
{
return float.IsPositiveInfinity(radius) || (p - node.ClosestPointOnNode(p)).sqrMagnitude < radius * radius;
}
public void InternalOnPostScan () {
if ( EndTransform == null || StartTransform == null ) return;
#if !ASTAR_NO_POINT_GRAPH
if ( AstarPath.active.astarData.pointGraph == null ) {
AstarPath.active.astarData.AddGraph ( new PointGraph () );
}
#endif
if ( startNode != null) {
NodeLink2 tmp;
if (reference.TryGetValue (startNode, out tmp) && tmp == this) reference.Remove (startNode);
}
if ( endNode != null) {
NodeLink2 tmp;
if (reference.TryGetValue (endNode, out tmp) && tmp == this) reference.Remove (endNode);
}
#if !ASTAR_NO_POINT_GRAPH
//Get nearest nodes from the first point graph, assuming both start and end transforms are nodes
startNode = AstarPath.active.astarData.pointGraph.AddNode ( (Int3)StartTransform.position );//AstarPath.active.astarData.pointGraph.GetNearest(StartTransform.position).node as PointNode;
endNode = AstarPath.active.astarData.pointGraph.AddNode ( (Int3)EndTransform.position ); //AstarPath.active.astarData.pointGraph.GetNearest(EndTransform.position).node as PointNode;
#else
throw new System.Exception ("Point graph is not included. Check your A* optimization settings.");
#endif
connectedNode1 = null;
connectedNode2 = null;
if (startNode == null || endNode == null) {
startNode = null;
endNode = null;
return;
}
postScanCalled = true;
reference[startNode] = this;
reference[endNode] = this;
Apply( true );
}
private static int SplitByZ(MeshNode[] nodes, int from, int to, int divider)
{
int num = to;
for (int i = from; i < num; i++)
{
if (nodes[i].position.z > divider)
{
num--;
MeshNode meshNode = nodes[num];
nodes[num] = nodes[i];
nodes[i] = meshNode;
i--;
}
}
return num;
}
/** Returns the closest point of the node */
public static Vector3 ClosestPointOnNode (MeshNode node, Int3[] vertices, Vector3 pos) {
return Polygon.ClosesPointOnTriangle (vertices[node[0]],vertices[node[1]],vertices[node[2]],pos);
}
private int GetBox(MeshNode node)
{
if (this.count >= this.arr.Length)
{
this.EnsureCapacity(this.count + 1);
}
this.arr[this.count] = new BBTree.BBTreeBox(node);
this.count++;
return this.count - 1;
}
public override void OnGraphsPostUpdate () {
//if (connectedNode1 != null && connectedNode2 != null) {
if (!AstarPath.active.isScanning) {
if (connectedNode1 != null && connectedNode1.Destroyed) {
connectedNode1 = null;
}
if (connectedNode2 != null && connectedNode2.Destroyed) {
connectedNode2 = null;
}
if (!postScanCalled) {
OnPostScan();
} else {
//OnPostScan will also call this method
/** \todo Can mess up pathfinding, wrap in delegate */
Apply( false );
}
}
}
private int RebuildFromInternal(MeshNode[] nodes, int from, int to, bool odd)
{
if (to - from <= 0)
{
throw new ArgumentException();
}
if (to - from == 1)
{
return this.GetBox(nodes[from]);
}
IntRect rect = BBTree.NodeBounds(nodes, from, to);
int box = this.GetBox(rect);
if (to - from == 2)
{
this.arr[box].left = this.GetBox(nodes[from]);
this.arr[box].right = this.GetBox(nodes[from + 1]);
return box;
}
int num;
if (odd)
{
int divider = (rect.xmin + rect.xmax) / 2;
num = BBTree.SplitByX(nodes, from, to, divider);
}
else
{
int divider2 = (rect.ymin + rect.ymax) / 2;
num = BBTree.SplitByZ(nodes, from, to, divider2);
}
if (num == from || num == to)
{
if (!odd)
{
int divider3 = (rect.xmin + rect.xmax) / 2;
num = BBTree.SplitByX(nodes, from, to, divider3);
}
else
{
int divider4 = (rect.ymin + rect.ymax) / 2;
num = BBTree.SplitByZ(nodes, from, to, divider4);
}
if (num == from || num == to)
{
num = (from + to) / 2;
}
}
this.arr[box].left = this.RebuildFromInternal(nodes, from, num, !odd);
this.arr[box].right = this.RebuildFromInternal(nodes, num, to, !odd);
return box;
}
public void InternalOnPostScan () {
if ( EndTransform == null || StartTransform == null ) return;
if ( AstarPath.active.astarData.pointGraph == null ) {
AstarPath.active.astarData.AddGraph ( new PointGraph () );
}
if ( startNode != null) {
NodeLink2 tmp;
if (reference.TryGetValue (startNode, out tmp) && tmp == this) reference.Remove (startNode);
}
if ( endNode != null) {
NodeLink2 tmp;
if (reference.TryGetValue (endNode, out tmp) && tmp == this) reference.Remove (endNode);
}
//Get nearest nodes from the first point graph, assuming both start and end transforms are nodes
startNode = AstarPath.active.astarData.pointGraph.AddNode ( (Int3)StartTransform.position );//AstarPath.active.astarData.pointGraph.GetNearest(StartTransform.position).node as PointNode;
endNode = AstarPath.active.astarData.pointGraph.AddNode ( (Int3)EndTransform.position ); //AstarPath.active.astarData.pointGraph.GetNearest(EndTransform.position).node as PointNode;
connectedNode1 = null;
connectedNode2 = null;
if (startNode == null || endNode == null) {
startNode = null;
endNode = null;
return;
}
postScanCalled = true;
reference[startNode] = this;
reference[endNode] = this;
Apply( true );
}
public void Apply(bool forceNewCheck)
{
NNConstraint none = NNConstraint.None;
none.distanceXZ = true;
int graphIndex = (int)this.startNode.GraphIndex;
none.graphMask = ~(((int)1) << graphIndex);
bool flag = true;
NNInfo nearest = AstarPath.active.GetNearest(this.StartTransform.position, none);
flag &= (nearest.node == this.connectedNode1) && (nearest.node != null);
this.connectedNode1 = nearest.node as MeshNode;
this.clamped1 = nearest.position;
if (this.connectedNode1 != null)
{
Debug.DrawRay((Vector3)this.connectedNode1.position, (Vector3)(Vector3.up * 5f), Color.red);
}
NNInfo info2 = AstarPath.active.GetNearest(this.EndTransform.position, none);
flag &= (info2.node == this.connectedNode2) && (info2.node != null);
this.connectedNode2 = info2.node as MeshNode;
this.clamped2 = info2.position;
if (this.connectedNode2 != null)
{
Debug.DrawRay((Vector3)this.connectedNode2.position, (Vector3)(Vector3.up * 5f), Color.cyan);
}
if ((this.connectedNode2 != null) && (this.connectedNode1 != null))
{
this.startNode.SetPosition((Int3)this.StartTransform.position);
this.endNode.SetPosition((Int3)this.EndTransform.position);
if (!flag || forceNewCheck)
{
this.RemoveConnections(this.startNode);
this.RemoveConnections(this.endNode);
Int3 num12 = (Int3)(this.StartTransform.position - this.EndTransform.position);
uint cost = (uint)Mathf.RoundToInt(num12.costMagnitude * this.costFactor);
this.startNode.AddConnection(this.endNode, cost);
this.endNode.AddConnection(this.startNode, cost);
Int3 rhs = this.connectedNode2.position - this.connectedNode1.position;
for (int i = 0; i < this.connectedNode1.GetVertexCount(); i++)
{
Int3 vertex = this.connectedNode1.GetVertex(i);
Int3 lineEnd = this.connectedNode1.GetVertex((i + 1) % this.connectedNode1.GetVertexCount());
Int3 num13 = lineEnd - vertex;
if (Int3.DotLong(num13.Normal2D(), rhs) <= 0L)
{
for (int j = 0; j < this.connectedNode2.GetVertexCount(); j++)
{
Int3 point = this.connectedNode2.GetVertex(j);
Int3 num9 = this.connectedNode2.GetVertex((j + 1) % this.connectedNode2.GetVertexCount());
Int3 num14 = num9 - point;
if ((Int3.DotLong(num14.Normal2D(), rhs) >= 0L) && (Int3.Angle(num9 - point, lineEnd - vertex) > 2.9670598109563189))
{
float num10 = 0f;
float num11 = 1f;
num11 = Math.Min(num11, VectorMath.ClosestPointOnLineFactor(vertex, lineEnd, point));
num10 = Math.Max(num10, VectorMath.ClosestPointOnLineFactor(vertex, lineEnd, num9));
if (num11 >= num10)
{
Vector3 vector = ((Vector3)(((Vector3)(lineEnd - vertex)) * num10)) + ((Vector3)vertex);
Vector3 vector2 = ((Vector3)(((Vector3)(lineEnd - vertex)) * num11)) + ((Vector3)vertex);
this.startNode.portalA = vector;
this.startNode.portalB = vector2;
this.endNode.portalA = vector2;
this.endNode.portalB = vector;
Int3 num15 = (Int3)(this.clamped1 - this.StartTransform.position);
this.connectedNode1.AddConnection(this.startNode, (uint)Mathf.RoundToInt(num15.costMagnitude * this.costFactor));
Int3 num16 = (Int3)(this.clamped2 - this.EndTransform.position);
this.connectedNode2.AddConnection(this.endNode, (uint)Mathf.RoundToInt(num16.costMagnitude * this.costFactor));
Int3 num17 = (Int3)(this.clamped1 - this.StartTransform.position);
this.startNode.AddConnection(this.connectedNode1, (uint)Mathf.RoundToInt(num17.costMagnitude * this.costFactor));
Int3 num18 = (Int3)(this.clamped2 - this.EndTransform.position);
this.endNode.AddConnection(this.connectedNode2, (uint)Mathf.RoundToInt(num18.costMagnitude * this.costFactor));
return;
}
Debug.LogError(string.Concat(new object[] { "Wait wut!? ", num10, " ", num11, " ", vertex, " ", lineEnd, " ", point, " ", num9, "\nTODO, fix this error" }));
}
}
}
}
}
}
}
