Merge pull request #3096 from erwincoumans/master

allow to set changeDynamics(body, link, contactProcessingThreshold) f…
bulletphysics · Oct 7, 2020 · afa73c2 · afa73c2
2 parents afa4fb5 + ad09940
commit afa73c2
Show file tree

Hide file tree

Showing 18 changed files with 294 additions and 14 deletions.
diff --git a/build_visual_studio_vr_pybullet_double.bat b/build_visual_studio_vr_pybullet_double.bat
@@ -18,7 +18,7 @@ rem SET myvar=c:\python-3.5.2
 cd build3
 
 
-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 
+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 
 
 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 
 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 

diff --git a/examples/SharedMemory/PhysicsClient.h b/examples/SharedMemory/PhysicsClient.h
@@ -77,6 +77,8 @@ class PhysicsClient
 
 	virtual void getCachedMassMatrix(int dofCountCheck, double* massMatrix) = 0;
 
+	virtual bool getCachedReturnData(struct b3UserDataValue* returnData) = 0;
+
 	virtual void setTimeOut(double timeOutInSeconds) = 0;
 	virtual double getTimeOut() const = 0;
 

diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp
@@ -3024,6 +3024,16 @@ B3_SHARED_API int b3GetStatusPluginCommandResult(b3SharedMemoryStatusHandle stat
 	return statusUniqueId;
 }
 
+B3_SHARED_API int b3GetStatusPluginCommandReturnData(b3PhysicsClientHandle physClient, struct b3UserDataValue* valueOut)
+{
+	PhysicsClient* cl = (PhysicsClient*)physClient;
+	if (cl)
+	{
+		return cl->getCachedReturnData(valueOut);
+	}
+	return false;
+}
+
 B3_SHARED_API int b3GetStatusPluginUniqueId(b3SharedMemoryStatusHandle statusHandle)
 {
 	int statusUniqueId = -1;
@@ -3059,7 +3069,7 @@ B3_SHARED_API void b3CustomCommandExecutePluginCommand(b3SharedMemoryCommandHand
 	{
 		command->m_updateFlags |= CMD_CUSTOM_COMMAND_EXECUTE_PLUGIN_COMMAND;
 		command->m_customCommandArgs.m_pluginUniqueId = pluginUniqueId;
-
+		command->m_customCommandArgs.m_startingReturnBytes = 0;
 		command->m_customCommandArgs.m_arguments.m_numInts = 0;
 		command->m_customCommandArgs.m_arguments.m_numFloats = 0;
 		command->m_customCommandArgs.m_arguments.m_text[0] = 0;

diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h
@@ -66,7 +66,8 @@ extern "C"
 	B3_SHARED_API void b3CustomCommandLoadPluginSetPostFix(b3SharedMemoryCommandHandle commandHandle, const char* postFix);
 	B3_SHARED_API int b3GetStatusPluginUniqueId(b3SharedMemoryStatusHandle statusHandle);
 	B3_SHARED_API int b3GetStatusPluginCommandResult(b3SharedMemoryStatusHandle statusHandle);
-
+	B3_SHARED_API int b3GetStatusPluginCommandReturnData(b3PhysicsClientHandle physClient, struct b3UserDataValue* valueOut);
+
 	B3_SHARED_API void b3CustomCommandUnloadPlugin(b3SharedMemoryCommandHandle commandHandle, int pluginUniqueId);
 	B3_SHARED_API void b3CustomCommandExecutePluginCommand(b3SharedMemoryCommandHandle commandHandle, int pluginUniqueId, const char* textArguments);
 	B3_SHARED_API void b3CustomCommandExecuteAddIntArgument(b3SharedMemoryCommandHandle commandHandle, int intVal);

diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp
@@ -69,6 +69,9 @@ struct PhysicsClientSharedMemoryInternalData
 	btHashMap<btHashInt, SharedMemoryUserData> m_userDataMap;
 	btHashMap<SharedMemoryUserDataHashKey, int> m_userDataHandleLookup;
 
+	btAlignedObjectArray<char> m_cachedReturnData;
+	b3UserDataValue m_cachedReturnDataValue;
+
 	SharedMemoryStatus m_tempBackupServerStatus;
 
 	SharedMemoryStatus m_lastServerStatus;
@@ -1350,8 +1353,22 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus()
 				b3Warning("Request getCollisionInfo failed");
 				break;
 			}
+
 			case CMD_CUSTOM_COMMAND_COMPLETED:
 			{
+
+				m_data->m_cachedReturnData.resize(serverCmd.m_customCommandResultArgs.m_returnDataSizeInBytes);
+				m_data->m_cachedReturnDataValue.m_length = serverCmd.m_customCommandResultArgs.m_returnDataSizeInBytes;
+
+				if (serverCmd.m_customCommandResultArgs.m_returnDataSizeInBytes)
+				{
+					m_data->m_cachedReturnDataValue.m_type = serverCmd.m_customCommandResultArgs.m_returnDataType;
+					m_data->m_cachedReturnDataValue.m_data1 = &m_data->m_cachedReturnData[0];
+					for (int i = 0; i < serverCmd.m_numDataStreamBytes; i++)
+					{
+						m_data->m_cachedReturnData[i + serverCmd.m_customCommandResultArgs.m_returnDataStart] = m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor[i];
+					}
+				}
 				break;
 			}
 			case CMD_CALCULATED_JACOBIAN_COMPLETED:
@@ -1664,6 +1681,7 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus()
 			m_data->m_lastServerStatus = m_data->m_tempBackupServerStatus;
 		}
 
+
 		if (serverCmd.m_type == CMD_REMOVE_USER_DATA_COMPLETED)
 		{
 			B3_PROFILE("CMD_REMOVE_USER_DATA_COMPLETED");
@@ -1851,6 +1869,23 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus()
 			return 0;
 		}
 
+		if (serverCmd.m_type == CMD_CUSTOM_COMMAND_COMPLETED)
+		{
+			int totalReceived = (serverCmd.m_numDataStreamBytes + serverCmd.m_customCommandResultArgs.m_returnDataStart);
+			int remaining = serverCmd.m_customCommandResultArgs.m_returnDataSizeInBytes - totalReceived;
+
+			if (remaining > 0)
+			{
+				// continue requesting return data
+				SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0];
+				command.m_type = CMD_CUSTOM_COMMAND;
+				command.m_customCommandArgs.m_startingReturnBytes =
+					totalReceived;
+				submitClientCommand(command);
+				return 0;
+			}
+		}
+
 		return &m_data->m_lastServerStatus;
 	}
 	else
