Notify plugins on certain events (e.g. body added).

This also adds a periodic tick function to plugins, giving them periodically control over the simulation thread, even when the simulation is paused.
This commit is contained in:
Tigran Gasparian 2018-07-24 14:12:16 +02:00
parent 0e1dce41ea
commit c59afb88c4
11 changed files with 252 additions and 28 deletions

View File

@ -44,6 +44,8 @@ public:
virtual void enableRealTimeSimulation(bool enableRealTimeSim)=0;
virtual bool isRealTimeSimulationEnabled() const=0;
virtual void tickPlugins() = 0;
virtual void enableCommandLogging(bool enable, const char* fileName)=0;
virtual void replayFromLogFile(const char* fileName)=0;
virtual void replayLogCommand(char* bufferServerToClient, int bufferSizeInBytes )=0;

View File

@ -1214,6 +1214,13 @@ bool PhysicsDirect::submitClientCommand(const struct SharedMemoryCommand& comman
bool hasStatus = m_data->m_commandProcessor->processCommand(command,m_data->m_serverStatus,&m_data->m_bulletStreamDataServerToClient[0],SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
m_data->m_hasStatus = hasStatus;
if (m_data->m_ownsCommandProcessor)
{
CommandProcessorInterface *commandProcessor = dynamic_cast<CommandProcessorInterface*>(m_data->m_commandProcessor);
btAssert(commandProcessor);
commandProcessor->tickPlugins();
}
/*if (hasStatus)
{
postProcessStatus(m_data->m_serverStatus);

View File

@ -1902,6 +1902,10 @@ void PhysicsServerCommandProcessor::processCollisionForces(btScalar timeStep)
#endif//B3_ENABLE_TINY_AUDIO
}
void PhysicsServerCommandProcessor::tickPlugins() {
m_data->m_pluginManager.tickPlugins();
}
void PhysicsServerCommandProcessor::tickPlugins(btScalar timeStep, bool isPreTick)
{
m_data->m_pluginManager.tickPlugins(timeStep, isPreTick);
@ -2855,6 +2859,11 @@ bool PhysicsServerCommandProcessor::processImportedObjects(const char* fileName,
}
}
}
b3Notification notification;
notification.m_notificationType = BODY_ADDED;
notification.m_bodyArgs.m_bodyUniqueId = bodyUniqueId;
m_data->m_pluginManager.addNotification(notification);
}
@ -5108,6 +5117,11 @@ bool PhysicsServerCommandProcessor::processAddUserDataCommand(const struct Share
serverStatusOut.m_userDataResponseArgs.m_valueType = clientCmd.m_addUserDataRequestArgs.m_valueType;
strcpy(serverStatusOut.m_userDataResponseArgs.m_key, clientCmd.m_addUserDataRequestArgs.m_key);
b3Notification notification;
notification.m_notificationType = USER_DATA_ADDED;
notification.m_userDataArgs.m_userDataId = userDataHandle;
m_data->m_pluginManager.addNotification(notification);
// Keep bufferServerToClient as-is.
return hasStatus;
}
@ -5134,6 +5148,12 @@ bool PhysicsServerCommandProcessor::processRemoveUserDataCommand(const struct Sh
serverStatusOut.m_removeUserDataResponseArgs = clientCmd.m_removeUserDataRequestArgs;
serverStatusOut.m_type = CMD_REMOVE_USER_DATA_COMPLETED;
b3Notification notification;
notification.m_notificationType = USER_DATA_REMOVED;
notification.m_userDataArgs.m_userDataId = clientCmd.m_removeUserDataRequestArgs.m_userDataId;
m_data->m_pluginManager.addNotification(notification);
return hasStatus;
}
@ -6481,6 +6501,11 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar
bodyHandle->m_softBody = psb;
serverStatusOut.m_loadSoftBodyResultArguments.m_objectUniqueId = bodyUniqueId;
serverStatusOut.m_type = CMD_LOAD_SOFT_BODY_COMPLETED;
b3Notification notification;
notification.m_notificationType = BODY_ADDED;
notification.m_bodyArgs.m_bodyUniqueId = bodyUniqueId;
m_data->m_pluginManager.addNotification(notification);
}
}
}
@ -6741,17 +6766,25 @@ bool PhysicsServerCommandProcessor::processForwardDynamicsCommand(const struct S
btScalar deltaTimeScaled = m_data->m_physicsDeltaTime*simTimeScalingFactor;
int numSteps = 0;
if (m_data->m_numSimulationSubSteps > 0)
{
m_data->m_dynamicsWorld->stepSimulation(deltaTimeScaled, m_data->m_numSimulationSubSteps, m_data->m_physicsDeltaTime / m_data->m_numSimulationSubSteps);
numSteps = m_data->m_dynamicsWorld->stepSimulation(deltaTimeScaled, m_data->m_numSimulationSubSteps, m_data->m_physicsDeltaTime / m_data->m_numSimulationSubSteps);
}
else
{
m_data->m_dynamicsWorld->stepSimulation(deltaTimeScaled, 0);
numSteps = m_data->m_dynamicsWorld->stepSimulation(deltaTimeScaled, 0);
}
if (numSteps > 0)
{
addTransformChangedNotifications();
}
SharedMemoryStatus& serverCmd =serverStatusOut;
serverCmd.m_type = CMD_STEP_FORWARD_SIMULATION_COMPLETED;
return hasStatus;
}
@ -7049,6 +7082,13 @@ bool PhysicsServerCommandProcessor::processChangeDynamicsInfoCommand(const struc
SharedMemoryStatus& serverCmd =serverStatusOut;
serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED;
b3Notification notification;
notification.m_notificationType = LINK_DYNAMICS_CHANGED;
notification.m_linkArgs.m_bodyUniqueId = bodyUniqueId;
notification.m_linkArgs.m_linkIndex = linkIndex;
m_data->m_pluginManager.addNotification(notification);
return hasStatus;
}
@ -7657,6 +7697,11 @@ bool PhysicsServerCommandProcessor::processCreateRigidBodyCommand(const struct S
rb->setUserIndex2(bodyUniqueId);
bodyHandle->m_rootLocalInertialFrame.setIdentity();
bodyHandle->m_rigidBody = rb;
b3Notification notification;
notification.m_notificationType = BODY_ADDED;
notification.m_bodyArgs.m_bodyUniqueId = bodyUniqueId;
m_data->m_pluginManager.addNotification(notification);
return hasStatus;
}
@ -8257,6 +8302,14 @@ bool PhysicsServerCommandProcessor::processRemoveBodyCommand(const struct Shared
}
m_data->m_guiHelper->setVisualizerFlag(COV_ENABLE_SYNC_RENDERING_INTERNAL,1);
for (int i=0;i<serverCmd.m_removeObjectArgs.m_numBodies;i++)
{
b3Notification notification;
notification.m_notificationType = BODY_REMOVED;
notification.m_bodyArgs.m_bodyUniqueId = serverCmd.m_removeObjectArgs.m_bodyUniqueIds[i];
m_data->m_pluginManager.addNotification(notification);
}
return hasStatus;
}
@ -9419,7 +9472,15 @@ bool PhysicsServerCommandProcessor::processUpdateVisualShapeCommand(const struct
}
serverCmd.m_type = CMD_VISUAL_SHAPE_UPDATE_COMPLETED;
return hasStatus;
b3Notification notification;
notification.m_notificationType = VISUAL_SHAPE_CHANGED;
notification.m_visualShapeArgs.m_bodyUniqueId = clientCmd.m_updateVisualShapeDataArguments.m_bodyUniqueId;
notification.m_visualShapeArgs.m_linkIndex = clientCmd.m_updateVisualShapeDataArguments.m_jointIndex;
notification.m_visualShapeArgs.m_visualShapeIndex = clientCmd.m_updateVisualShapeDataArguments.m_shapeIndex;
m_data->m_pluginManager.addNotification(notification);
return hasStatus;
}
bool PhysicsServerCommandProcessor::processChangeTextureCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
@ -9640,6 +9701,11 @@ bool PhysicsServerCommandProcessor::processLoadBulletCommand(const struct Shared
serverStatusOut.m_sdfLoadedArgs.m_numBodies++;
serverStatusOut.m_sdfLoadedArgs.m_bodyUniqueIds[i] = bodyUniqueId;
}
b3Notification notification;
notification.m_notificationType = BODY_ADDED;
notification.m_bodyArgs.m_bodyUniqueId = bodyUniqueId;
m_data->m_pluginManager.addNotification(notification);
}
}
}
@ -10439,11 +10505,59 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec,const
{
gNumSteps = numSteps;
gDtInSec = dtInSec;
addTransformChangedNotifications();
}
}
}
b3Notification createTransformChangedNotification(int bodyUniqueId, int linkIndex, const btCollisionObject* colObj)
{
b3Notification notification;
notification.m_notificationType = TRANSFORM_CHANGED;
notification.m_transformChangeArgs.m_bodyUniqueId = bodyUniqueId;
notification.m_transformChangeArgs.m_linkIndex = linkIndex;
const btTransform &tr = colObj->getWorldTransform();
notification.m_transformChangeArgs.m_worldPosition[0] = tr.getOrigin()[0];
notification.m_transformChangeArgs.m_worldPosition[1] = tr.getOrigin()[1];
notification.m_transformChangeArgs.m_worldPosition[2] = tr.getOrigin()[2];
notification.m_transformChangeArgs.m_worldRotation[0] = tr.getRotation()[0];
notification.m_transformChangeArgs.m_worldRotation[1] = tr.getRotation()[1];
notification.m_transformChangeArgs.m_worldRotation[2] = tr.getRotation()[2];
notification.m_transformChangeArgs.m_worldRotation[3] = tr.getRotation()[3];
const btVector3 &scaling = colObj->getCollisionShape()->getLocalScaling();
notification.m_transformChangeArgs.m_localScaling[0] = scaling[0];
notification.m_transformChangeArgs.m_localScaling[1] = scaling[1];
notification.m_transformChangeArgs.m_localScaling[2] = scaling[2];
return notification;
}
void PhysicsServerCommandProcessor::addTransformChangedNotifications()
{
b3AlignedObjectArray<int> usedHandles;
m_data->m_bodyHandles.getUsedHandles(usedHandles);
for (int i=0;i<usedHandles.size();i++) {
int bodyUniqueId = usedHandles[i];
InternalBodyData *bodyData = m_data->m_bodyHandles.getHandle(bodyUniqueId);
if (!bodyData) {
continue;
}
if (bodyData->m_multiBody && bodyData->m_multiBody->isAwake()) {
btMultiBody *mb = bodyData->m_multiBody;
m_data->m_pluginManager.addNotification(createTransformChangedNotification(bodyUniqueId, -1, mb->getBaseCollider()));
for (int linkIndex=0;linkIndex < mb->getNumLinks(); linkIndex++) {
m_data->m_pluginManager.addNotification(createTransformChangedNotification(bodyUniqueId, linkIndex, mb->getLinkCollider(linkIndex)));
}
}
else if (bodyData->m_rigidBody && bodyData->m_rigidBody->isActive()) {
m_data->m_pluginManager.addNotification(createTransformChangedNotification(bodyUniqueId, -1, bodyData->m_rigidBody));
}
}
}
void PhysicsServerCommandProcessor::resetSimulation()
{
@ -10490,6 +10604,10 @@ void PhysicsServerCommandProcessor::resetSimulation()
m_data->m_userDataHandles.exitHandles();
m_data->m_userDataHandles.initHandles();
m_data->m_userDataHandleLookup.clear();
b3Notification notification;
notification.m_notificationType = SIMULATION_RESET;
m_data->m_pluginManager.addNotification(notification);
}

View File

@ -155,6 +155,7 @@ public:
virtual void replayLogCommand(char* bufferServerToClient, int bufferSizeInBytes );
//logging of object states (position etc)
virtual void tickPlugins();
void tickPlugins(btScalar timeStep, bool isPreTick);
void logObjectStates(btScalar timeStep);
void processCollisionForces(btScalar timeStep);
@ -173,6 +174,9 @@ public:
virtual const btQuaternion& getVRTeleportOrientation() const;
virtual void setVRTeleportOrientation(const btQuaternion& vrTeleportOrn);
private:
void addTransformChangedNotifications();
};
#endif //PHYSICS_SERVER_COMMAND_PROCESSOR_H

View File

@ -471,6 +471,8 @@ void MotionThreadFunc(void* userPtr,void* lsMemory)
args->m_physicsServerPtr->processClientCommands();
numCmdSinceSleep1ms++;
}
args->m_physicsServerPtr->tickPlugins();
args->m_cs->lock();
cachedSharedParam = args->m_cs->getSharedParam(0);

View File

@ -242,7 +242,10 @@ bool PhysicsServerSharedMemory::isRealTimeSimulationEnabled() const
return m_data->m_commandProcessor->isRealTimeSimulationEnabled();
}
void PhysicsServerSharedMemory::tickPlugins()
{
m_data->m_commandProcessor->tickPlugins();
}
void PhysicsServerSharedMemory::processClientCommands()
{

View File

@ -32,6 +32,8 @@ public:
virtual void enableRealTimeSimulation(bool enableRealTimeSim);
virtual bool isRealTimeSimulationEnabled() const;
virtual void tickPlugins();
//bool supportsJointMotor(class btMultiBody* body, int linkIndex);

View File

@ -500,6 +500,61 @@ struct b3MouseEventsData
struct b3MouseEvent* m_mouseEvents;
};
enum b3NotificationType {
SIMULATION_RESET = 0,
BODY_ADDED = 1,
BODY_REMOVED = 2,
USER_DATA_ADDED = 3,
USER_DATA_REMOVED = 4,
LINK_DYNAMICS_CHANGED = 5,
VISUAL_SHAPE_CHANGED = 6,
TRANSFORM_CHANGED = 7,
};
struct b3BodyNotificationArgs
{
int m_bodyUniqueId;
};
struct b3UserDataNotificationArgs
{
int m_userDataId;
};
struct b3LinkNotificationArgs
{
int m_bodyUniqueId;
int m_linkIndex;
};
struct b3VisualShapeNotificationArgs
{
int m_bodyUniqueId;
int m_linkIndex;
int m_visualShapeIndex;
};
struct b3TransformChangeNotificationArgs
{
int m_bodyUniqueId;
int m_linkIndex;
double m_worldPosition[3];
double m_worldRotation[4];
double m_localScaling[3];
};
struct b3Notification
{
int m_notificationType;
union {
struct b3BodyNotificationArgs m_bodyArgs;
struct b3UserDataNotificationArgs m_userDataArgs;
struct b3LinkNotificationArgs m_linkArgs;
struct b3VisualShapeNotificationArgs m_visualShapeArgs;
struct b3TransformChangeNotificationArgs m_transformChangeArgs;
};
};
struct b3ContactPointData
{
//todo: expose some contact flags, such as telling which fields below are valid

View File

@ -38,6 +38,7 @@ struct b3Plugin
PFN_TICK m_preTickFunc;
PFN_TICK m_postTickFunc;
PFN_TICK m_tickPluginFunc;
PFN_GET_RENDER_INTERFACE m_getRendererFunc;
@ -52,6 +53,7 @@ struct b3Plugin
m_executeCommandFunc(0),
m_preTickFunc(0),
m_postTickFunc(0),
m_tickPluginFunc(0),
m_getRendererFunc(0),
m_userPointer(0)
{
@ -83,10 +85,12 @@ struct b3PluginManagerInternalData
b3AlignedObjectArray<b3KeyboardEvent> m_keyEvents;
b3AlignedObjectArray<b3VRControllerEvent> m_vrEvents;
b3AlignedObjectArray<b3MouseEvent> m_mouseEvents;
b3AlignedObjectArray<b3Notification> m_notifications[2];
int m_activeNotificationsBufferIndex;
int m_activeRendererPluginUid;
b3PluginManagerInternalData()
:m_activeRendererPluginUid(-1)
:m_activeRendererPluginUid(-1), m_activeNotificationsBufferIndex(0)
{
}
};
@ -140,6 +144,11 @@ void b3PluginManager::clearEvents()
m_data->m_mouseEvents.resize(0);
}
void b3PluginManager::addNotification(const struct b3Notification& notification)
{
m_data->m_notifications[m_data->m_activeNotificationsBufferIndex].push_back(notification);
}
int b3PluginManager::loadPlugin(const char* pluginPath, const char* postFixStr)
{
int pluginUniqueId = -1;
@ -165,6 +174,7 @@ int b3PluginManager::loadPlugin(const char* pluginPath, const char* postFixStr)
std::string executePluginCommandStr = std::string("executePluginCommand") + postFix;
std::string preTickPluginCallbackStr = std::string("preTickPluginCallback") + postFix;
std::string postTickPluginCallback = std::string("postTickPluginCallback") + postFix;
std::string tickPluginStr = std::string("tickPlugin") + postFix;
std::string getRendererStr = std::string("getRenderInterface") + postFix;
plugin->m_initFunc = (PFN_INIT)B3_DYNLIB_IMPORT(pluginHandle, initStr.c_str());
@ -172,6 +182,7 @@ int b3PluginManager::loadPlugin(const char* pluginPath, const char* postFixStr)
plugin->m_executeCommandFunc = (PFN_EXECUTE)B3_DYNLIB_IMPORT(pluginHandle, executePluginCommandStr.c_str());
plugin->m_preTickFunc = (PFN_TICK)B3_DYNLIB_IMPORT(pluginHandle, preTickPluginCallbackStr.c_str());
plugin->m_postTickFunc = (PFN_TICK)B3_DYNLIB_IMPORT(pluginHandle, postTickPluginCallback.c_str());
plugin->m_tickPluginFunc = (PFN_TICK)B3_DYNLIB_IMPORT(pluginHandle, tickPluginStr.c_str());
plugin->m_getRendererFunc = (PFN_GET_RENDER_INTERFACE)B3_DYNLIB_IMPORT(pluginHandle, getRendererStr.c_str());
if (plugin->m_initFunc && plugin->m_exitFunc && plugin->m_executeCommandFunc)
@ -236,7 +247,7 @@ void b3PluginManager::unloadPlugin(int pluginUniqueId)
b3PluginHandle* plugin = m_data->m_plugins.getHandle(pluginUniqueId);
if (plugin)
{
b3PluginContext context;
b3PluginContext context = {0};
context.m_userPointer = plugin->m_userPointer;
context.m_physClient = (b3PhysicsClientHandle) m_data->m_physicsDirect;
@ -266,7 +277,7 @@ void b3PluginManager::tickPlugins(double timeStep, bool isPreTick)
PFN_TICK tick = isPreTick? plugin->m_preTickFunc : plugin->m_postTickFunc;
if (tick)
{
b3PluginContext context;
b3PluginContext context = {0};
context.m_userPointer = plugin->m_userPointer;
context.m_physClient = (b3PhysicsClientHandle) m_data->m_physicsDirect;
context.m_numMouseEvents = m_data->m_mouseEvents.size();
@ -281,6 +292,39 @@ void b3PluginManager::tickPlugins(double timeStep, bool isPreTick)
}
}
void b3PluginManager::tickPlugins()
{
b3AlignedObjectArray<b3Notification> &notifications = m_data->m_notifications[m_data->m_activeNotificationsBufferIndex];
// Swap notification buffers.
m_data->m_activeNotificationsBufferIndex = 1 - m_data->m_activeNotificationsBufferIndex;
for (int i=0;i<m_data->m_pluginMap.size();i++)
{
int* pluginUidPtr = m_data->m_pluginMap.getAtIndex(i);
b3PluginHandle* plugin = 0;
if (pluginUidPtr)
{
int pluginUid = *pluginUidPtr;
plugin = m_data->m_plugins.getHandle(pluginUid);
}
else
{
continue;
}
if (plugin->m_tickPluginFunc) {
b3PluginContext context = {0};
context.m_userPointer = plugin->m_userPointer;
context.m_physClient = (b3PhysicsClientHandle) m_data->m_physicsDirect;
context.m_numNotifications = notifications.size();
context.m_notifications = notifications.size() ? &notifications[0] : 0;
plugin->m_tickPluginFunc(&context);
}
}
notifications.clear();
}
int b3PluginManager::executePluginCommand(int pluginUniqueId, const b3PluginArguments* arguments)
{
int result = -1;
@ -288,15 +332,9 @@ int b3PluginManager::executePluginCommand(int pluginUniqueId, const b3PluginArgu
b3PluginHandle* plugin = m_data->m_plugins.getHandle(pluginUniqueId);
if (plugin)
{
b3PluginContext context;
b3PluginContext context = {0};
context.m_userPointer = plugin->m_userPointer;
context.m_physClient = (b3PhysicsClientHandle) m_data->m_physicsDirect;
context.m_numMouseEvents = 0;
context.m_mouseEvents = 0;
context.m_numKeyEvents = 0;
context.m_keyEvents = 0;
context.m_numVRControllerEvents = 0;
context.m_vrControllerEvents = 0;
result = plugin->m_executeCommandFunc(&context, arguments);
plugin->m_userPointer = context.m_userPointer;
@ -329,15 +367,9 @@ int b3PluginManager::registerStaticLinkedPlugin(const char* pluginPath, PFN_INIT
m_data->m_pluginMap.insert(pluginPath, pluginUniqueId);
{
b3PluginContext context;
b3PluginContext context = {0};
context.m_userPointer = 0;
context.m_physClient = (b3PhysicsClientHandle) m_data->m_physicsDirect;
context.m_numMouseEvents = 0;
context.m_mouseEvents = 0;
context.m_numKeyEvents = 0;
context.m_keyEvents = 0;
context.m_numVRControllerEvents = 0;
context.m_vrControllerEvents = 0;
int result = pluginHandle->m_initFunc(&context);
pluginHandle->m_userPointer = context.m_userPointer;
@ -359,15 +391,9 @@ UrdfRenderingInterface* b3PluginManager::getRenderInterface()
b3PluginHandle* plugin = m_data->m_plugins.getHandle(m_data->m_activeRendererPluginUid);
if (plugin)
{
b3PluginContext context;
b3PluginContext context = {0};
context.m_userPointer = plugin->m_userPointer;
context.m_physClient = (b3PhysicsClientHandle) m_data->m_physicsDirect;
context.m_numMouseEvents = 0;
context.m_mouseEvents = 0;
context.m_numKeyEvents = 0;
context.m_keyEvents = 0;
context.m_numVRControllerEvents = 0;
context.m_vrControllerEvents = 0;
renderer = plugin->m_getRendererFunc(&context);
}

View File

@ -18,7 +18,10 @@ class b3PluginManager
void addEvents(const struct b3VRControllerEvent* vrControllerEvents, int numVRControllerEvents, const struct b3KeyboardEvent* keyEvents, int numKeyEvents, const struct b3MouseEvent* mouseEvents, int numMouseEvents);
void clearEvents();
void addNotification(const struct b3Notification& notification);
void tickPlugins(double timeStep, bool isPreTick);
void tickPlugins();
int registerStaticLinkedPlugin(const char* pluginPath, PFN_INIT initFunc,PFN_EXIT exitFunc, PFN_EXECUTE executeCommandFunc, PFN_TICK preTickFunc, PFN_TICK postTickFunc, PFN_GET_RENDER_INTERFACE getRendererFunc);

View File

@ -16,6 +16,8 @@ struct b3PluginContext
int m_numKeyEvents;
const struct b3MouseEvent* m_mouseEvents;
int m_numMouseEvents;
const struct b3Notification* m_notifications;
int m_numNotifications;
};