From d41e449d6f737f71b14fe944bdbbe2666a46135f Mon Sep 17 00:00:00 2001 From: mbennice Date: Thu, 14 Mar 2019 15:32:45 -0700 Subject: [PATCH] Adds a timestamp as a physics parameter. --- examples/SharedMemory/PhysicsClientC_API.cpp | 5 +++-- .../SharedMemory/PhysicsServerCommandProcessor.cpp | 13 ++++++++++--- examples/SharedMemory/SharedMemoryPublic.h | 1 + 3 files changed, 14 insertions(+), 5 deletions(-) diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 8eaac0a6f..08f868bb6 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -1082,8 +1082,9 @@ B3_SHARED_API int b3GetLinkState(b3PhysicsClientHandle physClient, b3SharedMemor int numLinks = status->m_sendActualStateArgs.m_numLinks; b3Assert(linkIndex < numLinks); - if ((bodyIndex >= 0) && (linkIndex >= 0) && linkIndex < numLinks) - { + if (status->m_sendActualStateArgs.m_stateDetails != nullptr && + (bodyIndex >= 0) && (linkIndex >= 0) && linkIndex < numLinks) + { b3Transform wlf, com, inertial; for (int i = 0; i < 3; ++i) diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index e04686e81..a5c117e10 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -31,7 +31,7 @@ #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" #include "Bullet3Common/b3HashMap.h" #include "../Utils/ChromeTraceUtil.h" -#include "stb_image/stb_image.h" +#include "third_party/stblib/stb_image.h" #include "BulletInverseDynamics/MultiBodyTree.hpp" #include "IKTrajectoryHelper.h" #include "btBulletDynamicsCommon.h" @@ -1613,6 +1613,8 @@ struct PhysicsServerCommandProcessorInternalData btScalar m_physicsDeltaTime; btScalar m_numSimulationSubSteps; + btScalar m_simulationTimestamp; + btAlignedObjectArray m_multiBodyJointFeedbacks; b3HashMap m_inverseDynamicsBodies; b3HashMap m_inverseKinematicsHelpers; @@ -7608,10 +7610,12 @@ bool PhysicsServerCommandProcessor::processForwardDynamicsCommand(const struct S if (m_data->m_numSimulationSubSteps > 0) { numSteps = m_data->m_dynamicsWorld->stepSimulation(deltaTimeScaled, m_data->m_numSimulationSubSteps, m_data->m_physicsDeltaTime / m_data->m_numSimulationSubSteps); + m_data->m_simulationTimestamp += deltaTimeScaled; } else { numSteps = m_data->m_dynamicsWorld->stepSimulation(deltaTimeScaled, 0); + m_data->m_simulationTimestamp += deltaTimeScaled; } if (numSteps > 0) @@ -8161,6 +8165,7 @@ bool PhysicsServerCommandProcessor::processRequestPhysicsSimulationParametersCom serverCmd.m_simulationParameterResultArgs.m_allowedCcdPenetration = m_data->m_dynamicsWorld->getDispatchInfo().m_allowedCcdPenetration; serverCmd.m_simulationParameterResultArgs.m_collisionFilterMode = m_data->m_broadphaseCollisionFilterCallback->m_filterMode; serverCmd.m_simulationParameterResultArgs.m_deltaTime = m_data->m_physicsDeltaTime; + serverCmd.m_simulationParameterResultArgs.m_simulationTimestamp = m_data->m_simulationTimestamp; serverCmd.m_simulationParameterResultArgs.m_contactBreakingThreshold = gContactBreakingThreshold; serverCmd.m_simulationParameterResultArgs.m_contactSlop = m_data->m_dynamicsWorld->getSolverInfo().m_linearSlop; serverCmd.m_simulationParameterResultArgs.m_enableSAT = m_data->m_dynamicsWorld->getDispatchInfo().m_enableSatConvex; @@ -11718,7 +11723,9 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec, const gSubStep = m_data->m_physicsDeltaTime; } - int numSteps = m_data->m_dynamicsWorld->stepSimulation(dtInSec * simTimeScalingFactor, maxSteps, gSubStep); + btScalar deltaTimeScaled = dtInSec * simTimeScalingFactor; + int numSteps = m_data->m_dynamicsWorld->stepSimulation(deltaTimeScaled, maxSteps, gSubStep); + m_data->m_simulationTimestamp += deltaTimeScaled; gDroppedSimulationSteps += numSteps > maxSteps ? numSteps - maxSteps : 0; if (numSteps) @@ -11796,7 +11803,7 @@ void PhysicsServerCommandProcessor::addTransformChangedNotifications() void PhysicsServerCommandProcessor::resetSimulation() { //clean up all data - + m_data->m_simulationTimestamp = 0; m_data->m_cachedVUrdfisualShapes.clear(); #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 53d29c732..c510d7bea 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -900,6 +900,7 @@ struct b3PluginArguments struct b3PhysicsSimulationParameters { double m_deltaTime; + double m_simulationTimestamp; // Output only timestamp of simulation. double m_gravityAcceleration[3]; int m_numSimulationSubSteps; int m_numSolverIterations;