private void SearchBoxClosest(int boxi, Vector3 p, ref float closestDist, NNConstraint constraint, ref NNInfoInternal nnInfo)
{
BBTree.BBTreeBox bbtreeBox = this.arr[boxi];
if (bbtreeBox.node != null)
{
if (BBTree.NodeIntersectsCircle(bbtreeBox.node, p, closestDist))
{
Vector3 vector = bbtreeBox.node.ClosestPointOnNode(p);
if (constraint == null || constraint.Suitable(bbtreeBox.node))
{
float sqrMagnitude = (vector - p).sqrMagnitude;
if (nnInfo.constrainedNode == null || sqrMagnitude < closestDist * closestDist)
{
nnInfo.constrainedNode = bbtreeBox.node;
nnInfo.constClampedPosition = vector;
closestDist = (float)Math.Sqrt((double)sqrMagnitude);
}
}
}
}
else
{
if (BBTree.RectIntersectsCircle(this.arr[bbtreeBox.left].rect, p, closestDist))
{
this.SearchBoxClosest(bbtreeBox.left, p, ref closestDist, constraint, ref nnInfo);
}
if (BBTree.RectIntersectsCircle(this.arr[bbtreeBox.right].rect, p, closestDist))
{
this.SearchBoxClosest(bbtreeBox.right, p, ref closestDist, constraint, ref nnInfo);
}
}
}
private void SearchBoxClosestXZ(int boxi, Vector3 p, ref float closestDist, NNConstraint constraint, ref NNInfo nnInfo)
{
BBTree.BBTreeBox bBTreeBox = this.arr[boxi];
if (bBTreeBox.node != null)
{
Vector3 constClampedPosition = bBTreeBox.node.ClosestPointOnNodeXZ(p);
if (constraint == null || constraint.Suitable(bBTreeBox.node))
{
float num = (constClampedPosition.x - p.x) * (constClampedPosition.x - p.x) + (constClampedPosition.z - p.z) * (constClampedPosition.z - p.z);
if (nnInfo.constrainedNode == null)
{
nnInfo.constrainedNode = bBTreeBox.node;
nnInfo.constClampedPosition = constClampedPosition;
closestDist = (float)Math.Sqrt((double)num);
}
else if (num < closestDist * closestDist)
{
nnInfo.constrainedNode = bBTreeBox.node;
nnInfo.constClampedPosition = constClampedPosition;
closestDist = (float)Math.Sqrt((double)num);
}
}
}
else
{
if (BBTree.RectIntersectsCircle(this.arr[bBTreeBox.left].rect, p, closestDist))
{
this.SearchBoxClosestXZ(bBTreeBox.left, p, ref closestDist, constraint, ref nnInfo);
}
if (BBTree.RectIntersectsCircle(this.arr[bBTreeBox.right].rect, p, closestDist))
{
this.SearchBoxClosestXZ(bBTreeBox.right, p, ref closestDist, constraint, ref nnInfo);
}
}
}
public static void RebuildBBTree(NavMeshGraph graph)
{
BBTree bBTree = graph.bbTree;
bBTree = (bBTree ?? new BBTree());
bBTree.RebuildFrom(graph.nodes);
graph.bbTree = bBTree;
}
private static int ExpansionRequired(IntRect r, IntRect r2)
{
int num = Math.Min(r.xmin, r2.xmin);
int num2 = Math.Max(r.xmax, r2.xmax);
int num3 = Math.Min(r.ymin, r2.ymin);
int num4 = Math.Max(r.ymax, r2.ymax);
return((num2 - num) * (num4 - num3) - BBTree.RectArea(r));
}
private static float ExpansionRequired(Rect r, Rect r2)
{
float num = Math.Min(r.xMin, r2.xMin);
float num2 = Math.Max(r.xMax, r2.xMax);
float num3 = Math.Min(r.yMin, r2.yMin);
float num4 = Math.Max(r.yMax, r2.yMax);
return((num2 - num) * (num4 - num3) - BBTree.RectArea(r));
}
/** Rebuilds the BBTree on a NavGraph.
* \astarpro
* \see NavMeshGraph.bbTree */
public static void RebuildBBTree(NavMeshGraph graph)
{
// Build Axis Aligned Bounding Box Tree
BBTree bbTree = graph.bbTree;
bbTree = bbTree ?? new BBTree();
bbTree.RebuildFrom(graph.nodes);
graph.bbTree = bbTree;
}
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);
}
private void GetOrderedChildren(ref int first, ref int second, out float firstDist, out float secondDist, Vector3 p)
{
firstDist = BBTree.SquaredRectPointDistance(this.tree[first].rect, p);
secondDist = BBTree.SquaredRectPointDistance(this.tree[second].rect, p);
if (secondDist < firstDist)
{
int num = first;
first = second;
second = num;
float num2 = firstDist;
firstDist = secondDist;
secondDist = num2;
}
}
private int RebuildFromInternal(TriangleMeshNode[] nodes, int[] permutation, IntRect[] nodeBounds, int from, int to, bool odd)
{
IntRect rect = BBTree.NodeBounds(permutation, nodeBounds, from, to);
int box = this.GetBox(rect);
if (to - from <= 4)
{
int num = this.tree[box].nodeOffset = this.leafNodes * 4;
this.EnsureNodeCapacity(num + 4);
this.leafNodes++;
for (int i = 0; i < 4; i++)
{
this.nodeLookup[num + i] = ((i >= to - from) ? null : nodes[permutation[from + i]]);
}
return(box);
}
int num2;
if (odd)
{
int divider = (rect.xmin + rect.xmax) / 2;
num2 = BBTree.SplitByX(nodes, permutation, from, to, divider);
}
else
{
int divider2 = (rect.ymin + rect.ymax) / 2;
num2 = BBTree.SplitByZ(nodes, permutation, from, to, divider2);
}
if (num2 == from || num2 == to)
{
if (!odd)
{
int divider3 = (rect.xmin + rect.xmax) / 2;
num2 = BBTree.SplitByX(nodes, permutation, from, to, divider3);
}
else
{
int divider4 = (rect.ymin + rect.ymax) / 2;
num2 = BBTree.SplitByZ(nodes, permutation, from, to, divider4);
}
if (num2 == from || num2 == to)
{
num2 = (from + to) / 2;
}
}
this.tree[box].left = this.RebuildFromInternal(nodes, permutation, nodeBounds, from, num2, !odd);
this.tree[box].right = this.RebuildFromInternal(nodes, permutation, nodeBounds, num2, to, !odd);
return(box);
}
public NNInfoInternal QueryClosest(Vector3 p, NNConstraint constraint, ref float distance, NNInfoInternal previous)
{
float num = distance * distance;
float num2 = num;
if (this.count > 0 && BBTree.SquaredRectPointDistance(this.tree[0].rect, p) < num)
{
this.SearchBoxClosest(0, p, ref num, constraint, ref previous);
if (num < num2)
{
distance = Mathf.Sqrt(num);
}
}
return(previous);
}
public static void RebuildBBTree(NavMeshGraph graph)
{
BBTree bBTree = graph.bbTree;
if (bBTree == null)
{
bBTree = new BBTree(graph);
}
bBTree.Clear();
TriangleMeshNode[] triNodes = graph.TriNodes;
for (int i = triNodes.Length - 1; i >= 0; i--)
{
bBTree.Insert(triNodes[i]);
}
graph.bbTree = bBTree;
}
public BBTreeBox(BBTree tree, MeshNode node)
{
this.node = node;
Vector3 vector = (Vector3)node.GetVertex(0);
Vector2 vector2 = new Vector2(vector.x, vector.z);
Vector2 vector3 = vector2;
for (int i = 1; i < node.GetVertexCount(); i++)
{
Vector3 vector4 = (Vector3)node.GetVertex(i);
vector2.x = Math.Min(vector2.x, vector4.x);
vector2.y = Math.Min(vector2.y, vector4.z);
vector3.x = Math.Max(vector3.x, vector4.x);
vector3.y = Math.Max(vector3.y, vector4.z);
}
this.rect = Rect.MinMaxRect(vector2.x, vector2.y, vector3.x, vector3.y);
this.left = (this.right = -1);
}
/** Rebuilds the BBTree on a NavGraph.
* \astarpro
* \see NavMeshGraph.bbTree */
public static void RebuildBBTree(NavMeshGraph graph)
{
//Build Axis Aligned Bounding Box Tree
BBTree bbTree = graph.bbTree;
bbTree = bbTree ?? new BBTree(graph);
bbTree.Clear();
var nodes = graph.nodes;
for (int i = nodes.Length - 1; i >= 0; i--)
{
bbTree.Insert(nodes[i]);
}
graph.bbTree = bbTree;
}
private void SearchBoxCircle(int boxi, Vector3 p, float radius, NNConstraint constraint, ref NNInfo nnInfo)
{
BBTree.BBTreeBox bBTreeBox = this.arr[boxi];
if (bBTreeBox.node != null)
{
if (BBTree.NodeIntersectsCircle(bBTreeBox.node, p, radius))
{
Vector3 vector = bBTreeBox.node.ClosestPointOnNode(p);
float sqrMagnitude = (vector - p).sqrMagnitude;
if (nnInfo.node == null)
{
nnInfo.node = bBTreeBox.node;
nnInfo.clampedPosition = vector;
}
else if (sqrMagnitude < (nnInfo.clampedPosition - p).sqrMagnitude)
{
nnInfo.node = bBTreeBox.node;
nnInfo.clampedPosition = vector;
}
if (constraint == null || constraint.Suitable(bBTreeBox.node))
{
if (nnInfo.constrainedNode == null)
{
nnInfo.constrainedNode = bBTreeBox.node;
nnInfo.constClampedPosition = vector;
}
else if (sqrMagnitude < (nnInfo.constClampedPosition - p).sqrMagnitude)
{
nnInfo.constrainedNode = bBTreeBox.node;
nnInfo.constClampedPosition = vector;
}
}
}
return;
}
if (BBTree.RectIntersectsCircle(this.arr[bBTreeBox.left].rect, p, radius))
{
this.SearchBoxCircle(bBTreeBox.left, p, radius, constraint, ref nnInfo);
}
if (BBTree.RectIntersectsCircle(this.arr[bBTreeBox.right].rect, p, radius))
{
this.SearchBoxCircle(bBTreeBox.right, p, radius, constraint, ref nnInfo);
}
}
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 BBTreeBox(BBTree tree, MeshNode node)
{
this.node = node;
Vector3 first = (Vector3)node.GetVertex(0);
Vector2 min = new Vector2(first.x, first.z);
Vector2 max = min;
for (int i = 1; i < node.GetVertexCount(); i++)
{
Vector3 p = (Vector3)node.GetVertex(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);
}
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);
}
public override void DeserializeExtraInfo(GraphSerializationContext ctx)
{
uint graphIndex = ctx.graphIndex;
TriangleMeshNode.SetNavmeshHolder((int)graphIndex, this);
int nodeCount = ctx.reader.ReadInt32();
int vertexCount = ctx.reader.ReadInt32();
if (nodeCount == -1)
{
nodes = new TriangleMeshNode[0];
_vertices = new Int3[0];
originalVertices = new Vector3[0];
return;
}
nodes = new TriangleMeshNode[nodeCount];
_vertices = new Int3[vertexCount];
originalVertices = new Vector3[vertexCount];
for (int i = 0; i < vertexCount; i++)
{
_vertices[i] = ctx.DeserializeInt3();
originalVertices[i] = ctx.DeserializeVector3();
}
bbTree = new BBTree();
for (int i = 0; i < nodeCount; i++)
{
nodes[i] = new TriangleMeshNode(active);
TriangleMeshNode node = nodes[i];
node.DeserializeNode(ctx);
node.UpdatePositionFromVertices();
}
bbTree.RebuildFrom(nodes);
}
private void SearchBox(int boxi, Vector3 p, NNConstraint constraint, ref NNInfo nnInfo)
{
BBTree.BBTreeBox bBTreeBox = this.arr[boxi];
if (bBTreeBox.node != null)
{
if (bBTreeBox.node.ContainsPoint((VInt3)p))
{
if (nnInfo.node == null)
{
nnInfo.node = bBTreeBox.node;
}
else if (Mathf.Abs(((Vector3)bBTreeBox.node.position).y - p.y) < Mathf.Abs(((Vector3)nnInfo.node.position).y - p.y))
{
nnInfo.node = bBTreeBox.node;
}
if (constraint.Suitable(bBTreeBox.node))
{
if (nnInfo.constrainedNode == null)
{
nnInfo.constrainedNode = bBTreeBox.node;
}
else if (Mathf.Abs((float)bBTreeBox.node.position.y - p.y) < Mathf.Abs((float)nnInfo.constrainedNode.position.y - p.y))
{
nnInfo.constrainedNode = bBTreeBox.node;
}
}
}
return;
}
if (BBTree.RectContains(this.arr[bBTreeBox.left].rect, p))
{
this.SearchBox(bBTreeBox.left, p, constraint, ref nnInfo);
}
if (BBTree.RectContains(this.arr[bBTreeBox.right].rect, p))
{
this.SearchBox(bBTreeBox.right, p, constraint, ref nnInfo);
}
}
public override void DeserializeExtraInfo(GraphSerializationContext ctx)
{
uint graphIndex = (uint)ctx.graphIndex;
TriangleMeshNode.SetNavmeshHolder((int)graphIndex, this);
int c1 = ctx.reader.ReadInt32();
int c2 = ctx.reader.ReadInt32();
if (c1 == -1)
{
nodes = new TriangleMeshNode[0];
_vertices = new Int3[0];
originalVertices = new Vector3[0];
}
nodes = new TriangleMeshNode[c1];
_vertices = new Int3[c2];
originalVertices = new Vector3[c2];
for (int i = 0; i < c2; i++)
{
_vertices[i] = new Int3(ctx.reader.ReadInt32(), ctx.reader.ReadInt32(), ctx.reader.ReadInt32());
originalVertices[i] = new Vector3(ctx.reader.ReadSingle(), ctx.reader.ReadSingle(), ctx.reader.ReadSingle());
}
bbTree = new BBTree(this);
for (int i = 0; i < c1; i++)
{
nodes[i] = new TriangleMeshNode(active);
TriangleMeshNode node = nodes[i];
node.DeserializeNode(ctx);
node.UpdatePositionFromVertices();
bbTree.Insert(node);
}
}
/** Rebuilds the BBTree on a NavGraph.
* \astarpro
* \see NavMeshGraph.bbTree */
public static void RebuildBBTree(NavGraph graph)
{
//Build Axis Aligned Bounding Box Tree
INavmesh meshGraph = graph as INavmesh;
BBTree bbTree = new BBTree (graph as INavmesh);
for (int i=0;i<graph.nodes.Length;i++) {
bbTree.Insert (graph.nodes[i] as MeshNode);
}
meshGraph.bbTree = bbTree;
}
public override void DeserializeExtraInfo (GraphSerializationContext ctx) {
uint graphIndex = (uint)ctx.graphIndex;
TriangleMeshNode.SetNavmeshHolder ((int)graphIndex,this);
int nodeCount = ctx.reader.ReadInt32();
int vertexCount = ctx.reader.ReadInt32();
if (nodeCount == -1) {
nodes = new TriangleMeshNode[0];
_vertices = new Int3[0];
originalVertices = new Vector3[0];
}
nodes = new TriangleMeshNode[nodeCount];
_vertices = new Int3[vertexCount];
originalVertices = new Vector3[vertexCount];
for (int i=0;i<vertexCount;i++) {
_vertices[i] = new Int3(ctx.reader.ReadInt32(), ctx.reader.ReadInt32(), ctx.reader.ReadInt32());
originalVertices[i] = new Vector3(ctx.reader.ReadSingle(), ctx.reader.ReadSingle(), ctx.reader.ReadSingle());
}
bbTree = new BBTree();
for (int i = 0; i < nodeCount;i++) {
nodes[i] = new TriangleMeshNode(active);
TriangleMeshNode node = nodes[i];
node.DeserializeNode(ctx);
node.UpdatePositionFromVertices();
}
bbTree.RebuildFrom (nodes);
}
/** Rebuilds the BBTree on a NavGraph.
* \astarpro
* \see NavMeshGraph.bbTree */
public static void RebuildBBTree (NavMeshGraph graph) {
//Build Axis Aligned Bounding Box Tree
NavMeshGraph meshGraph = graph as NavMeshGraph;
BBTree bbTree = new BBTree (graph as INavmeshHolder);
TriangleMeshNode[] nodes = meshGraph.TriNodes;
for (int i=nodes.Length-1;i>=0;i--) {
bbTree.Insert (nodes[i]);
}
meshGraph.bbTree = bbTree;
}
/** Relocates the nodes to match the newMatrix.
* The "oldMatrix" variable can be left out in this function call (only for this graph generator) since it is not used */
public override void RelocateNodes (Matrix4x4 oldMatrix, Matrix4x4 newMatrix) {
//base.RelocateNodes (oldMatrix,newMatrix);
if (vertices == null || vertices.Length == 0 || originalVertices == null || originalVertices.Length != vertices.Length) {
return;
}
for (int i=0;i<_vertices.Length;i++) {
//Vector3 tmp = inv.MultiplyPoint3x4 (vertices[i]);
//vertices[i] = (Int3)newMatrix.MultiplyPoint3x4 (tmp);
_vertices[i] = (Int3)newMatrix.MultiplyPoint3x4 ((Vector3)originalVertices[i]);
}
for (int i=0;i<nodes.Length;i++) {
TriangleMeshNode node = (TriangleMeshNode)nodes[i];
node.UpdatePositionFromVertices();
if (node.connections != null) {
for (int q=0;q<node.connections.Length;q++) {
node.connectionCosts[q] = (uint)(node.position-node.connections[q].position).costMagnitude;
}
}
}
SetMatrix (newMatrix);
// Make a new BBTree
bbTree = new BBTree(this);
for (int i=0;i<nodes.Length;i++) {
bbTree.Insert (nodes[i]);
}
}
public override void DeserializeExtraInfo (GraphSerializationContext ctx)
{
uint graphIndex = (uint)active.astarData.GetGraphIndex(this);
TriangleMeshNode.SetNavmeshHolder ((int)graphIndex,this);
int c1 = ctx.reader.ReadInt32();
int c2 = ctx.reader.ReadInt32();
if (c1 == -1) {
nodes = new TriangleMeshNode[0];
_vertices = new Int3[0];
originalVertices = new Vector3[0];
}
nodes = new TriangleMeshNode[c1];
_vertices = new Int3[c2];
originalVertices = new Vector3[c2];
for (int i=0;i<c2;i++) {
_vertices[i] = new Int3(ctx.reader.ReadInt32(), ctx.reader.ReadInt32(), ctx.reader.ReadInt32());
originalVertices[i] = new Vector3(ctx.reader.ReadSingle(), ctx.reader.ReadSingle(), ctx.reader.ReadSingle());
}
bbTree = new BBTree(this);
for (int i=0;i<c1;i++) {
nodes[i] = new TriangleMeshNode(active);
TriangleMeshNode node = nodes[i];
node.DeserializeNode(ctx);
node.GraphIndex = graphIndex;
node.UpdatePositionFromVertices();
bbTree.Insert (node);
}
}
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);
}
public BBTreeBox (BBTree tree, MeshNode node) {
this.node = node;
Vector3 first = (Vector3)node.GetVertex(0);
Vector2 min = new Vector2(first.x,first.z);
Vector2 max = min;
for (int i=1;i<node.GetVertexCount();i++) {
Vector3 p = (Vector3)node.GetVertex(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);
}
