FEM for Rigid Body Dynamics: Quaternion-based Rotation Handling and Inertia Tensors

Basic Equations of Rigid Body Dynamics#

In an FSI run or a DEM pile, a rigid body only has to keep distances fixed. No strain. The rotation model is what usually breaks that promise first.

Particle $i$ moves as translation of the center of mass plus a rotation.

$\mathbf{v}_i = \mathbf{v}_{cm} + \boldsymbol{\omega} \times \mathbf{r}_i$

$\mathbf{v}_{cm}$ is the center-of-mass velocity, $\boldsymbol{\omega}$ the angular velocity, and $\mathbf{r}_i$ the offset from the center of mass.

The axis slider and ω change which axis spins and how long one turn takes, even at the same angular-speed magnitude.

Raise ω and the body completes a turn sooner. Tilt the axis onto a body diagonal and check that no angle locks onto a single coordinate axis.

Linear Momentum and Angular Momentum#

Total linear momentum from Newton's second law is:

$\mathbf{P} = M \mathbf{v}_{cm}, \qquad \frac{d\mathbf{P}}{dt} = \mathbf{F}_{ext}$

Where $M = \sum_i m_i$ is the total mass. The acceleration of the center of mass is:

$M \dot{\mathbf{v}}_{cm} = \mathbf{F}_{ext}$

Total angular momentum is:

$\mathbf{L} = \mathbf{I} \boldsymbol{\omega}, \qquad \frac{d\mathbf{L}}{dt} = \boldsymbol{\tau}$

Where $\mathbf{I}$ is the inertia tensor and $\boldsymbol{\tau}$ is the total torque.

Inertia Tensor#

The inertia tensor relative to the center of mass is:

$I_{jk} = \sum_i m_i \left( |\mathbf{r}_i|^2 \delta_{jk} - r_{i,j} r_{i,k} \right)$

Extending to a continuum:

$\mathbf{I} = \int_V \rho \left( |\mathbf{r}|^2 \mathbf{E} - \mathbf{r} \otimes \mathbf{r} \right) dV$

Where $\mathbf{E}$ is the identity tensor, and $\mathbf{r} \otimes \mathbf{r}$ is the outer product.

When an object rotates, the inertia tensor is transformed by the orientation matrix $\mathbf{R}$.

$\mathbf{I}(t) = \mathbf{R}(t) \, \mathbf{I}_0 \, \mathbf{R}^T(t)$

Where $\mathbf{I}_0$ is the initial inertia tensor in the body frame, which only needs to be calculated once at the start of the simulation.

Rotation Representation Using Quaternions#

Representing rotation with Euler angles causes the gimbal lock problem. To avoid this, quaternions are used.

A quaternion $\mathbf{q} = (q_0, q_1, q_2, q_3)$ consists of a scalar part and a vector part.

$\mathbf{q} = q_0 + q_1 \mathbf{i} + q_2 \mathbf{j} + q_3 \mathbf{k}$

A rotation by an angle $\theta$ about a unit axis $\hat{\mathbf{n}}$ is:

$\mathbf{q} = \cos\frac{\theta}{2} + \sin\frac{\theta}{2}\left(n_x \mathbf{i} + n_y \mathbf{j} + n_z \mathbf{k}\right)$

The composite rotation of two quaternions $\mathbf{q}_1$ and $\mathbf{q}_2$ is expressed as a product.

$\mathbf{q}_{12} = \mathbf{q}_2 \mathbf{q}_1$

Using this, the orientation matrix $\mathbf{R}$ is explicitly expressed in terms of quaternion components.

The time derivative of a quaternion is linked to the angular velocity $\boldsymbol{\omega}$.

$\dot{\mathbf{q}} = \frac{1}{2} \begin{pmatrix} 0 \\ \boldsymbol{\omega} \end{pmatrix} \otimes \mathbf{q}$

The diagonal of $I_{jk}$ is $m(|\mathbf{r}|^2-r_j^2)$. The off-diagonal is $-m r_j r_k$. Put all the mass on the $x$ axis, so $r_y=r_z=0$, and $I_{xx}=0$ while $I_{yy}=I_{zz}=m x^2$. There is no lever arm about that axis. $\mathbf{I}_0$ is that sum, stored once in the body frame. In the world frame only $\mathbf{R}\mathbf{I}_0\mathbf{R}^T$ has to be rebuilt. Multiply a body-frame tensor by a world-frame angular velocity and the torque axis tilts off. The same congruence shows up when a stiffness tensor changes coordinates, which is what the constitutive-tensor transformation tracks inside an element.

