Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Merge remote-tracking branch 'bp/master'

Browse Source
This commit is contained in:
erwincoumans
2016-10-12 23:07:41 -07:00
parent 33d9603e16 729ae8d3d6
commit 1a62f21143
2 changed files with 31 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files

25
examples/SharedMemory/PhysicsClientSharedMemory.cpp
View File

@@ -615,16 +615,27 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
b3Warning("Contact Point Information Request failed"); b3Warning("Contact Point Information Request failed");
break; break;
} }
case CMD_SAVE_WORLD_COMPLETED:
break;
case CMD_SAVE_WORLD_COMPLETED:
break;
case CMD_SAVE_WORLD_FAILED: case CMD_SAVE_WORLD_FAILED:
{ {
b3Warning("Saving world failed"); b3Warning("Saving world failed");
break; break;
} }
case CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED:
{
break;
}
case CMD_CALCULATE_INVERSE_KINEMATICS_FAILED:
{
b3Warning("Calculate Inverse Kinematics Request failed");
break;
}
default: { default: {
b3Error("Unknown server status\n"); b3Error("Unknown server status %d\n", serverCmd.m_type);
btAssert(0); btAssert(0);
} }
}; };

24
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -1981,16 +1981,16 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
btVector3 linkLocalInertialOrigin = body->m_linkLocalInertialFrames[l].getOrigin(); btVector3 linkLocalInertialOrigin = body->m_linkLocalInertialFrames[l].getOrigin();
btQuaternion linkLocalInertialRotation = body->m_linkLocalInertialFrames[l].getRotation(); btQuaternion linkLocalInertialRotation = body->m_linkLocalInertialFrames[l].getRotation();
btVector3 linkOrigin = mb->getLink(l).m_cachedWorldTransform.getOrigin(); btVector3 linkCOMOrigin = mb->getLink(l).m_cachedWorldTransform.getOrigin();
btQuaternion linkRotation = mb->getLink(l).m_cachedWorldTransform.getRotation(); btQuaternion linkCOMRotation = mb->getLink(l).m_cachedWorldTransform.getRotation();
serverCmd.m_sendActualStateArgs.m_linkState[l*7+0] = linkOrigin.getX(); serverCmd.m_sendActualStateArgs.m_linkState[l*7+0] = linkCOMOrigin.getX();
serverCmd.m_sendActualStateArgs.m_linkState[l*7+1] = linkOrigin.getY(); serverCmd.m_sendActualStateArgs.m_linkState[l*7+1] = linkCOMOrigin.getY();
serverCmd.m_sendActualStateArgs.m_linkState[l*7+2] = linkOrigin.getZ(); serverCmd.m_sendActualStateArgs.m_linkState[l*7+2] = linkCOMOrigin.getZ();
serverCmd.m_sendActualStateArgs.m_linkState[l*7+3] = linkRotation.x(); serverCmd.m_sendActualStateArgs.m_linkState[l*7+3] = linkCOMRotation.x();
serverCmd.m_sendActualStateArgs.m_linkState[l*7+4] = linkRotation.y(); serverCmd.m_sendActualStateArgs.m_linkState[l*7+4] = linkCOMRotation.y();
serverCmd.m_sendActualStateArgs.m_linkState[l*7+5] = linkRotation.z(); serverCmd.m_sendActualStateArgs.m_linkState[l*7+5] = linkCOMRotation.z();
serverCmd.m_sendActualStateArgs.m_linkState[l*7+6] = linkRotation.w(); serverCmd.m_sendActualStateArgs.m_linkState[l*7+6] = linkCOMRotation.w();
serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+0] = linkLocalInertialOrigin.getX(); serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+0] = linkLocalInertialOrigin.getX();
serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+1] = linkLocalInertialOrigin.getY(); serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+1] = linkLocalInertialOrigin.getY();
@@ -2879,12 +2879,14 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
} }
ikHelperPtr->computeNullspaceVel(numDofs, &q_current[0], &lower_limit[0], &upper_limit[0], &joint_range[0], &rest_pose[0]); ikHelperPtr->computeNullspaceVel(numDofs, &q_current[0], &lower_limit[0], &upper_limit[0], &joint_range[0], &rest_pose[0]);
} }
btTransform endEffectorTransformWorld = bodyHandle->m_multiBody->getLink(endEffectorLinkIndex).m_cachedWorldTransform * bodyHandle->m_linkLocalInertialFrames[endEffectorLinkIndex].inverse();
btVector3DoubleData endEffectorWorldPosition; btVector3DoubleData endEffectorWorldPosition;
btVector3DoubleData endEffectorWorldOrientation; btVector3DoubleData endEffectorWorldOrientation;
btVector3 endEffectorPosWorld = bodyHandle->m_multiBody->getLink(endEffectorLinkIndex).m_cachedWorldTransform.getOrigin(); btVector3 endEffectorPosWorld = endEffectorTransformWorld.getOrigin();
btQuaternion endEffectorOriWorld = bodyHandle->m_multiBody->getLink(endEffectorLinkIndex).m_cachedWorldTransform.getRotation(); btQuaternion endEffectorOriWorld = endEffectorTransformWorld.getRotation();
btVector4 endEffectorOri(endEffectorOriWorld.x(),endEffectorOriWorld.y(),endEffectorOriWorld.z(),endEffectorOriWorld.w()); btVector4 endEffectorOri(endEffectorOriWorld.x(),endEffectorOriWorld.y(),endEffectorOriWorld.z(),endEffectorOriWorld.w());
endEffectorPosWorld.serializeDouble(endEffectorWorldPosition); endEffectorPosWorld.serializeDouble(endEffectorWorldPosition);