Constraining body orientations

[Tobs]

Hi

This might sound like madness but I'm trying to constrain the orientation of two bodies using torque impulses. In theory this could work but I'm having trouble calculating the correct torques.

My approach is to try and find the angular velocities that the bodies would need to correct their current orientation. I think I have it working for rigid constraints so I'll explain how I've done that first:

I find the difference in orientation between the two bodies:
DiffQuat = Quat1 * Inv(Quat2)

I then use this difference to find an angular velocity (I'm not convinced this part if correct):
AngVel = Vec(DiffQuat.x, DiffQuat.y, DiffQuat.z) * 2
AngVel += Body1->AngVel - Body2->AngVel

From this I'm then calculating angular velocities for each body using a ratio I've calculated based on their different inertias. I then apply the torques and things seem to work even if the results are a little loose.

My first question is then: Is what I've done reasonable or madness?

My second question is: How do I extend this approach so the constraint only works when the orientations exceed some limits I want to specify somehow?

p.s. I'm aware there are better ways of implementing constraints however understanding that stuff is a way off for me. Please be gentle

[Dirk Gregorius]

Let dq be your relative orientation. Note that you should consider that the inital orientation might not be an identity quaternion. Then the formula is:

q32 dq = Conj( q2 ) * q1 * Conj( q_initial )

Orientations are represented by uint quaternions. Therefore Conj( q ) == Inv( q )

1.) Extract the rotation axis:

v32 axis( dq.x, dq.y, dq.z );
Vec3Normalize( axis, axis );

2.) Extract angle

f32 angle = 2.0f * ArcCos( dq.w );

3.) This is a rotation so build angular velocity

f32 v_angle = angle / dt;

4.) Build angular velocity vector

v32 dw;
Vec3Scale( dw, axis, v_angle );

The next problem is to divide the delta velocity between the two bodies. You need to consider the masses or more precisely the moment of inertia.

w1 += MatInv( J1 ) / ( MatInv( J1 ) + MatInv( J2 ) ) * dw;
w2 += MatInv( J2 ) / ( MatInv( J1 ) + MatInv( J2 ) ) * dw;

HTH

-Dirk

[Tobs]

Hi Don

Your suggestion is pretty much what I'm doing apart from using an initial quaternion.

What exactly is the initial quaternion you mention?

Also do you have any insight into my second question about how I can extend this by subtracting the difference from some twist limits?

Thanks

[Dirk Gregorius]

The inital quaternion is the relative orientation of the two bodies when you setup your system.

Angle limits is a problem I am stuck at the moment as well. I have only a working solution for one limit. This doesn't work at the moment for two limits, e.g. a universal joint. So here is what I do:

Whenever I find that a limit is exceeded I set the relative angular velocity around that axis to zero.

Let s be the rotation axis in world space:

v32 dw;
Vec3Sub( dw, w1, w2 );
Vec3Project( dw, s );

// Bounciness
Vec3Scale( dw, dw, 1.0f + e );

w1 += MatInv( J1 ) / ( MatInv( J1 ) + MatInv( J2 ) ) * dw;
w2 += MatInv( J2 ) / ( MatInv( J1 ) + MatInv( J2 ) ) * dw;

Please let me know if this works for you. For a hinge the limit works.
You maybe want to look at the paper of Baltman "Verlet Integration and Constraints in a Six Degree of Freedom Rigid Body System". For computing the relative quaternions I suggest looking at the ODE source code - joints.cpp: hinge, hing2 and universal. Also Kenny Erleben's Dissertation will be useful to understand this.

What are you implementing - Jakobson, Baltman, Guendelman, ...? I extend the Guendelman approach, but currently I am stuck with limits as well. My system doesn't converge anymore or only incredibly slowly when adding two angle limits...

-Dirk

[Tobs]

I've implemented a system based on that same paper and I'm having similar problems.

I'm not looking into the slow convergence at the moment but you've reminded me that it's a problem.

It doesn't seem like there's an awful lot of help out there for implemented orientation constraints using this approach which is definitely frustrating!

[Dirk Gregorius]

I got the limits working. The first thing todo is use Lagrange multipliers on a body-body basis. Basically you take these steps:

1.) Setup the Jacobian for the limit
2.) Compute the limit error dv
3.) Transform the mass matrix into constraint space: VM = J * W * JT
4.) Invert the resulting square matrix
5.) Compute the Langrange multipliers: lambda = VM^-1 * dv
6.) Project result back into world coordinates: I = JT * lambda
7.) Apply impulses

In the Guendelman approach the constraint error is simple computed as follows:

float delta = alpha - limit;

Then you can transformthe delta into a velocity like this:

float dv = softness * delta / dt;

Don't set the angular velocity to zero like proposed in in the Baltman paper. For further information look at the Jiggle code.

You also like to look here for further information:

http://www.gamedev.net/community/forums ... _id=353071

For a working example you can look here:
http://www.dirkgregorius.de/primo03.rar

(Simply drag and drop the *.phx files onto the framework.exe)

-Dirk