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

+ need to reset rigid body using 'setCenterOfMassTransform' to reset world inertia tensor

Browse Source 
+ fixes in compound algorithm -> recompute contact points
+ add debug drawing to some demos.
+ revert btJacobianEntry change
+ replace dCROSSMAT by btVector3::getSkewSymmetricMatrix
This commit is contained in:
erwin.coumans
2008-12-02 04:01:56 +00:00
parent f0c302f65c
commit 5383ed4930
16 changed files with 215 additions and 127 deletions
Download Patch File Download Diff File Expand all files Collapse all files

16
src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp
View File

@@ -118,13 +118,15 @@ void btConeTwistConstraint::buildJacobian()
for (int i=0;i<3;i++)
{
new (&m_jac[i]) btJacobianEntry(
pivotAInW - m_rbA.getCenterOfMassPosition(),
pivotBInW - m_rbB.getCenterOfMassPosition(),
normal[i],
m_rbA.getInvInertiaDiagLocal(),
m_rbA.getInvMass(),
m_rbB.getInvInertiaDiagLocal(),
m_rbB.getInvMass());
m_rbA.getCenterOfMassTransform().getBasis().transpose(),
m_rbB.getCenterOfMassTransform().getBasis().transpose(),
pivotAInW - m_rbA.getCenterOfMassPosition(),
pivotBInW - m_rbB.getCenterOfMassPosition(),
normal[i],
m_rbA.getInvInertiaDiagLocal(),
m_rbA.getInvMass(),
m_rbB.getInvInertiaDiagLocal(),
m_rbB.getInvMass());
}
}

3
src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp
View File

