math: Organize headers and separate Geometry implementation