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;
}
