Making a game physics engine – rigid body kynematics and dynamics

[latexpage]

This article is a brief summary on how to write differential equations of motion for a rigid body. From these, we can use a finite approximation to calculate a rigid body’s pose (position and orientation) and motion (for both linear and angular velocity). It is very math heavy, and it is not intended as an explanation of each equation (for that I suggest a very good book on mechanics like Goldstein), but as a brief recap of all the equations needed to derive a physics engine for a game.

Transport theorem

The transport theorem allows to calculate the derivative of a time-dependent vector quantity in a fixed space, knowing the components in a rotating frame, and vice-versa.

Let’s consider two frames: one is a fixed frame in the world and the other is attached to a rigid body and moves with it. Let’s call the fixed frame $RC$ and the moving frame $R\Gamma$. The orientation of $R\Gamma$ with respect to $RC$ can be expressed with a rotation matrix $R$.  A rotation matrix can be seen as an operator that rotates points of a space, or as a change of coordinate matrix from the rotated space to the fixed space: in both cases the colums of the matrix are the components of the vectors of the base of the rotated space expressed in the fixed space.

A time-dependent vector quantity has components in both $RC$ and $R\Gamma$ and they are related by the rotation matrix with the following equation:

\[

(\vec{v})_{RC} = (R)_{R\Gamma->RC}  (\vec{v})_{R\Gamma}

\]

We can then calculate the time-derivative of the vector quantity $\vec{v}$:

\[

\frac{d}{dt}(\vec{v})_{RC} =  \frac{d}{dt}\bigg((R)_{R\Gamma->RC}(\vec{v})_{R\Gamma}\bigg) =  (R)_{R\Gamma->RC}\frac{d}{dt}(\vec{v})_{R\Gamma}  +   \frac{d}{dt}(R)_{R\Gamma->RC} (\vec{v})_{R\Gamma}

\]

with

\[

(\vec{v})_{R\Gamma} = (R^{-1})_{RC->R\Gamma}(\vec{v})_{RC}

\]

\[

\frac{d}{dt}(\vec{v})_{RC} = (R)_{R\Gamma->RC}\frac{d}{dt}(\vec{v})_{R\Gamma} + \underbrace {\frac{d}{dt}(R)_{R\Gamma->RC} (R^{-1})_{RC->R\Gamma}}_{(\Omega)_{RC}}(\vec{v})_{RC}

\]

\[

\frac{d}{dt}(\vec{v})_{RC} = (R)_{R\Gamma->RC}\frac{d}{dt}(\vec{v})_{R\Gamma}  + (\Omega)_{RC}(\vec{v})_{RC}

\]

$ \frac{d}{dt}(R)_{R\Gamma->RC} (R^{-1})_{RC->R\Gamma} $ is a skew-symmetric matrix which represents the angular velocity operator. Applying this operator to a vector is the same as taking the cross product with it:

\[

(\Omega)_{RC}(\vec{v})_{RC} = (\vec{\omega})_{RC} \times (\vec{v})_{RC}

\]

substituting this into () we get the transport theorem:

\[ \underbrace {\frac{d}{dt}(\vec{v})_{RC}}_{absolute\ derivative} = \underbrace {(R)_{R\Gamma->RC}\frac{d}{dt}(\vec{v})_{R\Gamma}}_{relative\  derivative} + \underbrace { (\vec{\omega})_{RC} \times (\vec{v})_{RC}}_{transport} \]

If we indicate derivation with respect to time with a dot and denote an absolute derivative (performed in components of the inertial frame) with an “a” subscript and the relative derivative (performed in components of the body’s rotating frame) with a “r” subscript:

\[

\dot{\vec{v_a}}  \stackrel{\text{\tiny def}}{=} \frac{d}{dt}(\vec{v})_{RC}

\]

\[

\dot{\vec{v_r}}  \stackrel{\text{\tiny def}}{=} \frac{d}{dt}(\vec{v})_{R\Gamma}

\]

 

we can write the derivative of a time dependent vector with the following vectorial notation:

\[

\dot{\vec{v_a}} = \dot{\vec{v_r}} + \vec{\omega} \times \vec{v}

\]

