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_)); }