Calculate Impulse/Torque for both bodies in a 3D fix joint constraint

Question

I have 2 rigid bodies (a & b) and 1 fix joint constraint (with relative transformation rela).

My objectives are to achieve :-

No. 1. b.transform = a.transform * rela
No. 2. Center of mass (a+b) doesn't change.
No. 3. (3rd Newton rule) Velocity of total system (a+b) doesn't change.
No. 4. (3rd Newton rule) Angular velocity of total system (a+b) doesn't change.
No. 5. Moving/rotating of both objects to solve it should be minimized.

I wish to apply impulse/torque to both bodies to make them gradually meets the requirement.
This video can depict what I want - (youtube link).

How to solve the value of impulse/torque that apply for each body?
I want a rough idea / algorithm.
It can be a description text without any code.

Example

Here is a sample problem and its correct solution (i.e. final resting state) :-

Code (draft)

Here is my current snippet, just in case :-

class Transform {
    Vec3 pos;
    Matrix33 basis;
};

Each rigid body has these fields :-

class RigidBody {
    float mass;
    Matrix33 inertiaTensor;
    Transform transform;
    Vec3 velocity;
    Vec3 angularVelocity;
};

The fix joint constraint is :-

class FixConstraint {
    Transform rela;
    RigidBody* a;
    RigidBody* b;
};

Draft of my poor solution

In nutshell, I have to modify 12 variables.

• position of a and b (xyz - 6 variables)
• orientation of a and b (angle xyz - 6 variables)

Then I can use "My objectives" No 1 & 2 to create some equations.
Then, at best, I have to solve 12 linear equation with 12 unknown variables.
I doubt if it has to be that hard.

My previous internet search

I have looked into various sources, but they mostly :-

• just drive into iteration solver.
• try to diagonalize matrix + jacobian : only expert can understand.
• "Why don't you look into (insert name of Physic Engine here) source code?" without beginner-friendly explanation.
• show how to use (name of Physic Engine) to create a fix joint constraint.

Here are some of the most useful ones :-

• Bullet Physics solver : https://github.com/svn2github/bullet/blob/master/tags/bullet-1.5f/BulletDynamics/ConstraintSolver/Point2PointConstraint.cpp
• "Stable, Robust, and Versatile Multibody Dynamics Animation" research paper http://image.diku.dk/kenny/download/erleben.05.thesis.pdf (see chapter 6)
• (edited-add) Featherstone's algorithm for ragdoll : https://en.wikipedia.org/wiki/Featherstone%27s_algorithm (but it focuses on many constraints rather than one)

(edited some wording and rule, thank fafl and Nico Schertler.)

---

(edited-add, after a few days)
I believe if I can crack "Point2PointConstraint.cpp" (of the Bullet Physics), I will be fully understand the algorithm and principle.

I also copy-paste it here, just in case.
Here is the first part :-

SimdVector3 normal(0,0,0);
for (int i=0;i<3;i++)
{
    normal[i] = 1;
    new (&m_jac[i]) JacobianEntry(
        m_rbA.getCenterOfMassTransform().getBasis().transpose(),
        m_rbB.getCenterOfMassTransform().getBasis().transpose(),
        m_rbA.getCenterOfMassTransform()*m_pivotInA - m_rbA.getCenterOfMassPosition(),
        m_rbB.getCenterOfMassTransform()*m_pivotInB - m_rbB.getCenterOfMassPosition(),
        normal,
        m_rbA.getInvInertiaDiagLocal(),
        m_rbA.getInvMass(),
        m_rbB.getInvInertiaDiagLocal(),
        m_rbB.getInvMass());
    normal[i] = 0;
}

Here is the second part :-

SimdVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_pivotInA;
SimdVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_pivotInB;
SimdVector3 normal(0,0,0);
for (int i=0;i<3;i++)
{       
    normal[i] = 1;
    SimdScalar jacDiagABInv = 1.f / m_jac[i].getDiagonal();

    SimdVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); 
    SimdVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
    //this jacobian entry could be re-used for all iterations

    SimdVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1);
    SimdVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2);
    SimdVector3 vel = vel1 - vel2;

    SimdScalar rel_vel;
    rel_vel = normal.dot(vel);

    //positional error (zeroth order error)
    SimdScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal

    SimdScalar impulse = depth*m_setting.m_tau/timeStep  * jacDiagABInv -  m_setting.m_damping * rel_vel * jacDiagABInv;

    SimdVector3 impulse_vector = normal * impulse;
    m_rbA.applyImpulse(impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition());
    m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition());

    normal[i] = 0;
}

Answer 1

As the name suggests, a constraint is a restriction imposed on the movement of two bodies. Hence to successfully model a constraint, one must fully understand what restrictions that constraint imposes on the two bodies and represent those constraints as an impulse or force equation. Impulse-based solvers are used over force-based solvers almost all the time since first-order (velocity) equations (and the numbers involved) are easier to compute and deal with than second-order (acceleration) equations. We will thus be looking at modelling impulses for general 1-D constraints (n-D constraints can be represented as one or more 1-D constraints).

