HingeConstraint constructor bug
Posted: Mon Mar 17, 2008 10:19 am
Hi,
I was toying with JBullet and had problems with HingeConstaint.
I first though it was a java port problem, but I checked in Bullet cpp source either :
When I use the constructor with pivot points and axis, the joint is correctly placed between two bodies but limits doesn't work (it works well with constructor giving 2 transforms).
After comparing results from both constructors, computed transforms are wrong (m_rbAFrame and m_rbBFrame).
Here the original code, with my comments :
A : Ok, start computing m_rbAFrame with the pseudo X axis
B : the projection of rbAxisA1 on axisInA (with is the pseudo Z axis)
C : ?? if the projection is >0, go on with rbAxisA1 ...
what happens if projection == 1.0 (ie rbAxisA1 and axisInA are aligned) in the cross product [E]?
D : take another axis to start with (Y), but store it in rbAxisA1 (so X and Y axis will be inverted ?)
F : Bullet is right handed, if I'm not wrong , so I think the cross product is inverted.
Here how I coded it (I test it in java with JBullet, so this is a cpp to java backporting, and I haven't compiled it) :
If this is correct, the constructor btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA) needs the same correction.
I was toying with JBullet and had problems with HingeConstaint.
I first though it was a java port problem, but I checked in Bullet cpp source either :
When I use the constructor with pivot points and axis, the joint is correctly placed between two bodies but limits doesn't work (it works well with constructor giving 2 transforms).
After comparing results from both constructors, computed transforms are wrong (m_rbAFrame and m_rbBFrame).
Here the original code, with my comments :
Code: Select all
btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB,
btVector3& axisInA,btVector3& axisInB)
:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),
m_angularOnly(false),
m_enableAngularMotor(false)
{
m_rbAFrame.getOrigin() = pivotInA;
// since no frame is given, assume this to be zero angle and just pick rb transform axis
btVector3 rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(0); <= [A]
btScalar projection = rbAxisA1.dot(axisInA); <= [B]
if (projection > SIMD_EPSILON)
rbAxisA1 = rbAxisA1*projection - axisInA; <= [C]
else
rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(1); <= [D]
btVector3 rbAxisA2 = rbAxisA1.cross(axisInA); <= [E]
m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(),
rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(),
rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() );
btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB);
btVector3 rbAxisB1 = quatRotate(rotationArc,rbAxisA1);
btVector3 rbAxisB2 = rbAxisB1.cross(axisInB); <= [F]
m_rbBFrame.getOrigin() = pivotInB;
m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),-axisInB.getX(),
rbAxisB1.getY(),rbAxisB2.getY(),-axisInB.getY(),
rbAxisB1.getZ(),rbAxisB2.getZ(),-axisInB.getZ() );
//start with free
m_lowerLimit = btScalar(1e30);
m_upperLimit = btScalar(-1e30);
m_biasFactor = 0.3f;
m_relaxationFactor = 1.0f;
m_limitSoftness = 0.9f;
m_solveLimit = false;
}
A : Ok, start computing m_rbAFrame with the pseudo X axis
B : the projection of rbAxisA1 on axisInA (with is the pseudo Z axis)
C : ?? if the projection is >0, go on with rbAxisA1 ...
what happens if projection == 1.0 (ie rbAxisA1 and axisInA are aligned) in the cross product [E]?
D : take another axis to start with (Y), but store it in rbAxisA1 (so X and Y axis will be inverted ?)
F : Bullet is right handed, if I'm not wrong , so I think the cross product is inverted.
Here how I coded it (I test it in java with JBullet, so this is a cpp to java backporting, and I haven't compiled it) :
Code: Select all
btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB,
btVector3& axisInA,btVector3& axisInB)
:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),
m_angularOnly(false),
m_enableAngularMotor(false)
{
m_rbAFrame.getOrigin() = pivotInA;
// since no frame is given, assume this to be zero angle and just pick rb transform axis
btVector3 rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(0);
btVector3 rbAxisA2 ;
btScalar projection = axisInA.dot(rbAxisA1);
if (abs(projection) > btScalar(1.0) - SIMD_EPSILON) { // <= not sure how to do a abs() in cpp
// axis are almost aligned, take a new axis (Y)
rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1);
rbAxisA2 = rbAxisA2 - axisInA*(axisInA.dot(rbAxisA2));
rbAxisA1 = rbAxisA2.cross(axisInA);
} else {
rbAxisA1 = rbAxisA1 - axisInA*projection;
rbAxisA2 = axisInA.cross(rbAxisA1);
}
m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(),
rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(),
rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() );
btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB);
btVector3 rbAxisB1 = quatRotate(rotationArc,rbAxisA1);
btVector3 rbAxisB2 = axisInB.cross(rbAxisB1);
m_rbBFrame.getOrigin() = pivotInB;
m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),-axisInB.getX(),
rbAxisB1.getY(),rbAxisB2.getY(),-axisInB.getY(),
rbAxisB1.getZ(),rbAxisB2.getZ(),-axisInB.getZ() );
//start with free
m_lowerLimit = btScalar(1e30);
m_upperLimit = btScalar(-1e30);
m_biasFactor = 0.3f;
m_relaxationFactor = 1.0f;
m_limitSoftness = 0.9f;
m_solveLimit = false;
}
If this is correct, the constructor btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA) needs the same correction.