116 lines
3.5 KiB
C#
116 lines
3.5 KiB
C#
//#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;
|
|
}
|
|
}
|
|
}
|