Re-added a basic Matrix3D class. Will still need to go back to optimize and add some functions. Also did some optimizations to Vector3D class, but have not yet coded out the Set() function.