Data Structures
First some data structures.
Linear Algebra
I assume there are Vector3 classes for 3 valued vectors and Quaternion classes for 4 valued rotations as well as Matrix3 for 3×3 inertia and rotations and other operations. Each obeys its corresponding rules of linear algebra (addition +, subtraction -, scaling *, and products *). Each object has its own rules on how to handle these operators. It might be a scale operation, or a matrix-vector product, or a quaternion product depending on the context.
Body Properties
Each body has a scalar mass m, and a body mass moment of inertia tensor I_body which are used in the equations of motion. You also should keep the inverse inertia I_body_inv = inverse( I_body ).
Body State
The current state of the body, contains its position r (Vector3), orientation q (Quaternion) and motion. But commonly instead of storing the two velocity vectors (translational v (Vector3), rotational ω (Vector 3)), the two momentum vectors are stored (linear p (Vector 3), angular L (Vector 3)). Each summed at the center of mass.
The state vector has 13 components since each vector has 3 and the quaternion q has 4
Y = [ r, q, p, L ]
You can choose to make Y an array/vector, or a struct with appropriate operators to perform linear algebra with.
Motion Extraction
Given Y you can always extract the translational velocity v and angular velocity ω of the body with the following algorithm
[v, ω] = GetMotion(Y)
// Get Motion from Y=[r, q, p, L]
r = Y[0:2];
q = Y[3:6];
p = Y[7:9];
L = Y[10:12];
// Get 3×3 rotation matrix R from orientation q
R = rotation(q);
// Get 3×3 inverse mass moment of inertia on the world frame
I_inv = R * I_body_inv * transpose(R);
// Get translational velocity vector
v = p / m;
// Get rotational velocity vector
ω = I_inv * L;
return [v, ω];
The reverse is also needed to set the initial conditions of the body
Y = GetState(r, q, v, ω)
// Get state vector from position & motion
// Get 3×3 rotation matrix R from orientation q
R = rotation(q);
// Get 3×3 mass moment of inertia on the world frame
I = R * I_body * transpose(R);
// Get translational momentum vector
p = m * v;
// Get angular momentum vector
L = I * ω;
return [r, q, p, L];
Integration
At each time step the applied load vectors (force F, torque τ) change the body state vector.
Body State Rate
The time rate of the body state, I call Y', is calculated as follows
Y' = GetRate(t, Y)
// Assemble the time rate of the body state Y at time t
r = Y[0:2];
q = Y[3:6];
p = Y[7:9];
L = Y[10:12];
// Get the motion of the body
[v, ω] = GetMotion(Y);
// Get the loading
[F, τ] = GetLoading(t, r, q, v, ω);
r' = v
q' = 0.5*quaterion(ω)*q
p' = F
L' = τ
return [r', q', p', L'];
where quaternion(ω) = [ωx,ωy,ωz,0] and quaterion product * is per quaternion rules.
Runge-Kutta 4
It is common to integrate over time using the RK4 method
ΔY = GetStep(t, Y, h)
// Integrate state vector Y, over time step h at time t
K0 = GetRate(t, Y);
K1 = GetRate(t+h/2, Y + h/2*K0);
K2 = GetRate(t+h/2, Y + h/2*K1);
K3 = GetRate(t+h, Y + h*K2);
return h/6*(K0 + 2*K1 + 2*K2 + K3);
Simulation
A simulation is the integration of the state vector over many finite time steps.
Body Loading
A loading function that describes the forces/torques on each body at each time frame is needed to evaluate the state rate
[F,τ] = GetLoading(t, r, q, v, ω)
// Describe the force vector F, and torque vector τ
// from time t, position r, orientation q,
// velocity v, and rot. velocity ω
Simulation Loop
And the simulation loop integrates until the target time is reached
Y = GetState(r, q, v, ω);
time = 0;
while(time<target_time)
{
Y += GetStep(time, Y, h);
time += h;
}
And there you have the basics of a single rigid body simulation.
Post Processing
When you have the body state Y you get the motion of the center of mass with [v, ω] = GetMotion(Y). But to answer the second question you need to the velocity of an arbitrary point v_A. The point is located with vector r_A
v_A = GetVelocityAtPoint(Y, r_A)
// Get velocity of world point A
r = Y[0:2]
[v, ω] = GetMotion(Y)
return v + cross(ω, r_A - r);
If you want the velocity of a point v_B riding on the body, and defined by a local vector r_B_body then
v_B = GetVelocityAtBodyPoint(Y, r_B_body)
// Get velocity of body point B
r = Y[0:2]
q = Y[3:6]
R = rotation(q)
r_A = r + R*r_B_body
return GetVelocityAtPoint(Y, r_A)
Note that over many steps, the orientation Quaternion q might drift from a unit quaternion and it would need to be re-normlized.
q = normalize(q)
// m = sqrt(qx^2+qy^2+qz^2+qw^2)
// q = q/m
In my experience, the drifting away from a unit quaternion does not get progressively worse, as it tends to drift away and then drift back to a unit quaternion, and the resulting error in orientation angle is actually pretty small.