Add debugging utility
Add some functions (geometry)