Merge pull request #2870 from erwincoumans/master

Fix applyExternalForce command for multibody links. Thanks to Avik fo…
This commit is contained in:
erwincoumans
2020-06-18 20:52:47 -07:00
committed by GitHub

View File

@@ -10951,7 +10951,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]);