/// <summary>
/// The closest point on the curve to the point p.
/// </summary>
public Vector2d Closest(Vector2d p)
{
double px = C0.x - p.x;
double pz = C0.y - p.y;
double ax = C1.x - C0.x;
double az = C1.y - C0.y;
double bx = C0.x - 2.0 * C1.x + C2.x;
double bz = C0.y - 2.0 * C1.y + C2.y;
double a = bx * bx + bz * bz;
double b = 3 * (ax * bx + az * bz);
double c = 2 * (ax * ax + az * az) + px * bx + pz * bz;
double d = px * ax + pz * az;
var roots = Polynomial3d.Solve(a, b, c, d);
double min = double.PositiveInfinity;
Vector2d closest = new Vector2d(min, min);
for (int i = 0; i < roots.real; i++)
{
double t = roots[i];
Vector2d v = Position(t);
double dist = Vector2d.SqrDistance(v, p);
if (dist < min)
{
min = dist;
closest = v;
}
}
return(closest);
}
/// <summary>
/// The closest point on the curve to the point p.
/// </summary>
public Vector2f Closest(Vector2f 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;
Vector2f closest = new Vector2f(min, min);
for (int i = 0; i < roots.real; i++)
{
float t = (float)roots[i];
Vector2f v = Position(t);
float dist = Vector2f.SqrDistance(v, p);
if (dist < min)
{
min = dist;
closest = v;
}
}
return(closest);
}
/// <summary>
/// If the segment ab intersects the curve.
/// </summary>
public bool Intersects(Vector2d a, Vector2d b)
{
//coefficients of quadratic
Vector2d c2 = C0 + C1 * -2.0 + C2;
Vector2d c1 = C0 * -2.0 + C1 * 2.0;
//Convert line to normal form: ax + by + c = 0
//Find normal to line: negative inverse of original line's slope
Vector2d n = new Vector2d(a.y - b.y, b.x - a.x);
//c coefficient for normal form of line
double 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, Vector2d.Dot(n, c2), Vector2d.Dot(n, c1), Vector2d.Dot(n, C0) + c);
Vector2d 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++)
{
double t = roots[i];
if (t < 0.0 || t > 1.0)
{
continue;
}
Vector2d 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);
}
