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

Split Bullet/src/LinearMath/btSerializer.cpp into btSerializer64.cpp to make it easier to rebuild serialization structure.

Browse Source 
Add several MSVC optimization flags to cmake.
Bump up VERSION because serialization format changed
Expose btScalar& jointMaxForce, btScalar& jointMaxVelocity to 'getJointInfo2' API, add backwards compatibility to examples\Importers\ImportURDFDemo\URDFImporterInterface::getJointInfo.

pybullet: expose 4 more fields to getJointInfo: jointLowerLimit/jointUpperLimit/jointMaxForce/jointMaxVelocity
fix performance issue in CMD_ACTUAL_STATE_UPDATE_COMPLETED
This commit is contained in:
Erwin Coumans
2017-03-26 13:06:46 -07:00
parent ed6530264f
commit 7503418c72
32 changed files with 1412 additions and 1026 deletions
Download Patch File Download Diff File Expand all files Collapse all files

135
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -1758,6 +1758,7 @@ int PhysicsServerCommandProcessor::createBodyInfoStream(int bodyUniqueId, char*
bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes )
{
BT_PROFILE("processCommand");
bool hasStatus = false;
@@ -1818,6 +1819,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
#endif
case CMD_STATE_LOGGING:
{
BT_PROFILE("CMD_STATE_LOGGING");
serverStatusOut.m_type = CMD_STATE_LOGGING_FAILED;
hasStatus = true;
@@ -1934,6 +1937,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
case CMD_SET_VR_CAMERA_STATE:
{
BT_PROFILE("CMD_SET_VR_CAMERA_STATE");
if (clientCmd.m_updateFlags & VR_CAMERA_ROOT_POSITION)
{
@@ -1960,6 +1964,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
case CMD_REQUEST_VR_EVENTS_DATA:
{
BT_PROFILE("CMD_REQUEST_VR_EVENTS_DATA");
serverStatusOut.m_sendVREvents.m_numVRControllerEvents = 0;
for (int i=0;i<MAX_VR_CONTROLLERS;i++)
{
@@ -1981,6 +1987,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
case CMD_REQUEST_KEYBOARD_EVENTS_DATA:
{
BT_PROFILE("CMD_REQUEST_KEYBOARD_EVENTS_DATA");
serverStatusOut.m_sendKeyboardEvents.m_numKeyboardEvents = m_data->m_keyboardEvents.size();
if (serverStatusOut.m_sendKeyboardEvents.m_numKeyboardEvents>MAX_KEYBOARD_EVENTS)
{
@@ -2016,6 +2024,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
case CMD_REQUEST_RAY_CAST_INTERSECTIONS:
{
BT_PROFILE("CMD_REQUEST_RAY_CAST_INTERSECTIONS");
btVector3 rayFromWorld(clientCmd.m_requestRaycastIntersections.m_rayFromPosition[0],
clientCmd.m_requestRaycastIntersections.m_rayFromPosition[1],
clientCmd.m_requestRaycastIntersections.m_rayFromPosition[2]);
@@ -2075,6 +2085,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
};
case CMD_REQUEST_DEBUG_LINES:
{
BT_PROFILE("CMD_REQUEST_DEBUG_LINES");
int curFlags =m_data->m_remoteDebugDrawer->getDebugMode();
int debugMode = clientCmd.m_requestDebugLinesArguments.m_debugMode;//clientCmd.btIDebugDraw::DBG_DrawWireframe|btIDebugDraw::DBG_DrawAabb;
@@ -2144,7 +2156,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
case CMD_REQUEST_CAMERA_IMAGE_DATA:
{
BT_PROFILE("CMD_REQUEST_CAMERA_IMAGE_DATA");
int startPixelIndex = clientCmd.m_requestPixelDataArguments.m_startPixelIndex;
int width = clientCmd.m_requestPixelDataArguments.m_pixelWidth;
int height = clientCmd.m_requestPixelDataArguments.m_pixelHeight;
@@ -2273,6 +2285,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
case CMD_SYNC_BODY_INFO:
{
BT_PROFILE("CMD_SYNC_BODY_INFO");
int numHandles = m_data->getNumHandles();
int actualNumBodies = 0;
for (int i=0;i<numHandles;i++)
@@ -2301,6 +2315,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
case CMD_REQUEST_BODY_INFO:
{
BT_PROFILE("CMD_REQUEST_BODY_INFO");
const SdfRequestInfoArgs& sdfInfoArgs = clientCmd.m_sdfRequestInfoArgs;
//stream info into memory
int streamSizeInBytes = createBodyInfoStream(sdfInfoArgs.m_bodyUniqueId, bufferServerToClient, bufferSizeInBytes);
@@ -2314,6 +2330,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
case CMD_SAVE_WORLD:
{
BT_PROFILE("CMD_SAVE_WORLD");
///this is a very rudimentary way to save the state of the world, for scene authoring
///many todo's, for example save the state of motor controllers etc.
@@ -2549,6 +2567,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
case CMD_LOAD_SDF:
{
BT_PROFILE("CMD_LOAD_SDF");
const SdfArgs& sdfArgs = clientCmd.m_sdfArguments;
if (m_data->m_verboseOutput)
{
@@ -2581,7 +2601,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
case CMD_LOAD_URDF:
{
BT_PROFILE("CMD_LOAD_URDF");
const UrdfArgs& urdfArgs = clientCmd.m_urdfArguments;
if (m_data->m_verboseOutput)
{
@@ -2686,6 +2706,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
case CMD_CREATE_SENSOR:
{
BT_PROFILE("CMD_CREATE_SENSOR");
if (m_data->m_verboseOutput)
{
b3Printf("Processed CMD_CREATE_SENSOR");
@@ -2754,6 +2776,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
case CMD_SEND_DESIRED_STATE:
{
BT_PROFILE("CMD_SEND_DESIRED_STATE");
if (m_data->m_verboseOutput)
{
b3Printf("Processed CMD_SEND_DESIRED_STATE");
@@ -2946,6 +2969,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
case CMD_REQUEST_ACTUAL_STATE:
{
BT_PROFILE("CMD_REQUEST_ACTUAL_STATE");
if (m_data->m_verboseOutput)
{
b3Printf("Sending the actual state (Q,U)");
@@ -3019,6 +3043,20 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
serverCmd.m_sendActualStateArgs.m_actualStateQdot[5] = mb->getBaseOmega()[2];
totalDegreeOfFreedomU += 6;//3 linear and 3 angular DOF
}
btAlignedObjectArray<btVector3> omega;
btAlignedObjectArray<btVector3> linVel;
bool computeLinkVelocities = ((clientCmd.m_updateFlags & ACTUAL_STATE_COMPUTE_LINKVELOCITY)!=0);
if (computeLinkVelocities)
{
omega.resize(mb->getNumLinks()+1);
linVel.resize(mb->getNumLinks()+1);
{
B3_PROFILE("compTreeLinkVelocities");
mb->compTreeLinkVelocities(&omega[0], &linVel[0]);
}
}
for (int l=0;l<mb->getNumLinks();l++)
{
for (int d=0;d<mb->getLink(l).m_posVarCount;d++)
@@ -3068,28 +3106,46 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
//}
}
}
btVector3 linkLocalInertialOrigin = body->m_linkLocalInertialFrames[l].getOrigin();
btQuaternion linkLocalInertialRotation = body->m_linkLocalInertialFrames[l].getRotation();
btVector3 linkLocalInertialOrigin = body->m_linkLocalInertialFrames[l].getOrigin();
btQuaternion linkLocalInertialRotation = body->m_linkLocalInertialFrames[l].getRotation();
btVector3 linkCOMOrigin = mb->getLink(l).m_cachedWorldTransform.getOrigin();
btQuaternion linkCOMRotation = mb->getLink(l).m_cachedWorldTransform.getRotation();
btVector3 linkCOMOrigin = mb->getLink(l).m_cachedWorldTransform.getOrigin();
btQuaternion linkCOMRotation = mb->getLink(l).m_cachedWorldTransform.getRotation();
serverCmd.m_sendActualStateArgs.m_linkState[l*7+0] = linkCOMOrigin.getX();
serverCmd.m_sendActualStateArgs.m_linkState[l*7+1] = linkCOMOrigin.getY();
serverCmd.m_sendActualStateArgs.m_linkState[l*7+2] = linkCOMOrigin.getZ();
serverCmd.m_sendActualStateArgs.m_linkState[l*7+3] = linkCOMRotation.x();
serverCmd.m_sendActualStateArgs.m_linkState[l*7+4] = linkCOMRotation.y();
serverCmd.m_sendActualStateArgs.m_linkState[l*7+5] = linkCOMRotation.z();
serverCmd.m_sendActualStateArgs.m_linkState[l*7+6] = linkCOMRotation.w();
serverCmd.m_sendActualStateArgs.m_linkState[l*7+0] = linkCOMOrigin.getX();
serverCmd.m_sendActualStateArgs.m_linkState[l*7+1] = linkCOMOrigin.getY();
serverCmd.m_sendActualStateArgs.m_linkState[l*7+2] = linkCOMOrigin.getZ();
serverCmd.m_sendActualStateArgs.m_linkState[l*7+3] = linkCOMRotation.x();
serverCmd.m_sendActualStateArgs.m_linkState[l*7+4] = linkCOMRotation.y();
serverCmd.m_sendActualStateArgs.m_linkState[l*7+5] = linkCOMRotation.z();
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+1] = linkLocalInertialOrigin.getY();
serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+2] = linkLocalInertialOrigin.getZ();
serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+3] = linkLocalInertialRotation.x();
serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+4] = linkLocalInertialRotation.y();
serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+5] = linkLocalInertialRotation.z();
serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+6] = linkLocalInertialRotation.w();
btVector3 worldLinVel(0,0,0);
btVector3 worldAngVel(0,0,0);
if (computeLinkVelocities)
{
const btMatrix3x3& linkRotMat = mb->getLink(l).m_cachedWorldTransform.getBasis();
worldLinVel = linkRotMat * linVel[l+1];
worldAngVel = linkRotMat * omega[l+1];
}
serverCmd.m_sendActualStateArgs.m_linkWorldVelocities[l*6+0] = worldLinVel[0];
serverCmd.m_sendActualStateArgs.m_linkWorldVelocities[l*6+1] = worldLinVel[1];
serverCmd.m_sendActualStateArgs.m_linkWorldVelocities[l*6+2] = worldLinVel[2];
serverCmd.m_sendActualStateArgs.m_linkWorldVelocities[l*6+3] = worldAngVel[0];
serverCmd.m_sendActualStateArgs.m_linkWorldVelocities[l*6+4] = worldAngVel[1];
serverCmd.m_sendActualStateArgs.m_linkWorldVelocities[l*6+5] = worldAngVel[2];
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+2] = linkLocalInertialOrigin.getZ();
serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+3] = linkLocalInertialRotation.x();
serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+4] = linkLocalInertialRotation.y();
serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+5] = linkLocalInertialRotation.z();
serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+6] = linkLocalInertialRotation.w();
}
@@ -3152,6 +3208,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
case CMD_STEP_FORWARD_SIMULATION:
{
BT_PROFILE("CMD_STEP_FORWARD_SIMULATION");
if (m_data->m_verboseOutput)
{
@@ -3186,6 +3244,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
case CMD_REQUEST_INTERNAL_DATA:
{
BT_PROFILE("CMD_REQUEST_INTERNAL_DATA");
//todo: also check version etc?
SharedMemoryStatus& serverCmd = serverStatusOut;
@@ -3208,6 +3268,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
};
case CMD_SEND_PHYSICS_SIMULATION_PARAMETERS:
{
BT_PROFILE("CMD_SEND_PHYSICS_SIMULATION_PARAMETERS");
if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_DELTA_TIME)
{
m_data->m_physicsDeltaTime = clientCmd.m_physSimParamArgs.m_deltaTime;
@@ -3283,6 +3345,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
};
case CMD_INIT_POSE:
{
BT_PROFILE("CMD_INIT_POSE");
if (m_data->m_verboseOutput)
{
b3Printf("Server Init Pose not implemented yet");
@@ -3431,7 +3495,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
case CMD_RESET_SIMULATION:
{
BT_PROFILE("CMD_RESET_SIMULATION");
resetSimulation();
SharedMemoryStatus& serverCmd =serverStatusOut;
@@ -3442,6 +3507,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
case CMD_CREATE_RIGID_BODY:
case CMD_CREATE_BOX_COLLISION_SHAPE:
{
BT_PROFILE("CMD_CREATE_RIGID_BODY");
btVector3 halfExtents(1,1,1);
if (clientCmd.m_updateFlags & BOX_SHAPE_HAS_HALF_EXTENTS)
{
@@ -3576,6 +3643,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
case CMD_PICK_BODY:
{
BT_PROFILE("CMD_PICK_BODY");
pickBody(btVector3(clientCmd.m_pickBodyArguments.m_rayFromWorld[0],
clientCmd.m_pickBodyArguments.m_rayFromWorld[1],
clientCmd.m_pickBodyArguments.m_rayFromWorld[2]),
@@ -3591,6 +3660,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
case CMD_MOVE_PICKED_BODY:
{
BT_PROFILE("CMD_MOVE_PICKED_BODY");
movePickedBody(btVector3(clientCmd.m_pickBodyArguments.m_rayFromWorld[0],
clientCmd.m_pickBodyArguments.m_rayFromWorld[1],
clientCmd.m_pickBodyArguments.m_rayFromWorld[2]),
@@ -3605,6 +3676,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
case CMD_REMOVE_PICKING_CONSTRAINT_BODY:
{
BT_PROFILE("CMD_REMOVE_PICKING_CONSTRAINT_BODY");
removePickingConstraint();
SharedMemoryStatus& serverCmd =serverStatusOut;
@@ -3614,6 +3686,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
case CMD_REQUEST_AABB_OVERLAP:
{
BT_PROFILE("CMD_REQUEST_AABB_OVERLAP");
SharedMemoryStatus& serverCmd = serverStatusOut;
int curObjectIndex = clientCmd.m_requestOverlappingObjectsArgs.m_startingOverlappingObjectIndex;
@@ -3671,6 +3744,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
case CMD_CONFIGURE_OPENGL_VISUALIZER:
{
BT_PROFILE("CMD_CONFIGURE_OPENGL_VISUALIZER");
SharedMemoryStatus& serverCmd = serverStatusOut;
serverCmd.m_type =CMD_CLIENT_COMMAND_COMPLETED;
@@ -3694,6 +3768,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
case CMD_REQUEST_CONTACT_POINT_INFORMATION:
{
BT_PROFILE("CMD_REQUEST_CONTACT_POINT_INFORMATION");
SharedMemoryStatus& serverCmd =serverStatusOut;
serverCmd.m_sendContactPointArgs.m_numContactPointsCopied = 0;
@@ -4004,6 +4079,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
case CMD_CALCULATE_INVERSE_DYNAMICS:
{
BT_PROFILE("CMD_CALCULATE_INVERSE_DYNAMICS");
SharedMemoryStatus& serverCmd = serverStatusOut;
InternalBodyHandle* bodyHandle = m_data->getHandle(clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId);
if (bodyHandle && bodyHandle->m_multiBody)
@@ -4054,6 +4130,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
case CMD_CALCULATE_JACOBIAN:
{
BT_PROFILE("CMD_CALCULATE_JACOBIAN");
SharedMemoryStatus& serverCmd = serverStatusOut;
InternalBodyHandle* bodyHandle = m_data->getHandle(clientCmd.m_calculateJacobianArguments.m_bodyUniqueId);
if (bodyHandle && bodyHandle->m_multiBody)
@@ -4110,6 +4188,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
case CMD_APPLY_EXTERNAL_FORCE:
{
BT_PROFILE("CMD_APPLY_EXTERNAL_FORCE");
if (m_data->m_verboseOutput)
{
b3Printf("CMD_APPLY_EXTERNAL_FORCE clientCmd = %d\n", clientCmd.m_sequenceNumber);
@@ -4209,6 +4289,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
case CMD_USER_CONSTRAINT:
{
BT_PROFILE("CMD_USER_CONSTRAINT");
SharedMemoryStatus& serverCmd =serverStatusOut;
serverCmd.m_type = CMD_USER_CONSTRAINT_FAILED;
hasStatus = true;
@@ -4439,6 +4521,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
case CMD_CALCULATE_INVERSE_KINEMATICS:
{
BT_PROFILE("CMD_CALCULATE_INVERSE_KINEMATICS");
SharedMemoryStatus& serverCmd = serverStatusOut;
serverCmd.m_type = CMD_CALCULATE_INVERSE_KINEMATICS_FAILED;
@@ -4608,7 +4691,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
case CMD_REQUEST_VISUAL_SHAPE_INFO:
{
BT_PROFILE("CMD_REQUEST_VISUAL_SHAPE_INFO");
SharedMemoryStatus& serverCmd = serverStatusOut;
serverCmd.m_type = CMD_VISUAL_SHAPE_INFO_FAILED;
//retrieve the visual shape information for a specific body
@@ -4637,6 +4720,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
case CMD_UPDATE_VISUAL_SHAPE:
{
BT_PROFILE("CMD_UPDATE_VISUAL_SHAPE");
SharedMemoryStatus& serverCmd = serverStatusOut;
serverCmd.m_type = CMD_VISUAL_SHAPE_UPDATE_FAILED;
@@ -4649,6 +4733,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
case CMD_LOAD_TEXTURE:
{
BT_PROFILE("CMD_LOAD_TEXTURE");
SharedMemoryStatus& serverCmd = serverStatusOut;
serverCmd.m_type = CMD_LOAD_TEXTURE_FAILED;
@@ -4668,7 +4753,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
case CMD_LOAD_BULLET:
{
BT_PROFILE("CMD_LOAD_BULLET");
SharedMemoryStatus& serverCmd = serverStatusOut;
btBulletWorldImporter* importer = new btBulletWorldImporter(m_data->m_dynamicsWorld);
@@ -4739,6 +4824,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
case CMD_SAVE_BULLET:
{
BT_PROFILE("CMD_SAVE_BULLET");
SharedMemoryStatus& serverCmd = serverStatusOut;
FILE* f = fopen(clientCmd.m_fileArguments.m_fileName, "wb");
@@ -4758,6 +4844,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
case CMD_LOAD_MJCF:
{
BT_PROFILE("CMD_LOAD_MJCF");
SharedMemoryStatus& serverCmd = serverStatusOut;
serverCmd.m_type = CMD_MJCF_LOADING_FAILED;
const MjcfArgs& mjcfArgs = clientCmd.m_mjcfArguments;
@@ -4792,6 +4879,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
case CMD_USER_DEBUG_DRAW:
{
BT_PROFILE("CMD_USER_DEBUG_DRAW");
SharedMemoryStatus& serverCmd = serverStatusOut;
serverCmd.m_type = CMD_USER_DEBUG_DRAW_FAILED;
hasStatus = true;
@@ -4916,6 +5004,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
default:
{
BT_PROFILE("CMD_UNKNOWN");
b3Error("Unknown command encountered");
SharedMemoryStatus& serverCmd =serverStatusOut;