#ifdef BT_ENABLE_PHYSX #include "PhysXServerCommandProcessor.h" #include "../../Utils/ChromeTraceUtil.h" #include #include "../SharedMemoryCommands.h" #include "LinearMath/btQuickprof.h" #include "Bullet3Common/b3AlignedObjectArray.h" #include "LinearMath/btMinMax.h" #include "Bullet3Common/b3FileUtils.h" #include "Bullet3Common/b3CommandLineArgs.h" #include "../../Utils/b3ResourcePath.h" #include "Bullet3Common/b3ResizablePool.h" #include "PxPhysicsAPI.h" #include "../Utils/b3BulletDefaultFileIO.h" #include "PhysXUrdfImporter.h" #include "PxTolerancesScale.h" #include "PxDefaultCpuDispatcher.h" #include "PxDefaultSimulationFilterShader.h" #include "URDF2PhysX.h" #include "../b3PluginManager.h" #include "PxRigidActorExt.h" #include "LinearMath/btThreads.h" #define STATIC_EGLRENDERER_PLUGIN #ifdef STATIC_EGLRENDERER_PLUGIN #include "../plugins/eglPlugin/eglRendererPlugin.h" #endif //STATIC_EGLRENDERER_PLUGIN //for serialization of data to client #include "BulletDynamics/Featherstone/btMultiBody.h" #include "../Extras/Serialize/BulletFileLoader/btBulletFile.h" #include "LinearMath/btSerializer.h" #include "PhysXUserData.h" class MyPhysXErrorCallback : public physx::PxErrorCallback { public: MyPhysXErrorCallback() { } ~MyPhysXErrorCallback() { } virtual void reportError(physx::PxErrorCode::Enum code, const char* message, const char* file, int line) { b3Printf("%s in file:%s line:%d\n", message, file, line); } }; class CustomProfilerCallback : public physx::PxProfilerCallback { public: virtual ~CustomProfilerCallback() {} virtual void* zoneStart(const char* eventName, bool detached, uint64_t contextId) { b3EnterProfileZone(eventName); return 0; } virtual void zoneEnd(void* profilerData, const char* eventName, bool detached, uint64_t contextId) { b3LeaveProfileZone(); } }; static CustomProfilerCallback gCustomProfilerCallback; struct InternalPhysXBodyData { physx::PxArticulationReducedCoordinate* mArticulation; physx::PxRigidDynamic* m_rigidDynamic; physx::PxRigidStatic* m_rigidStatic; std::string m_bodyName; InternalPhysXBodyData() { clear(); } //physx::PxArticulationJointReducedCoordinate* gDriveJoint; void clear() { mArticulation = 0; m_rigidDynamic = 0; m_rigidStatic = 0; m_bodyName = ""; } }; typedef b3PoolBodyHandle InternalPhysXBodyHandle; struct PhysXServerCommandProcessorInternalData : public physx::PxSimulationEventCallback, public physx::PxContactModifyCallback { bool m_isConnected; bool m_verboseOutput; double m_physicsDeltaTime; int m_numSimulationSubSteps; btSpinMutex m_taskLock; btAlignedObjectArray m_contactPoints; void onContactModify(physx::PxContactModifyPair* const pairs, physx::PxU32 count) { for (physx::PxU32 i = 0; i contacts; for (physx::PxU32 i = 0; i < nbPairs; i++) { physx::PxU32 contactCount = pairs[i].contactCount; if (contactCount) { contacts.resize(contactCount); pairs[i].extractContacts(&contacts[0], contactCount); for (physx::PxU32 j = 0; j < contactCount; j++) { const physx::PxContactPairPoint& contact = contacts[i]; b3ContactPointData srcPt; MyPhysXUserData* udA = (MyPhysXUserData*)pairHeader.actors[0]->userData; MyPhysXUserData* udB = (MyPhysXUserData*)pairHeader.actors[1]->userData; srcPt.m_bodyUniqueIdA = udA->m_bodyUniqueId; srcPt.m_linkIndexA = udA->m_linkIndex; srcPt.m_bodyUniqueIdB = udB->m_bodyUniqueId; srcPt.m_linkIndexB = udB->m_linkIndex; srcPt.m_positionOnAInWS[0] = contact.position.x + contact.separation*contact.normal.x; srcPt.m_positionOnAInWS[1] = contact.position.y + contact.separation*contact.normal.y; srcPt.m_positionOnAInWS[2] = contact.position.z + contact.separation*contact.normal.z; srcPt.m_positionOnBInWS[0] = contact.position.x - contact.separation*contact.normal.x; srcPt.m_positionOnBInWS[1] = contact.position.y - contact.separation*contact.normal.y; srcPt.m_positionOnBInWS[2] = contact.position.z - contact.separation*contact.normal.z; srcPt.m_contactNormalOnBInWS[0] = contact.normal.x; srcPt.m_contactNormalOnBInWS[1] = contact.normal.y; srcPt.m_contactNormalOnBInWS[2] = contact.normal.z; srcPt.m_contactDistance = contact.separation; srcPt.m_contactFlags = 0; srcPt.m_linearFrictionDirection1[0] = 0; srcPt.m_linearFrictionDirection1[1] = 0; srcPt.m_linearFrictionDirection1[2] = 0; srcPt.m_linearFrictionDirection2[0] = 0; srcPt.m_linearFrictionDirection2[1] = 0; srcPt.m_linearFrictionDirection2[2] = 0; srcPt.m_linearFrictionForce2 = 0; srcPt.m_normalForce = contact.impulse.dot(contact.normal); //compute friction direction from impulse projected in contact plane using contact normal. physx::PxVec3 fric = contact.impulse - contact.normal*srcPt.m_normalForce; double fricForce = fric.normalizeSafe(); if (fricForce) { srcPt.m_linearFrictionDirection1[0] = fric.x; srcPt.m_linearFrictionDirection1[1] = fric.y; srcPt.m_linearFrictionDirection1[2] = fric.z; srcPt.m_linearFrictionForce1 = fricForce; } m_contactPoints.push_back(srcPt); // std::cout << "Contact: bw " << pairHeader.actors[0]->getName() << " and " << pairHeader.actors[1]->getName() << " | " << contacts[j].position.x << "," << contacts[j].position.y << "," // << contacts[j].position.z << std::endl; } } } m_taskLock.unlock(); } void onConstraintBreak(physx::PxConstraintInfo* constraints, physx::PxU32 count) { PX_UNUSED(constraints); PX_UNUSED(count); } void onWake(physx::PxActor** actors, physx::PxU32 count) { PX_UNUSED(actors); PX_UNUSED(count); } void onSleep(physx::PxActor** actors, physx::PxU32 count) { PX_UNUSED(actors); PX_UNUSED(count); } void onTrigger(physx::PxTriggerPair* pairs, physx::PxU32 count) { PX_UNUSED(pairs); PX_UNUSED(count); } void onAdvance(const physx::PxRigidBody* const*, const physx::PxTransform*, const physx::PxU32) { } b3PluginManager m_pluginManager; physx::PxDefaultAllocator m_allocator; MyPhysXErrorCallback m_errorCallback; physx::PxFoundation* m_foundation; physx::PxPhysics* m_physics; physx::PxCooking* m_cooking; physx::PxDefaultCpuDispatcher* m_dispatcher; physx::PxScene* m_scene; physx::PxMaterial* m_material; //physx::PxPvd* m_pvd; b3ResizablePool m_bodyHandles; b3AlignedObjectArray m_mjcfRecentLoadedBodies; int m_profileTimingLoggingUid; int m_stateLoggersUniqueId; std::string m_profileTimingFileName; b3CommandLineArgs m_commandLineArgs; int m_userDebugParametersUid; btHashMap m_userDebugParameters; btAlignedObjectArray m_graphicsIndexToSegmentationMask; PhysXServerCommandProcessorInternalData(PhysXServerCommandProcessor* sdk, int argc, char* argv[]) : m_isConnected(false), m_verboseOutput(false), m_physicsDeltaTime(1. / 240.), m_numSimulationSubSteps(0), m_pluginManager(sdk), m_profileTimingLoggingUid(-1), m_stateLoggersUniqueId(1), m_commandLineArgs(argc,argv), m_userDebugParametersUid(0) { m_foundation = NULL; m_physics = NULL; m_cooking = NULL; m_dispatcher = NULL; m_scene = NULL; m_material = NULL; //m_pvd = NULL; #ifdef STATIC_EGLRENDERER_PLUGIN { bool initPlugin = false; b3PluginFunctions funcs(initPlugin_eglRendererPlugin, exitPlugin_eglRendererPlugin, executePluginCommand_eglRendererPlugin); funcs.m_getRendererFunc = getRenderInterface_eglRendererPlugin; int renderPluginId = m_pluginManager.registerStaticLinkedPlugin("eglRendererPlugin", funcs, initPlugin); m_pluginManager.selectPluginRenderer(renderPluginId); } #endif //STATIC_EGLRENDERER_PLUGIN } }; PhysXServerCommandProcessor::PhysXServerCommandProcessor(int argc, char* argv[]) { m_data = new PhysXServerCommandProcessorInternalData(this, argc, argv); } PhysXServerCommandProcessor::~PhysXServerCommandProcessor() { delete m_data; } physx::PxFilterFlags MyPhysXFilter(physx::PxFilterObjectAttributes attributes0, physx::PxFilterData filterData0, physx::PxFilterObjectAttributes attributes1, physx::PxFilterData filterData1, physx::PxPairFlags& pairFlags, const void* constantBlock, physx::PxU32 constantBlockSize) { PX_UNUSED(attributes0); PX_UNUSED(attributes1); PX_UNUSED(constantBlock); PX_UNUSED(constantBlockSize); // if (filterData0.word2 != 0 && filterData0.word2 == filterData1.word2) // return physx::PxFilterFlag::eKILL; pairFlags |= physx::PxPairFlag::eCONTACT_DEFAULT | physx::PxPairFlag::eNOTIFY_TOUCH_FOUND | physx::PxPairFlag::eDETECT_DISCRETE_CONTACT | physx::PxPairFlag::eNOTIFY_CONTACT_POINTS | physx::PxPairFlag::eMODIFY_CONTACTS; return physx::PxFilterFlag::eDEFAULT; } bool PhysXServerCommandProcessor::connect() { if (m_data->m_isConnected) { printf("already connected\n"); return true; } int result = 0; { m_data->m_foundation = PxCreateFoundation(PX_PHYSICS_VERSION, m_data->m_allocator, m_data->m_errorCallback); // This call should be performed after PVD is initialized, otherwise it will have no effect. PxSetProfilerCallback(&gCustomProfilerCallback); m_data->m_physics = PxCreatePhysics(PX_PHYSICS_VERSION, *m_data->m_foundation, physx::PxTolerancesScale(), true, 0); m_data->m_cooking = PxCreateCooking(PX_PHYSICS_VERSION, *m_data->m_foundation, physx::PxCookingParams(physx::PxTolerancesScale())); physx::PxU32 numCores = 1; m_data->m_commandLineArgs.GetCmdLineArgument("numCores", numCores); printf("PhysX numCores=%d\n", numCores); m_data->m_dispatcher = physx::PxDefaultCpuDispatcherCreate(numCores == 0 ? 0 : numCores - 1); physx::PxSceneDesc sceneDesc(m_data->m_physics->getTolerancesScale()); sceneDesc.gravity = physx::PxVec3(0.0f, -9.81f, 0.0f); sceneDesc.solverType = physx::PxSolverType::ePGS; std::string solver; m_data->m_commandLineArgs.GetCmdLineArgument("solver", solver); if (solver=="tgs") { sceneDesc.solverType = physx::PxSolverType::eTGS; printf("PhysX using TGS\n"); } else { printf("PhysX using PGS\n"); } sceneDesc.cpuDispatcher = m_data->m_dispatcher; //todo: add some boolean, to allow enable/disable of this contact filtering bool enableContactCallback = false; m_data->m_commandLineArgs.GetCmdLineArgument("enableContactCallback", enableContactCallback); if (enableContactCallback) { sceneDesc.filterShader = MyPhysXFilter; sceneDesc.simulationEventCallback = this->m_data; sceneDesc.contactModifyCallback = this->m_data; printf("PhysX enableContactCallback\n"); } else { sceneDesc.filterShader = physx::PxDefaultSimulationFilterShader; } m_data->m_scene = m_data->m_physics->createScene(sceneDesc); m_data->m_material = m_data->m_physics->createMaterial(0.5f, 0.5f, 0.f); PxInitExtensions(*m_data->m_physics, 0); //PxRigidStatic* groundPlane = PxCreatePlane(*gPhysics, PxPlane(0, 1, 0, 0), *gMaterial); //gScene->addActor(*groundPlane); result = 1; } if (result == 1) { m_data->m_isConnected = true; return true; } return false; } void PhysXServerCommandProcessor::resetSimulation() { //gArticulation->release(); m_data->m_scene->release(); m_data->m_dispatcher->release(); m_data->m_cooking->release(); m_data->m_physics->release(); //PxPvdTransport* transport = gPvd->getTransport(); //gPvd->release(); //transport->release(); PxCloseExtensions(); m_data->m_foundation->release(); } void PhysXServerCommandProcessor::disconnect() { resetSimulation(); m_data->m_isConnected = false; } bool PhysXServerCommandProcessor::isConnected() const { return m_data->m_isConnected; } bool PhysXServerCommandProcessor::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 PhysXServerCommandProcessor::processStateLoggingCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) { BT_PROFILE("CMD_STATE_LOGGING"); serverStatusOut.m_type = CMD_STATE_LOGGING_FAILED; bool hasStatus = true; 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_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; } } return hasStatus; } bool PhysXServerCommandProcessor::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; InternalPhysXBodyData* 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->mArticulation) { physx::PxArticulationCache* c = body->mArticulation->createCache(); body->mArticulation->copyInternalStateToCache(*c, physx::PxArticulationCache::ePOSITION | physx::PxArticulationCache::eVELOCITY);// physx::PxArticulationCache::eALL); physx::PxArticulationLink* physxLinks[64]; physx::PxU32 bufferSize = 64; physx::PxU32 startIndex = 0; int numLinks2 = body->mArticulation->getLinks(physxLinks, bufferSize, startIndex); btAlignedObjectArray dofStarts; dofStarts.resize(numLinks2); dofStarts[0] = 0; //We know that the root link does not have a joint //cache positions in PhysX may be reshuffled, see //http://gameworksdocs.nvidia.com/PhysX/4.0/documentation/PhysXGuide/Manual/Articulations.html for (int i = 1; i < numLinks2; ++i) { int llIndex = physxLinks[i]->getLinkIndex(); int dofs = physxLinks[i]->getInboundJointDof(); dofStarts[llIndex] = dofs; } int count = 0; for (int i = 1; i < numLinks2; ++i) { int dofs = dofStarts[i]; dofStarts[i] = count; count += dofs; } if (numLinks2 > 0) { int dofs = physxLinks[0]->getInboundJointDof(); physx::PxTransform pt = physxLinks[0]->getGlobalPose(); physx::PxVec3 linVel = physxLinks[0]->getLinearVelocity(); physx::PxVec3 angVel = physxLinks[0]->getAngularVelocity(); if (clientCmd.m_updateFlags & INIT_POSE_HAS_BASE_LINEAR_VELOCITY) { physxLinks[0]->setLinearVelocity(physx::PxVec3(baseLinVel[0], baseLinVel[1], baseLinVel[2])); } if (clientCmd.m_updateFlags & INIT_POSE_HAS_BASE_ANGULAR_VELOCITY) { physxLinks[0]->setAngularVelocity(physx::PxVec3(baseAngVel[0], baseAngVel[1], baseAngVel[2])); } if (clientCmd.m_updateFlags & INIT_POSE_HAS_INITIAL_POSITION) { pt.p.x = basePos[0]; pt.p.y = basePos[0]; pt.p.z = basePos[0]; physxLinks[0]->setGlobalPose(pt); } 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]); pt.q.x = baseOrn[0]; pt.q.y = baseOrn[1]; pt.q.z = baseOrn[2]; pt.q.w = baseOrn[3]; physxLinks[0]->setGlobalPose(pt); } } if (clientCmd.m_updateFlags & INIT_POSE_HAS_JOINT_STATE) { int uDofIndex = 6; int posVarCountIndex = 7; //skip 'root' link for (int i = 1; i < numLinks2; i++) { int physxCacheLinkIndex = physxLinks[i]->getLinkIndex(); int dofs = physxLinks[i]->getInboundJointDof(); int posVarCount = dofs;//?? bool hasPosVar = posVarCount > 0; for (int j = 0; j < posVarCount; j++) { if (clientCmd.m_initPoseArgs.m_hasInitialStateQ[posVarCountIndex + j] == 0) { hasPosVar = false; break; } } if (hasPosVar) { if (posVarCount == 1) { c->jointPosition[dofStarts[physxCacheLinkIndex]] = clientCmd.m_initPoseArgs.m_initialStateQ[posVarCountIndex]; } if (posVarCount == 3) { btQuaternion q( clientCmd.m_initPoseArgs.m_initialStateQ[posVarCountIndex], clientCmd.m_initPoseArgs.m_initialStateQ[posVarCountIndex + 1], clientCmd.m_initPoseArgs.m_initialStateQ[posVarCountIndex + 2], clientCmd.m_initPoseArgs.m_initialStateQ[posVarCountIndex + 3]); q.normalize(); //mb->setJointPosMultiDof(i, &q[0]); //double vel[6] = { 0, 0, 0, 0, 0, 0 }; //mb->setJointVelMultiDof(i, vel); } } bool hasVel = true; for (int j = 0; j < posVarCount; j++) { if (clientCmd.m_initPoseArgs.m_hasInitialStateQdot[uDofIndex + j] == 0) { hasVel = false; break; } } if (hasVel) { if (posVarCount == 1) { btScalar vel = clientCmd.m_initPoseArgs.m_initialStateQdot[uDofIndex]; c->jointVelocity[dofStarts[physxCacheLinkIndex]] = vel; } if (posVarCount == 3) { //mb->setJointVelMultiDof(i, &clientCmd.m_initPoseArgs.m_initialStateQdot[uDofIndex]); } } posVarCountIndex += dofs; uDofIndex += dofs;// mb->getLink(i).m_dofCount; } } body->mArticulation->applyCache(*c, physx::PxArticulationCache::ePOSITION| physx::PxArticulationCache::eVELOCITY); body->mArticulation->releaseCache(*c); } SharedMemoryStatus& serverCmd = serverStatusOut; serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED; return hasStatus; } bool PhysXServerCommandProcessor::processSendDesiredStateCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) { bool hasStatus = true; BT_PROFILE("CMD_SEND_DESIRED_STATE"); int bodyUniqueId = clientCmd.m_sendDesiredStateCommandArgument.m_bodyUniqueId; InternalPhysXBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); if (body && body->mArticulation) { physx::PxArticulationLink* physxLinks[64]; physx::PxU32 bufferSize = 64; physx::PxU32 startIndex = 0; int numLinks2 = body->mArticulation->getLinks(physxLinks, bufferSize, startIndex); //http://gameworksdocs.nvidia.com/PhysX/4.0/documentation/PhysXGuide/Manual/Articulations.html switch (clientCmd.m_sendDesiredStateCommandArgument.m_controlMode) { 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 = 1; link < numLinks2; link++) { int dofs = physxLinks[link]->getInboundJointDof(); physx::PxReal stiffness = 10.f; physx::PxReal damping = 0.1f; physx::PxReal forceLimit = PX_MAX_F32; if (dofs == 1) { physx::PxArticulationJointReducedCoordinate* joint = static_cast(physxLinks[link]->getInboundJoint()); btScalar desiredVelocity = 0.f; bool hasDesiredVelocity = false; physx::PxReal stiffness = 10.f; 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]; } joint->setDriveVelocity(physx::PxArticulationAxis::eTWIST, desiredVelocity); physx::PxReal damping = kd; stiffness = 0; joint->setDriveTarget(physx::PxArticulationAxis::eTWIST, 0.f); physx::PxReal forceLimit = 1000000.f; if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] & SIM_DESIRED_STATE_HAS_MAX_FORCE) != 0) { forceLimit = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex]; } joint->setDrive(physx::PxArticulationAxis::eTWIST, stiffness, damping, forceLimit); } } dofIndex += dofs; } } break; } case CONTROL_MODE_POSITION_VELOCITY_PD: { 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 int posIndex = 7; //skip 3 positional and 4 orientation (quaternion) positional degrees of freedom of the base for (int link = 1; link < numLinks2; link++) { int dofs = physxLinks[link]->getInboundJointDof(); physx::PxReal stiffness = 10.f; physx::PxReal damping = 0.1f; physx::PxReal forceLimit = PX_MAX_F32; if (dofs == 1) { physx::PxArticulationJointReducedCoordinate* joint = static_cast(physxLinks[link]->getInboundJoint()); btScalar desiredVelocity = 0.f; bool hasDesiredVelocity = false; physx::PxReal stiffness = 10.f; btScalar kd = 0.1f; btScalar kp = 0.f; if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] & SIM_DESIRED_STATE_HAS_QDOT) != 0) { desiredVelocity = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[dofIndex]; } btScalar desiredPosition = 0.f; if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[posIndex] & SIM_DESIRED_STATE_HAS_Q) != 0) { desiredPosition = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[posIndex]; if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] & SIM_DESIRED_STATE_HAS_KD) != 0) { kd = clientCmd.m_sendDesiredStateCommandArgument.m_Kd[dofIndex]; } physx::PxReal damping = kd; if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] & SIM_DESIRED_STATE_HAS_KP) != 0) { kp = clientCmd.m_sendDesiredStateCommandArgument.m_Kp[dofIndex]; stiffness = kp; } joint->setDriveVelocity(physx::PxArticulationAxis::eTWIST, desiredVelocity); physx::PxReal forceLimit = 1000000.f; if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] & SIM_DESIRED_STATE_HAS_MAX_FORCE) != 0) { forceLimit = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex]; } bool isAcceleration = false; joint->setDriveTarget(physx::PxArticulationAxis::eTWIST, desiredPosition); joint->setDrive(physx::PxArticulationAxis::eTWIST, stiffness, damping, forceLimit, isAcceleration); } } dofIndex += dofs; posIndex += dofs; } } break; } default: { } } } serverStatusOut.m_type = CMD_DESIRED_STATE_RECEIVED_COMPLETED; return hasStatus; } bool PhysXServerCommandProcessor::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; btVector3 newLocalInertiaDiagonal(clientCmd.m_changeDynamicsInfoArgs.m_localInertiaDiagonal[0], clientCmd.m_changeDynamicsInfoArgs.m_localInertiaDiagonal[1], clientCmd.m_changeDynamicsInfoArgs.m_localInertiaDiagonal[2]); btAssert(bodyUniqueId >= 0); InternalPhysXBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); if (body->mArticulation) { physx::PxArticulationLink* physxLinks[64]; physx::PxU32 bufferSize = 64; physx::PxU32 startIndex = 0; int physxLinkIndex = linkIndex + 1; int numLinks2 = body->mArticulation->getLinks(physxLinks, bufferSize, startIndex); if (physxLinkIndex >= 0 && physxLinkIndex < numLinks2) { if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_MASS) { physx::PxArticulationLink* childLink = physxLinks[physxLinkIndex]; physx::PxRigidBodyExt::updateMassAndInertia(*childLink, mass); } } } if (body->m_rigidDynamic) { //body->m_rigidDynamic->setMass(mass); //also update inertia if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_MASS) { physx::PxRigidBodyExt::updateMassAndInertia(*body->m_rigidDynamic, mass); } } if (body->m_rigidStatic) { } #if 0 if (body && body->m_multiBody) { btMultiBody* mb = body->m_multiBody; if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ACTIVATION_STATE) { if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateWakeUp) { mb->wakeUp(); } if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateSleep) { mb->goToSleep(); } if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateEnableSleeping) { mb->setCanSleep(true); } if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateDisableSleeping) { mb->setCanSleep(false); } } 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); } //handle switch from static/fixedBase to dynamic and vise-versa if (mass > 0) { bool isDynamic = true; if (mb->hasFixedBase()) { int collisionFilterGroup = isDynamic ? int(btBroadphaseProxy::DefaultFilter) : int(btBroadphaseProxy::StaticFilter); int collisionFilterMask = isDynamic ? int(btBroadphaseProxy::AllFilter) : int(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter); m_data->m_dynamicsWorld->removeCollisionObject(mb->getBaseCollider()); int oldFlags = mb->getBaseCollider()->getCollisionFlags(); mb->getBaseCollider()->setCollisionFlags(oldFlags & ~btCollisionObject::CF_STATIC_OBJECT); mb->setFixedBase(false); m_data->m_dynamicsWorld->addCollisionObject(mb->getBaseCollider(), collisionFilterGroup, collisionFilterMask); } } else { if (!mb->hasFixedBase()) { bool isDynamic = false; int collisionFilterGroup = isDynamic ? int(btBroadphaseProxy::DefaultFilter) : int(btBroadphaseProxy::StaticFilter); int collisionFilterMask = isDynamic ? int(btBroadphaseProxy::AllFilter) : int(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter); int oldFlags = mb->getBaseCollider()->getCollisionFlags(); mb->getBaseCollider()->setCollisionFlags(oldFlags | btCollisionObject::CF_STATIC_OBJECT); m_data->m_dynamicsWorld->removeCollisionObject(mb->getBaseCollider()); mb->setFixedBase(true); m_data->m_dynamicsWorld->addCollisionObject(mb->getBaseCollider(), collisionFilterGroup, collisionFilterMask); } } } if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LOCAL_INERTIA_DIAGONAL) { mb->setBaseInertia(newLocalInertiaDiagonal); } } 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; } } if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LOCAL_INERTIA_DIAGONAL) { mb->getLink(linkIndex).m_inertiaLocal = newLocalInertiaDiagonal; } } } } else { if (body && body->m_rigidBody) { if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ACTIVATION_STATE) { if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateEnableSleeping) { body->m_rigidBody->forceActivationState(ACTIVE_TAG); } if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateDisableSleeping) { body->m_rigidBody->forceActivationState(DISABLE_DEACTIVATION); } if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateWakeUp) { body->m_rigidBody->forceActivationState(ACTIVE_TAG); } if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateSleep) { body->m_rigidBody->forceActivationState(ISLAND_SLEEPING); } } 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); } if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LOCAL_INERTIA_DIAGONAL) { btScalar orgMass = body->m_rigidBody->getInvMass(); if (orgMass > 0) { body->m_rigidBody->setMassProps(mass, newLocalInertiaDiagonal); } } if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_CONTACT_PROCESSING_THRESHOLD) { body->m_rigidBody->setContactProcessingThreshold(clientCmd.m_changeDynamicsInfoArgs.m_contactProcessingThreshold); } if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_CCD_SWEPT_SPHERE_RADIUS) { body->m_rigidBody->setCcdSweptSphereRadius(clientCmd.m_changeDynamicsInfoArgs.m_ccdSweptSphereRadius); //for a given sphere radius, use a motion threshold of half the radius, before the ccd algorithm is enabled body->m_rigidBody->setCcdMotionThreshold(clientCmd.m_changeDynamicsInfoArgs.m_ccdSweptSphereRadius / 2.); } } } b3Notification notification; notification.m_notificationType = LINK_DYNAMICS_CHANGED; notification.m_linkArgs.m_bodyUniqueId = bodyUniqueId; notification.m_linkArgs.m_linkIndex = linkIndex; m_data->m_pluginManager.addNotification(notification); #endif SharedMemoryStatus& serverCmd = serverStatusOut; serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED; return hasStatus; } bool PhysXServerCommandProcessor::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_allowedCcdPenetration = 0;// m_data->m_dynamicsWorld->getDispatchInfo().m_allowedCcdPenetration; serverCmd.m_simulationParameterResultArgs.m_collisionFilterMode = 0;// m_data->m_broadphaseCollisionFilterCallback->m_filterMode; serverCmd.m_simulationParameterResultArgs.m_deltaTime = m_data->m_physicsDeltaTime; serverCmd.m_simulationParameterResultArgs.m_contactBreakingThreshold = 0;// gContactBreakingThreshold; serverCmd.m_simulationParameterResultArgs.m_contactSlop = 0;// m_data->m_dynamicsWorld->getSolverInfo().m_linearSlop; serverCmd.m_simulationParameterResultArgs.m_enableSAT = 0;// m_data->m_dynamicsWorld->getDispatchInfo().m_enableSatConvex; serverCmd.m_simulationParameterResultArgs.m_defaultGlobalCFM = 0;// m_data->m_dynamicsWorld->getSolverInfo().m_globalCfm; serverCmd.m_simulationParameterResultArgs.m_defaultContactERP = 0;// m_data->m_dynamicsWorld->getSolverInfo().m_erp2; serverCmd.m_simulationParameterResultArgs.m_defaultNonContactERP = 0;// m_data->m_dynamicsWorld->getSolverInfo().m_erp; serverCmd.m_simulationParameterResultArgs.m_deterministicOverlappingPairs = 0;// m_data->m_dynamicsWorld->getDispatchInfo().m_deterministicOverlappingPairs; serverCmd.m_simulationParameterResultArgs.m_enableConeFriction = 0;// (m_data->m_dynamicsWorld->getSolverInfo().m_solverMode & SOLVER_DISABLE_IMPLICIT_CONE_FRICTION) ? 0 : 1; serverCmd.m_simulationParameterResultArgs.m_enableFileCaching = 0;// b3IsFileCachingEnabled(); serverCmd.m_simulationParameterResultArgs.m_frictionCFM = 0;// m_data->m_dynamicsWorld->getSolverInfo().m_frictionCFM; serverCmd.m_simulationParameterResultArgs.m_frictionERP = 0;// m_data->m_dynamicsWorld->getSolverInfo().m_frictionERP; physx::PxVec3 grav = m_data->m_scene->getGravity(); serverCmd.m_simulationParameterResultArgs.m_gravityAcceleration[0] = grav.x; serverCmd.m_simulationParameterResultArgs.m_gravityAcceleration[1] = grav.y; serverCmd.m_simulationParameterResultArgs.m_gravityAcceleration[2] = grav.z; serverCmd.m_simulationParameterResultArgs.m_internalSimFlags = 0; serverCmd.m_simulationParameterResultArgs.m_jointFeedbackMode = 0; serverCmd.m_simulationParameterResultArgs.m_numSimulationSubSteps = 0;// m_data->m_numSimulationSubSteps; serverCmd.m_simulationParameterResultArgs.m_numSolverIterations = 0;// m_data->m_dynamicsWorld->getSolverInfo().m_numIterations; serverCmd.m_simulationParameterResultArgs.m_restitutionVelocityThreshold = 0;// m_data->m_dynamicsWorld->getSolverInfo().m_restitutionVelocityThreshold; serverCmd.m_simulationParameterResultArgs.m_solverResidualThreshold = 0;// m_data->m_dynamicsWorld->getSolverInfo().m_leastSquaresResidualThreshold; serverCmd.m_simulationParameterResultArgs.m_splitImpulsePenetrationThreshold = 0;// m_data->m_dynamicsWorld->getSolverInfo().m_splitImpulsePenetrationThreshold; serverCmd.m_simulationParameterResultArgs.m_useRealTimeSimulation = 0;// m_data->m_useRealTimeSimulation; serverCmd.m_simulationParameterResultArgs.m_useSplitImpulse = 0;// m_data->m_dynamicsWorld->getSolverInfo().m_splitImpulse; return hasStatus; } bool PhysXServerCommandProcessor::processRequestContactpointInformationCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) { SharedMemoryStatus& serverCmd = serverStatusOut; int totalBytesPerContact = sizeof(b3ContactPointData); int contactPointStorage = bufferSizeInBytes / totalBytesPerContact - 1; b3ContactPointData* contactData = (b3ContactPointData*)bufferServerToClient; int startContactPointIndex = clientCmd.m_requestContactPointArguments.m_startingContactPointIndex; int numContactPointBatch = btMin(int(m_data->m_contactPoints.size()), contactPointStorage); int endContactPointIndex = startContactPointIndex + numContactPointBatch; serverCmd.m_sendContactPointArgs.m_numContactPointsCopied = 0; for (int i = startContactPointIndex; i < endContactPointIndex; i++) { const b3ContactPointData& srcPt = m_data->m_contactPoints[i]; b3ContactPointData& destPt = contactData[serverCmd.m_sendContactPointArgs.m_numContactPointsCopied]; destPt = srcPt; serverCmd.m_sendContactPointArgs.m_numContactPointsCopied++; } serverCmd.m_sendContactPointArgs.m_startingContactPointIndex = startContactPointIndex; serverCmd.m_sendContactPointArgs.m_numRemainingContactPoints = m_data->m_contactPoints.size() - startContactPointIndex - serverCmd.m_sendContactPointArgs.m_numContactPointsCopied; serverCmd.m_numDataStreamBytes = totalBytesPerContact * serverCmd.m_sendContactPointArgs.m_numContactPointsCopied; serverCmd.m_type = CMD_CONTACT_POINT_INFORMATION_COMPLETED; return true; } bool PhysXServerCommandProcessor::processCreateCollisionShapeCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) { return false; } bool PhysXServerCommandProcessor::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 PhysXServerCommandProcessor::processUserDebugDrawCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) { ///dummy, so commands don't fail bool hasStatus = true; BT_PROFILE("CMD_USER_DEBUG_DRAW"); SharedMemoryStatus& serverCmd = serverStatusOut; int uid = 0; serverCmd.m_userDebugDrawArgs.m_debugItemUniqueId = uid; serverCmd.m_type = CMD_USER_DEBUG_DRAW_FAILED; if (clientCmd.m_updateFlags & USER_DEBUG_ADD_PARAMETER) { int uid = m_data->m_userDebugParametersUid++; double value = clientCmd.m_userDebugDrawArgs.m_startValue; m_data->m_userDebugParameters.insert(uid, value); serverCmd.m_userDebugDrawArgs.m_debugItemUniqueId = uid; serverCmd.m_type = CMD_USER_DEBUG_DRAW_COMPLETED; } if (clientCmd.m_updateFlags & USER_DEBUG_READ_PARAMETER) { double* valPtr = m_data->m_userDebugParameters[clientCmd.m_userDebugDrawArgs.m_itemUniqueId]; if (valPtr) { serverCmd.m_userDebugDrawArgs.m_parameterValue = *valPtr; serverCmd.m_type = CMD_USER_DEBUG_DRAW_PARAMETER_COMPLETED; } } if (clientCmd.m_updateFlags & USER_DEBUG_REMOVE_ALL) { m_data->m_userDebugParameters.clear(); serverCmd.m_type = CMD_USER_DEBUG_DRAW_COMPLETED; } if (clientCmd.m_updateFlags & USER_DEBUG_REMOVE_ONE_ITEM) { m_data->m_userDebugParameters.remove(clientCmd.m_userDebugDrawArgs.m_itemUniqueId); serverCmd.m_type = CMD_USER_DEBUG_DRAW_COMPLETED; } return hasStatus; } bool PhysXServerCommandProcessor::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 = true; serverStatusOut.m_type = CMD_INVALID_STATUS; serverStatusOut.m_numDataStreamBytes = 0; serverStatusOut.m_dataStream = 0; //consume the command switch (clientCmd.m_type) { case CMD_REQUEST_INTERNAL_DATA: { hasStatus = processRequestInternalDataCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); break; }; case CMD_SYNC_BODY_INFO: { hasStatus = processSyncBodyInfoCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); break; } case CMD_SYNC_USER_DATA: { hasStatus = processSyncUserDataCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); break; } case CMD_REQUEST_BODY_INFO: { hasStatus = processRequestBodyInfoCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); break; } case CMD_STEP_FORWARD_SIMULATION: { hasStatus = processForwardDynamicsCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); break; } case CMD_SEND_PHYSICS_SIMULATION_PARAMETERS: { hasStatus = processSendPhysicsParametersCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); break; }; case CMD_REQUEST_ACTUAL_STATE: { hasStatus = processRequestActualStateCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); break; } case CMD_RESET_SIMULATION: { hasStatus = processResetSimulationCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); break; } case CMD_LOAD_URDF: { hasStatus = processLoadURDFCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); break; } case CMD_CUSTOM_COMMAND: { hasStatus = processCustomCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); break; } case CMD_STATE_LOGGING: { hasStatus = processStateLoggingCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); break; } case CMD_INIT_POSE: { hasStatus = processInitPoseCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); break; } case CMD_SEND_DESIRED_STATE: { hasStatus = processSendDesiredStateCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); break; } case CMD_CHANGE_DYNAMICS_INFO: { hasStatus = processChangeDynamicsInfoCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); break; }; case CMD_REQUEST_PHYSICS_SIMULATION_PARAMETERS: { hasStatus = processRequestPhysicsSimulationParametersCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); break; } case CMD_REQUEST_CONTACT_POINT_INFORMATION: { hasStatus = processRequestContactpointInformationCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); break; } case CMD_CREATE_COLLISION_SHAPE: { hasStatus = processCreateCollisionShapeCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); break; } case CMD_SET_ADDITIONAL_SEARCH_PATH: { hasStatus = processSetAdditionalSearchPathCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); break; } case CMD_USER_DEBUG_DRAW: { hasStatus = processUserDebugDrawCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); break; } case CMD_REQUEST_CAMERA_IMAGE_DATA: { hasStatus = processRequestCameraImageCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); break; } #if 0 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_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_VISUAL_SHAPE: { hasStatus = processCreateVisualShapeCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); break; } case CMD_CREATE_MULTI_BODY: { hasStatus = processCreateMultiBodyCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); break; } case CMD_LOAD_MJCF: { hasStatus = processLoadMJCFCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); break; } case CMD_LOAD_SOFT_BODY: { hasStatus = processLoadSoftBodyCommand(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_REQUEST_COLLISION_INFO: { hasStatus = processRequestCollisionInfoCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); break; } case CMD_GET_DYNAMICS_INFO: { hasStatus = processGetDynamicsInfoCommand(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_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_REQUEST_COLLISION_SHAPE_INFO: { hasStatus = processRequestCollisionShapeInfoCommand(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_RESTORE_STATE: { hasStatus = processRestoreStateCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); break; } case CMD_SAVE_STATE: { hasStatus = processSaveStateCommand(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_REQUEST_USER_DATA: { hasStatus = processRequestUserDataCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); break; } case CMD_ADD_USER_DATA: { hasStatus = processAddUserDataCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); break; } case CMD_REMOVE_USER_DATA: { hasStatus = processRemoveUserDataCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); break; } #endif default: { BT_PROFILE("CMD_UNKNOWN"); printf("Unknown command encountered: %d", clientCmd.m_type); SharedMemoryStatus& serverCmd = serverStatusOut; serverCmd.m_type = CMD_UNKNOWN_COMMAND_FLUSHED; hasStatus = true; } }; return hasStatus; } bool PhysXServerCommandProcessor::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; int oldWidth; int oldHeight; m_data->m_pluginManager.getRenderInterface()->getWidthAndHeight(oldWidth, oldHeight); serverStatusOut.m_type = CMD_CAMERA_IMAGE_FAILED; if ((clientCmd.m_requestPixelDataArguments.m_startPixelIndex == 0) && (clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_PIXEL_WIDTH_HEIGHT) != 0) { if (m_data->m_pluginManager.getRenderInterface()) { m_data->m_pluginManager.getRenderInterface()->setWidthAndHeight(clientCmd.m_requestPixelDataArguments.m_pixelWidth, clientCmd.m_requestPixelDataArguments.m_pixelHeight); } } int flags = 0; if (clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_HAS_FLAGS) { flags = clientCmd.m_requestPixelDataArguments.m_flags; } if (m_data->m_pluginManager.getRenderInterface()) { m_data->m_pluginManager.getRenderInterface()->setFlags(flags); } 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]; float projTextureViewMat[16]; float projTextureProjMat[16]; if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES) == 0) { b3OpenGLVisualizerCameraInfo tmpCamResult; bool result = m_data->m_pluginManager.getRenderInterface()->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]; } } else { //failed m_data->m_pluginManager.getRenderInterface()->setWidthAndHeight(oldWidth, oldHeight); return hasStatus; } } else { for (int i = 0; i < 16; i++) { viewMat[i] = clientCmd.m_requestPixelDataArguments.m_viewMatrix[i]; projMat[i] = clientCmd.m_requestPixelDataArguments.m_projectionMatrix[i]; } } { if (m_data->m_pluginManager.getRenderInterface()) { if (clientCmd.m_requestPixelDataArguments.m_startPixelIndex == 0) { // printf("-------------------------------\nRendering\n"); if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_LIGHT_DIRECTION) != 0) { m_data->m_pluginManager.getRenderInterface()->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_pluginManager.getRenderInterface()->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_pluginManager.getRenderInterface()->setLightDistance(clientCmd.m_requestPixelDataArguments.m_lightDistance); } if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_SHADOW) != 0) { m_data->m_pluginManager.getRenderInterface()->setShadow((clientCmd.m_requestPixelDataArguments.m_hasShadow != 0)); } if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_AMBIENT_COEFF) != 0) { m_data->m_pluginManager.getRenderInterface()->setLightAmbientCoeff(clientCmd.m_requestPixelDataArguments.m_lightAmbientCoeff); } if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_DIFFUSE_COEFF) != 0) { m_data->m_pluginManager.getRenderInterface()->setLightDiffuseCoeff(clientCmd.m_requestPixelDataArguments.m_lightDiffuseCoeff); } if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_SPECULAR_COEFF) != 0) { m_data->m_pluginManager.getRenderInterface()->setLightSpecularCoeff(clientCmd.m_requestPixelDataArguments.m_lightSpecularCoeff); } if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES) != 0) { m_data->m_pluginManager.getRenderInterface()->render( clientCmd.m_requestPixelDataArguments.m_viewMatrix, clientCmd.m_requestPixelDataArguments.m_projectionMatrix); } else { b3OpenGLVisualizerCameraInfo tmpCamResult; bool result = m_data->m_pluginManager.getRenderInterface()->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_pluginManager.getRenderInterface()->render(tmpCamResult.m_viewMatrix, tmpCamResult.m_projectionMatrix); } else { m_data->m_pluginManager.getRenderInterface()->render(); } } } } if (m_data->m_pluginManager.getRenderInterface()) { if ((flags & ER_USE_PROJECTIVE_TEXTURE) != 0) { m_data->m_pluginManager.getRenderInterface()->setProjectiveTexture(true); if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_HAS_PROJECTIVE_TEXTURE_MATRICES) != 0) { for (int i = 0; i < 16; i++) { projTextureViewMat[i] = clientCmd.m_requestPixelDataArguments.m_projectiveTextureViewMatrix[i]; projTextureProjMat[i] = clientCmd.m_requestPixelDataArguments.m_projectiveTextureProjectionMatrix[i]; } } else // If no specified matrices for projective texture, then use the camera matrices. { for (int i = 0; i < 16; i++) { projTextureViewMat[i] = viewMat[i]; projTextureProjMat[i] = projMat[i]; } } m_data->m_pluginManager.getRenderInterface()->setProjectiveTextureMatrices(projTextureViewMat, projTextureProjMat); } else { m_data->m_pluginManager.getRenderInterface()->setProjectiveTexture(false); } if ((flags & ER_NO_SEGMENTATION_MASK) != 0) { segmentationMaskBuffer = 0; } m_data->m_pluginManager.getRenderInterface()->copyCameraImageData(pixelRGBA, numRequestedPixels, depthBuffer, numRequestedPixels, segmentationMaskBuffer, numRequestedPixels, startPixelIndex, &width, &height, &numPixelsCopied); m_data->m_pluginManager.getRenderInterface()->setProjectiveTexture(false); } #if 0 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); #endif } //each pixel takes 4 RGBA values and 1 float = 8 bytes } else { } m_data->m_pluginManager.getRenderInterface()->setWidthAndHeight(oldWidth, oldHeight); 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 PhysXServerCommandProcessor::processRequestInternalDataCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) { bool hasStatus = true; BT_PROFILE("CMD_REQUEST_INTERNAL_DATA"); SharedMemoryStatus& serverCmd = serverStatusOut; serverCmd.m_type = CMD_REQUEST_INTERNAL_DATA_COMPLETED; serverCmd.m_numDataStreamBytes = 0; return hasStatus; } bool PhysXServerCommandProcessor::processSyncBodyInfoCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) { bool hasStatus = true; BT_PROFILE("CMD_SYNC_BODY_INFO"); int actualNumBodies = 0; serverStatusOut.m_sdfLoadedArgs.m_numBodies = 0; serverStatusOut.m_sdfLoadedArgs.m_numUserConstraints = 0; serverStatusOut.m_type = CMD_SYNC_BODY_INFO_COMPLETED; return hasStatus; } bool PhysXServerCommandProcessor::processSyncUserDataCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) { bool hasStatus = true; BT_PROFILE("CMD_SYNC_USER_DATA"); int numIdentifiers = 0; serverStatusOut.m_syncUserDataArgs.m_numUserDataIdentifiers = numIdentifiers; serverStatusOut.m_type = CMD_SYNC_USER_DATA_COMPLETED; return hasStatus; } struct MyPhysXURDFImporter : public PhysXURDFImporter { b3PluginManager& m_pluginManager; MyPhysXURDFImporter(struct CommonFileIOInterface* fileIO, double globalScaling, int flags, b3PluginManager& pluginManager) :PhysXURDFImporter(fileIO, globalScaling, flags), m_pluginManager(pluginManager) { } int convertLinkVisualShapes3( int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfLink* linkPtr, const UrdfModel* model, int collisionObjectUniqueId, int bodyUniqueId, struct CommonFileIOInterface* fileIO) const { if (m_pluginManager.getRenderInterface()) { int graphicsUniqueId = m_pluginManager.getRenderInterface()->convertVisualShapes(linkIndex, pathPrefix, localInertiaFrame, linkPtr, model, collisionObjectUniqueId, bodyUniqueId, fileIO); return graphicsUniqueId; } return 0; } }; bool PhysXServerCommandProcessor::processLoadURDFCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) { BT_PROFILE("CMD_LOAD_URDF"); serverStatusOut.m_type = CMD_URDF_LOADING_FAILED; serverStatusOut.m_numDataStreamBytes = 0; const UrdfArgs& urdfArgs = clientCmd.m_urdfArguments; btAssert(m_data->m_foundation); if (!m_data->m_foundation) { b3Error("loadUrdf: No valid m_dynamicsWorld"); return false; } 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; btScalar globalScaling = 1.f; if (clientCmd.m_updateFlags & URDF_ARGS_USE_GLOBAL_SCALING) { globalScaling = urdfArgs.m_globalScaling; } b3BulletDefaultFileIO fileIO; 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]; } MyPhysXURDFImporter u2p(&fileIO, globalScaling, urdfArgs.m_urdfFlags, m_data->m_pluginManager); bool loadOk = u2p.loadURDF(urdfArgs.m_urdfFileName, useFixedBase); if (loadOk) { for (int m = 0; m < u2p.getNumModels(); m++) { u2p.activateModel(m); btTransform rootTrans; rootTrans.setOrigin(initialPos); rootTrans.setRotation(initialOrn); u2p.setRootTransformInWorld(rootTrans); //get a body index int bodyUniqueId = m_data->m_bodyHandles.allocHandle(); InternalPhysXBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(bodyUniqueId); //sd.m_bodyUniqueIds.push_back(bodyUniqueId); u2p.setBodyUniqueId(bodyUniqueId); { btScalar mass = 0; //bodyHandle->m_rootLocalInertialFrame.setIdentity(); bodyHandle->m_bodyName = u2p.getBodyName(); btVector3 localInertiaDiagonal(0, 0, 0); int urdfLinkIndex = u2p.getRootLinkIndex(); //u2p.getMassAndInertia2((urdfLinkIndex, mass, localInertiaDiagonal, bodyHandle->m_rootLocalInertialFrame, flags); } physx::PxBase* baseObj = URDF2PhysX(m_data->m_foundation,m_data->m_physics, m_data->m_cooking, m_data->m_scene, u2p, urdfArgs.m_urdfFlags, u2p.getPathPrefix(), rootTrans, &fileIO, useMultiBody); physx::PxRigidDynamic* c = baseObj->is(); physx::PxRigidStatic* rigidStatic = baseObj->is(); physx::PxRigidDynamic* rigidDynamic = baseObj->is(); physx::PxArticulationReducedCoordinate* articulation = 0; if (rigidDynamic) { bodyHandle->m_rigidDynamic = rigidDynamic; } if (rigidStatic) { bodyHandle->m_rigidStatic = rigidStatic; serverStatusOut.m_numDataStreamBytes = 0; serverStatusOut.m_type = CMD_URDF_LOADING_COMPLETED; serverStatusOut.m_dataStreamArguments.m_bodyUniqueId = bodyUniqueId; } if ((rigidDynamic == 0) && (rigidStatic == 0)) { articulation = (physx::PxArticulationReducedCoordinate*)baseObj; serverStatusOut.m_type = CMD_URDF_LOADING_COMPLETED; serverStatusOut.m_dataStreamArguments.m_bodyUniqueId = bodyUniqueId; } if (rigidStatic || rigidDynamic || articulation) { serverStatusOut.m_type = CMD_URDF_LOADING_COMPLETED; serverStatusOut.m_dataStreamArguments.m_bodyUniqueId = bodyUniqueId; sprintf(serverStatusOut.m_dataStreamArguments.m_bodyName, "%s", bodyHandle->m_bodyName.c_str()); } if (articulation) { bodyHandle->mArticulation = articulation; btDefaultSerializer ser(bufferSizeInBytes, (unsigned char*)bufferServerToClient); ser.startSerialization(); int len = sizeof(btMultiBodyData); btChunk* chunk = ser.allocate(len, 1); btMultiBodyData *mbd = (btMultiBodyData *)chunk->m_oldPtr; btTransform rootTrans; u2p.getRootTransformInWorld(rootTrans); rootTrans.getOrigin().serialize(mbd->m_baseWorldPosition); rootTrans.getRotation().serialize(mbd->m_baseWorldOrientation); btVector3 zero(0, 0, 0); zero.serialize(mbd->m_baseLinearVelocity); zero.serialize(mbd->m_baseAngularVelocity); ser.registerNameForPointer(bodyHandle->m_bodyName.c_str(), bodyHandle->m_bodyName.c_str()); { char *name = (char *)ser.findNameForPointer(bodyHandle->m_bodyName.c_str()); mbd->m_baseName = (char *)ser.getUniquePointer(name); if (mbd->m_baseName) { ser.serializeName(name); } } mbd->m_numLinks = articulation->getNbLinks()-1; if (mbd->m_numLinks) { int sz = sizeof(btMultiBodyLinkData); int numElem = mbd->m_numLinks; btChunk *chunk = ser.allocate(sz, numElem); physx::PxArticulationLink* physxLinks[64]; physx::PxU32 bufferSize = 64; physx::PxU32 startIndex = 0; int numLinks2 = articulation->getLinks(physxLinks, bufferSize, startIndex); btMultiBodyLinkData *memPtr = (btMultiBodyLinkData *)chunk->m_oldPtr; for (int j = 0; j < numElem; j++, memPtr++) { int i = j + 1; memPtr->m_jointType = 0;//todo memPtr->m_dofCount = physxLinks[i]->getInboundJointDof(); memPtr->m_posVarCount = physxLinks[i]->getInboundJointDof(); //?? physx::PxVec3 li = physxLinks[i]->getMassSpaceInertiaTensor(); btVector3 localInertia(li[0], li[1], li[2]); localInertia.serialize(memPtr->m_linkInertia); memPtr->m_linkMass = physxLinks[i]->getMass(); memPtr->m_parentIndex = i>0? physxLinks[i]->getInboundJoint()->getParentArticulationLink().getLinkIndex(): -1; memPtr->m_jointDamping = 0;//todophysxLinks[i]->getLinearDamping();//?? memPtr->m_jointFriction = 0;//todo memPtr->m_jointLowerLimit = 0;//todogetLink(i).m_jointLowerLimit; memPtr->m_jointUpperLimit = 0;//todogetLink(i).m_jointUpperLimit; memPtr->m_jointMaxForce = 0;//todogetLink(i).m_jointMaxForce; memPtr->m_jointMaxVelocity = 0;//todogetLink(i).m_jointMaxVelocity; //getLink(i).m_eVector.serialize(memPtr->m_parentComToThisPivotOffset); //getLink(i).m_dVector.serialize(memPtr->m_thisPivotToThisComOffset); //getLink(i).m_zeroRotParentToThis.serialize(memPtr->m_zeroRotParentToThis); { char *name = (char *)ser.findNameForPointer(physxLinks[i]->getName()); memPtr->m_linkName = (char *)ser.getUniquePointer(name); if (memPtr->m_linkName) { ser.serializeName(name); } } { char *name = (char *)ser.findNameForPointer(physxLinks[i]->getName()); memPtr->m_jointName = (char *)ser.getUniquePointer(name); if (memPtr->m_jointName) { ser.serializeName(name); } } memPtr->m_linkCollider = (btCollisionObjectData *)ser.getUniquePointer(0); } ser.finalizeChunk(chunk, btMultiBodyLinkDataName, BT_ARRAY_CODE, (void *)articulation); } mbd->m_links = mbd->m_numLinks ? (btMultiBodyLinkData *)ser.getUniquePointer((void *)articulation) : 0; // Fill padding with zeros to appease msan. #ifdef BT_USE_DOUBLE_PRECISION memset(mbd->m_padding, 0, sizeof(mbd->m_padding)); #endif const char* structType = btMultiBodyDataName; ser.finalizeChunk(chunk, structType, BT_MULTIBODY_CODE,0); int streamSizeInBytes = ser.getCurrentBufferSize(); serverStatusOut.m_numDataStreamBytes = streamSizeInBytes; } } #if 0 btTransform rootTrans; rootTrans.setOrigin(pos); rootTrans.setRotation(orn); u2b.setRootTransformInWorld(rootTrans); bool ok = processImportedObjects(fileName, bufferServerToClient, bufferSizeInBytes, useMultiBody, flags, u2b); if (ok) { if (m_data->m_sdfRecentLoadedBodies.size() == 1) { *bodyUniqueIdPtr = m_data->m_sdfRecentLoadedBodies[0]; } m_data->m_sdfRecentLoadedBodies.clear(); } #endif return true; } return false; } bool PhysXServerCommandProcessor::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 = 0; //createBodyInfoStream(sdfInfoArgs.m_bodyUniqueId, bufferServerToClient, bufferSizeInBytes); serverStatusOut.m_type = CMD_BODY_INFO_COMPLETED; serverStatusOut.m_numDataStreamBytes = streamSizeInBytes; return hasStatus; } bool PhysXServerCommandProcessor::processForwardDynamicsCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) { bool hasStatus = true; BT_PROFILE("CMD_STEP_FORWARD_SIMULATION"); int numArt = m_data->m_scene->getNbArticulations(); { B3_PROFILE("clear Contacts"); m_data->m_contactPoints.clear(); } { B3_PROFILE("PhysX_simulate_fetchResults"); m_data->m_scene->simulate(m_data->m_physicsDeltaTime); m_data->m_scene->fetchResults(true); } { B3_PROFILE("syncTransform"); if (m_data->m_pluginManager.getRenderInterface()) { //sync transforms... b3AlignedObjectArray usedHandles; m_data->m_bodyHandles.getUsedHandles(usedHandles); for (int i = 0; i < usedHandles.size(); i++) { InternalPhysXBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(usedHandles[i]); physx::PxRigidActor* actor = 0; if (bodyHandle->m_rigidDynamic) actor = bodyHandle->m_rigidDynamic; if (bodyHandle->m_rigidStatic) actor = bodyHandle->m_rigidStatic; if (actor) { btTransform tr; tr.setIdentity(); physx::PxTransform pt = actor->getGlobalPose(); tr.setOrigin(btVector3(pt.p[0], pt.p[1], pt.p[2])); tr.setRotation(btQuaternion(pt.q.x, pt.q.y, pt.q.z, pt.q.w)); btVector3 localScaling(1, 1, 1);//?? MyPhysXUserData* ud = (MyPhysXUserData*)actor->userData; m_data->m_pluginManager.getRenderInterface()->syncTransform(ud->m_graphicsUniqueId, tr, localScaling); } if (bodyHandle->mArticulation) { physx::PxArticulationLink* physxLinks[64]; physx::PxU32 bufferSize = 64; physx::PxU32 startIndex = 0; int numLinks2 = bodyHandle->mArticulation->getLinks(physxLinks, bufferSize, startIndex); for (int l = 0; l < numLinks2; l++) { MyPhysXUserData* ud = (MyPhysXUserData*)physxLinks[l]->userData; if (ud) { btTransform tr; tr.setIdentity(); physx::PxTransform pt = physxLinks[l]->getGlobalPose(); tr.setOrigin(btVector3(pt.p[0], pt.p[1], pt.p[2])); tr.setRotation(btQuaternion(pt.q.x, pt.q.y, pt.q.z, pt.q.w)); btVector3 localScaling(1, 1, 1);//?? m_data->m_pluginManager.getRenderInterface()->syncTransform(ud->m_graphicsUniqueId, tr, localScaling); } } } } } { B3_PROFILE("render"); //m_data->m_pluginManager.getRenderInterface()->render(); unsigned char* pixelRGBA = 0; int numRequestedPixels = 0; float* depthBuffer = 0; int* segmentationMaskBuffer = 0; int startPixelIndex = 0; int width = 1024; int height = 768; m_data->m_pluginManager.getRenderInterface()->getWidthAndHeight(width, height); int numPixelsCopied = 0; m_data->m_pluginManager.getRenderInterface()->copyCameraImageData(pixelRGBA, numRequestedPixels, depthBuffer, numRequestedPixels, segmentationMaskBuffer, numRequestedPixels, startPixelIndex, &width, &height, &numPixelsCopied); } } SharedMemoryStatus& serverCmd = serverStatusOut; serverCmd.m_type = CMD_STEP_FORWARD_SIMULATION_COMPLETED; return hasStatus; } bool PhysXServerCommandProcessor::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_GRAVITY) { btVector3 grav(clientCmd.m_physSimParamArgs.m_gravityAcceleration[0], clientCmd.m_physSimParamArgs.m_gravityAcceleration[1], clientCmd.m_physSimParamArgs.m_gravityAcceleration[2]); m_data->m_scene->setGravity(physx::PxVec3(grav[0], grav[1], grav[2])); if (m_data->m_verboseOutput) { b3Printf("Updated Gravity: %f,%f,%f", grav[0], grav[1], grav[2]); } } #if 0 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_ENABLE_CONE_FRICTION) { if (clientCmd.m_physSimParamArgs.m_enableConeFriction) { m_data->m_dynamicsWorld->getSolverInfo().m_solverMode &=~SOLVER_DISABLE_IMPLICIT_CONE_FRICTION; } else { m_data->m_dynamicsWorld->getSolverInfo().m_solverMode |=SOLVER_DISABLE_IMPLICIT_CONE_FRICTION; } } if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_DETERMINISTIC_OVERLAPPING_PAIRS) { m_data->m_dynamicsWorld->getDispatchInfo().m_deterministicOverlappingPairs = (clientCmd.m_physSimParamArgs.m_deterministicOverlappingPairs!=0); } if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_CCD_ALLOWED_PENETRATION) { m_data->m_dynamicsWorld->getDispatchInfo().m_allowedCcdPenetration = clientCmd.m_physSimParamArgs.m_allowedCcdPenetration; } if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_JOINT_FEEDBACK_MODE) { gJointFeedbackInWorldSpace = (clientCmd.m_physSimParamArgs.m_jointFeedbackMode&JOINT_FEEDBACK_IN_WORLD_SPACE)!=0; gJointFeedbackInJointFrame = (clientCmd.m_physSimParamArgs.m_jointFeedbackMode&JOINT_FEEDBACK_IN_JOINT_FRAME)!=0; } 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_SOLVER_RESIDULAL_THRESHOLD) { m_data->m_dynamicsWorld->getSolverInfo().m_leastSquaresResidualThreshold = clientCmd.m_physSimParamArgs.m_solverResidualThreshold; } if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_CONTACT_BREAKING_THRESHOLD) { gContactBreakingThreshold = clientCmd.m_physSimParamArgs.m_contactBreakingThreshold; } if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_CONTACT_SLOP) { m_data->m_dynamicsWorld->getSolverInfo().m_linearSlop = clientCmd.m_physSimParamArgs.m_contactSlop; } if (clientCmd.m_updateFlags&SIM_PARAM_ENABLE_SAT) { m_data->m_dynamicsWorld->getDispatchInfo().m_enableSatConvex = clientCmd.m_physSimParamArgs.m_enableSAT!=0; } 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_DEFAULT_GLOBAL_CFM) { m_data->m_dynamicsWorld->getSolverInfo().m_globalCfm = clientCmd.m_physSimParamArgs.m_defaultGlobalCFM; } if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_DEFAULT_FRICTION_CFM) { m_data->m_dynamicsWorld->getSolverInfo().m_frictionCFM = clientCmd.m_physSimParamArgs.m_frictionCFM; } 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); } #endif SharedMemoryStatus& serverCmd = serverStatusOut; serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED; return hasStatus; } bool PhysXServerCommandProcessor::processRequestActualStateCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) { bool hasStatus = true; serverStatusOut.m_type = CMD_ACTUAL_STATE_UPDATE_FAILED; int bodyUniqueId = clientCmd.m_requestActualStateInformationCommandArgument.m_bodyUniqueId; InternalPhysXBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(bodyUniqueId); if (bodyHandle->mArticulation) { BT_PROFILE("CMD_REQUEST_ACTUAL_STATE"); if (m_data->m_verboseOutput) { b3Printf("Sending the actual state (Q,U)"); } { SharedMemoryStatus& serverCmd = serverStatusOut; serverStatusOut.m_type = CMD_ACTUAL_STATE_UPDATE_COMPLETED; serverCmd.m_sendActualStateArgs.m_bodyUniqueId = bodyUniqueId; serverCmd.m_sendActualStateArgs.m_numLinks = bodyHandle->mArticulation->getNbLinks()-1; //skip base! int totalDegreeOfFreedomQ = 0; int totalDegreeOfFreedomU = 0; if (serverCmd.m_sendActualStateArgs.m_numLinks >= MAX_DEGREE_OF_FREEDOM) { serverStatusOut.m_type = CMD_ACTUAL_STATE_UPDATE_FAILED; hasStatus = true; return hasStatus; } bool computeForwardKinematics = ((clientCmd.m_updateFlags & ACTUAL_STATE_COMPUTE_FORWARD_KINEMATICS) != 0); bool computeLinkVelocities = ((clientCmd.m_updateFlags & ACTUAL_STATE_COMPUTE_LINKVELOCITY) != 0); if (computeForwardKinematics || computeLinkVelocities) { //todo:check this } physx::PxArticulationLink* physxLinks[64]; physx::PxU32 bufferSize = 64; physx::PxU32 startIndex = 0; int numLinks2 = bodyHandle->mArticulation->getLinks(physxLinks, bufferSize, startIndex); { serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[0] = 0; serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[1] = 0; serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[2] = 0; serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[3] = 0; serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[4] = 0; serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[5] = 0; serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[6] = 1; physx::PxArticulationLink* l = physxLinks[0]; physx::PxVec3 pos = l->getGlobalPose().p; physx::PxQuat orn = l->getGlobalPose().q; //base position in world space, carthesian serverCmd.m_sendActualStateArgs.m_actualStateQ[0] = pos[0]; serverCmd.m_sendActualStateArgs.m_actualStateQ[1] = pos[1]; serverCmd.m_sendActualStateArgs.m_actualStateQ[2] = pos[2]; //base orientation, quaternion x,y,z,w, in world space, carthesian serverCmd.m_sendActualStateArgs.m_actualStateQ[3] = orn.x; serverCmd.m_sendActualStateArgs.m_actualStateQ[4] = orn.y; serverCmd.m_sendActualStateArgs.m_actualStateQ[5] = orn.z; serverCmd.m_sendActualStateArgs.m_actualStateQ[6] = orn.w; totalDegreeOfFreedomQ += 7; //pos + quaternion physx::PxVec3 linVel = l->getLinearVelocity(); physx::PxVec3 angVel = l->getAngularVelocity(); //base linear velocity (in world space, carthesian) serverCmd.m_sendActualStateArgs.m_actualStateQdot[0] = linVel.x; serverCmd.m_sendActualStateArgs.m_actualStateQdot[1] = linVel.y; serverCmd.m_sendActualStateArgs.m_actualStateQdot[2] = linVel.z; //base angular velocity (in world space, carthesian) serverCmd.m_sendActualStateArgs.m_actualStateQdot[3] = angVel.x; serverCmd.m_sendActualStateArgs.m_actualStateQdot[4] = angVel.y; serverCmd.m_sendActualStateArgs.m_actualStateQdot[5] = angVel.z; totalDegreeOfFreedomU += 6; //3 linear and 3 angular DOF } physx::PxArticulationCache* c = bodyHandle->mArticulation->createCache(); bodyHandle->mArticulation->copyInternalStateToCache(*c, physx::PxArticulationCache::ePOSITION | physx::PxArticulationCache::eVELOCITY);// physx::PxArticulationCache::eALL); btAlignedObjectArray dofStarts; dofStarts.resize(numLinks2); dofStarts[0] = 0; //We know that the root link does not have a joint //cache positions in PhysX may be reshuffled, see //http://gameworksdocs.nvidia.com/PhysX/4.0/documentation/PhysXGuide/Manual/Articulations.html for (int i = 1; i < numLinks2; ++i) { int llIndex = physxLinks[i]->getLinkIndex(); int dofs = physxLinks[i]->getInboundJointDof(); dofStarts[llIndex] = dofs; } int count = 0; for (int i = 1; i < numLinks2; ++i) { int dofs = dofStarts[i]; dofStarts[i] = count; count += dofs; } //start at 1, 0 is the base/root, handled above for (int l = 1; l < numLinks2; l++) { int dofs = physxLinks[l]->getInboundJointDof(); int llIndex = physxLinks[l]->getLinkIndex(); for (int d = 0; d < dofs; d++) { serverCmd.m_sendActualStateArgs.m_actualStateQ[totalDegreeOfFreedomQ++] = c->jointPosition[dofStarts[llIndex + d]]; serverCmd.m_sendActualStateArgs.m_actualStateQ[totalDegreeOfFreedomU++] = c->jointVelocity[dofStarts[llIndex + d]]; } } serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomQ = totalDegreeOfFreedomQ; serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomU = totalDegreeOfFreedomU; hasStatus = true; } } return hasStatus; } bool PhysXServerCommandProcessor::processResetSimulationCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) { bool hasStatus = true; BT_PROFILE("CMD_RESET_SIMULATION"); resetSimulation(); SharedMemoryStatus& serverCmd = serverStatusOut; serverCmd.m_type = CMD_RESET_SIMULATION_COMPLETED; return hasStatus; } bool PhysXServerCommandProcessor::receiveStatus(struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) { return false; } #endif //BT_ENABLE_PHYSX