Add Binary Index Tree and modified GeometryUtils