HingeConstraint constructor bug

LvR
Posts: 13
Joined: Mon Mar 17, 2008 8:09 am

HingeConstraint constructor bug

Post by LvR »

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 :

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.
User avatar
Erwin Coumans
Site Admin
Posts: 4221
Joined: Sun Jun 26, 2005 6:43 pm
Location: California, USA
Contact:

Re: HingeConstraint constructor bug

Post by Erwin Coumans »

Thanks for pointing it out. Yes, there is already a bug report here:

http://code.google.com/p/bullet/issues/detail?id=41

We'll check it out,
Erwin
User avatar
Dragonlord
Posts: 198
Joined: Mon Sep 04, 2006 5:31 pm
Location: Switzerland
Contact:

Re: HingeConstraint constructor bug

Post by Dragonlord »

The above mentioned problem had been the cause of my hinge not working. After giving it some thought I used the other constructor which I had to hack to get it working the way I required it. The problem here is that the constructor here is not very useful in the first place for limited hinges. A point and an axis does not define uniquely a coordinate system which is required for a hinge with limits. For a hinge without limits this works and this is most probably why it doesn't work in the case with limits. I would therefore not call the constructor incorrect or malfunctioning. Mathematically it only works for a free hinge and for a limited hinge it is under-defined. Hence I would not propose to fix the constructor since it is doing fine what it's supposed to do. If you need a hinge with limits use a proper coordinate system ( using btTransform ) using the existing constructor for two bodies with two reference frames.
LvR
Posts: 13
Joined: Mon Mar 17, 2008 8:09 am

Re: HingeConstraint constructor bug

Post by LvR »

Thx for your answer, I should have check bug reports first ;), it would have save me time (but I haven't done maths for a while, so it's good to pratice again)

Maybe you should add some comments to the constructor if it's a known issue or if usage is limited with no limits.

And what do you think about my correction (which work with my test case with limits) ? The computed tranforms look more correct.
User avatar
Dragonlord
Posts: 198
Joined: Mon Sep 04, 2006 5:31 pm
Location: Switzerland
Contact:

Re: HingeConstraint constructor bug

Post by Dragonlord »

I took a closer look at the code on a piece of paper. The calculation there is indeed incorrect and results in a 0-vector in certain cases. Your code though also contains errors in my opinion but let's check this out in detail. I'll first sum write up your code in pseudo notation ( A is frame of body 1, p the projection, a the rotation axis and a* the final axes ):

Code: Select all

a1 = A.x
p = a ° a1
if abs( p ) ~ 1:
   a2 = A.y
   a2 -= a * ( a ° a2 )
   a1 = a2 x a
else:
   a1 -= a * p
   a2 = a x a1
First problem is the line "a2 -= a * ( a ° a2 )". If this code lock is executed a is mostly identical to (+/-)A.x . Therefore a ° A.y is roughly equal to (+/-)A.x ° A.y which is always 0. Hence the line has no influence at all. Now I sketched the axes calculated and somehow they don't match up. a1 and a2 have the sign reversed depending through which block of code you travel. Since it works for you I assume that the else-block has the correct signs and the if-block has the wrong ones. From my sketching on the paper though it looks to me as if your final x-axis is inverted. This staggers me a bit since after my knowledge it should not be inverted on frameA ( it is on frameB though ).

After all though I'm anyways not very happy with the dot-projection formula ( axis = something - x * ( x ° something ) ) and never use it in favor of a double cross product. After all this easier to read and faster ( 6 multiplications, 5 additions/subtractions and not SIMD capable for dot-projection versus 6 multiplications, 3 subtractions and SIMD capable for a double-cross ). Besides for the case that the axes are nearly identical there is no special calculation required not even a fabs. My pseudo code version looks like this ( untested ):

Code: Select all

p = a ° A.x
if p ~ 1:
   a1 = A.z
   a2 = A.y
elif p ~ -1:
   a1 = -A.z
   a2 = A.y
else:
   a2 = A.x x a
   a1 = a x a2
Maybe you can test this once out. I can't currently since my physics module is in-a-change state so I can't compile my test-scenario at the time being. For your question, in c++ calculating the absolute value of a floating point number uses the fabs() function. abs() is for integer values only.
LvR
Posts: 13
Joined: Mon Mar 17, 2008 8:09 am

