From 1a4783a581062dd8b8eecd5f406adb2dede070ec Mon Sep 17 00:00:00 2001 From: WenlongLu Date: Thu, 29 Oct 2020 23:34:34 -0700 Subject: [PATCH 1/5] Support kinematic multibody base --- .../CollisionDispatch/btCollisionObject.h | 1 + .../Featherstone/btMultiBody.cpp | 185 ++++++++++++------ src/BulletDynamics/Featherstone/btMultiBody.h | 50 ++++- .../Featherstone/btMultiBodyDynamicsWorld.cpp | 12 ++ .../Featherstone/btMultiBodyDynamicsWorld.h | 2 + 5 files changed, 179 insertions(+), 71 deletions(-) diff --git a/src/BulletCollision/CollisionDispatch/btCollisionObject.h b/src/BulletCollision/CollisionDispatch/btCollisionObject.h index 3a1c271af..e085c4089 100644 --- a/src/BulletCollision/CollisionDispatch/btCollisionObject.h +++ b/src/BulletCollision/CollisionDispatch/btCollisionObject.h @@ -127,6 +127,7 @@ public: enum CollisionFlags { + CF_DYNAMIC_OBJECT = 0, CF_STATIC_OBJECT = 1, CF_KINEMATIC_OBJECT = 2, CF_NO_CONTACT_RESPONSE = 4, diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp index 9862bd2e2..558562f60 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBody.cpp @@ -673,6 +673,28 @@ btScalar *btMultiBody::getJointTorqueMultiDof(int i) return &m_links[i].m_jointTorque[0]; } +bool btMultiBody::hasFixedBase() const +{ + return m_fixedBase || getBaseCollider()->isStaticObject(); +} + +bool btMultiBody::isBaseStaticOrKinematic() const +{ + return m_fixedBase || getBaseCollider()->isStaticOrKinematicObject(); +} + +bool btMultiBody::isBaseKinematic() const +{ + return getBaseCollider() && getBaseCollider()->isKinematicObject(); +} + +void btMultiBody::setBaseDynamicType(int dynamicType) +{ + int oldFlags = getBaseCollider()->getCollisionFlags(); + oldFlags &= ~(btCollisionObject::CF_STATIC_OBJECT | btCollisionObject::CF_KINEMATIC_OBJECT); + getBaseCollider()->setCollisionFlags(oldFlags | dynamicType); +} + inline btMatrix3x3 outerProduct(const btVector3 &v0, const btVector3 &v1) //renamed it from vecMulVecTranspose (http://en.wikipedia.org/wiki/Outer_product); maybe it should be moved to btVector3 like dot and cross? { btVector3 row0 = btVector3( @@ -798,7 +820,7 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar //create the vector of spatial velocity of the base by transforming global-coor linear and angular velocities into base-local coordinates spatVel[0].setVector(rot_from_parent[0] * base_omega, rot_from_parent[0] * base_vel); - if (m_fixedBase) + if (isBaseStaticOrKinematic()) { zeroAccSpatFrc[0].setZero(); } @@ -1049,7 +1071,7 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar // Second 'upward' loop // (part of TreeForwardDynamics in Mirtich) - if (m_fixedBase) + if (isBaseStaticOrKinematic()) { spatAcc[0].setZero(); } @@ -1434,7 +1456,7 @@ void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar // Fill in zero_acc // -- set to force/torque on the base, zero otherwise - if (m_fixedBase) + if (isBaseStaticOrKinematic()) { zeroAccSpatFrc[0].setZero(); } @@ -1496,7 +1518,7 @@ void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar // Second 'upward' loop // (part of TreeForwardDynamics in Mirtich) - if (m_fixedBase) + if (isBaseStaticOrKinematic()) { spatAcc[0].setZero(); } @@ -1552,23 +1574,26 @@ void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar void btMultiBody::predictPositionsMultiDof(btScalar dt) { int num_links = getNumLinks(); - // step position by adding dt * velocity - //btVector3 v = getBaseVel(); - //m_basePos += dt * v; - // - btScalar *pBasePos; - btScalar *pBaseVel = &m_realBuf[3]; //note: the !pqd case assumes m_realBuf holds with base velocity at 3,4,5 (should be wrapped for safety) + if(!isBaseKinematic()) + { + // step position by adding dt * velocity + //btVector3 v = getBaseVel(); + //m_basePos += dt * v; + // + btScalar *pBasePos; + btScalar *pBaseVel = &m_realBuf[3]; //note: the !pqd case assumes m_realBuf holds with base velocity at 3,4,5 (should be wrapped for safety) - // reset to current position - for (int i = 0; i < 3; ++i) - { - m_basePos_interpolate[i] = m_basePos[i]; - } - pBasePos = m_basePos_interpolate; - - pBasePos[0] += dt * pBaseVel[0]; - pBasePos[1] += dt * pBaseVel[1]; - pBasePos[2] += dt * pBaseVel[2]; + // reset to current position + for (int i = 0; i < 3; ++i) + { + m_basePos_interpolate[i] = m_basePos[i]; + } + pBasePos = m_basePos_interpolate; + + pBasePos[0] += dt * pBaseVel[0]; + pBasePos[1] += dt * pBaseVel[1]; + pBasePos[2] += dt * pBaseVel[2]; + } /////////////////////////////// //local functor for quaternion integration (to avoid error prone redundancy) @@ -1619,26 +1644,29 @@ void btMultiBody::predictPositionsMultiDof(btScalar dt) //pQuatUpdateFun(getBaseOmega(), m_baseQuat, true, dt); // - btScalar *pBaseQuat; + if(!isBaseKinematic()) + { + btScalar *pBaseQuat; - // reset to current orientation - for (int i = 0; i < 4; ++i) - { - m_baseQuat_interpolate[i] = m_baseQuat[i]; - } - pBaseQuat = m_baseQuat_interpolate; + // reset to current orientation + for (int i = 0; i < 4; ++i) + { + m_baseQuat_interpolate[i] = m_baseQuat[i]; + } + pBaseQuat = m_baseQuat_interpolate; - btScalar *pBaseOmega = &m_realBuf[0]; //note: the !pqd case assumes m_realBuf starts with base omega (should be wrapped for safety) - // - btQuaternion baseQuat; - baseQuat.setValue(pBaseQuat[0], pBaseQuat[1], pBaseQuat[2], pBaseQuat[3]); - btVector3 baseOmega; - baseOmega.setValue(pBaseOmega[0], pBaseOmega[1], pBaseOmega[2]); - pQuatUpdateFun(baseOmega, baseQuat, true, dt); - pBaseQuat[0] = baseQuat.x(); - pBaseQuat[1] = baseQuat.y(); - pBaseQuat[2] = baseQuat.z(); - pBaseQuat[3] = baseQuat.w(); + btScalar *pBaseOmega = &m_realBuf[0]; //note: the !pqd case assumes m_realBuf starts with base omega (should be wrapped for safety) + // + btQuaternion baseQuat; + baseQuat.setValue(pBaseQuat[0], pBaseQuat[1], pBaseQuat[2], pBaseQuat[3]); + btVector3 baseOmega; + baseOmega.setValue(pBaseOmega[0], pBaseOmega[1], pBaseOmega[2]); + pQuatUpdateFun(baseOmega, baseQuat, true, dt); + pBaseQuat[0] = baseQuat.x(); + pBaseQuat[1] = baseQuat.y(); + pBaseQuat[2] = baseQuat.z(); + pBaseQuat[3] = baseQuat.w(); + } // Finally we can update m_jointPos for each of the m_links for (int i = 0; i < num_links; ++i) @@ -1705,16 +1733,19 @@ void btMultiBody::predictPositionsMultiDof(btScalar dt) void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd) { int num_links = getNumLinks(); - // step position by adding dt * velocity - //btVector3 v = getBaseVel(); - //m_basePos += dt * v; - // - btScalar *pBasePos = (pq ? &pq[4] : m_basePos); - btScalar *pBaseVel = (pqd ? &pqd[3] : &m_realBuf[3]); //note: the !pqd case assumes m_realBuf holds with base velocity at 3,4,5 (should be wrapped for safety) - - pBasePos[0] += dt * pBaseVel[0]; - pBasePos[1] += dt * pBaseVel[1]; - pBasePos[2] += dt * pBaseVel[2]; + if(!isBaseKinematic()) + { + // step position by adding dt * velocity + //btVector3 v = getBaseVel(); + //m_basePos += dt * v; + // + btScalar *pBasePos = (pq ? &pq[4] : m_basePos); + btScalar *pBaseVel = (pqd ? &pqd[3] : &m_realBuf[3]); //note: the !pqd case assumes m_realBuf holds with base velocity at 3,4,5 (should be wrapped for safety) + + pBasePos[0] += dt * pBaseVel[0]; + pBasePos[1] += dt * pBaseVel[1]; + pBasePos[2] += dt * pBaseVel[2]; + } /////////////////////////////// //local functor for quaternion integration (to avoid error prone redundancy) @@ -1765,22 +1796,25 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd //pQuatUpdateFun(getBaseOmega(), m_baseQuat, true, dt); // - btScalar *pBaseQuat = pq ? pq : m_baseQuat; - btScalar *pBaseOmega = pqd ? pqd : &m_realBuf[0]; //note: the !pqd case assumes m_realBuf starts with base omega (should be wrapped for safety) - // - btQuaternion baseQuat; - baseQuat.setValue(pBaseQuat[0], pBaseQuat[1], pBaseQuat[2], pBaseQuat[3]); - btVector3 baseOmega; - baseOmega.setValue(pBaseOmega[0], pBaseOmega[1], pBaseOmega[2]); - pQuatUpdateFun(baseOmega, baseQuat, true, dt); - pBaseQuat[0] = baseQuat.x(); - pBaseQuat[1] = baseQuat.y(); - pBaseQuat[2] = baseQuat.z(); - pBaseQuat[3] = baseQuat.w(); + if(!isBaseKinematic()) + { + btScalar *pBaseQuat = pq ? pq : m_baseQuat; + btScalar *pBaseOmega = pqd ? pqd : &m_realBuf[0]; //note: the !pqd case assumes m_realBuf starts with base omega (should be wrapped for safety) + // + btQuaternion baseQuat; + baseQuat.setValue(pBaseQuat[0], pBaseQuat[1], pBaseQuat[2], pBaseQuat[3]); + btVector3 baseOmega; + baseOmega.setValue(pBaseOmega[0], pBaseOmega[1], pBaseOmega[2]); + pQuatUpdateFun(baseOmega, baseQuat, true, dt); + pBaseQuat[0] = baseQuat.x(); + pBaseQuat[1] = baseQuat.y(); + pBaseQuat[2] = baseQuat.z(); + pBaseQuat[3] = baseQuat.w(); - //printf("pBaseOmega = %.4f %.4f %.4f\n", pBaseOmega->x(), pBaseOmega->y(), pBaseOmega->z()); - //printf("pBaseVel = %.4f %.4f %.4f\n", pBaseVel->x(), pBaseVel->y(), pBaseVel->z()); - //printf("baseQuat = %.4f %.4f %.4f %.4f\n", pBaseQuat->x(), pBaseQuat->y(), pBaseQuat->z(), pBaseQuat->w()); + //printf("pBaseOmega = %.4f %.4f %.4f\n", pBaseOmega->x(), pBaseOmega->y(), pBaseOmega->z()); + //printf("pBaseVel = %.4f %.4f %.4f\n", pBaseVel->x(), pBaseVel->y(), pBaseVel->z()); + //printf("baseQuat = %.4f %.4f %.4f %.4f\n", pBaseQuat->x(), pBaseQuat->y(), pBaseQuat->z(), pBaseQuat->w()); + } if (pq) pq += 7; @@ -2137,8 +2171,15 @@ void btMultiBody::updateCollisionObjectInterpolationWorldTransforms(btAlignedObj world_to_local.resize(getNumLinks() + 1); local_origin.resize(getNumLinks() + 1); - world_to_local[0] = getInterpolateWorldToBaseRot(); - local_origin[0] = getInterpolateBasePos(); + if(isBaseKinematic()){ + world_to_local[0] = getWorldToBaseRot(); + local_origin[0] = getBasePos(); + } + else + { + world_to_local[0] = getInterpolateWorldToBaseRot(); + local_origin[0] = getInterpolateBasePos(); + } if (getBaseCollider()) { @@ -2284,3 +2325,17 @@ const char *btMultiBody::serialize(void *dataBuffer, class btSerializer *seriali return btMultiBodyDataName; } + +void btMultiBody::saveKinematicState(btScalar timeStep) +{ + //todo: clamp to some (user definable) safe minimum timestep, to limit maximum angular/linear velocities + if (timeStep != btScalar(0.)) + { + btVector3 linearVelocity, angularVelocity; + btTransformUtil::calculateVelocity(getInterpolateBaseWorldTransform(), getBaseWorldTransform(), timeStep, linearVelocity, angularVelocity); + setBaseVel(linearVelocity); + setBaseOmega(angularVelocity); + setInterpolateBaseWorldTransform(getBaseWorldTransform()); + printf("angular = %f %f %f\n",angularVelocity.getX(),angularVelocity.getY(),angularVelocity.getZ()); + } +} diff --git a/src/BulletDynamics/Featherstone/btMultiBody.h b/src/BulletDynamics/Featherstone/btMultiBody.h index f2acfab9b..cf965047e 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.h +++ b/src/BulletDynamics/Featherstone/btMultiBody.h @@ -210,7 +210,13 @@ public: void setBasePos(const btVector3 &pos) { m_basePos = pos; - m_basePos_interpolate = pos; + if(!isBaseKinematic()) + m_basePos_interpolate = pos; + } + + void setInterpolateBasePos(const btVector3 &pos) + { + m_basePos_interpolate = pos; } void setBaseWorldTransform(const btTransform &tr) @@ -227,17 +233,39 @@ public: return tr; } + void setInterpolateBaseWorldTransform(const btTransform &tr) + { + setInterpolateBasePos(tr.getOrigin()); + setInterpolateWorldToBaseRot(tr.getRotation().inverse()); + } + + btTransform getInterpolateBaseWorldTransform() const + { + btTransform tr; + tr.setOrigin(getInterpolateBasePos()); + tr.setRotation(getInterpolateWorldToBaseRot().inverse()); + return tr; + } + void setBaseVel(const btVector3 &vel) { m_realBuf[3] = vel[0]; m_realBuf[4] = vel[1]; m_realBuf[5] = vel[2]; } + void setWorldToBaseRot(const btQuaternion &rot) { m_baseQuat = rot; //m_baseQuat asumed to ba alias!? - m_baseQuat_interpolate = rot; + if(!isBaseKinematic()) + m_baseQuat_interpolate = rot; } + + void setInterpolateWorldToBaseRot(const btQuaternion &rot) + { + m_baseQuat_interpolate = rot; + } + void setBaseOmega(const btVector3 &omega) { m_realBuf[0] = omega[0]; @@ -245,6 +273,8 @@ public: m_realBuf[2] = omega[2]; } + void saveKinematicState(btScalar timeStep); + // // get/set pos/vel for child m_links (i = 0 to num_links-1) // @@ -520,14 +550,22 @@ public: void goToSleep(); void checkMotionAndSleepIfRequired(btScalar timestep); - bool hasFixedBase() const - { - return m_fixedBase; - } + bool hasFixedBase() const; + + bool isBaseKinematic() const; + + bool isBaseStaticOrKinematic() const; + + // set the dynamic type in the base's collision flags. + void setBaseDynamicType(int dynamicType); void setFixedBase(bool fixedBase) { m_fixedBase = fixedBase; + if(m_fixedBase) + setBaseDynamicType(btCollisionObject::CF_STATIC_OBJECT); + else + setBaseDynamicType(btCollisionObject::CF_DYNAMIC_OBJECT); } int getCompanionId() const diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp index f599c9ccb..fef95f0c4 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp @@ -869,6 +869,18 @@ void btMultiBodyDynamicsWorld::serializeMultiBodies(btSerializer* serializer) } } } + +void btMultiBodyDynamicsWorld::saveKinematicState(btScalar timeStep) +{ + btDiscreteDynamicsWorld::saveKinematicState(timeStep); + for(int i = 0; i < m_multiBodies.size(); i++) + { + btMultiBody* body = m_multiBodies[i]; + if(body->isBaseKinematic()) + body->saveKinematicState(timeStep); + } +} + // //void btMultiBodyDynamicsWorld::setSplitIslands(bool split) //{ diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h index 9ac46f4b6..d2d76c8b9 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h @@ -120,5 +120,7 @@ public: virtual void solveExternalForces(btContactSolverInfo& solverInfo); virtual void solveInternalConstraints(btContactSolverInfo& solverInfo); void buildIslands(); + + virtual void saveKinematicState(btScalar timeStep); }; #endif //BT_MULTIBODY_DYNAMICS_WORLD_H From b95f9d3afb2275c3ec32a400dd3b3a438b60d734 Mon Sep 17 00:00:00 2001 From: WenlongLu Date: Thu, 29 Oct 2020 23:34:53 -0700 Subject: [PATCH 2/5] Add kinematic multibody base example --- examples/ExampleBrowser/CMakeLists.txt | 3 +- examples/ExampleBrowser/ExampleEntries.cpp | 2 + .../MultiBody/KinematicMultiBodyExample.cpp | 189 ++++++++++++++++++ .../MultiBody/KinematicMultiBodyExample.h | 21 ++ 4 files changed, 214 insertions(+), 1 deletion(-) create mode 100644 examples/MultiBody/KinematicMultiBodyExample.cpp create mode 100644 examples/MultiBody/KinematicMultiBodyExample.h diff --git a/examples/ExampleBrowser/CMakeLists.txt b/examples/ExampleBrowser/CMakeLists.txt index 50a0b4f6e..0e92aa203 100644 --- a/examples/ExampleBrowser/CMakeLists.txt +++ b/examples/ExampleBrowser/CMakeLists.txt @@ -350,7 +350,8 @@ SET(BulletExampleBrowser_SRCS ../MultiBody/TestJointTorqueSetup.h ../MultiBody/InvertedPendulumPDControl.cpp ../MultiBody/InvertedPendulumPDControl.h - ../MultiBody/MultiBodyConstraintFeedback.cpp + ../MultiBody/MultiBodyConstraintFeedback.cpp + ../MultiBody/KinematicMultiBodyExample.cpp ../SoftDemo/SoftDemo.cpp ../SoftDemo/SoftDemo.h ../DeformableDemo/DeformableContact.cpp diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp index d6933444a..9f8ab035c 100644 --- a/examples/ExampleBrowser/ExampleEntries.cpp +++ b/examples/ExampleBrowser/ExampleEntries.cpp @@ -33,6 +33,7 @@ #include "../MultiBody/MultiBodyConstraintFeedback.h" #include "../MultiBody/MultiDofDemo.h" #include "../MultiBody/InvertedPendulumPDControl.h" +#include "../MultiBody/KinematicMultiBodyExample.h" #include "../RigidBody/RigidBodySoftContact.h" #include "../RigidBody/KinematicRigidBodyExample.h" @@ -156,6 +157,7 @@ static ExampleEntry gDefaultExamples[] = ExampleEntry(1, "Constraint Feedback", "The example shows how to receive joint reaction forces in a btMultiBody. Also the applied impulse is available for a btMultiBodyJointMotor", MultiBodyConstraintFeedbackCreateFunc), ExampleEntry(1, "Inverted Pendulum PD", "Keep an inverted pendulum up using open loop PD control", InvertedPendulumPDControlCreateFunc), ExampleEntry(1, "MultiBody Soft Contact", "Using the error correction parameter (ERP) and constraint force mixing (CFM) values for contacts to simulate compliant contact.", MultiBodySoftContactCreateFunc, 0), + ExampleEntry(1, "Kinematic MultiBody", "Let the user set the transform, the physics engine computes the velocity for one-way contact and friction interaction.", KinematicMultiBodyExampleCreateFunc), ExampleEntry(0, "Physics Client-Server"), ExampleEntry(1, "Physics Server", "Create a physics server that communicates with a physics client over shared memory. You can connect to the server using pybullet, a PhysicsClient or a UDP/TCP Bridge.", diff --git a/examples/MultiBody/KinematicMultiBodyExample.cpp b/examples/MultiBody/KinematicMultiBodyExample.cpp new file mode 100644 index 000000000..c6da2dd96 --- /dev/null +++ b/examples/MultiBody/KinematicMultiBodyExample.cpp @@ -0,0 +1,189 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2015 Google Inc. http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "KinematicMultiBodyExample.h" +//#define USE_MOTIONSTATE 1 +#include "btBulletDynamicsCommon.h" +#include "LinearMath/btTransformUtil.h" +#define ARRAY_SIZE_Y 5 +#define ARRAY_SIZE_X 5 +#define ARRAY_SIZE_Z 5 + +#include "LinearMath/btVector3.h" +#include "LinearMath/btAlignedObjectArray.h" +#include "../OpenGLWindow/ShapeData.h" + +#include "../CommonInterfaces/CommonMultiBodyBase.h" + +namespace { + +void kinematicPreTickCallback(btDynamicsWorld* world, btScalar deltaTime) +{ + btMultiBody* groundBody = (btMultiBody*)world->getWorldUserInfo(); + btTransform predictedTrans; + btVector3 linearVelocity(0, 0, 0); + btVector3 angularVelocity(0, 0.1, 0); + btTransformUtil::integrateTransform(groundBody->getBaseWorldTransform(), linearVelocity, angularVelocity, deltaTime, predictedTrans); + groundBody->setBaseWorldTransform(predictedTrans); + //groundBody->setBaseVel(linearVelocity); + //groundBody->setBaseOmega(angularVelocity); +} + +struct KinematicMultiBodyExample : public CommonMultiBodyBase +{ + btMultiBody* m_groundBody; + + KinematicMultiBodyExample(struct GUIHelperInterface* helper) + : CommonMultiBodyBase(helper), + m_groundBody(0) + { + } + + virtual void stepSimulation(float deltaTime) + { + if (m_dynamicsWorld) + { + m_dynamicsWorld->stepSimulation(deltaTime); + } + } + + virtual ~KinematicMultiBodyExample() {} + virtual void initPhysics(); + void resetCamera() + { + float dist = 4; + float pitch = -30; + float yaw = 50; + float targetPos[3] = {0, 0, 0}; + m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]); + } +}; + +void KinematicMultiBodyExample::initPhysics() +{ + m_guiHelper->setUpAxis(1); + + createEmptyDynamicsWorld(); + m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); + + if (m_dynamicsWorld->getDebugDrawer()) + m_dynamicsWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawWireframe + btIDebugDraw::DBG_DrawContactPoints); + + ///create a few basic rigid bodies + btScalar halfExtentsX = 10.0; + btScalar halfExtentsY = 0.1; + btScalar halfExtentsZ = 10.0; + + btBoxShape* groundShape = createBoxShape(btVector3(btScalar(10.), btScalar(0.1), btScalar(10.))); + btTransform groundTransform; + groundTransform.setIdentity(); + groundTransform.setOrigin(btVector3(0, -halfExtentsY, 0)); + m_collisionShapes.push_back(groundShape); + + + + { + bool floating = false; + int numLinks = 1; + bool canSleep = false; + btVector3 baseInertiaDiag(0.f, 0.f, 0.f); + float baseMass = 1.f; + + if (baseMass) + { + btCollisionShape* pTempBox = new btBoxShape(btVector3(10, 10, 10)); + pTempBox->calculateLocalInertia(baseMass, baseInertiaDiag); + delete pTempBox; + } + btTransform startTransform; + startTransform.setIdentity(); + + m_groundBody = new btMultiBody(numLinks, baseMass, baseInertiaDiag, !floating, canSleep); + m_groundBody->setBasePos(startTransform.getOrigin()); + m_groundBody->setWorldToBaseRot(startTransform.getRotation()); + // btVector3 vel(0, 0, 0); + // m_groundBody->setBaseVel(vel); + m_groundBody->finalizeMultiDof(); + m_dynamicsWorld->addMultiBody(m_groundBody); + + + btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(m_groundBody, -1); + col->setCollisionShape(groundShape); + bool isDynamic = (baseMass > 0 && floating); + int collisionFilterGroup = isDynamic ? int(btBroadphaseProxy::DefaultFilter) : int(btBroadphaseProxy::StaticFilter); + int collisionFilterMask = isDynamic ? int(btBroadphaseProxy::AllFilter) : int(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter); + m_dynamicsWorld->addCollisionObject(col, collisionFilterGroup, collisionFilterMask); //, 2,1+2); + m_groundBody->setBaseCollider(col); + m_groundBody->setBaseDynamicType(btCollisionObject::CF_KINEMATIC_OBJECT); + + //btVector3 color(0.0, 0.0, 0.5); + //m_guiHelper->createCollisionObjectGraphicsObject(col, color); + //m_groundBody->forceActivationState(DISABLE_DEACTIVATION); + //m_groundBody->setCollisionFlags(btCollisionObject::CF_KINEMATIC_OBJECT | btCollisionObject::CF_STATIC_OBJECT); + + } + m_dynamicsWorld->setInternalTickCallback(kinematicPreTickCallback, m_groundBody, true); + + if (1) + { + //create a few dynamic rigidbodies + // Re-using the same collision is better for memory usage and performance + + btBoxShape* colShape = createBoxShape(btVector3(.1, .1, .1)); + + //btCollisionShape* colShape = new btSphereShape(btScalar(1.)); + m_collisionShapes.push_back(colShape); + + /// Create Dynamic Objects + btTransform startTransform; + startTransform.setIdentity(); + + btScalar mass(1.f); + + //rigidbody is dynamic if and only if mass is non zero, otherwise static + bool isDynamic = (mass != 0.f); + + btVector3 localInertia(0, 0, 0); + if (isDynamic) + colShape->calculateLocalInertia(mass, localInertia); + + for (int k = 0; k < ARRAY_SIZE_Y; k++) + { + for (int i = 0; i < ARRAY_SIZE_X; i++) + { + for (int j = 0; j < ARRAY_SIZE_Z; j++) + { + startTransform.setOrigin(btVector3( + btScalar(0.2 * i), + btScalar(2 + .2 * k), + btScalar(0.2 * j))); + + createRigidBody(mass, startTransform, colShape); + } + } + } + } + + m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); +} + +} + +CommonExampleInterface* KinematicMultiBodyExampleCreateFunc(CommonExampleOptions& options) +{ + return new KinematicMultiBodyExample(options.m_guiHelper); +} + +B3_STANDALONE_EXAMPLE(KinematicMultiBodyExampleCreateFunc) diff --git a/examples/MultiBody/KinematicMultiBodyExample.h b/examples/MultiBody/KinematicMultiBodyExample.h new file mode 100644 index 000000000..1d6f44bb3 --- /dev/null +++ b/examples/MultiBody/KinematicMultiBodyExample.h @@ -0,0 +1,21 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2015 Google Inc. http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef KINEMATIC_MULTI_BODY_EXAMPLE_H +#define KINEMATIC_MULTI_BODY_EXAMPLE_H + +class CommonExampleInterface* KinematicMultiBodyExampleCreateFunc(struct CommonExampleOptions& options); + +#endif //KINEMATIC_MULTI_BODY_EXAMPLE_H From 2fc72577f1cb8d6724ba84c56385287e8a3f810a Mon Sep 17 00:00:00 2001 From: WenlongLu Date: Thu, 29 Oct 2020 23:35:42 -0700 Subject: [PATCH 3/5] minor example cleanup --- examples/MultiBody/KinematicMultiBodyExample.cpp | 11 ----------- 1 file changed, 11 deletions(-) diff --git a/examples/MultiBody/KinematicMultiBodyExample.cpp b/examples/MultiBody/KinematicMultiBodyExample.cpp index c6da2dd96..0eac4c0a3 100644 --- a/examples/MultiBody/KinematicMultiBodyExample.cpp +++ b/examples/MultiBody/KinematicMultiBodyExample.cpp @@ -37,8 +37,6 @@ void kinematicPreTickCallback(btDynamicsWorld* world, btScalar deltaTime) btVector3 angularVelocity(0, 0.1, 0); btTransformUtil::integrateTransform(groundBody->getBaseWorldTransform(), linearVelocity, angularVelocity, deltaTime, predictedTrans); groundBody->setBaseWorldTransform(predictedTrans); - //groundBody->setBaseVel(linearVelocity); - //groundBody->setBaseOmega(angularVelocity); } struct KinematicMultiBodyExample : public CommonMultiBodyBase @@ -113,8 +111,6 @@ void KinematicMultiBodyExample::initPhysics() m_groundBody = new btMultiBody(numLinks, baseMass, baseInertiaDiag, !floating, canSleep); m_groundBody->setBasePos(startTransform.getOrigin()); m_groundBody->setWorldToBaseRot(startTransform.getRotation()); - // btVector3 vel(0, 0, 0); - // m_groundBody->setBaseVel(vel); m_groundBody->finalizeMultiDof(); m_dynamicsWorld->addMultiBody(m_groundBody); @@ -128,19 +124,12 @@ void KinematicMultiBodyExample::initPhysics() m_groundBody->setBaseCollider(col); m_groundBody->setBaseDynamicType(btCollisionObject::CF_KINEMATIC_OBJECT); - //btVector3 color(0.0, 0.0, 0.5); - //m_guiHelper->createCollisionObjectGraphicsObject(col, color); - //m_groundBody->forceActivationState(DISABLE_DEACTIVATION); - //m_groundBody->setCollisionFlags(btCollisionObject::CF_KINEMATIC_OBJECT | btCollisionObject::CF_STATIC_OBJECT); - } m_dynamicsWorld->setInternalTickCallback(kinematicPreTickCallback, m_groundBody, true); - if (1) { //create a few dynamic rigidbodies // Re-using the same collision is better for memory usage and performance - btBoxShape* colShape = createBoxShape(btVector3(.1, .1, .1)); //btCollisionShape* colShape = new btSphereShape(btScalar(1.)); From 622b44d136b243291ce621698bd651cf77c650c2 Mon Sep 17 00:00:00 2001 From: WenlongLu Date: Tue, 10 Nov 2020 20:32:26 -0800 Subject: [PATCH 4/5] fix crashes from accessing base collider when it is not available. --- src/BulletDynamics/Featherstone/btMultiBody.cpp | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp index 558562f60..8c361cf8b 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBody.cpp @@ -675,12 +675,12 @@ btScalar *btMultiBody::getJointTorqueMultiDof(int i) bool btMultiBody::hasFixedBase() const { - return m_fixedBase || getBaseCollider()->isStaticObject(); + return m_fixedBase || (getBaseCollider() && getBaseCollider()->isStaticObject()); } bool btMultiBody::isBaseStaticOrKinematic() const { - return m_fixedBase || getBaseCollider()->isStaticOrKinematicObject(); + return m_fixedBase || (getBaseCollider() && getBaseCollider()->isStaticOrKinematicObject()); } bool btMultiBody::isBaseKinematic() const @@ -690,9 +690,11 @@ bool btMultiBody::isBaseKinematic() const void btMultiBody::setBaseDynamicType(int dynamicType) { - int oldFlags = getBaseCollider()->getCollisionFlags(); - oldFlags &= ~(btCollisionObject::CF_STATIC_OBJECT | btCollisionObject::CF_KINEMATIC_OBJECT); - getBaseCollider()->setCollisionFlags(oldFlags | dynamicType); + if(getBaseCollider()) { + int oldFlags = getBaseCollider()->getCollisionFlags(); + oldFlags &= ~(btCollisionObject::CF_STATIC_OBJECT | btCollisionObject::CF_KINEMATIC_OBJECT); + getBaseCollider()->setCollisionFlags(oldFlags | dynamicType); + } } inline btMatrix3x3 outerProduct(const btVector3 &v0, const btVector3 &v1) //renamed it from vecMulVecTranspose (http://en.wikipedia.org/wiki/Outer_product); maybe it should be moved to btVector3 like dot and cross? @@ -2336,6 +2338,5 @@ void btMultiBody::saveKinematicState(btScalar timeStep) setBaseVel(linearVelocity); setBaseOmega(angularVelocity); setInterpolateBaseWorldTransform(getBaseWorldTransform()); - printf("angular = %f %f %f\n",angularVelocity.getX(),angularVelocity.getY(),angularVelocity.getZ()); } } From 5a1977b176f7398430324a0763657d4fd135a75e Mon Sep 17 00:00:00 2001 From: WenlongLu Date: Wed, 11 Nov 2020 14:12:47 -0800 Subject: [PATCH 5/5] Remove the uninitialized child link in the example --- examples/MultiBody/KinematicMultiBodyExample.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/MultiBody/KinematicMultiBodyExample.cpp b/examples/MultiBody/KinematicMultiBodyExample.cpp index 0eac4c0a3..3daeedca8 100644 --- a/examples/MultiBody/KinematicMultiBodyExample.cpp +++ b/examples/MultiBody/KinematicMultiBodyExample.cpp @@ -94,7 +94,7 @@ void KinematicMultiBodyExample::initPhysics() { bool floating = false; - int numLinks = 1; + int numLinks = 0; bool canSleep = false; btVector3 baseInertiaDiag(0.f, 0.f, 0.f); float baseMass = 1.f;