geometry: KD Tree

Add KD Tree implementation.