public static void DeSerializeMeshNodes(INavmesh graph, Node[] nodes, AstarSerializer serializer)
{
System.IO.BinaryReader stream = serializer.readerStream;
for (int i = 0; i < nodes.Length; i++)
{
MeshNode node = nodes[i] as MeshNode;
if (node == null)
{
Debug.LogError("Serialization Error : Couldn't cast the node to the appropriate type - NavMeshGenerator");
return;
}
node.v1 = stream.ReadInt32();
node.v2 = stream.ReadInt32();
node.v3 = stream.ReadInt32();
}
int numVertices = stream.ReadInt32();
graph.vertices = new Int3[numVertices];
for (int i = 0; i < numVertices; i++)
{
int x = stream.ReadInt32();
int y = stream.ReadInt32();
int z = stream.ReadInt32();
graph.vertices[i] = new Int3(x, y, z);
}
RebuildBBTree(graph as NavGraph);
}
//These functions are for serialization, the static ones are there so other graphs using mesh nodes can serialize them more easily
public static void SerializeMeshNodes(INavmesh graph, Node[] nodes, AstarSerializer serializer)
{
System.IO.BinaryWriter stream = serializer.writerStream;
for (int i = 0; i < nodes.Length; i++)
{
MeshNode node = nodes[i] as MeshNode;
if (node == null)
{
Debug.LogError("Serialization Error : Couldn't cast the node to the appropriate type - NavMeshGenerator");
return;
}
stream.Write(node.v1);
stream.Write(node.v2);
stream.Write(node.v3);
}
Int3[] vertices = graph.vertices;
if (vertices == null)
{
vertices = new Int3[0];
}
stream.Write(vertices.Length);
for (int i = 0; i < vertices.Length; i++)
{
stream.Write(vertices[i].x);
stream.Write(vertices[i].y);
stream.Write(vertices[i].z);
}
}
public override void OnLatePostScan()
{
if (!Application.isPlaying)
{
return;
}
this.RemoveObstacles();
NavGraph[] graphs = AstarPath.active.graphs;
RVOSimulator rvosimulator = UnityEngine.Object.FindObjectOfType<RVOSimulator>();
if (rvosimulator == null)
{
throw new NullReferenceException("No RVOSimulator could be found in the scene. Please add one to any GameObject");
}
Simulator simulator = rvosimulator.GetSimulator();
for (int i = 0; i < graphs.Length; i++)
{
RecastGraph recastGraph = graphs[i] as RecastGraph;
if (recastGraph != null)
{
foreach (RecastGraph.NavmeshTile ng in recastGraph.GetTiles())
{
this.AddGraphObstacles(simulator, ng);
}
}
else
{
INavmesh navmesh = graphs[i] as INavmesh;
if (navmesh != null)
{
this.AddGraphObstacles(simulator, navmesh);
}
}
}
simulator.UpdateObstacles();
}
/// <summary>
/// Traces the contour of a navmesh.
///
/// [Open online documentation to see images]
///
/// This image is just used to illustrate the difference between chains and cycles. That it shows a grid graph is not relevant.
/// [Open online documentation to see images]
///
/// See: <see cref="GetContours(NavGraph)"/>
/// </summary>
/// <param name="navmesh">The navmesh-like object to trace. This can be a recast or navmesh graph or it could be a single tile in one such graph.</param>
/// <param name="results">Will be called once for each contour with the contour as a parameter as well as a boolean indicating if the contour is a cycle or a chain (see second image).</param>
public static void GetContours(INavmesh navmesh, System.Action <List <Int3>, bool> results)
{
// Assume 3 vertices per node
var uses = new bool[3];
var outline = new Dictionary <int, int>();
var vertexPositions = new Dictionary <int, Int3>();
var hasInEdge = new HashSet <int>();
navmesh.GetNodes(_node => {
var node = _node as TriangleMeshNode;
uses[0] = uses[1] = uses[2] = false;
if (node != null)
{
// Find out which edges are shared with other nodes
for (int j = 0; j < node.connections.Length; j++)
{
var other = node.connections[j].node as TriangleMeshNode;
// Not necessarily a TriangleMeshNode
if (other != null)
{
int a = node.SharedEdge(other);
if (a != -1)
{
uses[a] = true;
}
}
}
// Loop through all edges on the node
for (int j = 0; j < 3; j++)
{
// The edge is not shared with any other node
// I.e it is an exterior edge on the mesh
if (!uses[j])
{
var i1 = j;
var i2 = (j + 1) % node.GetVertexCount();
outline[node.GetVertexIndex(i1)] = node.GetVertexIndex(i2);
hasInEdge.Add(node.GetVertexIndex(i2));
vertexPositions[node.GetVertexIndex(i1)] = node.GetVertex(i1);
vertexPositions[node.GetVertexIndex(i2)] = node.GetVertex(i2);
}
}
}
});
Polygon.TraceContours(outline, hasInEdge, (chain, cycle) => {
List <Int3> vertices = ListPool <Int3> .Claim();
for (int i = 0; i < chain.Count; i++)
{
vertices.Add(vertexPositions[chain[i]]);
}
results(vertices, cycle);
});
}
public static NNInfo GetNearest(INavmesh graph, Node[] nodes, Vector3 position, NNConstraint constraint)
{
if (nodes == null || nodes.Length == 0)
{
Debug.LogError("NavGraph hasn't been generated yet or does not contain any nodes");
return(new NNInfo());
}
return(GetNearestForce(nodes, graph.vertices, position, constraint));
}
private void AddGraphObstacles(Simulator sim, INavmesh ng)
{
int[] uses = new int[3];
Dictionary <int, int> outline = new Dictionary <int, int>();
Dictionary <int, Int3> vertexPositions = new Dictionary <int, Int3>();
HashSet <int> hasInEdge = new HashSet <int>();
ng.GetNodes(delegate(GraphNode _node)
{
TriangleMeshNode triangleMeshNode = _node as TriangleMeshNode;
uses[0] = (uses[1] = (uses[2] = 0));
if (triangleMeshNode != null)
{
for (int i = 0; i < triangleMeshNode.connections.Length; i++)
{
TriangleMeshNode triangleMeshNode2 = triangleMeshNode.connections[i].node as TriangleMeshNode;
if (triangleMeshNode2 != null)
{
int num = triangleMeshNode.SharedEdge(triangleMeshNode2);
if (num != -1)
{
uses[num] = 1;
}
}
}
for (int j = 0; j < 3; j++)
{
if (uses[j] == 0)
{
int i2 = j;
int i3 = (j + 1) % triangleMeshNode.GetVertexCount();
outline[triangleMeshNode.GetVertexIndex(i2)] = triangleMeshNode.GetVertexIndex(i3);
hasInEdge.Add(triangleMeshNode.GetVertexIndex(i3));
vertexPositions[triangleMeshNode.GetVertexIndex(i2)] = triangleMeshNode.GetVertex(i2);
vertexPositions[triangleMeshNode.GetVertexIndex(i3)] = triangleMeshNode.GetVertex(i3);
}
}
}
});
List <Vector3> vertices = ListPool <Vector3> .Claim();
RVONavmesh.CompressContour(outline, hasInEdge, delegate(List <int> chain, bool cycle)
{
for (int i = 0; i < chain.Count; i++)
{
vertices.Add((Vector3)vertexPositions[chain[i]]);
}
this.obstacles.Add(sim.AddObstacle(vertices.ToArray(), this.wallHeight, cycle));
vertices.Clear();
});
ListPool <Vector3> .Release(vertices);
}
// Token: 0x060028F1 RID: 10481 RVA: 0x001BEA04 File Offset: 0x001BCC04
private void AddGraphObstacles(Simulator simulator, INavmesh navmesh)
{
GraphUtilities.GetContours(navmesh, delegate(List <Int3> vertices, bool cycle)
{
Vector3[] array = new Vector3[vertices.Count];
for (int i = 0; i < array.Length; i++)
{
array[i] = (Vector3)vertices[i];
}
ListPool <Int3> .Release(vertices);
this.obstacles.Add(simulator.AddObstacle(array, this.wallHeight, cycle));
});
}
/// <summary>Adds obstacles for a navmesh/recast graph</summary>
void AddGraphObstacles(Pathfinding.RVO.Simulator simulator, INavmesh navmesh)
{
GraphUtilities.GetContours(navmesh, (vertices, cycle) => {
var verticesV3 = new Vector3[vertices.Count];
for (int i = 0; i < verticesV3.Length; i++)
{
verticesV3[i] = (Vector3)vertices[i];
}
// Pool the 'vertices' list to reduce allocations
ListPool <Int3> .Release(vertices);
obstacles.Add(simulator.AddObstacle(verticesV3, wallHeight, cycle));
});
}
// Token: 0x0600229B RID: 8859 RVA: 0x00190E64 File Offset: 0x0018F064
public static void GetContours(INavmesh navmesh, Action <List <Int3>, bool> results)
{
bool[] uses = new bool[3];
Dictionary <int, int> outline = new Dictionary <int, int>();
Dictionary <int, Int3> vertexPositions = new Dictionary <int, Int3>();
HashSet <int> hasInEdge = new HashSet <int>();
navmesh.GetNodes(delegate(GraphNode _node)
{
TriangleMeshNode triangleMeshNode = _node as TriangleMeshNode;
uses[0] = (uses[1] = (uses[2] = false));
if (triangleMeshNode != null)
{
for (int i = 0; i < triangleMeshNode.connections.Length; i++)
{
TriangleMeshNode triangleMeshNode2 = triangleMeshNode.connections[i].node as TriangleMeshNode;
if (triangleMeshNode2 != null)
{
int num = triangleMeshNode.SharedEdge(triangleMeshNode2);
if (num != -1)
{
uses[num] = true;
}
}
}
for (int j = 0; j < 3; j++)
{
if (!uses[j])
{
int i2 = j;
int i3 = (j + 1) % triangleMeshNode.GetVertexCount();
outline[triangleMeshNode.GetVertexIndex(i2)] = triangleMeshNode.GetVertexIndex(i3);
hasInEdge.Add(triangleMeshNode.GetVertexIndex(i3));
vertexPositions[triangleMeshNode.GetVertexIndex(i2)] = triangleMeshNode.GetVertex(i2);
vertexPositions[triangleMeshNode.GetVertexIndex(i3)] = triangleMeshNode.GetVertex(i3);
}
}
}
});
Polygon.TraceContours(outline, hasInEdge, delegate(List <int> chain, bool cycle)
{
List <Int3> list = ListPool <Int3> .Claim();
for (int i = 0; i < chain.Count; i++)
{
list.Add(vertexPositions[chain[i]]);
}
results(list, cycle);
});
}
public void AddGraphObstacles(Simulator sim, NavGraph graph)
{
if (this.obstacles.Count > 0 && this.lastSim != null && this.lastSim != sim)
{
Debug.LogError("Simulator has changed but some old obstacles are still added for the previous simulator. Deleting previous obstacles.");
this.RemoveObstacles();
}
this.lastSim = sim;
INavmesh navmesh = graph as INavmesh;
if (navmesh == null)
{
return;
}
int[] uses = new int[20];
navmesh.GetNodes(delegate(GraphNode _node)
{
TriangleMeshNode triangleMeshNode = _node as TriangleMeshNode;
uses[0] = (uses[1] = (uses[2] = 0));
if (triangleMeshNode != null)
{
for (int i = 0; i < triangleMeshNode.connections.Length; i++)
{
TriangleMeshNode triangleMeshNode2 = triangleMeshNode.connections[i] as TriangleMeshNode;
if (triangleMeshNode2 != null)
{
int num = triangleMeshNode.SharedEdge(triangleMeshNode2);
if (num != -1)
{
uses[num] = 1;
}
}
}
for (int j = 0; j < 3; j++)
{
if (uses[j] == 0)
{
Vector3 a = (Vector3)triangleMeshNode.GetVertex(j);
Vector3 b = (Vector3)triangleMeshNode.GetVertex((j + 1) % triangleMeshNode.GetVertexCount());
float val = Math.Abs(a.y - b.y);
val = Math.Max(val, 5f);
this.obstacles.Add(sim.AddObstacle(a, b, this.wallHeight));
}
}
}
return(true);
});
}
public static NNInfo GetNearest(INavmesh graph, Node[] nodes, Vector3 position, NNConstraint constraint, bool accurateNearestNode)
{
if (nodes == null || nodes.Length == 0)
{
Debug.LogError("NavGraph hasn't been generated yet or does not contain any nodes");
return(new NNInfo());
}
if (constraint == null)
{
constraint = NNConstraint.None;
}
return(GetNearestForceBoth(nodes, graph.vertices, position, NNConstraint.None, accurateNearestNode));
}
public static void BuildFunnelCorridor(INavmesh graph, List <GraphNode> 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;
}
for (int i = startIndex; i < endIndex; i++)
{
//Find the connection between the nodes
TriangleMeshNode n1 = path[i] as TriangleMeshNode;
TriangleMeshNode n2 = path[i + 1] as TriangleMeshNode;
int a;
bool search = true;
for (a = 0; a < 3; a++)
{
for (int b = 0; b < 3; b++)
{
if (n1.GetVertexIndex(a) == n2.GetVertexIndex((b + 1) % 3) && n1.GetVertexIndex((a + 1) % 3) == n2.GetVertexIndex(b))
{
search = false;
break;
}
}
if (!search)
{
break;
}
}
if (a == 3)
{
left.Add((Vector3)n1.position);
right.Add((Vector3)n1.position);
left.Add((Vector3)n2.position);
right.Add((Vector3)n2.position);
}
else
{
left.Add((Vector3)n1.GetVertex(a));
right.Add((Vector3)n1.GetVertex((a + 1) % 3));
}
}
}
private void DrawTriangles()
{
// draw all graph nodes
GL.Begin(GL.TRIANGLES);
Vector3 offsetUp = new Vector3(0f, 0.1f, 0f);
for (int graphIndex = 0; graphIndex < AstarPath.active.graphs.Length; ++graphIndex)
{
NavGraph graph = AstarPath.active.graphs[graphIndex];
INavmesh ng = graph as INavmesh;
if (null != ng)
{
ng.GetNodes(delegate(GraphNode _node)
{
// if we are only displaying one region, and this is not that region
if (desplaySpecifiedRegion != DisplayAllRegions && desplaySpecifiedRegion != _node.Area)
{
return(true);
}
Color theColor = graph.NodeColor(_node, AstarPath.active.debugPathData);
theColor.a = navMeshAlpha;
TriangleMeshNode node = _node as TriangleMeshNode;
if (!node.Walkable)
{
theColor = Color.black;
}
GL.Color(theColor);
Vector3 vert0 = (Vector3)node.GetVertex(0);
Vector3 vert1 = (Vector3)node.GetVertex(1);
Vector3 vert2 = (Vector3)node.GetVertex(2);
GL.Vertex(vert0 + offsetUp);
GL.Vertex(vert1 + offsetUp);
GL.Vertex(vert2 + offsetUp);
return(true);
});
}
}
GL.End();
}
public void AddGraphObstacles(Simulator sim, NavGraph graph)
{
if (this.obstacles.get_Count() > 0 && this.lastSim != null && this.lastSim != sim)
{
this.RemoveObstacles();
}
this.lastSim = sim;
INavmesh navmesh = graph as INavmesh;
if (navmesh == null)
{
return;
}
int[] uses = new int[20];
navmesh.GetNodes(delegate(GraphNode _node)
{
TriangleMeshNode triangleMeshNode = _node as TriangleMeshNode;
uses[0] = (uses[1] = (uses[2] = 0));
if (triangleMeshNode != null)
{
for (int i = 0; i < triangleMeshNode.connections.Length; i++)
{
TriangleMeshNode triangleMeshNode2 = triangleMeshNode.connections[i] as TriangleMeshNode;
if (triangleMeshNode2 != null)
{
int num = triangleMeshNode.SharedEdge(triangleMeshNode2);
if (num != -1)
{
uses[num] = 1;
}
}
}
for (int j = 0; j < 3; j++)
{
if (uses[j] == 0)
{
VInt3 vertex = triangleMeshNode.GetVertex(j);
VInt3 vertex2 = triangleMeshNode.GetVertex((j + 1) % triangleMeshNode.GetVertexCount());
this.obstacles.Add(sim.AddObstacle(vertex, vertex2, this.wallHeight));
}
}
}
return(true);
});
}
static private int CalculateNumNavMeshTriangles()
{
int numTriangles = 0;
for (int graphIndex = 0; graphIndex < AstarPath.active.graphs.Length; ++graphIndex)
{
INavmesh ng = AstarPath.active.graphs[graphIndex] as INavmesh;
if (null != ng)
{
ng.GetNodes(delegate(GraphNode _node)
{
++numTriangles;
return(true);
});
}
}
return(numTriangles);
}
public override void OnLatePostScan()
{
if (!Application.isPlaying)
{
return;
}
Profiler.BeginSample("Update RVO Obstacles From Graphs");
RemoveObstacles();
NavGraph[] graphs = AstarPath.active.graphs;
RVOSimulator rvosim = RVOSimulator.active;
if (rvosim == null)
{
throw new System.NullReferenceException("No RVOSimulator could be found in the scene. Please add one to any GameObject");
}
// Remember which simulator these obstacles were added to
lastSim = rvosim.GetSimulator();
for (int i = 0; i < graphs.Length; i++)
{
RecastGraph recast = graphs[i] as RecastGraph;
INavmesh navmesh = graphs[i] as INavmesh;
GridGraph grid = graphs[i] as GridGraph;
if (recast != null)
{
foreach (var tile in recast.GetTiles())
{
AddGraphObstacles(lastSim, tile);
}
}
else if (navmesh != null)
{
AddGraphObstacles(lastSim, navmesh);
}
else if (grid != null)
{
AddGraphObstacles(lastSim, grid);
}
}
Profiler.EndSample();
}
public static void BuildFunnelCorridor(INavmesh graph, List <GraphNode> 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;
}
for (int i = startIndex; i < endIndex; i++)
{
TriangleMeshNode triangleMeshNode = path[i] as TriangleMeshNode;
TriangleMeshNode triangleMeshNode2 = path[i + 1] as TriangleMeshNode;
bool flag = true;
int j;
for (j = 0; j < 3; j++)
{
for (int k = 0; k < 3; k++)
{
if (triangleMeshNode.GetVertexIndex(j) == triangleMeshNode2.GetVertexIndex((k + 1) % 3) && triangleMeshNode.GetVertexIndex((j + 1) % 3) == triangleMeshNode2.GetVertexIndex(k))
{
flag = false;
break;
}
}
if (!flag)
{
break;
}
}
if (j == 3)
{
left.Add((Vector3)triangleMeshNode.position);
right.Add((Vector3)triangleMeshNode.position);
left.Add((Vector3)triangleMeshNode2.position);
right.Add((Vector3)triangleMeshNode2.position);
}
else
{
left.Add((Vector3)triangleMeshNode.GetVertex(j));
right.Add((Vector3)triangleMeshNode.GetVertex((j + 1) % 3));
}
}
}
//These functions are for serialization, the static ones are there so other graphs using mesh nodes can serialize them more easily
public static byte[] SerializeMeshNodes(INavmesh graph, Node[] nodes)
{
System.IO.MemoryStream mem = new System.IO.MemoryStream();
System.IO.BinaryWriter stream = new System.IO.BinaryWriter(mem);
for (int i = 0; i < nodes.Length; i++)
{
MeshNode node = nodes[i] as MeshNode;
if (node == null)
{
Debug.LogError("Serialization Error : Couldn't cast the node to the appropriate type - NavMeshGenerator. Omitting node data.");
return(null);
}
stream.Write(node.v1);
stream.Write(node.v2);
stream.Write(node.v3);
}
Int3[] vertices = graph.vertices;
if (vertices == null)
{
vertices = new Int3[0];
}
stream.Write(vertices.Length);
for (int i = 0; i < vertices.Length; i++)
{
stream.Write(vertices[i].x);
stream.Write(vertices[i].y);
stream.Write(vertices[i].z);
}
stream.Close();
return(mem.ToArray());
}
public override void OnLatePostScan()
{
if (!Application.isPlaying)
{
return;
}
this.RemoveObstacles();
NavGraph[] graphs = AstarPath.active.graphs;
RVOSimulator active = RVOSimulator.active;
if (active == null)
{
throw new NullReferenceException("No RVOSimulator could be found in the scene. Please add one to any GameObject");
}
this.lastSim = active.GetSimulator();
for (int i = 0; i < graphs.Length; i++)
{
RecastGraph recastGraph = graphs[i] as RecastGraph;
INavmesh navmesh = graphs[i] as INavmesh;
GridGraph gridGraph = graphs[i] as GridGraph;
if (recastGraph != null)
{
foreach (NavmeshTile ng in recastGraph.GetTiles())
{
this.AddGraphObstacles(this.lastSim, ng);
}
}
else if (navmesh != null)
{
this.AddGraphObstacles(this.lastSim, navmesh);
}
else if (gridGraph != null)
{
this.AddGraphObstacles(this.lastSim, gridGraph);
}
}
}
/** Returns if there is an obstacle between \a origin and \a end on the graph.
* \param [in] graph The graph to perform the search on
* \param [in] tmp_origin Point to start from
* \param [in] tmp_end Point to linecast to
* \param [out] hit Contains info on what was hit, see GraphHitInfo
* \param [in] hint You need to pass the node closest to the start point, if null, a search for the closest node will be done
* This is not the same as Physics.Linecast, this function traverses the \b graph and looks for collisions instead of checking for collider intersection.
* \astarpro */
public static bool Linecast (INavmesh graph, Vector3 tmp_origin, Vector3 tmp_end, GraphNode hint, out GraphHitInfo hit) {
return Linecast (graph, tmp_origin, tmp_end, hint, out hit, null);
}
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(n1.position);
right.Add(n1.position);
left.Add(n2.position);
right.Add(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(vertices[first]);
right.Add(vertices[second]);
lastLeftIndex = first;
lastRightIndex = second;
}
else if (first == lastRightIndex)
{
left.Add(vertices[second]);
right.Add(vertices[first]);
lastLeftIndex = second;
lastRightIndex = first;
}
else if (second == lastLeftIndex)
{
left.Add(vertices[second]);
right.Add(vertices[first]);
lastLeftIndex = second;
lastRightIndex = first;
}
else
{
left.Add(vertices[first]);
right.Add(vertices[second]);
lastLeftIndex = first;
lastRightIndex = second;
}
}
}
}
public static void UpdateArea (GraphUpdateObject o, INavmesh graph) {
Bounds bounds = o.bounds;
// Bounding rectangle with floating point coordinates
Rect r = Rect.MinMaxRect (bounds.min.x,bounds.min.z,bounds.max.x,bounds.max.z);
// Bounding rectangle with int coordinates
var r2 = new IntRect(
Mathf.FloorToInt(bounds.min.x*Int3.Precision),
Mathf.FloorToInt(bounds.min.z*Int3.Precision),
Mathf.FloorToInt(bounds.max.x*Int3.Precision),
Mathf.FloorToInt(bounds.max.z*Int3.Precision)
);
// Corners of the bounding rectangle
var a = new Int3(r2.xmin,0,r2.ymin);
var b = new Int3(r2.xmin,0,r2.ymax);
var c = new Int3(r2.xmax,0,r2.ymin);
var d = new Int3(r2.xmax,0,r2.ymax);
var ymin = ((Int3)bounds.min).y;
var ymax = ((Int3)bounds.max).y;
// Loop through all nodes
graph.GetNodes (_node => {
var node = _node as TriangleMeshNode;
bool inside = false;
int allLeft = 0;
int allRight = 0;
int allTop = 0;
int allBottom = 0;
// Check bounding box rect in XZ plane
for (int v=0;v<3;v++) {
Int3 p = node.GetVertex(v);
var vert = (Vector3)p;
if (r2.Contains (p.x,p.z)) {
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 (!inside) {
if (allLeft == 3 || allRight == 3 || allTop == 3 || allBottom == 3) {
return true;
}
}
// Check if the polygon edges intersect the bounding rect
for (int v = 0; v < 3; v++) {
int v2 = v > 1 ? 0 : v+1;
Int3 vert1 = node.GetVertex(v);
Int3 vert2 = node.GetVertex(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; }
}
// Check if the node contains any corner of the bounding rect
if (inside || node.ContainsPoint (a) || node.ContainsPoint (b) || node.ContainsPoint (c) || node.ContainsPoint (d)) {
inside = true;
}
if (!inside) {
return true;
}
int allAbove = 0;
int allBelow = 0;
// Check y coordinate
for (int v = 0; v < 3; v++) {
Int3 p = node.GetVertex(v);
var vert = (Vector3)p;
if (vert.y < ymin) allBelow++;
if (vert.y > ymax) allAbove++;
}
// Polygon is either completely above the bounding box or completely below it
if (allBelow == 3 || allAbove == 3) return true;
// Triangle is inside the bounding box!
// Update it!
o.WillUpdateNode(node);
o.Apply (node);
return true;
});
}
/** Adds obstacles for a graph */
public void AddGraphObstacles(Pathfinding.RVO.Simulator sim, NavGraph graph)
{
if (obstacles.Count > 0 && lastSim != null && lastSim != sim)
{
Debug.LogError("Simulator has changed but some old obstacles are still added for the previous simulator. Deleting previous obstacles.");
RemoveObstacles();
}
//Remember which simulator these obstacles were added to
lastSim = sim;
INavmesh ng = graph as INavmesh;
if (ng == null)
{
return;
}
//Assume less than 20 vertices per node (actually assumes 3, but I will change that some day)
int[] uses = new int[20];
ng.GetNodes(delegate(GraphNode _node) {
TriangleMeshNode node = _node as TriangleMeshNode;
uses[0] = uses[1] = uses[2] = 0;
if (node != null)
{
//Find out which edges are shared with other nodes
for (int j = 0; j < node.connections.Length; j++)
{
TriangleMeshNode other = node.connections[j] as TriangleMeshNode;
// Not necessarily a TriangleMeshNode
if (other != null)
{
int a = node.SharedEdge(other);
if (a != -1)
{
uses[a] = 1;
}
}
}
//Loop through all edges on the node
for (int j = 0; j < 3; j++)
{
//The edge is not shared with any other node
//I.e it is an exterior edge on the mesh
if (uses[j] == 0)
{
//The two vertices of the edge
Vector3 v1 = (Vector3)node.GetVertex(j);
Vector3 v2 = (Vector3)node.GetVertex((j + 1) % node.GetVertexCount());
//I think node vertices always should be clockwise, but it's good to be certain
/*if (!Polygon.IsClockwise (v1,v2,(Vector3)node.GetVertex((j+2) % node.GetVertexCount()))) {
* Vector3 tmp = v2;
* v2 = v1;
* v1 = tmp;
* }*/
#if ASTARDEBUG
Debug.DrawLine(v1, v2, Color.red);
Debug.DrawRay(v1, Vector3.up * wallHeight, Color.red);
#endif
//Find out the height of the wall/obstacle we are about to add
float height = System.Math.Abs(v1.y - v2.y);
height = System.Math.Max(height, 5);
//Add the edge as a line obstacle
obstacles.Add(sim.AddObstacle(v1, v2, wallHeight));
}
}
}
return(true);
});
}
public static bool Linecast(INavmesh graph, Vector3 tmp_origin, Vector3 tmp_end, GraphNode hint, out GraphHitInfo hit, List <GraphNode> trace)
{
Int3 @int = (Int3)tmp_end;
Int3 int2 = (Int3)tmp_origin;
hit = default(GraphHitInfo);
if (float.IsNaN(tmp_origin.x + tmp_origin.y + tmp_origin.z))
{
throw new ArgumentException("origin is NaN");
}
if (float.IsNaN(tmp_end.x + tmp_end.y + tmp_end.z))
{
throw new ArgumentException("end is NaN");
}
TriangleMeshNode triangleMeshNode = hint as TriangleMeshNode;
if (triangleMeshNode == null)
{
triangleMeshNode = ((graph as NavGraph).GetNearest(tmp_origin, NNConstraint.None).node as TriangleMeshNode);
if (triangleMeshNode == null)
{
Debug.LogError("Could not find a valid node to start from");
hit.point = tmp_origin;
return(true);
}
}
if (int2 == @int)
{
hit.node = triangleMeshNode;
return(false);
}
int2 = (Int3)triangleMeshNode.ClosestPointOnNode((Vector3)int2);
hit.origin = (Vector3)int2;
if (!triangleMeshNode.Walkable)
{
hit.point = (Vector3)int2;
hit.tangentOrigin = (Vector3)int2;
return(true);
}
List <Vector3> list = ListPool <Vector3> .Claim();
List <Vector3> list2 = ListPool <Vector3> .Claim();
int num = 0;
while (true)
{
num++;
if (num > 2000)
{
break;
}
TriangleMeshNode triangleMeshNode2 = null;
if (trace != null)
{
trace.Add(triangleMeshNode);
}
if (triangleMeshNode.ContainsPoint(@int))
{
goto Block_9;
}
for (int i = 0; i < triangleMeshNode.connections.Length; i++)
{
if (triangleMeshNode.connections[i].GraphIndex == triangleMeshNode.GraphIndex)
{
list.Clear();
list2.Clear();
if (triangleMeshNode.GetPortal(triangleMeshNode.connections[i], list, list2, false))
{
Vector3 vector = list[0];
Vector3 vector2 = list2[0];
if (Polygon.LeftNotColinear(vector, vector2, hit.origin) || !Polygon.LeftNotColinear(vector, vector2, tmp_end))
{
float num2;
float num3;
if (Polygon.IntersectionFactor(vector, vector2, hit.origin, tmp_end, out num2, out num3))
{
if (num3 >= 0f)
{
if (num2 >= 0f && num2 <= 1f)
{
triangleMeshNode2 = (triangleMeshNode.connections[i] as TriangleMeshNode);
break;
}
}
}
}
}
}
}
if (triangleMeshNode2 == null)
{
goto Block_18;
}
triangleMeshNode = triangleMeshNode2;
}
Debug.LogError("Linecast was stuck in infinite loop. Breaking.");
ListPool <Vector3> .Release(list);
ListPool <Vector3> .Release(list2);
return(true);
Block_9:
ListPool <Vector3> .Release(list);
ListPool <Vector3> .Release(list2);
return(false);
Block_18:
int vertexCount = triangleMeshNode.GetVertexCount();
for (int j = 0; j < vertexCount; j++)
{
Vector3 vector3 = (Vector3)triangleMeshNode.GetVertex(j);
Vector3 vector4 = (Vector3)triangleMeshNode.GetVertex((j + 1) % vertexCount);
if (Polygon.LeftNotColinear(vector3, vector4, hit.origin) || !Polygon.LeftNotColinear(vector3, vector4, tmp_end))
{
float num4;
float num5;
if (Polygon.IntersectionFactor(vector3, vector4, hit.origin, tmp_end, out num4, out num5))
{
if (num5 >= 0f)
{
if (num4 >= 0f && num4 <= 1f)
{
Vector3 point = vector3 + (vector4 - vector3) * num4;
hit.point = point;
hit.node = triangleMeshNode;
hit.tangent = vector4 - vector3;
hit.tangentOrigin = vector3;
ListPool <Vector3> .Release(list);
ListPool <Vector3> .Release(list2);
return(true);
}
}
}
}
}
Debug.LogWarning("Linecast failing because point not inside node, and line does not hit any edges of it");
ListPool <Vector3> .Release(list);
ListPool <Vector3> .Release(list2);
return(false);
}
public static void UpdateArea(GraphUpdateObject o, INavmesh graph)
{
//System.DateTime startTime = System.DateTime.UtcNow;
Bounds bounds = o.bounds;
Rect r = Rect.MinMaxRect(bounds.min.x, bounds.min.z, bounds.max.x, bounds.max.z);
var r2 = new IntRect(
Mathf.FloorToInt(bounds.min.x * Int3.Precision),
Mathf.FloorToInt(bounds.min.z * Int3.Precision),
Mathf.FloorToInt(bounds.max.x * Int3.Precision),
Mathf.FloorToInt(bounds.max.z * Int3.Precision)
);
var a = new Int3(r2.xmin, 0, r2.ymin);
var b = new Int3(r2.xmin, 0, r2.ymax);
var c = new Int3(r2.xmax, 0, r2.ymin);
var d = new Int3(r2.xmax, 0, r2.ymax);
graph.GetNodes(_node => {
var node = _node as TriangleMeshNode;
bool inside = false;
int allLeft = 0;
int allRight = 0;
int allTop = 0;
int allBottom = 0;
for (int v = 0; v < 3; v++)
{
Int3 p = node.GetVertex(v);
var vert = (Vector3)p;
if (r2.Contains(p.x, p.z))
{
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 (!inside)
{
if (allLeft == 3 || allRight == 3 || allTop == 3 || allBottom == 3)
{
return(true);
}
}
for (int v = 0; v < 3; v++)
{
int v2 = v > 1 ? 0 : v + 1;
Int3 vert1 = node.GetVertex(v);
Int3 vert2 = node.GetVertex(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 (node.ContainsPoint(a) || node.ContainsPoint(b) || node.ContainsPoint(c) || node.ContainsPoint(d))
{
inside = true;
}
if (!inside)
{
return(true);
}
o.WillUpdateNode(node);
o.Apply(node);
return(true);
});
}
/** Returns if there is an obstacle between \a origin and \a end on the graph.
* \param [in] graph The graph to perform the search on
* \param [in] tmp_origin Point to start from
* \param [in] tmp_end Point to linecast to
* \param [out] hit Contains info on what was hit, see GraphHitInfo
* \param [in] hint You need to pass the node closest to the start point, if null, a search for the closest node will be done
* \param trace If a list is passed, then it will be filled with all nodes the linecast traverses
* This is not the same as Physics.Linecast, this function traverses the \b graph and looks for collisions instead of checking for collider intersection.
* \astarpro */
public static bool Linecast(INavmesh graph, Vector3 tmp_origin, Vector3 tmp_end, GraphNode hint, out GraphHitInfo hit, List <GraphNode> trace)
{
var end = (Int3)tmp_end;
var origin = (Int3)tmp_origin;
hit = new GraphHitInfo();
if (float.IsNaN(tmp_origin.x + tmp_origin.y + tmp_origin.z))
{
throw new System.ArgumentException("origin is NaN");
}
if (float.IsNaN(tmp_end.x + tmp_end.y + tmp_end.z))
{
throw new System.ArgumentException("end is NaN");
}
var node = hint as TriangleMeshNode;
if (node == null)
{
node = (graph as NavGraph).GetNearest(tmp_origin, NNConstraint.None).node as TriangleMeshNode;
if (node == null)
{
Debug.LogError("Could not find a valid node to start from");
hit.point = tmp_origin;
return(true);
}
}
if (origin == end)
{
hit.node = node;
return(false);
}
origin = (Int3)node.ClosestPointOnNode((Vector3)origin);
hit.origin = (Vector3)origin;
if (!node.Walkable)
{
hit.point = (Vector3)origin;
hit.tangentOrigin = (Vector3)origin;
return(true);
}
List <Vector3> left = Pathfinding.Util.ListPool <Vector3> .Claim(); //new List<Vector3>(1);
List <Vector3> right = Pathfinding.Util.ListPool <Vector3> .Claim(); //new List<Vector3>(1);
int counter = 0;
while (true)
{
counter++;
if (counter > 2000)
{
Debug.LogError("Linecast was stuck in infinite loop. Breaking.");
Pathfinding.Util.ListPool <Vector3> .Release(left);
Pathfinding.Util.ListPool <Vector3> .Release(right);
return(true);
}
TriangleMeshNode newNode = null;
if (trace != null)
{
trace.Add(node);
}
if (node.ContainsPoint(end))
{
Pathfinding.Util.ListPool <Vector3> .Release(left);
Pathfinding.Util.ListPool <Vector3> .Release(right);
return(false);
}
for (int i = 0; i < node.connections.Length; i++)
{
//Nodes on other graphs should not be considered
//They might even be of other types (not MeshNode)
if (node.connections[i].GraphIndex != node.GraphIndex)
{
continue;
}
left.Clear();
right.Clear();
if (!node.GetPortal(node.connections[i], left, right, false))
{
continue;
}
Vector3 a = left[0];
Vector3 b = right[0];
//i.e Right or colinear
if (!Polygon.LeftNotColinear(a, b, hit.origin))
{
if (Polygon.LeftNotColinear(a, b, tmp_end))
{
//Since polygons are laid out in clockwise order, the ray would intersect (if intersecting) this edge going in to the node, not going out from it
continue;
}
}
float factor1, factor2;
if (Polygon.IntersectionFactor(a, b, hit.origin, tmp_end, out factor1, out factor2))
{
//Intersection behind the start
if (factor2 < 0)
{
continue;
}
if (factor1 >= 0 && factor1 <= 1)
{
newNode = node.connections[i] as TriangleMeshNode;
break;
}
}
}
if (newNode == null)
{
//Possible edge hit
int vs = node.GetVertexCount();
for (int i = 0; i < vs; i++)
{
var a = (Vector3)node.GetVertex(i);
var b = (Vector3)node.GetVertex((i + 1) % vs);
//i.e right or colinear
if (!Polygon.LeftNotColinear(a, b, hit.origin))
{
//Since polygons are laid out in clockwise order, the ray would intersect (if intersecting) this edge going in to the node, not going out from it
if (Polygon.LeftNotColinear(a, b, tmp_end))
{
//Since polygons are laid out in clockwise order, the ray would intersect (if intersecting) this edge going in to the node, not going out from it
continue;
}
}
float factor1, factor2;
if (Polygon.IntersectionFactor(a, b, hit.origin, tmp_end, out factor1, out factor2))
{
if (factor2 < 0)
{
continue;
}
if (factor1 >= 0 && factor1 <= 1)
{
Vector3 intersectionPoint = a + (b - a) * factor1;
hit.point = intersectionPoint;
hit.node = node;
hit.tangent = b - a;
hit.tangentOrigin = a;
Pathfinding.Util.ListPool <Vector3> .Release(left);
Pathfinding.Util.ListPool <Vector3> .Release(right);
return(true);
}
}
}
//Ok, this is wrong...
Debug.LogWarning("Linecast failing because point not inside node, and line does not hit any edges of it");
Pathfinding.Util.ListPool <Vector3> .Release(left);
Pathfinding.Util.ListPool <Vector3> .Release(right);
return(false);
}
node = newNode;
}
}
public static void UpdateArea (GraphUpdateObject o, INavmesh graph) {
//System.DateTime startTime = System.DateTime.UtcNow;
Bounds bounds = o.bounds;
Rect r = Rect.MinMaxRect (bounds.min.x,bounds.min.z,bounds.max.x,bounds.max.z);
var r2 = new IntRect(
Mathf.FloorToInt(bounds.min.x*Int3.Precision),
Mathf.FloorToInt(bounds.min.z*Int3.Precision),
Mathf.FloorToInt(bounds.max.x*Int3.Precision),
Mathf.FloorToInt(bounds.max.z*Int3.Precision)
);
var a = new Int3(r2.xmin,0,r2.ymin);
var b = new Int3(r2.xmin,0,r2.ymax);
var c = new Int3(r2.xmax,0,r2.ymin);
var d = new Int3(r2.xmax,0,r2.ymax);
graph.GetNodes (_node => {
var node = _node as TriangleMeshNode;
bool inside = false;
int allLeft = 0;
int allRight = 0;
int allTop = 0;
int allBottom = 0;
for (int v=0;v<3;v++) {
Int3 p = node.GetVertex(v);
var vert = (Vector3)p;
if (r2.Contains (p.x,p.z)) {
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 (!inside) {
if (allLeft == 3 || allRight == 3 || allTop == 3 || allBottom == 3) {
return true;
}
}
for (int v=0;v<3;v++) {
int v2 = v > 1 ? 0 : v+1;
Int3 vert1 = node.GetVertex(v);
Int3 vert2 = node.GetVertex(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 (node.ContainsPoint (a) || node.ContainsPoint (b) || node.ContainsPoint (c) || node.ContainsPoint (d)) {
inside = true;
}
if (!inside) {
return true;
}
o.WillUpdateNode(node);
o.Apply (node);
return true;
});
}
public static void UpdateArea(GraphUpdateObject o, INavmesh graph)
{
Bounds bounds = o.bounds;
// Bounding rectangle with floating point coordinates
Rect r = Rect.MinMaxRect(bounds.min.x, bounds.min.z, bounds.max.x, bounds.max.z);
// Bounding rectangle with int coordinates
var r2 = new IntRect(
Mathf.FloorToInt(bounds.min.x * Int3.Precision),
Mathf.FloorToInt(bounds.min.z * Int3.Precision),
Mathf.FloorToInt(bounds.max.x * Int3.Precision),
Mathf.FloorToInt(bounds.max.z * Int3.Precision)
);
// Corners of the bounding rectangle
var a = new Int3(r2.xmin, 0, r2.ymin);
var b = new Int3(r2.xmin, 0, r2.ymax);
var c = new Int3(r2.xmax, 0, r2.ymin);
var d = new Int3(r2.xmax, 0, r2.ymax);
var ymin = ((Int3)bounds.min).y;
var ymax = ((Int3)bounds.max).y;
// Loop through all nodes
graph.GetNodes(_node => {
var node = _node as TriangleMeshNode;
bool inside = false;
int allLeft = 0;
int allRight = 0;
int allTop = 0;
int allBottom = 0;
// Check bounding box rect in XZ plane
for (int v = 0; v < 3; v++)
{
Int3 p = node.GetVertex(v);
var vert = (Vector3)p;
if (r2.Contains(p.x, p.z))
{
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 (!inside)
{
if (allLeft == 3 || allRight == 3 || allTop == 3 || allBottom == 3)
{
return(true);
}
}
// Check if the polygon edges intersect the bounding rect
for (int v = 0; v < 3; v++)
{
int v2 = v > 1 ? 0 : v + 1;
Int3 vert1 = node.GetVertex(v);
Int3 vert2 = node.GetVertex(v2);
if (VectorMath.SegmentsIntersectXZ(a, b, vert1, vert2))
{
inside = true; break;
}
if (VectorMath.SegmentsIntersectXZ(a, c, vert1, vert2))
{
inside = true; break;
}
if (VectorMath.SegmentsIntersectXZ(c, d, vert1, vert2))
{
inside = true; break;
}
if (VectorMath.SegmentsIntersectXZ(d, b, vert1, vert2))
{
inside = true; break;
}
}
// Check if the node contains any corner of the bounding rect
if (inside || node.ContainsPoint(a) || node.ContainsPoint(b) || node.ContainsPoint(c) || node.ContainsPoint(d))
{
inside = true;
}
if (!inside)
{
return(true);
}
int allAbove = 0;
int allBelow = 0;
// Check y coordinate
for (int v = 0; v < 3; v++)
{
Int3 p = node.GetVertex(v);
if (p.y < ymin)
{
allBelow++;
}
if (p.y > ymax)
{
allAbove++;
}
}
// Polygon is either completely above the bounding box or completely below it
if (allBelow == 3 || allAbove == 3)
{
return(true);
}
// Triangle is inside the bounding box!
// Update it!
o.WillUpdateNode(node);
o.Apply(node);
return(true);
});
}
public static NNInfo GetNearest(INavmesh graph, Node[] nodes, Vector3 position, NNConstraint constraint, bool accurateNearestNode)
{
if (nodes == null || nodes.Length == 0) {
Debug.LogError ("NavGraph hasn't been generated yet or does not contain any nodes");
return new NNInfo ();
}
if (constraint == null) constraint = NNConstraint.None;
Int3[] vertices = graph.vertices;
//Query BBTree
if (graph.bbTree == null) {
return GetNearestForce (nodes,vertices, position, constraint, accurateNearestNode);
//Debug.LogError ("No Bounding Box Tree has been assigned");
//return new NNInfo ();
}
//Searches in radiuses of 0.05 - 0.2 - 0.45 ... 1.28 times the average of the width and depth of the bbTree
float w = (graph.bbTree.root.rect.width + graph.bbTree.root.rect.height)*0.5F*0.02F;
NNInfo query = graph.bbTree.QueryCircle (position,w,constraint);//graph.bbTree.Query (position,constraint);
if (query.node == null) {
for (int i=1;i<=8;i++) {
query = graph.bbTree.QueryCircle (position, i*i*w, constraint);
if (query.node != null || (i-1)*(i-1)*w > AstarPath.active.maxNearestNodeDistance*2) { // *2 for a margin
break;
}
}
}
if (query.node != null) {
query.clampedPosition = ClosestPointOnNode (query.node as MeshNode,vertices,position);
}
if (query.constrainedNode != null) {
if (constraint.constrainDistance && ((Vector3)query.constrainedNode.position - position).sqrMagnitude > AstarPath.active.maxNearestNodeDistanceSqr) {
query.constrainedNode = null;
} else {
query.constClampedPosition = ClosestPointOnNode (query.constrainedNode as MeshNode, vertices, position);
}
}
return query;
}
/** Returns if \a _b is visible from \a _a on the graph.
* \param [in] graph The graph to perform the search on
* \param [in] tmp_origin Point to start from
* \param [in] tmp_end Point to linecast to
* \param [out] hit Contains info on what was hit, see GraphHitInfo
* \param [in] hint You need to pass the node closest to the start point
* \param [in] thick An experimental feature can be enabled to use thick linecasts, does not always work as expected
* \param [in] thickness Thickness of the thick linecast
* This is not the same as Physics.Linecast, this function traverses the \b graph and looks for collisions instead of checking for collider intersection.
* \astarpro */
public static bool Linecast(INavmesh graph, Vector3 tmp_origin, Vector3 tmp_end, Node hint, bool thick, float thickness, out GraphHitInfo hit)
{
Int3 end = (Int3)tmp_end;
Int3 origin = (Int3)tmp_origin;
//bool thick = true;
//float thickness = 2F;
if (thickness <= 0) {
thick = false;
}
//System.DateTime startTime = System.DateTime.UtcNow;
hit = new GraphHitInfo ();
MeshNode node = hint as MeshNode;
if (node == null) {
Debug.LogError ("NavMeshGenerator:Linecast: The 'hint' must be a MeshNode");
return true;
}
if (origin == end) {
hit.node = node;
return false;
}
Int3[] vertices = graph.vertices;
origin = (Int3)node.ClosestPoint ((Vector3)origin, vertices);
hit.origin = (Vector3)origin;
Vector3 direction = (Vector3)(end-origin);
Vector3 normalizedDir = direction.normalized;
Int3 normalizedIntDir = (Int3)(normalizedDir*(1000.0f / Int3.Precision));
/*if (thick) {
Vector3 normal = Vector3.Cross (direction,Vector3.up).normalized;
//Debug.DrawLine (tmp_origin+normal*thickness,tmp_end+normal*thickness,Color.black);
//Debug.DrawLine (tmp_origin-normal*thickness,tmp_end-normal*thickness,Color.black);
} else {
//Debug.DrawLine (tmp_origin-Vector3.up,tmp_end-Vector3.up,Color.black);
}*/
#if ASTARDEBUG
Debug.DrawLine ((Vector3)origin,(Vector3)end,Color.black);
#endif
//Current position for tracing
Int3 currentPosition = origin;
//Intersected nodes
int count = 0;
//Temporary variables
int[] vs = new int[3];
//Reason for termination
string reason = "";
MeshNode previousNode = null;
if (!node.walkable) {
reason += " Node is unwalkable";
hit.point = (Vector3)origin;
hit.tangentOrigin = (Vector3)origin;
return true;
}
while (true) {
count++;
//Because of floating point errors, this can actually return true sometimes... I think
if (currentPosition == end) {
reason += " Current == End";
break;
}
if (ContainsPoint (node,(Vector3)end,vertices)) {
reason = "Contains point";
break;
}
if (count > 200) {
Debug.DrawRay ((Vector3)origin,normalizedDir*100,Color.cyan);
Debug.DrawRay ((Vector3)currentPosition,normalizedDir*100,Color.cyan);
Debug.LogError ("Possible infinite loop, intersected > 200 nodes");
Debug.Break ();
return true;
}
//Loop through all vertices
for (int i=0;i<3;i++) {
if (vertices[node[i]] == currentPosition) {
vs[i] = 0;
continue;
}
int tmp = Int3.Dot (normalizedIntDir, (vertices[node[i]]-currentPosition).NormalizeTo (1000));
vs[i] = tmp;
#if ASTARDEBUG
Debug.DrawRay (Vector3.Lerp ((Vector3)vertices[node[i]],(Vector3)node.position,0.5F),Vector3.up*2*tmp*0.001F,new Color (1,0.5F,0));
#endif
}
int max = 0;
if (vs[1] > vs[max]) max = 1;
if (vs[2] > vs[max]) max = 2;
int v1 = node[max];
int v2 = 0;
if (count == 70 || count == 71) {
string s2 = "Count "+count+" "+node.position+"\n";
for (int i=0;i<vs.Length;i++) {
s2 += vs[i].ToString ("0.00")+"\n";
}
Debug.Log (s2);
}
#if ASTARDEBUG
Debug.DrawLine ((Vector3)node.position+Vector3.up*count,(Vector3)vertices[v1]+Vector3.up*count,Color.yellow);
Debug.DrawRay ((Vector3)vertices[v1]+Vector3.up*count,Vector3.up,Color.yellow);
#endif
int preNodeV2 = 0;
long triangleArea = Polygon.TriangleArea2 (currentPosition,currentPosition+normalizedIntDir,vertices[v1]);
if (triangleArea == 0) {
//Polygon.IsColinear (currentPosition,currentPosition+normalizedIntDir,vertices[v1])) {
int max2 = -1;
for (int i=0;i<3;i++) {
if (max != i && (max2 == -1 || vs[i] > vs[max2])) {
max2 = i;
}
}
v2 = node[max2];
} else if (triangleArea < 0) {
//if (Polygon.Left (currentPosition,currentPosition+normalizedIntDir,vertices[v1])) {
preNodeV2 = max - 1 < 0 ? 2 : max-1;
v2 = node[preNodeV2];
} else {
preNodeV2 = max + 1 > 2 ? 0 : max+1;
v2 = node[preNodeV2];
}
Vector3 intersectionPoint;
bool intersectionSuccess = true;
if (thick) {
//Vector3 intersectionPoint = Polygon.IntersectionPoint (currentPosition,end,vertices[v1],vertices[v2]);
float intersectionFactor = Polygon.IntersectionFactor ((Vector3)vertices[v1],(Vector3)vertices[v2],(Vector3)currentPosition,(Vector3)end);
if (intersectionFactor < 0 || intersectionFactor > 1) {
Debug.LogError ("This should not happen");
hit.point = intersectionFactor < 0 ? (Vector3)vertices[v1] : (Vector3)vertices[v2];
return true;
}
Vector3 dir2 = (Vector3)(vertices[v2]-vertices[v1]);
intersectionPoint = (Vector3)vertices[v1]+dir2*intersectionFactor;
float magn = dir2.magnitude;
intersectionFactor *= magn;
if (intersectionFactor-thickness < 0) {
hit.point = (Vector3)vertices[v1];
return true;
} else if (intersectionFactor+thickness > magn) {
hit.point = (Vector3)vertices[v2];
return true;
}
} else {
float intersectionFactor = Polygon.IntersectionFactor ((Vector3)vertices[v1],(Vector3)vertices[v2],(Vector3)currentPosition,(Vector3)end);
//Lines were colinear
if (intersectionFactor == -1) {
intersectionSuccess = false;
}
intersectionFactor = Mathf.Clamp01 (intersectionFactor);
intersectionPoint = (Vector3)vertices[v1] + (Vector3)(vertices[v2]-vertices[v1])*intersectionFactor;
if (!intersectionSuccess) {
if ((vertices[v1]-currentPosition).sqrMagnitude >= (end-currentPosition).sqrMagnitude) {
intersectionPoint = (Vector3)end;
reason = "Colinear - Aborting";
break;
} else {
preNodeV2 = max != 0 && preNodeV2 != 0 ? 0 : (max != 1 && preNodeV2 != 1 ? 1 : 2);
v2 = node[preNodeV2];
intersectionPoint = (Vector3)vertices[v1];
intersectionSuccess = true;
reason = "Colinear - Continuing";
}
}
float distanceFactor = Mathfx.NearestPointFactor ((Vector3)origin,(Vector3)end,intersectionPoint);
if (distanceFactor > 1F) {
intersectionSuccess = false;
}
}
#if ASTARDEBUG
Debug.DrawLine ((Vector3)vertices[v1]+Vector3.up*count,(Vector3)vertices[v2]+Vector3.up*count,Color.magenta);
#endif
MeshNode nextNode = null;
bool breakOutFromLoop = false;
for (int i=0;i<node.connections.Length;i++) {
MeshNode other = node.connections[i] as MeshNode;
//Make sure the node is a MeshNode and that it doesn't of some reason link to itself.
if (other == null || other == node) {
continue;
}
int matches = 0;
for (int v=0;v<3;v++) {
if (other[v] == v1 || other[v] == v2) {
matches++;
}
}
if (matches == 2) {
//The node is the previous node, the endpoint must be in between the nodes (floating point error), abort
if (other == previousNode) {
reason += "Other == previous node\n";
breakOutFromLoop = true;
break;
} else {
nextNode = other;
}
break;
}
}
if (breakOutFromLoop) {
break;
}
if (nextNode == null || !intersectionSuccess || !nextNode.walkable) {
if (nextNode == null) {
reason += "No next node (wall)";
}
#if ASTARDEBUG
Debug.DrawLine ((Vector3)origin,(Vector3)intersectionPoint,Color.red);
#endif
hit.tangentOrigin = (Vector3)vertices[v1];
hit.tangent = (Vector3)(vertices[v2]-vertices[v1]);
hit.point = intersectionPoint;
hit.node = node;
return true;
} else {
#if ASTARDEBUG
Debug.DrawLine ((Vector3)node.position+Vector3.up*count,(Vector3)nextNode.position+Vector3.up*(count+1),Color.green);
#endif
previousNode = node;
node = nextNode;
currentPosition = (Int3)intersectionPoint;
}
}
#if ASTARDEBUG
Debug.DrawLine ((Vector3)origin,(Vector3)end,Color.green);
#endif
hit.node = node;
return false;
}
/** Returns if there is an obstacle between \a origin and \a end on the graph.
* \param [in] graph The graph to perform the search on
* \param [in] tmp_origin Point to start from
* \param [in] tmp_end Point to linecast to
* \param [out] hit Contains info on what was hit, see GraphHitInfo
* \param [in] hint You need to pass the node closest to the start point, if null, a search for the closest node will be done
* \param trace If a list is passed, then it will be filled with all nodes the linecast traverses
* This is not the same as Physics.Linecast, this function traverses the \b graph and looks for collisions instead of checking for collider intersection.
* \astarpro */
public static bool Linecast (INavmesh graph, Vector3 tmp_origin, Vector3 tmp_end, GraphNode hint, out GraphHitInfo hit, List<GraphNode> trace) {
Int3 end = (Int3)tmp_end;
Int3 origin = (Int3)tmp_origin;
hit = new GraphHitInfo ();
if (float.IsNaN (tmp_origin.x + tmp_origin.y + tmp_origin.z)) throw new System.ArgumentException ("origin is NaN");
if (float.IsNaN (tmp_end.x + tmp_end.y + tmp_end.z)) throw new System.ArgumentException ("end is NaN");
TriangleMeshNode node = hint as TriangleMeshNode;
if (node == null) {
node = (graph as NavGraph).GetNearest (tmp_origin, NNConstraint.None).node as TriangleMeshNode;
if (node == null) {
Debug.LogError ("Could not find a valid node to start from");
hit.point = tmp_origin;
return true;
}
}
if (origin == end) {
hit.node = node;
return false;
}
origin = (Int3)node.ClosestPointOnNode ((Vector3)origin);
hit.origin = (Vector3)origin;
if (!node.Walkable) {
hit.point = (Vector3)origin;
hit.tangentOrigin = (Vector3)origin;
return true;
}
List<Vector3> left = Pathfinding.Util.ListPool<Vector3>.Claim();//new List<Vector3>(1);
List<Vector3> right = Pathfinding.Util.ListPool<Vector3>.Claim();//new List<Vector3>(1);
while (true) {
TriangleMeshNode newNode = null;
if (trace != null) trace.Add (node);
if (node.ContainsPoint (end)) {
Pathfinding.Util.ListPool<Vector3>.Release(left);
Pathfinding.Util.ListPool<Vector3>.Release(right);
return false;
}
for (int i=0;i<node.connections.Length;i++) {
//Nodes on other graphs should not be considered
//They might even be of other types (not MeshNode)
if (node.connections[i].GraphIndex != node.GraphIndex) continue;
left.Clear();
right.Clear();
if (!node.GetPortal (node.connections[i],left,right,false)) continue;
Vector3 a = left[0];
Vector3 b = right[0];
//i.e Right or colinear
if (!Polygon.LeftNotColinear (a,b,hit.origin)) {
if (Polygon.LeftNotColinear (a, b, tmp_end)) {
//Since polygons are laid out in clockwise order, the ray would intersect (if intersecting) this edge going in to the node, not going out from it
continue;
}
}
float factor1, factor2;
if (Polygon.IntersectionFactor (a,b,hit.origin,tmp_end, out factor1, out factor2)) {
//Intersection behind the start
if (factor2 < 0) continue;
if (factor1 >= 0 && factor1 <= 1) {
newNode = node.connections[i] as TriangleMeshNode;
break;
}
}
}
if (newNode == null) {
//Possible edge hit
int vs = node.GetVertexCount();
for (int i=0;i<vs;i++) {
Vector3 a = (Vector3)node.GetVertex(i);
Vector3 b = (Vector3)node.GetVertex((i + 1) % vs);
//i.e right or colinear
if (!Polygon.LeftNotColinear (a,b,hit.origin)) {
//Since polygons are laid out in clockwise order, the ray would intersect (if intersecting) this edge going in to the node, not going out from it
if (Polygon.LeftNotColinear (a, b, tmp_end)) {
//Since polygons are laid out in clockwise order, the ray would intersect (if intersecting) this edge going in to the node, not going out from it
continue;
}
}
float factor1, factor2;
if (Polygon.IntersectionFactor (a,b,hit.origin,tmp_end, out factor1, out factor2)) {
if (factor2 < 0) continue;
if (factor1 >= 0 && factor1 <= 1) {
Vector3 intersectionPoint = a + (b-a)*factor1;
hit.point = intersectionPoint;
hit.node = node;
hit.tangent = b-a;
hit.tangentOrigin = a;
Pathfinding.Util.ListPool<Vector3>.Release(left);
Pathfinding.Util.ListPool<Vector3>.Release(right);
return true;
}
}
}
//Ok, this is wrong...
Debug.LogWarning ("Linecast failing because point not inside node, and line does not hit any edges of it");
Pathfinding.Util.ListPool<Vector3>.Release(left);
Pathfinding.Util.ListPool<Vector3>.Release(right);
return false;
}
node = newNode;
}
}
public BBTree(INavmesh graph)
{
this.graph = graph;
}
public static void UpdateArea (GraphUpdateObject o, INavmesh graph) {
//System.DateTime startTime = System.DateTime.UtcNow;
Bounds bounds = o.bounds;
Rect r = Rect.MinMaxRect (bounds.min.x,bounds.min.z,bounds.max.x,bounds.max.z);
IntRect r2 = new IntRect(
Mathf.FloorToInt(bounds.min.x*Int3.Precision),
Mathf.FloorToInt(bounds.min.z*Int3.Precision),
Mathf.FloorToInt(bounds.max.x*Int3.Precision),
Mathf.FloorToInt(bounds.max.z*Int3.Precision)
);
/*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
*/
Int3 a = new Int3(r2.xmin,0,r2.ymin);
Int3 b = new Int3(r2.xmin,0,r2.ymax);
Int3 c = new Int3(r2.xmax,0,r2.ymin);
Int3 d = new Int3(r2.xmax,0,r2.ymax);
Int3 ia = (Int3)a;
Int3 ib = (Int3)b;
Int3 ic = (Int3)c;
Int3 id = (Int3)d;
#if ASTARDEBUG
Debug.DrawLine (a,b,Color.white);
Debug.DrawLine (a,c,Color.white);
Debug.DrawLine (c,d,Color.white);
Debug.DrawLine (d,b,Color.white);
#endif
//for (int i=0;i<nodes.Length;i++) {
graph.GetNodes (delegate (GraphNode _node) {
TriangleMeshNode node = _node as TriangleMeshNode;
bool inside = false;
int allLeft = 0;
int allRight = 0;
int allTop = 0;
int allBottom = 0;
for (int v=0;v<3;v++) {
Int3 p = node.GetVertex(v);
Vector3 vert = (Vector3)p;
//Vector2 vert2D = new Vector2 (vert.x,vert.z);
if (r2.Contains (p.x,p.z)) {
//Debug.DrawRay (vert,Vector3.up*10,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) {
return true;
}
}
//Debug.DrawLine ((Vector3)node.GetVertex(0),(Vector3)node.GetVertex(1),Color.yellow);
//Debug.DrawLine ((Vector3)node.GetVertex(1),(Vector3)node.GetVertex(2),Color.yellow);
//Debug.DrawLine ((Vector3)node.GetVertex(2),(Vector3)node.GetVertex(0),Color.yellow);
for (int v=0;v<3;v++) {
int v2 = v > 1 ? 0 : v+1;
Int3 vert1 = node.GetVertex(v);
Int3 vert2 = node.GetVertex(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 (node.ContainsPoint (ia) || node.ContainsPoint (ib) || node.ContainsPoint (ic) || node.ContainsPoint (id)) {
inside = true;
}
if (!inside) {
return true;
}
o.WillUpdateNode(node);
o.Apply (node);
/*Debug.DrawLine ((Vector3)node.GetVertex(0),(Vector3)node.GetVertex(1),Color.blue);
Debug.DrawLine ((Vector3)node.GetVertex(1),(Vector3)node.GetVertex(2),Color.blue);
Debug.DrawLine ((Vector3)node.GetVertex(2),(Vector3)node.GetVertex(0),Color.blue);
Debug.Break ();*/
return true;
});
//System.DateTime endTime = System.DateTime.UtcNow;
//float theTime = (endTime-startTime).Ticks*0.0001F;
//Debug.Log ("Intersecting bounds with navmesh took "+theTime.ToString ("0.000")+" ms");
}
public static NNInfo GetNearest (INavmesh graph, Node[] nodes, Vector3 position, NNConstraint constraint) {
if (nodes == null || nodes.Length == 0) {
Debug.LogError ("NavGraph hasn't been generated yet or does not contain any nodes");
return new NNInfo ();
}
if (constraint == null) constraint = NNConstraint.None;
return GetNearestForce (nodes,graph.vertices, position, constraint);
}
public static bool Linecast(INavmesh graph, Vector3 tmp_origin, Vector3 tmp_end, GraphNode hint, out GraphHitInfo hit)
{
return(NavMeshGraph.Linecast(graph, tmp_origin, tmp_end, hint, out hit, null));
}
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 (n1.position);
right.Add (n1.position);
left.Add (n2.position);
right.Add (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 (vertices[first]);
right.Add (vertices[second]);
lastLeftIndex = first;
lastRightIndex = second;
} else if (first == lastRightIndex) {
left.Add (vertices[second]);
right.Add (vertices[first]);
lastLeftIndex = second;
lastRightIndex = first;
} else if (second == lastLeftIndex) {
left.Add (vertices[second]);
right.Add (vertices[first]);
lastLeftIndex = second;
lastRightIndex = first;
} else {
left.Add (vertices[first]);
right.Add (vertices[second]);
lastLeftIndex = first;
lastRightIndex = second;
}
}
}
}
public static void UpdateArea(GraphUpdateObject o, INavmesh graph)
{
Bounds bounds = o.bounds;
Rect r = Rect.MinMaxRect(bounds.min.x, bounds.min.z, bounds.max.x, bounds.max.z);
IntRect r2 = new IntRect(Mathf.FloorToInt(bounds.min.x * 1000f), Mathf.FloorToInt(bounds.min.z * 1000f), Mathf.FloorToInt(bounds.max.x * 1000f), Mathf.FloorToInt(bounds.max.z * 1000f));
Int3 a = new Int3(r2.xmin, 0, r2.ymin);
Int3 b = new Int3(r2.xmin, 0, r2.ymax);
Int3 c = new Int3(r2.xmax, 0, r2.ymin);
Int3 d = new Int3(r2.xmax, 0, r2.ymax);
graph.GetNodes(delegate(GraphNode _node)
{
TriangleMeshNode triangleMeshNode = _node as TriangleMeshNode;
bool flag = false;
int num = 0;
int num2 = 0;
int num3 = 0;
int num4 = 0;
for (int i = 0; i < 3; i++)
{
Int3 vertex = triangleMeshNode.GetVertex(i);
Vector3 vector = (Vector3)vertex;
if (r2.Contains(vertex.x, vertex.z))
{
flag = true;
break;
}
if (vector.x < r.xMin)
{
num++;
}
if (vector.x > r.xMax)
{
num2++;
}
if (vector.z < r.yMin)
{
num3++;
}
if (vector.z > r.yMax)
{
num4++;
}
}
if (!flag && (num == 3 || num2 == 3 || num3 == 3 || num4 == 3))
{
return(true);
}
for (int j = 0; j < 3; j++)
{
int i2 = (j <= 1) ? (j + 1) : 0;
Int3 vertex2 = triangleMeshNode.GetVertex(j);
Int3 vertex3 = triangleMeshNode.GetVertex(i2);
if (Polygon.Intersects(a, b, vertex2, vertex3))
{
flag = true;
break;
}
if (Polygon.Intersects(a, c, vertex2, vertex3))
{
flag = true;
break;
}
if (Polygon.Intersects(c, d, vertex2, vertex3))
{
flag = true;
break;
}
if (Polygon.Intersects(d, b, vertex2, vertex3))
{
flag = true;
break;
}
}
if (triangleMeshNode.ContainsPoint(a) || triangleMeshNode.ContainsPoint(b) || triangleMeshNode.ContainsPoint(c) || triangleMeshNode.ContainsPoint(d))
{
flag = true;
}
if (!flag)
{
return(true);
}
o.WillUpdateNode(triangleMeshNode);
o.Apply(triangleMeshNode);
return(true);
});
}
//These functions are for serialization, the static ones are there so other graphs using mesh nodes can serialize them more easily
public static void SerializeMeshNodes (INavmesh graph, Node[] nodes, AstarSerializer serializer) {
System.IO.BinaryWriter stream = serializer.writerStream;
for (int i=0;i<nodes.Length;i++) {
MeshNode node = nodes[i] as MeshNode;
if (node == null) {
Debug.LogError ("Serialization Error : Couldn't cast the node to the appropriate type - NavMeshGenerator");
return;
}
stream.Write (node.v1);
stream.Write (node.v2);
stream.Write (node.v3);
}
Int3[] vertices = graph.vertices;
if (vertices == null) {
vertices = new Int3[0];
}
stream.Write (vertices.Length);
for (int i=0;i<vertices.Length;i++) {
stream.Write (vertices[i].x);
stream.Write (vertices[i].y);
stream.Write (vertices[i].z);
}
}
/** Adds obstacles for a graph */
public void AddGraphObstacles(Pathfinding.RVO.Simulator sim, NavGraph graph)
{
if (obstacles.Count > 0 && lastSim != null && lastSim != sim)
{
Debug.LogError("Simulator has changed but some old obstacles are still added for the previous simulator. Deleting previous obstacles.");
RemoveObstacles();
}
//Remember which simulator these obstacles were added to
lastSim = sim;
INavmesh ng = graph as INavmesh;
if (ng == null)
{
return;
}
Node[] nodes = graph.nodes;
Int3[] vertices = ng.vertices;
int[] uses = new int[3];
for (int i = 0; i < nodes.Length; i++)
{
MeshNode node = nodes[i] as MeshNode;
uses[0] = uses[1] = uses[2] = 0;
if (node != null)
{
for (int j = 0; j < node.connections.Length; j++)
{
MeshNode other = node.connections[j] as MeshNode;
if (other == null)
{
continue;
}
int first = -1;
int second = -1;
for (int x = 0; x < 3; x++)
{
for (int y = 0; y < 3; y++)
{
if (node[x] == other[y] && first < 0)
{
first = x;
break;
}
else if (node[x] == other[y])
{
second = x;
break;
}
}
if (second >= 0)
{
break;
}
}
//Only shared one vertex
if (second == -1)
{
continue;
}
if ((first + 1) % 3 == second)
{
uses[first]++;
}
else //if ((second+1) % 3 == first) {
{
uses[second]++;
}
}
}
for (int j = 0; j < 3; j++)
{
if (uses[j] == 0)
{
Vector3 v1 = (Vector3)vertices[node[j]];
Vector3 v2 = (Vector3)vertices[node[(j + 1) % 3]];
//I think node vertices always should be clockwise, but it's good to be certain
if (!Polygon.IsClockwise(v1, v2, (Vector3)vertices[node[(j + 2) % 3]]))
{
Vector3 tmp = v2;
v2 = v1;
v1 = tmp;
}
#if ASTARDEBUG
Debug.DrawLine(v1, v2, Color.red);
Debug.DrawRay(v1, Vector3.up * wallHeight, Color.red);
#endif
float height = System.Math.Abs(v1.y - v2.y);
height = System.Math.Max(height, 5);
obstacles.Add(sim.AddObstacle(v1, v2, wallHeight));
}
}
}
}
public static void DeSerializeMeshNodes (INavmesh graph, Node[] nodes, AstarSerializer serializer) {
System.IO.BinaryReader stream = serializer.readerStream;
for (int i=0;i<nodes.Length;i++) {
MeshNode node = nodes[i] as MeshNode;
if (node == null) {
Debug.LogError ("Serialization Error : Couldn't cast the node to the appropriate type - NavMeshGenerator");
return;
}
node.v1 = stream.ReadInt32 ();
node.v2 = stream.ReadInt32 ();
node.v3 = stream.ReadInt32 ();
}
int numVertices = stream.ReadInt32 ();
graph.vertices = new Int3[numVertices];
for (int i=0;i<numVertices;i++) {
int x = stream.ReadInt32 ();
int y = stream.ReadInt32 ();
int z = stream.ReadInt32 ();
graph.vertices[i] = new Int3 (x,y,z);
}
RebuildBBTree (graph as NavGraph);
}
public static void UpdateArea(GraphUpdateObject o, INavmesh graph)
{
//System.DateTime startTime = System.DateTime.UtcNow;
Bounds bounds = o.bounds;
Rect r = Rect.MinMaxRect(bounds.min.x, bounds.min.z, bounds.max.x, bounds.max.z);
IntRect r2 = new IntRect(
Mathf.FloorToInt(bounds.min.x * Int3.Precision),
Mathf.FloorToInt(bounds.min.z * Int3.Precision),
Mathf.FloorToInt(bounds.max.x * Int3.Precision),
Mathf.FloorToInt(bounds.max.z * Int3.Precision)
);
Int3 a = new Int3(r2.xmin, 0, r2.ymin);
Int3 b = new Int3(r2.xmin, 0, r2.ymax);
Int3 c = new Int3(r2.xmax, 0, r2.ymin);
Int3 d = new Int3(r2.xmax, 0, r2.ymax);
Int3 ia = (Int3)a;
Int3 ib = (Int3)b;
Int3 ic = (Int3)c;
Int3 id = (Int3)d;
//for (int i=0;i<nodes.Length;i++) {
graph.GetNodes(delegate(GraphNode _node) {
TriangleMeshNode node = _node as TriangleMeshNode;
bool inside = false;
int allLeft = 0;
int allRight = 0;
int allTop = 0;
int allBottom = 0;
for (int v = 0; v < 3; v++)
{
Int3 p = node.GetVertex(v);
Vector3 vert = (Vector3)p;
//Vector2 vert2D = new Vector2 (vert.x,vert.z);
if (r2.Contains(p.x, p.z))
{
//Debug.DrawRay (vert,Vector3.up*10,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 (!inside)
{
if (allLeft == 3 || allRight == 3 || allTop == 3 || allBottom == 3)
{
return(true);
}
}
//Debug.DrawLine ((Vector3)node.GetVertex(0),(Vector3)node.GetVertex(1),Color.yellow);
//Debug.DrawLine ((Vector3)node.GetVertex(1),(Vector3)node.GetVertex(2),Color.yellow);
//Debug.DrawLine ((Vector3)node.GetVertex(2),(Vector3)node.GetVertex(0),Color.yellow);
for (int v = 0; v < 3; v++)
{
int v2 = v > 1 ? 0 : v + 1;
Int3 vert1 = node.GetVertex(v);
Int3 vert2 = node.GetVertex(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 (node.ContainsPoint(ia) || node.ContainsPoint(ib) || node.ContainsPoint(ic) || node.ContainsPoint(id))
{
inside = true;
}
if (!inside)
{
return(true);
}
o.WillUpdateNode(node);
o.Apply(node);
/*Debug.DrawLine ((Vector3)node.GetVertex(0),(Vector3)node.GetVertex(1),Color.blue);
* Debug.DrawLine ((Vector3)node.GetVertex(1),(Vector3)node.GetVertex(2),Color.blue);
* Debug.DrawLine ((Vector3)node.GetVertex(2),(Vector3)node.GetVertex(0),Color.blue);
* Debug.Break ();*/
return(true);
});
//System.DateTime endTime = System.DateTime.UtcNow;
//float theTime = (endTime-startTime).Ticks*0.0001F;
//Debug.Log ("Intersecting bounds with navmesh took "+theTime.ToString ("0.000")+" ms");
}
public static void BuildFunnelCorridor (INavmesh graph, List<GraphNode> 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;
}
for (int i=startIndex;i<endIndex;i++) {
//Find the connection between the nodes
TriangleMeshNode n1 = path[i] as TriangleMeshNode;
TriangleMeshNode n2 = path[i+1] as TriangleMeshNode;
int a;
bool search = true;
for (a=0;a<3;a++) {
for (int b=0;b<3;b++) {
if (n1.GetVertexIndex(a) == n2.GetVertexIndex((b+1)%3) && n1.GetVertexIndex((a+1)%3) == n2.GetVertexIndex(b)) {
search = false;
break;
}
}
if (!search) break;
}
if (a == 3) {
left.Add ((Vector3)n1.position);
right.Add ((Vector3)n1.position);
left.Add ((Vector3)n2.position);
right.Add ((Vector3)n2.position);
} else {
left.Add ((Vector3)n1.GetVertex(a));
right.Add ((Vector3)n1.GetVertex((a+1)%3));
}
}
}
/** Applies constrained movement from \a startPos to \a endPos.
* The result is stored in \a clampedPos.
* Returns the new current node */
public GraphNode ClampAlongNavmesh(Vector3 startPos, GraphNode _startNode, Vector3 endPos, out Vector3 clampedPos)
{
ConvexMeshNode startNode = (ConvexMeshNode)_startNode;
clampedPos = endPos;
Stack <ConvexMeshNode> stack = tmpStack; // Tiny stack
List <ConvexMeshNode> closed = tmpClosed; // Tiny closed list
stack.Clear();
closed.Clear();
Vector3 bestPos, p;
float bestDist = float.PositiveInfinity;
float d;
ConvexMeshNode bestRef = null;
// Search constraint
Vector3 searchPos = (startPos + endPos) / 2;
float searchRadius = Mathfx.MagnitudeXZ(startPos, endPos) / 2;
// Init
bestPos = startPos;
stack.Push(startNode);
closed.Add(startNode); // Self ref, start maker.
INavmesh graph = AstarData.GetGraph(startNode) as INavmesh;
if (graph == null)
{
//Debug.LogError ("Null graph, or the graph was no NavMeshGraph");
return(startNode);
}
#if ASTARDEBUG
Debug.DrawLine(startPos, endPos, Color.blue);
#endif
while (stack.Count > 0)
{
// Pop front.
ConvexMeshNode cur = stack.Pop();
// If target is inside the cur, stop search.
if (NavMeshGraph.ContainsPoint(cur, endPos, graph.vertices))
{
#if ASTARDEBUG
Debug.DrawRay(endPos, Vector3.up, Color.red);
#endif
bestRef = cur;
bestPos = endPos;
break;
}
// Follow edges or keep track of nearest point on blocking edge.
for (int i = 0, j = 2; i < 3; j = i++)
{
int sp = cur.GetVertexIndex(j);
int sq = cur.GetVertexIndex(i);
bool blocking = true;
ConvexMeshNode conn = null;
for (int q = 0; q < cur.connections.Length; q++)
{
conn = cur.connections[q] as ConvexMeshNode;
if (conn == null)
{
continue;
}
for (int i2 = 0, j2 = 2; i2 < 3; j2 = i2++)
{
int sp2 = conn.GetVertexIndex(j2);
int sq2 = conn.GetVertexIndex(i2);
if ((sp2 == sp && sq2 == sq) || (sp2 == sq && sq2 == sp))
{
blocking = false;
break;
}
}
if (!blocking)
{
break;
}
}
//Node neiRef = cur->nei[j];
if (blocking)
{
// Blocked edge, calc distance.
p = Mathfx.NearestPointStrictXZ((Vector3)graph.vertices[sp], (Vector3)graph.vertices[sq], endPos);
#if ASTARDEBUG
Debug.DrawLine((Vector3)graph.vertices[sp] + Vector3.up * 0.1F, (Vector3)graph.vertices[sq] + Vector3.up * 0.1F, Color.black);
#endif
d = Mathfx.MagnitudeXZ(p, endPos);
if (d < bestDist)
{
// Update nearest distance.
bestPos = p;
bestDist = d;
bestRef = cur;
}
}
else
{
// Skip already visited.
if (closed.Contains(conn))
{
continue;
}
// Store to closed with parent for trace back.
closed.Add(conn);
#if ASTARDEBUG
Debug.DrawLine((Vector3)cur.position, (Vector3)conn.position, Color.black);
Debug.DrawLine((Vector3)graph.vertices[sp] + Vector3.up * 0.1F, (Vector3)graph.vertices[sq] + Vector3.up * 0.1F, Color.blue);
#endif
// Non-blocked edge, follow if within search radius.
p = Mathfx.NearestPointStrictXZ((Vector3)graph.vertices[sp], (Vector3)graph.vertices[sq], searchPos);
d = Mathfx.MagnitudeXZ(p, searchPos);
if (d <= searchRadius)
{
#if ASTARDEBUG
Debug.DrawLine((Vector3)searchPos - Vector3.up * 0.1F, p - Vector3.up * 0.1F, Color.cyan);
#endif
stack.Push(conn);
}
#if ASTARDEBUG
else
{
Debug.DrawLine((Vector3)searchPos - Vector3.up * 0.1F, p - Vector3.up * 0.1F, Color.red);
}
#endif
}
}
}
// Trace back and store visited curgons.
/* followVisited(bestRef,visited,closed);
* // Store best movement position.*/
clampedPos = bestPos;
// Return number of visited curs.
return(bestRef); //visited.size();
}
//These functions are for serialization, the static ones are there so other graphs using mesh nodes can serialize them more easily
public static byte[] SerializeMeshNodes(INavmesh graph, Node[] nodes)
{
System.IO.MemoryStream mem = new System.IO.MemoryStream();
System.IO.BinaryWriter stream = new System.IO.BinaryWriter(mem);
for (int i=0;i<nodes.Length;i++) {
MeshNode node = nodes[i] as MeshNode;
if (node == null) {
Debug.LogError ("Serialization Error : Couldn't cast the node to the appropriate type - NavMeshGenerator. Omitting node data.");
return null;
}
stream.Write (node.v1);
stream.Write (node.v2);
stream.Write (node.v3);
}
Int3[] vertices = graph.vertices;
if (vertices == null) {
vertices = new Int3[0];
}
stream.Write (vertices.Length);
for (int i=0;i<vertices.Length;i++) {
stream.Write (vertices[i].x);
stream.Write (vertices[i].y);
stream.Write (vertices[i].z);
}
stream.Close ();
return mem.ToArray();
}
