From 1e134545116a321920b008c74683b13384a254d5 Mon Sep 17 00:00:00 2001 From: erwin coumans Date: Tue, 24 Mar 2015 15:55:05 -0700 Subject: [PATCH] Added experimental implicit gyroscopic force implementation, one by Michael Ewert, and another by Cooper (from OpenDE) Will also add Erin Catto's local implicit version from the GDC 2015 tutorial Added demo for btGeneric6DofSpring2Constraint, thanks to Gabor Puhr Add gfxBridge.autogenerateGraphicsObjects method for Bullet 2 demos in new framework (need to implement all Bullet 2 collision shape types...) Use 1,1,1 for local scaling in btStaticPlaneShape --- Demos/GyroscopicDemo/GyroscopicSetup.cpp | 63 +++ Demos/GyroscopicDemo/GyroscopicSetup.h | 14 + Demos/SerializeDemo/SerializeSetup.cpp | 2 +- Demos3/AllBullet2Demos/BulletDemoEntries.h | 9 +- Demos3/AllBullet2Demos/CMakeLists.txt | 4 + Demos3/AllBullet2Demos/premake4.lua | 4 + .../ConstraintDemo/ConstraintPhysicsSetup.cpp | 5 +- .../ConstraintDemo/Dof6Spring2Setup.cpp | 453 ++++++++++++++++++ .../bullet2/ConstraintDemo/Dof6Spring2Setup.h | 19 + .../MultiBodyDemo/MultiBodyVehicle.cpp | 8 +- .../Bullet2RigidBodyDemo.cpp | 84 +++- btgui/Bullet3AppSupport/CommonPhysicsSetup.h | 3 + .../CollisionShapes/btStaticPlaneShape.cpp | 2 +- .../ConstraintSolver/btContactSolverInfo.h | 4 +- .../btSequentialImpulseConstraintSolver.cpp | 15 +- src/BulletDynamics/Dynamics/btRigidBody.cpp | 171 ++++++- src/BulletDynamics/Dynamics/btRigidBody.h | 17 +- 17 files changed, 852 insertions(+), 25 deletions(-) create mode 100644 Demos/GyroscopicDemo/GyroscopicSetup.cpp create mode 100644 Demos/GyroscopicDemo/GyroscopicSetup.h create mode 100644 Demos3/bullet2/ConstraintDemo/Dof6Spring2Setup.cpp create mode 100644 Demos3/bullet2/ConstraintDemo/Dof6Spring2Setup.h diff --git a/Demos/GyroscopicDemo/GyroscopicSetup.cpp b/Demos/GyroscopicDemo/GyroscopicSetup.cpp new file mode 100644 index 000000000..af0cb123b --- /dev/null +++ b/Demos/GyroscopicDemo/GyroscopicSetup.cpp @@ -0,0 +1,63 @@ +#include "GyroscopicSetup.h" + +void GyroscopicSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge) +{ + gfxBridge.setUpAxis(2); + createEmptyDynamicsWorld(); + m_dynamicsWorld->setGravity(btVector3(0, 0, 0)); + gfxBridge.createPhysicsDebugDrawer(m_dynamicsWorld); + + //btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(50.),btScalar(50.),btScalar(0.5))); + btCollisionShape* groundShape = new btStaticPlaneShape(btVector3(0, 0, 1), 0); + + m_collisionShapes.push_back(groundShape); + btTransform groundTransform; + groundTransform.setIdentity(); + groundTransform.setOrigin(btVector3(0, 0, 0)); + btRigidBody* groundBody; + groundBody = createRigidBody(0, groundTransform, groundShape); + groundBody->setFriction(btSqrt(2)); + btVector3 positions[4] = { + btVector3(0.8, -5, 4), + btVector3(0.8, -2, 4), + btVector3(0.8, 2, 4), + btVector3(0.8, 5, 4) + + }; + int gyroflags[4] = { + 0,//none, no gyroscopic term + BT_ENABLE_GYROPSCOPIC_FORCE_EXPLICIT, + BT_ENABLE_GYROPSCOPIC_FORCE_IMPLICIT_EWERT, + BT_ENABLE_GYROPSCOPIC_FORCE_IMPLICIT_COOPER, + }; + + for (int i = 0; i<4; i++) + { + btCylinderShapeZ* top = new btCylinderShapeZ(btVector3(1, 1, 0.125)); + btCapsuleShapeZ* pin = new btCapsuleShapeZ(0.05, 1.5); + top->setMargin(0.01); + pin->setMargin(0.01); + btCompoundShape* compound = new btCompoundShape(); + compound->addChildShape(btTransform::getIdentity(), top); + compound->addChildShape(btTransform::getIdentity(), pin); + btVector3 localInertia; + top->calculateLocalInertia(1, localInertia); + btRigidBody* body = new btRigidBody(1, 0, compound, localInertia); + btTransform tr; + tr.setIdentity(); + tr.setOrigin(positions[i]); + body->setCenterOfMassTransform(tr); + body->setAngularVelocity(btVector3(1, 17, 3)); + body->setLinearVelocity(btVector3(0, 0, 0)); + body->setFriction(btSqrt(1)); + m_dynamicsWorld->addRigidBody(body); + body->setFlags(gyroflags[i]); + + body->setDamping(0.00001f, 0.0001f); + + + } + + gfxBridge.autogenerateGraphicsObjects(m_dynamicsWorld); +} + diff --git a/Demos/GyroscopicDemo/GyroscopicSetup.h b/Demos/GyroscopicDemo/GyroscopicSetup.h new file mode 100644 index 000000000..1a797c8c6 --- /dev/null +++ b/Demos/GyroscopicDemo/GyroscopicSetup.h @@ -0,0 +1,14 @@ + +#ifndef GYROSCOPIC_SETUP_H +#define GYROSCOPIC_SETUP_H + +#include "Bullet3AppSupport/CommonRigidBodySetup.h" + +struct GyroscopicSetup : public CommonRigidBodySetup +{ + virtual void initPhysics(GraphicsPhysicsBridge& gfxBridge); + +}; + + +#endif //GYROSCOPIC_SETUP_H diff --git a/Demos/SerializeDemo/SerializeSetup.cpp b/Demos/SerializeDemo/SerializeSetup.cpp index 32e748cd8..e0773b58e 100644 --- a/Demos/SerializeDemo/SerializeSetup.cpp +++ b/Demos/SerializeDemo/SerializeSetup.cpp @@ -52,7 +52,7 @@ void SerializeSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge) gfxBridge.setUpAxis(1); } - + gfxBridge.autogenerateGraphicsObjects(m_dynamicsWorld); } void SerializeSetup::stepSimulation(float deltaTime) diff --git a/Demos3/AllBullet2Demos/BulletDemoEntries.h b/Demos3/AllBullet2Demos/BulletDemoEntries.h index ebda8397c..581c60d42 100644 --- a/Demos3/AllBullet2Demos/BulletDemoEntries.h +++ b/Demos3/AllBullet2Demos/BulletDemoEntries.h @@ -12,11 +12,12 @@ #include "../bullet2/FeatherstoneMultiBodyDemo/BulletMultiBodyDemos.h" #include "../bullet2/FeatherstoneMultiBodyDemo/MultiDofDemo.h" - +#include "../bullet2/ConstraintDemo/Dof6Spring2Setup.h" #include "../bullet2/RagdollDemo/RagdollDemo.h" #include "../bullet2/LuaDemo/LuaPhysicsSetup.h" #include "../bullet2/ChainDemo/ChainDemo.h" #include "../../Demos/CcdPhysicsDemo/CcdPhysicsSetup.h" +#include "../../Demos/GyroscopicDemo/GyroscopicSetup.h" #include "../bullet2/ConstraintDemo/ConstraintPhysicsSetup.h" #include "../ImportURDFDemo/ImportURDFSetup.h" #include "../ImportObjDemo/ImportObjSetup.h" @@ -55,8 +56,10 @@ MYCREATEFUNC(MultiBodyVehicle); MYCREATEFUNC2(LuaDemoCreateFunc,LuaPhysicsSetup); MYCREATEFUNC(Serialize); MYCREATEFUNC(CcdPhysics); +MYCREATEFUNC(Gyroscopic); MYCREATEFUNC(KinematicObject); MYCREATEFUNC(ConstraintPhysics); +MYCREATEFUNC(Dof6Spring2); MYCREATEFUNC(ImportUrdf); MYCREATEFUNC2(ImportObjCreateFunc,ImportObjSetup); MYCREATEFUNC2(ImportSTLCreateFunc,ImportSTLSetup); @@ -94,8 +97,10 @@ static BulletDemoEntry allDemos[]= {1,"Raytracer",RaytracerPhysicsCreateFunc}, {1,"BasicDemo",BasicDemo::MyCreateFunc}, { 1, "CcdDemo", CcdPhysicsCreateFunc }, + { 1, "Gyroscopic", GyroscopicCreateFunc }, { 1, "Kinematic", KinematicObjectCreateFunc }, - { 1, "Constraints", ConstraintPhysicsCreateFunc }, + { 1, "HingeMotor", ConstraintPhysicsCreateFunc }, + {1,"6DofSpring2", Dof6Spring2CreateFunc}, { 1, "LuaDemo",LuaDemoCreateFunc}, {0,"File Formats", 0}, diff --git a/Demos3/AllBullet2Demos/CMakeLists.txt b/Demos3/AllBullet2Demos/CMakeLists.txt index 605750912..a846a092e 100644 --- a/Demos3/AllBullet2Demos/CMakeLists.txt +++ b/Demos3/AllBullet2Demos/CMakeLists.txt @@ -16,6 +16,8 @@ SET(App_AllBullet2Demos_SRCS ../../Demos/CcdPhysicsDemo/CcdPhysicsSetup.h ../../Demos/Raytracer/RaytracerSetup.cpp ../../Demos/Raytracer/RaytracerSetup.h + ../../Demos/GyroscopicDemo/GyroscopicSetup.cpp + ../../Demos/GyroscopicDemo/GyroscopicSetup.h ../../Demos/SerializeDemo/SerializeSetup.cpp ../../Extras/Serialize/BulletFileLoader/bChunk.cpp ../../Extras/Serialize/BulletFileLoader/bDNA.cpp @@ -27,6 +29,8 @@ SET(App_AllBullet2Demos_SRCS ../bullet2/MultiBodyDemo/MultiBodyVehicle.cpp ../bullet2/ConstraintDemo/ConstraintPhysicsSetup.cpp ../bullet2/ConstraintDemo/ConstraintPhysicsSetup.h + ../bullet2/ConstraintDemo/Dof6Spring2Setup.cpp + ../bullet2/ConstraintDemo/Dof6Spring2Setup.h ../bullet2/CoordinateFrameDemo/CoordinateFrameDemoPhysicsSetup.cpp ../bullet2/CoordinateFrameDemo/CoordinateFrameDemoPhysicsSetup.h ../bullet2/FeatherstoneMultiBodyDemo/BulletMultiBodyDemos.cpp diff --git a/Demos3/AllBullet2Demos/premake4.lua b/Demos3/AllBullet2Demos/premake4.lua index 7ff5a0396..17e17f8ac 100644 --- a/Demos3/AllBullet2Demos/premake4.lua +++ b/Demos3/AllBullet2Demos/premake4.lua @@ -57,6 +57,8 @@ "../../Demos/CcdPhysicsDemo/CcdPhysicsSetup.h", "../../Demos/Raytracer/RaytracerSetup.cpp", "../../Demos/Raytracer/RaytracerSetup.h", + "../../Demos/GyroscopicDemo/GyroscopicSetup.cpp", + "../../Demos/GyroscopicDemo/GyroscopicSetup.h", "../../Demos/SerializeDemo/SerializeSetup.cpp", "../../Extras/Serialize/BulletFileLoader/bChunk.cpp", "../../Extras/Serialize/BulletFileLoader/bDNA.cpp", @@ -66,6 +68,8 @@ "../../Extras/Serialize/BulletWorldImporter/btWorldImporter.cpp", "../bullet2/ConstraintDemo/ConstraintPhysicsSetup.cpp", "../bullet2/ConstraintDemo/ConstraintPhysicsSetup.h", + "../bullet2/ConstraintDemo/Dof6Spring2Setup.cpp", + "../bullet2/ConstraintDemo/Dof6Spring2Setup.h", -- "../bullet2/SoftDemo/SoftDemo.cpp", "../ImportColladaDemo/LoadMeshFromCollada.cpp", "../ImportColladaDemo/ImportColladaSetup.cpp", diff --git a/Demos3/bullet2/ConstraintDemo/ConstraintPhysicsSetup.cpp b/Demos3/bullet2/ConstraintDemo/ConstraintPhysicsSetup.cpp index b1bed5888..003ad00ae 100644 --- a/Demos3/bullet2/ConstraintDemo/ConstraintPhysicsSetup.cpp +++ b/Demos3/bullet2/ConstraintDemo/ConstraintPhysicsSetup.cpp @@ -72,7 +72,7 @@ void ConstraintPhysicsSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge) createEmptyDynamicsWorld(); gfxBridge.createPhysicsDebugDrawer(m_dynamicsWorld); -int mode = btIDebugDraw::DBG_DrawWireframe + int mode = btIDebugDraw::DBG_DrawWireframe +btIDebugDraw::DBG_DrawConstraints +btIDebugDraw::DBG_DrawConstraintLimits; m_dynamicsWorld->getDebugDrawer()->setDebugMode(mode); @@ -123,5 +123,6 @@ int mode = btIDebugDraw::DBG_DrawWireframe spDoorHinge->setDbgDrawSize(btScalar(5.f)); } - + + gfxBridge.autogenerateGraphicsObjects(m_dynamicsWorld); } diff --git a/Demos3/bullet2/ConstraintDemo/Dof6Spring2Setup.cpp b/Demos3/bullet2/ConstraintDemo/Dof6Spring2Setup.cpp new file mode 100644 index 000000000..78b8ebeac --- /dev/null +++ b/Demos3/bullet2/ConstraintDemo/Dof6Spring2Setup.cpp @@ -0,0 +1,453 @@ +#include "Dof6Spring2Setup.h" + +#include "btBulletDynamicsCommon.h" +#include "BulletDynamics/ConstraintSolver/btNNCGConstraintSolver.h" +#include "BulletDynamics/MLCPSolvers/btMLCPSolver.h" +#include "BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h" +#include "BulletDynamics/MLCPSolvers/btLemkeSolver.h" +#include "BulletDynamics/MLCPSolvers/btDantzigSolver.h" + +#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h" + + +#ifndef M_PI +#define M_PI 3.14159265358979323846 +#endif + +#ifndef M_PI_2 +#define M_PI_2 1.57079632679489661923 +#endif + +#ifndef M_PI_4 +#define M_PI_4 0.785398163397448309616 +#endif + + +extern float g_additionalBodyMass; + +//comment this out to compare with original spring constraint +#define USE_6DOF2 +#ifdef USE_6DOF2 + #define CONSTRAINT_TYPE btGeneric6DofSpring2Constraint + #define EXTRAPARAMS +#else + #define CONSTRAINT_TYPE btGeneric6DofSpringConstraint + #define EXTRAPARAMS ,true +#endif + +struct Dof6Spring2SetupInternalData +{ + btRigidBody* m_TranslateSpringBody; + btRigidBody* m_TranslateSpringBody2; + btRigidBody* m_RotateSpringBody; + btRigidBody* m_RotateSpringBody2; + btRigidBody* m_BouncingTranslateBody; + btRigidBody* m_MotorBody; + btRigidBody* m_ServoMotorBody; + btRigidBody* m_ChainLeftBody; + btRigidBody* m_ChainRightBody; + CONSTRAINT_TYPE* m_ServoMotorConstraint; + CONSTRAINT_TYPE* m_ChainLeftConstraint; + CONSTRAINT_TYPE* m_ChainRightConstraint; + + + float mDt; + + unsigned int frameID; + Dof6Spring2SetupInternalData() + : mDt(1./60.),frameID(0) + { + } +}; + +Dof6Spring2Setup::Dof6Spring2Setup() +{ + m_data = new Dof6Spring2SetupInternalData; +} +Dof6Spring2Setup::~Dof6Spring2Setup() +{ + exitPhysics(); + delete m_data; +} +void Dof6Spring2Setup::initPhysics(GraphicsPhysicsBridge& gfxBridge) +{ + // Setup the basic world + + m_collisionConfiguration = new btDefaultCollisionConfiguration(); + m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration); + btVector3 worldAabbMin(-10000,-10000,-10000); + btVector3 worldAabbMax(10000,10000,10000); + m_broadphase = new btAxisSweep3 (worldAabbMin, worldAabbMax); + +/////// uncomment the corresponding line to test a solver. + //m_solver = new btSequentialImpulseConstraintSolver; + m_solver = new btNNCGConstraintSolver; + //m_solver = new btMLCPSolver(new btSolveProjectedGaussSeidel()); + //m_solver = new btMLCPSolver(new btDantzigSolver()); + //m_solver = new btMLCPSolver(new btLemkeSolver()); + + m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher,m_broadphase,m_solver,m_collisionConfiguration); + m_dynamicsWorld->getDispatchInfo().m_useContinuous = true; + + m_dynamicsWorld->setGravity(btVector3(0,0,0)); + + // Setup a big ground box + { + btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(200.),btScalar(5.),btScalar(200.))); + btTransform groundTransform; + groundTransform.setIdentity(); + groundTransform.setOrigin(btVector3(0,-10,0)); +#define CREATE_GROUND_COLLISION_OBJECT 1 +#ifdef CREATE_GROUND_COLLISION_OBJECT + btCollisionObject* fixedGround = new btCollisionObject(); + fixedGround->setCollisionShape(groundShape); + fixedGround->setWorldTransform(groundTransform); + m_dynamicsWorld->addCollisionObject(fixedGround); +#else + localCreateRigidBody(btScalar(0.),groundTransform,groundShape); +#endif //CREATE_GROUND_COLLISION_OBJECT + } + + m_dynamicsWorld->getSolverInfo().m_numIterations = 100; + + btCollisionShape* shape; + btVector3 localInertia(0,0,0); + btDefaultMotionState* motionState; + btTransform bodyTransform; + btScalar mass; + btTransform localA; + btTransform localB; + CONSTRAINT_TYPE* constraint; + + //static body centered in the origo + mass = 0.0; + shape= new btBoxShape(btVector3(0.5,0.5,0.5)); + localInertia = btVector3(0,0,0); + bodyTransform.setIdentity(); + motionState = new btDefaultMotionState(bodyTransform); + btRigidBody* staticBody = new btRigidBody(mass,motionState,shape,localInertia); + +/////////// box with undamped translate spring attached to static body +/////////// the box should oscillate left-to-right forever + { + mass = 1.0; + shape= new btBoxShape(btVector3(0.5,0.5,0.5)); + shape->calculateLocalInertia(mass,localInertia); + bodyTransform.setIdentity(); + bodyTransform.setOrigin(btVector3(-2,0,-5)); + motionState = new btDefaultMotionState(bodyTransform); + m_data->m_TranslateSpringBody = new btRigidBody(mass,motionState,shape,localInertia); + m_data->m_TranslateSpringBody->setActivationState(DISABLE_DEACTIVATION); + m_dynamicsWorld->addRigidBody(m_data->m_TranslateSpringBody); + localA.setIdentity();localA.getOrigin() = btVector3(0,0,-5); + localB.setIdentity(); + constraint = new CONSTRAINT_TYPE(*staticBody, *m_data->m_TranslateSpringBody, localA, localB EXTRAPARAMS); + constraint->setLimit(0, 1,-1); + constraint->setLimit(1, 0, 0); + constraint->setLimit(2, 0, 0); + constraint->setLimit(3, 0, 0); + constraint->setLimit(4, 0, 0); + constraint->setLimit(5, 0, 0); + constraint->enableSpring(0, true); + constraint->setStiffness(0, 100); + #ifdef USE_6DOF2 + constraint->setDamping(0, 0); + #else + constraint->setDamping(0, 1); + #endif + constraint->setEquilibriumPoint(0, 0); + m_dynamicsWorld->addConstraint(constraint, true); + } + +/////////// box with rotate spring, attached to static body +/////////// box should swing (rotate) left-to-right forever + { + mass = 1.0; + shape= new btBoxShape(btVector3(0.5,0.5,0.5)); + shape->calculateLocalInertia(mass,localInertia); + bodyTransform.setIdentity(); + bodyTransform.getBasis().setEulerZYX(0,0,M_PI_2); + motionState = new btDefaultMotionState(bodyTransform); + m_data->m_RotateSpringBody = new btRigidBody(mass,motionState,shape,localInertia); + m_data->m_RotateSpringBody->setActivationState(DISABLE_DEACTIVATION); + m_dynamicsWorld->addRigidBody(m_data->m_RotateSpringBody); + localA.setIdentity();localA.getOrigin() = btVector3(0,0,0); + localB.setIdentity();localB.setOrigin(btVector3(0,0.5,0)); + constraint = new CONSTRAINT_TYPE(*staticBody, *m_data->m_RotateSpringBody, localA, localB EXTRAPARAMS); + constraint->setLimit(0, 0, 0); + constraint->setLimit(1, 0, 0); + constraint->setLimit(2, 0, 0); + constraint->setLimit(3, 0, 0); + constraint->setLimit(4, 0, 0); + constraint->setLimit(5, 1, -1); + constraint->enableSpring(5, true); + constraint->setStiffness(5, 100); + #ifdef USE_6DOF2 + constraint->setDamping(5, 0); + #else + constraint->setDamping(5, 1); + #endif + constraint->setEquilibriumPoint(0, 0); + m_dynamicsWorld->addConstraint(constraint, true); + } + +/////////// box with bouncing constraint, translation is bounced at the positive x limit, but not at the negative limit +/////////// bouncing can not be set independently at low and high limits, so two constraints will be created: one that defines the low (non bouncing) limit, and one that defines the high (bouncing) limit +/////////// the box should move to the left (as an impulse will be applied to it periodically) until it reaches its limit, then bounce back + { + mass = 1.0; + shape= new btBoxShape(btVector3(0.5,0.5,0.5)); + shape->calculateLocalInertia(mass,localInertia); + bodyTransform.setIdentity(); + bodyTransform.setOrigin(btVector3(0,0,-3)); + motionState = new btDefaultMotionState(bodyTransform); + m_data->m_BouncingTranslateBody = new btRigidBody(mass,motionState,shape,localInertia); + m_data->m_BouncingTranslateBody->setActivationState(DISABLE_DEACTIVATION); + m_data->m_BouncingTranslateBody->setDeactivationTime(btScalar(20000000)); + m_dynamicsWorld->addRigidBody(m_data->m_BouncingTranslateBody); + localA.setIdentity();localA.getOrigin() = btVector3(0,0,0); + localB.setIdentity(); + constraint = new CONSTRAINT_TYPE(*staticBody, *m_data->m_BouncingTranslateBody, localA, localB EXTRAPARAMS); + constraint->setLimit(0, -2, SIMD_INFINITY); + constraint->setLimit(1, 0, 0); + constraint->setLimit(2, -3, -3); + constraint->setLimit(3, 0, 0); + constraint->setLimit(4, 0, 0); + constraint->setLimit(5, 0, 0); + #ifdef USE_6DOF2 + constraint->setBounce(0,0); + #else //bounce is named restitution in 6dofspring, but not implemented for translational limit motor, so the following line has no effect + constraint->getTranslationalLimitMotor()->m_restitution = 0.0; + #endif + constraint->setParam(BT_CONSTRAINT_STOP_ERP,0.995,0); + constraint->setParam(BT_CONSTRAINT_STOP_CFM,0.0,0); + m_dynamicsWorld->addConstraint(constraint, true); + constraint = new CONSTRAINT_TYPE(*staticBody, *m_data->m_BouncingTranslateBody, localA, localB EXTRAPARAMS); + constraint->setLimit(0, -SIMD_INFINITY, 2); + constraint->setLimit(1, 0, 0); + constraint->setLimit(2, -3, -3); + constraint->setLimit(3, 0, 0); + constraint->setLimit(4, 0, 0); + constraint->setLimit(5, 0, 0); + #ifdef USE_6DOF2 + constraint->setBounce(0,1); + #else //bounce is named restitution in 6dofspring, but not implemented for translational limit motor, so the following line has no effect + constraint->getTranslationalLimitMotor()->m_restitution = 1.0; + #endif + constraint->setParam(BT_CONSTRAINT_STOP_ERP,0.995,0); + constraint->setParam(BT_CONSTRAINT_STOP_CFM,0.0,0); + m_dynamicsWorld->addConstraint(constraint, true); + } + +/////////// box with rotational motor, attached to static body +/////////// the box should rotate around the y axis + { + mass = 1.0; + shape= new btBoxShape(btVector3(0.5,0.5,0.5)); + shape->calculateLocalInertia(mass,localInertia); + bodyTransform.setIdentity(); + bodyTransform.setOrigin(btVector3(4,0,0)); + motionState = new btDefaultMotionState(bodyTransform); + m_data->m_MotorBody = new btRigidBody(mass,motionState,shape,localInertia); + m_data->m_MotorBody->setActivationState(DISABLE_DEACTIVATION); + m_dynamicsWorld->addRigidBody(m_data->m_MotorBody); + localA.setIdentity();localA.getOrigin() = btVector3(4,0,0); + localB.setIdentity(); + constraint = new CONSTRAINT_TYPE(*staticBody, *m_data->m_MotorBody, localA, localB EXTRAPARAMS); + constraint->setLimit(0, 0, 0); + constraint->setLimit(1, 0, 0); + constraint->setLimit(2, 0, 0); + constraint->setLimit(3, 0, 0); + constraint->setLimit(4, 0, 0); + constraint->setLimit(5, 1,-1); + #ifdef USE_6DOF2 + constraint->enableMotor(5,true); + constraint->setTargetVelocity(5,3.f); + constraint->setMaxMotorForce(5,10.f); + #else + constraint->getRotationalLimitMotor(2)->m_enableMotor = true; + constraint->getRotationalLimitMotor(2)->m_targetVelocity = 3.f; + constraint->getRotationalLimitMotor(2)->m_maxMotorForce = 10; + #endif + m_dynamicsWorld->addConstraint(constraint, true); + } + +/////////// box with rotational servo motor, attached to static body +/////////// the box should rotate around the y axis until it reaches its target +/////////// the target will be negated periodically + { + mass = 1.0; + shape= new btBoxShape(btVector3(0.5,0.5,0.5)); + shape->calculateLocalInertia(mass,localInertia); + bodyTransform.setIdentity(); + bodyTransform.setOrigin(btVector3(7,0,0)); + motionState = new btDefaultMotionState(bodyTransform); + m_data->m_ServoMotorBody = new btRigidBody(mass,motionState,shape,localInertia); + m_data->m_ServoMotorBody->setActivationState(DISABLE_DEACTIVATION); + m_dynamicsWorld->addRigidBody(m_data->m_ServoMotorBody); + localA.setIdentity();localA.getOrigin() = btVector3(7,0,0); + localB.setIdentity(); + constraint = new CONSTRAINT_TYPE(*staticBody, *m_data->m_ServoMotorBody, localA, localB EXTRAPARAMS); + constraint->setLimit(0, 0, 0); + constraint->setLimit(1, 0, 0); + constraint->setLimit(2, 0, 0); + constraint->setLimit(3, 0, 0); + constraint->setLimit(4, 0, 0); + constraint->setLimit(5, 1,-1); + #ifdef USE_6DOF2 + constraint->enableMotor(5,true); + constraint->setTargetVelocity(5,3.f); + constraint->setMaxMotorForce(5,10.f); + constraint->setServo(5,true); + constraint->setServoTarget(5, M_PI_2); + #else + constraint->getRotationalLimitMotor(2)->m_enableMotor = true; + constraint->getRotationalLimitMotor(2)->m_targetVelocity = 3.f; + constraint->getRotationalLimitMotor(2)->m_maxMotorForce = 10; + //servo motor is not implemented in 6dofspring constraint + #endif + m_dynamicsWorld->addConstraint(constraint, true); + m_data->m_ServoMotorConstraint = constraint; + } + +////////// chain of boxes linked together with fully limited rotational and translational constraints +////////// the chain will be pulled to the left and to the right periodically. They should strictly stick together. + { + btScalar limitConstraintStrength = 0.6; + int bodycount = 10; + btRigidBody* prevBody = 0; + for(int i = 0; i < bodycount; ++i) + { + mass = 1.0; + shape= new btBoxShape(btVector3(0.5,0.5,0.5)); + shape->calculateLocalInertia(mass,localInertia); + bodyTransform.setIdentity(); + bodyTransform.setOrigin(btVector3(- i,0,3)); + motionState = new btDefaultMotionState(bodyTransform); + btRigidBody* body = new btRigidBody(mass,motionState,shape,localInertia); + body->setActivationState(DISABLE_DEACTIVATION); + m_dynamicsWorld->addRigidBody(body); + if(prevBody != 0) + { + localB.setIdentity(); + localB.setOrigin(btVector3(0.5,0,0)); + btTransform localA; + localA.setIdentity(); + localA.setOrigin(btVector3(-0.5,0,0)); + CONSTRAINT_TYPE* constraint = new CONSTRAINT_TYPE(*prevBody, *body, localA, localB EXTRAPARAMS); + constraint->setLimit(0, -0.01, 0.01); + constraint->setLimit(1, 0, 0); + constraint->setLimit(2, 0, 0); + constraint->setLimit(3, 0, 0); + constraint->setLimit(4, 0, 0); + constraint->setLimit(5, 0, 0); + for(int a = 0; a < 6; ++a) + { + constraint->setParam(BT_CONSTRAINT_STOP_ERP,0.9,a); + constraint->setParam(BT_CONSTRAINT_STOP_CFM,0.0,a); + } + m_dynamicsWorld->addConstraint(constraint, true); + + if(i < bodycount - 1) + { + localA.setIdentity();localA.getOrigin() = btVector3(0,0,3); + localB.setIdentity(); + CONSTRAINT_TYPE* constraintZY = new CONSTRAINT_TYPE(*staticBody, *body, localA, localB EXTRAPARAMS); + constraintZY->setLimit(0, 1, -1); + m_dynamicsWorld->addConstraint(constraintZY, true); + } + } + else + { + localA.setIdentity();localA.getOrigin() = btVector3(bodycount,0,3); + localB.setIdentity(); + localB.setOrigin(btVector3(0,0,0)); + m_data->m_ChainLeftBody = body; + m_data->m_ChainLeftConstraint = new CONSTRAINT_TYPE(*staticBody, *body, localA, localB EXTRAPARAMS); + m_data->m_ChainLeftConstraint->setLimit(3,0,0); + m_data->m_ChainLeftConstraint->setLimit(4,0,0); + m_data->m_ChainLeftConstraint->setLimit(5,0,0); + for(int a = 0; a < 6; ++a) + { + m_data->m_ChainLeftConstraint->setParam(BT_CONSTRAINT_STOP_ERP,limitConstraintStrength,a); + m_data->m_ChainLeftConstraint->setParam(BT_CONSTRAINT_STOP_CFM,0.0,a); + } + m_dynamicsWorld->addConstraint(m_data->m_ChainLeftConstraint, true); + } + prevBody = body; + } + m_data->m_ChainRightBody = prevBody; + localA.setIdentity();localA.getOrigin() = btVector3(-bodycount,0,3); + localB.setIdentity(); + localB.setOrigin(btVector3(0,0,0)); + m_data->m_ChainRightConstraint = new CONSTRAINT_TYPE(*staticBody, *m_data->m_ChainRightBody, localA, localB EXTRAPARAMS); + m_data->m_ChainRightConstraint->setLimit(3,0,0); + m_data->m_ChainRightConstraint->setLimit(4,0,0); + m_data->m_ChainRightConstraint->setLimit(5,0,0); + for(int a = 0; a < 6; ++a) + { + m_data->m_ChainRightConstraint->setParam(BT_CONSTRAINT_STOP_ERP,limitConstraintStrength,a); + m_data->m_ChainRightConstraint->setParam(BT_CONSTRAINT_STOP_CFM,0.0,a); + } + } + gfxBridge.autogenerateGraphicsObjects(m_dynamicsWorld); +} + + +void Dof6Spring2Setup::animate() +{ + +/////// servo motor: flip its target periodically +#ifdef USE_6DOF2 + static float servoNextFrame = -1; + btScalar pos = m_data->m_ServoMotorConstraint->getRotationalLimitMotor(2)->m_currentPosition; + btScalar target = m_data->m_ServoMotorConstraint->getRotationalLimitMotor(2)->m_servoTarget; + if(servoNextFrame < 0) + { + m_data->m_ServoMotorConstraint->getRotationalLimitMotor(2)->m_servoTarget *= -1; + servoNextFrame = 3.0; + } + servoNextFrame -= m_data->mDt; +#endif + +/////// constraint chain: pull the chain left and right periodically + static float chainNextFrame = -1; + static bool left = true; + if(chainNextFrame < 0) + { + if(!left) + { + m_data->m_ChainRightBody->setActivationState(ACTIVE_TAG); + m_dynamicsWorld->removeConstraint(m_data->m_ChainRightConstraint); + m_dynamicsWorld->addConstraint(m_data->m_ChainLeftConstraint, true); + } + else + { + m_data->m_ChainLeftBody->setActivationState(ACTIVE_TAG); + m_dynamicsWorld->removeConstraint(m_data->m_ChainLeftConstraint); + m_dynamicsWorld->addConstraint(m_data->m_ChainRightConstraint, true); + } + chainNextFrame = 3.0; + left = !left; + } + chainNextFrame -= m_data->mDt; + +/////// bouncing constraint: push the box periodically + m_data->m_BouncingTranslateBody->setActivationState(ACTIVE_TAG); + static float bounceNextFrame = -1; + if(bounceNextFrame < 0) + { + m_data->m_BouncingTranslateBody->applyCentralImpulse(btVector3(10,0,0)); + bounceNextFrame = 3.0; + } + bounceNextFrame -= m_data->mDt; + + m_data->frameID++; +} + + +void Dof6Spring2Setup::stepSimulation(float deltaTime) +{ + animate(); + m_dynamicsWorld->stepSimulation(deltaTime); +} diff --git a/Demos3/bullet2/ConstraintDemo/Dof6Spring2Setup.h b/Demos3/bullet2/ConstraintDemo/Dof6Spring2Setup.h new file mode 100644 index 000000000..00980c500 --- /dev/null +++ b/Demos3/bullet2/ConstraintDemo/Dof6Spring2Setup.h @@ -0,0 +1,19 @@ +#ifndef GENERIC_6DOF_SPRING2_CONSTRAINT_DEMO_H +#define GENERIC_6DOF_SPRING2_CONSTRAINT_DEMO_H + +#include "Bullet3AppSupport/CommonRigidBodySetup.h" + +struct Dof6Spring2Setup : public CommonRigidBodySetup +{ + struct Dof6Spring2SetupInternalData* m_data; + + Dof6Spring2Setup(); + virtual ~Dof6Spring2Setup(); + virtual void initPhysics(GraphicsPhysicsBridge& gfxBridge); + + virtual void stepSimulation(float deltaTime); + + void animate(); +}; + +#endif //GENERIC_6DOF_SPRING2_CONSTRAINT_DEMO_H diff --git a/Demos3/bullet2/MultiBodyDemo/MultiBodyVehicle.cpp b/Demos3/bullet2/MultiBodyDemo/MultiBodyVehicle.cpp index e7823d281..7cb6eb744 100644 --- a/Demos3/bullet2/MultiBodyDemo/MultiBodyVehicle.cpp +++ b/Demos3/bullet2/MultiBodyDemo/MultiBodyVehicle.cpp @@ -215,18 +215,16 @@ void MultiBodyVehicleSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge) btBoxShape* box = new btBoxShape(groundHalfExtents); box->initializePolyhedralFeatures(); - gfxBridge.createCollisionShapeGraphicsObject(box); + btTransform start; start.setIdentity(); btVector3 groundOrigin(0,0,0); groundOrigin[upAxis]=-1.5; start.setOrigin(groundOrigin); btRigidBody* body = createRigidBody(0,start,box); - btVector4 color = colors[curColor]; - curColor++; - curColor&=3; - gfxBridge.createRigidBodyGraphicsObject(body,color); + } + gfxBridge.autogenerateGraphicsObjects(m_dynamicsWorld); } void MultiBodyVehicleSetup::stepSimulation(float deltaTime) diff --git a/btgui/Bullet3AppSupport/Bullet2RigidBodyDemo.cpp b/btgui/Bullet3AppSupport/Bullet2RigidBodyDemo.cpp index e68b95ba2..f08c79ad5 100644 --- a/btgui/Bullet3AppSupport/Bullet2RigidBodyDemo.cpp +++ b/btgui/Bullet3AppSupport/Bullet2RigidBodyDemo.cpp @@ -9,14 +9,25 @@ #include "MyDebugDrawer.h" #include "OpenGLWindow/GLInstanceGraphicsShape.h" +static btVector4 sColors[4] = +{ + btVector4(0.3,0.3,1,1), + btVector4(1,0,0,1), + btVector4(0,1,0,1), + btVector4(0,1,1,1), + //btVector4(1,1,0,1), +}; + + struct MyGraphicsPhysicsBridge : public GraphicsPhysicsBridge { CommonGraphicsApp* m_glApp; MyDebugDrawer* m_debugDraw; + int m_curColor; MyGraphicsPhysicsBridge(CommonGraphicsApp* glApp) - :m_glApp(glApp), m_debugDraw(0) + :m_glApp(glApp), m_debugDraw(0), m_curColor(0) { } virtual void createRigidBodyGraphicsObject(btRigidBody* body, const btVector3& color) @@ -53,8 +64,56 @@ struct MyGraphicsPhysicsBridge : public GraphicsPhysicsBridge //todo: support all collision shape types switch (collisionShape->getShapeType()) { + case STATIC_PLANE_PROXYTYPE: + { + //draw a box, oriented along the plane normal + const btStaticPlaneShape* staticPlaneShape = static_cast(collisionShape); + btScalar planeConst = staticPlaneShape->getPlaneConstant(); + const btVector3& planeNormal = staticPlaneShape->getPlaneNormal(); + btVector3 planeOrigin = planeNormal * planeConst; + btVector3 vec0,vec1; + btPlaneSpace1(planeNormal,vec0,vec1); + btScalar vecLen = 100.f; + btVector3 verts[4]; + + verts[0] = planeOrigin + vec0*vecLen + vec1*vecLen; + verts[1] = planeOrigin - vec0*vecLen + vec1*vecLen; + verts[2] = planeOrigin - vec0*vecLen - vec1*vecLen; + verts[3] = planeOrigin + vec0*vecLen - vec1*vecLen; + + int startIndex = verticesOut.size(); + indicesOut.push_back(startIndex+0); + indicesOut.push_back(startIndex+1); + indicesOut.push_back(startIndex+2); + indicesOut.push_back(startIndex+0); + indicesOut.push_back(startIndex+2); + indicesOut.push_back(startIndex+3); + + btVector3 triNormal = parentTransform.getBasis()*planeNormal; + + + for (int i=0;i<4;i++) + { + GLInstanceVertex vtx; + btVector3 pos =parentTransform*verts[i]; + vtx.xyzw[0] = pos.x(); + vtx.xyzw[1] = pos.y(); + vtx.xyzw[2] = pos.z(); + vtx.xyzw[3] = 0.f; + + vtx.normal[0] =triNormal.x(); + vtx.normal[1] =triNormal.y(); + vtx.normal[2] =triNormal.z(); + + vtx.uv[0] = 0.5f; + vtx.uv[1] = 0.5f; + verticesOut.push_back(vtx); + } + break; + } case TRIANGLE_MESH_SHAPE_PROXYTYPE: { + btAssert(0); break; } default: @@ -193,6 +252,29 @@ struct MyGraphicsPhysicsBridge : public GraphicsPhysicsBridge { m_glApp->setUpAxis(axis); } + + + btVector3 selectColor() + { + btVector4 color = sColors[m_curColor]; + m_curColor++; + m_curColor&=3; + return color; + } + + virtual void autogenerateGraphicsObjects(btDiscreteDynamicsWorld* rbWorld) + { + for (int i=0;igetNumCollisionObjects();i++) + { + btCollisionObject* colObj = rbWorld->getCollisionObjectArray()[i]; + //btRigidBody* body = btRigidBody::upcast(colObj); + //does this also work for btMultiBody/btMultiBodyLinkCollider? + createCollisionShapeGraphicsObject(colObj->getCollisionShape()); + btVector3 color= selectColor(); + createCollisionObjectGraphicsObject(colObj,color); + + } + } }; Bullet2RigidBodyDemo::Bullet2RigidBodyDemo(CommonGraphicsApp* app, CommonPhysicsSetup* physicsSetup) diff --git a/btgui/Bullet3AppSupport/CommonPhysicsSetup.h b/btgui/Bullet3AppSupport/CommonPhysicsSetup.h index dbd2801f2..6d4e37366 100644 --- a/btgui/Bullet3AppSupport/CommonPhysicsSetup.h +++ b/btgui/Bullet3AppSupport/CommonPhysicsSetup.h @@ -52,6 +52,9 @@ struct GraphicsPhysicsBridge { } + virtual void autogenerateGraphicsObjects(btDiscreteDynamicsWorld* rbWorld) + { + } }; ///Bullet 2 specific physics setup, that allows to share code between old and new demo frameworks diff --git a/src/BulletCollision/CollisionShapes/btStaticPlaneShape.cpp b/src/BulletCollision/CollisionShapes/btStaticPlaneShape.cpp index 38ef8f037..d17141e3f 100644 --- a/src/BulletCollision/CollisionShapes/btStaticPlaneShape.cpp +++ b/src/BulletCollision/CollisionShapes/btStaticPlaneShape.cpp @@ -21,7 +21,7 @@ subject to the following restrictions: btStaticPlaneShape::btStaticPlaneShape(const btVector3& planeNormal,btScalar planeConstant) : btConcaveShape (), m_planeNormal(planeNormal.normalized()), m_planeConstant(planeConstant), -m_localScaling(btScalar(0.),btScalar(0.),btScalar(0.)) +m_localScaling(btScalar(1.),btScalar(1.),btScalar(1.)) { m_shapeType = STATIC_PLANE_PROXYTYPE; // btAssert( btFuzzyZero(m_planeNormal.length() - btScalar(1.)) ); diff --git a/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h b/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h index c07e9bbd8..a3a0fa672 100644 --- a/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h +++ b/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h @@ -89,7 +89,7 @@ struct btContactSolverInfo : public btContactSolverInfoData m_solverMode = SOLVER_USE_WARMSTARTING | SOLVER_SIMD;// | SOLVER_RANDMIZE_ORDER; m_restingContactRestitutionThreshold = 2;//unused as of 2.81 m_minimumSolverBatchSize = 128; //try to combine islands until the amount of constraints reaches this limit - m_maxGyroscopicForce = 100.f; ///only used to clamp forces for bodies that have their BT_ENABLE_GYROPSCOPIC_FORCE flag set (using btRigidBody::setFlag) + 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. } }; @@ -111,7 +111,7 @@ struct btContactSolverInfoDoubleData double m_splitImpulseTurnErp; double m_linearSlop; double m_warmstartingFactor; - double m_maxGyroscopicForce; + double m_maxGyroscopicForce;///it is only used for 'explicit' version of gyroscopic force double m_singleAxisRollingFrictionThreshold; int m_numIterations; diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp index fb06b76c3..f01a22fc1 100644 --- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp +++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp @@ -1259,6 +1259,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol //convert all bodies + for (int i=0;igetFlags()&BT_ENABLE_GYROPSCOPIC_FORCE) + if (body->getFlags()&BT_ENABLE_GYROPSCOPIC_FORCE_EXPLICIT) { gyroForce = body->computeGyroscopicForce(infoGlobal.m_maxGyroscopicForce); solverBody.m_externalTorqueImpulse -= gyroForce*body->getInvInertiaTensorWorld()*infoGlobal.m_timeStep; } + if (body->getFlags()&BT_ENABLE_GYROPSCOPIC_FORCE_IMPLICIT_EWERT) + { + gyroForce = body->computeGyroscopicImpulseImplicit_Ewert(infoGlobal.m_timeStep); + solverBody.m_externalTorqueImpulse += gyroForce; + } + if (body->getFlags()&BT_ENABLE_GYROPSCOPIC_FORCE_IMPLICIT_COOPER) + { + gyroForce = body->computeGyroscopicImpulseImplicit_Cooper(infoGlobal.m_timeStep); + solverBody.m_externalTorqueImpulse += gyroForce; + } + + } } diff --git a/src/BulletDynamics/Dynamics/btRigidBody.cpp b/src/BulletDynamics/Dynamics/btRigidBody.cpp index 732d75963..48186d5d4 100644 --- a/src/BulletDynamics/Dynamics/btRigidBody.cpp +++ b/src/BulletDynamics/Dynamics/btRigidBody.cpp @@ -87,7 +87,7 @@ void btRigidBody::setupRigidBody(const btRigidBody::btRigidBodyConstructionInfo& setMassProps(constructionInfo.m_mass, constructionInfo.m_localInertia); updateInertiaTensor(); - m_rigidbodyFlags = 0; + m_rigidbodyFlags = BT_ENABLE_GYROPSCOPIC_FORCE_IMPLICIT_EWERT; m_deltaLinearVelocity.setZero(); @@ -257,12 +257,41 @@ void btRigidBody::updateInertiaTensor() } + +btVector3 btRigidBody::getLocalInertia() const +{ + + btVector3 inertiaLocal; + const btVector3 inertia = m_invInertiaLocal; + inertiaLocal.setValue(inertia.x() != btScalar(0.0) ? btScalar(1.0) / inertia.x() : btScalar(0.0), + inertia.y() != btScalar(0.0) ? btScalar(1.0) / inertia.y() : btScalar(0.0), + inertia.z() != btScalar(0.0) ? btScalar(1.0) / inertia.z() : btScalar(0.0)); + return inertiaLocal; +} + +inline btVector3 evalEulerEqn(const btVector3& w1, const btVector3& w0, const btVector3& T, const btScalar dt, + const btMatrix3x3 &I) +{ + const btVector3 w2 = I*w1 + w1.cross(I*w1)*dt - (T*dt + I*w0); + return w2; +} + +inline btMatrix3x3 evalEulerEqnDeriv(const btVector3& w1, const btVector3& w0, const btScalar dt, + const btMatrix3x3 &I) +{ + + btMatrix3x3 w1x, Iw1x; + const btVector3 Iwi = (I*w1); + w1.getSkewSymmetricMatrix(&w1x[0], &w1x[1], &w1x[2]); + Iwi.getSkewSymmetricMatrix(&Iw1x[0], &Iw1x[1], &Iw1x[2]); + + const btMatrix3x3 dfw1 = I + (w1x*I - Iw1x)*dt; + return dfw1; +} + btVector3 btRigidBody::computeGyroscopicForce(btScalar maxGyroscopicForce) const { - btVector3 inertiaLocal; - inertiaLocal[0] = 1.f/getInvInertiaDiagLocal()[0]; - inertiaLocal[1] = 1.f/getInvInertiaDiagLocal()[1]; - inertiaLocal[2] = 1.f/getInvInertiaDiagLocal()[2]; + btVector3 inertiaLocal = getLocalInertia(); btMatrix3x3 inertiaTensorWorld = getWorldTransform().getBasis().scaled(inertiaLocal) * getWorldTransform().getBasis().transpose(); btVector3 tmp = inertiaTensorWorld*getAngularVelocity(); btVector3 gf = getAngularVelocity().cross(tmp); @@ -274,6 +303,138 @@ btVector3 btRigidBody::computeGyroscopicForce(btScalar maxGyroscopicForce) const return gf; } +void btSetCrossMatrixMinus(btMatrix3x3& res, const btVector3& a) +{ + const btScalar a_0 = a[0], a_1 = a[1], a_2 = a[2]; + res.setValue(0, +a_2, -a_1, + -a_2, 0, +a_0, + +a_1, -a_0, 0); +} + +btVector3 btRigidBody::computeGyroscopicImpulseImplicit_Cooper(btScalar step) const +{ +#if 0 + dReal h = callContext->m_stepperCallContext->m_stepSize; // Step size + dVector3 L; // Compute angular momentum + dMultiply0_331(L, I, b->avel); +#endif + + btVector3 inertiaLocal = getLocalInertia(); + btMatrix3x3 inertiaTensorWorld = getWorldTransform().getBasis().scaled(inertiaLocal) * getWorldTransform().getBasis().transpose(); + btVector3 L = inertiaTensorWorld*getAngularVelocity(); + + btMatrix3x3 Itild(0, 0, 0, 0, 0, 0, 0, 0, 0); + +#if 0 + for (int ii = 0; ii<12; ++ii) { + Itild[ii] = Itild[ii] * h + I[ii]; + } +#endif + + btSetCrossMatrixMinus(Itild, L*step); + Itild += inertiaTensorWorld; + + +#if 0 +// Compute a new effective 'inertia tensor' +// for the implicit step: the cross-product +// matrix of the angular momentum plus the +// old tensor scaled by the timestep. +// Itild may not be symmetric pos-definite, +// but we can still use it to compute implicit +// gyroscopic torques. +dMatrix3 Itild = { 0 }; +dSetCrossMatrixMinus(Itild, L, 4); +for (int ii = 0; ii<12; ++ii) { + Itild[ii] = Itild[ii] * h + I[ii]; +} +#endif + + L *= step; + //Itild may not be symmetric pos-definite + btMatrix3x3 itInv = Itild.inverse(); + Itild = inertiaTensorWorld * itInv; + btMatrix3x3 ident(1,0,0,0,1,0,0,0,1); + Itild -= ident; + + + + +#if 0 +// Scale momentum by inverse time to get +// a sort of "torque" +dScaleVector3(L, dRecip(h)); +// Invert the pseudo-tensor +dMatrix3 itInv; +// This is a closed-form inversion. +// It's probably not numerically stable +// when dealing with small masses with +// a large asymmetry. +// An LU decomposition might be better. +if (dInvertMatrix3(itInv, Itild) != 0) { + // "Divide" the original tensor + // by the pseudo-tensor (on the right) + dMultiply0_333(Itild, I, itInv); + // Subtract an identity matrix + Itild[0] -= 1; Itild[5] -= 1; Itild[10] -= 1; + + // This new inertia matrix rotates the + // momentum to get a new set of torques + // that will work correctly when applied + // to the old inertia matrix as explicit + // torques with a semi-implicit update + // step. + dVector3 tau0; + dMultiply0_331(tau0, Itild, L); + + // Add the gyro torques to the torque + // accumulator + for (int ii = 0; ii<3; ++ii) { + b->tacc[ii] += tau0[ii]; + } +#endif + btVector3 tau0 = Itild * L; + return tau0; + return btVector3(0, 0, 0); +} + +btVector3 btRigidBody::computeGyroscopicImpulseImplicit_Ewert(btScalar step) const +{ + // use full newton-euler equations. common practice to drop the wxIw term. want it for better tumbling behavior. + // calculate using implicit euler step so it's stable. + + const btVector3 inertiaLocal = getLocalInertia(); + const btVector3 w0 = getAngularVelocity(); + + btMatrix3x3 I; + + I = m_worldTransform.getBasis().scaled(inertiaLocal) * + m_worldTransform.getBasis().transpose(); + + // use newtons method to find implicit solution for new angular velocity (w') + // f(w') = -(T*step + Iw) + Iw' + w' + w'xIw'*step = 0 + // df/dw' = I + 1xIw'*step + w'xI*step + + btVector3 w1 = w0; + + // one step of newton's method + { + const btVector3 fw = evalEulerEqn(w1, w0, btVector3(0, 0, 0), step, I); + const btMatrix3x3 dfw = evalEulerEqnDeriv(w1, w0, step, I); + + const btMatrix3x3 dfw_inv = dfw.inverse(); + btVector3 dw; + + dw = dfw_inv*fw; + + w1 -= dw; + } + + btVector3 gf = (w1 - w0); + return gf; +} + + void btRigidBody::integrateVelocities(btScalar step) { if (isStaticOrKinematicObject()) diff --git a/src/BulletDynamics/Dynamics/btRigidBody.h b/src/BulletDynamics/Dynamics/btRigidBody.h index 7ba7b1f23..b3934e99e 100644 --- a/src/BulletDynamics/Dynamics/btRigidBody.h +++ b/src/BulletDynamics/Dynamics/btRigidBody.h @@ -41,10 +41,11 @@ extern bool gDisableDeactivation; enum btRigidBodyFlags { BT_DISABLE_WORLD_GRAVITY = 1, - ///The BT_ENABLE_GYROPSCOPIC_FORCE can easily introduce instability - ///So generally it is best to not enable it. - ///If really needed, run at a high frequency like 1000 Hertz: ///See Demos/GyroscopicDemo for an example use - BT_ENABLE_GYROPSCOPIC_FORCE = 2 + ///BT_ENABLE_GYROPSCOPIC_FORCE flags is enabled by default in Bullet 2.83 and onwards. + ///See Demos/GyroscopicDemo and computeGyroscopicImpulseImplicit + BT_ENABLE_GYROPSCOPIC_FORCE_EXPLICIT = 2, + BT_ENABLE_GYROPSCOPIC_FORCE_IMPLICIT_COOPER=4, + BT_ENABLE_GYROPSCOPIC_FORCE_IMPLICIT_EWERT=8 }; @@ -529,7 +530,13 @@ public: return m_rigidbodyFlags; } - btVector3 computeGyroscopicForce(btScalar maxGyroscopicForce) const; + + + + btVector3 computeGyroscopicImpulseImplicit_Ewert(btScalar dt) const; + btVector3 computeGyroscopicImpulseImplicit_Cooper(btScalar step) const; + btVector3 computeGyroscopicForce(btScalar maxGyroscopicForce) const;//explicit version is best avoided, it gains energy + btVector3 getLocalInertia() const; ///////////////////////////////////////////////