diff --git a/data/plane_with_restitution.urdf b/data/plane_with_restitution.urdf new file mode 100644 index 000000000..5f2ebdd19 --- /dev/null +++ b/data/plane_with_restitution.urdf @@ -0,0 +1,29 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/data/sphere_with_restitution.urdf b/data/sphere_with_restitution.urdf new file mode 100644 index 000000000..63a6682a5 --- /dev/null +++ b/data/sphere_with_restitution.urdf @@ -0,0 +1,33 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index d438a906f..77fb802ee 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -392,6 +392,18 @@ int b3PhysicsParamSetEnableFileCaching(b3SharedMemoryCommandHandle commandHandle } +int b3PhysicsParamSetRestitutionVelocityThreshold(b3SharedMemoryCommandHandle commandHandle, double restitutionVelocityThreshold) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS); + + command->m_physSimParamArgs.m_restitutionVelocityThreshold = restitutionVelocityThreshold; + command->m_updateFlags |= SIM_PARAM_UPDATE_RESTITUTION_VELOCITY_THRESHOLD ; + return 0; + +} + + int b3PhysicsParamSetNumSolverIterations(b3SharedMemoryCommandHandle commandHandle, int numSolverIterations) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; @@ -1297,6 +1309,42 @@ int b3ChangeDynamicsInfoSetLateralFriction(b3SharedMemoryCommandHandle commandHa return 0; } +int b3ChangeDynamicsInfoSetSpinningFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double friction) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command->m_type == CMD_CHANGE_DYNAMICS_INFO); + command->m_changeDynamicsInfoArgs.m_bodyUniqueId = bodyUniqueId; + command->m_changeDynamicsInfoArgs.m_linkIndex = linkIndex; + command->m_changeDynamicsInfoArgs.m_spinningFriction = friction; + command->m_updateFlags |= CHANGE_DYNAMICS_INFO_SET_SPINNING_FRICTION; + return 0; + +} +int b3ChangeDynamicsInfoSetRollingFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double friction) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command->m_type == CMD_CHANGE_DYNAMICS_INFO); + command->m_changeDynamicsInfoArgs.m_bodyUniqueId = bodyUniqueId; + command->m_changeDynamicsInfoArgs.m_linkIndex = linkIndex; + command->m_changeDynamicsInfoArgs.m_rollingFriction = friction; + command->m_updateFlags |= CHANGE_DYNAMICS_INFO_SET_ROLLING_FRICTION; + return 0; + +} + + +int b3ChangeDynamicsInfoSetRestitution(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double restitution) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command->m_type == CMD_CHANGE_DYNAMICS_INFO); + command->m_changeDynamicsInfoArgs.m_bodyUniqueId = bodyUniqueId; + command->m_changeDynamicsInfoArgs.m_linkIndex = linkIndex; + command->m_changeDynamicsInfoArgs.m_restitution = restitution; + command->m_updateFlags |= CHANGE_DYNAMICS_INFO_SET_RESTITUTION; + return 0; + +} + b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3PhysicsClientHandle physClient, int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, struct b3JointInfo* info) { PhysicsClient* cl = (PhysicsClient* ) physClient; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 6dfe21718..0a730ee51 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -84,7 +84,12 @@ int b3GetDynamicsInfo(b3SharedMemoryStatusHandle statusHandle, struct b3Dynamics b3SharedMemoryCommandHandle b3InitChangeDynamicsInfo(b3PhysicsClientHandle physClient); int b3ChangeDynamicsInfoSetMass(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double mass); int b3ChangeDynamicsInfoSetLateralFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double lateralFriction); - +int b3ChangeDynamicsInfoSetSpinningFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double friction); +int b3ChangeDynamicsInfoSetRollingFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double friction); +int b3ChangeDynamicsInfoSetRestitution(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double restitution); + + + b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3PhysicsClientHandle physClient, int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, struct b3JointInfo* info); ///return a unique id for the user constraint, after successful creation, or -1 for an invalid constraint id @@ -222,6 +227,9 @@ int b3PhysicsParamSetSplitImpulsePenetrationThreshold(b3SharedMemoryCommandHandl int b3PhysicsParamSetContactBreakingThreshold(b3SharedMemoryCommandHandle commandHandle, double contactBreakingThreshold); int b3PhysicsParamSetMaxNumCommandsPer1ms(b3SharedMemoryCommandHandle commandHandle, int maxNumCmdPer1ms); int b3PhysicsParamSetEnableFileCaching(b3SharedMemoryCommandHandle commandHandle, int enableFileCaching); +int b3PhysicsParamSetRestitutionVelocityThreshold(b3SharedMemoryCommandHandle commandHandle, double restitutionVelocityThreshold); + + //b3PhysicsParamSetInternalSimFlags is for internal/temporary/easter-egg/experimental demo purposes //Use at own risk: magic things may or my not happen when calling this API diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 02635029e..50925cb14 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -3915,52 +3915,122 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { BT_PROFILE("CMD_CHANGE_DYNAMICS_INFO"); - if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_MASS) - { - int bodyUniqueId = clientCmd.m_changeDynamicsInfoArgs.m_bodyUniqueId; - int linkIndex = clientCmd.m_changeDynamicsInfoArgs.m_linkIndex; - double mass = clientCmd.m_changeDynamicsInfoArgs.m_mass; - btAssert(bodyUniqueId >= 0); - btAssert(linkIndex >= -1); + int bodyUniqueId = clientCmd.m_changeDynamicsInfoArgs.m_bodyUniqueId; + int linkIndex = clientCmd.m_changeDynamicsInfoArgs.m_linkIndex; + double mass = clientCmd.m_changeDynamicsInfoArgs.m_mass; + double lateralFriction = clientCmd.m_changeDynamicsInfoArgs.m_lateralFriction; + double spinningFriction = clientCmd.m_changeDynamicsInfoArgs.m_spinningFriction; + double rollingFriction = clientCmd.m_changeDynamicsInfoArgs.m_rollingFriction; + double restitution = clientCmd.m_changeDynamicsInfoArgs.m_restitution; + btAssert(bodyUniqueId >= 0); + btAssert(linkIndex >= -1); - InteralBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); - if (body && body->m_multiBody) + InteralBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); + + if (body && body->m_multiBody) + { + btMultiBody* mb = body->m_multiBody; + if (linkIndex == -1) { - btMultiBody* mb = body->m_multiBody; - if (linkIndex == -1) + if (mb->getBaseCollider()) { - mb->setBaseMass(mass); + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_RESTITUTION) + { + mb->getBaseCollider()->setRestitution(restitution); + } + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LATERAL_FRICTION) + { + mb->getBaseCollider()->setFriction(lateralFriction); + } + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_SPINNING_FRICTION) + { + mb->getBaseCollider()->setSpinningFriction(spinningFriction); + } + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ROLLING_FRICTION) + { + mb->getBaseCollider()->setRollingFriction(rollingFriction); + } } - else + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_MASS) + { + mb->setBaseMass(mass); + if (mb->getBaseCollider() && mb->getBaseCollider()->getCollisionShape()) + { + btVector3 localInertia; + mb->getBaseCollider()->getCollisionShape()->calculateLocalInertia(mass,localInertia); + mb->setBaseInertia(localInertia); + } + } + } + else + { + if (mb->getLinkCollider(linkIndex)) + { + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_RESTITUTION) + { + mb->getLinkCollider(linkIndex)->setRestitution(restitution); + } + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_SPINNING_FRICTION) + { + mb->getLinkCollider(linkIndex)->setSpinningFriction(spinningFriction); + } + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ROLLING_FRICTION) + { + mb->getLinkCollider(linkIndex)->setRollingFriction(rollingFriction); + } + + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LATERAL_FRICTION) + { + mb->getLinkCollider(linkIndex)->setFriction(lateralFriction); + } + + + } + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_MASS) { mb->getLink(linkIndex).m_mass = mass; + if (mb->getLinkCollider(linkIndex) && mb->getLinkCollider(linkIndex)->getCollisionShape()) + { + btVector3 localInertia; + mb->getLinkCollider(linkIndex)->getCollisionShape()->calculateLocalInertia(mass,localInertia); + mb->getLink(linkIndex).m_inertiaLocal = localInertia; + } + + } + } + } else + { + if (body && body->m_rigidBody) + { + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_RESTITUTION) + { + body->m_rigidBody->setRestitution(restitution); + } + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LATERAL_FRICTION) + { + body->m_rigidBody->setFriction(lateralFriction); + } + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_SPINNING_FRICTION) + { + body->m_rigidBody->setSpinningFriction(spinningFriction); + } + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ROLLING_FRICTION) + { + body->m_rigidBody->setRollingFriction(rollingFriction); + } + + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_MASS) + { + btVector3 localInertia; + if (body->m_rigidBody->getCollisionShape()) + { + body->m_rigidBody->getCollisionShape()->calculateLocalInertia(mass,localInertia); + } + body->m_rigidBody->setMassProps(mass,localInertia); } } } - if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LATERAL_FRICTION) - { - int bodyUniqueId = clientCmd.m_changeDynamicsInfoArgs.m_bodyUniqueId; - int linkIndex = clientCmd.m_changeDynamicsInfoArgs.m_linkIndex; - double lateralFriction = clientCmd.m_changeDynamicsInfoArgs.m_lateralFriction; - btAssert(bodyUniqueId >= 0); - btAssert(linkIndex >= -1); - - InteralBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); - if (body && body->m_multiBody) - { - btMultiBody* mb = body->m_multiBody; - if (linkIndex == -1) - { - mb->getBaseCollider()->setFriction(lateralFriction); - } - else - { - mb->getLinkCollider(linkIndex)->setFriction(lateralFriction); - } - } - } - SharedMemoryStatus& serverCmd =serverStatusOut; serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED; @@ -4070,6 +4140,12 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { m_data->m_dynamicsWorld->getSolverInfo().m_erp2 = clientCmd.m_physSimParamArgs.m_defaultContactERP; } + if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_RESTITUTION_VELOCITY_THRESHOLD) + { + m_data->m_dynamicsWorld->getSolverInfo().m_restitutionVelocityThreshold = clientCmd.m_physSimParamArgs.m_restitutionVelocityThreshold; + } + + if (clientCmd.m_updateFlags&SIM_PARAM_ENABLE_FILE_CACHING) { diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 0c3a44de1..ed686fb0c 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -114,6 +114,9 @@ enum EnumChangeDynamicsInfoFlags CHANGE_DYNAMICS_INFO_SET_MASS=1, CHANGE_DYNAMICS_INFO_SET_COM=2, CHANGE_DYNAMICS_INFO_SET_LATERAL_FRICTION=4, + CHANGE_DYNAMICS_INFO_SET_SPINNING_FRICTION=8, + CHANGE_DYNAMICS_INFO_SET_ROLLING_FRICTION=16, + CHANGE_DYNAMICS_INFO_SET_RESTITUTION=32, }; struct ChangeDynamicsInfoArgs @@ -123,6 +126,9 @@ struct ChangeDynamicsInfoArgs double m_mass; double m_COM[3]; double m_lateralFriction; + double m_spinningFriction; + double m_rollingFriction; + double m_restitution; }; struct GetDynamicsInfoArgs @@ -354,6 +360,8 @@ enum EnumSimParamUpdateFlags SIM_PARAM_UPDATE_CONTACT_BREAKING_THRESHOLD = 1024, SIM_PARAM_MAX_CMD_PER_1MS = 2048, SIM_PARAM_ENABLE_FILE_CACHING = 4096, + SIM_PARAM_UPDATE_RESTITUTION_VELOCITY_THRESHOLD = 8192, + }; @@ -387,6 +395,7 @@ struct SendPhysicsSimulationParameters double m_defaultContactERP; int m_collisionFilterMode; int m_enableFileCaching; + double m_restitutionVelocityThreshold; }; struct LoadBunnyArgs diff --git a/examples/pybullet/examples/restitution.py b/examples/pybullet/examples/restitution.py new file mode 100644 index 000000000..89f7483ec --- /dev/null +++ b/examples/pybullet/examples/restitution.py @@ -0,0 +1,38 @@ +#you can set the restitution (bouncyness) of an object in the URDF file +#or using changeDynamics + +import pybullet as p +import time + +p.connect(p.GUI) +restitutionId = p.addUserDebugParameter("restitution",0,1,1) +restitutionVelocityThresholdId = p.addUserDebugParameter("res. vel. threshold",0,3,0.2) + +lateralFrictionId = p.addUserDebugParameter("lateral friction",0,1,0.5) +spinningFrictionId = p.addUserDebugParameter("spinning friction",0,1,0.03) +rollingFrictionId = p.addUserDebugParameter("rolling friction",0,1,0.03) + +plane = p.loadURDF("plane_with_restitution.urdf") +sphere = p.loadURDF("sphere_with_restitution.urdf",[0,0,2]) + +p.setRealTimeSimulation(1) +p.setGravity(0,0,-10) +while (1): + restitution = p.readUserDebugParameter(restitutionId) + restitutionVelocityThreshold = p.readUserDebugParameter(restitutionVelocityThresholdId) + p.setPhysicsEngineParameter(restitutionVelocityThreshold=restitutionVelocityThreshold) + + lateralFriction = p.readUserDebugParameter(lateralFrictionId) + spinningFriction = p.readUserDebugParameter(spinningFrictionId) + rollingFriction = p.readUserDebugParameter(rollingFrictionId) + p.changeDynamics(plane,-1,lateralFriction=1) + p.changeDynamics(sphere,-1,lateralFriction=lateralFriction) + p.changeDynamics(sphere,-1,spinningFriction=spinningFriction) + p.changeDynamics(sphere,-1,rollingFriction=rollingFriction) + + p.changeDynamics(plane,-1,restitution=restitution) + p.changeDynamics(sphere,-1,restitution=restitution) + pos,orn=p.getBasePositionAndOrientation(sphere) + #print("pos=") + #print(pos) + time.sleep(0.01) \ No newline at end of file diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 788bd151e..1ea93fb7e 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -620,11 +620,15 @@ static PyObject* pybullet_changeDynamicsInfo(PyObject* self, PyObject* args, PyO int linkIndex = -2; double mass = -1; double lateralFriction = -1; + double spinningFriction= -1; + double rollingFriction = -1; + double restitution = -1; + b3PhysicsClientHandle sm = 0; int physicsClientId = 0; - static char* kwlist[] = {"bodyUniqueId", "linkIndex", "mass", "lateralFriction", "physicsClientId", NULL}; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|ddi", kwlist, &bodyUniqueId, &linkIndex,&mass, &lateralFriction, &physicsClientId)) + static char* kwlist[] = {"bodyUniqueId", "linkIndex", "mass", "lateralFriction", "spinningFriction", "rollingFriction","restitution", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|dddddi", kwlist, &bodyUniqueId, &linkIndex,&mass, &lateralFriction, &spinningFriction, &rollingFriction, &restitution, &physicsClientId)) { return NULL; } @@ -644,12 +648,23 @@ static PyObject* pybullet_changeDynamicsInfo(PyObject* self, PyObject* args, PyO { b3ChangeDynamicsInfoSetMass(command, bodyUniqueId, linkIndex, mass); } - if (lateralFriction >= 0) { b3ChangeDynamicsInfoSetLateralFriction(command, bodyUniqueId, linkIndex, lateralFriction); } - + if (spinningFriction>=0) + { + b3ChangeDynamicsInfoSetSpinningFriction(command, bodyUniqueId, linkIndex,spinningFriction); + } + if (rollingFriction>=0) + { + b3ChangeDynamicsInfoSetRollingFriction(command, bodyUniqueId, linkIndex,rollingFriction); + } + + if (restitution>=0) + { + b3ChangeDynamicsInfoSetRestitution(command, bodyUniqueId, linkIndex, restitution); + } statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); } @@ -725,14 +740,14 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar double contactBreakingThreshold = -1; int maxNumCmdPer1ms = -2; int enableFileCaching = -1; - + double restitutionVelocityThreshold=-1; b3PhysicsClientHandle sm = 0; int physicsClientId = 0; - static char* kwlist[] = {"fixedTimeStep", "numSolverIterations", "useSplitImpulse", "splitImpulsePenetrationThreshold", "numSubSteps", "collisionFilterMode", "contactBreakingThreshold", "maxNumCmdPer1ms", "enableFileCaching","physicsClientId", NULL}; + static char* kwlist[] = {"fixedTimeStep", "numSolverIterations", "useSplitImpulse", "splitImpulsePenetrationThreshold", "numSubSteps", "collisionFilterMode", "contactBreakingThreshold", "maxNumCmdPer1ms", "enableFileCaching","restitutionVelocityThreshold", "physicsClientId", NULL}; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diidiidiii", kwlist, &fixedTimeStep, &numSolverIterations, &useSplitImpulse, &splitImpulsePenetrationThreshold, &numSubSteps, - &collisionFilterMode, &contactBreakingThreshold, &maxNumCmdPer1ms, &enableFileCaching, &physicsClientId)) + if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diidiidiidi", kwlist, &fixedTimeStep, &numSolverIterations, &useSplitImpulse, &splitImpulsePenetrationThreshold, &numSubSteps, + &collisionFilterMode, &contactBreakingThreshold, &maxNumCmdPer1ms, &enableFileCaching, &restitutionVelocityThreshold, &physicsClientId)) { return NULL; } @@ -783,6 +798,10 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar b3PhysicsParamSetMaxNumCommandsPer1ms(command, maxNumCmdPer1ms); } + if (restitutionVelocityThreshold>=0) + { + b3PhysicsParamSetRestitutionVelocityThreshold(command, restitutionVelocityThreshold); + } if (enableFileCaching>=0) { b3PhysicsParamSetEnableFileCaching(command, enableFileCaching); @@ -921,7 +940,7 @@ static PyObject* pybullet_loadURDF(PyObject* self, PyObject* args, PyObject* key startOrnZ, startOrnW); if (useMaximalCoordinates>=0) { - b3LoadUrdfCommandSetUseMultiBody(command, useMaximalCoordinates); + b3LoadUrdfCommandSetUseMultiBody(command, useMaximalCoordinates==0); } if (useFixedBase) { diff --git a/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h b/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h index f3d4d45af..28d0c1dd4 100644 --- a/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h +++ b/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h @@ -62,6 +62,7 @@ struct btContactSolverInfoData btScalar m_maxGyroscopicForce; btScalar m_singleAxisRollingFrictionThreshold; btScalar m_leastSquaresResidualThreshold; + btScalar m_restitutionVelocityThreshold; }; @@ -97,6 +98,7 @@ struct btContactSolverInfo : public btContactSolverInfoData m_maxGyroscopicForce = 100.f; ///it is only used for 'explicit' version of gyroscopic force m_singleAxisRollingFrictionThreshold = 1e30f;///if the velocity is above this threshold, it will use a single constraint row (axis), otherwise 3 rows. m_leastSquaresResidualThreshold = 0.f; + m_restitutionVelocityThreshold = 0.2f;//if the relative velocity is below this threshold, there is zero restitution } }; diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp index 7a0ed924d..fb9d9335f 100644 --- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp +++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp @@ -506,8 +506,12 @@ void btSequentialImpulseConstraintSolver::initSolverBody(btSolverBody* solverBod -btScalar btSequentialImpulseConstraintSolver::restitutionCurve(btScalar rel_vel, btScalar restitution) +btScalar btSequentialImpulseConstraintSolver::restitutionCurve(btScalar rel_vel, btScalar restitution, btScalar velocityThreshold) { + //printf("rel_vel =%f\n", rel_vel); + if (btFabs(rel_vel)