1 #ifdef BT_ENABLE_PHYSX
2 #include "PhysXServerCommandProcessor.h"
3
4 #include "../../Utils/ChromeTraceUtil.h"
5
6 #include <stdio.h>
7 #include "../SharedMemoryCommands.h"
8 #include "LinearMath/btQuickprof.h"
9 #include "Bullet3Common/b3AlignedObjectArray.h"
10 #include "LinearMath/btMinMax.h"
11 #include "Bullet3Common/b3FileUtils.h"
12 #include "Bullet3Common/b3CommandLineArgs.h"
13 #include "../../Utils/b3ResourcePath.h"
14 #include "Bullet3Common/b3ResizablePool.h"
15 #include "PxPhysicsAPI.h"
16 #include "../Utils/b3BulletDefaultFileIO.h"
17 #include "PhysXUrdfImporter.h"
18 #include "PxTolerancesScale.h"
19 #include "PxDefaultCpuDispatcher.h"
20 #include "PxDefaultSimulationFilterShader.h"
21 #include "URDF2PhysX.h"
22 #include "../b3PluginManager.h"
23 #include "PxRigidActorExt.h"
24 #include "LinearMath/btThreads.h"
25
26 #define STATIC_EGLRENDERER_PLUGIN
27 #ifdef STATIC_EGLRENDERER_PLUGIN
28 #include "../plugins/eglPlugin/eglRendererPlugin.h"
29 #endif //STATIC_EGLRENDERER_PLUGIN
30
31 //for serialization of data to client
32 #include "BulletDynamics/Featherstone/btMultiBody.h"
33 #include "../Extras/Serialize/BulletFileLoader/btBulletFile.h"
34 #include "LinearMath/btSerializer.h"
35 #include "PhysXUserData.h"
36
37 class MyPhysXErrorCallback : public physx::PxErrorCallback
38 {
39 public:
MyPhysXErrorCallback()40 MyPhysXErrorCallback()
41 {
42 }
~MyPhysXErrorCallback()43 ~MyPhysXErrorCallback()
44 {
45 }
46
reportError(physx::PxErrorCode::Enum code,const char * message,const char * file,int line)47 virtual void reportError(physx::PxErrorCode::Enum code, const char* message, const char* file, int line)
48 {
49 b3Printf("%s in file:%s line:%d\n", message, file, line);
50 }
51 };
52
53 class CustomProfilerCallback : public physx::PxProfilerCallback
54 {
55 public:
~CustomProfilerCallback()56 virtual ~CustomProfilerCallback() {}
57
zoneStart(const char * eventName,bool detached,uint64_t contextId)58 virtual void* zoneStart(const char* eventName, bool detached, uint64_t contextId)
59 {
60 b3EnterProfileZone(eventName);
61 return 0;
62 }
63
zoneEnd(void * profilerData,const char * eventName,bool detached,uint64_t contextId)64 virtual void zoneEnd(void* profilerData, const char* eventName, bool detached, uint64_t contextId)
65 {
66 b3LeaveProfileZone();
67 }
68
69 };
70
71 static CustomProfilerCallback gCustomProfilerCallback;
72
73
74 struct InternalPhysXBodyData
75 {
76 physx::PxArticulationReducedCoordinate* mArticulation;
77 physx::PxRigidDynamic* m_rigidDynamic;
78 physx::PxRigidStatic* m_rigidStatic;
79
80 std::string m_bodyName;
81
InternalPhysXBodyDataInternalPhysXBodyData82 InternalPhysXBodyData()
83 {
84 clear();
85 }
86 //physx::PxArticulationJointReducedCoordinate* gDriveJoint;
clearInternalPhysXBodyData87 void clear()
88 {
89 mArticulation = 0;
90 m_rigidDynamic = 0;
91 m_rigidStatic = 0;
92
93 m_bodyName = "";
94 }
95 };
96
97
98
99 typedef b3PoolBodyHandle<InternalPhysXBodyData> InternalPhysXBodyHandle;
100
101
102
103 struct PhysXServerCommandProcessorInternalData : public physx::PxSimulationEventCallback, public physx::PxContactModifyCallback
104 {
105 bool m_isConnected;
106 bool m_verboseOutput;
107 double m_physicsDeltaTime;
108 int m_numSimulationSubSteps;
109 btSpinMutex m_taskLock;
110
111 btAlignedObjectArray<b3ContactPointData> m_contactPoints;
112
113
114
onContactModifyPhysXServerCommandProcessorInternalData115 void onContactModify(physx::PxContactModifyPair* const pairs, physx::PxU32 count)
116 {
117 for (physx::PxU32 i = 0; i<count; i++)
118 {
119 //...
120 }
121 }
122
onContactPhysXServerCommandProcessorInternalData123 void onContact(const physx::PxContactPairHeader& pairHeader, const physx::PxContactPair* pairs, physx::PxU32 nbPairs)
124 {
125 B3_PROFILE("onContact");
126 //todo: are there really multiple threads calling 'onContact'?
127 m_taskLock.lock();
128
129 btAlignedObjectArray<physx::PxContactPairPoint> contacts;
130 for (physx::PxU32 i = 0; i < nbPairs; i++)
131 {
132 physx::PxU32 contactCount = pairs[i].contactCount;
133 if (contactCount)
134 {
135 contacts.resize(contactCount);
136 pairs[i].extractContacts(&contacts[0], contactCount);
137 for (physx::PxU32 j = 0; j < contactCount; j++)
138 {
139 const physx::PxContactPairPoint& contact = contacts[i];
140 b3ContactPointData srcPt;
141 MyPhysXUserData* udA = (MyPhysXUserData*)pairHeader.actors[0]->userData;
142 MyPhysXUserData* udB = (MyPhysXUserData*)pairHeader.actors[1]->userData;
143 srcPt.m_bodyUniqueIdA = udA->m_bodyUniqueId;
144 srcPt.m_linkIndexA = udA->m_linkIndex;
145 srcPt.m_bodyUniqueIdB = udB->m_bodyUniqueId;
146 srcPt.m_linkIndexB = udB->m_linkIndex;
147 srcPt.m_positionOnAInWS[0] = contact.position.x + contact.separation*contact.normal.x;
148 srcPt.m_positionOnAInWS[1] = contact.position.y + contact.separation*contact.normal.y;
149 srcPt.m_positionOnAInWS[2] = contact.position.z + contact.separation*contact.normal.z;
150 srcPt.m_positionOnBInWS[0] = contact.position.x - contact.separation*contact.normal.x;
151 srcPt.m_positionOnBInWS[1] = contact.position.y - contact.separation*contact.normal.y;
152 srcPt.m_positionOnBInWS[2] = contact.position.z - contact.separation*contact.normal.z;
153 srcPt.m_contactNormalOnBInWS[0] = contact.normal.x;
154 srcPt.m_contactNormalOnBInWS[1] = contact.normal.y;
155 srcPt.m_contactNormalOnBInWS[2] = contact.normal.z;
156 srcPt.m_contactDistance = contact.separation;
157 srcPt.m_contactFlags = 0;
158 srcPt.m_linearFrictionDirection1[0] = 0;
159 srcPt.m_linearFrictionDirection1[1] = 0;
160 srcPt.m_linearFrictionDirection1[2] = 0;
161 srcPt.m_linearFrictionDirection2[0] = 0;
162 srcPt.m_linearFrictionDirection2[1] = 0;
163 srcPt.m_linearFrictionDirection2[2] = 0;
164
165 srcPt.m_linearFrictionForce2 = 0;
166
167 srcPt.m_normalForce = contact.impulse.dot(contact.normal);
168 //compute friction direction from impulse projected in contact plane using contact normal.
169 physx::PxVec3 fric = contact.impulse - contact.normal*srcPt.m_normalForce;
170 double fricForce = fric.normalizeSafe();
171 if (fricForce)
172 {
173 srcPt.m_linearFrictionDirection1[0] = fric.x;
174 srcPt.m_linearFrictionDirection1[1] = fric.y;
175 srcPt.m_linearFrictionDirection1[2] = fric.z;
176 srcPt.m_linearFrictionForce1 = fricForce;
177 }
178 m_contactPoints.push_back(srcPt);
179 // std::cout << "Contact: bw " << pairHeader.actors[0]->getName() << " and " << pairHeader.actors[1]->getName() << " | " << contacts[j].position.x << "," << contacts[j].position.y << ","
180 // << contacts[j].position.z << std::endl;
181 }
182 }
183 }
184 m_taskLock.unlock();
185 }
186
onConstraintBreakPhysXServerCommandProcessorInternalData187 void onConstraintBreak(physx::PxConstraintInfo* constraints, physx::PxU32 count)
188 {
189 PX_UNUSED(constraints);
190 PX_UNUSED(count);
191 }
192
onWakePhysXServerCommandProcessorInternalData193 void onWake(physx::PxActor** actors, physx::PxU32 count)
194 {
195 PX_UNUSED(actors);
196 PX_UNUSED(count);
197 }
198
onSleepPhysXServerCommandProcessorInternalData199 void onSleep(physx::PxActor** actors, physx::PxU32 count)
200 {
201 PX_UNUSED(actors);
202 PX_UNUSED(count);
203 }
204
onTriggerPhysXServerCommandProcessorInternalData205 void onTrigger(physx::PxTriggerPair* pairs, physx::PxU32 count)
206 {
207 PX_UNUSED(pairs);
208 PX_UNUSED(count);
209 }
210
onAdvancePhysXServerCommandProcessorInternalData211 void onAdvance(const physx::PxRigidBody* const*, const physx::PxTransform*, const physx::PxU32)
212 {
213 }
214
215
216 b3PluginManager m_pluginManager;
217
218
219 physx::PxDefaultAllocator m_allocator;
220 MyPhysXErrorCallback m_errorCallback;
221 physx::PxFoundation* m_foundation;
222 physx::PxPhysics* m_physics;
223 physx::PxCooking* m_cooking;
224 physx::PxDefaultCpuDispatcher* m_dispatcher;
225 physx::PxScene* m_scene;
226 physx::PxMaterial* m_material;
227 //physx::PxPvd* m_pvd;
228
229 b3ResizablePool<InternalPhysXBodyHandle> m_bodyHandles;
230
231
232
233
234 b3AlignedObjectArray<int> m_mjcfRecentLoadedBodies;
235
236 int m_profileTimingLoggingUid;
237 int m_stateLoggersUniqueId;
238 std::string m_profileTimingFileName;
239 b3CommandLineArgs m_commandLineArgs;
240 int m_userDebugParametersUid;
241 btHashMap<btHashInt, double> m_userDebugParameters;
242 btAlignedObjectArray<int> m_graphicsIndexToSegmentationMask;
243
PhysXServerCommandProcessorInternalDataPhysXServerCommandProcessorInternalData244 PhysXServerCommandProcessorInternalData(PhysXServerCommandProcessor* sdk, int argc, char* argv[])
245 : m_isConnected(false),
246 m_verboseOutput(false),
247 m_physicsDeltaTime(1. / 240.),
248 m_numSimulationSubSteps(0),
249 m_pluginManager(sdk),
250 m_profileTimingLoggingUid(-1),
251 m_stateLoggersUniqueId(1),
252 m_commandLineArgs(argc,argv),
253 m_userDebugParametersUid(0)
254 {
255
256 m_foundation = NULL;
257 m_physics = NULL;
258 m_cooking = NULL;
259 m_dispatcher = NULL;
260 m_scene = NULL;
261 m_material = NULL;
262 //m_pvd = NULL;
263
264 #ifdef STATIC_EGLRENDERER_PLUGIN
265 {
266 bool initPlugin = false;
267 b3PluginFunctions funcs(initPlugin_eglRendererPlugin, exitPlugin_eglRendererPlugin, executePluginCommand_eglRendererPlugin);
268 funcs.m_getRendererFunc = getRenderInterface_eglRendererPlugin;
269 int renderPluginId = m_pluginManager.registerStaticLinkedPlugin("eglRendererPlugin", funcs, initPlugin);
270 m_pluginManager.selectPluginRenderer(renderPluginId);
271 }
272 #endif //STATIC_EGLRENDERER_PLUGIN
273 }
274 };
275
PhysXServerCommandProcessor(int argc,char * argv[])276 PhysXServerCommandProcessor::PhysXServerCommandProcessor(int argc, char* argv[])
277 {
278
279 m_data = new PhysXServerCommandProcessorInternalData(this, argc, argv);
280 }
281
~PhysXServerCommandProcessor()282 PhysXServerCommandProcessor::~PhysXServerCommandProcessor()
283 {
284 delete m_data;
285 }
286
287
288
MyPhysXFilter(physx::PxFilterObjectAttributes attributes0,physx::PxFilterData filterData0,physx::PxFilterObjectAttributes attributes1,physx::PxFilterData filterData1,physx::PxPairFlags & pairFlags,const void * constantBlock,physx::PxU32 constantBlockSize)289 physx::PxFilterFlags MyPhysXFilter(physx::PxFilterObjectAttributes attributes0, physx::PxFilterData filterData0,
290 physx::PxFilterObjectAttributes attributes1, physx::PxFilterData filterData1,
291 physx::PxPairFlags& pairFlags, const void* constantBlock, physx::PxU32 constantBlockSize)
292 {
293 PX_UNUSED(attributes0);
294 PX_UNUSED(attributes1);
295 PX_UNUSED(constantBlock);
296 PX_UNUSED(constantBlockSize);
297 // if (filterData0.word2 != 0 && filterData0.word2 == filterData1.word2)
298 // return physx::PxFilterFlag::eKILL;
299 pairFlags |= physx::PxPairFlag::eCONTACT_DEFAULT | physx::PxPairFlag::eNOTIFY_TOUCH_FOUND
300 | physx::PxPairFlag::eDETECT_DISCRETE_CONTACT | physx::PxPairFlag::eNOTIFY_CONTACT_POINTS | physx::PxPairFlag::eMODIFY_CONTACTS;
301 return physx::PxFilterFlag::eDEFAULT;
302 }
303
304
305
connect()306 bool PhysXServerCommandProcessor::connect()
307 {
308 if (m_data->m_isConnected)
309 {
310 printf("already connected\n");
311 return true;
312 }
313
314 int result = 0;
315 {
316
317 m_data->m_foundation = PxCreateFoundation(PX_PHYSICS_VERSION, m_data->m_allocator, m_data->m_errorCallback);
318 // This call should be performed after PVD is initialized, otherwise it will have no effect.
319 PxSetProfilerCallback(&gCustomProfilerCallback);
320
321 m_data->m_physics = PxCreatePhysics(PX_PHYSICS_VERSION, *m_data->m_foundation, physx::PxTolerancesScale(), true, 0);
322 m_data->m_cooking = PxCreateCooking(PX_PHYSICS_VERSION, *m_data->m_foundation, physx::PxCookingParams(physx::PxTolerancesScale()));
323
324
325 physx::PxU32 numCores = 1;
326 m_data->m_commandLineArgs.GetCmdLineArgument("numCores", numCores);
327 printf("PhysX numCores=%d\n", numCores);
328 m_data->m_dispatcher = physx::PxDefaultCpuDispatcherCreate(numCores == 0 ? 0 : numCores - 1);
329
330 physx::PxSceneDesc sceneDesc(m_data->m_physics->getTolerancesScale());
331 sceneDesc.gravity = physx::PxVec3(0.0f, -9.81f, 0.0f);
332
333
334 sceneDesc.solverType = physx::PxSolverType::ePGS;
335 std::string solver;
336 m_data->m_commandLineArgs.GetCmdLineArgument("solver", solver);
337
338 if (solver=="tgs")
339 {
340 sceneDesc.solverType = physx::PxSolverType::eTGS;
341 printf("PhysX using TGS\n");
342 }
343 else
344 {
345 printf("PhysX using PGS\n");
346 }
347
348 sceneDesc.cpuDispatcher = m_data->m_dispatcher;
349
350 //todo: add some boolean, to allow enable/disable of this contact filtering
351 bool enableContactCallback = false;
352 m_data->m_commandLineArgs.GetCmdLineArgument("enableContactCallback", enableContactCallback);
353
354 if (enableContactCallback)
355 {
356 sceneDesc.filterShader = MyPhysXFilter;
357 sceneDesc.simulationEventCallback = this->m_data;
358 sceneDesc.contactModifyCallback = this->m_data;
359 printf("PhysX enableContactCallback\n");
360 }
361 else
362 {
363 sceneDesc.filterShader = physx::PxDefaultSimulationFilterShader;
364 }
365
366
367
368
369 m_data->m_scene = m_data->m_physics->createScene(sceneDesc);
370
371 m_data->m_material = m_data->m_physics->createMaterial(0.5f, 0.5f, 0.f);
372
373 PxInitExtensions(*m_data->m_physics, 0);
374
375
376 //PxRigidStatic* groundPlane = PxCreatePlane(*gPhysics, PxPlane(0, 1, 0, 0), *gMaterial);
377 //gScene->addActor(*groundPlane);
378
379 result = 1;
380 }
381 if (result == 1)
382 {
383 m_data->m_isConnected = true;
384 return true;
385 }
386
387 return false;
388 }
389
resetSimulation()390 void PhysXServerCommandProcessor::resetSimulation()
391 {
392 //gArticulation->release();
393 m_data->m_scene->release();
394 m_data->m_dispatcher->release();
395 m_data->m_cooking->release();
396 m_data->m_physics->release();
397 //PxPvdTransport* transport = gPvd->getTransport();
398 //gPvd->release();
399 //transport->release();
400 PxCloseExtensions();
401
402 m_data->m_foundation->release();
403 }
404
disconnect()405 void PhysXServerCommandProcessor::disconnect()
406 {
407 resetSimulation();
408
409 m_data->m_isConnected = false;
410 }
411
isConnected() const412 bool PhysXServerCommandProcessor::isConnected() const
413 {
414 return m_data->m_isConnected;
415 }
416
417
processCustomCommand(const struct SharedMemoryCommand & clientCmd,struct SharedMemoryStatus & serverStatusOut,char * bufferServerToClient,int bufferSizeInBytes)418 bool PhysXServerCommandProcessor::processCustomCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
419 {
420 bool hasStatus = true;
421
422 SharedMemoryStatus& serverCmd = serverStatusOut;
423 serverCmd.m_type = CMD_CUSTOM_COMMAND_FAILED;
424 serverCmd.m_customCommandResultArgs.m_pluginUniqueId = -1;
425
426 if (clientCmd.m_updateFlags & CMD_CUSTOM_COMMAND_LOAD_PLUGIN)
427 {
428 //pluginPath could be registered or load from disk
429 const char* postFix = "";
430 if (clientCmd.m_updateFlags & CMD_CUSTOM_COMMAND_LOAD_PLUGIN_POSTFIX)
431 {
432 postFix = clientCmd.m_customCommandArgs.m_postFix;
433 }
434
435 int pluginUniqueId = m_data->m_pluginManager.loadPlugin(clientCmd.m_customCommandArgs.m_pluginPath, postFix);
436 if (pluginUniqueId >= 0)
437 {
438 serverCmd.m_customCommandResultArgs.m_pluginUniqueId = pluginUniqueId;
439 serverCmd.m_type = CMD_CUSTOM_COMMAND_COMPLETED;
440 }
441 }
442 if (clientCmd.m_updateFlags & CMD_CUSTOM_COMMAND_UNLOAD_PLUGIN)
443 {
444 m_data->m_pluginManager.unloadPlugin(clientCmd.m_customCommandArgs.m_pluginUniqueId);
445 serverCmd.m_type = CMD_CUSTOM_COMMAND_COMPLETED;
446 }
447 if (clientCmd.m_updateFlags & CMD_CUSTOM_COMMAND_EXECUTE_PLUGIN_COMMAND)
448 {
449 int result = m_data->m_pluginManager.executePluginCommand(clientCmd.m_customCommandArgs.m_pluginUniqueId, &clientCmd.m_customCommandArgs.m_arguments);
450 serverCmd.m_customCommandResultArgs.m_executeCommandResult = result;
451 serverCmd.m_type = CMD_CUSTOM_COMMAND_COMPLETED;
452 }
453 return hasStatus;
454 }
455
456
processStateLoggingCommand(const struct SharedMemoryCommand & clientCmd,struct SharedMemoryStatus & serverStatusOut,char * bufferServerToClient,int bufferSizeInBytes)457 bool PhysXServerCommandProcessor::processStateLoggingCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
458 {
459 BT_PROFILE("CMD_STATE_LOGGING");
460
461 serverStatusOut.m_type = CMD_STATE_LOGGING_FAILED;
462 bool hasStatus = true;
463
464 if (clientCmd.m_stateLoggingArguments.m_logType == STATE_LOGGING_PROFILE_TIMINGS)
465 {
466 if (m_data->m_profileTimingLoggingUid < 0)
467 {
468 b3ChromeUtilsStartTimings();
469 m_data->m_profileTimingFileName = clientCmd.m_stateLoggingArguments.m_fileName;
470 int loggerUid = m_data->m_stateLoggersUniqueId++;
471 serverStatusOut.m_type = CMD_STATE_LOGGING_START_COMPLETED;
472 serverStatusOut.m_stateLoggingResultArgs.m_loggingUniqueId = loggerUid;
473 m_data->m_profileTimingLoggingUid = loggerUid;
474 }
475 }
476
477 if ((clientCmd.m_updateFlags & STATE_LOGGING_STOP_LOG) && clientCmd.m_stateLoggingArguments.m_loggingUniqueId >= 0)
478 {
479 if (clientCmd.m_stateLoggingArguments.m_loggingUniqueId == m_data->m_profileTimingLoggingUid)
480 {
481 serverStatusOut.m_type = CMD_STATE_LOGGING_COMPLETED;
482 b3ChromeUtilsStopTimingsAndWriteJsonFile(m_data->m_profileTimingFileName.c_str());
483 m_data->m_profileTimingLoggingUid = -1;
484 }
485 }
486
487
488 return hasStatus;
489 }
490
491
processInitPoseCommand(const struct SharedMemoryCommand & clientCmd,struct SharedMemoryStatus & serverStatusOut,char * bufferServerToClient,int bufferSizeInBytes)492 bool PhysXServerCommandProcessor::processInitPoseCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
493 {
494 bool hasStatus = true;
495
496 BT_PROFILE("CMD_INIT_POSE");
497
498 if (m_data->m_verboseOutput)
499 {
500 b3Printf("Server Init Pose not implemented yet");
501 }
502 int bodyUniqueId = clientCmd.m_initPoseArgs.m_bodyUniqueId;
503 InternalPhysXBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId);
504
505 btVector3 baseLinVel(0, 0, 0);
506 btVector3 baseAngVel(0, 0, 0);
507
508 if (clientCmd.m_updateFlags & INIT_POSE_HAS_BASE_LINEAR_VELOCITY)
509 {
510 baseLinVel.setValue(clientCmd.m_initPoseArgs.m_initialStateQdot[0],
511 clientCmd.m_initPoseArgs.m_initialStateQdot[1],
512 clientCmd.m_initPoseArgs.m_initialStateQdot[2]);
513 }
514 if (clientCmd.m_updateFlags & INIT_POSE_HAS_BASE_ANGULAR_VELOCITY)
515 {
516 baseAngVel.setValue(clientCmd.m_initPoseArgs.m_initialStateQdot[3],
517 clientCmd.m_initPoseArgs.m_initialStateQdot[4],
518 clientCmd.m_initPoseArgs.m_initialStateQdot[5]);
519 }
520 btVector3 basePos(0, 0, 0);
521 if (clientCmd.m_updateFlags & INIT_POSE_HAS_INITIAL_POSITION)
522 {
523 basePos = btVector3(
524 clientCmd.m_initPoseArgs.m_initialStateQ[0],
525 clientCmd.m_initPoseArgs.m_initialStateQ[1],
526 clientCmd.m_initPoseArgs.m_initialStateQ[2]);
527 }
528 btQuaternion baseOrn(0, 0, 0, 1);
529 if (clientCmd.m_updateFlags & INIT_POSE_HAS_INITIAL_ORIENTATION)
530 {
531 baseOrn.setValue(clientCmd.m_initPoseArgs.m_initialStateQ[3],
532 clientCmd.m_initPoseArgs.m_initialStateQ[4],
533 clientCmd.m_initPoseArgs.m_initialStateQ[5],
534 clientCmd.m_initPoseArgs.m_initialStateQ[6]);
535 }
536 if (body && body->mArticulation)
537 {
538 physx::PxArticulationCache* c = body->mArticulation->createCache();
539 body->mArticulation->copyInternalStateToCache(*c, physx::PxArticulationCache::ePOSITION | physx::PxArticulationCache::eVELOCITY);// physx::PxArticulationCache::eALL);
540 physx::PxArticulationLink* physxLinks[64];
541 physx::PxU32 bufferSize = 64;
542 physx::PxU32 startIndex = 0;
543 int numLinks2 = body->mArticulation->getLinks(physxLinks, bufferSize, startIndex);
544
545 btAlignedObjectArray<int> dofStarts;
546 dofStarts.resize(numLinks2);
547 dofStarts[0] = 0; //We know that the root link does not have a joint
548 //cache positions in PhysX may be reshuffled, see
549 //http://gameworksdocs.nvidia.com/PhysX/4.0/documentation/PhysXGuide/Manual/Articulations.html
550
551 for (int i = 1; i < numLinks2; ++i)
552 {
553 int llIndex = physxLinks[i]->getLinkIndex();
554 int dofs = physxLinks[i]->getInboundJointDof();
555
556 dofStarts[llIndex] = dofs;
557 }
558
559 int count = 0;
560 for (int i = 1; i < numLinks2; ++i)
561 {
562 int dofs = dofStarts[i];
563 dofStarts[i] = count;
564 count += dofs;
565 }
566
567 if (numLinks2 > 0)
568 {
569 int dofs = physxLinks[0]->getInboundJointDof();
570 physx::PxTransform pt = physxLinks[0]->getGlobalPose();
571 physx::PxVec3 linVel = physxLinks[0]->getLinearVelocity();
572 physx::PxVec3 angVel = physxLinks[0]->getAngularVelocity();
573
574
575 if (clientCmd.m_updateFlags & INIT_POSE_HAS_BASE_LINEAR_VELOCITY)
576 {
577 physxLinks[0]->setLinearVelocity(physx::PxVec3(baseLinVel[0], baseLinVel[1], baseLinVel[2]));
578 }
579
580 if (clientCmd.m_updateFlags & INIT_POSE_HAS_BASE_ANGULAR_VELOCITY)
581 {
582 physxLinks[0]->setAngularVelocity(physx::PxVec3(baseAngVel[0], baseAngVel[1], baseAngVel[2]));
583 }
584
585 if (clientCmd.m_updateFlags & INIT_POSE_HAS_INITIAL_POSITION)
586 {
587 pt.p.x = basePos[0];
588 pt.p.y = basePos[0];
589 pt.p.z = basePos[0];
590 physxLinks[0]->setGlobalPose(pt);
591 }
592 if (clientCmd.m_updateFlags & INIT_POSE_HAS_INITIAL_ORIENTATION)
593 {
594 btAssert(clientCmd.m_initPoseArgs.m_hasInitialStateQ[3] &&
595 clientCmd.m_initPoseArgs.m_hasInitialStateQ[4] &&
596 clientCmd.m_initPoseArgs.m_hasInitialStateQ[5] &&
597 clientCmd.m_initPoseArgs.m_hasInitialStateQ[6]);
598 pt.q.x = baseOrn[0];
599 pt.q.y = baseOrn[1];
600 pt.q.z = baseOrn[2];
601 pt.q.w = baseOrn[3];
602 physxLinks[0]->setGlobalPose(pt);
603 }
604 }
605 if (clientCmd.m_updateFlags & INIT_POSE_HAS_JOINT_STATE)
606 {
607 int uDofIndex = 6;
608 int posVarCountIndex = 7;
609
610 //skip 'root' link
611 for (int i = 1; i < numLinks2; i++)
612 {
613 int physxCacheLinkIndex = physxLinks[i]->getLinkIndex();
614 int dofs = physxLinks[i]->getInboundJointDof();
615 int posVarCount = dofs;//??
616 bool hasPosVar = posVarCount > 0;
617
618 for (int j = 0; j < posVarCount; j++)
619 {
620 if (clientCmd.m_initPoseArgs.m_hasInitialStateQ[posVarCountIndex + j] == 0)
621 {
622 hasPosVar = false;
623 break;
624 }
625 }
626 if (hasPosVar)
627 {
628 if (posVarCount == 1)
629 {
630 c->jointPosition[dofStarts[physxCacheLinkIndex]] = clientCmd.m_initPoseArgs.m_initialStateQ[posVarCountIndex];
631 }
632 if (posVarCount == 3)
633 {
634 btQuaternion q(
635 clientCmd.m_initPoseArgs.m_initialStateQ[posVarCountIndex],
636 clientCmd.m_initPoseArgs.m_initialStateQ[posVarCountIndex + 1],
637 clientCmd.m_initPoseArgs.m_initialStateQ[posVarCountIndex + 2],
638 clientCmd.m_initPoseArgs.m_initialStateQ[posVarCountIndex + 3]);
639 q.normalize();
640 //mb->setJointPosMultiDof(i, &q[0]);
641 //double vel[6] = { 0, 0, 0, 0, 0, 0 };
642 //mb->setJointVelMultiDof(i, vel);
643 }
644 }
645
646 bool hasVel = true;
647 for (int j = 0; j < posVarCount; j++)
648 {
649 if (clientCmd.m_initPoseArgs.m_hasInitialStateQdot[uDofIndex + j] == 0)
650 {
651 hasVel = false;
652 break;
653 }
654 }
655
656 if (hasVel)
657 {
658 if (posVarCount == 1)
659 {
660 btScalar vel = clientCmd.m_initPoseArgs.m_initialStateQdot[uDofIndex];
661 c->jointVelocity[dofStarts[physxCacheLinkIndex]] = vel;
662 }
663 if (posVarCount == 3)
664 {
665 //mb->setJointVelMultiDof(i, &clientCmd.m_initPoseArgs.m_initialStateQdot[uDofIndex]);
666 }
667 }
668
669 posVarCountIndex += dofs;
670 uDofIndex += dofs;// mb->getLink(i).m_dofCount;
671 }
672 }
673
674 body->mArticulation->applyCache(*c, physx::PxArticulationCache::ePOSITION| physx::PxArticulationCache::eVELOCITY);
675 body->mArticulation->releaseCache(*c);
676
677 }
678
679 SharedMemoryStatus& serverCmd = serverStatusOut;
680 serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED;
681 return hasStatus;
682 }
683
processSendDesiredStateCommand(const struct SharedMemoryCommand & clientCmd,struct SharedMemoryStatus & serverStatusOut,char * bufferServerToClient,int bufferSizeInBytes)684 bool PhysXServerCommandProcessor::processSendDesiredStateCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
685 {
686 bool hasStatus = true;
687 BT_PROFILE("CMD_SEND_DESIRED_STATE");
688
689 int bodyUniqueId = clientCmd.m_sendDesiredStateCommandArgument.m_bodyUniqueId;
690 InternalPhysXBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId);
691 if (body && body->mArticulation)
692 {
693 physx::PxArticulationLink* physxLinks[64];
694 physx::PxU32 bufferSize = 64;
695 physx::PxU32 startIndex = 0;
696 int numLinks2 = body->mArticulation->getLinks(physxLinks, bufferSize, startIndex);
697 //http://gameworksdocs.nvidia.com/PhysX/4.0/documentation/PhysXGuide/Manual/Articulations.html
698
699
700 switch (clientCmd.m_sendDesiredStateCommandArgument.m_controlMode)
701 {
702 case CONTROL_MODE_VELOCITY:
703 {
704 if (m_data->m_verboseOutput)
705 {
706 b3Printf("Using CONTROL_MODE_VELOCITY");
707 }
708
709 int numMotors = 0;
710 //find the joint motors and apply the desired velocity and maximum force/torque
711 {
712 int dofIndex = 6; //skip the 3 linear + 3 angular degree of freedom entries of the base
713 for (int link = 1; link < numLinks2; link++)
714 {
715 int dofs = physxLinks[link]->getInboundJointDof();
716 physx::PxReal stiffness = 10.f;
717 physx::PxReal damping = 0.1f;
718 physx::PxReal forceLimit = PX_MAX_F32;
719
720 if (dofs == 1)
721 {
722 physx::PxArticulationJointReducedCoordinate* joint = static_cast<physx::PxArticulationJointReducedCoordinate*>(physxLinks[link]->getInboundJoint());
723 btScalar desiredVelocity = 0.f;
724 bool hasDesiredVelocity = false;
725 physx::PxReal stiffness = 10.f;
726
727 if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] & SIM_DESIRED_STATE_HAS_QDOT) != 0)
728 {
729 desiredVelocity = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[dofIndex];
730 btScalar kd = 0.1f;
731 if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] & SIM_DESIRED_STATE_HAS_KD) != 0)
732 {
733 kd = clientCmd.m_sendDesiredStateCommandArgument.m_Kd[dofIndex];
734 }
735 joint->setDriveVelocity(physx::PxArticulationAxis::eTWIST, desiredVelocity);
736 physx::PxReal damping = kd;
737 stiffness = 0;
738 joint->setDriveTarget(physx::PxArticulationAxis::eTWIST, 0.f);
739 physx::PxReal forceLimit = 1000000.f;
740 if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] & SIM_DESIRED_STATE_HAS_MAX_FORCE) != 0)
741 {
742 forceLimit = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex];
743 }
744 joint->setDrive(physx::PxArticulationAxis::eTWIST, stiffness, damping, forceLimit);
745 }
746 }
747 dofIndex += dofs;
748 }
749 }
750 break;
751 }
752
753 case CONTROL_MODE_POSITION_VELOCITY_PD:
754 {
755 if (m_data->m_verboseOutput)
756 {
757 b3Printf("Using CONTROL_MODE_VELOCITY");
758 }
759
760 int numMotors = 0;
761 //find the joint motors and apply the desired velocity and maximum force/torque
762 {
763 int dofIndex = 6; //skip the 3 linear + 3 angular degree of freedom entries of the base
764 int posIndex = 7; //skip 3 positional and 4 orientation (quaternion) positional degrees of freedom of the base
765 for (int link = 1; link < numLinks2; link++)
766 {
767 int dofs = physxLinks[link]->getInboundJointDof();
768 physx::PxReal stiffness = 10.f;
769 physx::PxReal damping = 0.1f;
770 physx::PxReal forceLimit = PX_MAX_F32;
771
772
773 if (dofs == 1)
774 {
775 physx::PxArticulationJointReducedCoordinate* joint = static_cast<physx::PxArticulationJointReducedCoordinate*>(physxLinks[link]->getInboundJoint());
776 btScalar desiredVelocity = 0.f;
777 bool hasDesiredVelocity = false;
778 physx::PxReal stiffness = 10.f;
779 btScalar kd = 0.1f;
780 btScalar kp = 0.f;
781
782 if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] & SIM_DESIRED_STATE_HAS_QDOT) != 0)
783 {
784 desiredVelocity = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[dofIndex];
785 }
786
787 btScalar desiredPosition = 0.f;
788 if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[posIndex] & SIM_DESIRED_STATE_HAS_Q) != 0)
789 {
790 desiredPosition = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[posIndex];
791
792
793 if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] & SIM_DESIRED_STATE_HAS_KD) != 0)
794 {
795 kd = clientCmd.m_sendDesiredStateCommandArgument.m_Kd[dofIndex];
796 }
797 physx::PxReal damping = kd;
798
799 if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] & SIM_DESIRED_STATE_HAS_KP) != 0)
800 {
801 kp = clientCmd.m_sendDesiredStateCommandArgument.m_Kp[dofIndex];
802 stiffness = kp;
803 }
804
805 joint->setDriveVelocity(physx::PxArticulationAxis::eTWIST, desiredVelocity);
806
807
808
809 physx::PxReal forceLimit = 1000000.f;
810 if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] & SIM_DESIRED_STATE_HAS_MAX_FORCE) != 0)
811 {
812 forceLimit = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex];
813 }
814 bool isAcceleration = false;
815
816 joint->setDriveTarget(physx::PxArticulationAxis::eTWIST, desiredPosition);
817 joint->setDrive(physx::PxArticulationAxis::eTWIST, stiffness, damping, forceLimit, isAcceleration);
818 }
819 }
820
821 dofIndex += dofs;
822 posIndex += dofs;
823 }
824 }
825 break;
826 }
827 default:
828 {
829 }
830 }
831
832 }
833 serverStatusOut.m_type = CMD_DESIRED_STATE_RECEIVED_COMPLETED;
834 return hasStatus;
835 }
836
837
838
processChangeDynamicsInfoCommand(const struct SharedMemoryCommand & clientCmd,struct SharedMemoryStatus & serverStatusOut,char * bufferServerToClient,int bufferSizeInBytes)839 bool PhysXServerCommandProcessor::processChangeDynamicsInfoCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
840 {
841 bool hasStatus = true;
842 BT_PROFILE("CMD_CHANGE_DYNAMICS_INFO");
843
844 int bodyUniqueId = clientCmd.m_changeDynamicsInfoArgs.m_bodyUniqueId;
845 int linkIndex = clientCmd.m_changeDynamicsInfoArgs.m_linkIndex;
846 double mass = clientCmd.m_changeDynamicsInfoArgs.m_mass;
847 double lateralFriction = clientCmd.m_changeDynamicsInfoArgs.m_lateralFriction;
848 double spinningFriction = clientCmd.m_changeDynamicsInfoArgs.m_spinningFriction;
849 double rollingFriction = clientCmd.m_changeDynamicsInfoArgs.m_rollingFriction;
850 double restitution = clientCmd.m_changeDynamicsInfoArgs.m_restitution;
851 btVector3 newLocalInertiaDiagonal(clientCmd.m_changeDynamicsInfoArgs.m_localInertiaDiagonal[0],
852 clientCmd.m_changeDynamicsInfoArgs.m_localInertiaDiagonal[1],
853 clientCmd.m_changeDynamicsInfoArgs.m_localInertiaDiagonal[2]);
854
855 btAssert(bodyUniqueId >= 0);
856
857 InternalPhysXBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId);
858
859 if (body->mArticulation)
860 {
861
862 physx::PxArticulationLink* physxLinks[64];
863 physx::PxU32 bufferSize = 64;
864 physx::PxU32 startIndex = 0;
865 int physxLinkIndex = linkIndex + 1;
866
867 int numLinks2 = body->mArticulation->getLinks(physxLinks, bufferSize, startIndex);
868
869 if (physxLinkIndex >= 0 && physxLinkIndex < numLinks2)
870 {
871 if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_MASS)
872 {
873 physx::PxArticulationLink* childLink = physxLinks[physxLinkIndex];
874 physx::PxRigidBodyExt::updateMassAndInertia(*childLink, mass);
875 }
876 }
877 }
878 if (body->m_rigidDynamic)
879 {
880 //body->m_rigidDynamic->setMass(mass);
881 //also update inertia
882 if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_MASS)
883 {
884 physx::PxRigidBodyExt::updateMassAndInertia(*body->m_rigidDynamic, mass);
885 }
886 }
887 if (body->m_rigidStatic)
888 {
889
890 }
891 #if 0
892 if (body && body->m_multiBody)
893 {
894 btMultiBody* mb = body->m_multiBody;
895
896 if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ACTIVATION_STATE)
897 {
898 if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateWakeUp)
899 {
900 mb->wakeUp();
901 }
902 if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateSleep)
903 {
904 mb->goToSleep();
905 }
906 if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateEnableSleeping)
907 {
908 mb->setCanSleep(true);
909 }
910 if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateDisableSleeping)
911 {
912 mb->setCanSleep(false);
913 }
914 }
915
916 if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LINEAR_DAMPING)
917 {
918 mb->setLinearDamping(clientCmd.m_changeDynamicsInfoArgs.m_linearDamping);
919 }
920 if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ANGULAR_DAMPING)
921 {
922 mb->setAngularDamping(clientCmd.m_changeDynamicsInfoArgs.m_angularDamping);
923 }
924
925 if (linkIndex == -1)
926 {
927 if (mb->getBaseCollider())
928 {
929 if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_RESTITUTION)
930 {
931 mb->getBaseCollider()->setRestitution(restitution);
932 }
933
934 if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_CONTACT_STIFFNESS_AND_DAMPING)
935 {
936 mb->getBaseCollider()->setContactStiffnessAndDamping(clientCmd.m_changeDynamicsInfoArgs.m_contactStiffness, clientCmd.m_changeDynamicsInfoArgs.m_contactDamping);
937 }
938 if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LATERAL_FRICTION)
939 {
940 mb->getBaseCollider()->setFriction(lateralFriction);
941 }
942 if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_SPINNING_FRICTION)
943 {
944 mb->getBaseCollider()->setSpinningFriction(spinningFriction);
945 }
946 if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ROLLING_FRICTION)
947 {
948 mb->getBaseCollider()->setRollingFriction(rollingFriction);
949 }
950
951 if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_FRICTION_ANCHOR)
952 {
953 if (clientCmd.m_changeDynamicsInfoArgs.m_frictionAnchor)
954 {
955 mb->getBaseCollider()->setCollisionFlags(mb->getBaseCollider()->getCollisionFlags() | btCollisionObject::CF_HAS_FRICTION_ANCHOR);
956 }
957 else
958 {
959 mb->getBaseCollider()->setCollisionFlags(mb->getBaseCollider()->getCollisionFlags() & ~btCollisionObject::CF_HAS_FRICTION_ANCHOR);
960 }
961 }
962 }
963 if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_MASS)
964 {
965 mb->setBaseMass(mass);
966 if (mb->getBaseCollider() && mb->getBaseCollider()->getCollisionShape())
967 {
968 btVector3 localInertia;
969 mb->getBaseCollider()->getCollisionShape()->calculateLocalInertia(mass, localInertia);
970 mb->setBaseInertia(localInertia);
971 }
972
973 //handle switch from static/fixedBase to dynamic and vise-versa
974 if (mass > 0)
975 {
976 bool isDynamic = true;
977 if (mb->hasFixedBase())
978 {
979 int collisionFilterGroup = isDynamic ? int(btBroadphaseProxy::DefaultFilter) : int(btBroadphaseProxy::StaticFilter);
980 int collisionFilterMask = isDynamic ? int(btBroadphaseProxy::AllFilter) : int(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
981
982 m_data->m_dynamicsWorld->removeCollisionObject(mb->getBaseCollider());
983 int oldFlags = mb->getBaseCollider()->getCollisionFlags();
984 mb->getBaseCollider()->setCollisionFlags(oldFlags & ~btCollisionObject::CF_STATIC_OBJECT);
985 mb->setFixedBase(false);
986 m_data->m_dynamicsWorld->addCollisionObject(mb->getBaseCollider(), collisionFilterGroup, collisionFilterMask);
987
988 }
989 }
990 else
991 {
992 if (!mb->hasFixedBase())
993 {
994 bool isDynamic = false;
995 int collisionFilterGroup = isDynamic ? int(btBroadphaseProxy::DefaultFilter) : int(btBroadphaseProxy::StaticFilter);
996 int collisionFilterMask = isDynamic ? int(btBroadphaseProxy::AllFilter) : int(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
997 int oldFlags = mb->getBaseCollider()->getCollisionFlags();
998 mb->getBaseCollider()->setCollisionFlags(oldFlags | btCollisionObject::CF_STATIC_OBJECT);
999 m_data->m_dynamicsWorld->removeCollisionObject(mb->getBaseCollider());
1000 mb->setFixedBase(true);
1001 m_data->m_dynamicsWorld->addCollisionObject(mb->getBaseCollider(), collisionFilterGroup, collisionFilterMask);
1002 }
1003 }
1004
1005 }
1006 if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LOCAL_INERTIA_DIAGONAL)
1007 {
1008 mb->setBaseInertia(newLocalInertiaDiagonal);
1009 }
1010 }
1011 else
1012 {
1013 if (linkIndex >= 0 && linkIndex < mb->getNumLinks())
1014 {
1015 if (mb->getLinkCollider(linkIndex))
1016 {
1017 if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_RESTITUTION)
1018 {
1019 mb->getLinkCollider(linkIndex)->setRestitution(restitution);
1020 }
1021 if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_SPINNING_FRICTION)
1022 {
1023 mb->getLinkCollider(linkIndex)->setSpinningFriction(spinningFriction);
1024 }
1025 if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ROLLING_FRICTION)
1026 {
1027 mb->getLinkCollider(linkIndex)->setRollingFriction(rollingFriction);
1028 }
1029
1030 if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_FRICTION_ANCHOR)
1031 {
1032 if (clientCmd.m_changeDynamicsInfoArgs.m_frictionAnchor)
1033 {
1034 mb->getLinkCollider(linkIndex)->setCollisionFlags(mb->getLinkCollider(linkIndex)->getCollisionFlags() | btCollisionObject::CF_HAS_FRICTION_ANCHOR);
1035 }
1036 else
1037 {
1038 mb->getLinkCollider(linkIndex)->setCollisionFlags(mb->getLinkCollider(linkIndex)->getCollisionFlags() & ~btCollisionObject::CF_HAS_FRICTION_ANCHOR);
1039 }
1040 }
1041
1042 if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LATERAL_FRICTION)
1043 {
1044 mb->getLinkCollider(linkIndex)->setFriction(lateralFriction);
1045 }
1046
1047 if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_CONTACT_STIFFNESS_AND_DAMPING)
1048 {
1049 mb->getLinkCollider(linkIndex)->setContactStiffnessAndDamping(clientCmd.m_changeDynamicsInfoArgs.m_contactStiffness, clientCmd.m_changeDynamicsInfoArgs.m_contactDamping);
1050 }
1051 }
1052 if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_MASS)
1053 {
1054 mb->getLink(linkIndex).m_mass = mass;
1055 if (mb->getLinkCollider(linkIndex) && mb->getLinkCollider(linkIndex)->getCollisionShape())
1056 {
1057 btVector3 localInertia;
1058 mb->getLinkCollider(linkIndex)->getCollisionShape()->calculateLocalInertia(mass, localInertia);
1059 mb->getLink(linkIndex).m_inertiaLocal = localInertia;
1060 }
1061 }
1062 if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LOCAL_INERTIA_DIAGONAL)
1063 {
1064 mb->getLink(linkIndex).m_inertiaLocal = newLocalInertiaDiagonal;
1065 }
1066 }
1067 }
1068 }
1069 else
1070 {
1071 if (body && body->m_rigidBody)
1072 {
1073 if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ACTIVATION_STATE)
1074 {
1075 if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateEnableSleeping)
1076 {
1077 body->m_rigidBody->forceActivationState(ACTIVE_TAG);
1078 }
1079 if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateDisableSleeping)
1080 {
1081 body->m_rigidBody->forceActivationState(DISABLE_DEACTIVATION);
1082 }
1083 if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateWakeUp)
1084 {
1085 body->m_rigidBody->forceActivationState(ACTIVE_TAG);
1086 }
1087 if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateSleep)
1088 {
1089 body->m_rigidBody->forceActivationState(ISLAND_SLEEPING);
1090 }
1091 }
1092
1093 if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LINEAR_DAMPING)
1094 {
1095 btScalar angDamping = body->m_rigidBody->getAngularDamping();
1096 body->m_rigidBody->setDamping(clientCmd.m_changeDynamicsInfoArgs.m_linearDamping, angDamping);
1097 }
1098 if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ANGULAR_DAMPING)
1099 {
1100 btScalar linDamping = body->m_rigidBody->getLinearDamping();
1101 body->m_rigidBody->setDamping(linDamping, clientCmd.m_changeDynamicsInfoArgs.m_angularDamping);
1102 }
1103
1104 if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_CONTACT_STIFFNESS_AND_DAMPING)
1105 {
1106 body->m_rigidBody->setContactStiffnessAndDamping(clientCmd.m_changeDynamicsInfoArgs.m_contactStiffness, clientCmd.m_changeDynamicsInfoArgs.m_contactDamping);
1107 }
1108 if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_RESTITUTION)
1109 {
1110 body->m_rigidBody->setRestitution(restitution);
1111 }
1112 if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LATERAL_FRICTION)
1113 {
1114 body->m_rigidBody->setFriction(lateralFriction);
1115 }
1116 if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_SPINNING_FRICTION)
1117 {
1118 body->m_rigidBody->setSpinningFriction(spinningFriction);
1119 }
1120 if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ROLLING_FRICTION)
1121 {
1122 body->m_rigidBody->setRollingFriction(rollingFriction);
1123 }
1124
1125 if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_FRICTION_ANCHOR)
1126 {
1127 if (clientCmd.m_changeDynamicsInfoArgs.m_frictionAnchor)
1128 {
1129 body->m_rigidBody->setCollisionFlags(body->m_rigidBody->getCollisionFlags() | btCollisionObject::CF_HAS_FRICTION_ANCHOR);
1130 }
1131 else
1132 {
1133 body->m_rigidBody->setCollisionFlags(body->m_rigidBody->getCollisionFlags() & ~btCollisionObject::CF_HAS_FRICTION_ANCHOR);
1134 }
1135 }
1136
1137 if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_MASS)
1138 {
1139 btVector3 localInertia;
1140 if (body->m_rigidBody->getCollisionShape())
1141 {
1142 body->m_rigidBody->getCollisionShape()->calculateLocalInertia(mass, localInertia);
1143 }
1144 body->m_rigidBody->setMassProps(mass, localInertia);
1145 }
1146 if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LOCAL_INERTIA_DIAGONAL)
1147 {
1148 btScalar orgMass = body->m_rigidBody->getInvMass();
1149 if (orgMass > 0)
1150 {
1151 body->m_rigidBody->setMassProps(mass, newLocalInertiaDiagonal);
1152 }
1153 }
1154
1155 if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_CONTACT_PROCESSING_THRESHOLD)
1156 {
1157 body->m_rigidBody->setContactProcessingThreshold(clientCmd.m_changeDynamicsInfoArgs.m_contactProcessingThreshold);
1158 }
1159
1160 if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_CCD_SWEPT_SPHERE_RADIUS)
1161 {
1162 body->m_rigidBody->setCcdSweptSphereRadius(clientCmd.m_changeDynamicsInfoArgs.m_ccdSweptSphereRadius);
1163 //for a given sphere radius, use a motion threshold of half the radius, before the ccd algorithm is enabled
1164 body->m_rigidBody->setCcdMotionThreshold(clientCmd.m_changeDynamicsInfoArgs.m_ccdSweptSphereRadius / 2.);
1165 }
1166 }
1167 }
1168
1169 b3Notification notification;
1170 notification.m_notificationType = LINK_DYNAMICS_CHANGED;
1171 notification.m_linkArgs.m_bodyUniqueId = bodyUniqueId;
1172 notification.m_linkArgs.m_linkIndex = linkIndex;
1173 m_data->m_pluginManager.addNotification(notification);
1174
1175
1176 #endif
1177 SharedMemoryStatus& serverCmd = serverStatusOut;
1178 serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED;
1179
1180
1181 return hasStatus;
1182 }
1183
processRequestPhysicsSimulationParametersCommand(const struct SharedMemoryCommand & clientCmd,struct SharedMemoryStatus & serverStatusOut,char * bufferServerToClient,int bufferSizeInBytes)1184 bool PhysXServerCommandProcessor::processRequestPhysicsSimulationParametersCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
1185 {
1186 bool hasStatus = true;
1187 SharedMemoryStatus& serverCmd = serverStatusOut;
1188 serverCmd.m_type = CMD_REQUEST_PHYSICS_SIMULATION_PARAMETERS_COMPLETED;
1189
1190 serverCmd.m_simulationParameterResultArgs.m_allowedCcdPenetration = 0;// m_data->m_dynamicsWorld->getDispatchInfo().m_allowedCcdPenetration;
1191 serverCmd.m_simulationParameterResultArgs.m_collisionFilterMode = 0;// m_data->m_broadphaseCollisionFilterCallback->m_filterMode;
1192 serverCmd.m_simulationParameterResultArgs.m_deltaTime = m_data->m_physicsDeltaTime;
1193 serverCmd.m_simulationParameterResultArgs.m_contactBreakingThreshold = 0;// gContactBreakingThreshold;
1194 serverCmd.m_simulationParameterResultArgs.m_contactSlop = 0;// m_data->m_dynamicsWorld->getSolverInfo().m_linearSlop;
1195 serverCmd.m_simulationParameterResultArgs.m_enableSAT = 0;// m_data->m_dynamicsWorld->getDispatchInfo().m_enableSatConvex;
1196
1197 serverCmd.m_simulationParameterResultArgs.m_defaultGlobalCFM = 0;// m_data->m_dynamicsWorld->getSolverInfo().m_globalCfm;
1198 serverCmd.m_simulationParameterResultArgs.m_defaultContactERP = 0;// m_data->m_dynamicsWorld->getSolverInfo().m_erp2;
1199 serverCmd.m_simulationParameterResultArgs.m_defaultNonContactERP = 0;// m_data->m_dynamicsWorld->getSolverInfo().m_erp;
1200
1201 serverCmd.m_simulationParameterResultArgs.m_deterministicOverlappingPairs = 0;// m_data->m_dynamicsWorld->getDispatchInfo().m_deterministicOverlappingPairs;
1202 serverCmd.m_simulationParameterResultArgs.m_enableConeFriction = 0;// (m_data->m_dynamicsWorld->getSolverInfo().m_solverMode & SOLVER_DISABLE_IMPLICIT_CONE_FRICTION) ? 0 : 1;
1203 serverCmd.m_simulationParameterResultArgs.m_enableFileCaching = 0;// b3IsFileCachingEnabled();
1204 serverCmd.m_simulationParameterResultArgs.m_frictionCFM = 0;// m_data->m_dynamicsWorld->getSolverInfo().m_frictionCFM;
1205 serverCmd.m_simulationParameterResultArgs.m_frictionERP = 0;// m_data->m_dynamicsWorld->getSolverInfo().m_frictionERP;
1206
1207 physx::PxVec3 grav = m_data->m_scene->getGravity();
1208
1209 serverCmd.m_simulationParameterResultArgs.m_gravityAcceleration[0] = grav.x;
1210 serverCmd.m_simulationParameterResultArgs.m_gravityAcceleration[1] = grav.y;
1211 serverCmd.m_simulationParameterResultArgs.m_gravityAcceleration[2] = grav.z;
1212 serverCmd.m_simulationParameterResultArgs.m_internalSimFlags = 0;
1213 serverCmd.m_simulationParameterResultArgs.m_jointFeedbackMode = 0;
1214
1215 serverCmd.m_simulationParameterResultArgs.m_numSimulationSubSteps = 0;// m_data->m_numSimulationSubSteps;
1216 serverCmd.m_simulationParameterResultArgs.m_numSolverIterations = 0;// m_data->m_dynamicsWorld->getSolverInfo().m_numIterations;
1217 serverCmd.m_simulationParameterResultArgs.m_restitutionVelocityThreshold = 0;// m_data->m_dynamicsWorld->getSolverInfo().m_restitutionVelocityThreshold;
1218
1219 serverCmd.m_simulationParameterResultArgs.m_solverResidualThreshold = 0;// m_data->m_dynamicsWorld->getSolverInfo().m_leastSquaresResidualThreshold;
1220 serverCmd.m_simulationParameterResultArgs.m_splitImpulsePenetrationThreshold = 0;// m_data->m_dynamicsWorld->getSolverInfo().m_splitImpulsePenetrationThreshold;
1221 serverCmd.m_simulationParameterResultArgs.m_useRealTimeSimulation = 0;// m_data->m_useRealTimeSimulation;
1222 serverCmd.m_simulationParameterResultArgs.m_useSplitImpulse = 0;// m_data->m_dynamicsWorld->getSolverInfo().m_splitImpulse;
1223
1224 return hasStatus;
1225 }
1226
processRequestContactpointInformationCommand(const struct SharedMemoryCommand & clientCmd,struct SharedMemoryStatus & serverStatusOut,char * bufferServerToClient,int bufferSizeInBytes)1227 bool PhysXServerCommandProcessor::processRequestContactpointInformationCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
1228 {
1229 SharedMemoryStatus& serverCmd = serverStatusOut;
1230 int totalBytesPerContact = sizeof(b3ContactPointData);
1231 int contactPointStorage = bufferSizeInBytes / totalBytesPerContact - 1;
1232
1233 b3ContactPointData* contactData = (b3ContactPointData*)bufferServerToClient;
1234
1235 int startContactPointIndex = clientCmd.m_requestContactPointArguments.m_startingContactPointIndex;
1236 int numContactPointBatch = btMin(int(m_data->m_contactPoints.size()), contactPointStorage);
1237
1238 int endContactPointIndex = startContactPointIndex + numContactPointBatch;
1239 serverCmd.m_sendContactPointArgs.m_numContactPointsCopied = 0;
1240 for (int i = startContactPointIndex; i < endContactPointIndex; i++)
1241 {
1242 const b3ContactPointData& srcPt = m_data->m_contactPoints[i];
1243 b3ContactPointData& destPt = contactData[serverCmd.m_sendContactPointArgs.m_numContactPointsCopied];
1244 destPt = srcPt;
1245 serverCmd.m_sendContactPointArgs.m_numContactPointsCopied++;
1246 }
1247 serverCmd.m_sendContactPointArgs.m_startingContactPointIndex = startContactPointIndex;
1248 serverCmd.m_sendContactPointArgs.m_numRemainingContactPoints = m_data->m_contactPoints.size() - startContactPointIndex - serverCmd.m_sendContactPointArgs.m_numContactPointsCopied;
1249 serverCmd.m_numDataStreamBytes = totalBytesPerContact * serverCmd.m_sendContactPointArgs.m_numContactPointsCopied;
1250 serverCmd.m_type = CMD_CONTACT_POINT_INFORMATION_COMPLETED;
1251
1252
1253 return true;
1254 }
1255
processCreateCollisionShapeCommand(const struct SharedMemoryCommand & clientCmd,struct SharedMemoryStatus & serverStatusOut,char * bufferServerToClient,int bufferSizeInBytes)1256 bool PhysXServerCommandProcessor::processCreateCollisionShapeCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
1257 {
1258 return false;
1259 }
1260
1261
processSetAdditionalSearchPathCommand(const struct SharedMemoryCommand & clientCmd,struct SharedMemoryStatus & serverStatusOut,char * bufferServerToClient,int bufferSizeInBytes)1262 bool PhysXServerCommandProcessor::processSetAdditionalSearchPathCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
1263 {
1264 bool hasStatus = true;
1265
1266 BT_PROFILE("CMD_SET_ADDITIONAL_SEARCH_PATH");
1267 b3ResourcePath::setAdditionalSearchPath(clientCmd.m_searchPathArgs.m_path);
1268 serverStatusOut.m_type = CMD_CLIENT_COMMAND_COMPLETED;
1269 return hasStatus;
1270 }
1271
processUserDebugDrawCommand(const struct SharedMemoryCommand & clientCmd,struct SharedMemoryStatus & serverStatusOut,char * bufferServerToClient,int bufferSizeInBytes)1272 bool PhysXServerCommandProcessor::processUserDebugDrawCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
1273 {
1274 ///dummy, so commands don't fail
1275 bool hasStatus = true;
1276 BT_PROFILE("CMD_USER_DEBUG_DRAW");
1277 SharedMemoryStatus& serverCmd = serverStatusOut;
1278 int uid = 0;
1279 serverCmd.m_userDebugDrawArgs.m_debugItemUniqueId = uid;
1280 serverCmd.m_type = CMD_USER_DEBUG_DRAW_FAILED;
1281
1282 if (clientCmd.m_updateFlags & USER_DEBUG_ADD_PARAMETER)
1283 {
1284 int uid = m_data->m_userDebugParametersUid++;
1285 double value = clientCmd.m_userDebugDrawArgs.m_startValue;
1286 m_data->m_userDebugParameters.insert(uid, value);
1287 serverCmd.m_userDebugDrawArgs.m_debugItemUniqueId = uid;
1288 serverCmd.m_type = CMD_USER_DEBUG_DRAW_COMPLETED;
1289 }
1290 if (clientCmd.m_updateFlags & USER_DEBUG_READ_PARAMETER)
1291 {
1292 double* valPtr = m_data->m_userDebugParameters[clientCmd.m_userDebugDrawArgs.m_itemUniqueId];
1293 if (valPtr)
1294 {
1295 serverCmd.m_userDebugDrawArgs.m_parameterValue = *valPtr;
1296 serverCmd.m_type = CMD_USER_DEBUG_DRAW_PARAMETER_COMPLETED;
1297 }
1298 }
1299 if (clientCmd.m_updateFlags & USER_DEBUG_REMOVE_ALL)
1300 {
1301 m_data->m_userDebugParameters.clear();
1302 serverCmd.m_type = CMD_USER_DEBUG_DRAW_COMPLETED;
1303 }
1304 if (clientCmd.m_updateFlags & USER_DEBUG_REMOVE_ONE_ITEM)
1305 {
1306 m_data->m_userDebugParameters.remove(clientCmd.m_userDebugDrawArgs.m_itemUniqueId);
1307 serverCmd.m_type = CMD_USER_DEBUG_DRAW_COMPLETED;
1308 }
1309 return hasStatus;
1310
1311 }
1312
processCommand(const struct SharedMemoryCommand & clientCmd,struct SharedMemoryStatus & serverStatusOut,char * bufferServerToClient,int bufferSizeInBytes)1313 bool PhysXServerCommandProcessor::processCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
1314 {
1315 // BT_PROFILE("processCommand");
1316
1317 int sz = sizeof(SharedMemoryStatus);
1318 int sz2 = sizeof(SharedMemoryCommand);
1319
1320 bool hasStatus = true;
1321
1322 serverStatusOut.m_type = CMD_INVALID_STATUS;
1323 serverStatusOut.m_numDataStreamBytes = 0;
1324 serverStatusOut.m_dataStream = 0;
1325
1326 //consume the command
1327 switch (clientCmd.m_type)
1328 {
1329 case CMD_REQUEST_INTERNAL_DATA:
1330 {
1331 hasStatus = processRequestInternalDataCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
1332 break;
1333 };
1334
1335 case CMD_SYNC_BODY_INFO:
1336 {
1337 hasStatus = processSyncBodyInfoCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
1338 break;
1339 }
1340 case CMD_SYNC_USER_DATA:
1341 {
1342 hasStatus = processSyncUserDataCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
1343 break;
1344 }
1345 case CMD_REQUEST_BODY_INFO:
1346 {
1347 hasStatus = processRequestBodyInfoCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
1348 break;
1349 }
1350 case CMD_STEP_FORWARD_SIMULATION:
1351 {
1352 hasStatus = processForwardDynamicsCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
1353 break;
1354 }
1355
1356 case CMD_SEND_PHYSICS_SIMULATION_PARAMETERS:
1357 {
1358 hasStatus = processSendPhysicsParametersCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
1359 break;
1360 };
1361
1362 case CMD_REQUEST_ACTUAL_STATE:
1363 {
1364 hasStatus = processRequestActualStateCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
1365 break;
1366 }
1367
1368 case CMD_RESET_SIMULATION:
1369 {
1370 hasStatus = processResetSimulationCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
1371 break;
1372 }
1373
1374 case CMD_LOAD_URDF:
1375 {
1376 hasStatus = processLoadURDFCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
1377 break;
1378 }
1379 case CMD_CUSTOM_COMMAND:
1380 {
1381 hasStatus = processCustomCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
1382 break;
1383 }
1384 case CMD_STATE_LOGGING:
1385 {
1386 hasStatus = processStateLoggingCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
1387 break;
1388 }
1389
1390 case CMD_INIT_POSE:
1391 {
1392 hasStatus = processInitPoseCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
1393 break;
1394 }
1395
1396 case CMD_SEND_DESIRED_STATE:
1397 {
1398 hasStatus = processSendDesiredStateCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
1399 break;
1400 }
1401
1402 case CMD_CHANGE_DYNAMICS_INFO:
1403 {
1404 hasStatus = processChangeDynamicsInfoCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
1405 break;
1406 };
1407
1408 case CMD_REQUEST_PHYSICS_SIMULATION_PARAMETERS:
1409 {
1410 hasStatus = processRequestPhysicsSimulationParametersCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
1411 break;
1412 }
1413
1414 case CMD_REQUEST_CONTACT_POINT_INFORMATION:
1415 {
1416 hasStatus = processRequestContactpointInformationCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
1417 break;
1418 }
1419
1420 case CMD_CREATE_COLLISION_SHAPE:
1421 {
1422 hasStatus = processCreateCollisionShapeCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
1423 break;
1424 }
1425
1426 case CMD_SET_ADDITIONAL_SEARCH_PATH:
1427 {
1428 hasStatus = processSetAdditionalSearchPathCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
1429 break;
1430 }
1431
1432 case CMD_USER_DEBUG_DRAW:
1433 {
1434 hasStatus = processUserDebugDrawCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
1435 break;
1436 }
1437
1438 case CMD_REQUEST_CAMERA_IMAGE_DATA:
1439 {
1440 hasStatus = processRequestCameraImageCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
1441 break;
1442 }
1443
1444 #if 0
1445 case CMD_SET_VR_CAMERA_STATE:
1446 {
1447 hasStatus = processSetVRCameraStateCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
1448 break;
1449 }
1450 case CMD_REQUEST_VR_EVENTS_DATA:
1451 {
1452 hasStatus = processRequestVREventsCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
1453 break;
1454 };
1455 case CMD_REQUEST_MOUSE_EVENTS_DATA:
1456 {
1457 hasStatus = processRequestMouseEventsCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
1458 break;
1459 };
1460 case CMD_REQUEST_KEYBOARD_EVENTS_DATA:
1461 {
1462 hasStatus = processRequestKeyboardEventsCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
1463 break;
1464 };
1465
1466 case CMD_REQUEST_RAY_CAST_INTERSECTIONS:
1467 {
1468
1469 hasStatus = processRequestRaycastIntersectionsCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
1470 break;
1471 };
1472 case CMD_REQUEST_DEBUG_LINES:
1473 {
1474 hasStatus = processRequestDebugLinesCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
1475 break;
1476 }
1477
1478
1479 case CMD_REQUEST_BODY_INFO:
1480 {
1481 hasStatus = processRequestBodyInfoCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
1482 break;
1483 }
1484 case CMD_SAVE_WORLD:
1485 {
1486 hasStatus = processSaveWorldCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
1487 break;
1488 }
1489 case CMD_LOAD_SDF:
1490 {
1491 hasStatus = processLoadSDFCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
1492 break;
1493 }
1494
1495 case CMD_CREATE_VISUAL_SHAPE:
1496 {
1497 hasStatus = processCreateVisualShapeCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
1498 break;
1499 }
1500 case CMD_CREATE_MULTI_BODY:
1501 {
1502 hasStatus = processCreateMultiBodyCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
1503 break;
1504 }
1505
1506
1507 case CMD_LOAD_MJCF:
1508 {
1509 hasStatus = processLoadMJCFCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
1510 break;
1511 }
1512
1513 case CMD_LOAD_SOFT_BODY:
1514 {
1515 hasStatus = processLoadSoftBodyCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
1516 break;
1517 }
1518 case CMD_CREATE_SENSOR:
1519 {
1520 hasStatus = processCreateSensorCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
1521 break;
1522 }
1523 case CMD_PROFILE_TIMING:
1524 {
1525 hasStatus = processProfileTimingCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
1526 break;
1527 }
1528
1529
1530 case CMD_REQUEST_COLLISION_INFO:
1531 {
1532 hasStatus = processRequestCollisionInfoCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
1533 break;
1534 }
1535
1536
1537
1538
1539 case CMD_GET_DYNAMICS_INFO:
1540 {
1541 hasStatus = processGetDynamicsInfoCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
1542 break;
1543 }
1544
1545
1546
1547
1548 case CMD_CREATE_RIGID_BODY:
1549 {
1550 hasStatus = processCreateRigidBodyCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
1551 break;
1552 }
1553 case CMD_CREATE_BOX_COLLISION_SHAPE:
1554 {
1555 //for backward compatibility, CMD_CREATE_BOX_COLLISION_SHAPE is the same as CMD_CREATE_RIGID_BODY
1556 hasStatus = processCreateRigidBodyCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
1557 break;
1558 }
1559 case CMD_PICK_BODY:
1560 {
1561 hasStatus = processPickBodyCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
1562 break;
1563 }
1564 case CMD_MOVE_PICKED_BODY:
1565 {
1566 hasStatus = processMovePickedBodyCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
1567 break;
1568 }
1569 case CMD_REMOVE_PICKING_CONSTRAINT_BODY:
1570 {
1571 hasStatus = processRemovePickingConstraintCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
1572 break;
1573 }
1574 case CMD_REQUEST_AABB_OVERLAP:
1575 {
1576 hasStatus = processRequestAabbOverlapCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
1577 break;
1578 }
1579 case CMD_REQUEST_OPENGL_VISUALIZER_CAMERA:
1580 {
1581 hasStatus = processRequestOpenGLVisualizeCameraCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
1582 break;
1583 }
1584 case CMD_CONFIGURE_OPENGL_VISUALIZER:
1585 {
1586 hasStatus = processConfigureOpenGLVisualizerCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
1587 break;
1588 }
1589
1590 case CMD_CALCULATE_INVERSE_DYNAMICS:
1591 {
1592 hasStatus = processInverseDynamicsCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
1593 break;
1594 }
1595 case CMD_CALCULATE_JACOBIAN:
1596 {
1597 hasStatus = processCalculateJacobianCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
1598 break;
1599 }
1600 case CMD_CALCULATE_MASS_MATRIX:
1601 {
1602 hasStatus = processCalculateMassMatrixCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
1603 break;
1604 }
1605 case CMD_APPLY_EXTERNAL_FORCE:
1606 {
1607 hasStatus = processApplyExternalForceCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
1608 break;
1609 }
1610 case CMD_REMOVE_BODY:
1611 {
1612 hasStatus = processRemoveBodyCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
1613 break;
1614 }
1615 case CMD_USER_CONSTRAINT:
1616 {
1617 hasStatus = processCreateUserConstraintCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
1618 break;
1619 }
1620 case CMD_CALCULATE_INVERSE_KINEMATICS:
1621 {
1622 hasStatus = processCalculateInverseKinematicsCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
1623 break;
1624 }
1625 case CMD_REQUEST_VISUAL_SHAPE_INFO:
1626 {
1627 hasStatus = processRequestVisualShapeInfoCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
1628 break;
1629 }
1630 case CMD_REQUEST_COLLISION_SHAPE_INFO:
1631 {
1632 hasStatus = processRequestCollisionShapeInfoCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
1633 break;
1634 }
1635 case CMD_UPDATE_VISUAL_SHAPE:
1636 {
1637 hasStatus = processUpdateVisualShapeCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
1638 break;
1639 }
1640 case CMD_CHANGE_TEXTURE:
1641 {
1642 hasStatus = processChangeTextureCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
1643 break;
1644 }
1645 case CMD_LOAD_TEXTURE:
1646 {
1647 hasStatus = processLoadTextureCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
1648 break;
1649 }
1650 case CMD_RESTORE_STATE:
1651 {
1652 hasStatus = processRestoreStateCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
1653 break;
1654 }
1655
1656 case CMD_SAVE_STATE:
1657 {
1658 hasStatus = processSaveStateCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
1659 break;
1660 }
1661
1662 case CMD_LOAD_BULLET:
1663 {
1664 hasStatus = processLoadBulletCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
1665 break;
1666 }
1667 case CMD_SAVE_BULLET:
1668 {
1669 hasStatus = processSaveBulletCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
1670 break;
1671 }
1672 case CMD_LOAD_MJCF:
1673 {
1674 hasStatus = processLoadMJCFCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
1675 break;
1676 }
1677
1678 case CMD_REQUEST_USER_DATA:
1679 {
1680 hasStatus = processRequestUserDataCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
1681 break;
1682 }
1683 case CMD_ADD_USER_DATA:
1684 {
1685 hasStatus = processAddUserDataCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
1686 break;
1687 }
1688 case CMD_REMOVE_USER_DATA:
1689 {
1690 hasStatus = processRemoveUserDataCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
1691 break;
1692 }
1693 #endif
1694
1695 default:
1696 {
1697 BT_PROFILE("CMD_UNKNOWN");
1698 printf("Unknown command encountered: %d", clientCmd.m_type);
1699 SharedMemoryStatus& serverCmd = serverStatusOut;
1700 serverCmd.m_type = CMD_UNKNOWN_COMMAND_FLUSHED;
1701 hasStatus = true;
1702 }
1703 };
1704
1705 return hasStatus;
1706 }
1707
1708
1709
processRequestCameraImageCommand(const struct SharedMemoryCommand & clientCmd,struct SharedMemoryStatus & serverStatusOut,char * bufferServerToClient,int bufferSizeInBytes)1710 bool PhysXServerCommandProcessor::processRequestCameraImageCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
1711 {
1712 bool hasStatus = true;
1713 BT_PROFILE("CMD_REQUEST_CAMERA_IMAGE_DATA");
1714 int startPixelIndex = clientCmd.m_requestPixelDataArguments.m_startPixelIndex;
1715 int width = clientCmd.m_requestPixelDataArguments.m_pixelWidth;
1716 int height = clientCmd.m_requestPixelDataArguments.m_pixelHeight;
1717 int numPixelsCopied = 0;
1718
1719 int oldWidth;
1720 int oldHeight;
1721 m_data->m_pluginManager.getRenderInterface()->getWidthAndHeight(oldWidth, oldHeight);
1722
1723 serverStatusOut.m_type = CMD_CAMERA_IMAGE_FAILED;
1724
1725 if ((clientCmd.m_requestPixelDataArguments.m_startPixelIndex == 0) &&
1726 (clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_PIXEL_WIDTH_HEIGHT) != 0)
1727 {
1728 if (m_data->m_pluginManager.getRenderInterface())
1729 {
1730
1731 m_data->m_pluginManager.getRenderInterface()->setWidthAndHeight(clientCmd.m_requestPixelDataArguments.m_pixelWidth,
1732 clientCmd.m_requestPixelDataArguments.m_pixelHeight);
1733 }
1734 }
1735 int flags = 0;
1736 if (clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_HAS_FLAGS)
1737 {
1738 flags = clientCmd.m_requestPixelDataArguments.m_flags;
1739 }
1740 if (m_data->m_pluginManager.getRenderInterface())
1741 {
1742 m_data->m_pluginManager.getRenderInterface()->setFlags(flags);
1743 }
1744
1745 int numTotalPixels = width * height;
1746 int numRemainingPixels = numTotalPixels - startPixelIndex;
1747
1748 if (numRemainingPixels > 0)
1749 {
1750 int totalBytesPerPixel = 4 + 4 + 4; //4 for rgb, 4 for depth, 4 for segmentation mask
1751 int maxNumPixels = bufferSizeInBytes / totalBytesPerPixel - 1;
1752 unsigned char* pixelRGBA = (unsigned char*)bufferServerToClient;
1753 int numRequestedPixels = btMin(maxNumPixels, numRemainingPixels);
1754
1755 float* depthBuffer = (float*)(bufferServerToClient + numRequestedPixels * 4);
1756 int* segmentationMaskBuffer = (int*)(bufferServerToClient + numRequestedPixels * 8);
1757
1758 serverStatusOut.m_numDataStreamBytes = numRequestedPixels * totalBytesPerPixel;
1759 float viewMat[16];
1760 float projMat[16];
1761 float projTextureViewMat[16];
1762 float projTextureProjMat[16];
1763
1764 if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES) == 0)
1765 {
1766 b3OpenGLVisualizerCameraInfo tmpCamResult;
1767 bool result = m_data->m_pluginManager.getRenderInterface()->getCameraInfo(
1768 &tmpCamResult.m_width,
1769 &tmpCamResult.m_height,
1770 tmpCamResult.m_viewMatrix,
1771 tmpCamResult.m_projectionMatrix,
1772 tmpCamResult.m_camUp,
1773 tmpCamResult.m_camForward,
1774 tmpCamResult.m_horizontal,
1775 tmpCamResult.m_vertical,
1776 &tmpCamResult.m_yaw,
1777 &tmpCamResult.m_pitch,
1778 &tmpCamResult.m_dist,
1779 tmpCamResult.m_target);
1780 if (result)
1781 {
1782 for (int i = 0; i < 16; i++)
1783 {
1784 viewMat[i] = tmpCamResult.m_viewMatrix[i];
1785 projMat[i] = tmpCamResult.m_projectionMatrix[i];
1786 }
1787 }
1788 else
1789 {
1790 //failed
1791 m_data->m_pluginManager.getRenderInterface()->setWidthAndHeight(oldWidth, oldHeight);
1792 return hasStatus;
1793 }
1794 }
1795 else
1796 {
1797 for (int i = 0; i < 16; i++)
1798 {
1799 viewMat[i] = clientCmd.m_requestPixelDataArguments.m_viewMatrix[i];
1800 projMat[i] = clientCmd.m_requestPixelDataArguments.m_projectionMatrix[i];
1801 }
1802 }
1803
1804 {
1805 if (m_data->m_pluginManager.getRenderInterface())
1806 {
1807 if (clientCmd.m_requestPixelDataArguments.m_startPixelIndex == 0)
1808 {
1809 // printf("-------------------------------\nRendering\n");
1810
1811 if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_LIGHT_DIRECTION) != 0)
1812 {
1813 m_data->m_pluginManager.getRenderInterface()->setLightDirection(clientCmd.m_requestPixelDataArguments.m_lightDirection[0], clientCmd.m_requestPixelDataArguments.m_lightDirection[1], clientCmd.m_requestPixelDataArguments.m_lightDirection[2]);
1814 }
1815
1816 if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_LIGHT_COLOR) != 0)
1817 {
1818 m_data->m_pluginManager.getRenderInterface()->setLightColor(clientCmd.m_requestPixelDataArguments.m_lightColor[0], clientCmd.m_requestPixelDataArguments.m_lightColor[1], clientCmd.m_requestPixelDataArguments.m_lightColor[2]);
1819 }
1820
1821 if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_LIGHT_DISTANCE) != 0)
1822 {
1823 m_data->m_pluginManager.getRenderInterface()->setLightDistance(clientCmd.m_requestPixelDataArguments.m_lightDistance);
1824 }
1825
1826 if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_SHADOW) != 0)
1827 {
1828 m_data->m_pluginManager.getRenderInterface()->setShadow((clientCmd.m_requestPixelDataArguments.m_hasShadow != 0));
1829 }
1830
1831 if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_AMBIENT_COEFF) != 0)
1832 {
1833 m_data->m_pluginManager.getRenderInterface()->setLightAmbientCoeff(clientCmd.m_requestPixelDataArguments.m_lightAmbientCoeff);
1834 }
1835
1836 if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_DIFFUSE_COEFF) != 0)
1837 {
1838 m_data->m_pluginManager.getRenderInterface()->setLightDiffuseCoeff(clientCmd.m_requestPixelDataArguments.m_lightDiffuseCoeff);
1839 }
1840
1841 if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_SPECULAR_COEFF) != 0)
1842 {
1843 m_data->m_pluginManager.getRenderInterface()->setLightSpecularCoeff(clientCmd.m_requestPixelDataArguments.m_lightSpecularCoeff);
1844 }
1845
1846
1847 if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES) != 0)
1848 {
1849 m_data->m_pluginManager.getRenderInterface()->render(
1850 clientCmd.m_requestPixelDataArguments.m_viewMatrix,
1851 clientCmd.m_requestPixelDataArguments.m_projectionMatrix);
1852 }
1853 else
1854 {
1855 b3OpenGLVisualizerCameraInfo tmpCamResult;
1856 bool result = m_data->m_pluginManager.getRenderInterface()->getCameraInfo(
1857 &tmpCamResult.m_width,
1858 &tmpCamResult.m_height,
1859 tmpCamResult.m_viewMatrix,
1860 tmpCamResult.m_projectionMatrix,
1861 tmpCamResult.m_camUp,
1862 tmpCamResult.m_camForward,
1863 tmpCamResult.m_horizontal,
1864 tmpCamResult.m_vertical,
1865 &tmpCamResult.m_yaw,
1866 &tmpCamResult.m_pitch,
1867 &tmpCamResult.m_dist,
1868 tmpCamResult.m_target);
1869 if (result)
1870 {
1871 m_data->m_pluginManager.getRenderInterface()->render(tmpCamResult.m_viewMatrix, tmpCamResult.m_projectionMatrix);
1872 }
1873 else
1874 {
1875 m_data->m_pluginManager.getRenderInterface()->render();
1876 }
1877 }
1878 }
1879 }
1880
1881 if (m_data->m_pluginManager.getRenderInterface())
1882 {
1883 if ((flags & ER_USE_PROJECTIVE_TEXTURE) != 0)
1884 {
1885 m_data->m_pluginManager.getRenderInterface()->setProjectiveTexture(true);
1886 if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_HAS_PROJECTIVE_TEXTURE_MATRICES) != 0)
1887 {
1888 for (int i = 0; i < 16; i++)
1889 {
1890 projTextureViewMat[i] = clientCmd.m_requestPixelDataArguments.m_projectiveTextureViewMatrix[i];
1891 projTextureProjMat[i] = clientCmd.m_requestPixelDataArguments.m_projectiveTextureProjectionMatrix[i];
1892 }
1893 }
1894 else // If no specified matrices for projective texture, then use the camera matrices.
1895 {
1896 for (int i = 0; i < 16; i++)
1897 {
1898 projTextureViewMat[i] = viewMat[i];
1899 projTextureProjMat[i] = projMat[i];
1900 }
1901 }
1902 m_data->m_pluginManager.getRenderInterface()->setProjectiveTextureMatrices(projTextureViewMat, projTextureProjMat);
1903 }
1904 else
1905 {
1906 m_data->m_pluginManager.getRenderInterface()->setProjectiveTexture(false);
1907 }
1908
1909 if ((flags & ER_NO_SEGMENTATION_MASK) != 0)
1910 {
1911 segmentationMaskBuffer = 0;
1912 }
1913
1914 m_data->m_pluginManager.getRenderInterface()->copyCameraImageData(pixelRGBA, numRequestedPixels,
1915 depthBuffer, numRequestedPixels,
1916 segmentationMaskBuffer, numRequestedPixels,
1917 startPixelIndex, &width, &height, &numPixelsCopied);
1918 m_data->m_pluginManager.getRenderInterface()->setProjectiveTexture(false);
1919 }
1920
1921 #if 0
1922 m_data->m_guiHelper->debugDisplayCameraImageData(clientCmd.m_requestPixelDataArguments.m_viewMatrix,
1923 clientCmd.m_requestPixelDataArguments.m_projectionMatrix, pixelRGBA, numRequestedPixels,
1924 depthBuffer, numRequestedPixels,
1925 segmentationMaskBuffer, numRequestedPixels,
1926 startPixelIndex, width, height, &numPixelsCopied);
1927 #endif
1928 }
1929
1930 //each pixel takes 4 RGBA values and 1 float = 8 bytes
1931 }
1932 else
1933 {
1934 }
1935
1936 m_data->m_pluginManager.getRenderInterface()->setWidthAndHeight(oldWidth, oldHeight);
1937 serverStatusOut.m_type = CMD_CAMERA_IMAGE_COMPLETED;
1938
1939 serverStatusOut.m_sendPixelDataArguments.m_numPixelsCopied = numPixelsCopied;
1940 serverStatusOut.m_sendPixelDataArguments.m_numRemainingPixels = numRemainingPixels - numPixelsCopied;
1941 serverStatusOut.m_sendPixelDataArguments.m_startingPixelIndex = startPixelIndex;
1942 serverStatusOut.m_sendPixelDataArguments.m_imageWidth = width;
1943 serverStatusOut.m_sendPixelDataArguments.m_imageHeight = height;
1944 return hasStatus;
1945
1946 }
1947
processRequestInternalDataCommand(const struct SharedMemoryCommand & clientCmd,struct SharedMemoryStatus & serverStatusOut,char * bufferServerToClient,int bufferSizeInBytes)1948 bool PhysXServerCommandProcessor::processRequestInternalDataCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
1949 {
1950 bool hasStatus = true;
1951 BT_PROFILE("CMD_REQUEST_INTERNAL_DATA");
1952 SharedMemoryStatus& serverCmd = serverStatusOut;
1953 serverCmd.m_type = CMD_REQUEST_INTERNAL_DATA_COMPLETED;
1954 serverCmd.m_numDataStreamBytes = 0;
1955 return hasStatus;
1956 }
1957
processSyncBodyInfoCommand(const struct SharedMemoryCommand & clientCmd,struct SharedMemoryStatus & serverStatusOut,char * bufferServerToClient,int bufferSizeInBytes)1958 bool PhysXServerCommandProcessor::processSyncBodyInfoCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
1959 {
1960 bool hasStatus = true;
1961 BT_PROFILE("CMD_SYNC_BODY_INFO");
1962 int actualNumBodies = 0;
1963 serverStatusOut.m_sdfLoadedArgs.m_numBodies = 0;
1964 serverStatusOut.m_sdfLoadedArgs.m_numUserConstraints = 0;
1965 serverStatusOut.m_type = CMD_SYNC_BODY_INFO_COMPLETED;
1966 return hasStatus;
1967 }
1968
processSyncUserDataCommand(const struct SharedMemoryCommand & clientCmd,struct SharedMemoryStatus & serverStatusOut,char * bufferServerToClient,int bufferSizeInBytes)1969 bool PhysXServerCommandProcessor::processSyncUserDataCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
1970 {
1971 bool hasStatus = true;
1972 BT_PROFILE("CMD_SYNC_USER_DATA");
1973 int numIdentifiers = 0;
1974 serverStatusOut.m_syncUserDataArgs.m_numUserDataIdentifiers = numIdentifiers;
1975 serverStatusOut.m_type = CMD_SYNC_USER_DATA_COMPLETED;
1976 return hasStatus;
1977 }
1978
1979 struct MyPhysXURDFImporter : public PhysXURDFImporter
1980 {
1981 b3PluginManager& m_pluginManager;
1982
MyPhysXURDFImporterMyPhysXURDFImporter1983 MyPhysXURDFImporter(struct CommonFileIOInterface* fileIO, double globalScaling, int flags, b3PluginManager& pluginManager)
1984 :PhysXURDFImporter(fileIO, globalScaling, flags),
1985 m_pluginManager(pluginManager)
1986 {
1987
1988 }
1989
convertLinkVisualShapes3MyPhysXURDFImporter1990 int convertLinkVisualShapes3(
1991 int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame,
1992 const UrdfLink* linkPtr, const UrdfModel* model,
1993 int collisionObjectUniqueId, int bodyUniqueId, struct CommonFileIOInterface* fileIO) const
1994 {
1995
1996 if (m_pluginManager.getRenderInterface())
1997 {
1998 int graphicsUniqueId = m_pluginManager.getRenderInterface()->convertVisualShapes(linkIndex, pathPrefix, localInertiaFrame, linkPtr, model, collisionObjectUniqueId, bodyUniqueId, fileIO);
1999 return graphicsUniqueId;
2000 }
2001 return 0;
2002 }
2003
2004 };
2005
2006
processLoadURDFCommand(const struct SharedMemoryCommand & clientCmd,struct SharedMemoryStatus & serverStatusOut,char * bufferServerToClient,int bufferSizeInBytes)2007 bool PhysXServerCommandProcessor::processLoadURDFCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
2008 {
2009 BT_PROFILE("CMD_LOAD_URDF");
2010 serverStatusOut.m_type = CMD_URDF_LOADING_FAILED;
2011 serverStatusOut.m_numDataStreamBytes = 0;
2012
2013 const UrdfArgs& urdfArgs = clientCmd.m_urdfArguments;
2014
2015 btAssert(m_data->m_foundation);
2016 if (!m_data->m_foundation)
2017 {
2018 b3Error("loadUrdf: No valid m_dynamicsWorld");
2019 return false;
2020 }
2021
2022 bool useMultiBody = (clientCmd.m_updateFlags & URDF_ARGS_USE_MULTIBODY) ? (urdfArgs.m_useMultiBody != 0) : true;
2023 bool useFixedBase = (clientCmd.m_updateFlags & URDF_ARGS_USE_FIXED_BASE) ? (urdfArgs.m_useFixedBase != 0) : false;
2024
2025 btScalar globalScaling = 1.f;
2026 if (clientCmd.m_updateFlags & URDF_ARGS_USE_GLOBAL_SCALING)
2027 {
2028 globalScaling = urdfArgs.m_globalScaling;
2029 }
2030
2031 b3BulletDefaultFileIO fileIO;
2032
2033 btVector3 initialPos(0, 0, 0);
2034 btQuaternion initialOrn(0, 0, 0, 1);
2035 if (clientCmd.m_updateFlags & URDF_ARGS_INITIAL_POSITION)
2036 {
2037 initialPos[0] = urdfArgs.m_initialPosition[0];
2038 initialPos[1] = urdfArgs.m_initialPosition[1];
2039 initialPos[2] = urdfArgs.m_initialPosition[2];
2040 }
2041 int urdfFlags = 0;
2042 if (clientCmd.m_updateFlags & URDF_ARGS_HAS_CUSTOM_URDF_FLAGS)
2043 {
2044 urdfFlags = urdfArgs.m_urdfFlags;
2045 }
2046 if (clientCmd.m_updateFlags & URDF_ARGS_INITIAL_ORIENTATION)
2047 {
2048 initialOrn[0] = urdfArgs.m_initialOrientation[0];
2049 initialOrn[1] = urdfArgs.m_initialOrientation[1];
2050 initialOrn[2] = urdfArgs.m_initialOrientation[2];
2051 initialOrn[3] = urdfArgs.m_initialOrientation[3];
2052 }
2053
2054
2055 MyPhysXURDFImporter u2p(&fileIO, globalScaling, urdfArgs.m_urdfFlags, m_data->m_pluginManager);
2056
2057
2058
2059 bool loadOk = u2p.loadURDF(urdfArgs.m_urdfFileName, useFixedBase);
2060
2061
2062 if (loadOk)
2063 {
2064
2065
2066 for (int m = 0; m < u2p.getNumModels(); m++)
2067 {
2068 u2p.activateModel(m);
2069
2070 btTransform rootTrans;
2071 rootTrans.setOrigin(initialPos);
2072 rootTrans.setRotation(initialOrn);
2073 u2p.setRootTransformInWorld(rootTrans);
2074
2075 //get a body index
2076 int bodyUniqueId = m_data->m_bodyHandles.allocHandle();
2077
2078 InternalPhysXBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(bodyUniqueId);
2079
2080 //sd.m_bodyUniqueIds.push_back(bodyUniqueId);
2081
2082
2083
2084 u2p.setBodyUniqueId(bodyUniqueId);
2085 {
2086 btScalar mass = 0;
2087 //bodyHandle->m_rootLocalInertialFrame.setIdentity();
2088 bodyHandle->m_bodyName = u2p.getBodyName();
2089 btVector3 localInertiaDiagonal(0, 0, 0);
2090 int urdfLinkIndex = u2p.getRootLinkIndex();
2091 //u2p.getMassAndInertia2((urdfLinkIndex, mass, localInertiaDiagonal, bodyHandle->m_rootLocalInertialFrame, flags);
2092 }
2093
2094 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);
2095
2096 physx::PxRigidDynamic* c = baseObj->is<physx::PxRigidDynamic>();
2097 physx::PxRigidStatic* rigidStatic = baseObj->is<physx::PxRigidStatic>();
2098 physx::PxRigidDynamic* rigidDynamic = baseObj->is<physx::PxRigidDynamic>();
2099 physx::PxArticulationReducedCoordinate* articulation = 0;
2100 if (rigidDynamic)
2101 {
2102 bodyHandle->m_rigidDynamic = rigidDynamic;
2103 }
2104 if (rigidStatic)
2105 {
2106 bodyHandle->m_rigidStatic = rigidStatic;
2107 serverStatusOut.m_numDataStreamBytes = 0;
2108 serverStatusOut.m_type = CMD_URDF_LOADING_COMPLETED;
2109 serverStatusOut.m_dataStreamArguments.m_bodyUniqueId = bodyUniqueId;
2110 }
2111 if ((rigidDynamic == 0) && (rigidStatic == 0))
2112 {
2113 articulation = (physx::PxArticulationReducedCoordinate*)baseObj;
2114 serverStatusOut.m_type = CMD_URDF_LOADING_COMPLETED;
2115 serverStatusOut.m_dataStreamArguments.m_bodyUniqueId = bodyUniqueId;
2116 }
2117 if (rigidStatic || rigidDynamic || articulation)
2118 {
2119 serverStatusOut.m_type = CMD_URDF_LOADING_COMPLETED;
2120 serverStatusOut.m_dataStreamArguments.m_bodyUniqueId = bodyUniqueId;
2121 sprintf(serverStatusOut.m_dataStreamArguments.m_bodyName, "%s", bodyHandle->m_bodyName.c_str());
2122 }
2123
2124 if (articulation)
2125 {
2126 bodyHandle->mArticulation = articulation;
2127
2128
2129
2130 btDefaultSerializer ser(bufferSizeInBytes, (unsigned char*)bufferServerToClient);
2131
2132 ser.startSerialization();
2133
2134 int len = sizeof(btMultiBodyData);
2135 btChunk* chunk = ser.allocate(len, 1);
2136
2137
2138 btMultiBodyData *mbd = (btMultiBodyData *)chunk->m_oldPtr;
2139 btTransform rootTrans;
2140 u2p.getRootTransformInWorld(rootTrans);
2141 rootTrans.getOrigin().serialize(mbd->m_baseWorldPosition);
2142 rootTrans.getRotation().serialize(mbd->m_baseWorldOrientation);
2143 btVector3 zero(0, 0, 0);
2144
2145 zero.serialize(mbd->m_baseLinearVelocity);
2146 zero.serialize(mbd->m_baseAngularVelocity);
2147
2148 ser.registerNameForPointer(bodyHandle->m_bodyName.c_str(), bodyHandle->m_bodyName.c_str());
2149 {
2150 char *name = (char *)ser.findNameForPointer(bodyHandle->m_bodyName.c_str());
2151 mbd->m_baseName = (char *)ser.getUniquePointer(name);
2152 if (mbd->m_baseName)
2153 {
2154 ser.serializeName(name);
2155 }
2156 }
2157 mbd->m_numLinks = articulation->getNbLinks()-1;
2158
2159 if (mbd->m_numLinks)
2160 {
2161 int sz = sizeof(btMultiBodyLinkData);
2162 int numElem = mbd->m_numLinks;
2163 btChunk *chunk = ser.allocate(sz, numElem);
2164
2165 physx::PxArticulationLink* physxLinks[64];
2166 physx::PxU32 bufferSize = 64;
2167 physx::PxU32 startIndex = 0;
2168 int numLinks2 = articulation->getLinks(physxLinks, bufferSize, startIndex);
2169
2170 btMultiBodyLinkData *memPtr = (btMultiBodyLinkData *)chunk->m_oldPtr;
2171 for (int j = 0; j < numElem; j++, memPtr++)
2172 {
2173 int i = j + 1;
2174
2175
2176 memPtr->m_jointType = 0;//todo
2177 memPtr->m_dofCount = physxLinks[i]->getInboundJointDof();
2178 memPtr->m_posVarCount = physxLinks[i]->getInboundJointDof(); //??
2179
2180 physx::PxVec3 li = physxLinks[i]->getMassSpaceInertiaTensor();
2181 btVector3 localInertia(li[0], li[1], li[2]);
2182 localInertia.serialize(memPtr->m_linkInertia);
2183
2184 memPtr->m_linkMass = physxLinks[i]->getMass();
2185 memPtr->m_parentIndex = i>0? physxLinks[i]->getInboundJoint()->getParentArticulationLink().getLinkIndex(): -1;
2186 memPtr->m_jointDamping = 0;//todophysxLinks[i]->getLinearDamping();//??
2187 memPtr->m_jointFriction = 0;//todo
2188 memPtr->m_jointLowerLimit = 0;//todogetLink(i).m_jointLowerLimit;
2189 memPtr->m_jointUpperLimit = 0;//todogetLink(i).m_jointUpperLimit;
2190 memPtr->m_jointMaxForce = 0;//todogetLink(i).m_jointMaxForce;
2191 memPtr->m_jointMaxVelocity = 0;//todogetLink(i).m_jointMaxVelocity;
2192
2193 //getLink(i).m_eVector.serialize(memPtr->m_parentComToThisPivotOffset);
2194 //getLink(i).m_dVector.serialize(memPtr->m_thisPivotToThisComOffset);
2195 //getLink(i).m_zeroRotParentToThis.serialize(memPtr->m_zeroRotParentToThis);
2196
2197
2198 {
2199 char *name = (char *)ser.findNameForPointer(physxLinks[i]->getName());
2200 memPtr->m_linkName = (char *)ser.getUniquePointer(name);
2201 if (memPtr->m_linkName)
2202 {
2203 ser.serializeName(name);
2204 }
2205 }
2206 {
2207 char *name = (char *)ser.findNameForPointer(physxLinks[i]->getName());
2208 memPtr->m_jointName = (char *)ser.getUniquePointer(name);
2209 if (memPtr->m_jointName)
2210 {
2211 ser.serializeName(name);
2212 }
2213 }
2214 memPtr->m_linkCollider = (btCollisionObjectData *)ser.getUniquePointer(0);
2215 }
2216 ser.finalizeChunk(chunk, btMultiBodyLinkDataName, BT_ARRAY_CODE, (void *)articulation);
2217 }
2218 mbd->m_links = mbd->m_numLinks ? (btMultiBodyLinkData *)ser.getUniquePointer((void *)articulation) : 0;
2219
2220 // Fill padding with zeros to appease msan.
2221 #ifdef BT_USE_DOUBLE_PRECISION
2222 memset(mbd->m_padding, 0, sizeof(mbd->m_padding));
2223 #endif
2224
2225 const char* structType = btMultiBodyDataName;
2226 ser.finalizeChunk(chunk, structType, BT_MULTIBODY_CODE,0);
2227 int streamSizeInBytes = ser.getCurrentBufferSize();
2228 serverStatusOut.m_numDataStreamBytes = streamSizeInBytes;
2229
2230 }
2231 }
2232 #if 0
2233 btTransform rootTrans;
2234 rootTrans.setOrigin(pos);
2235 rootTrans.setRotation(orn);
2236 u2b.setRootTransformInWorld(rootTrans);
2237 bool ok = processImportedObjects(fileName, bufferServerToClient, bufferSizeInBytes, useMultiBody, flags, u2b);
2238 if (ok)
2239 {
2240 if (m_data->m_sdfRecentLoadedBodies.size() == 1)
2241 {
2242 *bodyUniqueIdPtr = m_data->m_sdfRecentLoadedBodies[0];
2243 }
2244 m_data->m_sdfRecentLoadedBodies.clear();
2245 }
2246 #endif
2247 return true;
2248 }
2249 return false;
2250 }
2251
processRequestBodyInfoCommand(const struct SharedMemoryCommand & clientCmd,struct SharedMemoryStatus & serverStatusOut,char * bufferServerToClient,int bufferSizeInBytes)2252 bool PhysXServerCommandProcessor::processRequestBodyInfoCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
2253 {
2254 bool hasStatus = true;
2255 BT_PROFILE("CMD_REQUEST_BODY_INFO");
2256
2257 const SdfRequestInfoArgs& sdfInfoArgs = clientCmd.m_sdfRequestInfoArgs;
2258 //stream info into memory
2259 int streamSizeInBytes = 0; //createBodyInfoStream(sdfInfoArgs.m_bodyUniqueId, bufferServerToClient, bufferSizeInBytes);
2260
2261 serverStatusOut.m_type = CMD_BODY_INFO_COMPLETED;
2262
2263
2264 serverStatusOut.m_numDataStreamBytes = streamSizeInBytes;
2265
2266 return hasStatus;
2267 }
2268
processForwardDynamicsCommand(const struct SharedMemoryCommand & clientCmd,struct SharedMemoryStatus & serverStatusOut,char * bufferServerToClient,int bufferSizeInBytes)2269 bool PhysXServerCommandProcessor::processForwardDynamicsCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
2270 {
2271 bool hasStatus = true;
2272
2273 BT_PROFILE("CMD_STEP_FORWARD_SIMULATION");
2274
2275 int numArt = m_data->m_scene->getNbArticulations();
2276
2277 {
2278 B3_PROFILE("clear Contacts");
2279 m_data->m_contactPoints.clear();
2280 }
2281 {
2282 B3_PROFILE("PhysX_simulate_fetchResults");
2283
2284 m_data->m_scene->simulate(m_data->m_physicsDeltaTime);
2285 m_data->m_scene->fetchResults(true);
2286 }
2287 {
2288 B3_PROFILE("syncTransform");
2289 if (m_data->m_pluginManager.getRenderInterface())
2290 {
2291
2292 //sync transforms...
2293
2294 b3AlignedObjectArray<int> usedHandles;
2295 m_data->m_bodyHandles.getUsedHandles(usedHandles);
2296
2297 for (int i = 0; i < usedHandles.size(); i++)
2298 {
2299 InternalPhysXBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(usedHandles[i]);
2300
2301
2302 physx::PxRigidActor* actor = 0;
2303 if (bodyHandle->m_rigidDynamic)
2304 actor = bodyHandle->m_rigidDynamic;
2305 if (bodyHandle->m_rigidStatic)
2306 actor = bodyHandle->m_rigidStatic;
2307
2308 if (actor)
2309 {
2310 btTransform tr;
2311 tr.setIdentity();
2312 physx::PxTransform pt = actor->getGlobalPose();
2313 tr.setOrigin(btVector3(pt.p[0], pt.p[1], pt.p[2]));
2314 tr.setRotation(btQuaternion(pt.q.x, pt.q.y, pt.q.z, pt.q.w));
2315 btVector3 localScaling(1, 1, 1);//??
2316 MyPhysXUserData* ud = (MyPhysXUserData*)actor->userData;
2317 m_data->m_pluginManager.getRenderInterface()->syncTransform(ud->m_graphicsUniqueId, tr, localScaling);
2318 }
2319
2320 if (bodyHandle->mArticulation)
2321 {
2322
2323 physx::PxArticulationLink* physxLinks[64];
2324 physx::PxU32 bufferSize = 64;
2325 physx::PxU32 startIndex = 0;
2326 int numLinks2 = bodyHandle->mArticulation->getLinks(physxLinks, bufferSize, startIndex);
2327
2328 for (int l = 0; l < numLinks2; l++)
2329 {
2330 MyPhysXUserData* ud = (MyPhysXUserData*)physxLinks[l]->userData;
2331 if (ud)
2332 {
2333 btTransform tr;
2334 tr.setIdentity();
2335 physx::PxTransform pt = physxLinks[l]->getGlobalPose();
2336 tr.setOrigin(btVector3(pt.p[0], pt.p[1], pt.p[2]));
2337 tr.setRotation(btQuaternion(pt.q.x, pt.q.y, pt.q.z, pt.q.w));
2338 btVector3 localScaling(1, 1, 1);//??
2339 m_data->m_pluginManager.getRenderInterface()->syncTransform(ud->m_graphicsUniqueId, tr, localScaling);
2340 }
2341 }
2342 }
2343 }
2344 }
2345
2346 {
2347 B3_PROFILE("render");
2348 //m_data->m_pluginManager.getRenderInterface()->render();
2349 unsigned char* pixelRGBA = 0;
2350 int numRequestedPixels = 0;
2351 float* depthBuffer = 0;
2352 int* segmentationMaskBuffer = 0;
2353 int startPixelIndex = 0;
2354
2355 int width = 1024;
2356 int height = 768;
2357 m_data->m_pluginManager.getRenderInterface()->getWidthAndHeight(width, height);
2358 int numPixelsCopied = 0;
2359
2360
2361 m_data->m_pluginManager.getRenderInterface()->copyCameraImageData(pixelRGBA, numRequestedPixels,
2362 depthBuffer, numRequestedPixels,
2363 segmentationMaskBuffer, numRequestedPixels,
2364 startPixelIndex, &width, &height, &numPixelsCopied);
2365 }
2366 }
2367
2368 SharedMemoryStatus& serverCmd = serverStatusOut;
2369 serverCmd.m_type = CMD_STEP_FORWARD_SIMULATION_COMPLETED;
2370 return hasStatus;
2371 }
2372
processSendPhysicsParametersCommand(const struct SharedMemoryCommand & clientCmd,struct SharedMemoryStatus & serverStatusOut,char * bufferServerToClient,int bufferSizeInBytes)2373 bool PhysXServerCommandProcessor::processSendPhysicsParametersCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
2374 {
2375 bool hasStatus = true;
2376
2377 BT_PROFILE("CMD_SEND_PHYSICS_SIMULATION_PARAMETERS");
2378
2379 if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_DELTA_TIME)
2380 {
2381 m_data->m_physicsDeltaTime = clientCmd.m_physSimParamArgs.m_deltaTime;
2382 }
2383
2384 if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_GRAVITY)
2385 {
2386 btVector3 grav(clientCmd.m_physSimParamArgs.m_gravityAcceleration[0],
2387 clientCmd.m_physSimParamArgs.m_gravityAcceleration[1],
2388 clientCmd.m_physSimParamArgs.m_gravityAcceleration[2]);
2389
2390 m_data->m_scene->setGravity(physx::PxVec3(grav[0], grav[1], grav[2]));
2391
2392 if (m_data->m_verboseOutput)
2393 {
2394 b3Printf("Updated Gravity: %f,%f,%f", grav[0], grav[1], grav[2]);
2395 }
2396 }
2397 #if 0
2398 if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_NUM_SOLVER_ITERATIONS)
2399 {
2400
2401 m_data->m_dynamicsWorld->getSolverInfo().m_numIterations = clientCmd.m_physSimParamArgs.m_numSolverIterations;
2402 }
2403
2404
2405 if (clientCmd.m_updateFlags & SIM_PARAM_ENABLE_CONE_FRICTION)
2406 {
2407 if (clientCmd.m_physSimParamArgs.m_enableConeFriction)
2408 {
2409 m_data->m_dynamicsWorld->getSolverInfo().m_solverMode &=~SOLVER_DISABLE_IMPLICIT_CONE_FRICTION;
2410 } else
2411 {
2412 m_data->m_dynamicsWorld->getSolverInfo().m_solverMode |=SOLVER_DISABLE_IMPLICIT_CONE_FRICTION;
2413 }
2414 }
2415 if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_DETERMINISTIC_OVERLAPPING_PAIRS)
2416 {
2417 m_data->m_dynamicsWorld->getDispatchInfo().m_deterministicOverlappingPairs = (clientCmd.m_physSimParamArgs.m_deterministicOverlappingPairs!=0);
2418 }
2419
2420 if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_CCD_ALLOWED_PENETRATION)
2421 {
2422 m_data->m_dynamicsWorld->getDispatchInfo().m_allowedCcdPenetration = clientCmd.m_physSimParamArgs.m_allowedCcdPenetration;
2423 }
2424
2425 if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_JOINT_FEEDBACK_MODE)
2426 {
2427 gJointFeedbackInWorldSpace = (clientCmd.m_physSimParamArgs.m_jointFeedbackMode&JOINT_FEEDBACK_IN_WORLD_SPACE)!=0;
2428 gJointFeedbackInJointFrame = (clientCmd.m_physSimParamArgs.m_jointFeedbackMode&JOINT_FEEDBACK_IN_JOINT_FRAME)!=0;
2429 }
2430
2431
2432 if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_REAL_TIME_SIMULATION)
2433 {
2434 m_data->m_useRealTimeSimulation = (clientCmd.m_physSimParamArgs.m_useRealTimeSimulation!=0);
2435 }
2436
2437 //see
2438 if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_INTERNAL_SIMULATION_FLAGS)
2439 {
2440 //these flags are for internal/temporary/easter-egg/experimental demo purposes, use at own risk
2441 gInternalSimFlags = clientCmd.m_physSimParamArgs.m_internalSimFlags;
2442 }
2443
2444
2445
2446 if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_SOLVER_RESIDULAL_THRESHOLD)
2447 {
2448 m_data->m_dynamicsWorld->getSolverInfo().m_leastSquaresResidualThreshold = clientCmd.m_physSimParamArgs.m_solverResidualThreshold;
2449 }
2450
2451
2452 if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_CONTACT_BREAKING_THRESHOLD)
2453 {
2454 gContactBreakingThreshold = clientCmd.m_physSimParamArgs.m_contactBreakingThreshold;
2455 }
2456
2457 if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_CONTACT_SLOP)
2458 {
2459 m_data->m_dynamicsWorld->getSolverInfo().m_linearSlop = clientCmd.m_physSimParamArgs.m_contactSlop;
2460 }
2461
2462 if (clientCmd.m_updateFlags&SIM_PARAM_ENABLE_SAT)
2463 {
2464 m_data->m_dynamicsWorld->getDispatchInfo().m_enableSatConvex = clientCmd.m_physSimParamArgs.m_enableSAT!=0;
2465 }
2466
2467
2468 if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_COLLISION_FILTER_MODE)
2469 {
2470 m_data->m_broadphaseCollisionFilterCallback->m_filterMode = clientCmd.m_physSimParamArgs.m_collisionFilterMode;
2471 }
2472
2473 if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_USE_SPLIT_IMPULSE)
2474 {
2475 m_data->m_dynamicsWorld->getSolverInfo().m_splitImpulse = clientCmd.m_physSimParamArgs.m_useSplitImpulse;
2476 }
2477 if (clientCmd.m_updateFlags &SIM_PARAM_UPDATE_SPLIT_IMPULSE_PENETRATION_THRESHOLD)
2478 {
2479 m_data->m_dynamicsWorld->getSolverInfo().m_splitImpulsePenetrationThreshold = clientCmd.m_physSimParamArgs.m_splitImpulsePenetrationThreshold;
2480 }
2481
2482
2483 if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_NUM_SIMULATION_SUB_STEPS)
2484 {
2485 m_data->m_numSimulationSubSteps = clientCmd.m_physSimParamArgs.m_numSimulationSubSteps;
2486 }
2487
2488 if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_DEFAULT_CONTACT_ERP)
2489 {
2490 m_data->m_dynamicsWorld->getSolverInfo().m_erp2 = clientCmd.m_physSimParamArgs.m_defaultContactERP;
2491 }
2492
2493 if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_DEFAULT_NON_CONTACT_ERP)
2494 {
2495 m_data->m_dynamicsWorld->getSolverInfo().m_erp = clientCmd.m_physSimParamArgs.m_defaultNonContactERP;
2496 }
2497
2498 if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_DEFAULT_FRICTION_ERP)
2499 {
2500 m_data->m_dynamicsWorld->getSolverInfo().m_frictionERP = clientCmd.m_physSimParamArgs.m_frictionERP;
2501 }
2502
2503 if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_DEFAULT_GLOBAL_CFM)
2504 {
2505 m_data->m_dynamicsWorld->getSolverInfo().m_globalCfm = clientCmd.m_physSimParamArgs.m_defaultGlobalCFM;
2506 }
2507
2508 if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_DEFAULT_FRICTION_CFM)
2509 {
2510 m_data->m_dynamicsWorld->getSolverInfo().m_frictionCFM = clientCmd.m_physSimParamArgs.m_frictionCFM;
2511 }
2512
2513 if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_RESTITUTION_VELOCITY_THRESHOLD)
2514 {
2515 m_data->m_dynamicsWorld->getSolverInfo().m_restitutionVelocityThreshold = clientCmd.m_physSimParamArgs.m_restitutionVelocityThreshold;
2516 }
2517
2518
2519
2520 if (clientCmd.m_updateFlags&SIM_PARAM_ENABLE_FILE_CACHING)
2521 {
2522 b3EnableFileCaching(clientCmd.m_physSimParamArgs.m_enableFileCaching);
2523 }
2524
2525 #endif
2526
2527 SharedMemoryStatus& serverCmd = serverStatusOut;
2528 serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED;
2529 return hasStatus;
2530 }
2531
processRequestActualStateCommand(const struct SharedMemoryCommand & clientCmd,struct SharedMemoryStatus & serverStatusOut,char * bufferServerToClient,int bufferSizeInBytes)2532 bool PhysXServerCommandProcessor::processRequestActualStateCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
2533 {
2534 bool hasStatus = true;
2535 serverStatusOut.m_type = CMD_ACTUAL_STATE_UPDATE_FAILED;
2536
2537 int bodyUniqueId = clientCmd.m_requestActualStateInformationCommandArgument.m_bodyUniqueId;
2538 InternalPhysXBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(bodyUniqueId);
2539
2540 if (bodyHandle->mArticulation)
2541 {
2542 BT_PROFILE("CMD_REQUEST_ACTUAL_STATE");
2543 if (m_data->m_verboseOutput)
2544 {
2545 b3Printf("Sending the actual state (Q,U)");
2546 }
2547
2548 {
2549 SharedMemoryStatus& serverCmd = serverStatusOut;
2550 serverStatusOut.m_type = CMD_ACTUAL_STATE_UPDATE_COMPLETED;
2551
2552 serverCmd.m_sendActualStateArgs.m_bodyUniqueId = bodyUniqueId;
2553 serverCmd.m_sendActualStateArgs.m_numLinks = bodyHandle->mArticulation->getNbLinks()-1; //skip base!
2554
2555 int totalDegreeOfFreedomQ = 0;
2556 int totalDegreeOfFreedomU = 0;
2557
2558 if (serverCmd.m_sendActualStateArgs.m_numLinks >= MAX_DEGREE_OF_FREEDOM)
2559 {
2560 serverStatusOut.m_type = CMD_ACTUAL_STATE_UPDATE_FAILED;
2561 hasStatus = true;
2562 return hasStatus;
2563 }
2564
2565 bool computeForwardKinematics = ((clientCmd.m_updateFlags & ACTUAL_STATE_COMPUTE_FORWARD_KINEMATICS) != 0);
2566 bool computeLinkVelocities = ((clientCmd.m_updateFlags & ACTUAL_STATE_COMPUTE_LINKVELOCITY) != 0);
2567
2568 if (computeForwardKinematics || computeLinkVelocities)
2569 {
2570 //todo:check this
2571 }
2572
2573 physx::PxArticulationLink* physxLinks[64];
2574 physx::PxU32 bufferSize = 64;
2575 physx::PxU32 startIndex = 0;
2576 int numLinks2 = bodyHandle->mArticulation->getLinks(physxLinks, bufferSize, startIndex);
2577
2578 {
2579 serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[0] = 0;
2580 serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[1] = 0;
2581 serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[2] = 0;
2582
2583 serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[3] = 0;
2584 serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[4] = 0;
2585 serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[5] = 0;
2586 serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[6] = 1;
2587
2588 physx::PxArticulationLink* l = physxLinks[0];
2589 physx::PxVec3 pos = l->getGlobalPose().p;
2590 physx::PxQuat orn = l->getGlobalPose().q;
2591 //base position in world space, carthesian
2592 serverCmd.m_sendActualStateArgs.m_actualStateQ[0] = pos[0];
2593 serverCmd.m_sendActualStateArgs.m_actualStateQ[1] = pos[1];
2594 serverCmd.m_sendActualStateArgs.m_actualStateQ[2] = pos[2];
2595
2596 //base orientation, quaternion x,y,z,w, in world space, carthesian
2597 serverCmd.m_sendActualStateArgs.m_actualStateQ[3] = orn.x;
2598 serverCmd.m_sendActualStateArgs.m_actualStateQ[4] = orn.y;
2599 serverCmd.m_sendActualStateArgs.m_actualStateQ[5] = orn.z;
2600 serverCmd.m_sendActualStateArgs.m_actualStateQ[6] = orn.w;
2601 totalDegreeOfFreedomQ += 7; //pos + quaternion
2602
2603 physx::PxVec3 linVel = l->getLinearVelocity();
2604 physx::PxVec3 angVel = l->getAngularVelocity();
2605
2606 //base linear velocity (in world space, carthesian)
2607 serverCmd.m_sendActualStateArgs.m_actualStateQdot[0] = linVel.x;
2608 serverCmd.m_sendActualStateArgs.m_actualStateQdot[1] = linVel.y;
2609 serverCmd.m_sendActualStateArgs.m_actualStateQdot[2] = linVel.z;
2610
2611 //base angular velocity (in world space, carthesian)
2612 serverCmd.m_sendActualStateArgs.m_actualStateQdot[3] = angVel.x;
2613 serverCmd.m_sendActualStateArgs.m_actualStateQdot[4] = angVel.y;
2614 serverCmd.m_sendActualStateArgs.m_actualStateQdot[5] = angVel.z;
2615 totalDegreeOfFreedomU += 6; //3 linear and 3 angular DOF
2616 }
2617
2618 physx::PxArticulationCache* c = bodyHandle->mArticulation->createCache();
2619 bodyHandle->mArticulation->copyInternalStateToCache(*c, physx::PxArticulationCache::ePOSITION | physx::PxArticulationCache::eVELOCITY);// physx::PxArticulationCache::eALL);
2620
2621 btAlignedObjectArray<int> dofStarts;
2622 dofStarts.resize(numLinks2);
2623 dofStarts[0] = 0; //We know that the root link does not have a joint
2624 //cache positions in PhysX may be reshuffled, see
2625 //http://gameworksdocs.nvidia.com/PhysX/4.0/documentation/PhysXGuide/Manual/Articulations.html
2626
2627 for (int i = 1; i < numLinks2; ++i)
2628 {
2629 int llIndex = physxLinks[i]->getLinkIndex();
2630 int dofs = physxLinks[i]->getInboundJointDof();
2631
2632 dofStarts[llIndex] = dofs;
2633 }
2634
2635 int count = 0;
2636 for (int i = 1; i < numLinks2; ++i)
2637 {
2638 int dofs = dofStarts[i];
2639 dofStarts[i] = count;
2640 count += dofs;
2641 }
2642
2643 //start at 1, 0 is the base/root, handled above
2644 for (int l = 1; l < numLinks2; l++)
2645 {
2646 int dofs = physxLinks[l]->getInboundJointDof();
2647 int llIndex = physxLinks[l]->getLinkIndex();
2648
2649 for (int d = 0; d < dofs; d++)
2650 {
2651 serverCmd.m_sendActualStateArgs.m_actualStateQ[totalDegreeOfFreedomQ++] = c->jointPosition[dofStarts[llIndex + d]];
2652 serverCmd.m_sendActualStateArgs.m_actualStateQ[totalDegreeOfFreedomU++] = c->jointVelocity[dofStarts[llIndex + d]];
2653 }
2654 }
2655
2656 serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomQ = totalDegreeOfFreedomQ;
2657 serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomU = totalDegreeOfFreedomU;
2658
2659 hasStatus = true;
2660 }
2661 }
2662
2663
2664 return hasStatus;
2665 }
2666
processResetSimulationCommand(const struct SharedMemoryCommand & clientCmd,struct SharedMemoryStatus & serverStatusOut,char * bufferServerToClient,int bufferSizeInBytes)2667 bool PhysXServerCommandProcessor::processResetSimulationCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
2668 {
2669 bool hasStatus = true;
2670 BT_PROFILE("CMD_RESET_SIMULATION");
2671
2672 resetSimulation();
2673
2674 SharedMemoryStatus& serverCmd = serverStatusOut;
2675 serverCmd.m_type = CMD_RESET_SIMULATION_COMPLETED;
2676 return hasStatus;
2677 }
2678
receiveStatus(struct SharedMemoryStatus & serverStatusOut,char * bufferServerToClient,int bufferSizeInBytes)2679 bool PhysXServerCommandProcessor::receiveStatus(struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
2680 {
2681 return false;
2682 }
2683
2684 #endif //BT_ENABLE_PHYSX