1478 lines
78 KiB
C#
1478 lines
78 KiB
C#
|
using System.Collections.Generic;
|
|||
|
|
using UnityEngine;
|
|||
|
|
|
|||
|
|
// Sci-Fi Ship Controller. Copyright (c) 2018-2023 SCSM Pty Ltd. All rights reserved.
|
|||
|
|
namespace SciFiShipController
|
|||
|
|
{
|
|||
|
|
/// <summary>
|
|||
|
|
/// Collection of common maths routines used in SSC.
|
|||
|
|
/// </summary>
|
|||
|
|
public class SSCMath
|
|||
|
|
{
|
|||
|
|
#region Static values
|
|||
|
|
public static readonly float e = 2.718281828459041f;
|
|||
|
|
|
|||
|
|
#endregion
|
|||
|
|
|
|||
|
|
#region Hash Code Maths
|
|||
|
|
|
|||
|
|
/// <summary>
|
|||
|
|
/// Returns a more deterministic hashcode from a new System GUID.
|
|||
|
|
/// </summary>
|
|||
|
|
/// <returns></returns>
|
|||
|
|
public static int GetHashCodeFromGuid()
|
|||
|
|
{
|
|||
|
|
return GetHashCode(System.Guid.NewGuid().ToString());
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/// <summary>
|
|||
|
|
/// Based on concept from corefx/src/Common/src/System/Text/StringOrCharArray.cs
|
|||
|
|
/// in .NET Core. This should be a little more deterministic than the standard
|
|||
|
|
/// GetHashCode from .NET. As this doesn't use pointers it should also be thread safe.
|
|||
|
|
/// </summary>
|
|||
|
|
/// <param name="stringToHash"></param>
|
|||
|
|
/// <returns></returns>
|
|||
|
|
public static int GetHashCode(string stringToHash)
|
|||
|
|
{
|
|||
|
|
// The hash values can overflow the max values of int. This will
|
|||
|
|
// avoid getting an overflowexception. Values should just wrap around.
|
|||
|
|
unchecked
|
|||
|
|
{
|
|||
|
|
int hash1 = (5381 << 16) + 5381;
|
|||
|
|
int hash2 = hash1;
|
|||
|
|
|
|||
|
|
for (int i = 0; i < stringToHash.Length; i += 2)
|
|||
|
|
{
|
|||
|
|
hash1 = ((hash1 << 5) + hash1) ^ stringToHash[i];
|
|||
|
|
if (i == stringToHash.Length - 1) { break; }
|
|||
|
|
hash2 = ((hash2 << 5) + hash2) ^ stringToHash[i + 1];
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// 1566083941 is a Mersenne prime Mp = 2^n - 1
|
|||
|
|
return hash1 + (hash2 * 1566083941);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
#endregion
|
|||
|
|
|
|||
|
|
#region Bezier Curve Maths
|
|||
|
|
|
|||
|
|
#region Bezier Path variables
|
|||
|
|
|
|||
|
|
private static Vector3 thisPointOnPath = Vector3.zero;
|
|||
|
|
private static Vector3 previousPointOnPath = Vector3.zero;
|
|||
|
|
|
|||
|
|
// NOTES
|
|||
|
|
// p0 is the first point, p1 is its control point (direction at p0 is towards p1)
|
|||
|
|
// p3 is the second point, p2 is its control point (direction at p3 is away from p2)
|
|||
|
|
// Bezier curve point: B = (1 - t)^3 * p0 + 3 * (1 - t)^2 * t + p1 + 3 * (1 - t) * t^2 * p2 + t^3 * p3
|
|||
|
|
// First derivative: B' = 3 * (1 - t)^2 * (p1 - p0) + 6 * (1 - t) * t * (p2 - p1) + 3 * t^2 * (p3 - p2)
|
|||
|
|
// Second derivative: B" = 6 * (1 - t) * (p2 - 2*p1 + p0) + 6 * t * (p3 - 2*p2 + p1)
|
|||
|
|
// Curvature: K = len(B' x B") / len(B')^3
|
|||
|
|
|
|||
|
|
#endregion
|
|||
|
|
|
|||
|
|
// TODO: Look at all function names / variable names, make sure they are all consistent with each other
|
|||
|
|
|
|||
|
|
#region Public Static Member API Methods
|
|||
|
|
|
|||
|
|
#region Basic Functions
|
|||
|
|
|
|||
|
|
/// <summary>
|
|||
|
|
/// Gets the point on the path given the index of the last path point and the t-value.
|
|||
|
|
/// The pointOnPath variable is set to the point on the path.
|
|||
|
|
/// If the return value is true, the operation was successful.
|
|||
|
|
/// </summary>
|
|||
|
|
/// <param name="pathData"></param>
|
|||
|
|
/// <param name="lastPathPointIndex"></param>
|
|||
|
|
/// <param name="tValue"></param>
|
|||
|
|
/// <param name="pointOnPath"></param>
|
|||
|
|
/// <returns></returns>
|
|||
|
|
public static bool GetPointOnPath(PathData pathData, int lastPathPointIndex, float tValue, ref Vector3 pointOnPath)
|
|||
|
|
{
|
|||
|
|
// Check that the path is valid
|
|||
|
|
if (pathData != null && pathData.pathLocationDataList != null)
|
|||
|
|
{
|
|||
|
|
// Check how many points there are in the path
|
|||
|
|
int pathDataLocationListCount = pathData.pathLocationDataList.Count;
|
|||
|
|
if (pathDataLocationListCount < 1)
|
|||
|
|
{
|
|||
|
|
// No points in the path, so this is not a valid path
|
|||
|
|
return false;
|
|||
|
|
}
|
|||
|
|
else if (pathDataLocationListCount == 1)
|
|||
|
|
{
|
|||
|
|
// Check if the single point is assigned to a valid Location
|
|||
|
|
if (pathData.pathLocationDataList[0].locationData.isUnassigned) { return false; }
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
// Only one point in the path, so simply return that point
|
|||
|
|
pointOnPath = pathData.pathLocationDataList[0].locationData.position;
|
|||
|
|
return true;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
else if (lastPathPointIndex < pathDataLocationListCount)
|
|||
|
|
{
|
|||
|
|
// Multiple points in the path, so perform the algorithm
|
|||
|
|
// Get the index of the next (assigned) path point
|
|||
|
|
// A potential optimisation would be to pass in the already determined pathDataLocationListCount...
|
|||
|
|
int nextPathPointIndex = SSCManager.GetNextPathLocationIndex(pathData, lastPathPointIndex, true);
|
|||
|
|
|
|||
|
|
// If there is no next assigned point, return false
|
|||
|
|
if (nextPathPointIndex < 0) { return false; }
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
// Call the internal algorithm function
|
|||
|
|
GetPointOnPathInternal(pathData, lastPathPointIndex, nextPathPointIndex, tValue, ref pointOnPath);
|
|||
|
|
return true;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
else { return false; }
|
|||
|
|
}
|
|||
|
|
else { return false; }
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/// <summary>
|
|||
|
|
/// Calculates the tangent, normal and binormal of the path given the index of the last path point and the t-value.
|
|||
|
|
/// The pathTangent variable is set to the (normalised) tangent of the path.
|
|||
|
|
/// The pathNormal variable is set to the (normalised) normal of the path.
|
|||
|
|
/// The pathBinormal variable is set to the (normalised) binormal of the path.
|
|||
|
|
/// If the return value is true, the operation was successful.
|
|||
|
|
/// </summary>
|
|||
|
|
/// <param name="pathData"></param>
|
|||
|
|
/// <param name="lastPathPointIndex"></param>
|
|||
|
|
/// <param name="tValue"></param>
|
|||
|
|
/// <param name="pathTangent"></param>
|
|||
|
|
/// <param name="pathNormal"></param>
|
|||
|
|
/// <param name="pathBinormal"></param>
|
|||
|
|
/// <returns></returns>
|
|||
|
|
public static bool GetPathFrenetData (PathData pathData, int lastPathPointIndex, float tValue, ref Vector3 pathTangent,
|
|||
|
|
ref Vector3 pathNormal, ref Vector3 pathBinormal)
|
|||
|
|
{
|
|||
|
|
// Check that the path is valid
|
|||
|
|
if (pathData != null && pathData.pathLocationDataList != null)
|
|||
|
|
{
|
|||
|
|
// Check how many points there are in the path
|
|||
|
|
int pathDataLocationListCount = pathData.pathLocationDataList.Count;
|
|||
|
|
if (pathDataLocationListCount <= 1)
|
|||
|
|
{
|
|||
|
|
// One or no points in the path, so this is not a valid path for curvature to be determined
|
|||
|
|
return false;
|
|||
|
|
}
|
|||
|
|
else if (lastPathPointIndex < pathDataLocationListCount)
|
|||
|
|
{
|
|||
|
|
// Multiple points in the path, so perform the algorithm
|
|||
|
|
// Get the index of the next path point
|
|||
|
|
int nextPathPointIndex = SSCManager.GetNextPathLocationIndex(pathData, lastPathPointIndex, true);
|
|||
|
|
|
|||
|
|
// If there is no next assigned point, return false
|
|||
|
|
if (nextPathPointIndex < 0) { return false; }
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
// Call the internal algorithm function to obtain the first and second derivatives
|
|||
|
|
// (which will be in the directions of the tangent and the normal respectively)
|
|||
|
|
GetPathFirstDerivativeInternal(pathData, lastPathPointIndex, nextPathPointIndex, tValue, ref pathTangent);
|
|||
|
|
GetPathSecondDerivativeInternal(pathData, lastPathPointIndex, nextPathPointIndex, tValue, ref pathNormal);
|
|||
|
|
// Normalise the calculated tangent
|
|||
|
|
pathTangent /= (float)System.Math.Sqrt((pathTangent.x * pathTangent.x) +
|
|||
|
|
(pathTangent.y * pathTangent.y) + (pathTangent.z * pathTangent.z));
|
|||
|
|
// Normalise the calculated normal
|
|||
|
|
pathNormal /= (float)System.Math.Sqrt((pathNormal.x * pathNormal.x) +
|
|||
|
|
(pathNormal.y * pathNormal.y) + (pathNormal.z * pathNormal.z));
|
|||
|
|
// Binormal is simply the cross product of the tangent and the normal
|
|||
|
|
// There is no need to normalise it, as the cross product of two orthogonal vectors of unit
|
|||
|
|
// length is itself of unit length
|
|||
|
|
// TODO optimise
|
|||
|
|
pathBinormal = Vector3.Cross(pathTangent, pathNormal);
|
|||
|
|
return true;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
else { return false; }
|
|||
|
|
}
|
|||
|
|
else { return false; }
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/// <summary>
|
|||
|
|
/// Calculates the tangent of the path given the index of the last path point and the t-value.
|
|||
|
|
/// The pathTangent variable is set to the (normalised) tangent of the path.
|
|||
|
|
/// If the return value is true, the operation was successful.
|
|||
|
|
/// NOTE: If you also want the normal and/or the binormal, call GetPathFrenetData instead (as it gets all
|
|||
|
|
/// three values at once more quickly).
|
|||
|
|
/// </summary>
|
|||
|
|
/// <param name="pathData"></param>
|
|||
|
|
/// <param name="lastPathPointIndex"></param>
|
|||
|
|
/// <param name="tValue"></param>
|
|||
|
|
/// <param name="pathTangent"></param>
|
|||
|
|
/// <returns></returns>
|
|||
|
|
public static bool GetPathTangent(PathData pathData, int lastPathPointIndex, float tValue, ref Vector3 pathTangent)
|
|||
|
|
{
|
|||
|
|
// Check that the path is valid
|
|||
|
|
if (pathData != null && pathData.pathLocationDataList != null)
|
|||
|
|
{
|
|||
|
|
// Check how many points there are in the path
|
|||
|
|
int pathDataLocationListCount = pathData.pathLocationDataList.Count;
|
|||
|
|
if (pathDataLocationListCount <= 1)
|
|||
|
|
{
|
|||
|
|
// One or no points in the path, so this is not a valid path for curvature to be determined
|
|||
|
|
return false;
|
|||
|
|
}
|
|||
|
|
else if (lastPathPointIndex < pathDataLocationListCount)
|
|||
|
|
{
|
|||
|
|
// Multiple points in the path, so perform the algorithm
|
|||
|
|
// Get the index of the next path point
|
|||
|
|
int nextPathPointIndex = SSCManager.GetNextPathLocationIndex(pathData, lastPathPointIndex, true);
|
|||
|
|
|
|||
|
|
// If there is no next assigned point, return false
|
|||
|
|
if (nextPathPointIndex < 0) { return false; }
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
// Call the internal algorithm function to obtain the first derivative (which will be in the
|
|||
|
|
// direction of the tangent)
|
|||
|
|
GetPathFirstDerivativeInternal(pathData, lastPathPointIndex, nextPathPointIndex, tValue, ref pathTangent);
|
|||
|
|
// Normalise the calculated tangent
|
|||
|
|
pathTangent /= (float)System.Math.Sqrt((pathTangent.x * pathTangent.x) +
|
|||
|
|
(pathTangent.y * pathTangent.y) + (pathTangent.z * pathTangent.z));
|
|||
|
|
return true;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
else { return false; }
|
|||
|
|
}
|
|||
|
|
else { return false; }
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/// <summary>
|
|||
|
|
/// Calculates the Frenet normal of the path given the index of the last path point and the t-value.
|
|||
|
|
/// The pathNormal variable is set to the (normalised) Frenet normal of the path.
|
|||
|
|
/// If the return value is true, the operation was successful.
|
|||
|
|
/// NOTE: If you also want the tangent and/or the binormal, call GetPathFrenetData instead (as it gets all
|
|||
|
|
/// three values at once more quickly).
|
|||
|
|
/// </summary>
|
|||
|
|
/// <param name="pathData"></param>
|
|||
|
|
/// <param name="lastPathPointIndex"></param>
|
|||
|
|
/// <param name="tValue"></param>
|
|||
|
|
/// <param name="pathNormal"></param>
|
|||
|
|
/// <returns></returns>
|
|||
|
|
public static bool GetPathNormal(PathData pathData, int lastPathPointIndex, float tValue, ref Vector3 pathNormal)
|
|||
|
|
{
|
|||
|
|
// Check that the path is valid
|
|||
|
|
if (pathData != null && pathData.pathLocationDataList != null)
|
|||
|
|
{
|
|||
|
|
// Check how many points there are in the path
|
|||
|
|
int pathDataLocationListCount = pathData.pathLocationDataList.Count;
|
|||
|
|
if (pathDataLocationListCount <= 1)
|
|||
|
|
{
|
|||
|
|
// One or no points in the path, so this is not a valid path for curvature to be determined
|
|||
|
|
return false;
|
|||
|
|
}
|
|||
|
|
else if (lastPathPointIndex < pathDataLocationListCount)
|
|||
|
|
{
|
|||
|
|
// Multiple points in the path, so perform the algorithm
|
|||
|
|
// Get the index of the next path point
|
|||
|
|
int nextPathPointIndex = SSCManager.GetNextPathLocationIndex(pathData, lastPathPointIndex, true);
|
|||
|
|
|
|||
|
|
// If there is no next assigned point, return false
|
|||
|
|
if (nextPathPointIndex < 0) { return false; }
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
// Call the internal algorithm function to obtain the second derivative (which will be in the
|
|||
|
|
// direction of the normal)
|
|||
|
|
GetPathSecondDerivativeInternal(pathData, lastPathPointIndex, nextPathPointIndex, tValue, ref pathNormal);
|
|||
|
|
// Normalised the calculated normal
|
|||
|
|
pathNormal /= (float)System.Math.Sqrt((pathNormal.x * pathNormal.x) +
|
|||
|
|
(pathNormal.y * pathNormal.y) + (pathNormal.z * pathNormal.z));
|
|||
|
|
return true;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
else { return false; }
|
|||
|
|
}
|
|||
|
|
else { return false; }
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/// <summary>
|
|||
|
|
/// Calculates the curvature of the path given the index of the last path point and the t-value.
|
|||
|
|
/// The curvature is the reciprocal of the radius of curvature (i.e. radius of curvature = 1 / curvature).
|
|||
|
|
/// The pathCurvature variable is set to the radius of curvature.
|
|||
|
|
/// If the return value is true, the operation was successful.
|
|||
|
|
/// </summary>
|
|||
|
|
/// <param name="pathData"></param>
|
|||
|
|
/// <param name="lastPathPointIndex"></param>
|
|||
|
|
/// <param name="tValue"></param>
|
|||
|
|
/// <param name="pathCurvature"></param>
|
|||
|
|
/// <returns></returns>
|
|||
|
|
public static bool GetPathCurvature(PathData pathData, int lastPathPointIndex, float tValue, ref float pathCurvature)
|
|||
|
|
{
|
|||
|
|
// Check that the path is valid
|
|||
|
|
if (pathData != null && pathData.pathLocationDataList != null)
|
|||
|
|
{
|
|||
|
|
// Check how many points there are in the path
|
|||
|
|
int pathDataLocationListCount = pathData.pathLocationDataList.Count;
|
|||
|
|
if (pathDataLocationListCount <= 1)
|
|||
|
|
{
|
|||
|
|
// One or no points in the path, so this is not a valid path for curvature to be determined
|
|||
|
|
return false;
|
|||
|
|
}
|
|||
|
|
else if (lastPathPointIndex < pathDataLocationListCount)
|
|||
|
|
{
|
|||
|
|
// Multiple points in the path, so perform the algorithm
|
|||
|
|
// Get the index of the next path point
|
|||
|
|
int nextPathPointIndex = SSCManager.GetNextPathLocationIndex(pathData, lastPathPointIndex, true);
|
|||
|
|
|
|||
|
|
// If there is no next assigned point, return false
|
|||
|
|
if (nextPathPointIndex < 0) { return false; }
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
// Call the internal algorithm function
|
|||
|
|
GetPathCurvatureInternal(pathData, lastPathPointIndex, nextPathPointIndex, tValue, ref pathCurvature);
|
|||
|
|
return true;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
else { return false; }
|
|||
|
|
}
|
|||
|
|
else { return false; }
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/// <summary>
|
|||
|
|
/// Calculates the curvature of the path (in a given plane defined by the plane normal) given the index of the
|
|||
|
|
/// last path point and the t-value.
|
|||
|
|
/// The curvature is the reciprocal of the radius of curvature (i.e. radius of curvature = 1 / curvature).
|
|||
|
|
/// The vector planeNormal passed in must be normalised.
|
|||
|
|
/// The pathCurvature variable is set to the radius of curvature.
|
|||
|
|
/// If the return value is true, the operation was successful.
|
|||
|
|
/// </summary>
|
|||
|
|
/// <param name="pathData"></param>
|
|||
|
|
/// <param name="lastPathPointIndex"></param>
|
|||
|
|
/// <param name="tValue"></param>
|
|||
|
|
/// <param name="pathCurvature"></param>
|
|||
|
|
/// <returns></returns>
|
|||
|
|
public static bool GetPathCurvatureInPlane(PathData pathData, int lastPathPointIndex, float tValue, Vector3 planeNormal, ref float pathCurvature)
|
|||
|
|
{
|
|||
|
|
// Check that the path is valid
|
|||
|
|
if (pathData != null && pathData.pathLocationDataList != null)
|
|||
|
|
{
|
|||
|
|
// Check how many points there are in the path
|
|||
|
|
int pathDataLocationListCount = pathData.pathLocationDataList.Count;
|
|||
|
|
if (pathDataLocationListCount <= 1)
|
|||
|
|
{
|
|||
|
|
// One or no points in the path, so this is not a valid path for curvature to be determined
|
|||
|
|
return false;
|
|||
|
|
}
|
|||
|
|
else if (lastPathPointIndex < pathDataLocationListCount)
|
|||
|
|
{
|
|||
|
|
// Multiple points in the path, so perform the algorithm
|
|||
|
|
// Get the index of the next path point
|
|||
|
|
int nextPathPointIndex = SSCManager.GetNextPathLocationIndex(pathData, lastPathPointIndex, true);
|
|||
|
|
|
|||
|
|
// If there is no next assigned point, return false
|
|||
|
|
if (nextPathPointIndex < 0) { return false; }
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
// Call the internal algorithm function
|
|||
|
|
GetPathCurvatureInPlaneInternal(pathData, lastPathPointIndex, nextPathPointIndex, tValue, planeNormal, ref pathCurvature);
|
|||
|
|
return true;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
else { return false; }
|
|||
|
|
}
|
|||
|
|
else { return false; }
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/// <summary>
|
|||
|
|
/// Calculates the velocity of a point on the path, given the position of the point on the path.
|
|||
|
|
/// The velocity variable is set to the velocity of the point.
|
|||
|
|
/// If the return value is true, the operation was successful.
|
|||
|
|
/// </summary>
|
|||
|
|
/// <param name="pathData"></param>
|
|||
|
|
/// <param name="pointOnPath"></param>
|
|||
|
|
/// <param name="velocity"></param>
|
|||
|
|
/// <returns></returns>
|
|||
|
|
public static bool GetPathVelocity (PathData pathData, Vector3 pointOnPath, ref Vector3 velocity)
|
|||
|
|
{
|
|||
|
|
// Check that the path is valid
|
|||
|
|
if (pathData != null)
|
|||
|
|
{
|
|||
|
|
// The path anchorPoint is typically a ShipDockingStation's current position.
|
|||
|
|
// The velocity consists of two components:
|
|||
|
|
// 1. The component due to the velocity of the path
|
|||
|
|
// 2. The component due to the angular velocity of the path
|
|||
|
|
velocity = pathData.worldVelocity + Vector3.Cross(pathData.worldAngularVelocity, pointOnPath - pathData.anchorPoint);
|
|||
|
|
return true;
|
|||
|
|
}
|
|||
|
|
else { return false; }
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
#endregion
|
|||
|
|
|
|||
|
|
#region Complex Functions
|
|||
|
|
|
|||
|
|
/// <summary>
|
|||
|
|
/// Gets the position and t-value of the closest point on the path to the target point, given the last path point index.
|
|||
|
|
/// The closestPointOnPath variable is set to the position of the closest point.
|
|||
|
|
/// The closestPointOnPathTValue variable is set to the t-value of the closest point.
|
|||
|
|
/// If the return value is true, the operation was successful.
|
|||
|
|
/// </summary>
|
|||
|
|
/// <param name="pathData"></param>
|
|||
|
|
/// <param name="lastPathPointIndex"></param>
|
|||
|
|
/// <param name="targetPoint"></param>
|
|||
|
|
/// <param name="closestPointOnPath"></param>
|
|||
|
|
/// <param name="closestPointOnPathTValue"></param>
|
|||
|
|
/// <returns></returns>
|
|||
|
|
public static bool FindClosestPointOnPath (PathData pathData, int lastPathPointIndex, Vector3 targetPoint, ref Vector3 closestPointOnPath, ref float closestPointOnPathTValue)
|
|||
|
|
{
|
|||
|
|
// Check that the path is valid
|
|||
|
|
if (pathData != null && pathData.pathLocationDataList != null)
|
|||
|
|
{
|
|||
|
|
// Check how many points there are in the path
|
|||
|
|
int pathDataLocationListCount = pathData.pathLocationDataList.Count;
|
|||
|
|
if (pathDataLocationListCount < 1)
|
|||
|
|
{
|
|||
|
|
// No points in the path, so this is not a valid path
|
|||
|
|
return false;
|
|||
|
|
}
|
|||
|
|
else if (pathDataLocationListCount == 1)
|
|||
|
|
{
|
|||
|
|
// Only one point in the path, so simply return 0 and the (only) Location
|
|||
|
|
closestPointOnPath = pathData.pathLocationDataList[0].locationData.position;
|
|||
|
|
closestPointOnPathTValue = 0f;
|
|||
|
|
return true;
|
|||
|
|
}
|
|||
|
|
else if (lastPathPointIndex < pathDataLocationListCount)
|
|||
|
|
{
|
|||
|
|
// Multiple points in the path, so perform the algorithm
|
|||
|
|
|
|||
|
|
// Get the index of the next path point
|
|||
|
|
int nextPathPointIndex = SSCManager.GetNextPathLocationIndex(pathData, lastPathPointIndex, true);
|
|||
|
|
|
|||
|
|
// If there is no next assigned point, return false
|
|||
|
|
if (nextPathPointIndex < 0) { return false; }
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
// First, do a rough check to find the neighbourhood of t-values we want to look at
|
|||
|
|
// Iterate through a number of evenly spaced t-values and find the closest one
|
|||
|
|
int roughTIterations = 5;
|
|||
|
|
float tValueIncrementSize = 1f / roughTIterations;
|
|||
|
|
closestPointOnPathTValue = 0f;
|
|||
|
|
float currentTValue = 0f;
|
|||
|
|
float closestPointOnPathSqrDist = Mathf.Infinity;
|
|||
|
|
float thisPointSqrDist;
|
|||
|
|
for (int i = 0; i <= roughTIterations; i++)
|
|||
|
|
{
|
|||
|
|
// Find the path point associated with this t-value
|
|||
|
|
GetPointOnPathInternal(pathData, lastPathPointIndex, nextPathPointIndex, currentTValue, ref thisPointOnPath);
|
|||
|
|
// Calculate the square of the distance from the this path point to the target point
|
|||
|
|
thisPointSqrDist = (thisPointOnPath.x - targetPoint.x) * (thisPointOnPath.x - targetPoint.x) +
|
|||
|
|
(thisPointOnPath.y - targetPoint.y) * (thisPointOnPath.y - targetPoint.y) +
|
|||
|
|
(thisPointOnPath.z - targetPoint.z) * (thisPointOnPath.z - targetPoint.z);
|
|||
|
|
// Compare the square distance with the current closest point
|
|||
|
|
if (thisPointSqrDist < closestPointOnPathSqrDist)
|
|||
|
|
{
|
|||
|
|
// If this point is closer than the current closest point, set it as the new closest point
|
|||
|
|
closestPointOnPath = thisPointOnPath;
|
|||
|
|
closestPointOnPathTValue = currentTValue;
|
|||
|
|
closestPointOnPathSqrDist = thisPointSqrDist;
|
|||
|
|
}
|
|||
|
|
// Increment the t-value
|
|||
|
|
currentTValue += tValueIncrementSize;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// Next, use a binary search to more accurately determine the t-value
|
|||
|
|
// Binary search algorithm:
|
|||
|
|
// Start with an initial search window size of half the rough T iteration size
|
|||
|
|
// Each iteration, search the path points at (t - search window size) and (t + search window size)
|
|||
|
|
// with "t" being the current t-value
|
|||
|
|
// - If either of these points is a closer path point, replace t with the new closest t-value
|
|||
|
|
// - Otherwise halve the search window size
|
|||
|
|
// Stop searching when the search window reaches a certain threshold size
|
|||
|
|
|
|||
|
|
float tSearchWindowSize = tValueIncrementSize * 0.5f;
|
|||
|
|
// TODO specify externally
|
|||
|
|
float tAccuracy = 0.0001f;
|
|||
|
|
|
|||
|
|
while (tSearchWindowSize > tAccuracy)
|
|||
|
|
{
|
|||
|
|
// Initially assume we will not find a closer path point
|
|||
|
|
bool foundCloserPathPoint = false;
|
|||
|
|
float newClosestTValue = closestPointOnPathTValue;
|
|||
|
|
|
|||
|
|
// Find the path point slightly further ahead of this t-value
|
|||
|
|
currentTValue = closestPointOnPathTValue + tSearchWindowSize;
|
|||
|
|
if (currentTValue > 1f) { currentTValue = 1f; }
|
|||
|
|
GetPointOnPathInternal(pathData, lastPathPointIndex, nextPathPointIndex, currentTValue, ref thisPointOnPath);
|
|||
|
|
// Calculate the square of the distance from this path point to the target point
|
|||
|
|
thisPointSqrDist = (thisPointOnPath.x - targetPoint.x) * (thisPointOnPath.x - targetPoint.x) +
|
|||
|
|
(thisPointOnPath.y - targetPoint.y) * (thisPointOnPath.y - targetPoint.y) +
|
|||
|
|
(thisPointOnPath.z - targetPoint.z) * (thisPointOnPath.z - targetPoint.z);
|
|||
|
|
// Compare the square distance with the current closest point
|
|||
|
|
if (thisPointSqrDist < closestPointOnPathSqrDist)
|
|||
|
|
{
|
|||
|
|
// If this point is closer than the current closest point, set it as the new closest point
|
|||
|
|
newClosestTValue = currentTValue;
|
|||
|
|
closestPointOnPath = thisPointOnPath;
|
|||
|
|
closestPointOnPathSqrDist = thisPointSqrDist;
|
|||
|
|
foundCloserPathPoint = true;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// Find the path point slightly further behind this t-value
|
|||
|
|
currentTValue = closestPointOnPathTValue - tSearchWindowSize;
|
|||
|
|
if (currentTValue < 0f) { currentTValue = 0f; }
|
|||
|
|
GetPointOnPathInternal(pathData, lastPathPointIndex, nextPathPointIndex, currentTValue, ref thisPointOnPath);
|
|||
|
|
/// Calculate the square of the distance from the this path point to the target point
|
|||
|
|
thisPointSqrDist = (thisPointOnPath.x - targetPoint.x) * (thisPointOnPath.x - targetPoint.x) +
|
|||
|
|
(thisPointOnPath.y - targetPoint.y) * (thisPointOnPath.y - targetPoint.y) +
|
|||
|
|
(thisPointOnPath.z - targetPoint.z) * (thisPointOnPath.z - targetPoint.z);
|
|||
|
|
// Compare the square distance with the current closest point
|
|||
|
|
if (thisPointSqrDist < closestPointOnPathSqrDist)
|
|||
|
|
{
|
|||
|
|
// If this point is closer than the current closest point, set it as the new closest point
|
|||
|
|
newClosestTValue = currentTValue;
|
|||
|
|
closestPointOnPath = thisPointOnPath;
|
|||
|
|
closestPointOnPathSqrDist = thisPointSqrDist;
|
|||
|
|
foundCloserPathPoint = true;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if (!foundCloserPathPoint)
|
|||
|
|
{
|
|||
|
|
// If neither of the path points we checked were closer then the closest path point,
|
|||
|
|
// halve the search window size
|
|||
|
|
tSearchWindowSize *= 0.5f;
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
// If we found a new closest path point, replace the old closest t-value with the new one
|
|||
|
|
closestPointOnPathTValue = newClosestTValue;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
return true;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
else { return false; }
|
|||
|
|
}
|
|||
|
|
else { return false; }
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/// <summary>
|
|||
|
|
/// Gets the position, t-value, lastPathPointIndex of the closest point on the path to the target point.
|
|||
|
|
/// The closestPointOnPath variable is set to the position of the closest point.
|
|||
|
|
/// The closestPointOnPathTValue variable is set to the t-value of the closest point.
|
|||
|
|
/// The lastPathPointIndex variable is set to the previous PathLocationData index on the path.
|
|||
|
|
/// If the return value is true, the operation was successful.
|
|||
|
|
/// NOTE: This should only be used if you do not know the lastPathPointIndex closest to the targetPoint.
|
|||
|
|
/// </summary>
|
|||
|
|
/// <param name="pathData"></param>
|
|||
|
|
/// <param name="targetPoint"></param>
|
|||
|
|
/// <param name="lastPathPointIndex"></param>
|
|||
|
|
/// <param name="closestPointOnPath"></param>
|
|||
|
|
/// <param name="closestPointOnPathTValue"></param>
|
|||
|
|
/// <returns></returns>
|
|||
|
|
public static bool FindClosestPointOnPath(PathData pathData, Vector3 targetPoint, ref Vector3 closestPointOnPath, ref float closestPointOnPathTValue, ref int lastPathPointIndex)
|
|||
|
|
{
|
|||
|
|
// Check how many points there are in the path
|
|||
|
|
int pathDataLocationListCount = pathData != null && pathData.pathLocationDataList != null ? pathData.pathLocationDataList.Count : 0;
|
|||
|
|
|
|||
|
|
if (pathDataLocationListCount < 1) { return false; }
|
|||
|
|
else if (pathDataLocationListCount == 1)
|
|||
|
|
{
|
|||
|
|
// Only one point in the path, so simply return 0 and the (only) Location
|
|||
|
|
closestPointOnPath = pathData.pathLocationDataList[0].locationData.position;
|
|||
|
|
closestPointOnPathTValue = 0f;
|
|||
|
|
return true;
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
float sqrMinDistance = float.MaxValue;
|
|||
|
|
int closestPathPointIndex = -1;
|
|||
|
|
|
|||
|
|
// Find the closest user defined Location
|
|||
|
|
|
|||
|
|
// Start loop with the index of the next assigned Location in the Path
|
|||
|
|
for (int currentPathPointIndex = SSCManager.GetNextPathLocationIndex(pathData, -1, false); currentPathPointIndex >= 0 && currentPathPointIndex < pathDataLocationListCount;)
|
|||
|
|
{
|
|||
|
|
Vector3 locationPosition = pathData.pathLocationDataList[currentPathPointIndex].locationData.position;
|
|||
|
|
|
|||
|
|
float sqrDistToLocation = (locationPosition.x - targetPoint.x) * (locationPosition.x - targetPoint.x) +
|
|||
|
|
(locationPosition.y - targetPoint.y) * (locationPosition.y - targetPoint.y) +
|
|||
|
|
(locationPosition.z - targetPoint.z) * (locationPosition.z - targetPoint.z);
|
|||
|
|
|
|||
|
|
// Is this the closest Location to the target so far?
|
|||
|
|
if (sqrDistToLocation < sqrMinDistance)
|
|||
|
|
{
|
|||
|
|
sqrMinDistance = sqrDistToLocation;
|
|||
|
|
closestPathPointIndex = currentPathPointIndex;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// Get the index of the next assigned Location in the Path
|
|||
|
|
currentPathPointIndex = SSCManager.GetNextPathLocationIndex(pathData, currentPathPointIndex, false);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// Did we find a closest Location?
|
|||
|
|
if (closestPathPointIndex >= 0)
|
|||
|
|
{
|
|||
|
|
// Get the Locations either side of the closest one
|
|||
|
|
int prevPathPointIndex = SSCManager.GetPreviousPathLocationIndex(pathData, closestPathPointIndex, pathData.isClosedCircuit);
|
|||
|
|
int nextPathPointIndex = SSCManager.GetNextPathLocationIndex(pathData, closestPathPointIndex, pathData.isClosedCircuit);
|
|||
|
|
|
|||
|
|
// Check which Location is the closest
|
|||
|
|
float sqrDistToPrevious = float.MaxValue;
|
|||
|
|
float sqrDistToNext = float.MaxValue;
|
|||
|
|
|
|||
|
|
if (prevPathPointIndex >= 0 && prevPathPointIndex < pathDataLocationListCount)
|
|||
|
|
{
|
|||
|
|
Vector3 locationPosition = pathData.pathLocationDataList[prevPathPointIndex].locationData.position;
|
|||
|
|
|
|||
|
|
sqrDistToPrevious = (locationPosition.x - targetPoint.x) * (locationPosition.x - targetPoint.x) +
|
|||
|
|
(locationPosition.y - targetPoint.y) * (locationPosition.y - targetPoint.y) +
|
|||
|
|
(locationPosition.z - targetPoint.z) * (locationPosition.z - targetPoint.z);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if (nextPathPointIndex >= 0 && nextPathPointIndex < pathDataLocationListCount)
|
|||
|
|
{
|
|||
|
|
Vector3 locationPosition = pathData.pathLocationDataList[nextPathPointIndex].locationData.position;
|
|||
|
|
|
|||
|
|
sqrDistToNext = (locationPosition.x - targetPoint.x) * (locationPosition.x - targetPoint.x) +
|
|||
|
|
(locationPosition.y - targetPoint.y) * (locationPosition.y - targetPoint.y) +
|
|||
|
|
(locationPosition.z - targetPoint.z) * (locationPosition.z - targetPoint.z);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if (sqrDistToPrevious < sqrDistToNext) { lastPathPointIndex = prevPathPointIndex; }
|
|||
|
|
else { lastPathPointIndex = closestPathPointIndex; }
|
|||
|
|
|
|||
|
|
return FindClosestPointOnPath(pathData, lastPathPointIndex, targetPoint, ref closestPointOnPath, ref closestPointOnPathTValue);
|
|||
|
|
}
|
|||
|
|
else { return false; }
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/// <summary>
|
|||
|
|
/// Gets the position, curvature and float path point index of a point on the path that is added distance ahead of a
|
|||
|
|
/// given other point on the path, specified by the index of the last path point and the t-value, and using approximately
|
|||
|
|
/// approximateIterations iterations.
|
|||
|
|
/// The newPointOnPath variable is set to the position of the calculated point.
|
|||
|
|
/// The pathCurvature variable is set to the curvature of the path at the calculated point.
|
|||
|
|
/// The newPointOnPathLastPathPointIndex variable is set to the index of the last path point before the calculated point.
|
|||
|
|
/// The newPointOnPathTValue variable is set to the t-value of the calculated point.
|
|||
|
|
/// If the return value is true, the operation was successful.
|
|||
|
|
/// </summary>
|
|||
|
|
/// <param name="pathData"></param>
|
|||
|
|
/// <param name="lastPathPointIndex"></param>
|
|||
|
|
/// <param name="tValue"></param>
|
|||
|
|
/// <param name="addedDistance"></param>
|
|||
|
|
/// <param name="newPointOnPath"></param>
|
|||
|
|
/// <param name="pathCurvature"></param>
|
|||
|
|
/// <param name="newPointOnPathLastPathPointIndex"></param>
|
|||
|
|
/// <param name="newPointOnPathTValue"></param>
|
|||
|
|
/// <returns></returns>
|
|||
|
|
public static bool GetFurtherPointOnPathData (PathData pathData, int lastPathPointIndex, float tValue, float addedDistance,
|
|||
|
|
int approximateIterations, ref Vector3 newPointOnPath, ref float pathCurvature, ref int newPointOnPathLastPathPointIndex,
|
|||
|
|
ref float newPointOnPathTValue)
|
|||
|
|
{
|
|||
|
|
// Check that the path is valid
|
|||
|
|
if (pathData != null && pathData.pathLocationDataList != null)
|
|||
|
|
{
|
|||
|
|
// Check how many points there are in the path
|
|||
|
|
int pathDataLocationListCount = pathData.pathLocationDataList.Count;
|
|||
|
|
if (pathDataLocationListCount <= 1)
|
|||
|
|
{
|
|||
|
|
// One or no points in the path, so this is not a valid path for distance to be added
|
|||
|
|
return false;
|
|||
|
|
}
|
|||
|
|
else if (lastPathPointIndex < pathDataLocationListCount)
|
|||
|
|
{
|
|||
|
|
// Multiple points in the path, so perform the algorithm
|
|||
|
|
|
|||
|
|
if (addedDistance > Mathf.Epsilon)
|
|||
|
|
{
|
|||
|
|
// Step 1: Determine which two points this new point on the path will be between
|
|||
|
|
// There are two possible scenarios:
|
|||
|
|
// - Case 1: The new point on the path is between the last point and the next point
|
|||
|
|
// - Case 2: The new point on the path is somewhere after the next point
|
|||
|
|
|
|||
|
|
// Get the index of the next path point
|
|||
|
|
int nextPathPointIndex = SSCManager.GetNextPathLocationIndex(pathData, lastPathPointIndex, true);
|
|||
|
|
|
|||
|
|
// If there is no next assigned point, return false
|
|||
|
|
if (nextPathPointIndex < 0) { return false; }
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
// TODO allow the distance from the current point on the path to the next path point to be passed in
|
|||
|
|
// and also passed out, to save calculation
|
|||
|
|
bool reachedEndOfPath = false;
|
|||
|
|
// For the first point, calculate the distance from the current point on the path to the next path point
|
|||
|
|
// using 10 line segments
|
|||
|
|
float distanceToNextPathPoint = 0f;
|
|||
|
|
float startTValue = tValue;
|
|||
|
|
GetDistanceBetweenTValuesInternal(pathData, lastPathPointIndex, nextPathPointIndex, startTValue, 1f, 10, ref distanceToNextPathPoint);
|
|||
|
|
// If the added distance is greater than the distance to the next path point, iteratively search
|
|||
|
|
// For the two path points the added distance point will be between
|
|||
|
|
while (distanceToNextPathPoint < addedDistance)
|
|||
|
|
{
|
|||
|
|
// If this path is not a closed circuit, don't allow going past the last path point
|
|||
|
|
if (!pathData.isClosedCircuit &&
|
|||
|
|
(nextPathPointIndex == SSCManager.GetLastAssignedLocationIdx(pathData) ||
|
|||
|
|
nextPathPointIndex == SSCManager.GetFirstAssignedLocationIdx(pathData)))
|
|||
|
|
{
|
|||
|
|
distanceToNextPathPoint = addedDistance + 1f;
|
|||
|
|
reachedEndOfPath = true;
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
// Subtract the distance to this point from the added distance
|
|||
|
|
addedDistance -= distanceToNextPathPoint;
|
|||
|
|
// Recaclulate the last and next path points
|
|||
|
|
lastPathPointIndex = SSCManager.GetNextPathLocationIndex(pathData, lastPathPointIndex, true);
|
|||
|
|
nextPathPointIndex = SSCManager.GetNextPathLocationIndex(pathData, lastPathPointIndex, true);
|
|||
|
|
// Get the distance between the new last and next path points
|
|||
|
|
distanceToNextPathPoint = pathData.pathLocationDataList[nextPathPointIndex].distanceFromPreviousLocation;
|
|||
|
|
// Start t-value for all later path points is always zero
|
|||
|
|
startTValue = 0f;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if (!reachedEndOfPath)
|
|||
|
|
{
|
|||
|
|
// Step 2: Determine the new point on the path at the given distance
|
|||
|
|
// Calculate (roughly) how much to increment the t-value each time, based on a given number of segments to use
|
|||
|
|
// Then iteratively calculate the length from each point to the next, until the total length exceeds the
|
|||
|
|
// added distance. Then interpolate between this point and the last point to get the new point on the path
|
|||
|
|
|
|||
|
|
// Calculate the t-value we expect the new point on the path to have
|
|||
|
|
// (using the assumption that t-values are linear)
|
|||
|
|
float expectedTValue = startTValue + ((1f - startTValue) * (addedDistance / distanceToNextPathPoint));
|
|||
|
|
|
|||
|
|
// Assume that we will go about as far as the expected t-value (from the start t-value),
|
|||
|
|
// and calculate a t-value increment size that will reach that t-value in the given number of iterations
|
|||
|
|
float tValueIncrementSize = (expectedTValue - startTValue) / approximateIterations;
|
|||
|
|
// Don't allow t-increment sizes less than 0.001
|
|||
|
|
if (tValueIncrementSize < 0.001f) { tValueIncrementSize = 0.001f; }
|
|||
|
|
|
|||
|
|
// Get the initial point on the path and the t-value of the next point on the path
|
|||
|
|
GetPointOnPathInternal(pathData, lastPathPointIndex, nextPathPointIndex, startTValue, ref previousPointOnPath);
|
|||
|
|
float thisPointOnPathTValue = startTValue + tValueIncrementSize;
|
|||
|
|
|
|||
|
|
// Loop through until we find the new point on the path
|
|||
|
|
bool foundNewPointOnPath = false;
|
|||
|
|
float totalDistance = 0f;
|
|||
|
|
while (!foundNewPointOnPath)
|
|||
|
|
{
|
|||
|
|
// Get the next point on the path
|
|||
|
|
GetPointOnPathInternal(pathData, lastPathPointIndex, nextPathPointIndex, thisPointOnPathTValue, ref thisPointOnPath);
|
|||
|
|
// Calculate the distance from the previous path point to this path point
|
|||
|
|
float distanceBetweenPointsOnPath = (float)System.Math.Sqrt(
|
|||
|
|
(thisPointOnPath.x - previousPointOnPath.x) * (thisPointOnPath.x - previousPointOnPath.x) +
|
|||
|
|
(thisPointOnPath.y - previousPointOnPath.y) * (thisPointOnPath.y - previousPointOnPath.y) +
|
|||
|
|
(thisPointOnPath.z - previousPointOnPath.z) * (thisPointOnPath.z - previousPointOnPath.z));
|
|||
|
|
// Add this distance to the total distance
|
|||
|
|
totalDistance += distanceBetweenPointsOnPath;
|
|||
|
|
// Compare the new total distance to the added distance
|
|||
|
|
if (totalDistance > addedDistance)
|
|||
|
|
{
|
|||
|
|
// If we have now gone further than the added distance, we know that the new point on the path will
|
|||
|
|
// be somewhere between the previous path point and this path point.
|
|||
|
|
// So simply linearly interpolate t-values between the two path points to give the t-value
|
|||
|
|
// of the new point on the path
|
|||
|
|
newPointOnPathTValue = thisPointOnPathTValue - (tValueIncrementSize *
|
|||
|
|
((totalDistance - addedDistance) / distanceBetweenPointsOnPath));
|
|||
|
|
foundNewPointOnPath = true;
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
// We have not yet gone further than the added distance, so we are still iterating
|
|||
|
|
// Increment the t-value
|
|||
|
|
thisPointOnPathTValue += tValueIncrementSize;
|
|||
|
|
if (thisPointOnPathTValue > 1f)
|
|||
|
|
{
|
|||
|
|
// If we have now gone further than the end of this section of the path (past the next path point),
|
|||
|
|
// we know that the new point on the path will be somewhere between this path point and the end of this
|
|||
|
|
// section of the path.
|
|||
|
|
// So simply linearly interpolate t-values between this path point and the end of this section of the path
|
|||
|
|
// to give the t-value of the new point on the path
|
|||
|
|
newPointOnPathTValue = 1f + ((1f - (thisPointOnPathTValue - tValueIncrementSize)) *
|
|||
|
|
((distanceToNextPathPoint - addedDistance) / (distanceBetweenPointsOnPath - totalDistance + distanceToNextPathPoint)));
|
|||
|
|
foundNewPointOnPath = true;
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
// Set this path point to be the previous path point
|
|||
|
|
previousPointOnPath = thisPointOnPath;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
// If we have reached the end of the path, simply use the last path point
|
|||
|
|
nextPathPointIndex = SSCManager.GetLastAssignedLocationIdx(pathData);
|
|||
|
|
lastPathPointIndex = SSCManager.GetPreviousPathLocationIndex(pathData, nextPathPointIndex, false);
|
|||
|
|
newPointOnPathTValue = 1f;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// Get the extra point on path data - position, curvature, last path point index
|
|||
|
|
GetPointOnPathInternal(pathData, lastPathPointIndex, nextPathPointIndex, newPointOnPathTValue, ref newPointOnPath);
|
|||
|
|
GetPathCurvatureInternal(pathData, lastPathPointIndex, nextPathPointIndex, newPointOnPathTValue, ref pathCurvature);
|
|||
|
|
newPointOnPathLastPathPointIndex = lastPathPointIndex;
|
|||
|
|
|
|||
|
|
return true;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
// Added distance is 0, so simply return the passed in path point
|
|||
|
|
newPointOnPathLastPathPointIndex = lastPathPointIndex;
|
|||
|
|
newPointOnPathTValue = tValue;
|
|||
|
|
// Get the index of the next path point
|
|||
|
|
int nextPathPointIndex = SSCManager.GetNextPathLocationIndex(pathData, lastPathPointIndex, true);
|
|||
|
|
// Get the point on the path and the curvature
|
|||
|
|
GetPointOnPathInternal(pathData, lastPathPointIndex, nextPathPointIndex, newPointOnPathTValue, ref newPointOnPath);
|
|||
|
|
GetPathCurvatureInternal(pathData, lastPathPointIndex, nextPathPointIndex, newPointOnPathTValue, ref pathCurvature);
|
|||
|
|
return true;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
else { return false; }
|
|||
|
|
}
|
|||
|
|
else { return false; }
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
#endregion
|
|||
|
|
|
|||
|
|
#region Path Computation Functions
|
|||
|
|
|
|||
|
|
/// <summary>
|
|||
|
|
/// Calculates the distance between two (consecutive) path points given the index of the last path point and the number of
|
|||
|
|
/// line segments to use during the calculation. More line segments will give a more accurate distance.
|
|||
|
|
/// The distanceBetweenPathPoints variable is set to the distance between the path points.
|
|||
|
|
/// If the return value is true, the operation was successful.
|
|||
|
|
/// </summary>
|
|||
|
|
/// <param name="pathData"></param>
|
|||
|
|
/// <param name="lastPathPointIndex"></param>
|
|||
|
|
/// <param name="pathSegments"></param>
|
|||
|
|
/// <param name="distanceBetweenPathPoints"></param>
|
|||
|
|
/// <returns></returns>
|
|||
|
|
public static bool GetDistanceBetweenPathPoints(PathData pathData, int lastPathPointIndex, int lineSegments, ref float distanceBetweenPathPoints)
|
|||
|
|
{
|
|||
|
|
// Check that the path is valid
|
|||
|
|
if (pathData != null && pathData.pathLocationDataList != null)
|
|||
|
|
{
|
|||
|
|
// Check how many points there are in the path
|
|||
|
|
int pathDataLocationListCount = pathData.pathLocationDataList.Count;
|
|||
|
|
if (pathDataLocationListCount <= 1)
|
|||
|
|
{
|
|||
|
|
// One or no points in the path, so this is not a valid path for distance to be calculated
|
|||
|
|
return false;
|
|||
|
|
}
|
|||
|
|
else if (lastPathPointIndex < pathDataLocationListCount)
|
|||
|
|
{
|
|||
|
|
// Multiple points in the path, so perform the algorithm
|
|||
|
|
// Get the index of the next path point
|
|||
|
|
int nextPathPointIndex = SSCManager.GetNextPathLocationIndex(pathData, lastPathPointIndex, true);
|
|||
|
|
|
|||
|
|
// If there is no next assigned point, return false
|
|||
|
|
if (nextPathPointIndex < 0) { return false; }
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
// Call the internal algorithm function
|
|||
|
|
GetDistanceBetweenTValuesInternal(pathData, lastPathPointIndex, nextPathPointIndex, 0f, 1f, lineSegments, ref distanceBetweenPathPoints);
|
|||
|
|
|
|||
|
|
return true;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
else { return false; }
|
|||
|
|
}
|
|||
|
|
else { return false; }
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
#endregion
|
|||
|
|
|
|||
|
|
#endregion
|
|||
|
|
|
|||
|
|
#region (Internal) Private Static Member Methods
|
|||
|
|
|
|||
|
|
// Path variables
|
|||
|
|
private static Vector3 pathPoint0 = Vector3.zero;
|
|||
|
|
private static Vector3 pathPoint1 = Vector3.zero;
|
|||
|
|
private static Vector3 pathPoint2 = Vector3.zero;
|
|||
|
|
private static Vector3 pathPoint3 = Vector3.zero;
|
|||
|
|
private static Vector3 thisPathPointFirstDerivative = Vector3.zero;
|
|||
|
|
private static Vector3 thisPathPointSecondDerivative = Vector3.zero;
|
|||
|
|
|
|||
|
|
/// <summary>
|
|||
|
|
/// Calculates the point on the path given the index of the last path point, the index of the next path point and the t-value.
|
|||
|
|
/// The pointOnPath variable is set to the point on the path.
|
|||
|
|
/// </summary>
|
|||
|
|
/// <param name="pathData"></param>
|
|||
|
|
/// <param name="lastPathPointIndex"></param>
|
|||
|
|
/// <param name="nextPathPointIndex"></param>
|
|||
|
|
/// <param name="tValue"></param>
|
|||
|
|
/// <param name="pointOnPath"></param>
|
|||
|
|
private static void GetPointOnPathInternal (PathData pathData, int lastPathPointIndex, int nextPathPointIndex, float tValue,
|
|||
|
|
ref Vector3 pointOnPath)
|
|||
|
|
{
|
|||
|
|
// Get path points
|
|||
|
|
pathPoint0 = pathData.pathLocationDataList[lastPathPointIndex].locationData.position;
|
|||
|
|
pathPoint3 = pathData.pathLocationDataList[nextPathPointIndex].locationData.position;
|
|||
|
|
// Get control points
|
|||
|
|
pathPoint1 = pathData.pathLocationDataList[lastPathPointIndex].outControlPoint;
|
|||
|
|
pathPoint2 = pathData.pathLocationDataList[nextPathPointIndex].inControlPoint;
|
|||
|
|
// Calculate new path point
|
|||
|
|
// Bezier curve point: B = (1 - t)^3 * p0 + 3 * (1 - t)^2 * t + p1 + 3 * (1 - t) * t^2 * p2 + t^3 * p3
|
|||
|
|
// Do it component-wise to improve performance
|
|||
|
|
pointOnPath.x = (1f - tValue) * (1f - tValue) * (1f - tValue) * pathPoint0.x +
|
|||
|
|
3f * (1f - tValue) * (1f - tValue) * tValue * pathPoint1.x +
|
|||
|
|
3f * (1f - tValue) * tValue * tValue * pathPoint2.x +
|
|||
|
|
tValue * tValue * tValue * pathPoint3.x;
|
|||
|
|
pointOnPath.y = (1f - tValue) * (1f - tValue) * (1f - tValue) * pathPoint0.y +
|
|||
|
|
3f * (1f - tValue) * (1f - tValue) * tValue * pathPoint1.y +
|
|||
|
|
3f * (1f - tValue) * tValue * tValue * pathPoint2.y +
|
|||
|
|
tValue * tValue * tValue * pathPoint3.y;
|
|||
|
|
pointOnPath.z = (1f - tValue) * (1f - tValue) * (1f - tValue) * pathPoint0.z +
|
|||
|
|
3f * (1f - tValue) * (1f - tValue) * tValue * pathPoint1.z +
|
|||
|
|
3f * (1f - tValue) * tValue * tValue * pathPoint2.z +
|
|||
|
|
tValue * tValue * tValue * pathPoint3.z;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/// <summary>
|
|||
|
|
/// Calculates the first derivative of the path given the index of the last path point, the index of the next path point and the t-value.
|
|||
|
|
/// The pathFirstDerivative variable is set to the first derivative of the path.
|
|||
|
|
/// </summary>
|
|||
|
|
/// <param name="pathData"></param>
|
|||
|
|
/// <param name="lastPathPointIndex"></param>
|
|||
|
|
/// <param name="nextPathPointIndex"></param>
|
|||
|
|
/// <param name="tValue"></param>
|
|||
|
|
/// <param name="pointOnPath"></param>
|
|||
|
|
private static void GetPathFirstDerivativeInternal (PathData pathData, int lastPathPointIndex, int nextPathPointIndex, float tValue,
|
|||
|
|
ref Vector3 pathFirstDerivative)
|
|||
|
|
{
|
|||
|
|
// Get path points
|
|||
|
|
pathPoint0 = pathData.pathLocationDataList[lastPathPointIndex].locationData.position;
|
|||
|
|
pathPoint3 = pathData.pathLocationDataList[nextPathPointIndex].locationData.position;
|
|||
|
|
// Get control points
|
|||
|
|
pathPoint1 = pathData.pathLocationDataList[lastPathPointIndex].outControlPoint;
|
|||
|
|
pathPoint2 = pathData.pathLocationDataList[nextPathPointIndex].inControlPoint;
|
|||
|
|
// Calculate new path point
|
|||
|
|
// First derivative: B' = 3 * (1 - t)^2 * (p1 - p0) + 6 * (1 - t) * t * (p2 - p1) + 3 * t^2 * (p3 - p2)
|
|||
|
|
// Do it component-wise to improve performance
|
|||
|
|
pathFirstDerivative.x = 3f * (1f - tValue) * (1f - tValue) * (pathPoint1.x - pathPoint0.x) +
|
|||
|
|
6f * (1f - tValue) * tValue * (pathPoint2.x - pathPoint1.x) +
|
|||
|
|
3f * tValue * tValue * (pathPoint3.x - pathPoint2.x);
|
|||
|
|
pathFirstDerivative.y = 3f * (1f - tValue) * (1f - tValue) * (pathPoint1.y - pathPoint0.y) +
|
|||
|
|
6f * (1f - tValue) * tValue * (pathPoint2.y - pathPoint1.y) +
|
|||
|
|
3f * tValue * tValue * (pathPoint3.y - pathPoint2.y);
|
|||
|
|
pathFirstDerivative.z = 3f * (1f - tValue) * (1f - tValue) * (pathPoint1.z - pathPoint0.z) +
|
|||
|
|
6f * (1f - tValue) * tValue * (pathPoint2.z - pathPoint1.z) +
|
|||
|
|
3f * tValue * tValue * (pathPoint3.z - pathPoint2.z);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/// <summary>
|
|||
|
|
/// Calculates the second derivative of the path given the index of the last path point, the index of the next path point and the t-value.
|
|||
|
|
/// The pathSecondDerivative variable is set to the first derivative of the path.
|
|||
|
|
/// </summary>
|
|||
|
|
/// <param name="pathData"></param>
|
|||
|
|
/// <param name="lastPathPointIndex"></param>
|
|||
|
|
/// <param name="nextPathPointIndex"></param>
|
|||
|
|
/// <param name="tValue"></param>
|
|||
|
|
/// <param name="pointOnPath"></param>
|
|||
|
|
private static void GetPathSecondDerivativeInternal (PathData pathData, int lastPathPointIndex, int nextPathPointIndex, float tValue,
|
|||
|
|
ref Vector3 pathSecondDerivative)
|
|||
|
|
{
|
|||
|
|
// Get path points
|
|||
|
|
pathPoint0 = pathData.pathLocationDataList[lastPathPointIndex].locationData.position;
|
|||
|
|
pathPoint3 = pathData.pathLocationDataList[nextPathPointIndex].locationData.position;
|
|||
|
|
// Get control points
|
|||
|
|
pathPoint1 = pathData.pathLocationDataList[lastPathPointIndex].outControlPoint;
|
|||
|
|
pathPoint2 = pathData.pathLocationDataList[nextPathPointIndex].inControlPoint;
|
|||
|
|
// Calculate new path point
|
|||
|
|
// Second derivative: B" = 6 * (1 - t) * (p2 - 2*p1 + p0) + 6 * t * (p3 - 2*p2 + p1)
|
|||
|
|
// Do it component-wise to improve performance
|
|||
|
|
pathSecondDerivative.x = 6f * (1f - tValue) * (pathPoint2.x - 2f * pathPoint1.x + pathPoint0.x) +
|
|||
|
|
6f * tValue * (pathPoint3.x - 2f * pathPoint2.x + pathPoint1.x);
|
|||
|
|
pathSecondDerivative.y = 6f * (1f - tValue) * (pathPoint2.y - 2f * pathPoint1.y + pathPoint0.y) +
|
|||
|
|
6f * tValue * (pathPoint3.y - 2f * pathPoint2.y + pathPoint1.y);
|
|||
|
|
pathSecondDerivative.z = 6f * (1f - tValue) * (pathPoint2.z - 2f * pathPoint1.z + pathPoint0.z) +
|
|||
|
|
6f * tValue * (pathPoint3.z - 2f * pathPoint2.z + pathPoint1.z);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/// <summary>
|
|||
|
|
/// Calculates the radius of curvature of a point on the path given the index of the last path point,
|
|||
|
|
/// the index of the next path point and the t-value.
|
|||
|
|
/// The pointPathCurvature variable is set to the radius of curvature.
|
|||
|
|
/// </summary>
|
|||
|
|
/// <param name="pathData"></param>
|
|||
|
|
/// <param name="lastPathPointIndex"></param>
|
|||
|
|
/// <param name="tValue"></param>
|
|||
|
|
/// <param name="pathPointCurvature"></param>
|
|||
|
|
/// <returns></returns>
|
|||
|
|
private static void GetPathCurvatureInternal (PathData pathData, int lastPathPointIndex, int nextPathPointIndex,
|
|||
|
|
float tValue, ref float pathPointCurvature)
|
|||
|
|
{
|
|||
|
|
// Get path points
|
|||
|
|
pathPoint0 = pathData.pathLocationDataList[lastPathPointIndex].locationData.position;
|
|||
|
|
pathPoint3 = pathData.pathLocationDataList[nextPathPointIndex].locationData.position;
|
|||
|
|
// Get control points
|
|||
|
|
pathPoint1 = pathData.pathLocationDataList[lastPathPointIndex].outControlPoint;
|
|||
|
|
pathPoint2 = pathData.pathLocationDataList[nextPathPointIndex].inControlPoint;
|
|||
|
|
// Calculate new path point
|
|||
|
|
// Curvature: K = len(B' x B") / len(B')^3
|
|||
|
|
// TODO optimise
|
|||
|
|
// Get first and second derivatives
|
|||
|
|
GetPathFirstDerivativeInternal(pathData, lastPathPointIndex, nextPathPointIndex, tValue, ref thisPathPointFirstDerivative);
|
|||
|
|
GetPathSecondDerivativeInternal(pathData, lastPathPointIndex, nextPathPointIndex, tValue, ref thisPathPointSecondDerivative);
|
|||
|
|
float firstDerivativeMagnitude = (float)System.Math.Sqrt((thisPathPointFirstDerivative.x * thisPathPointFirstDerivative.x) +
|
|||
|
|
(thisPathPointFirstDerivative.y * thisPathPointFirstDerivative.y) +
|
|||
|
|
(thisPathPointFirstDerivative.z * thisPathPointFirstDerivative.z));
|
|||
|
|
// Calculate curvature
|
|||
|
|
pathPointCurvature = Vector3.Cross(thisPathPointFirstDerivative, thisPathPointSecondDerivative).magnitude /
|
|||
|
|
(firstDerivativeMagnitude * firstDerivativeMagnitude * firstDerivativeMagnitude);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/// <summary>
|
|||
|
|
/// Calculates the radius of curvature (in a given plane defined by the plane normal) of a point on the path
|
|||
|
|
/// given the index of the last path point, the index of the next path point and the t-value.
|
|||
|
|
/// The vector planeNormal passed in must be normalised.
|
|||
|
|
/// The pointPathCurvature variable is set to the radius of curvature.
|
|||
|
|
/// </summary>
|
|||
|
|
/// <param name="pathData"></param>
|
|||
|
|
/// <param name="lastPathPointIndex"></param>
|
|||
|
|
/// <param name="tValue"></param>
|
|||
|
|
/// <param name="pathPointCurvature"></param>
|
|||
|
|
/// <returns></returns>
|
|||
|
|
private static void GetPathCurvatureInPlaneInternal(PathData pathData, int lastPathPointIndex, int nextPathPointIndex,
|
|||
|
|
float tValue, Vector3 planeNormal, ref float pathPointCurvature)
|
|||
|
|
{
|
|||
|
|
// Get path points
|
|||
|
|
pathPoint0 = pathData.pathLocationDataList[lastPathPointIndex].locationData.position;
|
|||
|
|
pathPoint3 = pathData.pathLocationDataList[nextPathPointIndex].locationData.position;
|
|||
|
|
// Get control points
|
|||
|
|
pathPoint1 = pathData.pathLocationDataList[lastPathPointIndex].outControlPoint;
|
|||
|
|
pathPoint2 = pathData.pathLocationDataList[nextPathPointIndex].inControlPoint;
|
|||
|
|
// Calculate new path point
|
|||
|
|
// Curvature: K = len(B' x B") / len(B')^3
|
|||
|
|
// Get first and second derivatives
|
|||
|
|
GetPathFirstDerivativeInternal(pathData, lastPathPointIndex, nextPathPointIndex, tValue, ref thisPathPointFirstDerivative);
|
|||
|
|
GetPathSecondDerivativeInternal(pathData, lastPathPointIndex, nextPathPointIndex, tValue, ref thisPathPointSecondDerivative);
|
|||
|
|
// Project derivatives into the plane defined by the given plane normal
|
|||
|
|
// Projection of a onto b = a.b / (|a|*|b|) * b/|b|
|
|||
|
|
// In this case the magnitude of the plane normal is 1
|
|||
|
|
thisPathPointFirstDerivative = Vector3.ProjectOnPlane(thisPathPointFirstDerivative, planeNormal);
|
|||
|
|
thisPathPointSecondDerivative = Vector3.ProjectOnPlane(thisPathPointSecondDerivative, planeNormal);
|
|||
|
|
float firstDerivativeMagnitude = thisPathPointFirstDerivative.magnitude;
|
|||
|
|
// Calculate curvature (TODO optimise)
|
|||
|
|
pathPointCurvature = Vector3.Cross(thisPathPointFirstDerivative, thisPathPointSecondDerivative).magnitude /
|
|||
|
|
(firstDerivativeMagnitude * firstDerivativeMagnitude * firstDerivativeMagnitude);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/// <summary>
|
|||
|
|
/// Calculates the distance between two points on the given the index of the last path point, the index of the next path point,
|
|||
|
|
/// the t-value of the start point on the path, the t-value of the end point of the path and the number of
|
|||
|
|
/// line segments to use during the calculation. More line segments will give a more accurate distance.
|
|||
|
|
/// The distanceBetweenTValues variable is set to the distance between the two points on the path.
|
|||
|
|
/// </summary>
|
|||
|
|
/// <param name="pathData"></param>
|
|||
|
|
/// <param name="lastPathPointIndex"></param>
|
|||
|
|
/// <param name="nextPathPointIndex"></param>
|
|||
|
|
/// <param name="startTValue"></param>
|
|||
|
|
/// <param name="endTValue"></param>
|
|||
|
|
/// <param name="lineSegments"></param>
|
|||
|
|
/// <param name="distanceBetweenTValues"></param>
|
|||
|
|
private static void GetDistanceBetweenTValuesInternal (PathData pathData, int lastPathPointIndex, int nextPathPointIndex,
|
|||
|
|
float startTValue, float endTValue, int lineSegments, ref float distanceBetweenTValues)
|
|||
|
|
{
|
|||
|
|
// Calculate how much to increment the t-value by for each iteration to achieve the specified number of line segments
|
|||
|
|
float tValueIncrementSize = (endTValue - startTValue) / lineSegments;
|
|||
|
|
float currentTValue = startTValue;
|
|||
|
|
distanceBetweenTValues = 0f;
|
|||
|
|
// Get the first path point
|
|||
|
|
GetPointOnPathInternal(pathData, lastPathPointIndex, nextPathPointIndex, currentTValue, ref thisPointOnPath);
|
|||
|
|
// Iterate through a number of evenly spaced t-values and add the lengths of the line segments together
|
|||
|
|
for (int i = 0; i < lineSegments; i++)
|
|||
|
|
{
|
|||
|
|
// Increment the t-value, and set the previous path point
|
|||
|
|
currentTValue += tValueIncrementSize;
|
|||
|
|
previousPointOnPath = thisPointOnPath;
|
|||
|
|
// Find the path point associated with this t-value
|
|||
|
|
GetPointOnPathInternal(pathData, lastPathPointIndex, nextPathPointIndex, currentTValue, ref thisPointOnPath);
|
|||
|
|
// Calculate the distance to this path point from the last point, and add it to the total distance
|
|||
|
|
distanceBetweenTValues += (float)System.Math.Sqrt(
|
|||
|
|
(thisPointOnPath.x - previousPointOnPath.x) * (thisPointOnPath.x - previousPointOnPath.x) +
|
|||
|
|
(thisPointOnPath.y - previousPointOnPath.y) * (thisPointOnPath.y - previousPointOnPath.y) +
|
|||
|
|
(thisPointOnPath.z - previousPointOnPath.z) * (thisPointOnPath.z - previousPointOnPath.z));
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
#endregion
|
|||
|
|
|
|||
|
|
#endregion
|
|||
|
|
|
|||
|
|
#region General Curve Methods
|
|||
|
|
|
|||
|
|
/// <summary>
|
|||
|
|
/// Evaluate an ease in-out curve.
|
|||
|
|
/// Time (t) is clamped between 0.0 and 1.0
|
|||
|
|
/// </summary>
|
|||
|
|
/// <param name="t"></param>
|
|||
|
|
/// <returns></returns>
|
|||
|
|
public static float EaseInOutCurve(float t)
|
|||
|
|
{
|
|||
|
|
t = t < 0f ? 0f : t > 1f ? 1f : t;
|
|||
|
|
|
|||
|
|
// f2(x) = x^2 / (x^2 + (1-x)^2)
|
|||
|
|
return (t * t) / ( (t * t) + ((1 - t) * (1 - t)) );
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/// <summary>
|
|||
|
|
/// Evaluate an ease in-out curve.
|
|||
|
|
/// Time (t) and strength are clamped between 0.0 and 1.0
|
|||
|
|
/// Stength = 0.0 - linear, Stength 1.0 = full ease-out.
|
|||
|
|
/// </summary>
|
|||
|
|
/// <param name="t"></param>
|
|||
|
|
/// <param name="strength"></param>
|
|||
|
|
/// <returns></returns>
|
|||
|
|
public static float EaseInOutCurve(float t, float strength)
|
|||
|
|
{
|
|||
|
|
t = t < 0f ? 0f : t > 1f ? 1f : t;
|
|||
|
|
strength = strength < 0f ? 0f : (strength > 1f ? 1f : strength);
|
|||
|
|
|
|||
|
|
return (1 - strength) * t + strength * EaseInOutCurve(t);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/// <summary>
|
|||
|
|
/// Evaluate an ease in-out curve with a smoother (slower) start and end.
|
|||
|
|
/// </summary>
|
|||
|
|
/// <param name="t"></param>
|
|||
|
|
/// <returns></returns>
|
|||
|
|
public static float EaseInOutCurve3X(float t)
|
|||
|
|
{
|
|||
|
|
t = t < 0f ? 0f : t > 1f ? 1f : t;
|
|||
|
|
|
|||
|
|
// f3(x) = x^3 / (x^3 + (1-x)^3)
|
|||
|
|
return (t * t * t) / ((t * t * t) + ((1 - t) * (1 - t) * (1 - t)));
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/// <summary>
|
|||
|
|
/// Evaluate an ease in-out curve with a smoother (slower) start and end.
|
|||
|
|
/// </summary>
|
|||
|
|
/// <param name="t"></param>
|
|||
|
|
/// <returns></returns>
|
|||
|
|
public static float EaseInOutCurve4X(float t)
|
|||
|
|
{
|
|||
|
|
t = t < 0f ? 0f : t > 1f ? 1f : t;
|
|||
|
|
|
|||
|
|
// f4(x) = 4^3 / (x^4 + (1-x)^4)
|
|||
|
|
return (t * t * t * t) / ((t * t * t * t) + ((1 - t) * (1 - t) * (1 - t) * (1 - t)));
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
#endregion
|
|||
|
|
|
|||
|
|
#region General Maths
|
|||
|
|
|
|||
|
|
/// <summary>
|
|||
|
|
/// Return the Vector2 as absolute (+ve) values.
|
|||
|
|
/// </summary>
|
|||
|
|
/// <param name="v"></param>
|
|||
|
|
/// <returns></returns>
|
|||
|
|
public static Vector2 Abs(Vector2 v)
|
|||
|
|
{
|
|||
|
|
return new Vector2(v.x < 0 ? -v.x : v.x, v.y < 0 ? -v.y : v.y);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/// <summary>
|
|||
|
|
/// Return the Vector3 as absolute (+ve) values.
|
|||
|
|
/// </summary>
|
|||
|
|
/// <param name="v"></param>
|
|||
|
|
/// <returns></returns>
|
|||
|
|
public static Vector3 Abs(Vector3 v)
|
|||
|
|
{
|
|||
|
|
return new Vector3(v.x < 0 ? -v.x : v.x, v.y < 0 ? -v.y : v.y, v.z < 0 ? -v.z : v.z);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/// <summary>
|
|||
|
|
/// Clamp a Vector2 to return x and y values between 0.0 and 1.0
|
|||
|
|
/// </summary>
|
|||
|
|
/// <param name="v"></param>
|
|||
|
|
/// <returns></returns>
|
|||
|
|
public static Vector2 Clamp(Vector2 v)
|
|||
|
|
{
|
|||
|
|
return new Vector2(v.x < 0f ? 0f : v.x > 1f ? 1f : v.x, v.y < 0f ? 0f : v.y > 1f ? 1f : v.y);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/// <summary>
|
|||
|
|
/// Clamp a Vector2 to return x and y values between the Min and Max values specified
|
|||
|
|
/// </summary>
|
|||
|
|
/// <param name="v"></param>
|
|||
|
|
/// <param name="xMin"></param>
|
|||
|
|
/// <param name="xMax"></param>
|
|||
|
|
/// <param name="yMin"></param>
|
|||
|
|
/// <param name="yMax"></param>
|
|||
|
|
/// <returns></returns>
|
|||
|
|
public static Vector2 Clamp(Vector2 v, float xMin, float xMax, float yMin, float yMax)
|
|||
|
|
{
|
|||
|
|
return new Vector2(v.x < xMin ? xMin : v.x > xMax ? xMax : v.x, v.y < yMin ? yMin : v.y > yMax ? yMax : v.y);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/// <summary>
|
|||
|
|
/// Apply damping to a float targetValue.
|
|||
|
|
/// </summary>
|
|||
|
|
/// <param name="currentValue"></param>
|
|||
|
|
/// <param name="targetValue"></param>
|
|||
|
|
/// <param name="deltaTime"></param>
|
|||
|
|
/// <param name="damping"></param>
|
|||
|
|
/// <returns></returns>
|
|||
|
|
public static float DampValue(float currentValue, float targetValue, float deltaTime, float damping)
|
|||
|
|
{
|
|||
|
|
if (damping <= 0f) { return targetValue; }
|
|||
|
|
else if (damping >= 1f || deltaTime < 0.0001f || currentValue == targetValue) { return currentValue; }
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
return Mathf.Lerp(currentValue, targetValue, 1f - Mathf.Pow(damping, deltaTime));
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/// <summary>
|
|||
|
|
/// Get the maximum absolute value of a Vector2 x and y values.
|
|||
|
|
/// e.g. v.x = -1.3, v.y = 0.5 return 1.3.
|
|||
|
|
/// </summary>
|
|||
|
|
/// <param name="v"></param>
|
|||
|
|
/// <returns></returns>
|
|||
|
|
public static float MaxAbs(Vector2 v)
|
|||
|
|
{
|
|||
|
|
float f1 = v.x < 0 ? -v.x : v.x;
|
|||
|
|
float f2 = v.y < 0 ? -v.y : v.y;
|
|||
|
|
|
|||
|
|
if (f1 > f2) { return f1; }
|
|||
|
|
else { return f2; }
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/// <summary>
|
|||
|
|
/// Normalise the value "x" to return values between 0 and 1
|
|||
|
|
/// given the potential range between "a" and "b"
|
|||
|
|
/// If "b" less than or equal to "a" this funtion will always return 0
|
|||
|
|
/// </summary>
|
|||
|
|
/// <param name="x"></param>
|
|||
|
|
/// <param name="a"></param>
|
|||
|
|
/// <param name="b"></param>
|
|||
|
|
/// <returns></returns>
|
|||
|
|
public static float Normalise(float x, float a, float b)
|
|||
|
|
{
|
|||
|
|
if (b <= a) { return 0f; }
|
|||
|
|
else { return ((x - a) * (1f / (b - a))); }
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
#endregion
|
|||
|
|
|
|||
|
|
#region Power Methods
|
|||
|
|
|
|||
|
|
// Faster version of Mathf.Pow for integer exponents
|
|||
|
|
public static float IntPow(float num, int pow)
|
|||
|
|
{
|
|||
|
|
if (pow != 0)
|
|||
|
|
{
|
|||
|
|
float ans = num;
|
|||
|
|
for (int i = 1; i < pow; i++)
|
|||
|
|
{
|
|||
|
|
ans *= num;
|
|||
|
|
}
|
|||
|
|
return ans;
|
|||
|
|
}
|
|||
|
|
else { return 1f; }
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// Faster version of Mathf.Pow for integer exponents and bases
|
|||
|
|
public static int IntPow(int num, int pow)
|
|||
|
|
{
|
|||
|
|
if (pow != 0)
|
|||
|
|
{
|
|||
|
|
int ans = num;
|
|||
|
|
for (int i = 1; i < pow; i++)
|
|||
|
|
{
|
|||
|
|
ans *= num;
|
|||
|
|
}
|
|||
|
|
return ans;
|
|||
|
|
}
|
|||
|
|
else { return 1; }
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
#endregion
|
|||
|
|
|
|||
|
|
#region Static Shape and Point Math Methods
|
|||
|
|
|
|||
|
|
private static bool IsInPolygon(List<Vector2> points, Vector2 sample)
|
|||
|
|
{
|
|||
|
|
bool isInPolygon = false;
|
|||
|
|
|
|||
|
|
int j = points.Count - 1;
|
|||
|
|
|
|||
|
|
for (int i = 0; i < points.Count; j = i++)
|
|||
|
|
{
|
|||
|
|
if (((points[i].y <= sample.y && sample.y < points[j].y) || (points[j].y <= sample.y && sample.y < points[i].y)) &&
|
|||
|
|
(sample.x < (points[j].x - points[i].x) * (sample.y - points[i].y) / (points[j].y - points[i].y) + points[i].x))
|
|||
|
|
isInPolygon = !isInPolygon;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
return isInPolygon;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/// <summary>
|
|||
|
|
/// Is the sample point inside the quad which has points p1, p2, p3 and p4?
|
|||
|
|
/// </summary>
|
|||
|
|
/// <param name="p1"></param>
|
|||
|
|
/// <param name="p2"></param>
|
|||
|
|
/// <param name="p3"></param>
|
|||
|
|
/// <param name="p4"></param>
|
|||
|
|
/// <param name="sample"></param>
|
|||
|
|
/// <returns></returns>
|
|||
|
|
public static bool IsInQuad(Vector3 p1, Vector3 p2, Vector3 p3, Vector3 p4, Vector3 sample)
|
|||
|
|
{
|
|||
|
|
return IsInTriangle(p1, p2, p3, sample) || IsInTriangle(p4, p2, p3, sample);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/// <summary>
|
|||
|
|
/// Is the sample point inside the triangle which has points: p1,p2 & p3
|
|||
|
|
/// </summary>
|
|||
|
|
/// <param name="p1"></param>
|
|||
|
|
/// <param name="p2"></param>
|
|||
|
|
/// <param name="p3"></param>
|
|||
|
|
/// <param name="sample"></param>
|
|||
|
|
/// <returns></returns>
|
|||
|
|
public static bool IsInTriangle(Vector3 p1, Vector3 p2, Vector3 p3, Vector3 sample)
|
|||
|
|
{
|
|||
|
|
bool halfPlaneSide1 = HalfPlaneSideSign(sample, p1, p2) < 0f;
|
|||
|
|
bool halfPlaneSide2 = HalfPlaneSideSign(sample, p2, p3) < 0f;
|
|||
|
|
bool halfPlaneSide3 = HalfPlaneSideSign(sample, p3, p1) < 0f;
|
|||
|
|
|
|||
|
|
return ((halfPlaneSide1 == halfPlaneSide2) && (halfPlaneSide2 == halfPlaneSide3));
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
public static float HalfPlaneSideSign(Vector3 p1, Vector3 p2, Vector3 p3)
|
|||
|
|
{
|
|||
|
|
return (p1.x - p3.x) * (p2.z - p3.z) - (p2.x - p3.x) * (p1.z - p3.z);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/// <summary>
|
|||
|
|
/// Allows for a "thickness" of each triangle to be specified to allow for error
|
|||
|
|
/// </summary>
|
|||
|
|
/// <param name="sp1"></param>
|
|||
|
|
/// <param name="sp2"></param>
|
|||
|
|
/// <param name="sample"></param>
|
|||
|
|
/// <returns></returns>
|
|||
|
|
public static float SquareDistanceToSide(Vector3 sp1, Vector3 sp2, Vector3 sample)
|
|||
|
|
{
|
|||
|
|
float squareSideLength = PlanarSquareDistance(sp1, sp2);
|
|||
|
|
float dotProduct = ((sample.x - sp1.x) * (sp2.x - sp1.x) + (sample.z - sp1.z) * (sp2.z - sp1.z)) / squareSideLength;
|
|||
|
|
if (dotProduct < 0)
|
|||
|
|
{
|
|||
|
|
return PlanarSquareDistance(sample, sp1);
|
|||
|
|
}
|
|||
|
|
else if (dotProduct <= 1)
|
|||
|
|
{
|
|||
|
|
return PlanarSquareDistance(sample, sp1) - dotProduct * dotProduct * squareSideLength;
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
return PlanarSquareDistance(sample, sp2);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/// <summary>
|
|||
|
|
/// Square distance calculation ignoring y distance
|
|||
|
|
/// </summary>
|
|||
|
|
/// <param name="p1"></param>
|
|||
|
|
/// <param name="p2"></param>
|
|||
|
|
/// <returns></returns>
|
|||
|
|
public static float PlanarSquareDistance(Vector3 p1, Vector3 p2)
|
|||
|
|
{
|
|||
|
|
// Basically pythagoras but without y and without final square root
|
|||
|
|
return (((p1.x - p2.x) * (p1.x - p2.x)) + ((p1.z - p2.z) * (p1.z - p2.z)));
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/// <summary>
|
|||
|
|
/// Find the closest central spline point
|
|||
|
|
/// </summary>
|
|||
|
|
/// <param name="splinePoints"></param>
|
|||
|
|
/// <param name="pointToMatch"></param>
|
|||
|
|
/// <returns></returns>
|
|||
|
|
public static int FindClosestPoint(Vector3[] splinePoints, Vector3 pointToMatch)
|
|||
|
|
{
|
|||
|
|
float sqrDist = 0f;
|
|||
|
|
float closestSqrDist = Mathf.Infinity;
|
|||
|
|
int closestPoint = 0;
|
|||
|
|
|
|||
|
|
if (splinePoints != null)
|
|||
|
|
{
|
|||
|
|
for (int i = 0; i < splinePoints.Length; i++)
|
|||
|
|
{
|
|||
|
|
sqrDist = PlanarSquareDistance(splinePoints[i], pointToMatch);
|
|||
|
|
if (sqrDist < closestSqrDist) { closestSqrDist = sqrDist; closestPoint = i; }
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
return closestPoint;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/// <summary>
|
|||
|
|
/// Find closest consecutive path point to this one
|
|||
|
|
/// </summary>
|
|||
|
|
/// <param name="splinePoints"></param>
|
|||
|
|
/// <param name="pointToMatch"></param>
|
|||
|
|
/// <param name="consecutiveTo"></param>
|
|||
|
|
/// <returns></returns>
|
|||
|
|
public static int FindClosestConsecutivePoint(Vector3[] splinePoints, Vector3 pointToMatch, int consecutiveTo)
|
|||
|
|
{
|
|||
|
|
int closestPoint = 0;
|
|||
|
|
|
|||
|
|
if (splinePoints != null)
|
|||
|
|
{
|
|||
|
|
// Check if the consecutive points exist
|
|||
|
|
bool c1Exists = consecutiveTo - 1 >= 0;
|
|||
|
|
bool c2Exists = splinePoints.Length > consecutiveTo + 1;
|
|||
|
|
if (c1Exists && c2Exists)
|
|||
|
|
{
|
|||
|
|
// Compare the distances to both of the consecutive points, return the closest point
|
|||
|
|
if (PlanarSquareDistance(splinePoints[consecutiveTo - 1], pointToMatch) < PlanarSquareDistance(splinePoints[consecutiveTo + 1], pointToMatch))
|
|||
|
|
{
|
|||
|
|
closestPoint = consecutiveTo - 1;
|
|||
|
|
}
|
|||
|
|
else { closestPoint = consecutiveTo + 1; }
|
|||
|
|
}
|
|||
|
|
// Return any point that exists
|
|||
|
|
else if (c1Exists) { closestPoint = consecutiveTo - 1; }
|
|||
|
|
else if (c2Exists) { closestPoint = consecutiveTo + 1; }
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
return closestPoint;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/// <summary>
|
|||
|
|
/// Find furthest consecutive path point to this one
|
|||
|
|
/// </summary>
|
|||
|
|
/// <param name="splinePoints"></param>
|
|||
|
|
/// <param name="pointToMatch"></param>
|
|||
|
|
/// <param name="consecutiveTo"></param>
|
|||
|
|
/// <returns></returns>
|
|||
|
|
public static int FindFurthestConsecutivePoint(Vector3[] splinePoints, Vector3 pointToMatch, int consecutiveTo)
|
|||
|
|
{
|
|||
|
|
int closestPoint = 0;
|
|||
|
|
|
|||
|
|
if (splinePoints != null)
|
|||
|
|
{
|
|||
|
|
// Check if the consecutive points exist
|
|||
|
|
bool c1Exists = consecutiveTo - 1 >= 0;
|
|||
|
|
bool c2Exists = splinePoints.Length > consecutiveTo + 1;
|
|||
|
|
if (c1Exists && c2Exists)
|
|||
|
|
{
|
|||
|
|
// Compare the distances to both of the consecutive points, return the furthest point
|
|||
|
|
if (PlanarSquareDistance(splinePoints[consecutiveTo - 1], pointToMatch) < PlanarSquareDistance(splinePoints[consecutiveTo + 1], pointToMatch))
|
|||
|
|
{
|
|||
|
|
closestPoint = consecutiveTo + 1;
|
|||
|
|
}
|
|||
|
|
else { closestPoint = consecutiveTo - 1; }
|
|||
|
|
}
|
|||
|
|
// Return any point that exists
|
|||
|
|
else if (c1Exists) { closestPoint = consecutiveTo - 1; }
|
|||
|
|
else if (c2Exists) { closestPoint = consecutiveTo + 1; }
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
return closestPoint;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
#endregion
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
}
|