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

Merge pull request #346 from erwincoumans/master

Browse Source 
Add global and local implicit Gyroscopic force integration, with Dzhanibekov effect demo
This commit is contained in:
erwincoumans
2015-03-25 14:54:24 -07:00
parent 8382ccdc3d 2ddd8f78c2
commit 4100db0c70
20 changed files with 971 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -0,0 +1,98 @@
#include "GyroscopicSetup.h"
static int gyroflags[5] = {
0,//none, no gyroscopic term
BT_ENABLE_GYROSCOPIC_FORCE_EXPLICIT,
BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_EWERT,
BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_CATTO,
BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_COOPER,
};
static const char* gyroNames[5] = {
"No Coriolis",
"Explicit",
"Ewert",
"Catto",
"Cooper",
};
void GyroscopicSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge)
{
gfxBridge.setUpAxis(2);
createEmptyDynamicsWorld();
m_dynamicsWorld->setGravity(btVector3(0, 0, 0));
gfxBridge.createPhysicsDebugDrawer(m_dynamicsWorld);
btVector3 positions[5] = {
btVector3( -10, 8,4),
btVector3( -5, 8,4),
btVector3( 0, 8,4),
btVector3( 5, 8,4),
btVector3( 10, 8,4),
};
for (int i = 0; i<5; i++)
{
btCylinderShapeZ* pin = new btCylinderShapeZ(btVector3(0.1,0.1, 0.2));
btBoxShape* box = new btBoxShape(btVector3(1,0.1,0.1));
box->setMargin(0.01);
pin->setMargin(0.01);
btCompoundShape* compound = new btCompoundShape();
compound->addChildShape(btTransform::getIdentity(), pin);
btTransform offsetBox(btMatrix3x3::getIdentity(),btVector3(0,0,0.2));
compound->addChildShape(offsetBox, box);
btScalar masses[2] = {0.3,0.1};
btVector3 localInertia;
btTransform principal;
compound->calculatePrincipalAxisTransform(masses,principal,localInertia);
btRigidBody* body = new btRigidBody(1, 0, compound, localInertia);
btTransform tr;
tr.setIdentity();
tr.setOrigin(positions[i]);
body->setCenterOfMassTransform(tr);
body->setAngularVelocity(btVector3(0, 0.1, 10));//51));
//body->setLinearVelocity(btVector3(3, 0, 0));
body->setFriction(btSqrt(1));
m_dynamicsWorld->addRigidBody(body);
body->setFlags(gyroflags[i]);
m_dynamicsWorld->getSolverInfo().m_maxGyroscopicForce = 10.f;
body->setDamping(0.0000f, 0.000f);
}
{
//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));
}
gfxBridge.autogenerateGraphicsObjects(m_dynamicsWorld);
}
void GyroscopicSetup::syncPhysicsToGraphics(GraphicsPhysicsBridge& gfxBridge)
{
CommonRigidBodySetup::syncPhysicsToGraphics(gfxBridge);
//render method names above objects
for (int i=0;i<m_dynamicsWorld->getNumCollisionObjects();i++)
{
btRigidBody* body = btRigidBody::upcast(m_dynamicsWorld->getCollisionObjectArray()[i]);
if (body && body->getInvMass()>0)
{
btTransform tr = body->getWorldTransform();
btVector3 pos = tr.getOrigin()+btVector3(0,0,2);
btScalar size=1;
gfxBridge.drawText3D(gyroNames[i],pos.x(),pos.y(),pos.z(),size);
}
}
}

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

