From a9b59b451137c22335231fcf155b034d2dd4cd7c Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Fri, 17 Nov 2017 16:14:18 -0800 Subject: [PATCH] Finally refactored the PhysicsServerCommandProcessor::processCommand switch statements into over 50 separate functions. Over time, some of those functions can be moved into separate plugins, similar to vrSyncPlugin (either statically/dynamically linked plugins or dynamically loaded at run-time) --- .../PhysicsServerCommandProcessor.cpp | 10862 ++++++++-------- .../PhysicsServerCommandProcessor.h | 55 + 2 files changed, 5655 insertions(+), 5262 deletions(-) diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 0f7652666..44ed1eb88 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -3064,5400 +3064,5738 @@ int PhysicsServerCommandProcessor::createBodyInfoStream(int bodyUniqueId, char* return streamSizeInBytes; } -bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes ) +bool PhysicsServerCommandProcessor::processStateLoggingCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) { -// BT_PROFILE("processCommand"); - bool hasStatus = false; - { - { - if (m_data->m_commandLogger) + BT_PROFILE("CMD_STATE_LOGGING"); + + serverStatusOut.m_type = CMD_STATE_LOGGING_FAILED; + bool hasStatus = true; + + if (clientCmd.m_updateFlags & STATE_LOGGING_START_LOG) + { + if (clientCmd.m_stateLoggingArguments.m_logType == STATE_LOGGING_PROFILE_TIMINGS) + { + if (m_data->m_profileTimingLoggingUid<0) { - m_data->m_commandLogger->logCommand(clientCmd); + b3ChromeUtilsStartTimings(); + m_data->m_profileTimingFileName = clientCmd.m_stateLoggingArguments.m_fileName; + int loggerUid = m_data->m_stateLoggersUniqueId++; + serverStatusOut.m_type = CMD_STATE_LOGGING_START_COMPLETED; + serverStatusOut.m_stateLoggingResultArgs.m_loggingUniqueId = loggerUid; + m_data->m_profileTimingLoggingUid = loggerUid; } - serverStatusOut.m_type = CMD_INVALID_STATUS; - serverStatusOut.m_numDataStreamBytes = 0; - serverStatusOut.m_dataStream = 0; - - //consume the command - switch (clientCmd.m_type) - { - - case CMD_STATE_LOGGING: - { - BT_PROFILE("CMD_STATE_LOGGING"); - - serverStatusOut.m_type = CMD_STATE_LOGGING_FAILED; - hasStatus = true; - - if (clientCmd.m_updateFlags & STATE_LOGGING_START_LOG) - { - if (clientCmd.m_stateLoggingArguments.m_logType == STATE_LOGGING_PROFILE_TIMINGS) - { - if (m_data->m_profileTimingLoggingUid<0) - { - b3ChromeUtilsStartTimings(); - m_data->m_profileTimingFileName = clientCmd.m_stateLoggingArguments.m_fileName; - int loggerUid = m_data->m_stateLoggersUniqueId++; - serverStatusOut.m_type = CMD_STATE_LOGGING_START_COMPLETED; - serverStatusOut.m_stateLoggingResultArgs.m_loggingUniqueId = loggerUid; - m_data->m_profileTimingLoggingUid = loggerUid; - } - } - if (clientCmd.m_stateLoggingArguments.m_logType == STATE_LOGGING_VIDEO_MP4) - { - //if (clientCmd.m_stateLoggingArguments.m_fileName) - { - int loggerUid = m_data->m_stateLoggersUniqueId++; - VideoMP4Loggger* logger = new VideoMP4Loggger(loggerUid,clientCmd.m_stateLoggingArguments.m_fileName,this->m_data->m_guiHelper); - m_data->m_stateLoggers.push_back(logger); - serverStatusOut.m_type = CMD_STATE_LOGGING_START_COMPLETED; - serverStatusOut.m_stateLoggingResultArgs.m_loggingUniqueId = loggerUid; - } - } + } + if (clientCmd.m_stateLoggingArguments.m_logType == STATE_LOGGING_VIDEO_MP4) + { + //if (clientCmd.m_stateLoggingArguments.m_fileName) + { + int loggerUid = m_data->m_stateLoggersUniqueId++; + VideoMP4Loggger* logger = new VideoMP4Loggger(loggerUid,clientCmd.m_stateLoggingArguments.m_fileName,this->m_data->m_guiHelper); + m_data->m_stateLoggers.push_back(logger); + serverStatusOut.m_type = CMD_STATE_LOGGING_START_COMPLETED; + serverStatusOut.m_stateLoggingResultArgs.m_loggingUniqueId = loggerUid; + } + } - if (clientCmd.m_stateLoggingArguments.m_logType == STATE_LOGGING_MINITAUR) - { + if (clientCmd.m_stateLoggingArguments.m_logType == STATE_LOGGING_MINITAUR) + { - std::string fileName = clientCmd.m_stateLoggingArguments.m_fileName; - //either provide the minitaur by object unique Id, or search for first multibody with 8 motors... + std::string fileName = clientCmd.m_stateLoggingArguments.m_fileName; + //either provide the minitaur by object unique Id, or search for first multibody with 8 motors... - if ((clientCmd.m_updateFlags & STATE_LOGGING_FILTER_OBJECT_UNIQUE_ID)&& (clientCmd.m_stateLoggingArguments.m_numBodyUniqueIds>0)) - { - int bodyUniqueId = clientCmd.m_stateLoggingArguments.m_bodyUniqueIds[0]; - InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); - if (body) - { - if (body->m_multiBody) - { - btAlignedObjectArray motorNames; - motorNames.push_back("motor_front_leftR_joint"); - motorNames.push_back("motor_front_leftL_joint"); - motorNames.push_back("motor_back_leftR_joint"); - motorNames.push_back("motor_back_leftL_joint"); - motorNames.push_back("motor_front_rightL_joint"); - motorNames.push_back("motor_front_rightR_joint"); - motorNames.push_back("motor_back_rightL_joint"); - motorNames.push_back("motor_back_rightR_joint"); + if ((clientCmd.m_updateFlags & STATE_LOGGING_FILTER_OBJECT_UNIQUE_ID)&& (clientCmd.m_stateLoggingArguments.m_numBodyUniqueIds>0)) + { + int bodyUniqueId = clientCmd.m_stateLoggingArguments.m_bodyUniqueIds[0]; + InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); + if (body) + { + if (body->m_multiBody) + { + btAlignedObjectArray motorNames; + motorNames.push_back("motor_front_leftR_joint"); + motorNames.push_back("motor_front_leftL_joint"); + motorNames.push_back("motor_back_leftR_joint"); + motorNames.push_back("motor_back_leftL_joint"); + motorNames.push_back("motor_front_rightL_joint"); + motorNames.push_back("motor_front_rightR_joint"); + motorNames.push_back("motor_back_rightL_joint"); + motorNames.push_back("motor_back_rightR_joint"); - btAlignedObjectArray motorIdList; - for (int m=0;mm_multiBody->getNumLinks();i++) - { - std::string jointName; - if (body->m_multiBody->getLink(i).m_jointName) - { - jointName = body->m_multiBody->getLink(i).m_jointName; - } - if (motorNames[m]==jointName) - { - motorIdList.push_back(i); - } - } - } - - if (motorIdList.size()==8) - { - int loggerUid = m_data->m_stateLoggersUniqueId++; - MinitaurStateLogger* logger = new MinitaurStateLogger(loggerUid,fileName,body->m_multiBody, motorIdList); - m_data->m_stateLoggers.push_back(logger); - serverStatusOut.m_type = CMD_STATE_LOGGING_START_COMPLETED; - serverStatusOut.m_stateLoggingResultArgs.m_loggingUniqueId = loggerUid; - } - } + btAlignedObjectArray motorIdList; + for (int m=0;mm_multiBody->getNumLinks();i++) + { + std::string jointName; + if (body->m_multiBody->getLink(i).m_jointName) + { + jointName = body->m_multiBody->getLink(i).m_jointName; + } + if (motorNames[m]==jointName) + { + motorIdList.push_back(i); } } } - if (clientCmd.m_stateLoggingArguments.m_logType == STATE_LOGGING_GENERIC_ROBOT) - { - std::string fileName = clientCmd.m_stateLoggingArguments.m_fileName; - - int loggerUid = m_data->m_stateLoggersUniqueId++; - int maxLogDof = 12; - if ((clientCmd.m_updateFlags & STATE_LOGGING_MAX_LOG_DOF)) - { - maxLogDof = clientCmd.m_stateLoggingArguments.m_maxLogDof; - } - - int logFlags = 0; - if (clientCmd.m_updateFlags & STATE_LOGGING_LOG_FLAGS) - { - logFlags = clientCmd.m_stateLoggingArguments.m_logFlags; - } - GenericRobotStateLogger* logger = new GenericRobotStateLogger(loggerUid,fileName,m_data->m_dynamicsWorld,maxLogDof, logFlags); - - if ((clientCmd.m_updateFlags & STATE_LOGGING_FILTER_OBJECT_UNIQUE_ID) && (clientCmd.m_stateLoggingArguments.m_numBodyUniqueIds>0)) - { - logger->m_filterObjectUniqueId = true; - for (int i = 0; i < clientCmd.m_stateLoggingArguments.m_numBodyUniqueIds; ++i) - { - int objectUniqueId = clientCmd.m_stateLoggingArguments.m_bodyUniqueIds[i]; - logger->m_bodyIdList.push_back(objectUniqueId); - } - } - - m_data->m_stateLoggers.push_back(logger); - serverStatusOut.m_type = CMD_STATE_LOGGING_START_COMPLETED; - serverStatusOut.m_stateLoggingResultArgs.m_loggingUniqueId = loggerUid; - } - if (clientCmd.m_stateLoggingArguments.m_logType == STATE_LOGGING_CONTACT_POINTS) + + if (motorIdList.size()==8) { - std::string fileName = clientCmd.m_stateLoggingArguments.m_fileName; int loggerUid = m_data->m_stateLoggersUniqueId++; - ContactPointsStateLogger* logger = new ContactPointsStateLogger(loggerUid,fileName,m_data->m_dynamicsWorld); - if ((clientCmd.m_updateFlags & STATE_LOGGING_FILTER_LINK_INDEX_A) && clientCmd.m_stateLoggingArguments.m_linkIndexA >= -1) - { - logger->m_filterLinkA = true; - logger->m_linkIndexA = clientCmd.m_stateLoggingArguments.m_linkIndexA; - } - if ((clientCmd.m_updateFlags & STATE_LOGGING_FILTER_LINK_INDEX_B) && clientCmd.m_stateLoggingArguments.m_linkIndexB >= -1) - { - logger->m_filterLinkB = true; - logger->m_linkIndexB = clientCmd.m_stateLoggingArguments.m_linkIndexB; - } - if ((clientCmd.m_updateFlags & STATE_LOGGING_FILTER_BODY_UNIQUE_ID_A) && clientCmd.m_stateLoggingArguments.m_bodyUniqueIdA > -1) - { - logger->m_bodyUniqueIdA = clientCmd.m_stateLoggingArguments.m_bodyUniqueIdA; - } - if ((clientCmd.m_updateFlags & STATE_LOGGING_FILTER_BODY_UNIQUE_ID_B) && clientCmd.m_stateLoggingArguments.m_bodyUniqueIdB > -1) - { - logger->m_bodyUniqueIdB = clientCmd.m_stateLoggingArguments.m_bodyUniqueIdB; - } + MinitaurStateLogger* logger = new MinitaurStateLogger(loggerUid,fileName,body->m_multiBody, motorIdList); m_data->m_stateLoggers.push_back(logger); serverStatusOut.m_type = CMD_STATE_LOGGING_START_COMPLETED; serverStatusOut.m_stateLoggingResultArgs.m_loggingUniqueId = loggerUid; } - if (clientCmd.m_stateLoggingArguments.m_logType ==STATE_LOGGING_VR_CONTROLLERS) - { - std::string fileName = clientCmd.m_stateLoggingArguments.m_fileName; - int loggerUid = m_data->m_stateLoggersUniqueId++; - int deviceFilterType = VR_DEVICE_CONTROLLER; - if (clientCmd.m_updateFlags & STATE_LOGGING_FILTER_DEVICE_TYPE) - { - deviceFilterType = clientCmd.m_stateLoggingArguments.m_deviceFilterType; - } - VRControllerStateLogger* logger = new VRControllerStateLogger(loggerUid,deviceFilterType, fileName); - m_data->m_stateLoggers.push_back(logger); - serverStatusOut.m_type = CMD_STATE_LOGGING_START_COMPLETED; - serverStatusOut.m_stateLoggingResultArgs.m_loggingUniqueId = loggerUid; + } + } + } + } + if (clientCmd.m_stateLoggingArguments.m_logType == STATE_LOGGING_GENERIC_ROBOT) + { + std::string fileName = clientCmd.m_stateLoggingArguments.m_fileName; + + int loggerUid = m_data->m_stateLoggersUniqueId++; + int maxLogDof = 12; + if ((clientCmd.m_updateFlags & STATE_LOGGING_MAX_LOG_DOF)) + { + maxLogDof = clientCmd.m_stateLoggingArguments.m_maxLogDof; + } - } - } - if ((clientCmd.m_updateFlags & STATE_LOGGING_STOP_LOG) && clientCmd.m_stateLoggingArguments.m_loggingUniqueId>=0) - { - if (clientCmd.m_stateLoggingArguments.m_loggingUniqueId == m_data->m_profileTimingLoggingUid) - { - serverStatusOut.m_type = CMD_STATE_LOGGING_COMPLETED; - b3ChromeUtilsStopTimingsAndWriteJsonFile(m_data->m_profileTimingFileName.c_str()); - m_data->m_profileTimingLoggingUid = -1; - } - else - { - serverStatusOut.m_type = CMD_STATE_LOGGING_COMPLETED; - for (int i=0;im_stateLoggers.size();i++) - { - if (m_data->m_stateLoggers[i]->m_loggingUniqueId==clientCmd.m_stateLoggingArguments.m_loggingUniqueId) - { - m_data->m_stateLoggers[i]->stop(); - delete m_data->m_stateLoggers[i]; - m_data->m_stateLoggers.removeAtIndex(i); - } - } - } - } - break; + int logFlags = 0; + if (clientCmd.m_updateFlags & STATE_LOGGING_LOG_FLAGS) + { + logFlags = clientCmd.m_stateLoggingArguments.m_logFlags; + } + GenericRobotStateLogger* logger = new GenericRobotStateLogger(loggerUid,fileName,m_data->m_dynamicsWorld,maxLogDof, logFlags); + + if ((clientCmd.m_updateFlags & STATE_LOGGING_FILTER_OBJECT_UNIQUE_ID) && (clientCmd.m_stateLoggingArguments.m_numBodyUniqueIds>0)) + { + logger->m_filterObjectUniqueId = true; + for (int i = 0; i < clientCmd.m_stateLoggingArguments.m_numBodyUniqueIds; ++i) + { + int objectUniqueId = clientCmd.m_stateLoggingArguments.m_bodyUniqueIds[i]; + logger->m_bodyIdList.push_back(objectUniqueId); + } + } + + m_data->m_stateLoggers.push_back(logger); + serverStatusOut.m_type = CMD_STATE_LOGGING_START_COMPLETED; + serverStatusOut.m_stateLoggingResultArgs.m_loggingUniqueId = loggerUid; + } + if (clientCmd.m_stateLoggingArguments.m_logType == STATE_LOGGING_CONTACT_POINTS) + { + std::string fileName = clientCmd.m_stateLoggingArguments.m_fileName; + int loggerUid = m_data->m_stateLoggersUniqueId++; + ContactPointsStateLogger* logger = new ContactPointsStateLogger(loggerUid,fileName,m_data->m_dynamicsWorld); + if ((clientCmd.m_updateFlags & STATE_LOGGING_FILTER_LINK_INDEX_A) && clientCmd.m_stateLoggingArguments.m_linkIndexA >= -1) + { + logger->m_filterLinkA = true; + logger->m_linkIndexA = clientCmd.m_stateLoggingArguments.m_linkIndexA; + } + if ((clientCmd.m_updateFlags & STATE_LOGGING_FILTER_LINK_INDEX_B) && clientCmd.m_stateLoggingArguments.m_linkIndexB >= -1) + { + logger->m_filterLinkB = true; + logger->m_linkIndexB = clientCmd.m_stateLoggingArguments.m_linkIndexB; + } + if ((clientCmd.m_updateFlags & STATE_LOGGING_FILTER_BODY_UNIQUE_ID_A) && clientCmd.m_stateLoggingArguments.m_bodyUniqueIdA > -1) + { + logger->m_bodyUniqueIdA = clientCmd.m_stateLoggingArguments.m_bodyUniqueIdA; + } + if ((clientCmd.m_updateFlags & STATE_LOGGING_FILTER_BODY_UNIQUE_ID_B) && clientCmd.m_stateLoggingArguments.m_bodyUniqueIdB > -1) + { + logger->m_bodyUniqueIdB = clientCmd.m_stateLoggingArguments.m_bodyUniqueIdB; + } + m_data->m_stateLoggers.push_back(logger); + serverStatusOut.m_type = CMD_STATE_LOGGING_START_COMPLETED; + serverStatusOut.m_stateLoggingResultArgs.m_loggingUniqueId = loggerUid; + } + if (clientCmd.m_stateLoggingArguments.m_logType ==STATE_LOGGING_VR_CONTROLLERS) + { + std::string fileName = clientCmd.m_stateLoggingArguments.m_fileName; + int loggerUid = m_data->m_stateLoggersUniqueId++; + int deviceFilterType = VR_DEVICE_CONTROLLER; + if (clientCmd.m_updateFlags & STATE_LOGGING_FILTER_DEVICE_TYPE) + { + deviceFilterType = clientCmd.m_stateLoggingArguments.m_deviceFilterType; + } + VRControllerStateLogger* logger = new VRControllerStateLogger(loggerUid,deviceFilterType, fileName); + m_data->m_stateLoggers.push_back(logger); + serverStatusOut.m_type = CMD_STATE_LOGGING_START_COMPLETED; + serverStatusOut.m_stateLoggingResultArgs.m_loggingUniqueId = loggerUid; + + } + } + if ((clientCmd.m_updateFlags & STATE_LOGGING_STOP_LOG) && clientCmd.m_stateLoggingArguments.m_loggingUniqueId>=0) + { + if (clientCmd.m_stateLoggingArguments.m_loggingUniqueId == m_data->m_profileTimingLoggingUid) + { + serverStatusOut.m_type = CMD_STATE_LOGGING_COMPLETED; + b3ChromeUtilsStopTimingsAndWriteJsonFile(m_data->m_profileTimingFileName.c_str()); + m_data->m_profileTimingLoggingUid = -1; + } + else + { + serverStatusOut.m_type = CMD_STATE_LOGGING_COMPLETED; + for (int i=0;im_stateLoggers.size();i++) + { + if (m_data->m_stateLoggers[i]->m_loggingUniqueId==clientCmd.m_stateLoggingArguments.m_loggingUniqueId) + { + m_data->m_stateLoggers[i]->stop(); + delete m_data->m_stateLoggers[i]; + m_data->m_stateLoggers.removeAtIndex(i); } - case CMD_SET_VR_CAMERA_STATE: + } + } + } + return hasStatus; +} + +bool PhysicsServerCommandProcessor::processRequestCameraImageCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + BT_PROFILE("CMD_REQUEST_CAMERA_IMAGE_DATA"); + int startPixelIndex = clientCmd.m_requestPixelDataArguments.m_startPixelIndex; + int width = clientCmd.m_requestPixelDataArguments.m_pixelWidth; + int height = clientCmd.m_requestPixelDataArguments.m_pixelHeight; + int numPixelsCopied = 0; + + + + if ((clientCmd.m_requestPixelDataArguments.m_startPixelIndex==0) && + (clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_PIXEL_WIDTH_HEIGHT)!=0) + { + m_data->m_visualConverter.setWidthAndHeight(clientCmd.m_requestPixelDataArguments.m_pixelWidth, + clientCmd.m_requestPixelDataArguments.m_pixelHeight); + } + + + int numTotalPixels = width*height; + int numRemainingPixels = numTotalPixels - startPixelIndex; + + + if (numRemainingPixels>0) + { + int totalBytesPerPixel = 4+4+4;//4 for rgb, 4 for depth, 4 for segmentation mask + int maxNumPixels = bufferSizeInBytes/totalBytesPerPixel-1; + unsigned char* pixelRGBA = (unsigned char*)bufferServerToClient; + int numRequestedPixels = btMin(maxNumPixels,numRemainingPixels); + + float* depthBuffer = (float*)(bufferServerToClient+numRequestedPixels*4); + int* segmentationMaskBuffer = (int*)(bufferServerToClient+numRequestedPixels*8); + + serverStatusOut.m_numDataStreamBytes = numRequestedPixels * totalBytesPerPixel; + float viewMat[16]; + float projMat[16]; + for (int i=0;i<16;i++) + { + viewMat[i] = clientCmd.m_requestPixelDataArguments.m_viewMatrix[i]; + projMat[i] = clientCmd.m_requestPixelDataArguments.m_projectionMatrix[i]; + } + if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES)==0) + { + b3OpenGLVisualizerCameraInfo tmpCamResult; + bool result = this->m_data->m_guiHelper->getCameraInfo( + &tmpCamResult.m_width, + &tmpCamResult.m_height, + tmpCamResult.m_viewMatrix, + tmpCamResult.m_projectionMatrix, + tmpCamResult.m_camUp, + tmpCamResult.m_camForward, + tmpCamResult.m_horizontal, + tmpCamResult.m_vertical, + &tmpCamResult.m_yaw, + &tmpCamResult.m_pitch, + &tmpCamResult.m_dist, + tmpCamResult.m_target); + if (result) + { + for (int i=0;i<16;i++) { - BT_PROFILE("CMD_SET_VR_CAMERA_STATE"); - - if (clientCmd.m_updateFlags & VR_CAMERA_ROOT_POSITION) - { - gVRTeleportPos1[0] = clientCmd.m_vrCameraStateArguments.m_rootPosition[0]; - gVRTeleportPos1[1] = clientCmd.m_vrCameraStateArguments.m_rootPosition[1]; - gVRTeleportPos1[2] = clientCmd.m_vrCameraStateArguments.m_rootPosition[2]; - } - if (clientCmd.m_updateFlags & VR_CAMERA_ROOT_ORIENTATION) - { - gVRTeleportOrn[0] = clientCmd.m_vrCameraStateArguments.m_rootOrientation[0]; - gVRTeleportOrn[1] = clientCmd.m_vrCameraStateArguments.m_rootOrientation[1]; - gVRTeleportOrn[2] = clientCmd.m_vrCameraStateArguments.m_rootOrientation[2]; - gVRTeleportOrn[3] = clientCmd.m_vrCameraStateArguments.m_rootOrientation[3]; - } - - if (clientCmd.m_updateFlags & VR_CAMERA_ROOT_TRACKING_OBJECT) - { - gVRTrackingObjectUniqueId = clientCmd.m_vrCameraStateArguments.m_trackingObjectUniqueId; - } - - if (clientCmd.m_updateFlags & VR_CAMERA_FLAG) - { - gVRTrackingObjectFlag = clientCmd.m_vrCameraStateArguments.m_trackingObjectFlag; - } - - serverStatusOut.m_type = CMD_CLIENT_COMMAND_COMPLETED; - hasStatus = true; - break; + viewMat[i] = tmpCamResult.m_viewMatrix[i]; + projMat[i] = tmpCamResult.m_projectionMatrix[i]; } - case CMD_REQUEST_VR_EVENTS_DATA: - { - //BT_PROFILE("CMD_REQUEST_VR_EVENTS_DATA"); - serverStatusOut.m_sendVREvents.m_numVRControllerEvents = 0; - - for (int i=0;im_vrControllerEvents.m_vrEvents[i]; - - if (clientCmd.m_updateFlags&event.m_deviceType) - { - if (event.m_numButtonEvents + event.m_numMoveEvents) - { - serverStatusOut.m_sendVREvents.m_controllerEvents[serverStatusOut.m_sendVREvents.m_numVRControllerEvents++] = event; - event.m_numButtonEvents = 0; - event.m_numMoveEvents = 0; - for (int b=0;bm_mouseEvents.size(); - if (serverStatusOut.m_sendMouseEvents.m_numMouseEvents>MAX_MOUSE_EVENTS) - { - serverStatusOut.m_sendMouseEvents.m_numMouseEvents = MAX_MOUSE_EVENTS; - } - for (int i=0;im_mouseEvents[i]; - } - - m_data->m_mouseEvents.resize(0); - serverStatusOut.m_type = CMD_REQUEST_MOUSE_EVENTS_DATA_COMPLETED; - hasStatus = true; - break; - }; - - - - case CMD_REQUEST_KEYBOARD_EVENTS_DATA: - { - //BT_PROFILE("CMD_REQUEST_KEYBOARD_EVENTS_DATA"); - - serverStatusOut.m_sendKeyboardEvents.m_numKeyboardEvents = m_data->m_keyboardEvents.size(); - if (serverStatusOut.m_sendKeyboardEvents.m_numKeyboardEvents>MAX_KEYBOARD_EVENTS) - { - serverStatusOut.m_sendKeyboardEvents.m_numKeyboardEvents = MAX_KEYBOARD_EVENTS; - } - for (int i=0;im_keyboardEvents[i]; - } - - btAlignedObjectArray events; - - //remove out-of-date events - for (int i=0;im_keyboardEvents.size();i++) - { - b3KeyboardEvent event = m_data->m_keyboardEvents[i]; - if (event.m_keyState & eButtonIsDown) - { - event.m_keyState = eButtonIsDown; - events.push_back(event); - } - } - m_data->m_keyboardEvents.resize(events.size()); - for (int i=0;im_keyboardEvents[i] = events[i]; - } - - serverStatusOut.m_type = CMD_REQUEST_KEYBOARD_EVENTS_DATA_COMPLETED; - hasStatus = true; - break; - }; - - case CMD_REQUEST_RAY_CAST_INTERSECTIONS: - { - BT_PROFILE("CMD_REQUEST_RAY_CAST_INTERSECTIONS"); - serverStatusOut.m_raycastHits.m_numRaycastHits = 0; - - for (int ray=0;raym_dynamicsWorld->rayTest(rayFromWorld,rayToWorld,rayResultCallback); - int rayHits = serverStatusOut.m_raycastHits.m_numRaycastHits; - - if (rayResultCallback.hasHit()) - { - serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitFraction - = rayResultCallback.m_closestHitFraction; - - int objectUniqueId = -1; - int linkIndex = -1; - - const btRigidBody* body = btRigidBody::upcast(rayResultCallback.m_collisionObject); - if (body) - { - objectUniqueId = rayResultCallback.m_collisionObject->getUserIndex2(); - } else - { - const btMultiBodyLinkCollider* mblB = btMultiBodyLinkCollider::upcast(rayResultCallback.m_collisionObject); - if (mblB && mblB->m_multiBody) - { - linkIndex = mblB->m_link; - objectUniqueId = mblB->m_multiBody->getUserIndex2(); - } - } - - serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitObjectUniqueId - = objectUniqueId; - serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitObjectLinkIndex - = linkIndex; - - serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitPositionWorld[0] - = rayResultCallback.m_hitPointWorld[0]; - serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitPositionWorld[1] - = rayResultCallback.m_hitPointWorld[1]; - serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitPositionWorld[2] - = rayResultCallback.m_hitPointWorld[2]; - - serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitNormalWorld[0] - = rayResultCallback.m_hitNormalWorld[0]; - serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitNormalWorld[1] - = rayResultCallback.m_hitNormalWorld[1]; - serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitNormalWorld[2] - = rayResultCallback.m_hitNormalWorld[2]; - - } else - { - serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitFraction = 1; - serverStatusOut.m_raycastHits.m_rayHits[serverStatusOut.m_raycastHits.m_numRaycastHits].m_hitObjectUniqueId = -1; - serverStatusOut.m_raycastHits.m_rayHits[serverStatusOut.m_raycastHits.m_numRaycastHits].m_hitObjectLinkIndex = -1; - serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitPositionWorld[0] = 0; - serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitPositionWorld[1] = 0; - serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitPositionWorld[2] = 0; - serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitNormalWorld[0] = 0; - serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitNormalWorld[1] = 0; - serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitNormalWorld[2] = 0; - } - serverStatusOut.m_raycastHits.m_numRaycastHits++; - } - serverStatusOut.m_type = CMD_REQUEST_RAY_CAST_INTERSECTIONS_COMPLETED; - hasStatus = true; - break; - }; - case CMD_REQUEST_DEBUG_LINES: - { - BT_PROFILE("CMD_REQUEST_DEBUG_LINES"); - - int curFlags =m_data->m_remoteDebugDrawer->getDebugMode(); - - int debugMode = clientCmd.m_requestDebugLinesArguments.m_debugMode;//clientCmd.btIDebugDraw::DBG_DrawWireframe|btIDebugDraw::DBG_DrawAabb; - int startingLineIndex = clientCmd.m_requestDebugLinesArguments.m_startingLineIndex; - if (startingLineIndex<0) - { - b3Warning("startingLineIndex should be non-negative"); - startingLineIndex = 0; - } - - if (clientCmd.m_requestDebugLinesArguments.m_startingLineIndex==0) - { - m_data->m_remoteDebugDrawer->m_lines2.resize(0); - //|btIDebugDraw::DBG_DrawAabb| - // btIDebugDraw::DBG_DrawConstraints |btIDebugDraw::DBG_DrawConstraintLimits ; - m_data->m_remoteDebugDrawer->setDebugMode(debugMode); - btIDebugDraw* oldDebugDrawer = m_data->m_dynamicsWorld->getDebugDrawer(); - m_data->m_dynamicsWorld->setDebugDrawer(m_data->m_remoteDebugDrawer); - m_data->m_dynamicsWorld->debugDrawWorld(); - m_data->m_dynamicsWorld->setDebugDrawer(oldDebugDrawer); - m_data->m_remoteDebugDrawer->setDebugMode(curFlags); - } - - //9 floats per line: 3 floats for 'from', 3 floats for 'to' and 3 floats for 'color' - int bytesPerLine = (sizeof(float) * 9); - int maxNumLines = bufferSizeInBytes/bytesPerLine-1; - if (startingLineIndex >m_data->m_remoteDebugDrawer->m_lines2.size()) - { - b3Warning("m_startingLineIndex exceeds total number of debug lines"); - startingLineIndex =m_data->m_remoteDebugDrawer->m_lines2.size(); - } - - int numLines = btMin(maxNumLines,m_data->m_remoteDebugDrawer->m_lines2.size()-startingLineIndex); - - if (numLines) - { - - float* linesFrom = (float*)bufferServerToClient; - float* linesTo = (float*)(bufferServerToClient+numLines*3*sizeof(float)); - float* linesColor = (float*)(bufferServerToClient+2*numLines*3*sizeof(float)); - - for (int i=0;im_remoteDebugDrawer->m_lines2[i+startingLineIndex].m_from.x(); - linesTo[i*3] = m_data->m_remoteDebugDrawer->m_lines2[i+startingLineIndex].m_to.x(); - linesColor[i*3] = m_data->m_remoteDebugDrawer->m_lines2[i+startingLineIndex].m_color.x(); - - linesFrom[i*3+1] = m_data->m_remoteDebugDrawer->m_lines2[i+startingLineIndex].m_from.y(); - linesTo[i*3+1] = m_data->m_remoteDebugDrawer->m_lines2[i+startingLineIndex].m_to.y(); - linesColor[i*3+1] = m_data->m_remoteDebugDrawer->m_lines2[i+startingLineIndex].m_color.y(); - - linesFrom[i*3+2] = m_data->m_remoteDebugDrawer->m_lines2[i+startingLineIndex].m_from.z(); - linesTo[i*3+2] = m_data->m_remoteDebugDrawer->m_lines2[i+startingLineIndex].m_to.z(); - linesColor[i*3+2] = m_data->m_remoteDebugDrawer->m_lines2[i+startingLineIndex].m_color.z(); - } - } - - serverStatusOut.m_type = CMD_DEBUG_LINES_COMPLETED; - serverStatusOut.m_numDataStreamBytes = numLines * bytesPerLine; - serverStatusOut.m_sendDebugLinesArgs.m_numDebugLines = numLines; - serverStatusOut.m_sendDebugLinesArgs.m_startingLineIndex = startingLineIndex; - serverStatusOut.m_sendDebugLinesArgs.m_numRemainingDebugLines = m_data->m_remoteDebugDrawer->m_lines2.size()-(startingLineIndex+numLines); - hasStatus = true; - - break; - } - - case CMD_REQUEST_CAMERA_IMAGE_DATA: - { - BT_PROFILE("CMD_REQUEST_CAMERA_IMAGE_DATA"); - int startPixelIndex = clientCmd.m_requestPixelDataArguments.m_startPixelIndex; - int width = clientCmd.m_requestPixelDataArguments.m_pixelWidth; - int height = clientCmd.m_requestPixelDataArguments.m_pixelHeight; - int numPixelsCopied = 0; - - - - if ((clientCmd.m_requestPixelDataArguments.m_startPixelIndex==0) && - (clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_PIXEL_WIDTH_HEIGHT)!=0) - { - m_data->m_visualConverter.setWidthAndHeight(clientCmd.m_requestPixelDataArguments.m_pixelWidth, - clientCmd.m_requestPixelDataArguments.m_pixelHeight); - } - - - int numTotalPixels = width*height; - int numRemainingPixels = numTotalPixels - startPixelIndex; - - - if (numRemainingPixels>0) - { - int totalBytesPerPixel = 4+4+4;//4 for rgb, 4 for depth, 4 for segmentation mask - int maxNumPixels = bufferSizeInBytes/totalBytesPerPixel-1; - unsigned char* pixelRGBA = (unsigned char*)bufferServerToClient; - int numRequestedPixels = btMin(maxNumPixels,numRemainingPixels); - - float* depthBuffer = (float*)(bufferServerToClient+numRequestedPixels*4); - int* segmentationMaskBuffer = (int*)(bufferServerToClient+numRequestedPixels*8); - - serverStatusOut.m_numDataStreamBytes = numRequestedPixels * totalBytesPerPixel; - float viewMat[16]; - float projMat[16]; - for (int i=0;i<16;i++) - { - viewMat[i] = clientCmd.m_requestPixelDataArguments.m_viewMatrix[i]; - projMat[i] = clientCmd.m_requestPixelDataArguments.m_projectionMatrix[i]; - } - if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES)==0) - { - b3OpenGLVisualizerCameraInfo tmpCamResult; - bool result = this->m_data->m_guiHelper->getCameraInfo( - &tmpCamResult.m_width, - &tmpCamResult.m_height, - tmpCamResult.m_viewMatrix, - tmpCamResult.m_projectionMatrix, - tmpCamResult.m_camUp, - tmpCamResult.m_camForward, - tmpCamResult.m_horizontal, - tmpCamResult.m_vertical, - &tmpCamResult.m_yaw, - &tmpCamResult.m_pitch, - &tmpCamResult.m_dist, - tmpCamResult.m_target); - if (result) - { - for (int i=0;i<16;i++) - { - viewMat[i] = tmpCamResult.m_viewMatrix[i]; - projMat[i] = tmpCamResult.m_projectionMatrix[i]; - } - } - } - bool handled = false; + } + } + bool handled = false; - if ((clientCmd.m_updateFlags & ER_BULLET_HARDWARE_OPENGL)!=0) - { + if ((clientCmd.m_updateFlags & ER_BULLET_HARDWARE_OPENGL)!=0) + { - m_data->m_guiHelper->copyCameraImageData(viewMat, - projMat,pixelRGBA,numRequestedPixels, - depthBuffer,numRequestedPixels, - segmentationMaskBuffer, numRequestedPixels, - startPixelIndex,width,height,&numPixelsCopied); + m_data->m_guiHelper->copyCameraImageData(viewMat, + projMat,pixelRGBA,numRequestedPixels, + depthBuffer,numRequestedPixels, + segmentationMaskBuffer, numRequestedPixels, + startPixelIndex,width,height,&numPixelsCopied); - if (numPixelsCopied>0) - { - handled = true; - m_data->m_guiHelper->debugDisplayCameraImageData(viewMat, - projMat,pixelRGBA,numRequestedPixels, - depthBuffer,numRequestedPixels, - 0, numRequestedPixels, - startPixelIndex,width,height,&numPixelsCopied); - } + if (numPixelsCopied>0) + { + handled = true; + m_data->m_guiHelper->debugDisplayCameraImageData(viewMat, + projMat,pixelRGBA,numRequestedPixels, + depthBuffer,numRequestedPixels, + 0, numRequestedPixels, + startPixelIndex,width,height,&numPixelsCopied); + } - } - if (!handled) - { + } + if (!handled) + { - if (clientCmd.m_requestPixelDataArguments.m_startPixelIndex==0) - { - // printf("-------------------------------\nRendering\n"); + if (clientCmd.m_requestPixelDataArguments.m_startPixelIndex==0) + { + // printf("-------------------------------\nRendering\n"); - if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_LIGHT_DIRECTION) != 0) - { - m_data->m_visualConverter.setLightDirection(clientCmd.m_requestPixelDataArguments.m_lightDirection[0], clientCmd.m_requestPixelDataArguments.m_lightDirection[1], clientCmd.m_requestPixelDataArguments.m_lightDirection[2]); - } + if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_LIGHT_DIRECTION) != 0) + { + m_data->m_visualConverter.setLightDirection(clientCmd.m_requestPixelDataArguments.m_lightDirection[0], clientCmd.m_requestPixelDataArguments.m_lightDirection[1], clientCmd.m_requestPixelDataArguments.m_lightDirection[2]); + } - if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_LIGHT_COLOR) != 0) - { - m_data->m_visualConverter.setLightColor(clientCmd.m_requestPixelDataArguments.m_lightColor[0], clientCmd.m_requestPixelDataArguments.m_lightColor[1], clientCmd.m_requestPixelDataArguments.m_lightColor[2]); - } + if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_LIGHT_COLOR) != 0) + { + m_data->m_visualConverter.setLightColor(clientCmd.m_requestPixelDataArguments.m_lightColor[0], clientCmd.m_requestPixelDataArguments.m_lightColor[1], clientCmd.m_requestPixelDataArguments.m_lightColor[2]); + } - if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_LIGHT_DISTANCE) != 0) - { - m_data->m_visualConverter.setLightDistance(clientCmd.m_requestPixelDataArguments.m_lightDistance); - } + if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_LIGHT_DISTANCE) != 0) + { + m_data->m_visualConverter.setLightDistance(clientCmd.m_requestPixelDataArguments.m_lightDistance); + } - if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_SHADOW) != 0) - { - m_data->m_visualConverter.setShadow((clientCmd.m_requestPixelDataArguments.m_hasShadow!=0)); - } + if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_SHADOW) != 0) + { + m_data->m_visualConverter.setShadow((clientCmd.m_requestPixelDataArguments.m_hasShadow!=0)); + } - if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_AMBIENT_COEFF) != 0) - { - m_data->m_visualConverter.setLightAmbientCoeff(clientCmd.m_requestPixelDataArguments.m_lightAmbientCoeff); - } + if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_AMBIENT_COEFF) != 0) + { + m_data->m_visualConverter.setLightAmbientCoeff(clientCmd.m_requestPixelDataArguments.m_lightAmbientCoeff); + } - if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_DIFFUSE_COEFF) != 0) - { - m_data->m_visualConverter.setLightDiffuseCoeff(clientCmd.m_requestPixelDataArguments.m_lightDiffuseCoeff); - } + if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_DIFFUSE_COEFF) != 0) + { + m_data->m_visualConverter.setLightDiffuseCoeff(clientCmd.m_requestPixelDataArguments.m_lightDiffuseCoeff); + } - if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_SPECULAR_COEFF) != 0) - { - m_data->m_visualConverter.setLightSpecularCoeff(clientCmd.m_requestPixelDataArguments.m_lightSpecularCoeff); - } - - if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES)!=0) - { - m_data->m_visualConverter.render( - clientCmd.m_requestPixelDataArguments.m_viewMatrix, - clientCmd.m_requestPixelDataArguments.m_projectionMatrix); - } else - { - b3OpenGLVisualizerCameraInfo tmpCamResult; - bool result = this->m_data->m_guiHelper->getCameraInfo( - &tmpCamResult.m_width, - &tmpCamResult.m_height, - tmpCamResult.m_viewMatrix, - tmpCamResult.m_projectionMatrix, - tmpCamResult.m_camUp, - tmpCamResult.m_camForward, - tmpCamResult.m_horizontal, - tmpCamResult.m_vertical, - &tmpCamResult.m_yaw, - &tmpCamResult.m_pitch, - &tmpCamResult.m_dist, - tmpCamResult.m_target); - if (result) - { - m_data->m_visualConverter.render(tmpCamResult.m_viewMatrix, - tmpCamResult.m_projectionMatrix); - } else - { - m_data->m_visualConverter.render(); - } - } - } - - m_data->m_visualConverter.copyCameraImageData(pixelRGBA,numRequestedPixels, - depthBuffer,numRequestedPixels, - segmentationMaskBuffer, numRequestedPixels, - startPixelIndex,&width,&height,&numPixelsCopied); - - m_data->m_guiHelper->debugDisplayCameraImageData(clientCmd.m_requestPixelDataArguments.m_viewMatrix, - clientCmd.m_requestPixelDataArguments.m_projectionMatrix,pixelRGBA,numRequestedPixels, - depthBuffer,numRequestedPixels, - segmentationMaskBuffer, numRequestedPixels, - startPixelIndex,width,height,&numPixelsCopied); - - } - - //each pixel takes 4 RGBA values and 1 float = 8 bytes - - } else - { - - } - - serverStatusOut.m_type = CMD_CAMERA_IMAGE_COMPLETED; - - serverStatusOut.m_sendPixelDataArguments.m_numPixelsCopied = numPixelsCopied; - serverStatusOut.m_sendPixelDataArguments.m_numRemainingPixels = numRemainingPixels - numPixelsCopied; - serverStatusOut.m_sendPixelDataArguments.m_startingPixelIndex = startPixelIndex; - serverStatusOut.m_sendPixelDataArguments.m_imageWidth = width; - serverStatusOut.m_sendPixelDataArguments.m_imageHeight= height; - hasStatus = true; - - break; + if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_SPECULAR_COEFF) != 0) + { + m_data->m_visualConverter.setLightSpecularCoeff(clientCmd.m_requestPixelDataArguments.m_lightSpecularCoeff); } - case CMD_SYNC_BODY_INFO: + if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES)!=0) { - BT_PROFILE("CMD_SYNC_BODY_INFO"); - - b3AlignedObjectArray usedHandles; - m_data->m_bodyHandles.getUsedHandles(usedHandles); - int actualNumBodies = 0; - for (int i=0;im_visualConverter.render( + clientCmd.m_requestPixelDataArguments.m_viewMatrix, + clientCmd.m_requestPixelDataArguments.m_projectionMatrix); + } else + { + b3OpenGLVisualizerCameraInfo tmpCamResult; + bool result = this->m_data->m_guiHelper->getCameraInfo( + &tmpCamResult.m_width, + &tmpCamResult.m_height, + tmpCamResult.m_viewMatrix, + tmpCamResult.m_projectionMatrix, + tmpCamResult.m_camUp, + tmpCamResult.m_camForward, + tmpCamResult.m_horizontal, + tmpCamResult.m_vertical, + &tmpCamResult.m_yaw, + &tmpCamResult.m_pitch, + &tmpCamResult.m_dist, + tmpCamResult.m_target); + if (result) { - int usedHandle = usedHandles[i]; - InternalBodyData* body = m_data->m_bodyHandles.getHandle(usedHandle); - if (body && (body->m_multiBody || body->m_rigidBody)) - { - serverStatusOut.m_sdfLoadedArgs.m_bodyUniqueIds[actualNumBodies++] = usedHandle; - } - } - serverStatusOut.m_sdfLoadedArgs.m_numBodies = actualNumBodies; - - int usz = m_data->m_userConstraints.size(); - serverStatusOut.m_sdfLoadedArgs.m_numUserConstraints = usz; - for (int i=0;im_visualConverter.render(tmpCamResult.m_viewMatrix, + tmpCamResult.m_projectionMatrix); + } else { - - int key = m_data->m_userConstraints.getKeyAtIndex(i).getUid1(); -// int uid = m_data->m_userConstraints.getAtIndex(i)->m_userConstraintData.m_userConstraintUniqueId; - serverStatusOut.m_sdfLoadedArgs.m_userConstraintUniqueIds[i] = key; + m_data->m_visualConverter.render(); } - - serverStatusOut.m_type = CMD_SYNC_BODY_INFO_COMPLETED; - hasStatus = true; - break; } - case CMD_REQUEST_BODY_INFO: - { - BT_PROFILE("CMD_REQUEST_BODY_INFO"); + } - const SdfRequestInfoArgs& sdfInfoArgs = clientCmd.m_sdfRequestInfoArgs; - //stream info into memory - int streamSizeInBytes = createBodyInfoStream(sdfInfoArgs.m_bodyUniqueId, bufferServerToClient, bufferSizeInBytes); + m_data->m_visualConverter.copyCameraImageData(pixelRGBA,numRequestedPixels, + depthBuffer,numRequestedPixels, + segmentationMaskBuffer, numRequestedPixels, + startPixelIndex,&width,&height,&numPixelsCopied); - serverStatusOut.m_type = CMD_BODY_INFO_COMPLETED; - serverStatusOut.m_dataStreamArguments.m_bodyUniqueId = sdfInfoArgs.m_bodyUniqueId; - serverStatusOut.m_dataStreamArguments.m_bodyName[0] = 0; - - InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(sdfInfoArgs.m_bodyUniqueId); - if (bodyHandle) - { - strcpy(serverStatusOut.m_dataStreamArguments.m_bodyName,bodyHandle->m_bodyName.c_str()); - } + m_data->m_guiHelper->debugDisplayCameraImageData(clientCmd.m_requestPixelDataArguments.m_viewMatrix, + clientCmd.m_requestPixelDataArguments.m_projectionMatrix,pixelRGBA,numRequestedPixels, + depthBuffer,numRequestedPixels, + segmentationMaskBuffer, numRequestedPixels, + startPixelIndex,width,height,&numPixelsCopied); - serverStatusOut.m_numDataStreamBytes = streamSizeInBytes; - - hasStatus = true; - break; - } - case CMD_SAVE_WORLD: - { - BT_PROFILE("CMD_SAVE_WORLD"); + } - ///this is a very rudimentary way to save the state of the world, for scene authoring - ///many todo's, for example save the state of motor controllers etc. + //each pixel takes 4 RGBA values and 1 float = 8 bytes + + } else + { + + } + + serverStatusOut.m_type = CMD_CAMERA_IMAGE_COMPLETED; + serverStatusOut.m_sendPixelDataArguments.m_numPixelsCopied = numPixelsCopied; + serverStatusOut.m_sendPixelDataArguments.m_numRemainingPixels = numRemainingPixels - numPixelsCopied; + serverStatusOut.m_sendPixelDataArguments.m_startingPixelIndex = startPixelIndex; + serverStatusOut.m_sendPixelDataArguments.m_imageWidth = width; + serverStatusOut.m_sendPixelDataArguments.m_imageHeight= height; + return hasStatus; +} + +bool PhysicsServerCommandProcessor::processSaveWorldCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = false; + BT_PROFILE("CMD_SAVE_WORLD"); + serverStatusOut.m_type = CMD_SAVE_WORLD_FAILED; + + ///this is a very rudimentary way to save the state of the world, for scene authoring + ///many todo's, for example save the state of motor controllers etc. + + { + //saveWorld(clientCmd.m_sdfArguments.m_sdfFileName); + int constraintCount = 0; + FILE* f = fopen(clientCmd.m_sdfArguments.m_sdfFileName,"w"); + if (f) + { + char line[1024]; + { + sprintf(line,"import pybullet as p\n"); + int len = strlen(line); + fwrite(line,len,1,f); + } + { + sprintf(line,"cin = p.connect(p.SHARED_MEMORY)\n"); + int len = strlen(line); + fwrite(line,len,1,f); + } + { + sprintf(line,"if (cin < 0):\n"); + int len = strlen(line); + fwrite(line,len,1,f); + } + { + sprintf(line," cin = p.connect(p.GUI)\n"); + int len = strlen(line); + fwrite(line,len,1,f); + } + + //for each objects ... + for (int i=0;im_saveWorldBodyData.size();i++) + { + SaveWorldObjectData& sd = m_data->m_saveWorldBodyData[i]; + + for (int i=0;im_bodyHandles.getHandle(bodyUniqueId); + if (body) { - char line[1024]; - { - sprintf(line,"import pybullet as p\n"); - int len = strlen(line); - fwrite(line,len,1,f); - } - { - sprintf(line,"cin = p.connect(p.SHARED_MEMORY)\n"); - int len = strlen(line); - fwrite(line,len,1,f); - } - { - sprintf(line,"if (cin < 0):\n"); - int len = strlen(line); - fwrite(line,len,1,f); - } - { - sprintf(line," cin = p.connect(p.GUI)\n"); - int len = strlen(line); - fwrite(line,len,1,f); - } - - //for each objects ... - for (int i=0;im_saveWorldBodyData.size();i++) - { - SaveWorldObjectData& sd = m_data->m_saveWorldBodyData[i]; - - for (int i=0;im_multiBody) { - { - int bodyUniqueId = sd.m_bodyUniqueIds[i]; - InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); - if (body) - { - if (body->m_multiBody) - { - btMultiBody* mb = body->m_multiBody; + btMultiBody* mb = body->m_multiBody; - btTransform comTr = mb->getBaseWorldTransform(); - btTransform tr = comTr * body->m_rootLocalInertialFrame.inverse(); + btTransform comTr = mb->getBaseWorldTransform(); + btTransform tr = comTr * body->m_rootLocalInertialFrame.inverse(); - if (strstr(sd.m_fileName.c_str(),".urdf")) - { - sprintf(line,"objects = [p.loadURDF(\"%s\", %f,%f,%f,%f,%f,%f,%f)]\n",sd.m_fileName.c_str(), - tr.getOrigin()[0],tr.getOrigin()[1],tr.getOrigin()[2], - tr.getRotation()[0],tr.getRotation()[1],tr.getRotation()[2],tr.getRotation()[3]); - int len = strlen(line); - fwrite(line,len,1,f); - } - - if (strstr(sd.m_fileName.c_str(),".sdf") && i==0) - { - sprintf(line,"objects = p.loadSDF(\"%s\")\n",sd.m_fileName.c_str()); - int len = strlen(line); - fwrite(line,len,1,f); - } - if (strstr(sd.m_fileName.c_str(),".xml") && i==0) - { - sprintf(line,"objects = p.loadMJCF(\"%s\")\n",sd.m_fileName.c_str()); - int len = strlen(line); - fwrite(line,len,1,f); - } - - if (strstr(sd.m_fileName.c_str(),".sdf") || strstr(sd.m_fileName.c_str(),".xml") || ((strstr(sd.m_fileName.c_str(),".urdf")) && mb->getNumLinks()) ) - { - sprintf(line,"ob = objects[%d]\n",i); - int len = strlen(line); - fwrite(line,len,1,f); - } - - if (strstr(sd.m_fileName.c_str(),".sdf")||strstr(sd.m_fileName.c_str(),".xml")) - { - sprintf(line,"p.resetBasePositionAndOrientation(ob,[%f,%f,%f],[%f,%f,%f,%f])\n", - comTr.getOrigin()[0],comTr.getOrigin()[1],comTr.getOrigin()[2], - comTr.getRotation()[0],comTr.getRotation()[1],comTr.getRotation()[2],comTr.getRotation()[3]); - int len = strlen(line); - fwrite(line,len,1,f); - } - - if (mb->getNumLinks()) - { - { - sprintf(line,"jointPositions=["); - int len = strlen(line); - fwrite(line,len,1,f); - } - - for (int i=0;igetNumLinks();i++) - { - btScalar jointPos = mb->getJointPosMultiDof(i)[0]; - if (igetNumLinks()-1) - { - sprintf(line," %f,",jointPos); - int len = strlen(line); - fwrite(line,len,1,f); - } else - { - sprintf(line," %f ",jointPos); - int len = strlen(line); - fwrite(line,len,1,f); - } - } - - { - sprintf(line,"]\nfor jointIndex in range (p.getNumJoints(ob)):\n\tp.resetJointState(ob,jointIndex,jointPositions[jointIndex])\n\n"); - int len = strlen(line); - fwrite(line,len,1,f); - } - } - } else - { - //todo: btRigidBody/btSoftBody etc case - } - } - } - - } - - //for URDF, load at origin, then reposition... - - - struct SaveWorldObjectData + if (strstr(sd.m_fileName.c_str(),".urdf")) { - b3AlignedObjectArray m_bodyUniqueIds; - std::string m_fileName; - }; - } - - //user constraints - { - for (int i=0;im_userConstraints.size();i++) - { - InteralUserConstraintData* ucptr = m_data->m_userConstraints.getAtIndex(i); - b3UserConstraint& uc = ucptr->m_userConstraintData; - - int parentBodyIndex=uc.m_parentBodyIndex; - int parentJointIndex=uc.m_parentJointIndex; - int childBodyIndex=uc.m_childBodyIndex; - int childJointIndex=uc.m_childJointIndex; - btVector3 jointAxis(uc.m_jointAxis[0],uc.m_jointAxis[1],uc.m_jointAxis[2]); - btVector3 pivotParent(uc.m_parentFrame[0],uc.m_parentFrame[1],uc.m_parentFrame[2]); - btVector3 pivotChild(uc.m_childFrame[0],uc.m_childFrame[1],uc.m_childFrame[2]); - btQuaternion ornFrameParent(uc.m_parentFrame[3],uc.m_parentFrame[4],uc.m_parentFrame[5],uc.m_parentFrame[6]); - btQuaternion ornFrameChild(uc.m_childFrame[3],uc.m_childFrame[4],uc.m_childFrame[5],uc.m_childFrame[6]); - { - char jointTypeStr[1024]="FIXED"; - bool hasKnownJointType = true; - - switch (uc.m_jointType) - { - case eRevoluteType: - { - sprintf(jointTypeStr,"p.JOINT_REVOLUTE"); - break; - } - case ePrismaticType: - { - sprintf(jointTypeStr,"p.JOINT_PRISMATIC"); - break; - } - case eSphericalType: - { - sprintf(jointTypeStr,"p.JOINT_SPHERICAL"); - break; - } - case ePlanarType: - { - sprintf(jointTypeStr,"p.JOINT_PLANAR"); - break; - } - case eFixedType : - { - sprintf(jointTypeStr,"p.JOINT_FIXED"); - break; - } - case ePoint2PointType: - { - sprintf(jointTypeStr,"p.JOINT_POINT2POINT"); - break; - } - case eGearType: - { - sprintf(jointTypeStr,"p.JOINT_GEAR"); - break; - } - default: - { - hasKnownJointType = false; - b3Warning("unknown constraint type in SAVE_WORLD"); - } - }; - if (hasKnownJointType) - { - { - sprintf(line,"cid%d = p.createConstraint(%d,%d,%d,%d,%s,[%f,%f,%f],[%f,%f,%f],[%f,%f,%f],[%f,%f,%f,%f],[%f,%f,%f,%f])\n", - constraintCount, - parentBodyIndex, - parentJointIndex, - childBodyIndex, - childJointIndex, - jointTypeStr, - jointAxis[0],jointAxis[1],jointAxis[2], - pivotParent[0],pivotParent[1],pivotParent[2], - pivotChild[0],pivotChild[1],pivotChild[2], - ornFrameParent[0],ornFrameParent[1],ornFrameParent[2],ornFrameParent[3], - ornFrameChild[0],ornFrameChild[1],ornFrameChild[2],ornFrameChild[3] - ); - int len = strlen(line); - fwrite(line,len,1,f); - } - { - sprintf(line,"p.changeConstraint(cid%d,maxForce=%f)\n",constraintCount,uc.m_maxAppliedForce); - int len = strlen(line); - fwrite(line,len,1,f); - constraintCount++; - } - } - } - } - } - - { - btVector3 grav=this->m_data->m_dynamicsWorld->getGravity(); - sprintf(line,"p.setGravity(%f,%f,%f)\n",grav[0],grav[1],grav[2]); - int len = strlen(line); - fwrite(line,len,1,f); - } - - - { - sprintf(line,"p.stepSimulation()\np.disconnect()\n"); + sprintf(line,"objects = [p.loadURDF(\"%s\", %f,%f,%f,%f,%f,%f,%f)]\n",sd.m_fileName.c_str(), + tr.getOrigin()[0],tr.getOrigin()[1],tr.getOrigin()[2], + tr.getRotation()[0],tr.getRotation()[1],tr.getRotation()[2],tr.getRotation()[3]); int len = strlen(line); fwrite(line,len,1,f); - } - fclose(f); + } + + if (strstr(sd.m_fileName.c_str(),".sdf") && i==0) + { + sprintf(line,"objects = p.loadSDF(\"%s\")\n",sd.m_fileName.c_str()); + int len = strlen(line); + fwrite(line,len,1,f); + } + if (strstr(sd.m_fileName.c_str(),".xml") && i==0) + { + sprintf(line,"objects = p.loadMJCF(\"%s\")\n",sd.m_fileName.c_str()); + int len = strlen(line); + fwrite(line,len,1,f); + } + + if (strstr(sd.m_fileName.c_str(),".sdf") || strstr(sd.m_fileName.c_str(),".xml") || ((strstr(sd.m_fileName.c_str(),".urdf")) && mb->getNumLinks()) ) + { + sprintf(line,"ob = objects[%d]\n",i); + int len = strlen(line); + fwrite(line,len,1,f); + } + + if (strstr(sd.m_fileName.c_str(),".sdf")||strstr(sd.m_fileName.c_str(),".xml")) + { + sprintf(line,"p.resetBasePositionAndOrientation(ob,[%f,%f,%f],[%f,%f,%f,%f])\n", + comTr.getOrigin()[0],comTr.getOrigin()[1],comTr.getOrigin()[2], + comTr.getRotation()[0],comTr.getRotation()[1],comTr.getRotation()[2],comTr.getRotation()[3]); + int len = strlen(line); + fwrite(line,len,1,f); + } + + if (mb->getNumLinks()) + { + { + sprintf(line,"jointPositions=["); + int len = strlen(line); + fwrite(line,len,1,f); + } + + for (int i=0;igetNumLinks();i++) + { + btScalar jointPos = mb->getJointPosMultiDof(i)[0]; + if (igetNumLinks()-1) + { + sprintf(line," %f,",jointPos); + int len = strlen(line); + fwrite(line,len,1,f); + } else + { + sprintf(line," %f ",jointPos); + int len = strlen(line); + fwrite(line,len,1,f); + } + } + + { + sprintf(line,"]\nfor jointIndex in range (p.getNumJoints(ob)):\n\tp.resetJointState(ob,jointIndex,jointPositions[jointIndex])\n\n"); + int len = strlen(line); + fwrite(line,len,1,f); + } + } + } else + { + //todo: btRigidBody/btSoftBody etc case + } } - - - serverStatusOut.m_type = CMD_SAVE_WORLD_COMPLETED; - hasStatus = true; - break; } - serverStatusOut.m_type = CMD_SAVE_WORLD_FAILED; - hasStatus = true; - break; + } - case CMD_LOAD_SDF: - { - BT_PROFILE("CMD_LOAD_SDF"); + + //for URDF, load at origin, then reposition... + - const SdfArgs& sdfArgs = clientCmd.m_sdfArguments; - if (m_data->m_verboseOutput) - { - b3Printf("Processed CMD_LOAD_SDF:%s", sdfArgs.m_sdfFileName); - } - bool useMultiBody=(clientCmd.m_updateFlags & URDF_ARGS_USE_MULTIBODY) ? (sdfArgs.m_useMultiBody!=0) : true; - - int flags = CUF_USE_SDF; //CUF_USE_URDF_INERTIA - btScalar globalScaling = 1.f; - if (clientCmd.m_updateFlags & URDF_ARGS_USE_GLOBAL_SCALING) - { - globalScaling = sdfArgs.m_globalScaling; - } - bool completedOk = loadSdf(sdfArgs.m_sdfFileName,bufferServerToClient, bufferSizeInBytes, useMultiBody, flags, globalScaling); - if (completedOk) - { - m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld); - - //serverStatusOut.m_type = CMD_SDF_LOADING_FAILED; - serverStatusOut.m_sdfLoadedArgs.m_numBodies = m_data->m_sdfRecentLoadedBodies.size(); - serverStatusOut.m_sdfLoadedArgs.m_numUserConstraints = 0; - int maxBodies = btMin(MAX_SDF_BODIES, serverStatusOut.m_sdfLoadedArgs.m_numBodies); - for (int i=0;im_sdfRecentLoadedBodies[i]; - } - - serverStatusOut.m_type = CMD_SDF_LOADING_COMPLETED; - } else - { - serverStatusOut.m_type = CMD_SDF_LOADING_FAILED; - } - hasStatus = true; - break; - } - case CMD_CREATE_COLLISION_SHAPE: + struct SaveWorldObjectData { - hasStatus = true; - serverStatusOut.m_type = CMD_CREATE_COLLISION_SHAPE_FAILED; - - btBulletWorldImporter* worldImporter = new btBulletWorldImporter(m_data->m_dynamicsWorld); + b3AlignedObjectArray m_bodyUniqueIds; + std::string m_fileName; + }; + } - btCollisionShape* shape = 0; - b3AlignedObjectArray urdfCollisionObjects; + //user constraints + { + for (int i=0;im_userConstraints.size();i++) + { + InteralUserConstraintData* ucptr = m_data->m_userConstraints.getAtIndex(i); + b3UserConstraint& uc = ucptr->m_userConstraintData; - btCompoundShape* compound = 0; - - if (clientCmd.m_createUserShapeArgs.m_numUserShapes>1) + int parentBodyIndex=uc.m_parentBodyIndex; + int parentJointIndex=uc.m_parentJointIndex; + int childBodyIndex=uc.m_childBodyIndex; + int childJointIndex=uc.m_childJointIndex; + btVector3 jointAxis(uc.m_jointAxis[0],uc.m_jointAxis[1],uc.m_jointAxis[2]); + btVector3 pivotParent(uc.m_parentFrame[0],uc.m_parentFrame[1],uc.m_parentFrame[2]); + btVector3 pivotChild(uc.m_childFrame[0],uc.m_childFrame[1],uc.m_childFrame[2]); + btQuaternion ornFrameParent(uc.m_parentFrame[3],uc.m_parentFrame[4],uc.m_parentFrame[5],uc.m_parentFrame[6]); + btQuaternion ornFrameChild(uc.m_childFrame[3],uc.m_childFrame[4],uc.m_childFrame[5],uc.m_childFrame[6]); { - compound = worldImporter->createCompoundShape(); - } - for (int i=0;im_data->m_dynamicsWorld->getGravity(); + sprintf(line,"p.setGravity(%f,%f,%f)\n",grav[0],grav[1],grav[2]); + int len = strlen(line); + fwrite(line,len,1,f); + } + + + { + sprintf(line,"p.stepSimulation()\np.disconnect()\n"); + int len = strlen(line); + fwrite(line,len,1,f); + } + fclose(f); + } + + + serverStatusOut.m_type = CMD_SAVE_WORLD_COMPLETED; + hasStatus = true; + } + + return hasStatus; +} + +bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + serverStatusOut.m_type = CMD_CREATE_COLLISION_SHAPE_FAILED; + + btBulletWorldImporter* worldImporter = new btBulletWorldImporter(m_data->m_dynamicsWorld); + + btCollisionShape* shape = 0; + b3AlignedObjectArray urdfCollisionObjects; + + btCompoundShape* compound = 0; + + if (clientCmd.m_createUserShapeArgs.m_numUserShapes>1) + { + compound = worldImporter->createCompoundShape(); + } + for (int i=0;icreateCompoundShape(); + } + } + + urdfColObj.m_linkLocalFrame = childTransform; + urdfColObj.m_sourceFileLocation = "memory"; + urdfColObj.m_name = "memory"; + urdfColObj.m_geometry.m_type = URDF_GEOM_UNKNOWN; + + switch (clientCmd.m_createUserShapeArgs.m_shapes[i].m_type) + { + case GEOM_SPHERE: + { + double radius = clientCmd.m_createUserShapeArgs.m_shapes[i].m_sphereRadius; + shape = worldImporter->createSphereShape(radius); + if (compound) + { + compound->addChildShape(childTransform,shape); + } + urdfColObj.m_geometry.m_type = URDF_GEOM_SPHERE; + urdfColObj.m_geometry.m_sphereRadius = radius; + break; + } + case GEOM_BOX: + { + //double halfExtents[3] = clientCmd.m_createUserShapeArgs.m_shapes[i].m_sphereRadius; + btVector3 halfExtents( + clientCmd.m_createUserShapeArgs.m_shapes[i].m_boxHalfExtents[0], + clientCmd.m_createUserShapeArgs.m_shapes[i].m_boxHalfExtents[1], + clientCmd.m_createUserShapeArgs.m_shapes[i].m_boxHalfExtents[2]); + shape = worldImporter->createBoxShape(halfExtents); + if (compound) + { + compound->addChildShape(childTransform,shape); + } + urdfColObj.m_geometry.m_type = URDF_GEOM_BOX; + urdfColObj.m_geometry.m_boxSize = 2.*halfExtents; + break; + } + case GEOM_CAPSULE: + { + shape = worldImporter->createCapsuleShapeZ(clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleRadius, + clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleHeight); + if (compound) + { + compound->addChildShape(childTransform,shape); + } + urdfColObj.m_geometry.m_type = URDF_GEOM_CAPSULE; + urdfColObj.m_geometry.m_capsuleRadius = clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleRadius; + urdfColObj.m_geometry.m_capsuleHeight = clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleHeight; + + break; + } + case GEOM_CYLINDER: + { + shape = worldImporter->createCylinderShapeZ(clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleRadius, + clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleHeight); + if (compound) + { + compound->addChildShape(childTransform,shape); + } + urdfColObj.m_geometry.m_type = URDF_GEOM_CYLINDER; + urdfColObj.m_geometry.m_capsuleRadius = clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleRadius; + urdfColObj.m_geometry.m_capsuleHeight = clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleHeight; + + break; + } + case GEOM_PLANE: + { + btVector3 planeNormal(clientCmd.m_createUserShapeArgs.m_shapes[i].m_planeNormal[0], + clientCmd.m_createUserShapeArgs.m_shapes[i].m_planeNormal[1], + clientCmd.m_createUserShapeArgs.m_shapes[i].m_planeNormal[2]); + + shape = worldImporter->createPlaneShape(planeNormal,0); + if (compound) + { + compound->addChildShape(childTransform,shape); + } + urdfColObj.m_geometry.m_type = URDF_GEOM_PLANE; + urdfColObj.m_geometry.m_planeNormal.setValue( + clientCmd.m_createUserShapeArgs.m_shapes[i].m_planeNormal[0], + clientCmd.m_createUserShapeArgs.m_shapes[i].m_planeNormal[1], + clientCmd.m_createUserShapeArgs.m_shapes[i].m_planeNormal[2]); + + break; + } + case GEOM_MESH: + { + btScalar defaultCollisionMargin = 0.001; + + btVector3 meshScale(clientCmd.m_createUserShapeArgs.m_shapes[i].m_meshScale[0], + clientCmd.m_createUserShapeArgs.m_shapes[i].m_meshScale[1], + clientCmd.m_createUserShapeArgs.m_shapes[i].m_meshScale[2]); + + const std::string& urdf_path=""; + + std::string fileName = clientCmd.m_createUserShapeArgs.m_shapes[i].m_meshFileName; + urdfColObj.m_geometry.m_type = URDF_GEOM_MESH; + urdfColObj.m_geometry.m_meshFileName = fileName; + + urdfColObj.m_geometry.m_meshScale = meshScale; + char relativeFileName[1024]; + char pathPrefix[1024]; + pathPrefix[0] = 0; + if (b3ResourcePath::findResourcePath(fileName.c_str(), relativeFileName, 1024)) + { + + b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024); + } + + const std::string& error_message_prefix=""; + std::string out_found_filename; + int out_type; + + bool foundFile = findExistingMeshFile(pathPrefix, relativeFileName,error_message_prefix,&out_found_filename, &out_type); + if (foundFile) + { + urdfColObj.m_geometry.m_meshFileType = out_type; + + if (out_type==UrdfGeometry::FILE_OBJ) + { + //create a convex hull for each shape, and store it in a btCompoundShape + + if (clientCmd.m_createUserShapeArgs.m_shapes[i].m_collisionFlags&GEOM_FORCE_CONCAVE_TRIMESH) + { + GLInstanceGraphicsShape* glmesh = LoadMeshFromObj(relativeFileName, pathPrefix); + + if (!glmesh || glmesh->m_numvertices<=0) + { + b3Warning("%s: cannot extract mesh from '%s'\n", pathPrefix, relativeFileName); + delete glmesh; + break; + } + btAlignedObjectArray convertedVerts; + convertedVerts.reserve(glmesh->m_numvertices); + + for (int i=0; im_numvertices; i++) + { + convertedVerts.push_back(btVector3( + glmesh->m_vertices->at(i).xyzw[0]*meshScale[0], + glmesh->m_vertices->at(i).xyzw[1]*meshScale[1], + glmesh->m_vertices->at(i).xyzw[2]*meshScale[2])); + } + + BT_PROFILE("convert trimesh"); + btTriangleMesh* meshInterface = new btTriangleMesh(); + this->m_data->m_meshInterfaces.push_back(meshInterface); + { + BT_PROFILE("convert vertices"); + + for (int i=0; im_numIndices/3; i++) + { + const btVector3& v0 = convertedVerts[glmesh->m_indices->at(i*3)]; + const btVector3& v1 = convertedVerts[glmesh->m_indices->at(i*3+1)]; + const btVector3& v2 = convertedVerts[glmesh->m_indices->at(i*3+2)]; + meshInterface->addTriangle(v0,v1,v2); + } + } + { + BT_PROFILE("create btBvhTriangleMeshShape"); + btBvhTriangleMeshShape* trimesh = new btBvhTriangleMeshShape(meshInterface,true,true); + m_data->m_collisionShapes.push_back(trimesh); + //trimesh->setLocalScaling(collision->m_geometry.m_meshScale); + shape = trimesh; + if (compound) + { + compound->addChildShape(childTransform,shape); + } + } + delete glmesh; + } else + { + + std::vector shapes; + std::string err = tinyobj::LoadObj(shapes,out_found_filename.c_str()); + + //shape = createConvexHullFromShapes(shapes, collision->m_geometry.m_meshScale); + //static btCollisionShape* createConvexHullFromShapes(std::vector& shapes, const btVector3& geomScale) + B3_PROFILE("createConvexHullFromShapes"); if (compound==0) { compound = worldImporter->createCompoundShape(); } + compound->setMargin(defaultCollisionMargin); + + for (int s = 0; s<(int)shapes.size(); s++) + { + btConvexHullShape* convexHull = worldImporter->createConvexHullShape(); + convexHull->setMargin(defaultCollisionMargin); + tinyobj::shape_t& shape = shapes[s]; + int faceCount = shape.mesh.indices.size(); + + for (int f = 0; faddPoint(pt*meshScale,false); + + pt.setValue(shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 0], + shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 1], + shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 2]); + convexHull->addPoint(pt*meshScale, false); + + pt.setValue(shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 0], + shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 1], + shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 2]); + convexHull->addPoint(pt*meshScale, false); + } + + convexHull->recalcLocalAabb(); + convexHull->optimizeConvexHull(); + compound->addChildShape(childTransform,convexHull); + } } + } + } + break; + } + default: + { + } + } + if (urdfColObj.m_geometry.m_type != URDF_GEOM_UNKNOWN) + { + urdfCollisionObjects.push_back(urdfColObj); + } + } - urdfColObj.m_linkLocalFrame = childTransform; - urdfColObj.m_sourceFileLocation = "memory"; - urdfColObj.m_name = "memory"; - urdfColObj.m_geometry.m_type = URDF_GEOM_UNKNOWN; + if (compound && compound->getNumChildShapes()) + { + shape = compound; + } - switch (clientCmd.m_createUserShapeArgs.m_shapes[i].m_type) - { - case GEOM_SPHERE: - { - double radius = clientCmd.m_createUserShapeArgs.m_shapes[i].m_sphereRadius; - shape = worldImporter->createSphereShape(radius); - if (compound) - { - compound->addChildShape(childTransform,shape); - } - urdfColObj.m_geometry.m_type = URDF_GEOM_SPHERE; - urdfColObj.m_geometry.m_sphereRadius = radius; - break; - } - case GEOM_BOX: - { - //double halfExtents[3] = clientCmd.m_createUserShapeArgs.m_shapes[i].m_sphereRadius; - btVector3 halfExtents( - clientCmd.m_createUserShapeArgs.m_shapes[i].m_boxHalfExtents[0], - clientCmd.m_createUserShapeArgs.m_shapes[i].m_boxHalfExtents[1], - clientCmd.m_createUserShapeArgs.m_shapes[i].m_boxHalfExtents[2]); - shape = worldImporter->createBoxShape(halfExtents); - if (compound) - { - compound->addChildShape(childTransform,shape); - } - urdfColObj.m_geometry.m_type = URDF_GEOM_BOX; - urdfColObj.m_geometry.m_boxSize = 2.*halfExtents; - break; - } - case GEOM_CAPSULE: - { - shape = worldImporter->createCapsuleShapeZ(clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleRadius, - clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleHeight); - if (compound) - { - compound->addChildShape(childTransform,shape); - } - urdfColObj.m_geometry.m_type = URDF_GEOM_CAPSULE; - urdfColObj.m_geometry.m_capsuleRadius = clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleRadius; - urdfColObj.m_geometry.m_capsuleHeight = clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleHeight; + if (shape) + { + int collisionShapeUid = m_data->m_userCollisionShapeHandles.allocHandle(); + InternalCollisionShapeHandle* handle = m_data->m_userCollisionShapeHandles.getHandle(collisionShapeUid); + handle->m_collisionShape = shape; + for (int i=0;im_urdfCollisionObjects.push_back(urdfCollisionObjects[i]); + } + serverStatusOut.m_createUserShapeResultArgs.m_userShapeUniqueId = collisionShapeUid; + m_data->m_worldImporters.push_back(worldImporter); + serverStatusOut.m_type = CMD_CREATE_COLLISION_SHAPE_COMPLETED; + } else + { + delete worldImporter; + } - break; - } - case GEOM_CYLINDER: - { - shape = worldImporter->createCylinderShapeZ(clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleRadius, - clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleHeight); - if (compound) - { - compound->addChildShape(childTransform,shape); - } - urdfColObj.m_geometry.m_type = URDF_GEOM_CYLINDER; - urdfColObj.m_geometry.m_capsuleRadius = clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleRadius; - urdfColObj.m_geometry.m_capsuleHeight = clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleHeight; + return hasStatus; +} - break; - } - case GEOM_PLANE: - { - btVector3 planeNormal(clientCmd.m_createUserShapeArgs.m_shapes[i].m_planeNormal[0], - clientCmd.m_createUserShapeArgs.m_shapes[i].m_planeNormal[1], - clientCmd.m_createUserShapeArgs.m_shapes[i].m_planeNormal[2]); +bool PhysicsServerCommandProcessor::processCreateVisualShapeCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + serverStatusOut.m_type = CMD_CREATE_VISUAL_SHAPE_FAILED; + double globalScaling = 1.f; - shape = worldImporter->createPlaneShape(planeNormal,0); - if (compound) - { - compound->addChildShape(childTransform,shape); - } - urdfColObj.m_geometry.m_type = URDF_GEOM_PLANE; - urdfColObj.m_geometry.m_planeNormal.setValue( - clientCmd.m_createUserShapeArgs.m_shapes[i].m_planeNormal[0], - clientCmd.m_createUserShapeArgs.m_shapes[i].m_planeNormal[1], - clientCmd.m_createUserShapeArgs.m_shapes[i].m_planeNormal[2]); - - break; - } - case GEOM_MESH: - { - btScalar defaultCollisionMargin = 0.001; + BulletURDFImporter u2b(m_data->m_guiHelper, &m_data->m_visualConverter, globalScaling); + u2b.setEnableTinyRenderer(m_data->m_enableTinyRenderer); + btTransform localInertiaFrame; + localInertiaFrame.setIdentity(); + btTransform childTrans; + childTrans.setIdentity(); + const char* pathPrefix = ""; + if (clientCmd.m_createUserShapeArgs.m_numUserShapes == 1) + { + int userShapeIndex = 0; - btVector3 meshScale(clientCmd.m_createUserShapeArgs.m_shapes[i].m_meshScale[0], - clientCmd.m_createUserShapeArgs.m_shapes[i].m_meshScale[1], - clientCmd.m_createUserShapeArgs.m_shapes[i].m_meshScale[2]); + UrdfVisual visualShape; + visualShape.m_geometry.m_type = (UrdfGeomTypes)clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_type; + char relativeFileName[1024]; + char pathPrefix[1024]; + pathPrefix[0] = 0; - const std::string& urdf_path=""; - - std::string fileName = clientCmd.m_createUserShapeArgs.m_shapes[i].m_meshFileName; - urdfColObj.m_geometry.m_type = URDF_GEOM_MESH; - urdfColObj.m_geometry.m_meshFileName = fileName; - - urdfColObj.m_geometry.m_meshScale = meshScale; - char relativeFileName[1024]; - char pathPrefix[1024]; - pathPrefix[0] = 0; - if (b3ResourcePath::findResourcePath(fileName.c_str(), relativeFileName, 1024)) - { - - b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024); - } + if (visualShape.m_geometry.m_type == URDF_GEOM_MESH) + { - const std::string& error_message_prefix=""; - std::string out_found_filename; - int out_type; + std::string fileName = clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_meshFileName; + const std::string& error_message_prefix=""; + std::string out_found_filename; + int out_type; + if (b3ResourcePath::findResourcePath(fileName.c_str(), relativeFileName, 1024)) + { + b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024); + } + + bool foundFile = findExistingMeshFile(pathPrefix, relativeFileName,error_message_prefix,&out_found_filename, &out_type); + visualShape.m_geometry.m_meshFileType = out_type; + visualShape.m_geometry.m_meshFileName=fileName; - bool foundFile = findExistingMeshFile(pathPrefix, relativeFileName,error_message_prefix,&out_found_filename, &out_type); - if (foundFile) + visualShape.m_geometry.m_meshScale.setValue(clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_meshScale[0], + clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_meshScale[1], + clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_meshScale[2]); + } + visualShape.m_name = "bla"; + visualShape.m_materialName=""; + visualShape.m_sourceFileLocation="blaat_line_10"; + visualShape.m_linkLocalFrame.setIdentity(); + visualShape.m_geometry.m_hasLocalMaterial = false; + + + btAlignedObjectArray vertices; + btAlignedObjectArray indices; + btTransform startTrans; startTrans.setIdentity(); + btAlignedObjectArray textures; + bool hasRGBA = (clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_visualFlags&GEOM_VISUAL_HAS_RGBA_COLOR)!=0;; + bool hasSpecular = (clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_visualFlags&GEOM_VISUAL_HAS_SPECULAR_COLOR)!=0;; + visualShape.m_geometry.m_hasLocalMaterial = hasRGBA|hasSpecular; + if (visualShape.m_geometry.m_hasLocalMaterial) + { + if (hasRGBA) + { + visualShape.m_geometry.m_localMaterial.m_matColor.m_rgbaColor.setValue( + clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_rgbaColor[0], + clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_rgbaColor[1], + clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_rgbaColor[2], + clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_rgbaColor[3]); + } else + { + + } + if (hasSpecular) + { + visualShape.m_geometry.m_localMaterial.m_matColor.m_specularColor.setValue( + clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_specularColor[0], + clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_specularColor[1], + clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_specularColor[2]); + } else + { + visualShape.m_geometry.m_localMaterial.m_matColor.m_specularColor.setValue(0.4,0.4,0.4); + } + } + + if (clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_hasChildTransform !=0) + { + childTrans.setOrigin(btVector3(clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_childPosition[0], + clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_childPosition[1], + clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_childPosition[2])); + childTrans.setRotation(btQuaternion( + clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_childOrientation[0], + clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_childOrientation[1], + clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_childOrientation[2], + clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_childOrientation[3])); + } + + + + u2b.convertURDFToVisualShapeInternal(&visualShape, pathPrefix, localInertiaFrame.inverse()*childTrans, vertices, indices,textures); + + if (vertices.size() && indices.size()) + { + if (1) + { + int textureIndex = -1; + if (textures.size()) + { + + textureIndex = m_data->m_guiHelper->registerTexture(textures[0].textureData1,textures[0].m_width,textures[0].m_height); + } + int graphicsIndex = -1; + { + B3_PROFILE("registerGraphicsShape"); + graphicsIndex = m_data->m_guiHelper->registerGraphicsShape(&vertices[0].xyzw[0], vertices.size(), &indices[0], indices.size(), B3_GL_TRIANGLES, textureIndex); + if (graphicsIndex>=0) + { + int visualShapeUniqueId = m_data->m_userVisualShapeHandles.allocHandle(); + InternalVisualShapeHandle* visualHandle = m_data->m_userVisualShapeHandles.getHandle(visualShapeUniqueId); + visualHandle->m_OpenGLGraphicsIndex = graphicsIndex; + visualHandle->m_tinyRendererVisualShapeIndex = -1; + //tinyrenderer doesn't separate shape versus instance, so create it when creating the multibody instance + //store needed info for tinyrenderer + visualHandle->m_localInertiaFrame = localInertiaFrame; + visualHandle->m_visualShape = visualShape; + visualHandle->m_pathPrefix = pathPrefix[0] ? pathPrefix : ""; + + serverStatusOut.m_createUserShapeResultArgs.m_userShapeUniqueId = visualShapeUniqueId; + serverStatusOut.m_type = CMD_CREATE_VISUAL_SHAPE_COMPLETED; + } + } + } + } + } + return hasStatus; +} + +bool PhysicsServerCommandProcessor::processCustomCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + + SharedMemoryStatus& serverCmd = serverStatusOut; + serverCmd.m_type = CMD_CUSTOM_COMMAND_FAILED; + serverCmd.m_customCommandResultArgs.m_pluginUniqueId = -1; + + if (clientCmd.m_updateFlags & CMD_CUSTOM_COMMAND_LOAD_PLUGIN) + { + //pluginPath could be registered or load from disk + const char* postFix = ""; + if (clientCmd.m_updateFlags & CMD_CUSTOM_COMMAND_LOAD_PLUGIN_POSTFIX) + { + postFix = clientCmd.m_customCommandArgs.m_postFix; + } + + int pluginUniqueId = m_data->m_pluginManager.loadPlugin(clientCmd.m_customCommandArgs.m_pluginPath, postFix); + if (pluginUniqueId>=0) + { + serverCmd.m_customCommandResultArgs.m_pluginUniqueId = pluginUniqueId; + serverCmd.m_type = CMD_CUSTOM_COMMAND_COMPLETED; + } + } + if (clientCmd.m_updateFlags & CMD_CUSTOM_COMMAND_UNLOAD_PLUGIN) + { + m_data->m_pluginManager.unloadPlugin(clientCmd.m_customCommandArgs.m_pluginUniqueId); + serverCmd.m_type = CMD_CUSTOM_COMMAND_COMPLETED; + } + if (clientCmd.m_updateFlags & CMD_CUSTOM_COMMAND_EXECUTE_PLUGIN_COMMAND) + { + + int result = m_data->m_pluginManager.executePluginCommand(clientCmd.m_customCommandArgs.m_pluginUniqueId, &clientCmd.m_customCommandArgs.m_arguments); + serverCmd.m_customCommandResultArgs.m_executeCommandResult = result; + serverCmd.m_type = CMD_CUSTOM_COMMAND_COMPLETED; + + } + return hasStatus; +} + + +bool PhysicsServerCommandProcessor::processUserDebugDrawCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + + BT_PROFILE("CMD_USER_DEBUG_DRAW"); + SharedMemoryStatus& serverCmd = serverStatusOut; + serverCmd.m_type = CMD_USER_DEBUG_DRAW_FAILED; + + + int trackingVisualShapeIndex = -1; + + if (clientCmd.m_userDebugDrawArgs.m_parentObjectUniqueId>=0) + { + InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(clientCmd.m_userDebugDrawArgs.m_parentObjectUniqueId); + if (bodyHandle) + { + int linkIndex = -1; + + if (bodyHandle->m_multiBody) + { + int linkIndex = clientCmd.m_userDebugDrawArgs.m_parentLinkIndex; + if (linkIndex ==-1) + { + if (bodyHandle->m_multiBody->getBaseCollider()) + { + trackingVisualShapeIndex = bodyHandle->m_multiBody->getBaseCollider()->getUserIndex(); + } + } else + { + if (linkIndex >=0 && linkIndex < bodyHandle->m_multiBody->getNumLinks()) + { + if (bodyHandle->m_multiBody->getLink(linkIndex).m_collider) + { + trackingVisualShapeIndex = bodyHandle->m_multiBody->getLink(linkIndex).m_collider->getUserIndex(); + } + } + + } + } + if (bodyHandle->m_rigidBody) + { + trackingVisualShapeIndex = bodyHandle->m_rigidBody->getUserIndex(); + } + } + } + + if (clientCmd.m_updateFlags & USER_DEBUG_ADD_PARAMETER) + { + int uid = m_data->m_guiHelper->addUserDebugParameter( + clientCmd.m_userDebugDrawArgs.m_text, + clientCmd.m_userDebugDrawArgs.m_rangeMin, + clientCmd.m_userDebugDrawArgs.m_rangeMax, + clientCmd.m_userDebugDrawArgs.m_startValue + ); + serverCmd.m_userDebugDrawArgs.m_debugItemUniqueId = uid; + serverCmd.m_type = CMD_USER_DEBUG_DRAW_COMPLETED; + } + if (clientCmd.m_updateFlags &USER_DEBUG_READ_PARAMETER) + { + + int ok = m_data->m_guiHelper->readUserDebugParameter( + clientCmd.m_userDebugDrawArgs.m_itemUniqueId, + &serverCmd.m_userDebugDrawArgs.m_parameterValue); + if (ok) + { + serverCmd.m_type = CMD_USER_DEBUG_DRAW_PARAMETER_COMPLETED; + } + } + if ((clientCmd.m_updateFlags & USER_DEBUG_SET_CUSTOM_OBJECT_COLOR) || (clientCmd.m_updateFlags & USER_DEBUG_REMOVE_CUSTOM_OBJECT_COLOR)) + { + int bodyUniqueId = clientCmd.m_userDebugDrawArgs.m_objectUniqueId; + InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); + if (body) + { + btCollisionObject* destColObj = 0; + + if (body->m_multiBody) + { + if (clientCmd.m_userDebugDrawArgs.m_linkIndex == -1) + { + destColObj = body->m_multiBody->getBaseCollider(); + } + else + { + if (clientCmd.m_userDebugDrawArgs.m_linkIndex >= 0 && clientCmd.m_userDebugDrawArgs.m_linkIndex < body->m_multiBody->getNumLinks()) + { + destColObj = body->m_multiBody->getLink(clientCmd.m_userDebugDrawArgs.m_linkIndex).m_collider; + } + } + + } + if (body->m_rigidBody) + { + destColObj = body->m_rigidBody; + } + + if (destColObj) + { + if (clientCmd.m_updateFlags & USER_DEBUG_REMOVE_CUSTOM_OBJECT_COLOR) + { + destColObj->removeCustomDebugColor(); + serverCmd.m_type = CMD_USER_DEBUG_DRAW_COMPLETED; + } + if (clientCmd.m_updateFlags & USER_DEBUG_SET_CUSTOM_OBJECT_COLOR) + { + btVector3 objectColorRGB; + objectColorRGB.setValue(clientCmd.m_userDebugDrawArgs.m_objectDebugColorRGB[0], + clientCmd.m_userDebugDrawArgs.m_objectDebugColorRGB[1], + clientCmd.m_userDebugDrawArgs.m_objectDebugColorRGB[2]); + destColObj->setCustomDebugColor(objectColorRGB); + serverCmd.m_type = CMD_USER_DEBUG_DRAW_COMPLETED; + } + } + } + } + + if (clientCmd.m_updateFlags & USER_DEBUG_HAS_TEXT) + { + //addUserDebugText3D( const double orientation[4], const double textColorRGB[3], double size, double lifeTime, int trackingObjectUniqueId, int optionFlags){return -1;} + + int optionFlags = clientCmd.m_userDebugDrawArgs.m_optionFlags; + + if ((clientCmd.m_updateFlags & USER_DEBUG_HAS_TEXT_ORIENTATION)==0) + { + optionFlags |= DEB_DEBUG_TEXT_ALWAYS_FACE_CAMERA; + } + + + int replaceItemUniqueId = -1; + if ((clientCmd.m_updateFlags & USER_DEBUG_HAS_REPLACE_ITEM_UNIQUE_ID)!=0) + { + replaceItemUniqueId = clientCmd.m_userDebugDrawArgs.m_replaceItemUniqueId; + } + + + int uid = m_data->m_guiHelper->addUserDebugText3D(clientCmd.m_userDebugDrawArgs.m_text, + clientCmd.m_userDebugDrawArgs.m_textPositionXYZ, + clientCmd.m_userDebugDrawArgs.m_textOrientation, + clientCmd.m_userDebugDrawArgs.m_textColorRGB, + clientCmd.m_userDebugDrawArgs.m_textSize, + clientCmd.m_userDebugDrawArgs.m_lifeTime, + trackingVisualShapeIndex, + optionFlags, + replaceItemUniqueId); + + if (uid>=0) + { + serverCmd.m_userDebugDrawArgs.m_debugItemUniqueId = uid; + serverCmd.m_type = CMD_USER_DEBUG_DRAW_COMPLETED; + } + } + + if (clientCmd.m_updateFlags & USER_DEBUG_HAS_LINE) + { + int uid = m_data->m_guiHelper->addUserDebugLine( + clientCmd.m_userDebugDrawArgs.m_debugLineFromXYZ, + clientCmd.m_userDebugDrawArgs.m_debugLineToXYZ, + clientCmd.m_userDebugDrawArgs.m_debugLineColorRGB, + clientCmd.m_userDebugDrawArgs.m_lineWidth, + clientCmd.m_userDebugDrawArgs.m_lifeTime, + trackingVisualShapeIndex); + + if (uid>=0) + { + serverCmd.m_userDebugDrawArgs.m_debugItemUniqueId = uid; + serverCmd.m_type = CMD_USER_DEBUG_DRAW_COMPLETED; + } + } + + + if (clientCmd.m_updateFlags & USER_DEBUG_REMOVE_ALL) + { + m_data->m_guiHelper->removeAllUserDebugItems(); + serverCmd.m_type = CMD_USER_DEBUG_DRAW_COMPLETED; + + } + if (clientCmd.m_updateFlags & USER_DEBUG_REMOVE_ONE_ITEM) + { + m_data->m_guiHelper->removeUserDebugItem(clientCmd.m_userDebugDrawArgs.m_itemUniqueId); + serverCmd.m_type = CMD_USER_DEBUG_DRAW_COMPLETED; + + } + return hasStatus; +} + +bool PhysicsServerCommandProcessor::processSetVRCameraStateCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + BT_PROFILE("CMD_SET_VR_CAMERA_STATE"); + + if (clientCmd.m_updateFlags & VR_CAMERA_ROOT_POSITION) + { + gVRTeleportPos1[0] = clientCmd.m_vrCameraStateArguments.m_rootPosition[0]; + gVRTeleportPos1[1] = clientCmd.m_vrCameraStateArguments.m_rootPosition[1]; + gVRTeleportPos1[2] = clientCmd.m_vrCameraStateArguments.m_rootPosition[2]; + } + if (clientCmd.m_updateFlags & VR_CAMERA_ROOT_ORIENTATION) + { + gVRTeleportOrn[0] = clientCmd.m_vrCameraStateArguments.m_rootOrientation[0]; + gVRTeleportOrn[1] = clientCmd.m_vrCameraStateArguments.m_rootOrientation[1]; + gVRTeleportOrn[2] = clientCmd.m_vrCameraStateArguments.m_rootOrientation[2]; + gVRTeleportOrn[3] = clientCmd.m_vrCameraStateArguments.m_rootOrientation[3]; + } + + if (clientCmd.m_updateFlags & VR_CAMERA_ROOT_TRACKING_OBJECT) + { + gVRTrackingObjectUniqueId = clientCmd.m_vrCameraStateArguments.m_trackingObjectUniqueId; + } + + if (clientCmd.m_updateFlags & VR_CAMERA_FLAG) + { + gVRTrackingObjectFlag = clientCmd.m_vrCameraStateArguments.m_trackingObjectFlag; + } + + serverStatusOut.m_type = CMD_CLIENT_COMMAND_COMPLETED; + return hasStatus; +} + + +bool PhysicsServerCommandProcessor::processRequestVREventsCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + //BT_PROFILE("CMD_REQUEST_VR_EVENTS_DATA"); + serverStatusOut.m_sendVREvents.m_numVRControllerEvents = 0; + + for (int i=0;im_vrControllerEvents.m_vrEvents[i]; + + if (clientCmd.m_updateFlags&event.m_deviceType) + { + if (event.m_numButtonEvents + event.m_numMoveEvents) + { + serverStatusOut.m_sendVREvents.m_controllerEvents[serverStatusOut.m_sendVREvents.m_numVRControllerEvents++] = event; + event.m_numButtonEvents = 0; + event.m_numMoveEvents = 0; + for (int b=0;bm_mouseEvents.size(); + if (serverStatusOut.m_sendMouseEvents.m_numMouseEvents>MAX_MOUSE_EVENTS) + { + serverStatusOut.m_sendMouseEvents.m_numMouseEvents = MAX_MOUSE_EVENTS; + } + for (int i=0;im_mouseEvents[i]; + } + + m_data->m_mouseEvents.resize(0); + serverStatusOut.m_type = CMD_REQUEST_MOUSE_EVENTS_DATA_COMPLETED; + return hasStatus; +} + +bool PhysicsServerCommandProcessor::processRequestKeyboardEventsCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + //BT_PROFILE("CMD_REQUEST_KEYBOARD_EVENTS_DATA"); + bool hasStatus = true; + serverStatusOut.m_sendKeyboardEvents.m_numKeyboardEvents = m_data->m_keyboardEvents.size(); + if (serverStatusOut.m_sendKeyboardEvents.m_numKeyboardEvents>MAX_KEYBOARD_EVENTS) + { + serverStatusOut.m_sendKeyboardEvents.m_numKeyboardEvents = MAX_KEYBOARD_EVENTS; + } + for (int i=0;im_keyboardEvents[i]; + } + + btAlignedObjectArray events; + + //remove out-of-date events + for (int i=0;im_keyboardEvents.size();i++) + { + b3KeyboardEvent event = m_data->m_keyboardEvents[i]; + if (event.m_keyState & eButtonIsDown) + { + event.m_keyState = eButtonIsDown; + events.push_back(event); + } + } + m_data->m_keyboardEvents.resize(events.size()); + for (int i=0;im_keyboardEvents[i] = events[i]; + } + + serverStatusOut.m_type = CMD_REQUEST_KEYBOARD_EVENTS_DATA_COMPLETED; + return hasStatus; +} + +bool PhysicsServerCommandProcessor::processRequestRaycastIntersectionsCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + BT_PROFILE("CMD_REQUEST_RAY_CAST_INTERSECTIONS"); + serverStatusOut.m_raycastHits.m_numRaycastHits = 0; + + for (int ray=0;raym_dynamicsWorld->rayTest(rayFromWorld,rayToWorld,rayResultCallback); + int rayHits = serverStatusOut.m_raycastHits.m_numRaycastHits; + + if (rayResultCallback.hasHit()) + { + serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitFraction + = rayResultCallback.m_closestHitFraction; + + int objectUniqueId = -1; + int linkIndex = -1; + + const btRigidBody* body = btRigidBody::upcast(rayResultCallback.m_collisionObject); + if (body) + { + objectUniqueId = rayResultCallback.m_collisionObject->getUserIndex2(); + } else + { + const btMultiBodyLinkCollider* mblB = btMultiBodyLinkCollider::upcast(rayResultCallback.m_collisionObject); + if (mblB && mblB->m_multiBody) + { + linkIndex = mblB->m_link; + objectUniqueId = mblB->m_multiBody->getUserIndex2(); + } + } + + serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitObjectUniqueId + = objectUniqueId; + serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitObjectLinkIndex + = linkIndex; + + serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitPositionWorld[0] + = rayResultCallback.m_hitPointWorld[0]; + serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitPositionWorld[1] + = rayResultCallback.m_hitPointWorld[1]; + serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitPositionWorld[2] + = rayResultCallback.m_hitPointWorld[2]; + + serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitNormalWorld[0] + = rayResultCallback.m_hitNormalWorld[0]; + serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitNormalWorld[1] + = rayResultCallback.m_hitNormalWorld[1]; + serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitNormalWorld[2] + = rayResultCallback.m_hitNormalWorld[2]; + + } else + { + serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitFraction = 1; + serverStatusOut.m_raycastHits.m_rayHits[serverStatusOut.m_raycastHits.m_numRaycastHits].m_hitObjectUniqueId = -1; + serverStatusOut.m_raycastHits.m_rayHits[serverStatusOut.m_raycastHits.m_numRaycastHits].m_hitObjectLinkIndex = -1; + serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitPositionWorld[0] = 0; + serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitPositionWorld[1] = 0; + serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitPositionWorld[2] = 0; + serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitNormalWorld[0] = 0; + serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitNormalWorld[1] = 0; + serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitNormalWorld[2] = 0; + } + serverStatusOut.m_raycastHits.m_numRaycastHits++; + } + serverStatusOut.m_type = CMD_REQUEST_RAY_CAST_INTERSECTIONS_COMPLETED; + return hasStatus; +} + +bool PhysicsServerCommandProcessor::processRequestDebugLinesCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + BT_PROFILE("CMD_REQUEST_DEBUG_LINES"); + int curFlags =m_data->m_remoteDebugDrawer->getDebugMode(); + + int debugMode = clientCmd.m_requestDebugLinesArguments.m_debugMode;//clientCmd.btIDebugDraw::DBG_DrawWireframe|btIDebugDraw::DBG_DrawAabb; + int startingLineIndex = clientCmd.m_requestDebugLinesArguments.m_startingLineIndex; + if (startingLineIndex<0) + { + b3Warning("startingLineIndex should be non-negative"); + startingLineIndex = 0; + } + + if (clientCmd.m_requestDebugLinesArguments.m_startingLineIndex==0) + { + m_data->m_remoteDebugDrawer->m_lines2.resize(0); + //|btIDebugDraw::DBG_DrawAabb| + // btIDebugDraw::DBG_DrawConstraints |btIDebugDraw::DBG_DrawConstraintLimits ; + m_data->m_remoteDebugDrawer->setDebugMode(debugMode); + btIDebugDraw* oldDebugDrawer = m_data->m_dynamicsWorld->getDebugDrawer(); + m_data->m_dynamicsWorld->setDebugDrawer(m_data->m_remoteDebugDrawer); + m_data->m_dynamicsWorld->debugDrawWorld(); + m_data->m_dynamicsWorld->setDebugDrawer(oldDebugDrawer); + m_data->m_remoteDebugDrawer->setDebugMode(curFlags); + } + + //9 floats per line: 3 floats for 'from', 3 floats for 'to' and 3 floats for 'color' + int bytesPerLine = (sizeof(float) * 9); + int maxNumLines = bufferSizeInBytes/bytesPerLine-1; + if (startingLineIndex >m_data->m_remoteDebugDrawer->m_lines2.size()) + { + b3Warning("m_startingLineIndex exceeds total number of debug lines"); + startingLineIndex =m_data->m_remoteDebugDrawer->m_lines2.size(); + } + + int numLines = btMin(maxNumLines,m_data->m_remoteDebugDrawer->m_lines2.size()-startingLineIndex); + + if (numLines) + { + + float* linesFrom = (float*)bufferServerToClient; + float* linesTo = (float*)(bufferServerToClient+numLines*3*sizeof(float)); + float* linesColor = (float*)(bufferServerToClient+2*numLines*3*sizeof(float)); + + for (int i=0;im_remoteDebugDrawer->m_lines2[i+startingLineIndex].m_from.x(); + linesTo[i*3] = m_data->m_remoteDebugDrawer->m_lines2[i+startingLineIndex].m_to.x(); + linesColor[i*3] = m_data->m_remoteDebugDrawer->m_lines2[i+startingLineIndex].m_color.x(); + + linesFrom[i*3+1] = m_data->m_remoteDebugDrawer->m_lines2[i+startingLineIndex].m_from.y(); + linesTo[i*3+1] = m_data->m_remoteDebugDrawer->m_lines2[i+startingLineIndex].m_to.y(); + linesColor[i*3+1] = m_data->m_remoteDebugDrawer->m_lines2[i+startingLineIndex].m_color.y(); + + linesFrom[i*3+2] = m_data->m_remoteDebugDrawer->m_lines2[i+startingLineIndex].m_from.z(); + linesTo[i*3+2] = m_data->m_remoteDebugDrawer->m_lines2[i+startingLineIndex].m_to.z(); + linesColor[i*3+2] = m_data->m_remoteDebugDrawer->m_lines2[i+startingLineIndex].m_color.z(); + } + } + + serverStatusOut.m_type = CMD_DEBUG_LINES_COMPLETED; + serverStatusOut.m_numDataStreamBytes = numLines * bytesPerLine; + serverStatusOut.m_sendDebugLinesArgs.m_numDebugLines = numLines; + serverStatusOut.m_sendDebugLinesArgs.m_startingLineIndex = startingLineIndex; + serverStatusOut.m_sendDebugLinesArgs.m_numRemainingDebugLines = m_data->m_remoteDebugDrawer->m_lines2.size()-(startingLineIndex+numLines); + + return hasStatus; +} + + +bool PhysicsServerCommandProcessor::processSyncBodyInfoCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + BT_PROFILE("CMD_SYNC_BODY_INFO"); + + b3AlignedObjectArray usedHandles; + m_data->m_bodyHandles.getUsedHandles(usedHandles); + int actualNumBodies = 0; + for (int i=0;im_bodyHandles.getHandle(usedHandle); + if (body && (body->m_multiBody || body->m_rigidBody)) + { + serverStatusOut.m_sdfLoadedArgs.m_bodyUniqueIds[actualNumBodies++] = usedHandle; + } + } + serverStatusOut.m_sdfLoadedArgs.m_numBodies = actualNumBodies; + + int usz = m_data->m_userConstraints.size(); + serverStatusOut.m_sdfLoadedArgs.m_numUserConstraints = usz; + for (int i=0;im_userConstraints.getKeyAtIndex(i).getUid1(); +// int uid = m_data->m_userConstraints.getAtIndex(i)->m_userConstraintData.m_userConstraintUniqueId; + serverStatusOut.m_sdfLoadedArgs.m_userConstraintUniqueIds[i] = key; + } + + serverStatusOut.m_type = CMD_SYNC_BODY_INFO_COMPLETED; + return hasStatus; +} + +bool PhysicsServerCommandProcessor::processSendDesiredStateCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + BT_PROFILE("CMD_SEND_DESIRED_STATE"); + if (m_data->m_verboseOutput) + { + b3Printf("Processed CMD_SEND_DESIRED_STATE"); + } + + int bodyUniqueId = clientCmd.m_sendDesiredStateCommandArgument.m_bodyUniqueId; + InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); + + if (body && body->m_multiBody) + { + btMultiBody* mb = body->m_multiBody; + btAssert(mb); + + switch (clientCmd.m_sendDesiredStateCommandArgument.m_controlMode) + { + case CONTROL_MODE_TORQUE: + { + if (m_data->m_verboseOutput) + { + b3Printf("Using CONTROL_MODE_TORQUE"); + } + // mb->clearForcesAndTorques(); + int torqueIndex = 6; + if ((clientCmd.m_updateFlags&SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0) + { + for (int link=0;linkgetNumLinks();link++) + { + + for (int dof=0;dofgetLink(link).m_dofCount;dof++) + { + double torque = 0.f; + if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[torqueIndex]&SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0) + { + torque = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[torqueIndex]; + mb->addJointTorqueMultiDof(link,dof,torque); + } + torqueIndex++; + } + } + } + break; + } + case CONTROL_MODE_VELOCITY: + { + if (m_data->m_verboseOutput) + { + b3Printf("Using CONTROL_MODE_VELOCITY"); + } + + int numMotors = 0; + //find the joint motors and apply the desired velocity and maximum force/torque + { + int dofIndex = 6;//skip the 3 linear + 3 angular degree of freedom entries of the base + for (int link=0;linkgetNumLinks();link++) + { + if (supportsJointMotor(mb,link)) + { + + btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)mb->getLink(link).m_userPtr; + + + if (motor) + { + btScalar desiredVelocity = 0.f; + bool hasDesiredVelocity = false; + + + if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex]&SIM_DESIRED_STATE_HAS_QDOT)!=0) { - urdfColObj.m_geometry.m_meshFileType = out_type; - - if (out_type==UrdfGeometry::FILE_OBJ) + desiredVelocity = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[dofIndex]; + btScalar kd = 0.1f; + if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] & SIM_DESIRED_STATE_HAS_KD)!=0) { - //create a convex hull for each shape, and store it in a btCompoundShape + kd = clientCmd.m_sendDesiredStateCommandArgument.m_Kd[dofIndex]; + } - if (clientCmd.m_createUserShapeArgs.m_shapes[i].m_collisionFlags&GEOM_FORCE_CONCAVE_TRIMESH) + motor->setVelocityTarget(desiredVelocity,kd); + + btScalar kp = 0.f; + motor->setPositionTarget(0,kp); + hasDesiredVelocity = true; + } + if (hasDesiredVelocity) + { + btScalar maxImp = 1000000.f*m_data->m_physicsDeltaTime; + if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex]&SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0) + { + maxImp = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex]*m_data->m_physicsDeltaTime; + } + motor->setMaxAppliedImpulse(maxImp); + } + numMotors++; + + } + } + dofIndex += mb->getLink(link).m_dofCount; + } + } + break; + } + + case CONTROL_MODE_POSITION_VELOCITY_PD: + { + if (m_data->m_verboseOutput) + { + b3Printf("Using CONTROL_MODE_POSITION_VELOCITY_PD"); + } + //compute the force base on PD control + + int numMotors = 0; + //find the joint motors and apply the desired velocity and maximum force/torque + { + int velIndex = 6;//skip the 3 linear + 3 angular degree of freedom velocity entries of the base + int posIndex = 7;//skip 3 positional and 4 orientation (quaternion) positional degrees of freedom of the base + for (int link=0;linkgetNumLinks();link++) + { + if (supportsJointMotor(mb,link)) + { + + + btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)mb->getLink(link).m_userPtr; + + if (motor) + { + + bool hasDesiredPosOrVel = false; + btScalar kp = 0.f; + btScalar kd = 0.f; + btScalar desiredVelocity = 0.f; + if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex] & SIM_DESIRED_STATE_HAS_QDOT)!=0) + { + hasDesiredPosOrVel = true; + desiredVelocity = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[velIndex]; + kd = 0.1; + } + btScalar desiredPosition = 0.f; + if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[posIndex] & SIM_DESIRED_STATE_HAS_Q)!=0) + { + hasDesiredPosOrVel = true; + desiredPosition = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[posIndex]; + kp = 0.1; + } + + if (hasDesiredPosOrVel) + { + + if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex] & SIM_DESIRED_STATE_HAS_KP)!=0) + { + kp = clientCmd.m_sendDesiredStateCommandArgument.m_Kp[velIndex]; + } + + if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex] & SIM_DESIRED_STATE_HAS_KD)!=0) + { + kd = clientCmd.m_sendDesiredStateCommandArgument.m_Kd[velIndex]; + } + + motor->setVelocityTarget(desiredVelocity,kd); + motor->setPositionTarget(desiredPosition,kp); + + btScalar maxImp = 1000000.f*m_data->m_physicsDeltaTime; + + if ((clientCmd.m_updateFlags & SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0) + maxImp = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[velIndex]*m_data->m_physicsDeltaTime; + + motor->setMaxAppliedImpulse(maxImp); + } + numMotors++; + } + + } + velIndex += mb->getLink(link).m_dofCount; + posIndex += mb->getLink(link).m_posVarCount; + } + } + + break; + } + default: + { + b3Warning("m_controlMode not implemented yet"); + break; + } + + } + } else + { + //support for non-btMultiBody, such as btRigidBody + + if (body && body->m_rigidBody) + { + btRigidBody* rb = body->m_rigidBody; + btAssert(rb); + + //switch (clientCmd.m_sendDesiredStateCommandArgument.m_controlMode) + { + //case CONTROL_MODE_TORQUE: + { + if (m_data->m_verboseOutput) + { + b3Printf("Using CONTROL_MODE_TORQUE"); + } + // mb->clearForcesAndTorques(); + ///see addJointInfoFromConstraint + int velIndex = 6; + int posIndex = 7; + //if ((clientCmd.m_updateFlags&SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0) + { + for (int link=0;linkm_rigidBodyJoints.size();link++) + { + btGeneric6DofSpring2Constraint* con = body->m_rigidBodyJoints[link]; + + btVector3 linearLowerLimit; + btVector3 linearUpperLimit; + btVector3 angularLowerLimit; + btVector3 angularUpperLimit; + + + //for (int dof=0;dofgetLink(link).m_dofCount;dof++) + { + + + { + + int torqueIndex = velIndex; + double torque = 100; + bool hasDesiredTorque = false; + if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex] & SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0) + { + torque = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[velIndex]; + hasDesiredTorque = true; + } + + bool hasDesiredPosOrVel = false; + btScalar qdotTarget = 0.f; + if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex] & SIM_DESIRED_STATE_HAS_QDOT)!=0) + { + hasDesiredPosOrVel = true; + qdotTarget = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[velIndex]; + } + btScalar qTarget = 0.f; + if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[posIndex] & SIM_DESIRED_STATE_HAS_Q)!=0) + { + hasDesiredPosOrVel = true; + qTarget = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[posIndex]; + } + + con->getLinearLowerLimit(linearLowerLimit); + con->getLinearUpperLimit(linearUpperLimit); + con->getAngularLowerLimit(angularLowerLimit); + con->getAngularUpperLimit(angularUpperLimit); + + if (linearLowerLimit.isZero() && linearUpperLimit.isZero() && angularLowerLimit.isZero() && angularUpperLimit.isZero()) + { + //fixed, don't do anything + } else + { + con->calculateTransforms(); + + if (linearLowerLimit.isZero() && linearUpperLimit.isZero()) { - GLInstanceGraphicsShape* glmesh = LoadMeshFromObj(relativeFileName, pathPrefix); - - if (!glmesh || glmesh->m_numvertices<=0) - { - b3Warning("%s: cannot extract mesh from '%s'\n", pathPrefix, relativeFileName); - delete glmesh; - break; - } - btAlignedObjectArray convertedVerts; - convertedVerts.reserve(glmesh->m_numvertices); - - for (int i=0; im_numvertices; i++) - { - convertedVerts.push_back(btVector3( - glmesh->m_vertices->at(i).xyzw[0]*meshScale[0], - glmesh->m_vertices->at(i).xyzw[1]*meshScale[1], - glmesh->m_vertices->at(i).xyzw[2]*meshScale[2])); - } + //eRevoluteType; + btVector3 limitRange = angularLowerLimit.absolute()+angularUpperLimit.absolute(); + int limitAxis = limitRange.maxAxis(); + const btTransform& transA = con->getCalculatedTransformA(); + const btTransform& transB = con->getCalculatedTransformB(); + btVector3 axisA = transA.getBasis().getColumn(limitAxis); + btVector3 axisB = transB.getBasis().getColumn(limitAxis); - BT_PROFILE("convert trimesh"); - btTriangleMesh* meshInterface = new btTriangleMesh(); - this->m_data->m_meshInterfaces.push_back(meshInterface); + switch (clientCmd.m_sendDesiredStateCommandArgument.m_controlMode) { - BT_PROFILE("convert vertices"); + case CONTROL_MODE_TORQUE: + { + if (hasDesiredTorque) + { + con->getRigidBodyA().applyTorque(torque*axisA); + con->getRigidBodyB().applyTorque(-torque*axisB); + } + break; + } + case CONTROL_MODE_VELOCITY: + { + if (hasDesiredPosOrVel) + { + con->enableMotor(3+limitAxis,true); + con->setTargetVelocity(3+limitAxis, qdotTarget); + //this is max motor force impulse + btScalar torqueImpulse = torque*m_data->m_dynamicsWorld->getSolverInfo().m_timeStep; + con->setMaxMotorForce(3+limitAxis,torqueImpulse); + } + break; + } + case CONTROL_MODE_POSITION_VELOCITY_PD: + { + if (hasDesiredPosOrVel) + { + con->setServo(3+limitAxis,true); + con->setServoTarget(3+limitAxis,-qTarget); + //next one is the maximum velocity to reach target position. + //the maximum velocity is limited by maxMotorForce + con->setTargetVelocity(3+limitAxis, 100); + //this is max motor force impulse + btScalar torqueImpulse = torque*m_data->m_dynamicsWorld->getSolverInfo().m_timeStep; + con->setMaxMotorForce(3+limitAxis,torqueImpulse); + con->enableMotor(3+limitAxis,true); + } + break; + } + default: + { + } + }; + + + - for (int i=0; im_numIndices/3; i++) - { - const btVector3& v0 = convertedVerts[glmesh->m_indices->at(i*3)]; - const btVector3& v1 = convertedVerts[glmesh->m_indices->at(i*3+1)]; - const btVector3& v2 = convertedVerts[glmesh->m_indices->at(i*3+2)]; - meshInterface->addTriangle(v0,v1,v2); - } - } - { - BT_PROFILE("create btBvhTriangleMeshShape"); - btBvhTriangleMeshShape* trimesh = new btBvhTriangleMeshShape(meshInterface,true,true); - m_data->m_collisionShapes.push_back(trimesh); - //trimesh->setLocalScaling(collision->m_geometry.m_meshScale); - shape = trimesh; - if (compound) - { - compound->addChildShape(childTransform,shape); - } - } - delete glmesh; } else { + //ePrismaticType; @todo + btVector3 limitRange = linearLowerLimit.absolute()+linearUpperLimit.absolute(); + int limitAxis = limitRange.maxAxis(); - std::vector shapes; - std::string err = tinyobj::LoadObj(shapes,out_found_filename.c_str()); + const btTransform& transA = con->getCalculatedTransformA(); + const btTransform& transB = con->getCalculatedTransformB(); + btVector3 axisA = transA.getBasis().getColumn(limitAxis); + btVector3 axisB = transB.getBasis().getColumn(limitAxis); - //shape = createConvexHullFromShapes(shapes, collision->m_geometry.m_meshScale); - //static btCollisionShape* createConvexHullFromShapes(std::vector& shapes, const btVector3& geomScale) - B3_PROFILE("createConvexHullFromShapes"); - if (compound==0) + switch (clientCmd.m_sendDesiredStateCommandArgument.m_controlMode) { - compound = worldImporter->createCompoundShape(); - } - compound->setMargin(defaultCollisionMargin); - - for (int s = 0; s<(int)shapes.size(); s++) - { - btConvexHullShape* convexHull = worldImporter->createConvexHullShape(); - convexHull->setMargin(defaultCollisionMargin); - tinyobj::shape_t& shape = shapes[s]; - int faceCount = shape.mesh.indices.size(); - - for (int f = 0; faddPoint(pt*meshScale,false); - - pt.setValue(shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 0], - shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 1], - shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 2]); - convexHull->addPoint(pt*meshScale, false); - - pt.setValue(shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 0], - shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 1], - shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 2]); - convexHull->addPoint(pt*meshScale, false); + con->getRigidBodyA().applyForce(-torque*axisA,btVector3(0,0,0)); + con->getRigidBodyB().applyForce(torque*axisB,btVector3(0,0,0)); + break; } - - convexHull->recalcLocalAabb(); - convexHull->optimizeConvexHull(); - compound->addChildShape(childTransform,convexHull); - } - } - } - } - break; - } - default: - { - } - } - if (urdfColObj.m_geometry.m_type != URDF_GEOM_UNKNOWN) - { - urdfCollisionObjects.push_back(urdfColObj); - } - } - - if (compound && compound->getNumChildShapes()) - { - shape = compound; - } - - if (shape) - { - int collisionShapeUid = m_data->m_userCollisionShapeHandles.allocHandle(); - InternalCollisionShapeHandle* handle = m_data->m_userCollisionShapeHandles.getHandle(collisionShapeUid); - handle->m_collisionShape = shape; - for (int i=0;im_urdfCollisionObjects.push_back(urdfCollisionObjects[i]); - } - serverStatusOut.m_createUserShapeResultArgs.m_userShapeUniqueId = collisionShapeUid; - m_data->m_worldImporters.push_back(worldImporter); - serverStatusOut.m_type = CMD_CREATE_COLLISION_SHAPE_COMPLETED; - } else - { - delete worldImporter; - } - - - break; - } - case CMD_CREATE_VISUAL_SHAPE: - { - hasStatus = true; - serverStatusOut.m_type = CMD_CREATE_VISUAL_SHAPE_FAILED; - { - double globalScaling = 1.f; - - BulletURDFImporter u2b(m_data->m_guiHelper, &m_data->m_visualConverter, globalScaling); - u2b.setEnableTinyRenderer(m_data->m_enableTinyRenderer); - btTransform localInertiaFrame; - localInertiaFrame.setIdentity(); - btTransform childTrans; - childTrans.setIdentity(); - const char* pathPrefix = ""; - if (clientCmd.m_createUserShapeArgs.m_numUserShapes == 1) - { - int userShapeIndex = 0; - - UrdfVisual visualShape; - visualShape.m_geometry.m_type = (UrdfGeomTypes)clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_type; - char relativeFileName[1024]; - char pathPrefix[1024]; - pathPrefix[0] = 0; - - if (visualShape.m_geometry.m_type == URDF_GEOM_MESH) - { - - std::string fileName = clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_meshFileName; - const std::string& error_message_prefix=""; - std::string out_found_filename; - int out_type; - if (b3ResourcePath::findResourcePath(fileName.c_str(), relativeFileName, 1024)) - { - b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024); - } - - bool foundFile = findExistingMeshFile(pathPrefix, relativeFileName,error_message_prefix,&out_found_filename, &out_type); - visualShape.m_geometry.m_meshFileType = out_type; - visualShape.m_geometry.m_meshFileName=fileName; - - visualShape.m_geometry.m_meshScale.setValue(clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_meshScale[0], - clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_meshScale[1], - clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_meshScale[2]); - } - visualShape.m_name = "bla"; - visualShape.m_materialName=""; - visualShape.m_sourceFileLocation="blaat_line_10"; - visualShape.m_linkLocalFrame.setIdentity(); - visualShape.m_geometry.m_hasLocalMaterial = false; - - - btAlignedObjectArray vertices; - btAlignedObjectArray indices; - btTransform startTrans; startTrans.setIdentity(); - btAlignedObjectArray textures; - bool hasRGBA = (clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_visualFlags&GEOM_VISUAL_HAS_RGBA_COLOR)!=0;; - bool hasSpecular = (clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_visualFlags&GEOM_VISUAL_HAS_SPECULAR_COLOR)!=0;; - visualShape.m_geometry.m_hasLocalMaterial = hasRGBA|hasSpecular; - if (visualShape.m_geometry.m_hasLocalMaterial) - { - if (hasRGBA) - { - visualShape.m_geometry.m_localMaterial.m_matColor.m_rgbaColor.setValue( - clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_rgbaColor[0], - clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_rgbaColor[1], - clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_rgbaColor[2], - clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_rgbaColor[3]); - } else - { - - } - if (hasSpecular) - { - visualShape.m_geometry.m_localMaterial.m_matColor.m_specularColor.setValue( - clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_specularColor[0], - clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_specularColor[1], - clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_specularColor[2]); - } else - { - visualShape.m_geometry.m_localMaterial.m_matColor.m_specularColor.setValue(0.4,0.4,0.4); - } - } - - if (clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_hasChildTransform !=0) - { - childTrans.setOrigin(btVector3(clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_childPosition[0], - clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_childPosition[1], - clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_childPosition[2])); - childTrans.setRotation(btQuaternion( - clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_childOrientation[0], - clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_childOrientation[1], - clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_childOrientation[2], - clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_childOrientation[3])); - } - - - - u2b.convertURDFToVisualShapeInternal(&visualShape, pathPrefix, localInertiaFrame.inverse()*childTrans, vertices, indices,textures); - - if (vertices.size() && indices.size()) - { - if (1) - { - int textureIndex = -1; - if (textures.size()) - { - - textureIndex = m_data->m_guiHelper->registerTexture(textures[0].textureData1,textures[0].m_width,textures[0].m_height); - } - int graphicsIndex = -1; - { - B3_PROFILE("registerGraphicsShape"); - graphicsIndex = m_data->m_guiHelper->registerGraphicsShape(&vertices[0].xyzw[0], vertices.size(), &indices[0], indices.size(), B3_GL_TRIANGLES, textureIndex); - if (graphicsIndex>=0) - { - int visualShapeUniqueId = m_data->m_userVisualShapeHandles.allocHandle(); - InternalVisualShapeHandle* visualHandle = m_data->m_userVisualShapeHandles.getHandle(visualShapeUniqueId); - visualHandle->m_OpenGLGraphicsIndex = graphicsIndex; - visualHandle->m_tinyRendererVisualShapeIndex = -1; - //tinyrenderer doesn't separate shape versus instance, so create it when creating the multibody instance - //store needed info for tinyrenderer - visualHandle->m_localInertiaFrame = localInertiaFrame; - visualHandle->m_visualShape = visualShape; - visualHandle->m_pathPrefix = pathPrefix[0] ? pathPrefix : ""; - - serverStatusOut.m_createUserShapeResultArgs.m_userShapeUniqueId = visualShapeUniqueId; - serverStatusOut.m_type = CMD_CREATE_VISUAL_SHAPE_COMPLETED; - } - } - } - } - } - } - - break; - } - case CMD_CREATE_MULTI_BODY: - { - hasStatus = true; - serverStatusOut.m_type = CMD_CREATE_MULTI_BODY_FAILED; - if (clientCmd.m_createMultiBodyArgs.m_baseLinkIndex>=0) - { - m_data->m_sdfRecentLoadedBodies.clear(); - - ProgrammaticUrdfInterface u2b(clientCmd.m_createMultiBodyArgs, m_data); - - bool useMultiBody = true; - if (clientCmd.m_updateFlags & MULT_BODY_USE_MAXIMAL_COORDINATES) - { - useMultiBody = false; - } - - int flags = 0; - bool ok = processImportedObjects("memory", bufferServerToClient, bufferSizeInBytes, useMultiBody, flags, u2b); - - if (ok) - { - int bodyUniqueId = -1; - - if (m_data->m_sdfRecentLoadedBodies.size()==1) - { - bodyUniqueId = m_data->m_sdfRecentLoadedBodies[0]; - } - m_data->m_sdfRecentLoadedBodies.clear(); - if (bodyUniqueId>=0) - { - m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld); - serverStatusOut.m_type = CMD_CREATE_MULTI_BODY_COMPLETED; - - int streamSizeInBytes = createBodyInfoStream(bodyUniqueId, bufferServerToClient, bufferSizeInBytes); - if (m_data->m_urdfLinkNameMapper.size()) - { - serverStatusOut.m_numDataStreamBytes = m_data->m_urdfLinkNameMapper.at(m_data->m_urdfLinkNameMapper.size()-1)->m_memSerializer->getCurrentBufferSize(); - } - serverStatusOut.m_dataStreamArguments.m_bodyUniqueId = bodyUniqueId; - InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); - strcpy(serverStatusOut.m_dataStreamArguments.m_bodyName, body->m_bodyName.c_str()); - } - } - - //ConvertURDF2Bullet(u2b,creation, rootTrans,m_data->m_dynamicsWorld,useMultiBody,u2b.getPathPrefix(),flags); - - - } - break; - } - case CMD_SET_ADDITIONAL_SEARCH_PATH: - { - BT_PROFILE("CMD_SET_ADDITIONAL_SEARCH_PATH"); - b3ResourcePath::setAdditionalSearchPath(clientCmd.m_searchPathArgs.m_path); - serverStatusOut.m_type = CMD_CLIENT_COMMAND_COMPLETED; - hasStatus = true; - break; - } - case CMD_LOAD_URDF: - { - BT_PROFILE("CMD_LOAD_URDF"); - const UrdfArgs& urdfArgs = clientCmd.m_urdfArguments; - if (m_data->m_verboseOutput) - { - b3Printf("Processed CMD_LOAD_URDF:%s", urdfArgs.m_urdfFileName); - } - btAssert((clientCmd.m_updateFlags&URDF_ARGS_FILE_NAME) !=0); - btAssert(urdfArgs.m_urdfFileName); - btVector3 initialPos(0,0,0); - btQuaternion initialOrn(0,0,0,1); - if (clientCmd.m_updateFlags & URDF_ARGS_INITIAL_POSITION) - { - initialPos[0] = urdfArgs.m_initialPosition[0]; - initialPos[1] = urdfArgs.m_initialPosition[1]; - initialPos[2] = urdfArgs.m_initialPosition[2]; - } - int urdfFlags = 0; - if (clientCmd.m_updateFlags & URDF_ARGS_HAS_CUSTOM_URDF_FLAGS) - { - urdfFlags = urdfArgs.m_urdfFlags; - } - if (clientCmd.m_updateFlags & URDF_ARGS_INITIAL_ORIENTATION) - { - initialOrn[0] = urdfArgs.m_initialOrientation[0]; - initialOrn[1] = urdfArgs.m_initialOrientation[1]; - initialOrn[2] = urdfArgs.m_initialOrientation[2]; - initialOrn[3] = urdfArgs.m_initialOrientation[3]; - } - bool useMultiBody=(clientCmd.m_updateFlags & URDF_ARGS_USE_MULTIBODY) ? (urdfArgs.m_useMultiBody!=0) : true; - bool useFixedBase = (clientCmd.m_updateFlags & URDF_ARGS_USE_FIXED_BASE) ? (urdfArgs.m_useFixedBase!=0): false; - int bodyUniqueId; - btScalar globalScaling = 1.f; - if (clientCmd.m_updateFlags & URDF_ARGS_USE_GLOBAL_SCALING) - { - globalScaling = urdfArgs.m_globalScaling; - } - //load the actual URDF and send a report: completed or failed - bool completedOk = loadUrdf(urdfArgs.m_urdfFileName, - initialPos,initialOrn, - useMultiBody, useFixedBase,&bodyUniqueId, bufferServerToClient, bufferSizeInBytes, urdfFlags, globalScaling); - - if (completedOk && bodyUniqueId>=0) - { - - - m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld); - - serverStatusOut.m_type = CMD_URDF_LOADING_COMPLETED; - - int streamSizeInBytes = createBodyInfoStream(bodyUniqueId, bufferServerToClient, bufferSizeInBytes); - - - if (m_data->m_urdfLinkNameMapper.size()) - { - serverStatusOut.m_numDataStreamBytes = m_data->m_urdfLinkNameMapper.at(m_data->m_urdfLinkNameMapper.size()-1)->m_memSerializer->getCurrentBufferSize(); - } - serverStatusOut.m_dataStreamArguments.m_bodyUniqueId = bodyUniqueId; - InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); - strcpy(serverStatusOut.m_dataStreamArguments.m_bodyName, body->m_bodyName.c_str()); - hasStatus = true; - - } else - { - serverStatusOut.m_type = CMD_URDF_LOADING_FAILED; - hasStatus = true; - } - - - - - break; - } - case CMD_LOAD_BUNNY: - { - serverStatusOut.m_type = CMD_UNKNOWN_COMMAND_FLUSHED; - hasStatus = true; -#ifdef USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD - double scale = 0.1; - double mass = 0.1; - double collisionMargin = 0.02; - if (clientCmd.m_updateFlags & LOAD_BUNNY_UPDATE_SCALE) - { - scale = clientCmd.m_loadBunnyArguments.m_scale; - } - if (clientCmd.m_updateFlags & LOAD_BUNNY_UPDATE_MASS) - { - mass = clientCmd.m_loadBunnyArguments.m_mass; - } - if (clientCmd.m_updateFlags & LOAD_BUNNY_UPDATE_COLLISION_MARGIN) - { - collisionMargin = clientCmd.m_loadBunnyArguments.m_collisionMargin; - } - m_data->m_softBodyWorldInfo.air_density = (btScalar)1.2; - m_data->m_softBodyWorldInfo.water_density = 0; - m_data->m_softBodyWorldInfo.water_offset = 0; - m_data->m_softBodyWorldInfo.water_normal = btVector3(0,0,0); - m_data->m_softBodyWorldInfo.m_gravity.setValue(0,0,-10); - m_data->m_softBodyWorldInfo.m_broadphase = m_data->m_broadphase; - m_data->m_softBodyWorldInfo.m_sparsesdf.Initialize(); - - btSoftBody* psb=btSoftBodyHelpers::CreateFromTriMesh(m_data->m_softBodyWorldInfo,gVerticesBunny, &gIndicesBunny[0][0], BUNNY_NUM_TRIANGLES); - - btSoftBody::Material* pm=psb->appendMaterial(); - pm->m_kLST = 1.0; - pm->m_flags -= btSoftBody::fMaterial::DebugDraw; - psb->generateBendingConstraints(2,pm); - psb->m_cfg.piterations = 50; - psb->m_cfg.kDF = 0.5; - psb->randomizeConstraints(); - psb->rotate(btQuaternion(0.70711,0,0,0.70711)); - psb->translate(btVector3(0,0,1.0)); - psb->scale(btVector3(scale,scale,scale)); - psb->setTotalMass(mass,true); - psb->getCollisionShape()->setMargin(collisionMargin); - - m_data->m_dynamicsWorld->addSoftBody(psb); - serverStatusOut.m_type = CMD_CLIENT_COMMAND_COMPLETED; -#endif - break; - } - case CMD_CREATE_SENSOR: - { - BT_PROFILE("CMD_CREATE_SENSOR"); - - if (m_data->m_verboseOutput) - { - b3Printf("Processed CMD_CREATE_SENSOR"); - } - int bodyUniqueId = clientCmd.m_createSensorArguments.m_bodyUniqueId; - InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); - if (body && body->m_multiBody) - { - btMultiBody* mb = body->m_multiBody; - btAssert(mb); - for (int i=0;igetLink(jointIndex).m_jointFeedback) - { - b3Warning("CMD_CREATE_SENSOR: sensor for joint [%d] already enabled", jointIndex); - } else - { - btMultiBodyJointFeedback* fb = new btMultiBodyJointFeedback(); - fb->m_reactionForces.setZero(); - mb->getLink(jointIndex).m_jointFeedback = fb; - m_data->m_multiBodyJointFeedbacks.push_back(fb); - }; - - } else - { - if (mb->getLink(jointIndex).m_jointFeedback) - { - m_data->m_multiBodyJointFeedbacks.remove(mb->getLink(jointIndex).m_jointFeedback); - delete mb->getLink(jointIndex).m_jointFeedback; - mb->getLink(jointIndex).m_jointFeedback=0; - } else - { - b3Warning("CMD_CREATE_SENSOR: cannot perform sensor removal request, no sensor on joint [%d]", jointIndex); - }; - - } - } - - } else - { - b3Warning("No btMultiBody in the world. btRigidBody/btTypedConstraint sensor not hooked up yet"); - } - -#if 0 - //todo(erwincoumans) here is some sample code to hook up a force/torque sensor for btTypedConstraint/btRigidBody - /* - for (int i=0;im_dynamicsWorld->getNumConstraints();i++) - { - btTypedConstraint* c = m_data->m_dynamicsWorld->getConstraint(i); - btJointFeedback* fb = new btJointFeedback(); - m_data->m_jointFeedbacks.push_back(fb); - c->setJointFeedback(fb); - - - } - */ -#endif - - serverStatusOut.m_type = CMD_CLIENT_COMMAND_COMPLETED; - hasStatus = true; - - break; - } - case CMD_PROFILE_TIMING: - { - { - B3_PROFILE("custom");//clientCmd.m_profile.m_name); - { - B3_PROFILE("event");//clientCmd.m_profile.m_name); - char** eventNamePtr = m_data->m_profileEvents[clientCmd.m_profile.m_name]; - char* eventName = 0; - if (eventNamePtr) - { - B3_PROFILE("reuse"); - eventName = *eventNamePtr; - - } else - { - B3_PROFILE("alloc"); - int len = strlen(clientCmd.m_profile.m_name); - eventName = new char[len+1]; - strcpy(eventName,clientCmd.m_profile.m_name); - eventName[len] = 0; - m_data->m_profileEvents.insert(eventName,eventName); - - } - - - { - { - B3_PROFILE("with");//clientCmd.m_profile.m_name); - { - B3_PROFILE("some");//clientCmd.m_profile.m_name); - { - B3_PROFILE("deep");//clientCmd.m_profile.m_name); - { - B3_PROFILE("level");//clientCmd.m_profile.m_name); + case CONTROL_MODE_VELOCITY: { - B3_PROFILE(eventName); - b3Clock::usleep(clientCmd.m_profile.m_durationInMicroSeconds); + con->enableMotor(limitAxis,true); + con->setTargetVelocity(limitAxis, -qdotTarget); + //this is max motor force impulse + btScalar torqueImpulse = torque*m_data->m_dynamicsWorld->getSolverInfo().m_timeStep; + con->setMaxMotorForce(limitAxis,torqueImpulse); + break; } - } - } - } - } - } - } - } - serverStatusOut.m_type = CMD_CLIENT_COMMAND_COMPLETED; - hasStatus = true; - break; - } - - case CMD_SEND_DESIRED_STATE: - { - BT_PROFILE("CMD_SEND_DESIRED_STATE"); - if (m_data->m_verboseOutput) - { - b3Printf("Processed CMD_SEND_DESIRED_STATE"); - } - - int bodyUniqueId = clientCmd.m_sendDesiredStateCommandArgument.m_bodyUniqueId; - InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); - - if (body && body->m_multiBody) - { - btMultiBody* mb = body->m_multiBody; - btAssert(mb); - - switch (clientCmd.m_sendDesiredStateCommandArgument.m_controlMode) - { - case CONTROL_MODE_TORQUE: - { - if (m_data->m_verboseOutput) - { - b3Printf("Using CONTROL_MODE_TORQUE"); - } - // mb->clearForcesAndTorques(); - int torqueIndex = 6; - if ((clientCmd.m_updateFlags&SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0) - { - for (int link=0;linkgetNumLinks();link++) - { - - for (int dof=0;dofgetLink(link).m_dofCount;dof++) - { - double torque = 0.f; - if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[torqueIndex]&SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0) - { - torque = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[torqueIndex]; - mb->addJointTorqueMultiDof(link,dof,torque); - } - torqueIndex++; - } - } - } - break; - } - case CONTROL_MODE_VELOCITY: - { - if (m_data->m_verboseOutput) - { - b3Printf("Using CONTROL_MODE_VELOCITY"); - } - - int numMotors = 0; - //find the joint motors and apply the desired velocity and maximum force/torque - { - int dofIndex = 6;//skip the 3 linear + 3 angular degree of freedom entries of the base - for (int link=0;linkgetNumLinks();link++) - { - if (supportsJointMotor(mb,link)) + case CONTROL_MODE_POSITION_VELOCITY_PD: { - - btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)mb->getLink(link).m_userPtr; - - - if (motor) - { - btScalar desiredVelocity = 0.f; - bool hasDesiredVelocity = false; - - - if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex]&SIM_DESIRED_STATE_HAS_QDOT)!=0) - { - desiredVelocity = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[dofIndex]; - btScalar kd = 0.1f; - if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] & SIM_DESIRED_STATE_HAS_KD)!=0) - { - kd = clientCmd.m_sendDesiredStateCommandArgument.m_Kd[dofIndex]; - } - - motor->setVelocityTarget(desiredVelocity,kd); - - btScalar kp = 0.f; - motor->setPositionTarget(0,kp); - hasDesiredVelocity = true; - } - if (hasDesiredVelocity) - { - btScalar maxImp = 1000000.f*m_data->m_physicsDeltaTime; - if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex]&SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0) - { - maxImp = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex]*m_data->m_physicsDeltaTime; - } - motor->setMaxAppliedImpulse(maxImp); - } - numMotors++; - - } + con->setServo(limitAxis,true); + con->setServoTarget(limitAxis,qTarget); + //next one is the maximum velocity to reach target position. + //the maximum velocity is limited by maxMotorForce + con->setTargetVelocity(limitAxis, 100); + //this is max motor force impulse + btScalar torqueImpulse = torque*m_data->m_dynamicsWorld->getSolverInfo().m_timeStep; + con->setMaxMotorForce(limitAxis,torqueImpulse); + con->enableMotor(limitAxis,true); + break; } - dofIndex += mb->getLink(link).m_dofCount; - } - } - break; - } - - case CONTROL_MODE_POSITION_VELOCITY_PD: - { - if (m_data->m_verboseOutput) - { - b3Printf("Using CONTROL_MODE_POSITION_VELOCITY_PD"); - } - //compute the force base on PD control - - int numMotors = 0; - //find the joint motors and apply the desired velocity and maximum force/torque - { - int velIndex = 6;//skip the 3 linear + 3 angular degree of freedom velocity entries of the base - int posIndex = 7;//skip 3 positional and 4 orientation (quaternion) positional degrees of freedom of the base - for (int link=0;linkgetNumLinks();link++) - { - if (supportsJointMotor(mb,link)) + default: { - - - btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)mb->getLink(link).m_userPtr; - - if (motor) - { - - bool hasDesiredPosOrVel = false; - btScalar kp = 0.f; - btScalar kd = 0.f; - btScalar desiredVelocity = 0.f; - if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex] & SIM_DESIRED_STATE_HAS_QDOT)!=0) - { - hasDesiredPosOrVel = true; - desiredVelocity = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[velIndex]; - kd = 0.1; - } - btScalar desiredPosition = 0.f; - if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[posIndex] & SIM_DESIRED_STATE_HAS_Q)!=0) - { - hasDesiredPosOrVel = true; - desiredPosition = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[posIndex]; - kp = 0.1; - } - - if (hasDesiredPosOrVel) - { - - if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex] & SIM_DESIRED_STATE_HAS_KP)!=0) - { - kp = clientCmd.m_sendDesiredStateCommandArgument.m_Kp[velIndex]; - } - - if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex] & SIM_DESIRED_STATE_HAS_KD)!=0) - { - kd = clientCmd.m_sendDesiredStateCommandArgument.m_Kd[velIndex]; - } - - motor->setVelocityTarget(desiredVelocity,kd); - motor->setPositionTarget(desiredPosition,kp); - - btScalar maxImp = 1000000.f*m_data->m_physicsDeltaTime; - - if ((clientCmd.m_updateFlags & SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0) - maxImp = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[velIndex]*m_data->m_physicsDeltaTime; - - motor->setMaxAppliedImpulse(maxImp); - } - numMotors++; - } - } - velIndex += mb->getLink(link).m_dofCount; - posIndex += mb->getLink(link).m_posVarCount; - } - } + }; - break; + } } - default: - { - b3Warning("m_controlMode not implemented yet"); - break; - } + }//fi + ///see addJointInfoFromConstraint + velIndex ++;//info.m_uIndex + posIndex ++;//info.m_qIndex - } - } else - { - //support for non-btMultiBody, such as btRigidBody - - if (body && body->m_rigidBody) - { - btRigidBody* rb = body->m_rigidBody; - btAssert(rb); - - //switch (clientCmd.m_sendDesiredStateCommandArgument.m_controlMode) - { - //case CONTROL_MODE_TORQUE: - { - if (m_data->m_verboseOutput) - { - b3Printf("Using CONTROL_MODE_TORQUE"); - } - // mb->clearForcesAndTorques(); - ///see addJointInfoFromConstraint - int velIndex = 6; - int posIndex = 7; - //if ((clientCmd.m_updateFlags&SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0) - { - for (int link=0;linkm_rigidBodyJoints.size();link++) - { - btGeneric6DofSpring2Constraint* con = body->m_rigidBodyJoints[link]; - - btVector3 linearLowerLimit; - btVector3 linearUpperLimit; - btVector3 angularLowerLimit; - btVector3 angularUpperLimit; - - - //for (int dof=0;dofgetLink(link).m_dofCount;dof++) - { - - - { - - int torqueIndex = velIndex; - double torque = 100; - bool hasDesiredTorque = false; - if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex] & SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0) - { - torque = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[velIndex]; - hasDesiredTorque = true; - } - - bool hasDesiredPosOrVel = false; - btScalar qdotTarget = 0.f; - if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex] & SIM_DESIRED_STATE_HAS_QDOT)!=0) - { - hasDesiredPosOrVel = true; - qdotTarget = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[velIndex]; - } - btScalar qTarget = 0.f; - if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[posIndex] & SIM_DESIRED_STATE_HAS_Q)!=0) - { - hasDesiredPosOrVel = true; - qTarget = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[posIndex]; - } - - con->getLinearLowerLimit(linearLowerLimit); - con->getLinearUpperLimit(linearUpperLimit); - con->getAngularLowerLimit(angularLowerLimit); - con->getAngularUpperLimit(angularUpperLimit); - - if (linearLowerLimit.isZero() && linearUpperLimit.isZero() && angularLowerLimit.isZero() && angularUpperLimit.isZero()) - { - //fixed, don't do anything - } else - { - con->calculateTransforms(); - - if (linearLowerLimit.isZero() && linearUpperLimit.isZero()) - { - //eRevoluteType; - btVector3 limitRange = angularLowerLimit.absolute()+angularUpperLimit.absolute(); - int limitAxis = limitRange.maxAxis(); - const btTransform& transA = con->getCalculatedTransformA(); - const btTransform& transB = con->getCalculatedTransformB(); - btVector3 axisA = transA.getBasis().getColumn(limitAxis); - btVector3 axisB = transB.getBasis().getColumn(limitAxis); - - switch (clientCmd.m_sendDesiredStateCommandArgument.m_controlMode) - { - case CONTROL_MODE_TORQUE: - { - if (hasDesiredTorque) - { - con->getRigidBodyA().applyTorque(torque*axisA); - con->getRigidBodyB().applyTorque(-torque*axisB); - } - break; - } - case CONTROL_MODE_VELOCITY: - { - if (hasDesiredPosOrVel) - { - con->enableMotor(3+limitAxis,true); - con->setTargetVelocity(3+limitAxis, qdotTarget); - //this is max motor force impulse - btScalar torqueImpulse = torque*m_data->m_dynamicsWorld->getSolverInfo().m_timeStep; - con->setMaxMotorForce(3+limitAxis,torqueImpulse); - } - break; - } - case CONTROL_MODE_POSITION_VELOCITY_PD: - { - if (hasDesiredPosOrVel) - { - con->setServo(3+limitAxis,true); - con->setServoTarget(3+limitAxis,-qTarget); - //next one is the maximum velocity to reach target position. - //the maximum velocity is limited by maxMotorForce - con->setTargetVelocity(3+limitAxis, 100); - //this is max motor force impulse - btScalar torqueImpulse = torque*m_data->m_dynamicsWorld->getSolverInfo().m_timeStep; - con->setMaxMotorForce(3+limitAxis,torqueImpulse); - con->enableMotor(3+limitAxis,true); - } - break; - } - default: - { - } - }; - - - - - } else - { - //ePrismaticType; @todo - btVector3 limitRange = linearLowerLimit.absolute()+linearUpperLimit.absolute(); - int limitAxis = limitRange.maxAxis(); - - const btTransform& transA = con->getCalculatedTransformA(); - const btTransform& transB = con->getCalculatedTransformB(); - btVector3 axisA = transA.getBasis().getColumn(limitAxis); - btVector3 axisB = transB.getBasis().getColumn(limitAxis); - - switch (clientCmd.m_sendDesiredStateCommandArgument.m_controlMode) - { - case CONTROL_MODE_TORQUE: - { - con->getRigidBodyA().applyForce(-torque*axisA,btVector3(0,0,0)); - con->getRigidBodyB().applyForce(torque*axisB,btVector3(0,0,0)); - break; - } - case CONTROL_MODE_VELOCITY: - { - con->enableMotor(limitAxis,true); - con->setTargetVelocity(limitAxis, -qdotTarget); - //this is max motor force impulse - btScalar torqueImpulse = torque*m_data->m_dynamicsWorld->getSolverInfo().m_timeStep; - con->setMaxMotorForce(limitAxis,torqueImpulse); - break; - } - case CONTROL_MODE_POSITION_VELOCITY_PD: - { - con->setServo(limitAxis,true); - con->setServoTarget(limitAxis,qTarget); - //next one is the maximum velocity to reach target position. - //the maximum velocity is limited by maxMotorForce - con->setTargetVelocity(limitAxis, 100); - //this is max motor force impulse - btScalar torqueImpulse = torque*m_data->m_dynamicsWorld->getSolverInfo().m_timeStep; - con->setMaxMotorForce(limitAxis,torqueImpulse); - con->enableMotor(limitAxis,true); - break; - } - default: - { - } - }; - - } - } - }//fi - ///see addJointInfoFromConstraint - velIndex ++;//info.m_uIndex - posIndex ++;//info.m_qIndex - - } - } - }//fi - //break; - } - - } - } //if (body && body->m_rigidBody) - } - - serverStatusOut.m_type = CMD_DESIRED_STATE_RECEIVED_COMPLETED; - hasStatus = true; - break; - } - case CMD_REQUEST_COLLISION_INFO: - { - SharedMemoryStatus& serverCmd = serverStatusOut; - serverStatusOut.m_type = CMD_REQUEST_COLLISION_INFO_FAILED; - hasStatus=true; - int bodyUniqueId = clientCmd.m_requestCollisionInfoArgs.m_bodyUniqueId; - InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); - - - if (body && body->m_multiBody) - { - btMultiBody* mb = body->m_multiBody; - serverStatusOut.m_type = CMD_REQUEST_COLLISION_INFO_COMPLETED; - serverCmd.m_sendCollisionInfoArgs.m_numLinks = body->m_multiBody->getNumLinks(); - serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[0] = 0; - serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[1] = 0; - serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[2] = 0; - - serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[0] = -1; - serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[1] = -1; - serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[2] = -1; - - if (body->m_multiBody->getBaseCollider()) - { - btTransform tr; - tr.setOrigin(mb->getBasePos()); - tr.setRotation(mb->getWorldToBaseRot().inverse()); - - btVector3 aabbMin,aabbMax; - body->m_multiBody->getBaseCollider()->getCollisionShape()->getAabb(tr,aabbMin,aabbMax); - serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[0] = aabbMin[0]; - serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[1] = aabbMin[1]; - serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[2] = aabbMin[2]; - - serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[0] = aabbMax[0]; - serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[1] = aabbMax[1]; - serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[2] = aabbMax[2]; - } - for (int l=0;lgetNumLinks();l++) - { - serverCmd.m_sendCollisionInfoArgs.m_linkWorldAABBsMin[3*l+0] = 0; - serverCmd.m_sendCollisionInfoArgs.m_linkWorldAABBsMin[3*l+1] = 0; - serverCmd.m_sendCollisionInfoArgs.m_linkWorldAABBsMin[3*l+2] = 0; - - serverCmd.m_sendCollisionInfoArgs.m_linkWorldAABBsMax[3*l+0] = -1; - serverCmd.m_sendCollisionInfoArgs.m_linkWorldAABBsMax[3*l+1] = -1; - serverCmd.m_sendCollisionInfoArgs.m_linkWorldAABBsMax[3*l+2] = -1; - - - - if (body->m_multiBody->getLink(l).m_collider) - { - btVector3 aabbMin,aabbMax; - body->m_multiBody->getLinkCollider(l)->getCollisionShape()->getAabb(mb->getLink(l).m_cachedWorldTransform,aabbMin,aabbMax); - - serverCmd.m_sendCollisionInfoArgs.m_linkWorldAABBsMin[3*l+0] = aabbMin[0]; - serverCmd.m_sendCollisionInfoArgs.m_linkWorldAABBsMin[3*l+1] = aabbMin[1]; - serverCmd.m_sendCollisionInfoArgs.m_linkWorldAABBsMin[3*l+2] = aabbMin[2]; - serverCmd.m_sendCollisionInfoArgs.m_linkWorldAABBsMax[3*l+0] = aabbMax[0]; - serverCmd.m_sendCollisionInfoArgs.m_linkWorldAABBsMax[3*l+1] = aabbMax[1]; - serverCmd.m_sendCollisionInfoArgs.m_linkWorldAABBsMax[3*l+2] = aabbMax[2]; - } - - } - } - else - { - if (body && body->m_rigidBody) - { - btRigidBody* rb = body->m_rigidBody; - SharedMemoryStatus& serverCmd = serverStatusOut; - serverStatusOut.m_type = CMD_REQUEST_COLLISION_INFO_COMPLETED; - serverCmd.m_sendCollisionInfoArgs.m_numLinks = 0; - serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[0] = 0; - serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[1] = 0; - serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[2] = 0; - - serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[0] = -1; - serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[1] = -1; - serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[2] = -1; - if (rb->getCollisionShape()) - { - btTransform tr = rb->getWorldTransform(); - - btVector3 aabbMin,aabbMax; - rb->getCollisionShape()->getAabb(tr,aabbMin,aabbMax); - serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[0] = aabbMin[0]; - serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[1] = aabbMin[1]; - serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[2] = aabbMin[2]; - - serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[0] = aabbMax[0]; - serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[1] = aabbMax[1]; - serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[2] = aabbMax[2]; } } - } - break; + }//fi + //break; } - case CMD_REQUEST_ACTUAL_STATE: - { - BT_PROFILE("CMD_REQUEST_ACTUAL_STATE"); - if (m_data->m_verboseOutput) - { - b3Printf("Sending the actual state (Q,U)"); - } - int bodyUniqueId = clientCmd.m_requestActualStateInformationCommandArgument.m_bodyUniqueId; - InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); - - - if (body && body->m_multiBody) - { - btMultiBody* mb = body->m_multiBody; - SharedMemoryStatus& serverCmd = serverStatusOut; - serverStatusOut.m_type = CMD_ACTUAL_STATE_UPDATE_COMPLETED; - - serverCmd.m_sendActualStateArgs.m_bodyUniqueId = bodyUniqueId; - serverCmd.m_sendActualStateArgs.m_numLinks = body->m_multiBody->getNumLinks(); - - int totalDegreeOfFreedomQ = 0; - int totalDegreeOfFreedomU = 0; - - if (mb->getNumLinks()>= MAX_DEGREE_OF_FREEDOM) - { - serverStatusOut.m_type = CMD_ACTUAL_STATE_UPDATE_FAILED; - hasStatus = true; - break; - } - - //always add the base, even for static (non-moving objects) - //so that we can easily move the 'fixed' base when needed - //do we don't use this conditional "if (!mb->hasFixedBase())" - { - btTransform tr; - tr.setOrigin(mb->getBasePos()); - tr.setRotation(mb->getWorldToBaseRot().inverse()); - - serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[0] = - body->m_rootLocalInertialFrame.getOrigin()[0]; - serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[1] = - body->m_rootLocalInertialFrame.getOrigin()[1]; - serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[2] = - body->m_rootLocalInertialFrame.getOrigin()[2]; - - serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[3] = - body->m_rootLocalInertialFrame.getRotation()[0]; - serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[4] = - body->m_rootLocalInertialFrame.getRotation()[1]; - serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[5] = - body->m_rootLocalInertialFrame.getRotation()[2]; - serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[6] = - body->m_rootLocalInertialFrame.getRotation()[3]; - - - - //base position in world space, carthesian - serverCmd.m_sendActualStateArgs.m_actualStateQ[0] = tr.getOrigin()[0]; - serverCmd.m_sendActualStateArgs.m_actualStateQ[1] = tr.getOrigin()[1]; - serverCmd.m_sendActualStateArgs.m_actualStateQ[2] = tr.getOrigin()[2]; - - //base orientation, quaternion x,y,z,w, in world space, carthesian - serverCmd.m_sendActualStateArgs.m_actualStateQ[3] = tr.getRotation()[0]; - serverCmd.m_sendActualStateArgs.m_actualStateQ[4] = tr.getRotation()[1]; - serverCmd.m_sendActualStateArgs.m_actualStateQ[5] = tr.getRotation()[2]; - serverCmd.m_sendActualStateArgs.m_actualStateQ[6] = tr.getRotation()[3]; - totalDegreeOfFreedomQ +=7;//pos + quaternion - - //base linear velocity (in world space, carthesian) - serverCmd.m_sendActualStateArgs.m_actualStateQdot[0] = mb->getBaseVel()[0]; - serverCmd.m_sendActualStateArgs.m_actualStateQdot[1] = mb->getBaseVel()[1]; - serverCmd.m_sendActualStateArgs.m_actualStateQdot[2] = mb->getBaseVel()[2]; - - //base angular velocity (in world space, carthesian) - serverCmd.m_sendActualStateArgs.m_actualStateQdot[3] = mb->getBaseOmega()[0]; - serverCmd.m_sendActualStateArgs.m_actualStateQdot[4] = mb->getBaseOmega()[1]; - serverCmd.m_sendActualStateArgs.m_actualStateQdot[5] = mb->getBaseOmega()[2]; - totalDegreeOfFreedomU += 6;//3 linear and 3 angular DOF - } - - btAlignedObjectArray omega; - btAlignedObjectArray linVel; - - bool computeForwardKinematics = ((clientCmd.m_updateFlags & ACTUAL_STATE_COMPUTE_FORWARD_KINEMATICS)!=0); - if (computeForwardKinematics) - { - B3_PROFILE("compForwardKinematics"); - btAlignedObjectArray world_to_local; - btAlignedObjectArray local_origin; - world_to_local.resize(mb->getNumLinks()+1); - local_origin.resize(mb->getNumLinks()+1); - mb->forwardKinematics(world_to_local,local_origin); - } - - bool computeLinkVelocities = ((clientCmd.m_updateFlags & ACTUAL_STATE_COMPUTE_LINKVELOCITY)!=0); - if (computeLinkVelocities) - { - omega.resize(mb->getNumLinks()+1); - linVel.resize(mb->getNumLinks()+1); - { - B3_PROFILE("compTreeLinkVelocities"); - mb->compTreeLinkVelocities(&omega[0], &linVel[0]); - } - } - for (int l=0;lgetNumLinks();l++) - { - for (int d=0;dgetLink(l).m_posVarCount;d++) - { - serverCmd.m_sendActualStateArgs.m_actualStateQ[totalDegreeOfFreedomQ++] = mb->getJointPosMultiDof(l)[d]; - } - for (int d=0;dgetLink(l).m_dofCount;d++) - { - serverCmd.m_sendActualStateArgs.m_actualStateQdot[totalDegreeOfFreedomU++] = mb->getJointVelMultiDof(l)[d]; - } - - if (0 == mb->getLink(l).m_jointFeedback) - { - for (int d=0;d<6;d++) - { - serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+d]=0; - } - } else - { - btVector3 sensedForce = mb->getLink(l).m_jointFeedback->m_reactionForces.getLinear(); - btVector3 sensedTorque = mb->getLink(l).m_jointFeedback->m_reactionForces.getAngular(); - - serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+0] = sensedForce[0]; - serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+1] = sensedForce[1]; - serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+2] = sensedForce[2]; - - serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+3] = sensedTorque[0]; - serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+4] = sensedTorque[1]; - serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+5] = sensedTorque[2]; - } - - serverCmd.m_sendActualStateArgs.m_jointMotorForce[l] = 0; - - if (supportsJointMotor(mb,l)) - { - - btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)body->m_multiBody->getLink(l).m_userPtr; - - if (motor && m_data->m_physicsDeltaTime>btScalar(0)) - { - btScalar force =motor->getAppliedImpulse(0)/m_data->m_physicsDeltaTime; - serverCmd.m_sendActualStateArgs.m_jointMotorForce[l] = - force; - //if (force>0) - //{ - // b3Printf("force = %f\n", force); - //} - } - } - btVector3 linkLocalInertialOrigin = body->m_linkLocalInertialFrames[l].getOrigin(); - btQuaternion linkLocalInertialRotation = body->m_linkLocalInertialFrames[l].getRotation(); - - btVector3 linkCOMOrigin = mb->getLink(l).m_cachedWorldTransform.getOrigin(); - btQuaternion linkCOMRotation = mb->getLink(l).m_cachedWorldTransform.getRotation(); - - serverCmd.m_sendActualStateArgs.m_linkState[l*7+0] = linkCOMOrigin.getX(); - serverCmd.m_sendActualStateArgs.m_linkState[l*7+1] = linkCOMOrigin.getY(); - serverCmd.m_sendActualStateArgs.m_linkState[l*7+2] = linkCOMOrigin.getZ(); - serverCmd.m_sendActualStateArgs.m_linkState[l*7+3] = linkCOMRotation.x(); - serverCmd.m_sendActualStateArgs.m_linkState[l*7+4] = linkCOMRotation.y(); - serverCmd.m_sendActualStateArgs.m_linkState[l*7+5] = linkCOMRotation.z(); - serverCmd.m_sendActualStateArgs.m_linkState[l*7+6] = linkCOMRotation.w(); - - - - btVector3 worldLinVel(0,0,0); - btVector3 worldAngVel(0,0,0); - - if (computeLinkVelocities) - { - const btMatrix3x3& linkRotMat = mb->getLink(l).m_cachedWorldTransform.getBasis(); - worldLinVel = linkRotMat * linVel[l+1]; - worldAngVel = linkRotMat * omega[l+1]; - } - - serverCmd.m_sendActualStateArgs.m_linkWorldVelocities[l*6+0] = worldLinVel[0]; - serverCmd.m_sendActualStateArgs.m_linkWorldVelocities[l*6+1] = worldLinVel[1]; - serverCmd.m_sendActualStateArgs.m_linkWorldVelocities[l*6+2] = worldLinVel[2]; - serverCmd.m_sendActualStateArgs.m_linkWorldVelocities[l*6+3] = worldAngVel[0]; - serverCmd.m_sendActualStateArgs.m_linkWorldVelocities[l*6+4] = worldAngVel[1]; - serverCmd.m_sendActualStateArgs.m_linkWorldVelocities[l*6+5] = worldAngVel[2]; - - serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+0] = linkLocalInertialOrigin.getX(); - serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+1] = linkLocalInertialOrigin.getY(); - serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+2] = linkLocalInertialOrigin.getZ(); - - serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+3] = linkLocalInertialRotation.x(); - serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+4] = linkLocalInertialRotation.y(); - serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+5] = linkLocalInertialRotation.z(); - serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+6] = linkLocalInertialRotation.w(); - - } - - - serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomQ = totalDegreeOfFreedomQ; - serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomU = totalDegreeOfFreedomU; - - hasStatus = true; - - } else - { - if (body && body->m_rigidBody) - { - btRigidBody* rb = body->m_rigidBody; - SharedMemoryStatus& serverCmd = serverStatusOut; - serverCmd.m_type = CMD_ACTUAL_STATE_UPDATE_COMPLETED; - - serverCmd.m_sendActualStateArgs.m_bodyUniqueId = bodyUniqueId; - serverCmd.m_sendActualStateArgs.m_numLinks = 0; - - int totalDegreeOfFreedomQ = 0; - int totalDegreeOfFreedomU = 0; - - btTransform tr = rb->getWorldTransform(); - //base position in world space, carthesian - serverCmd.m_sendActualStateArgs.m_actualStateQ[0] = tr.getOrigin()[0]; - serverCmd.m_sendActualStateArgs.m_actualStateQ[1] = tr.getOrigin()[1]; - serverCmd.m_sendActualStateArgs.m_actualStateQ[2] = tr.getOrigin()[2]; - - //base orientation, quaternion x,y,z,w, in world space, carthesian - serverCmd.m_sendActualStateArgs.m_actualStateQ[3] = tr.getRotation()[0]; - serverCmd.m_sendActualStateArgs.m_actualStateQ[4] = tr.getRotation()[1]; - serverCmd.m_sendActualStateArgs.m_actualStateQ[5] = tr.getRotation()[2]; - serverCmd.m_sendActualStateArgs.m_actualStateQ[6] = tr.getRotation()[3]; - totalDegreeOfFreedomQ +=7;//pos + quaternion - - //base linear velocity (in world space, carthesian) - serverCmd.m_sendActualStateArgs.m_actualStateQdot[0] = rb->getLinearVelocity()[0]; - serverCmd.m_sendActualStateArgs.m_actualStateQdot[1] = rb->getLinearVelocity()[1]; - serverCmd.m_sendActualStateArgs.m_actualStateQdot[2] = rb->getLinearVelocity()[2]; - - //base angular velocity (in world space, carthesian) - serverCmd.m_sendActualStateArgs.m_actualStateQdot[3] = rb->getAngularVelocity()[0]; - serverCmd.m_sendActualStateArgs.m_actualStateQdot[4] = rb->getAngularVelocity()[1]; - serverCmd.m_sendActualStateArgs.m_actualStateQdot[5] = rb->getAngularVelocity()[2]; - totalDegreeOfFreedomU += 6;//3 linear and 3 angular DOF - - serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomQ = totalDegreeOfFreedomQ; - serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomU = totalDegreeOfFreedomU; - - hasStatus = true; - } else - { - //b3Warning("Request state but no multibody or rigid body available"); - SharedMemoryStatus& serverCmd = serverStatusOut; - serverCmd.m_type = CMD_ACTUAL_STATE_UPDATE_FAILED; - hasStatus = true; - } - } - - break; - } - case CMD_STEP_FORWARD_SIMULATION: - { - BT_PROFILE("CMD_STEP_FORWARD_SIMULATION"); - - - if (m_data->m_verboseOutput) - { - b3Printf("Step simulation request"); - b3Printf("CMD_STEP_FORWARD_SIMULATION clientCmd = %d\n", clientCmd.m_sequenceNumber); - } - ///todo(erwincoumans) move this damping inside Bullet - for (int i=0;im_dynamicsWorld->getNumMultibodies();i++) - { - btMultiBody* mb = m_data->m_dynamicsWorld->getMultiBody(i); - for (int l=0;lgetNumLinks();l++) - { - for (int d=0;dgetLink(l).m_dofCount;d++) - { - double damping_coefficient = mb->getLink(l).m_jointDamping; - double damping = -damping_coefficient*mb->getJointVelMultiDof(l)[d]; - mb->addJointTorqueMultiDof(l, d, damping); - } - } - } - - btScalar deltaTimeScaled = m_data->m_physicsDeltaTime*simTimeScalingFactor; - - if (m_data->m_numSimulationSubSteps > 0) - { - m_data->m_dynamicsWorld->stepSimulation(deltaTimeScaled, m_data->m_numSimulationSubSteps, m_data->m_physicsDeltaTime / m_data->m_numSimulationSubSteps); - } - else - { - m_data->m_dynamicsWorld->stepSimulation(deltaTimeScaled, 0); - } - - SharedMemoryStatus& serverCmd =serverStatusOut; - serverCmd.m_type = CMD_STEP_FORWARD_SIMULATION_COMPLETED; - hasStatus = true; - break; - } - - case CMD_REQUEST_INTERNAL_DATA: - { - BT_PROFILE("CMD_REQUEST_INTERNAL_DATA"); - - //todo: also check version etc? - - SharedMemoryStatus& serverCmd = serverStatusOut; - serverCmd.m_type = CMD_REQUEST_INTERNAL_DATA_FAILED; - hasStatus = true; - - int sz = btDefaultSerializer::getMemoryDnaSizeInBytes(); - const char* memDna = btDefaultSerializer::getMemoryDna(); - if (sz < bufferSizeInBytes) - { - for (int i = 0; i < sz; i++) - { - bufferServerToClient[i] = memDna[i]; - } - serverCmd.m_type = CMD_REQUEST_INTERNAL_DATA_COMPLETED; - serverCmd.m_numDataStreamBytes = sz; - } - - break; - }; - case CMD_CHANGE_DYNAMICS_INFO: - { - BT_PROFILE("CMD_CHANGE_DYNAMICS_INFO"); - - int bodyUniqueId = clientCmd.m_changeDynamicsInfoArgs.m_bodyUniqueId; - int linkIndex = clientCmd.m_changeDynamicsInfoArgs.m_linkIndex; - double mass = clientCmd.m_changeDynamicsInfoArgs.m_mass; - double lateralFriction = clientCmd.m_changeDynamicsInfoArgs.m_lateralFriction; - double spinningFriction = clientCmd.m_changeDynamicsInfoArgs.m_spinningFriction; - double rollingFriction = clientCmd.m_changeDynamicsInfoArgs.m_rollingFriction; - double restitution = clientCmd.m_changeDynamicsInfoArgs.m_restitution; - btAssert(bodyUniqueId >= 0); - - InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); - - if (body && body->m_multiBody) - { - btMultiBody* mb = body->m_multiBody; - - if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LINEAR_DAMPING) - { - mb->setLinearDamping(clientCmd.m_changeDynamicsInfoArgs.m_linearDamping); - } - if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ANGULAR_DAMPING) - { - mb->setAngularDamping(clientCmd.m_changeDynamicsInfoArgs.m_angularDamping); - } - - if (linkIndex == -1) - { - if (mb->getBaseCollider()) - { - if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_RESTITUTION) - { - mb->getBaseCollider()->setRestitution(restitution); - } - - - if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_CONTACT_STIFFNESS_AND_DAMPING) - { - mb->getBaseCollider()->setContactStiffnessAndDamping(clientCmd.m_changeDynamicsInfoArgs.m_contactStiffness, clientCmd.m_changeDynamicsInfoArgs.m_contactDamping); - } - if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LATERAL_FRICTION) - { - mb->getBaseCollider()->setFriction(lateralFriction); - } - if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_SPINNING_FRICTION) - { - mb->getBaseCollider()->setSpinningFriction(spinningFriction); - } - if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ROLLING_FRICTION) - { - mb->getBaseCollider()->setRollingFriction(rollingFriction); - } - - if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_FRICTION_ANCHOR) - { - if (clientCmd.m_changeDynamicsInfoArgs.m_frictionAnchor) - { - mb->getBaseCollider()->setCollisionFlags(mb->getBaseCollider()->getCollisionFlags() | btCollisionObject::CF_HAS_FRICTION_ANCHOR); - } else - { - mb->getBaseCollider()->setCollisionFlags(mb->getBaseCollider()->getCollisionFlags() & ~btCollisionObject::CF_HAS_FRICTION_ANCHOR); - } - } - } - if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_MASS) - { - mb->setBaseMass(mass); - if (mb->getBaseCollider() && mb->getBaseCollider()->getCollisionShape()) - { - btVector3 localInertia; - mb->getBaseCollider()->getCollisionShape()->calculateLocalInertia(mass,localInertia); - mb->setBaseInertia(localInertia); - } - } - } - else - { - if (linkIndex >= 0 && linkIndex < mb->getNumLinks()) - { - if (mb->getLinkCollider(linkIndex)) - { - if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_RESTITUTION) - { - mb->getLinkCollider(linkIndex)->setRestitution(restitution); - } - if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_SPINNING_FRICTION) - { - mb->getLinkCollider(linkIndex)->setSpinningFriction(spinningFriction); - } - if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ROLLING_FRICTION) - { - mb->getLinkCollider(linkIndex)->setRollingFriction(rollingFriction); - } - - if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_FRICTION_ANCHOR) - { - if (clientCmd.m_changeDynamicsInfoArgs.m_frictionAnchor) - { - mb->getLinkCollider(linkIndex)->setCollisionFlags(mb->getLinkCollider(linkIndex)->getCollisionFlags() | btCollisionObject::CF_HAS_FRICTION_ANCHOR); - } else - { - mb->getLinkCollider(linkIndex)->setCollisionFlags(mb->getLinkCollider(linkIndex)->getCollisionFlags() & ~btCollisionObject::CF_HAS_FRICTION_ANCHOR); - } - } - - - if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LATERAL_FRICTION) - { - mb->getLinkCollider(linkIndex)->setFriction(lateralFriction); - } - - if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_CONTACT_STIFFNESS_AND_DAMPING) - { - mb->getLinkCollider(linkIndex)->setContactStiffnessAndDamping(clientCmd.m_changeDynamicsInfoArgs.m_contactStiffness, clientCmd.m_changeDynamicsInfoArgs.m_contactDamping); - } - - - } - if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_MASS) - { - mb->getLink(linkIndex).m_mass = mass; - if (mb->getLinkCollider(linkIndex) && mb->getLinkCollider(linkIndex)->getCollisionShape()) - { - btVector3 localInertia; - mb->getLinkCollider(linkIndex)->getCollisionShape()->calculateLocalInertia(mass,localInertia); - mb->getLink(linkIndex).m_inertiaLocal = localInertia; - } - } - } - } - } else - { - if (body && body->m_rigidBody) - { - if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LINEAR_DAMPING) - { - btScalar angDamping = body->m_rigidBody->getAngularDamping(); - body->m_rigidBody->setDamping(clientCmd.m_changeDynamicsInfoArgs.m_linearDamping,angDamping); - } - if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ANGULAR_DAMPING) - { - btScalar linDamping = body->m_rigidBody->getLinearDamping(); - body->m_rigidBody->setDamping(linDamping, clientCmd.m_changeDynamicsInfoArgs.m_angularDamping); - } - - if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_CONTACT_STIFFNESS_AND_DAMPING) - { - body->m_rigidBody->setContactStiffnessAndDamping(clientCmd.m_changeDynamicsInfoArgs.m_contactStiffness, clientCmd.m_changeDynamicsInfoArgs.m_contactDamping); - } - if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_RESTITUTION) - { - body->m_rigidBody->setRestitution(restitution); - } - if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LATERAL_FRICTION) - { - body->m_rigidBody->setFriction(lateralFriction); - } - if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_SPINNING_FRICTION) - { - body->m_rigidBody->setSpinningFriction(spinningFriction); - } - if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ROLLING_FRICTION) - { - body->m_rigidBody->setRollingFriction(rollingFriction); - } - - if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_FRICTION_ANCHOR) - { - if (clientCmd.m_changeDynamicsInfoArgs.m_frictionAnchor) - { - body->m_rigidBody->setCollisionFlags(body->m_rigidBody->getCollisionFlags() | btCollisionObject::CF_HAS_FRICTION_ANCHOR); - } else - { - body->m_rigidBody->setCollisionFlags(body->m_rigidBody->getCollisionFlags() & ~btCollisionObject::CF_HAS_FRICTION_ANCHOR); - } - } - - if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_MASS) - { - btVector3 localInertia; - if (body->m_rigidBody->getCollisionShape()) - { - body->m_rigidBody->getCollisionShape()->calculateLocalInertia(mass,localInertia); - } - body->m_rigidBody->setMassProps(mass,localInertia); - } - } - } - - - SharedMemoryStatus& serverCmd =serverStatusOut; - serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED; - hasStatus = true; - - break; - }; - case CMD_GET_DYNAMICS_INFO: - { - int bodyUniqueId = clientCmd.m_getDynamicsInfoArgs.m_bodyUniqueId; - int linkIndex = clientCmd.m_getDynamicsInfoArgs.m_linkIndex; - InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); - if (body && body->m_multiBody) - { - SharedMemoryStatus& serverCmd = serverStatusOut; - serverCmd.m_type = CMD_GET_DYNAMICS_INFO_COMPLETED; - - btMultiBody* mb = body->m_multiBody; - if (linkIndex == -1) - { - serverCmd.m_dynamicsInfo.m_mass = mb->getBaseMass(); - serverCmd.m_dynamicsInfo.m_lateralFrictionCoeff = mb->getBaseCollider()->getFriction(); - } - else - { - serverCmd.m_dynamicsInfo.m_mass = mb->getLinkMass(linkIndex); - if (mb->getLinkCollider(linkIndex)) - { - serverCmd.m_dynamicsInfo.m_lateralFrictionCoeff = mb->getLinkCollider(linkIndex)->getFriction(); - } - else - { - b3Warning("The dynamic info requested is not available"); - serverCmd.m_type = CMD_GET_DYNAMICS_INFO_FAILED; - } - } - hasStatus = true; - } - else - { - b3Warning("The dynamic info requested is not available"); - SharedMemoryStatus& serverCmd = serverStatusOut; - serverCmd.m_type = CMD_GET_DYNAMICS_INFO_FAILED; - hasStatus = true; - } - break; - } - - case CMD_REQUEST_PHYSICS_SIMULATION_PARAMETERS: - { - SharedMemoryStatus& serverCmd =serverStatusOut; - serverCmd.m_type = CMD_REQUEST_PHYSICS_SIMULATION_PARAMETERS_COMPLETED; - serverCmd.m_simulationParameterResultArgs.m_collisionFilterMode = m_data->m_broadphaseCollisionFilterCallback->m_filterMode; - serverCmd.m_simulationParameterResultArgs.m_contactBreakingThreshold = gContactBreakingThreshold; - serverCmd.m_simulationParameterResultArgs.m_defaultContactERP = m_data->m_dynamicsWorld->getSolverInfo().m_erp2; - serverCmd.m_simulationParameterResultArgs.m_defaultNonContactERP = m_data->m_dynamicsWorld->getSolverInfo().m_erp; - serverCmd.m_simulationParameterResultArgs.m_deltaTime = m_data->m_physicsDeltaTime; - serverCmd.m_simulationParameterResultArgs.m_enableFileCaching = b3IsFileCachingEnabled(); - serverCmd.m_simulationParameterResultArgs.m_frictionERP = m_data->m_dynamicsWorld->getSolverInfo().m_frictionERP; - btVector3 grav = m_data->m_dynamicsWorld->getGravity(); - serverCmd.m_simulationParameterResultArgs.m_gravityAcceleration[0] = grav[0]; - serverCmd.m_simulationParameterResultArgs.m_gravityAcceleration[1] = grav[1]; - serverCmd.m_simulationParameterResultArgs.m_gravityAcceleration[2] = grav[2]; - serverCmd.m_simulationParameterResultArgs.m_internalSimFlags = gInternalSimFlags; - serverCmd.m_simulationParameterResultArgs.m_numSimulationSubSteps = m_data->m_numSimulationSubSteps; - serverCmd.m_simulationParameterResultArgs.m_numSolverIterations = m_data->m_dynamicsWorld->getSolverInfo().m_numIterations; - serverCmd.m_simulationParameterResultArgs.m_restitutionVelocityThreshold = m_data->m_dynamicsWorld->getSolverInfo().m_restitutionVelocityThreshold; - serverCmd.m_simulationParameterResultArgs.m_splitImpulsePenetrationThreshold = m_data->m_dynamicsWorld->getSolverInfo().m_splitImpulsePenetrationThreshold; - serverCmd.m_simulationParameterResultArgs.m_useRealTimeSimulation = m_data->m_useRealTimeSimulation; - serverCmd.m_simulationParameterResultArgs.m_useSplitImpulse = m_data->m_dynamicsWorld->getSolverInfo().m_splitImpulse; - hasStatus = true; - break; - } - - case CMD_SEND_PHYSICS_SIMULATION_PARAMETERS: - { - BT_PROFILE("CMD_SEND_PHYSICS_SIMULATION_PARAMETERS"); - - if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_DELTA_TIME) - { - m_data->m_physicsDeltaTime = clientCmd.m_physSimParamArgs.m_deltaTime; - } - if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_REAL_TIME_SIMULATION) - { - m_data->m_useRealTimeSimulation = (clientCmd.m_physSimParamArgs.m_useRealTimeSimulation!=0); - } - - //see - if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_INTERNAL_SIMULATION_FLAGS) - { - //these flags are for internal/temporary/easter-egg/experimental demo purposes, use at own risk - gInternalSimFlags = clientCmd.m_physSimParamArgs.m_internalSimFlags; - } - - if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_GRAVITY) - { - btVector3 grav(clientCmd.m_physSimParamArgs.m_gravityAcceleration[0], - clientCmd.m_physSimParamArgs.m_gravityAcceleration[1], - clientCmd.m_physSimParamArgs.m_gravityAcceleration[2]); - this->m_data->m_dynamicsWorld->setGravity(grav); - if (m_data->m_verboseOutput) - { - b3Printf("Updated Gravity: %f,%f,%f",grav[0],grav[1],grav[2]); - } - - } - if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_NUM_SOLVER_ITERATIONS) - { - m_data->m_dynamicsWorld->getSolverInfo().m_numIterations = clientCmd.m_physSimParamArgs.m_numSolverIterations; - } - if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_CONTACT_BREAKING_THRESHOLD) - { - gContactBreakingThreshold = clientCmd.m_physSimParamArgs.m_contactBreakingThreshold; - } - - if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_COLLISION_FILTER_MODE) - { - m_data->m_broadphaseCollisionFilterCallback->m_filterMode = clientCmd.m_physSimParamArgs.m_collisionFilterMode; - } - - if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_USE_SPLIT_IMPULSE) - { - m_data->m_dynamicsWorld->getSolverInfo().m_splitImpulse = clientCmd.m_physSimParamArgs.m_useSplitImpulse; - } - if (clientCmd.m_updateFlags &SIM_PARAM_UPDATE_SPLIT_IMPULSE_PENETRATION_THRESHOLD) - { - m_data->m_dynamicsWorld->getSolverInfo().m_splitImpulsePenetrationThreshold = clientCmd.m_physSimParamArgs.m_splitImpulsePenetrationThreshold; - } - - - if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_NUM_SIMULATION_SUB_STEPS) - { - m_data->m_numSimulationSubSteps = clientCmd.m_physSimParamArgs.m_numSimulationSubSteps; - } - - if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_DEFAULT_CONTACT_ERP) - { - m_data->m_dynamicsWorld->getSolverInfo().m_erp2 = clientCmd.m_physSimParamArgs.m_defaultContactERP; - } - - if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_DEFAULT_NON_CONTACT_ERP) - { - m_data->m_dynamicsWorld->getSolverInfo().m_erp = clientCmd.m_physSimParamArgs.m_defaultNonContactERP; - } - - if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_DEFAULT_FRICTION_ERP) - { - m_data->m_dynamicsWorld->getSolverInfo().m_frictionERP = clientCmd.m_physSimParamArgs.m_frictionERP; - } - - - if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_RESTITUTION_VELOCITY_THRESHOLD) - { - m_data->m_dynamicsWorld->getSolverInfo().m_restitutionVelocityThreshold = clientCmd.m_physSimParamArgs.m_restitutionVelocityThreshold; - } - - - - if (clientCmd.m_updateFlags&SIM_PARAM_ENABLE_FILE_CACHING) - { - b3EnableFileCaching(clientCmd.m_physSimParamArgs.m_enableFileCaching); - } - - - SharedMemoryStatus& serverCmd =serverStatusOut; - serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED; - hasStatus = true; - break; - - }; - case CMD_INIT_POSE: - { - BT_PROFILE("CMD_INIT_POSE"); - - if (m_data->m_verboseOutput) - { - b3Printf("Server Init Pose not implemented yet"); - } - int bodyUniqueId = clientCmd.m_initPoseArgs.m_bodyUniqueId; - InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); - - btVector3 baseLinVel(0, 0, 0); - btVector3 baseAngVel(0, 0, 0); - - if (clientCmd.m_updateFlags & INIT_POSE_HAS_BASE_LINEAR_VELOCITY) - { - baseLinVel.setValue(clientCmd.m_initPoseArgs.m_initialStateQdot[0], - clientCmd.m_initPoseArgs.m_initialStateQdot[1], - clientCmd.m_initPoseArgs.m_initialStateQdot[2]); - } - if (clientCmd.m_updateFlags & INIT_POSE_HAS_BASE_ANGULAR_VELOCITY) - { - baseAngVel.setValue(clientCmd.m_initPoseArgs.m_initialStateQdot[3], - clientCmd.m_initPoseArgs.m_initialStateQdot[4], - clientCmd.m_initPoseArgs.m_initialStateQdot[5]); - } - btVector3 basePos(0, 0, 0); - if (clientCmd.m_updateFlags & INIT_POSE_HAS_INITIAL_POSITION) - { - basePos = btVector3( - clientCmd.m_initPoseArgs.m_initialStateQ[0], - clientCmd.m_initPoseArgs.m_initialStateQ[1], - clientCmd.m_initPoseArgs.m_initialStateQ[2]); - } - btQuaternion baseOrn(0, 0, 0, 1); - if (clientCmd.m_updateFlags & INIT_POSE_HAS_INITIAL_ORIENTATION) - { - baseOrn.setValue(clientCmd.m_initPoseArgs.m_initialStateQ[3], - clientCmd.m_initPoseArgs.m_initialStateQ[4], - clientCmd.m_initPoseArgs.m_initialStateQ[5], - clientCmd.m_initPoseArgs.m_initialStateQ[6]); - } - if (body && body->m_multiBody) - { - btMultiBody* mb = body->m_multiBody; - - - - if (clientCmd.m_updateFlags & INIT_POSE_HAS_BASE_LINEAR_VELOCITY) - { - mb->setBaseVel(baseLinVel); - } - - if (clientCmd.m_updateFlags & INIT_POSE_HAS_BASE_ANGULAR_VELOCITY) - { - mb->setBaseOmega(baseAngVel); - } - - - if (clientCmd.m_updateFlags & INIT_POSE_HAS_INITIAL_POSITION) - { - btVector3 zero(0,0,0); - btAssert(clientCmd.m_initPoseArgs.m_hasInitialStateQ[0] && - clientCmd.m_initPoseArgs.m_hasInitialStateQ[1] && - clientCmd.m_initPoseArgs.m_hasInitialStateQ[2]); - - mb->setBaseVel(baseLinVel); - mb->setBasePos(basePos); - } - if (clientCmd.m_updateFlags & INIT_POSE_HAS_INITIAL_ORIENTATION) - { - btAssert(clientCmd.m_initPoseArgs.m_hasInitialStateQ[3] && - clientCmd.m_initPoseArgs.m_hasInitialStateQ[4] && - clientCmd.m_initPoseArgs.m_hasInitialStateQ[5] && - clientCmd.m_initPoseArgs.m_hasInitialStateQ[6]); - - mb->setBaseOmega(baseAngVel); - btQuaternion invOrn(baseOrn); - - mb->setWorldToBaseRot(invOrn.inverse()); - } - if (clientCmd.m_updateFlags & INIT_POSE_HAS_JOINT_STATE) - { - int uDofIndex = 6; - int posVarCountIndex = 7; - for (int i=0;igetNumLinks();i++) - { - if ( (clientCmd.m_initPoseArgs.m_hasInitialStateQ[posVarCountIndex]) && (mb->getLink(i).m_dofCount==1)) - { - mb->setJointPos(i,clientCmd.m_initPoseArgs.m_initialStateQ[posVarCountIndex]); - mb->setJointVel(i,0);//backwards compatibility - } - if ((clientCmd.m_initPoseArgs.m_hasInitialStateQdot[uDofIndex]) && (mb->getLink(i).m_dofCount==1)) - { - btScalar vel = clientCmd.m_initPoseArgs.m_initialStateQdot[uDofIndex]; - mb->setJointVel(i,vel); - } - - posVarCountIndex += mb->getLink(i).m_posVarCount; - uDofIndex += mb->getLink(i).m_dofCount; - - } - } - - btAlignedObjectArray scratch_q; - btAlignedObjectArray scratch_m; - - mb->forwardKinematics(scratch_q,scratch_m); - int nLinks = mb->getNumLinks(); - scratch_q.resize(nLinks+1); - scratch_m.resize(nLinks+1); - - mb->updateCollisionObjectWorldTransforms(scratch_q,scratch_m); - - - } - - if (body && body->m_rigidBody) - { - if (clientCmd.m_updateFlags & INIT_POSE_HAS_BASE_LINEAR_VELOCITY) - { - body->m_rigidBody->setLinearVelocity(baseLinVel); - } - if (clientCmd.m_updateFlags & INIT_POSE_HAS_BASE_ANGULAR_VELOCITY) - { - body->m_rigidBody->setAngularVelocity(baseAngVel); - } - - if (clientCmd.m_updateFlags & INIT_POSE_HAS_INITIAL_POSITION) - { - body->m_rigidBody->getWorldTransform().setOrigin(basePos); - body->m_rigidBody->setLinearVelocity(baseLinVel); - } - - if (clientCmd.m_updateFlags & INIT_POSE_HAS_INITIAL_ORIENTATION) - { - body->m_rigidBody->getWorldTransform().setRotation(baseOrn); - body->m_rigidBody->setAngularVelocity(baseAngVel); - } - - } - - SharedMemoryStatus& serverCmd =serverStatusOut; - serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED; - hasStatus = true; - - break; - } - - - case CMD_RESET_SIMULATION: - { - - BT_PROFILE("CMD_RESET_SIMULATION"); - m_data->m_guiHelper->setVisualizerFlag(COV_ENABLE_SYNC_RENDERING_INTERNAL,0); - resetSimulation(); - m_data->m_guiHelper->setVisualizerFlag(COV_ENABLE_SYNC_RENDERING_INTERNAL,1); - - SharedMemoryStatus& serverCmd =serverStatusOut; - serverCmd.m_type = CMD_RESET_SIMULATION_COMPLETED; - hasStatus = true; - break; - } - case CMD_CREATE_RIGID_BODY: - case CMD_CREATE_BOX_COLLISION_SHAPE: - { - BT_PROFILE("CMD_CREATE_RIGID_BODY"); - - btVector3 halfExtents(1,1,1); - if (clientCmd.m_updateFlags & BOX_SHAPE_HAS_HALF_EXTENTS) - { - halfExtents = btVector3( - clientCmd.m_createBoxShapeArguments.m_halfExtentsX, - clientCmd.m_createBoxShapeArguments.m_halfExtentsY, - clientCmd.m_createBoxShapeArguments.m_halfExtentsZ); - } - btTransform startTrans; - startTrans.setIdentity(); - if (clientCmd.m_updateFlags & BOX_SHAPE_HAS_INITIAL_POSITION) - { - startTrans.setOrigin(btVector3( - clientCmd.m_createBoxShapeArguments.m_initialPosition[0], - clientCmd.m_createBoxShapeArguments.m_initialPosition[1], - clientCmd.m_createBoxShapeArguments.m_initialPosition[2])); - } - - if (clientCmd.m_updateFlags & BOX_SHAPE_HAS_INITIAL_ORIENTATION) - { - - startTrans.setRotation(btQuaternion( - clientCmd.m_createBoxShapeArguments.m_initialOrientation[0], - clientCmd.m_createBoxShapeArguments.m_initialOrientation[1], - clientCmd.m_createBoxShapeArguments.m_initialOrientation[2], - clientCmd.m_createBoxShapeArguments.m_initialOrientation[3])); - } - - btScalar mass = 0.f; - if (clientCmd.m_updateFlags & BOX_SHAPE_HAS_MASS) - { - mass = clientCmd.m_createBoxShapeArguments.m_mass; - } - - int shapeType = COLLISION_SHAPE_TYPE_BOX; - - if (clientCmd.m_updateFlags & BOX_SHAPE_HAS_COLLISION_SHAPE_TYPE) - { - shapeType = clientCmd.m_createBoxShapeArguments.m_collisionShapeType; - } - - btBulletWorldImporter* worldImporter = new btBulletWorldImporter(m_data->m_dynamicsWorld); - m_data->m_worldImporters.push_back(worldImporter); - - btCollisionShape* shape = 0; - - switch (shapeType) - { - case COLLISION_SHAPE_TYPE_CYLINDER_X: - { - btScalar radius = halfExtents[1]; - btScalar height = halfExtents[0]; - shape = worldImporter->createCylinderShapeX(radius,height); - break; - } - case COLLISION_SHAPE_TYPE_CYLINDER_Y: - { - btScalar radius = halfExtents[0]; - btScalar height = halfExtents[1]; - shape = worldImporter->createCylinderShapeY(radius,height); - break; - } - case COLLISION_SHAPE_TYPE_CYLINDER_Z: - { - btScalar radius = halfExtents[1]; - btScalar height = halfExtents[2]; - shape = worldImporter->createCylinderShapeZ(radius,height); - break; - } - case COLLISION_SHAPE_TYPE_CAPSULE_X: - { - btScalar radius = halfExtents[1]; - btScalar height = halfExtents[0]; - shape = worldImporter->createCapsuleShapeX(radius,height); - break; - } - case COLLISION_SHAPE_TYPE_CAPSULE_Y: - { - btScalar radius = halfExtents[0]; - btScalar height = halfExtents[1]; - shape = worldImporter->createCapsuleShapeY(radius,height); - break; - } - case COLLISION_SHAPE_TYPE_CAPSULE_Z: - { - btScalar radius = halfExtents[1]; - btScalar height = halfExtents[2]; - shape = worldImporter->createCapsuleShapeZ(radius,height); - break; - } - case COLLISION_SHAPE_TYPE_SPHERE: - { - btScalar radius = halfExtents[0]; - shape = worldImporter->createSphereShape(radius); - break; - } - case COLLISION_SHAPE_TYPE_BOX: - default: - { - shape = worldImporter->createBoxShape(halfExtents); - } - } - - - bool isDynamic = (mass>0); - btRigidBody* rb = worldImporter->createRigidBody(isDynamic,mass,startTrans,shape,0); - //m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld); - btVector4 colorRGBA(1,0,0,1); - if (clientCmd.m_updateFlags & BOX_SHAPE_HAS_COLOR) - { - colorRGBA[0] = clientCmd.m_createBoxShapeArguments.m_colorRGBA[0]; - colorRGBA[1] = clientCmd.m_createBoxShapeArguments.m_colorRGBA[1]; - colorRGBA[2] = clientCmd.m_createBoxShapeArguments.m_colorRGBA[2]; - colorRGBA[3] = clientCmd.m_createBoxShapeArguments.m_colorRGBA[3]; - } - m_data->m_guiHelper->createCollisionShapeGraphicsObject(rb->getCollisionShape()); - m_data->m_guiHelper->createCollisionObjectGraphicsObject(rb,colorRGBA); - - - SharedMemoryStatus& serverCmd =serverStatusOut; - serverCmd.m_type = CMD_RIGID_BODY_CREATION_COMPLETED; - - - int bodyUniqueId = m_data->m_bodyHandles.allocHandle(); - InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(bodyUniqueId); - serverCmd.m_rigidBodyCreateArgs.m_bodyUniqueId = bodyUniqueId; - rb->setUserIndex2(bodyUniqueId); - bodyHandle->m_rootLocalInertialFrame.setIdentity(); - bodyHandle->m_rigidBody = rb; - hasStatus = true; - break; - } - case CMD_PICK_BODY: - { - BT_PROFILE("CMD_PICK_BODY"); - - pickBody(btVector3(clientCmd.m_pickBodyArguments.m_rayFromWorld[0], - clientCmd.m_pickBodyArguments.m_rayFromWorld[1], - clientCmd.m_pickBodyArguments.m_rayFromWorld[2]), - btVector3(clientCmd.m_pickBodyArguments.m_rayToWorld[0], - clientCmd.m_pickBodyArguments.m_rayToWorld[1], - clientCmd.m_pickBodyArguments.m_rayToWorld[2])); - - - SharedMemoryStatus& serverCmd =serverStatusOut; - serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED; - hasStatus = true; - break; - } - case CMD_MOVE_PICKED_BODY: - { - BT_PROFILE("CMD_MOVE_PICKED_BODY"); - - movePickedBody(btVector3(clientCmd.m_pickBodyArguments.m_rayFromWorld[0], - clientCmd.m_pickBodyArguments.m_rayFromWorld[1], - clientCmd.m_pickBodyArguments.m_rayFromWorld[2]), - btVector3(clientCmd.m_pickBodyArguments.m_rayToWorld[0], - clientCmd.m_pickBodyArguments.m_rayToWorld[1], - clientCmd.m_pickBodyArguments.m_rayToWorld[2])); - - SharedMemoryStatus& serverCmd =serverStatusOut; - serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED; - hasStatus = true; - break; - } - case CMD_REMOVE_PICKING_CONSTRAINT_BODY: - { - BT_PROFILE("CMD_REMOVE_PICKING_CONSTRAINT_BODY"); - removePickingConstraint(); - - SharedMemoryStatus& serverCmd =serverStatusOut; - serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED; - hasStatus = true; - break; - } - case CMD_REQUEST_AABB_OVERLAP: - { - BT_PROFILE("CMD_REQUEST_AABB_OVERLAP"); - SharedMemoryStatus& serverCmd = serverStatusOut; - int curObjectIndex = clientCmd.m_requestOverlappingObjectsArgs.m_startingOverlappingObjectIndex; - - if (0== curObjectIndex) - { - //clientCmd.m_requestContactPointArguments.m_aabbQueryMin - btVector3 aabbMin, aabbMax; - aabbMin.setValue(clientCmd.m_requestOverlappingObjectsArgs.m_aabbQueryMin[0], - clientCmd.m_requestOverlappingObjectsArgs.m_aabbQueryMin[1], - clientCmd.m_requestOverlappingObjectsArgs.m_aabbQueryMin[2]); - aabbMax.setValue(clientCmd.m_requestOverlappingObjectsArgs.m_aabbQueryMax[0], - clientCmd.m_requestOverlappingObjectsArgs.m_aabbQueryMax[1], - clientCmd.m_requestOverlappingObjectsArgs.m_aabbQueryMax[2]); - - m_data->m_cachedOverlappingObjects.clear(); - - m_data->m_dynamicsWorld->getBroadphase()->aabbTest(aabbMin, aabbMax, m_data->m_cachedOverlappingObjects); - } - - - int totalBytesPerObject = sizeof(b3OverlappingObject); - int overlapCapacity = bufferSizeInBytes / totalBytesPerObject - 1; - int numOverlap = m_data->m_cachedOverlappingObjects.m_bodyUniqueIds.size(); - int remainingObjects = numOverlap - curObjectIndex; - - int curNumObjects = btMin(overlapCapacity, remainingObjects); - - if (numOverlap < overlapCapacity) - { - - b3OverlappingObject* overlapStorage = (b3OverlappingObject*)bufferServerToClient; - for (int i = 0; i < m_data->m_cachedOverlappingObjects.m_bodyUniqueIds.size(); i++) - { - overlapStorage[i].m_objectUniqueId = m_data->m_cachedOverlappingObjects.m_bodyUniqueIds[i]; - overlapStorage[i].m_linkIndex = m_data->m_cachedOverlappingObjects.m_links[i]; - } - - serverCmd.m_type = CMD_REQUEST_AABB_OVERLAP_COMPLETED; - - //int m_startingOverlappingObjectIndex; - //int m_numOverlappingObjectsCopied; - //int m_numRemainingOverlappingObjects; - serverCmd.m_sendOverlappingObjectsArgs.m_startingOverlappingObjectIndex = clientCmd.m_requestOverlappingObjectsArgs.m_startingOverlappingObjectIndex; - serverCmd.m_sendOverlappingObjectsArgs.m_numOverlappingObjectsCopied = m_data->m_cachedOverlappingObjects.m_bodyUniqueIds.size(); - serverCmd.m_sendOverlappingObjectsArgs.m_numRemainingOverlappingObjects = remainingObjects - curNumObjects; - } - else - { - serverCmd.m_type = CMD_REQUEST_AABB_OVERLAP_FAILED; - } - - hasStatus = true; - break; - } - - case CMD_REQUEST_OPENGL_VISUALIZER_CAMERA: - { - BT_PROFILE("CMD_REQUEST_OPENGL_VISUALIZER_CAMERA"); - SharedMemoryStatus& serverCmd = serverStatusOut; - bool result = this->m_data->m_guiHelper->getCameraInfo( - &serverCmd.m_visualizerCameraResultArgs.m_width, - &serverCmd.m_visualizerCameraResultArgs.m_height, - serverCmd.m_visualizerCameraResultArgs.m_viewMatrix, - serverCmd.m_visualizerCameraResultArgs.m_projectionMatrix, - serverCmd.m_visualizerCameraResultArgs.m_camUp, - serverCmd.m_visualizerCameraResultArgs.m_camForward, - serverCmd.m_visualizerCameraResultArgs.m_horizontal, - serverCmd.m_visualizerCameraResultArgs.m_vertical, - &serverCmd.m_visualizerCameraResultArgs.m_yaw, - &serverCmd.m_visualizerCameraResultArgs.m_pitch, - &serverCmd.m_visualizerCameraResultArgs.m_dist, - serverCmd.m_visualizerCameraResultArgs.m_target); - serverCmd.m_type = result ? CMD_REQUEST_OPENGL_VISUALIZER_CAMERA_COMPLETED: CMD_REQUEST_OPENGL_VISUALIZER_CAMERA_FAILED; - hasStatus = true; - break; - } - - case CMD_CONFIGURE_OPENGL_VISUALIZER: - { - BT_PROFILE("CMD_CONFIGURE_OPENGL_VISUALIZER"); - SharedMemoryStatus& serverCmd = serverStatusOut; - serverCmd.m_type =CMD_CLIENT_COMMAND_COMPLETED; - - hasStatus = true; - - if (clientCmd.m_updateFlags&COV_SET_FLAGS) - { - if (clientCmd.m_configureOpenGLVisualizerArguments.m_setFlag == COV_ENABLE_TINY_RENDERER) - { - m_data->m_enableTinyRenderer = clientCmd.m_configureOpenGLVisualizerArguments.m_setEnabled!=0; - } - m_data->m_guiHelper->setVisualizerFlag(clientCmd.m_configureOpenGLVisualizerArguments.m_setFlag, - clientCmd.m_configureOpenGLVisualizerArguments.m_setEnabled); - } - if (clientCmd.m_updateFlags&COV_SET_CAMERA_VIEW_MATRIX) - { - m_data->m_guiHelper->resetCamera( clientCmd.m_configureOpenGLVisualizerArguments.m_cameraDistance, - clientCmd.m_configureOpenGLVisualizerArguments.m_cameraYaw, - clientCmd.m_configureOpenGLVisualizerArguments.m_cameraPitch, - clientCmd.m_configureOpenGLVisualizerArguments.m_cameraTargetPosition[0], - clientCmd.m_configureOpenGLVisualizerArguments.m_cameraTargetPosition[1], - clientCmd.m_configureOpenGLVisualizerArguments.m_cameraTargetPosition[2]); - } - break; - } - - case CMD_REQUEST_CONTACT_POINT_INFORMATION: - { - BT_PROFILE("CMD_REQUEST_CONTACT_POINT_INFORMATION"); - SharedMemoryStatus& serverCmd =serverStatusOut; - serverCmd.m_sendContactPointArgs.m_numContactPointsCopied = 0; - - //make a snapshot of the contact manifolds into individual contact points - if (clientCmd.m_requestContactPointArguments.m_startingContactPointIndex == 0) - { - m_data->m_cachedContactPoints.resize(0); - - int mode = CONTACT_QUERY_MODE_REPORT_EXISTING_CONTACT_POINTS; - - if (clientCmd.m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_QUERY_MODE) - { - mode = clientCmd.m_requestContactPointArguments.m_mode; - } - - switch (mode) - { - case CONTACT_QUERY_MODE_REPORT_EXISTING_CONTACT_POINTS: - { - int numContactManifolds = m_data->m_dynamicsWorld->getDispatcher()->getNumManifolds(); - m_data->m_cachedContactPoints.reserve(numContactManifolds * 4); - for (int i = 0; i < numContactManifolds; i++) - { - const btPersistentManifold* manifold = m_data->m_dynamicsWorld->getDispatcher()->getInternalManifoldPointer()[i]; - int linkIndexA = -1; - int linkIndexB = -1; - - int objectIndexB = -1; - const btRigidBody* bodyB = btRigidBody::upcast(manifold->getBody1()); - if (bodyB) - { - objectIndexB = bodyB->getUserIndex2(); - } - const btMultiBodyLinkCollider* mblB = btMultiBodyLinkCollider::upcast(manifold->getBody1()); - if (mblB && mblB->m_multiBody) - { - linkIndexB = mblB->m_link; - objectIndexB = mblB->m_multiBody->getUserIndex2(); - } - - int objectIndexA = -1; - const btRigidBody* bodyA = btRigidBody::upcast(manifold->getBody0()); - if (bodyA) - { - objectIndexA = bodyA->getUserIndex2(); - } - const btMultiBodyLinkCollider* mblA = btMultiBodyLinkCollider::upcast(manifold->getBody0()); - if (mblA && mblA->m_multiBody) - { - linkIndexA = mblA->m_link; - objectIndexA = mblA->m_multiBody->getUserIndex2(); - } - btAssert(bodyA || mblA); - - //apply the filter, if the user provides it - bool swap = false; - if (clientCmd.m_requestContactPointArguments.m_objectAIndexFilter >= 0) - { - if (clientCmd.m_requestContactPointArguments.m_objectAIndexFilter == objectIndexA) - { - swap = false; - } - else if (clientCmd.m_requestContactPointArguments.m_objectAIndexFilter == objectIndexB) - { - swap = true; - } - else - { - continue; - } - } - - if (swap) - { - std::swap(objectIndexA, objectIndexB); - std::swap(linkIndexA, linkIndexB); - std::swap(bodyA, bodyB); - } - - //apply the second object filter, if the user provides it - if (clientCmd.m_requestContactPointArguments.m_objectBIndexFilter >= 0) - { - if (clientCmd.m_requestContactPointArguments.m_objectBIndexFilter != objectIndexB) - { - continue; - } - } - - if ( - (clientCmd.m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_LINK_INDEX_A_FILTER) && - clientCmd.m_requestContactPointArguments.m_linkIndexAIndexFilter != linkIndexA) - { - continue; - } - - if ( - (clientCmd.m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_LINK_INDEX_B_FILTER) && - clientCmd.m_requestContactPointArguments.m_linkIndexBIndexFilter != linkIndexB) - { - continue; - } - - for (int p = 0; p < manifold->getNumContacts(); p++) - { - - b3ContactPointData pt; - pt.m_bodyUniqueIdA = objectIndexA; - pt.m_bodyUniqueIdB = objectIndexB; - const btManifoldPoint& srcPt = manifold->getContactPoint(p); - pt.m_contactDistance = srcPt.getDistance(); - pt.m_contactFlags = 0; - pt.m_linkIndexA = linkIndexA; - pt.m_linkIndexB = linkIndexB; - for (int j = 0; j < 3; j++) - { - pt.m_contactNormalOnBInWS[j] = srcPt.m_normalWorldOnB[j]; - pt.m_positionOnAInWS[j] = srcPt.getPositionWorldOnA()[j]; - pt.m_positionOnBInWS[j] = srcPt.getPositionWorldOnB()[j]; - } - pt.m_normalForce = srcPt.getAppliedImpulse() / m_data->m_physicsDeltaTime; - // pt.m_linearFrictionForce = srcPt.m_appliedImpulseLateral1; - m_data->m_cachedContactPoints.push_back(pt); - } - } - break; - } - - case CONTACT_QUERY_MODE_COMPUTE_CLOSEST_POINTS: - { - //todo(erwincoumans) compute closest points between all, and vs all, pair - btScalar closestDistanceThreshold = 0.f; - - if (clientCmd.m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_CLOSEST_DISTANCE_THRESHOLD) - { - closestDistanceThreshold = clientCmd.m_requestContactPointArguments.m_closestDistanceThreshold; - } - - int bodyUniqueIdA = clientCmd.m_requestContactPointArguments.m_objectAIndexFilter; - int bodyUniqueIdB = clientCmd.m_requestContactPointArguments.m_objectBIndexFilter; - - bool hasLinkIndexAFilter = (0!=(clientCmd.m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_LINK_INDEX_A_FILTER)); - bool hasLinkIndexBFilter = (0!=(clientCmd.m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_LINK_INDEX_B_FILTER)); - - int linkIndexA = clientCmd.m_requestContactPointArguments.m_linkIndexAIndexFilter; - int linkIndexB = clientCmd.m_requestContactPointArguments.m_linkIndexBIndexFilter; - - btAlignedObjectArray setA; - btAlignedObjectArray setB; - btAlignedObjectArray setALinkIndex; - btAlignedObjectArray setBLinkIndex; - - if (bodyUniqueIdA >= 0) - { - InternalBodyData* bodyA = m_data->m_bodyHandles.getHandle(bodyUniqueIdA); - if (bodyA) - { - if (bodyA->m_multiBody) - { - if (bodyA->m_multiBody->getBaseCollider()) - { - if (!hasLinkIndexAFilter || (linkIndexA == -1)) - { - setA.push_back(bodyA->m_multiBody->getBaseCollider()); - setALinkIndex.push_back(-1); - } - } - for (int i = 0; i < bodyA->m_multiBody->getNumLinks(); i++) - { - if (bodyA->m_multiBody->getLink(i).m_collider) - { - if (!hasLinkIndexAFilter || (linkIndexA == i)) - { - setA.push_back(bodyA->m_multiBody->getLink(i).m_collider); - setALinkIndex.push_back(i); - } - } - } - } - if (bodyA->m_rigidBody) - { - setA.push_back(bodyA->m_rigidBody); - setALinkIndex.push_back(-1); - } - } - } - if (bodyUniqueIdB>=0) - { - InternalBodyData* bodyB = m_data->m_bodyHandles.getHandle(bodyUniqueIdB); - if (bodyB) - { - if (bodyB->m_multiBody) - { - if (bodyB->m_multiBody->getBaseCollider()) - { - if (!hasLinkIndexBFilter || (linkIndexB == -1)) - { - setB.push_back(bodyB->m_multiBody->getBaseCollider()); - setBLinkIndex.push_back(-1); - } - } - for (int i = 0; i < bodyB->m_multiBody->getNumLinks(); i++) - { - if (bodyB->m_multiBody->getLink(i).m_collider) - { - if (!hasLinkIndexBFilter || (linkIndexB ==i)) - { - setB.push_back(bodyB->m_multiBody->getLink(i).m_collider); - setBLinkIndex.push_back(i); - } - } - } - } - if (bodyB->m_rigidBody) - { - setB.push_back(bodyB->m_rigidBody); - setBLinkIndex.push_back(-1); - - } - } - } - - { - ///ContactResultCallback is used to report contact points - struct MyContactResultCallback : public btCollisionWorld::ContactResultCallback - { - int m_bodyUniqueIdA; - int m_bodyUniqueIdB; - int m_linkIndexA; - int m_linkIndexB; - btScalar m_deltaTime; - - btAlignedObjectArray& m_cachedContactPoints; - - MyContactResultCallback(btAlignedObjectArray& pointCache) - :m_cachedContactPoints(pointCache) - { - } - - virtual ~MyContactResultCallback() - { - } - - virtual bool needsCollision(btBroadphaseProxy* proxy0) const - { - //bool collides = (proxy0->m_collisionFilterGroup & m_collisionFilterMask) != 0; - //collides = collides && (m_collisionFilterGroup & proxy0->m_collisionFilterMask); - //return collides; - return true; - } - - virtual btScalar addSingleResult(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap, int partId0, int index0, const btCollisionObjectWrapper* colObj1Wrap, int partId1, int index1) - { - if (cp.m_distance1<=m_closestDistanceThreshold) - { - b3ContactPointData pt; - pt.m_bodyUniqueIdA = m_bodyUniqueIdA; - pt.m_bodyUniqueIdB = m_bodyUniqueIdB; - const btManifoldPoint& srcPt = cp; - pt.m_contactDistance = srcPt.getDistance(); - pt.m_contactFlags = 0; - pt.m_linkIndexA = m_linkIndexA; - pt.m_linkIndexB = m_linkIndexB; - for (int j = 0; j < 3; j++) - { - pt.m_contactNormalOnBInWS[j] = srcPt.m_normalWorldOnB[j]; - pt.m_positionOnAInWS[j] = srcPt.getPositionWorldOnA()[j]; - pt.m_positionOnBInWS[j] = srcPt.getPositionWorldOnB()[j]; - } - pt.m_normalForce = srcPt.getAppliedImpulse() / m_deltaTime; - // pt.m_linearFrictionForce = srcPt.m_appliedImpulseLateral1; - m_cachedContactPoints.push_back(pt); - } - return 1; - - } - }; - - - MyContactResultCallback cb(m_data->m_cachedContactPoints); - - cb.m_bodyUniqueIdA = bodyUniqueIdA; - cb.m_bodyUniqueIdB = bodyUniqueIdB; - cb.m_deltaTime = m_data->m_physicsDeltaTime; - - for (int i = 0; i < setA.size(); i++) - { - cb.m_linkIndexA = setALinkIndex[i]; - for (int j = 0; j < setB.size(); j++) - { - cb.m_linkIndexB = setBLinkIndex[j]; - cb.m_closestDistanceThreshold = closestDistanceThreshold; - this->m_data->m_dynamicsWorld->contactPairTest(setA[i], setB[j], cb); - } - } - } - - break; - } - default: - { - b3Warning("Unknown contact query mode: %d", mode); - } - - } - } - - int numContactPoints = m_data->m_cachedContactPoints.size(); - - - //b3ContactPoint - //struct b3ContactPointDynamics - - int totalBytesPerContact = sizeof(b3ContactPointData); - int contactPointStorage = bufferSizeInBytes/totalBytesPerContact-1; - - b3ContactPointData* contactData = (b3ContactPointData*)bufferServerToClient; - - int startContactPointIndex = clientCmd.m_requestContactPointArguments.m_startingContactPointIndex; - int numContactPointBatch = btMin(numContactPoints,contactPointStorage); - - int endContactPointIndex = startContactPointIndex+numContactPointBatch; - - for (int i=startContactPointIndex;im_cachedContactPoints[i]; - b3ContactPointData& destPt = contactData[serverCmd.m_sendContactPointArgs.m_numContactPointsCopied]; - destPt = srcPt; - serverCmd.m_sendContactPointArgs.m_numContactPointsCopied++; - } - - serverCmd.m_sendContactPointArgs.m_startingContactPointIndex = clientCmd.m_requestContactPointArguments.m_startingContactPointIndex; - serverCmd.m_sendContactPointArgs.m_numRemainingContactPoints = numContactPoints - clientCmd.m_requestContactPointArguments.m_startingContactPointIndex - serverCmd.m_sendContactPointArgs.m_numContactPointsCopied; - serverCmd.m_numDataStreamBytes = totalBytesPerContact * serverCmd.m_sendContactPointArgs.m_numContactPointsCopied; - serverCmd.m_type = CMD_CONTACT_POINT_INFORMATION_COMPLETED; //CMD_CONTACT_POINT_INFORMATION_FAILED, - hasStatus = true; - break; - } - case CMD_CALCULATE_INVERSE_DYNAMICS: - { - BT_PROFILE("CMD_CALCULATE_INVERSE_DYNAMICS"); - SharedMemoryStatus& serverCmd = serverStatusOut; - InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId); - if (bodyHandle && bodyHandle->m_multiBody) - { - serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_FAILED; - - btInverseDynamics::MultiBodyTree* tree = m_data->findOrCreateTree(bodyHandle->m_multiBody); - - if (tree) - { - int baseDofs = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 6; - const int num_dofs = bodyHandle->m_multiBody->getNumDofs(); - btInverseDynamics::vecx nu(num_dofs+baseDofs), qdot(num_dofs + baseDofs), q(num_dofs + baseDofs), joint_force(num_dofs + baseDofs); - for (int i = 0; i < num_dofs; i++) - { - q[i + baseDofs] = clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[i]; - qdot[i + baseDofs] = clientCmd.m_calculateInverseDynamicsArguments.m_jointVelocitiesQdot[i]; - nu[i+baseDofs] = clientCmd.m_calculateInverseDynamicsArguments.m_jointAccelerations[i]; - } - // Set the gravity to correspond to the world gravity - btInverseDynamics::vec3 id_grav(m_data->m_dynamicsWorld->getGravity()); - - if (-1 != tree->setGravityInWorldFrame(id_grav) && - -1 != tree->calculateInverseDynamics(q, qdot, nu, &joint_force)) - { - serverCmd.m_inverseDynamicsResultArgs.m_bodyUniqueId = clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId; - serverCmd.m_inverseDynamicsResultArgs.m_dofCount = num_dofs; - for (int i = 0; i < num_dofs; i++) - { - serverCmd.m_inverseDynamicsResultArgs.m_jointForces[i] = joint_force[i+baseDofs]; - } - serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED; - } - else - { - serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_FAILED; - } - } - - } - else - { - serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_FAILED; - } - - hasStatus = true; - break; - } - case CMD_CALCULATE_JACOBIAN: - { - BT_PROFILE("CMD_CALCULATE_JACOBIAN"); - - SharedMemoryStatus& serverCmd = serverStatusOut; - InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(clientCmd.m_calculateJacobianArguments.m_bodyUniqueId); - if (bodyHandle && bodyHandle->m_multiBody) - { - serverCmd.m_type = CMD_CALCULATED_JACOBIAN_FAILED; - - btInverseDynamics::MultiBodyTree* tree = m_data->findOrCreateTree(bodyHandle->m_multiBody); - - if (tree) - { - int baseDofs = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 6; - const int numDofs = bodyHandle->m_multiBody->getNumDofs(); - btInverseDynamics::vecx q(numDofs + baseDofs); - btInverseDynamics::vecx qdot(numDofs + baseDofs); - btInverseDynamics::vecx nu(numDofs + baseDofs); - btInverseDynamics::vecx joint_force(numDofs + baseDofs); - for (int i = 0; i < numDofs; i++) - { - q[i + baseDofs] = clientCmd.m_calculateJacobianArguments.m_jointPositionsQ[i]; - qdot[i + baseDofs] = clientCmd.m_calculateJacobianArguments.m_jointVelocitiesQdot[i]; - nu[i + baseDofs] = clientCmd.m_calculateJacobianArguments.m_jointAccelerations[i]; - } - // Set the gravity to correspond to the world gravity - btInverseDynamics::vec3 id_grav(m_data->m_dynamicsWorld->getGravity()); - if (-1 != tree->setGravityInWorldFrame(id_grav) && - -1 != tree->calculateInverseDynamics(q, qdot, nu, &joint_force)) - { - serverCmd.m_jacobianResultArgs.m_dofCount = numDofs + baseDofs; - // Set jacobian value - tree->calculateJacobians(q); - btInverseDynamics::mat3x jac_t(3, numDofs + baseDofs); - btInverseDynamics::mat3x jac_r(3, numDofs + baseDofs); - - // Note that inverse dynamics uses zero-based indexing of bodies, not starting from -1 for the base link. - tree->getBodyJacobianTrans(clientCmd.m_calculateJacobianArguments.m_linkIndex + 1, &jac_t); - tree->getBodyJacobianRot(clientCmd.m_calculateJacobianArguments.m_linkIndex + 1, &jac_r); - // Update the translational jacobian based on the desired local point. - // v_pt = v_frame + w x pt - // v_pt = J_t * qd + (J_r * qd) x pt - // v_pt = J_t * qd - pt x (J_r * qd) - // v_pt = J_t * qd - pt_x * J_r * qd) - // v_pt = (J_t - pt_x * J_r) * qd - // J_t_new = J_t - pt_x * J_r - btInverseDynamics::vec3 localPosition; - for (int i = 0; i < 3; ++i) { - localPosition(i) = clientCmd.m_calculateJacobianArguments.m_localPosition[i]; - } - // Only calculate if the localPosition is non-zero. - if (btInverseDynamics::maxAbs(localPosition) > 0.0) { - // Write the localPosition into world coordinates. - btInverseDynamics::mat33 world_rotation_body; - tree->getBodyTransform(clientCmd.m_calculateJacobianArguments.m_linkIndex + 1, &world_rotation_body); - localPosition = world_rotation_body * localPosition; - // Correct the translational jacobian. - btInverseDynamics::mat33 skewCrossProduct; - btInverseDynamics::skew(localPosition, &skewCrossProduct); - btInverseDynamics::mat3x jac_l(3, numDofs + baseDofs); - btInverseDynamics::mul(skewCrossProduct, jac_r, &jac_l); - btInverseDynamics::mat3x jac_t_new(3, numDofs + baseDofs); - btInverseDynamics::sub(jac_t, jac_l, &jac_t_new); - jac_t = jac_t_new; - } - // Fill in the result into the shared memory. - for (int i = 0; i < 3; ++i) - { - for (int j = 0; j < (numDofs + baseDofs); ++j) - { - int element = (numDofs + baseDofs) * i + j; - serverCmd.m_jacobianResultArgs.m_linearJacobian[element] = jac_t(i,j); - serverCmd.m_jacobianResultArgs.m_angularJacobian[element] = jac_r(i,j); - } - } - serverCmd.m_type = CMD_CALCULATED_JACOBIAN_COMPLETED; - } - else - { - serverCmd.m_type = CMD_CALCULATED_JACOBIAN_FAILED; - } - } - - } - else - { - serverCmd.m_type = CMD_CALCULATED_JACOBIAN_FAILED; - } - - hasStatus = true; - break; - } - case CMD_CALCULATE_MASS_MATRIX: - { - BT_PROFILE("CMD_CALCULATE_MASS_MATRIX"); - - SharedMemoryStatus& serverCmd = serverStatusOut; - serverCmd.m_type = CMD_CALCULATED_MASS_MATRIX_FAILED; - InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(clientCmd.m_calculateMassMatrixArguments.m_bodyUniqueId); - if (bodyHandle && bodyHandle->m_multiBody) - { - - btInverseDynamics::MultiBodyTree* tree = m_data->findOrCreateTree(bodyHandle->m_multiBody); - - if (tree) - { - int baseDofs = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 6; - const int numDofs = bodyHandle->m_multiBody->getNumDofs(); - const int totDofs = numDofs + baseDofs; - btInverseDynamics::vecx q(totDofs); - btInverseDynamics::matxx massMatrix(totDofs, totDofs); - for (int i = 0; i < numDofs; i++) - { - q[i + baseDofs] = clientCmd.m_calculateMassMatrixArguments.m_jointPositionsQ[i]; - } - if (-1 != tree->calculateMassMatrix(q, &massMatrix)) - { - serverCmd.m_massMatrixResultArgs.m_dofCount = totDofs; - // Fill in the result into the shared memory. - double* sharedBuf = (double*)bufferServerToClient; - int sizeInBytes = totDofs*totDofs*sizeof(double); - if (sizeInBytes < bufferSizeInBytes) - { - for (int i = 0; i < (totDofs); ++i) - { - for (int j = 0; j < (totDofs); ++j) - { - int element = (totDofs) * i + j; - - sharedBuf[element] = massMatrix(i,j); - } - } - serverCmd.m_type = CMD_CALCULATED_MASS_MATRIX_COMPLETED; - } - } - - } - - } - else - { - serverCmd.m_type = CMD_CALCULATED_MASS_MATRIX_FAILED; - } - - hasStatus = true; - break; - } - case CMD_APPLY_EXTERNAL_FORCE: - { - BT_PROFILE("CMD_APPLY_EXTERNAL_FORCE"); - - if (m_data->m_verboseOutput) - { - b3Printf("CMD_APPLY_EXTERNAL_FORCE clientCmd = %d\n", clientCmd.m_sequenceNumber); - } - for (int i = 0; i < clientCmd.m_externalForceArguments.m_numForcesAndTorques; ++i) - { - InternalBodyData* body = m_data->m_bodyHandles.getHandle(clientCmd.m_externalForceArguments.m_bodyUniqueIds[i]); - bool isLinkFrame = ((clientCmd.m_externalForceArguments.m_forceFlags[i] & EF_LINK_FRAME) != 0); - - if (body && body->m_multiBody) - { - btMultiBody* mb = body->m_multiBody; - - if ((clientCmd.m_externalForceArguments.m_forceFlags[i] & EF_FORCE)!=0) - { - btVector3 tmpForce(clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+0], - clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+1], - clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+2]); - btVector3 tmpPosition( - clientCmd.m_externalForceArguments.m_positions[i*3+0], - clientCmd.m_externalForceArguments.m_positions[i*3+1], - clientCmd.m_externalForceArguments.m_positions[i*3+2]); - - - if (clientCmd.m_externalForceArguments.m_linkIds[i] == -1) - { - btVector3 forceWorld = isLinkFrame ? mb->getBaseWorldTransform().getBasis()*tmpForce : tmpForce; - btVector3 relPosWorld = isLinkFrame ? mb->getBaseWorldTransform().getBasis()*tmpPosition : tmpPosition - mb->getBaseWorldTransform().getOrigin(); - mb->addBaseForce(forceWorld); - mb->addBaseTorque(relPosWorld.cross(forceWorld)); - //b3Printf("apply base force of %f,%f,%f at %f,%f,%f\n", forceWorld[0],forceWorld[1],forceWorld[2],positionLocal[0],positionLocal[1],positionLocal[2]); - } else - { - int link = clientCmd.m_externalForceArguments.m_linkIds[i]; - - btVector3 forceWorld = isLinkFrame ? mb->getLink(link).m_cachedWorldTransform.getBasis()*tmpForce : tmpForce; - btVector3 relPosWorld = isLinkFrame ? mb->getLink(link).m_cachedWorldTransform.getBasis()*tmpPosition : tmpPosition - mb->getBaseWorldTransform().getOrigin(); - mb->addLinkForce(link, forceWorld); - mb->addLinkTorque(link,relPosWorld.cross(forceWorld)); - //b3Printf("apply link force of %f,%f,%f at %f,%f,%f\n", forceWorld[0],forceWorld[1],forceWorld[2], positionLocal[0],positionLocal[1],positionLocal[2]); - } - } - if ((clientCmd.m_externalForceArguments.m_forceFlags[i] & EF_TORQUE)!=0) - { - btVector3 torqueLocal(clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+0], - clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+1], - clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+2]); - - if (clientCmd.m_externalForceArguments.m_linkIds[i] == -1) - { - btVector3 torqueWorld = isLinkFrame ? torqueLocal : mb->getBaseWorldTransform().getBasis()*torqueLocal; - mb->addBaseTorque(torqueWorld); - //b3Printf("apply base torque of %f,%f,%f\n", torqueWorld[0],torqueWorld[1],torqueWorld[2]); - } else - { - int link = clientCmd.m_externalForceArguments.m_linkIds[i]; - btVector3 torqueWorld = mb->getLink(link).m_cachedWorldTransform.getBasis()*torqueLocal; - mb->addLinkTorque(link, torqueWorld); - //b3Printf("apply link torque of %f,%f,%f\n", torqueWorld[0],torqueWorld[1],torqueWorld[2]); - } - } - } - - if (body && body->m_rigidBody) - { - btRigidBody* rb = body->m_rigidBody; - if ((clientCmd.m_externalForceArguments.m_forceFlags[i] & EF_FORCE) != 0) - { - btVector3 forceLocal(clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 0], - clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 1], - clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 2]); - btVector3 positionLocal( - clientCmd.m_externalForceArguments.m_positions[i * 3 + 0], - clientCmd.m_externalForceArguments.m_positions[i * 3 + 1], - clientCmd.m_externalForceArguments.m_positions[i * 3 + 2]); - - btVector3 forceWorld = isLinkFrame ? forceLocal : rb->getWorldTransform().getBasis()*forceLocal; - btVector3 relPosWorld = isLinkFrame ? positionLocal : rb->getWorldTransform().getBasis()*positionLocal; - rb->applyForce(forceWorld, relPosWorld); - - } - - if ((clientCmd.m_externalForceArguments.m_forceFlags[i] & EF_TORQUE) != 0) - { - btVector3 torqueLocal(clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 0], - clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 1], - clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 2]); - - btVector3 torqueWorld = isLinkFrame ? torqueLocal : rb->getWorldTransform().getBasis()*torqueLocal; - rb->applyTorque(torqueWorld); - } - } - } - - SharedMemoryStatus& serverCmd =serverStatusOut; - serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED; - hasStatus = true; - break; - } - case CMD_REMOVE_BODY: - { - SharedMemoryStatus& serverCmd =serverStatusOut; - serverCmd.m_type = CMD_REMOVE_BODY_FAILED; - serverCmd.m_removeObjectArgs.m_numBodies = 0; - serverCmd.m_removeObjectArgs.m_numUserConstraints = 0; - - m_data->m_guiHelper->setVisualizerFlag(COV_ENABLE_SYNC_RENDERING_INTERNAL,0); - - for (int i=0;im_bodyHandles.getHandle(bodyUniqueId); - if (bodyHandle) - { - if (bodyHandle->m_multiBody) - { - serverCmd.m_removeObjectArgs.m_bodyUniqueIds[serverCmd.m_removeObjectArgs.m_numBodies++] = bodyUniqueId; - - //also remove user constraints... - for (int i=m_data->m_dynamicsWorld->getNumMultiBodyConstraints()-1;i>=0;i--) - { - btMultiBodyConstraint* mbc = m_data->m_dynamicsWorld->getMultiBodyConstraint(i); - if ((mbc->getMultiBodyA() == bodyHandle->m_multiBody)||(mbc->getMultiBodyB()==bodyHandle->m_multiBody)) - { - m_data->m_dynamicsWorld->removeMultiBodyConstraint(mbc); - - //also remove user constraint and submit it as removed - for (int c=m_data->m_userConstraints.size()-1;c>=0;c--) - { - InteralUserConstraintData* userConstraintPtr = m_data->m_userConstraints.getAtIndex(c); - int userConstraintKey = m_data->m_userConstraints.getKeyAtIndex(c).getUid1(); - - if (userConstraintPtr->m_mbConstraint == mbc) - { - m_data->m_userConstraints.remove(userConstraintKey); - serverCmd.m_removeObjectArgs.m_userConstraintUniqueIds[serverCmd.m_removeObjectArgs.m_numUserConstraints++]=userConstraintKey; - } - } - - delete mbc; - - - } - } - - if (bodyHandle->m_multiBody->getBaseCollider()) - { - m_data->m_visualConverter.removeVisualShape(bodyHandle->m_multiBody->getBaseCollider()); - m_data->m_dynamicsWorld->removeCollisionObject(bodyHandle->m_multiBody->getBaseCollider()); - int graphicsIndex = bodyHandle->m_multiBody->getBaseCollider()->getUserIndex(); - m_data->m_guiHelper->removeGraphicsInstance(graphicsIndex); - } - for (int link=0;linkm_multiBody->getNumLinks();link++) - { - - if (bodyHandle->m_multiBody->getLink(link).m_collider) - { - m_data->m_visualConverter.removeVisualShape(bodyHandle->m_multiBody->getLink(link).m_collider); - m_data->m_dynamicsWorld->removeCollisionObject(bodyHandle->m_multiBody->getLink(link).m_collider); - int graphicsIndex = bodyHandle->m_multiBody->getLink(link).m_collider->getUserIndex(); - m_data->m_guiHelper->removeGraphicsInstance(graphicsIndex); - } - } - int numCollisionObjects = m_data->m_dynamicsWorld->getNumCollisionObjects(); - m_data->m_dynamicsWorld->removeMultiBody(bodyHandle->m_multiBody); - numCollisionObjects = m_data->m_dynamicsWorld->getNumCollisionObjects(); - //todo: clear all other remaining data, release memory etc - - delete bodyHandle->m_multiBody; - bodyHandle->m_multiBody=0; - serverCmd.m_type = CMD_REMOVE_BODY_COMPLETED; - } - if (bodyHandle->m_rigidBody) - { - m_data->m_visualConverter.removeVisualShape(bodyHandle->m_rigidBody); - serverCmd.m_removeObjectArgs.m_bodyUniqueIds[serverCmd.m_removeObjectArgs.m_numBodies++] = bodyUniqueId; - //todo: clear all other remaining data... - m_data->m_dynamicsWorld->removeRigidBody(bodyHandle->m_rigidBody); - int graphicsInstance = bodyHandle->m_rigidBody->getUserIndex2(); - m_data->m_guiHelper->removeGraphicsInstance(graphicsInstance); - delete bodyHandle->m_rigidBody; - bodyHandle->m_rigidBody=0; - serverCmd.m_type = CMD_REMOVE_BODY_COMPLETED; - } - } - - m_data->m_bodyHandles.freeHandle(bodyUniqueId); - } - m_data->m_guiHelper->setVisualizerFlag(COV_ENABLE_SYNC_RENDERING_INTERNAL,1); - - - hasStatus = true; - break; - } - case CMD_USER_CONSTRAINT: - { - BT_PROFILE("CMD_USER_CONSTRAINT"); - - SharedMemoryStatus& serverCmd =serverStatusOut; - serverCmd.m_type = CMD_USER_CONSTRAINT_FAILED; - hasStatus = true; - if (clientCmd.m_updateFlags & USER_CONSTRAINT_REQUEST_STATE) - { - int constraintUid = clientCmd.m_userConstraintArguments.m_userConstraintUniqueId; - InteralUserConstraintData* userConstraintPtr = m_data->m_userConstraints.find(constraintUid); - if (userConstraintPtr) - { - serverCmd.m_userConstraintStateResultArgs.m_numDofs = 0; - for (int i = 0; i < 6; i++) - { - serverCmd.m_userConstraintStateResultArgs.m_appliedConstraintForces[i] = 0; - } - if (userConstraintPtr->m_mbConstraint) - { - serverCmd.m_userConstraintStateResultArgs.m_numDofs = userConstraintPtr->m_mbConstraint->getNumRows(); - for (int i = 0; i < userConstraintPtr->m_mbConstraint->getNumRows(); i++) - { - serverCmd.m_userConstraintStateResultArgs.m_appliedConstraintForces[i] = userConstraintPtr->m_mbConstraint->getAppliedImpulse(i) / m_data->m_dynamicsWorld->getSolverInfo().m_timeStep; - } - serverCmd.m_type = CMD_USER_CONSTRAINT_REQUEST_STATE_COMPLETED; - } - } - - }; - if (clientCmd.m_updateFlags & USER_CONSTRAINT_REQUEST_INFO) - { - int userConstraintUidChange = clientCmd.m_userConstraintArguments.m_userConstraintUniqueId; - InteralUserConstraintData* userConstraintPtr = m_data->m_userConstraints.find(userConstraintUidChange); - if (userConstraintPtr) - { - serverCmd.m_userConstraintResultArgs = userConstraintPtr->m_userConstraintData; - - serverCmd.m_type = CMD_USER_CONSTRAINT_INFO_COMPLETED; - } - } - if (clientCmd.m_updateFlags & USER_CONSTRAINT_ADD_CONSTRAINT) - { - btScalar defaultMaxForce = 500.0; - InternalBodyData* parentBody = m_data->m_bodyHandles.getHandle(clientCmd.m_userConstraintArguments.m_parentBodyIndex); - if (parentBody && parentBody->m_multiBody) - { - if ((clientCmd.m_userConstraintArguments.m_parentJointIndex>=-1) && clientCmd.m_userConstraintArguments.m_parentJointIndex < parentBody->m_multiBody->getNumLinks()) - { - InternalBodyData* childBody = clientCmd.m_userConstraintArguments.m_childBodyIndex>=0 ? m_data->m_bodyHandles.getHandle(clientCmd.m_userConstraintArguments.m_childBodyIndex):0; - //also create a constraint with just a single multibody/rigid body without child - //if (childBody) - { - btVector3 pivotInParent(clientCmd.m_userConstraintArguments.m_parentFrame[0], clientCmd.m_userConstraintArguments.m_parentFrame[1], clientCmd.m_userConstraintArguments.m_parentFrame[2]); - btVector3 pivotInChild(clientCmd.m_userConstraintArguments.m_childFrame[0], clientCmd.m_userConstraintArguments.m_childFrame[1], clientCmd.m_userConstraintArguments.m_childFrame[2]); - btMatrix3x3 frameInParent(btQuaternion(clientCmd.m_userConstraintArguments.m_parentFrame[3], clientCmd.m_userConstraintArguments.m_parentFrame[4], clientCmd.m_userConstraintArguments.m_parentFrame[5], clientCmd.m_userConstraintArguments.m_parentFrame[6])); - btMatrix3x3 frameInChild(btQuaternion(clientCmd.m_userConstraintArguments.m_childFrame[3], clientCmd.m_userConstraintArguments.m_childFrame[4], clientCmd.m_userConstraintArguments.m_childFrame[5], clientCmd.m_userConstraintArguments.m_childFrame[6])); - btVector3 jointAxis(clientCmd.m_userConstraintArguments.m_jointAxis[0], clientCmd.m_userConstraintArguments.m_jointAxis[1], clientCmd.m_userConstraintArguments.m_jointAxis[2]); - - - - if (clientCmd.m_userConstraintArguments.m_jointType == eGearType) - { - if (childBody && childBody->m_multiBody) - { - if ((clientCmd.m_userConstraintArguments.m_childJointIndex>=-1) && (clientCmd.m_userConstraintArguments.m_childJointIndex m_multiBody->getNumLinks())) - { - btMultiBodyGearConstraint* multibodyGear = new btMultiBodyGearConstraint(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,childBody->m_multiBody,clientCmd.m_userConstraintArguments.m_childJointIndex,pivotInParent,pivotInChild,frameInParent,frameInChild); - multibodyGear->setMaxAppliedImpulse(defaultMaxForce); - m_data->m_dynamicsWorld->addMultiBodyConstraint(multibodyGear); - InteralUserConstraintData userConstraintData; - userConstraintData.m_mbConstraint = multibodyGear; - int uid = m_data->m_userConstraintUIDGenerator++; - serverCmd.m_userConstraintResultArgs = clientCmd.m_userConstraintArguments; - serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid; - serverCmd.m_userConstraintResultArgs.m_maxAppliedForce = defaultMaxForce; - userConstraintData.m_userConstraintData = serverCmd.m_userConstraintResultArgs; - m_data->m_userConstraints.insert(uid,userConstraintData); - serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED; - } - - } - } - else if (clientCmd.m_userConstraintArguments.m_jointType == eFixedType) - { - if (childBody && childBody->m_multiBody) - { - if ((clientCmd.m_userConstraintArguments.m_childJointIndex>=-1) && (clientCmd.m_userConstraintArguments.m_childJointIndex m_multiBody->getNumLinks())) - { - btMultiBodyFixedConstraint* multibodyFixed = new btMultiBodyFixedConstraint(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,childBody->m_multiBody,clientCmd.m_userConstraintArguments.m_childJointIndex,pivotInParent,pivotInChild,frameInParent,frameInChild); - multibodyFixed->setMaxAppliedImpulse(defaultMaxForce); - m_data->m_dynamicsWorld->addMultiBodyConstraint(multibodyFixed); - InteralUserConstraintData userConstraintData; - userConstraintData.m_mbConstraint = multibodyFixed; - int uid = m_data->m_userConstraintUIDGenerator++; - serverCmd.m_userConstraintResultArgs = clientCmd.m_userConstraintArguments; - serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid; - serverCmd.m_userConstraintResultArgs.m_maxAppliedForce = defaultMaxForce; - userConstraintData.m_userConstraintData = serverCmd.m_userConstraintResultArgs; - m_data->m_userConstraints.insert(uid,userConstraintData); - serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED; - } - - } - else - { - btRigidBody* rb = childBody? childBody->m_rigidBody : 0; - btMultiBodyFixedConstraint* rigidbodyFixed = new btMultiBodyFixedConstraint(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,rb,pivotInParent,pivotInChild,frameInParent,frameInChild); - rigidbodyFixed->setMaxAppliedImpulse(defaultMaxForce); - btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld; - world->addMultiBodyConstraint(rigidbodyFixed); - InteralUserConstraintData userConstraintData; - userConstraintData.m_mbConstraint = rigidbodyFixed; - int uid = m_data->m_userConstraintUIDGenerator++; - serverCmd.m_userConstraintResultArgs = clientCmd.m_userConstraintArguments; - serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid; - serverCmd.m_userConstraintResultArgs.m_maxAppliedForce = defaultMaxForce; - userConstraintData.m_userConstraintData = serverCmd.m_userConstraintResultArgs; - m_data->m_userConstraints.insert(uid,userConstraintData); - serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED; - } - - } - else if (clientCmd.m_userConstraintArguments.m_jointType == ePrismaticType) - { - if (childBody && childBody->m_multiBody) - { - btMultiBodySliderConstraint* multibodySlider = new btMultiBodySliderConstraint(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,childBody->m_multiBody,clientCmd.m_userConstraintArguments.m_childJointIndex,pivotInParent,pivotInChild,frameInParent,frameInChild,jointAxis); - multibodySlider->setMaxAppliedImpulse(defaultMaxForce); - m_data->m_dynamicsWorld->addMultiBodyConstraint(multibodySlider); - InteralUserConstraintData userConstraintData; - userConstraintData.m_mbConstraint = multibodySlider; - int uid = m_data->m_userConstraintUIDGenerator++; - serverCmd.m_userConstraintResultArgs = clientCmd.m_userConstraintArguments; - serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid; - serverCmd.m_userConstraintResultArgs.m_maxAppliedForce = defaultMaxForce; - userConstraintData.m_userConstraintData = serverCmd.m_userConstraintResultArgs; - m_data->m_userConstraints.insert(uid,userConstraintData); - serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED; - } - else - { - btRigidBody* rb = childBody? childBody->m_rigidBody : 0; - - btMultiBodySliderConstraint* rigidbodySlider = new btMultiBodySliderConstraint(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,rb,pivotInParent,pivotInChild,frameInParent,frameInChild,jointAxis); - rigidbodySlider->setMaxAppliedImpulse(defaultMaxForce); - btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld; - world->addMultiBodyConstraint(rigidbodySlider); - InteralUserConstraintData userConstraintData; - userConstraintData.m_mbConstraint = rigidbodySlider; - int uid = m_data->m_userConstraintUIDGenerator++; - serverCmd.m_userConstraintResultArgs = clientCmd.m_userConstraintArguments; - serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid; - serverCmd.m_userConstraintResultArgs.m_maxAppliedForce = defaultMaxForce; - userConstraintData.m_userConstraintData = serverCmd.m_userConstraintResultArgs; - m_data->m_userConstraints.insert(uid,userConstraintData); - serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED; } - - } else if (clientCmd.m_userConstraintArguments.m_jointType == ePoint2PointType) - { - if (childBody && childBody->m_multiBody) - { - btMultiBodyPoint2Point* p2p = new btMultiBodyPoint2Point(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,childBody->m_multiBody,clientCmd.m_userConstraintArguments.m_childJointIndex,pivotInParent,pivotInChild); - p2p->setMaxAppliedImpulse(defaultMaxForce); - m_data->m_dynamicsWorld->addMultiBodyConstraint(p2p); - InteralUserConstraintData userConstraintData; - userConstraintData.m_mbConstraint = p2p; - int uid = m_data->m_userConstraintUIDGenerator++; - serverCmd.m_userConstraintResultArgs = clientCmd.m_userConstraintArguments; - serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid; - serverCmd.m_userConstraintResultArgs.m_maxAppliedForce = defaultMaxForce; - userConstraintData.m_userConstraintData = serverCmd.m_userConstraintResultArgs; - m_data->m_userConstraints.insert(uid,userConstraintData); - serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED; - } - else - { - btRigidBody* rb = childBody? childBody->m_rigidBody : 0; - - btMultiBodyPoint2Point* p2p = new btMultiBodyPoint2Point(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,rb,pivotInParent,pivotInChild); - p2p->setMaxAppliedImpulse(defaultMaxForce); - btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld; - world->addMultiBodyConstraint(p2p); - InteralUserConstraintData userConstraintData; - userConstraintData.m_mbConstraint = p2p; - int uid = m_data->m_userConstraintUIDGenerator++; - serverCmd.m_userConstraintResultArgs = clientCmd.m_userConstraintArguments; - serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid; - serverCmd.m_userConstraintResultArgs.m_maxAppliedForce = defaultMaxForce; - userConstraintData.m_userConstraintData = serverCmd.m_userConstraintResultArgs; - m_data->m_userConstraints.insert(uid,userConstraintData); - serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED; - } - - } else - { - b3Warning("unknown constraint type"); - } - - - } - } - } - else - { - InternalBodyData* childBody = clientCmd.m_userConstraintArguments.m_childBodyIndex>=0 ? m_data->m_bodyHandles.getHandle(clientCmd.m_userConstraintArguments.m_childBodyIndex):0; - - if (parentBody && childBody) - { - if (parentBody->m_rigidBody) - { - - btRigidBody* parentRb = 0; - if (clientCmd.m_userConstraintArguments.m_parentJointIndex==-1) - { - parentRb = parentBody->m_rigidBody; - } else - { - if ((clientCmd.m_userConstraintArguments.m_parentJointIndex>=0) && - (clientCmd.m_userConstraintArguments.m_parentJointIndexm_rigidBodyJoints.size())) - { - parentRb = &parentBody->m_rigidBodyJoints[clientCmd.m_userConstraintArguments.m_parentJointIndex]->getRigidBodyB(); - } - } - - - btRigidBody* childRb = 0; - if (childBody->m_rigidBody) - { - - if (clientCmd.m_userConstraintArguments.m_childJointIndex==-1) - { - childRb = childBody->m_rigidBody; - } - else - { - if ((clientCmd.m_userConstraintArguments.m_childJointIndex>=0) - && (clientCmd.m_userConstraintArguments.m_childJointIndexm_rigidBodyJoints.size())) - { - childRb = &childBody->m_rigidBodyJoints[clientCmd.m_userConstraintArguments.m_childJointIndex]->getRigidBodyB(); - } - - } - } - - switch (clientCmd.m_userConstraintArguments.m_jointType) - { - case eRevoluteType: - { - break; - } - case ePrismaticType: - { - break; - } - - case eFixedType: - { - if (childRb && parentRb && (childRb!=parentRb)) - { - btVector3 pivotInParent(clientCmd.m_userConstraintArguments.m_parentFrame[0], clientCmd.m_userConstraintArguments.m_parentFrame[1], clientCmd.m_userConstraintArguments.m_parentFrame[2]); - btVector3 pivotInChild(clientCmd.m_userConstraintArguments.m_childFrame[0], clientCmd.m_userConstraintArguments.m_childFrame[1], clientCmd.m_userConstraintArguments.m_childFrame[2]); - - btTransform offsetTrA,offsetTrB; - offsetTrA.setIdentity(); - offsetTrA.setOrigin(pivotInParent); - offsetTrB.setIdentity(); - offsetTrB.setOrigin(pivotInChild); - - btGeneric6DofSpring2Constraint* dof6 = new btGeneric6DofSpring2Constraint(*parentRb, *childRb, offsetTrA, offsetTrB); - - dof6->setLinearLowerLimit(btVector3(0,0,0)); - dof6->setLinearUpperLimit(btVector3(0,0,0)); - - dof6->setAngularLowerLimit(btVector3(0,0,0)); - dof6->setAngularUpperLimit(btVector3(0,0,0)); - m_data->m_dynamicsWorld->addConstraint(dof6); - InteralUserConstraintData userConstraintData; - userConstraintData.m_rbConstraint = dof6; - int uid = m_data->m_userConstraintUIDGenerator++; - serverCmd.m_userConstraintResultArgs = clientCmd.m_userConstraintArguments; - serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid; - serverCmd.m_userConstraintResultArgs.m_maxAppliedForce = defaultMaxForce; - userConstraintData.m_userConstraintData = serverCmd.m_userConstraintResultArgs; - m_data->m_userConstraints.insert(uid,userConstraintData); - serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED; - } - - break; - } - - case ePoint2PointType: - { - if (childRb && parentRb && (childRb!=parentRb)) - { - btVector3 pivotInParent(clientCmd.m_userConstraintArguments.m_parentFrame[0], clientCmd.m_userConstraintArguments.m_parentFrame[1], clientCmd.m_userConstraintArguments.m_parentFrame[2]); - btVector3 pivotInChild(clientCmd.m_userConstraintArguments.m_childFrame[0], clientCmd.m_userConstraintArguments.m_childFrame[1], clientCmd.m_userConstraintArguments.m_childFrame[2]); - - btPoint2PointConstraint* p2p = new btPoint2PointConstraint(*parentRb,*childRb,pivotInParent,pivotInChild); - p2p->m_setting.m_impulseClamp = defaultMaxForce; - m_data->m_dynamicsWorld->addConstraint(p2p); - InteralUserConstraintData userConstraintData; - userConstraintData.m_rbConstraint = p2p; - int uid = m_data->m_userConstraintUIDGenerator++; - serverCmd.m_userConstraintResultArgs = clientCmd.m_userConstraintArguments; - serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid; - serverCmd.m_userConstraintResultArgs.m_maxAppliedForce = defaultMaxForce; - userConstraintData.m_userConstraintData = serverCmd.m_userConstraintResultArgs; - m_data->m_userConstraints.insert(uid,userConstraintData); - serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED; - } - break; - } - - case eGearType: - { - - if (childRb && parentRb && (childRb!=parentRb)) - { - btVector3 axisA(clientCmd.m_userConstraintArguments.m_jointAxis[0], - clientCmd.m_userConstraintArguments.m_jointAxis[1], - clientCmd.m_userConstraintArguments.m_jointAxis[2]); - //for now we use the same local axis for both objects - btVector3 axisB(clientCmd.m_userConstraintArguments.m_jointAxis[0], - clientCmd.m_userConstraintArguments.m_jointAxis[1], - clientCmd.m_userConstraintArguments.m_jointAxis[2]); - btScalar ratio=1; - btGearConstraint* gear = new btGearConstraint(*parentRb,*childRb, axisA,axisB,ratio); - m_data->m_dynamicsWorld->addConstraint(gear,true); - InteralUserConstraintData userConstraintData; - userConstraintData.m_rbConstraint = gear; - int uid = m_data->m_userConstraintUIDGenerator++; - serverCmd.m_userConstraintResultArgs = clientCmd.m_userConstraintArguments; - serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid; - serverCmd.m_userConstraintResultArgs.m_maxAppliedForce = defaultMaxForce; - userConstraintData.m_userConstraintData = serverCmd.m_userConstraintResultArgs; - m_data->m_userConstraints.insert(uid,userConstraintData); - serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED; - } - break; - } - case eSphericalType: - { - b3Warning("constraint type not handled yet"); - break; - } - case ePlanarType: - { - b3Warning("constraint type not handled yet"); - break; - } - default: - { - b3Warning("unknown constraint type"); - } - }; - } - } - } - } - - if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_CONSTRAINT) - { - serverCmd.m_type = CMD_CHANGE_USER_CONSTRAINT_FAILED; - int userConstraintUidChange = clientCmd.m_userConstraintArguments.m_userConstraintUniqueId; - InteralUserConstraintData* userConstraintPtr = m_data->m_userConstraints.find(userConstraintUidChange); - if (userConstraintPtr) - { - if (userConstraintPtr->m_mbConstraint) - { - if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_PIVOT_IN_B) - { - btVector3 pivotInB(clientCmd.m_userConstraintArguments.m_childFrame[0], - clientCmd.m_userConstraintArguments.m_childFrame[1], - clientCmd.m_userConstraintArguments.m_childFrame[2]); - userConstraintPtr->m_userConstraintData.m_childFrame[0] = clientCmd.m_userConstraintArguments.m_childFrame[0]; - userConstraintPtr->m_userConstraintData.m_childFrame[1] = clientCmd.m_userConstraintArguments.m_childFrame[1]; - userConstraintPtr->m_userConstraintData.m_childFrame[2] = clientCmd.m_userConstraintArguments.m_childFrame[2]; - userConstraintPtr->m_mbConstraint->setPivotInB(pivotInB); - } - if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_FRAME_ORN_IN_B) - { - btQuaternion childFrameOrn(clientCmd.m_userConstraintArguments.m_childFrame[3], - clientCmd.m_userConstraintArguments.m_childFrame[4], - clientCmd.m_userConstraintArguments.m_childFrame[5], - clientCmd.m_userConstraintArguments.m_childFrame[6]); - userConstraintPtr->m_userConstraintData.m_childFrame[3] = clientCmd.m_userConstraintArguments.m_childFrame[3]; - userConstraintPtr->m_userConstraintData.m_childFrame[4] = clientCmd.m_userConstraintArguments.m_childFrame[4]; - userConstraintPtr->m_userConstraintData.m_childFrame[5] = clientCmd.m_userConstraintArguments.m_childFrame[5]; - userConstraintPtr->m_userConstraintData.m_childFrame[6] = clientCmd.m_userConstraintArguments.m_childFrame[6]; - btMatrix3x3 childFrameBasis(childFrameOrn); - userConstraintPtr->m_mbConstraint->setFrameInB(childFrameBasis); - } - if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_MAX_FORCE) - { - btScalar maxImp = clientCmd.m_userConstraintArguments.m_maxAppliedForce*m_data->m_physicsDeltaTime; - userConstraintPtr->m_userConstraintData.m_maxAppliedForce = clientCmd.m_userConstraintArguments.m_maxAppliedForce; - userConstraintPtr->m_mbConstraint->setMaxAppliedImpulse(maxImp); - } - - if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_GEAR_RATIO) - { - userConstraintPtr->m_mbConstraint->setGearRatio(clientCmd.m_userConstraintArguments.m_gearRatio); - userConstraintPtr->m_userConstraintData.m_gearRatio = clientCmd.m_userConstraintArguments.m_gearRatio; - } - if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_RELATIVE_POSITION_TARGET) - { - userConstraintPtr->m_mbConstraint->setRelativePositionTarget(clientCmd.m_userConstraintArguments.m_relativePositionTarget); - userConstraintPtr->m_userConstraintData.m_relativePositionTarget = clientCmd.m_userConstraintArguments.m_relativePositionTarget; - } - if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_ERP) - { - userConstraintPtr->m_mbConstraint->setErp(clientCmd.m_userConstraintArguments.m_erp); - userConstraintPtr->m_userConstraintData.m_erp = clientCmd.m_userConstraintArguments.m_erp; - } - - if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_GEAR_AUX_LINK) - { - userConstraintPtr->m_mbConstraint->setGearAuxLink(clientCmd.m_userConstraintArguments.m_gearAuxLink); - userConstraintPtr->m_userConstraintData.m_gearAuxLink = clientCmd.m_userConstraintArguments.m_gearAuxLink; - } - - } - if (userConstraintPtr->m_rbConstraint) - { - if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_MAX_FORCE) - { - btScalar maxImp = clientCmd.m_userConstraintArguments.m_maxAppliedForce*m_data->m_physicsDeltaTime; - userConstraintPtr->m_userConstraintData.m_maxAppliedForce = clientCmd.m_userConstraintArguments.m_maxAppliedForce; - //userConstraintPtr->m_rbConstraint->setMaxAppliedImpulse(maxImp); - } - - if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_GEAR_RATIO) - { - if (userConstraintPtr->m_rbConstraint->getObjectType()==GEAR_CONSTRAINT_TYPE) - { - btGearConstraint* gear = (btGearConstraint*) userConstraintPtr->m_rbConstraint; - gear->setRatio(clientCmd.m_userConstraintArguments.m_gearRatio); - } - } - } - serverCmd.m_userConstraintResultArgs = clientCmd.m_userConstraintArguments; - serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = userConstraintUidChange; - serverCmd.m_updateFlags = clientCmd.m_updateFlags; - serverCmd.m_type = CMD_CHANGE_USER_CONSTRAINT_COMPLETED; - } - } - if (clientCmd.m_updateFlags & USER_CONSTRAINT_REMOVE_CONSTRAINT) - { - serverCmd.m_type = CMD_REMOVE_USER_CONSTRAINT_FAILED; - int userConstraintUidRemove = clientCmd.m_userConstraintArguments.m_userConstraintUniqueId; - InteralUserConstraintData* userConstraintPtr = m_data->m_userConstraints.find(userConstraintUidRemove); - if (userConstraintPtr) - { - if (userConstraintPtr->m_mbConstraint) - { - m_data->m_dynamicsWorld->removeMultiBodyConstraint(userConstraintPtr->m_mbConstraint); - delete userConstraintPtr->m_mbConstraint; - m_data->m_userConstraints.remove(userConstraintUidRemove); - } - if (userConstraintPtr->m_rbConstraint) - { - m_data->m_dynamicsWorld->removeConstraint(userConstraintPtr->m_rbConstraint); - delete userConstraintPtr->m_rbConstraint; - m_data->m_userConstraints.remove(userConstraintUidRemove); - } - serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = userConstraintUidRemove; - serverCmd.m_type = CMD_REMOVE_USER_CONSTRAINT_COMPLETED; - - - } - - - } - - break; - } - case CMD_CALCULATE_INVERSE_KINEMATICS: - { - BT_PROFILE("CMD_CALCULATE_INVERSE_KINEMATICS"); - SharedMemoryStatus& serverCmd = serverStatusOut; - serverCmd.m_type = CMD_CALCULATE_INVERSE_KINEMATICS_FAILED; - - InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(clientCmd.m_calculateInverseKinematicsArguments.m_bodyUniqueId); - if (bodyHandle && bodyHandle->m_multiBody) - { - IKTrajectoryHelper** ikHelperPtrPtr = m_data->m_inverseKinematicsHelpers.find(bodyHandle->m_multiBody); - IKTrajectoryHelper* ikHelperPtr = 0; - - - if (ikHelperPtrPtr) - { - ikHelperPtr = *ikHelperPtrPtr; - } - else - { - IKTrajectoryHelper* tmpHelper = new IKTrajectoryHelper; - m_data->m_inverseKinematicsHelpers.insert(bodyHandle->m_multiBody, tmpHelper); - ikHelperPtr = tmpHelper; - } - - int endEffectorLinkIndex = clientCmd.m_calculateInverseKinematicsArguments.m_endEffectorLinkIndex; - - - if (ikHelperPtr && (endEffectorLinkIndexm_multiBody->getNumLinks())) - { - const int numDofs = bodyHandle->m_multiBody->getNumDofs(); - int baseDofs = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 6; - b3AlignedObjectArray jacobian_linear; - jacobian_linear.resize(3*numDofs); - b3AlignedObjectArray jacobian_angular; - jacobian_angular.resize(3*numDofs); - int jacSize = 0; - - btInverseDynamics::MultiBodyTree* tree = m_data->findOrCreateTree(bodyHandle->m_multiBody); - - - - btAlignedObjectArray q_current; - q_current.resize(numDofs); - - - - if (tree && ((numDofs+ baseDofs) == tree->numDoFs())) - { - jacSize = jacobian_linear.size(); - // Set jacobian value - - - - btInverseDynamics::vecx nu(numDofs+baseDofs), qdot(numDofs + baseDofs), q(numDofs + baseDofs), joint_force(numDofs + baseDofs); - int DofIndex = 0; - for (int i = 0; i < bodyHandle->m_multiBody->getNumLinks(); ++i) { - if (bodyHandle->m_multiBody->getLink(i).m_jointType >= 0 && bodyHandle->m_multiBody->getLink(i).m_jointType <= 2) { // 0, 1, 2 represent revolute, prismatic, and spherical joint types respectively. Skip the fixed joints. - q_current[DofIndex] = bodyHandle->m_multiBody->getJointPos(i); - q[DofIndex+baseDofs] = bodyHandle->m_multiBody->getJointPos(i); - qdot[DofIndex+baseDofs] = 0; - nu[DofIndex+baseDofs] = 0; - DofIndex++; - } - } // Set the gravity to correspond to the world gravity - btInverseDynamics::vec3 id_grav(m_data->m_dynamicsWorld->getGravity()); - - if (-1 != tree->setGravityInWorldFrame(id_grav) && - -1 != tree->calculateInverseDynamics(q, qdot, nu, &joint_force)) - { - tree->calculateJacobians(q); - btInverseDynamics::mat3x jac_t(3, numDofs+ baseDofs); - btInverseDynamics::mat3x jac_r(3,numDofs + baseDofs); - // Note that inverse dynamics uses zero-based indexing of bodies, not starting from -1 for the base link. - tree->getBodyJacobianTrans(endEffectorLinkIndex+1, &jac_t); - tree->getBodyJacobianRot(endEffectorLinkIndex+1, &jac_r); - for (int i = 0; i < 3; ++i) - { - for (int j = 0; j < numDofs; ++j) - { - jacobian_linear[i*numDofs+j] = jac_t(i,(baseDofs+j)); - jacobian_angular[i*numDofs+j] = jac_r(i,(baseDofs+j)); - } - } - } - - - - btAlignedObjectArray q_new; - q_new.resize(numDofs); - int ikMethod = 0; - if ((clientCmd.m_updateFlags& IK_HAS_TARGET_ORIENTATION)&&(clientCmd.m_updateFlags&IK_HAS_NULL_SPACE_VELOCITY)) - { - //Nullspace task only works with DLS now. TODO: add nullspace task to SDLS. - ikMethod = IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE; - } - else if (clientCmd.m_updateFlags& IK_HAS_TARGET_ORIENTATION) - { - if (clientCmd.m_updateFlags & IK_SDLS) - { - ikMethod = IK2_VEL_SDLS_WITH_ORIENTATION; - } - else - { - ikMethod = IK2_VEL_DLS_WITH_ORIENTATION; - } - } - else if (clientCmd.m_updateFlags& IK_HAS_NULL_SPACE_VELOCITY) - { - //Nullspace task only works with DLS now. TODO: add nullspace task to SDLS. - ikMethod = IK2_VEL_DLS_WITH_NULLSPACE; - } - else - { - if (clientCmd.m_updateFlags & IK_SDLS) - { - ikMethod = IK2_VEL_SDLS; - } - else - { - ikMethod = IK2_VEL_DLS;; - } - } - - if (clientCmd.m_updateFlags& IK_HAS_NULL_SPACE_VELOCITY) - { - btAlignedObjectArray lower_limit; - btAlignedObjectArray upper_limit; - btAlignedObjectArray joint_range; - btAlignedObjectArray rest_pose; - lower_limit.resize(numDofs); - upper_limit.resize(numDofs); - joint_range.resize(numDofs); - rest_pose.resize(numDofs); - for (int i = 0; i < numDofs; ++i) - { - lower_limit[i] = clientCmd.m_calculateInverseKinematicsArguments.m_lowerLimit[i]; - upper_limit[i] = clientCmd.m_calculateInverseKinematicsArguments.m_upperLimit[i]; - joint_range[i] = clientCmd.m_calculateInverseKinematicsArguments.m_jointRange[i]; - rest_pose[i] = clientCmd.m_calculateInverseKinematicsArguments.m_restPose[i]; - } - ikHelperPtr->computeNullspaceVel(numDofs, &q_current[0], &lower_limit[0], &upper_limit[0], &joint_range[0], &rest_pose[0]); - } - - btTransform endEffectorTransformWorld = bodyHandle->m_multiBody->getLink(endEffectorLinkIndex).m_cachedWorldTransform * bodyHandle->m_linkLocalInertialFrames[endEffectorLinkIndex].inverse(); - - btVector3DoubleData endEffectorWorldPosition; - btQuaternionDoubleData endEffectorWorldOrientation; - - btVector3 endEffectorPosWorldOrg = endEffectorTransformWorld.getOrigin(); - btQuaternion endEffectorOriWorldOrg = endEffectorTransformWorld.getRotation(); - btTransform endEffectorWorld; - endEffectorWorld.setOrigin(endEffectorPosWorldOrg); - endEffectorWorld.setRotation(endEffectorOriWorldOrg); - - btTransform tr = bodyHandle->m_multiBody->getBaseWorldTransform(); - - btTransform endEffectorBaseCoord = tr.inverse()*endEffectorWorld; - - btQuaternion endEffectorOriBaseCoord= endEffectorBaseCoord.getRotation(); - - btVector4 endEffectorOri(endEffectorOriBaseCoord.x(), endEffectorOriBaseCoord.y(), endEffectorOriBaseCoord.z(), endEffectorOriBaseCoord.w()); - - endEffectorBaseCoord.getOrigin().serializeDouble(endEffectorWorldPosition); - endEffectorBaseCoord.getRotation().serializeDouble(endEffectorWorldOrientation); - - btVector3 targetPosWorld(clientCmd.m_calculateInverseKinematicsArguments.m_targetPosition[0], - clientCmd.m_calculateInverseKinematicsArguments.m_targetPosition[1], - clientCmd.m_calculateInverseKinematicsArguments.m_targetPosition[2]); - - btQuaternion targetOrnWorld(clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation[0], - clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation[1], - clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation[2], - clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation[3]); - btTransform targetWorld; - targetWorld.setOrigin(targetPosWorld); - targetWorld.setRotation(targetOrnWorld); - btTransform targetBaseCoord; - targetBaseCoord = tr.inverse()*targetWorld; - - btVector3DoubleData targetPosBaseCoord; - btQuaternionDoubleData targetOrnBaseCoord; - targetBaseCoord.getOrigin().serializeDouble(targetPosBaseCoord); - targetBaseCoord.getRotation().serializeDouble(targetOrnBaseCoord); - - // Set joint damping coefficents. A small default - // damping constant is added to prevent singularity - // with pseudo inverse. The user can set joint damping - // coefficients differently for each joint. The larger - // the damping coefficient is, the less we rely on - // this joint to achieve the IK target. - btAlignedObjectArray joint_damping; - joint_damping.resize(numDofs,0.5); - if (clientCmd.m_updateFlags& IK_HAS_JOINT_DAMPING) - { - for (int i = 0; i < numDofs; ++i) - { - joint_damping[i] = clientCmd.m_calculateInverseKinematicsArguments.m_jointDamping[i]; - } - } - ikHelperPtr->setDampingCoeff(numDofs, &joint_damping[0]); - - double targetDampCoeff[6] = { 1.0, 1.0, 1.0, 1.0, 1.0, 1.0 }; - ikHelperPtr->computeIK(targetPosBaseCoord.m_floats, targetOrnBaseCoord.m_floats, - endEffectorWorldPosition.m_floats, endEffectorWorldOrientation.m_floats, - &q_current[0], - numDofs, clientCmd.m_calculateInverseKinematicsArguments.m_endEffectorLinkIndex, - &q_new[0], ikMethod, &jacobian_linear[0], &jacobian_angular[0], jacSize*2, targetDampCoeff); - - serverCmd.m_inverseKinematicsResultArgs.m_bodyUniqueId =clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId; - for (int i=0;im_visualConverter.getNumVisualShapes(clientCmd.m_requestVisualShapeDataArguments.m_bodyUniqueId); - //int totalBytesPerVisualShape = sizeof (b3VisualShapeData); - //int visualShapeStorage = bufferSizeInBytes / totalBytesPerVisualShape - 1; - b3VisualShapeData* visualShapeStoragePtr = (b3VisualShapeData*)bufferServerToClient; - - int remain = totalNumVisualShapes - clientCmd.m_requestVisualShapeDataArguments.m_startingVisualShapeIndex; - int shapeIndex = clientCmd.m_requestVisualShapeDataArguments.m_startingVisualShapeIndex; - - int success = m_data->m_visualConverter.getVisualShapesData(clientCmd.m_requestVisualShapeDataArguments.m_bodyUniqueId, - shapeIndex, - visualShapeStoragePtr); - if (success) { - serverCmd.m_sendVisualShapeArgs.m_numRemainingVisualShapes = remain-1; - serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied = 1; - serverCmd.m_sendVisualShapeArgs.m_startingVisualShapeIndex = clientCmd.m_requestVisualShapeDataArguments.m_startingVisualShapeIndex; - serverCmd.m_sendVisualShapeArgs.m_bodyUniqueId = clientCmd.m_requestVisualShapeDataArguments.m_bodyUniqueId; - serverCmd.m_numDataStreamBytes = sizeof(b3VisualShapeData)*serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied; - serverCmd.m_type = CMD_VISUAL_SHAPE_INFO_COMPLETED; - } - hasStatus = true; - break; - } - case CMD_UPDATE_VISUAL_SHAPE: - { - BT_PROFILE("CMD_UPDATE_VISUAL_SHAPE"); - SharedMemoryStatus& serverCmd = serverStatusOut; - serverCmd.m_type = CMD_VISUAL_SHAPE_UPDATE_FAILED; - InternalTextureHandle* texHandle = 0; - - if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_TEXTURE) - { - texHandle = m_data->m_textureHandles.getHandle(clientCmd.m_updateVisualShapeDataArguments.m_textureUniqueId); - - if (clientCmd.m_updateVisualShapeDataArguments.m_textureUniqueId>=0) - { - if (texHandle) - { - m_data->m_visualConverter.activateShapeTexture(clientCmd.m_updateVisualShapeDataArguments.m_bodyUniqueId, clientCmd.m_updateVisualShapeDataArguments.m_jointIndex, clientCmd.m_updateVisualShapeDataArguments.m_shapeIndex, texHandle->m_tinyRendererTextureId); - } - } - } - - { - int bodyUniqueId = clientCmd.m_updateVisualShapeDataArguments.m_bodyUniqueId; - int linkIndex = clientCmd.m_updateVisualShapeDataArguments.m_jointIndex; - - InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(bodyUniqueId); - if (bodyHandle) - { - if (bodyHandle->m_multiBody) - { - if (linkIndex==-1) - { - if (bodyHandle->m_multiBody->getBaseCollider()) - { - int graphicsIndex = bodyHandle->m_multiBody->getBaseCollider()->getUserIndex(); - if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_TEXTURE) - { - if (texHandle) - { - int shapeIndex = m_data->m_guiHelper->getShapeIndexFromInstance(graphicsIndex); - m_data->m_guiHelper->replaceTexture(shapeIndex,texHandle->m_openglTextureId); - } - } - if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_RGBA_COLOR) - { - m_data->m_visualConverter.changeRGBAColor(bodyUniqueId,linkIndex,clientCmd.m_updateVisualShapeDataArguments.m_rgbaColor); - m_data->m_guiHelper->changeRGBAColor(graphicsIndex,clientCmd.m_updateVisualShapeDataArguments.m_rgbaColor); - } - if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_SPECULAR_COLOR) - { - m_data->m_guiHelper->changeSpecularColor(graphicsIndex,clientCmd.m_updateVisualShapeDataArguments.m_specularColor); - } - - } - } else - { - if (linkIndexm_multiBody->getNumLinks()) - { - if (bodyHandle->m_multiBody->getLink(linkIndex).m_collider) - { - int graphicsIndex = bodyHandle->m_multiBody->getLink(linkIndex).m_collider->getUserIndex(); - if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_TEXTURE) - { - if (texHandle) - { - int shapeIndex = m_data->m_guiHelper->getShapeIndexFromInstance(graphicsIndex); - m_data->m_guiHelper->replaceTexture(shapeIndex,texHandle->m_openglTextureId); - } - } - if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_RGBA_COLOR) - { - m_data->m_visualConverter.changeRGBAColor(bodyUniqueId,linkIndex,clientCmd.m_updateVisualShapeDataArguments.m_rgbaColor); - m_data->m_guiHelper->changeRGBAColor(graphicsIndex,clientCmd.m_updateVisualShapeDataArguments.m_rgbaColor); - } - if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_SPECULAR_COLOR) - { - m_data->m_guiHelper->changeSpecularColor(graphicsIndex,clientCmd.m_updateVisualShapeDataArguments.m_specularColor); - } - - } - } - } - } else - { - if (bodyHandle->m_rigidBody) - { - int graphicsIndex = bodyHandle->m_rigidBody->getUserIndex(); - if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_TEXTURE) - { - if (texHandle) - { - int shapeIndex = m_data->m_guiHelper->getShapeIndexFromInstance(graphicsIndex); - m_data->m_guiHelper->replaceTexture(shapeIndex,texHandle->m_openglTextureId); - } - } - if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_RGBA_COLOR) - { - m_data->m_visualConverter.changeRGBAColor(bodyUniqueId,linkIndex,clientCmd.m_updateVisualShapeDataArguments.m_rgbaColor); - m_data->m_guiHelper->changeRGBAColor(graphicsIndex,clientCmd.m_updateVisualShapeDataArguments.m_rgbaColor); - } - if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_SPECULAR_COLOR) - { - m_data->m_guiHelper->changeSpecularColor(graphicsIndex,clientCmd.m_updateVisualShapeDataArguments.m_specularColor); - } - } - } - } - } + } + } //if (body && body->m_rigidBody) + } + + serverStatusOut.m_type = CMD_DESIRED_STATE_RECEIVED_COMPLETED; + return hasStatus; +} + +bool PhysicsServerCommandProcessor::processRequestActualStateCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + serverStatusOut.m_type = CMD_ACTUAL_STATE_UPDATE_FAILED; - serverCmd.m_type = CMD_VISUAL_SHAPE_UPDATE_COMPLETED; - hasStatus = true; - - break; - } - - case CMD_CHANGE_TEXTURE: - { - SharedMemoryStatus& serverCmd = serverStatusOut; - serverCmd.m_type = CMD_CHANGE_TEXTURE_COMMAND_FAILED; - - InternalTextureHandle* texH = m_data->m_textureHandles.getHandle(clientCmd.m_changeTextureArgs.m_textureUniqueId); - if(texH) - { - int gltex = texH->m_openglTextureId; - m_data->m_guiHelper->changeTexture(gltex, - (const unsigned char*)bufferServerToClient, clientCmd.m_changeTextureArgs.m_width,clientCmd.m_changeTextureArgs.m_height); - - serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED; - } - hasStatus = true; - break; - } - case CMD_LOAD_TEXTURE: - { - BT_PROFILE("CMD_LOAD_TEXTURE"); - SharedMemoryStatus& serverCmd = serverStatusOut; - serverCmd.m_type = CMD_LOAD_TEXTURE_FAILED; - - char relativeFileName[1024]; - char pathPrefix[1024]; - - if(b3ResourcePath::findResourcePath(clientCmd.m_loadTextureArguments.m_textureFileName,relativeFileName,1024)) - { - b3FileUtils::extractPath(relativeFileName,pathPrefix,1024); - - int texHandle = m_data->m_textureHandles.allocHandle(); - InternalTextureHandle* texH = m_data->m_textureHandles.getHandle(texHandle); - if(texH) - { - texH->m_tinyRendererTextureId = -1; - texH->m_openglTextureId = -1; - - int uid = m_data->m_visualConverter.loadTextureFile(relativeFileName); - if(uid>=0) - { - texH->m_tinyRendererTextureId = uid; - } - - { - int width,height,n; - unsigned char* imageData= stbi_load(relativeFileName,&width,&height,&n,3); - - if(imageData) - { - texH->m_openglTextureId = m_data->m_guiHelper->registerTexture(imageData,width,height); - free(imageData); - } - else - { - b3Warning("Unsupported texture image format [%s]\n",relativeFileName); - } - } - serverCmd.m_loadTextureResultArguments.m_textureUniqueId = texHandle; - serverCmd.m_type = CMD_LOAD_TEXTURE_COMPLETED; - } - } - else - { - serverCmd.m_type = CMD_LOAD_TEXTURE_FAILED; - } - hasStatus = true; - - break; - } - - case CMD_LOAD_BULLET: - { - BT_PROFILE("CMD_LOAD_BULLET"); - SharedMemoryStatus& serverCmd = serverStatusOut; - btBulletWorldImporter* importer = new btBulletWorldImporter(m_data->m_dynamicsWorld); - - const char* prefix[] = { "", "./", "./data/", "../data/", "../../data/", "../../../data/", "../../../../data/" }; - int numPrefixes = sizeof(prefix) / sizeof(const char*); - char relativeFileName[1024]; - FILE* f = 0; - bool found = false; - - for (int i = 0; !f && iloadFile(relativeFileName); - if (ok) - { - - - int numRb = importer->getNumRigidBodies(); - serverStatusOut.m_sdfLoadedArgs.m_numBodies = 0; - serverStatusOut.m_sdfLoadedArgs.m_numUserConstraints = 0; - - for( int i=0;igetRigidBodyByIndex(i); - if (colObj) - { - btRigidBody* rb = btRigidBody::upcast(colObj); - if (rb) - { - int bodyUniqueId = m_data->m_bodyHandles.allocHandle(); - InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(bodyUniqueId); - colObj->setUserIndex2(bodyUniqueId); - bodyHandle->m_rigidBody = rb; - - if (serverStatusOut.m_sdfLoadedArgs.m_numBodiesm_guiHelper->autogenerateGraphicsObjects(m_data->m_dynamicsWorld); - hasStatus = true; - break; - } - } - serverCmd.m_type = CMD_BULLET_LOADING_FAILED; - hasStatus = true; - break; - } - - case CMD_SAVE_BULLET: - { - BT_PROFILE("CMD_SAVE_BULLET"); - SharedMemoryStatus& serverCmd = serverStatusOut; - - FILE* f = fopen(clientCmd.m_fileArguments.m_fileName, "wb"); - if (f) - { - btDefaultSerializer* ser = new btDefaultSerializer(); - m_data->m_dynamicsWorld->serialize(ser); - fwrite(ser->getBufferPointer(), ser->getCurrentBufferSize(), 1, f); - fclose(f); - serverCmd.m_type = CMD_BULLET_SAVING_COMPLETED; - delete ser; - } - serverCmd.m_type = CMD_BULLET_SAVING_FAILED; - hasStatus = true; - break; - } - - case CMD_LOAD_MJCF: - { - BT_PROFILE("CMD_LOAD_MJCF"); - SharedMemoryStatus& serverCmd = serverStatusOut; - serverCmd.m_type = CMD_MJCF_LOADING_FAILED; - const MjcfArgs& mjcfArgs = clientCmd.m_mjcfArguments; - if (m_data->m_verboseOutput) - { - b3Printf("Processed CMD_LOAD_MJCF:%s", mjcfArgs.m_mjcfFileName); - } - bool useMultiBody=(clientCmd.m_updateFlags & URDF_ARGS_USE_MULTIBODY) ? (mjcfArgs.m_useMultiBody!=0) : true; - int flags = CUF_USE_MJCF; - if (clientCmd.m_updateFlags&URDF_ARGS_HAS_CUSTOM_URDF_FLAGS) - { - flags |= clientCmd.m_mjcfArguments.m_flags; - } - - bool completedOk = loadMjcf(mjcfArgs.m_mjcfFileName,bufferServerToClient, bufferSizeInBytes, useMultiBody, flags); - if (completedOk) - { - m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld); - - serverStatusOut.m_sdfLoadedArgs.m_numBodies = m_data->m_sdfRecentLoadedBodies.size(); - serverStatusOut.m_sdfLoadedArgs.m_numUserConstraints = 0; - int maxBodies = btMin(MAX_SDF_BODIES, serverStatusOut.m_sdfLoadedArgs.m_numBodies); - for (int i=0;im_sdfRecentLoadedBodies[i]; - } - - serverStatusOut.m_type = CMD_MJCF_LOADING_COMPLETED; - } else - { - serverStatusOut.m_type = CMD_MJCF_LOADING_FAILED; - } - hasStatus = true; - break; - - } - - case CMD_USER_DEBUG_DRAW: - { - BT_PROFILE("CMD_USER_DEBUG_DRAW"); - SharedMemoryStatus& serverCmd = serverStatusOut; - serverCmd.m_type = CMD_USER_DEBUG_DRAW_FAILED; - hasStatus = true; - - - int trackingVisualShapeIndex = -1; - - if (clientCmd.m_userDebugDrawArgs.m_parentObjectUniqueId>=0) - { - InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(clientCmd.m_userDebugDrawArgs.m_parentObjectUniqueId); - if (bodyHandle) - { - int linkIndex = -1; - - if (bodyHandle->m_multiBody) - { - int linkIndex = clientCmd.m_userDebugDrawArgs.m_parentLinkIndex; - if (linkIndex ==-1) - { - if (bodyHandle->m_multiBody->getBaseCollider()) - { - trackingVisualShapeIndex = bodyHandle->m_multiBody->getBaseCollider()->getUserIndex(); - } - } else - { - if (linkIndex >=0 && linkIndex < bodyHandle->m_multiBody->getNumLinks()) - { - if (bodyHandle->m_multiBody->getLink(linkIndex).m_collider) - { - trackingVisualShapeIndex = bodyHandle->m_multiBody->getLink(linkIndex).m_collider->getUserIndex(); - } - } - - } - } - if (bodyHandle->m_rigidBody) - { - trackingVisualShapeIndex = bodyHandle->m_rigidBody->getUserIndex(); - } - } - } - - if (clientCmd.m_updateFlags & USER_DEBUG_ADD_PARAMETER) - { - int uid = m_data->m_guiHelper->addUserDebugParameter( - clientCmd.m_userDebugDrawArgs.m_text, - clientCmd.m_userDebugDrawArgs.m_rangeMin, - clientCmd.m_userDebugDrawArgs.m_rangeMax, - clientCmd.m_userDebugDrawArgs.m_startValue - ); - serverCmd.m_userDebugDrawArgs.m_debugItemUniqueId = uid; - serverCmd.m_type = CMD_USER_DEBUG_DRAW_COMPLETED; - } - if (clientCmd.m_updateFlags &USER_DEBUG_READ_PARAMETER) - { - - int ok = m_data->m_guiHelper->readUserDebugParameter( - clientCmd.m_userDebugDrawArgs.m_itemUniqueId, - &serverCmd.m_userDebugDrawArgs.m_parameterValue); - if (ok) - { - serverCmd.m_type = CMD_USER_DEBUG_DRAW_PARAMETER_COMPLETED; - } - } - if ((clientCmd.m_updateFlags & USER_DEBUG_SET_CUSTOM_OBJECT_COLOR) || (clientCmd.m_updateFlags & USER_DEBUG_REMOVE_CUSTOM_OBJECT_COLOR)) - { - int bodyUniqueId = clientCmd.m_userDebugDrawArgs.m_objectUniqueId; - InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); - if (body) - { - btCollisionObject* destColObj = 0; - - if (body->m_multiBody) - { - if (clientCmd.m_userDebugDrawArgs.m_linkIndex == -1) - { - destColObj = body->m_multiBody->getBaseCollider(); - } - else - { - if (clientCmd.m_userDebugDrawArgs.m_linkIndex >= 0 && clientCmd.m_userDebugDrawArgs.m_linkIndex < body->m_multiBody->getNumLinks()) - { - destColObj = body->m_multiBody->getLink(clientCmd.m_userDebugDrawArgs.m_linkIndex).m_collider; - } - } - - } - if (body->m_rigidBody) - { - destColObj = body->m_rigidBody; - } - - if (destColObj) - { - if (clientCmd.m_updateFlags & USER_DEBUG_REMOVE_CUSTOM_OBJECT_COLOR) - { - destColObj->removeCustomDebugColor(); - serverCmd.m_type = CMD_USER_DEBUG_DRAW_COMPLETED; - } - if (clientCmd.m_updateFlags & USER_DEBUG_SET_CUSTOM_OBJECT_COLOR) - { - btVector3 objectColorRGB; - objectColorRGB.setValue(clientCmd.m_userDebugDrawArgs.m_objectDebugColorRGB[0], - clientCmd.m_userDebugDrawArgs.m_objectDebugColorRGB[1], - clientCmd.m_userDebugDrawArgs.m_objectDebugColorRGB[2]); - destColObj->setCustomDebugColor(objectColorRGB); - serverCmd.m_type = CMD_USER_DEBUG_DRAW_COMPLETED; - } - } - } - } - - if (clientCmd.m_updateFlags & USER_DEBUG_HAS_TEXT) - { - //addUserDebugText3D( const double orientation[4], const double textColorRGB[3], double size, double lifeTime, int trackingObjectUniqueId, int optionFlags){return -1;} - - int optionFlags = clientCmd.m_userDebugDrawArgs.m_optionFlags; - - if ((clientCmd.m_updateFlags & USER_DEBUG_HAS_TEXT_ORIENTATION)==0) - { - optionFlags |= DEB_DEBUG_TEXT_ALWAYS_FACE_CAMERA; - } - - - int replaceItemUniqueId = -1; - if ((clientCmd.m_updateFlags & USER_DEBUG_HAS_REPLACE_ITEM_UNIQUE_ID)!=0) - { - replaceItemUniqueId = clientCmd.m_userDebugDrawArgs.m_replaceItemUniqueId; - } - - - int uid = m_data->m_guiHelper->addUserDebugText3D(clientCmd.m_userDebugDrawArgs.m_text, - clientCmd.m_userDebugDrawArgs.m_textPositionXYZ, - clientCmd.m_userDebugDrawArgs.m_textOrientation, - clientCmd.m_userDebugDrawArgs.m_textColorRGB, - clientCmd.m_userDebugDrawArgs.m_textSize, - clientCmd.m_userDebugDrawArgs.m_lifeTime, - trackingVisualShapeIndex, - optionFlags, - replaceItemUniqueId); - - if (uid>=0) - { - serverCmd.m_userDebugDrawArgs.m_debugItemUniqueId = uid; - serverCmd.m_type = CMD_USER_DEBUG_DRAW_COMPLETED; - } - } - - if (clientCmd.m_updateFlags & USER_DEBUG_HAS_LINE) - { - int uid = m_data->m_guiHelper->addUserDebugLine( - clientCmd.m_userDebugDrawArgs.m_debugLineFromXYZ, - clientCmd.m_userDebugDrawArgs.m_debugLineToXYZ, - clientCmd.m_userDebugDrawArgs.m_debugLineColorRGB, - clientCmd.m_userDebugDrawArgs.m_lineWidth, - clientCmd.m_userDebugDrawArgs.m_lifeTime, - trackingVisualShapeIndex); - - if (uid>=0) - { - serverCmd.m_userDebugDrawArgs.m_debugItemUniqueId = uid; - serverCmd.m_type = CMD_USER_DEBUG_DRAW_COMPLETED; - } - } + BT_PROFILE("CMD_REQUEST_ACTUAL_STATE"); + if (m_data->m_verboseOutput) + { + b3Printf("Sending the actual state (Q,U)"); + } + int bodyUniqueId = clientCmd.m_requestActualStateInformationCommandArgument.m_bodyUniqueId; + InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); - if (clientCmd.m_updateFlags & USER_DEBUG_REMOVE_ALL) - { - m_data->m_guiHelper->removeAllUserDebugItems(); - serverCmd.m_type = CMD_USER_DEBUG_DRAW_COMPLETED; + if (body && body->m_multiBody) + { + btMultiBody* mb = body->m_multiBody; + SharedMemoryStatus& serverCmd = serverStatusOut; + serverStatusOut.m_type = CMD_ACTUAL_STATE_UPDATE_COMPLETED; - } - if (clientCmd.m_updateFlags & USER_DEBUG_REMOVE_ONE_ITEM) - { - m_data->m_guiHelper->removeUserDebugItem(clientCmd.m_userDebugDrawArgs.m_itemUniqueId); - serverCmd.m_type = CMD_USER_DEBUG_DRAW_COMPLETED; + serverCmd.m_sendActualStateArgs.m_bodyUniqueId = bodyUniqueId; + serverCmd.m_sendActualStateArgs.m_numLinks = body->m_multiBody->getNumLinks(); - } + int totalDegreeOfFreedomQ = 0; + int totalDegreeOfFreedomU = 0; - break; - } - case CMD_CUSTOM_COMMAND: - { - SharedMemoryStatus& serverCmd = serverStatusOut; - serverCmd.m_type = CMD_CUSTOM_COMMAND_FAILED; - serverCmd.m_customCommandResultArgs.m_pluginUniqueId = -1; + if (mb->getNumLinks()>= MAX_DEGREE_OF_FREEDOM) + { + serverStatusOut.m_type = CMD_ACTUAL_STATE_UPDATE_FAILED; + hasStatus = true; + return hasStatus; + } - if (clientCmd.m_updateFlags & CMD_CUSTOM_COMMAND_LOAD_PLUGIN) - { - //pluginPath could be registered or load from disk - const char* postFix = ""; - if (clientCmd.m_updateFlags & CMD_CUSTOM_COMMAND_LOAD_PLUGIN_POSTFIX) - { - postFix = clientCmd.m_customCommandArgs.m_postFix; - } + //always add the base, even for static (non-moving objects) + //so that we can easily move the 'fixed' base when needed + //do we don't use this conditional "if (!mb->hasFixedBase())" + { + btTransform tr; + tr.setOrigin(mb->getBasePos()); + tr.setRotation(mb->getWorldToBaseRot().inverse()); + + serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[0] = + body->m_rootLocalInertialFrame.getOrigin()[0]; + serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[1] = + body->m_rootLocalInertialFrame.getOrigin()[1]; + serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[2] = + body->m_rootLocalInertialFrame.getOrigin()[2]; + + serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[3] = + body->m_rootLocalInertialFrame.getRotation()[0]; + serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[4] = + body->m_rootLocalInertialFrame.getRotation()[1]; + serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[5] = + body->m_rootLocalInertialFrame.getRotation()[2]; + serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[6] = + body->m_rootLocalInertialFrame.getRotation()[3]; - int pluginUniqueId = m_data->m_pluginManager.loadPlugin(clientCmd.m_customCommandArgs.m_pluginPath, postFix); - if (pluginUniqueId>=0) - { - serverCmd.m_customCommandResultArgs.m_pluginUniqueId = pluginUniqueId; - serverCmd.m_type = CMD_CUSTOM_COMMAND_COMPLETED; - } - } - if (clientCmd.m_updateFlags & CMD_CUSTOM_COMMAND_UNLOAD_PLUGIN) - { - m_data->m_pluginManager.unloadPlugin(clientCmd.m_customCommandArgs.m_pluginUniqueId); - serverCmd.m_type = CMD_CUSTOM_COMMAND_COMPLETED; - } - if (clientCmd.m_updateFlags & CMD_CUSTOM_COMMAND_EXECUTE_PLUGIN_COMMAND) - { - int result = m_data->m_pluginManager.executePluginCommand(clientCmd.m_customCommandArgs.m_pluginUniqueId, &clientCmd.m_customCommandArgs.m_arguments); - serverCmd.m_customCommandResultArgs.m_executeCommandResult = result; - serverCmd.m_type = CMD_CUSTOM_COMMAND_COMPLETED; - } + //base position in world space, carthesian + serverCmd.m_sendActualStateArgs.m_actualStateQ[0] = tr.getOrigin()[0]; + serverCmd.m_sendActualStateArgs.m_actualStateQ[1] = tr.getOrigin()[1]; + serverCmd.m_sendActualStateArgs.m_actualStateQ[2] = tr.getOrigin()[2]; - hasStatus = true; - break; - } - default: + //base orientation, quaternion x,y,z,w, in world space, carthesian + serverCmd.m_sendActualStateArgs.m_actualStateQ[3] = tr.getRotation()[0]; + serverCmd.m_sendActualStateArgs.m_actualStateQ[4] = tr.getRotation()[1]; + serverCmd.m_sendActualStateArgs.m_actualStateQ[5] = tr.getRotation()[2]; + serverCmd.m_sendActualStateArgs.m_actualStateQ[6] = tr.getRotation()[3]; + totalDegreeOfFreedomQ +=7;//pos + quaternion + + //base linear velocity (in world space, carthesian) + serverCmd.m_sendActualStateArgs.m_actualStateQdot[0] = mb->getBaseVel()[0]; + serverCmd.m_sendActualStateArgs.m_actualStateQdot[1] = mb->getBaseVel()[1]; + serverCmd.m_sendActualStateArgs.m_actualStateQdot[2] = mb->getBaseVel()[2]; + + //base angular velocity (in world space, carthesian) + serverCmd.m_sendActualStateArgs.m_actualStateQdot[3] = mb->getBaseOmega()[0]; + serverCmd.m_sendActualStateArgs.m_actualStateQdot[4] = mb->getBaseOmega()[1]; + serverCmd.m_sendActualStateArgs.m_actualStateQdot[5] = mb->getBaseOmega()[2]; + totalDegreeOfFreedomU += 6;//3 linear and 3 angular DOF + } + + btAlignedObjectArray omega; + btAlignedObjectArray linVel; + + bool computeForwardKinematics = ((clientCmd.m_updateFlags & ACTUAL_STATE_COMPUTE_FORWARD_KINEMATICS)!=0); + if (computeForwardKinematics) + { + B3_PROFILE("compForwardKinematics"); + btAlignedObjectArray world_to_local; + btAlignedObjectArray local_origin; + world_to_local.resize(mb->getNumLinks()+1); + local_origin.resize(mb->getNumLinks()+1); + mb->forwardKinematics(world_to_local,local_origin); + } + + bool computeLinkVelocities = ((clientCmd.m_updateFlags & ACTUAL_STATE_COMPUTE_LINKVELOCITY)!=0); + if (computeLinkVelocities) + { + omega.resize(mb->getNumLinks()+1); + linVel.resize(mb->getNumLinks()+1); + { + B3_PROFILE("compTreeLinkVelocities"); + mb->compTreeLinkVelocities(&omega[0], &linVel[0]); + } + } + for (int l=0;lgetNumLinks();l++) + { + for (int d=0;dgetLink(l).m_posVarCount;d++) + { + serverCmd.m_sendActualStateArgs.m_actualStateQ[totalDegreeOfFreedomQ++] = mb->getJointPosMultiDof(l)[d]; + } + for (int d=0;dgetLink(l).m_dofCount;d++) + { + serverCmd.m_sendActualStateArgs.m_actualStateQdot[totalDegreeOfFreedomU++] = mb->getJointVelMultiDof(l)[d]; + } + + if (0 == mb->getLink(l).m_jointFeedback) + { + for (int d=0;d<6;d++) { - BT_PROFILE("CMD_UNKNOWN"); - b3Error("Unknown command encountered"); - - SharedMemoryStatus& serverCmd =serverStatusOut; - serverCmd.m_type = CMD_UNKNOWN_COMMAND_FLUSHED; - hasStatus = true; - - + serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+d]=0; } - }; + } else + { + btVector3 sensedForce = mb->getLink(l).m_jointFeedback->m_reactionForces.getLinear(); + btVector3 sensedTorque = mb->getLink(l).m_jointFeedback->m_reactionForces.getAngular(); + + serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+0] = sensedForce[0]; + serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+1] = sensedForce[1]; + serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+2] = sensedForce[2]; + + serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+3] = sensedTorque[0]; + serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+4] = sensedTorque[1]; + serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+5] = sensedTorque[2]; + } + + serverCmd.m_sendActualStateArgs.m_jointMotorForce[l] = 0; + + if (supportsJointMotor(mb,l)) + { + + btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)body->m_multiBody->getLink(l).m_userPtr; + + if (motor && m_data->m_physicsDeltaTime>btScalar(0)) + { + btScalar force =motor->getAppliedImpulse(0)/m_data->m_physicsDeltaTime; + serverCmd.m_sendActualStateArgs.m_jointMotorForce[l] = + force; + //if (force>0) + //{ + // b3Printf("force = %f\n", force); + //} + } + } + btVector3 linkLocalInertialOrigin = body->m_linkLocalInertialFrames[l].getOrigin(); + btQuaternion linkLocalInertialRotation = body->m_linkLocalInertialFrames[l].getRotation(); + + btVector3 linkCOMOrigin = mb->getLink(l).m_cachedWorldTransform.getOrigin(); + btQuaternion linkCOMRotation = mb->getLink(l).m_cachedWorldTransform.getRotation(); + + serverCmd.m_sendActualStateArgs.m_linkState[l*7+0] = linkCOMOrigin.getX(); + serverCmd.m_sendActualStateArgs.m_linkState[l*7+1] = linkCOMOrigin.getY(); + serverCmd.m_sendActualStateArgs.m_linkState[l*7+2] = linkCOMOrigin.getZ(); + serverCmd.m_sendActualStateArgs.m_linkState[l*7+3] = linkCOMRotation.x(); + serverCmd.m_sendActualStateArgs.m_linkState[l*7+4] = linkCOMRotation.y(); + serverCmd.m_sendActualStateArgs.m_linkState[l*7+5] = linkCOMRotation.z(); + serverCmd.m_sendActualStateArgs.m_linkState[l*7+6] = linkCOMRotation.w(); + + + + btVector3 worldLinVel(0,0,0); + btVector3 worldAngVel(0,0,0); + + if (computeLinkVelocities) + { + const btMatrix3x3& linkRotMat = mb->getLink(l).m_cachedWorldTransform.getBasis(); + worldLinVel = linkRotMat * linVel[l+1]; + worldAngVel = linkRotMat * omega[l+1]; + } + + serverCmd.m_sendActualStateArgs.m_linkWorldVelocities[l*6+0] = worldLinVel[0]; + serverCmd.m_sendActualStateArgs.m_linkWorldVelocities[l*6+1] = worldLinVel[1]; + serverCmd.m_sendActualStateArgs.m_linkWorldVelocities[l*6+2] = worldLinVel[2]; + serverCmd.m_sendActualStateArgs.m_linkWorldVelocities[l*6+3] = worldAngVel[0]; + serverCmd.m_sendActualStateArgs.m_linkWorldVelocities[l*6+4] = worldAngVel[1]; + serverCmd.m_sendActualStateArgs.m_linkWorldVelocities[l*6+5] = worldAngVel[2]; + + serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+0] = linkLocalInertialOrigin.getX(); + serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+1] = linkLocalInertialOrigin.getY(); + serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+2] = linkLocalInertialOrigin.getZ(); + + serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+3] = linkLocalInertialRotation.x(); + serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+4] = linkLocalInertialRotation.y(); + serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+5] = linkLocalInertialRotation.z(); + serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+6] = linkLocalInertialRotation.w(); } + + + serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomQ = totalDegreeOfFreedomQ; + serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomU = totalDegreeOfFreedomU; + + hasStatus = true; + + } else + { + if (body && body->m_rigidBody) + { + btRigidBody* rb = body->m_rigidBody; + SharedMemoryStatus& serverCmd = serverStatusOut; + serverCmd.m_type = CMD_ACTUAL_STATE_UPDATE_COMPLETED; + + serverCmd.m_sendActualStateArgs.m_bodyUniqueId = bodyUniqueId; + serverCmd.m_sendActualStateArgs.m_numLinks = 0; + + int totalDegreeOfFreedomQ = 0; + int totalDegreeOfFreedomU = 0; + + btTransform tr = rb->getWorldTransform(); + //base position in world space, carthesian + serverCmd.m_sendActualStateArgs.m_actualStateQ[0] = tr.getOrigin()[0]; + serverCmd.m_sendActualStateArgs.m_actualStateQ[1] = tr.getOrigin()[1]; + serverCmd.m_sendActualStateArgs.m_actualStateQ[2] = tr.getOrigin()[2]; + + //base orientation, quaternion x,y,z,w, in world space, carthesian + serverCmd.m_sendActualStateArgs.m_actualStateQ[3] = tr.getRotation()[0]; + serverCmd.m_sendActualStateArgs.m_actualStateQ[4] = tr.getRotation()[1]; + serverCmd.m_sendActualStateArgs.m_actualStateQ[5] = tr.getRotation()[2]; + serverCmd.m_sendActualStateArgs.m_actualStateQ[6] = tr.getRotation()[3]; + totalDegreeOfFreedomQ +=7;//pos + quaternion + + //base linear velocity (in world space, carthesian) + serverCmd.m_sendActualStateArgs.m_actualStateQdot[0] = rb->getLinearVelocity()[0]; + serverCmd.m_sendActualStateArgs.m_actualStateQdot[1] = rb->getLinearVelocity()[1]; + serverCmd.m_sendActualStateArgs.m_actualStateQdot[2] = rb->getLinearVelocity()[2]; + + //base angular velocity (in world space, carthesian) + serverCmd.m_sendActualStateArgs.m_actualStateQdot[3] = rb->getAngularVelocity()[0]; + serverCmd.m_sendActualStateArgs.m_actualStateQdot[4] = rb->getAngularVelocity()[1]; + serverCmd.m_sendActualStateArgs.m_actualStateQdot[5] = rb->getAngularVelocity()[2]; + totalDegreeOfFreedomU += 6;//3 linear and 3 angular DOF + + serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomQ = totalDegreeOfFreedomQ; + serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomU = totalDegreeOfFreedomU; + + hasStatus = true; + } else + { + //b3Warning("Request state but no multibody or rigid body available"); + SharedMemoryStatus& serverCmd = serverStatusOut; + serverCmd.m_type = CMD_ACTUAL_STATE_UPDATE_FAILED; + hasStatus = true; + } + } + return hasStatus; +} + + +bool PhysicsServerCommandProcessor::processRequestContactpointInformationCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + BT_PROFILE("CMD_REQUEST_CONTACT_POINT_INFORMATION"); + SharedMemoryStatus& serverCmd =serverStatusOut; + serverCmd.m_sendContactPointArgs.m_numContactPointsCopied = 0; + + //make a snapshot of the contact manifolds into individual contact points + if (clientCmd.m_requestContactPointArguments.m_startingContactPointIndex == 0) + { + m_data->m_cachedContactPoints.resize(0); + + int mode = CONTACT_QUERY_MODE_REPORT_EXISTING_CONTACT_POINTS; + + if (clientCmd.m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_QUERY_MODE) + { + mode = clientCmd.m_requestContactPointArguments.m_mode; + } + + switch (mode) + { + case CONTACT_QUERY_MODE_REPORT_EXISTING_CONTACT_POINTS: + { + int numContactManifolds = m_data->m_dynamicsWorld->getDispatcher()->getNumManifolds(); + m_data->m_cachedContactPoints.reserve(numContactManifolds * 4); + for (int i = 0; i < numContactManifolds; i++) + { + const btPersistentManifold* manifold = m_data->m_dynamicsWorld->getDispatcher()->getInternalManifoldPointer()[i]; + int linkIndexA = -1; + int linkIndexB = -1; + + int objectIndexB = -1; + const btRigidBody* bodyB = btRigidBody::upcast(manifold->getBody1()); + if (bodyB) + { + objectIndexB = bodyB->getUserIndex2(); + } + const btMultiBodyLinkCollider* mblB = btMultiBodyLinkCollider::upcast(manifold->getBody1()); + if (mblB && mblB->m_multiBody) + { + linkIndexB = mblB->m_link; + objectIndexB = mblB->m_multiBody->getUserIndex2(); + } + + int objectIndexA = -1; + const btRigidBody* bodyA = btRigidBody::upcast(manifold->getBody0()); + if (bodyA) + { + objectIndexA = bodyA->getUserIndex2(); + } + const btMultiBodyLinkCollider* mblA = btMultiBodyLinkCollider::upcast(manifold->getBody0()); + if (mblA && mblA->m_multiBody) + { + linkIndexA = mblA->m_link; + objectIndexA = mblA->m_multiBody->getUserIndex2(); + } + btAssert(bodyA || mblA); + + //apply the filter, if the user provides it + bool swap = false; + if (clientCmd.m_requestContactPointArguments.m_objectAIndexFilter >= 0) + { + if (clientCmd.m_requestContactPointArguments.m_objectAIndexFilter == objectIndexA) + { + swap = false; + } + else if (clientCmd.m_requestContactPointArguments.m_objectAIndexFilter == objectIndexB) + { + swap = true; + } + else + { + continue; + } + } + + if (swap) + { + std::swap(objectIndexA, objectIndexB); + std::swap(linkIndexA, linkIndexB); + std::swap(bodyA, bodyB); + } + + //apply the second object filter, if the user provides it + if (clientCmd.m_requestContactPointArguments.m_objectBIndexFilter >= 0) + { + if (clientCmd.m_requestContactPointArguments.m_objectBIndexFilter != objectIndexB) + { + continue; + } + } + + if ( + (clientCmd.m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_LINK_INDEX_A_FILTER) && + clientCmd.m_requestContactPointArguments.m_linkIndexAIndexFilter != linkIndexA) + { + continue; + } + + if ( + (clientCmd.m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_LINK_INDEX_B_FILTER) && + clientCmd.m_requestContactPointArguments.m_linkIndexBIndexFilter != linkIndexB) + { + continue; + } + + for (int p = 0; p < manifold->getNumContacts(); p++) + { + + b3ContactPointData pt; + pt.m_bodyUniqueIdA = objectIndexA; + pt.m_bodyUniqueIdB = objectIndexB; + const btManifoldPoint& srcPt = manifold->getContactPoint(p); + pt.m_contactDistance = srcPt.getDistance(); + pt.m_contactFlags = 0; + pt.m_linkIndexA = linkIndexA; + pt.m_linkIndexB = linkIndexB; + for (int j = 0; j < 3; j++) + { + pt.m_contactNormalOnBInWS[j] = srcPt.m_normalWorldOnB[j]; + pt.m_positionOnAInWS[j] = srcPt.getPositionWorldOnA()[j]; + pt.m_positionOnBInWS[j] = srcPt.getPositionWorldOnB()[j]; + } + pt.m_normalForce = srcPt.getAppliedImpulse() / m_data->m_physicsDeltaTime; + // pt.m_linearFrictionForce = srcPt.m_appliedImpulseLateral1; + m_data->m_cachedContactPoints.push_back(pt); + } + } + break; + } + + case CONTACT_QUERY_MODE_COMPUTE_CLOSEST_POINTS: + { + //todo(erwincoumans) compute closest points between all, and vs all, pair + btScalar closestDistanceThreshold = 0.f; + + if (clientCmd.m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_CLOSEST_DISTANCE_THRESHOLD) + { + closestDistanceThreshold = clientCmd.m_requestContactPointArguments.m_closestDistanceThreshold; + } + + int bodyUniqueIdA = clientCmd.m_requestContactPointArguments.m_objectAIndexFilter; + int bodyUniqueIdB = clientCmd.m_requestContactPointArguments.m_objectBIndexFilter; + + bool hasLinkIndexAFilter = (0!=(clientCmd.m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_LINK_INDEX_A_FILTER)); + bool hasLinkIndexBFilter = (0!=(clientCmd.m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_LINK_INDEX_B_FILTER)); + + int linkIndexA = clientCmd.m_requestContactPointArguments.m_linkIndexAIndexFilter; + int linkIndexB = clientCmd.m_requestContactPointArguments.m_linkIndexBIndexFilter; + + btAlignedObjectArray setA; + btAlignedObjectArray setB; + btAlignedObjectArray setALinkIndex; + btAlignedObjectArray setBLinkIndex; + + if (bodyUniqueIdA >= 0) + { + InternalBodyData* bodyA = m_data->m_bodyHandles.getHandle(bodyUniqueIdA); + if (bodyA) + { + if (bodyA->m_multiBody) + { + if (bodyA->m_multiBody->getBaseCollider()) + { + if (!hasLinkIndexAFilter || (linkIndexA == -1)) + { + setA.push_back(bodyA->m_multiBody->getBaseCollider()); + setALinkIndex.push_back(-1); + } + } + for (int i = 0; i < bodyA->m_multiBody->getNumLinks(); i++) + { + if (bodyA->m_multiBody->getLink(i).m_collider) + { + if (!hasLinkIndexAFilter || (linkIndexA == i)) + { + setA.push_back(bodyA->m_multiBody->getLink(i).m_collider); + setALinkIndex.push_back(i); + } + } + } + } + if (bodyA->m_rigidBody) + { + setA.push_back(bodyA->m_rigidBody); + setALinkIndex.push_back(-1); + } + } + } + if (bodyUniqueIdB>=0) + { + InternalBodyData* bodyB = m_data->m_bodyHandles.getHandle(bodyUniqueIdB); + if (bodyB) + { + if (bodyB->m_multiBody) + { + if (bodyB->m_multiBody->getBaseCollider()) + { + if (!hasLinkIndexBFilter || (linkIndexB == -1)) + { + setB.push_back(bodyB->m_multiBody->getBaseCollider()); + setBLinkIndex.push_back(-1); + } + } + for (int i = 0; i < bodyB->m_multiBody->getNumLinks(); i++) + { + if (bodyB->m_multiBody->getLink(i).m_collider) + { + if (!hasLinkIndexBFilter || (linkIndexB ==i)) + { + setB.push_back(bodyB->m_multiBody->getLink(i).m_collider); + setBLinkIndex.push_back(i); + } + } + } + } + if (bodyB->m_rigidBody) + { + setB.push_back(bodyB->m_rigidBody); + setBLinkIndex.push_back(-1); + + } + } + } + + { + ///ContactResultCallback is used to report contact points + struct MyContactResultCallback : public btCollisionWorld::ContactResultCallback + { + int m_bodyUniqueIdA; + int m_bodyUniqueIdB; + int m_linkIndexA; + int m_linkIndexB; + btScalar m_deltaTime; + + btAlignedObjectArray& m_cachedContactPoints; + + MyContactResultCallback(btAlignedObjectArray& pointCache) + :m_cachedContactPoints(pointCache) + { + } + + virtual ~MyContactResultCallback() + { + } + + virtual bool needsCollision(btBroadphaseProxy* proxy0) const + { + //bool collides = (proxy0->m_collisionFilterGroup & m_collisionFilterMask) != 0; + //collides = collides && (m_collisionFilterGroup & proxy0->m_collisionFilterMask); + //return collides; + return true; + } + + virtual btScalar addSingleResult(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap, int partId0, int index0, const btCollisionObjectWrapper* colObj1Wrap, int partId1, int index1) + { + if (cp.m_distance1<=m_closestDistanceThreshold) + { + b3ContactPointData pt; + pt.m_bodyUniqueIdA = m_bodyUniqueIdA; + pt.m_bodyUniqueIdB = m_bodyUniqueIdB; + const btManifoldPoint& srcPt = cp; + pt.m_contactDistance = srcPt.getDistance(); + pt.m_contactFlags = 0; + pt.m_linkIndexA = m_linkIndexA; + pt.m_linkIndexB = m_linkIndexB; + for (int j = 0; j < 3; j++) + { + pt.m_contactNormalOnBInWS[j] = srcPt.m_normalWorldOnB[j]; + pt.m_positionOnAInWS[j] = srcPt.getPositionWorldOnA()[j]; + pt.m_positionOnBInWS[j] = srcPt.getPositionWorldOnB()[j]; + } + pt.m_normalForce = srcPt.getAppliedImpulse() / m_deltaTime; + // pt.m_linearFrictionForce = srcPt.m_appliedImpulseLateral1; + m_cachedContactPoints.push_back(pt); + } + return 1; + + } + }; + + + MyContactResultCallback cb(m_data->m_cachedContactPoints); + + cb.m_bodyUniqueIdA = bodyUniqueIdA; + cb.m_bodyUniqueIdB = bodyUniqueIdB; + cb.m_deltaTime = m_data->m_physicsDeltaTime; + + for (int i = 0; i < setA.size(); i++) + { + cb.m_linkIndexA = setALinkIndex[i]; + for (int j = 0; j < setB.size(); j++) + { + cb.m_linkIndexB = setBLinkIndex[j]; + cb.m_closestDistanceThreshold = closestDistanceThreshold; + this->m_data->m_dynamicsWorld->contactPairTest(setA[i], setB[j], cb); + } + } + } + + break; + } + default: + { + b3Warning("Unknown contact query mode: %d", mode); + } + + } + } + + int numContactPoints = m_data->m_cachedContactPoints.size(); + + + //b3ContactPoint + //struct b3ContactPointDynamics + + int totalBytesPerContact = sizeof(b3ContactPointData); + int contactPointStorage = bufferSizeInBytes/totalBytesPerContact-1; + + b3ContactPointData* contactData = (b3ContactPointData*)bufferServerToClient; + + int startContactPointIndex = clientCmd.m_requestContactPointArguments.m_startingContactPointIndex; + int numContactPointBatch = btMin(numContactPoints,contactPointStorage); + + int endContactPointIndex = startContactPointIndex+numContactPointBatch; + + for (int i=startContactPointIndex;im_cachedContactPoints[i]; + b3ContactPointData& destPt = contactData[serverCmd.m_sendContactPointArgs.m_numContactPointsCopied]; + destPt = srcPt; + serverCmd.m_sendContactPointArgs.m_numContactPointsCopied++; + } + + serverCmd.m_sendContactPointArgs.m_startingContactPointIndex = clientCmd.m_requestContactPointArguments.m_startingContactPointIndex; + serverCmd.m_sendContactPointArgs.m_numRemainingContactPoints = numContactPoints - clientCmd.m_requestContactPointArguments.m_startingContactPointIndex - serverCmd.m_sendContactPointArgs.m_numContactPointsCopied; + serverCmd.m_numDataStreamBytes = totalBytesPerContact * serverCmd.m_sendContactPointArgs.m_numContactPointsCopied; + serverCmd.m_type = CMD_CONTACT_POINT_INFORMATION_COMPLETED; //CMD_CONTACT_POINT_INFORMATION_FAILED, + + return hasStatus; +} + +bool PhysicsServerCommandProcessor::processRequestBodyInfoCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + BT_PROFILE("CMD_REQUEST_BODY_INFO"); + + const SdfRequestInfoArgs& sdfInfoArgs = clientCmd.m_sdfRequestInfoArgs; + //stream info into memory + int streamSizeInBytes = createBodyInfoStream(sdfInfoArgs.m_bodyUniqueId, bufferServerToClient, bufferSizeInBytes); + + serverStatusOut.m_type = CMD_BODY_INFO_COMPLETED; + serverStatusOut.m_dataStreamArguments.m_bodyUniqueId = sdfInfoArgs.m_bodyUniqueId; + serverStatusOut.m_dataStreamArguments.m_bodyName[0] = 0; + + InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(sdfInfoArgs.m_bodyUniqueId); + if (bodyHandle) + { + strcpy(serverStatusOut.m_dataStreamArguments.m_bodyName,bodyHandle->m_bodyName.c_str()); + } + + serverStatusOut.m_numDataStreamBytes = streamSizeInBytes; + + return hasStatus; + +} + +bool PhysicsServerCommandProcessor::processLoadSDFCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + BT_PROFILE("CMD_LOAD_SDF"); + + const SdfArgs& sdfArgs = clientCmd.m_sdfArguments; + if (m_data->m_verboseOutput) + { + b3Printf("Processed CMD_LOAD_SDF:%s", sdfArgs.m_sdfFileName); + } + bool useMultiBody=(clientCmd.m_updateFlags & URDF_ARGS_USE_MULTIBODY) ? (sdfArgs.m_useMultiBody!=0) : true; + + int flags = CUF_USE_SDF; //CUF_USE_URDF_INERTIA + btScalar globalScaling = 1.f; + if (clientCmd.m_updateFlags & URDF_ARGS_USE_GLOBAL_SCALING) + { + globalScaling = sdfArgs.m_globalScaling; + } + bool completedOk = loadSdf(sdfArgs.m_sdfFileName,bufferServerToClient, bufferSizeInBytes, useMultiBody, flags, globalScaling); + if (completedOk) + { + m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld); + + //serverStatusOut.m_type = CMD_SDF_LOADING_FAILED; + serverStatusOut.m_sdfLoadedArgs.m_numBodies = m_data->m_sdfRecentLoadedBodies.size(); + serverStatusOut.m_sdfLoadedArgs.m_numUserConstraints = 0; + int maxBodies = btMin(MAX_SDF_BODIES, serverStatusOut.m_sdfLoadedArgs.m_numBodies); + for (int i=0;im_sdfRecentLoadedBodies[i]; + } + + serverStatusOut.m_type = CMD_SDF_LOADING_COMPLETED; + } else + { + serverStatusOut.m_type = CMD_SDF_LOADING_FAILED; } return hasStatus; } -//static int skip=1; +bool PhysicsServerCommandProcessor::processCreateMultiBodyCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + + serverStatusOut.m_type = CMD_CREATE_MULTI_BODY_FAILED; + if (clientCmd.m_createMultiBodyArgs.m_baseLinkIndex>=0) + { + m_data->m_sdfRecentLoadedBodies.clear(); + + ProgrammaticUrdfInterface u2b(clientCmd.m_createMultiBodyArgs, m_data); + + bool useMultiBody = true; + if (clientCmd.m_updateFlags & MULT_BODY_USE_MAXIMAL_COORDINATES) + { + useMultiBody = false; + } + + int flags = 0; + bool ok = processImportedObjects("memory", bufferServerToClient, bufferSizeInBytes, useMultiBody, flags, u2b); + + if (ok) + { + int bodyUniqueId = -1; + + if (m_data->m_sdfRecentLoadedBodies.size()==1) + { + bodyUniqueId = m_data->m_sdfRecentLoadedBodies[0]; + } + m_data->m_sdfRecentLoadedBodies.clear(); + if (bodyUniqueId>=0) + { + m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld); + serverStatusOut.m_type = CMD_CREATE_MULTI_BODY_COMPLETED; + + int streamSizeInBytes = createBodyInfoStream(bodyUniqueId, bufferServerToClient, bufferSizeInBytes); + if (m_data->m_urdfLinkNameMapper.size()) + { + serverStatusOut.m_numDataStreamBytes = m_data->m_urdfLinkNameMapper.at(m_data->m_urdfLinkNameMapper.size()-1)->m_memSerializer->getCurrentBufferSize(); + } + serverStatusOut.m_dataStreamArguments.m_bodyUniqueId = bodyUniqueId; + InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); + strcpy(serverStatusOut.m_dataStreamArguments.m_bodyName, body->m_bodyName.c_str()); + } + } + + //ConvertURDF2Bullet(u2b,creation, rootTrans,m_data->m_dynamicsWorld,useMultiBody,u2b.getPathPrefix(),flags); + } + return hasStatus; +} + + +bool PhysicsServerCommandProcessor::processLoadURDFCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + serverStatusOut.m_type = CMD_URDF_LOADING_FAILED; + + BT_PROFILE("CMD_LOAD_URDF"); + const UrdfArgs& urdfArgs = clientCmd.m_urdfArguments; + if (m_data->m_verboseOutput) + { + b3Printf("Processed CMD_LOAD_URDF:%s", urdfArgs.m_urdfFileName); + } + btAssert((clientCmd.m_updateFlags&URDF_ARGS_FILE_NAME) !=0); + btAssert(urdfArgs.m_urdfFileName); + btVector3 initialPos(0,0,0); + btQuaternion initialOrn(0,0,0,1); + if (clientCmd.m_updateFlags & URDF_ARGS_INITIAL_POSITION) + { + initialPos[0] = urdfArgs.m_initialPosition[0]; + initialPos[1] = urdfArgs.m_initialPosition[1]; + initialPos[2] = urdfArgs.m_initialPosition[2]; + } + int urdfFlags = 0; + if (clientCmd.m_updateFlags & URDF_ARGS_HAS_CUSTOM_URDF_FLAGS) + { + urdfFlags = urdfArgs.m_urdfFlags; + } + if (clientCmd.m_updateFlags & URDF_ARGS_INITIAL_ORIENTATION) + { + initialOrn[0] = urdfArgs.m_initialOrientation[0]; + initialOrn[1] = urdfArgs.m_initialOrientation[1]; + initialOrn[2] = urdfArgs.m_initialOrientation[2]; + initialOrn[3] = urdfArgs.m_initialOrientation[3]; + } + bool useMultiBody=(clientCmd.m_updateFlags & URDF_ARGS_USE_MULTIBODY) ? (urdfArgs.m_useMultiBody!=0) : true; + bool useFixedBase = (clientCmd.m_updateFlags & URDF_ARGS_USE_FIXED_BASE) ? (urdfArgs.m_useFixedBase!=0): false; + int bodyUniqueId; + btScalar globalScaling = 1.f; + if (clientCmd.m_updateFlags & URDF_ARGS_USE_GLOBAL_SCALING) + { + globalScaling = urdfArgs.m_globalScaling; + } + //load the actual URDF and send a report: completed or failed + bool completedOk = loadUrdf(urdfArgs.m_urdfFileName, + initialPos,initialOrn, + useMultiBody, useFixedBase,&bodyUniqueId, bufferServerToClient, bufferSizeInBytes, urdfFlags, globalScaling); + + if (completedOk && bodyUniqueId>=0) + { + + + m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld); + + serverStatusOut.m_type = CMD_URDF_LOADING_COMPLETED; + + int streamSizeInBytes = createBodyInfoStream(bodyUniqueId, bufferServerToClient, bufferSizeInBytes); + + + if (m_data->m_urdfLinkNameMapper.size()) + { + serverStatusOut.m_numDataStreamBytes = m_data->m_urdfLinkNameMapper.at(m_data->m_urdfLinkNameMapper.size()-1)->m_memSerializer->getCurrentBufferSize(); + } + serverStatusOut.m_dataStreamArguments.m_bodyUniqueId = bodyUniqueId; + InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); + strcpy(serverStatusOut.m_dataStreamArguments.m_bodyName, body->m_bodyName.c_str()); + + } + + return hasStatus; +} + +bool PhysicsServerCommandProcessor::processLoadBunnyCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + serverStatusOut.m_type = CMD_UNKNOWN_COMMAND_FLUSHED; + bool hasStatus = true; +#ifdef USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD + double scale = 0.1; + double mass = 0.1; + double collisionMargin = 0.02; + if (clientCmd.m_updateFlags & LOAD_BUNNY_UPDATE_SCALE) + { + scale = clientCmd.m_loadBunnyArguments.m_scale; + } + if (clientCmd.m_updateFlags & LOAD_BUNNY_UPDATE_MASS) + { + mass = clientCmd.m_loadBunnyArguments.m_mass; + } + if (clientCmd.m_updateFlags & LOAD_BUNNY_UPDATE_COLLISION_MARGIN) + { + collisionMargin = clientCmd.m_loadBunnyArguments.m_collisionMargin; + } + m_data->m_softBodyWorldInfo.air_density = (btScalar)1.2; + m_data->m_softBodyWorldInfo.water_density = 0; + m_data->m_softBodyWorldInfo.water_offset = 0; + m_data->m_softBodyWorldInfo.water_normal = btVector3(0,0,0); + m_data->m_softBodyWorldInfo.m_gravity.setValue(0,0,-10); + m_data->m_softBodyWorldInfo.m_broadphase = m_data->m_broadphase; + m_data->m_softBodyWorldInfo.m_sparsesdf.Initialize(); + + btSoftBody* psb=btSoftBodyHelpers::CreateFromTriMesh(m_data->m_softBodyWorldInfo,gVerticesBunny, &gIndicesBunny[0][0], BUNNY_NUM_TRIANGLES); + + btSoftBody::Material* pm=psb->appendMaterial(); + pm->m_kLST = 1.0; + pm->m_flags -= btSoftBody::fMaterial::DebugDraw; + psb->generateBendingConstraints(2,pm); + psb->m_cfg.piterations = 50; + psb->m_cfg.kDF = 0.5; + psb->randomizeConstraints(); + psb->rotate(btQuaternion(0.70711,0,0,0.70711)); + psb->translate(btVector3(0,0,1.0)); + psb->scale(btVector3(scale,scale,scale)); + psb->setTotalMass(mass,true); + psb->getCollisionShape()->setMargin(collisionMargin); + + m_data->m_dynamicsWorld->addSoftBody(psb); + serverStatusOut.m_type = CMD_CLIENT_COMMAND_COMPLETED; +#endif + return hasStatus; +} + +bool PhysicsServerCommandProcessor::processCreateSensorCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + BT_PROFILE("CMD_CREATE_SENSOR"); + + if (m_data->m_verboseOutput) + { + b3Printf("Processed CMD_CREATE_SENSOR"); + } + int bodyUniqueId = clientCmd.m_createSensorArguments.m_bodyUniqueId; + InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); + if (body && body->m_multiBody) + { + btMultiBody* mb = body->m_multiBody; + btAssert(mb); + for (int i=0;igetLink(jointIndex).m_jointFeedback) + { + b3Warning("CMD_CREATE_SENSOR: sensor for joint [%d] already enabled", jointIndex); + } else + { + btMultiBodyJointFeedback* fb = new btMultiBodyJointFeedback(); + fb->m_reactionForces.setZero(); + mb->getLink(jointIndex).m_jointFeedback = fb; + m_data->m_multiBodyJointFeedbacks.push_back(fb); + }; + + } else + { + if (mb->getLink(jointIndex).m_jointFeedback) + { + m_data->m_multiBodyJointFeedbacks.remove(mb->getLink(jointIndex).m_jointFeedback); + delete mb->getLink(jointIndex).m_jointFeedback; + mb->getLink(jointIndex).m_jointFeedback=0; + } else + { + b3Warning("CMD_CREATE_SENSOR: cannot perform sensor removal request, no sensor on joint [%d]", jointIndex); + }; + + } + } + + } else + { + b3Warning("No btMultiBody in the world. btRigidBody/btTypedConstraint sensor not hooked up yet"); + } + +#if 0 + //todo(erwincoumans) here is some sample code to hook up a force/torque sensor for btTypedConstraint/btRigidBody + /* + for (int i=0;im_dynamicsWorld->getNumConstraints();i++) + { + btTypedConstraint* c = m_data->m_dynamicsWorld->getConstraint(i); + btJointFeedback* fb = new btJointFeedback(); + m_data->m_jointFeedbacks.push_back(fb); + c->setJointFeedback(fb); + + + } + */ +#endif + + serverStatusOut.m_type = CMD_CLIENT_COMMAND_COMPLETED; + return hasStatus; +} + +bool PhysicsServerCommandProcessor::processProfileTimingCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + + B3_PROFILE("custom");//clientCmd.m_profile.m_name); + { + B3_PROFILE("event");//clientCmd.m_profile.m_name); + char** eventNamePtr = m_data->m_profileEvents[clientCmd.m_profile.m_name]; + char* eventName = 0; + if (eventNamePtr) + { + B3_PROFILE("reuse"); + eventName = *eventNamePtr; + + } else + { + B3_PROFILE("alloc"); + int len = strlen(clientCmd.m_profile.m_name); + eventName = new char[len+1]; + strcpy(eventName,clientCmd.m_profile.m_name); + eventName[len] = 0; + m_data->m_profileEvents.insert(eventName,eventName); + + } + + + { + { + B3_PROFILE("with");//clientCmd.m_profile.m_name); + { + B3_PROFILE("some");//clientCmd.m_profile.m_name); + { + B3_PROFILE("deep");//clientCmd.m_profile.m_name); + { + B3_PROFILE("level");//clientCmd.m_profile.m_name); + { + B3_PROFILE(eventName); + b3Clock::usleep(clientCmd.m_profile.m_durationInMicroSeconds); + } + } + } + } + } + } + } + + serverStatusOut.m_type = CMD_CLIENT_COMMAND_COMPLETED; + hasStatus = true; + return hasStatus; +} + + +bool PhysicsServerCommandProcessor::processRequestCollisionInfoCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + SharedMemoryStatus& serverCmd = serverStatusOut; + serverStatusOut.m_type = CMD_REQUEST_COLLISION_INFO_FAILED; + hasStatus=true; + int bodyUniqueId = clientCmd.m_requestCollisionInfoArgs.m_bodyUniqueId; + InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); + + + if (body && body->m_multiBody) + { + btMultiBody* mb = body->m_multiBody; + serverStatusOut.m_type = CMD_REQUEST_COLLISION_INFO_COMPLETED; + serverCmd.m_sendCollisionInfoArgs.m_numLinks = body->m_multiBody->getNumLinks(); + serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[0] = 0; + serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[1] = 0; + serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[2] = 0; + + serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[0] = -1; + serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[1] = -1; + serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[2] = -1; + + if (body->m_multiBody->getBaseCollider()) + { + btTransform tr; + tr.setOrigin(mb->getBasePos()); + tr.setRotation(mb->getWorldToBaseRot().inverse()); + + btVector3 aabbMin,aabbMax; + body->m_multiBody->getBaseCollider()->getCollisionShape()->getAabb(tr,aabbMin,aabbMax); + serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[0] = aabbMin[0]; + serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[1] = aabbMin[1]; + serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[2] = aabbMin[2]; + + serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[0] = aabbMax[0]; + serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[1] = aabbMax[1]; + serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[2] = aabbMax[2]; + } + for (int l=0;lgetNumLinks();l++) + { + serverCmd.m_sendCollisionInfoArgs.m_linkWorldAABBsMin[3*l+0] = 0; + serverCmd.m_sendCollisionInfoArgs.m_linkWorldAABBsMin[3*l+1] = 0; + serverCmd.m_sendCollisionInfoArgs.m_linkWorldAABBsMin[3*l+2] = 0; + + serverCmd.m_sendCollisionInfoArgs.m_linkWorldAABBsMax[3*l+0] = -1; + serverCmd.m_sendCollisionInfoArgs.m_linkWorldAABBsMax[3*l+1] = -1; + serverCmd.m_sendCollisionInfoArgs.m_linkWorldAABBsMax[3*l+2] = -1; + + + + if (body->m_multiBody->getLink(l).m_collider) + { + btVector3 aabbMin,aabbMax; + body->m_multiBody->getLinkCollider(l)->getCollisionShape()->getAabb(mb->getLink(l).m_cachedWorldTransform,aabbMin,aabbMax); + + serverCmd.m_sendCollisionInfoArgs.m_linkWorldAABBsMin[3*l+0] = aabbMin[0]; + serverCmd.m_sendCollisionInfoArgs.m_linkWorldAABBsMin[3*l+1] = aabbMin[1]; + serverCmd.m_sendCollisionInfoArgs.m_linkWorldAABBsMin[3*l+2] = aabbMin[2]; + serverCmd.m_sendCollisionInfoArgs.m_linkWorldAABBsMax[3*l+0] = aabbMax[0]; + serverCmd.m_sendCollisionInfoArgs.m_linkWorldAABBsMax[3*l+1] = aabbMax[1]; + serverCmd.m_sendCollisionInfoArgs.m_linkWorldAABBsMax[3*l+2] = aabbMax[2]; + } + + } + } + else + { + if (body && body->m_rigidBody) + { + btRigidBody* rb = body->m_rigidBody; + SharedMemoryStatus& serverCmd = serverStatusOut; + serverStatusOut.m_type = CMD_REQUEST_COLLISION_INFO_COMPLETED; + serverCmd.m_sendCollisionInfoArgs.m_numLinks = 0; + serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[0] = 0; + serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[1] = 0; + serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[2] = 0; + + serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[0] = -1; + serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[1] = -1; + serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[2] = -1; + if (rb->getCollisionShape()) + { + btTransform tr = rb->getWorldTransform(); + + btVector3 aabbMin,aabbMax; + rb->getCollisionShape()->getAabb(tr,aabbMin,aabbMax); + serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[0] = aabbMin[0]; + serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[1] = aabbMin[1]; + serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[2] = aabbMin[2]; + + serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[0] = aabbMax[0]; + serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[1] = aabbMax[1]; + serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[2] = aabbMax[2]; + } + } + } + return hasStatus; +} + +bool PhysicsServerCommandProcessor::processForwardDynamicsCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + + BT_PROFILE("CMD_STEP_FORWARD_SIMULATION"); + + + if (m_data->m_verboseOutput) + { + b3Printf("Step simulation request"); + b3Printf("CMD_STEP_FORWARD_SIMULATION clientCmd = %d\n", clientCmd.m_sequenceNumber); + } + ///todo(erwincoumans) move this damping inside Bullet + for (int i=0;im_dynamicsWorld->getNumMultibodies();i++) + { + btMultiBody* mb = m_data->m_dynamicsWorld->getMultiBody(i); + for (int l=0;lgetNumLinks();l++) + { + for (int d=0;dgetLink(l).m_dofCount;d++) + { + double damping_coefficient = mb->getLink(l).m_jointDamping; + double damping = -damping_coefficient*mb->getJointVelMultiDof(l)[d]; + mb->addJointTorqueMultiDof(l, d, damping); + } + } + } + + btScalar deltaTimeScaled = m_data->m_physicsDeltaTime*simTimeScalingFactor; + + if (m_data->m_numSimulationSubSteps > 0) + { + m_data->m_dynamicsWorld->stepSimulation(deltaTimeScaled, m_data->m_numSimulationSubSteps, m_data->m_physicsDeltaTime / m_data->m_numSimulationSubSteps); + } + else + { + m_data->m_dynamicsWorld->stepSimulation(deltaTimeScaled, 0); + } + + SharedMemoryStatus& serverCmd =serverStatusOut; + serverCmd.m_type = CMD_STEP_FORWARD_SIMULATION_COMPLETED; + return hasStatus; + +} + +bool PhysicsServerCommandProcessor::processRequestInternalDataCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + + bool hasStatus = true; + BT_PROFILE("CMD_REQUEST_INTERNAL_DATA"); + + //todo: also check version etc? + + SharedMemoryStatus& serverCmd = serverStatusOut; + serverCmd.m_type = CMD_REQUEST_INTERNAL_DATA_FAILED; + + int sz = btDefaultSerializer::getMemoryDnaSizeInBytes(); + const char* memDna = btDefaultSerializer::getMemoryDna(); + if (sz < bufferSizeInBytes) + { + for (int i = 0; i < sz; i++) + { + bufferServerToClient[i] = memDna[i]; + } + serverCmd.m_type = CMD_REQUEST_INTERNAL_DATA_COMPLETED; + serverCmd.m_numDataStreamBytes = sz; + } + return hasStatus; +} + +bool PhysicsServerCommandProcessor::processChangeDynamicsInfoCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + BT_PROFILE("CMD_CHANGE_DYNAMICS_INFO"); + + int bodyUniqueId = clientCmd.m_changeDynamicsInfoArgs.m_bodyUniqueId; + int linkIndex = clientCmd.m_changeDynamicsInfoArgs.m_linkIndex; + double mass = clientCmd.m_changeDynamicsInfoArgs.m_mass; + double lateralFriction = clientCmd.m_changeDynamicsInfoArgs.m_lateralFriction; + double spinningFriction = clientCmd.m_changeDynamicsInfoArgs.m_spinningFriction; + double rollingFriction = clientCmd.m_changeDynamicsInfoArgs.m_rollingFriction; + double restitution = clientCmd.m_changeDynamicsInfoArgs.m_restitution; + btAssert(bodyUniqueId >= 0); + + InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); + + if (body && body->m_multiBody) + { + btMultiBody* mb = body->m_multiBody; + + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LINEAR_DAMPING) + { + mb->setLinearDamping(clientCmd.m_changeDynamicsInfoArgs.m_linearDamping); + } + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ANGULAR_DAMPING) + { + mb->setAngularDamping(clientCmd.m_changeDynamicsInfoArgs.m_angularDamping); + } + + if (linkIndex == -1) + { + if (mb->getBaseCollider()) + { + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_RESTITUTION) + { + mb->getBaseCollider()->setRestitution(restitution); + } + + + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_CONTACT_STIFFNESS_AND_DAMPING) + { + mb->getBaseCollider()->setContactStiffnessAndDamping(clientCmd.m_changeDynamicsInfoArgs.m_contactStiffness, clientCmd.m_changeDynamicsInfoArgs.m_contactDamping); + } + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LATERAL_FRICTION) + { + mb->getBaseCollider()->setFriction(lateralFriction); + } + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_SPINNING_FRICTION) + { + mb->getBaseCollider()->setSpinningFriction(spinningFriction); + } + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ROLLING_FRICTION) + { + mb->getBaseCollider()->setRollingFriction(rollingFriction); + } + + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_FRICTION_ANCHOR) + { + if (clientCmd.m_changeDynamicsInfoArgs.m_frictionAnchor) + { + mb->getBaseCollider()->setCollisionFlags(mb->getBaseCollider()->getCollisionFlags() | btCollisionObject::CF_HAS_FRICTION_ANCHOR); + } else + { + mb->getBaseCollider()->setCollisionFlags(mb->getBaseCollider()->getCollisionFlags() & ~btCollisionObject::CF_HAS_FRICTION_ANCHOR); + } + } + } + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_MASS) + { + mb->setBaseMass(mass); + if (mb->getBaseCollider() && mb->getBaseCollider()->getCollisionShape()) + { + btVector3 localInertia; + mb->getBaseCollider()->getCollisionShape()->calculateLocalInertia(mass,localInertia); + mb->setBaseInertia(localInertia); + } + } + } + else + { + if (linkIndex >= 0 && linkIndex < mb->getNumLinks()) + { + if (mb->getLinkCollider(linkIndex)) + { + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_RESTITUTION) + { + mb->getLinkCollider(linkIndex)->setRestitution(restitution); + } + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_SPINNING_FRICTION) + { + mb->getLinkCollider(linkIndex)->setSpinningFriction(spinningFriction); + } + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ROLLING_FRICTION) + { + mb->getLinkCollider(linkIndex)->setRollingFriction(rollingFriction); + } + + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_FRICTION_ANCHOR) + { + if (clientCmd.m_changeDynamicsInfoArgs.m_frictionAnchor) + { + mb->getLinkCollider(linkIndex)->setCollisionFlags(mb->getLinkCollider(linkIndex)->getCollisionFlags() | btCollisionObject::CF_HAS_FRICTION_ANCHOR); + } else + { + mb->getLinkCollider(linkIndex)->setCollisionFlags(mb->getLinkCollider(linkIndex)->getCollisionFlags() & ~btCollisionObject::CF_HAS_FRICTION_ANCHOR); + } + } + + + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LATERAL_FRICTION) + { + mb->getLinkCollider(linkIndex)->setFriction(lateralFriction); + } + + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_CONTACT_STIFFNESS_AND_DAMPING) + { + mb->getLinkCollider(linkIndex)->setContactStiffnessAndDamping(clientCmd.m_changeDynamicsInfoArgs.m_contactStiffness, clientCmd.m_changeDynamicsInfoArgs.m_contactDamping); + } + + + } + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_MASS) + { + mb->getLink(linkIndex).m_mass = mass; + if (mb->getLinkCollider(linkIndex) && mb->getLinkCollider(linkIndex)->getCollisionShape()) + { + btVector3 localInertia; + mb->getLinkCollider(linkIndex)->getCollisionShape()->calculateLocalInertia(mass,localInertia); + mb->getLink(linkIndex).m_inertiaLocal = localInertia; + } + } + } + } + } else + { + if (body && body->m_rigidBody) + { + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LINEAR_DAMPING) + { + btScalar angDamping = body->m_rigidBody->getAngularDamping(); + body->m_rigidBody->setDamping(clientCmd.m_changeDynamicsInfoArgs.m_linearDamping,angDamping); + } + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ANGULAR_DAMPING) + { + btScalar linDamping = body->m_rigidBody->getLinearDamping(); + body->m_rigidBody->setDamping(linDamping, clientCmd.m_changeDynamicsInfoArgs.m_angularDamping); + } + + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_CONTACT_STIFFNESS_AND_DAMPING) + { + body->m_rigidBody->setContactStiffnessAndDamping(clientCmd.m_changeDynamicsInfoArgs.m_contactStiffness, clientCmd.m_changeDynamicsInfoArgs.m_contactDamping); + } + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_RESTITUTION) + { + body->m_rigidBody->setRestitution(restitution); + } + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LATERAL_FRICTION) + { + body->m_rigidBody->setFriction(lateralFriction); + } + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_SPINNING_FRICTION) + { + body->m_rigidBody->setSpinningFriction(spinningFriction); + } + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ROLLING_FRICTION) + { + body->m_rigidBody->setRollingFriction(rollingFriction); + } + + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_FRICTION_ANCHOR) + { + if (clientCmd.m_changeDynamicsInfoArgs.m_frictionAnchor) + { + body->m_rigidBody->setCollisionFlags(body->m_rigidBody->getCollisionFlags() | btCollisionObject::CF_HAS_FRICTION_ANCHOR); + } else + { + body->m_rigidBody->setCollisionFlags(body->m_rigidBody->getCollisionFlags() & ~btCollisionObject::CF_HAS_FRICTION_ANCHOR); + } + } + + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_MASS) + { + btVector3 localInertia; + if (body->m_rigidBody->getCollisionShape()) + { + body->m_rigidBody->getCollisionShape()->calculateLocalInertia(mass,localInertia); + } + body->m_rigidBody->setMassProps(mass,localInertia); + } + } + } + + + SharedMemoryStatus& serverCmd =serverStatusOut; + serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED; + return hasStatus; +} + +bool PhysicsServerCommandProcessor::processSetAdditionalSearchPathCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + + BT_PROFILE("CMD_SET_ADDITIONAL_SEARCH_PATH"); + b3ResourcePath::setAdditionalSearchPath(clientCmd.m_searchPathArgs.m_path); + serverStatusOut.m_type = CMD_CLIENT_COMMAND_COMPLETED; + return hasStatus; +} + + +bool PhysicsServerCommandProcessor::processGetDynamicsInfoCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + SharedMemoryStatus& serverCmd = serverStatusOut; + serverCmd.m_type = CMD_GET_DYNAMICS_INFO_FAILED; + + int bodyUniqueId = clientCmd.m_getDynamicsInfoArgs.m_bodyUniqueId; + int linkIndex = clientCmd.m_getDynamicsInfoArgs.m_linkIndex; + InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); + if (body && body->m_multiBody) + { + SharedMemoryStatus& serverCmd = serverStatusOut; + serverCmd.m_type = CMD_GET_DYNAMICS_INFO_COMPLETED; + + btMultiBody* mb = body->m_multiBody; + if (linkIndex == -1) + { + serverCmd.m_dynamicsInfo.m_mass = mb->getBaseMass(); + serverCmd.m_dynamicsInfo.m_lateralFrictionCoeff = mb->getBaseCollider()->getFriction(); + } + else + { + serverCmd.m_dynamicsInfo.m_mass = mb->getLinkMass(linkIndex); + if (mb->getLinkCollider(linkIndex)) + { + serverCmd.m_dynamicsInfo.m_lateralFrictionCoeff = mb->getLinkCollider(linkIndex)->getFriction(); + } + else + { + b3Warning("The dynamic info requested is not available"); + serverCmd.m_type = CMD_GET_DYNAMICS_INFO_FAILED; + } + } + hasStatus = true; + } + return hasStatus; +} + + +bool PhysicsServerCommandProcessor::processRequestPhysicsSimulationParametersCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + SharedMemoryStatus& serverCmd =serverStatusOut; + serverCmd.m_type = CMD_REQUEST_PHYSICS_SIMULATION_PARAMETERS_COMPLETED; + serverCmd.m_simulationParameterResultArgs.m_collisionFilterMode = m_data->m_broadphaseCollisionFilterCallback->m_filterMode; + serverCmd.m_simulationParameterResultArgs.m_contactBreakingThreshold = gContactBreakingThreshold; + serverCmd.m_simulationParameterResultArgs.m_defaultContactERP = m_data->m_dynamicsWorld->getSolverInfo().m_erp2; + serverCmd.m_simulationParameterResultArgs.m_defaultNonContactERP = m_data->m_dynamicsWorld->getSolverInfo().m_erp; + serverCmd.m_simulationParameterResultArgs.m_deltaTime = m_data->m_physicsDeltaTime; + serverCmd.m_simulationParameterResultArgs.m_enableFileCaching = b3IsFileCachingEnabled(); + serverCmd.m_simulationParameterResultArgs.m_frictionERP = m_data->m_dynamicsWorld->getSolverInfo().m_frictionERP; + btVector3 grav = m_data->m_dynamicsWorld->getGravity(); + serverCmd.m_simulationParameterResultArgs.m_gravityAcceleration[0] = grav[0]; + serverCmd.m_simulationParameterResultArgs.m_gravityAcceleration[1] = grav[1]; + serverCmd.m_simulationParameterResultArgs.m_gravityAcceleration[2] = grav[2]; + serverCmd.m_simulationParameterResultArgs.m_internalSimFlags = gInternalSimFlags; + serverCmd.m_simulationParameterResultArgs.m_numSimulationSubSteps = m_data->m_numSimulationSubSteps; + serverCmd.m_simulationParameterResultArgs.m_numSolverIterations = m_data->m_dynamicsWorld->getSolverInfo().m_numIterations; + serverCmd.m_simulationParameterResultArgs.m_restitutionVelocityThreshold = m_data->m_dynamicsWorld->getSolverInfo().m_restitutionVelocityThreshold; + serverCmd.m_simulationParameterResultArgs.m_splitImpulsePenetrationThreshold = m_data->m_dynamicsWorld->getSolverInfo().m_splitImpulsePenetrationThreshold; + serverCmd.m_simulationParameterResultArgs.m_useRealTimeSimulation = m_data->m_useRealTimeSimulation; + serverCmd.m_simulationParameterResultArgs.m_useSplitImpulse = m_data->m_dynamicsWorld->getSolverInfo().m_splitImpulse; + return hasStatus; +} + +bool PhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + + BT_PROFILE("CMD_SEND_PHYSICS_SIMULATION_PARAMETERS"); + + if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_DELTA_TIME) + { + m_data->m_physicsDeltaTime = clientCmd.m_physSimParamArgs.m_deltaTime; + } + if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_REAL_TIME_SIMULATION) + { + m_data->m_useRealTimeSimulation = (clientCmd.m_physSimParamArgs.m_useRealTimeSimulation!=0); + } + + //see + if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_INTERNAL_SIMULATION_FLAGS) + { + //these flags are for internal/temporary/easter-egg/experimental demo purposes, use at own risk + gInternalSimFlags = clientCmd.m_physSimParamArgs.m_internalSimFlags; + } + + if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_GRAVITY) + { + btVector3 grav(clientCmd.m_physSimParamArgs.m_gravityAcceleration[0], + clientCmd.m_physSimParamArgs.m_gravityAcceleration[1], + clientCmd.m_physSimParamArgs.m_gravityAcceleration[2]); + this->m_data->m_dynamicsWorld->setGravity(grav); + if (m_data->m_verboseOutput) + { + b3Printf("Updated Gravity: %f,%f,%f",grav[0],grav[1],grav[2]); + } + + } + if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_NUM_SOLVER_ITERATIONS) + { + m_data->m_dynamicsWorld->getSolverInfo().m_numIterations = clientCmd.m_physSimParamArgs.m_numSolverIterations; + } + if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_CONTACT_BREAKING_THRESHOLD) + { + gContactBreakingThreshold = clientCmd.m_physSimParamArgs.m_contactBreakingThreshold; + } + + if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_COLLISION_FILTER_MODE) + { + m_data->m_broadphaseCollisionFilterCallback->m_filterMode = clientCmd.m_physSimParamArgs.m_collisionFilterMode; + } + + if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_USE_SPLIT_IMPULSE) + { + m_data->m_dynamicsWorld->getSolverInfo().m_splitImpulse = clientCmd.m_physSimParamArgs.m_useSplitImpulse; + } + if (clientCmd.m_updateFlags &SIM_PARAM_UPDATE_SPLIT_IMPULSE_PENETRATION_THRESHOLD) + { + m_data->m_dynamicsWorld->getSolverInfo().m_splitImpulsePenetrationThreshold = clientCmd.m_physSimParamArgs.m_splitImpulsePenetrationThreshold; + } + + + if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_NUM_SIMULATION_SUB_STEPS) + { + m_data->m_numSimulationSubSteps = clientCmd.m_physSimParamArgs.m_numSimulationSubSteps; + } + + if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_DEFAULT_CONTACT_ERP) + { + m_data->m_dynamicsWorld->getSolverInfo().m_erp2 = clientCmd.m_physSimParamArgs.m_defaultContactERP; + } + + if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_DEFAULT_NON_CONTACT_ERP) + { + m_data->m_dynamicsWorld->getSolverInfo().m_erp = clientCmd.m_physSimParamArgs.m_defaultNonContactERP; + } + + if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_DEFAULT_FRICTION_ERP) + { + m_data->m_dynamicsWorld->getSolverInfo().m_frictionERP = clientCmd.m_physSimParamArgs.m_frictionERP; + } + + + if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_RESTITUTION_VELOCITY_THRESHOLD) + { + m_data->m_dynamicsWorld->getSolverInfo().m_restitutionVelocityThreshold = clientCmd.m_physSimParamArgs.m_restitutionVelocityThreshold; + } + + + + if (clientCmd.m_updateFlags&SIM_PARAM_ENABLE_FILE_CACHING) + { + b3EnableFileCaching(clientCmd.m_physSimParamArgs.m_enableFileCaching); + } + + + SharedMemoryStatus& serverCmd =serverStatusOut; + serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED; + return hasStatus; +} + +bool PhysicsServerCommandProcessor::processInitPoseCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + + BT_PROFILE("CMD_INIT_POSE"); + + if (m_data->m_verboseOutput) + { + b3Printf("Server Init Pose not implemented yet"); + } + int bodyUniqueId = clientCmd.m_initPoseArgs.m_bodyUniqueId; + InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); + + btVector3 baseLinVel(0, 0, 0); + btVector3 baseAngVel(0, 0, 0); + + if (clientCmd.m_updateFlags & INIT_POSE_HAS_BASE_LINEAR_VELOCITY) + { + baseLinVel.setValue(clientCmd.m_initPoseArgs.m_initialStateQdot[0], + clientCmd.m_initPoseArgs.m_initialStateQdot[1], + clientCmd.m_initPoseArgs.m_initialStateQdot[2]); + } + if (clientCmd.m_updateFlags & INIT_POSE_HAS_BASE_ANGULAR_VELOCITY) + { + baseAngVel.setValue(clientCmd.m_initPoseArgs.m_initialStateQdot[3], + clientCmd.m_initPoseArgs.m_initialStateQdot[4], + clientCmd.m_initPoseArgs.m_initialStateQdot[5]); + } + btVector3 basePos(0, 0, 0); + if (clientCmd.m_updateFlags & INIT_POSE_HAS_INITIAL_POSITION) + { + basePos = btVector3( + clientCmd.m_initPoseArgs.m_initialStateQ[0], + clientCmd.m_initPoseArgs.m_initialStateQ[1], + clientCmd.m_initPoseArgs.m_initialStateQ[2]); + } + btQuaternion baseOrn(0, 0, 0, 1); + if (clientCmd.m_updateFlags & INIT_POSE_HAS_INITIAL_ORIENTATION) + { + baseOrn.setValue(clientCmd.m_initPoseArgs.m_initialStateQ[3], + clientCmd.m_initPoseArgs.m_initialStateQ[4], + clientCmd.m_initPoseArgs.m_initialStateQ[5], + clientCmd.m_initPoseArgs.m_initialStateQ[6]); + } + if (body && body->m_multiBody) + { + btMultiBody* mb = body->m_multiBody; + + + + if (clientCmd.m_updateFlags & INIT_POSE_HAS_BASE_LINEAR_VELOCITY) + { + mb->setBaseVel(baseLinVel); + } + + if (clientCmd.m_updateFlags & INIT_POSE_HAS_BASE_ANGULAR_VELOCITY) + { + mb->setBaseOmega(baseAngVel); + } + + + if (clientCmd.m_updateFlags & INIT_POSE_HAS_INITIAL_POSITION) + { + btVector3 zero(0,0,0); + btAssert(clientCmd.m_initPoseArgs.m_hasInitialStateQ[0] && + clientCmd.m_initPoseArgs.m_hasInitialStateQ[1] && + clientCmd.m_initPoseArgs.m_hasInitialStateQ[2]); + + mb->setBaseVel(baseLinVel); + mb->setBasePos(basePos); + } + if (clientCmd.m_updateFlags & INIT_POSE_HAS_INITIAL_ORIENTATION) + { + btAssert(clientCmd.m_initPoseArgs.m_hasInitialStateQ[3] && + clientCmd.m_initPoseArgs.m_hasInitialStateQ[4] && + clientCmd.m_initPoseArgs.m_hasInitialStateQ[5] && + clientCmd.m_initPoseArgs.m_hasInitialStateQ[6]); + + mb->setBaseOmega(baseAngVel); + btQuaternion invOrn(baseOrn); + + mb->setWorldToBaseRot(invOrn.inverse()); + } + if (clientCmd.m_updateFlags & INIT_POSE_HAS_JOINT_STATE) + { + int uDofIndex = 6; + int posVarCountIndex = 7; + for (int i=0;igetNumLinks();i++) + { + if ( (clientCmd.m_initPoseArgs.m_hasInitialStateQ[posVarCountIndex]) && (mb->getLink(i).m_dofCount==1)) + { + mb->setJointPos(i,clientCmd.m_initPoseArgs.m_initialStateQ[posVarCountIndex]); + mb->setJointVel(i,0);//backwards compatibility + } + if ((clientCmd.m_initPoseArgs.m_hasInitialStateQdot[uDofIndex]) && (mb->getLink(i).m_dofCount==1)) + { + btScalar vel = clientCmd.m_initPoseArgs.m_initialStateQdot[uDofIndex]; + mb->setJointVel(i,vel); + } + + posVarCountIndex += mb->getLink(i).m_posVarCount; + uDofIndex += mb->getLink(i).m_dofCount; + + } + } + + btAlignedObjectArray scratch_q; + btAlignedObjectArray scratch_m; + + mb->forwardKinematics(scratch_q,scratch_m); + int nLinks = mb->getNumLinks(); + scratch_q.resize(nLinks+1); + scratch_m.resize(nLinks+1); + + mb->updateCollisionObjectWorldTransforms(scratch_q,scratch_m); + + + } + + if (body && body->m_rigidBody) + { + if (clientCmd.m_updateFlags & INIT_POSE_HAS_BASE_LINEAR_VELOCITY) + { + body->m_rigidBody->setLinearVelocity(baseLinVel); + } + if (clientCmd.m_updateFlags & INIT_POSE_HAS_BASE_ANGULAR_VELOCITY) + { + body->m_rigidBody->setAngularVelocity(baseAngVel); + } + + if (clientCmd.m_updateFlags & INIT_POSE_HAS_INITIAL_POSITION) + { + body->m_rigidBody->getWorldTransform().setOrigin(basePos); + body->m_rigidBody->setLinearVelocity(baseLinVel); + } + + if (clientCmd.m_updateFlags & INIT_POSE_HAS_INITIAL_ORIENTATION) + { + body->m_rigidBody->getWorldTransform().setRotation(baseOrn); + body->m_rigidBody->setAngularVelocity(baseAngVel); + } + + } + + SharedMemoryStatus& serverCmd =serverStatusOut; + serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED; + + return hasStatus; +} + + +bool PhysicsServerCommandProcessor::processResetSimulationCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + BT_PROFILE("CMD_RESET_SIMULATION"); + m_data->m_guiHelper->setVisualizerFlag(COV_ENABLE_SYNC_RENDERING_INTERNAL,0); + resetSimulation(); + m_data->m_guiHelper->setVisualizerFlag(COV_ENABLE_SYNC_RENDERING_INTERNAL,1); + + SharedMemoryStatus& serverCmd =serverStatusOut; + serverCmd.m_type = CMD_RESET_SIMULATION_COMPLETED; + return hasStatus; +} + +bool PhysicsServerCommandProcessor::processCreateRigidBodyCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + SharedMemoryStatus& serverCmd =serverStatusOut; + serverCmd.m_type = CMD_RIGID_BODY_CREATION_COMPLETED; + + BT_PROFILE("CMD_CREATE_RIGID_BODY"); + + btVector3 halfExtents(1,1,1); + if (clientCmd.m_updateFlags & BOX_SHAPE_HAS_HALF_EXTENTS) + { + halfExtents = btVector3( + clientCmd.m_createBoxShapeArguments.m_halfExtentsX, + clientCmd.m_createBoxShapeArguments.m_halfExtentsY, + clientCmd.m_createBoxShapeArguments.m_halfExtentsZ); + } + btTransform startTrans; + startTrans.setIdentity(); + if (clientCmd.m_updateFlags & BOX_SHAPE_HAS_INITIAL_POSITION) + { + startTrans.setOrigin(btVector3( + clientCmd.m_createBoxShapeArguments.m_initialPosition[0], + clientCmd.m_createBoxShapeArguments.m_initialPosition[1], + clientCmd.m_createBoxShapeArguments.m_initialPosition[2])); + } + + if (clientCmd.m_updateFlags & BOX_SHAPE_HAS_INITIAL_ORIENTATION) + { + + startTrans.setRotation(btQuaternion( + clientCmd.m_createBoxShapeArguments.m_initialOrientation[0], + clientCmd.m_createBoxShapeArguments.m_initialOrientation[1], + clientCmd.m_createBoxShapeArguments.m_initialOrientation[2], + clientCmd.m_createBoxShapeArguments.m_initialOrientation[3])); + } + + btScalar mass = 0.f; + if (clientCmd.m_updateFlags & BOX_SHAPE_HAS_MASS) + { + mass = clientCmd.m_createBoxShapeArguments.m_mass; + } + + int shapeType = COLLISION_SHAPE_TYPE_BOX; + + if (clientCmd.m_updateFlags & BOX_SHAPE_HAS_COLLISION_SHAPE_TYPE) + { + shapeType = clientCmd.m_createBoxShapeArguments.m_collisionShapeType; + } + + btBulletWorldImporter* worldImporter = new btBulletWorldImporter(m_data->m_dynamicsWorld); + m_data->m_worldImporters.push_back(worldImporter); + + btCollisionShape* shape = 0; + + switch (shapeType) + { + case COLLISION_SHAPE_TYPE_CYLINDER_X: + { + btScalar radius = halfExtents[1]; + btScalar height = halfExtents[0]; + shape = worldImporter->createCylinderShapeX(radius,height); + break; + } + case COLLISION_SHAPE_TYPE_CYLINDER_Y: + { + btScalar radius = halfExtents[0]; + btScalar height = halfExtents[1]; + shape = worldImporter->createCylinderShapeY(radius,height); + break; + } + case COLLISION_SHAPE_TYPE_CYLINDER_Z: + { + btScalar radius = halfExtents[1]; + btScalar height = halfExtents[2]; + shape = worldImporter->createCylinderShapeZ(radius,height); + break; + } + case COLLISION_SHAPE_TYPE_CAPSULE_X: + { + btScalar radius = halfExtents[1]; + btScalar height = halfExtents[0]; + shape = worldImporter->createCapsuleShapeX(radius,height); + break; + } + case COLLISION_SHAPE_TYPE_CAPSULE_Y: + { + btScalar radius = halfExtents[0]; + btScalar height = halfExtents[1]; + shape = worldImporter->createCapsuleShapeY(radius,height); + break; + } + case COLLISION_SHAPE_TYPE_CAPSULE_Z: + { + btScalar radius = halfExtents[1]; + btScalar height = halfExtents[2]; + shape = worldImporter->createCapsuleShapeZ(radius,height); + break; + } + case COLLISION_SHAPE_TYPE_SPHERE: + { + btScalar radius = halfExtents[0]; + shape = worldImporter->createSphereShape(radius); + break; + } + case COLLISION_SHAPE_TYPE_BOX: + default: + { + shape = worldImporter->createBoxShape(halfExtents); + } + } + + + bool isDynamic = (mass>0); + btRigidBody* rb = worldImporter->createRigidBody(isDynamic,mass,startTrans,shape,0); + //m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld); + btVector4 colorRGBA(1,0,0,1); + if (clientCmd.m_updateFlags & BOX_SHAPE_HAS_COLOR) + { + colorRGBA[0] = clientCmd.m_createBoxShapeArguments.m_colorRGBA[0]; + colorRGBA[1] = clientCmd.m_createBoxShapeArguments.m_colorRGBA[1]; + colorRGBA[2] = clientCmd.m_createBoxShapeArguments.m_colorRGBA[2]; + colorRGBA[3] = clientCmd.m_createBoxShapeArguments.m_colorRGBA[3]; + } + m_data->m_guiHelper->createCollisionShapeGraphicsObject(rb->getCollisionShape()); + m_data->m_guiHelper->createCollisionObjectGraphicsObject(rb,colorRGBA); + + + int bodyUniqueId = m_data->m_bodyHandles.allocHandle(); + InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(bodyUniqueId); + serverCmd.m_rigidBodyCreateArgs.m_bodyUniqueId = bodyUniqueId; + rb->setUserIndex2(bodyUniqueId); + bodyHandle->m_rootLocalInertialFrame.setIdentity(); + bodyHandle->m_rigidBody = rb; + + return hasStatus; +} + + +bool PhysicsServerCommandProcessor::processPickBodyCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + + BT_PROFILE("CMD_PICK_BODY"); + + pickBody(btVector3(clientCmd.m_pickBodyArguments.m_rayFromWorld[0], + clientCmd.m_pickBodyArguments.m_rayFromWorld[1], + clientCmd.m_pickBodyArguments.m_rayFromWorld[2]), + btVector3(clientCmd.m_pickBodyArguments.m_rayToWorld[0], + clientCmd.m_pickBodyArguments.m_rayToWorld[1], + clientCmd.m_pickBodyArguments.m_rayToWorld[2])); + + + SharedMemoryStatus& serverCmd =serverStatusOut; + serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED; + return hasStatus; +} + +bool PhysicsServerCommandProcessor::processMovePickedBodyCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + + BT_PROFILE("CMD_MOVE_PICKED_BODY"); + + movePickedBody(btVector3(clientCmd.m_pickBodyArguments.m_rayFromWorld[0], + clientCmd.m_pickBodyArguments.m_rayFromWorld[1], + clientCmd.m_pickBodyArguments.m_rayFromWorld[2]), + btVector3(clientCmd.m_pickBodyArguments.m_rayToWorld[0], + clientCmd.m_pickBodyArguments.m_rayToWorld[1], + clientCmd.m_pickBodyArguments.m_rayToWorld[2])); + + SharedMemoryStatus& serverCmd =serverStatusOut; + serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED; + return hasStatus; +} + +bool PhysicsServerCommandProcessor::processRemovePickingConstraintCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + + BT_PROFILE("CMD_REMOVE_PICKING_CONSTRAINT_BODY"); + removePickingConstraint(); + + SharedMemoryStatus& serverCmd =serverStatusOut; + serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED; + return hasStatus; +} + + +bool PhysicsServerCommandProcessor::processRequestAabbOverlapCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + + BT_PROFILE("CMD_REQUEST_AABB_OVERLAP"); + SharedMemoryStatus& serverCmd = serverStatusOut; + int curObjectIndex = clientCmd.m_requestOverlappingObjectsArgs.m_startingOverlappingObjectIndex; + + if (0== curObjectIndex) + { + //clientCmd.m_requestContactPointArguments.m_aabbQueryMin + btVector3 aabbMin, aabbMax; + aabbMin.setValue(clientCmd.m_requestOverlappingObjectsArgs.m_aabbQueryMin[0], + clientCmd.m_requestOverlappingObjectsArgs.m_aabbQueryMin[1], + clientCmd.m_requestOverlappingObjectsArgs.m_aabbQueryMin[2]); + aabbMax.setValue(clientCmd.m_requestOverlappingObjectsArgs.m_aabbQueryMax[0], + clientCmd.m_requestOverlappingObjectsArgs.m_aabbQueryMax[1], + clientCmd.m_requestOverlappingObjectsArgs.m_aabbQueryMax[2]); + + m_data->m_cachedOverlappingObjects.clear(); + + m_data->m_dynamicsWorld->getBroadphase()->aabbTest(aabbMin, aabbMax, m_data->m_cachedOverlappingObjects); + } + + + int totalBytesPerObject = sizeof(b3OverlappingObject); + int overlapCapacity = bufferSizeInBytes / totalBytesPerObject - 1; + int numOverlap = m_data->m_cachedOverlappingObjects.m_bodyUniqueIds.size(); + int remainingObjects = numOverlap - curObjectIndex; + + int curNumObjects = btMin(overlapCapacity, remainingObjects); + + if (numOverlap < overlapCapacity) + { + + b3OverlappingObject* overlapStorage = (b3OverlappingObject*)bufferServerToClient; + for (int i = 0; i < m_data->m_cachedOverlappingObjects.m_bodyUniqueIds.size(); i++) + { + overlapStorage[i].m_objectUniqueId = m_data->m_cachedOverlappingObjects.m_bodyUniqueIds[i]; + overlapStorage[i].m_linkIndex = m_data->m_cachedOverlappingObjects.m_links[i]; + } + + serverCmd.m_type = CMD_REQUEST_AABB_OVERLAP_COMPLETED; + + //int m_startingOverlappingObjectIndex; + //int m_numOverlappingObjectsCopied; + //int m_numRemainingOverlappingObjects; + serverCmd.m_sendOverlappingObjectsArgs.m_startingOverlappingObjectIndex = clientCmd.m_requestOverlappingObjectsArgs.m_startingOverlappingObjectIndex; + serverCmd.m_sendOverlappingObjectsArgs.m_numOverlappingObjectsCopied = m_data->m_cachedOverlappingObjects.m_bodyUniqueIds.size(); + serverCmd.m_sendOverlappingObjectsArgs.m_numRemainingOverlappingObjects = remainingObjects - curNumObjects; + } + else + { + serverCmd.m_type = CMD_REQUEST_AABB_OVERLAP_FAILED; + } + return hasStatus; +} + +bool PhysicsServerCommandProcessor::processRequestOpenGLVisualizeCameraCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + + BT_PROFILE("CMD_REQUEST_OPENGL_VISUALIZER_CAMERA"); + SharedMemoryStatus& serverCmd = serverStatusOut; + bool result = this->m_data->m_guiHelper->getCameraInfo( + &serverCmd.m_visualizerCameraResultArgs.m_width, + &serverCmd.m_visualizerCameraResultArgs.m_height, + serverCmd.m_visualizerCameraResultArgs.m_viewMatrix, + serverCmd.m_visualizerCameraResultArgs.m_projectionMatrix, + serverCmd.m_visualizerCameraResultArgs.m_camUp, + serverCmd.m_visualizerCameraResultArgs.m_camForward, + serverCmd.m_visualizerCameraResultArgs.m_horizontal, + serverCmd.m_visualizerCameraResultArgs.m_vertical, + &serverCmd.m_visualizerCameraResultArgs.m_yaw, + &serverCmd.m_visualizerCameraResultArgs.m_pitch, + &serverCmd.m_visualizerCameraResultArgs.m_dist, + serverCmd.m_visualizerCameraResultArgs.m_target); + serverCmd.m_type = result ? CMD_REQUEST_OPENGL_VISUALIZER_CAMERA_COMPLETED: CMD_REQUEST_OPENGL_VISUALIZER_CAMERA_FAILED; + return hasStatus; +} + + +bool PhysicsServerCommandProcessor::processConfigureOpenGLVisualizerCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + + BT_PROFILE("CMD_CONFIGURE_OPENGL_VISUALIZER"); + SharedMemoryStatus& serverCmd = serverStatusOut; + serverCmd.m_type =CMD_CLIENT_COMMAND_COMPLETED; + + hasStatus = true; + + if (clientCmd.m_updateFlags&COV_SET_FLAGS) + { + if (clientCmd.m_configureOpenGLVisualizerArguments.m_setFlag == COV_ENABLE_TINY_RENDERER) + { + m_data->m_enableTinyRenderer = clientCmd.m_configureOpenGLVisualizerArguments.m_setEnabled!=0; + } + m_data->m_guiHelper->setVisualizerFlag(clientCmd.m_configureOpenGLVisualizerArguments.m_setFlag, + clientCmd.m_configureOpenGLVisualizerArguments.m_setEnabled); + } + if (clientCmd.m_updateFlags&COV_SET_CAMERA_VIEW_MATRIX) + { + m_data->m_guiHelper->resetCamera( clientCmd.m_configureOpenGLVisualizerArguments.m_cameraDistance, + clientCmd.m_configureOpenGLVisualizerArguments.m_cameraYaw, + clientCmd.m_configureOpenGLVisualizerArguments.m_cameraPitch, + clientCmd.m_configureOpenGLVisualizerArguments.m_cameraTargetPosition[0], + clientCmd.m_configureOpenGLVisualizerArguments.m_cameraTargetPosition[1], + clientCmd.m_configureOpenGLVisualizerArguments.m_cameraTargetPosition[2]); + } + return hasStatus; +} + +bool PhysicsServerCommandProcessor::processInverseDynamicsCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + + BT_PROFILE("CMD_CALCULATE_INVERSE_DYNAMICS"); + SharedMemoryStatus& serverCmd = serverStatusOut; + InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId); + if (bodyHandle && bodyHandle->m_multiBody) + { + serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_FAILED; + + btInverseDynamics::MultiBodyTree* tree = m_data->findOrCreateTree(bodyHandle->m_multiBody); + + if (tree) + { + int baseDofs = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 6; + const int num_dofs = bodyHandle->m_multiBody->getNumDofs(); + btInverseDynamics::vecx nu(num_dofs+baseDofs), qdot(num_dofs + baseDofs), q(num_dofs + baseDofs), joint_force(num_dofs + baseDofs); + for (int i = 0; i < num_dofs; i++) + { + q[i + baseDofs] = clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[i]; + qdot[i + baseDofs] = clientCmd.m_calculateInverseDynamicsArguments.m_jointVelocitiesQdot[i]; + nu[i+baseDofs] = clientCmd.m_calculateInverseDynamicsArguments.m_jointAccelerations[i]; + } + // Set the gravity to correspond to the world gravity + btInverseDynamics::vec3 id_grav(m_data->m_dynamicsWorld->getGravity()); + + if (-1 != tree->setGravityInWorldFrame(id_grav) && + -1 != tree->calculateInverseDynamics(q, qdot, nu, &joint_force)) + { + serverCmd.m_inverseDynamicsResultArgs.m_bodyUniqueId = clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId; + serverCmd.m_inverseDynamicsResultArgs.m_dofCount = num_dofs; + for (int i = 0; i < num_dofs; i++) + { + serverCmd.m_inverseDynamicsResultArgs.m_jointForces[i] = joint_force[i+baseDofs]; + } + serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED; + } + else + { + serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_FAILED; + } + } + + } + else + { + serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_FAILED; + } + + return hasStatus; +} + +bool PhysicsServerCommandProcessor::processCalculateJacobianCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + BT_PROFILE("CMD_CALCULATE_JACOBIAN"); + + SharedMemoryStatus& serverCmd = serverStatusOut; + InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(clientCmd.m_calculateJacobianArguments.m_bodyUniqueId); + if (bodyHandle && bodyHandle->m_multiBody) + { + serverCmd.m_type = CMD_CALCULATED_JACOBIAN_FAILED; + + btInverseDynamics::MultiBodyTree* tree = m_data->findOrCreateTree(bodyHandle->m_multiBody); + + if (tree) + { + int baseDofs = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 6; + const int numDofs = bodyHandle->m_multiBody->getNumDofs(); + btInverseDynamics::vecx q(numDofs + baseDofs); + btInverseDynamics::vecx qdot(numDofs + baseDofs); + btInverseDynamics::vecx nu(numDofs + baseDofs); + btInverseDynamics::vecx joint_force(numDofs + baseDofs); + for (int i = 0; i < numDofs; i++) + { + q[i + baseDofs] = clientCmd.m_calculateJacobianArguments.m_jointPositionsQ[i]; + qdot[i + baseDofs] = clientCmd.m_calculateJacobianArguments.m_jointVelocitiesQdot[i]; + nu[i + baseDofs] = clientCmd.m_calculateJacobianArguments.m_jointAccelerations[i]; + } + // Set the gravity to correspond to the world gravity + btInverseDynamics::vec3 id_grav(m_data->m_dynamicsWorld->getGravity()); + if (-1 != tree->setGravityInWorldFrame(id_grav) && + -1 != tree->calculateInverseDynamics(q, qdot, nu, &joint_force)) + { + serverCmd.m_jacobianResultArgs.m_dofCount = numDofs + baseDofs; + // Set jacobian value + tree->calculateJacobians(q); + btInverseDynamics::mat3x jac_t(3, numDofs + baseDofs); + btInverseDynamics::mat3x jac_r(3, numDofs + baseDofs); + + // Note that inverse dynamics uses zero-based indexing of bodies, not starting from -1 for the base link. + tree->getBodyJacobianTrans(clientCmd.m_calculateJacobianArguments.m_linkIndex + 1, &jac_t); + tree->getBodyJacobianRot(clientCmd.m_calculateJacobianArguments.m_linkIndex + 1, &jac_r); + // Update the translational jacobian based on the desired local point. + // v_pt = v_frame + w x pt + // v_pt = J_t * qd + (J_r * qd) x pt + // v_pt = J_t * qd - pt x (J_r * qd) + // v_pt = J_t * qd - pt_x * J_r * qd) + // v_pt = (J_t - pt_x * J_r) * qd + // J_t_new = J_t - pt_x * J_r + btInverseDynamics::vec3 localPosition; + for (int i = 0; i < 3; ++i) { + localPosition(i) = clientCmd.m_calculateJacobianArguments.m_localPosition[i]; + } + // Only calculate if the localPosition is non-zero. + if (btInverseDynamics::maxAbs(localPosition) > 0.0) { + // Write the localPosition into world coordinates. + btInverseDynamics::mat33 world_rotation_body; + tree->getBodyTransform(clientCmd.m_calculateJacobianArguments.m_linkIndex + 1, &world_rotation_body); + localPosition = world_rotation_body * localPosition; + // Correct the translational jacobian. + btInverseDynamics::mat33 skewCrossProduct; + btInverseDynamics::skew(localPosition, &skewCrossProduct); + btInverseDynamics::mat3x jac_l(3, numDofs + baseDofs); + btInverseDynamics::mul(skewCrossProduct, jac_r, &jac_l); + btInverseDynamics::mat3x jac_t_new(3, numDofs + baseDofs); + btInverseDynamics::sub(jac_t, jac_l, &jac_t_new); + jac_t = jac_t_new; + } + // Fill in the result into the shared memory. + for (int i = 0; i < 3; ++i) + { + for (int j = 0; j < (numDofs + baseDofs); ++j) + { + int element = (numDofs + baseDofs) * i + j; + serverCmd.m_jacobianResultArgs.m_linearJacobian[element] = jac_t(i,j); + serverCmd.m_jacobianResultArgs.m_angularJacobian[element] = jac_r(i,j); + } + } + serverCmd.m_type = CMD_CALCULATED_JACOBIAN_COMPLETED; + } + else + { + serverCmd.m_type = CMD_CALCULATED_JACOBIAN_FAILED; + } + } + + } + else + { + serverCmd.m_type = CMD_CALCULATED_JACOBIAN_FAILED; + } + + return hasStatus; +} + + +bool PhysicsServerCommandProcessor::processCalculateMassMatrixCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + BT_PROFILE("CMD_CALCULATE_MASS_MATRIX"); + + SharedMemoryStatus& serverCmd = serverStatusOut; + serverCmd.m_type = CMD_CALCULATED_MASS_MATRIX_FAILED; + InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(clientCmd.m_calculateMassMatrixArguments.m_bodyUniqueId); + if (bodyHandle && bodyHandle->m_multiBody) + { + + btInverseDynamics::MultiBodyTree* tree = m_data->findOrCreateTree(bodyHandle->m_multiBody); + + if (tree) + { + int baseDofs = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 6; + const int numDofs = bodyHandle->m_multiBody->getNumDofs(); + const int totDofs = numDofs + baseDofs; + btInverseDynamics::vecx q(totDofs); + btInverseDynamics::matxx massMatrix(totDofs, totDofs); + for (int i = 0; i < numDofs; i++) + { + q[i + baseDofs] = clientCmd.m_calculateMassMatrixArguments.m_jointPositionsQ[i]; + } + if (-1 != tree->calculateMassMatrix(q, &massMatrix)) + { + serverCmd.m_massMatrixResultArgs.m_dofCount = totDofs; + // Fill in the result into the shared memory. + double* sharedBuf = (double*)bufferServerToClient; + int sizeInBytes = totDofs*totDofs*sizeof(double); + if (sizeInBytes < bufferSizeInBytes) + { + for (int i = 0; i < (totDofs); ++i) + { + for (int j = 0; j < (totDofs); ++j) + { + int element = (totDofs) * i + j; + + sharedBuf[element] = massMatrix(i,j); + } + } + serverCmd.m_type = CMD_CALCULATED_MASS_MATRIX_COMPLETED; + } + } + + } + + } + else + { + serverCmd.m_type = CMD_CALCULATED_MASS_MATRIX_FAILED; + } + + return hasStatus; +} + + +bool PhysicsServerCommandProcessor::processApplyExternalForceCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + + BT_PROFILE("CMD_APPLY_EXTERNAL_FORCE"); + + if (m_data->m_verboseOutput) + { + b3Printf("CMD_APPLY_EXTERNAL_FORCE clientCmd = %d\n", clientCmd.m_sequenceNumber); + } + for (int i = 0; i < clientCmd.m_externalForceArguments.m_numForcesAndTorques; ++i) + { + InternalBodyData* body = m_data->m_bodyHandles.getHandle(clientCmd.m_externalForceArguments.m_bodyUniqueIds[i]); + bool isLinkFrame = ((clientCmd.m_externalForceArguments.m_forceFlags[i] & EF_LINK_FRAME) != 0); + + if (body && body->m_multiBody) + { + btMultiBody* mb = body->m_multiBody; + + if ((clientCmd.m_externalForceArguments.m_forceFlags[i] & EF_FORCE)!=0) + { + btVector3 tmpForce(clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+0], + clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+1], + clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+2]); + btVector3 tmpPosition( + clientCmd.m_externalForceArguments.m_positions[i*3+0], + clientCmd.m_externalForceArguments.m_positions[i*3+1], + clientCmd.m_externalForceArguments.m_positions[i*3+2]); + + + if (clientCmd.m_externalForceArguments.m_linkIds[i] == -1) + { + btVector3 forceWorld = isLinkFrame ? mb->getBaseWorldTransform().getBasis()*tmpForce : tmpForce; + btVector3 relPosWorld = isLinkFrame ? mb->getBaseWorldTransform().getBasis()*tmpPosition : tmpPosition - mb->getBaseWorldTransform().getOrigin(); + mb->addBaseForce(forceWorld); + mb->addBaseTorque(relPosWorld.cross(forceWorld)); + //b3Printf("apply base force of %f,%f,%f at %f,%f,%f\n", forceWorld[0],forceWorld[1],forceWorld[2],positionLocal[0],positionLocal[1],positionLocal[2]); + } else + { + int link = clientCmd.m_externalForceArguments.m_linkIds[i]; + + btVector3 forceWorld = isLinkFrame ? mb->getLink(link).m_cachedWorldTransform.getBasis()*tmpForce : tmpForce; + btVector3 relPosWorld = isLinkFrame ? mb->getLink(link).m_cachedWorldTransform.getBasis()*tmpPosition : tmpPosition - mb->getBaseWorldTransform().getOrigin(); + mb->addLinkForce(link, forceWorld); + mb->addLinkTorque(link,relPosWorld.cross(forceWorld)); + //b3Printf("apply link force of %f,%f,%f at %f,%f,%f\n", forceWorld[0],forceWorld[1],forceWorld[2], positionLocal[0],positionLocal[1],positionLocal[2]); + } + } + if ((clientCmd.m_externalForceArguments.m_forceFlags[i] & EF_TORQUE)!=0) + { + btVector3 torqueLocal(clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+0], + clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+1], + clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+2]); + + if (clientCmd.m_externalForceArguments.m_linkIds[i] == -1) + { + btVector3 torqueWorld = isLinkFrame ? torqueLocal : mb->getBaseWorldTransform().getBasis()*torqueLocal; + mb->addBaseTorque(torqueWorld); + //b3Printf("apply base torque of %f,%f,%f\n", torqueWorld[0],torqueWorld[1],torqueWorld[2]); + } else + { + int link = clientCmd.m_externalForceArguments.m_linkIds[i]; + btVector3 torqueWorld = mb->getLink(link).m_cachedWorldTransform.getBasis()*torqueLocal; + mb->addLinkTorque(link, torqueWorld); + //b3Printf("apply link torque of %f,%f,%f\n", torqueWorld[0],torqueWorld[1],torqueWorld[2]); + } + } + } + + if (body && body->m_rigidBody) + { + btRigidBody* rb = body->m_rigidBody; + if ((clientCmd.m_externalForceArguments.m_forceFlags[i] & EF_FORCE) != 0) + { + btVector3 forceLocal(clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 0], + clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 1], + clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 2]); + btVector3 positionLocal( + clientCmd.m_externalForceArguments.m_positions[i * 3 + 0], + clientCmd.m_externalForceArguments.m_positions[i * 3 + 1], + clientCmd.m_externalForceArguments.m_positions[i * 3 + 2]); + + btVector3 forceWorld = isLinkFrame ? forceLocal : rb->getWorldTransform().getBasis()*forceLocal; + btVector3 relPosWorld = isLinkFrame ? positionLocal : rb->getWorldTransform().getBasis()*positionLocal; + rb->applyForce(forceWorld, relPosWorld); + + } + + if ((clientCmd.m_externalForceArguments.m_forceFlags[i] & EF_TORQUE) != 0) + { + btVector3 torqueLocal(clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 0], + clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 1], + clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 2]); + + btVector3 torqueWorld = isLinkFrame ? torqueLocal : rb->getWorldTransform().getBasis()*torqueLocal; + rb->applyTorque(torqueWorld); + } + } + } + + SharedMemoryStatus& serverCmd =serverStatusOut; + serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED; + return hasStatus; +} + +bool PhysicsServerCommandProcessor::processRemoveBodyCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + + SharedMemoryStatus& serverCmd =serverStatusOut; + serverCmd.m_type = CMD_REMOVE_BODY_FAILED; + serverCmd.m_removeObjectArgs.m_numBodies = 0; + serverCmd.m_removeObjectArgs.m_numUserConstraints = 0; + + m_data->m_guiHelper->setVisualizerFlag(COV_ENABLE_SYNC_RENDERING_INTERNAL,0); + + for (int i=0;im_bodyHandles.getHandle(bodyUniqueId); + if (bodyHandle) + { + if (bodyHandle->m_multiBody) + { + serverCmd.m_removeObjectArgs.m_bodyUniqueIds[serverCmd.m_removeObjectArgs.m_numBodies++] = bodyUniqueId; + + //also remove user constraints... + for (int i=m_data->m_dynamicsWorld->getNumMultiBodyConstraints()-1;i>=0;i--) + { + btMultiBodyConstraint* mbc = m_data->m_dynamicsWorld->getMultiBodyConstraint(i); + if ((mbc->getMultiBodyA() == bodyHandle->m_multiBody)||(mbc->getMultiBodyB()==bodyHandle->m_multiBody)) + { + m_data->m_dynamicsWorld->removeMultiBodyConstraint(mbc); + + //also remove user constraint and submit it as removed + for (int c=m_data->m_userConstraints.size()-1;c>=0;c--) + { + InteralUserConstraintData* userConstraintPtr = m_data->m_userConstraints.getAtIndex(c); + int userConstraintKey = m_data->m_userConstraints.getKeyAtIndex(c).getUid1(); + + if (userConstraintPtr->m_mbConstraint == mbc) + { + m_data->m_userConstraints.remove(userConstraintKey); + serverCmd.m_removeObjectArgs.m_userConstraintUniqueIds[serverCmd.m_removeObjectArgs.m_numUserConstraints++]=userConstraintKey; + } + } + + delete mbc; + + + } + } + + if (bodyHandle->m_multiBody->getBaseCollider()) + { + m_data->m_visualConverter.removeVisualShape(bodyHandle->m_multiBody->getBaseCollider()); + m_data->m_dynamicsWorld->removeCollisionObject(bodyHandle->m_multiBody->getBaseCollider()); + int graphicsIndex = bodyHandle->m_multiBody->getBaseCollider()->getUserIndex(); + m_data->m_guiHelper->removeGraphicsInstance(graphicsIndex); + } + for (int link=0;linkm_multiBody->getNumLinks();link++) + { + + if (bodyHandle->m_multiBody->getLink(link).m_collider) + { + m_data->m_visualConverter.removeVisualShape(bodyHandle->m_multiBody->getLink(link).m_collider); + m_data->m_dynamicsWorld->removeCollisionObject(bodyHandle->m_multiBody->getLink(link).m_collider); + int graphicsIndex = bodyHandle->m_multiBody->getLink(link).m_collider->getUserIndex(); + m_data->m_guiHelper->removeGraphicsInstance(graphicsIndex); + } + } + int numCollisionObjects = m_data->m_dynamicsWorld->getNumCollisionObjects(); + m_data->m_dynamicsWorld->removeMultiBody(bodyHandle->m_multiBody); + numCollisionObjects = m_data->m_dynamicsWorld->getNumCollisionObjects(); + //todo: clear all other remaining data, release memory etc + + delete bodyHandle->m_multiBody; + bodyHandle->m_multiBody=0; + serverCmd.m_type = CMD_REMOVE_BODY_COMPLETED; + } + if (bodyHandle->m_rigidBody) + { + m_data->m_visualConverter.removeVisualShape(bodyHandle->m_rigidBody); + serverCmd.m_removeObjectArgs.m_bodyUniqueIds[serverCmd.m_removeObjectArgs.m_numBodies++] = bodyUniqueId; + //todo: clear all other remaining data... + m_data->m_dynamicsWorld->removeRigidBody(bodyHandle->m_rigidBody); + int graphicsInstance = bodyHandle->m_rigidBody->getUserIndex2(); + m_data->m_guiHelper->removeGraphicsInstance(graphicsInstance); + delete bodyHandle->m_rigidBody; + bodyHandle->m_rigidBody=0; + serverCmd.m_type = CMD_REMOVE_BODY_COMPLETED; + } + } + + m_data->m_bodyHandles.freeHandle(bodyUniqueId); + } + m_data->m_guiHelper->setVisualizerFlag(COV_ENABLE_SYNC_RENDERING_INTERNAL,1); + + return hasStatus; +} + +bool PhysicsServerCommandProcessor::processCreateUserConstraintCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + + BT_PROFILE("CMD_USER_CONSTRAINT"); + + SharedMemoryStatus& serverCmd =serverStatusOut; + serverCmd.m_type = CMD_USER_CONSTRAINT_FAILED; + hasStatus = true; + if (clientCmd.m_updateFlags & USER_CONSTRAINT_REQUEST_STATE) + { + int constraintUid = clientCmd.m_userConstraintArguments.m_userConstraintUniqueId; + InteralUserConstraintData* userConstraintPtr = m_data->m_userConstraints.find(constraintUid); + if (userConstraintPtr) + { + serverCmd.m_userConstraintStateResultArgs.m_numDofs = 0; + for (int i = 0; i < 6; i++) + { + serverCmd.m_userConstraintStateResultArgs.m_appliedConstraintForces[i] = 0; + } + if (userConstraintPtr->m_mbConstraint) + { + serverCmd.m_userConstraintStateResultArgs.m_numDofs = userConstraintPtr->m_mbConstraint->getNumRows(); + for (int i = 0; i < userConstraintPtr->m_mbConstraint->getNumRows(); i++) + { + serverCmd.m_userConstraintStateResultArgs.m_appliedConstraintForces[i] = userConstraintPtr->m_mbConstraint->getAppliedImpulse(i) / m_data->m_dynamicsWorld->getSolverInfo().m_timeStep; + } + serverCmd.m_type = CMD_USER_CONSTRAINT_REQUEST_STATE_COMPLETED; + } + } + + }; + if (clientCmd.m_updateFlags & USER_CONSTRAINT_REQUEST_INFO) + { + int userConstraintUidChange = clientCmd.m_userConstraintArguments.m_userConstraintUniqueId; + InteralUserConstraintData* userConstraintPtr = m_data->m_userConstraints.find(userConstraintUidChange); + if (userConstraintPtr) + { + serverCmd.m_userConstraintResultArgs = userConstraintPtr->m_userConstraintData; + + serverCmd.m_type = CMD_USER_CONSTRAINT_INFO_COMPLETED; + } + } + if (clientCmd.m_updateFlags & USER_CONSTRAINT_ADD_CONSTRAINT) + { + btScalar defaultMaxForce = 500.0; + InternalBodyData* parentBody = m_data->m_bodyHandles.getHandle(clientCmd.m_userConstraintArguments.m_parentBodyIndex); + if (parentBody && parentBody->m_multiBody) + { + if ((clientCmd.m_userConstraintArguments.m_parentJointIndex>=-1) && clientCmd.m_userConstraintArguments.m_parentJointIndex < parentBody->m_multiBody->getNumLinks()) + { + InternalBodyData* childBody = clientCmd.m_userConstraintArguments.m_childBodyIndex>=0 ? m_data->m_bodyHandles.getHandle(clientCmd.m_userConstraintArguments.m_childBodyIndex):0; + //also create a constraint with just a single multibody/rigid body without child + //if (childBody) + { + btVector3 pivotInParent(clientCmd.m_userConstraintArguments.m_parentFrame[0], clientCmd.m_userConstraintArguments.m_parentFrame[1], clientCmd.m_userConstraintArguments.m_parentFrame[2]); + btVector3 pivotInChild(clientCmd.m_userConstraintArguments.m_childFrame[0], clientCmd.m_userConstraintArguments.m_childFrame[1], clientCmd.m_userConstraintArguments.m_childFrame[2]); + btMatrix3x3 frameInParent(btQuaternion(clientCmd.m_userConstraintArguments.m_parentFrame[3], clientCmd.m_userConstraintArguments.m_parentFrame[4], clientCmd.m_userConstraintArguments.m_parentFrame[5], clientCmd.m_userConstraintArguments.m_parentFrame[6])); + btMatrix3x3 frameInChild(btQuaternion(clientCmd.m_userConstraintArguments.m_childFrame[3], clientCmd.m_userConstraintArguments.m_childFrame[4], clientCmd.m_userConstraintArguments.m_childFrame[5], clientCmd.m_userConstraintArguments.m_childFrame[6])); + btVector3 jointAxis(clientCmd.m_userConstraintArguments.m_jointAxis[0], clientCmd.m_userConstraintArguments.m_jointAxis[1], clientCmd.m_userConstraintArguments.m_jointAxis[2]); + + + + if (clientCmd.m_userConstraintArguments.m_jointType == eGearType) + { + if (childBody && childBody->m_multiBody) + { + if ((clientCmd.m_userConstraintArguments.m_childJointIndex>=-1) && (clientCmd.m_userConstraintArguments.m_childJointIndex m_multiBody->getNumLinks())) + { + btMultiBodyGearConstraint* multibodyGear = new btMultiBodyGearConstraint(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,childBody->m_multiBody,clientCmd.m_userConstraintArguments.m_childJointIndex,pivotInParent,pivotInChild,frameInParent,frameInChild); + multibodyGear->setMaxAppliedImpulse(defaultMaxForce); + m_data->m_dynamicsWorld->addMultiBodyConstraint(multibodyGear); + InteralUserConstraintData userConstraintData; + userConstraintData.m_mbConstraint = multibodyGear; + int uid = m_data->m_userConstraintUIDGenerator++; + serverCmd.m_userConstraintResultArgs = clientCmd.m_userConstraintArguments; + serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid; + serverCmd.m_userConstraintResultArgs.m_maxAppliedForce = defaultMaxForce; + userConstraintData.m_userConstraintData = serverCmd.m_userConstraintResultArgs; + m_data->m_userConstraints.insert(uid,userConstraintData); + serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED; + } + + } + } + else if (clientCmd.m_userConstraintArguments.m_jointType == eFixedType) + { + if (childBody && childBody->m_multiBody) + { + if ((clientCmd.m_userConstraintArguments.m_childJointIndex>=-1) && (clientCmd.m_userConstraintArguments.m_childJointIndex m_multiBody->getNumLinks())) + { + btMultiBodyFixedConstraint* multibodyFixed = new btMultiBodyFixedConstraint(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,childBody->m_multiBody,clientCmd.m_userConstraintArguments.m_childJointIndex,pivotInParent,pivotInChild,frameInParent,frameInChild); + multibodyFixed->setMaxAppliedImpulse(defaultMaxForce); + m_data->m_dynamicsWorld->addMultiBodyConstraint(multibodyFixed); + InteralUserConstraintData userConstraintData; + userConstraintData.m_mbConstraint = multibodyFixed; + int uid = m_data->m_userConstraintUIDGenerator++; + serverCmd.m_userConstraintResultArgs = clientCmd.m_userConstraintArguments; + serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid; + serverCmd.m_userConstraintResultArgs.m_maxAppliedForce = defaultMaxForce; + userConstraintData.m_userConstraintData = serverCmd.m_userConstraintResultArgs; + m_data->m_userConstraints.insert(uid,userConstraintData); + serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED; + } + + } + else + { + btRigidBody* rb = childBody? childBody->m_rigidBody : 0; + btMultiBodyFixedConstraint* rigidbodyFixed = new btMultiBodyFixedConstraint(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,rb,pivotInParent,pivotInChild,frameInParent,frameInChild); + rigidbodyFixed->setMaxAppliedImpulse(defaultMaxForce); + btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld; + world->addMultiBodyConstraint(rigidbodyFixed); + InteralUserConstraintData userConstraintData; + userConstraintData.m_mbConstraint = rigidbodyFixed; + int uid = m_data->m_userConstraintUIDGenerator++; + serverCmd.m_userConstraintResultArgs = clientCmd.m_userConstraintArguments; + serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid; + serverCmd.m_userConstraintResultArgs.m_maxAppliedForce = defaultMaxForce; + userConstraintData.m_userConstraintData = serverCmd.m_userConstraintResultArgs; + m_data->m_userConstraints.insert(uid,userConstraintData); + serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED; + } + + } + else if (clientCmd.m_userConstraintArguments.m_jointType == ePrismaticType) + { + if (childBody && childBody->m_multiBody) + { + btMultiBodySliderConstraint* multibodySlider = new btMultiBodySliderConstraint(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,childBody->m_multiBody,clientCmd.m_userConstraintArguments.m_childJointIndex,pivotInParent,pivotInChild,frameInParent,frameInChild,jointAxis); + multibodySlider->setMaxAppliedImpulse(defaultMaxForce); + m_data->m_dynamicsWorld->addMultiBodyConstraint(multibodySlider); + InteralUserConstraintData userConstraintData; + userConstraintData.m_mbConstraint = multibodySlider; + int uid = m_data->m_userConstraintUIDGenerator++; + serverCmd.m_userConstraintResultArgs = clientCmd.m_userConstraintArguments; + serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid; + serverCmd.m_userConstraintResultArgs.m_maxAppliedForce = defaultMaxForce; + userConstraintData.m_userConstraintData = serverCmd.m_userConstraintResultArgs; + m_data->m_userConstraints.insert(uid,userConstraintData); + serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED; + } + else + { + btRigidBody* rb = childBody? childBody->m_rigidBody : 0; + + btMultiBodySliderConstraint* rigidbodySlider = new btMultiBodySliderConstraint(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,rb,pivotInParent,pivotInChild,frameInParent,frameInChild,jointAxis); + rigidbodySlider->setMaxAppliedImpulse(defaultMaxForce); + btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld; + world->addMultiBodyConstraint(rigidbodySlider); + InteralUserConstraintData userConstraintData; + userConstraintData.m_mbConstraint = rigidbodySlider; + int uid = m_data->m_userConstraintUIDGenerator++; + serverCmd.m_userConstraintResultArgs = clientCmd.m_userConstraintArguments; + serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid; + serverCmd.m_userConstraintResultArgs.m_maxAppliedForce = defaultMaxForce; + userConstraintData.m_userConstraintData = serverCmd.m_userConstraintResultArgs; + m_data->m_userConstraints.insert(uid,userConstraintData); + serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED; } + + } else if (clientCmd.m_userConstraintArguments.m_jointType == ePoint2PointType) + { + if (childBody && childBody->m_multiBody) + { + btMultiBodyPoint2Point* p2p = new btMultiBodyPoint2Point(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,childBody->m_multiBody,clientCmd.m_userConstraintArguments.m_childJointIndex,pivotInParent,pivotInChild); + p2p->setMaxAppliedImpulse(defaultMaxForce); + m_data->m_dynamicsWorld->addMultiBodyConstraint(p2p); + InteralUserConstraintData userConstraintData; + userConstraintData.m_mbConstraint = p2p; + int uid = m_data->m_userConstraintUIDGenerator++; + serverCmd.m_userConstraintResultArgs = clientCmd.m_userConstraintArguments; + serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid; + serverCmd.m_userConstraintResultArgs.m_maxAppliedForce = defaultMaxForce; + userConstraintData.m_userConstraintData = serverCmd.m_userConstraintResultArgs; + m_data->m_userConstraints.insert(uid,userConstraintData); + serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED; + } + else + { + btRigidBody* rb = childBody? childBody->m_rigidBody : 0; + + btMultiBodyPoint2Point* p2p = new btMultiBodyPoint2Point(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,rb,pivotInParent,pivotInChild); + p2p->setMaxAppliedImpulse(defaultMaxForce); + btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld; + world->addMultiBodyConstraint(p2p); + InteralUserConstraintData userConstraintData; + userConstraintData.m_mbConstraint = p2p; + int uid = m_data->m_userConstraintUIDGenerator++; + serverCmd.m_userConstraintResultArgs = clientCmd.m_userConstraintArguments; + serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid; + serverCmd.m_userConstraintResultArgs.m_maxAppliedForce = defaultMaxForce; + userConstraintData.m_userConstraintData = serverCmd.m_userConstraintResultArgs; + m_data->m_userConstraints.insert(uid,userConstraintData); + serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED; + } + + } else + { + b3Warning("unknown constraint type"); + } + + + } + } + } + else + { + InternalBodyData* childBody = clientCmd.m_userConstraintArguments.m_childBodyIndex>=0 ? m_data->m_bodyHandles.getHandle(clientCmd.m_userConstraintArguments.m_childBodyIndex):0; + + if (parentBody && childBody) + { + if (parentBody->m_rigidBody) + { + + btRigidBody* parentRb = 0; + if (clientCmd.m_userConstraintArguments.m_parentJointIndex==-1) + { + parentRb = parentBody->m_rigidBody; + } else + { + if ((clientCmd.m_userConstraintArguments.m_parentJointIndex>=0) && + (clientCmd.m_userConstraintArguments.m_parentJointIndexm_rigidBodyJoints.size())) + { + parentRb = &parentBody->m_rigidBodyJoints[clientCmd.m_userConstraintArguments.m_parentJointIndex]->getRigidBodyB(); + } + } + + + btRigidBody* childRb = 0; + if (childBody->m_rigidBody) + { + + if (clientCmd.m_userConstraintArguments.m_childJointIndex==-1) + { + childRb = childBody->m_rigidBody; + } + else + { + if ((clientCmd.m_userConstraintArguments.m_childJointIndex>=0) + && (clientCmd.m_userConstraintArguments.m_childJointIndexm_rigidBodyJoints.size())) + { + childRb = &childBody->m_rigidBodyJoints[clientCmd.m_userConstraintArguments.m_childJointIndex]->getRigidBodyB(); + } + + } + } + + switch (clientCmd.m_userConstraintArguments.m_jointType) + { + case eRevoluteType: + { + break; + } + case ePrismaticType: + { + break; + } + + case eFixedType: + { + if (childRb && parentRb && (childRb!=parentRb)) + { + btVector3 pivotInParent(clientCmd.m_userConstraintArguments.m_parentFrame[0], clientCmd.m_userConstraintArguments.m_parentFrame[1], clientCmd.m_userConstraintArguments.m_parentFrame[2]); + btVector3 pivotInChild(clientCmd.m_userConstraintArguments.m_childFrame[0], clientCmd.m_userConstraintArguments.m_childFrame[1], clientCmd.m_userConstraintArguments.m_childFrame[2]); + + btTransform offsetTrA,offsetTrB; + offsetTrA.setIdentity(); + offsetTrA.setOrigin(pivotInParent); + offsetTrB.setIdentity(); + offsetTrB.setOrigin(pivotInChild); + + btGeneric6DofSpring2Constraint* dof6 = new btGeneric6DofSpring2Constraint(*parentRb, *childRb, offsetTrA, offsetTrB); + + dof6->setLinearLowerLimit(btVector3(0,0,0)); + dof6->setLinearUpperLimit(btVector3(0,0,0)); + + dof6->setAngularLowerLimit(btVector3(0,0,0)); + dof6->setAngularUpperLimit(btVector3(0,0,0)); + m_data->m_dynamicsWorld->addConstraint(dof6); + InteralUserConstraintData userConstraintData; + userConstraintData.m_rbConstraint = dof6; + int uid = m_data->m_userConstraintUIDGenerator++; + serverCmd.m_userConstraintResultArgs = clientCmd.m_userConstraintArguments; + serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid; + serverCmd.m_userConstraintResultArgs.m_maxAppliedForce = defaultMaxForce; + userConstraintData.m_userConstraintData = serverCmd.m_userConstraintResultArgs; + m_data->m_userConstraints.insert(uid,userConstraintData); + serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED; + } + + break; + } + + case ePoint2PointType: + { + if (childRb && parentRb && (childRb!=parentRb)) + { + btVector3 pivotInParent(clientCmd.m_userConstraintArguments.m_parentFrame[0], clientCmd.m_userConstraintArguments.m_parentFrame[1], clientCmd.m_userConstraintArguments.m_parentFrame[2]); + btVector3 pivotInChild(clientCmd.m_userConstraintArguments.m_childFrame[0], clientCmd.m_userConstraintArguments.m_childFrame[1], clientCmd.m_userConstraintArguments.m_childFrame[2]); + + btPoint2PointConstraint* p2p = new btPoint2PointConstraint(*parentRb,*childRb,pivotInParent,pivotInChild); + p2p->m_setting.m_impulseClamp = defaultMaxForce; + m_data->m_dynamicsWorld->addConstraint(p2p); + InteralUserConstraintData userConstraintData; + userConstraintData.m_rbConstraint = p2p; + int uid = m_data->m_userConstraintUIDGenerator++; + serverCmd.m_userConstraintResultArgs = clientCmd.m_userConstraintArguments; + serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid; + serverCmd.m_userConstraintResultArgs.m_maxAppliedForce = defaultMaxForce; + userConstraintData.m_userConstraintData = serverCmd.m_userConstraintResultArgs; + m_data->m_userConstraints.insert(uid,userConstraintData); + serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED; + } + break; + } + + case eGearType: + { + + if (childRb && parentRb && (childRb!=parentRb)) + { + btVector3 axisA(clientCmd.m_userConstraintArguments.m_jointAxis[0], + clientCmd.m_userConstraintArguments.m_jointAxis[1], + clientCmd.m_userConstraintArguments.m_jointAxis[2]); + //for now we use the same local axis for both objects + btVector3 axisB(clientCmd.m_userConstraintArguments.m_jointAxis[0], + clientCmd.m_userConstraintArguments.m_jointAxis[1], + clientCmd.m_userConstraintArguments.m_jointAxis[2]); + btScalar ratio=1; + btGearConstraint* gear = new btGearConstraint(*parentRb,*childRb, axisA,axisB,ratio); + m_data->m_dynamicsWorld->addConstraint(gear,true); + InteralUserConstraintData userConstraintData; + userConstraintData.m_rbConstraint = gear; + int uid = m_data->m_userConstraintUIDGenerator++; + serverCmd.m_userConstraintResultArgs = clientCmd.m_userConstraintArguments; + serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid; + serverCmd.m_userConstraintResultArgs.m_maxAppliedForce = defaultMaxForce; + userConstraintData.m_userConstraintData = serverCmd.m_userConstraintResultArgs; + m_data->m_userConstraints.insert(uid,userConstraintData); + serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED; + } + break; + } + case eSphericalType: + { + b3Warning("constraint type not handled yet"); + break; + } + case ePlanarType: + { + b3Warning("constraint type not handled yet"); + break; + } + default: + { + b3Warning("unknown constraint type"); + } + }; + } + } + } + } + + if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_CONSTRAINT) + { + serverCmd.m_type = CMD_CHANGE_USER_CONSTRAINT_FAILED; + int userConstraintUidChange = clientCmd.m_userConstraintArguments.m_userConstraintUniqueId; + InteralUserConstraintData* userConstraintPtr = m_data->m_userConstraints.find(userConstraintUidChange); + if (userConstraintPtr) + { + if (userConstraintPtr->m_mbConstraint) + { + if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_PIVOT_IN_B) + { + btVector3 pivotInB(clientCmd.m_userConstraintArguments.m_childFrame[0], + clientCmd.m_userConstraintArguments.m_childFrame[1], + clientCmd.m_userConstraintArguments.m_childFrame[2]); + userConstraintPtr->m_userConstraintData.m_childFrame[0] = clientCmd.m_userConstraintArguments.m_childFrame[0]; + userConstraintPtr->m_userConstraintData.m_childFrame[1] = clientCmd.m_userConstraintArguments.m_childFrame[1]; + userConstraintPtr->m_userConstraintData.m_childFrame[2] = clientCmd.m_userConstraintArguments.m_childFrame[2]; + userConstraintPtr->m_mbConstraint->setPivotInB(pivotInB); + } + if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_FRAME_ORN_IN_B) + { + btQuaternion childFrameOrn(clientCmd.m_userConstraintArguments.m_childFrame[3], + clientCmd.m_userConstraintArguments.m_childFrame[4], + clientCmd.m_userConstraintArguments.m_childFrame[5], + clientCmd.m_userConstraintArguments.m_childFrame[6]); + userConstraintPtr->m_userConstraintData.m_childFrame[3] = clientCmd.m_userConstraintArguments.m_childFrame[3]; + userConstraintPtr->m_userConstraintData.m_childFrame[4] = clientCmd.m_userConstraintArguments.m_childFrame[4]; + userConstraintPtr->m_userConstraintData.m_childFrame[5] = clientCmd.m_userConstraintArguments.m_childFrame[5]; + userConstraintPtr->m_userConstraintData.m_childFrame[6] = clientCmd.m_userConstraintArguments.m_childFrame[6]; + btMatrix3x3 childFrameBasis(childFrameOrn); + userConstraintPtr->m_mbConstraint->setFrameInB(childFrameBasis); + } + if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_MAX_FORCE) + { + btScalar maxImp = clientCmd.m_userConstraintArguments.m_maxAppliedForce*m_data->m_physicsDeltaTime; + userConstraintPtr->m_userConstraintData.m_maxAppliedForce = clientCmd.m_userConstraintArguments.m_maxAppliedForce; + userConstraintPtr->m_mbConstraint->setMaxAppliedImpulse(maxImp); + } + + if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_GEAR_RATIO) + { + userConstraintPtr->m_mbConstraint->setGearRatio(clientCmd.m_userConstraintArguments.m_gearRatio); + userConstraintPtr->m_userConstraintData.m_gearRatio = clientCmd.m_userConstraintArguments.m_gearRatio; + } + if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_RELATIVE_POSITION_TARGET) + { + userConstraintPtr->m_mbConstraint->setRelativePositionTarget(clientCmd.m_userConstraintArguments.m_relativePositionTarget); + userConstraintPtr->m_userConstraintData.m_relativePositionTarget = clientCmd.m_userConstraintArguments.m_relativePositionTarget; + } + if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_ERP) + { + userConstraintPtr->m_mbConstraint->setErp(clientCmd.m_userConstraintArguments.m_erp); + userConstraintPtr->m_userConstraintData.m_erp = clientCmd.m_userConstraintArguments.m_erp; + } + + if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_GEAR_AUX_LINK) + { + userConstraintPtr->m_mbConstraint->setGearAuxLink(clientCmd.m_userConstraintArguments.m_gearAuxLink); + userConstraintPtr->m_userConstraintData.m_gearAuxLink = clientCmd.m_userConstraintArguments.m_gearAuxLink; + } + + } + if (userConstraintPtr->m_rbConstraint) + { + if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_MAX_FORCE) + { + btScalar maxImp = clientCmd.m_userConstraintArguments.m_maxAppliedForce*m_data->m_physicsDeltaTime; + userConstraintPtr->m_userConstraintData.m_maxAppliedForce = clientCmd.m_userConstraintArguments.m_maxAppliedForce; + //userConstraintPtr->m_rbConstraint->setMaxAppliedImpulse(maxImp); + } + + if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_GEAR_RATIO) + { + if (userConstraintPtr->m_rbConstraint->getObjectType()==GEAR_CONSTRAINT_TYPE) + { + btGearConstraint* gear = (btGearConstraint*) userConstraintPtr->m_rbConstraint; + gear->setRatio(clientCmd.m_userConstraintArguments.m_gearRatio); + } + } + } + serverCmd.m_userConstraintResultArgs = clientCmd.m_userConstraintArguments; + serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = userConstraintUidChange; + serverCmd.m_updateFlags = clientCmd.m_updateFlags; + serverCmd.m_type = CMD_CHANGE_USER_CONSTRAINT_COMPLETED; + } + } + if (clientCmd.m_updateFlags & USER_CONSTRAINT_REMOVE_CONSTRAINT) + { + serverCmd.m_type = CMD_REMOVE_USER_CONSTRAINT_FAILED; + int userConstraintUidRemove = clientCmd.m_userConstraintArguments.m_userConstraintUniqueId; + InteralUserConstraintData* userConstraintPtr = m_data->m_userConstraints.find(userConstraintUidRemove); + if (userConstraintPtr) + { + if (userConstraintPtr->m_mbConstraint) + { + m_data->m_dynamicsWorld->removeMultiBodyConstraint(userConstraintPtr->m_mbConstraint); + delete userConstraintPtr->m_mbConstraint; + m_data->m_userConstraints.remove(userConstraintUidRemove); + } + if (userConstraintPtr->m_rbConstraint) + { + m_data->m_dynamicsWorld->removeConstraint(userConstraintPtr->m_rbConstraint); + delete userConstraintPtr->m_rbConstraint; + m_data->m_userConstraints.remove(userConstraintUidRemove); + } + serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = userConstraintUidRemove; + serverCmd.m_type = CMD_REMOVE_USER_CONSTRAINT_COMPLETED; + + + } + + + } + return hasStatus; +} + +bool PhysicsServerCommandProcessor::processCalculateInverseKinematicsCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + + BT_PROFILE("CMD_CALCULATE_INVERSE_KINEMATICS"); + SharedMemoryStatus& serverCmd = serverStatusOut; + serverCmd.m_type = CMD_CALCULATE_INVERSE_KINEMATICS_FAILED; + + InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(clientCmd.m_calculateInverseKinematicsArguments.m_bodyUniqueId); + if (bodyHandle && bodyHandle->m_multiBody) + { + IKTrajectoryHelper** ikHelperPtrPtr = m_data->m_inverseKinematicsHelpers.find(bodyHandle->m_multiBody); + IKTrajectoryHelper* ikHelperPtr = 0; + + + if (ikHelperPtrPtr) + { + ikHelperPtr = *ikHelperPtrPtr; + } + else + { + IKTrajectoryHelper* tmpHelper = new IKTrajectoryHelper; + m_data->m_inverseKinematicsHelpers.insert(bodyHandle->m_multiBody, tmpHelper); + ikHelperPtr = tmpHelper; + } + + int endEffectorLinkIndex = clientCmd.m_calculateInverseKinematicsArguments.m_endEffectorLinkIndex; + + + if (ikHelperPtr && (endEffectorLinkIndexm_multiBody->getNumLinks())) + { + const int numDofs = bodyHandle->m_multiBody->getNumDofs(); + int baseDofs = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 6; + b3AlignedObjectArray jacobian_linear; + jacobian_linear.resize(3*numDofs); + b3AlignedObjectArray jacobian_angular; + jacobian_angular.resize(3*numDofs); + int jacSize = 0; + + btInverseDynamics::MultiBodyTree* tree = m_data->findOrCreateTree(bodyHandle->m_multiBody); + + + + btAlignedObjectArray q_current; + q_current.resize(numDofs); + + + + if (tree && ((numDofs+ baseDofs) == tree->numDoFs())) + { + jacSize = jacobian_linear.size(); + // Set jacobian value + + + + btInverseDynamics::vecx nu(numDofs+baseDofs), qdot(numDofs + baseDofs), q(numDofs + baseDofs), joint_force(numDofs + baseDofs); + int DofIndex = 0; + for (int i = 0; i < bodyHandle->m_multiBody->getNumLinks(); ++i) { + if (bodyHandle->m_multiBody->getLink(i).m_jointType >= 0 && bodyHandle->m_multiBody->getLink(i).m_jointType <= 2) { // 0, 1, 2 represent revolute, prismatic, and spherical joint types respectively. Skip the fixed joints. + q_current[DofIndex] = bodyHandle->m_multiBody->getJointPos(i); + q[DofIndex+baseDofs] = bodyHandle->m_multiBody->getJointPos(i); + qdot[DofIndex+baseDofs] = 0; + nu[DofIndex+baseDofs] = 0; + DofIndex++; + } + } // Set the gravity to correspond to the world gravity + btInverseDynamics::vec3 id_grav(m_data->m_dynamicsWorld->getGravity()); + + if (-1 != tree->setGravityInWorldFrame(id_grav) && + -1 != tree->calculateInverseDynamics(q, qdot, nu, &joint_force)) + { + tree->calculateJacobians(q); + btInverseDynamics::mat3x jac_t(3, numDofs+ baseDofs); + btInverseDynamics::mat3x jac_r(3,numDofs + baseDofs); + // Note that inverse dynamics uses zero-based indexing of bodies, not starting from -1 for the base link. + tree->getBodyJacobianTrans(endEffectorLinkIndex+1, &jac_t); + tree->getBodyJacobianRot(endEffectorLinkIndex+1, &jac_r); + for (int i = 0; i < 3; ++i) + { + for (int j = 0; j < numDofs; ++j) + { + jacobian_linear[i*numDofs+j] = jac_t(i,(baseDofs+j)); + jacobian_angular[i*numDofs+j] = jac_r(i,(baseDofs+j)); + } + } + } + + + + btAlignedObjectArray q_new; + q_new.resize(numDofs); + int ikMethod = 0; + if ((clientCmd.m_updateFlags& IK_HAS_TARGET_ORIENTATION)&&(clientCmd.m_updateFlags&IK_HAS_NULL_SPACE_VELOCITY)) + { + //Nullspace task only works with DLS now. TODO: add nullspace task to SDLS. + ikMethod = IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE; + } + else if (clientCmd.m_updateFlags& IK_HAS_TARGET_ORIENTATION) + { + if (clientCmd.m_updateFlags & IK_SDLS) + { + ikMethod = IK2_VEL_SDLS_WITH_ORIENTATION; + } + else + { + ikMethod = IK2_VEL_DLS_WITH_ORIENTATION; + } + } + else if (clientCmd.m_updateFlags& IK_HAS_NULL_SPACE_VELOCITY) + { + //Nullspace task only works with DLS now. TODO: add nullspace task to SDLS. + ikMethod = IK2_VEL_DLS_WITH_NULLSPACE; + } + else + { + if (clientCmd.m_updateFlags & IK_SDLS) + { + ikMethod = IK2_VEL_SDLS; + } + else + { + ikMethod = IK2_VEL_DLS;; + } + } + + if (clientCmd.m_updateFlags& IK_HAS_NULL_SPACE_VELOCITY) + { + btAlignedObjectArray lower_limit; + btAlignedObjectArray upper_limit; + btAlignedObjectArray joint_range; + btAlignedObjectArray rest_pose; + lower_limit.resize(numDofs); + upper_limit.resize(numDofs); + joint_range.resize(numDofs); + rest_pose.resize(numDofs); + for (int i = 0; i < numDofs; ++i) + { + lower_limit[i] = clientCmd.m_calculateInverseKinematicsArguments.m_lowerLimit[i]; + upper_limit[i] = clientCmd.m_calculateInverseKinematicsArguments.m_upperLimit[i]; + joint_range[i] = clientCmd.m_calculateInverseKinematicsArguments.m_jointRange[i]; + rest_pose[i] = clientCmd.m_calculateInverseKinematicsArguments.m_restPose[i]; + } + ikHelperPtr->computeNullspaceVel(numDofs, &q_current[0], &lower_limit[0], &upper_limit[0], &joint_range[0], &rest_pose[0]); + } + + btTransform endEffectorTransformWorld = bodyHandle->m_multiBody->getLink(endEffectorLinkIndex).m_cachedWorldTransform * bodyHandle->m_linkLocalInertialFrames[endEffectorLinkIndex].inverse(); + + btVector3DoubleData endEffectorWorldPosition; + btQuaternionDoubleData endEffectorWorldOrientation; + + btVector3 endEffectorPosWorldOrg = endEffectorTransformWorld.getOrigin(); + btQuaternion endEffectorOriWorldOrg = endEffectorTransformWorld.getRotation(); + btTransform endEffectorWorld; + endEffectorWorld.setOrigin(endEffectorPosWorldOrg); + endEffectorWorld.setRotation(endEffectorOriWorldOrg); + + btTransform tr = bodyHandle->m_multiBody->getBaseWorldTransform(); + + btTransform endEffectorBaseCoord = tr.inverse()*endEffectorWorld; + + btQuaternion endEffectorOriBaseCoord= endEffectorBaseCoord.getRotation(); + + btVector4 endEffectorOri(endEffectorOriBaseCoord.x(), endEffectorOriBaseCoord.y(), endEffectorOriBaseCoord.z(), endEffectorOriBaseCoord.w()); + + endEffectorBaseCoord.getOrigin().serializeDouble(endEffectorWorldPosition); + endEffectorBaseCoord.getRotation().serializeDouble(endEffectorWorldOrientation); + + btVector3 targetPosWorld(clientCmd.m_calculateInverseKinematicsArguments.m_targetPosition[0], + clientCmd.m_calculateInverseKinematicsArguments.m_targetPosition[1], + clientCmd.m_calculateInverseKinematicsArguments.m_targetPosition[2]); + + btQuaternion targetOrnWorld(clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation[0], + clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation[1], + clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation[2], + clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation[3]); + btTransform targetWorld; + targetWorld.setOrigin(targetPosWorld); + targetWorld.setRotation(targetOrnWorld); + btTransform targetBaseCoord; + targetBaseCoord = tr.inverse()*targetWorld; + + btVector3DoubleData targetPosBaseCoord; + btQuaternionDoubleData targetOrnBaseCoord; + targetBaseCoord.getOrigin().serializeDouble(targetPosBaseCoord); + targetBaseCoord.getRotation().serializeDouble(targetOrnBaseCoord); + + // Set joint damping coefficents. A small default + // damping constant is added to prevent singularity + // with pseudo inverse. The user can set joint damping + // coefficients differently for each joint. The larger + // the damping coefficient is, the less we rely on + // this joint to achieve the IK target. + btAlignedObjectArray joint_damping; + joint_damping.resize(numDofs,0.5); + if (clientCmd.m_updateFlags& IK_HAS_JOINT_DAMPING) + { + for (int i = 0; i < numDofs; ++i) + { + joint_damping[i] = clientCmd.m_calculateInverseKinematicsArguments.m_jointDamping[i]; + } + } + ikHelperPtr->setDampingCoeff(numDofs, &joint_damping[0]); + + double targetDampCoeff[6] = { 1.0, 1.0, 1.0, 1.0, 1.0, 1.0 }; + ikHelperPtr->computeIK(targetPosBaseCoord.m_floats, targetOrnBaseCoord.m_floats, + endEffectorWorldPosition.m_floats, endEffectorWorldOrientation.m_floats, + &q_current[0], + numDofs, clientCmd.m_calculateInverseKinematicsArguments.m_endEffectorLinkIndex, + &q_new[0], ikMethod, &jacobian_linear[0], &jacobian_angular[0], jacSize*2, targetDampCoeff); + + serverCmd.m_inverseKinematicsResultArgs.m_bodyUniqueId =clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId; + for (int i=0;im_visualConverter.getNumVisualShapes(clientCmd.m_requestVisualShapeDataArguments.m_bodyUniqueId); + //int totalBytesPerVisualShape = sizeof (b3VisualShapeData); + //int visualShapeStorage = bufferSizeInBytes / totalBytesPerVisualShape - 1; + b3VisualShapeData* visualShapeStoragePtr = (b3VisualShapeData*)bufferServerToClient; + + int remain = totalNumVisualShapes - clientCmd.m_requestVisualShapeDataArguments.m_startingVisualShapeIndex; + int shapeIndex = clientCmd.m_requestVisualShapeDataArguments.m_startingVisualShapeIndex; + + int success = m_data->m_visualConverter.getVisualShapesData(clientCmd.m_requestVisualShapeDataArguments.m_bodyUniqueId, + shapeIndex, + visualShapeStoragePtr); + if (success) { + serverCmd.m_sendVisualShapeArgs.m_numRemainingVisualShapes = remain-1; + serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied = 1; + serverCmd.m_sendVisualShapeArgs.m_startingVisualShapeIndex = clientCmd.m_requestVisualShapeDataArguments.m_startingVisualShapeIndex; + serverCmd.m_sendVisualShapeArgs.m_bodyUniqueId = clientCmd.m_requestVisualShapeDataArguments.m_bodyUniqueId; + serverCmd.m_numDataStreamBytes = sizeof(b3VisualShapeData)*serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied; + serverCmd.m_type = CMD_VISUAL_SHAPE_INFO_COMPLETED; + } + return hasStatus; +} + + +bool PhysicsServerCommandProcessor::processUpdateVisualShapeCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + + BT_PROFILE("CMD_UPDATE_VISUAL_SHAPE"); + SharedMemoryStatus& serverCmd = serverStatusOut; + serverCmd.m_type = CMD_VISUAL_SHAPE_UPDATE_FAILED; + InternalTextureHandle* texHandle = 0; + + if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_TEXTURE) + { + texHandle = m_data->m_textureHandles.getHandle(clientCmd.m_updateVisualShapeDataArguments.m_textureUniqueId); + + if (clientCmd.m_updateVisualShapeDataArguments.m_textureUniqueId>=0) + { + if (texHandle) + { + m_data->m_visualConverter.activateShapeTexture(clientCmd.m_updateVisualShapeDataArguments.m_bodyUniqueId, clientCmd.m_updateVisualShapeDataArguments.m_jointIndex, clientCmd.m_updateVisualShapeDataArguments.m_shapeIndex, texHandle->m_tinyRendererTextureId); + } + } + } + + { + int bodyUniqueId = clientCmd.m_updateVisualShapeDataArguments.m_bodyUniqueId; + int linkIndex = clientCmd.m_updateVisualShapeDataArguments.m_jointIndex; + + InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(bodyUniqueId); + if (bodyHandle) + { + if (bodyHandle->m_multiBody) + { + if (linkIndex==-1) + { + if (bodyHandle->m_multiBody->getBaseCollider()) + { + int graphicsIndex = bodyHandle->m_multiBody->getBaseCollider()->getUserIndex(); + if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_TEXTURE) + { + if (texHandle) + { + int shapeIndex = m_data->m_guiHelper->getShapeIndexFromInstance(graphicsIndex); + m_data->m_guiHelper->replaceTexture(shapeIndex,texHandle->m_openglTextureId); + } + } + if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_RGBA_COLOR) + { + m_data->m_visualConverter.changeRGBAColor(bodyUniqueId,linkIndex,clientCmd.m_updateVisualShapeDataArguments.m_rgbaColor); + m_data->m_guiHelper->changeRGBAColor(graphicsIndex,clientCmd.m_updateVisualShapeDataArguments.m_rgbaColor); + } + if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_SPECULAR_COLOR) + { + m_data->m_guiHelper->changeSpecularColor(graphicsIndex,clientCmd.m_updateVisualShapeDataArguments.m_specularColor); + } + + } + } else + { + if (linkIndexm_multiBody->getNumLinks()) + { + if (bodyHandle->m_multiBody->getLink(linkIndex).m_collider) + { + int graphicsIndex = bodyHandle->m_multiBody->getLink(linkIndex).m_collider->getUserIndex(); + if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_TEXTURE) + { + if (texHandle) + { + int shapeIndex = m_data->m_guiHelper->getShapeIndexFromInstance(graphicsIndex); + m_data->m_guiHelper->replaceTexture(shapeIndex,texHandle->m_openglTextureId); + } + } + if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_RGBA_COLOR) + { + m_data->m_visualConverter.changeRGBAColor(bodyUniqueId,linkIndex,clientCmd.m_updateVisualShapeDataArguments.m_rgbaColor); + m_data->m_guiHelper->changeRGBAColor(graphicsIndex,clientCmd.m_updateVisualShapeDataArguments.m_rgbaColor); + } + if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_SPECULAR_COLOR) + { + m_data->m_guiHelper->changeSpecularColor(graphicsIndex,clientCmd.m_updateVisualShapeDataArguments.m_specularColor); + } + + } + } + } + } else + { + if (bodyHandle->m_rigidBody) + { + int graphicsIndex = bodyHandle->m_rigidBody->getUserIndex(); + if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_TEXTURE) + { + if (texHandle) + { + int shapeIndex = m_data->m_guiHelper->getShapeIndexFromInstance(graphicsIndex); + m_data->m_guiHelper->replaceTexture(shapeIndex,texHandle->m_openglTextureId); + } + } + if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_RGBA_COLOR) + { + m_data->m_visualConverter.changeRGBAColor(bodyUniqueId,linkIndex,clientCmd.m_updateVisualShapeDataArguments.m_rgbaColor); + m_data->m_guiHelper->changeRGBAColor(graphicsIndex,clientCmd.m_updateVisualShapeDataArguments.m_rgbaColor); + } + if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_SPECULAR_COLOR) + { + m_data->m_guiHelper->changeSpecularColor(graphicsIndex,clientCmd.m_updateVisualShapeDataArguments.m_specularColor); + } + } + } + } + } + + serverCmd.m_type = CMD_VISUAL_SHAPE_UPDATE_COMPLETED; + return hasStatus; +} + +bool PhysicsServerCommandProcessor::processChangeTextureCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + + SharedMemoryStatus& serverCmd = serverStatusOut; + serverCmd.m_type = CMD_CHANGE_TEXTURE_COMMAND_FAILED; + + InternalTextureHandle* texH = m_data->m_textureHandles.getHandle(clientCmd.m_changeTextureArgs.m_textureUniqueId); + if(texH) + { + int gltex = texH->m_openglTextureId; + m_data->m_guiHelper->changeTexture(gltex, + (const unsigned char*)bufferServerToClient, clientCmd.m_changeTextureArgs.m_width,clientCmd.m_changeTextureArgs.m_height); + + serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED; + } + return hasStatus; +} + +bool PhysicsServerCommandProcessor::processLoadTextureCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + + BT_PROFILE("CMD_LOAD_TEXTURE"); + SharedMemoryStatus& serverCmd = serverStatusOut; + serverCmd.m_type = CMD_LOAD_TEXTURE_FAILED; + + char relativeFileName[1024]; + char pathPrefix[1024]; + + if(b3ResourcePath::findResourcePath(clientCmd.m_loadTextureArguments.m_textureFileName,relativeFileName,1024)) + { + b3FileUtils::extractPath(relativeFileName,pathPrefix,1024); + + int texHandle = m_data->m_textureHandles.allocHandle(); + InternalTextureHandle* texH = m_data->m_textureHandles.getHandle(texHandle); + if(texH) + { + texH->m_tinyRendererTextureId = -1; + texH->m_openglTextureId = -1; + + int uid = m_data->m_visualConverter.loadTextureFile(relativeFileName); + if(uid>=0) + { + texH->m_tinyRendererTextureId = uid; + } + + { + int width,height,n; + unsigned char* imageData= stbi_load(relativeFileName,&width,&height,&n,3); + + if(imageData) + { + texH->m_openglTextureId = m_data->m_guiHelper->registerTexture(imageData,width,height); + free(imageData); + } + else + { + b3Warning("Unsupported texture image format [%s]\n",relativeFileName); + } + } + serverCmd.m_loadTextureResultArguments.m_textureUniqueId = texHandle; + serverCmd.m_type = CMD_LOAD_TEXTURE_COMPLETED; + } + } + else + { + serverCmd.m_type = CMD_LOAD_TEXTURE_FAILED; + } + return hasStatus; +} + +bool PhysicsServerCommandProcessor::processLoadBulletCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + BT_PROFILE("CMD_LOAD_BULLET"); + + bool hasStatus = true; + SharedMemoryStatus& serverCmd = serverStatusOut; + serverCmd.m_type = CMD_BULLET_LOADING_FAILED; + + btBulletWorldImporter* importer = new btBulletWorldImporter(m_data->m_dynamicsWorld); + + const char* prefix[] = { "", "./", "./data/", "../data/", "../../data/", "../../../data/", "../../../../data/" }; + int numPrefixes = sizeof(prefix) / sizeof(const char*); + char relativeFileName[1024]; + FILE* f = 0; + bool found = false; + + for (int i = 0; !f && iloadFile(relativeFileName); + if (ok) + { + + + int numRb = importer->getNumRigidBodies(); + serverStatusOut.m_sdfLoadedArgs.m_numBodies = 0; + serverStatusOut.m_sdfLoadedArgs.m_numUserConstraints = 0; + + for( int i=0;igetRigidBodyByIndex(i); + if (colObj) + { + btRigidBody* rb = btRigidBody::upcast(colObj); + if (rb) + { + int bodyUniqueId = m_data->m_bodyHandles.allocHandle(); + InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(bodyUniqueId); + colObj->setUserIndex2(bodyUniqueId); + bodyHandle->m_rigidBody = rb; + + if (serverStatusOut.m_sdfLoadedArgs.m_numBodiesm_guiHelper->autogenerateGraphicsObjects(m_data->m_dynamicsWorld); + } + } + return hasStatus; +} + +bool PhysicsServerCommandProcessor::processLoadMJCFCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + + BT_PROFILE("CMD_LOAD_MJCF"); + SharedMemoryStatus& serverCmd = serverStatusOut; + serverCmd.m_type = CMD_MJCF_LOADING_FAILED; + const MjcfArgs& mjcfArgs = clientCmd.m_mjcfArguments; + if (m_data->m_verboseOutput) + { + b3Printf("Processed CMD_LOAD_MJCF:%s", mjcfArgs.m_mjcfFileName); + } + bool useMultiBody=(clientCmd.m_updateFlags & URDF_ARGS_USE_MULTIBODY) ? (mjcfArgs.m_useMultiBody!=0) : true; + int flags = CUF_USE_MJCF; + if (clientCmd.m_updateFlags&URDF_ARGS_HAS_CUSTOM_URDF_FLAGS) + { + flags |= clientCmd.m_mjcfArguments.m_flags; + } + + bool completedOk = loadMjcf(mjcfArgs.m_mjcfFileName,bufferServerToClient, bufferSizeInBytes, useMultiBody, flags); + if (completedOk) + { + m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld); + + serverStatusOut.m_sdfLoadedArgs.m_numBodies = m_data->m_sdfRecentLoadedBodies.size(); + serverStatusOut.m_sdfLoadedArgs.m_numUserConstraints = 0; + int maxBodies = btMin(MAX_SDF_BODIES, serverStatusOut.m_sdfLoadedArgs.m_numBodies); + for (int i=0;im_sdfRecentLoadedBodies[i]; + } + + serverStatusOut.m_type = CMD_MJCF_LOADING_COMPLETED; + } else + { + serverStatusOut.m_type = CMD_MJCF_LOADING_FAILED; + } + return hasStatus; + +} + + +bool PhysicsServerCommandProcessor::processSaveBulletCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + + BT_PROFILE("CMD_SAVE_BULLET"); + SharedMemoryStatus& serverCmd = serverStatusOut; + + FILE* f = fopen(clientCmd.m_fileArguments.m_fileName, "wb"); + if (f) + { + btDefaultSerializer* ser = new btDefaultSerializer(); + m_data->m_dynamicsWorld->serialize(ser); + fwrite(ser->getBufferPointer(), ser->getCurrentBufferSize(), 1, f); + fclose(f); + serverCmd.m_type = CMD_BULLET_SAVING_COMPLETED; + delete ser; + } + serverCmd.m_type = CMD_BULLET_SAVING_FAILED; + return hasStatus; +} + +bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes ) +{ + // BT_PROFILE("processCommand"); + + int sz = sizeof(SharedMemoryStatus); + int sz2 = sizeof(SharedMemoryCommand); + + bool hasStatus = false; + + if (m_data->m_commandLogger) + { + m_data->m_commandLogger->logCommand(clientCmd); + } + serverStatusOut.m_type = CMD_INVALID_STATUS; + serverStatusOut.m_numDataStreamBytes = 0; + serverStatusOut.m_dataStream = 0; + + //consume the command + switch (clientCmd.m_type) + { + + case CMD_STATE_LOGGING: + { + hasStatus = processStateLoggingCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_SET_VR_CAMERA_STATE: + { + hasStatus = processSetVRCameraStateCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_REQUEST_VR_EVENTS_DATA: + { + hasStatus = processRequestVREventsCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + }; + case CMD_REQUEST_MOUSE_EVENTS_DATA: + { + hasStatus = processRequestMouseEventsCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + }; + case CMD_REQUEST_KEYBOARD_EVENTS_DATA: + { + hasStatus = processRequestKeyboardEventsCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + }; + + case CMD_REQUEST_RAY_CAST_INTERSECTIONS: + { + + hasStatus = processRequestRaycastIntersectionsCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + }; + case CMD_REQUEST_DEBUG_LINES: + { + hasStatus = processRequestDebugLinesCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + + case CMD_REQUEST_CAMERA_IMAGE_DATA: + { + hasStatus = processRequestCameraImageCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_SYNC_BODY_INFO: + { + hasStatus = processSyncBodyInfoCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_REQUEST_BODY_INFO: + { + hasStatus = processRequestBodyInfoCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_SAVE_WORLD: + { + hasStatus = processSaveWorldCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_LOAD_SDF: + { + hasStatus = processLoadSDFCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_CREATE_COLLISION_SHAPE: + { + hasStatus = processCreateCollisionShapeCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_CREATE_VISUAL_SHAPE: + { + hasStatus = processCreateVisualShapeCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_CREATE_MULTI_BODY: + { + hasStatus = processCreateMultiBodyCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_SET_ADDITIONAL_SEARCH_PATH: + { + hasStatus = processSetAdditionalSearchPathCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_LOAD_URDF: + { + hasStatus = processLoadURDFCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_LOAD_BUNNY: + { + hasStatus = processLoadBunnyCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_CREATE_SENSOR: + { + hasStatus = processCreateSensorCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_PROFILE_TIMING: + { + hasStatus = processProfileTimingCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + + case CMD_SEND_DESIRED_STATE: + { + hasStatus = processSendDesiredStateCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_REQUEST_COLLISION_INFO: + { + hasStatus = processRequestCollisionInfoCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + + case CMD_REQUEST_ACTUAL_STATE: + { + hasStatus = processRequestActualStateCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_STEP_FORWARD_SIMULATION: + { + hasStatus = processForwardDynamicsCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + + case CMD_REQUEST_INTERNAL_DATA: + { + hasStatus = processRequestInternalDataCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + }; + case CMD_CHANGE_DYNAMICS_INFO: + { + hasStatus = processChangeDynamicsInfoCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + }; + case CMD_GET_DYNAMICS_INFO: + { + hasStatus = processGetDynamicsInfoCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + + case CMD_REQUEST_PHYSICS_SIMULATION_PARAMETERS: + { + hasStatus = processRequestPhysicsSimulationParametersCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + + case CMD_SEND_PHYSICS_SIMULATION_PARAMETERS: + { + hasStatus = processSendPhysicsParametersCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + + }; + case CMD_INIT_POSE: + { + hasStatus = processInitPoseCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_RESET_SIMULATION: + { + hasStatus = processResetSimulationCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_CREATE_RIGID_BODY: + { + hasStatus = processCreateRigidBodyCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_CREATE_BOX_COLLISION_SHAPE: + { + //for backward compatibility, CMD_CREATE_BOX_COLLISION_SHAPE is the same as CMD_CREATE_RIGID_BODY + hasStatus = processCreateRigidBodyCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_PICK_BODY: + { + hasStatus = processPickBodyCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_MOVE_PICKED_BODY: + { + hasStatus = processMovePickedBodyCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_REMOVE_PICKING_CONSTRAINT_BODY: + { + hasStatus = processRemovePickingConstraintCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_REQUEST_AABB_OVERLAP: + { + hasStatus = processRequestAabbOverlapCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_REQUEST_OPENGL_VISUALIZER_CAMERA: + { + hasStatus = processRequestOpenGLVisualizeCameraCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_CONFIGURE_OPENGL_VISUALIZER: + { + hasStatus = processConfigureOpenGLVisualizerCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_REQUEST_CONTACT_POINT_INFORMATION: + { + hasStatus = processRequestContactpointInformationCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_CALCULATE_INVERSE_DYNAMICS: + { + hasStatus = processInverseDynamicsCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_CALCULATE_JACOBIAN: + { + hasStatus = processCalculateJacobianCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_CALCULATE_MASS_MATRIX: + { + hasStatus = processCalculateMassMatrixCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_APPLY_EXTERNAL_FORCE: + { + hasStatus = processApplyExternalForceCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_REMOVE_BODY: + { + hasStatus = processRemoveBodyCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_USER_CONSTRAINT: + { + hasStatus = processCreateUserConstraintCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_CALCULATE_INVERSE_KINEMATICS: + { + hasStatus = processCalculateInverseKinematicsCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_REQUEST_VISUAL_SHAPE_INFO: + { + hasStatus = processRequestVisualShapeInfoCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_UPDATE_VISUAL_SHAPE: + { + hasStatus = processUpdateVisualShapeCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_CHANGE_TEXTURE: + { + hasStatus = processChangeTextureCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_LOAD_TEXTURE: + { + hasStatus = processLoadTextureCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_LOAD_BULLET: + { + hasStatus = processLoadBulletCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_SAVE_BULLET: + { + hasStatus = processSaveBulletCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_LOAD_MJCF: + { + hasStatus = processLoadMJCFCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_USER_DEBUG_DRAW: + { + hasStatus = processUserDebugDrawCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_CUSTOM_COMMAND: + { + hasStatus = processCustomCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + default: + { + BT_PROFILE("CMD_UNKNOWN"); + b3Error("Unknown command encountered"); + SharedMemoryStatus& serverCmd =serverStatusOut; + serverCmd.m_type = CMD_UNKNOWN_COMMAND_FLUSHED; + hasStatus = true; + } + }; + + return hasStatus; +} void PhysicsServerCommandProcessor::syncPhysicsToGraphics() { m_data->m_guiHelper->syncPhysicsToGraphics(m_data->m_dynamicsWorld); } - void PhysicsServerCommandProcessor::renderScene(int renderFlags) { if (m_data->m_guiHelper) diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.h b/examples/SharedMemory/PhysicsServerCommandProcessor.h index d46203419..6177d78fe 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.h +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.h @@ -25,6 +25,61 @@ class PhysicsServerCommandProcessor : public CommandProcessorInterface protected: + bool processStateLoggingCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processRequestCameraImageCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processSaveWorldCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processCreateCollisionShapeCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processCreateVisualShapeCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processCustomCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processUserDebugDrawCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processSetVRCameraStateCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processRequestVREventsCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processRequestMouseEventsCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processRequestKeyboardEventsCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processRequestRaycastIntersectionsCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processRequestDebugLinesCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processSyncBodyInfoCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processSendDesiredStateCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processRequestActualStateCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processRequestContactpointInformationCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processRequestBodyInfoCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processLoadSDFCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processCreateMultiBodyCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processLoadURDFCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processLoadBunnyCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processCreateSensorCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processProfileTimingCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processRequestCollisionInfoCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processForwardDynamicsCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processRequestInternalDataCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processChangeDynamicsInfoCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processSetAdditionalSearchPathCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processGetDynamicsInfoCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processRequestPhysicsSimulationParametersCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processSendPhysicsParametersCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processInitPoseCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processResetSimulationCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processCreateRigidBodyCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processPickBodyCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processMovePickedBodyCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processRemovePickingConstraintCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processRequestAabbOverlapCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processRequestOpenGLVisualizeCameraCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processConfigureOpenGLVisualizerCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processInverseDynamicsCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processCalculateJacobianCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processCalculateMassMatrixCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processApplyExternalForceCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processRemoveBodyCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processCreateUserConstraintCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processCalculateInverseKinematicsCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processRequestVisualShapeInfoCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processUpdateVisualShapeCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processChangeTextureCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processLoadTextureCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processLoadBulletCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processSaveBulletCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processLoadMJCFCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); bool loadSdf(const char* fileName, char* bufferServerToClient, int bufferSizeInBytes, bool useMultiBody, int flags, btScalar globalScaling);