const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
    SharedMemoryStatus* stat = 0;

    if (!m_data->m_testBlock1) {
        return 0;
    }

    if (!m_data->m_waitingForServer) {
        return 0;
    }

    if (m_data->m_testBlock1->m_numServerCommands >
        m_data->m_testBlock1->m_numProcessedServerCommands) {
        btAssert(m_data->m_testBlock1->m_numServerCommands ==
                 m_data->m_testBlock1->m_numProcessedServerCommands + 1);

        const SharedMemoryStatus& serverCmd = m_data->m_testBlock1->m_serverCommands[0];
        m_data->m_lastServerStatus = serverCmd;

        EnumSharedMemoryServerStatus s = (EnumSharedMemoryServerStatus)serverCmd.m_type;
        // consume the command
        switch (serverCmd.m_type) {
            case CMD_CLIENT_COMMAND_COMPLETED: {
                if (m_data->m_verboseOutput) {
                    b3Printf("Server completed command");
                }
                break;
            }
            case CMD_URDF_LOADING_COMPLETED: {
                m_data->m_serverLoadUrdfOK = true;
                if (m_data->m_verboseOutput) {
                    b3Printf("Server loading the URDF OK\n");
                }

                if (serverCmd.m_dataStreamArguments.m_streamChunkLength > 0) {
                    bParse::btBulletFile* bf = new bParse::btBulletFile(
                        this->m_data->m_testBlock1->m_bulletStreamDataServerToClient,
                        serverCmd.m_dataStreamArguments.m_streamChunkLength);
                    bf->setFileDNAisMemoryDNA();
                    bf->parse(false);
                    m_data->m_robotMultiBodyData.push_back(bf);

                    for (int i = 0; i < bf->m_multiBodies.size(); i++) {
                        int flag = bf->getFlags();
                        int qOffset = 7;
                        int uOffset = 6;

                        if ((flag & bParse::FD_DOUBLE_PRECISION) != 0) {
                            Bullet::btMultiBodyDoubleData* mb =
                                (Bullet::btMultiBodyDoubleData*)bf->m_multiBodies[i];
                            if (mb->m_baseName) {
                                if (m_data->m_verboseOutput) {
                                    b3Printf("mb->m_baseName = %s\n", mb->m_baseName);
                                }
                            }

                            for (int link = 0; link < mb->m_numLinks; link++) {
                                {
                                    b3JointInfo info;
                                    info.m_flags = 0;
                                    info.m_jointIndex = link;
                                    info.m_qIndex =
                                        (0 < mb->m_links[link].m_posVarCount) ? qOffset : -1;
                                    info.m_uIndex =
                                        (0 < mb->m_links[link].m_dofCount) ? uOffset : -1;

                                    if (mb->m_links[link].m_linkName) {
                                        if (m_data->m_verboseOutput) {
                                            b3Printf("mb->m_links[%d].m_linkName = %s\n", link,
                                                     mb->m_links[link].m_linkName);
                                        }
                                        info.m_linkName = mb->m_links[link].m_linkName;
                                    }
                                    if (mb->m_links[link].m_jointName) {
                                        if (m_data->m_verboseOutput) {
                                            b3Printf("mb->m_links[%d].m_jointName = %s\n", link,
                                                     mb->m_links[link].m_jointName);
                                        }
                                        info.m_jointName = mb->m_links[link].m_jointName;
                                        info.m_jointType = mb->m_links[link].m_jointType;
                                    }
                                    if ((mb->m_links[link].m_jointType == eRevoluteType) ||
                                        (mb->m_links[link].m_jointType == ePrismaticType)) {
                                        info.m_flags |= JOINT_HAS_MOTORIZED_POWER;
                                    }
                                    m_data->m_jointInfo.push_back(info);
                                }
                                qOffset += mb->m_links[link].m_posVarCount;
                                uOffset += mb->m_links[link].m_dofCount;
                            }

                        } else {
                            Bullet::btMultiBodyFloatData* mb =
                                (Bullet::btMultiBodyFloatData*)bf->m_multiBodies[i];
                            if (mb->m_baseName) {
                                if (m_data->m_verboseOutput) {
                                    b3Printf("mb->m_baseName = %s\n", mb->m_baseName);
                                }
                            }
                            for (int link = 0; link < mb->m_numLinks; link++) {
                                {
                                    b3JointInfo info;
                                    info.m_flags = 0;
                                    info.m_jointIndex = link;
                                    info.m_qIndex =
                                        (0 < mb->m_links[link].m_posVarCount) ? qOffset : -1;
                                    info.m_uIndex =
                                        (0 < mb->m_links[link].m_dofCount) ? uOffset : -1;

                                    if (mb->m_links[link].m_linkName) {
                                        if (m_data->m_verboseOutput) {
                                            b3Printf("mb->m_links[%d].m_linkName = %s\n", link,
                                                     mb->m_links[link].m_linkName);
                                        }
                                        info.m_linkName = mb->m_links[link].m_linkName;
                                    }
                                    if (mb->m_links[link].m_jointName) {
                                        if (m_data->m_verboseOutput) {
                                            b3Printf("mb->m_links[%d].m_jointName = %s\n", link,
                                                     mb->m_links[link].m_jointName);
                                        }
                                        info.m_jointName = mb->m_links[link].m_jointName;
                                        info.m_jointType = mb->m_links[link].m_jointType;
                                    }
                                    if ((mb->m_links[link].m_jointType == eRevoluteType) ||
                                        (mb->m_links[link].m_jointType == ePrismaticType)) {
                                        info.m_flags |= JOINT_HAS_MOTORIZED_POWER;
                                    }
                                    m_data->m_jointInfo.push_back(info);
                                }
                                qOffset += mb->m_links[link].m_posVarCount;
                                uOffset += mb->m_links[link].m_dofCount;
                            }
                        }
                    }
                    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: {
                if (m_data->m_verboseOutput) {
                    b3Printf("Server received desired state");
                }
                break;
            }
            case CMD_STEP_FORWARD_SIMULATION_COMPLETED: {
                if (m_data->m_verboseOutput) {
                    b3Printf("Server completed step simulation");
                }
                break;
            }
            case CMD_URDF_LOADING_FAILED: {
                if (m_data->m_verboseOutput) {
                    b3Printf("Server failed loading the URDF...\n");
                }
                m_data->m_serverLoadUrdfOK = false;
                break;
            }

            case 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: {
                if (m_data->m_verboseOutput) {
                    b3Printf("Server failed receiving bullet data stream\n");
                }

                break;
            }

            case 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;
                if (m_data->m_verboseOutput) {
                    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,
                                    command.m_sendActualStateArgs.m_actualStateQ[i]);
                        } else {
                            sprintf(msg, "%s%f", msg,
                                    command.m_sendActualStateArgs.m_actualStateQ[i]);
                        }
                    }
                    sprintf(msg, "%s]", msg);
                }
                if (m_data->m_verboseOutput) {
                    b3Printf(msg);
                }

                {
                    sprintf(msg, "U=[");

                    for (int i = 0; i < numU; i++) {
                        if (i < numU - 1) {
                            sprintf(msg, "%s%f,", msg,
                                    command.m_sendActualStateArgs.m_actualStateQdot[i]);
                        } else {
                            sprintf(msg, "%s%f", msg,
                                    command.m_sendActualStateArgs.m_actualStateQdot[i]);
                        }
                    }
                    sprintf(msg, "%s]", msg);
                }
                if (m_data->m_verboseOutput) {
                    b3Printf(msg);
                }

                if (m_data->m_verboseOutput) {
                    b3Printf("\n");
                }
                break;
            }
            case CMD_RESET_SIMULATION_COMPLETED: {
                if (m_data->m_verboseOutput) {
                    b3Printf("CMD_RESET_SIMULATION_COMPLETED clean data\n");
                }
                for (int i = 0; i < m_data->m_robotMultiBodyData.size(); i++) {
                    delete m_data->m_robotMultiBodyData[i];
                }
                m_data->m_robotMultiBodyData.clear();

                m_data->m_jointInfo.clear();
                break;
            }
            case 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_bulletStreamDataServerToClient[0];
                float* linesTo =
                    (float*)(&m_data->m_testBlock1->m_bulletStreamDataServerToClient[0] +
                             numLines * 3 * sizeof(float));
                float* linesColor =
                    (float*)(&m_data->m_testBlock1->m_bulletStreamDataServerToClient[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_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;
            }

            default: {
                b3Error("Unknown server status\n");
                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_DEBUG_LINES_COMPLETED) &&
            (serverCmd.m_sendDebugLinesArgs.m_numRemainingDebugLines > 0)) {
            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;
        }

        return &m_data->m_lastServerStatus;

    } else {
        if (m_data->m_verboseOutput) {
            b3Printf("m_numServerStatus  = %d, processed = %d\n",
                     m_data->m_testBlock1->m_numServerCommands,
                     m_data->m_testBlock1->m_numProcessedServerCommands);
        }
    }
    return 0;
}
Exemple #2
0
bool PhysicsDirect::processDebugLines(const struct SharedMemoryCommand& orgCommand)
{
	SharedMemoryCommand command = orgCommand;

	const SharedMemoryStatus& serverCmd = m_data->m_serverStatus;

	do
	{
		bool hasStatus = m_data->m_commandProcessor->processCommand(command, m_data->m_serverStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);

		b3Clock clock;
		double startTime = clock.getTimeInSeconds();
		double timeOutInSeconds = m_data->m_timeOutInSeconds;

		while ((!hasStatus) && (clock.getTimeInSeconds() - startTime < timeOutInSeconds))
		{
			const SharedMemoryStatus* stat = processServerStatus();
			if (stat)
			{
				hasStatus = true;
			}
		}

		m_data->m_hasStatus = hasStatus;

		if (hasStatus)
		{
			btAssert(m_data->m_serverStatus.m_type == 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_bulletStreamDataServerToClient[0];
			float* linesTo =
				(float*)(&m_data->m_bulletStreamDataServerToClient[0] +
						 numLines * 3 * sizeof(float));
			float* linesColor =
				(float*)(&m_data->m_bulletStreamDataServerToClient[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;
			}

			if (serverCmd.m_sendDebugLinesArgs.m_numRemainingDebugLines > 0)
			{
				m_data->m_hasStatus = false;

				command.m_type = CMD_REQUEST_DEBUG_LINES;
				command.m_requestDebugLinesArguments.m_startingLineIndex =
					serverCmd.m_sendDebugLinesArgs.m_numDebugLines +
					serverCmd.m_sendDebugLinesArgs.m_startingLineIndex;
			}
		}

	} while (serverCmd.m_sendDebugLinesArgs.m_numRemainingDebugLines > 0);

	return m_data->m_hasStatus;
}
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) {
        btAssert(m_data->m_testBlock1->m_numServerCommands ==
                 m_data->m_testBlock1->m_numProcessedServerCommands + 1);

        const SharedMemoryStatus& serverCmd = m_data->m_testBlock1->m_serverCommands[0];
        m_data->m_lastServerStatus = serverCmd;

 //       EnumSharedMemoryServerStatus s = (EnumSharedMemoryServerStatus)serverCmd.m_type;
        // consume the command

        switch (serverCmd.m_type) {
            case CMD_CLIENT_COMMAND_COMPLETED: {
                if (m_data->m_verboseOutput) {
                    b3Printf("Server completed command");
                }
                break;
            }

			case CMD_MJCF_LOADING_COMPLETED:
			{
                if (m_data->m_verboseOutput) {
                    b3Printf("Server loading the MJCF OK\n");
                }
                break;
			}
            case CMD_SDF_LOADING_COMPLETED: {
                
                if (m_data->m_verboseOutput) {
                    b3Printf("Server loading the SDF OK\n");
                }

                break;
            }

            case 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);

                    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;
            }
            case CMD_DESIRED_STATE_RECEIVED_COMPLETED: {
                if (m_data->m_verboseOutput) {
                    b3Printf("Server received desired state");
                }
                break;
            }
            case CMD_STEP_FORWARD_SIMULATION_COMPLETED: {
                if (m_data->m_verboseOutput) {
                    b3Printf("Server completed step simulation");
                }
                break;
            }
            case CMD_URDF_LOADING_FAILED: {
                if (m_data->m_verboseOutput) {
                    b3Printf("Server failed loading the URDF...\n");
                }
                
                break;
            }
			case 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:
			{
				int cid = serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId;
				m_data->m_userConstraintInfoMap.insert(cid,serverCmd.m_userConstraintResultArgs);
				break;
			}
			case 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:
			{
				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;
					}
				}
				break;
			}

			case CMD_USER_CONSTRAINT_FAILED:
			{
				b3Warning("createConstraint failed");
				break;
			}
			case CMD_REMOVE_USER_CONSTRAINT_FAILED:
			{
				b3Warning("removeConstraint failed");
				break;
			}
			case CMD_CHANGE_USER_CONSTRAINT_FAILED:
			{
				b3Warning("changeConstraint failed");
				break;
			}
			case CMD_ACTUAL_STATE_UPDATE_FAILED:
			{
				b3Warning("request actual state failed");
				break;
			}
            case 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:
			{
                if (m_data->m_verboseOutput) {
                    b3Printf("Server failed loading the MJCF...\n");
                }
                break;
			}
             case CMD_SDF_LOADING_FAILED: {
                if (m_data->m_verboseOutput) {
                    b3Printf("Server failed loading the SDF...\n");
                }
                
                break;
            }

            case 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: {
                if (m_data->m_verboseOutput) {
                    b3Printf("Server failed receiving bullet data stream\n");
                }

                break;
            }

            case 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;
                if (m_data->m_verboseOutput) {
                    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,
                                    command.m_sendActualStateArgs.m_actualStateQ[i]);
                        } else {
                            sprintf(msg, "%s%f", msg,
                                    command.m_sendActualStateArgs.m_actualStateQ[i]);
                        }
                    }
                    sprintf(msg, "%s]", msg);
                }
                if (m_data->m_verboseOutput) {
                    b3Printf(msg);
                }

                {
                    sprintf(msg, "U=[");

                    for (int i = 0; i < numU; i++) {
                        if (i < numU - 1) {
                            sprintf(msg, "%s%f,", msg,
                                    command.m_sendActualStateArgs.m_actualStateQdot[i]);
                        } else {
                            sprintf(msg, "%s%f", msg,
                                    command.m_sendActualStateArgs.m_actualStateQdot[i]);
                        }
                    }
                    sprintf(msg, "%s]", msg);
                }
                if (m_data->m_verboseOutput) {
                    b3Printf(msg);
                }

                if (m_data->m_verboseOutput) {
                    b3Printf("\n");
                }
                break;
            }
            case CMD_RESET_SIMULATION_COMPLETED: {
                if (m_data->m_verboseOutput) {
                    b3Printf("CMD_RESET_SIMULATION_COMPLETED clean data\n");
                }
				resetData();

                break;
            }
            case 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:
			{

				break;
			}
            case 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:
            {
				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:
            {
                b3Warning("Camera image FAILED\n");
                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:
				{
					if (m_data->m_verboseOutput)
					{
						b3Printf("Raycast completed");
					}
					m_data->m_raycastHits.clear();
					for (int i=0;i<serverCmd.m_raycastHits.m_numRaycastHits;i++)
					{
						m_data->m_raycastHits.push_back(serverCmd.m_raycastHits.m_rayHits[i]);
					}
					break;
				}

			case 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_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:
                {
                    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:
                {
                    b3Warning("Contact Point Information Request failed");
                    break;
                }

			case CMD_SAVE_WORLD_COMPLETED:
				break;
					
			case CMD_SAVE_WORLD_FAILED:
			{
				b3Warning("Saving world  failed");
				break;
			}
			case CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED:
			{
                    break;
                }
            case CMD_CALCULATE_INVERSE_KINEMATICS_FAILED:
                {
                    b3Warning("Calculate Inverse Kinematics Request failed");
                    break;
                }
			case 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:
			{
				break;
			}
			case CMD_STATE_LOGGING_START_COMPLETED:
			{
				break;
			};
			case CMD_STATE_LOGGING_COMPLETED:
			{
				break;
			};

			case CMD_STATE_LOGGING_FAILED:
			{
				b3Warning("State Logging failed");
				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))
        {
			int numConstraints = serverCmd.m_sdfLoadedArgs.m_numUserConstraints;
			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;
                
                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_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)
        {
            //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)
		{
			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)
        {
            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)
		{
			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)
		{
			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_DEBUG_LINES_COMPLETED) &&
            (serverCmd.m_sendDebugLinesArgs.m_numRemainingDebugLines > 0)) {
            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;
        }

        return &m_data->m_lastServerStatus;

    } else {
        if (m_data->m_verboseOutput) {
            b3Printf("m_numServerStatus  = %d, processed = %d\n",
                     m_data->m_testBlock1->m_numServerCommands,
                     m_data->m_testBlock1->m_numProcessedServerCommands);
        }
    }
    return 0;
}
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) {
        btAssert(m_data->m_testBlock1->m_numServerCommands ==
                 m_data->m_testBlock1->m_numProcessedServerCommands + 1);

        const SharedMemoryStatus& serverCmd = m_data->m_testBlock1->m_serverCommands[0];
        m_data->m_lastServerStatus = serverCmd;

        EnumSharedMemoryServerStatus s = (EnumSharedMemoryServerStatus)serverCmd.m_type;
        // consume the command

        switch (serverCmd.m_type) {
            case CMD_CLIENT_COMMAND_COMPLETED: {
                if (m_data->m_verboseOutput) {
                    b3Printf("Server completed command");
                }
                break;
            }
            case CMD_SDF_LOADING_COMPLETED: {
                
                if (m_data->m_verboseOutput) {
                    b3Printf("Server loading the SDF OK\n");
                }

                break;
            }
            case CMD_URDF_LOADING_COMPLETED: {
                
                if (m_data->m_verboseOutput) {
                    b3Printf("Server loading the URDF OK\n");
                }

                if (serverCmd.m_dataStreamArguments.m_streamChunkLength > 0) {
                    bParse::btBulletFile bf(
                        this->m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor,
                        serverCmd.m_dataStreamArguments.m_streamChunkLength);
                    bf.setFileDNAisMemoryDNA();
                    bf.parse(false);
					int bodyUniqueId = serverCmd.m_dataStreamArguments.m_bodyUniqueId;

					BodyJointInfoCache* bodyJoints = new BodyJointInfoCache;
                    m_data->m_bodyJointMap.insert(bodyUniqueId,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;
            }
            case CMD_DESIRED_STATE_RECEIVED_COMPLETED: {
                if (m_data->m_verboseOutput) {
                    b3Printf("Server received desired state");
                }
                break;
            }
            case CMD_STEP_FORWARD_SIMULATION_COMPLETED: {
                if (m_data->m_verboseOutput) {
                    b3Printf("Server completed step simulation");
                }
                break;
            }
            case CMD_URDF_LOADING_FAILED: {
                if (m_data->m_verboseOutput) {
                    b3Printf("Server failed loading the URDF...\n");
                }
                
                break;
            }
            
            case 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_SDF_LOADING_FAILED: {
                if (m_data->m_verboseOutput) {
                    b3Printf("Server failed loading the SDF...\n");
                }
                
                break;
            }

            case 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: {
                if (m_data->m_verboseOutput) {
                    b3Printf("Server failed receiving bullet data stream\n");
                }

                break;
            }

            case 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;
                if (m_data->m_verboseOutput) {
                    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,
                                    command.m_sendActualStateArgs.m_actualStateQ[i]);
                        } else {
                            sprintf(msg, "%s%f", msg,
                                    command.m_sendActualStateArgs.m_actualStateQ[i]);
                        }
                    }
                    sprintf(msg, "%s]", msg);
                }
                if (m_data->m_verboseOutput) {
                    b3Printf(msg);
                }

                {
                    sprintf(msg, "U=[");

                    for (int i = 0; i < numU; i++) {
                        if (i < numU - 1) {
                            sprintf(msg, "%s%f,", msg,
                                    command.m_sendActualStateArgs.m_actualStateQdot[i]);
                        } else {
                            sprintf(msg, "%s%f", msg,
                                    command.m_sendActualStateArgs.m_actualStateQdot[i]);
                        }
                    }
                    sprintf(msg, "%s]", msg);
                }
                if (m_data->m_verboseOutput) {
                    b3Printf(msg);
                }

                if (m_data->m_verboseOutput) {
                    b3Printf("\n");
                }
                break;
            }
            case CMD_RESET_SIMULATION_COMPLETED: {
                if (m_data->m_verboseOutput) {
                    b3Printf("CMD_RESET_SIMULATION_COMPLETED clean data\n");
                }
				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++)
				{
					BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap.getAtIndex(i);
					if (bodyJointsPtr && *bodyJointsPtr)
					{
						BodyJointInfoCache* 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_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:
			{

				break;
			}
            case 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:
            {
				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:
            {
                b3Warning("Camera image FAILED\n");
                break;
            }
			case CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED:
			{
				break;
			}
			case CMD_CALCULATED_INVERSE_DYNAMICS_FAILED:
			{
				b3Warning("Inverse Dynamics computations failed");
				break;
			}
            case 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:
                {
                    b3Warning("Contact Point Information Request failed");
                    break;
                }

			case CMD_SAVE_WORLD_COMPLETED:
				break;
					
			case CMD_SAVE_WORLD_FAILED:
			{
				b3Warning("Saving world  failed");
				break;
			}
			case CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED:
			{
                    break;
                }
            case CMD_CALCULATE_INVERSE_KINEMATICS_FAILED:
                {
                    b3Warning("Calculate Inverse Kinematics Request failed");
                    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)
        {
            int numBodies = serverCmd.m_sdfLoadedArgs.m_numBodies;
            if (numBodies>0)
            {
                m_data->m_tempBackupServerStatus = m_data->m_lastServerStatus;
                
                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_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
            {
                m_data->m_lastServerStatus = m_data->m_tempBackupServerStatus;
            }
        }
        
        if (serverCmd.m_type == CMD_CONTACT_POINT_INFORMATION_COMPLETED)
        {
            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_CAMERA_IMAGE_COMPLETED)
		{
			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_DEBUG_LINES_COMPLETED) &&
            (serverCmd.m_sendDebugLinesArgs.m_numRemainingDebugLines > 0)) {
            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;
        }

        return &m_data->m_lastServerStatus;

    } else {
        if (m_data->m_verboseOutput) {
            b3Printf("m_numServerStatus  = %d, processed = %d\n",
                     m_data->m_testBlock1->m_numServerCommands,
                     m_data->m_testBlock1->m_numProcessedServerCommands);
        }
    }
    return 0;
}
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) {
        btAssert(m_data->m_testBlock1->m_numServerCommands ==
                 m_data->m_testBlock1->m_numProcessedServerCommands + 1);

        const SharedMemoryStatus& serverCmd = m_data->m_testBlock1->m_serverCommands[0];
        m_data->m_lastServerStatus = serverCmd;

        EnumSharedMemoryServerStatus s = (EnumSharedMemoryServerStatus)serverCmd.m_type;
        // consume the command

        switch (serverCmd.m_type) {
            case CMD_CLIENT_COMMAND_COMPLETED: {
                if (m_data->m_verboseOutput) {
                    b3Printf("Server completed command");
                }
                break;
            }
            case CMD_URDF_LOADING_COMPLETED: {
                m_data->m_serverLoadUrdfOK = true;
                if (m_data->m_verboseOutput) {
                    b3Printf("Server loading the URDF OK\n");
                }

                if (serverCmd.m_dataStreamArguments.m_streamChunkLength > 0) {
                    bParse::btBulletFile bf(
                        this->m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor,
                        serverCmd.m_dataStreamArguments.m_streamChunkLength);
                    bf.setFileDNAisMemoryDNA();
                    bf.parse(false);
					int bodyIndex = serverCmd.m_dataStreamArguments.m_bodyUniqueId;

					BodyJointInfoCache* bodyJoints = new BodyJointInfoCache;
                    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;
            }
            case CMD_DESIRED_STATE_RECEIVED_COMPLETED: {
                if (m_data->m_verboseOutput) {
                    b3Printf("Server received desired state");
                }
                break;
            }
            case CMD_STEP_FORWARD_SIMULATION_COMPLETED: {
                if (m_data->m_verboseOutput) {
                    b3Printf("Server completed step simulation");
                }
                break;
            }
            case CMD_URDF_LOADING_FAILED: {
                if (m_data->m_verboseOutput) {
                    b3Printf("Server failed loading the URDF...\n");
                }
                m_data->m_serverLoadUrdfOK = false;
                break;
            }

            case 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: {
                if (m_data->m_verboseOutput) {
                    b3Printf("Server failed receiving bullet data stream\n");
                }

                break;
            }

            case 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;
                if (m_data->m_verboseOutput) {
                    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,
                                    command.m_sendActualStateArgs.m_actualStateQ[i]);
                        } else {
                            sprintf(msg, "%s%f", msg,
                                    command.m_sendActualStateArgs.m_actualStateQ[i]);
                        }
                    }
                    sprintf(msg, "%s]", msg);
                }
                if (m_data->m_verboseOutput) {
                    b3Printf(msg);
                }

                {
                    sprintf(msg, "U=[");

                    for (int i = 0; i < numU; i++) {
                        if (i < numU - 1) {
                            sprintf(msg, "%s%f,", msg,
                                    command.m_sendActualStateArgs.m_actualStateQdot[i]);
                        } else {
                            sprintf(msg, "%s%f", msg,
                                    command.m_sendActualStateArgs.m_actualStateQdot[i]);
                        }
                    }
                    sprintf(msg, "%s]", msg);
                }
                if (m_data->m_verboseOutput) {
                    b3Printf(msg);
                }

                if (m_data->m_verboseOutput) {
                    b3Printf("\n");
                }
                break;
            }
            case CMD_RESET_SIMULATION_COMPLETED: {
                if (m_data->m_verboseOutput) {
                    b3Printf("CMD_RESET_SIMULATION_COMPLETED clean data\n");
                }
				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++)
				{
					BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap.getAtIndex(i);
					if (bodyJointsPtr && *bodyJointsPtr)
					{
						BodyJointInfoCache* 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_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:
			{

				break;
			}
            case 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:
            {
                b3Printf("Camera image OK\n");
                break;
            } 
            
            case CMD_CAMERA_IMAGE_FAILED:
            {
                b3Printf("Camera image FAILED\n");
                break;
            }

            default: {
                b3Error("Unknown server status\n");
                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_DEBUG_LINES_COMPLETED) &&
            (serverCmd.m_sendDebugLinesArgs.m_numRemainingDebugLines > 0)) {
            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;
        }

        return &m_data->m_lastServerStatus;

    } else {
        if (m_data->m_verboseOutput) {
            b3Printf("m_numServerStatus  = %d, processed = %d\n",
                     m_data->m_testBlock1->m_numServerCommands,
                     m_data->m_testBlock1->m_numProcessedServerCommands);
        }
    }
    return 0;
}