bool PhysicsDirect::submitClientCommand(const struct SharedMemoryCommand& command) { if (command.m_type==CMD_REQUEST_DEBUG_LINES) { return processDebugLines(command); } if (command.m_type==CMD_REQUEST_CAMERA_IMAGE_DATA) { return processCamera(command); } if (command.m_type == CMD_REQUEST_CONTACT_POINT_INFORMATION) { return processContactPointData(command); } if (command.m_type == CMD_REQUEST_VISUAL_SHAPE_INFO) { return processVisualShapeData(command); } if (command.m_type == CMD_REQUEST_AABB_OVERLAP) { return processOverlappingObjects(command); } bool hasStatus = m_data->m_commandProcessor->processCommand(command,m_data->m_serverStatus,&m_data->m_bulletStreamDataServerToClient[0],SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); m_data->m_hasStatus = hasStatus; /*if (hasStatus) { postProcessStatus(m_data->m_serverStatus); m_data->m_hasStatus = false; } */ return hasStatus; }
bool PhysicsDirect::submitClientCommand(const struct SharedMemoryCommand& command) { if (command.m_type==CMD_REQUEST_DEBUG_LINES) { return processDebugLines(command); } if (command.m_type==CMD_REQUEST_CAMERA_IMAGE_DATA) { return processCamera(command); } bool hasStatus = m_data->m_commandProcessor->processCommand(command,m_data->m_serverStatus,&m_data->m_bulletStreamDataServerToClient[0],SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); m_data->m_hasStatus = hasStatus; if (hasStatus) { const SharedMemoryStatus& serverCmd = m_data->m_serverStatus; switch (m_data->m_serverStatus.m_type) { case CMD_RESET_SIMULATION_COMPLETED: { m_data->m_debugLinesFrom.clear(); m_data->m_debugLinesTo.clear(); m_data->m_debugLinesColor.clear(); for (int i=0;i<m_data->m_bodyJointMap.size();i++) { BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap.getAtIndex(i); if (bodyJointsPtr && *bodyJointsPtr) { BodyJointInfoCache2* bodyJoints = *bodyJointsPtr; for (int j=0;j<bodyJoints->m_jointInfo.size();j++) { if (bodyJoints->m_jointInfo[j].m_jointName) { free(bodyJoints->m_jointInfo[j].m_jointName); } if (bodyJoints->m_jointInfo[j].m_linkName) { free(bodyJoints->m_jointInfo[j].m_linkName); } } delete (*bodyJointsPtr); } } m_data->m_bodyJointMap.clear(); break; } case CMD_URDF_LOADING_COMPLETED: { if (serverCmd.m_dataStreamArguments.m_streamChunkLength > 0) { bParse::btBulletFile bf( &m_data->m_bulletStreamDataServerToClient[0], serverCmd.m_dataStreamArguments.m_streamChunkLength); bf.setFileDNAisMemoryDNA(); bf.parse(false); int bodyIndex = serverCmd.m_dataStreamArguments.m_bodyUniqueId; BodyJointInfoCache2* bodyJoints = new BodyJointInfoCache2; m_data->m_bodyJointMap.insert(bodyIndex,bodyJoints); 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]; addJointInfoFromMultiBodyData(mb,bodyJoints, m_data->m_verboseOutput); } else { Bullet::btMultiBodyFloatData* mb = (Bullet::btMultiBodyFloatData*)bf.m_multiBodies[i]; 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; } default: { // b3Error("Unknown server status type"); } }; } return hasStatus; }