private bool linearProgram1(IList <Line> lines, int lineNo, float radius, Vector2 optVelocity, bool directionOpt, ref Vector2 result) { float dotProduct = lines[lineNo].point * lines[lineNo].direction; float discriminant = RVOMath.sqr(dotProduct) + RVOMath.sqr(radius) - RVOMath.absSq(lines[lineNo].point); if (discriminant < 0.0f) { /* Max speed circle fully invalidates line lineNo. */ return(false); } float sqrtDiscriminant = RVOMath.sqrt(discriminant); float tLeft = -dotProduct - sqrtDiscriminant; float tRight = -dotProduct + sqrtDiscriminant; for (int i = 0; i < lineNo; ++i) { float denominator = RVOMath.det(lines[lineNo].direction, lines[i].direction); float numerator = RVOMath.det(lines[i].direction, lines[lineNo].point - lines[i].point); if (RVOMath.fabs(denominator) <= RVOMath.RVO_EPSILON) { /* Lines lineNo and i are (almost) parallel. */ if (numerator < 0.0f) { return(false); } continue; } float t = numerator / denominator; if (denominator >= 0.0f) { /* Line i bounds line lineNo on the right. */ tRight = Math.Min(tRight, t); } else { /* Line i bounds line lineNo on the left. */ tLeft = Math.Max(tLeft, t); } if (tLeft > tRight) { return(false); } } if (directionOpt) { /* Optimize direction. */ if (optVelocity * lines[lineNo].direction > 0.0f) { /* 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. */ float t = lines[lineNo].direction * (optVelocity - lines[lineNo].point); //Debug.Log(tLeft.ToString() + " " + t.ToString() + " " + tRight.ToString()); 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); }