public double GetMinEpsilon(TPoint2D shortcutEnd, bool doExtremePointQueries)
{
double[] distances;
if (doExtremePointQueries)
{
//O(4log n)
distances = new []
{
Geometry2D.Distance(shortcutStart, shortcutEnd, upper.ExtremePointFromShortcutLine(shortcutEnd)),
Geometry2D.Distance(shortcutStart, shortcutEnd, lower.ExtremePointFromShortcutLine(shortcutEnd)),
upper.ExtremeDistanceLeftOfShortcut(shortcutEnd),
lower.ExtremeDistanceLeftOfShortcut(shortcutEnd)
};
}
else
{
//O(2log n)
distances = new []
{
upper.ExtremeDistanceLeftOfShortcut(shortcutEnd),
lower.ExtremeDistanceLeftOfShortcut(shortcutEnd)
};
}
return(distances.Max());
}
public override bool AfterShortcut(TPoint2D start, TPoint2D end)
{
//break when this is the last iteration
if (end.Index == Input.Trajectory.Count - 1 || end.Index == 0)
{
return(false);
}
var prunedLevels = new HashSet <int>();
foreach (var level in wedges.Keys)
{
var wedge = wedges[level];
wedge.Intersect(end, Input.GetEpsilon(level));
if (wedge.IsEmpty)
{
prunedLevels.Add(level);
}
}
foreach (var level in prunedLevels)
{
wedges.Remove(level);
}
return(wedges.Any());
}
public double ShortcutNormal(TPoint2D shortcutEnd)
{
//point normal up for upper hull, and down for lower hull
var leftNormal = ShortcutStart.X < shortcutEnd.X == IsUpper;
return(Normal(ShortcutStart, shortcutEnd, leftNormal));
}
public override void Clear()
{
ShortcutSet = null;
Source = null;
Targets = new JHashSet <TPoint2D>();
ShortcutSet = null;
}
//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 void RemovePoint(TPoint2D point)
{
foreach (var set in Shortcuts.Values)
{
set.Except(point);
}
}
public Wedge(TPoint2D origin, double startAngle, double endAngle)
{
Origin = origin;
StartAngle = Geometry2D.SimplifyRadians(startAngle);
EndAngle = Geometry2D.SimplifyRadians(endAngle);
IsFullPlane = false;
}
public SPFInput(Trajectory2D Trajectory, IShortcutSet ShortcutSet, TPoint2D Source, JHashSet <TPoint2D> Targets, bool CreatePath = true)
{
this.Trajectory = Trajectory;
this.ShortcutSet = ShortcutSet;
this.Source = Source;
this.Targets = Targets;
this.CreatePath = CreatePath;
}
public LinkedList <Interval> GetIntervals(TPoint2D point)
{
if (IntervalMap.ContainsKey(point))
{
return(IntervalMap[point]);
}
return(new LinkedList <Interval>());
}
public override void OnNewShortcutStart(TPoint2D start)
{
wedges = new Dictionary <int, Wedge>();
for (var level = 1; level <= Input.NumLevels; level++)
{
wedges[level] = new Wedge(start);
}
}
//informally: find the point where the attached edges are most parallel to (shortcutStart, shortcutEnd)
public TPoint2D ExtremePointFromShortcutLine(TPoint2D shortcutEnd)
{
var shortcutNormal = ShortcutNormal(shortcutEnd);
//get furthest point
TPoint2D point;
Points.TryFind(p => NormalBSTPredicate(p, shortcutNormal), out point);
return(point);
}
public ConvexHullHalf(TPoint2D shortcutStart, bool upper, ISearchTree <TPoint2D> points)
{
IsUpper = upper;
ShortcutStart = shortcutStart;
if (points != null)
{
Points = points;
InitializePoints();
}
}
public static int BSTPointComparison(TPoint2D p1, TPoint2D p2)
{
if (p1.X < p2.X)
{
return(-1);
}
if (p1.X > p2.X)
{
return(1);
}
return(0);
}
//worst case O(n^2), expected O(n)
public void Except(TPoint2D point)
{
var index = point.Index;
foreach (var start in IntervalMap.Keys)
{
if (start == point)
{
continue;
}
var intervals = IntervalMap[start];
var node = intervals.First;
while (node != null)
{
var nextNode = node.Next;
var interval = node.Value;
var startIndex = interval.Start.Index;
var endIndex = interval.End.Index;
if (startIndex == index && endIndex == index)
{
intervals.Remove(node);
}
else if (startIndex == index)
{
interval.Start = Trajectory[startIndex + 1];
}
else if (endIndex == index)
{
interval.End = Trajectory[endIndex - 1];
}
else if (startIndex < index && endIndex > index)
{
//second part
intervals.AddAfter(node, new Interval(Trajectory[startIndex + 1], interval.End));
//first part
interval.End = Trajectory[index - 1];
}
node = nextNode;
}
}
IntervalMap.Remove(point);
}
//ignores weight
public void PrependShortcut(TPoint2D start, TPoint2D end)
{
var intervalList = IntervalMap[start];
var firstInterval = intervalList.First?.Value;
if (firstInterval != null && firstInterval.Start.Index == end.Index + 1)
{
firstInterval.Start = end;
}
else
{
intervalList.AddFirst(new Interval(end, end));
}
}
//ignores weight
public void AppendShortcut(TPoint2D start, TPoint2D end)
{
var intervalList = IntervalMap[start];
var lastInterval = intervalList.Last?.Value;
if (lastInterval != null && lastInterval.End.Index == end.Index - 1)
{
lastInterval.End = end;
}
else
{
intervalList.AddLast(new Interval(end, end));
}
}
protected void DrawPoint(TPoint2D point, double radius, int numSegments, Color color, int?name = null)
{
GL.PushMatrix();
GL.Color3(color);
GL.Translate(point.X, point.Y, 1f);
if (name != null)
{
GL.LoadName((int)name);
}
GLUtil2D.DrawCircle(radius, numSegments);
GL.PopMatrix();
}
public bool Contains(TPoint2D point)
{
if (IsFullPlane)
{
return(true);
}
var A = Vector2d.Add(Origin.AsVector(), new Vector2d(Math.Cos(StartAngle), Math.Sin(StartAngle)));
var B = Vector2d.Add(Origin.AsVector(), new Vector2d(Math.Cos(EndAngle), Math.Sin(EndAngle)));
var AC = Geometry2D.Orient(Origin.AsVector(), A, point.AsVector());
var BC = Geometry2D.Orient(Origin.AsVector(), B, point.AsVector());
//either orientation is opposite or on the line AB
return(AC == 1 && BC == -1 || AC == 0 || BC == 0);
}
public void RemovePoint(TPoint2D point)
{
var shortcutsToRemove = new HashSet <Shortcut>();
foreach (var shortcut in MinLevels.Keys)
{
if (shortcut.Start == point || shortcut.End == point)
{
shortcutsToRemove.Add(shortcut);
}
}
foreach (var shortcut in shortcutsToRemove)
{
MinLevels.Remove(shortcut);
}
}
public void AppendInterval(TPoint2D start, int rangeStart, int rangeEnd)
{
var intervalList = IntervalMap[start];
var intervalStart = Trajectory[rangeStart];
var intervalEnd = Trajectory[rangeEnd];
var lastInterval = intervalList.Last?.Value;
if (lastInterval != null && lastInterval.End.Index == intervalStart.Index - 1)
{
lastInterval.End = intervalEnd;
}
else
{
intervalList.AddLast(new Interval(Trajectory[rangeStart], Trajectory[rangeEnd]));
}
}
private int RangeLeftOfShortcutLine(TPoint2D min, TPoint2D max, bool hullAngleCompliant, Vector2d lineStart, Vector2d lineEnd)
{
//find whether the start/end of the range represented by this subtree is to the left/right of this line
var minOrientation = Geometry2D.Orient(lineStart, lineEnd, min.AsVector());
var maxOrientation = Geometry2D.Orient(lineStart, lineEnd, max.AsVector());
//if one end of the range on the line, include/exclude based on the other end of the range
if (minOrientation == 0)
{
minOrientation = maxOrientation;
}
if (maxOrientation == 0)
{
maxOrientation = minOrientation;
}
if (maxOrientation == minOrientation)
{
//degenerate cases
if (min.X <= ShortcutStart.X && max.X >= ShortcutStart.X)
{
//range seems to be on the left of the line, but angle not compliant
if (!hullAngleCompliant && maxOrientation == 1)
{
return(0);
}
//range seems to be on the right of the line, but angle is compliant
if (hullAngleCompliant && maxOrientation == -1)
{
return(0);
}
}
//degenerate case: both ends of the range lie perfectly on the line
if (maxOrientation == 0)
{
return(hullAngleCompliant ? 1 : -1);
}
return(maxOrientation);
}
return(0);
}
public void PrependInterval(TPoint2D start, int rangeStart, int rangeEnd)
{
var intervalList = IntervalMap[start];
var intervalStart = Trajectory[rangeStart];
var intervalEnd = Trajectory[rangeEnd];
var firstInterval = intervalList.First?.Value;
if (firstInterval != null && firstInterval.Start.Index == intervalEnd.Index + 1)
{
firstInterval.Start = intervalStart;
}
else
{
intervalList.AddFirst(new Interval(Trajectory[rangeStart], Trajectory[rangeEnd]));
}
}
public override ShortcutPath FindShortestPath(IShortcutSet set, TPoint2D source, TPoint2D target, bool createPath = true)
{
var input = new SPFInput
{
Trajectory = set.Trajectory,
ShortcutSet = set,
Source = source,
Targets = new JHashSet <TPoint2D> {
target
},
CreatePath = createPath
};
SPFOutput output;
algorithm.Compute(input, out output);
return(output.GetPath(target));
}
private void HandleMouseDown(object sender, MouseEventArgs e)
{
int pickId = Pick(e.X, e.Y);
if (e.Button == MouseButtons.Left)
{
if (PickManager.PickingHit(pickId))
{ //clicked on point
lastSelectedPoint = (TPoint2D)PickManager.GetPickedObject(pickId);
}
else
{ //clicked on empty space for new point
int index = input.Trajectory.IndexOf(lastSelectedPoint);
if (!input.Trajectory.Any()) //fresh trajectory
{
index = -1;
}
else if (index == -1) //last selected point was removed
{
index = input.Trajectory.Count - 1;
}
var worldCoord = GetWorldCoordinates(e.X, e.Y);
var point = input.Trajectory.InsertPoint(worldCoord.X, worldCoord.Y, index + 1);
PickManager.AssignPickId(point);
lastSelectedPoint = point;
}
draggingPoint = true;
}
else if (e.Button == MouseButtons.Right)
{
if (PickManager.PickingHit(pickId))
{
//clicked on point
TPoint2D pointToBeRemoved = (TPoint2D)PickManager.GetPickedObject(pickId);
input.Trajectory.RemovePoint(pointToBeRemoved);
}
}
Refresh();
}
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);
}
//used to find which points to remove when updating convex hull
private int UpdatePredicate(TPoint2D newPoint, TPoint2D point, bool start)
{
var leftOfPoint = newPoint.X < point.X;
var rightOfPoint = newPoint.X > point.X;
if (start && leftOfPoint)
{
return(-1);
}
if (!start && rightOfPoint)
{
return(1);
}
var normalOnTheLeft = IsUpper && start || !IsUpper && !start;
var normal = Normal(point, newPoint, normalOnTheLeft);
return(NormalBSTPredicate(point, normal));
}
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);
}
//returns whether to go left or right in the BST, given a certain point and certain target normal
//informally this predicate aims to find the point whose adjacent edges e1, e2 have normals n1, n2 such that
//n1 <= normal <= n2
protected int NormalBSTPredicate(TPoint2D point, double normal)
{
TPoint2D leftNeighbor, rightNeighbor;
Points.TryGetPredecessor(point, out leftNeighbor);
Points.TryGetSuccessor(point, out rightNeighbor);
//when at the start, pick left normal to point straight left
var leftNormal = leftNeighbor != null?Normal(leftNeighbor, point, IsUpper) : ConvertToHullAngle(Math.PI);
//when at the end, pick right normal to point straight right
var rightNormal = rightNeighbor != null?Normal(point, rightNeighbor, IsUpper) : ConvertToHullAngle(0.0);
var leftSign = Math.Sign(IsUpper ? normal - rightNormal : rightNormal - normal);
var rightSign = Math.Sign(IsUpper ? normal - leftNormal : leftNormal - normal);
//point was found
if (leftSign == -rightSign || leftSign == 0 || rightSign == 0)
{
return(0);
}
//go left
if (leftSign == rightSign && leftSign == 1)
{
return(-1);
}
//go right
if (leftSign == rightSign && leftSign == -1)
{
return(1);
}
throw new InvalidOperationException("Illegal case detected");
}
public abstract Dictionary <TPoint2D, ShortcutPath> FindShortestPaths(IShortcutSet set, TPoint2D source, JHashSet <TPoint2D> targets, bool createPaths = true);
public abstract ShortcutPath FindShortestPath(IShortcutSet set, TPoint2D source, TPoint2D target, bool createPath = true);
public CalibrationPoint()
{
_img = new Vector2();
_real = new Vector3();
_realGridPos = new TPoint2D<int>(-1, -1);
}
private void ComputeDistanceToNextPoint_Row(int x, int y, ref Vector2 d, out TPoint2D<int> fromPoint)
{
if(x > 1)
{
d = CalibGrid[y, x - 1].GravityCenter - CalibGrid[y, x - 2].GravityCenter;
fromPoint = new TPoint2D<int>(y: y, x: x - 1);
}
else if(x == 1)
{
// Adding second in row -> use d from point 'above'
d = CalibGrid[y - 1, x].GravityCenter - CalibGrid[y - 1, x - 1].GravityCenter;
fromPoint = new TPoint2D<int>(y: y, x: x - 1);
}
else // x == 0
{
// Adding first in row -> move in y direction from 'above'
d = CalibGrid[y - 1, x].GravityCenter - CalibGrid[y - 2, x].GravityCenter;
fromPoint = new TPoint2D<int>(y: y - 1, x: x);
}
}
private void ComputeDistanceToNextPoint_Column(int x, int y, ref Vector2 d, out TPoint2D<int> fromPoint)
{
if(y > 1)
{
d = CalibGrid[y - 1, x].GravityCenter - CalibGrid[y - 2, x].GravityCenter;
fromPoint = new TPoint2D<int>(y: y - 1, x: x);
}
else if(y == 1)
{
// Adding second in column -> use d from point on 'left'
d = CalibGrid[y, x - 1].GravityCenter - CalibGrid[y - 1, x - 1].GravityCenter;
fromPoint = new TPoint2D<int>(y: y - 1, x: x);
}
else //y == 0
{
// Adding first in column -> move in x direction on 'left'
d = CalibGrid[y, x - 1].GravityCenter - CalibGrid[y, x - 2].GravityCenter;
fromPoint = new TPoint2D<int>(y: y, x: x - 1);
}
}
private void ComputeEstimatedPoint(int x, int y, Vector2 d, TPoint2D<int> fromPoint, out Vector2 eP, out int minIndex, out double edx, out double edy)
{
eP = CalibGrid[fromPoint.Y, fromPoint.X].GravityCenter + d;
minIndex = 0;
double minDist = (CalibShapes[0].GravityCenter - eP).LengthSquared();
for(int i = 0; i < CalibShapes.Count; ++i)
{
double dist = (CalibShapes[i].GravityCenter - eP).LengthSquared();
if(minDist > dist)
{
minDist = dist;
minIndex = i;
}
}
edx = ((CalibShapes[minIndex].GravityCenter - eP) * AxisX).LengthSquared();
edy = ((CalibShapes[minIndex].GravityCenter - eP) * AxisY).LengthSquared();
}
