Nonlinear CCD¶

We can also perform 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:

C++ Python

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:

C++ Python

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:

C++ Python

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.

C++ Python

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:

C++ Python

// 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, DEFAULT_CCD_TOLERANCE,
    DEFAULT_CCD_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.

Last update: Apr 03, 2024