Skip to content

Commit afa73c2

Browse files
author
erwincoumans
authored
Merge pull request #3096 from erwincoumans/master
allow to set changeDynamics(body, link, contactProcessingThreshold) f…
2 parents afa4fb5 + ad09940 commit afa73c2

18 files changed

+294
-14
lines changed

‎build_visual_studio_vr_pybullet_double.bat

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -18,7 +18,7 @@ rem SET myvar=c:\python-3.5.2
1818
cd build3
1919

2020

21-
premake4 --double --standalone-examples --enable_stable_pd --enable_multithreading --midi --enable_static_vr_plugin --enable_openvr --enable_pybullet --python_include_dir="%myvar%/include" --python_lib_dir="%myvar%/libs" --targetdir="../bin" vs2010
21+
premake4 --double --enable_stable_pd --enable_multithreading --midi --enable_static_vr_plugin --enable_openvr --enable_pybullet --python_include_dir="%myvar%/include" --python_lib_dir="%myvar%/libs" --targetdir="../bin" vs2010
2222

2323
rem premake4 --double --enable_multithreading --midi --enable_static_vr_plugin --enable_openvr --enable_pybullet --python_include_dir="%myvar%/include" --python_lib_dir="%myvar%/libs" --targetdir="../binserver" vs2010
2424
rem premake4 --double --enable_grpc --enable_multithreading --midi --enable_static_vr_plugin --enable_openvr --enable_pybullet --python_include_dir="%myvar%/include" --python_lib_dir="%myvar%/libs" --targetdir="../binserver" vs2010

‎examples/SharedMemory/PhysicsClient.h

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -77,6 +77,8 @@ class PhysicsClient
7777

7878
virtual void getCachedMassMatrix(int dofCountCheck, double* massMatrix) = 0;
7979

80+
virtual bool getCachedReturnData(struct b3UserDataValue* returnData) = 0;
81+
8082
virtual void setTimeOut(double timeOutInSeconds) = 0;
8183
virtual double getTimeOut() const = 0;
8284

‎examples/SharedMemory/PhysicsClientC_API.cpp

Lines changed: 11 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -3024,6 +3024,16 @@ B3_SHARED_API int b3GetStatusPluginCommandResult(b3SharedMemoryStatusHandle stat
30243024
return statusUniqueId;
30253025
}
30263026

