Enhancement: Utility functions for setting position, rotation, linear and angular velocity of a system of rigid bodies

[Federico Ferri]

These functions allow to set the position and rotation of a system of rigid bodies.

I call this system a rigid body array.

Setting the position and/or rotation of a rigid body array should act as if we were setting position and/or rotation of the central body (the effect on the central body is the same as calling dBodySetPosition/dBodySetRotation), while keeping a constant transformation between bodies part of the system.

Note: I didn't care giving the possibility of specifying an arbitrary point as center of the rigid body array, but only another body; one can always put a dummy body at the arbitrary position and use it as the arbitrary center of the rigid body array.

void dRigidBodyArraySetPosition(dBodyID *bodyArray, size_t arraySize, dBodyID center, dReal x, dReal y, dReal z) {
    const dReal *p0 = dBodyGetPosition(center);
    dVector3 ps = {x, y, z};
    dVector3 ps_p0;
    dOP(ps_p0, -, ps, p0);
    for(size_t i = 0; i < arraySize; i++) {
        const dReal *pi = dBodyGetPosition(bodyArray[i]);
        dVector3 p1;
        dOP(p1, +, ps_p0, pi);
        dBodySetPosition(bodyArray[i], p1[0], p1[1], p1[2]);
    }
}

void dRigidBodyArraySetRotation(dBodyID *bodyArray, size_t arraySize, dBodyID center, const dReal *Rs) {
    const dReal *p0 = dBodyGetPosition(center);
    const dReal *R0 = dBodyGetRotation(center);
    dMatrix3 R0Rs;
    dMULTIPLY0_333(R0Rs, R0, Rs);
    dMatrix3 R0RsR0t;
    dMULTIPLY2_333(R0RsR0t, R0Rs, R0);
    for(size_t i = 0; i < arraySize; i++) {
        const dReal *pi = dBodyGetPosition(bodyArray[i]);
        const dReal *Ri = dBodyGetRotation(bodyArray[i]);
        dMatrix3 R1;
        dMULTIPLY0_333(R1, R0RsR0t, Ri);
        dVector3 p1, pi_p0, R0RsR0t__pi_p0;
        dOP(pi_p0, -, pi, p0);
        dMULTIPLY0_331(R0RsR0t__pi_p0, R0RsR0t, pi_p0);
        dOP(p1, +, R0RsR0t__pi_p0, p0);
        dBodySetPosition(bodyArray[i], p1[0], p1[1], p1[2]);
        dBodySetRotation(bodyArray[i], R1);
    }
}

[Federico Ferri]

Here's a little note explaining the math of the rotation, if anyone is interested in understanding or extending it:

(you can read text with nicely rendered formulae here)

We want to apply the transformation

$$\begin{pmatrix} R_s & 0 \\ 0 & 1 \end{pmatrix}$$

to our rigid body array, composed by bodies $0, \ldots, n$, where body $0$ is the central body.

We denote with $R_i$ and $p_i$ respectively the rotation and position of body $i$.

We proceed as follows; for each body $i$:
1) apply inverse transformation of body $0$, i.e.
$$\begin{pmatrix} R_0 & p_0 \\ 0 & 1 \end{pmatrix}^{-1} = \begin{pmatrix} R_0^\top & -R_0^\top p_0 \\ 0 & 1 \end{pmatrix}$$ which will place body $i$ as if the entire body array would be translated to origin and aligned to world's axes.
2) apply desired transformation, i.e. $\left( R_s, 0 \right)$
3) re-apply transformation $\left( R_0, p_0 \right)$

so we compose this sequence of transformations (by multiplying to the left, hence they read right to left):

$$\begin{pmatrix} R_i^\prime & p_i^\prime \\ 0 & 1 \end{pmatrix} =
\begin{pmatrix} R_0 & p_0 \\ 0 & 1 \end{pmatrix}
\begin{pmatrix} R_s & 0 \\ 0 & 1 \end{pmatrix}
\begin{pmatrix} R_0^\top & -R_0^\top p_0 \\ 0 & 1 \end{pmatrix}
\begin{pmatrix} R_i & p_i \\ 0 & 1 \end{pmatrix}$$

and we obtain:

$$R_i^\prime = R_0 R_s R_0^\top R_i$$

and

$$p_i^\prime = R_0 R_s R_0^\top \left( p_i - p_o \right) + p_0$$

[Federico Ferri]

Also, functions for setting linear and angular velocity of the rigid body array would be nice to have

[Federico Ferri]

And here are the functions for setting linear/angular velocity of the rigid body array:

void dRigidBodyArraySetLinearVel(dBodyID *bodyArray, size_t arraySize, dBodyID center, dReal lx, dReal ly, dReal lz) {
    for(size_t i = 0; i < arraySize; i++) {
        dBodySetLinearVel(bodyArray[i], lx, ly, lz);
    }
}

void dRigidBodyArraySetAngularVel(dBodyID *bodyArray, size_t arraySize, dBodyID center, dReal ax, dReal ay, dReal az) {
    const dReal *p0 = dBodyGetPosition(center);
    dVector3 omega = {ax, ay, az};
    for(size_t i = 0; i < arraySize; i++) {
        const dReal *p = dBodyGetPosition(bodyArray[i]);
        dVector3 pdot, r;
        dOP(r, -, p, p0);
        dCalcVectorCross3(pdot, omega, r);
        dBodySetLinearVel(bodyArray[i], pdot[0], pdot[1], pdot[2]);
        dBodySetAngularVel(bodyArray[i], ax, ay, az);
    }
}

void dRigidBodyArrayAddLinearVel(dBodyID *bodyArray, size_t arraySize, dBodyID center, dReal lx, dReal ly, dReal lz) {
    for(size_t i = 0; i < arraySize; i++) {
        const dReal *v = dBodyGetLinearVel(bodyArray[i]);
        dBodySetLinearVel(bodyArray[i], v[0] + lx, v[1] + ly, v[2] + lz);
    }
}

void dRigidBodyArraySetVel(dBodyID *bodyArray, size_t arraySize, dBodyID center, dReal lx, dReal ly, dReal lz, dReal ax, dReal ay, dReal az) {
    dRigidBodyArraySetAngularVel(bodyArray, arraySize, center, ax, ay, az);
    dRigidBodyArrayAddLinearVel(bodyArray, arraySize, center, lx, ly, lz);
}

[Federico Ferri]

Updated title to include also linear and angular velocity