子弹物理 - 获取枢轴的世界位置

Question

我正在研究布娃娃，需要获得两个受约束的刚体之间的枢轴的世界位置。约束的类型是 btGeneric6DofConstraint 我的设置是这样的... ...

localA.setIdentity(); 
localA.setOrigin(btVector3(btScalar(-0.2), btScalar(0.15), btScalar(0.)));

localB.setIdentity();
localB.getBasis().setEulerZYX(SIMD_HALF_PI, 0, -SIMD_HALF_PI);
localB.setOrigin(btVector3(btScalar(0.), btScalar(-0.18), btScalar(0.)));

joint6DOF = new btGeneric6DofConstraint(*m_bodies[BODYPART_SPINE], *m_bodies[BODYPART_LEFT_UPPER_ARM], localA, localB, useLinearReferenceFrameA);
joint6DOF->setAngularLowerLimit(btVector3(-SIMD_PI * 0.8f, -SIMD_EPSILON, -SIMD_PI * 0.5f));
joint6DOF->setAngularUpperLimit(btVector3(SIMD_PI * 0.8f, SIMD_EPSILON, SIMD_PI * 0.5f));

m_joints[JOINT_LEFT_SHOULDER] = joint6DOF;
m_ownerWorld->addConstraint(m_joints[JOINT_LEFT_SHOULDER], true);

我在其他地方能找到的最接近的答案是 这个这暗示了数学在里面 btConeTwistConstraint::buildJacobian() 它设置了枢轴的初始位置（我想），但我一直无法从中得到任何有意义的结果。下面是我的尝试。

btVector3 originA = ragdoll->m_joints[i]->getRigidBodyA().getWorldTransform().getOrigin();
btVector3 originB = ragdoll->m_joints[i]->getRigidBodyB().getWorldTransform().getOrigin();
btTransform comA = ragdoll->m_joints[i]->getRigidBodyA().getCenterOfMassTransform();
btTransform comB = ragdoll->m_joints[i]->getRigidBodyB().getCenterOfMassTransform();

btVector3 relPos = (comA * originA) - (comB * originB);