Add geometry algorithms

Simple Geometry