private bool IsPositionSeen(PositionAndOrientation point)
{
List <BFSNode> matchingPositionNodes = seen_.GetObjects(new Rectangle(
(int)point.x - locationTolerance_ / 2, (int)point.y - locationTolerance_ / 2,
locationTolerance_, locationTolerance_));
return(matchingPositionNodes.Any(
x => MathTools.AnglesEqual(x.position.angle, point.angle, angleTolerance_)));
}
private bool ArePointsSame(PositionAndOrientation a, PositionAndOrientation b)
{
return(Math.Pow(a.x - b.x, 2.0d) + Math.Pow(a.y - b.y, 2.0d) <= locationToleranceSquared_
&&
MathTools.AnglesEqual(a.angle, b.angle, angleTolerance_));
}