//calculates the normal of the two given points
public double Normal(TPoint2D p1, TPoint2D p2, bool leftNormal)
{
var vec = Vector2d.Subtract(p2.AsVector(), p1.AsVector());
var normal = leftNormal ? vec.PerpendicularLeft : vec.PerpendicularRight;
return(ConvertToHullAngle(Geometry2D.Angle(normal)));
}
public EnhancedPoint ExtremePointLeftOfShortcut(TPoint2D shortcutEnd)
{
//construct line
var startToEnd = Vector2d.Subtract(shortcutEnd.AsVector(), ShortcutStart.AsVector());
var normal = startToEnd.PerpendicularLeft;
var normalAngle = Geometry2D.Angle(normal);
var lineStart = ShortcutStart.AsVector();
var lineEnd = new Vector2d(ShortcutStart.X + Math.Cos(normalAngle), ShortcutStart.Y + Math.Sin(normalAngle));
//determine whether the angle towards the left is properly facing upwards (upper hull), or downwards (lower hull)
//used for degenerate case handling
var angleTowardsLeft = Geometry2D.Angle(-startToEnd.X, -startToEnd.Y);
var hullAngle = ConvertToHullAngle(angleTowardsLeft);
var hullAngleCompliant = HullAngleCompliant(hullAngle);
EnhancedPoint furthestPoint;
Points.TryFindRangeData((min, max) => RangeLeftOfShortcutLine(min, max, hullAngleCompliant, lineStart, lineEnd), out furthestPoint);
return(furthestPoint);
}
public void Intersect(TPoint2D shortcutEnd, double epsilon)
{
//angle between shortcut line and epsilon circles
var distance = Geometry2D.Distance(Origin, shortcutEnd);
//if within epsilon circle, keep wedge and continue with next
if (distance <= epsilon)
{
return;
}
//get angle of shortcut line with respect to the unit circle
var worldAngle = Geometry2D.Angle(Origin, shortcutEnd);
var wedgeAngle = Math.Asin(epsilon / distance);
//build wedge
var wedgeStart = Geometry2D.SubtractRadians(worldAngle, wedgeAngle);
var wedgeEnd = Geometry2D.SumRadians(worldAngle, wedgeAngle);
var newWedge = new Wedge(Origin, wedgeStart, wedgeEnd);
//intersect wedge
Intersect(newWedge);
}