@@ -52,7 +52,8 @@ void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1,
btVector3 vel = vel1 - vel2;
btJacobianEntry jac(
btJacobianEntry jac(body1.getCenterOfMassTransform().getBasis().transpose(),
body2.getCenterOfMassTransform().getBasis().transpose(),
rel_pos1,rel_pos2,normal,body1.getInvInertiaDiagLocal(),body1.getInvMass(),
body2.getInvInertiaDiagLocal(),body2.getInvMass());

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

@@ -44,17 +44,6 @@ m_useSolveConstraintObsolete(true)
}
#define dCROSSMAT(A,a,skip,plus,minus) \
{ \
(A)[1] = minus (a)[2]; \
(A)[2] = plus (a)[1]; \
(A)[(skip)+0] = plus (a)[2]; \
(A)[(skip)+2] = minus (a)[0]; \
(A)[2*(skip)+0] = minus (a)[1]; \
(A)[2*(skip)+1] = plus (a)[0]; \
}
#define GENERIC_D6_DISABLE_WARMSTARTING 1
btScalar btGetMatrixElem(const btMatrix3x3& mat, int index);
@@ -374,25 +363,25 @@ void btGeneric6DofConstraint::buildLinearJacobian(
const btVector3 & pivotAInW,const btVector3 & pivotBInW)
{
new (&jacLinear) btJacobianEntry(
pivotAInW - m_rbA.getCenterOfMassPosition(),
pivotBInW - m_rbB.getCenterOfMassPosition(),
normalWorld,
m_rbA.getInvInertiaDiagLocal(),
m_rbA.getInvMass(),
m_rbB.getInvInertiaDiagLocal(),
m_rbB.getInvMass());
m_rbA.getCenterOfMassTransform().getBasis().transpose(),
m_rbB.getCenterOfMassTransform().getBasis().transpose(),
pivotAInW - m_rbA.getCenterOfMassPosition(),
pivotBInW - m_rbB.getCenterOfMassPosition(),
normalWorld,
m_rbA.getInvInertiaDiagLocal(),
m_rbA.getInvMass(),
m_rbB.getInvInertiaDiagLocal(),
m_rbB.getInvMass());
}
void btGeneric6DofConstraint::buildAngularJacobian(
btJacobianEntry & jacAngular,const btVector3 & jointAxisW)
{
new (&jacAngular) btJacobianEntry(jointAxisW,
m_rbA.getCenterOfMassTransform().getBasis().transpose(),
m_rbB.getCenterOfMassTransform().getBasis().transpose(),
m_rbA.getInvInertiaDiagLocal(),
m_rbB.getInvInertiaDiagLocal());
new (&jacAngular) btJacobianEntry(jointAxisW,
m_rbA.getCenterOfMassTransform().getBasis().transpose(),
m_rbB.getCenterOfMassTransform().getBasis().transpose(),
m_rbA.getInvInertiaDiagLocal(),
m_rbB.getInvInertiaDiagLocal());
}
@@ -517,12 +506,34 @@ int btGeneric6DofConstraint::setLinearLimits(btConstraintInfo2* info)
info->m_J1linearAxis[s+1] = 1;
info->m_J1linearAxis[2*s+2] = 1;
/*for (int i=0;i<3;i++)
{
if (m_useLinearReferenceFrameA)
{
btVector3* linear_axis = (btVector3* )&info->m_J1linearAxis[s*i];
*linear_axis = m_calculatedTransformA.getBasis().getColumn(i);
}
else
{
btVector3* linear_axis = (btVector3* )&info->m_J1linearAxis[s*i];
*linear_axis = m_calculatedTransformB.getBasis().getColumn(i);
}
}
*/
btVector3 a1,a2;
a1 = body0_trans.getBasis()*d6constraint->getFrameOffsetA().getOrigin();
//dMULTIPLY0_331 (a1, body0_mat,m_constraint->m_pivotInA);
dCROSSMAT (info->m_J1angularAxis,a1,s,-,+);
{
btVector3* angular0 = (btVector3*)(info->m_J1angularAxis);
btVector3* angular1 = (btVector3*)(info->m_J1angularAxis+info->rowskip);
btVector3* angular2 = (btVector3*)(info->m_J1angularAxis+2*info->rowskip);
btVector3 a1neg = -a1;
a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
}
/*info->m_J2linearAxis[0] = -1;
info->m_J2linearAxis[s+1] = -1;
@@ -530,15 +541,19 @@ int btGeneric6DofConstraint::setLinearLimits(btConstraintInfo2* info)
*/
a2 = body1_trans.getBasis()*d6constraint->getFrameOffsetB().getOrigin();
//dMULTIPLY0_331 (a2,body1_mat,m_constraint->m_pivotInB);
dCROSSMAT (info->m_J2angularAxis,a2,s,+,-);
{
btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
btVector3* angular1 = (btVector3*)(info->m_J2angularAxis+info->rowskip);
btVector3* angular2 = (btVector3*)(info->m_J2angularAxis+2*info->rowskip);
a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
}
// set right hand side
btScalar k = info->fps * info->erp;
for (int j=0; j<3; j++)
{
info->m_constraintError[s*j] = k * (a2[j] + body1_trans.getOrigin()[j] -
a1[j] - body0_trans.getOrigin()[j]);
info->m_constraintError[s*j] = k * (a2[j] + body1_trans.getOrigin()[j] - a1[j] - body0_trans.getOrigin()[j]);
}
return 3;

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

@@ -189,13 +189,15 @@ void btHingeConstraint::buildJacobian()
for (int i=0;i<3;i++)
{
new (&m_jac[i]) btJacobianEntry(
pivotAInW - m_rbA.getCenterOfMassPosition(),
pivotBInW - m_rbB.getCenterOfMassPosition(),
normal[i],
m_rbA.getInvInertiaDiagLocal(),
m_rbA.getInvMass(),
m_rbB.getInvInertiaDiagLocal(),
m_rbB.getInvMass());
m_rbA.getCenterOfMassTransform().getBasis().transpose(),
m_rbB.getCenterOfMassTransform().getBasis().transpose(),
pivotAInW - m_rbA.getCenterOfMassPosition(),
pivotBInW - m_rbB.getCenterOfMassPosition(),
normal[i],
m_rbA.getInvInertiaDiagLocal(),
m_rbA.getInvMass(),
m_rbB.getInvInertiaDiagLocal(),
m_rbB.getInvMass());
}
}

16
src/BulletDynamics/ConstraintSolver/btJacobianEntry.h
View File

