3D math library in C++