diff options
Diffstat (limited to 'Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Modifiers')
16 files changed, 2204 insertions, 0 deletions
diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Modifiers/AdvancedSmooth.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Modifiers/AdvancedSmooth.cs new file mode 100644 index 0000000..e807462 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Modifiers/AdvancedSmooth.cs @@ -0,0 +1,561 @@ +using System; +using System.Collections.Generic; +using UnityEngine; + +namespace Pathfinding { + /// <summary> + /// Smoothing by dividing path into turns and straight segments. + /// + /// Deprecated: This modifier is deprecated + /// </summary> + [System.Serializable] + [System.Obsolete("This modifier is deprecated")] + [HelpURL("https://arongranberg.com/astar/documentation/stable/advancedsmooth.html")] + public class AdvancedSmooth : MonoModifier { + public override int Order { get { return 40; } } + + public float turningRadius = 1.0F; + + public MaxTurn turnConstruct1 = new MaxTurn(); + public ConstantTurn turnConstruct2 = new ConstantTurn(); + + public override void Apply (Path p) { + Vector3[] vectorPath = p.vectorPath.ToArray(); + + if (vectorPath.Length <= 2) { + return; + } + + List<Vector3> newPath = new List<Vector3>(); + newPath.Add(vectorPath[0]); + + TurnConstructor.turningRadius = turningRadius; + + for (int i = 1; i < vectorPath.Length-1; i++) { + List<Turn> turnList = new List<Turn>(); + + TurnConstructor.Setup(i, vectorPath); + turnConstruct1.Prepare(i, vectorPath); + turnConstruct2.Prepare(i, vectorPath); + + TurnConstructor.PostPrepare(); + + if (i == 1) { + turnConstruct1.PointToTangent(turnList); + turnConstruct2.PointToTangent(turnList); + } else { + turnConstruct1.TangentToTangent(turnList); + turnConstruct2.TangentToTangent(turnList); + } + + EvaluatePaths(turnList, newPath); + + //Last point + if (i == vectorPath.Length-2) { + turnConstruct1.TangentToPoint(turnList); + turnConstruct2.TangentToPoint(turnList); + } + + EvaluatePaths(turnList, newPath); + } + + newPath.Add(vectorPath[vectorPath.Length-1]); + p.vectorPath = newPath; + } + + void EvaluatePaths (List<Turn> turnList, List<Vector3> output) { + turnList.Sort(); + + for (int j = 0; j < turnList.Count; j++) { + if (j == 0) { + turnList[j].GetPath(output); + } + } + + turnList.Clear(); + + if (TurnConstructor.changedPreviousTangent) { + turnConstruct1.OnTangentUpdate(); + turnConstruct2.OnTangentUpdate(); + } + } + + [System.Serializable] + /// <summary>Type of turn.</summary> + public class MaxTurn : TurnConstructor { + Vector3 preRightCircleCenter = Vector3.zero; + Vector3 preLeftCircleCenter = Vector3.zero; + + Vector3 rightCircleCenter; + Vector3 leftCircleCenter; + + double vaRight, vaLeft, preVaLeft, preVaRight; + + double gammaLeft, gammaRight; + + double betaRightRight, betaRightLeft, betaLeftRight, betaLeftLeft; + + double deltaRightLeft, deltaLeftRight; + + double alfaRightRight, alfaLeftLeft, alfaRightLeft, alfaLeftRight; + + public override void OnTangentUpdate () { + rightCircleCenter = current + normal * turningRadius; + leftCircleCenter = current - normal * turningRadius; + + vaRight = Atan2(current-rightCircleCenter); + vaLeft = vaRight + Math.PI; + } + + public override void Prepare (int i, Vector3[] vectorPath) { + preRightCircleCenter = rightCircleCenter; + preLeftCircleCenter = leftCircleCenter; + + rightCircleCenter = current + normal * turningRadius; + leftCircleCenter = current - normal * turningRadius; + + preVaRight = vaRight; + preVaLeft = vaLeft; + + vaRight = Atan2(current-rightCircleCenter); + vaLeft = vaRight + Math.PI; + } + + public override void TangentToTangent (List<Turn> turnList) { + alfaRightRight = Atan2(rightCircleCenter - preRightCircleCenter); // + Math.PI*0.5; //Angle tangent to the angle the previous circle (from the current circle) + alfaLeftLeft = Atan2(leftCircleCenter - preLeftCircleCenter); // + Math.PI*0.5; + alfaRightLeft = Atan2(leftCircleCenter - preRightCircleCenter); // + Math.PI*0.5; //RightLeft means: from the previous right circle to the current left circle + alfaLeftRight = Atan2(rightCircleCenter - preLeftCircleCenter); // + Math.PI*0.5; //LeftRight means: from the previous left circle to the current right circle + + double magnRightLeft = (leftCircleCenter - preRightCircleCenter).magnitude; + double magnLeftRight = (rightCircleCenter - preLeftCircleCenter).magnitude; + + bool noRightLeft = false; + bool noLeftRight = false; + + //Discard RightLeft and LeftRight paths if the circles lie to close to each other + if (magnRightLeft < turningRadius*2) { + magnRightLeft = turningRadius*2; + noRightLeft = true; + } + + if (magnLeftRight < turningRadius*2) { + magnLeftRight = turningRadius*2; + noLeftRight = true; + } + + deltaRightLeft = noRightLeft ? 0 : (ThreeSixtyRadians * 0.25) - Math.Asin(turningRadius*2 / magnRightLeft); //turn*2 should be r1 + r2 for circles with different radiis + deltaLeftRight = noLeftRight ? 0 : (ThreeSixtyRadians * 0.25) - Math.Asin(turningRadius*2 / magnLeftRight); //turn*2 should be r1 + r2 for circles with different radiis + + + //Length for the first turn + betaRightRight = ClockwiseAngle(preVaRight, alfaRightRight - ThreeSixtyRadians*0.25); // ThreeSixtyRadians * 0.25 = 90 degrees + betaRightLeft = ClockwiseAngle(preVaRight, alfaRightLeft - deltaRightLeft); + betaLeftRight = CounterClockwiseAngle(preVaLeft, alfaLeftRight + deltaLeftRight); + betaLeftLeft = CounterClockwiseAngle(preVaLeft, alfaLeftLeft + ThreeSixtyRadians*0.25); + + + //Add length for the second turn + betaRightRight += ClockwiseAngle(alfaRightRight - ThreeSixtyRadians*0.25, vaRight); + betaRightLeft += CounterClockwiseAngle(alfaRightLeft + deltaRightLeft, vaLeft); + betaLeftRight += ClockwiseAngle(alfaLeftRight - deltaLeftRight, vaRight); + betaLeftLeft += CounterClockwiseAngle(alfaLeftLeft + ThreeSixtyRadians*0.25, vaLeft); + + betaRightRight = GetLengthFromAngle(betaRightRight, turningRadius); + betaRightLeft = GetLengthFromAngle(betaRightLeft, turningRadius); + betaLeftRight = GetLengthFromAngle(betaLeftRight, turningRadius); + betaLeftLeft = GetLengthFromAngle(betaLeftLeft, turningRadius); + + Vector3 + pRightRight1, pRightRight2, + pRightLeft1, pRightLeft2, + pLeftRight1, pLeftRight2, + pLeftLeft1, pLeftLeft2; + + //Debug.Log ("=== DELTA VALUES===\nRightLeft "+ToDegrees (deltaRightLeft)+" - LeftRight "+ToDegrees (deltaLeftRight)); + //Set up points where the straigh segments starts and ends (between the turns) + pRightRight1 = AngleToVector(alfaRightRight - ThreeSixtyRadians*0.25)*turningRadius + preRightCircleCenter; + pRightLeft1 = AngleToVector(alfaRightLeft - deltaRightLeft)*turningRadius + preRightCircleCenter; + pLeftRight1 = AngleToVector(alfaLeftRight + deltaLeftRight)*turningRadius + preLeftCircleCenter; + pLeftLeft1 = AngleToVector(alfaLeftLeft + ThreeSixtyRadians*0.25)*turningRadius + preLeftCircleCenter; + + pRightRight2 = AngleToVector(alfaRightRight - ThreeSixtyRadians*0.25)*turningRadius + rightCircleCenter; + pRightLeft2 = AngleToVector(alfaRightLeft - deltaRightLeft + Math.PI)*turningRadius + leftCircleCenter; + pLeftRight2 = AngleToVector(alfaLeftRight + deltaLeftRight + Math.PI)*turningRadius + rightCircleCenter; + pLeftLeft2 = AngleToVector(alfaLeftLeft + ThreeSixtyRadians*0.25)*turningRadius + leftCircleCenter; + betaRightRight += (pRightRight1 - pRightRight2).magnitude; + betaRightLeft += (pRightLeft1 - pRightLeft2).magnitude; + betaLeftRight += (pLeftRight1 - pLeftRight2).magnitude; + betaLeftLeft += (pLeftLeft1 - pLeftLeft2).magnitude; + + if (noRightLeft) { + betaRightLeft += 10000000; + } + if (noLeftRight) { + betaLeftRight += 10000000; + } + + turnList.Add(new Turn((float)betaRightRight, this, 2)); + turnList.Add(new Turn((float)betaRightLeft, this, 3)); + turnList.Add(new Turn((float)betaLeftRight, this, 4)); + turnList.Add(new Turn((float)betaLeftLeft, this, 5)); + } + + public override void PointToTangent (List<Turn> turnList) { + bool noRight = false, noLeft = false; + + float rightMagn = (prev-rightCircleCenter).magnitude; + float leftMagn = (prev-leftCircleCenter).magnitude; + + if (rightMagn < turningRadius) + noRight = true; + if (leftMagn < turningRadius) + noLeft = true; + + double alfa = noRight ? 0 : Atan2(prev-rightCircleCenter); + double delta = noRight ? 0 : (ThreeSixtyRadians * 0.25) - Math.Asin(turningRadius / (prev-rightCircleCenter).magnitude); + + //Angle to the point where turning ends on the right circle + gammaRight = alfa + delta; + + double betaRight = noRight ? 0 : ClockwiseAngle(gammaRight, vaRight); + + double alfaLeft = noLeft ? 0 : Atan2(prev-leftCircleCenter); + double deltaLeft = noLeft ? 0 : (ThreeSixtyRadians * 0.25) - Math.Asin(turningRadius / (prev-leftCircleCenter).magnitude); + + //Angle to the point where turning ends + gammaLeft = alfaLeft - deltaLeft; + + double betaLeft = noLeft ? 0 : CounterClockwiseAngle(gammaLeft, vaLeft); + + if (!noRight) + turnList.Add(new Turn((float)betaRight, this, 0)); + if (!noLeft) + turnList.Add(new Turn((float)betaLeft, this, 1)); + } + + public override void TangentToPoint (List<Turn> turnList) { + bool noRight = false, noLeft = false; + + float rightMagn = (next-rightCircleCenter).magnitude; + float leftMagn = (next-leftCircleCenter).magnitude; + + if (rightMagn < turningRadius) + noRight = true; + if (leftMagn < turningRadius) + noLeft = true; + + if (!noRight) { + double alfa = Atan2(next-rightCircleCenter); + double delta = (ThreeSixtyRadians * 0.25) - Math.Asin(turningRadius / rightMagn); + + //Angle to the point where turning ends on the right circle + gammaRight = alfa - delta; + double betaRight = ClockwiseAngle(vaRight, gammaRight); + + turnList.Add(new Turn((float)betaRight, this, 6)); + } + + if (!noLeft) { + double alfaLeft = Atan2(next-leftCircleCenter); + double deltaLeft = (ThreeSixtyRadians * 0.25) - Math.Asin(turningRadius / leftMagn); + + //Angle to the point where turning ends + gammaLeft = alfaLeft + deltaLeft; + + double betaLeft = CounterClockwiseAngle(vaLeft, gammaLeft); + + + turnList.Add(new Turn((float)betaLeft, this, 7)); + } + } + + public override void GetPath (Turn turn, List<Vector3> output) { + switch (turn.id) { + case 0: + //Right - Point to tangent + AddCircleSegment(gammaRight, vaRight, true, rightCircleCenter, output, turningRadius); + break; + case 1: + //Left - Point to tangent + AddCircleSegment(gammaLeft, vaLeft, false, leftCircleCenter, output, turningRadius); + break; + case 2: + //Right Right - Tangent to tangent + AddCircleSegment(preVaRight, alfaRightRight - ThreeSixtyRadians*0.25, true, preRightCircleCenter, output, turningRadius); + AddCircleSegment(alfaRightRight - ThreeSixtyRadians*0.25, vaRight, true, rightCircleCenter, output, turningRadius); + break; + case 3: + //Right Left - Tangent to tangent + AddCircleSegment(preVaRight, alfaRightLeft - deltaRightLeft, true, preRightCircleCenter, output, turningRadius); + AddCircleSegment(alfaRightLeft - deltaRightLeft + Math.PI, vaLeft, false, leftCircleCenter, output, turningRadius); + break; + case 4: + //Left Right - Tangent to tangent + AddCircleSegment(preVaLeft, alfaLeftRight + deltaLeftRight, false, preLeftCircleCenter, output, turningRadius); + AddCircleSegment(alfaLeftRight + deltaLeftRight + Math.PI, vaRight, true, rightCircleCenter, output, turningRadius); + break; + case 5: + //Left Left - Tangent to tangent + AddCircleSegment(preVaLeft, alfaLeftLeft + ThreeSixtyRadians*0.25, false, preLeftCircleCenter, output, turningRadius); + AddCircleSegment(alfaLeftLeft + ThreeSixtyRadians*0.25, vaLeft, false, leftCircleCenter, output, turningRadius); + break; + case 6: + //Right - Tangent to point + AddCircleSegment(vaRight, gammaRight, true, rightCircleCenter, output, turningRadius); + break; + case 7: + //Left - Tangent to point + AddCircleSegment(vaLeft, gammaLeft, false, leftCircleCenter, output, turningRadius); + break; + } + } + } + + [System.Serializable] + /// <summary>Constant turning speed.</summary> + public class ConstantTurn : TurnConstructor { + Vector3 circleCenter; + double gamma1; + double gamma2; + + bool clockwise; + + public override void Prepare (int i, Vector3[] vectorPath) {} + + public override void TangentToTangent (List<Turn> turnList) { + Vector3 preNormal = Vector3.Cross(t1, Vector3.up); + + Vector3 dir = (current-prev); + Vector3 pos = dir*0.5F + prev; + + dir = Vector3.Cross(dir, Vector3.up); + + bool didIntersect; + circleCenter = VectorMath.LineDirIntersectionPointXZ(prev, preNormal, pos, dir, out didIntersect); + + if (!didIntersect) { + return; + } + + gamma1 = Atan2(prev-circleCenter); + gamma2 = Atan2(current-circleCenter); + + clockwise = !VectorMath.RightOrColinearXZ(circleCenter, prev, prev+t1); + + double beta = clockwise ? ClockwiseAngle(gamma1, gamma2) : CounterClockwiseAngle(gamma1, gamma2); + + beta = GetLengthFromAngle(beta, (circleCenter - current).magnitude); + + turnList.Add(new Turn((float)beta, this, 0)); + } + + public override void GetPath (Turn turn, List<Vector3> output) { + AddCircleSegment(gamma1, gamma2, clockwise, circleCenter, output, (circleCenter - current).magnitude); + + normal = (current - circleCenter).normalized; + t2 = Vector3.Cross(normal, Vector3.up).normalized; + normal = -normal; + + if (!clockwise) { + t2 = -t2; + normal = -normal; + } + + changedPreviousTangent = true; + } + } + + /// <summary>Abstract turn constructor.</summary> + public abstract class TurnConstructor { + /// <summary> + /// Constant bias to add to the path lengths. + /// This can be used to favor certain turn types before others. + /// By for example setting this to -5, paths from this path constructor will be chosen + /// if there are no other paths more than 5 world units shorter than this one (as opposed to just any shorter path) + /// </summary> + public float constantBias = 0; + + /// <summary> + /// Bias to multiply the path lengths with. This can be used to favor certain turn types before others. + /// See: <see cref="constantBias"/> + /// </summary> + public float factorBias = 1; + + public static float turningRadius = 1.0F; + + public const double ThreeSixtyRadians = Math.PI * 2; + + public static Vector3 prev, current, next; //The current points + public static Vector3 t1, t2; //The current tangents - t2 is at 'current', t1 is at 'prev' + public static Vector3 normal, prevNormal; //Normal at 'current' + + public static bool changedPreviousTangent = false; + + public abstract void Prepare(int i, Vector3[] vectorPath); + public virtual void OnTangentUpdate () {} + public virtual void PointToTangent (List<Turn> turnList) {} + public virtual void TangentToPoint (List<Turn> turnList) {} + public virtual void TangentToTangent (List<Turn> turnList) {} + public abstract void GetPath(Turn turn, List<Vector3> output); + //abstract void Evaluate (Turn turn); + + public static void Setup (int i, Vector3[] vectorPath) { + current = vectorPath[i]; + prev = vectorPath[i-1]; + next = vectorPath[i+1]; + + prev.y = current.y; + next.y = current.y; + + t1 = t2; + + t2 = (next-current).normalized - (prev-current).normalized; + t2 = t2.normalized; + + prevNormal = normal; + + normal = Vector3.Cross(t2, Vector3.up); + normal = normal.normalized; + } + + public static void PostPrepare () { + changedPreviousTangent = false; + } + + //Utilities + + public void AddCircleSegment (double startAngle, double endAngle, bool clockwise, Vector3 center, List<Vector3> output, float radius) { + double step = ThreeSixtyRadians / 100; + + if (clockwise) { + while (endAngle > startAngle+ThreeSixtyRadians) { + endAngle -= ThreeSixtyRadians; + } + + while (endAngle < startAngle) { + endAngle += ThreeSixtyRadians; + } + } else { + while (endAngle < startAngle-ThreeSixtyRadians) { + endAngle += ThreeSixtyRadians; + } + + while (endAngle > startAngle) { + endAngle -= ThreeSixtyRadians; + } + } + + //Add curve + if (clockwise) { + for (double i = startAngle; i < endAngle; i += step) { + output.Add(AngleToVector(i)*radius+center); + } + } else { + for (double i = startAngle; i > endAngle; i -= step) { + output.Add(AngleToVector(i)*radius+center); + } + } + + //Add last point + output.Add(AngleToVector(endAngle)*radius+center); + } + + public void DebugCircleSegment (Vector3 center, double startAngle, double endAngle, double radius, Color color) { + double step = ThreeSixtyRadians / 100; + + while (endAngle < startAngle) { + endAngle += ThreeSixtyRadians; + } + + Vector3 prev = AngleToVector(startAngle)*(float)radius+center; + for (double i = startAngle+step; i < endAngle; i += step) { + Debug.DrawLine(prev, AngleToVector(i)*(float)radius+center); + } + + Debug.DrawLine(prev, AngleToVector(endAngle)*(float)radius+center); + } + + public void DebugCircle (Vector3 center, double radius, Color color) { + double step = ThreeSixtyRadians / 100; + Vector3 prePos = AngleToVector(-step)*(float)radius+center; + + for (double i = 0; i < ThreeSixtyRadians; i += step) { + Vector3 pos = AngleToVector(i)*(float)radius+center; + Debug.DrawLine(prePos, pos, color); + prePos = pos; + } + } + + /// <summary>Returns the length of an circular arc with a radius and angle. Angle is specified in radians</summary> + public double GetLengthFromAngle (double angle, double radius) { + return radius * angle; + } + + /// <summary>Returns the angle between from and to in a clockwise direction</summary> + public double ClockwiseAngle (double from, double to) { + return ClampAngle(to - from); + } + + /// <summary>Returns the angle between from and to in a counter-clockwise direction</summary> + public double CounterClockwiseAngle (double from, double to) { + return ClampAngle(from - to); + } + + public Vector3 AngleToVector (double a) { + return new Vector3((float)Math.Cos(a), 0, (float)Math.Sin(a)); + } + + public double ToDegrees (double rad) { + return rad * Mathf.Rad2Deg; + } + + public double ClampAngle (double a) { + while (a < 0) { a += ThreeSixtyRadians; } + while (a > ThreeSixtyRadians) { a -= ThreeSixtyRadians; } + return a; + } + + public double Atan2 (Vector3 v) { + return Math.Atan2(v.z, v.x); + } + } + + //Turn class + /// <summary>Represents a turn in a path.</summary> + public struct Turn : IComparable<Turn> { + public float length; + public int id; + + public TurnConstructor constructor; + + public float score { + get { + return length*constructor.factorBias+constructor.constantBias; + } + } + + public Turn (float length, TurnConstructor constructor, int id = 0) { + this.length = length; + this.id = id; + this.constructor = constructor; + } + + public void GetPath (List<Vector3> output) { + constructor.GetPath(this, output); + } + + public int CompareTo (Turn t) { + return t.score > score ? -1 : (t.score < score ? 1 : 0); + } + + public static bool operator < (Turn lhs, Turn rhs) { + return lhs.score < rhs.score; + } + + public static bool operator > (Turn lhs, Turn rhs) { + return lhs.score > rhs.score; + } + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Modifiers/AdvancedSmooth.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Modifiers/AdvancedSmooth.cs.meta new file mode 100644 index 0000000..3a109d4 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Modifiers/AdvancedSmooth.cs.meta @@ -0,0 +1,7 @@ +fileFormatVersion: 2 +guid: a74058756f0df4e668a0e2226e7cfcea +MonoImporter: + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Modifiers/AlternativePath.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Modifiers/AlternativePath.cs new file mode 100644 index 0000000..16c9237 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Modifiers/AlternativePath.cs @@ -0,0 +1,94 @@ +using UnityEngine; +using System.Collections.Generic; + +namespace Pathfinding { + [AddComponentMenu("Pathfinding/Modifiers/Alternative Path Modifier")] + [System.Serializable] + /// <summary> + /// Applies penalty to the paths it processes telling other units to avoid choosing the same path. + /// + /// Note that this might not work properly if penalties are modified by other actions as well (e.g graph update objects which reset the penalty to zero). + /// It will only work when all penalty modifications are relative, i.e adding or subtracting penalties, but not when setting penalties + /// to specific values. + /// + /// When destroyed, it will correctly remove any added penalty. + /// </summary> + [HelpURL("https://arongranberg.com/astar/documentation/stable/alternativepath.html")] + public class AlternativePath : MonoModifier { +#if UNITY_EDITOR + [UnityEditor.MenuItem("CONTEXT/Seeker/Add Alternative Path Modifier")] + public static void AddComp (UnityEditor.MenuCommand command) { + (command.context as Component).gameObject.AddComponent(typeof(AlternativePath)); + } +#endif + + public override int Order { get { return 10; } } + + /// <summary>How much penalty (weight) to apply to nodes</summary> + public int penalty = 1000; + + /// <summary>Max number of nodes to skip in a row</summary> + public int randomStep = 10; + + /// <summary>The previous path</summary> + List<GraphNode> prevNodes = new List<GraphNode>(); + + /// <summary>The previous penalty used. Stored just in case it changes during operation</summary> + int prevPenalty; + + /// <summary>A random object</summary> + readonly System.Random rnd = new System.Random(); + + bool destroyed; + + public override void Apply (Path p) { + if (this == null) return; + + ApplyNow(p.path); + } + + protected void OnDestroy () { + destroyed = true; + ClearOnDestroy(); + } + + void ClearOnDestroy () { + InversePrevious(); + } + + void InversePrevious () { + // Remove previous penalty + if (prevNodes != null) { + bool warnPenalties = false; + for (int i = 0; i < prevNodes.Count; i++) { + if (prevNodes[i].Penalty < prevPenalty) { + warnPenalties = true; + prevNodes[i].Penalty = 0; + } else { + prevNodes[i].Penalty = (uint)(prevNodes[i].Penalty-prevPenalty); + } + } + if (warnPenalties) { + Debug.LogWarning("Penalty for some nodes has been reset while the AlternativePath modifier was active (possibly because of a graph update). Some penalties might be incorrect (they may be lower than expected for the affected nodes)"); + } + } + } + + void ApplyNow (List<GraphNode> nodes) { + InversePrevious(); + prevNodes.Clear(); + + if (destroyed) return; + + if (nodes != null) { + int rndStart = rnd.Next(randomStep); + for (int i = rndStart; i < nodes.Count; i += rnd.Next(1, randomStep)) { + nodes[i].Penalty = (uint)(nodes[i].Penalty+penalty); + prevNodes.Add(nodes[i]); + } + } + + prevPenalty = penalty; + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Modifiers/AlternativePath.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Modifiers/AlternativePath.cs.meta new file mode 100644 index 0000000..3bb120a --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Modifiers/AlternativePath.cs.meta @@ -0,0 +1,7 @@ +fileFormatVersion: 2 +guid: a537d958aa5bb4f35b0d7b40d72278f2 +MonoImporter: + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Modifiers/FunnelModifier.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Modifiers/FunnelModifier.cs new file mode 100644 index 0000000..7616d81 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Modifiers/FunnelModifier.cs @@ -0,0 +1,142 @@ +using UnityEngine; +using System.Collections.Generic; +using Pathfinding.Util; + +namespace Pathfinding { + [AddComponentMenu("Pathfinding/Modifiers/Funnel Modifier")] + [System.Serializable] + /// <summary> + /// Simplifies paths on navmesh graphs using the funnel algorithm. + /// The funnel algorithm is an algorithm which can, given a path corridor with nodes in the path where the nodes have an area, like triangles, it can find the shortest path inside it. + /// This makes paths on navmeshes look much cleaner and smoother. + /// [Open online documentation to see images] + /// + /// The funnel modifier also works on grid graphs using a different algorithm, but which yields visually similar results. + /// See: <see cref="GridStringPulling"/> + /// + /// Note: The <see cref="Pathfinding.RichAI"/> movement script has its own internal funnel modifier. + /// You do not need to attach this component if you are using the RichAI movement script. + /// + /// See: http://digestingduck.blogspot.se/2010/03/simple-stupid-funnel-algorithm.html + /// </summary> + [HelpURL("https://arongranberg.com/astar/documentation/stable/funnelmodifier.html")] + public class FunnelModifier : MonoModifier { + /// <summary> + /// Determines if funnel simplification is used. + /// When using the low quality setting only the funnel algorithm is used + /// but when the high quality setting an additional step is done to simplify the path even more. + /// + /// On tiled recast/navmesh graphs, but sometimes on normal ones as well, it can be good to simplify + /// the funnel as a post-processing step to make the paths straighter. + /// + /// This has a moderate performance impact during frames when a path calculation is completed. + /// This is why it is disabled by default. For any units that you want high + /// quality movement for you should enable it. + /// + /// [Open online documentation to see images] + /// + /// See: <see cref="Funnel.Simplify"/> + /// + /// Note: This is only used for recast/navmesh graphs. Not for grid graphs. + /// </summary> + public FunnelQuality quality = FunnelQuality.Medium; + + /// <summary> + /// Insert a vertex every time the path crosses a portal instead of only at the corners of the path. + /// The resulting path will have exactly one vertex per portal if this is enabled. + /// This may introduce vertices with the same position in the output (esp. in corners where many portals meet). + /// [Open online documentation to see images] + /// + /// Note: This is only used for recast/navmesh graphs. Not for grid graphs. + /// </summary> + public bool splitAtEveryPortal; + + /// <summary> + /// When using a grid graph, take penalties, tag penalties and <see cref="ITraversalProvider"/> penalties into account. + /// Enabling this is quite slow. It can easily make the modifier take twice the amount of time to run. + /// So unless you are using penalties/tags/ITraversalProvider penalties that you need to take into account when simplifying + /// the path, you should leave this disabled. + /// </summary> + public bool accountForGridPenalties = false; + + public enum FunnelQuality { + Medium, + High, + } + +#if UNITY_EDITOR + [UnityEditor.MenuItem("CONTEXT/Seeker/Add Funnel Modifier")] + public static void AddComp (UnityEditor.MenuCommand command) { + (command.context as Component).gameObject.AddComponent(typeof(FunnelModifier)); + } +#endif + + public override int Order { get { return 10; } } + + public override void Apply (Path p) { + if (p.path == null || p.path.Count == 0 || p.vectorPath == null || p.vectorPath.Count == 0) { + return; + } + + List<Vector3> funnelPath = ListPool<Vector3>.Claim(); + + // Split the path into different parts (separated by custom links) + // and run the funnel algorithm on each of them in turn + var parts = Funnel.SplitIntoParts(p); + + if (parts.Count == 0) { + // As a really special case, it might happen that the path contained only a single node + // and that node was part of a custom link (e.g added by the NodeLink2 component). + // In that case the SplitIntoParts method will not know what to do with it because it is + // neither a link (as only 1 of the 2 nodes of the link was part of the path) nor a normal + // path part. So it will skip it. This will cause it to return an empty list. + // In that case we want to simply keep the original path, which is just a single point. + return; + } + + if (quality == FunnelQuality.High) Funnel.Simplify(parts, ref p.path); + + for (int i = 0; i < parts.Count; i++) { + var part = parts[i]; + if (part.type == Funnel.PartType.NodeSequence) { + // If this is a grid graph (and not a hexagonal graph) then we can use a special + // string pulling algorithm for grid graphs which works a lot better. + if (p.path[part.startIndex].Graph is GridGraph gg && gg.neighbours != NumNeighbours.Six) { + // TODO: Avoid dynamic allocations + System.Func<GraphNode, uint> traversalCost = null; + if (accountForGridPenalties) { + traversalCost = p.GetTraversalCost; + } + System.Func<GraphNode, bool> filter = p.CanTraverse; + var result = GridStringPulling.Calculate(p.path, part.startIndex, part.endIndex, part.startPoint, part.endPoint, traversalCost, filter, int.MaxValue); + funnelPath.AddRange(result); + ListPool<Vector3>.Release(ref result); + } else { + var portals = Funnel.ConstructFunnelPortals(p.path, part); + var result = Funnel.Calculate(portals, splitAtEveryPortal); + funnelPath.AddRange(result); + ListPool<Vector3>.Release(ref portals.left); + ListPool<Vector3>.Release(ref portals.right); + ListPool<Vector3>.Release(ref result); + } + } else { + // non-link parts will add the start/end points for the adjacent parts. + // So if there is no non-link part before this one, then we need to add the start point of the link + // and if there is no non-link part after this one, then we need to add the end point. + if (i == 0 || parts[i-1].type == Funnel.PartType.OffMeshLink) { + funnelPath.Add(part.startPoint); + } + if (i == parts.Count - 1 || parts[i+1].type == Funnel.PartType.OffMeshLink) { + funnelPath.Add(part.endPoint); + } + } + } + + UnityEngine.Assertions.Assert.IsTrue(funnelPath.Count >= 1); + ListPool<Funnel.PathPart>.Release(ref parts); + // Pool the previous vectorPath + ListPool<Vector3>.Release(ref p.vectorPath); + p.vectorPath = funnelPath; + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Modifiers/FunnelModifier.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Modifiers/FunnelModifier.cs.meta new file mode 100644 index 0000000..a25184a --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Modifiers/FunnelModifier.cs.meta @@ -0,0 +1,7 @@ +fileFormatVersion: 2 +guid: 77f586f285b3847808d79083bd19ef1f +MonoImporter: + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Modifiers/Modifiers.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Modifiers/Modifiers.cs new file mode 100644 index 0000000..23295f1 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Modifiers/Modifiers.cs @@ -0,0 +1,89 @@ +using UnityEngine; + +namespace Pathfinding { + /// <summary> + /// Base for all path modifiers. + /// See: MonoModifier + /// Modifier + /// </summary> + public interface IPathModifier { + int Order { get; } + + void Apply(Path path); + void PreProcess(Path path); + } + + /// <summary> + /// Base class for path modifiers which are not attached to GameObjects. + /// See: MonoModifier + /// </summary> + [System.Serializable] + public abstract class PathModifier : IPathModifier { + [System.NonSerialized] + public Seeker seeker; + + /// <summary> + /// Modifiers will be executed from lower order to higher order. + /// This value is assumed to stay constant. + /// </summary> + public abstract int Order { get; } + + public void Awake (Seeker seeker) { + this.seeker = seeker; + if (seeker != null) { + seeker.RegisterModifier(this); + } + } + + public void OnDestroy (Seeker seeker) { + if (seeker != null) { + seeker.DeregisterModifier(this); + } + } + + public virtual void PreProcess (Path path) { + // Required by IPathModifier + } + + /// <summary>Main Post-Processing function</summary> + public abstract void Apply(Path path); + } + + /// <summary> + /// Base class for path modifiers which can be attached to GameObjects. + /// See: Menubar -> Component -> Pathfinding -> Modifiers + /// </summary> + [System.Serializable] + public abstract class MonoModifier : VersionedMonoBehaviour, IPathModifier { + [System.NonSerialized] + public Seeker seeker; + + /// <summary>Alerts the Seeker that this modifier exists</summary> + protected virtual void OnEnable () { + seeker = GetComponent<Seeker>(); + + if (seeker != null) { + seeker.RegisterModifier(this); + } + } + + protected virtual void OnDisable () { + if (seeker != null) { + seeker.DeregisterModifier(this); + } + } + + /// <summary> + /// Modifiers will be executed from lower order to higher order. + /// This value is assumed to stay constant. + /// </summary> + public abstract int Order { get; } + + public virtual void PreProcess (Path path) { + // Required by IPathModifier + } + + /// <summary>Called for each path that the Seeker calculates after the calculation has finished</summary> + public abstract void Apply(Path path); + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Modifiers/Modifiers.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Modifiers/Modifiers.cs.meta new file mode 100644 index 0000000..8f9e22e --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Modifiers/Modifiers.cs.meta @@ -0,0 +1,7 @@ +fileFormatVersion: 2 +guid: ce1f812d122764ce7a45f2dd03915e46 +MonoImporter: + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Modifiers/RadiusModifier.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Modifiers/RadiusModifier.cs new file mode 100644 index 0000000..6730f65 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Modifiers/RadiusModifier.cs @@ -0,0 +1,283 @@ +using UnityEngine; +using System; +using System.Collections.Generic; + +namespace Pathfinding { + /// <summary> + /// Radius path modifier for offsetting paths. + /// + /// The radius modifier will offset the path to create the effect + /// of adjusting it to the characters radius. + /// It gives good results on navmeshes which have not been offset with the + /// character radius during scan. Especially useful when characters with different + /// radiuses are used on the same navmesh. It is also useful when using + /// rvo local avoidance with the RVONavmesh since the RVONavmesh assumes the + /// navmesh has not been offset with the character radius. + /// + /// This modifier assumes all paths are in the XZ plane (i.e Y axis is up). + /// + /// It is recommended to use the Funnel Modifier on the path as well. + /// + /// [Open online documentation to see images] + /// + /// See: RVONavmesh + /// See: modifiers + /// + /// Also check out the howto page "Using Modifiers". + /// + /// Since: Added in 3.2.6 + /// </summary> + [AddComponentMenu("Pathfinding/Modifiers/Radius Offset Modifier")] + [HelpURL("https://arongranberg.com/astar/documentation/stable/radiusmodifier.html")] + public class RadiusModifier : MonoModifier { +#if UNITY_EDITOR + [UnityEditor.MenuItem("CONTEXT/Seeker/Add Radius Modifier")] + public static void AddComp (UnityEditor.MenuCommand command) { + (command.context as Component).gameObject.AddComponent(typeof(RadiusModifier)); + } +#endif + + public override int Order { get { return 41; } } + + /// <summary> + /// Radius of the circle segments generated. + /// Usually similar to the character radius. + /// </summary> + public float radius = 1f; + + /// <summary> + /// Detail of generated circle segments. + /// Measured as steps per full circle. + /// + /// It is more performant to use a low value. + /// For movement, using a high value will barely improve path quality. + /// </summary> + public float detail = 10; + + /// <summary> + /// Calculates inner tangents for a pair of circles. + /// + /// Add a to sigma to get the first tangent angle, subtract a from sigma to get the second tangent angle. + /// + /// Returns: True on success. False when the circles are overlapping. + /// </summary> + /// <param name="p1">Position of first circle</param> + /// <param name="p2">Position of the second circle</param> + /// <param name="r1">Radius of the first circle</param> + /// <param name="r2">Radius of the second circle</param> + /// <param name="a">Angle from the line joining the centers of the circles to the inner tangents.</param> + /// <param name="sigma">World angle from p1 to p2 (in XZ space)</param> + bool CalculateCircleInner (Vector3 p1, Vector3 p2, float r1, float r2, out float a, out float sigma) { + float dist = (p1-p2).magnitude; + + if (r1+r2 > dist) { + a = 0; + sigma = 0; + return false; + } + + a = (float)Math.Acos((r1+r2)/dist); + + sigma = (float)Math.Atan2(p2.z-p1.z, p2.x-p1.x); + return true; + } + + /// <summary> + /// Calculates outer tangents for a pair of circles. + /// + /// Add a to sigma to get the first tangent angle, subtract a from sigma to get the second tangent angle. + /// + /// Returns: True on success. False on failure (more specifically when |r1-r2| > |p1-p2| ) + /// </summary> + /// <param name="p1">Position of first circle</param> + /// <param name="p2">Position of the second circle</param> + /// <param name="r1">Radius of the first circle</param> + /// <param name="r2">Radius of the second circle</param> + /// <param name="a">Angle from the line joining the centers of the circles to the inner tangents.</param> + /// <param name="sigma">World angle from p1 to p2 (in XZ space)</param> + bool CalculateCircleOuter (Vector3 p1, Vector3 p2, float r1, float r2, out float a, out float sigma) { + float dist = (p1-p2).magnitude; + + if (Math.Abs(r1 - r2) > dist) { + a = 0; + sigma = 0; + return false; + } + a = (float)Math.Acos((r1-r2)/dist); + sigma = (float)Math.Atan2(p2.z-p1.z, p2.x-p1.x); + return true; + } + + [System.Flags] + enum TangentType { + OuterRight = 1 << 0, + InnerRightLeft = 1 << 1, + InnerLeftRight = 1 << 2, + OuterLeft = 1 << 3, + Outer = OuterRight | OuterLeft, + Inner = InnerRightLeft | InnerLeftRight + } + + TangentType CalculateTangentType (Vector3 p1, Vector3 p2, Vector3 p3, Vector3 p4) { + bool l1 = VectorMath.RightOrColinearXZ(p1, p2, p3); + bool l2 = VectorMath.RightOrColinearXZ(p2, p3, p4); + + return (TangentType)(1 << ((l1 ? 2 : 0) + (l2 ? 1 : 0))); + } + + TangentType CalculateTangentTypeSimple (Vector3 p1, Vector3 p2, Vector3 p3) { + bool l2 = VectorMath.RightOrColinearXZ(p1, p2, p3); + bool l1 = l2; + + return (TangentType)(1 << ((l1 ? 2 : 0) + (l2 ? 1 : 0))); + } + + public override void Apply (Path p) { + List<Vector3> vs = p.vectorPath; + + List<Vector3> res = Apply(vs); + + if (res != vs) { + Pathfinding.Util.ListPool<Vector3>.Release(ref p.vectorPath); + p.vectorPath = res; + } + } + + float[] radi = new float[10]; + float[] a1 = new float[10]; + float[] a2 = new float[10]; + bool[] dir = new bool[10]; + + /// <summary>Apply this modifier on a raw Vector3 list</summary> + public List<Vector3> Apply (List<Vector3> vs) { + if (vs == null || vs.Count < 3) return vs; + + /// <summary>TODO: Do something about these allocations</summary> + if (radi.Length < vs.Count) { + radi = new float[vs.Count]; + a1 = new float[vs.Count]; + a2 = new float[vs.Count]; + dir = new bool[vs.Count]; + } + + for (int i = 0; i < vs.Count; i++) { + radi[i] = radius; + } + + radi[0] = 0; + radi[vs.Count-1] = 0; + + int count = 0; + for (int i = 0; i < vs.Count-1; i++) { + count++; + if (count > 2*vs.Count) { + Debug.LogWarning("Could not resolve radiuses, the path is too complex. Try reducing the base radius"); + break; + } + + TangentType tt; + + if (i == 0) { + tt = CalculateTangentTypeSimple(vs[i], vs[i+1], vs[i+2]); + } else if (i == vs.Count-2) { + tt = CalculateTangentTypeSimple(vs[i-1], vs[i], vs[i+1]); + } else { + tt = CalculateTangentType(vs[i-1], vs[i], vs[i+1], vs[i+2]); + } + + //DrawCircle (vs[i], radi[i], Color.yellow); + + if ((tt & TangentType.Inner) != 0) { + //Angle to tangent + float a; + //Angle to other circle + float sigma; + + //Calculate angles to the next circle and angles for the inner tangents + if (!CalculateCircleInner(vs[i], vs[i+1], radi[i], radi[i+1], out a, out sigma)) { + //Failed, try modifying radiuses + float magn = (vs[i+1]-vs[i]).magnitude; + radi[i] = magn*(radi[i]/(radi[i]+radi[i+1])); + radi[i+1] = magn - radi[i]; + radi[i] *= 0.99f; + radi[i+1] *= 0.99f; + i -= 2; + continue; + } + + if (tt == TangentType.InnerRightLeft) { + a2[i] = sigma-a; + a1[i+1] = sigma-a + (float)Math.PI; + dir[i] = true; + } else { + a2[i] = sigma+a; + a1[i+1] = sigma+a + (float)Math.PI; + dir[i] = false; + } + } else { + float sigma; + float a; + + //Calculate angles to the next circle and angles for the outer tangents + if (!CalculateCircleOuter(vs[i], vs[i+1], radi[i], radi[i+1], out a, out sigma)) { + //Failed, try modifying radiuses + if (i == vs.Count-2) { + //The last circle has a fixed radius at 0, don't modify it + radi[i] = (vs[i+1]-vs[i]).magnitude; + radi[i] *= 0.99f; + i -= 1; + } else { + if (radi[i] > radi[i+1]) { + radi[i+1] = radi[i] - (vs[i+1]-vs[i]).magnitude; + } else { + radi[i+1] = radi[i] + (vs[i+1]-vs[i]).magnitude; + } + radi[i+1] *= 0.99f; + } + + + + i -= 1; + continue; + } + + if (tt == TangentType.OuterRight) { + a2[i] = sigma-a; + a1[i+1] = sigma-a; + dir[i] = true; + } else { + a2[i] = sigma+a; + a1[i+1] = sigma+a; + dir[i] = false; + } + } + } + + List<Vector3> res = Pathfinding.Util.ListPool<Vector3>.Claim(); + res.Add(vs[0]); + if (detail < 1) detail = 1; + float step = (float)(2*Math.PI)/detail; + for (int i = 1; i < vs.Count-1; i++) { + float start = a1[i]; + float end = a2[i]; + float rad = radi[i]; + + if (dir[i]) { + if (end < start) end += (float)Math.PI*2; + for (float t = start; t < end; t += step) { + res.Add(new Vector3((float)Math.Cos(t), 0, (float)Math.Sin(t))*rad + vs[i]); + } + } else { + if (start < end) start += (float)Math.PI*2; + for (float t = start; t > end; t -= step) { + res.Add(new Vector3((float)Math.Cos(t), 0, (float)Math.Sin(t))*rad + vs[i]); + } + } + } + + res.Add(vs[vs.Count-1]); + + return res; + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Modifiers/RadiusModifier.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Modifiers/RadiusModifier.cs.meta new file mode 100644 index 0000000..6a921c0 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Modifiers/RadiusModifier.cs.meta @@ -0,0 +1,7 @@ +fileFormatVersion: 2 +guid: 64ffa084e9f3d48b59e691352d40eb68 +MonoImporter: + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Modifiers/RaycastModifier.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Modifiers/RaycastModifier.cs new file mode 100644 index 0000000..0b92e78 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Modifiers/RaycastModifier.cs @@ -0,0 +1,331 @@ +using UnityEngine; +using System.Collections.Generic; + +namespace Pathfinding { + using Pathfinding.Util; + + /// <summary> + /// Simplifies a path using raycasting. + /// + /// This modifier will try to remove as many nodes as possible from the path using raycasting (linecasting) to validate the node removal. + /// You can use either graph raycasting or Physics.Raycast. + /// When using graph raycasting, the graph will be traversed and checked for obstacles. When physics raycasting is used, the Unity physics system + /// will be asked if there are any colliders which intersect the line that is currently being checked. + /// + /// See: https://docs.unity3d.com/ScriptReference/Physics.html + /// See: <see cref="Pathfinding.IRaycastableGraph"/> + /// + /// This modifier is primarily intended for grid graphs and layered grid graphs. Though depending on your game it may also be + /// useful for point graphs. However note that point graphs do not have any built-in raycasting so you need to use physics raycasting for that graph. + /// + /// For navmesh/recast graphs the <see cref="Pathfinding.FunnelModifier"/> is a much better and faster alternative. + /// + /// On grid graphs you can combine the FunnelModifier with this modifier by simply attaching both of them to a GameObject with a Seeker. + /// This may or may not give you better results. It will usually follow the border of the graph more closely when they are both used + /// however it more often misses some simplification opportunities. + /// When both modifiers are used then the funnel modifier will run first and simplify the path, and then this modifier will take + /// the output from the funnel modifier and try to simplify that even more. + /// + /// This modifier has several different quality levels. The highest quality is significantly slower than the + /// lowest quality level (10 times slower is not unusual). So make sure you pick the lowest quality that your game can get away with. + /// You can use the Unity profiler to see if it takes up any significant amount of time. It will show up under the heading "Running Path Modifiers". + /// + /// [Open online documentation to see images] + /// + /// See: modifiers (view in online documentation for working links) + /// </summary> + [AddComponentMenu("Pathfinding/Modifiers/Raycast Modifier")] + [RequireComponent(typeof(Seeker))] + [System.Serializable] + [HelpURL("https://arongranberg.com/astar/documentation/stable/raycastmodifier.html")] + public class RaycastModifier : MonoModifier { +#if UNITY_EDITOR + [UnityEditor.MenuItem("CONTEXT/Seeker/Add Raycast Simplifier Modifier")] + public static void AddComp (UnityEditor.MenuCommand command) { + (command.context as Component).gameObject.AddComponent(typeof(RaycastModifier)); + } +#endif + + public override int Order { get { return 40; } } + + /// <summary>Use Physics.Raycast to simplify the path</summary> + public bool useRaycasting = false; + + /// <summary> + /// Layer mask used for physics raycasting. + /// All objects with layers that are included in this mask will be treated as obstacles. + /// If you are using a grid graph you usually want this to be the same as the mask in the grid graph's 'Collision Testing' settings. + /// </summary> + public LayerMask mask = -1; + + /// <summary> + /// Checks around the line between two points, not just the exact line. + /// Make sure the ground is either too far below or is not inside the mask since otherwise the raycast might always hit the ground. + /// + /// See: https://docs.unity3d.com/ScriptReference/Physics.SphereCast.html + /// </summary> + [Tooltip("Checks around the line between two points, not just the exact line.\nMake sure the ground is either too far below or is not inside the mask since otherwise the raycast might always hit the ground.")] + public bool thickRaycast; + + /// <summary>Distance from the ray which will be checked for colliders</summary> + [Tooltip("Distance from the ray which will be checked for colliders")] + public float thickRaycastRadius; + + /// <summary> + /// Check for intersections with 2D colliders instead of 3D colliders. + /// Useful for 2D games. + /// + /// See: https://docs.unity3d.com/ScriptReference/Physics2D.html + /// </summary> + [Tooltip("Check for intersections with 2D colliders instead of 3D colliders.")] + public bool use2DPhysics; + + /// <summary> + /// Offset from the original positions to perform the raycast. + /// Can be useful to avoid the raycast intersecting the ground or similar things you do not want to it intersect + /// </summary> + [Tooltip("Offset from the original positions to perform the raycast.\nCan be useful to avoid the raycast intersecting the ground or similar things you do not want to it intersect")] + public Vector3 raycastOffset = Vector3.zero; + + /// <summary>Use raycasting on the graphs. Only currently works with GridGraph and NavmeshGraph and RecastGraph. </summary> + [Tooltip("Use raycasting on the graphs. Only currently works with GridGraph and NavmeshGraph and RecastGraph. This is a pro version feature.")] + public bool useGraphRaycasting = true; + + + /// <summary> + /// Higher quality modes will try harder to find a shorter path. + /// Higher qualities may be significantly slower than low quality. + /// [Open online documentation to see images] + /// </summary> + [Tooltip("When using the high quality mode the script will try harder to find a shorter path. This is significantly slower than the greedy low quality approach.")] + public Quality quality = Quality.Medium; + + public enum Quality { + /// <summary>One iteration using a greedy algorithm</summary> + Low, + /// <summary>Two iterations using a greedy algorithm</summary> + Medium, + /// <summary>One iteration using a dynamic programming algorithm</summary> + High, + /// <summary>Three iterations using a dynamic programming algorithm</summary> + Highest + } + + static readonly int[] iterationsByQuality = new [] { 1, 2, 1, 3 }; + static List<Vector3> buffer = new List<Vector3>(); + static float[] DPCosts = new float[16]; + static int[] DPParents = new int[16]; + + Filter cachedFilter = new Filter(); + + NNConstraint cachedNNConstraint = NNConstraint.None; + + class Filter { + public Path path; + public readonly System.Func<GraphNode, bool> cachedDelegate; + + public Filter() { + cachedDelegate = this.CanTraverse; + } + + bool CanTraverse (GraphNode node) { + return path.CanTraverse(node); + } + } + + public override void Apply (Path p) { + if (!useRaycasting && !useGraphRaycasting) return; + + var points = p.vectorPath; + cachedFilter.path = p; + + // Use the same graph mask as the path. + // We don't want to use the tag mask or other options for this though since then the linecasting will be will confused. + cachedNNConstraint.graphMask = p.nnConstraint.graphMask; + + if (ValidateLine(null, null, p.vectorPath[0], p.vectorPath[p.vectorPath.Count-1], cachedFilter.cachedDelegate, cachedNNConstraint)) { + // A very common case is that there is a straight line to the target. + var s = p.vectorPath[0]; + var e = p.vectorPath[p.vectorPath.Count-1]; + points.ClearFast(); + points.Add(s); + points.Add(e); + } else { + int iterations = iterationsByQuality[(int)quality]; + for (int it = 0; it < iterations; it++) { + if (it != 0) { + Polygon.Subdivide(points, buffer, 3); + Memory.Swap(ref buffer, ref points); + buffer.ClearFast(); + points.Reverse(); + } + + points = quality >= Quality.High ? ApplyDP(p, points, cachedFilter.cachedDelegate, cachedNNConstraint) : ApplyGreedy(p, points, cachedFilter.cachedDelegate, cachedNNConstraint); + } + if ((iterations % 2) == 0) points.Reverse(); + } + + p.vectorPath = points; + } + + List<Vector3> ApplyGreedy (Path p, List<Vector3> points, System.Func<GraphNode, bool> filter, NNConstraint nnConstraint) { + bool canBeOriginalNodes = points.Count == p.path.Count; + int startIndex = 0; + + while (startIndex < points.Count) { + Vector3 start = points[startIndex]; + var startNode = canBeOriginalNodes && points[startIndex] == (Vector3)p.path[startIndex].position ? p.path[startIndex] : null; + buffer.Add(start); + + // Do a binary search to find the furthest node we can see from this node + int mn = 1, mx = 2; + while (true) { + int endIndex = startIndex + mx; + if (endIndex >= points.Count) { + mx = points.Count - startIndex; + break; + } + Vector3 end = points[endIndex]; + var endNode = canBeOriginalNodes && end == (Vector3)p.path[endIndex].position ? p.path[endIndex] : null; + if (!ValidateLine(startNode, endNode, start, end, filter, nnConstraint)) break; + mn = mx; + mx *= 2; + } + + while (mn + 1 < mx) { + int mid = (mn + mx)/2; + int endIndex = startIndex + mid; + Vector3 end = points[endIndex]; + var endNode = canBeOriginalNodes && end == (Vector3)p.path[endIndex].position ? p.path[endIndex] : null; + + if (ValidateLine(startNode, endNode, start, end, filter, nnConstraint)) { + mn = mid; + } else { + mx = mid; + } + } + startIndex += mn; + } + + Memory.Swap(ref buffer, ref points); + buffer.ClearFast(); + return points; + } + + List<Vector3> ApplyDP (Path p, List<Vector3> points, System.Func<GraphNode, bool> filter, NNConstraint nnConstraint) { + if (DPCosts.Length < points.Count) { + DPCosts = new float[points.Count]; + DPParents = new int[points.Count]; + } + for (int i = 0; i < DPParents.Length; i++) DPCosts[i] = DPParents[i] = -1; + bool canBeOriginalNodes = points.Count == p.path.Count; + + for (int i = 0; i < points.Count; i++) { + float d = DPCosts[i]; + Vector3 start = points[i]; + var startIsOriginalNode = canBeOriginalNodes && start == (Vector3)p.path[i].position; + for (int j = i+1; j < points.Count; j++) { + // Total distance from the start to this point using the best simplified path + // The small additive constant is to make sure that the number of points is kept as small as possible + // even when the total distance is the same (which can happen with e.g multiple colinear points). + float d2 = d + (points[j] - start).magnitude + 0.0001f; + if (DPParents[j] == -1 || d2 < DPCosts[j]) { + var endIsOriginalNode = canBeOriginalNodes && points[j] == (Vector3)p.path[j].position; + if (j == i+1 || ValidateLine(startIsOriginalNode ? p.path[i] : null, endIsOriginalNode ? p.path[j] : null, start, points[j], filter, nnConstraint)) { + DPCosts[j] = d2; + DPParents[j] = i; + } else { + break; + } + } + } + } + + int c = points.Count - 1; + while (c != -1) { + buffer.Add(points[c]); + c = DPParents[c]; + } + buffer.Reverse(); + Memory.Swap(ref buffer, ref points); + buffer.ClearFast(); + return points; + } + + /// <summary> + /// Check if a straight path between v1 and v2 is valid. + /// If both n1 and n2 are supplied it is assumed that the line goes from the center of n1 to the center of n2 and a more optimized graph linecast may be done. + /// </summary> + protected bool ValidateLine (GraphNode n1, GraphNode n2, Vector3 v1, Vector3 v2, System.Func<GraphNode, bool> filter, NNConstraint nnConstraint) { + if (useRaycasting) { + // Use raycasting to check if a straight path between v1 and v2 is valid + if (use2DPhysics) { + if (thickRaycast && thickRaycastRadius > 0 && Physics2D.CircleCast(v1 + raycastOffset, thickRaycastRadius, v2 - v1, (v2 - v1).magnitude, mask)) { + return false; + } + + if (Physics2D.Linecast(v1+raycastOffset, v2+raycastOffset, mask)) { + return false; + } + } else { + // Perform a normal raycast + // This is done even if a thick raycast is also done because thick raycasts do not report collisions for + // colliders that overlapped the (imaginary) sphere at the origin of the thick raycast. + // If this raycast was not done then some obstacles could be missed. + // This is done before the normal raycast for performance. + // Normal raycasts are cheaper, so if it can be used to rule out a line earlier that's good. + if (Physics.Linecast(v1+raycastOffset, v2+raycastOffset, mask)) { + return false; + } + + // Perform a thick raycast (if enabled) + if (thickRaycast && thickRaycastRadius > 0) { + // Sphere cast doesn't detect collisions which are inside the start position of the sphere. + // That's why we do an additional check sphere which is slightly ahead of the start and which will catch most + // of these omissions. It's slightly ahead to avoid false positives that are actuall behind the agent. + if (Physics.CheckSphere(v1 + raycastOffset + (v2 - v1).normalized * thickRaycastRadius, thickRaycastRadius, mask)) { + return false; + } + if (Physics.SphereCast(new Ray(v1+raycastOffset, v2-v1), thickRaycastRadius, (v2-v1).magnitude, mask)) { + return false; + } + } + } + } + + if (useGraphRaycasting) { +#if !ASTAR_NO_GRID_GRAPH + bool betweenNodeCenters = n1 != null && n2 != null; +#endif + if (n1 == null) n1 = AstarPath.active.GetNearest(v1, nnConstraint).node; + if (n2 == null) n2 = AstarPath.active.GetNearest(v2, nnConstraint).node; + + if (n1 != null && n2 != null) { + // Use graph raycasting to check if a straight path between v1 and v2 is valid + NavGraph graph = n1.Graph; + NavGraph graph2 = n2.Graph; + + if (graph != graph2) { + return false; + } + + var rayGraph = graph as IRaycastableGraph; +#if !ASTAR_NO_GRID_GRAPH + GridGraph gg = graph as GridGraph; + if (betweenNodeCenters && gg != null) { + // If the linecast is exactly between the centers of two nodes on a grid graph then a more optimized linecast can be used. + // This method is also more stable when raycasting along a diagonal when the line just touches an obstacle. + // The normal linecast method may or may not detect that as a hit depending on floating point errors + // however this method never detect it as an obstacle (and that is very good for this component as it improves the simplification). + return !gg.Linecast(n1 as GridNodeBase, n2 as GridNodeBase, filter); + } else +#endif + if (rayGraph != null) { + return !rayGraph.Linecast(v1, v2, out GraphHitInfo _, null, filter); + } + } + } + return true; + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Modifiers/RaycastModifier.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Modifiers/RaycastModifier.cs.meta new file mode 100644 index 0000000..fe525ef --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Modifiers/RaycastModifier.cs.meta @@ -0,0 +1,7 @@ +fileFormatVersion: 2 +guid: b090bec917bbe4c3bbc612ef13f7c4a1 +MonoImporter: + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Modifiers/SimpleSmoothModifier.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Modifiers/SimpleSmoothModifier.cs new file mode 100644 index 0000000..5b87d2e --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Modifiers/SimpleSmoothModifier.cs @@ -0,0 +1,365 @@ +using UnityEngine; +using System.Collections.Generic; + +using Pathfinding.Util; + +namespace Pathfinding { + [AddComponentMenu("Pathfinding/Modifiers/Simple Smooth Modifier")] + [System.Serializable] + [RequireComponent(typeof(Seeker))] + /// <summary> + /// Modifier which smooths the path. This modifier can smooth a path by either moving the points closer together (Simple) or using Bezier curves (Bezier). + /// + /// Attach this component to the same GameObject as a Seeker component. + /// + /// This component will hook in to the Seeker's path post-processing system and will post process any paths it searches for. + /// Take a look at the Modifier Priorities settings on the Seeker component to determine where in the process this modifier should process the path. + /// + /// Several smoothing types are available, here follows a list of them and a short description of what they do, and how they work. + /// But the best way is really to experiment with them yourself. + /// + /// - <b>Simple</b> Smooths the path by drawing all points close to each other. This results in paths that might cut corners if you are not careful. + /// It will also subdivide the path to create more more points to smooth as otherwise it would still be quite rough. + /// [Open online documentation to see images] + /// - <b>Bezier</b> Smooths the path using Bezier curves. This results a smooth path which will always pass through all points in the path, but make sure it doesn't turn too quickly. + /// [Open online documentation to see images] + /// - <b>OffsetSimple</b> An alternative to Simple smooth which will offset the path outwards in each step to minimize the corner-cutting. + /// But be careful, if too high values are used, it will turn into loops and look really ugly. + /// - <b>Curved Non Uniform</b> [Open online documentation to see images] + /// + /// Note: Modifies vectorPath array + /// TODO: Make the smooth modifier take the world geometry into account when smoothing + /// </summary> + [HelpURL("https://arongranberg.com/astar/documentation/stable/simplesmoothmodifier.html")] + public class SimpleSmoothModifier : MonoModifier { +#if UNITY_EDITOR + [UnityEditor.MenuItem("CONTEXT/Seeker/Add Simple Smooth Modifier")] + public static void AddComp (UnityEditor.MenuCommand command) { + (command.context as Component).gameObject.AddComponent(typeof(SimpleSmoothModifier)); + } +#endif + + public override int Order { get { return 50; } } + + /// <summary>Type of smoothing to use</summary> + public SmoothType smoothType = SmoothType.Simple; + + /// <summary>Number of times to subdivide when not using a uniform length</summary> + [Tooltip("The number of times to subdivide (divide in half) the path segments. [0...inf] (recommended [1...10])")] + public int subdivisions = 2; + + /// <summary>Number of times to apply smoothing</summary> + [Tooltip("Number of times to apply smoothing")] + public int iterations = 2; + + /// <summary>Determines how much smoothing to apply in each smooth iteration. 0.5 usually produces the nicest looking curves.</summary> + [Tooltip("Determines how much smoothing to apply in each smooth iteration. 0.5 usually produces the nicest looking curves")] + [Range(0, 1)] + public float strength = 0.5F; + + /// <summary> + /// Toggle to divide all lines in equal length segments. + /// See: <see cref="maxSegmentLength"/> + /// </summary> + [Tooltip("Toggle to divide all lines in equal length segments")] + public bool uniformLength = true; + + /// <summary> + /// The length of the segments in the smoothed path when using <see cref="uniformLength"/>. + /// A high value yields rough paths and low value yields very smooth paths, but is slower + /// </summary> + [Tooltip("The length of each segment in the smoothed path. A high value yields rough paths and low value yields very smooth paths, but is slower")] + public float maxSegmentLength = 2F; + + /// <summary>Length factor of the bezier curves' tangents'</summary> + [Tooltip("Length factor of the bezier curves' tangents")] + public float bezierTangentLength = 0.4F; + + /// <summary>Offset to apply in each smoothing iteration when using Offset Simple. See: <see cref="smoothType"/></summary> + [Tooltip("Offset to apply in each smoothing iteration when using Offset Simple")] + public float offset = 0.2F; + + /// <summary>Roundness factor used for CurvedNonuniform</summary> + [Tooltip("How much to smooth the path. A higher value will give a smoother path, but might take the character far off the optimal path.")] + public float factor = 0.1F; + + public enum SmoothType { + Simple, + Bezier, + OffsetSimple, + CurvedNonuniform + } + + public override void Apply (Path p) { + // This should never trigger unless some other modifier has messed stuff up + if (p.vectorPath == null) { + Debug.LogWarning("Can't process NULL path (has another modifier logged an error?)"); + return; + } + + List<Vector3> path = null; + + switch (smoothType) { + case SmoothType.Simple: + path = SmoothSimple(p.vectorPath); break; + case SmoothType.Bezier: + path = SmoothBezier(p.vectorPath); break; + case SmoothType.OffsetSimple: + path = SmoothOffsetSimple(p.vectorPath); break; + case SmoothType.CurvedNonuniform: + path = CurvedNonuniform(p.vectorPath); break; + } + + if (path != p.vectorPath) { + ListPool<Vector3>.Release(ref p.vectorPath); + p.vectorPath = path; + } + } + + public List<Vector3> CurvedNonuniform (List<Vector3> path) { + if (maxSegmentLength <= 0) { + Debug.LogWarning("Max Segment Length is <= 0 which would cause DivByZero-exception or other nasty errors (avoid this)"); + return path; + } + + int pointCounter = 0; + for (int i = 0; i < path.Count-1; i++) { + //pointCounter += Mathf.FloorToInt ((path[i]-path[i+1]).magnitude / maxSegmentLength)+1; + + float dist = (path[i]-path[i+1]).magnitude; + //In order to avoid floating point errors as much as possible, and in lack of a better solution + //loop through it EXACTLY as the other code further down will + for (float t = 0; t <= dist; t += maxSegmentLength) { + pointCounter++; + } + } + + List<Vector3> subdivided = ListPool<Vector3>.Claim(pointCounter); + + // Set first velocity + Vector3 preEndVel = (path[1]-path[0]).normalized; + + for (int i = 0; i < path.Count-1; i++) { + float dist = (path[i]-path[i+1]).magnitude; + + Vector3 startVel1 = preEndVel; + Vector3 endVel1 = i < path.Count-2 ? ((path[i+2]-path[i+1]).normalized - (path[i]-path[i+1]).normalized).normalized : (path[i+1]-path[i]).normalized; + + Vector3 startVel = startVel1 * dist * factor; + Vector3 endVel = endVel1 * dist * factor; + + Vector3 start = path[i]; + Vector3 end = path[i+1]; + + float onedivdist = 1F / dist; + + for (float t = 0; t <= dist; t += maxSegmentLength) { + float t2 = t * onedivdist; + + subdivided.Add(GetPointOnCubic(start, end, startVel, endVel, t2)); + } + + preEndVel = endVel1; + } + + subdivided[subdivided.Count-1] = path[path.Count-1]; + + return subdivided; + } + + public static Vector3 GetPointOnCubic (Vector3 a, Vector3 b, Vector3 tan1, Vector3 tan2, float t) { + float t2 = t*t, t3 = t2*t; + + float h1 = 2*t3 - 3*t2 + 1; // calculate basis function 1 + float h2 = -2*t3 + 3*t2; // calculate basis function 2 + float h3 = t3 - 2*t2 + t; // calculate basis function 3 + float h4 = t3 - t2; // calculate basis function 4 + + return h1*a + // multiply and sum all funtions + h2*b + // together to build the interpolated + h3*tan1 + // point along the curve. + h4*tan2; + } + + 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 List<Vector3> SmoothSimple (List<Vector3> path) { + if (path.Count < 2) return path; + + List<Vector3> subdivided; + + if (uniformLength) { + // Clamp to a small value to avoid the path being divided into a huge number of segments + maxSegmentLength = Mathf.Max(maxSegmentLength, 0.005f); + + float pathLength = 0; + for (int i = 0; i < path.Count-1; i++) { + pathLength += Vector3.Distance(path[i], path[i+1]); + } + + int estimatedNumberOfSegments = Mathf.FloorToInt(pathLength / maxSegmentLength); + // Get a list with an initial capacity high enough so that we can add all points + subdivided = ListPool<Vector3>.Claim(estimatedNumberOfSegments+2); + + float distanceAlong = 0; + + // Sample points every [maxSegmentLength] world units along the path + for (int i = 0; i < path.Count-1; i++) { + var start = path[i]; + var end = path[i+1]; + + float length = Vector3.Distance(start, end); + + while (distanceAlong < length) { + subdivided.Add(Vector3.Lerp(start, end, distanceAlong / length)); + distanceAlong += maxSegmentLength; + } + + distanceAlong -= length; + } + + // Make sure we get the exact position of the last point + subdivided.Add(path[path.Count-1]); + } else { + subdivisions = Mathf.Max(subdivisions, 0); + + if (subdivisions > 10) { + Debug.LogWarning("Very large number of subdivisions. Cowardly refusing to subdivide every segment into more than " + (1 << subdivisions) + " subsegments"); + subdivisions = 10; + } + + int steps = 1 << subdivisions; + subdivided = ListPool<Vector3>.Claim((path.Count-1)*steps + 1); + Polygon.Subdivide(path, subdivided, steps); + } + + if (strength > 0) { + for (int it = 0; it < iterations; it++) { + Vector3 prev = subdivided[0]; + + for (int i = 1; i < subdivided.Count-1; i++) { + Vector3 tmp = subdivided[i]; + + // prev is at this point set to the value that subdivided[i-1] had before this loop started + // Move the point closer to the average of the adjacent points + subdivided[i] = Vector3.Lerp(tmp, (prev+subdivided[i+1])/2F, strength); + + prev = tmp; + } + } + } + + return subdivided; + } + + public List<Vector3> SmoothBezier (List<Vector3> path) { + if (subdivisions < 0) subdivisions = 0; + + int subMult = 1 << subdivisions; + List<Vector3> subdivided = ListPool<Vector3>.Claim(); + + for (int i = 0; i < path.Count-1; i++) { + Vector3 tangent1; + Vector3 tangent2; + if (i == 0) { + tangent1 = path[i+1]-path[i]; + } else { + tangent1 = path[i+1]-path[i-1]; + } + + if (i == path.Count-2) { + tangent2 = path[i]-path[i+1]; + } else { + tangent2 = path[i]-path[i+2]; + } + + tangent1 *= bezierTangentLength; + tangent2 *= bezierTangentLength; + + Vector3 v1 = path[i]; + Vector3 v2 = v1+tangent1; + Vector3 v4 = path[i+1]; + Vector3 v3 = v4+tangent2; + + for (int j = 0; j < subMult; j++) { + subdivided.Add(AstarSplines.CubicBezier(v1, v2, v3, v4, (float)j/subMult)); + } + } + + // Assign the last point + subdivided.Add(path[path.Count-1]); + + return subdivided; + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Modifiers/SimpleSmoothModifier.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Modifiers/SimpleSmoothModifier.cs.meta new file mode 100644 index 0000000..fc16196 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Modifiers/SimpleSmoothModifier.cs.meta @@ -0,0 +1,7 @@ +fileFormatVersion: 2 +guid: cb6a34d769a1e4ac7b0b30e433aa443c +MonoImporter: + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Modifiers/StartEndModifier.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Modifiers/StartEndModifier.cs new file mode 100644 index 0000000..0759e85 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Modifiers/StartEndModifier.cs @@ -0,0 +1,283 @@ +using UnityEngine; +using System.Collections.Generic; + +namespace Pathfinding { + [System.Serializable] + /// <summary> + /// Adjusts start and end points of a path. + /// + /// This modifier is included in the <see cref="Pathfinding.Seeker"/> component and is always used if you are using a Seeker. + /// When a path is calculated the resulting path will only be the positions of the nodes it passes through. + /// However often you may not want to navigate to the center of a specific node but instead to a point on the surface of a node. + /// This modifier will adjust the endpoints of the path. + /// + /// [Open online documentation to see images] + /// </summary> + public class StartEndModifier : PathModifier { + public override int Order { get { return 0; } } + + /// <summary> + /// Add points to the path instead of replacing them. + /// If for example <see cref="exactEndPoint"/> is set to ClosestOnNode then the path will be modified so that + /// the path goes first to the center of the last node in the path and then goes to the closest point + /// on the node to the end point in the path request. + /// + /// If this is false however then the relevant points in the path will simply be replaced. + /// In the above example the path would go directly to the closest point on the node without passing + /// through the center of the node. + /// </summary> + public bool addPoints; + + /// <summary> + /// How the start point of the path will be determined. + /// See: <see cref="Exactness"/> + /// </summary> + public Exactness exactStartPoint = Exactness.ClosestOnNode; + + /// <summary> + /// How the end point of the path will be determined. + /// See: <see cref="Exactness"/> + /// </summary> + public Exactness exactEndPoint = Exactness.ClosestOnNode; + + /// <summary> + /// Will be called when a path is processed. + /// The value which is returned will be used as the start point of the path + /// and potentially clamped depending on the value of the <see cref="exactStartPoint"/> field. + /// Only used for the Original, Interpolate and NodeConnection modes. + /// </summary> + public System.Func<Vector3> adjustStartPoint; + + /// <summary> + /// Sets where the start and end points of a path should be placed. + /// + /// Here is a legend showing what the different items in the above images represent. + /// The images above show a path coming in from the top left corner and ending at a node next to an obstacle as well as 2 different possible end points of the path and how they would be modified. + /// [Open online documentation to see images] + /// </summary> + public enum Exactness { + /// <summary> + /// The point is snapped to the position of the first/last node in the path. + /// Use this if your game is very tile based and you want your agents to stop precisely at the center of the nodes. + /// If you recalculate the path while the agent is moving you may want the start point snapping to be ClosestOnNode and the end point snapping to be SnapToNode however + /// as while an agent is moving it will likely not be right at the center of a node. + /// + /// [Open online documentation to see images] + /// </summary> + SnapToNode, + /// <summary> + /// The point is set to the exact point which was passed when creating the path request. + /// Note that if a path was for example requested to a point inside an obstacle, then the last point of the path will be inside that obstacle, which is usually not what you want. + /// Consider using the <see cref="Exactness.ClosestOnNode"/> option instead. + /// + /// [Open online documentation to see images] + /// </summary> + Original, + /// <summary> + /// The point is set to the closest point on the line between either the two first points or the two last points. + /// Usually you will want to use the NodeConnection mode instead since that is usually the behaviour that you really want. + /// This mode exists mostly for compatibility reasons. + /// [Open online documentation to see images] + /// Deprecated: Use NodeConnection instead. + /// </summary> + Interpolate, + /// <summary> + /// The point is set to the closest point on the surface of the node. Note that some node types (point nodes) do not have a surface, so the "closest point" is simply the node's position which makes this identical to <see cref="Exactness.SnapToNode"/>. + /// This is the mode that you almost always want to use in a free movement 3D world. + /// [Open online documentation to see images] + /// </summary> + ClosestOnNode, + /// <summary> + /// The point is set to the closest point on one of the connections from the start/end node. + /// This mode may be useful in a grid based or point graph based world when using the AILerp script. + /// + /// Note: If you are using this mode with a <see cref="Pathfinding.PointGraph"/> you probably also want to use the <see cref="Pathfinding.PointGraph.NodeDistanceMode Connection"/> for <see cref="PointGraph.nearestNodeDistanceMode"/>. + /// + /// [Open online documentation to see images] + /// </summary> + NodeConnection, + } + + /// <summary> + /// Do a straight line check from the node's center to the point determined by the <see cref="Exactness"/>. + /// There are very few cases where you will want to use this. It is mostly here for + /// backwards compatibility reasons. + /// + /// Version: Since 4.1 this field only has an effect for the <see cref="Exactness"/> mode Original because that's the only one where it makes sense. + /// </summary> + public bool useRaycasting; + public LayerMask mask = -1; + + /// <summary> + /// Do a straight line check from the node's center to the point determined by the <see cref="Exactness"/>. + /// See: <see cref="useRaycasting"/> + /// + /// Version: Since 4.1 this field only has an effect for the <see cref="Exactness"/> mode Original because that's the only one where it makes sense. + /// </summary> + public bool useGraphRaycasting; + + List<GraphNode> connectionBuffer; + System.Action<GraphNode> connectionBufferAddDelegate; + + public override void Apply (Path _p) { + var p = _p as ABPath; + + // This modifier only supports ABPaths (doesn't make much sense for other paths anyway) + if (p == null || p.vectorPath.Count == 0) return; + + bool singleNode = false; + + if (p.vectorPath.Count == 1 && !addPoints) { + // Duplicate first point + p.vectorPath.Add(p.vectorPath[0]); + singleNode = true; + } + + // Add instead of replacing points + bool forceAddStartPoint, forceAddEndPoint; + // Which connection the start/end point was on (only used for the Connection mode) + int closestStartConnection, closestEndConnection; + + Vector3 pStart = Snap(p, exactStartPoint, true, out forceAddStartPoint, out closestStartConnection); + Vector3 pEnd = Snap(p, exactEndPoint, false, out forceAddEndPoint, out closestEndConnection); + + // This is a special case when the path is only a single node and the Connection mode is used. + // (forceAddStartPoint/forceAddEndPoint is only used for the Connection mode) + // In this case the start and end points lie on the connections of the node. + // There are two cases: + // 1. If the start and end points lie on the same connection we do *not* want + // the path to pass through the node center but instead go directly from point to point. + // This is the case of closestStartConnection == closestEndConnection. + // 2. If the start and end points lie on different connections we *want* + // the path to pass through the node center as it goes from one connection to another one. + // However in any case we only want the node center to be added once to the path + // so we set forceAddStartPoint to false anyway. + if (singleNode) { + if (closestStartConnection == closestEndConnection) { + forceAddStartPoint = false; + forceAddEndPoint = false; + } else { + forceAddStartPoint = false; + } + } + + // Add or replace the start point + // Disable adding of points if the mode is SnapToNode since then + // the first item in vectorPath will very likely be the same as the + // position of the first node + if ((forceAddStartPoint || addPoints) && exactStartPoint != Exactness.SnapToNode) { + p.vectorPath.Insert(0, pStart); + } else { + p.vectorPath[0] = pStart; + } + + if ((forceAddEndPoint || addPoints) && exactEndPoint != Exactness.SnapToNode) { + p.vectorPath.Add(pEnd); + } else { + p.vectorPath[p.vectorPath.Count-1] = pEnd; + } + } + + 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]; + if (!path.CanTraverse(neighbour)) continue; + + // 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"); + } + } + + protected Vector3 GetClampedPoint (Vector3 from, Vector3 to, GraphNode hint) { + Vector3 point = to; + RaycastHit hit; + + if (useRaycasting && Physics.Linecast(from, to, out hit, mask)) { + point = hit.point; + } + + if (useGraphRaycasting && hint != null) { + var rayGraph = AstarData.GetGraph(hint) as IRaycastableGraph; + + if (rayGraph != null) { + GraphHitInfo graphHit; + if (rayGraph.Linecast(from, point, out graphHit)) { + point = graphHit.point; + } + } + } + + return point; + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Modifiers/StartEndModifier.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Modifiers/StartEndModifier.cs.meta new file mode 100644 index 0000000..382950c --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Modifiers/StartEndModifier.cs.meta @@ -0,0 +1,7 @@ +fileFormatVersion: 2 +guid: f72dd5db0de344ea19ae250ac5b68903 +MonoImporter: + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} |