Modelling the first-order (velocity) restriction of a 1-D constraint

The first step in modelling a constraint with impulses is representing the restriction(s) as first-order (velocity/momentum) equations. The fixed constraint in question (henceforth simply referred to as "the constraint") has a simple positional (zeroth order) restriction: the linear and angular positions in constraint space must stay the same (as specified by the relative transform rela). We can convert this into a first order restriction by levying the fact that in order to make the relative position of two bodies constant, their relative velocity must be zero. The constraint can thus be interpreted as ensuring that the relative linear and angular velocities of the two bodies in constraint space are zero.

The constraint can now be modeled as an impulse which must be applied to the system so that the relative velocity becomes zero. If v1 and v2 are the initial and final relative velocities of the system, m is the mass of the system and j is the impulse applied to satisfy the constraint, then

v2 = v1 + j/m    (1)
v2 = 0           (2)

The first equation holds true for any constraint (it is Newton's second law), but the second equation (henceforth referred to as the constraint equation) only holds true for the given constraint.

Modelling the zeroth-order (position) restriction of a 1-D constraint

Our model now ensures that the relative position stays constant (first-order restriction), but we still haven't enforced the zeroth-order restriction, i.e. the relative position of B with respect to A must be that which is specified by the relative transform rela. If x1 is the "error" in the constraint system position, then this can be "injected" into the impulse using a Baumgarte term beta * x1/h where h is the time step and beta is a parameter typically between 0 and 1. You can consider beta to be the fraction of the positional error you want the solver to resolve every frame.

Our constraint equation now becomes

v2 + beta*x1/h = 0

Note: If the solver uses semi-implicit integration, then x1 is the initial positional error, but if the solver uses implicit integration the error is actually x2 = x1 + v2*h.

Damping/softening the constraint

As you pointed out, the constraint you are looking for is a soft (or damped) constraint, since you want the movement to happen gradually. To damp the constraint exponentially, we simulate the exponential decay differential equation and add a value proportional to j (which is proportional to derivative of velocity dv = v2 - v1 as the time step gets infinitesimally small).

Our constraint equation now becomes

v2 + beta * x1/h + gamma * j = 0

Where gamma >= 0 is another parameter (also called the damping coefficient). If gamma = 0, the constraint will be undamped or rigid.

Eliminating v2 from the two equations and solving the resulting equation for j gives us

j = -(v1 + beta * x1/h) * (1/(gamma + 1/m))
                         | equivalent mass |

The solver applies this impulse and its negative in the direction of the axis (typically called the normal) of the constraint.

Adapting and using the general 1-D soft constraint

This general 1-D soft constraint impulse can be repurposed to create many different joints, like fixed joints, spring joints and so on. For a fixed joint, we want the constraint to constrict B's position and rotation in A's space to be the position and rotation of the relative transform. Thus, in A's space, x1 = B.position - rela.origin and v1 = relative velocity of A and B along x1. However, since we want to restrict the position in 3 dimensions (i.e. x1 is a 3d vector, not a scalar), we can solve the 1-D constraint on each of the three axes individually. This process can then be repeated for angular positions and velocities using the same constraint. The same process can be followed to derive the impulse for for any 1-D constraint.

Applying the constraint

Once computed, the impulse j must be applied to both the bodies in opposite directions so that the total impulse on the constraint system is zero. When positive, this impulse is meant to push the two bodies away, and when negative it means to pull them together. Hence, if the constraint normal points from body A to body B (as per convention), -j * normal is applied on body A and +j * normal is applied on body B.

The 1-D constraint can be extended to n-D if needed. Some constraints like the spring/distance constraint are along a line and hence are 1-D constraints by nature in n dimensional space and do not require multiple solution iterations for different axes.

You can also modify the constraint equation to take into account accumulated impulses while damping for stability if your solver uses warm starts. The damping term becomes gamma * (j + j0), where j0 is the accumulated impulse which is applied during the warm start.

Differences with Bullet 1.5 constraint solvers

It must be pointed out that the fixed constraint that you're looking for is actually Generic6DofConstraint (with all 6 degrees of freedom restricted) and not Point2PointConstraint (which is a ball and socket-like joint) in Bullet 1.5. You can look at its implementation for reference. The Bullet 1.5 constraint solvers use a slightly different (and slightly more hand-wavy) damped impulse equation (where they multiply the relative velocity with the damping factor but do not add it to the reciprocal of the equivalent mass) which damps the constraint a bit more. It's up to preference which one you want to choose, but I consider the one used here more justifiable and also much easier to adapt to other naturally soft constraints (for instance, parameterizing the constraint in terms of the spring frequency and damping ratio to simulate a damped spring).

You can also take a look at the hopefully self-explanatory code of the damped spring (soft distance constraint) solver in my 2D physics engine.