Refactor, add some simple height map to geometry code