Re: HingeConstraint constructor bug

Post by LvR »

Thx DragonLord.

I've first use a double cross product formula then reduce it to a dot product .... it's seems it's not a good optimisation

You are right to directly take A.* for new axes if p ~ +/-1

for the reverted X, I 'm completely confused, maybe it's a bug in my demo or in JBullet.
Can you explain in Bullet how are set up axis ?
In my demo, it's right handed, and Y is up, Z goes far and so X goes left ?

I'll test your code

I had also modified the line (reverted the cross product), is it correct for you ? :

Code: Select all

btVector3 rbAxisB2 =  axisInB.cross(rbAxisB1);
LvR
Posts: 13
Joined: Mon Mar 17, 2008 8:09 am

Re: HingeConstraint constructor bug

Post by LvR »

I must set up a c++ dev environnement to test, because in my java env I must inverse your cross-product and Z axes

Here is the code that works for me :

Code: Select all

p = a ° A.x
if p ~ 1:
   a1 = -A.z
   a2 = A.y
elif p ~ -1:
   a1 = A.z
   a2 = A.y
else:
   a2 = a x A.x
   a1 = a2 x a
User avatar
Dragonlord
Posts: 198
Joined: Mon Sep 04, 2006 5:31 pm
Location: Switzerland
Contact:

Re: HingeConstraint constructor bug

Post by Dragonlord »

To be honest I'm not 100% sure how the axes are used in Bullet since I use my own system and convert to Bullet using quaternions. But how I see it Bullet uses the same as OpenGL that is X goes right, Y goes up and Z out of the screen. Rotation of a hinge revolves around the Z-Axis from the X-Axis towards the Y-Axis ( CCW therefore ). But in the constructor of the hinge there is a flip on the axis at some place so one of the frames ( I think it's B but I have not dug deeper into all the constructors ) has the rotation axis inverted which also flips the X axis in succession.

EDIT: Just to make sure... is your Java cross-product left or right handed?
LvR
Posts: 13
Joined: Mon Mar 17, 2008 8:09 am

Re: HingeConstraint constructor bug

Post by LvR »

The cross product is ... a cross product, I think there is only one way to compute it.

