307 lines
12 KiB
C#
307 lines
12 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.Linq;
|
|
using Sirenix.Utilities;
|
|
using UnityEngine;
|
|
using UnityEngine.AI;
|
|
using VOID.Generic.Dict;
|
|
using VOID.Generic.Navigation;
|
|
using VOID.Generic.Objects.Traversable;
|
|
using VOID.Generic.Objects.Unit;
|
|
|
|
namespace VOID.Generic.Util
|
|
{
|
|
public static class NavMeshPathUtil
|
|
{
|
|
private const float CorrectingTargetRange = 5f;
|
|
private const float CorrectingLengthRange = 0.5f;
|
|
private const float CorrectingHeightRange = 0.75f;
|
|
|
|
public static Vector3 SamplePosition(NavMeshAgent agent, Vector3 position)
|
|
{
|
|
NavMesh.SamplePosition(position, out var hit, CorrectingTargetRange, agent.areaMask);
|
|
return hit.position;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Return NavMeshPath to given target destination from current position of agent.
|
|
/// </summary>
|
|
public static NavMeshPath GetPathTo(NavMeshAgent agent, Vector3 target)
|
|
{
|
|
var path = new NavMeshPath();
|
|
NavMesh.SamplePosition(target, out var correctedTarget, CorrectingTargetRange, agent.areaMask);
|
|
if (correctedTarget.hit) target = correctedTarget.position;
|
|
NavMesh.CalculatePath(agent.transform.position, target, agent.areaMask, path);
|
|
return path;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Alias to <see cref="GetPathShortenedByRange(List{Vector3}, Vector3, float)"/>
|
|
/// </summary>
|
|
/// <inheritdoc cref="GetPathShortenedByRange(List{Vector3}, Vector3, float)" />
|
|
public static List<Vector3> GetPathShortenedByRange(NavMeshPath path, Vector3 destination, float untilInRange)
|
|
{
|
|
return GetPathShortenedByRange(path.corners.ToList(), destination, untilInRange);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Path shortened by given range.
|
|
/// Result is List of Vector3 instead of NavMeshPath - because it is only internal :(
|
|
/// </summary>
|
|
public static List<Vector3> GetPathShortenedByRange(List<Vector3> path, Vector3 destination, float untilInRange)
|
|
{
|
|
// Invalid path with no corners?
|
|
if (path.Count == 0) return new List<Vector3>();
|
|
|
|
// First point of path is already in that range
|
|
if (Vector3.Distance(path.First(), destination) < untilInRange) return new List<Vector3> { path[0] };
|
|
|
|
for (var i = 0; i < path.Count - 1; i++)
|
|
{
|
|
var p1 = path[i];
|
|
var p2 = path[i + 1];
|
|
var intersects = LineIntersectSpherePoints(p1, p2, destination, untilInRange);
|
|
|
|
// No intersects point found
|
|
if (intersects.Length == 0) continue;
|
|
|
|
var fullDistance = Vector3.Distance(p1, p2);
|
|
var distance1 = Vector3.Distance(p1, intersects[0]) + Vector3.Distance(intersects[0], p2);
|
|
var distance2 = Vector3.Distance(p1, intersects[1]) + Vector3.Distance(intersects[1], p2);
|
|
var diff1 = Math.Abs(fullDistance - distance1);
|
|
var diff2 = Math.Abs(fullDistance - distance2);
|
|
|
|
if (diff1 < 0.05f) return path.Take(i + 1).Append(intersects[0]).ToList();
|
|
if (diff2 < 0.05f) return path.Take(i + 1).Append(intersects[1]).ToList();
|
|
}
|
|
|
|
// Path cant reach destination at given range
|
|
return path;
|
|
}
|
|
|
|
private static Vector3[] LineIntersectSpherePoints(Vector3 p1, Vector3 p2, Vector3 center, float radius)
|
|
{
|
|
var dp = new Vector3();
|
|
Vector3[] sect;
|
|
float a, b, c;
|
|
float bb4ac;
|
|
float mu1;
|
|
float mu2;
|
|
|
|
// get the distance between X and Z on the segment
|
|
dp.x = p2.x - p1.x;
|
|
dp.z = p2.z - p1.z;
|
|
// I don't get the math here
|
|
a = dp.x * dp.x + dp.z * dp.z;
|
|
b = 2 * (dp.x * (p1.x - center.x) + dp.z * (p1.z - center.z));
|
|
c = center.x * center.x + center.z * center.z;
|
|
c += p1.x * p1.x + p1.z * p1.z;
|
|
c -= 2 * (center.x * p1.x + center.z * p1.z);
|
|
c -= radius * radius;
|
|
bb4ac = b * b - 4 * a * c;
|
|
if (Mathf.Abs(a) < float.Epsilon || bb4ac < 0)
|
|
// line does not intersect
|
|
return Array.Empty<Vector3>();
|
|
mu1 = (-b + Mathf.Sqrt(bb4ac)) / (2 * a);
|
|
mu2 = (-b - Mathf.Sqrt(bb4ac)) / (2 * a);
|
|
sect = new Vector3[2];
|
|
sect[0] = new Vector3(p1.x + mu1 * (p2.x - p1.x), 0, p1.z + mu1 * (p2.z - p1.z));
|
|
sect[1] = new Vector3(p1.x + mu2 * (p2.x - p1.x), 0, p1.z + mu2 * (p2.z - p1.z));
|
|
|
|
return sect;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Correcting NavMeshPath with:
|
|
/// - Make whole path sticking to ground
|
|
/// - If path going through TraversableObject (NavMeshLink) then stick to it (with unit's offset)
|
|
/// Result is PathCorrected instead of NavMeshPath - because it is only internal :(
|
|
/// </summary>
|
|
public static PathCorrected GetCorrectedPath(List<Vector3> path, UnitObject forUnit)
|
|
{
|
|
if (path.IsNullOrEmpty()) return null;
|
|
|
|
// Starting point won't change
|
|
var correctedPath = new List<Vector3>(path.GetRange(0, 1));
|
|
var usedTraversables = new List<PathCorrectedLinkData>();
|
|
var canTraverse = true;
|
|
for (var i = 1; i < path.Count; i++)
|
|
{
|
|
if(canTraverse && NavigationManager.current.GetCachedTraversable(path[i - 1], out var traversable, out var isReverse))
|
|
{
|
|
// Line between those two points is connected by NavMeshLink, thats why
|
|
correctedPath.AddRange(GetCorrectedLinkPathBetweenPoints(path[i - 1], traversable, isReverse, forUnit));
|
|
usedTraversables.Add(new PathCorrectedLinkData() { traversable = traversable, isReverse = isReverse });
|
|
canTraverse = false;
|
|
}
|
|
else
|
|
{
|
|
correctedPath.AddRange(GetCorrectedPathBetweenPoints(path[i - 1], path[i]));
|
|
canTraverse = true;
|
|
}
|
|
}
|
|
return new PathCorrected() {path = correctedPath, traversables = usedTraversables};
|
|
}
|
|
|
|
private static List<Vector3> GetCorrectedLinkPathBetweenPoints(Vector3 start,
|
|
TraversableObject traversable, bool isReverse, UnitObject forUnit)
|
|
{
|
|
var unitLadderOffset = forUnit.unitData.ladderPosition.localPosition;
|
|
var correctedPath = traversable.traversableData.path.AsEnumerable();
|
|
if (isReverse) correctedPath = correctedPath.Reverse();
|
|
return correctedPath
|
|
.Select(t => t.position + t.InverseTransformDirection(unitLadderOffset))
|
|
.Prepend(start)
|
|
.ToList();
|
|
}
|
|
|
|
private static List<Vector3> GetCorrectedPathBetweenPoints(Vector3 from, Vector3 to)
|
|
{
|
|
var correctedPath = new List<Vector3>();
|
|
var stepsNeeded = Mathf.CeilToInt(Vector3.Distance(from, to) / CorrectingLengthRange);
|
|
var stepProgress = Vector3.Scale(to - from, new Vector3(1, 0, 1)) / stepsNeeded;
|
|
var point = from;
|
|
for (var i = 1; i <= stepsNeeded; i++)
|
|
{
|
|
point += stepProgress;
|
|
point = GetCorrectedPoint(point);
|
|
correctedPath.Add(point);
|
|
}
|
|
|
|
return correctedPath;
|
|
}
|
|
|
|
private static Vector3 GetCorrectedPoint(Vector3 point)
|
|
{
|
|
var isHit = Physics.Raycast(
|
|
new Vector3(point.x, point.y + CorrectingHeightRange, point.z),
|
|
Vector3.down, out var hit,
|
|
CorrectingHeightRange * 2f,
|
|
LayerManager.Static);
|
|
|
|
if (isHit) return hit.point;
|
|
|
|
var pointString = $"({point.x}, {point.y}, {point.z})";
|
|
Debug.LogWarning($"Can't find correct height of {pointString} in path. Using default point.");
|
|
return point;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Alias to <see cref="GetPathLength(List{Vector3})"/>
|
|
/// </summary>
|
|
/// <inheritdoc cref="GetPathLength(List{Vector3})" />
|
|
public static float GetPathLength(NavMeshPath navMeshPath)
|
|
{
|
|
return GetPathLength(navMeshPath.corners.ToList());
|
|
}
|
|
|
|
/// <summary>
|
|
/// Returns length of given path
|
|
/// </summary>
|
|
public static float GetPathLength(List<Vector3> points)
|
|
{
|
|
if (points.Count <= 2) return 0f;
|
|
|
|
var length = 0f;
|
|
for (var i = 1; i < points.Count; i++) length += RangeUtil.GetDistance(points[i - 1], points[i]);
|
|
|
|
return length;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Alias to <see cref="CalculatePathCost(List{Vector3})"/>
|
|
/// </summary>
|
|
/// <inheritdoc cref="CalculatePathCost(List{Vector3})" />
|
|
public static float CalculatePathCost(NavMeshPath navMeshPath)
|
|
{
|
|
return CalculatePathCost(navMeshPath.corners.ToList());
|
|
}
|
|
|
|
/// <summary>
|
|
/// Returns cost for given path
|
|
/// </summary>
|
|
public static float CalculatePathCost(List<Vector3> points)
|
|
{
|
|
float cost = 0;
|
|
NavMeshHit hit;
|
|
NavMesh.SamplePosition(points[0], out hit, 0.1f, NavMesh.AllAreas);
|
|
var rayStart = points[0];
|
|
var mask = hit.mask;
|
|
var areaIndex = IndexFromMask(mask);
|
|
|
|
for (var index = 1; index < points.Count; ++index)
|
|
{
|
|
var maxTries = 5;
|
|
do
|
|
{
|
|
var corner = points[index];
|
|
if (NavMesh.Raycast(rayStart, corner, out hit, mask))
|
|
{
|
|
cost += NavMesh.GetAreaCost(areaIndex) * hit.distance;
|
|
if (hit.mask != 0)
|
|
mask = hit.mask;
|
|
areaIndex = IndexFromMask(mask);
|
|
rayStart = hit.position;
|
|
|
|
if (hit.mask == 0)
|
|
rayStart += (corner - rayStart).normalized * 0.05f;
|
|
}
|
|
else
|
|
{
|
|
rayStart += (corner - rayStart).normalized * 0.05f;
|
|
}
|
|
|
|
maxTries--;
|
|
} while (hit.hit && maxTries > 0);
|
|
}
|
|
|
|
return cost;
|
|
}
|
|
|
|
private static int IndexFromMask(int mask)
|
|
{
|
|
if (mask == 0) return -1;
|
|
var i = 0;
|
|
while (mask != 1)
|
|
{
|
|
mask >>= 1;
|
|
++i;
|
|
}
|
|
|
|
return i;
|
|
}
|
|
|
|
public static void SplitPath(Vector3[] path, float movePoints, out Vector3[] reachablePath, out Vector3[] unreachablePath)
|
|
{
|
|
var currentLength = 0f;
|
|
for (var i = 1; i < path.Length; i++)
|
|
{
|
|
var distanceBetweenPoints = Vector3.Distance(path[i - 1], path[i]);
|
|
currentLength += distanceBetweenPoints;
|
|
|
|
// If still reachable then do nothing
|
|
if (currentLength <= movePoints) continue;
|
|
|
|
// Find exact point where unreachable path will start
|
|
var interpolate = (currentLength - movePoints) / distanceBetweenPoints;
|
|
var pointBetween = Vector3.Lerp(path[i], path[i - 1], interpolate);
|
|
|
|
reachablePath = path.Where((_, index) => index < i).Append(pointBetween).ToArray();
|
|
unreachablePath = path.Where((_, index) => index >= i).Prepend(pointBetween).ToArray();
|
|
return;
|
|
}
|
|
|
|
// Destination is reachale, so there is no unreachable path
|
|
reachablePath = path.ToArray();
|
|
unreachablePath = null;
|
|
}
|
|
|
|
public static void SplitReachablePath(Vector3[] reachablePath, out List<PathType> pathTypes, out List<Vector3[]> paths)
|
|
{
|
|
// TODO: right now there is no "dangerous" and "natural" areas!
|
|
// TODO: when those will be implemented then this place must be updated as well!
|
|
pathTypes = new List<PathType>() { PathType.Reachable };
|
|
paths = new List<Vector3[]>() { reachablePath };
|
|
}
|
|
}
|
|
} |