Implemented 3D plotting of objects. Need to get C code working, then test with actual values