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; ///////////////////////////////////////////////