577 lines
24 KiB
C#
577 lines
24 KiB
C#
using System.Collections.Generic;
|
|
using Pathfinding.Pooling;
|
|
using Unity.Mathematics;
|
|
using UnityEngine;
|
|
using UnityEngine.Profiling;
|
|
|
|
namespace Pathfinding {
|
|
/// <summary>
|
|
/// Simplifies a path on a grid graph using a string pulling algorithm.
|
|
/// This is based on a paper called "Toward a String-Pulling Approach to Path Smoothing on Grid Graphs",
|
|
/// with some optimizations as well as fixes for some edge cases that the paper didn't handle.
|
|
///
|
|
/// The result is conceptually similar to the well known funnel string pulling algorithm for navmesh graphs
|
|
/// but it uses a different algorithm.
|
|
///
|
|
/// This class is used by the <see cref="FunnelModifier"/> on grid graphs.
|
|
///
|
|
/// See: <see cref="Funnel"/>
|
|
/// See: <see cref="FunnelModifier"/>
|
|
/// See: article: https://ojs.aaai.org/index.php/SOCS/article/view/18541
|
|
/// </summary>
|
|
public static class GridStringPulling {
|
|
/// <summary>
|
|
/// Z
|
|
/// |
|
|
/// |
|
|
///
|
|
/// 3 2
|
|
/// \ | /
|
|
/// -- - X - ----- X
|
|
/// / | \
|
|
/// 0 1
|
|
///
|
|
/// |
|
|
/// |
|
|
/// </summary>
|
|
static int2[] directionToCorners = new int2[] {
|
|
new int2(0, 0),
|
|
new int2(FixedPrecisionScale, 0),
|
|
new int2(FixedPrecisionScale, FixedPrecisionScale),
|
|
new int2(0, FixedPrecisionScale),
|
|
};
|
|
|
|
static long Cross (int2 lhs, int2 rhs) {
|
|
return (long)lhs.x*(long)rhs.y - (long)lhs.y*(long)rhs.x;
|
|
}
|
|
|
|
static long Dot (int2 a, int2 b) {
|
|
return (long)a.x*(long)b.x + (long)a.y*(long)b.y;
|
|
}
|
|
|
|
static bool RightOrColinear (int2 a, int2 b, int2 p) {
|
|
return (long)(b.x - a.x) * (long)(p.y - a.y) - (long)(p.x - a.x) * (long)(b.y - a.y) <= 0;
|
|
}
|
|
|
|
static int2 Perpendicular (int2 v) {
|
|
return new int2(-v.y, v.x);
|
|
}
|
|
|
|
struct TriangleBounds {
|
|
int2 d1, d2, d3;
|
|
long t1, t2, t3;
|
|
|
|
public TriangleBounds(int2 p1, int2 p2, int2 p3) {
|
|
if (RightOrColinear(p1, p2, p3)) {
|
|
var tmp = p3;
|
|
p3 = p1;
|
|
p1 = tmp;
|
|
}
|
|
d1 = Perpendicular(p2 - p1);
|
|
d2 = Perpendicular(p3 - p2);
|
|
d3 = Perpendicular(p1 - p3);
|
|
t1 = Dot(d1, p1);
|
|
t2 = Dot(d2, p2);
|
|
t3 = Dot(d3, p3);
|
|
}
|
|
|
|
public bool Contains (int2 p) {
|
|
return Dot(d1, p) >= t1 && Dot(d2, p) >= t2 && Dot(d3, p) >= t3;
|
|
}
|
|
}
|
|
|
|
const int FixedPrecisionScale = 1024;
|
|
|
|
static int2 ToFixedPrecision (Vector2 p) {
|
|
return new int2(math.round(new float2(p)*FixedPrecisionScale));
|
|
}
|
|
|
|
static Vector2 FromFixedPrecision (int2 p) {
|
|
return (Vector2)(((float2)p) * (1.0f/FixedPrecisionScale));
|
|
}
|
|
|
|
/// <summary>Returns which side of the line a - b that p lies on</summary>
|
|
static Side Side2D (int2 a, int2 b, int2 p) {
|
|
var s = Cross(b-a, p-a);
|
|
|
|
return s > 0 ? Side.Left : (s < 0 ? Side.Right : Side.Colinear);
|
|
}
|
|
|
|
static Unity.Profiling.ProfilerMarker marker1 = new Unity.Profiling.ProfilerMarker("Linecast hit");
|
|
static Unity.Profiling.ProfilerMarker marker2 = new Unity.Profiling.ProfilerMarker("Linecast success");
|
|
static Unity.Profiling.ProfilerMarker marker3 = new Unity.Profiling.ProfilerMarker("Trace");
|
|
static Unity.Profiling.ProfilerMarker marker4 = new Unity.Profiling.ProfilerMarker("Neighbours");
|
|
static Unity.Profiling.ProfilerMarker marker5 = new Unity.Profiling.ProfilerMarker("Re-evaluate linecast");
|
|
static Unity.Profiling.ProfilerMarker marker6 = new Unity.Profiling.ProfilerMarker("Init");
|
|
static Unity.Profiling.ProfilerMarker marker7 = new Unity.Profiling.ProfilerMarker("Initloop");
|
|
|
|
/// <summary>
|
|
/// Intersection length of the given segment with a square of size Int3.Precision centered at nodeCenter.
|
|
/// The return value is between 0 and sqrt(2).
|
|
/// </summary>
|
|
public static float IntersectionLength (int2 nodeCenter, int2 segmentStart, int2 segmentEnd) {
|
|
// TODO: Calculations can be hoisted
|
|
var invNormal = math.rcp((float2)(segmentEnd - segmentStart));
|
|
var normalMagn = math.length((float2)(segmentEnd - segmentStart));
|
|
|
|
float tmin = float.NegativeInfinity, tmax = float.PositiveInfinity;
|
|
|
|
var normal = segmentEnd - segmentStart;
|
|
var bmin = nodeCenter; // - new int2(Int3.Precision/2, Int3.Precision/2);
|
|
var bmax = nodeCenter + new int2(FixedPrecisionScale, FixedPrecisionScale);
|
|
|
|
if (normal.x != 0.0) {
|
|
float tx1 = (bmin.x - segmentStart.x)*invNormal.x;
|
|
float tx2 = (bmax.x - segmentStart.x)*invNormal.x;
|
|
|
|
tmin = math.max(tmin, math.min(tx1, tx2));
|
|
tmax = math.min(tmax, math.max(tx1, tx2));
|
|
} else if (segmentStart.x < bmin.x || segmentStart.x > bmax.x) {
|
|
return 0.0f;
|
|
}
|
|
|
|
if (normal.y != 0.0) {
|
|
float ty1 = (bmin.y - segmentStart.y)*invNormal.y;
|
|
float ty2 = (bmax.y - segmentStart.y)*invNormal.y;
|
|
|
|
tmin = math.max(tmin, math.min(ty1, ty2));
|
|
tmax = math.min(tmax, math.max(ty1, ty2));
|
|
} else if (segmentStart.y < bmin.y || segmentStart.y > bmax.y) {
|
|
return 0.0f;
|
|
}
|
|
|
|
tmin = math.max(0, tmin);
|
|
tmax = math.min(1, tmax);
|
|
return math.max(tmax - tmin, 0)*normalMagn*(1.0f/FixedPrecisionScale);
|
|
}
|
|
|
|
internal static void TestIntersectionLength () {
|
|
var s = FixedPrecisionScale;
|
|
|
|
UnityEngine.Assertions.Assert.AreApproximatelyEqual(math.sqrt(2), IntersectionLength(new int2(1*s, 1*s), new int2(0, 0), new int2(2*s, 2*s)));
|
|
UnityEngine.Assertions.Assert.AreApproximatelyEqual(0.0f, IntersectionLength(new int2(1*s, 1*s), new int2(0, 0), new int2(0, 0)));
|
|
UnityEngine.Assertions.Assert.AreApproximatelyEqual(1.0f, IntersectionLength(new int2(1*s, 1*s), new int2(-1*s, s+1), new int2(2*s, s+1)));
|
|
UnityEngine.Assertions.Assert.AreApproximatelyEqual(1.0f, IntersectionLength(new int2(1*s, 1*s), new int2(2*s, s), new int2(-1*s, s)));
|
|
|
|
// All sides of the square should be included
|
|
UnityEngine.Assertions.Assert.AreApproximatelyEqual(1.0f, IntersectionLength(new int2(1*s, 1*s), new int2(s, s), new int2(s+s, s)));
|
|
UnityEngine.Assertions.Assert.AreApproximatelyEqual(1.0f, IntersectionLength(new int2(1*s, 1*s), new int2(s+s, s), new int2(s+s, s+s)));
|
|
UnityEngine.Assertions.Assert.AreApproximatelyEqual(1.0f, IntersectionLength(new int2(1*s, 1*s), new int2(s+s, s+s), new int2(s, s+s)));
|
|
UnityEngine.Assertions.Assert.AreApproximatelyEqual(1.0f, IntersectionLength(new int2(1*s, 1*s), new int2(s, s+s), new int2(s, s)));
|
|
}
|
|
|
|
/// <summary>
|
|
/// Cost of moving across all the nodes in the list, along the given segment.
|
|
/// It is assumed that the segment intersects the nodes. Any potentially intersecting nodes that are not part of the list will be ignored.
|
|
/// </summary>
|
|
static uint LinecastCost (List<GraphNode> trace, int2 segmentStart, int2 segmentEnd, GridGraph gg, System.Func<GraphNode, uint> traversalCost) {
|
|
// Check the cost of the segment compared to not taking this "shortcut"
|
|
uint cost = 0;
|
|
|
|
for (int k = 0; k < trace.Count; k++) {
|
|
var node = trace[k] as GridNodeBase;
|
|
// Note: this assumes the default grid connection costs are used. Which is relatively reasonable
|
|
// since they require changing the code to modify.
|
|
cost += (uint)(((float)traversalCost(node) + gg.nodeSize*Int3.Precision) * IntersectionLength(new int2(node.XCoordinateInGrid, node.ZCoordinateInGrid)*FixedPrecisionScale, segmentStart, segmentEnd));
|
|
}
|
|
return cost;
|
|
}
|
|
|
|
enum PredicateFailMode {
|
|
Undefined,
|
|
Turn,
|
|
LinecastObstacle,
|
|
LinecastCost,
|
|
ReachedEnd,
|
|
}
|
|
|
|
/// <summary>
|
|
/// Simplifies a path on a grid graph using a string pulling algorithm.
|
|
/// See the class documentation for more details.
|
|
/// </summary>
|
|
/// <param name="pathNodes">A list of input nodes. Only the slice of nodes from nodeStartIndex to nodeEndIndex (inclusive) will be used. These must all be of type GridNodeBase and must form a path (i.e. each node must be a neighbor to the next one in the list).</param>
|
|
/// <param name="nodeStartIndex">The index in pathNodes to start from.</param>
|
|
/// <param name="nodeEndIndex">The last index in pathNodes that is used.</param>
|
|
/// <param name="startPoint">A more exact start point for the path. This should be a point inside the first node (if not, it will be clamped to the node's surface).</param>
|
|
/// <param name="endPoint">A more exact end point for the path. This should be a point inside the first node (if not, it will be clamped to the node's surface).</param>
|
|
/// <param name="traversalCost">Can be used to specify how much it costs to traverse each node. If this is null, node penalties and tag penalties will be completely ignored.</param>
|
|
/// <param name="filter">Can be used to filter out additional nodes that should be treated as unwalkable. It is assumed that all nodes in pathNodes pass this filter.</param>
|
|
/// <param name="maxCorners">If you only need the first N points of the result, you can specify that here, to avoid unnecessary work.</param>
|
|
public static List<Vector3> Calculate (List<GraphNode> pathNodes, int nodeStartIndex, int nodeEndIndex, Vector3 startPoint, Vector3 endPoint, System.Func<GraphNode, uint> traversalCost = null, System.Func<GraphNode, bool> filter = null, int maxCorners = int.MaxValue) {
|
|
Profiler.BeginSample("Funnel");
|
|
marker6.Begin();
|
|
// A list of indices into the arrays defined below.
|
|
// Each index represents a point. But it's more convenient to use indices here and keep all the data separately (also probably faster).
|
|
var outputPath = ListPool<int>.Claim();
|
|
outputPath.Add(0);
|
|
|
|
var numInputNodes = nodeEndIndex - nodeStartIndex + 1;
|
|
var gg = pathNodes[nodeStartIndex].Graph as GridGraph;
|
|
var trace = ListPool<GraphNode>.Claim();
|
|
var turn = Side.Colinear;
|
|
int counter = 0;
|
|
|
|
// We will add two points, see comments inside the loop.
|
|
// We may end up adding even more points later, therefore we get arrays that are a bit larger than we need for the initial path.
|
|
numInputNodes += 2;
|
|
int numPoints = numInputNodes;
|
|
var nodes = ArrayPool<GridNodeBase>.Claim(numPoints*2);
|
|
var points = ArrayPool<int2>.Claim(numPoints*2);
|
|
var normalizedPoints = ArrayPool<int2>.Claim(numPoints*2);
|
|
var costs = ArrayPool<uint>.Claim(numPoints*2);
|
|
|
|
marker7.Begin();
|
|
uint costSoFar = 0;
|
|
// Go through all points and store all relevant data we need about them
|
|
for (int j = 0; j < numInputNodes; j++) {
|
|
// After the start-end modifier has adjusted the endpoints of the path, the line from the start point to the center of the second node in the path
|
|
// might not actually have line of sight.
|
|
// Assume the path starts at N1 with a diagonal move to node N2.
|
|
// The start-end modifier adjusts the start point of the path to point S.
|
|
// This causes the path to cut the corner to the unwalkable node in the bottom right.
|
|
// ┌─────────┬────────┐
|
|
// │ │ │
|
|
// │ N2 │ │
|
|
// │ \ │ │
|
|
// │ \ │ │
|
|
// ├───────\─┼────────┤
|
|
// │########\│ │
|
|
// │#########│S N1 │
|
|
// │#########│ │
|
|
// │#########│ │
|
|
// └─────────┴────────┘
|
|
// We can solve this case by making the path go from S to N1 and then to N2 instead of directly from S to N2.
|
|
// We also do the same thing for the end of the path.
|
|
// The clamping and indexing here is essentially equivalent to one insert at the beginning of the arrays and one at the end.
|
|
var node = nodes[j] = pathNodes[math.clamp(nodeStartIndex + j-1, nodeStartIndex, nodeEndIndex)] as GridNodeBase;
|
|
var gridCoordinates = new int2(node.XCoordinateInGrid, node.ZCoordinateInGrid);
|
|
var point = gridCoordinates * FixedPrecisionScale;
|
|
int2 normalized;
|
|
if (j == 0) {
|
|
normalized = ToFixedPrecision(node.NormalizePoint(startPoint));
|
|
normalized = math.clamp(normalized, int2.zero, new int2(FixedPrecisionScale, FixedPrecisionScale));
|
|
} else if (j == numInputNodes - 1) {
|
|
normalized = ToFixedPrecision(node.NormalizePoint(endPoint));
|
|
normalized = math.clamp(normalized, int2.zero, new int2(FixedPrecisionScale, FixedPrecisionScale));
|
|
} else {
|
|
normalized = new int2(FixedPrecisionScale/2, FixedPrecisionScale/2);
|
|
}
|
|
points[j] = point + normalized;
|
|
normalizedPoints[j] = normalized;
|
|
if (j > 0 && traversalCost != null) {
|
|
// Calculate the cost of moving along the original path
|
|
costSoFar += (uint)(((float)traversalCost(nodes[j-1]) + gg.nodeSize*Int3.Precision) * IntersectionLength(new int2(nodes[j-1].XCoordinateInGrid, nodes[j-1].ZCoordinateInGrid)*FixedPrecisionScale, points[j-1], points[j]));
|
|
costSoFar += (uint)(((float)traversalCost(nodes[j]) + gg.nodeSize*Int3.Precision) * IntersectionLength(gridCoordinates*FixedPrecisionScale, points[j-1], points[j]));
|
|
}
|
|
costs[j] = costSoFar;
|
|
}
|
|
marker7.End();
|
|
|
|
// We know that there is line of sight from the first point to the second point in the path.
|
|
var lastSuccessfulStart = 0;
|
|
var lastSuccessfulEnd = 1;
|
|
marker6.End();
|
|
|
|
int i = 1;
|
|
while (true) {
|
|
if (i >= numInputNodes) {
|
|
// We are done, add the last point
|
|
outputPath.Add(numInputNodes-1);
|
|
break;
|
|
}
|
|
if (outputPath.Count >= maxCorners) {
|
|
// We are done with the partial result
|
|
break;
|
|
}
|
|
|
|
counter++;
|
|
if (counter > 10000) {
|
|
Debug.LogError("Inf loop");
|
|
break;
|
|
}
|
|
|
|
// In the paper, they just use a straight forward loop over the input path.
|
|
// However, it is better for performance to use a binary search to figure out the next time we need to do something.
|
|
// We only need an 'i' which succeeds and 'i+1' which fails.
|
|
// The success in this case is defined by the predicate below. We only need to do stuff if that returns true.
|
|
var last = outputPath[outputPath.Count-1];
|
|
var normalizedLast = normalizedPoints[last];
|
|
var prev = outputPath.Count > 1 ? outputPath[outputPath.Count-2] : -1;
|
|
var nodeLast = nodes[last];
|
|
var upperBound = numInputNodes - i - 1;
|
|
|
|
// Lower and upper bounds for the binary search
|
|
int mn = 0;
|
|
// It is reasonable that most paths can be simplified at least a bit. Assume that seeing 4 or more nodes ahead is common.
|
|
int mx = math.min(4, upperBound);
|
|
var mxFailMode = PredicateFailMode.Undefined;
|
|
uint mxLinecastCost = 0;
|
|
|
|
// The calculations are not perfectly accurate. Allow the shortcut's cost to be a tiny bit higher.
|
|
const uint COST_FUDGE = 5;
|
|
|
|
GridHitInfo hit;
|
|
// First fire off linecasts to nodes exponentially further away until the predicate returns true.
|
|
while (true) {
|
|
var idx = i + mx;
|
|
|
|
var turnPredicate = outputPath.Count > 1 && Side2D(points[prev], points[last], points[idx]) != turn;
|
|
if (turnPredicate) {
|
|
mxFailMode = PredicateFailMode.Turn;
|
|
break;
|
|
} else {
|
|
trace.Clear();
|
|
if (gg.Linecast(nodeLast, normalizedLast, nodes[idx], normalizedPoints[idx], out hit, trace, filter)) {
|
|
mxFailMode = PredicateFailMode.LinecastObstacle;
|
|
break;
|
|
} else if (traversalCost != null) {
|
|
var cost = LinecastCost(trace, points[last], points[idx], gg, traversalCost);
|
|
if (cost > costs[idx] - costs[last] + COST_FUDGE) {
|
|
// The "shortcut" had such a high penalty that it's not worth taking it
|
|
mxFailMode = PredicateFailMode.LinecastCost;
|
|
mxLinecastCost = cost;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
if (mx < upperBound) {
|
|
mn = mx;
|
|
mx = math.min(mx*2, upperBound);
|
|
} else {
|
|
mxFailMode = PredicateFailMode.ReachedEnd;
|
|
break;
|
|
}
|
|
}
|
|
|
|
if (mxFailMode == PredicateFailMode.ReachedEnd) {
|
|
// Reached final node without any hits, we can stop here
|
|
outputPath.Add(numInputNodes-1);
|
|
break;
|
|
}
|
|
|
|
// Run a standard binary search
|
|
while (mx > mn + 1) {
|
|
int mid = (mn+mx)/2;
|
|
int idx = i + mid;
|
|
|
|
var turnPredicate = outputPath.Count > 1 && Side2D(points[prev], points[last], points[idx]) != turn;
|
|
bool pred = turnPredicate;
|
|
if (turnPredicate) {
|
|
mxFailMode = PredicateFailMode.Turn;
|
|
} else {
|
|
trace.Clear();
|
|
if (gg.Linecast(nodeLast, normalizedLast, nodes[idx], normalizedPoints[idx], out hit, trace, filter)) {
|
|
mxFailMode = PredicateFailMode.LinecastObstacle;
|
|
pred = true;
|
|
} else if (traversalCost != null) {
|
|
var cost = LinecastCost(trace, points[last], points[idx], gg, traversalCost);
|
|
if (cost > costs[idx] - costs[last] + COST_FUDGE) {
|
|
// The "shortcut" had such a high penalty that it's not worth taking it
|
|
mxFailMode = PredicateFailMode.LinecastCost;
|
|
mxLinecastCost = cost;
|
|
pred = true;
|
|
}
|
|
}
|
|
}
|
|
|
|
if (pred) {
|
|
mx = mid;
|
|
} else {
|
|
mn = mid;
|
|
}
|
|
}
|
|
|
|
// i+mn is now a succeeding index, and i+mn+1 (or i+mx) is a failing index
|
|
if (mn > 0) {
|
|
lastSuccessfulStart = last;
|
|
lastSuccessfulEnd = i+mn;
|
|
} else {
|
|
// We are not actually completely sure that i+mn is a succeeding index if mn=0
|
|
// So double check it.
|
|
// TODO: This is a lot of code duplication. Tidy this up.
|
|
var turnPredicate = outputPath.Count > 1 && Side2D(points[prev], points[last], points[i+mn]) != turn;
|
|
bool pred = turnPredicate;
|
|
if (turnPredicate) {
|
|
} else {
|
|
trace.Clear();
|
|
if (gg.Linecast(nodeLast, normalizedLast, nodes[i+mn], normalizedPoints[i+mn], out hit, trace, filter)) {
|
|
pred = true;
|
|
} else if (traversalCost != null) {
|
|
var cost = LinecastCost(trace, points[last], points[i+mn], gg, traversalCost);
|
|
if (cost > costs[i+mn] - costs[last] + COST_FUDGE) {
|
|
// The "shortcut" had such a high penalty that it's not worth taking it
|
|
mxLinecastCost = cost;
|
|
pred = true;
|
|
}
|
|
}
|
|
}
|
|
|
|
if (!pred) {
|
|
// Success!
|
|
lastSuccessfulStart = last;
|
|
lastSuccessfulEnd = i+mn;
|
|
}
|
|
}
|
|
|
|
// Move to the failing index
|
|
i += mx;
|
|
UnityEngine.Assertions.Assert.AreNotEqual(mxFailMode, PredicateFailMode.Undefined);
|
|
|
|
marker5.Begin();
|
|
trace.Clear();
|
|
trace.Clear();
|
|
if (mxFailMode == PredicateFailMode.LinecastCost) {
|
|
outputPath.Add(lastSuccessfulEnd);
|
|
turn = Side2D(points[last], points[lastSuccessfulEnd], points[i]);
|
|
// It is guaranteed that there is line of sight from lastSuccessfulStart to lastSuccessfulEnd
|
|
lastSuccessfulStart = lastSuccessfulEnd;
|
|
i--;
|
|
marker5.End();
|
|
continue;
|
|
} else if (mxFailMode == PredicateFailMode.LinecastObstacle) {
|
|
marker5.End();
|
|
// Draw.Line(nodes[last].UnNormalizePoint(FromFixedPrecision(normalizedPoints[last])), toNode.UnNormalizePoint(FromFixedPrecision(normalizedTo)), Color.red);
|
|
marker1.Begin();
|
|
marker3.Begin();
|
|
// Re-run a previously successfully linecast to get all nodes it traversed.
|
|
trace.Clear();
|
|
int chosenCorner;
|
|
if (gg.Linecast(nodes[lastSuccessfulStart], normalizedPoints[lastSuccessfulStart], nodes[lastSuccessfulEnd], normalizedPoints[lastSuccessfulEnd], out hit, trace, filter)) {
|
|
// Weird! This linecast should have succeeded.
|
|
// Maybe the path crosses some unwalkable nodes it shouldn't cross (the graph could have changed).
|
|
// Or possibly the linecast implementation doesn't handle some edge case (there are so many!)
|
|
// In any case, we fall back to just assuming there is a valid line of sight.
|
|
chosenCorner = lastSuccessfulEnd;
|
|
Debug.LogError("Inconsistent linecasts");
|
|
} else {
|
|
trace.Add(nodes[i]);
|
|
marker3.End();
|
|
marker4.Begin();
|
|
|
|
GridNodeBase candidateNode = null;
|
|
var candidateNormalizedPoint = new int2();
|
|
uint candidateCost = 0;
|
|
var dirToCandidateCorner = new int2();
|
|
var lastSuccessfulStartPoint = points[lastSuccessfulStart];
|
|
var lastSuccessfulEndPoint = points[lastSuccessfulEnd];
|
|
var dir = lastSuccessfulEndPoint - lastSuccessfulStartPoint;
|
|
var bounds = new TriangleBounds(
|
|
lastSuccessfulStartPoint,
|
|
lastSuccessfulEndPoint,
|
|
points[i]
|
|
);
|
|
|
|
var desiredSide = System.Math.Sign(Cross(dir, points[i] - lastSuccessfulStartPoint));
|
|
var candidateCostSoFar = costs[lastSuccessfulStart];
|
|
for (int j = 0; j < trace.Count; j++) {
|
|
var node = trace[j] as GridNodeBase;
|
|
var nodeGridPos = new int2(node.XCoordinateInGrid, node.ZCoordinateInGrid);
|
|
var nodeCenter = nodeGridPos * FixedPrecisionScale;
|
|
if (traversalCost != null) {
|
|
// Not perfectly accurate as it doesn't measure the cost to the exact corner
|
|
candidateCostSoFar += (uint)(((float)traversalCost(node) + gg.nodeSize*Int3.Precision) * IntersectionLength(nodeCenter, lastSuccessfulStartPoint, lastSuccessfulEndPoint));
|
|
}
|
|
for (int d = 0; d < 4; d++) {
|
|
if (!node.HasConnectionInDirection(d) || (filter != null && !filter(node.GetNeighbourAlongDirection(d)))) {
|
|
for (int q = 0; q < 2; q++) {
|
|
var ncorner = directionToCorners[(d+q)&0x3];
|
|
var corner = nodeCenter + ncorner;
|
|
|
|
if (!bounds.Contains(corner)) {
|
|
continue;
|
|
}
|
|
|
|
var dirToCorner = corner - lastSuccessfulStartPoint;
|
|
// We shouldn't pick corners at our current position
|
|
if (math.all(dirToCorner == 0)) continue;
|
|
if (math.all(corner == lastSuccessfulEndPoint)) continue;
|
|
|
|
var side = Cross(dirToCorner, dirToCandidateCorner);
|
|
if (candidateNode == null || System.Math.Sign(side) == desiredSide || (side == 0 && math.lengthsq(dirToCorner) > math.lengthsq(dirToCandidateCorner))) {
|
|
dirToCandidateCorner = dirToCorner;
|
|
candidateNode = node;
|
|
candidateNormalizedPoint = ncorner;
|
|
candidateCost = candidateCostSoFar;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
marker4.End();
|
|
|
|
if (candidateNode == null) {
|
|
// Fall back to adding the lastSuccessfulEnd node. We know there's line of sight to that one.
|
|
chosenCorner = lastSuccessfulEnd;
|
|
} else {
|
|
chosenCorner = numPoints;
|
|
// TODO: Reallocate
|
|
nodes[numPoints] = candidateNode;
|
|
normalizedPoints[numPoints] = candidateNormalizedPoint;
|
|
var gridCoordinates = new int2(candidateNode.XCoordinateInGrid, candidateNode.ZCoordinateInGrid);
|
|
points[numPoints] = gridCoordinates * FixedPrecisionScale + candidateNormalizedPoint;
|
|
costs[numPoints] = candidateCost;
|
|
numPoints++;
|
|
}
|
|
}
|
|
|
|
outputPath.Add(chosenCorner);
|
|
turn = Side2D(points[last], points[chosenCorner], points[i]);
|
|
// It is guaranteed that there is line of sight from lastSuccessfulStart to chosenCorner because of how we choose the corner.
|
|
lastSuccessfulStart = chosenCorner;
|
|
i--;
|
|
marker1.End();
|
|
continue;
|
|
} else {
|
|
marker5.End();
|
|
marker2.Begin();
|
|
lastSuccessfulStart = last;
|
|
lastSuccessfulEnd = i;
|
|
// Draw.Line(nodes[last].UnNormalizePoint(FromFixedPrecision(normalizedPoints[last])), toNode.UnNormalizePoint(FromFixedPrecision(normalizedTo)), Color.green);
|
|
if (outputPath.Count > 1) {
|
|
var spPrev = outputPath[outputPath.Count-2];
|
|
var nextTurn = Side2D(points[spPrev], points[last], points[i]);
|
|
// Check if the string is no longer taut. If it is not we can remove a previous point.
|
|
if (turn != nextTurn) {
|
|
// Draw.SphereOutline(nodes[pts[pts.Count-1]].UnNormalizePoint(FromFixedPrecision(normalizedPoints[pts[pts.Count-1]])), 0.05f, Color.black);
|
|
|
|
lastSuccessfulStart = outputPath[outputPath.Count-2];
|
|
lastSuccessfulEnd = outputPath[outputPath.Count-1];
|
|
|
|
outputPath.RemoveAt(outputPath.Count-1);
|
|
if (outputPath.Count > 1) {
|
|
last = spPrev;
|
|
spPrev = outputPath[outputPath.Count-2];
|
|
turn = Side2D(points[spPrev], points[last], points[i]);
|
|
} else {
|
|
// TODO: Should be separate value
|
|
turn = Side.Colinear;
|
|
}
|
|
i--;
|
|
marker2.End();
|
|
continue;
|
|
}
|
|
}
|
|
marker2.End();
|
|
}
|
|
}
|
|
|
|
Profiler.EndSample();
|
|
|
|
var result = ListPool<Vector3>.Claim(outputPath.Count);
|
|
for (int j = 0; j < outputPath.Count; j++) {
|
|
var idx = outputPath[j];
|
|
result.Add(nodes[idx].UnNormalizePoint(FromFixedPrecision(normalizedPoints[idx])));
|
|
}
|
|
|
|
ArrayPool<GridNodeBase>.Release(ref nodes);
|
|
ArrayPool<int2>.Release(ref points);
|
|
ArrayPool<int2>.Release(ref normalizedPoints);
|
|
ArrayPool<uint>.Release(ref costs);
|
|
ListPool<int>.Release(ref outputPath);
|
|
ListPool<GraphNode>.Release(ref trace);
|
|
return result;
|
|
}
|
|
}
|
|
}
|