314 lines
16 KiB
C#
314 lines
16 KiB
C#
#if BURST_PRESENT
|
|
using Unity.Burst;
|
|
#endif
|
|
using Unity.Collections;
|
|
using Unity.Mathematics;
|
|
|
|
namespace UnityEngine.XR.Interaction.Toolkit.Utilities.Curves
|
|
{
|
|
/// <summary>
|
|
/// A static class that provides utility methods for working with curves.
|
|
/// All functions are compiled with the Burst compiler if the Burst package is present.
|
|
/// </summary>
|
|
#if BURST_PRESENT
|
|
[BurstCompile]
|
|
#endif
|
|
static class CurveUtility
|
|
{
|
|
/// <summary>
|
|
/// Pre-multiplied 8x applied to the Unity mathematics float epsilon value, used for float approximate equality comparisons.
|
|
/// </summary>
|
|
const float k_EightEpsilon = math.EPSILON * 8f;
|
|
|
|
/// <summary>
|
|
/// Samples a point on a quadratic Bezier curve defined by three control points and a parameter t.
|
|
/// </summary>
|
|
/// <param name="p0">The first control point.</param>
|
|
/// <param name="p1">The second control point.</param>
|
|
/// <param name="p2">The third control point.</param>
|
|
/// <param name="t">The parameter t, ranging from 0 to 1.</param>
|
|
/// <param name="point">The output point on the curve.</param>
|
|
#if BURST_PRESENT
|
|
[BurstCompile]
|
|
#endif
|
|
public static void SampleQuadraticBezierPoint(in float3 p0, in float3 p1, in float3 p2, float t, out float3 point)
|
|
{
|
|
var u = 1f - t; // (1 - t)
|
|
var uu = u * u; // (1 - t)²
|
|
var tt = t * t; // t²
|
|
|
|
// (1 - t)²P₀ + 2(1 - t)tP₁ + t²P₂ where 0 ≤ t ≤ 1
|
|
// u²P₀ + 2utP₁ + t²P₂
|
|
point = (uu * p0) +
|
|
(2f * u * t * p1) +
|
|
(tt * p2);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Samples a point on a cubic Bezier curve defined by four control points and a parameter t.
|
|
/// </summary>
|
|
/// <param name="p0">The first control point.</param>
|
|
/// <param name="p1">The second control point.</param>
|
|
/// <param name="p2">The third control point.</param>
|
|
/// <param name="p3">The fourth control point.</param>
|
|
/// <param name="t">The parameter t, ranging from 0 to 1.</param>
|
|
/// <param name="point">The output point on the curve.</param>
|
|
#if BURST_PRESENT
|
|
[BurstCompile]
|
|
#endif
|
|
public static void SampleCubicBezierPoint(in float3 p0, in float3 p1, in float3 p2, in float3 p3, float t, out float3 point)
|
|
{
|
|
var u = 1f - t; // (1 - t)
|
|
var uu = u * u; // (1 - t)²
|
|
var uuu = uu * u; // (1 - t)³
|
|
var tt = t * t; // t²
|
|
var ttt = tt * t; // t³
|
|
|
|
// (1 - t)³P₀ + 3(1 - t)²tP₁ + 3(1 - t)t²P₂ + t³P₃ where 0 ≤ t ≤ 1
|
|
// u³P₀ + 3u²tP₁ + 3ut²P₂ + t³P₃
|
|
point = (uuu * p0) +
|
|
(3f * uu * t * p1) +
|
|
(3f * u * tt * p2) +
|
|
(ttt * p3);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Elevates a quadratic Bezier curve to a cubic Bezier curve by adding an extra control point.
|
|
/// </summary>
|
|
/// <param name="p0">The first control point of the quadratic curve.</param>
|
|
/// <param name="p1">The second control point of the quadratic curve.</param>
|
|
/// <param name="p2">The third control point of the quadratic curve.</param>
|
|
/// <param name="c0">The first control point of the cubic curve. (output)</param>
|
|
/// <param name="c1">The second control point of the cubic curve. (output)</param>
|
|
/// <param name="c2">The third control point of the cubic curve. (output)</param>
|
|
/// <param name="c3">The fourth control point of the cubic curve. (output)</param>
|
|
#if BURST_PRESENT
|
|
[BurstCompile]
|
|
#endif
|
|
public static void ElevateQuadraticToCubicBezier(in float3 p0, in float3 p1, in float3 p2, out float3 c0, out float3 c1, out float3 c2, out float3 c3)
|
|
{
|
|
// A Bezier curve of one degree can be reproduced by one of higher degree.
|
|
// Convert quadratic Bezier curve with control points P₀, P₁, P₂
|
|
// into a cubic Bezier curve with control points C₀, C₁, C₂, C₃.
|
|
// The end points remain the same.
|
|
c0 = p0;
|
|
c1 = p0 + (2f / 3f) * (p1 - p0);
|
|
c2 = p2 + (2f / 3f) * (p1 - p2);
|
|
c3 = p2;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Generates a cubic Bezier curve from a given line segment and a curve ratio.
|
|
/// </summary>
|
|
/// <param name="numTargetPoints">The number of points to generate for the curve.</param>
|
|
/// <param name="curveRatio">The ratio of the line length to use as the distance from the midpoint to the control point.</param>
|
|
/// <param name="lineOrigin">The starting point of the line segment.</param>
|
|
/// <param name="lineDirection">The normalized forward direction vector of the line segment.</param>
|
|
/// <param name="endPoint">The ending point of the line segment.</param>
|
|
/// <param name="targetPoints">A reference to a native array of <see cref="float3"/> that will store the generated curve points.</param>
|
|
#if UNITY_2022_2_OR_NEWER && BURST_PRESENT
|
|
[BurstCompile]
|
|
#endif
|
|
public static void GenerateCubicBezierCurve(int numTargetPoints, float curveRatio, in float3 lineOrigin, in float3 lineDirection, in float3 endPoint, ref NativeArray<float3> targetPoints)
|
|
{
|
|
var lineLength = math.length(endPoint - lineOrigin);
|
|
var adjustedMidPoint = lineOrigin + lineDirection * lineLength * curveRatio;
|
|
|
|
ElevateQuadraticToCubicBezier(lineOrigin, adjustedMidPoint, endPoint,
|
|
out var p0, out var p1, out var p2, out var p3);
|
|
|
|
// Set first point
|
|
targetPoints[0] = lineOrigin;
|
|
var interval = 1f / (numTargetPoints - 1);
|
|
for (var i = 1; i < numTargetPoints; ++i)
|
|
{
|
|
var percent = i * interval;
|
|
SampleCubicBezierPoint(p0, p1, p2, p3, percent, out var newPoint);
|
|
targetPoints[i] = newPoint;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Generates a cubic Bezier curve from a given curve segment and a curve ratio.
|
|
/// </summary>
|
|
/// <param name="numTargetPoints">The number of points to generate for the curve.</param>
|
|
/// <param name="curveRatio">The ratio of the curve length to use as the distance from the midpoint to the control point.</param>
|
|
/// <param name="startOffset">Fixed offset to omit from the start of the curve.</param>
|
|
/// <param name="endOffset">Fixed offset to omit from the end of the curve.</param>
|
|
/// <param name="curveOrigin">The starting point of the curve segment.</param>
|
|
/// <param name="curveDirection">The normalized forward direction vector of the curve segment.</param>
|
|
/// <param name="endPoint">The ending point of the curve segment.</param>
|
|
/// <param name="targetPoints">A reference to a native array of <see cref="float3"/> that will store the generated curve points.</param>
|
|
/// <param name="minLineLength">Minimum line length that will be computed before the function returns false.</param>
|
|
/// <returns>True if the cubic Bezier curve was successfully generated, false otherwise.</returns>
|
|
#if UNITY_2022_2_OR_NEWER && BURST_PRESENT
|
|
[BurstCompile]
|
|
#endif
|
|
public static bool TryGenerateCubicBezierCurve(int numTargetPoints, float curveRatio, in float3 curveOrigin, in float3 curveDirection, in float3 endPoint, ref NativeArray<float3> targetPoints, float minLineLength = 0.005f, float startOffset = 0f, float endOffset = 0f)
|
|
{
|
|
var lineLength = math.length(endPoint - curveOrigin);
|
|
var combinedOffset = startOffset + endOffset;
|
|
|
|
// The offsets exceed the length of the line
|
|
if (combinedOffset > lineLength || lineLength - combinedOffset < minLineLength)
|
|
return false;
|
|
|
|
float3 adjustedMidPoint;
|
|
if (curveRatio > 0)
|
|
adjustedMidPoint = curveOrigin + curveDirection * lineLength * curveRatio;
|
|
else
|
|
adjustedMidPoint = math.lerp(curveOrigin, endPoint, 0.5f);
|
|
|
|
return TryGenerateCubicBezierCurveCore(numTargetPoints, curveOrigin, adjustedMidPoint, endPoint, ref targetPoints, minLineLength, startOffset, endOffset);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Generates a cubic Bezier curve from a given curve segment and a curve ratio.
|
|
/// </summary>
|
|
/// <param name="numTargetPoints">The number of points to generate for the curve.</param>
|
|
/// <param name="startOffset">Fixed offset to omit from the start of the curve.</param>
|
|
/// <param name="endOffset">Fixed offset to omit from the end of the curve.</param>
|
|
/// <param name="curveOrigin">The starting point of the curve segment.</param>
|
|
/// <param name="midPoint">The midpoint the cubic bezier curve.</param>
|
|
/// <param name="endPoint">The ending point of the curve segment.</param>
|
|
/// <param name="targetPoints">A reference to a native array of <see cref="float3"/> that will store the generated curve points.</param>
|
|
/// <param name="minLineLength">Minimum line length that will be computed before the function returns false.</param>
|
|
/// <returns>True if the cubic Bezier curve was successfully generated, false otherwise.</returns>
|
|
#if UNITY_2022_2_OR_NEWER && BURST_PRESENT
|
|
[BurstCompile]
|
|
#endif
|
|
public static bool TryGenerateCubicBezierCurve(int numTargetPoints, in float3 curveOrigin, in float3 midPoint, in float3 endPoint, ref NativeArray<float3> targetPoints, float minLineLength = 0.005f, float startOffset = 0f, float endOffset = 0f)
|
|
{
|
|
var lineLength = math.length(endPoint - curveOrigin);
|
|
var combinedOffset = startOffset + endOffset;
|
|
|
|
// The offsets exceed the length of the line
|
|
if (combinedOffset > lineLength || lineLength - combinedOffset < minLineLength)
|
|
return false;
|
|
|
|
return TryGenerateCubicBezierCurveCore(numTargetPoints, curveOrigin, midPoint, endPoint, ref targetPoints, minLineLength, startOffset, endOffset);
|
|
}
|
|
|
|
static bool TryGenerateCubicBezierCurveCore(int numTargetPoints, in float3 curveOrigin, in float3 midPoint, in float3 endPoint, ref NativeArray<float3> targetPoints, float minLineLength = 0.005f, float startOffset = 0f, float endOffset = 0f)
|
|
{
|
|
ElevateQuadraticToCubicBezier(curveOrigin, midPoint, endPoint,
|
|
out var p0, out var p1, out var p2, out var p3);
|
|
|
|
bool hasStartOffset = startOffset > 0;
|
|
bool hasEndOffset = endOffset > 0;
|
|
|
|
float tStart = 0f;
|
|
float tEnd = 1f;
|
|
if (hasStartOffset || hasEndOffset)
|
|
{
|
|
var combinedOffset = startOffset + endOffset;
|
|
float totalCurveLength = ApproximateCubicBezierLength(p0, p1, p2, p3, math.max(numTargetPoints / 2, 4));
|
|
if (combinedOffset > totalCurveLength || totalCurveLength - (combinedOffset) < minLineLength)
|
|
return false;
|
|
|
|
if (hasStartOffset)
|
|
tStart = startOffset / totalCurveLength;
|
|
if (hasEndOffset)
|
|
tEnd = (totalCurveLength - endOffset) / totalCurveLength;
|
|
}
|
|
|
|
var interval = (tEnd - tStart) / (numTargetPoints - 1);
|
|
|
|
for (var i = 0; i < numTargetPoints; ++i)
|
|
{
|
|
var percent = tStart + i * interval;
|
|
SampleCubicBezierPoint(p0, p1, p2, p3, percent, out var newPoint);
|
|
targetPoints[i] = newPoint;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Calculates an approximate length of a cubic Bezier curve.
|
|
/// </summary>
|
|
/// <param name="p0">The starting point of the Bezier curve.</param>
|
|
/// <param name="p1">The first control point influencing the curve's shape.</param>
|
|
/// <param name="p2">The second control point influencing the curve's shape.</param>
|
|
/// <param name="p3">The ending point of the Bezier curve.</param>
|
|
/// <param name="subdivisions">The number of subdivisions used for approximation. Higher values increase accuracy.</param>
|
|
/// <returns>The approximate length of the cubic Bezier curve.</returns>
|
|
/// <remarks>
|
|
/// This method approximates the length of a cubic Bezier curve by subdividing it into smaller segments
|
|
/// and summing the lengths of these linear segments. The accuracy of the approximation increases
|
|
/// with the number of subdivisions. The method uses linear interpolation to find points along the curve at regular intervals.
|
|
/// </remarks>
|
|
#if BURST_PRESENT
|
|
[BurstCompile]
|
|
#endif
|
|
public static float ApproximateCubicBezierLength(in float3 p0, in float3 p1, in float3 p2, in float3 p3, int subdivisions)
|
|
{
|
|
float length = 0f;
|
|
float3 previousPoint = p0;
|
|
|
|
for (int i = 1; i <= subdivisions; i++)
|
|
{
|
|
float t = (float)i / subdivisions;
|
|
SampleCubicBezierPoint(p0, p1, p2, p3, t, out float3 currentPoint);
|
|
length += math.distance(currentPoint, previousPoint);
|
|
previousPoint = currentPoint;
|
|
}
|
|
|
|
return length;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Calculates the position of a projectile at a given time using constant acceleration formula.
|
|
/// </summary>
|
|
/// <param name="initialPosition">The initial position vector of the projectile.</param>
|
|
/// <param name="initialVelocity">The initial velocity vector of the projectile.</param>
|
|
/// <param name="constantAcceleration">The constant acceleration vector of the projectile, typically (0, -9.8, 0).</param>
|
|
/// <param name="time">The time at which to calculate the position.</param>
|
|
/// <param name="point">The output point on the curve.</param>
|
|
#if BURST_PRESENT
|
|
[BurstCompile]
|
|
#endif
|
|
public static void SampleProjectilePoint(in float3 initialPosition, in float3 initialVelocity, in float3 constantAcceleration, float time, out float3 point)
|
|
{
|
|
// Position of object in constant acceleration is:
|
|
// x(t) = x₀ + v₀t + 0.5at²
|
|
// where x₀ is the position at time 0,
|
|
// v₀ is the velocity vector at time 0,
|
|
// a is the constant acceleration vector
|
|
point = initialPosition + initialVelocity * time + constantAcceleration * (0.5f * time * time);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Calculates the time of flight for a projectile launched at a given angle and initial velocity.
|
|
/// </summary>
|
|
/// <param name="velocityMagnitude">The magnitude of the initial velocity vector.</param>
|
|
/// <param name="gravityAcceleration">The constant acceleration due to gravity (typically 9.8).</param>
|
|
/// <param name="angleRad">The launch angle in radians.</param>
|
|
/// <param name="height">The initial height of the projectile.</param>
|
|
/// <param name="extraFlightTime">An additional time to add to the flight time.</param>
|
|
/// <param name="flightTime">The output parameter for the calculated flight time.</param>
|
|
#if BURST_PRESENT
|
|
[BurstCompile]
|
|
#endif
|
|
public static void CalculateProjectileFlightTime(float velocityMagnitude, float gravityAcceleration, float angleRad, float height, float extraFlightTime, out float flightTime)
|
|
{
|
|
// Vertical velocity component Vy = v₀sinθ
|
|
// When initial height = 0,
|
|
// Time of flight = 2(initial velocity)(sine of launch angle) / (acceleration) = 2v₀sinθ/g
|
|
// When initial height > 0,
|
|
// Time of flight = [Vy + √(Vy² + 2gh)] / g
|
|
// The additional flight time property is added.
|
|
var vy = velocityMagnitude * angleRad;
|
|
if (height < 0f)
|
|
flightTime = 0f;
|
|
// Does the same math roughly as Mathf.Approximately(height, 0f)
|
|
else if (math.abs(height) < k_EightEpsilon)
|
|
flightTime = 2f * vy / gravityAcceleration;
|
|
else
|
|
flightTime = (vy + math.sqrt(vy * vy + 2f * gravityAcceleration * height)) / gravityAcceleration;
|
|
|
|
flightTime = math.max(flightTime + extraFlightTime, 0f);
|
|
}
|
|
}
|
|
}
|