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

fix rotation correction in btFixedConstraint, see Issue 769

Browse Source
This commit is contained in:
erwin.coumans@gmail.com
2013-12-04 19:06:28 +00:00
parent 1362b6ab48
commit d48e2c717c
2 changed files with 100 additions and 44 deletions
Download Patch File Download Diff File Expand all files Collapse all files

139
src/BulletDynamics/ConstraintSolver/btFixedConstraint.cpp
View File

@@ -23,9 +23,8 @@ subject to the following restrictions:
btFixedConstraint::btFixedConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& frameInA,const btTransform& frameInB) btFixedConstraint::btFixedConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& frameInA,const btTransform& frameInB)
:btTypedConstraint(FIXED_CONSTRAINT_TYPE,rbA,rbB) :btTypedConstraint(FIXED_CONSTRAINT_TYPE,rbA,rbB)
{ {
m_pivotInA = frameInA.getOrigin(); m_frameInA = frameInA;
m_pivotInB = frameInB.getOrigin(); m_frameInB = frameInB;
m_relTargetAB = frameInA.getRotation()*frameInB.getRotation().inverse();
} }
@@ -37,14 +36,16 @@ btFixedConstraint::~btFixedConstraint ()
void btFixedConstraint::getInfo1 (btConstraintInfo1* info) void btFixedConstraint::getInfo1 (btConstraintInfo1* info)
{ {
info->m_numConstraintRows = 6; info->m_numConstraintRows = 6;
info->nub = 6; info->nub = 0;
} }
void btFixedConstraint::getInfo2 (btConstraintInfo2* info) void btFixedConstraint::getInfo2 (btConstraintInfo2* info)
{ {
//fix the 3 linear degrees of freedom //fix the 3 linear degrees of freedom
const btTransform& transA = m_rbA.getCenterOfMassTransform();
const btTransform& transB = m_rbB.getCenterOfMassTransform();
const btVector3& worldPosA = m_rbA.getCenterOfMassTransform().getOrigin(); const btVector3& worldPosA = m_rbA.getCenterOfMassTransform().getOrigin();
const btMatrix3x3& worldOrnA = m_rbA.getCenterOfMassTransform().getBasis(); const btMatrix3x3& worldOrnA = m_rbA.getCenterOfMassTransform().getBasis();
const btVector3& worldPosB= m_rbB.getCenterOfMassTransform().getOrigin(); const btVector3& worldPosB= m_rbB.getCenterOfMassTransform().getOrigin();
@@ -55,15 +56,15 @@ void btFixedConstraint::getInfo2 (btConstraintInfo2* info)
info->m_J1linearAxis[info->rowskip+1] = 1; info->m_J1linearAxis[info->rowskip+1] = 1;
info->m_J1linearAxis[2*info->rowskip+2] = 1; info->m_J1linearAxis[2*info->rowskip+2] = 1;
btVector3 a1 = worldOrnA*m_pivotInA; btVector3 a1 = worldOrnA * m_frameInA.getOrigin();
{ {
btVector3* angular0 = (btVector3*)(info->m_J1angularAxis); btVector3* angular0 = (btVector3*)(info->m_J1angularAxis);
btVector3* angular1 = (btVector3*)(info->m_J1angularAxis+info->rowskip); btVector3* angular1 = (btVector3*)(info->m_J1angularAxis+info->rowskip);
btVector3* angular2 = (btVector3*)(info->m_J1angularAxis+2*info->rowskip); btVector3* angular2 = (btVector3*)(info->m_J1angularAxis+2*info->rowskip);
btVector3 a1neg = -a1; btVector3 a1neg = -a1;
a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2); a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
} }
if (info->m_J2linearAxis) if (info->m_J2linearAxis)
{ {
info->m_J2linearAxis[0] = -1; info->m_J2linearAxis[0] = -1;
@@ -71,10 +72,8 @@ void btFixedConstraint::getInfo2 (btConstraintInfo2* info)
info->m_J2linearAxis[2*info->rowskip+2] = -1; info->m_J2linearAxis[2*info->rowskip+2] = -1;
} }
btVector3 a2 = worldOrnB*m_pivotInB; btVector3 a2 = worldOrnB*m_frameInB.getOrigin();
{
{
// btVector3 a2n = -a2;
btVector3* angular0 = (btVector3*)(info->m_J2angularAxis); btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
btVector3* angular1 = (btVector3*)(info->m_J2angularAxis+info->rowskip); btVector3* angular1 = (btVector3*)(info->m_J2angularAxis+info->rowskip);
btVector3* angular2 = (btVector3*)(info->m_J2angularAxis+2*info->rowskip); btVector3* angular2 = (btVector3*)(info->m_J2angularAxis+2*info->rowskip);
@@ -88,42 +87,100 @@ void btFixedConstraint::getInfo2 (btConstraintInfo2* info)
int j; int j;
for (j=0; j<3; j++) for (j=0; j<3; j++)
{ {
info->m_constraintError[j*info->rowskip] = linearError[j]; info->m_constraintError[j*info->rowskip] = linearError[j];
//printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]); //printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]);
} }
//fix the 3 angular degrees of freedom btVector3 ivA = transA.getBasis() * m_frameInA.getBasis().getColumn(0);
btVector3 jvA = transA.getBasis() * m_frameInA.getBasis().getColumn(1);
btVector3 kvA = transA.getBasis() * m_frameInA.getBasis().getColumn(2);
btVector3 ivB = transB.getBasis() * m_frameInB.getBasis().getColumn(0);
btVector3 target;
btScalar x = ivB.dot(ivA);
btScalar y = ivB.dot(jvA);
btScalar z = ivB.dot(kvA);
btVector3 swingAxis(0,0,0);
{
if((!btFuzzyZero(y)) || (!(btFuzzyZero(z))))
{
swingAxis = -ivB.cross(ivA);
}
}
btVector3 vTwist(1,0,0);
int start_row = 3; // compute rotation of A wrt B (in constraint space)
int s = info->rowskip; btQuaternion qA = transA.getRotation() * m_frameInA.getRotation();
int start_index = start_row * s; btQuaternion qB = transB.getRotation() * m_frameInB.getRotation();
btQuaternion qAB = qB.inverse() * qA;
// split rotation into cone and twist
// (all this is done from B's perspective. Maybe I should be averaging axes...)
btVector3 vConeNoTwist = quatRotate(qAB, vTwist); vConeNoTwist.normalize();
btQuaternion qABCone = shortestArcQuat(vTwist, vConeNoTwist); qABCone.normalize();
btQuaternion qABTwist = qABCone.inverse() * qAB; qABTwist.normalize();
// 3 rows to make body rotations equal int row = 3;
info->m_J1angularAxis[start_index] = 1; int srow = row * info->rowskip;
info->m_J1angularAxis[start_index + s + 1] = 1; btVector3 ax1;
info->m_J1angularAxis[start_index + s*2+2] = 1; // angular limits
if ( info->m_J2angularAxis) {
{ btScalar *J1 = info->m_J1angularAxis;
info->m_J2angularAxis[start_index] = -1; btScalar *J2 = info->m_J2angularAxis;
info->m_J2angularAxis[start_index + s+1] = -1; btTransform trA = transA*m_frameInA;
info->m_J2angularAxis[start_index + s*2+2] = -1; btVector3 twistAxis = trA.getBasis().getColumn(0);
}
// set right hand side for the angular dofs btVector3 p = trA.getBasis().getColumn(1);
btVector3 q = trA.getBasis().getColumn(2);
int srow1 = srow + info->rowskip;
J1[srow+0] = p[0];
J1[srow+1] = p[1];
J1[srow+2] = p[2];
J1[srow1+0] = q[0];
J1[srow1+1] = q[1];
J1[srow1+2] = q[2];
J2[srow+0] = -p[0];
J2[srow+1] = -p[1];
J2[srow+2] = -p[2];
J2[srow1+0] = -q[0];
J2[srow1+1] = -q[1];
J2[srow1+2] = -q[2];
btScalar fact = info->fps;
info->m_constraintError[srow] = fact * swingAxis.dot(p);
info->m_constraintError[srow1] = fact * swingAxis.dot(q);
info->m_lowerLimit[srow] = -SIMD_INFINITY;
info->m_upperLimit[srow] = SIMD_INFINITY;
info->m_lowerLimit[srow1] = -SIMD_INFINITY;
info->m_upperLimit[srow1] = SIMD_INFINITY;
srow = srow1 + info->rowskip;
btVector3 diff; {
btScalar angle; btQuaternion qMinTwist = qABTwist;
btMatrix3x3 mrelCur = worldOrnA *worldOrnB.inverse(); btScalar twistAngle = qABTwist.getAngle();
btQuaternion qrelCur;
mrelCur.getRotation(qrelCur);
btTransformUtil::calculateDiffAxisAngleQuaternion(m_relTargetAB,qrelCur,diff,angle);
diff*=-angle;
for (j=0; j<3; j++)
{
info->m_constraintError[(3+j)*info->rowskip] = k * diff[j];
}
if (twistAngle > SIMD_PI) // long way around. flip quat and recalculate.
{
qMinTwist = -(qABTwist);
twistAngle = qMinTwist.getAngle();
}
if (twistAngle > SIMD_EPSILON)
{
twistAxis = btVector3(qMinTwist.x(), qMinTwist.y(), qMinTwist.z());
twistAxis.normalize();
twistAxis = quatRotate(qB, -twistAxis);
}
ax1 = twistAxis;
btScalar *J1 = info->m_J1angularAxis;
btScalar *J2 = info->m_J2angularAxis;
J1[srow+0] = ax1[0];
J1[srow+1] = ax1[1];
J1[srow+2] = ax1[2];
J2[srow+0] = -ax1[0];
J2[srow+1] = -ax1[1];
J2[srow+2] = -ax1[2];
btScalar k = info->fps;
info->m_constraintError[srow] = k * twistAngle;
info->m_lowerLimit[srow] = -SIMD_INFINITY;
info->m_upperLimit[srow] = SIMD_INFINITY;
}
}
} }

5
src/BulletDynamics/ConstraintSolver/btFixedConstraint.h
View File

@@ -20,10 +20,9 @@ subject to the following restrictions:
ATTRIBUTE_ALIGNED16(class) btFixedConstraint : public btTypedConstraint ATTRIBUTE_ALIGNED16(class) btFixedConstraint : public btTypedConstraint
{ {
btVector3 m_pivotInA;
btVector3 m_pivotInB;
btQuaternion m_relTargetAB;
btTransform m_frameInA;
btTransform m_frameInB;
public: public:
btFixedConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& frameInA,const btTransform& frameInB); btFixedConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& frameInA,const btTransform& frameInB);