Scarfski/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3.git synced 2026-09-22 02:04:30 +00:00
Code Issues Packages Projects Releases Wiki Activity

External force and torque frame fix

Browse Source
This commit is contained in:
Felix Büttner
2021-09-02 17:40:18 +02:00
parent 3fee64fb95
commit 4b6ec20b30
2 changed files with 391 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files

43
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -11821,7 +11821,6 @@ bool PhysicsServerCommandProcessor::processApplyExternalForceCommand(const struc
else
{
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->getLink(link).m_cachedWorldTransform.getOrigin();
mb->addLinkForce(link, forceWorld);
@@ -11831,20 +11830,20 @@ bool PhysicsServerCommandProcessor::processApplyExternalForceCommand(const struc
}
if ((clientCmd.m_externalForceArguments.m_forceFlags[i] & EF_TORQUE) != 0)
{
btVector3 torqueLocal(clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 0],
clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 1],
clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 2]);
btVector3 tmpTorque(clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 0],
clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 1],
clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 2]);
if (clientCmd.m_externalForceArguments.m_linkIds[i] == -1)
{
btVector3 torqueWorld = isLinkFrame ? torqueLocal : mb->getBaseWorldTransform().getBasis() * torqueLocal;
btVector3 torqueWorld = isLinkFrame ? mb->getBaseWorldTransform().getBasis() * tmpTorque : tmpTorque;
mb->addBaseTorque(torqueWorld);
//b3Printf("apply base torque of %f,%f,%f\n", torqueWorld[0],torqueWorld[1],torqueWorld[2]);
}
else
{
int link = clientCmd.m_externalForceArguments.m_linkIds[i];
btVector3 torqueWorld = mb->getLink(link).m_cachedWorldTransform.getBasis() * torqueLocal;
btVector3 torqueWorld = isLinkFrame ? mb->getLink(link).m_cachedWorldTransform.getBasis() * tmpTorque : tmpTorque;
mb->addLinkTorque(link, torqueWorld);
//b3Printf("apply link torque of %f,%f,%f\n", torqueWorld[0],torqueWorld[1],torqueWorld[2]);
}
@@ -11856,26 +11855,26 @@ bool PhysicsServerCommandProcessor::processApplyExternalForceCommand(const struc
btRigidBody* rb = body->m_rigidBody;
if ((clientCmd.m_externalForceArguments.m_forceFlags[i] & EF_FORCE) != 0)
{
btVector3 forceLocal(clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 0],
clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 1],
clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 2]);
btVector3 positionLocal(
btVector3 tmpForce(clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 0],
clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 1],
clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 2]);
btVector3 tmpPosition(
clientCmd.m_externalForceArguments.m_positions[i * 3 + 0],
clientCmd.m_externalForceArguments.m_positions[i * 3 + 1],
clientCmd.m_externalForceArguments.m_positions[i * 3 + 2]);
btVector3 forceWorld = isLinkFrame ? forceLocal : rb->getWorldTransform().getBasis() * forceLocal;
btVector3 relPosWorld = isLinkFrame ? positionLocal : rb->getWorldTransform().getBasis() * positionLocal;
btVector3 forceWorld = isLinkFrame ? rb->getWorldTransform().getBasis() * tmpForce : tmpForce;
btVector3 relPosWorld = isLinkFrame ? rb->getWorldTransform().getBasis() * tmpPosition : tmpPosition - rb->getWorldTransform().getOrigin();
rb->applyForce(forceWorld, relPosWorld);
}
if ((clientCmd.m_externalForceArguments.m_forceFlags[i] & EF_TORQUE) != 0)
{
btVector3 torqueLocal(clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 0],
clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 1],
clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 2]);
btVector3 tmpTorque(clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 0],
clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 1],
clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 2]);
btVector3 torqueWorld = isLinkFrame ? torqueLocal : rb->getWorldTransform().getBasis() * torqueLocal;
btVector3 torqueWorld = isLinkFrame ? rb->getWorldTransform().getBasis() * tmpTorque : tmpTorque;
rb->applyTorque(torqueWorld);
}
}
@@ -11887,16 +11886,16 @@ bool PhysicsServerCommandProcessor::processApplyExternalForceCommand(const struc
int link = clientCmd.m_externalForceArguments.m_linkIds[i];
if ((clientCmd.m_externalForceArguments.m_forceFlags[i] & EF_FORCE) != 0)
{
btVector3 forceLocal(clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 0],
clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 1],
clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 2]);
btVector3 positionLocal(
btVector3 tmpForce(clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 0],
clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 1],
clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 2]);
btVector3 tmpPosition(
clientCmd.m_externalForceArguments.m_positions[i * 3 + 0],
clientCmd.m_externalForceArguments.m_positions[i * 3 + 1],
clientCmd.m_externalForceArguments.m_positions[i * 3 + 2]);
btVector3 forceWorld = isLinkFrame ? forceLocal : sb->getWorldTransform().getBasis() * forceLocal;
btVector3 relPosWorld = isLinkFrame ? positionLocal : sb->getWorldTransform().getBasis() * positionLocal;
btVector3 forceWorld = isLinkFrame ? sb->getWorldTransform().getBasis() * tmpForce : tmpForce;
btVector3 relPosWorld = isLinkFrame ? sb->getWorldTransform().getBasis() * tmpPosition : tmpPosition - sb->getWorldTransform().getOrigin();
if (link >= 0 && link < sb->m_nodes.size())
{
sb->addForce(forceWorld, link);