Rigid Body Simulation · Projects

A rigid-body helicopter simulated in C++ from its rotational dynamics. Rotor blades generate lift and thrust; the body's state — position, orientation, and their velocities — advances by numerically integrating the Newton-Euler equations of motion with the Euler method.

Unlike a point mass, a rigid body has orientation and spins, so its state carries a position $x$ and linear velocity $v$ for the center of mass, plus an orientation (a rotation $R$ ) and an angular velocity $ω$ . Linear motion follows Newton's second law; rotation follows Euler's equation, coupling angular acceleration to torque through the inertia tensor $I$ :

m \dot{v} = k \sum f_{k}, I \dot{ω} = τ - ω \times (I ω) .

The $ω \times (I ω)$ term is the gyroscopic coupling; without it a tumbling body would not precess.

A rotor spinning at angular speed $Ω_{k}$ generates a thrust along its axis roughly proportional to the square of its speed, $f_{k} \approx κ Ω_{k}^{2} \hat{n}_{k}$ . Summed, the thrusts lift the craft; differences between them produce a net torque $τ = \sum_{k} r_{k} \times f_{k}$ about the center of mass, which is what tilts and yaws it. Both feed the equations above.

Seen from the side, each rotor makes an upward thrust (unequal here, which rolls the body) while gravity pulls down at the center of mass. Differential thrust is what tilts and turns the craft.

Steering comes from differential thrust. With the rotors laid out around the body, the three attitude moments come from spinning them unequally. Speeding up the rotors on one side and slowing the other tilts the thrust asymmetry into a roll about the forward axis; doing the same front-to-back produces pitch. Yaw is subtler: each rotor also drags against the air with a reaction torque opposite its spin, so running the clockwise rotors faster than the counter-clockwise ones leaves a net twist about the vertical axis without changing total lift. A controller therefore never commands torque directly — it solves for the four rotor speeds whose combined thrust and reaction torque hit a desired total lift and $(roll, pitch, yaw)$ , the inverse of the map above. The simulation stub closes that loop each frame: read the current state, compare it to the target attitude, set rotor speeds, integrate, repeat.

Algorithm:

\textsc{Simulate}()

— the per-frame loop that drives the update

1
state: position $\mathbf x$ , velocity $\mathbf v$ , rotation $R$ , angular velocity $\boldsymbol\omega$
2
repeat each frame
3
read the target attitude from input
4
set each rotor speed from the controller (differential thrust)
5
$\mathbf f_k \gets$ thrust of rotor $k$ at body offset $\mathbf r_k$
6
$\textsc{Step}(h)$
7
draw the craft from $\mathbf x, R$
8
until the window closes

A planar multicopter under a cascaded PD controller. Click anywhere in the frame to set a waypoint: the controller cannot push sideways directly, so it banks — the differential between the two orange thrust arrows tips the craft, the tilted total thrust carries it across, and the same arrows level it out and brake at the target. Nudge kicks the craft to show the recovery; the faded line is the flight path.

Multicopter (side view)

click anywhere to set a waypoint

rotor thrust (commanded)gravityflight pathwaypoint (click to move)

The coupled nonlinear system has no closed form, so the simulator steps it forward with semi-implicit Euler at a fixed timestep $h$ , taking velocities first and then positions from the new velocities:

Algorithm:

\textsc{Step}(h)

— one semi-implicit Euler update of the rigid body

1
input: timestep $h$ , rotor forces $\mathbf f_k$ at offsets $\mathbf r_k$
2
$\boldsymbol\tau \gets \sum_k \mathbf r_k \times \mathbf f_k$ ; $\ \mathbf f \gets \sum_k \mathbf f_k$
3
$\mathbf v \gets \mathbf v + h\,\mathbf f / m$
4
$\boldsymbol\omega \gets \boldsymbol\omega + h\,I^{-1}\!\left(\boldsymbol\tau - \boldsymbol\omega \times I\,\boldsymbol\omega\right)$
5
$\mathbf x \gets \mathbf x + h\,\mathbf v$
6
$R \gets \operatorname{orthonormalize}\!\left(R + h\,[\boldsymbol\omega]_\times R\right)$

The orientation update comes from $\dot{R} = [ω]_{\times} R$ , where $[ω]_{\times}$ is the skew-symmetric cross-product matrix; a Euler step drifts $R$ off the rotation group, so it is re-orthonormalized each frame. Updating velocity before position, rather than after, keeps the integrator stable at the timesteps a real-time simulation can afford, whereas explicit Euler gains energy and diverges.

References

Project repository
Reference notes: Linear Algebra

╌╌ END ╌╌