Calculating the position relative to a joint

[roy-t]

I'm trying to construct a matrix that transforms a vector in world coordinates to a vector in hinge coordinates.
(my main goal is to see where the center of mass is relative to the joint so I know how to control the joint motor in a balancing script)

However I seem to be unable to find the world position of the joint which I need in order to construct that matrix. With generic btTypedConstraints I was not able to get far so I started looking if I could solve this problem for the btHingeConstraint since there you have easy access to the connected bodies.

This is what I came up with:

Code: Select all

	
	btTransform body_world_matrix;
	joint->getRigidBodyA().getMotionState()->getWorldTransform(body_world_matrix);

	btTransform world_to_joint = ( body_world_matrix * joint->getAFrame() ).inverse();	

But when I visualize this the result doesn't seem right. I'm not sure if `body_world_matrix * joint->getAFrame() ` gives me the world coordinates of the joint since there is so little documentation I'm not sure what kind of offset getAFrame will give me from the world position of the A body. Anybody know how to calculate this matrix, preferable for btTypedConstraints but if that is too hard only for btHingeConstraints?