1 #include "PhysicsClientC_API.h"
2 #include "PhysicsClientSharedMemory.h"
3 #include "Bullet3Common/b3Scalar.h"
4 #include "Bullet3Common/b3Vector3.h"
5 #include "Bullet3Common/b3Matrix3x3.h"
6 #include "Bullet3Common/b3Transform.h"
7 #include "Bullet3Common/b3TransformUtil.h"
8
9 #include <string.h>
10 #include "SharedMemoryCommands.h"
11
b3LoadSdfCommandInit(b3PhysicsClientHandle physClient,const char * sdfFileName)12 B3_SHARED_API b3SharedMemoryCommandHandle b3LoadSdfCommandInit(b3PhysicsClientHandle physClient, const char* sdfFileName)
13 {
14 PhysicsClient* cl = (PhysicsClient*)physClient;
15 b3Assert(cl);
16 b3Assert(cl->canSubmitCommand());
17
18 struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
19 b3Assert(command);
20
21 return b3LoadSdfCommandInit2((b3SharedMemoryCommandHandle)command, sdfFileName);
22 }
23
b3LoadSdfCommandInit2(b3SharedMemoryCommandHandle commandHandle,const char * sdfFileName)24 B3_SHARED_API b3SharedMemoryCommandHandle b3LoadSdfCommandInit2(b3SharedMemoryCommandHandle commandHandle, const char* sdfFileName)
25 {
26 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
27 command->m_type = CMD_LOAD_SDF;
28 int len = strlen(sdfFileName);
29 if (len < MAX_SDF_FILENAME_LENGTH)
30 {
31 strcpy(command->m_sdfArguments.m_sdfFileName, sdfFileName);
32 }
33 else
34 {
35 command->m_sdfArguments.m_sdfFileName[0] = 0;
36 }
37 command->m_updateFlags = SDF_ARGS_FILE_NAME;
38
39 return (b3SharedMemoryCommandHandle)command;
40 }
41
b3SaveWorldCommandInit(b3PhysicsClientHandle physClient,const char * sdfFileName)42 B3_SHARED_API b3SharedMemoryCommandHandle b3SaveWorldCommandInit(b3PhysicsClientHandle physClient, const char* sdfFileName)
43 {
44 PhysicsClient* cl = (PhysicsClient*)physClient;
45 b3Assert(cl);
46 b3Assert(cl->canSubmitCommand());
47
48 struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
49 b3Assert(command);
50 command->m_type = CMD_SAVE_WORLD;
51 int len = strlen(sdfFileName);
52 if (len < MAX_SDF_FILENAME_LENGTH)
53 {
54 strcpy(command->m_sdfArguments.m_sdfFileName, sdfFileName);
55 }
56 else
57 {
58 command->m_sdfArguments.m_sdfFileName[0] = 0;
59 }
60 command->m_updateFlags = SDF_ARGS_FILE_NAME;
61
62 return (b3SharedMemoryCommandHandle)command;
63 }
64
b3LoadBulletCommandInit(b3PhysicsClientHandle physClient,const char * fileName)65 B3_SHARED_API b3SharedMemoryCommandHandle b3LoadBulletCommandInit(b3PhysicsClientHandle physClient, const char* fileName)
66 {
67 PhysicsClient* cl = (PhysicsClient*)physClient;
68 b3Assert(cl);
69 b3Assert(cl->canSubmitCommand());
70
71 if (cl->canSubmitCommand())
72 {
73 struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
74 b3Assert(command);
75 command->m_type = CMD_LOAD_BULLET;
76 int len = strlen(fileName);
77 if (len < MAX_URDF_FILENAME_LENGTH)
78 {
79 strcpy(command->m_fileArguments.m_fileName, fileName);
80 }
81 else
82 {
83 command->m_fileArguments.m_fileName[0] = 0;
84 }
85 command->m_updateFlags = 0;
86
87 return (b3SharedMemoryCommandHandle)command;
88 }
89 return 0;
90 }
91
b3LoadStateCommandInit(b3PhysicsClientHandle physClient)92 B3_SHARED_API b3SharedMemoryCommandHandle b3LoadStateCommandInit(b3PhysicsClientHandle physClient)
93 {
94 PhysicsClient* cl = (PhysicsClient*)physClient;
95 b3Assert(cl);
96 b3Assert(cl->canSubmitCommand());
97
98 if (cl->canSubmitCommand())
99 {
100 struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
101 b3Assert(command);
102 command->m_type = CMD_RESTORE_STATE;
103 command->m_updateFlags = 0;
104 command->m_loadStateArguments.m_fileName[0] = 0;
105 command->m_loadStateArguments.m_stateId = -1;
106 return (b3SharedMemoryCommandHandle)command;
107 }
108 return 0;
109 }
110
b3InitRemoveStateCommand(b3PhysicsClientHandle physClient,int stateId)111 B3_SHARED_API b3SharedMemoryCommandHandle b3InitRemoveStateCommand(b3PhysicsClientHandle physClient, int stateId)
112 {
113 PhysicsClient* cl = (PhysicsClient*)physClient;
114 b3Assert(cl);
115 b3Assert(cl->canSubmitCommand());
116
117 if (cl->canSubmitCommand())
118 {
119 struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
120 b3Assert(command);
121 command->m_type = CMD_REMOVE_STATE;
122 command->m_updateFlags = 0;
123 command->m_loadStateArguments.m_fileName[0] = 0;
124 command->m_loadStateArguments.m_stateId = stateId;
125 return (b3SharedMemoryCommandHandle)command;
126 }
127 return 0;
128 }
129
b3LoadStateSetStateId(b3SharedMemoryCommandHandle commandHandle,int stateId)130 B3_SHARED_API int b3LoadStateSetStateId(b3SharedMemoryCommandHandle commandHandle, int stateId)
131 {
132 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
133 b3Assert(command->m_type == CMD_RESTORE_STATE);
134 if (command->m_type == CMD_RESTORE_STATE)
135 {
136 command->m_loadStateArguments.m_stateId = stateId;
137 command->m_updateFlags |= CMD_LOAD_STATE_HAS_STATEID;
138 }
139 return 0;
140 }
141
b3LoadStateSetFileName(b3SharedMemoryCommandHandle commandHandle,const char * fileName)142 B3_SHARED_API int b3LoadStateSetFileName(b3SharedMemoryCommandHandle commandHandle, const char* fileName)
143 {
144 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
145 b3Assert(command->m_type == CMD_RESTORE_STATE);
146 if (command->m_type == CMD_RESTORE_STATE)
147 {
148 int len = strlen(fileName);
149 if (len < MAX_URDF_FILENAME_LENGTH)
150 {
151 strcpy(command->m_loadStateArguments.m_fileName, fileName);
152 }
153 else
154 {
155 command->m_loadStateArguments.m_fileName[0] = 0;
156 }
157 command->m_updateFlags |= CMD_LOAD_STATE_HAS_FILENAME;
158 }
159 return 0;
160 }
161
b3SaveStateCommandInit(b3PhysicsClientHandle physClient)162 B3_SHARED_API b3SharedMemoryCommandHandle b3SaveStateCommandInit(b3PhysicsClientHandle physClient)
163 {
164 PhysicsClient* cl = (PhysicsClient*)physClient;
165 b3Assert(cl);
166 b3Assert(cl->canSubmitCommand());
167
168 if (cl->canSubmitCommand())
169 {
170 struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
171 b3Assert(command);
172 command->m_type = CMD_SAVE_STATE;
173 command->m_updateFlags = 0;
174 return (b3SharedMemoryCommandHandle)command;
175 }
176 return 0;
177 }
178
b3GetStatusGetStateId(b3SharedMemoryStatusHandle statusHandle)179 B3_SHARED_API int b3GetStatusGetStateId(b3SharedMemoryStatusHandle statusHandle)
180 {
181 int stateId = -1;
182
183 const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
184 b3Assert(status);
185 b3Assert(status->m_type == CMD_SAVE_STATE_COMPLETED);
186 if (status && status->m_type == CMD_SAVE_STATE_COMPLETED)
187 {
188 stateId = status->m_saveStateResultArgs.m_stateId;
189 }
190 return stateId;
191 }
192
b3SaveBulletCommandInit(b3PhysicsClientHandle physClient,const char * fileName)193 B3_SHARED_API b3SharedMemoryCommandHandle b3SaveBulletCommandInit(b3PhysicsClientHandle physClient, const char* fileName)
194 {
195 PhysicsClient* cl = (PhysicsClient*)physClient;
196 b3Assert(cl);
197 b3Assert(cl->canSubmitCommand());
198
199 if (cl->canSubmitCommand())
200 {
201 struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
202 b3Assert(command);
203 command->m_type = CMD_SAVE_BULLET;
204 int len = strlen(fileName);
205 if (len < MAX_URDF_FILENAME_LENGTH)
206 {
207 strcpy(command->m_fileArguments.m_fileName, fileName);
208 }
209 else
210 {
211 command->m_fileArguments.m_fileName[0] = 0;
212 }
213 command->m_updateFlags = 0;
214
215 return (b3SharedMemoryCommandHandle)command;
216 }
217 return 0;
218 }
b3LoadMJCFCommandInit(b3PhysicsClientHandle physClient,const char * fileName)219 B3_SHARED_API b3SharedMemoryCommandHandle b3LoadMJCFCommandInit(b3PhysicsClientHandle physClient, const char* fileName)
220 {
221 PhysicsClient* cl = (PhysicsClient*)physClient;
222 b3Assert(cl);
223 b3Assert(cl->canSubmitCommand());
224
225 if (cl->canSubmitCommand())
226 {
227 struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
228 b3Assert(command);
229 return b3LoadMJCFCommandInit2((b3SharedMemoryCommandHandle)command, fileName);
230 }
231 return 0;
232 }
233
b3LoadMJCFCommandInit2(b3SharedMemoryCommandHandle commandHandle,const char * fileName)234 B3_SHARED_API b3SharedMemoryCommandHandle b3LoadMJCFCommandInit2(b3SharedMemoryCommandHandle commandHandle, const char* fileName)
235 {
236 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
237 command->m_type = CMD_LOAD_MJCF;
238 int len = strlen(fileName);
239 if (len < MAX_URDF_FILENAME_LENGTH)
240 {
241 strcpy(command->m_mjcfArguments.m_mjcfFileName, fileName);
242 }
243 else
244 {
245 command->m_mjcfArguments.m_mjcfFileName[0] = 0;
246 }
247 command->m_updateFlags = 0;
248
249 return (b3SharedMemoryCommandHandle)command;
250 }
251
b3LoadMJCFCommandSetFlags(b3SharedMemoryCommandHandle commandHandle,int flags)252 B3_SHARED_API void b3LoadMJCFCommandSetFlags(b3SharedMemoryCommandHandle commandHandle, int flags)
253 {
254 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
255 b3Assert(command->m_type == CMD_LOAD_MJCF);
256 if (command->m_type == CMD_LOAD_MJCF)
257 {
258 command->m_mjcfArguments.m_flags = flags;
259 command->m_updateFlags |= URDF_ARGS_HAS_CUSTOM_URDF_FLAGS;
260 }
261 }
262
b3LoadMJCFCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle,int useMultiBody)263 B3_SHARED_API void b3LoadMJCFCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody)
264 {
265 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
266 b3Assert(command->m_type == CMD_LOAD_MJCF);
267 if (command->m_type == CMD_LOAD_MJCF)
268 {
269 command->m_updateFlags |= URDF_ARGS_USE_MULTIBODY;
270 command->m_mjcfArguments.m_useMultiBody = useMultiBody;
271 }
272 }
273
b3LoadSoftBodyCommandInit(b3PhysicsClientHandle physClient,const char * fileName)274 B3_SHARED_API b3SharedMemoryCommandHandle b3LoadSoftBodyCommandInit(b3PhysicsClientHandle physClient, const char* fileName)
275 {
276 PhysicsClient* cl = (PhysicsClient*)physClient;
277 b3Assert(cl);
278 b3Assert(cl->canSubmitCommand());
279
280 if (cl->canSubmitCommand())
281 {
282 struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
283 b3Assert(command);
284 command->m_type = CMD_LOAD_SOFT_BODY;
285 int len = strlen(fileName);
286 if (len < MAX_FILENAME_LENGTH)
287 {
288 strcpy(command->m_loadSoftBodyArguments.m_fileName, fileName);
289 }
290 else
291 {
292 command->m_loadSoftBodyArguments.m_fileName[0] = 0;
293 }
294 command->m_updateFlags = LOAD_SOFT_BODY_FILE_NAME;
295
296 return (b3SharedMemoryCommandHandle)command;
297 }
298 return 0;
299 }
300
b3LoadSoftBodySetScale(b3SharedMemoryCommandHandle commandHandle,double scale)301 B3_SHARED_API int b3LoadSoftBodySetScale(b3SharedMemoryCommandHandle commandHandle, double scale)
302 {
303 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
304 b3Assert(command->m_type == CMD_LOAD_SOFT_BODY);
305 command->m_loadSoftBodyArguments.m_scale = scale;
306 command->m_updateFlags |= LOAD_SOFT_BODY_UPDATE_SCALE;
307 return 0;
308 }
309
b3LoadSoftBodySetMass(b3SharedMemoryCommandHandle commandHandle,double mass)310 B3_SHARED_API int b3LoadSoftBodySetMass(b3SharedMemoryCommandHandle commandHandle, double mass)
311 {
312 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
313 b3Assert(command->m_type == CMD_LOAD_SOFT_BODY);
314 command->m_loadSoftBodyArguments.m_mass = mass;
315 command->m_updateFlags |= LOAD_SOFT_BODY_UPDATE_MASS;
316 return 0;
317 }
318
b3LoadSoftBodySetCollisionMargin(b3SharedMemoryCommandHandle commandHandle,double collisionMargin)319 B3_SHARED_API int b3LoadSoftBodySetCollisionMargin(b3SharedMemoryCommandHandle commandHandle, double collisionMargin)
320 {
321 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
322 b3Assert(command->m_type == CMD_LOAD_SOFT_BODY);
323 command->m_loadSoftBodyArguments.m_collisionMargin = collisionMargin;
324 command->m_updateFlags |= LOAD_SOFT_BODY_UPDATE_COLLISION_MARGIN;
325 return 0;
326 }
327
328
b3LoadSoftBodySetStartPosition(b3SharedMemoryCommandHandle commandHandle,double startPosX,double startPosY,double startPosZ)329 B3_SHARED_API int b3LoadSoftBodySetStartPosition(b3SharedMemoryCommandHandle commandHandle, double startPosX, double startPosY, double startPosZ)
330 {
331 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
332 b3Assert(command->m_type == CMD_LOAD_SOFT_BODY);
333 command->m_loadSoftBodyArguments.m_initialPosition[0] = startPosX;
334 command->m_loadSoftBodyArguments.m_initialPosition[1] = startPosY;
335 command->m_loadSoftBodyArguments.m_initialPosition[2] = startPosZ;
336 command->m_updateFlags |= LOAD_SOFT_BODY_INITIAL_POSITION;
337 return 0;
338 }
339
b3LoadSoftBodySetStartOrientation(b3SharedMemoryCommandHandle commandHandle,double startOrnX,double startOrnY,double startOrnZ,double startOrnW)340 B3_SHARED_API int b3LoadSoftBodySetStartOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX, double startOrnY, double startOrnZ, double startOrnW)
341 {
342 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
343 b3Assert(command->m_type == CMD_LOAD_SOFT_BODY);
344 command->m_loadSoftBodyArguments.m_initialOrientation[0] = startOrnX;
345 command->m_loadSoftBodyArguments.m_initialOrientation[1] = startOrnY;
346 command->m_loadSoftBodyArguments.m_initialOrientation[2] = startOrnZ;
347 command->m_loadSoftBodyArguments.m_initialOrientation[3] = startOrnW;
348 command->m_updateFlags |= LOAD_SOFT_BODY_INITIAL_ORIENTATION;
349 return 0;
350 }
351
b3LoadSoftBodyUpdateSimMesh(b3SharedMemoryCommandHandle commandHandle,const char * filename)352 B3_SHARED_API int b3LoadSoftBodyUpdateSimMesh(b3SharedMemoryCommandHandle commandHandle, const char* filename)
353 {
354 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
355 b3Assert(command->m_type == CMD_LOAD_SOFT_BODY);
356 int len = strlen(filename);
357 if (len < MAX_FILENAME_LENGTH)
358 {
359 strcpy(command->m_loadSoftBodyArguments.m_simFileName, filename);
360 }
361 else
362 {
363 command->m_loadSoftBodyArguments.m_simFileName[0] = 0;
364 }
365 command->m_updateFlags |= LOAD_SOFT_BODY_SIM_MESH;
366 return 0;
367
368 }
369
b3LoadSoftBodyAddCorotatedForce(b3SharedMemoryCommandHandle commandHandle,double corotatedMu,double corotatedLambda)370 B3_SHARED_API int b3LoadSoftBodyAddCorotatedForce(b3SharedMemoryCommandHandle commandHandle, double corotatedMu, double corotatedLambda)
371 {
372 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
373 b3Assert(command->m_type == CMD_LOAD_SOFT_BODY);
374 command->m_loadSoftBodyArguments.m_corotatedMu = corotatedMu;
375 command->m_loadSoftBodyArguments.m_corotatedLambda = corotatedLambda;
376 command->m_updateFlags |= LOAD_SOFT_BODY_ADD_COROTATED_FORCE;
377 return 0;
378 }
379
b3LoadSoftBodyAddNeoHookeanForce(b3SharedMemoryCommandHandle commandHandle,double NeoHookeanMu,double NeoHookeanLambda,double NeoHookeanDamping)380 B3_SHARED_API int b3LoadSoftBodyAddNeoHookeanForce(b3SharedMemoryCommandHandle commandHandle, double NeoHookeanMu, double NeoHookeanLambda, double NeoHookeanDamping)
381 {
382 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
383 b3Assert(command->m_type == CMD_LOAD_SOFT_BODY);
384 command->m_loadSoftBodyArguments.m_NeoHookeanMu = NeoHookeanMu;
385 command->m_loadSoftBodyArguments.m_NeoHookeanLambda = NeoHookeanLambda;
386 command->m_loadSoftBodyArguments.m_NeoHookeanDamping = NeoHookeanDamping;
387 command->m_updateFlags |= LOAD_SOFT_BODY_ADD_NEOHOOKEAN_FORCE;
388 return 0;
389 }
390
b3LoadSoftBodyAddMassSpringForce(b3SharedMemoryCommandHandle commandHandle,double springElasticStiffness,double springDampingStiffness)391 B3_SHARED_API int b3LoadSoftBodyAddMassSpringForce(b3SharedMemoryCommandHandle commandHandle, double springElasticStiffness , double springDampingStiffness)
392 {
393 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
394 b3Assert(command->m_type == CMD_LOAD_SOFT_BODY);
395 command->m_loadSoftBodyArguments.m_springElasticStiffness = springElasticStiffness;
396 command->m_loadSoftBodyArguments.m_springDampingStiffness = springDampingStiffness;
397 command->m_updateFlags |= LOAD_SOFT_BODY_ADD_MASS_SPRING_FORCE;
398 return 0;
399 }
400
b3LoadSoftBodyAddGravityForce(b3SharedMemoryCommandHandle commandHandle,double gravityX,double gravityY,double gravityZ)401 B3_SHARED_API int b3LoadSoftBodyAddGravityForce(b3SharedMemoryCommandHandle commandHandle, double gravityX, double gravityY, double gravityZ)
402 {
403 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
404 b3Assert(command->m_type == CMD_LOAD_SOFT_BODY);
405 command->m_updateFlags |= LOAD_SOFT_BODY_ADD_GRAVITY_FORCE;
406 return 0;
407 }
408
b3LoadSoftBodySetCollisionHardness(b3SharedMemoryCommandHandle commandHandle,double collisionHardness)409 B3_SHARED_API int b3LoadSoftBodySetCollisionHardness(b3SharedMemoryCommandHandle commandHandle, double collisionHardness)
410 {
411 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
412 b3Assert(command->m_type == CMD_LOAD_SOFT_BODY);
413 command->m_loadSoftBodyArguments.m_collisionHardness = collisionHardness;
414 command->m_updateFlags |= LOAD_SOFT_BODY_SET_COLLISION_HARDNESS;
415 return 0;
416 }
417
b3LoadSoftBodySetRepulsionStiffness(b3SharedMemoryCommandHandle commandHandle,double stiffness)418 B3_SHARED_API int b3LoadSoftBodySetRepulsionStiffness(b3SharedMemoryCommandHandle commandHandle, double stiffness)
419 {
420 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
421 b3Assert(command->m_type == CMD_LOAD_SOFT_BODY);
422 command->m_loadSoftBodyArguments.m_repulsionStiffness = stiffness;
423 command->m_updateFlags |= LOAD_SOFT_BODY_SET_REPULSION_STIFFNESS;
424 return 0;
425 }
426
b3LoadSoftBodySetGravityFactor(b3SharedMemoryCommandHandle commandHandle,double gravFactor)427 B3_SHARED_API int b3LoadSoftBodySetGravityFactor(b3SharedMemoryCommandHandle commandHandle, double gravFactor)
428 {
429 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
430 b3Assert(command->m_type == CMD_LOAD_SOFT_BODY);
431 command->m_loadSoftBodyArguments.m_gravFactor = gravFactor;
432 command->m_updateFlags |= LOAD_SOFT_BODY_SET_GRAVITY_FACTOR;
433 return 0;
434 }
435
b3LoadSoftBodySetSelfCollision(b3SharedMemoryCommandHandle commandHandle,int useSelfCollision)436 B3_SHARED_API int b3LoadSoftBodySetSelfCollision(b3SharedMemoryCommandHandle commandHandle, int useSelfCollision)
437 {
438 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
439 b3Assert(command->m_type == CMD_LOAD_SOFT_BODY);
440 command->m_loadSoftBodyArguments.m_useSelfCollision = useSelfCollision;
441 command->m_updateFlags |= LOAD_SOFT_BODY_USE_SELF_COLLISION;
442 return 0;
443 }
444
b3LoadSoftBodySetFrictionCoefficient(b3SharedMemoryCommandHandle commandHandle,double frictionCoefficient)445 B3_SHARED_API int b3LoadSoftBodySetFrictionCoefficient(b3SharedMemoryCommandHandle commandHandle, double frictionCoefficient)
446 {
447 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
448 b3Assert(command->m_type == CMD_LOAD_SOFT_BODY);
449 command->m_loadSoftBodyArguments.m_frictionCoeff = frictionCoefficient;
450 command->m_updateFlags |= LOAD_SOFT_BODY_SET_FRICTION_COEFFICIENT;
451 return 0;
452 }
453
b3LoadSoftBodyUseBendingSprings(b3SharedMemoryCommandHandle commandHandle,int useBendingSprings,double bendingStiffness)454 B3_SHARED_API int b3LoadSoftBodyUseBendingSprings(b3SharedMemoryCommandHandle commandHandle, int useBendingSprings, double bendingStiffness)
455 {
456 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
457 b3Assert(command->m_type == CMD_LOAD_SOFT_BODY);
458 command->m_loadSoftBodyArguments.m_useBendingSprings = useBendingSprings;
459 command->m_loadSoftBodyArguments.m_springBendingStiffness = bendingStiffness;
460 command->m_updateFlags |= LOAD_SOFT_BODY_ADD_BENDING_SPRINGS;
461 return 0;
462 }
463
b3LoadSoftBodyUseAllDirectionDampingSprings(b3SharedMemoryCommandHandle commandHandle,int allDirectionDamping)464 B3_SHARED_API int b3LoadSoftBodyUseAllDirectionDampingSprings(b3SharedMemoryCommandHandle commandHandle, int allDirectionDamping)
465 {
466 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
467 b3Assert(command->m_type == CMD_LOAD_SOFT_BODY);
468 command->m_loadSoftBodyArguments.m_dampAllDirections = allDirectionDamping;
469 command->m_updateFlags |= LOAD_SOFT_BODY_SET_DAMPING_SPRING_MODE;
470 return 0;
471 }
472
b3LoadSoftBodyUseFaceContact(b3SharedMemoryCommandHandle commandHandle,int useFaceContact)473 B3_SHARED_API int b3LoadSoftBodyUseFaceContact(b3SharedMemoryCommandHandle commandHandle, int useFaceContact)
474 {
475 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
476 b3Assert(command->m_type == CMD_LOAD_SOFT_BODY);
477 command->m_loadSoftBodyArguments.m_useFaceContact = useFaceContact;
478 command->m_updateFlags |= LOAD_SOFT_BODY_USE_FACE_CONTACT;
479 return 0;
480 }
481
b3LoadUrdfCommandInit(b3PhysicsClientHandle physClient,const char * urdfFileName)482 B3_SHARED_API b3SharedMemoryCommandHandle b3LoadUrdfCommandInit(b3PhysicsClientHandle physClient, const char* urdfFileName)
483 {
484 PhysicsClient* cl = (PhysicsClient*)physClient;
485 b3Assert(cl);
486 b3Assert(cl->canSubmitCommand());
487
488 if (cl->canSubmitCommand())
489 {
490 struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
491 b3Assert(command);
492 command->m_type = CMD_LOAD_URDF;
493 int len = strlen(urdfFileName);
494 if (len < MAX_URDF_FILENAME_LENGTH)
495 {
496 strcpy(command->m_urdfArguments.m_urdfFileName, urdfFileName);
497 }
498 else
499 {
500 command->m_urdfArguments.m_urdfFileName[0] = 0;
501 }
502 command->m_updateFlags = URDF_ARGS_FILE_NAME;
503
504 return (b3SharedMemoryCommandHandle)command;
505 }
506 return 0;
507 }
508
b3LoadUrdfCommandInit2(b3SharedMemoryCommandHandle commandHandle,const char * urdfFileName)509 B3_SHARED_API b3SharedMemoryCommandHandle b3LoadUrdfCommandInit2(b3SharedMemoryCommandHandle commandHandle, const char* urdfFileName)
510 {
511 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
512 b3Assert(command);
513
514 command->m_type = CMD_LOAD_URDF;
515 int len = strlen(urdfFileName);
516 if (len < MAX_URDF_FILENAME_LENGTH)
517 {
518 strcpy(command->m_urdfArguments.m_urdfFileName, urdfFileName);
519 }
520 else
521 {
522 command->m_urdfArguments.m_urdfFileName[0] = 0;
523 }
524 command->m_updateFlags = URDF_ARGS_FILE_NAME;
525
526 return (b3SharedMemoryCommandHandle)command;
527 }
528
b3LoadUrdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle,int useMultiBody)529 B3_SHARED_API int b3LoadUrdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody)
530 {
531 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
532 b3Assert(command);
533 b3Assert(command->m_type == CMD_LOAD_URDF);
534 command->m_updateFlags |= URDF_ARGS_USE_MULTIBODY;
535 command->m_urdfArguments.m_useMultiBody = useMultiBody;
536
537 return 0;
538 }
539
b3LoadUrdfCommandSetGlobalScaling(b3SharedMemoryCommandHandle commandHandle,double globalScaling)540 B3_SHARED_API int b3LoadUrdfCommandSetGlobalScaling(b3SharedMemoryCommandHandle commandHandle, double globalScaling)
541 {
542 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
543 b3Assert(command);
544 b3Assert(command->m_type == CMD_LOAD_URDF);
545 command->m_updateFlags |= URDF_ARGS_USE_GLOBAL_SCALING;
546 command->m_urdfArguments.m_globalScaling = globalScaling;
547 return 0;
548 }
549
b3LoadSdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle,int useMultiBody)550 B3_SHARED_API int b3LoadSdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody)
551 {
552 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
553 b3Assert(command);
554 b3Assert(command->m_type == CMD_LOAD_SDF);
555 command->m_updateFlags |= URDF_ARGS_USE_MULTIBODY;
556 command->m_sdfArguments.m_useMultiBody = useMultiBody;
557
558 return 0;
559 }
560
b3LoadSdfCommandSetUseGlobalScaling(b3SharedMemoryCommandHandle commandHandle,double globalScaling)561 B3_SHARED_API int b3LoadSdfCommandSetUseGlobalScaling(b3SharedMemoryCommandHandle commandHandle, double globalScaling)
562 {
563 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
564 b3Assert(command);
565 b3Assert(command->m_type == CMD_LOAD_SDF);
566 command->m_updateFlags |= URDF_ARGS_USE_GLOBAL_SCALING;
567 command->m_sdfArguments.m_globalScaling = globalScaling;
568
569 return 0;
570 }
571
b3LoadUrdfCommandSetUseFixedBase(b3SharedMemoryCommandHandle commandHandle,int useFixedBase)572 B3_SHARED_API int b3LoadUrdfCommandSetUseFixedBase(b3SharedMemoryCommandHandle commandHandle, int useFixedBase)
573 {
574 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
575 b3Assert(command);
576 b3Assert(command->m_type == CMD_LOAD_URDF);
577 if (command && (command->m_type == CMD_LOAD_URDF))
578 {
579 command->m_updateFlags |= URDF_ARGS_USE_FIXED_BASE;
580 command->m_urdfArguments.m_useFixedBase = useFixedBase;
581 return 0;
582 }
583 return -1;
584 }
585
b3LoadUrdfCommandSetFlags(b3SharedMemoryCommandHandle commandHandle,int flags)586 B3_SHARED_API int b3LoadUrdfCommandSetFlags(b3SharedMemoryCommandHandle commandHandle, int flags)
587 {
588 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
589 b3Assert(command);
590 b3Assert(command->m_type == CMD_LOAD_URDF);
591 if (command && (command->m_type == CMD_LOAD_URDF))
592 {
593 command->m_updateFlags |= URDF_ARGS_HAS_CUSTOM_URDF_FLAGS;
594 command->m_urdfArguments.m_urdfFlags = flags;
595 }
596 return 0;
597 }
598
b3LoadUrdfCommandSetStartPosition(b3SharedMemoryCommandHandle commandHandle,double startPosX,double startPosY,double startPosZ)599 B3_SHARED_API int b3LoadUrdfCommandSetStartPosition(b3SharedMemoryCommandHandle commandHandle, double startPosX, double startPosY, double startPosZ)
600 {
601 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
602 b3Assert(command);
603 if (command)
604 {
605 b3Assert(command->m_type == CMD_LOAD_URDF);
606 if (command->m_type == CMD_LOAD_URDF)
607 {
608 command->m_urdfArguments.m_initialPosition[0] = startPosX;
609 command->m_urdfArguments.m_initialPosition[1] = startPosY;
610 command->m_urdfArguments.m_initialPosition[2] = startPosZ;
611 command->m_updateFlags |= URDF_ARGS_INITIAL_POSITION;
612 }
613 return 0;
614 }
615 return -1;
616 }
617
b3LoadUrdfCommandSetStartOrientation(b3SharedMemoryCommandHandle commandHandle,double startOrnX,double startOrnY,double startOrnZ,double startOrnW)618 B3_SHARED_API int b3LoadUrdfCommandSetStartOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX, double startOrnY, double startOrnZ, double startOrnW)
619 {
620 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
621 b3Assert(command);
622 if (command)
623 {
624 b3Assert(command->m_type == CMD_LOAD_URDF);
625 if (command->m_type == CMD_LOAD_URDF)
626 {
627 command->m_urdfArguments.m_initialOrientation[0] = startOrnX;
628 command->m_urdfArguments.m_initialOrientation[1] = startOrnY;
629 command->m_urdfArguments.m_initialOrientation[2] = startOrnZ;
630 command->m_urdfArguments.m_initialOrientation[3] = startOrnW;
631 command->m_updateFlags |= URDF_ARGS_INITIAL_ORIENTATION;
632 }
633 return 0;
634 }
635 return -1;
636 }
637
b3InitRequestPhysicsParamCommand(b3PhysicsClientHandle physClient)638 B3_SHARED_API b3SharedMemoryCommandHandle b3InitRequestPhysicsParamCommand(b3PhysicsClientHandle physClient)
639 {
640 PhysicsClient* cl = (PhysicsClient*)physClient;
641 b3Assert(cl);
642 b3Assert(cl->canSubmitCommand());
643 struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
644 b3Assert(command);
645 command->m_type = CMD_REQUEST_PHYSICS_SIMULATION_PARAMETERS;
646 command->m_updateFlags = 0;
647 return (b3SharedMemoryCommandHandle)command;
648 }
649
b3GetStatusPhysicsSimulationParameters(b3SharedMemoryStatusHandle statusHandle,struct b3PhysicsSimulationParameters * params)650 B3_SHARED_API int b3GetStatusPhysicsSimulationParameters(b3SharedMemoryStatusHandle statusHandle, struct b3PhysicsSimulationParameters* params)
651 {
652 const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
653 b3Assert(status);
654 b3Assert(status->m_type == CMD_REQUEST_PHYSICS_SIMULATION_PARAMETERS_COMPLETED);
655 if (status && status->m_type == CMD_REQUEST_PHYSICS_SIMULATION_PARAMETERS_COMPLETED)
656 {
657 *params = status->m_simulationParameterResultArgs;
658 return 1;
659 }
660 return 0;
661 }
662
b3InitPhysicsParamCommand(b3PhysicsClientHandle physClient)663 B3_SHARED_API b3SharedMemoryCommandHandle b3InitPhysicsParamCommand(b3PhysicsClientHandle physClient)
664 {
665 PhysicsClient* cl = (PhysicsClient*)physClient;
666 b3Assert(cl);
667 b3Assert(cl->canSubmitCommand());
668 struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
669 b3Assert(command);
670 return b3InitPhysicsParamCommand2((b3SharedMemoryCommandHandle)command);
671 }
672
b3InitPhysicsParamCommand2(b3SharedMemoryCommandHandle commandHandle)673 B3_SHARED_API b3SharedMemoryCommandHandle b3InitPhysicsParamCommand2(b3SharedMemoryCommandHandle commandHandle)
674 {
675 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
676 command->m_type = CMD_SEND_PHYSICS_SIMULATION_PARAMETERS;
677 command->m_updateFlags = 0;
678
679 return (b3SharedMemoryCommandHandle)command;
680 }
681
b3PhysicsParamSetGravity(b3SharedMemoryCommandHandle commandHandle,double gravx,double gravy,double gravz)682 B3_SHARED_API int b3PhysicsParamSetGravity(b3SharedMemoryCommandHandle commandHandle, double gravx, double gravy, double gravz)
683 {
684 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
685 b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
686 command->m_physSimParamArgs.m_gravityAcceleration[0] = gravx;
687 command->m_physSimParamArgs.m_gravityAcceleration[1] = gravy;
688 command->m_physSimParamArgs.m_gravityAcceleration[2] = gravz;
689 command->m_updateFlags |= SIM_PARAM_UPDATE_GRAVITY;
690 return 0;
691 }
692
b3PhysicsParamSetRealTimeSimulation(b3SharedMemoryCommandHandle commandHandle,int enableRealTimeSimulation)693 B3_SHARED_API int b3PhysicsParamSetRealTimeSimulation(b3SharedMemoryCommandHandle commandHandle, int enableRealTimeSimulation)
694 {
695 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
696 b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
697 command->m_physSimParamArgs.m_useRealTimeSimulation = (enableRealTimeSimulation != 0);
698 command->m_updateFlags |= SIM_PARAM_UPDATE_REAL_TIME_SIMULATION;
699 return 0;
700 }
701
b3PhysicsParamSetInternalSimFlags(b3SharedMemoryCommandHandle commandHandle,int flags)702 B3_SHARED_API int b3PhysicsParamSetInternalSimFlags(b3SharedMemoryCommandHandle commandHandle, int flags)
703 {
704 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
705 b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
706 command->m_physSimParamArgs.m_internalSimFlags = flags;
707 command->m_updateFlags |= SIM_PARAM_UPDATE_INTERNAL_SIMULATION_FLAGS;
708 return 0;
709 }
710
b3PhysicsParamSetUseSplitImpulse(b3SharedMemoryCommandHandle commandHandle,int useSplitImpulse)711 B3_SHARED_API int b3PhysicsParamSetUseSplitImpulse(b3SharedMemoryCommandHandle commandHandle, int useSplitImpulse)
712 {
713 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
714 b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
715
716 command->m_physSimParamArgs.m_useSplitImpulse = useSplitImpulse;
717 command->m_updateFlags |= SIM_PARAM_UPDATE_USE_SPLIT_IMPULSE;
718 return 0;
719 }
720
b3PhysicsParamSetSplitImpulsePenetrationThreshold(b3SharedMemoryCommandHandle commandHandle,double splitImpulsePenetrationThreshold)721 B3_SHARED_API int b3PhysicsParamSetSplitImpulsePenetrationThreshold(b3SharedMemoryCommandHandle commandHandle, double splitImpulsePenetrationThreshold)
722 {
723 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
724 b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
725
726 command->m_physSimParamArgs.m_splitImpulsePenetrationThreshold = splitImpulsePenetrationThreshold;
727 command->m_updateFlags |= SIM_PARAM_UPDATE_SPLIT_IMPULSE_PENETRATION_THRESHOLD;
728 return 0;
729 }
730
b3PhysicsParamSetContactBreakingThreshold(b3SharedMemoryCommandHandle commandHandle,double contactBreakingThreshold)731 B3_SHARED_API int b3PhysicsParamSetContactBreakingThreshold(b3SharedMemoryCommandHandle commandHandle, double contactBreakingThreshold)
732 {
733 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
734 b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
735
736 command->m_physSimParamArgs.m_contactBreakingThreshold = contactBreakingThreshold;
737 command->m_updateFlags |= SIM_PARAM_UPDATE_CONTACT_BREAKING_THRESHOLD;
738 return 0;
739 }
740
b3PhysicsParamSetMaxNumCommandsPer1ms(b3SharedMemoryCommandHandle commandHandle,int maxNumCmdPer1ms)741 B3_SHARED_API int b3PhysicsParamSetMaxNumCommandsPer1ms(b3SharedMemoryCommandHandle commandHandle, int maxNumCmdPer1ms)
742 {
743 //obsolete command
744 return 0;
745 }
746
b3PhysicsParamSetEnableFileCaching(b3SharedMemoryCommandHandle commandHandle,int enableFileCaching)747 B3_SHARED_API int b3PhysicsParamSetEnableFileCaching(b3SharedMemoryCommandHandle commandHandle, int enableFileCaching)
748 {
749 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
750 b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
751
752 command->m_physSimParamArgs.m_enableFileCaching = enableFileCaching;
753 command->m_updateFlags |= SIM_PARAM_ENABLE_FILE_CACHING;
754 return 0;
755 }
756
b3PhysicsParamSetRestitutionVelocityThreshold(b3SharedMemoryCommandHandle commandHandle,double restitutionVelocityThreshold)757 B3_SHARED_API int b3PhysicsParamSetRestitutionVelocityThreshold(b3SharedMemoryCommandHandle commandHandle, double restitutionVelocityThreshold)
758 {
759 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
760 b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
761
762 command->m_physSimParamArgs.m_restitutionVelocityThreshold = restitutionVelocityThreshold;
763 command->m_updateFlags |= SIM_PARAM_UPDATE_RESTITUTION_VELOCITY_THRESHOLD;
764 return 0;
765 }
766
b3PhysicsParamSetEnableConeFriction(b3SharedMemoryCommandHandle commandHandle,int enableConeFriction)767 B3_SHARED_API int b3PhysicsParamSetEnableConeFriction(b3SharedMemoryCommandHandle commandHandle, int enableConeFriction)
768 {
769 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
770 b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
771 command->m_physSimParamArgs.m_enableConeFriction = enableConeFriction;
772 command->m_updateFlags |= SIM_PARAM_ENABLE_CONE_FRICTION;
773 return 0;
774 }
775
b3PhysicsParameterSetDeterministicOverlappingPairs(b3SharedMemoryCommandHandle commandHandle,int deterministicOverlappingPairs)776 B3_SHARED_API int b3PhysicsParameterSetDeterministicOverlappingPairs(b3SharedMemoryCommandHandle commandHandle, int deterministicOverlappingPairs)
777 {
778 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
779 b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
780 command->m_physSimParamArgs.m_deterministicOverlappingPairs = deterministicOverlappingPairs;
781 command->m_updateFlags |= SIM_PARAM_UPDATE_DETERMINISTIC_OVERLAPPING_PAIRS;
782 return 0;
783 }
784
b3PhysicsParameterSetAllowedCcdPenetration(b3SharedMemoryCommandHandle commandHandle,double allowedCcdPenetration)785 B3_SHARED_API int b3PhysicsParameterSetAllowedCcdPenetration(b3SharedMemoryCommandHandle commandHandle, double allowedCcdPenetration)
786 {
787 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
788 b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
789 command->m_physSimParamArgs.m_allowedCcdPenetration = allowedCcdPenetration;
790 command->m_updateFlags |= SIM_PARAM_UPDATE_CCD_ALLOWED_PENETRATION;
791 return 0;
792 }
793
b3PhysicsParameterSetJointFeedbackMode(b3SharedMemoryCommandHandle commandHandle,int jointFeedbackMode)794 B3_SHARED_API int b3PhysicsParameterSetJointFeedbackMode(b3SharedMemoryCommandHandle commandHandle, int jointFeedbackMode)
795 {
796 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
797 b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
798 command->m_physSimParamArgs.m_jointFeedbackMode = jointFeedbackMode;
799 command->m_updateFlags |= SIM_PARAM_UPDATE_JOINT_FEEDBACK_MODE;
800 return 0;
801 }
802
b3PhysicsParamSetNumSolverIterations(b3SharedMemoryCommandHandle commandHandle,int numSolverIterations)803 B3_SHARED_API int b3PhysicsParamSetNumSolverIterations(b3SharedMemoryCommandHandle commandHandle, int numSolverIterations)
804 {
805 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
806 b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
807 command->m_physSimParamArgs.m_numSolverIterations = numSolverIterations;
808 command->m_updateFlags |= SIM_PARAM_UPDATE_NUM_SOLVER_ITERATIONS;
809 return 0;
810 }
811
b3PhysicsParamSetNumNonContactInnerIterations(b3SharedMemoryCommandHandle commandHandle,int numNonContactInnerIterations)812 B3_SHARED_API int b3PhysicsParamSetNumNonContactInnerIterations(b3SharedMemoryCommandHandle commandHandle, int numNonContactInnerIterations)
813 {
814 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
815 b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
816 command->m_physSimParamArgs.m_numNonContactInnerIterations = numNonContactInnerIterations;
817 command->m_updateFlags |= SIM_PARAM_UPDATE_NUM_NONCONTACT_INNER_ITERATIONS;
818 return 0;
819 }
820
b3PhysicsParamSetWarmStartingFactor(b3SharedMemoryCommandHandle commandHandle,double warmStartingFactor)821 B3_SHARED_API int b3PhysicsParamSetWarmStartingFactor(b3SharedMemoryCommandHandle commandHandle, double warmStartingFactor)
822 {
823 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
824 b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
825 command->m_physSimParamArgs.m_warmStartingFactor = warmStartingFactor;
826 command->m_updateFlags |= SIM_PARAM_UPDATE_WARM_STARTING_FACTOR;
827 return 0;
828 }
829
b3PhysicsParamSetArticulatedWarmStartingFactor(b3SharedMemoryCommandHandle commandHandle,double warmStartingFactor)830 B3_SHARED_API int b3PhysicsParamSetArticulatedWarmStartingFactor(b3SharedMemoryCommandHandle commandHandle, double warmStartingFactor)
831 {
832 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
833 b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
834 command->m_physSimParamArgs.m_articulatedWarmStartingFactor = warmStartingFactor;
835 command->m_updateFlags |= SIM_PARAM_UPDATE_ARTICULATED_WARM_STARTING_FACTOR;
836 return 0;
837 }
b3PhysicsParamSetSolverResidualThreshold(b3SharedMemoryCommandHandle commandHandle,double solverResidualThreshold)838 B3_SHARED_API int b3PhysicsParamSetSolverResidualThreshold(b3SharedMemoryCommandHandle commandHandle, double solverResidualThreshold)
839 {
840 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
841 b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
842 command->m_physSimParamArgs.m_solverResidualThreshold = solverResidualThreshold;
843 command->m_updateFlags |= SIM_PARAM_UPDATE_SOLVER_RESIDULAL_THRESHOLD;
844 return 0;
845 }
846
b3PhysicsParamSetContactSlop(b3SharedMemoryCommandHandle commandHandle,double contactSlop)847 B3_SHARED_API int b3PhysicsParamSetContactSlop(b3SharedMemoryCommandHandle commandHandle, double contactSlop)
848 {
849 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
850 b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
851 command->m_physSimParamArgs.m_contactSlop = contactSlop;
852 command->m_updateFlags |= SIM_PARAM_UPDATE_CONTACT_SLOP;
853 return 0;
854 }
855
b3PhysicsParameterSetEnableSAT(b3SharedMemoryCommandHandle commandHandle,int enableSAT)856 B3_SHARED_API int b3PhysicsParameterSetEnableSAT(b3SharedMemoryCommandHandle commandHandle, int enableSAT)
857 {
858 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
859 b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
860 command->m_physSimParamArgs.m_enableSAT = enableSAT;
861 command->m_updateFlags |= SIM_PARAM_ENABLE_SAT;
862 return 0;
863 }
864
b3PhysicsParameterSetConstraintSolverType(b3SharedMemoryCommandHandle commandHandle,int constraintSolverType)865 B3_SHARED_API int b3PhysicsParameterSetConstraintSolverType(b3SharedMemoryCommandHandle commandHandle, int constraintSolverType)
866 {
867 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
868 b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
869 command->m_physSimParamArgs.m_constraintSolverType = constraintSolverType;
870 command->m_updateFlags |= SIM_PARAM_CONSTRAINT_SOLVER_TYPE;
871 return 0;
872 }
873
b3PhysicsParameterSetMinimumSolverIslandSize(b3SharedMemoryCommandHandle commandHandle,int minimumSolverIslandSize)874 B3_SHARED_API int b3PhysicsParameterSetMinimumSolverIslandSize(b3SharedMemoryCommandHandle commandHandle, int minimumSolverIslandSize)
875 {
876 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
877 b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
878 command->m_physSimParamArgs.m_minimumSolverIslandSize = minimumSolverIslandSize;
879 command->m_updateFlags |= SIM_PARAM_CONSTRAINT_MIN_SOLVER_ISLAND_SIZE;
880 return 0;
881 }
882
b3PhysicsParamSetSolverAnalytics(b3SharedMemoryCommandHandle commandHandle,int reportSolverAnalytics)883 B3_SHARED_API int b3PhysicsParamSetSolverAnalytics(b3SharedMemoryCommandHandle commandHandle, int reportSolverAnalytics)
884 {
885 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
886 b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
887 command->m_physSimParamArgs.m_reportSolverAnalytics = reportSolverAnalytics;
888 command->m_updateFlags |= SIM_PARAM_REPORT_CONSTRAINT_SOLVER_ANALYTICS;
889 return 0;
890 }
891
892
893
b3PhysicsParamSetCollisionFilterMode(b3SharedMemoryCommandHandle commandHandle,int filterMode)894 B3_SHARED_API int b3PhysicsParamSetCollisionFilterMode(b3SharedMemoryCommandHandle commandHandle, int filterMode)
895 {
896 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
897 b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
898 command->m_physSimParamArgs.m_collisionFilterMode = filterMode;
899 command->m_updateFlags |= SIM_PARAM_UPDATE_COLLISION_FILTER_MODE;
900 return 0;
901 }
902
b3PhysicsParamSetTimeStep(b3SharedMemoryCommandHandle commandHandle,double timeStep)903 B3_SHARED_API int b3PhysicsParamSetTimeStep(b3SharedMemoryCommandHandle commandHandle, double timeStep)
904 {
905 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
906 b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
907 command->m_updateFlags |= SIM_PARAM_UPDATE_DELTA_TIME;
908 command->m_physSimParamArgs.m_deltaTime = timeStep;
909 return 0;
910 }
911
b3PhysicsParamSetNumSubSteps(b3SharedMemoryCommandHandle commandHandle,int numSubSteps)912 B3_SHARED_API int b3PhysicsParamSetNumSubSteps(b3SharedMemoryCommandHandle commandHandle, int numSubSteps)
913 {
914 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
915 b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
916 command->m_updateFlags |= SIM_PARAM_UPDATE_NUM_SIMULATION_SUB_STEPS;
917 command->m_physSimParamArgs.m_numSimulationSubSteps = numSubSteps;
918 return 0;
919 }
920
b3PhysicsParamSetDefaultContactERP(b3SharedMemoryCommandHandle commandHandle,double defaultContactERP)921 B3_SHARED_API int b3PhysicsParamSetDefaultContactERP(b3SharedMemoryCommandHandle commandHandle, double defaultContactERP)
922 {
923 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
924 b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
925 command->m_updateFlags |= SIM_PARAM_UPDATE_DEFAULT_CONTACT_ERP;
926 command->m_physSimParamArgs.m_defaultContactERP = defaultContactERP;
927 return 0;
928 }
b3PhysicsParamSetDefaultNonContactERP(b3SharedMemoryCommandHandle commandHandle,double defaultNonContactERP)929 B3_SHARED_API int b3PhysicsParamSetDefaultNonContactERP(b3SharedMemoryCommandHandle commandHandle, double defaultNonContactERP)
930 {
931 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
932 b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
933 command->m_updateFlags |= SIM_PARAM_UPDATE_DEFAULT_NON_CONTACT_ERP;
934 command->m_physSimParamArgs.m_defaultNonContactERP = defaultNonContactERP;
935 return 0;
936 }
b3PhysicsParamSetDefaultFrictionERP(b3SharedMemoryCommandHandle commandHandle,double frictionERP)937 B3_SHARED_API int b3PhysicsParamSetDefaultFrictionERP(b3SharedMemoryCommandHandle commandHandle, double frictionERP)
938 {
939 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
940 b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
941 command->m_updateFlags |= SIM_PARAM_UPDATE_DEFAULT_FRICTION_ERP;
942 command->m_physSimParamArgs.m_frictionERP = frictionERP;
943 return 0;
944 }
945
b3PhysicsParamSetDefaultGlobalCFM(b3SharedMemoryCommandHandle commandHandle,double defaultGlobalCFM)946 B3_SHARED_API int b3PhysicsParamSetDefaultGlobalCFM(b3SharedMemoryCommandHandle commandHandle, double defaultGlobalCFM)
947 {
948 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
949 b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
950 command->m_updateFlags |= SIM_PARAM_UPDATE_DEFAULT_GLOBAL_CFM;
951 command->m_physSimParamArgs.m_defaultGlobalCFM = defaultGlobalCFM;
952 return 0;
953 }
954
b3PhysicsParamSetDefaultFrictionCFM(b3SharedMemoryCommandHandle commandHandle,double frictionCFM)955 B3_SHARED_API int b3PhysicsParamSetDefaultFrictionCFM(b3SharedMemoryCommandHandle commandHandle, double frictionCFM)
956 {
957 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
958 b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
959 command->m_updateFlags |= SIM_PARAM_UPDATE_DEFAULT_FRICTION_CFM;
960 command->m_physSimParamArgs.m_frictionCFM = frictionCFM;
961 return 0;
962 }
963
b3PhysicsParameterSetSparseSdfVoxelSize(b3SharedMemoryCommandHandle commandHandle,double sparseSdfVoxelSize)964 B3_SHARED_API int b3PhysicsParameterSetSparseSdfVoxelSize(b3SharedMemoryCommandHandle commandHandle, double sparseSdfVoxelSize)
965 {
966 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
967 b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
968 command->m_updateFlags |= SIM_PARAM_UPDATE_SPARSE_SDF;
969 command->m_physSimParamArgs.m_sparseSdfVoxelSize = sparseSdfVoxelSize;
970 return 0;
971 }
972
b3InitStepSimulationCommand(b3PhysicsClientHandle physClient)973 B3_SHARED_API b3SharedMemoryCommandHandle b3InitStepSimulationCommand(b3PhysicsClientHandle physClient)
974 {
975 PhysicsClient* cl = (PhysicsClient*)physClient;
976 b3Assert(cl);
977 b3Assert(cl->canSubmitCommand());
978 struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
979 b3Assert(command);
980 return b3InitStepSimulationCommand2((b3SharedMemoryCommandHandle)command);
981 }
982
b3InitStepSimulationCommand2(b3SharedMemoryCommandHandle commandHandle)983 B3_SHARED_API b3SharedMemoryCommandHandle b3InitStepSimulationCommand2(b3SharedMemoryCommandHandle commandHandle)
984 {
985 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
986 command->m_type = CMD_STEP_FORWARD_SIMULATION;
987 command->m_updateFlags = 0;
988 return (b3SharedMemoryCommandHandle)command;
989 }
990
991
b3InitPerformCollisionDetectionCommand(b3PhysicsClientHandle physClient)992 B3_SHARED_API b3SharedMemoryCommandHandle b3InitPerformCollisionDetectionCommand(b3PhysicsClientHandle physClient)
993 {
994 PhysicsClient* cl = (PhysicsClient*)physClient;
995 b3Assert(cl);
996 b3Assert(cl->canSubmitCommand());
997 struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
998 b3Assert(command);
999 command->m_type = CMD_PERFORM_COLLISION_DETECTION;
1000 command->m_updateFlags = 0;
1001 return (b3SharedMemoryCommandHandle)command;
1002 }
1003
b3InitResetSimulationCommand(b3PhysicsClientHandle physClient)1004 B3_SHARED_API b3SharedMemoryCommandHandle b3InitResetSimulationCommand(b3PhysicsClientHandle physClient)
1005 {
1006 PhysicsClient* cl = (PhysicsClient*)physClient;
1007 b3Assert(cl);
1008 b3Assert(cl->canSubmitCommand());
1009 struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
1010 b3Assert(command);
1011 return b3InitResetSimulationCommand2((b3SharedMemoryCommandHandle)command);
1012 }
1013
b3InitResetSimulationCommand2(b3SharedMemoryCommandHandle commandHandle)1014 B3_SHARED_API b3SharedMemoryCommandHandle b3InitResetSimulationCommand2(b3SharedMemoryCommandHandle commandHandle)
1015 {
1016 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
1017 b3Assert(command);
1018 command->m_type = CMD_RESET_SIMULATION;
1019 command->m_updateFlags = 0;
1020 return (b3SharedMemoryCommandHandle)command;
1021 }
1022
b3InitResetSimulationSetFlags(b3SharedMemoryCommandHandle commandHandle,int flags)1023 B3_SHARED_API int b3InitResetSimulationSetFlags(b3SharedMemoryCommandHandle commandHandle, int flags)
1024 {
1025 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
1026 b3Assert(command);
1027 btAssert(command->m_type == CMD_RESET_SIMULATION);
1028 if (command->m_type == CMD_RESET_SIMULATION)
1029 {
1030 command->m_updateFlags = flags;
1031 }
1032 return 0;
1033 }
1034
1035
b3JointControlCommandInit(b3PhysicsClientHandle physClient,int controlMode)1036 B3_SHARED_API b3SharedMemoryCommandHandle b3JointControlCommandInit(b3PhysicsClientHandle physClient, int controlMode)
1037 {
1038 return b3JointControlCommandInit2(physClient, 0, controlMode);
1039 }
1040
b3JointControlCommandInit2(b3PhysicsClientHandle physClient,int bodyUniqueId,int controlMode)1041 B3_SHARED_API b3SharedMemoryCommandHandle b3JointControlCommandInit2(b3PhysicsClientHandle physClient, int bodyUniqueId, int controlMode)
1042 {
1043 PhysicsClient* cl = (PhysicsClient*)physClient;
1044 b3Assert(cl);
1045 b3Assert(cl->canSubmitCommand());
1046 struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
1047 b3Assert(command);
1048 return b3JointControlCommandInit2Internal((b3SharedMemoryCommandHandle)command, bodyUniqueId, controlMode);
1049 }
1050
b3JointControlCommandInit2Internal(b3SharedMemoryCommandHandle commandHandle,int bodyUniqueId,int controlMode)1051 B3_SHARED_API b3SharedMemoryCommandHandle b3JointControlCommandInit2Internal(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int controlMode)
1052 {
1053 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
1054 command->m_type = CMD_SEND_DESIRED_STATE;
1055 command->m_sendDesiredStateCommandArgument.m_controlMode = controlMode;
1056 command->m_sendDesiredStateCommandArgument.m_bodyUniqueId = bodyUniqueId;
1057 command->m_updateFlags = 0;
1058 for (int i = 0; i < MAX_DEGREE_OF_FREEDOM; i++)
1059 {
1060 command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[i] = 0;
1061
1062 }
1063 for (int i = 0; i < 7; i++)
1064 {
1065 command->m_sendDesiredStateCommandArgument.m_desiredStateQ[i] = 0;
1066 command->m_sendDesiredStateCommandArgument.m_desiredStateQdot[i] = 0;
1067 command->m_sendDesiredStateCommandArgument.m_Kp[i] = 0;
1068 command->m_sendDesiredStateCommandArgument.m_Kd[i] = 0;
1069 command->m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[i] = 0;
1070 command->m_sendDesiredStateCommandArgument.m_damping[i] = 0;
1071 }
1072 command->m_sendDesiredStateCommandArgument.m_desiredStateQ[3] = 1;
1073 return (b3SharedMemoryCommandHandle)command;
1074 }
1075
b3JointControlSetDesiredPosition(b3SharedMemoryCommandHandle commandHandle,int qIndex,double value)1076 B3_SHARED_API int b3JointControlSetDesiredPosition(b3SharedMemoryCommandHandle commandHandle, int qIndex, double value)
1077 {
1078 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
1079 b3Assert(command);
1080 if ((qIndex >= 0) && (qIndex < MAX_DEGREE_OF_FREEDOM))
1081 {
1082 command->m_sendDesiredStateCommandArgument.m_desiredStateQ[qIndex] = value;
1083 command->m_updateFlags |= SIM_DESIRED_STATE_HAS_Q;
1084 command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[qIndex] |= SIM_DESIRED_STATE_HAS_Q;
1085 }
1086 return 0;
1087 }
1088
b3JointControlSetDesiredPositionMultiDof(b3SharedMemoryCommandHandle commandHandle,int qIndex,const double * position,int dofCount)1089 B3_SHARED_API int b3JointControlSetDesiredPositionMultiDof(b3SharedMemoryCommandHandle commandHandle, int qIndex, const double* position, int dofCount)
1090 {
1091 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
1092 b3Assert(command);
1093 if ((qIndex >= 0) && ((qIndex + dofCount) < MAX_DEGREE_OF_FREEDOM) && dofCount > 0 && dofCount <= 4)
1094 {
1095 for (int dof = 0; dof < dofCount; dof++)
1096 {
1097 command->m_sendDesiredStateCommandArgument.m_desiredStateQ[qIndex + dof] = position[dof];
1098 command->m_updateFlags |= SIM_DESIRED_STATE_HAS_Q;
1099 command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[qIndex + dof] |= SIM_DESIRED_STATE_HAS_Q;
1100 }
1101 }
1102 return 0;
1103 }
1104
b3JointControlSetKp(b3SharedMemoryCommandHandle commandHandle,int dofIndex,double value)1105 B3_SHARED_API int b3JointControlSetKp(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value)
1106 {
1107 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
1108 b3Assert(command);
1109 if ((dofIndex >= 0) && (dofIndex < MAX_DEGREE_OF_FREEDOM))
1110 {
1111 command->m_sendDesiredStateCommandArgument.m_Kp[dofIndex] = value;
1112 command->m_updateFlags |= SIM_DESIRED_STATE_HAS_KP;
1113 command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_KP;
1114 }
1115 return 0;
1116 }
1117
b3JointControlSetKpMultiDof(b3SharedMemoryCommandHandle commandHandle,int dofIndex,double * kps,int dofCount)1118 B3_SHARED_API int b3JointControlSetKpMultiDof(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double* kps, int dofCount)
1119 {
1120 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
1121 b3Assert(command);
1122 if ((dofIndex >= 0) && (dofIndex < MAX_DEGREE_OF_FREEDOM ) && dofCount >= 0 && dofCount <= 4)
1123 {
1124 for (int dof = 0; dof < dofCount; dof++)
1125 {
1126 command->m_sendDesiredStateCommandArgument.m_Kp[dofIndex + dof] = kps[dof];
1127 command->m_updateFlags |= SIM_DESIRED_STATE_HAS_KP;
1128 command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex + dof] |= SIM_DESIRED_STATE_HAS_KP;
1129 }
1130 }
1131 return 0;
1132 }
1133
b3JointControlSetKd(b3SharedMemoryCommandHandle commandHandle,int dofIndex,double value)1134 B3_SHARED_API int b3JointControlSetKd(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value)
1135 {
1136 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
1137 b3Assert(command);
1138 if ((dofIndex >= 0) && (dofIndex < MAX_DEGREE_OF_FREEDOM))
1139 {
1140 command->m_sendDesiredStateCommandArgument.m_Kd[dofIndex] = value;
1141 command->m_updateFlags |= SIM_DESIRED_STATE_HAS_KD;
1142 command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_KD;
1143 }
1144 return 0;
1145 }
1146
b3JointControlSetKdMultiDof(b3SharedMemoryCommandHandle commandHandle,int dofIndex,double * kds,int dofCount)1147 B3_SHARED_API int b3JointControlSetKdMultiDof(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double* kds, int dofCount)
1148 {
1149 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
1150 b3Assert(command);
1151 if ((dofIndex >= 0) && (dofIndex < MAX_DEGREE_OF_FREEDOM ) && dofCount >= 0 && dofCount <= 4)
1152 {
1153 for (int dof = 0; dof < dofCount; dof++)
1154 {
1155 command->m_sendDesiredStateCommandArgument.m_Kd[dofIndex + dof] = kds[dof];
1156 command->m_updateFlags |= SIM_DESIRED_STATE_HAS_KD;
1157 command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex + dof] |= SIM_DESIRED_STATE_HAS_KD;
1158 }
1159 }
1160 return 0;
1161 }
1162
b3JointControlSetMaximumVelocity(b3SharedMemoryCommandHandle commandHandle,int dofIndex,double maximumVelocity)1163 B3_SHARED_API int b3JointControlSetMaximumVelocity(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double maximumVelocity)
1164 {
1165 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
1166 b3Assert(command);
1167 if ((dofIndex >= 0) && (dofIndex < MAX_DEGREE_OF_FREEDOM))
1168 {
1169 command->m_sendDesiredStateCommandArgument.m_rhsClamp[dofIndex] = maximumVelocity;
1170 command->m_updateFlags |= SIM_DESIRED_STATE_HAS_RHS_CLAMP;
1171 command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_RHS_CLAMP;
1172 }
1173 return 0;
1174 }
1175
b3JointControlSetDesiredVelocity(b3SharedMemoryCommandHandle commandHandle,int dofIndex,double value)1176 B3_SHARED_API int b3JointControlSetDesiredVelocity(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value)
1177 {
1178 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
1179 b3Assert(command);
1180 if ((dofIndex >= 0) && (dofIndex < MAX_DEGREE_OF_FREEDOM))
1181 {
1182 command->m_sendDesiredStateCommandArgument.m_desiredStateQdot[dofIndex] = value;
1183 command->m_updateFlags |= SIM_DESIRED_STATE_HAS_QDOT;
1184 command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_QDOT;
1185 }
1186 return 0;
1187 }
1188
b3JointControlSetDesiredVelocityMultiDof(b3SharedMemoryCommandHandle commandHandle,int dofIndex,const double * velocity,int dofCount)1189 B3_SHARED_API int b3JointControlSetDesiredVelocityMultiDof(b3SharedMemoryCommandHandle commandHandle, int dofIndex, const double* velocity, int dofCount)
1190 {
1191 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
1192 b3Assert(command);
1193 if ((dofIndex >= 0) && ((dofIndex+ dofCount) < MAX_DEGREE_OF_FREEDOM) && dofCount>=0 && dofCount<=4)
1194 {
1195 for (int dof = 0; dof < dofCount; dof++)
1196 {
1197 command->m_sendDesiredStateCommandArgument.m_desiredStateQdot[dofIndex+dof] = velocity[dof];
1198 command->m_updateFlags |= SIM_DESIRED_STATE_HAS_QDOT;
1199 command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex+dof] |= SIM_DESIRED_STATE_HAS_QDOT;
1200 }
1201 }
1202 return 0;
1203 }
1204
b3JointControlSetDesiredForceTorqueMultiDof(b3SharedMemoryCommandHandle commandHandle,int dofIndex,double * forces,int dofCount)1205 B3_SHARED_API int b3JointControlSetDesiredForceTorqueMultiDof(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double* forces, int dofCount)
1206 {
1207 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
1208 b3Assert(command);
1209 if ((dofIndex >= 0) && (dofIndex < MAX_DEGREE_OF_FREEDOM ) && dofCount >= 0 && dofCount <= 4)
1210 {
1211 for (int dof = 0; dof < dofCount; dof++)
1212 {
1213 command->m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex+dof] = forces[dof];
1214 command->m_updateFlags |= SIM_DESIRED_STATE_HAS_MAX_FORCE;
1215 command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex + dof] |= SIM_DESIRED_STATE_HAS_MAX_FORCE;
1216 }
1217 }
1218 return 0;
1219 }
1220
b3JointControlSetDamping(b3SharedMemoryCommandHandle commandHandle,int dofIndex,double value)1221 B3_SHARED_API int b3JointControlSetDamping(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value)
1222 {
1223 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
1224 b3Assert(command);
1225 if ((dofIndex >= 0) && (dofIndex < MAX_DEGREE_OF_FREEDOM))
1226 {
1227 command->m_sendDesiredStateCommandArgument.m_damping[dofIndex] = value;
1228 command->m_updateFlags |= SIM_DESIRED_STATE_HAS_DAMPING;
1229 command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_DAMPING;
1230 }
1231 return 0;
1232 }
1233
b3JointControlSetDampingMultiDof(b3SharedMemoryCommandHandle commandHandle,int dofIndex,double * damping,int dofCount)1234 B3_SHARED_API int b3JointControlSetDampingMultiDof(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double* damping, int dofCount)
1235 {
1236 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
1237 b3Assert(command);
1238 if ((dofIndex >= 0) && (dofIndex < MAX_DEGREE_OF_FREEDOM ) && dofCount >= 0 && dofCount <= 4)
1239 {
1240 for (int dof = 0; dof < dofCount; dof++)
1241 {
1242 command->m_sendDesiredStateCommandArgument.m_damping[dofIndex+dof] = damping[dof];
1243 command->m_updateFlags |= SIM_DESIRED_STATE_HAS_DAMPING;
1244 command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex + dof] |= SIM_DESIRED_STATE_HAS_DAMPING;
1245 }
1246 }
1247 return 0;
1248 }
1249
b3JointControlSetMaximumForce(b3SharedMemoryCommandHandle commandHandle,int dofIndex,double value)1250 B3_SHARED_API int b3JointControlSetMaximumForce(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value)
1251 {
1252 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
1253 b3Assert(command);
1254 if ((dofIndex >= 0) && (dofIndex < MAX_DEGREE_OF_FREEDOM))
1255 {
1256 command->m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex] = value;
1257 command->m_updateFlags |= SIM_DESIRED_STATE_HAS_MAX_FORCE;
1258 command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_MAX_FORCE;
1259 }
1260 return 0;
1261 }
1262
b3JointControlSetDesiredForceTorque(b3SharedMemoryCommandHandle commandHandle,int dofIndex,double value)1263 B3_SHARED_API int b3JointControlSetDesiredForceTorque(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value)
1264 {
1265 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
1266 b3Assert(command);
1267 if ((dofIndex >= 0) && (dofIndex < MAX_DEGREE_OF_FREEDOM))
1268 {
1269 command->m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex] = value;
1270 command->m_updateFlags |= SIM_DESIRED_STATE_HAS_MAX_FORCE;
1271 command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_MAX_FORCE;
1272 }
1273 return 0;
1274 }
1275
b3RequestActualStateCommandInit(b3PhysicsClientHandle physClient,int bodyUniqueId)1276 B3_SHARED_API b3SharedMemoryCommandHandle b3RequestActualStateCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId)
1277 {
1278 PhysicsClient* cl = (PhysicsClient*)physClient;
1279 b3Assert(cl);
1280 b3Assert(cl->canSubmitCommand());
1281 struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
1282 b3Assert(command);
1283 return b3RequestActualStateCommandInit2((b3SharedMemoryCommandHandle)command, bodyUniqueId);
1284 }
1285
b3RequestActualStateCommandInit2(b3SharedMemoryCommandHandle commandHandle,int bodyUniqueId)1286 B3_SHARED_API b3SharedMemoryCommandHandle b3RequestActualStateCommandInit2(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId)
1287 {
1288 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
1289 command->m_type = CMD_REQUEST_ACTUAL_STATE;
1290 command->m_updateFlags = 0;
1291 command->m_requestActualStateInformationCommandArgument.m_bodyUniqueId = bodyUniqueId;
1292 return (b3SharedMemoryCommandHandle)command;
1293 }
1294
b3RequestActualStateCommandComputeLinkVelocity(b3SharedMemoryCommandHandle commandHandle,int computeLinkVelocity)1295 B3_SHARED_API int b3RequestActualStateCommandComputeLinkVelocity(b3SharedMemoryCommandHandle commandHandle, int computeLinkVelocity)
1296 {
1297 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
1298 b3Assert(command);
1299 btAssert(command->m_type == CMD_REQUEST_ACTUAL_STATE);
1300 if (computeLinkVelocity && command->m_type == CMD_REQUEST_ACTUAL_STATE)
1301 {
1302 command->m_updateFlags |= ACTUAL_STATE_COMPUTE_LINKVELOCITY;
1303 }
1304 return 0;
1305 }
1306
b3RequestActualStateCommandComputeForwardKinematics(b3SharedMemoryCommandHandle commandHandle,int computeForwardKinematics)1307 B3_SHARED_API int b3RequestActualStateCommandComputeForwardKinematics(b3SharedMemoryCommandHandle commandHandle, int computeForwardKinematics)
1308 {
1309 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
1310 b3Assert(command);
1311 btAssert(command->m_type == CMD_REQUEST_ACTUAL_STATE);
1312 if (computeForwardKinematics && command->m_type == CMD_REQUEST_ACTUAL_STATE)
1313 {
1314 command->m_updateFlags |= ACTUAL_STATE_COMPUTE_FORWARD_KINEMATICS;
1315 }
1316 return 0;
1317 }
1318
b3GetJointState(b3PhysicsClientHandle physClient,b3SharedMemoryStatusHandle statusHandle,int jointIndex,b3JointSensorState * state)1319 B3_SHARED_API int b3GetJointState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int jointIndex, b3JointSensorState* state)
1320 {
1321
1322
1323
1324
1325 const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
1326 b3Assert(status);
1327 int bodyIndex = status->m_sendActualStateArgs.m_bodyUniqueId;
1328 b3Assert(bodyIndex >= 0);
1329 if (bodyIndex >= 0)
1330 {
1331 b3JointInfo info;
1332
1333 bool result = b3GetJointInfo(physClient, bodyIndex, jointIndex, &info) != 0;
1334 if (result && status->m_sendActualStateArgs.m_stateDetails)
1335 {
1336 if ((info.m_qIndex >= 0) && (info.m_uIndex >= 0) && (info.m_qIndex < MAX_DEGREE_OF_FREEDOM) && (info.m_uIndex < MAX_DEGREE_OF_FREEDOM))
1337 {
1338 state->m_jointPosition = status->m_sendActualStateArgs.m_stateDetails->m_actualStateQ[info.m_qIndex];
1339 state->m_jointVelocity = status->m_sendActualStateArgs.m_stateDetails->m_actualStateQdot[info.m_uIndex];
1340 }
1341 else
1342 {
1343 state->m_jointPosition = 0;
1344 state->m_jointVelocity = 0;
1345 }
1346 for (int ii(0); ii < 6; ++ii)
1347 {
1348 state->m_jointForceTorque[ii] = status->m_sendActualStateArgs.m_stateDetails->m_jointReactionForces[6 * jointIndex + ii];
1349 }
1350 state->m_jointMotorTorque = status->m_sendActualStateArgs.m_stateDetails->m_jointMotorForce[jointIndex];
1351 return 1;
1352 }
1353 }
1354 return 0;
1355 }
1356
b3GetJointStateMultiDof(b3PhysicsClientHandle physClient,b3SharedMemoryStatusHandle statusHandle,int jointIndex,b3JointSensorState2 * state)1357 B3_SHARED_API int b3GetJointStateMultiDof(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int jointIndex, b3JointSensorState2* state)
1358 {
1359 const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
1360 b3Assert(status);
1361 int bodyIndex = status->m_sendActualStateArgs.m_bodyUniqueId;
1362 b3Assert(bodyIndex >= 0);
1363 if (bodyIndex >= 0)
1364 {
1365 state->m_qDofSize = 0;
1366 state->m_uDofSize = 0;
1367 b3JointInfo info;
1368 bool result = b3GetJointInfo(physClient, bodyIndex, jointIndex, &info) != 0;
1369 if (result)
1370 {
1371
1372 if ((info.m_qIndex >= 0) && (info.m_uIndex >= 0) && (info.m_qIndex < MAX_DEGREE_OF_FREEDOM) && (info.m_uIndex < MAX_DEGREE_OF_FREEDOM))
1373 {
1374 state->m_qDofSize = info.m_qSize;
1375 state->m_uDofSize = info.m_uSize;
1376 for (int i = 0; i < state->m_qDofSize; i++)
1377 {
1378 state->m_jointPosition[i] = status->m_sendActualStateArgs.m_stateDetails->m_actualStateQ[info.m_qIndex + i];
1379 }
1380 for (int i = 0; i < state->m_uDofSize; i++)
1381 {
1382 state->m_jointVelocity[i] = status->m_sendActualStateArgs.m_stateDetails->m_actualStateQdot[info.m_uIndex+i];
1383 state->m_jointMotorTorqueMultiDof[i] = status->m_sendActualStateArgs.m_stateDetails->m_jointMotorForceMultiDof[info.m_uIndex + i];
1384 }
1385 }
1386 else
1387 {
1388 state->m_jointPosition[0] = 0;
1389 state->m_jointVelocity[0] = 0;
1390 }
1391 for (int ii(0); ii < 6; ++ii)
1392 {
1393 state->m_jointReactionForceTorque[ii] = status->m_sendActualStateArgs.m_stateDetails->m_jointReactionForces[6 * jointIndex + ii];
1394 }
1395
1396 return 1;
1397 }
1398 }
1399 return 0;
1400 }
1401
1402
b3GetLinkState(b3PhysicsClientHandle physClient,b3SharedMemoryStatusHandle statusHandle,int linkIndex,b3LinkState * state)1403 B3_SHARED_API int b3GetLinkState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int linkIndex, b3LinkState* state)
1404 {
1405 const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
1406 b3Assert(status);
1407 int bodyIndex = status->m_sendActualStateArgs.m_bodyUniqueId;
1408 b3Assert(bodyIndex >= 0);
1409 b3Assert(linkIndex >= 0);
1410 int numLinks = status->m_sendActualStateArgs.m_numLinks;
1411 b3Assert(linkIndex < numLinks);
1412
1413 if (status->m_sendActualStateArgs.m_stateDetails != NULL && (bodyIndex >= 0) && (linkIndex >= 0) && linkIndex < numLinks)
1414 {
1415 b3Transform wlf, com, inertial;
1416
1417 for (int i = 0; i < 3; ++i)
1418 {
1419 state->m_worldPosition[i] = status->m_sendActualStateArgs.m_stateDetails->m_linkState[7 * linkIndex + i];
1420 state->m_localInertialPosition[i] = status->m_sendActualStateArgs.m_stateDetails->m_linkLocalInertialFrames[7 * linkIndex + i];
1421 state->m_worldLinearVelocity[i] = status->m_sendActualStateArgs.m_stateDetails->m_linkWorldVelocities[6 * linkIndex + i];
1422 state->m_worldAngularVelocity[i] = status->m_sendActualStateArgs.m_stateDetails->m_linkWorldVelocities[6 * linkIndex + i + 3];
1423 }
1424 for (int i = 0; i < 4; ++i)
1425 {
1426 state->m_worldOrientation[i] = status->m_sendActualStateArgs.m_stateDetails->m_linkState[7 * linkIndex + 3 + i];
1427 state->m_localInertialOrientation[i] = status->m_sendActualStateArgs.m_stateDetails->m_linkLocalInertialFrames[7 * linkIndex + 3 + i];
1428 }
1429 com.setOrigin(b3MakeVector3(state->m_worldPosition[0], state->m_worldPosition[1], state->m_worldPosition[2]));
1430 com.setRotation(b3Quaternion(state->m_worldOrientation[0], state->m_worldOrientation[1], state->m_worldOrientation[2], state->m_worldOrientation[3]));
1431 inertial.setOrigin(b3MakeVector3(state->m_localInertialPosition[0], state->m_localInertialPosition[1], state->m_localInertialPosition[2]));
1432 inertial.setRotation(b3Quaternion(state->m_localInertialOrientation[0], state->m_localInertialOrientation[1], state->m_localInertialOrientation[2], state->m_localInertialOrientation[3]));
1433 wlf = com * inertial.inverse();
1434 for (int i = 0; i < 3; ++i)
1435 {
1436 state->m_worldLinkFramePosition[i] = wlf.getOrigin()[i];
1437 }
1438 b3Quaternion wlfOrn = wlf.getRotation();
1439 for (int i = 0; i < 4; ++i)
1440 {
1441 state->m_worldLinkFrameOrientation[i] = wlfOrn[i];
1442 }
1443 return 1;
1444 }
1445 return 0;
1446 }
1447
b3CreateCollisionShapeCommandInit(b3PhysicsClientHandle physClient)1448 B3_SHARED_API b3SharedMemoryCommandHandle b3CreateCollisionShapeCommandInit(b3PhysicsClientHandle physClient)
1449 {
1450 PhysicsClient* cl = (PhysicsClient*)physClient;
1451 b3Assert(cl);
1452 b3Assert(cl->canSubmitCommand());
1453 if (cl)
1454 {
1455 struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
1456 b3Assert(command);
1457 command->m_type = CMD_CREATE_COLLISION_SHAPE;
1458 command->m_updateFlags = 0;
1459 command->m_createUserShapeArgs.m_numUserShapes = 0;
1460 return (b3SharedMemoryCommandHandle)command;
1461 }
1462 return 0;
1463 }
1464
b3CreateCollisionShapeAddSphere(b3SharedMemoryCommandHandle commandHandle,double radius)1465 B3_SHARED_API int b3CreateCollisionShapeAddSphere(b3SharedMemoryCommandHandle commandHandle, double radius)
1466 {
1467 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
1468 b3Assert(command);
1469 b3Assert((command->m_type == CMD_CREATE_COLLISION_SHAPE) || (command->m_type == CMD_CREATE_VISUAL_SHAPE));
1470 if ((command->m_type == CMD_CREATE_COLLISION_SHAPE) || (command->m_type == CMD_CREATE_VISUAL_SHAPE))
1471 {
1472 int shapeIndex = command->m_createUserShapeArgs.m_numUserShapes;
1473 if (shapeIndex < MAX_COMPOUND_COLLISION_SHAPES)
1474 {
1475 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_type = GEOM_SPHERE;
1476 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_collisionFlags = 0;
1477 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_visualFlags = 0;
1478 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_hasChildTransform = 0;
1479 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_sphereRadius = radius;
1480 command->m_createUserShapeArgs.m_numUserShapes++;
1481 return shapeIndex;
1482 }
1483 }
1484 return -1;
1485 }
1486
b3ResetMeshDataCommandInit(b3PhysicsClientHandle physClient,int bodyUniqueId,int numVertices,const double * vertices)1487 B3_SHARED_API b3SharedMemoryCommandHandle b3ResetMeshDataCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, int numVertices, const double* vertices)
1488 {
1489 PhysicsClient* cl = (PhysicsClient*)physClient;
1490 b3Assert(cl);
1491 b3Assert(cl->canSubmitCommand());
1492 if (cl)
1493 {
1494 struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
1495 b3Assert(command);
1496 command->m_type = CMD_RESET_MESH_DATA;
1497 command->m_updateFlags = 0;
1498 command->m_resetMeshDataArgs.m_numVertices = numVertices;
1499 command->m_resetMeshDataArgs.m_bodyUniqueId = bodyUniqueId;
1500 command->m_resetMeshDataArgs.m_flags = 0;
1501 int totalUploadSizeInBytes = numVertices * sizeof(double) *3;
1502 cl->uploadBulletFileToSharedMemory((const char*)vertices, totalUploadSizeInBytes);
1503 return (b3SharedMemoryCommandHandle)command;
1504 }
1505 return 0;
1506 }
1507
1508
b3GetMeshDataCommandInit(b3PhysicsClientHandle physClient,int bodyUniqueId,int linkIndex)1509 B3_SHARED_API b3SharedMemoryCommandHandle b3GetMeshDataCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex)
1510 {
1511 PhysicsClient* cl = (PhysicsClient*)physClient;
1512 b3Assert(cl);
1513 b3Assert(cl->canSubmitCommand());
1514 if (cl)
1515 {
1516 struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
1517 b3Assert(command);
1518 command->m_type = CMD_REQUEST_MESH_DATA;
1519 command->m_updateFlags = 0;
1520 command->m_requestMeshDataArgs.m_startingVertex = 0;
1521 command->m_requestMeshDataArgs.m_bodyUniqueId = bodyUniqueId;
1522 command->m_requestMeshDataArgs.m_linkIndex = linkIndex;
1523 return (b3SharedMemoryCommandHandle)command;
1524 }
1525 return 0;
1526 }
1527
b3GetMeshDataSetCollisionShapeIndex(b3SharedMemoryCommandHandle commandHandle,int shapeIndex)1528 B3_SHARED_API void b3GetMeshDataSetCollisionShapeIndex(b3SharedMemoryCommandHandle commandHandle, int shapeIndex)
1529 {
1530 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
1531 b3Assert(command);
1532 b3Assert(command->m_type == CMD_REQUEST_MESH_DATA);
1533 if (command->m_type == CMD_REQUEST_MESH_DATA)
1534 {
1535 command->m_updateFlags = B3_MESH_DATA_COLLISIONSHAPEINDEX;
1536 command->m_requestMeshDataArgs.m_collisionShapeIndex = shapeIndex;
1537 }
1538 }
1539
b3GetMeshDataSetFlags(b3SharedMemoryCommandHandle commandHandle,int flags)1540 B3_SHARED_API void b3GetMeshDataSetFlags(b3SharedMemoryCommandHandle commandHandle, int flags)
1541 {
1542 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
1543 b3Assert(command);
1544 b3Assert(command->m_type == CMD_REQUEST_MESH_DATA);
1545 if (command->m_type == CMD_REQUEST_MESH_DATA)
1546 {
1547 command->m_updateFlags = B3_MESH_DATA_FLAGS;
1548 command->m_requestMeshDataArgs.m_flags = flags;
1549 }
1550 }
1551
b3GetMeshData(b3PhysicsClientHandle physClient,struct b3MeshData * meshData)1552 B3_SHARED_API void b3GetMeshData(b3PhysicsClientHandle physClient, struct b3MeshData* meshData)
1553 {
1554 PhysicsClient* cl = (PhysicsClient*)physClient;
1555 if (cl)
1556 {
1557 cl->getCachedMeshData(meshData);
1558 }
1559 }
1560
b3CreateVisualShapeAddSphere(b3SharedMemoryCommandHandle commandHandle,double radius)1561 B3_SHARED_API int b3CreateVisualShapeAddSphere(b3SharedMemoryCommandHandle commandHandle, double radius)
1562 {
1563 return b3CreateCollisionShapeAddSphere(commandHandle, radius);
1564 }
1565
b3CreateCollisionShapeAddBox(b3SharedMemoryCommandHandle commandHandle,const double halfExtents[3])1566 B3_SHARED_API int b3CreateCollisionShapeAddBox(b3SharedMemoryCommandHandle commandHandle, const double halfExtents[3])
1567 {
1568 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
1569 b3Assert(command);
1570 b3Assert((command->m_type == CMD_CREATE_COLLISION_SHAPE) || (command->m_type == CMD_CREATE_VISUAL_SHAPE));
1571 if ((command->m_type == CMD_CREATE_COLLISION_SHAPE) || (command->m_type == CMD_CREATE_VISUAL_SHAPE))
1572 {
1573 int shapeIndex = command->m_createUserShapeArgs.m_numUserShapes;
1574 if (shapeIndex < MAX_COMPOUND_COLLISION_SHAPES)
1575 {
1576 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_type = GEOM_BOX;
1577 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_collisionFlags = 0;
1578 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_visualFlags = 0;
1579 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_hasChildTransform = 0;
1580 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_boxHalfExtents[0] = halfExtents[0];
1581 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_boxHalfExtents[1] = halfExtents[1];
1582 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_boxHalfExtents[2] = halfExtents[2];
1583 command->m_createUserShapeArgs.m_numUserShapes++;
1584 return shapeIndex;
1585 }
1586 }
1587 return -1;
1588 }
1589
b3CreateVisualShapeAddBox(b3SharedMemoryCommandHandle commandHandle,const double halfExtents[])1590 B3_SHARED_API int b3CreateVisualShapeAddBox(b3SharedMemoryCommandHandle commandHandle, const double halfExtents[/*3*/])
1591 {
1592 return b3CreateCollisionShapeAddBox(commandHandle, halfExtents);
1593 }
1594
b3CreateCollisionShapeAddCapsule(b3SharedMemoryCommandHandle commandHandle,double radius,double height)1595 B3_SHARED_API int b3CreateCollisionShapeAddCapsule(b3SharedMemoryCommandHandle commandHandle, double radius, double height)
1596 {
1597 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
1598 b3Assert(command);
1599 b3Assert((command->m_type == CMD_CREATE_COLLISION_SHAPE) || (command->m_type == CMD_CREATE_VISUAL_SHAPE));
1600 if ((command->m_type == CMD_CREATE_COLLISION_SHAPE) || (command->m_type == CMD_CREATE_VISUAL_SHAPE))
1601 {
1602 int shapeIndex = command->m_createUserShapeArgs.m_numUserShapes;
1603 if (shapeIndex < MAX_COMPOUND_COLLISION_SHAPES)
1604 {
1605 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_type = GEOM_CAPSULE;
1606 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_collisionFlags = 0;
1607 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_visualFlags = 0;
1608 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_hasChildTransform = 0;
1609 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_hasFromTo = 0;
1610 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_capsuleRadius = radius;
1611 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_capsuleHeight = height;
1612 command->m_createUserShapeArgs.m_numUserShapes++;
1613 return shapeIndex;
1614 }
1615 }
1616 return -1;
1617 }
1618
b3CreateVisualShapeAddCapsule(b3SharedMemoryCommandHandle commandHandle,double radius,double height)1619 B3_SHARED_API int b3CreateVisualShapeAddCapsule(b3SharedMemoryCommandHandle commandHandle, double radius, double height)
1620 {
1621 return b3CreateCollisionShapeAddCapsule(commandHandle, radius, height);
1622 }
1623
b3CreateCollisionShapeAddHeightfield(b3SharedMemoryCommandHandle commandHandle,const char * fileName,const double meshScale[],double textureScaling)1624 B3_SHARED_API int b3CreateCollisionShapeAddHeightfield(b3SharedMemoryCommandHandle commandHandle, const char* fileName, const double meshScale[/*3*/], double textureScaling)
1625 {
1626 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
1627 b3Assert(command);
1628 b3Assert((command->m_type == CMD_CREATE_COLLISION_SHAPE) || (command->m_type == CMD_CREATE_VISUAL_SHAPE));
1629 if ((command->m_type == CMD_CREATE_COLLISION_SHAPE) || (command->m_type == CMD_CREATE_VISUAL_SHAPE))
1630 {
1631 int shapeIndex = command->m_createUserShapeArgs.m_numUserShapes;
1632 if (shapeIndex < MAX_COMPOUND_COLLISION_SHAPES)
1633 {
1634 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_type = GEOM_HEIGHTFIELD;
1635 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_collisionFlags = 0;
1636 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_visualFlags = 0;
1637 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_hasChildTransform = 0;
1638 strcpy(command->m_createUserShapeArgs.m_shapes[shapeIndex].m_meshFileName, fileName);
1639 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_meshScale[0] = meshScale[0];
1640 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_meshScale[1] = meshScale[1];
1641 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_meshScale[2] = meshScale[2];
1642 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_heightfieldTextureScaling = textureScaling;
1643 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_numHeightfieldRows = -1;
1644 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_numHeightfieldColumns = -1;
1645 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_replaceHeightfieldIndex = -1;
1646
1647 command->m_createUserShapeArgs.m_numUserShapes++;
1648 return shapeIndex;
1649 }
1650 }
1651 return -1;
1652 }
1653
b3CreateCollisionShapeAddHeightfield2(b3PhysicsClientHandle physClient,b3SharedMemoryCommandHandle commandHandle,const double meshScale[],double textureScaling,float * heightfieldData,int numHeightfieldRows,int numHeightfieldColumns,int replaceHeightfieldIndex)1654 B3_SHARED_API int b3CreateCollisionShapeAddHeightfield2(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, const double meshScale[/*3*/], double textureScaling, float* heightfieldData, int numHeightfieldRows, int numHeightfieldColumns, int replaceHeightfieldIndex)
1655 {
1656 PhysicsClient* cl = (PhysicsClient*)physClient;
1657 b3Assert(cl);
1658 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
1659 b3Assert(command);
1660 b3Assert((command->m_type == CMD_CREATE_COLLISION_SHAPE) || (command->m_type == CMD_CREATE_VISUAL_SHAPE));
1661 if ((command->m_type == CMD_CREATE_COLLISION_SHAPE) || (command->m_type == CMD_CREATE_VISUAL_SHAPE))
1662 {
1663 int shapeIndex = command->m_createUserShapeArgs.m_numUserShapes;
1664 if (shapeIndex < MAX_COMPOUND_COLLISION_SHAPES)
1665 {
1666 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_type = GEOM_HEIGHTFIELD;
1667 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_collisionFlags = 0;
1668 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_visualFlags = 0;
1669 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_hasChildTransform = 0;
1670 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_meshFileName[0] = 0;
1671 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_meshScale[0] = meshScale[0];
1672 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_meshScale[1] = meshScale[1];
1673 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_meshScale[2] = meshScale[2];
1674 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_heightfieldTextureScaling = textureScaling;
1675 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_numHeightfieldRows = numHeightfieldRows;
1676 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_numHeightfieldColumns = numHeightfieldColumns;
1677 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_replaceHeightfieldIndex = replaceHeightfieldIndex;
1678 cl->uploadBulletFileToSharedMemory((const char*)heightfieldData, numHeightfieldRows*numHeightfieldColumns* sizeof(float));
1679 command->m_createUserShapeArgs.m_numUserShapes++;
1680 return shapeIndex;
1681 }
1682 }
1683 return -1;
1684 }
1685
b3CreateCollisionShapeAddCylinder(b3SharedMemoryCommandHandle commandHandle,double radius,double height)1686 B3_SHARED_API int b3CreateCollisionShapeAddCylinder(b3SharedMemoryCommandHandle commandHandle, double radius, double height)
1687 {
1688 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
1689 b3Assert(command);
1690 b3Assert((command->m_type == CMD_CREATE_COLLISION_SHAPE) || (command->m_type == CMD_CREATE_VISUAL_SHAPE));
1691 if ((command->m_type == CMD_CREATE_COLLISION_SHAPE) || (command->m_type == CMD_CREATE_VISUAL_SHAPE))
1692 {
1693 int shapeIndex = command->m_createUserShapeArgs.m_numUserShapes;
1694 if (shapeIndex < MAX_COMPOUND_COLLISION_SHAPES)
1695 {
1696 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_type = GEOM_CYLINDER;
1697 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_collisionFlags = 0;
1698 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_visualFlags = 0;
1699 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_hasChildTransform = 0;
1700 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_capsuleRadius = radius;
1701 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_capsuleHeight = height;
1702 command->m_createUserShapeArgs.m_numUserShapes++;
1703 return shapeIndex;
1704 }
1705 }
1706 return -1;
1707 }
b3CreateVisualShapeAddCylinder(b3SharedMemoryCommandHandle commandHandle,double radius,double height)1708 B3_SHARED_API int b3CreateVisualShapeAddCylinder(b3SharedMemoryCommandHandle commandHandle, double radius, double height)
1709 {
1710 return b3CreateCollisionShapeAddCylinder(commandHandle, radius, height);
1711 }
1712
b3CreateCollisionShapeAddPlane(b3SharedMemoryCommandHandle commandHandle,const double planeNormal[3],double planeConstant)1713 B3_SHARED_API int b3CreateCollisionShapeAddPlane(b3SharedMemoryCommandHandle commandHandle, const double planeNormal[3], double planeConstant)
1714 {
1715 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
1716 b3Assert(command);
1717 b3Assert((command->m_type == CMD_CREATE_COLLISION_SHAPE) || (command->m_type == CMD_CREATE_VISUAL_SHAPE));
1718 if ((command->m_type == CMD_CREATE_COLLISION_SHAPE) || (command->m_type == CMD_CREATE_VISUAL_SHAPE))
1719 {
1720 int shapeIndex = command->m_createUserShapeArgs.m_numUserShapes;
1721 if (shapeIndex < MAX_COMPOUND_COLLISION_SHAPES)
1722 {
1723 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_type = GEOM_PLANE;
1724 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_collisionFlags = 0;
1725 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_visualFlags = 0;
1726 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_hasChildTransform = 0;
1727 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_planeNormal[0] = planeNormal[0];
1728 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_planeNormal[1] = planeNormal[1];
1729 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_planeNormal[2] = planeNormal[2];
1730 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_planeConstant = planeConstant;
1731 command->m_createUserShapeArgs.m_numUserShapes++;
1732 return shapeIndex;
1733 }
1734 }
1735 return -1;
1736 }
b3CreateVisualShapeAddPlane(b3SharedMemoryCommandHandle commandHandle,const double planeNormal[],double planeConstant)1737 B3_SHARED_API int b3CreateVisualShapeAddPlane(b3SharedMemoryCommandHandle commandHandle, const double planeNormal[/*3*/], double planeConstant)
1738 {
1739 return b3CreateCollisionShapeAddPlane(commandHandle, planeNormal, planeConstant);
1740 }
b3CreateCollisionShapeAddMesh(b3SharedMemoryCommandHandle commandHandle,const char * fileName,const double meshScale[3])1741 B3_SHARED_API int b3CreateCollisionShapeAddMesh(b3SharedMemoryCommandHandle commandHandle, const char* fileName, const double meshScale[3])
1742 {
1743 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
1744 b3Assert(command);
1745 b3Assert((command->m_type == CMD_CREATE_COLLISION_SHAPE) || (command->m_type == CMD_CREATE_VISUAL_SHAPE));
1746 if ((command->m_type == CMD_CREATE_COLLISION_SHAPE) || (command->m_type == CMD_CREATE_VISUAL_SHAPE))
1747 {
1748 int shapeIndex = command->m_createUserShapeArgs.m_numUserShapes;
1749 if (shapeIndex < MAX_COMPOUND_COLLISION_SHAPES && strlen(fileName) < VISUAL_SHAPE_MAX_PATH_LEN)
1750 {
1751 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_type = GEOM_MESH;
1752 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_collisionFlags = 0;
1753 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_visualFlags = 0;
1754 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_hasChildTransform = 0;
1755 strcpy(command->m_createUserShapeArgs.m_shapes[shapeIndex].m_meshFileName, fileName);
1756 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_meshScale[0] = meshScale[0];
1757 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_meshScale[1] = meshScale[1];
1758 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_meshScale[2] = meshScale[2];
1759 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_meshFileType = 0;
1760 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_numVertices = 0;
1761 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_numIndices = 0;
1762
1763 command->m_createUserShapeArgs.m_numUserShapes++;
1764 return shapeIndex;
1765 }
1766 }
1767 return -1;
1768 }
1769
b3CreateCollisionShapeAddConvexMesh(b3PhysicsClientHandle physClient,b3SharedMemoryCommandHandle commandHandle,const double meshScale[],const double * vertices,int numVertices)1770 B3_SHARED_API int b3CreateCollisionShapeAddConvexMesh(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, const double meshScale[/*3*/], const double* vertices, int numVertices)
1771 {
1772 PhysicsClient* cl = (PhysicsClient*)physClient;
1773 b3Assert(cl);
1774
1775 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
1776 b3Assert(command);
1777 b3Assert((command->m_type == CMD_CREATE_COLLISION_SHAPE) || (command->m_type == CMD_CREATE_VISUAL_SHAPE));
1778 if ((command->m_type == CMD_CREATE_COLLISION_SHAPE) || (command->m_type == CMD_CREATE_VISUAL_SHAPE))
1779 {
1780 int shapeIndex = command->m_createUserShapeArgs.m_numUserShapes;
1781 if (shapeIndex < MAX_COMPOUND_COLLISION_SHAPES && numVertices >= 0)
1782 {
1783 int i=0;
1784 if (numVertices>B3_MAX_NUM_VERTICES)
1785 numVertices=B3_MAX_NUM_VERTICES;
1786 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_type = GEOM_MESH;
1787 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_collisionFlags = 0;
1788 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_visualFlags = 0;
1789 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_hasChildTransform = 0;
1790 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_meshScale[0] = meshScale[0];
1791 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_meshScale[1] = meshScale[1];
1792 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_meshScale[2] = meshScale[2];
1793 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_meshFileType = 0;
1794 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_meshFileName[0]=0;
1795 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_numVertices = numVertices;
1796 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_numIndices = 0;
1797
1798 cl->uploadBulletFileToSharedMemory((const char*)vertices, numVertices * sizeof(double)*3);
1799 command->m_createUserShapeArgs.m_numUserShapes++;
1800 return shapeIndex;
1801 }
1802 }
1803 return -1;
1804 }
1805
b3CreateCollisionShapeAddConcaveMesh(b3PhysicsClientHandle physClient,b3SharedMemoryCommandHandle commandHandle,const double meshScale[],const double * vertices,int numVertices,const int * indices,int numIndices)1806 B3_SHARED_API int b3CreateCollisionShapeAddConcaveMesh(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, const double meshScale[/*3*/], const double* vertices, int numVertices, const int* indices, int numIndices)
1807 {
1808 PhysicsClient* cl = (PhysicsClient*)physClient;
1809 b3Assert(cl);
1810 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
1811 b3Assert(command);
1812 b3Assert((command->m_type == CMD_CREATE_COLLISION_SHAPE) || (command->m_type == CMD_CREATE_VISUAL_SHAPE));
1813 if ((command->m_type == CMD_CREATE_COLLISION_SHAPE) || (command->m_type == CMD_CREATE_VISUAL_SHAPE))
1814 {
1815 int shapeIndex = command->m_createUserShapeArgs.m_numUserShapes;
1816 if (shapeIndex < MAX_COMPOUND_COLLISION_SHAPES && numVertices >= 0 && numIndices >=0)
1817 {
1818 int i=0;
1819 if (numVertices>B3_MAX_NUM_VERTICES)
1820 numVertices=B3_MAX_NUM_VERTICES;
1821 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_type = GEOM_MESH;
1822 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_collisionFlags = GEOM_FORCE_CONCAVE_TRIMESH;
1823 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_visualFlags = 0;
1824 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_hasChildTransform = 0;
1825 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_meshScale[0] = meshScale[0];
1826 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_meshScale[1] = meshScale[1];
1827 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_meshScale[2] = meshScale[2];
1828 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_meshFileType = 0;
1829 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_meshFileName[0]=0;
1830 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_numVertices = numVertices;
1831
1832 int totalUploadSizeInBytes = numVertices * sizeof(double) *3 + numIndices * sizeof(int);
1833 char* data = new char[totalUploadSizeInBytes];
1834 double* vertexUpload = (double*)data;
1835 int* indexUpload = (int*)(data + numVertices*sizeof(double)*3);
1836
1837 for (i=0;i<numVertices;i++)
1838 {
1839 vertexUpload[i*3+0]=vertices[i*3+0];
1840 vertexUpload[i*3+1]=vertices[i*3+1];
1841 vertexUpload[i*3+2]=vertices[i*3+2];
1842 }
1843 if (numIndices>B3_MAX_NUM_INDICES)
1844 numIndices = B3_MAX_NUM_INDICES;
1845
1846 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_numIndices = numIndices;
1847 for (i=0;i<numIndices;i++)
1848 {
1849 indexUpload[i]=indices[i];
1850 }
1851 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_numNormals = 0;
1852 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_numUVs = 0;
1853
1854 command->m_createUserShapeArgs.m_numUserShapes++;
1855 cl->uploadBulletFileToSharedMemory(data, totalUploadSizeInBytes);
1856 delete [] data;
1857 return shapeIndex;
1858 }
1859 }
1860 return -1;
1861 }
1862
b3CreateVisualShapeAddMesh2(b3PhysicsClientHandle physClient,b3SharedMemoryCommandHandle commandHandle,const double meshScale[],const double * vertices,int numVertices,const int * indices,int numIndices,const double * normals,int numNormals,const double * uvs,int numUVs)1863 B3_SHARED_API int b3CreateVisualShapeAddMesh2(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, const double meshScale[/*3*/], const double* vertices, int numVertices, const int* indices, int numIndices, const double* normals, int numNormals, const double* uvs, int numUVs)
1864 {
1865 if (numUVs == 0 && numNormals == 0)
1866 {
1867 return b3CreateCollisionShapeAddConcaveMesh(physClient, commandHandle, meshScale, vertices, numVertices, indices, numIndices);
1868 }
1869
1870 PhysicsClient* cl = (PhysicsClient*)physClient;
1871 b3Assert(cl);
1872 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
1873 b3Assert(command);
1874 b3Assert((command->m_type == CMD_CREATE_COLLISION_SHAPE) || (command->m_type == CMD_CREATE_VISUAL_SHAPE));
1875 if ((command->m_type == CMD_CREATE_COLLISION_SHAPE) || (command->m_type == CMD_CREATE_VISUAL_SHAPE))
1876 {
1877 int shapeIndex = command->m_createUserShapeArgs.m_numUserShapes;
1878 if (shapeIndex < MAX_COMPOUND_COLLISION_SHAPES && numVertices >= 0 && numIndices >= 0)
1879 {
1880 int i = 0;
1881 if (numVertices>B3_MAX_NUM_VERTICES)
1882 numVertices = B3_MAX_NUM_VERTICES;
1883 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_type = GEOM_MESH;
1884 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_collisionFlags = GEOM_FORCE_CONCAVE_TRIMESH;
1885 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_visualFlags = 0;
1886 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_hasChildTransform = 0;
1887 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_meshScale[0] = meshScale[0];
1888 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_meshScale[1] = meshScale[1];
1889 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_meshScale[2] = meshScale[2];
1890 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_meshFileType = 0;
1891 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_meshFileName[0] = 0;
1892 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_numVertices = numVertices;
1893
1894 int totalUploadSizeInBytes = numVertices * sizeof(double) * 3 + numIndices * sizeof(int) + numNormals*sizeof(double)*3+numUVs*sizeof(double)*2;
1895 char* data = new char[totalUploadSizeInBytes];
1896 double* vertexUpload = (double*)data;
1897 int* indexUpload = (int*)(data + numVertices * sizeof(double) * 3);
1898 double* normalUpload = (double*)(data + numVertices * sizeof(double) * 3 + numIndices * sizeof(int));
1899 double* uvUpload = (double*)(data + numVertices * sizeof(double) * 3 + numIndices * sizeof(int)+ numNormals * sizeof(double) * 3);
1900 for (i = 0; i<numVertices; i++)
1901 {
1902 vertexUpload[i * 3 + 0] = vertices[i * 3 + 0];
1903 vertexUpload[i * 3 + 1] = vertices[i * 3 + 1];
1904 vertexUpload[i * 3 + 2] = vertices[i * 3 + 2];
1905 }
1906 if (numIndices>B3_MAX_NUM_INDICES)
1907 numIndices = B3_MAX_NUM_INDICES;
1908
1909 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_numIndices = numIndices;
1910 for (i = 0; i<numIndices; i++)
1911 {
1912 indexUpload[i] = indices[i];
1913 }
1914
1915 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_numNormals = numNormals;
1916 for (i = 0; i < numNormals; i++)
1917 {
1918 normalUpload[i * 3 + 0] = normals[i * 3 + 0];
1919 normalUpload[i * 3 + 1] = normals[i * 3 + 1];
1920 normalUpload[i * 3 + 2] = normals[i * 3 + 2];
1921 }
1922
1923 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_numUVs = numUVs;
1924 for (i = 0; i < numUVs; i++)
1925 {
1926 uvUpload[i * 2 + 0] = uvs[i * 2 + 0];
1927 uvUpload[i * 2 + 1] = uvs[i * 2 + 1];
1928 }
1929 command->m_createUserShapeArgs.m_numUserShapes++;
1930 cl->uploadBulletFileToSharedMemory(data, totalUploadSizeInBytes);
1931 delete[] data;
1932 return shapeIndex;
1933 }
1934 }
1935 return -1;
1936 }
1937
1938
b3CreateVisualShapeAddMesh(b3SharedMemoryCommandHandle commandHandle,const char * fileName,const double meshScale[])1939 B3_SHARED_API int b3CreateVisualShapeAddMesh(b3SharedMemoryCommandHandle commandHandle, const char* fileName, const double meshScale[/*3*/])
1940 {
1941 return b3CreateCollisionShapeAddMesh(commandHandle, fileName, meshScale);
1942 }
1943
b3CreateCollisionSetFlag(b3SharedMemoryCommandHandle commandHandle,int shapeIndex,int flags)1944 B3_SHARED_API void b3CreateCollisionSetFlag(b3SharedMemoryCommandHandle commandHandle, int shapeIndex, int flags)
1945 {
1946 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
1947 b3Assert(command);
1948 b3Assert((command->m_type == CMD_CREATE_COLLISION_SHAPE) || (command->m_type == CMD_CREATE_VISUAL_SHAPE));
1949 if ((command->m_type == CMD_CREATE_COLLISION_SHAPE) || (command->m_type == CMD_CREATE_VISUAL_SHAPE))
1950 {
1951 if (shapeIndex < command->m_createUserShapeArgs.m_numUserShapes)
1952 {
1953 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_collisionFlags |= flags;
1954 }
1955 }
1956 }
b3CreateVisualSetFlag(b3SharedMemoryCommandHandle commandHandle,int shapeIndex,int flags)1957 B3_SHARED_API void b3CreateVisualSetFlag(b3SharedMemoryCommandHandle commandHandle, int shapeIndex, int flags)
1958 {
1959 b3CreateCollisionSetFlag(commandHandle, shapeIndex, flags);
1960 }
1961
b3CreateCollisionShapeSetChildTransform(b3SharedMemoryCommandHandle commandHandle,int shapeIndex,const double childPosition[3],const double childOrientation[4])1962 B3_SHARED_API void b3CreateCollisionShapeSetChildTransform(b3SharedMemoryCommandHandle commandHandle, int shapeIndex, const double childPosition[3], const double childOrientation[4])
1963 {
1964 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
1965 b3Assert(command);
1966 b3Assert((command->m_type == CMD_CREATE_COLLISION_SHAPE) || (command->m_type == CMD_CREATE_VISUAL_SHAPE));
1967 if ((command->m_type == CMD_CREATE_COLLISION_SHAPE) || (command->m_type == CMD_CREATE_VISUAL_SHAPE))
1968 {
1969 if (shapeIndex < command->m_createUserShapeArgs.m_numUserShapes)
1970 {
1971 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_hasChildTransform = 1;
1972 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_childPosition[0] = childPosition[0];
1973 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_childPosition[1] = childPosition[1];
1974 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_childPosition[2] = childPosition[2];
1975 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_childOrientation[0] = childOrientation[0];
1976 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_childOrientation[1] = childOrientation[1];
1977 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_childOrientation[2] = childOrientation[2];
1978 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_childOrientation[3] = childOrientation[3];
1979 }
1980 }
1981 }
1982
b3CreateVisualShapeSetChildTransform(b3SharedMemoryCommandHandle commandHandle,int shapeIndex,const double childPosition[],const double childOrientation[])1983 B3_SHARED_API void b3CreateVisualShapeSetChildTransform(b3SharedMemoryCommandHandle commandHandle, int shapeIndex, const double childPosition[/*3*/], const double childOrientation[/*4*/])
1984 {
1985 b3CreateCollisionShapeSetChildTransform(commandHandle, shapeIndex, childPosition, childOrientation);
1986 }
1987
b3CreateVisualShapeSetRGBAColor(b3SharedMemoryCommandHandle commandHandle,int shapeIndex,const double rgbaColor[])1988 B3_SHARED_API void b3CreateVisualShapeSetRGBAColor(b3SharedMemoryCommandHandle commandHandle, int shapeIndex, const double rgbaColor[/*4*/])
1989 {
1990 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
1991 b3Assert(command);
1992 b3Assert((command->m_type == CMD_CREATE_COLLISION_SHAPE) || (command->m_type == CMD_CREATE_VISUAL_SHAPE));
1993 if ((command->m_type == CMD_CREATE_COLLISION_SHAPE) || (command->m_type == CMD_CREATE_VISUAL_SHAPE))
1994 {
1995 if (shapeIndex < command->m_createUserShapeArgs.m_numUserShapes)
1996 {
1997 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_rgbaColor[0] = rgbaColor[0];
1998 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_rgbaColor[1] = rgbaColor[1];
1999 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_rgbaColor[2] = rgbaColor[2];
2000 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_rgbaColor[3] = rgbaColor[3];
2001 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_visualFlags |= GEOM_VISUAL_HAS_RGBA_COLOR;
2002 }
2003 }
2004 }
2005
b3CreateVisualShapeSetSpecularColor(b3SharedMemoryCommandHandle commandHandle,int shapeIndex,const double specularColor[])2006 B3_SHARED_API void b3CreateVisualShapeSetSpecularColor(b3SharedMemoryCommandHandle commandHandle, int shapeIndex, const double specularColor[/*3*/])
2007 {
2008 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
2009 b3Assert(command);
2010 b3Assert((command->m_type == CMD_CREATE_COLLISION_SHAPE) || (command->m_type == CMD_CREATE_VISUAL_SHAPE));
2011 if ((command->m_type == CMD_CREATE_COLLISION_SHAPE) || (command->m_type == CMD_CREATE_VISUAL_SHAPE))
2012 {
2013 if (shapeIndex < command->m_createUserShapeArgs.m_numUserShapes)
2014 {
2015 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_specularColor[0] = specularColor[0];
2016 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_specularColor[1] = specularColor[1];
2017 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_specularColor[2] = specularColor[2];
2018 command->m_createUserShapeArgs.m_shapes[shapeIndex].m_visualFlags |= GEOM_VISUAL_HAS_SPECULAR_COLOR;
2019 }
2020 }
2021 }
2022
b3GetStatusCollisionShapeUniqueId(b3SharedMemoryStatusHandle statusHandle)2023 B3_SHARED_API int b3GetStatusCollisionShapeUniqueId(b3SharedMemoryStatusHandle statusHandle)
2024 {
2025 const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
2026 b3Assert(status);
2027 b3Assert(status->m_type == CMD_CREATE_COLLISION_SHAPE_COMPLETED);
2028 if (status && status->m_type == CMD_CREATE_COLLISION_SHAPE_COMPLETED)
2029 {
2030 return status->m_createUserShapeResultArgs.m_userShapeUniqueId;
2031 }
2032 return -1;
2033 }
2034
b3CreateVisualShapeCommandInit(b3PhysicsClientHandle physClient)2035 B3_SHARED_API b3SharedMemoryCommandHandle b3CreateVisualShapeCommandInit(b3PhysicsClientHandle physClient)
2036 {
2037 PhysicsClient* cl = (PhysicsClient*)physClient;
2038 b3Assert(cl);
2039 b3Assert(cl->canSubmitCommand());
2040 if (cl)
2041 {
2042 struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
2043 b3Assert(command);
2044 command->m_type = CMD_CREATE_VISUAL_SHAPE;
2045 command->m_updateFlags = 0;
2046 command->m_createUserShapeArgs.m_numUserShapes = 0;
2047 return (b3SharedMemoryCommandHandle)command;
2048 }
2049 return 0;
2050 }
2051
b3GetStatusVisualShapeUniqueId(b3SharedMemoryStatusHandle statusHandle)2052 B3_SHARED_API int b3GetStatusVisualShapeUniqueId(b3SharedMemoryStatusHandle statusHandle)
2053 {
2054 const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
2055 b3Assert(status);
2056 b3Assert(status->m_type == CMD_CREATE_VISUAL_SHAPE_COMPLETED);
2057 if (status && status->m_type == CMD_CREATE_VISUAL_SHAPE_COMPLETED)
2058 {
2059 return status->m_createUserShapeResultArgs.m_userShapeUniqueId;
2060 }
2061 return -1;
2062 }
2063
b3CreateMultiBodyCommandInit(b3PhysicsClientHandle physClient)2064 B3_SHARED_API b3SharedMemoryCommandHandle b3CreateMultiBodyCommandInit(b3PhysicsClientHandle physClient)
2065 {
2066 PhysicsClient* cl = (PhysicsClient*)physClient;
2067 b3Assert(cl);
2068 b3Assert(cl->canSubmitCommand());
2069 if (cl)
2070 {
2071 struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
2072 b3Assert(command);
2073 command->m_type = CMD_CREATE_MULTI_BODY;
2074 command->m_updateFlags = 0;
2075 command->m_createMultiBodyArgs.m_bodyName[0] = 0;
2076 command->m_createMultiBodyArgs.m_baseLinkIndex = -1;
2077 command->m_createMultiBodyArgs.m_numLinks = 0;
2078 command->m_createMultiBodyArgs.m_numBatchObjects = 0;
2079 return (b3SharedMemoryCommandHandle)command;
2080 }
2081 return 0;
2082 }
2083
2084 //batch creation is an performance feature to create a large number of multi bodies in one command
b3CreateMultiBodySetBatchPositions(b3PhysicsClientHandle physClient,b3SharedMemoryCommandHandle commandHandle,double * batchPositions,int numBatchObjects)2085 B3_SHARED_API int b3CreateMultiBodySetBatchPositions(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, double* batchPositions, int numBatchObjects)
2086 {
2087 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
2088 b3Assert(command);
2089 b3Assert(command->m_type == CMD_CREATE_MULTI_BODY);
2090 PhysicsClient* cl = (PhysicsClient*)physClient;
2091 b3Assert(cl);
2092 b3Assert(cl->canSubmitCommand());
2093
2094 if (cl && command->m_type == CMD_CREATE_MULTI_BODY)
2095 {
2096 command->m_createMultiBodyArgs.m_numBatchObjects = numBatchObjects;
2097 cl->uploadBulletFileToSharedMemory((const char*)batchPositions, sizeof(double) * 3 * numBatchObjects);
2098 }
2099 return 0;
2100 }
2101
b3CreateMultiBodyBase(b3SharedMemoryCommandHandle commandHandle,double mass,int collisionShapeUnique,int visualShapeUniqueId,const double basePosition[3],const double baseOrientation[4],const double baseInertialFramePosition[3],const double baseInertialFrameOrientation[4])2102 B3_SHARED_API int b3CreateMultiBodyBase(b3SharedMemoryCommandHandle commandHandle, double mass, int collisionShapeUnique, int visualShapeUniqueId, const double basePosition[3], const double baseOrientation[4], const double baseInertialFramePosition[3], const double baseInertialFrameOrientation[4])
2103 {
2104 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
2105 b3Assert(command);
2106 b3Assert(command->m_type == CMD_CREATE_MULTI_BODY);
2107 if (command->m_type == CMD_CREATE_MULTI_BODY)
2108 {
2109 int numLinks = command->m_createMultiBodyArgs.m_numLinks;
2110
2111 if (numLinks < MAX_CREATE_MULTI_BODY_LINKS)
2112 {
2113 int baseLinkIndex = numLinks;
2114 command->m_updateFlags |= MULTI_BODY_HAS_BASE;
2115 command->m_createMultiBodyArgs.m_baseLinkIndex = baseLinkIndex;
2116 command->m_createMultiBodyArgs.m_linkPositions[baseLinkIndex * 3 + 0] = basePosition[0];
2117 command->m_createMultiBodyArgs.m_linkPositions[baseLinkIndex * 3 + 1] = basePosition[1];
2118 command->m_createMultiBodyArgs.m_linkPositions[baseLinkIndex * 3 + 2] = basePosition[2];
2119
2120 command->m_createMultiBodyArgs.m_linkOrientations[baseLinkIndex * 4 + 0] = baseOrientation[0];
2121 command->m_createMultiBodyArgs.m_linkOrientations[baseLinkIndex * 4 + 1] = baseOrientation[1];
2122 command->m_createMultiBodyArgs.m_linkOrientations[baseLinkIndex * 4 + 2] = baseOrientation[2];
2123 command->m_createMultiBodyArgs.m_linkOrientations[baseLinkIndex * 4 + 3] = baseOrientation[3];
2124
2125 command->m_createMultiBodyArgs.m_linkInertias[baseLinkIndex * 3 + 0] = 0; //unused, is computed automatically. Will add a method to explicitly set it (with a flag), similar to loadURDF etc.
2126 command->m_createMultiBodyArgs.m_linkInertias[baseLinkIndex * 3 + 1] = 0;
2127 command->m_createMultiBodyArgs.m_linkInertias[baseLinkIndex * 3 + 2] = 0;
2128
2129 command->m_createMultiBodyArgs.m_linkInertialFramePositions[baseLinkIndex * 3 + 0] = baseInertialFramePosition[0];
2130 command->m_createMultiBodyArgs.m_linkInertialFramePositions[baseLinkIndex * 3 + 1] = baseInertialFramePosition[1];
2131 command->m_createMultiBodyArgs.m_linkInertialFramePositions[baseLinkIndex * 3 + 2] = baseInertialFramePosition[2];
2132
2133 command->m_createMultiBodyArgs.m_linkInertialFrameOrientations[baseLinkIndex * 4 + 0] = baseInertialFrameOrientation[0];
2134 command->m_createMultiBodyArgs.m_linkInertialFrameOrientations[baseLinkIndex * 4 + 1] = baseInertialFrameOrientation[1];
2135 command->m_createMultiBodyArgs.m_linkInertialFrameOrientations[baseLinkIndex * 4 + 2] = baseInertialFrameOrientation[2];
2136 command->m_createMultiBodyArgs.m_linkInertialFrameOrientations[baseLinkIndex * 4 + 3] = baseInertialFrameOrientation[3];
2137
2138 command->m_createMultiBodyArgs.m_linkCollisionShapeUniqueIds[baseLinkIndex] = collisionShapeUnique;
2139 command->m_createMultiBodyArgs.m_linkVisualShapeUniqueIds[baseLinkIndex] = visualShapeUniqueId;
2140
2141 command->m_createMultiBodyArgs.m_linkMasses[baseLinkIndex] = mass;
2142
2143 command->m_createMultiBodyArgs.m_linkParentIndices[baseLinkIndex] = -2; //no parent
2144 command->m_createMultiBodyArgs.m_linkJointAxis[baseLinkIndex + 0] = 0;
2145 command->m_createMultiBodyArgs.m_linkJointAxis[baseLinkIndex + 1] = 0;
2146 command->m_createMultiBodyArgs.m_linkJointAxis[baseLinkIndex + 2] = 0;
2147 command->m_createMultiBodyArgs.m_linkJointTypes[baseLinkIndex] = -1;
2148 command->m_createMultiBodyArgs.m_numLinks++;
2149 }
2150 return numLinks;
2151 }
2152 return -2;
2153 }
2154
b3CreateMultiBodyLink(b3SharedMemoryCommandHandle commandHandle,double linkMass,double linkCollisionShapeIndex,double linkVisualShapeIndex,const double linkPosition[3],const double linkOrientation[4],const double linkInertialFramePosition[3],const double linkInertialFrameOrientation[4],int linkParentIndex,int linkJointType,const double linkJointAxis[3])2155 B3_SHARED_API int b3CreateMultiBodyLink(b3SharedMemoryCommandHandle commandHandle, double linkMass, double linkCollisionShapeIndex,
2156 double linkVisualShapeIndex,
2157 const double linkPosition[3],
2158 const double linkOrientation[4],
2159 const double linkInertialFramePosition[3],
2160 const double linkInertialFrameOrientation[4],
2161 int linkParentIndex,
2162 int linkJointType,
2163 const double linkJointAxis[3])
2164 {
2165 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
2166 b3Assert(command);
2167 b3Assert(command->m_type == CMD_CREATE_MULTI_BODY);
2168 if (command->m_type == CMD_CREATE_MULTI_BODY)
2169 {
2170 int numLinks = command->m_createMultiBodyArgs.m_numLinks;
2171
2172 if (numLinks < MAX_CREATE_MULTI_BODY_LINKS)
2173 {
2174 int linkIndex = numLinks;
2175 command->m_updateFlags |= MULTI_BODY_HAS_BASE;
2176 command->m_createMultiBodyArgs.m_linkPositions[linkIndex * 3 + 0] = linkPosition[0];
2177 command->m_createMultiBodyArgs.m_linkPositions[linkIndex * 3 + 1] = linkPosition[1];
2178 command->m_createMultiBodyArgs.m_linkPositions[linkIndex * 3 + 2] = linkPosition[2];
2179
2180 command->m_createMultiBodyArgs.m_linkOrientations[linkIndex * 4 + 0] = linkOrientation[0];
2181 command->m_createMultiBodyArgs.m_linkOrientations[linkIndex * 4 + 1] = linkOrientation[1];
2182 command->m_createMultiBodyArgs.m_linkOrientations[linkIndex * 4 + 2] = linkOrientation[2];
2183 command->m_createMultiBodyArgs.m_linkOrientations[linkIndex * 4 + 3] = linkOrientation[3];
2184
2185 command->m_createMultiBodyArgs.m_linkInertias[linkIndex * 3 + 0] = linkMass;
2186 command->m_createMultiBodyArgs.m_linkInertias[linkIndex * 3 + 1] = linkMass;
2187 command->m_createMultiBodyArgs.m_linkInertias[linkIndex * 3 + 2] = linkMass;
2188
2189 command->m_createMultiBodyArgs.m_linkInertialFramePositions[linkIndex * 3 + 0] = linkInertialFramePosition[0];
2190 command->m_createMultiBodyArgs.m_linkInertialFramePositions[linkIndex * 3 + 1] = linkInertialFramePosition[1];
2191 command->m_createMultiBodyArgs.m_linkInertialFramePositions[linkIndex * 3 + 2] = linkInertialFramePosition[2];
2192
2193 command->m_createMultiBodyArgs.m_linkInertialFrameOrientations[linkIndex * 4 + 0] = linkInertialFrameOrientation[0];
2194 command->m_createMultiBodyArgs.m_linkInertialFrameOrientations[linkIndex * 4 + 1] = linkInertialFrameOrientation[1];
2195 command->m_createMultiBodyArgs.m_linkInertialFrameOrientations[linkIndex * 4 + 2] = linkInertialFrameOrientation[2];
2196 command->m_createMultiBodyArgs.m_linkInertialFrameOrientations[linkIndex * 4 + 3] = linkInertialFrameOrientation[3];
2197
2198 command->m_createMultiBodyArgs.m_linkCollisionShapeUniqueIds[linkIndex] = linkCollisionShapeIndex;
2199 command->m_createMultiBodyArgs.m_linkVisualShapeUniqueIds[linkIndex] = linkVisualShapeIndex;
2200
2201 command->m_createMultiBodyArgs.m_linkParentIndices[linkIndex] = linkParentIndex;
2202 command->m_createMultiBodyArgs.m_linkJointTypes[linkIndex] = linkJointType;
2203 command->m_createMultiBodyArgs.m_linkJointAxis[3 * linkIndex + 0] = linkJointAxis[0];
2204 command->m_createMultiBodyArgs.m_linkJointAxis[3 * linkIndex + 1] = linkJointAxis[1];
2205 command->m_createMultiBodyArgs.m_linkJointAxis[3 * linkIndex + 2] = linkJointAxis[2];
2206
2207 command->m_createMultiBodyArgs.m_linkMasses[linkIndex] = linkMass;
2208
2209 command->m_createMultiBodyArgs.m_numLinks++;
2210 return numLinks;
2211 }
2212 }
2213
2214 return -1;
2215 }
2216
2217 //useMaximalCoordinates are disabled by default, enabling them is experimental and not fully supported yet
b3CreateMultiBodyUseMaximalCoordinates(b3SharedMemoryCommandHandle commandHandle)2218 B3_SHARED_API void b3CreateMultiBodyUseMaximalCoordinates(b3SharedMemoryCommandHandle commandHandle)
2219 {
2220 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
2221 b3Assert(command);
2222 b3Assert(command->m_type == CMD_CREATE_MULTI_BODY);
2223 if (command->m_type == CMD_CREATE_MULTI_BODY)
2224 {
2225 command->m_updateFlags |= MULT_BODY_USE_MAXIMAL_COORDINATES;
2226 }
2227 }
2228
b3CreateMultiBodySetFlags(b3SharedMemoryCommandHandle commandHandle,int flags)2229 B3_SHARED_API void b3CreateMultiBodySetFlags(b3SharedMemoryCommandHandle commandHandle, int flags)
2230 {
2231 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
2232 b3Assert(command);
2233 b3Assert(command->m_type == CMD_CREATE_MULTI_BODY);
2234 if (command->m_type == CMD_CREATE_MULTI_BODY)
2235 {
2236 command->m_updateFlags |= MULT_BODY_HAS_FLAGS;
2237 command->m_createMultiBodyArgs.m_flags = flags;
2238 }
2239 }
2240
b3GetStatusMultiBodyUniqueId(b3SharedMemoryStatusHandle statusHandle)2241 B3_SHARED_API int b3GetStatusMultiBodyUniqueId(b3SharedMemoryStatusHandle statusHandle)
2242 {
2243 const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
2244 b3Assert(status);
2245 b3Assert(status->m_type == CMD_CREATE_MULTI_BODY_COMPLETED);
2246 if (status && status->m_type == CMD_CREATE_MULTI_BODY_COMPLETED)
2247 {
2248 return status->m_createMultiBodyResultArgs.m_bodyUniqueId;
2249 }
2250 return -1;
2251 }
2252
b3CreateBoxShapeCommandInit(b3PhysicsClientHandle physClient)2253 B3_SHARED_API b3SharedMemoryCommandHandle b3CreateBoxShapeCommandInit(b3PhysicsClientHandle physClient)
2254 {
2255 PhysicsClient* cl = (PhysicsClient*)physClient;
2256 b3Assert(cl);
2257 b3Assert(cl->canSubmitCommand());
2258 struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
2259 b3Assert(command);
2260 command->m_type = CMD_CREATE_BOX_COLLISION_SHAPE;
2261 command->m_updateFlags = 0;
2262 return (b3SharedMemoryCommandHandle)command;
2263 }
2264
b3CreateBoxCommandSetStartPosition(b3SharedMemoryCommandHandle commandHandle,double startPosX,double startPosY,double startPosZ)2265 B3_SHARED_API int b3CreateBoxCommandSetStartPosition(b3SharedMemoryCommandHandle commandHandle, double startPosX, double startPosY, double startPosZ)
2266 {
2267 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
2268 b3Assert(command);
2269 b3Assert(command->m_type == CMD_CREATE_BOX_COLLISION_SHAPE);
2270 command->m_updateFlags |= BOX_SHAPE_HAS_INITIAL_POSITION;
2271
2272 command->m_createBoxShapeArguments.m_initialPosition[0] = startPosX;
2273 command->m_createBoxShapeArguments.m_initialPosition[1] = startPosY;
2274 command->m_createBoxShapeArguments.m_initialPosition[2] = startPosZ;
2275 return 0;
2276 }
2277
b3CreateBoxCommandSetHalfExtents(b3SharedMemoryCommandHandle commandHandle,double halfExtentsX,double halfExtentsY,double halfExtentsZ)2278 B3_SHARED_API int b3CreateBoxCommandSetHalfExtents(b3SharedMemoryCommandHandle commandHandle, double halfExtentsX, double halfExtentsY, double halfExtentsZ)
2279 {
2280 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
2281 b3Assert(command);
2282 b3Assert(command->m_type == CMD_CREATE_BOX_COLLISION_SHAPE);
2283 command->m_updateFlags |= BOX_SHAPE_HAS_HALF_EXTENTS;
2284
2285 command->m_createBoxShapeArguments.m_halfExtentsX = halfExtentsX;
2286 command->m_createBoxShapeArguments.m_halfExtentsY = halfExtentsY;
2287 command->m_createBoxShapeArguments.m_halfExtentsZ = halfExtentsZ;
2288
2289 return 0;
2290 }
2291
b3CreateBoxCommandSetMass(b3SharedMemoryCommandHandle commandHandle,double mass)2292 B3_SHARED_API int b3CreateBoxCommandSetMass(b3SharedMemoryCommandHandle commandHandle, double mass)
2293 {
2294 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
2295 b3Assert(command);
2296 b3Assert(command->m_type == CMD_CREATE_BOX_COLLISION_SHAPE);
2297 command->m_updateFlags |= BOX_SHAPE_HAS_MASS;
2298 command->m_createBoxShapeArguments.m_mass = mass;
2299 return 0;
2300 }
2301
b3CreateBoxCommandSetCollisionShapeType(b3SharedMemoryCommandHandle commandHandle,int collisionShapeType)2302 B3_SHARED_API int b3CreateBoxCommandSetCollisionShapeType(b3SharedMemoryCommandHandle commandHandle, int collisionShapeType)
2303 {
2304 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
2305 b3Assert(command);
2306 b3Assert(command->m_type == CMD_CREATE_BOX_COLLISION_SHAPE);
2307 command->m_updateFlags |= BOX_SHAPE_HAS_COLLISION_SHAPE_TYPE;
2308 command->m_createBoxShapeArguments.m_collisionShapeType = collisionShapeType;
2309
2310 return 0;
2311 }
2312
b3CreateBoxCommandSetColorRGBA(b3SharedMemoryCommandHandle commandHandle,double red,double green,double blue,double alpha)2313 B3_SHARED_API int b3CreateBoxCommandSetColorRGBA(b3SharedMemoryCommandHandle commandHandle, double red, double green, double blue, double alpha)
2314 {
2315 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
2316 b3Assert(command);
2317 b3Assert(command->m_type == CMD_CREATE_BOX_COLLISION_SHAPE);
2318 command->m_updateFlags |= BOX_SHAPE_HAS_COLOR;
2319 command->m_createBoxShapeArguments.m_colorRGBA[0] = red;
2320 command->m_createBoxShapeArguments.m_colorRGBA[1] = green;
2321 command->m_createBoxShapeArguments.m_colorRGBA[2] = blue;
2322 command->m_createBoxShapeArguments.m_colorRGBA[3] = alpha;
2323 return 0;
2324 }
2325
b3CreateBoxCommandSetStartOrientation(b3SharedMemoryCommandHandle commandHandle,double startOrnX,double startOrnY,double startOrnZ,double startOrnW)2326 B3_SHARED_API int b3CreateBoxCommandSetStartOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX, double startOrnY, double startOrnZ, double startOrnW)
2327 {
2328 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
2329 b3Assert(command);
2330 b3Assert(command->m_type == CMD_CREATE_BOX_COLLISION_SHAPE);
2331 command->m_updateFlags |= BOX_SHAPE_HAS_INITIAL_ORIENTATION;
2332
2333 command->m_createBoxShapeArguments.m_initialOrientation[0] = startOrnX;
2334 command->m_createBoxShapeArguments.m_initialOrientation[1] = startOrnY;
2335 command->m_createBoxShapeArguments.m_initialOrientation[2] = startOrnZ;
2336 command->m_createBoxShapeArguments.m_initialOrientation[3] = startOrnW;
2337 return 0;
2338 }
2339
b3CreatePoseCommandInit(b3PhysicsClientHandle physClient,int bodyUniqueId)2340 B3_SHARED_API b3SharedMemoryCommandHandle b3CreatePoseCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId)
2341 {
2342 PhysicsClient* cl = (PhysicsClient*)physClient;
2343 b3Assert(cl);
2344 b3Assert(cl->canSubmitCommand());
2345 struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
2346 b3Assert(command);
2347 return b3CreatePoseCommandInit2((b3SharedMemoryCommandHandle)command, bodyUniqueId);
2348 }
2349
b3CreatePoseCommandInit2(b3SharedMemoryCommandHandle commandHandle,int bodyUniqueId)2350 B3_SHARED_API b3SharedMemoryCommandHandle b3CreatePoseCommandInit2(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId)
2351 {
2352 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
2353 command->m_type = CMD_INIT_POSE;
2354 command->m_updateFlags = 0;
2355 command->m_initPoseArgs.m_bodyUniqueId = bodyUniqueId;
2356 //a bit slow, initialing the full range to zero...
2357 for (int i = 0; i < MAX_DEGREE_OF_FREEDOM; i++)
2358 {
2359 command->m_initPoseArgs.m_hasInitialStateQ[i] = 0;
2360 command->m_initPoseArgs.m_hasInitialStateQdot[i] = 0;
2361 }
2362 return commandHandle;
2363 }
2364
b3CreatePoseCommandSetBasePosition(b3SharedMemoryCommandHandle commandHandle,double startPosX,double startPosY,double startPosZ)2365 B3_SHARED_API int b3CreatePoseCommandSetBasePosition(b3SharedMemoryCommandHandle commandHandle, double startPosX, double startPosY, double startPosZ)
2366 {
2367 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
2368 b3Assert(command);
2369 b3Assert(command->m_type == CMD_INIT_POSE);
2370 command->m_updateFlags |= INIT_POSE_HAS_INITIAL_POSITION;
2371 command->m_initPoseArgs.m_initialStateQ[0] = startPosX;
2372 command->m_initPoseArgs.m_initialStateQ[1] = startPosY;
2373 command->m_initPoseArgs.m_initialStateQ[2] = startPosZ;
2374
2375 command->m_initPoseArgs.m_hasInitialStateQ[0] = 1;
2376 command->m_initPoseArgs.m_hasInitialStateQ[1] = 1;
2377 command->m_initPoseArgs.m_hasInitialStateQ[2] = 1;
2378
2379 return 0;
2380 }
2381
b3CreatePoseCommandSetBaseScaling(b3SharedMemoryCommandHandle commandHandle,double scaling[3])2382 B3_SHARED_API int b3CreatePoseCommandSetBaseScaling(b3SharedMemoryCommandHandle commandHandle, double scaling[3])
2383 {
2384 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
2385 b3Assert(command);
2386 b3Assert(command->m_type == CMD_INIT_POSE);
2387 command->m_updateFlags |= INIT_POSE_HAS_SCALING;
2388
2389 command->m_initPoseArgs.m_scaling[0] = scaling[0];
2390 command->m_initPoseArgs.m_scaling[1] = scaling[1];
2391 command->m_initPoseArgs.m_scaling[2] = scaling[2];
2392
2393 return 0;
2394 }
2395
2396
2397
b3CreatePoseCommandSetBaseOrientation(b3SharedMemoryCommandHandle commandHandle,double startOrnX,double startOrnY,double startOrnZ,double startOrnW)2398 B3_SHARED_API int b3CreatePoseCommandSetBaseOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX, double startOrnY, double startOrnZ, double startOrnW)
2399 {
2400 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
2401 b3Assert(command);
2402 b3Assert(command->m_type == CMD_INIT_POSE);
2403 command->m_updateFlags |= INIT_POSE_HAS_INITIAL_ORIENTATION;
2404 command->m_initPoseArgs.m_initialStateQ[3] = startOrnX;
2405 command->m_initPoseArgs.m_initialStateQ[4] = startOrnY;
2406 command->m_initPoseArgs.m_initialStateQ[5] = startOrnZ;
2407 command->m_initPoseArgs.m_initialStateQ[6] = startOrnW;
2408
2409 command->m_initPoseArgs.m_hasInitialStateQ[3] = 1;
2410 command->m_initPoseArgs.m_hasInitialStateQ[4] = 1;
2411 command->m_initPoseArgs.m_hasInitialStateQ[5] = 1;
2412 command->m_initPoseArgs.m_hasInitialStateQ[6] = 1;
2413
2414 return 0;
2415 }
2416
b3CreatePoseCommandSetBaseLinearVelocity(b3SharedMemoryCommandHandle commandHandle,const double linVel[3])2417 B3_SHARED_API int b3CreatePoseCommandSetBaseLinearVelocity(b3SharedMemoryCommandHandle commandHandle, const double linVel[3])
2418 {
2419 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
2420 b3Assert(command);
2421 b3Assert(command->m_type == CMD_INIT_POSE);
2422 command->m_updateFlags |= INIT_POSE_HAS_BASE_LINEAR_VELOCITY;
2423 command->m_initPoseArgs.m_hasInitialStateQdot[0] = 1;
2424 command->m_initPoseArgs.m_hasInitialStateQdot[1] = 1;
2425 command->m_initPoseArgs.m_hasInitialStateQdot[2] = 1;
2426
2427 command->m_initPoseArgs.m_initialStateQdot[0] = linVel[0];
2428 command->m_initPoseArgs.m_initialStateQdot[1] = linVel[1];
2429 command->m_initPoseArgs.m_initialStateQdot[2] = linVel[2];
2430
2431 return 0;
2432 }
2433
b3CreatePoseCommandSetBaseAngularVelocity(b3SharedMemoryCommandHandle commandHandle,const double angVel[3])2434 B3_SHARED_API int b3CreatePoseCommandSetBaseAngularVelocity(b3SharedMemoryCommandHandle commandHandle, const double angVel[3])
2435 {
2436 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
2437 b3Assert(command);
2438 b3Assert(command->m_type == CMD_INIT_POSE);
2439 command->m_updateFlags |= INIT_POSE_HAS_BASE_ANGULAR_VELOCITY;
2440 command->m_initPoseArgs.m_hasInitialStateQdot[3] = 1;
2441 command->m_initPoseArgs.m_hasInitialStateQdot[4] = 1;
2442 command->m_initPoseArgs.m_hasInitialStateQdot[5] = 1;
2443
2444 command->m_initPoseArgs.m_initialStateQdot[3] = angVel[0];
2445 command->m_initPoseArgs.m_initialStateQdot[4] = angVel[1];
2446 command->m_initPoseArgs.m_initialStateQdot[5] = angVel[2];
2447
2448 return 0;
2449 }
2450
b3CreatePoseCommandSetJointPositions(b3SharedMemoryCommandHandle commandHandle,int numJointPositions,const double * jointPositions)2451 B3_SHARED_API int b3CreatePoseCommandSetJointPositions(b3SharedMemoryCommandHandle commandHandle, int numJointPositions, const double* jointPositions)
2452 {
2453 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
2454 b3Assert(command);
2455 b3Assert(command->m_type == CMD_INIT_POSE);
2456 command->m_updateFlags |= INIT_POSE_HAS_JOINT_STATE;
2457 for (int i = 0; i < numJointPositions; i++)
2458 {
2459 if ((i + 7) < MAX_DEGREE_OF_FREEDOM)
2460 {
2461 command->m_initPoseArgs.m_initialStateQ[i + 7] = jointPositions[i];
2462 command->m_initPoseArgs.m_hasInitialStateQ[i + 7] = 1;
2463 }
2464 }
2465 return 0;
2466 }
2467
b3CreatePoseCommandSetQ(b3SharedMemoryCommandHandle commandHandle,int numJointPositions,const double * q,const int * hasQ)2468 B3_SHARED_API int b3CreatePoseCommandSetQ(b3SharedMemoryCommandHandle commandHandle, int numJointPositions, const double* q, const int* hasQ)
2469 {
2470 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
2471 b3Assert(command);
2472 b3Assert(command->m_type == CMD_INIT_POSE);
2473 command->m_updateFlags |= INIT_POSE_HAS_JOINT_STATE;
2474 for (int i = 0; i < numJointPositions; i++)
2475 {
2476 if ((i) < MAX_DEGREE_OF_FREEDOM)
2477 {
2478 command->m_initPoseArgs.m_initialStateQ[i] = q[i];
2479 command->m_initPoseArgs.m_hasInitialStateQ[i] = hasQ[i];
2480 }
2481 }
2482 return 0;
2483 }
2484
b3CreatePoseCommandSetJointPosition(b3PhysicsClientHandle physClient,b3SharedMemoryCommandHandle commandHandle,int jointIndex,double jointPosition)2485 B3_SHARED_API int b3CreatePoseCommandSetJointPosition(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, int jointIndex, double jointPosition)
2486 {
2487 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
2488 b3Assert(command);
2489 b3Assert(command->m_type == CMD_INIT_POSE);
2490 command->m_updateFlags |= INIT_POSE_HAS_JOINT_STATE;
2491 b3JointInfo info;
2492 b3GetJointInfo(physClient, command->m_initPoseArgs.m_bodyUniqueId, jointIndex, &info);
2493 //btAssert((info.m_flags & JOINT_HAS_MOTORIZED_POWER) && info.m_qIndex >=0);
2494 if ((info.m_flags & JOINT_HAS_MOTORIZED_POWER) && info.m_qIndex >= 0)
2495 {
2496 command->m_initPoseArgs.m_initialStateQ[info.m_qIndex] = jointPosition;
2497 command->m_initPoseArgs.m_hasInitialStateQ[info.m_qIndex] = 1;
2498 }
2499 return 0;
2500 }
2501
b3CreatePoseCommandSetJointPositionMultiDof(b3PhysicsClientHandle physClient,b3SharedMemoryCommandHandle commandHandle,int jointIndex,const double * jointPosition,int posSize)2502 B3_SHARED_API int b3CreatePoseCommandSetJointPositionMultiDof(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, int jointIndex, const double* jointPosition, int posSize)
2503 {
2504 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
2505 b3Assert(command);
2506 b3Assert(command->m_type == CMD_INIT_POSE);
2507 command->m_updateFlags |= INIT_POSE_HAS_JOINT_STATE;
2508 b3JointInfo info;
2509 b3GetJointInfo(physClient, command->m_initPoseArgs.m_bodyUniqueId, jointIndex, &info);
2510 //if ((info.m_flags & JOINT_HAS_MOTORIZED_POWER) && info.m_qIndex >= 0)
2511 if (info.m_qIndex >= 0)
2512 {
2513 if (posSize == info.m_qSize)
2514 {
2515 for (int i = 0; i < posSize; i++)
2516 {
2517 command->m_initPoseArgs.m_initialStateQ[info.m_qIndex + i] = jointPosition[i];
2518 command->m_initPoseArgs.m_hasInitialStateQ[info.m_qIndex + i] = 1;
2519 }
2520 }
2521 }
2522 return 0;
2523 }
2524
b3CreatePoseCommandSetJointVelocity(b3PhysicsClientHandle physClient,b3SharedMemoryCommandHandle commandHandle,int jointIndex,double jointVelocity)2525 B3_SHARED_API int b3CreatePoseCommandSetJointVelocity(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, int jointIndex, double jointVelocity)
2526 {
2527 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
2528 b3Assert(command);
2529 b3Assert(command->m_type == CMD_INIT_POSE);
2530 command->m_updateFlags |= INIT_POSE_HAS_JOINT_VELOCITY;
2531 b3JointInfo info;
2532 b3GetJointInfo(physClient, command->m_initPoseArgs.m_bodyUniqueId, jointIndex, &info);
2533 //btAssert((info.m_flags & JOINT_HAS_MOTORIZED_POWER) && info.m_uIndex >=0);
2534 if ((info.m_flags & JOINT_HAS_MOTORIZED_POWER) && (info.m_uIndex >= 0) && (info.m_uIndex < MAX_DEGREE_OF_FREEDOM))
2535 {
2536 command->m_initPoseArgs.m_initialStateQdot[info.m_uIndex] = jointVelocity;
2537 command->m_initPoseArgs.m_hasInitialStateQdot[info.m_uIndex] = 1;
2538 }
2539 return 0;
2540 }
2541
b3CreatePoseCommandSetJointVelocityMultiDof(b3PhysicsClientHandle physClient,b3SharedMemoryCommandHandle commandHandle,int jointIndex,const double * jointVelocity,int velSize)2542 B3_SHARED_API int b3CreatePoseCommandSetJointVelocityMultiDof(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, int jointIndex, const double* jointVelocity, int velSize)
2543 {
2544 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
2545 b3Assert(command);
2546 b3Assert(command->m_type == CMD_INIT_POSE);
2547 command->m_updateFlags |= INIT_POSE_HAS_JOINT_VELOCITY;
2548 b3JointInfo info;
2549 b3GetJointInfo(physClient, command->m_initPoseArgs.m_bodyUniqueId, jointIndex, &info);
2550
2551 //if ((info.m_flags & JOINT_HAS_MOTORIZED_POWER) && (info.m_uIndex >= 0) && (info.m_uIndex < MAX_DEGREE_OF_FREEDOM))
2552 if ((info.m_uIndex >= 0) && (info.m_uIndex < MAX_DEGREE_OF_FREEDOM))
2553 {
2554 if (velSize == info.m_uSize)
2555 {
2556 for (int i = 0; i < velSize; i++)
2557 {
2558 command->m_initPoseArgs.m_initialStateQdot[info.m_uIndex + i] = jointVelocity[i];
2559 command->m_initPoseArgs.m_hasInitialStateQdot[info.m_uIndex + i] = 1;
2560 }
2561 }
2562
2563 }
2564 return 0;
2565 }
2566
2567
b3CreatePoseCommandSetJointVelocities(b3PhysicsClientHandle physClient,b3SharedMemoryCommandHandle commandHandle,int numJointVelocities,const double * jointVelocities)2568 B3_SHARED_API int b3CreatePoseCommandSetJointVelocities(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, int numJointVelocities, const double* jointVelocities)
2569 {
2570 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
2571 b3Assert(command);
2572 b3Assert(command->m_type == CMD_INIT_POSE);
2573
2574 command->m_updateFlags |= INIT_POSE_HAS_JOINT_VELOCITY;
2575 for (int i = 0; i < numJointVelocities; i++)
2576 {
2577 if ((i + 6) < MAX_DEGREE_OF_FREEDOM)
2578 {
2579 command->m_initPoseArgs.m_initialStateQdot[i + 6] = jointVelocities[i];
2580 command->m_initPoseArgs.m_hasInitialStateQdot[i + 6] = 1;
2581 }
2582 }
2583 return 0;
2584 }
2585
b3CreatePoseCommandSetQdots(b3SharedMemoryCommandHandle commandHandle,int numJointVelocities,const double * qDots,const int * hasQdots)2586 B3_SHARED_API int b3CreatePoseCommandSetQdots(b3SharedMemoryCommandHandle commandHandle, int numJointVelocities, const double* qDots, const int* hasQdots)
2587 {
2588 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
2589 b3Assert(command);
2590 b3Assert(command->m_type == CMD_INIT_POSE);
2591
2592 command->m_updateFlags |= INIT_POSE_HAS_JOINT_VELOCITY;
2593 for (int i = 0; i < numJointVelocities; i++)
2594 {
2595 if (i < MAX_DEGREE_OF_FREEDOM)
2596 {
2597 command->m_initPoseArgs.m_initialStateQdot[i] = qDots[i];
2598 command->m_initPoseArgs.m_hasInitialStateQdot[i] = hasQdots[i];
2599 }
2600 }
2601 return 0;
2602 }
2603
2604
2605
b3CreateSensorCommandInit(b3PhysicsClientHandle physClient,int bodyUniqueId)2606 B3_SHARED_API b3SharedMemoryCommandHandle b3CreateSensorCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId)
2607 {
2608 PhysicsClient* cl = (PhysicsClient*)physClient;
2609 b3Assert(cl);
2610 b3Assert(cl->canSubmitCommand());
2611 struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
2612 b3Assert(command);
2613
2614 command->m_type = CMD_CREATE_SENSOR;
2615 command->m_updateFlags = 0;
2616 command->m_createSensorArguments.m_numJointSensorChanges = 0;
2617 command->m_createSensorArguments.m_bodyUniqueId = bodyUniqueId;
2618 return (b3SharedMemoryCommandHandle)command;
2619 }
2620
b3CreateSensorEnable6DofJointForceTorqueSensor(b3SharedMemoryCommandHandle commandHandle,int jointIndex,int enable)2621 B3_SHARED_API int b3CreateSensorEnable6DofJointForceTorqueSensor(b3SharedMemoryCommandHandle commandHandle, int jointIndex, int enable)
2622 {
2623 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
2624 b3Assert(command);
2625 b3Assert(command->m_type == CMD_CREATE_SENSOR);
2626 int curIndex = command->m_createSensorArguments.m_numJointSensorChanges;
2627 command->m_createSensorArguments.m_sensorType[curIndex] = SENSOR_FORCE_TORQUE;
2628
2629 command->m_createSensorArguments.m_jointIndex[curIndex] = jointIndex;
2630 command->m_createSensorArguments.m_enableJointForceSensor[curIndex] = enable;
2631 command->m_createSensorArguments.m_numJointSensorChanges++;
2632 return 0;
2633 }
2634
b3CreateSensorEnableIMUForLink(b3SharedMemoryCommandHandle commandHandle,int linkIndex,int enable)2635 B3_SHARED_API int b3CreateSensorEnableIMUForLink(b3SharedMemoryCommandHandle commandHandle, int linkIndex, int enable)
2636 {
2637 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
2638 b3Assert(command);
2639 b3Assert(command->m_type == CMD_CREATE_SENSOR);
2640 int curIndex = command->m_createSensorArguments.m_numJointSensorChanges;
2641 command->m_createSensorArguments.m_sensorType[curIndex] = SENSOR_IMU;
2642 command->m_createSensorArguments.m_linkIndex[curIndex] = linkIndex;
2643 command->m_createSensorArguments.m_enableSensor[curIndex] = enable;
2644 command->m_createSensorArguments.m_numJointSensorChanges++;
2645 return 0;
2646 }
2647
b3DisconnectSharedMemory(b3PhysicsClientHandle physClient)2648 B3_SHARED_API void b3DisconnectSharedMemory(b3PhysicsClientHandle physClient)
2649 {
2650 PhysicsClient* cl = (PhysicsClient*)physClient;
2651 if (cl)
2652 {
2653 cl->disconnectSharedMemory();
2654 }
2655 delete cl;
2656 }
2657
b3ProcessServerStatus(b3PhysicsClientHandle physClient)2658 B3_SHARED_API b3SharedMemoryStatusHandle b3ProcessServerStatus(b3PhysicsClientHandle physClient)
2659 {
2660 PhysicsClient* cl = (PhysicsClient*)physClient;
2661 if (cl && cl->isConnected())
2662 {
2663 const SharedMemoryStatus* stat = cl->processServerStatus();
2664 return (b3SharedMemoryStatusHandle)stat;
2665 }
2666 return 0;
2667 }
2668
b3GetStatusType(b3SharedMemoryStatusHandle statusHandle)2669 B3_SHARED_API int b3GetStatusType(b3SharedMemoryStatusHandle statusHandle)
2670 {
2671 const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
2672 //b3Assert(status);
2673 if (status)
2674 {
2675 return status->m_type;
2676 }
2677 return CMD_INVALID_STATUS;
2678 }
2679
b3GetStatusForwardDynamicsAnalyticsData(b3SharedMemoryStatusHandle statusHandle,struct b3ForwardDynamicsAnalyticsArgs * analyticsData)2680 B3_SHARED_API int b3GetStatusForwardDynamicsAnalyticsData(b3SharedMemoryStatusHandle statusHandle, struct b3ForwardDynamicsAnalyticsArgs* analyticsData)
2681 {
2682 const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
2683 //b3Assert(status);
2684 if (status)
2685 {
2686 *analyticsData = status->m_forwardDynamicsAnalyticsArgs;
2687 return status->m_forwardDynamicsAnalyticsArgs.m_numIslands;
2688 }
2689 return 0;
2690 }
2691
2692
b3GetStatusBodyIndices(b3SharedMemoryStatusHandle statusHandle,int * bodyIndicesOut,int bodyIndicesCapacity)2693 B3_SHARED_API int b3GetStatusBodyIndices(b3SharedMemoryStatusHandle statusHandle, int* bodyIndicesOut, int bodyIndicesCapacity)
2694 {
2695 int numBodies = 0;
2696 const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
2697 b3Assert(status);
2698
2699 if (status)
2700 {
2701 switch (status->m_type)
2702 {
2703 case CMD_MJCF_LOADING_COMPLETED:
2704 case CMD_BULLET_LOADING_COMPLETED:
2705 case CMD_SDF_LOADING_COMPLETED:
2706 {
2707 int i, maxBodies;
2708 numBodies = status->m_sdfLoadedArgs.m_numBodies;
2709 maxBodies = btMin(bodyIndicesCapacity, numBodies);
2710 for (i = 0; i < maxBodies; i++)
2711 {
2712 bodyIndicesOut[i] = status->m_sdfLoadedArgs.m_bodyUniqueIds[i];
2713 }
2714 break;
2715 }
2716 }
2717 }
2718
2719 return numBodies;
2720 }
2721
b3GetStatusBodyIndex(b3SharedMemoryStatusHandle statusHandle)2722 B3_SHARED_API int b3GetStatusBodyIndex(b3SharedMemoryStatusHandle statusHandle)
2723 {
2724 const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
2725 b3Assert(status);
2726 int bodyId = -1;
2727
2728 if (status)
2729 {
2730 switch (status->m_type)
2731 {
2732 case CMD_URDF_LOADING_COMPLETED:
2733 {
2734 bodyId = status->m_dataStreamArguments.m_bodyUniqueId;
2735 break;
2736 }
2737 case CMD_RIGID_BODY_CREATION_COMPLETED:
2738 {
2739 bodyId = status->m_rigidBodyCreateArgs.m_bodyUniqueId;
2740 break;
2741 }
2742 case CMD_CREATE_MULTI_BODY_COMPLETED:
2743 {
2744 bodyId = status->m_dataStreamArguments.m_bodyUniqueId;
2745 break;
2746 }
2747 case CMD_LOAD_SOFT_BODY_COMPLETED:
2748 {
2749 bodyId = status->m_loadSoftBodyResultArguments.m_objectUniqueId;
2750 break;
2751 }
2752 default:
2753 {
2754 b3Assert(0);
2755 }
2756 };
2757 }
2758 return bodyId;
2759 }
2760
b3RequestCollisionInfoCommandInit(b3PhysicsClientHandle physClient,int bodyUniqueId)2761 B3_SHARED_API b3SharedMemoryCommandHandle b3RequestCollisionInfoCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId)
2762 {
2763 PhysicsClient* cl = (PhysicsClient*)physClient;
2764 b3Assert(cl);
2765 b3Assert(cl->canSubmitCommand());
2766 struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
2767 b3Assert(command);
2768 command->m_type = CMD_REQUEST_COLLISION_INFO;
2769 command->m_updateFlags = 0;
2770 command->m_requestCollisionInfoArgs.m_bodyUniqueId = bodyUniqueId;
2771 return (b3SharedMemoryCommandHandle)command;
2772 }
2773
b3GetStatusAABB(b3SharedMemoryStatusHandle statusHandle,int linkIndex,double aabbMin[3],double aabbMax[3])2774 B3_SHARED_API int b3GetStatusAABB(b3SharedMemoryStatusHandle statusHandle, int linkIndex, double aabbMin[3], double aabbMax[3])
2775 {
2776 const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
2777 btAssert(status);
2778 if (status == 0)
2779 return 0;
2780 const b3SendCollisionInfoArgs& args = status->m_sendCollisionInfoArgs;
2781 btAssert(status->m_type == CMD_REQUEST_COLLISION_INFO_COMPLETED);
2782 if (status->m_type != CMD_REQUEST_COLLISION_INFO_COMPLETED)
2783 return 0;
2784
2785 if (linkIndex == -1)
2786 {
2787 aabbMin[0] = args.m_rootWorldAABBMin[0];
2788 aabbMin[1] = args.m_rootWorldAABBMin[1];
2789 aabbMin[2] = args.m_rootWorldAABBMin[2];
2790
2791 aabbMax[0] = args.m_rootWorldAABBMax[0];
2792 aabbMax[1] = args.m_rootWorldAABBMax[1];
2793 aabbMax[2] = args.m_rootWorldAABBMax[2];
2794 return 1;
2795 }
2796
2797 if (linkIndex >= 0 && linkIndex < args.m_numLinks)
2798 {
2799 aabbMin[0] = args.m_linkWorldAABBsMin[linkIndex * 3 + 0];
2800 aabbMin[1] = args.m_linkWorldAABBsMin[linkIndex * 3 + 1];
2801 aabbMin[2] = args.m_linkWorldAABBsMin[linkIndex * 3 + 2];
2802
2803 aabbMax[0] = args.m_linkWorldAABBsMax[linkIndex * 3 + 0];
2804 aabbMax[1] = args.m_linkWorldAABBsMax[linkIndex * 3 + 1];
2805 aabbMax[2] = args.m_linkWorldAABBsMax[linkIndex * 3 + 2];
2806 return 1;
2807 }
2808
2809 return 0;
2810 }
2811
b3GetStatusActualState2(b3SharedMemoryStatusHandle statusHandle,int * bodyUniqueId,int * numLinks,int * numDegreeOfFreedomQ,int * numDegreeOfFreedomU,const double * rootLocalInertialFrame[],const double * actualStateQ[],const double * actualStateQdot[],const double * jointReactionForces[],const double * linkLocalInertialFrames[],const double * jointMotorForces[],const double * linkStates[],const double * linkWorldVelocities[])2812 B3_SHARED_API int b3GetStatusActualState2(b3SharedMemoryStatusHandle statusHandle,
2813 int* bodyUniqueId,
2814 int* numLinks,
2815 int* numDegreeOfFreedomQ,
2816 int* numDegreeOfFreedomU,
2817 const double* rootLocalInertialFrame[],
2818 const double* actualStateQ[],
2819 const double* actualStateQdot[],
2820 const double* jointReactionForces[],
2821 const double* linkLocalInertialFrames[],
2822 const double* jointMotorForces[],
2823 const double* linkStates[],
2824 const double* linkWorldVelocities[])
2825 {
2826 const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
2827 btAssert(status);
2828 if (status == 0)
2829 return 0;
2830
2831 b3GetStatusActualState(statusHandle, bodyUniqueId, numDegreeOfFreedomQ, numDegreeOfFreedomU,
2832 rootLocalInertialFrame, actualStateQ, actualStateQdot, jointReactionForces);
2833
2834 const SendActualStateArgs& args = status->m_sendActualStateArgs;
2835 if (numLinks)
2836 {
2837 *numLinks = args.m_numLinks;
2838 }
2839 if (linkLocalInertialFrames)
2840 {
2841 *linkLocalInertialFrames = args.m_stateDetails->m_linkLocalInertialFrames;
2842 }
2843 if (jointMotorForces)
2844 {
2845 *jointMotorForces = args.m_stateDetails->m_jointMotorForce;
2846 }
2847 if (linkStates)
2848 {
2849 *linkStates = args.m_stateDetails->m_linkState;
2850 }
2851 if (linkWorldVelocities)
2852 {
2853 *linkWorldVelocities = args.m_stateDetails->m_linkWorldVelocities;
2854 }
2855 return 1;
2856 }
2857
b3GetStatusActualState(b3SharedMemoryStatusHandle statusHandle,int * bodyUniqueId,int * numDegreeOfFreedomQ,int * numDegreeOfFreedomU,const double * rootLocalInertialFrame[],const double * actualStateQ[],const double * actualStateQdot[],const double * jointReactionForces[])2858 B3_SHARED_API int b3GetStatusActualState(b3SharedMemoryStatusHandle statusHandle,
2859 int* bodyUniqueId,
2860 int* numDegreeOfFreedomQ,
2861 int* numDegreeOfFreedomU,
2862 const double* rootLocalInertialFrame[],
2863 const double* actualStateQ[],
2864 const double* actualStateQdot[],
2865 const double* jointReactionForces[])
2866 {
2867 const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
2868 btAssert(status);
2869 if (status == 0)
2870 return 0;
2871 const SendActualStateArgs& args = status->m_sendActualStateArgs;
2872 btAssert(status->m_type == CMD_ACTUAL_STATE_UPDATE_COMPLETED);
2873 if (status->m_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED)
2874 return false;
2875
2876 if (bodyUniqueId)
2877 {
2878 *bodyUniqueId = args.m_bodyUniqueId;
2879 }
2880 if (numDegreeOfFreedomQ)
2881 {
2882 *numDegreeOfFreedomQ = args.m_numDegreeOfFreedomQ;
2883 }
2884 if (numDegreeOfFreedomU)
2885 {
2886 *numDegreeOfFreedomU = args.m_numDegreeOfFreedomU;
2887 }
2888 if (rootLocalInertialFrame)
2889 {
2890 *rootLocalInertialFrame = args.m_rootLocalInertialFrame;
2891 }
2892 if (actualStateQ)
2893 {
2894 *actualStateQ = args.m_stateDetails->m_actualStateQ;
2895 }
2896 if (actualStateQdot)
2897 {
2898 *actualStateQdot = args.m_stateDetails->m_actualStateQdot;
2899 }
2900 if (jointReactionForces)
2901 {
2902 *jointReactionForces = args.m_stateDetails->m_jointReactionForces;
2903 }
2904 return true;
2905 }
2906
b3CanSubmitCommand(b3PhysicsClientHandle physClient)2907 B3_SHARED_API int b3CanSubmitCommand(b3PhysicsClientHandle physClient)
2908 {
2909 PhysicsClient* cl = (PhysicsClient*)physClient;
2910 if (cl)
2911 {
2912 return (int)cl->canSubmitCommand();
2913 }
2914 return false;
2915 }
2916
b3SubmitClientCommand(b3PhysicsClientHandle physClient,const b3SharedMemoryCommandHandle commandHandle)2917 B3_SHARED_API int b3SubmitClientCommand(b3PhysicsClientHandle physClient, const b3SharedMemoryCommandHandle commandHandle)
2918 {
2919 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
2920 PhysicsClient* cl = (PhysicsClient*)physClient;
2921 b3Assert(command);
2922 b3Assert(cl);
2923 if (command && cl)
2924 {
2925 return (int)cl->submitClientCommand(*command);
2926 }
2927 return -1;
2928 }
2929
2930 #include "../Utils/b3Clock.h"
2931
b3SubmitClientCommandAndWaitStatus(b3PhysicsClientHandle physClient,const b3SharedMemoryCommandHandle commandHandle)2932 B3_SHARED_API b3SharedMemoryStatusHandle b3SubmitClientCommandAndWaitStatus(b3PhysicsClientHandle physClient, const b3SharedMemoryCommandHandle commandHandle)
2933 {
2934 B3_PROFILE("b3SubmitClientCommandAndWaitStatus");
2935 b3Clock clock;
2936 double startTime = clock.getTimeInSeconds();
2937
2938 b3SharedMemoryStatusHandle statusHandle = 0;
2939 b3Assert(commandHandle);
2940 b3Assert(physClient);
2941 if (physClient && commandHandle)
2942 {
2943 PhysicsClient* cl = (PhysicsClient*)physClient;
2944
2945 double timeOutInSeconds = cl->getTimeOut();
2946
2947 {
2948 B3_PROFILE("b3SubmitClientCommand");
2949 b3SubmitClientCommand(physClient, commandHandle);
2950 }
2951 {
2952 B3_PROFILE("b3ProcessServerStatus");
2953 while (cl->isConnected() && (statusHandle == 0) && (clock.getTimeInSeconds() - startTime < timeOutInSeconds))
2954 {
2955 clock.usleep(0);
2956 statusHandle = b3ProcessServerStatus(physClient);
2957 }
2958 }
2959 return (b3SharedMemoryStatusHandle)statusHandle;
2960 }
2961
2962 return 0;
2963 }
2964
2965 ///return the total number of bodies in the simulation
b3GetNumBodies(b3PhysicsClientHandle physClient)2966 B3_SHARED_API int b3GetNumBodies(b3PhysicsClientHandle physClient)
2967 {
2968 PhysicsClient* cl = (PhysicsClient*)physClient;
2969 return cl->getNumBodies();
2970 }
2971
b3GetNumUserConstraints(b3PhysicsClientHandle physClient)2972 B3_SHARED_API int b3GetNumUserConstraints(b3PhysicsClientHandle physClient)
2973 {
2974 PhysicsClient* cl = (PhysicsClient*)physClient;
2975 return cl->getNumUserConstraints();
2976 }
2977
b3GetUserConstraintInfo(b3PhysicsClientHandle physClient,int constraintUniqueId,struct b3UserConstraint * info)2978 B3_SHARED_API int b3GetUserConstraintInfo(b3PhysicsClientHandle physClient, int constraintUniqueId, struct b3UserConstraint* info)
2979 {
2980 PhysicsClient* cl = (PhysicsClient*)physClient;
2981 b3UserConstraint constraintInfo1;
2982 b3Assert(physClient);
2983 b3Assert(info);
2984 b3Assert(constraintUniqueId >= 0);
2985
2986 if (info == 0)
2987 return 0;
2988
2989 if (cl->getUserConstraintInfo(constraintUniqueId, constraintInfo1))
2990 {
2991 *info = constraintInfo1;
2992 return 1;
2993 }
2994 return 0;
2995 }
2996
2997 /// return the user constraint id, given the index in range [0 , b3GetNumUserConstraints() )
b3GetUserConstraintId(b3PhysicsClientHandle physClient,int serialIndex)2998 B3_SHARED_API int b3GetUserConstraintId(b3PhysicsClientHandle physClient, int serialIndex)
2999 {
3000 PhysicsClient* cl = (PhysicsClient*)physClient;
3001 return cl->getUserConstraintId(serialIndex);
3002 }
3003
3004 /// return the body unique id, given the index in range [0 , b3GetNumBodies() )
b3GetBodyUniqueId(b3PhysicsClientHandle physClient,int serialIndex)3005 B3_SHARED_API int b3GetBodyUniqueId(b3PhysicsClientHandle physClient, int serialIndex)
3006 {
3007 PhysicsClient* cl = (PhysicsClient*)physClient;
3008 return cl->getBodyUniqueId(serialIndex);
3009 }
3010
3011 ///given a body unique id, return the body information. See b3BodyInfo in SharedMemoryPublic.h
b3GetBodyInfo(b3PhysicsClientHandle physClient,int bodyUniqueId,struct b3BodyInfo * info)3012 B3_SHARED_API int b3GetBodyInfo(b3PhysicsClientHandle physClient, int bodyUniqueId, struct b3BodyInfo* info)
3013 {
3014 PhysicsClient* cl = (PhysicsClient*)physClient;
3015 return cl->getBodyInfo(bodyUniqueId, *info);
3016 }
3017
b3GetNumJoints(b3PhysicsClientHandle physClient,int bodyUniqueId)3018 B3_SHARED_API int b3GetNumJoints(b3PhysicsClientHandle physClient, int bodyUniqueId)
3019 {
3020 PhysicsClient* cl = (PhysicsClient*)physClient;
3021 return cl->getNumJoints(bodyUniqueId);
3022 }
3023
b3GetNumDofs(b3PhysicsClientHandle physClient,int bodyUniqueId)3024 B3_SHARED_API int b3GetNumDofs(b3PhysicsClientHandle physClient, int bodyUniqueId)
3025 {
3026 PhysicsClient* cl = (PhysicsClient*)physClient;
3027 return cl->getNumDofs(bodyUniqueId);
3028 }
3029
b3ComputeDofCount(b3PhysicsClientHandle physClient,int bodyUniqueId)3030 B3_SHARED_API int b3ComputeDofCount(b3PhysicsClientHandle physClient, int bodyUniqueId)
3031 {
3032 int nj = b3GetNumJoints(physClient, bodyUniqueId);
3033 int j = 0;
3034 int dofCountOrg = 0;
3035 for (j = 0; j < nj; j++)
3036 {
3037 struct b3JointInfo info;
3038 b3GetJointInfo(physClient, bodyUniqueId, j, &info);
3039 switch (info.m_jointType)
3040 {
3041 case eRevoluteType:
3042 {
3043 dofCountOrg += 1;
3044 break;
3045 }
3046 case ePrismaticType:
3047 {
3048 dofCountOrg += 1;
3049 break;
3050 }
3051 case eSphericalType:
3052 {
3053 return -1;
3054 }
3055 case ePlanarType:
3056 {
3057 return -2;
3058 }
3059 default:
3060 {
3061 //fixed joint has 0-dof and at the moment, we don't deal with planar, spherical etc
3062 }
3063 }
3064 }
3065 return dofCountOrg;
3066 }
3067
b3GetJointInfo(b3PhysicsClientHandle physClient,int bodyUniqueId,int jointIndex,struct b3JointInfo * info)3068 B3_SHARED_API int b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyUniqueId, int jointIndex, struct b3JointInfo* info)
3069 {
3070 PhysicsClient* cl = (PhysicsClient*)physClient;
3071 return cl->getJointInfo(bodyUniqueId, jointIndex, *info);
3072 }
3073
b3CreateCustomCommand(b3PhysicsClientHandle physClient)3074 B3_SHARED_API b3SharedMemoryCommandHandle b3CreateCustomCommand(b3PhysicsClientHandle physClient)
3075 {
3076 PhysicsClient* cl = (PhysicsClient*)physClient;
3077 b3Assert(cl);
3078 b3Assert(cl->canSubmitCommand());
3079 struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
3080 b3Assert(command);
3081 command->m_type = CMD_CUSTOM_COMMAND;
3082 command->m_updateFlags = 0;
3083 return (b3SharedMemoryCommandHandle)command;
3084 }
3085
b3CustomCommandLoadPlugin(b3SharedMemoryCommandHandle commandHandle,const char * pluginPath)3086 B3_SHARED_API void b3CustomCommandLoadPlugin(b3SharedMemoryCommandHandle commandHandle, const char* pluginPath)
3087 {
3088 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
3089 b3Assert(command->m_type == CMD_CUSTOM_COMMAND);
3090 if (command->m_type == CMD_CUSTOM_COMMAND)
3091 {
3092 command->m_updateFlags |= CMD_CUSTOM_COMMAND_LOAD_PLUGIN;
3093 command->m_customCommandArgs.m_pluginPath[0] = 0;
3094
3095 int len = strlen(pluginPath);
3096 if (len < MAX_FILENAME_LENGTH)
3097 {
3098 strcpy(command->m_customCommandArgs.m_pluginPath, pluginPath);
3099 }
3100 }
3101 }
3102
b3CustomCommandLoadPluginSetPostFix(b3SharedMemoryCommandHandle commandHandle,const char * postFix)3103 B3_SHARED_API void b3CustomCommandLoadPluginSetPostFix(b3SharedMemoryCommandHandle commandHandle, const char* postFix)
3104 {
3105 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
3106 b3Assert(command->m_type == CMD_CUSTOM_COMMAND);
3107 if (command->m_type == CMD_CUSTOM_COMMAND)
3108 {
3109 command->m_updateFlags |= CMD_CUSTOM_COMMAND_LOAD_PLUGIN_POSTFIX;
3110 command->m_customCommandArgs.m_postFix[0] = 0;
3111
3112 int len = strlen(postFix);
3113 if (len < MAX_FILENAME_LENGTH)
3114 {
3115 strcpy(command->m_customCommandArgs.m_postFix, postFix);
3116 }
3117 }
3118 }
3119
b3GetStatusPluginCommandResult(b3SharedMemoryStatusHandle statusHandle)3120 B3_SHARED_API int b3GetStatusPluginCommandResult(b3SharedMemoryStatusHandle statusHandle)
3121 {
3122 int statusUniqueId = -1;
3123
3124 const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
3125 b3Assert(status);
3126 if (status == 0)
3127 return statusUniqueId;
3128 b3Assert(status->m_type == CMD_CUSTOM_COMMAND_COMPLETED);
3129 if (status->m_type == CMD_CUSTOM_COMMAND_COMPLETED)
3130 {
3131 statusUniqueId = status->m_customCommandResultArgs.m_executeCommandResult;
3132 }
3133 return statusUniqueId;
3134 }
3135
b3GetStatusPluginCommandReturnData(b3PhysicsClientHandle physClient,struct b3UserDataValue * valueOut)3136 B3_SHARED_API int b3GetStatusPluginCommandReturnData(b3PhysicsClientHandle physClient, struct b3UserDataValue* valueOut)
3137 {
3138 PhysicsClient* cl = (PhysicsClient*)physClient;
3139 if (cl)
3140 {
3141 return cl->getCachedReturnData(valueOut);
3142 }
3143 return false;
3144 }
3145
b3GetStatusPluginUniqueId(b3SharedMemoryStatusHandle statusHandle)3146 B3_SHARED_API int b3GetStatusPluginUniqueId(b3SharedMemoryStatusHandle statusHandle)
3147 {
3148 int statusUniqueId = -1;
3149
3150 const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
3151 b3Assert(status);
3152 if (status)
3153 {
3154 b3Assert(status->m_type == CMD_CUSTOM_COMMAND_COMPLETED);
3155 if (status->m_type == CMD_CUSTOM_COMMAND_COMPLETED)
3156 {
3157 statusUniqueId = status->m_customCommandResultArgs.m_pluginUniqueId;
3158 }
3159 }
3160 return statusUniqueId;
3161 }
3162
b3CustomCommandUnloadPlugin(b3SharedMemoryCommandHandle commandHandle,int pluginUniqueId)3163 B3_SHARED_API void b3CustomCommandUnloadPlugin(b3SharedMemoryCommandHandle commandHandle, int pluginUniqueId)
3164 {
3165 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
3166 b3Assert(command->m_type == CMD_CUSTOM_COMMAND);
3167 if (command->m_type == CMD_CUSTOM_COMMAND)
3168 {
3169 command->m_updateFlags |= CMD_CUSTOM_COMMAND_UNLOAD_PLUGIN;
3170 command->m_customCommandArgs.m_pluginUniqueId = pluginUniqueId;
3171 }
3172 }
b3CustomCommandExecutePluginCommand(b3SharedMemoryCommandHandle commandHandle,int pluginUniqueId,const char * textArguments)3173 B3_SHARED_API void b3CustomCommandExecutePluginCommand(b3SharedMemoryCommandHandle commandHandle, int pluginUniqueId, const char* textArguments)
3174 {
3175 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
3176 b3Assert(command->m_type == CMD_CUSTOM_COMMAND);
3177 if (command->m_type == CMD_CUSTOM_COMMAND)
3178 {
3179 command->m_updateFlags |= CMD_CUSTOM_COMMAND_EXECUTE_PLUGIN_COMMAND;
3180 command->m_customCommandArgs.m_pluginUniqueId = pluginUniqueId;
3181 command->m_customCommandArgs.m_startingReturnBytes = 0;
3182 command->m_customCommandArgs.m_arguments.m_numInts = 0;
3183 command->m_customCommandArgs.m_arguments.m_numFloats = 0;
3184 command->m_customCommandArgs.m_arguments.m_text[0] = 0;
3185
3186 int len = textArguments ? strlen(textArguments) : 0;
3187
3188 if (len && len < MAX_FILENAME_LENGTH)
3189 {
3190 strcpy(command->m_customCommandArgs.m_arguments.m_text, textArguments);
3191 }
3192 }
3193 }
3194
b3CustomCommandExecuteAddIntArgument(b3SharedMemoryCommandHandle commandHandle,int intVal)3195 B3_SHARED_API void b3CustomCommandExecuteAddIntArgument(b3SharedMemoryCommandHandle commandHandle, int intVal)
3196 {
3197 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
3198 b3Assert(command->m_type == CMD_CUSTOM_COMMAND);
3199 b3Assert(command->m_updateFlags & CMD_CUSTOM_COMMAND_EXECUTE_PLUGIN_COMMAND);
3200 if (command->m_type == CMD_CUSTOM_COMMAND && (command->m_updateFlags & CMD_CUSTOM_COMMAND_EXECUTE_PLUGIN_COMMAND))
3201 {
3202 int numInts = command->m_customCommandArgs.m_arguments.m_numInts;
3203 if (numInts < B3_MAX_PLUGIN_ARG_SIZE)
3204 {
3205 command->m_customCommandArgs.m_arguments.m_ints[numInts] = intVal;
3206 command->m_customCommandArgs.m_arguments.m_numInts++;
3207 }
3208 }
3209 }
3210
b3CustomCommandExecuteAddFloatArgument(b3SharedMemoryCommandHandle commandHandle,float floatVal)3211 B3_SHARED_API void b3CustomCommandExecuteAddFloatArgument(b3SharedMemoryCommandHandle commandHandle, float floatVal)
3212 {
3213 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
3214 b3Assert(command->m_type == CMD_CUSTOM_COMMAND);
3215 b3Assert(command->m_updateFlags & CMD_CUSTOM_COMMAND_EXECUTE_PLUGIN_COMMAND);
3216 if (command->m_type == CMD_CUSTOM_COMMAND && (command->m_updateFlags & CMD_CUSTOM_COMMAND_EXECUTE_PLUGIN_COMMAND))
3217 {
3218 int numFloats = command->m_customCommandArgs.m_arguments.m_numFloats;
3219 if (numFloats < B3_MAX_PLUGIN_ARG_SIZE)
3220 {
3221 command->m_customCommandArgs.m_arguments.m_floats[numFloats] = floatVal;
3222 command->m_customCommandArgs.m_arguments.m_numFloats++;
3223 }
3224 }
3225 }
3226
b3GetDynamicsInfoCommandInit(b3PhysicsClientHandle physClient,int bodyUniqueId,int linkIndex)3227 B3_SHARED_API b3SharedMemoryCommandHandle b3GetDynamicsInfoCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex)
3228 {
3229 PhysicsClient* cl = (PhysicsClient*)physClient;
3230 b3Assert(cl);
3231 b3Assert(cl->canSubmitCommand());
3232 struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
3233 b3Assert(command);
3234 return b3GetDynamicsInfoCommandInit2((b3SharedMemoryCommandHandle)command, bodyUniqueId, linkIndex);
3235 }
3236
b3GetDynamicsInfoCommandInit2(b3SharedMemoryCommandHandle commandHandle,int bodyUniqueId,int linkIndex)3237 B3_SHARED_API b3SharedMemoryCommandHandle b3GetDynamicsInfoCommandInit2(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex)
3238 {
3239 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
3240 command->m_type = CMD_GET_DYNAMICS_INFO;
3241 command->m_getDynamicsInfoArgs.m_bodyUniqueId = bodyUniqueId;
3242 command->m_getDynamicsInfoArgs.m_linkIndex = linkIndex;
3243 return (b3SharedMemoryCommandHandle)command;
3244 }
3245
b3GetDynamicsInfo(b3SharedMemoryStatusHandle statusHandle,struct b3DynamicsInfo * info)3246 B3_SHARED_API int b3GetDynamicsInfo(b3SharedMemoryStatusHandle statusHandle, struct b3DynamicsInfo* info)
3247 {
3248 const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
3249 const b3DynamicsInfo& dynamicsInfo = status->m_dynamicsInfo;
3250 btAssert(status->m_type == CMD_GET_DYNAMICS_INFO_COMPLETED);
3251 if (status->m_type != CMD_GET_DYNAMICS_INFO_COMPLETED)
3252 return false;
3253
3254 if (info)
3255 {
3256 *info = dynamicsInfo;
3257 return true;
3258 }
3259 return false;
3260 }
3261
b3InitChangeDynamicsInfo(b3PhysicsClientHandle physClient)3262 B3_SHARED_API b3SharedMemoryCommandHandle b3InitChangeDynamicsInfo(b3PhysicsClientHandle physClient)
3263 {
3264 PhysicsClient* cl = (PhysicsClient*)physClient;
3265 b3Assert(cl);
3266 b3Assert(cl->canSubmitCommand());
3267 struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
3268 b3Assert(command);
3269 return b3InitChangeDynamicsInfo2((b3SharedMemoryCommandHandle)command);
3270 }
3271
b3InitChangeDynamicsInfo2(b3SharedMemoryCommandHandle commandHandle)3272 B3_SHARED_API b3SharedMemoryCommandHandle b3InitChangeDynamicsInfo2(b3SharedMemoryCommandHandle commandHandle)
3273 {
3274 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
3275 command->m_type = CMD_CHANGE_DYNAMICS_INFO;
3276 command->m_changeDynamicsInfoArgs.m_bodyUniqueId = -1;
3277 command->m_changeDynamicsInfoArgs.m_linkIndex = -2;
3278 command->m_updateFlags = 0;
3279 return commandHandle;
3280 }
3281
b3ChangeDynamicsInfoSetMass(b3SharedMemoryCommandHandle commandHandle,int bodyUniqueId,int linkIndex,double mass)3282 B3_SHARED_API int b3ChangeDynamicsInfoSetMass(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double mass)
3283 {
3284 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
3285 b3Assert(command->m_type == CMD_CHANGE_DYNAMICS_INFO);
3286 b3Assert(mass >= 0);
3287 command->m_changeDynamicsInfoArgs.m_bodyUniqueId = bodyUniqueId;
3288 command->m_changeDynamicsInfoArgs.m_linkIndex = linkIndex;
3289 command->m_changeDynamicsInfoArgs.m_mass = mass;
3290 command->m_updateFlags |= CHANGE_DYNAMICS_INFO_SET_MASS;
3291 return 0;
3292 }
3293
b3ChangeDynamicsInfoSetAnisotropicFriction(b3SharedMemoryCommandHandle commandHandle,int bodyUniqueId,int linkIndex,const double anisotropicFriction[])3294 B3_SHARED_API int b3ChangeDynamicsInfoSetAnisotropicFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, const double anisotropicFriction[])
3295 {
3296 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
3297 b3Assert(command->m_type == CMD_CHANGE_DYNAMICS_INFO);
3298 command->m_changeDynamicsInfoArgs.m_bodyUniqueId = bodyUniqueId;
3299 command->m_changeDynamicsInfoArgs.m_linkIndex = linkIndex;
3300 command->m_changeDynamicsInfoArgs.m_anisotropicFriction[0] = anisotropicFriction[0];
3301 command->m_changeDynamicsInfoArgs.m_anisotropicFriction[1] = anisotropicFriction[1];
3302 command->m_changeDynamicsInfoArgs.m_anisotropicFriction[2] = anisotropicFriction[2];
3303
3304 command->m_updateFlags |= CHANGE_DYNAMICS_INFO_SET_ANISOTROPIC_FRICTION;
3305 return 0;
3306 }
3307
b3ChangeDynamicsInfoSetJointLimit(b3SharedMemoryCommandHandle commandHandle,int bodyUniqueId,int linkIndex,double jointLowerLimit,double jointUpperLimit)3308 B3_SHARED_API int b3ChangeDynamicsInfoSetJointLimit(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double jointLowerLimit, double jointUpperLimit)
3309 {
3310 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
3311 b3Assert(command->m_type == CMD_CHANGE_DYNAMICS_INFO);
3312 command->m_changeDynamicsInfoArgs.m_bodyUniqueId = bodyUniqueId;
3313 command->m_changeDynamicsInfoArgs.m_linkIndex = linkIndex;
3314 command->m_changeDynamicsInfoArgs.m_jointLowerLimit = jointLowerLimit;
3315 command->m_changeDynamicsInfoArgs.m_jointUpperLimit = jointUpperLimit;
3316 command->m_updateFlags |= CHANGE_DYNAMICS_INFO_SET_JOINT_LIMITS;
3317 return 0;
3318 }
3319
b3ChangeDynamicsInfoSetJointLimitForce(b3SharedMemoryCommandHandle commandHandle,int bodyUniqueId,int linkIndex,double jointLimitForce)3320 B3_SHARED_API int b3ChangeDynamicsInfoSetJointLimitForce(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double jointLimitForce)
3321 {
3322 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
3323 b3Assert(command->m_type == CMD_CHANGE_DYNAMICS_INFO);
3324 command->m_changeDynamicsInfoArgs.m_bodyUniqueId = bodyUniqueId;
3325 command->m_changeDynamicsInfoArgs.m_linkIndex = linkIndex;
3326 command->m_changeDynamicsInfoArgs.m_jointLimitForce = jointLimitForce;
3327 command->m_updateFlags |= CHANGE_DYNAMICS_INFO_SET_JOINT_LIMIT_MAX_FORCE;
3328 return 0;
3329 }
3330
b3ChangeDynamicsInfoSetSleepThreshold(b3SharedMemoryCommandHandle commandHandle,int bodyUniqueId,double sleepThreshold)3331 B3_SHARED_API int b3ChangeDynamicsInfoSetSleepThreshold(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, double sleepThreshold)
3332 {
3333 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
3334 b3Assert(command->m_type == CMD_CHANGE_DYNAMICS_INFO);
3335 command->m_changeDynamicsInfoArgs.m_bodyUniqueId = bodyUniqueId;
3336 command->m_changeDynamicsInfoArgs.m_sleepThreshold = sleepThreshold;
3337 command->m_updateFlags |= CHANGE_DYNAMICS_INFO_SET_SLEEP_THRESHOLD;
3338 return 0;
3339 }
3340
3341
3342
b3ChangeDynamicsInfoSetDynamicType(b3SharedMemoryCommandHandle commandHandle,int bodyUniqueId,int linkIndex,int dynamicType)3343 B3_SHARED_API int b3ChangeDynamicsInfoSetDynamicType(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, int dynamicType)
3344 {
3345 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
3346 b3Assert(command->m_type == CMD_CHANGE_DYNAMICS_INFO);
3347 b3Assert(dynamicType == eDynamic || dynamicType == eStatic || dynamicType == eKinematic);
3348 command->m_changeDynamicsInfoArgs.m_bodyUniqueId = bodyUniqueId;
3349 command->m_changeDynamicsInfoArgs.m_linkIndex = linkIndex;
3350 command->m_changeDynamicsInfoArgs.m_dynamicType = dynamicType;
3351 command->m_updateFlags |= CHANGE_DYNAMICS_INFO_SET_DYNAMIC_TYPE;
3352 return 0;
3353 }
3354
3355
3356
3357
b3ChangeDynamicsInfoSetLocalInertiaDiagonal(b3SharedMemoryCommandHandle commandHandle,int bodyUniqueId,int linkIndex,const double localInertiaDiagonal[])3358 B3_SHARED_API int b3ChangeDynamicsInfoSetLocalInertiaDiagonal(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, const double localInertiaDiagonal[])
3359 {
3360 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
3361 b3Assert(command->m_type == CMD_CHANGE_DYNAMICS_INFO);
3362 command->m_changeDynamicsInfoArgs.m_bodyUniqueId = bodyUniqueId;
3363 command->m_changeDynamicsInfoArgs.m_linkIndex = linkIndex;
3364 command->m_changeDynamicsInfoArgs.m_localInertiaDiagonal[0] = localInertiaDiagonal[0];
3365 command->m_changeDynamicsInfoArgs.m_localInertiaDiagonal[1] = localInertiaDiagonal[1];
3366 command->m_changeDynamicsInfoArgs.m_localInertiaDiagonal[2] = localInertiaDiagonal[2];
3367
3368 command->m_updateFlags |= CHANGE_DYNAMICS_INFO_SET_LOCAL_INERTIA_DIAGONAL;
3369 return 0;
3370 }
3371
b3ChangeDynamicsInfoSetLateralFriction(b3SharedMemoryCommandHandle commandHandle,int bodyUniqueId,int linkIndex,double lateralFriction)3372 B3_SHARED_API int b3ChangeDynamicsInfoSetLateralFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double lateralFriction)
3373 {
3374 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
3375 b3Assert(command->m_type == CMD_CHANGE_DYNAMICS_INFO);
3376 command->m_changeDynamicsInfoArgs.m_bodyUniqueId = bodyUniqueId;
3377 command->m_changeDynamicsInfoArgs.m_linkIndex = linkIndex;
3378 command->m_changeDynamicsInfoArgs.m_lateralFriction = lateralFriction;
3379 command->m_updateFlags |= CHANGE_DYNAMICS_INFO_SET_LATERAL_FRICTION;
3380 return 0;
3381 }
3382
b3ChangeDynamicsInfoSetSpinningFriction(b3SharedMemoryCommandHandle commandHandle,int bodyUniqueId,int linkIndex,double friction)3383 B3_SHARED_API int b3ChangeDynamicsInfoSetSpinningFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double friction)
3384 {
3385 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
3386 b3Assert(command->m_type == CMD_CHANGE_DYNAMICS_INFO);
3387 command->m_changeDynamicsInfoArgs.m_bodyUniqueId = bodyUniqueId;
3388 command->m_changeDynamicsInfoArgs.m_linkIndex = linkIndex;
3389 command->m_changeDynamicsInfoArgs.m_spinningFriction = friction;
3390 command->m_updateFlags |= CHANGE_DYNAMICS_INFO_SET_SPINNING_FRICTION;
3391 return 0;
3392 }
b3ChangeDynamicsInfoSetRollingFriction(b3SharedMemoryCommandHandle commandHandle,int bodyUniqueId,int linkIndex,double friction)3393 B3_SHARED_API int b3ChangeDynamicsInfoSetRollingFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double friction)
3394 {
3395 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
3396 b3Assert(command->m_type == CMD_CHANGE_DYNAMICS_INFO);
3397 command->m_changeDynamicsInfoArgs.m_bodyUniqueId = bodyUniqueId;
3398 command->m_changeDynamicsInfoArgs.m_linkIndex = linkIndex;
3399 command->m_changeDynamicsInfoArgs.m_rollingFriction = friction;
3400 command->m_updateFlags |= CHANGE_DYNAMICS_INFO_SET_ROLLING_FRICTION;
3401 return 0;
3402 }
3403
b3ChangeDynamicsInfoSetRestitution(b3SharedMemoryCommandHandle commandHandle,int bodyUniqueId,int linkIndex,double restitution)3404 B3_SHARED_API int b3ChangeDynamicsInfoSetRestitution(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double restitution)
3405 {
3406 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
3407 b3Assert(command->m_type == CMD_CHANGE_DYNAMICS_INFO);
3408 command->m_changeDynamicsInfoArgs.m_bodyUniqueId = bodyUniqueId;
3409 command->m_changeDynamicsInfoArgs.m_linkIndex = linkIndex;
3410 command->m_changeDynamicsInfoArgs.m_restitution = restitution;
3411 command->m_updateFlags |= CHANGE_DYNAMICS_INFO_SET_RESTITUTION;
3412 return 0;
3413 }
b3ChangeDynamicsInfoSetLinearDamping(b3SharedMemoryCommandHandle commandHandle,int bodyUniqueId,double linearDamping)3414 B3_SHARED_API int b3ChangeDynamicsInfoSetLinearDamping(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, double linearDamping)
3415 {
3416 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
3417 b3Assert(command->m_type == CMD_CHANGE_DYNAMICS_INFO);
3418 command->m_changeDynamicsInfoArgs.m_bodyUniqueId = bodyUniqueId;
3419 command->m_changeDynamicsInfoArgs.m_linkIndex = -1;
3420 command->m_changeDynamicsInfoArgs.m_linearDamping = linearDamping;
3421 command->m_updateFlags |= CHANGE_DYNAMICS_INFO_SET_LINEAR_DAMPING;
3422 return 0;
3423 }
3424
b3ChangeDynamicsInfoSetAngularDamping(b3SharedMemoryCommandHandle commandHandle,int bodyUniqueId,double angularDamping)3425 B3_SHARED_API int b3ChangeDynamicsInfoSetAngularDamping(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, double angularDamping)
3426 {
3427 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
3428 b3Assert(command->m_type == CMD_CHANGE_DYNAMICS_INFO);
3429 command->m_changeDynamicsInfoArgs.m_bodyUniqueId = bodyUniqueId;
3430 command->m_changeDynamicsInfoArgs.m_linkIndex = -1;
3431 command->m_changeDynamicsInfoArgs.m_angularDamping = angularDamping;
3432 command->m_updateFlags |= CHANGE_DYNAMICS_INFO_SET_ANGULAR_DAMPING;
3433 return 0;
3434 }
3435
b3ChangeDynamicsInfoSetJointDamping(b3SharedMemoryCommandHandle commandHandle,int bodyUniqueId,int linkIndex,double jointDamping)3436 B3_SHARED_API int b3ChangeDynamicsInfoSetJointDamping(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double jointDamping)
3437 {
3438 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
3439 b3Assert(command->m_type == CMD_CHANGE_DYNAMICS_INFO);
3440 command->m_changeDynamicsInfoArgs.m_bodyUniqueId = bodyUniqueId;
3441 command->m_changeDynamicsInfoArgs.m_linkIndex = linkIndex;
3442 command->m_changeDynamicsInfoArgs.m_jointDamping = jointDamping;
3443 command->m_updateFlags |= CHANGE_DYNAMICS_INFO_SET_JOINT_DAMPING;
3444 return 0;
3445 }
3446
3447
b3ChangeDynamicsInfoSetContactStiffnessAndDamping(b3SharedMemoryCommandHandle commandHandle,int bodyUniqueId,int linkIndex,double contactStiffness,double contactDamping)3448 B3_SHARED_API int b3ChangeDynamicsInfoSetContactStiffnessAndDamping(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double contactStiffness, double contactDamping)
3449 {
3450 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
3451 b3Assert(command->m_type == CMD_CHANGE_DYNAMICS_INFO);
3452 command->m_changeDynamicsInfoArgs.m_bodyUniqueId = bodyUniqueId;
3453 command->m_changeDynamicsInfoArgs.m_linkIndex = linkIndex;
3454 command->m_changeDynamicsInfoArgs.m_contactStiffness = contactStiffness;
3455 command->m_changeDynamicsInfoArgs.m_contactDamping = contactDamping;
3456 command->m_updateFlags |= CHANGE_DYNAMICS_INFO_SET_CONTACT_STIFFNESS_AND_DAMPING;
3457 return 0;
3458 }
3459
b3ChangeDynamicsInfoSetFrictionAnchor(b3SharedMemoryCommandHandle commandHandle,int bodyUniqueId,int linkIndex,int frictionAnchor)3460 B3_SHARED_API int b3ChangeDynamicsInfoSetFrictionAnchor(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, int frictionAnchor)
3461 {
3462 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
3463 b3Assert(command->m_type == CMD_CHANGE_DYNAMICS_INFO);
3464 command->m_changeDynamicsInfoArgs.m_bodyUniqueId = bodyUniqueId;
3465 command->m_changeDynamicsInfoArgs.m_linkIndex = linkIndex;
3466 command->m_changeDynamicsInfoArgs.m_frictionAnchor = frictionAnchor;
3467 command->m_updateFlags |= CHANGE_DYNAMICS_INFO_SET_FRICTION_ANCHOR;
3468 return 0;
3469 }
3470
b3ChangeDynamicsInfoSetCcdSweptSphereRadius(b3SharedMemoryCommandHandle commandHandle,int bodyUniqueId,int linkIndex,double ccdSweptSphereRadius)3471 B3_SHARED_API int b3ChangeDynamicsInfoSetCcdSweptSphereRadius(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double ccdSweptSphereRadius)
3472 {
3473 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
3474 b3Assert(command->m_type == CMD_CHANGE_DYNAMICS_INFO);
3475 command->m_changeDynamicsInfoArgs.m_bodyUniqueId = bodyUniqueId;
3476 command->m_changeDynamicsInfoArgs.m_linkIndex = linkIndex;
3477 command->m_changeDynamicsInfoArgs.m_ccdSweptSphereRadius = ccdSweptSphereRadius;
3478 command->m_updateFlags |= CHANGE_DYNAMICS_INFO_SET_CCD_SWEPT_SPHERE_RADIUS;
3479 return 0;
3480 }
3481
b3ChangeDynamicsInfoSetContactProcessingThreshold(b3SharedMemoryCommandHandle commandHandle,int bodyUniqueId,int linkIndex,double contactProcessingThreshold)3482 B3_SHARED_API int b3ChangeDynamicsInfoSetContactProcessingThreshold(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double contactProcessingThreshold)
3483 {
3484 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
3485 b3Assert(command->m_type == CMD_CHANGE_DYNAMICS_INFO);
3486 command->m_changeDynamicsInfoArgs.m_bodyUniqueId = bodyUniqueId;
3487 command->m_changeDynamicsInfoArgs.m_linkIndex = linkIndex;
3488 command->m_changeDynamicsInfoArgs.m_contactProcessingThreshold = contactProcessingThreshold;
3489 command->m_updateFlags |= CHANGE_DYNAMICS_INFO_SET_CONTACT_PROCESSING_THRESHOLD;
3490 return 0;
3491 }
3492
b3ChangeDynamicsInfoSetMaxJointVelocity(b3SharedMemoryCommandHandle commandHandle,int bodyUniqueId,double maxJointVelocity)3493 B3_SHARED_API int b3ChangeDynamicsInfoSetMaxJointVelocity(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, double maxJointVelocity)
3494 {
3495 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
3496 b3Assert(command->m_type == CMD_CHANGE_DYNAMICS_INFO);
3497 command->m_changeDynamicsInfoArgs.m_bodyUniqueId = bodyUniqueId;
3498 command->m_changeDynamicsInfoArgs.m_linkIndex = -1;
3499 command->m_changeDynamicsInfoArgs.m_maxJointVelocity = maxJointVelocity;
3500 command->m_updateFlags |= CHANGE_DYNAMICS_INFO_SET_MAX_JOINT_VELOCITY;
3501 return 0;
3502 }
3503
b3ChangeDynamicsInfoSetCollisionMargin(b3SharedMemoryCommandHandle commandHandle,int bodyUniqueId,double collisionMargin)3504 B3_SHARED_API int b3ChangeDynamicsInfoSetCollisionMargin(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, double collisionMargin)
3505 {
3506 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
3507 b3Assert(command->m_type == CMD_CHANGE_DYNAMICS_INFO);
3508 command->m_changeDynamicsInfoArgs.m_bodyUniqueId = bodyUniqueId;
3509 command->m_changeDynamicsInfoArgs.m_linkIndex = -1;
3510 command->m_changeDynamicsInfoArgs.m_collisionMargin = collisionMargin;
3511 command->m_updateFlags |= CHANGE_DYNAMICS_INFO_SET_COLLISION_MARGIN;
3512 return 0;
3513 }
3514
3515
3516
3517
3518
b3ChangeDynamicsInfoSetActivationState(b3SharedMemoryCommandHandle commandHandle,int bodyUniqueId,int activationState)3519 B3_SHARED_API int b3ChangeDynamicsInfoSetActivationState(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int activationState)
3520 {
3521 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
3522 b3Assert(command->m_type == CMD_CHANGE_DYNAMICS_INFO);
3523 command->m_changeDynamicsInfoArgs.m_bodyUniqueId = bodyUniqueId;
3524 command->m_changeDynamicsInfoArgs.m_linkIndex = -1;
3525 command->m_changeDynamicsInfoArgs.m_activationState = activationState;
3526 command->m_updateFlags |= CHANGE_DYNAMICS_INFO_SET_ACTIVATION_STATE;
3527 return 0;
3528 }
3529
b3InitCreateSoftBodyAnchorConstraintCommand(b3PhysicsClientHandle physClient,int softBodyUniqueId,int nodeIndex,int bodyUniqueId,int linkIndex,const double bodyFramePosition[3])3530 B3_SHARED_API b3SharedMemoryCommandHandle b3InitCreateSoftBodyAnchorConstraintCommand(b3PhysicsClientHandle physClient, int softBodyUniqueId, int nodeIndex, int bodyUniqueId, int linkIndex, const double bodyFramePosition[3])
3531 {
3532 PhysicsClient* cl = (PhysicsClient*)physClient;
3533 b3Assert(cl);
3534 b3Assert(cl->canSubmitCommand());
3535 struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
3536 b3Assert(command);
3537 command->m_type = CMD_USER_CONSTRAINT;
3538 command->m_updateFlags = USER_CONSTRAINT_ADD_SOFT_BODY_ANCHOR;
3539 command->m_userConstraintArguments.m_parentBodyIndex = softBodyUniqueId;
3540 command->m_userConstraintArguments.m_parentJointIndex = nodeIndex;
3541 command->m_userConstraintArguments.m_childBodyIndex = bodyUniqueId;
3542 command->m_userConstraintArguments.m_childJointIndex = linkIndex;
3543 command->m_userConstraintArguments.m_childFrame[0] = bodyFramePosition[0];
3544 command->m_userConstraintArguments.m_childFrame[1] = bodyFramePosition[1];
3545 command->m_userConstraintArguments.m_childFrame[2] = bodyFramePosition[2];
3546 command->m_userConstraintArguments.m_childFrame[3] = 0.;
3547 command->m_userConstraintArguments.m_childFrame[4] = 0.;
3548 command->m_userConstraintArguments.m_childFrame[5] = 0.;
3549 command->m_userConstraintArguments.m_childFrame[6] = 1.;
3550
3551 return (b3SharedMemoryCommandHandle)command;
3552 }
3553
b3InitCreateUserConstraintCommand(b3PhysicsClientHandle physClient,int parentBodyUniqueId,int parentJointIndex,int childBodyUniqueId,int childJointIndex,struct b3JointInfo * info)3554 B3_SHARED_API b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3PhysicsClientHandle physClient, int parentBodyUniqueId, int parentJointIndex, int childBodyUniqueId, int childJointIndex, struct b3JointInfo* info)
3555 {
3556 PhysicsClient* cl = (PhysicsClient*)physClient;
3557 b3Assert(cl);
3558 b3Assert(cl->canSubmitCommand());
3559 struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
3560 b3Assert(command);
3561 return b3InitCreateUserConstraintCommand2((b3SharedMemoryCommandHandle)command, parentBodyUniqueId, parentJointIndex, childBodyUniqueId, childJointIndex, info);
3562 }
3563
b3InitCreateUserConstraintCommand2(b3SharedMemoryCommandHandle commandHandle,int parentBodyUniqueId,int parentJointIndex,int childBodyUniqueId,int childJointIndex,struct b3JointInfo * info)3564 B3_SHARED_API b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand2(b3SharedMemoryCommandHandle commandHandle, int parentBodyUniqueId, int parentJointIndex, int childBodyUniqueId, int childJointIndex, struct b3JointInfo* info)
3565 {
3566 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
3567 command->m_type = CMD_USER_CONSTRAINT;
3568 command->m_updateFlags = USER_CONSTRAINT_ADD_CONSTRAINT;
3569
3570 command->m_userConstraintArguments.m_parentBodyIndex = parentBodyUniqueId;
3571 command->m_userConstraintArguments.m_parentJointIndex = parentJointIndex;
3572 command->m_userConstraintArguments.m_childBodyIndex = childBodyUniqueId;
3573 command->m_userConstraintArguments.m_childJointIndex = childJointIndex;
3574 for (int i = 0; i < 7; ++i)
3575 {
3576 command->m_userConstraintArguments.m_parentFrame[i] = info->m_parentFrame[i];
3577 command->m_userConstraintArguments.m_childFrame[i] = info->m_childFrame[i];
3578 }
3579 for (int i = 0; i < 3; ++i)
3580 {
3581 command->m_userConstraintArguments.m_jointAxis[i] = info->m_jointAxis[i];
3582 }
3583 command->m_userConstraintArguments.m_jointType = info->m_jointType;
3584 return (b3SharedMemoryCommandHandle)command;
3585 }
3586
b3InitChangeUserConstraintCommand(b3PhysicsClientHandle physClient,int userConstraintUniqueId)3587 B3_SHARED_API b3SharedMemoryCommandHandle b3InitChangeUserConstraintCommand(b3PhysicsClientHandle physClient, int userConstraintUniqueId)
3588 {
3589 PhysicsClient* cl = (PhysicsClient*)physClient;
3590 b3Assert(cl);
3591 b3Assert(cl->canSubmitCommand());
3592 struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
3593 b3Assert(command);
3594 command->m_type = CMD_USER_CONSTRAINT;
3595 command->m_updateFlags = USER_CONSTRAINT_CHANGE_CONSTRAINT;
3596 command->m_userConstraintArguments.m_userConstraintUniqueId = userConstraintUniqueId;
3597 return (b3SharedMemoryCommandHandle)command;
3598 }
3599
b3InitChangeUserConstraintSetPivotInB(b3SharedMemoryCommandHandle commandHandle,const double jointChildPivot[])3600 B3_SHARED_API int b3InitChangeUserConstraintSetPivotInB(b3SharedMemoryCommandHandle commandHandle, const double jointChildPivot[])
3601 {
3602 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
3603 b3Assert(command);
3604 b3Assert(command->m_type == CMD_USER_CONSTRAINT);
3605
3606 b3Assert(command->m_updateFlags & USER_CONSTRAINT_CHANGE_CONSTRAINT);
3607
3608 command->m_updateFlags |= USER_CONSTRAINT_CHANGE_PIVOT_IN_B;
3609
3610 command->m_userConstraintArguments.m_childFrame[0] = jointChildPivot[0];
3611 command->m_userConstraintArguments.m_childFrame[1] = jointChildPivot[1];
3612 command->m_userConstraintArguments.m_childFrame[2] = jointChildPivot[2];
3613 return 0;
3614 }
b3InitChangeUserConstraintSetFrameInB(b3SharedMemoryCommandHandle commandHandle,const double jointChildFrameOrn[])3615 B3_SHARED_API int b3InitChangeUserConstraintSetFrameInB(b3SharedMemoryCommandHandle commandHandle, const double jointChildFrameOrn[])
3616 {
3617 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
3618 b3Assert(command);
3619 b3Assert(command->m_type == CMD_USER_CONSTRAINT);
3620 b3Assert(command->m_updateFlags & USER_CONSTRAINT_CHANGE_CONSTRAINT);
3621
3622 command->m_updateFlags |= USER_CONSTRAINT_CHANGE_FRAME_ORN_IN_B;
3623
3624 command->m_userConstraintArguments.m_childFrame[3] = jointChildFrameOrn[0];
3625 command->m_userConstraintArguments.m_childFrame[4] = jointChildFrameOrn[1];
3626 command->m_userConstraintArguments.m_childFrame[5] = jointChildFrameOrn[2];
3627 command->m_userConstraintArguments.m_childFrame[6] = jointChildFrameOrn[3];
3628
3629 return 0;
3630 }
3631
b3InitChangeUserConstraintSetMaxForce(b3SharedMemoryCommandHandle commandHandle,double maxAppliedForce)3632 B3_SHARED_API int b3InitChangeUserConstraintSetMaxForce(b3SharedMemoryCommandHandle commandHandle, double maxAppliedForce)
3633 {
3634 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
3635 b3Assert(command);
3636 b3Assert(command->m_type == CMD_USER_CONSTRAINT);
3637 b3Assert(command->m_updateFlags & USER_CONSTRAINT_CHANGE_CONSTRAINT);
3638
3639 command->m_updateFlags |= USER_CONSTRAINT_CHANGE_MAX_FORCE;
3640 command->m_userConstraintArguments.m_maxAppliedForce = maxAppliedForce;
3641
3642 return 0;
3643 }
b3InitChangeUserConstraintSetGearRatio(b3SharedMemoryCommandHandle commandHandle,double gearRatio)3644 B3_SHARED_API int b3InitChangeUserConstraintSetGearRatio(b3SharedMemoryCommandHandle commandHandle, double gearRatio)
3645 {
3646 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
3647 b3Assert(command);
3648 b3Assert(command->m_type == CMD_USER_CONSTRAINT);
3649 b3Assert(command->m_updateFlags & USER_CONSTRAINT_CHANGE_CONSTRAINT);
3650
3651 command->m_updateFlags |= USER_CONSTRAINT_CHANGE_GEAR_RATIO;
3652 command->m_userConstraintArguments.m_gearRatio = gearRatio;
3653
3654 return 0;
3655 }
3656
b3InitChangeUserConstraintSetGearAuxLink(b3SharedMemoryCommandHandle commandHandle,int gearAuxLink)3657 B3_SHARED_API int b3InitChangeUserConstraintSetGearAuxLink(b3SharedMemoryCommandHandle commandHandle, int gearAuxLink)
3658 {
3659 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
3660 b3Assert(command);
3661 b3Assert(command->m_type == CMD_USER_CONSTRAINT);
3662 b3Assert(command->m_updateFlags & USER_CONSTRAINT_CHANGE_CONSTRAINT);
3663
3664 command->m_updateFlags |= USER_CONSTRAINT_CHANGE_GEAR_AUX_LINK;
3665 command->m_userConstraintArguments.m_gearAuxLink = gearAuxLink;
3666
3667 return 0;
3668 }
3669
b3InitChangeUserConstraintSetRelativePositionTarget(b3SharedMemoryCommandHandle commandHandle,double relativePositionTarget)3670 B3_SHARED_API int b3InitChangeUserConstraintSetRelativePositionTarget(b3SharedMemoryCommandHandle commandHandle, double relativePositionTarget)
3671 {
3672 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
3673 b3Assert(command);
3674 b3Assert(command->m_type == CMD_USER_CONSTRAINT);
3675 b3Assert(command->m_updateFlags & USER_CONSTRAINT_CHANGE_CONSTRAINT);
3676 command->m_updateFlags |= USER_CONSTRAINT_CHANGE_RELATIVE_POSITION_TARGET;
3677 command->m_userConstraintArguments.m_relativePositionTarget = relativePositionTarget;
3678
3679 return 0;
3680 }
b3InitChangeUserConstraintSetERP(b3SharedMemoryCommandHandle commandHandle,double erp)3681 B3_SHARED_API int b3InitChangeUserConstraintSetERP(b3SharedMemoryCommandHandle commandHandle, double erp)
3682 {
3683 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
3684 b3Assert(command);
3685 b3Assert(command->m_type == CMD_USER_CONSTRAINT);
3686 b3Assert(command->m_updateFlags & USER_CONSTRAINT_CHANGE_CONSTRAINT);
3687
3688 command->m_updateFlags |= USER_CONSTRAINT_CHANGE_ERP;
3689 command->m_userConstraintArguments.m_erp = erp;
3690
3691 return 0;
3692 }
3693
b3InitGetUserConstraintStateCommand(b3PhysicsClientHandle physClient,int constraintUniqueId)3694 B3_SHARED_API b3SharedMemoryCommandHandle b3InitGetUserConstraintStateCommand(b3PhysicsClientHandle physClient, int constraintUniqueId)
3695 {
3696 PhysicsClient* cl = (PhysicsClient*)physClient;
3697 b3Assert(cl);
3698 b3Assert(cl->canSubmitCommand());
3699 struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
3700 b3Assert(command);
3701
3702 command->m_type = CMD_USER_CONSTRAINT;
3703 command->m_updateFlags = USER_CONSTRAINT_REQUEST_STATE;
3704 command->m_userConstraintArguments.m_userConstraintUniqueId = constraintUniqueId;
3705 return (b3SharedMemoryCommandHandle)command;
3706 }
3707
b3GetStatusUserConstraintState(b3SharedMemoryStatusHandle statusHandle,struct b3UserConstraintState * constraintState)3708 B3_SHARED_API int b3GetStatusUserConstraintState(b3SharedMemoryStatusHandle statusHandle, struct b3UserConstraintState* constraintState)
3709 {
3710 const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
3711 if (status)
3712 {
3713 btAssert(status->m_type == CMD_USER_CONSTRAINT_REQUEST_STATE_COMPLETED);
3714 if (status && status->m_type == CMD_USER_CONSTRAINT_REQUEST_STATE_COMPLETED)
3715 {
3716 int i = 0;
3717 constraintState->m_numDofs = status->m_userConstraintStateResultArgs.m_numDofs;
3718 for (i = 0; i < constraintState->m_numDofs; i++)
3719 {
3720 constraintState->m_appliedConstraintForces[i] = status->m_userConstraintStateResultArgs.m_appliedConstraintForces[i];
3721 }
3722 for (; i < 6; i++)
3723 {
3724 constraintState->m_appliedConstraintForces[i] = 0;
3725 }
3726 return 1;
3727 }
3728 }
3729 return 0;
3730 }
3731
b3InitRemoveUserConstraintCommand(b3PhysicsClientHandle physClient,int userConstraintUniqueId)3732 B3_SHARED_API b3SharedMemoryCommandHandle b3InitRemoveUserConstraintCommand(b3PhysicsClientHandle physClient, int userConstraintUniqueId)
3733 {
3734 PhysicsClient* cl = (PhysicsClient*)physClient;
3735 b3Assert(cl);
3736 b3Assert(cl->canSubmitCommand());
3737 struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
3738 b3Assert(command);
3739
3740 command->m_type = CMD_USER_CONSTRAINT;
3741 command->m_updateFlags = USER_CONSTRAINT_REMOVE_CONSTRAINT;
3742 command->m_userConstraintArguments.m_userConstraintUniqueId = userConstraintUniqueId;
3743 return (b3SharedMemoryCommandHandle)command;
3744 }
3745
b3InitRemoveBodyCommand(b3PhysicsClientHandle physClient,int bodyUniqueId)3746 B3_SHARED_API b3SharedMemoryCommandHandle b3InitRemoveBodyCommand(b3PhysicsClientHandle physClient, int bodyUniqueId)
3747 {
3748 PhysicsClient* cl = (PhysicsClient*)physClient;
3749 b3Assert(cl);
3750 b3Assert(cl->canSubmitCommand());
3751 struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
3752 b3Assert(command);
3753
3754 command->m_type = CMD_REMOVE_BODY;
3755 command->m_updateFlags = BODY_DELETE_FLAG;
3756 command->m_removeObjectArgs.m_numBodies = 1;
3757 command->m_removeObjectArgs.m_bodyUniqueIds[0] = bodyUniqueId;
3758 command->m_removeObjectArgs.m_numUserCollisionShapes = 0;
3759 command->m_removeObjectArgs.m_numUserConstraints = 0;
3760
3761 return (b3SharedMemoryCommandHandle)command;
3762 }
3763
b3InitRemoveCollisionShapeCommand(b3PhysicsClientHandle physClient,int collisionShapeId)3764 B3_SHARED_API b3SharedMemoryCommandHandle b3InitRemoveCollisionShapeCommand(b3PhysicsClientHandle physClient, int collisionShapeId)
3765 {
3766 PhysicsClient* cl = (PhysicsClient*)physClient;
3767 b3Assert(cl);
3768 b3Assert(cl->canSubmitCommand());
3769 struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
3770 b3Assert(command);
3771
3772 command->m_type = CMD_REMOVE_BODY;
3773 command->m_updateFlags = 0;
3774 command->m_removeObjectArgs.m_numBodies = 0;
3775 command->m_removeObjectArgs.m_numUserConstraints = 0;
3776 command->m_removeObjectArgs.m_numUserCollisionShapes = 1;
3777 command->m_removeObjectArgs.m_userCollisionShapes[0] = collisionShapeId;
3778 return (b3SharedMemoryCommandHandle)command;
3779 }
b3GetStatusUserConstraintUniqueId(b3SharedMemoryStatusHandle statusHandle)3780 B3_SHARED_API int b3GetStatusUserConstraintUniqueId(b3SharedMemoryStatusHandle statusHandle)
3781 {
3782 const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
3783 b3Assert(status);
3784 if (status)
3785 {
3786 b3Assert(status->m_type == CMD_USER_CONSTRAINT_COMPLETED);
3787 if (status && status->m_type == CMD_USER_CONSTRAINT_COMPLETED)
3788 {
3789 return status->m_userConstraintResultArgs.m_userConstraintUniqueId;
3790 }
3791 }
3792 return -1;
3793 }
3794
b3PickBody(b3PhysicsClientHandle physClient,double rayFromWorldX,double rayFromWorldY,double rayFromWorldZ,double rayToWorldX,double rayToWorldY,double rayToWorldZ)3795 B3_SHARED_API b3SharedMemoryCommandHandle b3PickBody(b3PhysicsClientHandle physClient, double rayFromWorldX,
3796 double rayFromWorldY, double rayFromWorldZ,
3797 double rayToWorldX, double rayToWorldY, double rayToWorldZ)
3798 {
3799 PhysicsClient* cl = (PhysicsClient*)physClient;
3800 b3Assert(cl);
3801 b3Assert(cl->canSubmitCommand());
3802 struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
3803 b3Assert(command);
3804 command->m_type = CMD_PICK_BODY;
3805 command->m_pickBodyArguments.m_rayFromWorld[0] = rayFromWorldX;
3806 command->m_pickBodyArguments.m_rayFromWorld[1] = rayFromWorldY;
3807 command->m_pickBodyArguments.m_rayFromWorld[2] = rayFromWorldZ;
3808 command->m_pickBodyArguments.m_rayToWorld[0] = rayToWorldX;
3809 command->m_pickBodyArguments.m_rayToWorld[1] = rayToWorldY;
3810 command->m_pickBodyArguments.m_rayToWorld[2] = rayToWorldZ;
3811 return (b3SharedMemoryCommandHandle)command;
3812 }
3813
b3MovePickedBody(b3PhysicsClientHandle physClient,double rayFromWorldX,double rayFromWorldY,double rayFromWorldZ,double rayToWorldX,double rayToWorldY,double rayToWorldZ)3814 B3_SHARED_API b3SharedMemoryCommandHandle b3MovePickedBody(b3PhysicsClientHandle physClient, double rayFromWorldX,
3815 double rayFromWorldY, double rayFromWorldZ,
3816 double rayToWorldX, double rayToWorldY,
3817 double rayToWorldZ)
3818 {
3819 PhysicsClient* cl = (PhysicsClient*)physClient;
3820 b3Assert(cl);
3821 b3Assert(cl->canSubmitCommand());
3822 struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
3823 b3Assert(command);
3824 command->m_type = CMD_MOVE_PICKED_BODY;
3825 command->m_pickBodyArguments.m_rayFromWorld[0] = rayFromWorldX;
3826 command->m_pickBodyArguments.m_rayFromWorld[1] = rayFromWorldY;
3827 command->m_pickBodyArguments.m_rayFromWorld[2] = rayFromWorldZ;
3828 command->m_pickBodyArguments.m_rayToWorld[0] = rayToWorldX;
3829 command->m_pickBodyArguments.m_rayToWorld[1] = rayToWorldY;
3830 command->m_pickBodyArguments.m_rayToWorld[2] = rayToWorldZ;
3831 return (b3SharedMemoryCommandHandle)command;
3832 }
3833
b3RemovePickingConstraint(b3PhysicsClientHandle physClient)3834 B3_SHARED_API b3SharedMemoryCommandHandle b3RemovePickingConstraint(b3PhysicsClientHandle physClient)
3835 {
3836 PhysicsClient* cl = (PhysicsClient*)physClient;
3837 b3Assert(cl);
3838 b3Assert(cl->canSubmitCommand());
3839 struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
3840 b3Assert(command);
3841 command->m_type = CMD_REMOVE_PICKING_CONSTRAINT_BODY;
3842 return (b3SharedMemoryCommandHandle)command;
3843 }
3844
b3CreateRaycastCommandInit(b3PhysicsClientHandle physClient,double rayFromWorldX,double rayFromWorldY,double rayFromWorldZ,double rayToWorldX,double rayToWorldY,double rayToWorldZ)3845 B3_SHARED_API b3SharedMemoryCommandHandle b3CreateRaycastCommandInit(b3PhysicsClientHandle physClient, double rayFromWorldX,
3846 double rayFromWorldY, double rayFromWorldZ,
3847 double rayToWorldX, double rayToWorldY, double rayToWorldZ)
3848 {
3849 PhysicsClient* cl = (PhysicsClient*)physClient;
3850 b3Assert(cl);
3851 b3Assert(cl->canSubmitCommand());
3852 struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
3853 b3Assert(command);
3854 command->m_type = CMD_REQUEST_RAY_CAST_INTERSECTIONS;
3855 command->m_requestRaycastIntersections.m_numCommandRays = 1;
3856 command->m_requestRaycastIntersections.m_numStreamingRays = 0;
3857 command->m_requestRaycastIntersections.m_numThreads = 1;
3858 command->m_requestRaycastIntersections.m_parentObjectUniqueId = -1;
3859 command->m_requestRaycastIntersections.m_parentLinkIndex=-1;
3860
3861 command->m_requestRaycastIntersections.m_numCommandRays = 1;
3862 command->m_requestRaycastIntersections.m_fromToRays[0].m_rayFromPosition[0] = rayFromWorldX;
3863 command->m_requestRaycastIntersections.m_fromToRays[0].m_rayFromPosition[1] = rayFromWorldY;
3864 command->m_requestRaycastIntersections.m_fromToRays[0].m_rayFromPosition[2] = rayFromWorldZ;
3865
3866 command->m_requestRaycastIntersections.m_fromToRays[0].m_rayToPosition[0] = rayToWorldX;
3867 command->m_requestRaycastIntersections.m_fromToRays[0].m_rayToPosition[1] = rayToWorldY;
3868 command->m_requestRaycastIntersections.m_fromToRays[0].m_rayToPosition[2] = rayToWorldZ;
3869 command->m_requestRaycastIntersections.m_reportHitNumber = -1;
3870 command->m_requestRaycastIntersections.m_collisionFilterMask = -1;
3871 command->m_requestRaycastIntersections.m_fractionEpsilon = B3_EPSILON;
3872
3873
3874
3875 return (b3SharedMemoryCommandHandle)command;
3876 }
3877
b3CreateRaycastBatchCommandInit(b3PhysicsClientHandle physClient)3878 B3_SHARED_API b3SharedMemoryCommandHandle b3CreateRaycastBatchCommandInit(b3PhysicsClientHandle physClient)
3879 {
3880 PhysicsClient* cl = (PhysicsClient*)physClient;
3881 b3Assert(cl);
3882 b3Assert(cl->canSubmitCommand());
3883 struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
3884 b3Assert(command);
3885 command->m_type = CMD_REQUEST_RAY_CAST_INTERSECTIONS;
3886 command->m_updateFlags = 0;
3887 command->m_requestRaycastIntersections.m_numCommandRays = 0;
3888 command->m_requestRaycastIntersections.m_numStreamingRays = 0;
3889 command->m_requestRaycastIntersections.m_numThreads = 1;
3890 command->m_requestRaycastIntersections.m_parentObjectUniqueId = -1;
3891 command->m_requestRaycastIntersections.m_parentLinkIndex=-1;
3892 command->m_requestRaycastIntersections.m_reportHitNumber = -1;
3893 command->m_requestRaycastIntersections.m_collisionFilterMask = -1;
3894 command->m_requestRaycastIntersections.m_fractionEpsilon = B3_EPSILON;
3895
3896 return (b3SharedMemoryCommandHandle)command;
3897 }
3898
b3RaycastBatchSetNumThreads(b3SharedMemoryCommandHandle commandHandle,int numThreads)3899 B3_SHARED_API void b3RaycastBatchSetNumThreads(b3SharedMemoryCommandHandle commandHandle, int numThreads)
3900 {
3901 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
3902 b3Assert(command);
3903 b3Assert(command->m_type == CMD_REQUEST_RAY_CAST_INTERSECTIONS);
3904 command->m_requestRaycastIntersections.m_numThreads = numThreads;
3905 }
3906
b3RaycastBatchAddRay(b3SharedMemoryCommandHandle commandHandle,const double rayFromWorld[3],const double rayToWorld[3])3907 B3_SHARED_API void b3RaycastBatchAddRay(b3SharedMemoryCommandHandle commandHandle, const double rayFromWorld[3], const double rayToWorld[3])
3908 {
3909 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
3910 b3Assert(command);
3911 b3Assert(command->m_type == CMD_REQUEST_RAY_CAST_INTERSECTIONS);
3912 if (command->m_type == CMD_REQUEST_RAY_CAST_INTERSECTIONS)
3913 {
3914 int numRays = command->m_requestRaycastIntersections.m_numCommandRays;
3915 if (numRays < MAX_RAY_INTERSECTION_BATCH_SIZE)
3916 {
3917 command->m_requestRaycastIntersections.m_fromToRays[numRays].m_rayFromPosition[0] = rayFromWorld[0];
3918 command->m_requestRaycastIntersections.m_fromToRays[numRays].m_rayFromPosition[1] = rayFromWorld[1];
3919 command->m_requestRaycastIntersections.m_fromToRays[numRays].m_rayFromPosition[2] = rayFromWorld[2];
3920
3921 command->m_requestRaycastIntersections.m_fromToRays[numRays].m_rayToPosition[0] = rayToWorld[0];
3922 command->m_requestRaycastIntersections.m_fromToRays[numRays].m_rayToPosition[1] = rayToWorld[1];
3923 command->m_requestRaycastIntersections.m_fromToRays[numRays].m_rayToPosition[2] = rayToWorld[2];
3924
3925 command->m_requestRaycastIntersections.m_numCommandRays++;
3926 }
3927 }
3928 }
3929
b3RaycastBatchAddRays(b3PhysicsClientHandle physClient,b3SharedMemoryCommandHandle commandHandle,const double * rayFromWorldArray,const double * rayToWorldArray,int numRays)3930 B3_SHARED_API void b3RaycastBatchAddRays(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, const double* rayFromWorldArray, const double* rayToWorldArray, int numRays)
3931 {
3932 PhysicsClient* cl = (PhysicsClient*)physClient;
3933 b3Assert(cl);
3934
3935 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
3936 b3Assert(command);
3937 b3Assert(command->m_type == CMD_REQUEST_RAY_CAST_INTERSECTIONS);
3938 b3Assert(numRays < MAX_RAY_INTERSECTION_BATCH_SIZE_STREAMING);
3939 if (command->m_type == CMD_REQUEST_RAY_CAST_INTERSECTIONS)
3940 {
3941 cl->uploadRaysToSharedMemory(*command, rayFromWorldArray, rayToWorldArray, numRays);
3942 }
3943 }
3944
b3RaycastBatchSetParentObject(b3SharedMemoryCommandHandle commandHandle,int parentObjectUniqueId,int parentLinkIndex)3945 B3_SHARED_API void b3RaycastBatchSetParentObject(b3SharedMemoryCommandHandle commandHandle, int parentObjectUniqueId, int parentLinkIndex)
3946 {
3947 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
3948 b3Assert(command);
3949 b3Assert(command->m_type == CMD_REQUEST_RAY_CAST_INTERSECTIONS);
3950 command->m_requestRaycastIntersections.m_parentObjectUniqueId = parentObjectUniqueId;
3951 command->m_requestRaycastIntersections.m_parentLinkIndex = parentLinkIndex;
3952 }
3953
b3RaycastBatchSetReportHitNumber(b3SharedMemoryCommandHandle commandHandle,int reportHitNumber)3954 B3_SHARED_API void b3RaycastBatchSetReportHitNumber(b3SharedMemoryCommandHandle commandHandle, int reportHitNumber)
3955 {
3956 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
3957 b3Assert(command);
3958 b3Assert(command->m_type == CMD_REQUEST_RAY_CAST_INTERSECTIONS);
3959 command->m_requestRaycastIntersections.m_reportHitNumber= reportHitNumber;
3960 }
b3RaycastBatchSetCollisionFilterMask(b3SharedMemoryCommandHandle commandHandle,int collisionFilterMask)3961 B3_SHARED_API void b3RaycastBatchSetCollisionFilterMask(b3SharedMemoryCommandHandle commandHandle, int collisionFilterMask)
3962 {
3963 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
3964 b3Assert(command);
3965 b3Assert(command->m_type == CMD_REQUEST_RAY_CAST_INTERSECTIONS);
3966 command->m_requestRaycastIntersections.m_collisionFilterMask = collisionFilterMask;
3967 }
3968
b3RaycastBatchSetFractionEpsilon(b3SharedMemoryCommandHandle commandHandle,double fractionEpsilon)3969 B3_SHARED_API void b3RaycastBatchSetFractionEpsilon(b3SharedMemoryCommandHandle commandHandle, double fractionEpsilon)
3970 {
3971 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
3972 b3Assert(command);
3973 b3Assert(command->m_type == CMD_REQUEST_RAY_CAST_INTERSECTIONS);
3974 command->m_requestRaycastIntersections.m_fractionEpsilon = fractionEpsilon;
3975 }
3976
3977
b3GetRaycastInformation(b3PhysicsClientHandle physClient,struct b3RaycastInformation * raycastInfo)3978 B3_SHARED_API void b3GetRaycastInformation(b3PhysicsClientHandle physClient, struct b3RaycastInformation* raycastInfo)
3979 {
3980 PhysicsClient* cl = (PhysicsClient*)physClient;
3981 if (cl)
3982 {
3983 cl->getCachedRaycastHits(raycastInfo);
3984 }
3985 }
3986
3987 ///If you re-connected to an existing server, or server changed otherwise, sync the body info
b3InitSyncBodyInfoCommand(b3PhysicsClientHandle physClient)3988 B3_SHARED_API b3SharedMemoryCommandHandle b3InitSyncBodyInfoCommand(b3PhysicsClientHandle physClient)
3989 {
3990 PhysicsClient* cl = (PhysicsClient*)physClient;
3991 b3Assert(cl);
3992 b3Assert(cl->canSubmitCommand());
3993 struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
3994 b3Assert(command);
3995
3996 command->m_type = CMD_SYNC_BODY_INFO;
3997 return (b3SharedMemoryCommandHandle)command;
3998 }
3999
b3InitRequestBodyInfoCommand(b3PhysicsClientHandle physClient,int bodyUniqueId)4000 B3_SHARED_API b3SharedMemoryCommandHandle b3InitRequestBodyInfoCommand(b3PhysicsClientHandle physClient, int bodyUniqueId)
4001 {
4002 PhysicsClient* cl = (PhysicsClient*)physClient;
4003 b3Assert(cl);
4004 b3Assert(cl->canSubmitCommand());
4005 struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
4006 b3Assert(command);
4007
4008 command->m_type = CMD_REQUEST_BODY_INFO;
4009 command->m_sdfRequestInfoArgs.m_bodyUniqueId = bodyUniqueId;
4010 return (b3SharedMemoryCommandHandle)command;
4011 }
4012
b3InitSyncUserDataCommand(b3PhysicsClientHandle physClient)4013 B3_SHARED_API b3SharedMemoryCommandHandle b3InitSyncUserDataCommand(b3PhysicsClientHandle physClient)
4014 {
4015 PhysicsClient* cl = (PhysicsClient*)physClient;
4016 b3Assert(cl);
4017 b3Assert(cl->canSubmitCommand());
4018 struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
4019 b3Assert(command);
4020
4021 command->m_type = CMD_SYNC_USER_DATA;
4022 command->m_syncUserDataRequestArgs.m_numRequestedBodies = 0;
4023 return (b3SharedMemoryCommandHandle)command;
4024 }
4025
b3AddBodyToSyncUserDataRequest(b3SharedMemoryCommandHandle commandHandle,int bodyUniqueId)4026 B3_SHARED_API void b3AddBodyToSyncUserDataRequest(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId)
4027 {
4028 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
4029 b3Assert(command);
4030 b3Assert(command->m_type == CMD_SYNC_USER_DATA);
4031
4032 command->m_syncUserDataRequestArgs.m_requestedBodyIds[command->m_syncUserDataRequestArgs.m_numRequestedBodies++] = bodyUniqueId;
4033 }
4034
4035
b3InitAddUserDataCommand(b3PhysicsClientHandle physClient,int bodyUniqueId,int linkIndex,int visualShapeIndex,const char * key,UserDataValueType valueType,int valueLength,const void * valueData)4036 B3_SHARED_API b3SharedMemoryCommandHandle b3InitAddUserDataCommand(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, int visualShapeIndex, const char* key, UserDataValueType valueType, int valueLength, const void* valueData)
4037 {
4038 PhysicsClient* cl = (PhysicsClient*)physClient;
4039 b3Assert(strlen(key) < MAX_USER_DATA_KEY_LENGTH);
4040 b3Assert(cl);
4041 b3Assert(cl->canSubmitCommand());
4042 struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
4043 b3Assert(command);
4044
4045 command->m_type = CMD_ADD_USER_DATA;
4046 command->m_addUserDataRequestArgs.m_bodyUniqueId = bodyUniqueId;
4047 command->m_addUserDataRequestArgs.m_linkIndex = linkIndex;
4048 command->m_addUserDataRequestArgs.m_visualShapeIndex = visualShapeIndex;
4049 command->m_addUserDataRequestArgs.m_valueType = valueType;
4050 command->m_addUserDataRequestArgs.m_valueLength = valueLength;
4051 strcpy(command->m_addUserDataRequestArgs.m_key, key);
4052 cl->uploadBulletFileToSharedMemory((const char*)valueData, valueLength);
4053
4054 return (b3SharedMemoryCommandHandle)command;
4055 }
4056
b3InitRemoveUserDataCommand(b3PhysicsClientHandle physClient,int userDataId)4057 B3_SHARED_API b3SharedMemoryCommandHandle b3InitRemoveUserDataCommand(b3PhysicsClientHandle physClient, int userDataId)
4058 {
4059 PhysicsClient* cl = (PhysicsClient*)physClient;
4060 b3Assert(cl);
4061 b3Assert(cl->canSubmitCommand());
4062 struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
4063 b3Assert(command);
4064
4065 command->m_type = CMD_REMOVE_USER_DATA;
4066 command->m_removeUserDataRequestArgs.m_userDataId = userDataId;
4067
4068 return (b3SharedMemoryCommandHandle)command;
4069 }
4070
b3GetUserData(b3PhysicsClientHandle physClient,int userDataId,struct b3UserDataValue * valueOut)4071 B3_SHARED_API int b3GetUserData(b3PhysicsClientHandle physClient, int userDataId, struct b3UserDataValue* valueOut)
4072 {
4073 PhysicsClient* cl = (PhysicsClient*)physClient;
4074 if (cl)
4075 {
4076 return cl->getCachedUserData(userDataId, *valueOut);
4077 }
4078 return false;
4079 }
4080
b3GetUserDataId(b3PhysicsClientHandle physClient,int bodyUniqueId,int linkIndex,int visualShapeIndex,const char * key)4081 B3_SHARED_API int b3GetUserDataId(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, int visualShapeIndex, const char* key)
4082 {
4083 PhysicsClient* cl = (PhysicsClient*)physClient;
4084 if (cl)
4085 {
4086 return cl->getCachedUserDataId(bodyUniqueId, linkIndex, visualShapeIndex, key);
4087 }
4088 return -1;
4089 }
4090
b3GetUserDataIdFromStatus(b3SharedMemoryStatusHandle statusHandle)4091 B3_SHARED_API int b3GetUserDataIdFromStatus(b3SharedMemoryStatusHandle statusHandle)
4092 {
4093 const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
4094 if (status)
4095 {
4096 btAssert(status->m_type == CMD_ADD_USER_DATA_COMPLETED);
4097 return status->m_userDataResponseArgs.m_userDataId;
4098 }
4099 return -1;
4100 }
4101
b3GetNumUserData(b3PhysicsClientHandle physClient,int bodyUniqueId)4102 B3_SHARED_API int b3GetNumUserData(b3PhysicsClientHandle physClient, int bodyUniqueId)
4103 {
4104 PhysicsClient* cl = (PhysicsClient*)physClient;
4105 if (cl)
4106 {
4107 return cl->getNumUserData(bodyUniqueId);
4108 }
4109 return 0;
4110 }
4111
b3GetUserDataInfo(b3PhysicsClientHandle physClient,int bodyUniqueId,int userDataIndex,const char ** keyOut,int * userDataIdOut,int * linkIndexOut,int * visualShapeIndexOut)4112 B3_SHARED_API void b3GetUserDataInfo(b3PhysicsClientHandle physClient, int bodyUniqueId, int userDataIndex, const char** keyOut, int* userDataIdOut, int* linkIndexOut, int* visualShapeIndexOut)
4113 {
4114 PhysicsClient* cl = (PhysicsClient*)physClient;
4115 if (cl)
4116 {
4117 cl->getUserDataInfo(bodyUniqueId, userDataIndex, keyOut, userDataIdOut, linkIndexOut, visualShapeIndexOut);
4118 }
4119 }
4120
b3InitRequestDebugLinesCommand(b3PhysicsClientHandle physClient,int debugMode)4121 B3_SHARED_API b3SharedMemoryCommandHandle b3InitRequestDebugLinesCommand(b3PhysicsClientHandle physClient, int debugMode)
4122 {
4123 PhysicsClient* cl = (PhysicsClient*)physClient;
4124 b3Assert(cl);
4125 b3Assert(cl->canSubmitCommand());
4126 struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
4127 b3Assert(command);
4128
4129 command->m_type = CMD_REQUEST_DEBUG_LINES;
4130 command->m_requestDebugLinesArguments.m_debugMode = debugMode;
4131 command->m_requestDebugLinesArguments.m_startingLineIndex = 0;
4132 return (b3SharedMemoryCommandHandle)command;
4133 }
b3GetDebugLines(b3PhysicsClientHandle physClient,struct b3DebugLines * lines)4134 B3_SHARED_API void b3GetDebugLines(b3PhysicsClientHandle physClient, struct b3DebugLines* lines)
4135 {
4136 PhysicsClient* cl = (PhysicsClient*)physClient;
4137
4138 b3Assert(lines);
4139 if (lines)
4140 {
4141 lines->m_numDebugLines = cl->getNumDebugLines();
4142 lines->m_linesFrom = cl->getDebugLinesFrom();
4143 lines->m_linesTo = cl->getDebugLinesTo();
4144 lines->m_linesColor = cl->getDebugLinesColor();
4145 }
4146 }
4147
4148 /// Add/remove user-specific debug lines and debug text messages
b3InitUserDebugDrawAddLine3D(b3PhysicsClientHandle physClient,const double fromXYZ[3],const double toXYZ[3],const double colorRGB[3],const double lineWidth,const double lifeTime)4149 B3_SHARED_API b3SharedMemoryCommandHandle b3InitUserDebugDrawAddLine3D(b3PhysicsClientHandle physClient, const double fromXYZ[3], const double toXYZ[3], const double colorRGB[3], const double lineWidth, const double lifeTime)
4150 {
4151 PhysicsClient* cl = (PhysicsClient*)physClient;
4152 b3Assert(cl);
4153 b3Assert(cl->canSubmitCommand());
4154 struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
4155 b3Assert(command);
4156 command->m_type = CMD_USER_DEBUG_DRAW;
4157 command->m_updateFlags = USER_DEBUG_HAS_LINE; //USER_DEBUG_HAS_TEXT
4158
4159 command->m_userDebugDrawArgs.m_debugLineFromXYZ[0] = fromXYZ[0];
4160 command->m_userDebugDrawArgs.m_debugLineFromXYZ[1] = fromXYZ[1];
4161 command->m_userDebugDrawArgs.m_debugLineFromXYZ[2] = fromXYZ[2];
4162
4163 command->m_userDebugDrawArgs.m_debugLineToXYZ[0] = toXYZ[0];
4164 command->m_userDebugDrawArgs.m_debugLineToXYZ[1] = toXYZ[1];
4165 command->m_userDebugDrawArgs.m_debugLineToXYZ[2] = toXYZ[2];
4166
4167 command->m_userDebugDrawArgs.m_debugLineColorRGB[0] = colorRGB[0];
4168 command->m_userDebugDrawArgs.m_debugLineColorRGB[1] = colorRGB[1];
4169 command->m_userDebugDrawArgs.m_debugLineColorRGB[2] = colorRGB[2];
4170
4171 command->m_userDebugDrawArgs.m_lineWidth = lineWidth;
4172 command->m_userDebugDrawArgs.m_lifeTime = lifeTime;
4173 command->m_userDebugDrawArgs.m_parentObjectUniqueId = -1;
4174 command->m_userDebugDrawArgs.m_parentLinkIndex = -1;
4175 command->m_userDebugDrawArgs.m_optionFlags = 0;
4176
4177 return (b3SharedMemoryCommandHandle)command;
4178 }
4179
b3InitUserDebugDrawAddText3D(b3PhysicsClientHandle physClient,const char * txt,const double positionXYZ[3],const double colorRGB[3],double textSize,double lifeTime)4180 B3_SHARED_API b3SharedMemoryCommandHandle b3InitUserDebugDrawAddText3D(b3PhysicsClientHandle physClient, const char* txt, const double positionXYZ[3], const double colorRGB[3], double textSize, double lifeTime)
4181 {
4182 PhysicsClient* cl = (PhysicsClient*)physClient;
4183 b3Assert(cl);
4184 b3Assert(cl->canSubmitCommand());
4185 struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
4186 b3Assert(command);
4187 command->m_type = CMD_USER_DEBUG_DRAW;
4188 command->m_updateFlags = USER_DEBUG_HAS_TEXT;
4189
4190 int len = strlen(txt);
4191 if (len < MAX_FILENAME_LENGTH)
4192 {
4193 strcpy(command->m_userDebugDrawArgs.m_text, txt);
4194 }
4195 else
4196 {
4197 command->m_userDebugDrawArgs.m_text[0] = 0;
4198 }
4199 command->m_userDebugDrawArgs.m_textPositionXYZ[0] = positionXYZ[0];
4200 command->m_userDebugDrawArgs.m_textPositionXYZ[1] = positionXYZ[1];
4201 command->m_userDebugDrawArgs.m_textPositionXYZ[2] = positionXYZ[2];
4202
4203 command->m_userDebugDrawArgs.m_textColorRGB[0] = colorRGB[0];
4204 command->m_userDebugDrawArgs.m_textColorRGB[1] = colorRGB[1];
4205 command->m_userDebugDrawArgs.m_textColorRGB[2] = colorRGB[2];
4206
4207 command->m_userDebugDrawArgs.m_textSize = textSize;
4208
4209 command->m_userDebugDrawArgs.m_lifeTime = lifeTime;
4210 command->m_userDebugDrawArgs.m_parentObjectUniqueId = -1;
4211 command->m_userDebugDrawArgs.m_parentLinkIndex = -1;
4212
4213 command->m_userDebugDrawArgs.m_optionFlags = 0;
4214
4215 return (b3SharedMemoryCommandHandle)command;
4216 }
4217
b3UserDebugTextSetOptionFlags(b3SharedMemoryCommandHandle commandHandle,int optionFlags)4218 B3_SHARED_API void b3UserDebugTextSetOptionFlags(b3SharedMemoryCommandHandle commandHandle, int optionFlags)
4219 {
4220 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
4221 b3Assert(command);
4222 b3Assert(command->m_type == CMD_USER_DEBUG_DRAW);
4223 b3Assert(command->m_updateFlags & USER_DEBUG_HAS_TEXT);
4224 command->m_userDebugDrawArgs.m_optionFlags = optionFlags;
4225 command->m_updateFlags |= USER_DEBUG_HAS_OPTION_FLAGS;
4226 }
4227
b3UserDebugTextSetOrientation(b3SharedMemoryCommandHandle commandHandle,const double orientation[4])4228 B3_SHARED_API void b3UserDebugTextSetOrientation(b3SharedMemoryCommandHandle commandHandle, const double orientation[4])
4229 {
4230 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
4231 b3Assert(command);
4232 b3Assert(command->m_type == CMD_USER_DEBUG_DRAW);
4233 b3Assert(command->m_updateFlags & USER_DEBUG_HAS_TEXT);
4234 command->m_userDebugDrawArgs.m_textOrientation[0] = orientation[0];
4235 command->m_userDebugDrawArgs.m_textOrientation[1] = orientation[1];
4236 command->m_userDebugDrawArgs.m_textOrientation[2] = orientation[2];
4237 command->m_userDebugDrawArgs.m_textOrientation[3] = orientation[3];
4238 command->m_updateFlags |= USER_DEBUG_HAS_TEXT_ORIENTATION;
4239 }
4240
b3UserDebugItemSetReplaceItemUniqueId(b3SharedMemoryCommandHandle commandHandle,int replaceItemUniqueId)4241 B3_SHARED_API void b3UserDebugItemSetReplaceItemUniqueId(b3SharedMemoryCommandHandle commandHandle, int replaceItemUniqueId)
4242 {
4243 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
4244 b3Assert(command);
4245 b3Assert(command->m_type == CMD_USER_DEBUG_DRAW);
4246 command->m_userDebugDrawArgs.m_replaceItemUniqueId = replaceItemUniqueId;
4247 command->m_updateFlags |= USER_DEBUG_HAS_REPLACE_ITEM_UNIQUE_ID;
4248 }
4249
b3UserDebugItemSetParentObject(b3SharedMemoryCommandHandle commandHandle,int objectUniqueId,int linkIndex)4250 B3_SHARED_API void b3UserDebugItemSetParentObject(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId, int linkIndex)
4251 {
4252 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
4253 b3Assert(command);
4254 b3Assert(command->m_type == CMD_USER_DEBUG_DRAW);
4255
4256 command->m_updateFlags |= USER_DEBUG_HAS_PARENT_OBJECT;
4257 command->m_userDebugDrawArgs.m_parentObjectUniqueId = objectUniqueId;
4258 command->m_userDebugDrawArgs.m_parentLinkIndex = linkIndex;
4259 }
4260
b3InitUserDebugAddParameter(b3PhysicsClientHandle physClient,const char * txt,double rangeMin,double rangeMax,double startValue)4261 B3_SHARED_API b3SharedMemoryCommandHandle b3InitUserDebugAddParameter(b3PhysicsClientHandle physClient, const char* txt, double rangeMin, double rangeMax, double startValue)
4262 {
4263 PhysicsClient* cl = (PhysicsClient*)physClient;
4264 b3Assert(cl);
4265 b3Assert(cl->canSubmitCommand());
4266 struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
4267 b3Assert(command);
4268 command->m_type = CMD_USER_DEBUG_DRAW;
4269 command->m_updateFlags = USER_DEBUG_ADD_PARAMETER;
4270 int len = strlen(txt);
4271 if (len < MAX_FILENAME_LENGTH)
4272 {
4273 strcpy(command->m_userDebugDrawArgs.m_text, txt);
4274 }
4275 else
4276 {
4277 command->m_userDebugDrawArgs.m_text[0] = 0;
4278 }
4279
4280 command->m_userDebugDrawArgs.m_rangeMin = rangeMin;
4281 command->m_userDebugDrawArgs.m_rangeMax = rangeMax;
4282 command->m_userDebugDrawArgs.m_startValue = startValue;
4283 command->m_userDebugDrawArgs.m_parentObjectUniqueId = -1;
4284 command->m_userDebugDrawArgs.m_optionFlags = 0;
4285 return (b3SharedMemoryCommandHandle)command;
4286 }
4287
b3InitUserDebugReadParameter(b3PhysicsClientHandle physClient,int debugItemUniqueId)4288 B3_SHARED_API b3SharedMemoryCommandHandle b3InitUserDebugReadParameter(b3PhysicsClientHandle physClient, int debugItemUniqueId)
4289 {
4290 PhysicsClient* cl = (PhysicsClient*)physClient;
4291 b3Assert(cl);
4292 b3Assert(cl->canSubmitCommand());
4293 struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
4294 b3Assert(command);
4295 command->m_type = CMD_USER_DEBUG_DRAW;
4296 command->m_updateFlags = USER_DEBUG_READ_PARAMETER;
4297 command->m_userDebugDrawArgs.m_itemUniqueId = debugItemUniqueId;
4298 command->m_userDebugDrawArgs.m_parentObjectUniqueId = -1;
4299 return (b3SharedMemoryCommandHandle)command;
4300 }
4301
b3GetStatusDebugParameterValue(b3SharedMemoryStatusHandle statusHandle,double * paramValue)4302 B3_SHARED_API int b3GetStatusDebugParameterValue(b3SharedMemoryStatusHandle statusHandle, double* paramValue)
4303 {
4304 const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
4305 if (status)
4306 {
4307 btAssert(status->m_type == CMD_USER_DEBUG_DRAW_PARAMETER_COMPLETED);
4308 if (paramValue && (status->m_type == CMD_USER_DEBUG_DRAW_PARAMETER_COMPLETED))
4309 {
4310 *paramValue = status->m_userDebugDrawArgs.m_parameterValue;
4311 return 1;
4312 }
4313 }
4314 return 0;
4315 }
4316
b3InitUserDebugDrawRemove(b3PhysicsClientHandle physClient,int debugItemUniqueId)4317 B3_SHARED_API b3SharedMemoryCommandHandle b3InitUserDebugDrawRemove(b3PhysicsClientHandle physClient, int debugItemUniqueId)
4318 {
4319 PhysicsClient* cl = (PhysicsClient*)physClient;
4320 b3Assert(cl);
4321 b3Assert(cl->canSubmitCommand());
4322 struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
4323 b3Assert(command);
4324 command->m_type = CMD_USER_DEBUG_DRAW;
4325 command->m_updateFlags = USER_DEBUG_REMOVE_ONE_ITEM;
4326 command->m_userDebugDrawArgs.m_itemUniqueId = debugItemUniqueId;
4327 command->m_userDebugDrawArgs.m_parentObjectUniqueId = -1;
4328 return (b3SharedMemoryCommandHandle)command;
4329 }
4330
b3InitUserDebugDrawRemoveAll(b3PhysicsClientHandle physClient)4331 B3_SHARED_API b3SharedMemoryCommandHandle b3InitUserDebugDrawRemoveAll(b3PhysicsClientHandle physClient)
4332 {
4333 PhysicsClient* cl = (PhysicsClient*)physClient;
4334 b3Assert(cl);
4335 b3Assert(cl->canSubmitCommand());
4336 struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
4337 b3Assert(command);
4338 command->m_type = CMD_USER_DEBUG_DRAW;
4339 command->m_updateFlags = USER_DEBUG_REMOVE_ALL;
4340 command->m_userDebugDrawArgs.m_parentObjectUniqueId = -1;
4341 return (b3SharedMemoryCommandHandle)command;
4342 }
4343
b3InitUserRemoveAllParameters(b3PhysicsClientHandle physClient)4344 B3_SHARED_API b3SharedMemoryCommandHandle b3InitUserRemoveAllParameters(b3PhysicsClientHandle physClient)
4345 {
4346 PhysicsClient* cl = (PhysicsClient*)physClient;
4347 b3Assert(cl);
4348 b3Assert(cl->canSubmitCommand());
4349 struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
4350 b3Assert(command);
4351 command->m_type = CMD_USER_DEBUG_DRAW;
4352 command->m_updateFlags = USER_DEBUG_REMOVE_ALL_PARAMETERS;
4353 command->m_userDebugDrawArgs.m_parentObjectUniqueId = -1;
4354 return (b3SharedMemoryCommandHandle)command;
4355 }
4356
4357
b3GetDebugItemUniqueId(b3SharedMemoryStatusHandle statusHandle)4358 B3_SHARED_API int b3GetDebugItemUniqueId(b3SharedMemoryStatusHandle statusHandle)
4359 {
4360 const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
4361 btAssert(status->m_type == CMD_USER_DEBUG_DRAW_COMPLETED);
4362 if (status->m_type != CMD_USER_DEBUG_DRAW_COMPLETED)
4363 return -1;
4364
4365 return status->m_userDebugDrawArgs.m_debugItemUniqueId;
4366 }
4367
b3InitDebugDrawingCommand(b3PhysicsClientHandle physClient)4368 B3_SHARED_API b3SharedMemoryCommandHandle b3InitDebugDrawingCommand(b3PhysicsClientHandle physClient)
4369 {
4370 PhysicsClient* cl = (PhysicsClient*)physClient;
4371 b3Assert(cl);
4372 b3Assert(cl->canSubmitCommand());
4373 struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
4374 b3Assert(command);
4375 command->m_type = CMD_USER_DEBUG_DRAW;
4376 command->m_updateFlags = 0;
4377 command->m_userDebugDrawArgs.m_parentObjectUniqueId = -1;
4378 return (b3SharedMemoryCommandHandle)command;
4379 }
4380
b3SetDebugObjectColor(b3SharedMemoryCommandHandle commandHandle,int objectUniqueId,int linkIndex,const double objectColorRGB[3])4381 B3_SHARED_API void b3SetDebugObjectColor(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId, int linkIndex, const double objectColorRGB[3])
4382 {
4383 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
4384 b3Assert(command);
4385 b3Assert(command->m_type == CMD_USER_DEBUG_DRAW);
4386 command->m_updateFlags |= USER_DEBUG_SET_CUSTOM_OBJECT_COLOR;
4387 command->m_userDebugDrawArgs.m_objectUniqueId = objectUniqueId;
4388 command->m_userDebugDrawArgs.m_linkIndex = linkIndex;
4389 command->m_userDebugDrawArgs.m_objectDebugColorRGB[0] = objectColorRGB[0];
4390 command->m_userDebugDrawArgs.m_objectDebugColorRGB[1] = objectColorRGB[1];
4391 command->m_userDebugDrawArgs.m_objectDebugColorRGB[2] = objectColorRGB[2];
4392 }
4393
b3RemoveDebugObjectColor(b3SharedMemoryCommandHandle commandHandle,int objectUniqueId,int linkIndex)4394 B3_SHARED_API void b3RemoveDebugObjectColor(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId, int linkIndex)
4395 {
4396 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
4397 b3Assert(command);
4398 b3Assert(command->m_type == CMD_USER_DEBUG_DRAW);
4399 command->m_updateFlags |= USER_DEBUG_REMOVE_CUSTOM_OBJECT_COLOR;
4400 command->m_userDebugDrawArgs.m_objectUniqueId = objectUniqueId;
4401 command->m_userDebugDrawArgs.m_linkIndex = linkIndex;
4402 }
4403
4404 ///request an image from a simulated camera, using a software renderer.
b3InitRequestCameraImage(b3PhysicsClientHandle physClient)4405 B3_SHARED_API b3SharedMemoryCommandHandle b3InitRequestCameraImage(b3PhysicsClientHandle physClient)
4406 {
4407 PhysicsClient* cl = (PhysicsClient*)physClient;
4408 b3Assert(cl);
4409 b3Assert(cl->canSubmitCommand());
4410 struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
4411 b3Assert(command);
4412 return b3InitRequestCameraImage2((b3SharedMemoryCommandHandle)command);
4413 }
4414
b3InitRequestCameraImage2(b3SharedMemoryCommandHandle commandHandle)4415 B3_SHARED_API b3SharedMemoryCommandHandle b3InitRequestCameraImage2(b3SharedMemoryCommandHandle commandHandle)
4416 {
4417 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
4418 command->m_type = CMD_REQUEST_CAMERA_IMAGE_DATA;
4419 command->m_requestPixelDataArguments.m_startPixelIndex = 0;
4420 command->m_updateFlags = 0; //REQUEST_PIXEL_ARGS_USE_HARDWARE_OPENGL;
4421 return (b3SharedMemoryCommandHandle)command;
4422 }
4423
b3RequestCameraImageSelectRenderer(b3SharedMemoryCommandHandle commandHandle,int renderer)4424 B3_SHARED_API void b3RequestCameraImageSelectRenderer(b3SharedMemoryCommandHandle commandHandle, int renderer)
4425 {
4426 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
4427 b3Assert(command);
4428 b3Assert(command->m_type == CMD_REQUEST_CAMERA_IMAGE_DATA);
4429 b3Assert(renderer > (1 << 15));
4430 if (renderer > (1 << 15))
4431 {
4432 command->m_updateFlags |= renderer;
4433 }
4434 }
4435
b3RequestCameraImageSetFlags(b3SharedMemoryCommandHandle commandHandle,int flags)4436 B3_SHARED_API void b3RequestCameraImageSetFlags(b3SharedMemoryCommandHandle commandHandle, int flags)
4437 {
4438 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
4439 b3Assert(command);
4440 b3Assert(command->m_type == CMD_REQUEST_CAMERA_IMAGE_DATA);
4441 if (command->m_type == CMD_REQUEST_CAMERA_IMAGE_DATA)
4442 {
4443 command->m_requestPixelDataArguments.m_flags = flags;
4444 command->m_updateFlags |= REQUEST_PIXEL_ARGS_HAS_FLAGS;
4445 }
4446 }
4447
b3RequestCameraImageSetCameraMatrices(b3SharedMemoryCommandHandle commandHandle,float viewMatrix[16],float projectionMatrix[16])4448 B3_SHARED_API void b3RequestCameraImageSetCameraMatrices(b3SharedMemoryCommandHandle commandHandle, float viewMatrix[16], float projectionMatrix[16])
4449 {
4450 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
4451 b3Assert(command);
4452 b3Assert(command->m_type == CMD_REQUEST_CAMERA_IMAGE_DATA);
4453 for (int i = 0; i < 16; i++)
4454 {
4455 command->m_requestPixelDataArguments.m_projectionMatrix[i] = projectionMatrix[i];
4456 command->m_requestPixelDataArguments.m_viewMatrix[i] = viewMatrix[i];
4457 }
4458 command->m_updateFlags |= REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES;
4459 }
4460
b3RequestCameraImageSetLightDirection(b3SharedMemoryCommandHandle commandHandle,const float lightDirection[3])4461 B3_SHARED_API void b3RequestCameraImageSetLightDirection(b3SharedMemoryCommandHandle commandHandle, const float lightDirection[3])
4462 {
4463 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
4464 b3Assert(command);
4465 b3Assert(command->m_type == CMD_REQUEST_CAMERA_IMAGE_DATA);
4466 for (int i = 0; i < 3; i++)
4467 {
4468 command->m_requestPixelDataArguments.m_lightDirection[i] = lightDirection[i];
4469 }
4470 command->m_updateFlags |= REQUEST_PIXEL_ARGS_SET_LIGHT_DIRECTION;
4471 }
4472
b3RequestCameraImageSetLightColor(b3SharedMemoryCommandHandle commandHandle,const float lightColor[3])4473 B3_SHARED_API void b3RequestCameraImageSetLightColor(b3SharedMemoryCommandHandle commandHandle, const float lightColor[3])
4474 {
4475 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
4476 b3Assert(command);
4477 b3Assert(command->m_type == CMD_REQUEST_CAMERA_IMAGE_DATA);
4478 for (int i = 0; i < 3; i++)
4479 {
4480 command->m_requestPixelDataArguments.m_lightColor[i] = lightColor[i];
4481 }
4482 command->m_updateFlags |= REQUEST_PIXEL_ARGS_SET_LIGHT_COLOR;
4483 }
4484
b3RequestCameraImageSetLightDistance(b3SharedMemoryCommandHandle commandHandle,float lightDistance)4485 B3_SHARED_API void b3RequestCameraImageSetLightDistance(b3SharedMemoryCommandHandle commandHandle, float lightDistance)
4486 {
4487 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
4488 b3Assert(command);
4489 b3Assert(command->m_type == CMD_REQUEST_CAMERA_IMAGE_DATA);
4490 command->m_requestPixelDataArguments.m_lightDistance = lightDistance;
4491 command->m_updateFlags |= REQUEST_PIXEL_ARGS_SET_LIGHT_DISTANCE;
4492 }
4493
b3RequestCameraImageSetLightAmbientCoeff(b3SharedMemoryCommandHandle commandHandle,float lightAmbientCoeff)4494 B3_SHARED_API void b3RequestCameraImageSetLightAmbientCoeff(b3SharedMemoryCommandHandle commandHandle, float lightAmbientCoeff)
4495 {
4496 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
4497 b3Assert(command);
4498 b3Assert(command->m_type == CMD_REQUEST_CAMERA_IMAGE_DATA);
4499 command->m_requestPixelDataArguments.m_lightAmbientCoeff = lightAmbientCoeff;
4500 command->m_updateFlags |= REQUEST_PIXEL_ARGS_SET_AMBIENT_COEFF;
4501 }
4502
b3RequestCameraImageSetLightDiffuseCoeff(b3SharedMemoryCommandHandle commandHandle,float lightDiffuseCoeff)4503 B3_SHARED_API void b3RequestCameraImageSetLightDiffuseCoeff(b3SharedMemoryCommandHandle commandHandle, float lightDiffuseCoeff)
4504 {
4505 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
4506 b3Assert(command);
4507 b3Assert(command->m_type == CMD_REQUEST_CAMERA_IMAGE_DATA);
4508 command->m_requestPixelDataArguments.m_lightDiffuseCoeff = lightDiffuseCoeff;
4509 command->m_updateFlags |= REQUEST_PIXEL_ARGS_SET_DIFFUSE_COEFF;
4510 }
4511
b3RequestCameraImageSetLightSpecularCoeff(b3SharedMemoryCommandHandle commandHandle,float lightSpecularCoeff)4512 B3_SHARED_API void b3RequestCameraImageSetLightSpecularCoeff(b3SharedMemoryCommandHandle commandHandle, float lightSpecularCoeff)
4513 {
4514 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
4515 b3Assert(command);
4516 b3Assert(command->m_type == CMD_REQUEST_CAMERA_IMAGE_DATA);
4517 command->m_requestPixelDataArguments.m_lightSpecularCoeff = lightSpecularCoeff;
4518 command->m_updateFlags |= REQUEST_PIXEL_ARGS_SET_SPECULAR_COEFF;
4519 }
4520
b3RequestCameraImageSetShadow(b3SharedMemoryCommandHandle commandHandle,int hasShadow)4521 B3_SHARED_API void b3RequestCameraImageSetShadow(b3SharedMemoryCommandHandle commandHandle, int hasShadow)
4522 {
4523 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
4524 b3Assert(command);
4525 b3Assert(command->m_type == CMD_REQUEST_CAMERA_IMAGE_DATA);
4526 command->m_requestPixelDataArguments.m_hasShadow = hasShadow;
4527 command->m_updateFlags |= REQUEST_PIXEL_ARGS_SET_SHADOW;
4528 }
4529
b3RequestCameraImageSetProjectiveTextureMatrices(b3SharedMemoryCommandHandle commandHandle,float viewMatrix[16],float projectionMatrix[16])4530 B3_SHARED_API void b3RequestCameraImageSetProjectiveTextureMatrices(b3SharedMemoryCommandHandle commandHandle, float viewMatrix[16], float projectionMatrix[16])
4531 {
4532 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
4533 b3Assert(command);
4534 b3Assert(command->m_type == CMD_REQUEST_CAMERA_IMAGE_DATA);
4535 for (int i = 0; i < 16; i++)
4536 {
4537 command->m_requestPixelDataArguments.m_projectiveTextureProjectionMatrix[i] = projectionMatrix[i];
4538 command->m_requestPixelDataArguments.m_projectiveTextureViewMatrix[i] = viewMatrix[i];
4539 }
4540 command->m_updateFlags |= REQUEST_PIXEL_ARGS_HAS_PROJECTIVE_TEXTURE_MATRICES;
4541 }
4542
b3ComputePositionFromViewMatrix(const float viewMatrix[16],float cameraPosition[3],float cameraTargetPosition[3],float cameraUp[3])4543 B3_SHARED_API void b3ComputePositionFromViewMatrix(const float viewMatrix[16], float cameraPosition[3], float cameraTargetPosition[3], float cameraUp[3])
4544 {
4545 b3Matrix3x3 r(viewMatrix[0], viewMatrix[4], viewMatrix[8], viewMatrix[1], viewMatrix[5], viewMatrix[9], viewMatrix[2], viewMatrix[6], viewMatrix[10]);
4546 b3Vector3 p = b3MakeVector3(viewMatrix[12], viewMatrix[13], viewMatrix[14]);
4547 b3Transform t(r, p);
4548 b3Transform tinv = t.inverse();
4549 b3Matrix3x3 basis = tinv.getBasis();
4550 b3Vector3 origin = tinv.getOrigin();
4551 b3Vector3 s = b3MakeVector3(basis[0][0], basis[1][0], basis[2][0]);
4552 b3Vector3 u = b3MakeVector3(basis[0][1], basis[1][1], basis[2][1]);
4553 b3Vector3 f = b3MakeVector3(-basis[0][2], -basis[1][2], -basis[2][2]);
4554 b3Vector3 eye = origin;
4555 cameraPosition[0] = eye[0];
4556 cameraPosition[1] = eye[1];
4557 cameraPosition[2] = eye[2];
4558 b3Vector3 center = f + eye;
4559 cameraTargetPosition[0] = center[0];
4560 cameraTargetPosition[1] = center[1];
4561 cameraTargetPosition[2] = center[2];
4562 cameraUp[0] = u[0];
4563 cameraUp[1] = u[1];
4564 cameraUp[2] = u[2];
4565 }
4566
b3ComputeViewMatrixFromPositions(const float cameraPosition[3],const float cameraTargetPosition[3],const float cameraUp[3],float viewMatrix[16])4567 B3_SHARED_API void b3ComputeViewMatrixFromPositions(const float cameraPosition[3], const float cameraTargetPosition[3], const float cameraUp[3], float viewMatrix[16])
4568 {
4569 b3Vector3 eye = b3MakeVector3(cameraPosition[0], cameraPosition[1], cameraPosition[2]);
4570 b3Vector3 center = b3MakeVector3(cameraTargetPosition[0], cameraTargetPosition[1], cameraTargetPosition[2]);
4571 b3Vector3 up = b3MakeVector3(cameraUp[0], cameraUp[1], cameraUp[2]);
4572 b3Vector3 f = (center - eye).normalized();
4573 b3Vector3 u = up.normalized();
4574 b3Vector3 s = (f.cross(u)).normalized();
4575 u = s.cross(f);
4576
4577 viewMatrix[0 * 4 + 0] = s.x;
4578 viewMatrix[1 * 4 + 0] = s.y;
4579 viewMatrix[2 * 4 + 0] = s.z;
4580
4581 viewMatrix[0 * 4 + 1] = u.x;
4582 viewMatrix[1 * 4 + 1] = u.y;
4583 viewMatrix[2 * 4 + 1] = u.z;
4584
4585 viewMatrix[0 * 4 + 2] = -f.x;
4586 viewMatrix[1 * 4 + 2] = -f.y;
4587 viewMatrix[2 * 4 + 2] = -f.z;
4588
4589 viewMatrix[0 * 4 + 3] = 0.f;
4590 viewMatrix[1 * 4 + 3] = 0.f;
4591 viewMatrix[2 * 4 + 3] = 0.f;
4592
4593 viewMatrix[3 * 4 + 0] = -s.dot(eye);
4594 viewMatrix[3 * 4 + 1] = -u.dot(eye);
4595 viewMatrix[3 * 4 + 2] = f.dot(eye);
4596 viewMatrix[3 * 4 + 3] = 1.f;
4597 }
4598
b3ComputeViewMatrixFromYawPitchRoll(const float cameraTargetPosition[3],float distance,float yaw,float pitch,float roll,int upAxis,float viewMatrix[16])4599 B3_SHARED_API void b3ComputeViewMatrixFromYawPitchRoll(const float cameraTargetPosition[3], float distance, float yaw, float pitch, float roll, int upAxis, float viewMatrix[16])
4600 {
4601 b3Vector3 camUpVector;
4602 b3Vector3 camForward;
4603 b3Vector3 camPos;
4604 b3Vector3 camTargetPos = b3MakeVector3(cameraTargetPosition[0], cameraTargetPosition[1], cameraTargetPosition[2]);
4605 b3Vector3 eyePos = b3MakeVector3(0, 0, 0);
4606
4607 b3Scalar yawRad = yaw * b3Scalar(0.01745329251994329547); // rads per deg
4608 b3Scalar pitchRad = pitch * b3Scalar(0.01745329251994329547); // rads per deg
4609 b3Scalar rollRad = roll * b3Scalar(0.01745329251994329547); // rads per deg
4610 b3Quaternion eyeRot;
4611
4612 int forwardAxis(-1);
4613 switch (upAxis)
4614 {
4615 case 1:
4616 forwardAxis = 2;
4617 camUpVector = b3MakeVector3(0, 1, 0);
4618 eyeRot.setEulerZYX(rollRad, yawRad, -pitchRad);
4619 break;
4620 case 2:
4621 forwardAxis = 1;
4622 camUpVector = b3MakeVector3(0, 0, 1);
4623 eyeRot.setEulerZYX(yawRad, rollRad, pitchRad);
4624 break;
4625 default:
4626 return;
4627 };
4628
4629 eyePos[forwardAxis] = -distance;
4630
4631 camForward = b3MakeVector3(eyePos[0], eyePos[1], eyePos[2]);
4632 if (camForward.length2() < B3_EPSILON)
4633 {
4634 camForward.setValue(1.f, 0.f, 0.f);
4635 }
4636 else
4637 {
4638 camForward.normalize();
4639 }
4640
4641 eyePos = b3Matrix3x3(eyeRot) * eyePos;
4642 camUpVector = b3Matrix3x3(eyeRot) * camUpVector;
4643
4644 camPos = eyePos;
4645 camPos += camTargetPos;
4646
4647 float camPosf[4] = {camPos[0], camPos[1], camPos[2], 0};
4648 float camPosTargetf[4] = {camTargetPos[0], camTargetPos[1], camTargetPos[2], 0};
4649 float camUpf[4] = {camUpVector[0], camUpVector[1], camUpVector[2], 0};
4650
4651 b3ComputeViewMatrixFromPositions(camPosf, camPosTargetf, camUpf, viewMatrix);
4652 }
4653
b3ComputeProjectionMatrix(float left,float right,float bottom,float top,float nearVal,float farVal,float projectionMatrix[16])4654 B3_SHARED_API void b3ComputeProjectionMatrix(float left, float right, float bottom, float top, float nearVal, float farVal, float projectionMatrix[16])
4655 {
4656 projectionMatrix[0 * 4 + 0] = (float(2) * nearVal) / (right - left);
4657 projectionMatrix[0 * 4 + 1] = float(0);
4658 projectionMatrix[0 * 4 + 2] = float(0);
4659 projectionMatrix[0 * 4 + 3] = float(0);
4660
4661 projectionMatrix[1 * 4 + 0] = float(0);
4662 projectionMatrix[1 * 4 + 1] = (float(2) * nearVal) / (top - bottom);
4663 projectionMatrix[1 * 4 + 2] = float(0);
4664 projectionMatrix[1 * 4 + 3] = float(0);
4665
4666 projectionMatrix[2 * 4 + 0] = (right + left) / (right - left);
4667 projectionMatrix[2 * 4 + 1] = (top + bottom) / (top - bottom);
4668 projectionMatrix[2 * 4 + 2] = -(farVal + nearVal) / (farVal - nearVal);
4669 projectionMatrix[2 * 4 + 3] = float(-1);
4670
4671 projectionMatrix[3 * 4 + 0] = float(0);
4672 projectionMatrix[3 * 4 + 1] = float(0);
4673 projectionMatrix[3 * 4 + 2] = -(float(2) * farVal * nearVal) / (farVal - nearVal);
4674 projectionMatrix[3 * 4 + 3] = float(0);
4675 }
4676
b3ComputeProjectionMatrixFOV(float fov,float aspect,float nearVal,float farVal,float projectionMatrix[16])4677 B3_SHARED_API void b3ComputeProjectionMatrixFOV(float fov, float aspect, float nearVal, float farVal, float projectionMatrix[16])
4678 {
4679 float yScale = 1.0 / tan((B3_PI / 180.0) * fov / 2);
4680 float xScale = yScale / aspect;
4681
4682 projectionMatrix[0 * 4 + 0] = xScale;
4683 projectionMatrix[0 * 4 + 1] = float(0);
4684 projectionMatrix[0 * 4 + 2] = float(0);
4685 projectionMatrix[0 * 4 + 3] = float(0);
4686
4687 projectionMatrix[1 * 4 + 0] = float(0);
4688 projectionMatrix[1 * 4 + 1] = yScale;
4689 projectionMatrix[1 * 4 + 2] = float(0);
4690 projectionMatrix[1 * 4 + 3] = float(0);
4691
4692 projectionMatrix[2 * 4 + 0] = 0;
4693 projectionMatrix[2 * 4 + 1] = 0;
4694 projectionMatrix[2 * 4 + 2] = (nearVal + farVal) / (nearVal - farVal);
4695 projectionMatrix[2 * 4 + 3] = float(-1);
4696
4697 projectionMatrix[3 * 4 + 0] = float(0);
4698 projectionMatrix[3 * 4 + 1] = float(0);
4699 projectionMatrix[3 * 4 + 2] = (float(2) * farVal * nearVal) / (nearVal - farVal);
4700 projectionMatrix[3 * 4 + 3] = float(0);
4701 }
4702
b3RequestCameraImageSetViewMatrix2(b3SharedMemoryCommandHandle commandHandle,const float cameraTargetPosition[3],float distance,float yaw,float pitch,float roll,int upAxis)4703 B3_SHARED_API void b3RequestCameraImageSetViewMatrix2(b3SharedMemoryCommandHandle commandHandle, const float cameraTargetPosition[3], float distance, float yaw, float pitch, float roll, int upAxis)
4704 {
4705 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
4706 b3Assert(command);
4707 b3Assert(command->m_type == CMD_REQUEST_CAMERA_IMAGE_DATA);
4708
4709 b3ComputeViewMatrixFromYawPitchRoll(cameraTargetPosition, distance, yaw, pitch, roll, upAxis, command->m_requestPixelDataArguments.m_viewMatrix);
4710 command->m_updateFlags |= REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES;
4711 }
4712
b3RequestCameraImageSetViewMatrix(b3SharedMemoryCommandHandle commandHandle,const float cameraPosition[3],const float cameraTargetPosition[3],const float cameraUp[3])4713 B3_SHARED_API void b3RequestCameraImageSetViewMatrix(b3SharedMemoryCommandHandle commandHandle, const float cameraPosition[3], const float cameraTargetPosition[3], const float cameraUp[3])
4714 {
4715 float viewMatrix[16];
4716 b3ComputeViewMatrixFromPositions(cameraPosition, cameraTargetPosition, cameraUp, viewMatrix);
4717
4718 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
4719 b3Assert(command);
4720 b3Assert(command->m_type == CMD_REQUEST_CAMERA_IMAGE_DATA);
4721
4722 b3ComputeViewMatrixFromPositions(cameraPosition, cameraTargetPosition, cameraUp, command->m_requestPixelDataArguments.m_viewMatrix);
4723
4724 command->m_updateFlags |= REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES;
4725 }
4726
b3RequestCameraImageSetProjectionMatrix(b3SharedMemoryCommandHandle commandHandle,float left,float right,float bottom,float top,float nearVal,float farVal)4727 B3_SHARED_API void b3RequestCameraImageSetProjectionMatrix(b3SharedMemoryCommandHandle commandHandle, float left, float right, float bottom, float top, float nearVal, float farVal)
4728 {
4729 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
4730 b3Assert(command);
4731 b3Assert(command->m_type == CMD_REQUEST_CAMERA_IMAGE_DATA);
4732
4733 b3ComputeProjectionMatrix(left, right, bottom, top, nearVal, farVal, command->m_requestPixelDataArguments.m_projectionMatrix);
4734
4735 command->m_updateFlags |= REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES;
4736 }
4737
b3RequestCameraImageSetFOVProjectionMatrix(b3SharedMemoryCommandHandle commandHandle,float fov,float aspect,float nearVal,float farVal)4738 B3_SHARED_API void b3RequestCameraImageSetFOVProjectionMatrix(b3SharedMemoryCommandHandle commandHandle, float fov, float aspect, float nearVal, float farVal)
4739 {
4740 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
4741 b3Assert(command);
4742 b3Assert(command->m_type == CMD_REQUEST_CAMERA_IMAGE_DATA);
4743
4744 b3ComputeProjectionMatrixFOV(fov, aspect, nearVal, farVal, command->m_requestPixelDataArguments.m_projectionMatrix);
4745
4746 command->m_updateFlags |= REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES;
4747 }
4748
b3RequestCameraImageSetPixelResolution(b3SharedMemoryCommandHandle commandHandle,int width,int height)4749 B3_SHARED_API void b3RequestCameraImageSetPixelResolution(b3SharedMemoryCommandHandle commandHandle, int width, int height)
4750 {
4751 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
4752 b3Assert(command);
4753 b3Assert(command->m_type == CMD_REQUEST_CAMERA_IMAGE_DATA);
4754 command->m_requestPixelDataArguments.m_pixelWidth = width;
4755 command->m_requestPixelDataArguments.m_pixelHeight = height;
4756 command->m_updateFlags |= REQUEST_PIXEL_ARGS_SET_PIXEL_WIDTH_HEIGHT;
4757 }
4758
b3GetCameraImageData(b3PhysicsClientHandle physClient,struct b3CameraImageData * imageData)4759 B3_SHARED_API void b3GetCameraImageData(b3PhysicsClientHandle physClient, struct b3CameraImageData* imageData)
4760 {
4761 PhysicsClient* cl = (PhysicsClient*)physClient;
4762 if (cl)
4763 {
4764 cl->getCachedCameraImage(imageData);
4765 }
4766 }
4767
4768 ///request an contact point information
b3InitRequestContactPointInformation(b3PhysicsClientHandle physClient)4769 B3_SHARED_API b3SharedMemoryCommandHandle b3InitRequestContactPointInformation(b3PhysicsClientHandle physClient)
4770 {
4771 PhysicsClient* cl = (PhysicsClient*)physClient;
4772 b3Assert(cl);
4773 b3Assert(cl->canSubmitCommand());
4774 struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
4775 b3Assert(command);
4776 command->m_type = CMD_REQUEST_CONTACT_POINT_INFORMATION;
4777 command->m_requestContactPointArguments.m_startingContactPointIndex = 0;
4778 command->m_requestContactPointArguments.m_objectAIndexFilter = -1;
4779 command->m_requestContactPointArguments.m_objectBIndexFilter = -1;
4780 command->m_requestContactPointArguments.m_linkIndexAIndexFilter = -2;
4781 command->m_requestContactPointArguments.m_linkIndexBIndexFilter = -2;
4782 command->m_updateFlags = 0;
4783 return (b3SharedMemoryCommandHandle)command;
4784 }
4785
b3SetContactFilterBodyA(b3SharedMemoryCommandHandle commandHandle,int bodyUniqueIdA)4786 B3_SHARED_API void b3SetContactFilterBodyA(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdA)
4787 {
4788 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
4789 b3Assert(command);
4790 b3Assert(command->m_type == CMD_REQUEST_CONTACT_POINT_INFORMATION);
4791 command->m_requestContactPointArguments.m_objectAIndexFilter = bodyUniqueIdA;
4792 }
4793
b3SetContactFilterLinkA(b3SharedMemoryCommandHandle commandHandle,int linkIndexA)4794 B3_SHARED_API void b3SetContactFilterLinkA(b3SharedMemoryCommandHandle commandHandle, int linkIndexA)
4795 {
4796 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
4797 b3Assert(command);
4798 b3Assert(command->m_type == CMD_REQUEST_CONTACT_POINT_INFORMATION);
4799 command->m_updateFlags |= CMD_REQUEST_CONTACT_POINT_HAS_LINK_INDEX_A_FILTER;
4800 command->m_requestContactPointArguments.m_linkIndexAIndexFilter = linkIndexA;
4801 }
4802
b3SetContactFilterLinkB(b3SharedMemoryCommandHandle commandHandle,int linkIndexB)4803 B3_SHARED_API void b3SetContactFilterLinkB(b3SharedMemoryCommandHandle commandHandle, int linkIndexB)
4804 {
4805 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
4806 b3Assert(command);
4807 b3Assert(command->m_type == CMD_REQUEST_CONTACT_POINT_INFORMATION);
4808 command->m_updateFlags |= CMD_REQUEST_CONTACT_POINT_HAS_LINK_INDEX_B_FILTER;
4809 command->m_requestContactPointArguments.m_linkIndexBIndexFilter = linkIndexB;
4810 }
4811
b3SetClosestDistanceFilterLinkA(b3SharedMemoryCommandHandle commandHandle,int linkIndexA)4812 B3_SHARED_API void b3SetClosestDistanceFilterLinkA(b3SharedMemoryCommandHandle commandHandle, int linkIndexA)
4813 {
4814 b3SetContactFilterLinkA(commandHandle, linkIndexA);
4815 }
4816
b3SetClosestDistanceFilterLinkB(b3SharedMemoryCommandHandle commandHandle,int linkIndexB)4817 B3_SHARED_API void b3SetClosestDistanceFilterLinkB(b3SharedMemoryCommandHandle commandHandle, int linkIndexB)
4818 {
4819 b3SetContactFilterLinkB(commandHandle, linkIndexB);
4820 }
4821
b3SetContactFilterBodyB(b3SharedMemoryCommandHandle commandHandle,int bodyUniqueIdB)4822 B3_SHARED_API void b3SetContactFilterBodyB(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdB)
4823 {
4824 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
4825 b3Assert(command);
4826 b3Assert(command->m_type == CMD_REQUEST_CONTACT_POINT_INFORMATION);
4827 command->m_requestContactPointArguments.m_objectBIndexFilter = bodyUniqueIdB;
4828 }
4829
4830 ///compute the closest points between two bodies
b3InitClosestDistanceQuery(b3PhysicsClientHandle physClient)4831 B3_SHARED_API b3SharedMemoryCommandHandle b3InitClosestDistanceQuery(b3PhysicsClientHandle physClient)
4832 {
4833 b3SharedMemoryCommandHandle commandHandle = b3InitRequestContactPointInformation(physClient);
4834 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
4835 b3Assert(command);
4836 b3Assert(command->m_type == CMD_REQUEST_CONTACT_POINT_INFORMATION);
4837 command->m_updateFlags = CMD_REQUEST_CONTACT_POINT_HAS_QUERY_MODE;
4838 command->m_requestContactPointArguments.m_mode = CONTACT_QUERY_MODE_COMPUTE_CLOSEST_POINTS;
4839 return commandHandle;
4840 }
4841
b3SetClosestDistanceFilterBodyA(b3SharedMemoryCommandHandle commandHandle,int bodyUniqueIdA)4842 B3_SHARED_API void b3SetClosestDistanceFilterBodyA(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdA)
4843 {
4844 b3SetContactFilterBodyA(commandHandle, bodyUniqueIdA);
4845 }
4846
b3SetClosestDistanceFilterBodyB(b3SharedMemoryCommandHandle commandHandle,int bodyUniqueIdB)4847 B3_SHARED_API void b3SetClosestDistanceFilterBodyB(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdB)
4848 {
4849 b3SetContactFilterBodyB(commandHandle, bodyUniqueIdB);
4850 }
4851
b3SetClosestDistanceThreshold(b3SharedMemoryCommandHandle commandHandle,double distance)4852 B3_SHARED_API void b3SetClosestDistanceThreshold(b3SharedMemoryCommandHandle commandHandle, double distance)
4853 {
4854 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
4855 b3Assert(command);
4856 b3Assert(command->m_type == CMD_REQUEST_CONTACT_POINT_INFORMATION);
4857 command->m_updateFlags |= CMD_REQUEST_CONTACT_POINT_HAS_CLOSEST_DISTANCE_THRESHOLD;
4858 command->m_requestContactPointArguments.m_closestDistanceThreshold = distance;
4859 }
4860
b3SetClosestDistanceFilterCollisionShapeA(b3SharedMemoryCommandHandle commandHandle,int collisionShapeA)4861 B3_SHARED_API void b3SetClosestDistanceFilterCollisionShapeA(b3SharedMemoryCommandHandle commandHandle, int collisionShapeA)
4862 {
4863 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
4864 b3Assert(command);
4865 b3Assert(command->m_type == CMD_REQUEST_CONTACT_POINT_INFORMATION);
4866 command->m_updateFlags |= CMD_REQUEST_CONTACT_POINT_HAS_COLLISION_SHAPE_A;
4867 command->m_requestContactPointArguments.m_collisionShapeA = collisionShapeA;
4868 }
4869
b3SetClosestDistanceFilterCollisionShapeB(b3SharedMemoryCommandHandle commandHandle,int collisionShapeB)4870 B3_SHARED_API void b3SetClosestDistanceFilterCollisionShapeB(b3SharedMemoryCommandHandle commandHandle, int collisionShapeB)
4871 {
4872 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
4873 b3Assert(command);
4874 b3Assert(command->m_type == CMD_REQUEST_CONTACT_POINT_INFORMATION);
4875 command->m_updateFlags |= CMD_REQUEST_CONTACT_POINT_HAS_COLLISION_SHAPE_B;
4876 command->m_requestContactPointArguments.m_collisionShapeB = collisionShapeB;
4877 }
4878
b3SetClosestDistanceFilterCollisionShapePositionA(b3SharedMemoryCommandHandle commandHandle,const double collisionShapePositionA[])4879 B3_SHARED_API void b3SetClosestDistanceFilterCollisionShapePositionA(b3SharedMemoryCommandHandle commandHandle, const double collisionShapePositionA[/*3*/])
4880 {
4881 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
4882 b3Assert(command);
4883 b3Assert(command->m_type == CMD_REQUEST_CONTACT_POINT_INFORMATION);
4884 command->m_updateFlags |= CMD_REQUEST_CONTACT_POINT_HAS_COLLISION_SHAPE_POSITION_A;
4885 command->m_requestContactPointArguments.m_collisionShapePositionA[0] = collisionShapePositionA[0];
4886 command->m_requestContactPointArguments.m_collisionShapePositionA[1] = collisionShapePositionA[1];
4887 command->m_requestContactPointArguments.m_collisionShapePositionA[2] = collisionShapePositionA[2];
4888 }
4889
b3SetClosestDistanceFilterCollisionShapePositionB(b3SharedMemoryCommandHandle commandHandle,const double collisionShapePositionB[])4890 B3_SHARED_API void b3SetClosestDistanceFilterCollisionShapePositionB(b3SharedMemoryCommandHandle commandHandle, const double collisionShapePositionB[/*3*/])
4891 {
4892 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
4893 b3Assert(command);
4894 b3Assert(command->m_type == CMD_REQUEST_CONTACT_POINT_INFORMATION);
4895 command->m_updateFlags |= CMD_REQUEST_CONTACT_POINT_HAS_COLLISION_SHAPE_POSITION_B;
4896 command->m_requestContactPointArguments.m_collisionShapePositionB[0] = collisionShapePositionB[0];
4897 command->m_requestContactPointArguments.m_collisionShapePositionB[1] = collisionShapePositionB[1];
4898 command->m_requestContactPointArguments.m_collisionShapePositionB[2] = collisionShapePositionB[2];
4899 }
4900
b3SetClosestDistanceFilterCollisionShapeOrientationA(b3SharedMemoryCommandHandle commandHandle,const double collisionShapeOrientationA[])4901 B3_SHARED_API void b3SetClosestDistanceFilterCollisionShapeOrientationA(b3SharedMemoryCommandHandle commandHandle, const double collisionShapeOrientationA[/*4*/])
4902 {
4903 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
4904 b3Assert(command);
4905 b3Assert(command->m_type == CMD_REQUEST_CONTACT_POINT_INFORMATION);
4906 command->m_updateFlags |= CMD_REQUEST_CONTACT_POINT_HAS_COLLISION_SHAPE_ORIENTATION_A;
4907 command->m_requestContactPointArguments.m_collisionShapeOrientationA[0] = collisionShapeOrientationA[0];
4908 command->m_requestContactPointArguments.m_collisionShapeOrientationA[1] = collisionShapeOrientationA[1];
4909 command->m_requestContactPointArguments.m_collisionShapeOrientationA[2] = collisionShapeOrientationA[2];
4910 command->m_requestContactPointArguments.m_collisionShapeOrientationA[3] = collisionShapeOrientationA[3];
4911 }
4912
b3SetClosestDistanceFilterCollisionShapeOrientationB(b3SharedMemoryCommandHandle commandHandle,const double collisionShapeOrientationB[])4913 B3_SHARED_API void b3SetClosestDistanceFilterCollisionShapeOrientationB(b3SharedMemoryCommandHandle commandHandle, const double collisionShapeOrientationB[/*4*/])
4914 {
4915 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
4916 b3Assert(command);
4917 b3Assert(command->m_type == CMD_REQUEST_CONTACT_POINT_INFORMATION);
4918 command->m_updateFlags |= CMD_REQUEST_CONTACT_POINT_HAS_COLLISION_SHAPE_ORIENTATION_B;
4919 command->m_requestContactPointArguments.m_collisionShapeOrientationB[0] = collisionShapeOrientationB[0];
4920 command->m_requestContactPointArguments.m_collisionShapeOrientationB[1] = collisionShapeOrientationB[1];
4921 command->m_requestContactPointArguments.m_collisionShapeOrientationB[2] = collisionShapeOrientationB[2];
4922 command->m_requestContactPointArguments.m_collisionShapeOrientationB[3] = collisionShapeOrientationB[3];
4923 }
4924
4925 ///get all the bodies that touch a given axis aligned bounding box specified in world space (min and max coordinates)
b3InitAABBOverlapQuery(b3PhysicsClientHandle physClient,const double aabbMin[3],const double aabbMax[3])4926 B3_SHARED_API b3SharedMemoryCommandHandle b3InitAABBOverlapQuery(b3PhysicsClientHandle physClient, const double aabbMin[3], const double aabbMax[3])
4927 {
4928 PhysicsClient* cl = (PhysicsClient*)physClient;
4929 b3Assert(cl);
4930 b3Assert(cl->canSubmitCommand());
4931 struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
4932 b3Assert(command);
4933 command->m_type = CMD_REQUEST_AABB_OVERLAP;
4934 command->m_updateFlags = 0;
4935 command->m_requestOverlappingObjectsArgs.m_startingOverlappingObjectIndex = 0;
4936 command->m_requestOverlappingObjectsArgs.m_aabbQueryMin[0] = aabbMin[0];
4937 command->m_requestOverlappingObjectsArgs.m_aabbQueryMin[1] = aabbMin[1];
4938 command->m_requestOverlappingObjectsArgs.m_aabbQueryMin[2] = aabbMin[2];
4939
4940 command->m_requestOverlappingObjectsArgs.m_aabbQueryMax[0] = aabbMax[0];
4941 command->m_requestOverlappingObjectsArgs.m_aabbQueryMax[1] = aabbMax[1];
4942 command->m_requestOverlappingObjectsArgs.m_aabbQueryMax[2] = aabbMax[2];
4943 return (b3SharedMemoryCommandHandle)command;
4944 }
4945
b3GetAABBOverlapResults(b3PhysicsClientHandle physClient,struct b3AABBOverlapData * data)4946 B3_SHARED_API void b3GetAABBOverlapResults(b3PhysicsClientHandle physClient, struct b3AABBOverlapData* data)
4947 {
4948 PhysicsClient* cl = (PhysicsClient*)physClient;
4949 if (cl)
4950 {
4951 cl->getCachedOverlappingObjects(data);
4952 }
4953 }
4954
b3GetContactPointInformation(b3PhysicsClientHandle physClient,struct b3ContactInformation * contactPointData)4955 B3_SHARED_API void b3GetContactPointInformation(b3PhysicsClientHandle physClient, struct b3ContactInformation* contactPointData)
4956 {
4957 PhysicsClient* cl = (PhysicsClient*)physClient;
4958 if (cl)
4959 {
4960 cl->getCachedContactPointInformation(contactPointData);
4961 }
4962 }
4963
b3GetClosestPointInformation(b3PhysicsClientHandle physClient,struct b3ContactInformation * contactPointInfo)4964 B3_SHARED_API void b3GetClosestPointInformation(b3PhysicsClientHandle physClient, struct b3ContactInformation* contactPointInfo)
4965 {
4966 b3GetContactPointInformation(physClient, contactPointInfo);
4967 }
4968
b3InitRequestCollisionShapeInformation(b3PhysicsClientHandle physClient,int bodyUniqueId,int linkIndex)4969 B3_SHARED_API b3SharedMemoryCommandHandle b3InitRequestCollisionShapeInformation(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex)
4970 {
4971 PhysicsClient* cl = (PhysicsClient*)physClient;
4972 b3Assert(cl);
4973 b3Assert(cl->canSubmitCommand());
4974 struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
4975 b3Assert(command);
4976 command->m_type = CMD_REQUEST_COLLISION_SHAPE_INFO;
4977 command->m_requestCollisionShapeDataArguments.m_bodyUniqueId = bodyUniqueId;
4978 command->m_requestCollisionShapeDataArguments.m_linkIndex = linkIndex;
4979
4980 command->m_updateFlags = 0;
4981 return (b3SharedMemoryCommandHandle)command;
4982 }
b3GetCollisionShapeInformation(b3PhysicsClientHandle physClient,struct b3CollisionShapeInformation * collisionShapeInfo)4983 B3_SHARED_API void b3GetCollisionShapeInformation(b3PhysicsClientHandle physClient, struct b3CollisionShapeInformation* collisionShapeInfo)
4984 {
4985 PhysicsClient* cl = (PhysicsClient*)physClient;
4986 if (cl)
4987 {
4988 cl->getCachedCollisionShapeInformation(collisionShapeInfo);
4989 }
4990 }
4991
4992 //request visual shape information
b3InitRequestVisualShapeInformation(b3PhysicsClientHandle physClient,int bodyUniqueIdA)4993 B3_SHARED_API b3SharedMemoryCommandHandle b3InitRequestVisualShapeInformation(b3PhysicsClientHandle physClient, int bodyUniqueIdA)
4994 {
4995 PhysicsClient* cl = (PhysicsClient*)physClient;
4996 b3Assert(cl);
4997 b3Assert(cl->canSubmitCommand());
4998 struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
4999 b3Assert(command);
5000 command->m_type = CMD_REQUEST_VISUAL_SHAPE_INFO;
5001 command->m_requestVisualShapeDataArguments.m_bodyUniqueId = bodyUniqueIdA;
5002 command->m_requestVisualShapeDataArguments.m_startingVisualShapeIndex = 0;
5003 command->m_updateFlags = 0;
5004 return (b3SharedMemoryCommandHandle)command;
5005 }
5006
b3GetVisualShapeInformation(b3PhysicsClientHandle physClient,struct b3VisualShapeInformation * visualShapeInfo)5007 B3_SHARED_API void b3GetVisualShapeInformation(b3PhysicsClientHandle physClient, struct b3VisualShapeInformation* visualShapeInfo)
5008 {
5009 PhysicsClient* cl = (PhysicsClient*)physClient;
5010 if (cl)
5011 {
5012 cl->getCachedVisualShapeInformation(visualShapeInfo);
5013 }
5014 }
5015
b3CreateChangeTextureCommandInit(b3PhysicsClientHandle physClient,int textureUniqueId,int width,int height,const char * rgbPixels)5016 B3_SHARED_API b3SharedMemoryCommandHandle b3CreateChangeTextureCommandInit(b3PhysicsClientHandle physClient, int textureUniqueId, int width, int height, const char* rgbPixels)
5017 {
5018 PhysicsClient* cl = (PhysicsClient*)physClient;
5019 b3Assert(cl);
5020 b3Assert(cl->canSubmitCommand());
5021 struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
5022 b3Assert(command);
5023 command->m_type = CMD_CHANGE_TEXTURE;
5024
5025 command->m_changeTextureArgs.m_textureUniqueId = textureUniqueId;
5026 command->m_changeTextureArgs.m_width = width;
5027 command->m_changeTextureArgs.m_height = height;
5028 int numPixels = width * height;
5029 cl->uploadBulletFileToSharedMemory(rgbPixels, numPixels * 3);
5030 command->m_updateFlags = 0;
5031 return (b3SharedMemoryCommandHandle)command;
5032 }
5033
b3InitLoadTexture(b3PhysicsClientHandle physClient,const char * filename)5034 B3_SHARED_API b3SharedMemoryCommandHandle b3InitLoadTexture(b3PhysicsClientHandle physClient, const char* filename)
5035 {
5036 PhysicsClient* cl = (PhysicsClient*)physClient;
5037 b3Assert(cl);
5038 b3Assert(cl->canSubmitCommand());
5039 struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
5040 b3Assert(command);
5041 command->m_type = CMD_LOAD_TEXTURE;
5042 int len = strlen(filename);
5043 if (len < MAX_FILENAME_LENGTH)
5044 {
5045 strcpy(command->m_loadTextureArguments.m_textureFileName, filename);
5046 }
5047 else
5048 {
5049 command->m_loadTextureArguments.m_textureFileName[0] = 0;
5050 }
5051 command->m_updateFlags = 0;
5052 return (b3SharedMemoryCommandHandle)command;
5053 }
5054
b3GetStatusTextureUniqueId(b3SharedMemoryStatusHandle statusHandle)5055 B3_SHARED_API int b3GetStatusTextureUniqueId(b3SharedMemoryStatusHandle statusHandle)
5056 {
5057 int uid = -1;
5058 const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
5059 if (status)
5060 {
5061 btAssert(status->m_type == CMD_LOAD_TEXTURE_COMPLETED);
5062 if (status->m_type == CMD_LOAD_TEXTURE_COMPLETED)
5063 {
5064 uid = status->m_loadTextureResultArguments.m_textureUniqueId;
5065 }
5066 }
5067 return uid;
5068 }
5069
b3InitUpdateVisualShape(b3PhysicsClientHandle physClient,int bodyUniqueId,int jointIndex,int shapeIndex,int textureUniqueId)5070 B3_SHARED_API b3SharedMemoryCommandHandle b3InitUpdateVisualShape(b3PhysicsClientHandle physClient, int bodyUniqueId, int jointIndex, int shapeIndex, int textureUniqueId)
5071 {
5072 PhysicsClient* cl = (PhysicsClient*)physClient;
5073 b3Assert(cl);
5074 b3Assert(cl->canSubmitCommand());
5075 struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
5076 b3Assert(command);
5077 command->m_type = CMD_UPDATE_VISUAL_SHAPE;
5078 command->m_updateVisualShapeDataArguments.m_bodyUniqueId = bodyUniqueId;
5079 command->m_updateVisualShapeDataArguments.m_jointIndex = jointIndex;
5080 command->m_updateVisualShapeDataArguments.m_shapeIndex = shapeIndex;
5081 command->m_updateVisualShapeDataArguments.m_textureUniqueId = textureUniqueId;
5082 command->m_updateFlags = 0;
5083
5084 if (textureUniqueId >= 0)
5085 {
5086 command->m_updateFlags |= CMD_UPDATE_VISUAL_SHAPE_TEXTURE;
5087 }
5088 return (b3SharedMemoryCommandHandle)command;
5089 }
5090
b3InitUpdateVisualShape2(b3PhysicsClientHandle physClient,int bodyUniqueId,int jointIndex,int shapeIndex)5091 B3_SHARED_API b3SharedMemoryCommandHandle b3InitUpdateVisualShape2(b3PhysicsClientHandle physClient, int bodyUniqueId, int jointIndex, int shapeIndex)
5092 {
5093 PhysicsClient* cl = (PhysicsClient*)physClient;
5094 b3Assert(cl);
5095 b3Assert(cl->canSubmitCommand());
5096 struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
5097 b3Assert(command);
5098 command->m_type = CMD_UPDATE_VISUAL_SHAPE;
5099 command->m_updateVisualShapeDataArguments.m_bodyUniqueId = bodyUniqueId;
5100 command->m_updateVisualShapeDataArguments.m_jointIndex = jointIndex;
5101 command->m_updateVisualShapeDataArguments.m_shapeIndex = shapeIndex;
5102 command->m_updateVisualShapeDataArguments.m_textureUniqueId = -2;
5103 command->m_updateVisualShapeDataArguments.m_flags = 0;
5104 command->m_updateFlags = 0;
5105 return (b3SharedMemoryCommandHandle)command;
5106 }
5107
b3UpdateVisualShapeTexture(b3SharedMemoryCommandHandle commandHandle,int textureUniqueId)5108 B3_SHARED_API void b3UpdateVisualShapeTexture(b3SharedMemoryCommandHandle commandHandle, int textureUniqueId)
5109 {
5110 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
5111 b3Assert(command);
5112 b3Assert(command->m_type == CMD_UPDATE_VISUAL_SHAPE);
5113
5114 if (command->m_type == CMD_UPDATE_VISUAL_SHAPE)
5115 {
5116 if (textureUniqueId >= -1)
5117 {
5118 command->m_updateFlags |= CMD_UPDATE_VISUAL_SHAPE_TEXTURE;
5119 command->m_updateVisualShapeDataArguments.m_textureUniqueId = textureUniqueId;
5120 }
5121 }
5122 }
5123
b3UpdateVisualShapeRGBAColor(b3SharedMemoryCommandHandle commandHandle,const double rgbaColor[4])5124 B3_SHARED_API void b3UpdateVisualShapeRGBAColor(b3SharedMemoryCommandHandle commandHandle, const double rgbaColor[4])
5125 {
5126 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
5127 b3Assert(command);
5128 b3Assert(command->m_type == CMD_UPDATE_VISUAL_SHAPE);
5129
5130 if (command->m_type == CMD_UPDATE_VISUAL_SHAPE)
5131 {
5132 command->m_updateVisualShapeDataArguments.m_rgbaColor[0] = rgbaColor[0];
5133 command->m_updateVisualShapeDataArguments.m_rgbaColor[1] = rgbaColor[1];
5134 command->m_updateVisualShapeDataArguments.m_rgbaColor[2] = rgbaColor[2];
5135 command->m_updateVisualShapeDataArguments.m_rgbaColor[3] = rgbaColor[3];
5136 command->m_updateFlags |= CMD_UPDATE_VISUAL_SHAPE_RGBA_COLOR;
5137 }
5138 }
5139
b3UpdateVisualShapeFlags(b3SharedMemoryCommandHandle commandHandle,int flags)5140 B3_SHARED_API void b3UpdateVisualShapeFlags(b3SharedMemoryCommandHandle commandHandle, int flags)
5141 {
5142 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
5143 b3Assert(command);
5144 b3Assert(command->m_type == CMD_UPDATE_VISUAL_SHAPE);
5145
5146 if (command->m_type == CMD_UPDATE_VISUAL_SHAPE)
5147 {
5148 command->m_updateVisualShapeDataArguments.m_flags = flags;
5149 command->m_updateFlags |= CMD_UPDATE_VISUAL_SHAPE_FLAGS;
5150 }
5151 }
5152
5153
5154
b3UpdateVisualShapeSpecularColor(b3SharedMemoryCommandHandle commandHandle,const double specularColor[3])5155 B3_SHARED_API void b3UpdateVisualShapeSpecularColor(b3SharedMemoryCommandHandle commandHandle, const double specularColor[3])
5156 {
5157 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
5158 b3Assert(command);
5159 b3Assert(command->m_type == CMD_UPDATE_VISUAL_SHAPE);
5160
5161 if (command->m_type == CMD_UPDATE_VISUAL_SHAPE)
5162 {
5163 command->m_updateVisualShapeDataArguments.m_specularColor[0] = specularColor[0];
5164 command->m_updateVisualShapeDataArguments.m_specularColor[1] = specularColor[1];
5165 command->m_updateVisualShapeDataArguments.m_specularColor[2] = specularColor[2];
5166 command->m_updateFlags |= CMD_UPDATE_VISUAL_SHAPE_SPECULAR_COLOR;
5167 }
5168 }
5169
b3ApplyExternalForceCommandInit(b3PhysicsClientHandle physClient)5170 B3_SHARED_API b3SharedMemoryCommandHandle b3ApplyExternalForceCommandInit(b3PhysicsClientHandle physClient)
5171 {
5172 PhysicsClient* cl = (PhysicsClient*)physClient;
5173 b3Assert(cl);
5174 b3Assert(cl->canSubmitCommand());
5175 struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
5176 b3Assert(command);
5177
5178 command->m_type = CMD_APPLY_EXTERNAL_FORCE;
5179 command->m_updateFlags = 0;
5180 command->m_externalForceArguments.m_numForcesAndTorques = 0;
5181 return (b3SharedMemoryCommandHandle)command;
5182 }
5183
b3ApplyExternalForce(b3SharedMemoryCommandHandle commandHandle,int bodyUniqueId,int linkId,const double force[3],const double position[3],int flag)5184 B3_SHARED_API void b3ApplyExternalForce(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkId, const double force[3], const double position[3], int flag)
5185 {
5186 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
5187 b3Assert(command);
5188 b3Assert(command->m_type == CMD_APPLY_EXTERNAL_FORCE);
5189 int index = command->m_externalForceArguments.m_numForcesAndTorques;
5190 command->m_externalForceArguments.m_bodyUniqueIds[index] = bodyUniqueId;
5191 command->m_externalForceArguments.m_linkIds[index] = linkId;
5192 command->m_externalForceArguments.m_forceFlags[index] = EF_FORCE + flag;
5193 for (int i = 0; i < 3; ++i)
5194 {
5195 command->m_externalForceArguments.m_forcesAndTorques[index + i] = force[i];
5196 command->m_externalForceArguments.m_positions[index + i] = position[i];
5197 }
5198
5199 command->m_externalForceArguments.m_numForcesAndTorques++;
5200 }
5201
b3ApplyExternalTorque(b3SharedMemoryCommandHandle commandHandle,int bodyUniqueId,int linkId,const double torque[3],int flag)5202 B3_SHARED_API void b3ApplyExternalTorque(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkId, const double torque[3], int flag)
5203 {
5204 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
5205 b3Assert(command);
5206 b3Assert(command->m_type == CMD_APPLY_EXTERNAL_FORCE);
5207 int index = command->m_externalForceArguments.m_numForcesAndTorques;
5208 command->m_externalForceArguments.m_bodyUniqueIds[index] = bodyUniqueId;
5209 command->m_externalForceArguments.m_linkIds[index] = linkId;
5210 command->m_externalForceArguments.m_forceFlags[index] = EF_TORQUE + flag;
5211
5212 for (int i = 0; i < 3; ++i)
5213 {
5214 command->m_externalForceArguments.m_forcesAndTorques[index + i] = torque[i];
5215 }
5216 command->m_externalForceArguments.m_numForcesAndTorques++;
5217 }
5218
5219 ///compute the forces to achieve an acceleration, given a state q and qdot using inverse dynamics
b3CalculateInverseDynamicsCommandInit(b3PhysicsClientHandle physClient,int bodyUniqueId,const double * jointPositionsQ,const double * jointVelocitiesQdot,const double * jointAccelerations)5220 B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateInverseDynamicsCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId,
5221 const double* jointPositionsQ, const double* jointVelocitiesQdot, const double* jointAccelerations)
5222 {
5223 PhysicsClient* cl = (PhysicsClient*)physClient;
5224 b3Assert(cl);
5225 b3Assert(cl->canSubmitCommand());
5226 struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
5227 b3Assert(command);
5228
5229 command->m_type = CMD_CALCULATE_INVERSE_DYNAMICS;
5230 command->m_updateFlags = 0;
5231 command->m_calculateInverseDynamicsArguments.m_bodyUniqueId = bodyUniqueId;
5232 command->m_calculateInverseDynamicsArguments.m_flags = 0;
5233
5234 int dofCount = b3ComputeDofCount(physClient, bodyUniqueId);
5235
5236 for (int i = 0; i < dofCount; i++)
5237 {
5238 command->m_calculateInverseDynamicsArguments.m_jointPositionsQ[i] = jointPositionsQ[i];
5239 command->m_calculateInverseDynamicsArguments.m_jointVelocitiesQdot[i] = jointVelocitiesQdot[i];
5240 command->m_calculateInverseDynamicsArguments.m_jointAccelerations[i] = jointAccelerations[i];
5241 }
5242 command->m_calculateInverseDynamicsArguments.m_dofCountQ = dofCount;
5243 command->m_calculateInverseDynamicsArguments.m_dofCountQdot = dofCount;
5244
5245 return (b3SharedMemoryCommandHandle)command;
5246 }
5247
5248 ///compute the forces to achieve an acceleration, given a state q and qdot using inverse dynamics
b3CalculateInverseDynamicsCommandInit2(b3PhysicsClientHandle physClient,int bodyUniqueId,const double * jointPositionsQ,int dofCountQ,const double * jointVelocitiesQdot,const double * jointAccelerations,int dofCountQdot)5249 B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateInverseDynamicsCommandInit2(b3PhysicsClientHandle physClient, int bodyUniqueId,
5250 const double* jointPositionsQ, int dofCountQ, const double* jointVelocitiesQdot, const double* jointAccelerations, int dofCountQdot)
5251 {
5252 PhysicsClient* cl = (PhysicsClient*)physClient;
5253 b3Assert(cl);
5254 b3Assert(cl->canSubmitCommand());
5255 struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
5256 b3Assert(command);
5257
5258 command->m_type = CMD_CALCULATE_INVERSE_DYNAMICS;
5259 command->m_updateFlags = 0;
5260 command->m_calculateInverseDynamicsArguments.m_bodyUniqueId = bodyUniqueId;
5261 command->m_calculateInverseDynamicsArguments.m_flags = 0;
5262
5263 command->m_calculateInverseDynamicsArguments.m_dofCountQ = dofCountQ;
5264 for (int i = 0; i < dofCountQ; i++)
5265 {
5266 command->m_calculateInverseDynamicsArguments.m_jointPositionsQ[i] = jointPositionsQ[i];
5267 }
5268
5269 command->m_calculateInverseDynamicsArguments.m_dofCountQdot = dofCountQdot;
5270 for (int i=0;i<dofCountQdot;i++)
5271 {
5272 command->m_calculateInverseDynamicsArguments.m_jointVelocitiesQdot[i] = jointVelocitiesQdot[i];
5273 command->m_calculateInverseDynamicsArguments.m_jointAccelerations[i] = jointAccelerations[i];
5274 }
5275
5276 return (b3SharedMemoryCommandHandle)command;
5277 }
5278
b3CalculateInverseDynamicsSetFlags(b3SharedMemoryCommandHandle commandHandle,int flags)5279 B3_SHARED_API void b3CalculateInverseDynamicsSetFlags(b3SharedMemoryCommandHandle commandHandle, int flags)
5280 {
5281 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
5282 command->m_calculateInverseDynamicsArguments.m_flags = flags;
5283 }
5284
b3GetStatusInverseDynamicsJointForces(b3SharedMemoryStatusHandle statusHandle,int * bodyUniqueId,int * dofCount,double * jointForces)5285 B3_SHARED_API int b3GetStatusInverseDynamicsJointForces(b3SharedMemoryStatusHandle statusHandle,
5286 int* bodyUniqueId,
5287 int* dofCount,
5288 double* jointForces)
5289 {
5290 const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
5291 if (status == 0)
5292 return false;
5293
5294 btAssert(status->m_type == CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED);
5295 if (status->m_type != CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED)
5296 return false;
5297
5298 if (dofCount)
5299 {
5300 *dofCount = status->m_inverseDynamicsResultArgs.m_dofCount;
5301 }
5302 if (bodyUniqueId)
5303 {
5304 *bodyUniqueId = status->m_inverseDynamicsResultArgs.m_bodyUniqueId;
5305 }
5306 if (jointForces)
5307 {
5308 for (int i = 0; i < status->m_inverseDynamicsResultArgs.m_dofCount; i++)
5309 {
5310 jointForces[i] = status->m_inverseDynamicsResultArgs.m_jointForces[i];
5311 }
5312 }
5313
5314 return true;
5315 }
5316
b3CalculateJacobianCommandInit(b3PhysicsClientHandle physClient,int bodyUniqueId,int linkIndex,const double * localPosition,const double * jointPositionsQ,const double * jointVelocitiesQdot,const double * jointAccelerations)5317 B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateJacobianCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, const double* localPosition, const double* jointPositionsQ, const double* jointVelocitiesQdot, const double* jointAccelerations)
5318 {
5319 PhysicsClient* cl = (PhysicsClient*)physClient;
5320 b3Assert(cl);
5321 b3Assert(cl->canSubmitCommand());
5322 struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
5323 b3Assert(command);
5324
5325 command->m_type = CMD_CALCULATE_JACOBIAN;
5326 command->m_updateFlags = 0;
5327 command->m_calculateJacobianArguments.m_bodyUniqueId = bodyUniqueId;
5328 command->m_calculateJacobianArguments.m_linkIndex = linkIndex;
5329 command->m_calculateJacobianArguments.m_localPosition[0] = localPosition[0];
5330 command->m_calculateJacobianArguments.m_localPosition[1] = localPosition[1];
5331 command->m_calculateJacobianArguments.m_localPosition[2] = localPosition[2];
5332
5333 int numDofs = b3ComputeDofCount(physClient, bodyUniqueId);
5334 for (int i = 0; i < numDofs; i++)
5335 {
5336 command->m_calculateJacobianArguments.m_jointPositionsQ[i] = jointPositionsQ[i];
5337 command->m_calculateJacobianArguments.m_jointVelocitiesQdot[i] = jointVelocitiesQdot[i];
5338 command->m_calculateJacobianArguments.m_jointAccelerations[i] = jointAccelerations[i];
5339 }
5340
5341 return (b3SharedMemoryCommandHandle)command;
5342 }
5343
b3GetStatusJacobian(b3SharedMemoryStatusHandle statusHandle,int * dofCount,double * linearJacobian,double * angularJacobian)5344 B3_SHARED_API int b3GetStatusJacobian(b3SharedMemoryStatusHandle statusHandle, int* dofCount, double* linearJacobian, double* angularJacobian)
5345 {
5346 const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
5347 if (status == 0)
5348 return false;
5349
5350 btAssert(status->m_type == CMD_CALCULATED_JACOBIAN_COMPLETED);
5351 if (status->m_type != CMD_CALCULATED_JACOBIAN_COMPLETED)
5352 return false;
5353
5354 if (dofCount)
5355 {
5356 *dofCount = status->m_jacobianResultArgs.m_dofCount;
5357 }
5358 if (linearJacobian)
5359 {
5360 for (int i = 0; i < status->m_jacobianResultArgs.m_dofCount * 3; i++)
5361 {
5362 linearJacobian[i] = status->m_jacobianResultArgs.m_linearJacobian[i];
5363 }
5364 }
5365 if (angularJacobian)
5366 {
5367 for (int i = 0; i < status->m_jacobianResultArgs.m_dofCount * 3; i++)
5368 {
5369 angularJacobian[i] = status->m_jacobianResultArgs.m_angularJacobian[i];
5370 }
5371 }
5372
5373 return true;
5374 }
b3CalculateMassMatrixSetFlags(b3SharedMemoryCommandHandle commandHandle,int flags)5375 B3_SHARED_API void b3CalculateMassMatrixSetFlags(b3SharedMemoryCommandHandle commandHandle, int flags)
5376 {
5377 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
5378 command->m_calculateMassMatrixArguments.m_flags = flags;
5379 }
5380
b3CalculateMassMatrixCommandInit(b3PhysicsClientHandle physClient,int bodyUniqueId,const double * jointPositionsQ,int dofCountQ)5381 B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateMassMatrixCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, const double* jointPositionsQ, int dofCountQ)
5382 {
5383 PhysicsClient* cl = (PhysicsClient*)physClient;
5384 b3Assert(cl);
5385 b3Assert(cl->canSubmitCommand());
5386 struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
5387 b3Assert(command);
5388
5389 command->m_type = CMD_CALCULATE_MASS_MATRIX;
5390 command->m_updateFlags = 0;
5391
5392 for (int i = 0; i < dofCountQ; i++)
5393 {
5394 command->m_calculateMassMatrixArguments.m_jointPositionsQ[i] = jointPositionsQ[i];
5395 }
5396 command->m_calculateMassMatrixArguments.m_bodyUniqueId = bodyUniqueId;
5397 command->m_calculateMassMatrixArguments.m_dofCountQ = dofCountQ;
5398 command->m_calculateMassMatrixArguments.m_flags = 0;
5399
5400 return (b3SharedMemoryCommandHandle)command;
5401 }
5402
b3GetStatusMassMatrix(b3PhysicsClientHandle physClient,b3SharedMemoryStatusHandle statusHandle,int * dofCount,double * massMatrix)5403 B3_SHARED_API int b3GetStatusMassMatrix(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int* dofCount, double* massMatrix)
5404 {
5405 PhysicsClient* cl = (PhysicsClient*)physClient;
5406 b3Assert(cl);
5407
5408 const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
5409 if (status == 0)
5410 return false;
5411
5412 btAssert(status->m_type == CMD_CALCULATED_MASS_MATRIX_COMPLETED);
5413 if (status->m_type != CMD_CALCULATED_MASS_MATRIX_COMPLETED)
5414 return false;
5415
5416 if (dofCount)
5417 {
5418 *dofCount = status->m_massMatrixResultArgs.m_dofCount;
5419 }
5420 if (massMatrix)
5421 {
5422 cl->getCachedMassMatrix(status->m_massMatrixResultArgs.m_dofCount, massMatrix);
5423 }
5424
5425 return true;
5426 }
5427
b3CollisionFilterCommandInit(b3PhysicsClientHandle physClient)5428 B3_SHARED_API b3SharedMemoryCommandHandle b3CollisionFilterCommandInit(b3PhysicsClientHandle physClient)
5429 {
5430 PhysicsClient* cl = (PhysicsClient*)physClient;
5431 b3Assert(cl);
5432 b3Assert(cl->canSubmitCommand());
5433 struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
5434 b3Assert(command);
5435 command->m_type = CMD_COLLISION_FILTER;
5436 command->m_collisionFilterArgs.m_bodyUniqueIdA = -1;
5437 command->m_collisionFilterArgs.m_bodyUniqueIdB = -1;
5438 command->m_collisionFilterArgs.m_linkIndexA = -2;
5439 command->m_collisionFilterArgs.m_linkIndexB = -2;
5440 command->m_collisionFilterArgs.m_enableCollision = 0;
5441 command->m_updateFlags = 0;
5442 return (b3SharedMemoryCommandHandle)command;
5443 }
5444
b3SetCollisionFilterPair(b3SharedMemoryCommandHandle commandHandle,int bodyUniqueIdA,int bodyUniqueIdB,int linkIndexA,int linkIndexB,int enableCollision)5445 B3_SHARED_API void b3SetCollisionFilterPair(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdA,
5446 int bodyUniqueIdB, int linkIndexA, int linkIndexB, int enableCollision)
5447 {
5448 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
5449 b3Assert(command);
5450 b3Assert(command->m_type == CMD_COLLISION_FILTER);
5451 command->m_updateFlags = B3_COLLISION_FILTER_PAIR;
5452 command->m_collisionFilterArgs.m_bodyUniqueIdA = bodyUniqueIdA;
5453 command->m_collisionFilterArgs.m_bodyUniqueIdB = bodyUniqueIdB;
5454 command->m_collisionFilterArgs.m_linkIndexA = linkIndexA;
5455 command->m_collisionFilterArgs.m_linkIndexB = linkIndexB;
5456 command->m_collisionFilterArgs.m_enableCollision = enableCollision;
5457 }
5458
b3SetCollisionFilterGroupMask(b3SharedMemoryCommandHandle commandHandle,int bodyUniqueIdA,int linkIndexA,int collisionFilterGroup,int collisionFilterMask)5459 B3_SHARED_API void b3SetCollisionFilterGroupMask(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdA,
5460 int linkIndexA, int collisionFilterGroup, int collisionFilterMask)
5461 {
5462 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
5463 b3Assert(command);
5464 b3Assert(command->m_type == CMD_COLLISION_FILTER);
5465 command->m_updateFlags = B3_COLLISION_FILTER_GROUP_MASK;
5466 command->m_collisionFilterArgs.m_bodyUniqueIdA = bodyUniqueIdA;
5467 command->m_collisionFilterArgs.m_linkIndexA = linkIndexA;
5468 command->m_collisionFilterArgs.m_collisionFilterGroup = collisionFilterGroup;
5469 command->m_collisionFilterArgs.m_collisionFilterMask = collisionFilterMask;
5470 }
5471
5472 ///compute the joint positions to move the end effector to a desired target using inverse kinematics
b3CalculateInverseKinematicsCommandInit(b3PhysicsClientHandle physClient,int bodyUniqueId)5473 B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateInverseKinematicsCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId)
5474 {
5475 PhysicsClient* cl = (PhysicsClient*)physClient;
5476 b3Assert(cl);
5477 b3Assert(cl->canSubmitCommand());
5478 struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
5479 b3Assert(command);
5480
5481 command->m_type = CMD_CALCULATE_INVERSE_KINEMATICS;
5482 command->m_updateFlags = 0;
5483 command->m_calculateInverseKinematicsArguments.m_bodyUniqueId = bodyUniqueId;
5484
5485 return (b3SharedMemoryCommandHandle)command;
5486 }
5487
b3CalculateInverseKinematicsAddTargetPurePosition(b3SharedMemoryCommandHandle commandHandle,int endEffectorLinkIndex,const double targetPosition[3])5488 B3_SHARED_API void b3CalculateInverseKinematicsAddTargetPurePosition(b3SharedMemoryCommandHandle commandHandle, int endEffectorLinkIndex, const double targetPosition[3])
5489 {
5490 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
5491 b3Assert(command);
5492 b3Assert(command->m_type == CMD_CALCULATE_INVERSE_KINEMATICS);
5493 command->m_updateFlags |= IK_HAS_TARGET_POSITION;
5494 command->m_calculateInverseKinematicsArguments.m_endEffectorLinkIndices[0] = endEffectorLinkIndex;
5495 command->m_calculateInverseKinematicsArguments.m_targetPositions[0] = targetPosition[0];
5496 command->m_calculateInverseKinematicsArguments.m_targetPositions[1] = targetPosition[1];
5497 command->m_calculateInverseKinematicsArguments.m_targetPositions[2] = targetPosition[2];
5498 command->m_calculateInverseKinematicsArguments.m_numEndEffectorLinkIndices = 1;
5499
5500 command->m_calculateInverseKinematicsArguments.m_targetOrientation[0] = 0;
5501 command->m_calculateInverseKinematicsArguments.m_targetOrientation[1] = 0;
5502 command->m_calculateInverseKinematicsArguments.m_targetOrientation[2] = 0;
5503 command->m_calculateInverseKinematicsArguments.m_targetOrientation[3] = 1;
5504 }
5505
b3CalculateInverseKinematicsAddTargetsPurePosition(b3SharedMemoryCommandHandle commandHandle,int numEndEffectorLinkIndices,const int * endEffectorIndices,const double * targetPositions)5506 B3_SHARED_API void b3CalculateInverseKinematicsAddTargetsPurePosition(b3SharedMemoryCommandHandle commandHandle, int numEndEffectorLinkIndices, const int* endEffectorIndices, const double* targetPositions)
5507 {
5508 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
5509 b3Assert(command);
5510 b3Assert(command->m_type == CMD_CALCULATE_INVERSE_KINEMATICS);
5511 command->m_updateFlags |= IK_HAS_TARGET_POSITION;
5512 command->m_calculateInverseKinematicsArguments.m_numEndEffectorLinkIndices = numEndEffectorLinkIndices;
5513
5514 for (int i = 0; i < numEndEffectorLinkIndices; i++)
5515 {
5516 command->m_calculateInverseKinematicsArguments.m_endEffectorLinkIndices[i] = endEffectorIndices[i];
5517 command->m_calculateInverseKinematicsArguments.m_targetPositions[i * 3 + 0] = targetPositions[i * 3 + 0];
5518 command->m_calculateInverseKinematicsArguments.m_targetPositions[i * 3 + 1] = targetPositions[i * 3 + 1];
5519 command->m_calculateInverseKinematicsArguments.m_targetPositions[i * 3 + 2] = targetPositions[i * 3 + 2];
5520 }
5521 command->m_calculateInverseKinematicsArguments.m_targetOrientation[0] = 0;
5522 command->m_calculateInverseKinematicsArguments.m_targetOrientation[1] = 0;
5523 command->m_calculateInverseKinematicsArguments.m_targetOrientation[2] = 0;
5524 command->m_calculateInverseKinematicsArguments.m_targetOrientation[3] = 1;
5525 }
5526
b3CalculateInverseKinematicsAddTargetPositionWithOrientation(b3SharedMemoryCommandHandle commandHandle,int endEffectorLinkIndex,const double targetPosition[3],const double targetOrientation[4])5527 B3_SHARED_API void b3CalculateInverseKinematicsAddTargetPositionWithOrientation(b3SharedMemoryCommandHandle commandHandle, int endEffectorLinkIndex, const double targetPosition[3], const double targetOrientation[4])
5528 {
5529 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
5530 b3Assert(command);
5531 b3Assert(command->m_type == CMD_CALCULATE_INVERSE_KINEMATICS);
5532 command->m_updateFlags |= IK_HAS_TARGET_POSITION + IK_HAS_TARGET_ORIENTATION;
5533 command->m_calculateInverseKinematicsArguments.m_endEffectorLinkIndices[0] = endEffectorLinkIndex;
5534 command->m_calculateInverseKinematicsArguments.m_numEndEffectorLinkIndices = 1;
5535
5536 command->m_calculateInverseKinematicsArguments.m_targetPositions[0] = targetPosition[0];
5537 command->m_calculateInverseKinematicsArguments.m_targetPositions[1] = targetPosition[1];
5538 command->m_calculateInverseKinematicsArguments.m_targetPositions[2] = targetPosition[2];
5539
5540 command->m_calculateInverseKinematicsArguments.m_targetOrientation[0] = targetOrientation[0];
5541 command->m_calculateInverseKinematicsArguments.m_targetOrientation[1] = targetOrientation[1];
5542 command->m_calculateInverseKinematicsArguments.m_targetOrientation[2] = targetOrientation[2];
5543 command->m_calculateInverseKinematicsArguments.m_targetOrientation[3] = targetOrientation[3];
5544 }
5545
b3CalculateInverseKinematicsPosWithNullSpaceVel(b3SharedMemoryCommandHandle commandHandle,int numDof,int endEffectorLinkIndex,const double targetPosition[3],const double * lowerLimit,const double * upperLimit,const double * jointRange,const double * restPose)5546 B3_SHARED_API void b3CalculateInverseKinematicsPosWithNullSpaceVel(b3SharedMemoryCommandHandle commandHandle, int numDof, int endEffectorLinkIndex, const double targetPosition[3], const double* lowerLimit, const double* upperLimit, const double* jointRange, const double* restPose)
5547 {
5548 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
5549 b3Assert(command);
5550 b3Assert(command->m_type == CMD_CALCULATE_INVERSE_KINEMATICS);
5551 command->m_updateFlags |= IK_HAS_TARGET_POSITION + IK_HAS_NULL_SPACE_VELOCITY;
5552 command->m_calculateInverseKinematicsArguments.m_endEffectorLinkIndices[0] = endEffectorLinkIndex;
5553 command->m_calculateInverseKinematicsArguments.m_numEndEffectorLinkIndices = 1;
5554
5555 command->m_calculateInverseKinematicsArguments.m_targetPositions[0] = targetPosition[0];
5556 command->m_calculateInverseKinematicsArguments.m_targetPositions[1] = targetPosition[1];
5557 command->m_calculateInverseKinematicsArguments.m_targetPositions[2] = targetPosition[2];
5558
5559 for (int i = 0; i < numDof; ++i)
5560 {
5561 command->m_calculateInverseKinematicsArguments.m_lowerLimit[i] = lowerLimit[i];
5562 command->m_calculateInverseKinematicsArguments.m_upperLimit[i] = upperLimit[i];
5563 command->m_calculateInverseKinematicsArguments.m_jointRange[i] = jointRange[i];
5564 command->m_calculateInverseKinematicsArguments.m_restPose[i] = restPose[i];
5565 }
5566 }
5567
b3CalculateInverseKinematicsPosOrnWithNullSpaceVel(b3SharedMemoryCommandHandle commandHandle,int numDof,int endEffectorLinkIndex,const double targetPosition[3],const double targetOrientation[4],const double * lowerLimit,const double * upperLimit,const double * jointRange,const double * restPose)5568 B3_SHARED_API void b3CalculateInverseKinematicsPosOrnWithNullSpaceVel(b3SharedMemoryCommandHandle commandHandle, int numDof, int endEffectorLinkIndex, const double targetPosition[3], const double targetOrientation[4], const double* lowerLimit, const double* upperLimit, const double* jointRange, const double* restPose)
5569 {
5570 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
5571 b3Assert(command);
5572 b3Assert(command->m_type == CMD_CALCULATE_INVERSE_KINEMATICS);
5573 command->m_updateFlags |= IK_HAS_TARGET_POSITION + IK_HAS_TARGET_ORIENTATION + IK_HAS_NULL_SPACE_VELOCITY;
5574 command->m_calculateInverseKinematicsArguments.m_endEffectorLinkIndices[0] = endEffectorLinkIndex;
5575 command->m_calculateInverseKinematicsArguments.m_numEndEffectorLinkIndices = 1;
5576
5577 command->m_calculateInverseKinematicsArguments.m_targetPositions[0] = targetPosition[0];
5578 command->m_calculateInverseKinematicsArguments.m_targetPositions[1] = targetPosition[1];
5579 command->m_calculateInverseKinematicsArguments.m_targetPositions[2] = targetPosition[2];
5580
5581 command->m_calculateInverseKinematicsArguments.m_targetOrientation[0] = targetOrientation[0];
5582 command->m_calculateInverseKinematicsArguments.m_targetOrientation[1] = targetOrientation[1];
5583 command->m_calculateInverseKinematicsArguments.m_targetOrientation[2] = targetOrientation[2];
5584 command->m_calculateInverseKinematicsArguments.m_targetOrientation[3] = targetOrientation[3];
5585
5586 for (int i = 0; i < numDof; ++i)
5587 {
5588 command->m_calculateInverseKinematicsArguments.m_lowerLimit[i] = lowerLimit[i];
5589 command->m_calculateInverseKinematicsArguments.m_upperLimit[i] = upperLimit[i];
5590 command->m_calculateInverseKinematicsArguments.m_jointRange[i] = jointRange[i];
5591 command->m_calculateInverseKinematicsArguments.m_restPose[i] = restPose[i];
5592 }
5593 }
5594
b3CalculateInverseKinematicsSetCurrentPositions(b3SharedMemoryCommandHandle commandHandle,int numDof,const double * currentJointPositions)5595 B3_SHARED_API void b3CalculateInverseKinematicsSetCurrentPositions(b3SharedMemoryCommandHandle commandHandle, int numDof, const double* currentJointPositions)
5596 {
5597 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
5598 b3Assert(command);
5599 b3Assert(command->m_type == CMD_CALCULATE_INVERSE_KINEMATICS);
5600 command->m_updateFlags |= IK_HAS_CURRENT_JOINT_POSITIONS;
5601 for (int i = 0; i < numDof; ++i)
5602 {
5603 command->m_calculateInverseKinematicsArguments.m_currentPositions[i] = currentJointPositions[i];
5604 }
5605 }
5606
b3CalculateInverseKinematicsSetMaxNumIterations(b3SharedMemoryCommandHandle commandHandle,int maxNumIterations)5607 B3_SHARED_API void b3CalculateInverseKinematicsSetMaxNumIterations(b3SharedMemoryCommandHandle commandHandle, int maxNumIterations)
5608 {
5609 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
5610 b3Assert(command);
5611 b3Assert(command->m_type == CMD_CALCULATE_INVERSE_KINEMATICS);
5612 command->m_updateFlags |= IK_HAS_MAX_ITERATIONS;
5613 command->m_calculateInverseKinematicsArguments.m_maxNumIterations = maxNumIterations;
5614 }
5615
b3CalculateInverseKinematicsSetResidualThreshold(b3SharedMemoryCommandHandle commandHandle,double residualThreshold)5616 B3_SHARED_API void b3CalculateInverseKinematicsSetResidualThreshold(b3SharedMemoryCommandHandle commandHandle, double residualThreshold)
5617 {
5618 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
5619 b3Assert(command);
5620 b3Assert(command->m_type == CMD_CALCULATE_INVERSE_KINEMATICS);
5621 command->m_updateFlags |= IK_HAS_RESIDUAL_THRESHOLD;
5622 command->m_calculateInverseKinematicsArguments.m_residualThreshold = residualThreshold;
5623 }
5624
b3CalculateInverseKinematicsSetJointDamping(b3SharedMemoryCommandHandle commandHandle,int numDof,const double * jointDampingCoeff)5625 B3_SHARED_API void b3CalculateInverseKinematicsSetJointDamping(b3SharedMemoryCommandHandle commandHandle, int numDof, const double* jointDampingCoeff)
5626 {
5627 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
5628 b3Assert(command);
5629 b3Assert(command->m_type == CMD_CALCULATE_INVERSE_KINEMATICS);
5630 command->m_updateFlags |= IK_HAS_JOINT_DAMPING;
5631
5632 for (int i = 0; i < numDof; ++i)
5633 {
5634 command->m_calculateInverseKinematicsArguments.m_jointDamping[i] = jointDampingCoeff[i];
5635 }
5636 }
5637
b3CalculateInverseKinematicsSelectSolver(b3SharedMemoryCommandHandle commandHandle,int solver)5638 B3_SHARED_API void b3CalculateInverseKinematicsSelectSolver(b3SharedMemoryCommandHandle commandHandle, int solver)
5639 {
5640 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
5641 b3Assert(command);
5642 b3Assert(command->m_type == CMD_CALCULATE_INVERSE_KINEMATICS);
5643 command->m_updateFlags |= solver;
5644 }
5645
b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle statusHandle,int * bodyUniqueId,int * dofCount,double * jointPositions)5646 B3_SHARED_API int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle statusHandle,
5647 int* bodyUniqueId,
5648 int* dofCount,
5649 double* jointPositions)
5650 {
5651 const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
5652 btAssert(status);
5653 if (status == 0)
5654 return 0;
5655 btAssert(status->m_type == CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED);
5656 if (status->m_type != CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED)
5657 return 0;
5658
5659 if (dofCount)
5660 {
5661 *dofCount = status->m_inverseKinematicsResultArgs.m_dofCount;
5662 }
5663 if (bodyUniqueId)
5664 {
5665 *bodyUniqueId = status->m_inverseKinematicsResultArgs.m_bodyUniqueId;
5666 }
5667 if (jointPositions)
5668 {
5669 for (int i = 0; i < status->m_inverseKinematicsResultArgs.m_dofCount; i++)
5670 {
5671 jointPositions[i] = status->m_inverseKinematicsResultArgs.m_jointPositions[i];
5672 }
5673 }
5674
5675 return 1;
5676 }
5677
b3RequestVREventsCommandInit(b3PhysicsClientHandle physClient)5678 B3_SHARED_API b3SharedMemoryCommandHandle b3RequestVREventsCommandInit(b3PhysicsClientHandle physClient)
5679 {
5680 PhysicsClient* cl = (PhysicsClient*)physClient;
5681 b3Assert(cl);
5682 b3Assert(cl->canSubmitCommand());
5683 struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
5684 b3Assert(command);
5685
5686 command->m_type = CMD_REQUEST_VR_EVENTS_DATA;
5687 command->m_updateFlags = VR_DEVICE_CONTROLLER;
5688 return (b3SharedMemoryCommandHandle)command;
5689 }
5690
b3VREventsSetDeviceTypeFilter(b3SharedMemoryCommandHandle commandHandle,int deviceTypeFilter)5691 B3_SHARED_API void b3VREventsSetDeviceTypeFilter(b3SharedMemoryCommandHandle commandHandle, int deviceTypeFilter)
5692 {
5693 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
5694 b3Assert(command);
5695 if (command->m_type == CMD_REQUEST_VR_EVENTS_DATA)
5696 {
5697 command->m_updateFlags = deviceTypeFilter;
5698 }
5699 }
5700
b3GetVREventsData(b3PhysicsClientHandle physClient,struct b3VREventsData * vrEventsData)5701 B3_SHARED_API void b3GetVREventsData(b3PhysicsClientHandle physClient, struct b3VREventsData* vrEventsData)
5702 {
5703 PhysicsClient* cl = (PhysicsClient*)physClient;
5704 if (cl)
5705 {
5706 cl->getCachedVREvents(vrEventsData);
5707 }
5708 }
5709
b3SetVRCameraStateCommandInit(b3PhysicsClientHandle physClient)5710 B3_SHARED_API b3SharedMemoryCommandHandle b3SetVRCameraStateCommandInit(b3PhysicsClientHandle physClient)
5711 {
5712 PhysicsClient* cl = (PhysicsClient*)physClient;
5713 b3Assert(cl);
5714 b3Assert(cl->canSubmitCommand());
5715 struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
5716 b3Assert(command);
5717
5718 command->m_type = CMD_SET_VR_CAMERA_STATE;
5719 command->m_updateFlags = 0;
5720
5721 return (b3SharedMemoryCommandHandle)command;
5722 }
5723
b3SetVRCameraRootPosition(b3SharedMemoryCommandHandle commandHandle,const double rootPos[3])5724 B3_SHARED_API int b3SetVRCameraRootPosition(b3SharedMemoryCommandHandle commandHandle, const double rootPos[3])
5725 {
5726 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
5727 b3Assert(command);
5728 b3Assert(command->m_type == CMD_SET_VR_CAMERA_STATE);
5729 command->m_updateFlags |= VR_CAMERA_ROOT_POSITION;
5730 command->m_vrCameraStateArguments.m_rootPosition[0] = rootPos[0];
5731 command->m_vrCameraStateArguments.m_rootPosition[1] = rootPos[1];
5732 command->m_vrCameraStateArguments.m_rootPosition[2] = rootPos[2];
5733 return 0;
5734 }
5735
b3SetVRCameraRootOrientation(b3SharedMemoryCommandHandle commandHandle,const double rootOrn[4])5736 B3_SHARED_API int b3SetVRCameraRootOrientation(b3SharedMemoryCommandHandle commandHandle, const double rootOrn[4])
5737 {
5738 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
5739 b3Assert(command);
5740 b3Assert(command->m_type == CMD_SET_VR_CAMERA_STATE);
5741 command->m_updateFlags |= VR_CAMERA_ROOT_ORIENTATION;
5742 command->m_vrCameraStateArguments.m_rootOrientation[0] = rootOrn[0];
5743 command->m_vrCameraStateArguments.m_rootOrientation[1] = rootOrn[1];
5744 command->m_vrCameraStateArguments.m_rootOrientation[2] = rootOrn[2];
5745 command->m_vrCameraStateArguments.m_rootOrientation[3] = rootOrn[3];
5746
5747 return 0;
5748 }
5749
b3SetVRCameraTrackingObject(b3SharedMemoryCommandHandle commandHandle,int objectUniqueId)5750 B3_SHARED_API int b3SetVRCameraTrackingObject(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId)
5751 {
5752 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
5753 b3Assert(command);
5754 b3Assert(command->m_type == CMD_SET_VR_CAMERA_STATE);
5755 command->m_updateFlags |= VR_CAMERA_ROOT_TRACKING_OBJECT;
5756 command->m_vrCameraStateArguments.m_trackingObjectUniqueId = objectUniqueId;
5757 return 0;
5758 }
5759
b3SetVRCameraTrackingObjectFlag(b3SharedMemoryCommandHandle commandHandle,int flag)5760 B3_SHARED_API int b3SetVRCameraTrackingObjectFlag(b3SharedMemoryCommandHandle commandHandle, int flag)
5761 {
5762 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
5763 b3Assert(command);
5764 b3Assert(command->m_type == CMD_SET_VR_CAMERA_STATE);
5765 command->m_updateFlags |= VR_CAMERA_FLAG;
5766 command->m_vrCameraStateArguments.m_trackingObjectFlag = flag;
5767 return 0;
5768 }
5769
b3RequestKeyboardEventsCommandInit(b3PhysicsClientHandle physClient)5770 B3_SHARED_API b3SharedMemoryCommandHandle b3RequestKeyboardEventsCommandInit(b3PhysicsClientHandle physClient)
5771 {
5772 PhysicsClient* cl = (PhysicsClient*)physClient;
5773 b3Assert(cl);
5774 b3Assert(cl->canSubmitCommand());
5775 struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
5776 b3Assert(command);
5777 return b3RequestKeyboardEventsCommandInit2((b3SharedMemoryCommandHandle)command);
5778 }
5779
b3RequestKeyboardEventsCommandInit2(b3SharedMemoryCommandHandle commandHandle)5780 B3_SHARED_API b3SharedMemoryCommandHandle b3RequestKeyboardEventsCommandInit2(b3SharedMemoryCommandHandle commandHandle)
5781 {
5782 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
5783 command->m_type = CMD_REQUEST_KEYBOARD_EVENTS_DATA;
5784 command->m_updateFlags = 0;
5785
5786 return (b3SharedMemoryCommandHandle)command;
5787 }
5788
b3GetKeyboardEventsData(b3PhysicsClientHandle physClient,struct b3KeyboardEventsData * keyboardEventsData)5789 B3_SHARED_API void b3GetKeyboardEventsData(b3PhysicsClientHandle physClient, struct b3KeyboardEventsData* keyboardEventsData)
5790 {
5791 PhysicsClient* cl = (PhysicsClient*)physClient;
5792 if (cl)
5793 {
5794 cl->getCachedKeyboardEvents(keyboardEventsData);
5795 }
5796 }
5797
b3RequestMouseEventsCommandInit(b3PhysicsClientHandle physClient)5798 B3_SHARED_API b3SharedMemoryCommandHandle b3RequestMouseEventsCommandInit(b3PhysicsClientHandle physClient)
5799 {
5800 PhysicsClient* cl = (PhysicsClient*)physClient;
5801 b3Assert(cl);
5802 b3Assert(cl->canSubmitCommand());
5803 struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
5804 b3Assert(command);
5805
5806 command->m_type = CMD_REQUEST_MOUSE_EVENTS_DATA;
5807 command->m_updateFlags = 0;
5808
5809 return (b3SharedMemoryCommandHandle)command;
5810 }
5811
b3GetMouseEventsData(b3PhysicsClientHandle physClient,struct b3MouseEventsData * mouseEventsData)5812 B3_SHARED_API void b3GetMouseEventsData(b3PhysicsClientHandle physClient, struct b3MouseEventsData* mouseEventsData)
5813 {
5814 PhysicsClient* cl = (PhysicsClient*)physClient;
5815 if (cl)
5816 {
5817 cl->getCachedMouseEvents(mouseEventsData);
5818 }
5819 }
5820
b3ProfileTimingCommandInit(b3PhysicsClientHandle physClient,const char * name)5821 B3_SHARED_API b3SharedMemoryCommandHandle b3ProfileTimingCommandInit(b3PhysicsClientHandle physClient, const char* name)
5822 {
5823 PhysicsClient* cl = (PhysicsClient*)physClient;
5824 b3Assert(cl);
5825 b3Assert(cl->canSubmitCommand());
5826 struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
5827 b3Assert(command);
5828
5829 int len = name ? strlen(name) : 0;
5830 command->m_type = CMD_PROFILE_TIMING;
5831 if (len > 0 && len < (MAX_FILENAME_LENGTH + 1))
5832 {
5833
5834 strcpy(command->m_profile.m_name, name);
5835 command->m_profile.m_name[len] = 0;
5836 }
5837 else
5838 {
5839 command->m_profile.m_name[0] = 0;
5840 }
5841 command->m_profile.m_type = -1;
5842 command->m_profile.m_durationInMicroSeconds = 0;
5843 return (b3SharedMemoryCommandHandle)command;
5844 }
5845
b3PushProfileTiming(b3PhysicsClientHandle physClient,const char * timingName)5846 B3_SHARED_API void b3PushProfileTiming(b3PhysicsClientHandle physClient, const char* timingName)
5847 {
5848 PhysicsClient* cl = (PhysicsClient*)physClient;
5849 b3Assert(cl);
5850 cl->pushProfileTiming(timingName);
5851 }
5852
b3PopProfileTiming(b3PhysicsClientHandle physClient)5853 B3_SHARED_API void b3PopProfileTiming(b3PhysicsClientHandle physClient)
5854 {
5855 PhysicsClient* cl = (PhysicsClient*)physClient;
5856 b3Assert(cl);
5857 cl->popProfileTiming();
5858 }
5859
b3SetProfileTimingDuractionInMicroSeconds(b3SharedMemoryCommandHandle commandHandle,int duration)5860 B3_SHARED_API void b3SetProfileTimingDuractionInMicroSeconds(b3SharedMemoryCommandHandle commandHandle, int duration)
5861 {
5862 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
5863 b3Assert(command);
5864 b3Assert(command->m_type == CMD_PROFILE_TIMING);
5865 if (command->m_type == CMD_PROFILE_TIMING)
5866 {
5867 command->m_profile.m_durationInMicroSeconds = duration;
5868 }
5869 }
5870
b3SetProfileTimingType(b3SharedMemoryCommandHandle commandHandle,int type)5871 B3_SHARED_API void b3SetProfileTimingType(b3SharedMemoryCommandHandle commandHandle, int type)
5872 {
5873 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
5874 b3Assert(command);
5875 b3Assert(command->m_type == CMD_PROFILE_TIMING);
5876 if (command->m_type == CMD_PROFILE_TIMING)
5877 {
5878 command->m_profile.m_type = type;
5879 }
5880 }
5881
5882
b3StateLoggingCommandInit(b3PhysicsClientHandle physClient)5883 B3_SHARED_API b3SharedMemoryCommandHandle b3StateLoggingCommandInit(b3PhysicsClientHandle physClient)
5884 {
5885 PhysicsClient* cl = (PhysicsClient*)physClient;
5886 b3Assert(cl);
5887 b3Assert(cl->canSubmitCommand());
5888 struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
5889 b3Assert(command);
5890
5891 command->m_type = CMD_STATE_LOGGING;
5892 command->m_updateFlags = 0;
5893 command->m_stateLoggingArguments.m_numBodyUniqueIds = 0;
5894 command->m_stateLoggingArguments.m_deviceFilterType = VR_DEVICE_CONTROLLER;
5895
5896 return (b3SharedMemoryCommandHandle)command;
5897 }
5898
b3StateLoggingStart(b3SharedMemoryCommandHandle commandHandle,int loggingType,const char * fileName)5899 B3_SHARED_API int b3StateLoggingStart(b3SharedMemoryCommandHandle commandHandle, int loggingType, const char* fileName)
5900 {
5901 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
5902 b3Assert(command);
5903 b3Assert(command->m_type == CMD_STATE_LOGGING);
5904 if (command->m_type == CMD_STATE_LOGGING)
5905 {
5906 command->m_updateFlags |= STATE_LOGGING_START_LOG;
5907 int len = strlen(fileName);
5908 if (len < MAX_FILENAME_LENGTH)
5909 {
5910 strcpy(command->m_stateLoggingArguments.m_fileName, fileName);
5911 }
5912 else
5913 {
5914 command->m_stateLoggingArguments.m_fileName[0] = 0;
5915 }
5916 command->m_stateLoggingArguments.m_logType = loggingType;
5917 }
5918 return 0;
5919 }
5920
b3GetStatusLoggingUniqueId(b3SharedMemoryStatusHandle statusHandle)5921 B3_SHARED_API int b3GetStatusLoggingUniqueId(b3SharedMemoryStatusHandle statusHandle)
5922 {
5923 const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
5924 b3Assert(status);
5925 b3Assert(status->m_type == CMD_STATE_LOGGING_START_COMPLETED);
5926 if (status && status->m_type == CMD_STATE_LOGGING_START_COMPLETED)
5927 {
5928 return status->m_stateLoggingResultArgs.m_loggingUniqueId;
5929 }
5930 return -1;
5931 }
5932
b3StateLoggingAddLoggingObjectUniqueId(b3SharedMemoryCommandHandle commandHandle,int objectUniqueId)5933 B3_SHARED_API int b3StateLoggingAddLoggingObjectUniqueId(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId)
5934 {
5935 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
5936 b3Assert(command);
5937 b3Assert(command->m_type == CMD_STATE_LOGGING);
5938 if (command->m_type == CMD_STATE_LOGGING)
5939 {
5940 command->m_updateFlags |= STATE_LOGGING_FILTER_OBJECT_UNIQUE_ID;
5941 if (command->m_stateLoggingArguments.m_numBodyUniqueIds < MAX_SDF_BODIES)
5942 {
5943 command->m_stateLoggingArguments.m_bodyUniqueIds[command->m_stateLoggingArguments.m_numBodyUniqueIds++] = objectUniqueId;
5944 }
5945 }
5946 return 0;
5947 }
5948
b3StateLoggingSetLinkIndexA(b3SharedMemoryCommandHandle commandHandle,int linkIndexA)5949 B3_SHARED_API int b3StateLoggingSetLinkIndexA(b3SharedMemoryCommandHandle commandHandle, int linkIndexA)
5950 {
5951 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
5952 b3Assert(command);
5953 b3Assert(command->m_type = CMD_STATE_LOGGING);
5954 if (command->m_type == CMD_STATE_LOGGING)
5955 {
5956 command->m_updateFlags |= STATE_LOGGING_FILTER_LINK_INDEX_A;
5957 command->m_stateLoggingArguments.m_linkIndexA = linkIndexA;
5958 }
5959 return 0;
5960 }
5961
b3StateLoggingSetLinkIndexB(b3SharedMemoryCommandHandle commandHandle,int linkIndexB)5962 B3_SHARED_API int b3StateLoggingSetLinkIndexB(b3SharedMemoryCommandHandle commandHandle, int linkIndexB)
5963 {
5964 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
5965 b3Assert(command);
5966 b3Assert(command->m_type = CMD_STATE_LOGGING);
5967 if (command->m_type == CMD_STATE_LOGGING)
5968 {
5969 command->m_updateFlags |= STATE_LOGGING_FILTER_LINK_INDEX_B;
5970 command->m_stateLoggingArguments.m_linkIndexB = linkIndexB;
5971 }
5972 return 0;
5973 }
5974
b3StateLoggingSetBodyAUniqueId(b3SharedMemoryCommandHandle commandHandle,int bodyAUniqueId)5975 B3_SHARED_API int b3StateLoggingSetBodyAUniqueId(b3SharedMemoryCommandHandle commandHandle, int bodyAUniqueId)
5976 {
5977 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
5978 b3Assert(command);
5979 b3Assert(command->m_type = CMD_STATE_LOGGING);
5980 if (command->m_type == CMD_STATE_LOGGING)
5981 {
5982 command->m_updateFlags |= STATE_LOGGING_FILTER_BODY_UNIQUE_ID_A;
5983 command->m_stateLoggingArguments.m_bodyUniqueIdA = bodyAUniqueId;
5984 }
5985 return 0;
5986 }
5987
b3StateLoggingSetBodyBUniqueId(b3SharedMemoryCommandHandle commandHandle,int bodyBUniqueId)5988 B3_SHARED_API int b3StateLoggingSetBodyBUniqueId(b3SharedMemoryCommandHandle commandHandle, int bodyBUniqueId)
5989 {
5990 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
5991 b3Assert(command);
5992 b3Assert(command->m_type = CMD_STATE_LOGGING);
5993 if (command->m_type == CMD_STATE_LOGGING)
5994 {
5995 command->m_updateFlags |= STATE_LOGGING_FILTER_BODY_UNIQUE_ID_B;
5996 command->m_stateLoggingArguments.m_bodyUniqueIdB = bodyBUniqueId;
5997 }
5998 return 0;
5999 }
6000
b3StateLoggingSetMaxLogDof(b3SharedMemoryCommandHandle commandHandle,int maxLogDof)6001 B3_SHARED_API int b3StateLoggingSetMaxLogDof(b3SharedMemoryCommandHandle commandHandle, int maxLogDof)
6002 {
6003 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
6004 b3Assert(command);
6005 b3Assert(command->m_type == CMD_STATE_LOGGING);
6006 if (command->m_type == CMD_STATE_LOGGING)
6007 {
6008 command->m_updateFlags |= STATE_LOGGING_MAX_LOG_DOF;
6009 command->m_stateLoggingArguments.m_maxLogDof = maxLogDof;
6010 }
6011 return 0;
6012 }
6013
b3StateLoggingSetLogFlags(b3SharedMemoryCommandHandle commandHandle,int logFlags)6014 B3_SHARED_API int b3StateLoggingSetLogFlags(b3SharedMemoryCommandHandle commandHandle, int logFlags)
6015 {
6016 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
6017 b3Assert(command);
6018 b3Assert(command->m_type == CMD_STATE_LOGGING);
6019 if (command->m_type == CMD_STATE_LOGGING)
6020 {
6021 command->m_updateFlags |= STATE_LOGGING_LOG_FLAGS;
6022 command->m_stateLoggingArguments.m_logFlags = logFlags;
6023 }
6024 return 0;
6025 }
6026
b3StateLoggingSetDeviceTypeFilter(b3SharedMemoryCommandHandle commandHandle,int deviceTypeFilter)6027 B3_SHARED_API int b3StateLoggingSetDeviceTypeFilter(b3SharedMemoryCommandHandle commandHandle, int deviceTypeFilter)
6028 {
6029 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
6030 b3Assert(command);
6031 b3Assert(command->m_type == CMD_STATE_LOGGING);
6032 if (command->m_type == CMD_STATE_LOGGING)
6033 {
6034 command->m_updateFlags |= STATE_LOGGING_FILTER_DEVICE_TYPE;
6035 command->m_stateLoggingArguments.m_deviceFilterType = deviceTypeFilter;
6036 }
6037 return 0;
6038 }
6039
b3StateLoggingStop(b3SharedMemoryCommandHandle commandHandle,int loggingUid)6040 B3_SHARED_API int b3StateLoggingStop(b3SharedMemoryCommandHandle commandHandle, int loggingUid)
6041 {
6042 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
6043 b3Assert(command);
6044 b3Assert(command->m_type == CMD_STATE_LOGGING);
6045 if (command->m_type == CMD_STATE_LOGGING)
6046 {
6047 command->m_updateFlags |= STATE_LOGGING_STOP_LOG;
6048 command->m_stateLoggingArguments.m_loggingUniqueId = loggingUid;
6049 }
6050 return 0;
6051 }
6052
6053 ///configure the 3D OpenGL debug visualizer (enable/disable GUI widgets, shadows, position camera etc)
b3InitConfigureOpenGLVisualizer(b3PhysicsClientHandle physClient)6054 B3_SHARED_API b3SharedMemoryCommandHandle b3InitConfigureOpenGLVisualizer(b3PhysicsClientHandle physClient)
6055 {
6056 PhysicsClient* cl = (PhysicsClient*)physClient;
6057 b3Assert(cl);
6058 //b3Assert(cl->canSubmitCommand());
6059 struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
6060 b3Assert(command);
6061
6062 return b3InitConfigureOpenGLVisualizer2((b3SharedMemoryCommandHandle)command);
6063 }
6064
b3InitConfigureOpenGLVisualizer2(b3SharedMemoryCommandHandle commandHandle)6065 B3_SHARED_API b3SharedMemoryCommandHandle b3InitConfigureOpenGLVisualizer2(b3SharedMemoryCommandHandle commandHandle)
6066 {
6067 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
6068 command->m_type = CMD_CONFIGURE_OPENGL_VISUALIZER;
6069 command->m_updateFlags = 0;
6070
6071 return (b3SharedMemoryCommandHandle)command;
6072 }
6073
b3ConfigureOpenGLVisualizerSetVisualizationFlags(b3SharedMemoryCommandHandle commandHandle,int flag,int enabled)6074 B3_SHARED_API void b3ConfigureOpenGLVisualizerSetVisualizationFlags(b3SharedMemoryCommandHandle commandHandle, int flag, int enabled)
6075 {
6076 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
6077 b3Assert(command);
6078 b3Assert(command->m_type == CMD_CONFIGURE_OPENGL_VISUALIZER);
6079 if (command->m_type == CMD_CONFIGURE_OPENGL_VISUALIZER)
6080 {
6081 command->m_updateFlags |= COV_SET_FLAGS;
6082 command->m_configureOpenGLVisualizerArguments.m_setFlag = flag;
6083 command->m_configureOpenGLVisualizerArguments.m_setEnabled = enabled;
6084 }
6085 }
6086
b3ConfigureOpenGLVisualizerSetLightPosition(b3SharedMemoryCommandHandle commandHandle,const float lightPosition[3])6087 B3_SHARED_API void b3ConfigureOpenGLVisualizerSetLightPosition(b3SharedMemoryCommandHandle commandHandle, const float lightPosition[3])
6088 {
6089 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
6090 b3Assert(command);
6091 b3Assert(command->m_type == CMD_CONFIGURE_OPENGL_VISUALIZER);
6092 if (command->m_type == CMD_CONFIGURE_OPENGL_VISUALIZER)
6093 {
6094 command->m_updateFlags |= COV_SET_LIGHT_POSITION;
6095 command->m_configureOpenGLVisualizerArguments.m_lightPosition[0] = lightPosition[0];
6096 command->m_configureOpenGLVisualizerArguments.m_lightPosition[1] = lightPosition[1];
6097 command->m_configureOpenGLVisualizerArguments.m_lightPosition[2] = lightPosition[2];
6098 }
6099 }
6100
b3ConfigureOpenGLVisualizerSetLightRgbBackground(b3SharedMemoryCommandHandle commandHandle,const float rgbBackground[3])6101 B3_SHARED_API void b3ConfigureOpenGLVisualizerSetLightRgbBackground(b3SharedMemoryCommandHandle commandHandle, const float rgbBackground[3])
6102 {
6103 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
6104 b3Assert(command);
6105 b3Assert(command->m_type == CMD_CONFIGURE_OPENGL_VISUALIZER);
6106 if (command->m_type == CMD_CONFIGURE_OPENGL_VISUALIZER)
6107 {
6108 command->m_updateFlags |= COV_SET_RGB_BACKGROUND;
6109 command->m_configureOpenGLVisualizerArguments.m_rgbBackground[0] = rgbBackground[0];
6110 command->m_configureOpenGLVisualizerArguments.m_rgbBackground[1] = rgbBackground[1];
6111 command->m_configureOpenGLVisualizerArguments.m_rgbBackground[2] = rgbBackground[2];
6112 }
6113 }
6114
6115
b3ConfigureOpenGLVisualizerSetShadowMapResolution(b3SharedMemoryCommandHandle commandHandle,int shadowMapResolution)6116 B3_SHARED_API void b3ConfigureOpenGLVisualizerSetShadowMapResolution(b3SharedMemoryCommandHandle commandHandle, int shadowMapResolution)
6117 {
6118 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
6119 b3Assert(command);
6120 b3Assert(command->m_type == CMD_CONFIGURE_OPENGL_VISUALIZER);
6121 if (command->m_type == CMD_CONFIGURE_OPENGL_VISUALIZER)
6122 {
6123 command->m_updateFlags |= COV_SET_SHADOWMAP_RESOLUTION;
6124 command->m_configureOpenGLVisualizerArguments.m_shadowMapResolution = shadowMapResolution;
6125 }
6126 }
6127
b3ConfigureOpenGLVisualizerSetShadowMapIntensity(b3SharedMemoryCommandHandle commandHandle,double shadowMapIntensity)6128 B3_SHARED_API void b3ConfigureOpenGLVisualizerSetShadowMapIntensity(b3SharedMemoryCommandHandle commandHandle, double shadowMapIntensity)
6129 {
6130 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
6131 b3Assert(command);
6132 b3Assert(command->m_type == CMD_CONFIGURE_OPENGL_VISUALIZER);
6133 if (command->m_type == CMD_CONFIGURE_OPENGL_VISUALIZER)
6134 {
6135 command->m_updateFlags |= COV_SET_SHADOWMAP_INTENSITY;
6136 command->m_configureOpenGLVisualizerArguments.m_shadowMapIntensity = shadowMapIntensity;
6137 }
6138 }
6139
6140
b3ConfigureOpenGLVisualizerSetShadowMapWorldSize(b3SharedMemoryCommandHandle commandHandle,int shadowMapWorldSize)6141 B3_SHARED_API void b3ConfigureOpenGLVisualizerSetShadowMapWorldSize(b3SharedMemoryCommandHandle commandHandle, int shadowMapWorldSize)
6142 {
6143 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
6144 b3Assert(command);
6145 b3Assert(command->m_type == CMD_CONFIGURE_OPENGL_VISUALIZER);
6146 if (command->m_type == CMD_CONFIGURE_OPENGL_VISUALIZER)
6147 {
6148 command->m_updateFlags |= COV_SET_SHADOWMAP_WORLD_SIZE;
6149 command->m_configureOpenGLVisualizerArguments.m_shadowMapWorldSize = shadowMapWorldSize;
6150 }
6151 }
6152
6153
b3ConfigureOpenGLVisualizerSetRemoteSyncTransformInterval(b3SharedMemoryCommandHandle commandHandle,double remoteSyncTransformInterval)6154 B3_SHARED_API void b3ConfigureOpenGLVisualizerSetRemoteSyncTransformInterval(b3SharedMemoryCommandHandle commandHandle, double remoteSyncTransformInterval)
6155 {
6156 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
6157 b3Assert(command);
6158 b3Assert(command->m_type == CMD_CONFIGURE_OPENGL_VISUALIZER);
6159 if (command->m_type == CMD_CONFIGURE_OPENGL_VISUALIZER)
6160 {
6161 command->m_updateFlags |= COV_SET_REMOTE_SYNC_TRANSFORM_INTERVAL;
6162 command->m_configureOpenGLVisualizerArguments.m_remoteSyncTransformInterval = remoteSyncTransformInterval;
6163 }
6164 }
6165
6166
b3ConfigureOpenGLVisualizerSetViewMatrix(b3SharedMemoryCommandHandle commandHandle,float cameraDistance,float cameraPitch,float cameraYaw,const float cameraTargetPosition[3])6167 B3_SHARED_API void b3ConfigureOpenGLVisualizerSetViewMatrix(b3SharedMemoryCommandHandle commandHandle, float cameraDistance, float cameraPitch, float cameraYaw, const float cameraTargetPosition[3])
6168 {
6169 struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
6170 b3Assert(command);
6171 b3Assert(command->m_type == CMD_CONFIGURE_OPENGL_VISUALIZER);
6172
6173 if (command->m_type == CMD_CONFIGURE_OPENGL_VISUALIZER)
6174 {
6175 command->m_updateFlags |= COV_SET_CAMERA_VIEW_MATRIX;
6176 command->m_configureOpenGLVisualizerArguments.m_cameraDistance = cameraDistance;
6177 command->m_configureOpenGLVisualizerArguments.m_cameraPitch = cameraPitch;
6178 command->m_configureOpenGLVisualizerArguments.m_cameraYaw = cameraYaw;
6179 command->m_configureOpenGLVisualizerArguments.m_cameraTargetPosition[0] = cameraTargetPosition[0];
6180 command->m_configureOpenGLVisualizerArguments.m_cameraTargetPosition[1] = cameraTargetPosition[1];
6181 command->m_configureOpenGLVisualizerArguments.m_cameraTargetPosition[2] = cameraTargetPosition[2];
6182 }
6183 }
6184
b3InitRequestOpenGLVisualizerCameraCommand(b3PhysicsClientHandle physClient)6185 B3_SHARED_API b3SharedMemoryCommandHandle b3InitRequestOpenGLVisualizerCameraCommand(b3PhysicsClientHandle physClient)
6186 {
6187 PhysicsClient* cl = (PhysicsClient*)physClient;
6188 b3Assert(cl);
6189 b3Assert(cl->canSubmitCommand());
6190 struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
6191 b3Assert(command);
6192
6193 command->m_type = CMD_REQUEST_OPENGL_VISUALIZER_CAMERA;
6194 command->m_updateFlags = 0;
6195
6196 return (b3SharedMemoryCommandHandle)command;
6197 }
6198
b3GetStatusOpenGLVisualizerCamera(b3SharedMemoryStatusHandle statusHandle,b3OpenGLVisualizerCameraInfo * camera)6199 B3_SHARED_API int b3GetStatusOpenGLVisualizerCamera(b3SharedMemoryStatusHandle statusHandle, b3OpenGLVisualizerCameraInfo* camera)
6200 {
6201 const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
6202 //b3Assert(status);
6203 if (status && status->m_type == CMD_REQUEST_OPENGL_VISUALIZER_CAMERA_COMPLETED)
6204 {
6205 *camera = status->m_visualizerCameraResultArgs;
6206 return 1;
6207 }
6208 return 0;
6209 }
6210
b3SetTimeOut(b3PhysicsClientHandle physClient,double timeOutInSeconds)6211 B3_SHARED_API void b3SetTimeOut(b3PhysicsClientHandle physClient, double timeOutInSeconds)
6212 {
6213 PhysicsClient* cl = (PhysicsClient*)physClient;
6214 b3Assert(cl);
6215 if (cl)
6216 {
6217 cl->setTimeOut(timeOutInSeconds);
6218 }
6219 }
6220
b3GetTimeOut(b3PhysicsClientHandle physClient)6221 B3_SHARED_API double b3GetTimeOut(b3PhysicsClientHandle physClient)
6222 {
6223 PhysicsClient* cl = (PhysicsClient*)physClient;
6224 b3Assert(cl);
6225 if (cl)
6226 {
6227 return cl->getTimeOut();
6228 }
6229 return -1;
6230 }
6231
b3SetAdditionalSearchPath(b3PhysicsClientHandle physClient,const char * path)6232 B3_SHARED_API b3SharedMemoryCommandHandle b3SetAdditionalSearchPath(b3PhysicsClientHandle physClient, const char* path)
6233 {
6234 PhysicsClient* cl = (PhysicsClient*)physClient;
6235 b3Assert(cl);
6236 b3Assert(cl->canSubmitCommand());
6237 struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
6238 b3Assert(command);
6239 command->m_type = CMD_SET_ADDITIONAL_SEARCH_PATH;
6240 command->m_updateFlags = 0;
6241 int len = strlen(path);
6242 if (len < MAX_FILENAME_LENGTH)
6243 {
6244 strcpy(command->m_searchPathArgs.m_path, path);
6245 }
6246
6247 return (b3SharedMemoryCommandHandle)command;
6248 }
6249
b3MultiplyTransforms(const double posA[3],const double ornA[4],const double posB[3],const double ornB[4],double outPos[3],double outOrn[4])6250 B3_SHARED_API void b3MultiplyTransforms(const double posA[3], const double ornA[4], const double posB[3], const double ornB[4], double outPos[3], double outOrn[4])
6251 {
6252 b3Transform trA;
6253 b3Transform trB;
6254 trA.setOrigin(b3MakeVector3(posA[0], posA[1], posA[2]));
6255 trA.setRotation(b3Quaternion(ornA[0], ornA[1], ornA[2], ornA[3]));
6256 trB.setOrigin(b3MakeVector3(posB[0], posB[1], posB[2]));
6257 trB.setRotation(b3Quaternion(ornB[0], ornB[1], ornB[2], ornB[3]));
6258 b3Transform res = trA * trB;
6259 outPos[0] = res.getOrigin()[0];
6260 outPos[1] = res.getOrigin()[1];
6261 outPos[2] = res.getOrigin()[2];
6262 b3Quaternion orn = res.getRotation();
6263 outOrn[0] = orn[0];
6264 outOrn[1] = orn[1];
6265 outOrn[2] = orn[2];
6266 outOrn[3] = orn[3];
6267 }
6268
b3InvertTransform(const double pos[3],const double orn[4],double outPos[3],double outOrn[4])6269 B3_SHARED_API void b3InvertTransform(const double pos[3], const double orn[4], double outPos[3], double outOrn[4])
6270 {
6271 b3Transform tr;
6272 tr.setOrigin(b3MakeVector3(pos[0], pos[1], pos[2]));
6273 tr.setRotation(b3Quaternion(orn[0], orn[1], orn[2], orn[3]));
6274 b3Transform trInv = tr.inverse();
6275 outPos[0] = trInv.getOrigin()[0];
6276 outPos[1] = trInv.getOrigin()[1];
6277 outPos[2] = trInv.getOrigin()[2];
6278 b3Quaternion invOrn = trInv.getRotation();
6279 outOrn[0] = invOrn[0];
6280 outOrn[1] = invOrn[1];
6281 outOrn[2] = invOrn[2];
6282 outOrn[3] = invOrn[3];
6283 }
6284
b3QuaternionSlerp(const double startQuat[],const double endQuat[],double interpolationFraction,double outOrn[])6285 B3_SHARED_API void b3QuaternionSlerp(const double startQuat[/*4*/], const double endQuat[/*4*/], double interpolationFraction, double outOrn[/*4*/])
6286 {
6287 b3Quaternion start(startQuat[0], startQuat[1], startQuat[2], startQuat[3]);
6288 b3Quaternion end(endQuat[0], endQuat[1], endQuat[2], endQuat[3]);
6289 b3Quaternion result = start.slerp(end, interpolationFraction);
6290 outOrn[0] = result[0];
6291 outOrn[1] = result[1];
6292 outOrn[2] = result[2];
6293 outOrn[3] = result[3];
6294 }
6295
b3RotateVector(const double quat[],const double vec[],double vecOut[])6296 B3_SHARED_API void b3RotateVector(const double quat[/*4*/], const double vec[/*3*/], double vecOut[/*3*/])
6297 {
6298 b3Quaternion q(quat[0], quat[1], quat[2], quat[3]);
6299 b3Vector3 v = b3MakeVector3(vec[0], vec[1], vec[2]);
6300 b3Vector3 vout = b3QuatRotate(q, v);
6301 vecOut[0] = vout[0];
6302 vecOut[1] = vout[1];
6303 vecOut[2] = vout[2];
6304 }
6305
b3CalculateVelocityQuaternion(const double startQuat[],const double endQuat[],double deltaTime,double angVelOut[])6306 B3_SHARED_API void b3CalculateVelocityQuaternion(const double startQuat[/*4*/], const double endQuat[/*4*/], double deltaTime, double angVelOut[/*3*/])
6307 {
6308 b3Quaternion start(startQuat[0], startQuat[1], startQuat[2], startQuat[3]);
6309 b3Quaternion end(endQuat[0], endQuat[1], endQuat[2], endQuat[3]);
6310 b3Vector3 pos=b3MakeVector3(0, 0, 0);
6311 b3Vector3 linVel, angVel;
6312 b3TransformUtil::calculateVelocityQuaternion(pos, pos, start, end, deltaTime, linVel, angVel);
6313 angVelOut[0] = angVel[0];
6314 angVelOut[1] = angVel[1];
6315 angVelOut[2] = angVel[2];
6316 }
6317
b3GetQuaternionFromAxisAngle(const double axis[],double angle,double outQuat[])6318 B3_SHARED_API void b3GetQuaternionFromAxisAngle(const double axis[/*3*/], double angle, double outQuat[/*4*/])
6319 {
6320 b3Quaternion quat(b3MakeVector3(axis[0], axis[1], axis[2]), angle);
6321 outQuat[0] = quat[0];
6322 outQuat[1] = quat[1];
6323 outQuat[2] = quat[2];
6324 outQuat[3] = quat[3];
6325 }
b3GetAxisAngleFromQuaternion(const double quat[],double axis[],double * angle)6326 B3_SHARED_API void b3GetAxisAngleFromQuaternion(const double quat[/*4*/], double axis[/*3*/], double* angle)
6327 {
6328 b3Quaternion q(quat[0], quat[1], quat[2], quat[3]);
6329 b3Vector3 ax = q.getAxis();
6330 axis[0] = ax[0];
6331 axis[1] = ax[1];
6332 axis[2] = ax[2];
6333 *angle = q.getAngle();
6334 }
6335
b3GetQuaternionDifference(const double startQuat[],const double endQuat[],double outOrn[])6336 B3_SHARED_API void b3GetQuaternionDifference(const double startQuat[/*4*/], const double endQuat[/*4*/], double outOrn[/*4*/])
6337 {
6338 b3Quaternion orn0(startQuat[0], startQuat[1], startQuat[2], startQuat[3]);
6339 b3Quaternion orn1a(endQuat[0], endQuat[1], endQuat[2], endQuat[3]);
6340 b3Quaternion orn1 = orn0.nearest(orn1a);
6341 b3Quaternion dorn = orn1 * orn0.inverse();
6342 outOrn[0] = dorn[0];
6343 outOrn[1] = dorn[1];
6344 outOrn[2] = dorn[2];
6345 outOrn[3] = dorn[3];
6346 }
6347
b3GetMatrixElem(const b3Matrix3x3 & mat,int index)6348 b3Scalar b3GetMatrixElem(const b3Matrix3x3& mat, int index)
6349 {
6350 int i = index % 3;
6351 int j = index / 3;
6352 return mat[i][j];
6353 }
6354
6355
MyMatrixToEulerXYZ(const b3Matrix3x3 & mat,b3Vector3 & xyz)6356 static bool MyMatrixToEulerXYZ(const b3Matrix3x3& mat, b3Vector3& xyz)
6357 {
6358 // rot = cy*cz -cy*sz sy
6359 // cz*sx*sy+cx*sz cx*cz-sx*sy*sz -cy*sx
6360 // -cx*cz*sy+sx*sz cz*sx+cx*sy*sz cx*cy
6361
6362 b3Scalar fi = b3GetMatrixElem(mat, 2);
6363 if (fi < b3Scalar(1.0f))
6364 {
6365 if (fi > b3Scalar(-1.0f))
6366 {
6367 xyz[0] = b3Atan2(-b3GetMatrixElem(mat, 5), b3GetMatrixElem(mat, 8));
6368 xyz[1] = b3Asin(b3GetMatrixElem(mat, 2));
6369 xyz[2] = b3Atan2(-b3GetMatrixElem(mat, 1), b3GetMatrixElem(mat, 0));
6370 return true;
6371 }
6372 else
6373 {
6374 // WARNING. Not unique. XA - ZA = -atan2(r10,r11)
6375 xyz[0] = -b3Atan2(b3GetMatrixElem(mat, 3), b3GetMatrixElem(mat, 4));
6376 xyz[1] = -B3_HALF_PI;
6377 xyz[2] = b3Scalar(0.0);
6378 return false;
6379 }
6380 }
6381 else
6382 {
6383 // WARNING. Not unique. XAngle + ZAngle = atan2(r10,r11)
6384 xyz[0] = b3Atan2(b3GetMatrixElem(mat, 3), b3GetMatrixElem(mat, 4));
6385 xyz[1] = B3_HALF_PI;
6386 xyz[2] = 0.0;
6387 }
6388 return false;
6389 }
6390
6391
b3GetAxisDifferenceQuaternion(const double startQuat[],const double endQuat[],double axisOut[])6392 B3_SHARED_API void b3GetAxisDifferenceQuaternion(const double startQuat[/*4*/], const double endQuat[/*4*/], double axisOut[/*3*/])
6393 {
6394 b3Quaternion currentQuat(startQuat[0], startQuat[1], startQuat[2], startQuat[3]);
6395 b3Quaternion desiredQuat(endQuat[0], endQuat[1], endQuat[2], endQuat[3]);
6396
6397 b3Quaternion relRot = currentQuat.inverse() * desiredQuat;
6398 b3Vector3 angleDiff;
6399 MyMatrixToEulerXYZ(b3Matrix3x3(relRot), angleDiff);
6400 axisOut[0] = angleDiff[0];
6401 axisOut[1] = angleDiff[1];
6402 axisOut[2] = angleDiff[2];
6403
6404 }
6405
6406 #ifdef BT_ENABLE_VHACD
6407 #include "VHACD.h"
6408 #include <string>
6409
6410 int main_vhacd_ext(const std::string& fileNameIn, const std::string& fileNameOut, const std::string& fileNameLog, VHACD::IVHACD::Parameters& paramsVHACD);
6411
b3VHACD(const char * fileNameInput,const char * fileNameOutput,const char * fileNameLogging,double concavity,double alpha,double beta,double gamma,double minVolumePerCH,int resolution,int maxNumVerticesPerCH,int depth,int planeDownsampling,int convexhullDownsampling,int pca,int mode,int convexhullApproximation)6412 B3_SHARED_API void b3VHACD(const char* fileNameInput, const char* fileNameOutput, const char* fileNameLogging,
6413 double concavity, double alpha, double beta, double gamma, double minVolumePerCH, int resolution,
6414 int maxNumVerticesPerCH, int depth, int planeDownsampling, int convexhullDownsampling,
6415 int pca, int mode, int convexhullApproximation)
6416 {
6417 std::string fileNameIn(fileNameInput);
6418 std::string fileNameOut(fileNameOutput);
6419 std::string fileNameLog(fileNameLogging);
6420 VHACD::IVHACD::Parameters params;
6421 if (concavity >= 0)
6422 params.m_concavity = concavity;
6423 if (alpha >= 0)
6424 params.m_alpha = alpha;
6425 if (beta >= 0)
6426 params.m_beta = beta;
6427 if (gamma >= 0)
6428 params.m_gamma = gamma;
6429 if (minVolumePerCH >= 0)
6430 params.m_minVolumePerCH = minVolumePerCH;
6431 if (resolution >= 0)
6432 params.m_resolution = (unsigned int)resolution;
6433 if (maxNumVerticesPerCH >= 0)
6434 params.m_maxNumVerticesPerCH = (unsigned int)maxNumVerticesPerCH;
6435 if (depth >= 0)
6436 params.m_depth = depth;
6437 if (planeDownsampling >= 0)
6438 params.m_planeDownsampling = planeDownsampling;
6439 if (convexhullDownsampling >= 0)
6440 params.m_convexhullDownsampling = convexhullDownsampling;
6441 if (pca >= 0)
6442 params.m_pca = pca;
6443 if (mode >= 0)
6444 params.m_mode = mode;
6445 if (convexhullApproximation >= 0)
6446 params.m_convexhullApproximation = convexhullApproximation;
6447 main_vhacd_ext(fileNameIn, fileNameOut, fileNameLog, params);
6448 }
6449 #endif
6450
6451
6452