At $\theta=0$, $\cos(\theta/2)=1$ and $\sin(\theta/2)=0$, so $\mathbf{q}=(1,0,0,0)$. That is the quaternion the code starts with. At $\theta=\pi$, $\cos(\pi/2)=0$ and $\sin(\pi/2)=1$, the scalar dies, and the vector part is just the axis $\hat{\mathbf{n}}$. At $\theta=2\pi$, $\cos\pi=-1$, so $\mathbf{q}=(-1,0,0,0)$. The rotation matches the identity. Only the sign flipped. A quaternion covers one full turn twice. The $(1,1)$ entry of $\mathbf{R}$ is $1-2(q_2^2+q_3^2)$, which is 1 for this identity value, and the off-diagonal entries are 0. The $1/2$ in front of $\dot{\mathbf{q}}$ is the same factor as $\theta/2$ in the angle formula. Integrate it raw and $|\mathbf{q}|$ walks away from 1. A linear tet has 4 nodes and 3 displacements each, 12 degrees of freedom. The rigid constraint cuts that to 3 translations plus 3 rotations, 6 in all. $\sum_i L_i=1$ only keeps the coordinates consistent. It does not create those 6 by itself.

FEM Discretization and Tetrahedral Shape Functions#

When discretizing the displacement field of a rigid body in FEM, the shape functions of a tetrahedral element are:

$N_i(\xi, \eta, \zeta) = L_i, \quad i = 1, 2, 3, 4$

Where $L_i$ are dimensionless barycentric coordinates and satisfy the condition $\sum_i L_i = 1$.

In linear tetrahedral elements, displacement interpolation is:

$\mathbf{u}(\mathbf{x}) = \sum_{i=1}^{4} N_i(\mathbf{x}) \, \mathbf{u}_i$

Applying rigid body constraints reduces the degrees of freedom to 6 (3 linear + 3 rotational).

Python Implementation Example#

import numpy as np
 
class RigidBody:
    def __init__(self, mass, I_body):
        self.M = mass
        self.I0 = I_body          # Body frame inertia tensor (3x3)
        self.x_cm = np.zeros(3)   # Center of mass position
        self.v_cm = np.zeros(3)   # Center of mass velocity
        self.q = np.array([1., 0., 0., 0.])  # Quaternion (w, x, y, z)
        self.omega = np.zeros(3)  # Angular velocity
 
    def quaternion_to_rotation(self):
        w, x, y, z = self.q
        return np.array([
            [1-2*(y**2+z**2),  2*(x*y - w*z),  2*(x*z + w*y)],
            [2*(x*y + w*z),  1-2*(x**2+z**2),  2*(y*z - w*x)],
            [2*(x*z - w*y),    2*(y*z + w*x),  1-2*(x**2+y**2)]
        ])
 
    def inertia_world(self):
        R = self.quaternion_to_rotation()
        return R @ self.I0 @ R.T
 
    def step(self, F_ext, tau_ext, dt):
        # Linear motion integration
        a_cm = F_ext / self.M
        self.v_cm += a_cm * dt
        self.x_cm += self.v_cm * dt
 
        # Rotational motion integration
        I_world = self.inertia_world()
        alpha = np.linalg.solve(I_world, tau_ext - np.cross(self.omega, I_world @ self.omega))
        self.omega += alpha * dt
 
        # Quaternion update
        omega_q = np.concatenate([[0.], self.omega])
        q_dot = 0.5 * quaternion_multiply(omega_q, self.q)
        self.q += q_dot * dt
        self.q /= np.linalg.norm(self.q)  # Normalize unit quaternion
 
def quaternion_multiply(q1, q2):
    w1, x1, y1, z1 = q1
    w2, x2, y2, z2 = q2
    return np.array([
        w1*w2 - x1*x2 - y1*y2 - z1*z2,
        w1*x2 + x1*w2 + y1*z2 - z1*y2,
        w1*y2 - x1*z2 + y1*w2 + z1*x2,
        w1*z2 + x1*y2 - y1*x2 + z1*w2
    ])

Skip the renormalization and R stops being a rotation#

Leave $|\mathbf{q}|$ unscaled and the columns of $\mathbf{R}$ lose unit length. The line q /= norm(q) is that scaling. Torque computed in the world frame, then multiplied by the stored $\mathbf{I}_0$, points $\boldsymbol{\alpha}$ along the wrong axis. The world tensor is $\mathbf{I}_{world} = \mathbf{R} \mathbf{I}_0 \mathbf{R}^T$ before $\mathbf{L}=\mathbf{I}\boldsymbol{\omega}$ is consistent. The explicit ceiling is $\Delta t < 2 / \omega_{max}$. A high $\omega_{max}$ means the step shrinks first.

A straight blend between two poses sinks inside the unit sphere. SLERP is the curve that hits $\mathbf{q}_0$ at $t=0$ and $\mathbf{q}_1$ at $t=1$.

$\mathbf{q}(t) = \mathbf{q}_0 \left(\mathbf{q}_0^{-1} \mathbf{q}_1\right)^t$

LERP does not stay on that sphere. Count rotational freedom together with element strain and the constraint eats the motion, the same way shear locking in a thin shell stiffens a plate that should bend. A rigid body is 6 degrees of freedom, not 12.