Changed the sorting to preserve the original point indices.
Added GeometryUtils so we don't stick everything in Intersector.