Rigid Body Simulation Basics — Part 2: From Positional Constraints to Velocity Space Constraints

August 31, 2023
Rss Fetcher

Rigid Body Simulation Basics — Part 2: From Positional Constraints to Velocity Space Constraints

Screen captures from constraint-based rigid body simulators by author

Introduction

In Part 1, we covered the core idea of the velocity-space constraint-based rigid body simulation. We derived a constrained convex optimization problem from Newton’s Second Law and the velocity-space constraints. The problem was then converted to a mixed linear complementarity problem, and it was solved by an iterative or a pivot-based solver. We have not yet covered the main topic of Part 2:

How do we make velocity-space constraints?

In this article, we will derive velocity-space constraints from positional ones. We still limit our discussion to the two-dimensional linear motions and omit the treatment of the angles and orientations to focus on the core argument. However, applying it to the angular motions in the three-dimensional space should be easy once you have the basic idea.

This article is organized as follows. First, we introduce the positional constraints and study their characteristics briefly, and then we convert the positional constraints into the velocity space by the linearization technique.

Finally, we look at the part of the code of the sample program where the constraints are generated. Let’s begin.

Positional Constraints

Assume we are given a positional constraint between two rigid bodies, k0 and k1:

Or, if it is between one rigid body k0 and a fixture in the world coordinate space, we have the following:

where xₖ₀(t) and xₖ₁(t) are the positions of the center of mass of the rigid bodies k0 and k1 at time step t, respectively, and

indicates either ≥(inequality) or =(equality) for notational convenience. The nᵢ₀ ∈ R², nᵢ₁ ∈ R², and bᵢ ∈ R are the parameters of the linear constraint. As you can see, we deal with the linear constraints, i.e., LHS is the linear function of x. Let’s also introduce the following three new vectors for notational convenience.

Then, the constraint above will be more compact, and it will look like this:

For the constraints with a single term on the LHS of the equation (2), we can conceptually use a dummy rigid body k1 for xₖ₁(t) and set nᵢ₁ to 0.
We want to convert it to a velocity-space constraint.

And we would like Fᵢ to be a linear function of vₖ₁(t). We will later find out this is indeed the case. The converted velocity-space constraint will be expressed as follows:

First, let’s examine the characteristics of the vector nᵢ and the scalar bᵢ briefly. Please take a look at Fig. 1.

Figure 1: nᵢ₀, nᵢ₁, and bᵢ as functions of xₖ₀(t) and xₖ₁(t).

As we still limit our discussion to the two-dimensional linear motions, nᵢ₀ and nᵢ₁ are usually in the opposite direction of each other and of equal length, often the unit length. Usually, nᵢ∗ indicates the direction along which two objects should stay away or stay at the specified distance. And ∆t·bᵢ indicates the signed distance of xₖ₁ from xₖ₀ in the direction of nᵢ₁, or vice versa. As we are dealing with rigid bodies, i.e., they don’t deform over time, nᵢ and bᵢ are not functions of time t but functions of xₖ.

In fact, for the linear velocities, it is usually the case that only the relative position of xₖ₁ to xₖ₀ matters, i.e., they are translation-invariant. In this case, let xᵣ = xₖ₁ − xₖ₀, and

If the two objects are polygons, it is mostly the case that nᵢ∗and bᵢ are constant in the vicinity of the current xₖ₀, and xₖ₁.

We have a rough idea about what nᵢ∗and bᵢ look like. Now, let’s derive velocity-space constraints from positional ones.

Conversion by Linearization

This section will discuss the standard way to convert positional constraints to velocity-space ones. It is called ‘linearization’ [SS98].

In preparation, we introduce three pairs of positions as follows. Please take a look at Fig. 2.

Figure 2: xₖ(t_c), xₖ^*(t_c+∆t), and xₖ(t_c+∆t).

xₖ(t_c) denotes the pair of positions at the current time step t_c.
xₖ^*(t_c+∆t) denotes the pair of positions at the next time step t_c + ∆t found without the constraints, i.e., only by Newton’s Second Law. The xₖ^*(t_c+∆t) may violate some of the constraints.
xₖ(t_c+∆t) is the pair we want to find for the next time step in the feasible region.

Please note that xₖ(t_c) and xₖ^*(t_c+∆t) are known, and xₖ(t_c+∆t) is unknown at this point in the simulation.

Let

Then, the positional constraints will become:

Let’s take the gradient of aᵢ(xₖ).

This shows the rate of the change of aᵢ(xₖ) if we moved xₖ slightly in each axis of xₖ.

Now, let’s approximate aᵢ(xₖ(t_c+∆t)) from aᵢ(xₖ^*(t_c+∆t)) and

by the first-order Taylor expansion (hence ’linearization’):

From here on, we denote

by ∇^∗aᵢ(xₖ), and denote xₖ^*(t_c+∆t) by xₖ^* to avoid cluttering the equation. Then the equation (10) will become:

We want to express the constraint in terms of