It is important to note the difference between the space in which the derivation is performed and the space in whose components the derivative is represented. In fact, we can represent the above equation in $R\Gamma$ by left-multiplying by the rotation matrix:

\[ (R^-1)_{RC->R\Gamma}\frac{d}{dt}(\vec{v})_{RC} = (R^-1)_{RC->R\Gamma}(R)_{R\Gamma->RC}\frac{d}{dt}(\vec{v})_{R\Gamma} + (R^-1)_{RC->R\Gamma} \bigg((\vec{\omega})_{RC} \times (\vec{v})_{RC}\bigg) \]

with

\[

(\vec{v})_{RC} = (R)_{R\Gamma->RC}(\vec{v})_{R\Gamma}

\]

and

\[

(\vec{\omega})_{RC} \times (\vec{v})_{RC} = (\Omega)_{RC}(\vec{v})_{RC}

\]

substituting we obtain:

\[ (R^-1)_{RC->R\Gamma}\frac{d}{dt}(\vec{v})_{RC} = \underbrace{(R^-1)_{RC->R\Gamma}(R)_{R\Gamma->RC}}_{Id}\frac{d}{dt}(\vec{v})_{R\Gamma} + \underbrace{(R^-1)_{RC->R\Gamma}(\Omega)_{RC}(R)_{R\Gamma->RC}}_{(\Omega)_{R\Gamma}}(\vec{v})_{R\Gamma} \]

\[ (R^-1)_{RC->R\Gamma}\frac{d}{dt}(\vec{v})_{RC} = \frac{d}{dt}(\vec{v})_{R\Gamma} + (\Omega)_{R\Gamma}(\vec{v})_{R\Gamma} \]

which is the vectorial equation nn written in $R\Gamma$.

The transport theorem will be useful when we need to calculate the derivative of the angular momentum  of a rotating body.

 

Rigid body kynematics

Let’s consider a point of a rigid body $P$, a reference frame $R\Gamma$ attached to the body, with origin $O’$, and an (inertial) world-fixed frame $RC$ with origin $O$: the point’s coordinates in the two frames are related by:

\[

(\overrightarrow{OP})_{RC} = (\overrightarrow{OO’})_{RC}  + (R)_{R\Gamma->RC}(\overrightarrow{O’P})_{R\Gamma}

\]

where $R$ is the rotation/change of coordinates matrix. If we calculate the absolute velocity of the point, since $(\overrightarrow{OP})_{R\Gamma}$ components are constant in $R\Gamma$ we obtain:

\[

\frac{d}{dt}(\overrightarrow{OP})_{RC} = \frac{d}{dt}(\overrightarrow{OO’})_{RC} + \frac{d}{dt}(R)_{R\Gamma->RC}(\overrightarrow{O’P})_{R\Gamma}

\]

with

\[(\overrightarrow{O’P})_{R\Gamma} = (R^{-1})_{RC->R\Gamma}(\overrightarrow{O’P})_{RC}\]

substituting:

\[ \frac{d}{dt}(\overrightarrow{OP})_{RC} = \frac{d}{dt}(\overrightarrow{OO’})_{RC}  + \underbrace {\frac{d}{dt}(R)_{R\Gamma->RC}(R^{-1})_{RC->R\Gamma} }_{(\Omega)_{RC}} (\overrightarrow{O’P})_{RC}  \]

as we’ve seen in the preceding section, $ \frac{d}{dt}(R)_{R\Gamma->RC} (R^{-1})_{RC->R\Gamma} $ is a skew-symmetric angular velocity operator (whose components anre written in $RC$) that we indicate as $\Omega$:

\[ \frac{d}{dt}(\overrightarrow{OP})_{RC} = \frac{d}{dt}(\overrightarrow{OO’})_{RC}+ (\Omega)_{RC} (\overrightarrow{O’P})_{RC} \]

Its RC components are

\[

(\Omega)_{RC} = \begin{bmatrix}

0 & -w_z & w_y \\

w_z & 0 & -w_x \\

-w_y & w_x & 0

\end{bmatrix}

\]

