MiniGames/Assets/Scivolo Character Controller/Scripts/Internal/SeparationOfBestFitCalculator.cs

116 lines
3.5 KiB
C#
Raw Normal View History

2025-07-11 15:42:48 +05:00
//#define MB_DEBUG
using UnityEngine;
using static MenteBacata.ScivoloCharacterController.Internal.Math;
namespace MenteBacata.ScivoloCharacterController.Internal
{
/*
* Given a set of separation vectors where each resolves an overlap with a single collider, this class provides a method
* to calculate a single separation vector which is a good match for all of them.
* In practice it uses the steepest descent method to approximate the vector x that minimizes the sum of squared errors
* given by the function:
* F(x) = 1/2 Sum (<x, direction_i> - distance_i)^2
* Where direction_i and distance_i are respectively the direction and the distance of the i-th separation vector of the
* given set.
*/
public static class SeparationOfBestFitCalculator
{
private const int maxIterations = 10;
private static readonly float[] errors = new float[OverlapResolver.maxOverlaps];
/// <summary>
/// Gets an approximation of the separation vector of best fit the given a set separation vectors.
/// </summary>
public static Vector3 GetSeparationOfBestFit(Vector3[] directions, float[] distances, int n)
{
Vector3 x = GetInitialGuess(directions, distances, n);
for (int i = 0; i < maxIterations; i++)
{
PopulateErrors(x, directions, distances, n);
var gradient = GetGradient(directions, n);
if (IsCircaZero(gradient))
break;
if (TryGetStep(gradient, directions, n, out Vector3 step))
{
x += step;
}
else
break;
}
return x;
}
private static Vector3 GetGradientAt(Vector3 x, Vector3[] directions, float[] distances, int n)
{
PopulateErrors(x, directions, distances, n);
return GetGradient(directions, n);
}
private static Vector3 GetInitialGuess(Vector3[] directions, float[] distances, int n)
{
var x = new Vector3(0f, 0f, 0f);
for (int i = 0; i < n; i++)
{
x += distances[i] * directions[i];
}
return x;
}
private static void PopulateErrors(Vector3 x, Vector3[] directions, float[] distances, int n)
{
for (int i = 0; i < n; i++)
{
errors[i] = Dot(x, directions[i]) - distances[i];
}
}
private static Vector3 GetGradient(Vector3[] directions, int n)
{
var d = new Vector3(0f, 0f, 0f);
for (int i = 0; i < n; i++)
{
d += errors[i] * directions[i];
}
return d;
}
private static bool TryGetStep(Vector3 gradient, Vector3[] directions, int n, out Vector3 step)
{
float num = 0f, denum = 0f;
// It's not necessary to normalize, but if the gradient magnitude is small it could leads to very small denum.
var gradientDirection = Normalized(gradient);
for (int i = 0; i < n; i++)
{
float dot = Dot(gradientDirection, directions[i]);
num += dot * errors[i];
denum += dot * dot;
}
if (IsCircaZero(denum))
{
step = new Vector3();
return false;
}
step = (-num / denum) * gradientDirection;
return true;
}
}
}