@@ -34,6 +34,8 @@ public:
btJacobianEntry() {};
//constraint between two different rigidbodies
btJacobianEntry(
const btMatrix3x3& world2A,
const btMatrix3x3& world2B,
const btVector3& rel_pos1,const btVector3& rel_pos2,
const btVector3& jointAxis,
const btVector3& inertiaInvA,
@@ -42,8 +44,8 @@ public:
const btScalar massInvB)
:m_linearJointAxis(jointAxis)
{
m_aJ = (rel_pos1.cross(m_linearJointAxis));
m_bJ = (rel_pos2.cross(-m_linearJointAxis));
m_aJ = world2A*(rel_pos1.cross(m_linearJointAxis));
m_bJ = world2B*(rel_pos2.cross(-m_linearJointAxis));
m_0MinvJt = inertiaInvA * m_aJ;
m_1MinvJt = inertiaInvB * m_bJ;
m_Adiag = massInvA + m_0MinvJt.dot(m_aJ) + massInvB + m_1MinvJt.dot(m_bJ);
@@ -59,8 +61,8 @@ public:
const btVector3& inertiaInvB)
:m_linearJointAxis(btVector3(btScalar(0.),btScalar(0.),btScalar(0.)))
{
m_aJ= jointAxis;
m_bJ = -jointAxis;
m_aJ= world2A*jointAxis;
m_bJ = world2B*-jointAxis;
m_0MinvJt = inertiaInvA * m_aJ;
m_1MinvJt = inertiaInvB * m_bJ;
m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ);
@@ -93,8 +95,8 @@ public:
const btScalar massInvA)
:m_linearJointAxis(jointAxis)
{
m_aJ= (rel_pos1.cross(jointAxis));
m_bJ = (rel_pos2.cross(-jointAxis));
m_aJ= world2A*(rel_pos1.cross(jointAxis));
m_bJ = world2A*(rel_pos2.cross(-jointAxis));
m_0MinvJt = inertiaInvA * m_aJ;
m_1MinvJt = btVector3(btScalar(0.),btScalar(0.),btScalar(0.));
m_Adiag = massInvA + m_0MinvJt.dot(m_aJ);
@@ -128,7 +130,7 @@ public:
return sum[0]+sum[1]+sum[2];
}
btScalar getRelativeVelocity(const btVector3& linvelA,const btVector3& angvelA,const btVector3& linvelB,const btVector3& angvelB) const
btScalar getRelativeVelocity(const btVector3& linvelA,const btVector3& angvelA,const btVector3& linvelB,const btVector3& angvelB)
{
btVector3 linrel = linvelA - linvelB;
btVector3 angvela = angvelA * m_aJ;

107
src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp
View File

@@ -43,7 +43,7 @@ m_useSolveConstraintObsolete(false)
void btPoint2PointConstraint::buildJacobian()
{
if (m_useSolveConstraintObsolete)
///we need it for both methods
{
m_appliedImpulse = btScalar(0.);
@@ -53,15 +53,16 @@ void btPoint2PointConstraint::buildJacobian()
{
normal[i] = 1;
new (&m_jac[i]) btJacobianEntry(
m_rbA.getCenterOfMassTransform()*m_pivotInA - m_rbA.getCenterOfMassPosition(),
m_rbB.getCenterOfMassTransform()*m_pivotInB - m_rbB.getCenterOfMassPosition(),
normal,
m_rbA.getInvInertiaDiagLocal(),
m_rbA.getInvMass(),
m_rbB.getInvInertiaDiagLocal(),
m_rbB.getInvMass());
normal[i] = 0;
m_rbA.getCenterOfMassTransform().getBasis().transpose(),
m_rbB.getCenterOfMassTransform().getBasis().transpose(),
m_rbA.getCenterOfMassTransform()*m_pivotInA - m_rbA.getCenterOfMassPosition(),
m_rbB.getCenterOfMassTransform()*m_pivotInB - m_rbB.getCenterOfMassPosition(),
normal,
m_rbA.getInvInertiaDiagLocal(),
m_rbA.getInvMass(),
m_rbB.getInvInertiaDiagLocal(),
m_rbB.getInvMass());
normal[i] = 0;
}
}
@@ -80,16 +81,6 @@ void btPoint2PointConstraint::getInfo1 (btConstraintInfo1* info)
info->nub = 3;
}
}
#define dCROSSMAT(A,a,skip,plus,minus) \
{ \
(A)[1] = minus (a)[2]; \
(A)[2] = plus (a)[1]; \
(A)[(skip)+0] = plus (a)[2]; \
(A)[(skip)+2] = minus (a)[0]; \
(A)[2*(skip)+0] = minus (a)[1]; \
(A)[2*(skip)+1] = plus (a)[0]; \
}
#include <stdio.h>
void btPoint2PointConstraint::getInfo2 (btConstraintInfo2* info)
{
@@ -102,48 +93,55 @@ void btPoint2PointConstraint::getInfo2 (btConstraintInfo2* info)
body1_trans = m_rbB.getCenterOfMassTransform();
// anchor points in global coordinates with respect to body PORs.
int s = info->rowskip;
// set jacobian
info->m_J1linearAxis[0] = 1;
info->m_J1linearAxis[s+1] = 1;
info->m_J1linearAxis[2*s+2] = 1;
info->m_J1linearAxis[info->rowskip+1] = 1;
info->m_J1linearAxis[2*info->rowskip+2] = 1;
btVector3 a1,a2;
a1 = body0_trans.getBasis()*getPivotInA();
//dMULTIPLY0_331 (a1, body0_mat,m_constraint->m_pivotInA);
dCROSSMAT (info->m_J1angularAxis,a1,s,-,+);
btVector3 a1 = body0_trans.getBasis()*getPivotInA();
{
btVector3* angular0 = (btVector3*)(info->m_J1angularAxis);
btVector3* angular1 = (btVector3*)(info->m_J1angularAxis+info->rowskip);
btVector3* angular2 = (btVector3*)(info->m_J1angularAxis+2*info->rowskip);
btVector3 a1neg = -a1;
a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
}
/*info->m_J2linearAxis[0] = -1;
info->m_J2linearAxis[s+1] = -1;
info->m_J2linearAxis[2*s+2] = -1;
*/
a2 = body1_trans.getBasis()*getPivotInB();
//dMULTIPLY0_331 (a2,body1_mat,m_constraint->m_pivotInB);
//dCROSSMAT (info->m_J2angularAxis,a2,s,+,-);
dCROSSMAT (info->m_J2angularAxis,a2,s,+,-);
btVector3 a2 = body1_trans.getBasis()*getPivotInB();
{
btVector3 a2n = -a2;
btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
btVector3* angular1 = (btVector3*)(info->m_J2angularAxis+info->rowskip);
btVector3* angular2 = (btVector3*)(info->m_J2angularAxis+2*info->rowskip);
a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
}
// set right hand side
btScalar k = info->fps * info->erp;
int j;
for (j=0; j<3; j++)
{
info->m_constraintError[j*s] = k * (a2[j] + body1_trans.getOrigin()[j] - a1[j] - body0_trans.getOrigin()[j]);
info->m_constraintError[j*info->rowskip] = k * (a2[j] + body1_trans.getOrigin()[j] - a1[j] - body0_trans.getOrigin()[j]);
//printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]);
}
btScalar impulseClamp = m_setting.m_impulseClamp;
btScalar impulseClamp = m_setting.m_impulseClamp;//
for (j=0; j<3; j++)
{
if (impulseClamp > 0)
if (m_setting.m_impulseClamp > 0)
{
info->m_lowerLimit[j*s] = -impulseClamp;
info->m_upperLimit[j*s] = impulseClamp;
info->m_lowerLimit[j*info->rowskip] = -impulseClamp;
info->m_upperLimit[j*info->rowskip] = impulseClamp;
}
}
@@ -190,24 +188,33 @@ void btPoint2PointConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolv
//positional error (zeroth order error)
btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
btScalar impulse = depth*m_setting.m_tau/timeStep * jacDiagABInv - m_setting.m_damping * rel_vel * jacDiagABInv;
btScalar deltaImpulse = depth*m_setting.m_tau/timeStep * jacDiagABInv - m_setting.m_damping * rel_vel * jacDiagABInv;
btScalar impulseClamp = m_setting.m_impulseClamp;
if (impulseClamp > 0)
const btScalar sum = btScalar(m_appliedImpulse) + deltaImpulse;
if (sum < -impulseClamp)
{
if (impulse < -impulseClamp)
impulse = -impulseClamp;
if (impulse > impulseClamp)
impulse = impulseClamp;
deltaImpulse = -impulseClamp-m_appliedImpulse;
m_appliedImpulse = -impulseClamp;
}
else if (sum > impulseClamp)
{
deltaImpulse = impulseClamp-m_appliedImpulse;
m_appliedImpulse = impulseClamp;
}
else
{
m_appliedImpulse = sum;
}
m_appliedImpulse+=impulse;
btVector3 impulse_vector = normal * impulse;
btVector3 impulse_vector = normal * deltaImpulse;
btVector3 ftorqueAxis1 = rel_pos1.cross(normal);
btVector3 ftorqueAxis2 = rel_pos2.cross(normal);
bodyA.applyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,impulse);
bodyB.applyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-impulse);
bodyA.applyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,deltaImpulse);
bodyB.applyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-deltaImpulse);
normal[i] = 0;

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