@@ -0,0 +1,16 @@
#ifndef GYROSCOPIC_SETUP_H
#define GYROSCOPIC_SETUP_H
#include "Bullet3AppSupport/CommonRigidBodySetup.h"
struct GyroscopicSetup : public CommonRigidBodySetup
{
virtual void initPhysics(GraphicsPhysicsBridge& gfxBridge);
virtual void syncPhysicsToGraphics(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

9
Demos3/AllBullet2Demos/main.cpp
View File

@@ -32,6 +32,7 @@
#include "Bullet3AppSupport/GwenTextureWindow.h"
#include "Bullet3AppSupport/GraphingTexture.h"
#include "Bullet3AppSupport/Common2dCanvasInterface.h"
#include "Bullet3Common/b3CommandLineArgs.h"
#include "OpenGLWindow/SimpleCamera.h"
#include "OpenGLWindow/SimpleOpenGL2Renderer.h"
@@ -462,6 +463,8 @@ struct QuickCanvas : public Common2dCanvasInterface
extern float shadowMapWorldSize;
int main(int argc, char* argv[])
{
b3CommandLineArgs args(argc,argv);
shadowMapWorldSize = 25;
b3Clock clock;
@@ -483,7 +486,13 @@ int main(int argc, char* argv[])
simpleApp = new SimpleOpenGL3App("AllBullet2Demos",width,height);
app = simpleApp;
}
char* gVideoFileName = 0;
args.GetCmdLineArgument("mp4",gVideoFileName);
if (gVideoFileName)
simpleApp->dumpFramesToVideo(gVideoFileName);
s_instancingRenderer = app->m_renderer;
s_window = app->m_window;
prevMouseMoveCallback = s_window->getMouseMoveCallback();

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

5
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);
@@ -123,5 +123,6 @@ 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)

90
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,35 @@ 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);
}
}
virtual void drawText3D( const char* txt, float posX, float posY, float posZ, float size)
{
btAssert(m_glApp);
m_glApp->drawText3D(txt,posX,posY,posZ,size);
}
};
Bullet2RigidBodyDemo::Bullet2RigidBodyDemo(CommonGraphicsApp* app, CommonPhysicsSetup* physicsSetup)

8
btgui/Bullet3AppSupport/CommonPhysicsSetup.h
View File

@@ -52,6 +52,14 @@ struct GraphicsPhysicsBridge
{
}
virtual void autogenerateGraphicsObjects(btDiscreteDynamicsWorld* rbWorld)
{
}
virtual void drawText3D( const char* txt, float posX, float posZY, float posZ, float size)
{
}
};
///Bullet 2 specific physics setup, that allows to share code between old and new demo frameworks

14
btgui/OpenGLWindow/SimpleOpenGL3App.cpp
View File

