From 9a169f26333141b1f9e0ef4609c7ef3dba2f5835 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Tue, 12 Dec 2006 03:08:15 +0000 Subject: added some new Bullet files, and upgraded to latest Bullet 2.x Please make sure to have extern/bullet/src/LinearMath/btAlignedAllocator.cpp in your build, if you add the files by name, instead of wildcard *.cpp --- .../ConstraintSolver/btConstraintSolver.h | 4 +- .../ConstraintSolver/btGeneric6DofConstraint.cpp | 2 +- .../ConstraintSolver/btHingeConstraint.cpp | 206 ++++++++------------- .../ConstraintSolver/btHingeConstraint.h | 15 +- .../btSequentialImpulseConstraintSolver.cpp | 35 +++- .../btSequentialImpulseConstraintSolver.h | 4 +- 6 files changed, 128 insertions(+), 138 deletions(-) (limited to 'extern/bullet2/src/BulletDynamics/ConstraintSolver') diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h index e073797f989..ce90f798c04 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h @@ -18,7 +18,7 @@ subject to the following restrictions: class btPersistentManifold; class btRigidBody; - +class btTypedConstraint; struct btContactSolverInfo; struct btBroadphaseProxy; class btIDebugDraw; @@ -31,7 +31,7 @@ public: virtual ~btConstraintSolver() {} - virtual float solveGroup(btPersistentManifold** manifold,int numManifolds,const btContactSolverInfo& info,class btIDebugDraw* debugDrawer = 0) = 0; + virtual float solveGroup(btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints, const btContactSolverInfo& info,class btIDebugDraw* debugDrawer = 0) = 0; }; diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp index b5783f824d4..b2132a8d4f3 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp @@ -289,7 +289,7 @@ void btGeneric6DofConstraint::solveConstraint(btScalar timeStep) btScalar loLimit = m_upperLimit[i+3] > -3.1415 ? m_lowerLimit[i+3] : -1e30f; btScalar hiLimit = m_upperLimit[i+3] < 3.1415 ? m_upperLimit[i+3] : 1e30f; - float projAngle = -2.*xyz[i]; + float projAngle = -2.f*xyz[i]; if (projAngle < loLimit) { diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp index 7b2a9ba6244..f72278e2cbf 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp @@ -19,7 +19,8 @@ subject to the following restrictions: #include "LinearMath/btTransformUtil.h" -btHingeConstraint::btHingeConstraint() +btHingeConstraint::btHingeConstraint(): +m_enableAngularMotor(false) { } @@ -28,7 +29,8 @@ btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const bt :btTypedConstraint(rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB), m_axisInA(axisInA), m_axisInB(-axisInB), -m_angularOnly(false) +m_angularOnly(false), +m_enableAngularMotor(false) { } @@ -39,7 +41,8 @@ btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA, m_axisInA(axisInA), //fixed axis in worldspace m_axisInB(rbA.getCenterOfMassTransform().getBasis() * -axisInA), -m_angularOnly(false) +m_angularOnly(false), +m_enableAngularMotor(false) { } @@ -73,11 +76,16 @@ void btHingeConstraint::buildJacobian() //these two jointAxis require equal angular velocities for both bodies //this is unused for now, it's a todo - btVector3 axisWorldA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_axisInA; - btVector3 jointAxis0; - btVector3 jointAxis1; - btPlaneSpace1(axisWorldA,jointAxis0,jointAxis1); + btVector3 jointAxis0local; + btVector3 jointAxis1local; + btPlaneSpace1(m_axisInA,jointAxis0local,jointAxis1local); + + getRigidBodyA().getCenterOfMassTransform().getBasis() * m_axisInA; + btVector3 jointAxis0 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis0local; + btVector3 jointAxis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis1local; + btVector3 hingeAxisWorld = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_axisInA; + new (&m_jacAng[0]) btJacobianEntry(jointAxis0, m_rbA.getCenterOfMassTransform().getBasis().transpose(), m_rbB.getCenterOfMassTransform().getBasis().transpose(), @@ -90,107 +98,18 @@ void btHingeConstraint::buildJacobian() m_rbA.getInvInertiaDiagLocal(), m_rbB.getInvInertiaDiagLocal()); + new (&m_jacAng[2]) btJacobianEntry(hingeAxisWorld, + m_rbA.getCenterOfMassTransform().getBasis().transpose(), + m_rbB.getCenterOfMassTransform().getBasis().transpose(), + m_rbA.getInvInertiaDiagLocal(), + m_rbB.getInvInertiaDiagLocal()); + + } void btHingeConstraint::solveConstraint(btScalar timeStep) { -//#define NEW_IMPLEMENTATION - -#ifdef NEW_IMPLEMENTATION - btScalar tau = 0.3f; - btScalar damping = 1.f; - - btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_pivotInA; - btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_pivotInB; - - // Dirk: Don't we need to update this after each applied impulse - btVector3 angvelA; // = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity(); - btVector3 angvelB; // = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity(); - - - if (!m_angularOnly) - { - btVector3 normal(0,0,0); - - for (int i=0;i<3;i++) - { - normal[i] = 1; - btScalar jacDiagABInv = 1.f / m_jac[i].getDiagonal(); - - btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); - btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); - - btVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1); - btVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2); - btVector3 vel = vel1 - vel2; - - // Dirk: Get new angular velocity since it changed after applying an impulse - angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity(); - angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity(); - - //velocity error (first order error) - btScalar rel_vel = m_jac[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA, - m_rbB.getLinearVelocity(),angvelB); - - //positional error (zeroth order error) - btScalar depth = -(pivotAInW - pivotBInW).dot(normal); - - btScalar impulse = tau*depth/timeStep * jacDiagABInv - damping * rel_vel * jacDiagABInv; - - btVector3 impulse_vector = normal * impulse; - m_rbA.applyImpulse( impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition()); - m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition()); - - normal[i] = 0; - } - } - - ///solve angular part - - // get axes in world space - btVector3 axisA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_axisInA; - btVector3 axisB = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_axisInB; - - // constraint axes in world space - btVector3 jointAxis0; - btVector3 jointAxis1; - btPlaneSpace1(axisA,jointAxis0,jointAxis1); - - - // Dirk: Get new angular velocity since it changed after applying an impulse - angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity(); - angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity(); - - btScalar jacDiagABInv0 = 1.f / m_jacAng[0].getDiagonal(); - btScalar rel_vel0 = m_jacAng[0].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA, - m_rbB.getLinearVelocity(),angvelB); - float tau1 = tau;//0.f; - - btScalar impulse0 = (tau1 * axisB.dot(jointAxis1) / timeStep - damping * rel_vel0) * jacDiagABInv0; - btVector3 angular_impulse0 = jointAxis0 * impulse0; - - m_rbA.applyTorqueImpulse( angular_impulse0); - m_rbB.applyTorqueImpulse(-angular_impulse0); - - - - // Dirk: Get new angular velocity since it changed after applying an impulse - angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity(); - angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity(); - - btScalar jacDiagABInv1 = 1.f / m_jacAng[1].getDiagonal(); - btScalar rel_vel1 = m_jacAng[1].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA, - m_rbB.getLinearVelocity(),angvelB);; - - btScalar impulse1 = -(tau1 * axisB.dot(jointAxis0) / timeStep + damping * rel_vel1) * jacDiagABInv1; - btVector3 angular_impulse1 = jointAxis1 * impulse1; - - m_rbA.applyTorqueImpulse( angular_impulse1); - m_rbB.applyTorqueImpulse(-angular_impulse1); - -#else - btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_pivotInA; btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_pivotInB; @@ -237,37 +156,68 @@ void btHingeConstraint::solveConstraint(btScalar timeStep) const btVector3& angVelA = getRigidBodyA().getAngularVelocity(); const btVector3& angVelB = getRigidBodyB().getAngularVelocity(); - btVector3 angA = angVelA - axisA * axisA.dot(angVelA); - btVector3 angB = angVelB - axisB * axisB.dot(angVelB); - btVector3 velrel = angA-angB; - - //solve angular velocity correction - float relaxation = 1.f; - float len = velrel.length(); - if (len > 0.00001f) + + btVector3 angVelAroundHingeAxisA = axisA * axisA.dot(angVelA); + btVector3 angVelAroundHingeAxisB = axisB * axisB.dot(angVelB); + + btVector3 angAorthog = angVelA - angVelAroundHingeAxisA; + btVector3 angBorthog = angVelB - angVelAroundHingeAxisB; + btVector3 velrelOrthog = angAorthog-angBorthog; { - btVector3 normal = velrel.normalized(); - float denom = getRigidBodyA().computeAngularImpulseDenominator(normal) + - getRigidBodyB().computeAngularImpulseDenominator(normal); - // scale for mass and relaxation - velrel *= (1.f/denom) * 0.9; + //solve orthogonal angular velocity correction + float relaxation = 1.f; + float len = velrelOrthog.length(); + if (len > 0.00001f) + { + btVector3 normal = velrelOrthog.normalized(); + float denom = getRigidBodyA().computeAngularImpulseDenominator(normal) + + getRigidBodyB().computeAngularImpulseDenominator(normal); + // scale for mass and relaxation + //todo: expose this 0.9 factor to developer + velrelOrthog *= (1.f/denom) * 0.9f; + } + + //solve angular positional correction + btVector3 angularError = -axisA.cross(axisB) *(1.f/timeStep); + float len2 = angularError.length(); + if (len2>0.00001f) + { + btVector3 normal2 = angularError.normalized(); + float denom2 = getRigidBodyA().computeAngularImpulseDenominator(normal2) + + getRigidBodyB().computeAngularImpulseDenominator(normal2); + angularError *= (1.f/denom2) * relaxation; + } + + m_rbA.applyTorqueImpulse(-velrelOrthog+angularError); + m_rbB.applyTorqueImpulse(velrelOrthog-angularError); } - //solve angular positional correction - btVector3 angularError = -axisA.cross(axisB) *(1.f/timeStep); - float len2 = angularError.length(); - if (len2>0.00001f) + //apply motor + if (m_enableAngularMotor) { - btVector3 normal2 = angularError.normalized(); - float denom2 = getRigidBodyA().computeAngularImpulseDenominator(normal2) + - getRigidBodyB().computeAngularImpulseDenominator(normal2); - angularError *= (1.f/denom2) * relaxation; - } + //todo: add limits too + btVector3 angularLimit(0,0,0); + + btVector3 velrel = angVelAroundHingeAxisA - angVelAroundHingeAxisB; + btScalar projRelVel = velrel.dot(axisA); + + btScalar desiredMotorVel = m_motorTargetVelocity; + btScalar motor_relvel = desiredMotorVel - projRelVel; - m_rbA.applyTorqueImpulse(-velrel+angularError); - m_rbB.applyTorqueImpulse(velrel-angularError); + float denom3 = getRigidBodyA().computeAngularImpulseDenominator(axisA) + + getRigidBodyB().computeAngularImpulseDenominator(axisA); + + btScalar unclippedMotorImpulse = (1.f/denom3) * motor_relvel;; + //todo: should clip against accumulated impulse + btScalar clippedMotorImpulse = unclippedMotorImpulse > m_maxMotorImpulse ? m_maxMotorImpulse : unclippedMotorImpulse; + clippedMotorImpulse = clippedMotorImpulse < -m_maxMotorImpulse ? -m_maxMotorImpulse : clippedMotorImpulse; + btVector3 motorImp = clippedMotorImpulse * axisA; + + m_rbA.applyTorqueImpulse(motorImp+angularLimit); + m_rbB.applyTorqueImpulse(-motorImp-angularLimit); + + } } -#endif } diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h index 3bade2b091d..553ec135c8a 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h @@ -29,14 +29,18 @@ class btRigidBody; class btHingeConstraint : public btTypedConstraint { btJacobianEntry m_jac[3]; //3 orthogonal linear constraints - btJacobianEntry m_jacAng[2]; //2 orthogonal angular constraints + btJacobianEntry m_jacAng[3]; //2 orthogonal angular constraints+ 1 for limit/motor btVector3 m_pivotInA; btVector3 m_pivotInB; btVector3 m_axisInA; btVector3 m_axisInB; - bool m_angularOnly; + bool m_angularOnly; + + float m_motorTargetVelocity; + float m_maxMotorImpulse; + bool m_enableAngularMotor; public: @@ -66,7 +70,12 @@ public: m_angularOnly = angularOnly; } - + void enableAngularMotor(bool enableMotor,float targetVelocity,float maxMotorImpulse) + { + m_enableAngularMotor = enableMotor; + m_motorTargetVelocity = targetVelocity; + m_maxMotorImpulse = maxMotorImpulse; + } }; diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp index ce85fc398be..94eece73e7c 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp @@ -23,6 +23,8 @@ subject to the following restrictions: #include "LinearMath/btIDebugDraw.h" #include "btJacobianEntry.h" #include "LinearMath/btMinMax.h" +#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h" + #ifdef USE_PROFILE #include "LinearMath/btQuickprof.h" @@ -123,7 +125,7 @@ btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver() } /// btSequentialImpulseConstraintSolver Sequentially applies impulses -float btSequentialImpulseConstraintSolver3::solveGroup(btPersistentManifold** manifoldPtr, int numManifolds,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) +float btSequentialImpulseConstraintSolver3::solveGroup(btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) { btContactSolverInfo info = infoGlobal; @@ -151,6 +153,15 @@ float btSequentialImpulseConstraintSolver3::solveGroup(btPersistentManifold** ma } } } + + { + int j; + for (j=0;jbuildJacobian(); + } + } //should traverse the contacts random order... int iteration; @@ -171,6 +182,12 @@ float btSequentialImpulseConstraintSolver3::solveGroup(btPersistentManifold** ma } } + for (j=0;jsolveConstraint(info.m_timeStep); + } + for (j=0;jbuildJacobian(); + } + } //should traverse the contacts random order... int iteration; @@ -230,6 +255,12 @@ float btSequentialImpulseConstraintSolver::solveGroup(btPersistentManifold** man { int j; + for (j=0;jsolveConstraint(info.m_timeStep); + } + for (j=0;j