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