1 #include "PhysicsServerCommandProcessor.h"
2 #include "../CommonInterfaces/CommonRenderInterface.h"
3 #include "plugins/b3PluginCollisionInterface.h"
4 #include "../Importers/ImportURDFDemo/BulletUrdfImporter.h"
5 #include "../Importers/ImportURDFDemo/MyMultiBodyCreator.h"
6 #include "../Importers/ImportURDFDemo/URDF2Bullet.h"
7 #include "../Importers/ImportURDFDemo/UrdfFindMeshFile.h"
8
9 #include "../Extras/InverseDynamics/btMultiBodyTreeCreator.hpp"
10
11 #include "BulletCollision/CollisionDispatch/btInternalEdgeUtility.h"
12 #include "../Importers/ImportMeshUtility/b3ImportMeshUtility.h"
13 #include "BulletDynamics/MLCPSolvers/btDantzigSolver.h"
14 #include "BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h"
15 #include "BulletDynamics/MLCPSolvers/btMLCPSolver.h"
16
17 #include "BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.h"
18 #include "BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.h"
19 #include "BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h"
20
21 //#define USE_DISCRETE_DYNAMICS_WORLD
22 //#define SKIP_DEFORMABLE_BODY
23 //#define SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
24
25 #include "../Utils/b3BulletDefaultFileIO.h"
26 #include "BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h"
27 #include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
28 #include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h"
29 #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
30 #include "BulletDynamics/Featherstone/btMultiBodyJointFeedback.h"
31 #include "BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h"
32 #include "BulletDynamics/Featherstone/btMultiBodyGearConstraint.h"
33 #include "../Importers/ImportURDFDemo/UrdfParser.h"
34 #include "../Utils/b3ResourcePath.h"
35 #include "Bullet3Common/b3FileUtils.h"
36 #include "../OpenGLWindow/GLInstanceGraphicsShape.h"
37 #include "BulletDynamics/Featherstone/btMultiBodySliderConstraint.h"
38 #include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h"
39 #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
40 #include "Bullet3Common/b3HashMap.h"
41 #include "../Utils/ChromeTraceUtil.h"
42 #include "SharedMemoryPublic.h"
43 #include "stb_image/stb_image.h"
44 #include "BulletInverseDynamics/MultiBodyTree.hpp"
45 #include "IKTrajectoryHelper.h"
46 #include "btBulletDynamicsCommon.h"
47 #include "../Utils/RobotLoggingUtil.h"
48 #include "LinearMath/btTransform.h"
49 #include "../Importers/ImportMJCFDemo/BulletMJCFImporter.h"
50 #include "../Importers/ImportObjDemo/LoadMeshFromObj.h"
51 #include "../Importers/ImportSTLDemo/LoadMeshFromSTL.h"
52 #include "../Extras/Serialize/BulletWorldImporter/btMultiBodyWorldImporter.h"
53 #include "BulletDynamics/Featherstone/btMultiBodyJointMotor.h"
54 #include "LinearMath/btSerializer.h"
55 #include "Bullet3Common/b3Logging.h"
56 #include "../CommonInterfaces/CommonGUIHelperInterface.h"
57 #include "SharedMemoryCommands.h"
58 #include "LinearMath/btRandom.h"
59 #include "Bullet3Common/b3ResizablePool.h"
60 #include "../Utils/b3Clock.h"
61 #include "b3PluginManager.h"
62 #include "../Extras/Serialize/BulletFileLoader/btBulletFile.h"
63 #include "BulletCollision/NarrowPhaseCollision/btRaycastCallback.h"
64 #include "LinearMath/TaskScheduler/btThreadSupportInterface.h"
65 #include "Wavefront/tiny_obj_loader.h"
66 #ifndef SKIP_COLLISION_FILTER_PLUGIN
67 #include "plugins/collisionFilterPlugin/collisionFilterPlugin.h"
68 #endif
69
70 #ifdef ENABLE_STATIC_GRPC_PLUGIN
71 #include "plugins/grpcPlugin/grpcPlugin.h"
72 #endif //ENABLE_STATIC_GRPC_PLUGIN
73
74 #ifndef SKIP_STATIC_PD_CONTROL_PLUGIN
75 #include "plugins/pdControlPlugin/pdControlPlugin.h"
76 #endif //SKIP_STATIC_PD_CONTROL_PLUGIN
77
78 #ifdef STATIC_LINK_SPD_PLUGIN
79 #include "plugins/stablePDPlugin/BulletConversion.h"
80 #include "plugins/stablePDPlugin/RBDModel.h"
81 #include "plugins/stablePDPlugin/RBDUtil.h"
82 #endif
83
84 #ifdef STATIC_LINK_VR_PLUGIN
85 #include "plugins/vrSyncPlugin/vrSyncPlugin.h"
86 #endif
87
88 #ifdef STATIC_EGLRENDERER_PLUGIN
89 #include "plugins/eglPlugin/eglRendererPlugin.h"
90 #endif //STATIC_EGLRENDERER_PLUGIN
91
92 #ifndef SKIP_STATIC_TINYRENDERER_PLUGIN
93 #include "plugins/tinyRendererPlugin/tinyRendererPlugin.h"
94 #endif
95
96 #ifdef B3_ENABLE_FILEIO_PLUGIN
97 #include "plugins/fileIOPlugin/fileIOPlugin.h"
98 #endif //B3_DISABLE_FILEIO_PLUGIN
99
100 #ifdef B3_ENABLE_TINY_AUDIO
101 #include "../TinyAudio/b3SoundEngine.h"
102 #endif
103
104 #ifdef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
105 #define SKIP_DEFORMABLE_BODY 1
106 #endif
107
108 #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
109 #include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h"
110 #include "BulletSoftBody/btSoftBodySolvers.h"
111 #include "BulletSoftBody/btSoftBodyHelpers.h"
112 #include "BulletSoftBody/btSoftMultiBodyDynamicsWorld.h"
113 #include "BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h"
114 #include "BulletSoftBody/btDeformableBodySolver.h"
115 #include "BulletSoftBody/btDeformableMultiBodyConstraintSolver.h"
116 #include "../SoftDemo/BunnyMesh.h"
117 #endif //SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
118
119 #ifndef SKIP_DEFORMABLE_BODY
120 #include "BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h"
121 #include "BulletSoftBody/btDeformableBodySolver.h"
122 #include "BulletSoftBody/btDeformableMultiBodyConstraintSolver.h"
123 #endif //SKIP_DEFORMABLE_BODY
124
125 #include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
126
127 int gInternalSimFlags = 0;
128 bool gResetSimulation = 0;
129 int gVRTrackingObjectUniqueId = -1;
130 int gVRTrackingObjectFlag = VR_CAMERA_TRACK_OBJECT_ORIENTATION;
131
132 btTransform gVRTrackingObjectTr = btTransform::getIdentity();
133
134 btVector3 gVRTeleportPos1(0, 0, 0);
135 btQuaternion gVRTeleportOrn(0, 0, 0, 1);
136
137 btScalar simTimeScalingFactor = 1;
138 btScalar gRhsClamp = 1.f;
139
140 #include "../CommonInterfaces/CommonFileIOInterface.h"
141
142 class b3ThreadPool
143 {
144 public:
b3ThreadPool(const char * name="b3ThreadPool")145 b3ThreadPool(const char* name = "b3ThreadPool")
146 {
147 btThreadSupportInterface::ConstructionInfo info(name, threadFunction);
148 m_threadSupportInterface = btThreadSupportInterface::create(info);
149 }
150
~b3ThreadPool()151 ~b3ThreadPool()
152 {
153 delete m_threadSupportInterface;
154 }
155
numWorkers() const156 const int numWorkers() const { return m_threadSupportInterface->getNumWorkerThreads(); }
157
runTask(int threadIdx,btThreadSupportInterface::ThreadFunc func,void * arg)158 void runTask(int threadIdx, btThreadSupportInterface::ThreadFunc func, void* arg)
159 {
160 FunctionContext& ctx = m_functionContexts[threadIdx];
161 ctx.func = func;
162 ctx.arg = arg;
163 m_threadSupportInterface->runTask(threadIdx, (void*)&ctx);
164 }
165
waitForAllTasks()166 void waitForAllTasks()
167 {
168 BT_PROFILE("b3ThreadPool_waitForAllTasks");
169 m_threadSupportInterface->waitForAllTasks();
170 }
171
172 private:
173 struct FunctionContext
174 {
175 btThreadSupportInterface::ThreadFunc func;
176 void* arg;
177 };
178
threadFunction(void * userPtr)179 static void threadFunction(void* userPtr)
180 {
181 BT_PROFILE("b3ThreadPool_threadFunction");
182 FunctionContext* ctx = (FunctionContext*)userPtr;
183 ctx->func(ctx->arg);
184 }
185
186 btThreadSupportInterface* m_threadSupportInterface;
187 FunctionContext m_functionContexts[BT_MAX_THREAD_COUNT];
188 };
189
190 struct SharedMemoryDebugDrawer : public btIDebugDraw
191 {
192 int m_debugMode;
193 btAlignedObjectArray<SharedMemLines> m_lines2;
194
SharedMemoryDebugDrawerSharedMemoryDebugDrawer195 SharedMemoryDebugDrawer()
196 : m_debugMode(0)
197 {
198 }
drawContactPointSharedMemoryDebugDrawer199 virtual void drawContactPoint(const btVector3& PointOnB, const btVector3& normalOnB, btScalar distance, int lifeTime, const btVector3& color)
200 {
201 }
202
reportErrorWarningSharedMemoryDebugDrawer203 virtual void reportErrorWarning(const char* warningString)
204 {
205 }
206
draw3dTextSharedMemoryDebugDrawer207 virtual void draw3dText(const btVector3& location, const char* textString)
208 {
209 }
210
setDebugModeSharedMemoryDebugDrawer211 virtual void setDebugMode(int debugMode)
212 {
213 m_debugMode = debugMode;
214 }
215
getDebugModeSharedMemoryDebugDrawer216 virtual int getDebugMode() const
217 {
218 return m_debugMode;
219 }
drawLineSharedMemoryDebugDrawer220 virtual void drawLine(const btVector3& from, const btVector3& to, const btVector3& color)
221 {
222 SharedMemLines line;
223 line.m_from = from;
224 line.m_to = to;
225 line.m_color = color;
226 m_lines2.push_back(line);
227 }
228 };
229
230 struct InternalVisualShapeData
231 {
232 int m_tinyRendererVisualShapeIndex;
233 int m_OpenGLGraphicsIndex;
234
235 b3AlignedObjectArray<UrdfVisual> m_visualShapes;
236
237 b3AlignedObjectArray<std::string> m_pathPrefixes;
238
clearInternalVisualShapeData239 void clear()
240 {
241 m_tinyRendererVisualShapeIndex = -1;
242 m_OpenGLGraphicsIndex = -1;
243 m_visualShapes.clear();
244 m_pathPrefixes.clear();
245 }
246 };
247
248 struct InternalCollisionShapeData
249 {
250 btCollisionShape* m_collisionShape;
251 b3AlignedObjectArray<UrdfCollision> m_urdfCollisionObjects;
252 int m_used;
InternalCollisionShapeDataInternalCollisionShapeData253 InternalCollisionShapeData()
254 : m_collisionShape(0),
255 m_used(0)
256 {
257 }
clearInternalCollisionShapeData258 void clear()
259 {
260 m_collisionShape = 0;
261 m_used = 0;
262 }
263 };
264
265 #include "SharedMemoryUserData.h"
266
267 struct InternalBodyData
268 {
269 btMultiBody* m_multiBody;
270 btRigidBody* m_rigidBody;
271 #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
272 btSoftBody* m_softBody;
273
274 #endif
275 int m_testData;
276 std::string m_bodyName;
277
278 btTransform m_rootLocalInertialFrame;
279 btAlignedObjectArray<btTransform> m_linkLocalInertialFrames;
280 btAlignedObjectArray<btGeneric6DofSpring2Constraint*> m_rigidBodyJoints;
281 btAlignedObjectArray<std::string> m_rigidBodyJointNames;
282 btAlignedObjectArray<std::string> m_rigidBodyLinkNames;
283 btAlignedObjectArray<int> m_userDataHandles;
284
285 #ifdef B3_ENABLE_TINY_AUDIO
286 b3HashMap<btHashInt, SDFAudioSource> m_audioSources;
287 #endif //B3_ENABLE_TINY_AUDIO
288
InternalBodyDataInternalBodyData289 InternalBodyData()
290 {
291 clear();
292 }
293
clearInternalBodyData294 void clear()
295 {
296 m_multiBody = 0;
297 m_rigidBody = 0;
298 #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
299 m_softBody = 0;
300
301 #endif
302 m_testData = 0;
303 m_bodyName = "";
304 m_rootLocalInertialFrame.setIdentity();
305 m_linkLocalInertialFrames.clear();
306 m_rigidBodyJoints.clear();
307 m_rigidBodyJointNames.clear();
308 m_rigidBodyLinkNames.clear();
309 m_userDataHandles.clear();
310 }
311 };
312
313 struct InteralUserConstraintData
314 {
315 btTypedConstraint* m_rbConstraint;
316 btMultiBodyConstraint* m_mbConstraint;
317
318 b3UserConstraint m_userConstraintData;
319
320 int m_sbHandle;
321 int m_sbNodeIndex;
322 btScalar m_sbNodeMass;
323
InteralUserConstraintDataInteralUserConstraintData324 InteralUserConstraintData()
325 : m_rbConstraint(0),
326 m_mbConstraint(0),
327 m_sbHandle(-1),
328 m_sbNodeIndex(-1),
329 m_sbNodeMass(-1)
330 {
331 }
332 };
333
334 struct InternalTextureData
335 {
336 int m_tinyRendererTextureId;
337 int m_openglTextureId;
clearInternalTextureData338 void clear()
339 {
340 m_tinyRendererTextureId = -1;
341 m_openglTextureId = -1;
342 }
343 };
344
345 typedef b3PoolBodyHandle<InternalTextureData> InternalTextureHandle;
346 typedef b3PoolBodyHandle<InternalBodyData> InternalBodyHandle;
347 typedef b3PoolBodyHandle<InternalCollisionShapeData> InternalCollisionShapeHandle;
348 typedef b3PoolBodyHandle<InternalVisualShapeData> InternalVisualShapeHandle;
349
350 class btCommandChunk
351 {
352 public:
353 int m_chunkCode;
354 int m_length;
355 void* m_oldPtr;
356 int m_dna_nr;
357 int m_number;
358 };
359
360 class bCommandChunkPtr4
361 {
362 public:
bCommandChunkPtr4()363 bCommandChunkPtr4() {}
364 int code;
365 int len;
366 union {
367 int m_uniqueInt;
368 };
369 int dna_nr;
370 int nr;
371 };
372
373 // ----------------------------------------------------- //
374 class bCommandChunkPtr8
375 {
376 public:
bCommandChunkPtr8()377 bCommandChunkPtr8() {}
378 int code, len;
379 union {
380 int m_uniqueInts[2];
381 };
382 int dna_nr, nr;
383 };
384
385 struct CommandLogger
386 {
387 FILE* m_file;
388
writeHeaderCommandLogger389 void writeHeader(unsigned char* buffer) const
390 {
391 #ifdef BT_USE_DOUBLE_PRECISION
392 memcpy(buffer, "BT3CMDd", 7);
393 #else
394 memcpy(buffer, "BT3CMDf", 7);
395 #endif //BT_USE_DOUBLE_PRECISION
396
397 int littleEndian = 1;
398 littleEndian = ((char*)&littleEndian)[0];
399
400 if (sizeof(void*) == 8)
401 {
402 buffer[7] = '-';
403 }
404 else
405 {
406 buffer[7] = '_';
407 }
408
409 if (littleEndian)
410 {
411 buffer[8] = 'v';
412 }
413 else
414 {
415 buffer[8] = 'V';
416 }
417
418 buffer[9] = 0;
419 buffer[10] = 0;
420 buffer[11] = 0;
421
422 int ver = btGetVersion();
423 if (ver >= 0 && ver < 999)
424 {
425 sprintf((char*)&buffer[9], "%d", ver);
426 }
427 }
428
logCommandCommandLogger429 void logCommand(const SharedMemoryCommand& command)
430 {
431 if (m_file)
432 {
433 btCommandChunk chunk;
434 chunk.m_chunkCode = command.m_type;
435 chunk.m_oldPtr = 0;
436 chunk.m_dna_nr = 0;
437 chunk.m_length = sizeof(SharedMemoryCommand);
438 chunk.m_number = 1;
439 fwrite((const char*)&chunk, sizeof(btCommandChunk), 1, m_file);
440
441 switch (command.m_type)
442 {
443 case CMD_LOAD_MJCF:
444 {
445 fwrite((const char*)&command.m_updateFlags, sizeof(int), 1, m_file);
446 fwrite((const char*)&command.m_mjcfArguments, sizeof(MjcfArgs), 1, m_file);
447 break;
448 }
449 case CMD_REQUEST_BODY_INFO:
450 {
451 fwrite((const char*)&command.m_updateFlags, sizeof(int), 1, m_file);
452 fwrite((const char*)&command.m_sdfRequestInfoArgs, sizeof(SdfRequestInfoArgs), 1, m_file);
453 break;
454 }
455 case CMD_REQUEST_VISUAL_SHAPE_INFO:
456 {
457 fwrite((const char*)&command.m_updateFlags, sizeof(int), 1, m_file);
458 fwrite((const char*)&command.m_requestVisualShapeDataArguments, sizeof(RequestVisualShapeDataArgs), 1, m_file);
459 break;
460 }
461 case CMD_LOAD_URDF:
462 {
463 fwrite((const char*)&command.m_updateFlags, sizeof(int), 1, m_file);
464 fwrite((const char*)&command.m_urdfArguments, sizeof(UrdfArgs), 1, m_file);
465 break;
466 }
467 case CMD_INIT_POSE:
468 {
469 fwrite((const char*)&command.m_updateFlags, sizeof(int), 1, m_file);
470 fwrite((const char*)&command.m_initPoseArgs, sizeof(InitPoseArgs), 1, m_file);
471 break;
472 };
473 case CMD_REQUEST_ACTUAL_STATE:
474 {
475 fwrite((const char*)&command.m_updateFlags, sizeof(int), 1, m_file);
476 fwrite((const char*)&command.m_requestActualStateInformationCommandArgument,
477 sizeof(RequestActualStateArgs), 1, m_file);
478 break;
479 };
480 case CMD_SEND_DESIRED_STATE:
481 {
482 fwrite((const char*)&command.m_updateFlags, sizeof(int), 1, m_file);
483 fwrite((const char*)&command.m_sendDesiredStateCommandArgument, sizeof(SendDesiredStateArgs), 1, m_file);
484 break;
485 }
486 case CMD_SEND_PHYSICS_SIMULATION_PARAMETERS:
487 {
488 fwrite((const char*)&command.m_updateFlags, sizeof(int), 1, m_file);
489 fwrite((const char*)&command.m_physSimParamArgs, sizeof(b3PhysicsSimulationParameters), 1, m_file);
490 break;
491 }
492 case CMD_REQUEST_CONTACT_POINT_INFORMATION:
493 {
494 fwrite((const char*)&command.m_updateFlags, sizeof(int), 1, m_file);
495 fwrite((const char*)&command.m_requestContactPointArguments, sizeof(RequestContactDataArgs), 1, m_file);
496 break;
497 }
498 case CMD_STEP_FORWARD_SIMULATION:
499 case CMD_RESET_SIMULATION:
500 case CMD_REQUEST_INTERNAL_DATA:
501 {
502 break;
503 };
504 default:
505 {
506 fwrite((const char*)&command, sizeof(SharedMemoryCommand), 1, m_file);
507 }
508 };
509 }
510 }
511
CommandLoggerCommandLogger512 CommandLogger(const char* fileName)
513 {
514 m_file = fopen(fileName, "wb");
515 if (m_file)
516 {
517 unsigned char buf[15];
518 buf[12] = 12;
519 buf[13] = 13;
520 buf[14] = 14;
521 writeHeader(buf);
522 fwrite(buf, 12, 1, m_file);
523 }
524 }
~CommandLoggerCommandLogger525 virtual ~CommandLogger()
526 {
527 if (m_file)
528 {
529 fclose(m_file);
530 }
531 }
532 };
533
534 struct CommandLogPlayback
535 {
536 unsigned char m_header[12];
537 FILE* m_file;
538 bool m_bitsVary;
539 bool m_fileIs64bit;
540
CommandLogPlaybackCommandLogPlayback541 CommandLogPlayback(const char* fileName)
542 {
543 m_file = fopen(fileName, "rb");
544 if (m_file)
545 {
546 size_t bytesRead;
547 bytesRead = fread(m_header, 12, 1, m_file);
548 }
549 unsigned char c = m_header[7];
550 m_fileIs64bit = (c == '-');
551
552 const bool VOID_IS_8 = ((sizeof(void*) == 8));
553 m_bitsVary = (VOID_IS_8 != m_fileIs64bit);
554 }
~CommandLogPlaybackCommandLogPlayback555 virtual ~CommandLogPlayback()
556 {
557 if (m_file)
558 {
559 fclose(m_file);
560 m_file = 0;
561 }
562 }
processNextCommandCommandLogPlayback563 bool processNextCommand(SharedMemoryCommand* cmd)
564 {
565 //for a little while, keep this flag to be able to read 'old' log files
566 //#define BACKWARD_COMPAT
567 #if BACKWARD_COMPAT
568 SharedMemoryCommand unused;
569 #endif //BACKWARD_COMPAT
570 bool result = false;
571 size_t s = 0;
572 if (m_file)
573 {
574 int commandType = -1;
575
576 if (m_fileIs64bit)
577 {
578 bCommandChunkPtr8 chunk8;
579 s = fread((void*)&chunk8, sizeof(bCommandChunkPtr8), 1, m_file);
580 commandType = chunk8.code;
581 }
582 else
583 {
584 bCommandChunkPtr4 chunk4;
585 s = fread((void*)&chunk4, sizeof(bCommandChunkPtr4), 1, m_file);
586 commandType = chunk4.code;
587 }
588
589 if (s == 1)
590 {
591 memset(cmd, 0, sizeof(SharedMemoryCommand));
592 cmd->m_type = commandType;
593
594 #ifdef BACKWARD_COMPAT
595 s = fread(&unused, sizeof(SharedMemoryCommand), 1, m_file);
596 cmd->m_updateFlags = unused.m_updateFlags;
597 #endif
598
599 switch (commandType)
600 {
601 case CMD_LOAD_MJCF:
602 {
603 #ifdef BACKWARD_COMPAT
604 cmd->m_mjcfArguments = unused.m_mjcfArguments;
605 #else
606 s = fread(&cmd->m_updateFlags, sizeof(int), 1, m_file);
607 s = fread(&cmd->m_mjcfArguments, sizeof(MjcfArgs), 1, m_file);
608 #endif
609 result = true;
610 break;
611 }
612 case CMD_REQUEST_BODY_INFO:
613 {
614 #ifdef BACKWARD_COMPAT
615 cmd->m_sdfRequestInfoArgs = unused.m_sdfRequestInfoArgs;
616 #else
617 s = fread(&cmd->m_updateFlags, sizeof(int), 1, m_file);
618 s = fread(&cmd->m_sdfRequestInfoArgs, sizeof(SdfRequestInfoArgs), 1, m_file);
619 #endif
620 result = true;
621 break;
622 }
623 case CMD_REQUEST_VISUAL_SHAPE_INFO:
624 {
625 #ifdef BACKWARD_COMPAT
626 cmd->m_requestVisualShapeDataArguments = unused.m_requestVisualShapeDataArguments;
627 #else
628 s = fread(&cmd->m_updateFlags, sizeof(int), 1, m_file);
629 s = fread(&cmd->m_requestVisualShapeDataArguments, sizeof(RequestVisualShapeDataArgs), 1, m_file);
630 #endif
631 result = true;
632 break;
633 }
634 case CMD_LOAD_URDF:
635 {
636 #ifdef BACKWARD_COMPAT
637 cmd->m_urdfArguments = unused.m_urdfArguments;
638 #else
639 s = fread(&cmd->m_updateFlags, sizeof(int), 1, m_file);
640 s = fread(&cmd->m_urdfArguments, sizeof(UrdfArgs), 1, m_file);
641 #endif
642 result = true;
643 break;
644 }
645 case CMD_INIT_POSE:
646 {
647 #ifdef BACKWARD_COMPAT
648 cmd->m_initPoseArgs = unused.m_initPoseArgs;
649 #else
650 s = fread(&cmd->m_updateFlags, sizeof(int), 1, m_file);
651 s = fread(&cmd->m_initPoseArgs, sizeof(InitPoseArgs), 1, m_file);
652
653 #endif
654 result = true;
655 break;
656 };
657 case CMD_REQUEST_ACTUAL_STATE:
658 {
659 #ifdef BACKWARD_COMPAT
660 cmd->m_requestActualStateInformationCommandArgument = unused.m_requestActualStateInformationCommandArgument;
661 #else
662 s = fread(&cmd->m_updateFlags, sizeof(int), 1, m_file);
663 s = fread(&cmd->m_requestActualStateInformationCommandArgument, sizeof(RequestActualStateArgs), 1, m_file);
664 #endif
665 result = true;
666 break;
667 };
668 case CMD_SEND_DESIRED_STATE:
669 {
670 #ifdef BACKWARD_COMPAT
671 cmd->m_sendDesiredStateCommandArgument = unused.m_sendDesiredStateCommandArgument;
672 #else
673 s = fread(&cmd->m_updateFlags, sizeof(int), 1, m_file);
674 s = fread(&cmd->m_sendDesiredStateCommandArgument, sizeof(SendDesiredStateArgs), 1, m_file);
675
676 #endif
677 result = true;
678 break;
679 }
680 case CMD_SEND_PHYSICS_SIMULATION_PARAMETERS:
681 {
682 #ifdef BACKWARD_COMPAT
683 cmd->m_physSimParamArgs = unused.m_physSimParamArgs;
684 #else
685 s = fread(&cmd->m_updateFlags, sizeof(int), 1, m_file);
686 s = fread(&cmd->m_physSimParamArgs, sizeof(b3PhysicsSimulationParameters), 1, m_file);
687
688 #endif
689 result = true;
690 break;
691 }
692 case CMD_REQUEST_CONTACT_POINT_INFORMATION:
693 {
694 #ifdef BACKWARD_COMPAT
695 cmd->m_requestContactPointArguments = unused.m_requestContactPointArguments;
696 #else
697 s = fread(&cmd->m_updateFlags, sizeof(int), 1, m_file);
698 s = fread(&cmd->m_requestContactPointArguments, sizeof(RequestContactDataArgs), 1, m_file);
699
700 #endif
701 result = true;
702 break;
703 }
704 case CMD_STEP_FORWARD_SIMULATION:
705 case CMD_RESET_SIMULATION:
706 case CMD_REQUEST_INTERNAL_DATA:
707 {
708 result = true;
709 break;
710 }
711 default:
712 {
713 s = fread(cmd, sizeof(SharedMemoryCommand), 1, m_file);
714 result = (s == 1);
715 }
716 };
717 }
718 }
719 return result;
720 }
721 };
722
723 struct SaveWorldObjectData
724 {
725 b3AlignedObjectArray<int> m_bodyUniqueIds;
726 std::string m_fileName;
727 };
728
729 struct MyBroadphaseCallback : public btBroadphaseAabbCallback
730 {
731 b3AlignedObjectArray<int> m_bodyUniqueIds;
732 b3AlignedObjectArray<int> m_links;
733
MyBroadphaseCallbackMyBroadphaseCallback734 MyBroadphaseCallback()
735 {
736 }
~MyBroadphaseCallbackMyBroadphaseCallback737 virtual ~MyBroadphaseCallback()
738 {
739 }
clearMyBroadphaseCallback740 void clear()
741 {
742 m_bodyUniqueIds.clear();
743 m_links.clear();
744 }
processMyBroadphaseCallback745 virtual bool process(const btBroadphaseProxy* proxy)
746 {
747 btCollisionObject* colObj = (btCollisionObject*)proxy->m_clientObject;
748 btMultiBodyLinkCollider* mbl = btMultiBodyLinkCollider::upcast(colObj);
749 if (mbl)
750 {
751 int bodyUniqueId = mbl->m_multiBody->getUserIndex2();
752 m_bodyUniqueIds.push_back(bodyUniqueId);
753 m_links.push_back(mbl->m_link);
754 return true;
755 }
756 int bodyUniqueId = colObj->getUserIndex2();
757 if (bodyUniqueId >= 0)
758 {
759 m_bodyUniqueIds.push_back(bodyUniqueId);
760 //it is not a multibody, so use -1 otherwise
761 m_links.push_back(-1);
762 }
763 return true;
764 }
765 };
766
767 struct MyOverlapFilterCallback : public btOverlapFilterCallback
768 {
769 int m_filterMode;
770 b3PluginManager* m_pluginManager;
771
MyOverlapFilterCallbackMyOverlapFilterCallback772 MyOverlapFilterCallback(b3PluginManager* pluginManager)
773 : m_filterMode(B3_FILTER_GROUPAMASKB_AND_GROUPBMASKA),
774 m_pluginManager(pluginManager)
775 {
776 }
777
~MyOverlapFilterCallbackMyOverlapFilterCallback778 virtual ~MyOverlapFilterCallback()
779 {
780 }
781 // return true when pairs need collision
needBroadphaseCollisionMyOverlapFilterCallback782 virtual bool needBroadphaseCollision(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) const
783 {
784 b3PluginCollisionInterface* collisionInterface = m_pluginManager->getCollisionInterface();
785
786 if (collisionInterface && collisionInterface->getNumRules())
787 {
788 int objectUniqueIdB = -1, linkIndexB = -1;
789 btCollisionObject* colObjB = (btCollisionObject*)proxy1->m_clientObject;
790 btMultiBodyLinkCollider* mblB = btMultiBodyLinkCollider::upcast(colObjB);
791 if (mblB)
792 {
793 objectUniqueIdB = mblB->m_multiBody->getUserIndex2();
794 linkIndexB = mblB->m_link;
795 }
796 else
797 {
798 objectUniqueIdB = colObjB->getUserIndex2();
799 linkIndexB = -1;
800 }
801 int objectUniqueIdA = -1, linkIndexA = -1;
802 btCollisionObject* colObjA = (btCollisionObject*)proxy0->m_clientObject;
803 btMultiBodyLinkCollider* mblA = btMultiBodyLinkCollider::upcast(colObjA);
804 if (mblA)
805 {
806 objectUniqueIdA = mblA->m_multiBody->getUserIndex2();
807 linkIndexA = mblA->m_link;
808 }
809 else
810 {
811 objectUniqueIdA = colObjA->getUserIndex2();
812 linkIndexA = -1;
813 }
814 int collisionFilterGroupA = proxy0->m_collisionFilterGroup;
815 int collisionFilterMaskA = proxy0->m_collisionFilterMask;
816 int collisionFilterGroupB = proxy1->m_collisionFilterGroup;
817 int collisionFilterMaskB = proxy1->m_collisionFilterMask;
818
819 return collisionInterface->needsBroadphaseCollision(objectUniqueIdA, linkIndexA,
820 collisionFilterGroupA, collisionFilterMaskA,
821 objectUniqueIdB, linkIndexB, collisionFilterGroupB, collisionFilterMaskB, m_filterMode);
822 }
823 else
824 {
825 if (m_filterMode == B3_FILTER_GROUPAMASKB_AND_GROUPBMASKA)
826 {
827 bool collides = (proxy0->m_collisionFilterGroup & proxy1->m_collisionFilterMask) != 0;
828 collides = collides && (proxy1->m_collisionFilterGroup & proxy0->m_collisionFilterMask);
829 return collides;
830 }
831
832 if (m_filterMode == B3_FILTER_GROUPAMASKB_OR_GROUPBMASKA)
833 {
834 bool collides = (proxy0->m_collisionFilterGroup & proxy1->m_collisionFilterMask) != 0;
835 collides = collides || (proxy1->m_collisionFilterGroup & proxy0->m_collisionFilterMask);
836 return collides;
837 }
838 return false;
839 }
840 }
841 };
842
843 struct InternalStateLogger
844 {
845 int m_loggingUniqueId;
846 int m_loggingType;
847
InternalStateLoggerInternalStateLogger848 InternalStateLogger()
849 : m_loggingUniqueId(0),
850 m_loggingType(0)
851 {
852 }
~InternalStateLoggerInternalStateLogger853 virtual ~InternalStateLogger() {}
854
855 virtual void stop() = 0;
856 virtual void logState(btScalar timeStep) = 0;
857 };
858
859 struct VideoMP4Loggger : public InternalStateLogger
860 {
861 struct GUIHelperInterface* m_guiHelper;
862 std::string m_fileName;
VideoMP4LogggerVideoMP4Loggger863 VideoMP4Loggger(int loggerUid, const char* fileName, GUIHelperInterface* guiHelper)
864 : m_guiHelper(guiHelper)
865 {
866 m_fileName = fileName;
867 m_loggingUniqueId = loggerUid;
868 m_loggingType = STATE_LOGGING_VIDEO_MP4;
869 m_guiHelper->dumpFramesToVideo(fileName);
870 }
871
stopVideoMP4Loggger872 virtual void stop()
873 {
874 m_guiHelper->dumpFramesToVideo(0);
875 }
logStateVideoMP4Loggger876 virtual void logState(btScalar timeStep)
877 {
878 //dumping video frames happens in another thread
879 //we could add some overlay of timestamp here, if needed/wanted
880 }
881 };
882
883 struct MinitaurStateLogger : public InternalStateLogger
884 {
885 int m_loggingTimeStamp;
886 std::string m_fileName;
887 int m_minitaurBodyUniqueId;
888 FILE* m_logFileHandle;
889
890 std::string m_structTypes;
891 btMultiBody* m_minitaurMultiBody;
892 btAlignedObjectArray<int> m_motorIdList;
893
MinitaurStateLoggerMinitaurStateLogger894 MinitaurStateLogger(int loggingUniqueId, const std::string& fileName, btMultiBody* minitaurMultiBody, btAlignedObjectArray<int>& motorIdList)
895 : m_loggingTimeStamp(0),
896 m_logFileHandle(0),
897 m_minitaurMultiBody(minitaurMultiBody)
898 {
899 m_loggingUniqueId = loggingUniqueId;
900 m_loggingType = STATE_LOGGING_MINITAUR;
901 m_motorIdList.resize(motorIdList.size());
902 for (int m = 0; m < motorIdList.size(); m++)
903 {
904 m_motorIdList[m] = motorIdList[m];
905 }
906
907 btAlignedObjectArray<std::string> structNames;
908 //'t', 'r', 'p', 'y', 'q0', 'q1', 'q2', 'q3', 'q4', 'q5', 'q6', 'q7', 'u0', 'u1', 'u2', 'u3', 'u4', 'u5', 'u6', 'u7', 'xd', 'mo'
909 structNames.push_back("t");
910 structNames.push_back("r");
911 structNames.push_back("p");
912 structNames.push_back("y");
913
914 structNames.push_back("q0");
915 structNames.push_back("q1");
916 structNames.push_back("q2");
917 structNames.push_back("q3");
918 structNames.push_back("q4");
919 structNames.push_back("q5");
920 structNames.push_back("q6");
921 structNames.push_back("q7");
922
923 structNames.push_back("u0");
924 structNames.push_back("u1");
925 structNames.push_back("u2");
926 structNames.push_back("u3");
927 structNames.push_back("u4");
928 structNames.push_back("u5");
929 structNames.push_back("u6");
930 structNames.push_back("u7");
931
932 structNames.push_back("dx");
933 structNames.push_back("mo");
934
935 m_structTypes = "IffffffffffffffffffffB";
936 const char* fileNameC = fileName.c_str();
937
938 m_logFileHandle = createMinitaurLogFile(fileNameC, structNames, m_structTypes);
939 }
stopMinitaurStateLogger940 virtual void stop()
941 {
942 if (m_logFileHandle)
943 {
944 closeMinitaurLogFile(m_logFileHandle);
945 m_logFileHandle = 0;
946 }
947 }
948
logStateMinitaurStateLogger949 virtual void logState(btScalar timeStep)
950 {
951 if (m_logFileHandle)
952 {
953 //btVector3 pos = m_minitaurMultiBody->getBasePos();
954
955 MinitaurLogRecord logData;
956 //'t', 'r', 'p', 'y', 'q0', 'q1', 'q2', 'q3', 'q4', 'q5', 'q6', 'q7', 'u0', 'u1', 'u2', 'u3', 'u4', 'u5', 'u6', 'u7', 'xd', 'mo'
957 btScalar motorDir[8] = {1, 1, 1, 1, 1, 1, 1, 1};
958
959 btQuaternion orn = m_minitaurMultiBody->getBaseWorldTransform().getRotation();
960 btMatrix3x3 mat(orn);
961 btScalar roll = 0;
962 btScalar pitch = 0;
963 btScalar yaw = 0;
964
965 mat.getEulerZYX(yaw, pitch, roll);
966
967 logData.m_values.push_back(m_loggingTimeStamp);
968 logData.m_values.push_back((float)roll);
969 logData.m_values.push_back((float)pitch);
970 logData.m_values.push_back((float)yaw);
971
972 for (int i = 0; i < 8; i++)
973 {
974 float jointAngle = (float)motorDir[i] * m_minitaurMultiBody->getJointPos(m_motorIdList[i]);
975 logData.m_values.push_back(jointAngle);
976 }
977 for (int i = 0; i < 8; i++)
978 {
979 btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)m_minitaurMultiBody->getLink(m_motorIdList[i]).m_userPtr;
980
981 if (motor && timeStep > btScalar(0))
982 {
983 btScalar force = motor->getAppliedImpulse(0) / timeStep;
984 logData.m_values.push_back((float)force);
985 }
986 }
987 //x is forward component, estimated speed forward
988 float xd_speed = m_minitaurMultiBody->getBaseVel()[0];
989 logData.m_values.push_back(xd_speed);
990 char mode = 6;
991 logData.m_values.push_back(mode);
992
993 //at the moment, appendMinitaurLogData will directly write to disk (potential delay)
994 //better to fill a huge memory buffer and once in a while write it to disk
995 appendMinitaurLogData(m_logFileHandle, m_structTypes, logData);
996
997 fflush(m_logFileHandle);
998
999 m_loggingTimeStamp++;
1000 }
1001 }
1002 };
1003
1004 struct b3VRControllerEvents
1005 {
1006 b3VRControllerEvent m_vrEvents[MAX_VR_CONTROLLERS];
1007
b3VRControllerEventsb3VRControllerEvents1008 b3VRControllerEvents()
1009 {
1010 init();
1011 }
1012
~b3VRControllerEventsb3VRControllerEvents1013 virtual ~b3VRControllerEvents()
1014 {
1015 }
1016
initb3VRControllerEvents1017 void init()
1018 {
1019 for (int i = 0; i < MAX_VR_CONTROLLERS; i++)
1020 {
1021 m_vrEvents[i].m_deviceType = 0;
1022 m_vrEvents[i].m_numButtonEvents = 0;
1023 m_vrEvents[i].m_numMoveEvents = 0;
1024 for (int b = 0; b < MAX_VR_BUTTONS; b++)
1025 {
1026 m_vrEvents[i].m_buttons[b] = 0;
1027 }
1028 }
1029 }
1030
addNewVREventsb3VRControllerEvents1031 void addNewVREvents(const struct b3VRControllerEvent* vrEvents, int numVREvents)
1032 {
1033 //update m_vrEvents
1034 for (int i = 0; i < numVREvents; i++)
1035 {
1036 int controlledId = vrEvents[i].m_controllerId;
1037 if (vrEvents[i].m_numMoveEvents)
1038 {
1039 m_vrEvents[controlledId].m_analogAxis = vrEvents[i].m_analogAxis;
1040 for (int a = 0; a < 10; a++)
1041 {
1042 m_vrEvents[controlledId].m_auxAnalogAxis[a] = vrEvents[i].m_auxAnalogAxis[a];
1043 }
1044 }
1045 else
1046 {
1047 m_vrEvents[controlledId].m_analogAxis = 0;
1048 for (int a = 0; a < 10; a++)
1049 {
1050 m_vrEvents[controlledId].m_auxAnalogAxis[a] = 0;
1051 }
1052 }
1053 if (vrEvents[i].m_numMoveEvents + vrEvents[i].m_numButtonEvents)
1054 {
1055 m_vrEvents[controlledId].m_controllerId = vrEvents[i].m_controllerId;
1056 m_vrEvents[controlledId].m_deviceType = vrEvents[i].m_deviceType;
1057
1058 m_vrEvents[controlledId].m_pos[0] = vrEvents[i].m_pos[0];
1059 m_vrEvents[controlledId].m_pos[1] = vrEvents[i].m_pos[1];
1060 m_vrEvents[controlledId].m_pos[2] = vrEvents[i].m_pos[2];
1061 m_vrEvents[controlledId].m_orn[0] = vrEvents[i].m_orn[0];
1062 m_vrEvents[controlledId].m_orn[1] = vrEvents[i].m_orn[1];
1063 m_vrEvents[controlledId].m_orn[2] = vrEvents[i].m_orn[2];
1064 m_vrEvents[controlledId].m_orn[3] = vrEvents[i].m_orn[3];
1065 }
1066
1067 m_vrEvents[controlledId].m_numButtonEvents += vrEvents[i].m_numButtonEvents;
1068 m_vrEvents[controlledId].m_numMoveEvents += vrEvents[i].m_numMoveEvents;
1069 for (int b = 0; b < MAX_VR_BUTTONS; b++)
1070 {
1071 m_vrEvents[controlledId].m_buttons[b] |= vrEvents[i].m_buttons[b];
1072 if (vrEvents[i].m_buttons[b] & eButtonIsDown)
1073 {
1074 m_vrEvents[controlledId].m_buttons[b] |= eButtonIsDown;
1075 }
1076 else
1077 {
1078 m_vrEvents[controlledId].m_buttons[b] &= ~eButtonIsDown;
1079 }
1080 }
1081 }
1082 };
1083 };
1084
1085 struct VRControllerStateLogger : public InternalStateLogger
1086 {
1087 b3VRControllerEvents m_vrEvents;
1088 int m_loggingTimeStamp;
1089 int m_deviceTypeFilter;
1090 std::string m_fileName;
1091 FILE* m_logFileHandle;
1092 std::string m_structTypes;
1093
VRControllerStateLoggerVRControllerStateLogger1094 VRControllerStateLogger(int loggingUniqueId, int deviceTypeFilter, const std::string& fileName)
1095 : m_loggingTimeStamp(0),
1096 m_deviceTypeFilter(deviceTypeFilter),
1097 m_fileName(fileName),
1098 m_logFileHandle(0)
1099 {
1100 m_loggingUniqueId = loggingUniqueId;
1101 m_loggingType = STATE_LOGGING_VR_CONTROLLERS;
1102
1103 btAlignedObjectArray<std::string> structNames;
1104 structNames.push_back("stepCount");
1105 structNames.push_back("timeStamp");
1106 structNames.push_back("controllerId");
1107 structNames.push_back("numMoveEvents");
1108 structNames.push_back("m_numButtonEvents");
1109 structNames.push_back("posX");
1110 structNames.push_back("posY");
1111 structNames.push_back("posZ");
1112 structNames.push_back("oriX");
1113 structNames.push_back("oriY");
1114 structNames.push_back("oriZ");
1115 structNames.push_back("oriW");
1116 structNames.push_back("analogAxis");
1117 structNames.push_back("buttons0");
1118 structNames.push_back("buttons1");
1119 structNames.push_back("buttons2");
1120 structNames.push_back("buttons3");
1121 structNames.push_back("buttons4");
1122 structNames.push_back("buttons5");
1123 structNames.push_back("buttons6");
1124 structNames.push_back("deviceType");
1125 m_structTypes = "IfIIIffffffffIIIIIIII";
1126
1127 const char* fileNameC = fileName.c_str();
1128 m_logFileHandle = createMinitaurLogFile(fileNameC, structNames, m_structTypes);
1129 }
stopVRControllerStateLogger1130 virtual void stop()
1131 {
1132 if (m_logFileHandle)
1133 {
1134 closeMinitaurLogFile(m_logFileHandle);
1135 m_logFileHandle = 0;
1136 }
1137 }
logStateVRControllerStateLogger1138 virtual void logState(btScalar timeStep)
1139 {
1140 if (m_logFileHandle)
1141 {
1142 int stepCount = m_loggingTimeStamp;
1143 float timeStamp = m_loggingTimeStamp * timeStep;
1144
1145 for (int i = 0; i < MAX_VR_CONTROLLERS; i++)
1146 {
1147 b3VRControllerEvent& event = m_vrEvents.m_vrEvents[i];
1148 if (m_deviceTypeFilter & event.m_deviceType)
1149 {
1150 if (event.m_numButtonEvents + event.m_numMoveEvents)
1151 {
1152 MinitaurLogRecord logData;
1153
1154 //serverStatusOut.m_sendVREvents.m_controllerEvents[serverStatusOut.m_sendVREvents.m_numVRControllerEvents++] = event;
1155 //log the event
1156 logData.m_values.push_back(stepCount);
1157 logData.m_values.push_back(timeStamp);
1158 logData.m_values.push_back(event.m_controllerId);
1159 logData.m_values.push_back(event.m_numMoveEvents);
1160 logData.m_values.push_back(event.m_numButtonEvents);
1161 logData.m_values.push_back(event.m_pos[0]);
1162 logData.m_values.push_back(event.m_pos[1]);
1163 logData.m_values.push_back(event.m_pos[2]);
1164 logData.m_values.push_back(event.m_orn[0]);
1165 logData.m_values.push_back(event.m_orn[1]);
1166 logData.m_values.push_back(event.m_orn[2]);
1167 logData.m_values.push_back(event.m_orn[3]);
1168 logData.m_values.push_back(event.m_analogAxis);
1169 int packedButtons[7] = {0, 0, 0, 0, 0, 0, 0};
1170
1171 int packedButtonIndex = 0;
1172 int packedButtonShift = 0;
1173 //encode the 64 buttons into 7 int (3 bits each), each int stores 10 buttons
1174 for (int b = 0; b < MAX_VR_BUTTONS; b++)
1175 {
1176 int buttonMask = event.m_buttons[b];
1177 buttonMask = buttonMask << (packedButtonShift * 3);
1178 packedButtons[packedButtonIndex] |= buttonMask;
1179 packedButtonShift++;
1180
1181 if (packedButtonShift >= 10)
1182 {
1183 packedButtonShift = 0;
1184 packedButtonIndex++;
1185 if (packedButtonIndex >= 7)
1186 {
1187 btAssert(0);
1188 break;
1189 }
1190 }
1191 }
1192
1193 for (int b = 0; b < 7; b++)
1194 {
1195 logData.m_values.push_back(packedButtons[b]);
1196 }
1197 logData.m_values.push_back(event.m_deviceType);
1198 appendMinitaurLogData(m_logFileHandle, m_structTypes, logData);
1199
1200 event.m_numButtonEvents = 0;
1201 event.m_numMoveEvents = 0;
1202 for (int b = 0; b < MAX_VR_BUTTONS; b++)
1203 {
1204 event.m_buttons[b] = 0;
1205 }
1206 }
1207 }
1208 }
1209
1210 fflush(m_logFileHandle);
1211 m_loggingTimeStamp++;
1212 }
1213 }
1214 };
1215
1216 struct GenericRobotStateLogger : public InternalStateLogger
1217 {
1218 float m_loggingTimeStamp;
1219 std::string m_fileName;
1220 FILE* m_logFileHandle;
1221 std::string m_structTypes;
1222 const btMultiBodyDynamicsWorld* m_dynamicsWorld;
1223 btAlignedObjectArray<int> m_bodyIdList;
1224 bool m_filterObjectUniqueId;
1225 int m_maxLogDof;
1226 int m_logFlags;
1227
GenericRobotStateLoggerGenericRobotStateLogger1228 GenericRobotStateLogger(int loggingUniqueId, const std::string& fileName, const btMultiBodyDynamicsWorld* dynamicsWorld, int maxLogDof, int logFlags)
1229 : m_loggingTimeStamp(0),
1230 m_logFileHandle(0),
1231 m_dynamicsWorld(dynamicsWorld),
1232 m_filterObjectUniqueId(false),
1233 m_maxLogDof(maxLogDof),
1234 m_logFlags(logFlags)
1235 {
1236 m_loggingUniqueId = loggingUniqueId;
1237 m_loggingType = STATE_LOGGING_GENERIC_ROBOT;
1238
1239 btAlignedObjectArray<std::string> structNames;
1240 structNames.push_back("stepCount");
1241 structNames.push_back("timeStamp");
1242 structNames.push_back("objectId");
1243 structNames.push_back("posX");
1244 structNames.push_back("posY");
1245 structNames.push_back("posZ");
1246 structNames.push_back("oriX");
1247 structNames.push_back("oriY");
1248 structNames.push_back("oriZ");
1249 structNames.push_back("oriW");
1250 structNames.push_back("velX");
1251 structNames.push_back("velY");
1252 structNames.push_back("velZ");
1253 structNames.push_back("omegaX");
1254 structNames.push_back("omegaY");
1255 structNames.push_back("omegaZ");
1256 structNames.push_back("qNum");
1257
1258 m_structTypes = "IfifffffffffffffI";
1259
1260 for (int i = 0; i < m_maxLogDof; i++)
1261 {
1262 m_structTypes.append("f");
1263 char jointName[256];
1264 sprintf(jointName, "q%d", i);
1265 structNames.push_back(jointName);
1266 }
1267
1268 for (int i = 0; i < m_maxLogDof; i++)
1269 {
1270 m_structTypes.append("f");
1271 char jointName[256];
1272 sprintf(jointName, "u%d", i);
1273 structNames.push_back(jointName);
1274 }
1275
1276 if (m_logFlags & STATE_LOG_JOINT_TORQUES)
1277 {
1278 for (int i = 0; i < m_maxLogDof; i++)
1279 {
1280 m_structTypes.append("f");
1281 char jointName[256];
1282 sprintf(jointName, "t%d", i);
1283 structNames.push_back(jointName);
1284 }
1285 }
1286
1287 const char* fileNameC = fileName.c_str();
1288
1289 m_logFileHandle = createMinitaurLogFile(fileNameC, structNames, m_structTypes);
1290 }
stopGenericRobotStateLogger1291 virtual void stop()
1292 {
1293 if (m_logFileHandle)
1294 {
1295 closeMinitaurLogFile(m_logFileHandle);
1296 m_logFileHandle = 0;
1297 }
1298 }
1299
logStateGenericRobotStateLogger1300 virtual void logState(btScalar timeStep)
1301 {
1302 if (m_logFileHandle)
1303 {
1304 for (int i = 0; i < m_dynamicsWorld->getNumMultibodies(); i++)
1305 {
1306 const btMultiBody* mb = m_dynamicsWorld->getMultiBody(i);
1307 int objectUniqueId = mb->getUserIndex2();
1308 if (m_filterObjectUniqueId && m_bodyIdList.findLinearSearch2(objectUniqueId) < 0)
1309 {
1310 continue;
1311 }
1312
1313 MinitaurLogRecord logData;
1314 int stepCount = m_loggingTimeStamp;
1315 float timeStamp = m_loggingTimeStamp * m_dynamicsWorld->getSolverInfo().m_timeStep;
1316 logData.m_values.push_back(stepCount);
1317 logData.m_values.push_back(timeStamp);
1318
1319 btVector3 pos = mb->getBasePos();
1320 btQuaternion ori = mb->getWorldToBaseRot().inverse();
1321 btVector3 vel = mb->getBaseVel();
1322 btVector3 omega = mb->getBaseOmega();
1323
1324 float posX = pos[0];
1325 float posY = pos[1];
1326 float posZ = pos[2];
1327 float oriX = ori.x();
1328 float oriY = ori.y();
1329 float oriZ = ori.z();
1330 float oriW = ori.w();
1331 float velX = vel[0];
1332 float velY = vel[1];
1333 float velZ = vel[2];
1334 float omegaX = omega[0];
1335 float omegaY = omega[1];
1336 float omegaZ = omega[2];
1337
1338 logData.m_values.push_back(objectUniqueId);
1339 logData.m_values.push_back(posX);
1340 logData.m_values.push_back(posY);
1341 logData.m_values.push_back(posZ);
1342 logData.m_values.push_back(oriX);
1343 logData.m_values.push_back(oriY);
1344 logData.m_values.push_back(oriZ);
1345 logData.m_values.push_back(oriW);
1346 logData.m_values.push_back(velX);
1347 logData.m_values.push_back(velY);
1348 logData.m_values.push_back(velZ);
1349 logData.m_values.push_back(omegaX);
1350 logData.m_values.push_back(omegaY);
1351 logData.m_values.push_back(omegaZ);
1352
1353 int numDofs = mb->getNumDofs();
1354 logData.m_values.push_back(numDofs);
1355 int numJoints = mb->getNumLinks();
1356
1357 for (int j = 0; j < numJoints; ++j)
1358 {
1359 if (mb->getLink(j).m_jointType == 0 || mb->getLink(j).m_jointType == 1)
1360 {
1361 float q = mb->getJointPos(j);
1362 logData.m_values.push_back(q);
1363 }
1364 }
1365 for (int j = numDofs; j < m_maxLogDof; ++j)
1366 {
1367 float q = 0.0;
1368 logData.m_values.push_back(q);
1369 }
1370
1371 for (int j = 0; j < numJoints; ++j)
1372 {
1373 if (mb->getLink(j).m_jointType == 0 || mb->getLink(j).m_jointType == 1)
1374 {
1375 float v = mb->getJointVel(j);
1376 logData.m_values.push_back(v);
1377 }
1378 }
1379 for (int j = numDofs; j < m_maxLogDof; ++j)
1380 {
1381 float v = 0.0;
1382 logData.m_values.push_back(v);
1383 }
1384
1385 if (m_logFlags & STATE_LOG_JOINT_TORQUES)
1386 {
1387 for (int j = 0; j < numJoints; ++j)
1388 {
1389 if (mb->getLink(j).m_jointType == 0 || mb->getLink(j).m_jointType == 1)
1390 {
1391 float jointTorque = 0;
1392 if (m_logFlags & STATE_LOG_JOINT_MOTOR_TORQUES)
1393 {
1394 btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)mb->getLink(j).m_userPtr;
1395 if (motor)
1396 {
1397 jointTorque += motor->getAppliedImpulse(0) / timeStep;
1398 }
1399 }
1400 if (m_logFlags & STATE_LOG_JOINT_USER_TORQUES)
1401 {
1402 if (mb->getLink(j).m_jointType == 0 || mb->getLink(j).m_jointType == 1)
1403 {
1404 jointTorque += mb->getJointTorque(j); //these are the 'user' applied external torques
1405 }
1406 }
1407 logData.m_values.push_back(jointTorque);
1408 }
1409 }
1410 for (int j = numDofs; j < m_maxLogDof; ++j)
1411 {
1412 float u = 0.0;
1413 logData.m_values.push_back(u);
1414 }
1415 }
1416
1417 //at the moment, appendMinitaurLogData will directly write to disk (potential delay)
1418 //better to fill a huge memory buffer and once in a while write it to disk
1419 appendMinitaurLogData(m_logFileHandle, m_structTypes, logData);
1420 fflush(m_logFileHandle);
1421 }
1422
1423 m_loggingTimeStamp++;
1424 }
1425 }
1426 };
1427 struct ContactPointsStateLogger : public InternalStateLogger
1428 {
1429 int m_loggingTimeStamp;
1430
1431 std::string m_fileName;
1432 FILE* m_logFileHandle;
1433 std::string m_structTypes;
1434 btMultiBodyDynamicsWorld* m_dynamicsWorld;
1435 bool m_filterLinkA;
1436 bool m_filterLinkB;
1437 int m_linkIndexA;
1438 int m_linkIndexB;
1439 int m_bodyUniqueIdA;
1440 int m_bodyUniqueIdB;
1441
ContactPointsStateLoggerContactPointsStateLogger1442 ContactPointsStateLogger(int loggingUniqueId, const std::string& fileName, btMultiBodyDynamicsWorld* dynamicsWorld)
1443 : m_loggingTimeStamp(0),
1444 m_fileName(fileName),
1445 m_logFileHandle(0),
1446 m_dynamicsWorld(dynamicsWorld),
1447 m_filterLinkA(false),
1448 m_filterLinkB(false),
1449 m_linkIndexA(-2),
1450 m_linkIndexB(-2),
1451 m_bodyUniqueIdA(-1),
1452 m_bodyUniqueIdB(-1)
1453 {
1454 m_loggingUniqueId = loggingUniqueId;
1455 m_loggingType = STATE_LOGGING_CONTACT_POINTS;
1456
1457 btAlignedObjectArray<std::string> structNames;
1458 structNames.push_back("stepCount");
1459 structNames.push_back("timeStamp");
1460 structNames.push_back("contactFlag");
1461 structNames.push_back("bodyUniqueIdA");
1462 structNames.push_back("bodyUniqueIdB");
1463 structNames.push_back("linkIndexA");
1464 structNames.push_back("linkIndexB");
1465 structNames.push_back("positionOnAX");
1466 structNames.push_back("positionOnAY");
1467 structNames.push_back("positionOnAZ");
1468 structNames.push_back("positionOnBX");
1469 structNames.push_back("positionOnBY");
1470 structNames.push_back("positionOnBZ");
1471 structNames.push_back("contactNormalOnBX");
1472 structNames.push_back("contactNormalOnBY");
1473 structNames.push_back("contactNormalOnBZ");
1474 structNames.push_back("contactDistance");
1475 structNames.push_back("normalForce");
1476 m_structTypes = "IfIiiiifffffffffff";
1477
1478 const char* fileNameC = fileName.c_str();
1479 m_logFileHandle = createMinitaurLogFile(fileNameC, structNames, m_structTypes);
1480 }
stopContactPointsStateLogger1481 virtual void stop()
1482 {
1483 if (m_logFileHandle)
1484 {
1485 closeMinitaurLogFile(m_logFileHandle);
1486 m_logFileHandle = 0;
1487 }
1488 }
logStateContactPointsStateLogger1489 virtual void logState(btScalar timeStep)
1490 {
1491 if (m_logFileHandle)
1492 {
1493 int numContactManifolds = m_dynamicsWorld->getDispatcher()->getNumManifolds();
1494 for (int i = 0; i < numContactManifolds; i++)
1495 {
1496 const btPersistentManifold* manifold = m_dynamicsWorld->getDispatcher()->getInternalManifoldPointer()[i];
1497 int linkIndexA = -1;
1498 int linkIndexB = -1;
1499
1500 int objectIndexB = -1;
1501
1502 const btRigidBody* bodyB = btRigidBody::upcast(manifold->getBody1());
1503 if (bodyB)
1504 {
1505 objectIndexB = bodyB->getUserIndex2();
1506 }
1507 const btMultiBodyLinkCollider* mblB = btMultiBodyLinkCollider::upcast(manifold->getBody1());
1508 if (mblB && mblB->m_multiBody)
1509 {
1510 linkIndexB = mblB->m_link;
1511 objectIndexB = mblB->m_multiBody->getUserIndex2();
1512 if (m_filterLinkB && (m_linkIndexB != linkIndexB))
1513 {
1514 continue;
1515 }
1516 }
1517
1518 int objectIndexA = -1;
1519 const btRigidBody* bodyA = btRigidBody::upcast(manifold->getBody0());
1520 if (bodyA)
1521 {
1522 objectIndexA = bodyA->getUserIndex2();
1523 }
1524 const btMultiBodyLinkCollider* mblA = btMultiBodyLinkCollider::upcast(manifold->getBody0());
1525 if (mblA && mblA->m_multiBody)
1526 {
1527 linkIndexA = mblA->m_link;
1528 objectIndexA = mblA->m_multiBody->getUserIndex2();
1529 if (m_filterLinkA && (m_linkIndexA != linkIndexA))
1530 {
1531 continue;
1532 }
1533 }
1534
1535 btAssert(bodyA || mblA);
1536
1537 //apply the filter, if the user provides it
1538 if (m_bodyUniqueIdA >= 0)
1539 {
1540 if ((m_bodyUniqueIdA != objectIndexA) &&
1541 (m_bodyUniqueIdA != objectIndexB))
1542 continue;
1543 }
1544
1545 //apply the second object filter, if the user provides it
1546 if (m_bodyUniqueIdB >= 0)
1547 {
1548 if ((m_bodyUniqueIdB != objectIndexA) &&
1549 (m_bodyUniqueIdB != objectIndexB))
1550 continue;
1551 }
1552
1553 for (int p = 0; p < manifold->getNumContacts(); p++)
1554 {
1555 MinitaurLogRecord logData;
1556 int stepCount = m_loggingTimeStamp;
1557 float timeStamp = m_loggingTimeStamp * timeStep;
1558 logData.m_values.push_back(stepCount);
1559 logData.m_values.push_back(timeStamp);
1560
1561 const btManifoldPoint& srcPt = manifold->getContactPoint(p);
1562
1563 logData.m_values.push_back(0); // reserved contact flag
1564 logData.m_values.push_back(objectIndexA);
1565 logData.m_values.push_back(objectIndexB);
1566 logData.m_values.push_back(linkIndexA);
1567 logData.m_values.push_back(linkIndexB);
1568 logData.m_values.push_back((float)(srcPt.getPositionWorldOnA()[0]));
1569 logData.m_values.push_back((float)(srcPt.getPositionWorldOnA()[1]));
1570 logData.m_values.push_back((float)(srcPt.getPositionWorldOnA()[2]));
1571 logData.m_values.push_back((float)(srcPt.getPositionWorldOnB()[0]));
1572 logData.m_values.push_back((float)(srcPt.getPositionWorldOnB()[1]));
1573 logData.m_values.push_back((float)(srcPt.getPositionWorldOnB()[2]));
1574 logData.m_values.push_back((float)(srcPt.m_normalWorldOnB[0]));
1575 logData.m_values.push_back((float)(srcPt.m_normalWorldOnB[1]));
1576 logData.m_values.push_back((float)(srcPt.m_normalWorldOnB[2]));
1577 logData.m_values.push_back((float)(srcPt.getDistance()));
1578 logData.m_values.push_back((float)(srcPt.getAppliedImpulse() / timeStep));
1579
1580 appendMinitaurLogData(m_logFileHandle, m_structTypes, logData);
1581 fflush(m_logFileHandle);
1582 }
1583 }
1584 m_loggingTimeStamp++;
1585 }
1586 }
1587 };
1588
1589 struct SaveStateData
1590 {
1591 bParse::btBulletFile* m_bulletFile;
1592 btSerializer* m_serializer;
1593 };
1594
1595 struct PhysicsServerCommandProcessorInternalData
1596 {
1597 ///handle management
1598 b3ResizablePool<InternalTextureHandle> m_textureHandles;
1599 b3ResizablePool<InternalBodyHandle> m_bodyHandles;
1600 b3ResizablePool<InternalCollisionShapeHandle> m_userCollisionShapeHandles;
1601 b3ResizablePool<InternalVisualShapeHandle> m_userVisualShapeHandles;
1602 b3ResizablePool<b3PoolBodyHandle<SharedMemoryUserData> > m_userDataHandles;
1603 btHashMap<SharedMemoryUserDataHashKey, int> m_userDataHandleLookup;
1604
1605 b3PluginManager m_pluginManager;
1606
1607 bool m_useRealTimeSimulation;
1608
1609 b3VRControllerEvents m_vrControllerEvents;
1610
1611 btAlignedObjectArray<SaveStateData> m_savedStates;
1612
1613 btAlignedObjectArray<b3KeyboardEvent> m_keyboardEvents;
1614 btAlignedObjectArray<b3MouseEvent> m_mouseEvents;
1615
1616 CommandLogger* m_commandLogger;
1617 int m_commandLoggingUid;
1618
1619 CommandLogPlayback* m_logPlayback;
1620 int m_logPlaybackUid;
1621
1622 btScalar m_physicsDeltaTime;
1623 btScalar m_numSimulationSubSteps;
1624 btScalar m_simulationTimestamp;
1625 btAlignedObjectArray<btMultiBodyJointFeedback*> m_multiBodyJointFeedbacks;
1626 b3HashMap<btHashPtr, btInverseDynamics::MultiBodyTree*> m_inverseDynamicsBodies;
1627 b3HashMap<btHashPtr, IKTrajectoryHelper*> m_inverseKinematicsHelpers;
1628
1629 int m_userConstraintUIDGenerator;
1630 b3HashMap<btHashInt, InteralUserConstraintData> m_userConstraints;
1631
1632 b3AlignedObjectArray<SaveWorldObjectData> m_saveWorldBodyData;
1633
1634 #ifdef USE_DISCRETE_DYNAMICS_WORLD
1635 btAlignedObjectArray<btWorldImporter*> m_worldImporters;
1636 #else
1637 btAlignedObjectArray<btMultiBodyWorldImporter*> m_worldImporters;
1638 #endif
1639
1640 btAlignedObjectArray<std::string*> m_strings;
1641
1642 btAlignedObjectArray<btCollisionShape*> m_collisionShapes;
1643 btAlignedObjectArray<const unsigned char*> m_heightfieldDatas;
1644 btAlignedObjectArray<int> m_allocatedTextures;
1645 btAlignedObjectArray<unsigned char*> m_allocatedTexturesRequireFree;
1646 btHashMap<btHashPtr, UrdfCollision> m_bulletCollisionShape2UrdfCollision;
1647 btAlignedObjectArray<btStridingMeshInterface*> m_meshInterfaces;
1648
1649 MyOverlapFilterCallback* m_broadphaseCollisionFilterCallback;
1650 btHashedOverlappingPairCache* m_pairCache;
1651 btBroadphaseInterface* m_broadphase;
1652 btCollisionDispatcher* m_dispatcher;
1653
1654 #ifdef USE_DISCRETE_DYNAMICS_WORLD
1655 btSequentialImpulseConstraintSolver* m_solver;
1656 #else
1657 btMultiBodyConstraintSolver* m_solver;
1658 #endif
1659
1660 btDefaultCollisionConfiguration* m_collisionConfiguration;
1661
1662 #ifndef SKIP_DEFORMABLE_BODY
1663 btSoftBody* m_pickedSoftBody;
1664 btDeformableMousePickingForce* m_mouseForce;
1665 btScalar m_maxPickingForce;
1666 btDeformableBodySolver* m_deformablebodySolver;
1667 btAlignedObjectArray<btDeformableLagrangianForce*> m_lf;
1668 #endif
1669
1670 #ifdef USE_DISCRETE_DYNAMICS_WORLD
1671 btDiscreteDynamicsWorld* m_dynamicsWorld;
1672 #else
1673 btMultiBodyDynamicsWorld* m_dynamicsWorld;
1674 #endif
1675
1676
1677 int m_constraintSolverType;
1678 SharedMemoryDebugDrawer* m_remoteDebugDrawer;
1679
1680 btAlignedObjectArray<b3ContactPointData> m_cachedContactPoints;
1681 MyBroadphaseCallback m_cachedOverlappingObjects;
1682
1683 btAlignedObjectArray<int> m_sdfRecentLoadedBodies;
1684 btAlignedObjectArray<int> m_graphicsIndexToSegmentationMask;
1685
1686 btAlignedObjectArray<InternalStateLogger*> m_stateLoggers;
1687 int m_stateLoggersUniqueId;
1688 int m_profileTimingLoggingUid;
1689 std::string m_profileTimingFileName;
1690
1691 struct GUIHelperInterface* m_guiHelper;
1692
1693 int m_sharedMemoryKey;
1694 bool m_enableTinyRenderer;
1695
1696 bool m_verboseOutput;
1697
1698 //data for picking objects
1699 class btRigidBody* m_pickedBody;
1700
1701 int m_savedActivationState;
1702 class btTypedConstraint* m_pickedConstraint;
1703 class btMultiBodyPoint2Point* m_pickingMultiBodyPoint2Point;
1704 btVector3 m_oldPickingPos;
1705 btVector3 m_hitPos;
1706 btScalar m_oldPickingDist;
1707 bool m_prevCanSleep;
1708 int m_pdControlPlugin;
1709 int m_collisionFilterPlugin;
1710 int m_grpcPlugin;
1711
1712 #ifdef B3_ENABLE_TINY_AUDIO
1713 b3SoundEngine m_soundEngine;
1714 #endif
1715
1716 b3HashMap<b3HashString, char*> m_profileEvents;
1717 b3HashMap<b3HashString, UrdfVisualShapeCache> m_cachedVUrdfisualShapes;
1718
1719 b3ThreadPool* m_threadPool;
1720 btScalar m_defaultCollisionMargin;
1721
1722 double m_remoteSyncTransformTime;
1723 double m_remoteSyncTransformInterval;
1724 bool m_useAlternativeDeformableIndexing;
1725
PhysicsServerCommandProcessorInternalDataPhysicsServerCommandProcessorInternalData1726 PhysicsServerCommandProcessorInternalData(PhysicsCommandProcessorInterface* proc)
1727 : m_pluginManager(proc),
1728 m_useRealTimeSimulation(false),
1729 m_commandLogger(0),
1730 m_commandLoggingUid(-1),
1731 m_logPlayback(0),
1732 m_logPlaybackUid(-1),
1733 m_physicsDeltaTime(1. / 240.),
1734 m_numSimulationSubSteps(0),
1735 m_simulationTimestamp(0),
1736 m_userConstraintUIDGenerator(1),
1737 m_broadphaseCollisionFilterCallback(0),
1738 m_pairCache(0),
1739 m_broadphase(0),
1740 m_dispatcher(0),
1741 m_solver(0),
1742 m_collisionConfiguration(0),
1743 #ifndef SKIP_DEFORMABLE_BODY
1744 m_pickedSoftBody(0),
1745 m_mouseForce(0),
1746 m_maxPickingForce(0.3),
1747 m_deformablebodySolver(0),
1748 #endif
1749 m_dynamicsWorld(0),
1750 m_constraintSolverType(-1),
1751 m_remoteDebugDrawer(0),
1752 m_stateLoggersUniqueId(0),
1753 m_profileTimingLoggingUid(-1),
1754 m_guiHelper(0),
1755 m_sharedMemoryKey(SHARED_MEMORY_KEY),
1756 m_enableTinyRenderer(true),
1757 m_verboseOutput(false),
1758 m_pickedBody(0),
1759 m_pickedConstraint(0),
1760 m_pickingMultiBodyPoint2Point(0),
1761 m_pdControlPlugin(-1),
1762 m_collisionFilterPlugin(-1),
1763 m_grpcPlugin(-1),
1764 m_threadPool(0),
1765 m_defaultCollisionMargin(0.001),
1766 m_remoteSyncTransformTime(1. / 30.),
1767 m_remoteSyncTransformInterval(1. / 30.),
1768 m_useAlternativeDeformableIndexing(false)
1769 {
1770 {
1771 //register static plugins:
1772 #ifdef STATIC_LINK_VR_PLUGIN
1773 b3PluginFunctions funcs(initPlugin_vrSyncPlugin, exitPlugin_vrSyncPlugin, executePluginCommand_vrSyncPlugin);
1774 funcs.m_preTickFunc = preTickPluginCallback_vrSyncPlugin;
1775 m_pluginManager.registerStaticLinkedPlugin("vrSyncPlugin", funcs);
1776 #endif //STATIC_LINK_VR_PLUGIN
1777 }
1778 #ifndef SKIP_STATIC_PD_CONTROL_PLUGIN
1779 {
1780 //int b3PluginManager::registerStaticLinkedPlugin(const char* pluginPath, PFN_INIT initFunc, PFN_EXIT exitFunc, PFN_EXECUTE executeCommandFunc, PFN_TICK preTickFunc, PFN_TICK postTickFunc, PFN_GET_RENDER_INTERFACE getRendererFunc, PFN_TICK processClientCommandsFunc, PFN_GET_COLLISION_INTERFACE getCollisionFunc, bool initPlugin)
1781 b3PluginFunctions funcs(initPlugin_pdControlPlugin, exitPlugin_pdControlPlugin, executePluginCommand_pdControlPlugin);
1782 funcs.m_preTickFunc = preTickPluginCallback_pdControlPlugin;
1783 m_pdControlPlugin = m_pluginManager.registerStaticLinkedPlugin("pdControlPlugin", funcs);
1784 }
1785 #endif //SKIP_STATIC_PD_CONTROL_PLUGIN
1786
1787 #ifndef SKIP_COLLISION_FILTER_PLUGIN
1788 {
1789 b3PluginFunctions funcs(initPlugin_collisionFilterPlugin, exitPlugin_collisionFilterPlugin, executePluginCommand_collisionFilterPlugin);
1790 funcs.m_getCollisionFunc = getCollisionInterface_collisionFilterPlugin;
1791 m_collisionFilterPlugin = m_pluginManager.registerStaticLinkedPlugin("collisionFilterPlugin", funcs);
1792 m_pluginManager.selectCollisionPlugin(m_collisionFilterPlugin);
1793 }
1794 #endif
1795
1796 #ifdef ENABLE_STATIC_GRPC_PLUGIN
1797 {
1798 b3PluginFunctions funcs(initPlugin_grpcPlugin, exitPlugin_grpcPlugin, executePluginCommand_grpcPlugin);
1799 funcs.m_processClientCommandsFunc = processClientCommands_grpcPlugin;
1800 m_grpcPlugin = m_pluginManager.registerStaticLinkedPlugin("grpcPlugin", funcs);
1801 }
1802 #endif //ENABLE_STATIC_GRPC_PLUGIN
1803
1804 #ifdef STATIC_EGLRENDERER_PLUGIN
1805 {
1806 bool initPlugin = false;
1807 b3PluginFunctions funcs(initPlugin_eglRendererPlugin, exitPlugin_eglRendererPlugin, executePluginCommand_eglRendererPlugin);
1808 funcs.m_getRendererFunc = getRenderInterface_eglRendererPlugin;
1809 int renderPluginId = m_pluginManager.registerStaticLinkedPlugin("eglRendererPlugin", funcs, initPlugin);
1810 m_pluginManager.selectPluginRenderer(renderPluginId);
1811 }
1812 #endif //STATIC_EGLRENDERER_PLUGIN
1813
1814 #ifndef SKIP_STATIC_TINYRENDERER_PLUGIN
1815 {
1816 b3PluginFunctions funcs(initPlugin_tinyRendererPlugin, exitPlugin_tinyRendererPlugin, executePluginCommand_tinyRendererPlugin);
1817 funcs.m_getRendererFunc = getRenderInterface_tinyRendererPlugin;
1818 int renderPluginId = m_pluginManager.registerStaticLinkedPlugin("tinyRendererPlugin", funcs);
1819 m_pluginManager.selectPluginRenderer(renderPluginId);
1820 }
1821 #endif
1822
1823 #ifdef B3_ENABLE_FILEIO_PLUGIN
1824 {
1825 b3PluginFunctions funcs(initPlugin_fileIOPlugin, exitPlugin_fileIOPlugin, executePluginCommand_fileIOPlugin);
1826 funcs.m_fileIoFunc = getFileIOFunc_fileIOPlugin;
1827 int renderPluginId = m_pluginManager.registerStaticLinkedPlugin("fileIOPlugin", funcs);
1828 m_pluginManager.selectFileIOPlugin(renderPluginId);
1829 }
1830 #endif
1831
1832 m_vrControllerEvents.init();
1833
1834 m_bodyHandles.exitHandles();
1835 m_bodyHandles.initHandles();
1836 m_userCollisionShapeHandles.exitHandles();
1837 m_userCollisionShapeHandles.initHandles();
1838
1839 m_userVisualShapeHandles.exitHandles();
1840 m_userVisualShapeHandles.initHandles();
1841 }
1842
1843 #ifdef STATIC_LINK_SPD_PLUGIN
1844 b3HashMap<btHashPtr, cRBDModel*> m_rbdModels;
1845
convertPosePhysicsServerCommandProcessorInternalData1846 static void convertPose(const btMultiBody* multiBody, const double* jointPositionsQ, const double* jointVelocitiesQdot, Eigen::VectorXd& pose, Eigen::VectorXd& vel)
1847 {
1848 int baseDofQ = multiBody->hasFixedBase() ? 0 : 7;
1849 int baseDofQdot = multiBody->hasFixedBase() ? 0 : 6;
1850
1851 pose.resize(7 + multiBody->getNumPosVars());
1852 vel.resize(7 + multiBody->getNumPosVars()); //??
1853
1854 btTransform tr = multiBody->getBaseWorldTransform();
1855 int dofsrc = 0;
1856 int velsrcdof = 0;
1857 if (baseDofQ == 7)
1858 {
1859 pose[0] = jointPositionsQ[dofsrc++];
1860 pose[1] = jointPositionsQ[dofsrc++];
1861 pose[2] = jointPositionsQ[dofsrc++];
1862
1863 double quatXYZW[4];
1864 quatXYZW[0] = jointPositionsQ[dofsrc++];
1865 quatXYZW[1] = jointPositionsQ[dofsrc++];
1866 quatXYZW[2] = jointPositionsQ[dofsrc++];
1867 quatXYZW[3] = jointPositionsQ[dofsrc++];
1868
1869 pose[3] = quatXYZW[3];
1870 pose[4] = quatXYZW[0];
1871 pose[5] = quatXYZW[1];
1872 pose[6] = quatXYZW[2];
1873 }
1874 else
1875 {
1876 pose[0] = tr.getOrigin()[0];
1877 pose[1] = tr.getOrigin()[1];
1878 pose[2] = tr.getOrigin()[2];
1879 pose[3] = tr.getRotation()[3];
1880 pose[4] = tr.getRotation()[0];
1881 pose[5] = tr.getRotation()[1];
1882 pose[6] = tr.getRotation()[2];
1883 }
1884 if (baseDofQdot == 6)
1885 {
1886 vel[0] = jointVelocitiesQdot[velsrcdof++];
1887 vel[1] = jointVelocitiesQdot[velsrcdof++];
1888 vel[2] = jointVelocitiesQdot[velsrcdof++];
1889 vel[3] = jointVelocitiesQdot[velsrcdof++];
1890 vel[4] = jointVelocitiesQdot[velsrcdof++];
1891 vel[5] = jointVelocitiesQdot[velsrcdof++];
1892 vel[6] = jointVelocitiesQdot[velsrcdof++];
1893 vel[6] = 0;
1894 }
1895 else
1896 {
1897 vel[0] = multiBody->getBaseVel()[0];
1898 vel[1] = multiBody->getBaseVel()[1];
1899 vel[2] = multiBody->getBaseVel()[2];
1900 vel[3] = multiBody->getBaseOmega()[0];
1901 vel[4] = multiBody->getBaseOmega()[1];
1902 vel[5] = multiBody->getBaseOmega()[2];
1903 vel[6] = 0;
1904 }
1905 int dof = 7;
1906 int veldof = 7;
1907
1908 for (int l = 0; l < multiBody->getNumLinks(); l++)
1909 {
1910 switch (multiBody->getLink(l).m_jointType)
1911 {
1912 case btMultibodyLink::eRevolute:
1913 case btMultibodyLink::ePrismatic:
1914 {
1915 pose[dof++] = jointPositionsQ[dofsrc++];
1916 vel[veldof++] = jointVelocitiesQdot[velsrcdof++];
1917 break;
1918 }
1919 case btMultibodyLink::eSpherical:
1920 {
1921 double quatXYZW[4];
1922 quatXYZW[0] = jointPositionsQ[dofsrc++];
1923 quatXYZW[1] = jointPositionsQ[dofsrc++];
1924 quatXYZW[2] = jointPositionsQ[dofsrc++];
1925 quatXYZW[3] = jointPositionsQ[dofsrc++];
1926
1927 pose[dof++] = quatXYZW[3];
1928 pose[dof++] = quatXYZW[0];
1929 pose[dof++] = quatXYZW[1];
1930 pose[dof++] = quatXYZW[2];
1931 vel[veldof++] = jointVelocitiesQdot[velsrcdof++];
1932 vel[veldof++] = jointVelocitiesQdot[velsrcdof++];
1933 vel[veldof++] = jointVelocitiesQdot[velsrcdof++];
1934 vel[veldof++] = jointVelocitiesQdot[velsrcdof++];
1935 break;
1936 }
1937 case btMultibodyLink::eFixed:
1938 {
1939 break;
1940 }
1941 default:
1942 {
1943 assert(0);
1944 }
1945 }
1946 }
1947 }
1948
findOrCreateRBDModelPhysicsServerCommandProcessorInternalData1949 cRBDModel* findOrCreateRBDModel(btMultiBody* multiBody, const double* jointPositionsQ, const double* jointVelocitiesQdot)
1950 {
1951 cRBDModel* rbdModel = 0;
1952 cRBDModel** rbdModelPtr = m_rbdModels.find(multiBody);
1953 if (rbdModelPtr)
1954 {
1955 rbdModel = *rbdModelPtr;
1956 }
1957 else
1958 {
1959 rbdModel = new cRBDModel();
1960 Eigen::MatrixXd bodyDefs;
1961 Eigen::MatrixXd jointMat;
1962 btExtractJointBodyFromBullet(multiBody, bodyDefs, jointMat);
1963 btVector3 grav = m_dynamicsWorld->getGravity();
1964 tVector3 gravity(grav[0], grav[1], grav[2], 0);
1965 rbdModel->Init(jointMat, bodyDefs, gravity);
1966 m_rbdModels.insert(multiBody, rbdModel);
1967 }
1968
1969 //sync pose and vel
1970
1971 Eigen::VectorXd pose, vel;
1972 PhysicsServerCommandProcessorInternalData::convertPose(multiBody, jointPositionsQ, jointVelocitiesQdot, pose, vel);
1973
1974 btVector3 gravOrg = m_dynamicsWorld->getGravity();
1975 tVector grav(gravOrg[0], gravOrg[1], gravOrg[2], 0);
1976 rbdModel->SetGravity(grav);
1977 {
1978 BT_PROFILE("rbdModel::Update");
1979 rbdModel->Update(pose, vel);
1980 }
1981
1982 return rbdModel;
1983 }
1984
1985 #endif
1986
findOrCreateTreePhysicsServerCommandProcessorInternalData1987 btInverseDynamics::MultiBodyTree* findOrCreateTree(btMultiBody* multiBody)
1988 {
1989 btInverseDynamics::MultiBodyTree* tree = 0;
1990
1991 btInverseDynamics::MultiBodyTree** treePtrPtr =
1992 m_inverseDynamicsBodies.find(multiBody);
1993
1994 if (treePtrPtr)
1995 {
1996 tree = *treePtrPtr;
1997 }
1998 else
1999 {
2000 btInverseDynamics::btMultiBodyTreeCreator id_creator;
2001 if (-1 == id_creator.createFromBtMultiBody(multiBody, false))
2002 {
2003 }
2004 else
2005 {
2006 tree = btInverseDynamics::CreateMultiBodyTree(id_creator);
2007 m_inverseDynamicsBodies.insert(multiBody, tree);
2008 }
2009 }
2010
2011 return tree;
2012 }
2013 };
2014
setGuiHelper(struct GUIHelperInterface * guiHelper)2015 void PhysicsServerCommandProcessor::setGuiHelper(struct GUIHelperInterface* guiHelper)
2016 {
2017 if (guiHelper)
2018 {
2019 guiHelper->createPhysicsDebugDrawer(m_data->m_dynamicsWorld);
2020 }
2021 else
2022 {
2023 //state loggers use guiHelper, so remove them before the guiHelper is deleted
2024 deleteStateLoggers();
2025 if (m_data->m_guiHelper && m_data->m_dynamicsWorld && m_data->m_dynamicsWorld->getDebugDrawer())
2026 {
2027 m_data->m_dynamicsWorld->setDebugDrawer(0);
2028 }
2029 }
2030 m_data->m_guiHelper = guiHelper;
2031 }
2032
PhysicsServerCommandProcessor()2033 PhysicsServerCommandProcessor::PhysicsServerCommandProcessor()
2034 : m_data(0)
2035 {
2036 m_data = new PhysicsServerCommandProcessorInternalData(this);
2037
2038 createEmptyDynamicsWorld();
2039 }
2040
~PhysicsServerCommandProcessor()2041 PhysicsServerCommandProcessor::~PhysicsServerCommandProcessor()
2042 {
2043 deleteDynamicsWorld();
2044 if (m_data->m_commandLogger)
2045 {
2046 delete m_data->m_commandLogger;
2047 m_data->m_commandLogger = 0;
2048 }
2049 for (int i = 0; i < m_data->m_profileEvents.size(); i++)
2050 {
2051 char* event = *m_data->m_profileEvents.getAtIndex(i);
2052 delete[] event;
2053 }
2054 if (m_data->m_threadPool)
2055 delete m_data->m_threadPool;
2056
2057 for (int i = 0; i < m_data->m_savedStates.size(); i++)
2058 {
2059 delete m_data->m_savedStates[i].m_bulletFile;
2060 delete m_data->m_savedStates[i].m_serializer;
2061 }
2062
2063 delete m_data;
2064 }
2065
preTickCallback(btDynamicsWorld * world,btScalar timeStep)2066 void preTickCallback(btDynamicsWorld* world, btScalar timeStep)
2067 {
2068 PhysicsServerCommandProcessor* proc = (PhysicsServerCommandProcessor*)world->getWorldUserInfo();
2069
2070 proc->tickPlugins(timeStep, true);
2071 }
2072
logCallback(btDynamicsWorld * world,btScalar timeStep)2073 void logCallback(btDynamicsWorld* world, btScalar timeStep)
2074 {
2075 //handle the logging and playing sounds
2076 PhysicsServerCommandProcessor* proc = (PhysicsServerCommandProcessor*)world->getWorldUserInfo();
2077 proc->processCollisionForces(timeStep);
2078 proc->logObjectStates(timeStep);
2079
2080 proc->tickPlugins(timeStep, false);
2081 }
2082
MyContactAddedCallback(btManifoldPoint & cp,const btCollisionObjectWrapper * colObj0Wrap,int partId0,int index0,const btCollisionObjectWrapper * colObj1Wrap,int partId1,int index1)2083 bool MyContactAddedCallback(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap, int partId0, int index0, const btCollisionObjectWrapper* colObj1Wrap, int partId1, int index1)
2084 {
2085 btAdjustInternalEdgeContacts(cp, colObj1Wrap, colObj0Wrap, partId1, index1);
2086 return true;
2087 }
2088
MyContactDestroyedCallback(void * userPersistentData)2089 bool MyContactDestroyedCallback(void* userPersistentData)
2090 {
2091 //printf("destroyed\n");
2092 return false;
2093 }
2094
MyContactProcessedCallback(btManifoldPoint & cp,void * body0,void * body1)2095 bool MyContactProcessedCallback(btManifoldPoint& cp, void* body0, void* body1)
2096 {
2097 //printf("processed\n");
2098 return false;
2099 }
MyContactStartedCallback(btPersistentManifold * const & manifold)2100 void MyContactStartedCallback(btPersistentManifold* const& manifold)
2101 {
2102 //printf("started\n");
2103 }
MyContactEndedCallback(btPersistentManifold * const & manifold)2104 void MyContactEndedCallback(btPersistentManifold* const& manifold)
2105 {
2106 // printf("ended\n");
2107 }
2108
processCollisionForces(btScalar timeStep)2109 void PhysicsServerCommandProcessor::processCollisionForces(btScalar timeStep)
2110 {
2111 #ifdef B3_ENABLE_TINY_AUDIO
2112 //this is experimental at the moment: impulse thresholds, sound parameters will be exposed in C-API/pybullet.
2113 //audio will go into a wav file, as well as real-time output to speakers/headphones using RtAudio/DAC.
2114
2115 int numContactManifolds = m_data->m_dynamicsWorld->getDispatcher()->getNumManifolds();
2116 for (int i = 0; i < numContactManifolds; i++)
2117 {
2118 const btPersistentManifold* manifold = m_data->m_dynamicsWorld->getDispatcher()->getInternalManifoldPointer()[i];
2119
2120 bool objHasSound[2];
2121 objHasSound[0] = (0 != (manifold->getBody0()->getCollisionFlags() & btCollisionObject::CF_HAS_COLLISION_SOUND_TRIGGER));
2122 objHasSound[1] = (0 != (manifold->getBody1()->getCollisionFlags() & btCollisionObject::CF_HAS_COLLISION_SOUND_TRIGGER));
2123 const btCollisionObject* colObjs[2] = {manifold->getBody0(), manifold->getBody1()};
2124
2125 for (int ob = 0; ob < 2; ob++)
2126 {
2127 if (objHasSound[ob])
2128 {
2129 int uid0 = -1;
2130 int linkIndex = -2;
2131
2132 const btMultiBodyLinkCollider* mblB = btMultiBodyLinkCollider::upcast(colObjs[ob]);
2133 if (mblB && mblB->m_multiBody)
2134 {
2135 linkIndex = mblB->m_link;
2136 uid0 = mblB->m_multiBody->getUserIndex2();
2137 }
2138 const btRigidBody* bodyB = btRigidBody::upcast(colObjs[ob]);
2139 if (bodyB)
2140 {
2141 uid0 = bodyB->getUserIndex2();
2142 linkIndex = -1;
2143 }
2144
2145 if ((uid0 < 0) || (linkIndex < -1))
2146 continue;
2147
2148 InternalBodyHandle* bodyHandle0 = m_data->m_bodyHandles.getHandle(uid0);
2149 SDFAudioSource* audioSrc = bodyHandle0->m_audioSources[linkIndex];
2150 if (audioSrc == 0)
2151 continue;
2152
2153 for (int p = 0; p < manifold->getNumContacts(); p++)
2154 {
2155 double imp = manifold->getContactPoint(p).getAppliedImpulse();
2156 //printf ("manifold %d, contact %d, lifeTime:%d, appliedImpulse:%f\n",i,p, manifold->getContactPoint(p).getLifeTime(),imp);
2157
2158 if (imp > audioSrc->m_collisionForceThreshold && manifold->getContactPoint(p).getLifeTime() == 1)
2159 {
2160 int soundSourceIndex = m_data->m_soundEngine.getAvailableSoundSource();
2161 if (soundSourceIndex >= 0)
2162 {
2163 b3SoundMessage msg;
2164 msg.m_attackRate = audioSrc->m_attackRate;
2165 msg.m_decayRate = audioSrc->m_decayRate;
2166 msg.m_sustainLevel = audioSrc->m_sustainLevel;
2167 msg.m_releaseRate = audioSrc->m_releaseRate;
2168 msg.m_amplitude = audioSrc->m_gain;
2169 msg.m_frequency = audioSrc->m_pitch;
2170 msg.m_type = B3_SOUND_SOURCE_WAV_FILE;
2171 msg.m_wavId = audioSrc->m_userIndex;
2172 msg.m_autoKeyOff = true;
2173 m_data->m_soundEngine.startSound(soundSourceIndex, msg);
2174 }
2175 }
2176 }
2177 }
2178 }
2179 }
2180 #endif //B3_ENABLE_TINY_AUDIO
2181 }
2182
processClientCommands()2183 void PhysicsServerCommandProcessor::processClientCommands()
2184 {
2185 m_data->m_pluginManager.tickPlugins(0, B3_PROCESS_CLIENT_COMMANDS_TICK);
2186 }
2187
reportNotifications()2188 void PhysicsServerCommandProcessor::reportNotifications()
2189 {
2190 m_data->m_pluginManager.reportNotifications();
2191 }
2192
tickPlugins(btScalar timeStep,bool isPreTick)2193 void PhysicsServerCommandProcessor::tickPlugins(btScalar timeStep, bool isPreTick)
2194 {
2195 b3PluginManagerTickMode tickMode = isPreTick ? B3_PRE_TICK_MODE : B3_POST_TICK_MODE;
2196 m_data->m_pluginManager.tickPlugins(timeStep, tickMode);
2197 if (!isPreTick)
2198 {
2199 //clear events after each postTick, so we don't receive events multiple ticks
2200 m_data->m_pluginManager.clearEvents();
2201 }
2202 }
2203
logObjectStates(btScalar timeStep)2204 void PhysicsServerCommandProcessor::logObjectStates(btScalar timeStep)
2205 {
2206 for (int i = 0; i < m_data->m_stateLoggers.size(); i++)
2207 {
2208 m_data->m_stateLoggers[i]->logState(timeStep);
2209 }
2210 }
2211
2212 struct ProgrammaticUrdfInterface : public URDFImporterInterface
2213 {
2214 int m_bodyUniqueId;
2215
2216 const b3CreateMultiBodyArgs& m_createBodyArgs;
2217 mutable b3AlignedObjectArray<btCollisionShape*> m_allocatedCollisionShapes;
2218 PhysicsServerCommandProcessorInternalData* m_data;
2219 int m_flags;
2220
ProgrammaticUrdfInterfaceProgrammaticUrdfInterface2221 ProgrammaticUrdfInterface(const b3CreateMultiBodyArgs& bodyArgs, PhysicsServerCommandProcessorInternalData* data, int flags)
2222 : m_bodyUniqueId(-1),
2223 m_createBodyArgs(bodyArgs),
2224 m_data(data),
2225 m_flags(flags)
2226 {
2227 }
2228
~ProgrammaticUrdfInterfaceProgrammaticUrdfInterface2229 virtual ~ProgrammaticUrdfInterface()
2230 {
2231 }
2232
loadURDFProgrammaticUrdfInterface2233 virtual bool loadURDF(const char* fileName, bool forceFixedBase = false)
2234 {
2235 b3Assert(0);
2236 return false;
2237 }
2238
getPathPrefixProgrammaticUrdfInterface2239 virtual const char* getPathPrefix()
2240 {
2241 return "";
2242 }
2243
2244 ///return >=0 for the root link index, -1 if there is no root link
getRootLinkIndexProgrammaticUrdfInterface2245 virtual int getRootLinkIndex() const
2246 {
2247 return m_createBodyArgs.m_baseLinkIndex;
2248 }
2249
2250 ///pure virtual interfaces, precondition is a valid linkIndex (you can assert/terminate if the linkIndex is out of range)
getLinkNameProgrammaticUrdfInterface2251 virtual std::string getLinkName(int linkIndex) const
2252 {
2253
2254 std::string linkName = "link";
2255 char numstr[21]; // enough to hold all numbers up to 64-bits
2256 sprintf(numstr, "%d", linkIndex);
2257 linkName = linkName + numstr;
2258 return linkName;
2259 }
2260
2261 //various derived class in internal source code break with new pure virtual methods, so provide some default implementation
getBodyNameProgrammaticUrdfInterface2262 virtual std::string getBodyName() const
2263 {
2264 return m_createBodyArgs.m_bodyName;
2265 }
2266
2267 /// optional method to provide the link color. return true if the color is available and copied into colorRGBA, return false otherwise
getLinkColorProgrammaticUrdfInterface2268 virtual bool getLinkColor(int linkIndex, btVector4& colorRGBA) const
2269 {
2270 b3Assert(0);
2271 return false;
2272 }
2273
2274 mutable btHashMap<btHashInt, UrdfMaterialColor> m_linkColors;
2275
getLinkColor2ProgrammaticUrdfInterface2276 virtual bool getLinkColor2(int linkIndex, struct UrdfMaterialColor& matCol) const
2277 {
2278 if (m_flags & URDF_USE_MATERIAL_COLORS_FROM_MTL)
2279 {
2280 const UrdfMaterialColor* matColPtr = m_linkColors[linkIndex];
2281 if (matColPtr)
2282 {
2283 matCol = *matColPtr;
2284 if ((m_flags & CUF_USE_MATERIAL_TRANSPARANCY_FROM_MTL) == 0)
2285 {
2286 matCol.m_rgbaColor[3] = 1;
2287 }
2288
2289 return true;
2290 }
2291 }
2292 else
2293 {
2294 if (m_createBodyArgs.m_linkVisualShapeUniqueIds[linkIndex] >= 0)
2295 {
2296 const InternalVisualShapeHandle* visHandle = m_data->m_userVisualShapeHandles.getHandle(m_createBodyArgs.m_linkVisualShapeUniqueIds[linkIndex]);
2297 if (visHandle)
2298 {
2299 for (int i = 0; i < visHandle->m_visualShapes.size(); i++)
2300 {
2301 if (visHandle->m_visualShapes[i].m_geometry.m_hasLocalMaterial)
2302 {
2303 matCol = visHandle->m_visualShapes[i].m_geometry.m_localMaterial.m_matColor;
2304 return true;
2305 }
2306 }
2307 }
2308 }
2309 }
2310 return false;
2311 }
2312
getCollisionGroupAndMaskProgrammaticUrdfInterface2313 virtual int getCollisionGroupAndMask(int linkIndex, int& colGroup, int& colMask) const
2314 {
2315 return 0;
2316 }
2317 ///this API will likely change, don't override it!
getLinkContactInfoProgrammaticUrdfInterface2318 virtual bool getLinkContactInfo(int linkIndex, URDFLinkContactInfo& contactInfo) const
2319 {
2320 return false;
2321 }
2322
getLinkAudioSourceProgrammaticUrdfInterface2323 virtual bool getLinkAudioSource(int linkIndex, SDFAudioSource& audioSource) const
2324 {
2325 b3Assert(0);
2326 return false;
2327 }
2328
getJointNameProgrammaticUrdfInterface2329 virtual std::string getJointName(int linkIndex) const
2330 {
2331 std::string jointName = "joint";
2332 char numstr[21]; // enough to hold all numbers up to 64-bits
2333 sprintf(numstr, "%d", linkIndex);
2334 jointName = jointName + numstr;
2335 return jointName;
2336 }
2337
2338 //fill mass and inertial data. If inertial data is missing, please initialize mass, inertia to sensitive values, and inertialFrame to identity.
getMassAndInertiaProgrammaticUrdfInterface2339 virtual void getMassAndInertia(int urdfLinkIndex, btScalar& mass, btVector3& localInertiaDiagonal, btTransform& inertialFrame) const
2340 {
2341 if (urdfLinkIndex >= 0 && urdfLinkIndex < m_createBodyArgs.m_numLinks)
2342 {
2343 mass = m_createBodyArgs.m_linkMasses[urdfLinkIndex];
2344 localInertiaDiagonal.setValue(
2345 m_createBodyArgs.m_linkInertias[urdfLinkIndex * 3 + 0],
2346 m_createBodyArgs.m_linkInertias[urdfLinkIndex * 3 + 1],
2347 m_createBodyArgs.m_linkInertias[urdfLinkIndex * 3 + 2]);
2348 inertialFrame.setOrigin(btVector3(
2349 m_createBodyArgs.m_linkInertialFramePositions[urdfLinkIndex * 3 + 0],
2350 m_createBodyArgs.m_linkInertialFramePositions[urdfLinkIndex * 3 + 1],
2351 m_createBodyArgs.m_linkInertialFramePositions[urdfLinkIndex * 3 + 2]));
2352 inertialFrame.setRotation(btQuaternion(
2353 m_createBodyArgs.m_linkInertialFrameOrientations[urdfLinkIndex * 4 + 0],
2354 m_createBodyArgs.m_linkInertialFrameOrientations[urdfLinkIndex * 4 + 1],
2355 m_createBodyArgs.m_linkInertialFrameOrientations[urdfLinkIndex * 4 + 2],
2356 m_createBodyArgs.m_linkInertialFrameOrientations[urdfLinkIndex * 4 + 3]));
2357 }
2358 else
2359 {
2360 mass = 0;
2361 localInertiaDiagonal.setValue(0, 0, 0);
2362 inertialFrame.setIdentity();
2363 }
2364 }
2365
2366 ///fill an array of child link indices for this link, btAlignedObjectArray behaves like a std::vector so just use push_back and resize(0) if needed
getLinkChildIndicesProgrammaticUrdfInterface2367 virtual void getLinkChildIndices(int urdfLinkIndex, btAlignedObjectArray<int>& childLinkIndices) const
2368 {
2369 for (int i = 0; i < m_createBodyArgs.m_numLinks; i++)
2370 {
2371 if (m_createBodyArgs.m_linkParentIndices[i] == urdfLinkIndex)
2372 {
2373 childLinkIndices.push_back(i);
2374 }
2375 }
2376 }
2377
getJointInfoProgrammaticUrdfInterface2378 virtual bool getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction) const
2379 {
2380 return false;
2381 };
2382
getJointInfo2ProgrammaticUrdfInterface2383 virtual bool getJointInfo2(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction, btScalar& jointMaxForce, btScalar& jointMaxVelocity) const
2384 {
2385 bool isValid = false;
2386
2387 int jointTypeOrg = m_createBodyArgs.m_linkJointTypes[urdfLinkIndex];
2388
2389 switch (jointTypeOrg)
2390 {
2391 case eRevoluteType:
2392 {
2393 isValid = true;
2394 jointType = URDFRevoluteJoint;
2395 break;
2396 }
2397 case ePrismaticType:
2398 {
2399 isValid = true;
2400 jointType = URDFPrismaticJoint;
2401 break;
2402 }
2403 case eFixedType:
2404 {
2405 isValid = true;
2406 jointType = URDFFixedJoint;
2407 break;
2408 }
2409 case eSphericalType:
2410 {
2411 isValid = true;
2412 jointType = URDFSphericalJoint;
2413 break;
2414 }
2415 //case ePlanarType:
2416 //case eFixedType:
2417 //case ePoint2PointType:
2418 //case eGearType:
2419 default:
2420 {
2421 }
2422 };
2423
2424 if (isValid)
2425 {
2426 //backwards compatibility for custom file importers
2427 jointMaxForce = 0;
2428 jointMaxVelocity = 0;
2429 jointFriction = 0;
2430 jointDamping = 0;
2431 jointLowerLimit = 1;
2432 jointUpperLimit = -1;
2433
2434 parent2joint.setOrigin(btVector3(
2435 m_createBodyArgs.m_linkPositions[urdfLinkIndex * 3 + 0],
2436 m_createBodyArgs.m_linkPositions[urdfLinkIndex * 3 + 1],
2437 m_createBodyArgs.m_linkPositions[urdfLinkIndex * 3 + 2]));
2438 parent2joint.setRotation(btQuaternion(
2439 m_createBodyArgs.m_linkOrientations[urdfLinkIndex * 4 + 0],
2440 m_createBodyArgs.m_linkOrientations[urdfLinkIndex * 4 + 1],
2441 m_createBodyArgs.m_linkOrientations[urdfLinkIndex * 4 + 2],
2442 m_createBodyArgs.m_linkOrientations[urdfLinkIndex * 4 + 3]));
2443
2444 linkTransformInWorld.setIdentity();
2445
2446 jointAxisInJointSpace.setValue(
2447 m_createBodyArgs.m_linkJointAxis[3 * urdfLinkIndex + 0],
2448 m_createBodyArgs.m_linkJointAxis[3 * urdfLinkIndex + 1],
2449 m_createBodyArgs.m_linkJointAxis[3 * urdfLinkIndex + 2]);
2450 }
2451 return isValid;
2452 };
2453
getRootTransformInWorldProgrammaticUrdfInterface2454 virtual bool getRootTransformInWorld(btTransform& rootTransformInWorld) const
2455 {
2456 int baseLinkIndex = m_createBodyArgs.m_baseLinkIndex;
2457
2458 rootTransformInWorld.setOrigin(btVector3(
2459 m_createBodyArgs.m_linkPositions[baseLinkIndex * 3 + 0],
2460 m_createBodyArgs.m_linkPositions[baseLinkIndex * 3 + 1],
2461 m_createBodyArgs.m_linkPositions[baseLinkIndex * 3 + 2]));
2462 rootTransformInWorld.setRotation(btQuaternion(
2463 m_createBodyArgs.m_linkOrientations[baseLinkIndex * 4 + 0],
2464 m_createBodyArgs.m_linkOrientations[baseLinkIndex * 4 + 1],
2465 m_createBodyArgs.m_linkOrientations[baseLinkIndex * 4 + 2],
2466 m_createBodyArgs.m_linkOrientations[baseLinkIndex * 4 + 3]));
2467 return true;
2468 }
setRootTransformInWorldProgrammaticUrdfInterface2469 virtual void setRootTransformInWorld(const btTransform& rootTransformInWorld)
2470 {
2471 b3Assert(0);
2472 }
2473
convertLinkVisualShapesProgrammaticUrdfInterface2474 virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const
2475 {
2476 int graphicsIndex = -1;
2477 double globalScaling = 1.f; //todo!
2478 int flags = 0;
2479 CommonFileIOInterface* fileIO = m_data->m_pluginManager.getFileIOInterface();
2480
2481 BulletURDFImporter u2b(m_data->m_guiHelper, m_data->m_pluginManager.getRenderInterface(), fileIO, globalScaling, flags);
2482 u2b.setEnableTinyRenderer(m_data->m_enableTinyRenderer);
2483
2484 btAlignedObjectArray<GLInstanceVertex> vertices;
2485 btAlignedObjectArray<int> indices;
2486 btTransform startTrans;
2487 startTrans.setIdentity();
2488 btAlignedObjectArray<BulletURDFTexture> textures;
2489
2490 if (m_createBodyArgs.m_linkVisualShapeUniqueIds[linkIndex] >= 0)
2491 {
2492 InternalVisualShapeHandle* visHandle = m_data->m_userVisualShapeHandles.getHandle(m_createBodyArgs.m_linkVisualShapeUniqueIds[linkIndex]);
2493 if (visHandle)
2494 {
2495 if (visHandle->m_OpenGLGraphicsIndex >= 0)
2496 {
2497 //instancing. assume the inertial frame is identical
2498 graphicsIndex = visHandle->m_OpenGLGraphicsIndex;
2499 }
2500 else
2501 {
2502 for (int v = 0; v < visHandle->m_visualShapes.size(); v++)
2503 {
2504 b3ImportMeshData meshData;
2505 u2b.convertURDFToVisualShapeInternal(&visHandle->m_visualShapes[v], pathPrefix, localInertiaFrame.inverse() * visHandle->m_visualShapes[v].m_linkLocalFrame, vertices, indices, textures, meshData);
2506 if ((meshData.m_flags & B3_IMPORT_MESH_HAS_RGBA_COLOR) &&
2507 (meshData.m_flags & B3_IMPORT_MESH_HAS_SPECULAR_COLOR))
2508 {
2509 UrdfMaterialColor matCol;
2510 matCol.m_rgbaColor.setValue(meshData.m_rgbaColor[0],
2511 meshData.m_rgbaColor[1],
2512 meshData.m_rgbaColor[2],
2513 meshData.m_rgbaColor[3]);
2514 matCol.m_specularColor.setValue(meshData.m_specularColor[0],
2515 meshData.m_specularColor[1],
2516 meshData.m_specularColor[2]);
2517 m_linkColors.insert(linkIndex, matCol);
2518 }
2519 }
2520
2521 if (vertices.size() && indices.size())
2522 {
2523 if (1)
2524 {
2525 int textureIndex = -1;
2526 if (textures.size())
2527 {
2528 textureIndex = m_data->m_guiHelper->registerTexture(textures[0].textureData1, textures[0].m_width, textures[0].m_height);
2529 }
2530
2531 {
2532 B3_PROFILE("registerGraphicsShape");
2533 graphicsIndex = m_data->m_guiHelper->registerGraphicsShape(&vertices[0].xyzw[0], vertices.size(), &indices[0], indices.size(), B3_GL_TRIANGLES, textureIndex);
2534 visHandle->m_OpenGLGraphicsIndex = graphicsIndex;
2535 }
2536 }
2537 }
2538 }
2539 }
2540 }
2541
2542 //delete textures
2543 for (int i = 0; i < textures.size(); i++)
2544 {
2545 B3_PROFILE("free textureData");
2546 if (!textures[i].m_isCached)
2547 {
2548 m_data->m_allocatedTexturesRequireFree.push_back(textures[i].textureData1);
2549 }
2550 }
2551
2552 return graphicsIndex;
2553 }
2554
convertLinkVisualShapes2ProgrammaticUrdfInterface2555 virtual void convertLinkVisualShapes2(int linkIndex, int urdfIndex, const char* pathPrefix, const btTransform& localInertiaFrame, class btCollisionObject* colObj, int bodyUniqueId) const
2556 {
2557 //if there is a visual, use it, otherwise convert collision shape back into UrdfCollision...
2558
2559 UrdfModel model; // = m_data->m_urdfParser.getModel();
2560 UrdfLink link;
2561
2562 if (m_createBodyArgs.m_linkVisualShapeUniqueIds[urdfIndex] >= 0)
2563 {
2564 const InternalVisualShapeHandle* visHandle = m_data->m_userVisualShapeHandles.getHandle(m_createBodyArgs.m_linkVisualShapeUniqueIds[urdfIndex]);
2565 if (visHandle)
2566 {
2567 for (int i = 0; i < visHandle->m_visualShapes.size(); i++)
2568 {
2569 link.m_visualArray.push_back(visHandle->m_visualShapes[i]);
2570 }
2571 }
2572 }
2573
2574 if (link.m_visualArray.size() == 0)
2575 {
2576 int colShapeUniqueId = m_createBodyArgs.m_linkCollisionShapeUniqueIds[urdfIndex];
2577 if (colShapeUniqueId >= 0)
2578 {
2579 InternalCollisionShapeHandle* handle = m_data->m_userCollisionShapeHandles.getHandle(colShapeUniqueId);
2580 if (handle)
2581 {
2582 for (int i = 0; i < handle->m_urdfCollisionObjects.size(); i++)
2583 {
2584 link.m_collisionArray.push_back(handle->m_urdfCollisionObjects[i]);
2585 }
2586 }
2587 }
2588 }
2589 //UrdfVisual vis;
2590 //link.m_visualArray.push_back(vis);
2591 //UrdfLink*const* linkPtr = model.m_links.getAtIndex(urdfIndex);
2592 if (m_data->m_pluginManager.getRenderInterface())
2593 {
2594 CommonFileIOInterface* fileIO = m_data->m_pluginManager.getFileIOInterface();
2595 int visualShapeUniqueid = m_data->m_pluginManager.getRenderInterface()->convertVisualShapes(
2596 linkIndex,
2597 pathPrefix,
2598 localInertiaFrame,
2599 &link,
2600 &model,
2601 colObj->getBroadphaseHandle()->getUid(),
2602 bodyUniqueId,
2603 fileIO);
2604
2605 colObj->getCollisionShape()->setUserIndex2(visualShapeUniqueid);
2606 colObj->setUserIndex3(visualShapeUniqueid);
2607 }
2608 }
setBodyUniqueIdProgrammaticUrdfInterface2609 virtual void setBodyUniqueId(int bodyId)
2610 {
2611 m_bodyUniqueId = bodyId;
2612 }
getBodyUniqueIdProgrammaticUrdfInterface2613 virtual int getBodyUniqueId() const
2614 {
2615 return m_bodyUniqueId;
2616 }
2617
2618 //default implementation for backward compatibility
convertLinkCollisionShapesProgrammaticUrdfInterface2619 virtual class btCompoundShape* convertLinkCollisionShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const
2620 {
2621 btCompoundShape* compound = new btCompoundShape();
2622
2623 int colShapeUniqueId = m_createBodyArgs.m_linkCollisionShapeUniqueIds[linkIndex];
2624 if (colShapeUniqueId >= 0)
2625 {
2626 InternalCollisionShapeHandle* handle = m_data->m_userCollisionShapeHandles.getHandle(colShapeUniqueId);
2627 if (handle && handle->m_collisionShape)
2628 {
2629 handle->m_used++;
2630 if (handle->m_collisionShape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE)
2631 {
2632 btCompoundShape* childCompound = (btCompoundShape*)handle->m_collisionShape;
2633 for (int c = 0; c < childCompound->getNumChildShapes(); c++)
2634 {
2635 btTransform childTrans = childCompound->getChildTransform(c);
2636 btCollisionShape* childShape = childCompound->getChildShape(c);
2637 btTransform tr = localInertiaFrame.inverse() * childTrans;
2638 compound->addChildShape(tr, childShape);
2639 }
2640 }
2641 else
2642 {
2643 btTransform childTrans;
2644 childTrans.setIdentity();
2645 compound->addChildShape(localInertiaFrame.inverse() * childTrans, handle->m_collisionShape);
2646 }
2647 }
2648 }
2649 m_allocatedCollisionShapes.push_back(compound);
2650 return compound;
2651 }
2652
getNumAllocatedCollisionShapesProgrammaticUrdfInterface2653 virtual int getNumAllocatedCollisionShapes() const
2654 {
2655 return m_allocatedCollisionShapes.size();
2656 }
2657
getAllocatedCollisionShapeProgrammaticUrdfInterface2658 virtual class btCollisionShape* getAllocatedCollisionShape(int index)
2659 {
2660 return m_allocatedCollisionShapes[index];
2661 }
getNumModelsProgrammaticUrdfInterface2662 virtual int getNumModels() const
2663 {
2664 return 1;
2665 }
activateModelProgrammaticUrdfInterface2666 virtual void activateModel(int /*modelIndex*/)
2667 {
2668 }
2669 };
2670
getDeformableWorld()2671 btDeformableMultiBodyDynamicsWorld* PhysicsServerCommandProcessor::getDeformableWorld()
2672 {
2673 btDeformableMultiBodyDynamicsWorld* world = 0;
2674 if (m_data->m_dynamicsWorld && m_data->m_dynamicsWorld->getWorldType() == BT_DEFORMABLE_MULTIBODY_DYNAMICS_WORLD)
2675 {
2676 world = (btDeformableMultiBodyDynamicsWorld*)m_data->m_dynamicsWorld;
2677 }
2678 return world;
2679 }
2680
getSoftWorld()2681 btSoftMultiBodyDynamicsWorld* PhysicsServerCommandProcessor::getSoftWorld()
2682 {
2683 btSoftMultiBodyDynamicsWorld* world = 0;
2684 if (m_data->m_dynamicsWorld && m_data->m_dynamicsWorld->getWorldType() == BT_SOFT_MULTIBODY_DYNAMICS_WORLD)
2685 {
2686 world = (btSoftMultiBodyDynamicsWorld*)m_data->m_dynamicsWorld;
2687 }
2688 return world;
2689 }
2690
createEmptyDynamicsWorld(int flags)2691 void PhysicsServerCommandProcessor::createEmptyDynamicsWorld(int flags)
2692 {
2693 m_data->m_constraintSolverType = eConstraintSolverLCP_SI;
2694 ///collision configuration contains default setup for memory, collision setup
2695 //m_collisionConfiguration->setConvexConvexMultipointIterations();
2696 #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
2697 m_data->m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration();
2698 #else
2699 m_data->m_collisionConfiguration = new btDefaultCollisionConfiguration();
2700 #endif
2701 ///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
2702 m_data->m_dispatcher = new btCollisionDispatcher(m_data->m_collisionConfiguration);
2703
2704 m_data->m_broadphaseCollisionFilterCallback = new MyOverlapFilterCallback(&m_data->m_pluginManager);
2705 m_data->m_broadphaseCollisionFilterCallback->m_filterMode = B3_FILTER_GROUPAMASKB_OR_GROUPBMASKA;
2706
2707 m_data->m_pairCache = new btHashedOverlappingPairCache();
2708
2709 m_data->m_pairCache->setOverlapFilterCallback(m_data->m_broadphaseCollisionFilterCallback);
2710
2711 //int maxProxies = 32768;
2712 if (flags & RESET_USE_SIMPLE_BROADPHASE)
2713 {
2714 m_data->m_broadphase = new btSimpleBroadphase(65536, m_data->m_pairCache);
2715 }
2716 else
2717 {
2718 btDbvtBroadphase* bv = new btDbvtBroadphase(m_data->m_pairCache);
2719 bv->setVelocityPrediction(0);
2720 m_data->m_broadphase = bv;
2721 }
2722
2723 if (flags & RESET_USE_DEFORMABLE_WORLD)
2724 {
2725 #ifndef SKIP_DEFORMABLE_BODY
2726 m_data->m_deformablebodySolver = new btDeformableBodySolver();
2727 btDeformableMultiBodyConstraintSolver* solver = new btDeformableMultiBodyConstraintSolver;
2728 m_data->m_solver = solver;
2729 solver->setDeformableSolver(m_data->m_deformablebodySolver);
2730 m_data->m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, solver, m_data->m_collisionConfiguration, m_data->m_deformablebodySolver);
2731 #endif
2732 }
2733
2734
2735
2736 if ((0 == m_data->m_dynamicsWorld) && (0 == (flags & RESET_USE_DISCRETE_DYNAMICS_WORLD)))
2737 {
2738
2739 #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
2740 m_data->m_solver = new btMultiBodyConstraintSolver;
2741 m_data->m_dynamicsWorld = new btSoftMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, m_data->m_solver, m_data->m_collisionConfiguration);
2742 #else
2743 #ifdef USE_DISCRETE_DYNAMICS_WORLD
2744 m_data->m_solver = new btSequentialImpulseConstraintSolver;
2745 m_data->m_dynamicsWorld = new btDiscreteDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, m_data->m_solver, m_data->m_collisionConfiguration);
2746 #else
2747 m_data->m_solver = new btMultiBodyConstraintSolver;
2748 m_data->m_dynamicsWorld = new btMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, m_data->m_solver, m_data->m_collisionConfiguration);
2749 #endif
2750 #endif
2751 }
2752
2753 if (0 == m_data->m_dynamicsWorld)
2754 {
2755 #ifdef USE_DISCRETE_DYNAMICS_WORLD
2756 m_data->m_solver = new btSequentialImpulseConstraintSolver;
2757 m_data->m_dynamicsWorld = new btDiscreteDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, m_data->m_solver, m_data->m_collisionConfiguration);
2758 #else
2759
2760 m_data->m_solver = new btMultiBodyConstraintSolver;
2761 m_data->m_dynamicsWorld = new btMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, m_data->m_solver, m_data->m_collisionConfiguration);
2762 #endif
2763 }
2764
2765 //may help improve run-time performance for many sleeping objects
2766 m_data->m_dynamicsWorld->setForceUpdateAllAabbs(false);
2767
2768 //Workaround: in a VR application, where we avoid synchronizing between GFX/Physics threads, we don't want to resize this array, so pre-allocate it
2769 m_data->m_dynamicsWorld->getCollisionObjectArray().reserve(128 * 1024);
2770
2771 m_data->m_remoteDebugDrawer = new SharedMemoryDebugDrawer();
2772
2773 m_data->m_dynamicsWorld->setGravity(btVector3(0, 0, 0));
2774 m_data->m_dynamicsWorld->getSolverInfo().m_erp2 = 0.08;
2775
2776 m_data->m_dynamicsWorld->getSolverInfo().m_frictionERP = 0.2; //need to check if there are artifacts with frictionERP
2777 m_data->m_dynamicsWorld->getSolverInfo().m_linearSlop = 0.00001;
2778 m_data->m_dynamicsWorld->getSolverInfo().m_numIterations = 50;
2779 m_data->m_dynamicsWorld->getSolverInfo().m_minimumSolverBatchSize = 0;
2780 m_data->m_dynamicsWorld->getSolverInfo().m_warmstartingFactor = 0.1;
2781 gDbvtMargin = btScalar(0);
2782 m_data->m_dynamicsWorld->getSolverInfo().m_leastSquaresResidualThreshold = 1e-7;
2783
2784 if (m_data->m_guiHelper)
2785 {
2786 m_data->m_guiHelper->createPhysicsDebugDrawer(m_data->m_dynamicsWorld);
2787 }
2788 bool isPreTick = false;
2789 m_data->m_dynamicsWorld->setInternalTickCallback(logCallback, this, isPreTick);
2790 isPreTick = true;
2791 m_data->m_dynamicsWorld->setInternalTickCallback(preTickCallback, this, isPreTick);
2792
2793 gContactAddedCallback = MyContactAddedCallback;
2794
2795 #ifdef B3_ENABLE_TINY_AUDIO
2796 m_data->m_soundEngine.init(16, true);
2797
2798 //we don't use those callbacks (yet), experimental
2799
2800 // gContactDestroyedCallback = MyContactDestroyedCallback;
2801 // gContactProcessedCallback = MyContactProcessedCallback;
2802 // gContactStartedCallback = MyContactStartedCallback;
2803 // gContactEndedCallback = MyContactEndedCallback;
2804 #endif
2805 }
2806
deleteStateLoggers()2807 void PhysicsServerCommandProcessor::deleteStateLoggers()
2808 {
2809 for (int i = 0; i < m_data->m_stateLoggers.size(); i++)
2810 {
2811 m_data->m_stateLoggers[i]->stop();
2812 delete m_data->m_stateLoggers[i];
2813 }
2814 m_data->m_stateLoggers.clear();
2815 }
2816
deleteCachedInverseKinematicsBodies()2817 void PhysicsServerCommandProcessor::deleteCachedInverseKinematicsBodies()
2818 {
2819 for (int i = 0; i < m_data->m_inverseKinematicsHelpers.size(); i++)
2820 {
2821 IKTrajectoryHelper** ikHelperPtr = m_data->m_inverseKinematicsHelpers.getAtIndex(i);
2822 if (ikHelperPtr)
2823 {
2824 IKTrajectoryHelper* ikHelper = *ikHelperPtr;
2825 delete ikHelper;
2826 }
2827 }
2828 m_data->m_inverseKinematicsHelpers.clear();
2829 }
deleteCachedInverseDynamicsBodies()2830 void PhysicsServerCommandProcessor::deleteCachedInverseDynamicsBodies()
2831 {
2832 for (int i = 0; i < m_data->m_inverseDynamicsBodies.size(); i++)
2833 {
2834 btInverseDynamics::MultiBodyTree** treePtrPtr = m_data->m_inverseDynamicsBodies.getAtIndex(i);
2835 if (treePtrPtr)
2836 {
2837 btInverseDynamics::MultiBodyTree* tree = *treePtrPtr;
2838 delete tree;
2839 }
2840 }
2841 m_data->m_inverseDynamicsBodies.clear();
2842 }
2843
deleteDynamicsWorld()2844 void PhysicsServerCommandProcessor::deleteDynamicsWorld()
2845 {
2846 #ifdef B3_ENABLE_TINY_AUDIO
2847 m_data->m_soundEngine.exit();
2848 //gContactDestroyedCallback = 0;
2849 //gContactProcessedCallback = 0;
2850 //gContactStartedCallback = 0;
2851 //gContactEndedCallback = 0;
2852 #endif
2853
2854 deleteCachedInverseDynamicsBodies();
2855 deleteCachedInverseKinematicsBodies();
2856 deleteStateLoggers();
2857
2858 m_data->m_userConstraints.clear();
2859 m_data->m_saveWorldBodyData.clear();
2860
2861 for (int i = 0; i < m_data->m_multiBodyJointFeedbacks.size(); i++)
2862 {
2863 delete m_data->m_multiBodyJointFeedbacks[i];
2864 }
2865 m_data->m_multiBodyJointFeedbacks.clear();
2866
2867 for (int i = 0; i < m_data->m_worldImporters.size(); i++)
2868 {
2869 m_data->m_worldImporters[i]->deleteAllData();
2870 delete m_data->m_worldImporters[i];
2871 }
2872 m_data->m_worldImporters.clear();
2873
2874 #ifdef ENABLE_LINK_MAPPER
2875 for (int i = 0; i < m_data->m_urdfLinkNameMapper.size(); i++)
2876 {
2877 delete m_data->m_urdfLinkNameMapper[i];
2878 }
2879 m_data->m_urdfLinkNameMapper.clear();
2880 #endif //ENABLE_LINK_MAPPER
2881
2882 for (int i = 0; i < m_data->m_strings.size(); i++)
2883 {
2884 delete m_data->m_strings[i];
2885 }
2886 m_data->m_strings.clear();
2887
2888 btAlignedObjectArray<btTypedConstraint*> constraints;
2889 btAlignedObjectArray<btMultiBodyConstraint*> mbconstraints;
2890
2891 if (m_data->m_dynamicsWorld)
2892 {
2893 int i;
2894 for (i = m_data->m_dynamicsWorld->getNumConstraints() - 1; i >= 0; i--)
2895 {
2896 btTypedConstraint* constraint = m_data->m_dynamicsWorld->getConstraint(i);
2897 constraints.push_back(constraint);
2898 m_data->m_dynamicsWorld->removeConstraint(constraint);
2899 }
2900 #ifndef USE_DISCRETE_DYNAMICS_WORLD
2901 for (i = m_data->m_dynamicsWorld->getNumMultiBodyConstraints() - 1; i >= 0; i--)
2902 {
2903 btMultiBodyConstraint* mbconstraint = m_data->m_dynamicsWorld->getMultiBodyConstraint(i);
2904 mbconstraints.push_back(mbconstraint);
2905 m_data->m_dynamicsWorld->removeMultiBodyConstraint(mbconstraint);
2906 }
2907 #endif
2908 for (i = m_data->m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--)
2909 {
2910 btCollisionObject* obj = m_data->m_dynamicsWorld->getCollisionObjectArray()[i];
2911 btRigidBody* body = btRigidBody::upcast(obj);
2912 if (body && body->getMotionState())
2913 {
2914 delete body->getMotionState();
2915 }
2916 m_data->m_dynamicsWorld->removeCollisionObject(obj);
2917 delete obj;
2918 }
2919 #ifndef USE_DISCRETE_DYNAMICS_WORLD
2920 for (i = m_data->m_dynamicsWorld->getNumMultibodies() - 1; i >= 0; i--)
2921 {
2922 btMultiBody* mb = m_data->m_dynamicsWorld->getMultiBody(i);
2923 m_data->m_dynamicsWorld->removeMultiBody(mb);
2924 delete mb;
2925 }
2926 #endif
2927 #ifndef SKIP_DEFORMABLE_BODY
2928 for (int j = 0; j < m_data->m_lf.size(); j++)
2929 {
2930 btDeformableLagrangianForce* force = m_data->m_lf[j];
2931 delete force;
2932 }
2933 m_data->m_lf.clear();
2934 #endif
2935 #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
2936 {
2937 btSoftMultiBodyDynamicsWorld* softWorld = getSoftWorld();
2938 if (softWorld)
2939 {
2940 for (i = softWorld->getSoftBodyArray().size() - 1; i >= 0; i--)
2941 {
2942 btSoftBody* sb = softWorld->getSoftBodyArray()[i];
2943 softWorld->removeSoftBody(sb);
2944 delete sb;
2945 }
2946 }
2947 }
2948 #endif //SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
2949
2950 #ifndef SKIP_DEFORMABLE_BODY
2951 {
2952 btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld();
2953 if (deformWorld)
2954 {
2955 for (i = deformWorld->getSoftBodyArray().size() - 1; i >= 0; i--)
2956 {
2957 btSoftBody* sb = deformWorld->getSoftBodyArray()[i];
2958 deformWorld->removeSoftBody(sb);
2959 delete sb;
2960 }
2961 }
2962 }
2963 #endif
2964 }
2965
2966 for (int i = 0; i < constraints.size(); i++)
2967 {
2968 delete constraints[i];
2969 }
2970 constraints.clear();
2971 for (int i = 0; i < mbconstraints.size(); i++)
2972 {
2973 delete mbconstraints[i];
2974 }
2975 mbconstraints.clear();
2976 //delete collision shapes
2977 for (int j = 0; j < m_data->m_collisionShapes.size(); j++)
2978 {
2979 btCollisionShape* shape = m_data->m_collisionShapes[j];
2980
2981 //check for internal edge utility, delete memory
2982 if (shape->getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE)
2983 {
2984 btBvhTriangleMeshShape* trimesh = (btBvhTriangleMeshShape*)shape;
2985 if (trimesh->getTriangleInfoMap())
2986 {
2987 delete trimesh->getTriangleInfoMap();
2988 }
2989 }
2990 if (shape->getShapeType() == TERRAIN_SHAPE_PROXYTYPE)
2991 {
2992 btHeightfieldTerrainShape* terrain = (btHeightfieldTerrainShape*)shape;
2993 if (terrain->getTriangleInfoMap())
2994 {
2995 delete terrain->getTriangleInfoMap();
2996 }
2997 }
2998 delete shape;
2999 }
3000 for (int j = 0; j < m_data->m_heightfieldDatas.size(); j++)
3001 {
3002 delete[] m_data->m_heightfieldDatas[j];
3003 }
3004
3005 for (int j = 0; j < m_data->m_meshInterfaces.size(); j++)
3006 {
3007 delete m_data->m_meshInterfaces[j];
3008 }
3009
3010 if (m_data->m_guiHelper)
3011 {
3012 for (int j = 0; j < m_data->m_allocatedTextures.size(); j++)
3013 {
3014 int texId = m_data->m_allocatedTextures[j];
3015 m_data->m_guiHelper->removeTexture(texId);
3016 }
3017 }
3018
3019 for (int i = 0; i < m_data->m_allocatedTexturesRequireFree.size(); i++)
3020 {
3021 //we can't free them right away, due to caching based on memory pointer in PhysicsServerExample
3022 free(m_data->m_allocatedTexturesRequireFree[i]);
3023 }
3024 m_data->m_heightfieldDatas.clear();
3025 m_data->m_allocatedTextures.clear();
3026 m_data->m_allocatedTexturesRequireFree.clear();
3027 m_data->m_meshInterfaces.clear();
3028 m_data->m_collisionShapes.clear();
3029 m_data->m_bulletCollisionShape2UrdfCollision.clear();
3030 m_data->m_graphicsIndexToSegmentationMask.clear();
3031
3032 delete m_data->m_dynamicsWorld;
3033 m_data->m_dynamicsWorld = 0;
3034
3035 delete m_data->m_remoteDebugDrawer;
3036 m_data->m_remoteDebugDrawer = 0;
3037
3038 #if !defined(SKIP_DEFORMABLE_BODY) && !defined(SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD)
3039 delete m_data->m_deformablebodySolver;
3040 m_data->m_deformablebodySolver = 0;
3041 #endif
3042
3043 delete m_data->m_solver;
3044 m_data->m_solver = 0;
3045
3046 delete m_data->m_broadphase;
3047 m_data->m_broadphase = 0;
3048
3049 delete m_data->m_pairCache;
3050 m_data->m_pairCache = 0;
3051
3052 delete m_data->m_broadphaseCollisionFilterCallback;
3053 m_data->m_broadphaseCollisionFilterCallback = 0;
3054
3055 delete m_data->m_dispatcher;
3056 m_data->m_dispatcher = 0;
3057
3058 delete m_data->m_collisionConfiguration;
3059 m_data->m_collisionConfiguration = 0;
3060 m_data->m_userConstraintUIDGenerator = 1;
3061 #ifdef STATIC_LINK_SPD_PLUGIN
3062 for (int i = 0; i < m_data->m_rbdModels.size(); i++)
3063 {
3064 delete *(m_data->m_rbdModels.getAtIndex(i));
3065 }
3066 m_data->m_rbdModels.clear();
3067 #endif //STATIC_LINK_SPD_PLUGIN
3068 }
3069
supportsJointMotor(btMultiBody * mb,int mbLinkIndex)3070 bool PhysicsServerCommandProcessor::supportsJointMotor(btMultiBody* mb, int mbLinkIndex)
3071 {
3072 bool canHaveMotor = (mb->getLink(mbLinkIndex).m_jointType == btMultibodyLink::eRevolute || mb->getLink(mbLinkIndex).m_jointType == btMultibodyLink::ePrismatic);
3073 return canHaveMotor;
3074 }
3075
3076 //for testing, create joint motors for revolute and prismatic joints
createJointMotors(btMultiBody * mb)3077 void PhysicsServerCommandProcessor::createJointMotors(btMultiBody* mb)
3078 {
3079 int numLinks = mb->getNumLinks();
3080 for (int i = 0; i < numLinks; i++)
3081 {
3082 int mbLinkIndex = i;
3083 float maxMotorImpulse = 1.f;
3084
3085 if (supportsJointMotor(mb, mbLinkIndex))
3086 {
3087 int dof = 0;
3088 btScalar desiredVelocity = 0.f;
3089 btMultiBodyJointMotor* motor = new btMultiBodyJointMotor(mb, mbLinkIndex, dof, desiredVelocity, maxMotorImpulse);
3090 motor->setPositionTarget(0, 0);
3091 motor->setVelocityTarget(0, 1);
3092 //motor->setRhsClamp(gRhsClamp);
3093 //motor->setMaxAppliedImpulse(0);
3094 mb->getLink(mbLinkIndex).m_userPtr = motor;
3095 #ifndef USE_DISCRETE_DYNAMICS_WORLD
3096 m_data->m_dynamicsWorld->addMultiBodyConstraint(motor);
3097 #endif
3098 motor->finalizeMultiDof();
3099 }
3100 if (mb->getLink(mbLinkIndex).m_jointType == btMultibodyLink::eSpherical)
3101 {
3102 btMultiBodySphericalJointMotor* motor = new btMultiBodySphericalJointMotor(mb, mbLinkIndex, 1000 * maxMotorImpulse);
3103 mb->getLink(mbLinkIndex).m_userPtr = motor;
3104 #ifndef USE_DISCRETE_DYNAMICS_WORLD
3105 m_data->m_dynamicsWorld->addMultiBodyConstraint(motor);
3106 #endif
3107 motor->finalizeMultiDof();
3108 }
3109 }
3110 }
3111
addUserData(int bodyUniqueId,int linkIndex,int visualShapeIndex,const char * key,const char * valueBytes,int valueLength,int valueType)3112 int PhysicsServerCommandProcessor::addUserData(int bodyUniqueId, int linkIndex, int visualShapeIndex, const char* key, const char* valueBytes, int valueLength, int valueType)
3113 {
3114 InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId);
3115 if (!body)
3116 {
3117 return -1;
3118 }
3119
3120 SharedMemoryUserDataHashKey userDataIdentifier(key, bodyUniqueId, linkIndex, visualShapeIndex);
3121
3122 int* userDataHandlePtr = m_data->m_userDataHandleLookup.find(userDataIdentifier);
3123 int userDataHandle = userDataHandlePtr ? *userDataHandlePtr : m_data->m_userDataHandles.allocHandle();
3124
3125 SharedMemoryUserData* userData = m_data->m_userDataHandles.getHandle(userDataHandle);
3126 if (!userData)
3127 {
3128 return -1;
3129 }
3130
3131 if (!userDataHandlePtr)
3132 {
3133 userData->m_key = key;
3134 userData->m_bodyUniqueId = bodyUniqueId;
3135 userData->m_linkIndex = linkIndex;
3136 userData->m_visualShapeIndex = visualShapeIndex;
3137 m_data->m_userDataHandleLookup.insert(userDataIdentifier, userDataHandle);
3138 body->m_userDataHandles.push_back(userDataHandle);
3139 }
3140 userData->replaceValue(valueBytes, valueLength, valueType);
3141 return userDataHandle;
3142 }
3143
addUserData(const btHashMap<btHashString,std::string> & user_data_entries,int bodyUniqueId,int linkIndex,int visualShapeIndex)3144 void PhysicsServerCommandProcessor::addUserData(const btHashMap<btHashString, std::string>& user_data_entries, int bodyUniqueId, int linkIndex, int visualShapeIndex)
3145 {
3146 for (int i = 0; i < user_data_entries.size(); ++i)
3147 {
3148 const std::string key = user_data_entries.getKeyAtIndex(i).m_string1;
3149 const std::string* value = user_data_entries.getAtIndex(i);
3150 if (value)
3151 {
3152 addUserData(bodyUniqueId, linkIndex, visualShapeIndex, key.c_str(), value->c_str(),
3153 value->size() + 1, USER_DATA_VALUE_TYPE_STRING);
3154 }
3155 }
3156 }
3157
processImportedObjects(const char * fileName,char * bufferServerToClient,int bufferSizeInBytes,bool useMultiBody,int flags,URDFImporterInterface & u2b)3158 bool PhysicsServerCommandProcessor::processImportedObjects(const char* fileName, char* bufferServerToClient, int bufferSizeInBytes, bool useMultiBody, int flags, URDFImporterInterface& u2b)
3159 {
3160 bool loadOk = true;
3161
3162 btTransform rootTrans;
3163 rootTrans.setIdentity();
3164 if (m_data->m_verboseOutput)
3165 {
3166 b3Printf("loaded %s OK!", fileName);
3167 }
3168 SaveWorldObjectData sd;
3169 sd.m_fileName = fileName;
3170
3171 for (int m = 0; m < u2b.getNumModels(); m++)
3172 {
3173 u2b.activateModel(m);
3174 btMultiBody* mb = 0;
3175 btRigidBody* rb = 0;
3176
3177 //get a body index
3178 int bodyUniqueId = m_data->m_bodyHandles.allocHandle();
3179
3180 InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(bodyUniqueId);
3181
3182 sd.m_bodyUniqueIds.push_back(bodyUniqueId);
3183
3184 u2b.setBodyUniqueId(bodyUniqueId);
3185 {
3186 btScalar mass = 0;
3187 bodyHandle->m_rootLocalInertialFrame.setIdentity();
3188 bodyHandle->m_bodyName = u2b.getBodyName();
3189 btVector3 localInertiaDiagonal(0, 0, 0);
3190 int urdfLinkIndex = u2b.getRootLinkIndex();
3191 u2b.getMassAndInertia2(urdfLinkIndex, mass, localInertiaDiagonal, bodyHandle->m_rootLocalInertialFrame, flags);
3192 }
3193
3194 //todo: move these internal API called inside the 'ConvertURDF2Bullet' call, hidden from the user
3195 //int rootLinkIndex = u2b.getRootLinkIndex();
3196 //b3Printf("urdf root link index = %d\n",rootLinkIndex);
3197 MyMultiBodyCreator creation(m_data->m_guiHelper);
3198
3199 u2b.getRootTransformInWorld(rootTrans);
3200 //CUF_RESERVED is a temporary flag, for backward compatibility purposes
3201 flags |= CUF_RESERVED;
3202
3203 if (flags & CUF_ENABLE_CACHED_GRAPHICS_SHAPES)
3204 {
3205 {
3206 UrdfVisualShapeCache* tmpPtr = m_data->m_cachedVUrdfisualShapes[fileName];
3207 if (tmpPtr == 0)
3208 {
3209 m_data->m_cachedVUrdfisualShapes.insert(fileName, UrdfVisualShapeCache());
3210 }
3211 }
3212 UrdfVisualShapeCache* cachedVisualShapesPtr = m_data->m_cachedVUrdfisualShapes[fileName];
3213
3214 ConvertURDF2Bullet(u2b, creation, rootTrans, m_data->m_dynamicsWorld, useMultiBody, u2b.getPathPrefix(), flags, cachedVisualShapesPtr);
3215
3216 }
3217 else
3218 {
3219
3220 ConvertURDF2Bullet(u2b, creation, rootTrans, m_data->m_dynamicsWorld, useMultiBody, u2b.getPathPrefix(), flags);
3221
3222 }
3223
3224 mb = creation.getBulletMultiBody();
3225 rb = creation.getRigidBody();
3226 if (rb)
3227 rb->setUserIndex2(bodyUniqueId);
3228
3229 if (mb)
3230 mb->setUserIndex2(bodyUniqueId);
3231
3232 if (mb)
3233 {
3234 bodyHandle->m_multiBody = mb;
3235
3236 m_data->m_sdfRecentLoadedBodies.push_back(bodyUniqueId);
3237
3238 int segmentationMask = bodyUniqueId;
3239
3240 {
3241 int graphicsIndex = -1;
3242 if (mb->getBaseCollider())
3243 {
3244 graphicsIndex = mb->getBaseCollider()->getUserIndex();
3245 }
3246 if (graphicsIndex >= 0)
3247 {
3248 if (m_data->m_graphicsIndexToSegmentationMask.size() < (graphicsIndex + 1))
3249 {
3250 m_data->m_graphicsIndexToSegmentationMask.resize(graphicsIndex + 1);
3251 }
3252 m_data->m_graphicsIndexToSegmentationMask[graphicsIndex] = segmentationMask;
3253 }
3254 }
3255
3256 createJointMotors(mb);
3257
3258 #ifdef B3_ENABLE_TINY_AUDIO
3259 {
3260 SDFAudioSource audioSource;
3261 int urdfRootLink = u2b.getRootLinkIndex(); //LinkIndex = creation.m_mb2urdfLink[-1];
3262 if (u2b.getLinkAudioSource(urdfRootLink, audioSource))
3263 {
3264 int flags = mb->getBaseCollider()->getCollisionFlags();
3265 mb->getBaseCollider()->setCollisionFlags(flags | btCollisionObject::CF_HAS_COLLISION_SOUND_TRIGGER);
3266 audioSource.m_userIndex = m_data->m_soundEngine.loadWavFile(audioSource.m_uri.c_str());
3267 if (audioSource.m_userIndex >= 0)
3268 {
3269 bodyHandle->m_audioSources.insert(-1, audioSource);
3270 }
3271 }
3272 }
3273 #endif
3274 //disable serialization of the collision objects (they are too big, and the client likely doesn't need them);
3275
3276 bodyHandle->m_linkLocalInertialFrames.reserve(mb->getNumLinks());
3277 for (int i = 0; i < mb->getNumLinks(); i++)
3278 {
3279 //disable serialization of the collision objects
3280
3281 int urdfLinkIndex = creation.m_mb2urdfLink[i];
3282 btScalar mass;
3283 btVector3 localInertiaDiagonal(0, 0, 0);
3284 btTransform localInertialFrame;
3285 u2b.getMassAndInertia2(urdfLinkIndex, mass, localInertiaDiagonal, localInertialFrame, flags);
3286 bodyHandle->m_linkLocalInertialFrames.push_back(localInertialFrame);
3287
3288 std::string* linkName = new std::string(u2b.getLinkName(urdfLinkIndex).c_str());
3289 m_data->m_strings.push_back(linkName);
3290
3291 mb->getLink(i).m_linkName = linkName->c_str();
3292
3293 {
3294 int graphicsIndex = -1;
3295 if (mb->getLinkCollider(i))
3296 {
3297 graphicsIndex = mb->getLinkCollider(i)->getUserIndex();
3298 }
3299 if (graphicsIndex >= 0)
3300 {
3301 int linkIndex = i;
3302 if (m_data->m_graphicsIndexToSegmentationMask.size() < (graphicsIndex + 1))
3303 {
3304 m_data->m_graphicsIndexToSegmentationMask.resize(graphicsIndex + 1);
3305 }
3306 int segmentationMask = bodyUniqueId + ((linkIndex + 1) << 24);
3307 m_data->m_graphicsIndexToSegmentationMask[graphicsIndex] = segmentationMask;
3308 }
3309 }
3310
3311 std::string* jointName = new std::string(u2b.getJointName(urdfLinkIndex).c_str());
3312 m_data->m_strings.push_back(jointName);
3313
3314 mb->getLink(i).m_jointName = jointName->c_str();
3315
3316 #ifdef B3_ENABLE_TINY_AUDIO
3317 {
3318 SDFAudioSource audioSource;
3319 int urdfLinkIndex = creation.m_mb2urdfLink[i];
3320 if (u2b.getLinkAudioSource(urdfLinkIndex, audioSource))
3321 {
3322 int flags = mb->getLink(i).m_collider->getCollisionFlags();
3323 mb->getLink(i).m_collider->setCollisionFlags(flags | btCollisionObject::CF_HAS_COLLISION_SOUND_TRIGGER);
3324 audioSource.m_userIndex = m_data->m_soundEngine.loadWavFile(audioSource.m_uri.c_str());
3325 if (audioSource.m_userIndex >= 0)
3326 {
3327 bodyHandle->m_audioSources.insert(i, audioSource);
3328 }
3329 }
3330 }
3331 #endif
3332 }
3333 std::string* baseName = new std::string(u2b.getLinkName(u2b.getRootLinkIndex()));
3334 m_data->m_strings.push_back(baseName);
3335 mb->setBaseName(baseName->c_str());
3336
3337 #if 0
3338 btAlignedObjectArray<char> urdf;
3339 mb->dumpUrdf(urdf);
3340 FILE* f = fopen("e:/pybullet.urdf", "w");
3341 if (f)
3342 {
3343 fwrite(&urdf[0], urdf.size(), 1, f);
3344 fclose(f);
3345 }
3346 #endif
3347 }
3348 else
3349 {
3350 int segmentationMask = bodyUniqueId;
3351 if (rb)
3352 {
3353 int graphicsIndex = -1;
3354 {
3355 graphicsIndex = rb->getUserIndex();
3356 }
3357 if (graphicsIndex >= 0)
3358 {
3359 if (m_data->m_graphicsIndexToSegmentationMask.size() < (graphicsIndex + 1))
3360 {
3361 m_data->m_graphicsIndexToSegmentationMask.resize(graphicsIndex + 1);
3362 }
3363 m_data->m_graphicsIndexToSegmentationMask[graphicsIndex] = segmentationMask;
3364 }
3365 }
3366
3367 //b3Warning("No multibody loaded from URDF. Could add btRigidBody+btTypedConstraint solution later.");
3368 bodyHandle->m_rigidBody = rb;
3369 rb->setUserIndex2(bodyUniqueId);
3370 m_data->m_sdfRecentLoadedBodies.push_back(bodyUniqueId);
3371
3372 std::string* baseName = new std::string(u2b.getLinkName(u2b.getRootLinkIndex()));
3373 m_data->m_strings.push_back(baseName);
3374 bodyHandle->m_bodyName = *baseName;
3375
3376 int numJoints = creation.getNum6DofConstraints();
3377 bodyHandle->m_rigidBodyJoints.reserve(numJoints);
3378 bodyHandle->m_rigidBodyJointNames.reserve(numJoints);
3379 bodyHandle->m_rigidBodyLinkNames.reserve(numJoints);
3380 for (int i = 0; i < numJoints; i++)
3381 {
3382 int urdfLinkIndex = creation.m_mb2urdfLink[i];
3383
3384 btGeneric6DofSpring2Constraint* con = creation.get6DofConstraint(i);
3385
3386 std::string* linkName = new std::string(u2b.getLinkName(urdfLinkIndex).c_str());
3387 m_data->m_strings.push_back(linkName);
3388
3389 std::string* jointName = new std::string(u2b.getJointName(urdfLinkIndex).c_str());
3390 m_data->m_strings.push_back(jointName);
3391
3392 bodyHandle->m_rigidBodyJointNames.push_back(*jointName);
3393 bodyHandle->m_rigidBodyLinkNames.push_back(*linkName);
3394
3395 bodyHandle->m_rigidBodyJoints.push_back(con);
3396 }
3397 }
3398
3399 {
3400 if (m_data->m_pluginManager.getRenderInterface())
3401 {
3402 int currentOpenGLTextureIndex = 0;
3403 int totalNumVisualShapes = m_data->m_pluginManager.getRenderInterface()->getNumVisualShapes(bodyUniqueId);
3404
3405 for (int shapeIndex = 0; shapeIndex < totalNumVisualShapes; shapeIndex++)
3406 {
3407 b3VisualShapeData tmpShape;
3408 int success = m_data->m_pluginManager.getRenderInterface()->getVisualShapesData(bodyUniqueId, shapeIndex, &tmpShape);
3409 if (success)
3410 {
3411 if (tmpShape.m_tinyRendererTextureId >= 0)
3412 {
3413 int openglTextureUniqueId = -1;
3414
3415 //find companion opengl texture unique id and create a 'textureUid'
3416 if (currentOpenGLTextureIndex < u2b.getNumAllocatedTextures())
3417 {
3418 openglTextureUniqueId = u2b.getAllocatedTexture(currentOpenGLTextureIndex++);
3419 }
3420 //if (openglTextureUniqueId>=0)
3421 {
3422 int texHandle = m_data->m_textureHandles.allocHandle();
3423 InternalTextureHandle* texH = m_data->m_textureHandles.getHandle(texHandle);
3424 if (texH)
3425 {
3426 texH->m_tinyRendererTextureId = tmpShape.m_tinyRendererTextureId;
3427 texH->m_openglTextureId = openglTextureUniqueId;
3428 }
3429 }
3430 }
3431 }
3432 }
3433 }
3434 }
3435
3436 // Because the link order between UrdfModel and MultiBody may be different,
3437 // create a mapping from link name to link index in order to apply the user
3438 // data to the correct link in the MultiBody.
3439 btHashMap<btHashString, int> linkNameToIndexMap;
3440 if (bodyHandle->m_multiBody)
3441 {
3442 btMultiBody* mb = bodyHandle->m_multiBody;
3443 linkNameToIndexMap.insert(mb->getBaseName(), -1);
3444 for (int linkIndex = 0; linkIndex < mb->getNumLinks(); ++linkIndex)
3445 {
3446 linkNameToIndexMap.insert(mb->getLink(linkIndex).m_linkName, linkIndex);
3447 }
3448 }
3449
3450 const UrdfModel* urdfModel = u2b.getUrdfModel();
3451 if (urdfModel)
3452 {
3453 addUserData(urdfModel->m_userData, bodyUniqueId);
3454 for (int i = 0; i < urdfModel->m_links.size(); ++i)
3455 {
3456 const UrdfLink* link = *urdfModel->m_links.getAtIndex(i);
3457 int* linkIndex = linkNameToIndexMap.find(link->m_name.c_str());
3458 if (linkIndex)
3459 {
3460 addUserData(link->m_userData, bodyUniqueId, *linkIndex);
3461 for (int visualShapeIndex = 0; visualShapeIndex < link->m_visualArray.size(); ++visualShapeIndex)
3462 {
3463 addUserData(link->m_visualArray.at(visualShapeIndex).m_userData, bodyUniqueId, *linkIndex, visualShapeIndex);
3464 }
3465 }
3466 }
3467 }
3468
3469 b3Notification notification;
3470 notification.m_notificationType = BODY_ADDED;
3471 notification.m_bodyArgs.m_bodyUniqueId = bodyUniqueId;
3472 m_data->m_pluginManager.addNotification(notification);
3473 }
3474
3475 for (int i = 0; i < u2b.getNumAllocatedTextures(); i++)
3476 {
3477 int texId = u2b.getAllocatedTexture(i);
3478 m_data->m_allocatedTextures.push_back(texId);
3479 }
3480
3481 /*
3482 int texHandle = m_data->m_textureHandles.allocHandle();
3483 InternalTextureHandle* texH = m_data->m_textureHandles.getHandle(texHandle);
3484 if(texH)
3485 {
3486 texH->m_tinyRendererTextureId = -1;
3487 texH->m_openglTextureId = -1;
3488 */
3489
3490 for (int i = 0; i < u2b.getNumAllocatedMeshInterfaces(); i++)
3491 {
3492 m_data->m_meshInterfaces.push_back(u2b.getAllocatedMeshInterface(i));
3493 }
3494
3495 for (int i = 0; i < u2b.getNumAllocatedCollisionShapes(); i++)
3496 {
3497 btCollisionShape* shape = u2b.getAllocatedCollisionShape(i);
3498 m_data->m_collisionShapes.push_back(shape);
3499 UrdfCollision urdfCollision;
3500 if (u2b.getUrdfFromCollisionShape(shape, urdfCollision))
3501 {
3502 m_data->m_bulletCollisionShape2UrdfCollision.insert(shape, urdfCollision);
3503 }
3504 if (shape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE)
3505 {
3506 btCompoundShape* compound = (btCompoundShape*)shape;
3507 for (int c = 0; c < compound->getNumChildShapes(); c++)
3508 {
3509 btCollisionShape* childShape = compound->getChildShape(c);
3510 if (u2b.getUrdfFromCollisionShape(childShape, urdfCollision))
3511 {
3512 m_data->m_bulletCollisionShape2UrdfCollision.insert(childShape, urdfCollision);
3513 }
3514 }
3515 }
3516 }
3517
3518 m_data->m_saveWorldBodyData.push_back(sd);
3519
3520 syncPhysicsToGraphics2();
3521 return loadOk;
3522 }
3523
3524 struct MyMJCFLogger2 : public MJCFErrorLogger
3525 {
reportErrorMyMJCFLogger23526 virtual void reportError(const char* error)
3527 {
3528 b3Error(error);
3529 }
reportWarningMyMJCFLogger23530 virtual void reportWarning(const char* warning)
3531 {
3532 b3Warning(warning);
3533 }
printMessageMyMJCFLogger23534 virtual void printMessage(const char* msg)
3535 {
3536 b3Printf(msg);
3537 }
3538 };
3539
loadMjcf(const char * fileName,char * bufferServerToClient,int bufferSizeInBytes,bool useMultiBody,int flags)3540 bool PhysicsServerCommandProcessor::loadMjcf(const char* fileName, char* bufferServerToClient, int bufferSizeInBytes, bool useMultiBody, int flags)
3541 {
3542 btAssert(m_data->m_dynamicsWorld);
3543 if (!m_data->m_dynamicsWorld)
3544 {
3545 b3Error("loadSdf: No valid m_dynamicsWorld");
3546 return false;
3547 }
3548
3549 m_data->m_sdfRecentLoadedBodies.clear();
3550
3551 CommonFileIOInterface* fileIO = m_data->m_pluginManager.getFileIOInterface();
3552 BulletMJCFImporter u2b(m_data->m_guiHelper, m_data->m_pluginManager.getRenderInterface(), fileIO, flags);
3553
3554 bool useFixedBase = false;
3555 MyMJCFLogger2 logger;
3556 bool loadOk = u2b.loadMJCF(fileName, &logger, useFixedBase);
3557 if (loadOk)
3558 {
3559 processImportedObjects(fileName, bufferServerToClient, bufferSizeInBytes, useMultiBody, flags, u2b);
3560 }
3561 return loadOk;
3562 }
3563
loadSdf(const char * fileName,char * bufferServerToClient,int bufferSizeInBytes,bool useMultiBody,int flags,btScalar globalScaling)3564 bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferServerToClient, int bufferSizeInBytes, bool useMultiBody, int flags, btScalar globalScaling)
3565 {
3566 btAssert(m_data->m_dynamicsWorld);
3567 if (!m_data->m_dynamicsWorld)
3568 {
3569 b3Error("loadSdf: No valid m_dynamicsWorld");
3570 return false;
3571 }
3572
3573 m_data->m_sdfRecentLoadedBodies.clear();
3574 CommonFileIOInterface* fileIO = m_data->m_pluginManager.getFileIOInterface();
3575 BulletURDFImporter u2b(m_data->m_guiHelper, m_data->m_pluginManager.getRenderInterface(), fileIO, globalScaling, flags);
3576 u2b.setEnableTinyRenderer(m_data->m_enableTinyRenderer);
3577
3578 bool forceFixedBase = false;
3579 bool loadOk = u2b.loadSDF(fileName, forceFixedBase);
3580
3581 if (loadOk)
3582 {
3583 processImportedObjects(fileName, bufferServerToClient, bufferSizeInBytes, useMultiBody, flags, u2b);
3584 }
3585 return loadOk;
3586 }
3587
loadUrdf(const char * fileName,const btVector3 & pos,const btQuaternion & orn,bool useMultiBody,bool useFixedBase,int * bodyUniqueIdPtr,char * bufferServerToClient,int bufferSizeInBytes,int orgFlags,btScalar globalScaling)3588 bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVector3& pos, const btQuaternion& orn,
3589 bool useMultiBody, bool useFixedBase, int* bodyUniqueIdPtr, char* bufferServerToClient, int bufferSizeInBytes, int orgFlags, btScalar globalScaling)
3590 {
3591 //clear the LOAD_SDF_FILE=1 bit, which is reserved for internal use of loadSDF command.
3592 int flags = orgFlags & ~1;
3593 m_data->m_sdfRecentLoadedBodies.clear();
3594 *bodyUniqueIdPtr = -1;
3595
3596 BT_PROFILE("loadURDF");
3597 btAssert(m_data->m_dynamicsWorld);
3598 if (!m_data->m_dynamicsWorld)
3599 {
3600 b3Error("loadUrdf: No valid m_dynamicsWorld");
3601 return false;
3602 }
3603
3604 CommonFileIOInterface* fileIO = m_data->m_pluginManager.getFileIOInterface();
3605 BulletURDFImporter u2b(m_data->m_guiHelper, m_data->m_pluginManager.getRenderInterface(), fileIO, globalScaling, flags);
3606 u2b.setEnableTinyRenderer(m_data->m_enableTinyRenderer);
3607 bool loadOk = u2b.loadURDF(fileName, useFixedBase);
3608
3609 if (loadOk)
3610 {
3611 btTransform rootTrans;
3612 rootTrans.setOrigin(pos);
3613 rootTrans.setRotation(orn);
3614 u2b.setRootTransformInWorld(rootTrans);
3615 if (!(u2b.getDeformableModel().m_visualFileName.empty()))
3616 {
3617 bool use_self_collision = false;
3618 use_self_collision = (flags & CUF_USE_SELF_COLLISION);
3619 bool ok = processDeformable(u2b.getDeformableModel(), pos, orn, bodyUniqueIdPtr, bufferServerToClient, bufferSizeInBytes, globalScaling, use_self_collision);
3620 if (ok)
3621 {
3622 const UrdfModel* urdfModel = u2b.getUrdfModel();
3623 if (urdfModel)
3624 {
3625 addUserData(urdfModel->m_userData, *bodyUniqueIdPtr);
3626 }
3627 return true;
3628 }
3629 else
3630 {
3631 return false;
3632 }
3633 }
3634 bool ok = processImportedObjects(fileName, bufferServerToClient, bufferSizeInBytes, useMultiBody, flags, u2b);
3635 if (ok)
3636 {
3637 if (m_data->m_sdfRecentLoadedBodies.size() == 1)
3638 {
3639 *bodyUniqueIdPtr = m_data->m_sdfRecentLoadedBodies[0];
3640 }
3641 m_data->m_sdfRecentLoadedBodies.clear();
3642 }
3643 return ok;
3644 }
3645 return false;
3646 }
3647
replayLogCommand(char * bufferServerToClient,int bufferSizeInBytes)3648 void PhysicsServerCommandProcessor::replayLogCommand(char* bufferServerToClient, int bufferSizeInBytes)
3649 {
3650 if (m_data->m_logPlayback)
3651 {
3652 SharedMemoryCommand clientCmd;
3653 SharedMemoryStatus serverStatus;
3654
3655 bool hasCommand = m_data->m_logPlayback->processNextCommand(&clientCmd);
3656 if (hasCommand)
3657 {
3658 processCommand(clientCmd, serverStatus, bufferServerToClient, bufferSizeInBytes);
3659 }
3660 }
3661 }
3662
createBodyInfoStream(int bodyUniqueId,char * bufferServerToClient,int bufferSizeInBytes)3663 int PhysicsServerCommandProcessor::createBodyInfoStream(int bodyUniqueId, char* bufferServerToClient, int bufferSizeInBytes)
3664 {
3665 int streamSizeInBytes = 0;
3666 //serialize the btMultiBody and send the data to the client. This is one way to get the link/joint names across the (shared memory) wire
3667
3668 InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(bodyUniqueId);
3669 if (!bodyHandle) return 0;
3670 if (bodyHandle->m_multiBody)
3671 {
3672 btMultiBody* mb = bodyHandle->m_multiBody;
3673 btDefaultSerializer ser(bufferSizeInBytes, (unsigned char*)bufferServerToClient);
3674
3675 ser.startSerialization();
3676
3677 //disable serialization of the collision objects (they are too big, and the client likely doesn't need them);
3678 ser.m_skipPointers.insert(mb->getBaseCollider(), 0);
3679 if (mb->getBaseName())
3680 {
3681 ser.registerNameForPointer(mb->getBaseName(), mb->getBaseName());
3682 }
3683
3684 bodyHandle->m_linkLocalInertialFrames.reserve(mb->getNumLinks());
3685 for (int i = 0; i < mb->getNumLinks(); i++)
3686 {
3687 //disable serialization of the collision objects
3688 ser.m_skipPointers.insert(mb->getLink(i).m_collider, 0);
3689 ser.registerNameForPointer(mb->getLink(i).m_linkName, mb->getLink(i).m_linkName);
3690 ser.registerNameForPointer(mb->getLink(i).m_jointName, mb->getLink(i).m_jointName);
3691 }
3692
3693 ser.registerNameForPointer(mb->getBaseName(), mb->getBaseName());
3694
3695 int len = mb->calculateSerializeBufferSize();
3696 btChunk* chunk = ser.allocate(len, 1);
3697 const char* structType = mb->serialize(chunk->m_oldPtr, &ser);
3698 ser.finalizeChunk(chunk, structType, BT_MULTIBODY_CODE, mb);
3699 streamSizeInBytes = ser.getCurrentBufferSize();
3700 }
3701 else if (bodyHandle->m_rigidBody)
3702 {
3703 btRigidBody* rb = bodyHandle->m_rigidBody;
3704 btDefaultSerializer ser(bufferSizeInBytes, (unsigned char*)bufferServerToClient);
3705
3706 ser.startSerialization();
3707 ser.registerNameForPointer(bodyHandle->m_rigidBody, bodyHandle->m_bodyName.c_str());
3708 //disable serialization of the collision objects (they are too big, and the client likely doesn't need them);
3709 for (int i = 0; i < bodyHandle->m_rigidBodyJoints.size(); i++)
3710 {
3711 const btGeneric6DofSpring2Constraint* con = bodyHandle->m_rigidBodyJoints.at(i);
3712
3713 ser.registerNameForPointer(con, bodyHandle->m_rigidBodyJointNames[i].c_str());
3714 ser.registerNameForPointer(&con->getRigidBodyB(), bodyHandle->m_rigidBodyLinkNames[i].c_str());
3715
3716 const btRigidBody& bodyA = con->getRigidBodyA();
3717
3718 int len = con->calculateSerializeBufferSize();
3719 btChunk* chunk = ser.allocate(len, 1);
3720 const char* structType = con->serialize(chunk->m_oldPtr, &ser);
3721 ser.finalizeChunk(chunk, structType, BT_CONSTRAINT_CODE, (void*)con);
3722 }
3723 streamSizeInBytes = ser.getCurrentBufferSize();
3724 }
3725 #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
3726 else if (bodyHandle->m_softBody)
3727 {
3728 //minimum serialization, registerNameForPointer
3729 btSoftBody* sb = bodyHandle->m_softBody;
3730 btDefaultSerializer ser(bufferSizeInBytes, (unsigned char*)bufferServerToClient);
3731
3732 ser.startSerialization();
3733 int len = sb->calculateSerializeBufferSize();
3734 btChunk* chunk = ser.allocate(len, 1);
3735 const char* structType = sb->serialize(chunk->m_oldPtr, &ser);
3736 ser.finalizeChunk(chunk, structType, BT_SOFTBODY_CODE, sb);
3737 streamSizeInBytes = ser.getCurrentBufferSize();
3738 }
3739 #endif
3740 return streamSizeInBytes;
3741 }
3742
processStateLoggingCommand(const struct SharedMemoryCommand & clientCmd,struct SharedMemoryStatus & serverStatusOut,char * bufferServerToClient,int bufferSizeInBytes)3743 bool PhysicsServerCommandProcessor::processStateLoggingCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
3744 {
3745 BT_PROFILE("CMD_STATE_LOGGING");
3746
3747 serverStatusOut.m_type = CMD_STATE_LOGGING_FAILED;
3748 bool hasStatus = true;
3749
3750 if (clientCmd.m_updateFlags & STATE_LOGGING_START_LOG)
3751 {
3752 if (clientCmd.m_stateLoggingArguments.m_logType == STATE_LOGGING_ALL_COMMANDS)
3753 {
3754 if (m_data->m_commandLogger == 0)
3755 {
3756 enableCommandLogging(true, clientCmd.m_stateLoggingArguments.m_fileName);
3757 serverStatusOut.m_type = CMD_STATE_LOGGING_START_COMPLETED;
3758 int loggerUid = m_data->m_stateLoggersUniqueId++;
3759 m_data->m_commandLoggingUid = loggerUid;
3760 serverStatusOut.m_stateLoggingResultArgs.m_loggingUniqueId = loggerUid;
3761 }
3762 }
3763
3764 if (clientCmd.m_stateLoggingArguments.m_logType == STATE_REPLAY_ALL_COMMANDS)
3765 {
3766 if (m_data->m_logPlayback == 0)
3767 {
3768 replayFromLogFile(clientCmd.m_stateLoggingArguments.m_fileName);
3769 serverStatusOut.m_type = CMD_STATE_LOGGING_START_COMPLETED;
3770 int loggerUid = m_data->m_stateLoggersUniqueId++;
3771 m_data->m_logPlaybackUid = loggerUid;
3772 serverStatusOut.m_stateLoggingResultArgs.m_loggingUniqueId = loggerUid;
3773 }
3774 }
3775
3776 if (clientCmd.m_stateLoggingArguments.m_logType == STATE_LOGGING_PROFILE_TIMINGS)
3777 {
3778 if (m_data->m_profileTimingLoggingUid < 0)
3779 {
3780 b3ChromeUtilsStartTimings();
3781 m_data->m_profileTimingFileName = clientCmd.m_stateLoggingArguments.m_fileName;
3782 int loggerUid = m_data->m_stateLoggersUniqueId++;
3783 serverStatusOut.m_type = CMD_STATE_LOGGING_START_COMPLETED;
3784 serverStatusOut.m_stateLoggingResultArgs.m_loggingUniqueId = loggerUid;
3785 m_data->m_profileTimingLoggingUid = loggerUid;
3786 }
3787 }
3788 if (clientCmd.m_stateLoggingArguments.m_logType == STATE_LOGGING_VIDEO_MP4)
3789 {
3790 //if (clientCmd.m_stateLoggingArguments.m_fileName)
3791 {
3792 int loggerUid = m_data->m_stateLoggersUniqueId++;
3793 VideoMP4Loggger* logger = new VideoMP4Loggger(loggerUid, clientCmd.m_stateLoggingArguments.m_fileName, this->m_data->m_guiHelper);
3794 m_data->m_stateLoggers.push_back(logger);
3795 serverStatusOut.m_type = CMD_STATE_LOGGING_START_COMPLETED;
3796 serverStatusOut.m_stateLoggingResultArgs.m_loggingUniqueId = loggerUid;
3797 }
3798 }
3799
3800 if (clientCmd.m_stateLoggingArguments.m_logType == STATE_LOGGING_MINITAUR)
3801 {
3802 std::string fileName = clientCmd.m_stateLoggingArguments.m_fileName;
3803 //either provide the minitaur by object unique Id, or search for first multibody with 8 motors...
3804
3805 if ((clientCmd.m_updateFlags & STATE_LOGGING_FILTER_OBJECT_UNIQUE_ID) && (clientCmd.m_stateLoggingArguments.m_numBodyUniqueIds > 0))
3806 {
3807 int bodyUniqueId = clientCmd.m_stateLoggingArguments.m_bodyUniqueIds[0];
3808 InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId);
3809 if (body)
3810 {
3811 if (body->m_multiBody)
3812 {
3813 btAlignedObjectArray<std::string> motorNames;
3814 motorNames.push_back("motor_front_leftR_joint");
3815 motorNames.push_back("motor_front_leftL_joint");
3816 motorNames.push_back("motor_back_leftR_joint");
3817 motorNames.push_back("motor_back_leftL_joint");
3818 motorNames.push_back("motor_front_rightL_joint");
3819 motorNames.push_back("motor_front_rightR_joint");
3820 motorNames.push_back("motor_back_rightL_joint");
3821 motorNames.push_back("motor_back_rightR_joint");
3822
3823 btAlignedObjectArray<int> motorIdList;
3824 for (int m = 0; m < motorNames.size(); m++)
3825 {
3826 for (int i = 0; i < body->m_multiBody->getNumLinks(); i++)
3827 {
3828 std::string jointName;
3829 if (body->m_multiBody->getLink(i).m_jointName)
3830 {
3831 jointName = body->m_multiBody->getLink(i).m_jointName;
3832 }
3833 if (motorNames[m] == jointName)
3834 {
3835 motorIdList.push_back(i);
3836 }
3837 }
3838 }
3839
3840 if (motorIdList.size() == 8)
3841 {
3842 int loggerUid = m_data->m_stateLoggersUniqueId++;
3843 MinitaurStateLogger* logger = new MinitaurStateLogger(loggerUid, fileName, body->m_multiBody, motorIdList);
3844 m_data->m_stateLoggers.push_back(logger);
3845 serverStatusOut.m_type = CMD_STATE_LOGGING_START_COMPLETED;
3846 serverStatusOut.m_stateLoggingResultArgs.m_loggingUniqueId = loggerUid;
3847 }
3848 }
3849 }
3850 }
3851 }
3852 #ifndef USE_DISCRETE_DYNAMICS_WORLD
3853 if (clientCmd.m_stateLoggingArguments.m_logType == STATE_LOGGING_GENERIC_ROBOT)
3854 {
3855
3856 std::string fileName = clientCmd.m_stateLoggingArguments.m_fileName;
3857
3858 int loggerUid = m_data->m_stateLoggersUniqueId++;
3859 int maxLogDof = 12;
3860 if ((clientCmd.m_updateFlags & STATE_LOGGING_MAX_LOG_DOF))
3861 {
3862 maxLogDof = clientCmd.m_stateLoggingArguments.m_maxLogDof;
3863 }
3864
3865 int logFlags = 0;
3866 if (clientCmd.m_updateFlags & STATE_LOGGING_LOG_FLAGS)
3867 {
3868 logFlags = clientCmd.m_stateLoggingArguments.m_logFlags;
3869 }
3870 GenericRobotStateLogger* logger = new GenericRobotStateLogger(loggerUid, fileName, m_data->m_dynamicsWorld, maxLogDof, logFlags);
3871
3872 if ((clientCmd.m_updateFlags & STATE_LOGGING_FILTER_OBJECT_UNIQUE_ID) && (clientCmd.m_stateLoggingArguments.m_numBodyUniqueIds > 0))
3873 {
3874 logger->m_filterObjectUniqueId = true;
3875 for (int i = 0; i < clientCmd.m_stateLoggingArguments.m_numBodyUniqueIds; ++i)
3876 {
3877 int objectUniqueId = clientCmd.m_stateLoggingArguments.m_bodyUniqueIds[i];
3878 logger->m_bodyIdList.push_back(objectUniqueId);
3879 }
3880 }
3881
3882 m_data->m_stateLoggers.push_back(logger);
3883 serverStatusOut.m_type = CMD_STATE_LOGGING_START_COMPLETED;
3884 serverStatusOut.m_stateLoggingResultArgs.m_loggingUniqueId = loggerUid;
3885 }
3886 if (clientCmd.m_stateLoggingArguments.m_logType == STATE_LOGGING_CONTACT_POINTS)
3887 {
3888 std::string fileName = clientCmd.m_stateLoggingArguments.m_fileName;
3889 int loggerUid = m_data->m_stateLoggersUniqueId++;
3890 ContactPointsStateLogger* logger = new ContactPointsStateLogger(loggerUid, fileName, m_data->m_dynamicsWorld);
3891 if ((clientCmd.m_updateFlags & STATE_LOGGING_FILTER_LINK_INDEX_A) && clientCmd.m_stateLoggingArguments.m_linkIndexA >= -1)
3892 {
3893 logger->m_filterLinkA = true;
3894 logger->m_linkIndexA = clientCmd.m_stateLoggingArguments.m_linkIndexA;
3895 }
3896 if ((clientCmd.m_updateFlags & STATE_LOGGING_FILTER_LINK_INDEX_B) && clientCmd.m_stateLoggingArguments.m_linkIndexB >= -1)
3897 {
3898 logger->m_filterLinkB = true;
3899 logger->m_linkIndexB = clientCmd.m_stateLoggingArguments.m_linkIndexB;
3900 }
3901 if ((clientCmd.m_updateFlags & STATE_LOGGING_FILTER_BODY_UNIQUE_ID_A) && clientCmd.m_stateLoggingArguments.m_bodyUniqueIdA > -1)
3902 {
3903 logger->m_bodyUniqueIdA = clientCmd.m_stateLoggingArguments.m_bodyUniqueIdA;
3904 }
3905 if ((clientCmd.m_updateFlags & STATE_LOGGING_FILTER_BODY_UNIQUE_ID_B) && clientCmd.m_stateLoggingArguments.m_bodyUniqueIdB > -1)
3906 {
3907 logger->m_bodyUniqueIdB = clientCmd.m_stateLoggingArguments.m_bodyUniqueIdB;
3908 }
3909 m_data->m_stateLoggers.push_back(logger);
3910 serverStatusOut.m_type = CMD_STATE_LOGGING_START_COMPLETED;
3911 serverStatusOut.m_stateLoggingResultArgs.m_loggingUniqueId = loggerUid;
3912 }
3913 #endif
3914 if (clientCmd.m_stateLoggingArguments.m_logType == STATE_LOGGING_VR_CONTROLLERS)
3915 {
3916 std::string fileName = clientCmd.m_stateLoggingArguments.m_fileName;
3917 int loggerUid = m_data->m_stateLoggersUniqueId++;
3918 int deviceFilterType = VR_DEVICE_CONTROLLER;
3919 if (clientCmd.m_updateFlags & STATE_LOGGING_FILTER_DEVICE_TYPE)
3920 {
3921 deviceFilterType = clientCmd.m_stateLoggingArguments.m_deviceFilterType;
3922 }
3923 VRControllerStateLogger* logger = new VRControllerStateLogger(loggerUid, deviceFilterType, fileName);
3924 m_data->m_stateLoggers.push_back(logger);
3925 serverStatusOut.m_type = CMD_STATE_LOGGING_START_COMPLETED;
3926 serverStatusOut.m_stateLoggingResultArgs.m_loggingUniqueId = loggerUid;
3927 }
3928 }
3929 if ((clientCmd.m_updateFlags & STATE_LOGGING_STOP_LOG) && clientCmd.m_stateLoggingArguments.m_loggingUniqueId >= 0)
3930 {
3931 if (clientCmd.m_stateLoggingArguments.m_loggingUniqueId == m_data->m_logPlaybackUid)
3932 {
3933 if (m_data->m_logPlayback)
3934 {
3935 delete m_data->m_logPlayback;
3936 m_data->m_logPlayback = 0;
3937 m_data->m_logPlaybackUid = -1;
3938 }
3939 }
3940
3941 if (clientCmd.m_stateLoggingArguments.m_loggingUniqueId == m_data->m_commandLoggingUid)
3942 {
3943 if (m_data->m_commandLogger)
3944 {
3945 enableCommandLogging(false, 0);
3946 serverStatusOut.m_type = CMD_STATE_LOGGING_COMPLETED;
3947 m_data->m_commandLoggingUid = -1;
3948 }
3949 }
3950
3951 if (clientCmd.m_stateLoggingArguments.m_loggingUniqueId == m_data->m_profileTimingLoggingUid)
3952 {
3953 serverStatusOut.m_type = CMD_STATE_LOGGING_COMPLETED;
3954 b3ChromeUtilsStopTimingsAndWriteJsonFile(m_data->m_profileTimingFileName.c_str());
3955 m_data->m_profileTimingLoggingUid = -1;
3956 }
3957 else
3958 {
3959 serverStatusOut.m_type = CMD_STATE_LOGGING_COMPLETED;
3960 for (int i = 0; i < m_data->m_stateLoggers.size(); i++)
3961 {
3962 if (m_data->m_stateLoggers[i]->m_loggingUniqueId == clientCmd.m_stateLoggingArguments.m_loggingUniqueId)
3963 {
3964 m_data->m_stateLoggers[i]->stop();
3965 delete m_data->m_stateLoggers[i];
3966 m_data->m_stateLoggers.removeAtIndex(i);
3967 }
3968 }
3969 }
3970 }
3971 return hasStatus;
3972 }
3973
processRequestCameraImageCommand(const struct SharedMemoryCommand & clientCmd,struct SharedMemoryStatus & serverStatusOut,char * bufferServerToClient,int bufferSizeInBytes)3974 bool PhysicsServerCommandProcessor::processRequestCameraImageCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
3975 {
3976 bool hasStatus = true;
3977 BT_PROFILE("CMD_REQUEST_CAMERA_IMAGE_DATA");
3978 int startPixelIndex = clientCmd.m_requestPixelDataArguments.m_startPixelIndex;
3979 int width = clientCmd.m_requestPixelDataArguments.m_pixelWidth;
3980 int height = clientCmd.m_requestPixelDataArguments.m_pixelHeight;
3981 int numPixelsCopied = 0;
3982
3983 if ((clientCmd.m_requestPixelDataArguments.m_startPixelIndex == 0) &&
3984 (clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_PIXEL_WIDTH_HEIGHT) != 0)
3985 {
3986 if (m_data->m_pluginManager.getRenderInterface())
3987 {
3988 m_data->m_pluginManager.getRenderInterface()->setWidthAndHeight(clientCmd.m_requestPixelDataArguments.m_pixelWidth,
3989 clientCmd.m_requestPixelDataArguments.m_pixelHeight);
3990 }
3991 }
3992 int flags = 0;
3993 if (clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_HAS_FLAGS)
3994 {
3995 flags = clientCmd.m_requestPixelDataArguments.m_flags;
3996 }
3997 if (m_data->m_pluginManager.getRenderInterface())
3998 {
3999 m_data->m_pluginManager.getRenderInterface()->setFlags(flags);
4000 }
4001
4002 int numTotalPixels = width * height;
4003 int numRemainingPixels = numTotalPixels - startPixelIndex;
4004
4005 if (numRemainingPixels > 0)
4006 {
4007 int totalBytesPerPixel = 4 + 4 + 4; //4 for rgb, 4 for depth, 4 for segmentation mask
4008 int maxNumPixels = bufferSizeInBytes / totalBytesPerPixel - 1;
4009 unsigned char* pixelRGBA = (unsigned char*)bufferServerToClient;
4010 int numRequestedPixels = btMin(maxNumPixels, numRemainingPixels);
4011
4012 float* depthBuffer = (float*)(bufferServerToClient + numRequestedPixels * 4);
4013 int* segmentationMaskBuffer = (int*)(bufferServerToClient + numRequestedPixels * 8);
4014
4015 serverStatusOut.m_numDataStreamBytes = numRequestedPixels * totalBytesPerPixel;
4016 float viewMat[16];
4017 float projMat[16];
4018 float projTextureViewMat[16];
4019 float projTextureProjMat[16];
4020 for (int i = 0; i < 16; i++)
4021 {
4022 viewMat[i] = clientCmd.m_requestPixelDataArguments.m_viewMatrix[i];
4023 projMat[i] = clientCmd.m_requestPixelDataArguments.m_projectionMatrix[i];
4024 }
4025 if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES) == 0)
4026 {
4027 b3OpenGLVisualizerCameraInfo tmpCamResult;
4028 bool result = this->m_data->m_guiHelper->getCameraInfo(
4029 &tmpCamResult.m_width,
4030 &tmpCamResult.m_height,
4031 tmpCamResult.m_viewMatrix,
4032 tmpCamResult.m_projectionMatrix,
4033 tmpCamResult.m_camUp,
4034 tmpCamResult.m_camForward,
4035 tmpCamResult.m_horizontal,
4036 tmpCamResult.m_vertical,
4037 &tmpCamResult.m_yaw,
4038 &tmpCamResult.m_pitch,
4039 &tmpCamResult.m_dist,
4040 tmpCamResult.m_target);
4041 if (result)
4042 {
4043 for (int i = 0; i < 16; i++)
4044 {
4045 viewMat[i] = tmpCamResult.m_viewMatrix[i];
4046 projMat[i] = tmpCamResult.m_projectionMatrix[i];
4047 }
4048 }
4049 }
4050 bool handled = false;
4051
4052 if ((clientCmd.m_updateFlags & ER_BULLET_HARDWARE_OPENGL) != 0)
4053 {
4054 if ((flags & ER_USE_PROJECTIVE_TEXTURE) != 0)
4055 {
4056 this->m_data->m_guiHelper->setProjectiveTexture(true);
4057 if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_HAS_PROJECTIVE_TEXTURE_MATRICES) != 0)
4058 {
4059 for (int i = 0; i < 16; i++)
4060 {
4061 projTextureViewMat[i] = clientCmd.m_requestPixelDataArguments.m_projectiveTextureViewMatrix[i];
4062 projTextureProjMat[i] = clientCmd.m_requestPixelDataArguments.m_projectiveTextureProjectionMatrix[i];
4063 }
4064 }
4065 else // If no specified matrices for projective texture, then use the camera matrices.
4066 {
4067 for (int i = 0; i < 16; i++)
4068 {
4069 projTextureViewMat[i] = viewMat[i];
4070 projTextureProjMat[i] = projMat[i];
4071 }
4072 }
4073 this->m_data->m_guiHelper->setProjectiveTextureMatrices(projTextureViewMat, projTextureProjMat);
4074 }
4075 else
4076 {
4077 this->m_data->m_guiHelper->setProjectiveTexture(false);
4078 }
4079
4080 if ((flags & ER_NO_SEGMENTATION_MASK) != 0)
4081 {
4082 segmentationMaskBuffer = 0;
4083 }
4084 m_data->m_guiHelper->copyCameraImageData(viewMat,
4085 projMat, pixelRGBA, numRequestedPixels,
4086 depthBuffer, numRequestedPixels,
4087 segmentationMaskBuffer, numRequestedPixels,
4088 startPixelIndex, width, height, &numPixelsCopied);
4089
4090 if (numPixelsCopied > 0)
4091 {
4092 //convert segmentation mask
4093
4094 if (segmentationMaskBuffer)
4095 {
4096 for (int i = 0; i < numPixelsCopied; i++)
4097 {
4098 int graphicsSegMask = segmentationMaskBuffer[i];
4099 int segMask = -1;
4100 if ((graphicsSegMask >= 0) && (graphicsSegMask < m_data->m_graphicsIndexToSegmentationMask.size()))
4101 {
4102 segMask = m_data->m_graphicsIndexToSegmentationMask[graphicsSegMask];
4103 }
4104 if ((flags & ER_SEGMENTATION_MASK_OBJECT_AND_LINKINDEX) == 0)
4105 {
4106 if (segMask >= 0)
4107 {
4108 segMask &= ((1 << 24) - 1);
4109 }
4110 }
4111 segmentationMaskBuffer[i] = segMask;
4112 }
4113 }
4114
4115 handled = true;
4116 m_data->m_guiHelper->debugDisplayCameraImageData(viewMat,
4117 projMat, pixelRGBA, numRequestedPixels,
4118 depthBuffer, numRequestedPixels,
4119 segmentationMaskBuffer, numRequestedPixels,
4120 startPixelIndex, width, height, &numPixelsCopied);
4121 }
4122 }
4123 if (!handled)
4124 {
4125 if (m_data->m_pluginManager.getRenderInterface())
4126 {
4127 if (clientCmd.m_requestPixelDataArguments.m_startPixelIndex == 0)
4128 {
4129 // printf("-------------------------------\nRendering\n");
4130
4131 if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_LIGHT_DIRECTION) != 0)
4132 {
4133 m_data->m_pluginManager.getRenderInterface()->setLightDirection(clientCmd.m_requestPixelDataArguments.m_lightDirection[0], clientCmd.m_requestPixelDataArguments.m_lightDirection[1], clientCmd.m_requestPixelDataArguments.m_lightDirection[2]);
4134 }
4135
4136 if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_LIGHT_COLOR) != 0)
4137 {
4138 m_data->m_pluginManager.getRenderInterface()->setLightColor(clientCmd.m_requestPixelDataArguments.m_lightColor[0], clientCmd.m_requestPixelDataArguments.m_lightColor[1], clientCmd.m_requestPixelDataArguments.m_lightColor[2]);
4139 }
4140
4141 if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_LIGHT_DISTANCE) != 0)
4142 {
4143 m_data->m_pluginManager.getRenderInterface()->setLightDistance(clientCmd.m_requestPixelDataArguments.m_lightDistance);
4144 }
4145
4146 if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_SHADOW) != 0)
4147 {
4148 m_data->m_pluginManager.getRenderInterface()->setShadow((clientCmd.m_requestPixelDataArguments.m_hasShadow != 0));
4149 }
4150
4151 if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_AMBIENT_COEFF) != 0)
4152 {
4153 m_data->m_pluginManager.getRenderInterface()->setLightAmbientCoeff(clientCmd.m_requestPixelDataArguments.m_lightAmbientCoeff);
4154 }
4155
4156 if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_DIFFUSE_COEFF) != 0)
4157 {
4158 m_data->m_pluginManager.getRenderInterface()->setLightDiffuseCoeff(clientCmd.m_requestPixelDataArguments.m_lightDiffuseCoeff);
4159 }
4160
4161 if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_SPECULAR_COEFF) != 0)
4162 {
4163 m_data->m_pluginManager.getRenderInterface()->setLightSpecularCoeff(clientCmd.m_requestPixelDataArguments.m_lightSpecularCoeff);
4164 }
4165
4166 for (int i = 0; i < m_data->m_dynamicsWorld->getNumCollisionObjects(); i++)
4167 {
4168 const btCollisionObject* colObj = m_data->m_dynamicsWorld->getCollisionObjectArray()[i];
4169 btVector3 localScaling(1, 1, 1);
4170 m_data->m_pluginManager.getRenderInterface()->syncTransform(colObj->getUserIndex3(), colObj->getWorldTransform(), localScaling);
4171
4172 const btCollisionShape* collisionShape = colObj->getCollisionShape();
4173 if (collisionShape->getShapeType() == SOFTBODY_SHAPE_PROXYTYPE)
4174 {
4175 #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
4176 const btSoftBody* psb = (const btSoftBody*)colObj;
4177 if (psb->getUserIndex3() >= 0)
4178 {
4179
4180 btAlignedObjectArray<btVector3> vertices;
4181 btAlignedObjectArray<btVector3> normals;
4182 if (psb->m_renderNodes.size() == 0)
4183 {
4184
4185 vertices.resize(psb->m_faces.size() * 3);
4186 normals.resize(psb->m_faces.size() * 3);
4187
4188 for (int i = 0; i < psb->m_faces.size(); i++) // Foreach face
4189 {
4190 for (int k = 0; k < 3; k++) // Foreach vertex on a face
4191 {
4192 int currentIndex = i * 3 + k;
4193 vertices[currentIndex] = psb->m_faces[i].m_n[k]->m_x;
4194 normals[currentIndex] = psb->m_faces[i].m_n[k]->m_n;
4195 }
4196 }
4197 }
4198 else
4199 {
4200 vertices.resize(psb->m_renderNodes.size());
4201
4202 for (int i = 0; i < psb->m_renderNodes.size(); i++) // Foreach face
4203 {
4204 vertices[i] = psb->m_renderNodes[i].m_x;
4205 }
4206 }
4207 m_data->m_pluginManager.getRenderInterface()->updateShape(psb->getUserIndex3(), &vertices[0], vertices.size(), &normals[0],normals.size());
4208 }
4209 #endif //SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
4210 }
4211 }
4212
4213 if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES) != 0)
4214 {
4215 m_data->m_pluginManager.getRenderInterface()->render(
4216 clientCmd.m_requestPixelDataArguments.m_viewMatrix,
4217 clientCmd.m_requestPixelDataArguments.m_projectionMatrix);
4218 }
4219 else
4220 {
4221 b3OpenGLVisualizerCameraInfo tmpCamResult;
4222 bool result = this->m_data->m_guiHelper->getCameraInfo(
4223 &tmpCamResult.m_width,
4224 &tmpCamResult.m_height,
4225 tmpCamResult.m_viewMatrix,
4226 tmpCamResult.m_projectionMatrix,
4227 tmpCamResult.m_camUp,
4228 tmpCamResult.m_camForward,
4229 tmpCamResult.m_horizontal,
4230 tmpCamResult.m_vertical,
4231 &tmpCamResult.m_yaw,
4232 &tmpCamResult.m_pitch,
4233 &tmpCamResult.m_dist,
4234 tmpCamResult.m_target);
4235 if (result)
4236 {
4237 m_data->m_pluginManager.getRenderInterface()->render(tmpCamResult.m_viewMatrix, tmpCamResult.m_projectionMatrix);
4238 }
4239 else
4240 {
4241 m_data->m_pluginManager.getRenderInterface()->render();
4242 }
4243 }
4244 }
4245 }
4246
4247 if (m_data->m_pluginManager.getRenderInterface())
4248 {
4249 if ((flags & ER_USE_PROJECTIVE_TEXTURE) != 0)
4250 {
4251 m_data->m_pluginManager.getRenderInterface()->setProjectiveTexture(true);
4252 if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_HAS_PROJECTIVE_TEXTURE_MATRICES) != 0)
4253 {
4254 for (int i = 0; i < 16; i++)
4255 {
4256 projTextureViewMat[i] = clientCmd.m_requestPixelDataArguments.m_projectiveTextureViewMatrix[i];
4257 projTextureProjMat[i] = clientCmd.m_requestPixelDataArguments.m_projectiveTextureProjectionMatrix[i];
4258 }
4259 }
4260 else // If no specified matrices for projective texture, then use the camera matrices.
4261 {
4262 for (int i = 0; i < 16; i++)
4263 {
4264 projTextureViewMat[i] = viewMat[i];
4265 projTextureProjMat[i] = projMat[i];
4266 }
4267 }
4268 m_data->m_pluginManager.getRenderInterface()->setProjectiveTextureMatrices(projTextureViewMat, projTextureProjMat);
4269 }
4270 else
4271 {
4272 m_data->m_pluginManager.getRenderInterface()->setProjectiveTexture(false);
4273 }
4274
4275 if ((flags & ER_NO_SEGMENTATION_MASK) != 0)
4276 {
4277 segmentationMaskBuffer = 0;
4278 }
4279
4280 m_data->m_pluginManager.getRenderInterface()->copyCameraImageData(pixelRGBA, numRequestedPixels,
4281 depthBuffer, numRequestedPixels,
4282 segmentationMaskBuffer, numRequestedPixels,
4283 startPixelIndex, &width, &height, &numPixelsCopied);
4284 m_data->m_pluginManager.getRenderInterface()->setProjectiveTexture(false);
4285 }
4286
4287 m_data->m_guiHelper->debugDisplayCameraImageData(clientCmd.m_requestPixelDataArguments.m_viewMatrix,
4288 clientCmd.m_requestPixelDataArguments.m_projectionMatrix, pixelRGBA, numRequestedPixels,
4289 depthBuffer, numRequestedPixels,
4290 segmentationMaskBuffer, numRequestedPixels,
4291 startPixelIndex, width, height, &numPixelsCopied);
4292 }
4293
4294 //each pixel takes 4 RGBA values and 1 float = 8 bytes
4295 }
4296 else
4297 {
4298 }
4299
4300 serverStatusOut.m_type = CMD_CAMERA_IMAGE_COMPLETED;
4301
4302 serverStatusOut.m_sendPixelDataArguments.m_numPixelsCopied = numPixelsCopied;
4303 serverStatusOut.m_sendPixelDataArguments.m_numRemainingPixels = numRemainingPixels - numPixelsCopied;
4304 serverStatusOut.m_sendPixelDataArguments.m_startingPixelIndex = startPixelIndex;
4305 serverStatusOut.m_sendPixelDataArguments.m_imageWidth = width;
4306 serverStatusOut.m_sendPixelDataArguments.m_imageHeight = height;
4307 return hasStatus;
4308 }
4309
processSaveWorldCommand(const struct SharedMemoryCommand & clientCmd,struct SharedMemoryStatus & serverStatusOut,char * bufferServerToClient,int bufferSizeInBytes)4310 bool PhysicsServerCommandProcessor::processSaveWorldCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
4311 {
4312 bool hasStatus = false;
4313 BT_PROFILE("CMD_SAVE_WORLD");
4314 serverStatusOut.m_type = CMD_SAVE_WORLD_FAILED;
4315
4316 ///this is a very rudimentary way to save the state of the world, for scene authoring
4317 ///many todo's, for example save the state of motor controllers etc.
4318
4319 {
4320 //saveWorld(clientCmd.m_sdfArguments.m_sdfFileName);
4321 int constraintCount = 0;
4322 FILE* f = fopen(clientCmd.m_sdfArguments.m_sdfFileName, "w");
4323 if (f)
4324 {
4325 char line[2048];
4326 {
4327 sprintf(line, "import pybullet as p\n");
4328 int len = strlen(line);
4329 fwrite(line, len, 1, f);
4330 }
4331 {
4332 sprintf(line, "cin = p.connect(p.SHARED_MEMORY)\n");
4333 int len = strlen(line);
4334 fwrite(line, len, 1, f);
4335 }
4336 {
4337 sprintf(line, "if (cin < 0):\n");
4338 int len = strlen(line);
4339 fwrite(line, len, 1, f);
4340 }
4341 {
4342 sprintf(line, " cin = p.connect(p.GUI)\n");
4343 int len = strlen(line);
4344 fwrite(line, len, 1, f);
4345 }
4346
4347 //for each objects ...
4348 for (int i = 0; i < m_data->m_saveWorldBodyData.size(); i++)
4349 {
4350 SaveWorldObjectData& sd = m_data->m_saveWorldBodyData[i];
4351
4352 for (int i = 0; i < sd.m_bodyUniqueIds.size(); i++)
4353 {
4354 {
4355 int bodyUniqueId = sd.m_bodyUniqueIds[i];
4356 InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId);
4357 if (body)
4358 {
4359 if (body->m_multiBody)
4360 {
4361 btMultiBody* mb = body->m_multiBody;
4362
4363 btTransform comTr = mb->getBaseWorldTransform();
4364 btTransform tr = comTr * body->m_rootLocalInertialFrame.inverse();
4365
4366 if (strstr(sd.m_fileName.c_str(), ".urdf"))
4367 {
4368 sprintf(line, "objects = [p.loadURDF(\"%s\", %f,%f,%f,%f,%f,%f,%f)]\n", sd.m_fileName.c_str(),
4369 tr.getOrigin()[0], tr.getOrigin()[1], tr.getOrigin()[2],
4370 tr.getRotation()[0], tr.getRotation()[1], tr.getRotation()[2], tr.getRotation()[3]);
4371 int len = strlen(line);
4372 fwrite(line, len, 1, f);
4373 }
4374
4375 if (strstr(sd.m_fileName.c_str(), ".sdf") && i == 0)
4376 {
4377 sprintf(line, "objects = p.loadSDF(\"%s\")\n", sd.m_fileName.c_str());
4378 int len = strlen(line);
4379 fwrite(line, len, 1, f);
4380 }
4381 if (strstr(sd.m_fileName.c_str(), ".xml") && i == 0)
4382 {
4383 sprintf(line, "objects = p.loadMJCF(\"%s\")\n", sd.m_fileName.c_str());
4384 int len = strlen(line);
4385 fwrite(line, len, 1, f);
4386 }
4387
4388 if (strstr(sd.m_fileName.c_str(), ".sdf") || strstr(sd.m_fileName.c_str(), ".xml") || ((strstr(sd.m_fileName.c_str(), ".urdf")) && mb->getNumLinks()))
4389 {
4390 sprintf(line, "ob = objects[%d]\n", i);
4391 int len = strlen(line);
4392 fwrite(line, len, 1, f);
4393 }
4394
4395 if (strstr(sd.m_fileName.c_str(), ".sdf") || strstr(sd.m_fileName.c_str(), ".xml"))
4396 {
4397 sprintf(line, "p.resetBasePositionAndOrientation(ob,[%f,%f,%f],[%f,%f,%f,%f])\n",
4398 comTr.getOrigin()[0], comTr.getOrigin()[1], comTr.getOrigin()[2],
4399 comTr.getRotation()[0], comTr.getRotation()[1], comTr.getRotation()[2], comTr.getRotation()[3]);
4400 int len = strlen(line);
4401 fwrite(line, len, 1, f);
4402 }
4403
4404 if (mb->getNumLinks())
4405 {
4406 {
4407 sprintf(line, "jointPositions=[");
4408 int len = strlen(line);
4409 fwrite(line, len, 1, f);
4410 }
4411
4412 for (int i = 0; i < mb->getNumLinks(); i++)
4413 {
4414 btScalar jointPos = mb->getJointPosMultiDof(i)[0];
4415 if (i < mb->getNumLinks() - 1)
4416 {
4417 sprintf(line, " %f,", jointPos);
4418 int len = strlen(line);
4419 fwrite(line, len, 1, f);
4420 }
4421 else
4422 {
4423 sprintf(line, " %f ", jointPos);
4424 int len = strlen(line);
4425 fwrite(line, len, 1, f);
4426 }
4427 }
4428
4429 {
4430 sprintf(line, "]\nfor jointIndex in range (p.getNumJoints(ob)):\n\tp.resetJointState(ob,jointIndex,jointPositions[jointIndex])\n\n");
4431 int len = strlen(line);
4432 fwrite(line, len, 1, f);
4433 }
4434 }
4435 }
4436 else
4437 {
4438 //todo: btRigidBody/btSoftBody etc case
4439 }
4440 }
4441 }
4442 }
4443
4444 //for URDF, load at origin, then reposition...
4445
4446 struct SaveWorldObjectData
4447 {
4448 b3AlignedObjectArray<int> m_bodyUniqueIds;
4449 std::string m_fileName;
4450 };
4451 }
4452
4453 //user constraints
4454 {
4455 for (int i = 0; i < m_data->m_userConstraints.size(); i++)
4456 {
4457 InteralUserConstraintData* ucptr = m_data->m_userConstraints.getAtIndex(i);
4458 b3UserConstraint& uc = ucptr->m_userConstraintData;
4459
4460 int parentBodyIndex = uc.m_parentBodyIndex;
4461 int parentJointIndex = uc.m_parentJointIndex;
4462 int childBodyIndex = uc.m_childBodyIndex;
4463 int childJointIndex = uc.m_childJointIndex;
4464 btVector3 jointAxis(uc.m_jointAxis[0], uc.m_jointAxis[1], uc.m_jointAxis[2]);
4465 btVector3 pivotParent(uc.m_parentFrame[0], uc.m_parentFrame[1], uc.m_parentFrame[2]);
4466 btVector3 pivotChild(uc.m_childFrame[0], uc.m_childFrame[1], uc.m_childFrame[2]);
4467 btQuaternion ornFrameParent(uc.m_parentFrame[3], uc.m_parentFrame[4], uc.m_parentFrame[5], uc.m_parentFrame[6]);
4468 btQuaternion ornFrameChild(uc.m_childFrame[3], uc.m_childFrame[4], uc.m_childFrame[5], uc.m_childFrame[6]);
4469 {
4470 char jointTypeStr[1024] = "FIXED";
4471 bool hasKnownJointType = true;
4472
4473 switch (uc.m_jointType)
4474 {
4475 case eRevoluteType:
4476 {
4477 sprintf(jointTypeStr, "p.JOINT_REVOLUTE");
4478 break;
4479 }
4480 case ePrismaticType:
4481 {
4482 sprintf(jointTypeStr, "p.JOINT_PRISMATIC");
4483 break;
4484 }
4485 case eSphericalType:
4486 {
4487 sprintf(jointTypeStr, "p.JOINT_SPHERICAL");
4488 break;
4489 }
4490 case ePlanarType:
4491 {
4492 sprintf(jointTypeStr, "p.JOINT_PLANAR");
4493 break;
4494 }
4495 case eFixedType:
4496 {
4497 sprintf(jointTypeStr, "p.JOINT_FIXED");
4498 break;
4499 }
4500 case ePoint2PointType:
4501 {
4502 sprintf(jointTypeStr, "p.JOINT_POINT2POINT");
4503 break;
4504 }
4505 case eGearType:
4506 {
4507 sprintf(jointTypeStr, "p.JOINT_GEAR");
4508 break;
4509 }
4510 default:
4511 {
4512 hasKnownJointType = false;
4513 b3Warning("unknown constraint type in SAVE_WORLD");
4514 }
4515 };
4516 if (hasKnownJointType)
4517 {
4518 {
4519 sprintf(line, "cid%d = p.createConstraint(%d,%d,%d,%d,%s,[%f,%f,%f],[%f,%f,%f],[%f,%f,%f],[%f,%f,%f,%f],[%f,%f,%f,%f])\n",
4520 constraintCount,
4521 parentBodyIndex,
4522 parentJointIndex,
4523 childBodyIndex,
4524 childJointIndex,
4525 jointTypeStr,
4526 jointAxis[0], jointAxis[1], jointAxis[2],
4527 pivotParent[0], pivotParent[1], pivotParent[2],
4528 pivotChild[0], pivotChild[1], pivotChild[2],
4529 ornFrameParent[0], ornFrameParent[1], ornFrameParent[2], ornFrameParent[3],
4530 ornFrameChild[0], ornFrameChild[1], ornFrameChild[2], ornFrameChild[3]);
4531 int len = strlen(line);
4532 fwrite(line, len, 1, f);
4533 }
4534 {
4535 sprintf(line, "p.changeConstraint(cid%d,maxForce=%f)\n", constraintCount, uc.m_maxAppliedForce);
4536 int len = strlen(line);
4537 fwrite(line, len, 1, f);
4538 constraintCount++;
4539 }
4540 }
4541 }
4542 }
4543 }
4544
4545 {
4546 btVector3 grav = this->m_data->m_dynamicsWorld->getGravity();
4547 sprintf(line, "p.setGravity(%f,%f,%f)\n", grav[0], grav[1], grav[2]);
4548 int len = strlen(line);
4549 fwrite(line, len, 1, f);
4550 }
4551
4552 {
4553 sprintf(line, "p.stepSimulation()\np.disconnect()\n");
4554 int len = strlen(line);
4555 fwrite(line, len, 1, f);
4556 }
4557 fclose(f);
4558 }
4559
4560 serverStatusOut.m_type = CMD_SAVE_WORLD_COMPLETED;
4561 hasStatus = true;
4562 }
4563
4564 return hasStatus;
4565 }
4566
4567 #define MYLINELENGTH 16 * 32768
4568
MyGetRawHeightfieldData(CommonFileIOInterface & fileIO,PHY_ScalarType type,const char * fileName,int & width,int & height,btScalar & minHeight,btScalar & maxHeight)4569 static unsigned char* MyGetRawHeightfieldData(CommonFileIOInterface& fileIO, PHY_ScalarType type, const char* fileName, int& width, int& height,
4570 btScalar& minHeight,
4571 btScalar& maxHeight)
4572 {
4573 std::string ext;
4574 std::string fn(fileName);
4575 std::string ext_ = fn.substr(fn.size() - 4);
4576 for (std::string::iterator i = ext_.begin(); i != ext_.end(); ++i)
4577 {
4578 ext += char(tolower(*i));
4579 }
4580
4581 if (ext != ".txt")
4582 {
4583 char relativeFileName[1024];
4584 int found = fileIO.findResourcePath(fileName, relativeFileName, 1024);
4585
4586 b3AlignedObjectArray<char> buffer;
4587 buffer.reserve(1024);
4588 int fileId = fileIO.fileOpen(relativeFileName, "rb");
4589 if (fileId >= 0)
4590 {
4591 int size = fileIO.getFileSize(fileId);
4592 if (size > 0)
4593 {
4594 buffer.resize(size);
4595 int actual = fileIO.fileRead(fileId, &buffer[0], size);
4596 if (actual != size)
4597 {
4598 b3Warning("STL filesize mismatch!\n");
4599 buffer.resize(0);
4600 }
4601 }
4602 fileIO.fileClose(fileId);
4603 }
4604
4605 if (buffer.size())
4606 {
4607 int n;
4608
4609 unsigned char* image = stbi_load_from_memory((const unsigned char*)&buffer[0], buffer.size(), &width, &height, &n, 3);
4610 if (image)
4611 {
4612 fileIO.fileClose(fileId);
4613 int nElements = width * height;
4614 int bytesPerElement = sizeof(btScalar);
4615 btAssert(bytesPerElement > 0 && "bad bytes per element");
4616
4617 int nBytes = nElements * bytesPerElement;
4618 unsigned char* raw = new unsigned char[nBytes];
4619 btAssert(raw && "out of memory");
4620
4621 unsigned char* p = raw;
4622 for (int j = 0; j < height; ++j)
4623
4624 {
4625 for (int i = 0; i < width; ++i)
4626 {
4627 float z = double(image[(width - 1 - i) * 3 + width * j * 3]) * (1. / 255.);
4628 btScalar* pf = (btScalar*)p;
4629 *pf = z;
4630 p += bytesPerElement;
4631 // update min/max
4632 if (!i && !j)
4633 {
4634 minHeight = z;
4635 maxHeight = z;
4636 }
4637 else
4638 {
4639 if (z < minHeight)
4640 {
4641 minHeight = z;
4642 }
4643 if (z > maxHeight)
4644 {
4645 maxHeight = z;
4646 }
4647 }
4648 }
4649 }
4650 free(image);
4651
4652 return raw;
4653 }
4654 }
4655 }
4656
4657 if (ext == ".txt")
4658 {
4659 //read a csv file as used in DeepLoco
4660 {
4661 char relativePath[1024];
4662 int found = fileIO.findResourcePath(fileName, relativePath, 1024);
4663 btAlignedObjectArray<char> lineBuffer;
4664 lineBuffer.resize(MYLINELENGTH);
4665 int slot = fileIO.fileOpen(relativePath, "r");
4666 int rows = 0;
4667 int cols = 0;
4668
4669 btAlignedObjectArray<double> allValues;
4670 if (slot >= 0)
4671 {
4672 char* lineChar;
4673 while (lineChar = fileIO.readLine(slot, &lineBuffer[0], MYLINELENGTH))
4674 {
4675 rows = 0;
4676 std::string line(lineChar);
4677 int pos = 0;
4678 while (pos < line.length())
4679 {
4680 int nextPos = pos + 1;
4681 while (nextPos < line.length())
4682 {
4683 if (line[nextPos - 1] == ',')
4684 {
4685 break;
4686 }
4687 nextPos++;
4688 }
4689 std::string substr = line.substr(pos, nextPos - pos - 1);
4690
4691 double v;
4692 if (sscanf(substr.c_str(), "%lf", &v) == 1)
4693 {
4694 allValues.push_back(v);
4695 rows++;
4696 }
4697 pos = nextPos;
4698 }
4699 cols++;
4700 }
4701 width = rows;
4702 height = cols;
4703
4704 fileIO.fileClose(slot);
4705 int nElements = width * height;
4706 // std::cerr << " nElements = " << nElements << "\n";
4707
4708 int bytesPerElement = sizeof(btScalar);
4709 // std::cerr << " bytesPerElement = " << bytesPerElement << "\n";
4710 btAssert(bytesPerElement > 0 && "bad bytes per element");
4711
4712 long nBytes = nElements * bytesPerElement;
4713 // std::cerr << " nBytes = " << nBytes << "\n";
4714 unsigned char* raw = new unsigned char[nBytes];
4715 btAssert(raw && "out of memory");
4716
4717 unsigned char* p = raw;
4718 for (int i = 0; i < width; ++i)
4719 {
4720 for (int j = 0; j < height; ++j)
4721 {
4722 double z = allValues[i + width * j];
4723 //convertFromFloat(p, z, type);
4724 btScalar* pf = (btScalar*)p;
4725 *pf = z;
4726 p += bytesPerElement;
4727 // update min/max
4728 if (!i && !j)
4729 {
4730 minHeight = z;
4731 maxHeight = z;
4732 }
4733 else
4734 {
4735 if (z < minHeight)
4736 {
4737 minHeight = z;
4738 }
4739 if (z > maxHeight)
4740 {
4741 maxHeight = z;
4742 }
4743 }
4744 }
4745 }
4746 return raw;
4747 }
4748 }
4749 }
4750
4751 return 0;
4752 }
4753
4754 class MyTriangleCollector4 : public btTriangleCallback
4755 {
4756 public:
4757 btAlignedObjectArray<GLInstanceVertex>* m_pVerticesOut;
4758 btAlignedObjectArray<int>* m_pIndicesOut;
4759 btVector3 m_aabbMin, m_aabbMax;
4760 btScalar m_textureScaling;
4761
MyTriangleCollector4(const btVector3 & aabbMin,const btVector3 & aabbMax)4762 MyTriangleCollector4(const btVector3& aabbMin, const btVector3& aabbMax)
4763 :m_aabbMin(aabbMin), m_aabbMax(aabbMax), m_textureScaling(1)
4764 {
4765 m_pVerticesOut = 0;
4766 m_pIndicesOut = 0;
4767 }
4768
processTriangle(btVector3 * tris,int partId,int triangleIndex)4769 virtual void processTriangle(btVector3* tris, int partId, int triangleIndex)
4770 {
4771 for (int k = 0; k < 3; k++)
4772 {
4773 GLInstanceVertex v;
4774 v.xyzw[3] = 0;
4775
4776 btVector3 normal = (tris[0] - tris[1]).cross(tris[0] - tris[2]);
4777 normal.safeNormalize();
4778 for (int l = 0; l < 3; l++)
4779 {
4780 v.xyzw[l] = tris[k][l];
4781 v.normal[l] = normal[l];
4782 }
4783
4784 btVector3 extents = m_aabbMax - m_aabbMin;
4785
4786 v.uv[0] = (1. - ((v.xyzw[0] - m_aabbMin[0]) / (m_aabbMax[0] - m_aabbMin[0]))) * m_textureScaling;
4787 v.uv[1] = (1. - (v.xyzw[1] - m_aabbMin[1]) / (m_aabbMax[1] - m_aabbMin[1])) * m_textureScaling;
4788
4789 m_pIndicesOut->push_back(m_pVerticesOut->size());
4790 m_pVerticesOut->push_back(v);
4791 }
4792 }
4793 };
processCreateCollisionShapeCommand(const struct SharedMemoryCommand & clientCmd,struct SharedMemoryStatus & serverStatusOut,char * bufferServerToClient,int bufferSizeInBytes)4794 bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
4795 {
4796 bool hasStatus = true;
4797 serverStatusOut.m_type = CMD_CREATE_COLLISION_SHAPE_FAILED;
4798
4799 #ifdef USE_DISCRETE_DYNAMICS_WORLD
4800 btWorldImporter* worldImporter = new btWorldImporter(m_data->m_dynamicsWorld);
4801 #else
4802 btMultiBodyWorldImporter* worldImporter = new btMultiBodyWorldImporter(m_data->m_dynamicsWorld);
4803 #endif
4804
4805 btCollisionShape* shape = 0;
4806 b3AlignedObjectArray<UrdfCollision> urdfCollisionObjects;
4807
4808 btCompoundShape* compound = 0;
4809
4810 if (clientCmd.m_createUserShapeArgs.m_numUserShapes > 1)
4811 {
4812 compound = worldImporter->createCompoundShape();
4813 compound->setMargin(m_data->m_defaultCollisionMargin);
4814 }
4815 for (int i = 0; i < clientCmd.m_createUserShapeArgs.m_numUserShapes; i++)
4816 {
4817 GLInstanceGraphicsShape* glmesh = 0;
4818 char pathPrefix[1024] = "";
4819 char relativeFileName[1024] = "";
4820 UrdfCollision urdfColObj;
4821
4822 btTransform childTransform;
4823 childTransform.setIdentity();
4824 if (clientCmd.m_createUserShapeArgs.m_shapes[i].m_hasChildTransform)
4825 {
4826 childTransform.setOrigin(btVector3(clientCmd.m_createUserShapeArgs.m_shapes[i].m_childPosition[0],
4827 clientCmd.m_createUserShapeArgs.m_shapes[i].m_childPosition[1],
4828 clientCmd.m_createUserShapeArgs.m_shapes[i].m_childPosition[2]));
4829 childTransform.setRotation(btQuaternion(
4830 clientCmd.m_createUserShapeArgs.m_shapes[i].m_childOrientation[0],
4831 clientCmd.m_createUserShapeArgs.m_shapes[i].m_childOrientation[1],
4832 clientCmd.m_createUserShapeArgs.m_shapes[i].m_childOrientation[2],
4833 clientCmd.m_createUserShapeArgs.m_shapes[i].m_childOrientation[3]));
4834 if (compound == 0)
4835 {
4836 compound = worldImporter->createCompoundShape();
4837 }
4838 }
4839
4840 urdfColObj.m_linkLocalFrame = childTransform;
4841 urdfColObj.m_sourceFileLocation = "memory";
4842 urdfColObj.m_name = "memory";
4843 urdfColObj.m_geometry.m_type = URDF_GEOM_UNKNOWN;
4844
4845 switch (clientCmd.m_createUserShapeArgs.m_shapes[i].m_type)
4846 {
4847 case GEOM_SPHERE:
4848 {
4849 double radius = clientCmd.m_createUserShapeArgs.m_shapes[i].m_sphereRadius;
4850 shape = worldImporter->createSphereShape(radius);
4851 shape->setMargin(m_data->m_defaultCollisionMargin);
4852 if (compound)
4853 {
4854 compound->addChildShape(childTransform, shape);
4855 }
4856 urdfColObj.m_geometry.m_type = URDF_GEOM_SPHERE;
4857 urdfColObj.m_geometry.m_sphereRadius = radius;
4858 break;
4859 }
4860 case GEOM_BOX:
4861 {
4862 //double halfExtents[3] = clientCmd.m_createUserShapeArgs.m_shapes[i].m_sphereRadius;
4863 btVector3 halfExtents(
4864 clientCmd.m_createUserShapeArgs.m_shapes[i].m_boxHalfExtents[0],
4865 clientCmd.m_createUserShapeArgs.m_shapes[i].m_boxHalfExtents[1],
4866 clientCmd.m_createUserShapeArgs.m_shapes[i].m_boxHalfExtents[2]);
4867 shape = worldImporter->createBoxShape(halfExtents);
4868 shape->setMargin(m_data->m_defaultCollisionMargin);
4869 if (compound)
4870 {
4871 compound->addChildShape(childTransform, shape);
4872 }
4873 urdfColObj.m_geometry.m_type = URDF_GEOM_BOX;
4874 urdfColObj.m_geometry.m_boxSize = 2. * halfExtents;
4875 break;
4876 }
4877 case GEOM_CAPSULE:
4878 {
4879 shape = worldImporter->createCapsuleShapeZ(clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleRadius,
4880 clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleHeight);
4881 shape->setMargin(m_data->m_defaultCollisionMargin);
4882 if (compound)
4883 {
4884 compound->addChildShape(childTransform, shape);
4885 }
4886 urdfColObj.m_geometry.m_type = URDF_GEOM_CAPSULE;
4887 urdfColObj.m_geometry.m_capsuleRadius = clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleRadius;
4888 urdfColObj.m_geometry.m_capsuleHeight = clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleHeight;
4889
4890 break;
4891 }
4892 case GEOM_CYLINDER:
4893 {
4894 shape = worldImporter->createCylinderShapeZ(clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleRadius,
4895 0.5 * clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleHeight);
4896 shape->setMargin(m_data->m_defaultCollisionMargin);
4897 if (compound)
4898 {
4899 compound->addChildShape(childTransform, shape);
4900 }
4901 urdfColObj.m_geometry.m_type = URDF_GEOM_CYLINDER;
4902 urdfColObj.m_geometry.m_capsuleRadius = clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleRadius;
4903 urdfColObj.m_geometry.m_capsuleHeight = clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleHeight;
4904
4905 break;
4906 }
4907 case GEOM_HEIGHTFIELD:
4908 {
4909 int width;
4910 int height;
4911 btScalar minHeight, maxHeight;
4912 PHY_ScalarType scalarType = PHY_FLOAT;
4913 CommonFileIOInterface* fileIO = m_data->m_pluginManager.getFileIOInterface();
4914 const unsigned char* heightfieldData = 0;
4915 if (clientCmd.m_createUserShapeArgs.m_shapes[i].m_numHeightfieldColumns > 0 &&
4916 clientCmd.m_createUserShapeArgs.m_shapes[i].m_numHeightfieldRows > 0)
4917 {
4918 width = clientCmd.m_createUserShapeArgs.m_shapes[i].m_numHeightfieldRows;
4919 height = clientCmd.m_createUserShapeArgs.m_shapes[i].m_numHeightfieldColumns;
4920 float* heightfieldDataSrc = (float*)bufferServerToClient;
4921 heightfieldData = new unsigned char[width * height * sizeof(btScalar)];
4922 btScalar* datafl = (btScalar*)heightfieldData;
4923 minHeight = heightfieldDataSrc[0];
4924 maxHeight = heightfieldDataSrc[0];
4925 for (int i = 0; i < width * height; i++)
4926 {
4927 datafl[i] = heightfieldDataSrc[i];
4928 minHeight = btMin(minHeight, (btScalar)datafl[i]);
4929 maxHeight = btMax(maxHeight, (btScalar)datafl[i]);
4930 }
4931 }
4932 else
4933 {
4934 heightfieldData = MyGetRawHeightfieldData(*fileIO, scalarType, clientCmd.m_createUserShapeArgs.m_shapes[i].m_meshFileName, width, height, minHeight, maxHeight);
4935 }
4936 if (heightfieldData)
4937 {
4938 //replace heightfield data or create new heightfield
4939 if (clientCmd.m_createUserShapeArgs.m_shapes[i].m_replaceHeightfieldIndex >= 0)
4940 {
4941 int collisionShapeUid = clientCmd.m_createUserShapeArgs.m_shapes[i].m_replaceHeightfieldIndex;
4942
4943 InternalCollisionShapeHandle* handle = m_data->m_userCollisionShapeHandles.getHandle(collisionShapeUid);
4944 if (handle && handle->m_collisionShape && handle->m_collisionShape->getShapeType() == TERRAIN_SHAPE_PROXYTYPE)
4945 {
4946 btHeightfieldTerrainShape* terrainShape = (btHeightfieldTerrainShape*)handle->m_collisionShape;
4947 btScalar* heightfieldDest = (btScalar*)terrainShape->getHeightfieldRawData();
4948 //replace the data
4949
4950 btScalar* datafl = (btScalar*)heightfieldData;
4951
4952 for (int i = 0; i < width * height; i++)
4953 {
4954 heightfieldDest[i] = datafl[i];
4955 }
4956 //update graphics
4957
4958 btAlignedObjectArray<GLInstanceVertex> gfxVertices;
4959 btAlignedObjectArray<int> indices;
4960 int strideInBytes = 9 * sizeof(float);
4961
4962 btVector3 aabbMin, aabbMax;
4963 btTransform tr;
4964 tr.setIdentity();
4965 terrainShape->getAabb(tr, aabbMin, aabbMax);
4966 MyTriangleCollector4 col(aabbMin, aabbMax);
4967 col.m_pVerticesOut = &gfxVertices;
4968 col.m_pIndicesOut = &indices;
4969
4970 terrainShape->processAllTriangles(&col, aabbMin, aabbMax);
4971 if (gfxVertices.size() && indices.size())
4972 {
4973 m_data->m_guiHelper->updateShape(terrainShape->getUserIndex(), &gfxVertices[0].xyzw[0], gfxVertices.size());
4974 }
4975
4976 btAlignedObjectArray<btVector3> vts;
4977 btAlignedObjectArray<btVector3> normals;
4978 vts.resize(gfxVertices.size());
4979 normals.resize(gfxVertices.size());
4980
4981 for (int v = 0; v < gfxVertices.size(); v++)
4982 {
4983 vts[v].setValue(gfxVertices[v].xyzw[0], gfxVertices[v].xyzw[1], gfxVertices[v].xyzw[2]);
4984 normals[v].setValue(gfxVertices[v].normal[0], gfxVertices[v].normal[1], gfxVertices[v].normal[2]);
4985 }
4986
4987 m_data->m_pluginManager.getRenderInterface()->updateShape(terrainShape->getUserIndex2(), &vts[0], vts.size(), &normals[0], normals.size());
4988
4989
4990 terrainShape->clearAccelerator();
4991 terrainShape->buildAccelerator();
4992
4993 btTriangleInfoMap* oldTriangleInfoMap = terrainShape->getTriangleInfoMap();
4994 delete (oldTriangleInfoMap);
4995 terrainShape->setTriangleInfoMap(0);
4996
4997 if (clientCmd.m_createUserShapeArgs.m_shapes[i].m_collisionFlags & GEOM_CONCAVE_INTERNAL_EDGE)
4998 {
4999 btTriangleInfoMap* triangleInfoMap = new btTriangleInfoMap();
5000 btGenerateInternalEdgeInfo(terrainShape, triangleInfoMap);
5001 }
5002 serverStatusOut.m_createUserShapeResultArgs.m_userShapeUniqueId = collisionShapeUid;
5003 delete worldImporter;
5004
5005 serverStatusOut.m_type = CMD_CREATE_COLLISION_SHAPE_COMPLETED;
5006 }
5007
5008 delete heightfieldData;
5009 return hasStatus;
5010 }
5011 else
5012 {
5013 btScalar gridSpacing = 0.5;
5014 btScalar gridHeightScale = 1. / 256.;
5015
5016 bool flipQuadEdges = false;
5017 int upAxis = 2;
5018 /*btHeightfieldTerrainShape* heightfieldShape = worldImporter->createHeightfieldShape( width, height,
5019 heightfieldData,
5020 gridHeightScale,
5021 minHeight, maxHeight,
5022 upAxis, int(scalarType), flipQuadEdges);
5023 */
5024 btHeightfieldTerrainShape* heightfieldShape = new btHeightfieldTerrainShape(width, height,
5025 heightfieldData,
5026 gridHeightScale,
5027 minHeight, maxHeight,
5028 upAxis, scalarType, flipQuadEdges);
5029 m_data->m_collisionShapes.push_back(heightfieldShape);
5030 double textureScaling = clientCmd.m_createUserShapeArgs.m_shapes[i].m_heightfieldTextureScaling;
5031 heightfieldShape->setUserValue3(textureScaling);
5032 shape = heightfieldShape;
5033 if (upAxis == 2)
5034 heightfieldShape->setFlipTriangleWinding(true);
5035
5036 // scale the shape
5037 btVector3 localScaling(clientCmd.m_createUserShapeArgs.m_shapes[i].m_meshScale[0],
5038 clientCmd.m_createUserShapeArgs.m_shapes[i].m_meshScale[1],
5039 clientCmd.m_createUserShapeArgs.m_shapes[i].m_meshScale[2]);
5040
5041 heightfieldShape->setLocalScaling(localScaling);
5042 //buildAccelerator is optional, it may not support all features.
5043 heightfieldShape->buildAccelerator();
5044
5045 if (clientCmd.m_createUserShapeArgs.m_shapes[i].m_collisionFlags & GEOM_CONCAVE_INTERNAL_EDGE)
5046 {
5047 btTriangleInfoMap* triangleInfoMap = new btTriangleInfoMap();
5048 btGenerateInternalEdgeInfo(heightfieldShape, triangleInfoMap);
5049 }
5050 this->m_data->m_heightfieldDatas.push_back(heightfieldData);
5051
5052
5053 btAlignedObjectArray<GLInstanceVertex> gfxVertices;
5054 btAlignedObjectArray<int> indices;
5055 int strideInBytes = 9 * sizeof(float);
5056
5057 btTransform tr;
5058 tr.setIdentity();
5059 btVector3 aabbMin, aabbMax;
5060 heightfieldShape->getAabb(tr, aabbMin, aabbMax);
5061
5062 MyTriangleCollector4 col(aabbMin, aabbMax);
5063 col.m_pVerticesOut = &gfxVertices;
5064 col.m_pIndicesOut = &indices;
5065
5066
5067 heightfieldShape->processAllTriangles(&col, aabbMin, aabbMax);
5068 if (gfxVertices.size() && indices.size())
5069 {
5070
5071 urdfColObj.m_geometry.m_type = URDF_GEOM_HEIGHTFIELD;
5072 urdfColObj.m_geometry.m_meshFileType = UrdfGeometry::MEMORY_VERTICES;
5073 urdfColObj.m_geometry.m_normals.resize(gfxVertices.size());
5074 urdfColObj.m_geometry.m_vertices.resize(gfxVertices.size());
5075 urdfColObj.m_geometry.m_uvs.resize(gfxVertices.size());
5076 for (int v = 0; v < gfxVertices.size(); v++)
5077 {
5078 urdfColObj.m_geometry.m_vertices[v].setValue(gfxVertices[v].xyzw[0], gfxVertices[v].xyzw[1], gfxVertices[v].xyzw[2]);
5079 urdfColObj.m_geometry.m_uvs[v].setValue(gfxVertices[v].uv[0], gfxVertices[v].uv[1], 0);
5080 urdfColObj.m_geometry.m_normals[v].setValue(gfxVertices[v].normal[0], gfxVertices[v].normal[1], gfxVertices[v].normal[2]);
5081 }
5082 urdfColObj.m_geometry.m_indices.resize(indices.size());
5083 for (int ii = 0; ii < indices.size(); ii++)
5084 {
5085 urdfColObj.m_geometry.m_indices[ii] = indices[ii];
5086 }
5087 }
5088 }
5089 }
5090 break;
5091 }
5092 case GEOM_PLANE:
5093 {
5094 btVector3 planeNormal(clientCmd.m_createUserShapeArgs.m_shapes[i].m_planeNormal[0],
5095 clientCmd.m_createUserShapeArgs.m_shapes[i].m_planeNormal[1],
5096 clientCmd.m_createUserShapeArgs.m_shapes[i].m_planeNormal[2]);
5097
5098 shape = worldImporter->createPlaneShape(planeNormal, clientCmd.m_createUserShapeArgs.m_shapes[i].m_planeConstant);
5099 shape->setMargin(m_data->m_defaultCollisionMargin);
5100 if (compound)
5101 {
5102 compound->addChildShape(childTransform, shape);
5103 }
5104 urdfColObj.m_geometry.m_type = URDF_GEOM_PLANE;
5105 urdfColObj.m_geometry.m_planeNormal.setValue(
5106 clientCmd.m_createUserShapeArgs.m_shapes[i].m_planeNormal[0],
5107 clientCmd.m_createUserShapeArgs.m_shapes[i].m_planeNormal[1],
5108 clientCmd.m_createUserShapeArgs.m_shapes[i].m_planeNormal[2]);
5109
5110 break;
5111 }
5112 case GEOM_MESH:
5113 {
5114 btVector3 meshScale(clientCmd.m_createUserShapeArgs.m_shapes[i].m_meshScale[0],
5115 clientCmd.m_createUserShapeArgs.m_shapes[i].m_meshScale[1],
5116 clientCmd.m_createUserShapeArgs.m_shapes[i].m_meshScale[2]);
5117
5118 const std::string& urdf_path = "";
5119
5120 std::string fileName = clientCmd.m_createUserShapeArgs.m_shapes[i].m_meshFileName;
5121 urdfColObj.m_geometry.m_type = URDF_GEOM_MESH;
5122 urdfColObj.m_geometry.m_meshFileName = fileName;
5123
5124 urdfColObj.m_geometry.m_meshScale = meshScale;
5125 CommonFileIOInterface* fileIO = m_data->m_pluginManager.getFileIOInterface();
5126 pathPrefix[0] = 0;
5127 if (fileIO->findResourcePath(fileName.c_str(), relativeFileName, 1024))
5128 {
5129 b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024);
5130 }
5131
5132 const std::string& error_message_prefix = "";
5133 std::string out_found_filename;
5134 int out_type;
5135
5136 if (clientCmd.m_createUserShapeArgs.m_shapes[i].m_numVertices)
5137 {
5138 int numVertices = clientCmd.m_createUserShapeArgs.m_shapes[i].m_numVertices;
5139 int numIndices = clientCmd.m_createUserShapeArgs.m_shapes[i].m_numIndices;
5140
5141 //int totalUploadSizeInBytes = numVertices * sizeof(double) * 3 + numIndices * sizeof(int);
5142
5143 char* data = bufferServerToClient;
5144 double* vertexUpload = (double*)data;
5145 int* indexUpload = (int*)(data + numVertices * sizeof(double) * 3);
5146
5147 if (compound == 0)
5148 {
5149 compound = worldImporter->createCompoundShape();
5150 }
5151 compound->setMargin(m_data->m_defaultCollisionMargin);
5152
5153 if (clientCmd.m_createUserShapeArgs.m_shapes[i].m_numIndices)
5154 {
5155 BT_PROFILE("convert trimesh2");
5156 btTriangleMesh* meshInterface = new btTriangleMesh();
5157 this->m_data->m_meshInterfaces.push_back(meshInterface);
5158 {
5159 BT_PROFILE("convert vertices2");
5160
5161 for (int j = 0; j < clientCmd.m_createUserShapeArgs.m_shapes[i].m_numIndices / 3; j++)
5162 {
5163 int i0 = indexUpload[j * 3 + 0];
5164 int i1 = indexUpload[j * 3 + 1];
5165 int i2 = indexUpload[j * 3 + 2];
5166
5167 btVector3 v0(vertexUpload[i0 * 3 + 0],
5168 vertexUpload[i0 * 3 + 1],
5169 vertexUpload[i0 * 3 + 2]);
5170 btVector3 v1(vertexUpload[i1 * 3 + 0],
5171 vertexUpload[i1 * 3 + 1],
5172 vertexUpload[i1 * 3 + 2]);
5173 btVector3 v2(vertexUpload[i2 * 3 + 0],
5174 vertexUpload[i2 * 3 + 1],
5175 vertexUpload[i2 * 3 + 2]);
5176 meshInterface->addTriangle(v0 * meshScale, v1 * meshScale, v2 * meshScale);
5177 }
5178 }
5179
5180 {
5181 BT_PROFILE("create btBvhTriangleMeshShape");
5182 btBvhTriangleMeshShape* trimesh = new btBvhTriangleMeshShape(meshInterface, true, true);
5183 m_data->m_collisionShapes.push_back(trimesh);
5184
5185 if (clientCmd.m_createUserShapeArgs.m_shapes[i].m_collisionFlags & GEOM_CONCAVE_INTERNAL_EDGE)
5186 {
5187 btTriangleInfoMap* triangleInfoMap = new btTriangleInfoMap();
5188 btGenerateInternalEdgeInfo(trimesh, triangleInfoMap);
5189 }
5190 shape = trimesh;
5191 if (compound)
5192 {
5193 compound->addChildShape(childTransform, shape);
5194 shape->setMargin(m_data->m_defaultCollisionMargin);
5195 }
5196 }
5197 }
5198 else
5199 {
5200 btConvexHullShape* convexHull = worldImporter->createConvexHullShape();
5201 convexHull->setMargin(m_data->m_defaultCollisionMargin);
5202
5203 for (int v = 0; v < clientCmd.m_createUserShapeArgs.m_shapes[i].m_numVertices; v++)
5204 {
5205 btVector3 pt(vertexUpload[v * 3 + 0],
5206 vertexUpload[v * 3 + 1],
5207 vertexUpload[v * 3 + 2]);
5208 convexHull->addPoint(pt * meshScale, false);
5209 }
5210
5211 convexHull->recalcLocalAabb();
5212 convexHull->optimizeConvexHull();
5213 compound->addChildShape(childTransform, convexHull);
5214 }
5215 urdfColObj.m_geometry.m_meshFileType = UrdfGeometry::MEMORY_VERTICES;
5216 break;
5217 }
5218
5219 bool foundFile = UrdfFindMeshFile(fileIO, pathPrefix, relativeFileName, error_message_prefix, &out_found_filename, &out_type);
5220 if (foundFile)
5221 {
5222 urdfColObj.m_geometry.m_meshFileType = out_type;
5223
5224 if (clientCmd.m_createUserShapeArgs.m_shapes[i].m_collisionFlags & GEOM_FORCE_CONCAVE_TRIMESH)
5225 {
5226 CommonFileIOInterface* fileIO = m_data->m_pluginManager.getFileIOInterface();
5227 if (out_type == UrdfGeometry::FILE_STL)
5228 {
5229 CommonFileIOInterface* fileIO(m_data->m_pluginManager.getFileIOInterface());
5230 glmesh = LoadMeshFromSTL(relativeFileName, fileIO);
5231 }
5232
5233 if (out_type == UrdfGeometry::FILE_OBJ)
5234 {
5235 CommonFileIOInterface* fileIO = m_data->m_pluginManager.getFileIOInterface();
5236 glmesh = LoadMeshFromObj(relativeFileName, pathPrefix, fileIO);
5237 }
5238 //btBvhTriangleMeshShape is created below
5239 }
5240 else
5241 {
5242 if (out_type == UrdfGeometry::FILE_STL)
5243 {
5244 CommonFileIOInterface* fileIO(m_data->m_pluginManager.getFileIOInterface());
5245 glmesh = LoadMeshFromSTL(relativeFileName, fileIO);
5246
5247 B3_PROFILE("createConvexHullFromShapes");
5248 if (compound == 0)
5249 {
5250 compound = worldImporter->createCompoundShape();
5251 }
5252 btConvexHullShape* convexHull = worldImporter->createConvexHullShape();
5253 convexHull->setMargin(m_data->m_defaultCollisionMargin);
5254 for (int vv = 0; vv < glmesh->m_numvertices; vv++)
5255 {
5256 btVector3 pt(
5257 glmesh->m_vertices->at(vv).xyzw[0],
5258 glmesh->m_vertices->at(vv).xyzw[1],
5259 glmesh->m_vertices->at(vv).xyzw[2]);
5260 convexHull->addPoint(pt * meshScale, false);
5261 }
5262 if (clientCmd.m_createUserShapeArgs.m_shapes[i].m_collisionFlags & GEOM_INITIALIZE_SAT_FEATURES)
5263 {
5264 convexHull->initializePolyhedralFeatures();
5265 }
5266
5267 convexHull->recalcLocalAabb();
5268 convexHull->optimizeConvexHull();
5269
5270 compound->addChildShape(childTransform, convexHull);
5271 delete glmesh;
5272 glmesh = 0;
5273 }
5274 if (out_type == UrdfGeometry::FILE_OBJ)
5275 {
5276 //create a convex hull for each shape, and store it in a btCompoundShape
5277
5278 std::vector<tinyobj::shape_t> shapes;
5279 tinyobj::attrib_t attribute;
5280 std::string err = tinyobj::LoadObj(attribute, shapes, out_found_filename.c_str(), "", fileIO);
5281
5282 //shape = createConvexHullFromShapes(shapes, collision->m_geometry.m_meshScale);
5283 //static btCollisionShape* createConvexHullFromShapes(std::vector<tinyobj::shape_t>& shapes, const btVector3& geomScale)
5284 B3_PROFILE("createConvexHullFromShapes");
5285 if (compound == 0)
5286 {
5287 compound = worldImporter->createCompoundShape();
5288 }
5289 compound->setMargin(m_data->m_defaultCollisionMargin);
5290
5291 for (int s = 0; s < (int)shapes.size(); s++)
5292 {
5293 btConvexHullShape* convexHull = worldImporter->createConvexHullShape();
5294 convexHull->setMargin(m_data->m_defaultCollisionMargin);
5295 tinyobj::shape_t& shape = shapes[s];
5296 int faceCount = shape.mesh.indices.size();
5297
5298 for (int f = 0; f < faceCount; f += 3)
5299 {
5300 btVector3 pt;
5301 pt.setValue(attribute.vertices[3 * shape.mesh.indices[f + 0].vertex_index + 0],
5302 attribute.vertices[3 * shape.mesh.indices[f + 0].vertex_index + 1],
5303 attribute.vertices[3 * shape.mesh.indices[f + 0].vertex_index + 2]);
5304
5305 convexHull->addPoint(pt * meshScale, false);
5306
5307 pt.setValue(attribute.vertices[3 * shape.mesh.indices[f + 1].vertex_index + 0],
5308 attribute.vertices[3 * shape.mesh.indices[f + 1].vertex_index + 1],
5309 attribute.vertices[3 * shape.mesh.indices[f + 1].vertex_index + 2]);
5310 convexHull->addPoint(pt * meshScale, false);
5311
5312 pt.setValue(attribute.vertices[3 * shape.mesh.indices[f + 2].vertex_index + 0],
5313 attribute.vertices[3 * shape.mesh.indices[f + 2].vertex_index + 1],
5314 attribute.vertices[3 * shape.mesh.indices[f + 2].vertex_index + 2]);
5315 convexHull->addPoint(pt * meshScale, false);
5316 }
5317
5318 if (clientCmd.m_createUserShapeArgs.m_shapes[i].m_collisionFlags & GEOM_INITIALIZE_SAT_FEATURES)
5319 {
5320 convexHull->initializePolyhedralFeatures();
5321 }
5322 convexHull->recalcLocalAabb();
5323 convexHull->optimizeConvexHull();
5324
5325 compound->addChildShape(childTransform, convexHull);
5326 }
5327 }
5328 }
5329 }
5330 break;
5331 }
5332
5333 default:
5334 {
5335 }
5336 }
5337 if (urdfColObj.m_geometry.m_type != URDF_GEOM_UNKNOWN)
5338 {
5339 urdfCollisionObjects.push_back(urdfColObj);
5340 }
5341
5342 if (glmesh)
5343 {
5344 btVector3 meshScale(clientCmd.m_createUserShapeArgs.m_shapes[i].m_meshScale[0],
5345 clientCmd.m_createUserShapeArgs.m_shapes[i].m_meshScale[1],
5346 clientCmd.m_createUserShapeArgs.m_shapes[i].m_meshScale[2]);
5347
5348 if (!glmesh || glmesh->m_numvertices <= 0)
5349 {
5350 b3Warning("%s: cannot extract mesh from '%s'\n", pathPrefix, relativeFileName);
5351 delete glmesh;
5352 }
5353 else
5354 {
5355 btAlignedObjectArray<btVector3> convertedVerts;
5356 convertedVerts.reserve(glmesh->m_numvertices);
5357
5358 for (int v = 0; v < glmesh->m_numvertices; v++)
5359 {
5360 convertedVerts.push_back(btVector3(
5361 glmesh->m_vertices->at(v).xyzw[0] * meshScale[0],
5362 glmesh->m_vertices->at(v).xyzw[1] * meshScale[1],
5363 glmesh->m_vertices->at(v).xyzw[2] * meshScale[2]));
5364 }
5365
5366 if (clientCmd.m_createUserShapeArgs.m_shapes[i].m_collisionFlags & GEOM_FORCE_CONCAVE_TRIMESH)
5367 {
5368 BT_PROFILE("convert trimesh");
5369 btTriangleMesh* meshInterface = new btTriangleMesh();
5370 this->m_data->m_meshInterfaces.push_back(meshInterface);
5371 {
5372 BT_PROFILE("convert vertices");
5373
5374 for (int i = 0; i < glmesh->m_numIndices / 3; i++)
5375 {
5376 const btVector3& v0 = convertedVerts[glmesh->m_indices->at(i * 3)];
5377 const btVector3& v1 = convertedVerts[glmesh->m_indices->at(i * 3 + 1)];
5378 const btVector3& v2 = convertedVerts[glmesh->m_indices->at(i * 3 + 2)];
5379 meshInterface->addTriangle(v0, v1, v2);
5380 }
5381 }
5382
5383 {
5384 BT_PROFILE("create btBvhTriangleMeshShape");
5385 btBvhTriangleMeshShape* trimesh = new btBvhTriangleMeshShape(meshInterface, true, true);
5386 m_data->m_collisionShapes.push_back(trimesh);
5387
5388 if (clientCmd.m_createUserShapeArgs.m_shapes[i].m_collisionFlags & GEOM_CONCAVE_INTERNAL_EDGE)
5389 {
5390 btTriangleInfoMap* triangleInfoMap = new btTriangleInfoMap();
5391 btGenerateInternalEdgeInfo(trimesh, triangleInfoMap);
5392 }
5393 //trimesh->setLocalScaling(collision->m_geometry.m_meshScale);
5394 shape = trimesh;
5395 if (compound)
5396 {
5397 compound->addChildShape(childTransform, shape);
5398 shape->setMargin(m_data->m_defaultCollisionMargin);
5399 }
5400 }
5401 delete glmesh;
5402 }
5403 else
5404 {
5405 //convex mesh
5406
5407 if (compound == 0)
5408 {
5409 compound = worldImporter->createCompoundShape();
5410 }
5411 compound->setMargin(m_data->m_defaultCollisionMargin);
5412
5413 {
5414 btConvexHullShape* convexHull = worldImporter->createConvexHullShape();
5415 convexHull->setMargin(m_data->m_defaultCollisionMargin);
5416
5417 for (int v = 0; v < convertedVerts.size(); v++)
5418 {
5419 btVector3 pt = convertedVerts[v];
5420 convexHull->addPoint(pt, false);
5421 }
5422
5423 convexHull->recalcLocalAabb();
5424 convexHull->optimizeConvexHull();
5425 compound->addChildShape(childTransform, convexHull);
5426 }
5427 }
5428 }
5429 }
5430 }
5431 if (compound && compound->getNumChildShapes())
5432 {
5433 shape = compound;
5434 }
5435
5436 if (shape)
5437 {
5438 int collisionShapeUid = m_data->m_userCollisionShapeHandles.allocHandle();
5439 InternalCollisionShapeHandle* handle = m_data->m_userCollisionShapeHandles.getHandle(collisionShapeUid);
5440 handle->m_collisionShape = shape;
5441 for (int i = 0; i < urdfCollisionObjects.size(); i++)
5442 {
5443 handle->m_urdfCollisionObjects.push_back(urdfCollisionObjects[i]);
5444 }
5445 serverStatusOut.m_createUserShapeResultArgs.m_userShapeUniqueId = collisionShapeUid;
5446 m_data->m_worldImporters.push_back(worldImporter);
5447 serverStatusOut.m_type = CMD_CREATE_COLLISION_SHAPE_COMPLETED;
5448 }
5449 else
5450 {
5451 delete worldImporter;
5452 }
5453
5454 return hasStatus;
5455 }
5456
gatherVertices(const btTransform & trans,const btCollisionShape * colShape,btAlignedObjectArray<btVector3> & verticesOut,int collisionShapeIndex)5457 static void gatherVertices(const btTransform& trans, const btCollisionShape* colShape, btAlignedObjectArray<btVector3>& verticesOut, int collisionShapeIndex)
5458 {
5459 switch (colShape->getShapeType())
5460 {
5461 case COMPOUND_SHAPE_PROXYTYPE:
5462 {
5463 const btCompoundShape* compound = (const btCompoundShape*)colShape;
5464 for (int i = 0; i < compound->getNumChildShapes(); i++)
5465 {
5466 btTransform childTr = trans * compound->getChildTransform(i);
5467 if ((collisionShapeIndex < 0) || (collisionShapeIndex == i))
5468 {
5469 gatherVertices(childTr, compound->getChildShape(i), verticesOut, collisionShapeIndex);
5470 }
5471 }
5472 break;
5473 }
5474 case CONVEX_HULL_SHAPE_PROXYTYPE:
5475 {
5476 const btConvexHullShape* convex = (const btConvexHullShape*)colShape;
5477 btVector3 vtx;
5478 for (int i = 0; i < convex->getNumVertices(); i++)
5479 {
5480 convex->getVertex(i, vtx);
5481 btVector3 trVertex = trans * vtx;
5482 verticesOut.push_back(trVertex);
5483 }
5484 break;
5485 }
5486 default:
5487 {
5488 printf("?\n");
5489 }
5490 }
5491 }
5492
processResetMeshDataCommand(const struct SharedMemoryCommand & clientCmd,struct SharedMemoryStatus & serverStatusOut,char * bufferServerToClient,int bufferSizeInBytes)5493 bool PhysicsServerCommandProcessor::processResetMeshDataCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
5494 {
5495 bool hasStatus = true;
5496 BT_PROFILE("CMD_REQUEST_MESH_DATA");
5497 serverStatusOut.m_type = CMD_RESET_MESH_DATA_FAILED;
5498 int sizeInBytes = 0;
5499
5500 InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(clientCmd.m_requestMeshDataArgs.m_bodyUniqueId);
5501 if (bodyHandle)
5502 {
5503 int totalBytesPerVertex = sizeof(btVector3);
5504 double* vertexUpload = (double*)bufferServerToClient;
5505
5506 #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
5507
5508 if (bodyHandle->m_softBody)
5509 {
5510 btSoftBody* psb = bodyHandle->m_softBody;
5511
5512 int numVertices = psb->m_nodes.size();
5513 if (clientCmd.m_resetMeshDataArgs.m_numVertices == numVertices)
5514 {
5515 for (int i = 0; i < numVertices; ++i)
5516 {
5517 btSoftBody::Node& n = psb->m_nodes[i];
5518 n.m_x.setValue(vertexUpload[i*3+0], vertexUpload[i*3+1],vertexUpload[i*3+2]);
5519 }
5520 serverStatusOut.m_type = CMD_RESET_MESH_DATA_COMPLETED;
5521 }
5522 }
5523 #endif //SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
5524 }
5525 serverStatusOut.m_numDataStreamBytes = 0;
5526
5527 return hasStatus;
5528 }
5529
processRequestMeshDataCommand(const struct SharedMemoryCommand & clientCmd,struct SharedMemoryStatus & serverStatusOut,char * bufferServerToClient,int bufferSizeInBytes)5530 bool PhysicsServerCommandProcessor::processRequestMeshDataCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
5531 {
5532 bool hasStatus = true;
5533 BT_PROFILE("CMD_REQUEST_MESH_DATA");
5534 serverStatusOut.m_type = CMD_REQUEST_MESH_DATA_FAILED;
5535 serverStatusOut.m_numDataStreamBytes = 0;
5536 int sizeInBytes = 0;
5537
5538 InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(clientCmd.m_requestMeshDataArgs.m_bodyUniqueId);
5539 if (bodyHandle)
5540 {
5541 int totalBytesPerVertex = sizeof(btVector3);
5542 btVector3* verticesOut = (btVector3*)bufferServerToClient;
5543 const btCollisionShape* colShape = 0;
5544
5545 if (bodyHandle->m_multiBody)
5546 {
5547 //collision shape
5548
5549 if (clientCmd.m_requestMeshDataArgs.m_linkIndex == -1)
5550 {
5551 colShape = bodyHandle->m_multiBody->getBaseCollider()->getCollisionShape();
5552 }
5553 else
5554 {
5555 colShape = bodyHandle->m_multiBody->getLinkCollider(clientCmd.m_requestMeshDataArgs.m_linkIndex)->getCollisionShape();
5556 }
5557 }
5558 if (bodyHandle->m_rigidBody)
5559 {
5560 colShape = bodyHandle->m_rigidBody->getCollisionShape();
5561 }
5562
5563 if (colShape)
5564 {
5565 btAlignedObjectArray<btVector3> vertices;
5566 btTransform tr;
5567 tr.setIdentity();
5568 int collisionShapeIndex = -1;
5569 if (clientCmd.m_updateFlags & B3_MESH_DATA_COLLISIONSHAPEINDEX)
5570 {
5571 collisionShapeIndex = clientCmd.m_requestMeshDataArgs.m_collisionShapeIndex;
5572 }
5573 gatherVertices(tr, colShape, vertices, collisionShapeIndex);
5574
5575 int numVertices = vertices.size();
5576 int maxNumVertices = bufferSizeInBytes / totalBytesPerVertex - 1;
5577 int numVerticesRemaining = numVertices - clientCmd.m_requestMeshDataArgs.m_startingVertex;
5578 int verticesCopied = btMin(maxNumVertices, numVerticesRemaining);
5579
5580 if (verticesCopied > 0)
5581 {
5582 memcpy(verticesOut, &vertices[0], sizeof(btVector3) * verticesCopied);
5583 }
5584
5585 sizeInBytes = verticesCopied * sizeof(btVector3);
5586 serverStatusOut.m_type = CMD_REQUEST_MESH_DATA_COMPLETED;
5587 serverStatusOut.m_sendMeshDataArgs.m_numVerticesCopied = verticesCopied;
5588 serverStatusOut.m_sendMeshDataArgs.m_startingVertex = clientCmd.m_requestMeshDataArgs.m_startingVertex;
5589 serverStatusOut.m_sendMeshDataArgs.m_numVerticesRemaining = numVerticesRemaining - verticesCopied;
5590 }
5591
5592 #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
5593
5594 if (bodyHandle->m_softBody)
5595 {
5596 btSoftBody* psb = bodyHandle->m_softBody;
5597
5598 int flags = 0;
5599 if (clientCmd.m_updateFlags & B3_MESH_DATA_FLAGS)
5600 {
5601 flags = clientCmd.m_requestMeshDataArgs.m_flags;
5602 }
5603
5604 bool separateRenderMesh = false;
5605 if ((flags & B3_MESH_DATA_SIMULATION_MESH) == 0)
5606 {
5607 separateRenderMesh = (psb->m_renderNodes.size() != 0);
5608 }
5609 int numVertices = separateRenderMesh ? psb->m_renderNodes.size() : psb->m_nodes.size();
5610 int maxNumVertices = bufferSizeInBytes / totalBytesPerVertex - 1;
5611 int numVerticesRemaining = numVertices - clientCmd.m_requestMeshDataArgs.m_startingVertex;
5612 int verticesCopied = btMin(maxNumVertices, numVerticesRemaining);
5613 for (int i = 0; i < verticesCopied; ++i)
5614 {
5615 if (separateRenderMesh)
5616 {
5617
5618 const btSoftBody::RenderNode& n = psb->m_renderNodes[i + clientCmd.m_requestMeshDataArgs.m_startingVertex];
5619 verticesOut[i].setValue(n.m_x.x(), n.m_x.y(), n.m_x.z());
5620 }
5621 else
5622 {
5623 const btSoftBody::Node& n = psb->m_nodes[i + clientCmd.m_requestMeshDataArgs.m_startingVertex];
5624 verticesOut[i].setValue(n.m_x.x(), n.m_x.y(), n.m_x.z());
5625 }
5626 }
5627 sizeInBytes = verticesCopied * sizeof(btVector3);
5628 serverStatusOut.m_type = CMD_REQUEST_MESH_DATA_COMPLETED;
5629 serverStatusOut.m_sendMeshDataArgs.m_numVerticesCopied = verticesCopied;
5630 serverStatusOut.m_sendMeshDataArgs.m_startingVertex = clientCmd.m_requestMeshDataArgs.m_startingVertex;
5631 serverStatusOut.m_sendMeshDataArgs.m_numVerticesRemaining = numVerticesRemaining - verticesCopied;
5632 }
5633 #endif //SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
5634 }
5635
5636 serverStatusOut.m_numDataStreamBytes = sizeInBytes;
5637
5638 return hasStatus;
5639 }
5640
processCreateVisualShapeCommand(const struct SharedMemoryCommand & clientCmd,struct SharedMemoryStatus & serverStatusOut,char * bufferServerToClient,int bufferSizeInBytes)5641 bool PhysicsServerCommandProcessor::processCreateVisualShapeCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
5642 {
5643 bool hasStatus = true;
5644 serverStatusOut.m_type = CMD_CREATE_VISUAL_SHAPE_FAILED;
5645 double globalScaling = 1.f;
5646 int flags = 0;
5647 CommonFileIOInterface* fileIO = m_data->m_pluginManager.getFileIOInterface();
5648 BulletURDFImporter u2b(m_data->m_guiHelper, m_data->m_pluginManager.getRenderInterface(), fileIO, globalScaling, flags);
5649 u2b.setEnableTinyRenderer(m_data->m_enableTinyRenderer);
5650 btTransform localInertiaFrame;
5651 localInertiaFrame.setIdentity();
5652
5653 const char* pathPrefix = "";
5654 int visualShapeUniqueId = -1;
5655
5656 UrdfVisual visualShape;
5657 for (int userShapeIndex = 0; userShapeIndex < clientCmd.m_createUserShapeArgs.m_numUserShapes; userShapeIndex++)
5658 {
5659 btTransform childTrans;
5660 childTrans.setIdentity();
5661 visualShape.m_geometry.m_type = (UrdfGeomTypes)clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_type;
5662 char relativeFileName[1024];
5663 char pathPrefix[1024];
5664 pathPrefix[0] = 0;
5665
5666 const b3CreateUserShapeData& visShape = clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex];
5667
5668 switch (visualShape.m_geometry.m_type)
5669 {
5670 case URDF_GEOM_CYLINDER:
5671 {
5672 visualShape.m_geometry.m_capsuleHeight = visShape.m_capsuleHeight;
5673 visualShape.m_geometry.m_capsuleRadius = visShape.m_capsuleRadius;
5674 break;
5675 }
5676 case URDF_GEOM_BOX:
5677 {
5678 visualShape.m_geometry.m_boxSize.setValue(2. * visShape.m_boxHalfExtents[0],
5679 2. * visShape.m_boxHalfExtents[1],
5680 2. * visShape.m_boxHalfExtents[2]);
5681 break;
5682 }
5683 case URDF_GEOM_SPHERE:
5684 {
5685 visualShape.m_geometry.m_sphereRadius = visShape.m_sphereRadius;
5686 break;
5687 }
5688 case URDF_GEOM_CAPSULE:
5689 {
5690 visualShape.m_geometry.m_hasFromTo = visShape.m_hasFromTo;
5691 if (visualShape.m_geometry.m_hasFromTo)
5692 {
5693 visualShape.m_geometry.m_capsuleFrom.setValue(visShape.m_capsuleFrom[0],
5694 visShape.m_capsuleFrom[1],
5695 visShape.m_capsuleFrom[2]);
5696 visualShape.m_geometry.m_capsuleTo.setValue(visShape.m_capsuleTo[0],
5697 visShape.m_capsuleTo[1],
5698 visShape.m_capsuleTo[2]);
5699 }
5700 else
5701 {
5702 visualShape.m_geometry.m_capsuleHeight = visShape.m_capsuleHeight;
5703 visualShape.m_geometry.m_capsuleRadius = visShape.m_capsuleRadius;
5704 }
5705 break;
5706 }
5707 case URDF_GEOM_MESH:
5708 {
5709 std::string fileName = clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_meshFileName;
5710 if (fileName.length())
5711 {
5712 const std::string& error_message_prefix = "";
5713 std::string out_found_filename;
5714 int out_type;
5715 if (fileIO->findResourcePath(fileName.c_str(), relativeFileName, 1024))
5716 {
5717 b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024);
5718 }
5719
5720 bool foundFile = UrdfFindMeshFile(fileIO, pathPrefix, relativeFileName, error_message_prefix, &out_found_filename, &out_type);
5721 if (foundFile)
5722 {
5723 visualShape.m_geometry.m_meshFileType = out_type;
5724 visualShape.m_geometry.m_meshFileName = fileName;
5725 }
5726 else
5727 {
5728 }
5729 }
5730 else
5731 {
5732 visualShape.m_geometry.m_meshFileType = UrdfGeometry::MEMORY_VERTICES;
5733 int numVertices = clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_numVertices;
5734 int numIndices = clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_numIndices;
5735 int numUVs = clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_numUVs;
5736 int numNormals = clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_numNormals;
5737
5738 if (numVertices > 0 && numIndices > 0)
5739 {
5740 char* data = bufferServerToClient;
5741 double* vertexUpload = (double*)data;
5742 int* indexUpload = (int*)(data + numVertices * sizeof(double) * 3);
5743 double* normalUpload = (double*)(data + numVertices * sizeof(double) * 3 + numIndices * sizeof(int));
5744 double* uvUpload = (double*)(data + numVertices * sizeof(double) * 3 + numIndices * sizeof(int) + numNormals * sizeof(double) * 3);
5745
5746 for (int i = 0; i < numIndices; i++)
5747 {
5748 visualShape.m_geometry.m_indices.push_back(indexUpload[i]);
5749 }
5750 for (int i = 0; i < numVertices; i++)
5751 {
5752 btVector3 v0(vertexUpload[i * 3 + 0],
5753 vertexUpload[i * 3 + 1],
5754 vertexUpload[i * 3 + 2]);
5755 visualShape.m_geometry.m_vertices.push_back(v0);
5756 }
5757 for (int i = 0; i < numNormals; i++)
5758 {
5759 btVector3 normal(normalUpload[i * 3 + 0],
5760 normalUpload[i * 3 + 1],
5761 normalUpload[i * 3 + 2]);
5762 visualShape.m_geometry.m_normals.push_back(normal);
5763 }
5764 for (int i = 0; i < numUVs; i++)
5765 {
5766 btVector3 uv(uvUpload[i * 2 + 0], uvUpload[i * 2 + 1], 0);
5767 visualShape.m_geometry.m_uvs.push_back(uv);
5768 }
5769 }
5770 }
5771 visualShape.m_geometry.m_meshScale.setValue(clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_meshScale[0],
5772 clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_meshScale[1],
5773 clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_meshScale[2]);
5774 break;
5775 }
5776
5777 default:
5778 {
5779 }
5780 };
5781 visualShape.m_name = "in_memory";
5782 visualShape.m_materialName = "";
5783 visualShape.m_sourceFileLocation = "in_memory_unknown_line";
5784 visualShape.m_linkLocalFrame.setIdentity();
5785 visualShape.m_geometry.m_hasLocalMaterial = false;
5786
5787 bool hasRGBA = (clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_visualFlags & GEOM_VISUAL_HAS_RGBA_COLOR) != 0;
5788 ;
5789 bool hasSpecular = (clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_visualFlags & GEOM_VISUAL_HAS_SPECULAR_COLOR) != 0;
5790 ;
5791 visualShape.m_geometry.m_hasLocalMaterial = hasRGBA | hasSpecular;
5792 if (visualShape.m_geometry.m_hasLocalMaterial)
5793 {
5794 if (hasRGBA)
5795 {
5796 visualShape.m_geometry.m_localMaterial.m_matColor.m_rgbaColor.setValue(
5797 clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_rgbaColor[0],
5798 clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_rgbaColor[1],
5799 clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_rgbaColor[2],
5800 clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_rgbaColor[3]);
5801 }
5802 else
5803 {
5804 visualShape.m_geometry.m_localMaterial.m_matColor.m_rgbaColor.setValue(1, 1, 1, 1);
5805 }
5806 if (hasSpecular)
5807 {
5808 visualShape.m_geometry.m_localMaterial.m_matColor.m_specularColor.setValue(
5809 clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_specularColor[0],
5810 clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_specularColor[1],
5811 clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_specularColor[2]);
5812 }
5813 else
5814 {
5815 visualShape.m_geometry.m_localMaterial.m_matColor.m_specularColor.setValue(0.4, 0.4, 0.4);
5816 }
5817 }
5818
5819 if (clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_hasChildTransform != 0)
5820 {
5821 childTrans.setOrigin(btVector3(clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_childPosition[0],
5822 clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_childPosition[1],
5823 clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_childPosition[2]));
5824 childTrans.setRotation(btQuaternion(
5825 clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_childOrientation[0],
5826 clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_childOrientation[1],
5827 clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_childOrientation[2],
5828 clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_childOrientation[3]));
5829 }
5830
5831 if (visualShapeUniqueId < 0)
5832 {
5833 visualShapeUniqueId = m_data->m_userVisualShapeHandles.allocHandle();
5834 }
5835 InternalVisualShapeHandle* visualHandle = m_data->m_userVisualShapeHandles.getHandle(visualShapeUniqueId);
5836 visualHandle->m_OpenGLGraphicsIndex = -1;
5837 visualHandle->m_tinyRendererVisualShapeIndex = -1;
5838 //tinyrenderer doesn't separate shape versus instance, so create it when creating the multibody instance
5839 //store needed info for tinyrenderer
5840
5841 visualShape.m_linkLocalFrame = childTrans;
5842 visualHandle->m_visualShapes.push_back(visualShape);
5843 visualHandle->m_pathPrefixes.push_back(pathPrefix[0] ? pathPrefix : "");
5844
5845 serverStatusOut.m_createUserShapeResultArgs.m_userShapeUniqueId = visualShapeUniqueId;
5846 serverStatusOut.m_type = CMD_CREATE_VISUAL_SHAPE_COMPLETED;
5847 }
5848
5849 return hasStatus;
5850 }
5851
processCustomCommand(const struct SharedMemoryCommand & clientCmd,struct SharedMemoryStatus & serverStatusOut,char * bufferServerToClient,int bufferSizeInBytes)5852 bool PhysicsServerCommandProcessor::processCustomCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
5853 {
5854 bool hasStatus = true;
5855
5856 SharedMemoryStatus& serverCmd = serverStatusOut;
5857 serverCmd.m_type = CMD_CUSTOM_COMMAND_FAILED;
5858 serverCmd.m_customCommandResultArgs.m_returnDataSizeInBytes = 0;
5859 serverCmd.m_customCommandResultArgs.m_returnDataType = -1;
5860 serverCmd.m_customCommandResultArgs.m_returnDataStart = 0;
5861
5862 serverCmd.m_customCommandResultArgs.m_pluginUniqueId = -1;
5863
5864 if (clientCmd.m_updateFlags & CMD_CUSTOM_COMMAND_LOAD_PLUGIN)
5865 {
5866 //pluginPath could be registered or load from disk
5867 const char* postFix = "";
5868 if (clientCmd.m_updateFlags & CMD_CUSTOM_COMMAND_LOAD_PLUGIN_POSTFIX)
5869 {
5870 postFix = clientCmd.m_customCommandArgs.m_postFix;
5871 }
5872
5873 int pluginUniqueId = m_data->m_pluginManager.loadPlugin(clientCmd.m_customCommandArgs.m_pluginPath, postFix);
5874 if (pluginUniqueId >= 0)
5875 {
5876 serverCmd.m_customCommandResultArgs.m_pluginUniqueId = pluginUniqueId;
5877
5878 serverCmd.m_type = CMD_CUSTOM_COMMAND_COMPLETED;
5879 }
5880 }
5881 if (clientCmd.m_updateFlags & CMD_CUSTOM_COMMAND_UNLOAD_PLUGIN)
5882 {
5883 m_data->m_pluginManager.unloadPlugin(clientCmd.m_customCommandArgs.m_pluginUniqueId);
5884 serverCmd.m_type = CMD_CUSTOM_COMMAND_COMPLETED;
5885 }
5886
5887 if (clientCmd.m_updateFlags & CMD_CUSTOM_COMMAND_EXECUTE_PLUGIN_COMMAND)
5888 {
5889 int startBytes = clientCmd.m_customCommandArgs.m_startingReturnBytes;
5890 if (startBytes == 0)
5891 {
5892 int result = m_data->m_pluginManager.executePluginCommand(clientCmd.m_customCommandArgs.m_pluginUniqueId, &clientCmd.m_customCommandArgs.m_arguments);
5893 serverCmd.m_customCommandResultArgs.m_executeCommandResult = result;
5894 }
5895 const b3UserDataValue* returnData = m_data->m_pluginManager.getReturnData(clientCmd.m_customCommandArgs.m_pluginUniqueId);
5896 if (returnData)
5897 {
5898 int totalRemain = returnData->m_length - startBytes;
5899 int numBytes = totalRemain <= bufferSizeInBytes ? totalRemain : bufferSizeInBytes;
5900 serverStatusOut.m_numDataStreamBytes = numBytes;
5901 for (int i = 0; i < numBytes; i++)
5902 {
5903 bufferServerToClient[i] = returnData->m_data1[i+ startBytes];
5904 }
5905 serverCmd.m_customCommandResultArgs.m_returnDataSizeInBytes = returnData->m_length;
5906 serverCmd.m_customCommandResultArgs.m_returnDataType = returnData->m_type;
5907 serverCmd.m_customCommandResultArgs.m_returnDataStart = startBytes;
5908 }
5909 else
5910 {
5911 serverStatusOut.m_numDataStreamBytes = 0;
5912 }
5913
5914 serverCmd.m_type = CMD_CUSTOM_COMMAND_COMPLETED;
5915 }
5916 return hasStatus;
5917 }
5918
processUserDebugDrawCommand(const struct SharedMemoryCommand & clientCmd,struct SharedMemoryStatus & serverStatusOut,char * bufferServerToClient,int bufferSizeInBytes)5919 bool PhysicsServerCommandProcessor::processUserDebugDrawCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
5920 {
5921 bool hasStatus = true;
5922
5923 BT_PROFILE("CMD_USER_DEBUG_DRAW");
5924 SharedMemoryStatus& serverCmd = serverStatusOut;
5925 serverCmd.m_type = CMD_USER_DEBUG_DRAW_FAILED;
5926
5927 int trackingVisualShapeIndex = -1;
5928
5929 if (clientCmd.m_userDebugDrawArgs.m_parentObjectUniqueId >= 0)
5930 {
5931 InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(clientCmd.m_userDebugDrawArgs.m_parentObjectUniqueId);
5932 if (bodyHandle)
5933 {
5934 int linkIndex = -1;
5935
5936 if (bodyHandle->m_multiBody)
5937 {
5938 int linkIndex = clientCmd.m_userDebugDrawArgs.m_parentLinkIndex;
5939 if (linkIndex == -1)
5940 {
5941 if (bodyHandle->m_multiBody->getBaseCollider())
5942 {
5943 trackingVisualShapeIndex = bodyHandle->m_multiBody->getBaseCollider()->getUserIndex();
5944 }
5945 }
5946 else
5947 {
5948 if (linkIndex >= 0 && linkIndex < bodyHandle->m_multiBody->getNumLinks())
5949 {
5950 if (bodyHandle->m_multiBody->getLink(linkIndex).m_collider)
5951 {
5952 trackingVisualShapeIndex = bodyHandle->m_multiBody->getLink(linkIndex).m_collider->getUserIndex();
5953 }
5954 }
5955 }
5956 }
5957 if (bodyHandle->m_rigidBody)
5958 {
5959 trackingVisualShapeIndex = bodyHandle->m_rigidBody->getUserIndex();
5960 }
5961 }
5962 }
5963
5964 if (clientCmd.m_updateFlags & USER_DEBUG_ADD_PARAMETER)
5965 {
5966 int uid = m_data->m_guiHelper->addUserDebugParameter(
5967 clientCmd.m_userDebugDrawArgs.m_text,
5968 clientCmd.m_userDebugDrawArgs.m_rangeMin,
5969 clientCmd.m_userDebugDrawArgs.m_rangeMax,
5970 clientCmd.m_userDebugDrawArgs.m_startValue);
5971 serverCmd.m_userDebugDrawArgs.m_debugItemUniqueId = uid;
5972 serverCmd.m_type = CMD_USER_DEBUG_DRAW_COMPLETED;
5973 }
5974 if (clientCmd.m_updateFlags & USER_DEBUG_READ_PARAMETER)
5975 {
5976 int ok = m_data->m_guiHelper->readUserDebugParameter(
5977 clientCmd.m_userDebugDrawArgs.m_itemUniqueId,
5978 &serverCmd.m_userDebugDrawArgs.m_parameterValue);
5979 if (ok)
5980 {
5981 serverCmd.m_type = CMD_USER_DEBUG_DRAW_PARAMETER_COMPLETED;
5982 }
5983 }
5984 if ((clientCmd.m_updateFlags & USER_DEBUG_SET_CUSTOM_OBJECT_COLOR) || (clientCmd.m_updateFlags & USER_DEBUG_REMOVE_CUSTOM_OBJECT_COLOR))
5985 {
5986 int bodyUniqueId = clientCmd.m_userDebugDrawArgs.m_objectUniqueId;
5987 InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId);
5988 if (body)
5989 {
5990 btCollisionObject* destColObj = 0;
5991
5992 if (body->m_multiBody)
5993 {
5994 if (clientCmd.m_userDebugDrawArgs.m_linkIndex == -1)
5995 {
5996 destColObj = body->m_multiBody->getBaseCollider();
5997 }
5998 else
5999 {
6000 if (clientCmd.m_userDebugDrawArgs.m_linkIndex >= 0 && clientCmd.m_userDebugDrawArgs.m_linkIndex < body->m_multiBody->getNumLinks())
6001 {
6002 destColObj = body->m_multiBody->getLink(clientCmd.m_userDebugDrawArgs.m_linkIndex).m_collider;
6003 }
6004 }
6005 }
6006 if (body->m_rigidBody)
6007 {
6008 destColObj = body->m_rigidBody;
6009 }
6010
6011 if (destColObj)
6012 {
6013 if (clientCmd.m_updateFlags & USER_DEBUG_REMOVE_CUSTOM_OBJECT_COLOR)
6014 {
6015 destColObj->removeCustomDebugColor();
6016 serverCmd.m_type = CMD_USER_DEBUG_DRAW_COMPLETED;
6017 }
6018 if (clientCmd.m_updateFlags & USER_DEBUG_SET_CUSTOM_OBJECT_COLOR)
6019 {
6020 btVector3 objectColorRGB;
6021 objectColorRGB.setValue(clientCmd.m_userDebugDrawArgs.m_objectDebugColorRGB[0],
6022 clientCmd.m_userDebugDrawArgs.m_objectDebugColorRGB[1],
6023 clientCmd.m_userDebugDrawArgs.m_objectDebugColorRGB[2]);
6024 destColObj->setCustomDebugColor(objectColorRGB);
6025 serverCmd.m_type = CMD_USER_DEBUG_DRAW_COMPLETED;
6026 }
6027 }
6028 }
6029 }
6030
6031 if (clientCmd.m_updateFlags & USER_DEBUG_HAS_TEXT)
6032 {
6033 //addUserDebugText3D( const double orientation[4], const double textColorRGB[3], double size, double lifeTime, int trackingObjectUniqueId, int optionFlags){return -1;}
6034
6035 int optionFlags = clientCmd.m_userDebugDrawArgs.m_optionFlags;
6036
6037 if ((clientCmd.m_updateFlags & USER_DEBUG_HAS_TEXT_ORIENTATION) == 0)
6038 {
6039 optionFlags |= DEB_DEBUG_TEXT_ALWAYS_FACE_CAMERA;
6040 }
6041
6042 int replaceItemUniqueId = -1;
6043 if ((clientCmd.m_updateFlags & USER_DEBUG_HAS_REPLACE_ITEM_UNIQUE_ID) != 0)
6044 {
6045 replaceItemUniqueId = clientCmd.m_userDebugDrawArgs.m_replaceItemUniqueId;
6046 }
6047
6048 int uid = m_data->m_guiHelper->addUserDebugText3D(clientCmd.m_userDebugDrawArgs.m_text,
6049 clientCmd.m_userDebugDrawArgs.m_textPositionXYZ,
6050 clientCmd.m_userDebugDrawArgs.m_textOrientation,
6051 clientCmd.m_userDebugDrawArgs.m_textColorRGB,
6052 clientCmd.m_userDebugDrawArgs.m_textSize,
6053 clientCmd.m_userDebugDrawArgs.m_lifeTime,
6054 trackingVisualShapeIndex,
6055 optionFlags,
6056 replaceItemUniqueId);
6057
6058 if (uid >= 0)
6059 {
6060 serverCmd.m_userDebugDrawArgs.m_debugItemUniqueId = uid;
6061 serverCmd.m_type = CMD_USER_DEBUG_DRAW_COMPLETED;
6062 }
6063 }
6064
6065 if (clientCmd.m_updateFlags & USER_DEBUG_HAS_LINE)
6066 {
6067 int replaceItemUid = -1;
6068 if (clientCmd.m_updateFlags & USER_DEBUG_HAS_REPLACE_ITEM_UNIQUE_ID)
6069 {
6070 replaceItemUid = clientCmd.m_userDebugDrawArgs.m_replaceItemUniqueId;
6071 }
6072
6073 int uid = m_data->m_guiHelper->addUserDebugLine(
6074 clientCmd.m_userDebugDrawArgs.m_debugLineFromXYZ,
6075 clientCmd.m_userDebugDrawArgs.m_debugLineToXYZ,
6076 clientCmd.m_userDebugDrawArgs.m_debugLineColorRGB,
6077 clientCmd.m_userDebugDrawArgs.m_lineWidth,
6078 clientCmd.m_userDebugDrawArgs.m_lifeTime,
6079 trackingVisualShapeIndex,
6080 replaceItemUid);
6081
6082 if (uid >= 0)
6083 {
6084 serverCmd.m_userDebugDrawArgs.m_debugItemUniqueId = uid;
6085 serverCmd.m_type = CMD_USER_DEBUG_DRAW_COMPLETED;
6086 }
6087 }
6088
6089 if (clientCmd.m_updateFlags & USER_DEBUG_REMOVE_ALL)
6090 {
6091 m_data->m_guiHelper->removeAllUserDebugItems();
6092 serverCmd.m_type = CMD_USER_DEBUG_DRAW_COMPLETED;
6093 }
6094
6095 if (clientCmd.m_updateFlags & USER_DEBUG_REMOVE_ALL_PARAMETERS)
6096 {
6097 m_data->m_guiHelper->removeAllUserParameters();
6098 serverCmd.m_type = CMD_USER_DEBUG_DRAW_COMPLETED;
6099 }
6100
6101 if (clientCmd.m_updateFlags & USER_DEBUG_REMOVE_ONE_ITEM)
6102 {
6103 m_data->m_guiHelper->removeUserDebugItem(clientCmd.m_userDebugDrawArgs.m_itemUniqueId);
6104 serverCmd.m_type = CMD_USER_DEBUG_DRAW_COMPLETED;
6105 }
6106 return hasStatus;
6107 }
6108
processSetVRCameraStateCommand(const struct SharedMemoryCommand & clientCmd,struct SharedMemoryStatus & serverStatusOut,char * bufferServerToClient,int bufferSizeInBytes)6109 bool PhysicsServerCommandProcessor::processSetVRCameraStateCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
6110 {
6111 bool hasStatus = true;
6112 BT_PROFILE("CMD_SET_VR_CAMERA_STATE");
6113
6114 if (clientCmd.m_updateFlags & VR_CAMERA_ROOT_POSITION)
6115 {
6116 gVRTeleportPos1[0] = clientCmd.m_vrCameraStateArguments.m_rootPosition[0];
6117 gVRTeleportPos1[1] = clientCmd.m_vrCameraStateArguments.m_rootPosition[1];
6118 gVRTeleportPos1[2] = clientCmd.m_vrCameraStateArguments.m_rootPosition[2];
6119 }
6120 if (clientCmd.m_updateFlags & VR_CAMERA_ROOT_ORIENTATION)
6121 {
6122 gVRTeleportOrn[0] = clientCmd.m_vrCameraStateArguments.m_rootOrientation[0];
6123 gVRTeleportOrn[1] = clientCmd.m_vrCameraStateArguments.m_rootOrientation[1];
6124 gVRTeleportOrn[2] = clientCmd.m_vrCameraStateArguments.m_rootOrientation[2];
6125 gVRTeleportOrn[3] = clientCmd.m_vrCameraStateArguments.m_rootOrientation[3];
6126 }
6127
6128 if (clientCmd.m_updateFlags & VR_CAMERA_ROOT_TRACKING_OBJECT)
6129 {
6130 gVRTrackingObjectUniqueId = clientCmd.m_vrCameraStateArguments.m_trackingObjectUniqueId;
6131 }
6132
6133 if (clientCmd.m_updateFlags & VR_CAMERA_FLAG)
6134 {
6135 gVRTrackingObjectFlag = clientCmd.m_vrCameraStateArguments.m_trackingObjectFlag;
6136 }
6137
6138 serverStatusOut.m_type = CMD_CLIENT_COMMAND_COMPLETED;
6139 return hasStatus;
6140 }
6141
processRequestVREventsCommand(const struct SharedMemoryCommand & clientCmd,struct SharedMemoryStatus & serverStatusOut,char * bufferServerToClient,int bufferSizeInBytes)6142 bool PhysicsServerCommandProcessor::processRequestVREventsCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
6143 {
6144 bool hasStatus = true;
6145 //BT_PROFILE("CMD_REQUEST_VR_EVENTS_DATA");
6146 serverStatusOut.m_sendVREvents.m_numVRControllerEvents = 0;
6147
6148 for (int i = 0; i < MAX_VR_CONTROLLERS; i++)
6149 {
6150 b3VRControllerEvent& event = m_data->m_vrControllerEvents.m_vrEvents[i];
6151
6152 if (clientCmd.m_updateFlags & event.m_deviceType)
6153 {
6154 if (event.m_numButtonEvents + event.m_numMoveEvents)
6155 {
6156 serverStatusOut.m_sendVREvents.m_controllerEvents[serverStatusOut.m_sendVREvents.m_numVRControllerEvents++] = event;
6157 event.m_numButtonEvents = 0;
6158 event.m_numMoveEvents = 0;
6159 for (int b = 0; b < MAX_VR_BUTTONS; b++)
6160 {
6161 event.m_buttons[b] = 0;
6162 }
6163 }
6164 }
6165 }
6166 serverStatusOut.m_type = CMD_REQUEST_VR_EVENTS_DATA_COMPLETED;
6167 return hasStatus;
6168 }
6169
processRequestMouseEventsCommand(const struct SharedMemoryCommand & clientCmd,struct SharedMemoryStatus & serverStatusOut,char * bufferServerToClient,int bufferSizeInBytes)6170 bool PhysicsServerCommandProcessor::processRequestMouseEventsCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
6171 {
6172 bool hasStatus = true;
6173 serverStatusOut.m_sendMouseEvents.m_numMouseEvents = m_data->m_mouseEvents.size();
6174 if (serverStatusOut.m_sendMouseEvents.m_numMouseEvents > MAX_MOUSE_EVENTS)
6175 {
6176 serverStatusOut.m_sendMouseEvents.m_numMouseEvents = MAX_MOUSE_EVENTS;
6177 }
6178 for (int i = 0; i < serverStatusOut.m_sendMouseEvents.m_numMouseEvents; i++)
6179 {
6180 serverStatusOut.m_sendMouseEvents.m_mouseEvents[i] = m_data->m_mouseEvents[i];
6181 }
6182
6183 m_data->m_mouseEvents.resize(0);
6184 serverStatusOut.m_type = CMD_REQUEST_MOUSE_EVENTS_DATA_COMPLETED;
6185 return hasStatus;
6186 }
6187
processRequestKeyboardEventsCommand(const struct SharedMemoryCommand & clientCmd,struct SharedMemoryStatus & serverStatusOut,char * bufferServerToClient,int bufferSizeInBytes)6188 bool PhysicsServerCommandProcessor::processRequestKeyboardEventsCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
6189 {
6190 //BT_PROFILE("CMD_REQUEST_KEYBOARD_EVENTS_DATA");
6191 bool hasStatus = true;
6192 serverStatusOut.m_sendKeyboardEvents.m_numKeyboardEvents = m_data->m_keyboardEvents.size();
6193 if (serverStatusOut.m_sendKeyboardEvents.m_numKeyboardEvents > MAX_KEYBOARD_EVENTS)
6194 {
6195 serverStatusOut.m_sendKeyboardEvents.m_numKeyboardEvents = MAX_KEYBOARD_EVENTS;
6196 }
6197 for (int i = 0; i < serverStatusOut.m_sendKeyboardEvents.m_numKeyboardEvents; i++)
6198 {
6199 serverStatusOut.m_sendKeyboardEvents.m_keyboardEvents[i] = m_data->m_keyboardEvents[i];
6200 }
6201
6202 btAlignedObjectArray<b3KeyboardEvent> events;
6203
6204 //remove out-of-date events
6205 for (int i = 0; i < m_data->m_keyboardEvents.size(); i++)
6206 {
6207 b3KeyboardEvent event = m_data->m_keyboardEvents[i];
6208 if (event.m_keyState & eButtonIsDown)
6209 {
6210 event.m_keyState = eButtonIsDown;
6211 events.push_back(event);
6212 }
6213 }
6214 m_data->m_keyboardEvents.resize(events.size());
6215 for (int i = 0; i < events.size(); i++)
6216 {
6217 m_data->m_keyboardEvents[i] = events[i];
6218 }
6219
6220 serverStatusOut.m_type = CMD_REQUEST_KEYBOARD_EVENTS_DATA_COMPLETED;
6221 return hasStatus;
6222 }
6223
6224 #if __cplusplus >= 201103L
6225 #include <atomic>
6226
6227 struct CastSyncInfo
6228 {
6229 std::atomic<int> m_nextTaskNumber;
6230
CastSyncInfoCastSyncInfo6231 CastSyncInfo() : m_nextTaskNumber(0) {}
6232
getNextTaskCastSyncInfo6233 inline int getNextTask()
6234 {
6235 return m_nextTaskNumber++;
6236 }
6237 };
6238 #else // __cplusplus >= 201103L
6239 struct CastSyncInfo
6240 {
6241 volatile int m_nextTaskNumber;
6242 btSpinMutex m_taskLock;
6243
CastSyncInfoCastSyncInfo6244 CastSyncInfo() : m_nextTaskNumber(0) {}
6245
getNextTaskCastSyncInfo6246 inline int getNextTask()
6247 {
6248 m_taskLock.lock();
6249 const int taskNr = m_nextTaskNumber++;
6250 m_taskLock.unlock();
6251 return taskNr;
6252 }
6253 };
6254 #endif // __cplusplus >= 201103L
6255
6256 struct FilteredClosestRayResultCallback : public btCollisionWorld::ClosestRayResultCallback
6257 {
FilteredClosestRayResultCallbackFilteredClosestRayResultCallback6258 FilteredClosestRayResultCallback(const btVector3& rayFromWorld, const btVector3& rayToWorld, int collisionFilterMask)
6259 : btCollisionWorld::ClosestRayResultCallback(rayFromWorld, rayToWorld),
6260 m_collisionFilterMask(collisionFilterMask)
6261 {
6262 }
6263
6264 int m_collisionFilterMask;
6265
addSingleResultFilteredClosestRayResultCallback6266 virtual btScalar addSingleResult(btCollisionWorld::LocalRayResult& rayResult, bool normalInWorldSpace)
6267 {
6268 bool collides = (rayResult.m_collisionObject->getBroadphaseHandle()->m_collisionFilterGroup & m_collisionFilterMask) != 0;
6269 if (!collides)
6270 return m_closestHitFraction;
6271 return btCollisionWorld::ClosestRayResultCallback::addSingleResult(rayResult, normalInWorldSpace);
6272 }
6273 };
6274
6275 struct FilteredAllHitsRayResultCallback : public btCollisionWorld::AllHitsRayResultCallback
6276 {
FilteredAllHitsRayResultCallbackFilteredAllHitsRayResultCallback6277 FilteredAllHitsRayResultCallback(const btVector3& rayFromWorld, const btVector3& rayToWorld, int collisionFilterMask, btScalar fractionEpsilon)
6278 : btCollisionWorld::AllHitsRayResultCallback(rayFromWorld, rayToWorld),
6279 m_collisionFilterMask(collisionFilterMask),
6280 m_fractionEpsilon(fractionEpsilon)
6281 {
6282 }
6283
6284 int m_collisionFilterMask;
6285 btScalar m_fractionEpsilon;
6286
addSingleResultFilteredAllHitsRayResultCallback6287 virtual btScalar addSingleResult(btCollisionWorld::LocalRayResult& rayResult, bool normalInWorldSpace)
6288 {
6289 bool collides = (rayResult.m_collisionObject->getBroadphaseHandle()->m_collisionFilterGroup & m_collisionFilterMask) != 0;
6290 if (!collides)
6291 return m_closestHitFraction;
6292 //remove duplicate hits:
6293 //same collision object, link index and hit fraction
6294 bool isDuplicate = false;
6295
6296 for (int i = 0; i < m_collisionObjects.size(); i++)
6297 {
6298 if (m_collisionObjects[i] == rayResult.m_collisionObject)
6299 {
6300 btScalar diffFraction = m_hitFractions[i] - rayResult.m_hitFraction;
6301 if (btEqual(diffFraction, m_fractionEpsilon))
6302 {
6303 isDuplicate = true;
6304 break;
6305 }
6306 }
6307 }
6308 if (isDuplicate)
6309 return m_closestHitFraction;
6310
6311 return btCollisionWorld::AllHitsRayResultCallback::addSingleResult(rayResult, normalInWorldSpace);
6312 }
6313 };
6314
6315 struct BatchRayCaster
6316 {
6317 b3ThreadPool* m_threadPool;
6318 CastSyncInfo* m_syncInfo;
6319 const btCollisionWorld* m_world;
6320 const b3RayData* m_rayInputBuffer;
6321 b3RayHitInfo* m_hitInfoOutputBuffer;
6322 int m_numRays;
6323 int m_reportHitNumber;
6324 int m_collisionFilterMask;
6325 btScalar m_fractionEpsilon;
6326
BatchRayCasterBatchRayCaster6327 BatchRayCaster(b3ThreadPool* threadPool, const btCollisionWorld* world, const b3RayData* rayInputBuffer, b3RayHitInfo* hitInfoOutputBuffer, int numRays, int reportHitNumber, int collisionFilterMask, btScalar fractionEpsilon)
6328 : m_threadPool(threadPool), m_world(world), m_rayInputBuffer(rayInputBuffer), m_hitInfoOutputBuffer(hitInfoOutputBuffer), m_numRays(numRays), m_reportHitNumber(reportHitNumber), m_collisionFilterMask(collisionFilterMask), m_fractionEpsilon(fractionEpsilon)
6329 {
6330 m_syncInfo = new CastSyncInfo;
6331 }
6332
~BatchRayCasterBatchRayCaster6333 ~BatchRayCaster()
6334 {
6335 delete m_syncInfo;
6336 }
6337
castRaysBatchRayCaster6338 void castRays(int numWorkers)
6339 {
6340 #if BT_THREADSAFE
6341 if (numWorkers <= 1)
6342 {
6343 castSequentially();
6344 }
6345 else
6346 {
6347 {
6348 BT_PROFILE("BatchRayCaster_startingWorkerThreads");
6349 int numTasks = btMin(m_threadPool->numWorkers(), numWorkers - 1);
6350 for (int i = 0; i < numTasks; i++)
6351 {
6352 m_threadPool->runTask(i, BatchRayCaster::rayCastWorker, this);
6353 }
6354 }
6355 rayCastWorker(this);
6356 m_threadPool->waitForAllTasks();
6357 }
6358 #else // BT_THREADSAFE
6359 castSequentially();
6360 #endif // BT_THREADSAFE
6361 }
6362
rayCastWorkerBatchRayCaster6363 static void rayCastWorker(void* arg)
6364 {
6365 BT_PROFILE("BatchRayCaster_raycastWorker");
6366 BatchRayCaster* const obj = (BatchRayCaster*)arg;
6367 const int numRays = obj->m_numRays;
6368 int taskNr;
6369 while (true)
6370 {
6371 {
6372 BT_PROFILE("CastSyncInfo_getNextTask");
6373 taskNr = obj->m_syncInfo->getNextTask();
6374 }
6375 if (taskNr >= numRays)
6376 return;
6377 obj->processRay(taskNr);
6378 }
6379 }
6380
castSequentiallyBatchRayCaster6381 void castSequentially()
6382 {
6383 for (int i = 0; i < m_numRays; i++)
6384 {
6385 processRay(i);
6386 }
6387 }
6388
processRayBatchRayCaster6389 void processRay(int ray)
6390 {
6391 BT_PROFILE("BatchRayCaster_processRay");
6392 const double* from = m_rayInputBuffer[ray].m_rayFromPosition;
6393 const double* to = m_rayInputBuffer[ray].m_rayToPosition;
6394 btVector3 rayFromWorld(from[0], from[1], from[2]);
6395 btVector3 rayToWorld(to[0], to[1], to[2]);
6396
6397 FilteredClosestRayResultCallback rayResultCallback(rayFromWorld, rayToWorld, m_collisionFilterMask);
6398 rayResultCallback.m_flags |= btTriangleRaycastCallback::kF_UseGjkConvexCastRaytest;
6399 if (m_reportHitNumber >= 0)
6400 {
6401 //compute all hits, and select the m_reportHitNumber, if available
6402 FilteredAllHitsRayResultCallback allResultsCallback(rayFromWorld, rayToWorld, m_collisionFilterMask, m_fractionEpsilon);
6403 allResultsCallback.m_flags |= btTriangleRaycastCallback::kF_UseGjkConvexCastRaytest;
6404 m_world->rayTest(rayFromWorld, rayToWorld, allResultsCallback);
6405 if (allResultsCallback.m_collisionObjects.size() > m_reportHitNumber)
6406 {
6407 rayResultCallback.m_collisionObject = allResultsCallback.m_collisionObjects[m_reportHitNumber];
6408 rayResultCallback.m_closestHitFraction = allResultsCallback.m_hitFractions[m_reportHitNumber];
6409 rayResultCallback.m_hitNormalWorld = allResultsCallback.m_hitNormalWorld[m_reportHitNumber];
6410 rayResultCallback.m_hitPointWorld = allResultsCallback.m_hitPointWorld[m_reportHitNumber];
6411 }
6412 }
6413 else
6414 {
6415 m_world->rayTest(rayFromWorld, rayToWorld, rayResultCallback);
6416 }
6417
6418 b3RayHitInfo& hit = m_hitInfoOutputBuffer[ray];
6419 if (rayResultCallback.hasHit())
6420 {
6421 hit.m_hitFraction = rayResultCallback.m_closestHitFraction;
6422
6423 int objectUniqueId = -1;
6424 int linkIndex = -1;
6425
6426 const btRigidBody* body = btRigidBody::upcast(rayResultCallback.m_collisionObject);
6427 #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
6428 const btSoftBody* softBody = btSoftBody::upcast(rayResultCallback.m_collisionObject);
6429 if (softBody)
6430 {
6431 objectUniqueId = rayResultCallback.m_collisionObject->getUserIndex2();
6432 }
6433 #endif //SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
6434 if (body)
6435 {
6436 objectUniqueId = rayResultCallback.m_collisionObject->getUserIndex2();
6437 }
6438 else
6439 {
6440 const btMultiBodyLinkCollider* mblB = btMultiBodyLinkCollider::upcast(rayResultCallback.m_collisionObject);
6441 if (mblB && mblB->m_multiBody)
6442 {
6443 linkIndex = mblB->m_link;
6444 objectUniqueId = mblB->m_multiBody->getUserIndex2();
6445 }
6446 }
6447
6448 hit.m_hitObjectUniqueId = objectUniqueId;
6449 hit.m_hitObjectLinkIndex = linkIndex;
6450
6451 hit.m_hitPositionWorld[0] = rayResultCallback.m_hitPointWorld[0];
6452 hit.m_hitPositionWorld[1] = rayResultCallback.m_hitPointWorld[1];
6453 hit.m_hitPositionWorld[2] = rayResultCallback.m_hitPointWorld[2];
6454 hit.m_hitNormalWorld[0] = rayResultCallback.m_hitNormalWorld[0];
6455 hit.m_hitNormalWorld[1] = rayResultCallback.m_hitNormalWorld[1];
6456 hit.m_hitNormalWorld[2] = rayResultCallback.m_hitNormalWorld[2];
6457 }
6458 else
6459 {
6460 hit.m_hitFraction = 1;
6461 hit.m_hitObjectUniqueId = -1;
6462 hit.m_hitObjectLinkIndex = -1;
6463 hit.m_hitPositionWorld[0] = 0;
6464 hit.m_hitPositionWorld[1] = 0;
6465 hit.m_hitPositionWorld[2] = 0;
6466 hit.m_hitNormalWorld[0] = 0;
6467 hit.m_hitNormalWorld[1] = 0;
6468 hit.m_hitNormalWorld[2] = 0;
6469 }
6470 }
6471 };
6472
createThreadPool()6473 void PhysicsServerCommandProcessor::createThreadPool()
6474 {
6475 #ifdef BT_THREADSAFE
6476 if (m_data->m_threadPool == 0)
6477 {
6478 m_data->m_threadPool = new b3ThreadPool("PhysicsServerCommandProcessorThreadPool");
6479 }
6480 #endif //BT_THREADSAFE
6481 }
6482
processRequestRaycastIntersectionsCommand(const struct SharedMemoryCommand & clientCmd,struct SharedMemoryStatus & serverStatusOut,char * bufferServerToClient,int bufferSizeInBytes)6483 bool PhysicsServerCommandProcessor::processRequestRaycastIntersectionsCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
6484 {
6485 bool hasStatus = true;
6486 BT_PROFILE("CMD_REQUEST_RAY_CAST_INTERSECTIONS");
6487 serverStatusOut.m_raycastHits.m_numRaycastHits = 0;
6488
6489 const int numCommandRays = clientCmd.m_requestRaycastIntersections.m_numCommandRays;
6490 const int numStreamingRays = clientCmd.m_requestRaycastIntersections.m_numStreamingRays;
6491 const int totalRays = numCommandRays + numStreamingRays;
6492 int numThreads = clientCmd.m_requestRaycastIntersections.m_numThreads;
6493 int reportHitNumber = clientCmd.m_requestRaycastIntersections.m_reportHitNumber;
6494 int collisionFilterMask = clientCmd.m_requestRaycastIntersections.m_collisionFilterMask;
6495 btScalar fractionEpsilon = clientCmd.m_requestRaycastIntersections.m_fractionEpsilon;
6496 if (numThreads == 0)
6497 {
6498 // When 0 is specified, Bullet can decide how many threads to use.
6499 // About 16 rays per thread seems to work reasonably well.
6500 numThreads = btMax(1, totalRays / 16);
6501 }
6502 if (numThreads > 1)
6503 {
6504 createThreadPool();
6505 }
6506
6507 btAlignedObjectArray<b3RayData> rays;
6508 rays.resize(totalRays);
6509 if (numCommandRays)
6510 {
6511 memcpy(&rays[0], &clientCmd.m_requestRaycastIntersections.m_fromToRays[0], numCommandRays * sizeof(b3RayData));
6512 }
6513 if (numStreamingRays)
6514 {
6515 memcpy(&rays[numCommandRays], bufferServerToClient, numStreamingRays * sizeof(b3RayData));
6516 }
6517
6518 if (clientCmd.m_requestRaycastIntersections.m_parentObjectUniqueId >= 0)
6519 {
6520 btTransform tr;
6521 tr.setIdentity();
6522
6523 InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(clientCmd.m_requestRaycastIntersections.m_parentObjectUniqueId);
6524 if (bodyHandle)
6525 {
6526 int linkIndex = -1;
6527 if (bodyHandle->m_multiBody)
6528 {
6529 int linkIndex = clientCmd.m_requestRaycastIntersections.m_parentLinkIndex;
6530 if (linkIndex == -1)
6531 {
6532 tr = bodyHandle->m_multiBody->getBaseWorldTransform();
6533 }
6534 else
6535 {
6536 if (linkIndex >= 0 && linkIndex < bodyHandle->m_multiBody->getNumLinks())
6537 {
6538 tr = bodyHandle->m_multiBody->getLink(linkIndex).m_cachedWorldTransform;
6539 }
6540 }
6541 }
6542 if (bodyHandle->m_rigidBody)
6543 {
6544 tr = bodyHandle->m_rigidBody->getWorldTransform();
6545 }
6546 //convert all rays into world space
6547 for (int i = 0; i < totalRays; i++)
6548 {
6549 btVector3 localPosTo(rays[i].m_rayToPosition[0], rays[i].m_rayToPosition[1], rays[i].m_rayToPosition[2]);
6550 btVector3 worldPosTo = tr * localPosTo;
6551
6552 btVector3 localPosFrom(rays[i].m_rayFromPosition[0], rays[i].m_rayFromPosition[1], rays[i].m_rayFromPosition[2]);
6553 btVector3 worldPosFrom = tr * localPosFrom;
6554 rays[i].m_rayFromPosition[0] = worldPosFrom[0];
6555 rays[i].m_rayFromPosition[1] = worldPosFrom[1];
6556 rays[i].m_rayFromPosition[2] = worldPosFrom[2];
6557 rays[i].m_rayToPosition[0] = worldPosTo[0];
6558 rays[i].m_rayToPosition[1] = worldPosTo[1];
6559 rays[i].m_rayToPosition[2] = worldPosTo[2];
6560 }
6561 }
6562 }
6563
6564 BatchRayCaster batchRayCaster(m_data->m_threadPool, m_data->m_dynamicsWorld, &rays[0], (b3RayHitInfo*)bufferServerToClient, totalRays, reportHitNumber, collisionFilterMask, fractionEpsilon);
6565 batchRayCaster.castRays(numThreads);
6566
6567 serverStatusOut.m_numDataStreamBytes = totalRays * sizeof(b3RayData);
6568 serverStatusOut.m_raycastHits.m_numRaycastHits = totalRays;
6569 serverStatusOut.m_type = CMD_REQUEST_RAY_CAST_INTERSECTIONS_COMPLETED;
6570 return hasStatus;
6571 }
6572
processRequestDebugLinesCommand(const struct SharedMemoryCommand & clientCmd,struct SharedMemoryStatus & serverStatusOut,char * bufferServerToClient,int bufferSizeInBytes)6573 bool PhysicsServerCommandProcessor::processRequestDebugLinesCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
6574 {
6575 bool hasStatus = true;
6576 BT_PROFILE("CMD_REQUEST_DEBUG_LINES");
6577 int curFlags = m_data->m_remoteDebugDrawer->getDebugMode();
6578
6579 int debugMode = clientCmd.m_requestDebugLinesArguments.m_debugMode; //clientCmd.btIDebugDraw::DBG_DrawWireframe|btIDebugDraw::DBG_DrawAabb;
6580 int startingLineIndex = clientCmd.m_requestDebugLinesArguments.m_startingLineIndex;
6581 if (startingLineIndex < 0)
6582 {
6583 b3Warning("startingLineIndex should be non-negative");
6584 startingLineIndex = 0;
6585 }
6586
6587 if (clientCmd.m_requestDebugLinesArguments.m_startingLineIndex == 0)
6588 {
6589 m_data->m_remoteDebugDrawer->m_lines2.resize(0);
6590 //|btIDebugDraw::DBG_DrawAabb|
6591 // btIDebugDraw::DBG_DrawConstraints |btIDebugDraw::DBG_DrawConstraintLimits ;
6592 m_data->m_remoteDebugDrawer->setDebugMode(debugMode);
6593 btIDebugDraw* oldDebugDrawer = m_data->m_dynamicsWorld->getDebugDrawer();
6594 m_data->m_dynamicsWorld->setDebugDrawer(m_data->m_remoteDebugDrawer);
6595 m_data->m_dynamicsWorld->debugDrawWorld();
6596 m_data->m_dynamicsWorld->setDebugDrawer(oldDebugDrawer);
6597 m_data->m_remoteDebugDrawer->setDebugMode(curFlags);
6598 }
6599
6600 //9 floats per line: 3 floats for 'from', 3 floats for 'to' and 3 floats for 'color'
6601 int bytesPerLine = (sizeof(float) * 9);
6602 int maxNumLines = bufferSizeInBytes / bytesPerLine - 1;
6603 if (startingLineIndex > m_data->m_remoteDebugDrawer->m_lines2.size())
6604 {
6605 b3Warning("m_startingLineIndex exceeds total number of debug lines");
6606 startingLineIndex = m_data->m_remoteDebugDrawer->m_lines2.size();
6607 }
6608
6609 int numLines = btMin(maxNumLines, m_data->m_remoteDebugDrawer->m_lines2.size() - startingLineIndex);
6610
6611 if (numLines)
6612 {
6613 float* linesFrom = (float*)bufferServerToClient;
6614 float* linesTo = (float*)(bufferServerToClient + numLines * 3 * sizeof(float));
6615 float* linesColor = (float*)(bufferServerToClient + 2 * numLines * 3 * sizeof(float));
6616
6617 for (int i = 0; i < numLines; i++)
6618 {
6619 linesFrom[i * 3] = m_data->m_remoteDebugDrawer->m_lines2[i + startingLineIndex].m_from.x();
6620 linesTo[i * 3] = m_data->m_remoteDebugDrawer->m_lines2[i + startingLineIndex].m_to.x();
6621 linesColor[i * 3] = m_data->m_remoteDebugDrawer->m_lines2[i + startingLineIndex].m_color.x();
6622
6623 linesFrom[i * 3 + 1] = m_data->m_remoteDebugDrawer->m_lines2[i + startingLineIndex].m_from.y();
6624 linesTo[i * 3 + 1] = m_data->m_remoteDebugDrawer->m_lines2[i + startingLineIndex].m_to.y();
6625 linesColor[i * 3 + 1] = m_data->m_remoteDebugDrawer->m_lines2[i + startingLineIndex].m_color.y();
6626
6627 linesFrom[i * 3 + 2] = m_data->m_remoteDebugDrawer->m_lines2[i + startingLineIndex].m_from.z();
6628 linesTo[i * 3 + 2] = m_data->m_remoteDebugDrawer->m_lines2[i + startingLineIndex].m_to.z();
6629 linesColor[i * 3 + 2] = m_data->m_remoteDebugDrawer->m_lines2[i + startingLineIndex].m_color.z();
6630 }
6631 }
6632
6633 serverStatusOut.m_type = CMD_DEBUG_LINES_COMPLETED;
6634 serverStatusOut.m_numDataStreamBytes = numLines * bytesPerLine;
6635 serverStatusOut.m_sendDebugLinesArgs.m_numDebugLines = numLines;
6636 serverStatusOut.m_sendDebugLinesArgs.m_startingLineIndex = startingLineIndex;
6637 serverStatusOut.m_sendDebugLinesArgs.m_numRemainingDebugLines = m_data->m_remoteDebugDrawer->m_lines2.size() - (startingLineIndex + numLines);
6638
6639 return hasStatus;
6640 }
6641
processSyncBodyInfoCommand(const struct SharedMemoryCommand & clientCmd,struct SharedMemoryStatus & serverStatusOut,char * bufferServerToClient,int bufferSizeInBytes)6642 bool PhysicsServerCommandProcessor::processSyncBodyInfoCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
6643 {
6644 bool hasStatus = true;
6645 BT_PROFILE("CMD_SYNC_BODY_INFO");
6646
6647 b3AlignedObjectArray<int> usedHandles;
6648 m_data->m_bodyHandles.getUsedHandles(usedHandles);
6649 int actualNumBodies = 0;
6650 int* bodyUids = (int*)bufferServerToClient;
6651
6652 for (int i = 0; i < usedHandles.size(); i++)
6653 {
6654 int usedHandle = usedHandles[i];
6655 InternalBodyData* body = m_data->m_bodyHandles.getHandle(usedHandle);
6656 #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
6657 if (body && (body->m_multiBody || body->m_rigidBody || body->m_softBody))
6658 #else
6659 if (body && (body->m_multiBody || body->m_rigidBody))
6660 #endif
6661 {
6662 bodyUids[actualNumBodies++] = usedHandle;
6663 }
6664 }
6665 serverStatusOut.m_sdfLoadedArgs.m_numBodies = actualNumBodies;
6666
6667 int usz = m_data->m_userConstraints.size();
6668 int* constraintUid = bodyUids + actualNumBodies;
6669 serverStatusOut.m_sdfLoadedArgs.m_numUserConstraints = usz;
6670
6671 for (int i = 0; i < usz; i++)
6672 {
6673 int key = m_data->m_userConstraints.getKeyAtIndex(i).getUid1();
6674 constraintUid[i] = key;
6675 }
6676
6677 serverStatusOut.m_numDataStreamBytes = sizeof(int) * (actualNumBodies + usz);
6678
6679 serverStatusOut.m_type = CMD_SYNC_BODY_INFO_COMPLETED;
6680 return hasStatus;
6681 }
6682
processSyncUserDataCommand(const struct SharedMemoryCommand & clientCmd,struct SharedMemoryStatus & serverStatusOut,char * bufferServerToClient,int bufferSizeInBytes)6683 bool PhysicsServerCommandProcessor::processSyncUserDataCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
6684 {
6685 bool hasStatus = true;
6686 BT_PROFILE("CMD_SYNC_USER_DATA");
6687
6688 b3AlignedObjectArray<int> userDataHandles;
6689 if (clientCmd.m_syncUserDataRequestArgs.m_numRequestedBodies == 0)
6690 {
6691 m_data->m_userDataHandles.getUsedHandles(userDataHandles);
6692 }
6693 else
6694 {
6695 for (int i = 0; i < clientCmd.m_syncUserDataRequestArgs.m_numRequestedBodies; ++i)
6696 {
6697 const int bodyUniqueId = clientCmd.m_syncUserDataRequestArgs.m_requestedBodyIds[i];
6698 InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId);
6699 if (!body)
6700 {
6701 return hasStatus;
6702 }
6703 for (int j = 0; j < body->m_userDataHandles.size(); ++j)
6704 {
6705 userDataHandles.push_back(body->m_userDataHandles[j]);
6706 }
6707 }
6708 }
6709 int sizeInBytes = sizeof(int) * userDataHandles.size();
6710 if (userDataHandles.size())
6711 {
6712 memcpy(bufferServerToClient, &userDataHandles[0], sizeInBytes);
6713 }
6714 // Only clear the client-side cache when a full sync is requested
6715 serverStatusOut.m_syncUserDataArgs.m_clearCachedUserDataEntries = clientCmd.m_syncUserDataRequestArgs.m_numRequestedBodies == 0;
6716 serverStatusOut.m_syncUserDataArgs.m_numUserDataIdentifiers = userDataHandles.size();
6717 serverStatusOut.m_numDataStreamBytes = sizeInBytes;
6718 serverStatusOut.m_type = CMD_SYNC_USER_DATA_COMPLETED;
6719
6720 return hasStatus;
6721 }
6722
processRequestUserDataCommand(const struct SharedMemoryCommand & clientCmd,struct SharedMemoryStatus & serverStatusOut,char * bufferServerToClient,int bufferSizeInBytes)6723 bool PhysicsServerCommandProcessor::processRequestUserDataCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
6724 {
6725 bool hasStatus = true;
6726 BT_PROFILE("CMD_REQUEST_USER_DATA");
6727 serverStatusOut.m_type = CMD_REQUEST_USER_DATA_FAILED;
6728
6729 SharedMemoryUserData* userData = m_data->m_userDataHandles.getHandle(clientCmd.m_userDataRequestArgs.m_userDataId);
6730 if (!userData)
6731 {
6732 return hasStatus;
6733 }
6734
6735 btAssert(bufferSizeInBytes >= userData->m_bytes.size());
6736 serverStatusOut.m_userDataResponseArgs.m_userDataId = clientCmd.m_userDataRequestArgs.m_userDataId;
6737 serverStatusOut.m_userDataResponseArgs.m_bodyUniqueId = userData->m_bodyUniqueId;
6738 serverStatusOut.m_userDataResponseArgs.m_linkIndex = userData->m_linkIndex;
6739 serverStatusOut.m_userDataResponseArgs.m_visualShapeIndex = userData->m_visualShapeIndex;
6740 serverStatusOut.m_userDataResponseArgs.m_valueType = userData->m_type;
6741 serverStatusOut.m_userDataResponseArgs.m_valueLength = userData->m_bytes.size();
6742 serverStatusOut.m_type = CMD_REQUEST_USER_DATA_COMPLETED;
6743
6744 strcpy(serverStatusOut.m_userDataResponseArgs.m_key, userData->m_key.c_str());
6745 if (userData->m_bytes.size())
6746 {
6747 memcpy(bufferServerToClient, &userData->m_bytes[0], userData->m_bytes.size());
6748 }
6749 serverStatusOut.m_numDataStreamBytes = userData->m_bytes.size();
6750 return hasStatus;
6751 }
6752
processAddUserDataCommand(const struct SharedMemoryCommand & clientCmd,struct SharedMemoryStatus & serverStatusOut,char * bufferServerToClient,int bufferSizeInBytes)6753 bool PhysicsServerCommandProcessor::processAddUserDataCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
6754 {
6755 bool hasStatus = true;
6756 BT_PROFILE("CMD_ADD_USER_DATA");
6757 serverStatusOut.m_type = CMD_ADD_USER_DATA_FAILED;
6758
6759 const AddUserDataRequestArgs& addUserDataArgs = clientCmd.m_addUserDataRequestArgs;
6760 if (addUserDataArgs.m_bodyUniqueId < 0 || addUserDataArgs.m_bodyUniqueId >= m_data->m_bodyHandles.getNumHandles())
6761 {
6762 return hasStatus;
6763 }
6764 int userDataHandle = addUserData(
6765 addUserDataArgs.m_bodyUniqueId, addUserDataArgs.m_linkIndex,
6766 addUserDataArgs.m_visualShapeIndex, addUserDataArgs.m_key,
6767 bufferServerToClient, addUserDataArgs.m_valueLength,
6768 addUserDataArgs.m_valueType);
6769 if (userDataHandle < 0)
6770 {
6771 return hasStatus;
6772 }
6773
6774 serverStatusOut.m_type = CMD_ADD_USER_DATA_COMPLETED;
6775 UserDataResponseArgs& userDataResponseArgs = serverStatusOut.m_userDataResponseArgs;
6776 userDataResponseArgs.m_userDataId = userDataHandle;
6777 userDataResponseArgs.m_bodyUniqueId = addUserDataArgs.m_bodyUniqueId;
6778 userDataResponseArgs.m_linkIndex = addUserDataArgs.m_linkIndex;
6779 userDataResponseArgs.m_visualShapeIndex = addUserDataArgs.m_visualShapeIndex;
6780 userDataResponseArgs.m_valueLength = addUserDataArgs.m_valueLength;
6781 userDataResponseArgs.m_valueType = addUserDataArgs.m_valueType;
6782 strcpy(userDataResponseArgs.m_key, addUserDataArgs.m_key);
6783
6784 b3Notification notification;
6785 notification.m_notificationType = USER_DATA_ADDED;
6786 b3UserDataNotificationArgs& userDataArgs = notification.m_userDataArgs;
6787 userDataArgs.m_userDataId = userDataHandle;
6788 userDataArgs.m_bodyUniqueId = addUserDataArgs.m_bodyUniqueId;
6789 userDataArgs.m_linkIndex = addUserDataArgs.m_linkIndex;
6790 userDataArgs.m_visualShapeIndex = addUserDataArgs.m_visualShapeIndex;
6791 strcpy(userDataArgs.m_key, addUserDataArgs.m_key);
6792 m_data->m_pluginManager.addNotification(notification);
6793
6794 // Keep bufferServerToClient as-is.
6795 return hasStatus;
6796 }
6797
processCollisionFilterCommand(const struct SharedMemoryCommand & clientCmd,struct SharedMemoryStatus & serverStatusOut,char * bufferServerToClient,int bufferSizeInBytes)6798 bool PhysicsServerCommandProcessor::processCollisionFilterCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
6799 {
6800 serverStatusOut.m_type = CMD_CLIENT_COMMAND_COMPLETED;
6801 b3PluginCollisionInterface* collisionInterface = m_data->m_pluginManager.getCollisionInterface();
6802 if (collisionInterface)
6803 {
6804 if (clientCmd.m_updateFlags & B3_COLLISION_FILTER_PAIR)
6805 {
6806 collisionInterface->setBroadphaseCollisionFilter(clientCmd.m_collisionFilterArgs.m_bodyUniqueIdA,
6807 clientCmd.m_collisionFilterArgs.m_bodyUniqueIdB,
6808 clientCmd.m_collisionFilterArgs.m_linkIndexA,
6809 clientCmd.m_collisionFilterArgs.m_linkIndexB,
6810 clientCmd.m_collisionFilterArgs.m_enableCollision);
6811
6812 btAlignedObjectArray<InternalBodyData*> bodies;
6813
6814 //now also 'refresh' the broadphase collision pairs involved
6815 if (clientCmd.m_collisionFilterArgs.m_bodyUniqueIdA >= 0)
6816 {
6817 bodies.push_back(m_data->m_bodyHandles.getHandle(clientCmd.m_collisionFilterArgs.m_bodyUniqueIdA));
6818 }
6819 if (clientCmd.m_collisionFilterArgs.m_bodyUniqueIdB >= 0)
6820 {
6821 bodies.push_back(m_data->m_bodyHandles.getHandle(clientCmd.m_collisionFilterArgs.m_bodyUniqueIdB));
6822 }
6823 for (int i = 0; i < bodies.size(); i++)
6824 {
6825 InternalBodyData* body = bodies[i];
6826 if (body)
6827 {
6828 if (body->m_multiBody)
6829 {
6830 if (body->m_multiBody->getBaseCollider())
6831 {
6832 m_data->m_dynamicsWorld->refreshBroadphaseProxy(body->m_multiBody->getBaseCollider());
6833 }
6834 for (int i = 0; i < body->m_multiBody->getNumLinks(); i++)
6835 {
6836 if (body->m_multiBody->getLinkCollider(i))
6837 {
6838 m_data->m_dynamicsWorld->refreshBroadphaseProxy(body->m_multiBody->getLinkCollider(i));
6839 }
6840 }
6841 }
6842 else
6843 {
6844 //btRigidBody case
6845 if (body->m_rigidBody)
6846 {
6847 m_data->m_dynamicsWorld->refreshBroadphaseProxy(body->m_rigidBody);
6848 }
6849 }
6850 }
6851 }
6852 }
6853 if (clientCmd.m_updateFlags & B3_COLLISION_FILTER_GROUP_MASK)
6854 {
6855 InternalBodyData* body = m_data->m_bodyHandles.getHandle(clientCmd.m_collisionFilterArgs.m_bodyUniqueIdA);
6856 if (body)
6857 {
6858 btCollisionObject* colObj = 0;
6859 if (body->m_multiBody)
6860 {
6861 if (clientCmd.m_collisionFilterArgs.m_linkIndexA == -1)
6862 {
6863 colObj = body->m_multiBody->getBaseCollider();
6864 }
6865 else
6866 {
6867 if (clientCmd.m_collisionFilterArgs.m_linkIndexA >= 0 && clientCmd.m_collisionFilterArgs.m_linkIndexA < body->m_multiBody->getNumLinks())
6868 {
6869 colObj = body->m_multiBody->getLinkCollider(clientCmd.m_collisionFilterArgs.m_linkIndexA);
6870 }
6871 }
6872 }
6873 else
6874 {
6875 if (body->m_rigidBody)
6876 {
6877 colObj = body->m_rigidBody;
6878 }
6879 }
6880 if (colObj)
6881 {
6882 colObj->getBroadphaseHandle()->m_collisionFilterGroup = clientCmd.m_collisionFilterArgs.m_collisionFilterGroup;
6883 colObj->getBroadphaseHandle()->m_collisionFilterMask = clientCmd.m_collisionFilterArgs.m_collisionFilterMask;
6884 m_data->m_dynamicsWorld->refreshBroadphaseProxy(colObj);
6885 }
6886 }
6887 }
6888 }
6889 return true;
6890 }
6891
processRemoveUserDataCommand(const struct SharedMemoryCommand & clientCmd,struct SharedMemoryStatus & serverStatusOut,char * bufferServerToClient,int bufferSizeInBytes)6892 bool PhysicsServerCommandProcessor::processRemoveUserDataCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
6893 {
6894 bool hasStatus = true;
6895 BT_PROFILE("CMD_REMOVE_USER_DATA");
6896 serverStatusOut.m_type = CMD_REMOVE_USER_DATA_FAILED;
6897
6898 SharedMemoryUserData* userData = m_data->m_userDataHandles.getHandle(clientCmd.m_removeUserDataRequestArgs.m_userDataId);
6899 if (!userData)
6900 {
6901 return hasStatus;
6902 }
6903
6904 InternalBodyData* body = m_data->m_bodyHandles.getHandle(userData->m_bodyUniqueId);
6905 if (!body)
6906 {
6907 return hasStatus;
6908 }
6909 body->m_userDataHandles.remove(clientCmd.m_removeUserDataRequestArgs.m_userDataId);
6910
6911 b3Notification notification;
6912 notification.m_notificationType = USER_DATA_REMOVED;
6913 b3UserDataNotificationArgs& userDataArgs = notification.m_userDataArgs;
6914 userDataArgs.m_userDataId = clientCmd.m_removeUserDataRequestArgs.m_userDataId;
6915 userDataArgs.m_bodyUniqueId = userData->m_bodyUniqueId;
6916 userDataArgs.m_linkIndex = userData->m_linkIndex;
6917 userDataArgs.m_visualShapeIndex = userData->m_visualShapeIndex;
6918 strcpy(userDataArgs.m_key, userData->m_key.c_str());
6919
6920 m_data->m_userDataHandleLookup.remove(SharedMemoryUserDataHashKey(userData));
6921 m_data->m_userDataHandles.freeHandle(clientCmd.m_removeUserDataRequestArgs.m_userDataId);
6922
6923 serverStatusOut.m_removeUserDataResponseArgs = clientCmd.m_removeUserDataRequestArgs;
6924 serverStatusOut.m_type = CMD_REMOVE_USER_DATA_COMPLETED;
6925
6926 m_data->m_pluginManager.addNotification(notification);
6927 return hasStatus;
6928 }
6929
processSendDesiredStateCommand(const struct SharedMemoryCommand & clientCmd,struct SharedMemoryStatus & serverStatusOut,char * bufferServerToClient,int bufferSizeInBytes)6930 bool PhysicsServerCommandProcessor::processSendDesiredStateCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
6931 {
6932 bool hasStatus = true;
6933 BT_PROFILE("CMD_SEND_DESIRED_STATE");
6934 if (m_data->m_verboseOutput)
6935 {
6936 b3Printf("Processed CMD_SEND_DESIRED_STATE");
6937 }
6938
6939 int bodyUniqueId = clientCmd.m_sendDesiredStateCommandArgument.m_bodyUniqueId;
6940 InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId);
6941
6942 if (body && body->m_multiBody)
6943 {
6944 btMultiBody* mb = body->m_multiBody;
6945 btAssert(mb);
6946
6947 switch (clientCmd.m_sendDesiredStateCommandArgument.m_controlMode)
6948 {
6949 case CONTROL_MODE_PD:
6950 {
6951 if (m_data->m_verboseOutput)
6952 {
6953 b3Printf("Using CONTROL_MODE_PD");
6954 }
6955
6956 b3PluginArguments args;
6957 args.m_ints[1] = bodyUniqueId;
6958 //find the joint motors and apply the desired velocity and maximum force/torque
6959 {
6960 args.m_numInts = 0;
6961 args.m_numFloats = 0;
6962 //syncBodies is expensive/slow, use it only once
6963 m_data->m_pluginManager.executePluginCommand(m_data->m_pdControlPlugin, &args);
6964
6965 int velIndex = 6; //skip the 3 linear + 3 angular degree of freedom velocity entries of the base
6966 int posIndex = 7; //skip 3 positional and 4 orientation (quaternion) positional degrees of freedom of the base
6967 for (int link = 0; link < mb->getNumLinks(); link++)
6968 {
6969 if (supportsJointMotor(mb, link))
6970 {
6971 bool hasDesiredPosOrVel = false;
6972 btScalar desiredVelocity = 0.f;
6973 if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex] & SIM_DESIRED_STATE_HAS_QDOT) != 0)
6974 {
6975 hasDesiredPosOrVel = true;
6976 desiredVelocity = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[velIndex];
6977 args.m_floats[2] = 0.1; // kd
6978 }
6979 btScalar desiredPosition = 0.f;
6980 if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[posIndex] & SIM_DESIRED_STATE_HAS_Q) != 0)
6981 {
6982 hasDesiredPosOrVel = true;
6983 desiredPosition = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[posIndex];
6984 args.m_floats[3] = 0.1; // kp
6985 }
6986
6987 if (hasDesiredPosOrVel)
6988 {
6989 if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex] & SIM_DESIRED_STATE_HAS_KP) != 0)
6990 {
6991 args.m_floats[3] = clientCmd.m_sendDesiredStateCommandArgument.m_Kp[velIndex];
6992 }
6993 if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex] & SIM_DESIRED_STATE_HAS_KD) != 0)
6994 {
6995 args.m_floats[2] = clientCmd.m_sendDesiredStateCommandArgument.m_Kd[velIndex];
6996 }
6997
6998 args.m_floats[1] = desiredVelocity;
6999 //clamp position
7000 if (mb->getLink(link).m_jointLowerLimit <= mb->getLink(link).m_jointUpperLimit)
7001 {
7002 btClamp(desiredPosition, mb->getLink(link).m_jointLowerLimit, mb->getLink(link).m_jointUpperLimit);
7003 }
7004 args.m_floats[0] = desiredPosition;
7005
7006 btScalar maxImp = 1000000.f;
7007 if ((clientCmd.m_updateFlags & SIM_DESIRED_STATE_HAS_MAX_FORCE) != 0)
7008 maxImp = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[velIndex];
7009 args.m_floats[4] = maxImp;
7010
7011 args.m_ints[2] = link;
7012 args.m_numInts = 3;
7013 args.m_numFloats = 5;
7014
7015 args.m_ints[0] = eSetPDControl;
7016 if (args.m_floats[4] < B3_EPSILON)
7017 {
7018 args.m_ints[0] = eRemovePDControl;
7019 }
7020 m_data->m_pluginManager.executePluginCommand(m_data->m_pdControlPlugin, &args);
7021 }
7022 }
7023 velIndex += mb->getLink(link).m_dofCount;
7024 posIndex += mb->getLink(link).m_posVarCount;
7025 }
7026 }
7027 break;
7028 }
7029 case CONTROL_MODE_TORQUE:
7030 {
7031 if (m_data->m_verboseOutput)
7032 {
7033 b3Printf("Using CONTROL_MODE_TORQUE");
7034 }
7035 // mb->clearForcesAndTorques();
7036 int torqueIndex = 6;
7037 if ((clientCmd.m_updateFlags & SIM_DESIRED_STATE_HAS_MAX_FORCE) != 0)
7038 {
7039 for (int link = 0; link < mb->getNumLinks(); link++)
7040 {
7041 for (int dof = 0; dof < mb->getLink(link).m_dofCount; dof++)
7042 {
7043 double torque = 0.f;
7044 if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[torqueIndex] & SIM_DESIRED_STATE_HAS_MAX_FORCE) != 0)
7045 {
7046 torque = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[torqueIndex];
7047 mb->addJointTorqueMultiDof(link, dof, torque);
7048 }
7049 torqueIndex++;
7050 }
7051 }
7052 }
7053 break;
7054 }
7055 case CONTROL_MODE_VELOCITY:
7056 {
7057 if (m_data->m_verboseOutput)
7058 {
7059 b3Printf("Using CONTROL_MODE_VELOCITY");
7060 }
7061
7062 int numMotors = 0;
7063 //find the joint motors and apply the desired velocity and maximum force/torque
7064 {
7065 int dofIndex = 6; //skip the 3 linear + 3 angular degree of freedom entries of the base
7066 for (int link = 0; link < mb->getNumLinks(); link++)
7067 {
7068 if (supportsJointMotor(mb, link))
7069 {
7070 btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)mb->getLink(link).m_userPtr;
7071
7072 if (motor)
7073 {
7074 btScalar desiredVelocity = 0.f;
7075 bool hasDesiredVelocity = false;
7076
7077 if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] & SIM_DESIRED_STATE_HAS_QDOT) != 0)
7078 {
7079 desiredVelocity = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[dofIndex];
7080 btScalar kd = 0.1f;
7081 if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] & SIM_DESIRED_STATE_HAS_KD) != 0)
7082 {
7083 kd = clientCmd.m_sendDesiredStateCommandArgument.m_Kd[dofIndex];
7084 }
7085
7086 motor->setVelocityTarget(desiredVelocity, kd);
7087
7088 btScalar kp = 0.f;
7089 motor->setPositionTarget(0, kp);
7090 hasDesiredVelocity = true;
7091 }
7092 if (hasDesiredVelocity)
7093 {
7094 //disable velocity clamp in velocity mode
7095 motor->setRhsClamp(SIMD_INFINITY);
7096
7097 btScalar maxImp = 1000000.f * m_data->m_physicsDeltaTime;
7098 if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] & SIM_DESIRED_STATE_HAS_MAX_FORCE) != 0)
7099 {
7100 maxImp = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex] * m_data->m_physicsDeltaTime;
7101 }
7102 motor->setMaxAppliedImpulse(maxImp);
7103 }
7104 numMotors++;
7105 }
7106 }
7107 dofIndex += mb->getLink(link).m_dofCount;
7108 }
7109 }
7110 break;
7111 }
7112
7113 case CONTROL_MODE_POSITION_VELOCITY_PD:
7114 {
7115 if (m_data->m_verboseOutput)
7116 {
7117 b3Printf("Using CONTROL_MODE_POSITION_VELOCITY_PD");
7118 }
7119 //compute the force base on PD control
7120
7121 int numMotors = 0;
7122 //find the joint motors and apply the desired velocity and maximum force/torque
7123 {
7124 int velIndex = 6; //skip the 3 linear + 3 angular degree of freedom velocity entries of the base
7125 int posIndex = 7; //skip 3 positional and 4 orientation (quaternion) positional degrees of freedom of the base
7126 for (int link = 0; link < mb->getNumLinks(); link++)
7127 {
7128 if (supportsJointMotor(mb, link))
7129 {
7130 btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)mb->getLink(link).m_userPtr;
7131
7132 if (motor)
7133 {
7134 if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex] & SIM_DESIRED_STATE_HAS_RHS_CLAMP) != 0)
7135 {
7136 motor->setRhsClamp(clientCmd.m_sendDesiredStateCommandArgument.m_rhsClamp[velIndex]);
7137 }
7138
7139 bool hasDesiredPosOrVel = false;
7140 btScalar kp = 0.f;
7141 btScalar kd = 0.f;
7142 btScalar desiredVelocity = 0.f;
7143 if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex] & SIM_DESIRED_STATE_HAS_QDOT) != 0)
7144 {
7145 hasDesiredPosOrVel = true;
7146 desiredVelocity = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[velIndex];
7147 kd = 0.1;
7148 }
7149 btScalar desiredPosition = 0.f;
7150 if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[posIndex] & SIM_DESIRED_STATE_HAS_Q) != 0)
7151 {
7152 hasDesiredPosOrVel = true;
7153 desiredPosition = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[posIndex];
7154 kp = 0.1;
7155 }
7156
7157 if (hasDesiredPosOrVel)
7158 {
7159 if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex] & SIM_DESIRED_STATE_HAS_KP) != 0)
7160 {
7161 kp = clientCmd.m_sendDesiredStateCommandArgument.m_Kp[velIndex];
7162 }
7163
7164 if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex] & SIM_DESIRED_STATE_HAS_KD) != 0)
7165 {
7166 kd = clientCmd.m_sendDesiredStateCommandArgument.m_Kd[velIndex];
7167 }
7168
7169 motor->setVelocityTarget(desiredVelocity, kd);
7170 //todo: instead of clamping, combine the motor and limit
7171 //and combine handling of limit force and motor force.
7172
7173 //clamp position
7174 if (mb->getLink(link).m_jointLowerLimit <= mb->getLink(link).m_jointUpperLimit)
7175 {
7176 btClamp(desiredPosition, mb->getLink(link).m_jointLowerLimit, mb->getLink(link).m_jointUpperLimit);
7177 }
7178 motor->setPositionTarget(desiredPosition, kp);
7179
7180 btScalar maxImp = 1000000.f * m_data->m_physicsDeltaTime;
7181
7182 if ((clientCmd.m_updateFlags & SIM_DESIRED_STATE_HAS_MAX_FORCE) != 0)
7183 maxImp = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[velIndex] * m_data->m_physicsDeltaTime;
7184
7185 motor->setMaxAppliedImpulse(maxImp);
7186 }
7187 numMotors++;
7188 }
7189 }
7190
7191 if (mb->getLink(link).m_jointType == btMultibodyLink::eSpherical)
7192 {
7193 btMultiBodySphericalJointMotor* motor = (btMultiBodySphericalJointMotor*)mb->getLink(link).m_userPtr;
7194 if (motor)
7195 {
7196 if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex] & SIM_DESIRED_STATE_HAS_RHS_CLAMP) != 0)
7197 {
7198 motor->setRhsClamp(clientCmd.m_sendDesiredStateCommandArgument.m_rhsClamp[velIndex]);
7199 }
7200 bool hasDesiredPosOrVel = false;
7201 btVector3 kp(0, 0, 0);
7202 btVector3 kd(0, 0, 0);
7203 btVector3 desiredVelocity(0, 0, 0);
7204 if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex] & SIM_DESIRED_STATE_HAS_QDOT) != 0)
7205 {
7206 hasDesiredPosOrVel = true;
7207 desiredVelocity.setValue(
7208 clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[velIndex + 0],
7209 clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[velIndex + 1],
7210 clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[velIndex + 2]);
7211 kd.setValue(0.1, 0.1, 0.1);
7212 }
7213 btQuaternion desiredPosition(0, 0, 0, 1);
7214 if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[posIndex] & SIM_DESIRED_STATE_HAS_Q) != 0)
7215 {
7216 hasDesiredPosOrVel = true;
7217 desiredPosition.setValue(
7218 clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[posIndex + 0],
7219 clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[posIndex + 1],
7220 clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[posIndex + 2],
7221 clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[posIndex + 3]);
7222 kp.setValue(0.1, 0.1, 0.1);
7223 }
7224
7225 if (hasDesiredPosOrVel)
7226 {
7227 bool useMultiDof = true;
7228
7229 if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex] & SIM_DESIRED_STATE_HAS_KP) != 0)
7230 {
7231 kp.setValue(
7232 clientCmd.m_sendDesiredStateCommandArgument.m_Kp[velIndex + 0],
7233 clientCmd.m_sendDesiredStateCommandArgument.m_Kp[velIndex + 0],
7234 clientCmd.m_sendDesiredStateCommandArgument.m_Kp[velIndex + 0]);
7235 }
7236 if (((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex+0] & SIM_DESIRED_STATE_HAS_KP) != 0) &&
7237 ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex+1] & SIM_DESIRED_STATE_HAS_KP) != 0) &&
7238 ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex+2] & SIM_DESIRED_STATE_HAS_KP) != 0)
7239 )
7240 {
7241 kp.setValue(
7242 clientCmd.m_sendDesiredStateCommandArgument.m_Kp[velIndex + 0],
7243 clientCmd.m_sendDesiredStateCommandArgument.m_Kp[velIndex + 1],
7244 clientCmd.m_sendDesiredStateCommandArgument.m_Kp[velIndex + 2]);
7245 } else
7246 {
7247 useMultiDof = false;
7248 }
7249
7250 if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex] & SIM_DESIRED_STATE_HAS_KD) != 0)
7251 {
7252 kd.setValue(
7253 clientCmd.m_sendDesiredStateCommandArgument.m_Kd[velIndex + 0],
7254 clientCmd.m_sendDesiredStateCommandArgument.m_Kd[velIndex + 0],
7255 clientCmd.m_sendDesiredStateCommandArgument.m_Kd[velIndex + 0]);
7256 }
7257
7258 if (((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex+0] & SIM_DESIRED_STATE_HAS_KD) != 0) &&
7259 ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex+1] & SIM_DESIRED_STATE_HAS_KD) != 0) &&
7260 ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex+2] & SIM_DESIRED_STATE_HAS_KD) != 0))
7261 {
7262 kd.setValue(
7263 clientCmd.m_sendDesiredStateCommandArgument.m_Kd[velIndex + 0],
7264 clientCmd.m_sendDesiredStateCommandArgument.m_Kd[velIndex + 1],
7265 clientCmd.m_sendDesiredStateCommandArgument.m_Kd[velIndex + 2]);
7266 } else
7267 {
7268 useMultiDof = false;
7269 }
7270
7271 btVector3 maxImp(
7272 1000000.f * m_data->m_physicsDeltaTime,
7273 1000000.f * m_data->m_physicsDeltaTime,
7274 1000000.f * m_data->m_physicsDeltaTime);
7275
7276 if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex] & SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0)
7277 {
7278 maxImp.setValue(
7279 clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[velIndex + 0] * m_data->m_physicsDeltaTime,
7280 clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[velIndex + 0] * m_data->m_physicsDeltaTime,
7281 clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[velIndex + 0] * m_data->m_physicsDeltaTime);
7282 }
7283
7284 if (((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex+0] & SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0) &&
7285 ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex+1] & SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0) &&
7286 ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex+2] & SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0))
7287 {
7288 maxImp.setValue(
7289 clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[velIndex + 0] * m_data->m_physicsDeltaTime,
7290 clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[velIndex + 1] * m_data->m_physicsDeltaTime,
7291 clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[velIndex + 2] * m_data->m_physicsDeltaTime);
7292 } else
7293 {
7294 useMultiDof = false;
7295 }
7296
7297 if (useMultiDof)
7298 {
7299 motor->setVelocityTargetMultiDof(desiredVelocity, kd);
7300 motor->setPositionTargetMultiDof(desiredPosition, kp);
7301 motor->setMaxAppliedImpulseMultiDof(maxImp);
7302 } else
7303 {
7304 motor->setVelocityTarget(desiredVelocity, kd[0]);
7305 //todo: instead of clamping, combine the motor and limit
7306 //and combine handling of limit force and motor force.
7307
7308 //clamp position
7309 //if (mb->getLink(link).m_jointLowerLimit <= mb->getLink(link).m_jointUpperLimit)
7310 //{
7311 // btClamp(desiredPosition, mb->getLink(link).m_jointLowerLimit, mb->getLink(link).m_jointUpperLimit);
7312 //}
7313 motor->setPositionTarget(desiredPosition, kp[0]);
7314 motor->setMaxAppliedImpulse(maxImp[0]);
7315 }
7316
7317 btVector3 damping(1.f, 1.f, 1.f);
7318 if ((clientCmd.m_updateFlags & SIM_DESIRED_STATE_HAS_DAMPING) != 0) {
7319 if (
7320 (clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex+0] & SIM_DESIRED_STATE_HAS_DAMPING)&&
7321 (clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex+1] & SIM_DESIRED_STATE_HAS_DAMPING)&&
7322 (clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex+2] & SIM_DESIRED_STATE_HAS_DAMPING)
7323 )
7324 {
7325 damping.setValue(
7326 clientCmd.m_sendDesiredStateCommandArgument.m_damping[velIndex + 0],
7327 clientCmd.m_sendDesiredStateCommandArgument.m_damping[velIndex + 1],
7328 clientCmd.m_sendDesiredStateCommandArgument.m_damping[velIndex + 2]);
7329 } else
7330 {
7331 damping.setValue(
7332 clientCmd.m_sendDesiredStateCommandArgument.m_damping[velIndex + 0],
7333 clientCmd.m_sendDesiredStateCommandArgument.m_damping[velIndex + 0],
7334 clientCmd.m_sendDesiredStateCommandArgument.m_damping[velIndex + 0]);
7335 }
7336 }
7337 motor->setDamping(damping);
7338 }
7339 numMotors++;
7340 }
7341 }
7342 velIndex += mb->getLink(link).m_dofCount;
7343 posIndex += mb->getLink(link).m_posVarCount;
7344 }
7345 }
7346
7347 break;
7348 }
7349 #ifdef STATIC_LINK_SPD_PLUGIN
7350 case CONTROL_MODE_STABLE_PD:
7351 {
7352 int posVal = body->m_multiBody->getNumPosVars();
7353 btAlignedObjectArray<double> zeroVel;
7354 int dof = 7 + posVal;
7355 zeroVel.resize(dof);
7356 //clientCmd.m_sendDesiredStateCommandArgument.
7357 //current positions and velocities
7358 btAlignedObjectArray<double> jointPositionsQ;
7359 btAlignedObjectArray<double> jointVelocitiesQdot;
7360 btTransform baseTr = body->m_multiBody->getBaseWorldTransform();
7361 #if 1
7362 jointPositionsQ.push_back(baseTr.getOrigin()[0]);
7363 jointPositionsQ.push_back(baseTr.getOrigin()[1]);
7364 jointPositionsQ.push_back(baseTr.getOrigin()[2]);
7365 jointPositionsQ.push_back(baseTr.getRotation()[0]);
7366 jointPositionsQ.push_back(baseTr.getRotation()[1]);
7367 jointPositionsQ.push_back(baseTr.getRotation()[2]);
7368 jointPositionsQ.push_back(baseTr.getRotation()[3]);
7369 jointVelocitiesQdot.push_back(body->m_multiBody->getBaseVel()[0]);
7370 jointVelocitiesQdot.push_back(body->m_multiBody->getBaseVel()[1]);
7371 jointVelocitiesQdot.push_back(body->m_multiBody->getBaseVel()[2]);
7372 jointVelocitiesQdot.push_back(body->m_multiBody->getBaseOmega()[0]);
7373 jointVelocitiesQdot.push_back(body->m_multiBody->getBaseOmega()[1]);
7374 jointVelocitiesQdot.push_back(body->m_multiBody->getBaseOmega()[2]);
7375 jointVelocitiesQdot.push_back(0);
7376 #else
7377 for (int i = 0; i < 7; i++)
7378 {
7379 jointPositionsQ.push_back(0);
7380 jointVelocitiesQdot.push_back(0);
7381 }
7382 jointPositionsQ[6] = 1;
7383 #endif
7384 for (int i = 0; i < body->m_multiBody->getNumLinks(); i++)
7385 {
7386 switch (body->m_multiBody->getLink(i).m_jointType)
7387 {
7388 case btMultibodyLink::eSpherical:
7389 {
7390 btScalar* jointPos = body->m_multiBody->getJointPosMultiDof(i);
7391 jointPositionsQ.push_back(jointPos[0]);
7392 jointPositionsQ.push_back(jointPos[1]);
7393 jointPositionsQ.push_back(jointPos[2]);
7394 jointPositionsQ.push_back(jointPos[3]);
7395 btScalar* jointVel = body->m_multiBody->getJointVelMultiDof(i);
7396 jointVelocitiesQdot.push_back(jointVel[0]);
7397 jointVelocitiesQdot.push_back(jointVel[1]);
7398 jointVelocitiesQdot.push_back(jointVel[2]);
7399 jointVelocitiesQdot.push_back(0);
7400 break;
7401 }
7402 case btMultibodyLink::ePrismatic:
7403 case btMultibodyLink::eRevolute:
7404 {
7405 btScalar* jointPos = body->m_multiBody->getJointPosMultiDof(i);
7406 jointPositionsQ.push_back(jointPos[0]);
7407 btScalar* jointVel = body->m_multiBody->getJointVelMultiDof(i);
7408 jointVelocitiesQdot.push_back(jointVel[0]);
7409 break;
7410 }
7411 case btMultibodyLink::eFixed:
7412 {
7413 //skip
7414 break;
7415 }
7416 default:
7417 {
7418 b3Error("Unsupported joint type");
7419 btAssert(0);
7420 }
7421 }
7422 }
7423
7424 cRBDModel* rbdModel = 0;
7425
7426 {
7427 BT_PROFILE("findOrCreateRBDModel");
7428 rbdModel = m_data->findOrCreateRBDModel(body->m_multiBody, &jointPositionsQ[0], &jointVelocitiesQdot[0]);
7429 }
7430 if (rbdModel)
7431 {
7432 int num_dof = jointPositionsQ.size();
7433 const Eigen::VectorXd& pose = rbdModel->GetPose();
7434 const Eigen::VectorXd& vel = rbdModel->GetVel();
7435
7436 Eigen::Map<Eigen::VectorXd> mKp((double*)clientCmd.m_sendDesiredStateCommandArgument.m_Kp, num_dof);
7437 Eigen::Map<Eigen::VectorXd> mKd((double*)clientCmd.m_sendDesiredStateCommandArgument.m_Kd, num_dof);
7438 Eigen::Map<Eigen::VectorXd> maxForce((double*)clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque, num_dof);
7439
7440 Eigen::DiagonalMatrix<double, Eigen::Dynamic> Kp_mat = mKp.asDiagonal();
7441 Eigen::DiagonalMatrix<double, Eigen::Dynamic> Kd_mat = mKd.asDiagonal();
7442
7443 Eigen::MatrixXd M = rbdModel->GetMassMat();
7444 //rbdModel->UpdateBiasForce();
7445 const Eigen::VectorXd& C = rbdModel->GetBiasForce();
7446 M.diagonal() += m_data->m_physicsDeltaTime * mKd;
7447
7448 Eigen::VectorXd pose_inc;
7449
7450 const Eigen::MatrixXd& joint_mat = rbdModel->GetJointMat();
7451 {
7452 BT_PROFILE("cKinTree::VelToPoseDiff");
7453 cKinTree::VelToPoseDiff(joint_mat, rbdModel->GetPose(), rbdModel->GetVel(), pose_inc);
7454 }
7455
7456 //tar_pose needs to be reshuffled?
7457 Eigen::VectorXd tar_pose, tar_vel;
7458
7459 {
7460 BT_PROFILE("convertPose");
7461 PhysicsServerCommandProcessorInternalData::convertPose(body->m_multiBody,
7462 (double*)clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ,
7463 (double*)clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot,
7464 tar_pose, tar_vel);
7465 }
7466
7467 pose_inc = pose + m_data->m_physicsDeltaTime * pose_inc;
7468
7469 {
7470 BT_PROFILE("cKinTree::PostProcessPose");
7471 cKinTree::PostProcessPose(joint_mat, pose_inc);
7472 }
7473
7474 Eigen::VectorXd pose_err;
7475 {
7476 BT_PROFILE("cKinTree::CalcVel");
7477 cKinTree::CalcVel(joint_mat, pose_inc, tar_pose, 1, pose_err);
7478 }
7479 for (int i = 0; i < 7; i++)
7480 {
7481 pose_err[i] = 0;
7482 }
7483
7484 Eigen::VectorXd vel_err = tar_vel - vel;
7485 Eigen::VectorXd acc;
7486
7487 {
7488 BT_PROFILE("acc");
7489 acc = Kp_mat * pose_err + Kd_mat * vel_err - C;
7490 }
7491
7492 {
7493 BT_PROFILE("M.ldlt().solve");
7494 acc = M.ldlt().solve(acc);
7495 }
7496 Eigen::VectorXd out_tau = Eigen::VectorXd::Zero(num_dof);
7497 out_tau += Kp_mat * pose_err + Kd_mat * (vel_err - m_data->m_physicsDeltaTime * acc);
7498 //clamp the forces
7499 out_tau = out_tau.cwiseMax(-maxForce);
7500 out_tau = out_tau.cwiseMin(maxForce);
7501 //apply the forces
7502 int torqueIndex = 7;
7503 for (int link = 0; link < mb->getNumLinks(); link++)
7504 {
7505 int dofCount = mb->getLink(link).m_dofCount;
7506 int dofOffset = mb->getLink(link).m_dofOffset;
7507 if (dofCount == 3)
7508 {
7509 for (int dof = 0; dof < 3; dof++)
7510 {
7511 double torque = out_tau[torqueIndex + dof];
7512 mb->addJointTorqueMultiDof(link, dof, torque);
7513 }
7514 torqueIndex += 4;
7515 }
7516 if (dofCount == 1)
7517 {
7518 double torque = out_tau[torqueIndex];
7519 mb->addJointTorqueMultiDof(link, 0, torque);
7520 torqueIndex++;
7521 }
7522 }
7523 }
7524
7525 break;
7526 }
7527 #endif
7528 default:
7529 {
7530 b3Warning("m_controlMode not implemented yet");
7531 break;
7532 }
7533 }
7534 }
7535 else
7536 {
7537 //support for non-btMultiBody, such as btRigidBody
7538
7539 if (body && body->m_rigidBody)
7540 {
7541 btRigidBody* rb = body->m_rigidBody;
7542 btAssert(rb);
7543
7544 //switch (clientCmd.m_sendDesiredStateCommandArgument.m_controlMode)
7545 {
7546 //case CONTROL_MODE_TORQUE:
7547 {
7548 if (m_data->m_verboseOutput)
7549 {
7550 b3Printf("Using CONTROL_MODE_TORQUE");
7551 }
7552 // mb->clearForcesAndTorques();
7553 ///see addJointInfoFromConstraint
7554 int velIndex = 6;
7555 int posIndex = 7;
7556 //if ((clientCmd.m_updateFlags&SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0)
7557 {
7558 for (int link = 0; link < body->m_rigidBodyJoints.size(); link++)
7559 {
7560 btGeneric6DofSpring2Constraint* con = body->m_rigidBodyJoints[link];
7561
7562 btVector3 linearLowerLimit;
7563 btVector3 linearUpperLimit;
7564 btVector3 angularLowerLimit;
7565 btVector3 angularUpperLimit;
7566
7567 //for (int dof=0;dof<mb->getLink(link).m_dofCount;dof++)
7568 {
7569 {
7570 int torqueIndex = velIndex;
7571 double torque = 100;
7572 bool hasDesiredTorque = false;
7573 if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex] & SIM_DESIRED_STATE_HAS_MAX_FORCE) != 0)
7574 {
7575 torque = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[velIndex];
7576 hasDesiredTorque = true;
7577 }
7578
7579 bool hasDesiredPosOrVel = false;
7580 btScalar qdotTarget = 0.f;
7581 if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex] & SIM_DESIRED_STATE_HAS_QDOT) != 0)
7582 {
7583 hasDesiredPosOrVel = true;
7584 qdotTarget = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[velIndex];
7585 }
7586 btScalar qTarget = 0.f;
7587 if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[posIndex] & SIM_DESIRED_STATE_HAS_Q) != 0)
7588 {
7589 hasDesiredPosOrVel = true;
7590 qTarget = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[posIndex];
7591 }
7592
7593 con->getLinearLowerLimit(linearLowerLimit);
7594 con->getLinearUpperLimit(linearUpperLimit);
7595 con->getAngularLowerLimit(angularLowerLimit);
7596 con->getAngularUpperLimit(angularUpperLimit);
7597
7598 if (linearLowerLimit.isZero() && linearUpperLimit.isZero() && angularLowerLimit.isZero() && angularUpperLimit.isZero())
7599 {
7600 //fixed, don't do anything
7601 }
7602 else
7603 {
7604 con->calculateTransforms();
7605
7606 if (linearLowerLimit.isZero() && linearUpperLimit.isZero())
7607 {
7608 //eRevoluteType;
7609 btVector3 limitRange = angularLowerLimit.absolute() + angularUpperLimit.absolute();
7610 int limitAxis = limitRange.maxAxis();
7611 const btTransform& transA = con->getCalculatedTransformA();
7612 const btTransform& transB = con->getCalculatedTransformB();
7613 btVector3 axisA = transA.getBasis().getColumn(limitAxis);
7614 btVector3 axisB = transB.getBasis().getColumn(limitAxis);
7615
7616 switch (clientCmd.m_sendDesiredStateCommandArgument.m_controlMode)
7617 {
7618 case CONTROL_MODE_TORQUE:
7619 {
7620 if (hasDesiredTorque)
7621 {
7622 con->getRigidBodyA().applyTorque(torque * axisA);
7623 con->getRigidBodyB().applyTorque(-torque * axisB);
7624 }
7625 break;
7626 }
7627 case CONTROL_MODE_VELOCITY:
7628 {
7629 if (hasDesiredPosOrVel)
7630 {
7631 con->enableMotor(3 + limitAxis, true);
7632 con->setTargetVelocity(3 + limitAxis, qdotTarget);
7633 con->setMaxMotorForce(3 + limitAxis, torque);
7634 }
7635 break;
7636 }
7637 case CONTROL_MODE_POSITION_VELOCITY_PD:
7638 {
7639 if (hasDesiredPosOrVel)
7640 {
7641 con->setServo(3 + limitAxis, true);
7642 con->setServoTarget(3 + limitAxis, -qTarget);
7643 //next one is the maximum velocity to reach target position.
7644 //the maximum velocity is limited by maxMotorForce
7645 con->setTargetVelocity(3 + limitAxis, 100);
7646 con->setMaxMotorForce(3 + limitAxis, torque);
7647 con->enableMotor(3 + limitAxis, true);
7648 }
7649 break;
7650 }
7651 default:
7652 {
7653 }
7654 };
7655 }
7656 else
7657 {
7658 //ePrismaticType; @todo
7659 btVector3 limitRange = linearLowerLimit.absolute() + linearUpperLimit.absolute();
7660 int limitAxis = limitRange.maxAxis();
7661
7662 const btTransform& transA = con->getCalculatedTransformA();
7663 const btTransform& transB = con->getCalculatedTransformB();
7664 btVector3 axisA = transA.getBasis().getColumn(limitAxis);
7665 btVector3 axisB = transB.getBasis().getColumn(limitAxis);
7666
7667 switch (clientCmd.m_sendDesiredStateCommandArgument.m_controlMode)
7668 {
7669 case CONTROL_MODE_TORQUE:
7670 {
7671 con->getRigidBodyA().applyForce(-torque * axisA, btVector3(0, 0, 0));
7672 con->getRigidBodyB().applyForce(torque * axisB, btVector3(0, 0, 0));
7673 break;
7674 }
7675 case CONTROL_MODE_VELOCITY:
7676 {
7677 con->enableMotor(limitAxis, true);
7678 con->setTargetVelocity(limitAxis, -qdotTarget);
7679 con->setMaxMotorForce(limitAxis, torque);
7680 break;
7681 }
7682 case CONTROL_MODE_POSITION_VELOCITY_PD:
7683 {
7684 con->setServo(limitAxis, true);
7685 con->setServoTarget(limitAxis, qTarget);
7686 //next one is the maximum velocity to reach target position.
7687 //the maximum velocity is limited by maxMotorForce
7688 con->setTargetVelocity(limitAxis, 100);
7689 con->setMaxMotorForce(limitAxis, torque);
7690 con->enableMotor(limitAxis, true);
7691 break;
7692 }
7693 default:
7694 {
7695 }
7696 };
7697 }
7698 }
7699 } //fi
7700 ///see addJointInfoFromConstraint
7701 velIndex++; //info.m_uIndex
7702 posIndex++; //info.m_qIndex
7703 }
7704 }
7705 } //fi
7706 //break;
7707 }
7708 }
7709 } //if (body && body->m_rigidBody)
7710 }
7711
7712 serverStatusOut.m_type = CMD_DESIRED_STATE_RECEIVED_COMPLETED;
7713 return hasStatus;
7714 }
7715
processRequestActualStateCommand(const struct SharedMemoryCommand & clientCmd,struct SharedMemoryStatus & serverStatusOut,char * bufferServerToClient,int bufferSizeInBytes)7716 bool PhysicsServerCommandProcessor::processRequestActualStateCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
7717 {
7718 bool hasStatus = true;
7719 serverStatusOut.m_type = CMD_ACTUAL_STATE_UPDATE_FAILED;
7720
7721 BT_PROFILE("CMD_REQUEST_ACTUAL_STATE");
7722 if (m_data->m_verboseOutput)
7723 {
7724 b3Printf("Sending the actual state (Q,U)");
7725 }
7726 int bodyUniqueId = clientCmd.m_requestActualStateInformationCommandArgument.m_bodyUniqueId;
7727 InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId);
7728
7729 //we store the state details in the shared memory block, to reduce status size
7730 SendActualStateSharedMemoryStorage* stateDetails = (SendActualStateSharedMemoryStorage*)bufferServerToClient;
7731
7732 //make sure the storage fits, otherwise
7733 btAssert(sizeof(SendActualStateSharedMemoryStorage) < bufferSizeInBytes);
7734 if (sizeof(SendActualStateSharedMemoryStorage) > bufferSizeInBytes)
7735 {
7736 //this forces an error report
7737 body = 0;
7738 }
7739 if (body && body->m_multiBody)
7740 {
7741 btMultiBody* mb = body->m_multiBody;
7742 SharedMemoryStatus& serverCmd = serverStatusOut;
7743
7744 serverStatusOut.m_type = CMD_ACTUAL_STATE_UPDATE_COMPLETED;
7745
7746 serverCmd.m_sendActualStateArgs.m_bodyUniqueId = bodyUniqueId;
7747 serverCmd.m_sendActualStateArgs.m_numLinks = body->m_multiBody->getNumLinks();
7748 serverCmd.m_numDataStreamBytes = sizeof(SendActualStateSharedMemoryStorage);
7749 serverCmd.m_sendActualStateArgs.m_stateDetails = 0;
7750 int totalDegreeOfFreedomQ = 0;
7751 int totalDegreeOfFreedomU = 0;
7752
7753 if (mb->getNumLinks() >= MAX_DEGREE_OF_FREEDOM)
7754 {
7755 serverStatusOut.m_type = CMD_ACTUAL_STATE_UPDATE_FAILED;
7756 hasStatus = true;
7757 return hasStatus;
7758 }
7759
7760 //always add the base, even for static (non-moving objects)
7761 //so that we can easily move the 'fixed' base when needed
7762 //do we don't use this conditional "if (!mb->hasFixedBase())"
7763 {
7764 btTransform tr;
7765 tr.setOrigin(mb->getBasePos());
7766 tr.setRotation(mb->getWorldToBaseRot().inverse());
7767
7768 serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[0] =
7769 body->m_rootLocalInertialFrame.getOrigin()[0];
7770 serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[1] =
7771 body->m_rootLocalInertialFrame.getOrigin()[1];
7772 serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[2] =
7773 body->m_rootLocalInertialFrame.getOrigin()[2];
7774
7775 serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[3] =
7776 body->m_rootLocalInertialFrame.getRotation()[0];
7777 serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[4] =
7778 body->m_rootLocalInertialFrame.getRotation()[1];
7779 serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[5] =
7780 body->m_rootLocalInertialFrame.getRotation()[2];
7781 serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[6] =
7782 body->m_rootLocalInertialFrame.getRotation()[3];
7783
7784 //base position in world space, cartesian
7785 stateDetails->m_actualStateQ[0] = tr.getOrigin()[0];
7786 stateDetails->m_actualStateQ[1] = tr.getOrigin()[1];
7787 stateDetails->m_actualStateQ[2] = tr.getOrigin()[2];
7788
7789 //base orientation, quaternion x,y,z,w, in world space, cartesian
7790 stateDetails->m_actualStateQ[3] = tr.getRotation()[0];
7791 stateDetails->m_actualStateQ[4] = tr.getRotation()[1];
7792 stateDetails->m_actualStateQ[5] = tr.getRotation()[2];
7793 stateDetails->m_actualStateQ[6] = tr.getRotation()[3];
7794 totalDegreeOfFreedomQ += 7; //pos + quaternion
7795
7796 //base linear velocity (in world space, cartesian)
7797 stateDetails->m_actualStateQdot[0] = mb->getBaseVel()[0];
7798 stateDetails->m_actualStateQdot[1] = mb->getBaseVel()[1];
7799 stateDetails->m_actualStateQdot[2] = mb->getBaseVel()[2];
7800
7801 //base angular velocity (in world space, cartesian)
7802 stateDetails->m_actualStateQdot[3] = mb->getBaseOmega()[0];
7803 stateDetails->m_actualStateQdot[4] = mb->getBaseOmega()[1];
7804 stateDetails->m_actualStateQdot[5] = mb->getBaseOmega()[2];
7805 totalDegreeOfFreedomU += 6; //3 linear and 3 angular DOF
7806 }
7807
7808 btAlignedObjectArray<btVector3> omega;
7809 btAlignedObjectArray<btVector3> linVel;
7810
7811 bool computeForwardKinematics = ((clientCmd.m_updateFlags & ACTUAL_STATE_COMPUTE_FORWARD_KINEMATICS) != 0);
7812 if (computeForwardKinematics)
7813 {
7814 B3_PROFILE("compForwardKinematics");
7815 btAlignedObjectArray<btQuaternion> world_to_local;
7816 btAlignedObjectArray<btVector3> local_origin;
7817 world_to_local.resize(mb->getNumLinks() + 1);
7818 local_origin.resize(mb->getNumLinks() + 1);
7819 mb->forwardKinematics(world_to_local, local_origin);
7820 }
7821
7822 bool computeLinkVelocities = ((clientCmd.m_updateFlags & ACTUAL_STATE_COMPUTE_LINKVELOCITY) != 0);
7823 if (computeLinkVelocities)
7824 {
7825 omega.resize(mb->getNumLinks() + 1);
7826 linVel.resize(mb->getNumLinks() + 1);
7827 {
7828 B3_PROFILE("compTreeLinkVelocities");
7829 mb->compTreeLinkVelocities(&omega[0], &linVel[0]);
7830 }
7831 }
7832 for (int l = 0; l < mb->getNumLinks(); l++)
7833 {
7834 for (int d = 0; d < mb->getLink(l).m_posVarCount; d++)
7835 {
7836 stateDetails->m_actualStateQ[totalDegreeOfFreedomQ++] = mb->getJointPosMultiDof(l)[d];
7837 }
7838 for (int d = 0; d < mb->getLink(l).m_dofCount; d++)
7839 {
7840 stateDetails->m_jointMotorForceMultiDof[totalDegreeOfFreedomU] = 0;
7841
7842 if (mb->getLink(l).m_jointType == btMultibodyLink::eSpherical)
7843 {
7844 btMultiBodySphericalJointMotor* motor = (btMultiBodySphericalJointMotor*)mb->getLink(l).m_userPtr;
7845 if (motor)
7846 {
7847 btScalar impulse = motor->getAppliedImpulse(d);
7848 btScalar force = impulse / m_data->m_physicsDeltaTime;
7849 stateDetails->m_jointMotorForceMultiDof[totalDegreeOfFreedomU] = force;
7850 }
7851 }
7852 else
7853 {
7854 if (supportsJointMotor(mb, l))
7855 {
7856 btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)body->m_multiBody->getLink(l).m_userPtr;
7857
7858 if (motor && m_data->m_physicsDeltaTime > btScalar(0))
7859 {
7860 btScalar force = motor->getAppliedImpulse(0) / m_data->m_physicsDeltaTime;
7861 stateDetails->m_jointMotorForceMultiDof[totalDegreeOfFreedomU] = force;
7862 }
7863 }
7864 }
7865
7866 stateDetails->m_actualStateQdot[totalDegreeOfFreedomU++] = mb->getJointVelMultiDof(l)[d];
7867 }
7868
7869 if (0 == mb->getLink(l).m_jointFeedback)
7870 {
7871 for (int d = 0; d < 6; d++)
7872 {
7873 stateDetails->m_jointReactionForces[l * 6 + d] = 0;
7874 }
7875 }
7876 else
7877 {
7878 btVector3 sensedForce = mb->getLink(l).m_jointFeedback->m_reactionForces.getLinear();
7879 btVector3 sensedTorque = mb->getLink(l).m_jointFeedback->m_reactionForces.getAngular();
7880
7881 stateDetails->m_jointReactionForces[l * 6 + 0] = sensedForce[0];
7882 stateDetails->m_jointReactionForces[l * 6 + 1] = sensedForce[1];
7883 stateDetails->m_jointReactionForces[l * 6 + 2] = sensedForce[2];
7884
7885 stateDetails->m_jointReactionForces[l * 6 + 3] = sensedTorque[0];
7886 stateDetails->m_jointReactionForces[l * 6 + 4] = sensedTorque[1];
7887 stateDetails->m_jointReactionForces[l * 6 + 5] = sensedTorque[2];
7888 }
7889
7890 stateDetails->m_jointMotorForce[l] = 0;
7891
7892 if (supportsJointMotor(mb, l))
7893 {
7894 btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)body->m_multiBody->getLink(l).m_userPtr;
7895
7896 if (motor && m_data->m_physicsDeltaTime > btScalar(0))
7897 {
7898 btScalar force = motor->getAppliedImpulse(0) / m_data->m_physicsDeltaTime;
7899 stateDetails->m_jointMotorForce[l] =
7900 force;
7901 //if (force>0)
7902 //{
7903 // b3Printf("force = %f\n", force);
7904 //}
7905 }
7906 }
7907 btVector3 linkLocalInertialOrigin = body->m_linkLocalInertialFrames[l].getOrigin();
7908 btQuaternion linkLocalInertialRotation = body->m_linkLocalInertialFrames[l].getRotation();
7909
7910 btVector3 linkCOMOrigin = mb->getLink(l).m_cachedWorldTransform.getOrigin();
7911 btQuaternion linkCOMRotation = mb->getLink(l).m_cachedWorldTransform.getRotation();
7912
7913 stateDetails->m_linkState[l * 7 + 0] = linkCOMOrigin.getX();
7914 stateDetails->m_linkState[l * 7 + 1] = linkCOMOrigin.getY();
7915 stateDetails->m_linkState[l * 7 + 2] = linkCOMOrigin.getZ();
7916 stateDetails->m_linkState[l * 7 + 3] = linkCOMRotation.x();
7917 stateDetails->m_linkState[l * 7 + 4] = linkCOMRotation.y();
7918 stateDetails->m_linkState[l * 7 + 5] = linkCOMRotation.z();
7919 stateDetails->m_linkState[l * 7 + 6] = linkCOMRotation.w();
7920
7921 btVector3 worldLinVel(0, 0, 0);
7922 btVector3 worldAngVel(0, 0, 0);
7923
7924 if (computeLinkVelocities)
7925 {
7926 const btMatrix3x3& linkRotMat = mb->getLink(l).m_cachedWorldTransform.getBasis();
7927 worldLinVel = linkRotMat * linVel[l + 1];
7928 worldAngVel = linkRotMat * omega[l + 1];
7929 }
7930
7931 stateDetails->m_linkWorldVelocities[l * 6 + 0] = worldLinVel[0];
7932 stateDetails->m_linkWorldVelocities[l * 6 + 1] = worldLinVel[1];
7933 stateDetails->m_linkWorldVelocities[l * 6 + 2] = worldLinVel[2];
7934 stateDetails->m_linkWorldVelocities[l * 6 + 3] = worldAngVel[0];
7935 stateDetails->m_linkWorldVelocities[l * 6 + 4] = worldAngVel[1];
7936 stateDetails->m_linkWorldVelocities[l * 6 + 5] = worldAngVel[2];
7937
7938 stateDetails->m_linkLocalInertialFrames[l * 7 + 0] = linkLocalInertialOrigin.getX();
7939 stateDetails->m_linkLocalInertialFrames[l * 7 + 1] = linkLocalInertialOrigin.getY();
7940 stateDetails->m_linkLocalInertialFrames[l * 7 + 2] = linkLocalInertialOrigin.getZ();
7941
7942 stateDetails->m_linkLocalInertialFrames[l * 7 + 3] = linkLocalInertialRotation.x();
7943 stateDetails->m_linkLocalInertialFrames[l * 7 + 4] = linkLocalInertialRotation.y();
7944 stateDetails->m_linkLocalInertialFrames[l * 7 + 5] = linkLocalInertialRotation.z();
7945 stateDetails->m_linkLocalInertialFrames[l * 7 + 6] = linkLocalInertialRotation.w();
7946 }
7947
7948 serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomQ = totalDegreeOfFreedomQ;
7949 serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomU = totalDegreeOfFreedomU;
7950
7951 hasStatus = true;
7952 }
7953 else if (body && body->m_rigidBody)
7954 {
7955 btRigidBody* rb = body->m_rigidBody;
7956 SharedMemoryStatus& serverCmd = serverStatusOut;
7957 serverCmd.m_type = CMD_ACTUAL_STATE_UPDATE_COMPLETED;
7958
7959 serverCmd.m_sendActualStateArgs.m_bodyUniqueId = bodyUniqueId;
7960 serverCmd.m_sendActualStateArgs.m_numLinks = 0;
7961 serverCmd.m_numDataStreamBytes = sizeof(SendActualStateSharedMemoryStorage);
7962 serverCmd.m_sendActualStateArgs.m_stateDetails = 0;
7963
7964 serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[0] =
7965 body->m_rootLocalInertialFrame.getOrigin()[0];
7966 serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[1] =
7967 body->m_rootLocalInertialFrame.getOrigin()[1];
7968 serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[2] =
7969 body->m_rootLocalInertialFrame.getOrigin()[2];
7970
7971 serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[3] =
7972 body->m_rootLocalInertialFrame.getRotation()[0];
7973 serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[4] =
7974 body->m_rootLocalInertialFrame.getRotation()[1];
7975 serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[5] =
7976 body->m_rootLocalInertialFrame.getRotation()[2];
7977 serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[6] =
7978 body->m_rootLocalInertialFrame.getRotation()[3];
7979
7980 btTransform tr = rb->getWorldTransform();
7981 //base position in world space, cartesian
7982 stateDetails->m_actualStateQ[0] = tr.getOrigin()[0];
7983 stateDetails->m_actualStateQ[1] = tr.getOrigin()[1];
7984 stateDetails->m_actualStateQ[2] = tr.getOrigin()[2];
7985
7986 //base orientation, quaternion x,y,z,w, in world space, cartesian
7987 stateDetails->m_actualStateQ[3] = tr.getRotation()[0];
7988 stateDetails->m_actualStateQ[4] = tr.getRotation()[1];
7989 stateDetails->m_actualStateQ[5] = tr.getRotation()[2];
7990 stateDetails->m_actualStateQ[6] = tr.getRotation()[3];
7991 int totalDegreeOfFreedomQ = 7; //pos + quaternion
7992
7993 //base linear velocity (in world space, cartesian)
7994 stateDetails->m_actualStateQdot[0] = rb->getLinearVelocity()[0];
7995 stateDetails->m_actualStateQdot[1] = rb->getLinearVelocity()[1];
7996 stateDetails->m_actualStateQdot[2] = rb->getLinearVelocity()[2];
7997
7998 //base angular velocity (in world space, cartesian)
7999 stateDetails->m_actualStateQdot[3] = rb->getAngularVelocity()[0];
8000 stateDetails->m_actualStateQdot[4] = rb->getAngularVelocity()[1];
8001 stateDetails->m_actualStateQdot[5] = rb->getAngularVelocity()[2];
8002 int totalDegreeOfFreedomU = 6; //3 linear and 3 angular DOF
8003
8004 serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomQ = totalDegreeOfFreedomQ;
8005 serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomU = totalDegreeOfFreedomU;
8006
8007 hasStatus = true;
8008 }
8009 #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
8010 else if (body && body->m_softBody)
8011 {
8012 btSoftBody* sb = body->m_softBody;
8013 SharedMemoryStatus& serverCmd = serverStatusOut;
8014 serverCmd.m_type = CMD_ACTUAL_STATE_UPDATE_COMPLETED;
8015 serverCmd.m_sendActualStateArgs.m_bodyUniqueId = bodyUniqueId;
8016 serverCmd.m_sendActualStateArgs.m_numLinks = 0;
8017 serverCmd.m_numDataStreamBytes = sizeof(SendActualStateSharedMemoryStorage);
8018 serverCmd.m_sendActualStateArgs.m_stateDetails = 0;
8019
8020 serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[0] =
8021 body->m_rootLocalInertialFrame.getOrigin()[0];
8022 serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[1] =
8023 body->m_rootLocalInertialFrame.getOrigin()[1];
8024 serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[2] =
8025 body->m_rootLocalInertialFrame.getOrigin()[2];
8026
8027 serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[3] =
8028 body->m_rootLocalInertialFrame.getRotation()[0];
8029 serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[4] =
8030 body->m_rootLocalInertialFrame.getRotation()[1];
8031 serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[5] =
8032 body->m_rootLocalInertialFrame.getRotation()[2];
8033 serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[6] =
8034 body->m_rootLocalInertialFrame.getRotation()[3];
8035
8036 btVector3 center_of_mass(sb->getCenterOfMass());
8037 btTransform tr = sb->getRigidTransform();
8038 //base position in world space, cartesian
8039 stateDetails->m_actualStateQ[0] = center_of_mass[0];
8040 stateDetails->m_actualStateQ[1] = center_of_mass[1];
8041 stateDetails->m_actualStateQ[2] = center_of_mass[2];
8042
8043 //base orientation, quaternion x,y,z,w, in world space, cartesian
8044 stateDetails->m_actualStateQ[3] = tr.getRotation()[0];
8045 stateDetails->m_actualStateQ[4] = tr.getRotation()[1];
8046 stateDetails->m_actualStateQ[5] = tr.getRotation()[2];
8047 stateDetails->m_actualStateQ[6] = tr.getRotation()[3];
8048
8049 int totalDegreeOfFreedomQ = 7; //pos + quaternion
8050 int totalDegreeOfFreedomU = 6; //3 linear and 3 angular DOF
8051
8052 serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomQ = totalDegreeOfFreedomQ;
8053 serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomU = totalDegreeOfFreedomU;
8054
8055 hasStatus = true;
8056 }
8057 #endif
8058 else
8059 {
8060 //b3Warning("Request state but no multibody or rigid body available");
8061 SharedMemoryStatus& serverCmd = serverStatusOut;
8062 serverCmd.m_type = CMD_ACTUAL_STATE_UPDATE_FAILED;
8063 hasStatus = true;
8064 }
8065 return hasStatus;
8066 }
8067
processRequestDeformableContactpointHelper(const struct SharedMemoryCommand & clientCmd)8068 bool PhysicsServerCommandProcessor::processRequestDeformableContactpointHelper(const struct SharedMemoryCommand& clientCmd)
8069 {
8070 #ifndef SKIP_DEFORMABLE_BODY
8071 btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld();
8072 if (!deformWorld)
8073 {
8074 return false;
8075 }
8076
8077 for (int i = deformWorld->getSoftBodyArray().size() - 1; i >= 0; i--)
8078 {
8079 btSoftBody* psb = deformWorld->getSoftBodyArray()[i];
8080 btAlignedObjectArray<b3ContactPointData> distinctContactPoints;
8081 btAlignedObjectArray<btSoftBody::Node*> nodesInContact;
8082 for (int c = 0; c < psb->m_faceRigidContacts.size(); c++)
8083 {
8084 const btSoftBody::DeformableFaceRigidContact* contact = &psb->m_faceRigidContacts[c];
8085 // calculate normal and tangent impulse
8086 btVector3 impulse = contact->m_cti.m_impulse;
8087 btVector3 impulseNormal = impulse.dot(contact->m_cti.m_normal) * contact->m_cti.m_normal;
8088 btVector3 impulseTangent = impulse - impulseNormal;
8089 // get node in contact
8090 int contactNodeIdx = contact->m_bary.maxAxis();
8091 btSoftBody::Node* node = contact->m_face->m_n[contactNodeIdx];
8092 // check if node is already in the list
8093 int idx = nodesInContact.findLinearSearch2(node);
8094
8095 //apply the filter, if the user provides it
8096 int linkIndexA = -1;
8097 int linkIndexB = -1;
8098 int objectIndexA = psb->getUserIndex2();
8099
8100 int objectIndexB = -1;
8101 const btRigidBody* bodyB = btRigidBody::upcast(contact->m_cti.m_colObj);
8102 if (bodyB)
8103 {
8104 objectIndexB = bodyB->getUserIndex2();
8105 }
8106 const btMultiBodyLinkCollider* mblB = btMultiBodyLinkCollider::upcast(contact->m_cti.m_colObj);
8107 if (mblB && mblB->m_multiBody)
8108 {
8109 linkIndexB = mblB->m_link;
8110 objectIndexB = mblB->m_multiBody->getUserIndex2();
8111 }
8112 bool swap = false;
8113 if (clientCmd.m_requestContactPointArguments.m_objectAIndexFilter >= 0)
8114 {
8115 if (clientCmd.m_requestContactPointArguments.m_objectAIndexFilter == objectIndexA)
8116 {
8117 swap = false;
8118 }
8119 else if (clientCmd.m_requestContactPointArguments.m_objectAIndexFilter == objectIndexB)
8120 {
8121 swap = true;
8122 }
8123 else
8124 {
8125 continue;
8126 }
8127 }
8128
8129 if (swap)
8130 {
8131 std::swap(objectIndexA, objectIndexB);
8132 std::swap(linkIndexA, linkIndexB);
8133 }
8134
8135 //apply the second object filter, if the user provides it
8136 if (clientCmd.m_requestContactPointArguments.m_objectBIndexFilter >= 0)
8137 {
8138 if (clientCmd.m_requestContactPointArguments.m_objectBIndexFilter != objectIndexB)
8139 {
8140 continue;
8141 }
8142 }
8143
8144 if (
8145 (clientCmd.m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_LINK_INDEX_A_FILTER) &&
8146 clientCmd.m_requestContactPointArguments.m_linkIndexAIndexFilter != linkIndexA)
8147 {
8148 continue;
8149 }
8150
8151 if (
8152 (clientCmd.m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_LINK_INDEX_B_FILTER) &&
8153 clientCmd.m_requestContactPointArguments.m_linkIndexBIndexFilter != linkIndexB)
8154 {
8155 continue;
8156 }
8157
8158 if (idx < 0)
8159 {
8160 // add new node and contact point
8161 nodesInContact.push_back(node);
8162 b3ContactPointData pt;
8163 pt.m_bodyUniqueIdA = objectIndexA;
8164 pt.m_bodyUniqueIdB = objectIndexB;
8165 pt.m_contactDistance = -contact->m_cti.m_offset;
8166 pt.m_contactFlags = 0;
8167 pt.m_linkIndexA = linkIndexA;
8168 pt.m_linkIndexB = linkIndexB;
8169 for (int j = 0; j < 3; j++)
8170 {
8171 if (swap)
8172 {
8173 pt.m_contactNormalOnBInWS[j] = -contact->m_cti.m_normal[j];
8174 pt.m_positionOnAInWS[j] = node->m_x[j] - pt.m_contactDistance * pt.m_contactNormalOnBInWS[j]; // not really precise because of margins in btSoftBody.cpp:line 2912
8175 // node is force application point, therefore node position is contact point (not contact->m_contactPoint, because not equal to node)
8176 pt.m_positionOnBInWS[j] = node->m_x[j];
8177 }
8178 else
8179 {
8180 pt.m_contactNormalOnBInWS[j] = contact->m_cti.m_normal[j];
8181 // node is force application point, therefore node position is contact point (not contact->m_contactPoint, because not equal to node)
8182 pt.m_positionOnAInWS[j] = node->m_x[j];
8183 pt.m_positionOnBInWS[j] = node->m_x[j] - pt.m_contactDistance * pt.m_contactNormalOnBInWS[j]; // not really precise because of margins in btSoftBody.cpp:line 2912
8184 }
8185 }
8186 pt.m_normalForce = (impulseNormal / m_data->m_physicsDeltaTime).norm();
8187 pt.m_linearFrictionForce1 = (impulseTangent.dot(contact->t1) * contact->t1 / m_data->m_physicsDeltaTime).norm();
8188 pt.m_linearFrictionForce2 = (impulseTangent.dot(contact->t2) * contact->t2 / m_data->m_physicsDeltaTime).norm();
8189 for (int j = 0; j < 3; j++)
8190 {
8191 pt.m_linearFrictionDirection1[j] = contact->t1[j];
8192 pt.m_linearFrictionDirection2[j] = contact->t2[j];
8193 }
8194 distinctContactPoints.push_back(pt);
8195 }
8196 else
8197 {
8198 // add values to existing contact point
8199 b3ContactPointData* pt = &distinctContactPoints[idx];
8200 // current normal force of node
8201 btVector3 normalForce = btVector3(btScalar(pt->m_contactNormalOnBInWS[0]),
8202 btScalar(pt->m_contactNormalOnBInWS[1]),
8203 btScalar(pt->m_contactNormalOnBInWS[2])) * pt->m_normalForce;
8204 // add normal force of additional node contact
8205 btScalar swapFactor = swap ? -1.0 : 1.0;
8206 normalForce += swapFactor * contact->m_cti.m_normal * (impulseNormal / m_data->m_physicsDeltaTime).norm();
8207 // get magnitude of normal force
8208 pt->m_normalForce = normalForce.norm();
8209 // get direction of normal force
8210 if (!normalForce.fuzzyZero())
8211 {
8212 // normalize for unit vectors if above numerical threshold
8213 normalForce.normalize();
8214 for (int j = 0; j < 3; j++)
8215 {
8216 pt->m_contactNormalOnBInWS[j] = normalForce[j];
8217 }
8218 }
8219
8220 // add magnitudes of tangential forces in existing directions
8221 btVector3 linearFrictionDirection1 = btVector3(btScalar(pt->m_linearFrictionDirection1[0]),
8222 btScalar(pt->m_linearFrictionDirection1[1]),
8223 btScalar(pt->m_linearFrictionDirection1[2]));
8224 btVector3 linearFrictionDirection2 = btVector3(btScalar(pt->m_linearFrictionDirection2[0]),
8225 btScalar(pt->m_linearFrictionDirection2[1]),
8226 btScalar(pt->m_linearFrictionDirection2[2]));
8227 pt->m_linearFrictionForce1 = (impulseTangent.dot(linearFrictionDirection1) * linearFrictionDirection1 / m_data->m_physicsDeltaTime).norm();
8228 pt->m_linearFrictionForce2 = (impulseTangent.dot(linearFrictionDirection2) * linearFrictionDirection2 / m_data->m_physicsDeltaTime).norm();
8229 }
8230 }
8231
8232 int num_contact_points = m_data->m_cachedContactPoints.size() + distinctContactPoints.size();
8233 m_data->m_cachedContactPoints.reserve(num_contact_points);
8234 // add points to contact points cache
8235 for (int p = 0; p < distinctContactPoints.size(); p++)
8236 {
8237 m_data->m_cachedContactPoints.push_back(distinctContactPoints[p]);
8238 }
8239 }
8240 #endif
8241 return true;
8242 }
8243
processRequestContactpointInformationCommand(const struct SharedMemoryCommand & clientCmd,struct SharedMemoryStatus & serverStatusOut,char * bufferServerToClient,int bufferSizeInBytes)8244 bool PhysicsServerCommandProcessor::processRequestContactpointInformationCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
8245 {
8246 bool hasStatus = true;
8247 BT_PROFILE("CMD_REQUEST_CONTACT_POINT_INFORMATION");
8248 SharedMemoryStatus& serverCmd = serverStatusOut;
8249 serverCmd.m_sendContactPointArgs.m_numContactPointsCopied = 0;
8250
8251 //make a snapshot of the contact manifolds into individual contact points
8252 if (clientCmd.m_requestContactPointArguments.m_startingContactPointIndex == 0)
8253 {
8254 m_data->m_cachedContactPoints.resize(0);
8255
8256 int mode = CONTACT_QUERY_MODE_REPORT_EXISTING_CONTACT_POINTS;
8257
8258 if (clientCmd.m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_QUERY_MODE)
8259 {
8260 mode = clientCmd.m_requestContactPointArguments.m_mode;
8261 }
8262
8263 switch (mode)
8264 {
8265 case CONTACT_QUERY_MODE_REPORT_EXISTING_CONTACT_POINTS:
8266 {
8267 int numContactManifolds = m_data->m_dynamicsWorld->getDispatcher()->getNumManifolds();
8268 m_data->m_cachedContactPoints.reserve(numContactManifolds * 4);
8269 for (int i = 0; i < numContactManifolds; i++)
8270 {
8271 const btPersistentManifold* manifold = m_data->m_dynamicsWorld->getDispatcher()->getInternalManifoldPointer()[i];
8272 int linkIndexA = -1;
8273 int linkIndexB = -1;
8274
8275 int objectIndexB = -1;
8276 const btRigidBody* bodyB = btRigidBody::upcast(manifold->getBody1());
8277 if (bodyB)
8278 {
8279 objectIndexB = bodyB->getUserIndex2();
8280 }
8281 const btMultiBodyLinkCollider* mblB = btMultiBodyLinkCollider::upcast(manifold->getBody1());
8282 if (mblB && mblB->m_multiBody)
8283 {
8284 linkIndexB = mblB->m_link;
8285 objectIndexB = mblB->m_multiBody->getUserIndex2();
8286 }
8287
8288 int objectIndexA = -1;
8289 const btRigidBody* bodyA = btRigidBody::upcast(manifold->getBody0());
8290 if (bodyA)
8291 {
8292 objectIndexA = bodyA->getUserIndex2();
8293 }
8294 const btMultiBodyLinkCollider* mblA = btMultiBodyLinkCollider::upcast(manifold->getBody0());
8295 if (mblA && mblA->m_multiBody)
8296 {
8297 linkIndexA = mblA->m_link;
8298 objectIndexA = mblA->m_multiBody->getUserIndex2();
8299 }
8300 btAssert(bodyA || mblA);
8301
8302 //apply the filter, if the user provides it
8303 bool swap = false;
8304 if (clientCmd.m_requestContactPointArguments.m_objectAIndexFilter >= 0)
8305 {
8306 if (clientCmd.m_requestContactPointArguments.m_objectAIndexFilter == objectIndexA)
8307 {
8308 swap = false;
8309 }
8310 else if (clientCmd.m_requestContactPointArguments.m_objectAIndexFilter == objectIndexB)
8311 {
8312 swap = true;
8313 }
8314 else
8315 {
8316 continue;
8317 }
8318 }
8319
8320 if (swap)
8321 {
8322 std::swap(objectIndexA, objectIndexB);
8323 std::swap(linkIndexA, linkIndexB);
8324 std::swap(bodyA, bodyB);
8325 }
8326
8327 //apply the second object filter, if the user provides it
8328 if (clientCmd.m_requestContactPointArguments.m_objectBIndexFilter >= 0)
8329 {
8330 if (clientCmd.m_requestContactPointArguments.m_objectBIndexFilter != objectIndexB)
8331 {
8332 continue;
8333 }
8334 }
8335
8336 if (
8337 (clientCmd.m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_LINK_INDEX_A_FILTER) &&
8338 clientCmd.m_requestContactPointArguments.m_linkIndexAIndexFilter != linkIndexA)
8339 {
8340 continue;
8341 }
8342
8343 if (
8344 (clientCmd.m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_LINK_INDEX_B_FILTER) &&
8345 clientCmd.m_requestContactPointArguments.m_linkIndexBIndexFilter != linkIndexB)
8346 {
8347 continue;
8348 }
8349
8350 for (int p = 0; p < manifold->getNumContacts(); p++)
8351 {
8352 b3ContactPointData pt;
8353 pt.m_bodyUniqueIdA = objectIndexA;
8354 pt.m_bodyUniqueIdB = objectIndexB;
8355 const btManifoldPoint& srcPt = manifold->getContactPoint(p);
8356 pt.m_contactDistance = srcPt.getDistance();
8357 pt.m_contactFlags = 0;
8358 pt.m_linkIndexA = linkIndexA;
8359 pt.m_linkIndexB = linkIndexB;
8360 for (int j = 0; j < 3; j++)
8361 {
8362 if (swap)
8363 {
8364 pt.m_contactNormalOnBInWS[j] = -srcPt.m_normalWorldOnB[j];
8365 pt.m_positionOnAInWS[j] = srcPt.getPositionWorldOnB()[j];
8366 pt.m_positionOnBInWS[j] = srcPt.getPositionWorldOnA()[j];
8367 }
8368 else
8369 {
8370 pt.m_contactNormalOnBInWS[j] = srcPt.m_normalWorldOnB[j];
8371 pt.m_positionOnAInWS[j] = srcPt.getPositionWorldOnA()[j];
8372 pt.m_positionOnBInWS[j] = srcPt.getPositionWorldOnB()[j];
8373 }
8374 }
8375 pt.m_normalForce = srcPt.getAppliedImpulse() / m_data->m_physicsDeltaTime;
8376 pt.m_linearFrictionForce1 = srcPt.m_appliedImpulseLateral1 / m_data->m_physicsDeltaTime;
8377 pt.m_linearFrictionForce2 = srcPt.m_appliedImpulseLateral2 / m_data->m_physicsDeltaTime;
8378 for (int j = 0; j < 3; j++)
8379 {
8380 pt.m_linearFrictionDirection1[j] = srcPt.m_lateralFrictionDir1[j];
8381 pt.m_linearFrictionDirection2[j] = srcPt.m_lateralFrictionDir2[j];
8382 }
8383 m_data->m_cachedContactPoints.push_back(pt);
8384 }
8385 }
8386
8387 #ifndef SKIP_DEFORMABLE_BODY
8388 processRequestDeformableContactpointHelper(clientCmd);
8389 #endif
8390 break;
8391 }
8392
8393 case CONTACT_QUERY_MODE_COMPUTE_CLOSEST_POINTS:
8394 {
8395 //todo(erwincoumans) compute closest points between all, and vs all, pair
8396 btScalar closestDistanceThreshold = 0.f;
8397
8398 if (clientCmd.m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_CLOSEST_DISTANCE_THRESHOLD)
8399 {
8400 closestDistanceThreshold = clientCmd.m_requestContactPointArguments.m_closestDistanceThreshold;
8401 }
8402
8403 int bodyUniqueIdA = clientCmd.m_requestContactPointArguments.m_objectAIndexFilter;
8404 int bodyUniqueIdB = clientCmd.m_requestContactPointArguments.m_objectBIndexFilter;
8405
8406 bool hasLinkIndexAFilter = (0 != (clientCmd.m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_LINK_INDEX_A_FILTER));
8407 bool hasLinkIndexBFilter = (0 != (clientCmd.m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_LINK_INDEX_B_FILTER));
8408
8409 int linkIndexA = clientCmd.m_requestContactPointArguments.m_linkIndexAIndexFilter;
8410 int linkIndexB = clientCmd.m_requestContactPointArguments.m_linkIndexBIndexFilter;
8411
8412 btAlignedObjectArray<btCollisionObject*> setA;
8413 btAlignedObjectArray<btCollisionObject*> setB;
8414 btAlignedObjectArray<int> setALinkIndex;
8415 btAlignedObjectArray<int> setBLinkIndex;
8416
8417 btCollisionObject colObA;
8418 btCollisionObject colObB;
8419
8420 int collisionShapeA = (clientCmd.m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_COLLISION_SHAPE_A) ? clientCmd.m_requestContactPointArguments.m_collisionShapeA : -1;
8421 int collisionShapeB = (clientCmd.m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_COLLISION_SHAPE_B) ? clientCmd.m_requestContactPointArguments.m_collisionShapeB : -1;
8422
8423 if (collisionShapeA >= 0)
8424 {
8425 btVector3 posA(0, 0, 0);
8426 if (clientCmd.m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_COLLISION_SHAPE_POSITION_A)
8427 {
8428 posA.setValue(clientCmd.m_requestContactPointArguments.m_collisionShapePositionA[0],
8429 clientCmd.m_requestContactPointArguments.m_collisionShapePositionA[1],
8430 clientCmd.m_requestContactPointArguments.m_collisionShapePositionA[2]);
8431 }
8432 btQuaternion ornA(0, 0, 0, 1);
8433 if (clientCmd.m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_COLLISION_SHAPE_ORIENTATION_A)
8434 {
8435 ornA.setValue(clientCmd.m_requestContactPointArguments.m_collisionShapeOrientationA[0],
8436 clientCmd.m_requestContactPointArguments.m_collisionShapeOrientationA[1],
8437 clientCmd.m_requestContactPointArguments.m_collisionShapeOrientationA[2],
8438 clientCmd.m_requestContactPointArguments.m_collisionShapeOrientationA[3]);
8439 }
8440 InternalCollisionShapeHandle* handle = m_data->m_userCollisionShapeHandles.getHandle(collisionShapeA);
8441
8442 if (handle && handle->m_collisionShape)
8443 {
8444 colObA.setCollisionShape(handle->m_collisionShape);
8445 btTransform tr;
8446 tr.setIdentity();
8447 tr.setOrigin(posA);
8448 tr.setRotation(ornA);
8449 colObA.setWorldTransform(tr);
8450 setA.push_back(&colObA);
8451 setALinkIndex.push_back(-2);
8452 }
8453 else
8454 {
8455 b3Warning("collisionShapeA provided is not valid.");
8456 }
8457 }
8458 if (collisionShapeB >= 0)
8459 {
8460 btVector3 posB(0, 0, 0);
8461 if (clientCmd.m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_COLLISION_SHAPE_POSITION_B)
8462 {
8463 posB.setValue(clientCmd.m_requestContactPointArguments.m_collisionShapePositionB[0],
8464 clientCmd.m_requestContactPointArguments.m_collisionShapePositionB[1],
8465 clientCmd.m_requestContactPointArguments.m_collisionShapePositionB[2]);
8466 }
8467 btQuaternion ornB(0, 0, 0, 1);
8468 if (clientCmd.m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_COLLISION_SHAPE_ORIENTATION_B)
8469 {
8470 ornB.setValue(clientCmd.m_requestContactPointArguments.m_collisionShapeOrientationB[0],
8471 clientCmd.m_requestContactPointArguments.m_collisionShapeOrientationB[1],
8472 clientCmd.m_requestContactPointArguments.m_collisionShapeOrientationB[2],
8473 clientCmd.m_requestContactPointArguments.m_collisionShapeOrientationB[3]);
8474 }
8475
8476 InternalCollisionShapeHandle* handle = m_data->m_userCollisionShapeHandles.getHandle(collisionShapeB);
8477 if (handle && handle->m_collisionShape)
8478 {
8479 colObB.setCollisionShape(handle->m_collisionShape);
8480 btTransform tr;
8481 tr.setIdentity();
8482 tr.setOrigin(posB);
8483 tr.setRotation(ornB);
8484 colObB.setWorldTransform(tr);
8485 setB.push_back(&colObB);
8486 setBLinkIndex.push_back(-2);
8487 }
8488 else
8489 {
8490 b3Warning("collisionShapeB provided is not valid.");
8491 }
8492 }
8493
8494 if (bodyUniqueIdA >= 0)
8495 {
8496 InternalBodyData* bodyA = m_data->m_bodyHandles.getHandle(bodyUniqueIdA);
8497 if (bodyA)
8498 {
8499 if (bodyA->m_multiBody)
8500 {
8501 if (bodyA->m_multiBody->getBaseCollider())
8502 {
8503 if (!hasLinkIndexAFilter || (linkIndexA == -1))
8504 {
8505 setA.push_back(bodyA->m_multiBody->getBaseCollider());
8506 setALinkIndex.push_back(-1);
8507 }
8508 }
8509 for (int i = 0; i < bodyA->m_multiBody->getNumLinks(); i++)
8510 {
8511 if (bodyA->m_multiBody->getLink(i).m_collider)
8512 {
8513 if (!hasLinkIndexAFilter || (linkIndexA == i))
8514 {
8515 setA.push_back(bodyA->m_multiBody->getLink(i).m_collider);
8516 setALinkIndex.push_back(i);
8517 }
8518 }
8519 }
8520 }
8521 if (bodyA->m_rigidBody)
8522 {
8523 setA.push_back(bodyA->m_rigidBody);
8524 setALinkIndex.push_back(-1);
8525 }
8526 }
8527 }
8528 if (bodyUniqueIdB >= 0)
8529 {
8530 InternalBodyData* bodyB = m_data->m_bodyHandles.getHandle(bodyUniqueIdB);
8531 if (bodyB)
8532 {
8533 if (bodyB->m_multiBody)
8534 {
8535 if (bodyB->m_multiBody->getBaseCollider())
8536 {
8537 if (!hasLinkIndexBFilter || (linkIndexB == -1))
8538 {
8539 setB.push_back(bodyB->m_multiBody->getBaseCollider());
8540 setBLinkIndex.push_back(-1);
8541 }
8542 }
8543 for (int i = 0; i < bodyB->m_multiBody->getNumLinks(); i++)
8544 {
8545 if (bodyB->m_multiBody->getLink(i).m_collider)
8546 {
8547 if (!hasLinkIndexBFilter || (linkIndexB == i))
8548 {
8549 setB.push_back(bodyB->m_multiBody->getLink(i).m_collider);
8550 setBLinkIndex.push_back(i);
8551 }
8552 }
8553 }
8554 }
8555 if (bodyB->m_rigidBody)
8556 {
8557 setB.push_back(bodyB->m_rigidBody);
8558 setBLinkIndex.push_back(-1);
8559 }
8560 }
8561 }
8562
8563 {
8564 ///ContactResultCallback is used to report contact points
8565 struct MyContactResultCallback : public btCollisionWorld::ContactResultCallback
8566 {
8567 int m_bodyUniqueIdA;
8568 int m_bodyUniqueIdB;
8569 int m_linkIndexA;
8570 int m_linkIndexB;
8571 btScalar m_deltaTime;
8572
8573 btAlignedObjectArray<b3ContactPointData>& m_cachedContactPoints;
8574
8575 MyContactResultCallback(btAlignedObjectArray<b3ContactPointData>& pointCache)
8576 : m_cachedContactPoints(pointCache)
8577 {
8578 }
8579
8580 virtual ~MyContactResultCallback()
8581 {
8582 }
8583
8584 virtual bool needsCollision(btBroadphaseProxy* proxy0) const
8585 {
8586 //bool collides = (proxy0->m_collisionFilterGroup & m_collisionFilterMask) != 0;
8587 //collides = collides && (m_collisionFilterGroup & proxy0->m_collisionFilterMask);
8588 //return collides;
8589 return true;
8590 }
8591
8592 virtual btScalar addSingleResult(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap, int partId0, int index0, const btCollisionObjectWrapper* colObj1Wrap, int partId1, int index1)
8593 {
8594 const btCollisionObject* colObj = (btCollisionObject*)colObj0Wrap->getCollisionObject();
8595 const btMultiBodyLinkCollider* mbl = btMultiBodyLinkCollider::upcast(colObj);
8596 int bodyUniqueId = -1;
8597 if (mbl)
8598 {
8599 bodyUniqueId = mbl->m_multiBody->getUserIndex2();
8600 }
8601 else
8602 {
8603 bodyUniqueId = colObj->getUserIndex2();
8604 }
8605
8606
8607 bool isSwapped = m_bodyUniqueIdA != bodyUniqueId;
8608
8609 if (cp.m_distance1 <= m_closestDistanceThreshold)
8610 {
8611 b3ContactPointData pt;
8612 pt.m_bodyUniqueIdA = m_bodyUniqueIdA;
8613 pt.m_bodyUniqueIdB = m_bodyUniqueIdB;
8614 const btManifoldPoint& srcPt = cp;
8615 pt.m_contactDistance = srcPt.getDistance();
8616 pt.m_contactFlags = 0;
8617 pt.m_linkIndexA = m_linkIndexA;
8618 pt.m_linkIndexB = m_linkIndexB;
8619 for (int j = 0; j < 3; j++)
8620 {
8621 if (isSwapped)
8622 {
8623 pt.m_contactNormalOnBInWS[j] = -srcPt.m_normalWorldOnB[j];
8624 pt.m_positionOnAInWS[j] = srcPt.getPositionWorldOnB()[j];
8625 pt.m_positionOnBInWS[j] = srcPt.getPositionWorldOnA()[j];
8626 }
8627 else
8628 {
8629 pt.m_contactNormalOnBInWS[j] = srcPt.m_normalWorldOnB[j];
8630 pt.m_positionOnAInWS[j] = srcPt.getPositionWorldOnA()[j];
8631 pt.m_positionOnBInWS[j] = srcPt.getPositionWorldOnB()[j];
8632 }
8633 }
8634 pt.m_normalForce = srcPt.getAppliedImpulse() / m_deltaTime;
8635 pt.m_linearFrictionForce1 = srcPt.m_appliedImpulseLateral1 / m_deltaTime;
8636 pt.m_linearFrictionForce2 = srcPt.m_appliedImpulseLateral2 / m_deltaTime;
8637 for (int j = 0; j < 3; j++)
8638 {
8639 pt.m_linearFrictionDirection1[j] = srcPt.m_lateralFrictionDir1[j];
8640 pt.m_linearFrictionDirection2[j] = srcPt.m_lateralFrictionDir2[j];
8641 }
8642 m_cachedContactPoints.push_back(pt);
8643 }
8644 return 1;
8645 }
8646 };
8647
8648 MyContactResultCallback cb(m_data->m_cachedContactPoints);
8649
8650 cb.m_bodyUniqueIdA = bodyUniqueIdA;
8651 cb.m_bodyUniqueIdB = bodyUniqueIdB;
8652 cb.m_deltaTime = m_data->m_numSimulationSubSteps > 0 ? m_data->m_physicsDeltaTime / m_data->m_numSimulationSubSteps : m_data->m_physicsDeltaTime;
8653
8654 for (int i = 0; i < setA.size(); i++)
8655 {
8656 cb.m_linkIndexA = setALinkIndex[i];
8657 for (int j = 0; j < setB.size(); j++)
8658 {
8659 cb.m_linkIndexB = setBLinkIndex[j];
8660 cb.m_closestDistanceThreshold = closestDistanceThreshold;
8661 this->m_data->m_dynamicsWorld->contactPairTest(setA[i], setB[j], cb);
8662 }
8663 }
8664 }
8665
8666 break;
8667 }
8668 default:
8669 {
8670 b3Warning("Unknown contact query mode: %d", mode);
8671 }
8672 }
8673 }
8674
8675 int numContactPoints = m_data->m_cachedContactPoints.size();
8676
8677 //b3ContactPoint
8678 //struct b3ContactPointDynamics
8679
8680 int totalBytesPerContact = sizeof(b3ContactPointData);
8681 int contactPointStorage = bufferSizeInBytes / totalBytesPerContact - 1;
8682
8683 b3ContactPointData* contactData = (b3ContactPointData*)bufferServerToClient;
8684
8685 int startContactPointIndex = clientCmd.m_requestContactPointArguments.m_startingContactPointIndex;
8686 int numContactPointBatch = btMin(numContactPoints, contactPointStorage);
8687
8688 int endContactPointIndex = startContactPointIndex + numContactPointBatch;
8689
8690 for (int i = startContactPointIndex; i < endContactPointIndex; i++)
8691 {
8692 const b3ContactPointData& srcPt = m_data->m_cachedContactPoints[i];
8693 b3ContactPointData& destPt = contactData[serverCmd.m_sendContactPointArgs.m_numContactPointsCopied];
8694 destPt = srcPt;
8695 serverCmd.m_sendContactPointArgs.m_numContactPointsCopied++;
8696 }
8697
8698 serverCmd.m_sendContactPointArgs.m_startingContactPointIndex = clientCmd.m_requestContactPointArguments.m_startingContactPointIndex;
8699 serverCmd.m_sendContactPointArgs.m_numRemainingContactPoints = numContactPoints - clientCmd.m_requestContactPointArguments.m_startingContactPointIndex - serverCmd.m_sendContactPointArgs.m_numContactPointsCopied;
8700 serverCmd.m_numDataStreamBytes = totalBytesPerContact * serverCmd.m_sendContactPointArgs.m_numContactPointsCopied;
8701 serverCmd.m_type = CMD_CONTACT_POINT_INFORMATION_COMPLETED; //CMD_CONTACT_POINT_INFORMATION_FAILED,
8702
8703 return hasStatus;
8704 }
8705
processRequestBodyInfoCommand(const struct SharedMemoryCommand & clientCmd,struct SharedMemoryStatus & serverStatusOut,char * bufferServerToClient,int bufferSizeInBytes)8706 bool PhysicsServerCommandProcessor::processRequestBodyInfoCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
8707 {
8708 bool hasStatus = true;
8709 BT_PROFILE("CMD_REQUEST_BODY_INFO");
8710
8711 const SdfRequestInfoArgs& sdfInfoArgs = clientCmd.m_sdfRequestInfoArgs;
8712 //stream info into memory
8713 int streamSizeInBytes = createBodyInfoStream(sdfInfoArgs.m_bodyUniqueId, bufferServerToClient, bufferSizeInBytes);
8714
8715 serverStatusOut.m_type = CMD_BODY_INFO_COMPLETED;
8716 serverStatusOut.m_dataStreamArguments.m_bodyUniqueId = sdfInfoArgs.m_bodyUniqueId;
8717 serverStatusOut.m_dataStreamArguments.m_bodyName[0] = 0;
8718
8719 InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(sdfInfoArgs.m_bodyUniqueId);
8720 if (bodyHandle)
8721 {
8722 strcpy(serverStatusOut.m_dataStreamArguments.m_bodyName, bodyHandle->m_bodyName.c_str());
8723 }
8724
8725 serverStatusOut.m_numDataStreamBytes = streamSizeInBytes;
8726
8727 return hasStatus;
8728 }
8729
processLoadSDFCommand(const struct SharedMemoryCommand & clientCmd,struct SharedMemoryStatus & serverStatusOut,char * bufferServerToClient,int bufferSizeInBytes)8730 bool PhysicsServerCommandProcessor::processLoadSDFCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
8731 {
8732 bool hasStatus = true;
8733 BT_PROFILE("CMD_LOAD_SDF");
8734
8735 const SdfArgs& sdfArgs = clientCmd.m_sdfArguments;
8736 if (m_data->m_verboseOutput)
8737 {
8738 b3Printf("Processed CMD_LOAD_SDF:%s", sdfArgs.m_sdfFileName);
8739 }
8740 bool useMultiBody = (clientCmd.m_updateFlags & URDF_ARGS_USE_MULTIBODY) ? (sdfArgs.m_useMultiBody != 0) : true;
8741
8742 int flags = CUF_USE_SDF; //CUF_USE_URDF_INERTIA
8743 btScalar globalScaling = 1.f;
8744 if (clientCmd.m_updateFlags & URDF_ARGS_USE_GLOBAL_SCALING)
8745 {
8746 globalScaling = sdfArgs.m_globalScaling;
8747 }
8748 bool completedOk = loadSdf(sdfArgs.m_sdfFileName, bufferServerToClient, bufferSizeInBytes, useMultiBody, flags, globalScaling);
8749 if (completedOk)
8750 {
8751 m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld);
8752
8753 //serverStatusOut.m_type = CMD_SDF_LOADING_FAILED;
8754 serverStatusOut.m_sdfLoadedArgs.m_numBodies = m_data->m_sdfRecentLoadedBodies.size();
8755 serverStatusOut.m_sdfLoadedArgs.m_numUserConstraints = 0;
8756 int maxBodies = btMin(MAX_SDF_BODIES, serverStatusOut.m_sdfLoadedArgs.m_numBodies);
8757 for (int i = 0; i < maxBodies; i++)
8758 {
8759 serverStatusOut.m_sdfLoadedArgs.m_bodyUniqueIds[i] = m_data->m_sdfRecentLoadedBodies[i];
8760 }
8761
8762 serverStatusOut.m_type = CMD_SDF_LOADING_COMPLETED;
8763 }
8764 else
8765 {
8766 serverStatusOut.m_type = CMD_SDF_LOADING_FAILED;
8767 }
8768 return hasStatus;
8769 }
8770
processCreateMultiBodyCommand(const struct SharedMemoryCommand & clientCmd,struct SharedMemoryStatus & serverStatusOut,char * bufferServerToClient,int bufferSizeInBytes)8771 bool PhysicsServerCommandProcessor::processCreateMultiBodyCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
8772 {
8773 if (clientCmd.m_createMultiBodyArgs.m_numBatchObjects > 0)
8774 {
8775 //batch of objects, to speed up creation time
8776 bool result = false;
8777 SharedMemoryCommand clientCmd2 = clientCmd;
8778 int baseLinkIndex = clientCmd.m_createMultiBodyArgs.m_baseLinkIndex;
8779 double* basePositionAndOrientations = (double*)bufferServerToClient;
8780 for (int i = 0; i < clientCmd2.m_createMultiBodyArgs.m_numBatchObjects; i++)
8781 {
8782 clientCmd2.m_createMultiBodyArgs.m_linkPositions[baseLinkIndex * 3 + 0] = basePositionAndOrientations[0 + i * 3];
8783 clientCmd2.m_createMultiBodyArgs.m_linkPositions[baseLinkIndex * 3 + 1] = basePositionAndOrientations[1 + i * 3];
8784 clientCmd2.m_createMultiBodyArgs.m_linkPositions[baseLinkIndex * 3 + 2] = basePositionAndOrientations[2 + i * 3];
8785 if (i == (clientCmd2.m_createMultiBodyArgs.m_numBatchObjects - 1))
8786 {
8787 result = processCreateMultiBodyCommandSingle(clientCmd2, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
8788 }
8789 else
8790 {
8791 result = processCreateMultiBodyCommandSingle(clientCmd2, serverStatusOut, 0, 0);
8792 }
8793 }
8794 m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld);
8795 return result;
8796 }
8797 return processCreateMultiBodyCommandSingle(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
8798 }
8799
processCreateMultiBodyCommandSingle(const struct SharedMemoryCommand & clientCmd,struct SharedMemoryStatus & serverStatusOut,char * bufferServerToClient,int bufferSizeInBytes)8800 bool PhysicsServerCommandProcessor::processCreateMultiBodyCommandSingle(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
8801 {
8802 BT_PROFILE("processCreateMultiBodyCommand2");
8803 bool hasStatus = true;
8804
8805 serverStatusOut.m_type = CMD_CREATE_MULTI_BODY_FAILED;
8806 if (clientCmd.m_createMultiBodyArgs.m_baseLinkIndex >= 0)
8807 {
8808 m_data->m_sdfRecentLoadedBodies.clear();
8809
8810 int flags = 0;
8811
8812 if (clientCmd.m_updateFlags & MULT_BODY_HAS_FLAGS)
8813 {
8814 flags = clientCmd.m_createMultiBodyArgs.m_flags;
8815 }
8816
8817 ProgrammaticUrdfInterface u2b(clientCmd.m_createMultiBodyArgs, m_data, flags);
8818
8819 bool useMultiBody = true;
8820 if (clientCmd.m_updateFlags & MULT_BODY_USE_MAXIMAL_COORDINATES)
8821 {
8822 useMultiBody = false;
8823 }
8824
8825 bool ok = 0;
8826 {
8827 BT_PROFILE("processImportedObjects");
8828 ok = processImportedObjects("memory", bufferServerToClient, bufferSizeInBytes, useMultiBody, flags, u2b);
8829 }
8830
8831 if (ok)
8832 {
8833 BT_PROFILE("post process");
8834 int bodyUniqueId = -1;
8835
8836 if (m_data->m_sdfRecentLoadedBodies.size() == 1)
8837 {
8838 bodyUniqueId = m_data->m_sdfRecentLoadedBodies[0];
8839 }
8840 m_data->m_sdfRecentLoadedBodies.clear();
8841 if (bodyUniqueId >= 0)
8842 {
8843 serverStatusOut.m_type = CMD_CREATE_MULTI_BODY_COMPLETED;
8844 if (bufferSizeInBytes > 0 && serverStatusOut.m_numDataStreamBytes == 0)
8845 {
8846 {
8847 BT_PROFILE("autogenerateGraphicsObjects");
8848 m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld);
8849 }
8850
8851 BT_PROFILE("createBodyInfoStream");
8852 int streamSizeInBytes = createBodyInfoStream(bodyUniqueId, bufferServerToClient, bufferSizeInBytes);
8853 serverStatusOut.m_numDataStreamBytes = streamSizeInBytes;
8854
8855 serverStatusOut.m_dataStreamArguments.m_bodyUniqueId = bodyUniqueId;
8856 InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId);
8857 strcpy(serverStatusOut.m_dataStreamArguments.m_bodyName, body->m_bodyName.c_str());
8858 }
8859 }
8860 }
8861
8862 //ConvertURDF2Bullet(u2b,creation, rootTrans,m_data->m_dynamicsWorld,useMultiBody,u2b.getPathPrefix(),flags);
8863 }
8864 return hasStatus;
8865 }
8866
processLoadURDFCommand(const struct SharedMemoryCommand & clientCmd,struct SharedMemoryStatus & serverStatusOut,char * bufferServerToClient,int bufferSizeInBytes)8867 bool PhysicsServerCommandProcessor::processLoadURDFCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
8868 {
8869 bool hasStatus = true;
8870 serverStatusOut.m_type = CMD_URDF_LOADING_FAILED;
8871
8872 BT_PROFILE("CMD_LOAD_URDF");
8873 const UrdfArgs& urdfArgs = clientCmd.m_urdfArguments;
8874 if (m_data->m_verboseOutput)
8875 {
8876 b3Printf("Processed CMD_LOAD_URDF:%s", urdfArgs.m_urdfFileName);
8877 }
8878 btAssert((clientCmd.m_updateFlags & URDF_ARGS_FILE_NAME) != 0);
8879 btAssert(urdfArgs.m_urdfFileName);
8880 btVector3 initialPos(0, 0, 0);
8881 btQuaternion initialOrn(0, 0, 0, 1);
8882 if (clientCmd.m_updateFlags & URDF_ARGS_INITIAL_POSITION)
8883 {
8884 initialPos[0] = urdfArgs.m_initialPosition[0];
8885 initialPos[1] = urdfArgs.m_initialPosition[1];
8886 initialPos[2] = urdfArgs.m_initialPosition[2];
8887 }
8888 int urdfFlags = 0;
8889 if (clientCmd.m_updateFlags & URDF_ARGS_HAS_CUSTOM_URDF_FLAGS)
8890 {
8891 urdfFlags = urdfArgs.m_urdfFlags;
8892 }
8893 if (clientCmd.m_updateFlags & URDF_ARGS_INITIAL_ORIENTATION)
8894 {
8895 initialOrn[0] = urdfArgs.m_initialOrientation[0];
8896 initialOrn[1] = urdfArgs.m_initialOrientation[1];
8897 initialOrn[2] = urdfArgs.m_initialOrientation[2];
8898 initialOrn[3] = urdfArgs.m_initialOrientation[3];
8899 }
8900 bool useMultiBody = (clientCmd.m_updateFlags & URDF_ARGS_USE_MULTIBODY) ? (urdfArgs.m_useMultiBody != 0) : true;
8901 bool useFixedBase = (clientCmd.m_updateFlags & URDF_ARGS_USE_FIXED_BASE) ? (urdfArgs.m_useFixedBase != 0) : false;
8902 int bodyUniqueId;
8903 btScalar globalScaling = 1.f;
8904 if (clientCmd.m_updateFlags & URDF_ARGS_USE_GLOBAL_SCALING)
8905 {
8906 globalScaling = urdfArgs.m_globalScaling;
8907 }
8908 //load the actual URDF and send a report: completed or failed
8909 bool completedOk = loadUrdf(urdfArgs.m_urdfFileName,
8910 initialPos, initialOrn,
8911 useMultiBody, useFixedBase, &bodyUniqueId, bufferServerToClient, bufferSizeInBytes, urdfFlags, globalScaling);
8912
8913 if (completedOk && bodyUniqueId >= 0)
8914 {
8915 m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld);
8916
8917 serverStatusOut.m_type = CMD_URDF_LOADING_COMPLETED;
8918
8919 int streamSizeInBytes = createBodyInfoStream(bodyUniqueId, bufferServerToClient, bufferSizeInBytes);
8920 serverStatusOut.m_numDataStreamBytes = streamSizeInBytes;
8921
8922 #ifdef ENABLE_LINK_MAPPER
8923 if (m_data->m_urdfLinkNameMapper.size())
8924 {
8925 serverStatusOut.m_numDataStreamBytes = m_data->m_urdfLinkNameMapper.at(m_data->m_urdfLinkNameMapper.size() - 1)->m_memSerializer->getCurrentBufferSize();
8926 }
8927 #endif
8928 serverStatusOut.m_dataStreamArguments.m_bodyUniqueId = bodyUniqueId;
8929 InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId);
8930 strcpy(serverStatusOut.m_dataStreamArguments.m_bodyName, body->m_bodyName.c_str());
8931 }
8932
8933 return hasStatus;
8934 }
8935
constructUrdfDeformable(const struct SharedMemoryCommand & clientCmd,UrdfDeformable & deformable,bool verbose)8936 void constructUrdfDeformable(const struct SharedMemoryCommand& clientCmd, UrdfDeformable& deformable, bool verbose)
8937 {
8938 const LoadSoftBodyArgs& loadSoftBodyArgs = clientCmd.m_loadSoftBodyArguments;
8939 if (verbose)
8940 {
8941 b3Printf("Processed CMD_LOAD_SOFT_BODY:%s", loadSoftBodyArgs.m_fileName);
8942 }
8943 btAssert((clientCmd.m_updateFlags & LOAD_SOFT_BODY_FILE_NAME) != 0);
8944 btAssert(loadSoftBodyArgs.m_fileName);
8945
8946 if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_UPDATE_MASS)
8947 {
8948 deformable.m_mass = loadSoftBodyArgs.m_mass;
8949 }
8950 if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_UPDATE_COLLISION_MARGIN)
8951 {
8952 deformable.m_collisionMargin = loadSoftBodyArgs.m_collisionMargin;
8953 }
8954 deformable.m_visualFileName = loadSoftBodyArgs.m_fileName;
8955 if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_SIM_MESH)
8956 {
8957 deformable.m_simFileName = loadSoftBodyArgs.m_simFileName;
8958 }
8959 else
8960 {
8961 deformable.m_simFileName = "";
8962 }
8963 #ifndef SKIP_DEFORMABLE_BODY
8964 deformable.m_springCoefficients.elastic_stiffness = loadSoftBodyArgs.m_springElasticStiffness;
8965 deformable.m_springCoefficients.damping_stiffness = loadSoftBodyArgs.m_springDampingStiffness;
8966 if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_BENDING_SPRINGS)
8967 {
8968 deformable.m_springCoefficients.bending_stiffness = loadSoftBodyArgs.m_springBendingStiffness;
8969 }
8970
8971 if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_SET_DAMPING_SPRING_MODE)
8972 {
8973 deformable.m_springCoefficients.damp_all_directions = loadSoftBodyArgs.m_dampAllDirections;
8974 }
8975
8976 if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_COROTATED_FORCE)
8977 {
8978 deformable.m_corotatedCoefficients.mu = loadSoftBodyArgs.m_corotatedMu;
8979 deformable.m_corotatedCoefficients.lambda = loadSoftBodyArgs.m_corotatedLambda;
8980 }
8981
8982 if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_NEOHOOKEAN_FORCE)
8983 {
8984 deformable.m_neohookeanCoefficients.mu = loadSoftBodyArgs.m_NeoHookeanMu;
8985 deformable.m_neohookeanCoefficients.lambda = loadSoftBodyArgs.m_NeoHookeanLambda;
8986 deformable.m_neohookeanCoefficients.damping = loadSoftBodyArgs.m_NeoHookeanDamping;
8987 }
8988 if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_SET_FRICTION_COEFFICIENT)
8989 {
8990 deformable.m_friction = loadSoftBodyArgs.m_frictionCoeff;
8991 }
8992 if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_SET_REPULSION_STIFFNESS)
8993 {
8994 deformable.m_repulsionStiffness = loadSoftBodyArgs.m_repulsionStiffness;
8995 }
8996 if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_SET_GRAVITY_FACTOR)
8997 {
8998 deformable.m_gravFactor = loadSoftBodyArgs.m_gravFactor;
8999 }
9000 #endif
9001 }
9002
processDeformable(const UrdfDeformable & deformable,const btVector3 & pos,const btQuaternion & orn,int * bodyUniqueId,char * bufferServerToClient,int bufferSizeInBytes,btScalar scale,bool useSelfCollision)9003 bool PhysicsServerCommandProcessor::processDeformable(const UrdfDeformable& deformable, const btVector3& pos, const btQuaternion& orn, int* bodyUniqueId, char* bufferServerToClient, int bufferSizeInBytes, btScalar scale, bool useSelfCollision)
9004 {
9005 #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
9006 btSoftBody* psb = NULL;
9007 CommonFileIOInterface* fileIO(m_data->m_pluginManager.getFileIOInterface());
9008 char relativeFileName[1024];
9009 char pathPrefix[1024];
9010 pathPrefix[0] = 0;
9011 if (fileIO->findResourcePath(deformable.m_visualFileName.c_str(), relativeFileName, 1024))
9012 {
9013 b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024);
9014 }
9015 const std::string& error_message_prefix = "";
9016 std::string out_found_filename, out_found_sim_filename;
9017 int out_type(0), out_sim_type(0);
9018
9019 bool foundFile = UrdfFindMeshFile(fileIO, pathPrefix, relativeFileName, error_message_prefix, &out_found_filename, &out_type);
9020 if (!deformable.m_simFileName.empty())
9021 {
9022 bool foundSimMesh = UrdfFindMeshFile(fileIO, pathPrefix, deformable.m_simFileName, error_message_prefix, &out_found_sim_filename, &out_sim_type);
9023 }
9024 else
9025 {
9026 out_sim_type = out_type;
9027 out_found_sim_filename = out_found_filename;
9028 }
9029 if (out_sim_type == UrdfGeometry::FILE_OBJ)
9030 {
9031 std::vector<tinyobj::shape_t> shapes;
9032 tinyobj::attrib_t attribute;
9033 std::string err = tinyobj::LoadObj(attribute, shapes, out_found_sim_filename.c_str(), "", fileIO);
9034 if (!shapes.empty())
9035 {
9036 const tinyobj::shape_t& shape = shapes[0];
9037 btAlignedObjectArray<btScalar> vertices;
9038 btAlignedObjectArray<int> indices;
9039 for (int i = 0; i < attribute.vertices.size(); i++)
9040 {
9041 vertices.push_back(attribute.vertices[i]);
9042 }
9043 for (int i = 0; i < shape.mesh.indices.size(); i++)
9044 {
9045 indices.push_back(shape.mesh.indices[i].vertex_index);
9046 }
9047 int numTris = shape.mesh.indices.size() / 3;
9048 if (numTris > 0)
9049 {
9050 {
9051 btSoftMultiBodyDynamicsWorld* softWorld = getSoftWorld();
9052 if (softWorld)
9053 {
9054 psb = btSoftBodyHelpers::CreateFromTriMesh(softWorld->getWorldInfo(), &vertices[0], &indices[0], numTris);
9055 if (!psb)
9056 {
9057 printf("Load deformable failed\n");
9058 return false;
9059 }
9060 }
9061 }
9062 {
9063 btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld();
9064 if (deformWorld)
9065 {
9066 psb = btSoftBodyHelpers::CreateFromTriMesh(deformWorld->getWorldInfo(), &vertices[0], &indices[0], numTris);
9067 if (!psb)
9068 {
9069 printf("Load deformable failed\n");
9070 return false;
9071 }
9072 }
9073 }
9074 }
9075 }
9076 #ifndef SKIP_DEFORMABLE_BODY
9077 btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld();
9078 if (deformWorld && deformable.m_springCoefficients.elastic_stiffness > 0.)
9079 {
9080 btDeformableLagrangianForce* springForce =
9081 new btDeformableMassSpringForce(deformable.m_springCoefficients.elastic_stiffness,
9082 deformable.m_springCoefficients.damping_stiffness,
9083 !deformable.m_springCoefficients.damp_all_directions,
9084 deformable.m_springCoefficients.bending_stiffness);
9085 deformWorld->addForce(psb, springForce);
9086 m_data->m_lf.push_back(springForce);
9087 }
9088 #endif
9089 }
9090 else if (out_sim_type == UrdfGeometry::FILE_VTK)
9091 {
9092 #ifndef SKIP_DEFORMABLE_BODY
9093 btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld();
9094 if (deformWorld)
9095 {
9096 psb = btSoftBodyHelpers::CreateFromVtkFile(deformWorld->getWorldInfo(), out_found_sim_filename.c_str());
9097 if (!psb)
9098 {
9099 printf("Load deformable failed\n");
9100 return false;
9101 }
9102 btScalar corotated_mu(0.), corotated_lambda(0.);
9103 corotated_mu = deformable.m_corotatedCoefficients.mu;
9104 corotated_lambda = deformable.m_corotatedCoefficients.lambda;
9105 if (corotated_mu > 0 || corotated_lambda > 0)
9106 {
9107 btDeformableLagrangianForce* corotatedForce = new btDeformableCorotatedForce(corotated_mu, corotated_lambda);
9108 deformWorld->addForce(psb, corotatedForce);
9109 m_data->m_lf.push_back(corotatedForce);
9110 }
9111 btScalar neohookean_mu, neohookean_lambda, neohookean_damping;
9112 neohookean_mu = deformable.m_neohookeanCoefficients.mu;
9113 neohookean_lambda = deformable.m_neohookeanCoefficients.lambda;
9114 neohookean_damping = deformable.m_neohookeanCoefficients.damping;
9115 if (neohookean_mu > 0 || neohookean_lambda > 0)
9116 {
9117 btDeformableLagrangianForce* neohookeanForce = new btDeformableNeoHookeanForce(neohookean_mu, neohookean_lambda, neohookean_damping);
9118 deformWorld->addForce(psb, neohookeanForce);
9119 m_data->m_lf.push_back(neohookeanForce);
9120 }
9121
9122 btScalar spring_elastic_stiffness, spring_damping_stiffness, spring_bending_stiffness;
9123 spring_elastic_stiffness = deformable.m_springCoefficients.elastic_stiffness;
9124 spring_damping_stiffness = deformable.m_springCoefficients.damping_stiffness;
9125 spring_bending_stiffness = deformable.m_springCoefficients.bending_stiffness;
9126 if (spring_elastic_stiffness > 0.)
9127 {
9128 btDeformableLagrangianForce* springForce = new btDeformableMassSpringForce(spring_elastic_stiffness, spring_damping_stiffness, true, spring_bending_stiffness);
9129 deformWorld->addForce(psb, springForce);
9130 m_data->m_lf.push_back(springForce);
9131 }
9132 }
9133 #endif
9134 }
9135 b3ImportMeshData meshData;
9136
9137 if (psb != NULL)
9138 {
9139 #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
9140 // load render mesh
9141 if ((out_found_sim_filename != out_found_filename) || ((out_sim_type == UrdfGeometry::FILE_OBJ)))
9142 {
9143 // load render mesh
9144 if (!m_data->m_useAlternativeDeformableIndexing)
9145 {
9146
9147 float rgbaColor[4] = { 1,1,1,1 };
9148
9149 if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(
9150 out_found_filename.c_str(), meshData, fileIO))
9151 {
9152
9153 for (int v = 0; v < meshData.m_gfxShape->m_numvertices; v++)
9154 {
9155 btSoftBody::RenderNode n;
9156 n.m_x.setValue(
9157 meshData.m_gfxShape->m_vertices->at(v).xyzw[0],
9158 meshData.m_gfxShape->m_vertices->at(v).xyzw[1],
9159 meshData.m_gfxShape->m_vertices->at(v).xyzw[2]);
9160 n.m_uv1.setValue(meshData.m_gfxShape->m_vertices->at(v).uv[0],
9161 meshData.m_gfxShape->m_vertices->at(v).uv[1],
9162 0.);
9163 n.m_normal.setValue(meshData.m_gfxShape->m_vertices->at(v).normal[0],
9164 meshData.m_gfxShape->m_vertices->at(v).normal[1],
9165 meshData.m_gfxShape->m_vertices->at(v).normal[2]);
9166 psb->m_renderNodes.push_back(n);
9167 }
9168 for (int f = 0; f < meshData.m_gfxShape->m_numIndices; f += 3)
9169 {
9170 btSoftBody::RenderFace ff;
9171 ff.m_n[0] = &psb->m_renderNodes[meshData.m_gfxShape->m_indices->at(f + 0)];
9172 ff.m_n[1] = &psb->m_renderNodes[meshData.m_gfxShape->m_indices->at(f + 1)];
9173 ff.m_n[2] = &psb->m_renderNodes[meshData.m_gfxShape->m_indices->at(f + 2)];
9174 psb->m_renderFaces.push_back(ff);
9175 }
9176 }
9177 }
9178 else
9179 {
9180 tinyobj::attrib_t attribute;
9181 std::vector<tinyobj::shape_t> shapes;
9182
9183 std::string err = tinyobj::LoadObj(attribute, shapes, out_found_filename.c_str(), pathPrefix, m_data->m_pluginManager.getFileIOInterface());
9184
9185 for (int s = 0; s < (int)shapes.size(); s++)
9186 {
9187 tinyobj::shape_t& shape = shapes[s];
9188 int faceCount = shape.mesh.indices.size();
9189 int vertexCount = attribute.vertices.size() / 3;
9190 for (int v = 0; v < vertexCount; v++)
9191 {
9192 btSoftBody::RenderNode n;
9193 n.m_x = btVector3(attribute.vertices[3 * v], attribute.vertices[3 * v + 1], attribute.vertices[3 * v + 2]);
9194 psb->m_renderNodes.push_back(n);
9195 }
9196 for (int f = 0; f < faceCount; f += 3)
9197 {
9198 if (f < 0 && f >= int(shape.mesh.indices.size()))
9199 {
9200 continue;
9201 }
9202 tinyobj::index_t v_0 = shape.mesh.indices[f];
9203 tinyobj::index_t v_1 = shape.mesh.indices[f + 1];
9204 tinyobj::index_t v_2 = shape.mesh.indices[f + 2];
9205 btSoftBody::RenderFace ff;
9206 ff.m_n[0] = &psb->m_renderNodes[v_0.vertex_index];
9207 ff.m_n[1] = &psb->m_renderNodes[v_1.vertex_index];
9208 ff.m_n[2] = &psb->m_renderNodes[v_2.vertex_index];
9209 psb->m_renderFaces.push_back(ff);
9210 }
9211 }
9212 }
9213 if (out_sim_type == UrdfGeometry::FILE_VTK)
9214 {
9215 btSoftBodyHelpers::interpolateBarycentricWeights(psb);
9216 }
9217 else if (out_sim_type == UrdfGeometry::FILE_OBJ)
9218 {
9219 btSoftBodyHelpers::extrapolateBarycentricWeights(psb);
9220 }
9221 }
9222 else
9223 {
9224 psb->m_renderNodes.resize(0);
9225 }
9226 #endif
9227 #ifndef SKIP_DEFORMABLE_BODY
9228 btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld();
9229 if (deformWorld)
9230 {
9231
9232 btVector3 gravity = m_data->m_dynamicsWorld->getGravity();
9233 btDeformableLagrangianForce* gravityForce = new btDeformableGravityForce(gravity);
9234 deformWorld->addForce(psb, gravityForce);
9235 m_data->m_lf.push_back(gravityForce);
9236 btScalar collision_hardness = 1;
9237 psb->m_cfg.kKHR = collision_hardness;
9238 psb->m_cfg.kCHR = collision_hardness;
9239
9240 psb->m_cfg.kDF = deformable.m_friction;
9241 if (deformable.m_springCoefficients.bending_stiffness)
9242 {
9243 psb->generateBendingConstraints(deformable.m_springCoefficients.bending_stride);
9244 }
9245 btSoftBody::Material* pm = psb->appendMaterial();
9246 pm->m_flags -= btSoftBody::fMaterial::DebugDraw;
9247
9248 // turn on the collision flag for deformable
9249 // collision between deformable and rigid
9250 psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
9251 // turn on face contact for multibodies
9252 psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_MDF;
9253 /// turn on face contact for rigid body
9254 psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDF;
9255 // collion between deformable and deformable and self-collision
9256 psb->m_cfg.collisions |= btSoftBody::fCollision::VF_DD;
9257 psb->setCollisionFlags(0);
9258 psb->setTotalMass(deformable.m_mass);
9259 psb->setSelfCollision(useSelfCollision);
9260 psb->setSpringStiffness(deformable.m_repulsionStiffness);
9261 psb->setGravityFactor(deformable.m_gravFactor);
9262 psb->setCacheBarycenter(deformable.m_cache_barycenter);
9263 psb->initializeFaceTree();
9264 }
9265 #endif //SKIP_DEFORMABLE_BODY
9266 #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
9267 btSoftMultiBodyDynamicsWorld* softWorld = getSoftWorld();
9268 if (softWorld)
9269 {
9270 btSoftBody::Material* pm = psb->appendMaterial();
9271 pm->m_kLST = 0.5;
9272 pm->m_flags -= btSoftBody::fMaterial::DebugDraw;
9273 psb->generateBendingConstraints(2, pm);
9274 psb->m_cfg.piterations = 20;
9275 psb->m_cfg.kDF = 0.5;
9276 //turn on softbody vs softbody collision
9277 psb->m_cfg.collisions |= btSoftBody::fCollision::VF_SS;
9278 psb->randomizeConstraints();
9279 psb->setTotalMass(deformable.m_mass, true);
9280 }
9281 #endif //SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
9282 psb->scale(btVector3(scale, scale, scale));
9283 psb->rotate(orn);
9284 psb->translate(pos);
9285
9286 psb->getCollisionShape()->setMargin(deformable.m_collisionMargin);
9287 psb->getCollisionShape()->setUserPointer(psb);
9288 #ifndef SKIP_DEFORMABLE_BODY
9289 if (deformWorld)
9290 {
9291 deformWorld->addSoftBody(psb);
9292 }
9293 else
9294 #endif //SKIP_DEFORMABLE_BODY
9295 {
9296 btSoftMultiBodyDynamicsWorld* softWorld = getSoftWorld();
9297 if (softWorld)
9298 {
9299 softWorld->addSoftBody(psb);
9300 }
9301 }
9302
9303 *bodyUniqueId = m_data->m_bodyHandles.allocHandle();
9304 InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(*bodyUniqueId);
9305 bodyHandle->m_softBody = psb;
9306 psb->setUserIndex2(*bodyUniqueId);
9307
9308 b3VisualShapeData visualShape;
9309
9310 visualShape.m_objectUniqueId = *bodyUniqueId;
9311 visualShape.m_linkIndex = -1;
9312 visualShape.m_visualGeometryType = URDF_GEOM_MESH;
9313 //dimensions just contains the scale
9314 visualShape.m_dimensions[0] = 1;
9315 visualShape.m_dimensions[1] = 1;
9316 visualShape.m_dimensions[2] = 1;
9317 //filename
9318 strncpy(visualShape.m_meshAssetFileName, relativeFileName, VISUAL_SHAPE_MAX_PATH_LEN);
9319 visualShape.m_meshAssetFileName[VISUAL_SHAPE_MAX_PATH_LEN - 1] = 0;
9320 //position and orientation
9321 visualShape.m_localVisualFrame[0] = 0;
9322 visualShape.m_localVisualFrame[1] = 0;
9323 visualShape.m_localVisualFrame[2] = 0;
9324 visualShape.m_localVisualFrame[3] = 0;
9325 visualShape.m_localVisualFrame[4] = 0;
9326 visualShape.m_localVisualFrame[5] = 0;
9327 visualShape.m_localVisualFrame[6] = 1;
9328 //color and ids to be set by the renderer
9329 visualShape.m_rgbaColor[0] = 1;
9330 visualShape.m_rgbaColor[1] = 1;
9331 visualShape.m_rgbaColor[2] = 1;
9332 visualShape.m_rgbaColor[3] = 1;
9333 visualShape.m_tinyRendererTextureId = -1;
9334 visualShape.m_textureUniqueId = -1;
9335 visualShape.m_openglTextureId = -1;
9336
9337 if (meshData.m_gfxShape)
9338 {
9339 int texUid1 = -1;
9340 if (meshData.m_textureHeight > 0 && meshData.m_textureWidth > 0 && meshData.m_textureImage1)
9341 {
9342 texUid1 = m_data->m_guiHelper->registerTexture(meshData.m_textureImage1, meshData.m_textureWidth, meshData.m_textureHeight);
9343 }
9344 visualShape.m_openglTextureId = texUid1;
9345 int shapeUid1 = m_data->m_guiHelper->registerGraphicsShape(&meshData.m_gfxShape->m_vertices->at(0).xyzw[0], meshData.m_gfxShape->m_numvertices, &meshData.m_gfxShape->m_indices->at(0), meshData.m_gfxShape->m_numIndices, B3_GL_TRIANGLES, texUid1);
9346 psb->getCollisionShape()->setUserIndex(shapeUid1);
9347 float position[4] = { 0,0,0,1 };
9348 float orientation[4] = { 0,0,0,1 };
9349 float color[4] = { 1,1,1,1 };
9350 float scaling[4] = { 1,1,1,1 };
9351 int instanceUid = m_data->m_guiHelper->registerGraphicsInstance(shapeUid1, position, orientation, color, scaling);
9352 psb->setUserIndex(instanceUid);
9353
9354 if (m_data->m_enableTinyRenderer)
9355 {
9356 int texUid2 = m_data->m_pluginManager.getRenderInterface()->registerTexture(meshData.m_textureImage1, meshData.m_textureWidth, meshData.m_textureHeight);
9357 visualShape.m_tinyRendererTextureId = texUid2;
9358 int linkIndex = -1;
9359 int softBodyGraphicsShapeUid = m_data->m_pluginManager.getRenderInterface()->registerShapeAndInstance(
9360 visualShape,
9361 &meshData.m_gfxShape->m_vertices->at(0).xyzw[0],
9362 meshData.m_gfxShape->m_numvertices,
9363 &meshData.m_gfxShape->m_indices->at(0),
9364 meshData.m_gfxShape->m_numIndices,
9365 B3_GL_TRIANGLES,
9366 texUid2,
9367 psb->getBroadphaseHandle()->getUid(),
9368 *bodyUniqueId,
9369 linkIndex);
9370
9371 psb->setUserIndex3(softBodyGraphicsShapeUid);
9372 }
9373 delete meshData.m_gfxShape;
9374 meshData.m_gfxShape = 0;
9375 }
9376 else
9377 {
9378 //m_data->m_guiHelper->createCollisionShapeGraphicsObject(psb->getCollisionShape());
9379
9380 btAlignedObjectArray<GLInstanceVertex> gfxVertices;
9381 btAlignedObjectArray<int> indices;
9382 int strideInBytes = 9 * sizeof(float);
9383 gfxVertices.resize(psb->m_faces.size() * 3);
9384 for (int i = 0; i < psb->m_faces.size(); i++) // Foreach face
9385 {
9386 for (int k = 0; k < 3; k++) // Foreach vertex on a face
9387 {
9388 int currentIndex = i * 3 + k;
9389 for (int j = 0; j < 3; j++)
9390 {
9391 gfxVertices[currentIndex].xyzw[j] = psb->m_faces[i].m_n[k]->m_x[j];
9392 }
9393 for (int j = 0; j < 3; j++)
9394 {
9395 gfxVertices[currentIndex].normal[j] = psb->m_faces[i].m_n[k]->m_n[j];
9396 }
9397 for (int j = 0; j < 2; j++)
9398 {
9399 gfxVertices[currentIndex].uv[j] = btFabs(btFabs(10. * psb->m_faces[i].m_n[k]->m_x[j]));
9400 }
9401 indices.push_back(currentIndex);
9402 }
9403 }
9404 if (gfxVertices.size() && indices.size())
9405 {
9406 int red = 173;
9407 int green = 199;
9408 int blue = 255;
9409
9410 int texWidth = 256;
9411 int texHeight = 256;
9412 btAlignedObjectArray<unsigned char> texels;
9413 texels.resize(texWidth* texHeight * 3);
9414 for (int i = 0; i < texWidth * texHeight * 3; i++)
9415 texels[i] = 255;
9416 for (int i = 0; i < texWidth; i++)
9417 {
9418 for (int j = 0; j < texHeight; j++)
9419 {
9420 int a = i < texWidth / 2 ? 1 : 0;
9421 int b = j < texWidth / 2 ? 1 : 0;
9422
9423 if (a == b)
9424 {
9425 texels[(i + j * texWidth) * 3 + 0] = red;
9426 texels[(i + j * texWidth) * 3 + 1] = green;
9427 texels[(i + j * texWidth) * 3 + 2] = blue;
9428 }
9429 }
9430 }
9431
9432 int texId = m_data->m_guiHelper->registerTexture(&texels[0], texWidth, texHeight);
9433 visualShape.m_openglTextureId = texId;
9434 int shapeId = m_data->m_guiHelper->registerGraphicsShape(&gfxVertices[0].xyzw[0], gfxVertices.size(), &indices[0], indices.size(), B3_GL_TRIANGLES, texId);
9435 b3Assert(shapeId >= 0);
9436 psb->getCollisionShape()->setUserIndex(shapeId);
9437 if (m_data->m_enableTinyRenderer)
9438 {
9439
9440 int texUid2 = m_data->m_pluginManager.getRenderInterface()->registerTexture(&texels[0], texWidth, texHeight);
9441 visualShape.m_tinyRendererTextureId = texUid2;
9442 int linkIndex = -1;
9443 int softBodyGraphicsShapeUid = m_data->m_pluginManager.getRenderInterface()->registerShapeAndInstance(
9444 visualShape,
9445 &gfxVertices[0].xyzw[0], gfxVertices.size(), &indices[0], indices.size(), B3_GL_TRIANGLES, texUid2,
9446 psb->getBroadphaseHandle()->getUid(),
9447 *bodyUniqueId,
9448 linkIndex);
9449 psb->setUserIndex3(softBodyGraphicsShapeUid);
9450 }
9451 }
9452 }
9453
9454
9455
9456 btAlignedObjectArray<btVector3> vertices;
9457 btAlignedObjectArray<btVector3> normals;
9458 if (psb->m_renderNodes.size() == 0)
9459 {
9460 psb->m_renderNodes.resize(psb->m_faces.size()*3);
9461 vertices.resize(psb->m_faces.size() * 3);
9462 normals.resize(psb->m_faces.size() * 3);
9463
9464 for (int i = 0; i < psb->m_faces.size(); i++) // Foreach face
9465 {
9466
9467 for (int k = 0; k < 3; k++) // Foreach vertex on a face
9468 {
9469 int currentIndex = i * 3 + k;
9470 for (int j = 0; j < 3; j++)
9471 {
9472 psb->m_renderNodes[currentIndex].m_x[j] = psb->m_faces[i].m_n[k]->m_x[j];
9473 }
9474 for (int j = 0; j < 3; j++)
9475 {
9476 psb->m_renderNodes[currentIndex].m_normal[j] = psb->m_faces[i].m_n[k]->m_n[j];
9477 }
9478 for (int j = 0; j < 2; j++)
9479 {
9480 psb->m_renderNodes[currentIndex].m_uv1[j] = btFabs(10*psb->m_faces[i].m_n[k]->m_x[j]);
9481 }
9482 psb->m_renderNodes[currentIndex].m_uv1[2] = 0;
9483 vertices[currentIndex] = psb->m_faces[i].m_n[k]->m_x;
9484 normals[currentIndex] = psb->m_faces[i].m_n[k]->m_n;
9485 }
9486 }
9487 btSoftBodyHelpers::extrapolateBarycentricWeights(psb);
9488 }
9489 else
9490 {
9491 vertices.resize(psb->m_renderNodes.size());
9492 normals.resize(psb->m_renderNodes.size());
9493 for (int i = 0; i < psb->m_renderNodes.size(); i++) // Foreach face
9494 {
9495 vertices[i] = psb->m_renderNodes[i].m_x;
9496 normals[i] = psb->m_renderNodes[i].m_normal;
9497 }
9498 }
9499 m_data->m_pluginManager.getRenderInterface()->updateShape(psb->getUserIndex3(), &vertices[0], vertices.size(), &normals[0], normals.size());
9500
9501 if (!deformable.m_name.empty())
9502 {
9503 bodyHandle->m_bodyName = deformable.m_name;
9504 }
9505 else
9506 {
9507 int pos = strlen(relativeFileName) - 1;
9508 while (pos >= 0 && relativeFileName[pos] != '/')
9509 {
9510 pos--;
9511 }
9512 btAssert(strlen(relativeFileName) - pos - 5 > 0);
9513 std::string object_name(std::string(relativeFileName).substr(pos + 1, strlen(relativeFileName) - 5 - pos));
9514 bodyHandle->m_bodyName = object_name;
9515 }
9516 b3Notification notification;
9517 notification.m_notificationType = BODY_ADDED;
9518 notification.m_bodyArgs.m_bodyUniqueId = *bodyUniqueId;
9519 m_data->m_pluginManager.addNotification(notification);
9520 }
9521 #endif
9522 return true;
9523 }
9524
processLoadSoftBodyCommand(const struct SharedMemoryCommand & clientCmd,struct SharedMemoryStatus & serverStatusOut,char * bufferServerToClient,int bufferSizeInBytes)9525 bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
9526 {
9527 serverStatusOut.m_type = CMD_LOAD_SOFT_BODY_FAILED;
9528 bool hasStatus = true;
9529 #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
9530 UrdfDeformable deformable;
9531
9532 constructUrdfDeformable(clientCmd, deformable, m_data->m_verboseOutput);
9533 // const LoadSoftBodyArgs& loadSoftBodyArgs = clientCmd.m_loadSoftBodyArguments;
9534 btVector3 initialPos(0, 0, 0);
9535 if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_INITIAL_POSITION)
9536 {
9537 initialPos[0] = clientCmd.m_loadSoftBodyArguments.m_initialPosition[0];
9538 initialPos[1] = clientCmd.m_loadSoftBodyArguments.m_initialPosition[1];
9539 initialPos[2] = clientCmd.m_loadSoftBodyArguments.m_initialPosition[2];
9540 }
9541 btQuaternion initialOrn(0, 0, 0, 1);
9542 if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_INITIAL_ORIENTATION)
9543 {
9544 initialOrn[0] = clientCmd.m_loadSoftBodyArguments.m_initialOrientation[0];
9545 initialOrn[1] = clientCmd.m_loadSoftBodyArguments.m_initialOrientation[1];
9546 initialOrn[2] = clientCmd.m_loadSoftBodyArguments.m_initialOrientation[2];
9547 initialOrn[3] = clientCmd.m_loadSoftBodyArguments.m_initialOrientation[3];
9548 }
9549
9550 double scale = 1;
9551 if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_UPDATE_SCALE)
9552 {
9553 scale = clientCmd.m_loadSoftBodyArguments.m_scale;
9554 }
9555 bool use_self_collision = false;
9556 if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_USE_SELF_COLLISION)
9557 {
9558 use_self_collision = clientCmd.m_loadSoftBodyArguments.m_useSelfCollision;
9559 }
9560
9561 int bodyUniqueId = -1;
9562 bool completedOk = processDeformable(deformable, initialPos, initialOrn, &bodyUniqueId, bufferServerToClient, bufferSizeInBytes, scale, use_self_collision);
9563 if (completedOk && bodyUniqueId >= 0)
9564 {
9565 m_data->m_guiHelper->autogenerateGraphicsObjects(m_data->m_dynamicsWorld);
9566 serverStatusOut.m_type = CMD_LOAD_SOFT_BODY_COMPLETED;
9567
9568 int streamSizeInBytes = createBodyInfoStream(bodyUniqueId, bufferServerToClient, bufferSizeInBytes);
9569 serverStatusOut.m_numDataStreamBytes = streamSizeInBytes;
9570
9571 #ifdef ENABLE_LINK_MAPPER
9572 if (m_data->m_urdfLinkNameMapper.size())
9573 {
9574 serverStatusOut.m_numDataStreamBytes = m_data->m_urdfLinkNameMapper.at(m_data->m_urdfLinkNameMapper.size() - 1)->m_memSerializer->getCurrentBufferSize();
9575 }
9576 #endif
9577 serverStatusOut.m_dataStreamArguments.m_bodyUniqueId = bodyUniqueId;
9578 InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId);
9579 strcpy(serverStatusOut.m_dataStreamArguments.m_bodyName, body->m_bodyName.c_str());
9580 serverStatusOut.m_loadSoftBodyResultArguments.m_objectUniqueId = bodyUniqueId;
9581 }
9582 #endif
9583 return hasStatus;
9584 }
9585
processCreateSensorCommand(const struct SharedMemoryCommand & clientCmd,struct SharedMemoryStatus & serverStatusOut,char * bufferServerToClient,int bufferSizeInBytes)9586 bool PhysicsServerCommandProcessor::processCreateSensorCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
9587 {
9588 bool hasStatus = true;
9589 BT_PROFILE("CMD_CREATE_SENSOR");
9590
9591 if (m_data->m_verboseOutput)
9592 {
9593 b3Printf("Processed CMD_CREATE_SENSOR");
9594 }
9595 int bodyUniqueId = clientCmd.m_createSensorArguments.m_bodyUniqueId;
9596 InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId);
9597 if (body && body->m_multiBody)
9598 {
9599 btMultiBody* mb = body->m_multiBody;
9600 btAssert(mb);
9601 for (int i = 0; i < clientCmd.m_createSensorArguments.m_numJointSensorChanges; i++)
9602 {
9603 int jointIndex = clientCmd.m_createSensorArguments.m_jointIndex[i];
9604 if (clientCmd.m_createSensorArguments.m_enableJointForceSensor[i])
9605 {
9606 if (mb->getLink(jointIndex).m_jointFeedback)
9607 {
9608 b3Warning("CMD_CREATE_SENSOR: sensor for joint [%d] already enabled", jointIndex);
9609 }
9610 else
9611 {
9612 btMultiBodyJointFeedback* fb = new btMultiBodyJointFeedback();
9613 fb->m_reactionForces.setZero();
9614 mb->getLink(jointIndex).m_jointFeedback = fb;
9615 m_data->m_multiBodyJointFeedbacks.push_back(fb);
9616 };
9617 }
9618 else
9619 {
9620 if (mb->getLink(jointIndex).m_jointFeedback)
9621 {
9622 m_data->m_multiBodyJointFeedbacks.remove(mb->getLink(jointIndex).m_jointFeedback);
9623 delete mb->getLink(jointIndex).m_jointFeedback;
9624 mb->getLink(jointIndex).m_jointFeedback = 0;
9625 }
9626 else
9627 {
9628 b3Warning("CMD_CREATE_SENSOR: cannot perform sensor removal request, no sensor on joint [%d]", jointIndex);
9629 };
9630 }
9631 }
9632 }
9633 else
9634 {
9635 b3Warning("No btMultiBody in the world. btRigidBody/btTypedConstraint sensor not hooked up yet");
9636 }
9637
9638 #if 0
9639 //todo(erwincoumans) here is some sample code to hook up a force/torque sensor for btTypedConstraint/btRigidBody
9640 /*
9641 for (int i=0;i<m_data->m_dynamicsWorld->getNumConstraints();i++)
9642 {
9643 btTypedConstraint* c = m_data->m_dynamicsWorld->getConstraint(i);
9644 btJointFeedback* fb = new btJointFeedback();
9645 m_data->m_jointFeedbacks.push_back(fb);
9646 c->setJointFeedback(fb);
9647
9648
9649 }
9650 */
9651 #endif
9652
9653 serverStatusOut.m_type = CMD_CLIENT_COMMAND_COMPLETED;
9654 return hasStatus;
9655 }
9656
processProfileTimingCommand(const struct SharedMemoryCommand & clientCmd,struct SharedMemoryStatus & serverStatusOut,char * bufferServerToClient,int bufferSizeInBytes)9657 bool PhysicsServerCommandProcessor::processProfileTimingCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
9658 {
9659 bool hasStatus = true;
9660
9661 {
9662 if (clientCmd.m_profile.m_type == 0)
9663 {
9664 char** eventNamePtr = m_data->m_profileEvents[clientCmd.m_profile.m_name];
9665 char* eventName = 0;
9666 if (eventNamePtr)
9667 {
9668 eventName = *eventNamePtr;
9669 }
9670 else
9671 {
9672 int len = strlen(clientCmd.m_profile.m_name);
9673 eventName = new char[len + 1];
9674 strcpy(eventName, clientCmd.m_profile.m_name);
9675 eventName[len] = 0;
9676 m_data->m_profileEvents.insert(eventName, eventName);
9677 }
9678
9679 b3EnterProfileZone(eventName);
9680 }
9681 if (clientCmd.m_profile.m_type == 1)
9682 {
9683 b3LeaveProfileZone();
9684 }
9685 }
9686
9687 serverStatusOut.m_type = CMD_CLIENT_COMMAND_COMPLETED;
9688 hasStatus = true;
9689 return hasStatus;
9690 }
9691
setDefaultRootWorldAABB(SharedMemoryStatus & serverCmd)9692 void setDefaultRootWorldAABB(SharedMemoryStatus& serverCmd)
9693 {
9694 serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[0] = 0;
9695 serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[1] = 0;
9696 serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[2] = 0;
9697
9698 serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[0] = -1;
9699 serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[1] = -1;
9700 serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[2] = -1;
9701 }
9702
processRequestCollisionInfoCommand(const struct SharedMemoryCommand & clientCmd,struct SharedMemoryStatus & serverStatusOut,char * bufferServerToClient,int bufferSizeInBytes)9703 bool PhysicsServerCommandProcessor::processRequestCollisionInfoCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
9704 {
9705 bool hasStatus = true;
9706 SharedMemoryStatus& serverCmd = serverStatusOut;
9707 serverStatusOut.m_type = CMD_REQUEST_COLLISION_INFO_FAILED;
9708 hasStatus = true;
9709 int bodyUniqueId = clientCmd.m_requestCollisionInfoArgs.m_bodyUniqueId;
9710 InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId);
9711
9712 if (body && body->m_multiBody)
9713 {
9714 btMultiBody* mb = body->m_multiBody;
9715 serverStatusOut.m_type = CMD_REQUEST_COLLISION_INFO_COMPLETED;
9716 serverCmd.m_sendCollisionInfoArgs.m_numLinks = body->m_multiBody->getNumLinks();
9717 setDefaultRootWorldAABB(serverCmd);
9718
9719 if (body->m_multiBody->getBaseCollider())
9720 {
9721 btTransform tr;
9722 tr.setOrigin(mb->getBasePos());
9723 tr.setRotation(mb->getWorldToBaseRot().inverse());
9724
9725 btVector3 aabbMin, aabbMax;
9726 body->m_multiBody->getBaseCollider()->getCollisionShape()->getAabb(tr, aabbMin, aabbMax);
9727 serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[0] = aabbMin[0];
9728 serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[1] = aabbMin[1];
9729 serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[2] = aabbMin[2];
9730
9731 serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[0] = aabbMax[0];
9732 serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[1] = aabbMax[1];
9733 serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[2] = aabbMax[2];
9734 }
9735 for (int l = 0; l < mb->getNumLinks(); l++)
9736 {
9737 serverCmd.m_sendCollisionInfoArgs.m_linkWorldAABBsMin[3 * l + 0] = 0;
9738 serverCmd.m_sendCollisionInfoArgs.m_linkWorldAABBsMin[3 * l + 1] = 0;
9739 serverCmd.m_sendCollisionInfoArgs.m_linkWorldAABBsMin[3 * l + 2] = 0;
9740
9741 serverCmd.m_sendCollisionInfoArgs.m_linkWorldAABBsMax[3 * l + 0] = -1;
9742 serverCmd.m_sendCollisionInfoArgs.m_linkWorldAABBsMax[3 * l + 1] = -1;
9743 serverCmd.m_sendCollisionInfoArgs.m_linkWorldAABBsMax[3 * l + 2] = -1;
9744
9745 if (body->m_multiBody->getLink(l).m_collider)
9746 {
9747 btVector3 aabbMin, aabbMax;
9748 body->m_multiBody->getLinkCollider(l)->getCollisionShape()->getAabb(mb->getLink(l).m_cachedWorldTransform, aabbMin, aabbMax);
9749
9750 serverCmd.m_sendCollisionInfoArgs.m_linkWorldAABBsMin[3 * l + 0] = aabbMin[0];
9751 serverCmd.m_sendCollisionInfoArgs.m_linkWorldAABBsMin[3 * l + 1] = aabbMin[1];
9752 serverCmd.m_sendCollisionInfoArgs.m_linkWorldAABBsMin[3 * l + 2] = aabbMin[2];
9753 serverCmd.m_sendCollisionInfoArgs.m_linkWorldAABBsMax[3 * l + 0] = aabbMax[0];
9754 serverCmd.m_sendCollisionInfoArgs.m_linkWorldAABBsMax[3 * l + 1] = aabbMax[1];
9755 serverCmd.m_sendCollisionInfoArgs.m_linkWorldAABBsMax[3 * l + 2] = aabbMax[2];
9756 }
9757 }
9758 }
9759 else if (body && body->m_rigidBody)
9760 {
9761 btRigidBody* rb = body->m_rigidBody;
9762 SharedMemoryStatus& serverCmd = serverStatusOut;
9763 serverStatusOut.m_type = CMD_REQUEST_COLLISION_INFO_COMPLETED;
9764 serverCmd.m_sendCollisionInfoArgs.m_numLinks = 0;
9765 setDefaultRootWorldAABB(serverCmd);
9766
9767 if (rb->getCollisionShape())
9768 {
9769 btTransform tr = rb->getWorldTransform();
9770
9771 btVector3 aabbMin, aabbMax;
9772 rb->getCollisionShape()->getAabb(tr, aabbMin, aabbMax);
9773 serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[0] = aabbMin[0];
9774 serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[1] = aabbMin[1];
9775 serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[2] = aabbMin[2];
9776
9777 serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[0] = aabbMax[0];
9778 serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[1] = aabbMax[1];
9779 serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[2] = aabbMax[2];
9780 }
9781 }
9782 #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
9783 else if (body && body->m_softBody)
9784 {
9785 btSoftBody* sb = body->m_softBody;
9786 serverCmd.m_sendCollisionInfoArgs.m_numLinks = 0;
9787 btVector3 aabbMin, aabbMax;
9788 sb->getAabb(aabbMin, aabbMax);
9789
9790 serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[0] = aabbMin[0];
9791 serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[1] = aabbMin[1];
9792 serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[2] = aabbMin[2];
9793
9794 serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[0] = aabbMax[0];
9795 serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[1] = aabbMax[1];
9796 serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[2] = aabbMax[2];
9797
9798 SharedMemoryStatus& serverCmd = serverStatusOut;
9799 serverStatusOut.m_type = CMD_REQUEST_COLLISION_INFO_COMPLETED;
9800 }
9801 #endif
9802 return hasStatus;
9803 }
9804
performCollisionDetectionCommand(const struct SharedMemoryCommand & clientCmd,struct SharedMemoryStatus & serverStatusOut,char * bufferServerToClient,int bufferSizeInBytes)9805 bool PhysicsServerCommandProcessor::performCollisionDetectionCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
9806 {
9807 bool hasStatus = true;
9808
9809 BT_PROFILE("CMD_PERFORM_COLLISION_DETECTION");
9810
9811 if (m_data->m_verboseOutput)
9812 {
9813 b3Printf("Perform Collision Detection command");
9814 b3Printf("CMD_PERFORM_COLLISION_DETECTION clientCmd = %d\n", clientCmd.m_sequenceNumber);
9815 }
9816
9817 m_data->m_dynamicsWorld->performDiscreteCollisionDetection();
9818 SharedMemoryStatus& serverCmd = serverStatusOut;
9819 serverCmd.m_type = CMD_PERFORM_COLLISION_DETECTION_COMPLETED;
9820 return true;
9821 }
9822
processForwardDynamicsCommand(const struct SharedMemoryCommand & clientCmd,struct SharedMemoryStatus & serverStatusOut,char * bufferServerToClient,int bufferSizeInBytes)9823 bool PhysicsServerCommandProcessor::processForwardDynamicsCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
9824 {
9825 bool hasStatus = true;
9826
9827 BT_PROFILE("CMD_STEP_FORWARD_SIMULATION");
9828
9829 if (m_data->m_verboseOutput)
9830 {
9831 b3Printf("Step simulation request");
9832 b3Printf("CMD_STEP_FORWARD_SIMULATION clientCmd = %d\n", clientCmd.m_sequenceNumber);
9833 }
9834 #ifndef USE_DISCRETE_DYNAMICS_WORLD
9835 ///todo(erwincoumans) move this damping inside Bullet
9836 for (int i = 0; i < m_data->m_dynamicsWorld->getNumMultibodies(); i++)
9837 {
9838 btMultiBody* mb = m_data->m_dynamicsWorld->getMultiBody(i);
9839 for (int l = 0; l < mb->getNumLinks(); l++)
9840 {
9841 for (int d = 0; d < mb->getLink(l).m_dofCount; d++)
9842 {
9843 double damping_coefficient = mb->getLink(l).m_jointDamping;
9844 double damping = -damping_coefficient * mb->getJointVelMultiDof(l)[d];
9845 mb->addJointTorqueMultiDof(l, d, damping);
9846 }
9847 }
9848 }
9849 #endif
9850 btScalar deltaTimeScaled = m_data->m_physicsDeltaTime * simTimeScalingFactor;
9851
9852 int numSteps = 0;
9853 if (m_data->m_numSimulationSubSteps > 0)
9854 {
9855 numSteps = m_data->m_dynamicsWorld->stepSimulation(deltaTimeScaled, m_data->m_numSimulationSubSteps, m_data->m_physicsDeltaTime / m_data->m_numSimulationSubSteps);
9856 m_data->m_simulationTimestamp += deltaTimeScaled;
9857 }
9858 else
9859 {
9860 numSteps = m_data->m_dynamicsWorld->stepSimulation(deltaTimeScaled, 0);
9861 m_data->m_simulationTimestamp += deltaTimeScaled;
9862 }
9863
9864 if (numSteps > 0)
9865 {
9866 addBodyChangedNotifications();
9867 }
9868
9869 SharedMemoryStatus& serverCmd = serverStatusOut;
9870
9871 serverCmd.m_forwardDynamicsAnalyticsArgs.m_numSteps = numSteps;
9872
9873 btAlignedObjectArray<btSolverAnalyticsData> islandAnalyticsData;
9874 #ifndef USE_DISCRETE_DYNAMICS_WORLD
9875 m_data->m_dynamicsWorld->getAnalyticsData(islandAnalyticsData);
9876 #endif
9877 serverCmd.m_forwardDynamicsAnalyticsArgs.m_numIslands = islandAnalyticsData.size();
9878 int numIslands = btMin(islandAnalyticsData.size(), MAX_ISLANDS_ANALYTICS);
9879
9880 for (int i = 0; i < numIslands; i++)
9881 {
9882 serverCmd.m_forwardDynamicsAnalyticsArgs.m_numSolverCalls = islandAnalyticsData[i].m_numSolverCalls;
9883 serverCmd.m_forwardDynamicsAnalyticsArgs.m_islandData[i].m_islandId = islandAnalyticsData[i].m_islandId;
9884 serverCmd.m_forwardDynamicsAnalyticsArgs.m_islandData[i].m_numBodies = islandAnalyticsData[i].m_numBodies;
9885 serverCmd.m_forwardDynamicsAnalyticsArgs.m_islandData[i].m_numIterationsUsed = islandAnalyticsData[i].m_numIterationsUsed;
9886 serverCmd.m_forwardDynamicsAnalyticsArgs.m_islandData[i].m_remainingLeastSquaresResidual = islandAnalyticsData[i].m_remainingLeastSquaresResidual;
9887 serverCmd.m_forwardDynamicsAnalyticsArgs.m_islandData[i].m_numContactManifolds = islandAnalyticsData[i].m_numContactManifolds;
9888 }
9889 serverCmd.m_type = CMD_STEP_FORWARD_SIMULATION_COMPLETED;
9890
9891 m_data->m_remoteSyncTransformTime += deltaTimeScaled;
9892 if (m_data->m_remoteSyncTransformTime >= m_data->m_remoteSyncTransformInterval)
9893 {
9894 m_data->m_remoteSyncTransformTime = 0;
9895 syncPhysicsToGraphics2();
9896 }
9897
9898 return hasStatus;
9899 }
9900
processRequestInternalDataCommand(const struct SharedMemoryCommand & clientCmd,struct SharedMemoryStatus & serverStatusOut,char * bufferServerToClient,int bufferSizeInBytes)9901 bool PhysicsServerCommandProcessor::processRequestInternalDataCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
9902 {
9903 bool hasStatus = true;
9904 BT_PROFILE("CMD_REQUEST_INTERNAL_DATA");
9905
9906 //todo: also check version etc?
9907
9908 SharedMemoryStatus& serverCmd = serverStatusOut;
9909 serverCmd.m_type = CMD_REQUEST_INTERNAL_DATA_FAILED;
9910
9911 int sz = btDefaultSerializer::getMemoryDnaSizeInBytes();
9912 const char* memDna = btDefaultSerializer::getMemoryDna();
9913 if (sz < bufferSizeInBytes)
9914 {
9915 for (int i = 0; i < sz; i++)
9916 {
9917 bufferServerToClient[i] = memDna[i];
9918 }
9919 serverCmd.m_type = CMD_REQUEST_INTERNAL_DATA_COMPLETED;
9920 serverCmd.m_numDataStreamBytes = sz;
9921 }
9922 return hasStatus;
9923 }
9924
processChangeDynamicsInfoCommand(const struct SharedMemoryCommand & clientCmd,struct SharedMemoryStatus & serverStatusOut,char * bufferServerToClient,int bufferSizeInBytes)9925 bool PhysicsServerCommandProcessor::processChangeDynamicsInfoCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
9926 {
9927 bool hasStatus = true;
9928 BT_PROFILE("CMD_CHANGE_DYNAMICS_INFO");
9929
9930 int bodyUniqueId = clientCmd.m_changeDynamicsInfoArgs.m_bodyUniqueId;
9931 int linkIndex = clientCmd.m_changeDynamicsInfoArgs.m_linkIndex;
9932 double mass = clientCmd.m_changeDynamicsInfoArgs.m_mass;
9933 double lateralFriction = clientCmd.m_changeDynamicsInfoArgs.m_lateralFriction;
9934 double spinningFriction = clientCmd.m_changeDynamicsInfoArgs.m_spinningFriction;
9935 double rollingFriction = clientCmd.m_changeDynamicsInfoArgs.m_rollingFriction;
9936 double restitution = clientCmd.m_changeDynamicsInfoArgs.m_restitution;
9937 btVector3 newLocalInertiaDiagonal(clientCmd.m_changeDynamicsInfoArgs.m_localInertiaDiagonal[0],
9938 clientCmd.m_changeDynamicsInfoArgs.m_localInertiaDiagonal[1],
9939 clientCmd.m_changeDynamicsInfoArgs.m_localInertiaDiagonal[2]);
9940 btVector3 anisotropicFriction(clientCmd.m_changeDynamicsInfoArgs.m_anisotropicFriction[0],
9941 clientCmd.m_changeDynamicsInfoArgs.m_anisotropicFriction[1],
9942 clientCmd.m_changeDynamicsInfoArgs.m_anisotropicFriction[2]);
9943
9944 if (bodyUniqueId >= 0)
9945 {
9946 InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId);
9947 #ifndef USE_DISCRETE_DYNAMICS_WORLD
9948 if (body && body->m_multiBody)
9949 {
9950 btMultiBody* mb = body->m_multiBody;
9951
9952 if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ACTIVATION_STATE)
9953 {
9954 if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateWakeUp)
9955 {
9956 mb->wakeUp();
9957 }
9958 if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateSleep)
9959 {
9960 mb->goToSleep();
9961 }
9962 if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateEnableSleeping)
9963 {
9964 mb->setCanSleep(true);
9965 }
9966 if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateDisableSleeping)
9967 {
9968 mb->setCanSleep(false);
9969 }
9970 if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateEnableWakeup)
9971 {
9972 mb->setCanWakeup(true);
9973 }
9974 if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateDisableWakeup)
9975 {
9976 mb->setCanWakeup(false);
9977 }
9978 }
9979
9980 if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LINEAR_DAMPING)
9981 {
9982 mb->setLinearDamping(clientCmd.m_changeDynamicsInfoArgs.m_linearDamping);
9983 }
9984 if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ANGULAR_DAMPING)
9985 {
9986 mb->setAngularDamping(clientCmd.m_changeDynamicsInfoArgs.m_angularDamping);
9987 }
9988
9989 if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_SLEEP_THRESHOLD)
9990 {
9991 mb->setSleepThreshold(clientCmd.m_changeDynamicsInfoArgs.m_sleepThreshold);
9992 }
9993
9994 if (linkIndex == -1)
9995 {
9996 if (mb->getBaseCollider())
9997 {
9998 if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_RESTITUTION)
9999 {
10000 mb->getBaseCollider()->setRestitution(restitution);
10001 }
10002
10003 if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_CONTACT_STIFFNESS_AND_DAMPING)
10004 {
10005 mb->getBaseCollider()->setContactStiffnessAndDamping(clientCmd.m_changeDynamicsInfoArgs.m_contactStiffness, clientCmd.m_changeDynamicsInfoArgs.m_contactDamping);
10006 }
10007 if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LATERAL_FRICTION)
10008 {
10009 mb->getBaseCollider()->setFriction(lateralFriction);
10010 }
10011 if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_SPINNING_FRICTION)
10012 {
10013 mb->getBaseCollider()->setSpinningFriction(spinningFriction);
10014 }
10015 if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ROLLING_FRICTION)
10016 {
10017 mb->getBaseCollider()->setRollingFriction(rollingFriction);
10018 }
10019
10020 if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_FRICTION_ANCHOR)
10021 {
10022 if (clientCmd.m_changeDynamicsInfoArgs.m_frictionAnchor)
10023 {
10024 mb->getBaseCollider()->setCollisionFlags(mb->getBaseCollider()->getCollisionFlags() | btCollisionObject::CF_HAS_FRICTION_ANCHOR);
10025 }
10026 else
10027 {
10028 mb->getBaseCollider()->setCollisionFlags(mb->getBaseCollider()->getCollisionFlags() & ~btCollisionObject::CF_HAS_FRICTION_ANCHOR);
10029 }
10030 }
10031 }
10032 if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_MASS)
10033 {
10034 mb->setBaseMass(mass);
10035 if (mb->getBaseCollider() && mb->getBaseCollider()->getCollisionShape())
10036 {
10037 btVector3 localInertia;
10038 mb->getBaseCollider()->getCollisionShape()->calculateLocalInertia(mass, localInertia);
10039 mb->setBaseInertia(localInertia);
10040 }
10041
10042 //handle switch from static/fixedBase to dynamic and vise-versa
10043 if (mass > 0)
10044 {
10045 bool isDynamic = true;
10046 if (mb->hasFixedBase())
10047 {
10048 int collisionFilterGroup = isDynamic ? int(btBroadphaseProxy::DefaultFilter) : int(btBroadphaseProxy::StaticFilter);
10049 int collisionFilterMask = isDynamic ? int(btBroadphaseProxy::AllFilter) : int(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
10050
10051 m_data->m_dynamicsWorld->removeCollisionObject(mb->getBaseCollider());
10052 int oldFlags = mb->getBaseCollider()->getCollisionFlags();
10053 mb->getBaseCollider()->setCollisionFlags(oldFlags & ~btCollisionObject::CF_STATIC_OBJECT);
10054 mb->setFixedBase(false);
10055 m_data->m_dynamicsWorld->addCollisionObject(mb->getBaseCollider(), collisionFilterGroup, collisionFilterMask);
10056 }
10057 }
10058 else
10059 {
10060 if (!mb->hasFixedBase())
10061 {
10062 bool isDynamic = false;
10063 int collisionFilterGroup = isDynamic ? int(btBroadphaseProxy::DefaultFilter) : int(btBroadphaseProxy::StaticFilter);
10064 int collisionFilterMask = isDynamic ? int(btBroadphaseProxy::AllFilter) : int(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
10065 int oldFlags = mb->getBaseCollider()->getCollisionFlags();
10066 mb->getBaseCollider()->setCollisionFlags(oldFlags | btCollisionObject::CF_STATIC_OBJECT);
10067 m_data->m_dynamicsWorld->removeCollisionObject(mb->getBaseCollider());
10068 mb->setFixedBase(true);
10069 m_data->m_dynamicsWorld->addCollisionObject(mb->getBaseCollider(), collisionFilterGroup, collisionFilterMask);
10070 }
10071 }
10072 }
10073 if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LOCAL_INERTIA_DIAGONAL)
10074 {
10075 mb->setBaseInertia(newLocalInertiaDiagonal);
10076 }
10077 if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ANISOTROPIC_FRICTION)
10078 {
10079 mb->getBaseCollider()->setAnisotropicFriction(anisotropicFriction);
10080 }
10081
10082 if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_CONTACT_PROCESSING_THRESHOLD)
10083 {
10084 mb->getBaseCollider()->setContactProcessingThreshold(clientCmd.m_changeDynamicsInfoArgs.m_contactProcessingThreshold);
10085 }
10086
10087 if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_MAX_JOINT_VELOCITY)
10088 {
10089 mb->setMaxCoordinateVelocity(clientCmd.m_changeDynamicsInfoArgs.m_maxJointVelocity);
10090 }
10091 if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_COLLISION_MARGIN)
10092 {
10093 mb->getBaseCollider()->getCollisionShape()->setMargin(clientCmd.m_changeDynamicsInfoArgs.m_collisionMargin);
10094 if (mb->getBaseCollider()->getCollisionShape()->isCompound())
10095 {
10096 btCompoundShape* compound = (btCompoundShape*)mb->getBaseCollider()->getCollisionShape();
10097 for (int s = 0; s < compound->getNumChildShapes(); s++)
10098 {
10099 compound->getChildShape(s)->setMargin(clientCmd.m_changeDynamicsInfoArgs.m_collisionMargin);
10100 }
10101 }
10102 }
10103 if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_DYNAMIC_TYPE)
10104 {
10105 int dynamic_type = clientCmd.m_changeDynamicsInfoArgs.m_dynamicType;
10106 mb->setBaseDynamicType(dynamic_type);
10107
10108 bool isDynamic = dynamic_type == eDynamic;
10109 int collisionFilterGroup = isDynamic ? int(btBroadphaseProxy::DefaultFilter) : int(btBroadphaseProxy::StaticFilter);
10110 int collisionFilterMask = isDynamic ? int(btBroadphaseProxy::AllFilter) : int(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
10111 m_data->m_dynamicsWorld->removeCollisionObject(mb->getBaseCollider());
10112 m_data->m_dynamicsWorld->addCollisionObject(mb->getBaseCollider(), collisionFilterGroup, collisionFilterMask);
10113 }
10114 }
10115 else
10116 {
10117 if (linkIndex >= 0 && linkIndex < mb->getNumLinks())
10118 {
10119
10120 if ((clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_JOINT_LIMIT_MAX_FORCE) ||
10121 (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_JOINT_LIMITS))
10122 {
10123
10124 btMultiBodyJointLimitConstraint* limC = 0;
10125
10126 int numConstraints = m_data->m_dynamicsWorld->getNumMultiBodyConstraints();
10127 for (int c = 0; c < numConstraints; c++)
10128 {
10129 btMultiBodyConstraint* mbc = m_data->m_dynamicsWorld->getMultiBodyConstraint(c);
10130 if (mbc->getConstraintType() == MULTIBODY_CONSTRAINT_LIMIT)
10131 {
10132 if ((mbc->getMultiBodyA() == mb) && (mbc->getLinkA() == linkIndex))
10133 {
10134 limC = (btMultiBodyJointLimitConstraint*)mbc;
10135 }
10136 }
10137 }
10138
10139
10140 if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_JOINT_LIMITS)
10141 {
10142 //find a joint limit
10143 btScalar prevUpper = mb->getLink(linkIndex).m_jointUpperLimit;
10144 btScalar prevLower = mb->getLink(linkIndex).m_jointLowerLimit;
10145 btScalar lower = clientCmd.m_changeDynamicsInfoArgs.m_jointLowerLimit;
10146 btScalar upper = clientCmd.m_changeDynamicsInfoArgs.m_jointUpperLimit;
10147 bool enableLimit = lower <= upper;
10148
10149 if (enableLimit)
10150 {
10151 if (limC == 0)
10152 {
10153 limC = new btMultiBodyJointLimitConstraint(mb, linkIndex, lower, upper);
10154 m_data->m_dynamicsWorld->addMultiBodyConstraint(limC);
10155 }
10156 else
10157 {
10158 limC->setLowerBound(lower);
10159 limC->setUpperBound(upper);
10160 }
10161 mb->getLink(linkIndex).m_jointLowerLimit = lower;
10162 mb->getLink(linkIndex).m_jointUpperLimit = upper;
10163 }
10164 else
10165 {
10166 if (limC)
10167 {
10168 m_data->m_dynamicsWorld->removeMultiBodyConstraint(limC);
10169 delete limC;
10170 limC = 0;
10171 }
10172 mb->getLink(linkIndex).m_jointLowerLimit = 1;
10173 mb->getLink(linkIndex).m_jointUpperLimit = -1;
10174 }
10175 }
10176
10177 if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_JOINT_LIMIT_MAX_FORCE)
10178 {
10179 btScalar fixedTimeSubStep = m_data->m_numSimulationSubSteps > 0 ? m_data->m_physicsDeltaTime / m_data->m_numSimulationSubSteps : m_data->m_physicsDeltaTime;
10180 btScalar maxImpulse = clientCmd.m_changeDynamicsInfoArgs.m_jointLimitForce * fixedTimeSubStep;
10181 if (limC)
10182 {
10183 //convert from force to impulse
10184 limC->setMaxAppliedImpulse(maxImpulse);
10185 }
10186 }
10187 }
10188
10189 if (mb->getLinkCollider(linkIndex))
10190 {
10191 if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_RESTITUTION)
10192 {
10193 mb->getLinkCollider(linkIndex)->setRestitution(restitution);
10194 }
10195 if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_SPINNING_FRICTION)
10196 {
10197 mb->getLinkCollider(linkIndex)->setSpinningFriction(spinningFriction);
10198 }
10199 if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ROLLING_FRICTION)
10200 {
10201 mb->getLinkCollider(linkIndex)->setRollingFriction(rollingFriction);
10202 }
10203
10204 if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_FRICTION_ANCHOR)
10205 {
10206 if (clientCmd.m_changeDynamicsInfoArgs.m_frictionAnchor)
10207 {
10208 mb->getLinkCollider(linkIndex)->setCollisionFlags(mb->getLinkCollider(linkIndex)->getCollisionFlags() | btCollisionObject::CF_HAS_FRICTION_ANCHOR);
10209 }
10210 else
10211 {
10212 mb->getLinkCollider(linkIndex)->setCollisionFlags(mb->getLinkCollider(linkIndex)->getCollisionFlags() & ~btCollisionObject::CF_HAS_FRICTION_ANCHOR);
10213 }
10214 }
10215
10216 if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LATERAL_FRICTION)
10217 {
10218 mb->getLinkCollider(linkIndex)->setFriction(lateralFriction);
10219 }
10220
10221 if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_CONTACT_STIFFNESS_AND_DAMPING)
10222 {
10223 mb->getLinkCollider(linkIndex)->setContactStiffnessAndDamping(clientCmd.m_changeDynamicsInfoArgs.m_contactStiffness, clientCmd.m_changeDynamicsInfoArgs.m_contactDamping);
10224 }
10225 if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_COLLISION_MARGIN)
10226 {
10227 mb->getLinkCollider(linkIndex)->getCollisionShape()->setMargin(clientCmd.m_changeDynamicsInfoArgs.m_collisionMargin);
10228 }
10229 }
10230
10231 if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_JOINT_DAMPING)
10232 {
10233 mb->getLink(linkIndex).m_jointDamping = clientCmd.m_changeDynamicsInfoArgs.m_jointDamping;
10234 }
10235
10236 if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_MASS)
10237 {
10238 mb->getLink(linkIndex).m_mass = mass;
10239 if (mb->getLinkCollider(linkIndex) && mb->getLinkCollider(linkIndex)->getCollisionShape())
10240 {
10241 btVector3 localInertia;
10242 mb->getLinkCollider(linkIndex)->getCollisionShape()->calculateLocalInertia(mass, localInertia);
10243 mb->getLink(linkIndex).m_inertiaLocal = localInertia;
10244 }
10245 }
10246 if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LOCAL_INERTIA_DIAGONAL)
10247 {
10248 mb->getLink(linkIndex).m_inertiaLocal = newLocalInertiaDiagonal;
10249 }
10250 if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ANISOTROPIC_FRICTION)
10251 {
10252 mb->getLinkCollider(linkIndex)->setAnisotropicFriction(anisotropicFriction);
10253 }
10254 if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_CONTACT_PROCESSING_THRESHOLD)
10255 {
10256 mb->getLinkCollider(linkIndex)->setContactProcessingThreshold(clientCmd.m_changeDynamicsInfoArgs.m_contactProcessingThreshold);
10257 }
10258 if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_DYNAMIC_TYPE)
10259 {
10260 int dynamic_type = clientCmd.m_changeDynamicsInfoArgs.m_dynamicType;
10261 mb->setLinkDynamicType(linkIndex, dynamic_type);
10262
10263 bool isDynamic = dynamic_type == eDynamic;
10264 int collisionFilterGroup = isDynamic ? int(btBroadphaseProxy::DefaultFilter) : int(btBroadphaseProxy::StaticFilter);
10265 int collisionFilterMask = isDynamic ? int(btBroadphaseProxy::AllFilter) : int(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
10266 m_data->m_dynamicsWorld->removeCollisionObject(mb->getLinkCollider(linkIndex));
10267 m_data->m_dynamicsWorld->addCollisionObject(mb->getLinkCollider(linkIndex), collisionFilterGroup, collisionFilterMask);
10268 }
10269 }
10270 }
10271 }
10272 else
10273 #endif
10274 {
10275 btRigidBody* rb = 0;
10276 if (body && body->m_rigidBody)
10277 {
10278
10279 if (linkIndex == -1)
10280 {
10281 rb = body->m_rigidBody;
10282 }
10283 else
10284 {
10285 if (linkIndex >= 0 && linkIndex < body->m_rigidBodyJoints.size())
10286 {
10287 btRigidBody* parentRb = &body->m_rigidBodyJoints[linkIndex]->getRigidBodyA();
10288 btRigidBody* childRb = &body->m_rigidBodyJoints[linkIndex]->getRigidBodyB();
10289 rb = childRb;
10290 }
10291 }
10292 }
10293
10294 if (rb)
10295 {
10296 if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ACTIVATION_STATE)
10297 {
10298 if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateEnableSleeping)
10299 {
10300 rb->forceActivationState(ACTIVE_TAG);
10301 }
10302 if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateDisableSleeping)
10303 {
10304 rb->forceActivationState(DISABLE_DEACTIVATION);
10305 }
10306 if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateWakeUp)
10307 {
10308 rb->forceActivationState(ACTIVE_TAG);
10309 rb->setDeactivationTime(0.0);
10310 }
10311 if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateSleep)
10312 {
10313 rb->forceActivationState(ISLAND_SLEEPING);
10314 }
10315 }
10316
10317 if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LINEAR_DAMPING)
10318 {
10319 btScalar angDamping = rb->getAngularDamping();
10320 rb->setDamping(clientCmd.m_changeDynamicsInfoArgs.m_linearDamping, angDamping);
10321 }
10322 if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ANGULAR_DAMPING)
10323 {
10324 btScalar linDamping = rb->getLinearDamping();
10325 rb->setDamping(linDamping, clientCmd.m_changeDynamicsInfoArgs.m_angularDamping);
10326 }
10327
10328 if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_CONTACT_STIFFNESS_AND_DAMPING)
10329 {
10330 rb->setContactStiffnessAndDamping(clientCmd.m_changeDynamicsInfoArgs.m_contactStiffness, clientCmd.m_changeDynamicsInfoArgs.m_contactDamping);
10331 }
10332 if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_RESTITUTION)
10333 {
10334 rb->setRestitution(restitution);
10335 }
10336 if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LATERAL_FRICTION)
10337 {
10338 rb->setFriction(lateralFriction);
10339 }
10340 if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_SPINNING_FRICTION)
10341 {
10342 rb->setSpinningFriction(spinningFriction);
10343 }
10344 if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ROLLING_FRICTION)
10345 {
10346 rb->setRollingFriction(rollingFriction);
10347 }
10348
10349 if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_FRICTION_ANCHOR)
10350 {
10351 if (clientCmd.m_changeDynamicsInfoArgs.m_frictionAnchor)
10352 {
10353 rb->setCollisionFlags(rb->getCollisionFlags() | btCollisionObject::CF_HAS_FRICTION_ANCHOR);
10354 }
10355 else
10356 {
10357 rb->setCollisionFlags(rb->getCollisionFlags() & ~btCollisionObject::CF_HAS_FRICTION_ANCHOR);
10358 }
10359 }
10360
10361 if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_MASS)
10362 {
10363 btVector3 localInertia;
10364 if (rb->getCollisionShape())
10365 {
10366 rb->getCollisionShape()->calculateLocalInertia(mass, localInertia);
10367 }
10368 rb->setMassProps(mass, localInertia);
10369 }
10370 if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LOCAL_INERTIA_DIAGONAL)
10371 {
10372 btScalar orgMass = rb->getInvMass();
10373 if (orgMass > 0)
10374 {
10375 rb->setMassProps(mass, newLocalInertiaDiagonal);
10376 }
10377 }
10378 if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ANISOTROPIC_FRICTION)
10379 {
10380 rb->setAnisotropicFriction(anisotropicFriction);
10381 }
10382
10383 if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_CONTACT_PROCESSING_THRESHOLD)
10384 {
10385 rb->setContactProcessingThreshold(clientCmd.m_changeDynamicsInfoArgs.m_contactProcessingThreshold);
10386 }
10387
10388 if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_CCD_SWEPT_SPHERE_RADIUS)
10389 {
10390 rb->setCcdSweptSphereRadius(clientCmd.m_changeDynamicsInfoArgs.m_ccdSweptSphereRadius);
10391 //for a given sphere radius, use a motion threshold of half the radius, before the ccd algorithm is enabled
10392 rb->setCcdMotionThreshold(clientCmd.m_changeDynamicsInfoArgs.m_ccdSweptSphereRadius / 2.);
10393 }
10394 if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_COLLISION_MARGIN)
10395 {
10396 rb->getCollisionShape()->setMargin(clientCmd.m_changeDynamicsInfoArgs.m_collisionMargin);
10397 }
10398 if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_DYNAMIC_TYPE)
10399 {
10400 int dynamic_type = clientCmd.m_changeDynamicsInfoArgs.m_dynamicType;
10401 // If mass is zero, the object cannot be set to be dynamic.
10402 if (!(rb->getInvMass() != btScalar(0.) || dynamic_type != eDynamic)) {
10403 int collision_flags = rb->getCollisionFlags();
10404 collision_flags &= ~(btCollisionObject::CF_STATIC_OBJECT | btCollisionObject::CF_KINEMATIC_OBJECT);
10405 collision_flags |= dynamic_type;
10406 rb->setCollisionFlags(collision_flags);
10407 bool isDynamic = dynamic_type == eDynamic;
10408 int collisionFilterGroup = isDynamic ? int(btBroadphaseProxy::DefaultFilter) : int(btBroadphaseProxy::StaticFilter);
10409 int collisionFilterMask = isDynamic ? int(btBroadphaseProxy::AllFilter) : int(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
10410 m_data->m_dynamicsWorld->removeCollisionObject(rb);
10411 m_data->m_dynamicsWorld->addCollisionObject(rb, collisionFilterGroup, collisionFilterMask);
10412 }
10413 }
10414
10415 if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_SLEEP_THRESHOLD)
10416 {
10417 btScalar threshold2 = btSqrt(clientCmd.m_changeDynamicsInfoArgs.m_sleepThreshold);
10418 rb->setSleepingThresholds(threshold2,threshold2);
10419 }
10420
10421 }
10422 }
10423 #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
10424 if (body && body->m_softBody)
10425 {
10426 btSoftBody* psb = body->m_softBody;
10427 if (psb)
10428 {
10429 if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ACTIVATION_STATE)
10430 {
10431 if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateEnableSleeping)
10432 {
10433 psb->forceActivationState(ACTIVE_TAG);
10434 }
10435 if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateDisableSleeping)
10436 {
10437 psb->forceActivationState(DISABLE_DEACTIVATION);
10438 }
10439 if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateWakeUp)
10440 {
10441 psb->forceActivationState(ACTIVE_TAG);
10442 psb->setDeactivationTime(0.0);
10443 }
10444 if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateSleep)
10445 {
10446 psb->forceActivationState(ISLAND_SLEEPING);
10447 }
10448 }
10449 }
10450 }
10451 #endif
10452 }
10453 SharedMemoryStatus& serverCmd = serverStatusOut;
10454 serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED;
10455
10456 b3Notification notification;
10457 notification.m_notificationType = LINK_DYNAMICS_CHANGED;
10458 notification.m_linkArgs.m_bodyUniqueId = bodyUniqueId;
10459 notification.m_linkArgs.m_linkIndex = linkIndex;
10460 m_data->m_pluginManager.addNotification(notification);
10461
10462 return hasStatus;
10463 }
10464
processSetAdditionalSearchPathCommand(const struct SharedMemoryCommand & clientCmd,struct SharedMemoryStatus & serverStatusOut,char * bufferServerToClient,int bufferSizeInBytes)10465 bool PhysicsServerCommandProcessor::processSetAdditionalSearchPathCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
10466 {
10467 bool hasStatus = true;
10468
10469 BT_PROFILE("CMD_SET_ADDITIONAL_SEARCH_PATH");
10470 b3ResourcePath::setAdditionalSearchPath(clientCmd.m_searchPathArgs.m_path);
10471 serverStatusOut.m_type = CMD_CLIENT_COMMAND_COMPLETED;
10472 return hasStatus;
10473 }
10474
processGetDynamicsInfoCommand(const struct SharedMemoryCommand & clientCmd,struct SharedMemoryStatus & serverStatusOut,char * bufferServerToClient,int bufferSizeInBytes)10475 bool PhysicsServerCommandProcessor::processGetDynamicsInfoCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
10476 {
10477 bool hasStatus = true;
10478 SharedMemoryStatus& serverCmd = serverStatusOut;
10479 serverCmd.m_type = CMD_GET_DYNAMICS_INFO_FAILED;
10480
10481 int bodyUniqueId = clientCmd.m_getDynamicsInfoArgs.m_bodyUniqueId;
10482 int linkIndex = clientCmd.m_getDynamicsInfoArgs.m_linkIndex;
10483 InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId);
10484 if (body && body->m_multiBody)
10485 {
10486 SharedMemoryStatus& serverCmd = serverStatusOut;
10487 serverCmd.m_type = CMD_GET_DYNAMICS_INFO_COMPLETED;
10488 serverCmd.m_dynamicsInfo.m_bodyType = BT_MULTI_BODY;
10489
10490 btMultiBody* mb = body->m_multiBody;
10491 if (linkIndex == -1)
10492 {
10493 serverCmd.m_dynamicsInfo.m_mass = mb->getBaseMass();
10494 if (mb->getBaseCollider())
10495 {
10496 serverCmd.m_dynamicsInfo.m_activationState = mb->getBaseCollider()->getActivationState();
10497 serverCmd.m_dynamicsInfo.m_contactProcessingThreshold = mb->getBaseCollider()->getContactProcessingThreshold();
10498 serverCmd.m_dynamicsInfo.m_ccdSweptSphereRadius = mb->getBaseCollider()->getCcdSweptSphereRadius();
10499 serverCmd.m_dynamicsInfo.m_frictionAnchor = mb->getBaseCollider()->getCollisionFlags() & btCollisionObject::CF_HAS_FRICTION_ANCHOR;
10500 serverCmd.m_dynamicsInfo.m_collisionMargin = mb->getBaseCollider()->getCollisionShape()->getMargin();
10501 serverCmd.m_dynamicsInfo.m_dynamicType = mb->getBaseCollider()->getCollisionFlags() & (btCollisionObject::CF_STATIC_OBJECT | btCollisionObject::CF_KINEMATIC_OBJECT);
10502 }
10503 else
10504 {
10505 serverCmd.m_dynamicsInfo.m_activationState = 0;
10506 serverCmd.m_dynamicsInfo.m_contactProcessingThreshold = 0;
10507 serverCmd.m_dynamicsInfo.m_ccdSweptSphereRadius = 0;
10508 serverCmd.m_dynamicsInfo.m_frictionAnchor = 0;
10509 serverCmd.m_dynamicsInfo.m_collisionMargin = 0;
10510 serverCmd.m_dynamicsInfo.m_dynamicType = 0;
10511 }
10512 serverCmd.m_dynamicsInfo.m_localInertialDiagonal[0] = mb->getBaseInertia()[0];
10513 serverCmd.m_dynamicsInfo.m_localInertialDiagonal[1] = mb->getBaseInertia()[1];
10514 serverCmd.m_dynamicsInfo.m_localInertialDiagonal[2] = mb->getBaseInertia()[2];
10515 serverCmd.m_dynamicsInfo.m_lateralFrictionCoeff = mb->getBaseCollider()->getFriction();
10516
10517 serverCmd.m_dynamicsInfo.m_localInertialFrame[0] = body->m_rootLocalInertialFrame.getOrigin()[0];
10518 serverCmd.m_dynamicsInfo.m_localInertialFrame[1] = body->m_rootLocalInertialFrame.getOrigin()[1];
10519 serverCmd.m_dynamicsInfo.m_localInertialFrame[2] = body->m_rootLocalInertialFrame.getOrigin()[2];
10520 serverCmd.m_dynamicsInfo.m_localInertialFrame[3] = body->m_rootLocalInertialFrame.getRotation()[0];
10521 serverCmd.m_dynamicsInfo.m_localInertialFrame[4] = body->m_rootLocalInertialFrame.getRotation()[1];
10522 serverCmd.m_dynamicsInfo.m_localInertialFrame[5] = body->m_rootLocalInertialFrame.getRotation()[2];
10523 serverCmd.m_dynamicsInfo.m_localInertialFrame[6] = body->m_rootLocalInertialFrame.getRotation()[3];
10524
10525 serverCmd.m_dynamicsInfo.m_angularDamping = body->m_multiBody->getAngularDamping();
10526 serverCmd.m_dynamicsInfo.m_linearDamping = body->m_multiBody->getLinearDamping();
10527
10528 serverCmd.m_dynamicsInfo.m_restitution = mb->getBaseCollider()->getRestitution();
10529 serverCmd.m_dynamicsInfo.m_rollingFrictionCoeff = mb->getBaseCollider()->getRollingFriction();
10530 serverCmd.m_dynamicsInfo.m_spinningFrictionCoeff = mb->getBaseCollider()->getSpinningFriction();
10531
10532 if (mb->getBaseCollider()->getCollisionFlags() & btCollisionObject::CF_HAS_CONTACT_STIFFNESS_DAMPING)
10533 {
10534 serverCmd.m_dynamicsInfo.m_contactStiffness = mb->getBaseCollider()->getContactStiffness();
10535 serverCmd.m_dynamicsInfo.m_contactDamping = mb->getBaseCollider()->getContactDamping();
10536 }
10537 else
10538 {
10539 serverCmd.m_dynamicsInfo.m_contactStiffness = -1;
10540 serverCmd.m_dynamicsInfo.m_contactDamping = -1;
10541 }
10542 }
10543 else
10544 {
10545 serverCmd.m_dynamicsInfo.m_mass = mb->getLinkMass(linkIndex);
10546
10547 if (mb->getLinkCollider(linkIndex))
10548 {
10549 serverCmd.m_dynamicsInfo.m_activationState = mb->getLinkCollider(linkIndex)->getActivationState();
10550 serverCmd.m_dynamicsInfo.m_contactProcessingThreshold = mb->getLinkCollider(linkIndex)->getContactProcessingThreshold();
10551 serverCmd.m_dynamicsInfo.m_ccdSweptSphereRadius = mb->getLinkCollider(linkIndex)->getCcdSweptSphereRadius();
10552 serverCmd.m_dynamicsInfo.m_frictionAnchor = mb->getLinkCollider(linkIndex)->getCollisionFlags() & btCollisionObject::CF_HAS_FRICTION_ANCHOR;
10553 serverCmd.m_dynamicsInfo.m_collisionMargin = mb->getLinkCollider(linkIndex)->getCollisionShape()->getMargin();
10554 serverCmd.m_dynamicsInfo.m_dynamicType = mb->getLinkCollider(linkIndex)->getCollisionFlags() & (btCollisionObject::CF_STATIC_OBJECT | btCollisionObject::CF_KINEMATIC_OBJECT);
10555 }
10556 else
10557 {
10558 serverCmd.m_dynamicsInfo.m_activationState = 0;
10559 serverCmd.m_dynamicsInfo.m_contactProcessingThreshold = 0;
10560 serverCmd.m_dynamicsInfo.m_ccdSweptSphereRadius = 0;
10561 serverCmd.m_dynamicsInfo.m_frictionAnchor = 0;
10562 serverCmd.m_dynamicsInfo.m_collisionMargin = 0;
10563 serverCmd.m_dynamicsInfo.m_dynamicType = 0;
10564 }
10565
10566 serverCmd.m_dynamicsInfo.m_localInertialDiagonal[0] = mb->getLinkInertia(linkIndex)[0];
10567 serverCmd.m_dynamicsInfo.m_localInertialDiagonal[1] = mb->getLinkInertia(linkIndex)[1];
10568 serverCmd.m_dynamicsInfo.m_localInertialDiagonal[2] = mb->getLinkInertia(linkIndex)[2];
10569
10570 serverCmd.m_dynamicsInfo.m_localInertialFrame[0] = body->m_linkLocalInertialFrames[linkIndex].getOrigin()[0];
10571 serverCmd.m_dynamicsInfo.m_localInertialFrame[1] = body->m_linkLocalInertialFrames[linkIndex].getOrigin()[1];
10572 serverCmd.m_dynamicsInfo.m_localInertialFrame[2] = body->m_linkLocalInertialFrames[linkIndex].getOrigin()[2];
10573 serverCmd.m_dynamicsInfo.m_localInertialFrame[3] = body->m_linkLocalInertialFrames[linkIndex].getRotation()[0];
10574 serverCmd.m_dynamicsInfo.m_localInertialFrame[4] = body->m_linkLocalInertialFrames[linkIndex].getRotation()[1];
10575 serverCmd.m_dynamicsInfo.m_localInertialFrame[5] = body->m_linkLocalInertialFrames[linkIndex].getRotation()[2];
10576 serverCmd.m_dynamicsInfo.m_localInertialFrame[6] = body->m_linkLocalInertialFrames[linkIndex].getRotation()[3];
10577
10578 serverCmd.m_dynamicsInfo.m_angularDamping = body->m_multiBody->getAngularDamping();
10579 serverCmd.m_dynamicsInfo.m_linearDamping = body->m_multiBody->getLinearDamping();
10580
10581 if (mb->getLinkCollider(linkIndex))
10582 {
10583 serverCmd.m_dynamicsInfo.m_lateralFrictionCoeff = mb->getLinkCollider(linkIndex)->getFriction();
10584 serverCmd.m_dynamicsInfo.m_restitution = mb->getLinkCollider(linkIndex)->getRestitution();
10585 serverCmd.m_dynamicsInfo.m_rollingFrictionCoeff = mb->getLinkCollider(linkIndex)->getRollingFriction();
10586 serverCmd.m_dynamicsInfo.m_spinningFrictionCoeff = mb->getLinkCollider(linkIndex)->getSpinningFriction();
10587
10588 if (mb->getLinkCollider(linkIndex)->getCollisionFlags() & btCollisionObject::CF_HAS_CONTACT_STIFFNESS_DAMPING)
10589 {
10590 serverCmd.m_dynamicsInfo.m_contactStiffness = mb->getLinkCollider(linkIndex)->getContactStiffness();
10591 serverCmd.m_dynamicsInfo.m_contactDamping = mb->getLinkCollider(linkIndex)->getContactDamping();
10592 }
10593 else
10594 {
10595 serverCmd.m_dynamicsInfo.m_contactStiffness = -1;
10596 serverCmd.m_dynamicsInfo.m_contactDamping = -1;
10597 }
10598 }
10599 else
10600 {
10601 b3Warning("The dynamic info requested is not available");
10602 serverCmd.m_type = CMD_GET_DYNAMICS_INFO_FAILED;
10603 }
10604 }
10605 hasStatus = true;
10606 }
10607 else if (body && body->m_rigidBody)
10608 {
10609 SharedMemoryStatus& serverCmd = serverStatusOut;
10610 serverCmd.m_type = CMD_GET_DYNAMICS_INFO_COMPLETED;
10611 serverCmd.m_dynamicsInfo.m_bodyType = BT_RIGID_BODY;
10612
10613 btRigidBody* rb = body->m_rigidBody;
10614
10615
10616 serverCmd.m_dynamicsInfo.m_localInertialDiagonal[0] = rb->getLocalInertia()[0];
10617 serverCmd.m_dynamicsInfo.m_localInertialDiagonal[1] = rb->getLocalInertia()[1];
10618 serverCmd.m_dynamicsInfo.m_localInertialDiagonal[2] = rb->getLocalInertia()[2];
10619
10620 serverCmd.m_dynamicsInfo.m_lateralFrictionCoeff = rb->getFriction();
10621 serverCmd.m_dynamicsInfo.m_rollingFrictionCoeff = rb->getRollingFriction();
10622 serverCmd.m_dynamicsInfo.m_spinningFrictionCoeff = rb->getSpinningFriction();
10623 serverCmd.m_dynamicsInfo.m_angularDamping = rb->getAngularDamping();
10624 serverCmd.m_dynamicsInfo.m_linearDamping = rb->getLinearDamping();
10625 serverCmd.m_dynamicsInfo.m_mass = rb->getMass();
10626 serverCmd.m_dynamicsInfo.m_collisionMargin = rb->getCollisionShape() ? rb->getCollisionShape()->getMargin() : 0;
10627 serverCmd.m_dynamicsInfo.m_dynamicType = rb->getCollisionFlags() & (btCollisionObject::CF_STATIC_OBJECT | btCollisionObject::CF_KINEMATIC_OBJECT);
10628 }
10629 #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
10630 else if (body && body->m_softBody)
10631 {
10632 SharedMemoryStatus& serverCmd = serverStatusOut;
10633 serverCmd.m_type = CMD_GET_DYNAMICS_INFO_COMPLETED;
10634 serverCmd.m_dynamicsInfo.m_bodyType = BT_SOFT_BODY;
10635 serverCmd.m_dynamicsInfo.m_collisionMargin = 0;
10636 }
10637 #endif
10638 return hasStatus;
10639 }
10640
processRequestPhysicsSimulationParametersCommand(const struct SharedMemoryCommand & clientCmd,struct SharedMemoryStatus & serverStatusOut,char * bufferServerToClient,int bufferSizeInBytes)10641 bool PhysicsServerCommandProcessor::processRequestPhysicsSimulationParametersCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
10642 {
10643 bool hasStatus = true;
10644 SharedMemoryStatus& serverCmd = serverStatusOut;
10645 serverCmd.m_type = CMD_REQUEST_PHYSICS_SIMULATION_PARAMETERS_COMPLETED;
10646
10647 serverCmd.m_simulationParameterResultArgs.m_allowedCcdPenetration = m_data->m_dynamicsWorld->getDispatchInfo().m_allowedCcdPenetration;
10648 serverCmd.m_simulationParameterResultArgs.m_collisionFilterMode = m_data->m_broadphaseCollisionFilterCallback->m_filterMode;
10649 serverCmd.m_simulationParameterResultArgs.m_deltaTime = m_data->m_physicsDeltaTime;
10650 serverCmd.m_simulationParameterResultArgs.m_simulationTimestamp = m_data->m_simulationTimestamp;
10651 serverCmd.m_simulationParameterResultArgs.m_contactBreakingThreshold = gContactBreakingThreshold;
10652 serverCmd.m_simulationParameterResultArgs.m_contactSlop = m_data->m_dynamicsWorld->getSolverInfo().m_linearSlop;
10653 serverCmd.m_simulationParameterResultArgs.m_enableSAT = m_data->m_dynamicsWorld->getDispatchInfo().m_enableSatConvex;
10654
10655 serverCmd.m_simulationParameterResultArgs.m_defaultGlobalCFM = m_data->m_dynamicsWorld->getSolverInfo().m_globalCfm;
10656 serverCmd.m_simulationParameterResultArgs.m_defaultContactERP = m_data->m_dynamicsWorld->getSolverInfo().m_erp2;
10657 serverCmd.m_simulationParameterResultArgs.m_defaultNonContactERP = m_data->m_dynamicsWorld->getSolverInfo().m_erp;
10658 serverCmd.m_simulationParameterResultArgs.m_deltaTime = m_data->m_physicsDeltaTime;
10659 serverCmd.m_simulationParameterResultArgs.m_deterministicOverlappingPairs = m_data->m_dynamicsWorld->getDispatchInfo().m_deterministicOverlappingPairs;
10660 serverCmd.m_simulationParameterResultArgs.m_enableConeFriction = (m_data->m_dynamicsWorld->getSolverInfo().m_solverMode & SOLVER_DISABLE_IMPLICIT_CONE_FRICTION) ? 0 : 1;
10661 serverCmd.m_simulationParameterResultArgs.m_enableFileCaching = b3IsFileCachingEnabled();
10662 serverCmd.m_simulationParameterResultArgs.m_frictionCFM = m_data->m_dynamicsWorld->getSolverInfo().m_frictionCFM;
10663 serverCmd.m_simulationParameterResultArgs.m_frictionERP = m_data->m_dynamicsWorld->getSolverInfo().m_frictionERP;
10664 btVector3 grav = m_data->m_dynamicsWorld->getGravity();
10665 serverCmd.m_simulationParameterResultArgs.m_gravityAcceleration[0] = grav[0];
10666 serverCmd.m_simulationParameterResultArgs.m_gravityAcceleration[1] = grav[1];
10667 serverCmd.m_simulationParameterResultArgs.m_gravityAcceleration[2] = grav[2];
10668 serverCmd.m_simulationParameterResultArgs.m_internalSimFlags = gInternalSimFlags;
10669 serverCmd.m_simulationParameterResultArgs.m_jointFeedbackMode = 0;
10670 if (m_data->m_dynamicsWorld->getSolverInfo().m_jointFeedbackInWorldSpace)
10671 {
10672 serverCmd.m_simulationParameterResultArgs.m_jointFeedbackMode |= JOINT_FEEDBACK_IN_WORLD_SPACE;
10673 }
10674 if (m_data->m_dynamicsWorld->getSolverInfo().m_jointFeedbackInJointFrame)
10675 {
10676 serverCmd.m_simulationParameterResultArgs.m_jointFeedbackMode |= JOINT_FEEDBACK_IN_JOINT_FRAME;
10677 }
10678
10679 serverCmd.m_simulationParameterResultArgs.m_numSimulationSubSteps = m_data->m_numSimulationSubSteps;
10680 serverCmd.m_simulationParameterResultArgs.m_numSolverIterations = m_data->m_dynamicsWorld->getSolverInfo().m_numIterations;
10681 serverCmd.m_simulationParameterResultArgs.m_numNonContactInnerIterations = m_data->m_dynamicsWorld->getSolverInfo().m_numNonContactInnerIterations;
10682 serverCmd.m_simulationParameterResultArgs.m_restitutionVelocityThreshold = m_data->m_dynamicsWorld->getSolverInfo().m_restitutionVelocityThreshold;
10683
10684 serverCmd.m_simulationParameterResultArgs.m_solverResidualThreshold = m_data->m_dynamicsWorld->getSolverInfo().m_leastSquaresResidualThreshold;
10685 serverCmd.m_simulationParameterResultArgs.m_splitImpulsePenetrationThreshold = m_data->m_dynamicsWorld->getSolverInfo().m_splitImpulsePenetrationThreshold;
10686 serverCmd.m_simulationParameterResultArgs.m_useRealTimeSimulation = m_data->m_useRealTimeSimulation;
10687 serverCmd.m_simulationParameterResultArgs.m_useSplitImpulse = m_data->m_dynamicsWorld->getSolverInfo().m_splitImpulse;
10688
10689 return hasStatus;
10690 }
10691
processSendPhysicsParametersCommand(const struct SharedMemoryCommand & clientCmd,struct SharedMemoryStatus & serverStatusOut,char * bufferServerToClient,int bufferSizeInBytes)10692 bool PhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
10693 {
10694 bool hasStatus = true;
10695
10696 BT_PROFILE("CMD_SEND_PHYSICS_SIMULATION_PARAMETERS");
10697
10698 if (clientCmd.m_updateFlags & SIM_PARAM_ENABLE_CONE_FRICTION)
10699 {
10700 if (clientCmd.m_physSimParamArgs.m_enableConeFriction)
10701 {
10702 m_data->m_dynamicsWorld->getSolverInfo().m_solverMode &= ~SOLVER_DISABLE_IMPLICIT_CONE_FRICTION;
10703 }
10704 else
10705 {
10706 m_data->m_dynamicsWorld->getSolverInfo().m_solverMode |= SOLVER_DISABLE_IMPLICIT_CONE_FRICTION;
10707 }
10708 }
10709 if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_DETERMINISTIC_OVERLAPPING_PAIRS)
10710 {
10711 m_data->m_dynamicsWorld->getDispatchInfo().m_deterministicOverlappingPairs = (clientCmd.m_physSimParamArgs.m_deterministicOverlappingPairs != 0);
10712 }
10713
10714 if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_CCD_ALLOWED_PENETRATION)
10715 {
10716 m_data->m_dynamicsWorld->getDispatchInfo().m_allowedCcdPenetration = clientCmd.m_physSimParamArgs.m_allowedCcdPenetration;
10717 }
10718
10719 if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_JOINT_FEEDBACK_MODE)
10720 {
10721 m_data->m_dynamicsWorld->getSolverInfo().m_jointFeedbackInWorldSpace = (clientCmd.m_physSimParamArgs.m_jointFeedbackMode & JOINT_FEEDBACK_IN_WORLD_SPACE) != 0;
10722 m_data->m_dynamicsWorld->getSolverInfo().m_jointFeedbackInJointFrame = (clientCmd.m_physSimParamArgs.m_jointFeedbackMode & JOINT_FEEDBACK_IN_JOINT_FRAME) != 0;
10723 }
10724
10725 if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_DELTA_TIME)
10726 {
10727 m_data->m_physicsDeltaTime = clientCmd.m_physSimParamArgs.m_deltaTime;
10728 }
10729 if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_REAL_TIME_SIMULATION)
10730 {
10731 m_data->m_useRealTimeSimulation = (clientCmd.m_physSimParamArgs.m_useRealTimeSimulation != 0);
10732 }
10733
10734 //see
10735 if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_INTERNAL_SIMULATION_FLAGS)
10736 {
10737 //these flags are for internal/temporary/easter-egg/experimental demo purposes, use at own risk
10738 gInternalSimFlags = clientCmd.m_physSimParamArgs.m_internalSimFlags;
10739 m_data->m_useAlternativeDeformableIndexing =
10740 (clientCmd.m_physSimParamArgs.m_internalSimFlags & eDeformableAlternativeIndexing) != 0;
10741 }
10742
10743 if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_GRAVITY)
10744 {
10745 btVector3 grav(clientCmd.m_physSimParamArgs.m_gravityAcceleration[0],
10746 clientCmd.m_physSimParamArgs.m_gravityAcceleration[1],
10747 clientCmd.m_physSimParamArgs.m_gravityAcceleration[2]);
10748 this->m_data->m_dynamicsWorld->setGravity(grav);
10749 #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
10750 btSoftMultiBodyDynamicsWorld* softWorld = getSoftWorld();
10751 if (softWorld)
10752 {
10753 softWorld->getWorldInfo().m_gravity = grav;
10754 }
10755 btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld();
10756 if (deformWorld)
10757 {
10758 deformWorld->getWorldInfo().m_gravity = grav;
10759 for (int i = 0; i < m_data->m_lf.size(); ++i)
10760 {
10761 btDeformableLagrangianForce* force = m_data->m_lf[i];
10762 if (force->getForceType() == BT_GRAVITY_FORCE)
10763 {
10764 btDeformableGravityForce* gforce = (btDeformableGravityForce*)force;
10765 gforce->m_gravity = grav;
10766 }
10767 }
10768 }
10769
10770 #endif
10771 if (m_data->m_verboseOutput)
10772 {
10773 b3Printf("Updated Gravity: %f,%f,%f", grav[0], grav[1], grav[2]);
10774 }
10775 }
10776 if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_NUM_SOLVER_ITERATIONS)
10777 {
10778 m_data->m_dynamicsWorld->getSolverInfo().m_numIterations = clientCmd.m_physSimParamArgs.m_numSolverIterations;
10779 }
10780
10781 if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_NUM_NONCONTACT_INNER_ITERATIONS)
10782 {
10783 m_data->m_dynamicsWorld->getSolverInfo().m_numNonContactInnerIterations = clientCmd.m_physSimParamArgs.m_numNonContactInnerIterations;
10784 }
10785
10786 if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_SOLVER_RESIDULAL_THRESHOLD)
10787 {
10788 m_data->m_dynamicsWorld->getSolverInfo().m_leastSquaresResidualThreshold = clientCmd.m_physSimParamArgs.m_solverResidualThreshold;
10789 }
10790
10791 if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_CONTACT_BREAKING_THRESHOLD)
10792 {
10793 gContactBreakingThreshold = clientCmd.m_physSimParamArgs.m_contactBreakingThreshold;
10794 }
10795
10796 if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_CONTACT_SLOP)
10797 {
10798 m_data->m_dynamicsWorld->getSolverInfo().m_linearSlop = clientCmd.m_physSimParamArgs.m_contactSlop;
10799 }
10800
10801 if (clientCmd.m_updateFlags & SIM_PARAM_ENABLE_SAT)
10802 {
10803 m_data->m_dynamicsWorld->getDispatchInfo().m_enableSatConvex = clientCmd.m_physSimParamArgs.m_enableSAT != 0;
10804 }
10805 if (clientCmd.m_updateFlags & SIM_PARAM_CONSTRAINT_SOLVER_TYPE)
10806 {
10807 //check if the current type is different from requested one
10808 if (m_data->m_constraintSolverType != clientCmd.m_physSimParamArgs.m_constraintSolverType)
10809 {
10810 m_data->m_constraintSolverType = clientCmd.m_physSimParamArgs.m_constraintSolverType;
10811
10812 btConstraintSolver* oldSolver = m_data->m_dynamicsWorld->getConstraintSolver();
10813
10814 #ifdef USE_DISCRETE_DYNAMICS_WORLD
10815 btSequentialImpulseConstraintSolver* newSolver = 0;
10816 #else
10817 btMultiBodyConstraintSolver* newSolver = 0;
10818 #endif
10819
10820 switch (clientCmd.m_physSimParamArgs.m_constraintSolverType)
10821 {
10822 case eConstraintSolverLCP_SI:
10823 {
10824 newSolver = new btMultiBodyConstraintSolver;
10825 b3Printf("PyBullet: Constraint Solver: btMultiBodyConstraintSolver\n");
10826 break;
10827 }
10828 case eConstraintSolverLCP_PGS:
10829 {
10830 btSolveProjectedGaussSeidel* mlcp = new btSolveProjectedGaussSeidel();
10831 #ifdef USE_DISCRETE_DYNAMICS_WORLD
10832 newSolver = new btMLCPSolver(mlcp);
10833 #else
10834 newSolver = new btMultiBodyMLCPConstraintSolver(mlcp);
10835 #endif
10836
10837 b3Printf("PyBullet: Constraint Solver: MLCP + PGS\n");
10838 break;
10839 }
10840 case eConstraintSolverLCP_DANTZIG:
10841 {
10842 btDantzigSolver* mlcp = new btDantzigSolver();
10843 newSolver = new btMultiBodyMLCPConstraintSolver(mlcp);
10844 b3Printf("PyBullet: Constraint Solver: MLCP + Dantzig\n");
10845 break;
10846 }
10847 case eConstraintSolverLCP_BLOCK_PGS:
10848 {
10849 break;
10850 }
10851 default:
10852 {
10853 }
10854 };
10855
10856 if (newSolver)
10857 {
10858 delete oldSolver;
10859
10860 #ifdef USE_DISCRETE_DYNAMICS_WORLD
10861 m_data->m_dynamicsWorld->setConstraintSolver(newSolver);
10862 #else
10863 m_data->m_dynamicsWorld->setMultiBodyConstraintSolver(newSolver);
10864 #endif
10865 m_data->m_solver = newSolver;
10866 printf("switched solver\n");
10867 }
10868 }
10869 }
10870
10871 if (clientCmd.m_updateFlags & SIM_PARAM_CONSTRAINT_MIN_SOLVER_ISLAND_SIZE)
10872 {
10873 m_data->m_dynamicsWorld->getSolverInfo().m_minimumSolverBatchSize = clientCmd.m_physSimParamArgs.m_minimumSolverIslandSize;
10874 }
10875
10876 if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_COLLISION_FILTER_MODE)
10877 {
10878 m_data->m_broadphaseCollisionFilterCallback->m_filterMode = clientCmd.m_physSimParamArgs.m_collisionFilterMode;
10879 }
10880
10881 if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_USE_SPLIT_IMPULSE)
10882 {
10883 m_data->m_dynamicsWorld->getSolverInfo().m_splitImpulse = clientCmd.m_physSimParamArgs.m_useSplitImpulse;
10884 }
10885 if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_SPLIT_IMPULSE_PENETRATION_THRESHOLD)
10886 {
10887 m_data->m_dynamicsWorld->getSolverInfo().m_splitImpulsePenetrationThreshold = clientCmd.m_physSimParamArgs.m_splitImpulsePenetrationThreshold;
10888 }
10889
10890 if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_NUM_SIMULATION_SUB_STEPS)
10891 {
10892 m_data->m_numSimulationSubSteps = clientCmd.m_physSimParamArgs.m_numSimulationSubSteps;
10893 }
10894
10895 if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_DEFAULT_CONTACT_ERP)
10896 {
10897 m_data->m_dynamicsWorld->getSolverInfo().m_erp2 = clientCmd.m_physSimParamArgs.m_defaultContactERP;
10898 }
10899
10900 if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_DEFAULT_NON_CONTACT_ERP)
10901 {
10902 m_data->m_dynamicsWorld->getSolverInfo().m_erp = clientCmd.m_physSimParamArgs.m_defaultNonContactERP;
10903 }
10904
10905 if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_DEFAULT_FRICTION_ERP)
10906 {
10907 m_data->m_dynamicsWorld->getSolverInfo().m_frictionERP = clientCmd.m_physSimParamArgs.m_frictionERP;
10908 }
10909
10910 if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_DEFAULT_GLOBAL_CFM)
10911 {
10912 m_data->m_dynamicsWorld->getSolverInfo().m_globalCfm = clientCmd.m_physSimParamArgs.m_defaultGlobalCFM;
10913 }
10914
10915 if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_DEFAULT_FRICTION_CFM)
10916 {
10917 m_data->m_dynamicsWorld->getSolverInfo().m_frictionCFM = clientCmd.m_physSimParamArgs.m_frictionCFM;
10918 }
10919
10920 if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_SPARSE_SDF)
10921 {
10922 #ifndef SKIP_DEFORMABLE_BODY
10923 {
10924 btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld();
10925 if (deformWorld)
10926 {
10927 deformWorld->getWorldInfo().m_sparsesdf.setDefaultVoxelsz(clientCmd.m_physSimParamArgs.m_sparseSdfVoxelSize);
10928 deformWorld->getWorldInfo().m_sparsesdf.Reset();
10929 }
10930 }
10931 #endif
10932 #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
10933 {
10934 btSoftMultiBodyDynamicsWorld* softWorld = getSoftWorld();
10935 if (softWorld)
10936 {
10937 softWorld->getWorldInfo().m_sparsesdf.setDefaultVoxelsz(clientCmd.m_physSimParamArgs.m_sparseSdfVoxelSize);
10938 softWorld->getWorldInfo().m_sparsesdf.Reset();
10939 }
10940 }
10941 #endif
10942 }
10943
10944 if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_RESTITUTION_VELOCITY_THRESHOLD)
10945 {
10946 m_data->m_dynamicsWorld->getSolverInfo().m_restitutionVelocityThreshold = clientCmd.m_physSimParamArgs.m_restitutionVelocityThreshold;
10947 }
10948
10949 if (clientCmd.m_updateFlags & SIM_PARAM_ENABLE_FILE_CACHING)
10950 {
10951 b3EnableFileCaching(clientCmd.m_physSimParamArgs.m_enableFileCaching);
10952 m_data->m_pluginManager.getFileIOInterface()->enableFileCaching(clientCmd.m_physSimParamArgs.m_enableFileCaching != 0);
10953 }
10954
10955 if (clientCmd.m_updateFlags & SIM_PARAM_REPORT_CONSTRAINT_SOLVER_ANALYTICS)
10956 {
10957 m_data->m_dynamicsWorld->getSolverInfo().m_reportSolverAnalytics = clientCmd.m_physSimParamArgs.m_reportSolverAnalytics;
10958 }
10959
10960 if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_WARM_STARTING_FACTOR)
10961 {
10962 m_data->m_dynamicsWorld->getSolverInfo().m_warmstartingFactor = clientCmd.m_physSimParamArgs.m_warmStartingFactor;
10963 }
10964
10965 if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_ARTICULATED_WARM_STARTING_FACTOR)
10966 {
10967 m_data->m_dynamicsWorld->getSolverInfo().m_solverMode |= SOLVER_USE_ARTICULATED_WARMSTARTING;
10968 m_data->m_dynamicsWorld->getSolverInfo().m_articulatedWarmstartingFactor = clientCmd.m_physSimParamArgs.m_articulatedWarmStartingFactor;
10969 }
10970 SharedMemoryStatus& serverCmd = serverStatusOut;
10971 serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED;
10972 return hasStatus;
10973 }
10974
processInitPoseCommand(const struct SharedMemoryCommand & clientCmd,struct SharedMemoryStatus & serverStatusOut,char * bufferServerToClient,int bufferSizeInBytes)10975 bool PhysicsServerCommandProcessor::processInitPoseCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
10976 {
10977 bool hasStatus = true;
10978
10979 BT_PROFILE("CMD_INIT_POSE");
10980
10981 if (m_data->m_verboseOutput)
10982 {
10983 b3Printf("Server Init Pose not implemented yet");
10984 }
10985 int bodyUniqueId = clientCmd.m_initPoseArgs.m_bodyUniqueId;
10986 InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId);
10987
10988 btVector3 baseLinVel(0, 0, 0);
10989 btVector3 baseAngVel(0, 0, 0);
10990
10991 if (clientCmd.m_updateFlags & INIT_POSE_HAS_BASE_LINEAR_VELOCITY)
10992 {
10993 baseLinVel.setValue(clientCmd.m_initPoseArgs.m_initialStateQdot[0],
10994 clientCmd.m_initPoseArgs.m_initialStateQdot[1],
10995 clientCmd.m_initPoseArgs.m_initialStateQdot[2]);
10996 }
10997 if (clientCmd.m_updateFlags & INIT_POSE_HAS_BASE_ANGULAR_VELOCITY)
10998 {
10999 baseAngVel.setValue(clientCmd.m_initPoseArgs.m_initialStateQdot[3],
11000 clientCmd.m_initPoseArgs.m_initialStateQdot[4],
11001 clientCmd.m_initPoseArgs.m_initialStateQdot[5]);
11002 }
11003 btVector3 basePos(0, 0, 0);
11004 if (clientCmd.m_updateFlags & INIT_POSE_HAS_INITIAL_POSITION)
11005 {
11006 basePos = btVector3(
11007 clientCmd.m_initPoseArgs.m_initialStateQ[0],
11008 clientCmd.m_initPoseArgs.m_initialStateQ[1],
11009 clientCmd.m_initPoseArgs.m_initialStateQ[2]);
11010 }
11011 btQuaternion baseOrn(0, 0, 0, 1);
11012 if (clientCmd.m_updateFlags & INIT_POSE_HAS_INITIAL_ORIENTATION)
11013 {
11014 baseOrn.setValue(clientCmd.m_initPoseArgs.m_initialStateQ[3],
11015 clientCmd.m_initPoseArgs.m_initialStateQ[4],
11016 clientCmd.m_initPoseArgs.m_initialStateQ[5],
11017 clientCmd.m_initPoseArgs.m_initialStateQ[6]);
11018 }
11019 if (body && body->m_multiBody)
11020 {
11021 btMultiBody* mb = body->m_multiBody;
11022
11023 if (clientCmd.m_updateFlags & INIT_POSE_HAS_SCALING)
11024 {
11025 btVector3 scaling(clientCmd.m_initPoseArgs.m_scaling[0], clientCmd.m_initPoseArgs.m_scaling[1], clientCmd.m_initPoseArgs.m_scaling[2]);
11026
11027 mb->getBaseCollider()->getCollisionShape()->setLocalScaling(scaling);
11028 //refresh broadphase
11029 m_data->m_dynamicsWorld->getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(
11030 mb->getBaseCollider()->getBroadphaseHandle(),
11031 m_data->m_dynamicsWorld->getDispatcher());
11032 //also visuals
11033 int graphicsIndex = mb->getBaseCollider()->getUserIndex();
11034 m_data->m_guiHelper->changeScaling(graphicsIndex, clientCmd.m_initPoseArgs.m_scaling);
11035 }
11036
11037 if (clientCmd.m_updateFlags & INIT_POSE_HAS_BASE_LINEAR_VELOCITY)
11038 {
11039 mb->setBaseVel(baseLinVel);
11040 }
11041
11042 if (clientCmd.m_updateFlags & INIT_POSE_HAS_BASE_ANGULAR_VELOCITY)
11043 {
11044 mb->setBaseOmega(baseAngVel);
11045 }
11046
11047 if (clientCmd.m_updateFlags & INIT_POSE_HAS_INITIAL_POSITION)
11048 {
11049 btVector3 zero(0, 0, 0);
11050 btAssert(clientCmd.m_initPoseArgs.m_hasInitialStateQ[0] &&
11051 clientCmd.m_initPoseArgs.m_hasInitialStateQ[1] &&
11052 clientCmd.m_initPoseArgs.m_hasInitialStateQ[2]);
11053
11054 mb->setBaseVel(baseLinVel);
11055 mb->setBasePos(basePos);
11056 }
11057 if (clientCmd.m_updateFlags & INIT_POSE_HAS_INITIAL_ORIENTATION)
11058 {
11059 btAssert(clientCmd.m_initPoseArgs.m_hasInitialStateQ[3] &&
11060 clientCmd.m_initPoseArgs.m_hasInitialStateQ[4] &&
11061 clientCmd.m_initPoseArgs.m_hasInitialStateQ[5] &&
11062 clientCmd.m_initPoseArgs.m_hasInitialStateQ[6]);
11063
11064 mb->setBaseOmega(baseAngVel);
11065 btQuaternion invOrn(baseOrn);
11066
11067 mb->setWorldToBaseRot(invOrn.inverse());
11068 }
11069 if (clientCmd.m_updateFlags & INIT_POSE_HAS_JOINT_STATE)
11070 {
11071 int uDofIndex = 6;
11072 int posVarCountIndex = 7;
11073 for (int i = 0; i < mb->getNumLinks(); i++)
11074 {
11075 int posVarCount = mb->getLink(i).m_posVarCount;
11076 bool hasPosVar = posVarCount > 0;
11077
11078 for (int j = 0; j < posVarCount; j++)
11079 {
11080 if (clientCmd.m_initPoseArgs.m_hasInitialStateQ[posVarCountIndex + j] == 0)
11081 {
11082 hasPosVar = false;
11083 break;
11084 }
11085 }
11086 if (hasPosVar)
11087 {
11088 if (mb->getLink(i).m_dofCount == 1)
11089 {
11090 mb->setJointPos(i, clientCmd.m_initPoseArgs.m_initialStateQ[posVarCountIndex]);
11091 mb->setJointVel(i, 0); //backwards compatibility
11092 }
11093 if (mb->getLink(i).m_dofCount == 3)
11094 {
11095 btQuaternion q(
11096 clientCmd.m_initPoseArgs.m_initialStateQ[posVarCountIndex],
11097 clientCmd.m_initPoseArgs.m_initialStateQ[posVarCountIndex + 1],
11098 clientCmd.m_initPoseArgs.m_initialStateQ[posVarCountIndex + 2],
11099 clientCmd.m_initPoseArgs.m_initialStateQ[posVarCountIndex + 3]);
11100 q.normalize();
11101 mb->setJointPosMultiDof(i, &q[0]);
11102 double vel[6] = {0, 0, 0, 0, 0, 0};
11103 mb->setJointVelMultiDof(i, vel);
11104 }
11105 }
11106
11107 bool hasVel = true;
11108 for (int j = 0; j < mb->getLink(i).m_dofCount; j++)
11109 {
11110 if (clientCmd.m_initPoseArgs.m_hasInitialStateQdot[uDofIndex + j] == 0)
11111 {
11112 hasVel = false;
11113 break;
11114 }
11115 }
11116
11117 if (hasVel)
11118 {
11119 if (mb->getLink(i).m_dofCount == 1)
11120 {
11121 btScalar vel = clientCmd.m_initPoseArgs.m_initialStateQdot[uDofIndex];
11122 mb->setJointVel(i, vel);
11123 }
11124 if (mb->getLink(i).m_dofCount == 3)
11125 {
11126 mb->setJointVelMultiDof(i, &clientCmd.m_initPoseArgs.m_initialStateQdot[uDofIndex]);
11127 }
11128 }
11129
11130 posVarCountIndex += mb->getLink(i).m_posVarCount;
11131 uDofIndex += mb->getLink(i).m_dofCount;
11132 }
11133 }
11134
11135 btAlignedObjectArray<btQuaternion> scratch_q;
11136 btAlignedObjectArray<btVector3> scratch_m;
11137
11138 mb->forwardKinematics(scratch_q, scratch_m);
11139 int nLinks = mb->getNumLinks();
11140 scratch_q.resize(nLinks + 1);
11141 scratch_m.resize(nLinks + 1);
11142
11143 mb->updateCollisionObjectWorldTransforms(scratch_q, scratch_m);
11144
11145 m_data->m_dynamicsWorld->updateSingleAabb(mb->getBaseCollider());
11146 for (int i=0;i<mb->getNumLinks();i++)
11147 {
11148 m_data->m_dynamicsWorld->updateSingleAabb(mb->getLinkCollider(i));
11149 }
11150 }
11151
11152 if (body && body->m_rigidBody)
11153 {
11154 if (clientCmd.m_updateFlags & INIT_POSE_HAS_BASE_LINEAR_VELOCITY)
11155 {
11156 body->m_rigidBody->setLinearVelocity(baseLinVel);
11157 }
11158 if (clientCmd.m_updateFlags & INIT_POSE_HAS_BASE_ANGULAR_VELOCITY)
11159 {
11160 body->m_rigidBody->setAngularVelocity(baseAngVel);
11161 }
11162
11163 if (clientCmd.m_updateFlags & INIT_POSE_HAS_INITIAL_POSITION)
11164 {
11165 body->m_rigidBody->getWorldTransform().setOrigin(basePos);
11166 body->m_rigidBody->setLinearVelocity(baseLinVel);
11167 }
11168
11169 if (clientCmd.m_updateFlags & INIT_POSE_HAS_INITIAL_ORIENTATION)
11170 {
11171 body->m_rigidBody->getWorldTransform().setRotation(baseOrn);
11172 body->m_rigidBody->setAngularVelocity(baseAngVel);
11173 }
11174 m_data->m_dynamicsWorld->updateSingleAabb(body->m_rigidBody);
11175 }
11176 #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
11177 if (body && body->m_softBody)
11178 {
11179 if (clientCmd.m_updateFlags & INIT_POSE_HAS_BASE_LINEAR_VELOCITY)
11180 {
11181 body->m_softBody->setLinearVelocity(baseLinVel);
11182 }
11183 if (clientCmd.m_updateFlags & INIT_POSE_HAS_BASE_ANGULAR_VELOCITY)
11184 {
11185 body->m_softBody->setAngularVelocity(baseAngVel);
11186 }
11187 if (clientCmd.m_updateFlags & INIT_POSE_HAS_INITIAL_POSITION || clientCmd.m_updateFlags & INIT_POSE_HAS_INITIAL_ORIENTATION)
11188 {
11189 btTransform tr;
11190 tr.setIdentity();
11191 if (clientCmd.m_updateFlags & INIT_POSE_HAS_INITIAL_POSITION)
11192 {
11193 tr.setOrigin(basePos);
11194 }
11195 if (clientCmd.m_updateFlags & INIT_POSE_HAS_INITIAL_ORIENTATION)
11196 {
11197 tr.setRotation(baseOrn);
11198 }
11199 body->m_softBody->transformTo(tr);
11200 }
11201 m_data->m_dynamicsWorld->updateSingleAabb(body->m_softBody);
11202 }
11203 #endif
11204 syncPhysicsToGraphics2();
11205
11206 SharedMemoryStatus& serverCmd = serverStatusOut;
11207 serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED;
11208
11209 return hasStatus;
11210 }
11211
processResetSimulationCommand(const struct SharedMemoryCommand & clientCmd,struct SharedMemoryStatus & serverStatusOut,char * bufferServerToClient,int bufferSizeInBytes)11212 bool PhysicsServerCommandProcessor::processResetSimulationCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
11213 {
11214 bool hasStatus = true;
11215 BT_PROFILE("CMD_RESET_SIMULATION");
11216 m_data->m_guiHelper->setVisualizerFlag(COV_ENABLE_SYNC_RENDERING_INTERNAL, 0);
11217
11218 resetSimulation(clientCmd.m_updateFlags);
11219 m_data->m_guiHelper->setVisualizerFlag(COV_ENABLE_SYNC_RENDERING_INTERNAL, 1);
11220
11221 SharedMemoryStatus& serverCmd = serverStatusOut;
11222 serverCmd.m_type = CMD_RESET_SIMULATION_COMPLETED;
11223 return hasStatus;
11224 }
11225
processCreateRigidBodyCommand(const struct SharedMemoryCommand & clientCmd,struct SharedMemoryStatus & serverStatusOut,char * bufferServerToClient,int bufferSizeInBytes)11226 bool PhysicsServerCommandProcessor::processCreateRigidBodyCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
11227 {
11228 bool hasStatus = true;
11229 SharedMemoryStatus& serverCmd = serverStatusOut;
11230 serverCmd.m_type = CMD_RIGID_BODY_CREATION_COMPLETED;
11231
11232 BT_PROFILE("CMD_CREATE_RIGID_BODY");
11233
11234 btVector3 halfExtents(1, 1, 1);
11235 if (clientCmd.m_updateFlags & BOX_SHAPE_HAS_HALF_EXTENTS)
11236 {
11237 halfExtents = btVector3(
11238 clientCmd.m_createBoxShapeArguments.m_halfExtentsX,
11239 clientCmd.m_createBoxShapeArguments.m_halfExtentsY,
11240 clientCmd.m_createBoxShapeArguments.m_halfExtentsZ);
11241 }
11242 btTransform startTrans;
11243 startTrans.setIdentity();
11244 if (clientCmd.m_updateFlags & BOX_SHAPE_HAS_INITIAL_POSITION)
11245 {
11246 startTrans.setOrigin(btVector3(
11247 clientCmd.m_createBoxShapeArguments.m_initialPosition[0],
11248 clientCmd.m_createBoxShapeArguments.m_initialPosition[1],
11249 clientCmd.m_createBoxShapeArguments.m_initialPosition[2]));
11250 }
11251
11252 if (clientCmd.m_updateFlags & BOX_SHAPE_HAS_INITIAL_ORIENTATION)
11253 {
11254 startTrans.setRotation(btQuaternion(
11255 clientCmd.m_createBoxShapeArguments.m_initialOrientation[0],
11256 clientCmd.m_createBoxShapeArguments.m_initialOrientation[1],
11257 clientCmd.m_createBoxShapeArguments.m_initialOrientation[2],
11258 clientCmd.m_createBoxShapeArguments.m_initialOrientation[3]));
11259 }
11260
11261 btScalar mass = 0.f;
11262 if (clientCmd.m_updateFlags & BOX_SHAPE_HAS_MASS)
11263 {
11264 mass = clientCmd.m_createBoxShapeArguments.m_mass;
11265 }
11266
11267 int shapeType = COLLISION_SHAPE_TYPE_BOX;
11268
11269 if (clientCmd.m_updateFlags & BOX_SHAPE_HAS_COLLISION_SHAPE_TYPE)
11270 {
11271 shapeType = clientCmd.m_createBoxShapeArguments.m_collisionShapeType;
11272 }
11273
11274 #ifdef USE_DISCRETE_DYNAMICS_WORLD
11275 btWorldImporter* worldImporter = new btWorldImporter(m_data->m_dynamicsWorld);
11276 #else
11277 btMultiBodyWorldImporter* worldImporter = new btMultiBodyWorldImporter(m_data->m_dynamicsWorld);
11278 #endif
11279 m_data->m_worldImporters.push_back(worldImporter);
11280
11281 btCollisionShape* shape = 0;
11282
11283 switch (shapeType)
11284 {
11285 case COLLISION_SHAPE_TYPE_CYLINDER_X:
11286 {
11287 btScalar radius = halfExtents[1];
11288 btScalar height = halfExtents[0];
11289 shape = worldImporter->createCylinderShapeX(radius, height);
11290 break;
11291 }
11292 case COLLISION_SHAPE_TYPE_CYLINDER_Y:
11293 {
11294 btScalar radius = halfExtents[0];
11295 btScalar height = halfExtents[1];
11296 shape = worldImporter->createCylinderShapeY(radius, height);
11297 break;
11298 }
11299 case COLLISION_SHAPE_TYPE_CYLINDER_Z:
11300 {
11301 btScalar radius = halfExtents[1];
11302 btScalar height = halfExtents[2];
11303 shape = worldImporter->createCylinderShapeZ(radius, height);
11304 break;
11305 }
11306 case COLLISION_SHAPE_TYPE_CAPSULE_X:
11307 {
11308 btScalar radius = halfExtents[1];
11309 btScalar height = halfExtents[0];
11310 shape = worldImporter->createCapsuleShapeX(radius, height);
11311 break;
11312 }
11313 case COLLISION_SHAPE_TYPE_CAPSULE_Y:
11314 {
11315 btScalar radius = halfExtents[0];
11316 btScalar height = halfExtents[1];
11317 shape = worldImporter->createCapsuleShapeY(radius, height);
11318 break;
11319 }
11320 case COLLISION_SHAPE_TYPE_CAPSULE_Z:
11321 {
11322 btScalar radius = halfExtents[1];
11323 btScalar height = halfExtents[2];
11324 shape = worldImporter->createCapsuleShapeZ(radius, height);
11325 break;
11326 }
11327 case COLLISION_SHAPE_TYPE_SPHERE:
11328 {
11329 btScalar radius = halfExtents[0];
11330 shape = worldImporter->createSphereShape(radius);
11331 break;
11332 }
11333 case COLLISION_SHAPE_TYPE_BOX:
11334 default:
11335 {
11336 shape = worldImporter->createBoxShape(halfExtents);
11337 }
11338 }
11339
11340 bool isDynamic = (mass > 0);
11341 btRigidBody* rb = worldImporter->createRigidBody(isDynamic, mass, startTrans, shape, 0);
11342 //m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld);
11343 btVector4 colorRGBA(1, 0, 0, 1);
11344 if (clientCmd.m_updateFlags & BOX_SHAPE_HAS_COLOR)
11345 {
11346 colorRGBA[0] = clientCmd.m_createBoxShapeArguments.m_colorRGBA[0];
11347 colorRGBA[1] = clientCmd.m_createBoxShapeArguments.m_colorRGBA[1];
11348 colorRGBA[2] = clientCmd.m_createBoxShapeArguments.m_colorRGBA[2];
11349 colorRGBA[3] = clientCmd.m_createBoxShapeArguments.m_colorRGBA[3];
11350 }
11351 m_data->m_guiHelper->createCollisionShapeGraphicsObject(rb->getCollisionShape());
11352 m_data->m_guiHelper->createCollisionObjectGraphicsObject(rb, colorRGBA);
11353
11354 int bodyUniqueId = m_data->m_bodyHandles.allocHandle();
11355 InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(bodyUniqueId);
11356 serverCmd.m_rigidBodyCreateArgs.m_bodyUniqueId = bodyUniqueId;
11357 rb->setUserIndex2(bodyUniqueId);
11358 bodyHandle->m_rootLocalInertialFrame.setIdentity();
11359 bodyHandle->m_rigidBody = rb;
11360
11361 b3Notification notification;
11362 notification.m_notificationType = BODY_ADDED;
11363 notification.m_bodyArgs.m_bodyUniqueId = bodyUniqueId;
11364 m_data->m_pluginManager.addNotification(notification);
11365
11366 return hasStatus;
11367 }
11368
processPickBodyCommand(const struct SharedMemoryCommand & clientCmd,struct SharedMemoryStatus & serverStatusOut,char * bufferServerToClient,int bufferSizeInBytes)11369 bool PhysicsServerCommandProcessor::processPickBodyCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
11370 {
11371 bool hasStatus = true;
11372
11373 BT_PROFILE("CMD_PICK_BODY");
11374
11375 pickBody(btVector3(clientCmd.m_pickBodyArguments.m_rayFromWorld[0],
11376 clientCmd.m_pickBodyArguments.m_rayFromWorld[1],
11377 clientCmd.m_pickBodyArguments.m_rayFromWorld[2]),
11378 btVector3(clientCmd.m_pickBodyArguments.m_rayToWorld[0],
11379 clientCmd.m_pickBodyArguments.m_rayToWorld[1],
11380 clientCmd.m_pickBodyArguments.m_rayToWorld[2]));
11381
11382 SharedMemoryStatus& serverCmd = serverStatusOut;
11383 serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED;
11384 return hasStatus;
11385 }
11386
processMovePickedBodyCommand(const struct SharedMemoryCommand & clientCmd,struct SharedMemoryStatus & serverStatusOut,char * bufferServerToClient,int bufferSizeInBytes)11387 bool PhysicsServerCommandProcessor::processMovePickedBodyCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
11388 {
11389 bool hasStatus = true;
11390
11391 BT_PROFILE("CMD_MOVE_PICKED_BODY");
11392
11393 movePickedBody(btVector3(clientCmd.m_pickBodyArguments.m_rayFromWorld[0],
11394 clientCmd.m_pickBodyArguments.m_rayFromWorld[1],
11395 clientCmd.m_pickBodyArguments.m_rayFromWorld[2]),
11396 btVector3(clientCmd.m_pickBodyArguments.m_rayToWorld[0],
11397 clientCmd.m_pickBodyArguments.m_rayToWorld[1],
11398 clientCmd.m_pickBodyArguments.m_rayToWorld[2]));
11399
11400 SharedMemoryStatus& serverCmd = serverStatusOut;
11401 serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED;
11402 return hasStatus;
11403 }
11404
processRemovePickingConstraintCommand(const struct SharedMemoryCommand & clientCmd,struct SharedMemoryStatus & serverStatusOut,char * bufferServerToClient,int bufferSizeInBytes)11405 bool PhysicsServerCommandProcessor::processRemovePickingConstraintCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
11406 {
11407 bool hasStatus = true;
11408
11409 BT_PROFILE("CMD_REMOVE_PICKING_CONSTRAINT_BODY");
11410 removePickingConstraint();
11411
11412 SharedMemoryStatus& serverCmd = serverStatusOut;
11413 serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED;
11414 return hasStatus;
11415 }
11416
processRequestAabbOverlapCommand(const struct SharedMemoryCommand & clientCmd,struct SharedMemoryStatus & serverStatusOut,char * bufferServerToClient,int bufferSizeInBytes)11417 bool PhysicsServerCommandProcessor::processRequestAabbOverlapCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
11418 {
11419 bool hasStatus = true;
11420
11421 BT_PROFILE("CMD_REQUEST_AABB_OVERLAP");
11422 SharedMemoryStatus& serverCmd = serverStatusOut;
11423 int curObjectIndex = clientCmd.m_requestOverlappingObjectsArgs.m_startingOverlappingObjectIndex;
11424
11425 if (0 == curObjectIndex)
11426 {
11427 //clientCmd.m_requestContactPointArguments.m_aabbQueryMin
11428 btVector3 aabbMin, aabbMax;
11429 aabbMin.setValue(clientCmd.m_requestOverlappingObjectsArgs.m_aabbQueryMin[0],
11430 clientCmd.m_requestOverlappingObjectsArgs.m_aabbQueryMin[1],
11431 clientCmd.m_requestOverlappingObjectsArgs.m_aabbQueryMin[2]);
11432 aabbMax.setValue(clientCmd.m_requestOverlappingObjectsArgs.m_aabbQueryMax[0],
11433 clientCmd.m_requestOverlappingObjectsArgs.m_aabbQueryMax[1],
11434 clientCmd.m_requestOverlappingObjectsArgs.m_aabbQueryMax[2]);
11435
11436 m_data->m_cachedOverlappingObjects.clear();
11437
11438 m_data->m_dynamicsWorld->getBroadphase()->aabbTest(aabbMin, aabbMax, m_data->m_cachedOverlappingObjects);
11439 }
11440
11441 int totalBytesPerObject = sizeof(b3OverlappingObject);
11442 int overlapCapacity = bufferSizeInBytes / totalBytesPerObject - 1;
11443 int numOverlap = m_data->m_cachedOverlappingObjects.m_bodyUniqueIds.size();
11444 int remainingObjects = numOverlap - curObjectIndex;
11445
11446 int curNumObjects = btMin(overlapCapacity, remainingObjects);
11447
11448 if (numOverlap < overlapCapacity)
11449 {
11450 b3OverlappingObject* overlapStorage = (b3OverlappingObject*)bufferServerToClient;
11451 for (int i = 0; i < m_data->m_cachedOverlappingObjects.m_bodyUniqueIds.size(); i++)
11452 {
11453 overlapStorage[i].m_objectUniqueId = m_data->m_cachedOverlappingObjects.m_bodyUniqueIds[i];
11454 overlapStorage[i].m_linkIndex = m_data->m_cachedOverlappingObjects.m_links[i];
11455 }
11456 serverCmd.m_numDataStreamBytes = numOverlap * totalBytesPerObject;
11457 serverCmd.m_type = CMD_REQUEST_AABB_OVERLAP_COMPLETED;
11458
11459 //int m_startingOverlappingObjectIndex;
11460 //int m_numOverlappingObjectsCopied;
11461 //int m_numRemainingOverlappingObjects;
11462 serverCmd.m_sendOverlappingObjectsArgs.m_startingOverlappingObjectIndex = clientCmd.m_requestOverlappingObjectsArgs.m_startingOverlappingObjectIndex;
11463 serverCmd.m_sendOverlappingObjectsArgs.m_numOverlappingObjectsCopied = m_data->m_cachedOverlappingObjects.m_bodyUniqueIds.size();
11464 serverCmd.m_sendOverlappingObjectsArgs.m_numRemainingOverlappingObjects = remainingObjects - curNumObjects;
11465 }
11466 else
11467 {
11468 serverCmd.m_type = CMD_REQUEST_AABB_OVERLAP_FAILED;
11469 }
11470 return hasStatus;
11471 }
11472
processRequestOpenGLVisualizeCameraCommand(const struct SharedMemoryCommand & clientCmd,struct SharedMemoryStatus & serverStatusOut,char * bufferServerToClient,int bufferSizeInBytes)11473 bool PhysicsServerCommandProcessor::processRequestOpenGLVisualizeCameraCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
11474 {
11475 bool hasStatus = true;
11476
11477 BT_PROFILE("CMD_REQUEST_OPENGL_VISUALIZER_CAMERA");
11478 SharedMemoryStatus& serverCmd = serverStatusOut;
11479 bool result = this->m_data->m_guiHelper->getCameraInfo(
11480 &serverCmd.m_visualizerCameraResultArgs.m_width,
11481 &serverCmd.m_visualizerCameraResultArgs.m_height,
11482 serverCmd.m_visualizerCameraResultArgs.m_viewMatrix,
11483 serverCmd.m_visualizerCameraResultArgs.m_projectionMatrix,
11484 serverCmd.m_visualizerCameraResultArgs.m_camUp,
11485 serverCmd.m_visualizerCameraResultArgs.m_camForward,
11486 serverCmd.m_visualizerCameraResultArgs.m_horizontal,
11487 serverCmd.m_visualizerCameraResultArgs.m_vertical,
11488 &serverCmd.m_visualizerCameraResultArgs.m_yaw,
11489 &serverCmd.m_visualizerCameraResultArgs.m_pitch,
11490 &serverCmd.m_visualizerCameraResultArgs.m_dist,
11491 serverCmd.m_visualizerCameraResultArgs.m_target);
11492 serverCmd.m_type = result ? CMD_REQUEST_OPENGL_VISUALIZER_CAMERA_COMPLETED : CMD_REQUEST_OPENGL_VISUALIZER_CAMERA_FAILED;
11493 return hasStatus;
11494 }
11495
processConfigureOpenGLVisualizerCommand(const struct SharedMemoryCommand & clientCmd,struct SharedMemoryStatus & serverStatusOut,char * bufferServerToClient,int bufferSizeInBytes)11496 bool PhysicsServerCommandProcessor::processConfigureOpenGLVisualizerCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
11497 {
11498 bool hasStatus = true;
11499
11500 BT_PROFILE("CMD_CONFIGURE_OPENGL_VISUALIZER");
11501 SharedMemoryStatus& serverCmd = serverStatusOut;
11502 serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED;
11503
11504 hasStatus = true;
11505
11506 if (clientCmd.m_updateFlags & COV_SET_FLAGS)
11507 {
11508 if (clientCmd.m_configureOpenGLVisualizerArguments.m_setFlag == COV_ENABLE_TINY_RENDERER)
11509 {
11510 m_data->m_enableTinyRenderer = clientCmd.m_configureOpenGLVisualizerArguments.m_setEnabled != 0;
11511 }
11512 m_data->m_guiHelper->setVisualizerFlag(clientCmd.m_configureOpenGLVisualizerArguments.m_setFlag,
11513 clientCmd.m_configureOpenGLVisualizerArguments.m_setEnabled);
11514 }
11515 if (clientCmd.m_updateFlags & COV_SET_CAMERA_VIEW_MATRIX)
11516 {
11517 m_data->m_guiHelper->resetCamera(clientCmd.m_configureOpenGLVisualizerArguments.m_cameraDistance,
11518 clientCmd.m_configureOpenGLVisualizerArguments.m_cameraYaw,
11519 clientCmd.m_configureOpenGLVisualizerArguments.m_cameraPitch,
11520 clientCmd.m_configureOpenGLVisualizerArguments.m_cameraTargetPosition[0],
11521 clientCmd.m_configureOpenGLVisualizerArguments.m_cameraTargetPosition[1],
11522 clientCmd.m_configureOpenGLVisualizerArguments.m_cameraTargetPosition[2]);
11523 }
11524 if (m_data->m_guiHelper->getRenderInterface())
11525 {
11526 if (clientCmd.m_updateFlags & COV_SET_LIGHT_POSITION)
11527 {
11528 m_data->m_guiHelper->getRenderInterface()->setLightPosition(clientCmd.m_configureOpenGLVisualizerArguments.m_lightPosition);
11529 }
11530 if (clientCmd.m_updateFlags & COV_SET_RGB_BACKGROUND)
11531 {
11532 m_data->m_guiHelper->setBackgroundColor(clientCmd.m_configureOpenGLVisualizerArguments.m_rgbBackground);
11533 }
11534 if (clientCmd.m_updateFlags & COV_SET_SHADOWMAP_RESOLUTION)
11535 {
11536 m_data->m_guiHelper->getRenderInterface()->setShadowMapResolution(clientCmd.m_configureOpenGLVisualizerArguments.m_shadowMapResolution);
11537 }
11538
11539 if (clientCmd.m_updateFlags & COV_SET_SHADOWMAP_INTENSITY)
11540 {
11541 m_data->m_guiHelper->getRenderInterface()->setShadowMapIntensity(clientCmd.m_configureOpenGLVisualizerArguments.m_shadowMapIntensity);
11542 }
11543
11544
11545 if (clientCmd.m_updateFlags & COV_SET_SHADOWMAP_WORLD_SIZE)
11546 {
11547 float worldSize = clientCmd.m_configureOpenGLVisualizerArguments.m_shadowMapWorldSize;
11548 m_data->m_guiHelper->getRenderInterface()->setShadowMapWorldSize(worldSize);
11549 }
11550 }
11551
11552 if (clientCmd.m_updateFlags & COV_SET_REMOTE_SYNC_TRANSFORM_INTERVAL)
11553 {
11554 m_data->m_remoteSyncTransformInterval = clientCmd.m_configureOpenGLVisualizerArguments.m_remoteSyncTransformInterval;
11555 }
11556
11557 return hasStatus;
11558 }
11559
processInverseDynamicsCommand(const struct SharedMemoryCommand & clientCmd,struct SharedMemoryStatus & serverStatusOut,char * bufferServerToClient,int bufferSizeInBytes)11560 bool PhysicsServerCommandProcessor::processInverseDynamicsCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
11561 {
11562 bool hasStatus = true;
11563
11564 BT_PROFILE("CMD_CALCULATE_INVERSE_DYNAMICS");
11565 SharedMemoryStatus& serverCmd = serverStatusOut;
11566 InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId);
11567 serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_FAILED;
11568 if (bodyHandle && bodyHandle->m_multiBody)
11569 {
11570 if (clientCmd.m_calculateInverseDynamicsArguments.m_flags & 1)
11571 {
11572 #ifdef STATIC_LINK_SPD_PLUGIN
11573 {
11574 cRBDModel* rbdModel = m_data->findOrCreateRBDModel(bodyHandle->m_multiBody,
11575 clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ,
11576 clientCmd.m_calculateInverseDynamicsArguments.m_jointVelocitiesQdot);
11577 if (rbdModel)
11578 {
11579 int posVal = bodyHandle->m_multiBody->getNumPosVars();
11580
11581 Eigen::VectorXd acc2 = Eigen::VectorXd::Zero(7 + posVal);
11582 Eigen::VectorXd out_tau = Eigen::VectorXd::Zero(7 + posVal);
11583 cRBDUtil::SolveInvDyna(*rbdModel, acc2, out_tau);
11584 int dof = 7 + bodyHandle->m_multiBody->getNumPosVars();
11585 for (int i = 0; i < dof; i++)
11586 {
11587 serverCmd.m_inverseDynamicsResultArgs.m_jointForces[i] = out_tau[i];
11588 }
11589 serverCmd.m_inverseDynamicsResultArgs.m_bodyUniqueId = clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId;
11590 serverCmd.m_inverseDynamicsResultArgs.m_dofCount = dof;
11591
11592 serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED;
11593 }
11594 }
11595 #endif
11596 }
11597 else
11598 {
11599 btInverseDynamics::MultiBodyTree* tree = m_data->findOrCreateTree(bodyHandle->m_multiBody);
11600
11601 int baseDofQ = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 7;
11602 int baseDofQdot = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 6;
11603 const int num_dofs = bodyHandle->m_multiBody->getNumDofs();
11604
11605 if (tree && clientCmd.m_calculateInverseDynamicsArguments.m_dofCountQ == (baseDofQ + num_dofs) &&
11606 clientCmd.m_calculateInverseDynamicsArguments.m_dofCountQdot == (baseDofQdot + num_dofs))
11607 {
11608 btInverseDynamics::vecx nu(num_dofs + baseDofQdot), qdot(num_dofs + baseDofQdot), q(num_dofs + baseDofQdot), joint_force(num_dofs + baseDofQdot);
11609
11610 //for floating base, inverse dynamics expects euler angle x,y,z and position x,y,z in that order
11611 //PyBullet expects xyz and quaternion in that order, so convert and swap to have a more consistent PyBullet API
11612 if (baseDofQ)
11613 {
11614 btVector3 pos(clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[0],
11615 clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[1],
11616 clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[2]);
11617
11618 btQuaternion orn(clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[3],
11619 clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[4],
11620 clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[5],
11621 clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[6]);
11622 btScalar yawZ, pitchY, rollX;
11623 orn.getEulerZYX(yawZ, pitchY, rollX);
11624 q[0] = rollX;
11625 q[1] = pitchY;
11626 q[2] = yawZ;
11627 q[3] = pos[0];
11628 q[4] = pos[1];
11629 q[5] = pos[2];
11630 }
11631 for (int i = 0; i < num_dofs; i++)
11632 {
11633 q[i + baseDofQ] = clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[i + baseDofQ];
11634 }
11635 for (int i = 0; i < num_dofs + baseDofQdot; i++)
11636 {
11637 qdot[i] = clientCmd.m_calculateInverseDynamicsArguments.m_jointVelocitiesQdot[i];
11638 nu[i] = clientCmd.m_calculateInverseDynamicsArguments.m_jointAccelerations[i];
11639 }
11640
11641 // Set the gravity to correspond to the world gravity
11642 btInverseDynamics::vec3 id_grav(m_data->m_dynamicsWorld->getGravity());
11643
11644 if (-1 != tree->setGravityInWorldFrame(id_grav) &&
11645 -1 != tree->calculateInverseDynamics(q, qdot, nu, &joint_force))
11646 {
11647 serverCmd.m_inverseDynamicsResultArgs.m_bodyUniqueId = clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId;
11648 serverCmd.m_inverseDynamicsResultArgs.m_dofCount = num_dofs + baseDofQdot;
11649
11650 //inverse dynamics stores angular before linear, swap it to have a consistent PyBullet API.
11651 if (baseDofQdot)
11652 {
11653 serverCmd.m_inverseDynamicsResultArgs.m_jointForces[0] = joint_force[3];
11654 serverCmd.m_inverseDynamicsResultArgs.m_jointForces[1] = joint_force[4];
11655 serverCmd.m_inverseDynamicsResultArgs.m_jointForces[2] = joint_force[5];
11656 serverCmd.m_inverseDynamicsResultArgs.m_jointForces[3] = joint_force[0];
11657 serverCmd.m_inverseDynamicsResultArgs.m_jointForces[4] = joint_force[1];
11658 serverCmd.m_inverseDynamicsResultArgs.m_jointForces[5] = joint_force[2];
11659 }
11660
11661 for (int i = baseDofQdot; i < num_dofs + baseDofQdot; i++)
11662 {
11663 serverCmd.m_inverseDynamicsResultArgs.m_jointForces[i] = joint_force[i];
11664 }
11665 serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED;
11666 }
11667 else
11668 {
11669 serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_FAILED;
11670 }
11671 }
11672 }
11673 }
11674 else
11675 {
11676 serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_FAILED;
11677 }
11678
11679 return hasStatus;
11680 }
11681
processCalculateJacobianCommand(const struct SharedMemoryCommand & clientCmd,struct SharedMemoryStatus & serverStatusOut,char * bufferServerToClient,int bufferSizeInBytes)11682 bool PhysicsServerCommandProcessor::processCalculateJacobianCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
11683 {
11684 bool hasStatus = true;
11685 BT_PROFILE("CMD_CALCULATE_JACOBIAN");
11686
11687 SharedMemoryStatus& serverCmd = serverStatusOut;
11688 InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(clientCmd.m_calculateJacobianArguments.m_bodyUniqueId);
11689 if (bodyHandle && bodyHandle->m_multiBody)
11690 {
11691 serverCmd.m_type = CMD_CALCULATED_JACOBIAN_FAILED;
11692
11693 btInverseDynamics::MultiBodyTree* tree = m_data->findOrCreateTree(bodyHandle->m_multiBody);
11694
11695 if (tree)
11696 {
11697 int baseDofs = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 6;
11698 const int numDofs = bodyHandle->m_multiBody->getNumDofs();
11699 btInverseDynamics::vecx q(numDofs + baseDofs);
11700 btInverseDynamics::vecx qdot(numDofs + baseDofs);
11701 btInverseDynamics::vecx nu(numDofs + baseDofs);
11702 btInverseDynamics::vecx joint_force(numDofs + baseDofs);
11703 for (int i = 0; i < numDofs; i++)
11704 {
11705 q[i + baseDofs] = clientCmd.m_calculateJacobianArguments.m_jointPositionsQ[i];
11706 qdot[i + baseDofs] = clientCmd.m_calculateJacobianArguments.m_jointVelocitiesQdot[i];
11707 nu[i + baseDofs] = clientCmd.m_calculateJacobianArguments.m_jointAccelerations[i];
11708 }
11709 // Set the gravity to correspond to the world gravity
11710 btInverseDynamics::vec3 id_grav(m_data->m_dynamicsWorld->getGravity());
11711 if (-1 != tree->setGravityInWorldFrame(id_grav) &&
11712 -1 != tree->calculateInverseDynamics(q, qdot, nu, &joint_force))
11713 {
11714 serverCmd.m_jacobianResultArgs.m_dofCount = numDofs + baseDofs;
11715 // Set jacobian value
11716 tree->calculateJacobians(q);
11717 btInverseDynamics::mat3x jac_t(3, numDofs + baseDofs);
11718 btInverseDynamics::mat3x jac_r(3, numDofs + baseDofs);
11719
11720 // Note that inverse dynamics uses zero-based indexing of bodies, not starting from -1 for the base link.
11721 tree->getBodyJacobianTrans(clientCmd.m_calculateJacobianArguments.m_linkIndex + 1, &jac_t);
11722 tree->getBodyJacobianRot(clientCmd.m_calculateJacobianArguments.m_linkIndex + 1, &jac_r);
11723 // Update the translational jacobian based on the desired local point.
11724 // v_pt = v_frame + w x pt
11725 // v_pt = J_t * qd + (J_r * qd) x pt
11726 // v_pt = J_t * qd - pt x (J_r * qd)
11727 // v_pt = J_t * qd - pt_x * J_r * qd)
11728 // v_pt = (J_t - pt_x * J_r) * qd
11729 // J_t_new = J_t - pt_x * J_r
11730 btInverseDynamics::vec3 localPosition;
11731 for (int i = 0; i < 3; ++i)
11732 {
11733 localPosition(i) = clientCmd.m_calculateJacobianArguments.m_localPosition[i];
11734 }
11735 // Only calculate if the localPosition is non-zero.
11736 if (btInverseDynamics::maxAbs(localPosition) > 0.0)
11737 {
11738 // Write the localPosition into world coordinates.
11739 btInverseDynamics::mat33 world_rotation_body;
11740 tree->getBodyTransform(clientCmd.m_calculateJacobianArguments.m_linkIndex + 1, &world_rotation_body);
11741 localPosition = world_rotation_body * localPosition;
11742 // Correct the translational jacobian.
11743 btInverseDynamics::mat33 skewCrossProduct;
11744 btInverseDynamics::skew(localPosition, &skewCrossProduct);
11745 btInverseDynamics::mat3x jac_l(3, numDofs + baseDofs);
11746 btInverseDynamics::mul(skewCrossProduct, jac_r, &jac_l);
11747 btInverseDynamics::mat3x jac_t_new(3, numDofs + baseDofs);
11748 btInverseDynamics::sub(jac_t, jac_l, &jac_t_new);
11749 jac_t = jac_t_new;
11750 }
11751 // Fill in the result into the shared memory.
11752 for (int i = 0; i < 3; ++i)
11753 {
11754 for (int j = 0; j < (numDofs + baseDofs); ++j)
11755 {
11756 int element = (numDofs + baseDofs) * i + j;
11757 serverCmd.m_jacobianResultArgs.m_linearJacobian[element] = jac_t(i, j);
11758 serverCmd.m_jacobianResultArgs.m_angularJacobian[element] = jac_r(i, j);
11759 }
11760 }
11761 serverCmd.m_type = CMD_CALCULATED_JACOBIAN_COMPLETED;
11762 }
11763 else
11764 {
11765 serverCmd.m_type = CMD_CALCULATED_JACOBIAN_FAILED;
11766 }
11767 }
11768 }
11769 else
11770 {
11771 serverCmd.m_type = CMD_CALCULATED_JACOBIAN_FAILED;
11772 }
11773
11774 return hasStatus;
11775 }
11776
processCalculateMassMatrixCommand(const struct SharedMemoryCommand & clientCmd,struct SharedMemoryStatus & serverStatusOut,char * bufferServerToClient,int bufferSizeInBytes)11777 bool PhysicsServerCommandProcessor::processCalculateMassMatrixCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
11778 {
11779 bool hasStatus = true;
11780 BT_PROFILE("CMD_CALCULATE_MASS_MATRIX");
11781
11782 SharedMemoryStatus& serverCmd = serverStatusOut;
11783 serverCmd.m_type = CMD_CALCULATED_MASS_MATRIX_FAILED;
11784 InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(clientCmd.m_calculateMassMatrixArguments.m_bodyUniqueId);
11785 if (bodyHandle && bodyHandle->m_multiBody)
11786 {
11787 if (clientCmd.m_calculateMassMatrixArguments.m_flags & 1)
11788 {
11789 #ifdef STATIC_LINK_SPD_PLUGIN
11790 {
11791 int posVal = bodyHandle->m_multiBody->getNumPosVars();
11792 btAlignedObjectArray<double> zeroVel;
11793 int dof = 7 + posVal;
11794 zeroVel.resize(dof);
11795 cRBDModel* rbdModel = m_data->findOrCreateRBDModel(bodyHandle->m_multiBody, clientCmd.m_calculateMassMatrixArguments.m_jointPositionsQ,
11796 &zeroVel[0]);
11797 if (rbdModel)
11798 {
11799 //Eigen::MatrixXd out_mass;
11800 //cRBDUtil::BuildMassMat(*rbdModel, out_mass);
11801 const Eigen::MatrixXd& out_mass = rbdModel->GetMassMat();
11802 int skipDofs = 0; // 7 - baseDofQ;
11803 int totDofs = dof;
11804 serverCmd.m_massMatrixResultArgs.m_dofCount = totDofs - skipDofs;
11805 // Fill in the result into the shared memory.
11806 double* sharedBuf = (double*)bufferServerToClient;
11807 int sizeInBytes = totDofs * totDofs * sizeof(double);
11808 if (sizeInBytes < bufferSizeInBytes)
11809 {
11810 for (int i = skipDofs; i < (totDofs); ++i)
11811 {
11812 for (int j = skipDofs; j < (totDofs); ++j)
11813 {
11814 int element = (totDofs - skipDofs) * (i - skipDofs) + (j - skipDofs);
11815 double v = out_mass(i, j);
11816 if (i == j && v == 0)
11817 {
11818 v = 1;
11819 }
11820 sharedBuf[element] = v;
11821 }
11822 }
11823 serverCmd.m_type = CMD_CALCULATED_MASS_MATRIX_COMPLETED;
11824 }
11825 }
11826 }
11827 #endif
11828 }
11829 else
11830 {
11831 btInverseDynamics::MultiBodyTree* tree = m_data->findOrCreateTree(bodyHandle->m_multiBody);
11832
11833 if (tree)
11834 {
11835 int baseDofs = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 6;
11836 const int numDofs = bodyHandle->m_multiBody->getNumDofs();
11837 const int totDofs = numDofs + baseDofs;
11838 btInverseDynamics::vecx q(totDofs);
11839 btInverseDynamics::matxx massMatrix(totDofs, totDofs);
11840 for (int i = 0; i < numDofs; i++)
11841 {
11842 q[i + baseDofs] = clientCmd.m_calculateMassMatrixArguments.m_jointPositionsQ[i];
11843 }
11844 if (-1 != tree->calculateMassMatrix(q, &massMatrix))
11845 {
11846 serverCmd.m_massMatrixResultArgs.m_dofCount = totDofs;
11847 // Fill in the result into the shared memory.
11848 double* sharedBuf = (double*)bufferServerToClient;
11849 int sizeInBytes = totDofs * totDofs * sizeof(double);
11850 if (sizeInBytes < bufferSizeInBytes)
11851 {
11852 for (int i = 0; i < (totDofs); ++i)
11853 {
11854 for (int j = 0; j < (totDofs); ++j)
11855 {
11856 int element = (totDofs)*i + j;
11857
11858 sharedBuf[element] = massMatrix(i, j);
11859 }
11860 }
11861 serverCmd.m_numDataStreamBytes = sizeInBytes;
11862 serverCmd.m_type = CMD_CALCULATED_MASS_MATRIX_COMPLETED;
11863 }
11864 }
11865 }
11866 }
11867 }
11868 else
11869 {
11870 serverCmd.m_type = CMD_CALCULATED_MASS_MATRIX_FAILED;
11871 }
11872
11873 return hasStatus;
11874 }
11875
processApplyExternalForceCommand(const struct SharedMemoryCommand & clientCmd,struct SharedMemoryStatus & serverStatusOut,char * bufferServerToClient,int bufferSizeInBytes)11876 bool PhysicsServerCommandProcessor::processApplyExternalForceCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
11877 {
11878 bool hasStatus = true;
11879
11880 BT_PROFILE("CMD_APPLY_EXTERNAL_FORCE");
11881
11882 if (m_data->m_verboseOutput)
11883 {
11884 b3Printf("CMD_APPLY_EXTERNAL_FORCE clientCmd = %d\n", clientCmd.m_sequenceNumber);
11885 }
11886 for (int i = 0; i < clientCmd.m_externalForceArguments.m_numForcesAndTorques; ++i)
11887 {
11888 InternalBodyData* body = m_data->m_bodyHandles.getHandle(clientCmd.m_externalForceArguments.m_bodyUniqueIds[i]);
11889 bool isLinkFrame = ((clientCmd.m_externalForceArguments.m_forceFlags[i] & EF_LINK_FRAME) != 0);
11890
11891 if (body && body->m_multiBody)
11892 {
11893 btMultiBody* mb = body->m_multiBody;
11894
11895 if ((clientCmd.m_externalForceArguments.m_forceFlags[i] & EF_FORCE) != 0)
11896 {
11897 btVector3 tmpForce(clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 0],
11898 clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 1],
11899 clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 2]);
11900 btVector3 tmpPosition(
11901 clientCmd.m_externalForceArguments.m_positions[i * 3 + 0],
11902 clientCmd.m_externalForceArguments.m_positions[i * 3 + 1],
11903 clientCmd.m_externalForceArguments.m_positions[i * 3 + 2]);
11904
11905 if (clientCmd.m_externalForceArguments.m_linkIds[i] == -1)
11906 {
11907 btVector3 forceWorld = isLinkFrame ? mb->getBaseWorldTransform().getBasis() * tmpForce : tmpForce;
11908 btVector3 relPosWorld = isLinkFrame ? mb->getBaseWorldTransform().getBasis() * tmpPosition : tmpPosition - mb->getBaseWorldTransform().getOrigin();
11909 mb->addBaseForce(forceWorld);
11910 mb->addBaseTorque(relPosWorld.cross(forceWorld));
11911 //b3Printf("apply base force of %f,%f,%f at %f,%f,%f\n", forceWorld[0],forceWorld[1],forceWorld[2],positionLocal[0],positionLocal[1],positionLocal[2]);
11912 }
11913 else
11914 {
11915 int link = clientCmd.m_externalForceArguments.m_linkIds[i];
11916 btVector3 forceWorld = isLinkFrame ? mb->getLink(link).m_cachedWorldTransform.getBasis() * tmpForce : tmpForce;
11917 btVector3 relPosWorld = isLinkFrame ? mb->getLink(link).m_cachedWorldTransform.getBasis() * tmpPosition : tmpPosition - mb->getLink(link).m_cachedWorldTransform.getOrigin();
11918 mb->addLinkForce(link, forceWorld);
11919 mb->addLinkTorque(link, relPosWorld.cross(forceWorld));
11920 //b3Printf("apply link force of %f,%f,%f at %f,%f,%f\n", forceWorld[0],forceWorld[1],forceWorld[2], positionLocal[0],positionLocal[1],positionLocal[2]);
11921 }
11922 }
11923 if ((clientCmd.m_externalForceArguments.m_forceFlags[i] & EF_TORQUE) != 0)
11924 {
11925 btVector3 tmpTorque(clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 0],
11926 clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 1],
11927 clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 2]);
11928
11929 if (clientCmd.m_externalForceArguments.m_linkIds[i] == -1)
11930 {
11931 btVector3 torqueWorld = isLinkFrame ? mb->getBaseWorldTransform().getBasis() * tmpTorque : tmpTorque;
11932 mb->addBaseTorque(torqueWorld);
11933 //b3Printf("apply base torque of %f,%f,%f\n", torqueWorld[0],torqueWorld[1],torqueWorld[2]);
11934 }
11935 else
11936 {
11937 int link = clientCmd.m_externalForceArguments.m_linkIds[i];
11938 btVector3 torqueWorld = isLinkFrame ? mb->getLink(link).m_cachedWorldTransform.getBasis() * tmpTorque : tmpTorque;
11939 mb->addLinkTorque(link, torqueWorld);
11940 //b3Printf("apply link torque of %f,%f,%f\n", torqueWorld[0],torqueWorld[1],torqueWorld[2]);
11941 }
11942 }
11943 }
11944
11945 if (body && body->m_rigidBody)
11946 {
11947 btRigidBody* rb = body->m_rigidBody;
11948 if ((clientCmd.m_externalForceArguments.m_forceFlags[i] & EF_FORCE) != 0)
11949 {
11950 btVector3 tmpForce(clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 0],
11951 clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 1],
11952 clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 2]);
11953 btVector3 tmpPosition(
11954 clientCmd.m_externalForceArguments.m_positions[i * 3 + 0],
11955 clientCmd.m_externalForceArguments.m_positions[i * 3 + 1],
11956 clientCmd.m_externalForceArguments.m_positions[i * 3 + 2]);
11957
11958 btVector3 forceWorld = isLinkFrame ? rb->getWorldTransform().getBasis() * tmpForce : tmpForce;
11959 btVector3 relPosWorld = isLinkFrame ? rb->getWorldTransform().getBasis() * tmpPosition : tmpPosition - rb->getWorldTransform().getOrigin();
11960 rb->applyForce(forceWorld, relPosWorld);
11961 }
11962
11963 if ((clientCmd.m_externalForceArguments.m_forceFlags[i] & EF_TORQUE) != 0)
11964 {
11965 btVector3 tmpTorque(clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 0],
11966 clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 1],
11967 clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 2]);
11968
11969 btVector3 torqueWorld = isLinkFrame ? rb->getWorldTransform().getBasis() * tmpTorque : tmpTorque;
11970 rb->applyTorque(torqueWorld);
11971 }
11972 }
11973
11974 #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
11975 if (body && body->m_softBody)
11976 {
11977 btSoftBody* sb = body->m_softBody;
11978 int link = clientCmd.m_externalForceArguments.m_linkIds[i];
11979 if ((clientCmd.m_externalForceArguments.m_forceFlags[i] & EF_FORCE) != 0)
11980 {
11981 btVector3 tmpForce(clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 0],
11982 clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 1],
11983 clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 2]);
11984 btVector3 tmpPosition(
11985 clientCmd.m_externalForceArguments.m_positions[i * 3 + 0],
11986 clientCmd.m_externalForceArguments.m_positions[i * 3 + 1],
11987 clientCmd.m_externalForceArguments.m_positions[i * 3 + 2]);
11988
11989 btVector3 forceWorld = isLinkFrame ? sb->getWorldTransform().getBasis() * tmpForce : tmpForce;
11990 btVector3 relPosWorld = isLinkFrame ? sb->getWorldTransform().getBasis() * tmpPosition : tmpPosition - sb->getWorldTransform().getOrigin();
11991 if (link >= 0 && link < sb->m_nodes.size())
11992 {
11993 sb->addForce(forceWorld, link);
11994 }
11995 }
11996 }
11997 #endif
11998
11999 }
12000
12001 SharedMemoryStatus& serverCmd = serverStatusOut;
12002 serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED;
12003 return hasStatus;
12004 }
12005
processRemoveBodyCommand(const struct SharedMemoryCommand & clientCmd,struct SharedMemoryStatus & serverStatusOut,char * bufferServerToClient,int bufferSizeInBytes)12006 bool PhysicsServerCommandProcessor::processRemoveBodyCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
12007 {
12008 bool hasStatus = true;
12009
12010 SharedMemoryStatus& serverCmd = serverStatusOut;
12011 serverCmd.m_type = CMD_REMOVE_BODY_FAILED;
12012 serverCmd.m_removeObjectArgs.m_numBodies = 0;
12013 serverCmd.m_removeObjectArgs.m_numUserConstraints = 0;
12014
12015 m_data->m_guiHelper->setVisualizerFlag(COV_ENABLE_SYNC_RENDERING_INTERNAL, 0);
12016
12017 for (int i = 0; i < clientCmd.m_removeObjectArgs.m_numBodies; i++)
12018 {
12019 int bodyUniqueId = clientCmd.m_removeObjectArgs.m_bodyUniqueIds[i];
12020 InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(bodyUniqueId);
12021 if (bodyHandle)
12022 {
12023 #ifndef USE_DISCRETE_DYNAMICS_WORLD
12024 if (bodyHandle->m_multiBody)
12025 {
12026 serverCmd.m_removeObjectArgs.m_bodyUniqueIds[serverCmd.m_removeObjectArgs.m_numBodies++] = bodyUniqueId;
12027
12028 if (m_data->m_pickingMultiBodyPoint2Point && m_data->m_pickingMultiBodyPoint2Point->getMultiBodyA() == bodyHandle->m_multiBody)
12029 {
12030 //memory will be deleted in the code that follows
12031 m_data->m_pickingMultiBodyPoint2Point = 0;
12032 }
12033
12034 //also remove user constraints...
12035 for (int i = m_data->m_dynamicsWorld->getNumMultiBodyConstraints() - 1; i >= 0; i--)
12036 {
12037 btMultiBodyConstraint* mbc = m_data->m_dynamicsWorld->getMultiBodyConstraint(i);
12038 if ((mbc->getMultiBodyA() == bodyHandle->m_multiBody) || (mbc->getMultiBodyB() == bodyHandle->m_multiBody))
12039 {
12040 m_data->m_dynamicsWorld->removeMultiBodyConstraint(mbc);
12041
12042 //also remove user constraint and submit it as removed
12043 for (int c = m_data->m_userConstraints.size() - 1; c >= 0; c--)
12044 {
12045 InteralUserConstraintData* userConstraintPtr = m_data->m_userConstraints.getAtIndex(c);
12046 int userConstraintKey = m_data->m_userConstraints.getKeyAtIndex(c).getUid1();
12047
12048 if (userConstraintPtr->m_mbConstraint == mbc)
12049 {
12050 m_data->m_userConstraints.remove(userConstraintKey);
12051 serverCmd.m_removeObjectArgs.m_userConstraintUniqueIds[serverCmd.m_removeObjectArgs.m_numUserConstraints++] = userConstraintKey;
12052 }
12053 }
12054
12055 delete mbc;
12056 }
12057 }
12058
12059 if (bodyHandle->m_multiBody->getBaseCollider())
12060 {
12061 if (m_data->m_pluginManager.getRenderInterface())
12062 {
12063 m_data->m_pluginManager.getRenderInterface()->removeVisualShape(bodyHandle->m_multiBody->getBaseCollider()->getUserIndex3());
12064 }
12065 m_data->m_dynamicsWorld->removeCollisionObject(bodyHandle->m_multiBody->getBaseCollider());
12066 int graphicsIndex = bodyHandle->m_multiBody->getBaseCollider()->getUserIndex();
12067 m_data->m_guiHelper->removeGraphicsInstance(graphicsIndex);
12068 delete bodyHandle->m_multiBody->getBaseCollider();
12069 }
12070 for (int link = 0; link < bodyHandle->m_multiBody->getNumLinks(); link++)
12071 {
12072 btCollisionObject* colObj = bodyHandle->m_multiBody->getLink(link).m_collider;
12073 if (colObj)
12074 {
12075 if (m_data->m_pluginManager.getRenderInterface())
12076 {
12077 m_data->m_pluginManager.getRenderInterface()->removeVisualShape(bodyHandle->m_multiBody->getLink(link).m_collider->getUserIndex3());
12078 }
12079 m_data->m_dynamicsWorld->removeCollisionObject(bodyHandle->m_multiBody->getLink(link).m_collider);
12080 int graphicsIndex = bodyHandle->m_multiBody->getLink(link).m_collider->getUserIndex();
12081 m_data->m_guiHelper->removeGraphicsInstance(graphicsIndex);
12082 delete colObj;
12083 }
12084 }
12085 int numCollisionObjects = m_data->m_dynamicsWorld->getNumCollisionObjects();
12086 m_data->m_dynamicsWorld->removeMultiBody(bodyHandle->m_multiBody);
12087 numCollisionObjects = m_data->m_dynamicsWorld->getNumCollisionObjects();
12088
12089 delete bodyHandle->m_multiBody;
12090 bodyHandle->m_multiBody = 0;
12091 serverCmd.m_type = CMD_REMOVE_BODY_COMPLETED;
12092 }
12093 #endif
12094 if (bodyHandle->m_rigidBody)
12095 {
12096 if (m_data->m_pluginManager.getRenderInterface())
12097 {
12098 m_data->m_pluginManager.getRenderInterface()->removeVisualShape(bodyHandle->m_rigidBody->getUserIndex3());
12099 }
12100 serverCmd.m_removeObjectArgs.m_bodyUniqueIds[serverCmd.m_removeObjectArgs.m_numBodies++] = bodyUniqueId;
12101
12102 if (m_data->m_pickedConstraint && m_data->m_pickedBody == bodyHandle->m_rigidBody)
12103 {
12104 m_data->m_pickedConstraint = 0;
12105 m_data->m_pickedBody = 0;
12106 }
12107
12108 //todo: clear all other remaining data...
12109 m_data->m_dynamicsWorld->removeRigidBody(bodyHandle->m_rigidBody);
12110 int graphicsInstance = bodyHandle->m_rigidBody->getUserIndex2();
12111 m_data->m_guiHelper->removeGraphicsInstance(graphicsInstance);
12112 delete bodyHandle->m_rigidBody;
12113 bodyHandle->m_rigidBody = 0;
12114 serverCmd.m_type = CMD_REMOVE_BODY_COMPLETED;
12115 }
12116 #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
12117 if (bodyHandle->m_softBody)
12118 {
12119 btSoftBody* psb = bodyHandle->m_softBody;
12120 if (m_data->m_pluginManager.getRenderInterface())
12121 {
12122 m_data->m_pluginManager.getRenderInterface()->removeVisualShape(psb->getUserIndex3());
12123 }
12124 serverCmd.m_removeObjectArgs.m_bodyUniqueIds[serverCmd.m_removeObjectArgs.m_numBodies++] = bodyUniqueId;
12125 btSoftMultiBodyDynamicsWorld* softWorld = getSoftWorld();
12126 if (softWorld)
12127 {
12128 softWorld->removeSoftBody(psb);
12129 }
12130 btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld();
12131 if (deformWorld)
12132 {
12133 deformWorld->removeSoftBody(psb);
12134 }
12135
12136 int graphicsInstance = psb->getUserIndex2();
12137 m_data->m_guiHelper->removeGraphicsInstance(graphicsInstance);
12138 delete psb;
12139 psb = 0;
12140 serverCmd.m_type = CMD_REMOVE_BODY_COMPLETED;
12141 }
12142 #endif
12143 for (int i = 0; i < bodyHandle->m_userDataHandles.size(); i++)
12144 {
12145 int userDataHandle = bodyHandle->m_userDataHandles[i];
12146 SharedMemoryUserData* userData = m_data->m_userDataHandles.getHandle(userDataHandle);
12147 m_data->m_userDataHandleLookup.remove(SharedMemoryUserDataHashKey(userData));
12148 m_data->m_userDataHandles.freeHandle(userDataHandle);
12149 }
12150 m_data->m_bodyHandles.freeHandle(bodyUniqueId);
12151 }
12152 }
12153
12154 for (int i = 0; i < clientCmd.m_removeObjectArgs.m_numUserCollisionShapes; i++)
12155 {
12156 int removeCollisionShapeId = clientCmd.m_removeObjectArgs.m_userCollisionShapes[i];
12157 InternalCollisionShapeHandle* handle = m_data->m_userCollisionShapeHandles.getHandle(removeCollisionShapeId);
12158 if (handle && handle->m_collisionShape)
12159 {
12160 if (handle->m_used)
12161 {
12162 b3Warning("Don't remove collision shape: it is used.");
12163 }
12164 else
12165 {
12166 b3Warning("TODO: dealloc");
12167 int foundIndex = -1;
12168
12169 for (int i = 0; i < m_data->m_worldImporters.size(); i++)
12170 {
12171 #ifdef USE_DISCRETE_DYNAMICS_WORLD
12172 btWorldImporter* importer = m_data->m_worldImporters[i];
12173 #else
12174 btMultiBodyWorldImporter* importer = m_data->m_worldImporters[i];
12175 #endif
12176 for (int c = 0; c < importer->getNumCollisionShapes(); c++)
12177 {
12178 if (importer->getCollisionShapeByIndex(c) == handle->m_collisionShape)
12179 {
12180 if ((importer->getNumRigidBodies() == 0) &&
12181 (importer->getNumConstraints() == 0))
12182 {
12183 foundIndex = i;
12184 break;
12185 }
12186 }
12187 }
12188 }
12189 if (foundIndex >= 0)
12190 {
12191 #ifdef USE_DISCRETE_DYNAMICS_WORLD
12192 btWorldImporter* importer = m_data->m_worldImporters[foundIndex];
12193 #else
12194 btMultiBodyWorldImporter* importer = m_data->m_worldImporters[foundIndex];
12195 #endif
12196 m_data->m_worldImporters.removeAtIndex(foundIndex);
12197 importer->deleteAllData();
12198 delete importer;
12199 m_data->m_userCollisionShapeHandles.freeHandle(removeCollisionShapeId);
12200 serverCmd.m_type = CMD_REMOVE_BODY_COMPLETED;
12201 }
12202 }
12203 }
12204 }
12205
12206 m_data->m_guiHelper->setVisualizerFlag(COV_ENABLE_SYNC_RENDERING_INTERNAL, 1);
12207
12208 for (int i = 0; i < serverCmd.m_removeObjectArgs.m_numBodies; i++)
12209 {
12210 b3Notification notification;
12211 notification.m_notificationType = BODY_REMOVED;
12212 notification.m_bodyArgs.m_bodyUniqueId = serverCmd.m_removeObjectArgs.m_bodyUniqueIds[i];
12213 m_data->m_pluginManager.addNotification(notification);
12214 }
12215
12216 return hasStatus;
12217 }
12218
processCreateUserConstraintCommand(const struct SharedMemoryCommand & clientCmd,struct SharedMemoryStatus & serverStatusOut,char * bufferServerToClient,int bufferSizeInBytes)12219 bool PhysicsServerCommandProcessor::processCreateUserConstraintCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
12220 {
12221 bool hasStatus = true;
12222
12223 BT_PROFILE("CMD_USER_CONSTRAINT");
12224
12225 SharedMemoryStatus& serverCmd = serverStatusOut;
12226 serverCmd.m_type = CMD_USER_CONSTRAINT_FAILED;
12227 hasStatus = true;
12228 if (clientCmd.m_updateFlags & USER_CONSTRAINT_ADD_SOFT_BODY_ANCHOR)
12229 {
12230 #ifndef SKIP_DEFORMABLE_BODY
12231 InternalBodyHandle* sbodyHandle = m_data->m_bodyHandles.getHandle(clientCmd.m_userConstraintArguments.m_parentBodyIndex);
12232 if (sbodyHandle)
12233 {
12234 if (sbodyHandle->m_softBody)
12235 {
12236 int nodeIndex = clientCmd.m_userConstraintArguments.m_parentJointIndex;
12237 if (nodeIndex >= 0 && nodeIndex < sbodyHandle->m_softBody->m_nodes.size())
12238 {
12239 int bodyUniqueId = clientCmd.m_userConstraintArguments.m_childBodyIndex;
12240 if (bodyUniqueId < 0)
12241 {
12242 //fixed anchor (mass = 0)
12243 InteralUserConstraintData userConstraintData;
12244 userConstraintData.m_sbHandle = clientCmd.m_userConstraintArguments.m_parentBodyIndex;
12245 userConstraintData.m_sbNodeIndex = nodeIndex;
12246 userConstraintData.m_sbNodeMass = sbodyHandle->m_softBody->getMass(nodeIndex);
12247 sbodyHandle->m_softBody->setMass(nodeIndex, 0.0);
12248 int uid = m_data->m_userConstraintUIDGenerator++;
12249 m_data->m_userConstraints.insert(uid, userConstraintData);
12250 serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid;
12251 serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED;
12252 }
12253 else
12254 {
12255 InternalBodyHandle* mbodyHandle = m_data->m_bodyHandles.getHandle(bodyUniqueId);
12256 if (mbodyHandle && mbodyHandle->m_multiBody)
12257 {
12258 btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld();
12259 if (deformWorld)
12260 {
12261 int linkIndex = clientCmd.m_userConstraintArguments.m_childJointIndex;
12262 if (linkIndex < 0)
12263 {
12264 sbodyHandle->m_softBody->appendDeformableAnchor(nodeIndex, mbodyHandle->m_multiBody->getBaseCollider());
12265 }
12266 else
12267 {
12268 if (linkIndex < mbodyHandle->m_multiBody->getNumLinks())
12269 {
12270 sbodyHandle->m_softBody->appendDeformableAnchor(nodeIndex, mbodyHandle->m_multiBody->getLinkCollider(linkIndex));
12271 }
12272 }
12273 }
12274 }
12275 if (mbodyHandle && mbodyHandle->m_rigidBody)
12276 {
12277 btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld();
12278 if (deformWorld)
12279 {
12280 //todo: expose those values
12281 bool disableCollisionBetweenLinkedBodies = true;
12282 //btVector3 localPivot(0,0,0);
12283 sbodyHandle->m_softBody->appendDeformableAnchor(nodeIndex, mbodyHandle->m_rigidBody);
12284 }
12285
12286 #if 1
12287 btSoftMultiBodyDynamicsWorld* softWorld = getSoftWorld();
12288 if (softWorld)
12289 {
12290 bool disableCollisionBetweenLinkedBodies = true;
12291 btVector3 localPivot(clientCmd.m_userConstraintArguments.m_childFrame[0],
12292 clientCmd.m_userConstraintArguments.m_childFrame[1],
12293 clientCmd.m_userConstraintArguments.m_childFrame[2]);
12294
12295 sbodyHandle->m_softBody->appendAnchor(nodeIndex, mbodyHandle->m_rigidBody, localPivot, disableCollisionBetweenLinkedBodies);
12296 }
12297 #endif
12298 }
12299 int uid = m_data->m_userConstraintUIDGenerator++;
12300 serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid;
12301 InteralUserConstraintData userConstraintData;
12302 userConstraintData.m_sbHandle = clientCmd.m_userConstraintArguments.m_parentBodyIndex;
12303 userConstraintData.m_sbNodeIndex = nodeIndex;
12304 m_data->m_userConstraints.insert(uid, userConstraintData);
12305 serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED;
12306 }
12307 }
12308 }
12309 }
12310 #endif
12311 }
12312 if (clientCmd.m_updateFlags & USER_CONSTRAINT_REQUEST_STATE)
12313 {
12314 int constraintUid = clientCmd.m_userConstraintArguments.m_userConstraintUniqueId;
12315 InteralUserConstraintData* userConstraintPtr = m_data->m_userConstraints.find(constraintUid);
12316 if (userConstraintPtr)
12317 {
12318 serverCmd.m_userConstraintStateResultArgs.m_numDofs = 0;
12319 for (int i = 0; i < 6; i++)
12320 {
12321 serverCmd.m_userConstraintStateResultArgs.m_appliedConstraintForces[i] = 0;
12322 }
12323 if (userConstraintPtr->m_mbConstraint)
12324 {
12325 serverCmd.m_userConstraintStateResultArgs.m_numDofs = userConstraintPtr->m_mbConstraint->getNumRows();
12326 for (int i = 0; i < userConstraintPtr->m_mbConstraint->getNumRows(); i++)
12327 {
12328 serverCmd.m_userConstraintStateResultArgs.m_appliedConstraintForces[i] = userConstraintPtr->m_mbConstraint->getAppliedImpulse(i) / m_data->m_dynamicsWorld->getSolverInfo().m_timeStep;
12329 }
12330 serverCmd.m_type = CMD_USER_CONSTRAINT_REQUEST_STATE_COMPLETED;
12331 }
12332 }
12333 };
12334 if (clientCmd.m_updateFlags & USER_CONSTRAINT_REQUEST_INFO)
12335 {
12336 int userConstraintUidChange = clientCmd.m_userConstraintArguments.m_userConstraintUniqueId;
12337 InteralUserConstraintData* userConstraintPtr = m_data->m_userConstraints.find(userConstraintUidChange);
12338 if (userConstraintPtr)
12339 {
12340 serverCmd.m_userConstraintResultArgs = userConstraintPtr->m_userConstraintData;
12341
12342 serverCmd.m_type = CMD_USER_CONSTRAINT_INFO_COMPLETED;
12343 }
12344 }
12345 if (clientCmd.m_updateFlags & USER_CONSTRAINT_ADD_CONSTRAINT)
12346 {
12347 btScalar defaultMaxForce = 500.0;
12348 InternalBodyData* parentBody = m_data->m_bodyHandles.getHandle(clientCmd.m_userConstraintArguments.m_parentBodyIndex);
12349 #ifndef USE_DISCRETE_DYNAMICS_WORLD
12350 if (parentBody && parentBody->m_multiBody)
12351 {
12352 if ((clientCmd.m_userConstraintArguments.m_parentJointIndex >= -1) && clientCmd.m_userConstraintArguments.m_parentJointIndex < parentBody->m_multiBody->getNumLinks())
12353 {
12354 InternalBodyData* childBody = clientCmd.m_userConstraintArguments.m_childBodyIndex >= 0 ? m_data->m_bodyHandles.getHandle(clientCmd.m_userConstraintArguments.m_childBodyIndex) : 0;
12355 //also create a constraint with just a single multibody/rigid body without child
12356 //if (childBody)
12357 {
12358 btVector3 pivotInParent(clientCmd.m_userConstraintArguments.m_parentFrame[0], clientCmd.m_userConstraintArguments.m_parentFrame[1], clientCmd.m_userConstraintArguments.m_parentFrame[2]);
12359 btVector3 pivotInChild(clientCmd.m_userConstraintArguments.m_childFrame[0], clientCmd.m_userConstraintArguments.m_childFrame[1], clientCmd.m_userConstraintArguments.m_childFrame[2]);
12360 btMatrix3x3 frameInParent(btQuaternion(clientCmd.m_userConstraintArguments.m_parentFrame[3], clientCmd.m_userConstraintArguments.m_parentFrame[4], clientCmd.m_userConstraintArguments.m_parentFrame[5], clientCmd.m_userConstraintArguments.m_parentFrame[6]));
12361 btMatrix3x3 frameInChild(btQuaternion(clientCmd.m_userConstraintArguments.m_childFrame[3], clientCmd.m_userConstraintArguments.m_childFrame[4], clientCmd.m_userConstraintArguments.m_childFrame[5], clientCmd.m_userConstraintArguments.m_childFrame[6]));
12362 btVector3 jointAxis(clientCmd.m_userConstraintArguments.m_jointAxis[0], clientCmd.m_userConstraintArguments.m_jointAxis[1], clientCmd.m_userConstraintArguments.m_jointAxis[2]);
12363
12364 if (clientCmd.m_userConstraintArguments.m_jointType == eGearType)
12365 {
12366 if (childBody && childBody->m_multiBody)
12367 {
12368 if ((clientCmd.m_userConstraintArguments.m_childJointIndex >= -1) && (clientCmd.m_userConstraintArguments.m_childJointIndex < childBody->m_multiBody->getNumLinks()))
12369 {
12370 btMultiBodyGearConstraint* multibodyGear = new btMultiBodyGearConstraint(parentBody->m_multiBody, clientCmd.m_userConstraintArguments.m_parentJointIndex, childBody->m_multiBody, clientCmd.m_userConstraintArguments.m_childJointIndex, pivotInParent, pivotInChild, frameInParent, frameInChild);
12371 multibodyGear->setMaxAppliedImpulse(defaultMaxForce);
12372 m_data->m_dynamicsWorld->addMultiBodyConstraint(multibodyGear);
12373 InteralUserConstraintData userConstraintData;
12374 userConstraintData.m_mbConstraint = multibodyGear;
12375 int uid = m_data->m_userConstraintUIDGenerator++;
12376 serverCmd.m_userConstraintResultArgs = clientCmd.m_userConstraintArguments;
12377 serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid;
12378 serverCmd.m_userConstraintResultArgs.m_maxAppliedForce = defaultMaxForce;
12379 userConstraintData.m_userConstraintData = serverCmd.m_userConstraintResultArgs;
12380 m_data->m_userConstraints.insert(uid, userConstraintData);
12381 serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED;
12382 }
12383 }
12384 }
12385 else if (clientCmd.m_userConstraintArguments.m_jointType == eFixedType)
12386 {
12387 if (childBody && childBody->m_multiBody)
12388 {
12389 if ((clientCmd.m_userConstraintArguments.m_childJointIndex >= -1) && (clientCmd.m_userConstraintArguments.m_childJointIndex < childBody->m_multiBody->getNumLinks()))
12390 {
12391 btMultiBodyFixedConstraint* multibodyFixed = new btMultiBodyFixedConstraint(parentBody->m_multiBody, clientCmd.m_userConstraintArguments.m_parentJointIndex, childBody->m_multiBody, clientCmd.m_userConstraintArguments.m_childJointIndex, pivotInParent, pivotInChild, frameInParent, frameInChild);
12392 multibodyFixed->setMaxAppliedImpulse(defaultMaxForce);
12393 m_data->m_dynamicsWorld->addMultiBodyConstraint(multibodyFixed);
12394 InteralUserConstraintData userConstraintData;
12395 userConstraintData.m_mbConstraint = multibodyFixed;
12396 int uid = m_data->m_userConstraintUIDGenerator++;
12397 serverCmd.m_userConstraintResultArgs = clientCmd.m_userConstraintArguments;
12398 serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid;
12399 serverCmd.m_userConstraintResultArgs.m_maxAppliedForce = defaultMaxForce;
12400 userConstraintData.m_userConstraintData = serverCmd.m_userConstraintResultArgs;
12401 m_data->m_userConstraints.insert(uid, userConstraintData);
12402 serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED;
12403 }
12404 }
12405 else
12406 {
12407 btRigidBody* rb = childBody ? childBody->m_rigidBody : 0;
12408 btMultiBodyFixedConstraint* rigidbodyFixed = new btMultiBodyFixedConstraint(parentBody->m_multiBody, clientCmd.m_userConstraintArguments.m_parentJointIndex, rb, pivotInParent, pivotInChild, frameInParent, frameInChild);
12409 rigidbodyFixed->setMaxAppliedImpulse(defaultMaxForce);
12410 btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*)m_data->m_dynamicsWorld;
12411 world->addMultiBodyConstraint(rigidbodyFixed);
12412 InteralUserConstraintData userConstraintData;
12413 userConstraintData.m_mbConstraint = rigidbodyFixed;
12414 int uid = m_data->m_userConstraintUIDGenerator++;
12415 serverCmd.m_userConstraintResultArgs = clientCmd.m_userConstraintArguments;
12416 serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid;
12417 serverCmd.m_userConstraintResultArgs.m_maxAppliedForce = defaultMaxForce;
12418 userConstraintData.m_userConstraintData = serverCmd.m_userConstraintResultArgs;
12419 m_data->m_userConstraints.insert(uid, userConstraintData);
12420 serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED;
12421 }
12422 }
12423 else if (clientCmd.m_userConstraintArguments.m_jointType == ePrismaticType)
12424 {
12425 if (childBody && childBody->m_multiBody)
12426 {
12427 btMultiBodySliderConstraint* multibodySlider = new btMultiBodySliderConstraint(parentBody->m_multiBody, clientCmd.m_userConstraintArguments.m_parentJointIndex, childBody->m_multiBody, clientCmd.m_userConstraintArguments.m_childJointIndex, pivotInParent, pivotInChild, frameInParent, frameInChild, jointAxis);
12428 multibodySlider->setMaxAppliedImpulse(defaultMaxForce);
12429 m_data->m_dynamicsWorld->addMultiBodyConstraint(multibodySlider);
12430 InteralUserConstraintData userConstraintData;
12431 userConstraintData.m_mbConstraint = multibodySlider;
12432 int uid = m_data->m_userConstraintUIDGenerator++;
12433 serverCmd.m_userConstraintResultArgs = clientCmd.m_userConstraintArguments;
12434 serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid;
12435 serverCmd.m_userConstraintResultArgs.m_maxAppliedForce = defaultMaxForce;
12436 userConstraintData.m_userConstraintData = serverCmd.m_userConstraintResultArgs;
12437 m_data->m_userConstraints.insert(uid, userConstraintData);
12438 serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED;
12439 }
12440 else
12441 {
12442 btRigidBody* rb = childBody ? childBody->m_rigidBody : 0;
12443
12444 btMultiBodySliderConstraint* rigidbodySlider = new btMultiBodySliderConstraint(parentBody->m_multiBody, clientCmd.m_userConstraintArguments.m_parentJointIndex, rb, pivotInParent, pivotInChild, frameInParent, frameInChild, jointAxis);
12445 rigidbodySlider->setMaxAppliedImpulse(defaultMaxForce);
12446 btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*)m_data->m_dynamicsWorld;
12447 world->addMultiBodyConstraint(rigidbodySlider);
12448 InteralUserConstraintData userConstraintData;
12449 userConstraintData.m_mbConstraint = rigidbodySlider;
12450 int uid = m_data->m_userConstraintUIDGenerator++;
12451 serverCmd.m_userConstraintResultArgs = clientCmd.m_userConstraintArguments;
12452 serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid;
12453 serverCmd.m_userConstraintResultArgs.m_maxAppliedForce = defaultMaxForce;
12454 userConstraintData.m_userConstraintData = serverCmd.m_userConstraintResultArgs;
12455 m_data->m_userConstraints.insert(uid, userConstraintData);
12456 serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED;
12457 }
12458 }
12459 else if (clientCmd.m_userConstraintArguments.m_jointType == ePoint2PointType)
12460 {
12461 if (childBody && childBody->m_multiBody)
12462 {
12463 btMultiBodyPoint2Point* p2p = new btMultiBodyPoint2Point(parentBody->m_multiBody, clientCmd.m_userConstraintArguments.m_parentJointIndex, childBody->m_multiBody, clientCmd.m_userConstraintArguments.m_childJointIndex, pivotInParent, pivotInChild);
12464 p2p->setMaxAppliedImpulse(defaultMaxForce);
12465 m_data->m_dynamicsWorld->addMultiBodyConstraint(p2p);
12466 InteralUserConstraintData userConstraintData;
12467 userConstraintData.m_mbConstraint = p2p;
12468 int uid = m_data->m_userConstraintUIDGenerator++;
12469 serverCmd.m_userConstraintResultArgs = clientCmd.m_userConstraintArguments;
12470 serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid;
12471 serverCmd.m_userConstraintResultArgs.m_maxAppliedForce = defaultMaxForce;
12472 userConstraintData.m_userConstraintData = serverCmd.m_userConstraintResultArgs;
12473 m_data->m_userConstraints.insert(uid, userConstraintData);
12474 serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED;
12475 }
12476 else
12477 {
12478 btRigidBody* rb = childBody ? childBody->m_rigidBody : 0;
12479
12480 btMultiBodyPoint2Point* p2p = new btMultiBodyPoint2Point(parentBody->m_multiBody, clientCmd.m_userConstraintArguments.m_parentJointIndex, rb, pivotInParent, pivotInChild);
12481 p2p->setMaxAppliedImpulse(defaultMaxForce);
12482 btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*)m_data->m_dynamicsWorld;
12483 world->addMultiBodyConstraint(p2p);
12484 InteralUserConstraintData userConstraintData;
12485 userConstraintData.m_mbConstraint = p2p;
12486 int uid = m_data->m_userConstraintUIDGenerator++;
12487 serverCmd.m_userConstraintResultArgs = clientCmd.m_userConstraintArguments;
12488 serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid;
12489 serverCmd.m_userConstraintResultArgs.m_maxAppliedForce = defaultMaxForce;
12490 userConstraintData.m_userConstraintData = serverCmd.m_userConstraintResultArgs;
12491 m_data->m_userConstraints.insert(uid, userConstraintData);
12492 serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED;
12493 }
12494 }
12495 else
12496 {
12497 b3Warning("unknown constraint type");
12498 }
12499 }
12500 }
12501 }
12502 else
12503 #endif
12504 {
12505 InternalBodyData* childBody = clientCmd.m_userConstraintArguments.m_childBodyIndex >= 0 ? m_data->m_bodyHandles.getHandle(clientCmd.m_userConstraintArguments.m_childBodyIndex) : 0;
12506
12507 if (parentBody && childBody)
12508 {
12509 if (parentBody->m_rigidBody)
12510 {
12511 btRigidBody* parentRb = 0;
12512 if (clientCmd.m_userConstraintArguments.m_parentJointIndex == -1)
12513 {
12514 parentRb = parentBody->m_rigidBody;
12515 }
12516 else
12517 {
12518 if ((clientCmd.m_userConstraintArguments.m_parentJointIndex >= 0) &&
12519 (clientCmd.m_userConstraintArguments.m_parentJointIndex < parentBody->m_rigidBodyJoints.size()))
12520 {
12521 parentRb = &parentBody->m_rigidBodyJoints[clientCmd.m_userConstraintArguments.m_parentJointIndex]->getRigidBodyB();
12522 }
12523 }
12524
12525 btRigidBody* childRb = 0;
12526 if (childBody->m_rigidBody)
12527 {
12528 if (clientCmd.m_userConstraintArguments.m_childJointIndex == -1)
12529 {
12530 childRb = childBody->m_rigidBody;
12531 }
12532 else
12533 {
12534 if ((clientCmd.m_userConstraintArguments.m_childJointIndex >= 0) && (clientCmd.m_userConstraintArguments.m_childJointIndex < childBody->m_rigidBodyJoints.size()))
12535 {
12536 childRb = &childBody->m_rigidBodyJoints[clientCmd.m_userConstraintArguments.m_childJointIndex]->getRigidBodyB();
12537 }
12538 }
12539 }
12540
12541 switch (clientCmd.m_userConstraintArguments.m_jointType)
12542 {
12543 case eRevoluteType:
12544 {
12545 break;
12546 }
12547 case ePrismaticType:
12548 {
12549 break;
12550 }
12551
12552 case eFixedType:
12553 {
12554 if (childRb && parentRb && (childRb != parentRb))
12555 {
12556 btVector3 pivotInParent(clientCmd.m_userConstraintArguments.m_parentFrame[0], clientCmd.m_userConstraintArguments.m_parentFrame[1], clientCmd.m_userConstraintArguments.m_parentFrame[2]);
12557 btVector3 pivotInChild(clientCmd.m_userConstraintArguments.m_childFrame[0], clientCmd.m_userConstraintArguments.m_childFrame[1], clientCmd.m_userConstraintArguments.m_childFrame[2]);
12558
12559 btTransform offsetTrA, offsetTrB;
12560 offsetTrA.setIdentity();
12561 offsetTrA.setOrigin(pivotInParent);
12562 offsetTrB.setIdentity();
12563 offsetTrB.setOrigin(pivotInChild);
12564
12565 btGeneric6DofSpring2Constraint* dof6 = new btGeneric6DofSpring2Constraint(*parentRb, *childRb, offsetTrA, offsetTrB);
12566
12567 dof6->setLinearLowerLimit(btVector3(0, 0, 0));
12568 dof6->setLinearUpperLimit(btVector3(0, 0, 0));
12569
12570 dof6->setAngularLowerLimit(btVector3(0, 0, 0));
12571 dof6->setAngularUpperLimit(btVector3(0, 0, 0));
12572 m_data->m_dynamicsWorld->addConstraint(dof6);
12573 InteralUserConstraintData userConstraintData;
12574 userConstraintData.m_rbConstraint = dof6;
12575 int uid = m_data->m_userConstraintUIDGenerator++;
12576 serverCmd.m_userConstraintResultArgs = clientCmd.m_userConstraintArguments;
12577 serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid;
12578 serverCmd.m_userConstraintResultArgs.m_maxAppliedForce = defaultMaxForce;
12579 userConstraintData.m_userConstraintData = serverCmd.m_userConstraintResultArgs;
12580 m_data->m_userConstraints.insert(uid, userConstraintData);
12581 serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED;
12582 }
12583
12584 break;
12585 }
12586
12587 case ePoint2PointType:
12588 {
12589 if (childRb && parentRb && (childRb != parentRb))
12590 {
12591 btVector3 pivotInParent(clientCmd.m_userConstraintArguments.m_parentFrame[0], clientCmd.m_userConstraintArguments.m_parentFrame[1], clientCmd.m_userConstraintArguments.m_parentFrame[2]);
12592 btVector3 pivotInChild(clientCmd.m_userConstraintArguments.m_childFrame[0], clientCmd.m_userConstraintArguments.m_childFrame[1], clientCmd.m_userConstraintArguments.m_childFrame[2]);
12593
12594 btPoint2PointConstraint* p2p = new btPoint2PointConstraint(*parentRb, *childRb, pivotInParent, pivotInChild);
12595 p2p->m_setting.m_impulseClamp = defaultMaxForce;
12596 m_data->m_dynamicsWorld->addConstraint(p2p);
12597 InteralUserConstraintData userConstraintData;
12598 userConstraintData.m_rbConstraint = p2p;
12599 int uid = m_data->m_userConstraintUIDGenerator++;
12600 serverCmd.m_userConstraintResultArgs = clientCmd.m_userConstraintArguments;
12601 serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid;
12602 serverCmd.m_userConstraintResultArgs.m_maxAppliedForce = defaultMaxForce;
12603 userConstraintData.m_userConstraintData = serverCmd.m_userConstraintResultArgs;
12604 m_data->m_userConstraints.insert(uid, userConstraintData);
12605 serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED;
12606 }
12607 break;
12608 }
12609
12610 case eGearType:
12611 {
12612 if (childRb && parentRb && (childRb != parentRb))
12613 {
12614 btVector3 axisA(clientCmd.m_userConstraintArguments.m_jointAxis[0],
12615 clientCmd.m_userConstraintArguments.m_jointAxis[1],
12616 clientCmd.m_userConstraintArguments.m_jointAxis[2]);
12617 //for now we use the same local axis for both objects
12618 btVector3 axisB(clientCmd.m_userConstraintArguments.m_jointAxis[0],
12619 clientCmd.m_userConstraintArguments.m_jointAxis[1],
12620 clientCmd.m_userConstraintArguments.m_jointAxis[2]);
12621 btScalar ratio = 1;
12622 btGearConstraint* gear = new btGearConstraint(*parentRb, *childRb, axisA, axisB, ratio);
12623 m_data->m_dynamicsWorld->addConstraint(gear, true);
12624 InteralUserConstraintData userConstraintData;
12625 userConstraintData.m_rbConstraint = gear;
12626 int uid = m_data->m_userConstraintUIDGenerator++;
12627 serverCmd.m_userConstraintResultArgs = clientCmd.m_userConstraintArguments;
12628 serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid;
12629 serverCmd.m_userConstraintResultArgs.m_maxAppliedForce = defaultMaxForce;
12630 userConstraintData.m_userConstraintData = serverCmd.m_userConstraintResultArgs;
12631 m_data->m_userConstraints.insert(uid, userConstraintData);
12632 serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED;
12633 }
12634 break;
12635 }
12636 case eSphericalType:
12637 {
12638 b3Warning("constraint type not handled yet");
12639 break;
12640 }
12641 case ePlanarType:
12642 {
12643 b3Warning("constraint type not handled yet");
12644 break;
12645 }
12646 default:
12647 {
12648 b3Warning("unknown constraint type");
12649 }
12650 };
12651 }
12652 }
12653 }
12654 }
12655
12656 if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_CONSTRAINT)
12657 {
12658 btScalar fixedTimeSubStep = m_data->m_numSimulationSubSteps > 0 ? m_data->m_physicsDeltaTime / m_data->m_numSimulationSubSteps : m_data->m_physicsDeltaTime;
12659
12660 serverCmd.m_type = CMD_CHANGE_USER_CONSTRAINT_FAILED;
12661 int userConstraintUidChange = clientCmd.m_userConstraintArguments.m_userConstraintUniqueId;
12662 InteralUserConstraintData* userConstraintPtr = m_data->m_userConstraints.find(userConstraintUidChange);
12663 if (userConstraintPtr)
12664 {
12665 if (userConstraintPtr->m_mbConstraint)
12666 {
12667 if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_PIVOT_IN_B)
12668 {
12669 btVector3 pivotInB(clientCmd.m_userConstraintArguments.m_childFrame[0],
12670 clientCmd.m_userConstraintArguments.m_childFrame[1],
12671 clientCmd.m_userConstraintArguments.m_childFrame[2]);
12672 userConstraintPtr->m_userConstraintData.m_childFrame[0] = clientCmd.m_userConstraintArguments.m_childFrame[0];
12673 userConstraintPtr->m_userConstraintData.m_childFrame[1] = clientCmd.m_userConstraintArguments.m_childFrame[1];
12674 userConstraintPtr->m_userConstraintData.m_childFrame[2] = clientCmd.m_userConstraintArguments.m_childFrame[2];
12675 userConstraintPtr->m_mbConstraint->setPivotInB(pivotInB);
12676 }
12677 if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_FRAME_ORN_IN_B)
12678 {
12679 btQuaternion childFrameOrn(clientCmd.m_userConstraintArguments.m_childFrame[3],
12680 clientCmd.m_userConstraintArguments.m_childFrame[4],
12681 clientCmd.m_userConstraintArguments.m_childFrame[5],
12682 clientCmd.m_userConstraintArguments.m_childFrame[6]);
12683 userConstraintPtr->m_userConstraintData.m_childFrame[3] = clientCmd.m_userConstraintArguments.m_childFrame[3];
12684 userConstraintPtr->m_userConstraintData.m_childFrame[4] = clientCmd.m_userConstraintArguments.m_childFrame[4];
12685 userConstraintPtr->m_userConstraintData.m_childFrame[5] = clientCmd.m_userConstraintArguments.m_childFrame[5];
12686 userConstraintPtr->m_userConstraintData.m_childFrame[6] = clientCmd.m_userConstraintArguments.m_childFrame[6];
12687 btMatrix3x3 childFrameBasis(childFrameOrn);
12688 userConstraintPtr->m_mbConstraint->setFrameInB(childFrameBasis);
12689 }
12690 if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_MAX_FORCE)
12691 {
12692 btScalar maxImp = clientCmd.m_userConstraintArguments.m_maxAppliedForce * fixedTimeSubStep;
12693
12694 userConstraintPtr->m_userConstraintData.m_maxAppliedForce = clientCmd.m_userConstraintArguments.m_maxAppliedForce;
12695 userConstraintPtr->m_mbConstraint->setMaxAppliedImpulse(maxImp);
12696 }
12697
12698 if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_GEAR_RATIO)
12699 {
12700 userConstraintPtr->m_mbConstraint->setGearRatio(clientCmd.m_userConstraintArguments.m_gearRatio);
12701 userConstraintPtr->m_userConstraintData.m_gearRatio = clientCmd.m_userConstraintArguments.m_gearRatio;
12702 }
12703 if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_RELATIVE_POSITION_TARGET)
12704 {
12705 userConstraintPtr->m_mbConstraint->setRelativePositionTarget(clientCmd.m_userConstraintArguments.m_relativePositionTarget);
12706 userConstraintPtr->m_userConstraintData.m_relativePositionTarget = clientCmd.m_userConstraintArguments.m_relativePositionTarget;
12707 }
12708 if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_ERP)
12709 {
12710 userConstraintPtr->m_mbConstraint->setErp(clientCmd.m_userConstraintArguments.m_erp);
12711 userConstraintPtr->m_userConstraintData.m_erp = clientCmd.m_userConstraintArguments.m_erp;
12712 }
12713
12714 if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_GEAR_AUX_LINK)
12715 {
12716 userConstraintPtr->m_mbConstraint->setGearAuxLink(clientCmd.m_userConstraintArguments.m_gearAuxLink);
12717 userConstraintPtr->m_userConstraintData.m_gearAuxLink = clientCmd.m_userConstraintArguments.m_gearAuxLink;
12718 }
12719 }
12720 if (userConstraintPtr->m_rbConstraint)
12721 {
12722 if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_MAX_FORCE)
12723 {
12724 btScalar maxImp = clientCmd.m_userConstraintArguments.m_maxAppliedForce * fixedTimeSubStep;
12725 userConstraintPtr->m_userConstraintData.m_maxAppliedForce = clientCmd.m_userConstraintArguments.m_maxAppliedForce;
12726 //userConstraintPtr->m_rbConstraint->setMaxAppliedImpulse(maxImp);
12727 }
12728
12729 if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_GEAR_RATIO)
12730 {
12731 if (userConstraintPtr->m_rbConstraint->getObjectType() == GEAR_CONSTRAINT_TYPE)
12732 {
12733 btGearConstraint* gear = (btGearConstraint*)userConstraintPtr->m_rbConstraint;
12734 gear->setRatio(clientCmd.m_userConstraintArguments.m_gearRatio);
12735 }
12736 }
12737 }
12738 serverCmd.m_userConstraintResultArgs = clientCmd.m_userConstraintArguments;
12739 serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = userConstraintUidChange;
12740 serverCmd.m_updateFlags = clientCmd.m_updateFlags;
12741 serverCmd.m_type = CMD_CHANGE_USER_CONSTRAINT_COMPLETED;
12742 }
12743 }
12744 if (clientCmd.m_updateFlags & USER_CONSTRAINT_REMOVE_CONSTRAINT)
12745 {
12746 serverCmd.m_type = CMD_REMOVE_USER_CONSTRAINT_FAILED;
12747 int userConstraintUidRemove = clientCmd.m_userConstraintArguments.m_userConstraintUniqueId;
12748 InteralUserConstraintData* userConstraintPtr = m_data->m_userConstraints.find(userConstraintUidRemove);
12749 if (userConstraintPtr)
12750 {
12751 #ifndef USE_DISCRETE_DYNAMICS_WORLD
12752 if (userConstraintPtr->m_mbConstraint)
12753 {
12754 m_data->m_dynamicsWorld->removeMultiBodyConstraint(userConstraintPtr->m_mbConstraint);
12755 delete userConstraintPtr->m_mbConstraint;
12756 m_data->m_userConstraints.remove(userConstraintUidRemove);
12757 }
12758 #endif//USE_DISCRETE_DYNAMICS_WORLD
12759 if (userConstraintPtr->m_rbConstraint)
12760 {
12761 m_data->m_dynamicsWorld->removeConstraint(userConstraintPtr->m_rbConstraint);
12762 delete userConstraintPtr->m_rbConstraint;
12763 m_data->m_userConstraints.remove(userConstraintUidRemove);
12764 }
12765 #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
12766
12767 if (userConstraintPtr->m_sbHandle >= 0)
12768 {
12769 InternalBodyHandle* sbodyHandle = m_data->m_bodyHandles.getHandle(userConstraintPtr->m_sbHandle);
12770 if (sbodyHandle)
12771 {
12772 if (sbodyHandle->m_softBody)
12773 {
12774 if (userConstraintPtr->m_sbNodeMass >= 0)
12775 {
12776 sbodyHandle->m_softBody->setMass(userConstraintPtr->m_sbNodeIndex, userConstraintPtr->m_sbNodeMass);
12777 }
12778 else
12779 {
12780 sbodyHandle->m_softBody->removeAnchor(userConstraintPtr->m_sbNodeIndex);
12781 }
12782 }
12783 }
12784 }
12785 #endif
12786 serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = userConstraintUidRemove;
12787 serverCmd.m_type = CMD_REMOVE_USER_CONSTRAINT_COMPLETED;
12788 }
12789 }
12790 return hasStatus;
12791 }
12792
processCalculateInverseKinematicsCommand(const struct SharedMemoryCommand & clientCmd,struct SharedMemoryStatus & serverStatusOut,char * bufferServerToClient,int bufferSizeInBytes)12793 bool PhysicsServerCommandProcessor::processCalculateInverseKinematicsCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
12794 {
12795 bool hasStatus = true;
12796
12797 BT_PROFILE("CMD_CALCULATE_INVERSE_KINEMATICS");
12798 SharedMemoryStatus& serverCmd = serverStatusOut;
12799 serverCmd.m_type = CMD_CALCULATE_INVERSE_KINEMATICS_FAILED;
12800
12801 InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(clientCmd.m_calculateInverseKinematicsArguments.m_bodyUniqueId);
12802 if (bodyHandle && bodyHandle->m_multiBody)
12803 {
12804 IKTrajectoryHelper** ikHelperPtrPtr = m_data->m_inverseKinematicsHelpers.find(bodyHandle->m_multiBody);
12805 IKTrajectoryHelper* ikHelperPtr = 0;
12806
12807 if (ikHelperPtrPtr)
12808 {
12809 ikHelperPtr = *ikHelperPtrPtr;
12810 }
12811 else
12812 {
12813 IKTrajectoryHelper* tmpHelper = new IKTrajectoryHelper;
12814 m_data->m_inverseKinematicsHelpers.insert(bodyHandle->m_multiBody, tmpHelper);
12815 ikHelperPtr = tmpHelper;
12816 }
12817
12818 int endEffectorLinkIndex = clientCmd.m_calculateInverseKinematicsArguments.m_endEffectorLinkIndices[0];
12819
12820 btAlignedObjectArray<double> startingPositions;
12821 startingPositions.reserve(bodyHandle->m_multiBody->getNumLinks());
12822
12823 btVector3 targetPosWorld(clientCmd.m_calculateInverseKinematicsArguments.m_targetPositions[0],
12824 clientCmd.m_calculateInverseKinematicsArguments.m_targetPositions[1],
12825 clientCmd.m_calculateInverseKinematicsArguments.m_targetPositions[2]);
12826
12827 btQuaternion targetOrnWorld(clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation[0],
12828 clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation[1],
12829 clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation[2],
12830 clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation[3]);
12831
12832 btTransform targetBaseCoord;
12833 if (clientCmd.m_updateFlags & IK_HAS_CURRENT_JOINT_POSITIONS)
12834 {
12835 targetBaseCoord.setOrigin(targetPosWorld);
12836 targetBaseCoord.setRotation(targetOrnWorld);
12837 }
12838 else
12839 {
12840 btTransform targetWorld;
12841 targetWorld.setOrigin(targetPosWorld);
12842 targetWorld.setRotation(targetOrnWorld);
12843 btTransform tr = bodyHandle->m_multiBody->getBaseWorldTransform();
12844 targetBaseCoord = tr.inverse() * targetWorld;
12845 }
12846
12847 {
12848 int DofIndex = 0;
12849 for (int i = 0; i < bodyHandle->m_multiBody->getNumLinks(); ++i)
12850 {
12851 if (bodyHandle->m_multiBody->getLink(i).m_jointType >= 0 && bodyHandle->m_multiBody->getLink(i).m_jointType <= 2)
12852 {
12853 // 0, 1, 2 represent revolute, prismatic, and spherical joint types respectively. Skip the fixed joints.
12854 double curPos = 0;
12855 if (clientCmd.m_updateFlags & IK_HAS_CURRENT_JOINT_POSITIONS)
12856 {
12857 curPos = clientCmd.m_calculateInverseKinematicsArguments.m_currentPositions[DofIndex];
12858 }
12859 else
12860 {
12861 curPos = bodyHandle->m_multiBody->getJointPos(i);
12862 }
12863 startingPositions.push_back(curPos);
12864 DofIndex++;
12865 }
12866 }
12867 }
12868
12869 int numIterations = 20;
12870 if (clientCmd.m_updateFlags & IK_HAS_MAX_ITERATIONS)
12871 {
12872 numIterations = clientCmd.m_calculateInverseKinematicsArguments.m_maxNumIterations;
12873 }
12874 double residualThreshold = 1e-4;
12875 if (clientCmd.m_updateFlags & IK_HAS_RESIDUAL_THRESHOLD)
12876 {
12877 residualThreshold = clientCmd.m_calculateInverseKinematicsArguments.m_residualThreshold;
12878 }
12879
12880 btScalar currentDiff = 1e30f;
12881 b3AlignedObjectArray<double> jacobian_linear;
12882 b3AlignedObjectArray<double> jacobian_angular;
12883 btAlignedObjectArray<double> q_current;
12884 btAlignedObjectArray<double> q_new;
12885 btAlignedObjectArray<double> lower_limit;
12886 btAlignedObjectArray<double> upper_limit;
12887 btAlignedObjectArray<double> joint_range;
12888 btAlignedObjectArray<double> rest_pose;
12889 const int numDofs = bodyHandle->m_multiBody->getNumDofs();
12890 int baseDofs = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 6;
12891 btInverseDynamics::vecx nu(numDofs + baseDofs), qdot(numDofs + baseDofs), q(numDofs + baseDofs), joint_force(numDofs + baseDofs);
12892
12893 for (int i = 0; i < numIterations && currentDiff > residualThreshold; i++)
12894 {
12895 BT_PROFILE("InverseKinematics1Step");
12896 if (ikHelperPtr && (endEffectorLinkIndex < bodyHandle->m_multiBody->getNumLinks()))
12897 {
12898 jacobian_linear.resize(3 * numDofs);
12899 jacobian_angular.resize(3 * numDofs);
12900 int jacSize = 0;
12901
12902 btInverseDynamics::MultiBodyTree* tree = m_data->findOrCreateTree(bodyHandle->m_multiBody);
12903
12904 q_current.resize(numDofs);
12905
12906 if (tree && ((numDofs + baseDofs) == tree->numDoFs()))
12907 {
12908 btInverseDynamics::vec3 world_origin;
12909 btInverseDynamics::mat33 world_rot;
12910
12911 jacSize = jacobian_linear.size();
12912 // Set jacobian value
12913
12914 int DofIndex = 0;
12915 for (int i = 0; i < bodyHandle->m_multiBody->getNumLinks(); ++i)
12916 {
12917 if (bodyHandle->m_multiBody->getLink(i).m_jointType >= 0 && bodyHandle->m_multiBody->getLink(i).m_jointType <= 2)
12918 {
12919 // 0, 1, 2 represent revolute, prismatic, and spherical joint types respectively. Skip the fixed joints.
12920
12921 double curPos = startingPositions[DofIndex];
12922 q_current[DofIndex] = curPos;
12923 q[DofIndex + baseDofs] = curPos;
12924 qdot[DofIndex + baseDofs] = 0;
12925 nu[DofIndex + baseDofs] = 0;
12926 DofIndex++;
12927 }
12928 } // Set the gravity to correspond to the world gravity
12929 btInverseDynamics::vec3 id_grav(m_data->m_dynamicsWorld->getGravity());
12930
12931 {
12932 BT_PROFILE("calculateInverseDynamics");
12933 if (-1 != tree->setGravityInWorldFrame(id_grav) &&
12934 -1 != tree->calculateInverseDynamics(q, qdot, nu, &joint_force))
12935 {
12936 tree->calculateJacobians(q);
12937 btInverseDynamics::mat3x jac_t(3, numDofs + baseDofs);
12938 btInverseDynamics::mat3x jac_r(3, numDofs + baseDofs);
12939 // Note that inverse dynamics uses zero-based indexing of bodies, not starting from -1 for the base link.
12940 tree->getBodyJacobianTrans(endEffectorLinkIndex + 1, &jac_t);
12941 tree->getBodyJacobianRot(endEffectorLinkIndex + 1, &jac_r);
12942
12943 //calculatePositionKinematics is already done inside calculateInverseDynamics
12944 tree->getBodyOrigin(endEffectorLinkIndex + 1, &world_origin);
12945 tree->getBodyTransform(endEffectorLinkIndex + 1, &world_rot);
12946
12947 for (int i = 0; i < 3; ++i)
12948 {
12949 for (int j = 0; j < numDofs; ++j)
12950 {
12951 jacobian_linear[i * numDofs + j] = jac_t(i, (baseDofs + j));
12952 jacobian_angular[i * numDofs + j] = jac_r(i, (baseDofs + j));
12953 }
12954 }
12955 }
12956 }
12957
12958 q_new.resize(numDofs);
12959 int ikMethod = 0;
12960 if ((clientCmd.m_updateFlags & IK_HAS_TARGET_ORIENTATION) && (clientCmd.m_updateFlags & IK_HAS_NULL_SPACE_VELOCITY))
12961 {
12962 //Nullspace task only works with DLS now. TODO: add nullspace task to SDLS.
12963 ikMethod = IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE;
12964 }
12965 else if (clientCmd.m_updateFlags & IK_HAS_TARGET_ORIENTATION)
12966 {
12967 if (clientCmd.m_updateFlags & IK_SDLS)
12968 {
12969 ikMethod = IK2_VEL_SDLS_WITH_ORIENTATION;
12970 }
12971 else
12972 {
12973 ikMethod = IK2_VEL_DLS_WITH_ORIENTATION;
12974 }
12975 }
12976 else if (clientCmd.m_updateFlags & IK_HAS_NULL_SPACE_VELOCITY)
12977 {
12978 //Nullspace task only works with DLS now. TODO: add nullspace task to SDLS.
12979 ikMethod = IK2_VEL_DLS_WITH_NULLSPACE;
12980 }
12981 else
12982 {
12983 if (clientCmd.m_updateFlags & IK_SDLS)
12984 {
12985 ikMethod = IK2_VEL_SDLS;
12986 }
12987 else
12988 {
12989 ikMethod = IK2_VEL_DLS;
12990 ;
12991 }
12992 }
12993
12994 if (clientCmd.m_updateFlags & IK_HAS_NULL_SPACE_VELOCITY)
12995 {
12996 lower_limit.resize(numDofs);
12997 upper_limit.resize(numDofs);
12998 joint_range.resize(numDofs);
12999 rest_pose.resize(numDofs);
13000 for (int i = 0; i < numDofs; ++i)
13001 {
13002 lower_limit[i] = clientCmd.m_calculateInverseKinematicsArguments.m_lowerLimit[i];
13003 upper_limit[i] = clientCmd.m_calculateInverseKinematicsArguments.m_upperLimit[i];
13004 joint_range[i] = clientCmd.m_calculateInverseKinematicsArguments.m_jointRange[i];
13005 rest_pose[i] = clientCmd.m_calculateInverseKinematicsArguments.m_restPose[i];
13006 }
13007 {
13008 BT_PROFILE("computeNullspaceVel");
13009 ikHelperPtr->computeNullspaceVel(numDofs, &q_current[0], &lower_limit[0], &upper_limit[0], &joint_range[0], &rest_pose[0]);
13010 }
13011 }
13012
13013 //btTransform endEffectorTransformWorld = bodyHandle->m_multiBody->getLink(endEffectorLinkIndex).m_cachedWorldTransform * bodyHandle->m_linkLocalInertialFrames[endEffectorLinkIndex].inverse();
13014
13015 btVector3DoubleData endEffectorWorldPosition;
13016 btQuaternionDoubleData endEffectorWorldOrientation;
13017
13018 //get the position from the inverse dynamics (based on q) instead of endEffectorTransformWorld
13019 btVector3 endEffectorPosWorldOrg = world_origin;
13020 btQuaternion endEffectorOriWorldOrg;
13021 world_rot.getRotation(endEffectorOriWorldOrg);
13022
13023 btTransform endEffectorBaseCoord;
13024 endEffectorBaseCoord.setOrigin(endEffectorPosWorldOrg);
13025 endEffectorBaseCoord.setRotation(endEffectorOriWorldOrg);
13026
13027 //don't need the next two lines
13028 //btTransform linkInertiaInv = bodyHandle->m_linkLocalInertialFrames[endEffectorLinkIndex].inverse();
13029 //endEffectorBaseCoord = endEffectorBaseCoord * linkInertiaInv;
13030
13031 //btTransform tr = bodyHandle->m_multiBody->getBaseWorldTransform();
13032 //endEffectorBaseCoord = tr.inverse()*endEffectorTransformWorld;
13033 //endEffectorBaseCoord = tr.inverse()*endEffectorTransformWorld;
13034
13035 btQuaternion endEffectorOriBaseCoord = endEffectorBaseCoord.getRotation();
13036
13037 //btVector4 endEffectorOri(endEffectorOriBaseCoord.x(), endEffectorOriBaseCoord.y(), endEffectorOriBaseCoord.z(), endEffectorOriBaseCoord.w());
13038
13039 endEffectorBaseCoord.getOrigin().serializeDouble(endEffectorWorldPosition);
13040 endEffectorBaseCoord.getRotation().serializeDouble(endEffectorWorldOrientation);
13041
13042 //diff
13043 currentDiff = (endEffectorBaseCoord.getOrigin() - targetBaseCoord.getOrigin()).length();
13044
13045 btVector3DoubleData targetPosBaseCoord;
13046 btQuaternionDoubleData targetOrnBaseCoord;
13047 targetBaseCoord.getOrigin().serializeDouble(targetPosBaseCoord);
13048 targetBaseCoord.getRotation().serializeDouble(targetOrnBaseCoord);
13049
13050 // Set joint damping coefficents. A small default
13051 // damping constant is added to prevent singularity
13052 // with pseudo inverse. The user can set joint damping
13053 // coefficients differently for each joint. The larger
13054 // the damping coefficient is, the less we rely on
13055 // this joint to achieve the IK target.
13056 btAlignedObjectArray<double> joint_damping;
13057 joint_damping.resize(numDofs, 0.5);
13058 if (clientCmd.m_updateFlags & IK_HAS_JOINT_DAMPING)
13059 {
13060 for (int i = 0; i < numDofs; ++i)
13061 {
13062 joint_damping[i] = clientCmd.m_calculateInverseKinematicsArguments.m_jointDamping[i];
13063 }
13064 }
13065 ikHelperPtr->setDampingCoeff(numDofs, &joint_damping[0]);
13066
13067 double targetDampCoeff[6] = {1.0, 1.0, 1.0, 1.0, 1.0, 1.0};
13068
13069 {
13070 BT_PROFILE("computeIK");
13071 ikHelperPtr->computeIK(targetPosBaseCoord.m_floats, targetOrnBaseCoord.m_floats,
13072 endEffectorWorldPosition.m_floats, endEffectorWorldOrientation.m_floats,
13073 &q_current[0],
13074 numDofs, clientCmd.m_calculateInverseKinematicsArguments.m_endEffectorLinkIndices[0],
13075 &q_new[0], ikMethod, &jacobian_linear[0], &jacobian_angular[0], jacSize * 2, targetDampCoeff);
13076 }
13077 serverCmd.m_inverseKinematicsResultArgs.m_bodyUniqueId = clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId;
13078 for (int i = 0; i < numDofs; i++)
13079 {
13080 serverCmd.m_inverseKinematicsResultArgs.m_jointPositions[i] = q_new[i];
13081 }
13082 serverCmd.m_inverseKinematicsResultArgs.m_dofCount = numDofs;
13083 serverCmd.m_type = CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED;
13084 for (int i = 0; i < numDofs; i++)
13085 {
13086 startingPositions[i] = q_new[i];
13087 }
13088 }
13089 }
13090 }
13091 }
13092 return hasStatus;
13093 }
13094
processCalculateInverseKinematicsCommand2(const struct SharedMemoryCommand & clientCmd,struct SharedMemoryStatus & serverStatusOut,char * bufferServerToClient,int bufferSizeInBytes)13095 bool PhysicsServerCommandProcessor::processCalculateInverseKinematicsCommand2(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
13096 {
13097 bool hasStatus = true;
13098
13099 BT_PROFILE("CMD_CALCULATE_INVERSE_KINEMATICS");
13100 SharedMemoryStatus& serverCmd = serverStatusOut;
13101 serverCmd.m_type = CMD_CALCULATE_INVERSE_KINEMATICS_FAILED;
13102
13103 InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(clientCmd.m_calculateInverseKinematicsArguments.m_bodyUniqueId);
13104 if (bodyHandle && bodyHandle->m_multiBody)
13105 {
13106 IKTrajectoryHelper** ikHelperPtrPtr = m_data->m_inverseKinematicsHelpers.find(bodyHandle->m_multiBody);
13107 IKTrajectoryHelper* ikHelperPtr = 0;
13108
13109 if (ikHelperPtrPtr)
13110 {
13111 ikHelperPtr = *ikHelperPtrPtr;
13112 }
13113 else
13114 {
13115 IKTrajectoryHelper* tmpHelper = new IKTrajectoryHelper;
13116 m_data->m_inverseKinematicsHelpers.insert(bodyHandle->m_multiBody, tmpHelper);
13117 ikHelperPtr = tmpHelper;
13118 }
13119
13120 btAlignedObjectArray<double> startingPositions;
13121 startingPositions.reserve(bodyHandle->m_multiBody->getNumLinks());
13122
13123 {
13124 int DofIndex = 0;
13125 for (int i = 0; i < bodyHandle->m_multiBody->getNumLinks(); ++i)
13126 {
13127 if (bodyHandle->m_multiBody->getLink(i).m_jointType >= 0 && bodyHandle->m_multiBody->getLink(i).m_jointType <= 2)
13128 {
13129 // 0, 1, 2 represent revolute, prismatic, and spherical joint types respectively. Skip the fixed joints.
13130 double curPos = 0;
13131 if (clientCmd.m_updateFlags & IK_HAS_CURRENT_JOINT_POSITIONS)
13132 {
13133 curPos = clientCmd.m_calculateInverseKinematicsArguments.m_currentPositions[DofIndex];
13134 }
13135 else
13136 {
13137 curPos = bodyHandle->m_multiBody->getJointPos(i);
13138 }
13139 startingPositions.push_back(curPos);
13140 DofIndex++;
13141 }
13142 }
13143 }
13144
13145 int numIterations = 20;
13146 if (clientCmd.m_updateFlags & IK_HAS_MAX_ITERATIONS)
13147 {
13148 numIterations = clientCmd.m_calculateInverseKinematicsArguments.m_maxNumIterations;
13149 }
13150 double residualThreshold = 1e-4;
13151 if (clientCmd.m_updateFlags & IK_HAS_RESIDUAL_THRESHOLD)
13152 {
13153 residualThreshold = clientCmd.m_calculateInverseKinematicsArguments.m_residualThreshold;
13154 }
13155
13156 btScalar currentDiff = 1e30f;
13157 b3AlignedObjectArray<double> endEffectorTargetWorldPositions;
13158 b3AlignedObjectArray<double> endEffectorTargetWorldOrientations;
13159 b3AlignedObjectArray<double> endEffectorCurrentWorldPositions;
13160 b3AlignedObjectArray<double> jacobian_linear;
13161 b3AlignedObjectArray<double> jacobian_angular;
13162 btAlignedObjectArray<double> q_current;
13163 btAlignedObjectArray<double> q_new;
13164 btAlignedObjectArray<double> lower_limit;
13165 btAlignedObjectArray<double> upper_limit;
13166 btAlignedObjectArray<double> joint_range;
13167 btAlignedObjectArray<double> rest_pose;
13168 const int numDofs = bodyHandle->m_multiBody->getNumDofs();
13169 int baseDofs = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 6;
13170 btInverseDynamics::vecx nu(numDofs + baseDofs), qdot(numDofs + baseDofs), q(numDofs + baseDofs), joint_force(numDofs + baseDofs);
13171
13172 endEffectorTargetWorldPositions.resize(0);
13173 endEffectorTargetWorldPositions.reserve(clientCmd.m_calculateInverseKinematicsArguments.m_numEndEffectorLinkIndices * 3);
13174 endEffectorTargetWorldOrientations.resize(0);
13175 endEffectorTargetWorldOrientations.reserve(clientCmd.m_calculateInverseKinematicsArguments.m_numEndEffectorLinkIndices * 4);
13176
13177 bool validEndEffectorLinkIndices = true;
13178
13179 for (int ne = 0; ne < clientCmd.m_calculateInverseKinematicsArguments.m_numEndEffectorLinkIndices; ne++)
13180 {
13181 int endEffectorLinkIndex = clientCmd.m_calculateInverseKinematicsArguments.m_endEffectorLinkIndices[ne];
13182 validEndEffectorLinkIndices = validEndEffectorLinkIndices && (endEffectorLinkIndex < bodyHandle->m_multiBody->getNumLinks());
13183
13184 btVector3 targetPosWorld(clientCmd.m_calculateInverseKinematicsArguments.m_targetPositions[ne * 3 + 0],
13185 clientCmd.m_calculateInverseKinematicsArguments.m_targetPositions[ne * 3 + 1],
13186 clientCmd.m_calculateInverseKinematicsArguments.m_targetPositions[ne * 3 + 2]);
13187
13188 btQuaternion targetOrnWorld(clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation[ne * 4 + 0],
13189 clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation[ne * 4 + 1],
13190 clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation[ne * 4 + 2],
13191 clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation[ne * 4 + 3]);
13192
13193 btTransform targetBaseCoord;
13194 if (clientCmd.m_updateFlags & IK_HAS_CURRENT_JOINT_POSITIONS)
13195 {
13196 targetBaseCoord.setOrigin(targetPosWorld);
13197 targetBaseCoord.setRotation(targetOrnWorld);
13198 }
13199 else
13200 {
13201 btTransform targetWorld;
13202 targetWorld.setOrigin(targetPosWorld);
13203 targetWorld.setRotation(targetOrnWorld);
13204 btTransform tr = bodyHandle->m_multiBody->getBaseWorldTransform();
13205 targetBaseCoord = tr.inverse() * targetWorld;
13206 }
13207
13208 btVector3DoubleData targetPosBaseCoord;
13209 btQuaternionDoubleData targetOrnBaseCoord;
13210 targetBaseCoord.getOrigin().serializeDouble(targetPosBaseCoord);
13211 targetBaseCoord.getRotation().serializeDouble(targetOrnBaseCoord);
13212
13213 endEffectorTargetWorldPositions.push_back(targetPosBaseCoord.m_floats[0]);
13214 endEffectorTargetWorldPositions.push_back(targetPosBaseCoord.m_floats[1]);
13215 endEffectorTargetWorldPositions.push_back(targetPosBaseCoord.m_floats[2]);
13216
13217 endEffectorTargetWorldOrientations.push_back(targetOrnBaseCoord.m_floats[0]);
13218 endEffectorTargetWorldOrientations.push_back(targetOrnBaseCoord.m_floats[1]);
13219 endEffectorTargetWorldOrientations.push_back(targetOrnBaseCoord.m_floats[2]);
13220 endEffectorTargetWorldOrientations.push_back(targetOrnBaseCoord.m_floats[3]);
13221 }
13222 for (int i = 0; i < numIterations && currentDiff > residualThreshold; i++)
13223 {
13224 BT_PROFILE("InverseKinematics1Step");
13225 if (ikHelperPtr && validEndEffectorLinkIndices)
13226 {
13227 jacobian_linear.resize(clientCmd.m_calculateInverseKinematicsArguments.m_numEndEffectorLinkIndices * 3 * numDofs);
13228 jacobian_angular.resize(clientCmd.m_calculateInverseKinematicsArguments.m_numEndEffectorLinkIndices * 3 * numDofs);
13229 int jacSize = 0;
13230
13231 btInverseDynamics::MultiBodyTree* tree = m_data->findOrCreateTree(bodyHandle->m_multiBody);
13232
13233 q_current.resize(numDofs);
13234
13235 if (tree && ((numDofs + baseDofs) == tree->numDoFs()))
13236 {
13237 btInverseDynamics::vec3 world_origin;
13238 btInverseDynamics::mat33 world_rot;
13239
13240 jacSize = jacobian_linear.size();
13241 // Set jacobian value
13242
13243 int DofIndex = 0;
13244 for (int i = 0; i < bodyHandle->m_multiBody->getNumLinks(); ++i)
13245 {
13246 if (bodyHandle->m_multiBody->getLink(i).m_jointType >= 0 && bodyHandle->m_multiBody->getLink(i).m_jointType <= 2)
13247 {
13248 // 0, 1, 2 represent revolute, prismatic, and spherical joint types respectively. Skip the fixed joints.
13249 double curPos = startingPositions[DofIndex];
13250 q_current[DofIndex] = curPos;
13251 q[DofIndex + baseDofs] = curPos;
13252 qdot[DofIndex + baseDofs] = 0;
13253 nu[DofIndex + baseDofs] = 0;
13254 DofIndex++;
13255 }
13256 } // Set the gravity to correspond to the world gravity
13257 btInverseDynamics::vec3 id_grav(m_data->m_dynamicsWorld->getGravity());
13258
13259 {
13260 BT_PROFILE("calculateInverseDynamics");
13261 if (-1 != tree->setGravityInWorldFrame(id_grav) &&
13262 -1 != tree->calculateInverseDynamics(q, qdot, nu, &joint_force))
13263 {
13264 tree->calculateJacobians(q);
13265 btInverseDynamics::mat3x jac_t(3, numDofs + baseDofs);
13266 btInverseDynamics::mat3x jac_r(3, numDofs + baseDofs);
13267 currentDiff = 0;
13268
13269 endEffectorCurrentWorldPositions.resize(0);
13270 endEffectorCurrentWorldPositions.reserve(clientCmd.m_calculateInverseKinematicsArguments.m_numEndEffectorLinkIndices * 3);
13271
13272 for (int ne = 0; ne < clientCmd.m_calculateInverseKinematicsArguments.m_numEndEffectorLinkIndices; ne++)
13273 {
13274 int endEffectorLinkIndex2 = clientCmd.m_calculateInverseKinematicsArguments.m_endEffectorLinkIndices[ne];
13275
13276 // Note that inverse dynamics uses zero-based indexing of bodies, not starting from -1 for the base link.
13277 tree->getBodyJacobianTrans(endEffectorLinkIndex2 + 1, &jac_t);
13278 tree->getBodyJacobianRot(endEffectorLinkIndex2 + 1, &jac_r);
13279
13280 //calculatePositionKinematics is already done inside calculateInverseDynamics
13281
13282 tree->getBodyOrigin(endEffectorLinkIndex2 + 1, &world_origin);
13283 tree->getBodyTransform(endEffectorLinkIndex2 + 1, &world_rot);
13284
13285 for (int i = 0; i < 3; ++i)
13286 {
13287 for (int j = 0; j < numDofs; ++j)
13288 {
13289 jacobian_linear[(ne * 3 + i) * numDofs + j] = jac_t(i, (baseDofs + j));
13290 jacobian_angular[(ne * 3 + i) * numDofs + j] = jac_r(i, (baseDofs + j));
13291 }
13292 }
13293
13294 endEffectorCurrentWorldPositions.push_back(world_origin[0]);
13295 endEffectorCurrentWorldPositions.push_back(world_origin[1]);
13296 endEffectorCurrentWorldPositions.push_back(world_origin[2]);
13297
13298 btInverseDynamics::vec3 targetPos(btVector3(endEffectorTargetWorldPositions[ne * 3 + 0],
13299 endEffectorTargetWorldPositions[ne * 3 + 1],
13300 endEffectorTargetWorldPositions[ne * 3 + 2]));
13301 //diff
13302 currentDiff = btMax(currentDiff, (world_origin - targetPos).length());
13303 }
13304 }
13305 }
13306
13307 q_new.resize(numDofs);
13308 int ikMethod = 0;
13309 if ((clientCmd.m_updateFlags & IK_HAS_TARGET_ORIENTATION) && (clientCmd.m_updateFlags & IK_HAS_NULL_SPACE_VELOCITY))
13310 {
13311 //Nullspace task only works with DLS now. TODO: add nullspace task to SDLS.
13312 ikMethod = IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE;
13313 }
13314 else if (clientCmd.m_updateFlags & IK_HAS_TARGET_ORIENTATION)
13315 {
13316 if (clientCmd.m_updateFlags & IK_SDLS)
13317 {
13318 ikMethod = IK2_VEL_SDLS_WITH_ORIENTATION;
13319 }
13320 else
13321 {
13322 ikMethod = IK2_VEL_DLS_WITH_ORIENTATION;
13323 }
13324 }
13325 else if (clientCmd.m_updateFlags & IK_HAS_NULL_SPACE_VELOCITY)
13326 {
13327 //Nullspace task only works with DLS now. TODO: add nullspace task to SDLS.
13328 ikMethod = IK2_VEL_DLS_WITH_NULLSPACE;
13329 }
13330 else
13331 {
13332 if (clientCmd.m_updateFlags & IK_SDLS)
13333 {
13334 ikMethod = IK2_VEL_SDLS;
13335 }
13336 else
13337 {
13338 ikMethod = IK2_VEL_DLS;
13339 ;
13340 }
13341 }
13342
13343 if (clientCmd.m_updateFlags & IK_HAS_NULL_SPACE_VELOCITY)
13344 {
13345 lower_limit.resize(numDofs);
13346 upper_limit.resize(numDofs);
13347 joint_range.resize(numDofs);
13348 rest_pose.resize(numDofs);
13349 for (int i = 0; i < numDofs; ++i)
13350 {
13351 lower_limit[i] = clientCmd.m_calculateInverseKinematicsArguments.m_lowerLimit[i];
13352 upper_limit[i] = clientCmd.m_calculateInverseKinematicsArguments.m_upperLimit[i];
13353 joint_range[i] = clientCmd.m_calculateInverseKinematicsArguments.m_jointRange[i];
13354 rest_pose[i] = clientCmd.m_calculateInverseKinematicsArguments.m_restPose[i];
13355 }
13356 {
13357 BT_PROFILE("computeNullspaceVel");
13358 ikHelperPtr->computeNullspaceVel(numDofs, &q_current[0], &lower_limit[0], &upper_limit[0], &joint_range[0], &rest_pose[0]);
13359 }
13360 }
13361
13362 //btTransform endEffectorTransformWorld = bodyHandle->m_multiBody->getLink(endEffectorLinkIndex).m_cachedWorldTransform * bodyHandle->m_linkLocalInertialFrames[endEffectorLinkIndex].inverse();
13363
13364 btVector3DoubleData endEffectorWorldPosition;
13365 btQuaternionDoubleData endEffectorWorldOrientation;
13366
13367 //get the position from the inverse dynamics (based on q) instead of endEffectorTransformWorld
13368 btVector3 endEffectorPosWorldOrg = world_origin;
13369 btQuaternion endEffectorOriWorldOrg;
13370 world_rot.getRotation(endEffectorOriWorldOrg);
13371
13372 btTransform endEffectorBaseCoord;
13373 endEffectorBaseCoord.setOrigin(endEffectorPosWorldOrg);
13374 endEffectorBaseCoord.setRotation(endEffectorOriWorldOrg);
13375
13376 //don't need the next two lines
13377 //btTransform linkInertiaInv = bodyHandle->m_linkLocalInertialFrames[endEffectorLinkIndex].inverse();
13378 //endEffectorBaseCoord = endEffectorBaseCoord * linkInertiaInv;
13379
13380 //btTransform tr = bodyHandle->m_multiBody->getBaseWorldTransform();
13381 //endEffectorBaseCoord = tr.inverse()*endEffectorTransformWorld;
13382 //endEffectorBaseCoord = tr.inverse()*endEffectorTransformWorld;
13383
13384 btQuaternion endEffectorOriBaseCoord = endEffectorBaseCoord.getRotation();
13385
13386 //btVector4 endEffectorOri(endEffectorOriBaseCoord.x(), endEffectorOriBaseCoord.y(), endEffectorOriBaseCoord.z(), endEffectorOriBaseCoord.w());
13387
13388 endEffectorBaseCoord.getOrigin().serializeDouble(endEffectorWorldPosition);
13389 endEffectorBaseCoord.getRotation().serializeDouble(endEffectorWorldOrientation);
13390
13391 // Set joint damping coefficents. A small default
13392 // damping constant is added to prevent singularity
13393 // with pseudo inverse. The user can set joint damping
13394 // coefficients differently for each joint. The larger
13395 // the damping coefficient is, the less we rely on
13396 // this joint to achieve the IK target.
13397 btAlignedObjectArray<double> joint_damping;
13398 joint_damping.resize(numDofs, 0.5);
13399 if (clientCmd.m_updateFlags & IK_HAS_JOINT_DAMPING)
13400 {
13401 for (int i = 0; i < numDofs; ++i)
13402 {
13403 joint_damping[i] = clientCmd.m_calculateInverseKinematicsArguments.m_jointDamping[i];
13404 }
13405 }
13406 ikHelperPtr->setDampingCoeff(numDofs, &joint_damping[0]);
13407
13408 double targetDampCoeff[6] = {1.0, 1.0, 1.0, 1.0, 1.0, 1.0};
13409 bool performedIK = false;
13410
13411 if (clientCmd.m_calculateInverseKinematicsArguments.m_numEndEffectorLinkIndices == 1)
13412 {
13413 BT_PROFILE("computeIK");
13414 ikHelperPtr->computeIK(&endEffectorTargetWorldPositions[0],
13415 &endEffectorTargetWorldOrientations[0],
13416 endEffectorWorldPosition.m_floats, endEffectorWorldOrientation.m_floats,
13417 &q_current[0],
13418 numDofs, clientCmd.m_calculateInverseKinematicsArguments.m_endEffectorLinkIndices[0],
13419 &q_new[0], ikMethod, &jacobian_linear[0], &jacobian_angular[0], jacSize * 2, targetDampCoeff);
13420 performedIK = true;
13421 }
13422 else
13423 {
13424 if (clientCmd.m_calculateInverseKinematicsArguments.m_numEndEffectorLinkIndices > 1)
13425 {
13426 ikHelperPtr->computeIK2(&endEffectorTargetWorldPositions[0],
13427 &endEffectorCurrentWorldPositions[0],
13428 clientCmd.m_calculateInverseKinematicsArguments.m_numEndEffectorLinkIndices,
13429 //endEffectorWorldOrientation.m_floats,
13430 &q_current[0],
13431 numDofs,
13432 &q_new[0], ikMethod, &jacobian_linear[0], targetDampCoeff);
13433 performedIK = true;
13434 }
13435 }
13436 if (performedIK)
13437 {
13438 serverCmd.m_inverseKinematicsResultArgs.m_bodyUniqueId = clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId;
13439 for (int i = 0; i < numDofs; i++)
13440 {
13441 serverCmd.m_inverseKinematicsResultArgs.m_jointPositions[i] = q_new[i];
13442 }
13443 serverCmd.m_inverseKinematicsResultArgs.m_dofCount = numDofs;
13444 serverCmd.m_type = CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED;
13445 for (int i = 0; i < numDofs; i++)
13446 {
13447 startingPositions[i] = q_new[i];
13448 }
13449 }
13450 }
13451 }
13452 }
13453 }
13454 return hasStatus;
13455 }
13456
13457 // PyModule_AddIntConstant(m, "GEOM_SPHERE", GEOM_SPHERE);
13458 // PyModule_AddIntConstant(m, "GEOM_BOX", GEOM_BOX);
13459 // PyModule_AddIntConstant(m, "GEOM_CYLINDER", GEOM_CYLINDER);
13460 // PyModule_AddIntConstant(m, "GEOM_MESH", GEOM_MESH);
13461 // PyModule_AddIntConstant(m, "GEOM_PLANE", GEOM_PLANE);
13462 // PyModule_AddIntConstant(m, "GEOM_CAPSULE", GEOM_CAPSULE);
13463
extractCollisionShapes(const btCollisionShape * colShape,const btTransform & transform,b3CollisionShapeData * collisionShapeBuffer,int maxCollisionShapes)13464 int PhysicsServerCommandProcessor::extractCollisionShapes(const btCollisionShape* colShape, const btTransform& transform, b3CollisionShapeData* collisionShapeBuffer, int maxCollisionShapes)
13465 {
13466 if (maxCollisionShapes <= 0)
13467 {
13468 b3Warning("No space in buffer");
13469 return 0;
13470 }
13471
13472 int numConverted = 0;
13473
13474 collisionShapeBuffer[0].m_localCollisionFrame[0] = transform.getOrigin()[0];
13475 collisionShapeBuffer[0].m_localCollisionFrame[1] = transform.getOrigin()[1];
13476 collisionShapeBuffer[0].m_localCollisionFrame[2] = transform.getOrigin()[2];
13477 collisionShapeBuffer[0].m_localCollisionFrame[3] = transform.getRotation()[0];
13478 collisionShapeBuffer[0].m_localCollisionFrame[4] = transform.getRotation()[1];
13479 collisionShapeBuffer[0].m_localCollisionFrame[5] = transform.getRotation()[2];
13480 collisionShapeBuffer[0].m_localCollisionFrame[6] = transform.getRotation()[3];
13481 collisionShapeBuffer[0].m_meshAssetFileName[0] = 0;
13482
13483 switch (colShape->getShapeType())
13484 {
13485 case MULTI_SPHERE_SHAPE_PROXYTYPE:
13486 {
13487 btCapsuleShapeZ* capsule = (btCapsuleShapeZ*)colShape;
13488 collisionShapeBuffer[0].m_collisionGeometryType = GEOM_CAPSULE;
13489 collisionShapeBuffer[0].m_dimensions[0] = 2. * capsule->getHalfHeight();
13490 collisionShapeBuffer[0].m_dimensions[1] = capsule->getRadius();
13491 collisionShapeBuffer[0].m_dimensions[2] = 0;
13492 numConverted++;
13493 break;
13494 break;
13495 }
13496 case STATIC_PLANE_PROXYTYPE:
13497 {
13498 btStaticPlaneShape* plane = (btStaticPlaneShape*)colShape;
13499 collisionShapeBuffer[0].m_collisionGeometryType = GEOM_PLANE;
13500 collisionShapeBuffer[0].m_dimensions[0] = plane->getPlaneNormal()[0];
13501 collisionShapeBuffer[0].m_dimensions[1] = plane->getPlaneNormal()[1];
13502 collisionShapeBuffer[0].m_dimensions[2] = plane->getPlaneNormal()[2];
13503 numConverted += 1;
13504 break;
13505 }
13506 case TRIANGLE_MESH_SHAPE_PROXYTYPE:
13507 case SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE:
13508 case CONVEX_HULL_SHAPE_PROXYTYPE:
13509 {
13510 UrdfCollision* urdfCol = m_data->m_bulletCollisionShape2UrdfCollision.find(colShape);
13511 if (urdfCol && (urdfCol->m_geometry.m_type == URDF_GEOM_MESH))
13512 {
13513 collisionShapeBuffer[0].m_collisionGeometryType = GEOM_MESH;
13514 collisionShapeBuffer[0].m_dimensions[0] = urdfCol->m_geometry.m_meshScale[0];
13515 collisionShapeBuffer[0].m_dimensions[1] = urdfCol->m_geometry.m_meshScale[1];
13516 collisionShapeBuffer[0].m_dimensions[2] = urdfCol->m_geometry.m_meshScale[2];
13517 strcpy(collisionShapeBuffer[0].m_meshAssetFileName, urdfCol->m_geometry.m_meshFileName.c_str());
13518 numConverted += 1;
13519 }
13520 else
13521 {
13522 collisionShapeBuffer[0].m_collisionGeometryType = GEOM_MESH;
13523 sprintf(collisionShapeBuffer[0].m_meshAssetFileName, "unknown_file");
13524 collisionShapeBuffer[0].m_dimensions[0] = 1;
13525 collisionShapeBuffer[0].m_dimensions[1] = 1;
13526 collisionShapeBuffer[0].m_dimensions[2] = 1;
13527 numConverted++;
13528 }
13529
13530 break;
13531 }
13532 case CAPSULE_SHAPE_PROXYTYPE:
13533 {
13534 btCapsuleShapeZ* capsule = (btCapsuleShapeZ*)colShape;
13535 collisionShapeBuffer[0].m_collisionGeometryType = GEOM_CAPSULE;
13536 collisionShapeBuffer[0].m_dimensions[0] = 2. * capsule->getHalfHeight();
13537 collisionShapeBuffer[0].m_dimensions[1] = capsule->getRadius();
13538 collisionShapeBuffer[0].m_dimensions[2] = 0;
13539 numConverted++;
13540 break;
13541 }
13542 case CYLINDER_SHAPE_PROXYTYPE:
13543 {
13544 btCylinderShapeZ* cyl = (btCylinderShapeZ*)colShape;
13545 collisionShapeBuffer[0].m_collisionGeometryType = GEOM_CYLINDER;
13546 collisionShapeBuffer[0].m_dimensions[0] = 2. * cyl->getHalfExtentsWithMargin().getZ();
13547 collisionShapeBuffer[0].m_dimensions[1] = cyl->getHalfExtentsWithMargin().getX();
13548 collisionShapeBuffer[0].m_dimensions[2] = 0;
13549 numConverted++;
13550 break;
13551 }
13552 case BOX_SHAPE_PROXYTYPE:
13553 {
13554 btBoxShape* box = (btBoxShape*)colShape;
13555 btVector3 halfExtents = box->getHalfExtentsWithMargin();
13556 collisionShapeBuffer[0].m_collisionGeometryType = GEOM_BOX;
13557 collisionShapeBuffer[0].m_dimensions[0] = 2. * halfExtents[0];
13558 collisionShapeBuffer[0].m_dimensions[1] = 2. * halfExtents[1];
13559 collisionShapeBuffer[0].m_dimensions[2] = 2. * halfExtents[2];
13560 numConverted++;
13561 break;
13562 }
13563 case SPHERE_SHAPE_PROXYTYPE:
13564 {
13565 btSphereShape* sphere = (btSphereShape*)colShape;
13566 collisionShapeBuffer[0].m_collisionGeometryType = GEOM_SPHERE;
13567 collisionShapeBuffer[0].m_dimensions[0] = sphere->getRadius();
13568 collisionShapeBuffer[0].m_dimensions[1] = sphere->getRadius();
13569 collisionShapeBuffer[0].m_dimensions[2] = sphere->getRadius();
13570 numConverted++;
13571 break;
13572 }
13573 case COMPOUND_SHAPE_PROXYTYPE:
13574 {
13575 //it could be a compound mesh from a wavefront OBJ, check it
13576 UrdfCollision* urdfCol = m_data->m_bulletCollisionShape2UrdfCollision.find(colShape);
13577 if (urdfCol && (urdfCol->m_geometry.m_type == URDF_GEOM_MESH))
13578 {
13579 collisionShapeBuffer[0].m_collisionGeometryType = GEOM_MESH;
13580 collisionShapeBuffer[0].m_dimensions[0] = urdfCol->m_geometry.m_meshScale[0];
13581 collisionShapeBuffer[0].m_dimensions[1] = urdfCol->m_geometry.m_meshScale[1];
13582 collisionShapeBuffer[0].m_dimensions[2] = urdfCol->m_geometry.m_meshScale[2];
13583 strcpy(collisionShapeBuffer[0].m_meshAssetFileName, urdfCol->m_geometry.m_meshFileName.c_str());
13584 numConverted += 1;
13585 }
13586 else
13587 {
13588 //recurse, accumulate childTransform
13589 btCompoundShape* compound = (btCompoundShape*)colShape;
13590 for (int i = 0; i < compound->getNumChildShapes(); i++)
13591 {
13592 btTransform childTrans = transform * compound->getChildTransform(i);
13593 int remain = maxCollisionShapes - numConverted;
13594 int converted = extractCollisionShapes(compound->getChildShape(i), childTrans, &collisionShapeBuffer[numConverted], remain);
13595 numConverted += converted;
13596 }
13597 }
13598 break;
13599 }
13600 default:
13601 {
13602 b3Warning("Unexpected collision shape type in PhysicsServerCommandProcessor::extractCollisionShapes");
13603 }
13604 };
13605
13606 return numConverted;
13607 }
13608
processRequestCollisionShapeInfoCommand(const struct SharedMemoryCommand & clientCmd,struct SharedMemoryStatus & serverStatusOut,char * bufferServerToClient,int bufferSizeInBytes)13609 bool PhysicsServerCommandProcessor::processRequestCollisionShapeInfoCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
13610 {
13611 bool hasStatus = true;
13612
13613 BT_PROFILE("CMD_REQUEST_COLLISION_SHAPE_INFO");
13614 SharedMemoryStatus& serverCmd = serverStatusOut;
13615 serverCmd.m_type = CMD_COLLISION_SHAPE_INFO_FAILED;
13616 int bodyUniqueId = clientCmd.m_requestCollisionShapeDataArguments.m_bodyUniqueId;
13617 int linkIndex = clientCmd.m_requestCollisionShapeDataArguments.m_linkIndex;
13618 InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(bodyUniqueId);
13619 if (bodyHandle)
13620 {
13621 if (bodyHandle->m_multiBody)
13622 {
13623 b3CollisionShapeData* collisionShapeStoragePtr = (b3CollisionShapeData*)bufferServerToClient;
13624 collisionShapeStoragePtr->m_objectUniqueId = bodyUniqueId;
13625 collisionShapeStoragePtr->m_linkIndex = linkIndex;
13626 int totalBytesPerObject = sizeof(b3CollisionShapeData);
13627 int maxNumColObjects = bufferSizeInBytes / totalBytesPerObject - 1;
13628 btTransform childTrans;
13629 childTrans.setIdentity();
13630 serverCmd.m_sendCollisionShapeArgs.m_bodyUniqueId = bodyUniqueId;
13631 serverCmd.m_sendCollisionShapeArgs.m_linkIndex = linkIndex;
13632
13633 if (linkIndex == -1)
13634 {
13635 if (bodyHandle->m_multiBody->getBaseCollider())
13636 {
13637 //extract shape info from base collider
13638 int numConvertedCollisionShapes = extractCollisionShapes(bodyHandle->m_multiBody->getBaseCollider()->getCollisionShape(), childTrans, collisionShapeStoragePtr, maxNumColObjects);
13639 serverCmd.m_numDataStreamBytes = numConvertedCollisionShapes * sizeof(b3CollisionShapeData);
13640 serverCmd.m_sendCollisionShapeArgs.m_numCollisionShapes = numConvertedCollisionShapes;
13641 serverCmd.m_type = CMD_COLLISION_SHAPE_INFO_COMPLETED;
13642 }
13643 }
13644 else
13645 {
13646 if (linkIndex >= 0 && linkIndex < bodyHandle->m_multiBody->getNumLinks() && bodyHandle->m_multiBody->getLinkCollider(linkIndex))
13647 {
13648 int numConvertedCollisionShapes = extractCollisionShapes(bodyHandle->m_multiBody->getLinkCollider(linkIndex)->getCollisionShape(), childTrans, collisionShapeStoragePtr, maxNumColObjects);
13649 serverCmd.m_numDataStreamBytes = numConvertedCollisionShapes * sizeof(b3CollisionShapeData);
13650 serverCmd.m_sendCollisionShapeArgs.m_numCollisionShapes = numConvertedCollisionShapes;
13651 serverCmd.m_type = CMD_COLLISION_SHAPE_INFO_COMPLETED;
13652 }
13653 }
13654 }
13655 }
13656
13657 return hasStatus;
13658 }
processRequestVisualShapeInfoCommand(const struct SharedMemoryCommand & clientCmd,struct SharedMemoryStatus & serverStatusOut,char * bufferServerToClient,int bufferSizeInBytes)13659 bool PhysicsServerCommandProcessor::processRequestVisualShapeInfoCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
13660 {
13661 bool hasStatus = true;
13662
13663 BT_PROFILE("CMD_REQUEST_VISUAL_SHAPE_INFO");
13664 SharedMemoryStatus& serverCmd = serverStatusOut;
13665 serverCmd.m_type = CMD_VISUAL_SHAPE_INFO_FAILED;
13666 //retrieve the visual shape information for a specific body
13667
13668 if (m_data->m_pluginManager.getRenderInterface())
13669 {
13670 int totalNumVisualShapes = m_data->m_pluginManager.getRenderInterface()->getNumVisualShapes(clientCmd.m_requestVisualShapeDataArguments.m_bodyUniqueId);
13671 //int totalBytesPerVisualShape = sizeof (b3VisualShapeData);
13672 //int visualShapeStorage = bufferSizeInBytes / totalBytesPerVisualShape - 1;
13673
13674 //set serverCmd.m_sendVisualShapeArgs when totalNumVisualShapes is zero
13675 if (totalNumVisualShapes == 0)
13676 {
13677 serverCmd.m_sendVisualShapeArgs.m_numRemainingVisualShapes = 0;
13678 serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied = 0;
13679 serverCmd.m_sendVisualShapeArgs.m_startingVisualShapeIndex = clientCmd.m_requestVisualShapeDataArguments.m_startingVisualShapeIndex;
13680 serverCmd.m_sendVisualShapeArgs.m_bodyUniqueId = clientCmd.m_requestVisualShapeDataArguments.m_bodyUniqueId;
13681 serverCmd.m_numDataStreamBytes = sizeof(b3VisualShapeData) * serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied;
13682 serverCmd.m_type = CMD_VISUAL_SHAPE_INFO_COMPLETED;
13683 }
13684
13685 else
13686 {
13687 b3VisualShapeData* visualShapeStoragePtr = (b3VisualShapeData*)bufferServerToClient;
13688
13689 int remain = totalNumVisualShapes - clientCmd.m_requestVisualShapeDataArguments.m_startingVisualShapeIndex;
13690 int shapeIndex = clientCmd.m_requestVisualShapeDataArguments.m_startingVisualShapeIndex;
13691
13692 int success = m_data->m_pluginManager.getRenderInterface()->getVisualShapesData(clientCmd.m_requestVisualShapeDataArguments.m_bodyUniqueId,
13693 shapeIndex,
13694 visualShapeStoragePtr);
13695 if (success)
13696 {
13697 //find the matching texture unique ids.
13698 if (visualShapeStoragePtr->m_tinyRendererTextureId >= 0)
13699 {
13700 b3AlignedObjectArray<int> usedHandles;
13701 m_data->m_textureHandles.getUsedHandles(usedHandles);
13702
13703 for (int i = 0; i < usedHandles.size(); i++)
13704 {
13705 int texHandle = usedHandles[i];
13706 InternalTextureHandle* texH = m_data->m_textureHandles.getHandle(texHandle);
13707 if (texH && (texH->m_tinyRendererTextureId == visualShapeStoragePtr->m_tinyRendererTextureId))
13708 {
13709 visualShapeStoragePtr->m_openglTextureId = texH->m_openglTextureId;
13710 visualShapeStoragePtr->m_textureUniqueId = texHandle;
13711 }
13712 }
13713 }
13714
13715 serverCmd.m_sendVisualShapeArgs.m_numRemainingVisualShapes = remain - 1;
13716 serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied = 1;
13717 serverCmd.m_sendVisualShapeArgs.m_startingVisualShapeIndex = clientCmd.m_requestVisualShapeDataArguments.m_startingVisualShapeIndex;
13718 serverCmd.m_sendVisualShapeArgs.m_bodyUniqueId = clientCmd.m_requestVisualShapeDataArguments.m_bodyUniqueId;
13719 serverCmd.m_numDataStreamBytes = sizeof(b3VisualShapeData) * serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied;
13720 serverCmd.m_type = CMD_VISUAL_SHAPE_INFO_COMPLETED;
13721 }
13722 else
13723 {
13724 b3Warning("failed to get shape info");
13725 }
13726 }
13727 }
13728 return hasStatus;
13729 }
13730
processUpdateVisualShapeCommand(const struct SharedMemoryCommand & clientCmd,struct SharedMemoryStatus & serverStatusOut,char * bufferServerToClient,int bufferSizeInBytes)13731 bool PhysicsServerCommandProcessor::processUpdateVisualShapeCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
13732 {
13733 bool hasStatus = true;
13734
13735 BT_PROFILE("CMD_UPDATE_VISUAL_SHAPE");
13736 SharedMemoryStatus& serverCmd = serverStatusOut;
13737 serverCmd.m_type = CMD_VISUAL_SHAPE_UPDATE_FAILED;
13738 InternalTextureHandle* texHandle = 0;
13739
13740 if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_TEXTURE)
13741 {
13742 if (clientCmd.m_updateVisualShapeDataArguments.m_textureUniqueId >= 0)
13743 {
13744 texHandle = m_data->m_textureHandles.getHandle(clientCmd.m_updateVisualShapeDataArguments.m_textureUniqueId);
13745 }
13746
13747 if (clientCmd.m_updateVisualShapeDataArguments.m_textureUniqueId >= -1)
13748 {
13749 if (texHandle)
13750 {
13751 if (m_data->m_pluginManager.getRenderInterface())
13752 {
13753 m_data->m_pluginManager.getRenderInterface()->changeShapeTexture(clientCmd.m_updateVisualShapeDataArguments.m_bodyUniqueId,
13754 clientCmd.m_updateVisualShapeDataArguments.m_jointIndex,
13755 clientCmd.m_updateVisualShapeDataArguments.m_shapeIndex,
13756 texHandle->m_tinyRendererTextureId);
13757 }
13758 }
13759 else
13760 {
13761 m_data->m_pluginManager.getRenderInterface()->changeShapeTexture(clientCmd.m_updateVisualShapeDataArguments.m_bodyUniqueId,
13762 clientCmd.m_updateVisualShapeDataArguments.m_jointIndex,
13763 clientCmd.m_updateVisualShapeDataArguments.m_shapeIndex,-1);
13764 }
13765 }
13766 }
13767
13768 {
13769 int bodyUniqueId = clientCmd.m_updateVisualShapeDataArguments.m_bodyUniqueId;
13770 int linkIndex = clientCmd.m_updateVisualShapeDataArguments.m_jointIndex;
13771
13772 InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(bodyUniqueId);
13773 if (bodyHandle)
13774 {
13775 if (bodyHandle->m_multiBody)
13776 {
13777 if (linkIndex == -1)
13778 {
13779 if (bodyHandle->m_multiBody->getBaseCollider())
13780 {
13781 int graphicsIndex = bodyHandle->m_multiBody->getBaseCollider()->getUserIndex();
13782 if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_TEXTURE)
13783 {
13784 int shapeIndex = m_data->m_guiHelper->getShapeIndexFromInstance(graphicsIndex);
13785 if (texHandle)
13786 {
13787 m_data->m_guiHelper->replaceTexture(shapeIndex, texHandle->m_openglTextureId);
13788 }
13789 else
13790 {
13791 m_data->m_guiHelper->replaceTexture(shapeIndex, -1);
13792 }
13793 }
13794 if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_RGBA_COLOR)
13795 {
13796 if (m_data->m_pluginManager.getRenderInterface())
13797 {
13798 m_data->m_pluginManager.getRenderInterface()->changeRGBAColor(bodyUniqueId, linkIndex,
13799 clientCmd.m_updateVisualShapeDataArguments.m_shapeIndex,
13800 clientCmd.m_updateVisualShapeDataArguments.m_rgbaColor);
13801 }
13802 m_data->m_guiHelper->changeRGBAColor(graphicsIndex, clientCmd.m_updateVisualShapeDataArguments.m_rgbaColor);
13803 }
13804 if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_SPECULAR_COLOR)
13805 {
13806 m_data->m_guiHelper->changeSpecularColor(graphicsIndex, clientCmd.m_updateVisualShapeDataArguments.m_specularColor);
13807 }
13808 }
13809 }
13810 else
13811 {
13812 if (linkIndex < bodyHandle->m_multiBody->getNumLinks())
13813 {
13814 if (bodyHandle->m_multiBody->getLink(linkIndex).m_collider)
13815 {
13816 int graphicsIndex = bodyHandle->m_multiBody->getLink(linkIndex).m_collider->getUserIndex();
13817 if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_TEXTURE)
13818 {
13819 int shapeIndex = m_data->m_guiHelper->getShapeIndexFromInstance(graphicsIndex);
13820 if (texHandle)
13821 {
13822 m_data->m_guiHelper->replaceTexture(shapeIndex, texHandle->m_openglTextureId);
13823 }
13824 else
13825 {
13826 m_data->m_guiHelper->replaceTexture(shapeIndex, -1);
13827 }
13828 }
13829 if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_RGBA_COLOR)
13830 {
13831 if (m_data->m_pluginManager.getRenderInterface())
13832 {
13833 m_data->m_pluginManager.getRenderInterface()->changeRGBAColor(bodyUniqueId, linkIndex,
13834 clientCmd.m_updateVisualShapeDataArguments.m_shapeIndex, clientCmd.m_updateVisualShapeDataArguments.m_rgbaColor);
13835 }
13836 m_data->m_guiHelper->changeRGBAColor(graphicsIndex, clientCmd.m_updateVisualShapeDataArguments.m_rgbaColor);
13837 }
13838 if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_SPECULAR_COLOR)
13839 {
13840 m_data->m_guiHelper->changeSpecularColor(graphicsIndex, clientCmd.m_updateVisualShapeDataArguments.m_specularColor);
13841 }
13842 }
13843 }
13844 }
13845 }
13846 else
13847 {
13848 if (bodyHandle->m_rigidBody)
13849 {
13850 int graphicsIndex = bodyHandle->m_rigidBody->getUserIndex();
13851 if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_TEXTURE)
13852 {
13853 if (texHandle)
13854 {
13855 int shapeIndex = m_data->m_guiHelper->getShapeIndexFromInstance(graphicsIndex);
13856 m_data->m_guiHelper->replaceTexture(shapeIndex, texHandle->m_openglTextureId);
13857 }
13858 }
13859 if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_RGBA_COLOR)
13860 {
13861 if (m_data->m_pluginManager.getRenderInterface())
13862 {
13863 m_data->m_pluginManager.getRenderInterface()->changeRGBAColor(bodyUniqueId, linkIndex,
13864 clientCmd.m_updateVisualShapeDataArguments.m_shapeIndex, clientCmd.m_updateVisualShapeDataArguments.m_rgbaColor);
13865 }
13866 m_data->m_guiHelper->changeRGBAColor(graphicsIndex, clientCmd.m_updateVisualShapeDataArguments.m_rgbaColor);
13867 }
13868 if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_SPECULAR_COLOR)
13869 {
13870 m_data->m_guiHelper->changeSpecularColor(graphicsIndex, clientCmd.m_updateVisualShapeDataArguments.m_specularColor);
13871 }
13872 }
13873 #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
13874
13875 else if (bodyHandle->m_softBody)
13876 {
13877 int graphicsIndex = bodyHandle->m_softBody->getUserIndex();
13878 if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_TEXTURE)
13879 {
13880 int shapeIndex = m_data->m_guiHelper->getShapeIndexFromInstance(graphicsIndex);
13881 if (texHandle)
13882 {
13883 m_data->m_guiHelper->replaceTexture(shapeIndex, texHandle->m_openglTextureId);
13884 }
13885 else
13886 {
13887 m_data->m_guiHelper->replaceTexture(shapeIndex, -1);
13888 }
13889 }
13890
13891 if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_RGBA_COLOR)
13892 {
13893 if (m_data->m_pluginManager.getRenderInterface())
13894 {
13895 m_data->m_pluginManager.getRenderInterface()->changeRGBAColor(bodyUniqueId, linkIndex,
13896 clientCmd.m_updateVisualShapeDataArguments.m_shapeIndex, clientCmd.m_updateVisualShapeDataArguments.m_rgbaColor);
13897 }
13898 m_data->m_guiHelper->changeRGBAColor(graphicsIndex, clientCmd.m_updateVisualShapeDataArguments.m_rgbaColor);
13899 }
13900
13901 if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_FLAGS)
13902 {
13903 if (m_data->m_pluginManager.getRenderInterface())
13904 {
13905 m_data->m_pluginManager.getRenderInterface()->changeInstanceFlags(bodyUniqueId, linkIndex,
13906 clientCmd.m_updateVisualShapeDataArguments.m_shapeIndex,
13907 clientCmd.m_updateVisualShapeDataArguments.m_flags);
13908 }
13909 m_data->m_guiHelper->changeInstanceFlags(graphicsIndex,
13910 clientCmd.m_updateVisualShapeDataArguments.m_flags);
13911 }
13912
13913 }
13914 #endif
13915 }
13916 }
13917 }
13918
13919 serverCmd.m_type = CMD_VISUAL_SHAPE_UPDATE_COMPLETED;
13920
13921 b3Notification notification;
13922 notification.m_notificationType = VISUAL_SHAPE_CHANGED;
13923 notification.m_visualShapeArgs.m_bodyUniqueId = clientCmd.m_updateVisualShapeDataArguments.m_bodyUniqueId;
13924 notification.m_visualShapeArgs.m_linkIndex = clientCmd.m_updateVisualShapeDataArguments.m_jointIndex;
13925 notification.m_visualShapeArgs.m_visualShapeIndex = clientCmd.m_updateVisualShapeDataArguments.m_shapeIndex;
13926 m_data->m_pluginManager.addNotification(notification);
13927
13928 return hasStatus;
13929 }
13930
processChangeTextureCommand(const struct SharedMemoryCommand & clientCmd,struct SharedMemoryStatus & serverStatusOut,char * bufferServerToClient,int bufferSizeInBytes)13931 bool PhysicsServerCommandProcessor::processChangeTextureCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
13932 {
13933 bool hasStatus = true;
13934
13935 SharedMemoryStatus& serverCmd = serverStatusOut;
13936 serverCmd.m_type = CMD_CHANGE_TEXTURE_COMMAND_FAILED;
13937
13938 InternalTextureHandle* texH = m_data->m_textureHandles.getHandle(clientCmd.m_changeTextureArgs.m_textureUniqueId);
13939 if (texH)
13940 {
13941 int gltex = texH->m_openglTextureId;
13942 m_data->m_guiHelper->changeTexture(gltex,
13943 (const unsigned char*)bufferServerToClient, clientCmd.m_changeTextureArgs.m_width, clientCmd.m_changeTextureArgs.m_height);
13944
13945 serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED;
13946 }
13947 return hasStatus;
13948 }
13949
processLoadTextureCommand(const struct SharedMemoryCommand & clientCmd,struct SharedMemoryStatus & serverStatusOut,char * bufferServerToClient,int bufferSizeInBytes)13950 bool PhysicsServerCommandProcessor::processLoadTextureCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
13951 {
13952 bool hasStatus = true;
13953
13954 BT_PROFILE("CMD_LOAD_TEXTURE");
13955 SharedMemoryStatus& serverCmd = serverStatusOut;
13956 serverCmd.m_type = CMD_LOAD_TEXTURE_FAILED;
13957
13958 char relativeFileName[1024];
13959 char pathPrefix[1024];
13960
13961 CommonFileIOInterface* fileIO(m_data->m_pluginManager.getFileIOInterface());
13962 if (fileIO->findResourcePath(clientCmd.m_loadTextureArguments.m_textureFileName, relativeFileName, 1024))
13963 {
13964 b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024);
13965
13966 int texHandle = m_data->m_textureHandles.allocHandle();
13967 InternalTextureHandle* texH = m_data->m_textureHandles.getHandle(texHandle);
13968 if (texH)
13969 {
13970 texH->m_tinyRendererTextureId = -1;
13971 texH->m_openglTextureId = -1;
13972
13973 int uid = -1;
13974 if (m_data->m_pluginManager.getRenderInterface())
13975 {
13976 uid = m_data->m_pluginManager.getRenderInterface()->loadTextureFile(relativeFileName, fileIO);
13977 }
13978 if (uid >= 0)
13979 {
13980 texH->m_tinyRendererTextureId = uid;
13981 }
13982
13983 {
13984 int width, height, n;
13985 unsigned char* imageData = 0;
13986
13987 CommonFileIOInterface* fileIO = m_data->m_pluginManager.getFileIOInterface();
13988 if (fileIO)
13989 {
13990 b3AlignedObjectArray<char> buffer;
13991 buffer.reserve(1024);
13992 int fileId = fileIO->fileOpen(relativeFileName, "rb");
13993 if (fileId >= 0)
13994 {
13995 int size = fileIO->getFileSize(fileId);
13996 if (size > 0)
13997 {
13998 buffer.resize(size);
13999 int actual = fileIO->fileRead(fileId, &buffer[0], size);
14000 if (actual != size)
14001 {
14002 b3Warning("image filesize mismatch!\n");
14003 buffer.resize(0);
14004 }
14005 }
14006 fileIO->fileClose(fileId);
14007 }
14008 if (buffer.size())
14009 {
14010 imageData = stbi_load_from_memory((const unsigned char*)&buffer[0], buffer.size(), &width, &height, &n, 3);
14011 }
14012 }
14013 else
14014 {
14015 imageData = stbi_load(relativeFileName, &width, &height, &n, 3);
14016 }
14017
14018 if (imageData)
14019 {
14020 texH->m_openglTextureId = m_data->m_guiHelper->registerTexture(imageData, width, height);
14021 m_data->m_allocatedTexturesRequireFree.push_back(imageData);
14022 }
14023 else
14024 {
14025 b3Warning("Unsupported texture image format [%s]\n", relativeFileName);
14026 }
14027 }
14028 serverCmd.m_loadTextureResultArguments.m_textureUniqueId = texHandle;
14029 serverCmd.m_type = CMD_LOAD_TEXTURE_COMPLETED;
14030 }
14031 }
14032 else
14033 {
14034 serverCmd.m_type = CMD_LOAD_TEXTURE_FAILED;
14035 }
14036 return hasStatus;
14037 }
14038
processSaveStateCommand(const struct SharedMemoryCommand & clientCmd,struct SharedMemoryStatus & serverStatusOut,char * bufferServerToClient,int bufferSizeInBytes)14039 bool PhysicsServerCommandProcessor::processSaveStateCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
14040 {
14041 BT_PROFILE("CMD_SAVE_STATE");
14042 bool hasStatus = true;
14043 SharedMemoryStatus& serverCmd = serverStatusOut;
14044 serverCmd.m_type = CMD_SAVE_STATE_FAILED;
14045
14046 btDefaultSerializer* ser = new btDefaultSerializer();
14047 int currentFlags = ser->getSerializationFlags();
14048 ser->setSerializationFlags(currentFlags | BT_SERIALIZE_CONTACT_MANIFOLDS);
14049 m_data->m_dynamicsWorld->serialize(ser);
14050 bParse::btBulletFile* bulletFile = new bParse::btBulletFile((char*)ser->getBufferPointer(), ser->getCurrentBufferSize());
14051 bulletFile->parse(false);
14052 if (bulletFile->ok())
14053 {
14054 serverCmd.m_type = CMD_SAVE_STATE_COMPLETED;
14055 //re-use state if available
14056 int reuseStateId = -1;
14057 for (int i = 0; i < m_data->m_savedStates.size(); i++)
14058 {
14059 if (m_data->m_savedStates[i].m_bulletFile == 0)
14060 {
14061 reuseStateId = i;
14062 break;
14063 }
14064 }
14065 SaveStateData sd;
14066 sd.m_bulletFile = bulletFile;
14067 sd.m_serializer = ser;
14068 if (reuseStateId >= 0)
14069 {
14070 serverCmd.m_saveStateResultArgs.m_stateId = reuseStateId;
14071 m_data->m_savedStates[reuseStateId] = sd;
14072 }
14073 else
14074 {
14075 serverCmd.m_saveStateResultArgs.m_stateId = m_data->m_savedStates.size();
14076 m_data->m_savedStates.push_back(sd);
14077 }
14078 }
14079 return hasStatus;
14080 }
14081
processRemoveStateCommand(const struct SharedMemoryCommand & clientCmd,struct SharedMemoryStatus & serverStatusOut,char * bufferServerToClient,int bufferSizeInBytes)14082 bool PhysicsServerCommandProcessor::processRemoveStateCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
14083 {
14084 BT_PROFILE("CMD_REMOVE_STATE");
14085 bool hasStatus = true;
14086 SharedMemoryStatus& serverCmd = serverStatusOut;
14087 serverCmd.m_type = CMD_REMOVE_STATE_FAILED;
14088
14089 if (clientCmd.m_loadStateArguments.m_stateId >= 0)
14090 {
14091 if (clientCmd.m_loadStateArguments.m_stateId < m_data->m_savedStates.size())
14092 {
14093 SaveStateData& sd = m_data->m_savedStates[clientCmd.m_loadStateArguments.m_stateId];
14094 delete sd.m_bulletFile;
14095 delete sd.m_serializer;
14096 sd.m_bulletFile = 0;
14097 sd.m_serializer = 0;
14098 serverCmd.m_type = CMD_REMOVE_STATE_COMPLETED;
14099 }
14100 }
14101 return hasStatus;
14102 }
14103
processRestoreStateCommand(const struct SharedMemoryCommand & clientCmd,struct SharedMemoryStatus & serverStatusOut,char * bufferServerToClient,int bufferSizeInBytes)14104 bool PhysicsServerCommandProcessor::processRestoreStateCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
14105 {
14106
14107 BT_PROFILE("CMD_RESTORE_STATE");
14108 bool hasStatus = true;
14109 SharedMemoryStatus& serverCmd = serverStatusOut;
14110 serverCmd.m_type = CMD_RESTORE_STATE_FAILED;
14111 #ifndef USE_DISCRETE_DYNAMICS_WORLD
14112 btMultiBodyWorldImporter* importer = new btMultiBodyWorldImporter(m_data->m_dynamicsWorld);
14113
14114 importer->setImporterFlags(eRESTORE_EXISTING_OBJECTS);
14115
14116 bool ok = false;
14117
14118 if (clientCmd.m_loadStateArguments.m_stateId >= 0)
14119 {
14120 if (clientCmd.m_loadStateArguments.m_stateId < m_data->m_savedStates.size())
14121 {
14122 bParse::btBulletFile* bulletFile = m_data->m_savedStates[clientCmd.m_loadStateArguments.m_stateId].m_bulletFile;
14123 if (bulletFile)
14124 {
14125 ok = importer->convertAllObjects(bulletFile);
14126 }
14127 }
14128 }
14129 else
14130 {
14131 bool found = false;
14132 char fileName[1024];
14133 fileName[0] = 0;
14134
14135 CommonFileIOInterface* fileIO = m_data->m_pluginManager.getFileIOInterface();
14136 b3AlignedObjectArray<char> buffer;
14137 buffer.reserve(1024);
14138 if (fileIO)
14139 {
14140 int fileId = -1;
14141 found = fileIO->findResourcePath(clientCmd.m_fileArguments.m_fileName, fileName, 1024);
14142 if (found)
14143 {
14144 fileId = fileIO->fileOpen(fileName, "rb");
14145 }
14146 if (fileId >= 0)
14147 {
14148 int size = fileIO->getFileSize(fileId);
14149 if (size > 0)
14150 {
14151 buffer.resize(size);
14152 int actual = fileIO->fileRead(fileId, &buffer[0], size);
14153 if (actual != size)
14154 {
14155 b3Warning("image filesize mismatch!\n");
14156 buffer.resize(0);
14157 }
14158 else
14159 {
14160 found = true;
14161 }
14162 }
14163 fileIO->fileClose(fileId);
14164 }
14165 }
14166
14167 if (found && buffer.size())
14168 {
14169 ok = importer->loadFileFromMemory(&buffer[0], buffer.size());
14170 }
14171 else
14172 {
14173 b3Error("Error in restoreState: cannot load file %s\n", clientCmd.m_fileArguments.m_fileName);
14174 }
14175 }
14176 delete importer;
14177 if (ok)
14178 {
14179 serverCmd.m_type = CMD_RESTORE_STATE_COMPLETED;
14180 }
14181 #endif
14182 return hasStatus;
14183 }
14184
processLoadBulletCommand(const struct SharedMemoryCommand & clientCmd,struct SharedMemoryStatus & serverStatusOut,char * bufferServerToClient,int bufferSizeInBytes)14185 bool PhysicsServerCommandProcessor::processLoadBulletCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
14186 {
14187 BT_PROFILE("CMD_LOAD_BULLET");
14188
14189 bool hasStatus = true;
14190 SharedMemoryStatus& serverCmd = serverStatusOut;
14191 serverCmd.m_type = CMD_BULLET_LOADING_FAILED;
14192
14193 #ifndef USE_DISCRETE_DYNAMICS_WORLD
14194 //btBulletWorldImporter* importer = new btBulletWorldImporter(m_data->m_dynamicsWorld);
14195 btMultiBodyWorldImporter* importer = new btMultiBodyWorldImporter(m_data->m_dynamicsWorld);
14196
14197 bool found = false;
14198
14199 CommonFileIOInterface* fileIO = m_data->m_pluginManager.getFileIOInterface();
14200 b3AlignedObjectArray<char> buffer;
14201 buffer.reserve(1024);
14202 if (fileIO)
14203 {
14204 char fileName[1024];
14205 int fileId = -1;
14206 found = fileIO->findResourcePath(clientCmd.m_fileArguments.m_fileName, fileName, 1024);
14207 if (found)
14208 {
14209 fileId = fileIO->fileOpen(fileName, "rb");
14210 }
14211 if (fileId >= 0)
14212 {
14213 int size = fileIO->getFileSize(fileId);
14214 if (size > 0)
14215 {
14216 buffer.resize(size);
14217 int actual = fileIO->fileRead(fileId, &buffer[0], size);
14218 if (actual != size)
14219 {
14220 b3Warning("image filesize mismatch!\n");
14221 buffer.resize(0);
14222 }
14223 else
14224 {
14225 found = true;
14226 }
14227 }
14228 fileIO->fileClose(fileId);
14229 }
14230 }
14231
14232 if (found && buffer.size())
14233 {
14234 bool ok = importer->loadFileFromMemory(&buffer[0], buffer.size());
14235 if (ok)
14236 {
14237 int numRb = importer->getNumRigidBodies();
14238 serverStatusOut.m_sdfLoadedArgs.m_numBodies = 0;
14239 serverStatusOut.m_sdfLoadedArgs.m_numUserConstraints = 0;
14240
14241 for (int i = 0; i < numRb; i++)
14242 {
14243 btCollisionObject* colObj = importer->getRigidBodyByIndex(i);
14244 if (colObj)
14245 {
14246 btRigidBody* rb = btRigidBody::upcast(colObj);
14247 if (rb)
14248 {
14249 int bodyUniqueId = m_data->m_bodyHandles.allocHandle();
14250 InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(bodyUniqueId);
14251 colObj->setUserIndex2(bodyUniqueId);
14252 bodyHandle->m_rigidBody = rb;
14253
14254 if (serverStatusOut.m_sdfLoadedArgs.m_numBodies < MAX_SDF_BODIES)
14255 {
14256 serverStatusOut.m_sdfLoadedArgs.m_numBodies++;
14257 serverStatusOut.m_sdfLoadedArgs.m_bodyUniqueIds[i] = bodyUniqueId;
14258 }
14259
14260 b3Notification notification;
14261 notification.m_notificationType = BODY_ADDED;
14262 notification.m_bodyArgs.m_bodyUniqueId = bodyUniqueId;
14263 m_data->m_pluginManager.addNotification(notification);
14264 }
14265 }
14266 }
14267
14268 serverCmd.m_type = CMD_BULLET_LOADING_COMPLETED;
14269 m_data->m_guiHelper->autogenerateGraphicsObjects(m_data->m_dynamicsWorld);
14270 }
14271 }
14272 #endif
14273 return hasStatus;
14274 }
14275
processLoadMJCFCommand(const struct SharedMemoryCommand & clientCmd,struct SharedMemoryStatus & serverStatusOut,char * bufferServerToClient,int bufferSizeInBytes)14276 bool PhysicsServerCommandProcessor::processLoadMJCFCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
14277 {
14278 bool hasStatus = true;
14279
14280 BT_PROFILE("CMD_LOAD_MJCF");
14281 SharedMemoryStatus& serverCmd = serverStatusOut;
14282 serverCmd.m_type = CMD_MJCF_LOADING_FAILED;
14283 const MjcfArgs& mjcfArgs = clientCmd.m_mjcfArguments;
14284 if (m_data->m_verboseOutput)
14285 {
14286 b3Printf("Processed CMD_LOAD_MJCF:%s", mjcfArgs.m_mjcfFileName);
14287 }
14288 bool useMultiBody = (clientCmd.m_updateFlags & URDF_ARGS_USE_MULTIBODY) ? (mjcfArgs.m_useMultiBody != 0) : true;
14289 int flags = CUF_USE_MJCF;
14290 if (clientCmd.m_updateFlags & URDF_ARGS_HAS_CUSTOM_URDF_FLAGS)
14291 {
14292 flags |= clientCmd.m_mjcfArguments.m_flags;
14293 }
14294
14295 bool completedOk = loadMjcf(mjcfArgs.m_mjcfFileName, bufferServerToClient, bufferSizeInBytes, useMultiBody, flags);
14296 if (completedOk)
14297 {
14298 m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld);
14299
14300 serverStatusOut.m_sdfLoadedArgs.m_numBodies = m_data->m_sdfRecentLoadedBodies.size();
14301 serverStatusOut.m_sdfLoadedArgs.m_numUserConstraints = 0;
14302 int maxBodies = btMin(MAX_SDF_BODIES, serverStatusOut.m_sdfLoadedArgs.m_numBodies);
14303 for (int i = 0; i < maxBodies; i++)
14304 {
14305 serverStatusOut.m_sdfLoadedArgs.m_bodyUniqueIds[i] = m_data->m_sdfRecentLoadedBodies[i];
14306 }
14307
14308 serverStatusOut.m_type = CMD_MJCF_LOADING_COMPLETED;
14309 }
14310 else
14311 {
14312 serverStatusOut.m_type = CMD_MJCF_LOADING_FAILED;
14313 }
14314 return hasStatus;
14315 }
14316
processSaveBulletCommand(const struct SharedMemoryCommand & clientCmd,struct SharedMemoryStatus & serverStatusOut,char * bufferServerToClient,int bufferSizeInBytes)14317 bool PhysicsServerCommandProcessor::processSaveBulletCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
14318 {
14319 bool hasStatus = true;
14320
14321 BT_PROFILE("CMD_SAVE_BULLET");
14322 SharedMemoryStatus& serverCmd = serverStatusOut;
14323
14324 FILE* f = fopen(clientCmd.m_fileArguments.m_fileName, "wb");
14325 if (f)
14326 {
14327 btDefaultSerializer* ser = new btDefaultSerializer();
14328 int currentFlags = ser->getSerializationFlags();
14329 ser->setSerializationFlags(currentFlags | BT_SERIALIZE_CONTACT_MANIFOLDS);
14330
14331 m_data->m_dynamicsWorld->serialize(ser);
14332 fwrite(ser->getBufferPointer(), ser->getCurrentBufferSize(), 1, f);
14333 fclose(f);
14334 serverCmd.m_type = CMD_BULLET_SAVING_COMPLETED;
14335 delete ser;
14336 return hasStatus;
14337 }
14338 serverCmd.m_type = CMD_BULLET_SAVING_FAILED;
14339 return hasStatus;
14340 }
14341
processCommand(const struct SharedMemoryCommand & clientCmd,struct SharedMemoryStatus & serverStatusOut,char * bufferServerToClient,int bufferSizeInBytes)14342 bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
14343 {
14344 // BT_PROFILE("processCommand");
14345
14346 int sz = sizeof(SharedMemoryStatus);
14347 int sz2 = sizeof(SharedMemoryCommand);
14348
14349 bool hasStatus = false;
14350
14351 if (m_data->m_commandLogger)
14352 {
14353 m_data->m_commandLogger->logCommand(clientCmd);
14354 }
14355 serverStatusOut.m_type = CMD_INVALID_STATUS;
14356 serverStatusOut.m_numDataStreamBytes = 0;
14357 serverStatusOut.m_dataStream = 0;
14358
14359 //consume the command
14360 switch (clientCmd.m_type)
14361 {
14362 case CMD_STATE_LOGGING:
14363 {
14364 hasStatus = processStateLoggingCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
14365 break;
14366 }
14367 case CMD_SET_VR_CAMERA_STATE:
14368 {
14369 hasStatus = processSetVRCameraStateCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
14370 break;
14371 }
14372 case CMD_REQUEST_VR_EVENTS_DATA:
14373 {
14374 hasStatus = processRequestVREventsCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
14375 break;
14376 };
14377 case CMD_REQUEST_MOUSE_EVENTS_DATA:
14378 {
14379 hasStatus = processRequestMouseEventsCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
14380 break;
14381 };
14382 case CMD_REQUEST_KEYBOARD_EVENTS_DATA:
14383 {
14384 hasStatus = processRequestKeyboardEventsCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
14385 break;
14386 };
14387
14388 case CMD_REQUEST_RAY_CAST_INTERSECTIONS:
14389 {
14390 hasStatus = processRequestRaycastIntersectionsCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
14391 break;
14392 };
14393 case CMD_REQUEST_DEBUG_LINES:
14394 {
14395 hasStatus = processRequestDebugLinesCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
14396 break;
14397 }
14398
14399 case CMD_REQUEST_CAMERA_IMAGE_DATA:
14400 {
14401 hasStatus = processRequestCameraImageCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
14402 break;
14403 }
14404 case CMD_SYNC_BODY_INFO:
14405 {
14406 hasStatus = processSyncBodyInfoCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
14407 break;
14408 }
14409 case CMD_REQUEST_BODY_INFO:
14410 {
14411 hasStatus = processRequestBodyInfoCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
14412 break;
14413 }
14414 case CMD_SAVE_WORLD:
14415 {
14416 hasStatus = processSaveWorldCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
14417 break;
14418 }
14419 case CMD_LOAD_SDF:
14420 {
14421 hasStatus = processLoadSDFCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
14422 break;
14423 }
14424 case CMD_CREATE_COLLISION_SHAPE:
14425 {
14426 hasStatus = processCreateCollisionShapeCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
14427 break;
14428 }
14429 case CMD_CREATE_VISUAL_SHAPE:
14430 {
14431 hasStatus = processCreateVisualShapeCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
14432 break;
14433 }
14434 case CMD_REQUEST_MESH_DATA:
14435 {
14436 hasStatus = processRequestMeshDataCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
14437 break;
14438 }
14439 case CMD_RESET_MESH_DATA:
14440 {
14441 hasStatus = processResetMeshDataCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
14442 break;
14443 }
14444
14445 case CMD_CREATE_MULTI_BODY:
14446 {
14447 hasStatus = processCreateMultiBodyCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
14448 break;
14449 }
14450 case CMD_SET_ADDITIONAL_SEARCH_PATH:
14451 {
14452 hasStatus = processSetAdditionalSearchPathCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
14453 break;
14454 }
14455 case CMD_LOAD_URDF:
14456 {
14457 hasStatus = processLoadURDFCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
14458 break;
14459 }
14460 case CMD_LOAD_SOFT_BODY:
14461 {
14462 hasStatus = processLoadSoftBodyCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
14463 break;
14464 }
14465 case CMD_CREATE_SENSOR:
14466 {
14467 hasStatus = processCreateSensorCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
14468 break;
14469 }
14470 case CMD_PROFILE_TIMING:
14471 {
14472 hasStatus = processProfileTimingCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
14473 break;
14474 }
14475
14476 case CMD_SEND_DESIRED_STATE:
14477 {
14478 hasStatus = processSendDesiredStateCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
14479 break;
14480 }
14481 case CMD_REQUEST_COLLISION_INFO:
14482 {
14483 hasStatus = processRequestCollisionInfoCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
14484 break;
14485 }
14486 case CMD_REQUEST_ACTUAL_STATE:
14487 {
14488 hasStatus = processRequestActualStateCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
14489 break;
14490 }
14491 case CMD_STEP_FORWARD_SIMULATION:
14492 {
14493 hasStatus = processForwardDynamicsCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
14494 break;
14495 }
14496 case CMD_PERFORM_COLLISION_DETECTION:
14497 {
14498 hasStatus = performCollisionDetectionCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
14499 break;
14500 }
14501
14502 case CMD_REQUEST_INTERNAL_DATA:
14503 {
14504 hasStatus = processRequestInternalDataCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
14505 break;
14506 };
14507 case CMD_CHANGE_DYNAMICS_INFO:
14508 {
14509 hasStatus = processChangeDynamicsInfoCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
14510 break;
14511 };
14512 case CMD_GET_DYNAMICS_INFO:
14513 {
14514 hasStatus = processGetDynamicsInfoCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
14515 break;
14516 }
14517
14518 case CMD_REQUEST_PHYSICS_SIMULATION_PARAMETERS:
14519 {
14520 hasStatus = processRequestPhysicsSimulationParametersCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
14521 break;
14522 }
14523
14524 case CMD_SEND_PHYSICS_SIMULATION_PARAMETERS:
14525 {
14526 hasStatus = processSendPhysicsParametersCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
14527 break;
14528 };
14529 case CMD_INIT_POSE:
14530 {
14531 hasStatus = processInitPoseCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
14532 break;
14533 }
14534 case CMD_RESET_SIMULATION:
14535 {
14536 hasStatus = processResetSimulationCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
14537 break;
14538 }
14539 case CMD_CREATE_RIGID_BODY:
14540 {
14541 hasStatus = processCreateRigidBodyCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
14542 break;
14543 }
14544 case CMD_CREATE_BOX_COLLISION_SHAPE:
14545 {
14546 //for backward compatibility, CMD_CREATE_BOX_COLLISION_SHAPE is the same as CMD_CREATE_RIGID_BODY
14547 hasStatus = processCreateRigidBodyCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
14548 break;
14549 }
14550 case CMD_PICK_BODY:
14551 {
14552 hasStatus = processPickBodyCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
14553 break;
14554 }
14555 case CMD_MOVE_PICKED_BODY:
14556 {
14557 hasStatus = processMovePickedBodyCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
14558 break;
14559 }
14560 case CMD_REMOVE_PICKING_CONSTRAINT_BODY:
14561 {
14562 hasStatus = processRemovePickingConstraintCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
14563 break;
14564 }
14565 case CMD_REQUEST_AABB_OVERLAP:
14566 {
14567 hasStatus = processRequestAabbOverlapCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
14568 break;
14569 }
14570 case CMD_REQUEST_OPENGL_VISUALIZER_CAMERA:
14571 {
14572 hasStatus = processRequestOpenGLVisualizeCameraCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
14573 break;
14574 }
14575 case CMD_CONFIGURE_OPENGL_VISUALIZER:
14576 {
14577 hasStatus = processConfigureOpenGLVisualizerCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
14578 break;
14579 }
14580 case CMD_REQUEST_CONTACT_POINT_INFORMATION:
14581 {
14582 hasStatus = processRequestContactpointInformationCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
14583 break;
14584 }
14585 case CMD_CALCULATE_INVERSE_DYNAMICS:
14586 {
14587 hasStatus = processInverseDynamicsCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
14588 break;
14589 }
14590 case CMD_CALCULATE_JACOBIAN:
14591 {
14592 hasStatus = processCalculateJacobianCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
14593 break;
14594 }
14595 case CMD_CALCULATE_MASS_MATRIX:
14596 {
14597 hasStatus = processCalculateMassMatrixCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
14598 break;
14599 }
14600 case CMD_APPLY_EXTERNAL_FORCE:
14601 {
14602 hasStatus = processApplyExternalForceCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
14603 break;
14604 }
14605 case CMD_REMOVE_BODY:
14606 {
14607 hasStatus = processRemoveBodyCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
14608 break;
14609 }
14610 case CMD_USER_CONSTRAINT:
14611 {
14612 hasStatus = processCreateUserConstraintCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
14613 break;
14614 }
14615 case CMD_CALCULATE_INVERSE_KINEMATICS:
14616 {
14617 if (clientCmd.m_calculateInverseKinematicsArguments.m_numEndEffectorLinkIndices == 1)
14618 {
14619 hasStatus = processCalculateInverseKinematicsCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
14620 }
14621 else
14622 {
14623 hasStatus = processCalculateInverseKinematicsCommand2(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
14624 }
14625 break;
14626 }
14627 case CMD_REQUEST_VISUAL_SHAPE_INFO:
14628 {
14629 hasStatus = processRequestVisualShapeInfoCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
14630 break;
14631 }
14632 case CMD_REQUEST_COLLISION_SHAPE_INFO:
14633 {
14634 hasStatus = processRequestCollisionShapeInfoCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
14635 break;
14636 }
14637 case CMD_UPDATE_VISUAL_SHAPE:
14638 {
14639 hasStatus = processUpdateVisualShapeCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
14640 break;
14641 }
14642 case CMD_CHANGE_TEXTURE:
14643 {
14644 hasStatus = processChangeTextureCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
14645 break;
14646 }
14647 case CMD_LOAD_TEXTURE:
14648 {
14649 hasStatus = processLoadTextureCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
14650 break;
14651 }
14652 case CMD_RESTORE_STATE:
14653 {
14654 hasStatus = processRestoreStateCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
14655 break;
14656 }
14657
14658 case CMD_SAVE_STATE:
14659 {
14660 hasStatus = processSaveStateCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
14661 break;
14662 }
14663 case CMD_REMOVE_STATE:
14664 {
14665 hasStatus = processRemoveStateCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
14666 break;
14667 }
14668
14669 case CMD_LOAD_BULLET:
14670 {
14671 hasStatus = processLoadBulletCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
14672 break;
14673 }
14674 case CMD_SAVE_BULLET:
14675 {
14676 hasStatus = processSaveBulletCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
14677 break;
14678 }
14679 case CMD_LOAD_MJCF:
14680 {
14681 hasStatus = processLoadMJCFCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
14682 break;
14683 }
14684 case CMD_USER_DEBUG_DRAW:
14685 {
14686 hasStatus = processUserDebugDrawCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
14687 break;
14688 }
14689 case CMD_CUSTOM_COMMAND:
14690 {
14691 hasStatus = processCustomCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
14692 break;
14693 }
14694 case CMD_SYNC_USER_DATA:
14695 {
14696 hasStatus = processSyncUserDataCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
14697 break;
14698 }
14699 case CMD_REQUEST_USER_DATA:
14700 {
14701 hasStatus = processRequestUserDataCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
14702 break;
14703 }
14704 case CMD_ADD_USER_DATA:
14705 {
14706 hasStatus = processAddUserDataCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
14707 break;
14708 }
14709 case CMD_REMOVE_USER_DATA:
14710 {
14711 hasStatus = processRemoveUserDataCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
14712 break;
14713 }
14714 case CMD_COLLISION_FILTER:
14715 {
14716 hasStatus = processCollisionFilterCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
14717 break;
14718 }
14719 default:
14720 {
14721 BT_PROFILE("CMD_UNKNOWN");
14722 b3Error("Unknown command encountered");
14723 SharedMemoryStatus& serverCmd = serverStatusOut;
14724 serverCmd.m_type = CMD_UNKNOWN_COMMAND_FLUSHED;
14725 hasStatus = true;
14726 }
14727 };
14728
14729 return hasStatus;
14730 }
14731
syncPhysicsToGraphics()14732 void PhysicsServerCommandProcessor::syncPhysicsToGraphics()
14733 {
14734 m_data->m_guiHelper->syncPhysicsToGraphics(m_data->m_dynamicsWorld);
14735 }
14736
syncPhysicsToGraphics2()14737 void PhysicsServerCommandProcessor::syncPhysicsToGraphics2()
14738 {
14739 m_data->m_guiHelper->syncPhysicsToGraphics2(m_data->m_dynamicsWorld);
14740 }
14741
renderScene(int renderFlags)14742 void PhysicsServerCommandProcessor::renderScene(int renderFlags)
14743 {
14744 if (m_data->m_guiHelper)
14745 {
14746 if (0 == (renderFlags & COV_DISABLE_SYNC_RENDERING))
14747 {
14748 m_data->m_guiHelper->syncPhysicsToGraphics(m_data->m_dynamicsWorld);
14749 }
14750
14751 m_data->m_guiHelper->render(m_data->m_dynamicsWorld);
14752 }
14753 }
14754
physicsDebugDraw(int debugDrawFlags)14755 void PhysicsServerCommandProcessor::physicsDebugDraw(int debugDrawFlags)
14756 {
14757 if (m_data->m_dynamicsWorld)
14758 {
14759 if (m_data->m_dynamicsWorld->getDebugDrawer())
14760 {
14761 m_data->m_dynamicsWorld->getDebugDrawer()->setDebugMode(debugDrawFlags);
14762 m_data->m_dynamicsWorld->debugDrawWorld();
14763
14764 #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
14765 {
14766 btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld();
14767 if (deformWorld)
14768 {
14769 for (int i = 0; i < deformWorld->getSoftBodyArray().size(); i++)
14770 {
14771 btSoftBody* psb = (btSoftBody*)deformWorld->getSoftBodyArray()[i];
14772 if (m_data->m_dynamicsWorld->getDebugDrawer() && !(m_data->m_dynamicsWorld->getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe)))
14773 {
14774 //btSoftBodyHelpers::DrawFrame(psb,m_data->m_dynamicsWorld->getDebugDrawer());
14775 btSoftBodyHelpers::Draw(psb, m_data->m_dynamicsWorld->getDebugDrawer(), deformWorld->getDrawFlags());
14776 }
14777 }
14778 }
14779 }
14780 {
14781 btSoftMultiBodyDynamicsWorld* softWorld = getSoftWorld();
14782 if (softWorld)
14783 {
14784 for (int i = 0; i < softWorld->getSoftBodyArray().size(); i++)
14785 {
14786 btSoftBody* psb = (btSoftBody*)softWorld->getSoftBodyArray()[i];
14787 if (m_data->m_dynamicsWorld->getDebugDrawer() && !(m_data->m_dynamicsWorld->getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe)))
14788 {
14789 //btSoftBodyHelpers::DrawFrame(psb,m_data->m_dynamicsWorld->getDebugDrawer());
14790 btSoftBodyHelpers::Draw(psb, m_data->m_dynamicsWorld->getDebugDrawer(), softWorld->getDrawFlags());
14791 }
14792 }
14793 }
14794 }
14795 #endif
14796 }
14797 }
14798 }
14799
14800 struct MyResultCallback : public btCollisionWorld::ClosestRayResultCallback
14801 {
14802 int m_faceId;
14803
MyResultCallbackMyResultCallback14804 MyResultCallback(const btVector3& rayFromWorld, const btVector3& rayToWorld)
14805 : btCollisionWorld::ClosestRayResultCallback(rayFromWorld, rayToWorld),
14806 m_faceId(-1)
14807 {
14808 }
14809
needsCollisionMyResultCallback14810 virtual bool needsCollision(btBroadphaseProxy* proxy0) const
14811 {
14812 return true;
14813 }
14814
addSingleResultMyResultCallback14815 virtual btScalar addSingleResult(btCollisionWorld::LocalRayResult& rayResult, bool normalInWorldSpace)
14816 {
14817 //caller already does the filter on the m_closestHitFraction
14818 btAssert(rayResult.m_hitFraction <= m_closestHitFraction);
14819
14820 m_closestHitFraction = rayResult.m_hitFraction;
14821 m_collisionObject = rayResult.m_collisionObject;
14822 if (rayResult.m_localShapeInfo)
14823 {
14824 m_faceId = rayResult.m_localShapeInfo->m_triangleIndex;
14825 }
14826 else
14827 {
14828 m_faceId = -1;
14829 }
14830 if (normalInWorldSpace)
14831 {
14832 m_hitNormalWorld = rayResult.m_hitNormalLocal;
14833 }
14834 else
14835 {
14836 ///need to transform normal into worldspace
14837 m_hitNormalWorld = m_collisionObject->getWorldTransform().getBasis() * rayResult.m_hitNormalLocal;
14838 }
14839 m_hitPointWorld.setInterpolate3(m_rayFromWorld, m_rayToWorld, rayResult.m_hitFraction);
14840 return rayResult.m_hitFraction;
14841 }
14842 };
14843
pickBody(const btVector3 & rayFromWorld,const btVector3 & rayToWorld)14844 bool PhysicsServerCommandProcessor::pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld)
14845 {
14846 if (m_data->m_dynamicsWorld == 0)
14847 return false;
14848
14849 //btCollisionWorld::ClosestRayResultCallback rayCallback(rayFromWorld, rayToWorld);
14850 MyResultCallback rayCallback(rayFromWorld, rayToWorld);
14851 rayCallback.m_flags |= btTriangleRaycastCallback::kF_UseGjkConvexCastRaytest;
14852 m_data->m_dynamicsWorld->rayTest(rayFromWorld, rayToWorld, rayCallback);
14853 if (rayCallback.hasHit())
14854 {
14855 btVector3 pickPos = rayCallback.m_hitPointWorld;
14856
14857 btRigidBody* body = (btRigidBody*)btRigidBody::upcast(rayCallback.m_collisionObject);
14858 if (body)
14859 {
14860 //other exclusions?
14861 if (!(body->isStaticObject() || body->isKinematicObject()))
14862 {
14863 m_data->m_pickedBody = body;
14864 m_data->m_savedActivationState = body->getActivationState();
14865 if (m_data->m_savedActivationState==ISLAND_SLEEPING)
14866 {
14867 m_data->m_savedActivationState = ACTIVE_TAG;
14868 }
14869 m_data->m_pickedBody->setActivationState(DISABLE_DEACTIVATION);
14870 m_data->m_pickedBody->setDeactivationTime(0);
14871 //printf("pickPos=%f,%f,%f\n",pickPos.getX(),pickPos.getY(),pickPos.getZ());
14872 btVector3 localPivot = body->getCenterOfMassTransform().inverse() * pickPos;
14873 btPoint2PointConstraint* p2p = new btPoint2PointConstraint(*body, localPivot);
14874 m_data->m_dynamicsWorld->addConstraint(p2p, true);
14875 m_data->m_pickedConstraint = p2p;
14876 btScalar mousePickClamping = 30.f;
14877 p2p->m_setting.m_impulseClamp = mousePickClamping;
14878 //very weak constraint for picking
14879 p2p->m_setting.m_tau = 0.001f;
14880 }
14881 }
14882 else
14883 {
14884 btMultiBodyLinkCollider* multiCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(rayCallback.m_collisionObject);
14885 if (multiCol && multiCol->m_multiBody)
14886 {
14887 m_data->m_prevCanSleep = multiCol->m_multiBody->getCanSleep();
14888 multiCol->m_multiBody->setCanSleep(false);
14889
14890 btVector3 pivotInA = multiCol->m_multiBody->worldPosToLocal(multiCol->m_link, pickPos);
14891
14892 btMultiBodyPoint2Point* p2p = new btMultiBodyPoint2Point(multiCol->m_multiBody, multiCol->m_link, 0, pivotInA, pickPos);
14893 //if you add too much energy to the system, causing high angular velocities, simulation 'explodes'
14894 //see also http://www.bulletphysics.org/Bullet/phpBB3/viewtopic.php?f=4&t=949
14895 //so we try to avoid it by clamping the maximum impulse (force) that the mouse pick can apply
14896 //it is not satisfying, hopefully we find a better solution (higher order integrator, using joint friction using a zero-velocity target motor with limited force etc?)
14897 btScalar scaling = 10;
14898 p2p->setMaxAppliedImpulse(2 * scaling);
14899
14900 btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*)m_data->m_dynamicsWorld;
14901 world->addMultiBodyConstraint(p2p);
14902 m_data->m_pickingMultiBodyPoint2Point = p2p;
14903 }
14904 else
14905 {
14906 #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
14907 //deformable/soft body?
14908 btSoftBody* psb = (btSoftBody*)btSoftBody::upcast(rayCallback.m_collisionObject);
14909 if (psb)
14910 {
14911 btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld();
14912 if (deformWorld)
14913 {
14914 int face_id = rayCallback.m_faceId;
14915 if (face_id >= 0 && face_id < psb->m_faces.size())
14916 {
14917 m_data->m_pickedSoftBody = psb;
14918 psb->setActivationState(DISABLE_DEACTIVATION);
14919 const btSoftBody::Face& f = psb->m_faces[face_id];
14920 btDeformableMousePickingForce* mouse_force = new btDeformableMousePickingForce(100, 0, f, pickPos, m_data->m_maxPickingForce);
14921 m_data->m_mouseForce = mouse_force;
14922
14923 deformWorld->addForce(psb, mouse_force);
14924 }
14925 }
14926 }
14927 #endif
14928 }
14929 }
14930
14931 // pickObject(pickPos, rayCallback.m_collisionObject);
14932 m_data->m_oldPickingPos = rayToWorld;
14933 m_data->m_hitPos = pickPos;
14934 m_data->m_oldPickingDist = (pickPos - rayFromWorld).length();
14935 // printf("hit !\n");
14936 //add p2p
14937 }
14938 return false;
14939 }
14940
movePickedBody(const btVector3 & rayFromWorld,const btVector3 & rayToWorld)14941 bool PhysicsServerCommandProcessor::movePickedBody(const btVector3& rayFromWorld, const btVector3& rayToWorld)
14942 {
14943 if (m_data->m_pickedBody && m_data->m_pickedConstraint)
14944 {
14945 btPoint2PointConstraint* pickCon = static_cast<btPoint2PointConstraint*>(m_data->m_pickedConstraint);
14946 if (pickCon)
14947 {
14948 //keep it at the same picking distance
14949
14950 btVector3 dir = rayToWorld - rayFromWorld;
14951 dir.normalize();
14952 dir *= m_data->m_oldPickingDist;
14953
14954 btVector3 newPivotB = rayFromWorld + dir;
14955 pickCon->setPivotB(newPivotB);
14956 }
14957 }
14958
14959 if (m_data->m_pickingMultiBodyPoint2Point)
14960 {
14961 //keep it at the same picking distance
14962
14963 btVector3 dir = rayToWorld - rayFromWorld;
14964 dir.normalize();
14965 dir *= m_data->m_oldPickingDist;
14966
14967 btVector3 newPivotB = rayFromWorld + dir;
14968
14969 m_data->m_pickingMultiBodyPoint2Point->setPivotInB(newPivotB);
14970 }
14971
14972 #ifndef SKIP_DEFORMABLE_BODY
14973 if (m_data->m_pickedSoftBody)
14974 {
14975 if (m_data->m_pickedSoftBody && m_data->m_mouseForce)
14976 {
14977 btVector3 newPivot;
14978 btVector3 dir = rayToWorld - rayFromWorld;
14979 dir.normalize();
14980 dir *= m_data->m_oldPickingDist;
14981 newPivot = rayFromWorld + dir;
14982 m_data->m_mouseForce->setMousePos(newPivot);
14983 }
14984 }
14985 #endif
14986
14987 return false;
14988 }
14989
removePickingConstraint()14990 void PhysicsServerCommandProcessor::removePickingConstraint()
14991 {
14992 if (m_data->m_pickedConstraint)
14993 {
14994 m_data->m_dynamicsWorld->removeConstraint(m_data->m_pickedConstraint);
14995 delete m_data->m_pickedConstraint;
14996 m_data->m_pickedConstraint = 0;
14997 m_data->m_pickedBody->forceActivationState(m_data->m_savedActivationState);
14998 m_data->m_pickedBody = 0;
14999 }
15000 if (m_data->m_pickingMultiBodyPoint2Point)
15001 {
15002 m_data->m_pickingMultiBodyPoint2Point->getMultiBodyA()->setCanSleep(m_data->m_prevCanSleep);
15003 btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*)m_data->m_dynamicsWorld;
15004 world->removeMultiBodyConstraint(m_data->m_pickingMultiBodyPoint2Point);
15005 delete m_data->m_pickingMultiBodyPoint2Point;
15006 m_data->m_pickingMultiBodyPoint2Point = 0;
15007 }
15008
15009 #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
15010 //deformable/soft body?
15011 btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld();
15012 if (deformWorld && m_data->m_mouseForce)
15013 {
15014 deformWorld->removeForce(m_data->m_pickedSoftBody, m_data->m_mouseForce);
15015 delete m_data->m_mouseForce;
15016 m_data->m_mouseForce = 0;
15017 m_data->m_pickedSoftBody = 0;
15018 }
15019 #endif
15020 }
15021
enableCommandLogging(bool enable,const char * fileName)15022 void PhysicsServerCommandProcessor::enableCommandLogging(bool enable, const char* fileName)
15023 {
15024 if (enable)
15025 {
15026 if (0 == m_data->m_commandLogger)
15027 {
15028 m_data->m_commandLogger = new CommandLogger(fileName);
15029 }
15030 }
15031 else
15032 {
15033 if (0 != m_data->m_commandLogger)
15034 {
15035 delete m_data->m_commandLogger;
15036 m_data->m_commandLogger = 0;
15037 }
15038 }
15039 }
15040
replayFromLogFile(const char * fileName)15041 void PhysicsServerCommandProcessor::replayFromLogFile(const char* fileName)
15042 {
15043 CommandLogPlayback* pb = new CommandLogPlayback(fileName);
15044 m_data->m_logPlayback = pb;
15045 }
15046
15047 int gDroppedSimulationSteps = 0;
15048 int gNumSteps = 0;
15049 double gDtInSec = 0.f;
15050 double gSubStep = 0.f;
15051
enableRealTimeSimulation(bool enableRealTimeSim)15052 void PhysicsServerCommandProcessor::enableRealTimeSimulation(bool enableRealTimeSim)
15053 {
15054 m_data->m_useRealTimeSimulation = enableRealTimeSim;
15055 }
15056
isRealTimeSimulationEnabled() const15057 bool PhysicsServerCommandProcessor::isRealTimeSimulationEnabled() const
15058 {
15059 return m_data->m_useRealTimeSimulation;
15060 }
15061
stepSimulationRealTime(double dtInSec,const struct b3VRControllerEvent * vrControllerEvents,int numVRControllerEvents,const struct b3KeyboardEvent * keyEvents,int numKeyEvents,const struct b3MouseEvent * mouseEvents,int numMouseEvents)15062 void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec, const struct b3VRControllerEvent* vrControllerEvents, int numVRControllerEvents, const struct b3KeyboardEvent* keyEvents, int numKeyEvents, const struct b3MouseEvent* mouseEvents, int numMouseEvents)
15063 {
15064 m_data->m_vrControllerEvents.addNewVREvents(vrControllerEvents, numVRControllerEvents);
15065 m_data->m_pluginManager.addEvents(vrControllerEvents, numVRControllerEvents, keyEvents, numKeyEvents, mouseEvents, numMouseEvents);
15066
15067 for (int i = 0; i < m_data->m_stateLoggers.size(); i++)
15068 {
15069 if (m_data->m_stateLoggers[i]->m_loggingType == STATE_LOGGING_VR_CONTROLLERS)
15070 {
15071 VRControllerStateLogger* vrLogger = (VRControllerStateLogger*)m_data->m_stateLoggers[i];
15072 vrLogger->m_vrEvents.addNewVREvents(vrControllerEvents, numVRControllerEvents);
15073 }
15074 }
15075
15076 for (int ii = 0; ii < numMouseEvents; ii++)
15077 {
15078 const b3MouseEvent& event = mouseEvents[ii];
15079 bool found = false;
15080 //search a matching one first, otherwise add new event
15081 for (int e = 0; e < m_data->m_mouseEvents.size(); e++)
15082 {
15083 if (event.m_eventType == m_data->m_mouseEvents[e].m_eventType)
15084 {
15085 if (event.m_eventType == MOUSE_MOVE_EVENT)
15086 {
15087 m_data->m_mouseEvents[e].m_mousePosX = event.m_mousePosX;
15088 m_data->m_mouseEvents[e].m_mousePosY = event.m_mousePosY;
15089 found = true;
15090 }
15091 else if ((event.m_eventType == MOUSE_BUTTON_EVENT) && event.m_buttonIndex == m_data->m_mouseEvents[e].m_buttonIndex)
15092 {
15093 m_data->m_mouseEvents[e].m_buttonState |= event.m_buttonState;
15094 if (event.m_buttonState & eButtonIsDown)
15095 {
15096 m_data->m_mouseEvents[e].m_buttonState |= eButtonIsDown;
15097 }
15098 else
15099 {
15100 m_data->m_mouseEvents[e].m_buttonState &= ~eButtonIsDown;
15101 }
15102 found = true;
15103 }
15104 }
15105 }
15106 if (!found)
15107 {
15108 m_data->m_mouseEvents.push_back(event);
15109 }
15110 }
15111
15112 for (int i = 0; i < numKeyEvents; i++)
15113 {
15114 const b3KeyboardEvent& event = keyEvents[i];
15115 bool found = false;
15116 //search a matching one first, otherwise add new event
15117 for (int e = 0; e < m_data->m_keyboardEvents.size(); e++)
15118 {
15119 if (event.m_keyCode == m_data->m_keyboardEvents[e].m_keyCode)
15120 {
15121 m_data->m_keyboardEvents[e].m_keyState |= event.m_keyState;
15122 if (event.m_keyState & eButtonIsDown)
15123 {
15124 m_data->m_keyboardEvents[e].m_keyState |= eButtonIsDown;
15125 }
15126 else
15127 {
15128 m_data->m_keyboardEvents[e].m_keyState &= ~eButtonIsDown;
15129 }
15130 found = true;
15131 }
15132 }
15133 if (!found)
15134 {
15135 m_data->m_keyboardEvents.push_back(event);
15136 }
15137 }
15138 if (gResetSimulation)
15139 {
15140 resetSimulation();
15141 gResetSimulation = false;
15142 }
15143
15144 if (gVRTrackingObjectUniqueId >= 0)
15145 {
15146 InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(gVRTrackingObjectUniqueId);
15147 if (bodyHandle && bodyHandle->m_multiBody)
15148 {
15149 // gVRTrackingObjectTr = bodyHandle->m_multiBody->getBaseWorldTransform();
15150
15151 if (gVRTrackingObjectUniqueId >= 0)
15152 {
15153 gVRTrackingObjectTr.setOrigin(bodyHandle->m_multiBody->getBaseWorldTransform().getOrigin());
15154 gVRTeleportPos1 = gVRTrackingObjectTr.getOrigin();
15155 }
15156 if (gVRTrackingObjectFlag & VR_CAMERA_TRACK_OBJECT_ORIENTATION)
15157 {
15158 gVRTrackingObjectTr.setBasis(bodyHandle->m_multiBody->getBaseWorldTransform().getBasis());
15159 gVRTeleportOrn = gVRTrackingObjectTr.getRotation();
15160 }
15161 }
15162 }
15163
15164 if ((m_data->m_useRealTimeSimulation) && m_data->m_guiHelper)
15165 {
15166 int maxSteps = m_data->m_numSimulationSubSteps + 3;
15167 if (m_data->m_numSimulationSubSteps)
15168 {
15169 gSubStep = m_data->m_physicsDeltaTime / m_data->m_numSimulationSubSteps;
15170 }
15171 else
15172 {
15173 gSubStep = m_data->m_physicsDeltaTime;
15174 }
15175
15176 btScalar deltaTimeScaled = dtInSec * simTimeScalingFactor;
15177 int numSteps = m_data->m_dynamicsWorld->stepSimulation(deltaTimeScaled, maxSteps, gSubStep);
15178 m_data->m_simulationTimestamp += deltaTimeScaled;
15179 gDroppedSimulationSteps += numSteps > maxSteps ? numSteps - maxSteps : 0;
15180
15181 if (numSteps)
15182 {
15183 gNumSteps = numSteps;
15184 gDtInSec = dtInSec;
15185
15186 addBodyChangedNotifications();
15187 }
15188 }
15189 }
15190
createTransformChangedNotification(int bodyUniqueId,int linkIndex,const btCollisionObject * colObj)15191 b3Notification createTransformChangedNotification(int bodyUniqueId, int linkIndex, const btCollisionObject* colObj)
15192 {
15193 b3Notification notification;
15194 notification.m_notificationType = TRANSFORM_CHANGED;
15195 notification.m_transformChangeArgs.m_bodyUniqueId = bodyUniqueId;
15196 notification.m_transformChangeArgs.m_linkIndex = linkIndex;
15197
15198 const btTransform& tr = colObj->getWorldTransform();
15199 notification.m_transformChangeArgs.m_worldPosition[0] = tr.getOrigin()[0];
15200 notification.m_transformChangeArgs.m_worldPosition[1] = tr.getOrigin()[1];
15201 notification.m_transformChangeArgs.m_worldPosition[2] = tr.getOrigin()[2];
15202
15203 notification.m_transformChangeArgs.m_worldRotation[0] = tr.getRotation()[0];
15204 notification.m_transformChangeArgs.m_worldRotation[1] = tr.getRotation()[1];
15205 notification.m_transformChangeArgs.m_worldRotation[2] = tr.getRotation()[2];
15206 notification.m_transformChangeArgs.m_worldRotation[3] = tr.getRotation()[3];
15207
15208 const btVector3& scaling = colObj->getCollisionShape()->getLocalScaling();
15209 notification.m_transformChangeArgs.m_localScaling[0] = scaling[0];
15210 notification.m_transformChangeArgs.m_localScaling[1] = scaling[1];
15211 notification.m_transformChangeArgs.m_localScaling[2] = scaling[2];
15212 return notification;
15213 }
15214
15215 #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
createSoftBodyChangedNotification(int bodyUniqueId,int linkIndex)15216 b3Notification createSoftBodyChangedNotification(int bodyUniqueId, int linkIndex)
15217 {
15218 b3Notification notification;
15219 notification.m_notificationType = SOFTBODY_CHANGED;
15220 notification.m_softBodyChangeArgs.m_bodyUniqueId = bodyUniqueId;
15221 notification.m_softBodyChangeArgs.m_linkIndex = linkIndex;
15222 return notification;
15223 }
15224 #endif
15225
addBodyChangedNotifications()15226 void PhysicsServerCommandProcessor::addBodyChangedNotifications()
15227 {
15228 b3Notification notification;
15229 notification.m_notificationType = SIMULATION_STEPPED;
15230 m_data->m_pluginManager.addNotification(notification);
15231
15232 b3AlignedObjectArray<int> usedHandles;
15233 m_data->m_bodyHandles.getUsedHandles(usedHandles);
15234 for (int i = 0; i < usedHandles.size(); i++)
15235 {
15236 int bodyUniqueId = usedHandles[i];
15237 InternalBodyData* bodyData = m_data->m_bodyHandles.getHandle(bodyUniqueId);
15238 if (!bodyData)
15239 {
15240 continue;
15241 }
15242 if (bodyData->m_multiBody)
15243 {
15244 btMultiBody* mb = bodyData->m_multiBody;
15245 if (mb->getBaseCollider()->isActive())
15246 {
15247 m_data->m_pluginManager.addNotification(createTransformChangedNotification(bodyUniqueId, -1, mb->getBaseCollider()));
15248 }
15249 for (int linkIndex = 0; linkIndex < mb->getNumLinks(); linkIndex++)
15250 {
15251 if (mb->getLinkCollider(linkIndex)->isActive())
15252 {
15253 m_data->m_pluginManager.addNotification(createTransformChangedNotification(bodyUniqueId, linkIndex, mb->getLinkCollider(linkIndex)));
15254 }
15255 }
15256 }
15257 else if (bodyData->m_rigidBody && bodyData->m_rigidBody->isActive())
15258 {
15259 m_data->m_pluginManager.addNotification(createTransformChangedNotification(bodyUniqueId, -1, bodyData->m_rigidBody));
15260 }
15261 #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
15262 else if (bodyData->m_softBody)
15263 {
15264 int linkIndex = -1;
15265 m_data->m_pluginManager.addNotification(createSoftBodyChangedNotification(bodyUniqueId, linkIndex));
15266 }
15267 #endif
15268 }
15269 }
15270
resetSimulation(int flags)15271 void PhysicsServerCommandProcessor::resetSimulation(int flags)
15272 {
15273 //clean up all data
15274 m_data->m_remoteSyncTransformTime = m_data->m_remoteSyncTransformInterval;
15275
15276 m_data->m_simulationTimestamp = 0;
15277 m_data->m_cachedVUrdfisualShapes.clear();
15278
15279 #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
15280 if (m_data && m_data->m_dynamicsWorld)
15281 {
15282 {
15283 btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld();
15284 if (deformWorld)
15285 {
15286 deformWorld->getWorldInfo().m_sparsesdf.Reset();
15287 }
15288 }
15289 {
15290 btSoftMultiBodyDynamicsWorld* softWorld = getSoftWorld();
15291 if (softWorld)
15292 {
15293 softWorld->getWorldInfo().m_sparsesdf.Reset();
15294 }
15295 }
15296 }
15297 #endif
15298 if (m_data && m_data->m_guiHelper)
15299 {
15300 m_data->m_guiHelper->removeAllGraphicsInstances();
15301 m_data->m_guiHelper->removeAllUserDebugItems();
15302 }
15303 if (m_data)
15304 {
15305 if (m_data->m_pluginManager.getRenderInterface())
15306 {
15307 m_data->m_pluginManager.getRenderInterface()->resetAll();
15308 }
15309
15310 if (m_data->m_pluginManager.getCollisionInterface())
15311 {
15312 m_data->m_pluginManager.getCollisionInterface()->resetAll();
15313 }
15314
15315 for (int i = 0; i < m_data->m_savedStates.size(); i++)
15316 {
15317 delete m_data->m_savedStates[i].m_bulletFile;
15318 delete m_data->m_savedStates[i].m_serializer;
15319 }
15320 m_data->m_savedStates.clear();
15321 }
15322
15323 removePickingConstraint();
15324
15325 deleteDynamicsWorld();
15326 createEmptyDynamicsWorld(flags);
15327
15328 m_data->m_bodyHandles.exitHandles();
15329 m_data->m_bodyHandles.initHandles();
15330
15331 m_data->m_userCollisionShapeHandles.exitHandles();
15332 m_data->m_userCollisionShapeHandles.initHandles();
15333
15334 m_data->m_userDataHandles.exitHandles();
15335 m_data->m_userDataHandles.initHandles();
15336 m_data->m_userDataHandleLookup.clear();
15337
15338 b3Notification notification;
15339 notification.m_notificationType = SIMULATION_RESET;
15340 m_data->m_pluginManager.addNotification(notification);
15341
15342 syncPhysicsToGraphics2();
15343 }
15344
setTimeOut(double)15345 void PhysicsServerCommandProcessor::setTimeOut(double /*timeOutInSeconds*/)
15346 {
15347 }
15348
getVRTeleportPosition() const15349 const btVector3& PhysicsServerCommandProcessor::getVRTeleportPosition() const
15350 {
15351 return gVRTeleportPos1;
15352 }
setVRTeleportPosition(const btVector3 & vrTeleportPos)15353 void PhysicsServerCommandProcessor::setVRTeleportPosition(const btVector3& vrTeleportPos)
15354 {
15355 gVRTeleportPos1 = vrTeleportPos;
15356 }
getVRTeleportOrientation() const15357 const btQuaternion& PhysicsServerCommandProcessor::getVRTeleportOrientation() const
15358 {
15359 return gVRTeleportOrn;
15360 }
setVRTeleportOrientation(const btQuaternion & vrTeleportOrn)15361 void PhysicsServerCommandProcessor::setVRTeleportOrientation(const btQuaternion& vrTeleportOrn)
15362 {
15363 gVRTeleportOrn = vrTeleportOrn;
15364 }
15365