So I've been trying to implement a revolute joint using quaternion constraints as described by Claude Lacoursière in Game Physics Pearls, but without too much luck. It seems to only work when the bodies are not rotated and the joint frame is axis-aligned.

I've attached a pdf I made this morning of my derivation and what follows is some of the relevant code:

inline mat4_t L_matrix(const quat_t &q)
{
    mat4_t result;

    result.a =  q.w; result.e = -q.z; result.i =  q.y; result.m = q.x;
    result.b =  q.z; result.f =  q.w; result.j = -q.x; result.n = q.y;
    result.c = -q.y; result.g =  q.x; result.k =  q.w; result.o = q.z;
    result.d = -q.x; result.h = -q.y; result.l = -q.z; result.p = q.w;

    return result;
}

inline mat4x3_t GT_matrix(const quat_t &q)
{
    mat4x3_t result;

    result.a =  q.w; result.e = -q.z; result.i =  q.y;
    result.b =  q.z; result.f =  q.w; result.j = -q.x;
    result.c = -q.y; result.g =  q.x; result.k =  q.w;
    result.d = -q.x; result.h = -q.y; result.l = -q.z;

    return result;
}

inline mat4_t R_matrix(const quat_t &q)
{
    mat4_t result;

    result.a =  q.w; result.e =  q.z; result.i = -q.y; result.m = q.x;
    result.b = -q.z; result.f =  q.w; result.j =  q.x; result.n = q.y;
    result.c =  q.y; result.g = -q.x; result.k =  q.w; result.o = q.z;
    result.d = -q.x; result.h = -q.y; result.l = -q.z; result.p = q.w;

    return result;
}

inline mat4x3_t ET_matrix(const quat_t &q)
{
    mat4x3_t result;

    result.a =  q.w; result.e =  q.z; result.i = -q.y;
    result.b = -q.z; result.f =  q.w; result.j =  q.x;
    result.c =  q.y; result.g = -q.x; result.k =  q.w;
    result.d = -q.x; result.h = -q.y; result.l = -q.z;

    return result;
}

Initialisation:

    quat_t e = conjugate(body1->xform.q) * frame;
    quat_t f = conjugate(body2->xform.q) * frame;

    mat2x4_t P;

    P.a = 1.0f; P.c = 0.0f; P.e = 0.0f; P.g = 0.0f;
    P.b = 0.0f; P.d = 1.0f; P.f = 0.0f; P.h = 0.0f;

    joint->P = P * R_matrix(f) * transpose(L_matrix(e));

Pre-step:

    quat_t rq = conjugate(body1->xform.q) * body2->xform.q;

    mat2x3_t G1 = -0.5f * joint->P * ET_matrix(rq);
    mat2x3_t G2 =  0.5f * joint->P * GT_matrix(rq);

    joint->G1 = G1;
    joint->G2 = G2;

    mat2_t K = (G1 * body1->mass_ang_world_inv * transpose(G1))
             + (G2 * body2->mass_ang_world_inv * transpose(G2));

    joint->inv_K = inverse(K);

Solving the velocity constraint:

    const mat2x3_t &G1 = joint->G1;
    const mat2x3_t &G2 = joint->G2;

    vec2_t GV = (G1 * body1->vel_ang) + (G2 * body2->vel_ang);
    vec2_t lambda = joint->inv_K * -GV;

    body1->vel_ang += body1->mass_ang_world_inv * transpose(G1) * lambda;
    body2->vel_ang += body2->mass_ang_world_inv * transpose(G2) * lambda;
 

It would be greatly appreciated if someone who has experience with this (Dirk Gregorius?) to take a look at this and point out where I'm going wrong.

I implemented that a long time ago and ran into two issues iirc. One was that in the book there is some inconsistency about using global vs body frame angular velocity. This has an affect on the quaternion derivative. Let w be the world space angular velocity and w' the angular velocity in body space. Then

dq/dt = 0.5 * w * q = 0.5 * q * w'

This can be easily proven by simply plugging w = q * w' conjugate( q ) into the world space form. When you derive things I would have an eye on this.

The other issue with quaternion constraints operating on the relative orientation is that you need to be careful that your Jacobian entries are in world space (and not in the local space of your reference body). In world space they must be also equal and opposite. I would assert on this in your code. You might be simply missing to transform the Jacobian entries to world space.

Sorry, I am not an expert on this formulation and it has been a long time since I looked at this. We derived our own version without the E and L matrices to simplify things. Sadly it is not published yet and it is too long to put here into a post.

Thank you very much for responding. I just did a derivation using dq/dt = 0.5 * w * q instead of the other way around and it seems to work now. The book actually mentions that q and w are reversed when using body-space angular velocity or when using left-handed multiplication, but it gets it wrong. So it is actually making the very mistake it is warning against.

Awesome! I am glad that it is working now! I should have catched this while editing the book. It actually does not make sense to derive this using body space angular velocity since most solvers used in games are using world space angular velocities in the accumulators.