Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Added experimental implicit gyroscopic force implementation, one by Michael Ewert, and another by Cooper (from OpenDE)

Browse Source 
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
This commit is contained in:
erwin coumans
2015-03-24 15:55:05 -07:00
parent 29949b63cc
commit 1e13454511
17 changed files with 852 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files

63
Demos/GyroscopicDemo/GyroscopicSetup.cpp Normal file
View File

@@ -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);
}

14
Demos/GyroscopicDemo/GyroscopicSetup.h Normal file
View File

@@ -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

2
Demos/SerializeDemo/SerializeSetup.cpp
View File

@@ -52,7 +52,7 @@ void SerializeSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge)
gfxBridge.setUpAxis(1);
}
gfxBridge.autogenerateGraphicsObjects(m_dynamicsWorld);
}
void SerializeSetup::stepSimulation(float deltaTime)

9
Demos3/AllBullet2Demos/BulletDemoEntries.h
View File

@@ -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},

4
Demos3/AllBullet2Demos/CMakeLists.txt
View File

@@ -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

4
Demos3/AllBullet2Demos/premake4.lua
View File

@@ -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",

3
Demos3/bullet2/ConstraintDemo/ConstraintPhysicsSetup.cpp
View File

@@ -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);
@@ -124,4 +124,5 @@ int mode = btIDebugDraw::DBG_DrawWireframe
spDoorHinge->setDbgDrawSize(btScalar(5.f));
}
gfxBridge.autogenerateGraphicsObjects(m_dynamicsWorld);
}

453
Demos3/bullet2/ConstraintDemo/Dof6Spring2Setup.cpp Normal file
View File

@@ -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);
}

19
Demos3/bullet2/ConstraintDemo/Dof6Spring2Setup.h Normal file
View File

@@ -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

8
Demos3/bullet2/MultiBodyDemo/MultiBodyVehicle.cpp
View File

@@ -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)

84
btgui/Bullet3AppSupport/Bullet2RigidBodyDemo.cpp
View File

@@ -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<const btStaticPlaneShape*>(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;i<rbWorld->getNumCollisionObjects();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)

3
btgui/Bullet3AppSupport/CommonPhysicsSetup.h
View File

@@ -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

2
src/BulletCollision/CollisionShapes/btStaticPlaneShape.cpp
View File

@@ -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.)) );

4
src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h
View File

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

15
src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
View File

@@ -1259,6 +1259,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
//convert all bodies
for (int i=0;i<numBodies;i++)
{
int bodyId = getOrInitSolverBody(*bodies[i],infoGlobal.m_timeStep);
@@ -1268,11 +1269,23 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
{
btSolverBody& solverBody = m_tmpSolverBodyPool[bodyId];
btVector3 gyroForce (0,0,0);
if (body->getFlags()&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;
}
}
}

171
src/BulletDynamics/Dynamics/btRigidBody.cpp
View File

@@ -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())

17
src/BulletDynamics/Dynamics/btRigidBody.h
View File

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