protected (bool, Vector2) FindNearestPlayer(Vector2 direction, float viewCone)
{
Vector2 playerVector = new Vector2(0, 0);
float minPlayerDistance = Single.PositiveInfinity;
foreach (var player in GameObject.FindObjectsOfType(typeof(PlayerController)) as PlayerController[])
{
Vector2 target = player.Center - Rigidbody2D.position;
float distance = target.magnitude;
if (Vector2.Angle(target, direction) <= viewCone / 2 && distance < minPlayerDistance &&
// if no gameObject blocks line of sight to player
Physics2D.LinecastNonAlloc(Rigidbody2D.position, player.Center, _hit) == 2)
{
minPlayerDistance = distance;
playerVector = target;
}
}
return(!Single.IsInfinity(minPlayerDistance), playerVector.normalized);
}
/// <summary>
/// Compute spline coefficients for the specified x,y points.
/// This does the "natural spline" style for ends.
/// This can extrapolate off the ends of the splines.
/// You must provide points in X sort order.
/// </summary>
/// <param name="x">Input. X coordinates to fit.</param>
/// <param name="y">Input. Y coordinates to fit.</param>
/// <param name="startSlope">Optional slope constraint for the first point. Single.NaN means no constraint.</param>
/// <param name="endSlope">Optional slope constraint for the final point. Single.NaN means no constraint.</param>
/// <param name="debug">Turn on console output. Default is false.</param>
public void Fit(float[] x, float[] y, float startSlope = float.NaN, float endSlope = float.NaN, bool debug = false)
{
if (Single.IsInfinity(startSlope) || Single.IsInfinity(endSlope))
{
throw new Exception("startSlope and endSlope cannot be infinity.");
}
// Save x and y for eval
this.xOrig = x;
this.yOrig = y;
int n = x.Length;
float[] r = new float[n]; // the right hand side numbers: wikipedia page overloads b
TriDiagonalMatrixF m = new TriDiagonalMatrixF(n);
float dx1, dx2, dy1, dy2;
// First row is different (equation 16 from the article)
if (float.IsNaN(startSlope))
{
dx1 = x[1] - x[0];
m.C[0] = 1.0f / dx1;
m.B[0] = 2.0f * m.C[0];
r[0] = 3 * (y[1] - y[0]) / (dx1 * dx1);
}
else
{
m.B[0] = 1;
r[0] = startSlope;
}
// Body rows (equation 15 from the article)
for (int i = 1; i < n - 1; i++)
{
dx1 = x[i] - x[i - 1];
dx2 = x[i + 1] - x[i];
m.A[i] = 1.0f / dx1;
m.C[i] = 1.0f / dx2;
m.B[i] = 2.0f * (m.A[i] + m.C[i]);
dy1 = y[i] - y[i - 1];
dy2 = y[i + 1] - y[i];
r[i] = 3 * (dy1 / (dx1 * dx1) + dy2 / (dx2 * dx2));
}
// Last row also different (equation 17 from the article)
if (float.IsNaN(endSlope))
{
dx1 = x[n - 1] - x[n - 2];
dy1 = y[n - 1] - y[n - 2];
m.A[n - 1] = 1.0f / dx1;
m.B[n - 1] = 2.0f * m.A[n - 1];
r[n - 1] = 3 * (dy1 / (dx1 * dx1));
}
else
{
m.B[n - 1] = 1;
r[n - 1] = endSlope;
}
// k is the solution to the matrix
float[] k = m.Solve(r);
// a and b are each spline's coefficients
this.a = new float[n - 1];
this.b = new float[n - 1];
for (int i = 1; i < n; i++)
{
dx1 = x[i] - x[i - 1];
dy1 = y[i] - y[i - 1];
a[i - 1] = k[i - 1] * dx1 - dy1; // equation 10 from the article
b[i - 1] = -k[i] * dx1 + dy1; // equation 11 from the article
}
}
/// <summary>
/// Returns a value indicating whether the specified number evaluates to negative infinity
/// </summary>
/// <param name="number">a floating point number</param>
/// <returns>a boolean</returns>
public static bool IsInfinity(Real number)
{
return(Numeric.IsInfinity((Numeric)number));
}
/// <summary>
/// An implementation of the line search for the Wolfe conditions, from Nocedal & Wright
/// </summary>
internal virtual bool LineSearch(IChannel ch, bool force)
{
Contracts.AssertValue(ch);
Float dirDeriv = VectorUtils.DotProduct(ref _dir, ref _grad);
if (dirDeriv == 0)
{
throw ch.Process(new PrematureConvergenceException(this, "Directional derivative is zero. You may be sitting on the optimum."));
}
// if a non-descent direction is chosen, the line search will break anyway, so throw here
// The most likely reasons for this is a bug in your function's gradient computation,
ch.Check(dirDeriv < 0, "L-BFGS chose a non-descent direction.");
Float c1 = (Float)1e-4 * dirDeriv;
Float c2 = (Float)0.9 * dirDeriv;
Float alpha = (Iter == 1 ? (1 / VectorUtils.Norm(_dir)) : 1);
PointValueDeriv last = new PointValueDeriv(0, LastValue, dirDeriv);
PointValueDeriv aLo = new PointValueDeriv();
PointValueDeriv aHi = new PointValueDeriv();
// initial bracketing phase
while (true)
{
VectorUtils.AddMultInto(ref _x, alpha, ref _dir, ref _newX);
if (EnforceNonNegativity)
{
VBufferUtils.Apply(ref _newX, delegate(int ind, ref Float newXval)
{
if (newXval < 0.0)
{
newXval = 0;
}
});
}
Value = Eval(ref _newX, ref _newGrad);
GradientCalculations++;
if (Float.IsPositiveInfinity(Value))
{
alpha /= 2;
continue;
}
if (!FloatUtils.IsFinite(Value))
{
throw ch.Except("Optimizer unable to proceed with loss function yielding {0}", Value);
}
dirDeriv = VectorUtils.DotProduct(ref _dir, ref _newGrad);
PointValueDeriv curr = new PointValueDeriv(alpha, Value, dirDeriv);
if ((curr.V > LastValue + c1 * alpha) || (last.A > 0 && curr.V >= last.V))
{
aLo = last;
aHi = curr;
break;
}
else if (Math.Abs(curr.D) <= -c2)
{
return(true);
}
else if (curr.D >= 0)
{
aLo = curr;
aHi = last;
break;
}
last = curr;
if (alpha == 0)
{
alpha = Float.Epsilon; // Robust to divisional underflow.
}
else
{
alpha *= 2;
}
}
Float minChange = (Float)0.01;
int maxSteps = 10;
// this loop is the "zoom" procedure described in Nocedal & Wright
for (int step = 0; ; ++step)
{
if (step == maxSteps && !force)
{
return(false);
}
PointValueDeriv left = aLo.A < aHi.A ? aLo : aHi;
PointValueDeriv right = aLo.A < aHi.A ? aHi : aLo;
if (left.D > 0 && right.D < 0)
{
// interpolating cubic would have max in range, not min (can this happen?)
// set a to the one with smaller value
alpha = aLo.V < aHi.V ? aLo.A : aHi.A;
}
else
{
alpha = CubicInterp(aLo, aHi);
if (Float.IsNaN(alpha) || Float.IsInfinity(alpha))
{
alpha = (aLo.A + aHi.A) / 2;
}
}
// this is to ensure that the new point is within bounds
// and that the change is reasonably sized
Float ub = (minChange * left.A + (1 - minChange) * right.A);
if (alpha > ub)
{
alpha = ub;
}
Float lb = (minChange * right.A + (1 - minChange) * left.A);
if (alpha < lb)
{
alpha = lb;
}
VectorUtils.AddMultInto(ref _x, alpha, ref _dir, ref _newX);
if (EnforceNonNegativity)
{
VBufferUtils.Apply(ref _newX, delegate(int ind, ref Float newXval)
{
if (newXval < 0.0)
{
newXval = 0;
}
});
}
Value = Eval(ref _newX, ref _newGrad);
GradientCalculations++;
if (!FloatUtils.IsFinite(Value))
{
throw ch.Except("Optimizer unable to proceed with loss function yielding {0}", Value);
}
dirDeriv = VectorUtils.DotProduct(ref _dir, ref _newGrad);
PointValueDeriv curr = new PointValueDeriv(alpha, Value, dirDeriv);
if ((curr.V > LastValue + c1 * alpha) || (curr.V >= aLo.V))
{
if (aHi.A == curr.A)
{
if (force)
{
throw ch.Process(new PrematureConvergenceException(this, "Step size interval numerically zero."));
}
else
{
return(false);
}
}
aHi = curr;
}
else if (Math.Abs(curr.D) <= -c2)
{
return(true);
}
else
{
if (curr.D * (aHi.A - aLo.A) >= 0)
{
aHi = aLo;
}
if (aLo.A == curr.A)
{
if (force)
{
throw ch.Process(new PrematureConvergenceException(this, "Step size interval numerically zero."));
}
else
{
return(false);
}
}
aLo = curr;
}
}
}
