/*****************************************
Inspiration for finding the closest point :
http://stackoverflow.com/questions/849211/shortest-distance-between-a-point-and-a-line-segment/1501725#1501725
******************************************/
/// <summary>
/// Find the closest point on a segment from another point. The segment is expected to be a straight line between two points
/// </summary>
/// <param name="lineStart">The first point of the segment</param>
/// <param name="lineEnd">The last point of the segment</param>
/// <param name="point">The point projected on the line</param>
/// <param name="distance"></param>
/// <returns></returns>
public static Vertex GetClosestPointOnLine(Vertex lineStart, Vertex lineEnd, Vertex point, out double distance)
{
//Test if the line has a length <> 0
double dist2 = ComputeSquareDistanceBetweenPoints(lineStart, lineEnd);
if (dist2 == 0)
{
distance = ComputeDistanceBetweenPoints(point, lineStart);
return lineStart;
}
//Compute the projection of on the line
double t = ((point.X - lineStart.X) * (lineEnd.X - lineStart.X) + (point.Y - lineStart.Y) * (lineEnd.Y - lineStart.Y)) / dist2;
if (t < 0)//point projection falls beyond the first node of the segment
{
distance = ComputeDistanceBetweenPoints(point, lineStart);
return lineStart;
}
else if (t > 1)//point projection falls beyond the first node of the segment
{
distance = ComputeDistanceBetweenPoints(point, lineEnd);
return lineEnd;
}
Vertex projectionPoint = new Vertex(lineStart.X + t * (lineEnd.X - lineStart.X), lineStart.Y + t * (lineEnd.Y - lineStart.Y));
distance = ComputeDistanceBetweenPoints(point, projectionPoint);
return projectionPoint;
}
/// <summary>
/// Fetch the distance between two points
/// </summary>
/// <param name="p1">A point</param>
/// <param name="p2">A point</param>
/// <returns></returns>
public static double ComputeDistanceBetweenPoints(Vertex p1, Vertex p2)
{
if (p1.X == p2.X && p1.Y == p2.Y)
{
return 0;
}
return Math.Sqrt(ComputeSquareDistanceBetweenPoints(p1, p2));
}
public ParabolaProblemInformation(Vertex focusPoint, Vertex directixSegmentStart, Vertex directixSegmentEnd, Vertex parabolaStart, Vertex parabolaEnd)
{
FocusPoint = focusPoint;
DirectixSegmentStart = directixSegmentStart;
DirectixSegmentEnd = directixSegmentEnd;
ParabolaStart = parabolaStart;
ParabolaEnd = parabolaEnd;
}
/// <summary>
/// Undo the rotation done for a point.
/// </summary>
/// <param name="p">The point to rotate.</param>
/// <param name="theta">The angle in radians</param>
/// <param name="shift_x">The translation along the x-axis used during the rotation.</param>
/// <param name="shift_y">The translation along the y-axis used during the rotation.</param>
/// <returns></returns>
public static Vertex Unrotate(Vertex p, double theta, double shift_x, double shift_y)
{
double cos = Math.Cos(theta);
double sin = Math.Sin(theta);
return new Vertex(
(p.X * cos) - (p.Y * sin) + shift_x,
(p.X * sin) + (p.Y * cos) + shift_y
);
}
/// <summary>
/// Rotate a point around another point (anchor)
/// </summary>
/// <param name="point">The anchor point used for the rotation</param>
/// <param name="theta">The angle for the rotation</param>
/// <returns>The rotated point</returns>
public static Vertex Rotate(Vertex point, double theta)
{
double t = -1 * theta;
double cos = Math.Cos(t);
double sin = Math.Sin(t);
return new Vertex(
(point.X * cos) - (point.Y * sin),
(point.X * sin) + (point.Y * cos)
);
}
public UnsolvableVertexException(ParabolaProblemInformation nonRotatedInformation, ParabolaProblemInformation rotatedInformation, Vertex boostVertex, Vertex computedVertex,
double distanceBoostVertexToFocus, double distanceComputedVertexToFocus, double distanceBoostVertexToDirectix, double distanceComputedVertexToDirectix)
{
InputParabolaProblemInfo = nonRotatedInformation;
RoatatedParabolaProblemInfo = rotatedInformation;
BoostVertex = boostVertex;
ComputedVertex = computedVertex;
DistanceBoostVertexToFocus = distanceBoostVertexToFocus;
DistanceComputedVertexToFocus = distanceComputedVertexToFocus;
DistanceBoostVertexToDirectix = distanceBoostVertexToDirectix;
DistanceComputedVertexToDirectix = distanceComputedVertexToDirectix;
}
/// <summary>
/// Compute the vector between two points
/// </summary>
/// <param name="lineStart">The first point of the segment</param>
/// <param name="lineEnd">The last point of the segment</param>
/// <returns></returns>
public static Vertex ComputeVector(Vertex lineStart, Vertex lineEnd)
{
return new Vertex(lineEnd.X - lineStart.X, lineEnd.Y - lineStart.Y);
}
/// <summary>
/// Fetch the square distance between 2 points
/// </summary>
/// <param name="p1">A point</param>
/// <param name="p2">A point</param>
/// <returns></returns>
public static double ComputeSquareDistanceBetweenPoints(Vertex p1, Vertex p2)
{
return Math.Pow(p1.X - p2.X, 2) + Math.Pow(p1.Y - p2.Y, 2);
}
/// <summary>
/// Compute a point on the line at a specific distance.
/// </summary>
/// <param name="p1">The first point of the line.</param>
/// <param name="p2">The last point of the line.</param>
/// <param name="distanceOnLine">The distance on the line where the point will be fetched.</param>
/// <returns></returns>
public static Vertex GetPointAtDistance(Vertex p1, Vertex p2, double distanceOnLine)
{
double dx = p2.X - p1.X;
double dy = p2.Y - p1.Y;
double l = Math.Sqrt(Math.Pow(dx,2) + Math.Pow(dy,2));
if(distanceOnLine > l)
throw new Exception ("Length is greater than the length of the segment");
return new Vertex(p1.X + dx/l * distanceOnLine, p1.Y + dy/l * distanceOnLine);
}
/// <summary>
/// Find the closest point on a segment from another point. The segment is expected to be a straight line between two points
/// </summary>
/// <param name="lineStart">The first point of the segment</param>
/// <param name="lineEnd">The last point of the segment</param>
/// <param name="point">The point projected on the line</param>
/// <returns></returns>
public static Vertex GetClosestPointOnLine(Vertex lineStart, Vertex lineEnd, Vertex point)
{
double distance = 0;
return GetClosestPointOnLine(lineStart, lineEnd, point, out distance);
}
/// <summary>
/// Rotate a point by an angle around another point (origin).
/// The rotation happen clockwise.
/// https://www.siggraph.org/education/materials/HyperGraph/modeling/mod_tran/2drota.htm
/// The orgin point is computed as the point moved by a shift on X and Y axis.
/// </summary>
/// <param name="p">The point to be rotated</param>
/// <param name="theta">The angle for the rotation (in radians).</param>
/// <param name="shift_x">The translation along the x-axis used during the rotation.</param>
/// <param name="shift_y">The translation along the y-axis used during the rotation.</param>
/// <returns></returns>
public static Vertex Rotate(Vertex p, double theta, double shift_x, double shift_y)
{
return Rotate(new Vertex(p.X - shift_x, p.Y - shift_y), theta);
}
/// <summary>
/// Interpolate parabola points between two points. The equation is computed by turning the input segment
/// into the directix of the parabola, and the input point into the focus of the parabola. The directix used
/// to solve the parabola is horizontal (parallel to x-axis).
/// </summary>
/// <param name="focus">The input point that will be used as a focus point.</param>
/// <param name="dir">The input segment that will be used a the directix</param>
/// <param name="par_start">The point on the parabola used as a starting point.</param>
/// <param name="par_end">The point on the parabola used as a ending point.</param>
/// <param name="max_distance">The maximum distance between 2 points on the parabola</param>
/// <param name="tolerance">The maximum distance between 2 points on the parabola</param>
/// <returns></returns>
public static List<Vertex> Densify(Vertex focus, Vertex dir_start, Vertex dir_end, Vertex par_start, Vertex par_end, double max_distance, double tolerance)
{
if (max_distance <= 0)
throw new ArgumentOutOfRangeException(String.Format("The maximum distance must be greater than 0. Value passed: {0}", max_distance));
if (tolerance < 0)
throw new ArgumentOutOfRangeException(String.Format("The tolerance must be greater than or equal to 0. Value passed: {0}", tolerance));
#region Rotate Input Points
//Compute the information required to perform rotation
double shift_X = Math.Min(dir_start.X, dir_end.X);
double shift_Y = Math.Min(dir_start.Y, dir_end.Y);
double angle = GetLineAngleAsRadiant(dir_start, dir_end);
Vertex focus_rotated = Rotation.Rotate(
focus,
angle,
shift_X,
shift_Y
);
Vertex dir_startPoint_rotated = Rotation.Rotate(
dir_start,
angle,
shift_X,
shift_Y
);
Vertex dir_endPoint_rotated = Rotation.Rotate(
dir_end,
angle,
shift_X,
shift_Y);
Vertex par_startPoint_rotated = Rotation.Rotate(
par_start,
angle,
shift_X,
shift_Y
);
Vertex par_endPoint_rotated = Rotation.Rotate(
par_end,
angle,
shift_X,
shift_Y
);
#endregion
#region Validate the equation on first and last points given by Boost
//Set parabola parameters
double directrix = dir_endPoint_rotated.Y;
double snapTolerance = 5;
List<Vertex> densified_rotated = new List<Vertex>();
Stack<Vertex> next = new Stack<Vertex>();
ParabolaProblemInformation nonRotatedInformation = new ParabolaProblemInformation(
focus,
dir_start,
dir_end,
par_start,
par_end
);
double distanceFocusToDirectix = 0;
Distance.GetClosestPointOnLine(focus, dir_start, dir_end, out distanceFocusToDirectix);
if (distanceFocusToDirectix == 0)
throw new FocusOnDirectixException(nonRotatedInformation);
ParabolaProblemInformation rotatedInformation = new ParabolaProblemInformation(
focus_rotated,
dir_startPoint_rotated,
dir_endPoint_rotated,
par_startPoint_rotated,
par_endPoint_rotated
);
List<Tuple<Vertex, Vertex>> points = new List <Tuple<Vertex, Vertex>>();
points.Add(
Tuple.Create<Vertex, Vertex>(
par_startPoint_rotated,
new Vertex(par_startPoint_rotated.X, ParabolaY(par_startPoint_rotated.X, focus_rotated, directrix))
)
);
points.Add(
Tuple.Create<Vertex, Vertex>(
par_endPoint_rotated,
new Vertex(par_endPoint_rotated.X, ParabolaY(par_endPoint_rotated.X, focus_rotated, directrix))
)
);
foreach (var point in points)
{
double delta = point.Item1.Y > point.Item2.Y ?
point.Item1.Y - point.Item2.Y : point.Item2.Y - point.Item1.Y;
if (delta > snapTolerance)
{
GenerateParabolaIssueInformation(rotatedInformation, nonRotatedInformation, point.Item1, point.Item2, 0.001);
throw new Exception(
String.Format(
"The computed Y on the parabola for the starting / ending point is different from the rotated point returned by Boost. Difference: {0}",
delta)
);
}
}
#endregion
#region Compute Intermediate Points (Rotated)
Vertex previous = points[0].Item2;
densified_rotated.Add(previous);
next.Push(points[1].Item2);
while (next.Count > 0)
{
Vertex current = next.Peek();
double mid_cord_x = (previous.X + current.X) / 2;
Vertex mid_curve = new Vertex(mid_cord_x, ParabolaY(mid_cord_x, focus_rotated, directrix));
double distance = Distance.ComputeDistanceBetweenPoints(current, previous);
if (distance > max_distance)
{
next.Push(mid_curve);
}
else
{
next.Pop();
densified_rotated.Add(current);
previous = current;
}
}
#endregion
#region Unrotate and validate
List<Vertex> densified = densified_rotated.Select(w => Rotation.Unrotate(w, angle, shift_X, shift_Y)).ToList();
//reset the first and last points so they match exactly.
if (Math.Abs(densified[0].X - par_start.X) > snapTolerance ||
Math.Abs(densified[0].Y - par_start.Y) > snapTolerance)
throw new Exception(String.Format("Segmented curve start point is not correct. Tolerance exeeded in X ({0}) or Y ({1})",
Math.Abs(densified[0].X - par_start.X), Math.Abs(densified[0].Y - par_start.Y)));
densified[0] = par_start;
if (Math.Abs(densified[densified.Count - 1].X - par_end.X) > snapTolerance ||
Math.Abs(densified[densified.Count - 1].Y - par_end.Y) > snapTolerance)
throw new Exception(String.Format("Segmented curve end point is not correct. Tolerance exeeded in X ({0}) or Y ({1})",
Math.Abs(densified[densified.Count - 1].X - par_end.X), Math.Abs(densified[densified.Count - 1].Y - par_end.Y)));
densified[densified.Count - 1] = par_end;
#endregion
return densified;
}
/// <summary>
/// Find the position of y on the parabolar given focus and directix y. The equation to find a point on the parabola
/// given those two value is: (x−a)2+b2−c2=2(b−c)y so y=((x−a)2+b2−c2)/2(b−c)
/// </summary>
/// <param name="x">The x value for which a y value is wanted</param>
/// <param name="focus">The focus of the parabola</param>
/// <param name="directrix_y">The y value of the directx line</param>
/// <returns>The y value associated with x</returns>
static double ParabolaY(double x, Vertex focus, double directrix_y)
{
return (Math.Pow(x - focus.X, 2) + Math.Pow(focus.Y, 2) - Math.Pow(directrix_y, 2)) / (2 * (focus.Y - directrix_y));
}
/// <summary>
/// Return the angle between 2 points as radians
/// </summary>
/// <param name="start">The first point</param>
/// <param name="end">The second point</param>
/// <returns>The angle in radians</returns>
static double GetLineAngleAsRadiant(Vertex start, Vertex end)
{
return Math.Atan2(end.Y - start.Y, end.X - start.X);
}
/// <summary>
/// Generate an exception when the point computed by the parabola equation is different from the point computed by boost.
/// </summary>
/// <param name="rotatedInformation">The information used to solve parabola. This is the is the information before the rotation.</param>
/// <param name="nonRotatedInformation">The information used to solve parabola. This is the is the information after the rotation.</param>
/// <param name="boostPoint">The point on the parabola returned by Boost.</param>
/// <param name="parabolaPoint">The point on the parabola computed.</param>
/// <param name="tolerance">The tolerance used to decide if an exception need to be raise.</param>
private static void GenerateParabolaIssueInformation(ParabolaProblemInformation rotatedInformation, ParabolaProblemInformation nonRotatedInformation, Vertex boostPoint, Vertex parabolaPoint, double tolerance)
{
if (tolerance < 0)
throw new ArgumentOutOfRangeException(String.Format("Tolenrance must be greater than 0"));
double minX = Math.Min(Math.Min(Math.Min(rotatedInformation.DirectixSegmentStart.X, rotatedInformation.DirectixSegmentStart.X), boostPoint.X), parabolaPoint.X);
double maxX = Math.Max(Math.Max(Math.Max(rotatedInformation.DirectixSegmentStart.X, rotatedInformation.DirectixSegmentStart.X), boostPoint.X), parabolaPoint.X);
//Compute the distance between the input parabola point
double distanceBoostPointToFocus = Distance.ComputeDistanceBetweenPoints(boostPoint, rotatedInformation.FocusPoint);
double distanceBoostPointToDirectix = 0;
Distance.GetClosestPointOnLine(
new Vertex(minX, rotatedInformation.DirectixSegmentEnd.Y),
new Vertex(maxX, rotatedInformation.DirectixSegmentEnd.Y),
boostPoint,
out distanceBoostPointToDirectix
);
double distanceComputedPointToFocus = Distance.ComputeDistanceBetweenPoints(parabolaPoint, rotatedInformation.FocusPoint);
double distanceComputedPointToDirectix = 0;
Distance.GetClosestPointOnLine(
new Vertex(minX, rotatedInformation.DirectixSegmentEnd.Y),
new Vertex(maxX, rotatedInformation.DirectixSegmentEnd.Y),
parabolaPoint,
out distanceComputedPointToDirectix
);
double distanceDiff = distanceComputedPointToFocus > distanceComputedPointToDirectix ?
distanceComputedPointToFocus - distanceComputedPointToDirectix : distanceComputedPointToDirectix - distanceComputedPointToFocus;
if (distanceDiff < tolerance || Double.IsNaN(distanceDiff) || Double.IsInfinity(distanceDiff))
throw new UnsolvableVertexException(nonRotatedInformation, rotatedInformation, boostPoint, parabolaPoint,
distanceBoostPointToFocus, distanceComputedPointToFocus, distanceBoostPointToDirectix, distanceComputedPointToDirectix);
}