/**
* <summary>Solves a one-dimensional linear program on a specified line
* subject to linear constraints defined by lines and a circular
* constraint.</summary>
*
* <returns>True if successful.</returns>
*
* <param name="lines">Lines defining the linear constraints.</param>
* <param name="lineNo">The specified line constraint.</param>
* <param name="radius">The radius of the circular constraint.</param>
* <param name="optVelocity">The optimization velocity.</param>
* <param name="directionOpt">True if the direction should be optimized.
* </param>
* <param name="result">A reference to the result of the linear program.
* </param>
*/
private bool linearProgram1(IList <Line> lines, int lineNo, KInt radius, KInt2 optVelocity, bool directionOpt, ref KInt2 result)
{
KInt dotProduct = RVOMath.Dot(lines[lineNo].point, lines[lineNo].direction);
KInt discriminant = RVOMath.sqr(dotProduct) + RVOMath.sqr(radius) - RVOMath.absSq(lines[lineNo].point);
if (discriminant < 0)
{
/* Max speed circle fully invalidates line lineNo. */
return(false);
}
KInt sqrtDiscriminant = RVOMath.sqrt(discriminant);
KInt tLeft = -dotProduct - sqrtDiscriminant;
KInt tRight = -dotProduct + sqrtDiscriminant;
for (int i = 0; i < lineNo; ++i)
{
KInt denominator = RVOMath.det(lines[lineNo].direction, lines[i].direction);
KInt numerator = RVOMath.det(lines[i].direction, lines[lineNo].point - lines[i].point);
if (RVOMath.fabs(denominator) <= 0)
{
/* Lines lineNo and i are (almost) parallel. */
if (numerator < 0)
{
return(false);
}
continue;
}
KInt t = numerator / denominator;
if (denominator >= 0)
{
/* Line i bounds line lineNo on the right. */
tRight = KInt.Min(tRight, t);
}
else
{
/* Line i bounds line lineNo on the left. */
tLeft = KInt.Max(tLeft, t);
}
if (tLeft > tRight)
{
return(false);
}
}
if (directionOpt)
{
/* Optimize direction. */
if (RVOMath.Dot(optVelocity, lines[lineNo].direction) > 0)
{
/* Take right extreme. */
result = lines[lineNo].point + tRight * lines[lineNo].direction;
}
else
{
/* Take left extreme. */
result = lines[lineNo].point + tLeft * lines[lineNo].direction;
}
}
else
{
/* Optimize closest point. */
KInt t = RVOMath.Dot(lines[lineNo].direction, (optVelocity - lines[lineNo].point));
if (t < tLeft)
{
result = lines[lineNo].point + tLeft * lines[lineNo].direction;
}
else if (t > tRight)
{
result = lines[lineNo].point + tRight * lines[lineNo].direction;
}
else
{
result = lines[lineNo].point + t * lines[lineNo].direction;
}
}
return(true);
}