3027+
B3_SHARED_API int b3GetStatusPluginCommandReturnData(b3PhysicsClientHandle physClient, struct b3UserDataValue* valueOut)
3028+
{
3029+
PhysicsClient* cl = (PhysicsClient*)physClient;
3030+
if (cl)
3031+
{
3032+
return cl->getCachedReturnData(valueOut);
3033+
}
3034+
return false;
3035+
}
3036+
30273037
B3_SHARED_API int b3GetStatusPluginUniqueId(b3SharedMemoryStatusHandle statusHandle)
30283038
{
30293039
int statusUniqueId = -1;
@@ -3059,7 +3069,7 @@ B3_SHARED_API void b3CustomCommandExecutePluginCommand(b3SharedMemoryCommandHand
30593069
{
30603070
command->m_updateFlags |= CMD_CUSTOM_COMMAND_EXECUTE_PLUGIN_COMMAND;
30613071
command->m_customCommandArgs.m_pluginUniqueId = pluginUniqueId;
3062-
3072+
command->m_customCommandArgs.m_startingReturnBytes = 0;
30633073
command->m_customCommandArgs.m_arguments.m_numInts = 0;
30643074
command->m_customCommandArgs.m_arguments.m_numFloats = 0;
30653075
command->m_customCommandArgs.m_arguments.m_text[0] = 0;

‎examples/SharedMemory/PhysicsClientC_API.h

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -66,7 +66,8 @@ extern "C"
6666
B3_SHARED_API void b3CustomCommandLoadPluginSetPostFix(b3SharedMemoryCommandHandle commandHandle, const char* postFix);
6767
B3_SHARED_API int b3GetStatusPluginUniqueId(b3SharedMemoryStatusHandle statusHandle);
6868
B3_SHARED_API int b3GetStatusPluginCommandResult(b3SharedMemoryStatusHandle statusHandle);
69-
69+
B3_SHARED_API int b3GetStatusPluginCommandReturnData(b3PhysicsClientHandle physClient, struct b3UserDataValue* valueOut);
70+
7071
B3_SHARED_API void b3CustomCommandUnloadPlugin(b3SharedMemoryCommandHandle commandHandle, int pluginUniqueId);
7172
B3_SHARED_API void b3CustomCommandExecutePluginCommand(b3SharedMemoryCommandHandle commandHandle, int pluginUniqueId, const char* textArguments);
7273
B3_SHARED_API void b3CustomCommandExecuteAddIntArgument(b3SharedMemoryCommandHandle commandHandle, int intVal);

‎examples/SharedMemory/PhysicsClientSharedMemory.cpp

Lines changed: 45 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -69,6 +69,9 @@ struct PhysicsClientSharedMemoryInternalData
6969
btHashMap<btHashInt, SharedMemoryUserData> m_userDataMap;
7070
btHashMap<SharedMemoryUserDataHashKey, int> m_userDataHandleLookup;
7171

72+
btAlignedObjectArray<char> m_cachedReturnData;
73+
b3UserDataValue m_cachedReturnDataValue;
74+
7275
SharedMemoryStatus m_tempBackupServerStatus;
7376

7477
SharedMemoryStatus m_lastServerStatus;
@@ -1350,8 +1353,22 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus()
13501353
b3Warning("Request getCollisionInfo failed");
13511354
break;
13521355
}
1356+
13531357
case CMD_CUSTOM_COMMAND_COMPLETED:
13541358
{
1359+
1360+
m_data->m_cachedReturnData.resize(serverCmd.m_customCommandResultArgs.m_returnDataSizeInBytes);
1361+
m_data->m_cachedReturnDataValue.m_length = serverCmd.m_customCommandResultArgs.m_returnDataSizeInBytes;
1362+
1363+
if (serverCmd.m_customCommandResultArgs.m_returnDataSizeInBytes)
1364+
{
1365+
m_data->m_cachedReturnDataValue.m_type = serverCmd.m_customCommandResultArgs.m_returnDataType;
1366+
m_data->m_cachedReturnDataValue.m_data1 = &m_data->m_cachedReturnData[0];
1367+
for (int i = 0; i < serverCmd.m_numDataStreamBytes; i++)
1368+
{
1369+
m_data->m_cachedReturnData[i + serverCmd.m_customCommandResultArgs.m_returnDataStart] = m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor[i];
1370+
}
1371+
}
13551372
break;
13561373
}
13571374
case CMD_CALCULATED_JACOBIAN_COMPLETED:
@@ -1664,6 +1681,7 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus()
16641681
m_data->m_lastServerStatus = m_data->m_tempBackupServerStatus;
16651682
}
16661683

1684+
16671685
if (serverCmd.m_type == CMD_REMOVE_USER_DATA_COMPLETED)
16681686
{
16691687
B3_PROFILE("CMD_REMOVE_USER_DATA_COMPLETED");
@@ -1851,6 +1869,23 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus()
18511869
return 0;
18521870
}
18531871

1872+
if (serverCmd.m_type == CMD_CUSTOM_COMMAND_COMPLETED)
1873+
{
1874+
int totalReceived = (serverCmd.m_numDataStreamBytes + serverCmd.m_customCommandResultArgs.m_returnDataStart);
1875+
int remaining = serverCmd.m_customCommandResultArgs.m_returnDataSizeInBytes - totalReceived;
1876+
1877+
if (remaining > 0)
1878+
{
1879+
// continue requesting return data
1880+
SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0];
1881+
command.m_type = CMD_CUSTOM_COMMAND;
1882+
command.m_customCommandArgs.m_startingReturnBytes =
1883+
totalReceived;
1884+
submitClientCommand(command);
1885+
return 0;
1886+
}
1887+
}
1888+
18541889
return &m_data->m_lastServerStatus;
18551890
}
18561891
else
@@ -2006,6 +2041,16 @@ void PhysicsClientSharedMemory::getCachedMassMatrix(int dofCountCheck, double* m
20062041
}
20072042
}
20082043

