private static void GetAllNodesByVert(ref List <TMNodeInfo> nodeInfos, TriangleMeshNode startNode, int vertIndex)
{
if (nodeInfos == null)
{
nodeInfos = new List <TMNodeInfo>();
}
for (int i = 0; i < nodeInfos.Count; i++)
{
TMNodeInfo info2 = nodeInfos[i];
if (info2.node == startNode)
{
return;
}
}
int num2 = -1;
if (startNode.v0 == vertIndex)
{
num2 = 0;
}
else if (startNode.v1 == vertIndex)
{
num2 = 1;
}
else if (startNode.v2 == vertIndex)
{
num2 = 2;
}
else
{
return;
}
TMNodeInfo item = new TMNodeInfo {
vi = num2,
node = startNode,
v0 = startNode.GetVertex(num2 % 3),
v1 = startNode.GetVertex((num2 + 1) % 3),
v2 = startNode.GetVertex((num2 + 2) % 3)
};
nodeInfos.Add(item);
if (startNode.connections != null)
{
for (int j = 0; j < startNode.connections.Length; j++)
{
TriangleMeshNode node = startNode.connections[j] as TriangleMeshNode;
if ((node != null) && (node.GraphIndex == startNode.GraphIndex))
{
GetAllNodesByVert(ref nodeInfos, node, vertIndex);
}
}
}
}
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);
}
private static void GetAllNodesByVert(ref List <PathfindingUtility.TMNodeInfo> nodeInfos, TriangleMeshNode startNode, int vertIndex)
{
if (nodeInfos == null)
{
nodeInfos = new List <PathfindingUtility.TMNodeInfo>();
}
for (int i = 0; i < nodeInfos.get_Count(); i++)
{
if (nodeInfos.get_Item(i).node == startNode)
{
return;
}
}
int num;
if (startNode.v0 == vertIndex)
{
num = 0;
}
else if (startNode.v1 == vertIndex)
{
num = 1;
}
else
{
if (startNode.v2 != vertIndex)
{
return;
}
num = 2;
}
PathfindingUtility.TMNodeInfo tMNodeInfo = default(PathfindingUtility.TMNodeInfo);
tMNodeInfo.vi = num;
tMNodeInfo.node = startNode;
tMNodeInfo.v0 = startNode.GetVertex(num % 3);
tMNodeInfo.v1 = startNode.GetVertex((num + 1) % 3);
tMNodeInfo.v2 = startNode.GetVertex((num + 2) % 3);
nodeInfos.Add(tMNodeInfo);
if (startNode.connections != null)
{
for (int j = 0; j < startNode.connections.Length; j++)
{
TriangleMeshNode triangleMeshNode = startNode.connections[j] as TriangleMeshNode;
if (triangleMeshNode != null && triangleMeshNode.GraphIndex == startNode.GraphIndex)
{
PathfindingUtility.GetAllNodesByVert(ref nodeInfos, triangleMeshNode, vertIndex);
}
}
}
}
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 List <TriangleMeshNode> GetAllNodesInRadius(Vector3 origin, float radius)
{
//float radiusSquared = radius * radius;
TriangleMeshNode root = AStarPathfindingUtils.GetNearestNodeOnNavMesh(origin);
List <TriangleMeshNode> validNodes = new List <TriangleMeshNode>();
EB.Collections.Queue <TriangleMeshNode> toCheck = new EB.Collections.Queue <TriangleMeshNode>();
HashSet <TriangleMeshNode> visited = new HashSet <TriangleMeshNode>();
toCheck.Enqueue(root);
while (toCheck.Count > 0)
{
TriangleMeshNode curNode = toCheck.Dequeue();
if (SphereXZTriangleIntersect(origin, radius, (Vector3)curNode.GetVertex(0), (Vector3)curNode.GetVertex(1), (Vector3)curNode.GetVertex(2)))
{
validNodes.Add(curNode);
for (int i = 0; i < curNode.connections.Length; i++)
{
TriangleMeshNode connection = curNode.connections[i] as TriangleMeshNode;
if (!visited.Contains(connection))
{
toCheck.Enqueue(connection);
}
}
}
visited.Add(curNode);
}
return(validNodes);
}
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);
});
}
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();
}
// see if an infinite virtical ray intersects the plane of the triangle node
public static bool VirticalRayPlaneIntersection(Vector2 rayFrom, TriangleMeshNode node, ref Vector3 intersectionPoint)
{
Vector3 vertZero = (Vector3)node.GetVertex(0);
Vector3 vertOne = (Vector3)node.GetVertex(1);
Vector3 vertTwo = (Vector3)node.GetVertex(2);
Vector3 nodeNormal = Vector3.Cross(vertOne - vertZero, vertTwo - vertZero).normalized;
Plane plane = new Plane(nodeNormal, (Vector3)node.position); // create a plane from the triangle normal and a position on the triangle plane
// make sure our ray starts below the triangle, so a virtical up ray will hit it
float minTriangleY = Mathf.Min(vertZero.y, Mathf.Min(vertOne.y, vertTwo.y)) - 1f;
Vector3 rayFrom3d = new Vector3(rayFrom.x, minTriangleY, rayFrom.y);
Ray ray = new Ray(rayFrom3d, Vector3.up);
float dist = 0f;
if (plane.Raycast(ray, out dist))
{
intersectionPoint = rayFrom3d + Vector3.up * dist;
return(true);
}
return(false);
}
public static List <Vector3> GetRandomPointsOnNavmesh(Vector3 origin, int count, float maxRadius, float minRadius = 0.0f)
{
List <TriangleMeshNode> validNodes = GetAllNodesInRadius(origin, maxRadius);
// remove nodes in smaller radius
if (minRadius > 0.0f)
{
List <TriangleMeshNode> invalidNodes = GetAllNodesInRadius(origin, minRadius);
for (int i = 0; i < invalidNodes.Count; i++)
{
validNodes.Remove(invalidNodes[i]);
}
}
List <Vector3> points = new List <Vector3>();
if (validNodes.Count == 0)
{
//EB.Debug.LogError("No valid navmesh nodes found!");
return(points);
}
for (int i = 0; i < count; i++)
{
TriangleMeshNode randomNode = validNodes[UnityEngine.Random.Range(0, validNodes.Count)];
Vector3 randomPoint = RandomPointInTrangle((Vector3)randomNode.GetVertex(0), (Vector3)randomNode.GetVertex(1), (Vector3)randomNode.GetVertex(2));
float dist = (randomPoint - origin).magnitude;
if (dist <= maxRadius && dist > minRadius)
{
points.Add(randomPoint);
}
else
{
points.Add(origin + (randomPoint - origin).normalized * (minRadius + maxRadius) / 2.0f);
}
}
return(points);
}
private void DrawTriangleLines()
{
GL.Begin(GL.LINES);
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);
GL.Color(theColor);
TriangleMeshNode node = _node as TriangleMeshNode;
Vector3 vertZero = (Vector3)node.GetVertex(0);
Vector3 vertOne = (Vector3)node.GetVertex(1);
Vector3 vertTwo = (Vector3)node.GetVertex(2);
GL.Vertex(vertZero);
GL.Vertex(vertOne);
GL.Vertex(vertOne);
GL.Vertex(vertTwo);
GL.Vertex(vertTwo);
GL.Vertex(vertZero);
GL.Color(Color.blue);
// draw a line to all of the connected nodes (only if a two way connection exists)
for (int nodeConnect = 0; nodeConnect < node.connections.Length; nodeConnect++)
{
TriangleMeshNode connectedNode = node.connections[nodeConnect] as TriangleMeshNode;
if (null != connectedNode)
{ // go over all the nodes that this connected node is connected to in order to check that one of them is the original node, thus a two way connection exists
for (int connectedNodeConnect = 0; connectedNodeConnect < connectedNode.connections.Length; connectedNodeConnect++)
{
TriangleMeshNode originalNode = connectedNode.connections[connectedNodeConnect] as TriangleMeshNode;
if (originalNode == node) // is this the original node, meaning a two way connection exists
{
GL.Vertex((Vector3)node.position);
GL.Vertex((Vector3)connectedNode.position);
break;
}
}
}
}
return(true);
});
}
}
GL.End();
}
public void AddGraphObstacles(Simulator sim, INavmesh ng)
{
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;
int[] uses = new int[20];
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 j = 0; j < triangleMeshNode.connections.Length; j++)
{
TriangleMeshNode triangleMeshNode2 = triangleMeshNode.connections[j] as TriangleMeshNode;
if (triangleMeshNode2 != null)
{
int num3 = triangleMeshNode.SharedEdge(triangleMeshNode2);
if (num3 != -1)
{
uses[num3] = 1;
}
}
}
for (int k = 0; k < 3; k++)
{
if (uses[k] == 0)
{
int i2 = k;
int i3 = (k + 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);
}
}
}
return(true);
});
for (int i = 0; i < 2; i++)
{
bool flag = i == 1;
foreach (int num in new List <int>(outline.Keys))
{
if (flag || !hasInEdge.Contains(num))
{
int key = num;
List <Vector3> list = new List <Vector3>();
list.Add((Vector3)vertexPositions[key]);
while (outline.ContainsKey(key))
{
int num2 = outline[key];
outline.Remove(key);
Vector3 item = (Vector3)vertexPositions[num2];
list.Add(item);
if (num2 == num)
{
break;
}
key = num2;
}
if (list.Count > 1)
{
sim.AddObstacle(list.ToArray(), this.wallHeight, flag);
}
}
}
}
}
private static void MoveAlongEdge(TriangleMeshNode node, int edge, Int3 srcLoc, Int3 destLoc, out Int3 result, bool checkAnotherEdge = true)
{
Int3 vertex = node.GetVertex(edge);
Int3 vertex2 = node.GetVertex((edge + 1) % 3);
Int3 vInt = destLoc - srcLoc;
vInt.y = 0;
Int3 vInt2 = vertex2 - vertex;
vInt2.y = 0;
vInt2.NormalizeTo(1000);
int num;
num = vInt2.x * vInt.x + vInt2.z * vInt.z;
bool flag;
Int3 rhs = Polygon.IntersectionPoint(ref vertex, ref vertex2, ref srcLoc, ref destLoc, out flag);
if (!flag)
{
if (!Polygon.IsColinear(vertex, vertex2, srcLoc) || !Polygon.IsColinear(vertex, vertex2, destLoc))
{
result = srcLoc;
return;
}
if (num >= 0)
{
int num2 = vInt2.x * (vertex2.x - vertex.x) + vInt2.z * (vertex2.z - vertex.z);
int num3 = vInt2.x * (destLoc.x - vertex.x) + vInt2.z * (destLoc.z - vertex.z);
rhs = ((num2 <= num3) ? vertex2 : destLoc);
}
else
{
int num4 = -vInt2.x * (vertex.x - vertex2.x) - vInt2.z * (vertex.z - vertex2.z);
int num5 = -vInt2.x * (destLoc.x - vertex2.x) - vInt2.z * (destLoc.z - vertex2.z);
rhs = ((Mathf.Abs(num4) <= Mathf.Abs(num5)) ? vertex : destLoc);
}
}
int num6 = -IntMath.Sqrt(vertex.XZSqrMagnitude(rhs) * 1000000L);
int num7 = IntMath.Sqrt(vertex2.XZSqrMagnitude(rhs) * 1000000L);
if (num >= num6 && num <= num7)
{
result = IntMath.Divide(vInt2, (long)num, 1000000L) + rhs;
if (!node.ContainsPoint(result))
{
Vector3 vector = (Vector3)(vertex2 - vertex);
vector.y = 0f;
vector.Normalize();
Int3 lhs = vertex2 - vertex;
lhs.y = 0;
lhs *= 10000;
long num8 = (long)lhs.magnitude;
IntFactor vFactor = default(IntFactor);
vFactor.numerator = (long)num;
vFactor.denominator = num8 * 1000L;
int num9;
int num10;
PathfindingUtility.getMinMax(out num9, out num10, (long)lhs.x, ref vFactor);
int num11;
int num12;
PathfindingUtility.getMinMax(out num11, out num12, (long)lhs.z, ref vFactor);
if (!PathfindingUtility.MakePointInTriangle(ref result, node, num9, num10, num11, num12, srcLoc) && !PathfindingUtility.MakePointInTriangle(ref result, node, num9 - 4, num10 + 4, num11 - 4, num12 + 4, srcLoc))
{
result = srcLoc;
}
}
if (PathfindingUtility.MoveAxisY)
{
PathfindingUtility.CalculateY(ref result, node);
}
}
else
{
int rhs2;
int edge2;
Int3 vInt4;
if (num < num6)
{
rhs2 = num - num6;
edge2 = (edge + 2) % 3;
vInt4 = vertex;
}
else
{
rhs2 = num - num7;
edge2 = (edge + 1) % 3;
vInt4 = vertex2;
}
Int3 vInt5 = vInt2 * rhs2 / 1000000f;
int startEdge;
TriangleMeshNode neighborByEdge = node.GetNeighborByEdge(edge2, out startEdge);
if (neighborByEdge != null)
{
PathfindingUtility.checkedNodes.Add(node);
PathfindingUtility.MoveFromNode(neighborByEdge, startEdge, vInt4, vInt5 + vInt4, out result);
}
else
{
if (checkAnotherEdge)
{
Int3 vertex3 = node.GetVertex((edge + 2) % 3);
Int3 lhs2 = (vertex3 - vInt4).NormalizeTo(1000);
if (Int3.Dot(lhs2, vInt5) > 0)
{
PathfindingUtility.checkedNodes.Add(node);
PathfindingUtility.MoveAlongEdge(node, edge2, vInt4, vInt5 + vInt4, out result, false);
return;
}
}
result = vInt4;
}
}
}
private static void MoveAlongEdge(TriangleMeshNode node, int edge, VInt3 srcLoc, VInt3 destLoc, out VInt3 result)
{
bool flag;
DebugHelper.Assert((edge >= 0) && (edge <= 2));
VInt3 vertex = node.GetVertex(edge);
VInt3 num2 = node.GetVertex((edge + 1) % 3);
VInt3 num3 = destLoc - srcLoc;
num3.y = 0;
VInt3 a = num2 - vertex;
a.y = 0;
a.NormalizeTo(0x3e8);
int num5 = (a.x * num3.x) + (a.z * num3.z);
VInt3 rhs = Polygon.IntersectionPoint(ref vertex, ref num2, ref srcLoc, ref destLoc, out flag);
if (!flag)
{
if (!Polygon.IsColinear(vertex, num2, srcLoc) || !Polygon.IsColinear(vertex, num2, destLoc))
{
result = srcLoc;
return;
}
if (num5 >= 0)
{
int num7 = (a.x * (num2.x - vertex.x)) + (a.z * (num2.z - vertex.z));
int num8 = (a.x * (destLoc.x - vertex.x)) + (a.z * (destLoc.z - vertex.z));
rhs = (num7 <= num8) ? num2 : destLoc;
DebugHelper.Assert((num7 >= 0) && (num8 >= 0));
}
else
{
int num9 = (-a.x * (vertex.x - num2.x)) - (a.z * (vertex.z - num2.z));
int num10 = (-a.x * (destLoc.x - num2.x)) - (a.z * (destLoc.z - num2.z));
rhs = (Mathf.Abs(num9) <= Mathf.Abs(num10)) ? vertex : destLoc;
DebugHelper.Assert((num9 >= 0) && (num10 >= 0));
}
}
int num11 = -IntMath.Sqrt(vertex.XZSqrMagnitude(rhs) * 0xf4240L);
int num12 = IntMath.Sqrt(num2.XZSqrMagnitude(rhs) * 0xf4240L);
if ((num5 >= num11) && (num5 <= num12))
{
result = IntMath.Divide(a, (long)num5, 0xf4240L) + srcLoc;
if (!node.ContainsPoint(result))
{
int num15;
int num16;
int num17;
int num18;
Vector3 vector = (Vector3)(num2 - vertex);
vector.y = 0f;
vector.Normalize();
VInt3 num13 = num2 - vertex;
num13.y = 0;
num13 *= 0x2710;
long magnitude = num13.magnitude;
VFactor factor = new VFactor {
nom = num5,
den = magnitude * 0x3e8L
};
getMinMax(out num15, out num17, (long)num13.x, ref factor);
getMinMax(out num16, out num18, (long)num13.z, ref factor);
if (!MakePointInTriangle(ref result, node, num15, num17, num16, num18, srcLoc) && !MakePointInTriangle(ref result, node, num15 - 4, num17 + 4, num16 - 4, num18 + 4, srcLoc))
{
result = srcLoc;
}
}
if (MoveAxisY)
{
CalculateY(ref result, node);
}
}
else
{
int num19;
int num20;
VInt3 num21;
int num22;
if (num5 < num11)
{
num19 = num5 - num11;
num20 = (edge + 2) % 3;
num21 = vertex;
}
else
{
num19 = num5 - num12;
num20 = (edge + 1) % 3;
num21 = num2;
}
TriangleMeshNode neighborByEdge = node.GetNeighborByEdge(num20, out num22);
if (neighborByEdge != null)
{
VInt3 num23 = ((VInt3)((a * num19) / 1000000f)) + num21;
checkedNodes.Add(node);
MoveFromNode(neighborByEdge, num22, num21, num23, out result);
}
else
{
result = num21;
}
}
}
private static void MoveAlongEdge(TriangleMeshNode node, int edge, VInt3 srcLoc, VInt3 destLoc, MoveDirectionState state, out VInt3 result, bool checkAnotherEdge = true)
{
DebugHelper.Assert(edge >= 0 && edge <= 2);
VInt3 vertex = node.GetVertex(edge);
VInt3 vertex2 = node.GetVertex((edge + 1) % 3);
VInt3 vInt = destLoc - srcLoc;
vInt.y = 0;
VInt3 vInt2 = vertex2 - vertex;
vInt2.y = 0;
vInt2.NormalizeTo(1000);
int num;
if (state != null)
{
num = vInt.magnitude2D * 1000;
VInt3 vInt3 = state.enabled ? state.firstAdjDir : vInt;
if (VInt3.Dot(ref vInt2, ref vInt3) < 0)
{
num = -num;
vInt3 = -vInt2;
}
else
{
vInt3 = vInt2;
}
if (!state.enabled)
{
state.enabled = true;
state.firstAdjDir = VInt3.Lerp(vInt, vInt3, 1, 3);
state.firstDir = state.curDir;
state.adjDir = vInt3;
}
else if (VInt3.Dot(ref state.adjDir, ref vInt3) >= 0)
{
state.adjDir = vInt3;
}
else
{
num = 0;
}
state.applied = true;
}
else
{
num = vInt2.x * vInt.x + vInt2.z * vInt.z;
}
bool flag;
VInt3 rhs = Polygon.IntersectionPoint(ref vertex, ref vertex2, ref srcLoc, ref destLoc, out flag);
if (!flag)
{
if (!Polygon.IsColinear(vertex, vertex2, srcLoc) || !Polygon.IsColinear(vertex, vertex2, destLoc))
{
result = srcLoc;
return;
}
if (num >= 0)
{
int num2 = vInt2.x * (vertex2.x - vertex.x) + vInt2.z * (vertex2.z - vertex.z);
int num3 = vInt2.x * (destLoc.x - vertex.x) + vInt2.z * (destLoc.z - vertex.z);
rhs = ((num2 > num3) ? destLoc : vertex2);
DebugHelper.Assert(num2 >= 0 && num3 >= 0);
}
else
{
int num4 = -vInt2.x * (vertex.x - vertex2.x) - vInt2.z * (vertex.z - vertex2.z);
int num5 = -vInt2.x * (destLoc.x - vertex2.x) - vInt2.z * (destLoc.z - vertex2.z);
rhs = ((Mathf.Abs(num4) > Mathf.Abs(num5)) ? destLoc : vertex);
DebugHelper.Assert(num4 >= 0 && num5 >= 0);
}
}
int num6 = -IntMath.Sqrt(vertex.XZSqrMagnitude(rhs) * 1000000L);
int num7 = IntMath.Sqrt(vertex2.XZSqrMagnitude(rhs) * 1000000L);
if (num >= num6 && num <= num7)
{
result = IntMath.Divide(vInt2, (long)num, 1000000L) + rhs;
if (!node.ContainsPoint(result))
{
Vector3 vector = (Vector3)(vertex2 - vertex);
vector.y = 0f;
vector.Normalize();
VInt3 lhs = vertex2 - vertex;
lhs.y = 0;
lhs *= 10000;
long num8 = (long)lhs.magnitude;
VFactor vFactor = default(VFactor);
vFactor.nom = (long)num;
vFactor.den = num8 * 1000L;
int num9;
int num10;
PathfindingUtility.getMinMax(out num9, out num10, (long)lhs.x, ref vFactor);
int num11;
int num12;
PathfindingUtility.getMinMax(out num11, out num12, (long)lhs.z, ref vFactor);
if (!PathfindingUtility.MakePointInTriangle(ref result, node, num9, num10, num11, num12, srcLoc) && !PathfindingUtility.MakePointInTriangle(ref result, node, num9 - 4, num10 + 4, num11 - 4, num12 + 4, srcLoc))
{
result = srcLoc;
}
}
if (PathfindingUtility.MoveAxisY)
{
PathfindingUtility.CalculateY(ref result, node);
}
}
else
{
int rhs2;
int edge2;
VInt3 vInt4;
if (num < num6)
{
rhs2 = num - num6;
edge2 = (edge + 2) % 3;
vInt4 = vertex;
}
else
{
rhs2 = num - num7;
edge2 = (edge + 1) % 3;
vInt4 = vertex2;
}
VInt3 vInt5 = vInt2 * rhs2 / 1000000f;
int startEdge;
TriangleMeshNode neighborByEdge = node.GetNeighborByEdge(edge2, out startEdge);
if (neighborByEdge != null)
{
PathfindingUtility.checkedNodes.Add(node);
PathfindingUtility.MoveFromNode(neighborByEdge, startEdge, vInt4, vInt5 + vInt4, state, out result);
}
else
{
if (checkAnotherEdge)
{
VInt3 vertex3 = node.GetVertex((edge + 2) % 3);
VInt3 lhs2 = (vertex3 - vInt4).NormalizeTo(1000);
if (VInt3.Dot(lhs2, vInt5) > 0)
{
PathfindingUtility.checkedNodes.Add(node);
PathfindingUtility.MoveAlongEdge(node, edge2, vInt4, vInt5 + vInt4, state, out result, false);
return;
}
}
result = vInt4;
}
}
}
private static void SearchReflectionPointFromIndex(Vector3d startPosi, int index, FixedABPath path, List <Vector3d> points)
{
var graph = AstarPath.active.graphs[0] as RecastGraph;
bool findStartPoint = true;
Vector3d left = new Vector3d();
Vector3d right = new Vector3d();
int leftIndex = -1;
int rightIndex = -1;
int startPosiIndex = -1;
for (int i = index; i < path.path.Count; i++)
{
if (i != 0)
{
TriangleMeshNode prenode = path.path[i - 1] as TriangleMeshNode;
TriangleMeshNode curnode = path.path[i] as TriangleMeshNode;
int a, b;
a = prenode.SharedEdge(curnode);
b = a == 2?0:a + 1;
Int3 l = new Int3(); Int3 r = new Int3();
if (a != -1)
{
l = prenode.GetVertex(a);
}
if (b != -1)
{
r = prenode.GetVertex(b);
}
if (findStartPoint)
{
left = Int3.ToVector3D(l);
right = Int3.ToVector3D(r);
leftIndex = i;
rightIndex = i;
findStartPoint = false;
}
else
{
var tempL = Int3.ToVector3D(l);
var tempR = Int3.ToVector3D(r);
var relationL = GetRelation(left - startPosi, right - startPosi, tempL - startPosi);
var relationR = GetRelation(left - startPosi, right - startPosi, tempR - startPosi);
//寻找拐点
if (relationR == VectorRelation.Left && relationL == VectorRelation.Left)
{
bool add = AddPoint(left, points);
startPosi = left;
startPosiIndex = leftIndex;
if (add)
{
SpawnGO(left, i);
}
break;
}
else if (relationR == VectorRelation.Right && relationL == VectorRelation.Right)
{
bool add = AddPoint(right, points);
startPosi = right;
startPosiIndex = rightIndex;
if (add)
{
SpawnGO(right, i);
}
break;
}
else if (relationR == VectorRelation.Equal && relationL == VectorRelation.Right)
{
bool add = AddPoint(right, points);
startPosi = right;
startPosiIndex = rightIndex;
if (add)
{
SpawnGO(right, i);
}
break;
}
else if (relationR == VectorRelation.Equal && relationL == VectorRelation.Left)
{
bool add = AddPoint(left, points);
startPosi = left;
startPosiIndex = leftIndex;
if (add)
{
SpawnGO(left, i);
}
break;
}
//else if (relationL == VectorRelation.Equal && relationR == VectorRelation.Left)
//{
// bool add = AddPoint(left, points);
// startPosi = left;
// startPosiIndex = i;
// if (add)
// SpawnGO(left, i);
// break;
//}
//else if (relationL == VectorRelation.Equal && relationR == VectorRelation.Right)
//{
// bool add = AddPoint(right, points);
// startPosi = right;
// startPosiIndex = i;
// if(add)
// SpawnGO(right, i);
// break;
//}
//改变左右
if (relationR == VectorRelation.Inside && relationL == VectorRelation.Inside)
{
left = tempL;
right = tempR;
leftIndex = i;
rightIndex = i;
}
if (relationL == VectorRelation.Inside || relationL == VectorRelation.Equal)
{
left = tempL;
leftIndex = i;
}
if (relationR == VectorRelation.Inside || relationR == VectorRelation.Equal)
{
right = tempR;
rightIndex = i;
}
if (i == path.path.Count - 1)
{
var relationEndPoint = GetRelation(left - startPosi, right - startPosi,
path.EndPoint - startPosi);
if (relationEndPoint == VectorRelation.Left)
{
AddPoint(left, points);
SpawnGO(left, i);
}
else if (relationEndPoint == VectorRelation.Right)
{
AddPoint(right, points);
SpawnGO(right, i);
}
}
}
}
}
if (startPosiIndex != -1)
{
startPosiIndex++;
SearchReflectionPointFromIndex(startPosi, startPosiIndex, path, points);
}
}
private static void MoveAlongEdge(TriangleMeshNode node, int edge, VInt3 srcLoc, VInt3 destLoc, MoveDirectionState state, out VInt3 result, bool checkAnotherEdge = true)
{
bool flag;
DebugHelper.Assert((edge >= 0) && (edge <= 2));
VInt3 vertex = node.GetVertex(edge);
VInt3 num2 = node.GetVertex((edge + 1) % 3);
VInt3 a = destLoc - srcLoc;
a.y = 0;
VInt3 lhs = num2 - vertex;
lhs.y = 0;
lhs.NormalizeTo(0x3e8);
int num5 = 0;
if (state != null)
{
num5 = a.magnitude2D * 0x3e8;
VInt3 num6 = !state.enabled ? a : state.firstAdjDir;
if (VInt3.Dot(ref lhs, ref num6) < 0)
{
num5 = -num5;
num6 = -lhs;
}
else
{
num6 = lhs;
}
if (!state.enabled)
{
state.enabled = true;
state.firstAdjDir = VInt3.Lerp(a, num6, 1, 3);
state.firstDir = state.curDir;
state.adjDir = num6;
}
else if (VInt3.Dot(ref state.adjDir, ref num6) >= 0)
{
state.adjDir = num6;
}
else
{
num5 = 0;
}
state.applied = true;
}
else
{
num5 = (lhs.x * a.x) + (lhs.z * a.z);
}
VInt3 rhs = Polygon.IntersectionPoint(ref vertex, ref num2, ref srcLoc, ref destLoc, out flag);
if (!flag)
{
if (!Polygon.IsColinear(vertex, num2, srcLoc) || !Polygon.IsColinear(vertex, num2, destLoc))
{
result = srcLoc;
return;
}
if (num5 >= 0)
{
int num8 = (lhs.x * (num2.x - vertex.x)) + (lhs.z * (num2.z - vertex.z));
int num9 = (lhs.x * (destLoc.x - vertex.x)) + (lhs.z * (destLoc.z - vertex.z));
rhs = (num8 <= num9) ? num2 : destLoc;
DebugHelper.Assert((num8 >= 0) && (num9 >= 0));
}
else
{
int num10 = (-lhs.x * (vertex.x - num2.x)) - (lhs.z * (vertex.z - num2.z));
int num11 = (-lhs.x * (destLoc.x - num2.x)) - (lhs.z * (destLoc.z - num2.z));
rhs = (Mathf.Abs(num10) <= Mathf.Abs(num11)) ? vertex : destLoc;
DebugHelper.Assert((num10 >= 0) && (num11 >= 0));
}
}
int num12 = -IntMath.Sqrt(vertex.XZSqrMagnitude(rhs) * 0xf4240L);
int num13 = IntMath.Sqrt(num2.XZSqrMagnitude(rhs) * 0xf4240L);
if ((num5 >= num12) && (num5 <= num13))
{
result = IntMath.Divide(lhs, (long)num5, 0xf4240L) + rhs;
if (!node.ContainsPoint(result))
{
int num16;
int num17;
int num18;
int num19;
Vector3 vector = (Vector3)(num2 - vertex);
vector.y = 0f;
vector.Normalize();
VInt3 num14 = num2 - vertex;
num14.y = 0;
num14 *= 0x2710;
long magnitude = num14.magnitude;
VFactor factor = new VFactor {
nom = num5,
den = magnitude * 0x3e8L
};
getMinMax(out num16, out num18, (long)num14.x, ref factor);
getMinMax(out num17, out num19, (long)num14.z, ref factor);
if (!MakePointInTriangle(ref result, node, num16, num18, num17, num19, srcLoc) && !MakePointInTriangle(ref result, node, num16 - 4, num18 + 4, num17 - 4, num19 + 4, srcLoc))
{
result = srcLoc;
}
}
if (MoveAxisY)
{
CalculateY(ref result, node);
}
}
else
{
int num20;
int num21;
VInt3 num22;
int num24;
if (num5 < num12)
{
num20 = num5 - num12;
num21 = (edge + 2) % 3;
num22 = vertex;
}
else
{
num20 = num5 - num13;
num21 = (edge + 1) % 3;
num22 = num2;
}
VInt3 num23 = (VInt3)((lhs * num20) / 1000000f);
TriangleMeshNode neighborByEdge = node.GetNeighborByEdge(num21, out num24);
if (neighborByEdge != null)
{
checkedNodes.Add(node);
MoveFromNode(neighborByEdge, num24, num22, num23 + num22, state, out result);
}
else
{
if (checkAnotherEdge)
{
VInt3 num27 = node.GetVertex((edge + 2) % 3) - num22;
if (VInt3.Dot(num27.NormalizeTo(0x3e8), num23) > 0)
{
checkedNodes.Add(node);
MoveAlongEdge(node, num21, num22, num23 + num22, state, out result, false);
return;
}
}
result = num22;
}
}
}
// this is a utility function used by IsVisibleOnNavMeshOptimized, before calling this function is should be determined that the line 'from' -> 'to' starts outside
// the triangle, and enters the triangle, this function will then determine if the line exits the triangle
// the exitPoint returned will be on the ground plane
private static bool DoesLineExitTriangle(Vector3 from, Vector3 to, TriangleMeshNode currentTriangle, ref HashSet <TriangleMeshNode> previousTriangles, ref TriangleMeshNode nextTriangle, ref Vector3 exitPoint)
{
if (currentTriangle.ContainsPoint((Int3)to)) // does the line end in this triangle
{
return(false); // the line does not exit this triangle
}
const int NumVerts = 3;
if (null != currentTriangle.connections)
{
for (int connect = 0; connect < currentTriangle.connections.Length; connect++) // go through all our connections
{
TriangleMeshNode other = currentTriangle.connections[connect] as TriangleMeshNode;
int edge = -1;
int otherEdge = -1;
if (other != null && !previousTriangles.Contains(other) && SharedEdge(currentTriangle, other, ref edge, ref otherEdge)) // get the connecting edge
{
bool intersectsEdge = false;
bool intersectsOtherEdge = true;
// check to see if the line enters the other triangle through this edge
exitPoint = VectorMath.SegmentIntersectionPointXZ(from, to, (Vector3)currentTriangle.GetVertex(edge), (Vector3)currentTriangle.GetVertex((edge + 1) % NumVerts), out intersectsEdge);
int tileIndex = (currentTriangle.GetVertexIndex(0) >> RecastGraph.TileIndexOffset) & RecastGraph.TileIndexMask;
int otherTileIndex = (other.GetVertexIndex(0) >> RecastGraph.TileIndexOffset) & RecastGraph.TileIndexMask;
if (tileIndex != otherTileIndex) // if the edjoining triangles are on different tiles, the edges may not match exactly, so we need to be sure that both triangles edges are intersected
{
exitPoint = VectorMath.SegmentIntersectionPointXZ(from, to, (Vector3)other.GetVertex(otherEdge), (Vector3)other.GetVertex((otherEdge + 1) % NumVerts), out intersectsOtherEdge);
}
if (intersectsEdge && intersectsOtherEdge)
{
nextTriangle = other;
return(true);
}
}
}
}
Vector3 prevVertPos = (Vector3)currentTriangle.GetVertex(0);
float farthestIntersectionSqr = -1f;
for (int vert = 1; vert <= NumVerts; ++vert) // go over all the edges, to see which one the line passes out of, so we can calculate the triangle exit point
{
Vector3 vertPos = (Vector3)currentTriangle.GetVertex(vert % NumVerts);
bool intersects = false;
Vector3 potentialExitPoint = VectorMath.SegmentIntersectionPointXZ(from, to, prevVertPos, vertPos, out intersects);
if (intersects)
{
float intersectionSqr = (potentialExitPoint - from).sqrMagnitude;
// we're looking for the farthest intersection, as the nearest intersection would have been the edge intersected when the line entered the triangle
if (intersectionSqr > farthestIntersectionSqr)
{
farthestIntersectionSqr = intersectionSqr;
exitPoint = potentialExitPoint;
}
}
prevVertPos = vertPos;
}
return(farthestIntersectionSqr > 0f);
}
/** 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);
});
}
private void DrawNodeCosts()
{
if (doDrawNodeCosts)
{
PlayerController player = PlayerManager.LocalPlayerController();
if (null != player)
{
NNInfo info = AstarPath.active.GetNearest(player.GetComponent <LocomotionComponentAPP>().Destination);
TriangleMeshNode node = info.node as TriangleMeshNode;
if (null != node)
{
for (int i = 0; i < 3; ++i)
{
int next = (i + 1) % 3;
Vector3 one = (Vector3)node.GetVertex(i);
Vector3 two = (Vector3)node.GetVertex(next);
GLRenderingUtils.DoDrawLine(one, two, Color.white);
}
int highestConnectionCostIndex = 0;
for (int connect = 1; connect < node.connections.Length; ++connect)
{
if (node.connectionCosts[connect] > node.connectionCosts[highestConnectionCostIndex])
{
highestConnectionCostIndex = connect;
}
}
Color[] colors = new Color[] { Color.red, Color.green, Color.blue };
string[] colorNames = new string[] { "Color.red", "Color.green", "Color.blue" };
const float HeightOffset = 2f;
for (int connect = 0; connect < node.connections.Length; ++connect)
{
TriangleMeshNode nodeConnect = node.connections[connect] as TriangleMeshNode;
GLRenderingUtils.DoDrawLine((Vector3)node.position, (Vector3)nodeConnect.position, colors[Mathf.Min(connect, colors.Length - 1)]);
float dist = GameUtils.SubXZ((Vector3)node.position, (Vector3)nodeConnect.position).magnitude;
Vector3 offset = new Vector3(0f, ((float)node.connectionCosts[connect] / (float)node.connectionCosts[highestConnectionCostIndex]) * HeightOffset, 0f);
for (int i = 0; i < 3; ++i)
{
int next = (i + 1) % 3;
Vector3 one = (Vector3)nodeConnect.GetVertex(i);
Vector3 two = (Vector3)nodeConnect.GetVertex(next);
Vector3 oneOffset = one + offset;
Vector3 twoOffset = two + offset;
Color col = colors[Mathf.Min(connect, colors.Length - 1)];
GLRenderingUtils.DoDrawLine(oneOffset, twoOffset, col);
GLRenderingUtils.DoDrawLine(one, oneOffset, col);
GLRenderingUtils.DoDrawLine(two, twoOffset, col);
}
if (doPrintNodeCosts)
{
DebugSystem.Log(colorNames[Mathf.Min(connect, colorNames.Length - 1)] + ": Cost: " + node.connectionCosts[connect] + " Dist: " + dist, "Pathfinding");
}
}
}
}
}
doPrintNodeCosts = false;
}