And therefore, we want to have it in the LHS of equation 11. Let’s take the term with xₖ(t_c+∆t), which we want to eliminate from LHS, as it’s an unknown value. For that, we apply the first-order Taylor
expansion backwards from t_c+∆t to t_c as follows:

Then substitute xₖ(t_c+∆t) in equation (11) with equation (12), and we get

and then sort the terms to

This is the velocity space constraint we wanted to find. We know xₖ(t_c), and can easily calculate xₖ^*. The only tricky part is ∇^∗aᵢ(xₖ), which is usually calculated from the curvature of the two rigid bodies.

If nᵢ and bᵢ stay constant, for most cases of the polygon-polygon contact, we can simplify it further with the following argument.

Since nᵢ and bᵢ stay constant:

therefore

and the equation gets simplified as follows:

As aᵢ(xₖ^*) = nᵢ · xₖ^* + bᵢ, the equation can be further simplified to:

This is the most useful form of the constraints in the velocity space we wanted to find. It works well for cases where the curvature of the contact surface is not significant, such as polygon-polygon contacts. Even for curved contacts such as sphere-sphere contacts, we can apply this technique with the caveat described in the section Practical Tips below.

Example

Now, let’s look at a simple unilateral constraint in action. Please take a look at Fig. 3.

It solves the collision of a falling disc against a slope. The following parametric form of s expresses the slope floor.

where p_f is a point on the floor surface, n_f is the surface normal, and n_f^⊥ is the unit vector perpendicular to n_f. The disc’s radius is denoted by rₖ. Then, the signed distance of the disc to the floor is:

Assume we have detected a collision of the disc to the floor at the time step t_c+∆t as shown in Fig. 3, i.e., aᵢ(t_c+∆t) < 0. Also, assume the collision detector has given us the contact normal n_f. Then, the velocity-space unilateral constraint to solve the collision will look like this:

We can apply the same argument to the collisions between two rigid bodies and the joints with bilateral constraints. Chapter 7 of [ESHD05], especially Sections 7.5 to 7.9, contains the comprehensive treatment of the constraints for most joint types.

Sample Program

The code is available in my GitHub repo. See README for the installation instructions. Take a look at the section ‘Sample Porgram’ in my previous article first if you haven’t.

Let’s resume our exploration of the code. In the previous article, we briefly looked at the code organization in Simulation and its top-level function Simulator::update(). This article will examine the construction of unilateral and bilateral constraints. The unilateral constraints are used to solve the collisions among the discs and against the walls, and the bilateral constraints keep the seven small discs chained together.

Construction of the unilateral constraints

Please see the code snippet below. Simulator::update() calls detectCollisions() at (1), which in turn calls detectCollisionPair() at (2) and detectCollisionAgainstWalls() at (3). These latter two functions generate unilateral constraints.

class Simulator {
    ...
    void update( ... ) {
        ...
        detectCollisions( delta_t ); // <= (1)
        ...
    }
    ...
    void detectCollisions( const float delta_t )
    {
        for ( int i = 0; i < m_discs.size(); i++ ) {

            for ( int j = i + 1; j < m_discs.size(); j++ ) {

                detectCollisionPair( m_discs[i], m_discs[j], delta_t );// <=(2)
            }
            detectCollisionAgainstWalls( m_discs[i], delta_t ); // <= (3)
        }
    }
};

Let’s start with the simplest one: detectCollisionAgainstWalls(), which checks one disc for possible collisions against four walls: left, right, upper, and lower. Let’s look at the test against the left wall as an example.

    void detectCollisionAgainstWalls( ChainedDisc* d0, const float delta_t )
    {
        if ( d0->m_com_tmp.x - d0->m_radius <= -0.5f * m_area_width ) {// <=(4)

            const auto signed_dist = d0->m_com.x - d0->m_radius + 0.5f * m_area_width; // <= (5)
            const Vec2 n0{ 1.0f, 0.0f }; // <= (5)

            auto* constraint = new VelocityConstraint{ VelocityConstraint::Unilateral, d0, nullptr, n0, n0, -1.0f * signed_dist / delta_t };
            m_constraints.push_back( constraint );
        }
        ...
    }

This code is best explained graphically. Please take a look at Fig. 4 below.

The ‘if’ condition at (4) tests if there is an overlap between the left wall and the left side of the disc. Please note that d0->m_com_tmp holds the temporary position xₖ^*(t_c+∆t) at the next time step t_c+∆t with no constraints applied, while d0->m_com holds the position xₖ(t_c) at the current time step t_c.

If there is an overlap (often called penetration), then we make one unilateral constraint. The signed_dist and n0 at (5) in the code above correspond to the value of aᵢ(xₖ(t_c)) and nᵢ in equation (18). You can see that the vector n0 is oriented such that the impulse is directed to move the disc further against the wall. We perform the same tests for the other three walls.

