Nonlinear CCD

We also implement CCD of nonlinear trajectories (of linear geometry) using the method of Ferguson et al. [2021]. While Ferguson et al. [2021] introduce their method in the context of rigid bodies, it can be applied to any nonlinear trajectory of linear geometry.

The method works by transforming the nonlinear trajectories into (adaptive) piecewise linear trajectories with an envelope/minimum separation around each piece, enclosing the nonlinear trajectory. The method then performs CCD on the piecewise linear trajectories to find the earliest time of impact.

We provide the following functions to perform nonlinear CCD:

Each of these functions take as input a ipc::NonlinearTrajectory object for the endpoints of the linear geometry.

Each of these functions take as input a ipctk.NonlinearTrajectory object for the endpoints of the linear geometry.

For example, the following code defines a rigid trajectory in 2D in order to perform nonlinear CCD between a point and edge:

class Rigid2DTrajectory : virtual public ipc::NonlinearTrajectory {
public:
    Rigid2DTrajectory(
        const Eigen::Vector2d& _position,
        const Eigen::Vector2d& _translation,
        const Eigen::Vector2d& _delta_translation,
        const double _rotation,
        const double _delta_rotation)
        : position(_position)
        , translation(_translation)
        , delta_translation(_delta_translation)
        , rotation(_rotation)
        , delta_rotation(_delta_rotation)
    {
    }

    VectorMax3d operator()(const double t) const override
    {
        const Eigen::Matrix2d R =
            Eigen::Rotation2D<double>(rotation + t * delta_rotation)
                .toRotationMatrix();

        return R * position + translation + t * delta_translation;
    }

    double
    max_distance_from_linear(const double t0, const double t1) const override
    {
        if (delta_rotation * (t1 - t0) >= 2 * igl::PI) {
            // This is the most conservative estimate
            return 2 * position.norm(); // 2 * radius
        }

        const VectorMax3d p_t0 = (*this)(t0);
        const VectorMax3d p_t1 = (*this)(t1);
        return ((*this)((t0 + t1) / 2) - ((p_t1 - p_t0) * 0.5 + p_t0)).norm();
    }

protected:
    Eigen::Vector2d position;
    Eigen::Vector2d translation;
    Eigen::Vector2d delta_translation;
    double rotation;
    double delta_rotation;
};

class Rigid2DTrajectory(ipctk.NonlinearTrajectory):
    def __init__(self, position, translation, delta_translation, rotation, delta_rotation):
        ipctk.NonlinearTrajectory.__init__(self)
        self.position = position
        self.translation = translation
        self.delta_translation = delta_translation
        self.rotation = rotation
        self.delta_rotation = delta_rotation

    def __call__(self, t):
        theta = self.rotation + t * self.delta_rotation
        R = np.array([[np.cos(theta), -np.sin(theta)],
                      [np.sin(theta), np.cos(theta)]])
        return R @ self.position + self.translation + t * self.delta_translation

    def max_distance_from_linear(self, t0, t1):
        if self.delta_rotation * (t1 - t0) >= 2 * np.pi:
            # This is the most conservative estimate
            return 2 * np.linalg.norm(self.position)  # 2 * radius
        p_t0 = self(t0)
        p_t1 = self(t1)
        return np.linalg.norm(self((t0 + t1) / 2) - ((p_t1 - p_t0) * 0.5 + p_t0))

Defining the Trajectory

Let’s dive deeper by breaking down the implementation of Rigid2DTrajectory. The first function we need to implement is the call operator:

VectorMax3d operator()(const double t) const override
{
    const Eigen::Matrix2d R =
        Eigen::Rotation2D<double>(rotation + t * delta_rotation)
            .toRotationMatrix();

    return R * position + translation + t * delta_translation;
}

def __call__(self, t):
    theta = self.rotation + t * self.delta_rotation
    R = np.array([[np.cos(theta), -np.sin(theta)],
                  [np.sin(theta), np.cos(theta)]])
    return R @ self.position + self.translation + t * self.delta_translation

This function computes the position of the point at a time \(t \in [0, 1]\). This defines the trajectory of the point. In this case, we have a rigid body with a center of mass (COM) at the origin. The trajectory of the point is given by:

\[x(t) := R(\theta + t \Delta \theta) \bar{x} + T + t \Delta T\]

where \(\theta\) is the angle of rotation about the COM, \(T\) is the translation of the COM, \(\Delta \theta\) and \(\Delta T\) are the updates to \(\theta\) and \(T\), respectively, and \(\bar{x}\) is the position of the point in the interial frame.

Computing a Conservative Envelope

The second function we need to implement is max_distance_from_linear.

double
max_distance_from_linear(const double t0, const double t1) const override
{
    if (delta_rotation * (t1 - t0) >= 2 * igl::PI) {
        // This is the most conservative estimate
        return 2 * position.norm(); // 2 * radius
    }

    const VectorMax3d p_t0 = (*this)(t0);
    const VectorMax3d p_t1 = (*this)(t1);
    return ((*this)((t0 + t1) / 2) - ((p_t1 - p_t0) * 0.5 + p_t0)).norm();
}

def max_distance_from_linear(self, t0, t1):
    if self.delta_rotation * (t1 - t0) >= 2 * np.pi:
        # This is the most conservative estimate
        return 2 * np.linalg.norm(self.position)  # 2 * radius
    p_t0 = self(t0)
    p_t1 = self(t1)
    return np.linalg.norm(self((t0 + t1) / 2) - ((p_t1 - p_t0) * 0.5 + p_t0))