@@ -2006,6 +2041,16 @@ void PhysicsClientSharedMemory::getCachedMassMatrix(int dofCountCheck, double* m
 	}
 }
 
+bool PhysicsClientSharedMemory::getCachedReturnData(b3UserDataValue* returnData)
+{
+	if (m_data->m_cachedReturnDataValue.m_length)
+	{
+		*returnData = m_data->m_cachedReturnDataValue;
+		return true;
+	}
+	return false;
+
+}
 void PhysicsClientSharedMemory::getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo)
 {
 	visualShapesInfo->m_numVisualShapes = m_data->m_cachedVisualShapes.size();

diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.h b/examples/SharedMemory/PhysicsClientSharedMemory.h
@@ -90,6 +90,8 @@ class PhysicsClientSharedMemory : public PhysicsClient
 
 	virtual void getCachedMassMatrix(int dofCountCheck, double* massMatrix);
 
+	virtual bool getCachedReturnData(b3UserDataValue* returnData);
+
 	virtual void setTimeOut(double timeOutInSeconds);
 	virtual double getTimeOut() const;
 

diff --git a/examples/SharedMemory/PhysicsDirect.cpp b/examples/SharedMemory/PhysicsDirect.cpp
@@ -79,6 +79,10 @@ struct PhysicsDirectInternalData
 	btHashMap<btHashInt, SharedMemoryUserData> m_userDataMap;
 	btHashMap<SharedMemoryUserDataHashKey, int> m_userDataHandleLookup;
 
+	btAlignedObjectArray<char> m_cachedReturnData;
+	b3UserDataValue m_cachedReturnDataValue;
+
+
 	PhysicsCommandProcessorInterface* m_commandProcessor;
 	bool m_ownsCommandProcessor;
 	double m_timeOutInSeconds;
@@ -1113,10 +1117,7 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd
 			b3Warning("Request mesh data failed");
 			break;
 		}
-		case CMD_CUSTOM_COMMAND_COMPLETED:
-		{
-			break;
-		}
+
 		case CMD_CUSTOM_COMMAND_FAILED:
 		{
 			b3Warning("custom plugin command failed");
@@ -1309,14 +1310,95 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd
 		{
 			break;
 		}
+		case CMD_CUSTOM_COMMAND_COMPLETED:
+		{
+			break;
+		}
 		default:
 		{
 			//b3Warning("Unknown server status type");
 		}
+
 	};
 }