@@ -382,6 +382,16 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
return 0.f;
}
if (1)
{
int j;
for (j=0;j<numConstraints;j++)
{
btTypedConstraint* constraint = constraints[j];
constraint->buildJacobian();
}
}
btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
initSolverBody(&fixedBody,0);
@@ -406,10 +416,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
btTypedConstraint::btConstraintInfo1 info1;
info1.m_numConstraintRows = 0;
if (m_tmpSolverNonContactConstraintPool.size()>3)
{
printf("dsadas\n");
}
///setup the btSolverConstraints
int currentRow = 0;
@@ -492,7 +499,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
sum += iMJlB.dot(solverConstraint.m_contactNormal);
sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal);
solverConstraint.m_jacDiagABInv = 1./sum;
solverConstraint.m_jacDiagABInv = btScalar(1.)/sum;
}
@@ -551,6 +558,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
///this is a bad test and results in jitter -> always solve for those zero-distanc contacts!
///-> if (cp.getDistance() <= btScalar(0.))
//if (cp.getDistance() <= manifold->getContactBreakingThreshold())
{
const btVector3& pos1 = cp.getPositionWorldOnA();
@@ -772,15 +780,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
btContactSolverInfo info = infoGlobal;
if (1)
{
int j;
for (j=0;j<numConstraints;j++)
{
btTypedConstraint* constraint = constraints[j];
constraint->buildJacobian();
}
}
int numConstraintPool = m_tmpSolverContactConstraintPool.size();
int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();

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

@@ -119,6 +119,8 @@ void btSliderConstraint::buildJacobianInt(btRigidBody& rbA, btRigidBody& rbB, co
{
normalWorld = m_calculatedTransformA.getBasis().getColumn(i);
new (&m_jacLin[i]) btJacobianEntry(
rbA.getCenterOfMassTransform().getBasis().transpose(),
rbB.getCenterOfMassTransform().getBasis().transpose(),
m_relPosA,
m_relPosB,
normalWorld,

9
src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp
View File

@@ -61,10 +61,9 @@ void btSolve2LinearConstraint::resolveUnilateralPairConstraint(
//this jacobian entry could be re-used for all iterations
btJacobianEntry jacA(
rel_posA1,rel_posA2,normalA,invInertiaADiag,invMassA,
btJacobianEntry jacA(world2A,world2B,rel_posA1,rel_posA2,normalA,invInertiaADiag,invMassA,
invInertiaBDiag,invMassB);
btJacobianEntry jacB(rel_posB1,rel_posB2,normalB,invInertiaADiag,invMassA,
btJacobianEntry jacB(world2A,world2B,rel_posB1,rel_posB2,normalB,invInertiaADiag,invMassA,
invInertiaBDiag,invMassB);
//const btScalar vel0 = jacA.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);
@@ -151,9 +150,9 @@ void btSolve2LinearConstraint::resolveBilateralPairConstraint(
//this jacobian entry could be re-used for all iterations
btJacobianEntry jacA(rel_posA1,rel_posA2,normalA,invInertiaADiag,invMassA,
btJacobianEntry jacA(world2A,world2B,rel_posA1,rel_posA2,normalA,invInertiaADiag,invMassA,
invInertiaBDiag,invMassB);
btJacobianEntry jacB(rel_posB1,rel_posB2,normalB,invInertiaADiag,invMassA,
btJacobianEntry jacB(world2A,world2B,rel_posB1,rel_posB2,normalB,invInertiaADiag,invMassA,
invInertiaBDiag,invMassB);
//const btScalar vel0 = jacA.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);