Add Mat4D identity matrix
Rearrange operations in Mat4D and add transpose
Conversion to Vec4D from Vec3D and Point3D