Update to new MathGeoLib. Closes #621