This function computes the maximum distance over a time interval \([t_0, t_1]\) between the nonlinear trajectory and a line segment from \(x(t_0)\) to \(x(t_1)\). Mathematically this function computes

\[\min_{t\in[0, 1]} \|x((t_1 - t_0) t + t_0) - ((x(t_1) - x(t_0))t + x(t_0))\|,\]

for a given start and end time \(t_0\) and \(t_1\), respectively.

In the case of a 2D rigid body, we can compute this value analytically because we know the \(\arg\!\min\):

\[\underset{t\in[0, 1]}{\arg\!\min} \|x((t_1 - t_0) t + t_0) - ((x(t_1) - x(t_0))t + x(t_0))\| = 0.5,\]

for \((t_1 - t_0) \Delta \theta \leq \pi/2\), otherwise we can use the most conservative envelope radius of \(2 \|\bar{x}\|\).

Performing Nonlinear CCD

Last, we use the Rigid2DTrajectory to perform nonlinear CCD between a point and edge:

// Static point
const Rigid2DTrajectory p(
    Eigen::Vector2d(0, 0.5), Eigen::Vector2d::Zero(),
    Eigen::Vector2d::Zero(), 0, 0);
// Rotating edge
const Rigid2DTrajectory e0(
    Eigen::Vector2d(-1, 0), Eigen::Vector2d::Zero(),
    Eigen::Vector2d::Zero(), 0, igl::PI);
const Rigid2DTrajectory e1(
    Eigen::Vector2d(+1, 0), Eigen::Vector2d::Zero(),
    Eigen::Vector2d::Zero(), 0, igl::PI);

double toi;
bool collision = ipc::point_edge_nonlinear_ccd(
    p, e0, e1, toi, /*tmax=*/1.0, /*min_distance=*/0,
    TightInclusionCCD::DEFAULT_TOLERANCE,
    TightInclusionCCD::DEFAULT_MAX_ITERATIONS,
    // increase the conservative_rescaling from 0.8 to 0.9 to get a more
    // accurate estimate
    /*conservative_rescaling=*/0.9);

CHECK(collision);
CHECK((0.49 <= toi && toi <= 0.5)); // conservative estimate

p = Rigid2DTrajectory(
    np.array([0, 0.5]), np.zeros(2), np.zeros(2), 0, 0)
e0 = Rigid2DTrajectory(
    np.array([-1, 0]), np.zeros(2), np.zeros(2), 0, np.pi)
e1 = Rigid2DTrajectory(
    np.array([1, 0]), np.zeros(2), np.zeros(2), 0, np.pi)

# increase the conservative_rescaling from 0.8 to 0.9 to get a more accurate estimate
collision, toi = ipctk.point_edge_nonlinear_ccd(
    p, e0, e1, conservative_rescaling=0.9)

assert collision
assert 0.49 <= toi <= 0.5  # conservative estimate

Note

We adjust the conservative_rescaling parameter to get a more accurate time of impact (TOI), but in practice, this is not needed as a more conservative estimate of the TOI is sufficient to avoid penetrations.

Interval-Based Nonlinear CCD

Warning

The following section requires enabling the filib interval arithmetic library. filib is licensed under L-GPL 2.1, so special care must be taken when using it. See the filib dependency note for more information.

If an analytic expression for the max_distance_from_linear function is not available, we can use interval arithmetic to compute a conservative envelope.

The ipc::IntervalNonlinearTrajectory class does this for us and all we need to provide is a function to compute the point’s position over a time interval.

The ipctk.IntervalNonlinearTrajectory class does this for us and all we need to provide is a function to compute the point’s position over a time interval.

Conservative Envelope

Our implementation of the max_distance_from_linear function is as follows.

Start by defining a linear interpolation function:

\[\operatorname{lerp}(a, b, t) := (b - a) t + a,\]

which interpolates between two points \(a\) and \(b\) at time \(t\).

The exact envelope from above is bounded by a interval approximation:

\[\begin{split}\begin{align} &\max_{t \in [0, 1]} \| p(\operatorname{lerp}(t_0, t_1, t)) - \operatorname{lerp}(p(t_0), p(t_1), t) \|_2\\ &\leq \sup(\| p_{\Box}([t_0, t_1]) - ((p(t_1) - p(t_0)) \cdot [0, 1] + p(t_0)) \|_2). \end{align}\end{split}\]

Therefore, we can compute the a conservative estimate of the envelope by implementing \(p_{\Box}([t_0, t_1])\) and \(p(t)\).

Note

In practice we subdivide the interval into smaller intervals and compute the envelope over each subinterval. This is done to create a more accurate estimate.

Interval Arithmetic

Interval arithmetic is a method to compute bounds on the range of a function over an interval. We can construct a vector of intervals to represent the position of the point over a time interval.

The following code snippet shows an example of how to use interval arithmetic to compute the position of a point over a time interval:

#include <ipc/utils/interval.hpp>

using namespace ipc;

Vector2I position(
    const VectorMax3d& center,
    const VectorMax3d& point,
    const double omega,
    const Interval& t)
{
    // 2×2 matrix of intervals representing the rotation matrix
    Matrix2I R;
    R << cos(omega * t), -sin(omega * t),
         sin(omega * t),  cos(omega * t);
    return R * (point - center) + center;
}

The full documentation for the Interval class can be found here.

import numpy as np

from ipctk.filib import Interval, sin, cos

def position(center, point, omega, t : Interval):
    R = np.array([
        [cos(omega * t), -sin(omega * t)],
        [sin(omega * t),  cos(omega * t)]
    ])
    return R @ (point - center) + center

The full documentation for the filib python bindings can be found here.

Last update: Dec 12, 2024