#include "PhysicsClientSharedMemory.h" #include "PosixSharedMemory.h" #include "Win32SharedMemory.h" #include "Bullet3Common/b3AlignedObjectArray.h" #include "Bullet3Common/b3Vector3.h" #include #include "Bullet3Common/b3HashMap.h" #include "Bullet3Common/b3Logging.h" #include "../Utils/b3ResourcePath.h" #include "../../Extras/Serialize/BulletFileLoader/btBulletFile.h" #include "../../Extras/Serialize/BulletFileLoader/autogenerated/bullet.h" #include "SharedMemoryBlock.h" #include "BodyJointInfoUtility.h" #include "SharedMemoryUserData.h" #include "LinearMath/btQuickprof.h" struct BodyJointInfoCache { std::string m_baseName; b3AlignedObjectArray m_jointInfo; std::string m_bodyName; btAlignedObjectArray m_userDataIds; int m_numDofs; ~BodyJointInfoCache() { } }; struct PhysicsClientSharedMemoryInternalData { SharedMemoryInterface* m_sharedMemory; bool m_ownsSharedMemory; SharedMemoryBlock* m_testBlock1; btAlignedObjectArray m_profileTimings; btHashMap m_profileTimingStringArray; btHashMap m_bodyJointMap; btHashMap m_userConstraintInfoMap; btAlignedObjectArray m_debugLinesFrom; btAlignedObjectArray m_debugLinesTo; btAlignedObjectArray m_debugLinesColor; int m_cachedCameraPixelsWidth; int m_cachedCameraPixelsHeight; btAlignedObjectArray m_cachedCameraPixelsRGBA; btAlignedObjectArray m_cachedCameraDepthBuffer; btAlignedObjectArray m_cachedSegmentationMaskBuffer; btAlignedObjectArray m_cachedContactPoints; btAlignedObjectArray m_cachedOverlappingObjects; btAlignedObjectArray m_cachedVisualShapes; btAlignedObjectArray m_cachedCollisionShapes; b3MeshData m_cachedMeshData; btAlignedObjectArray m_cachedVertexPositions; btAlignedObjectArray m_cachedVREvents; btAlignedObjectArray m_cachedKeyboardEvents; btAlignedObjectArray m_cachedMouseEvents; btAlignedObjectArray m_cachedMassMatrix; btAlignedObjectArray m_raycastHits; btAlignedObjectArray m_bodyIdsRequestInfo; btAlignedObjectArray m_constraintIdsRequestInfo; btAlignedObjectArray m_userDataIdsRequestInfo; btHashMap m_userDataMap; btHashMap m_userDataHandleLookup; btAlignedObjectArray m_cachedReturnData; b3UserDataValue m_cachedReturnDataValue; SharedMemoryStatus m_tempBackupServerStatus; SharedMemoryStatus m_lastServerStatus; SendActualStateSharedMemoryStorage m_cachedState; int m_counter; bool m_isConnected; bool m_waitingForServer; bool m_hasLastServerStatus; int m_sharedMemoryKey; bool m_verboseOutput; double m_timeOutInSeconds; PhysicsClientSharedMemoryInternalData() : m_sharedMemory(0), m_ownsSharedMemory(false), m_testBlock1(0), m_cachedCameraPixelsWidth(0), m_cachedCameraPixelsHeight(0), m_counter(0), m_isConnected(false), m_waitingForServer(false), m_hasLastServerStatus(false), m_sharedMemoryKey(SHARED_MEMORY_KEY), m_verboseOutput(false), m_timeOutInSeconds(1e30) { m_cachedMeshData.m_numVertices = 0; m_cachedMeshData.m_vertices = 0; } void processServerStatus(); bool canSubmitCommand() const; }; int PhysicsClientSharedMemory::getNumBodies() const { return m_data->m_bodyJointMap.size(); } int PhysicsClientSharedMemory::getBodyUniqueId(int serialIndex) const { if ((serialIndex >= 0) && (serialIndex < getNumBodies())) { return m_data->m_bodyJointMap.getKeyAtIndex(serialIndex).getUid1(); } return -1; } bool PhysicsClientSharedMemory::getBodyInfo(int bodyUniqueId, struct b3BodyInfo& info) const { BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId]; if (bodyJointsPtr && *bodyJointsPtr) { BodyJointInfoCache* bodyJoints = *bodyJointsPtr; strcpy(info.m_baseName, bodyJoints->m_baseName.c_str()); strcpy(info.m_bodyName, bodyJoints->m_bodyName.c_str()); return true; } return false; } int PhysicsClientSharedMemory::getNumJoints(int bodyUniqueId) const { BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId]; if (bodyJointsPtr && *bodyJointsPtr) { BodyJointInfoCache* bodyJoints = *bodyJointsPtr; return bodyJoints->m_jointInfo.size(); } return 0; } int PhysicsClientSharedMemory::getNumDofs(int bodyUniqueId) const { BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId]; if (bodyJointsPtr && *bodyJointsPtr) { BodyJointInfoCache* bodyJoints = *bodyJointsPtr; return bodyJoints->m_numDofs; } return 0; } bool PhysicsClientSharedMemory::getJointInfo(int bodyUniqueId, int jointIndex, b3JointInfo& info) const { BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId]; if (bodyJointsPtr && *bodyJointsPtr) { BodyJointInfoCache* bodyJoints = *bodyJointsPtr; if ((jointIndex >= 0) && (jointIndex < bodyJoints->m_jointInfo.size())) { info = bodyJoints->m_jointInfo[jointIndex]; info.m_qSize = 0; info.m_uSize = 0; switch (info.m_jointType) { case eSphericalType: { info.m_qSize = 4;//quaterion x,y,z,w info.m_uSize = 3; break; } case ePlanarType: { info.m_qSize = 2; info.m_uSize = 2; break; } case ePrismaticType: case eRevoluteType: { info.m_qSize = 1; info.m_uSize = 1; break; } default: { } } return true; } } return false; } int PhysicsClientSharedMemory::getNumUserConstraints() const { return m_data->m_userConstraintInfoMap.size(); } int PhysicsClientSharedMemory::getUserConstraintInfo(int constraintUniqueId, struct b3UserConstraint& info) const { b3UserConstraint* constraintPtr = m_data->m_userConstraintInfoMap[constraintUniqueId]; if (constraintPtr) { info = *constraintPtr; return 1; } return 0; } int PhysicsClientSharedMemory::getUserConstraintId(int serialIndex) const { if ((serialIndex >= 0) && (serialIndex < getNumUserConstraints())) { return m_data->m_userConstraintInfoMap.getKeyAtIndex(serialIndex).getUid1(); } return -1; } PhysicsClientSharedMemory::PhysicsClientSharedMemory() { m_data = new PhysicsClientSharedMemoryInternalData; #ifdef _WIN32 m_data->m_sharedMemory = new Win32SharedMemoryClient(); #else m_data->m_sharedMemory = new PosixSharedMemory(); #endif m_data->m_ownsSharedMemory = true; } PhysicsClientSharedMemory::~PhysicsClientSharedMemory() { if (m_data->m_isConnected) { disconnectSharedMemory(); } resetData(); for (int i = 0; i < m_data->m_profileTimingStringArray.size(); i++) { std::string** str = m_data->m_profileTimingStringArray.getAtIndex(i); if (str) { delete *str; } } m_data->m_profileTimingStringArray.clear(); if (m_data->m_ownsSharedMemory) { delete m_data->m_sharedMemory; } delete m_data; } void PhysicsClientSharedMemory::removeCachedBody(int bodyUniqueId) { BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId]; if (bodyJointsPtr && *bodyJointsPtr) { for (int i = 0; i < (*bodyJointsPtr)->m_userDataIds.size(); i++) { const int userDataId = (*bodyJointsPtr)->m_userDataIds[i]; SharedMemoryUserData* userData = m_data->m_userDataMap[userDataId]; m_data->m_userDataHandleLookup.remove(SharedMemoryUserDataHashKey(userData)); m_data->m_userDataMap.remove(userDataId); } delete (*bodyJointsPtr); m_data->m_bodyJointMap.remove(bodyUniqueId); } } void PhysicsClientSharedMemory::clearCachedBodies() { for (int i = 0; i < m_data->m_bodyJointMap.size(); i++) { BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap.getAtIndex(i); if (bodyJointsPtr && *bodyJointsPtr) { delete (*bodyJointsPtr); } } m_data->m_bodyJointMap.clear(); } void PhysicsClientSharedMemory::resetData() { m_data->m_debugLinesFrom.clear(); m_data->m_debugLinesTo.clear(); m_data->m_debugLinesColor.clear(); m_data->m_userConstraintInfoMap.clear(); m_data->m_userDataMap.clear(); m_data->m_userDataHandleLookup.clear(); clearCachedBodies(); } void PhysicsClientSharedMemory::setSharedMemoryKey(int key) { m_data->m_sharedMemoryKey = key; } void PhysicsClientSharedMemory::setSharedMemoryInterface(class SharedMemoryInterface* sharedMem) { if (sharedMem) { if (m_data->m_sharedMemory && m_data->m_ownsSharedMemory) { delete m_data->m_sharedMemory; } m_data->m_ownsSharedMemory = false; m_data->m_sharedMemory = sharedMem; }; } void PhysicsClientSharedMemory::disconnectSharedMemory() { if (m_data->m_isConnected && m_data->m_sharedMemory) { m_data->m_sharedMemory->releaseSharedMemory(m_data->m_sharedMemoryKey, SHARED_MEMORY_SIZE); } m_data->m_isConnected = false; } bool PhysicsClientSharedMemory::isConnected() const { return m_data->m_isConnected && (m_data->m_testBlock1->m_magicId == SHARED_MEMORY_MAGIC_NUMBER); } bool PhysicsClientSharedMemory::connect() { /// server always has to create and initialize shared memory bool allowCreation = false; m_data->m_testBlock1 = (SharedMemoryBlock*)m_data->m_sharedMemory->allocateSharedMemory( m_data->m_sharedMemoryKey, SHARED_MEMORY_SIZE, allowCreation); if (m_data->m_testBlock1) { if (m_data->m_testBlock1->m_magicId != SHARED_MEMORY_MAGIC_NUMBER) { //there is no chance people are still using this software 100 years from now ;-) if ((m_data->m_testBlock1->m_magicId < 211705023) && (m_data->m_testBlock1->m_magicId >= 201705023)) { b3Error("Error: physics server version mismatch (expected %d got %d)\n", SHARED_MEMORY_MAGIC_NUMBER, m_data->m_testBlock1->m_magicId); } else { b3Error("Error connecting to shared memory: please start server before client\n"); } m_data->m_sharedMemory->releaseSharedMemory(m_data->m_sharedMemoryKey, SHARED_MEMORY_SIZE); m_data->m_testBlock1 = 0; return false; } else { if (m_data->m_verboseOutput) { b3Printf("Connected to existing shared memory, status OK.\n"); } m_data->m_isConnected = true; } } else { //b3Warning("Cannot connect to shared memory"); return false; } #if 0 if (m_data->m_isConnected) { //get all existing bodies and body info... SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0]; //now transfer the information of the individual objects etc. command.m_type = CMD_REQUEST_BODY_INFO; command.m_sdfRequestInfoArgs.m_bodyUniqueId = 37; submitClientCommand(command); double startTime = clock.getTimeInSeconds(); double timeOutInSeconds = 10; const SharedMemoryStatus* status = 0; while ((status == 0) && (clock.getTimeInSeconds()-startTime < timeOutInSeconds)) { status = processServerStatus(); } //submitClientCommand(command); } #endif return true; } ///todo(erwincoumans) refactor this: merge with PhysicsDirect::processBodyJointInfo void PhysicsClientSharedMemory::processBodyJointInfo(int bodyUniqueId, const SharedMemoryStatus& serverCmd) { bParse::btBulletFile bf( &this->m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor[0], serverCmd.m_numDataStreamBytes); bf.setFileDNAisMemoryDNA(); bf.parse(false); BodyJointInfoCache* bodyJoints = new BodyJointInfoCache; m_data->m_bodyJointMap.insert(bodyUniqueId, bodyJoints); bodyJoints->m_bodyName = serverCmd.m_dataStreamArguments.m_bodyName; for (int i = 0; i < bf.m_multiBodies.size(); i++) { int flag = bf.getFlags(); if ((flag & bParse::FD_DOUBLE_PRECISION) != 0) { Bullet::btMultiBodyDoubleData* mb = (Bullet::btMultiBodyDoubleData*)bf.m_multiBodies[i]; bodyJoints->m_baseName = mb->m_baseName; addJointInfoFromMultiBodyData(mb, bodyJoints, m_data->m_verboseOutput); } else { Bullet::btMultiBodyFloatData* mb = (Bullet::btMultiBodyFloatData*)bf.m_multiBodies[i]; bodyJoints->m_baseName = mb->m_baseName; addJointInfoFromMultiBodyData(mb, bodyJoints, m_data->m_verboseOutput); } } if (bf.ok()) { if (m_data->m_verboseOutput) { b3Printf("Received robot description ok!\n"); } } else { b3Warning("Robot description not received"); } } template void addJointInfoFromConstraint(int linkIndex, const T* con, U* bodyJoints, bool verboseOutput) { b3JointInfo info; info.m_jointName[0] = 0; info.m_linkName[0] = 0; info.m_flags = 0; info.m_jointIndex = linkIndex; info.m_qIndex = linkIndex + 7; info.m_uIndex = linkIndex + 6; //derive type from limits if (con->m_typeConstraintData.m_name) { strcpy(info.m_jointName, con->m_typeConstraintData.m_name); //info.m_linkName = strDup(con->m_typeConstraintData.m_name); } btVector3 linearLowerLimit(con->m_linearLowerLimit.m_floats[0], con->m_linearLowerLimit.m_floats[1], con->m_linearLowerLimit.m_floats[2]); btVector3 linearUpperLimit(con->m_linearUpperLimit.m_floats[0], con->m_linearUpperLimit.m_floats[1], con->m_linearUpperLimit.m_floats[2]); btVector3 angularLowerLimit(con->m_angularLowerLimit.m_floats[0], con->m_angularLowerLimit.m_floats[1], con->m_angularLowerLimit.m_floats[2]); btVector3 angularUpperLimit(con->m_angularUpperLimit.m_floats[0], con->m_angularUpperLimit.m_floats[1], con->m_angularUpperLimit.m_floats[2]); //very simple, rudimentary extraction of constaint type, from limits info.m_jointType = eFixedType; info.m_jointDamping = 0; //mb->m_links[link].m_jointDamping; info.m_jointFriction = 0; //mb->m_links[link].m_jointFriction; info.m_jointLowerLimit = 0; //mb->m_links[link].m_jointLowerLimit; info.m_jointUpperLimit = 0; //mb->m_links[link].m_jointUpperLimit; info.m_jointMaxForce = 0; //mb->m_links[link].m_jointMaxForce; info.m_jointMaxVelocity = 0; //mb->m_links[link].m_jointMaxVelocity; if (linearLowerLimit.isZero() && linearUpperLimit.isZero() && angularLowerLimit.isZero() && angularUpperLimit.isZero()) { info.m_jointType = eFixedType; } else { if (linearLowerLimit.isZero() && linearUpperLimit.isZero()) { info.m_jointType = eRevoluteType; btVector3 limitRange = angularLowerLimit.absolute() + angularUpperLimit.absolute(); int limitAxis = limitRange.maxAxis(); info.m_jointLowerLimit = angularLowerLimit[limitAxis]; info.m_jointUpperLimit = angularUpperLimit[limitAxis]; } else { info.m_jointType = ePrismaticType; btVector3 limitRange = linearLowerLimit.absolute() + linearUpperLimit.absolute(); int limitAxis = limitRange.maxAxis(); info.m_jointLowerLimit = linearLowerLimit[limitAxis]; info.m_jointUpperLimit = linearUpperLimit[limitAxis]; } } //if (mb->m_links[link].m_linkName) { if ((info.m_jointType == eRevoluteType) || (info.m_jointType == ePrismaticType)) { info.m_flags |= JOINT_HAS_MOTORIZED_POWER; } bodyJoints->m_jointInfo.push_back(info); }; const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { // SharedMemoryStatus* stat = 0; if (!m_data->m_testBlock1) { m_data->m_lastServerStatus.m_type = CMD_SHARED_MEMORY_NOT_INITIALIZED; return &m_data->m_lastServerStatus; } if (!m_data->m_waitingForServer) { return 0; } if (m_data->m_testBlock1->m_magicId != SHARED_MEMORY_MAGIC_NUMBER) { m_data->m_lastServerStatus.m_type = CMD_SHARED_MEMORY_NOT_INITIALIZED; return &m_data->m_lastServerStatus; } if (m_data->m_testBlock1->m_numServerCommands > m_data->m_testBlock1->m_numProcessedServerCommands) { B3_PROFILE("processServerCMD"); btAssert(m_data->m_testBlock1->m_numServerCommands == m_data->m_testBlock1->m_numProcessedServerCommands + 1); const SharedMemoryStatus& serverCmd = m_data->m_testBlock1->m_serverCommands[0]; if (serverCmd.m_type==CMD_ACTUAL_STATE_UPDATE_COMPLETED) { SendActualStateSharedMemoryStorage* serverState = (SendActualStateSharedMemoryStorage*)m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor; m_data->m_cachedState = *serverState; //ideally we provided a 'getCachedState' but that would require changing the API, so we store a pointer instead. m_data->m_testBlock1->m_serverCommands[0].m_sendActualStateArgs.m_stateDetails = &m_data->m_cachedState; } m_data->m_lastServerStatus = serverCmd; // EnumSharedMemoryServerStatus s = (EnumSharedMemoryServerStatus)serverCmd.m_type; // consume the command switch (serverCmd.m_type) { case CMD_CLIENT_COMMAND_COMPLETED: { B3_PROFILE("CMD_CLIENT_COMMAND_COMPLETED"); if (m_data->m_verboseOutput) { b3Printf("Server completed command"); } break; } case CMD_MJCF_LOADING_COMPLETED: { B3_PROFILE("CMD_MJCF_LOADING_COMPLETED"); if (m_data->m_verboseOutput) { b3Printf("Server loading the MJCF OK\n"); } break; } case CMD_SDF_LOADING_COMPLETED: { B3_PROFILE("CMD_SDF_LOADING_COMPLETED"); if (m_data->m_verboseOutput) { b3Printf("Server loading the SDF OK\n"); } break; } case CMD_CREATE_MULTI_BODY_COMPLETED: case CMD_URDF_LOADING_COMPLETED: { B3_PROFILE("CMD_URDF_LOADING_COMPLETED"); if (m_data->m_verboseOutput) { b3Printf("Server loading the URDF OK\n"); } if (serverCmd.m_numDataStreamBytes > 0) { bParse::btBulletFile bf( this->m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor, serverCmd.m_numDataStreamBytes); bf.setFileDNAisMemoryDNA(); bf.parse(false); int bodyUniqueId = serverCmd.m_dataStreamArguments.m_bodyUniqueId; BodyJointInfoCache* bodyJoints = new BodyJointInfoCache; m_data->m_bodyJointMap.insert(bodyUniqueId, bodyJoints); bodyJoints->m_bodyName = serverCmd.m_dataStreamArguments.m_bodyName; for (int i = 0; i < bf.m_constraints.size(); i++) { int flag = bf.getFlags(); if ((flag & bParse::FD_DOUBLE_PRECISION) != 0) { Bullet::btGeneric6DofSpring2ConstraintDoubleData2* con = (Bullet::btGeneric6DofSpring2ConstraintDoubleData2*)bf.m_constraints[i]; addJointInfoFromConstraint(i, con, bodyJoints, m_data->m_verboseOutput); } else { Bullet::btGeneric6DofSpring2ConstraintData* con = (Bullet::btGeneric6DofSpring2ConstraintData*)bf.m_constraints[i]; addJointInfoFromConstraint(i, con, bodyJoints, m_data->m_verboseOutput); } } for (int i = 0; i < bf.m_multiBodies.size(); i++) { int flag = bf.getFlags(); if ((flag & bParse::FD_DOUBLE_PRECISION) != 0) { Bullet::btMultiBodyDoubleData* mb = (Bullet::btMultiBodyDoubleData*)bf.m_multiBodies[i]; if (mb->m_baseName) { bodyJoints->m_baseName = mb->m_baseName; } addJointInfoFromMultiBodyData(mb, bodyJoints, m_data->m_verboseOutput); } else { Bullet::btMultiBodyFloatData* mb = (Bullet::btMultiBodyFloatData*)bf.m_multiBodies[i]; if (mb->m_baseName) { bodyJoints->m_baseName = mb->m_baseName; } addJointInfoFromMultiBodyData(mb, bodyJoints, m_data->m_verboseOutput); } } if (bf.ok()) { if (m_data->m_verboseOutput) { b3Printf("Received robot description ok!\n"); } } else { b3Warning("Robot description not received"); } } break; } case CMD_DESIRED_STATE_RECEIVED_COMPLETED: { B3_PROFILE("CMD_DESIRED_STATE_RECEIVED_COMPLETED"); if (m_data->m_verboseOutput) { b3Printf("Server received desired state"); } break; } case CMD_STEP_FORWARD_SIMULATION_COMPLETED: { B3_PROFILE("CMD_STEP_FORWARD_SIMULATION_COMPLETED"); if (m_data->m_verboseOutput) { b3Printf("Server completed step simulation"); } break; } case CMD_URDF_LOADING_FAILED: { B3_PROFILE("CMD_URDF_LOADING_FAILED"); if (m_data->m_verboseOutput) { b3Printf("Server failed loading the URDF...\n"); } break; } case CMD_USER_CONSTRAINT_REQUEST_STATE_COMPLETED: { break; } case CMD_USER_CONSTRAINT_INFO_COMPLETED: { B3_PROFILE("CMD_USER_CONSTRAINT_INFO_COMPLETED"); int cid = serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId; m_data->m_userConstraintInfoMap.insert(cid, serverCmd.m_userConstraintResultArgs); break; } case CMD_USER_CONSTRAINT_COMPLETED: { B3_PROFILE("CMD_USER_CONSTRAINT_COMPLETED"); int cid = serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId; m_data->m_userConstraintInfoMap.insert(cid, serverCmd.m_userConstraintResultArgs); break; } case CMD_REMOVE_USER_CONSTRAINT_COMPLETED: { B3_PROFILE("CMD_REMOVE_USER_CONSTRAINT_COMPLETED"); int cid = serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId; m_data->m_userConstraintInfoMap.remove(cid); break; } case CMD_CHANGE_USER_CONSTRAINT_COMPLETED: { B3_PROFILE("CMD_CHANGE_USER_CONSTRAINT_COMPLETED"); int cid = serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId; b3UserConstraint* userConstraintPtr = m_data->m_userConstraintInfoMap[cid]; if (userConstraintPtr) { const b3UserConstraint* serverConstraint = &serverCmd.m_userConstraintResultArgs; if (serverCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_PIVOT_IN_B) { userConstraintPtr->m_childFrame[0] = serverConstraint->m_childFrame[0]; userConstraintPtr->m_childFrame[1] = serverConstraint->m_childFrame[1]; userConstraintPtr->m_childFrame[2] = serverConstraint->m_childFrame[2]; } if (serverCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_FRAME_ORN_IN_B) { userConstraintPtr->m_childFrame[3] = serverConstraint->m_childFrame[3]; userConstraintPtr->m_childFrame[4] = serverConstraint->m_childFrame[4]; userConstraintPtr->m_childFrame[5] = serverConstraint->m_childFrame[5]; userConstraintPtr->m_childFrame[6] = serverConstraint->m_childFrame[6]; } if (serverCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_MAX_FORCE) { userConstraintPtr->m_maxAppliedForce = serverConstraint->m_maxAppliedForce; } if (serverCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_GEAR_RATIO) { userConstraintPtr->m_gearRatio = serverConstraint->m_gearRatio; } if (serverCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_RELATIVE_POSITION_TARGET) { userConstraintPtr->m_relativePositionTarget = serverConstraint->m_relativePositionTarget; } if (serverCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_ERP) { userConstraintPtr->m_erp = serverConstraint->m_erp; } if (serverCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_GEAR_AUX_LINK) { userConstraintPtr->m_gearAuxLink = serverConstraint->m_gearAuxLink; } } break; } case CMD_USER_CONSTRAINT_FAILED: { B3_PROFILE("CMD_USER_CONSTRAINT_FAILED"); b3Warning("createConstraint failed"); break; } case CMD_REMOVE_USER_CONSTRAINT_FAILED: { B3_PROFILE("CMD_REMOVE_USER_CONSTRAINT_FAILED"); b3Warning("removeConstraint failed"); break; } case CMD_CHANGE_USER_CONSTRAINT_FAILED: { B3_PROFILE("CMD_CHANGE_USER_CONSTRAINT_FAILED"); //b3Warning("changeConstraint failed"); break; } case CMD_ACTUAL_STATE_UPDATE_FAILED: { B3_PROFILE("CMD_ACTUAL_STATE_UPDATE_FAILED"); b3Warning("request actual state failed"); break; } case CMD_BODY_INFO_COMPLETED: { B3_PROFILE("CMD_BODY_INFO_COMPLETED"); if (m_data->m_verboseOutput) { b3Printf("Received body info\n"); } int bodyUniqueId = serverCmd.m_dataStreamArguments.m_bodyUniqueId; processBodyJointInfo(bodyUniqueId, serverCmd); break; } case CMD_MJCF_LOADING_FAILED: { B3_PROFILE("CMD_MJCF_LOADING_FAILED"); if (m_data->m_verboseOutput) { b3Printf("Server failed loading the MJCF...\n"); } break; } case CMD_SDF_LOADING_FAILED: { B3_PROFILE("CMD_SDF_LOADING_FAILED"); if (m_data->m_verboseOutput) { b3Printf("Server failed loading the SDF...\n"); } break; } case CMD_BULLET_DATA_STREAM_RECEIVED_COMPLETED: { B3_PROFILE("CMD_BULLET_DATA_STREAM_RECEIVED_COMPLETED"); if (m_data->m_verboseOutput) { b3Printf("Server received bullet data stream OK\n"); } break; } case CMD_BULLET_DATA_STREAM_RECEIVED_FAILED: { B3_PROFILE("CMD_BULLET_DATA_STREAM_RECEIVED_FAILED"); if (m_data->m_verboseOutput) { b3Printf("Server failed receiving bullet data stream\n"); } break; } case CMD_ACTUAL_STATE_UPDATE_COMPLETED: { B3_PROFILE("CMD_ACTUAL_STATE_UPDATE_COMPLETED"); if (m_data->m_verboseOutput) { b3Printf("Received actual state\n"); SharedMemoryStatus& command = m_data->m_testBlock1->m_serverCommands[0]; int numQ = command.m_sendActualStateArgs.m_numDegreeOfFreedomQ; int numU = command.m_sendActualStateArgs.m_numDegreeOfFreedomU; b3Printf("size Q = %d, size U = %d\n", numQ, numU); char msg[1024]; { sprintf(msg, "Q=["); for (int i = 0; i < numQ; i++) { if (i < numQ - 1) { sprintf(msg, "%s%f,", msg, m_data->m_cachedState.m_actualStateQ[i]); } else { sprintf(msg, "%s%f", msg, m_data->m_cachedState.m_actualStateQ[i]); } } sprintf(msg, "%s]", msg); } b3Printf(msg); sprintf(msg, "U=["); for (int i = 0; i < numU; i++) { if (i < numU - 1) { sprintf(msg, "%s%f,", msg, m_data->m_cachedState.m_actualStateQdot[i]); } else { sprintf(msg, "%s%f", msg, m_data->m_cachedState.m_actualStateQdot[i]); } } sprintf(msg, "%s]", msg); b3Printf(msg); b3Printf("\n"); } break; } case CMD_RESET_SIMULATION_COMPLETED: { B3_PROFILE("CMD_RESET_SIMULATION_COMPLETED"); if (m_data->m_verboseOutput) { b3Printf("CMD_RESET_SIMULATION_COMPLETED clean data\n"); } resetData(); break; } case CMD_DEBUG_LINES_COMPLETED: { B3_PROFILE("CMD_DEBUG_LINES_COMPLETED"); if (m_data->m_verboseOutput) { b3Printf("Success receiving %d debug lines", serverCmd.m_sendDebugLinesArgs.m_numDebugLines); } int numLines = serverCmd.m_sendDebugLinesArgs.m_numDebugLines; float* linesFrom = (float*)&m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor[0]; float* linesTo = (float*)(&m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor[0] + numLines * 3 * sizeof(float)); float* linesColor = (float*)(&m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor[0] + 2 * numLines * 3 * sizeof(float)); m_data->m_debugLinesFrom.resize(serverCmd.m_sendDebugLinesArgs.m_startingLineIndex + numLines); m_data->m_debugLinesTo.resize(serverCmd.m_sendDebugLinesArgs.m_startingLineIndex + numLines); m_data->m_debugLinesColor.resize( serverCmd.m_sendDebugLinesArgs.m_startingLineIndex + numLines); for (int i = 0; i < numLines; i++) { TmpFloat3 from = CreateTmpFloat3(linesFrom[i * 3], linesFrom[i * 3 + 1], linesFrom[i * 3 + 2]); TmpFloat3 to = CreateTmpFloat3(linesTo[i * 3], linesTo[i * 3 + 1], linesTo[i * 3 + 2]); TmpFloat3 color = CreateTmpFloat3(linesColor[i * 3], linesColor[i * 3 + 1], linesColor[i * 3 + 2]); m_data ->m_debugLinesFrom[serverCmd.m_sendDebugLinesArgs.m_startingLineIndex + i] = from; m_data->m_debugLinesTo[serverCmd.m_sendDebugLinesArgs.m_startingLineIndex + i] = to; m_data->m_debugLinesColor[serverCmd.m_sendDebugLinesArgs.m_startingLineIndex + i] = color; } break; } case CMD_RIGID_BODY_CREATION_COMPLETED: { B3_PROFILE("CMD_RIGID_BODY_CREATION_COMPLETED"); break; } case CMD_DEBUG_LINES_OVERFLOW_FAILED: { B3_PROFILE("CMD_DEBUG_LINES_OVERFLOW_FAILED"); b3Warning("Error receiving debug lines"); m_data->m_debugLinesFrom.resize(0); m_data->m_debugLinesTo.resize(0); m_data->m_debugLinesColor.resize(0); break; } case CMD_CAMERA_IMAGE_COMPLETED: { B3_PROFILE("CMD_CAMERA_IMAGE_COMPLETED"); if (m_data->m_verboseOutput) { b3Printf("Camera image OK\n"); } int numBytesPerPixel = 4; //RGBA int numTotalPixels = serverCmd.m_sendPixelDataArguments.m_startingPixelIndex + serverCmd.m_sendPixelDataArguments.m_numPixelsCopied + serverCmd.m_sendPixelDataArguments.m_numRemainingPixels; m_data->m_cachedCameraPixelsWidth = 0; m_data->m_cachedCameraPixelsHeight = 0; int numPixels = serverCmd.m_sendPixelDataArguments.m_imageWidth * serverCmd.m_sendPixelDataArguments.m_imageHeight; m_data->m_cachedCameraPixelsRGBA.reserve(numPixels * numBytesPerPixel); m_data->m_cachedCameraDepthBuffer.resize(numTotalPixels); m_data->m_cachedSegmentationMaskBuffer.resize(numTotalPixels); m_data->m_cachedCameraPixelsRGBA.resize(numTotalPixels * numBytesPerPixel); unsigned char* rgbaPixelsReceived = (unsigned char*)&m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor[0]; // printf("pixel = %d\n", rgbaPixelsReceived[0]); float* depthBuffer = (float*)&(m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor[serverCmd.m_sendPixelDataArguments.m_numPixelsCopied * 4]); int* segmentationMaskBuffer = (int*)&(m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor[serverCmd.m_sendPixelDataArguments.m_numPixelsCopied * 8]); for (int i = 0; i < serverCmd.m_sendPixelDataArguments.m_numPixelsCopied; i++) { m_data->m_cachedCameraDepthBuffer[i + serverCmd.m_sendPixelDataArguments.m_startingPixelIndex] = depthBuffer[i]; } for (int i = 0; i < serverCmd.m_sendPixelDataArguments.m_numPixelsCopied; i++) { m_data->m_cachedSegmentationMaskBuffer[i + serverCmd.m_sendPixelDataArguments.m_startingPixelIndex] = segmentationMaskBuffer[i]; } for (int i = 0; i < serverCmd.m_sendPixelDataArguments.m_numPixelsCopied * numBytesPerPixel; i++) { m_data->m_cachedCameraPixelsRGBA[i + serverCmd.m_sendPixelDataArguments.m_startingPixelIndex * numBytesPerPixel] = rgbaPixelsReceived[i]; } break; } case CMD_CAMERA_IMAGE_FAILED: { B3_PROFILE("CMD_CAMERA_IMAGE_FAILED"); b3Warning("Camera image FAILED\n"); break; } case CMD_REQUEST_MESH_DATA_COMPLETED: { m_data->m_cachedVertexPositions.resize(serverCmd.m_sendMeshDataArgs.m_startingVertex + serverCmd.m_sendMeshDataArgs.m_numVerticesCopied); btVector3* verticesReceived = (btVector3*)m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor; for (int i = 0; i < serverCmd.m_sendMeshDataArgs.m_numVerticesCopied; i++) { m_data->m_cachedVertexPositions[i + serverCmd.m_sendMeshDataArgs.m_startingVertex].x = verticesReceived[i].x(); m_data->m_cachedVertexPositions[i + serverCmd.m_sendMeshDataArgs.m_startingVertex].y = verticesReceived[i].y(); m_data->m_cachedVertexPositions[i + serverCmd.m_sendMeshDataArgs.m_startingVertex].z = verticesReceived[i].z(); m_data->m_cachedVertexPositions[i + serverCmd.m_sendMeshDataArgs.m_startingVertex].w = verticesReceived[i].w(); } break; } case CMD_REQUEST_MESH_DATA_FAILED: { b3Warning("Request mesh data failed"); break; } case CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED: { break; } case CMD_CALCULATED_INVERSE_DYNAMICS_FAILED: { b3Warning("Inverse Dynamics computations failed"); break; } case CMD_REQUEST_AABB_OVERLAP_FAILED: { b3Warning("Overlapping object query failed"); break; } case CMD_REQUEST_RAY_CAST_INTERSECTIONS_COMPLETED: { B3_PROFILE("m_raycastHits"); if (m_data->m_verboseOutput) { b3Printf("Raycast completed"); } m_data->m_raycastHits.clear(); b3RayHitInfo* rayHits = (b3RayHitInfo*)m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor; for (int i = 0; i < serverCmd.m_raycastHits.m_numRaycastHits; i++) { m_data->m_raycastHits.push_back(rayHits[i]); } break; } case CMD_REQUEST_VR_EVENTS_DATA_COMPLETED: { B3_PROFILE("CMD_REQUEST_VR_EVENTS_DATA_COMPLETED"); if (m_data->m_verboseOutput) { b3Printf("Request VR Events completed"); } m_data->m_cachedVREvents.clear(); for (int i = 0; i < serverCmd.m_sendVREvents.m_numVRControllerEvents; i++) { m_data->m_cachedVREvents.push_back(serverCmd.m_sendVREvents.m_controllerEvents[i]); } break; } case CMD_REQUEST_KEYBOARD_EVENTS_DATA_COMPLETED: { B3_PROFILE("CMD_REQUEST_KEYBOARD_EVENTS_DATA_COMPLETED"); if (m_data->m_verboseOutput) { b3Printf("Request keyboard events completed"); } m_data->m_cachedKeyboardEvents.resize(serverCmd.m_sendKeyboardEvents.m_numKeyboardEvents); for (int i = 0; i < serverCmd.m_sendKeyboardEvents.m_numKeyboardEvents; i++) { m_data->m_cachedKeyboardEvents[i] = serverCmd.m_sendKeyboardEvents.m_keyboardEvents[i]; } break; } case CMD_REQUEST_MOUSE_EVENTS_DATA_COMPLETED: { B3_PROFILE("CMD_REQUEST_MOUSE_EVENTS_DATA_COMPLETED"); if (m_data->m_verboseOutput) { b3Printf("Request mouse events completed"); } m_data->m_cachedMouseEvents.resize(serverCmd.m_sendMouseEvents.m_numMouseEvents); for (int i = 0; i < serverCmd.m_sendMouseEvents.m_numMouseEvents; i++) { m_data->m_cachedMouseEvents[i] = serverCmd.m_sendMouseEvents.m_mouseEvents[i]; } break; } case CMD_REQUEST_AABB_OVERLAP_COMPLETED: { B3_PROFILE("CMD_REQUEST_AABB_OVERLAP_COMPLETED"); if (m_data->m_verboseOutput) { b3Printf("Overlapping object request completed"); } int startOverlapIndex = serverCmd.m_sendOverlappingObjectsArgs.m_startingOverlappingObjectIndex; int numOverlapCopied = serverCmd.m_sendOverlappingObjectsArgs.m_numOverlappingObjectsCopied; m_data->m_cachedOverlappingObjects.resize(startOverlapIndex + numOverlapCopied); b3OverlappingObject* objects = (b3OverlappingObject*)m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor; for (int i = 0; i < numOverlapCopied; i++) { m_data->m_cachedOverlappingObjects[startOverlapIndex + i] = objects[i]; } break; } case CMD_CONTACT_POINT_INFORMATION_COMPLETED: { B3_PROFILE("CMD_CONTACT_POINT_INFORMATION_COMPLETED"); if (m_data->m_verboseOutput) { b3Printf("Contact Point Information Request OK\n"); } int startContactIndex = serverCmd.m_sendContactPointArgs.m_startingContactPointIndex; int numContactsCopied = serverCmd.m_sendContactPointArgs.m_numContactPointsCopied; m_data->m_cachedContactPoints.resize(startContactIndex + numContactsCopied); b3ContactPointData* contactData = (b3ContactPointData*)m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor; for (int i = 0; i < numContactsCopied; i++) { m_data->m_cachedContactPoints[startContactIndex + i] = contactData[i]; } break; } case CMD_CONTACT_POINT_INFORMATION_FAILED: { B3_PROFILE("CMD_CONTACT_POINT_INFORMATION_FAILED"); b3Warning("Contact Point Information Request failed"); break; } case CMD_SAVE_WORLD_COMPLETED: { B3_PROFILE("CMD_SAVE_WORLD_COMPLETED"); break; } case CMD_SAVE_WORLD_FAILED: { B3_PROFILE("CMD_SAVE_WORLD_FAILED"); b3Warning("Saving world failed"); break; } case CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED: { B3_PROFILE("CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED"); break; } case CMD_CALCULATE_INVERSE_KINEMATICS_FAILED: { B3_PROFILE("CMD_CALCULATE_INVERSE_KINEMATICS_FAILED"); b3Warning("Calculate Inverse Kinematics Request failed"); break; } case CMD_VISUAL_SHAPE_INFO_COMPLETED: { B3_PROFILE("CMD_VISUAL_SHAPE_INFO_COMPLETED"); if (m_data->m_verboseOutput) { b3Printf("Visual Shape Information Request OK\n"); } int startVisualShapeIndex = serverCmd.m_sendVisualShapeArgs.m_startingVisualShapeIndex; int numVisualShapesCopied = serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied; m_data->m_cachedVisualShapes.resize(startVisualShapeIndex + numVisualShapesCopied); b3VisualShapeData* shapeData = (b3VisualShapeData*)m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor; for (int i = 0; i < numVisualShapesCopied; i++) { m_data->m_cachedVisualShapes[startVisualShapeIndex + i] = shapeData[i]; } break; } case CMD_VISUAL_SHAPE_INFO_FAILED: { b3Warning("Visual Shape Info Request failed"); break; } case CMD_VISUAL_SHAPE_UPDATE_COMPLETED: { break; } case CMD_VISUAL_SHAPE_UPDATE_FAILED: { b3Warning("Visual Shape Update failed"); break; } case CMD_LOAD_TEXTURE_COMPLETED: { break; } case CMD_LOAD_TEXTURE_FAILED: { b3Warning("Load texture failed"); break; } case CMD_BULLET_LOADING_COMPLETED: { break; } case CMD_BULLET_LOADING_FAILED: { b3Warning("Load .bullet failed"); break; } case CMD_BULLET_SAVING_FAILED: { b3Warning("Save .bullet failed"); break; } case CMD_USER_DEBUG_DRAW_PARAMETER_COMPLETED: case CMD_USER_DEBUG_DRAW_COMPLETED: { break; } case CMD_USER_DEBUG_DRAW_FAILED: { b3Warning("User debug draw failed"); break; } case CMD_SYNC_BODY_INFO_COMPLETED: { clearCachedBodies(); break; } case CMD_STATE_LOGGING_START_COMPLETED: { break; }; case CMD_STATE_LOGGING_COMPLETED: { break; }; case CMD_STATE_LOGGING_FAILED: { b3Warning("State Logging failed"); break; } case CMD_REQUEST_OPENGL_VISUALIZER_CAMERA_FAILED: { b3Warning("Request visualizer camera failed"); break; } case CMD_REQUEST_OPENGL_VISUALIZER_CAMERA_COMPLETED: { break; } case CMD_REMOVE_BODY_COMPLETED: { break; } case CMD_REMOVE_BODY_FAILED: { b3Warning("Removing body failed"); break; } case CMD_GET_DYNAMICS_INFO_COMPLETED: { break; } case CMD_GET_DYNAMICS_INFO_FAILED: { b3Warning("Request dynamics info failed"); break; } case CMD_CREATE_COLLISION_SHAPE_FAILED: { b3Warning("Request createCollisionShape failed"); break; } case CMD_CREATE_COLLISION_SHAPE_COMPLETED: case CMD_CREATE_VISUAL_SHAPE_COMPLETED: { break; } case CMD_CREATE_MULTI_BODY_FAILED: { b3Warning("Request createMultiBody failed"); break; } case CMD_CREATE_VISUAL_SHAPE_FAILED: { b3Warning("Request createVisualShape failed"); break; } case CMD_REQUEST_COLLISION_INFO_COMPLETED: { break; } case CMD_REQUEST_COLLISION_INFO_FAILED: { b3Warning("Request getCollisionInfo failed"); break; } case CMD_CUSTOM_COMMAND_COMPLETED: { m_data->m_cachedReturnData.resize(serverCmd.m_customCommandResultArgs.m_returnDataSizeInBytes); m_data->m_cachedReturnDataValue.m_length = serverCmd.m_customCommandResultArgs.m_returnDataSizeInBytes; if (serverCmd.m_customCommandResultArgs.m_returnDataSizeInBytes) { m_data->m_cachedReturnDataValue.m_type = serverCmd.m_customCommandResultArgs.m_returnDataType; m_data->m_cachedReturnDataValue.m_data1 = &m_data->m_cachedReturnData[0]; for (int i = 0; i < serverCmd.m_numDataStreamBytes; i++) { m_data->m_cachedReturnData[i + serverCmd.m_customCommandResultArgs.m_returnDataStart] = m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor[i]; } } break; } case CMD_CALCULATED_JACOBIAN_COMPLETED: { break; } case CMD_CALCULATED_JACOBIAN_FAILED: { b3Warning("jacobian calculation failed"); break; } case CMD_CUSTOM_COMMAND_FAILED: { b3Warning("custom plugin command failed"); break; } case CMD_CALCULATED_MASS_MATRIX_FAILED: { b3Warning("calculate mass matrix failed"); break; } case CMD_CALCULATED_MASS_MATRIX_COMPLETED: { double* matrixData = (double*)&this->m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor[0]; m_data->m_cachedMassMatrix.resize(serverCmd.m_massMatrixResultArgs.m_dofCount * serverCmd.m_massMatrixResultArgs.m_dofCount); for (int i = 0; i < serverCmd.m_massMatrixResultArgs.m_dofCount * serverCmd.m_massMatrixResultArgs.m_dofCount; i++) { m_data->m_cachedMassMatrix[i] = matrixData[i]; } break; } case CMD_REQUEST_PHYSICS_SIMULATION_PARAMETERS_COMPLETED: { break; } case CMD_SAVE_STATE_COMPLETED: { break; } case CMD_RESTORE_STATE_FAILED: { b3Warning("restoreState failed"); break; } case CMD_RESTORE_STATE_COMPLETED: { break; } case CMD_BULLET_SAVING_COMPLETED: { break; } case CMD_COLLISION_SHAPE_INFO_FAILED: { b3Warning("getCollisionShapeData failed"); break; } case CMD_COLLISION_SHAPE_INFO_COMPLETED: { B3_PROFILE("CMD_COLLISION_SHAPE_INFO_COMPLETED"); if (m_data->m_verboseOutput) { b3Printf("Collision Shape Information Request OK\n"); } int numCollisionShapesCopied = serverCmd.m_sendCollisionShapeArgs.m_numCollisionShapes; m_data->m_cachedCollisionShapes.resize(numCollisionShapesCopied); b3CollisionShapeData* shapeData = (b3CollisionShapeData*)m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor; for (int i = 0; i < numCollisionShapesCopied; i++) { m_data->m_cachedCollisionShapes[i] = shapeData[i]; } break; } case CMD_LOAD_SOFT_BODY_FAILED: { B3_PROFILE("CMD_LOAD_SOFT_BODY_FAILED"); if (m_data->m_verboseOutput) { b3Printf("Server failed loading the SoftBody...\n"); } break; } case CMD_LOAD_SOFT_BODY_COMPLETED: { B3_PROFILE("CMD_LOAD_SOFT_BODY_COMPLETED"); if (m_data->m_verboseOutput) { b3Printf("Server loading the SoftBody OK\n"); } b3Assert(serverCmd.m_numDataStreamBytes); if (serverCmd.m_numDataStreamBytes > 0) { bParse::btBulletFile bf( this->m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor, serverCmd.m_numDataStreamBytes); bf.setFileDNAisMemoryDNA(); bf.parse(false); int bodyUniqueId = serverCmd.m_dataStreamArguments.m_bodyUniqueId; BodyJointInfoCache* bodyJoints = new BodyJointInfoCache; m_data->m_bodyJointMap.insert(bodyUniqueId, bodyJoints); bodyJoints->m_bodyName = serverCmd.m_dataStreamArguments.m_bodyName; bodyJoints->m_baseName = serverCmd.m_dataStreamArguments.m_bodyName; if (bf.ok()) { if (m_data->m_verboseOutput) { b3Printf("Received robot description ok!\n"); } } else { b3Warning("Robot description not received when loading soft body!"); } } break; } case CMD_SYNC_USER_DATA_FAILED: { b3Warning("Synchronizing user data failed."); break; } case CMD_REQUEST_USER_DATA_FAILED: { b3Warning("Requesting user data failed"); break; } case CMD_ADD_USER_DATA_FAILED: { b3Warning("Adding user data failed (do the specified body and link exist?)"); break; } case CMD_REMOVE_USER_DATA_FAILED: { b3Warning("Removing user data failed"); break; } case CMD_REQUEST_USER_DATA_COMPLETED: case CMD_SYNC_USER_DATA_COMPLETED: case CMD_REMOVE_USER_DATA_COMPLETED: case CMD_ADD_USER_DATA_COMPLETED: case CMD_REMOVE_STATE_FAILED: case CMD_REMOVE_STATE_COMPLETED: { break; } default: { b3Error("Unknown server status %d\n", serverCmd.m_type); btAssert(0); } }; m_data->m_testBlock1->m_numProcessedServerCommands++; // we don't have more than 1 command outstanding (in total, either server or client) btAssert(m_data->m_testBlock1->m_numProcessedServerCommands == m_data->m_testBlock1->m_numServerCommands); if (m_data->m_testBlock1->m_numServerCommands == m_data->m_testBlock1->m_numProcessedServerCommands) { m_data->m_waitingForServer = false; } else { m_data->m_waitingForServer = true; } if ((serverCmd.m_type == CMD_SDF_LOADING_COMPLETED) || (serverCmd.m_type == CMD_MJCF_LOADING_COMPLETED) || (serverCmd.m_type == CMD_SYNC_BODY_INFO_COMPLETED)) { 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); } } 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(); SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0]; //now transfer the information of the individual objects etc. command.m_type = CMD_REQUEST_BODY_INFO; command.m_sdfRequestInfoArgs.m_bodyUniqueId = bodyId; submitClientCommand(command); return 0; } } if (serverCmd.m_type == CMD_SYNC_USER_DATA_COMPLETED) { B3_PROFILE("CMD_SYNC_USER_DATA_COMPLETED"); if (serverCmd.m_syncUserDataArgs.m_clearCachedUserDataEntries) { // Remove all cached user data entries. for (int i = 0; i < m_data->m_bodyJointMap.size(); i++) { BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap.getAtIndex(i); if (bodyJointsPtr && *bodyJointsPtr) { (*bodyJointsPtr)->m_userDataIds.clear(); } } m_data->m_userDataMap.clear(); m_data->m_userDataHandleLookup.clear(); } const int numIdentifiers = serverCmd.m_syncUserDataArgs.m_numUserDataIdentifiers; if (numIdentifiers > 0) { m_data->m_tempBackupServerStatus = m_data->m_lastServerStatus; const int* identifiers = (int*)m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor; m_data->m_userDataIdsRequestInfo.reserve(numIdentifiers - 1); // Store the identifiers that still need to be requested. for (int i = 0; i < numIdentifiers - 1; i++) { m_data->m_userDataIdsRequestInfo.push_back(identifiers[i]); } // Request individual user data entries, start with last identifier. SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0]; command.m_type = CMD_REQUEST_USER_DATA; command.m_userDataRequestArgs.m_userDataId = identifiers[numIdentifiers - 1]; submitClientCommand(command); return 0; } } if (serverCmd.m_type == CMD_ADD_USER_DATA_COMPLETED || serverCmd.m_type == CMD_REQUEST_USER_DATA_COMPLETED) { B3_PROFILE("CMD_ADD_USER_DATA_COMPLETED"); const UserDataResponseArgs response = serverCmd.m_userDataResponseArgs; BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[response.m_bodyUniqueId]; if (bodyJointsPtr && *bodyJointsPtr) { const char* dataStream = m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor; SharedMemoryUserData* userData = m_data->m_userDataMap[response.m_userDataId]; if (userData) { // Only replace the value. userData->replaceValue(dataStream, response.m_valueLength, response.m_valueType); } else { // Add a new user data entry. const char* key = response.m_key; m_data->m_userDataMap.insert(response.m_userDataId, SharedMemoryUserData(key, response.m_bodyUniqueId, response.m_linkIndex, response.m_visualShapeIndex)); userData = m_data->m_userDataMap[response.m_userDataId]; userData->replaceValue(dataStream, response.m_valueLength, response.m_valueType); m_data->m_userDataHandleLookup.insert(SharedMemoryUserDataHashKey(userData), response.m_userDataId); (*bodyJointsPtr)->m_userDataIds.push_back(response.m_userDataId); } } } if (serverCmd.m_type == CMD_REQUEST_USER_DATA_COMPLETED) { if (m_data->m_userDataIdsRequestInfo.size() > 0) { // Request individual user data entries. const int userDataId = m_data->m_userDataIdsRequestInfo[m_data->m_userDataIdsRequestInfo.size() - 1]; m_data->m_userDataIdsRequestInfo.pop_back(); SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0]; command.m_type = CMD_REQUEST_USER_DATA; command.m_userDataRequestArgs.m_userDataId = userDataId; submitClientCommand(command); return 0; } m_data->m_lastServerStatus = m_data->m_tempBackupServerStatus; } if (serverCmd.m_type == CMD_REMOVE_USER_DATA_COMPLETED) { B3_PROFILE("CMD_REMOVE_USER_DATA_COMPLETED"); const int userDataId = serverCmd.m_removeUserDataResponseArgs.m_userDataId; SharedMemoryUserData* userData = m_data->m_userDataMap[userDataId]; if (userData) { BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[userData->m_bodyUniqueId]; if (bodyJointsPtr && *bodyJointsPtr) { (*bodyJointsPtr)->m_userDataIds.remove(userDataId); } m_data->m_userDataHandleLookup.remove(SharedMemoryUserDataHashKey(userData)); m_data->m_userDataMap.remove(userDataId); } } if (serverCmd.m_type == CMD_REMOVE_BODY_COMPLETED) { for (int i = 0; i < serverCmd.m_removeObjectArgs.m_numBodies; i++) { int bodyUniqueId = serverCmd.m_removeObjectArgs.m_bodyUniqueIds[i]; removeCachedBody(bodyUniqueId); } for (int i = 0; i < serverCmd.m_removeObjectArgs.m_numUserConstraints; i++) { int key = serverCmd.m_removeObjectArgs.m_userConstraintUniqueIds[i]; m_data->m_userConstraintInfoMap.remove(key); } } if (serverCmd.m_type == CMD_USER_CONSTRAINT_INFO_COMPLETED) { B3_PROFILE("CMD_USER_CONSTRAINT_INFO_COMPLETED"); if (m_data->m_constraintIdsRequestInfo.size()) { int cid = m_data->m_constraintIdsRequestInfo[m_data->m_constraintIdsRequestInfo.size() - 1]; m_data->m_constraintIdsRequestInfo.pop_back(); SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0]; command.m_type = CMD_USER_CONSTRAINT; command.m_updateFlags = USER_CONSTRAINT_REQUEST_INFO; command.m_userConstraintArguments.m_userConstraintUniqueId = cid; submitClientCommand(command); return 0; } else { m_data->m_lastServerStatus = m_data->m_tempBackupServerStatus; } } if (serverCmd.m_type == CMD_BODY_INFO_COMPLETED) { B3_PROFILE("CMD_BODY_INFO_COMPLETED"); //are there any bodies left to be processed? if (m_data->m_bodyIdsRequestInfo.size()) { int bodyId = m_data->m_bodyIdsRequestInfo[m_data->m_bodyIdsRequestInfo.size() - 1]; m_data->m_bodyIdsRequestInfo.pop_back(); SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0]; //now transfer the information of the individual objects etc. command.m_type = CMD_REQUEST_BODY_INFO; command.m_sdfRequestInfoArgs.m_bodyUniqueId = bodyId; submitClientCommand(command); return 0; } else { if (m_data->m_constraintIdsRequestInfo.size()) { int cid = m_data->m_constraintIdsRequestInfo[m_data->m_constraintIdsRequestInfo.size() - 1]; m_data->m_constraintIdsRequestInfo.pop_back(); SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0]; command.m_type = CMD_USER_CONSTRAINT; command.m_updateFlags = USER_CONSTRAINT_REQUEST_INFO; command.m_userConstraintArguments.m_userConstraintUniqueId = cid; submitClientCommand(command); return 0; } else { m_data->m_lastServerStatus = m_data->m_tempBackupServerStatus; } } } if (serverCmd.m_type == CMD_REQUEST_AABB_OVERLAP_COMPLETED) { B3_PROFILE("CMD_REQUEST_AABB_OVERLAP_COMPLETED2"); SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0]; if (serverCmd.m_sendOverlappingObjectsArgs.m_numRemainingOverlappingObjects > 0 && serverCmd.m_sendOverlappingObjectsArgs.m_numOverlappingObjectsCopied) { command.m_type = CMD_REQUEST_AABB_OVERLAP; command.m_requestOverlappingObjectsArgs.m_startingOverlappingObjectIndex = serverCmd.m_sendOverlappingObjectsArgs.m_startingOverlappingObjectIndex + serverCmd.m_sendOverlappingObjectsArgs.m_numOverlappingObjectsCopied; submitClientCommand(command); return 0; } } if (serverCmd.m_type == CMD_CONTACT_POINT_INFORMATION_COMPLETED) { B3_PROFILE("CMD_CONTACT_POINT_INFORMATION_COMPLETED2"); SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0]; if (serverCmd.m_sendContactPointArgs.m_numRemainingContactPoints > 0 && serverCmd.m_sendContactPointArgs.m_numContactPointsCopied) { command.m_type = CMD_REQUEST_CONTACT_POINT_INFORMATION; command.m_requestContactPointArguments.m_startingContactPointIndex = serverCmd.m_sendContactPointArgs.m_startingContactPointIndex + serverCmd.m_sendContactPointArgs.m_numContactPointsCopied; command.m_requestContactPointArguments.m_objectAIndexFilter = -1; command.m_requestContactPointArguments.m_objectBIndexFilter = -1; submitClientCommand(command); return 0; } } if (serverCmd.m_type == CMD_VISUAL_SHAPE_INFO_COMPLETED) { B3_PROFILE("CMD_VISUAL_SHAPE_INFO_COMPLETED2"); SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0]; if (serverCmd.m_sendVisualShapeArgs.m_numRemainingVisualShapes > 0 && serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied) { command.m_type = CMD_REQUEST_VISUAL_SHAPE_INFO; command.m_requestVisualShapeDataArguments.m_startingVisualShapeIndex = serverCmd.m_sendVisualShapeArgs.m_startingVisualShapeIndex + serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied; command.m_requestVisualShapeDataArguments.m_bodyUniqueId = serverCmd.m_sendVisualShapeArgs.m_bodyUniqueId; submitClientCommand(command); return 0; } } if (serverCmd.m_type == CMD_CAMERA_IMAGE_COMPLETED) { B3_PROFILE("CMD_CAMERA_IMAGE_COMPLETED2"); SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0]; if (serverCmd.m_sendPixelDataArguments.m_numRemainingPixels > 0 && serverCmd.m_sendPixelDataArguments.m_numPixelsCopied) { // continue requesting remaining pixels command.m_type = CMD_REQUEST_CAMERA_IMAGE_DATA; command.m_requestPixelDataArguments.m_startPixelIndex = serverCmd.m_sendPixelDataArguments.m_startingPixelIndex + serverCmd.m_sendPixelDataArguments.m_numPixelsCopied; submitClientCommand(command); return 0; } else { m_data->m_cachedCameraPixelsWidth = serverCmd.m_sendPixelDataArguments.m_imageWidth; m_data->m_cachedCameraPixelsHeight = serverCmd.m_sendPixelDataArguments.m_imageHeight; } } if (serverCmd.m_type == CMD_REQUEST_MESH_DATA_COMPLETED) { B3_PROFILE("CMD_REQUEST_MESH_DATA_COMPLETED"); SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0]; if (serverCmd.m_sendMeshDataArgs.m_numVerticesRemaining > 0 && serverCmd.m_sendMeshDataArgs.m_numVerticesCopied) { command.m_type = CMD_REQUEST_MESH_DATA; command.m_requestMeshDataArgs.m_startingVertex = serverCmd.m_sendMeshDataArgs.m_startingVertex + serverCmd.m_sendMeshDataArgs.m_numVerticesCopied; submitClientCommand(command); return 0; } else { m_data->m_cachedMeshData.m_numVertices = serverCmd.m_sendMeshDataArgs.m_startingVertex + serverCmd.m_sendMeshDataArgs.m_numVerticesCopied; } } if ((serverCmd.m_type == CMD_DEBUG_LINES_COMPLETED) && (serverCmd.m_sendDebugLinesArgs.m_numRemainingDebugLines > 0)) { B3_PROFILE("CMD_DEBUG_LINES_COMPLETED2"); SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0]; // continue requesting debug lines for drawing command.m_type = CMD_REQUEST_DEBUG_LINES; command.m_requestDebugLinesArguments.m_startingLineIndex = serverCmd.m_sendDebugLinesArgs.m_numDebugLines + serverCmd.m_sendDebugLinesArgs.m_startingLineIndex; submitClientCommand(command); return 0; } if (serverCmd.m_type == CMD_CUSTOM_COMMAND_COMPLETED) { int totalReceived = (serverCmd.m_numDataStreamBytes + serverCmd.m_customCommandResultArgs.m_returnDataStart); int remaining = serverCmd.m_customCommandResultArgs.m_returnDataSizeInBytes - totalReceived; if (remaining > 0) { // continue requesting return data SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0]; command.m_type = CMD_CUSTOM_COMMAND; command.m_customCommandArgs.m_startingReturnBytes = totalReceived; submitClientCommand(command); return 0; } } return &m_data->m_lastServerStatus; } else { if (m_data->m_verboseOutput) { B3_PROFILE("m_verboseOutput"); b3Printf("m_numServerStatus = %d, processed = %d\n", m_data->m_testBlock1->m_numServerCommands, m_data->m_testBlock1->m_numProcessedServerCommands); } } return 0; } bool PhysicsClientSharedMemory::canSubmitCommand() const { if (m_data->m_isConnected && !m_data->m_waitingForServer) { if (m_data->m_testBlock1->m_magicId == SHARED_MEMORY_MAGIC_NUMBER) { return true; } else { return false; } } return false; } struct SharedMemoryCommand* PhysicsClientSharedMemory::getAvailableSharedMemoryCommand() { static int sequence = 0; m_data->m_testBlock1->m_clientCommands[0].m_sequenceNumber = sequence++; return &m_data->m_testBlock1->m_clientCommands[0]; } bool PhysicsClientSharedMemory::submitClientCommand(const SharedMemoryCommand& command) { /// at the moment we allow a maximum of 1 outstanding command, so we check for this // once the server processed the command and returns a status, we clear the flag // "m_data->m_waitingForServer" and allow submitting the next command if (!m_data->m_waitingForServer) { if (&m_data->m_testBlock1->m_clientCommands[0] != &command) { m_data->m_testBlock1->m_clientCommands[0] = command; } m_data->m_testBlock1->m_numClientCommands++; m_data->m_waitingForServer = true; return true; } return false; } void PhysicsClientSharedMemory::uploadBulletFileToSharedMemory(const char* data, int len) { btAssert(len < SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); if (len >= SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE) { b3Warning("uploadBulletFileToSharedMemory %d exceeds max size %d\n", len, SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); } else { for (int i = 0; i < len; i++) { //m_data->m_testBlock1->m_bulletStreamDataClientToServer[i] = data[i]; m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor[i] = data[i]; } } } void PhysicsClientSharedMemory::uploadRaysToSharedMemory(struct SharedMemoryCommand& command, const double* rayFromWorldArray, const double* rayToWorldArray, int numRays) { int curNumStreamingRays = command.m_requestRaycastIntersections.m_numStreamingRays; int newNumRays = curNumStreamingRays + numRays; btAssert(newNumRays < MAX_RAY_INTERSECTION_BATCH_SIZE_STREAMING); if (newNumRays < MAX_RAY_INTERSECTION_BATCH_SIZE_STREAMING) { for (int i = 0; i < numRays; i++) { b3RayData* rayDataStream = (b3RayData*)m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor; rayDataStream[curNumStreamingRays + i].m_rayFromPosition[0] = rayFromWorldArray[i * 3 + 0]; rayDataStream[curNumStreamingRays + i].m_rayFromPosition[1] = rayFromWorldArray[i * 3 + 1]; rayDataStream[curNumStreamingRays + i].m_rayFromPosition[2] = rayFromWorldArray[i * 3 + 2]; rayDataStream[curNumStreamingRays + i].m_rayToPosition[0] = rayToWorldArray[i * 3 + 0]; rayDataStream[curNumStreamingRays + i].m_rayToPosition[1] = rayToWorldArray[i * 3 + 1]; rayDataStream[curNumStreamingRays + i].m_rayToPosition[2] = rayToWorldArray[i * 3 + 2]; command.m_requestRaycastIntersections.m_numStreamingRays++; } } } void PhysicsClientSharedMemory::getCachedCameraImage(struct b3CameraImageData* cameraData) { cameraData->m_pixelWidth = m_data->m_cachedCameraPixelsWidth; cameraData->m_pixelHeight = m_data->m_cachedCameraPixelsHeight; cameraData->m_depthValues = m_data->m_cachedCameraDepthBuffer.size() ? &m_data->m_cachedCameraDepthBuffer[0] : 0; cameraData->m_rgbColorData = m_data->m_cachedCameraPixelsRGBA.size() ? &m_data->m_cachedCameraPixelsRGBA[0] : 0; cameraData->m_segmentationMaskValues = m_data->m_cachedSegmentationMaskBuffer.size() ? &m_data->m_cachedSegmentationMaskBuffer[0] : 0; } void PhysicsClientSharedMemory::getCachedContactPointInformation(struct b3ContactInformation* contactPointData) { contactPointData->m_numContactPoints = m_data->m_cachedContactPoints.size(); contactPointData->m_contactPointData = contactPointData->m_numContactPoints ? &m_data->m_cachedContactPoints[0] : 0; } void PhysicsClientSharedMemory::getCachedOverlappingObjects(struct b3AABBOverlapData* overlappingObjects) { overlappingObjects->m_numOverlappingObjects = m_data->m_cachedOverlappingObjects.size(); overlappingObjects->m_overlappingObjects = m_data->m_cachedOverlappingObjects.size() ? &m_data->m_cachedOverlappingObjects[0] : 0; } void PhysicsClientSharedMemory::getCachedVREvents(struct b3VREventsData* vrEventsData) { vrEventsData->m_numControllerEvents = m_data->m_cachedVREvents.size(); vrEventsData->m_controllerEvents = vrEventsData->m_numControllerEvents ? &m_data->m_cachedVREvents[0] : 0; } void PhysicsClientSharedMemory::getCachedKeyboardEvents(struct b3KeyboardEventsData* keyboardEventsData) { keyboardEventsData->m_numKeyboardEvents = m_data->m_cachedKeyboardEvents.size(); keyboardEventsData->m_keyboardEvents = keyboardEventsData->m_numKeyboardEvents ? &m_data->m_cachedKeyboardEvents[0] : 0; } void PhysicsClientSharedMemory::getCachedMouseEvents(struct b3MouseEventsData* mouseEventsData) { mouseEventsData->m_numMouseEvents = m_data->m_cachedMouseEvents.size(); mouseEventsData->m_mouseEvents = mouseEventsData->m_numMouseEvents ? &m_data->m_cachedMouseEvents[0] : 0; } void PhysicsClientSharedMemory::getCachedRaycastHits(struct b3RaycastInformation* raycastHits) { raycastHits->m_numRayHits = m_data->m_raycastHits.size(); raycastHits->m_rayHits = raycastHits->m_numRayHits ? &m_data->m_raycastHits[0] : 0; } void PhysicsClientSharedMemory::getCachedMassMatrix(int dofCountCheck, double* massMatrix) { int sz = dofCountCheck * dofCountCheck; if (sz == m_data->m_cachedMassMatrix.size()) { for (int i = 0; i < sz; i++) { massMatrix[i] = m_data->m_cachedMassMatrix[i]; } } } bool PhysicsClientSharedMemory::getCachedReturnData(b3UserDataValue* returnData) { if (m_data->m_cachedReturnDataValue.m_length) { *returnData = m_data->m_cachedReturnDataValue; return true; } return false; } void PhysicsClientSharedMemory::getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo) { visualShapesInfo->m_numVisualShapes = m_data->m_cachedVisualShapes.size(); visualShapesInfo->m_visualShapeData = visualShapesInfo->m_numVisualShapes ? &m_data->m_cachedVisualShapes[0] : 0; } void PhysicsClientSharedMemory::getCachedCollisionShapeInformation(struct b3CollisionShapeInformation* collisionShapesInfo) { collisionShapesInfo->m_numCollisionShapes = m_data->m_cachedCollisionShapes.size(); collisionShapesInfo->m_collisionShapeData = collisionShapesInfo->m_numCollisionShapes ? &m_data->m_cachedCollisionShapes[0] : 0; } void PhysicsClientSharedMemory::getCachedMeshData(struct b3MeshData* meshData) { m_data->m_cachedMeshData.m_numVertices = m_data->m_cachedVertexPositions.size(); m_data->m_cachedMeshData.m_vertices = m_data->m_cachedMeshData.m_numVertices ? &m_data->m_cachedVertexPositions[0] : 0; *meshData = m_data->m_cachedMeshData; } const float* PhysicsClientSharedMemory::getDebugLinesFrom() const { if (m_data->m_debugLinesFrom.size()) { return &m_data->m_debugLinesFrom[0].m_x; } return 0; } const float* PhysicsClientSharedMemory::getDebugLinesTo() const { if (m_data->m_debugLinesTo.size()) { return &m_data->m_debugLinesTo[0].m_x; } return 0; } const float* PhysicsClientSharedMemory::getDebugLinesColor() const { if (m_data->m_debugLinesColor.size()) { return &m_data->m_debugLinesColor[0].m_x; } return 0; } int PhysicsClientSharedMemory::getNumDebugLines() const { return m_data->m_debugLinesFrom.size(); } void PhysicsClientSharedMemory::setTimeOut(double timeOutInSeconds) { m_data->m_timeOutInSeconds = timeOutInSeconds; } double PhysicsClientSharedMemory::getTimeOut() const { return m_data->m_timeOutInSeconds; } bool PhysicsClientSharedMemory::getCachedUserData(int userDataId, struct b3UserDataValue& valueOut) const { SharedMemoryUserData* userDataPtr = m_data->m_userDataMap[userDataId]; if (!userDataPtr) { return false; } valueOut.m_type = (userDataPtr)->m_type; valueOut.m_length = userDataPtr->m_bytes.size(); valueOut.m_data1 = userDataPtr->m_bytes.size() ? &userDataPtr->m_bytes[0] : 0; return true; } int PhysicsClientSharedMemory::getCachedUserDataId(int bodyUniqueId, int linkIndex, int visualShapeIndex, const char* key) const { int* userDataId = m_data->m_userDataHandleLookup.find(SharedMemoryUserDataHashKey(key, bodyUniqueId, linkIndex, visualShapeIndex)); if (!userDataId) { return -1; } return *userDataId; } int PhysicsClientSharedMemory::getNumUserData(int bodyUniqueId) const { BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId]; if (!bodyJointsPtr || !(*bodyJointsPtr)) { return 0; } return (*bodyJointsPtr)->m_userDataIds.size(); } void PhysicsClientSharedMemory::getUserDataInfo(int bodyUniqueId, int userDataIndex, const char** keyOut, int* userDataIdOut, int* linkIndexOut, int* visualShapeIndexOut) const { BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId]; if (!bodyJointsPtr || !(*bodyJointsPtr) || userDataIndex < 0 || userDataIndex > (*bodyJointsPtr)->m_userDataIds.size()) { *keyOut = 0; *userDataIdOut = -1; return; } int userDataId = (*bodyJointsPtr)->m_userDataIds[userDataIndex]; SharedMemoryUserData* userData = m_data->m_userDataMap[userDataId]; *userDataIdOut = userDataId; *keyOut = userData->m_key.c_str(); *linkIndexOut = userData->m_linkIndex; *visualShapeIndexOut = userData->m_visualShapeIndex; } void PhysicsClientSharedMemory::pushProfileTiming(const char* timingName) { std::string** strPtr = m_data->m_profileTimingStringArray[timingName]; std::string* str = 0; if (strPtr) { str = *strPtr; } else { str = new std::string(timingName); m_data->m_profileTimingStringArray.insert(timingName, str); } m_data->m_profileTimings.push_back(new CProfileSample(str->c_str())); } void PhysicsClientSharedMemory::popProfileTiming() { if (m_data->m_profileTimings.size()) { CProfileSample* sample = m_data->m_profileTimings[m_data->m_profileTimings.size() - 1]; m_data->m_profileTimings.pop_back(); delete sample; } }