2044+
bool PhysicsClientSharedMemory::getCachedReturnData(b3UserDataValue* returnData)
2045+
{
2046+
if (m_data->m_cachedReturnDataValue.m_length)
2047+
{
2048+
*returnData = m_data->m_cachedReturnDataValue;
2049+
return true;
2050+
}
2051+
return false;
2052+
2053+
}
20092054
void PhysicsClientSharedMemory::getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo)
20102055
{
20112056
visualShapesInfo->m_numVisualShapes = m_data->m_cachedVisualShapes.size();

‎examples/SharedMemory/PhysicsClientSharedMemory.h

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -90,6 +90,8 @@ class PhysicsClientSharedMemory : public PhysicsClient
9090

9191
virtual void getCachedMassMatrix(int dofCountCheck, double* massMatrix);
9292

93+
virtual bool getCachedReturnData(b3UserDataValue* returnData);
94+
9395
virtual void setTimeOut(double timeOutInSeconds);
9496
virtual double getTimeOut() const;
9597

‎examples/SharedMemory/PhysicsDirect.cpp

Lines changed: 96 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -79,6 +79,10 @@ struct PhysicsDirectInternalData
7979
btHashMap<btHashInt, SharedMemoryUserData> m_userDataMap;
8080
btHashMap<SharedMemoryUserDataHashKey, int> m_userDataHandleLookup;
8181

82+
btAlignedObjectArray<char> m_cachedReturnData;
83+
b3UserDataValue m_cachedReturnDataValue;
84+
85+
8286
PhysicsCommandProcessorInterface* m_commandProcessor;
8387
bool m_ownsCommandProcessor;
8488
double m_timeOutInSeconds;
@@ -1113,10 +1117,7 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd
11131117
b3Warning("Request mesh data failed");
11141118
break;
11151119
}
1116-
case CMD_CUSTOM_COMMAND_COMPLETED:
1117-
{
1118-
break;
1119-
}
1120+
11201121
case CMD_CUSTOM_COMMAND_FAILED:
11211122
{
11221123
b3Warning("custom plugin command failed");
@@ -1309,14 +1310,95 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd
13091310
{
13101311
break;
13111312
}
1313+
case CMD_CUSTOM_COMMAND_COMPLETED:
1314+
{
1315+
break;
1316+
}
13121317
default:
13131318
{
13141319
//b3Warning("Unknown server status type");
13151320
}
1321+
13161322
};
13171323
}
1324+
1325+
1326+
bool PhysicsDirect::processCustomCommand(const struct SharedMemoryCommand& orgCommand)
1327+
{
1328+
SharedMemoryCommand command = orgCommand;
1329+
1330+
const SharedMemoryStatus& serverCmd = m_data->m_serverStatus;
1331+
1332+
int remaining = 0;
1333+
do
1334+
{
1335+
bool hasStatus = m_data->m_commandProcessor->processCommand(command, m_data->m_serverStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
1336+
1337+
b3Clock clock;
1338+
double startTime = clock.getTimeInSeconds();
1339+
double timeOutInSeconds = m_data->m_timeOutInSeconds;
1340+
1341+
while ((!hasStatus) && (clock.getTimeInSeconds() - startTime < timeOutInSeconds))
1342+
{
1343+
const SharedMemoryStatus* stat = processServerStatus();
1344+
if (stat)
1345+
{
1346+
hasStatus = true;
1347+
}
1348+
}
1349+
1350+
m_data->m_hasStatus = hasStatus;
1351+
1352+
if (hasStatus)
1353+
{
1354+
1355+
if (m_data->m_verboseOutput)
1356+
{
1357+
b3Printf("Success receiving %d return data\n",
1358+
serverCmd.m_numDataStreamBytes);
1359+
}
1360+
1361+
1362+
btAssert(m_data->m_serverStatus.m_type == CMD_CUSTOM_COMMAND_COMPLETED);
1363+
1364+
if (m_data->m_serverStatus.m_type == CMD_CUSTOM_COMMAND_COMPLETED)
1365+
{
1366+
m_data->m_cachedReturnData.resize(serverCmd.m_customCommandResultArgs.m_returnDataSizeInBytes);
1367+
m_data->m_cachedReturnDataValue.m_length = serverCmd.m_customCommandResultArgs.m_returnDataSizeInBytes;
1368+
1369+
if (serverCmd.m_customCommandResultArgs.m_returnDataSizeInBytes)
1370+
{
1371+
m_data->m_cachedReturnDataValue.m_type = serverCmd.m_customCommandResultArgs.m_returnDataType;
1372+
m_data->m_cachedReturnDataValue.m_data1 = &m_data->m_cachedReturnData[0];
1373+
for (int i = 0; i < serverCmd.m_numDataStreamBytes; i++)
1374+
{
1375+
m_data->m_cachedReturnData[i+ serverCmd.m_customCommandResultArgs.m_returnDataStart] = m_data->m_bulletStreamDataServerToClient[i];
1376+
}
1377+
}
1378+
int totalReceived = serverCmd.m_numDataStreamBytes + serverCmd.m_customCommandResultArgs.m_returnDataStart;
1379+
remaining = serverCmd.m_customCommandResultArgs.m_returnDataSizeInBytes - totalReceived;
1380+
1381+
if (remaining > 0)
1382+
{
1383+
m_data->m_hasStatus = false;
1384+
command.m_type = CMD_CUSTOM_COMMAND;
1385+
command.m_customCommandArgs.m_startingReturnBytes =
1386+
totalReceived;
1387+
}
1388+
}
1389+
}
1390+
1391+
} while (remaining > 0);
1392+
1393+
return m_data->m_hasStatus;
1394+
}
1395+
13181396
bool PhysicsDirect::submitClientCommand(const struct SharedMemoryCommand& command)
13191397
{
1398+
if (command.m_type == CMD_CUSTOM_COMMAND)
1399+
{
1400+
return processCustomCommand(command);
1401+
}
13201402
if (command.m_type == CMD_REQUEST_DEBUG_LINES)
13211403
{
13221404
return processDebugLines(command);
@@ -1651,6 +1733,16 @@ void PhysicsDirect::getCachedMassMatrix(int dofCountCheck, double* massMatrix)
16511733
}
16521734
}
16531735

1736+
bool PhysicsDirect::getCachedReturnData(b3UserDataValue* returnData)
1737+
{
1738+
if (m_data->m_cachedReturnDataValue.m_length)
1739+
{
1740+
*returnData = m_data->m_cachedReturnDataValue;
1741+
return true;
1742+
}
1743+
return false;
1744+
}
1745+
16541746
void PhysicsDirect::setTimeOut(double timeOutInSeconds)
16551747
{
16561748
m_data->m_timeOutInSeconds = timeOutInSeconds;

‎examples/SharedMemory/PhysicsDirect.h

Lines changed: 5 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -22,12 +22,14 @@ class PhysicsDirect : public PhysicsClient
2222

2323
bool processVisualShapeData(const struct SharedMemoryCommand& orgCommand);
2424

25-
bool processMeshData(const struct SharedMemoryCommand& orgCommand);
25+
bool processMeshData(const struct SharedMemoryCommand& orgCommand);
2626

2727
void processBodyJointInfo(int bodyUniqueId, const struct SharedMemoryStatus& serverCmd);
2828

2929
void processAddUserData(const struct SharedMemoryStatus& serverCmd);
3030

31+
bool processCustomCommand(const struct SharedMemoryCommand& orgCommand);
32+
3133
void postProcessStatus(const struct SharedMemoryStatus& serverCmd);
3234

3335
void resetData();
@@ -112,6 +114,8 @@ class PhysicsDirect : public PhysicsClient
112114

113115
virtual void getCachedMassMatrix(int dofCountCheck, double* massMatrix);
114116

117+
virtual bool getCachedReturnData(b3UserDataValue* returnData);
118+
115119
//the following APIs are for internal use for visualization:
116120
virtual bool connect(struct GUIHelperInterface* guiHelper);
117121
virtual void renderScene();

‎examples/SharedMemory/PhysicsLoopBack.cpp

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -227,6 +227,10 @@ void PhysicsLoopBack::getCachedMassMatrix(int dofCountCheck, double* massMatrix)
227227
{
228228
m_data->m_physicsClient->getCachedMassMatrix(dofCountCheck, massMatrix);
229229
}
230+
bool PhysicsLoopBack::getCachedReturnData(struct b3UserDataValue* returnData)
231+
{
232+
return m_data->m_physicsClient->getCachedReturnData(returnData);
233+
}
230234

231235
void PhysicsLoopBack::setTimeOut(double timeOutInSeconds)
232236
{

‎examples/SharedMemory/PhysicsLoopBack.h

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -88,6 +88,8 @@ class PhysicsLoopBack : public PhysicsClient
8888

8989
virtual void getCachedMassMatrix(int dofCountCheck, double* massMatrix);
9090

91+
virtual bool getCachedReturnData(struct b3UserDataValue* returnData);
92+
9193
virtual void setTimeOut(double timeOutInSeconds);
9294
virtual double getTimeOut() const;
9395

0 commit comments

Comments
 (0)