/// <summary>
/// The closest point on the curve to the point p.
/// </summary>
public Vector2 Closest(Vector2 p)
{
float px = C0.x - p.x;
float pz = C0.y - p.y;
float ax = C1.x - C0.x;
float az = C1.y - C0.y;
float bx = C0.x - 2.0f * C1.x + C2.x;
float bz = C0.y - 2.0f * C1.y + C2.y;
float a = bx * bx + bz * bz;
float b = 3 * (ax * bx + az * bz);
float c = 2 * (ax * ax + az * az) + px * bx + pz * bz;
float d = px * ax + pz * az;
var roots = Polynomial3d.Solve(a, b, c, d);
float min = float.PositiveInfinity;
Vector2 closest = new Vector2(min, min);
for (int i = 0; i < roots.real; i++)
{
float t = (float)roots[i];
Vector2 v = Position(t);
float dist = (v - p).sqrMagnitude;
if (dist < min)
{
min = dist;
closest = v;
}
}
return(closest);
}
/// <summary>
/// If the segment ab intersects the curve.
/// </summary>
public bool Intersects(Vector2 a, Vector2 b)
{
//coefficients of quadratic
Vector2 c2 = C0 + C1 * -2.0f + C2;
Vector2 c1 = C0 * -2.0f + C1 * 2.0f;
//Convert line to normal form: ax + by + c = 0
//Find normal to line: negative inverse of original line's slope
Vector2 n = new Vector2(a.y - b.y, b.x - a.x);
//c coefficient for normal form of line
float c = a.x * b.y - b.x * a.y;
//Transform coefficients to line's coordinate system and find roots of cubic
var roots = Polynomial3d.Solve(1, Vector2.Dot(n, c2), Vector2.Dot(n, c1), Vector2.Dot(n, C0) + c);
Vector2 min, max;
min.x = Math.Min(a.x, b.x);
min.y = Math.Min(a.y, b.y);
max.x = Math.Max(a.x, b.x);
max.y = Math.Max(a.y, b.y);
for (int i = 0; i < roots.real; i++)
{
float t = (float)roots[i];
if (t < 0.0f || t > 1.0f)
{
continue;
}
Vector2 v0 = Position(t);
if (a.x == b.x)
{
if (min.y <= v0.y && v0.y <= max.y)
{
return(true);
}
}
else if (a.y == b.y)
{
if (min.x <= v0.x && v0.x <= max.x)
{
return(true);
}
}
else if (min.x <= v0.x && v0.x <= max.x && min.y <= v0.y && v0.y <= max.y)
{
return(true);
}
}
return(false);
}