But, assuming a is the wanted Z, a2 the wanted Y, and that Bullet is right handed (I've check that in demos from bullet 2.77 in visual studio express 2008), the right cross-product should be :

a2 = a x A.x ( Y = Z x X )

I'll write a little test in cpp and correct the HingeConstraint with this code and we'll see.
User avatar
Dragonlord
Posts: 198
Joined: Mon Sep 04, 2006 5:31 pm
Location: Switzerland
Contact:

Re: HingeConstraint constructor bug

Post by Dragonlord »

No, the cross-product is different depending on what coordinate system you use. If you use OpenGL ( right handed system ) use your right hand. The cross product formed between your thumb and the point finger turns out to be the middle finger. For a left handed system use the other hand. The direction of the final vector is exactly opposite. Or you can say it a bit different. X x Y = Z no matter what system you are in but in a right handed system Z points out of the screen while in a left handed system it points into the screen. So it's possible that java implements a different coordinate system which would explain the flipped X axis.
LvR
Posts: 13
Joined: Mon Mar 17, 2008 8:09 am

Re: HingeConstraint constructor bug

Post by LvR »

Yes, so in a right-handed system, i think that is correct ?

Code: Select all

a2 = a x A.x ( Y = Z x X )
the good question is : what system bullet use internally : left or right handed ?
User avatar
Dragonlord
Posts: 198
Joined: Mon Sep 04, 2006 5:31 pm
Location: Switzerland
Contact:

Re: HingeConstraint constructor bug

Post by Dragonlord »

Right handed. It's based on the OpenGL coordinate system as far as I found out.
LvR
Posts: 13
Joined: Mon Mar 17, 2008 8:09 am

Re: HingeConstraint constructor bug

Post by LvR »

OK, I've set up a visual studio environnement and have a repoduction case. Just put this in BasicDemo :

Code: Select all

void BasicDemo::initPhysics()
{
	setCameraDistance(btScalar(8.));

	m_collisionConfiguration = new btDefaultCollisionConfiguration();
	m_dispatcher = new	btCollisionDispatcher(m_collisionConfiguration);
	btVector3 worldAabbMin(-10000,-10000,-10000);
	btVector3 worldAabbMax(10000,10000,10000);
	m_overlappingPairCache = new btAxisSweep3(worldAabbMin,worldAabbMax,MAX_PROXIES);
	btSequentialImpulseConstraintSolver* sol = new btSequentialImpulseConstraintSolver;
	m_solver = sol;
	m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher,m_overlappingPairCache,m_solver,m_collisionConfiguration);

	m_dynamicsWorld->setGravity(btVector3(0,-10,0));

	///create a few basic rigid bodies
	btCollisionShape* shape1 = new btBoxShape(btVector3(0.5f,0.2f,0.4f));
	m_collisionShapes.push_back(shape1);
	btTransform tmpTrans;
	tmpTrans.setIdentity();
	tmpTrans.setOrigin(btVector3(0,2.0f,0));
	btRigidBody* body1 = localCreateRigidBody( btScalar(0.), tmpTrans, shape1);

	btCollisionShape* shape2 = new btBoxShape(btVector3(0.5f, 0.1f, 0.2f));
	m_collisionShapes.push_back(shape2);
	tmpTrans.setIdentity();
	tmpTrans.setOrigin(btVector3(1.0f, 2.0f, 0));
	btRigidBody* body2 = localCreateRigidBody( btScalar(1.), tmpTrans, shape2);

	btVector3 pivotInA=btVector3(0.5f, 0.0f, 0.0f);
        btVector3 pivotInB=btVector3(-0.5f, 0.0f, 0.0f);
                
	// Change this as you want to test different axis
        btVector3 axisInA=btVector3(0.0f, 0.0f, 1.0f);
        btVector3 axisInB=btVector3(0.0f, 0.0f, 1.0f);

	axisInA.normalize();
	axisInB.normalize();
	btHingeConstraint* hingeC = new btHingeConstraint(*body1, *body2, pivotInA, pivotInB, axisInA, axisInB);

    hingeC->setLimit( -SIMD_HALF_PI *0.5f , SIMD_HALF_PI *0.5f);
                
    m_dynamicsWorld->addConstraint(hingeC, true);
	clientResetScene();
}

And I now I have the good (and tested) HingeConstraint constructror :

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 (projection >= 1.0f - SIMD_EPSILON) {
		rbAxisA1 = -rbA.getCenterOfMassTransform().getBasis().getColumn(2);
		rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1);
	} else if (projection <= -1.0f + SIMD_EPSILON) {
		rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(2);
		rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1);		
	} else {
		rbAxisA2 = axisInA.cross(rbAxisA1);
		rbAxisA1 = rbAxisA2.cross(axisInA);
	}

	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;

}
And also correct inverse() method in btQuaternion.h used by quatRotate(), as I've mention in another post :

Code: Select all

	btQuaternion inverse() const
	{
		return btQuaternion(-m_x, -m_y, -m_z, m_unusedW);
	}


Hope you can add these corrections to Bullet.
User avatar
Erwin Coumans
Site Admin
Posts: 4221
Joined: Sun Jun 26, 2005 6:43 pm
Location: California, USA
Contact:

Re: HingeConstraint constructor bug

Post by Erwin Coumans »

The fix (and the other on quaternion inverse) have been applied to Bullet, ready for 2.68 release.
http://code.google.com/p/bullet/issues/detail?id=47

Thanks also to Eddybox and Dragonlord for the discussion.
Erwin
User avatar
Dragonlord
Posts: 198
Joined: Mon Sep 04, 2006 5:31 pm
Location: Switzerland
Contact:

Re: HingeConstraint constructor bug

Post by Dragonlord »

Finally got to the point with my editor where I can test constraint better ( and in realtime ). The hinge constraint seems to be very volatile. I don't know why but especially the linear constraints ( or locks in this case ) are very unstable. There's always jittering ( and not small, sometimes very huge ) if a more complex model ( a ragdoll for example ) is attached to the joint. It's not an integration error since using a ball-socket constraint ( again with 3 linear locks as the hinge ) has no jittering at ( works perfect ). I've to test this hinge constraint more. I'm pretty sure this overshooting of the door I mentioned earlier is linked to this linear limits problem.
Post Reply