A seen, s skew-symmetric operator can be written as a cross product in $R^3$:

\[ (\Omega)_{RC} (\overrightarrow{O’P})_{RC} = (\omega)_{RC} \times (\overrightarrow{O’P})_{RC} \]

and substituting we get

\[ \underbrace {\frac{d}{dt}(\overrightarrow{OP})_{RC}}_{\vec{v_P}} = \underbrace {\frac{d}{dt}(\overrightarrow{OO’})_{RC}}_{\vec{v_{O’}}} + (\omega)_{RC} \times (\overrightarrow{O’P})_{RC} \]

So if we indicate the absolute (relative to the world/$RC$) velocity of $P$  and of the origin $O’$ respectively as $\vec{v}_P$ and $\vec{v}_{O’}$, we get the fundamental equation of the kynematics of rigid bodies:

\[

(\vec{v_{P}})_{RC} = (\vec{v_{O’}})_{RC} + (\omega)_{RC} \times (\overrightarrow{O’P})_{RC}

\]

 

Particle systems dynamics

Newton’s second law of dynamics states that a force acting on a material particle causes an acceleration that is inverseli proportional to the particle’s mass. This is true for all particles in the system:

\[  m_i\vec{a}_i  = \vec{f}_i    \      (i = 1,…,N)\]

the resulting force acting on a particle is the sum of all internal and external forces: internal forces are exchanged with all other particles of the system, external forces are external to the system:

\[ \vec{f}_i = \vec{f}_i^{ext} + \vec{f}_i^{int} \]

\[ m_i\vec{a}_i = \vec{f}_i^{ext} + \vec{f}_i^{int} \ ( i = 1,…,N) \]

if we sum for all the particles in the system:

\[  \sum_{i = 1}^N m_i\vec{a}_i = \sum_{i = 1}^N (\vec{f}_i^{ext} + \vec{f}_i^{int}) \]

Newton’s third law of dynamics affirms that two particles exchange two equal and opposite forces , so each interaction cancels itself out:

\[ \sum_{i = 1}^N (\vec{f}_i^{ext} + \vec{f}_i^{int}) = \vec{F}^{ext}\]

so we get

\[  \sum_{i = 1}^N m_i\vec{a}_i = \vec{F}^{ext} \]

 

Let’s now take the moment of each term:

\[  \overrightarrow{AP}_i \times m_i\vec{a}_i  = \overrightarrow{AP}_i \times \vec{f}_i    \      (i = 1,…,N)\]

\[ \vec{f}_i = \vec{f}_i^{ext} + \vec{f}_i^{int} \]

\[  \overrightarrow{AP}_i \times m_i\vec{a}_i  = \overrightarrow{AP}_i \times \vec{f}_i ^{ext} + \overrightarrow{AP}_i \times \vec{f}_i ^{int} \      (i = 1,…,N)\]

\[ \sum_{i = 1}^N \overrightarrow{AP}_i \times m_i\vec{a}_i  = \sum_{i = 1}^N \overrightarrow{AP}_i \times \vec{f}_i ^{ext} + \sum_{i = 1}^N \overrightarrow{AP}_i \times \vec{f}_i ^{int} \]

each couple of forces exchanged by two particles is a null torque:

\[ \sum_{i = 1}^N \overrightarrow{AP}_i \times \vec{f}_i ^{ext} + \sum_{i = 1}^N \overrightarrow{AP}_i \times \vec{f}_i  = M_A^{ext} \]

so we obtain

\[ \sum_{i = 1}^N \overrightarrow{AP}_i \times m_i\vec{a}_i  = M_A^{ext} \]

 

Conservation of linear momentum

Linear momentum of a particle is defined as the mass times its velocity:

\[ \vec{q}_i = m_i\vec{v}_i \]

summing on all particles in a system we get the total linear momentum:

\[ \sum_{i = 1}^N  \vec{q}_i = \sum_{i = 1}^N m_i\vec{v}_i = M\vec{v}_G = \vec{Q} \]

the derivative of total linear momentum

