+ astar project

1 files changed, 673 insertions, 0 deletions

diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Nodes/TriangleMeshNode.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Nodes/TriangleMeshNode.cs
new file mode 100644
index 0000000..e7142a6
--- /dev/null
+++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Nodes/TriangleMeshNode.cs
@@ -0,0 +1,673 @@
+#pragma warning disable 0162
+using UnityEngine;
+using Pathfinding.Serialization;
+using UnityEngine.Assertions;
+using Unity.Mathematics;
+using Pathfinding.Util;
+using Unity.Burst;
+
+namespace Pathfinding {
+	/// <summary>Interface for something that holds a triangle based navmesh</summary>
+	public interface INavmeshHolder : ITransformedGraph, INavmesh {
+		/// <summary>Position of vertex number i in the world</summary>
+		Int3 GetVertex(int i);
+
+		/// <summary>
+		/// Position of vertex number i in coordinates local to the graph.
+		/// The up direction is always the +Y axis for these coordinates.
+		/// </summary>
+		Int3 GetVertexInGraphSpace(int i);
+
+		int GetVertexArrayIndex(int index);
+
+		/// <summary>Transforms coordinates from graph space to world space</summary>
+		void GetTileCoordinates(int tileIndex, out int x, out int z);
+	}
+
+	/// <summary>Node represented by a triangle</summary>
+	[Unity.Burst.BurstCompile]
+	// Sealing the class provides a nice performance boost (~5-10%) during pathfinding, because the JIT can inline more things and use non-virtual calls.
+	public sealed class TriangleMeshNode : MeshNode {
+		public TriangleMeshNode () {
+			HierarchicalNodeIndex = 0;
+			NodeIndex = DestroyedNodeIndex;
+		}
+
+		public TriangleMeshNode (AstarPath astar) {
+			astar.InitializeNode(this);
+		}
+
+		/// <summary>
+		/// Legacy compatibility.
+		/// Enabling this will make pathfinding use node centers, which leads to less accurate paths (but it's faster).
+		/// </summary>
+		public const bool InaccuratePathSearch = false;
+		internal override int PathNodeVariants => InaccuratePathSearch ? 1 : 3;
+
+		/// <summary>Internal vertex index for the first vertex</summary>
+		public int v0;
+
+		/// <summary>Internal vertex index for the second vertex</summary>
+		public int v1;
+
+		/// <summary>Internal vertex index for the third vertex</summary>
+		public int v2;
+
+		/// <summary>Holds INavmeshHolder references for all graph indices to be able to access them in a performant manner</summary>
+		static INavmeshHolder[] _navmeshHolders = new INavmeshHolder[0];
+
+		/// <summary>Used for synchronised access to the <see cref="_navmeshHolders"/> array</summary>
+		static readonly System.Object lockObject = new System.Object();
+
+		[System.Runtime.CompilerServices.MethodImpl(System.Runtime.CompilerServices.MethodImplOptions.AggressiveInlining)]
+		public static INavmeshHolder GetNavmeshHolder (uint graphIndex) {
+			return _navmeshHolders[(int)graphIndex];
+		}
+
+		/// <summary>
+		/// Tile index in the recast or navmesh graph that this node is part of.
+		/// See: <see cref="NavmeshBase.GetTiles"/>
+		/// </summary>
+		public int TileIndex => (v0 >> NavmeshBase.TileIndexOffset) & NavmeshBase.TileIndexMask;
+
+		/// <summary>
+		/// Sets the internal navmesh holder for a given graph index.
+		/// Warning: Internal method
+		/// </summary>
+		public static void SetNavmeshHolder (int graphIndex, INavmeshHolder graph) {
+			// We need to lock to make sure that
+			// the resize operation is thread safe
+			lock (lockObject) {
+				if (graphIndex >= _navmeshHolders.Length) {
+					var gg = new INavmeshHolder[graphIndex+1];
+					_navmeshHolders.CopyTo(gg, 0);
+					_navmeshHolders = gg;
+				}
+				_navmeshHolders[graphIndex] = graph;
+			}
+		}
+
+		public static void ClearNavmeshHolder (int graphIndex, INavmeshHolder graph) {
+			lock (lockObject) {
+				if (graphIndex < _navmeshHolders.Length && _navmeshHolders[graphIndex] == graph) {
+					_navmeshHolders[graphIndex] = null;
+				}
+			}
+		}
+
+		/// <summary>Set the position of this node to the average of its 3 vertices</summary>
+		public void UpdatePositionFromVertices () {
+			Int3 a, b, c;
+
+			GetVertices(out a, out b, out c);
+			position = (a + b + c) * 0.333333f;
+		}
+
+		/// <summary>
+		/// Return a number identifying a vertex.
+		/// This number does not necessarily need to be a index in an array but two different vertices (in the same graph) should
+		/// not have the same vertex numbers.
+		/// </summary>
+		[System.Runtime.CompilerServices.MethodImpl(System.Runtime.CompilerServices.MethodImplOptions.AggressiveInlining)]
+		public int GetVertexIndex (int i) {
+			return i == 0 ? v0 : (i == 1 ? v1 : v2);
+		}
+
+		/// <summary>
+		/// Return a number specifying an index in the source vertex array.
+		/// The vertex array can for example be contained in a recast tile, or be a navmesh graph, that is graph dependant.
+		/// This is slower than GetVertexIndex, if you only need to compare vertices, use GetVertexIndex.
+		/// </summary>
+		public int GetVertexArrayIndex (int i) {
+			return GetNavmeshHolder(GraphIndex).GetVertexArrayIndex(i == 0 ? v0 : (i == 1 ? v1 : v2));
+		}
+
+		/// <summary>Returns all 3 vertices of this node in world space</summary>
+		public void GetVertices (out Int3 v0, out Int3 v1, out Int3 v2) {
+			// Get the object holding the vertex data for this node
+			// This is usually a graph or a recast graph tile
+			var holder = GetNavmeshHolder(GraphIndex);
+
+			v0 = holder.GetVertex(this.v0);
+			v1 = holder.GetVertex(this.v1);
+			v2 = holder.GetVertex(this.v2);
+		}
+
+		/// <summary>Returns all 3 vertices of this node in graph space</summary>
+		public void GetVerticesInGraphSpace (out Int3 v0, out Int3 v1, out Int3 v2) {
+			// Get the object holding the vertex data for this node
+			// This is usually a graph or a recast graph tile
+			var holder = GetNavmeshHolder(GraphIndex);
+
+			v0 = holder.GetVertexInGraphSpace(this.v0);
+			v1 = holder.GetVertexInGraphSpace(this.v1);
+			v2 = holder.GetVertexInGraphSpace(this.v2);
+		}
+
+		[System.Runtime.CompilerServices.MethodImpl(System.Runtime.CompilerServices.MethodImplOptions.AggressiveInlining)]
+		public override Int3 GetVertex (int i) {
+			return GetNavmeshHolder(GraphIndex).GetVertex(GetVertexIndex(i));
+		}
+
+		public Int3 GetVertexInGraphSpace (int i) {
+			return GetNavmeshHolder(GraphIndex).GetVertexInGraphSpace(GetVertexIndex(i));
+		}
+
+		public override int GetVertexCount () {
+			// A triangle has 3 vertices
+			return 3;
+		}
+
+		/// <summary>
+		/// Projects the given point onto the plane of this node's surface.
+		///
+		/// The point will be projected down to a plane that contains the surface of the node.
+		/// If the point is not contained inside the node, it is projected down onto this plane anyway.
+		/// </summary>
+		public Vector3 ProjectOnSurface (Vector3 point) {
+			Int3 a, b, c;
+
+			GetVertices(out a, out b, out c);
+			var pa = (Vector3)a;
+			var pb = (Vector3)b;
+			var pc = (Vector3)c;
+			var up = Vector3.Cross(pb-pa, pc-pa).normalized;
+			return point - up * Vector3.Dot(up, point-pa);
+		}
+
+		public override Vector3 ClosestPointOnNode (Vector3 p) {
+			Int3 a, b, c;
+
+			GetVertices(out a, out b, out c);
+			return Pathfinding.Polygon.ClosestPointOnTriangle((float3)(Vector3)a, (float3)(Vector3)b, (float3)(Vector3)c, (float3)p);
+		}
+
+		/// <summary>
+		/// Closest point on the node when seen from above.
+		/// This method is mostly for internal use as the <see cref="Pathfinding.NavmeshBase.Linecast"/> methods use it.
+		///
+		/// - The returned point is the closest one on the node to p when seen from above (relative to the graph).
+		///   This is important mostly for sloped surfaces.
+		/// - The returned point is an Int3 point in graph space.
+		/// - It is guaranteed to be inside the node, so if you call <see cref="ContainsPointInGraphSpace"/> with the return value from this method the result is guaranteed to be true.
+		///
+		/// This method is slower than e.g <see cref="ClosestPointOnNode"/> or <see cref="ClosestPointOnNodeXZ"/>.
+		/// However they do not have the same guarantees as this method has.
+		/// </summary>
+		internal Int3 ClosestPointOnNodeXZInGraphSpace (Vector3 p) {
+			// Get the vertices that make up the triangle
+			Int3 a, b, c;
+
+			GetVerticesInGraphSpace(out a, out b, out c);
+
+			// Convert p to graph space
+			p = GetNavmeshHolder(GraphIndex).transform.InverseTransform(p);
+
+			// Find the closest point on the triangle to p when looking at the triangle from above (relative to the graph)
+			var closest = Pathfinding.Polygon.ClosestPointOnTriangleXZ((Vector3)a, (Vector3)b, (Vector3)c, p);
+
+			// Make sure the point is actually inside the node
+			var i3closest = (Int3)closest;
+			if (ContainsPointInGraphSpace(i3closest)) {
+				// Common case
+				return i3closest;
+			} else {
+				// Annoying...
+				// The closest point when converted from floating point coordinates to integer coordinates
+				// is not actually inside the node. It needs to be inside the node for some methods
+				// (like for example Linecast) to work properly.
+
+				// Try the 8 integer coordinates around the closest point
+				// and check if any one of them are completely inside the node.
+				// This will most likely succeed as it should be very close.
+				for (int dx = -1; dx <= 1; dx++) {
+					for (int dz = -1; dz <= 1; dz++) {
+						if ((dx != 0 || dz != 0)) {
+							var candidate = new Int3(i3closest.x + dx, i3closest.y, i3closest.z + dz);
+							if (ContainsPointInGraphSpace(candidate)) return candidate;
+						}
+					}
+				}
+
+				// Happens veery rarely.
+				// Pick the closest vertex of the triangle.
+				// The vertex is guaranteed to be inside the triangle.
+				var da = (a - i3closest).sqrMagnitudeLong;
+				var db = (b - i3closest).sqrMagnitudeLong;
+				var dc = (c - i3closest).sqrMagnitudeLong;
+				return da < db ? (da < dc ? a : c) : (db < dc ? b : c);
+			}
+		}
+
+		public override Vector3 ClosestPointOnNodeXZ (Vector3 p) {
+			// Get all 3 vertices for this node
+			GetVertices(out Int3 tp1, out Int3 tp2, out Int3 tp3);
+			return Polygon.ClosestPointOnTriangleXZ((Vector3)tp1, (Vector3)tp2, (Vector3)tp3, p);
+		}
+
+		/// <summary>
+		/// Checks if point is inside the node when seen from above.
+		///
+		/// Note that <see cref="ContainsPointInGraphSpace"/> is faster than this method as it avoids
+		/// some coordinate transformations. If you are repeatedly calling this method
+		/// on many different nodes but with the same point then you should consider
+		/// transforming the point first and then calling ContainsPointInGraphSpace.
+		///
+		/// <code>
+		/// Int3 p = (Int3)graph.transform.InverseTransform(point);
+		///
+		/// node.ContainsPointInGraphSpace(p);
+		/// </code>
+		/// </summary>
+		public override bool ContainsPoint (Vector3 p) {
+			return ContainsPointInGraphSpace((Int3)GetNavmeshHolder(GraphIndex).transform.InverseTransform(p));
+		}
+
+		/// <summary>Checks if point is inside the node when seen from above, as defined by the movement plane</summary>
+		public bool ContainsPoint (Vector3 p, NativeMovementPlane movementPlane) {
+			// Get all 3 vertices for this node
+			GetVertices(out var a, out var b, out var c);
+			var pa = (int3)a;
+			var pb = (int3)b;
+			var pc = (int3)c;
+			var pp = (int3)(Int3)p;
+			return Polygon.ContainsPoint(ref pa, ref pb, ref pc, ref pp, ref movementPlane);
+		}
+
+		/// <summary>
+		/// Checks if point is inside the node in graph space.
+		///
+		/// In graph space the up direction is always the Y axis so in principle
+		/// we project the triangle down on the XZ plane and check if the point is inside the 2D triangle there.
+		/// </summary>
+		public override bool ContainsPointInGraphSpace (Int3 p) {
+			// Get all 3 vertices for this node
+			GetVerticesInGraphSpace(out var a, out var b, out var c);
+
+			if ((long)(b.x - a.x) * (long)(p.z - a.z) - (long)(p.x - a.x) * (long)(b.z - a.z) > 0) return false;
+
+			if ((long)(c.x - b.x) * (long)(p.z - b.z) - (long)(p.x - b.x) * (long)(c.z - b.z) > 0) return false;
+
+			if ((long)(a.x - c.x) * (long)(p.z - c.z) - (long)(p.x - c.x) * (long)(a.z - c.z) > 0) return false;
+
+			return true;
+			// Equivalent code, but the above code is faster
+			//return Polygon.IsClockwiseMargin (a,b, p) && Polygon.IsClockwiseMargin (b,c, p) && Polygon.IsClockwiseMargin (c,a, p);
+
+			//return Polygon.ContainsPoint(g.GetVertex(v0),g.GetVertex(v1),g.GetVertex(v2),p);
+		}
+
+		public static readonly Unity.Profiling.ProfilerMarker MarkerDecode = new Unity.Profiling.ProfilerMarker("Decode");
+		public static readonly Unity.Profiling.ProfilerMarker MarkerGetVertices = new Unity.Profiling.ProfilerMarker("GetVertex");
+		public static readonly Unity.Profiling.ProfilerMarker MarkerClosest = new Unity.Profiling.ProfilerMarker("MarkerClosest");
+
+		public override Int3 DecodeVariantPosition (uint pathNodeIndex, uint fractionAlongEdge) {
+			var edge = (int)(pathNodeIndex - NodeIndex);
+			var p1 = GetVertex(edge);
+			var p2 = GetVertex((edge + 1) % 3);
+			InterpolateEdge(ref p1, ref p2, fractionAlongEdge, out var pos);
+			return pos;
+		}
+
+		[BurstCompile(FloatMode = FloatMode.Fast)]
+		static void InterpolateEdge (ref Int3 p1, ref Int3 p2, uint fractionAlongEdge, out Int3 pos) {
+			var p = (int3)math.lerp((float3)(int3)p1, (float3)(int3)p2, PathNode.UnQuantizeFractionAlongEdge(fractionAlongEdge));
+			pos = new Int3(p.x, p.y, p.z);
+		}
+
+		public override void OpenAtPoint (Path path, uint pathNodeIndex, Int3 point, uint gScore) {
+			if (InaccuratePathSearch) {
+				Open(path, pathNodeIndex, gScore);
+			} else {
+				OpenAtPoint(path, pathNodeIndex, point, -1, gScore);
+			}
+		}
+
+		public override void Open (Path path, uint pathNodeIndex, uint gScore) {
+			var pathHandler = (path as IPathInternals).PathHandler;
+			if (InaccuratePathSearch) {
+				var pn = pathHandler.pathNodes[pathNodeIndex];
+				if (pn.flag1) path.OpenCandidateConnectionsToEndNode(position, pathNodeIndex, NodeIndex, gScore);
+
+				if (connections != null) {
+					// Iterate over all adjacent nodes
+					for (int i = connections.Length-1; i >= 0; i--) {
+						var conn = connections[i];
+						var other = conn.node;
+						if (conn.isOutgoing && other.NodeIndex != pn.parentIndex) {
+							path.OpenCandidateConnection(pathNodeIndex, other.NodeIndex, gScore, conn.cost + path.GetTraversalCost(other), 0, other.position);
+						}
+					}
+				}
+				return;
+			}
+			// One path node variant is created for each side of the triangle
+			// This particular path node represents just one of the sides of the triangle.
+			var edge = (int)(pathNodeIndex - NodeIndex);
+			OpenAtPoint(path, pathNodeIndex, DecodeVariantPosition(pathNodeIndex, pathHandler.pathNodes[pathNodeIndex].fractionAlongEdge), edge, gScore);
+		}
+
+		void OpenAtPoint (Path path, uint pathNodeIndex, Int3 pos, int edge, uint gScore) {
+			var pathHandler = (path as IPathInternals).PathHandler;
+			var pn = pathHandler.pathNodes[pathNodeIndex];
+			if (pn.flag1) path.OpenCandidateConnectionsToEndNode(pos, pathNodeIndex, NodeIndex, gScore);
+			int visitedEdges = 0;
+			bool cameFromOtherEdgeInThisTriangle = pn.parentIndex >= NodeIndex && pn.parentIndex < NodeIndex + 3;
+
+			if (connections != null) {
+				// Iterate over all adjacent nodes
+				for (int i = connections.Length-1; i >= 0; i--) {
+					var conn = connections[i];
+					if (!conn.isOutgoing) continue;
+					var other = conn.node;
+
+					// Check if we are moving from a side of this triangle, to the corresponding side on an adjacent triangle.
+					if (conn.isEdgeShared) {
+						var sharedEdgeOnOtherNode = conn.adjacentShapeEdge;
+						var adjacentPathNodeIndex = other.NodeIndex + (uint)sharedEdgeOnOtherNode;
+
+						// Skip checking our parent node. This is purely a performance optimization.
+						if (adjacentPathNodeIndex == pn.parentIndex) continue;
+
+						if (conn.shapeEdge == edge) {
+							// Make sure we can traverse the neighbour
+							if (path.CanTraverse(this, other)) {
+								var tOther = other as TriangleMeshNode;
+
+								// Fast path out if we know we have already searched this node and we cannot improve it
+								if (!path.ShouldConsiderPathNode(adjacentPathNodeIndex)) {
+									continue;
+								}
+
+								if (conn.edgesAreIdentical) {
+									// The edge on the other node is identical to this edge (but reversed).
+									// This means that no other node can reach the other node through that edge.
+									// This is great, because we can then skip adding that node to the heap just
+									// to immediatelly pop it again. This is a performance optimization.
+
+									var otherEnteringCost = path.GetTraversalCost(other);
+									ref var otherPathNode = ref pathHandler.pathNodes[adjacentPathNodeIndex];
+									otherPathNode.pathID = path.pathID;
+									otherPathNode.heapIndex = BinaryHeap.NotInHeap;
+									otherPathNode.parentIndex = pathNodeIndex;
+									otherPathNode.fractionAlongEdge = PathNode.ReverseFractionAlongEdge(pn.fractionAlongEdge);
+									// Make sure the path gets information about us having visited this in-between node,
+									// even if we never add it to the heap
+									path.OnVisitNode(adjacentPathNodeIndex, uint.MaxValue, gScore + otherEnteringCost);
+									pathHandler.LogVisitedNode(adjacentPathNodeIndex, uint.MaxValue, gScore + otherEnteringCost);
+
+									tOther.OpenAtPoint(path, adjacentPathNodeIndex, pos, sharedEdgeOnOtherNode, gScore + otherEnteringCost);
+								} else {
+									OpenSingleEdge(path, pathNodeIndex, tOther, sharedEdgeOnOtherNode, pos, gScore);
+								}
+							}
+						} else {
+							// The other node is a node which shares a different edge with this node.
+							// We will consider this connection at another time.
+
+							// However, we will consider the move to another side of this triangle,
+							// namely to the side that *is* shared with the other node.
+							// If a side of this triangle doesn't share an edge with any connection, we will
+							// not bother searching it (we will not reach this part of the code), because
+							// we know its a dead end.
+
+							// If we came from another side of this triangle, it is completely redundant to try to move back to
+							// another edge in this triangle, because we could always have reached it faster from the parent.
+							// We also make sure we don't attempt to move to the same edge twice, as that's just a waste of time.
+							if (!cameFromOtherEdgeInThisTriangle && (visitedEdges & (1 << conn.shapeEdge)) == 0) {
+								visitedEdges |= 1 << conn.shapeEdge;
+								OpenSingleEdge(path, pathNodeIndex, this, conn.shapeEdge, pos, gScore);
+							}
+						}
+					} else if (!cameFromOtherEdgeInThisTriangle) {
+						// This is a connection to some other node type, most likely. For example an off-mesh link.
+						if (path.CanTraverse(this, other) && path.ShouldConsiderPathNode(other.NodeIndex)) {
+							var cost = (uint)(other.position - pos).costMagnitude;
+
+							if (edge != -1) {
+								// We are moving from an edge of this triangle
+								path.OpenCandidateConnection(pathNodeIndex, other.NodeIndex, gScore, cost, 0, other.position);
+							} else {
+								// In some situations we may be moving directly from one off-mesh link to another one without
+								// passing through any concrete nodes in between. In this case we need to create a temporary node
+								// to allow the correct path to be reconstructed later. The only important part of the temporary
+								// node is that we save this node as the associated node.
+								// This is somewhat ugly, and it limits the number of times we can encounter this case during
+								// a single search (there's a limit to the number of temporary nodes we can have at the same time).
+								// Fortunately, this case only happens if there is more than 1 off-mesh link connected to a single
+								// node, which is quite rare in most games.
+								// In this case, pathNodeIndex will be another node's index, not a path node belonging to this node.
+								var viaNode = pathHandler.AddTemporaryNode(new TemporaryNode {
+									associatedNode = NodeIndex,
+									position = pos,
+									targetIndex = 0,
+									type = TemporaryNodeType.Ignore,
+								});
+								ref var viaPathNode = ref pathHandler.pathNodes[viaNode];
+								viaPathNode.pathID = path.pathID;
+								viaPathNode.parentIndex = pathNodeIndex;
+								path.OpenCandidateConnection(viaNode, other.NodeIndex, gScore, cost, 0, other.position);
+							}
+						}
+					}
+				}
+			}
+		}
+
+		void OpenSingleEdge (Path path, uint pathNodeIndex, TriangleMeshNode other, int sharedEdgeOnOtherNode, Int3 pos, uint gScore) {
+			var adjacentPathNodeIndex = other.NodeIndex + (uint)sharedEdgeOnOtherNode;
+
+			// Fast path out if we know we have already searched this node and we cannot improve it
+			if (!path.ShouldConsiderPathNode(adjacentPathNodeIndex)) {
+				return;
+			}
+
+			var s1 = other.GetVertex(sharedEdgeOnOtherNode);
+			var s2 = other.GetVertex((sharedEdgeOnOtherNode + 1) % 3);
+
+			var pathHandler = (path as IPathInternals).PathHandler;
+			// TODO: Incorrect, counts nodes multiple times
+			var otherEnteringCost = path.GetTraversalCost(other);
+
+			var candidateG = gScore + otherEnteringCost;
+
+			OpenSingleEdgeBurst(
+				ref s1,
+				ref s2,
+				ref pos,
+				path.pathID,
+				pathNodeIndex,
+				adjacentPathNodeIndex,
+				other.NodeIndex,
+				candidateG,
+				ref pathHandler.pathNodes,
+				ref pathHandler.heap,
+				ref path.heuristicObjectiveInternal
+				);
+		}
+
+		[Unity.Burst.BurstCompile]
+		static void OpenSingleEdgeBurst (ref Int3 s1, ref Int3 s2, ref Int3 pos, ushort pathID, uint pathNodeIndex, uint candidatePathNodeIndex, uint candidateNodeIndex, uint candidateG, ref UnsafeSpan<PathNode> pathNodes, ref BinaryHeap heap, ref HeuristicObjective heuristicObjective) {
+			CalculateBestEdgePosition(ref s1, ref s2, ref pos, out var closestPointAlongEdge, out var quantizedFractionAlongEdge, out var cost);
+			candidateG += cost;
+
+			var pars = new Path.OpenCandidateParams {
+				pathID = pathID,
+				parentPathNode = pathNodeIndex,
+				targetPathNode = candidatePathNodeIndex,
+				targetNodeIndex = candidateNodeIndex,
+				candidateG = candidateG,
+				fractionAlongEdge = quantizedFractionAlongEdge,
+				targetNodePosition = closestPointAlongEdge,
+				pathNodes = pathNodes,
+			};
+			Path.OpenCandidateConnectionBurst(ref pars, ref heap, ref heuristicObjective);
+		}
+
+		[Unity.Burst.BurstCompile]
+		static void CalculateBestEdgePosition (ref Int3 s1, ref Int3 s2, ref Int3 pos, out int3 closestPointAlongEdge, out uint quantizedFractionAlongEdge, out uint cost) {
+			// Find the closest point on the other edge. From here on, we will let the position of that path node be this closest point.
+			// This is much better than using the edge midpoint, and also better than any interpolation between closestFractionAlongEdge
+			// and the midpoint (0.5).
+			// In my tests, using the edge midpoint leads to path costs that are rougly 1.3-1.6 times greater than the real distance,
+			// but using the closest point leads to path costs that are only 1.1-1.2 times greater than the real distance.
+			// Using triangle centers is the worst option, it leads to path costs that are roughly 1.6-2.0 times greater than the real distance.
+			// Triangle centers were always used before version 4.3.67.
+			var v1 = (float3)(int3)s1;
+			var v2 = (float3)(int3)s2;
+			var posi = (int3)pos;
+			var closestFractionAlongEdge = math.clamp(VectorMath.ClosestPointOnLineFactor(v1, v2, (float3)posi), 0, 1);
+			quantizedFractionAlongEdge = PathNode.QuantizeFractionAlongEdge(closestFractionAlongEdge);
+			closestFractionAlongEdge = PathNode.UnQuantizeFractionAlongEdge(quantizedFractionAlongEdge);
+			var closestPointAlongEdgeV = math.lerp(v1, v2, closestFractionAlongEdge);
+			closestPointAlongEdge = (int3)closestPointAlongEdgeV;
+
+			var diff = posi - closestPointAlongEdge;
+			cost = (uint)new Int3(diff.x, diff.y, diff.z).costMagnitude;
+		}
+
+		/// <summary>
+		/// Returns the edge which is shared with other.
+		///
+		/// If there is no shared edge between the two nodes, then -1 is returned.
+		///
+		/// The vertices in the edge can be retrieved using
+		/// <code>
+		/// var edge = node.SharedEdge(other);
+		/// var a = node.GetVertex(edge);
+		/// var b = node.GetVertex((edge+1) % node.GetVertexCount());
+		/// </code>
+		///
+		/// See: <see cref="GetPortal"/> which also handles edges that are shared over tile borders and some types of node links
+		/// </summary>
+		public int SharedEdge (GraphNode other) {
+			var edge = -1;
+
+			if (connections != null) {
+				for (int i = 0; i < connections.Length; i++) {
+					if (connections[i].node == other && connections[i].isEdgeShared) edge = connections[i].shapeEdge;
+				}
+			}
+			return edge;
+		}
+
+		public override bool GetPortal (GraphNode toNode, out Vector3 left, out Vector3 right) {
+			return GetPortal(toNode, out left, out right, out _, out _);
+		}
+
+		public bool GetPortalInGraphSpace (TriangleMeshNode toNode, out Int3 a, out Int3 b, out int aIndex, out int bIndex) {
+			aIndex = -1;
+			bIndex = -1;
+			a = Int3.zero;
+			b = Int3.zero;
+
+			// If the nodes are in different graphs, this function has no idea on how to find a shared edge.
+			if (toNode.GraphIndex != GraphIndex) return false;
+
+			int edge = -1;
+			int otherEdge = -1;
+			if (connections != null) {
+				for (int i = 0; i < connections.Length; i++) {
+					if (connections[i].node == toNode && connections[i].isEdgeShared) {
+						edge = connections[i].shapeEdge;
+						otherEdge = connections[i].adjacentShapeEdge;
+					}
+				}
+			}
+
+			// -1: No connection was found between the nodes
+			if (edge == -1) return false;
+
+			aIndex = edge;
+			bIndex = (edge + 1) % 3;
+
+			// Get the vertices of the shared edge for the first node
+			var graph = GetNavmeshHolder(GraphIndex);
+			a = graph.GetVertexInGraphSpace(GetVertexIndex(aIndex));
+			b = graph.GetVertexInGraphSpace(GetVertexIndex(bIndex));
+
+			// Get tiles the nodes are contained in
+			int tileIndex1 = TileIndex;
+			int tileIndex2 = toNode.TileIndex;
+
+			if (tileIndex1 != tileIndex2) {
+				// When the nodes are in different tiles, the edges may not be completely identical
+				// so another technique is needed.
+
+				// When the nodes are in different tiles, they might not share exactly the same edge
+				// so we clamp the portal to the segment of the edges which they both have..
+
+				// Get the vertices of the shared edge for the second node
+				Int3 v2a = toNode.GetVertexInGraphSpace(otherEdge);
+				Int3 v2b = toNode.GetVertexInGraphSpace((otherEdge+1) % 3);
+				graph.GetTileCoordinates(tileIndex1, out var tileX1, out var tileZ1);
+				graph.GetTileCoordinates(tileIndex2, out var tileX2, out var tileZ2);
+				var axis = tileX1 == tileX2 ? 0 : 2;
+				Assert.IsTrue(axis == 0 ? tileX1 == tileX2 : tileZ1 == tileZ2);
+				// This tile-edge aligned coordinate of the vertices should ideally be identical.
+				// But somewhere in the pipeline some errors may crop up, and thus they may be off by one.
+				// TODO: Fix this.
+				Assert.IsTrue(Mathf.Abs(a[2 - axis] - b[2 - axis]) <= 1);
+				var mn = Mathf.Min(v2a[axis], v2b[axis]);
+				var mx = Mathf.Max(v2a[axis], v2b[axis]);
+
+				a[axis] = Mathf.Clamp(a[axis], mn, mx);
+				b[axis] = Mathf.Clamp(b[axis], mn, mx);
+			}
+
+			return true;
+		}
+
+		public bool GetPortal (GraphNode toNode, out Vector3 left, out Vector3 right, out int aIndex, out int bIndex) {
+			if (toNode is TriangleMeshNode toTriNode && GetPortalInGraphSpace(toTriNode, out var a, out var b, out aIndex, out bIndex)) {
+				var graph = GetNavmeshHolder(GraphIndex);
+				// All triangles should be laid out in clockwise order so b is the rightmost vertex (seen from this node)
+				left = graph.transform.Transform((Vector3)a);
+				right = graph.transform.Transform((Vector3)b);
+				return true;
+			} else {
+				aIndex = -1;
+				bIndex = -1;
+				left = Vector3.zero;
+				right = Vector3.zero;
+				return false;
+			}
+		}
+
+		/// <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 override Vector3 RandomPointOnSurface () {
+			// Find a random point inside the triangle
+			// This generates uniformly distributed trilinear coordinates
+			// See http://mathworld.wolfram.com/TrianglePointPicking.html
+			float2 r;
+
+			do {
+				r = AstarMath.ThreadSafeRandomFloat2();
+			} while (r.x+r.y > 1);
+
+			// Pick the point corresponding to the trilinear coordinate
+			GetVertices(out var v0, out var v1, out var v2);
+			return ((Vector3)(v1-v0))*r.x + ((Vector3)(v2-v0))*r.y + (Vector3)v0;
+		}
+
+		public override void SerializeNode (GraphSerializationContext ctx) {
+			base.SerializeNode(ctx);
+			ctx.writer.Write(v0);
+			ctx.writer.Write(v1);
+			ctx.writer.Write(v2);
+		}
+
+		public override void DeserializeNode (GraphSerializationContext ctx) {
+			base.DeserializeNode(ctx);
+			v0 = ctx.reader.ReadInt32();
+			v1 = ctx.reader.ReadInt32();
+			v2 = ctx.reader.ReadInt32();
+		}
+	}
+}