3D Math: extend fmath.h and add fgeom.h

[snacchus]

This adds fgeom.h, which contains structs for vectors, quaternions and matrices plus functions that implement common 3D math operations.