\[ \frac{d}{dt}\vec{Q} = \sum_{i = 1}^N \frac{d}{dt} m_i\vec{v}_i = \sum_{i = 1}^N m_i \frac{d}{dt}\vec{v}_i = \sum_{i = 1}^N m_i \vec{a}_i \]

with

\[ \sum_{i = 1}^N m_i \vec{a}_i = M\vec{a}_G \]

so we get the conservation law for total linear momentum of a system of particles

\[ \frac{d}{dt}\vec{Q} = M\vec{a}_G = \vec{F}^{ext} \]

This equation is valid for rigis bodies as well.

Conservation of angular momentum

The angular momentum of a particle with respect to a point $A$ is defined as

\[ \vec{k_i}_A = \overrightarrow{AP}_i \times m_i\vec{v}_i \]

summing for all particles in a system we get the total angular momentum with respect to &A&:

\[ \sum_{i = 1}^N \vec{k_i}_A = \sum_{i = 1}^N \overrightarrow{AP}_i \times m_i\vec{v}_i = \vec{K_A}\]

the derivative of total angular momentum:

\[

\frac{d}{dt}\vec{K_A} = \sum_{i = 1}^N \frac{d}{dt} (\overrightarrow{AP_i} \times m_i\vec{v}_i) = \sum_{i = 1}^N \bigg(\frac{d}{dt} \overrightarrow{AP_i}\times m_i\vec{v}_i  +  \overrightarrow{AP_i} \times m_i\frac{d}{dt} \vec{v}_i \bigg) =  \sum_{i = 1}^N \bigg(\frac{d}{dt}(\overrightarrow{OP_i} – \overrightarrow{OA}) \times m_i\vec{v}_i + \overrightarrow{AP_i} \times m_i\vec{a_i}\bigg)

\]

with

\[ \sum_{i = 1}^N  \frac{d}{dt}\overrightarrow{OP_i}  \times m_i\vec{v}_i  = \sum_{i = 1}^N \vec{v_i} \times m_i\vec{v_i} = 0 \]

and

\[ -\sum_{i = 1}^N  \frac{d}{dt}\overrightarrow{OA} \times m_i\vec{v}_i  =  -\vec{v_A} \times \sum_{i = 1}^N \ m_i\vec{v_i} = -\vec{v_A} \times M\vec{v_G} \]

substituting:

\[ \frac{d}{dt}\vec{K_A} = -\vec{v_A} \times M\vec{v_G}  + \sum_{i = 1}^N \overrightarrow{AP_i} \times m_i\vec{a_i} \]

since we know from equation above that

\[ \sum_{i = 1}^N \overrightarrow{AP_i} \times m_i\vec{a_i} = M_A^{ext} \]

we obtain the conservation law for the total angular momentum of a system of particles with respect to a point $A$:

\[ \frac{d}{dt}\vec{K_A} + \vec{v_A} \times M\vec{v_G} = M_A^{ext} \]

if A == G the second term on the left side of the equation is null:

\[ \frac{d}{dt}\vec{K_A} = M_A^{ext} \]

Angular momentum for a rigid body

For a rigid body we can express the field of velocity as

\[ \vec{v}_i = \vec{v}_A + \vec{\omega} \times \overrightarrow{AP}_i  \]

if we substitute into the definition of total angular momentum:

\[ \vec{K}_A = \sum_{i = 1}^N \overrightarrow{AP}_i \times m_i\vec{v}_i = \sum_{i = 1}^N m_i\overrightarrow{AP}_i \times (\vec{v}_A + \vec{\omega} \times \overrightarrow{AP}_i) = \sum_{i = 1}^N (m_i\overrightarrow{AP}_i \times \vec{v}_A + m_i\overrightarrow{AP}_i \times \vec{\omega} \times \overrightarrow{AP}_i)\]

 

\[  \sum_{i = 1}^N m_i\overrightarrow{AP}_i \times \vec{v}_A =  M\overrightarrow{AG} \times \vec{v}_A \]

the triple cross product can be expressed as

