Rigid Body Dynamics Huamin Wang The motion of a rigid body contains two parts: linear motion and angular motion. In this docu- ment, we will study both motions. We will also discuss how to detect collisions and estimate collision responses. To simplify our notations, we will follow the standard formats: bold fonts are vectors, italic fonts are scalars, and UPPERCASES are matrices. The physics quantities associated with linear motion are: position, velocity, force, ... The physics quantities associated with angular motion are: orientation, angular velocity, torque, ... 1 Linear Motion Let us first examine the linear motion of a rigid body. Without considering the orientation, this is equivalent to simulating the motion of the body mass center x(t), which changes as a function of time t. Assuming that the initial model is centered at the origin as shown in Figure 1a. We can simply treat x(t) as the position of the rigid body. To model the linear motion, we also define the derivative of x(t) to time t, which is commonly known as the linear velocity v(t). To animate the linear motion, our goal is to update the position and the velocity of the center, each time the Update function gets called. Let the time of the last Update call be t0 and the time of the current call be t1. The time difference ∆t = t1 − t0 is known as the time step. Given the position and the velocity at t0, we calculate the new position and the new velocity as: ( 1 R t1 v(t1) = v(t0) + f(t)dt ≈ v(t0) + ∆tf(t0)=M; M t0 (1) x(t ) = x(t ) + R t1 v(t)dt ≈ x(t ) + ∆tv(t ); 1 0 t0 0 1 in which M is the mass of the object and f(t0) is the total force evaluated at time t0. If we assume that the mass distributed at each vertex i is mi, the total mass is simply their sum: X M = mi: (2) i Note that Equation 6 is just an approximation. Formally, this is called explicit time integration. We will discuss other time integrators later. One question here is how to evaluate f(t0). This vector represents the total force applied on the gravity 2 whole rigid body. It contains the gravity force f (t0) = Mg, where g = [0; −9:8m=s ; 0] is the gravity acceleration. It contains other forces as well, such as the collision force. Many simulators also model the damping effect by some damping force. Let us do not do this for now. 2 Angular Motion To simulate the angular motion of a rigid body, we must define everything in an angular way. 2.1 Quaternion The first and foremost question is how to define the orientation, i.e., \angular position". One idea is to use the matrix. However, only an orthonormal matrix can be a rotational matrix and we must keep enforcing the orthonormality, which is not very convenient. Alternatively, we can define a bunch 1 ܘ௜ ௜ሺݐሻܚ ሺݐሻܠ ۼ ௜ܘ ܘ௜ ௜ሺݐሻܚ ௜ሺݐሻܚ ௜ሺݐሻܚ+ሺݐሻܠ ሺݐሻ ሻ ሻܠ ሺݐሻܠ ௜ሺݐܚ+ ሺݐܠ ௜ሺݐሻܚ+ሺݐሻܠ ௜ሺݐሻܚ ሺݐሻ+࣓ሺݐሻ ൈܞ (a) Rest configuration (b) Moved configuration (c) Collision configuration Figure 1: A rigid body under different configurations. of rotation angles around X, Y, and Z axes, known as Euler angles. Again, that is not easy to derive the formula for their time evolution. Our solution is to use quaternion. A quaternion is a 4D vector q = [x; y; z; w], or q = [v; w], which contains a 3D vector v and a scalar w. A quaternion must be normalized: x2 + y2 + z2 + w2 = 1: (3) A quaternion represents a rotation around axis v with an angle θ, such as w = cos(θ=2) and x2 + y2 + z2 = sin2(θ=2). A quaternion can be converted into a rotation matrix as: 2 w2 + x2 − y2 − z2 2(xy − wz) 2(xz + wy) 3 R = 4 2(xy + wz) w2 − x2 + y2 − z2 2(yz − xw) 5 : (4) 2(xz − wy) 2(yz + xw) w2 − x2 − y2 + z2 Compared with a rotational matrix, a quaternion is very easy to maintain: we just have to make it normalized all the time. Quaternion arithmetic is defined as follows. Let q1 = [v1; w1] and q2 = [v2; w2] be two quaternion vectors and s be a scalar. We have: q1 + q2 = [v1 + v2; w1 + w2]; sq1 = [sv1; sw1]; (5) q1q2 = [w1v2 + w2v1 + v1 × v2; w1w2 − v1 · v2]: Intuitively, q1q2 means rotation by q2, and then rotation by q1. 2.2 Time Evolution The angular motion of a rigid body can be updated as follows: ( R t1 −1 −1 !(t1) = !(t0) + I (t)τ (t)dt ≈ !(t0) + ∆tI (t0)τ (t0); t0 (6) q(t ) = q(t ) + 1 R t1 !^(t)q(t)dt ≈ x(t ) + 1 ∆t!^(t )q(t ); 1 0 2 t0 0 2 1 0 where !^ = [!; 0] is a quaternion extended from ! and !^(t1)q(t0) is the product of two quaternions. The 3D vector !(t) is the angular velocity. Its direction represents the angular velocity axis and its magnitude represents the rotation speed. The matrix I−1(t) is the body inertia. Like the mass, the inertia indicates the resistance of the body to rotational motion. But unlike the mass, the inertia 2 is not constant. Intuitively, when a body rotates, its resistance to the rotation around X, Y, Z axes will also change. So we must update the inertia as a function of time: I(t) = R(t)IbodyRT(t); (7) where R(t) is the rotational matrix calculated using the quaternion q(t) at time t, and Ibody is the inertia in the original configuration: body X T T I = (pi pi)1 − pipi : (8) i Here pi the original position of a vertex i and 1 is the 3-by-3 identity matrix. Now the only question is: how to evaluate the torque τ (t0) at time t0? Let fi(t) be the force applied at an individual vertex i, we calculate the torque as: X X τ (t0) = ri(t) × fi(t) = (R(t)pi) × fi(t); (9) i i where ri(t) is the vector from the center to vertex i, after the body is rotated as shown in Figure 1b. Note that we do not need to consider the gravity force in angular motion. This is because the gravity should not cause the body to spin. Mathematically, this implies the torques caused by the gravity are summed to zero. 3 Collision Handling To make rigid body simulation interesting, we must use interesting forces, specially the force caused by collision. To begin with, let us consider the status a vertex i shown in Figure 1c. Its position at time t is x(t) + ri(t), and its velocity at time t is: vt + !(t) × ri(t), where ri(t) = R(t)pi is the vector from the center to i. We claim that the vertex is in collision with the floor, if two conditions are satisfied: • a. Its position xi(t) is below the ground floor: y = 0. • b. Its velocity vi(t) is still going down. Let N = [0; 1; 0] be the upward vector. We formulate the two conditions as: x (t) · N = (x(t) + r (t)) · N < 0; i i (10) vi(t) · N = (vt + !(t) × ri(t)) · N < 0: By testing these two conditions, we can know if vertex i is in collision at time t. 3.1 Collision Response Once we find the collision, we need to remove it by applying collision response to the rigid body. There are many ways to calculate the collision response. Here we choose to use a collision impulse method, which gives more plausible results than other approaches, such as penalty forces. Our basic idea is to formulate the expected vertex velocity after collision handling. We know that before collision, the vertex is moving toward the ground floor: vi(t) · N < 0. So after collision new handling, we hope the vertex can move away from the ground floor: vi (t) · N = −µvi(t) · N > 0, in which µ 2 [0; 1) is called the restitution coefficient. Assuming that the velocity change happens only in the Y direction (friction free), we have: new new new vi (t) = v (t) + ! (t) × ri(t) = vi(t) − (µ + 1) (vi(t) · N) N: (11) 3 To achieve vnew(t) and !new(t), we assume that there is a sudden but unknown impulse j applied at vertex i and it causes the following changes to the velocities: vnew(t) = v(t) + j=M; new −1 (12) ! (t) = !(t) + I (t)(ri(t) × j) : By combining Equation 11 and Equation 12, we get: −1 j=M + I (t)(ri(t) × j) × ri(t) = −(µ + 1) (vi(t) · N) N: (13) To solve Equation 13, we propose to define the cross product by a matrix product: 2 3 0 −riz riy ∗ ri(t) × j = Ri j = 4 riz 0 −rix 5 j; (14) −riy rix 0 where ri(t) = [rix; riy; riz]. So we get: 1 Kj = 1 − R∗I−1R∗ j = −(µ + 1) (v (t) · N) N; (15) M i i i and we can then solve j using the inverse of the matrix K. Once we get j, we use it to update v(t) and !(t) respectively. That concludes our collision handling process. 3.2 More Details There are more implementation details. What if there are many vertices in collision? Just detect them all and use the average displacement to the center as ri(t) instead. Why oscillation? If you see oscillation when the body sits on the floor, this is because it keeps bouncing back and forth due to the restitution coefficient.