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

remove Cooper implicit (it was just for comparison, it is buggy)

Browse Source 
add btMatrix3x3::solve33, thanks to Erin Catto, and added safety against division by zero
This commit is contained in:
Erwin Coumans
2015-03-25 19:33:02 -07:00
parent 2ddd8f78c2
commit 9931dd9684
5 changed files with 40 additions and 114 deletions
Download Patch File Download Diff File Expand all files Collapse all files

17
Demos/GyroscopicDemo/GyroscopicSetup.cpp
View File

@@ -1,19 +1,17 @@
#include "GyroscopicSetup.h"
static int gyroflags[5] = {
static int gyroflags[4] = {
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,
BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_CATTO
};
static const char* gyroNames[5] = {
static const char* gyroNames[4] = {
"No Coriolis",
"Explicit",
"Ewert",
"Catto",
"Cooper",
"Implicit",
"Local Implicit"
};
@@ -25,16 +23,15 @@ void GyroscopicSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge)
gfxBridge.createPhysicsDebugDrawer(m_dynamicsWorld);
btVector3 positions[5] = {
btVector3 positions[4] = {
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++)
for (int i = 0; i<4; i++)
{
btCylinderShapeZ* pin = new btCylinderShapeZ(btVector3(0.1,0.1, 0.2));
btBoxShape* box = new btBoxShape(btVector3(1,0.1,0.1));

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

@@ -1271,7 +1271,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
btVector3 gyroForce (0,0,0);
if (body->getFlags()&BT_ENABLE_GYROSCOPIC_FORCE_EXPLICIT)
{
gyroForce = body->computeGyroscopicForce(infoGlobal.m_maxGyroscopicForce);
gyroForce = body->computeGyroscopicForceExplicit(infoGlobal.m_maxGyroscopicForce);
solverBody.m_externalTorqueImpulse -= gyroForce*body->getInvInertiaTensorWorld()*infoGlobal.m_timeStep;
}
if (body->getFlags()&BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_EWERT)
@@ -1279,11 +1279,6 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
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);

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

@@ -289,7 +289,7 @@ inline btMatrix3x3 evalEulerEqnDeriv(const btVector3& w1, const btVector3& w0, c
return dfw1;
}
btVector3 btRigidBody::computeGyroscopicForce(btScalar maxGyroscopicForce) const
btVector3 btRigidBody::computeGyroscopicForceExplicit(btScalar maxGyroscopicForce) const
{
btVector3 inertiaLocal = getLocalInertia();
btMatrix3x3 inertiaTensorWorld = getWorldTransform().getBasis().scaled(inertiaLocal) * getWorldTransform().getBasis().transpose();
@@ -338,8 +338,9 @@ btVector3 btRigidBody::computeGyroscopicImpulseImplicit_Catto(btScalar step) con
// Jacobian
btMatrix3x3 J = Ib + (skew0*Ib - skew1)*step;
btMatrix3x3 Jinv = J.inverse();
btVector3 omega_div = Jinv*f;
// btMatrix3x3 Jinv = J.inverse();
// btVector3 omega_div = Jinv*f;
btVector3 omega_div = J.solve33(f);
// Single Newton-Raphson update
omegab = omegab - omega_div;//Solve33(J, f);
@@ -351,93 +352,6 @@ btVector3 btRigidBody::computeGyroscopicImpulseImplicit_Catto(btScalar step) con
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.
@@ -462,10 +376,10 @@ btVector3 btRigidBody::computeGyroscopicImpulseImplicit_Ewert(btScalar step) con
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;
dw = dfw.solve33(fw);
//const btMatrix3x3 dfw_inv = dfw.inverse();
//dw = dfw_inv*fw;
w1 -= dw;
}

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

@@ -44,9 +44,8 @@ enum btRigidBodyFlags
///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,
BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_EWERT=4,
BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_CATTO=8,
};
@@ -535,10 +534,10 @@ public:
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
///explicit version is best avoided, it gains energy
btVector3 computeGyroscopicForceExplicit(btScalar maxGyroscopicForce) const;
btVector3 getLocalInertia() const;
///////////////////////////////////////////////

21
src/LinearMath/btMatrix3x3.h
View File

@@ -610,6 +610,27 @@ public:
/**@brief Return the inverse of the matrix */
btMatrix3x3 inverse() const;
/// Solve A * x = b, where b is a column vector. This is more efficient
/// than computing the inverse in one-shot cases.
///Solve33 is from Box2d, thanks to Erin Catto,
btVector3 solve33(const btVector3& b) const
{
btVector3 col1 = getColumn(0);
btVector3 col2 = getColumn(1);
btVector3 col3 = getColumn(2);
btScalar det = btDot(col1, btCross(col2, col3));
if (btFabs(det)>SIMD_EPSILON)
{
det = 1.0f / det;
}
btVector3 x;
x[0] = det * btDot(b, btCross(col2, col3));
x[1] = det * btDot(col1, btCross(b, col3));
x[2] = det * btDot(col1, btCross(col2, b));
return x;
}
btMatrix3x3 transposeTimes(const btMatrix3x3& m) const;
btMatrix3x3 timesTranspose(const btMatrix3x3& m) const;