public static bool SegmentsIntersectXZ(Int3 start1, Int3 end1, Int3 start2, Int3 end2)
{
return(VectorMath.RightOrColinearXZ(start1, end1, start2) != VectorMath.RightOrColinearXZ(start1, end1, end2) && VectorMath.RightOrColinearXZ(start2, end2, start1) != VectorMath.RightOrColinearXZ(start2, end2, end1));
}
bool FindNextCorners(Vector3 origin, int startIndex, List <Vector3> funnelPath, int numCorners, out bool lastCorner)
{
lastCorner = false;
if (left == null)
{
throw new System.Exception("left list is null");
}
if (right == null)
{
throw new System.Exception("right list is null");
}
if (funnelPath == null)
{
throw new System.ArgumentNullException("funnelPath");
}
if (left.Count != right.Count)
{
throw new System.ArgumentException("left and right lists must have equal length");
}
int diagonalCount = left.Count;
if (diagonalCount == 0)
{
throw new System.ArgumentException("no diagonals");
}
if (diagonalCount - startIndex < 3)
{
//Direct path
funnelPath.Add(left[diagonalCount - 1]);
lastCorner = true;
return(true);
}
#if ASTARDEBUG
for (int i = startIndex; i < left.Count - 1; i++)
{
Debug.DrawLine(left[i], left[i + 1], Color.red);
Debug.DrawLine(right[i], right[i + 1], Color.magenta);
Debug.DrawRay(right[i], Vector3.up, Color.magenta);
}
for (int i = 0; i < left.Count; i++)
{
Debug.DrawLine(right[i], left[i], Color.cyan);
}
#endif
//Remove identical vertices
while (left[startIndex + 1] == left[startIndex + 2] && right[startIndex + 1] == right[startIndex + 2])
{
//System.Console.WriteLine ("Removing identical left and right");
//left.RemoveAt (1);
//right.RemoveAt (1);
startIndex++;
if (diagonalCount - startIndex <= 3)
{
return(false);
}
}
Vector3 swPoint = left[startIndex + 2];
if (swPoint == left[startIndex + 1])
{
swPoint = right[startIndex + 2];
}
//Test
while (VectorMath.IsColinearXZ(origin, left[startIndex + 1], right[startIndex + 1]) || VectorMath.RightOrColinearXZ(left[startIndex + 1], right[startIndex + 1], swPoint) == VectorMath.RightOrColinearXZ(left[startIndex + 1], right[startIndex + 1], origin))
{
#if ASTARDEBUG
Debug.DrawLine(left[startIndex + 1], right[startIndex + 1], new Color(0, 0, 0, 0.5F));
Debug.DrawLine(origin, swPoint, new Color(0, 0, 0, 0.5F));
#endif
//left.RemoveAt (1);
//right.RemoveAt (1);
startIndex++;
if (diagonalCount - startIndex < 3)
{
//Debug.Log ("#2 " + left.Count + " - " + startIndex + " = " + (left.Count-startIndex));
//Direct path
funnelPath.Add(left[diagonalCount - 1]);
lastCorner = true;
return(true);
}
swPoint = left[startIndex + 2];
if (swPoint == left[startIndex + 1])
{
swPoint = right[startIndex + 2];
}
}
//funnelPath.Add (origin);
Vector3 portalApex = origin;
Vector3 portalLeft = left[startIndex + 1];
Vector3 portalRight = right[startIndex + 1];
int apexIndex = startIndex + 0;
int rightIndex = startIndex + 1;
int leftIndex = startIndex + 1;
for (int i = startIndex + 2; i < diagonalCount; i++)
{
if (funnelPath.Count >= numCorners)
{
return(true);
}
if (funnelPath.Count > 2000)
{
Debug.LogWarning("Avoiding infinite loop. Remove this check if you have this long paths.");
break;
}
Vector3 pLeft = left[i];
Vector3 pRight = right[i];
/*Debug.DrawLine (portalApex,portalLeft,Color.red);
* Debug.DrawLine (portalApex,portalRight,Color.yellow);
* Debug.DrawLine (portalApex,left,Color.cyan);
* Debug.DrawLine (portalApex,right,Color.cyan);*/
if (VectorMath.SignedTriangleAreaTimes2XZ(portalApex, portalRight, pRight) >= 0)
{
if (portalApex == portalRight || VectorMath.SignedTriangleAreaTimes2XZ(portalApex, portalLeft, pRight) <= 0)
{
portalRight = pRight;
rightIndex = i;
}
else
{
funnelPath.Add(portalLeft);
portalApex = portalLeft;
apexIndex = leftIndex;
portalLeft = portalApex;
portalRight = portalApex;
leftIndex = apexIndex;
rightIndex = apexIndex;
i = apexIndex;
continue;
}
}
if (VectorMath.SignedTriangleAreaTimes2XZ(portalApex, portalLeft, pLeft) <= 0)
{
if (portalApex == portalLeft || VectorMath.SignedTriangleAreaTimes2XZ(portalApex, portalRight, pLeft) >= 0)
{
portalLeft = pLeft;
leftIndex = i;
}
else
{
funnelPath.Add(portalRight);
portalApex = portalRight;
apexIndex = rightIndex;
portalLeft = portalApex;
portalRight = portalApex;
leftIndex = apexIndex;
rightIndex = apexIndex;
i = apexIndex;
continue;
}
}
}
lastCorner = true;
funnelPath.Add(left[diagonalCount - 1]);
return(true);
}
public override Vector3 ClosestPointOnNodeXZ(Vector3 _p)
{
// Get the object holding the vertex data for this node
// This is usually a graph or a recast graph tile
INavmeshHolder g = GetNavmeshHolder(GraphIndex);
// Get all 3 vertices for this node
Int3 tp1 = g.GetVertex(v0);
Int3 tp2 = g.GetVertex(v1);
Int3 tp3 = g.GetVertex(v2);
// We need the point as an Int3
var p = (Int3)_p;
// Save the original y coordinate, we will return a point with the same y coordinate
int oy = p.y;
// Assumes the triangle vertices are laid out in (counter?)clockwise order
tp1.y = 0;
tp2.y = 0;
tp3.y = 0;
p.y = 0;
if ((long)(tp2.x - tp1.x) * (long)(p.z - tp1.z) - (long)(p.x - tp1.x) * (long)(tp2.z - tp1.z) > 0)
{
float f = Mathf.Clamp01(VectorMath.ClosestPointOnLineFactor(tp1, tp2, p));
return(new Vector3(tp1.x + (tp2.x - tp1.x) * f, oy, tp1.z + (tp2.z - tp1.z) * f) * Int3.PrecisionFactor);
}
else if ((long)(tp3.x - tp2.x) * (long)(p.z - tp2.z) - (long)(p.x - tp2.x) * (long)(tp3.z - tp2.z) > 0)
{
float f = Mathf.Clamp01(VectorMath.ClosestPointOnLineFactor(tp2, tp3, p));
return(new Vector3(tp2.x + (tp3.x - tp2.x) * f, oy, tp2.z + (tp3.z - tp2.z) * f) * Int3.PrecisionFactor);
}
else if ((long)(tp1.x - tp3.x) * (long)(p.z - tp3.z) - (long)(p.x - tp3.x) * (long)(tp1.z - tp3.z) > 0)
{
float f = Mathf.Clamp01(VectorMath.ClosestPointOnLineFactor(tp3, tp1, p));
return(new Vector3(tp3.x + (tp1.x - tp3.x) * f, oy, tp3.z + (tp1.z - tp3.z) * f) * Int3.PrecisionFactor);
}
else
{
return(_p);
}
/*
* Equivalent to the above, but the above uses manual inlining
* if (!VectorMath.RightOrColinearXZ (tp1, tp2, p)) {
* float f = Mathf.Clamp01 (VectorMath.ClosestPointOnLineFactor (tp1, tp2, p));
* return new Vector3(tp1.x + (tp2.x-tp1.x)*f, oy, tp1.z + (tp2.z-tp1.z)*f)*Int3.PrecisionFactor;
* } else if (!VectorMath.RightOrColinearXZ (tp2, tp3, p)) {
* float f = Mathf.Clamp01 (VectorMath.ClosestPointOnLineFactor (tp2, tp3, p));
* return new Vector3(tp2.x + (tp3.x-tp2.x)*f, oy, tp2.z + (tp3.z-tp2.z)*f)*Int3.PrecisionFactor;
* } else if (!VectorMath.RightOrColinearXZ (tp3, tp1, p)) {
* float f = Mathf.Clamp01 (VectorMath.ClosestPointOnLineFactor (tp3, tp1, p));
* return new Vector3(tp3.x + (tp1.x-tp3.x)*f, oy, tp3.z + (tp1.z-tp3.z)*f)*Int3.PrecisionFactor;
* } else {
* return _p;
* }*/
/* Almost equivalent to the above, but this is slower
* Vector3 tp1 = (Vector3)g.GetVertex(v0);
* Vector3 tp2 = (Vector3)g.GetVertex(v1);
* Vector3 tp3 = (Vector3)g.GetVertex(v2);
* tp1.y = 0;
* tp2.y = 0;
* tp3.y = 0;
* _p.y = 0;
* return Pathfinding.Polygon.ClosestPointOnTriangle (tp1,tp2,tp3,_p);*/
}
/** 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 Left or colinear
if (!VectorMath.RightXZ(a, b, hit.origin))
{
if (VectorMath.RightXZ(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 (VectorMath.LineIntersectionFactorXZ(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 left or colinear
if (!VectorMath.RightXZ(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 (VectorMath.RightXZ(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 (VectorMath.LineIntersectionFactorXZ(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;
}
}
/** Generates a navmesh. Based on the supplied vertices and triangles */
void GenerateNodes(Vector3[] vectorVertices, int[] triangles, out Vector3[] originalVertices, out Int3[] vertices)
{
UnityEngine.Profiling.Profiler.BeginSample("Init");
if (vectorVertices.Length == 0 || triangles.Length == 0)
{
originalVertices = vectorVertices;
vertices = new Int3[0];
nodes = new TriangleMeshNode[0];
return;
}
vertices = new Int3[vectorVertices.Length];
int c = 0;
for (int i = 0; i < vertices.Length; i++)
{
vertices[i] = (Int3)matrix.MultiplyPoint3x4(vectorVertices[i]);
}
var hashedVerts = new Dictionary <Int3, int>();
var newVertices = new int[vertices.Length];
UnityEngine.Profiling.Profiler.EndSample();
UnityEngine.Profiling.Profiler.BeginSample("Hashing");
for (int i = 0; i < vertices.Length; i++)
{
if (!hashedVerts.ContainsKey(vertices[i]))
{
newVertices[c] = i;
hashedVerts.Add(vertices[i], c);
c++;
}
}
for (int x = 0; x < triangles.Length; x++)
{
Int3 vertex = vertices[triangles[x]];
triangles[x] = hashedVerts[vertex];
}
Int3[] totalIntVertices = vertices;
vertices = new Int3[c];
originalVertices = new Vector3[c];
for (int i = 0; i < c; i++)
{
vertices[i] = totalIntVertices[newVertices[i]];
originalVertices[i] = vectorVertices[newVertices[i]];
}
UnityEngine.Profiling.Profiler.EndSample();
UnityEngine.Profiling.Profiler.BeginSample("Constructing Nodes");
nodes = new TriangleMeshNode[triangles.Length / 3];
int graphIndex = active.astarData.GetGraphIndex(this);
// Does not have to set this, it is set in ScanInternal
//TriangleMeshNode.SetNavmeshHolder ((int)graphIndex,this);
for (int i = 0; i < nodes.Length; i++)
{
nodes[i] = new TriangleMeshNode(active);
TriangleMeshNode node = nodes[i]; //new MeshNode ();
node.GraphIndex = (uint)graphIndex;
node.Penalty = initialPenalty;
node.Walkable = true;
node.v0 = triangles[i * 3];
node.v1 = triangles[i * 3 + 1];
node.v2 = triangles[i * 3 + 2];
if (!VectorMath.IsClockwiseXZ(vertices[node.v0], vertices[node.v1], vertices[node.v2]))
{
//Debug.DrawLine (vertices[node.v0],vertices[node.v1],Color.red);
//Debug.DrawLine (vertices[node.v1],vertices[node.v2],Color.red);
//Debug.DrawLine (vertices[node.v2],vertices[node.v0],Color.red);
int tmp = node.v0;
node.v0 = node.v2;
node.v2 = tmp;
}
if (VectorMath.IsColinearXZ(vertices[node.v0], vertices[node.v1], vertices[node.v2]))
{
Debug.DrawLine((Vector3)vertices[node.v0], (Vector3)vertices[node.v1], Color.red);
Debug.DrawLine((Vector3)vertices[node.v1], (Vector3)vertices[node.v2], Color.red);
Debug.DrawLine((Vector3)vertices[node.v2], (Vector3)vertices[node.v0], Color.red);
}
// Make sure position is correctly set
node.UpdatePositionFromVertices();
}
UnityEngine.Profiling.Profiler.EndSample();
var sides = new Dictionary <Int2, TriangleMeshNode>();
for (int i = 0, j = 0; i < triangles.Length; j += 1, i += 3)
{
sides[new Int2(triangles[i + 0], triangles[i + 1])] = nodes[j];
sides[new Int2(triangles[i + 1], triangles[i + 2])] = nodes[j];
sides[new Int2(triangles[i + 2], triangles[i + 0])] = nodes[j];
}
UnityEngine.Profiling.Profiler.BeginSample("Connecting Nodes");
var connections = new List <MeshNode>();
var connectionCosts = new List <uint>();
for (int i = 0, j = 0; i < triangles.Length; j += 1, i += 3)
{
connections.Clear();
connectionCosts.Clear();
TriangleMeshNode node = nodes[j];
for (int q = 0; q < 3; q++)
{
TriangleMeshNode other;
if (sides.TryGetValue(new Int2(triangles[i + ((q + 1) % 3)], triangles[i + q]), out other))
{
connections.Add(other);
connectionCosts.Add((uint)(node.position - other.position).costMagnitude);
}
}
node.connections = connections.ToArray();
node.connectionCosts = connectionCosts.ToArray();
}
UnityEngine.Profiling.Profiler.EndSample();
UnityEngine.Profiling.Profiler.BeginSample("Rebuilding BBTree");
RebuildBBTree(this);
UnityEngine.Profiling.Profiler.EndSample();
#if ASTARDEBUG
for (int i = 0; i < nodes.Length; i++)
{
TriangleMeshNode node = nodes[i] as TriangleMeshNode;
float a1 = VectorMath.SignedTriangleAreaTimes2XZ((Vector3)vertices[node.v0], (Vector3)vertices[node.v1], (Vector3)vertices[node.v2]);
long a2 = VectorMath.SignedTriangleAreaTimes2XZ(vertices[node.v0], vertices[node.v1], vertices[node.v2]);
if (a1 * a2 < 0)
{
Debug.LogError(a1 + " " + a2);
}
if (VectorMath.IsClockwiseXZ(vertices[node.v0], vertices[node.v1], vertices[node.v2]))
{
Debug.DrawLine((Vector3)vertices[node.v0], (Vector3)vertices[node.v1], Color.green);
Debug.DrawLine((Vector3)vertices[node.v1], (Vector3)vertices[node.v2], Color.green);
Debug.DrawLine((Vector3)vertices[node.v2], (Vector3)vertices[node.v0], Color.green);
}
else
{
Debug.DrawLine((Vector3)vertices[node.v0], (Vector3)vertices[node.v1], Color.red);
Debug.DrawLine((Vector3)vertices[node.v1], (Vector3)vertices[node.v2], Color.red);
Debug.DrawLine((Vector3)vertices[node.v2], (Vector3)vertices[node.v0], Color.red);
}
}
#endif
}
// Token: 0x0600251A RID: 9498 RVA: 0x0019D24C File Offset: 0x0019B44C
public static void UpdateArea(GraphUpdateObject o, INavmeshHolder graph)
{
Bounds bounds = graph.transform.InverseTransform(o.bounds);
IntRect irect = new IntRect(Mathf.FloorToInt(bounds.min.x * 1000f), Mathf.FloorToInt(bounds.min.z * 1000f), Mathf.CeilToInt(bounds.max.x * 1000f), Mathf.CeilToInt(bounds.max.z * 1000f));
Int3 a = new Int3(irect.xmin, 0, irect.ymin);
Int3 b = new Int3(irect.xmin, 0, irect.ymax);
Int3 c = new Int3(irect.xmax, 0, irect.ymin);
Int3 d = new Int3(irect.xmax, 0, irect.ymax);
int ymin = ((Int3)bounds.min).y;
int ymax = ((Int3)bounds.max).y;
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 vertexInGraphSpace = triangleMeshNode.GetVertexInGraphSpace(i);
if (irect.Contains(vertexInGraphSpace.x, vertexInGraphSpace.z))
{
flag = true;
break;
}
if (vertexInGraphSpace.x < irect.xmin)
{
num++;
}
if (vertexInGraphSpace.x > irect.xmax)
{
num2++;
}
if (vertexInGraphSpace.z < irect.ymin)
{
num3++;
}
if (vertexInGraphSpace.z > irect.ymax)
{
num4++;
}
}
if (!flag && (num == 3 || num2 == 3 || num3 == 3 || num4 == 3))
{
return;
}
for (int j = 0; j < 3; j++)
{
int i2 = (j > 1) ? 0 : (j + 1);
Int3 vertexInGraphSpace2 = triangleMeshNode.GetVertexInGraphSpace(j);
Int3 vertexInGraphSpace3 = triangleMeshNode.GetVertexInGraphSpace(i2);
if (VectorMath.SegmentsIntersectXZ(a, b, vertexInGraphSpace2, vertexInGraphSpace3))
{
flag = true;
break;
}
if (VectorMath.SegmentsIntersectXZ(a, c, vertexInGraphSpace2, vertexInGraphSpace3))
{
flag = true;
break;
}
if (VectorMath.SegmentsIntersectXZ(c, d, vertexInGraphSpace2, vertexInGraphSpace3))
{
flag = true;
break;
}
if (VectorMath.SegmentsIntersectXZ(d, b, vertexInGraphSpace2, vertexInGraphSpace3))
{
flag = true;
break;
}
}
if (flag || triangleMeshNode.ContainsPointInGraphSpace(a) || triangleMeshNode.ContainsPointInGraphSpace(b) || triangleMeshNode.ContainsPointInGraphSpace(c) || triangleMeshNode.ContainsPointInGraphSpace(d))
{
flag = true;
}
if (!flag)
{
return;
}
int num5 = 0;
int num6 = 0;
for (int k = 0; k < 3; k++)
{
Int3 vertexInGraphSpace4 = triangleMeshNode.GetVertexInGraphSpace(k);
if (vertexInGraphSpace4.y < ymin)
{
num6++;
}
if (vertexInGraphSpace4.y > ymax)
{
num5++;
}
}
if (num6 == 3 || num5 == 3)
{
return;
}
o.WillUpdateNode(triangleMeshNode);
o.Apply(triangleMeshNode);
});
}
// Update is called once per frame
void LateUpdate()
{
if (prevNode == null)
{
//Good Game
//var nninfo = AstarPath.active.GetNearest(transform.position);
var nninfo = AstarPath.active.GetNearest((VInt3)transform.position);
prevNode = nninfo.node;
//Good Game
//prevPos = transform.position;
prevPos = (VInt3)transform.position;
}
if (prevNode == null)
{
return;
}
if (prevNode != null)
{
var graph = AstarData.GetGraph(prevNode) as IRaycastableGraph;
if (graph != null)
{
GraphHitInfo hit;
//Good Game
//if (graph.Linecast(prevPos, transform.position, prevNode, out hit)) {
if (graph.Linecast(prevPos, (VInt3)transform.position, prevNode, out hit))
{
//Good Game
//hit.point.y = transform.position.y;
hit.point.y = (int)transform.position.y;
//Good Game
//Vector3 closest = VectorMath.ClosestPointOnLine(hit.tangentOrigin, hit.tangentOrigin+hit.tangent, transform.position);
Vector3 closest = VectorMath.ClosestPointOnLine((Vector3)hit.tangentOrigin, (Vector3)(hit.tangentOrigin + hit.tangent), transform.position);
//Good Game
//Vector3 ohit = hit.point;
Vector3 ohit = (Vector3)hit.point;
ohit = ohit + Vector3.ClampMagnitude((Vector3)hit.node.position - ohit, 0.008f);
//Good Game
/*if (graph.Linecast(ohit, closest, hit.node, out hit)) {
* hit.point.y = transform.position.y;
* transform.position = hit.point;*/
if (graph.Linecast((VInt3)ohit, (VInt3)closest, hit.node, out hit))
{
hit.point.y = (int)transform.position.y;
transform.position = (Vector3)hit.point;
}
else
{
closest.y = transform.position.y;
transform.position = closest;
}
}
prevNode = hit.node;
}
}
//Good Game
//prevPos = transform.position;
prevPos = (VInt3)transform.position;
}
public static bool ContainsPoint(TriangleMeshNode node, Vector3 pos, Int3[] vertices)
{
if (!VectorMath.IsClockwiseMarginXZ((Vector3)vertices[node.v0], (Vector3)vertices[node.v1], (Vector3)vertices[node.v2]))
{
Debug.LogError("Noes!");
}
return(VectorMath.IsClockwiseMarginXZ((Vector3)vertices[node.v0], (Vector3)vertices[node.v1], pos) && VectorMath.IsClockwiseMarginXZ((Vector3)vertices[node.v1], (Vector3)vertices[node.v2], pos) && VectorMath.IsClockwiseMarginXZ((Vector3)vertices[node.v2], (Vector3)vertices[node.v0], pos));
}
private void GenerateNodes(Vector3[] vectorVertices, int[] triangles, out Vector3[] originalVertices, out Int3[] vertices)
{
if (vectorVertices.Length == 0 || triangles.Length == 0)
{
originalVertices = vectorVertices;
vertices = new Int3[0];
this.nodes = new TriangleMeshNode[0];
return;
}
vertices = new Int3[vectorVertices.Length];
int num = 0;
for (int i = 0; i < vertices.Length; i++)
{
vertices[i] = (Int3)this.matrix.MultiplyPoint3x4(vectorVertices[i]);
}
Dictionary <Int3, int> dictionary = new Dictionary <Int3, int>();
int[] array = new int[vertices.Length];
for (int j = 0; j < vertices.Length; j++)
{
if (!dictionary.ContainsKey(vertices[j]))
{
array[num] = j;
dictionary.Add(vertices[j], num);
num++;
}
}
for (int k = 0; k < triangles.Length; k++)
{
Int3 key = vertices[triangles[k]];
triangles[k] = dictionary[key];
}
Int3[] array2 = vertices;
vertices = new Int3[num];
originalVertices = new Vector3[num];
for (int l = 0; l < num; l++)
{
vertices[l] = array2[array[l]];
originalVertices[l] = vectorVertices[array[l]];
}
this.nodes = new TriangleMeshNode[triangles.Length / 3];
int graphIndex = this.active.astarData.GetGraphIndex(this);
for (int m = 0; m < this.nodes.Length; m++)
{
this.nodes[m] = new TriangleMeshNode(this.active);
TriangleMeshNode triangleMeshNode = this.nodes[m];
triangleMeshNode.GraphIndex = (uint)graphIndex;
triangleMeshNode.Penalty = this.initialPenalty;
triangleMeshNode.Walkable = true;
triangleMeshNode.v0 = triangles[m * 3];
triangleMeshNode.v1 = triangles[m * 3 + 1];
triangleMeshNode.v2 = triangles[m * 3 + 2];
if (!VectorMath.IsClockwiseXZ(vertices[triangleMeshNode.v0], vertices[triangleMeshNode.v1], vertices[triangleMeshNode.v2]))
{
int v = triangleMeshNode.v0;
triangleMeshNode.v0 = triangleMeshNode.v2;
triangleMeshNode.v2 = v;
}
if (VectorMath.IsColinearXZ(vertices[triangleMeshNode.v0], vertices[triangleMeshNode.v1], vertices[triangleMeshNode.v2]))
{
Debug.DrawLine((Vector3)vertices[triangleMeshNode.v0], (Vector3)vertices[triangleMeshNode.v1], Color.red);
Debug.DrawLine((Vector3)vertices[triangleMeshNode.v1], (Vector3)vertices[triangleMeshNode.v2], Color.red);
Debug.DrawLine((Vector3)vertices[triangleMeshNode.v2], (Vector3)vertices[triangleMeshNode.v0], Color.red);
}
triangleMeshNode.UpdatePositionFromVertices();
}
Dictionary <Int2, TriangleMeshNode> dictionary2 = new Dictionary <Int2, TriangleMeshNode>();
int n = 0;
int num2 = 0;
while (n < triangles.Length)
{
dictionary2[new Int2(triangles[n], triangles[n + 1])] = this.nodes[num2];
dictionary2[new Int2(triangles[n + 1], triangles[n + 2])] = this.nodes[num2];
dictionary2[new Int2(triangles[n + 2], triangles[n])] = this.nodes[num2];
num2++;
n += 3;
}
List <MeshNode> list = new List <MeshNode>();
List <uint> list2 = new List <uint>();
int num3 = 0;
int num4 = 0;
while (num3 < triangles.Length)
{
list.Clear();
list2.Clear();
TriangleMeshNode triangleMeshNode2 = this.nodes[num4];
for (int num5 = 0; num5 < 3; num5++)
{
TriangleMeshNode triangleMeshNode3;
if (dictionary2.TryGetValue(new Int2(triangles[num3 + (num5 + 1) % 3], triangles[num3 + num5]), out triangleMeshNode3))
{
list.Add(triangleMeshNode3);
list2.Add((uint)(triangleMeshNode2.position - triangleMeshNode3.position).costMagnitude);
}
}
triangleMeshNode2.connections = list.ToArray();
triangleMeshNode2.connectionCosts = list2.ToArray();
num4++;
num3 += 3;
}
NavMeshGraph.RebuildBBTree(this);
}
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);
int ymin = ((Int3)bounds.min).y;
int ymax = ((Int3)bounds.max).y;
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 (VectorMath.SegmentsIntersectXZ(a, b, vertex2, vertex3))
{
flag = true;
break;
}
if (VectorMath.SegmentsIntersectXZ(a, c, vertex2, vertex3))
{
flag = true;
break;
}
if (VectorMath.SegmentsIntersectXZ(c, d, vertex2, vertex3))
{
flag = true;
break;
}
if (VectorMath.SegmentsIntersectXZ(d, b, vertex2, vertex3))
{
flag = true;
break;
}
}
if (flag || triangleMeshNode.ContainsPoint(a) || triangleMeshNode.ContainsPoint(b) || triangleMeshNode.ContainsPoint(c) || triangleMeshNode.ContainsPoint(d))
{
flag = true;
}
if (!flag)
{
return(true);
}
int num5 = 0;
int num6 = 0;
for (int k = 0; k < 3; k++)
{
Int3 vertex4 = triangleMeshNode.GetVertex(k);
if (vertex4.y < ymin)
{
num6++;
}
if (vertex4.y > ymax)
{
num5++;
}
}
if (num6 == 3 || num5 == 3)
{
return(true);
}
o.WillUpdateNode(triangleMeshNode);
o.Apply(triangleMeshNode);
return(true);
});
}
public bool ContainsPoint(TriangleMeshNode node, Vector3 pos)
{
return(VectorMath.IsClockwiseXZ((Vector3)this.vertices[node.v0], (Vector3)this.vertices[node.v1], pos) && VectorMath.IsClockwiseXZ((Vector3)this.vertices[node.v1], (Vector3)this.vertices[node.v2], pos) && VectorMath.IsClockwiseXZ((Vector3)this.vertices[node.v2], (Vector3)this.vertices[node.v0], pos));
}
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 (VectorMath.RightXZ(vector, vector2, hit.origin) || !VectorMath.RightXZ(vector, vector2, tmp_end))
{
float num2;
float num3;
if (VectorMath.LineIntersectionFactorXZ(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 (VectorMath.RightXZ(vector3, vector4, hit.origin) || !VectorMath.RightXZ(vector3, vector4, tmp_end))
{
float num4;
float num5;
if (VectorMath.LineIntersectionFactorXZ(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);
}
Vector3 Snap(ABPath path, Exactness mode, bool start, out bool forceAddPoint, out int closestConnectionIndex)
{
var index = start ? 0 : path.path.Count - 1;
var node = path.path[index];
var nodePos = (Vector3)node.position;
closestConnectionIndex = 0;
forceAddPoint = false;
switch (mode)
{
case Exactness.ClosestOnNode:
return(start ? path.startPoint : path.endPoint);
case Exactness.SnapToNode:
return(nodePos);
case Exactness.Original:
case Exactness.Interpolate:
case Exactness.NodeConnection:
Vector3 relevantPoint;
if (start)
{
relevantPoint = adjustStartPoint != null?adjustStartPoint() : path.originalStartPoint;
}
else
{
relevantPoint = path.originalEndPoint;
}
switch (mode)
{
case Exactness.Original:
return(GetClampedPoint(nodePos, relevantPoint, node));
case Exactness.Interpolate:
// Adjacent node to either the start node or the end node in the path
var adjacentNode = path.path[Mathf.Clamp(index + (start ? 1 : -1), 0, path.path.Count - 1)];
return(VectorMath.ClosestPointOnSegment(nodePos, (Vector3)adjacentNode.position,
relevantPoint));
case Exactness.NodeConnection:
// This code uses some tricks to avoid allocations
// even though it uses delegates heavily
// The connectionBufferAddDelegate delegate simply adds whatever node
// it is called with to the connectionBuffer
connectionBuffer = connectionBuffer ?? new List <GraphNode>();
connectionBufferAddDelegate = connectionBufferAddDelegate ??
(System.Action <GraphNode>)connectionBuffer.Add;
// Adjacent node to either the start node or the end node in the path
adjacentNode = path.path[Mathf.Clamp(index + (start ? 1 : -1), 0, path.path.Count - 1)];
// Add all neighbours of #node to the connectionBuffer
node.GetConnections(connectionBufferAddDelegate);
var bestPos = nodePos;
var bestDist = float.PositiveInfinity;
// Loop through all neighbours
// Do it in reverse order because the length of the connectionBuffer
// will change during iteration
for (int i = connectionBuffer.Count - 1; i >= 0; i--)
{
var neighbour = connectionBuffer[i];
// Find the closest point on the connection between the nodes
// and check if the distance to that point is lower than the previous best
var closest = VectorMath.ClosestPointOnSegment(nodePos, (Vector3)neighbour.position,
relevantPoint);
var dist = (closest - relevantPoint).sqrMagnitude;
if (dist < bestDist)
{
bestPos = closest;
bestDist = dist;
closestConnectionIndex = i;
// If this node is not the adjacent node
// then the path should go through the start node as well
forceAddPoint = neighbour != adjacentNode;
}
}
connectionBuffer.Clear();
return(bestPos);
default:
throw new System.ArgumentException(
"Cannot reach this point, but the compiler is not smart enough to realize that.");
}
default:
throw new System.ArgumentException("Invalid mode");
}
}
public static Vector2 LineIntersectionPoint(Vector2 start1, Vector2 end1, Vector2 start2, Vector2 end2)
{
bool flag;
return(VectorMath.LineIntersectionPoint(start1, end1, start2, end2, out flag));
}
void TraverseFunnel(RichFunnel fn, float deltaTime, out Vector3 nextPosition, out Quaternion nextRotation)
{
// Clamp the current position to the navmesh
// and update the list of upcoming corners in the path
// and store that in the 'nextCorners' field
var position3D = UpdateTarget(fn);
float elevation;
Vector2 position = movementPlane.ToPlane(position3D, out elevation);
// Only find nearby walls every 5th frame to improve performance
if (Time.frameCount % 5 == 0 && wallForce > 0 && wallDist > 0)
{
wallBuffer.Clear();
fn.FindWalls(wallBuffer, wallDist);
}
// Target point
steeringTarget = nextCorners[0];
Vector2 targetPoint = movementPlane.ToPlane(steeringTarget);
// Direction to target
Vector2 dir = targetPoint - position;
// Normalized direction to the target
Vector2 normdir = VectorMath.Normalize(dir, out distanceToSteeringTarget);
// Calculate force from walls
Vector2 wallForceVector = CalculateWallForce(position, elevation, normdir);
Vector2 targetVelocity;
if (approachingPartEndpoint)
{
targetVelocity = slowdownTime > 0 ? Vector2.zero : normdir * maxSpeed;
// Reduce the wall avoidance force as we get closer to our target
wallForceVector *= System.Math.Min(distanceToSteeringTarget / 0.5f, 1);
if (distanceToSteeringTarget <= endReachedDistance)
{
// Reached the end of the path or an off mesh link
NextPart();
}
}
else
{
var nextNextCorner = nextCorners.Count > 1 ? movementPlane.ToPlane(nextCorners[1]) : position + 2 * dir;
targetVelocity = (nextNextCorner - targetPoint).normalized * maxSpeed;
}
var forwards = movementPlane.ToPlane(simulatedRotation * (rotationIn2D ? Vector3.up : Vector3.forward));
Vector2 accel = MovementUtilities.CalculateAccelerationToReachPoint(targetPoint - position, targetVelocity, velocity2D, acceleration, rotationSpeed, maxSpeed, forwards);
// Update the velocity using the acceleration
velocity2D += (accel + wallForceVector * wallForce) * deltaTime;
// Distance to the end of the path (almost as the crow flies)
//Good Game
//var distanceToEndOfPath = distanceToSteeringTarget + Vector3.Distance(steeringTarget, fn.exactEnd);
var distanceToEndOfPath = distanceToSteeringTarget + Vector3.Distance(steeringTarget, (Vector3)fn.exactEnd);
var slowdownFactor = distanceToEndOfPath < maxSpeed *slowdownTime?Mathf.Sqrt(distanceToEndOfPath / (maxSpeed *slowdownTime)) : 1;
FinalMovement(position3D, deltaTime, distanceToEndOfPath, slowdownFactor, out nextPosition, out nextRotation);
}
public List <Vector3> SmoothOffsetSimple(List <Vector3> path)
{
if (path.Count <= 2 || iterations <= 0)
{
return(path);
}
if (iterations > 12)
{
Debug.LogWarning("A very high iteration count was passed, won't let this one through");
return(path);
}
int maxLength = (path.Count - 2) * (int)Mathf.Pow(2, iterations) + 2;
List <Vector3> subdivided = ListPool <Vector3> .Claim(maxLength);
List <Vector3> subdivided2 = ListPool <Vector3> .Claim(maxLength);
for (int i = 0; i < maxLength; i++)
{
subdivided.Add(Vector3.zero); subdivided2.Add(Vector3.zero);
}
for (int i = 0; i < path.Count; i++)
{
subdivided[i] = path[i];
}
for (int iteration = 0; iteration < iterations; iteration++)
{
int currentPathLength = (path.Count - 2) * (int)Mathf.Pow(2, iteration) + 2;
//Switch the arrays
List <Vector3> tmp = subdivided;
subdivided = subdivided2;
subdivided2 = tmp;
const float nextMultiplier = 1F;
for (int i = 0; i < currentPathLength - 1; i++)
{
Vector3 current = subdivided2[i];
Vector3 next = subdivided2[i + 1];
Vector3 normal = Vector3.Cross(next - current, Vector3.up);
normal = normal.normalized;
bool firstRight = false;
bool secondRight = false;
bool setFirst = false;
bool setSecond = false;
if (i != 0 && !VectorMath.IsColinearXZ(current, next, subdivided2[i - 1]))
{
setFirst = true;
firstRight = VectorMath.RightOrColinearXZ(current, next, subdivided2[i - 1]);
}
if (i < currentPathLength - 1 && !VectorMath.IsColinearXZ(current, next, subdivided2[i + 2]))
{
setSecond = true;
secondRight = VectorMath.RightOrColinearXZ(current, next, subdivided2[i + 2]);
}
if (setFirst)
{
subdivided[i * 2] = current + (firstRight ? normal * offset * nextMultiplier : -normal * offset * nextMultiplier);
}
else
{
subdivided[i * 2] = current;
}
if (setSecond)
{
subdivided[i * 2 + 1] = next + (secondRight ? normal * offset * nextMultiplier : -normal * offset * nextMultiplier);
}
else
{
subdivided[i * 2 + 1] = next;
}
}
subdivided[(path.Count - 2) * (int)Mathf.Pow(2, iteration + 1) + 2 - 1] = subdivided2[currentPathLength - 1];
}
ListPool <Vector3> .Release(ref subdivided2);
return(subdivided);
}
public static void UpdateArea(GraphUpdateObject o, INavmeshHolder graph)
{
Bounds bounds = graph.transform.InverseTransform(o.bounds);
// Bounding rectangle with integer coordinates
var irect = new IntRect(
Mathf.FloorToInt(bounds.min.x * Int3.Precision),
Mathf.FloorToInt(bounds.min.z * Int3.Precision),
Mathf.CeilToInt(bounds.max.x * Int3.Precision),
Mathf.CeilToInt(bounds.max.z * Int3.Precision)
);
// Corners of the bounding rectangle
var a = new Int3(irect.xmin, 0, irect.ymin);
var b = new Int3(irect.xmin, 0, irect.ymax);
var c = new Int3(irect.xmax, 0, irect.ymin);
var d = new Int3(irect.xmax, 0, irect.ymax);
var ymin = ((Int3)bounds.min).y;
var ymax = ((Int3)bounds.max).y;
// Loop through all nodes and check if they intersect the bounding box
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.GetVertexInGraphSpace(v);
if (irect.Contains(p.x, p.z))
{
inside = true;
break;
}
if (p.x < irect.xmin)
{
allLeft++;
}
if (p.x > irect.xmax)
{
allRight++;
}
if (p.z < irect.ymin)
{
allTop++;
}
if (p.z > irect.ymax)
{
allBottom++;
}
}
if (!inside && (allLeft == 3 || allRight == 3 || allTop == 3 || allBottom == 3))
{
return;
}
// 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.GetVertexInGraphSpace(v);
Int3 vert2 = node.GetVertexInGraphSpace(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.ContainsPointInGraphSpace(a) || node.ContainsPointInGraphSpace(b) || node.ContainsPointInGraphSpace(c) || node.ContainsPointInGraphSpace(d))
{
inside = true;
}
if (!inside)
{
return;
}
int allAbove = 0;
int allBelow = 0;
// Check y coordinate
for (int v = 0; v < 3; v++)
{
Int3 p = node.GetVertexInGraphSpace(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;
}
// Triangle is inside the bounding box!
// Update it!
o.WillUpdateNode(node);
o.Apply(node);
});
}
private Vector3 Snap(ABPath path, Exactness mode, bool start, out bool forceAddPoint)
{
Vector3 originalEndPoint;
GraphNode node2;
int num = !start ? (path.path.Count - 1) : 0;
GraphNode hint = path.path[num];
Vector3 position = (Vector3)hint.position;
forceAddPoint = false;
switch (mode)
{
case Exactness.SnapToNode:
return(position);
case Exactness.Original:
case Exactness.Interpolate:
case Exactness.NodeConnection:
if (!start)
{
originalEndPoint = path.originalEndPoint;
break;
}
originalEndPoint = (this.adjustStartPoint == null) ? path.originalStartPoint : this.adjustStartPoint();
break;
case Exactness.ClosestOnNode:
return(this.GetClampedPoint(position, !start ? path.endPoint : path.startPoint, hint));
default:
throw new ArgumentException("Invalid mode");
}
switch (mode)
{
case Exactness.Original:
return(this.GetClampedPoint(position, originalEndPoint, hint));
case Exactness.Interpolate:
{
Vector3 point = this.GetClampedPoint(position, originalEndPoint, hint);
node2 = path.path[Mathf.Clamp(num + (!start ? -1 : 1), 0, path.path.Count - 1)];
return(VectorMath.ClosestPointOnSegment(position, (Vector3)node2.position, point));
}
case Exactness.NodeConnection:
{
if (this.connectionBuffer == null)
{
}
this.connectionBuffer = new List <GraphNode>();
if (this.connectionBufferAddDelegate == null)
{
}
this.connectionBufferAddDelegate = new GraphNodeDelegate(this.connectionBuffer.Add);
node2 = path.path[Mathf.Clamp(num + (!start ? -1 : 1), 0, path.path.Count - 1)];
hint.GetConnections(this.connectionBufferAddDelegate);
Vector3 vector4 = position;
float positiveInfinity = float.PositiveInfinity;
for (int i = this.connectionBuffer.Count - 1; i >= 0; i--)
{
GraphNode node3 = this.connectionBuffer[i];
Vector3 vector5 = VectorMath.ClosestPointOnSegment(position, (Vector3)node3.position, originalEndPoint);
Vector3 vector6 = vector5 - originalEndPoint;
float sqrMagnitude = vector6.sqrMagnitude;
if (sqrMagnitude < positiveInfinity)
{
vector4 = vector5;
positiveInfinity = sqrMagnitude;
forceAddPoint = node3 != node2;
}
}
this.connectionBuffer.Clear();
return(vector4);
}
}
throw new ArgumentException("Cannot reach this point, but the compiler is not smart enough to realize that.");
}
/** Updates an area in the list graph.
* Recalculates possibly affected connections, i.e all connectionlines passing trough the bounds of the \a guo will be recalculated
* \astarpro */
public void UpdateArea(GraphUpdateObject guo)
{
if (nodes == null)
{
return;
}
for (int i = 0; i < nodeCount; i++)
{
if (guo.bounds.Contains((Vector3)nodes[i].position))
{
guo.WillUpdateNode(nodes[i]);
guo.Apply(nodes[i]);
}
}
if (guo.updatePhysics)
{
//Use a copy of the bounding box, we should not change the GUO's bounding box since it might be used for other graph updates
Bounds bounds = guo.bounds;
if (thickRaycast)
{
//Expand the bounding box to account for the thick raycast
bounds.Expand(thickRaycastRadius * 2);
}
//Create two temporary arrays used for holding new connections and costs
List <Connection> tmp_arr = Pathfinding.Util.ListPool <Connection> .Claim();
for (int i = 0; i < nodeCount; i++)
{
PointNode node = nodes[i];
var nodePos = (Vector3)node.position;
List <Connection> conn = null;
for (int j = 0; j < nodeCount; j++)
{
if (j == i)
{
continue;
}
var otherNodePos = (Vector3)nodes[j].position;
// Check if this connection intersects the bounding box.
// If it does we need to recalculate that connection.
if (VectorMath.SegmentIntersectsBounds(bounds, nodePos, otherNodePos))
{
float dist;
PointNode other = nodes[j];
bool contains = node.ContainsConnection(other);
bool validConnection = IsValidConnection(node, other, out dist);
if (!contains && validConnection)
{
// A new connection should be added
if (conn == null)
{
tmp_arr.Clear();
conn = tmp_arr;
conn.AddRange(node.connections);
}
uint cost = (uint)Mathf.RoundToInt(dist * Int3.FloatPrecision);
conn.Add(new Connection {
node = other, cost = cost
});
}
else if (contains && !validConnection)
{
// A connection should be removed
if (conn == null)
{
tmp_arr.Clear();
conn = tmp_arr;
conn.AddRange(node.connections);
}
for (int q = 0; q < conn.Count; q++)
{
if (conn[q].node == other)
{
conn.RemoveAt(q);
break;
}
}
}
}
}
// Save the new connections if any were changed
if (conn != null)
{
node.connections = conn.ToArray();
}
}
// Release buffers back to the pool
Pathfinding.Util.ListPool <Connection> .Release(tmp_arr);
}
}
public static bool RunFunnel(List <Vector3> left, List <Vector3> right, List <Vector3> funnelPath)
{
if (left == null)
{
throw new ArgumentNullException("left");
}
if (right == null)
{
throw new ArgumentNullException("right");
}
if (funnelPath == null)
{
throw new ArgumentNullException("funnelPath");
}
if (left.Count != right.Count)
{
throw new ArgumentException("left and right lists must have equal length");
}
while (left[1] == left[2] && right[1] == right[2])
{
left.RemoveAt(1);
right.RemoveAt(1);
if (left.Count <= 3)
{
return(false);
}
}
Vector3 vector = left[2];
if (vector == left[1])
{
vector = right[2];
}
while (VectorMath.IsColinearXZ(left[0], left[1], right[1]) || VectorMath.RightOrColinearXZ(left[1], right[1], vector) == VectorMath.RightOrColinearXZ(left[1], right[1], left[0]))
{
left.RemoveAt(1);
right.RemoveAt(1);
if (left.Count <= 3)
{
return(false);
}
vector = left[2];
if (vector == left[1])
{
vector = right[2];
}
}
if (!VectorMath.IsClockwiseXZ(left[0], left[1], right[1]) && !VectorMath.IsColinearXZ(left[0], left[1], right[1]))
{
List <Vector3> list = left;
left = right;
right = list;
}
funnelPath.Add(left[0]);
Vector3 vector2 = left[0];
Vector3 vector3 = left[1];
Vector3 vector4 = right[1];
int num = 1;
int num2 = 1;
int i = 2;
while (i < left.Count)
{
if (funnelPath.Count > 2000)
{
Debug.LogWarning("Avoiding infinite loop. Remove this check if you have this long paths.");
break;
}
Vector3 vector5 = left[i];
Vector3 vector6 = right[i];
if (VectorMath.SignedTriangleAreaTimes2XZ(vector2, vector4, vector6) < 0f)
{
goto IL_26B;
}
if (vector2 == vector4 || VectorMath.SignedTriangleAreaTimes2XZ(vector2, vector3, vector6) <= 0f)
{
vector4 = vector6;
num = i;
goto IL_26B;
}
funnelPath.Add(vector3);
vector2 = vector3;
int num3 = num2;
vector3 = vector2;
vector4 = vector2;
num2 = num3;
num = num3;
i = num3;
IL_2CF:
i++;
continue;
IL_26B:
if (VectorMath.SignedTriangleAreaTimes2XZ(vector2, vector3, vector5) > 0f)
{
goto IL_2CF;
}
if (vector2 == vector3 || VectorMath.SignedTriangleAreaTimes2XZ(vector2, vector4, vector5) >= 0f)
{
vector3 = vector5;
num2 = i;
goto IL_2CF;
}
funnelPath.Add(vector4);
vector2 = vector4;
num3 = num;
vector3 = vector2;
vector4 = vector2;
num2 = num3;
num = num3;
i = num3;
goto IL_2CF;
}
funnelPath.Add(left[left.Count - 1]);
return(true);
}
// Token: 0x06002619 RID: 9753 RVA: 0x001A76DC File Offset: 0x001A58DC
void IUpdatableGraph.UpdateArea(GraphUpdateObject guo)
{
if (this.nodes == null)
{
return;
}
for (int i = 0; i < this.nodeCount; i++)
{
PointNode pointNode = this.nodes[i];
if (guo.bounds.Contains((Vector3)pointNode.position))
{
guo.WillUpdateNode(pointNode);
guo.Apply(pointNode);
}
}
if (guo.updatePhysics)
{
Bounds bounds = guo.bounds;
if (this.thickRaycast)
{
bounds.Expand(this.thickRaycastRadius * 2f);
}
List <Connection> list = ListPool <Connection> .Claim();
for (int j = 0; j < this.nodeCount; j++)
{
PointNode pointNode2 = this.nodes[j];
Vector3 a = (Vector3)pointNode2.position;
List <Connection> list2 = null;
for (int k = 0; k < this.nodeCount; k++)
{
if (k != j)
{
Vector3 b = (Vector3)this.nodes[k].position;
if (VectorMath.SegmentIntersectsBounds(bounds, a, b))
{
PointNode pointNode3 = this.nodes[k];
bool flag = pointNode2.ContainsConnection(pointNode3);
float num;
bool flag2 = this.IsValidConnection(pointNode2, pointNode3, out num);
if (list2 == null && flag != flag2)
{
list.Clear();
list2 = list;
list2.AddRange(pointNode2.connections);
}
if (!flag && flag2)
{
uint cost = (uint)Mathf.RoundToInt(num * 1000f);
list2.Add(new Connection(pointNode3, cost, byte.MaxValue));
}
else if (flag && !flag2)
{
for (int l = 0; l < list2.Count; l++)
{
if (list2[l].node == pointNode3)
{
list2.RemoveAt(l);
break;
}
}
}
}
}
}
if (list2 != null)
{
pointNode2.connections = list2.ToArray();
}
}
ListPool <Connection> .Release(ref list);
}
}
/// <summary>TODO: This is the area in XZ space, use full 3D space for higher correctness maybe?</summary>
public override float SurfaceArea()
{
var holder = GetNavmeshHolder(GraphIndex);
return(System.Math.Abs(VectorMath.SignedTriangleAreaTimes2XZ(holder.GetVertex(v0), holder.GetVertex(v1), holder.GetVertex(v2))) * 0.5f);
}
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 void Apply(bool forceNewCheck)
{
//TODO
//This function assumes that connections from the n1,n2 nodes never need to be removed in the future (e.g because the nodes move or something)
NNConstraint nn = NNConstraint.None;
nn.distanceXZ = true;
int graph = (int)startNode.GraphIndex;
//Search all graphs but the one which start and end nodes are on
nn.graphMask = ~(1 << graph);
bool same = true;
if (true)
{
NNInfo n1 = AstarPath.active.GetNearest(StartTransform.position, nn);
same &= n1.node == connectedNode1 && n1.node != null;
connectedNode1 = n1.node as MeshNode;
clamped1 = n1.clampedPosition;
if (connectedNode1 != null)
{
Debug.DrawRay((Vector3)connectedNode1.position, Vector3.up * 5, Color.red);
}
}
if (true)
{
NNInfo n2 = AstarPath.active.GetNearest(EndTransform.position, nn);
same &= n2.node == connectedNode2 && n2.node != null;
connectedNode2 = n2.node as MeshNode;
clamped2 = n2.clampedPosition;
if (connectedNode2 != null)
{
Debug.DrawRay((Vector3)connectedNode2.position, Vector3.up * 5, Color.cyan);
}
}
if (connectedNode2 == null || connectedNode1 == null)
{
return;
}
startNode.SetPosition((Int3)StartTransform.position);
endNode.SetPosition((Int3)EndTransform.position);
if (same && !forceNewCheck)
{
return;
}
RemoveConnections(startNode);
RemoveConnections(endNode);
uint cost = (uint)Mathf.RoundToInt(((Int3)(StartTransform.position - EndTransform.position)).costMagnitude * costFactor);
startNode.AddConnection(endNode, cost);
endNode.AddConnection(startNode, cost);
Int3 dir = connectedNode2.position - connectedNode1.position;
for (int a = 0; a < connectedNode1.GetVertexCount(); a++)
{
Int3 va1 = connectedNode1.GetVertex(a);
Int3 va2 = connectedNode1.GetVertex((a + 1) % connectedNode1.GetVertexCount());
if (Int3.DotLong((va2 - va1).Normal2D(), dir) > 0)
{
continue;
}
for (int b = 0; b < connectedNode2.GetVertexCount(); b++)
{
Int3 vb1 = connectedNode2.GetVertex(b);
Int3 vb2 = connectedNode2.GetVertex((b + 1) % connectedNode2.GetVertexCount());
if (Int3.DotLong((vb2 - vb1).Normal2D(), dir) < 0)
{
continue;
}
//Debug.DrawLine ((Vector3)va1, (Vector3)va2, Color.magenta);
//Debug.DrawLine ((Vector3)vb1, (Vector3)vb2, Color.cyan);
//Debug.Break ();
if (Int3.Angle((vb2 - vb1), (va2 - va1)) > (170.0 / 360.0f) * Mathf.PI * 2)
{
float t1 = 0;
float t2 = 1;
t2 = System.Math.Min(t2, VectorMath.ClosestPointOnLineFactor(va1, va2, vb1));
t1 = System.Math.Max(t1, VectorMath.ClosestPointOnLineFactor(va1, va2, vb2));
if (t2 < t1)
{
Debug.LogError("Wait wut!? " + t1 + " " + t2 + " " + va1 + " " + va2 + " " + vb1 + " " + vb2 + "\nTODO, fix this error");
}
else
{
Vector3 pa = (Vector3)(va2 - va1) * t1 + (Vector3)va1;
Vector3 pb = (Vector3)(va2 - va1) * t2 + (Vector3)va1;
startNode.portalA = pa;
startNode.portalB = pb;
endNode.portalA = pb;
endNode.portalB = pa;
//Add connections between nodes, or replace old connections if existing
connectedNode1.AddConnection(startNode, (uint)Mathf.RoundToInt(((Int3)(clamped1 - StartTransform.position)).costMagnitude * costFactor));
connectedNode2.AddConnection(endNode, (uint)Mathf.RoundToInt(((Int3)(clamped2 - EndTransform.position)).costMagnitude * costFactor));
startNode.AddConnection(connectedNode1, (uint)Mathf.RoundToInt(((Int3)(clamped1 - StartTransform.position)).costMagnitude * costFactor));
endNode.AddConnection(connectedNode2, (uint)Mathf.RoundToInt(((Int3)(clamped2 - EndTransform.position)).costMagnitude * costFactor));
return;
}
}
}
}
}
/// <summary>
/// Searches for the node the agent is inside.
/// This will also clamp the position to the navmesh
/// and repair the funnel cooridor if the agent moves slightly outside it.
///
/// Returns: True if nodes along the path have been destroyed so that a path recalculation is required
/// </summary>
bool ClampToNavmeshInternal(ref Vector3 position)
{
var previousNode = nodes[currentNode];
if (previousNode.Destroyed)
{
return(true);
}
// Check if we are in the same node as we were in during the last frame and otherwise do a more extensive search
if (previousNode.ContainsPoint(position))
{
return(false);
}
// This part of the code is relatively seldom called
// Most of the time we are still on the same node as during the previous frame
var que = navmeshClampQueue;
var allVisited = navmeshClampList;
var parent = navmeshClampDict;
previousNode.TemporaryFlag1 = true;
parent[previousNode] = null;
que.Enqueue(previousNode);
allVisited.Add(previousNode);
float bestDistance = float.PositiveInfinity;
Vector3 bestPoint = position;
TriangleMeshNode bestNode = null;
while (que.Count > 0)
{
var node = que.Dequeue();
// Snap to the closest point in XZ space (keep the Y coordinate)
// If we would have snapped to the closest point in 3D space, the agent
// might slow down when traversing slopes
var closest = node.ClosestPointOnNodeXZ(position);
var dist = VectorMath.MagnitudeXZ(closest - position);
// Check if this node is any closer than the previous best node.
// Allow for a small margin to both avoid floating point errors and to allow
// moving past very small local minima.
if (dist <= bestDistance * 1.05f + 0.001f)
{
if (dist < bestDistance)
{
bestDistance = dist;
bestPoint = closest;
bestNode = node;
}
for (int i = 0; i < node.connections.Length; i++)
{
var neighbour = node.connections[i].node as TriangleMeshNode;
if (neighbour != null && !neighbour.TemporaryFlag1)
{
neighbour.TemporaryFlag1 = true;
parent[neighbour] = node;
que.Enqueue(neighbour);
allVisited.Add(neighbour);
}
}
}
}
if (bestNode == null)
{
Debug.LogError(previousNode.position + " " + previousNode.ClosestPointOnNode(position) + " " + position + " " + bestDistance);
}
UnityEngine.Assertions.Assert.IsNotNull(bestNode);
for (int i = 0; i < allVisited.Count; i++)
{
allVisited[i].TemporaryFlag1 = false;
}
allVisited.ClearFast();
var closestNodeInPath = nodes.IndexOf(bestNode);
// Move the x and z coordinates of the chararacter but not the y coordinate
// because the navmesh surface may not line up with the ground
position.x = bestPoint.x;
position.z = bestPoint.z;
// Check if the closest node
// was on the path already or if we need to adjust it
if (closestNodeInPath == -1)
{
// Reuse this list, because why not.
var prefix = navmeshClampList;
while (closestNodeInPath == -1)
{
prefix.Add(bestNode);
bestNode = parent[bestNode];
closestNodeInPath = nodes.IndexOf(bestNode);
}
// We have found a node containing the position, but it is outside the funnel
// Recalculate the funnel to include this node
exactStart = position;
UpdateFunnelCorridor(closestNodeInPath, prefix);
prefix.ClearFast();
// Restart from the first node in the updated path
currentNode = 0;
}
else
{
currentNode = closestNodeInPath;
}
parent.Clear();
// Do a quick check to see if the next node in the path has been destroyed
// If that is the case then we should plan a new path immediately
return(currentNode + 1 < nodes.Count && nodes[currentNode + 1].Destroyed);
}
public static bool RunFunnel(List <Vector3> left, List <Vector3> right, List <Vector3> funnelPath)
{
if (left == null)
{
throw new ArgumentNullException("left");
}
if (right == null)
{
throw new ArgumentNullException("right");
}
if (funnelPath == null)
{
throw new ArgumentNullException("funnelPath");
}
if (left.Count != right.Count)
{
throw new ArgumentException("left and right lists must have equal length");
}
if (left.Count <= 3)
{
return(false);
}
while ((left[1] == left[2]) && (right[1] == right[2]))
{
left.RemoveAt(1);
right.RemoveAt(1);
if (left.Count <= 3)
{
return(false);
}
}
Vector3 p = left[2];
if (p == left[1])
{
p = right[2];
}
while (VectorMath.IsColinearXZ(left[0], left[1], right[1]) || (VectorMath.RightOrColinearXZ(left[1], right[1], p) == VectorMath.RightOrColinearXZ(left[1], right[1], left[0])))
{
left.RemoveAt(1);
right.RemoveAt(1);
if (left.Count <= 3)
{
return(false);
}
p = left[2];
if (p == left[1])
{
p = right[2];
}
}
if (!VectorMath.IsClockwiseXZ(left[0], left[1], right[1]) && !VectorMath.IsColinearXZ(left[0], left[1], right[1]))
{
List <Vector3> list = left;
left = right;
right = list;
}
funnelPath.Add(left[0]);
Vector3 a = left[0];
Vector3 b = left[1];
Vector3 vector4 = right[1];
int num = 0;
int num2 = 1;
int num3 = 1;
for (int i = 2; i < left.Count; i++)
{
if (funnelPath.Count > 0x7d0)
{
Debug.LogWarning("Avoiding infinite loop. Remove this check if you have this long paths.");
break;
}
Vector3 c = left[i];
Vector3 vector6 = right[i];
if (VectorMath.SignedTriangleAreaTimes2XZ(a, vector4, vector6) >= 0f)
{
if ((a == vector4) || (VectorMath.SignedTriangleAreaTimes2XZ(a, b, vector6) <= 0f))
{
vector4 = vector6;
num2 = i;
}
else
{
funnelPath.Add(b);
a = b;
num = num3;
b = a;
vector4 = a;
num3 = num;
num2 = num;
i = num;
continue;
}
}
if (VectorMath.SignedTriangleAreaTimes2XZ(a, b, c) <= 0f)
{
if ((a == b) || (VectorMath.SignedTriangleAreaTimes2XZ(a, vector4, c) >= 0f))
{
b = c;
num3 = i;
}
else
{
funnelPath.Add(vector4);
a = vector4;
num = num2;
b = a;
vector4 = a;
num3 = num;
num2 = num;
i = num;
}
}
}
funnelPath.Add(left[left.Count - 1]);
return(true);
}
/** Updates an area in the list graph.
* Recalculates possibly affected connections, i.e all connectionlines passing trough the bounds of the \a guo will be recalculated
* \astarpro */
public void UpdateArea(GraphUpdateObject guo)
{
if (nodes == null)
{
return;
}
for (int i = 0; i < nodeCount; i++)
{
if (guo.bounds.Contains((Vector3)nodes[i].position))
{
guo.WillUpdateNode(nodes[i]);
guo.Apply(nodes[i]);
}
}
if (guo.updatePhysics)
{
//Use a copy of the bounding box, we should not change the GUO's bounding box since it might be used for other graph updates
Bounds bounds = guo.bounds;
if (thickRaycast)
{
//Expand the bounding box to account for the thick raycast
bounds.Expand(thickRaycastRadius * 2);
}
//Create two temporary arrays used for holding new connections and costs
List <GraphNode> tmp_arr = Pathfinding.Util.ListPool <GraphNode> .Claim();
List <uint> tmp_arr2 = Pathfinding.Util.ListPool <uint> .Claim();
for (int i = 0; i < nodeCount; i++)
{
PointNode node = nodes[i];
var a = (Vector3)node.position;
List <GraphNode> conn = null;
List <uint> costs = null;
for (int j = 0; j < nodeCount; j++)
{
if (j == i)
{
continue;
}
var b = (Vector3)nodes[j].position;
if (VectorMath.SegmentIntersectsBounds(bounds, a, b))
{
float dist;
PointNode other = nodes[j];
bool contains = node.ContainsConnection(other);
bool validConnection = IsValidConnection(node, other, out dist);
if (!contains && validConnection)
{
// A new connection should be added
if (conn == null)
{
tmp_arr.Clear();
tmp_arr2.Clear();
conn = tmp_arr;
costs = tmp_arr2;
conn.AddRange(node.connections);
costs.AddRange(node.connectionCosts);
}
uint cost = (uint)Mathf.RoundToInt(dist * Int3.FloatPrecision);
conn.Add(other);
costs.Add(cost);
}
else if (contains && !validConnection)
{
// A connection should be removed
if (conn == null)
{
tmp_arr.Clear();
tmp_arr2.Clear();
conn = tmp_arr;
costs = tmp_arr2;
conn.AddRange(node.connections);
costs.AddRange(node.connectionCosts);
}
int p = conn.IndexOf(other);
//Shouldn't have to check for it, but who knows what might go wrong
if (p != -1)
{
conn.RemoveAt(p);
costs.RemoveAt(p);
}
}
}
}
// Save the new connections if any were changed
if (conn != null)
{
node.connections = conn.ToArray();
node.connectionCosts = costs.ToArray();
}
}
// Release buffers back to the pool
Pathfinding.Util.ListPool <GraphNode> .Release(tmp_arr);
Pathfinding.Util.ListPool <uint> .Release(tmp_arr2);
}
}
public bool FindNextCorners(Vector3 origin, int startIndex, List <Vector3> funnelPath, int numCorners, out bool lastCorner)
{
lastCorner = false;
if (this.left == null)
{
throw new Exception("left list is null");
}
if (this.right == null)
{
throw new Exception("right list is null");
}
if (funnelPath == null)
{
throw new ArgumentNullException("funnelPath");
}
if (this.left.Count != this.right.Count)
{
throw new ArgumentException("left and right lists must have equal length");
}
int count = this.left.Count;
if (count == 0)
{
throw new ArgumentException("no diagonals");
}
if ((count - startIndex) < 3)
{
funnelPath.Add(this.left[count - 1]);
lastCorner = true;
return(true);
}
while ((this.left[startIndex + 1] == this.left[startIndex + 2]) && (this.right[startIndex + 1] == this.right[startIndex + 2]))
{
startIndex++;
if ((count - startIndex) <= 3)
{
return(false);
}
}
Vector3 p = this.left[startIndex + 2];
if (p == this.left[startIndex + 1])
{
p = this.right[startIndex + 2];
}
while (VectorMath.IsColinearXZ(origin, this.left[startIndex + 1], this.right[startIndex + 1]) || (VectorMath.RightOrColinearXZ(this.left[startIndex + 1], this.right[startIndex + 1], p) == VectorMath.RightOrColinearXZ(this.left[startIndex + 1], this.right[startIndex + 1], origin)))
{
startIndex++;
if ((count - startIndex) < 3)
{
funnelPath.Add(this.left[count - 1]);
lastCorner = true;
return(true);
}
p = this.left[startIndex + 2];
if (p == this.left[startIndex + 1])
{
p = this.right[startIndex + 2];
}
}
Vector3 a = origin;
Vector3 b = this.left[startIndex + 1];
Vector3 vector4 = this.right[startIndex + 1];
int num2 = startIndex;
int num3 = startIndex + 1;
int num4 = startIndex + 1;
for (int i = startIndex + 2; i < count; i++)
{
if (funnelPath.Count >= numCorners)
{
return(true);
}
if (funnelPath.Count > 0x7d0)
{
Debug.LogWarning("Avoiding infinite loop. Remove this check if you have this long paths.");
break;
}
Vector3 c = this.left[i];
Vector3 vector6 = this.right[i];
if (VectorMath.SignedTriangleAreaTimes2XZ(a, vector4, vector6) >= 0f)
{
if ((a == vector4) || (VectorMath.SignedTriangleAreaTimes2XZ(a, b, vector6) <= 0f))
{
vector4 = vector6;
num3 = i;
}
else
{
funnelPath.Add(b);
a = b;
num2 = num4;
b = a;
vector4 = a;
num4 = num2;
num3 = num2;
i = num2;
continue;
}
}
if (VectorMath.SignedTriangleAreaTimes2XZ(a, b, c) <= 0f)
{
if ((a == b) || (VectorMath.SignedTriangleAreaTimes2XZ(a, vector4, c) >= 0f))
{
b = c;
num4 = i;
}
else
{
funnelPath.Add(vector4);
a = vector4;
num2 = num3;
b = a;
vector4 = a;
num4 = num2;
num3 = num2;
i = num2;
}
}
}
lastCorner = true;
funnelPath.Add(this.left[count - 1]);
return(true);
}
void CalculateMeshContour()
{
if (mesh == null)
{
return;
}
edges.Clear();
pointers.Clear();
Vector3[] verts = mesh.vertices;
int[] tris = mesh.triangles;
for (int i = 0; i < tris.Length; i += 3)
{
// Make sure it is clockwise
if (VectorMath.IsClockwiseXZ(verts[tris[i + 0]], verts[tris[i + 1]], verts[tris[i + 2]]))
{
int tmp = tris[i + 0];
tris[i + 0] = tris[i + 2];
tris[i + 2] = tmp;
}
edges[new Int2(tris[i + 0], tris[i + 1])] = i;
edges[new Int2(tris[i + 1], tris[i + 2])] = i;
edges[new Int2(tris[i + 2], tris[i + 0])] = i;
}
// Construct a list of pointers along all edges
for (int i = 0; i < tris.Length; i += 3)
{
for (int j = 0; j < 3; j++)
{
if (!edges.ContainsKey(new Int2(tris[i + ((j + 1) % 3)], tris[i + ((j + 0) % 3)])))
{
pointers[tris[i + ((j + 0) % 3)]] = tris[i + ((j + 1) % 3)];
}
}
}
var contourBuffer = new List <Vector3[]>();
List <Vector3> buffer = Pathfinding.Util.ListPool <Vector3> .Claim();
// Follow edge pointers to generate the contours
for (int i = 0; i < verts.Length; i++)
{
if (pointers.ContainsKey(i))
{
buffer.Clear();
int s = i;
do
{
int tmp = pointers[s];
//This path has been taken before
if (tmp == -1)
{
break;
}
pointers[s] = -1;
buffer.Add(verts[s]);
s = tmp;
if (s == -1)
{
Debug.LogError("Invalid Mesh '" + mesh.name + " in " + gameObject.name);
break;
}
} while (s != i);
if (buffer.Count > 0)
{
contourBuffer.Add(buffer.ToArray());
}
}
}
// Return lists to the pool
Pathfinding.Util.ListPool <Vector3> .Release(ref buffer);
contours = contourBuffer.ToArray();
}
public static bool IsClockwiseOrColinear(Int2 a, Int2 b, Int2 c)
{
return(VectorMath.RightOrColinear(a, b, c));
}