+
+
+bool PhysicsDirect::processCustomCommand(const struct SharedMemoryCommand& orgCommand)
+{
+	SharedMemoryCommand command = orgCommand;
+
+	const SharedMemoryStatus& serverCmd = m_data->m_serverStatus;
+
+	int remaining = 0;
+	do
+	{
+		bool hasStatus = m_data->m_commandProcessor->processCommand(command, m_data->m_serverStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
+
+		b3Clock clock;
+		double startTime = clock.getTimeInSeconds();
+		double timeOutInSeconds = m_data->m_timeOutInSeconds;
+
+		while ((!hasStatus) && (clock.getTimeInSeconds() - startTime < timeOutInSeconds))
+		{
+			const SharedMemoryStatus* stat = processServerStatus();
+			if (stat)
+			{
+				hasStatus = true;
+			}
+		}
+
+		m_data->m_hasStatus = hasStatus;
+
+		if (hasStatus)
+		{
+
+			if (m_data->m_verboseOutput)
+			{
+				b3Printf("Success receiving %d return data\n",
+					serverCmd.m_numDataStreamBytes);
+			}
+
+
+			btAssert(m_data->m_serverStatus.m_type == CMD_CUSTOM_COMMAND_COMPLETED);
+
+			if (m_data->m_serverStatus.m_type == CMD_CUSTOM_COMMAND_COMPLETED)
+			{
+				m_data->m_cachedReturnData.resize(serverCmd.m_customCommandResultArgs.m_returnDataSizeInBytes);
+				m_data->m_cachedReturnDataValue.m_length = serverCmd.m_customCommandResultArgs.m_returnDataSizeInBytes;
+
+				if (serverCmd.m_customCommandResultArgs.m_returnDataSizeInBytes)
+				{
+					m_data->m_cachedReturnDataValue.m_type = serverCmd.m_customCommandResultArgs.m_returnDataType;
+					m_data->m_cachedReturnDataValue.m_data1 = &m_data->m_cachedReturnData[0];
+					for (int i = 0; i < serverCmd.m_numDataStreamBytes; i++)
+					{
+						m_data->m_cachedReturnData[i+ serverCmd.m_customCommandResultArgs.m_returnDataStart] = m_data->m_bulletStreamDataServerToClient[i];
+					}
+				}
+				int totalReceived = serverCmd.m_numDataStreamBytes + serverCmd.m_customCommandResultArgs.m_returnDataStart;
+				remaining = serverCmd.m_customCommandResultArgs.m_returnDataSizeInBytes - totalReceived;
+
+				if (remaining > 0)
+				{
+					m_data->m_hasStatus = false;
+					command.m_type = CMD_CUSTOM_COMMAND;
+					command.m_customCommandArgs.m_startingReturnBytes =
+						totalReceived;
+				}
+			}
+		}
+
+	} while (remaining > 0);
+
+	return m_data->m_hasStatus;
+}
+
 bool PhysicsDirect::submitClientCommand(const struct SharedMemoryCommand& command)
 {
+	if (command.m_type == CMD_CUSTOM_COMMAND)
+	{
+		return processCustomCommand(command);
+	}
 	if (command.m_type == CMD_REQUEST_DEBUG_LINES)
 	{
 		return processDebugLines(command);
@@ -1651,6 +1733,16 @@ void PhysicsDirect::getCachedMassMatrix(int dofCountCheck, double* massMatrix)
 	}
 }
 
+bool PhysicsDirect::getCachedReturnData(b3UserDataValue* returnData)
+{
+	if (m_data->m_cachedReturnDataValue.m_length)
+	{
+		*returnData = m_data->m_cachedReturnDataValue;
+		return true;
+	}
+	return false;
+}
+
 void PhysicsDirect::setTimeOut(double timeOutInSeconds)
 {
 	m_data->m_timeOutInSeconds = timeOutInSeconds;

diff --git a/examples/SharedMemory/PhysicsDirect.h b/examples/SharedMemory/PhysicsDirect.h
@@ -22,12 +22,14 @@ class PhysicsDirect : public PhysicsClient
 
 	bool processVisualShapeData(const struct SharedMemoryCommand& orgCommand);
 
-  bool processMeshData(const struct SharedMemoryCommand& orgCommand);
+	bool processMeshData(const struct SharedMemoryCommand& orgCommand);
 
 	void processBodyJointInfo(int bodyUniqueId, const struct SharedMemoryStatus& serverCmd);
 
 	void processAddUserData(const struct SharedMemoryStatus& serverCmd);
 
+	bool processCustomCommand(const struct SharedMemoryCommand& orgCommand);
+
 	void postProcessStatus(const struct SharedMemoryStatus& serverCmd);
 
 	void resetData();
@@ -112,6 +114,8 @@ class PhysicsDirect : public PhysicsClient
 
 	virtual void getCachedMassMatrix(int dofCountCheck, double* massMatrix);
 
+	virtual bool getCachedReturnData(b3UserDataValue* returnData);
+
 	//the following APIs are for internal use for visualization:
 	virtual bool connect(struct GUIHelperInterface* guiHelper);
 	virtual void renderScene();

diff --git a/examples/SharedMemory/PhysicsLoopBack.cpp b/examples/SharedMemory/PhysicsLoopBack.cpp
@@ -227,6 +227,10 @@ void PhysicsLoopBack::getCachedMassMatrix(int dofCountCheck, double* massMatrix)
 {
 	m_data->m_physicsClient->getCachedMassMatrix(dofCountCheck, massMatrix);
 }
+bool PhysicsLoopBack::getCachedReturnData(struct b3UserDataValue* returnData)
+{
+	return m_data->m_physicsClient->getCachedReturnData(returnData);
+}
 
 void PhysicsLoopBack::setTimeOut(double timeOutInSeconds)
 {

diff --git a/examples/SharedMemory/PhysicsLoopBack.h b/examples/SharedMemory/PhysicsLoopBack.h
@@ -88,6 +88,8 @@ class PhysicsLoopBack : public PhysicsClient
 
 	virtual void getCachedMassMatrix(int dofCountCheck, double* massMatrix);
 
+	virtual bool getCachedReturnData(struct b3UserDataValue* returnData);
+
 	virtual void setTimeOut(double timeOutInSeconds);
 	virtual double getTimeOut() const;