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

process contact and non-contact constraints inside the same iteration loop

Browse Source 
added first draft for hingeConstraint motor
This commit is contained in:
ejcoumans
2006-12-12 03:15:11 +00:00
parent a4541d2470
commit 6dff5a218e
15 changed files with 267 additions and 203 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

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

2
src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp
View File

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

204
src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp
View File

@@ -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);
m_rbA.applyTorqueImpulse(-velrel+angularError);
m_rbB.applyTorqueImpulse(velrel-angularError);
btVector3 velrel = angVelAroundHingeAxisA - angVelAroundHingeAxisB;
btScalar projRelVel = velrel.dot(axisA);
btScalar desiredMotorVel = m_motorTargetVelocity;
btScalar motor_relvel = desiredMotorVel - projRelVel;
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
}

15
src/BulletDynamics/ConstraintSolver/btHingeConstraint.h
View File

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

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

@@ -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;j<numConstraints;j++)
{
btTypedConstraint* constraint = constraints[j];
constraint->buildJacobian();
}
}
//should traverse the contacts random order...
int iteration;
@@ -171,6 +182,12 @@ float btSequentialImpulseConstraintSolver3::solveGroup(btPersistentManifold** ma
}
}
for (j=0;j<numConstraints;j++)
{
btTypedConstraint* constraint = constraints[j];
constraint->solveConstraint(info.m_timeStep);
}
for (j=0;j<totalPoints;j++)
{
btPersistentManifold* manifold = manifoldPtr[gOrder[j].m_manifoldIndex];
@@ -197,7 +214,7 @@ float btSequentialImpulseConstraintSolver3::solveGroup(btPersistentManifold** ma
/// btSequentialImpulseConstraintSolver Sequentially applies impulses
float btSequentialImpulseConstraintSolver::solveGroup(btPersistentManifold** manifoldPtr, int numManifolds,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
float btSequentialImpulseConstraintSolver::solveGroup(btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
{
btContactSolverInfo info = infoGlobal;
@@ -222,6 +239,14 @@ float btSequentialImpulseConstraintSolver::solveGroup(btPersistentManifold** man
}
}
}
{
int j;
for (j=0;j<numConstraints;j++)
{
btTypedConstraint* constraint = constraints[j];
constraint->buildJacobian();
}
}
//should traverse the contacts random order...
int iteration;
@@ -230,6 +255,12 @@ float btSequentialImpulseConstraintSolver::solveGroup(btPersistentManifold** man
{
int j;
for (j=0;j<numConstraints;j++)
{
btTypedConstraint* constraint = constraints[j];
constraint->solveConstraint(info.m_timeStep);
}
for (j=0;j<numManifolds;j++)
{
btPersistentManifold* manifold = manifoldPtr[j];

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

@@ -68,7 +68,7 @@ public:
virtual ~btSequentialImpulseConstraintSolver() {}
virtual float solveGroup(btPersistentManifold** manifold,int numManifolds,const btContactSolverInfo& info, btIDebugDraw* debugDrawer=0);
virtual float solveGroup(btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer=0);
void setSolverMode(int mode)
{
@@ -88,7 +88,7 @@ public:
btSequentialImpulseConstraintSolver3();
virtual float solveGroup(btPersistentManifold** manifold,int numManifolds,const btContactSolverInfo& info, btIDebugDraw* debugDrawer=0);
virtual float solveGroup(btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer=0);
};