Fix applyExternalForce command for multibody links. Thanks to Avik for the report.

This commit is contained in:
Erwin Coumans
2020-06-19 03:37:30 -07:00
parent 4786ca1df2
commit ebef331bbf
@@ -10961,7 +10961,7 @@ bool PhysicsServerCommandProcessor::processApplyExternalForceCommand(const struc
int link = clientCmd.m_externalForceArguments.m_linkIds[i];
btVector3 forceWorld = isLinkFrame ? mb->getLink(link).m_cachedWorldTransform.getBasis() * tmpForce : tmpForce;
btVector3 relPosWorld = isLinkFrame ? mb->getLink(link).m_cachedWorldTransform.getBasis() * tmpPosition : tmpPosition - mb->getBaseWorldTransform().getOrigin();
btVector3 relPosWorld = isLinkFrame ? mb->getLink(link).m_cachedWorldTransform.getBasis() * tmpPosition : tmpPosition - mb->getLink(link).m_cachedWorldTransform.getOrigin();
mb->addLinkForce(link, forceWorld);
mb->addLinkTorque(link, relPosWorld.cross(forceWorld));
//b3Printf("apply link force of %f,%f,%f at %f,%f,%f\n", forceWorld[0],forceWorld[1],forceWorld[2], positionLocal[0],positionLocal[1],positionLocal[2]);