diff --git a/examples/CommonInterfaces/CommonGUIHelperInterface.h b/examples/CommonInterfaces/CommonGUIHelperInterface.h index d65cb6581..d5b72a0c8 100644 --- a/examples/CommonInterfaces/CommonGUIHelperInterface.h +++ b/examples/CommonInterfaces/CommonGUIHelperInterface.h @@ -45,6 +45,7 @@ struct GUIHelperInterface virtual void removeGraphicsInstance(int graphicsUid) {} virtual void changeInstanceFlags(int instanceUid, int flags) {} virtual void changeRGBAColor(int instanceUid, const double rgbaColor[4]) {} + virtual void changeScaling(int instanceUid, const double scaling[3]) {} virtual void changeSpecularColor(int instanceUid, const double specularColor[3]) {} virtual void changeTexture(int textureUniqueId, const unsigned char* rgbTexels, int width, int height) {} virtual void updateShape(int shapeIndex, float* vertices) {} @@ -151,6 +152,7 @@ struct DummyGUIHelper : public GUIHelperInterface virtual void removeAllGraphicsInstances() {} virtual void removeGraphicsInstance(int graphicsUid) {} virtual void changeRGBAColor(int instanceUid, const double rgbaColor[4]) {} + virtual void changeScaling(int instanceUid, const double scaling[3]) {} virtual Common2dCanvasInterface* get2dCanvasInterface() { diff --git a/examples/ExampleBrowser/OpenGLGuiHelper.cpp b/examples/ExampleBrowser/OpenGLGuiHelper.cpp index 610329425..8afdad793 100644 --- a/examples/ExampleBrowser/OpenGLGuiHelper.cpp +++ b/examples/ExampleBrowser/OpenGLGuiHelper.cpp @@ -388,6 +388,14 @@ void OpenGLGuiHelper::changeInstanceFlags(int instanceUid, int flags) m_data->m_glApp->m_renderer->writeSingleInstanceFlagsToCPU( flags, instanceUid); } } +void OpenGLGuiHelper::changeScaling(int instanceUid, const double scaling[3]) +{ + if (instanceUid >= 0) + { + m_data->m_glApp->m_renderer->writeSingleInstanceScaleToCPU(scaling, instanceUid); + }; +} + void OpenGLGuiHelper::changeRGBAColor(int instanceUid, const double rgbaColor[4]) { if (instanceUid >= 0) diff --git a/examples/ExampleBrowser/OpenGLGuiHelper.h b/examples/ExampleBrowser/OpenGLGuiHelper.h index bdaa22772..8c9b0e70e 100644 --- a/examples/ExampleBrowser/OpenGLGuiHelper.h +++ b/examples/ExampleBrowser/OpenGLGuiHelper.h @@ -29,6 +29,7 @@ struct OpenGLGuiHelper : public GUIHelperInterface virtual void removeGraphicsInstance(int graphicsUid); virtual void changeInstanceFlags(int instanceUid, int flags); virtual void changeRGBAColor(int instanceUid, const double rgbaColor[4]); + virtual void changeScaling(int instanceUid, const double scaling[3]); virtual void changeSpecularColor(int instanceUid, const double specularColor[3]); virtual void changeTexture(int textureUniqueId, const unsigned char* rgbTexels, int width, int height); virtual void removeTexture(int textureUid); diff --git a/examples/SharedMemory/GraphicsServerExample.cpp b/examples/SharedMemory/GraphicsServerExample.cpp index b63d36c65..3e92b75c4 100644 --- a/examples/SharedMemory/GraphicsServerExample.cpp +++ b/examples/SharedMemory/GraphicsServerExample.cpp @@ -488,6 +488,18 @@ void TCPThreadFunc(void* userPtr, void* lsMemory) printf("GFX_CMD_CHANGE_RGBA_COLOR\n"); break; } + case GFX_CMD_CHANGE_SCALING: + { + args->submitCommand(); + while (args->isCommandOutstanding()) + { + clock.usleep(0); + } + if (gVerboseNetworkMessagesServer) + printf("GFX_CMD_CHANGE_SCALING\n"); + break; + } + case GFX_CMD_GET_CAMERA_INFO: { args->submitCommand(); @@ -833,6 +845,14 @@ public: m_args.processCommand(); break; } + + case GFX_CMD_CHANGE_SCALING: + { + m_guiHelper->changeScaling(clientCmd.m_changeScalingCommand.m_graphicsUid, clientCmd.m_changeScalingCommand.m_scaling); + m_args.processCommand(); + break; + } + case GFX_CMD_GET_CAMERA_INFO: { serverStatusOut.m_type = GFX_CMD_GET_CAMERA_INFO_FAILED; diff --git a/examples/SharedMemory/GraphicsSharedMemoryCommands.h b/examples/SharedMemory/GraphicsSharedMemoryCommands.h index c612d9b96..2fbaf459f 100644 --- a/examples/SharedMemory/GraphicsSharedMemoryCommands.h +++ b/examples/SharedMemory/GraphicsSharedMemoryCommands.h @@ -112,6 +112,14 @@ struct GraphicsChangeRGBAColorCommand double m_rgbaColor[4]; }; +struct GraphicsChangeScalingCommand +{ + int m_graphicsUid; + double m_scaling[3]; +}; + + + struct GraphicsGetCameraInfoStatus { int width; @@ -150,6 +158,7 @@ struct GraphicsSharedMemoryCommand struct GraphicsSyncTransformsCommand m_syncTransformsCommand; struct GraphicsRemoveInstanceCommand m_removeGraphicsInstanceCommand; struct GraphicsChangeRGBAColorCommand m_changeRGBAColorCommand; + struct GraphicsChangeScalingCommand m_changeScalingCommand; }; }; diff --git a/examples/SharedMemory/GraphicsSharedMemoryPublic.h b/examples/SharedMemory/GraphicsSharedMemoryPublic.h index 486721c72..4a80736bd 100644 --- a/examples/SharedMemory/GraphicsSharedMemoryPublic.h +++ b/examples/SharedMemory/GraphicsSharedMemoryPublic.h @@ -22,6 +22,7 @@ enum EnumGraphicsSharedMemoryClientCommand GFX_CMD_REMOVE_SINGLE_GRAPHICS_INSTANCE, GFX_CMD_CHANGE_RGBA_COLOR, GFX_CMD_GET_CAMERA_INFO, + GFX_CMD_CHANGE_SCALING, //don't go beyond this command! GFX_CMD_MAX_CLIENT_COMMANDS, }; diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index e65a7be93..b3f4d4552 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -2240,6 +2240,22 @@ B3_SHARED_API int b3CreatePoseCommandSetBasePosition(b3SharedMemoryCommandHandle return 0; } +B3_SHARED_API int b3CreatePoseCommandSetBaseScaling(b3SharedMemoryCommandHandle commandHandle, double scaling[3]) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_INIT_POSE); + command->m_updateFlags |= INIT_POSE_HAS_SCALING; + + command->m_initPoseArgs.m_scaling[0] = scaling[0]; + command->m_initPoseArgs.m_scaling[1] = scaling[1]; + command->m_initPoseArgs.m_scaling[2] = scaling[2]; + + return 0; +} + + + B3_SHARED_API int b3CreatePoseCommandSetBaseOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX, double startOrnY, double startOrnZ, double startOrnW) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 41d1a9fba..560546736 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -574,6 +574,8 @@ extern "C" B3_SHARED_API int b3CreatePoseCommandSetBaseOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX, double startOrnY, double startOrnZ, double startOrnW); B3_SHARED_API int b3CreatePoseCommandSetBaseLinearVelocity(b3SharedMemoryCommandHandle commandHandle, const double linVel[/*3*/]); B3_SHARED_API int b3CreatePoseCommandSetBaseAngularVelocity(b3SharedMemoryCommandHandle commandHandle, const double angVel[/*3*/]); + B3_SHARED_API int b3CreatePoseCommandSetBaseScaling(b3SharedMemoryCommandHandle commandHandle, double scaling[/* 3*/]); + B3_SHARED_API int b3CreatePoseCommandSetJointPositions(b3SharedMemoryCommandHandle commandHandle, int numJointPositions, const double* jointPositions); B3_SHARED_API int b3CreatePoseCommandSetJointPosition(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, int jointIndex, double jointPosition); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 0dbab0d41..862a75557 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -8063,7 +8063,9 @@ bool PhysicsServerCommandProcessor::processRequestContactpointInformationCommand cb.m_bodyUniqueIdA = bodyUniqueIdA; cb.m_bodyUniqueIdB = bodyUniqueIdB; - cb.m_deltaTime = m_data->m_physicsDeltaTime; + cb.m_deltaTime = m_data->m_numSimulationSubSteps>0 ? + m_data->m_physicsDeltaTime/ m_data->m_numSimulationSubSteps : + m_data->m_physicsDeltaTime; for (int i = 0; i < setA.size(); i++) { @@ -10047,6 +10049,20 @@ bool PhysicsServerCommandProcessor::processInitPoseCommand(const struct SharedMe { btMultiBody* mb = body->m_multiBody; + if (clientCmd.m_updateFlags & INIT_POSE_HAS_SCALING) + { + btVector3 scaling(clientCmd.m_initPoseArgs.m_scaling[0], clientCmd.m_initPoseArgs.m_scaling[1], clientCmd.m_initPoseArgs.m_scaling[2]); + + mb->getBaseCollider()->getCollisionShape()->setLocalScaling(scaling); + //refresh broadphase + m_data->m_dynamicsWorld->getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs( + mb->getBaseCollider()->getBroadphaseHandle(), + m_data->m_dynamicsWorld->getDispatcher()); + //also visuals + int graphicsIndex = mb->getBaseCollider()->getUserIndex(); + m_data->m_guiHelper->changeScaling(graphicsIndex, clientCmd.m_initPoseArgs.m_scaling); + } + if (clientCmd.m_updateFlags & INIT_POSE_HAS_BASE_LINEAR_VELOCITY) { mb->setBaseVel(baseLinVel); @@ -11605,6 +11621,10 @@ bool PhysicsServerCommandProcessor::processCreateUserConstraintCommand(const str if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_CONSTRAINT) { + btScalar fixedTimeSubStep = m_data->m_numSimulationSubSteps > 0 ? + m_data->m_physicsDeltaTime / m_data->m_numSimulationSubSteps : + m_data->m_physicsDeltaTime; + serverCmd.m_type = CMD_CHANGE_USER_CONSTRAINT_FAILED; int userConstraintUidChange = clientCmd.m_userConstraintArguments.m_userConstraintUniqueId; InteralUserConstraintData* userConstraintPtr = m_data->m_userConstraints.find(userConstraintUidChange); @@ -11637,7 +11657,8 @@ bool PhysicsServerCommandProcessor::processCreateUserConstraintCommand(const str } if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_MAX_FORCE) { - btScalar maxImp = clientCmd.m_userConstraintArguments.m_maxAppliedForce * m_data->m_physicsDeltaTime; + btScalar maxImp = clientCmd.m_userConstraintArguments.m_maxAppliedForce * fixedTimeSubStep; + userConstraintPtr->m_userConstraintData.m_maxAppliedForce = clientCmd.m_userConstraintArguments.m_maxAppliedForce; userConstraintPtr->m_mbConstraint->setMaxAppliedImpulse(maxImp); } @@ -11668,7 +11689,7 @@ bool PhysicsServerCommandProcessor::processCreateUserConstraintCommand(const str { if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_MAX_FORCE) { - btScalar maxImp = clientCmd.m_userConstraintArguments.m_maxAppliedForce * m_data->m_physicsDeltaTime; + btScalar maxImp = clientCmd.m_userConstraintArguments.m_maxAppliedForce * fixedTimeSubStep; userConstraintPtr->m_userConstraintData.m_maxAppliedForce = clientCmd.m_userConstraintArguments.m_maxAppliedForce; //userConstraintPtr->m_rbConstraint->setMaxAppliedImpulse(maxImp); } diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index 7cde642a2..3a159f581 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -128,6 +128,7 @@ enum MultiThreadedGUIHelperCommunicationEnums eGUIHelperRemoveTexture, eGUIHelperSetVisualizerFlagCheckRenderedFrame, eGUIHelperUpdateShape, + eGUIHelperChangeGraphicsInstanceScaling, eGUIUserDebugRemoveAllParameters, }; @@ -1035,6 +1036,19 @@ public: workerThreadWait(); } + int m_graphicsInstanceChangeScaling; + double m_baseScaling[3]; + virtual void changeScaling(int instanceUid, const double scaling[3]) + { + m_graphicsInstanceChangeScaling = instanceUid; + m_baseScaling[0] = scaling[0]; + m_baseScaling[1] = scaling[1]; + m_baseScaling[2] = scaling[2]; + m_cs->lock(); + m_cs->setSharedParam(1, eGUIHelperChangeGraphicsInstanceScaling); + workerThreadWait(); + } + double m_specularColor[3]; int m_graphicsInstanceChangeSpecular; virtual void changeSpecularColor(int instanceUid, const double specularColor[3]) @@ -2181,6 +2195,16 @@ void PhysicsServerExample::updateGraphics() m_multiThreadedHelper->mainThreadRelease(); break; } + + case eGUIHelperChangeGraphicsInstanceScaling: + { + B3_PROFILE("eGUIHelperChangeGraphicsInstanceScaling"); + + m_multiThreadedHelper->m_childGuiHelper->changeScaling(m_multiThreadedHelper->m_graphicsInstanceChangeScaling, m_multiThreadedHelper->m_baseScaling); + m_multiThreadedHelper->mainThreadRelease(); + break; + } + case eGUIHelperChangeGraphicsInstanceSpecularColor: { B3_PROFILE("eGUIHelperChangeGraphicsInstanceSpecularColor"); diff --git a/examples/SharedMemory/RemoteGUIHelper.cpp b/examples/SharedMemory/RemoteGUIHelper.cpp index a7bed235d..b5754e185 100644 --- a/examples/SharedMemory/RemoteGUIHelper.cpp +++ b/examples/SharedMemory/RemoteGUIHelper.cpp @@ -529,6 +529,11 @@ void RemoteGUIHelper::removeGraphicsInstance(int graphicsUid) } } } + +void RemoteGUIHelper::changeScaling(int instanceUid, const double scaling[3]) +{ + +} void RemoteGUIHelper::changeRGBAColor(int instanceUid, const double rgbaColor[4]) { GraphicsSharedMemoryCommand* cmd = m_data->getAvailableSharedMemoryCommand(); diff --git a/examples/SharedMemory/RemoteGUIHelper.h b/examples/SharedMemory/RemoteGUIHelper.h index 1931091e5..3d74b03fd 100644 --- a/examples/SharedMemory/RemoteGUIHelper.h +++ b/examples/SharedMemory/RemoteGUIHelper.h @@ -38,6 +38,7 @@ struct RemoteGUIHelper : public GUIHelperInterface virtual void removeAllGraphicsInstances(); virtual void removeGraphicsInstance(int graphicsUid); virtual void changeRGBAColor(int instanceUid, const double rgbaColor[4]); + virtual void changeScaling(int instanceUid, const double scaling[3]); virtual Common2dCanvasInterface* get2dCanvasInterface(); diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 3ab8776c9..b892df048 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -215,6 +215,7 @@ enum EnumInitPoseFlags INIT_POSE_HAS_BASE_LINEAR_VELOCITY = 8, INIT_POSE_HAS_BASE_ANGULAR_VELOCITY = 16, INIT_POSE_HAS_JOINT_VELOCITY = 32, + INIT_POSE_HAS_SCALING=64, }; ///InitPoseArgs is mainly to initialize (teleport) the robot in a particular position @@ -228,6 +229,7 @@ struct InitPoseArgs double m_initialStateQ[MAX_DEGREE_OF_FREEDOM]; int m_hasInitialStateQdot[MAX_DEGREE_OF_FREEDOM]; double m_initialStateQdot[MAX_DEGREE_OF_FREEDOM]; + double m_scaling[3]; }; struct RequestDebugLinesArgs diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index e5c97d7fd..f0789b258 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -4891,6 +4891,67 @@ static PyObject* pybullet_resetBasePositionAndOrientation(PyObject* self, return Py_None; } +static PyObject* pybullet_changeScaling(PyObject* self, + PyObject* args, PyObject* keywds) +{ + { + int bodyUniqueId; + PyObject* scalingObj; + double scaling[3]; + + b3PhysicsClientHandle sm = 0; + + int physicsClientId = 0; + static char* kwlist[] = { "bodyUniqueId", "scaling", "physicsClientId", NULL }; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "iO|i", kwlist, &bodyUniqueId, &scalingObj, &physicsClientId)) + { + return NULL; + } + sm = getPhysicsClient(physicsClientId); + if (sm == 0) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + { + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + + { + PyObject* seq; + int len, i; + seq = PySequence_Fast(scalingObj, "expected a sequence"); + len = PySequence_Size(scalingObj); + if (len == 3) + { + for (i = 0; i < 3; i++) + { + scaling[i] = pybullet_internalGetFloatFromSequence(seq, i); + } + } + else + { + PyErr_SetString(SpamError, "scaling needs a 3 coordinates [x,y,z]."); + Py_DECREF(seq); + return NULL; + } + Py_DECREF(seq); + } + + + + commandHandle = b3CreatePoseCommandInit(sm, bodyUniqueId); + b3CreatePoseCommandSetBaseScaling(commandHandle, scaling); + + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + } + } + Py_INCREF(Py_None); + return Py_None; +} + + // Get the a single joint info for a specific bodyUniqueId // // Args: @@ -12218,6 +12279,12 @@ static PyMethodDef SpamMethods[] = { "Reset the world position and orientation of the base of the object " "instantaneously, not through physics simulation. (x,y,z) position vector " "and (x,y,z,w) quaternion orientation."}, + + { "unsupportedChangeScaling", + (PyCFunction)pybullet_changeScaling, METH_VARARGS | METH_KEYWORDS, + "Change the scaling of the base of an object." + "Warning: unsupported rudimentary feature that has many limitations." + }, {"getBaseVelocity", (PyCFunction)pybullet_getBaseVelocity, METH_VARARGS | METH_KEYWORDS,