/// <summary>
/// Minimise the stored distance function over a given interval
/// </summary>
/// <param name="eval"> Spline evaluator </param>
/// <param name="interval"> Interval size </param>
/// <param name="iterations"> Number of iterations </param>
/// <returns> Point in interval that minimises stored distance function </returns>
public float GetClosestPointInInterval( Evaluator eval, float interval, int iterations )
{
m_S[ 0 ] = eval.GlobalT;
m_S[ 1 ] = eval.GlobalT + ( interval / 2 );
m_S[ 2 ] = eval.GlobalT + interval;
float minT = m_S[ 0 ];
float maxT = m_S[ 2 ];
m_Points[ 0 ] = eval.EvaluatePositionWithClampedGlobalT( m_S[ 0 ] );
m_Points[ 1 ] = eval.EvaluatePositionWithClampedGlobalT( m_S[ 1 ] );
m_Points[ 2 ] = eval.EvaluatePositionWithClampedGlobalT( m_S[ 2 ] );
m_Distances[ 0 ] = m_Distance( m_Points[ 0 ] );
m_Distances[ 1 ] = m_Distance( m_Points[ 1 ] );
m_Distances[ 2 ] = m_Distance( m_Points[ 2 ] );
int BestIndex = 0;
float ClosestDistance = float.MaxValue;
for ( int k = 0; k < iterations; ++k )
{
m_S2[ 0 ] = m_S[ 0 ] * m_S[ 0 ];
m_S2[ 1 ] = m_S[ 1 ] * m_S[ 1 ];
m_S2[ 2 ] = m_S[ 2 ] * m_S[ 2 ];
float PolyEstimateNum = ( ( m_S2[ 1 ] - m_S2[ 2 ] ) * m_Distances[ 0 ] ) +
( ( m_S2[ 2 ] - m_S2[ 0 ] ) * m_Distances[ 1 ] ) +
( ( m_S2[ 0 ] - m_S2[ 1 ] ) * m_Distances[ 2 ] );
float PolyEstimateDen = ( ( m_S[ 1 ] - m_S[ 2 ] ) * m_Distances[ 0 ] ) +
( ( m_S[ 2 ] - m_S[ 0 ] ) * m_Distances[ 1 ] ) +
( ( m_S[ 0 ] - m_S[ 1 ] ) * m_Distances[ 2 ] );
if ( Math.Abs( PolyEstimateDen ) < 0.0001f )
{
break;
}
m_S[ 3 ] = Utils.Clamp( 0.5f * ( PolyEstimateNum / PolyEstimateDen ), minT, maxT );
m_Points[ 3 ] = eval.EvaluatePositionWithClampedGlobalT( m_S[ 3 ] );
m_Distances[ 3 ] = m_Distance( m_Points[ 3 ] );
m_PValues[ 0 ] = EvaluatePolynomial( m_S[ 0 ] );
m_PValues[ 1 ] = EvaluatePolynomial( m_S[ 1 ] );
m_PValues[ 2 ] = EvaluatePolynomial( m_S[ 2 ] );
m_PValues[ 3 ] = EvaluatePolynomial( m_S[ 3 ] );
// Which estimate created the furthest distance?
int FurthestIndex = 0;
float FurthestDistance = -float.MaxValue;
for ( int Index = 0; Index < 4; ++Index )
{
if ( m_PValues[ Index ] > FurthestDistance )
{
FurthestDistance = m_PValues[ Index ];
FurthestIndex = Index;
}
}
// Remake arrays m_S and m_Distances using the 3 closest estimates
int AddAt = 0;
ClosestDistance = float.MaxValue;
for ( int Index = 0; Index < 4; ++Index )
{
if ( Index != FurthestIndex )
{
m_S[ AddAt ] = m_S[ Index ];
m_Distances[ AddAt ] = m_Distances[ Index ];
m_Points[ AddAt ] = m_Points[ Index ];
m_PValues[ AddAt ] = m_PValues[ Index ];
if ( m_PValues[ AddAt ] < ClosestDistance )
{
BestIndex = AddAt;
}
++AddAt;
}
}
}
if ( ClosestDistance < m_ClosestDistance )
{
m_ClosestDistance = ClosestDistance;
m_ClosestFraction = m_S[ BestIndex ];
}
return m_ClosestFraction;
}