Basically spent the whole day figuring out how the 4×4 phantom matrix equates to the rendering matrix (I would have said OpenGL, but that’s not true anymore. I am using the lovely math libraries from the OpenGL SuperBible 5th Edition, which makes it kinda look like the OGL of Yore.

Initially I thought I’d just use the vector components of the rotation 3×3 from the Phantom to get the orientation of the tip, but for some reason, parts of the matrix appear inverted. So instead of using them directly, I multiply the modelviewmatrix by the phantom matrix, Amazingly, this works perfectly.

To make sure that this works, I rendered a sphere at the +X, +Y and +Z axis in the local coordinate frame. Everything tracks. So now I can create my gripper class and get the positions of the end effectors from the class. And since the position is in the global coordinate frame, it kind of comes along for free.

Here’s a picture of everything working:

Tomorrow, I’ll build the gripper class and start feeding that to the Phantom. The issue will be to sum the force vectors from all the end effectors in a reasonable way.