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

Merge pull request #2404 from erwincoumans/master

Browse Source 
fix issues in previous softbody commits (always check for m_multibodyWorld == 0 before using the pointer in ::render method)
This commit is contained in:
erwincoumans
2019-09-09 19:17:50 -07:00
committed by GitHub
parent 33260e9406 db9bc4f835
commit 80f12f8886
5 changed files with 761 additions and 730 deletions
Download Patch File Download Diff File Expand all files Collapse all files

28
examples/SharedMemory/PhysicsClientSharedMemory.cpp
View File

@@ -1529,20 +1529,46 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus()
{
B3_PROFILE("CMD_LOADING_COMPLETED");
int numConstraints = serverCmd.m_sdfLoadedArgs.m_numUserConstraints;
int numBodies = serverCmd.m_sdfLoadedArgs.m_numBodies;
if (serverCmd.m_type == CMD_SYNC_BODY_INFO_COMPLETED)
{
int* ids = (int*)m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor;
int* constraintUids = ids + numBodies;
for (int i = 0; i < numConstraints; i++)
{
int constraintUid = constraintUids[i];
m_data->m_constraintIdsRequestInfo.push_back(constraintUid);
}
}
else
{
for (int i = 0; i < numConstraints; i++)
{
int constraintUid = serverCmd.m_sdfLoadedArgs.m_userConstraintUniqueIds[i];
m_data->m_constraintIdsRequestInfo.push_back(constraintUid);
}
int numBodies = serverCmd.m_sdfLoadedArgs.m_numBodies;
}
if (numBodies > 0)
{
m_data->m_tempBackupServerStatus = m_data->m_lastServerStatus;
if (serverCmd.m_type == CMD_SYNC_BODY_INFO_COMPLETED)
{
int* bodyIds = (int*)m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor;
for (int i = 0; i < numBodies; i++)
{
m_data->m_bodyIdsRequestInfo.push_back(bodyIds[i]);
}
}
else
{
for (int i = 0; i < numBodies; i++)
{
m_data->m_bodyIdsRequestInfo.push_back(serverCmd.m_sdfLoadedArgs.m_bodyUniqueIds[i]);
}
}
int bodyId = m_data->m_bodyIdsRequestInfo[m_data->m_bodyIdsRequestInfo.size() - 1];
m_data->m_bodyIdsRequestInfo.pop_back();

50
examples/SharedMemory/PhysicsDirect.cpp
View File

@@ -688,7 +688,10 @@ void PhysicsDirect::processBodyJointInfo(int bodyUniqueId, const SharedMemorySta
{
bf.setFileDNAisMemoryDNA();
}
{
BT_PROFILE("bf.parse");
bf.parse(false);
}
BodyJointInfoCache2* bodyJoints = new BodyJointInfoCache2;
m_data->m_bodyJointMap.insert(bodyUniqueId, bodyJoints);
@@ -718,6 +721,7 @@ void PhysicsDirect::processBodyJointInfo(int bodyUniqueId, const SharedMemorySta
bodyJoints->m_baseName = mb->m_baseName;
}
addJointInfoFromMultiBodyData(mb, bodyJoints, m_data->m_verboseOutput);
}
}
if (bf.ok())
@@ -919,17 +923,57 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd
break;
}
case CMD_SYNC_BODY_INFO_COMPLETED:
clearCachedBodies();
case CMD_MJCF_LOADING_COMPLETED:
case CMD_SDF_LOADING_COMPLETED:
{
//we'll stream further info from the physics server
//so serverCmd will be invalid, make a copy
btAlignedObjectArray<int> bodyIdArray;
btAlignedObjectArray<int> constraintIdArray;
int numConstraints = serverCmd.m_sdfLoadedArgs.m_numUserConstraints;
int numBodies = serverCmd.m_sdfLoadedArgs.m_numBodies;
bodyIdArray.reserve(numBodies);
constraintIdArray.reserve(numConstraints);
if (serverCmd.m_type == CMD_SYNC_BODY_INFO_COMPLETED)
{
clearCachedBodies();
const int* bodyIds = (int*)m_data->m_bulletStreamDataServerToClient;
const int* constaintIds = bodyIds + numBodies;
for (int i = 0; i < numConstraints; i++)
{
int constraintUid = constaintIds[i];
constraintIdArray.push_back(constraintUid);
}
for (int i = 0; i < numBodies; i++)
{
int bodyUid = bodyIds[i];
bodyIdArray.push_back(bodyUid);
}
}
else
{
for (int i = 0; i < numConstraints; i++)
{
int constraintUid = serverCmd.m_sdfLoadedArgs.m_userConstraintUniqueIds[i];
constraintIdArray.push_back(constraintUid);
}
for (int i = 0; i < numBodies; i++)
{
int bodyUid = serverCmd.m_sdfLoadedArgs.m_bodyUniqueIds[i];
bodyIdArray.push_back(bodyUid);
}
}
for (int i = 0; i < numConstraints; i++)
{
int constraintUid = constraintIdArray[i];
m_data->m_tmpInfoRequestCommand.m_type = CMD_USER_CONSTRAINT;
m_data->m_tmpInfoRequestCommand.m_updateFlags = USER_CONSTRAINT_REQUEST_INFO;
@@ -953,10 +997,10 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd
}
}
int numBodies = serverCmd.m_sdfLoadedArgs.m_numBodies;
for (int i = 0; i < numBodies; i++)
{
int bodyUniqueId = serverCmd.m_sdfLoadedArgs.m_bodyUniqueIds[i];
int bodyUniqueId = bodyIdArray[i];
m_data->m_tmpInfoRequestCommand.m_type = CMD_REQUEST_BODY_INFO;
m_data->m_tmpInfoRequestCommand.m_sdfRequestInfoArgs.m_bodyUniqueId = bodyUniqueId;

145
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -58,7 +58,6 @@
#include "plugins/collisionFilterPlugin/collisionFilterPlugin.h"
#endif
#ifdef ENABLE_STATIC_GRPC_PLUGIN
#include "plugins/grpcPlugin/grpcPlugin.h"
#endif //ENABLE_STATIC_GRPC_PLUGIN
@@ -67,8 +66,6 @@
#include "plugins/pdControlPlugin/pdControlPlugin.h"
#endif //SKIP_STATIC_PD_CONTROL_PLUGIN
#ifdef STATIC_LINK_SPD_PLUGIN
#include "plugins/stablePDPlugin/BulletConversion.h"
#include "plugins/stablePDPlugin/RBDModel.h"
@@ -87,7 +84,6 @@
#include "plugins/tinyRendererPlugin/tinyRendererPlugin.h"
#endif
#ifdef B3_ENABLE_FILEIO_PLUGIN
#include "plugins/fileIOPlugin/fileIOPlugin.h"
#endif //B3_DISABLE_FILEIO_PLUGIN
@@ -2228,7 +2224,6 @@ struct ProgrammaticUrdfInterface : public URDFImporterInterface
virtual bool getLinkColor2(int linkIndex, struct UrdfMaterialColor& matCol) const
{
if (m_flags & URDF_USE_MATERIAL_COLORS_FROM_MTL)
{
const UrdfMaterialColor* matColPtr = m_linkColors[linkIndex];
@@ -2242,7 +2237,8 @@ struct ProgrammaticUrdfInterface : public URDFImporterInterface
return true;
}
} else
}
else
{
if (m_createBodyArgs.m_linkVisualShapeUniqueIds[linkIndex] >= 0)
{
@@ -2903,8 +2899,7 @@ void PhysicsServerCommandProcessor::deleteDynamicsWorld()
bool PhysicsServerCommandProcessor::supportsJointMotor(btMultiBody* mb, int mbLinkIndex)
{
bool canHaveMotor = (mb->getLink(mbLinkIndex).m_jointType == btMultibodyLink::eRevolute
|| mb->getLink(mbLinkIndex).m_jointType == btMultibodyLink::ePrismatic);
bool canHaveMotor = (mb->getLink(mbLinkIndex).m_jointType == btMultibodyLink::eRevolute || mb->getLink(mbLinkIndex).m_jointType == btMultibodyLink::ePrismatic);
return canHaveMotor;
}
@@ -2953,8 +2948,6 @@ bool PhysicsServerCommandProcessor::processImportedObjects(const char* fileName,
SaveWorldObjectData sd;
sd.m_fileName = fileName;
for (int m = 0; m < u2b.getNumModels(); m++)
{
u2b.activateModel(m);
@@ -3127,7 +3120,6 @@ bool PhysicsServerCommandProcessor::processImportedObjects(const char* fileName,
fclose(f);
}
#endif
}
else
{
@@ -3181,8 +3173,6 @@ bool PhysicsServerCommandProcessor::processImportedObjects(const char* fileName,
}
{
if (m_data->m_pluginManager.getRenderInterface())
{
int currentOpenGLTextureIndex = 0;
@@ -3432,7 +3422,8 @@ int PhysicsServerCommandProcessor::createBodyInfoStream(int bodyUniqueId, char*
ser.finalizeChunk(chunk, structType, BT_MULTIBODY_CODE, mb);
streamSizeInBytes = ser.getCurrentBufferSize();
}
else if(bodyHandle->m_rigidBody){
else if (bodyHandle->m_rigidBody)
{
btRigidBody* rb = bodyHandle->m_rigidBody;
btDefaultSerializer ser(bufferSizeInBytes, (unsigned char*)bufferServerToClient);
@@ -3456,7 +3447,8 @@ int PhysicsServerCommandProcessor::createBodyInfoStream(int bodyUniqueId, char*
streamSizeInBytes = ser.getCurrentBufferSize();
}
#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
else if(bodyHandle->m_softBody){
else if (bodyHandle->m_softBody)
{
//minimum serialization, registerNameForPointer
btSoftBody* sb = bodyHandle->m_softBody;
btDefaultSerializer ser(bufferSizeInBytes, (unsigned char*)bufferServerToClient);
@@ -4255,17 +4247,12 @@ bool PhysicsServerCommandProcessor::processSaveWorldCommand(const struct SharedM
return hasStatus;
}
#define MYLINELENGTH 16 * 32768
static unsigned char* MyGetRawHeightfieldData(CommonFileIOInterface& fileIO, PHY_ScalarType type, const char* fileName, int& width, int& height,
btScalar& minHeight,
btScalar& maxHeight)
{
std::string ext;
std::string fn(fileName);
std::string ext_ = fn.substr(fn.size() - 4);
@@ -4274,10 +4261,8 @@ static unsigned char* MyGetRawHeightfieldData(CommonFileIOInterface& fileIO, PHY
ext += char(tolower(*i));
}
if (ext != ".txt")
{
char relativeFileName[1024];
int found = fileIO.findResourcePath(fileName, relativeFileName, 1024);
@@ -4320,10 +4305,8 @@ static unsigned char* MyGetRawHeightfieldData(CommonFileIOInterface& fileIO, PHY
for (int j = 0; j < height; ++j)
{
for (int i = 0; i < width; ++i)
{
float z = double(image[(width - 1 - i) * 3 + width * j * 3]) * (1. / 255.);
btScalar* pf = (btScalar*)p;
*pf = z;
@@ -4397,7 +4380,6 @@ static unsigned char* MyGetRawHeightfieldData(CommonFileIOInterface& fileIO, PHY
pos = nextPos;
}
cols++;
}
width = rows;
height = cols;
@@ -4418,10 +4400,8 @@ static unsigned char* MyGetRawHeightfieldData(CommonFileIOInterface& fileIO, PHY
unsigned char* p = raw;
for (int i = 0; i < width; ++i)
{
for (int j = 0; j < height; ++j)
{
double z = allValues[i + width * j];
//convertFromFloat(p, z, type);
btScalar* pf = (btScalar*)p;
@@ -4444,7 +4424,6 @@ static unsigned char* MyGetRawHeightfieldData(CommonFileIOInterface& fileIO, PHY
maxHeight = z;
}
}
}
}
return raw;
@@ -4452,7 +4431,6 @@ static unsigned char* MyGetRawHeightfieldData(CommonFileIOInterface& fileIO, PHY
}
}
return 0;
}
@@ -4608,7 +4586,6 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str
if (clientCmd.m_createUserShapeArgs.m_shapes[i].m_numHeightfieldColumns > 0 &&
clientCmd.m_createUserShapeArgs.m_shapes[i].m_numHeightfieldRows > 0)
{
width = clientCmd.m_createUserShapeArgs.m_shapes[i].m_numHeightfieldRows;
height = clientCmd.m_createUserShapeArgs.m_shapes[i].m_numHeightfieldColumns;
float* heightfieldDataSrc = (float*)bufferServerToClient;
@@ -4629,7 +4606,6 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str
}
if (heightfieldData)
{
//replace heightfield data or create new heightfield
if (clientCmd.m_createUserShapeArgs.m_shapes[i].m_replaceHeightfieldIndex >= 0)
{
@@ -4654,7 +4630,6 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str
btAlignedObjectArray<int> indices;
int strideInBytes = 9 * sizeof(float);
MyTriangleCollector4 col;
col.m_pVerticesOut = &gfxVertices;
col.m_pIndicesOut = &indices;
@@ -4673,7 +4648,6 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str
terrainShape->clearAccelerator();
terrainShape->buildAccelerator();
btTriangleInfoMap* oldTriangleInfoMap = terrainShape->getTriangleInfoMap();
delete (oldTriangleInfoMap);
terrainShape->setTriangleInfoMap(0);
@@ -4693,7 +4667,6 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str
}
else
{
btScalar gridSpacing = 0.5;
btScalar gridHeightScale = 1. / 256.;
@@ -4782,7 +4755,6 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str
if (clientCmd.m_createUserShapeArgs.m_shapes[i].m_numVertices)
{
int numVertices = clientCmd.m_createUserShapeArgs.m_shapes[i].m_numVertices;
int numIndices = clientCmd.m_createUserShapeArgs.m_shapes[i].m_numIndices;
@@ -4792,7 +4764,6 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str
double* vertexUpload = (double*)data;
int* indexUpload = (int*)(data + numVertices * sizeof(double) * 3);
if (compound == 0)
{
compound = worldImporter->createCompoundShape();
@@ -4851,7 +4822,6 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str
for (int v = 0; v < clientCmd.m_createUserShapeArgs.m_shapes[i].m_numVertices; v++)
{
btVector3 pt(vertexUpload[v * 3 + 0],
vertexUpload[v * 3 + 1],
vertexUpload[v * 3 + 2]);
@@ -5102,7 +5072,6 @@ bool PhysicsServerCommandProcessor::processRequestMeshDataCommand(const struct S
serverStatusOut.m_sendMeshDataArgs.m_numVerticesRemaining = numVerticesRemaining - verticesCopied;
}
#endif //SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
}
serverStatusOut.m_numDataStreamBytes = 0;
@@ -5197,7 +5166,6 @@ bool PhysicsServerCommandProcessor::processCreateVisualShapeCommand(const struct
}
else
{
}
}
else
@@ -5910,7 +5878,6 @@ bool PhysicsServerCommandProcessor::processRequestRaycastIntersectionsCommand(co
}
}
BatchRayCaster batchRayCaster(m_data->m_threadPool, m_data->m_dynamicsWorld, &rays[0], (b3RayHitInfo*)bufferServerToClient, totalRays);
batchRayCaster.castRays(numThreads);
@@ -5996,6 +5963,8 @@ bool PhysicsServerCommandProcessor::processSyncBodyInfoCommand(const struct Shar
b3AlignedObjectArray<int> usedHandles;
m_data->m_bodyHandles.getUsedHandles(usedHandles);
int actualNumBodies = 0;
int* bodyUids = (int*)bufferServerToClient;
for (int i = 0; i < usedHandles.size(); i++)
{
int usedHandle = usedHandles[i];
@@ -6006,18 +5975,18 @@ bool PhysicsServerCommandProcessor::processSyncBodyInfoCommand(const struct Shar
if (body && (body->m_multiBody || body->m_rigidBody))
#endif
{
serverStatusOut.m_sdfLoadedArgs.m_bodyUniqueIds[actualNumBodies++] = usedHandle;
bodyUids[actualNumBodies++] = usedHandle;
}
}
serverStatusOut.m_sdfLoadedArgs.m_numBodies = actualNumBodies;
int usz = m_data->m_userConstraints.size();
int* constraintUid = bodyUids + actualNumBodies;
serverStatusOut.m_sdfLoadedArgs.m_numUserConstraints = usz;
for (int i = 0; i < usz; i++)
{
int key = m_data->m_userConstraints.getKeyAtIndex(i).getUid1();
// int uid = m_data->m_userConstraints.getAtIndex(i)->m_userConstraintData.m_userConstraintUniqueId;
serverStatusOut.m_sdfLoadedArgs.m_userConstraintUniqueIds[i] = key;
constraintUid[i] = key;
}
serverStatusOut.m_type = CMD_SYNC_BODY_INFO_COMPLETED;
@@ -6676,7 +6645,6 @@ bool PhysicsServerCommandProcessor::processSendDesiredStateCommand(const struct
}
}
cRBDModel* rbdModel = 0;
{
@@ -6685,7 +6653,6 @@ bool PhysicsServerCommandProcessor::processSendDesiredStateCommand(const struct
}
if (rbdModel)
{
int num_dof = jointPositionsQ.size();
const Eigen::VectorXd& pose = rbdModel->GetPose();
const Eigen::VectorXd& vel = rbdModel->GetVel();
@@ -6777,8 +6744,6 @@ bool PhysicsServerCommandProcessor::processSendDesiredStateCommand(const struct
torqueIndex++;
}
}
}
break;
@@ -7867,9 +7832,6 @@ bool PhysicsServerCommandProcessor::processCreateMultiBodyCommandSingle(const st
useMultiBody = false;
}
bool ok = 0;
{
BT_PROFILE("processImportedObjects");
@@ -7888,7 +7850,6 @@ bool PhysicsServerCommandProcessor::processCreateMultiBodyCommandSingle(const st
m_data->m_sdfRecentLoadedBodies.clear();
if (bodyUniqueId >= 0)
{
serverStatusOut.m_type = CMD_CREATE_MULTI_BODY_COMPLETED;
if (bufferSizeInBytes > 0 && serverStatusOut.m_numDataStreamBytes == 0)
{
@@ -7998,7 +7959,6 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar
btAssert((clientCmd.m_updateFlags & LOAD_SOFT_BODY_FILE_NAME) != 0);
btAssert(loadSoftBodyArgs.m_fileName);
btVector3 initialPos(0, 0, 0);
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_INITIAL_POSITION)
{
@@ -8301,21 +8261,17 @@ bool PhysicsServerCommandProcessor::processProfileTimingCommand(const struct Sha
{
bool hasStatus = true;
{
if (clientCmd.m_profile.m_type == 0)
{
char** eventNamePtr = m_data->m_profileEvents[clientCmd.m_profile.m_name];
char* eventName = 0;
if (eventNamePtr)
{
eventName = *eventNamePtr;
}
else
{
int len = strlen(clientCmd.m_profile.m_name);
eventName = new char[len + 1];
strcpy(eventName, clientCmd.m_profile.m_name);
@@ -8329,7 +8285,6 @@ bool PhysicsServerCommandProcessor::processProfileTimingCommand(const struct Sha
{
b3LeaveProfileZone();
}
}
serverStatusOut.m_type = CMD_CLIENT_COMMAND_COMPLETED;
@@ -8428,7 +8383,8 @@ bool PhysicsServerCommandProcessor::processRequestCollisionInfoCommand(const str
}
}
#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
else if (body && body->m_softBody){
else if (body && body->m_softBody)
{
SharedMemoryStatus& serverCmd = serverStatusOut;
serverStatusOut.m_type = CMD_REQUEST_COLLISION_INFO_COMPLETED;
serverCmd.m_sendCollisionInfoArgs.m_numLinks = 0;
@@ -8591,7 +8547,6 @@ bool PhysicsServerCommandProcessor::processChangeDynamicsInfoCommand(const struc
{
mb->setCanWakeup(false);
}
}
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LINEAR_DAMPING)
@@ -8665,7 +8620,6 @@ bool PhysicsServerCommandProcessor::processChangeDynamicsInfoCommand(const struc
mb->getBaseCollider()->setCollisionFlags(oldFlags & ~btCollisionObject::CF_STATIC_OBJECT);
mb->setFixedBase(false);
m_data->m_dynamicsWorld->addCollisionObject(mb->getBaseCollider(), collisionFilterGroup, collisionFilterMask);
}
}
else
@@ -8682,7 +8636,6 @@ bool PhysicsServerCommandProcessor::processChangeDynamicsInfoCommand(const struc
m_data->m_dynamicsWorld->addCollisionObject(mb->getBaseCollider(), collisionFilterGroup, collisionFilterMask);
}
}
}
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LOCAL_INERTIA_DIAGONAL)
{
@@ -8697,7 +8650,6 @@ bool PhysicsServerCommandProcessor::processChangeDynamicsInfoCommand(const struc
{
mb->setMaxCoordinateVelocity(clientCmd.m_changeDynamicsInfoArgs.m_maxJointVelocity);
}
}
else
{
@@ -8769,7 +8721,6 @@ bool PhysicsServerCommandProcessor::processChangeDynamicsInfoCommand(const struc
}
else
{
btRigidBody* rb = 0;
if (body && body->m_rigidBody)
{
@@ -8785,7 +8736,6 @@ bool PhysicsServerCommandProcessor::processChangeDynamicsInfoCommand(const struc
btRigidBody* childRb = &body->m_rigidBodyJoints[linkIndex]->getRigidBodyB();
rb = childRb;
}
}
}
@@ -8882,8 +8832,6 @@ bool PhysicsServerCommandProcessor::processChangeDynamicsInfoCommand(const struc
rb->setContactProcessingThreshold(clientCmd.m_changeDynamicsInfoArgs.m_contactProcessingThreshold);
}
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_CCD_SWEPT_SPHERE_RADIUS)
{
rb->setCcdSweptSphereRadius(clientCmd.m_changeDynamicsInfoArgs.m_ccdSweptSphereRadius);
@@ -9054,7 +9002,8 @@ bool PhysicsServerCommandProcessor::processGetDynamicsInfoCommand(const struct S
serverCmd.m_dynamicsInfo.m_mass = rb->getMass();
}
#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
else if (body && body->m_softBody){
else if (body && body->m_softBody)
{
SharedMemoryStatus& serverCmd = serverStatusOut;
serverCmd.m_type = CMD_GET_DYNAMICS_INFO_COMPLETED;
serverCmd.m_dynamicsInfo.m_bodyType = BT_SOFT_BODY;
@@ -9119,7 +9068,6 @@ bool PhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const st
BT_PROFILE("CMD_SEND_PHYSICS_SIMULATION_PARAMETERS");
if (clientCmd.m_updateFlags & SIM_PARAM_ENABLE_CONE_FRICTION)
{
if (clientCmd.m_physSimParamArgs.m_enableConeFriction)
@@ -9849,11 +9797,9 @@ bool PhysicsServerCommandProcessor::processConfigureOpenGLVisualizerCommand(cons
m_data->m_remoteSyncTransformInterval = clientCmd.m_configureOpenGLVisualizerArguments.m_remoteSyncTransformInterval;
}
return hasStatus;
}
bool PhysicsServerCommandProcessor::processInverseDynamicsCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
bool hasStatus = true;
@@ -9887,11 +9833,11 @@ bool PhysicsServerCommandProcessor::processInverseDynamicsCommand(const struct S
serverCmd.m_inverseDynamicsResultArgs.m_dofCount = dof;
serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED;
}
}
#endif
} else
}
else
{
btInverseDynamics::MultiBodyTree* tree = m_data->findOrCreateTree(bodyHandle->m_multiBody);
@@ -9902,7 +9848,6 @@ bool PhysicsServerCommandProcessor::processInverseDynamicsCommand(const struct S
if (tree && clientCmd.m_calculateInverseDynamicsArguments.m_dofCountQ == (baseDofQ + num_dofs) &&
clientCmd.m_calculateInverseDynamicsArguments.m_dofCountQdot == (baseDofQdot + num_dofs))
{
btInverseDynamics::vecx nu(num_dofs + baseDofQdot), qdot(num_dofs + baseDofQdot), q(num_dofs + baseDofQdot), joint_force(num_dofs + baseDofQdot);
//for floating base, inverse dynamics expects euler angle x,y,z and position x,y,z in that order
@@ -10129,7 +10074,6 @@ bool PhysicsServerCommandProcessor::processCalculateMassMatrixCommand(const stru
}
else
{
btInverseDynamics::MultiBodyTree* tree = m_data->findOrCreateTree(bodyHandle->m_multiBody);
if (tree)
@@ -10934,7 +10878,6 @@ bool PhysicsServerCommandProcessor::processCreateUserConstraintCommand(const str
return hasStatus;
}
bool PhysicsServerCommandProcessor::processCalculateInverseKinematicsCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
bool hasStatus = true;
@@ -11262,11 +11205,9 @@ bool PhysicsServerCommandProcessor::processCalculateInverseKinematicsCommand2(co
ikHelperPtr = tmpHelper;
}
btAlignedObjectArray<double> startingPositions;
startingPositions.reserve(bodyHandle->m_multiBody->getNumLinks());
{
int DofIndex = 0;
for (int i = 0; i < bodyHandle->m_multiBody->getNumLinks(); ++i)
@@ -11325,7 +11266,6 @@ bool PhysicsServerCommandProcessor::processCalculateInverseKinematicsCommand2(co
for (int ne = 0; ne < clientCmd.m_calculateInverseKinematicsArguments.m_numEndEffectorLinkIndices; ne++)
{
int endEffectorLinkIndex = clientCmd.m_calculateInverseKinematicsArguments.m_endEffectorLinkIndices[ne];
validEndEffectorLinkIndices = validEndEffectorLinkIndices && (endEffectorLinkIndex < bodyHandle->m_multiBody->getNumLinks());
@@ -11419,7 +11359,6 @@ bool PhysicsServerCommandProcessor::processCalculateInverseKinematicsCommand2(co
for (int ne = 0; ne < clientCmd.m_calculateInverseKinematicsArguments.m_numEndEffectorLinkIndices; ne++)
{
int endEffectorLinkIndex2 = clientCmd.m_calculateInverseKinematicsArguments.m_endEffectorLinkIndices[ne];
// Note that inverse dynamics uses zero-based indexing of bodies, not starting from -1 for the base link.
@@ -11449,7 +11388,6 @@ bool PhysicsServerCommandProcessor::processCalculateInverseKinematicsCommand2(co
endEffectorTargetWorldPositions[ne * 3 + 2]));
//diff
currentDiff = btMax(currentDiff, (world_origin - targetPos).length());
}
}
}
@@ -11538,9 +11476,6 @@ bool PhysicsServerCommandProcessor::processCalculateInverseKinematicsCommand2(co
endEffectorBaseCoord.getOrigin().serializeDouble(endEffectorWorldPosition);
endEffectorBaseCoord.getRotation().serializeDouble(endEffectorWorldOrientation);
// Set joint damping coefficents. A small default
// damping constant is added to prevent singularity
// with pseudo inverse. The user can set joint damping
@@ -11563,7 +11498,6 @@ bool PhysicsServerCommandProcessor::processCalculateInverseKinematicsCommand2(co
if (clientCmd.m_calculateInverseKinematicsArguments.m_numEndEffectorLinkIndices == 1)
{
BT_PROFILE("computeIK");
ikHelperPtr->computeIK(&endEffectorTargetWorldPositions[0],
&endEffectorTargetWorldOrientations[0],
@@ -11575,7 +11509,6 @@ bool PhysicsServerCommandProcessor::processCalculateInverseKinematicsCommand2(co
}
else
{
if (clientCmd.m_calculateInverseKinematicsArguments.m_numEndEffectorLinkIndices > 1)
{
ikHelperPtr->computeIK2(&endEffectorTargetWorldPositions[0],
@@ -11812,7 +11745,8 @@ bool PhysicsServerCommandProcessor::processRequestVisualShapeInfoCommand(const s
//int visualShapeStorage = bufferSizeInBytes / totalBytesPerVisualShape - 1;
//set serverCmd.m_sendVisualShapeArgs when totalNumVisualShapes is zero
if (totalNumVisualShapes==0) {
if (totalNumVisualShapes == 0)
{
serverCmd.m_sendVisualShapeArgs.m_numRemainingVisualShapes = 0;
serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied = 0;
serverCmd.m_sendVisualShapeArgs.m_startingVisualShapeIndex = clientCmd.m_requestVisualShapeDataArguments.m_startingVisualShapeIndex;
@@ -11821,7 +11755,8 @@ bool PhysicsServerCommandProcessor::processRequestVisualShapeInfoCommand(const s
serverCmd.m_type = CMD_VISUAL_SHAPE_INFO_COMPLETED;
}
else{
else
{
b3VisualShapeData* visualShapeStoragePtr = (b3VisualShapeData*)bufferServerToClient;
int remain = totalNumVisualShapes - clientCmd.m_requestVisualShapeDataArguments.m_startingVisualShapeIndex;
@@ -11857,7 +11792,8 @@ bool PhysicsServerCommandProcessor::processRequestVisualShapeInfoCommand(const s
serverCmd.m_numDataStreamBytes = sizeof(b3VisualShapeData) * serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied;
serverCmd.m_type = CMD_VISUAL_SHAPE_INFO_COMPLETED;
}
else{
else
{
b3Warning("failed to get shape info");
}
}
@@ -11892,7 +11828,8 @@ bool PhysicsServerCommandProcessor::processUpdateVisualShapeCommand(const struct
clientCmd.m_updateVisualShapeDataArguments.m_shapeIndex,
texHandle->m_tinyRendererTextureId);
}
} else
}
else
{
m_data->m_pluginManager.getRenderInterface()->changeShapeTexture(clientCmd.m_updateVisualShapeDataArguments.m_bodyUniqueId,
clientCmd.m_updateVisualShapeDataArguments.m_jointIndex,
@@ -11922,7 +11859,8 @@ bool PhysicsServerCommandProcessor::processUpdateVisualShapeCommand(const struct
if (texHandle)
{
m_data->m_guiHelper->replaceTexture(shapeIndex, texHandle->m_openglTextureId);
} else
}
else
{
m_data->m_guiHelper->replaceTexture(shapeIndex, -1);
}
@@ -12103,7 +12041,8 @@ bool PhysicsServerCommandProcessor::processLoadTextureCommand(const struct Share
{
imageData = stbi_load_from_memory((const unsigned char*)&buffer[0], buffer.size(), &width, &height, &n, 3);
}
} else
}
else
{
imageData = stbi_load(relativeFileName, &width, &height, &n, 3);
}
@@ -12172,7 +12111,6 @@ bool PhysicsServerCommandProcessor::processSaveStateCommand(const struct SharedM
return hasStatus;
}
bool PhysicsServerCommandProcessor::processRemoveStateCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
BT_PROFILE("CMD_REMOVE_STATE");
@@ -12195,7 +12133,6 @@ bool PhysicsServerCommandProcessor::processRemoveStateCommand(const struct Share
return hasStatus;
}
bool PhysicsServerCommandProcessor::processRestoreStateCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
BT_PROFILE("CMD_RESTORE_STATE");
@@ -12230,7 +12167,6 @@ bool PhysicsServerCommandProcessor::processRestoreStateCommand(const struct Shar
buffer.reserve(1024);
if (fileIO)
{
int fileId = -1;
found = fileIO->findResourcePath(clientCmd.m_fileArguments.m_fileName, fileName, 1024);
if (found)
@@ -12248,7 +12184,8 @@ bool PhysicsServerCommandProcessor::processRestoreStateCommand(const struct Shar
{
b3Warning("image filesize mismatch!\n");
buffer.resize(0);
} else
}
else
{
found = true;
}
@@ -12260,7 +12197,8 @@ bool PhysicsServerCommandProcessor::processRestoreStateCommand(const struct Shar
if (found && buffer.size())
{
ok = importer->loadFileFromMemory(&buffer[0], buffer.size());
} else
}
else
{
b3Error("Error in restoreState: cannot load file %s\n", clientCmd.m_fileArguments.m_fileName);
}
@@ -12310,7 +12248,8 @@ bool PhysicsServerCommandProcessor::processLoadBulletCommand(const struct Shared
{
b3Warning("image filesize mismatch!\n");
buffer.resize(0);
} else
}
else
{
found = true;
}
@@ -12695,7 +12634,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
if (clientCmd.m_calculateInverseKinematicsArguments.m_numEndEffectorLinkIndices == 1)
{
hasStatus = processCalculateInverseKinematicsCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
} else
}
else
{
hasStatus = processCalculateInverseKinematicsCommand2(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
}
@@ -12811,13 +12751,11 @@ void PhysicsServerCommandProcessor::syncPhysicsToGraphics()
m_data->m_guiHelper->syncPhysicsToGraphics(m_data->m_dynamicsWorld);
}
void PhysicsServerCommandProcessor::syncPhysicsToGraphics2()
{
m_data->m_guiHelper->syncPhysicsToGraphics2(m_data->m_dynamicsWorld);
}
void PhysicsServerCommandProcessor::renderScene(int renderFlags)
{
if (m_data->m_guiHelper)
@@ -12830,6 +12768,10 @@ void PhysicsServerCommandProcessor::renderScene(int renderFlags)
m_data->m_guiHelper->render(m_data->m_dynamicsWorld);
#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
#ifndef SKIP_DEFORMABLE_BODY
if (m_data->m_dynamicsWorld)
{
if (m_data->m_dynamicsWorld->getSoftBodyArray().size())
{
for (int i = 0; i < m_data->m_dynamicsWorld->getSoftBodyArray().size(); i++)
{
btSoftBody* psb = (btSoftBody*)m_data->m_dynamicsWorld->getSoftBodyArray()[i];
@@ -12840,6 +12782,8 @@ void PhysicsServerCommandProcessor::renderScene(int renderFlags)
}
m_data->m_guiHelper->drawDebugDrawerLines();
m_data->m_guiHelper->clearLines();
}
}
#endif
#endif
}
@@ -13311,7 +13255,6 @@ void PhysicsServerCommandProcessor::resetSimulation()
m_data->m_pluginManager.addNotification(notification);
syncPhysicsToGraphics2();
}
void PhysicsServerCommandProcessor::setTimeOut(double /*timeOutInSeconds*/)

5
examples/SharedMemory/PhysicsServerExample.cpp
View File

@@ -654,15 +654,20 @@ public:
{
if (m_debugDraw)
{
m_csGUI->lock();
//draw stuff and flush?
m_debugDraw->drawDebugDrawerLines();
m_csGUI->unlock();
}
}
virtual void clearLines()
{
m_csGUI->lock();
if (m_debugDraw)
{
m_debugDraw->clearLines();
}
m_csGUI->unlock();
}
GUIHelperInterface* m_childGuiHelper;

13
examples/pybullet/pybullet.c
View File

@@ -9024,10 +9024,23 @@ static PyObject* pybullet_createMultiBody(PyObject* self, PyObject* args, PyObje
if (statusType == CMD_CREATE_MULTI_BODY_COMPLETED)
{
int uid = b3GetStatusBodyIndex(statusHandle);
if (numBatchPositions > 0)
{
PyObject* pyResultList = PyTuple_New(numBatchPositions );
for (i = 0; i < numBatchPositions; i++)
{
PyTuple_SetItem(pyResultList, i, PyLong_FromLong(uid - numBatchPositions + i + 1));
}
return pyResultList;
}
else
{
PyObject* ob = PyLong_FromLong(uid);
return ob;
}
}
}
else
{
if (seqLinkMasses)