\[  \sum_{i = 1}^N (m_i\overrightarrow{AP}_i \times  \vec{\omega} \times \overrightarrow{AP}_i) = \sum_{i = 1}^N  (m_i(\overrightarrow{AP}_i \cdot \overrightarrow{AP}_i)\vec{\omega} – m_i(\overrightarrow{AP}_i \cdot\vec{\omega})\overrightarrow{AP}_i)  =  \sum_{i = 1}^N  (m_i\|AP_i\|^2\vec{\omega} – m_i(\overrightarrow{AP}_i \cdot\vec{\omega})\overrightarrow{AP}_i)\]

Now, if we project this vectorial equation on the axis of $RC$ we get

\[ x: \sum_{i = 1}^N (m_i((x _i – x_A)^2 + (y _i – y_A)^2 + (z _i – z_A)^2)\omega_x – m_i((x_i – x_A)\omega_x + (y_i – y_A)\omega_y + (z_i – z_A)\omega_z)(x_i – x_A) \]

\[ y: \sum_{i = 1}^N (m_i((x _i – x_A)^2 + (y _i – y_A)^2 + (z _i – z_A)^2)\omega_y – m_i((x_i – x_A)\omega_x + (y_i – y_A)\omega_y + (z_i – z_A)\omega_z)(y_i – y_A) \]

\[ z: \sum_{i = 1}^N (m_i((x _i – x_A)^2 + (y _i – y_A)^2 + (z _i – z_A)^2)\omega_z – m_i((x_i – x_A)\omega_x + (y_i – y_A)\omega_y + (z_i – z_A)\omega_z)(z_i – z_A) \]

 

\[ x: \sum_{i = 1}^N (m_i((x _i – x_A)^2 + (y _i – y_A)^2 + (z _i – z_A)^2)\omega_x – m_i((x_i – x_A)^2\omega_x + (x_i – x_A)(y_i – y_A)\omega_y + (x_i – x_A)(z_i – z_A)\omega_z) \]

\[ y: \sum_{i = 1}^N (m_i((x _i – x_A)^2 + (y _i – y_A)^2 + (z _i – z_A)^2)\omega_y – m_i((y_i – y_A)(x_i – x_A)\omega_x + (y_i – y_A)^2\omega_y + (y_i – y_A)(z_i – z_A)\omega_z) \]

\[ z: \sum_{i = 1}^N (m_i((x _i – x_A)^2 + (y _i – y_A)^2 + (z _i – z_A)^2)\omega_z – m_i((z_i – z_A)(x_i – x_A)\omega_x + (z_i – z_A)(y_i – y_A)\omega_y + (z_i – z_A)^2\omega_z) \]

 

\[ x: \sum_{i = 1}^N \underbrace{m_i((y _i – y_A)^2 + (z _i – z_A)^2)}_{I_x}\omega_x – \underbrace{m_i(x_i – x_A)(y_i – y_A)}_{I_{xy}}\omega_y  -\underbrace{m_i(x_i – x_A)(z_i – z_A)}_{I_{xz}}\omega_z) \]

\[ y: \sum_{i = 1}^N -\underbrace{m_i(y_i – y_A)(x_i – x_A)}_{I_{yx}}\omega_x + \underbrace{m_i((x _i – x_A)^2 + (z _i – z_A)^2)}_{I_y}\omega_y – \underbrace{m_i(y_i – y_A)(z_i – z_A)}_{I_{yz}}\omega_z \]

\[ z: \sum_{i = 1}^N – \underbrace{m_i(z_i – z_A)(x_i – x_A)}_{I_{zx}}\omega_x – \underbrace{m_i(z_i – z_A)(y_i – y_A)}_{I_{zy}}\omega_y  + \underbrace{m_i((x _i – x_A)^2 + (y _i – y_A)^2 )}_{I_z}\omega_z   \]

 

we call $I_x$, $I_y$ and $I_z$ moments of inertia and $I_{xy} = I_{yx}$, $I_{xz} = I_{zx}$ and $I_{yz} = I_{zy}$ products of inertia:

\[

I^A =

\begin{bmatrix}

I^A_x && I^A_{xy} && I^A_{xz} \\

I^A_{yx} && I^A_y && I^A_{yz} \\

I^A_{zx} && I^A_{zy} && I^A_z

\end{bmatrix}

\]

$I^A$ is the inertia tensor or mass matrix of the body with respect to the point A. We can use the inertia tensor to express the total angular momentum of the body:

\[ \vec{K}_A = M\overrightarrow{AG} \times \vec{v}_A + I^A\vec{\omega} \]

what we are interested in is the absolute derivative of the total angular momentum. Remembering the transport theorem above:

\[ \frac{d}{dt} \vec{K}_A = \frac{d}{dt}(M\overrightarrow{AG} \times \vec{v}_A + I^A\vec{\omega}) \]

with

\[\frac{d}{dt}M\overrightarrow{AG} \times \vec{v}_A = -\vec{v}_A \times \vec{Q} \]

we can write the second term on the right end of the equation in $RC$ components:

\[

\begin{split}

\frac{d}{dt}\bigg((I^A)_{RC}(\vec{\omega})_{RC})\bigg) =  \frac{d}{dt}\bigg((R)_{R\Gamma->RC}\bigg((I^A)_{R\Gamma}(\vec{\omega})_{R\Gamma})\bigg)\bigg) = \\

= \underbrace{\frac{d}{dt}(R)_{R\Gamma->RC}(R^{-1})_{RC->R\Gamma}}_{(\Omega)_{RC}}\bigg((I^A)_{RC}(\vec{\omega})_{RC}\bigg) + (R)_{R\Gamma->RC} \frac{d}{dt}\bigg((I^A)_{R\Gamma}(\vec{\omega})_{R\Gamma}\bigg)

\end{split}

\]

 

I is constant in $R\Gamma$:

 

\[

\begin{split}

\frac{d}{dt}\bigg((I^A)_{RC}(\vec{\omega})_{RC})\bigg) = \underbrace{(\Omega)_{RC} \bigg((I^A)_{RC}(\vec{\omega})_{RC}\bigg)}_{(\vec{\omega})_{RC} \times \bigg((I^A)_{RC}(\vec{\omega})_{RC}\bigg) }+ (R)_{R\Gamma->RC} \bigg((I^A)_{R\Gamma}\frac{d}{dt} (\vec{\omega})_{R\Gamma}\bigg) = \\

= (\vec{\omega})_{RC} \times \bigg((I^A)_{RC}(\vec{\omega})_{RC}\bigg) +  (R)_{R\Gamma->RC} \bigg((I^A)_{R\Gamma}\frac{d}{dt} (\vec{\omega})_{R\Gamma}\bigg)

\end{split}

\]

substituting into the original equation we get

\[

\frac{d}{dt}(\vec{K}_A)_{RC} = (-\vec{v}_A)_{RC} \times (\vec{Q})_{RC} + (\vec{\omega})_{RC} \times \bigg((I^A)_{RC}(\vec{\omega})_{RC}\bigg) +  (R)_{R\Gamma->RC} \bigg((I^A)_{R\Gamma}\frac{d}{dt} (\vec{\omega})_{R\Gamma}\bigg) = (\vec{M}_A^{ext})_{RC}

\]

which is the conservation law of total angular momentum for a rigid body

if A = = G the equation simplifies:

\[

\frac{d}{dt}(\vec{K}_G)_{RC} = (\vec{\omega})_{RC} \times \bigg((I^G)_{RC}(\vec{\omega})_{RC}\bigg) +  (R)_{R\Gamma->RC} \bigg((I^G)_{R\Gamma}\frac{d}{dt} (\vec{\omega})_{R\Gamma}\bigg)  = (\vec{M}_G^{ext})_{RC}

\]

 

\[ \dot{\vec{K}}_G_a = I\dot{\vec{\omega}}_r + \vec{\omega} \times I\vec{\omega} \]

 

Equations of motion for a rigid body

We now have the equations describing the linear and angular motion of a rigid body in space:

\[ \dot{\vec{Q}}_a = M\vec{a}_G = \vec{F}^{ext} \]

\[ \dot{\vec{K}}_G_a = I\dot{\vec{\omega}}_r + \vec{\omega} \times I\vec{\omega} = \vec{M}_G^{ext} \]