Updated geometry.cpp with more functions