@@ -744,6 +744,7 @@ void SimpleOpenGL3App::swapBuffer()
}
m_window->startRendering();
}
// see also http://blog.mmacklin.com/2013/06/11/real-time-video-capture-with-ffmpeg/
void SimpleOpenGL3App::dumpFramesToVideo(const char* mp4FileName)
{
@@ -751,8 +752,17 @@ void SimpleOpenGL3App::dumpFramesToVideo(const char* mp4FileName)
int height = (int)m_window->getRetinaScale()*m_instancingRenderer->getScreenHeight();
char cmd[8192];
sprintf(cmd,"ffmpeg -r 60 -f rawvideo -pix_fmt rgba -s %dx%d -i - "
"-threads 0 -y -crf 0 -b 50000k -vf vflip %s",width,height,mp4FileName);
#ifdef _WIN32
sprintf(cmd, "ffmpeg -r 60 -f rawvideo -pix_fmt rgba -s %dx%d -i - "
"-y -crf 0 -b:v 1500000 -an -vcodec h264 -vf vflip %s", width, height, mp4FileName);
#else
sprintf(cmd, "ffmpeg -r 60 -f rawvideo -pix_fmt rgba -s %dx%d -i - "
"-threads 0 -y -crf 0 -b 50000k -vf vflip %s", width, height, mp4FileName);
#endif
//sprintf(cmd,"ffmpeg -r 60 -f rawvideo -pix_fmt rgba -s %dx%d -i - "
// "-threads 0 -y -crf 0 -b 50000k -vf vflip %s",width,height,mp4FileName);
// sprintf(cmd,"ffmpeg -r 60 -f rawvideo -pix_fmt rgba -s %dx%d -i - "
// "-threads 0 -preset fast -y -crf 21 -vf vflip %s",width,height,mp4FileName);

1
build3/premake4.lua
View File

@@ -94,6 +94,7 @@
{
'ARCHS = "$(ARCHS_STANDARD_32_BIT) $(ARCHS_STANDARD_64_BIT)"',
'VALID_ARCHS = "x86_64 i386"',
-- 'SDKROOT = "macosx10.9"',
}
end

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;

21
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,29 @@ 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_GYROSCOPIC_FORCE_EXPLICIT)
{
gyroForce = body->computeGyroscopicForce(infoGlobal.m_maxGyroscopicForce);
solverBody.m_externalTorqueImpulse -= gyroForce*body->getInvInertiaTensorWorld()*infoGlobal.m_timeStep;
}
if (body->getFlags()&BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_EWERT)
{
gyroForce = body->computeGyroscopicImpulseImplicit_Ewert(infoGlobal.m_timeStep);
solverBody.m_externalTorqueImpulse += gyroForce;
}
if (body->getFlags()&BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_COOPER)
{
gyroForce = body->computeGyroscopicImpulseImplicit_Cooper(infoGlobal.m_timeStep);
solverBody.m_externalTorqueImpulse += gyroForce;
}
if (body->getFlags()&BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_CATTO)
{
gyroForce = body->computeGyroscopicImpulseImplicit_Catto(infoGlobal.m_timeStep);
solverBody.m_externalTorqueImpulse += gyroForce;
}
}
}

211
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_GYROSCOPIC_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,178 @@ 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_Catto(btScalar step) const
{
btVector3 idl = getLocalInertia();
btVector3 omega1 = getAngularVelocity();
btQuaternion q = getWorldTransform().getRotation();
// Convert to body coordinates
btVector3 omegab = quatRotate(q.inverse(), omega1);
btMatrix3x3 Ib;
Ib.setValue(idl.x(),0,0,
0,idl.y(),0,
0,0,idl.z());
btVector3 ibo = Ib*omegab;
// Residual vector
btVector3 f = step * omegab.cross(ibo);
btMatrix3x3 skew0;
omegab.getSkewSymmetricMatrix(&skew0[0], &skew0[1], &skew0[2]);
btVector3 om = Ib*omegab;
btMatrix3x3 skew1;
om.getSkewSymmetricMatrix(&skew1[0],&skew1[1],&skew1[2]);
// Jacobian
btMatrix3x3 J = Ib + (skew0*Ib - skew1)*step;
btMatrix3x3 Jinv = J.inverse();
btVector3 omega_div = Jinv*f;
// Single Newton-Raphson update
omegab = omegab - omega_div;//Solve33(J, f);
// Back to world coordinates
btVector3 omega2 = quatRotate(q,omegab);
btVector3 gf = omega2-omega1;
return gf;
}
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;
// printf("tau0 = %f,%f,%f\n",tau0.x(),tau0.y(),tau0.z());
return tau0;
}
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())

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

@@ -41,10 +41,12 @@ 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_GYROSCOPIC_FORCE_EXPLICIT = 2,
BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_COOPER=4,
BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_EWERT=8,
BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_CATTO=16,
};
@@ -529,7 +531,15 @@ public:
return m_rigidbodyFlags;
}
btVector3 computeGyroscopicForce(btScalar maxGyroscopicForce) const;
btVector3 computeGyroscopicImpulseImplicit_Ewert(btScalar dt) const;
btVector3 computeGyroscopicImpulseImplicit_Cooper(btScalar step) const;
btVector3 computeGyroscopicImpulseImplicit_Catto(btScalar step) const;
btVector3 computeGyroscopicForce(btScalar maxGyroscopicForce) const;//explicit version is best avoided, it gains energy
btVector3 getLocalInertia() const;
///////////////////////////////////////////////