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

Basic Equations of Rigid Body Dynamics#

Accurately describing the motion of rigid bodies is essential in Fluid-Structure Interaction (FSI) analysis or Discrete Element Method (DEM) simulations. A rigid body is an object without deformation, where the distance between any two points always remains constant.

The velocity of an arbitrary particle $i$ within a rigid body is decomposed as follows:

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

Where $\mathbf{v}_{cm}$ is the velocity of the center of mass, $\boldsymbol{\omega}$ is the angular velocity, and $\mathbf{r}_i$ is the displacement vector from the center of mass to particle $i$.

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}$

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
    ])

Practical Considerations#

1. Forgetting Quaternion Normalization As numerical integration repeats, $|\mathbf{q}| \neq 1$ can occur, causing the rotation matrix to become non-orthogonal. q /= norm(q) must be performed at every time step.

2. Inertia Tensor Coordinate System Confusion $\mathbf{I}_0$ is defined in the body-fixed coordinate system (body frame). If torque was calculated in the world frame, it must be applied after conversion to $\mathbf{I}_{world} = \mathbf{R} \mathbf{I}_0 \mathbf{R}^T$.

3. Time Step Selection Numerical stability is guaranteed only when the condition $\Delta t < 2 / \omega_{max}$ (similar to the CFL condition) is satisfied for the maximum natural frequency $\omega_{max}$ of the rigid body. Higher speed rotating objects require smaller $\Delta t$.

4. Quaternion Interpolation (SLERP) When interpolating between two orientations, Spherical Linear Interpolation (SLERP) should be used instead of Linear Interpolation (LERP) to ensure constant velocity rotation.

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

Combining rigid body dynamics and FEM is the basis for FSI, DEM, and multi-body dynamics simulations. Proper implementation of quaternion-based rotation representation allows stable simulation of arbitrary 3D rotations without gimbal lock.

Tweak the axis and ω to verify quaternion rotation has no gimbal lock.