Next, let’s look at the disc-disc collisions in detectCollisionPair().

    void detectCollisionPair( ChainedDisc* d0,  ChainedDisc* d1, const float delta_t )
    {
        if ( d0->m_next == d1 || d0->m_prev == d1 || d1->m_next == d0 || d1->m_prev == d0 ) { // <= (6)
            return;
        }

        const auto v_1_to_0_tmp = d0->m_com_tmp - d1->m_com_tmp;
        const auto sq_len_tmp   = v_1_to_0_tmp.sq_length();
        const auto min_dist = d0->m_radius + d1->m_radius;

        if ( sq_len_tmp <= min_dist * min_dist + EPSILON ) { // <= (7)

            const auto v_1_to_0 = d0->m_com - d1->m_com; // <= (8)
            const auto len = v_1_to_0.length(); // <= (9)
            if ( len >= EPSILON ) { // <= (11)
                const auto signed_dist = len - min_dist; // <= (10)
            
                const auto n0 = v_1_to_0 / len;
                const auto n1 = n0 * -1.0f;

                auto* constraint = new VelocityConstraint{ VelocityConstraint::Unilateral, d0, d1, n0, n1, -1.0f * signed_dist / delta_t };
                m_constraints.push_back( constraint );
            }
        }
    }

The check at (6) excludes the collision tests between two chained discs. The code at (7) tests if there is a penetration between two discs in the temporary positions xₖ^*(t_c+∆t). If there is, then we construct one unilateral constraint.

At (8), (9), and (10), we generate the parameters for the unilateral constraints. The reason why m_com instead of m_com_tmp is used will be described below in Practical Tips.

The check at (11) avoids cases where the two discs are almost concentric. We simply ignore those cases as the vector v_1_to_0 degenerates to a point, and we can’t get the direction information from it. In a more robust simulator, we would have to deal with those cases using other information, such as velocity.

Practical tips for the contact generation for the curved contacts

We must be careful when applying the ‘linearization’ with equation (14) if the contact surfaces are significantly curved. The disc-disc contacts with small radii above fall in those cases. To apply equation (14), let’s find the gradient of aᵢ(xᵣ) using xᵣ = x₁ − x₀. The aᵢ(xᵣ), which can be interpreted as the closest distance between two discs, is defined as follows:

where r₀ and r₁ are the radii of the discs. The gradient of aᵢ(xᵣ) is:

Then using the equation (14),

The asterisk version of xᵣ corresponds to v_1_to_0_tmp in the code above. However, at (8), (9), and (10), we use v_1_to_0, which corresponds to the following:

If the curvature of the surface curves is significant, we can no longer apply the linearization, i.e., the difference between xᵣ/||xᵣ|| and x^*ᵣ/||x^*ᵣ|| becomes nonnegligible. In this case, equation (25) empirically provides a more stable simulation. The simulation would become particularly unstable if the radii of the discs were small or if the time step is big.

Construction of the bilateral constraints

Now, let’s look at the bilateral constraints. Bilateral constraints are used to chain the seven small discs together.

class Simulator {
    ...
    void update( ... ) {
        ...
        constructBilateralConstraints( delta_t );
        ...
    }
    ...

    void constructBilateralConstraints( const float delta_t )
    {
        for ( auto* disc : m_discs ) {

            if ( disc->m_next != nullptr ) {

                linkTwoDiscs( disc, disc->m_next, delta_t );
            }
        }
    }

    void linkTwoDiscs( ChainedDisc* d0, ChainedDisc* d1, const float delta_t )
    {
        const auto v_1_to_0    = d0->m_com - d1->m_com;
        const auto len         = v_1_to_0.length();
        const auto signed_dist = len - ( d0->m_radius + d1->m_radius );
            
        const auto n0 = v_1_to_0 / len;
        const auto n1 = n0 * -1.0f;

        auto* constraint = new VelocityConstraint{ VelocityConstraint::Bilateral, d0, d1, n0, n1, -1.0f * signed_dist / delta_t };
        m_constraints.push_back( constraint );
    }
};

The function linkTwoDiscs() generates one bilateral constraint. In fact, the only difference from the constraint generated by detectCollisionPair() is the equality instead of inequality. The equality means that the two discs be kept at a specific distance (signed distance of 0 in this case).

That’s pretty much it for the contact generation. We have covered a few simple cases, but the basic principle is the same when we deal with more complex cases using a proper collision detector. It’s also similar when we expand the simulation to the angular motions. We can apply the same techniques described in this article.

Conclusion

In this article, we have covered the topics of velocity-space linear constraints. We have discussed some crude intuitive meanings of nᵢ and bᵢ, which is useful when you start building constraints from collisions and joints.

We have used the linearization technique to convert the positional constraints to the velocity ones. Finally, we have seen a simple example of a collision of a disc against a floor and derived a unilateral constraint in the velocity space.

We have also looked at the potential risk of applying the linearization to the curved contacts and discussed a practical way to handle those cases. Then, we looked at the code of the sample programs where the unilateral constraints are generated for the collisions, and the bilateral constraints are generated to keep the seven discs chained together.

I hope you enjoyed reading the article and playing with the sample program. In the coming articles, I plan to expand the argument to include angular motions and orientations in the two-dimensional space or dig a bit deeper into the MLCP solvers.

Happy programming.