<|start_filename|>tests/distance/test_point_line.cpp<|end_filename|>
#include <catch2/catch.hpp>

#include <iostream>

#include <finitediff.hpp>

#include <ipc/distance/point_line.hpp>
#include <ipc/utils/eigen_ext.hpp>

using namespace ipc;

TEST_CASE("Point-line distance", "[distance][point-line]")
{
    int dim = GENERATE(2, 3);

    double y_point = GENERATE(take(10, random(-100.0, 100.0)));
    VectorMax3d p = VectorMax3d::Zero(dim);
    p.y() = y_point;

    double y_line = GENERATE(take(10, random(-100.0, 100.0)));
    VectorMax3d e0 = VectorMax3d::Zero(dim);
    VectorMax3d e1 = VectorMax3d::Zero(dim);
    e0.x() = -1;
    e0.y() = y_line;
    e1.x() = 1;
    e1.y() = y_line;

    double distance = point_line_distance(p, e0, e1);
    double expected_distance = abs(y_point - y_line);
    CHECK(distance == Approx(expected_distance * expected_distance));
}

TEST_CASE("Point-line distance gradient", "[distance][point-line][gradient]")
{
    // int dim = GENERATE(2, 3);
    int dim = 2;

    double y_point = GENERATE(take(10, random(-10.0, 10.0)));
    VectorMax3d p = VectorMax3d::Zero(dim);
    p.y() = y_point;

    double y_line = GENERATE(take(10, random(-10.0, 10.0)));
    VectorMax3d e0 = VectorMax3d::Zero(dim);
    VectorMax3d e1 = VectorMax3d::Zero(dim);
    e0.x() = -1;
    e0.y() = y_line;
    e1.x() = 1;
    e1.y() = y_line;

    Eigen::VectorXd grad;
    point_line_distance_gradient(p, e0, e1, grad);

    // Compute the gradient using finite differences
    Eigen::VectorXd x(3 * dim);
    x.segment(0 * dim, dim) = p;
    x.segment(1 * dim, dim) = e0;
    x.segment(2 * dim, dim) = e1;
    auto f = [&dim](const Eigen::VectorXd& x) {
        return point_line_distance(
            x.head(dim), x.segment(dim, dim), x.tail(dim));
    };
    Eigen::VectorXd fgrad;
    fd::finite_gradient(x, f, fgrad);

    CAPTURE(dim, y_point, y_line);
    CHECK(fd::compare_gradient(grad, fgrad));
}

TEST_CASE("Point-line distance hessian", "[distance][point-line][hessian]")
{
    int dim = GENERATE(2, 3);

    double y_point = GENERATE(take(10, random(-10.0, 10.0)));
    VectorMax3d p = VectorMax3d::Zero(dim);
    p.y() = y_point;

    double y_line = GENERATE(take(10, random(-10.0, 10.0)));
    VectorMax3d e0 = VectorMax3d::Zero(dim);
    VectorMax3d e1 = VectorMax3d::Zero(dim);
    e0.x() = -1;
    e0.y() = y_line;
    e1.x() = 1;
    e1.y() = y_line;

    Eigen::MatrixXd hess;
    point_line_distance_hessian(p, e0, e1, hess);

    // Compute the gradient using finite differences
    Eigen::VectorXd x(3 * dim);
    x.segment(0 * dim, dim) = p;
    x.segment(1 * dim, dim) = e0;
    x.segment(2 * dim, dim) = e1;
    auto f = [&dim](const Eigen::VectorXd& x) {
        return point_line_distance(
            x.head(dim), x.segment(dim, dim), x.tail(dim));
    };
    Eigen::MatrixXd fhess;
    fd::finite_hessian(x, f, fhess);

    CAPTURE(dim, y_point, y_line);
    CHECK(fd::compare_hessian(hess, fhess, 1e-2));
}

TEST_CASE(
    "Point-line distance hessian case 1",
    "[distance][point-line][hessian][case1]")
{
    Eigen::Vector3d p(-10.8386, 10, -3.91955);
    Eigen::Vector3d e0(0, 0, -1);
    Eigen::Vector3d e1(-1, 0, 1);

    Eigen::MatrixXd hess;
    point_line_distance_hessian(p, e0, e1, hess);

    // Compute the gradient using finite differences
    Eigen::VectorXd x(9);
    x.segment<3>(0) = p;
    x.segment<3>(3) = e0;
    x.segment<3>(6) = e1;
    auto f = [](const Eigen::VectorXd& x) {
        return point_line_distance(x.head(3), x.segment(3, 3), x.tail(3));
    };
    Eigen::MatrixXd fhess;
    fd::finite_hessian(x, f, fhess);

    CHECK(fd::compare_hessian(hess, fhess, 1e-2));
}


<|start_filename|>src/utils/intersection.hpp<|end_filename|>
#pragma once

#include <Eigen/Core>

namespace ipc {

bool is_edge_intersecting_triangle(
    const Eigen::Vector3d& e0,
    const Eigen::Vector3d& e1,
    const Eigen::Vector3d& t0,
    const Eigen::Vector3d& t1,
    const Eigen::Vector3d& t2);

} // namespace ipc


<|start_filename|>src/utils/eigen_ext.hpp<|end_filename|>
#pragma once

#include <Eigen/Core>
#include <Eigen/SparseCore>

namespace ipc {

// Boolean scalar
typedef Eigen::Array<bool, Eigen::Dynamic, 1> ArrayXb;
typedef Eigen::Matrix<bool, Eigen::Dynamic, 1> VectorXb;
typedef Eigen::Matrix<bool, 3, 1> Vector3b;
typedef Eigen::Matrix<bool, Eigen::Dynamic, Eigen::Dynamic> MatrixXb;

template <typename T, int dim, int max_dim = dim>
using Vector = Eigen::Matrix<T, dim, 1, Eigen::ColMajor, max_dim, 1>;
template <typename T, int dim, int max_dim = dim>
using RowVector = Eigen::Matrix<T, 1, dim, Eigen::RowMajor, 1, max_dim>;
template <typename T> using Vector1 = Vector<T, 1>;
template <typename T> using Vector2 = Vector<T, 2>;
template <typename T> using Vector3 = Vector<T, 3>;
template <typename T> using VectorX = Vector<T, Eigen::Dynamic>;
template <typename T> using RowVector2 = RowVector<T, 2>;
template <typename T> using RowVector3 = RowVector<T, 3>;
template <typename T> using RowVectorX = RowVector<T, Eigen::Dynamic>;
template <typename T> using Matrix2 = Eigen::Matrix<T, 2, 2>;
template <typename T> using Matrix3 = Eigen::Matrix<T, 3, 3>;
template <typename T>
using MatrixX = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>;

using Vector1d = Vector1<double>;
/// @brief A dynamic size matrix with a fixed maximum size of 3 x 1
template <typename T> using VectorMax2 = Vector<T, Eigen::Dynamic, 2>;
/// @brief A dynamic size matrix with a fixed maximum size of 3 x 1
template <typename T> using VectorMax3 = Vector<T, Eigen::Dynamic, 3>;
/// @brief A dynamic size matrix with a fixed maximum size of 2 x 1
using VectorMax2d = VectorMax2<double>;
/// @brief A dynamic size matrix with a fixed maximum size of 3 x 1
using VectorMax3d = VectorMax3<double>;
/// @brief A dynamic size matrix with a fixed maximum size of 3 x 1
using VectorMax3i = VectorMax3<int>;
/// @brief A dynamic size matrix with a fixed maximum size of 6 x 1
template <typename T> using VectorMax6 = Vector<T, Eigen::Dynamic, 6>;
/// @brief A dynamic size matrix with a fixed maximum size of 6 x 1
using VectorMax6d = VectorMax6<double>;
/// @brief A dynamic size matrix with a fixed maximum size of 6 x 1
using VectorMax6b = VectorMax6<bool>;
/// @brief A dynamic size matrix with a fixed maximum size of 9 x 1
template <typename T> using VectorMax9 = Vector<T, Eigen::Dynamic, 9>;
/// @brief A dynamic size matrix with a fixed maximum size of 9 x 1
using VectorMax9d = VectorMax9<double>;
/// @brief A dynamic size matrix with a fixed maximum size of 12 x 1
template <typename T> using VectorMax12 = Vector<T, Eigen::Dynamic, 12>;
/// @brief A dynamic size matrix with a fixed maximum size of 12 x 1
using VectorMax12d = VectorMax12<double>;
template <typename T, int max_rows, int max_cols>
using MatrixMax = Eigen::Matrix<
    T,
    Eigen::Dynamic,
    Eigen::Dynamic,
    Eigen::ColMajor,
    max_rows,
    max_cols>;
/// @brief A dynamic size matrix with a fixed maximum size of 3 x 3
template <typename T> using MatrixMax2 = MatrixMax<T, 2, 2>;
/// @brief A dynamic size matrix with a fixed maximum size of 3 x 3
template <typename T> using MatrixMax3 = MatrixMax<T, 3, 3>;
/// @brief A dynamic size matrix with a fixed maximum size of 6 x 6
template <typename T> using MatrixMax6 = MatrixMax<T, 6, 6>;
/// @brief A dynamic size matrix with a fixed maximum size of 9 x 9
template <typename T> using MatrixMax9 = MatrixMax<T, 9, 9>;
/// @brief A dynamic size matrix with a fixed maximum size of 12 x 12
template <typename T> using MatrixMax12 = MatrixMax<T, 12, 12>;
/// @brief A dynamic size matrix with a fixed maximum size of 3 x 3
using MatrixMax2d = MatrixMax2<double>;
/// @brief A dynamic size matrix with a fixed maximum size of 3 x 3
using MatrixMax3d = MatrixMax3<double>;
/// @brief A dynamic size matrix with a fixed maximum size of 6 x 6
using MatrixMax6d = MatrixMax6<double>;
/// @brief A dynamic size matrix with a fixed maximum size of 12 x 12
using MatrixMax9d = MatrixMax9<double>;
/// @brief A dynamic size matrix with a fixed maximum size of 12 x 12
using MatrixMax12d = MatrixMax12<double>;
/// @brief A dynamic size diagonal matrix
using DiagonalMatrixXd = Eigen::DiagonalMatrix<double, Eigen::Dynamic>;
/// @brief A dynamic size diagonal matrix with a fixed maximum size of 6 x 6
using DiagonalMatrixMax6d = Eigen::DiagonalMatrix<double, Eigen::Dynamic, 6>;

/// @brief A dynamic size array with a fixed maximum size of 2 x 1
template <typename T>
using ArrayMax2 = Eigen::Array<T, Eigen::Dynamic, 1, Eigen::ColMajor, 2, 1>;
/// @brief A dynamic size array with a fixed maximum size of 2 x 1
template <typename T>
using ArrayMax3 = Eigen::Array<T, Eigen::Dynamic, 1, Eigen::ColMajor, 3, 1>;
/// @brief A dynamic size array with a fixed maximum size of 2 x 1
using ArrayMax2d = ArrayMax2<double>;
/// @brief A dynamic size array with a fixed maximum size of 3 x 1
using ArrayMax3d = ArrayMax3<double>;
/// @brief A dynamic size array with a fixed maximum size of 3 x 1
using ArrayMax3i = ArrayMax3<int>;

/// Matrix Projection onto Positive Definite Cone
template <
    typename _Scalar,
    int _Rows,
    int _Cols,
    int _Options,
    int _MaxRows,
    int _MaxCols>
Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>
project_to_pd(
    const Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>& A,
    double eps = 1e-8);
/// Matrix Projection onto Positive Semi-Definite Cone
template <
    typename _Scalar,
    int _Rows,
    int _Cols,
    int _Options,
    int _MaxRows,
    int _MaxCols>
Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>
project_to_psd(
    const Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>&
        A);

/// Cross product for dynamically sized matrices.
template <
    typename DerivedA,
    typename DerivedB,
    typename Result = Eigen::Matrix<
        typename DerivedA::Scalar,
        DerivedA::RowsAtCompileTime,
        DerivedA::ColsAtCompileTime>>
Result cross(
    const Eigen::MatrixBase<DerivedA>& a, const Eigen::MatrixBase<DerivedB>& b);

} // namespace ipc

#include "eigen_ext.tpp"


<|start_filename|>src/friction/friction.hpp<|end_filename|>
#pragma once

#include <Eigen/Core>
#include <Eigen/Sparse>

#include <ipc/collision_constraint.hpp>
#include <ipc/friction/friction_constraint.hpp>
#include <ipc/utils/eigen_ext.hpp>

namespace ipc {

void construct_friction_constraint_set(
    const Eigen::MatrixXd& V,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    const Constraints& contact_constraint_set,
    double dhat,
    double barrier_stiffness,
    double mu,
    FrictionConstraints& friction_constraint_set);

void construct_friction_constraint_set(
    const Eigen::MatrixXd& V,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    const Constraints& contact_constraint_set,
    double dhat,
    double barrier_stiffness,
    const Eigen::VectorXd& mus,
    FrictionConstraints& friction_constraint_set);

void construct_friction_constraint_set(
    const Eigen::MatrixXd& V,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    const Constraints& contact_constraint_set,
    double dhat,
    double barrier_stiffness,
    const Eigen::VectorXd& mus,
    const std::function<double(double, double)>& blend_mu,
    FrictionConstraints& friction_constraint_set);

/// @brief Compute the friction potential between to positions.
///
/// @param V0 Vertex positions at start of time-step (rowwise)
/// @param V1 Current vertex positions (rowwise)
/// @param E  Edge vertex indicies
/// @param F  Face vertex indicies (empty in 2D)
/// @param friction_constraint_set
/// @param epsv_times_h
template <typename T>
T compute_friction_potential(
    const Eigen::MatrixXd& V0,
    const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& V1,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    const FrictionConstraints& friction_constraint_set,
    double epsv_times_h);

Eigen::VectorXd compute_friction_potential_gradient(
    const Eigen::MatrixXd& V0,
    const Eigen::MatrixXd& V1,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    const FrictionConstraints& friction_constraint_set,
    double epsv_times_h);

Eigen::SparseMatrix<double> compute_friction_potential_hessian(
    const Eigen::MatrixXd& V0,
    const Eigen::MatrixXd& V1,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    const FrictionConstraints& friction_constraint_set,
    double epsv_times_h,
    bool project_hessian_to_psd = true);

} // namespace ipc

#include "friction.tpp"


<|start_filename|>src/distance/edge_edge_mollifier.cpp<|end_filename|>
#include <ipc/distance/edge_edge_mollifier.hpp>

namespace ipc {
namespace autogen {

    // This function was generated by the Symbolic Math Toolbox version 8.3.
    // 01-Nov-2019 16:54:23
    void edge_edge_cross_squarednorm_gradient(
        double v01,
        double v02,
        double v03,
        double v11,
        double v12,
        double v13,
        double v21,
        double v22,
        double v23,
        double v31,
        double v32,
        double v33,
        double g[12])
    {
        double t8, t9, t10, t11, t12, t13, t23, t24, t25, t26, t27, t28, t29,
            t30, t31, t32, t33;

        t8 = -v11 + v01;
        t9 = -v12 + v02;
        t10 = -v13 + v03;
        t11 = -v31 + v21;
        t12 = -v32 + v22;
        t13 = -v33 + v23;
        t23 = t8 * t12 + -(t9 * t11);
        t24 = t8 * t13 + -(t10 * t11);
        t25 = t9 * t13 + -(t10 * t12);
        t26 = t8 * t23 * 2.0;
        t27 = t9 * t23 * 2.0;
        t28 = t8 * t24 * 2.0;
        t29 = t10 * t24 * 2.0;
        t30 = t9 * t25 * 2.0;
        t31 = t10 * t25 * 2.0;
        t32 = t11 * t23 * 2.0;
        t33 = t12 * t23 * 2.0;
        t23 = t11 * t24 * 2.0;
        t10 = t13 * t24 * 2.0;
        t9 = t12 * t25 * 2.0;
        t8 = t13 * t25 * 2.0;
        g[0] = t33 + t10;
        g[1] = -t32 + t8;
        g[2] = -t23 - t9;
        g[3] = -t33 - t10;
        g[4] = t32 - t8;
        g[5] = t23 + t9;
        g[6] = -t27 - t29;
        g[7] = t26 - t31;
        g[8] = t28 + t30;
        g[9] = t27 + t29;
        g[10] = -t26 + t31;
        g[11] = -t28 - t30;
    }

    // This function was generated by the Symbolic Math Toolbox version 8.3.
    // 01-Nov-2019 16:54:23
    void edge_edge_cross_squarednorm_hessian(
        double v01,
        double v02,
        double v03,
        double v11,
        double v12,
        double v13,
        double v21,
        double v22,
        double v23,
        double v31,
        double v32,
        double v33,
        double H[144])
    {
        double t8, t9, t10, t11, t12, t13, t32, t33, t34, t35, t48, t36, t49,
            t37, t38, t39, t40, t41, t42, t43, t44, t45, t46, t47, t50, t51,
            t52, t20, t23, t24, t25, t86, t87, t88, t74, t75, t76, t77, t78,
            t79, t89, t90, t91, t92, t93, t94, t95;

        t8 = -v11 + v01;
        t9 = -v12 + v02;
        t10 = -v13 + v03;
        t11 = -v31 + v21;
        t12 = -v32 + v22;
        t13 = -v33 + v23;
        t32 = t8 * t9 * 2.0;
        t33 = t8 * t10 * 2.0;
        t34 = t9 * t10 * 2.0;
        t35 = t8 * t11 * 2.0;
        t48 = t8 * t12;
        t36 = t48 * 2.0;
        t49 = t9 * t11;
        t37 = t49 * 2.0;
        t38 = t48 * 4.0;
        t48 = t8 * t13;
        t39 = t48 * 2.0;
        t40 = t49 * 4.0;
        t41 = t9 * t12 * 2.0;
        t49 = t10 * t11;
        t42 = t49 * 2.0;
        t43 = t48 * 4.0;
        t48 = t9 * t13;
        t44 = t48 * 2.0;
        t45 = t49 * 4.0;
        t49 = t10 * t12;
        t46 = t49 * 2.0;
        t47 = t48 * 4.0;
        t48 = t49 * 4.0;
        t49 = t10 * t13 * 2.0;
        t50 = t11 * t12 * 2.0;
        t51 = t11 * t13 * 2.0;
        t52 = t12 * t13 * 2.0;
        t20 = t8 * t8 * 2.0;
        t9 = t9 * t9 * 2.0;
        t8 = t10 * t10 * 2.0;
        t23 = t11 * t11 * 2.0;
        t24 = t12 * t12 * 2.0;
        t25 = t13 * t13 * 2.0;
        t86 = t35 + t41;
        t87 = t35 + t49;
        t88 = t41 + t49;
        t74 = t20 + t9;
        t75 = t20 + t8;
        t76 = t9 + t8;
        t77 = t23 + t24;
        t78 = t23 + t25;
        t79 = t24 + t25;
        t89 = t40 + -t36;
        t90 = t36 + -t40;
        t91 = t37 + -t38;
        t92 = t38 + -t37;
        t93 = t45 + -t39;
        t94 = t39 + -t45;
        t95 = t42 + -t43;
        t37 = t43 + -t42;
        t39 = t48 + -t44;
        t45 = t44 + -t48;
        t38 = t46 + -t47;
        t40 = t47 + -t46;
        t36 = -t35 + -t41;
        t13 = -t35 + -t49;
        t11 = -t41 + -t49;
        t12 = -t20 + -t9;
        t10 = -t20 + -t8;
        t8 = -t9 + -t8;
        t9 = -t23 + -t24;
        t49 = -t23 + -t25;
        t48 = -t24 + -t25;
        H[0] = t79;
        H[1] = -t50;
        H[2] = -t51;
        H[3] = t48;
        H[4] = t50;
        H[5] = t51;
        H[6] = t11;
        H[7] = t92;
        H[8] = t37;
        H[9] = t88;
        H[10] = t91;
        H[11] = t95;
        H[12] = -t50;
        H[13] = t78;
        H[14] = -t52;
        H[15] = t50;
        H[16] = t49;
        H[17] = t52;
        H[18] = t89;
        H[19] = t13;
        H[20] = t40;
        H[21] = t90;
        H[22] = t87;
        H[23] = t38;
        H[24] = -t51;
        H[25] = -t52;
        H[26] = t77;
        H[27] = t51;
        H[28] = t52;
        H[29] = t9;
        H[30] = t93;
        H[31] = t39;
        H[32] = t36;
        H[33] = t94;
        H[34] = t45;
        H[35] = t86;
        H[36] = t48;
        H[37] = t50;
        H[38] = t51;
        H[39] = t79;
        H[40] = -t50;
        H[41] = -t51;
        H[42] = t88;
        H[43] = t91;
        H[44] = t95;
        H[45] = t11;
        H[46] = t92;
        H[47] = t37;
        H[48] = t50;
        H[49] = t49;
        H[50] = t52;
        H[51] = -t50;
        H[52] = t78;
        H[53] = -t52;
        H[54] = t90;
        H[55] = t87;
        H[56] = t38;
        H[57] = t89;
        H[58] = t13;
        H[59] = t40;
        H[60] = t51;
        H[61] = t52;
        H[62] = t9;
        H[63] = -t51;
        H[64] = -t52;
        H[65] = t77;
        H[66] = t94;
        H[67] = t45;
        H[68] = t86;
        H[69] = t93;
        H[70] = t39;
        H[71] = t36;
        H[72] = t11;
        H[73] = t89;
        H[74] = t93;
        H[75] = t88;
        H[76] = t90;
        H[77] = t94;
        H[78] = t76;
        H[79] = -t32;
        H[80] = -t33;
        H[81] = t8;
        H[82] = t32;
        H[83] = t33;
        H[84] = t92;
        H[85] = t13;
        H[86] = t39;
        H[87] = t91;
        H[88] = t87;
        H[89] = t45;
        H[90] = -t32;
        H[91] = t75;
        H[92] = -t34;
        H[93] = t32;
        H[94] = t10;
        H[95] = t34;
        H[96] = t37;
        H[97] = t40;
        H[98] = t36;
        H[99] = t95;
        H[100] = t38;
        H[101] = t86;
        H[102] = -t33;
        H[103] = -t34;
        H[104] = t74;
        H[105] = t33;
        H[106] = t34;
        H[107] = t12;
        H[108] = t88;
        H[109] = t90;
        H[110] = t94;
        H[111] = t11;
        H[112] = t89;
        H[113] = t93;
        H[114] = t8;
        H[115] = t32;
        H[116] = t33;
        H[117] = t76;
        H[118] = -t32;
        H[119] = -t33;
        H[120] = t91;
        H[121] = t87;
        H[122] = t45;
        H[123] = t92;
        H[124] = t13;
        H[125] = t39;
        H[126] = t32;
        H[127] = t10;
        H[128] = t34;
        H[129] = -t32;
        H[130] = t75;
        H[131] = -t34;
        H[132] = t95;
        H[133] = t38;
        H[134] = t86;
        H[135] = t37;
        H[136] = t40;
        H[137] = t36;
        H[138] = t33;
        H[139] = t34;
        H[140] = t12;
        H[141] = -t33;
        H[142] = -t34;
        H[143] = t74;
    }

} // namespace autogen
} // namespace ipc


<|start_filename|>src/barrier/adaptive_stiffness.cpp<|end_filename|>
// Fucntions for computing the initial and updated barrier stiffnesses.

#include <ipc/barrier/adaptive_stiffness.hpp>

#include <algorithm> // std::min/max

#include <ipc/ipc.hpp>
#include <ipc/barrier/barrier.hpp>
#include <ipc/utils/world_bbox_diagonal_length.hpp>

namespace ipc {

double initial_barrier_stiffness(
    double bbox_diagonal,
    double dhat,
    double average_mass,
    const Eigen::VectorXd& grad_energy,
    const Eigen::VectorXd& grad_barrier,
    double& max_barrier_stiffness,
    double min_barrier_stiffness_scale,
    double dmin)
{
    assert(average_mass > 0 && min_barrier_stiffness_scale > 0);
    assert(bbox_diagonal > 0);

    double dhat_squared = dhat * dhat;
    double dmin_squared = dmin * dmin;

    // Find a good initial value for κ
    double d0 = 1e-8 * bbox_diagonal + dmin;
    d0 *= d0;
    if (d0 - dmin_squared >= 2 * dmin * dhat + dhat_squared) {
        d0 = dmin * dhat + 0.5 * dhat_squared; // NOTE: this is untested
    }
    double min_barrier_stiffness = 4 * d0
        * barrier_hessian(d0 - dmin_squared, 2 * dmin * dhat + dhat_squared);
    min_barrier_stiffness =
        min_barrier_stiffness_scale * average_mass / min_barrier_stiffness;
    assert(std::isfinite(min_barrier_stiffness));

    max_barrier_stiffness = 100 * min_barrier_stiffness;

    double kappa = 1.0;
    if (grad_barrier.squaredNorm() > 0) {
        // If this value is negative it will be clamped to κ_min anyways
        kappa = -grad_barrier.dot(grad_energy) / grad_barrier.squaredNorm();
        assert(std::isfinite(kappa));
    }

    return std::min(
        max_barrier_stiffness, std::max(min_barrier_stiffness, kappa));
}

// Adaptive κ
void update_barrier_stiffness(
    double prev_min_distance,
    double min_distance,
    double max_barrier_stiffness,
    double& barrier_stiffness,
    double bbox_diagonal,
    double dhat_epsilon_scale,
    double dmin)
{
    // Is the barrier having a difficulty pushing the bodies apart?
    double dhat_epsilon = dhat_epsilon_scale * (bbox_diagonal + dmin);
    dhat_epsilon *= dhat_epsilon;
    if (prev_min_distance < dhat_epsilon && min_distance < dhat_epsilon
        && min_distance < prev_min_distance) {
        // Then increase the barrier stiffness.
        barrier_stiffness =
            std::min(max_barrier_stiffness, 2 * barrier_stiffness);
    }
}

// Adaptive κ
void update_barrier_stiffness(
    const Constraints& constraint_set,
    const Eigen::MatrixXd& V,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    double prev_min_distance,
    double& min_distance,
    double max_barrier_stiffness,
    double& barrier_stiffness,
    double dhat_epsilon_scale,
    double dmin)
{
    // Use a temporay variable in case &prev_min_distance == &min_distance
    double current_min_distance =
        compute_minimum_distance(V, E, F, constraint_set);

    update_barrier_stiffness(
        prev_min_distance, current_min_distance, max_barrier_stiffness,
        barrier_stiffness, world_bbox_diagonal_length(V), dhat_epsilon_scale,
        dmin);

    min_distance = current_min_distance;
}

} // namespace ipc


<|start_filename|>src/barrier/barrier.tpp<|end_filename|>
#pragma once
#include <ipc/barrier/barrier.hpp>

namespace ipc {

template <typename T> T barrier(const T& d, double dhat)
{
    if (d <= T(0)) {
        return T(std::numeric_limits<double>::infinity());
    }
    if (d >= dhat) {
        return T(0);
    }
    // b(d) = -(d-d̂)²ln(d / d̂)
    T dhat_T = T(dhat);
    return -(d - dhat_T) * (d - dhat_T) * log(d / dhat_T);
}

} // namespace ipc


<|start_filename|>python/src/utils.hpp<|end_filename|>
#include <pybind11/pybind11.h>
#include <pybind11/eigen.h>

#include <Eigen/Core>

template <typename Derived>
void assert_2D_or_3D_vector(
    const Eigen::MatrixBase<Derived>& v, const std::string& name)
{
    if ((v.size() != 2 && v.size() != 3) || (v.rows() != 1 && v.cols() != 1)) {
        throw pybind11::value_error(
            "Parameter " + name + " has invalid size: expected " + name
            + " to be a 2D or 3D vector but got " + name + ".shape = ["
            + std::to_string(v.rows()) + ", " + std::to_string(v.cols()) + "]");
    }
}

template <typename Derived>
void assert_3D_vector(
    const Eigen::MatrixBase<Derived>& v, const std::string& name)
{
    if (v.size() != 3 || (v.rows() != 1 && v.cols() != 1)) {
        throw pybind11::value_error(
            "Parameter " + name + " has invalid size: expected " + name
            + " to be a 3D vector but got " + name + ".shape = ["
            + std::to_string(v.rows()) + ", " + std::to_string(v.cols()) + "]");
    }
}

template <typename DerivedV, typename DerivedVCopy>
inline void copy_vector(
    const Eigen::MatrixBase<DerivedV>& v,
    Eigen::MatrixBase<DerivedVCopy>& v_copy)
{
    if (v.cols() == 1) {
        v_copy = v;
    } else {
        v_copy = v.transpose();
    }
}


<|start_filename|>tests/test_utils.cpp<|end_filename|>
#include "test_utils.hpp"

#include <igl/dirname.h>
#include <igl/edges.h>
#include <igl/read_triangle_mesh.h>

#include <ipc/utils/eigen_ext.hpp>

// Flatten the matrix rowwise
Eigen::VectorXd flatten(const Eigen::MatrixXd& X)
{
    Eigen::MatrixXd XT = X.transpose();
    return Eigen::VectorXd(Eigen::Map<Eigen::VectorXd>(XT.data(), XT.size()));
}

/// Unflatten rowwise
Eigen::MatrixXd unflatten(const Eigen::VectorXd& x, int dim)
{
    assert(x.size() % dim == 0);
    Eigen::MatrixXd unflat_x(x.size() / dim, dim);
    for (int i = 0; i < x.size(); i++) {
        unflat_x(i / dim, i % dim) = x(i);
    }
    return unflat_x;
}

TEST_CASE("Flatten and unflatten", "[utils]")
{
    Eigen::MatrixXd X = Eigen::MatrixXd::Random(1000, 3);
    Eigen::MatrixXd R = unflatten(flatten(X), X.cols());
    CHECK(X == R);
}

bool load_mesh(
    const std::string& mesh_name,
    Eigen::MatrixXd& V,
    Eigen::MatrixXi& E,
    Eigen::MatrixXi& F)
{
    bool success = igl::read_triangle_mesh(TEST_DATA_DIR + mesh_name, V, F);
    if (F.size()) {
        igl::edges(F, E);
    }
    return success && V.size() && F.size() && E.size();
}

void mmcvids_to_constraints(
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    const Eigen::MatrixXi& mmcvids,
    ipc::Constraints& constraints)
{
    for (int mmcvid_i = 0; mmcvid_i < mmcvids.rows(); mmcvid_i++) {
        const auto mmcvid = mmcvids.row(mmcvid_i);

        if (mmcvid[0] >= 0) { // Is EE?
            int ei, ej;
            // Find the edge index
            for (ei = 0; ei < E.rows(); ei++) {
                if (E(ei, 0) == mmcvid[0] && E(ei, 1) == mmcvid[1]) {
                    break;
                }
            }
            // Find the edge index
            for (ej = 0; ej < E.rows(); ej++) {
                if (E(ej, 0) == mmcvid[2] && E(ej, 1) == mmcvid[3]) {
                    break;
                }
            }
            assert(ei < E.rows() && ej < E.rows());
            constraints.ee_constraints.emplace_back(ei, ej, 0.0);
        } else {
            if (mmcvid[2] < 0) { // Is VV?
                constraints.vv_constraints.emplace_back(
                    -mmcvid[0] - 1, mmcvid[1]);
                assert(-mmcvid[3] >= 1);
                constraints.vv_constraints.back().multiplicity = -mmcvid[3];

            } else if (mmcvid[3] < 0) { // Is EV?
                int ei;
                for (ei = 0; ei < E.rows(); ei++) {
                    if (E(ei, 0) == mmcvid[1] && E(ei, 1) == mmcvid[2]) {
                        break;
                    }
                }
                assert(ei < E.rows());
                constraints.ev_constraints.emplace_back(ei, -mmcvid[0] - 1);
                constraints.ev_constraints.back().multiplicity = -mmcvid[3];

            } else { // Is FV.
                int fi;
                for (fi = 0; fi < F.rows(); fi++) {
                    if (F(fi, 0) == mmcvid[1] && F(fi, 1) == mmcvid[2]
                        && F(fi, 2) == mmcvid[3]) {
                        break;
                    }
                }
                assert(fi < F.rows());
                constraints.fv_constraints.emplace_back(fi, -mmcvid[0] - 1);
            }
        }
    }
}

// Attempts to move the generator to the next element.
// Returns true if successful (and thus has another element that can be
// read)
bool RotationGenerator::next()
{
    double angle = ipc::Vector1d::Random()[0];
    R = Eigen::AngleAxisd(angle, Eigen::Vector3d::Random().normalized());
    return true;
}

// Precondition:
// The generator is either freshly constructed or the last call to next()
// returned true
Eigen::Matrix3d const& RotationGenerator::get() const { return R; }

Catch::Generators::GeneratorWrapper<Eigen::Matrix3d> RotationGenerator::create()
{
    return Catch::Generators::GeneratorWrapper<Eigen::Matrix3d>(
        std::unique_ptr<RotationGenerator>(new RotationGenerator()));
}


<|start_filename|>tests/distance/test_edge_edge.cpp<|end_filename|>
#include <catch2/catch.hpp>

#include <finitediff.hpp>
#include <igl/PI.h>

#include <ipc/distance/edge_edge.hpp>
#include <ipc/utils/eigen_ext.hpp>

using namespace ipc;

TEST_CASE("Edge-edge distance", "[distance][edge-edge]")
{
    double e0y = GENERATE(-10, -1, -1e-4, 0, 1e-4, 1, 10);
    Eigen::Vector3d e00(-1, e0y, 0);
    Eigen::Vector3d e01(1, e0y, 0);
    Eigen::Vector3d e10(0, 0, -1);
    Eigen::Vector3d e11(0, 0, 1);

    Eigen::Vector3d e0_closest, e1_closest;
    double shiftx = GENERATE(-2, 0, 2);
    double shiftz = GENERATE(-2, 0, 2);
    double e0x = shiftx + GENERATE(-1, -0.5, 0, 0.5, 1);
    double e0z = shiftz + GENERATE(-1, -0.5, 0, 0.5, 1);
    e00.x() += e0x;
    e01.x() += e0x;
    e00.z() += e0z;
    e01.z() += e0z;
    e0_closest =
        shiftx > 1 ? e00 : (shiftx < -1 ? e01 : Eigen::Vector3d(0, e0y, e0z));
    e1_closest =
        shiftz > 1 ? e11 : (shiftz < -1 ? e10 : Eigen::Vector3d(0, 0, e0z));

    double distance = edge_edge_distance(e00, e01, e10, e11);
    double expected_distance = point_point_distance(e0_closest, e1_closest);

    CAPTURE(e0x, e0y, e0z, edge_edge_distance_type(e00, e01, e10, e11));
    CHECK(distance == Approx(expected_distance).margin(1e-12));
}

TEST_CASE("Edge-edge distance degenerate case", "[distance][edge-edge]")
{
    double e0y = GENERATE(-10, -1, -1e-4, 0, 1e-4, 1, 10);
    Eigen::Vector3d e00(-1, e0y, 0);
    Eigen::Vector3d e01(1, e0y, 0);
    Eigen::Vector3d e10(0, 0, -1);
    Eigen::Vector3d e11(0, 0, 1);

    double theta =
        GENERATE(-2, -1.5, -1, -0.123124, 0, 0.2342352, 0.5, 1, 1.5, 2, 50, 51)
        * igl::PI;
    Eigen::Matrix3d R =
        Eigen::AngleAxisd(theta, Eigen::Vector3d::UnitY()).toRotationMatrix();

    SECTION("e0 rotating")
    {
        e00 = R * e00;
        e01 = R * e01;
    }
    SECTION("e1 rotating")
    {
        e10 = R * e10;
        e11 = R * e11;
    }

    double distance = edge_edge_distance(e00, e01, e10, e11);
    CHECK(distance == Approx(e0y * e0y).margin(1e-12));
}

TEST_CASE(
    "Edge-edge distance degenerate case not overlapping",
    "[distance][edge-edge]")
{
    double e0y = GENERATE(-10, -1, -1e-4, 0, 1e-4, 1, 10);
    double gap = GENERATE(0, 0.01, 0.1, 1);
    Eigen::Vector3d e00(gap, e0y, 0);
    Eigen::Vector3d e01(1, e0y, 0);
    Eigen::Vector3d e10(-1, 0, 0);
    Eigen::Vector3d e11(-gap, 0, 0);

    SECTION("original order") {}
    SECTION("swap e0") { std::swap(e00, e01); }
    SECTION("swap e1") { std::swap(e10, e11); }
    SECTION("swap e0 and e1")
    {
        std::swap(e00, e01);
        std::swap(e10, e11);
    }

    double distance = edge_edge_distance(e00, e01, e10, e11);
    double expected_distance = point_point_distance(
        Eigen::Vector3d(gap, e0y, 0), Eigen::Vector3d(-gap, 0, 0));

    CHECK(distance == Approx(expected_distance).margin(1e-12));
}

TEST_CASE("Edge-edge distance gradient", "[distance][edge-edge][gradient]")
{
    double e0y = GENERATE(-10, -1, -1e-4, 0, 1e-4, 1, 10);
    Eigen::Vector3d e00(-1, e0y, 0);
    Eigen::Vector3d e01(1, e0y, 0);
    Eigen::Vector3d e10(0, 0, -1);
    Eigen::Vector3d e11(0, 0, 1);

    Eigen::Vector3d e0_closest, e1_closest;
    double shiftx = GENERATE(-2, 0, 2);
    double shiftz = GENERATE(-2, 0, 2);
    double e0x = shiftx + GENERATE(-1, -0.5, 0, 0.5, 1);
    double e0z = shiftz + GENERATE(-1, -0.5, 0, 0.5, 1);
    e00.x() += e0x;
    e01.x() += e0x;
    e00.z() += e0z;
    e01.z() += e0z;
    e0_closest =
        shiftx > 1 ? e00 : (shiftx < -1 ? e01 : Eigen::Vector3d(0, e0y, e0z));
    e1_closest =
        shiftz > 1 ? e11 : (shiftz < -1 ? e10 : Eigen::Vector3d(0, 0, e0z));

    Eigen::Matrix<double, 12, 1> grad;
    edge_edge_distance_gradient(e00, e01, e10, e11, grad);

    // Compute the gradient using finite differences
    Eigen::VectorXd x(12);
    x.segment<3>(0) = e00;
    x.segment<3>(3) = e01;
    x.segment<3>(6) = e10;
    x.segment<3>(9) = e11;
    auto f = [](const Eigen::VectorXd& x) {
        return edge_edge_distance(
            x.segment<3>(0), x.segment<3>(3), x.segment<3>(6), x.segment<3>(9));
    };
    Eigen::VectorXd fgrad;
    fd::finite_gradient(x, f, fgrad);

    CAPTURE(e0x, e0y, e0z, edge_edge_distance_type(e00, e01, e10, e11));
    CHECK(fd::compare_gradient(grad, fgrad));
}

TEST_CASE(
    "Parallel edge-edge distance gradient", "[distance][edge-edge][gradient]")
{
    // Generate a geometric space of
    double angle = 0;
    SECTION("Almost parallel")
    {
        double exponent = GENERATE(range(-6, 3));
        angle = pow(10, exponent) * igl::PI / 180.0;
    }
    // SECTION("Parallel") { angle = 0; }

    Eigen::Vector3d e00(-1.0, 0, 0), e01(1.0, 0, 0),
        e10(cos(angle), 1, sin(angle)),
        e11(cos(angle + igl::PI), 1, sin(angle + igl::PI));

    double distance = edge_edge_distance(e00, e01, e10, e11);
    Eigen::VectorXd grad;
    edge_edge_distance_gradient(e00, e01, e10, e11, grad);

    // Compute the gradient using finite differences
    Eigen::VectorXd x(12);
    x.segment<3>(0) = e00;
    x.segment<3>(3) = e01;
    x.segment<3>(6) = e10;
    x.segment<3>(9) = e11;
    auto f = [](const Eigen::VectorXd& x) {
        return edge_edge_distance(
            x.segment<3>(0), x.segment<3>(3), x.segment<3>(6), x.segment<3>(9));
    };
    Eigen::VectorXd fgrad;
    fd::finite_gradient(x, f, fgrad);

    CAPTURE(angle, (grad - fgrad).squaredNorm());
    CHECK(distance == Approx(1.0));
    CHECK(fd::compare_gradient(grad, fgrad));
    // CHECK(distance.getHessian().squaredNorm() != Approx(0.0));
}


<|start_filename|>src/distance/edge_edge_mollifier.hpp<|end_filename|>
#pragma once

#include <ipc/distance/distance_type.hpp>
#include <ipc/distance/line_line.hpp>
#include <ipc/distance/point_edge.hpp>
#include <ipc/distance/point_point.hpp>

namespace ipc {

/// @brief Compute the squared norm of the edge-edge cross product.
/// @param ea0 The first vertex of the first edge.
/// @param ea1 The second vertex of the first edge.
/// @param eb0 The first vertex of the second edge.
/// @param eb1 The second vertex of the second edge.
/// @return The squared norm of the edge-edge cross product.
template <
    typename DerivedEA0,
    typename DerivedEA1,
    typename DerivedEB0,
    typename DerivedEB1>
auto edge_edge_cross_squarednorm(
    const Eigen::MatrixBase<DerivedEA0>& ea0,
    const Eigen::MatrixBase<DerivedEA1>& ea1,
    const Eigen::MatrixBase<DerivedEB0>& eb0,
    const Eigen::MatrixBase<DerivedEB1>& eb1)
{
    assert(ea0.size() == 3);
    assert(ea1.size() == 3);
    assert(eb0.size() == 3);
    assert(eb1.size() == 3);

    return cross(ea1 - ea0, eb1 - eb0).squaredNorm();
}

// Symbolically generated derivatives;
namespace autogen {
    void edge_edge_cross_squarednorm_gradient(
        double v01,
        double v02,
        double v03,
        double v11,
        double v12,
        double v13,
        double v21,
        double v22,
        double v23,
        double v31,
        double v32,
        double v33,
        double g[12]);

    void edge_edge_cross_squarednorm_hessian(
        double v01,
        double v02,
        double v03,
        double v11,
        double v12,
        double v13,
        double v21,
        double v22,
        double v23,
        double v31,
        double v32,
        double v33,
        double H[144]);
} // namespace autogen

/// @brief Compute the gradient of the squared norm of the edge cross product.
/// @param[in] ea0 The first vertex of the first edge.
/// @param[in] ea1 The second vertex of the first edge.
/// @param[in] eb0 The first vertex of the second edge.
/// @param[in] eb1 The second vertex of the second edge.
/// @param[out] grad The gradient of the squared norm of the edge cross product
///                  wrt ea0, ea1, eb0, and eb1.
template <
    typename DerivedEA0,
    typename DerivedEA1,
    typename DerivedEB0,
    typename DerivedEB1,
    typename DerivedGrad>
void edge_edge_cross_squarednorm_gradient(
    const Eigen::MatrixBase<DerivedEA0>& ea0,
    const Eigen::MatrixBase<DerivedEA1>& ea1,
    const Eigen::MatrixBase<DerivedEB0>& eb0,
    const Eigen::MatrixBase<DerivedEB1>& eb1,
    Eigen::PlainObjectBase<DerivedGrad>& grad)
{
    assert(ea0.size() == 3);
    assert(ea1.size() == 3);
    assert(eb0.size() == 3);
    assert(eb1.size() == 3);

    grad.resize(ea0.size() + ea1.size() + eb0.size() + eb1.size());
    autogen::edge_edge_cross_squarednorm_gradient(
        ea0[0], ea0[1], ea0[2], ea1[0], ea1[1], ea1[2], eb0[0], eb0[1], eb0[2],
        eb1[0], eb1[1], eb1[2], grad.data());
}

/// @brief Compute the hessian of the squared norm of the edge cross product.
/// @param ea0 The first vertex of the first edge.
/// @param ea1 The second vertex of the first edge.
/// @param eb0 The first vertex of the second edge.
/// @param eb1 The second vertex of the second edge.
/// @return The squared norm of the edge-edge cross product.
/// @param[out] hess The hessian of the squared norm of the edge cross product
///                  wrt ea0, ea1, eb0, and eb1.
template <
    typename DerivedEA0,
    typename DerivedEA1,
    typename DerivedEB0,
    typename DerivedEB1,
    typename DerivedHess>
void edge_edge_cross_squarednorm_hessian(
    const Eigen::MatrixBase<DerivedEA0>& ea0,
    const Eigen::MatrixBase<DerivedEA1>& ea1,
    const Eigen::MatrixBase<DerivedEB0>& eb0,
    const Eigen::MatrixBase<DerivedEB1>& eb1,
    Eigen::PlainObjectBase<DerivedHess>& hess)
{
    assert(ea0.size() == 3);
    assert(ea1.size() == 3);
    assert(eb0.size() == 3);
    assert(eb1.size() == 3);

    hess.resize(
        ea0.size() + ea1.size() + eb0.size() + eb1.size(),
        ea0.size() + ea1.size() + eb0.size() + eb1.size());
    autogen::edge_edge_cross_squarednorm_hessian(
        ea0[0], ea0[1], ea0[2], ea1[0], ea1[1], ea1[2], eb0[0], eb0[1], eb0[2],
        eb1[0], eb1[1], eb1[2], hess.data());
}

/// @brief Mollifier function for edge-edge distance.
/// @param x Squared norm of the edge-edge cross product.
/// @param eps_x Mollifier activation threshold.
/// @return The mollifier coefficient to premultiply the edge-edge distance.
template <typename T> T edge_edge_mollifier(const T& x, double eps_x)
{
    T x_div_eps_x = x / eps_x;
    return (-x_div_eps_x + 2.0) * x_div_eps_x;
}

/// @brief The gradient of the mollifier function for edge-edge distance.
/// @param x Squared norm of the edge-edge cross product.
/// @param eps_x Mollifier activation threshold.
/// @return The gradient of the mollifier function for edge-edge distance wrt x.
template <typename T> T edge_edge_mollifier_gradient(const T& x, double eps_x)
{
    T one_div_eps_x = 1.0 / eps_x;
    return 2.0 * one_div_eps_x * (-one_div_eps_x * x + 1.0);
}

/// @brief The hessian of the mollifier function for edge-edge distance.
/// @param x Squared norm of the edge-edge cross product.
/// @param eps_x Mollifier activation threshold.
/// @return The hessian of the mollifier function for edge-edge distance wrt x.
template <typename T> T edge_edge_mollifier_hessian(const T& x, double eps_x)
{
    return -2.0 / (eps_x * eps_x);
}

/// @brief Compute a mollifier for the edge-edge distance.
///
/// This helps smooth the non-smoothness at close to parallel edges.
///
/// @param ea0 The first vertex of the first edge.
/// @param ea1 The second vertex of the first edge.
/// @param eb0 The first vertex of the second edge.
/// @param eb1 The second vertex of the second edge.
/// @param eps_x Mollifier activation threshold.
/// @return The mollifier coefficient to premultiply the edge-edge distance.
template <
    typename DerivedEA0,
    typename DerivedEA1,
    typename DerivedEB0,
    typename DerivedEB1>
inline auto edge_edge_mollifier(
    const Eigen::MatrixBase<DerivedEA0>& ea0,
    const Eigen::MatrixBase<DerivedEA1>& ea1,
    const Eigen::MatrixBase<DerivedEB0>& eb0,
    const Eigen::MatrixBase<DerivedEB1>& eb1,
    const double eps_x)
{
    assert(ea0.size() == 3);
    assert(ea1.size() == 3);
    assert(eb0.size() == 3);
    assert(eb1.size() == 3);

    auto ee_cross_norm_sqr = edge_edge_cross_squarednorm(ea0, ea1, eb0, eb1);
    if (ee_cross_norm_sqr < eps_x) {
        return edge_edge_mollifier(ee_cross_norm_sqr, eps_x);
    } else {
        return decltype(ee_cross_norm_sqr)(1.0);
    }
}

/// @brief Compute the gradient of the mollifier for the edge-edge distance.
/// @param[in] ea0 The first vertex of the first edge.
/// @param[in] ea1 The second vertex of the first edge.
/// @param[in] eb0 The first vertex of the second edge.
/// @param[in] eb1 The second vertex of the second edge.
/// @param eps_x[in] Mollifier activation threshold.
/// @param[out] grad The gradient of the mollifier.
template <
    typename DerivedEA0,
    typename DerivedEA1,
    typename DerivedEB0,
    typename DerivedEB1,
    typename DerivedGrad>
void edge_edge_mollifier_gradient(
    const Eigen::MatrixBase<DerivedEA0>& ea0,
    const Eigen::MatrixBase<DerivedEA1>& ea1,
    const Eigen::MatrixBase<DerivedEB0>& eb0,
    const Eigen::MatrixBase<DerivedEB1>& eb1,
    const double eps_x,
    Eigen::PlainObjectBase<DerivedGrad>& grad)
{
    int dim = ea0.size();
    assert(ea1.size() == dim);
    assert(eb0.size() == dim);
    assert(eb1.size() == dim);

    grad.resize(4 * dim);

    auto ee_cross_norm_sqr = edge_edge_cross_squarednorm(ea0, ea1, eb0, eb1);
    if (ee_cross_norm_sqr < eps_x) {
        edge_edge_cross_squarednorm_gradient(ea0, ea1, eb0, eb1, grad);
        grad *= edge_edge_mollifier_gradient(ee_cross_norm_sqr, eps_x);
    } else {
        grad.setZero();
    }
}

/// @brief Compute the hessian of the mollifier for the edge-edge distance.
/// @param[in] ea0 The first vertex of the first edge.
/// @param[in] ea1 The second vertex of the first edge.
/// @param[in] eb0 The first vertex of the second edge.
/// @param[in] eb1 The second vertex of the second edge.
/// @param eps_x[in] Mollifier activation threshold.
/// @param[out] hess The hessian of the mollifier.
template <
    typename DerivedEA0,
    typename DerivedEA1,
    typename DerivedEB0,
    typename DerivedEB1,
    typename DerivedHess>
void edge_edge_mollifier_hessian(
    const Eigen::MatrixBase<DerivedEA0>& ea0,
    const Eigen::MatrixBase<DerivedEA1>& ea1,
    const Eigen::MatrixBase<DerivedEB0>& eb0,
    const Eigen::MatrixBase<DerivedEB1>& eb1,
    const double eps_x,
    Eigen::PlainObjectBase<DerivedHess>& hess)
{
    int dim = ea0.size();
    assert(ea1.size() == dim);
    assert(eb0.size() == dim);
    assert(eb1.size() == dim);

    hess.resize(4 * dim, 4 * dim);

    auto ee_cross_norm_sqr = edge_edge_cross_squarednorm(ea0, ea1, eb0, eb1);
    if (ee_cross_norm_sqr < eps_x) {
        edge_edge_cross_squarednorm_hessian(ea0, ea1, eb0, eb1, hess);
        hess *= edge_edge_mollifier_gradient(ee_cross_norm_sqr, eps_x);

        Eigen::Matrix<typename DerivedHess::Scalar, Eigen::Dynamic, 1> grad(
            4 * dim);
        edge_edge_cross_squarednorm_gradient(ea0, ea1, eb0, eb1, grad);

        hess += (edge_edge_mollifier_hessian(ee_cross_norm_sqr, eps_x) * grad)
            * grad.transpose();
    } else {
        hess.setZero();
    }
}

/// @brief Compute the threshold of the mollifier edge-edge distance.
///
/// This values is computed based on the edges at rest length.
///
/// @param ea0_rest The rest position of the first vertex of the first edge.
/// @param ea1_rest The rest position of the second vertex of the first edge.
/// @param eb0_rest The rest position of the first vertex of the second edge.
/// @param eb1_rest The rest position of the second vertex of the second edge.
/// @return Threshold for edge-edge mollification.
template <
    typename DerivedEA0,
    typename DerivedEA1,
    typename DerivedEB0,
    typename DerivedEB1>
double edge_edge_mollifier_threshold(
    const Eigen::MatrixBase<DerivedEA0>& ea0_rest,
    const Eigen::MatrixBase<DerivedEA1>& ea1_rest,
    const Eigen::MatrixBase<DerivedEB0>& eb0_rest,
    const Eigen::MatrixBase<DerivedEB1>& eb1_rest)
{
    assert(ea0_rest.size() == 3);
    assert(ea1_rest.size() == 3);
    assert(eb0_rest.size() == 3);
    assert(eb1_rest.size() == 3);

    return 1.0e-3 * (ea0_rest - ea1_rest).squaredNorm()
        * (eb0_rest - eb1_rest).squaredNorm();
}

} // namespace ipc


<|start_filename|>src/ccd/inexact_point_edge.hpp<|end_filename|>
#pragma once

#include <Eigen/Core>

namespace ipc {

bool inexact_point_edge_ccd_2D(
    const Eigen::Vector2d& p_t0,
    const Eigen::Vector2d& e0_t0,
    const Eigen::Vector2d& e1_t0,
    const Eigen::Vector2d& p_t1,
    const Eigen::Vector2d& e0_t1,
    const Eigen::Vector2d& e1_t1,
    double& toi,
    const double conservative_rescaling);

} // namespace ipc


<|start_filename|>tests/static_vs_dynamic.cpp<|end_filename|>
#include <catch2/catch.hpp>

#include <ipc/distance/point_triangle.hpp>
#include <ipc/utils/eigen_ext.hpp>

using namespace ipc;

TEST_CASE("Template dynamic vs static", "[!benchmark][eigen]")
{
    Eigen::MatrixXd V = Eigen::MatrixXd::Random(100, 3);

    int vi = 0, t0i = 50, t1i = 75, t2i = 99;

    BENCHMARK("Static")
    {
        MatrixMax12d hess;

        Eigen::Vector3d p = V.row(vi);
        Eigen::Vector3d t0 = V.row(t0i);
        Eigen::Vector3d t1 = V.row(t1i);
        Eigen::Vector3d t2 = V.row(t2i);

        point_triangle_distance_hessian(p, t0, t1, t2, hess);
    };

    BENCHMARK("Dynamic")
    {
        MatrixMax12d hess;
        point_triangle_distance_hessian(
            V.row(vi), V.row(t0i), V.row(t1i), V.row(t2i), hess);
    };
}


<|start_filename|>python/src/distance/line_line.cpp<|end_filename|>
#include <pybind11/pybind11.h>
#include <pybind11/eigen.h>

#include <ipc/distance/line_line.hpp>

#include "../utils.hpp"

namespace py = pybind11;
using namespace ipc;

void define_line_line_distance_functions(py::module_& m)
{
    m.def(
        "line_line_distance",
        [](const Eigen::Vector3d& ea0, const Eigen::Vector3d& ea1,
           const Eigen::Vector3d& eb0, const Eigen::Vector3d& eb1) {
            return line_line_distance(ea0, ea1, eb0, eb1);
        },
        R"ipc_Qu8mg5v7(
        Compute the distance between a two infinite lines in 3D.

        Parameters:
            ea0: first vertex of the edge defining the first line
            ea1: second vertex of the edge defining the first line
            eb0: first vertex of the edge defining the second line
            eb1: second vertex of the edge defining the second line

        Returns:
            The distance between the two lines.

        Note:
            The distance is actually squared distance.

        Warning:
            If the lines are parallel this function returns a distance of zero.
        )ipc_Qu8mg5v7",
        py::arg("ea0"), py::arg("ea1"), py::arg("eb0"), py::arg("eb1"));

    m.def(
        "line_line_distance_gradient",
        [](const Eigen::Vector3d& ea0, const Eigen::Vector3d& ea1,
           const Eigen::Vector3d& eb0, const Eigen::Vector3d& eb1) {
            Vector<double, 12> grad;
            line_line_distance_gradient(ea0, ea1, eb0, eb1, grad);
            return grad;
        },
        R"ipc_Qu8mg5v7(
        Compute the gradient of the distance between a two lines in 3D.

        Parameters:
            ea0: first vertex of the edge defining the first line
            ea1: second vertex of the edge defining the first line
            eb0: first vertex of the edge defining the second line
            eb1: second vertex of the edge defining the second line

        Returns:
            The gradient of the distance wrt ea0, ea1, eb0, and eb1.

        Note:
            The distance is actually squared distance.

        Warning:
            If the lines are parallel this function returns a distance of zero.
        )ipc_Qu8mg5v7",
        py::arg("ea0"), py::arg("ea1"), py::arg("eb0"), py::arg("eb1"));

    m.def(
        "line_line_distance_hessian",
        [](const Eigen::Vector3d& ea0, const Eigen::Vector3d& ea1,
           const Eigen::Vector3d& eb0, const Eigen::Vector3d& eb1) {
            Eigen::Matrix<double, 12, 12> hess;
            line_line_distance_hessian(ea0, ea1, eb0, eb1, hess);
            return hess;
        },
        R"ipc_Qu8mg5v7(
        Compute the hessian of the distance between a two lines in 3D.

        Parameters:
            ea0: first vertex of the edge defining the first line
            ea1: second vertex of the edge defining the first line
            eb0: first vertex of the edge defining the second line
            eb1: second vertex of the edge defining the second line

        Returns:
            The hessian of the distance wrt ea0, ea1, eb0, and eb1.

        Note:
            The distance is actually squared distance.

        Warning:
            If the lines are parallel this function returns a distance of zero.
        )ipc_Qu8mg5v7",
        py::arg("ea0"), py::arg("ea1"), py::arg("eb0"), py::arg("eb1"));
}


<|start_filename|>src/friction/friction.tpp<|end_filename|>
#pragma once
#include <ipc/friction/friction.hpp>

#include <tbb/parallel_for.h>
#include <tbb/blocked_range.h>
#include <tbb/enumerable_thread_specific.h>

namespace ipc {

template <typename T>
T compute_friction_potential(
    const Eigen::MatrixXd& V0,
    const MatrixX<T>& V1,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    const FrictionConstraints& friction_constraint_set,
    double epsv_times_h)
{
    if (friction_constraint_set.empty()) {
        return T(0);
    }

    // absolute linear dislacement of each point
    MatrixX<T> U = V1 - V0.cast<T>();

    tbb::enumerable_thread_specific<T> storage(0);

    const size_t num_vv = friction_constraint_set.vv_constraints.size();
    const size_t num_ev = friction_constraint_set.ev_constraints.size();
    const size_t num_ee = friction_constraint_set.ee_constraints.size();
    const size_t num_fv = friction_constraint_set.fv_constraints.size();

    tbb::parallel_for(
        tbb::blocked_range<size_t>(size_t(0), friction_constraint_set.size()),
        [&](tbb::blocked_range<size_t> r) {
            auto& local_potential = storage.local();
            for (size_t i = r.begin(); i < r.end(); i++) {

                size_t ci = i;
                if (ci < num_vv) {
                    local_potential +=
                        friction_constraint_set.vv_constraints[ci]
                            .compute_potential(U, E, F, epsv_times_h);
                } else if ((ci -= num_vv) < num_ev) {
                    local_potential +=
                        friction_constraint_set.ev_constraints[ci]
                            .compute_potential(U, E, F, epsv_times_h);
                } else if ((ci -= num_ev) < num_ee) {
                    local_potential +=
                        friction_constraint_set.ee_constraints[ci]
                            .compute_potential(U, E, F, epsv_times_h);
                } else {
                    ci -= num_ee;
                    assert(ci < num_fv);
                    local_potential +=
                        friction_constraint_set.fv_constraints[ci]
                            .compute_potential(U, E, F, epsv_times_h);
                }
            }
        });

    T potential(0);
    for (const auto& local_potential : storage) {
        potential += local_potential;
    }
    return potential;
}

} // namespace ipc


<|start_filename|>src/distance/line_line.cpp<|end_filename|>
#include <ipc/distance/line_line.hpp>

namespace ipc {
namespace autogen {

    // This function was generated by the Symbolic Math Toolbox version 8.3.
    // 14-Jun-2019 13:58:25
    void line_line_distance_gradient(
        double v01,
        double v02,
        double v03,
        double v11,
        double v12,
        double v13,
        double v21,
        double v22,
        double v23,
        double v31,
        double v32,
        double v33,
        double g[12])
    {
        double t11, t12, t13, t14, t15, t16, t17, t18, t19, t32, t33, t34, t35,
            t36, t37, t44, t45, t46, t75, t77, t76, t78, t79, t80, t81, t83;

        t11 = -v11 + v01;
        t12 = -v12 + v02;
        t13 = -v13 + v03;
        t14 = -v21 + v01;
        t15 = -v22 + v02;
        t16 = -v23 + v03;
        t17 = -v31 + v21;
        t18 = -v32 + v22;
        t19 = -v33 + v23;
        t32 = t14 * t18;
        t33 = t15 * t17;
        t34 = t14 * t19;
        t35 = t16 * t17;
        t36 = t15 * t19;
        t37 = t16 * t18;
        t44 = t11 * t18 + -(t12 * t17);
        t45 = t11 * t19 + -(t13 * t17);
        t46 = t12 * t19 + -(t13 * t18);
        t75 = 1.0 / ((t44 * t44 + t45 * t45) + t46 * t46);
        t77 = (t16 * t44 + t14 * t46) + -(t15 * t45);
        t76 = t75 * t75;
        t78 = t77 * t77;
        t79 = (t12 * t44 * 2.0 + t13 * t45 * 2.0) * t76 * t78;
        t80 = (t11 * t45 * 2.0 + t12 * t46 * 2.0) * t76 * t78;
        t81 = (t18 * t44 * 2.0 + t19 * t45 * 2.0) * t76 * t78;
        t18 = (t17 * t45 * 2.0 + t18 * t46 * 2.0) * t76 * t78;
        t83 = (t11 * t44 * 2.0 + -(t13 * t46 * 2.0)) * t76 * t78;
        t19 = (t17 * t44 * 2.0 + -(t19 * t46 * 2.0)) * t76 * t78;
        t76 = t75 * t77;
        g[0] = -t81 + t76 * ((-t36 + t37) + t46) * 2.0;
        g[1] = t19 - t76 * ((-t34 + t35) + t45) * 2.0;
        g[2] = t18 + t76 * ((-t32 + t33) + t44) * 2.0;
        g[3] = t81 + t76 * (t36 - t37) * 2.0;
        g[4] = -t19 - t76 * (t34 - t35) * 2.0;
        g[5] = -t18 + t76 * (t32 - t33) * 2.0;
        t17 = t12 * t16 + -(t13 * t15);
        g[6] = t79 - t76 * (t17 + t46) * 2.0;
        t18 = t11 * t16 + -(t13 * t14);
        g[7] = -t83 + t76 * (t18 + t45) * 2.0;
        t19 = t11 * t15 + -(t12 * t14);
        g[8] = -t80 - t76 * (t19 + t44) * 2.0;
        g[9] = -t79 + t76 * t17 * 2.0;
        g[10] = t83 - t76 * t18 * 2.0;
        g[11] = t80 + t76 * t19 * 2.0;
    }

    // This function was generated by the Symbolic Math Toolbox version 8.3.
    // 14-Jun-2019 13:58:38
    void line_line_distance_hessian(
        double v01,
        double v02,
        double v03,
        double v11,
        double v12,
        double v13,
        double v21,
        double v22,
        double v23,
        double v31,
        double v32,
        double v33,
        double H[144])
    {
        double t11, t12, t13, t14, t15, t16, t26, t27, t28, t47, t48, t49, t50,
            t51, t52, t53, t54, t55, t56, t57, t58, t59, t65, t73, t35, t36,
            t37, t38, t39, t40, t98, t99, t100, t101, t103, t105, t107, t108,
            t109, t137, t138, t139, t140, t141, t142, t143, t144, t145, t146,
            t147, t148, t156, t159, t157, t262, t263, t264, t265, t266, t267,
            t268, t269, t270, t271, t272, t273, t274, t275, t276, t277, t278,
            t279, t298, t299, t300, t301, t302, t303, t310, t311, t312, t313,
            t314, t315, t322, t323, t325, t326, t327, t328, t329, t330, t335,
            t337, t339, t340, t341, t342, t343, t345, t348, t353, t356, t358,
            t359, t360, t362, t367, t368, t369, t371, t374, t377, t382, t386,
            t387, t398, t399, t403, t408, t423, t424, t427, t428, t431, t432,
            t433, t434, t437, t438, t441, t442, t446, t451, t455, t456, t467,
            t468, t472, t477, t491, t492, t495, t497, t499, t500, t503, t504,
            t506, t508, t550, t568, t519_tmp, b_t519_tmp, t519, t520_tmp,
            b_t520_tmp, t520, t521_tmp, b_t521_tmp, t521, t522_tmp, b_t522_tmp,
            t522, t523_tmp, b_t523_tmp, t523, t524_tmp, b_t524_tmp, t524, t525,
            t526, t527, t528, t529, t530, t531, t532, t533, t534, t535, t536,
            t537, t538, t539, t540, t542, t543, t544;

        t11 = -v11 + v01;
        t12 = -v12 + v02;
        t13 = -v13 + v03;
        t14 = -v21 + v01;
        t15 = -v22 + v02;
        t16 = -v23 + v03;
        t26 = -v31 + v21;
        t27 = -v32 + v22;
        t28 = -v33 + v23;
        t47 = t11 * t27;
        t48 = t12 * t26;
        t49 = t11 * t28;
        t50 = t13 * t26;
        t51 = t12 * t28;
        t52 = t13 * t27;
        t53 = t14 * t27;
        t54 = t15 * t26;
        t55 = t14 * t28;
        t56 = t16 * t26;
        t57 = t15 * t28;
        t58 = t16 * t27;
        t59 = t11 * t26 * 2.0;
        t65 = t12 * t27 * 2.0;
        t73 = t13 * t28 * 2.0;
        t35 = t11 * t11 * 2.0;
        t36 = t12 * t12 * 2.0;
        t37 = t13 * t13 * 2.0;
        t38 = t26 * t26 * 2.0;
        t39 = t27 * t27 * 2.0;
        t40 = t28 * t28 * 2.0;
        t98 = t11 * t15 + -(t12 * t14);
        t99 = t11 * t16 + -(t13 * t14);
        t100 = t12 * t16 + -(t13 * t15);
        t101 = t47 + -t48;
        t103 = t49 + -t50;
        t105 = t51 + -t52;
        t107 = t53 + -t54;
        t108 = t55 + -t56;
        t109 = t57 + -t58;
        t137 = t98 + t101;
        t138 = t99 + t103;
        t139 = t100 + t105;
        t140 = (t54 + -t53) + t101;
        t141 = (t56 + -t55) + t103;
        t142 = (t58 + -t57) + t105;
        t143 = t12 * t101 * 2.0 + t13 * t103 * 2.0;
        t144 = t11 * t103 * 2.0 + t12 * t105 * 2.0;
        t145 = t27 * t101 * 2.0 + t28 * t103 * 2.0;
        t146 = t26 * t103 * 2.0 + t27 * t105 * 2.0;
        t147 = t11 * t101 * 2.0 + -(t13 * t105 * 2.0);
        t148 = t26 * t101 * 2.0 + -(t28 * t105 * 2.0);
        t156 = 1.0 / ((t101 * t101 + t103 * t103) + t105 * t105);
        t159 = (t16 * t101 + t14 * t105) + -(t15 * t103);
        t157 = t156 * t156;
        t57 = pow(t156, 3.0);
        t58 = t159 * t159;
        t262 = t11 * t156 * t159 * 2.0;
        t263 = t12 * t156 * t159 * 2.0;
        t264 = t13 * t156 * t159 * 2.0;
        t265 = t14 * t156 * t159 * 2.0;
        t266 = t15 * t156 * t159 * 2.0;
        t267 = t16 * t156 * t159 * 2.0;
        t268 = (-v31 + v01) * t156 * t159 * 2.0;
        t269 = (-v21 + v11) * t156 * t159 * 2.0;
        t270 = (-v32 + v02) * t156 * t159 * 2.0;
        t271 = (-v22 + v12) * t156 * t159 * 2.0;
        t272 = (-v33 + v03) * t156 * t159 * 2.0;
        t273 = (-v23 + v13) * t156 * t159 * 2.0;
        t274 = (-v31 + v11) * t156 * t159 * 2.0;
        t275 = (-v32 + v12) * t156 * t159 * 2.0;
        t276 = (-v33 + v13) * t156 * t159 * 2.0;
        t277 = t26 * t156 * t159 * 2.0;
        t278 = t27 * t156 * t159 * 2.0;
        t279 = t28 * t156 * t159 * 2.0;
        t298 = t11 * t12 * t157 * t58 * 2.0;
        t299 = t11 * t13 * t157 * t58 * 2.0;
        t300 = t12 * t13 * t157 * t58 * 2.0;
        t301 = t26 * t27 * t157 * t58 * 2.0;
        t302 = t26 * t28 * t157 * t58 * 2.0;
        t303 = t27 * t28 * t157 * t58 * 2.0;
        t310 = (t35 + t36) * t157 * t58;
        t311 = (t35 + t37) * t157 * t58;
        t312 = (t36 + t37) * t157 * t58;
        t313 = (t38 + t39) * t157 * t58;
        t314 = (t38 + t40) * t157 * t58;
        t315 = (t39 + t40) * t157 * t58;
        t322 = (t59 + t65) * t157 * t58;
        t323 = (t59 + t73) * t157 * t58;
        t59 = (t65 + t73) * t157 * t58;
        t325 = (t47 * 2.0 + -(t48 * 4.0)) * t157 * t58;
        t53 = -t157 * t58;
        t56 = t48 * 2.0 - t47 * 4.0;
        t326 = t53 * t56;
        t327 = (t49 * 2.0 + -(t50 * 4.0)) * t157 * t58;
        t55 = t50 * 2.0 - t49 * 4.0;
        t328 = t53 * t55;
        t329 = (t51 * 2.0 + -(t52 * 4.0)) * t157 * t58;
        t54 = t52 * 2.0 - t51 * 4.0;
        t330 = t53 * t54;
        t53 = t157 * t58;
        t335 = t53 * t56;
        t337 = t53 * t55;
        t339 = t53 * t54;
        t340 = t143 * t143 * t57 * t58 * 2.0;
        t341 = t144 * t144 * t57 * t58 * 2.0;
        t342 = t145 * t145 * t57 * t58 * 2.0;
        t343 = t146 * t146 * t57 * t58 * 2.0;
        t345 = t147 * t147 * t57 * t58 * 2.0;
        t348 = t148 * t148 * t57 * t58 * 2.0;
        t36 = t98 * t143 * t157 * t159 * 2.0;
        t353 = t99 * t143 * t157 * t159 * 2.0;
        t356 = t99 * t144 * t157 * t159 * 2.0;
        t65 = t100 * t144 * t157 * t159 * 2.0;
        t358 = t107 * t143 * t157 * t159 * 2.0;
        t359 = t98 * t145 * t157 * t159 * 2.0;
        t360 = t108 * t143 * t157 * t159 * 2.0;
        t54 = t107 * t144 * t157 * t159 * 2.0;
        t362 = t99 * t145 * t157 * t159 * 2.0;
        t53 = t98 * t146 * t157 * t159 * 2.0;
        t56 = t109 * t143 * t157 * t159 * 2.0;
        t27 = t108 * t144 * t157 * t159 * 2.0;
        t55 = t100 * t145 * t157 * t159 * 2.0;
        t367 = t99 * t146 * t157 * t159 * 2.0;
        t368 = t109 * t144 * t157 * t159 * 2.0;
        t369 = t100 * t146 * t157 * t159 * 2.0;
        t38 = t107 * t145 * t157 * t159 * 2.0;
        t371 = t108 * t145 * t157 * t159 * 2.0;
        t374 = t108 * t146 * t157 * t159 * 2.0;
        t28 = t109 * t146 * t157 * t159 * 2.0;
        t377 = t98 * t147 * t157 * t159 * 2.0;
        t382 = t100 * t147 * t157 * t159 * 2.0;
        t386 = t107 * t147 * t157 * t159 * 2.0;
        t387 = t98 * t148 * t157 * t159 * 2.0;
        t103 = t108 * t147 * t157 * t159 * 2.0;
        t101 = t99 * t148 * t157 * t159 * 2.0;
        t398 = t109 * t147 * t157 * t159 * 2.0;
        t399 = t100 * t148 * t157 * t159 * 2.0;
        t403 = t107 * t148 * t157 * t159 * 2.0;
        t408 = t109 * t148 * t157 * t159 * 2.0;
        t73 = t137 * t143 * t157 * t159 * 2.0;
        t423 = t138 * t143 * t157 * t159 * 2.0;
        t424 = t138 * t144 * t157 * t159 * 2.0;
        t37 = t139 * t144 * t157 * t159 * 2.0;
        t427 = t140 * t143 * t157 * t159 * 2.0;
        t428 = t137 * t145 * t157 * t159 * 2.0;
        t16 = t140 * t144 * t157 * t159 * 2.0;
        t11 = t137 * t146 * t157 * t159 * 2.0;
        t431 = t141 * t143 * t157 * t159 * 2.0;
        t432 = t138 * t145 * t157 * t159 * 2.0;
        t433 = t141 * t144 * t157 * t159 * 2.0;
        t434 = t138 * t146 * t157 * t159 * 2.0;
        t105 = t142 * t143 * t157 * t159 * 2.0;
        t14 = t139 * t145 * t157 * t159 * 2.0;
        t437 = t142 * t144 * t157 * t159 * 2.0;
        t438 = t139 * t146 * t157 * t159 * 2.0;
        t35 = t140 * t145 * t157 * t159 * 2.0;
        t441 = t141 * t145 * t157 * t159 * 2.0;
        t442 = t141 * t146 * t157 * t159 * 2.0;
        t39 = t142 * t146 * t157 * t159 * 2.0;
        t446 = t137 * t147 * t157 * t159 * 2.0;
        t451 = t139 * t147 * t157 * t159 * 2.0;
        t455 = t140 * t147 * t157 * t159 * 2.0;
        t456 = t137 * t148 * t157 * t159 * 2.0;
        t13 = t141 * t147 * t157 * t159 * 2.0;
        t26 = t138 * t148 * t157 * t159 * 2.0;
        t467 = t142 * t147 * t157 * t159 * 2.0;
        t468 = t139 * t148 * t157 * t159 * 2.0;
        t472 = t140 * t148 * t157 * t159 * 2.0;
        t477 = t142 * t148 * t157 * t159 * 2.0;
        t47 = t143 * t144 * t57 * t58 * 2.0;
        t15 = t143 * t145 * t57 * t58 * 2.0;
        t491 = t143 * t146 * t57 * t58 * 2.0;
        t492 = t144 * t145 * t57 * t58 * 2.0;
        t12 = t144 * t146 * t57 * t58 * 2.0;
        t40 = t145 * t146 * t57 * t58 * 2.0;
        t495 = t143 * t147 * t57 * t58 * 2.0;
        t497 = t144 * t147 * t57 * t58 * 2.0;
        t499 = t143 * t148 * t57 * t58 * 2.0;
        t500 = t145 * t147 * t57 * t58 * 2.0;
        t503 = t146 * t147 * t57 * t58 * 2.0;
        t504 = t144 * t148 * t57 * t58 * 2.0;
        t506 = t145 * t148 * t57 * t58 * 2.0;
        t508 = t146 * t148 * t57 * t58 * 2.0;
        t57 = t147 * t148 * t57 * t58 * 2.0;
        t550 =
            ((((t98 * t109 * t156 * 2.0 + -t266) + t337) + t359) + t368) + t492;
        t568 =
            ((((t108 * t137 * t156 * 2.0 + -t268) + t330) + t27) + t456) + t504;
        t519_tmp = t139 * t143 * t157 * t159;
        b_t519_tmp = t100 * t143 * t157 * t159;
        t519 =
            (((-(t100 * t139 * t156 * 2.0) + t312) + -t340) + b_t519_tmp * 2.0)
            + t519_tmp * 2.0;
        t520_tmp = t140 * t146 * t157 * t159;
        b_t520_tmp = t107 * t146 * t157 * t159;
        t520 = (((t107 * t140 * t156 * 2.0 + t313) + -t343) + b_t520_tmp * 2.0)
            + -(t520_tmp * 2.0);
        t521_tmp = t142 * t145 * t157 * t159;
        b_t521_tmp = t109 * t145 * t157 * t159;
        t521 =
            (((t109 * t142 * t156 * 2.0 + t315) + -t342) + -(b_t521_tmp * 2.0))
            + t521_tmp * 2.0;
        t522_tmp = t137 * t144 * t157 * t159;
        b_t522_tmp = t98 * t144 * t157 * t159;
        t522 = (((-(t98 * t137 * t156 * 2.0) + t310) + -t341)
                + -(b_t522_tmp * 2.0))
            + -(t522_tmp * 2.0);
        t523_tmp = t138 * t147 * t157 * t159;
        b_t523_tmp = t99 * t147 * t157 * t159;
        t523 =
            (((-(t99 * t138 * t156 * 2.0) + t311) + -t345) + b_t523_tmp * 2.0)
            + t523_tmp * 2.0;
        t524_tmp = t141 * t148 * t157 * t159;
        b_t524_tmp = t108 * t148 * t157 * t159;
        t524 =
            (((t108 * t141 * t156 * 2.0 + t314) + -t348) + -(b_t524_tmp * 2.0))
            + t524_tmp * 2.0;
        t525 = (((t98 * t100 * t156 * 2.0 + t299) + t65) + -t36) + -t47;
        t526 = (((t107 * t109 * t156 * 2.0 + t302) + t38) + -t28) + -t40;
        t527 = (((-(t98 * t99 * t156 * 2.0) + t300) + t377) + -t356) + t497;
        t528 = (((-(t99 * t100 * t156 * 2.0) + t298) + t353) + t382) + -t495;
        t529 = (((-(t107 * t108 * t156 * 2.0) + t303) + t374) + -t403) + t508;
        t530 = (((-(t108 * t109 * t156 * 2.0) + t301) + -t371) + -t408) + -t506;
        t531 = (((t98 * t107 * t156 * 2.0 + t322) + t54) + -t53) + -t12;
        t532 = (((t100 * t109 * t156 * 2.0 + t59) + t55) + -t56) + -t15;
        t533 = (((t99 * t108 * t156 * 2.0 + t323) + t101) + -t103) + -t57;
        t534 = (((t98 * t140 * t156 * 2.0 + -t322) + t53) + t16) + t12;
        t535 = (((-(t107 * t137 * t156 * 2.0) + -t322) + -t54) + t11) + t12;
        t536 = (((t100 * t142 * t156 * 2.0 + -t59) + -t55) + -t105) + t15;
        t537 = (((-(t109 * t139 * t156 * 2.0) + -t59) + t56) + -t14) + t15;
        t538 = (((t99 * t141 * t156 * 2.0 + -t323) + -t101) + -t13) + t57;
        t539 = (((-(t108 * t138 * t156 * 2.0) + -t323) + t103) + -t26) + t57;
        t540 = (((t137 * t139 * t156 * 2.0 + t299) + t37) + -t73) + -t47;
        t148 = (((t140 * t142 * t156 * 2.0 + t302) + t39) + -t35) + -t40;
        t542 = (((-(t137 * t138 * t156 * 2.0) + t300) + t446) + -t424) + t497;
        t543 = (((-(t138 * t139 * t156 * 2.0) + t298) + t423) + t451) + -t495;
        t544 = (((-(t140 * t141 * t156 * 2.0) + t303) + t472) + -t442) + t508;
        t53 = (((-(t141 * t142 * t156 * 2.0) + t301) + t441) + t477) + -t506;
        t157 = (((-(t139 * t142 * t156 * 2.0) + t59) + t105) + t14) + -t15;
        t159 = (((-(t137 * t140 * t156 * 2.0) + t322) + -t16) + -t11) + -t12;
        t147 = (((-(t138 * t141 * t156 * 2.0) + t323) + t13) + t26) + -t57;
        t146 = ((((t100 * t107 * t156 * 2.0 + t266) + t327) + -t358) + -t369)
            + t491;
        t145 = ((((-(t99 * t107 * t156 * 2.0) + -t265) + t329) + t367) + t386)
            + -t503;
        t144 = ((((-(t100 * t108 * t156 * 2.0) + -t267) + t325) + t360) + -t399)
            + t499;
        t143 = ((((-(t99 * t109 * t156 * 2.0) + t267) + t335) + -t362) + t398)
            + t500;
        t52 = ((((-(t98 * t108 * t156 * 2.0) + t265) + t339) + -t27) + -t387)
            + -t504;
        t51 =
            ((((t109 * t140 * t156 * 2.0 + -t278) + -t302) + t28) + t35) + t40;
        t50 = ((((-(t98 * t139 * t156 * 2.0) + t263) + -t299) + t36) + -t37)
            + t47;
        t49 =
            ((((t107 * t142 * t156 * 2.0 + t278) + -t302) + -t38) + -t39) + t40;
        t48 = ((((-(t100 * t137 * t156 * 2.0) + -t263) + -t299) + -t65) + t73)
            + t47;
        t47 = ((((t99 * t137 * t156 * 2.0 + t262) + -t300) + t356) + -t446)
            + -t497;
        t73 = ((((t100 * t138 * t156 * 2.0 + t264) + -t298) + -t382) + -t423)
            + t495;
        t65 = ((((-(t109 * t141 * t156 * 2.0) + t279) + -t301) + t408) + -t441)
            + t506;
        t59 = ((((t98 * t138 * t156 * 2.0 + -t262) + -t300) + -t377) + t424)
            + -t497;
        t40 = ((((t99 * t139 * t156 * 2.0 + -t264) + -t298) + -t353) + -t451)
            + t495;
        t39 = ((((-(t107 * t141 * t156 * 2.0) + -t277) + -t303) + t403) + t442)
            + -t508;
        t38 = ((((-(t108 * t142 * t156 * 2.0) + -t279) + -t301) + t371) + -t477)
            + t506;
        t37 = ((((-(t108 * t140 * t156 * 2.0) + t277) + -t303) + -t374) + -t472)
            + -t508;
        t36 = ((((t98 * t142 * t156 * 2.0 + t271) + t328) + -t359) + t437)
            + -t492;
        t35 = ((((-(t109 * t137 * t156 * 2.0) + t270) + t328) + -t368) + -t428)
            + -t492;
        t28 = ((((t100 * t140 * t156 * 2.0 + -t271) + -t327) + t369) + -t427)
            + -t491;
        t27 = ((((-(t98 * t141 * t156 * 2.0) + -t269) + t330) + t387) + -t433)
            + t504;
        t26 = ((((t109 * t138 * t156 * 2.0 + -t272) + t326) + -t398) + t432)
            + -t500;
        t13 = ((((-(t107 * t139 * t156 * 2.0) + -t270) + -t327) + t358) + t438)
            + -t491;
        t12 = ((((-(t99 * t142 * t156 * 2.0) + -t273) + t326) + t362) + t467)
            + -t500;
        t11 = ((((-(t99 * t140 * t156 * 2.0) + t269) + -t329) + -t367) + t455)
            + t503;
        t16 = ((((t107 * t138 * t156 * 2.0 + t268) + -t329) + -t386) + -t434)
            + t503;
        t15 = ((((-(t100 * t141 * t156 * 2.0) + t273) + -t325) + t399) + t431)
            + -t499;
        t14 = ((((t108 * t139 * t156 * 2.0 + t272) + -t325) + -t360) + t468)
            + -t499;
        t105 = ((((-(t139 * t140 * t156 * 2.0) + t275) + t327) + t427) + -t438)
            + t491;
        t103 = ((((t138 * t140 * t156 * 2.0 + -t274) + t329) + t434) + -t455)
            + -t503;
        t101 = ((((-(t137 * t142 * t156 * 2.0) + -t275) + t337) + t428) + -t437)
            + t492;
        t58 = ((((t139 * t141 * t156 * 2.0 + -t276) + t325) + -t431) + -t468)
            + t499;
        t57 = ((((t137 * t141 * t156 * 2.0 + t274) + t339) + t433) + -t456)
            + -t504;
        t56 = ((((t138 * t142 * t156 * 2.0 + t276) + t335) + -t432) + -t467)
            + t500;
        t55 = -t315 + t342;
        H[0] = (t55 + t142 * t142 * t156 * 2.0) - t521_tmp * 4.0;
        H[1] = t53;
        H[2] = t148;
        H[3] = t521;
        H[4] = t38;
        H[5] = t49;
        H[6] = t157;
        H[7] = t56;
        H[8] = t101;
        H[9] = t536;
        H[10] = t12;
        H[11] = t36;
        H[12] = t53;
        t54 = -t314 + t348;
        H[13] = (t54 + t141 * t141 * t156 * 2.0) - t524_tmp * 4.0;
        H[14] = t544;
        H[15] = t65;
        H[16] = t524;
        H[17] = t39;
        H[18] = t58;
        H[19] = t147;
        H[20] = t57;
        H[21] = t15;
        H[22] = t538;
        H[23] = t27;
        H[24] = t148;
        H[25] = t544;
        t53 = -t313 + t343;
        H[26] = (t53 + t140 * t140 * t156 * 2.0) + t520_tmp * 4.0;
        H[27] = t51;
        H[28] = t37;
        H[29] = t520;
        H[30] = t105;
        H[31] = t103;
        H[32] = t159;
        H[33] = t28;
        H[34] = t11;
        H[35] = t534;
        H[36] = t521;
        H[37] = t65;
        H[38] = t51;
        H[39] = (t55 + t109 * t109 * t156 * 2.0) + b_t521_tmp * 4.0;
        H[40] = t530;
        H[41] = t526;
        H[42] = t537;
        H[43] = t26;
        H[44] = t35;
        H[45] = t532;
        H[46] = t143;
        H[47] = t550;
        H[48] = t38;
        H[49] = t524;
        H[50] = t37;
        H[51] = t530;
        H[52] = (t54 + t108 * t108 * t156 * 2.0) + b_t524_tmp * 4.0;
        H[53] = t529;
        H[54] = t14;
        H[55] = t539;
        H[56] = t568;
        H[57] = t144;
        H[58] = t533;
        H[59] = t52;
        H[60] = t49;
        H[61] = t39;
        H[62] = t520;
        H[63] = t526;
        H[64] = t529;
        H[65] = (t53 + t107 * t107 * t156 * 2.0) - b_t520_tmp * 4.0;
        H[66] = t13;
        H[67] = t16;
        H[68] = t535;
        H[69] = t146;
        H[70] = t145;
        H[71] = t531;
        H[72] = t157;
        H[73] = t58;
        H[74] = t105;
        H[75] = t537;
        H[76] = t14;
        H[77] = t13;
        t55 = -t312 + t340;
        H[78] = (t55 + t139 * t139 * t156 * 2.0) - t519_tmp * 4.0;
        H[79] = t543;
        H[80] = t540;
        H[81] = t519;
        H[82] = t40;
        H[83] = t50;
        H[84] = t56;
        H[85] = t147;
        H[86] = t103;
        H[87] = t26;
        H[88] = t539;
        H[89] = t16;
        H[90] = t543;
        t54 = -t311 + t345;
        H[91] = (t54 + t138 * t138 * t156 * 2.0) - t523_tmp * 4.0;
        H[92] = t542;
        H[93] = t73;
        H[94] = t523;
        H[95] = t59;
        H[96] = t101;
        H[97] = t57;
        H[98] = t159;
        H[99] = t35;
        H[100] = t568;
        H[101] = t535;
        H[102] = t540;
        H[103] = t542;
        t53 = -t310 + t341;
        H[104] = (t53 + t137 * t137 * t156 * 2.0) + t522_tmp * 4.0;
        H[105] = t48;
        H[106] = t47;
        H[107] = t522;
        H[108] = t536;
        H[109] = t15;
        H[110] = t28;
        H[111] = t532;
        H[112] = t144;
        H[113] = t146;
        H[114] = t519;
        H[115] = t73;
        H[116] = t48;
        H[117] = (t55 + t100 * t100 * t156 * 2.0) - b_t519_tmp * 4.0;
        H[118] = t528;
        H[119] = t525;
        H[120] = t12;
        H[121] = t538;
        H[122] = t11;
        H[123] = t143;
        H[124] = t533;
        H[125] = t145;
        H[126] = t40;
        H[127] = t523;
        H[128] = t47;
        H[129] = t528;
        H[130] = (t54 + t99 * t99 * t156 * 2.0) - b_t523_tmp * 4.0;
        H[131] = t527;
        H[132] = t36;
        H[133] = t27;
        H[134] = t534;
        H[135] = t550;
        H[136] = t52;
        H[137] = t531;
        H[138] = t50;
        H[139] = t59;
        H[140] = t522;
        H[141] = t525;
        H[142] = t527;
        H[143] = (t53 + t98 * t98 * t156 * 2.0) + b_t522_tmp * 4.0;
    }

} // namespace autogen
} // namespace ipc


<|start_filename|>cmake/find/FindSSE.cmake<|end_filename|>
################################################################################
# Copyright (c) 2016, Project Chrono Development Team
# All rights reserved.
# This file is licensed to you under the BSD 3-Clause "New" or "Revised" License.
# See https://github.com/projectchrono/chrono/blob/develop/LICENSE for details.
################################################################################
#
# This script checks for the highest level of SSE support on the host
# by compiling and running small C++ programs that uses SSE intrinsics.
#
# You can invoke this module using the following command:
#
#   FIND_PACKAGE(SSE [major[.minor]] [EXACT] [QUIET|REQUIRED])
#
# where the version string is one of:
#
#   1.0 for SSE support
#   2.0 for SSE2 support
#   3.0 for SSE3 support
#   3.1 for SSSE3 support
#   4.1 for SSE 4.1 support
#   4.2 for SSE 4.2 support
#
# Note that any ".0" in the above version string is optional.
#
# If any SSE support is detected, the following variables are set:
#
#   SSE_FOUND   = 1
#   SSE_VERSION = the requested version, if EXACT is true, or
#                 the highest SSE version found.
#   SSE_FLAGS = compile flags for the version of SSE found
#
# If SSE is not supported on the host platform, these variables are
# not set. If QUIET is true, the module does not print a message if
# SSE if missing. If REQUIRED is true, the module produces a fatal
# error if SSE support is missing.
#
set(SSE_FLAGS)
set(SSE_FOUND)
set(DETECTED_SSE_41)
set(DETECTED_SSE_42)
set(DETECTED_SSE_10)
set(DETECTED_SSE_20)
set(DETECTED_SSE_30)

if(CMAKE_COMPILER_IS_GNUCC OR CMAKE_COMPILER_IS_GNUCXX)
  execute_process(COMMAND ${CMAKE_CXX_COMPILER} "-dumpversion" OUTPUT_VARIABLE GCC_VERSION_STRING)
  if(GCC_VERSION_STRING VERSION_GREATER 4.2 AND NOT APPLE AND NOT CMAKE_CROSSCOMPILING)
    SET(SSE_FLAGS "${SSE_FLAGS} -march=native")
    message(STATUS "Using CPU native flags for SSE optimization: ${SSE_FLAGS}")
  endif()
endif()

include(CheckCXXSourceRuns)
set(CMAKE_REQUIRED_FLAGS)


# Generate a list of SSE versions to test.
if(SSE_FIND_VERSION_EXACT)
  if(SSE_FIND_VERSION VERSION_EQUAL "4.2")
    set(_SSE_TEST_42 1)
  elseif(SSE_FIND_VERSION VERSION_EQUAL "4.1")
    set(_SSE_TEST_41 1)
  elseif(SSE_FIND_VERSION VERSION_EQUAL "3.0")
    set(_SSE_TEST_30 1)
  elseif(SSE_FIND_VERSION VERSION_EQUAL "2.0")
    set(_SSE_TEST_20 1)
  elseif(SSE_FIND_VERSION VERSION_EQUAL "1.0")
    set(_SSE_TEST_10 1)
  endif()
else()
  if(NOT SSE_FIND_VERSION VERSION_GREATER "4.2")
    set(_SSE_TEST_42 1)
  endif()
  if(NOT SSE_FIND_VERSION VERSION_GREATER "4.1")
    set(_SSE_TEST_41 1)
  endif()
  if(NOT SSE_FIND_VERSION VERSION_GREATER "3.0")
    set(_SSE_TEST_30 1)
  endif()
  if(NOT SSE_FIND_VERSION VERSION_GREATER "2.0")
    set(_SSE_TEST_20 1)
  endif()
  if(NOT SSE_FIND_VERSION VERSION_GREATER "1.0")
    set(_SSE_TEST_10 1)
  endif()
endif()


# Check for SSE 4.2 support.
if(_SSE_TEST_42)
  if(CMAKE_COMPILER_IS_GNUCC OR CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
    set(CMAKE_REQUIRED_FLAGS "-msse4.2")
  endif()
  check_cxx_source_runs("
  #include <emmintrin.h>
  #include <nmmintrin.h>
  int main()
  {
    long long a[2] = {  1, 2 };
    long long b[2] = { -1, 3 };
    long long c[2];
    __m128i va = _mm_loadu_si128((__m128i*)a);
    __m128i vb = _mm_loadu_si128((__m128i*)b);
    __m128i vc = _mm_cmpgt_epi64(va, vb);

    _mm_storeu_si128((__m128i*)c, vc);
    if (c[0] == -1LL && c[1] == 0LL)
      return 0;
    else
      return 1;
  }"
  DETECTED_SSE_42)
endif()

# Check for SSE 4.1 support.
if(_SSE_TEST_41)
  if(CMAKE_COMPILER_IS_GNUCC OR CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
    set(CMAKE_REQUIRED_FLAGS "-msse4.1")
  endif()
  check_cxx_source_runs("
  #include <emmintrin.h>
  #include <smmintrin.h>
  int main()
  {
    long long a[2] = {  1, 2 };
    long long b[2] = { -1, 2 };
    long long c[2];
    __m128i va = _mm_loadu_si128((__m128i*)a);
    __m128i vb = _mm_loadu_si128((__m128i*)b);
    __m128i vc = _mm_cmpeq_epi64(va, vb);

    _mm_storeu_si128((__m128i*)c, vc);
    if (c[0] == 0LL && c[1] == -1LL)
      return 0;
    else
      return 1;
  }" DETECTED_SSE_41)
endif()

# Check for SSE 3 support.
if(_SSE_TEST_30)
  if(CMAKE_COMPILER_IS_GNUCC OR CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
    set(CMAKE_REQUIRED_FLAGS "-msse3")
  endif()
  check_cxx_source_runs("
  #include <emmintrin.h>
  #ifdef _WIN32
    #include <intrin.h>
  #else
    #include <x86intrin.h>
  #endif

  int main()
  {
    float a[4] = { 1.0f, 2.0f, 3.0f, 4.0f };
    float b[4] = { 3.0f, 5.0f, 7.0f, 9.0f };
    float c[4];

    __m128 va = _mm_loadu_ps(a);
    __m128 vb = _mm_loadu_ps(b);
    __m128 vc = _mm_hadd_ps(va, vb);

    _mm_storeu_ps(c, vc);
    if (c[0] == 3.0f && c[1] == 7.0f && c[2] == 8.0f && c[3] == 16.0f)
      return 0;
    else
      return 1;
  }" DETECTED_SSE_30)
endif()

# Check for SSE2 support.
if(_SSE_TEST_20)
  if(CMAKE_COMPILER_IS_GNUCC OR CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
    set(CMAKE_REQUIRED_FLAGS "-msse2")
  elseif(MSVC AND NOT CMAKE_CL_64)
    set(CMAKE_REQUIRED_FLAGS "/arch:SSE2")
  endif()
  check_cxx_source_runs("
  #include <emmintrin.h>
  int main()
  {
    int a[4] = { 1, 2,  3,  4 };
    int b[4] = { 3, 6, -4, -4 };
    int c[4];

    __m128i va = _mm_loadu_si128((__m128i*)a);
    __m128i vb = _mm_loadu_si128((__m128i*)b);
    __m128i vc = _mm_add_epi32(va, vb);

    _mm_storeu_si128((__m128i*)c, vc);
    if (c[0] == 4 && c[1] == 8 && c[2] == -1 && c[3] == 0)
      return 0;
    else
      return 1;
  }" DETECTED_SSE_20)
endif()

# Check for SSE support.
if(_SSE_TEST_10)
  if(CMAKE_COMPILER_IS_GNUCC OR CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
    set(CMAKE_REQUIRED_FLAGS "-msse")
  elseif(MSVC AND NOT CMAKE_CL_64)
    set(CMAKE_REQUIRED_FLAGS "/arch:SSE")
  endif()
  check_cxx_source_runs("
  #include <emmintrin.h>
  int main()
  {
    float a[4] = { 1.0f, 2.0f, 3.0f, 4.0f };
    float b[4] = { 2.0f, 3.0f, 4.0f, 5.0f };
    float c[4];
    __m128 va = _mm_loadu_ps(a);
    __m128 vb = _mm_loadu_ps(b);
    __m128 vc = _mm_add_ps(va, vb);

    _mm_storeu_ps(c, vc);
    if (c[0] == 3.0f && c[1] == 5.0f && c[2] == 7.0f && c[3] == 9.0f)
      return 0;
    else
      return 1;
  }" DETECTED_SSE_10)
endif()

set(CMAKE_REQUIRED_FLAGS)

if(DETECTED_SSE_42)
   set(SSE_VERSION "4.2")
   set(SSE_STR "4_2")
   set(SSE_FOUND 1)
elseif(DETECTED_SSE_41)
   set(SSE_VERSION "4.1")
   set(SSE_STR "4_1")
   set(SSE_FOUND 1)
elseif(DETECTED_SSE_30)
   set(SSE_VERSION "3.0")
   set(SSE_STR "3_0")
   set(SSE_FOUND 1)
elseif(DETECTED_SSE_20)
   set(SSE_VERSION "2.0")
   set(SSE_STR "2_0")
   set(SSE_FOUND 1)
elseif(DETECTED_SSE_10)
   set(SSE_VERSION "1.0")
   set(SSE_STR "1_0")
   set(SSE_FOUND 1)
endif()


if(CMAKE_COMPILER_IS_GNUCC OR CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  if(DETECTED_SSE_42)
     set(SSE_FLAGS "${SSE_FLAGS} -msse4.2 -mfpmath=sse")
  elseif(DETECTED_SSE_41)
     set(SSE_FLAGS "${SSE_FLAGS} -msse4.1 -mfpmath=sse")
  elseif(DETECTED_SSE_30)
     set(SSE_FLAGS "${SSE_FLAGS} -msse3 -mfpmath=sse")
  elseif(DETECTED_SSE_20)
     set(SSE_FLAGS "${SSE_FLAGS} -msse2 -mfpmath=sse")
  elseif(DETECTED_SSE_10)
     set(SSE_FLAGS "${SSE_FLAGS} -msse -mfpmath=sse")
  else()
     # Setting -ffloat-store to alleviate 32bit vs 64bit discrepancies on non-SSE platforms.
     set(SSE_FLAGS "-ffloat-store")
  endif()
elseif(CMAKE_CXX_COMPILER_ID MATCHES "Intel")
  set(SSE_FLAGS "-xHost")
elseif(MSVC)
  if(DETECTED_SSE_20)
     set(SSE_FLAGS "${SSE_FLAGS} /arch:SSE2")
  elseif(DETECTED_SSE_10)
     set(SSE_FLAGS "${SSE_FLAGS} /arch:SSE")
  endif()
endif()

if(SSE_FOUND)
  message(STATUS "  Found SSE ${SSE_VERSION} extensions, using flags: ${SSE_FLAGS}")
else()
  message(STATUS "  No SSE support found")
  set(SSE_FLAGS "")
endif()

set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} ${SSE_FLAGS}")
set(CMAKE_CXX_FLAGS_RELWITHDEBINFO "${CMAKE_CXX_FLAGS_RELWITHDEBINFO} ${SSE_FLAGS}")
set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} ${SSE_FLAGS}")

return()
#-------------------------------------

# If no SSE support is found, print an error message.
if(SSE_FIND_VERSION)
  set(_SSE_ERROR_MESSAGE "SSE ${SSE_FIND_VERSION} support is not found on this architecture")
else()
  set(_SSE_ERROR_MESSAGE "SSE support is not found on this architecture")
endif()

if(SSE_FIND_REQUIRED)
  message(FATAL_ERROR "${_SSE_ERROR_MESSAGE}")
elseif(NOT SSE_FIND_QUIETLY)
  message(STATUS "${_SSE_ERROR_MESSAGE}")
endif()


<|start_filename|>tests/broad_phase/brute_force_comparison.cpp<|end_filename|>
#include <catch2/catch.hpp>

#include <tbb/parallel_sort.h>

#include <ipc/broad_phase/brute_force.hpp>

void brute_force_comparison(
    const Eigen::MatrixXd& V0,
    const Eigen::MatrixXd& V1,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    const Eigen::VectorXi& group_ids,
    ipc::Candidates& candidates,
    const double inflation_radius)
{
    using namespace ipc;

    Candidates bf_candidates;

    auto can_collide = [&group_ids](size_t vi, size_t vj) {
        return group_ids.size() == 0 || group_ids(vi) != group_ids(vj);
    };

    detect_collision_candidates_brute_force(
        V0, V1, E, F, bf_candidates,
        /*queryEV=*/true, /*queryEE=*/true, /*queryFV=*/true,
        /*perform_aabb_check=*/false, inflation_radius, can_collide);

    auto& ev_candidates = candidates.ev_candidates;
    auto& ee_candidates = candidates.ee_candidates;
    auto& fv_candidates = candidates.fv_candidates;
    auto& bf_ev_candidates = bf_candidates.ev_candidates;
    auto& bf_ee_candidates = bf_candidates.ee_candidates;
    auto& bf_fv_candidates = bf_candidates.fv_candidates;

    CHECK(ev_candidates.size() <= bf_ev_candidates.size());
    CHECK(ee_candidates.size() <= bf_ee_candidates.size());
    CHECK(fv_candidates.size() <= bf_fv_candidates.size());

    tbb::parallel_sort(ev_candidates.begin(), ev_candidates.end());
    tbb::parallel_sort(bf_ev_candidates.begin(), bf_ev_candidates.end());
    int ci = 0;
    for (int bf_ci = 0; bf_ci < bf_ev_candidates.size(); bf_ci++) {
        if (ev_candidates.size() <= ci
            || bf_ev_candidates[bf_ci] != ev_candidates[ci]) {
            // Perform CCD to make sure the candidate is not a collision
            double toi;
            bool hit = bf_ev_candidates[bf_ci].ccd(
                V0, V1, E, F, toi, /*tmax=*/1.0, /*tolerance=*/1e-6,
                /*max_iterations=*/1e7, /*conservative_rescaling=*/1.0);
            CHECK(!hit); // Check for FN

        } else {
            ci++;
        }
    }
    CHECK(ci >= ev_candidates.size());

    tbb::parallel_sort(ee_candidates.begin(), ee_candidates.end());
    tbb::parallel_sort(bf_ee_candidates.begin(), bf_ee_candidates.end());
    ci = 0;
    for (int bf_ci = 0; bf_ci < bf_ee_candidates.size(); bf_ci++) {
        if (ee_candidates.size() <= ci
            || bf_ee_candidates[bf_ci] != ee_candidates[ci]) {
            // Perform CCD to make sure the candidate is not a collision
            double toi;
            bool hit = bf_ee_candidates[bf_ci].ccd(
                V0, V1, E, F, toi, /*tmax=*/1.0, /*tolerance=*/1e-6,
                /*max_iterations=*/1e7, /*conservative_rescaling=*/1.0);
            CHECK(!hit); // Check for FN

        } else {
            ci++;
        }
    }
    CHECK(ci >= ee_candidates.size());

    tbb::parallel_sort(fv_candidates.begin(), fv_candidates.end());
    tbb::parallel_sort(bf_fv_candidates.begin(), bf_fv_candidates.end());
    ci = 0;
    for (int bf_ci = 0; bf_ci < bf_fv_candidates.size(); bf_ci++) {
        if (fv_candidates.size() <= ci
            || bf_fv_candidates[bf_ci] != fv_candidates[ci]) {
            // Perform CCD to make sure the candidate is not a collision
            double toi;
            bool hit = bf_fv_candidates[bf_ci].ccd(
                V0, V1, E, F, toi, /*tmax=*/1.0, /*tolerance=*/1e-6,
                /*max_iterations=*/1e7, /*conservative_rescaling=*/1.0);
            CHECK(!hit); // Check for FN
        } else {
            ci++;
        }
    }
    CHECK(ci >= fv_candidates.size());
}


<|start_filename|>src/distance/point_plane.hpp<|end_filename|>
#pragma once

#include <Eigen/Core>
#include <Eigen/Geometry>

#include <ipc/utils/eigen_ext.hpp>

namespace ipc {

/// @brief Compute the distance between a point and a plane.
/// @note The distance is actually squared distance.
/// @param p The point.
/// @param origin The origin of the plane.
/// @param normal The normal of the plane.
/// @return The distance between the point and plane.
template <typename DerivedP, typename DerivedOrigin, typename DerivedNormal>
auto point_plane_distance(
    const Eigen::MatrixBase<DerivedP>& p,
    const Eigen::MatrixBase<DerivedOrigin>& origin,
    const Eigen::MatrixBase<DerivedNormal>& normal)
{
    auto point_to_plane = (p - origin).dot(normal);
    return point_to_plane * point_to_plane / normal.squaredNorm();
}

/// @brief Compute the distance between a point and a plane.
/// @note The distance is actually squared distance.
/// @param p The point.
/// @param t0 The first vertex of the triangle.
/// @param t1 The second vertex of the triangle.
/// @param t2 The third vertex of the triangle.
/// @return The distance between the point and plane.
template <
    typename DerivedP,
    typename DerivedT0,
    typename DerivedT1,
    typename DerivedT2>
auto point_plane_distance(
    const Eigen::MatrixBase<DerivedP>& p,
    const Eigen::MatrixBase<DerivedT0>& t0,
    const Eigen::MatrixBase<DerivedT1>& t1,
    const Eigen::MatrixBase<DerivedT2>& t2)
{
    assert(p.size() == 3);
    assert(t0.size() == 3);
    assert(t1.size() == 3);
    assert(t2.size() == 3);

    auto normal = cross(t1 - t0, t2 - t0);
    return point_plane_distance(p, t0, normal);
}

// Symbolically generated derivatives;
namespace autogen {
    void point_plane_distance_gradient(
        double v01,
        double v02,
        double v03,
        double v11,
        double v12,
        double v13,
        double v21,
        double v22,
        double v23,
        double v31,
        double v32,
        double v33,
        double g[12]);

    void point_plane_distance_hessian(
        double v01,
        double v02,
        double v03,
        double v11,
        double v12,
        double v13,
        double v21,
        double v22,
        double v23,
        double v31,
        double v32,
        double v33,
        double H[144]);
} // namespace autogen

/// @brief Compute the gradient of the distance between a point and a plane.
/// @note The distance is actually squared distance.
/// @param[in] p The point.
/// @param[in] origin The origin of the plane.
/// @param[in] normal The normal of the plane.
/// @param[out] grad The gradient of the distance wrt p.
template <
    typename DerivedP,
    typename DerivedOrigin,
    typename DerivedNormal,
    typename DerivedGrad>
void point_plane_distance_gradient(
    const Eigen::MatrixBase<DerivedP>& p,
    const Eigen::MatrixBase<DerivedOrigin>& origin,
    const Eigen::MatrixBase<DerivedNormal>& normal,
    Eigen::PlainObjectBase<DerivedGrad>& grad)
{
    grad = (2 * (p - origin).dot(normal)) / normal.squaredNorm() * normal;
}

/// @brief Compute the gradient of the distance between a point and a plane.
/// @note The distance is actually squared distance.
/// @param[in] p The point.
/// @param[in] t0 The first vertex of the triangle.
/// @param[in] t1 The second vertex of the triangle.
/// @param[in] t2 The third vertex of the triangle.
/// @param[out] grad The gradient of the distance wrt p, t0, t1, and t2.
template <
    typename DerivedP,
    typename DerivedT0,
    typename DerivedT1,
    typename DerivedT2,
    typename DerivedGrad>
void point_plane_distance_gradient(
    const Eigen::MatrixBase<DerivedP>& p,
    const Eigen::MatrixBase<DerivedT0>& t0,
    const Eigen::MatrixBase<DerivedT1>& t1,
    const Eigen::MatrixBase<DerivedT2>& t2,
    Eigen::PlainObjectBase<DerivedGrad>& grad)
{
    assert(p.size() == 3);
    assert(t0.size() == 3);
    assert(t1.size() == 3);
    assert(t2.size() == 3);

    grad.resize(p.size() + t0.size() + t1.size() + t2.size());
    autogen::point_plane_distance_gradient(
        p[0], p[1], p[2], t0[0], t0[1], t0[2], t1[0], t1[1], t1[2], t2[0],
        t2[1], t2[2], grad.data());
}

/// @brief Compute the hessian of the distance between a point and a plane.
/// @note The distance is actually squared distance.
/// @param[in] p The point.
/// @param[in] origin The origin of the plane.
/// @param[in] normal The normal of the plane.
/// @param[out] hess The hessian of the distance wrt p.
template <
    typename DerivedP,
    typename DerivedOrigin,
    typename DerivedNormal,
    typename DerivedHess>
void point_plane_distance_hessian(
    const Eigen::MatrixBase<DerivedP>& p,
    const Eigen::MatrixBase<DerivedOrigin>& origin,
    const Eigen::MatrixBase<DerivedNormal>& normal,
    Eigen::PlainObjectBase<DerivedHess>& hess)
{
    if (normal.cols() == 1) {
        // (n×1)(n×1)ᵀ = (n×n)
        hess = 2 / normal.squaredNorm() * normal * normal.transpose();
    } else {
        assert(normal.rows() == 1);
        // (1×n)ᵀ(1×n) = (n×n)
        hess = 2 / normal.squaredNorm() * normal.transpose() * normal;
    }
}

/// @brief Compute the hessian of the distance between a point and a plane.
/// @note The distance is actually squared distance.
/// @param[in] p The point.
/// @param[in] t0 The first vertex of the triangle.
/// @param[in] t1 The second vertex of the triangle.
/// @param[in] t2 The third vertex of the triangle.
/// @param[out] hess The hessian of the distance wrt p, t0, t1, and t2.
template <
    typename DerivedP,
    typename DerivedT0,
    typename DerivedT1,
    typename DerivedT2,
    typename DerivedHess>
void point_plane_distance_hessian(
    const Eigen::MatrixBase<DerivedP>& p,
    const Eigen::MatrixBase<DerivedT0>& t0,
    const Eigen::MatrixBase<DerivedT1>& t1,
    const Eigen::MatrixBase<DerivedT2>& t2,
    Eigen::PlainObjectBase<DerivedHess>& hess)
{
    assert(p.size() == 3);
    assert(t0.size() == 3);
    assert(t1.size() == 3);
    assert(t2.size() == 3);

    hess.resize(
        p.size() + t0.size() + t1.size() + t2.size(),
        p.size() + t0.size() + t1.size() + t2.size());
    autogen::point_plane_distance_hessian(
        p[0], p[1], p[2], t0[0], t0[1], t0[2], t1[0], t1[1], t1[2], t2[0],
        t2[1], t2[2], hess.data());
}

} // namespace ipc


<|start_filename|>src/implicits/plane.cpp<|end_filename|>
#include <ipc/implicits/plane.hpp>

#include <ipc/distance/point_plane.hpp>
#include <ipc/ccd/point_static_plane.hpp>

#include <tbb/parallel_for.h>
#include <tbb/blocked_range.h>
#include <tbb/enumerable_thread_specific.h>

namespace ipc {

void construct_point_plane_constraint_set(
    const Eigen::MatrixXd& V,
    const Eigen::MatrixXd& plane_origins,
    const Eigen::MatrixXd& plane_normals,
    const double dhat,
    std::vector<PlaneVertexConstraint>& pv_constraints,
    const double dmin,
    const std::function<bool(size_t, size_t)>& can_collide)
{
    pv_constraints.clear();

    double dhat_squared = dhat * dhat;
    double dmin_squared = dmin * dmin;

    // Cull the candidates by measuring the distance and dropping those that are
    // greater than dhat.

    size_t n_planes = plane_origins.rows();
    assert(plane_normals.rows() == n_planes);

    for (size_t vi = 0; vi < V.rows(); vi++) {
        for (size_t pi = 0; pi < n_planes; pi++) {
            if (!can_collide(vi, pi)) {
                continue;
            }

            const auto& plane_origin = plane_origins.row(pi);
            const auto& plane_normal = plane_normals.row(pi);

            double distance_sqr =
                point_plane_distance(V.row(vi), plane_origin, plane_normal);

            if (distance_sqr - dmin_squared < 2 * dmin * dhat + dhat_squared) {
                pv_constraints.emplace_back(plane_origin, plane_normal, vi);
                pv_constraints.back().minimum_distance = dmin;
            }
        }
    }
}

///////////////////////////////////////////////////////////////////////////////

bool is_step_point_plane_collision_free(
    const Eigen::MatrixXd& V0,
    const Eigen::MatrixXd& V1,
    const Eigen::MatrixXd& plane_origins,
    const Eigen::MatrixXd& plane_normals,
    const std::function<bool(size_t, size_t)>& can_collide)
{
    size_t n_planes = plane_origins.rows();
    assert(plane_normals.rows() == n_planes);
    assert(V0.rows() == V1.rows());

    for (size_t vi = 0; vi < V0.rows(); vi++) {
        for (size_t pi = 0; pi < n_planes; pi++) {
            if (!can_collide(vi, pi)) {
                continue;
            }

            const auto& plane_origin = plane_origins.row(pi);
            const auto& plane_normal = plane_normals.row(pi);

            double toi;
            bool is_collision = point_static_plane_ccd(
                V0.row(vi), V1.row(vi), plane_origin, plane_normal, toi);

            if (is_collision) {
                return false;
            }
        }
    }

    return true;
}

///////////////////////////////////////////////////////////////////////////////

bool compute_point_plane_collision_free_stepsize(
    const Eigen::MatrixXd& V0,
    const Eigen::MatrixXd& V1,
    const Eigen::MatrixXd& plane_origins,
    const Eigen::MatrixXd& plane_normals,
    const std::function<bool(size_t, size_t)>& can_collide)
{
    size_t n_planes = plane_origins.rows();
    assert(plane_normals.rows() == n_planes);
    assert(V0.rows() == V1.rows());

    tbb::enumerable_thread_specific<double> storage(1);

    // Do a single block range over all three candidate vectors
    tbb::parallel_for(
        tbb::blocked_range<size_t>(0, V0.rows()),
        [&](tbb::blocked_range<size_t> r) {
            double& earliest_toi = storage.local();

            for (size_t vi = r.begin(); vi < r.end(); vi++) {
                for (size_t pi = 0; pi < n_planes; pi++) {
                    if (!can_collide(vi, pi)) {
                        continue;
                    }

                    const auto& plane_origin = plane_origins.row(pi);
                    const auto& plane_normal = plane_normals.row(pi);

                    double toi;
                    bool are_colliding = point_static_plane_ccd(
                        V0.row(vi), V1.row(vi), plane_origin, plane_normal,
                        toi);

                    if (are_colliding) {
                        if (toi < earliest_toi) {
                            earliest_toi = toi;
                        }
                    }
                }
            }
        });

    double earliest_toi = 1;
    for (const auto& local_earliest_toi : storage) {
        earliest_toi = std::min(earliest_toi, local_earliest_toi);
    }
    assert(earliest_toi >= 0 && earliest_toi <= 1.0);
    return earliest_toi;
}

} // namespace ipc


<|start_filename|>python/src/utils/thread_limiter.cpp<|end_filename|>
#include <pybind11/pybind11.h>

#include <memory>

#include <tbb/global_control.h>
#include <tbb/task_scheduler_init.h>

#include <ipc/utils/logger.hpp>

namespace py = pybind11;

static std::shared_ptr<tbb::global_control> thread_limiter;

int get_num_threads()
{
    return tbb::global_control::active_value(
        tbb::global_control::max_allowed_parallelism);
}

void set_num_threads(int nthreads)
{
    if (nthreads <= 0) {
        nthreads = tbb::task_scheduler_init::default_num_threads();
    } else if (nthreads > tbb::task_scheduler_init::default_num_threads()) {
        IPC_LOG(warn(
            "Attempting to use more threads than available ({:d} > {:d})!",
            nthreads, tbb::task_scheduler_init::default_num_threads()));
        nthreads = tbb::task_scheduler_init::default_num_threads();
    }
    thread_limiter = std::make_shared<tbb::global_control>(
        tbb::global_control::max_allowed_parallelism, nthreads);
}

void define_thread_limiter_functions(py::module_& m)
{
    m.def(
        "get_num_threads", &get_num_threads,
        "get maximum number of threads to use");
    m.def(
        "set_num_threads", &set_num_threads,
        "set maximum number of threads to use", py::arg("nthreads"));
}


<|start_filename|>tests/ccd/collision_generator.cpp<|end_filename|>
#include "collision_generator.hpp"

#include <iomanip>
#include <stdlib.h> /* rand */

TestImpactGenerator::TestImpactGenerator(size_t value, bool rigid)
    : rigid(rigid)
    , current_i(0)
    , max_i(value)
{
    current = generate_random_impact(rigid);
}

bool TestImpactGenerator::next()
{
    current = generate_random_impact(rigid);
    return ++current_i < max_i;
}

TestImpact const& TestImpactGenerator::get() const { return current; }

Catch::Generators::GeneratorWrapper<TestImpact>
random_impacts(size_t value, bool rigid)
{
    return Catch::Generators::GeneratorWrapper<TestImpact>(
        std::make_unique<TestImpactGenerator>(value, rigid));
}

TestImpact generate_random_impact(const bool rigid)
{
    static const double kEPSILON = 1E-8;

    const auto rand_vec2 = []() {
        static const double kSCALE = 10.0;
        return kSCALE * Eigen::Vector2d::Random();
    };

    TestImpact impact;

    // set first edge random positions keeping in larger than kEPSILON
    impact.e0_t0 = rand_vec2();
    impact.e1_t0 = rand_vec2();

    Eigen::Vector2d e_dir = impact.e1_t0 - impact.e0_t0;
    double e_len = e_dir.norm();
    if (e_len < kEPSILON) {
        impact.e1_t0 += (e_dir) / e_len * kEPSILON;
        assert((impact.e1_t0 - impact.e0_t0).norm() >= kEPSILON);
    }

    // set first edge random velocities keeping in larger than kEPSILON.
    // note that this doest NOT ensure its length is > 0 over the full
    // time.
    impact.de0 = rand_vec2();
    if (rigid) {
        impact.de1 = impact.de0;
    } else {
        impact.de1 = rand_vec2();
        e_dir = (impact.e1_t0 + impact.de1) - (impact.e0_t0 + impact.de0);
        e_len = e_dir.norm();
        if (e_len < kEPSILON) {
            impact.de1 += (e_dir) / e_len * kEPSILON;
            assert(
                ((impact.e1_t0 + impact.de1) - (impact.e0_t0 + impact.de0))
                    .norm()
                >= kEPSILON);
        }
    }

    // Generate a time and position for the impact.
    // How do we ensure it is the first time of impact?
    impact.toi = double(rand()) / RAND_MAX;
    Eigen::Vector2d e0_t0_toi = impact.e0_t0 + impact.de0 * impact.toi;
    Eigen::Vector2d e1_t0_toi = impact.e1_t0 + impact.de1 * impact.toi;
    double alpha = double(rand()) / RAND_MAX;
    Eigen::Vector2d pc = e0_t0_toi + alpha * (e1_t0_toi - e0_t0_toi);

    // pc = p_t0 + dp * toi → (pc - p_t0)/toi = dp
    impact.p_t0 = rand_vec2();
    impact.dp = (pc - impact.p_t0) / impact.toi;

    return impact;
}


<|start_filename|>tests/ccd/benchmark_ccd.cpp<|end_filename|>
#include <catch2/catch.hpp>

#include <Eigen/Core>

#include <igl/IO>
#include <igl/edges.h>
#include <igl/Timer.h>

#include <ipc/ipc.hpp>

using namespace ipc;

TEST_CASE("Benchmark earliest toi", "[!benchmark][ccd][earliest_toi]")
{
    Eigen::MatrixXd V0, V1;
    Eigen::MatrixXi E, F;
    Eigen::VectorXi group_ids;

    std::string mesh_path =
        std::string(TEST_DATA_DIR) + "slow-broadphase-ccd/s0.obj";
    bool success = igl::read_triangle_mesh(mesh_path, V0, F);
    if (!success) {
        return; // Data is private
    }

    mesh_path = std::string(TEST_DATA_DIR) + "slow-broadphase-ccd/s1.obj";
    success = igl::read_triangle_mesh(mesh_path, V1, F);
    if (!success) {
        return; // Data is private
    }

    igl::edges(F, E);

    BroadPhaseMethod method = GENERATE(
        BroadPhaseMethod::BRUTE_FORCE, BroadPhaseMethod::HASH_GRID
        //,BroadPhaseMethod::SPATIAL_HASH
    );

    double tolerance = 1e-6;
    int max_iterations = 1e7;

    BENCHMARK("Earliest ToI")
    {
        double stpesize = compute_collision_free_stepsize(
            V0, V1, /*codim_V=*/Eigen::VectorXi(), E, F, method, tolerance,
            max_iterations);
    };
}


<|start_filename|>src/broad_phase/collision_candidate.cpp<|end_filename|>
#include <ipc/broad_phase/collision_candidate.hpp>

#include <algorithm> // std::min/max

#include <ipc/utils/logger.hpp>

namespace ipc {

VertexVertexCandidate::VertexVertexCandidate(
    long vertex0_index, long vertex1_index)
    : vertex0_index(vertex0_index)
    , vertex1_index(vertex1_index)
{
}

bool VertexVertexCandidate::operator==(const VertexVertexCandidate& other) const
{
    return vertex0_index == other.vertex0_index
        && vertex1_index == other.vertex1_index;
}

bool VertexVertexCandidate::operator<(const VertexVertexCandidate& other) const
{
    if (vertex0_index == other.vertex0_index) {
        return vertex1_index < other.vertex1_index;
    }
    return vertex0_index < other.vertex0_index;
}

///////////////////////////////////////////////////////////////////////////////

EdgeVertexCandidate::EdgeVertexCandidate(long edge_index, long vertex_index)
    : edge_index(edge_index)
    , vertex_index(vertex_index)
{
}

bool EdgeVertexCandidate::operator==(const EdgeVertexCandidate& other) const
{
    return edge_index == other.edge_index && vertex_index == other.vertex_index;
}

bool EdgeVertexCandidate::operator<(const EdgeVertexCandidate& other) const
{
    if (edge_index == other.edge_index) {
        return vertex_index < other.vertex_index;
    }
    return edge_index < other.edge_index;
}

bool EdgeVertexCandidate::ccd(
    const Eigen::MatrixXd& V0,
    const Eigen::MatrixXd& V1,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    double& toi,
    const double tmax,
    const double tolerance,
    const long max_iterations,
    const double conservative_rescaling) const
{
    return point_edge_ccd(
        // Point at t=0
        V0.row(vertex_index),
        // Edge at t=0
        V0.row(E(edge_index, 0)), V0.row(E(edge_index, 1)),
        // Point at t=1
        V1.row(vertex_index),
        // Edge at t=1
        V1.row(E(edge_index, 0)), V1.row(E(edge_index, 1)), //
        toi, tmax, tolerance, max_iterations, conservative_rescaling);
}

///////////////////////////////////////////////////////////////////////////////

EdgeEdgeCandidate::EdgeEdgeCandidate(long edge0_index, long edge1_index)
    : edge0_index(edge0_index)
    , edge1_index(edge1_index)
{
}

bool EdgeEdgeCandidate::operator==(const EdgeEdgeCandidate& other) const
{
    // (i, j) == (i, j) || (i, j) == (j, i)
    return (this->edge0_index == other.edge0_index
            && this->edge1_index == other.edge1_index)
        || (this->edge0_index == other.edge1_index
            && this->edge1_index == other.edge0_index);
}

bool EdgeEdgeCandidate::operator<(const EdgeEdgeCandidate& other) const
{
    long this_min = std::min(this->edge0_index, this->edge1_index);
    long other_min = std::min(other.edge0_index, other.edge1_index);
    if (this_min == other_min) {
        return std::max(this->edge0_index, this->edge1_index)
            < std::max(other.edge0_index, other.edge1_index);
    }
    return this_min < other_min;
}

bool EdgeEdgeCandidate::ccd(
    const Eigen::MatrixXd& V0,
    const Eigen::MatrixXd& V1,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    double& toi,
    const double tmax,
    const double tolerance,
    const long max_iterations,
    const double conservative_rescaling) const
{
    return edge_edge_ccd(
        // Edge 1 at t=0
        V0.row(E(edge0_index, 0)), V0.row(E(edge0_index, 1)),
        // Edge 2 at t=0
        V0.row(E(edge1_index, 0)), V0.row(E(edge1_index, 1)),
        // Edge 1 at t=1
        V1.row(E(edge0_index, 0)), V1.row(E(edge0_index, 1)),
        // Edge 2 at t=1
        V1.row(E(edge1_index, 0)), V1.row(E(edge1_index, 1)), //
        toi, tmax, tolerance, max_iterations, conservative_rescaling);
}

///////////////////////////////////////////////////////////////////////////////

EdgeFaceCandidate::EdgeFaceCandidate(long edge_index, long face_index)
    : edge_index(edge_index)
    , face_index(face_index)
{
}

bool EdgeFaceCandidate::operator==(const EdgeFaceCandidate& other) const
{
    return edge_index == other.edge_index && face_index == other.face_index;
}

bool EdgeFaceCandidate::operator<(const EdgeFaceCandidate& other) const
{
    if (edge_index == other.edge_index) {
        return face_index < other.face_index;
    }
    return edge_index < other.edge_index;
}

///////////////////////////////////////////////////////////////////////////////

FaceVertexCandidate::FaceVertexCandidate(long face_index, long vertex_index)
    : face_index(face_index)
    , vertex_index(vertex_index)
{
}

bool FaceVertexCandidate::operator==(const FaceVertexCandidate& other) const
{
    return face_index == other.face_index && vertex_index == other.vertex_index;
}

bool FaceVertexCandidate::operator<(const FaceVertexCandidate& other) const
{
    if (face_index == other.face_index) {
        return vertex_index < other.vertex_index;
    }
    return face_index < other.face_index;
}

bool FaceVertexCandidate::ccd(
    const Eigen::MatrixXd& V0,
    const Eigen::MatrixXd& V1,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    double& toi,
    const double tmax,
    const double tolerance,
    const long max_iterations,
    const double conservative_rescaling) const
{
    return point_triangle_ccd(
        // Point at t=0
        V0.row(vertex_index),
        // Triangle at t=0
        V0.row(F(face_index, 0)), V0.row(F(face_index, 1)),
        V0.row(F(face_index, 2)),
        // Point at t=1
        V1.row(vertex_index),
        // Triangle at t=1
        V1.row(F(face_index, 0)), V1.row(F(face_index, 1)),
        V1.row(F(face_index, 2)), //
        toi, tmax, tolerance, max_iterations, conservative_rescaling);
}

///////////////////////////////////////////////////////////////////////////////

size_t Candidates::size() const
{
    return ev_candidates.size() + ee_candidates.size() + fv_candidates.size();
}

bool Candidates::empty() const
{
    return ev_candidates.empty() && ee_candidates.empty()
        && fv_candidates.empty();
}

void Candidates::clear()
{
    ev_candidates.clear();
    ee_candidates.clear();
    fv_candidates.clear();
}

ContinuousCollisionCandidate& Candidates::operator[](size_t idx)
{
    if (idx < 0) {
        throw std::out_of_range("Constraint index is out of range!");
    }
    if (idx < ev_candidates.size()) {
        return ev_candidates[idx];
    }
    idx -= ev_candidates.size();
    if (idx < ee_candidates.size()) {
        return ee_candidates[idx];
    }
    idx -= ee_candidates.size();
    if (idx < fv_candidates.size()) {
        return fv_candidates[idx];
    }
    throw std::out_of_range("Constraint index is out of range!");
}

const ContinuousCollisionCandidate& Candidates::operator[](size_t idx) const
{
    if (idx < 0) {
        throw std::out_of_range("Constraint index is out of range!");
    }
    if (idx < ev_candidates.size()) {
        return ev_candidates[idx];
    }
    idx -= ev_candidates.size();
    if (idx < ee_candidates.size()) {
        return ee_candidates[idx];
    }
    idx -= ee_candidates.size();
    if (idx < fv_candidates.size()) {
        return fv_candidates[idx];
    }
    throw std::out_of_range("Constraint index is out of range!");
}

bool Candidates::save_obj(
    const std::string& filename,
    const Eigen::MatrixXd& V,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F)
{
    std::ofstream obj(filename, std::ios::out);
    if (!obj.is_open()) {
        return false;
    }
    ipc::save_obj(obj, V, E, F, ev_candidates);
    ipc::save_obj(obj, V, E, F, ee_candidates);
    ipc::save_obj(obj, V, F, F, fv_candidates);
    return true;
}

///////////////////////////////////////////////////////////////////////////////

void save_obj(
    std::ofstream& out,
    const Eigen::MatrixXd& V,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    const std::vector<EdgeVertexCandidate>& ev_candidates)
{
    static const Eigen::IOFormat OBJ_VERTEX_FORMAT(
        Eigen::FullPrecision, Eigen::DontAlignCols, " ", "", "v ", "\n", "",
        "");
    out << "o EV\n";
    int i = 1;
    for (const auto& ev_candidate : ev_candidates) {
        out << V.row(E(ev_candidate.edge_index, 0)).format(OBJ_VERTEX_FORMAT);
        out << V.row(E(ev_candidate.edge_index, 1)).format(OBJ_VERTEX_FORMAT);
        out << V.row(ev_candidate.vertex_index).format(OBJ_VERTEX_FORMAT);
        out << fmt::format("l {:d} {:d}\n", i, i + 1);
        i += 3;
    }
}

void save_obj(
    std::ofstream& out,
    const Eigen::MatrixXd& V,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    const std::vector<EdgeEdgeCandidate>& ee_candidates)
{
    static const Eigen::IOFormat OBJ_VERTEX_FORMAT(
        Eigen::FullPrecision, Eigen::DontAlignCols, " ", "", "v ", "\n", "",
        "");
    out << "o EE\n";
    int i = 1;
    for (const auto& ee_candidate : ee_candidates) {
        out << V.row(E(ee_candidate.edge0_index, 0)).format(OBJ_VERTEX_FORMAT);
        out << V.row(E(ee_candidate.edge0_index, 1)).format(OBJ_VERTEX_FORMAT);
        out << V.row(E(ee_candidate.edge1_index, 0)).format(OBJ_VERTEX_FORMAT);
        out << V.row(E(ee_candidate.edge1_index, 1)).format(OBJ_VERTEX_FORMAT);
        out << fmt::format("l {:d} {:d}\n", i + 0, i + 1);
        out << fmt::format("l {:d} {:d}\n", i + 2, i + 3);
        i += 4;
    }
}

void save_obj(
    std::ofstream& out,
    const Eigen::MatrixXd& V,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    const std::vector<FaceVertexCandidate>& fv_candidates)
{
    static const Eigen::IOFormat OBJ_VERTEX_FORMAT(
        Eigen::FullPrecision, Eigen::DontAlignCols, " ", "", "v ", "\n", "",
        "");
    out << "o FV\n";
    int i = 1;
    for (const auto& fv_candidate : fv_candidates) {
        out << V.row(F(fv_candidate.face_index, 0)).format(OBJ_VERTEX_FORMAT);
        out << V.row(F(fv_candidate.face_index, 1)).format(OBJ_VERTEX_FORMAT);
        out << V.row(F(fv_candidate.face_index, 2)).format(OBJ_VERTEX_FORMAT);
        out << V.row(fv_candidate.vertex_index).format(OBJ_VERTEX_FORMAT);
        out << fmt::format("f {:d} {:d} {:d}\n", i, i + 1, i + 2);
        i += 4;
    }
}

void save_obj(
    std::ofstream& out,
    const Eigen::MatrixXd& V,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    const std::vector<EdgeFaceCandidate>& ef_candidates)
{
    static const Eigen::IOFormat OBJ_VERTEX_FORMAT(
        Eigen::FullPrecision, Eigen::DontAlignCols, " ", "", "v ", "\n", "",
        "");
    out << "o EF\n";
    int i = 1;
    for (const auto& ef_candidate : ef_candidates) {
        out << V.row(E(ef_candidate.edge_index, 0)).format(OBJ_VERTEX_FORMAT);
        out << V.row(E(ef_candidate.edge_index, 1)).format(OBJ_VERTEX_FORMAT);
        out << V.row(F(ef_candidate.face_index, 0)).format(OBJ_VERTEX_FORMAT);
        out << V.row(F(ef_candidate.face_index, 1)).format(OBJ_VERTEX_FORMAT);
        out << V.row(F(ef_candidate.face_index, 2)).format(OBJ_VERTEX_FORMAT);
        out << fmt::format("l {:d} {:d}\n", i, i + 1);
        out << fmt::format("f {:d} {:d} {:d}\n", i + 2, i + 3, i + 4);
        i += 5;
    }
}

} // namespace ipc


<|start_filename|>python/src/distance/edge_edge.cpp<|end_filename|>
#include <pybind11/pybind11.h>
#include <pybind11/eigen.h>

#include <ipc/distance/distance_type.hpp>
#include <ipc/distance/edge_edge.hpp>

#include "../utils.hpp"

namespace py = pybind11;
using namespace ipc;

void define_edge_edge_distance_functions(py::module_& m)
{
    m.def(
        "edge_edge_distance",
        [](const Eigen::Vector3d& ea0, const Eigen::Vector3d& ea1,
           const Eigen::Vector3d& eb0, const Eigen::Vector3d& eb1,
           const EdgeEdgeDistanceType* dtype) {
            if (dtype == nullptr) {
                return edge_edge_distance(ea0, ea1, eb0, eb1);
            } else {
                return edge_edge_distance(ea0, ea1, eb0, eb1, *dtype);
            }
        },
        R"ipc_Qu8mg5v7(
        Compute the distance between a two lines segments in 3D.

        Parameters:
            ea0: first vertex of the first edge
            ea1: second vertex of the first edge
            eb0: first vertex of the second edge
            eb1: second vertex of the second edge
            dtype: (optional) edge-edge distance type to compute

        Returns:
            The distance between the two edges.

        Note:
            The distance is actually squared distance.
        )ipc_Qu8mg5v7",
        py::arg("ea0"), py::arg("ea1"), py::arg("eb0"), py::arg("eb1"),
        py::arg("dtype") = py::none());

    m.def(
        "edge_edge_distance_gradient",
        [](const Eigen::Vector3d& ea0, const Eigen::Vector3d& ea1,
           const Eigen::Vector3d& eb0, const Eigen::Vector3d& eb1,
           const EdgeEdgeDistanceType* dtype) {
            Vector<double, 12> grad;
            if (dtype == nullptr) {
                edge_edge_distance_gradient(ea0, ea1, eb0, eb1, grad);
            } else {
                edge_edge_distance_gradient(ea0, ea1, eb0, eb1, *dtype, grad);
            }
            return grad;
        },
        R"ipc_Qu8mg5v7(
        Compute the gradient of the distance between a two lines segments.

        Parameters:
            ea0: first vertex of the first edge
            ea1: second vertex of the first edge
            eb0: first vertex of the second edge
            eb1: second vertex of the second edge
            dtype: (optional) point edge distance type to compute

        Returns:
            The gradient of the distance wrt ea0, ea1, eb0, and eb1.

        Note:
            The distance is actually squared distance.
        )ipc_Qu8mg5v7",
        py::arg("ea0"), py::arg("ea1"), py::arg("eb0"), py::arg("eb1"),
        py::arg("dtype") = py::none());

    m.def(
        "edge_edge_distance_hessian",
        [](const Eigen::Vector3d& ea0, const Eigen::Vector3d& ea1,
           const Eigen::Vector3d& eb0, const Eigen::Vector3d& eb1,
           const EdgeEdgeDistanceType* dtype) {
            Eigen::Matrix<double, 12, 12> hess;
            if (dtype == nullptr) {
                edge_edge_distance_hessian(ea0, ea1, eb0, eb1, hess);
            } else {
                edge_edge_distance_hessian(ea0, ea1, eb0, eb1, *dtype, hess);
            }
            return hess;
        },
        R"ipc_Qu8mg5v7(
        Compute the hessian of the distance between a two lines segments.

        Parameters:
            ea0: first vertex of the first edge
            ea1: second vertex of the first edge
            eb0: first vertex of the second edge
            eb1: second vertex of the second edge
            dtype: (optional) point edge distance type to compute

        Returns:
            The hessian of the distance wrt ea0, ea1, eb0, and eb1.

        Note:
            The distance is actually squared distance.
        )ipc_Qu8mg5v7",
        py::arg("ea0"), py::arg("ea1"), py::arg("eb0"), py::arg("eb1"),
        py::arg("dtype") = py::none());
}


<|start_filename|>src/broad_phase/hash_grid.hpp<|end_filename|>
#pragma once

#include <vector>

#include <Eigen/Core>

#include <ipc/broad_phase/collision_candidate.hpp>
#include <ipc/utils/eigen_ext.hpp>

namespace ipc {

/// @brief Axis aligned bounding-box of some type
class AABB {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    AABB() { }

    AABB(const ArrayMax3d& min, const ArrayMax3d& max);

    AABB(const AABB& aabb1, const AABB& aabb2)
        : AABB(aabb1.min.min(aabb2.min), aabb1.max.max(aabb2.max))
    {
    }

    AABB(const AABB& aabb1, const AABB& aabb2, const AABB& aabb3)
        : AABB(
            aabb1.min.min(aabb2.min).min(aabb3.min),
            aabb1.max.max(aabb2.max).max(aabb3.max))
    {
    }

    static bool are_overlapping(const AABB& a, const AABB& b);

    inline const ArrayMax3d& getMin() const { return min; }
    inline const ArrayMax3d& getMax() const { return max; }
    // inline const ArrayMax3d& getHalfExtent() const { return half_extent; }
    // inline const ArrayMax3d& getCenter() const { return center; }

protected:
    ArrayMax3d min;
    ArrayMax3d max;
    // ArrayMax3d half_extent;
    // ArrayMax3d center;
};

/// @brief An entry into the hash grid as a (key, value) pair.
class HashItem {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    int key;   /// @brief The key of the item.
    int id;    /// @brief The value of the item.
    AABB aabb; /// @brief The axis-aligned bounding box of the element

    /// @brief Construct a hash item as a (key, value) pair.
    HashItem(int key, int id, const AABB& aabb)
        : key(key)
        , id(id)
        , aabb(aabb)
    {
    }

    /// @brief Compare HashItems by their keys for sorting.
    bool operator<(const HashItem& other) const
    {
        if (key == other.key) {
            return id < other.id;
        }
        return key < other.key;
    }
};

class HashGrid {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    double cellSize() const { return m_cellSize; }
    const ArrayMax3i& gridSize() const { return m_gridSize; }
    const ArrayMax3d& domainMin() const { return m_domainMin; }
    const ArrayMax3d& domainMax() const { return m_domainMax; }

    void resizeFromBox(
        const ArrayMax3d& min, const ArrayMax3d& max, double cellSize);

    void resize(
        const Eigen::MatrixXd& vertices_t0,
        const Eigen::MatrixXd& vertices_t1,
        const Eigen::MatrixXi& edges,
        const double inflation_radius = 0.0);

    void resize(
        const Eigen::MatrixXd& vertices,
        const Eigen::MatrixXi& edges,
        const double inflation_radius = 0.0)
    {
        resize(vertices, vertices, edges, inflation_radius);
    }

    /// @brief Add a vertex as a AABB containing the time swept edge.
    void addVertex(
        const VectorMax3d& vertex_t0,
        const VectorMax3d& vertex_t1,
        const long index,
        const double inflation_radius = 0.0);

    /// @brief Add a vertex.
    void addVertex(
        const VectorMax3d& vertex,
        const long index,
        const double inflation_radius = 0.0)
    {
        addVertex(vertex, vertex, index, inflation_radius);
    }

    /// @brief Add all vertices as AABBs containing the time swept edge.
    void addVertices(
        const Eigen::MatrixXd& vertices_t0,
        const Eigen::MatrixXd& vertices_t1,
        const double inflation_radius = 0.0);

    /// @brief Add all vertices.
    void addVertices(
        const Eigen::MatrixXd& vertices, const double inflation_radius = 0.0)
    {
        addVertices(vertices, vertices, inflation_radius);
    }

    /// @brief Add all selected vertices as AABBs containing the time swept
    /// edge.
    void addSelectVertices(
        const Eigen::MatrixXd& vertices_t0,
        const Eigen::MatrixXd& vertices_t1,
        const Eigen::VectorXi& vertex_indices,
        const double inflation_radius = 0.0);

    /// @brief Add all selected vertices.
    void addSelectVertices(
        const Eigen::MatrixXd& vertices,
        const Eigen::VectorXi& vertex_indices,
        const double inflation_radius = 0.0)
    {
        addSelectVertices(vertices, vertices, vertex_indices, inflation_radius);
    }

    /// @brief Add all vertices as AABBs containing the time swept edge.
    void addVerticesFromEdges(
        const Eigen::MatrixXd& vertices_t0,
        const Eigen::MatrixXd& vertices_t1,
        const Eigen::MatrixXi& edges,
        const double inflation_radius = 0.0);

    /// @brief Add all vertices as AABBs containing the time swept edge.
    void addVerticesFromEdges(
        const Eigen::MatrixXd& vertices,
        const Eigen::MatrixXi& edges,
        const double inflation_radius = 0.0)
    {
        addVerticesFromEdges(vertices, vertices, edges, inflation_radius);
    }

    /// @brief Add an edge as a AABB containing the time swept quad.
    void addEdge(
        const VectorMax3d& edge_vertex0_t0,
        const VectorMax3d& edge_vertex1_t0,
        const VectorMax3d& edge_vertex0_t1,
        const VectorMax3d& edge_vertex1_t1,
        const long index,
        const double inflation_radius = 0.0);

    /// @brief Add an edge as a AABB.
    void addEdge(
        const VectorMax3d& edge_vertex0,
        const VectorMax3d& edge_vertex1,
        const long index,
        const double inflation_radius = 0.0)
    {
        addEdge(
            edge_vertex0, edge_vertex1, edge_vertex0, edge_vertex1, index,
            inflation_radius);
    }

    /// @brief Add all edges as AABBs containing the time swept quad.
    void addEdges(
        const Eigen::MatrixXd& vertices_t0,
        const Eigen::MatrixXd& vertices_t1,
        const Eigen::MatrixXi& edges,
        const double inflation_radius = 0.0);

    /// @brief Add all edges as AABBs.
    void addEdges(
        const Eigen::MatrixXd& vertices,
        const Eigen::MatrixXi& edges,
        const double inflation_radius = 0.0)
    {
        addEdges(vertices, vertices, edges, inflation_radius);
    }

    /// @brief Add an edge as a AABB containing the time swept prism.
    void addFace(
        const VectorMax3d& face_vertex0_t0,
        const VectorMax3d& face_vertex1_t0,
        const VectorMax3d& face_vertex2_t0,
        const VectorMax3d& face_vertex0_t1,
        const VectorMax3d& face_vertex1_t1,
        const VectorMax3d& face_vertex2_t1,
        const long index,
        const double inflation_radius = 0.0);

    /// @brief Add an edge as a AABB.
    void addFace(
        const VectorMax3d& face_vertex0,
        const VectorMax3d& face_vertex1,
        const VectorMax3d& face_vertex2,
        const long index,
        const double inflation_radius = 0.0)
    {
        addFace(
            face_vertex0, face_vertex1, face_vertex2, face_vertex0,
            face_vertex1, face_vertex2, index, inflation_radius);
    }

    /// @brief Add all edges as AABBs containing the time swept prism.
    void addFaces(
        const Eigen::MatrixXd& vertices_t0,
        const Eigen::MatrixXd& vertices_t1,
        const Eigen::MatrixXi& faces,
        const double inflation_radius = 0.0);

    /// @brief Add all edges as AABBs.
    void addFaces(
        const Eigen::MatrixXd& vertices,
        const Eigen::MatrixXi& faces,
        const double inflation_radius = 0.0)
    {
        addFaces(vertices, vertices, faces, inflation_radius);
    }

    /// @brief Compute the candidate edge-vertex candidate collisisons.
    void getVertexEdgePairs(
        const Eigen::MatrixXi& edges,
        std::vector<EdgeVertexCandidate>& ev_candidates,
        const std::function<bool(size_t, size_t)>& can_vertices_collide =
            [](size_t, size_t) { return true; });

    /// @brief Compute the candidate edge-edge candidate collisions.
    void getEdgeEdgePairs(
        const Eigen::MatrixXi& edges,
        std::vector<EdgeEdgeCandidate>& ee_candidates,
        const std::function<bool(size_t, size_t)>& can_vertices_collide =
            [](size_t, size_t) { return true; });

    /// @brief Compute the candidate edge-face candidate intersections.
    void getEdgeFacePairs(
        const Eigen::MatrixXi& edges,
        const Eigen::MatrixXi& faces,
        std::vector<EdgeFaceCandidate>& ef_candidates,
        const std::function<bool(size_t, size_t)>& can_vertices_collide =
            [](size_t, size_t) { return true; });

    /// @brief Compute the candidate edge-edge candidate collisions.
    void getFaceVertexPairs(
        const Eigen::MatrixXi& faces,
        std::vector<FaceVertexCandidate>& fv_candidates,
        const std::function<bool(size_t, size_t)>& can_vertices_collide =
            [](size_t, size_t) { return true; });

    /// @brief Clear the hash grid.
    inline void clear()
    {
        m_vertexItems.clear();
        m_edgeItems.clear();
        m_faceItems.clear();
    }

protected:
    /// @brief Add an AABB of the extents to the hash grid.
    void addElement(
        const AABB& aabb, const int id, std::vector<HashItem>& items) const;

    /// @brief Add a vertex as a AABB containing the time swept edge.
    void addVertex(
        const VectorMax3d& vertex_t0,
        const VectorMax3d& vertex_t1,
        const long index,
        std::vector<HashItem>& vertex_items,
        const double inflation_radius = 0.0) const;

    /// @brief Add an edge as a AABB containing the time swept quad.
    void addEdge(
        const VectorMax3d& edge_vertex0_t0,
        const VectorMax3d& edge_vertex1_t0,
        const VectorMax3d& edge_vertex0_t1,
        const VectorMax3d& edge_vertex1_t1,
        const long index,
        std::vector<HashItem>& edge_items,
        const double inflation_radius = 0.0) const;

    /// @brief Add an edge as a AABB containing the time swept quad.
    void addFace(
        const VectorMax3d& face_vertex0_t0,
        const VectorMax3d& face_vertex1_t0,
        const VectorMax3d& face_vertex2_t0,
        const VectorMax3d& face_vertex0_t1,
        const VectorMax3d& face_vertex1_t1,
        const VectorMax3d& face_vertex2_t1,
        const long index,
        std::vector<HashItem>& face_items,
        const double inflation_radius = 0.0) const;

    /// @brief Create the hash of a cell location.
    inline long hash(int x, int y, int z) const
    {
        assert(x >= 0 && y >= 0 && z >= 0);
        assert(
            x < m_gridSize[0] && y < m_gridSize[1]
            && (m_gridSize.size() == 2 || z < m_gridSize[2]));
        return (z * m_gridSize[1] + y) * m_gridSize[0] + x;
    }

private:
    template <typename T>
    void getPairs(
        const std::function<bool(int, int)>& is_endpoint,
        const std::function<bool(int, int)>& can_collide,
        std::vector<HashItem>& items0,
        std::vector<HashItem>& items1,
        T& candidates);

    template <typename T>
    void getPairs(
        const std::function<bool(int, int)>& is_endpoint,
        const std::function<bool(int, int)>& can_collide,
        std::vector<HashItem>& items,
        T& candidates);

protected:
    double m_cellSize;
    ArrayMax3i m_gridSize;
    ArrayMax3d m_domainMin;
    ArrayMax3d m_domainMax;

    std::vector<HashItem> m_vertexItems;
    std::vector<HashItem> m_edgeItems;
    std::vector<HashItem> m_faceItems;
};

} // namespace ipc


<|start_filename|>tests/main.cpp<|end_filename|>
// Catch2 Documentation: https://github.com/catchorg/Catch2/tree/master/docs

#define CATCH_CONFIG_RUNNER
#include <catch2/catch.hpp>

#include <tbb/global_control.h>
#include <tbb/task_scheduler_init.h>
#include <thread>

#include <ipc/utils/logger.hpp>

#ifdef IPC_TOOLKIT_WITH_LOGGER
Catch::clara::ParserResult parse_log_level(int const d, int& log_level)
{
    if (d < 0 || d > spdlog::level::off) {
        return Catch::clara::ParserResult::runtimeError(
            "Log level must be between 0 and 6");
    } else {
        log_level = d;
        return Catch::clara::ParserResult::ok(
            Catch::clara::ParseResultType::Matched);
    }
}
#endif

Catch::clara::ParserResult parse_num_threads(int const d, int& num_threads)
{
    if (num_threads <= 0) {
        num_threads = tbb::task_scheduler_init::default_num_threads();
    } else if (num_threads > tbb::task_scheduler_init::default_num_threads()) {
        IPC_LOG(warn(
            "Attempting to use more threads than available ({:d} > {:d})!",
            num_threads, tbb::task_scheduler_init::default_num_threads()));
    }
    return Catch::clara::ParserResult::ok(
        Catch::clara::ParseResultType::Matched);
}

int main(int argc, char* argv[])
{
    Catch::Session session; // There must be exactly one instance

    // Build a new parser on top of Catch's
    using namespace Catch::clara;
    auto cli = session.cli();

#ifdef IPC_TOOLKIT_WITH_LOGGER
    int log_level = spdlog::level::warn;
    cli |=
        Opt([&log_level](int const d) { return parse_log_level(d, log_level); },
            "log_level")["--log"]["--logger-level"](
            "logger verbosity level int (0-6)");
#endif

    int num_threads = tbb::task_scheduler_init::default_num_threads();
    cli |=
        Opt([&num_threads](
                int const d) { return parse_num_threads(d, num_threads); },
            "num_threads")["--nthreads"]["--num-threads"](
            "maximum number of threads to use");

    session.cli(cli);

    int returnCode = session.applyCommandLine(argc, argv);
    if (returnCode != 0) // Indicates a command line error
        return returnCode;

    IPC_LOG(set_level(static_cast<spdlog::level::level_enum>(log_level)));

    tbb::global_control thread_limiter(
        tbb::global_control::max_allowed_parallelism, num_threads);

    return session.run();
}


<|start_filename|>src/friction/relative_displacement.hpp<|end_filename|>
#pragma once

#include <Eigen/Core>

#include <ipc/utils/eigen_ext.hpp>

namespace ipc {

///////////////////////////////////////////////////////////////////////////////
// Point - Point

template <typename DerivedDP0, typename DerivedDP1>
inline auto point_point_relative_displacement(
    const Eigen::MatrixBase<DerivedDP0>& dp0,
    const Eigen::MatrixBase<DerivedDP1>& dp1)
{
    return dp0 - dp1;
}

template <
    typename DerivedDisp,
    typename DerivedBasis,
    typename T = typename DerivedDisp::Scalar>
inline VectorMax6<T> point_point_relative_mesh_displacements(
    const Eigen::MatrixBase<DerivedDisp>& tangent_relative_displacement,
    const Eigen::MatrixBase<DerivedBasis>& basis)
{
    int dim = basis.rows();
    VectorMax6<T> mesh_displacements(2 * dim);
    mesh_displacements.head(dim) = basis * tangent_relative_displacement;
    mesh_displacements.tail(dim) = -mesh_displacements.head(dim);
    return mesh_displacements;
}

template <typename DerivedBasis>
inline MatrixMax<typename DerivedBasis::Scalar, 2, 6>
point_point_TT(const Eigen::MatrixBase<DerivedBasis>& basis)
{
    MatrixMax<typename DerivedBasis::Scalar, 2, 6> TT(
        basis.cols(), 2 * basis.rows());
    TT.leftCols(basis.rows()) = basis.transpose();
    TT.rightCols(basis.rows()) = -basis.transpose();
    return TT;
}

///////////////////////////////////////////////////////////////////////////////
// Point - Edge

template <
    typename DerivedDP,
    typename DerivedDE0,
    typename DerivedDE1,
    typename T>
inline auto point_edge_relative_displacement(
    const Eigen::MatrixBase<DerivedDP>& dp,
    const Eigen::MatrixBase<DerivedDE0>& de0,
    const Eigen::MatrixBase<DerivedDE1>& de1,
    const T& alpha)
{
    return dp - ((de1 - de0) * alpha + de0);
}

template <typename DerivedDisp, typename DerivedBasis, typename T>
inline VectorMax9<T> point_edge_relative_mesh_displacements(
    const Eigen::MatrixBase<DerivedDisp>& tangent_relative_displacement,
    const Eigen::MatrixBase<DerivedBasis>& basis,
    const T& alpha)
{
    int dim = basis.rows();
    VectorMax9<T> mesh_displacements(3 * dim);
    mesh_displacements.head(dim) = basis * tangent_relative_displacement;
    mesh_displacements.segment(1 * dim, dim) =
        (alpha - 1.0) * mesh_displacements.head(dim);
    mesh_displacements.tail(dim) = -alpha * mesh_displacements.head(dim);
    return mesh_displacements;
}

template <typename DerivedBasis, typename T>
inline MatrixMax<typename DerivedBasis::Scalar, 2, 9>
point_edge_TT(const Eigen::MatrixBase<DerivedBasis>& basis, const T& alpha)
{
    MatrixMax<typename DerivedBasis::Scalar, 2, 9> TT(
        basis.cols(), 3 * basis.rows());
    TT.leftCols(basis.rows()) = basis.transpose();
    TT.middleCols(basis.rows(), basis.rows()) =
        (alpha - 1.0) * basis.transpose();
    TT.rightCols(basis.rows()) = -alpha * basis.transpose();
    return TT;
}

///////////////////////////////////////////////////////////////////////////////
// Edge - Edge

/// Compute the relative displacement of the edges.
template <
    typename DerivedDEA0,
    typename DerivedDEA1,
    typename DerivedDEB0,
    typename DerivedDEB1,
    typename DerivedBarycentricCoordinates>
inline auto edge_edge_relative_displacement(
    const Eigen::MatrixBase<DerivedDEA0>& dea0,
    const Eigen::MatrixBase<DerivedDEA1>& dea1,
    const Eigen::MatrixBase<DerivedDEB0>& deb0,
    const Eigen::MatrixBase<DerivedDEB1>& deb1,
    const Eigen::MatrixBase<DerivedBarycentricCoordinates>&
        barrycentric_coordinates)
{
    // closest_point_a_displacement - closest_point_b_displacement
    return ((dea1 - dea0) * barrycentric_coordinates[0] + dea0)
        - ((deb1 - deb0) * barrycentric_coordinates[1] + deb0);
}

template <
    typename DerivedDisp,
    typename DerivedBasis,
    typename DerivedBeta,
    typename T = typename DerivedDisp::Scalar>
inline VectorMax12<T> edge_edge_relative_mesh_displacements(
    const Eigen::MatrixBase<DerivedDisp>& tangent_relative_displacement,
    const Eigen::MatrixBase<DerivedBasis>& basis,
    const Eigen::MatrixBase<DerivedBeta>& gamma)
{
    VectorMax3<T> rel_disp = basis * tangent_relative_displacement;
    int dim = rel_disp.size();
    VectorMax12<T> mesh_displacements(4 * dim);
    mesh_displacements.head(dim) = (1.0 - gamma[0]) * rel_disp; // dea0
    mesh_displacements.segment(dim, dim) = gamma[0] * rel_disp; // dea1
    mesh_displacements.segment(2 * dim, dim) =
        (gamma[1] - 1.0) * rel_disp;                     // deb0
    mesh_displacements.tail(dim) = -gamma[1] * rel_disp; // deb1
    return mesh_displacements;
}

template <typename DerivedBasis, typename DerivedGamma>
inline MatrixMax<typename DerivedBasis::Scalar, 2, 12> edge_edge_TT(
    const Eigen::MatrixBase<DerivedBasis>& basis,
    const Eigen::MatrixBase<DerivedGamma>& gamma)
{
    MatrixMax<typename DerivedBasis::Scalar, 2, 12> TT(
        basis.cols(), 4 * basis.rows());
    TT.leftCols(basis.rows()) = (1.0 - gamma[0]) * basis.transpose();
    TT.middleCols(basis.rows(), basis.rows()) = gamma[0] * basis.transpose();
    TT.middleCols(2 * basis.rows(), basis.rows()) =
        (gamma[1] - 1.0) * basis.transpose();
    TT.rightCols(basis.rows()) = -gamma[1] * basis.transpose();
    return TT;
}

///////////////////////////////////////////////////////////////////////////////
// Point - Triangle

/// Compute the relative displacement of the point to the triangle.
template <
    typename DerivedDP,
    typename DerivedDT0,
    typename DerivedDT1,
    typename DerivedDT2,
    typename DerivedBarycentricCoordinates>
inline auto point_triangle_relative_displacement(
    const Eigen::MatrixBase<DerivedDP>& dp,
    const Eigen::MatrixBase<DerivedDT0>& dt0,
    const Eigen::MatrixBase<DerivedDT1>& dt1,
    const Eigen::MatrixBase<DerivedDT2>& dt2,
    const Eigen::MatrixBase<DerivedBarycentricCoordinates>&
        barycentric_coordinates)
{
    // Compute the displacement of the closest point and subtract it from the
    // points displacement.
    return dp
        - (dt0 + barycentric_coordinates[0] * (dt1 - dt0)
           + barycentric_coordinates[1] * (dt2 - dt0));
}

template <
    typename DerivedDisp,
    typename DerivedBasis,
    typename DerivedBeta,
    typename T = typename DerivedDisp::Scalar>
inline VectorMax12<T> point_triangle_relative_mesh_displacements(
    const Eigen::MatrixBase<DerivedDisp>& tangent_relative_displacement,
    const Eigen::MatrixBase<DerivedBasis>& basis,
    const Eigen::MatrixBase<DerivedBeta>& beta)
{
    int dim = basis.rows();
    VectorMax12<T> mesh_displacements(4 * dim);
    mesh_displacements.head(dim) = basis * tangent_relative_displacement;
    mesh_displacements.segment(dim, dim) =
        (-1 + beta[0] + beta[1]) * mesh_displacements.head(dim);
    mesh_displacements.segment(2 * dim, dim) =
        -beta[0] * mesh_displacements.head(dim);
    mesh_displacements.tail(dim) = -beta[1] * mesh_displacements.head(dim);
    return mesh_displacements;
}

template <typename DerivedBasis, typename DerivedBeta>
inline MatrixMax<typename DerivedBasis::Scalar, 2, 12> point_triangle_TT(
    const Eigen::MatrixBase<DerivedBasis>& basis,
    const Eigen::MatrixBase<DerivedBeta>& beta)
{
    MatrixMax<typename DerivedBasis::Scalar, 2, 12> TT(
        basis.cols(), 4 * basis.rows());
    TT.leftCols(basis.rows()) = basis.transpose();
    TT.middleCols(basis.rows(), basis.rows()) =
        (-1 + beta[0] + beta[1]) * basis.transpose();
    TT.middleCols(2 * basis.rows(), basis.rows()) =
        -beta[0] * basis.transpose();
    TT.rightCols(basis.rows()) = -beta[1] * basis.transpose();
    return TT;
}

} // namespace ipc


<|start_filename|>python/src/distance/point_plane.cpp<|end_filename|>
#include <pybind11/pybind11.h>
#include <pybind11/eigen.h>

#include <ipc/distance/point_plane.hpp>

#include "../utils.hpp"

namespace py = pybind11;
using namespace ipc;

void define_point_plane_distance_functions(py::module_& m)
{
    m.def(
        "point_plane_distance",
        [](const Eigen::Vector3d& p, const Eigen::Vector3d& t0,
           const Eigen::Vector3d& t1, const Eigen::Vector3d& t2) {
            return point_plane_distance(p, t0, t1, t2);
        },
        R"ipc_Qu8mg5v7(
        Compute the distance between a point and a plane.

        Parameters:
            p : point
            t0: first vertex of the triangle
            t1: second vertex of the triangle
            t2: third vertex of the triangle

        Returns:
            The distance between the point and plane.

        Note:
            The distance is actually squared distance.
        )ipc_Qu8mg5v7",
        py::arg("p"), py::arg("t0"), py::arg("t1"), py::arg("t1"));

    m.def(
        "point_plane_distance_gradient",
        [](const Eigen::Vector3d& p, const Eigen::Vector3d& t0,
           const Eigen::Vector3d& t1, const Eigen::Vector3d& t2) {
            Vector<double, 12> grad;
            point_plane_distance_gradient(p, t0, t1, t2, grad);
            return grad;
        },
        R"ipc_Qu8mg5v7(
        Compute the gradient of the distance between a point and a plane.

        Parameters:
            p : point
            t0: first vertex of the triangle
            t1: second vertex of the triangle
            t2: third vertex of the triangle

        Returns:
            The gradient of the distance wrt p, t0, t1, and t2.

        Note:
            The distance is actually squared distance.
        )ipc_Qu8mg5v7",
        py::arg("p"), py::arg("t0"), py::arg("t1"), py::arg("t1"));

    m.def(
        "point_plane_distance_hessian",
        [](const Eigen::Vector3d& p, const Eigen::Vector3d& t0,
           const Eigen::Vector3d& t1, const Eigen::Vector3d& t2) {
            Eigen::Matrix<double, 12, 12> hess;
            point_plane_distance_hessian(p, t0, t1, t2, hess);
            return hess;
        },
        R"ipc_Qu8mg5v7(
        Compute the hessian of the distance between a point and a plane.

        Parameters:
            p : point
            t0: first vertex of the triangle
            t1: second vertex of the triangle
            t2: third vertex of the triangle

        Returns:
            The hessian of the distance wrt p, t0, t1, and t2.

        Note:
            The distance is actually squared distance.
        )ipc_Qu8mg5v7",
        py::arg("p"), py::arg("t0"), py::arg("t1"), py::arg("t1"));
}


<|start_filename|>src/utils/unordered_map_and_set.hpp<|end_filename|>
#pragma once

// Comment out this line to switch to the standard unordered_map/set
// #define IPC_TOOLKIT_USE_ROBIN_MAP

#ifdef IPC_TOOLKIT_USE_ROBIN_MAP

#include <tsl/robin_map.h>
#include <tsl/robin_set.h>

namespace ipc {

template <typename K, typename V> using unordered_map = tsl::robin_map<K, V>;
template <typename T> using unordered_set = tsl::robin_set<T>;

} // namespace ipc

#else

#include <unordered_map>
#include <unordered_set>

namespace ipc {

using std::unordered_map;
using std::unordered_set;

} // namespace ipc

#endif


<|start_filename|>tests/barrier/test_barrier.cpp<|end_filename|>
#include <Eigen/Core>
#include <catch2/catch.hpp>

#include <finitediff.hpp>

#include <ipc/barrier/barrier.hpp>

TEST_CASE("Test barrier derivatives", "[barrier]")
{
    double dhat = GENERATE(range(-5, 2));
    dhat = pow(10, dhat);

    double d =
        GENERATE_COPY(take(10, random(dhat / 2, 0.9 * dhat))); // ∈ [0, d̂]
    Eigen::Matrix<double, 1, 1> d_vec;
    d_vec << d;

    // Check gradient

    Eigen::VectorXd fgrad(1);
    fd::finite_gradient(
        d_vec,
        [&](const Eigen::VectorXd& d) { return ipc::barrier(d[0], dhat); },
        fgrad);

    Eigen::VectorXd grad(1);
    grad << ipc::barrier_gradient(d, dhat);

    CAPTURE(dhat, d, fgrad(0), grad(0));
    CHECK(fd::compare_gradient(fgrad, grad));

    // Check hessian

    fd::finite_gradient(
        d_vec,
        [&](const Eigen::VectorXd& d) {
            return ipc::barrier_gradient(d[0], dhat);
        },
        fgrad);

    grad << ipc::barrier_hessian(d, dhat);

    CAPTURE(dhat, d, fgrad(0), grad(0));
    CHECK(fd::compare_gradient(fgrad, grad));
}


<|start_filename|>tests/friction/test_friction.cpp<|end_filename|>
#include <catch2/catch.hpp>

#include <array>
#include <vector>

#include <finitediff.hpp>
#include <igl/edges.h>
#include <nlohmann/json.hpp>

#include <ipc/friction/friction.hpp>
#include <ipc/utils/logger.hpp>

#include "../test_utils.hpp"

using namespace ipc;

TEST_CASE(
    "Test friction gradient and hessian", "[friction][gradient][hessian][2D]")
{
#ifdef NDEBUG
    double mu = GENERATE(range(0.0, 1.0, 0.1));
    double epsv_times_h = pow(10, GENERATE(range(-6, 0)));
    double dhat = pow(10, GENERATE(range(-4, 0)));
    double barrier_stiffness = pow(10, GENERATE(range(0, 2)));
#else
    double mu = GENERATE(range(0.0, 1.0, 0.1));
    double epsv_times_h = pow(10, GENERATE(range(-6, 0, 2)));
    double dhat = pow(10, GENERATE(range(-4, 0, 2)));
    double barrier_stiffness = 100;
#endif
    CAPTURE(mu, epsv_times_h, dhat, barrier_stiffness);

    Eigen::MatrixXd V0, V1;
    Eigen::MatrixXi E, F;
    Constraints contact_constraint_set;
    SECTION("point-triangle")
    {
        V0.resize(4, 3);
        double d = GENERATE_COPY(range(0.0, 2 * dhat, 2 * dhat / 10.0));
        V0.row(0) << 0, d, 0;   // point at t=0
        V0.row(1) << -1, 0, 1;  // triangle vertex 0 at t=0
        V0.row(2) << 2, 0, 0;   // triangle vertex 1 at t=0
        V0.row(3) << -1, 0, -1; // triangle vertex 2 at t=0

        V1 = V0;
        double dy = GENERATE(-1, 1, 1e-1);
        V1.row(0) << 1, d + dy, 0; // point at t=1

        F.resize(1, 3);
        F << 1, 2, 3;
        igl::edges(F, E);
        REQUIRE(E.rows() == 3);

        contact_constraint_set.fv_constraints.emplace_back(0, 0);
    }
    SECTION("edge-edge")
    {
        V0.resize(4, 3);
        double d = GENERATE_COPY(range(0.0, 2 * dhat, 2 * dhat / 10.0));
        V0.row(0) << -1, d, 0; // edge a vertex 0 at t=0
        V0.row(1) << 1, d, 0;  // edge a vertex 1 at t=0
        V0.row(2) << 0, 0, -1; // edge b vertex 0 at t=0
        V0.row(3) << 0, 0, 1;  // edge b vertex 1 at t=0

        V1 = V0;
        double dy = GENERATE(-1, 1, 1e-1);
        V1.row(0) << 0.5, d, 0; // edge a vertex 0 at t=1
        V1.row(1) << 2.5, d, 0; // edge a vertex 1 at t=1

        E.resize(2, 2);
        E.row(0) << 0, 1;
        E.row(1) << 2, 3;

        contact_constraint_set.ee_constraints.emplace_back(0, 1, 0.0);
    }
    SECTION("point-edge")
    {
        V0.resize(3, 3);
        double d = GENERATE_COPY(range(0.0, 2 * dhat, 2 * dhat / 10.0));
        V0.row(0) << -0.5, d, 0; // point at t=0
        V0.row(1) << 0, 0, -1;   // edge vertex 0 at t=0
        V0.row(2) << 0, 0, 1;    // edge vertex 1 at t=0

        V1 = V0;
        double dy = GENERATE(-1, 1, 1e-1);
        V1.row(0) << 0.5, d, 0; // point at t=1

        E.resize(1, 2);
        E.row(0) << 1, 2;

        contact_constraint_set.ev_constraints.emplace_back(0, 1);
    }
    SECTION("point-point")
    {
        V0.resize(2, 3);
        double d = GENERATE_COPY(range(0.0, 2 * dhat, 2 * dhat / 10.0));
        V0.row(0) << -1, d, 0; // point 0 at t=0
        V0.row(1) << 1, d, 0;  // point 1 at t=0

        V1 = V0;
        double dy = GENERATE(-1, 1, 1e-1);
        V1.row(0) << 0.5, d, 0;  // edge a vertex 0 at t=1
        V1.row(1) << -0.5, d, 0; // edge a vertex 1 at t=1

        contact_constraint_set.vv_constraints.emplace_back(0, 1);
    }
    SECTION("point-edge")
    {
        V0.resize(3, 2);
        double d = GENERATE_COPY(range(0.0, 2 * dhat, 2 * dhat / 10.0));
        V0.row(0) << -0.5, d; // point at t=0
        V0.row(1) << -1, 0;   // edge vertex 0 at t=0
        V0.row(2) << 1, 0;    // edge vertex 1 at t=0

        V1 = V0;
        double dy = GENERATE(-1, 1, 1e-1);
        V1.row(0) << 0.5, d; // point at t=1

        E.resize(1, 2);
        E.row(0) << 1, 2;

        contact_constraint_set.ev_constraints.emplace_back(0, 1);
    }
    SECTION("point-point")
    {
        V0.resize(2, 2);
        double d = GENERATE_COPY(range(0.0, 2 * dhat, 2 * dhat / 10.0));
        V0.row(0) << -1, d; // point 0 at t=0
        V0.row(1) << 1, d;  // point 1 at t=0

        V1 = V0;
        double dy = GENERATE(-1, 1, 1e-1);
        V1.row(0) << 0.5, d;  // edge a vertex 0 at t=1
        V1.row(1) << -0.5, d; // edge a vertex 1 at t=1

        contact_constraint_set.vv_constraints.emplace_back(0, 1);
    }

    FrictionConstraints friction_constraint_set;
    construct_friction_constraint_set(
        V0, E, F, contact_constraint_set, dhat, barrier_stiffness, mu,
        friction_constraint_set);

    Eigen::VectorXd grad = compute_friction_potential_gradient(
        V0, V1, E, F, friction_constraint_set, epsv_times_h);

    // Compute the gradient using finite differences
    auto f = [&](const Eigen::VectorXd& x) {
        return compute_friction_potential(
            V0, unflatten(x, V1.cols()), E, F, friction_constraint_set,
            epsv_times_h);
    };
    Eigen::VectorXd fgrad;
    fd::finite_gradient(flatten(V1), f, fgrad);
    CHECK(fd::compare_gradient(grad, fgrad));

    Eigen::MatrixXd hess = compute_friction_potential_hessian(
        V0, V1, E, F, friction_constraint_set, epsv_times_h);
    Eigen::MatrixXd fhess;
    fd::finite_hessian(flatten(V1), f, fhess);
    CHECK(fd::compare_hessian(hess, fhess, 1e-3));
}

void mmcvids_to_friction_constraints(
    Eigen::MatrixXi& E,
    Eigen::MatrixXi& F,
    const Eigen::MatrixXi& mmcvids,
    Eigen::VectorXd normal_force_magnitudes,
    const Eigen::MatrixXd& closest_points,
    const Eigen::MatrixXd& tangent_bases,
    FrictionConstraints& constraints)
{
    std::vector<Eigen::Vector2i> edges;
    std::vector<Eigen::Vector3i> faces;
    for (int i = 0; i < mmcvids.rows(); i++) {
        const auto mmcvid = mmcvids.row(i);
        FrictionConstraint* constraint;

        if (mmcvid[0] >= 0) { // Is EE?
            edges.emplace_back(mmcvid[0], mmcvid[1]);
            edges.emplace_back(mmcvid[2], mmcvid[3]);
            constraints.ee_constraints.emplace_back(
                edges.size() - 2, edges.size() - 1);
            constraint = &(constraints.ee_constraints.back());
        } else {
            if (mmcvid[2] < 0) { // Is VV?
                constraints.vv_constraints.emplace_back(
                    -mmcvid[0] - 1, mmcvid[1]);
                assert(-mmcvid[3] >= 1);
                constraints.vv_constraints.back().multiplicity = -mmcvid[3];
                normal_force_magnitudes[i] /= -mmcvid[3];
                constraint = &(constraints.vv_constraints.back());

            } else if (mmcvid[3] < 0) { // Is EV?
                edges.emplace_back(mmcvid[1], mmcvid[2]);
                constraints.ev_constraints.emplace_back(
                    edges.size() - 1, -mmcvid[0] - 1);
                constraints.ev_constraints.back().multiplicity = -mmcvid[3];
                normal_force_magnitudes[i] /= -mmcvid[3];
                constraint = &(constraints.ev_constraints.back());

            } else { // Is FV.
                faces.emplace_back(mmcvid[1], mmcvid[2], mmcvid[3]);
                constraints.fv_constraints.emplace_back(
                    faces.size() - 1, -mmcvid[0] - 1);
                constraint = &(constraints.fv_constraints.back());
            }
        }

        constraint->closest_point = closest_points.row(i);
        constraint->tangent_basis = tangent_bases.middleRows(3 * i, 3);
        constraint->normal_force_magnitude = normal_force_magnitudes[i];
    }

    E.resize(edges.size(), 2);
    for (int i = 0; i < edges.size(); i++) {
        E.row(i) = edges[i];
    }
    F.resize(faces.size(), 3);
    for (int i = 0; i < faces.size(); i++) {
        F.row(i) = faces[i];
    }
}

template <typename T>
inline void from_json(const nlohmann::json& json, VectorX<T>& vec)
{
    vec =
        Eigen::Map<VectorX<T>>(json.get<std::vector<T>>().data(), json.size());
}

template <typename T>
void from_json(const nlohmann::json& json, MatrixX<T>& mat)
{
    typedef std::vector<std::vector<T>> L;
    L list = json.get<L>();

    size_t num_rows = list.size();
    if (num_rows == 0) {
        return;
    }
    size_t num_cols = list[0].size();
    mat.resize(num_rows, num_cols);

    for (size_t i = 0; i < num_rows; ++i) {
        assert(num_cols == list[i].size());
        mat.row(i) = Eigen::Map<RowVectorX<T>>(list[i].data(), num_cols);
    }
}

bool read_ipc_friction_data(
    const std::string& filename,
    Eigen::MatrixXd& V_start,
    Eigen::MatrixXd& V_lagged,
    Eigen::MatrixXd& V_end,
    Eigen::MatrixXi& E,
    Eigen::MatrixXi& F,
    Constraints& constraint_set,
    FrictionConstraints& friction_constraint_set,
    double& dhat,
    double& barrier_stiffness,
    double& epsv_times_h,
    double& mu,
    double& potential,
    Eigen::VectorXd& grad,
    Eigen::SparseMatrix<double>& hess)
{
    nlohmann::json data;

    std::ifstream input(filename);
    if (input.good()) {
        data = nlohmann::json::parse(input, nullptr, false);
    } else {
        spdlog::error("Unable to open IPC friction data file: {}", filename);
        return false;
    }

    if (data.is_discarded()) {
        spdlog::error("IPC friction data JSON is invalid: {}", filename);
        return false;
    }

    // Parameters
    dhat = sqrt(data["dhat_squared"].get<double>());
    barrier_stiffness = data["barrier_stiffness"];
    epsv_times_h = sqrt(data["epsv_times_h_squared"].get<double>());
    mu = data["mu"];

    // Dissipative potential value
    potential = data["energy"];

    // Potential gradient
    from_json(data["gradient"], grad);

    // Potential hessian
    std::vector<Eigen::Triplet<double>> hessian_triplets;
    hessian_triplets.reserve(data["hessian_triplets"].size());
    for (std::tuple<long, long, double> triplet : data["hessian_triplets"]) {
        hessian_triplets.emplace_back(
            std::get<0>(triplet), std::get<1>(triplet), std::get<2>(triplet));
    }
    hess.resize(grad.size(), grad.size());
    hess.setFromTriplets(hessian_triplets.begin(), hessian_triplets.end());

    // Mesh
    from_json(data["V_start"], V_start);
    from_json(data["V_lagged"], V_lagged);
    from_json(data["V_end"], V_end);

    // MMVCIDs
    Eigen::MatrixXi mmcvids;
    from_json(data["mmcvids"], mmcvids);

    Eigen::VectorXd lambda;
    from_json(data["normal_force_magnitudes"], lambda);

    Eigen::MatrixXd coords;
    from_json(data["closest_point_coordinates"], coords);

    Eigen::MatrixXd bases;
    from_json(data["tangent_bases"], bases);

    mmcvids_to_friction_constraints(
        E, F, mmcvids, lambda, coords, bases, friction_constraint_set);
    mmcvids_to_constraints(E, F, mmcvids, constraint_set);
    for (int i = 0; i < friction_constraint_set.size(); i++) {
        friction_constraint_set[i].mu = mu;
    }

    return true;
}

TEST_CASE(
    "Compare IPC friction derivatives", "[friction][gradient][hessian][data]")
{
    Eigen::MatrixXd V_start, V_lagged, V_end;
    Eigen::MatrixXi E, F;
    Constraints contact_constraint_set;
    FrictionConstraints expected_friction_constraint_set;
    double dhat, barrier_stiffness, epsv_times_h, mu;
    double expected_potential;
    Eigen::VectorXd expected_grad;
    Eigen::SparseMatrix<double> expected_hess;

    std::string scene_folder;
    int file_number;
    SECTION("cube_cube")
    {
        scene_folder = "friction/cube_cube";
        file_number = GENERATE(range(0, 446));
    }
    // SECTION("chain")
    // {
    //     scene_folder = "friction/chain";
    //     file_number = GENERATE(range(0, 401));
    // }
    CAPTURE(scene_folder, file_number);

    bool success = read_ipc_friction_data(
        fmt::format(
            "{}{}/friction_data_{:d}.json", TEST_DATA_DIR, scene_folder,
            file_number),
        V_start, V_lagged, V_end, E, F, contact_constraint_set,
        expected_friction_constraint_set, dhat, barrier_stiffness, epsv_times_h,
        mu, expected_potential, expected_grad, expected_hess);
    REQUIRE(success);

    FrictionConstraints friction_constraint_set;
    construct_friction_constraint_set(
        V_lagged, E, F, contact_constraint_set, dhat, barrier_stiffness, mu,
        friction_constraint_set);

    REQUIRE(friction_constraint_set.size() == contact_constraint_set.size());
    REQUIRE(
        friction_constraint_set.size()
        == expected_friction_constraint_set.size());

    REQUIRE(V_start.size() == V_lagged.size());
    REQUIRE(V_start.size() == V_end.size());
    REQUIRE(V_start.size() == expected_grad.size());

    for (int i = 0; i < friction_constraint_set.size(); i++) {
        CAPTURE(i);
        const FrictionConstraint& constraint = friction_constraint_set[i];
        const FrictionConstraint& expected_constraint =
            expected_friction_constraint_set[i];
        if (constraint.closest_point.size() == 1) {
            CHECK(
                constraint.closest_point[0]
                == Approx(expected_constraint.closest_point[0]));
        } else {
            CHECK(constraint.closest_point.isApprox(
                expected_constraint.closest_point, 1e-12));
        }
        CHECK(constraint.tangent_basis.isApprox(
            expected_constraint.tangent_basis, 1e-12));
        CHECK(
            constraint.normal_force_magnitude
            == Approx(expected_constraint.normal_force_magnitude));
        CHECK(constraint.mu == Approx(expected_constraint.mu));

        CHECK(
            constraint.vertex_indices(E, F)
            == expected_constraint.vertex_indices(E, F));
    }

    double potential = compute_friction_potential(
        V_start, V_end, E, F, friction_constraint_set, epsv_times_h);

    CHECK(potential == Approx(expected_potential));

    Eigen::VectorXd grad = compute_friction_potential_gradient(
        V_start, V_end, E, F, friction_constraint_set, epsv_times_h);

    CHECK(grad.isApprox(expected_grad));

    Eigen::SparseMatrix<double> hess = compute_friction_potential_hessian(
        V_start, V_end, E, F, friction_constraint_set, epsv_times_h);

    CHECK(hess.isApprox(expected_hess));
}


<|start_filename|>src/distance/point_edge.hpp<|end_filename|>
#pragma once

#include <stdexcept> // std::invalid_argument

#include <ipc/distance/distance_type.hpp>
#include <ipc/distance/point_line.hpp>
#include <ipc/distance/point_point.hpp>

namespace ipc {

/// @brief Compute the distance between a point and edge in 2D or 3D.
/// @note The distance is actually squared distance.
/// @param[in] p The point.
/// @param[in] e0 The first vertex of the edge.
/// @param[in] e1 The second vertex of the edge.
/// @return The distance between the point and edge.
template <typename DerivedP, typename DerivedE0, typename DerivedE1>
auto point_edge_distance(
    const Eigen::MatrixBase<DerivedP>& p,
    const Eigen::MatrixBase<DerivedE0>& e0,
    const Eigen::MatrixBase<DerivedE1>& e1)
{
    assert(p.size() == 2 || p.size() == 3);
    assert(e0.size() == 2 || e0.size() == 3);
    assert(e1.size() == 2 || e1.size() == 3);
    return point_edge_distance(p, e0, e1, point_edge_distance_type(p, e0, e1));
}

/// @brief Compute the distance between a point and edge in 2D or 3D.
/// @note The distance is actually squared distance.
/// @param[in] p The point.
/// @param[in] e0 The first vertex of the edge.
/// @param[in] e1 The second vertex of the edge.
/// @param[in] dtype The point edge distance type to compute.
/// @return The distance between the point and edge.
template <typename DerivedP, typename DerivedE0, typename DerivedE1>
auto point_edge_distance(
    const Eigen::MatrixBase<DerivedP>& p,
    const Eigen::MatrixBase<DerivedE0>& e0,
    const Eigen::MatrixBase<DerivedE1>& e1,
    const PointEdgeDistanceType dtype)
{
    assert(p.size() == 2 || p.size() == 3);
    assert(e0.size() == 2 || e0.size() == 3);
    assert(e1.size() == 2 || e1.size() == 3);

    switch (dtype) {
    case PointEdgeDistanceType::P_E0:
        return point_point_distance(p, e0);
    case PointEdgeDistanceType::P_E1:
        return point_point_distance(p, e1);
    case PointEdgeDistanceType::P_E:
        return point_line_distance(p, e0, e1);
    }

    throw std::invalid_argument(
        "Invalid distance type for point-edge distance!");
}

/// @brief Compute the gradient of the distance between a point and edge.
/// @note The distance is actually squared distance.
/// @param[in] p The point.
/// @param[in] e0 The first vertex of the edge.
/// @param[in] e1 The second vertex of the edge.
/// @param[out] grad Gradient of the distance wrt p, e0, and e1.
template <
    typename DerivedP,
    typename DerivedE0,
    typename DerivedE1,
    typename DerivedGrad>
void point_edge_distance_gradient(
    const Eigen::MatrixBase<DerivedP>& p,
    const Eigen::MatrixBase<DerivedE0>& e0,
    const Eigen::MatrixBase<DerivedE1>& e1,
    Eigen::PlainObjectBase<DerivedGrad>& grad)
{
    assert(p.size() == 2 || p.size() == 3);
    assert(e0.size() == 2 || e0.size() == 3);
    assert(e1.size() == 2 || e1.size() == 3);

    return point_edge_distance_gradient(
        p, e0, e1, point_edge_distance_type(p, e0, e1), grad);
}

/// @brief Compute the gradient of the distance between a point and edge.
/// @note The distance is actually squared distance.
/// @param[in] p The point.
/// @param[in] e0 The first vertex of the edge.
/// @param[in] e1 The second vertex of the edge.
/// @param[in] dtype The point edge distance type to compute.
/// @param[out] grad The gradient of the distance wrt p, e0, and e1.
template <
    typename DerivedP,
    typename DerivedE0,
    typename DerivedE1,
    typename DerivedGrad>
void point_edge_distance_gradient(
    const Eigen::MatrixBase<DerivedP>& p,
    const Eigen::MatrixBase<DerivedE0>& e0,
    const Eigen::MatrixBase<DerivedE1>& e1,
    const PointEdgeDistanceType dtype,
    Eigen::PlainObjectBase<DerivedGrad>& grad)
{
    int dim = p.size();
    assert(e0.size() == dim);
    assert(e1.size() == dim);

    grad.resize(3 * dim);
    grad.setZero();

    VectorMax6<typename DerivedGrad::Scalar> local_grad;
    switch (dtype) {
    case PointEdgeDistanceType::P_E0:
        point_point_distance_gradient(p, e0, local_grad);
        grad.head(2 * dim) = local_grad;
        break;

    case PointEdgeDistanceType::P_E1:
        point_point_distance_gradient(p, e1, local_grad);
        grad.head(dim) = local_grad.head(dim);
        grad.tail(dim) = local_grad.tail(dim);
        break;

    case PointEdgeDistanceType::P_E:
        point_line_distance_gradient(p, e0, e1, grad);
        break;

    default:
        throw std::invalid_argument(
            "Invalid distance type for point-edge distance gradient!");
    }
}

/// @brief Compute the hessian of the distance between a point and edge.
/// @note The distance is actually squared distance.
/// @param[in] p The point.
/// @param[in] e0 The first vertex of the edge.
/// @param[in] e1 The second vertex of the edge.
/// @param[out] hess The hessian of the distance wrt p, e0, and e1.
template <
    typename DerivedP,
    typename DerivedE0,
    typename DerivedE1,
    typename DerivedHess>
void point_edge_distance_hessian(
    const Eigen::MatrixBase<DerivedP>& p,
    const Eigen::MatrixBase<DerivedE0>& e0,
    const Eigen::MatrixBase<DerivedE1>& e1,
    Eigen::PlainObjectBase<DerivedHess>& hess)
{
    assert(p.size() == 2 || p.size() == 3);
    assert(e0.size() == 2 || e0.size() == 3);
    assert(e1.size() == 2 || e1.size() == 3);

    return point_edge_distance_hessian(
        p, e0, e1, point_edge_distance_type(p, e0, e1), hess);
}

/// @brief Compute the hessian of the distance between a point and edge.
/// @note The distance is actually squared distance.
/// @param[in] p The point.
/// @param[in] e0 The first vertex of the edge.
/// @param[in] e1 The second vertex of the edge.
/// @param[in] dtype The point edge distance type to compute.
/// @param[out] hess The hessian of the distance wrt p, e0, and e1.
template <
    typename DerivedP,
    typename DerivedE0,
    typename DerivedE1,
    typename DerivedHess>
void point_edge_distance_hessian(
    const Eigen::MatrixBase<DerivedP>& p,
    const Eigen::MatrixBase<DerivedE0>& e0,
    const Eigen::MatrixBase<DerivedE1>& e1,
    const PointEdgeDistanceType dtype,
    Eigen::PlainObjectBase<DerivedHess>& hess)
{
    int dim = p.size();
    assert(e0.size() == dim);
    assert(e1.size() == dim);

    hess.resize(3 * dim, 3 * dim);
    hess.setZero();

    MatrixMax6<typename DerivedHess::Scalar> local_hess;
    switch (dtype) {
    case PointEdgeDistanceType::P_E0:
        point_point_distance_hessian(p, e0, local_hess);
        hess.topLeftCorner(2 * dim, 2 * dim) = local_hess;
        break;

    case PointEdgeDistanceType::P_E1:
        point_point_distance_hessian(p, e1, local_hess);
        hess.topLeftCorner(dim, dim) = local_hess.topLeftCorner(dim, dim);
        hess.topRightCorner(dim, dim) = local_hess.topRightCorner(dim, dim);
        hess.bottomLeftCorner(dim, dim) = local_hess.bottomLeftCorner(dim, dim);
        hess.bottomRightCorner(dim, dim) =
            local_hess.bottomRightCorner(dim, dim);
        break;

    case PointEdgeDistanceType::P_E:
        point_line_distance_hessian(p, e0, e1, hess);
        break;

    default:
        throw std::invalid_argument(
            "Invalid distance type for point-edge distance hessian!");
    }
}

} // namespace ipc


<|start_filename|>python/src/barrier/barrier.cpp<|end_filename|>
#include <pybind11/pybind11.h>

#include <ipc/barrier/barrier.hpp>

namespace py = pybind11;
using namespace ipc;

void define_barrier_functions(py::module_& m)
{
    m.def(
        "barrier", &barrier<double>,
        R"ipc_Qu8mg5v7(
        Function that grows to infinity as d approaches 0 from the right.

        .. math:: b(d) = -(d-\hat{d})^2\ln\left(\frac{d}{\hat{d}}\right)

        Parameters:
            d: distance
            dhat: activation distance of the barrier

        Returns:
            The value of the barrier function at d.

        )ipc_Qu8mg5v7",
        py::arg("d"), py::arg("dhat"));

    m.def(
        "barrier_gradient", &barrier_gradient,
        R"ipc_Qu8mg5v7(
        Derivative of the barrier function.

        .. math:: b'(d) = (\hat{d}-d) \left( 2\ln\left( \frac{d}{\hat{d}} \right) - \frac{\hat{d}}{d} + 1\right)

        Parameters:
            d: distance
            dhat: activation distance of the barrier

        Returns:
            The derivative of the barrier wrt d.
        )ipc_Qu8mg5v7",
        py::arg("d"), py::arg("dhat"));

    m.def(
        "barrier_hessian", &barrier_hessian,
        R"ipc_Qu8mg5v7(
        Second derivative of the barrier function.

        .. math:: b''(d) = \left( \frac{\hat{d}}{d} + 2 \right) \frac{\hat{d}}{d} - 2\ln\left( \frac{d}{\hat{d}} \right) - 3

        Parameters:
            d: distance
            dhat: activation distance of the barrier

        Returns:
            The second derivative of the barrier wrt d.
        )ipc_Qu8mg5v7",
        py::arg("d"), py::arg("dhat"));
}


<|start_filename|>src/broad_phase/broad_phase.cpp<|end_filename|>
#include <ipc/broad_phase/broad_phase.hpp>

#include <ipc/broad_phase/brute_force.hpp>
#include <ipc/broad_phase/spatial_hash.hpp>
#include <ipc/broad_phase/hash_grid.hpp>

namespace ipc {

void build_ignore_vertices(
    size_t num_vertices,
    const Eigen::VectorXi& codim_V,
    const Eigen::MatrixXi& E,
    std::vector<bool>& ignore_vertex)
{
    ignore_vertex.resize(num_vertices, true);
    for (size_t i = 0; i < codim_V.size(); i++) {
        assert(codim_V(i) < num_vertices);
        ignore_vertex[codim_V(i)] = false;
    }
    // Column first because colmajor
    for (size_t ej = 0; ej < E.cols(); ej++) {
        for (size_t ei = 0; ei < E.rows(); ei++) {
            assert(E(ei, ej) < num_vertices);
            ignore_vertex[E(ei, ej)] = false;
        }
    }
}

void construct_collision_candidates(
    const Eigen::MatrixXd& V,
    const Eigen::VectorXi& codim_V,
    bool include_all_vertices,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    Candidates& candidates,
    double inflation_radius,
    const BroadPhaseMethod& method,
    const std::function<bool(size_t, size_t)>& can_collide)
{
    int dim = V.cols();

    candidates.clear();

    switch (method) {
    case BroadPhaseMethod::BRUTE_FORCE: {
        std::vector<bool> ignore_vertex;
        if (!include_all_vertices) {
            build_ignore_vertices(V.rows(), codim_V, E, ignore_vertex);
        }
        auto adjusted_can_collide = [&](size_t vi, size_t vj) {
            return (ignore_vertex.empty()
                    || (!ignore_vertex[vi] && !ignore_vertex[vj]))
                && can_collide(vi, vj);
        };
        detect_collision_candidates_brute_force(
            V, E, F, candidates,
            /*detect_edge_vertex=*/dim == 2,
            /*detect_edge_edge=*/dim == 3,
            /*detect_face_vertex=*/dim == 3,
            /*perform_aabb_check=*/true,
            /*aabb_inflation_radius=*/inflation_radius, adjusted_can_collide);
    } break;
    case BroadPhaseMethod::HASH_GRID: {
        HashGrid hash_grid;
        hash_grid.resize(V, E, inflation_radius);

        // Assumes the edges connect to all boundary vertices
        if (include_all_vertices) {
            hash_grid.addVertices(V, inflation_radius);
        } else {
            hash_grid.addVerticesFromEdges(V, E, inflation_radius);
            hash_grid.addSelectVertices(V, codim_V, inflation_radius);
        }
        hash_grid.addEdges(V, E, inflation_radius);
        if (dim == 3) {
            // These are not needed for 2D
            hash_grid.addFaces(V, F, inflation_radius);
        }

        if (dim == 2) {
            // This is not needed for 3D
            hash_grid.getVertexEdgePairs(
                E, candidates.ev_candidates, can_collide);
        } else {
            // These are not needed for 2D
            hash_grid.getEdgeEdgePairs(
                E, candidates.ee_candidates, can_collide);
            hash_grid.getFaceVertexPairs(
                F, candidates.fv_candidates, can_collide);
        }
    } break;
    case BroadPhaseMethod::SPATIAL_HASH: {
        // TODO: Use include_all_vertices and codim_V
        assert(include_all_vertices);
        // TODO: Use can_collide
        SpatialHash sh(V, E, F, inflation_radius);
        sh.queryMeshForCandidates(
            V, E, F, candidates,
            /*queryEV=*/dim == 2,
            /*queryEE=*/dim == 3,
            /*queryFV=*/dim == 3);
    } break;
    }
}

void construct_collision_candidates(
    const Eigen::MatrixXd& V,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    Candidates& candidates,
    double inflation_radius,
    const BroadPhaseMethod& method,
    const std::function<bool(size_t, size_t)>& can_collide)
{
    construct_collision_candidates(
        V, /*codim_V=*/Eigen::VectorXi(), /*include_all_vertices=*/true, E, F,
        candidates, inflation_radius, method, can_collide);
}

void construct_collision_candidates(
    const Eigen::MatrixXd& V,
    const Eigen::VectorXi& codim_V,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    Candidates& candidates,
    double inflation_radius,
    const BroadPhaseMethod& method,
    const std::function<bool(size_t, size_t)>& can_collide)
{
    construct_collision_candidates(
        V, codim_V, /*include_all_vertices=*/false, E, F, candidates,
        inflation_radius, method, can_collide);
}

void construct_collision_candidates(
    const Eigen::MatrixXd& V0,
    const Eigen::MatrixXd& V1,
    const Eigen::VectorXi& codim_V,
    bool include_all_vertices,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    Candidates& candidates,
    double inflation_radius,
    const BroadPhaseMethod& method,
    const std::function<bool(size_t, size_t)>& can_collide)
{
    int dim = V0.cols();
    assert(V1.cols() == dim);

    candidates.clear();

    switch (method) {
    case BroadPhaseMethod::BRUTE_FORCE: {
        std::vector<bool> ignore_vertex;
        if (!include_all_vertices) {
            build_ignore_vertices(V0.rows(), codim_V, E, ignore_vertex);
        }
        auto adjusted_can_collide = [&](size_t vi, size_t vj) {
            return (ignore_vertex.empty()
                    || (!ignore_vertex[vi] && !ignore_vertex[vj]))
                && can_collide(vi, vj);
        };
        detect_collision_candidates_brute_force(
            V0, V1, E, F, candidates,
            /*detect_edge_vertex=*/dim == 2,
            /*detect_edge_edge=*/dim == 3,
            /*detect_face_vertex=*/dim == 3,
            /*perform_aabb_check=*/true,
            /*aabb_inflation_radius=*/inflation_radius, adjusted_can_collide);
    } break;
    case BroadPhaseMethod::HASH_GRID: {
        HashGrid hash_grid;
        hash_grid.resize(V0, V1, E, inflation_radius);

        // Assumes the edges connect to all boundary vertices
        if (include_all_vertices) {
            hash_grid.addVertices(V0, V1, inflation_radius);
        } else {
            hash_grid.addVerticesFromEdges(V0, V1, E, inflation_radius);
            hash_grid.addSelectVertices(V0, V1, codim_V, inflation_radius);
        }
        hash_grid.addEdges(V0, V1, E, inflation_radius);
        if (dim == 3) {
            // These are not needed for 2D
            hash_grid.addFaces(V0, V1, F, inflation_radius);
        }

        if (dim == 2) {
            // This is not needed for 3D
            hash_grid.getVertexEdgePairs(
                E, candidates.ev_candidates, can_collide);
        } else {
            // These are not needed for 2D
            hash_grid.getEdgeEdgePairs(
                E, candidates.ee_candidates, can_collide);
            hash_grid.getFaceVertexPairs(
                F, candidates.fv_candidates, can_collide);
        }
    } break;
    case BroadPhaseMethod::SPATIAL_HASH: {
        // TODO: Use include_all_vertices and codim_V
        assert(include_all_vertices);
        // TODO: Use can_collide
        SpatialHash sh(V0, V1, E, F, inflation_radius);
        sh.queryMeshForCandidates(
            V0, V1, E, F, candidates,
            /*queryEV=*/dim == 2,
            /*queryEE=*/dim == 3,
            /*queryFV=*/dim == 3);
    } break;
    }
}

void construct_collision_candidates(
    const Eigen::MatrixXd& V0,
    const Eigen::MatrixXd& V1,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    Candidates& candidates,
    double inflation_radius,
    const BroadPhaseMethod& method,
    const std::function<bool(size_t, size_t)>& can_collide)
{
    construct_collision_candidates(
        V0, V1, /*codim_V=*/Eigen::VectorXi(), /*include_all_vertices=*/true, E,
        F, candidates, inflation_radius, method, can_collide);
}

void construct_collision_candidates(
    const Eigen::MatrixXd& V0,
    const Eigen::MatrixXd& V1,
    const Eigen::VectorXi& codim_V,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    Candidates& candidates,
    double inflation_radius,
    const BroadPhaseMethod& method,
    const std::function<bool(size_t, size_t)>& can_collide)
{
    construct_collision_candidates(
        V0, V1, codim_V, /*include_all_vertices=*/false, E, F, candidates,
        inflation_radius, method, can_collide);
}

} // namespace ipc


<|start_filename|>src/broad_phase/hash_grid.cpp<|end_filename|>
#include <ipc/broad_phase/hash_grid.hpp>

#include <algorithm> // std::min/max

#include <tbb/enumerable_thread_specific.h>
#include <tbb/blocked_range2d.h>
#include <tbb/parallel_for.h>
#include <tbb/parallel_sort.h>

#include <ipc/broad_phase/voxel_size_heuristic.hpp>
#include <ipc/utils/vertex_to_min_edge.hpp>
#include <ipc/utils/logger.hpp>

// NOTE: Uncomment for expiermental parallel version of getPairs()
// #define IPC_HASH_GRID_PARALLEL_GET_PAIR

namespace ipc {

AABB::AABB(const ArrayMax3d& min, const ArrayMax3d& max)
    : min(min)
    , max(max)
{
    assert(min.size() == max.size());
    assert((min <= max).all());
    // half_extent = (max - min) / 2;
    // center = min + half_extent;
}

bool AABB::are_overlapping(const AABB& a, const AABB& b)
{
    assert(a.min.size() == b.min.size());

    // NOTE: This is a faster check (https://gamedev.stackexchange.com/a/587),
    // but it is inexact and can result in false negatives.
    // return (abs(a.center.x() - b.center.x())
    //         <= (a.half_extent.x() + b.half_extent.x()))
    //     && (abs(a.center.y() - b.center.y())
    //         <= (a.half_extent.y() + b.half_extent.y()))
    //     && (a.min.size() == 2
    //         || abs(a.center.z() - b.center.z())
    //             <= (a.half_extent.z() + b.half_extent.z()));

    // This on the otherhand, is exact because there is no rounding.
    return (a.min <= b.max).all() && (b.min <= a.max).all();
};

typedef tbb::enumerable_thread_specific<std::vector<HashItem>>
    ThreadSpecificHashItems;

void merge_local_items(
    const ThreadSpecificHashItems& storages, std::vector<HashItem>& items)
{
    // size up the hash items
    size_t num_items = items.size();
    for (const auto& local_items : storages) {
        num_items += local_items.size();
    }
    // serial merge!
    items.reserve(num_items);
    for (const auto& local_items : storages) {
        items.insert(items.end(), local_items.begin(), local_items.end());
    }
}

void HashGrid::resizeFromBox(
    const ArrayMax3d& min, const ArrayMax3d& max, double cellSize)
{
    clear();
    assert(cellSize != 0.0);
    m_cellSize = cellSize;
    m_domainMin = min;
    m_domainMax = max;
    m_gridSize = ((max - min) / m_cellSize).ceil().cast<int>().max(1);
    IPC_LOG(trace(
        "hash-grid resized with a size of {:d}x{:d}x{:d}", m_gridSize[0],
        m_gridSize[1], m_gridSize.size() == 3 ? m_gridSize[2] : 1));
}

/// @brief Compute an AABB around a given 2D mesh.
void calculate_mesh_extents(
    const Eigen::MatrixXd& vertices_t0,
    const Eigen::MatrixXd& vertices_t1,
    ArrayMax3d& lower_bound,
    ArrayMax3d& upper_bound)
{
    ArrayMax3d lower_bound_t0 = vertices_t0.colwise().minCoeff();
    ArrayMax3d upper_bound_t0 = vertices_t0.colwise().maxCoeff();
    ArrayMax3d lower_bound_t1 = vertices_t1.colwise().minCoeff();
    ArrayMax3d upper_bound_t1 = vertices_t1.colwise().maxCoeff();
    lower_bound = lower_bound_t0.min(lower_bound_t1);
    upper_bound = upper_bound_t0.max(upper_bound_t1);
}

void HashGrid::resize(
    const Eigen::MatrixXd& vertices_t0,
    const Eigen::MatrixXd& vertices_t1,
    const Eigen::MatrixXi& edges,
    const double inflation_radius)
{
    ArrayMax3d mesh_min, mesh_max;
    calculate_mesh_extents(vertices_t0, vertices_t1, mesh_min, mesh_max);
    double cell_size = suggest_good_voxel_size(
        vertices_t0, vertices_t1, edges, inflation_radius);
    resizeFromBox(
        mesh_min - inflation_radius, mesh_max + inflation_radius, cell_size);
}

/// @brief Compute a AABB for a vertex moving through time (i.e. temporal edge).
void calculate_vertex_extents(
    const VectorMax3d& vertex_t0,
    const VectorMax3d& vertex_t1,
    ArrayMax3d& lower_bound,
    ArrayMax3d& upper_bound)
{
    lower_bound = vertex_t0.cwiseMin(vertex_t1);
    upper_bound = vertex_t0.cwiseMax(vertex_t1);
}

void HashGrid::addVertex(
    const VectorMax3d& vertex_t0,
    const VectorMax3d& vertex_t1,
    const long index,
    const double inflation_radius)
{
    addVertex(vertex_t0, vertex_t1, index, m_vertexItems, inflation_radius);
}

void HashGrid::addVertex(
    const VectorMax3d& vertex_t0,
    const VectorMax3d& vertex_t1,
    const long index,
    std::vector<HashItem>& vertex_items,
    const double inflation_radius) const
{
    ArrayMax3d lower_bound, upper_bound;
    calculate_vertex_extents(vertex_t0, vertex_t1, lower_bound, upper_bound);
    addElement(
        AABB(lower_bound - inflation_radius, upper_bound + inflation_radius),
        index, vertex_items);
}

void HashGrid::addVertices(
    const Eigen::MatrixXd& vertices_t0,
    const Eigen::MatrixXd& vertices_t1,
    const double inflation_radius)
{
    assert(vertices_t0.rows() == vertices_t1.rows());

    ThreadSpecificHashItems storage;

    tbb::parallel_for(
        tbb::blocked_range<long>(0l, long(vertices_t0.rows())),
        [&](const tbb::blocked_range<long>& range) {
            ThreadSpecificHashItems::reference local_items = storage.local();

            for (long i = range.begin(); i != range.end(); i++) {
                addVertex(
                    vertices_t0.row(i), vertices_t1.row(i), i, local_items,
                    inflation_radius);
            }
        });

    merge_local_items(storage, m_vertexItems);
}

void HashGrid::addSelectVertices(
    const Eigen::MatrixXd& vertices_t0,
    const Eigen::MatrixXd& vertices_t1,
    const Eigen::VectorXi& vertex_indices,
    const double inflation_radius)
{
    assert(vertices_t0.rows() == vertices_t1.rows());

    ThreadSpecificHashItems storage;

    tbb::parallel_for(
        tbb::blocked_range<long>(0l, long(vertex_indices.size())),
        [&](const tbb::blocked_range<long>& range) {
            ThreadSpecificHashItems::reference local_items = storage.local();

            for (long i = range.begin(); i != range.end(); i++) {
                const size_t vi = vertex_indices[i];
                addVertex(
                    vertices_t0.row(vi), vertices_t1.row(vi), vi, local_items,
                    inflation_radius);
            }
        });

    merge_local_items(storage, m_vertexItems);
}

void HashGrid::addVerticesFromEdges(
    const Eigen::MatrixXd& vertices_t0,
    const Eigen::MatrixXd& vertices_t1,
    const Eigen::MatrixXi& edges,
    const double inflation_radius)
{
    assert(vertices_t0.rows() == vertices_t1.rows());

    std::vector<size_t> V2E = vertex_to_min_edge(vertices_t0.rows(), edges);

    ThreadSpecificHashItems storage;

    tbb::parallel_for(
        tbb::blocked_range<long>(0l, long(edges.rows())),
        [&](const tbb::blocked_range<long>& range) {
            ThreadSpecificHashItems::reference local_items = storage.local();

            for (long ei = range.begin(); ei < range.end(); ei++) {
                for (long ej = 0; ej < edges.cols(); ej++) {
                    const size_t vi = edges(ei, ej);
                    if (V2E[vi] == ei) {
                        addVertex(
                            vertices_t0.row(vi), vertices_t1.row(vi), vi,
                            local_items, inflation_radius);
                    }
                }
            }
        });

    merge_local_items(storage, m_vertexItems);
}

/// @brief Compute a AABB for an edge moving through time (i.e. temporal quad).
void calculate_edge_extents(
    const VectorMax3d& edge_vertex0_t0,
    const VectorMax3d& edge_vertex1_t0,
    const VectorMax3d& edge_vertex0_t1,
    const VectorMax3d& edge_vertex1_t1,
    ArrayMax3d& lower_bound,
    ArrayMax3d& upper_bound)
{
    lower_bound = edge_vertex0_t0.cwiseMin(edge_vertex1_t0)
                      .cwiseMin(edge_vertex0_t1)
                      .cwiseMin(edge_vertex1_t1);
    upper_bound = edge_vertex0_t0.cwiseMax(edge_vertex1_t0)
                      .cwiseMax(edge_vertex0_t1)
                      .cwiseMax(edge_vertex1_t1);
}

void HashGrid::addEdge(
    const VectorMax3d& edge_vertex0_t0,
    const VectorMax3d& edge_vertex1_t0,
    const VectorMax3d& edge_vertex0_t1,
    const VectorMax3d& edge_vertex1_t1,
    const long index,
    const double inflation_radius)
{
    addEdge(
        edge_vertex0_t0, edge_vertex1_t0, edge_vertex0_t1, edge_vertex1_t1,
        index, m_edgeItems, inflation_radius);
}

void HashGrid::addEdge(
    const VectorMax3d& edge_vertex0_t0,
    const VectorMax3d& edge_vertex1_t0,
    const VectorMax3d& edge_vertex0_t1,
    const VectorMax3d& edge_vertex1_t1,
    const long index,
    std::vector<HashItem>& edge_items,
    const double inflation_radius) const
{
    ArrayMax3d lower_bound, upper_bound;
    calculate_edge_extents(
        edge_vertex0_t0, edge_vertex1_t0, edge_vertex0_t1, edge_vertex1_t1,
        lower_bound, upper_bound);
    addElement(
        AABB(lower_bound - inflation_radius, upper_bound + inflation_radius),
        index, edge_items);
}

void HashGrid::addEdges(
    const Eigen::MatrixXd& vertices_t0,
    const Eigen::MatrixXd& vertices_t1,
    const Eigen::MatrixXi& edges,
    const double inflation_radius)
{
    assert(vertices_t0.rows() == vertices_t1.rows());

    ThreadSpecificHashItems storage;

    tbb::parallel_for(
        tbb::blocked_range<long>(0l, long(edges.rows())),
        [&](const tbb::blocked_range<long>& range) {
            ThreadSpecificHashItems::reference local_items = storage.local();

            for (long i = range.begin(); i != range.end(); i++) {
                addEdge(
                    vertices_t0.row(edges(i, 0)), vertices_t0.row(edges(i, 1)),
                    vertices_t1.row(edges(i, 0)), vertices_t1.row(edges(i, 1)),
                    i, local_items, inflation_radius);
            }
        });

    merge_local_items(storage, m_edgeItems);
}

/// @brief Compute a AABB for an edge moving through time (i.e. temporal quad).
void calculate_face_extents(
    const VectorMax3d& face_vertex0_t0,
    const VectorMax3d& face_vertex1_t0,
    const VectorMax3d& face_vertex2_t0,
    const VectorMax3d& face_vertex0_t1,
    const VectorMax3d& face_vertex1_t1,
    const VectorMax3d& face_vertex2_t1,
    ArrayMax3d& lower_bound,
    ArrayMax3d& upper_bound)
{
    lower_bound = face_vertex0_t0.cwiseMin(face_vertex1_t0)
                      .cwiseMin(face_vertex2_t0)
                      .cwiseMin(face_vertex0_t1)
                      .cwiseMin(face_vertex1_t1)
                      .cwiseMin(face_vertex2_t1);
    upper_bound = face_vertex0_t0.cwiseMax(face_vertex1_t0)
                      .cwiseMax(face_vertex2_t0)
                      .cwiseMax(face_vertex0_t1)
                      .cwiseMax(face_vertex1_t1)
                      .cwiseMax(face_vertex2_t1);
}

void HashGrid::addFace(
    const VectorMax3d& face_vertex0_t0,
    const VectorMax3d& face_vertex1_t0,
    const VectorMax3d& face_vertex2_t0,
    const VectorMax3d& face_vertex0_t1,
    const VectorMax3d& face_vertex1_t1,
    const VectorMax3d& face_vertex2_t1,
    const long index,
    const double inflation_radius)
{
    addFace(
        face_vertex0_t0, face_vertex1_t0, face_vertex2_t0, face_vertex0_t1,
        face_vertex1_t1, face_vertex2_t1, index, m_faceItems, inflation_radius);
}

void HashGrid::addFace(
    const VectorMax3d& face_vertex0_t0,
    const VectorMax3d& face_vertex1_t0,
    const VectorMax3d& face_vertex2_t0,
    const VectorMax3d& face_vertex0_t1,
    const VectorMax3d& face_vertex1_t1,
    const VectorMax3d& face_vertex2_t1,
    const long index,
    std::vector<HashItem>& face_items,
    const double inflation_radius) const
{
    ArrayMax3d lower_bound, upper_bound;
    calculate_face_extents(
        face_vertex0_t0, face_vertex1_t0, face_vertex2_t0, //
        face_vertex0_t1, face_vertex1_t1, face_vertex2_t1, //
        lower_bound, upper_bound);
    addElement(
        AABB(lower_bound - inflation_radius, upper_bound + inflation_radius),
        index, face_items);
}

void HashGrid::addFaces(
    const Eigen::MatrixXd& vertices_t0,
    const Eigen::MatrixXd& vertices_t1,
    const Eigen::MatrixXi& faces,
    const double inflation_radius)
{
    assert(vertices_t0.rows() == vertices_t1.rows());

    ThreadSpecificHashItems storage;

    tbb::parallel_for(
        tbb::blocked_range<long>(0l, long(faces.rows())),
        [&](const tbb::blocked_range<long>& range) {
            ThreadSpecificHashItems::reference local_items = storage.local();

            for (long i = range.begin(); i != range.end(); i++) {
                addFace(
                    vertices_t0.row(faces(i, 0)), vertices_t0.row(faces(i, 1)),
                    vertices_t0.row(faces(i, 2)), vertices_t1.row(faces(i, 0)),
                    vertices_t1.row(faces(i, 1)), vertices_t1.row(faces(i, 2)),
                    i, local_items, inflation_radius);
            }
        });

    merge_local_items(storage, m_faceItems);
}

void HashGrid::addElement(
    const AABB& aabb, const int id, std::vector<HashItem>& items) const
{
    ArrayMax3i int_min =
        ((aabb.getMin() - m_domainMin) / m_cellSize).cast<int>();
    // We can round down to -1, but not less
    assert((int_min >= -1).all());
    assert((int_min <= m_gridSize).all());
    int_min = int_min.max(0).min(m_gridSize - 1);

    ArrayMax3i int_max =
        ((aabb.getMax() - m_domainMin) / m_cellSize).cast<int>();
    assert((int_max >= -1).all());
    assert((int_max <= m_gridSize).all());
    int_max = int_max.max(0).min(m_gridSize - 1);
    assert((int_min <= int_max).all());

    int min_z = int_min.size() == 3 ? int_min.z() : 0;
    int max_z = int_max.size() == 3 ? int_max.z() : 0;
    for (int x = int_min.x(); x <= int_max.x(); ++x) {
        for (int y = int_min.y(); y <= int_max.y(); ++y) {
            for (int z = min_z; z <= max_z; ++z) {
                items.emplace_back(hash(x, y, z), id, aabb);
            }
        }
    }
}

template <typename T>
void HashGrid::getPairs(
    const std::function<bool(int, int)>& is_endpoint,
    const std::function<bool(int, int)>& can_collide,
    std::vector<HashItem>& items0,
    std::vector<HashItem>& items1,
    T& candidates)
{
    // Sorted all they (key,value) pairs, where key is the hash key, and value
    // is the element index
    tbb::parallel_sort(items0.begin(), items0.end());
    tbb::parallel_sort(items1.begin(), items1.end());

    // Entries with the same key means they share a cell (that cell index
    // hashes to the same key) and should be flagged for low-level intersection
    // testing. We loop over the entire sorted set of (key,value) pairs
    // creating Candidate entries for pairs with the same key

    // 1. Find start of cells in items0 and items1. Store the start and end of
    // each cell in a vector.
    int num_cells = m_gridSize.prod();

    std::vector<long> cell_starts0(num_cells, -1);
    std::vector<long> cell_ends0(num_cells, -1);
    for (long i = 0; i < items0.size(); i++) {
        if (i == 0 || items0[i].key != items0[i - 1].key) {
            cell_starts0[items0[i].key] = i;
        }
        if (i == items0.size() - 1 || items0[i].key != items0[i + 1].key) {
            cell_ends0[items0[i].key] = i;
        }
    }

    std::vector<long> cell_starts1(num_cells, -1);
    std::vector<long> cell_ends1(num_cells, -1);
    for (long i = 0; i < items1.size(); i++) {
        if (i == 0 || items1[i].key != items1[i - 1].key) {
            cell_starts1[items1[i].key] = i;
        }
        if (i == items1.size() - 1 || items1[i].key != items1[i + 1].key) {
            cell_ends1[items1[i].key] = i;
        }
    }

    // 2. Perform a double for loop for each cell, building the candidates.
#ifdef IPC_HASH_GRID_PARALLEL_GET_PAIR
    tbb::enumerable_thread_specific<T> storages;
    tbb::parallel_for(
        tbb::blocked_range<int>(0, num_cells),
        [&](const tbb::blocked_range<int>& c_ids) {
            for (int ci = c_ids.begin(); ci < c_ids.end(); ci++) {
                auto& local_candidates = storages.local();
#else
    for (int ci = 0; ci < num_cells; ci++) {
        auto& local_candidates = candidates;
#endif
                // Check if the cell is empty
                if (cell_starts0[ci] < 0 || cell_starts1[ci] < 0) {
                    continue;
                }
                assert(cell_ends0[ci] >= 0 && cell_ends1[ci] >= 0);

                for (int i = cell_starts0[ci]; i <= cell_ends0[ci]; i++) {
                    for (int j = cell_starts1[ci]; j <= cell_ends1[ci]; j++) {
                        if (!is_endpoint(items0[i].id, items1[j].id)
                            && can_collide(items0[i].id, items1[j].id)
                            && AABB::are_overlapping(
                                items0[i].aabb, items1[j].aabb)) {
                            local_candidates.emplace_back(
                                items0[i].id, items1[j].id);
                        }
                    }
                }
            }
#ifdef IPC_HASH_GRID_PARALLEL_GET_PAIR
        });

    // size up the candidates
    size_t num_candidates = candidates.size();
    for (const auto& local_candidates : storages) {
        num_candidates += local_candidates.size();
    }
    // serial merge!
    candidates.reserve(num_candidates);
    for (const auto& local_candidates : storages) {
        candidates.insert(
            candidates.end(), local_candidates.begin(), local_candidates.end());
    }
#endif

    // WARNING: This version is buggy and I am not sure why.
    /*
    int i = 0, j_start = 0;
    while (i < items0.size() && j_start < items1.size()) {
        const HashItem& item0 = items0[i];

        int j = j_start;
        while (j < items1.size()) {
            const HashItem& item1 = items1[j];

            if (item0.key == item1.key) {
                if (!is_endpoint(item0.id, item1.id)
                    && can_collide(item0.id, item1.id)
                    && AABB::are_overlapping(item0.aabb, item1.aabb)) {
                    candidates.emplace_back(item0.id, item1.id);
                }
            } else {
                break;
            }
            j++;
        }

        if (i == items0.size() - 1 || item0.key != items0[i + 1].key) {
            j_start = j;
        }
        i++;
    }
    */

    // Remove the duplicate candidates
    tbb::parallel_sort(candidates.begin(), candidates.end());
    auto new_end = std::unique(candidates.begin(), candidates.end());
    candidates.erase(new_end, candidates.end());
}

template <typename T>
void HashGrid::getPairs(
    const std::function<bool(int, int)>& is_endpoint,
    const std::function<bool(int, int)>& can_collide,
    std::vector<HashItem>& items,
    T& candidates)
{
    // Sorted all they (key,value) pairs, where key is the hash key, and
    // value is the element index
    tbb::parallel_sort(items.begin(), items.end());

#ifndef IPC_HASH_GRID_PARALLEL_GET_PAIR
    // Entries with the same key means they share a cell (that cell index
    // hashes to the same key) and should be flagged for low-level
    // intersection testing. So we loop over the entire sorted set of
    // (key,value) pairs creating Candidate entries for pairs with the same key
    for (int i = 0; i < items.size(); i++) {
        const HashItem& item0 = items[i];
        for (int j = i + 1; j < items.size(); j++) {
            const HashItem& item1 = items[j];
            if (item0.key == item1.key) {
                if (!is_endpoint(item0.id, item1.id)
                    && can_collide(item0.id, item1.id)
                    && AABB::are_overlapping(item0.aabb, item1.aabb)) {
                    candidates.emplace_back(item0.id, item1.id);
                }
            } else {
                break; // This avoids a brute force comparison
            }
        }
    }
#else
    tbb::enumerable_thread_specific<T> storages;

    tbb::parallel_for(
        tbb::blocked_range2d<int>(0, items.size(), 0, items.size()),
        [&](const tbb::blocked_range2d<int>& r) {
            if (r.rows().begin() >= r.cols().end()) {
                return; // i needs to be less than j
            }

            auto& local_candidates = storages.local();

            for (int i = r.rows().begin(); i < r.rows().end(); i++) {
                const HashItem& item0 = items[i];

                if (i >= r.cols().end()) {
                    return; // i will increase but r.cols().end() will not
                }

                // i < r.cols().end() → i + 1 <= r.cols().end()
                int j_start = std::max(i + 1, r.cols().begin());
                assert(j_start > i);

                for (int j = j_start; j < r.cols().end(); j++) {
                    const HashItem& item1 = items[j];

                    if (item0.key == item1.key) {
                        if (!is_endpoint(item0.id, item1.id)
                            && can_collide(item0.id, item1.id)
                            && AABB::are_overlapping(item0.aabb, item1.aabb)) {
                            local_candidates.emplace_back(item0.id, item1.id);
                        }
                    } else {
                        break; // This avoids a brute force comparison
                    }
                }
            }
        });

    // size up the candidates
    size_t num_candidates = candidates.size();
    for (const auto& local_candidates : storages) {
        num_candidates += local_candidates.size();
    }
    // serial merge!
    candidates.reserve(num_candidates);
    for (const auto& local_candidates : storages) {
        candidates.insert(
            candidates.end(), local_candidates.begin(), local_candidates.end());
    }
#endif

    // Remove the duplicate candidates
    tbb::parallel_sort(candidates.begin(), candidates.end());
    auto new_end = std::unique(candidates.begin(), candidates.end());
    candidates.erase(new_end, candidates.end());
}

void HashGrid::getVertexEdgePairs(
    const Eigen::MatrixXi& edges,
    std::vector<EdgeVertexCandidate>& ev_candidates,
    const std::function<bool(size_t, size_t)>& can_vertices_collide)
{
    auto is_endpoint = [&](int ei, int vi) {
        return edges(ei, 0) == vi || edges(ei, 1) == vi;
    };

    auto can_collide = [&](int ei, int vi) {
        return can_vertices_collide(vi, edges(ei, 0))
            || can_vertices_collide(vi, edges(ei, 1));
    };

    getPairs(
        is_endpoint, can_collide, m_edgeItems, m_vertexItems, ev_candidates);
}

void HashGrid::getEdgeEdgePairs(
    const Eigen::MatrixXi& edges,
    std::vector<EdgeEdgeCandidate>& ee_candidates,
    const std::function<bool(size_t, size_t)>& can_vertices_collide)
{
    auto is_endpoint = [&](int ei, int ej) {
        return edges(ei, 0) == edges(ej, 0) || edges(ei, 0) == edges(ej, 1)
            || edges(ei, 1) == edges(ej, 0) || edges(ei, 1) == edges(ej, 1);
    };

    auto can_collide = [&](int ei, int ej) {
        return can_vertices_collide(edges(ei, 0), edges(ej, 0))
            || can_vertices_collide(edges(ei, 0), edges(ej, 1))
            || can_vertices_collide(edges(ei, 1), edges(ej, 0))
            || can_vertices_collide(edges(ei, 1), edges(ej, 1));
    };

    getPairs(is_endpoint, can_collide, m_edgeItems, ee_candidates);
}

void HashGrid::getEdgeFacePairs(
    const Eigen::MatrixXi& edges,
    const Eigen::MatrixXi& faces,
    std::vector<EdgeFaceCandidate>& ef_candidates,
    const std::function<bool(size_t, size_t)>& can_vertices_collide)
{
    auto is_endpoint = [&](int ei, int fi) {
        // Check if the edge and face have a common end-point
        return edges(ei, 0) == faces(fi, 0) || edges(ei, 0) == faces(fi, 1)
            || edges(ei, 0) == faces(fi, 2) || edges(ei, 1) == faces(fi, 0)
            || edges(ei, 1) == faces(fi, 1) || edges(ei, 1) == faces(fi, 2);
    };

    auto can_collide = [&](int ei, int fi) {
        return can_vertices_collide(edges(ei, 0), faces(fi, 0))
            || can_vertices_collide(edges(ei, 0), faces(fi, 1))
            || can_vertices_collide(edges(ei, 0), faces(fi, 2))
            || can_vertices_collide(edges(ei, 1), faces(fi, 0))
            || can_vertices_collide(edges(ei, 1), faces(fi, 1))
            || can_vertices_collide(edges(ei, 1), faces(fi, 2));
    };

    getPairs(is_endpoint, can_collide, m_edgeItems, m_faceItems, ef_candidates);
}

void HashGrid::getFaceVertexPairs(
    const Eigen::MatrixXi& faces,
    std::vector<FaceVertexCandidate>& fv_candidates,
    const std::function<bool(size_t, size_t)>& can_vertices_collide)
{
    auto is_endpoint = [&](int fi, int vi) {
        return vi == faces(fi, 0) || vi == faces(fi, 1) || vi == faces(fi, 2);
    };

    auto can_collide = [&](int fi, int vi) {
        return can_vertices_collide(vi, faces(fi, 0))
            || can_vertices_collide(vi, faces(fi, 1))
            || can_vertices_collide(vi, faces(fi, 2));
    };

    getPairs(
        is_endpoint, can_collide, m_faceItems, m_vertexItems, fv_candidates);
}

} // namespace ipc


<|start_filename|>src/friction/tangent_basis.hpp<|end_filename|>
#pragma once

#include <Eigen/Core>
#include <Eigen/Geometry>

#include <ipc/utils/eigen_ext.hpp>

namespace ipc {

// Point - Point

template <
    typename DerivedP0,
    typename DerivedP1,
    typename T = typename DerivedP0::Scalar>
inline MatrixMax<T, 3, 2> point_point_tangent_basis(
    const Eigen::MatrixBase<DerivedP0>& p0,
    const Eigen::MatrixBase<DerivedP1>& p1)
{
    if (p0.size() == 2) {
        assert(p1.size() == 2);

        MatrixMax<T, 3, 2> basis(2, 1);

        auto p0_to_p1 = (p1 - p0).normalized();

        basis(0) = -p0_to_p1(1);
        basis(1) = p0_to_p1(0);

        return basis;
    } else {
        assert(p0.size() == 3 && p1.size() == 3);

        MatrixMax<T, 3, 2> basis(3, 2);

        auto p0_to_p1 = p1 - p0;

        Vector3<T> cross_x = cross(Vector3<T>::UnitX(), p0_to_p1);
        Vector3<T> cross_y = cross(Vector3<T>::UnitY(), p0_to_p1);

        if (cross_x.squaredNorm() > cross_y.squaredNorm()) {
            basis.col(0) = cross_x.normalized();
            basis.col(1) = cross(p0_to_p1, cross_x).normalized();
        } else {
            basis.col(0) = cross_y.normalized();
            basis.col(1) = cross(p0_to_p1, cross_y).normalized();
        }

        return basis;
    }
}

///////////////////////////////////////////////////////////////////////////////
// Point - Edge

template <
    typename DerivedP,
    typename DerivedE0,
    typename DerivedE1,
    typename T = typename DerivedP::Scalar>
inline MatrixMax<T, 3, 2> point_edge_tangent_basis(
    const Eigen::MatrixBase<DerivedP>& p,
    const Eigen::MatrixBase<DerivedE0>& e0,
    const Eigen::MatrixBase<DerivedE1>& e1)
{
    if (p.size() == 2) {
        assert(e0.size() == 2 && e1.size() == 2);

        MatrixMax<T, 3, 2> basis(2, 1);

        basis.col(0) = (e1 - e0).normalized();

        return basis;
    } else {
        assert(p.size() == 3 && e0.size() == 3 && e1.size() == 3);

        MatrixMax<T, 3, 2> basis(3, 2);

        auto e = e1 - e0;
        basis.col(0) = e.normalized();
        basis.col(1) = cross(e, p - e0).normalized();

        return basis;
    }
}

///////////////////////////////////////////////////////////////////////////////
// Edge - Edge

/// Compute a basis for the space tangent to the edge-edge pair.
template <
    typename DerivedEA0,
    typename DerivedEA1,
    typename DerivedEB0,
    typename DerivedEB1,
    typename T = typename DerivedEA0::Scalar>
inline Eigen::Matrix<T, 3, 2> edge_edge_tangent_basis(
    const Eigen::MatrixBase<DerivedEA0>& ea0,
    const Eigen::MatrixBase<DerivedEA1>& ea1,
    const Eigen::MatrixBase<DerivedEB0>& eb0,
    const Eigen::MatrixBase<DerivedEB1>& eb1)
{
    assert(ea0.size() == 3 && ea1.size() == 3);
    assert(eb0.size() == 3 && eb1.size() == 3);

    Eigen::Matrix<T, 3, 2> basis;

    auto ea = ea1 - ea0; // Edge A direction
    // The first basis vector is along edge A.
    basis.col(0) = ea.normalized();
    // The second basis vector is orthogonal to the first and the edge-edge
    // normal.
    auto normal = cross(ea, eb1 - eb0);
    // The normal will be zero if the edges are parallel (i.e. coplanar).
    assert(normal.norm() != 0);
    basis.col(1) = cross(normal, ea).normalized();

    return basis;
}

///////////////////////////////////////////////////////////////////////////////
// Point - Triangle

/// Compute a basis for the space tangent to the point-triangle pair.
template <
    typename DerivedP,
    typename DerivedT0,
    typename DerivedT1,
    typename DerivedT2,
    typename T = typename DerivedP::Scalar>
inline Eigen::Matrix<T, 3, 2> point_triangle_tangent_basis(
    const Eigen::MatrixBase<DerivedP>& p,
    const Eigen::MatrixBase<DerivedT0>& t0,
    const Eigen::MatrixBase<DerivedT1>& t1,
    const Eigen::MatrixBase<DerivedT2>& t2)
{
    assert(p.size() == 3 && t0.size() == 3 && t1.size() == 3 && t2.size() == 3);

    Eigen::Matrix<T, 3, 2> basis;

    auto e0 = t1 - t0;
    // The first basis vector is along first edge of the triangle.
    basis.col(0) = e0.normalized();
    // The second basis vector is orthogonal to the first and the triangle
    // normal.
    auto normal = cross(e0, t2 - t0);
    assert(normal.norm() != 0);
    basis.col(1) = cross(normal, e0).normalized();

    return basis;
}

} // namespace ipc


<|start_filename|>tests/friction/test_tangent_basis.cpp<|end_filename|>
#include <catch2/catch.hpp>

#include <ipc/friction/tangent_basis.hpp>

using namespace ipc;

TEST_CASE(
    "Point-triangle tangent basis", "[friction][point-triangle][tangent_basis]")
{
    Eigen::Vector3d p(0, 1, 0), t0(-1, 0, 1), t1(1, 0, 1), t2(0, 0, -1);

    Eigen::Matrix<double, 3, 2> basis =
        point_triangle_tangent_basis(p, t0, t1, t2);
    CAPTURE(basis);
    CHECK(abs(basis.col(0).dot(Eigen::Vector3d::UnitX())) == Approx(1));
    CHECK(abs(basis.col(1).dot(Eigen::Vector3d::UnitZ())) == Approx(1));
}

TEST_CASE("Edge-edge tangent basis", "[friction][edge-edge][tangent_basis]")
{
    Eigen::Vector3d ea0(-1, 0, 0), ea1(1, 0, 0), eb0(0, 0, -1), eb1(0, 0, 1);

    Eigen::Matrix<double, 3, 2> basis =
        edge_edge_tangent_basis(ea0, ea1, eb0, eb1);
    CAPTURE(basis);
    CHECK(abs(basis.col(0).dot(Eigen::Vector3d::UnitX())) == Approx(1));
    CHECK(abs(basis.col(1).dot(Eigen::Vector3d::UnitZ())) == Approx(1));
}

TEST_CASE("Point-edge tangent basis", "[friction][point-edge][tangent_basis]")
{
    Eigen::Vector3d p(0, 1, 0), e0(-1, 0, 0), e1(1, 0, 0);

    Eigen::Matrix<double, 3, 2> basis = point_edge_tangent_basis(p, e0, e1);
    CAPTURE(basis);
    CHECK(abs(basis.col(0).dot(Eigen::Vector3d::UnitX())) == Approx(1));
    CHECK(abs(basis.col(1).dot(Eigen::Vector3d::UnitZ())) == Approx(1));
}

TEST_CASE("Point-point tangent basis", "[friction][point-point][tangent_basis]")
{
    Eigen::Vector3d p0(0, 0, 0), p1(0, 0, 1);

    Eigen::Matrix<double, 3, 2> basis = point_point_tangent_basis(p0, p1);
    CAPTURE(basis);
    CHECK(abs(basis.col(0).dot(Eigen::Vector3d::UnitX())) == Approx(1));
    CHECK(abs(basis.col(1).dot(Eigen::Vector3d::UnitY())) == Approx(1));
}

TEST_CASE(
    "Point-edge tangent basis in 2D",
    "[friction][point-edge][tangent_basis][2D]")
{
    Eigen::Vector2d p(0, 1), e0(-1, 0), e1(1, 0);

    MatrixMax<double, 2, 1> basis = point_edge_tangent_basis(p, e0, e1);
    CAPTURE(basis);
    CHECK(abs(basis.dot(Eigen::Vector2d::UnitX())) == Approx(1));
}

TEST_CASE(
    "Point-point tangent basis in 2D",
    "[friction][point-point][tangent_basis][2D]")
{
    Eigen::Vector2d p0(0, 0), p1(0, 1);

    Eigen::Matrix<double, 2, 1> basis = point_point_tangent_basis(p0, p1);
    CAPTURE(basis);
    CHECK(abs(basis.dot(Eigen::Vector2d::UnitX())) == Approx(1));
}


<|start_filename|>tests/broad_phase/brute_force_comparison.hpp<|end_filename|>
#pragma once

#include <Eigen/Core>

#include <ipc/broad_phase/collision_candidate.hpp>

void brute_force_comparison(
    const Eigen::MatrixXd& V0,
    const Eigen::MatrixXd& V1,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    const Eigen::VectorXi& group_ids,
    ipc::Candidates& candidates,
    const double inflation_radius);


<|start_filename|>src/ccd/point_static_plane.cpp<|end_filename|>
#include <ipc/ccd/point_static_plane.hpp>

#include <ipc/distance/point_plane.hpp>

#include <array>

namespace ipc {

inline bool is_in_01(double x) { return 0 <= x && x <= 1; };

bool point_static_plane_ccd(
    const VectorMax3d& p_t0,
    const VectorMax3d& p_t1,
    const VectorMax3d& plane_origin,
    const VectorMax3d& plane_normal,
    double& toi,
    double conservative_rescaling)
{
    static constexpr double SMALL_TOI = 1e-6;

    int dim = p_t0.size();
    assert(p_t1.size() == dim);
    assert(plane_origin.size() == dim);
    assert(plane_normal.size() == dim);

    double initial_distance =
        sqrt(point_plane_distance(p_t0, plane_origin, plane_normal));

    if (initial_distance == 0) {
        IPC_LOG(warn("Initial point-plane distance is 0, returning toi=0!"));
        toi = 0;
        return true;
    }

    auto compute_toi = [&](double d) -> double {
        return (d * plane_normal.norm() + plane_normal.dot(plane_origin - p_t0))
            / plane_normal.dot(p_t1 - p_t0);
    };
    auto compute_tois = [&compute_toi](double d) -> std::array<double, 2> {
        return { { compute_toi(d), compute_toi(-d) } };
    };

    double min_distance = (1.0 - conservative_rescaling) * initial_distance;
    assert(min_distance < initial_distance);
    std::array<double, 2> tois = compute_tois(min_distance);

    bool is_impacting = is_in_01(tois[0]) || is_in_01(tois[1]);
    if (is_in_01(tois[0]) && is_in_01(tois[1])) {
        toi = std::min(tois[0], tois[1]);
    } else if (is_in_01(tois[0])) {
        toi = tois[0];
    } else if (is_in_01(tois[1])) {
        toi = tois[1];
    }

    if (is_impacting && toi < SMALL_TOI) {
        toi = compute_toi(/*d=*/0);
        is_impacting = is_in_01(toi);
        if (is_impacting) {
            toi *= conservative_rescaling;
            if (toi == 0) {
                IPC_LOG(warn(
                    "Point-static plane CCD is overly conservative (toi={:g} "
                    "and trajectory_length={:g}, but initial_distance={:g})!",
                    toi, (p_t1 - p_t0).norm(), initial_distance));
            }
        }
    }

    return is_impacting;
}

} // namespace ipc


<|start_filename|>tests/broad_phase/test_broad_phase.cpp<|end_filename|>
#include <catch2/catch.hpp>

#include <Eigen/Core>

#include <igl/IO>
#include <igl/edges.h>
#include <igl/Timer.h>

#include <ipc/ipc.hpp>
#include <ipc/broad_phase/hash_grid.hpp>
#include <ipc/broad_phase/spatial_hash.hpp>
#include <ipc/ccd/ccd.hpp>
#include <ipc/utils/faces_to_edges.hpp>

#include <broad_phase/brute_force_comparison.hpp>

using namespace ipc;

TEST_CASE("Vertex-Vertex Broad Phase", "[ccd][broad_phase]")
{
    Eigen::MatrixXd V_t0(4, 2);
    V_t0.row(0) << 1.11111, 0.5;  // edge 0 vertex 0
    V_t0.row(1) << 1.11111, 0.75; // edge 0 vertex 1
    V_t0.row(2) << 1, 0.5;        // edge 1 vertex 0
    V_t0.row(3) << 1, 0.75;       // edge 1 vertex 1

    Eigen::MatrixXd V_t1 = V_t0;
    V_t1.row(0) << 0.888889, 0.5;  // edge 0 vertex 0
    V_t1.row(1) << 0.888889, 0.75; // edge 0 vertex 1

    Eigen::MatrixXi E(2, 2);
    E.row(0) << 1, 0;
    E.row(1) << 2, 3;

    bool ignore_internal_vertices = GENERATE(false, true);

    BroadPhaseMethod method = GENERATE(
        BroadPhaseMethod::BRUTE_FORCE, BroadPhaseMethod::HASH_GRID,
        BroadPhaseMethod::SPATIAL_HASH);

    if (method == BroadPhaseMethod::SPATIAL_HASH && ignore_internal_vertices) {
        return; // Not implemented
    }

    double tolerance = 1e-6;
    int max_iterations = 1e7;

    bool is_valid_step;
    if (ignore_internal_vertices) {
        is_valid_step = ipc::is_step_collision_free(
            V_t0, V_t1, Eigen::VectorXi(), E, /*F=*/Eigen::MatrixXi(), method,
            tolerance, max_iterations);
    } else {
        is_valid_step = ipc::is_step_collision_free(
            V_t0, V_t1, E, /*F=*/Eigen::MatrixXi(), method, tolerance,
            max_iterations);
    }

    CAPTURE(ignore_internal_vertices);
    CHECK(!is_valid_step);
}

TEST_CASE("Entire 2D Mesh", "[ccd][broad_phase]")
{
    Eigen::MatrixXd V_t0;
    igl::readCSV(std::string(TEST_DATA_DIR) + "V_t0.csv", V_t0);

    Eigen::MatrixXd V_t1;
    igl::readCSV(std::string(TEST_DATA_DIR) + "V_t1.csv", V_t1);

    Eigen::MatrixXi E;
    igl::readCSV(std::string(TEST_DATA_DIR) + "E.csv", E);
    E.array() -= 1; // NOTE: Convert from OBJ format to index

    Eigen::VectorXi codim_V;
    Eigen::MatrixXi F;

    BroadPhaseMethod method = GENERATE(
        BroadPhaseMethod::BRUTE_FORCE, BroadPhaseMethod::HASH_GRID
        // , BroadPhaseMethod::SPATIAL_HASH
    );

    double tolerance = 1e-6;
    int max_iterations = 1e7;

    bool is_valid_step;
    SECTION("2D")
    {
        is_valid_step = ipc::is_step_collision_free(
            V_t0.leftCols(2), V_t1.leftCols(2), codim_V, E, F, method,
            tolerance, max_iterations);
    }
    SECTION("3D")
    {
        is_valid_step = ipc::is_step_collision_free(
            V_t0, V_t1, codim_V, E, F, method, tolerance, max_iterations);
    }

    CAPTURE(method);
    CHECK(!is_valid_step);
}

TEST_CASE(
    "Test construct_constraint_set() with codimensional points",
    "[construct_constraint_set][broad_phase]")
{
    const double dhat = 1e-3;
    Eigen::MatrixXd V_rest, V;
    igl::readDMAT(
        std::string(TEST_DATA_DIR) + "codim-points/V_rest.dmat", V_rest);
    igl::readDMAT(std::string(TEST_DATA_DIR) + "codim-points/V.dmat", V);
    Eigen::MatrixXi E, F;
    igl::readDMAT(std::string(TEST_DATA_DIR) + "codim-points/E.dmat", E);
    igl::readDMAT(std::string(TEST_DATA_DIR) + "codim-points/F.dmat", F);
    const Eigen::MatrixXi F2E = faces_to_edges(F, E);

#ifdef NDEBUG
    BroadPhaseMethod method = GENERATE(
        BroadPhaseMethod::BRUTE_FORCE, BroadPhaseMethod::HASH_GRID //,
        // BroadPhaseMethod::SPATIAL_HASH
    );
#else
    BroadPhaseMethod method =
        GENERATE(BroadPhaseMethod::HASH_GRID //,
                                             // BroadPhaseMethod::SPATIAL_HASH
        );
#endif

    Constraints constraint_set;
    construct_constraint_set(
        V_rest, V, E, F, dhat, constraint_set, F2E, /*dmin=*/0, method);

    CHECK(constraint_set.size() != 0);
}

TEST_CASE(
    "Compare HashGrid/SpatialHash against brute force",
    "[hash_grid][spatial_hash][brute_force]")
{
    using namespace ipc;

    Eigen::MatrixXd V0, U;
    Eigen::MatrixXi E, F;
    Eigen::VectorXi group_ids;

    SECTION("Simple")
    {
        V0.resize(4, 3);
        V0.row(0) << -1, -1, 0;
        V0.row(1) << 1, -1, 0;
        V0.row(2) << 0, 1, 1;
        V0.row(3) << 0, 1, -1;

        E.resize(2, 2);
        E.row(0) << 0, 1;
        E.row(1) << 2, 3;

        SECTION("Without group ids") { }
        SECTION("With group ids")
        {
            group_ids.resize(4);
            group_ids << 0, 0, 1, 1;
        }

        F.resize(0, 3);

        U = Eigen::MatrixXd::Zero(V0.rows(), V0.cols());
        U.col(1).head(2).setConstant(2);
        U.col(1).tail(2).setConstant(-2);
    }
    SECTION("Complex")
    {
        // #ifdef NDEBUG
        //         std::string filename =
        //             GENERATE(std::string("cube.obj"),
        //             std::string("bunny.obj"));
        // #else
        std::string filename = "cube.obj";
        // #endif
        std::string mesh_path = std::string(TEST_DATA_DIR) + filename;
        bool success = igl::read_triangle_mesh(mesh_path, V0, F);
        REQUIRE(success);
        igl::edges(F, E);

        U = Eigen::MatrixXd::Zero(V0.rows(), V0.cols());
        U.col(1).setOnes();
    }

    Candidates candidates;

    // int check_hash_grid = GENERATE(0, 1);
    int check_hash_grid = 1;

    double inflation_radius = 1e-2; // GENERATE(0.0, 1e-4, 1e-3, 1e-2, 1e-1);

    auto can_collide = [&group_ids](size_t vi, size_t vj) {
        return group_ids.size() == 0 || group_ids(vi) != group_ids(vj);
    };

    std::function<void(const Eigen::MatrixXd&)> build_candidates;
    if (check_hash_grid) {
        build_candidates = [&](const Eigen::MatrixXd& V1) {
            candidates.clear();

            HashGrid hashgrid;
            hashgrid.resize(V0, V1, E, inflation_radius);
            hashgrid.addVertices(V0, V1, inflation_radius);
            hashgrid.addEdges(V0, V1, E, inflation_radius);
            hashgrid.addFaces(V0, V1, F, inflation_radius);

            hashgrid.getVertexEdgePairs(
                E, candidates.ev_candidates, can_collide);
            hashgrid.getEdgeEdgePairs(E, candidates.ee_candidates, can_collide);
            hashgrid.getFaceVertexPairs(
                F, candidates.fv_candidates, can_collide);
        };
    } else {
        build_candidates = [&](const Eigen::MatrixXd& V1) {
            candidates.clear();

            SpatialHash sh;
            sh.build(V0, V1, E, F, inflation_radius);
            // TODO: use can_collide
            group_ids.resize(0);
            sh.queryMeshForCandidates(
                V0, V1, E, F, candidates,
                /*queryEV=*/true, /*queryEE=*/true, /*queryFV=*/true);
        };
    }

    for (int i = 0; i < 2; i++) {
        Eigen::MatrixXd V1 = V0 + U;

        build_candidates(V1);

        brute_force_comparison(
            V0, V1, E, F, group_ids, candidates, inflation_radius);

        U.setRandom();
        U *= 3;
    }
}


<|start_filename|>tests/distance/test_point_plane.cpp<|end_filename|>
#include <catch2/catch.hpp>

#include <finitediff.hpp>

#include <ipc/distance/point_plane.hpp>

using namespace ipc;

TEST_CASE("Point-plane distance", "[distance][point-plane]")
{
    double x = GENERATE(take(10, random(-100.0, 100.0)));
    double y = GENERATE(take(10, random(-100.0, 100.0)));
    double z = GENERATE(take(10, random(-100.0, 100.0)));
    Eigen::Vector3d p(x, y, z);

    double y_plane = GENERATE(take(10, random(-100.0, 100.0)));
    Eigen::Vector3d t0(-1, y_plane, 0), t1(1, y_plane, -1), t2(1, y_plane, 0);

    double distance = point_plane_distance(p, t0, t1, t2);
    double expected_distance = abs(y - y_plane);
    CHECK(distance == Approx(expected_distance * expected_distance));
}

TEST_CASE("Point-plane distance gradient", "[distance][point-plane][gradient]")
{
    double x = GENERATE(take(10, random(-10.0, 10.0)));
    double y = GENERATE(take(10, random(-10.0, 10.0)));
    double z = GENERATE(take(10, random(-10.0, 10.0)));
    Eigen::Vector3d p(x, y, z);

    double y_plane = GENERATE(take(10, random(-10.0, 10.0)));
    Eigen::Vector3d t0(-1, y_plane, 0), t1(1, y_plane, -1), t2(1, y_plane, 0);

    Eigen::VectorXd grad;
    point_plane_distance_gradient(p, t0, t1, t2, grad);

    Eigen::VectorXd x_vec(12);
    x_vec << p, t0, t1, t2;
    Eigen::VectorXd expected_grad;
    expected_grad.resize(grad.size());
    fd::finite_gradient(
        x_vec,
        [](const Eigen::VectorXd& x) {
            return point_plane_distance(
                x.head<3>(), x.segment<3>(3), x.segment<3>(6), x.tail<3>());
        },
        expected_grad);

    CAPTURE((grad - expected_grad).norm());
    CHECK(fd::compare_gradient(grad, expected_grad));
}

TEST_CASE("Point-plane distance hessian", "[distance][point-plane][hessian]")
{
    double x = GENERATE(take(10, random(-10.0, 10.0)));
    double y = GENERATE(take(10, random(-10.0, 10.0)));
    double z = GENERATE(take(10, random(-10.0, 10.0)));
    Eigen::Vector3d p(x, y, z);

    double y_plane = GENERATE(take(10, random(-10.0, 10.0)));
    Eigen::Vector3d t0(-1, y_plane, 0), t1(1, y_plane, -1), t2(1, y_plane, 0);

    Eigen::MatrixXd hess;
    point_plane_distance_hessian(p, t0, t1, t2, hess);

    Eigen::VectorXd x_vec(12);
    x_vec << p, t0, t1, t2;
    Eigen::MatrixXd expected_hess;
    expected_hess.resize(hess.rows(), hess.cols());
    fd::finite_hessian(
        x_vec,
        [](const Eigen::VectorXd& x) {
            return point_plane_distance(
                x.head<3>(), x.segment<3>(3), x.segment<3>(6), x.tail<3>());
        },
        expected_hess);

    CAPTURE((hess - expected_hess).norm());
    CHECK(fd::compare_hessian(hess, expected_hess, 5e-2));
}


<|start_filename|>cmake/recipes/evouga_ccd.cmake<|end_filename|>
# <NAME>'s CCD Library

if(TARGET evouga::ccd)
    return()
endif()

message(STATUS "Third-party: creating target 'evouga::ccd'")

include(FetchContent)
FetchContent_Declare(
    evccd
    GIT_REPOSITORY https://github.com/evouga/collisiondetection.git
    GIT_TAG e5fe5c9767207df5047e375fb20180a665ae186f
    GIT_SHALLOW FALSE
)

FetchContent_GetProperties(evccd)
if(NOT evccd_POPULATED)
    FetchContent_Populate(evccd)
endif()

# file(GLOB EVOUGA_CCD_SOURCE_FILES "${evccd_SOURCE_DIR}/src/*.cpp")
add_library(evouga_ccd
    "${evccd_SOURCE_DIR}/src/CTCD.cpp"
)
add_library(evouga::ccd ALIAS evouga_ccd)

target_include_directories(evouga_ccd PUBLIC "${evccd_SOURCE_DIR}/include")

include(eigen)
target_link_libraries(evouga_ccd PUBLIC Eigen3::Eigen)

# Turn off floating point contraction for CCD robustness
target_compile_options(evouga_ccd PRIVATE "-ffp-contract=off")


<|start_filename|>tests/distance/test_point_triangle.cpp<|end_filename|>
#include <catch2/catch.hpp>

#include <finitediff.hpp>
#include <igl/PI.h>

#include <ipc/distance/point_triangle.hpp>
#include <ipc/utils/eigen_ext.hpp>

using namespace ipc;

inline Eigen::Vector2d
edge_normal(const Eigen::Vector2d& e0, const Eigen::Vector2d& e1)
{
    Eigen::Vector2d e = e1 - e0;
    Eigen::Vector2d normal(-e.y(), e.x());
    return normal.normalized();
}

TEST_CASE("Point-triangle distance", "[distance][point-triangle]")
{
    double py = GENERATE(-10, -1, -1e-12, 0, 1e-12, 1, 10);
    Eigen::Vector3d p(0, py, 0);
    Eigen::Vector3d t0(-1, 0, 1);
    Eigen::Vector3d t1(1, 0, 1);
    Eigen::Vector3d t2(0, 0, -1);

    Eigen::Vector3d closest_point;
    SECTION("closest to triangle")
    {
        double pz = GENERATE(0, -1 + 1e-12, -1, 1, 1 - 1e-12);
        p.z() = pz;
        closest_point = p;
        closest_point.y() = 0;
    }
    SECTION("closest to t0")
    {
        double px = GENERATE(-1, -1 - 1e-12, -11);
        p.x() = px;
        p.z() = t0.z();
        closest_point = t0;
    }
    SECTION("closest to t1")
    {
        double px = GENERATE(1, 1 + 1e-12, 11);
        p.x() = px;
        p.z() = t1.z();
        closest_point = t1;
    }
    SECTION("closest to t2")
    {
        double pz = GENERATE(-1, -1 - 1e-12, -11);
        p.z() = pz;
        closest_point = t2;
    }
    SECTION("closest to t0t1")
    {
        double alpha = GENERATE(0.0, 1e-4, 0.5, 1.0 - 1e-4, 1.0);
        closest_point = (t1 - t0) * alpha + t0;
        Eigen::Vector2d perp = edge_normal(
            Eigen::Vector2d(t0.x(), t0.z()), Eigen::Vector2d(t1.x(), t1.z()));
        double scale = GENERATE(0, 1e-12, 1e-4, 1, 2, 11, 1000);
        p.x() = closest_point.x() + scale * perp.x();
        p.z() = closest_point.z() + scale * perp.y();
    }
    SECTION("closest to t1t2")
    {
        double alpha = GENERATE(0.0, 1e-4, 0.5, 1.0 - 1e-4, 1.0);
        closest_point = (t2 - t1) * alpha + t1;
        Eigen::Vector2d perp = edge_normal(
            Eigen::Vector2d(t1.x(), t1.z()), Eigen::Vector2d(t2.x(), t2.z()));
        double scale = GENERATE(0, 1e-12, 1e-4, 1, 2, 11, 1000);
        p.x() = closest_point.x() + scale * perp.x();
        p.z() = closest_point.z() + scale * perp.y();
    }
    SECTION("closest to t2t0")
    {
        double alpha = GENERATE(0.0, 1e-4, 0.5, 1.0 - 1e-4, 1.0);
        closest_point = (t0 - t2) * alpha + t2;
        Eigen::Vector2d perp = edge_normal(
            Eigen::Vector2d(t2.x(), t2.z()), Eigen::Vector2d(t0.x(), t0.z()));
        double scale = GENERATE(0, 1e-12, 1e-4, 1, 2, 11, 1000);
        p.x() = closest_point.x() + scale * perp.x();
        p.z() = closest_point.z() + scale * perp.y();
    }

    double distance = point_triangle_distance(p, t0, t1, t2);
    CAPTURE(py, closest_point.x(), closest_point.y(), closest_point.z());
    CHECK(
        distance
        == Approx(point_point_distance(p, closest_point)).margin(1e-12));
}

TEST_CASE(
    "Point-triangle distance gradient", "[distance][point-triangle][grad]")
{
    double py = GENERATE(-10, -1, -1e-12, 0, 1e-12, 1, 10);
    Eigen::Vector3d p(0, py, 0);
    Eigen::Vector3d t0(-1, 0, 1);
    Eigen::Vector3d t1(1, 0, 1);
    Eigen::Vector3d t2(0, 0, -1);

    SECTION("closest to triangle")
    {
        double pz = GENERATE(0, -1 + 1e-12, -1, 1, 1 - 1e-12);
        p.z() = pz;
    }
    SECTION("closest to t0")
    {
        double px = GENERATE(-1, -1 - 1e-12, -11);
        p.x() = px;
        p.z() = t0.z();
    }
    SECTION("closest to t1")
    {
        double px = GENERATE(1, 1 + 1e-12, 11);
        p.x() = px;
        p.z() = t1.z();
    }
    SECTION("closest to t2")
    {
        double pz = GENERATE(-1, -1 - 1e-12, -11);
        p.z() = pz;
    }
    SECTION("closest to t0t1")
    {
        double alpha = GENERATE(0.0, 1e-4, 0.5, 1.0 - 1e-4, 1.0);
        Eigen::Vector3d closest_point = (t1 - t0) * alpha + t0;
        Eigen::Vector2d perp = edge_normal(
            Eigen::Vector2d(t0.x(), t0.z()), Eigen::Vector2d(t1.x(), t1.z()));
        // double scale = GENERATE(0, 1e-12, 1e-4, 1, 2, 11, 1000);
        double scale = GENERATE(0, 1e-12, 1e-4, 1, 2, 11);
        p.x() = closest_point.x() + scale * perp.x();
        p.z() = closest_point.z() + scale * perp.y();
    }
    SECTION("closest to t1t2")
    {
        double alpha = GENERATE(0.0, 1e-4, 0.5, 1.0 - 1e-4, 1.0);
        Eigen::Vector3d closest_point = (t2 - t1) * alpha + t1;
        Eigen::Vector2d perp = edge_normal(
            Eigen::Vector2d(t1.x(), t1.z()), Eigen::Vector2d(t2.x(), t2.z()));
        // double scale = GENERATE(0, 1e-12, 1e-4, 1, 2, 11, 1000);
        double scale = GENERATE(0, 1e-12, 1e-4, 1, 2, 11);
        p.x() = closest_point.x() + scale * perp.x();
        p.z() = closest_point.z() + scale * perp.y();
    }
    SECTION("closest to t2t0")
    {
        double alpha = GENERATE(0.0, 1e-4, 0.5, 1.0 - 1e-4, 1.0);
        Eigen::Vector3d closest_point = (t0 - t2) * alpha + t2;
        Eigen::Vector2d perp = edge_normal(
            Eigen::Vector2d(t2.x(), t2.z()), Eigen::Vector2d(t0.x(), t0.z()));
        // double scale = GENERATE(0, 1e-12, 1e-4, 1, 2, 11, 1000);
        double scale = GENERATE(0, 1e-12, 1e-4, 1, 2, 11);
        p.x() = closest_point.x() + scale * perp.x();
        p.z() = closest_point.z() + scale * perp.y();
    }

    Eigen::VectorXd x(12);
    x.segment<3>(0) = p;
    x.segment<3>(3) = t0;
    x.segment<3>(6) = t1;
    x.segment<3>(9) = t2;
    // Compute the gradient using finite differences
    auto f = [&](const Eigen::VectorXd& x) {
        return point_triangle_distance(
            x.segment<3>(0), x.segment<3>(3), x.segment<3>(6), x.segment<3>(9));
    };

    Eigen::VectorXd grad;
    point_triangle_distance_gradient(p, t0, t1, t2, grad);

    Eigen::VectorXd fgrad;
    fd::finite_gradient(x, f, fgrad);

    CHECK(fd::compare_gradient(grad, fgrad));
}

TEST_CASE("Point-triangle distance hessian", "[distance][point-triangle][hess]")
{
    double py = GENERATE(-10, -1, -1e-12, 0, 1e-12, 1, 10);
    Eigen::Vector3d p(0, py, 0);
    Eigen::Vector3d t0(-1, 0, 1);
    Eigen::Vector3d t1(1, 0, 1);
    Eigen::Vector3d t2(0, 0, -1);

    SECTION("closest to triangle")
    {
        double pz = GENERATE(0, -1 + 1e-12, -1, 1, 1 - 1e-12);
        p.z() = pz;
    }
    SECTION("closest to t0")
    {
        double px = GENERATE(-1, -1 - 1e-12, -11);
        p.x() = px;
        p.z() = t0.z();
    }
    SECTION("closest to t1")
    {
        double px = GENERATE(1, 1 + 1e-12, 11);
        p.x() = px;
        p.z() = t1.z();
    }
    SECTION("closest to t2")
    {
        double pz = GENERATE(-1, -1 - 1e-12, -11);
        p.z() = pz;
    }
    SECTION("closest to t0t1")
    {
        double alpha = GENERATE(0.0, 1e-4, 0.5, 1.0 - 1e-4, 1.0);
        Eigen::Vector3d closest_point = (t1 - t0) * alpha + t0;
        Eigen::Vector2d perp = edge_normal(
            Eigen::Vector2d(t0.x(), t0.z()), Eigen::Vector2d(t1.x(), t1.z()));
        // double scale = GENERATE(0, 1e-12, 1e-4, 1, 2, 11, 1000);
        double scale = GENERATE(0, 1e-12, 1e-4, 1, 2, 11);
        p.x() = closest_point.x() + scale * perp.x();
        p.z() = closest_point.z() + scale * perp.y();
    }
    SECTION("closest to t1t2")
    {
        double alpha = GENERATE(0.0, 1e-4, 0.5, 1.0 - 1e-4, 1.0);
        Eigen::Vector3d closest_point = (t2 - t1) * alpha + t1;
        Eigen::Vector2d perp = edge_normal(
            Eigen::Vector2d(t1.x(), t1.z()), Eigen::Vector2d(t2.x(), t2.z()));
        // double scale = GENERATE(0, 1e-12, 1e-4, 1, 2, 11, 1000);
        double scale = GENERATE(0, 1e-12, 1e-4, 1, 2, 11);
        p.x() = closest_point.x() + scale * perp.x();
        p.z() = closest_point.z() + scale * perp.y();
    }
    SECTION("closest to t2t0")
    {
        double alpha = GENERATE(0.0, 1e-4, 0.5, 1.0 - 1e-4, 1.0);
        Eigen::Vector3d closest_point = (t0 - t2) * alpha + t2;
        Eigen::Vector2d perp = edge_normal(
            Eigen::Vector2d(t2.x(), t2.z()), Eigen::Vector2d(t0.x(), t0.z()));
        // double scale = GENERATE(0, 1e-12, 1e-4, 1, 2, 11, 1000);
        double scale = GENERATE(0, 1e-12, 1e-4, 1, 2, 11);
        p.x() = closest_point.x() + scale * perp.x();
        p.z() = closest_point.z() + scale * perp.y();
    }

    PointTriangleDistanceType dtype =
        point_triangle_distance_type(p, t0, t1, t2);

    Eigen::VectorXd x(12);
    x.segment<3>(0) = p;
    x.segment<3>(3) = t0;
    x.segment<3>(6) = t1;
    x.segment<3>(9) = t2;
    // Compute the gradient using finite differences
    auto f = [&](const Eigen::VectorXd& x) {
        return point_triangle_distance(
            x.segment<3>(0), x.segment<3>(3), x.segment<3>(6), x.segment<3>(9),
            dtype);
    };

    Eigen::MatrixXd hess;
    point_triangle_distance_hessian(p, t0, t1, t2, hess);

    Eigen::MatrixXd fhess;
    fd::finite_hessian(x, f, fhess);

    CAPTURE(dtype);
    CHECK(fd::compare_hessian(hess, fhess, 1e-2));
}


<|start_filename|>src/ccd/point_static_plane.hpp<|end_filename|>
#pragma once

#include <ipc/ccd/ccd.hpp>

namespace ipc {

bool point_static_plane_ccd(
    const VectorMax3d& p_t0,
    const VectorMax3d& p_t1,
    const VectorMax3d& plane_origin,
    const VectorMax3d& plane_normal,
    double& toi,
    const double conservative_rescaling = DEFAULT_CCD_CONSERVATIVE_RESCALING);

} // namespace ipc


<|start_filename|>python/src/distance/point_triangle.cpp<|end_filename|>
#include <pybind11/pybind11.h>
#include <pybind11/eigen.h>

#include <ipc/distance/distance_type.hpp>
#include <ipc/distance/point_triangle.hpp>

#include "../utils.hpp"

namespace py = pybind11;
using namespace ipc;

void define_point_triangle_distance_functions(py::module_& m)
{
    m.def(
        "point_triangle_distance",
        [](const Eigen::Vector3d& p, const Eigen::Vector3d& t0,
           const Eigen::Vector3d& t1, const Eigen::Vector3d& t2,
           const PointTriangleDistanceType* dtype) {
            if (dtype == nullptr) {
                return point_triangle_distance(p, t0, t1, t2);
            } else {
                return point_triangle_distance(p, t0, t1, t2, *dtype);
            }
        },
        R"ipc_Qu8mg5v7(
        Compute the distance between a two lines segments in 3D.

        Parameters:
            p : point.
            t0: first vertex of the triangle.
            t1: second vertex of the triangle.
            t2: third vertex of the triangle.
            dtype: (Optional) point-triangle distance type to compute

        Returns:
            The distance between the point and triangle.

        Note:
            The distance is actually squared distance.
        )ipc_Qu8mg5v7",
        py::arg("p"), py::arg("t0"), py::arg("t1"), py::arg("t2"),
        py::arg("dtype") = py::none());

    m.def(
        "point_triangle_distance_gradient",
        [](const Eigen::Vector3d& p, const Eigen::Vector3d& t0,
           const Eigen::Vector3d& t1, const Eigen::Vector3d& t2,
           const PointTriangleDistanceType* dtype) {
            Vector<double, 12> grad;
            if (dtype == nullptr) {
                point_triangle_distance_gradient(p, t0, t1, t2, grad);
            } else {
                point_triangle_distance_gradient(p, t0, t1, t2, *dtype, grad);
            }
            return grad;
        },
        R"ipc_Qu8mg5v7(
        Compute the gradient of the distance between a two lines segments.

        Parameters:
            p : point
            t0: first vertex of the triangle
            t1: second vertex of the triangle
            t2: third vertex of the triangle
            dtype: (optional) point-triangle distance type to compute

        Returns:
            The gradient of the distance wrt ea0, ea1, eb0, and eb1.

        Note:
            The distance is actually squared distance.
        )ipc_Qu8mg5v7",
        py::arg("p"), py::arg("t0"), py::arg("t1"), py::arg("t2"),
        py::arg("dtype") = py::none());

    m.def(
        "point_triangle_distance_hessian",
        [](const Eigen::Vector3d& p, const Eigen::Vector3d& t0,
           const Eigen::Vector3d& t1, const Eigen::Vector3d& t2,
           const PointTriangleDistanceType* dtype) {
            Eigen::Matrix<double, 12, 12> hess;
            if (dtype == nullptr) {
                point_triangle_distance_hessian(p, t0, t1, t2, hess);
            } else {
                point_triangle_distance_hessian(p, t0, t1, t2, *dtype, hess);
            }
            return hess;
        },
        R"ipc_Qu8mg5v7(
        Compute the hessian of the distance between a two lines segments.

        Parameters:
            p : point
            t0: first vertex of the triangle
            t1: second vertex of the triangle
            t2: third vertex of the triangle
            dtype: (optional) point-triangle distance type to compute

        Returns:
            The hessian of the distance wrt ea0, ea1, eb0, and eb1.

        Note:
            The distance is actually squared distance.
        )ipc_Qu8mg5v7",
        py::arg("p"), py::arg("t0"), py::arg("t1"), py::arg("t2"),
        py::arg("dtype") = py::none());
}


<|start_filename|>src/utils/faces_to_edges.cpp<|end_filename|>
#include <ipc/utils/faces_to_edges.hpp>

#include <ipc/utils/unordered_map_and_set.hpp>

namespace ipc {

Eigen::MatrixXi
faces_to_edges(const Eigen::MatrixXi& F, const Eigen::MatrixXi& E)
{
    auto max_vi = E.maxCoeff();
    auto hash_edge = [&max_vi](const Eigen::RowVector2i& e) {
        return e.minCoeff() * max_vi + e.maxCoeff();
    };
    auto eq_edges = [](const Eigen::RowVector2i& ea,
                       const Eigen::RowVector2i& eb) {
        return ea.minCoeff() == eb.minCoeff() && ea.maxCoeff() == eb.maxCoeff();
    };

    unordered_map<
        Eigen::RowVector2i, int, decltype(hash_edge), decltype(eq_edges)>
        edge_map(/*bucket_count=*/E.rows(), hash_edge, eq_edges);
    for (int ei = 0; ei < E.rows(); ei++) {
        edge_map[E.row(ei)] = ei;
    }

    Eigen::MatrixXi F2E(F.rows(), F.cols());
    for (int fi = 0; fi < F.rows(); fi++) {
        for (int fj = 0; fj < F.cols(); fj++) {
            Eigen::RowVector2i e(F(fi, fj), F(fi, (fj + 1) % F.cols()));
            auto search = edge_map.find(e);
            if (search != edge_map.end()) {
                F2E(fi, fj) = search->second;
            } else {
                throw std::runtime_error("Unable to find edge!");
            }
        }
    }

    return F2E;
}

} // namespace ipc


<|start_filename|>src/utils/logger.hpp<|end_filename|>
#pragma once

#ifdef IPC_TOOLKIT_WITH_LOGGER

#include <spdlog/spdlog.h>
#include <spdlog/async.h>
#include <spdlog/fmt/bundled/ranges.h>

#include <ostream>

/// Helper macro that makes it easier to disable logging completly at compile
/// time (e.g., IPC_LOG(info("A useful info log"))).
#define IPC_LOG(message) ipc::logger().message
/// Helper macro that make it easier to disable logging completly at compile
/// time (e.g., IPC_LOGGER(init())).
#define IPC_LOGGER(function) ipc::Logger::function

namespace ipc {

struct Logger {
    static std::shared_ptr<spdlog::async_logger> _logger;

    // By default, write to stdout, but don't write to any file
    static void init(
        bool use_cout = true,
        const std::string& filename = "",
        bool truncate = true);
    static void init(std::ostream& os);
    static void init(std::vector<spdlog::sink_ptr>& sinks);
};

// Retrieve current logger, or create one if not available
inline spdlog::async_logger& logger()
{
    if (!Logger::_logger) {
        Logger::init();
    }
    return *Logger::_logger;
}

} // namespace ipc

#else

#include <fmt/format.h>

// Dummy macros to disable logging completly.
#define IPC_LOG(message)
#define IPC_LOGGER(function)

#endif


<|start_filename|>src/distance/point_line.cpp<|end_filename|>
#include <ipc/distance/point_line.hpp>

namespace ipc {
namespace autogen {

    // This function was generated by the Symbolic Math Toolbox version 8.3.
    // 16-Mar-2020 15:56:29
    void point_line_distance_gradient_2D(
        double v01,
        double v02,
        double v11,
        double v12,
        double v21,
        double v22,
        double g[6])
    {
        double t13, t14, t23, t25, t24, t26, t27;

        t13 = -v21 + v11;
        t14 = -v22 + v12;
        t23 = 1.0 / (t13 * t13 + t14 * t14);
        t25 = ((v11 * v22 + -(v12 * v21)) + t14 * v01) + -(t13 * v02);
        t24 = t23 * t23;
        t26 = t25 * t25;
        t27 = (v11 * 2.0 + -(v21 * 2.0)) * t24 * t26;
        t26 *= (v12 * 2.0 + -(v22 * 2.0)) * t24;
        g[0] = t14 * t23 * t25 * 2.0;
        g[1] = t13 * t23 * t25 * -2.0;
        t24 = t23 * t25;
        g[2] = -t27 - t24 * (-v22 + v02) * 2.0;
        g[3] = -t26 + t24 * (-v21 + v01) * 2.0;
        g[4] = t27 + t24 * (v02 - v12) * 2.0;
        g[5] = t26 - t24 * (v01 - v11) * 2.0;
    }

    // This function was generated by the Symbolic Math Toolbox version 8.3.
    // 10-Jun-2019 18:02:37
    void point_line_distance_gradient_3D(
        double v01,
        double v02,
        double v03,
        double v11,
        double v12,
        double v13,
        double v21,
        double v22,
        double v23,
        double g[9])
    {
        double t17, t18, t19, t20, t21, t22, t23, t24, t25, t42, t44, t45, t46,
            t43, t50, t51, t52;

        t17 = -v11 + v01;
        t18 = -v12 + v02;
        t19 = -v13 + v03;
        t20 = -v21 + v01;
        t21 = -v22 + v02;
        t22 = -v23 + v03;
        t23 = -v21 + v11;
        t24 = -v22 + v12;
        t25 = -v23 + v13;
        t42 = 1.0 / ((t23 * t23 + t24 * t24) + t25 * t25);
        t44 = t17 * t21 + -(t18 * t20);
        t45 = t17 * t22 + -(t19 * t20);
        t46 = t18 * t22 + -(t19 * t21);
        t43 = t42 * t42;
        t50 = (t44 * t44 + t45 * t45) + t46 * t46;
        t51 = (v11 * 2.0 + -(v21 * 2.0)) * t43 * t50;
        t52 = (v12 * 2.0 + -(v22 * 2.0)) * t43 * t50;
        t43 = (v13 * 2.0 + -(v23 * 2.0)) * t43 * t50;
        g[0] = t42 * (t24 * t44 * 2.0 + t25 * t45 * 2.0);
        g[1] = -t42 * (t23 * t44 * 2.0 - t25 * t46 * 2.0);
        g[2] = -t42 * (t23 * t45 * 2.0 + t24 * t46 * 2.0);
        g[3] = -t51 - t42 * (t21 * t44 * 2.0 + t22 * t45 * 2.0);
        g[4] = -t52 + t42 * (t20 * t44 * 2.0 - t22 * t46 * 2.0);
        g[5] = -t43 + t42 * (t20 * t45 * 2.0 + t21 * t46 * 2.0);
        g[6] = t51 + t42 * (t18 * t44 * 2.0 + t19 * t45 * 2.0);
        g[7] = t52 - t42 * (t17 * t44 * 2.0 - t19 * t46 * 2.0);
        g[8] = t43 - t42 * (t17 * t45 * 2.0 + t18 * t46 * 2.0);
    }

    // This function was generated by the Symbolic Math Toolbox version 8.3.
    // 16-Mar-2020 15:56:29
    void point_line_distance_hessian_2D(
        double v01,
        double v02,
        double v11,
        double v12,
        double v21,
        double v22,
        double H[36])
    {
        double t15, t16, t17, t18, t19, t20, t21, t22, t23, t24, t31, t34, t32,
            t33, t35, t60, t59, t62, t64, t65, t68, t71, t72, t75, t76, t77,
            t78, t79, t90, t92, t94, t96, t99, t93, t97, t98, t100, t102_tmp;

        t15 = -v11 + v01;
        t16 = -v12 + v02;
        t17 = -v21 + v01;
        t18 = -v22 + v02;
        t19 = -v21 + v11;
        t20 = -v22 + v12;
        t21 = v11 * 2.0 + -(v21 * 2.0);
        t22 = v12 * 2.0 + -(v22 * 2.0);
        t23 = t19 * t19;
        t24 = t20 * t20;
        t31 = 1.0 / (t23 + t24);
        t34 = ((v11 * v22 + -(v12 * v21)) + t20 * v01) + -(t19 * v02);
        t32 = t31 * t31;
        t33 = pow(t31, 3.0);
        t35 = t34 * t34;
        t60 = t31 * t34 * 2.0;
        t59 = -(t19 * t20 * t31 * 2.0);
        t62 = t32 * t35 * 2.0;
        t64 = t21 * t21 * t33 * t35 * 2.0;
        t65 = t22 * t22 * t33 * t35 * 2.0;
        t68 = t15 * t21 * t32 * t34 * 2.0;
        t71 = t16 * t22 * t32 * t34 * 2.0;
        t72 = t17 * t21 * t32 * t34 * 2.0;
        t75 = t18 * t22 * t32 * t34 * 2.0;
        t76 = t19 * t21 * t32 * t34 * 2.0;
        t77 = t20 * t21 * t32 * t34 * 2.0;
        t78 = t19 * t22 * t32 * t34 * 2.0;
        t79 = t20 * t22 * t32 * t34 * 2.0;
        t90 = t21 * t22 * t33 * t35 * 2.0;
        t92 = t16 * t20 * t31 * 2.0 + t77;
        t94 = -(t17 * t19 * t31 * 2.0) + t78;
        t96 = (t18 * t19 * t31 * 2.0 + -t60) + t76;
        t99 = (-(t15 * t20 * t31 * 2.0) + -t60) + t79;
        t93 = t15 * t19 * t31 * 2.0 + -t78;
        t35 = -(t18 * t20 * t31 * 2.0) + -t77;
        t97 = (t17 * t20 * t31 * 2.0 + t60) + -t79;
        t98 = (-(t16 * t19 * t31 * 2.0) + t60) + -t76;
        t100 = ((-(t15 * t16 * t31 * 2.0) + t71) + -t68) + t90;
        t19 = ((-(t17 * t18 * t31 * 2.0) + t75) + -t72) + t90;
        t102_tmp = t17 * t22 * t32 * t34;
        t76 = t15 * t22 * t32 * t34;
        t22 = (((-(t15 * t17 * t31 * 2.0) + t62) + -t65) + t76 * 2.0)
            + t102_tmp * 2.0;
        t33 = t18 * t21 * t32 * t34;
        t20 = t16 * t21 * t32 * t34;
        t79 = (((-(t16 * t18 * t31 * 2.0) + t62) + -t64) + -(t20 * 2.0))
            + -(t33 * 2.0);
        t77 = (((t15 * t18 * t31 * 2.0 + t60) + t68) + -t75) + -t90;
        t78 = (((t16 * t17 * t31 * 2.0 + -t60) + t72) + -t71) + -t90;
        H[0] = t24 * t31 * 2.0;
        H[1] = t59;
        H[2] = t35;
        H[3] = t97;
        H[4] = t92;
        H[5] = t99;
        H[6] = t59;
        H[7] = t23 * t31 * 2.0;
        H[8] = t96;
        H[9] = t94;
        H[10] = t98;
        H[11] = t93;
        H[12] = t35;
        H[13] = t96;
        t35 = -t62 + t64;
        H[14] = (t35 + t18 * t18 * t31 * 2.0) + t33 * 4.0;
        H[15] = t19;
        H[16] = t79;
        H[17] = t77;
        H[18] = t97;
        H[19] = t94;
        H[20] = t19;
        t33 = -t62 + t65;
        H[21] = (t33 + t17 * t17 * t31 * 2.0) - t102_tmp * 4.0;
        H[22] = t78;
        H[23] = t22;
        H[24] = t92;
        H[25] = t98;
        H[26] = t79;
        H[27] = t78;
        H[28] = (t35 + t16 * t16 * t31 * 2.0) + t20 * 4.0;
        H[29] = t100;
        H[30] = t99;
        H[31] = t93;
        H[32] = t77;
        H[33] = t22;
        H[34] = t100;
        H[35] = (t33 + t15 * t15 * t31 * 2.0) - t76 * 4.0;
    }

    // This function was generated by the Symbolic Math Toolbox version 8.3.
    // 10-Jun-2019 18:02:39
    void point_line_distance_hessian_3D(
        double v01,
        double v02,
        double v03,
        double v11,
        double v12,
        double v13,
        double v21,
        double v22,
        double v23,
        double H[81])
    {
        double t17, t18, t19, t20, t21, t22, t23, t24, t25, t26, t27, t28, t35,
            t36, t37, t50, t51, t52, t53, t54, t55, t56, t62, t70, t71, t75,
            t79, t80, t84, t88, t38, t39, t40, t41, t42, t43, t44, t46, t48,
            t57, t58, t60, t63, t65, t67, t102, t103, t104, t162, t163, t164,
            t213, t214, t215, t216, t217, t218, t225, t226, t227, t229, t230,
            t311, t231, t232, t233, t234, t235, t236, t237, t238, t239, t240,
            t245, t279, t281, t282, t283, t287, t289, t247, t248, t249, t250,
            t251, t252, t253, t293, t295, t299, t300, t303, t304, t294, t297,
            t301, t302;

        t17 = -v11 + v01;
        t18 = -v12 + v02;
        t19 = -v13 + v03;
        t20 = -v21 + v01;
        t21 = -v22 + v02;
        t22 = -v23 + v03;
        t23 = -v21 + v11;
        t24 = -v22 + v12;
        t25 = -v23 + v13;
        t26 = v11 * 2.0 + -(v21 * 2.0);
        t27 = v12 * 2.0 + -(v22 * 2.0);
        t28 = v13 * 2.0 + -(v23 * 2.0);
        t35 = t23 * t23;
        t36 = t24 * t24;
        t37 = t25 * t25;
        t50 = t17 * t21;
        t51 = t18 * t20;
        t52 = t17 * t22;
        t53 = t19 * t20;
        t54 = t18 * t22;
        t55 = t19 * t21;
        t56 = t17 * t20 * 2.0;
        t62 = t18 * t21 * 2.0;
        t70 = t19 * t22 * 2.0;
        t71 = t17 * t23 * 2.0;
        t75 = t18 * t24 * 2.0;
        t79 = t19 * t25 * 2.0;
        t80 = t20 * t23 * 2.0;
        t84 = t21 * t24 * 2.0;
        t88 = t22 * t25 * 2.0;
        t38 = t17 * t17 * 2.0;
        t39 = t18 * t18 * 2.0;
        t40 = t19 * t19 * 2.0;
        t41 = t20 * t20 * 2.0;
        t42 = t21 * t21 * 2.0;
        t43 = t22 * t22 * 2.0;
        t44 = t35 * 2.0;
        t46 = t36 * 2.0;
        t48 = t37 * 2.0;
        t57 = t50 * 2.0;
        t58 = t51 * 2.0;
        t60 = t52 * 2.0;
        t63 = t53 * 2.0;
        t65 = t54 * 2.0;
        t67 = t55 * 2.0;
        t102 = 1.0 / ((t35 + t36) + t37);
        t36 = t50 + -t51;
        t35 = t52 + -t53;
        t37 = t54 + -t55;
        t103 = t102 * t102;
        t104 = pow(t102, 3.0);
        t162 = -(t23 * t24 * t102 * 2.0);
        t163 = -(t23 * t25 * t102 * 2.0);
        t164 = -(t24 * t25 * t102 * 2.0);
        t213 = t18 * t36 * 2.0 + t19 * t35 * 2.0;
        t214 = t17 * t35 * 2.0 + t18 * t37 * 2.0;
        t215 = t21 * t36 * 2.0 + t22 * t35 * 2.0;
        t216 = t20 * t35 * 2.0 + t21 * t37 * 2.0;
        t217 = t24 * t36 * 2.0 + t25 * t35 * 2.0;
        t218 = t23 * t35 * 2.0 + t24 * t37 * 2.0;
        t35 = (t36 * t36 + t35 * t35) + t37 * t37;
        t225 = t17 * t36 * 2.0 + -(t19 * t37 * 2.0);
        t226 = t20 * t36 * 2.0 + -(t22 * t37 * 2.0);
        t227 = t23 * t36 * 2.0 + -(t25 * t37 * 2.0);
        t36 = t26 * t103;
        t229 = t36 * t213;
        t37 = t27 * t103;
        t230 = t37 * t213;
        t311 = t28 * t103;
        t231 = t311 * t213;
        t232 = t36 * t214;
        t233 = t37 * t214;
        t234 = t311 * t214;
        t235 = t36 * t215;
        t236 = t37 * t215;
        t237 = t311 * t215;
        t238 = t36 * t216;
        t239 = t37 * t216;
        t240 = t311 * t216;
        t214 = t36 * t217;
        t215 = t37 * t217;
        t216 = t311 * t217;
        t217 = t36 * t218;
        t245 = t37 * t218;
        t213 = t311 * t218;
        t279 = t103 * t35 * 2.0;
        t281 = t26 * t26 * t104 * t35 * 2.0;
        t282 = t27 * t27 * t104 * t35 * 2.0;
        t283 = t28 * t28 * t104 * t35 * 2.0;
        t287 = t26 * t27 * t104 * t35 * 2.0;
        t218 = t26 * t28 * t104 * t35 * 2.0;
        t289 = t27 * t28 * t104 * t35 * 2.0;
        t247 = t36 * t225;
        t248 = t37 * t225;
        t249 = t311 * t225;
        t250 = t36 * t226;
        t251 = t37 * t226;
        t252 = t311 * t226;
        t253 = t36 * t227;
        t35 = t37 * t227;
        t36 = t311 * t227;
        t293 = t102 * (t75 + t79) + t214;
        t295 = -(t102 * (t80 + t84)) + t213;
        t299 = t102 * ((t63 + t22 * t23 * 2.0) + -t60) + t217;
        t300 = t102 * ((t67 + t22 * t24 * 2.0) + -t65) + t245;
        t303 = -(t102 * ((t57 + t17 * t24 * 2.0) + -t58)) + t215;
        t304 = -(t102 * ((t60 + t17 * t25 * 2.0) + -t63)) + t216;
        t294 = t102 * (t71 + t75) + -t213;
        t297 = -(t102 * (t80 + t88)) + t35;
        t88 = -(t102 * (t84 + t88)) + -t214;
        t301 = t102 * ((t58 + t21 * t23 * 2.0) + -t57) + t253;
        t302 = t102 * ((t65 + t21 * t25 * 2.0) + -t67) + t36;
        t84 = t102 * ((t57 + t20 * t24 * 2.0) + -t58) + -t215;
        t80 = t102 * ((t60 + t20 * t25 * 2.0) + -t63) + -t216;
        t75 = -(t102 * ((t63 + t19 * t23 * 2.0) + -t60)) + -t217;
        t227 = -(t102 * ((t67 + t19 * t24 * 2.0) + -t65)) + -t245;
        t311 = ((-(t17 * t19 * t102 * 2.0) + t231) + -t232) + t218;
        t245 = ((-(t20 * t22 * t102 * 2.0) + t237) + -t238) + t218;
        t226 = ((-t102 * (t67 - t54 * 4.0) + t233) + t252) + -t289;
        t28 = ((-t102 * (t63 - t52 * 4.0) + t232) + -t237) + -t218;
        t27 = ((-t102 * (t58 - t50 * 4.0) + t247) + -t236) + -t287;
        t225 = ((-(t102 * (t65 + -(t55 * 4.0))) + t239) + t249) + -t289;
        t26 = ((-(t102 * (t60 + -(t53 * 4.0))) + t238) + -t231) + -t218;
        t103 = ((-(t102 * (t57 + -(t51 * 4.0))) + t250) + -t230) + -t287;
        t104 = (((-(t102 * (t56 + t62)) + t234) + t240) + t279) + -t283;
        t218 = (((-(t102 * (t56 + t70)) + t248) + t251) + t279) + -t282;
        t217 = (((-(t102 * (t62 + t70)) + -t229) + -t235) + t279) + -t281;
        t216 = t102 * (t71 + t79) + -t35;
        t215 = -(t102 * ((t58 + t18 * t23 * 2.0) + -t57)) + -t253;
        t214 = -(t102 * ((t65 + t18 * t25 * 2.0) + -t67)) + -t36;
        t213 = ((-(t17 * t18 * t102 * 2.0) + t230) + -t247) + t287;
        t37 = ((-(t20 * t21 * t102 * 2.0) + t236) + -t250) + t287;
        t36 = ((-(t18 * t19 * t102 * 2.0) + -t233) + -t249) + t289;
        t35 = ((-(t21 * t22 * t102 * 2.0) + -t239) + -t252) + t289;
        H[0] = t102 * (t46 + t48);
        H[1] = t162;
        H[2] = t163;
        H[3] = t88;
        H[4] = t84;
        H[5] = t80;
        H[6] = t293;
        H[7] = t303;
        H[8] = t304;
        H[9] = t162;
        H[10] = t102 * (t44 + t48);
        H[11] = t164;
        H[12] = t301;
        H[13] = t297;
        H[14] = t302;
        H[15] = t215;
        H[16] = t216;
        H[17] = t214;
        H[18] = t163;
        H[19] = t164;
        H[20] = t102 * (t44 + t46);
        H[21] = t299;
        H[22] = t300;
        H[23] = t295;
        H[24] = t75;
        H[25] = t227;
        H[26] = t294;
        H[27] = t88;
        H[28] = t301;
        H[29] = t299;
        H[30] = ((t235 * 2.0 + -t279) + t281) + t102 * (t42 + t43);
        H[31] = t37;
        H[32] = t245;
        H[33] = t217;
        H[34] = t27;
        H[35] = t28;
        H[36] = t84;
        H[37] = t297;
        H[38] = t300;
        H[39] = t37;
        H[40] = ((t251 * -2.0 + -t279) + t282) + t102 * (t41 + t43);
        H[41] = t35;
        H[42] = t103;
        H[43] = t218;
        H[44] = t226;
        H[45] = t80;
        H[46] = t302;
        H[47] = t295;
        H[48] = t245;
        H[49] = t35;
        H[50] = ((t240 * -2.0 + -t279) + t283) + t102 * (t41 + t42);
        H[51] = t26;
        H[52] = t225;
        H[53] = t104;
        H[54] = t293;
        H[55] = t215;
        H[56] = t75;
        H[57] = t217;
        H[58] = t103;
        H[59] = t26;
        H[60] = ((t229 * 2.0 + -t279) + t281) + t102 * (t39 + t40);
        H[61] = t213;
        H[62] = t311;
        H[63] = t303;
        H[64] = t216;
        H[65] = t227;
        H[66] = t27;
        H[67] = t218;
        H[68] = t225;
        H[69] = t213;
        H[70] = ((t248 * -2.0 + -t279) + t282) + t102 * (t38 + t40);
        H[71] = t36;
        H[72] = t304;
        H[73] = t214;
        H[74] = t294;
        H[75] = t28;
        H[76] = t226;
        H[77] = t104;
        H[78] = t311;
        H[79] = t36;
        H[80] = ((t234 * -2.0 + -t279) + t283) + t102 * (t38 + t39);
    }

} // namespace autogen
} // namespace ipc


<|start_filename|>src/friction/closest_point.hpp<|end_filename|>
#pragma once

#include <Eigen/Cholesky>
#include <Eigen/Core>

#include <ipc/utils/eigen_ext.hpp>

namespace ipc {

///////////////////////////////////////////////////////////////////////////////
// Point - Edge

template <
    typename DerivedP,
    typename DerivedE0,
    typename DerivedE1,
    typename T = typename DerivedP::Scalar>
inline T point_edge_closest_point(
    const Eigen::MatrixBase<DerivedP>& p,
    const Eigen::MatrixBase<DerivedE0>& e0,
    const Eigen::MatrixBase<DerivedE1>& e1)
{
    auto e = e1 - e0;
    return (p - e0).dot(e) / e.squaredNorm();
}

///////////////////////////////////////////////////////////////////////////////
// Edge - Edge

/// Compute the barycentric coordinates of the closest points
template <
    typename DerivedEA0,
    typename DerivedEA1,
    typename DerivedEB0,
    typename DerivedEB1,
    typename T = typename DerivedEA0::Scalar>
inline Vector2<T> edge_edge_closest_point(
    const Eigen::MatrixBase<DerivedEA0>& ea0,
    const Eigen::MatrixBase<DerivedEA1>& ea1,
    const Eigen::MatrixBase<DerivedEB0>& eb0,
    const Eigen::MatrixBase<DerivedEB1>& eb1)
{
    assert(ea0.size() == 3);
    assert(ea1.size() == 3);
    assert(eb0.size() == 3);
    assert(eb1.size() == 3);

    auto eb_to_ea = ea0 - eb0;
    auto ea = ea1 - ea0;
    auto eb = eb1 - eb0;

    Eigen::Matrix<T, 2, 2> coefMtr;
    coefMtr(0, 0) = ea.squaredNorm();
    coefMtr(0, 1) = coefMtr(1, 0) = -eb.dot(ea);
    coefMtr(1, 1) = eb.squaredNorm();

    Vector2<T> rhs;
    rhs[0] = -eb_to_ea.dot(ea);
    rhs[1] = eb_to_ea.dot(eb);

    return coefMtr.ldlt().solve(rhs);
}

///////////////////////////////////////////////////////////////////////////////
// Point - Triangle

/// Compute the barycentric coordinates of the closest point on the triangle.
template <
    typename DerivedP,
    typename DerivedT0,
    typename DerivedT1,
    typename DerivedT2,
    typename T = typename DerivedP::Scalar>
inline Vector2<T> point_triangle_closest_point(
    const Eigen::MatrixBase<DerivedP>& p,
    const Eigen::MatrixBase<DerivedT0>& t0,
    const Eigen::MatrixBase<DerivedT1>& t1,
    const Eigen::MatrixBase<DerivedT2>& t2)
{
    assert(p.size() == 3);
    assert(t0.size() == 3);
    assert(t1.size() == 3);
    assert(t2.size() == 3);

    Eigen::Matrix<T, 2, 3> basis;
    basis.row(0) = RowVector3<T>(t1 - t0); // edge 0
    basis.row(1) = RowVector3<T>(t2 - t0); // edge 1
    Matrix2<T> A = basis * basis.transpose();
    Vector2<T> b = basis * Vector3<T>(p - t0);
    Vector2<T> x = A.ldlt().solve(b);
    assert((A * x - b).norm() < 1e-10);
    return x;
}

} // namespace ipc


<|start_filename|>src/barrier/barrier.cpp<|end_filename|>
// Barrier functions that grow to infinity as x -> 0+. Includes gradient and
// hessian functions, too. These barrier functions can be used to impose
// inequlity constraints on a function.
#include <ipc/barrier/barrier.hpp>

namespace ipc {

double barrier_gradient(double d, double dhat)
{
    if (d <= 0.0 || d >= dhat) {
        return 0.0;
    }
    // b(d) = -(d - d̂)²ln(d / d̂)
    // b'(d) = -2(d - d̂)ln(d / d̂) - (d-d̂)²(1 / d)
    //       = (d - d̂) * (-2ln(d/d̂) - (d - d̂) / d)
    //       = (d̂ - d) * (2ln(d/d̂) - d̂/d + 1)
    return (dhat - d) * (2 * log(d / dhat) - dhat / d + 1);
}

double barrier_hessian(double d, double dhat)
{
    if (d <= 0.0 || d >= dhat) {
        return 0.0;
    }

    double dhat_d = dhat / d;

    return (dhat_d + 2) * dhat_d - 2 * log(d / dhat) - 3;
}

} // namespace ipc


<|start_filename|>python/src/distance/point_edge.cpp<|end_filename|>
#include <pybind11/pybind11.h>
#include <pybind11/eigen.h>

#include <ipc/distance/distance_type.hpp>
#include <ipc/distance/point_edge.hpp>

#include "../utils.hpp"

namespace py = pybind11;
using namespace ipc;

void define_point_edge_distance_functions(py::module_& m)
{
    m.def(
        "point_edge_distance",
        [](const VectorMax3d& p, const VectorMax3d& e0, const VectorMax3d& e1,
           const PointEdgeDistanceType* dtype) {
            assert_2D_or_3D_vector(p, "p");
            assert_2D_or_3D_vector(e0, "e0");
            assert_2D_or_3D_vector(e1, "e1");
            if (dtype == nullptr) {
                return point_edge_distance(p, e0, e1);
            } else {
                return point_edge_distance(p, e0, e1, *dtype);
            }
        },
        R"ipc_Qu8mg5v7(
        Compute the distance between a point and edge in 2D or 3D.

        Parameters:
            p: point
            e0: first vertex of the edge
            e1: second vertex of the edge
            dtype: (optional) point edge distance type to compute

        Returns:
            The distance between the point and edge

        Note:
            The distance is actually squared distance.
        )ipc_Qu8mg5v7",
        py::arg("p"), py::arg("e0"), py::arg("e1"),
        py::arg("dtype") = py::none());

    m.def(
        "point_edge_distance_gradient",
        [](const VectorMax3d& p, const VectorMax3d& e0, const VectorMax3d& e1,
           const PointEdgeDistanceType* dtype) {
            assert_2D_or_3D_vector(p, "p");
            assert_2D_or_3D_vector(e0, "e0");
            assert_2D_or_3D_vector(e1, "e1");
            VectorMax9d grad;
            if (dtype == nullptr) {
                point_edge_distance_gradient(p, e0, e1, grad);
            } else {
                point_edge_distance_gradient(p, e0, e1, *dtype, grad);
            }
            return grad;
        },
        R"ipc_Qu8mg5v7(
        Compute the gradient of the distance between a point and edge.

        Parameters:
            p: point
            e0: first vertex of the edge
            e1: second vertex of the edge
            dtype: (optional) point edge distance type to compute

        Returns:
            The gradient of the distance wrt p, e0, and e1.

        Note:
            The distance is actually squared distance.
        )ipc_Qu8mg5v7",
        py::arg("p"), py::arg("e0"), py::arg("e1"),
        py::arg("dtype") = py::none());

    m.def(
        "point_edge_distance_hessian",
        [](const VectorMax3d& p, const VectorMax3d& e0, const VectorMax3d& e1,
           const PointEdgeDistanceType* dtype) {
            assert_2D_or_3D_vector(p, "p");
            assert_2D_or_3D_vector(e0, "e0");
            assert_2D_or_3D_vector(e1, "e1");
            MatrixMax9d hess;
            if (dtype == nullptr) {
                point_edge_distance_hessian(p, e0, e1, hess);
            } else {
                point_edge_distance_hessian(p, e0, e1, *dtype, hess);
            }
            return hess;
        },
        R"ipc_Qu8mg5v7(
        Compute the hessian of the distance between a point and edge.

        Parameters:
            p: point
            e0: first vertex of the edge
            e1: second vertex of the edge
            dtype: (optional) point edge distance type to compute

        Returns:
            The hessian of the distance wrt p, e0, and e1.

        Note:
            The distance is actually squared distance.
        )ipc_Qu8mg5v7",
        py::arg("p"), py::arg("e0"), py::arg("e1"),
        py::arg("dtype") = py::none());
}


<|start_filename|>tests/distance/test_line_line.cpp<|end_filename|>
#include <catch2/catch.hpp>

#include <finitediff.hpp>
#include <igl/PI.h>

#include <ipc/distance/line_line.hpp>
#include <ipc/utils/eigen_ext.hpp>

using namespace ipc;

TEST_CASE("Line-line distance", "[distance][line-line]")
{
    double ya = GENERATE(take(10, random(-100.0, 100.0)));
    Eigen::Vector3d ea0(-1, ya, 0), ea1(1, ya, 0);

    double yb = GENERATE(take(10, random(-100.0, 100.0)));
    Eigen::Vector3d eb0(0, yb, -1), eb1(0, yb, 1);

    double distance = line_line_distance(ea0, ea1, eb0, eb1);
    double expected_distance = abs(ya - yb);
    CHECK(distance == Approx(expected_distance * expected_distance));
}

TEST_CASE("Line-line distance gradient", "[distance][line-line][gradient]")
{
    double ya = GENERATE(take(10, random(-10.0, 10.0)));
    Eigen::Vector3d ea0(-1, ya, 0), ea1(1, ya, 0);

    double yb = GENERATE(take(10, random(-10.0, 10.0)));
    Eigen::Vector3d eb0(0, yb, -1), eb1(0, yb, 1);

    Eigen::VectorXd grad;
    line_line_distance_gradient(ea0, ea1, eb0, eb1, grad);

    Eigen::VectorXd x(12);
    x << ea0, ea1, eb0, eb1;
    Eigen::VectorXd expected_grad;
    expected_grad.resize(grad.size());
    fd::finite_gradient(
        x,
        [](const Eigen::VectorXd& x) {
            return line_line_distance(
                x.head<3>(), x.segment<3>(3), x.segment<3>(6), x.tail<3>());
        },
        expected_grad);

    CAPTURE(ya, yb, (grad - expected_grad).norm());
    bool is_grad_correct = fd::compare_gradient(grad, expected_grad);
    CHECK(is_grad_correct);
}

TEST_CASE("Line-line distance hessian", "[distance][line-line][hessian]")
{
    double ya = GENERATE(take(10, random(-10.0, 10.0)));
    Eigen::Vector3d ea0(-1, ya, 0), ea1(1, ya, 0);

    double yb = GENERATE(take(10, random(-10.0, 10.0)));
    Eigen::Vector3d eb0(0, yb, -1), eb1(0, yb, 1);

    Eigen::MatrixXd hess;
    line_line_distance_hessian(ea0, ea1, eb0, eb1, hess);

    Eigen::VectorXd x(12);
    x << ea0, ea1, eb0, eb1;
    Eigen::MatrixXd expected_hess;
    expected_hess.resize(hess.rows(), hess.cols());
    fd::finite_hessian(
        x,
        [](const Eigen::VectorXd& x) {
            return line_line_distance(
                x.head<3>(), x.segment<3>(3), x.segment<3>(6), x.tail<3>());
        },
        expected_hess);

    CAPTURE(ya, yb, (hess - expected_hess).norm());
    CHECK(fd::compare_hessian(hess, expected_hess, 1e-2));
}


<|start_filename|>src/ccd/aabb.hpp<|end_filename|>
#pragma once

#include <Eigen/Core>
#include <ipc/utils/eigen_ext.hpp>

namespace ipc {

// Discrete collision detection

bool point_edge_aabb_cd(
    const VectorMax3d& p,
    const VectorMax3d& e0,
    const VectorMax3d& e1,
    double dist);

bool edge_edge_aabb_cd(
    const VectorMax3d& ea0,
    const VectorMax3d& ea1,
    const VectorMax3d& eb0,
    const VectorMax3d& eb1,
    double dist);

bool point_triangle_aabb_cd(
    const Eigen::Vector3d& p,
    const Eigen::Vector3d& t0,
    const Eigen::Vector3d& t1,
    const Eigen::Vector3d& t2,
    double dist);

bool edge_triangle_aabb_cd(
    const Eigen::Vector3d& e0,
    const Eigen::Vector3d& e1,
    const Eigen::Vector3d& t0,
    const Eigen::Vector3d& t1,
    const Eigen::Vector3d& t2,
    double dist);

// Continous collision detection

bool point_edge_aabb_ccd(
    const VectorMax3d& p_t0,
    const VectorMax3d& e0_t0,
    const VectorMax3d& e1_t0,
    const VectorMax3d& p_t1,
    const VectorMax3d& e0_t1,
    const VectorMax3d& e1_t1,
    double dist);

bool edge_edge_aabb_ccd(
    const Eigen::Vector3d& ea0_t0,
    const Eigen::Vector3d& ea1_t0,
    const Eigen::Vector3d& eb0_t0,
    const Eigen::Vector3d& eb1_t0,
    const Eigen::Vector3d& ea0_t1,
    const Eigen::Vector3d& ea1_t1,
    const Eigen::Vector3d& eb0_t1,
    const Eigen::Vector3d& eb1_t1,
    double dist);

bool point_triangle_aabb_ccd(
    const Eigen::Vector3d& p_t0,
    const Eigen::Vector3d& t0_t0,
    const Eigen::Vector3d& t1_t0,
    const Eigen::Vector3d& t2_t0,
    const Eigen::Vector3d& p_t1,
    const Eigen::Vector3d& t0_t1,
    const Eigen::Vector3d& t1_t1,
    const Eigen::Vector3d& t2_t1,
    double dist);

} // namespace ipc


<|start_filename|>tests/test_utils.hpp<|end_filename|>
#pragma once

#include <catch2/catch.hpp>

#include <string>

#include <Eigen/Core>

#include <ipc/collision_constraint.hpp>

// Flatten the matrix rowwise
Eigen::VectorXd flatten(const Eigen::MatrixXd& X);

/// Unflatten rowwise
Eigen::MatrixXd unflatten(const Eigen::VectorXd& x, int dim);

bool load_mesh(
    const std::string& mesh_name,
    Eigen::MatrixXd& V,
    Eigen::MatrixXi& E,
    Eigen::MatrixXi& F);

void mmcvids_to_constraints(
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    const Eigen::MatrixXi& mmcvids,
    ipc::Constraints& constraints);

class RotationGenerator
    : public Catch::Generators::IGenerator<Eigen::Matrix3d> {
public:
    // Attempts to move the generator to the next element.
    // Returns true if successful (and thus has another element that can be
    // read)
    virtual bool next();

    // Precondition:
    // The generator is either freshly constructed or the last call to next()
    // returned true
    virtual Eigen::Matrix3d const& get() const;

    static Catch::Generators::GeneratorWrapper<Eigen::Matrix3d> create();

protected:
    Eigen::Matrix3d R = Eigen::Matrix3d::Identity();
};


<|start_filename|>tests/friction/test_relative_displacement.cpp<|end_filename|>
#include <catch2/catch.hpp>

#include <ipc/friction/closest_point.hpp>
#include <ipc/friction/relative_displacement.hpp>

using namespace ipc;

TEST_CASE(
    "Point-triangle relative displacement",
    "[friction][point-triangle][relative_displacement]")
{
    Eigen::Vector3d p(0, 1, 0), t0(-1, 0, 1), t1(1, 0, 1), t2(0, 0, -1);
    Eigen::Vector3d dp = Eigen::Vector3d::Random(), dt0, dt1, dt2;
    dt0 = dt1 = dt2 = Eigen::Vector3d::Zero();

    Eigen::Vector2d barycentric_coords =
        point_triangle_closest_point(p, t0, t1, t2);

    Eigen::Vector3d relative_displacement =
        point_triangle_relative_displacement(
            dp, dt0, dt1, dt2, barycentric_coords);

    CHECK((dp - relative_displacement).norm() == Approx(0).margin(1e-12));
}

TEST_CASE(
    "Edge-edge relative displacement",
    "[friction][edge-edge][relative_displacement]")
{
    Eigen::Vector3d ea0(-1, 0, 0), ea1(1, 0, 0), eb0(0, 0, -1), eb1(0, 0, 1);
    Eigen::Vector3d dea0, dea1, deb0(0, 0, 0), deb1(0, 0, 0);
    dea0 = dea1 = Eigen::Vector3d::Random();

    Eigen::Vector2d barycentric_coords =
        edge_edge_closest_point(ea0, ea1, eb0, eb1);

    Eigen::Vector3d relative_displacement = edge_edge_relative_displacement(
        dea0, dea1, deb0, deb1, barycentric_coords);

    CHECK((dea0 - relative_displacement).norm() == Approx(0).margin(1e-12));
}

TEST_CASE(
    "Point-edge relative displacement",
    "[friction][point-edge][relative_displacement]")
{
    Eigen::Vector3d p(0, 1, 0), e0(-1, 0, 0), e1(1, 0, 0);
    Eigen::Vector3d dp = Eigen::Vector3d::Random(), de0(0, 0, 0), de1(0, 0, 0);

    double alpha = point_edge_closest_point(p, e0, e1);

    Eigen::Vector3d relative_displacement =
        point_edge_relative_displacement(dp, de0, de1, alpha);
    CHECK((dp - relative_displacement).norm() == Approx(0).margin(1e-12));
}

TEST_CASE(
    "Point-point relative displacement",
    "[friction][point-point][relative_displacement]")
{
    Eigen::Vector3d dp0 = Eigen::Vector3d::Random(), dp1(0, 0, 0);

    Eigen::Vector3d relative_displacement =
        point_point_relative_displacement(dp0, dp1);
    CHECK((dp0 - relative_displacement).norm() == Approx(0).margin(1e-12));
}

TEST_CASE(
    "Point-edge relative displacement in 2D",
    "[friction][point-edge][relative_displacement][2D]")
{
    Eigen::Vector2d p(0, 1), e0(-1, 0), e1(1, 0);
    Eigen::Vector2d dp = Eigen::Vector2d::Random(), de0(0, 0), de1(0, 0);

    double alpha = point_edge_closest_point(p, e0, e1);

    Eigen::Vector2d relative_displacement =
        point_edge_relative_displacement(dp, de0, de1, alpha);
    CHECK((dp - relative_displacement).norm() == Approx(0).margin(1e-12));
}

TEST_CASE(
    "Point-point relative displacement in 2D",
    "[friction][point-point][relative_displacement][2D]")
{
    Eigen::Vector2d dp0 = Eigen::Vector2d::Random(), dp1(0, 0);

    Eigen::Vector2d relative_displacement =
        point_point_relative_displacement(dp0, dp1);
    CHECK((dp0 - relative_displacement).norm() == Approx(0).margin(1e-12));
}


<|start_filename|>src/distance/point_line.hpp<|end_filename|>
#pragma once

#include <Eigen/Core>

#include <ipc/distance/point_point.hpp>

namespace ipc {

/// @brief Compute the distance between a point and line in 2D or 3D.
/// @note The distance is actually squared distance.
/// @param p The point.
/// @param e0 The first vertex of the edge defining the line.
/// @param e1 The second vertex of the edge defining the line.
/// @return The distance between the point and line.
template <typename DerivedP, typename DerivedE0, typename DerivedE1>
auto point_line_distance(
    const Eigen::MatrixBase<DerivedP>& p,
    const Eigen::MatrixBase<DerivedE0>& e0,
    const Eigen::MatrixBase<DerivedE1>& e1)
{
    assert(p.size() == 2 || p.size() == 3);
    assert(e0.size() == 2 || e0.size() == 3);
    assert(e1.size() == 2 || e1.size() == 3);

    if (p.size() == 2) {
        auto e = e1 - e0;
        auto numerator =
            (e[1] * p[0] - e[0] * p[1] + e1[0] * e0[1] - e1[1] * e0[0]);
        return numerator * numerator / e.squaredNorm();
    } else {
        return cross(e0 - p, e1 - p).squaredNorm() / (e1 - e0).squaredNorm();
    }
}

// Symbolically generated derivatives;
namespace autogen {
    void point_line_distance_gradient_2D(
        double v01,
        double v02,
        double v11,
        double v12,
        double v21,
        double v22,
        double g[6]);

    void point_line_distance_gradient_3D(
        double v01,
        double v02,
        double v03,
        double v11,
        double v12,
        double v13,
        double v21,
        double v22,
        double v23,
        double g[9]);

    void point_line_distance_hessian_2D(
        double v01,
        double v02,
        double v11,
        double v12,
        double v21,
        double v22,
        double H[36]);

    void point_line_distance_hessian_3D(
        double v01,
        double v02,
        double v03,
        double v11,
        double v12,
        double v13,
        double v21,
        double v22,
        double v23,
        double H[81]);
} // namespace autogen

/// @brief Compute the gradient of the distance between a point and line.
/// @note The distance is actually squared distance.
/// @param[in] p The point.
/// @param[in] e0 The first vertex of the edge defining the line.
/// @param[in] e1 The second vertex of the edge defining the line.
/// @param[out] grad The gradient of the distance wrt p, e0, and e1.
template <
    typename DerivedP,
    typename DerivedE0,
    typename DerivedE1,
    typename DerivedGrad>
void point_line_distance_gradient(
    const Eigen::MatrixBase<DerivedP>& p,
    const Eigen::MatrixBase<DerivedE0>& e0,
    const Eigen::MatrixBase<DerivedE1>& e1,
    Eigen::PlainObjectBase<DerivedGrad>& grad)
{
    assert(p.size() == 2 || p.size() == 3);
    assert(e0.size() == 2 || e0.size() == 3);
    assert(e1.size() == 2 || e1.size() == 3);

    grad.resize(p.size() + e0.size() + e1.size());
    if (p.size() == 2) {
        autogen::point_line_distance_gradient_2D(
            p[0], p[1], e0[0], e0[1], e1[0], e1[1], grad.data());
    } else {
        autogen::point_line_distance_gradient_3D(
            p[0], p[1], p[2], e0[0], e0[1], e0[2], e1[0], e1[1], e1[2],
            grad.data());
    }
}

/// @brief Compute the hessian of the distance between a point and line.
/// @note The distance is actually squared distance.
/// @param[in] p The point.
/// @param[in] e0 The first vertex of the edge defining the line.
/// @param[in] e1 The second vertex of the edge defining the line.
/// @param[out] hess The hessian of the distance wrt p, e0, and e1.
template <
    typename DerivedP,
    typename DerivedE0,
    typename DerivedE1,
    typename DerivedHess>
void point_line_distance_hessian(
    const Eigen::MatrixBase<DerivedP>& p,
    const Eigen::MatrixBase<DerivedE0>& e0,
    const Eigen::MatrixBase<DerivedE1>& e1,
    Eigen::PlainObjectBase<DerivedHess>& hess)
{
    assert(p.size() == 2 || p.size() == 3);
    assert(e0.size() == 2 || e0.size() == 3);
    assert(e1.size() == 2 || e1.size() == 3);

    hess.resize(
        p.size() + e0.size() + e1.size(), p.size() + e0.size() + e1.size());
    if (p.size() == 2) {
        autogen::point_line_distance_hessian_2D(
            p[0], p[1], e0[0], e0[1], e1[0], e1[1], hess.data());
    } else {
        autogen::point_line_distance_hessian_3D(
            p[0], p[1], p[2], e0[0], e0[1], e0[2], e1[0], e1[1], e1[2],
            hess.data());
    }
}

} // namespace ipc


<|start_filename|>python/src/bindings.cpp<|end_filename|>
#include <pybind11/pybind11.h>

namespace py = pybind11;

void define_barrier_functions(py::module_& m);

void define_broad_phase_functions(py::module_& m);

void define_collision_candidate_classes(py::module_& m);
void define_collision_constraint_classes(py::module_& m);

void define_distance_type_functions(py::module_& m);
void define_edge_edge_mollifier_functions(py::module_& m);
void define_edge_edge_distance_functions(py::module_& m);
void define_line_line_distance_functions(py::module_& m);
void define_point_edge_distance_functions(py::module_& m);
void define_point_line_distance_functions(py::module_& m);
void define_point_point_distance_functions(py::module_& m);
void define_point_plane_distance_functions(py::module_& m);
void define_point_triangle_distance_functions(py::module_& m);

void define_ipc_functions(py::module_& m);

void define_logger_functions(py::module_& m);
void define_thread_limiter_functions(py::module_& m);

PYBIND11_MODULE(ipctk, m)
{
    m.doc() = "IPC Toolkit";

    define_barrier_functions(m);

    define_broad_phase_functions(m);

    define_collision_candidate_classes(m);
    define_collision_constraint_classes(m);

    define_distance_type_functions(m);
    define_edge_edge_mollifier_functions(m);
    define_edge_edge_distance_functions(m);
    define_line_line_distance_functions(m);
    define_point_edge_distance_functions(m);
    define_point_line_distance_functions(m);
    define_point_point_distance_functions(m);
    define_point_plane_distance_functions(m);
    define_point_triangle_distance_functions(m);

    define_ipc_functions(m);

    define_logger_functions(m);
    define_thread_limiter_functions(m);
}


<|start_filename|>src/utils/eigen_ext.tpp<|end_filename|>
#include <ipc/utils/eigen_ext.hpp>

#include <stdexcept> // std::runtime_error

#include <Eigen/Eigenvalues>

#include <ipc/utils/logger.hpp>

namespace ipc {

// Matrix Projection onto Positive Definite Cone
template <
    typename _Scalar,
    int _Rows,
    int _Cols,
    int _Options,
    int _MaxRows,
    int _MaxCols>
Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>
project_to_pd(
    const Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>& A,
    double eps)
{
    assert(eps > 0);

    // https://math.stackexchange.com/q/2776803
    Eigen::SelfAdjointEigenSolver<
        Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>>
        eigensolver(A);
    if (eigensolver.info() != Eigen::Success) {
        IPC_LOG(error("unable to project matrix onto positive definite cone"));
        throw std::runtime_error(
            "unable to project matrix onto positive definite cone");
    }
    // Check if all eigen values are positive.
    // The eigenvalues are sorted in increasing order.
    if (eigensolver.eigenvalues()[0] > 0.0) {
        return A;
    }
    Eigen::DiagonalMatrix<double, Eigen::Dynamic> D(eigensolver.eigenvalues());
    // Save a little time and only project the negative or zero values
    for (int i = 0; i < A.rows(); i++) {
        if (D.diagonal()[i] <= 0.0) {
            D.diagonal()[i] = eps;
        } else {
            break;
        }
    }
    return eigensolver.eigenvectors() * D
        * eigensolver.eigenvectors().transpose();
}

// Matrix Projection onto Positive Semi-Definite Cone
template <
    typename _Scalar,
    int _Rows,
    int _Cols,
    int _Options,
    int _MaxRows,
    int _MaxCols>
Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>
project_to_psd(
    const Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>& A)
{
    // https://math.stackexchange.com/q/2776803
    Eigen::SelfAdjointEigenSolver<
        Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>>
        eigensolver(A);
    if (eigensolver.info() != Eigen::Success) {
        IPC_LOG(
            error("unable to project matrix onto positive semi-definite cone"));
        throw std::runtime_error(
            "unable to project matrix onto positive definite cone");
    }
    // Check if all eigen values are zero or positive.
    // The eigenvalues are sorted in increasing order.
    if (eigensolver.eigenvalues()[0] >= 0.0) {
        return A;
    }
    Eigen::DiagonalMatrix<double, Eigen::Dynamic> D(eigensolver.eigenvalues());
    // Save a little time and only project the negative values
    for (int i = 0; i < A.rows(); i++) {
        if (D.diagonal()[i] < 0.0) {
            D.diagonal()[i] = 0.0;
        } else {
            break;
        }
    }
    return eigensolver.eigenvectors() * D
        * eigensolver.eigenvectors().transpose();
}

template <typename DerivedA, typename DerivedB, typename Result>
Result cross(
    const Eigen::MatrixBase<DerivedA>& a, const Eigen::MatrixBase<DerivedB>& b)
{
    assert(a.size() == 3 && b.size() == 3);
    Result c(a.rows(), a.cols());
    c(0) = a(1) * b(2) - a(2) * b(1);
    c(1) = a(2) * b(0) - a(0) * b(2);
    c(2) = a(0) * b(1) - a(1) * b(0);
    return c;
}

} // namespace ipc


<|start_filename|>src/broad_phase/brute_force.hpp<|end_filename|>
#pragma once

#include <Eigen/Core>

#include <ipc/broad_phase/collision_candidate.hpp>

namespace ipc {

/// Find all static collisions in one time step using brute-force.
void detect_collision_candidates_brute_force(
    const Eigen::MatrixXd& V,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    Candidates& candidates,
    bool detect_edge_vertex = false,
    bool detect_edge_edge = true,
    bool detect_face_vertex = true,
    bool perform_aabb_check = false,
    double aabb_inflation_radius = 0,
    const std::function<bool(size_t, size_t)>& can_collide =
        [](size_t, size_t) { return true; });

void detect_edge_vertex_collision_candidates_brute_force(
    const Eigen::MatrixXd& V,
    const Eigen::MatrixXi& E,
    std::vector<EdgeVertexCandidate>& ev_candidates,
    bool perform_aabb_check = false,
    double aabb_inflation_radius = 0,
    const std::function<bool(size_t, size_t)>& can_collide =
        [](size_t, size_t) { return true; });

void detect_edge_edge_collision_candidates_brute_force(
    const Eigen::MatrixXd& V,
    const Eigen::MatrixXi& E,
    std::vector<EdgeEdgeCandidate>& ee_candidates,
    bool perform_aabb_check = false,
    double aabb_inflation_radius = 0,
    const std::function<bool(size_t, size_t)>& can_collide =
        [](size_t, size_t) { return true; });

void detect_face_vertex_collision_candidates_brute_force(
    const Eigen::MatrixXd& V,
    const Eigen::MatrixXi& F,
    std::vector<FaceVertexCandidate>& fv_candidates,
    bool perform_aabb_check = false,
    double aabb_inflation_radius = 0,
    const std::function<bool(size_t, size_t)>& can_collide =
        [](size_t, size_t) { return true; });

void detect_edge_face_collision_candidates_brute_force(
    const Eigen::MatrixXd& V,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    std::vector<EdgeFaceCandidate>& ef_candidates,
    bool perform_aabb_check = false,
    double aabb_inflation_radius = 0,
    const std::function<bool(size_t, size_t)>& can_collide =
        [](size_t, size_t) { return true; });

/// Find all continous collisions in one time step using brute-force.
void detect_collision_candidates_brute_force(
    const Eigen::MatrixXd& V0,
    const Eigen::MatrixXd& V1,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    Candidates& candidates,
    bool detect_edge_vertex = false,
    bool detect_edge_edge = true,
    bool detect_face_vertex = true,
    bool perform_aabb_check = false,
    double aabb_inflation_radius = 0,
    const std::function<bool(size_t, size_t)>& can_collide =
        [](size_t, size_t) { return true; });

void detect_edge_vertex_collision_candidates_brute_force(
    const Eigen::MatrixXd& V0,
    const Eigen::MatrixXd& V1,
    const Eigen::MatrixXi& E,
    std::vector<EdgeVertexCandidate>& ev_candidates,
    bool perform_aabb_check = false,
    double aabb_inflation_radius = 0,
    const std::function<bool(size_t, size_t)>& can_collide =
        [](size_t, size_t) { return true; });

void detect_edge_edge_collision_candidates_brute_force(
    const Eigen::MatrixXd& V0,
    const Eigen::MatrixXd& V1,
    const Eigen::MatrixXi& E,
    std::vector<EdgeEdgeCandidate>& ee_candidates,
    bool perform_aabb_check = false,
    double aabb_inflation_radius = 0,
    const std::function<bool(size_t, size_t)>& can_collide =
        [](size_t, size_t) { return true; });

void detect_face_vertex_collision_candidates_brute_force(
    const Eigen::MatrixXd& V0,
    const Eigen::MatrixXd& V1,
    const Eigen::MatrixXi& F,
    std::vector<FaceVertexCandidate>& fv_candidates,
    bool perform_aabb_check = false,
    double aabb_inflation_radius = 0,
    const std::function<bool(size_t, size_t)>& can_collide =
        [](size_t, size_t) { return true; });

} // namespace ipc


<|start_filename|>src/distance/point_plane.cpp<|end_filename|>
#include <ipc/distance/point_plane.hpp>

namespace ipc {
namespace autogen {

    // This function was generated by the Symbolic Math Toolbox version 8.3.
    // 10-Jun-2019 17:42:16
    void point_plane_distance_gradient(
        double v01,
        double v02,
        double v03,
        double v11,
        double v12,
        double v13,
        double v21,
        double v22,
        double v23,
        double v31,
        double v32,
        double v33,
        double g[12])
    {
        double t11, t12, t13, t14, t15, t16, t17, t18, t19, t20, t21, t22, t32,
            t33, t34, t43, t45, t44, t46;

        t11 = -v11 + v01;
        t12 = -v12 + v02;
        t13 = -v13 + v03;
        t14 = -v21 + v11;
        t15 = -v22 + v12;
        t16 = -v23 + v13;
        t17 = -v31 + v11;
        t18 = -v32 + v12;
        t19 = -v33 + v13;
        t20 = -v31 + v21;
        t21 = -v32 + v22;
        t22 = -v33 + v23;
        t32 = t14 * t18 + -(t15 * t17);
        t33 = t14 * t19 + -(t16 * t17);
        t34 = t15 * t19 + -(t16 * t18);
        t43 = 1.0 / ((t32 * t32 + t33 * t33) + t34 * t34);
        t45 = (t13 * t32 + t11 * t34) + -(t12 * t33);
        t44 = t43 * t43;
        t46 = t45 * t45;
        g[0] = t34 * t43 * t45 * 2.0;
        g[1] = t33 * t43 * t45 * -2.0;
        g[2] = t32 * t43 * t45 * 2.0;
        t45 *= t43;
        g[3] = -t44 * t46 * (t21 * t32 * 2.0 + t22 * t33 * 2.0)
            - t45 * ((t34 + t12 * t22) - t13 * t21) * 2.0;
        t43 = t44 * t46;
        g[4] = t43 * (t20 * t32 * 2.0 - t22 * t34 * 2.0)
            + t45 * ((t33 + t11 * t22) - t13 * t20) * 2.0;
        g[5] = t43 * (t20 * t33 * 2.0 + t21 * t34 * 2.0)
            - t45 * ((t32 + t11 * t21) - t12 * t20) * 2.0;
        g[6] = t45 * (t12 * t19 - t13 * t18) * 2.0
            + t43 * (t18 * t32 * 2.0 + t19 * t33 * 2.0);
        g[7] = t45 * (t11 * t19 - t13 * t17) * -2.0
            - t43 * (t17 * t32 * 2.0 - t19 * t34 * 2.0);
        g[8] = t45 * (t11 * t18 - t12 * t17) * 2.0
            - t43 * (t17 * t33 * 2.0 + t18 * t34 * 2.0);
        g[9] = t45 * (t12 * t16 - t13 * t15) * -2.0
            - t43 * (t15 * t32 * 2.0 + t16 * t33 * 2.0);
        g[10] = t45 * (t11 * t16 - t13 * t14) * 2.0
            + t43 * (t14 * t32 * 2.0 - t16 * t34 * 2.0);
        g[11] = t45 * (t11 * t15 - t12 * t14) * -2.0
            + t43 * (t14 * t33 * 2.0 + t15 * t34 * 2.0);
    }

    // This function was generated by the Symbolic Math Toolbox version 8.3.
    // 10-Jun-2019 17:42:25
    void point_plane_distance_hessian(
        double v01,
        double v02,
        double v03,
        double v11,
        double v12,
        double v13,
        double v21,
        double v22,
        double v23,
        double v31,
        double v32,
        double v33,
        double H[144])
    {
        double t11, t12, t13, t18, t20, t22, t23, t24, t25, t26, t27, t28, t65,
            t66, t67, t68, t69, t70, t71, t77, t85, t86, t90, t94, t95, t99,
            t103, t38, t39, t40, t41, t42, t43, t44, t45, t46, t72, t73, t75,
            t78, t80, t82, t125, t126, t127, t128, t129, t130, t131, t133, t135,
            t149, t150, t151, t189, t190, t191, t192, t193, t194, t195, t196,
            t197, t198, t199, t200, t202, t205, t203, t204, t206, t241, t309,
            t310, t312, t313, t314, t315, t316, t317, t318, t319, t321, t322,
            t323, t324, t325, t261, t262, t599, t600, t602, t605, t609, t610,
            t611, t613, t615, t616, t621, t622, t623, t625, t645, t646_tmp,
            t646, t601, t603, t604, t606, t607, t608, t612, t614, t617, t618,
            t619, t620, t624, t626, t627, t628, t629, t630, t631, t632, t633,
            t634, t635, t636, t637, t638;

        t11 = -v11 + v01;
        t12 = -v12 + v02;
        t13 = -v13 + v03;
        t18 = -v21 + v11;
        t20 = -v22 + v12;
        t22 = -v23 + v13;
        t23 = -v31 + v11;
        t24 = -v32 + v12;
        t25 = -v33 + v13;
        t26 = -v31 + v21;
        t27 = -v32 + v22;
        t28 = -v33 + v23;
        t65 = t18 * t24;
        t66 = t20 * t23;
        t67 = t18 * t25;
        t68 = t22 * t23;
        t69 = t20 * t25;
        t70 = t22 * t24;
        t71 = t18 * t23 * 2.0;
        t77 = t20 * t24 * 2.0;
        t85 = t22 * t25 * 2.0;
        t86 = t18 * t26 * 2.0;
        t90 = t20 * t27 * 2.0;
        t94 = t22 * t28 * 2.0;
        t95 = t23 * t26 * 2.0;
        t99 = t24 * t27 * 2.0;
        t103 = t25 * t28 * 2.0;
        t38 = t18 * t18 * 2.0;
        t39 = t20 * t20 * 2.0;
        t40 = t22 * t22 * 2.0;
        t41 = t23 * t23 * 2.0;
        t42 = t24 * t24 * 2.0;
        t43 = t25 * t25 * 2.0;
        t44 = t26 * t26 * 2.0;
        t45 = t27 * t27 * 2.0;
        t46 = t28 * t28 * 2.0;
        t72 = t65 * 2.0;
        t73 = t66 * 2.0;
        t75 = t67 * 2.0;
        t78 = t68 * 2.0;
        t80 = t69 * 2.0;
        t82 = t70 * 2.0;
        t125 = t11 * t20 + -(t12 * t18);
        t126 = t11 * t22 + -(t13 * t18);
        t127 = t12 * t22 + -(t13 * t20);
        t128 = t11 * t24 + -(t12 * t23);
        t129 = t11 * t25 + -(t13 * t23);
        t130 = t12 * t25 + -(t13 * t24);
        t131 = t65 + -t66;
        t133 = t67 + -t68;
        t135 = t69 + -t70;
        t149 = t131 * t131;
        t150 = t133 * t133;
        t151 = t135 * t135;
        t189 = (t11 * t27 + -(t12 * t26)) + t131;
        t190 = (t11 * t28 + -(t13 * t26)) + t133;
        t191 = (t12 * t28 + -(t13 * t27)) + t135;
        t192 = t20 * t131 * 2.0 + t22 * t133 * 2.0;
        t193 = t18 * t133 * 2.0 + t20 * t135 * 2.0;
        t194 = t24 * t131 * 2.0 + t25 * t133 * 2.0;
        t195 = t23 * t133 * 2.0 + t24 * t135 * 2.0;
        t196 = t27 * t131 * 2.0 + t28 * t133 * 2.0;
        t197 = t26 * t133 * 2.0 + t27 * t135 * 2.0;
        t198 = t18 * t131 * 2.0 + -(t22 * t135 * 2.0);
        t199 = t23 * t131 * 2.0 + -(t25 * t135 * 2.0);
        t200 = t26 * t131 * 2.0 + -(t28 * t135 * 2.0);
        t202 = 1.0 / ((t149 + t150) + t151);
        t205 = (t13 * t131 + t11 * t135) + -(t12 * t133);
        t203 = t202 * t202;
        t204 = pow(t202, 3.0);
        t206 = t205 * t205;
        t241 = t131 * t135 * t202 * 2.0;
        t309 = t11 * t202 * t205 * 2.0;
        t310 = t12 * t202 * t205 * 2.0;
        t13 = t13 * t202 * t205 * 2.0;
        t312 = (-v21 + v01) * t202 * t205 * 2.0;
        t313 = (-v22 + v02) * t202 * t205 * 2.0;
        t314 = (-v23 + v03) * t202 * t205 * 2.0;
        t315 = (-v31 + v01) * t202 * t205 * 2.0;
        t316 = t18 * t202 * t205 * 2.0;
        t317 = (-v32 + v02) * t202 * t205 * 2.0;
        t318 = t20 * t202 * t205 * 2.0;
        t319 = (-v33 + v03) * t202 * t205 * 2.0;
        t11 = t22 * t202 * t205 * 2.0;
        t321 = t23 * t202 * t205 * 2.0;
        t322 = t24 * t202 * t205 * 2.0;
        t323 = t25 * t202 * t205 * 2.0;
        t324 = t26 * t202 * t205 * 2.0;
        t325 = t27 * t202 * t205 * 2.0;
        t12 = t28 * t202 * t205 * 2.0;
        t261 = -(t131 * t133 * t202 * 2.0);
        t262 = -(t133 * t135 * t202 * 2.0);
        t599 = t130 * t135 * t202 * 2.0 + t135 * t194 * t203 * t205 * 2.0;
        t600 = -(t125 * t131 * t202 * 2.0) + t131 * t193 * t203 * t205 * 2.0;
        t602 = t129 * t133 * t202 * 2.0 + t133 * t199 * t203 * t205 * 2.0;
        t605 = -(t131 * t189 * t202 * 2.0) + t131 * t197 * t203 * t205 * 2.0;
        t609 =
            (t127 * t133 * t202 * 2.0 + -t11) + t133 * t192 * t203 * t205 * 2.0;
        t610 =
            (t126 * t135 * t202 * 2.0 + t11) + t135 * t198 * t203 * t205 * 2.0;
        t611 = (t130 * t131 * t202 * 2.0 + -t322)
            + t131 * t194 * t203 * t205 * 2.0;
        t613 = (t126 * t131 * t202 * 2.0 + -t316)
            + t131 * t198 * t203 * t205 * 2.0;
        t615 = (-(t125 * t135 * t202 * 2.0) + -t318)
            + t135 * t193 * t203 * t205 * 2.0;
        t616 = (-(t128 * t133 * t202 * 2.0) + -t321)
            + t133 * t195 * t203 * t205 * 2.0;
        t621 =
            (t133 * t191 * t202 * 2.0 + -t12) + t133 * t196 * t203 * t205 * 2.0;
        t622 =
            (t135 * t190 * t202 * 2.0 + t12) + t135 * t200 * t203 * t205 * 2.0;
        t623 = (t131 * t190 * t202 * 2.0 + -t324)
            + t131 * t200 * t203 * t205 * 2.0;
        t625 = (-(t135 * t189 * t202 * 2.0) + -t325)
            + t135 * t197 * t203 * t205 * 2.0;
        t645 = ((((t127 * t129 * t202 * 2.0 + -t13)
                  + (t72 + -(t66 * 4.0)) * t203 * t206)
                 + t129 * t192 * t203 * t205 * 2.0)
                + t127 * t199 * t203 * t205 * 2.0)
            + t192 * t199 * t204 * t206 * 2.0;
        t646_tmp = t203 * t206;
        t646 =
            ((((t126 * t130 * t202 * 2.0 + t13) + t646_tmp * (t73 - t65 * 4.0))
              + t126 * t194 * t203 * t205 * 2.0)
             + t130 * t198 * t203 * t205 * 2.0)
            + t194 * t198 * t204 * t206 * 2.0;
        t601 = t128 * t131 * t202 * 2.0 + -(t131 * t195 * t203 * t205 * 2.0);
        t603 = -(t127 * t135 * t202 * 2.0) + -(t135 * t192 * t203 * t205 * 2.0);
        t604 = -(t126 * t133 * t202 * 2.0) + -(t133 * t198 * t203 * t205 * 2.0);
        t606 = -(t135 * t191 * t202 * 2.0) + -(t135 * t196 * t203 * t205 * 2.0);
        t607 = -(t133 * t190 * t202 * 2.0) + -(t133 * t200 * t203 * t205 * 2.0);
        t608 = (t125 * t133 * t202 * 2.0 + t316)
            + -(t133 * t193 * t203 * t205 * 2.0);
        t612 = (t128 * t135 * t202 * 2.0 + t322)
            + -(t135 * t195 * t203 * t205 * 2.0);
        t614 = (-(t127 * t131 * t202 * 2.0) + t318)
            + -(t131 * t192 * t203 * t205 * 2.0);
        t617 = (-(t130 * t133 * t202 * 2.0) + t323)
            + -(t133 * t194 * t203 * t205 * 2.0);
        t618 = (-(t129 * t131 * t202 * 2.0) + t321)
            + -(t131 * t199 * t203 * t205 * 2.0);
        t619 = (-(t129 * t135 * t202 * 2.0) + -t323)
            + -(t135 * t199 * t203 * t205 * 2.0);
        t620 = (t133 * t189 * t202 * 2.0 + t324)
            + -(t133 * t197 * t203 * t205 * 2.0);
        t624 = (-(t131 * t191 * t202 * 2.0) + t325)
            + -(t131 * t196 * t203 * t205 * 2.0);
        t626 = (((t125 * t127 * t202 * 2.0 + t18 * t22 * t203 * t206 * 2.0)
                 + t125 * t192 * t203 * t205 * 2.0)
                + -(t127 * t193 * t203 * t205 * 2.0))
            + -(t192 * t193 * t204 * t206 * 2.0);
        t627 = (((t128 * t130 * t202 * 2.0 + t23 * t25 * t203 * t206 * 2.0)
                 + t128 * t194 * t203 * t205 * 2.0)
                + -(t130 * t195 * t203 * t205 * 2.0))
            + -(t194 * t195 * t204 * t206 * 2.0);
        t628 = (((-(t125 * t126 * t202 * 2.0) + t20 * t22 * t203 * t206 * 2.0)
                 + t126 * t193 * t203 * t205 * 2.0)
                + -(t125 * t198 * t203 * t205 * 2.0))
            + t193 * t198 * t204 * t206 * 2.0;
        t629 = (((-(t128 * t129 * t202 * 2.0) + t24 * t25 * t203 * t206 * 2.0)
                 + t129 * t195 * t203 * t205 * 2.0)
                + -(t128 * t199 * t203 * t205 * 2.0))
            + t195 * t199 * t204 * t206 * 2.0;
        t630 = (((-(t126 * t127 * t202 * 2.0) + t18 * t20 * t203 * t206 * 2.0)
                 + -(t126 * t192 * t203 * t205 * 2.0))
                + -(t127 * t198 * t203 * t205 * 2.0))
            + -(t192 * t198 * t204 * t206 * 2.0);
        t631 = (((-(t129 * t130 * t202 * 2.0) + t23 * t24 * t203 * t206 * 2.0)
                 + -(t129 * t194 * t203 * t205 * 2.0))
                + -(t130 * t199 * t203 * t205 * 2.0))
            + -(t194 * t199 * t204 * t206 * 2.0);
        t632 = (((-(t125 * t128 * t202 * 2.0) + (t71 + t77) * t203 * t206)
                 + t128 * t193 * t203 * t205 * 2.0)
                + t125 * t195 * t203 * t205 * 2.0)
            + -(t193 * t195 * t204 * t206 * 2.0);
        t633 = (((-(t127 * t130 * t202 * 2.0) + (t77 + t85) * t203 * t206)
                 + -(t130 * t192 * t203 * t205 * 2.0))
                + -(t127 * t194 * t203 * t205 * 2.0))
            + -(t192 * t194 * t204 * t206 * 2.0);
        t634 = (((-(t126 * t129 * t202 * 2.0) + (t71 + t85) * t203 * t206)
                 + -(t129 * t198 * t203 * t205 * 2.0))
                + -(t126 * t199 * t203 * t205 * 2.0))
            + -(t198 * t199 * t204 * t206 * 2.0);
        t635 = (((t127 * t191 * t202 * 2.0 + -((t90 + t94) * t203 * t206))
                 + t127 * t196 * t203 * t205 * 2.0)
                + t191 * t192 * t203 * t205 * 2.0)
            + t192 * t196 * t204 * t206 * 2.0;
        t636 = (((-(t128 * t189 * t202 * 2.0) + (t95 + t99) * t203 * t206)
                 + t128 * t197 * t203 * t205 * 2.0)
                + t189 * t195 * t203 * t205 * 2.0)
            + -(t195 * t197 * t204 * t206 * 2.0);
        t637 = (((t125 * t189 * t202 * 2.0 + -((t86 + t90) * t203 * t206))
                 + -(t125 * t197 * t203 * t205 * 2.0))
                + -(t189 * t193 * t203 * t205 * 2.0))
            + t193 * t197 * t204 * t206 * 2.0;
        t638 = (((-(t130 * t191 * t202 * 2.0) + (t99 + t103) * t203 * t206)
                 + -(t130 * t196 * t203 * t205 * 2.0))
                + -(t191 * t194 * t203 * t205 * 2.0))
            + -(t194 * t196 * t204 * t206 * 2.0);
        t86 = (((t126 * t190 * t202 * 2.0 + -((t86 + t94) * t203 * t206))
                + t126 * t200 * t203 * t205 * 2.0)
               + t190 * t198 * t203 * t205 * 2.0)
            + t198 * t200 * t204 * t206 * 2.0;
        t71 = (((-(t129 * t190 * t202 * 2.0) + (t95 + t103) * t203 * t206)
                + -(t129 * t200 * t203 * t205 * 2.0))
               + -(t190 * t199 * t203 * t205 * 2.0))
            + -(t199 * t200 * t204 * t206 * 2.0);
        t85 = (((t189 * t191 * t202 * 2.0 + t26 * t28 * t203 * t206 * 2.0)
                + t189 * t196 * t203 * t205 * 2.0)
               + -(t191 * t197 * t203 * t205 * 2.0))
            + -(t196 * t197 * t204 * t206 * 2.0);
        t90 = (((-(t189 * t190 * t202 * 2.0) + t27 * t28 * t203 * t206 * 2.0)
                + t190 * t197 * t203 * t205 * 2.0)
               + -(t189 * t200 * t203 * t205 * 2.0))
            + t197 * t200 * t204 * t206 * 2.0;
        t99 = (((-(t190 * t191 * t202 * 2.0) + t26 * t27 * t203 * t206 * 2.0)
                + -(t190 * t196 * t203 * t205 * 2.0))
               + -(t191 * t200 * t203 * t205 * 2.0))
            + -(t196 * t200 * t204 * t206 * 2.0);
        t77 = ((((-(t127 * t128 * t202 * 2.0) + t310)
                 + (t75 + -(t68 * 4.0)) * t203 * t206)
                + t127 * t195 * t203 * t205 * 2.0)
               + -(t128 * t192 * t203 * t205 * 2.0))
            + t192 * t195 * t204 * t206 * 2.0;
        t131 = ((((t126 * t128 * t202 * 2.0 + -t309)
                  + (t80 + -(t70 * 4.0)) * t203 * t206)
                 + t128 * t198 * t203 * t205 * 2.0)
                + -(t126 * t195 * t203 * t205 * 2.0))
            + -(t195 * t198 * t204 * t206 * 2.0);
        t133 = ((((-(t125 * t130 * t202 * 2.0) + -t310)
                  + t646_tmp * (t78 - t67 * 4.0))
                 + t130 * t193 * t203 * t205 * 2.0)
                + -(t125 * t194 * t203 * t205 * 2.0))
            + t193 * t194 * t204 * t206 * 2.0;
        t325 =
            ((((t125 * t129 * t202 * 2.0 + t309) + t646_tmp * (t82 - t69 * 4.0))
              + t125 * t199 * t203 * t205 * 2.0)
             + -(t129 * t193 * t203 * t205 * 2.0))
            + -(t193 * t199 * t204 * t206 * 2.0);
        t135 = ((((t125 * t191 * t202 * 2.0 + t313)
                  + ((t75 + t18 * t28 * 2.0) + -t78) * t203 * t206)
                 + t125 * t196 * t203 * t205 * 2.0)
                + -(t191 * t193 * t203 * t205 * 2.0))
            + -(t193 * t196 * t204 * t206 * 2.0);
        t324 = ((((t127 * t189 * t202 * 2.0 + -t313)
                  + ((t78 + t22 * t26 * 2.0) + -t75) * t203 * t206)
                 + -(t127 * t197 * t203 * t205 * 2.0))
                + t189 * t192 * t203 * t205 * 2.0)
            + -(t192 * t197 * t204 * t206 * 2.0);
        t318 = ((((-(t126 * t189 * t202 * 2.0) + t312)
                  + ((t82 + t22 * t27 * 2.0) + -t80) * t203 * t206)
                 + t126 * t197 * t203 * t205 * 2.0)
                + -(t189 * t198 * t203 * t205 * 2.0))
            + t197 * t198 * t204 * t206 * 2.0;
        t321 = ((((-(t130 * t189 * t202 * 2.0) + t317)
                  + -(((t78 + t25 * t26 * 2.0) + -t75) * t203 * t206))
                 + t130 * t197 * t203 * t205 * 2.0)
                + -(t189 * t194 * t203 * t205 * 2.0))
            + t194 * t197 * t204 * t206 * 2.0;
        t323 = ((((t129 * t191 * t202 * 2.0 + t319)
                  + -(((t72 + t23 * t27 * 2.0) + -t73) * t203 * t206))
                 + t129 * t196 * t203 * t205 * 2.0)
                + t191 * t199 * t203 * t205 * 2.0)
            + t196 * t199 * t204 * t206 * 2.0;
        t322 = ((((-(t125 * t190 * t202 * 2.0) + -t312)
                  + ((t80 + t20 * t28 * 2.0) + -t82) * t203 * t206)
                 + -(t125 * t200 * t203 * t205 * 2.0))
                + t190 * t193 * t203 * t205 * 2.0)
            + t193 * t200 * t204 * t206 * 2.0;
        t316 = ((((t130 * t190 * t202 * 2.0 + -t319)
                  + -(((t73 + t24 * t26 * 2.0) + -t72) * t203 * t206))
                 + t130 * t200 * t203 * t205 * 2.0)
                + t190 * t194 * t203 * t205 * 2.0)
            + t194 * t200 * t204 * t206 * 2.0;
        t65 = ((((-(t128 * t191 * t202 * 2.0) + -t317)
                 + -(((t75 + t23 * t28 * 2.0) + -t78) * t203 * t206))
                + -(t128 * t196 * t203 * t205 * 2.0))
               + t191 * t195 * t203 * t205 * 2.0)
            + t195 * t196 * t204 * t206 * 2.0;
        t66 = ((((-(t127 * t190 * t202 * 2.0) + t314)
                 + ((t73 + t20 * t26 * 2.0) + -t72) * t203 * t206)
                + -(t127 * t200 * t203 * t205 * 2.0))
               + -(t190 * t192 * t203 * t205 * 2.0))
            + -(t192 * t200 * t204 * t206 * 2.0);
        t13 = ((((t128 * t190 * t202 * 2.0 + t315)
                 + -(((t80 + t24 * t28 * 2.0) + -t82) * t203 * t206))
                + t128 * t200 * t203 * t205 * 2.0)
               + -(t190 * t195 * t203 * t205 * 2.0))
            + -(t195 * t200 * t204 * t206 * 2.0);
        t12 = ((((-(t126 * t191 * t202 * 2.0) + -t314)
                 + ((t72 + t18 * t27 * 2.0) + -t73) * t203 * t206)
                + -(t126 * t196 * t203 * t205 * 2.0))
               + -(t191 * t198 * t203 * t205 * 2.0))
            + -(t196 * t198 * t204 * t206 * 2.0);
        t11 = ((((t129 * t189 * t202 * 2.0 + -t315)
                 + -(((t82 + t25 * t27 * 2.0) + -t80) * t203 * t206))
                + -(t129 * t197 * t203 * t205 * 2.0))
               + t189 * t199 * t203 * t205 * 2.0)
            + -(t197 * t199 * t204 * t206 * 2.0);
        H[0] = t151 * t202 * 2.0;
        H[1] = t262;
        H[2] = t241;
        H[3] = t606;
        H[4] = t622;
        H[5] = t625;
        H[6] = t599;
        H[7] = t619;
        H[8] = t612;
        H[9] = t603;
        H[10] = t610;
        H[11] = t615;
        H[12] = t262;
        H[13] = t150 * t202 * 2.0;
        H[14] = t261;
        H[15] = t621;
        H[16] = t607;
        H[17] = t620;
        H[18] = t617;
        H[19] = t602;
        H[20] = t616;
        H[21] = t609;
        H[22] = t604;
        H[23] = t608;
        H[24] = t241;
        H[25] = t261;
        H[26] = t149 * t202 * 2.0;
        H[27] = t624;
        H[28] = t623;
        H[29] = t605;
        H[30] = t611;
        H[31] = t618;
        H[32] = t601;
        H[33] = t614;
        H[34] = t613;
        H[35] = t600;
        H[36] = t606;
        H[37] = t621;
        H[38] = t624;
        H[39] = ((t191 * t191 * t202 * 2.0 + t196 * t196 * t204 * t206 * 2.0)
                 - t646_tmp * (t45 + t46))
            + t191 * t196 * t203 * t205 * 4.0;
        H[40] = t99;
        H[41] = t85;
        H[42] = t638;
        H[43] = t323;
        H[44] = t65;
        H[45] = t635;
        H[46] = t12;
        H[47] = t135;
        H[48] = t622;
        H[49] = t607;
        H[50] = t623;
        H[51] = t99;
        H[52] = ((t190 * t190 * t202 * 2.0 + t200 * t200 * t204 * t206 * 2.0)
                 - t646_tmp * (t44 + t46))
            + t190 * t200 * t203 * t205 * 4.0;
        H[53] = t90;
        H[54] = t316;
        H[55] = t71;
        H[56] = t13;
        H[57] = t66;
        H[58] = t86;
        H[59] = t322;
        H[60] = t625;
        H[61] = t620;
        H[62] = t605;
        H[63] = t85;
        H[64] = t90;
        H[65] = ((t189 * t189 * t202 * 2.0 + t197 * t197 * t204 * t206 * 2.0)
                 - t646_tmp * (t44 + t45))
            - t189 * t197 * t203 * t205 * 4.0;
        H[66] = t321;
        H[67] = t11;
        H[68] = t636;
        H[69] = t324;
        H[70] = t318;
        H[71] = t637;
        H[72] = t599;
        H[73] = t617;
        H[74] = t611;
        H[75] = t638;
        H[76] = t316;
        H[77] = t321;
        H[78] = ((t130 * t130 * t202 * 2.0 + t194 * t194 * t204 * t206 * 2.0)
                 - t646_tmp * (t42 + t43))
            + t130 * t194 * t203 * t205 * 4.0;
        H[79] = t631;
        H[80] = t627;
        H[81] = t633;
        H[82] = t646;
        H[83] = t133;
        H[84] = t619;
        H[85] = t602;
        H[86] = t618;
        H[87] = t323;
        H[88] = t71;
        H[89] = t11;
        H[90] = t631;
        H[91] = ((t129 * t129 * t202 * 2.0 + t199 * t199 * t204 * t206 * 2.0)
                 - t646_tmp * (t41 + t43))
            + t129 * t199 * t203 * t205 * 4.0;
        H[92] = t629;
        H[93] = t645;
        H[94] = t634;
        H[95] = t325;
        H[96] = t612;
        H[97] = t616;
        H[98] = t601;
        H[99] = t65;
        H[100] = t13;
        H[101] = t636;
        H[102] = t627;
        H[103] = t629;
        H[104] = ((t128 * t128 * t202 * 2.0 + t195 * t195 * t204 * t206 * 2.0)
                  - t646_tmp * (t41 + t42))
            - t128 * t195 * t203 * t205 * 4.0;
        H[105] = t77;
        H[106] = t131;
        H[107] = t632;
        H[108] = t603;
        H[109] = t609;
        H[110] = t614;
        H[111] = t635;
        H[112] = t66;
        H[113] = t324;
        H[114] = t633;
        H[115] = t645;
        H[116] = t77;
        H[117] = ((t127 * t127 * t202 * 2.0 + t192 * t192 * t204 * t206 * 2.0)
                  - t646_tmp * (t39 + t40))
            + t127 * t192 * t203 * t205 * 4.0;
        H[118] = t630;
        H[119] = t626;
        H[120] = t610;
        H[121] = t604;
        H[122] = t613;
        H[123] = t12;
        H[124] = t86;
        H[125] = t318;
        H[126] = t646;
        H[127] = t634;
        H[128] = t131;
        H[129] = t630;
        H[130] = ((t126 * t126 * t202 * 2.0 + t198 * t198 * t204 * t206 * 2.0)
                  - t646_tmp * (t38 + t40))
            + t126 * t198 * t203 * t205 * 4.0;
        H[131] = t628;
        H[132] = t615;
        H[133] = t608;
        H[134] = t600;
        H[135] = t135;
        H[136] = t322;
        H[137] = t637;
        H[138] = t133;
        H[139] = t325;
        H[140] = t632;
        H[141] = t626;
        H[142] = t628;
        H[143] = ((t125 * t125 * t202 * 2.0 + t193 * t193 * t204 * t206 * 2.0)
                  - t646_tmp * (t38 + t39))
            - t125 * t193 * t203 * t205 * 4.0;
    }

} // namespace autogen
} // namespace ipc


<|start_filename|>python/src/utils/logger.cpp<|end_filename|>
#include <pybind11/pybind11.h>

#include <ipc/utils/logger.hpp>

namespace py = pybind11;
using namespace ipc;

void define_logger_functions(py::module_& m)
{
#ifdef IPC_TOOLKIT_WITH_LOGGER
    py::enum_<spdlog::level::level_enum>(m, "LoggerLevel")
        .value("trace", spdlog::level::level_enum::trace)
        .value("debug", spdlog::level::level_enum::debug)
        .value("info", spdlog::level::level_enum::info)
        .value("warn", spdlog::level::level_enum::warn)
        .value("error", spdlog::level::level_enum::err)
        .value("critical", spdlog::level::level_enum::critical)
        .value("off", spdlog::level::level_enum::off)
        .export_values();

    m.def(
        "set_logger_level",
        [](const spdlog::level::level_enum& level) {
            logger().set_level(level);
        },
        "Set log level", py::arg("level"));
#endif
}


<|start_filename|>src/distance/distance_type.tpp<|end_filename|>
#pragma once

#include <iostream>

#include <Eigen/Geometry>

#include <ipc/utils/eigen_ext.hpp>
#include <ipc/utils/logger.hpp>

namespace ipc {

template <typename DerivedP, typename DerivedE0, typename DerivedE1>
PointEdgeDistanceType point_edge_distance_type(
    const Eigen::MatrixBase<DerivedP>& p,
    const Eigen::MatrixBase<DerivedE0>& e0,
    const Eigen::MatrixBase<DerivedE1>& e1)
{
    assert(p.size() == 2 || p.size() == 3);
    assert(e0.size() == 2 || e0.size() == 3);
    assert(e1.size() == 2 || e1.size() == 3);

    const auto e = e1 - e0;
    const auto e_length_sqr = e.squaredNorm();
    if (e_length_sqr == 0) {
        IPC_LOG(warn("Degenerate edge in point_edge_distance_type!"));
        return PointEdgeDistanceType::P_E0; // WARNING: use arbitrary end-point
    }
    auto ratio = e.dot(p - e0) / e_length_sqr;
    if (ratio < 0) {
        return PointEdgeDistanceType::P_E0; // PP (p-e0)
    } else if (ratio > 1) {
        return PointEdgeDistanceType::P_E1; // PP (p-e1)
    } else {
        return PointEdgeDistanceType::P_E; // PE
    }
}

template <
    typename DerivedP,
    typename DerivedT0,
    typename DerivedT1,
    typename DerivedT2>
PointTriangleDistanceType point_triangle_distance_type(
    const Eigen::MatrixBase<DerivedP>& p,
    const Eigen::MatrixBase<DerivedT0>& t0,
    const Eigen::MatrixBase<DerivedT1>& t1,
    const Eigen::MatrixBase<DerivedT2>& t2)
{
    typedef typename DerivedP::Scalar T;

    assert(p.size() == 3);
    assert(t0.size() == 3);
    assert(t1.size() == 3);
    assert(t2.size() == 3);

    Eigen::Matrix<T, 2, 3> basis;
    basis.row(0) = t1 - t0;
    basis.row(1) = t2 - t0;

    auto normal = cross(basis.row(0), basis.row(1));

    Eigen::Matrix<T, 2, 3> param;

    basis.row(1) = cross(basis.row(0), normal);
    param.col(0) =
        (basis * basis.transpose()).ldlt().solve(basis * Vector3<T>(p - t0));
    if (param(0, 0) > 0.0 && param(0, 0) < 1.0 && param(1, 0) >= 0.0) {
        return PointTriangleDistanceType::P_E0; // edge 0 is the closest
    } else {
        basis.row(0) = t2 - t1;
        basis.row(1) = cross(basis.row(0), normal);
        param.col(1) = (basis * basis.transpose())
                           .ldlt()
                           .solve(basis * Vector3<T>(p - t1));
        if (param(0, 1) > 0.0 && param(0, 1) < 1.0 && param(1, 1) >= 0.0) {
            return PointTriangleDistanceType::P_E1; // edge 1 is the closest
        } else {
            basis.row(0) = t0 - t2;

            basis.row(1) = cross(basis.row(0), normal);
            param.col(2) = (basis * basis.transpose())
                               .ldlt()
                               .solve(basis * Vector3<T>(p - t2));
            if (param(0, 2) > 0.0 && param(0, 2) < 1.0 && param(1, 2) >= 0.0) {
                return PointTriangleDistanceType::P_E2; // edge 2 is the closest
            } else {
                if (param(0, 0) <= 0.0 && param(0, 2) >= 1.0) {
                    // vertex 0 is the closest
                    return PointTriangleDistanceType::P_T0;
                } else if (param(0, 1) <= 0.0 && param(0, 0) >= 1.0) {
                    // vertex 1 is the closest
                    return PointTriangleDistanceType::P_T1;
                } else if (param(0, 2) <= 0.0 && param(0, 1) >= 1.0) {
                    // vertex 2 is the closest
                    return PointTriangleDistanceType::P_T2;
                } else {
                    return PointTriangleDistanceType::P_T;
                }
            }
        }
    }
}

// A more robust implementation of http://geomalgorithms.com/a07-_distance.html
template <
    typename DerivedEA0,
    typename DerivedEA1,
    typename DerivedEB0,
    typename DerivedEB1>
EdgeEdgeDistanceType edge_edge_distance_type(
    const Eigen::MatrixBase<DerivedEA0>& ea0,
    const Eigen::MatrixBase<DerivedEA1>& ea1,
    const Eigen::MatrixBase<DerivedEB0>& eb0,
    const Eigen::MatrixBase<DerivedEB1>& eb1)
{
    assert(ea0.size() == 3);
    assert(ea1.size() == 3);
    assert(eb0.size() == 3);
    assert(eb1.size() == 3);

    auto u = ea1 - ea0;
    auto v = eb1 - eb0;
    auto w = ea0 - eb0;

    auto a = u.squaredNorm(); // always >= 0
    auto b = u.dot(v);
    auto c = v.squaredNorm(); // always >= 0
    auto d = u.dot(w);
    auto e = v.dot(w);
    auto D = a * c - b * b; // always >= 0
    auto tD = D;            // tc = tN / tD, default tD = D >= 0

    EdgeEdgeDistanceType defaultCase = EdgeEdgeDistanceType::EA_EB;

    // compute the line parameters of the two closest points
    auto sN = (b * e - c * d);
    decltype(sN) tN;
    if (sN <= 0.0) { // sc < 0 => the s=0 edge is visible
        tN = e;
        tD = c;
        defaultCase = EdgeEdgeDistanceType::EA0_EB;
    } else if (sN >= D) { // sc > 1  => the s=1 edge is visible
        tN = e + b;
        tD = c;
        defaultCase = EdgeEdgeDistanceType::EA1_EB;
    } else {
        tN = (a * e - b * d);
        if (tN > 0.0 && tN < tD
            && (cross(u, v).squaredNorm() < 1.0e-20 * a * c)) {
            // avoid nearly parallel EE
            if (sN < D / 2) {
                tN = e;
                tD = c;
                defaultCase = EdgeEdgeDistanceType::EA0_EB;
            } else {
                tN = e + b;
                tD = c;
                defaultCase = EdgeEdgeDistanceType::EA1_EB;
            }
        }
        // else defaultCase stays EdgeEdgeDistanceType::EA_EB
    }

    if (tN <= 0.0) { // tc < 0 => the t=0 edge is visible
        // recompute sc for this edge
        if (-d <= 0.0) {
            return EdgeEdgeDistanceType::EA0_EB0;
        } else if (-d >= a) {
            return EdgeEdgeDistanceType::EA1_EB0;
        } else {
            return EdgeEdgeDistanceType::EA_EB0;
        }
    } else if (tN >= tD) { // tc > 1  => the t=1 edge is visible
        // recompute sc for this edge
        if ((-d + b) <= 0.0) {
            return EdgeEdgeDistanceType::EA0_EB1;
        } else if ((-d + b) >= a) {
            return EdgeEdgeDistanceType::EA1_EB1;
        } else {
            return EdgeEdgeDistanceType::EA_EB1;
        }
    }

    return defaultCase;
}

} // namespace ipc


<|start_filename|>cmake/ipc_toolkit/ipc_toolkit_set_source_group.cmake<|end_filename|>
# Set source group for IDE like Visual Studio or XCode
function(ipc_toolkit_set_source_group)
  foreach(filepath IN ITEMS ${ARGN})
    get_filename_component(folderpath "${filepath}" DIRECTORY)
    get_filename_component(foldername "${folderpath}" NAME)
    source_group(foldername FILES "${filepath}")
  endforeach()
endfunction()


<|start_filename|>python/src/collision_constraint.cpp<|end_filename|>
#include <pybind11/pybind11.h>
#include <pybind11/eigen.h>
#include <pybind11/stl.h>
#include <pybind11/iostream.h>
#include <pybind11/operators.h>

#include <ipc/collision_constraint.hpp>

namespace py = pybind11;
using namespace ipc;

void define_collision_constraint_classes(py::module_& m)
{
    py::class_<CollisionConstraint>(m, "CollisionConstraint")
        .def_readwrite(
            "minimum_distance", &CollisionConstraint::minimum_distance)
        .def(
            "vertex_indices", &CollisionConstraint::vertex_indices,
            R"ipc_Qu8mg5v7(
            Get the indices of the vertices

            Parameters:
                E: edge matrix of mesh
                F: face matrix of mesh

            Returns:
                List of vertex indices
            )ipc_Qu8mg5v7",
            py::arg("E"), py::arg("F"))
        .def(
            "compute_potential", &CollisionConstraint::compute_potential,
            py::arg("V"), py::arg("E"), py::arg("F"), py::arg("dhat"))
        .def(
            "compute_potential_gradient",
            &CollisionConstraint::compute_potential_gradient, py::arg("V"),
            py::arg("E"), py::arg("F"), py::arg("dhat"))
        .def(
            "compute_potential_hessian",
            &CollisionConstraint::compute_potential_hessian, py::arg("V"),
            py::arg("E"), py::arg("F"), py::arg("dhat"),
            py::arg("project_to_psd"));

    py::class_<
        VertexVertexConstraint, VertexVertexCandidate, CollisionConstraint>(
        m, "VertexVertexConstraint")
        .def(py::init<long, long>())
        .def_readwrite("multiplicity", &VertexVertexConstraint::multiplicity);

    py::class_<EdgeVertexConstraint, EdgeVertexCandidate, CollisionConstraint>(
        m, "EdgeVertexConstraint")
        .def(py::init<long, long>())
        .def_readwrite("multiplicity", &EdgeVertexConstraint::multiplicity);

    py::class_<EdgeEdgeConstraint, EdgeEdgeCandidate, CollisionConstraint>(
        m, "EdgeEdgeConstraint")
        .def(py::init<long, long, double>())
        .def_readwrite("eps_x", &EdgeEdgeConstraint::eps_x);

    py::class_<FaceVertexConstraint, FaceVertexCandidate, CollisionConstraint>(
        m, "FaceVertexConstraint")
        .def(py::init<long, long>());

    py::class_<Constraints>(m, "Constraints")
        .def(py::init())
        .def("size", &Constraints::size)
        .def("num_constraints", &Constraints::num_constraints)
        .def("empty", &Constraints::empty)
        .def("clear", &Constraints::clear)
        .def(
            "__getitem__",
            [](Constraints& self, size_t idx) -> CollisionConstraint* {
                return &self[idx];
            })
        .def_readwrite("vv_constraints", &Constraints::vv_constraints)
        .def_readwrite("ev_constraints", &Constraints::ev_constraints)
        .def_readwrite("ee_constraints", &Constraints::ee_constraints)
        .def_readwrite("fv_constraints", &Constraints::fv_constraints);
}


<|start_filename|>src/utils/world_bbox_diagonal_length.hpp<|end_filename|>
#pragma once

#include <Eigen/Core>

namespace ipc {

inline double world_bbox_diagonal_length(const Eigen::MatrixXd& V)
{
    return (V.colwise().maxCoeff() - V.colwise().minCoeff()).norm();
}

} // namespace ipc


<|start_filename|>tests/ccd/collision_generator.hpp<|end_filename|>
#pragma once

#include <memory>

#include <Eigen/Core>
#include <catch2/catch.hpp>

struct TestImpact {
    Eigen::Vector2d p_t0, e0_t0, e1_t0;
    Eigen::Vector2d dp, de0, de1;
    double toi;
};

TestImpact generate_random_impact(const bool rigid);

class TestImpactGenerator : public Catch::Generators::IGenerator<TestImpact> {
protected:
    TestImpact current;
    bool rigid;
    size_t current_i, max_i;

public:
    TestImpactGenerator(size_t value, bool rigid);

    bool next() override;

    TestImpact const& get() const override;
};

Catch::Generators::GeneratorWrapper<TestImpact>
random_impacts(size_t value, bool rigid);


<|start_filename|>src/broad_phase/spatial_hash.hpp<|end_filename|>
// Modified version of SpatialHash.hpp from IPC codebase.
// Originally created by <NAME>.
#pragma once

#include <vector>

#include <Eigen/Core>
#include <ipc/utils/eigen_ext.hpp>

#include <ipc/broad_phase/collision_candidate.hpp>
#include <ipc/utils/unordered_map_and_set.hpp>

namespace ipc {

class SpatialHash {
public: // data
    VectorMax3d leftBottomCorner, rightTopCorner;
    ArrayMax3i voxelCount;
    double one_div_voxelSize;
    int voxelCount0x1;

    int edgeStartInd, triStartInd;

    unordered_map<int, std::vector<int>> voxel;
    std::vector<std::vector<int>> pointAndEdgeOccupancy;

protected:
    int dim;
    double builtInRadius;

public: // constructor
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    SpatialHash() {}

    SpatialHash(
        const Eigen::MatrixXd& V,
        const Eigen::MatrixXi& E,
        const Eigen::MatrixXi& F,
        double inflation_radius = 0,
        double voxelSize = -1)
    {
        build(V, E, F, inflation_radius, voxelSize);
    }

    SpatialHash(
        const Eigen::MatrixXd& V0,
        const Eigen::MatrixXd& V1,
        const Eigen::MatrixXi& E,
        const Eigen::MatrixXi& F,
        double inflation_radius = 0,
        double voxelSize = -1)
    {
        build(V0, V1, E, F, inflation_radius, voxelSize);
    }

public: // API
    void build(
        const Eigen::MatrixXd& V,
        const Eigen::MatrixXi& E,
        const Eigen::MatrixXi& F,
        double inflation_radius = 0,
        double voxelSize = -1);

    void build(
        const Eigen::MatrixXd& V0,
        const Eigen::MatrixXd& V1,
        const Eigen::MatrixXi& E,
        const Eigen::MatrixXi& F,
        double inflation_radius = 0,
        double voxelSize = -1);

    void clear()
    {
        voxel.clear();
        pointAndEdgeOccupancy.clear();
    }

    void queryPointForTriangles(
        const VectorMax3d& p,
        unordered_set<int>& triInds,
        double radius = 0) const;

    void queryPointForTriangles(
        const VectorMax3d& p_t0,
        const VectorMax3d& p_t1,
        unordered_set<int>& triInds,
        double radius = 0) const;

    void queryPointForPrimitives(
        const VectorMax3d& p_t0,
        const VectorMax3d& p_t1,
        unordered_set<int>& vertInds,
        unordered_set<int>& edgeInds,
        unordered_set<int>& triInds,
        double radius = 0) const;

    void queryEdgeForPE(
        const VectorMax3d& e0,
        const VectorMax3d& e1,
        std::vector<int>& vertInds,
        std::vector<int>& edgeInds,
        double radius = 0) const;

    void queryEdgeForEdges(
        const VectorMax3d& ea0,
        const VectorMax3d& ea1,
        std::vector<int>& edgeInds,
        double radius = 0,
        int eai = -1) const;

    void queryEdgeForEdgesWithBBoxCheck(
        const Eigen::MatrixXd& V,
        const Eigen::MatrixXi& E,
        const VectorMax3d& ea0,
        const VectorMax3d& ea1,
        std::vector<int>& edgeInds,
        double radius = 0,
        int eai = -1) const;

    void queryEdgeForEdges(
        const VectorMax3d& ea0_t0,
        const VectorMax3d& ea1_t0,
        const VectorMax3d& ea0_t1,
        const VectorMax3d& ea1_t1,
        std::vector<int>& edgeInds,
        double radius = 0,
        int eai = -1) const;

    void queryTriangleForPoints(
        const VectorMax3d& t0,
        const VectorMax3d& t1,
        const VectorMax3d& t2,
        unordered_set<int>& pointInds,
        double radius = 0) const;

    void queryTriangleForPoints(
        const VectorMax3d& t0_t0,
        const VectorMax3d& t1_t0,
        const VectorMax3d& t2_t0,
        const VectorMax3d& t0_t1,
        const VectorMax3d& t1_t1,
        const VectorMax3d& t2_t1,
        unordered_set<int>& pointInds,
        double radius = 0) const;

    void queryTriangleForEdges(
        const VectorMax3d& t0,
        const VectorMax3d& t1,
        const VectorMax3d& t2,
        unordered_set<int>& edgeInds,
        double radius = 0) const;

    void queryEdgeForTriangles(
        const VectorMax3d& e0,
        const VectorMax3d& e1,
        unordered_set<int>& triInds,
        double radius = 0) const;

    void queryPointForPrimitives(
        int vi,
        unordered_set<int>& vertInds,
        unordered_set<int>& edgeInds,
        unordered_set<int>& triInds) const;

    void queryPointForEdges(int vi, unordered_set<int>& edgeInds) const;

    void queryPointForTriangles(int vi, unordered_set<int>& triInds) const;

    // will only put edges with larger than ei index into edgeInds
    void queryEdgeForEdges(int eai, unordered_set<int>& edgeInds) const;

    void queryEdgeForEdgesWithBBoxCheck(
        const Eigen::MatrixXd& V0,
        const Eigen::MatrixXd& V1,
        const Eigen::MatrixXi& E,
        int eai,
        unordered_set<int>& edgeInds) const;

    void queryMeshForCandidates(
        const Eigen::MatrixXd& V,
        const Eigen::MatrixXi& E,
        const Eigen::MatrixXi& F,
        Candidates& candidates,
        bool queryEV = false,
        bool queryEE = true,
        bool queryFV = true) const;

    void queryMeshForCandidates(
        const Eigen::MatrixXd& V0,
        const Eigen::MatrixXd& V1,
        const Eigen::MatrixXi& E,
        const Eigen::MatrixXi& F,
        Candidates& candidates,
        bool queryEV = false,
        bool queryEE = true,
        bool queryFV = true) const;

public: // helper functions
    int locateVoxelIndex(const VectorMax3d& p) const;

    void locateVoxelAxisIndex(
        const VectorMax3d& p, ArrayMax3i& voxelAxisIndex) const;

    int voxelAxisIndex2VoxelIndex(const ArrayMax3i& voxelAxisIndex) const;

    int voxelAxisIndex2VoxelIndex(int ix, int iy, int iz) const;
};

} // namespace ipc


<|start_filename|>tests/broad_phase/test_hash_grid.cpp<|end_filename|>
#include <catch2/catch.hpp>

#include <Eigen/Core>

#include <ipc/broad_phase/hash_grid.hpp>

using namespace ipc;

// TEST_CASE("AABB initilization", "[hash_grid][AABB]")
// {
//     int dim = GENERATE(2, 3);
//     CAPTURE(dim);
//     AABB aabb;
//     ArrayMax3d actual_center(dim);
//     SECTION("Empty AABB")
//     {
//         aabb = AABB(ArrayMax3d::Zero(dim), ArrayMax3d::Zero(dim));
//         actual_center.setZero();
//     }
//     SECTION("Box centered at zero")
//     {
//         ArrayMax3d min =
//             ArrayMax3d::Random(dim).array() - 1; // in range [-2, 0]
//         ArrayMax3d max = -min;
//         aabb = AABB(min, max);
//         actual_center.setZero();
//     }
//     SECTION("Box not centered at zero")
//     {
//         ArrayMax3d min(dim), max(dim);
//         if (dim == 2) {
//             min << 5.1, 3.14;
//             max << 10.4, 7.89;
//             actual_center << 7.75, 5.515;
//         } else {
//             min << 5.1, 3.14, 7.94;
//             max << 10.4, 7.89, 10.89;
//             actual_center << 7.75, 5.515, 9.415;
//         }
//         aabb = AABB(min, max);
//     }
//     ArrayMax3d center_diff = aabb.getCenter() - actual_center;
//     CHECK(center_diff.matrix().norm() == Approx(0.0).margin(1e-12));
// }

TEST_CASE("AABB overlapping", "[has_grid][AABB]")
{
    AABB a, b;
    bool are_overlapping = false;
    SECTION("a to the right of b")
    {
        a = AABB(Eigen::Array2d(-1, 0), Eigen::Array2d(0, 1));
        SECTION("overlapping")
        {
            b = AABB(Eigen::Array2d(-0.5, 0), Eigen::Array2d(0.5, 1));
            are_overlapping = true;
        }
        SECTION("not overlapping")
        {
            b = AABB(Eigen::Array2d(0.5, 0), Eigen::Array2d(1.5, 1));
            are_overlapping = false;
        }
    }
    SECTION("b to the right of a")
    {
        b = AABB(Eigen::Array2d(-1, 0), Eigen::Array2d(0, 1));
        SECTION("overlapping")
        {
            a = AABB(Eigen::Array2d(-0.5, 0), Eigen::Array2d(0.5, 1));
            are_overlapping = true;
        }
        SECTION("not overlapping")
        {
            a = AABB(Eigen::Array2d(0.5, 0), Eigen::Array2d(1.5, 1));
            are_overlapping = false;
        }
    }
    SECTION("a above b")
    {
        a = AABB(Eigen::Array2d(0, -1), Eigen::Array2d(1, 0));
        SECTION("overlapping")
        {
            b = AABB(Eigen::Array2d(0, -0.5), Eigen::Array2d(1, 0.5));
            are_overlapping = true;
        }
        SECTION("not overlapping")
        {
            b = AABB(Eigen::Array2d(0, 0.5), Eigen::Array2d(1, 1.5));
            are_overlapping = false;
        }
    }
    SECTION("a above b")
    {
        b = AABB(Eigen::Array2d(0, -1), Eigen::Array2d(1, 0));
        SECTION("overlapping")
        {
            a = AABB(Eigen::Array2d(0, -0.5), Eigen::Array2d(1, 0.5));
            are_overlapping = true;
        }
        SECTION("not overlapping")
        {
            a = AABB(Eigen::Array2d(0, 0.5), Eigen::Array2d(1, 1.5));
            are_overlapping = false;
        }
    }
    CHECK(AABB::are_overlapping(a, b) == are_overlapping);
}


<|start_filename|>src/ccd/aabb.cpp<|end_filename|>
#include <ipc/ccd/aabb.hpp>

namespace ipc {

// Discrete collision detection

bool point_edge_aabb_cd(
    const VectorMax3d& p,
    const VectorMax3d& e0,
    const VectorMax3d& e1,
    double dist)
{
    const ArrayMax3d max_e = e0.array().max(e1.array());
    const ArrayMax3d min_e = e0.array().min(e1.array());
    return (p.array() <= max_e + dist).all()
        && (min_e <= p.array() + dist).all();
}

bool edge_edge_aabb_cd(
    const VectorMax3d& ea0,
    const VectorMax3d& ea1,
    const VectorMax3d& eb0,
    const VectorMax3d& eb1,
    double dist)
{
    const ArrayMax3d max_a = ea0.array().max(ea1.array());
    const ArrayMax3d min_a = ea0.array().min(ea1.array());
    const ArrayMax3d max_b = eb0.array().max(eb1.array());
    const ArrayMax3d min_b = eb0.array().min(eb1.array());
    return (min_a <= max_b + dist).all() && (min_b <= max_a + dist).all();
}

bool point_triangle_aabb_cd(
    const Eigen::Vector3d& p,
    const Eigen::Vector3d& t0,
    const Eigen::Vector3d& t1,
    const Eigen::Vector3d& t2,
    double dist)
{
    const Eigen::Array3d max_tri = t0.array().max(t1.array()).max(t2.array());
    const Eigen::Array3d min_tri = t0.array().min(t1.array()).min(t2.array());
    return (p.array() <= max_tri + dist).all()
        && (min_tri <= p.array() + dist).all();
}

bool edge_triangle_aabb_cd(
    const Eigen::Vector3d& e0,
    const Eigen::Vector3d& e1,
    const Eigen::Vector3d& t0,
    const Eigen::Vector3d& t1,
    const Eigen::Vector3d& t2,
    double dist)
{
    const Eigen::Array3d max_e = e0.array().max(e1.array());
    const Eigen::Array3d min_e = e0.array().min(e1.array());
    const Eigen::Array3d max_tri = t0.array().max(t1.array()).max(t2.array());
    const Eigen::Array3d min_tri = t0.array().min(t1.array()).min(t2.array());
    return (min_e <= max_tri + dist).all() && (min_tri <= max_e + dist).all();
}

// Continous collision detection

bool point_edge_aabb_ccd(
    const VectorMax3d& p_t0,
    const VectorMax3d& e0_t0,
    const VectorMax3d& e1_t0,
    const VectorMax3d& p_t1,
    const VectorMax3d& e0_t1,
    const VectorMax3d& e1_t1,
    double dist)
{
    const ArrayMax3d max_p = p_t0.array().max(p_t1.array());
    const ArrayMax3d min_p = p_t0.array().min(p_t1.array());
    const ArrayMax3d max_e =
        e0_t0.array().max(e1_t0.array()).max(e0_t1.array()).max(e1_t1.array());
    const ArrayMax3d min_e =
        e0_t0.array().min(e1_t0.array()).min(e0_t1.array()).min(e1_t1.array());
    return (min_p <= max_e + dist).all() && (min_e <= max_p + dist).all();
}

bool point_triangle_aabb_ccd(
    const Eigen::Vector3d& p_t0,
    const Eigen::Vector3d& t0_t0,
    const Eigen::Vector3d& t1_t0,
    const Eigen::Vector3d& t2_t0,
    const Eigen::Vector3d& p_t1,
    const Eigen::Vector3d& t0_t1,
    const Eigen::Vector3d& t1_t1,
    const Eigen::Vector3d& t2_t1,
    double dist)
{
    const Eigen::Array3d max_p = p_t0.array().max((p_t1).array());
    const Eigen::Array3d min_p = p_t0.array().min((p_t1).array());
    const Eigen::Array3d max_tri = t0_t0.array()
                                       .max(t1_t0.array())
                                       .max(t2_t0.array())
                                       .max(t0_t1.array())
                                       .max(t1_t1.array())
                                       .max(t2_t1.array());
    const Eigen::Array3d min_tri = t0_t0.array()
                                       .min(t1_t0.array())
                                       .min(t2_t0.array())
                                       .min(t0_t1.array())
                                       .min(t1_t1.array())
                                       .min(t2_t1.array());
    return (min_p <= max_tri + dist).all() && (min_tri <= max_p + dist).all();
}

bool edge_edge_aabb_ccd(
    const Eigen::Vector3d& ea0_t0,
    const Eigen::Vector3d& ea1_t0,
    const Eigen::Vector3d& eb0_t0,
    const Eigen::Vector3d& eb1_t0,
    const Eigen::Vector3d& ea0_t1,
    const Eigen::Vector3d& ea1_t1,
    const Eigen::Vector3d& eb0_t1,
    const Eigen::Vector3d& eb1_t1,
    double dist)
{
    const Eigen::Array3d max_a = ea0_t0.array()
                                     .max(ea1_t0.array())
                                     .max(ea0_t1.array())
                                     .max(ea1_t1.array());
    const Eigen::Array3d min_a = ea0_t0.array()
                                     .min(ea1_t0.array())
                                     .min(ea0_t1.array())
                                     .min(ea1_t1.array());
    const Eigen::Array3d max_b = eb0_t0.array()
                                     .max(eb1_t0.array())
                                     .max(eb0_t1.array())
                                     .max(eb1_t1.array());
    const Eigen::Array3d min_b = eb0_t0.array()
                                     .min(eb1_t0.array())
                                     .min(eb0_t1.array())
                                     .min(eb1_t1.array());
    return (min_a <= max_b + dist).all() && (min_b <= max_a + dist).all();
}

} // namespace ipc


<|start_filename|>tests/distance/test_point_point.cpp<|end_filename|>
#include <catch2/catch.hpp>

#include <finitediff.hpp>

#include <ipc/distance/point_point.hpp>
#include <ipc/utils/eigen_ext.hpp>

using namespace ipc;

TEST_CASE("Point-point distance", "[distance][point-point]")
{
    int dim = GENERATE(2, 3);
    VectorMax3d p0 = VectorMax3d::Zero(dim);
    VectorMax3d p1 = VectorMax3d::Zero(dim);
    double expected_distance = GENERATE(-10, -1, -1e-12, 0, 1e-12, 1, 10);
    SECTION("Aligned with X-axis") { p1(0) = expected_distance; }
    SECTION("Diagonal vector")
    {
        p1.setOnes();
        p1.normalize();
        p1 *= expected_distance;
    }
    double distance = point_point_distance(p0, p1);
    CHECK(distance == Approx(expected_distance * expected_distance));
}

TEST_CASE("Point-point distance gradient", "[distance][point-point][gradient]")
{
    int dim = GENERATE(2, 3);
    VectorMax3d p0 = VectorMax3d::Zero(dim);
    VectorMax3d p1 = VectorMax3d::Zero(dim);
    double expected_distance = GENERATE(-10, -1, -1e-12, 0, 1e-12, 1, 10);
    SECTION("Aligned with X-axis") { p1(0) = expected_distance; }
    SECTION("Diagonal vector")
    {
        p1.setOnes();
        p1.normalize();
        p1 *= expected_distance;
    }

    Eigen::VectorXd grad;
    point_point_distance_gradient(p0, p1, grad);

    // Compute the gradient using finite differences
    Eigen::VectorXd x(2 * dim);
    x.head(dim) = p0;
    x.tail(dim) = p1;
    auto f = [&dim](const Eigen::VectorXd& x) {
        return point_point_distance(x.head(dim), x.tail(dim));
    };
    Eigen::VectorXd fgrad;
    fd::finite_gradient(x, f, fgrad);

    CHECK(fd::compare_gradient(grad, fgrad));
}

TEST_CASE("Point-point distance hessian", "[distance][point-point][hessian]")
{
    int dim = GENERATE(2, 3);
    VectorMax3d p0 = VectorMax3d::Zero(dim);
    VectorMax3d p1 = VectorMax3d::Zero(dim);
    double expected_distance = GENERATE(-10, -1, -1e-12, 0, 1e-12, 1, 10);
    SECTION("Aligned with X-axis") { p1(0) = expected_distance; }
    SECTION("Diagonal vector")
    {
        p1.setOnes();
        p1.normalize();
        p1 *= expected_distance;
    }

    Eigen::MatrixXd hess;
    point_point_distance_hessian(p0, p1, hess);

    // Compute the gradient using finite differences
    Eigen::VectorXd x(2 * dim);
    x.head(dim) = p0;
    x.tail(dim) = p1;
    auto f = [&dim](const Eigen::VectorXd& x) {
        Eigen::VectorXd grad;
        point_point_distance_gradient(x.head(dim), x.tail(dim), grad);
        return grad;
    };
    Eigen::MatrixXd fhess;
    fd::finite_jacobian(x, f, fhess);

    CAPTURE((hess - fhess).squaredNorm());
    CHECK(fd::compare_hessian(hess, fhess));
}


<|start_filename|>tests/friction/test_closest_point.cpp<|end_filename|>
#include <catch2/catch.hpp>

#include <ipc/friction/closest_point.hpp>

using namespace ipc;

TEST_CASE(
    "Point-triangle closest point", "[friction][point-triangle][closest_point]")
{
    Eigen::Vector3d t0(-1, 0, 1), t1(1, 0, 1), t2(0, 0, -1);
    Eigen::Vector2d expected_coords(0.5, 0.5);
    Eigen::Vector3d p =
        t0 + expected_coords[0] * (t1 - t0) + expected_coords[1] * (t2 - t0);
    // p = 1 * t0 + u * t1 - u * t0 + v * t2 - v * t0
    //   = (1 - u - v) * t0 + u * t1 + v * t2
    //   =  w * t0 + u * t1 + v * t2

    Eigen::Vector2d barycentric_coords =
        point_triangle_closest_point(p, t0, t1, t2);
    Eigen::Vector3d p_actual = t0 + barycentric_coords[0] * (t1 - t0)
        + barycentric_coords[1] * (t2 - t0);
    CAPTURE(barycentric_coords);
    CHECK((p - p_actual).norm() == Approx(0).margin(1e-12));
}

TEST_CASE("Edge-edge closest point", "[friction][edge-edge][closest_point]")
{
    Eigen::Vector3d ea0(-1, 0, 0), ea1(1, 0, 0), eb0(0, 0, -1), eb1(0, 0, 1);

    Eigen::Vector2d barycentric_coords =
        edge_edge_closest_point(ea0, ea1, eb0, eb1);
    CAPTURE(barycentric_coords);
    CHECK(barycentric_coords[0] == Approx(0.5));
    CHECK(barycentric_coords[1] == Approx(0.5));
}

TEST_CASE("Point-edge closest point", "[friction][point-edge][closest_point]")
{
    Eigen::Vector3d p(0, 1, 0), e0(-1, 0, 0), e1(1, 0, 0);

    double alpha = point_edge_closest_point(p, e0, e1);
    CHECK(alpha == Approx(0.5));
}

TEST_CASE(
    "Point-edge closest point in 2D",
    "[friction][point-edge][closest_point][2D]")
{
    Eigen::Vector2d p(0, 1), e0(-1, 0), e1(1, 0);

    double alpha = point_edge_closest_point(p, e0, e1);
    CHECK(alpha == Approx(0.5));
}


<|start_filename|>src/distance/edge_edge.hpp<|end_filename|>
#pragma once

#include <stdexcept> // std::invalid_argument

#include <ipc/distance/distance_type.hpp>
#include <ipc/distance/line_line.hpp>
#include <ipc/distance/point_edge.hpp>
#include <ipc/distance/point_point.hpp>

namespace ipc {

/// @brief Compute the distance between a two lines segments in 3D.
/// @note The distance is actually squared distance.
/// @param ea0 The first vertex of the first edge.
/// @param ea1 The second vertex of the first edge.
/// @param eb0 The first vertex of the second edge.
/// @param eb1 The second vertex of the second edge.
/// @return The distance between the two edges.
template <
    typename DerivedEA0,
    typename DerivedEA1,
    typename DerivedEB0,
    typename DerivedEB1>
auto edge_edge_distance(
    const Eigen::MatrixBase<DerivedEA0>& ea0,
    const Eigen::MatrixBase<DerivedEA1>& ea1,
    const Eigen::MatrixBase<DerivedEB0>& eb0,
    const Eigen::MatrixBase<DerivedEB1>& eb1)
{
    assert(ea0.size() == 3);
    assert(ea1.size() == 3);
    assert(eb0.size() == 3);
    assert(eb1.size() == 3);

    return edge_edge_distance(
        ea0, ea1, eb0, eb1, edge_edge_distance_type(ea0, ea1, eb0, eb1));
}

/// @brief Compute the distance between a two lines segments in 3D.
/// @note The distance is actually squared distance.
/// @param ea0 The first vertex of the first edge.
/// @param ea1 The second vertex of the first edge.
/// @param eb0 The first vertex of the second edge.
/// @param eb1 The second vertex of the second edge.
/// @param dtype The point edge distance type to compute.
/// @return The distance between the two edges.
template <
    typename DerivedEA0,
    typename DerivedEA1,
    typename DerivedEB0,
    typename DerivedEB1>
auto edge_edge_distance(
    const Eigen::MatrixBase<DerivedEA0>& ea0,
    const Eigen::MatrixBase<DerivedEA1>& ea1,
    const Eigen::MatrixBase<DerivedEB0>& eb0,
    const Eigen::MatrixBase<DerivedEB1>& eb1,
    const EdgeEdgeDistanceType dtype)
{
    assert(ea0.size() == 3);
    assert(ea1.size() == 3);
    assert(eb0.size() == 3);
    assert(eb1.size() == 3);

    switch (dtype) {
    case EdgeEdgeDistanceType::EA0_EB0:
        return point_point_distance(ea0, eb0);

    case EdgeEdgeDistanceType::EA0_EB1:
        return point_point_distance(ea0, eb1);

    case EdgeEdgeDistanceType::EA1_EB0:
        return point_point_distance(ea1, eb0);

    case EdgeEdgeDistanceType::EA1_EB1:
        return point_point_distance(ea1, eb1);

    case EdgeEdgeDistanceType::EA_EB0:
        return point_edge_distance(eb0, ea0, ea1);

    case EdgeEdgeDistanceType::EA_EB1:
        return point_edge_distance(eb1, ea0, ea1);

    case EdgeEdgeDistanceType::EA0_EB:
        return point_edge_distance(ea0, eb0, eb1);

    case EdgeEdgeDistanceType::EA1_EB:
        return point_edge_distance(ea1, eb0, eb1);

    case EdgeEdgeDistanceType::EA_EB:
        return line_line_distance(ea0, ea1, eb0, eb1);
    }

    throw std::invalid_argument(
        "Invalid distance type for edge-edge distance!");
}

/// @brief Compute the gradient of the distance between a two lines segments.
/// @note The distance is actually squared distance.
/// @param[in] ea0 The first vertex of the first edge.
/// @param[in] ea1 The second vertex of the first edge.
/// @param[in] eb0 The first vertex of the second edge.
/// @param[in] eb1 The second vertex of the second edge.
/// @param[out] grad The gradient of the distance wrt ea0, ea1, eb0, and eb1.
template <
    typename DerivedEA0,
    typename DerivedEA1,
    typename DerivedEB0,
    typename DerivedEB1,
    typename DerivedGrad>
void edge_edge_distance_gradient(
    const Eigen::MatrixBase<DerivedEA0>& ea0,
    const Eigen::MatrixBase<DerivedEA1>& ea1,
    const Eigen::MatrixBase<DerivedEB0>& eb0,
    const Eigen::MatrixBase<DerivedEB1>& eb1,
    Eigen::PlainObjectBase<DerivedGrad>& grad)
{
    assert(ea0.size() == 3);
    assert(ea1.size() == 3);
    assert(eb0.size() == 3);
    assert(eb1.size() == 3);

    return edge_edge_distance_gradient(
        ea0, ea1, eb0, eb1, edge_edge_distance_type(ea0, ea1, eb0, eb1), grad);
}

/// @brief Compute the gradient of the distance between a two lines segments.
/// @note The distance is actually squared distance.
/// @param[in] ea0 The first vertex of the first edge.
/// @param[in] ea1 The second vertex of the first edge.
/// @param[in] eb0 The first vertex of the second edge.
/// @param[in] eb1 The second vertex of the second edge.
/// @param[in] dtype The point edge distance type to compute.
/// @param[out] grad The gradient of the distance wrt ea0, ea1, eb0, and eb1.
template <
    typename DerivedEA0,
    typename DerivedEA1,
    typename DerivedEB0,
    typename DerivedEB1,
    typename DerivedGrad>
void edge_edge_distance_gradient(
    const Eigen::MatrixBase<DerivedEA0>& ea0,
    const Eigen::MatrixBase<DerivedEA1>& ea1,
    const Eigen::MatrixBase<DerivedEB0>& eb0,
    const Eigen::MatrixBase<DerivedEB1>& eb1,
    const EdgeEdgeDistanceType dtype,
    Eigen::PlainObjectBase<DerivedGrad>& grad)
{
    int dim = ea0.size();
    assert(ea1.size() == dim);
    assert(eb0.size() == dim);
    assert(eb1.size() == dim);

    grad.resize(4 * dim);
    grad.setZero();

    VectorMax9<typename DerivedGrad::Scalar> local_grad;
    switch (dtype) {
    case EdgeEdgeDistanceType::EA0_EB0:
        point_point_distance_gradient(ea0, eb0, local_grad);
        grad.head(dim) = local_grad.head(dim);
        grad.segment(2 * dim, dim) = local_grad.tail(dim);
        break;

    case EdgeEdgeDistanceType::EA0_EB1:
        point_point_distance_gradient(ea0, eb1, local_grad);
        grad.head(dim) = local_grad.head(dim);
        grad.tail(dim) = local_grad.tail(dim);
        break;

    case EdgeEdgeDistanceType::EA1_EB0:
        point_point_distance_gradient(ea1, eb0, local_grad);
        grad.segment(dim, dim) = local_grad.head(dim);
        grad.segment(2 * dim, dim) = local_grad.tail(dim);
        break;

    case EdgeEdgeDistanceType::EA1_EB1:
        point_point_distance_gradient(ea1, eb1, local_grad);
        grad.segment(dim, dim) = local_grad.head(dim);
        grad.tail(dim) = local_grad.tail(dim);
        break;

    case EdgeEdgeDistanceType::EA_EB0:
        point_edge_distance_gradient(eb0, ea0, ea1, local_grad);
        grad.head(2 * dim) = local_grad.tail(2 * dim);
        grad.segment(2 * dim, dim) = local_grad.head(dim);
        break;

    case EdgeEdgeDistanceType::EA_EB1:
        point_edge_distance_gradient(eb1, ea0, ea1, local_grad);
        grad.head(2 * dim) = local_grad.tail(2 * dim);
        grad.tail(dim) = local_grad.head(dim);
        break;

    case EdgeEdgeDistanceType::EA0_EB:
        point_edge_distance_gradient(ea0, eb0, eb1, local_grad);
        grad.head(dim) = local_grad.head(dim);
        grad.tail(2 * dim) = local_grad.tail(2 * dim);
        break;

    case EdgeEdgeDistanceType::EA1_EB:
        point_edge_distance_gradient(ea1, eb0, eb1, local_grad);
        grad.tail(3 * dim) = local_grad;
        break;

    case EdgeEdgeDistanceType::EA_EB:
        line_line_distance_gradient(ea0, ea1, eb0, eb1, grad);
        break;

    default:
        throw std::invalid_argument(
            "Invalid distance type for edge-edge distance gradient!");
    }
}

/// @brief Compute the hessian of the distance between a two lines segments.
/// @note The distance is actually squared distance.
/// @param[in] ea0 The first vertex of the first edge.
/// @param[in] ea1 The second vertex of the first edge.
/// @param[in] eb0 The first vertex of the second edge.
/// @param[in] eb1 The second vertex of the second edge.
/// @param[out] hess The hessian of the distance wrt ea0, ea1, eb0, and eb1.
template <
    typename DerivedEA0,
    typename DerivedEA1,
    typename DerivedEB0,
    typename DerivedEB1,
    typename DerivedHess>
void edge_edge_distance_hessian(
    const Eigen::MatrixBase<DerivedEA0>& ea0,
    const Eigen::MatrixBase<DerivedEA1>& ea1,
    const Eigen::MatrixBase<DerivedEB0>& eb0,
    const Eigen::MatrixBase<DerivedEB1>& eb1,
    Eigen::PlainObjectBase<DerivedHess>& hess)
{
    assert(ea0.size() == 3);
    assert(ea1.size() == 3);
    assert(eb0.size() == 3);
    assert(eb1.size() == 3);

    return edge_edge_distance_hessian(
        ea0, ea1, eb0, eb1, edge_edge_distance_type(ea0, ea1, eb0, eb1), hess);
}

/// @brief Compute the hessian of the distance between a two lines segments.
/// @note The distance is actually squared distance.
/// @param[in] ea0 The first vertex of the first edge.
/// @param[in] ea1 The second vertex of the first edge.
/// @param[in] eb0 The first vertex of the second edge.
/// @param[in] eb1 The second vertex of the second edge.
/// @param[in] dtype The point edge distance type to compute.
/// @param[out] hess The hessian of the distance wrt ea0, ea1, eb0, and eb1.
template <
    typename DerivedEA0,
    typename DerivedEA1,
    typename DerivedEB0,
    typename DerivedEB1,
    typename DerivedHess>
void edge_edge_distance_hessian(
    const Eigen::MatrixBase<DerivedEA0>& ea0,
    const Eigen::MatrixBase<DerivedEA1>& ea1,
    const Eigen::MatrixBase<DerivedEB0>& eb0,
    const Eigen::MatrixBase<DerivedEB1>& eb1,
    const EdgeEdgeDistanceType dtype,
    Eigen::PlainObjectBase<DerivedHess>& hess)
{
    int dim = ea0.size();
    assert(ea1.size() == dim);
    assert(eb0.size() == dim);
    assert(eb1.size() == dim);

    hess.resize(4 * dim, 4 * dim);
    hess.setZero();

    MatrixMax9<typename DerivedHess::Scalar> local_hess;
    switch (dtype) {
    case EdgeEdgeDistanceType::EA0_EB0:
        point_point_distance_hessian(ea0, eb0, local_hess);
        hess.topLeftCorner(dim, dim) = local_hess.topLeftCorner(dim, dim);
        hess.block(0, 2 * dim, dim, dim) = local_hess.topRightCorner(dim, dim);
        hess.block(2 * dim, 0, dim, dim) =
            local_hess.bottomLeftCorner(dim, dim);
        hess.block(2 * dim, 2 * dim, dim, dim) =
            local_hess.bottomRightCorner(dim, dim);
        break;

    case EdgeEdgeDistanceType::EA0_EB1:
        point_point_distance_hessian(ea0, eb1, local_hess);
        hess.topLeftCorner(dim, dim) = local_hess.topLeftCorner(dim, dim);
        hess.topRightCorner(dim, dim) = local_hess.topRightCorner(dim, dim);
        hess.bottomLeftCorner(dim, dim) = local_hess.bottomLeftCorner(dim, dim);
        hess.bottomRightCorner(dim, dim) =
            local_hess.bottomRightCorner(dim, dim);
        break;

    case EdgeEdgeDistanceType::EA1_EB0:
        point_point_distance_hessian(ea1, eb0, local_hess);
        hess.block(dim, dim, 2 * dim, 2 * dim) = local_hess;
        break;

    case EdgeEdgeDistanceType::EA1_EB1:
        point_point_distance_hessian(ea1, eb1, local_hess);
        hess.block(dim, dim, dim, dim) = local_hess.topLeftCorner(dim, dim);
        hess.block(dim, 3 * dim, dim, dim) =
            local_hess.topRightCorner(dim, dim);
        hess.block(3 * dim, dim, dim, dim) =
            local_hess.bottomLeftCorner(dim, dim);
        hess.bottomRightCorner(dim, dim) =
            local_hess.bottomRightCorner(dim, dim);
        break;

    case EdgeEdgeDistanceType::EA_EB0:
        point_edge_distance_hessian(eb0, ea0, ea1, local_hess);
        hess.topLeftCorner(2 * dim, 2 * dim) =
            local_hess.bottomRightCorner(2 * dim, 2 * dim);
        hess.block(0, 2 * dim, 2 * dim, dim) =
            local_hess.bottomLeftCorner(2 * dim, dim);
        hess.block(2 * dim, 0, dim, 2 * dim) =
            local_hess.topRightCorner(dim, 2 * dim);
        hess.block(2 * dim, 2 * dim, dim, dim) =
            local_hess.topLeftCorner(dim, dim);
        break;

    case EdgeEdgeDistanceType::EA_EB1:
        point_edge_distance_hessian(eb1, ea0, ea1, local_hess);
        hess.topLeftCorner(2 * dim, 2 * dim) =
            local_hess.bottomRightCorner(2 * dim, 2 * dim);
        hess.topRightCorner(2 * dim, dim) =
            local_hess.bottomLeftCorner(2 * dim, dim);
        hess.bottomLeftCorner(dim, 2 * dim) =
            local_hess.topRightCorner(dim, 2 * dim);
        hess.bottomRightCorner(dim, dim) = local_hess.topLeftCorner(dim, dim);
        break;

    case EdgeEdgeDistanceType::EA0_EB:
        point_edge_distance_hessian(ea0, eb0, eb1, local_hess);
        hess.topLeftCorner(dim, dim) = local_hess.topLeftCorner(dim, dim);
        hess.topRightCorner(dim, 2 * dim) =
            local_hess.topRightCorner(dim, 2 * dim);
        hess.bottomLeftCorner(2 * dim, dim) =
            local_hess.bottomLeftCorner(2 * dim, dim);
        hess.bottomRightCorner(2 * dim, 2 * dim) =
            local_hess.bottomRightCorner(2 * dim, 2 * dim);
        break;

    case EdgeEdgeDistanceType::EA1_EB:
        point_edge_distance_hessian(ea1, eb0, eb1, local_hess);
        hess.bottomRightCorner(3 * dim, 3 * dim) = local_hess;
        break;

    case EdgeEdgeDistanceType::EA_EB:
        line_line_distance_hessian(ea0, ea1, eb0, eb1, hess);
        break;

    default:
        throw std::invalid_argument(
            "Invalid distance type for edge-edge distance hessian!");
    }
}

} // namespace ipc


<|start_filename|>tests/ccd/test_ccd.cpp<|end_filename|>
#include <catch2/catch.hpp>

#include <ipc/ipc.hpp>
#include <ipc/ccd/ccd.hpp>
#include <ipc/ccd/point_static_plane.hpp>

#include <test_utils.hpp>

#include "collision_generator.hpp"

using namespace ipc;

static const double EPSILON = std::numeric_limits<float>::epsilon();

TEST_CASE("Point-edge 2D CCD", "[ccd]")
{
    Eigen::Vector2d p_t0, p_t1, e0_t0, e0_t1, e1_t0, e1_t1;
    double toi_expected;
    bool is_collision_expected;
    SECTION("Edge becomes degenerate")
    {
        p_t0 << 0, 1;
        e0_t0 << -1, 0;
        e1_t0 << 1, 0;
        SECTION("Edge degenerates before impact")
        {
            p_t1 << 0, -1;
            e0_t1 << 3, 0;
            e1_t1 << -3, 0;
            // The edge will become degenerate at t=0.25
        }
        SECTION("Edge degenerates after impact")
        {
            p_t1 << 0, -1;
            e0_t1 << 0.5, 0;
            e1_t1 << -0.5, 0;
            // The edge will become degenerate at t=2/3
        }
        // The point will collide with the edge at t=0.5
        is_collision_expected = true;
        toi_expected = 0.5;
    }
    SECTION("Edge moving right; point moving left")
    {
        p_t0 << -1, 0;
        e0_t0 << 1, -1;
        e1_t0 << 1, 1;

        p_t1 << 1, 0;
        e0_t1 << -1, -1;
        e1_t1 << -1, 1;

        is_collision_expected = true;
        toi_expected = 0.5;
    }
    SECTION("Point on edge's line moving towards edge")
    {
        p_t0 << 0, 0;
        e0_t0 << 0, 1;
        e1_t0 << 0, 2;

        p_t1 << 0, 2;
        e0_t1 << 0, 1;
        e1_t1 << 0, 2;

        is_collision_expected = true;
        toi_expected = 0.5;
    }
    SECTION("Point and edge moving parallel")
    {
        p_t0 << 0, 1;
        e0_t0 << 1, 0;
        e1_t0 << 1, 2;

        p_t1 << 0, 2;
        e0_t1 << 1, 1;
        e1_t1 << 1, 3;

        is_collision_expected = false;
    }
    SECTION("Point moving right; edge stretching vertically")
    {
        e1_t0 << 1, -1;
        e1_t1 << 1, -2;
        SECTION("Swap vertices order e_0 = [0, 2]")
        {
            p_t0 << 1, 1;
            e0_t0 << 0, 0;

            p_t1 << 1, 2;
            e0_t1 << 1, 0;

            std::swap(p_t0, e0_t0);
            std::swap(p_t1, e0_t1);
        }
        SECTION("Swap vertices order e_0 = [1, 2]")
        {
            p_t0 << 0, 0;
            e0_t0 << 1, 1;

            p_t1 << 1, 0;
            e0_t1 << 1, 2;
        }
        is_collision_expected = true;
        toi_expected = 1.0;
    }
    SECTION("Point-point")
    {
        p_t0 << 1.11111, 0.5;
        p_t1 << 0.888889, 0.5;
        e0_t0 << 1, 0.5;
        e1_t0 << 1, 0.75;
        e0_t1 = e0_t0;
        e1_t1 = e1_t0;

        is_collision_expected = true;
        toi_expected = 0.5;
    }

    double toi, alpha;
    bool is_colliding = point_edge_ccd(
        p_t0, e0_t0, e1_t0, p_t1, e0_t1, e1_t1, toi,
        /*tmax=*/1.0,
        /*tolerance=*/1e-6,
        /*max_iterations=*/1e7,
        /*conservative_rescaling=*/1.0);
    REQUIRE(is_colliding == is_collision_expected);
    if (is_collision_expected) {
        CAPTURE(
            p_t0.transpose(), e0_t0.transpose(), e1_t0.transpose(),
            p_t1.transpose(), e0_t1.transpose(), e1_t1.transpose());
        CHECK(toi <= toi_expected);
    }
}

// ---------------------------------------------------
// Helper functions
// ---------------------------------------------------

/// Compares the time of impact of different implementations
/// against the expected time of impact
void check_toi(
    const Eigen::Vector2d& p_t0,
    const Eigen::Vector2d& e0_t0,
    const Eigen::Vector2d& e1_t0,
    const Eigen::Vector2d& p_t1,
    const Eigen::Vector2d& e0_t1,
    const Eigen::Vector2d& e1_t1,
    const double toi_expected)
{
    double toi_actual;
    // check autodiff code
    toi_actual = -1.0;
    bool has_collision = point_edge_ccd(
        p_t0, e0_t0, e1_t0, p_t1, e0_t1, e1_t1, toi_actual,
        /*tmax=*/1.0,
        /*tolerance=*/1e-6,
        /*max_iterations=*/1e7,
        /*conservative_rescaling=*/1.0);
    CHECK(has_collision);
    CHECK(toi_actual <= toi_expected);
}

// ---------------------------------------------------
// Tests
// ---------------------------------------------------
TEST_CASE("Point-edge 2D ToI", "[ccd][toi]")
{
    Eigen::Vector2d p_t0, e0_t0, e1_t0;
    Eigen::Vector2d dp, de0, de1;

    double expected_toi = -1;

    SECTION("perpendicular") // (alpha=0.5)
    {
        p_t0 << 0.0, 1.0;
        e0_t0 << -1.0, 0.0;
        e1_t0 << 1.0, 0.0;

        // touches, intersects, passes-trough
        auto vel = GENERATE(1.0 + 1e-6, 2.0, 4.0);
        // moving direction:
        //  -> ->, -> -, -> <-, - <-, <- <-
        auto j = GENERATE(0, 1, 2, 3, 4);
        // extension, no-deform, compression,
        auto dx = GENERATE(0.5, 0.0, -0.5);

        dp << 0.0, -(3 - j) * vel / 2.0;
        de0 << -dx, (j - 1.0) * vel / 2.0;
        de1 << dx, (j - 1.0) * vel / 2.0;

        expected_toi = 1.0 / vel;
    }

    SECTION("double impact") // (rotating edge)
    {
        e0_t0 << -1.0, 0.0;
        e1_t0 << 1.0, 0.0;
        p_t0 << 0.0, 0.5;

        de0 << 1.6730970740318298, 0.8025388419628143;
        de1 << -1.616142749786377, -0.6420311331748962;
        dp << 0.0, -1.0;

        expected_toi = 0.4482900963;
    }

    SECTION("random")
    {
        // This times out on Windows debug
#if defined(NDEBUG) || !(defined(WIN32) || defined(_WIN32) || defined(__WIN32))
        auto impact = GENERATE(random_impacts(100, /*rigid=*/true));
#else
        auto impact = GENERATE(random_impacts(10, /*rigid=*/true));
#endif

        p_t0 = impact.p_t0;
        e0_t0 = impact.e0_t0;
        e1_t0 = impact.e1_t0;

        dp = impact.dp;
        de0 = impact.de0;
        de1 = impact.de1;

        expected_toi = impact.toi;
    }

#ifdef NDEBUG           // This case takes forever to run
    SECTION("parallel") //  (alpha=0 || alpha = 1)
    {
        p_t0 << 0.5, 0.0;
        e0_t0 << -0.5, 0.0;
        e1_t0 << -1.5, 0.0;

        // touches, intersects, passes-trough
        // double vel = GENERATE(1.0 + 1e-6, 2.0, 4.0);
        double vel = 2.0;
        // moving: both (same), ij, both (op), kl, both (same)
        // double j = GENERATE(0, 1, 2, 3, 4);
        double j = 0;
        // extension, no-deform, compression,
        // double dy = GENERATE(0.5, 0.0, -0.5);
        double dy = 0.0;

        dp << -(3 - j) * vel / 2.0, 0.0;
        de0 << (j - 1.0) * vel / 2.0, 0.0;
        // we only move one so we don't change the toi
        de1 << (j - 1.0) * vel / 2.0, dy;

        expected_toi = 1.0 / vel;
    }
#endif

    Eigen::Vector2d p_t1 = p_t0 + dp;
    Eigen::Vector2d e0_t1 = e0_t0 + de0;
    Eigen::Vector2d e1_t1 = e1_t0 + de1;

    check_toi(p_t0, e0_t0, e1_t0, p_t1, e0_t1, e1_t1, expected_toi);
    // Flip the edges
    check_toi(p_t0, e1_t0, e0_t0, p_t1, e1_t1, e0_t1, expected_toi);
}

TEST_CASE("Repeated CCD", "[ccd][repeat]")
{
    const double FIRST_TOL = 1e-6, SECOND_TOL = 1e-7;
    const double FIRST_MAX_ITER = 1e6, SECOND_MAX_ITER = 1e6;

    // BroadPhaseMethod broadphase_method =
    //     GENERATE(BroadPhaseMethod::HASH_GRID, BroadPhaseMethod::BRUTE_FORCE);
    BroadPhaseMethod broadphase_method = BroadPhaseMethod::HASH_GRID;
    Eigen::VectorXi codim_V;
    double inflation_radius = 0;

    bool recompute_candidates = GENERATE(false, true);

    std::string t0_filename, t1_filename;
    SECTION("tooth")
    {
        t0_filename = "ccd-failure/repeated_toi_tooth_0.obj";
        t1_filename = "ccd-failure/repeated_toi_tooth_1.obj";
    }
    SECTION("hip")
    {
        t0_filename = "ccd-failure/repeated_toi_hip_0.obj";
        t1_filename = "ccd-failure/repeated_toi_hip_1.obj";
    }
    SECTION("hip small repeated toi")
    {
        t0_filename = "ccd-failure/small_repeated_toi_hip_0.obj";
        t1_filename = "ccd-failure/small_repeated_toi_hip_1.obj";
    }
    SECTION("hip inf-repeat 0")
    {
        t0_filename = "ccd-failure/inf_repeated_toi_hip_0.obj";
        t1_filename = "ccd-failure/inf_repeated_toi_hip_1.obj";
    }
    SECTION("hip inf-repeat 1")
    {
        t0_filename = "ccd-failure/s0121.obj";
        t1_filename = "ccd-failure/s1121.obj";
    }
    SECTION("hip inf-repeat 2")
    {
        t0_filename = "ccd-failure/s0110.obj";
        t1_filename = "ccd-failure/s1110.obj";
    }

    Eigen::MatrixXd V0, V1;
    Eigen::MatrixXi E, F;
    bool success =
        load_mesh(t0_filename, V0, E, F) && load_mesh(t1_filename, V1, E, F);
    if (!success) {
        return;
    }
    // REQUIRE(success);

    Candidates candidates;
    construct_collision_candidates(
        V0, V1, codim_V, E, F, candidates, inflation_radius, broadphase_method);

    bool has_collisions = !is_step_collision_free(
        candidates, V0, V1, E, F, FIRST_TOL, FIRST_MAX_ITER);

    double stepsize = compute_collision_free_stepsize(
        candidates, V0, V1, E, F, FIRST_TOL, FIRST_MAX_ITER);

    if (!has_collisions) {
        CHECK(stepsize == 1.0);
        return;
    }

    double collision_free_step_size = stepsize;
    bool has_collisions_repeated;
    double stepsize_repeated;
    do {
        Eigen::MatrixXd Vt = (V1 - V0) * collision_free_step_size + V0;
        // CHECK(!has_intersections(Vt, E, F));

        if (recompute_candidates) {
            construct_collision_candidates(
                V0, Vt, codim_V, E, F, candidates, inflation_radius,
                broadphase_method);
        }

        has_collisions_repeated = !is_step_collision_free(
            candidates, V0, Vt, E, F, SECOND_TOL, SECOND_MAX_ITER);

        stepsize_repeated = compute_collision_free_stepsize(
            candidates, V0, Vt, E, F, SECOND_TOL, SECOND_MAX_ITER);

        CAPTURE(
            t0_filename, t1_filename, broadphase_method, recompute_candidates,
            has_collisions, collision_free_step_size, has_collisions_repeated,
            stepsize_repeated);
        CHECK(!has_collisions_repeated);
        CHECK(stepsize_repeated == 1.0);

        collision_free_step_size *= stepsize_repeated;
    } while (has_collisions_repeated && stepsize_repeated != 1.0);
}

// This test takes too long
// TEST_CASE("Point-Triangle 3D CCD", "[ccd][point-triangle][timeout]")
// {
//     // point
//     double v0z = GENERATE(0.0, -1.0);
//     Eigen::Vector3d v0(0, 1, v0z);
//     // triangle = (v1, v2, v3)
//     Eigen::Vector3d v1(-1, 0, 1);
//     Eigen::Vector3d v2(1, 0, 1);
//     Eigen::Vector3d v3(0, 0, -1);
//
//     // displacements
//     double u0y =
//         -GENERATE(-1.0, 0.0, 0.5 - EPSILON, 0.5, 0.5 + EPSILON, 1.0, 2.0);
//     double u0z = GENERATE(-EPSILON, 0.0, EPSILON);
//     Eigen::Vector3d u0(0, u0y, u0z);
//     double u1y =
//         GENERATE(-1.0, 0.0, 0.5 - EPSILON, 0.5, 0.5 + EPSILON, 1.0, 2.0);
//     Eigen::Vector3d u1(0, u1y, 0);
//
//     bool is_collision_expected = ((-u0y + u1y >= 1) && (v0z + u0z >=
//     v3.z()));
//
//     double toi;
//     bool is_colliding = point_triangle_ccd(
//         v0, v1, v2, v3, v0 + u0, v1 + u1, v2 + u1, v3 + u1, toi);
//
//     CAPTURE(v0z, u0y, u1y, u0z, EPSILON);
//     REQUIRE(is_colliding >= is_collision_expected);
// }

// Disable these tests in debug because they take a long time.
#ifdef NDEBUG

TEST_CASE("Edge-Edge 3D CCD", "[ccd][edge-edge]")
{
    // e0 = (v0, v1)
    Eigen::Vector3d v0(-1, -1, 0);
    Eigen::Vector3d v1(1, -1, 0);
    // e2 = (v2, v3)
    // double e1x = GENERATE(
    //     -1 - EPSILON, -1, -1 + EPSILON, -0.5, 0, 0.5, 1 - EPSILON, 1,
    //     1 + EPSILON);
    double e1x = GENERATE(0);
    Eigen::Vector3d v2(e1x, 1, -1);
    Eigen::Vector3d v3(e1x, 1, 1);

    // displacements
    double y_displacement =
        GENERATE(-1.0, 0.0, 1 - EPSILON, 1.0, 1 + EPSILON, 2.0);

    Eigen::Vector3d u0, u1;
    bool is_collision_expected;
    SECTION("moving")
    {
        u0 << 0, y_displacement, 0;
        u1 << 0, -y_displacement, 0;
        is_collision_expected = y_displacement >= 1.0 && e1x >= -1 && e1x <= 1;
    }
    SECTION("fixed")
    {
        u0 << 0, 2 * y_displacement, 0;
        u1.setZero();
        is_collision_expected = y_displacement >= 2.0 && e1x >= -1 && e1x <= 1;
    }

    double toi;
    bool is_colliding =
        edge_edge_ccd(v0, v1, v2, v3, v0 + u0, v1 + u0, v2 + u1, v3 + u1, toi);

    CAPTURE(y_displacement, e1x);
    CHECK(is_colliding >= is_collision_expected);
}

// Only run this slow case on Linux and macOS
#if !defined(WIN32)

TEST_CASE("Zhongshi test case", "[ccd][point-triangle][zhongshi]")
{
    double qy = GENERATE(-EPSILON, 0, EPSILON);

    Eigen::Vector3d q;
    q << 0, qy, 0;

    Eigen::Vector3d b0;
    b0 << 0, 0, 0;
    Eigen::Vector3d b1;
    b1 << 0, 1, 0;
    Eigen::Vector3d b2;
    b2 << 1, 0, 0;

    Eigen::Vector3d t0;
    t0 << 0, 0, 1;
    Eigen::Vector3d t1;
    t1 << 0, 1, 1;
    Eigen::Vector3d t2;
    t2 << 1, 0, 1;

    Eigen::Vector3d q1;
    q1 << 0, qy, 0;

    bool is_collision_expected = q.y() >= 0;

    double toi;
    bool is_colliding = point_triangle_ccd(q, b0, b1, b2, q1, t0, t1, t2, toi);

    CAPTURE(qy);
    CHECK(is_colliding >= is_collision_expected);
}

#endif

TEST_CASE("Bolun test case", "[ccd][point-triangle][bolun]")
{
    Eigen::Vector3d x0(0.1, 0.1, 0.1), x1(0, 0, 1), x2(1, 0, 1), x3(0, 1, 1),
        x0b(0.1, 0.1, 0.1), x1b(0, 0, 0), x2b(0, 1, 0), x3b(1, 0, 0);

    bool is_collision_expected = true;

    double toi;
    bool is_colliding =
        point_triangle_ccd(x0, x1, x2, x3, x0b, x1b, x2b, x3b, toi);

    CHECK(is_colliding >= is_collision_expected);
}

TEST_CASE("Dobule root test case", "[ccd][edge-edge][double-root]")
{
    const Eigen::Vector3d a0s(-3.0022200, 0.2362580, 0.0165247);
    const Eigen::Vector3d a1s(-3.2347850, 0.8312380, -0.1151003);
    const Eigen::Vector3d a0e(-2.8995600, 0.0345838, 0.0638580);
    const Eigen::Vector3d a1e(-3.1716930, 0.6104858, -0.0713340);
    const Eigen::Vector3d b0(-3.0319900, 0.3148750, 0.0000000);
    const Eigen::Vector3d b1(-2.8548800, 0.0900349, 0.0000000);

    bool is_collision_expected = true;

    double toi;
    bool is_colliding = edge_edge_ccd(a0s, a1s, b0, b1, a0e, a1e, b0, b1, toi);

    CHECK(is_colliding >= is_collision_expected);
}

TEST_CASE("Double root test case 2", "[ccd][edge-edge][double-root]")
{
    const Eigen::Vector3d a0s(0, 0, 1);
    const Eigen::Vector3d a1s(0, 1, 1);
    Eigen::Vector3d a0e(1, 1, 0);
    Eigen::Vector3d a1e(0, 0, 0);
    const Eigen::Vector3d b0(0.1, 0.2, 2);
    const Eigen::Vector3d b1(0.1, 0.2, -1);

    double t = GENERATE(0.5, 0.8, 0.88, 0.9, 1.0);
    a0e = (a0e - a0s) * t + a0s;
    a1e = (a1e - a1s) * t + a1s;

    bool is_collision_expected = true;

    double toi;
    bool is_colliding = edge_edge_ccd(a0s, a1s, b0, b1, a0e, a1e, b0, b1, toi);

    CHECK(is_colliding >= is_collision_expected);
}

#endif

TEST_CASE("Point-Plane CCD", "[ccd][point-plane]")
{
    Eigen::Vector3d p_t0 = Eigen::Vector3d::Random();

    Eigen::Vector3d origin = Eigen::Vector3d::Random();
    origin.x() = origin.z() = 0;
    Eigen::Vector3d normal(0, 1, 0);

    Eigen::Vector3d p_t1 = p_t0;
    double t = 0;
    SECTION("Known t")
    {
        t = GENERATE(1e-16, 1e-8, 1e-6, 1e-4, 0.1, 0.5, 0.8, 0.88, 0.9, 1.0);
    }
    SECTION("Random t") { t = GENERATE(take(100, random(0.0, 1.0))); }
    if (t == 0) {
        return;
    }
    p_t1.y() = (origin.y() + (t - 1) * p_t0.y()) / t;

    double toi;
    bool is_colliding =
        point_static_plane_ccd(p_t0, p_t1, origin, normal, toi, 0.99);

    CAPTURE(p_t0.x(), p_t0.y(), p_t0.z(), p_t1.x(), p_t1.y(), p_t1.z());
    CHECK(is_colliding);
    CHECK(toi <= t);
}

TEST_CASE("No Zero ToI CCD", "[ccd][no-zero-toi]")
{
    Eigen::Vector3d p_t0, t0_t0, t1_t0, t2_t0, p_t1, t0_t1, t1_t1, t2_t1;
    p_t0 << 0.0133653, 0.100651, -0.0215935;
    t0_t0 << 0.0100485, 0.0950896, -0.0171013;
    t1_t0 << 0.0130388, 0.100666, -0.0218112;
    t2_t0 << 0.015413, 0.100554, -0.0202265;
    p_t1 << 0.0133652999767858, 0.099670000268615, -0.0215934999996444;
    t0_t1 << 0.0100484999799995, 0.0941086002577558, -0.0171012999972189;
    t1_t1 << 0.0130387999724314, 0.0996850002629403, -0.0218111999936902;
    t2_t1 << 0.0154129999740718, 0.0995730002646605, -0.020226499996014;

    double toi;
    bool is_impacting = point_triangle_ccd(
        p_t0, t0_t0, t1_t0, t2_t0, p_t1, t0_t1, t1_t1, t2_t1, toi);

    CAPTURE(toi);
    CHECK(!is_impacting);
}

TEST_CASE("Slow EE CCD", "[ccd][edge-edge][slow]")
{
    Eigen::Vector3d ea0_t0, ea1_t0, eb0_t0, eb1_t0, ea0_t1, ea1_t1, eb0_t1,
        eb1_t1;
    ea0_t0 << 1, 0.50803125, 2.10835646075301e-18;
    ea1_t0 << -2.38233935445388e-18, 0.50803125, 1;
    eb0_t0 << -4.99999999958867e-07, 0.5, 0;
    eb1_t0 << -4.99999999958867e-07, 0.5, 1;
    ea0_t1 << 1, 0.47124375, 4.11078309465837e-18;
    ea1_t1 << -2.8526707189104e-18, 0.47124375, 1;
    eb0_t1 << -4.99999999958867e-07, 0.5, 0;
    eb1_t1 << -4.99999999958867e-07, 0.5, 1;

    // BENCHMARK("compute toi")
    // {
    //     double toi;
    //     bool is_impacting = edge_edge_ccd(
    //         ea0_t0, ea1_t0, eb0_t0, eb1_t0, ea0_t1, ea1_t1, eb0_t1, eb1_t1,
    //         toi);
    // };

    double tol = DEFAULT_CCD_TOLERANCE;
    long max_iter = DEFAULT_CCD_MAX_ITERATIONS;
    while (tol < 1) {
        double toi;
        bool is_impacting = edge_edge_ccd(
            ea0_t0, ea1_t0, eb0_t0, eb1_t0, ea0_t1, ea1_t1, eb0_t1, eb1_t1, toi,
            1.0, tol, max_iter);
        tol *= 10;

        CAPTURE(toi);
        CHECK(is_impacting);
    }
}


<|start_filename|>python/src/distance/point_point.cpp<|end_filename|>
#include <pybind11/pybind11.h>
#include <pybind11/eigen.h>

#include <ipc/distance/point_point.hpp>
#include <ipc/distance/point_triangle.hpp>

#include "../utils.hpp"

namespace py = pybind11;
using namespace ipc;

void define_point_point_distance_functions(py::module_& m)
{
    m.def(
        "point_point_distance",
        [](const VectorMax3d& p0, const VectorMax3d& p1) {
            assert_2D_or_3D_vector(p0, "p0");
            assert_2D_or_3D_vector(p1, "p1");
            return point_point_distance(p0, p1);
        },
        R"ipc_Qu8mg5v7(
        Compute the distance between two points.

        Parameters:
            p0: first point
            p1: second point

        Returns:
            The distance between p0 and p1

        Note:
            The distance is actually squared distance.
        )ipc_Qu8mg5v7",
        py::arg("p0"), py::arg("p1"));

    m.def(
        "point_point_distance_gradient",
        [](const VectorMax3d& p0, const VectorMax3d& p1) {
            assert_2D_or_3D_vector(p0, "p0");
            assert_2D_or_3D_vector(p1, "p1");
            VectorMax6<double> grad;
            point_point_distance_gradient(p0, p1, grad);
            return grad;
        },
        R"ipc_Qu8mg5v7(
        Compute the gradient of the distance between two points.

        Parameters:
            p0: first point
            p1: second point

        Returns:
            The gradient of the distance wrt p0 and p1.

        Note:
            The distance is actually squared distance.
        )ipc_Qu8mg5v7",
        py::arg("p0"), py::arg("p1"));

    m.def(
        "point_point_distance_hessian",
        [](const VectorMax3d& p0, const VectorMax3d& p1) {
            assert_2D_or_3D_vector(p0, "p0");
            assert_2D_or_3D_vector(p1, "p1");
            MatrixMax6<double> hess;
            point_point_distance_hessian(p0, p1, hess);
            return hess;
        },
        R"ipc_Qu8mg5v7(
        Compute the hessian of the distance between a point and point.

        Parameters:
            p0: first point
            p1: second point

        Returns:
            The hessian of the distance wrt p0 and p1.

        Note:
            The distance is actually squared distance.
        )ipc_Qu8mg5v7",
        py::arg("p0"), py::arg("p1"));
}


<|start_filename|>src/barrier/adaptive_stiffness.hpp<|end_filename|>
// Fucntions for computing the initial and updated barrier stiffnesses.

#pragma once

#include <Eigen/Core>

#include <ipc/collision_constraint.hpp>

namespace ipc {

/// Compute an inital barrier stiffness using the barrier potential gradient.
double initial_barrier_stiffness(
    double bbox_diagonal,
    double dhat,
    double average_mass,
    const Eigen::VectorXd& grad_energy,
    const Eigen::VectorXd& grad_barrier,
    double& max_barrier_stiffness,
    double min_barrier_stiffness_scale = 1e11,
    double dmin = 0);

/// Update the barrier stiffness if the distance is decreasing and less than
/// dhat_epsilon_scale * diag.
void update_barrier_stiffness(
    double prev_min_distance,
    double min_distance,
    double max_barrier_stiffness,
    double& barrier_stiffness,
    double bbox_diagonal,
    double dhat_epsilon_scale = 1e-9,
    double dmin = 0);

/// Update the barrier stiffness if the distance is decreasing and less than
/// dhat_epsilon_scale * diag.
void update_barrier_stiffness(
    const Constraints& constraint_set,
    const Eigen::MatrixXd& V,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    double prev_min_distance,
    double& min_distance,
    double max_barrier_stiffness,
    double& barrier_stiffness,
    double dhat_epsilon_scale = 1e-9,
    double dmin = 0);

} // namespace ipc


<|start_filename|>src/utils/intersection.cpp<|end_filename|>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <ipc/utils/eigen_ext.hpp>

#include <igl/predicates/predicates.h>

#ifdef IPC_TOOLKIT_WITH_RATIONAL_INTERSECTION
#include <ipc/utils/rational.hpp>
#endif

namespace ipc {

#ifdef IPC_TOOLKIT_WITH_RATIONAL_INTERSECTION
bool is_edge_intersecting_triangle_rational(
    const Eigen::Vector3d& e0_float,
    const Eigen::Vector3d& e1_float,
    const Eigen::Vector3d& t0_float,
    const Eigen::Vector3d& t1_float,
    const Eigen::Vector3d& t2_float)
{
    Vector3<Rational> e0, e1, t0, t1, t2;

    for (int d = 0; d < 3; ++d) {
        e0[d] = e0_float[d];
        e1[d] = e1_float[d];

        t0[d] = t0_float[d];
        t1[d] = t1_float[d];
        t2[d] = t2_float[d];
    }

    const Rational d = e0[0] * t0[1] * t1[2] - e0[0] * t0[1] * t2[2]
        - e0[0] * t0[2] * t1[1] + e0[0] * t0[2] * t2[1] + e0[0] * t1[1] * t2[2]
        - e0[0] * t1[2] * t2[1] - e0[1] * t0[0] * t1[2] + e0[1] * t0[0] * t2[2]
        + e0[1] * t0[2] * t1[0] - e0[1] * t0[2] * t2[0] - e0[1] * t1[0] * t2[2]
        + e0[1] * t1[2] * t2[0] + e0[2] * t0[0] * t1[1] - e0[2] * t0[0] * t2[1]
        - e0[2] * t0[1] * t1[0] + e0[2] * t0[1] * t2[0] + e0[2] * t1[0] * t2[1]
        - e0[2] * t1[1] * t2[0] - e1[0] * t0[1] * t1[2] + e1[0] * t0[1] * t2[2]
        + e1[0] * t0[2] * t1[1] - e1[0] * t0[2] * t2[1] - e1[0] * t1[1] * t2[2]
        + e1[0] * t1[2] * t2[1] + e1[1] * t0[0] * t1[2] - e1[1] * t0[0] * t2[2]
        - e1[1] * t0[2] * t1[0] + e1[1] * t0[2] * t2[0] + e1[1] * t1[0] * t2[2]
        - e1[1] * t1[2] * t2[0] - e1[2] * t0[0] * t1[1] + e1[2] * t0[0] * t2[1]
        + e1[2] * t0[1] * t1[0] - e1[2] * t0[1] * t2[0] - e1[2] * t1[0] * t2[1]
        + e1[2] * t1[1] * t2[0];
    if (d.get_sign() == 0) {
        return true;
    }

    // t is the parametric coordinate for the edge
    const Rational t =
        (e0[0] * t0[1] * t1[2] - e0[0] * t0[1] * t2[2] - e0[0] * t0[2] * t1[1]
         + e0[0] * t0[2] * t2[1] + e0[0] * t1[1] * t2[2] - e0[0] * t1[2] * t2[1]
         - e0[1] * t0[0] * t1[2] + e0[1] * t0[0] * t2[2] + e0[1] * t0[2] * t1[0]
         - e0[1] * t0[2] * t2[0] - e0[1] * t1[0] * t2[2] + e0[1] * t1[2] * t2[0]
         + e0[2] * t0[0] * t1[1] - e0[2] * t0[0] * t2[1] - e0[2] * t0[1] * t1[0]
         + e0[2] * t0[1] * t2[0] + e0[2] * t1[0] * t2[1] - e0[2] * t1[1] * t2[0]
         - t0[0] * t1[1] * t2[2] + t0[0] * t1[2] * t2[1] + t0[1] * t1[0] * t2[2]
         - t0[1] * t1[2] * t2[0] - t0[2] * t1[0] * t2[1]
         + t0[2] * t1[1] * t2[0])
        / d;

    if (t < 0 || t > 1) {
        return false;
    }

    // u is the first barycentric coordinate for the triangle
    const Rational u =
        (-e0[0] * e1[1] * t0[2] + e0[0] * e1[1] * t2[2] + e0[0] * e1[2] * t0[1]
         - e0[0] * e1[2] * t2[1] - e0[0] * t0[1] * t2[2] + e0[0] * t0[2] * t2[1]
         + e0[1] * e1[0] * t0[2] - e0[1] * e1[0] * t2[2] - e0[1] * e1[2] * t0[0]
         + e0[1] * e1[2] * t2[0] + e0[1] * t0[0] * t2[2] - e0[1] * t0[2] * t2[0]
         - e0[2] * e1[0] * t0[1] + e0[2] * e1[0] * t2[1] + e0[2] * e1[1] * t0[0]
         - e0[2] * e1[1] * t2[0] - e0[2] * t0[0] * t2[1] + e0[2] * t0[1] * t2[0]
         + e1[0] * t0[1] * t2[2] - e1[0] * t0[2] * t2[1] - e1[1] * t0[0] * t2[2]
         + e1[1] * t0[2] * t2[0] + e1[2] * t0[0] * t2[1]
         - e1[2] * t0[1] * t2[0])
        / d;
    // v is the second barycentric coordinate for the triangle
    const Rational v =
        (e0[0] * e1[1] * t0[2] - e0[0] * e1[1] * t1[2] - e0[0] * e1[2] * t0[1]
         + e0[0] * e1[2] * t1[1] + e0[0] * t0[1] * t1[2] - e0[0] * t0[2] * t1[1]
         - e0[1] * e1[0] * t0[2] + e0[1] * e1[0] * t1[2] + e0[1] * e1[2] * t0[0]
         - e0[1] * e1[2] * t1[0] - e0[1] * t0[0] * t1[2] + e0[1] * t0[2] * t1[0]
         + e0[2] * e1[0] * t0[1] - e0[2] * e1[0] * t1[1] - e0[2] * e1[1] * t0[0]
         + e0[2] * e1[1] * t1[0] + e0[2] * t0[0] * t1[1] - e0[2] * t0[1] * t1[0]
         - e1[0] * t0[1] * t1[2] + e1[0] * t0[2] * t1[1] + e1[1] * t0[0] * t1[2]
         - e1[1] * t0[2] * t1[0] - e1[2] * t0[0] * t1[1]
         + e1[2] * t0[1] * t1[0])
        / d;

    return u >= 0 && u <= 1 && v >= 0 && v <= 1 && u + v <= 1;
}
#endif

bool is_edge_intersecting_triangle(
    const Eigen::Vector3d& e0,
    const Eigen::Vector3d& e1,
    const Eigen::Vector3d& t0,
    const Eigen::Vector3d& t1,
    const Eigen::Vector3d& t2)
{
    igl::predicates::exactinit();
    const auto ori1 = igl::predicates::orient3d(t0, t1, t2, e0);
    const auto ori2 = igl::predicates::orient3d(t0, t1, t2, e1);

    if (ori1 != igl::predicates::Orientation::COPLANAR
        && ori2 != igl::predicates::Orientation::COPLANAR && ori1 == ori2) {
        // edge is completly on one side of the plane that triangle is in
        return false;
    }

#ifdef IPC_TOOLKIT_USE_RATIONAL_INTERSECTION
    return is_edge_intersecting_triangle_rational(e0, e1, t0, t1, t2);
#else
    Eigen::Matrix3d M;
    M.col(0) = t1 - t0;
    M.col(1) = t2 - t0;
    M.col(2) = e0 - e1;
    Eigen::Vector3d uvt = M.fullPivLu().solve(e0 - t0);
    return uvt[0] >= 0.0 && uvt[1] >= 0.0 && uvt[0] + uvt[1] <= 1.0
        && uvt[2] >= 0.0 && uvt[2] <= 1.0;
#endif
}

} // namespace ipc


<|start_filename|>src/broad_phase/brute_force.cpp<|end_filename|>
#include <ipc/broad_phase/brute_force.hpp>

#include <algorithm> // std::min/max

#include <tbb/enumerable_thread_specific.h>
#include <tbb/parallel_for.h>
#include <tbb/blocked_range2d.h>

#include <ipc/ccd/aabb.hpp>

namespace ipc {

template <typename Candidate>
void merge_local_candidates(
    const tbb::enumerable_thread_specific<std::vector<Candidate>>& storages,
    std::vector<Candidate>& candidates)
{
    // size up the candidates
    size_t num_candidates = candidates.size();
    for (const auto& local_candidates : storages) {
        num_candidates += local_candidates.size();
    }
    // serial merge!
    candidates.reserve(num_candidates);
    for (const auto& local_candidates : storages) {
        candidates.insert(
            candidates.end(), local_candidates.begin(), local_candidates.end());
    }
}

///////////////////////////////////////////////////////////////////////////////
// Discrete Collision Detection

void detect_collision_candidates_brute_force(
    const Eigen::MatrixXd& V,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    Candidates& candidates,
    bool detect_edge_vertex,
    bool detect_edge_edge,
    bool detect_face_vertex,
    bool perform_aabb_check,
    double aabb_inflation_radius,
    const std::function<bool(size_t, size_t)>& can_collide)
{
    assert(E.size() == 0 || E.cols() == 2);
    assert(F.size() == 0 || F.cols() == 3);

    if (detect_edge_vertex) {
        detect_edge_vertex_collision_candidates_brute_force(
            V, E, candidates.ev_candidates, perform_aabb_check,
            aabb_inflation_radius, can_collide);
    }

    if (detect_edge_edge) {
        // Use the original can_collide because edge vertices are not
        // codimensional.
        detect_edge_edge_collision_candidates_brute_force(
            V, E, candidates.ee_candidates, perform_aabb_check,
            aabb_inflation_radius, can_collide);
    }

    if (detect_face_vertex) {
        detect_face_vertex_collision_candidates_brute_force(
            V, F, candidates.fv_candidates, perform_aabb_check,
            aabb_inflation_radius, can_collide);
    }
}

void detect_edge_vertex_collision_candidates_brute_force(
    const Eigen::MatrixXd& V,
    const Eigen::MatrixXi& E,
    std::vector<EdgeVertexCandidate>& ev_candidates,
    bool perform_aabb_check,
    double aabb_inflation_radius,
    const std::function<bool(size_t, size_t)>& can_collide)
{
    tbb::enumerable_thread_specific<std::vector<EdgeVertexCandidate>> storages;

    tbb::parallel_for(
        tbb::blocked_range2d<long>(0l, long(E.rows()), 0l, long(V.rows())),
        [&](const tbb::blocked_range2d<long>& r) {
            auto& local_candidates = storages.local();

            // Loop over edges
            for (long ei = r.rows().begin(); ei < r.rows().end(); ei++) {
                const size_t &e0i = E(ei, 0), &e1i = E(ei, 1);

                // Loop over vertices
                for (long vi = r.cols().begin(); vi < r.cols().end(); vi++) {
                    // Check that the vertex is not an endpoint of the edge
                    if (vi == e0i || vi == e1i) {
                        continue;
                    }

                    if (!can_collide(vi, e0i) && !can_collide(vi, e1i)) {
                        continue;
                    }

                    bool aabb_intersect = !perform_aabb_check
                        || point_edge_aabb_cd(
                            V.row(vi), V.row(E(ei, 0)), V.row(E(ei, 1)),
                            2 * aabb_inflation_radius);
                    if (aabb_intersect) {
                        local_candidates.emplace_back(ei, vi);
                    }
                }
            }
        });

    merge_local_candidates(storages, ev_candidates);
}

void detect_edge_edge_collision_candidates_brute_force(
    const Eigen::MatrixXd& V,
    const Eigen::MatrixXi& E,
    std::vector<EdgeEdgeCandidate>& ee_candidates,
    bool perform_aabb_check,
    double aabb_inflation_radius,
    const std::function<bool(size_t, size_t)>& can_collide)
{
    tbb::enumerable_thread_specific<std::vector<EdgeEdgeCandidate>> storages;

    long num_edges = E.rows();
    tbb::parallel_for(
        tbb::blocked_range2d<long>(0l, num_edges, 0l, num_edges),
        [&](const tbb::blocked_range2d<long>& r) {
            auto& local_candidates = storages.local();
            // eai < ebi
            long eai_end = std::min(r.rows().end(), r.cols().end());

            for (long eai = r.rows().begin(); eai < eai_end; eai++) {
                const size_t &ea0i = E(eai, 0), &ea1i = E(eai, 1);

                // i < r.cols().end() → i + 1 <= r.cols().end()
                int ebi_begin = std::max(eai + 1, r.cols().begin());
                assert(ebi_begin > eai);

                for (long ebi = ebi_begin; ebi < r.cols().end(); ebi++) {
                    const size_t &eb0i = E(ebi, 0), &eb1i = E(ebi, 1);

                    // Check for common end points
                    if (ea0i == eb0i || ea0i == eb1i || ea1i == eb0i
                        || ea1i == eb1i) {
                        continue;
                    }

                    if (!can_collide(ea0i, eb0i) && !can_collide(ea0i, eb1i)
                        && !can_collide(ea1i, eb0i)
                        && !can_collide(ea1i, eb1i)) {
                        continue;
                    }

                    bool aabb_intersect = !perform_aabb_check
                        || edge_edge_aabb_cd(
                            V.row(ea0i), V.row(ea1i), V.row(eb0i), V.row(eb1i),
                            2 * aabb_inflation_radius);
                    if (aabb_intersect) {
                        local_candidates.emplace_back(eai, ebi);
                    }
                }
            }
        });

    merge_local_candidates(storages, ee_candidates);
}

void detect_face_vertex_collision_candidates_brute_force(
    const Eigen::MatrixXd& V,
    const Eigen::MatrixXi& F,
    std::vector<FaceVertexCandidate>& fv_candidates,
    bool perform_aabb_check,
    double aabb_inflation_radius,
    const std::function<bool(size_t, size_t)>& can_collide)
{
    tbb::enumerable_thread_specific<std::vector<FaceVertexCandidate>> storages;

    tbb::parallel_for(
        tbb::blocked_range2d<long>(0l, long(F.rows()), 0l, long(V.rows())),
        [&](const tbb::blocked_range2d<long>& r) {
            auto& local_candidates = storages.local();

            // Loop over faces
            for (int fi = r.rows().begin(); fi < r.rows().end(); fi++) {
                const size_t &f0i = F(fi, 0), &f1i = F(fi, 1), &f2i = F(fi, 2);

                // Loop over vertices
                for (int vi = r.cols().begin(); vi < r.cols().end(); vi++) {
                    // Check for common end points
                    if (vi == f0i || vi == f1i || vi == f2i) {
                        continue;
                    }

                    if (!can_collide(vi, f0i) && !can_collide(vi, f1i)
                        && !can_collide(vi, f2i)) {
                        continue;
                    }

                    bool aabb_intersect = !perform_aabb_check
                        || point_triangle_aabb_cd(
                            V.row(vi), V.row(f0i), V.row(f1i), V.row(f2i),
                            2 * aabb_inflation_radius);
                    if (aabb_intersect) {
                        local_candidates.emplace_back(fi, vi);
                    }
                }
            }
        });

    merge_local_candidates(storages, fv_candidates);
}

void detect_edge_face_collision_candidates_brute_force(
    const Eigen::MatrixXd& V,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    std::vector<EdgeFaceCandidate>& ef_candidates,
    bool perform_aabb_check,
    double aabb_inflation_radius,
    const std::function<bool(size_t, size_t)>& can_collide)
{
    tbb::enumerable_thread_specific<std::vector<EdgeFaceCandidate>> storages;

    tbb::parallel_for(
        tbb::blocked_range2d<long>(0l, long(E.rows()), 0l, long(F.rows())),
        [&](const tbb::blocked_range2d<long>& r) {
            auto& local_candidates = storages.local();

            // Loop over faces
            for (int ei = r.rows().begin(); ei < r.rows().end(); ei++) {
                const size_t &e0i = E(ei, 0), &e1i = E(ei, 1);

                // Loop over vertices
                for (int fi = r.cols().begin(); fi < r.cols().end(); fi++) {
                    const size_t &f0i = F(fi, 0), &f1i = F(fi, 1),
                                 &f2i = F(fi, 2);

                    // Check for common end points
                    if (e0i == f0i || e0i == f1i || e0i == f2i || e1i == f0i
                        || e1i == f1i || e1i == f2i) {
                        continue;
                    }

                    if (!can_collide(e0i, f0i) && !can_collide(e0i, f1i)
                        && !can_collide(e0i, f2i) && !can_collide(e1i, f0i)
                        && !can_collide(e1i, f1i) && !can_collide(e1i, f2i)) {
                        continue;
                    }

                    bool aabb_intersect = !perform_aabb_check
                        || edge_triangle_aabb_cd(
                            V.row(e0i), V.row(e1i), V.row(f0i), V.row(f1i),
                            V.row(f2i), 2 * aabb_inflation_radius);
                    if (aabb_intersect) {
                        local_candidates.emplace_back(ei, fi);
                    }
                }
            }
        });

    merge_local_candidates(storages, ef_candidates);
}

///////////////////////////////////////////////////////////////////////////////
// Continuous Collision Detection

void detect_collision_candidates_brute_force(
    const Eigen::MatrixXd& V0,
    const Eigen::MatrixXd& V1,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    Candidates& candidates,
    bool detect_edge_vertex,
    bool detect_edge_edge,
    bool detect_face_vertex,
    bool perform_aabb_check,
    double aabb_inflation_radius,
    const std::function<bool(size_t, size_t)>& can_collide)
{
    assert(V0.rows() == V1.rows() && V0.cols() == V1.cols());
    assert(E.size() == 0 || E.cols() == 2);
    assert(F.size() == 0 || F.cols() == 3);

    if (detect_edge_vertex) {
        detect_edge_vertex_collision_candidates_brute_force(
            V0, V1, E, candidates.ev_candidates, perform_aabb_check,
            aabb_inflation_radius, can_collide);
    }

    if (detect_edge_edge) {
        // Use the original can_collide because edge vertices are not
        // codimensional.
        detect_edge_edge_collision_candidates_brute_force(
            V0, V1, E, candidates.ee_candidates, perform_aabb_check,
            aabb_inflation_radius, can_collide);
    }

    if (detect_face_vertex) {
        detect_face_vertex_collision_candidates_brute_force(
            V0, V1, F, candidates.fv_candidates, perform_aabb_check,
            aabb_inflation_radius, can_collide);
    }
}

void detect_edge_vertex_collision_candidates_brute_force(
    const Eigen::MatrixXd& V0,
    const Eigen::MatrixXd& V1,
    const Eigen::MatrixXi& E,
    std::vector<EdgeVertexCandidate>& ev_candidates,
    bool perform_aabb_check,
    double aabb_inflation_radius,
    const std::function<bool(size_t, size_t)>& can_collide)
{
    assert(V0.rows() == V1.rows() && V0.cols() == V1.cols());

    tbb::enumerable_thread_specific<std::vector<EdgeVertexCandidate>> storages;

    tbb::parallel_for(
        tbb::blocked_range2d<long>(0l, long(E.rows()), 0l, long(V0.rows())),
        [&](const tbb::blocked_range2d<long>& r) {
            auto& local_candidates = storages.local();

            // Loop over edges
            for (long ei = r.rows().begin(); ei < r.rows().end(); ei++) {
                const size_t &e0i = E(ei, 0), &e1i = E(ei, 1);

                // Loop over vertices
                for (long vi = r.cols().begin(); vi < r.cols().end(); vi++) {
                    // Check that the vertex is not an endpoint of the edge
                    if (vi == e0i || vi == e1i) {
                        continue;
                    }

                    if (!can_collide(vi, e0i) && !can_collide(vi, e1i)) {
                        continue;
                    }

                    bool aabb_intersect = !perform_aabb_check
                        || point_edge_aabb_ccd(
                            V0.row(vi), V0.row(e0i), V0.row(e0i), V1.row(vi),
                            V1.row(e0i), V1.row(e1i),
                            2 * aabb_inflation_radius);
                    if (aabb_intersect) {
                        local_candidates.emplace_back(ei, vi);
                    }
                }
            }
        });

    merge_local_candidates(storages, ev_candidates);
}

void detect_edge_edge_collision_candidates_brute_force(
    const Eigen::MatrixXd& V0,
    const Eigen::MatrixXd& V1,
    const Eigen::MatrixXi& E,
    std::vector<EdgeEdgeCandidate>& ee_candidates,
    bool perform_aabb_check,
    double aabb_inflation_radius,
    const std::function<bool(size_t, size_t)>& can_collide)
{
    assert(V0.rows() == V1.rows() && V0.cols() == V1.cols());

    tbb::enumerable_thread_specific<std::vector<EdgeEdgeCandidate>> storages;

    long num_edges = E.rows();
    tbb::parallel_for(
        tbb::blocked_range2d<long>(0l, num_edges, 0l, num_edges),
        [&](const tbb::blocked_range2d<long>& r) {
            auto& local_candidates = storages.local();
            // eai < ebi
            long eai_end = std::min(r.rows().end(), r.cols().end());

            for (long eai = r.rows().begin(); eai < eai_end; eai++) {
                const size_t &ea0i = E(eai, 0), &ea1i = E(eai, 1);

                // i < r.cols().end() → i + 1 <= r.cols().end()
                int ebi_begin = std::max(eai + 1, r.cols().begin());
                assert(ebi_begin > eai);

                for (long ebi = ebi_begin; ebi < r.cols().end(); ebi++) {
                    const size_t &eb0i = E(ebi, 0), &eb1i = E(ebi, 1);

                    // Check for common end points
                    if (ea0i == eb0i || ea0i == eb1i || ea1i == eb0i
                        || ea1i == eb1i) {
                        continue;
                    }

                    if (!can_collide(ea0i, eb0i) && !can_collide(ea0i, eb1i)
                        && !can_collide(ea1i, eb0i)
                        && !can_collide(ea1i, eb1i)) {
                        continue;
                    }

                    bool aabb_intersect = !perform_aabb_check
                        || edge_edge_aabb_ccd(
                            V0.row(ea0i), V0.row(ea1i), V0.row(eb0i),
                            V0.row(eb1i), V1.row(ea0i), V1.row(ea1i),
                            V1.row(eb0i), V1.row(eb1i),
                            2 * aabb_inflation_radius);
                    if (aabb_intersect) {
                        local_candidates.emplace_back(eai, ebi);
                    }
                }
            }
        });

    merge_local_candidates(storages, ee_candidates);
}

void detect_face_vertex_collision_candidates_brute_force(
    const Eigen::MatrixXd& V0,
    const Eigen::MatrixXd& V1,
    const Eigen::MatrixXi& F,
    std::vector<FaceVertexCandidate>& fv_candidates,
    bool perform_aabb_check,
    double aabb_inflation_radius,
    const std::function<bool(size_t, size_t)>& can_collide)
{
    assert(V0.rows() == V1.rows() && V0.cols() == V1.cols());

    tbb::enumerable_thread_specific<std::vector<FaceVertexCandidate>> storages;

    tbb::parallel_for(
        tbb::blocked_range2d<long>(0l, long(F.rows()), 0l, long(V0.rows())),
        [&](const tbb::blocked_range2d<long>& r) {
            auto& local_candidates = storages.local();

            // Loop over faces
            for (int fi = r.rows().begin(); fi < r.rows().end(); fi++) {
                const size_t &f0i = F(fi, 0), &f1i = F(fi, 1), &f2i = F(fi, 2);

                // Loop over vertices
                for (int vi = r.cols().begin(); vi < r.cols().end(); vi++) {
                    // Check for common end points
                    if (vi == f0i || vi == f1i || vi == f2i) {
                        continue;
                    }

                    if (!can_collide(vi, f0i) && !can_collide(vi, f1i)
                        && !can_collide(vi, f2i)) {
                        continue;
                    }

                    bool aabb_intersect = !perform_aabb_check
                        || point_triangle_aabb_ccd(
                            V0.row(vi), V0.row(f0i), V0.row(f1i), V0.row(f2i),
                            V1.row(vi), V1.row(f0i), V1.row(f1i), V1.row(f2i),
                            2 * aabb_inflation_radius);
                    if (aabb_intersect) {
                        local_candidates.emplace_back(fi, vi);
                    }
                }
            }
        });

    merge_local_candidates(storages, fv_candidates);
}

} // namespace ipc


<|start_filename|>src/utils/eigen_ext.cpp<|end_filename|>
#include <ipc/utils/eigen_ext.hpp>

namespace ipc {

} // namespace ipc


<|start_filename|>src/collision_constraint.hpp<|end_filename|>
#pragma once

#include <Eigen/Core>

#include <ipc/broad_phase/collision_candidate.hpp>
#include <ipc/utils/eigen_ext.hpp>

namespace ipc {

struct CollisionConstraint {
public:
    virtual ~CollisionConstraint() {}

    virtual std::vector<long> vertex_indices(
        const Eigen::MatrixXi& E, const Eigen::MatrixXi& F) const = 0;

    virtual double compute_distance(
        const Eigen::MatrixXd& V,
        const Eigen::MatrixXi& E,
        const Eigen::MatrixXi& F) const = 0;

    virtual VectorMax12d compute_distance_gradient(
        const Eigen::MatrixXd& V,
        const Eigen::MatrixXi& E,
        const Eigen::MatrixXi& F) const = 0;

    virtual MatrixMax12d compute_distance_hessian(
        const Eigen::MatrixXd& V,
        const Eigen::MatrixXi& E,
        const Eigen::MatrixXi& F) const = 0;

    virtual double compute_potential(
        const Eigen::MatrixXd& V,
        const Eigen::MatrixXi& E,
        const Eigen::MatrixXi& F,
        const double dhat) const;

    virtual VectorMax12d compute_potential_gradient(
        const Eigen::MatrixXd& V,
        const Eigen::MatrixXi& E,
        const Eigen::MatrixXi& F,
        const double dhat) const;

    virtual MatrixMax12d compute_potential_hessian(
        const Eigen::MatrixXd& V,
        const Eigen::MatrixXi& E,
        const Eigen::MatrixXi& F,
        const double dhat,
        const bool project_hessian_to_psd) const;

    double minimum_distance = 0;
};

///////////////////////////////////////////////////////////////////////////////

struct VertexVertexConstraint : VertexVertexCandidate, CollisionConstraint {
    VertexVertexConstraint(long vertex0_index, long vertex1_index);
    VertexVertexConstraint(const VertexVertexCandidate& candidate);

    std::vector<long> vertex_indices(
        const Eigen::MatrixXi& E, const Eigen::MatrixXi& F) const override
    {
        return { { vertex0_index, vertex1_index } };
    }

    double compute_distance(
        const Eigen::MatrixXd& V,
        const Eigen::MatrixXi& E,
        const Eigen::MatrixXi& F) const override;

    VectorMax12d compute_distance_gradient(
        const Eigen::MatrixXd& V,
        const Eigen::MatrixXi& E,
        const Eigen::MatrixXi& F) const override;

    MatrixMax12d compute_distance_hessian(
        const Eigen::MatrixXd& V,
        const Eigen::MatrixXi& E,
        const Eigen::MatrixXi& F) const override;

    double compute_potential(
        const Eigen::MatrixXd& V,
        const Eigen::MatrixXi& E,
        const Eigen::MatrixXi& F,
        const double dhat) const override;

    VectorMax12d compute_potential_gradient(
        const Eigen::MatrixXd& V,
        const Eigen::MatrixXi& E,
        const Eigen::MatrixXi& F,
        const double dhat) const override;

    MatrixMax12d compute_potential_hessian(
        const Eigen::MatrixXd& V,
        const Eigen::MatrixXi& E,
        const Eigen::MatrixXi& F,
        const double dhat,
        const bool project_hessian_to_psd) const override;

    unsigned int multiplicity = 1;
};

///////////////////////////////////////////////////////////////////////////////

struct EdgeVertexConstraint : EdgeVertexCandidate, CollisionConstraint {
    EdgeVertexConstraint(long edge_index, long vertex_index);
    EdgeVertexConstraint(const EdgeVertexCandidate& candidate);

    std::vector<long> vertex_indices(
        const Eigen::MatrixXi& E, const Eigen::MatrixXi& F) const override
    {
        return { { vertex_index, E(edge_index, 0), E(edge_index, 1) } };
    }

    double compute_distance(
        const Eigen::MatrixXd& V,
        const Eigen::MatrixXi& E,
        const Eigen::MatrixXi& F) const override;

    VectorMax12d compute_distance_gradient(
        const Eigen::MatrixXd& V,
        const Eigen::MatrixXi& E,
        const Eigen::MatrixXi& F) const override;

    MatrixMax12d compute_distance_hessian(
        const Eigen::MatrixXd& V,
        const Eigen::MatrixXi& E,
        const Eigen::MatrixXi& F) const override;

    double compute_potential(
        const Eigen::MatrixXd& V,
        const Eigen::MatrixXi& E,
        const Eigen::MatrixXi& F,
        const double dhat) const override;

    VectorMax12d compute_potential_gradient(
        const Eigen::MatrixXd& V,
        const Eigen::MatrixXi& E,
        const Eigen::MatrixXi& F,
        const double dhat) const override;

    MatrixMax12d compute_potential_hessian(
        const Eigen::MatrixXd& V,
        const Eigen::MatrixXi& E,
        const Eigen::MatrixXi& F,
        const double dhat,
        const bool project_hessian_to_psd) const override;

    unsigned int multiplicity = 1;
};

///////////////////////////////////////////////////////////////////////////////

struct EdgeEdgeConstraint : EdgeEdgeCandidate, CollisionConstraint {
    EdgeEdgeConstraint(long edge0_index, long edge1_index, double eps_x);
    EdgeEdgeConstraint(const EdgeEdgeCandidate& candidate, double eps_x);

    std::vector<long> vertex_indices(
        const Eigen::MatrixXi& E, const Eigen::MatrixXi& F) const override
    {
        return { { E(edge0_index, 0), E(edge0_index, 1), //
                   E(edge1_index, 0), E(edge1_index, 1) } };
    }

    double compute_distance(
        const Eigen::MatrixXd& V,
        const Eigen::MatrixXi& E,
        const Eigen::MatrixXi& F) const override;

    VectorMax12d compute_distance_gradient(
        const Eigen::MatrixXd& V,
        const Eigen::MatrixXi& E,
        const Eigen::MatrixXi& F) const override;

    MatrixMax12d compute_distance_hessian(
        const Eigen::MatrixXd& V,
        const Eigen::MatrixXi& E,
        const Eigen::MatrixXi& F) const override;

    double compute_potential(
        const Eigen::MatrixXd& V,
        const Eigen::MatrixXi& E,
        const Eigen::MatrixXi& F,
        const double dhat) const override;

    VectorMax12d compute_potential_gradient(
        const Eigen::MatrixXd& V,
        const Eigen::MatrixXi& E,
        const Eigen::MatrixXi& F,
        const double dhat) const override;

    MatrixMax12d compute_potential_hessian(
        const Eigen::MatrixXd& V,
        const Eigen::MatrixXi& E,
        const Eigen::MatrixXi& F,
        const double dhat,
        const bool project_hessian_to_psd) const override;

    double eps_x;
};

///////////////////////////////////////////////////////////////////////////////

struct FaceVertexConstraint : FaceVertexCandidate, CollisionConstraint {
    FaceVertexConstraint(long face_index, long vertex_index);
    FaceVertexConstraint(const FaceVertexCandidate& candidate);

    std::vector<long> vertex_indices(
        const Eigen::MatrixXi& E, const Eigen::MatrixXi& F) const override
    {
        return { { vertex_index, //
                   F(face_index, 0), F(face_index, 1), F(face_index, 2) } };
    }

    double compute_distance(
        const Eigen::MatrixXd& V,
        const Eigen::MatrixXi& E,
        const Eigen::MatrixXi& F) const override;

    VectorMax12d compute_distance_gradient(
        const Eigen::MatrixXd& V,
        const Eigen::MatrixXi& E,
        const Eigen::MatrixXi& F) const override;

    MatrixMax12d compute_distance_hessian(
        const Eigen::MatrixXd& V,
        const Eigen::MatrixXi& E,
        const Eigen::MatrixXi& F) const override;
};

///////////////////////////////////////////////////////////////////////////////

struct PlaneVertexConstraint : CollisionConstraint {
    PlaneVertexConstraint(
        const VectorMax3d& plane_origin,
        const VectorMax3d& plane_normal,
        const long vertex_index);

    std::vector<long> vertex_indices(
        const Eigen::MatrixXi& E, const Eigen::MatrixXi& F) const override
    {
        return { vertex_index };
    }

    double compute_distance(
        const Eigen::MatrixXd& V,
        const Eigen::MatrixXi& E,
        const Eigen::MatrixXi& F) const override;

    VectorMax12d compute_distance_gradient(
        const Eigen::MatrixXd& V,
        const Eigen::MatrixXi& E,
        const Eigen::MatrixXi& F) const override;

    MatrixMax12d compute_distance_hessian(
        const Eigen::MatrixXd& V,
        const Eigen::MatrixXi& E,
        const Eigen::MatrixXi& F) const override;

    VectorMax3d plane_origin;
    VectorMax3d plane_normal;
    long vertex_index;
};

///////////////////////////////////////////////////////////////////////////////

struct Constraints {
    std::vector<VertexVertexConstraint> vv_constraints;
    std::vector<EdgeVertexConstraint> ev_constraints;
    std::vector<EdgeEdgeConstraint> ee_constraints;
    std::vector<FaceVertexConstraint> fv_constraints;
    std::vector<PlaneVertexConstraint> pv_constraints;

    size_t size() const;

    size_t num_constraints() const;

    bool empty() const;

    void clear();

    CollisionConstraint& operator[](size_t idx);
    const CollisionConstraint& operator[](size_t idx) const;
};

} // namespace ipc


<|start_filename|>src/barrier/barrier.hpp<|end_filename|>
// Barrier functions that grow to infinity as x -> 0+. Includes gradient and
// hessian functions, too. These barrier functions can be used to impose
// inequlity constraints on a function.

#pragma once

#include <Eigen/Core>

namespace ipc {

/// @brief Function that grows to infinity as x approaches 0 from the right.
///
/// \f$b(d) = -(d-\hat{d})^2\ln\left(\frac{d}{\hat{d}}\right)\f$
///
/// @param d The distance.
/// @param dhat Activation distance of the barrier.
/// @return The value of the barrier function at d.
template <typename T> T barrier(const T& d, double dhat);

/// @brief Derivative of the barrier function.
///
/// \f$b'(d) = (\hat{d}-d) \left( 2\ln\left( \frac{d}{\hat{d}} \right) -
/// \frac{\hat{d}}{d} + 1\right)\f$
///
/// @param d The distance.
/// @param dhat Activation distance of the barrier.
/// @return The derivative of the barrier wrt d.
double barrier_gradient(double d, double dhat);

/// @brief Second derivative of the barrier function.
///
/// \f$b''(d) = \left( \frac{\hat{d}}{d} + 2 \right) \frac{\hat{d}}{d} -
/// 2\ln\left( \frac{d}{\hat{d}} \right) - 3\f$
///
/// @param d The distance.
/// @param dhat Activation distance of the barrier.
/// @return The second derivative of the barrier wrt d.
double barrier_hessian(double d, double dhat);

} // namespace ipc

#include "barrier.tpp"


<|start_filename|>tests/distance/test_point_edge.cpp<|end_filename|>
#include <catch2/catch.hpp>

#include <finitediff.hpp>
#include <igl/PI.h>

#include <ipc/distance/point_edge.hpp>
#include <ipc/utils/eigen_ext.hpp>

using namespace ipc;

TEST_CASE("Point-edge distance", "[distance][point-edge]")
{
    int dim = GENERATE(2, 3);
    double expected_distance = GENERATE(-10, -1, -1e-12, 0, 1e-12, 1, 10);
    VectorMax3d p = VectorMax3d::Zero(dim);
    p.y() = expected_distance;
    VectorMax3d e0 = VectorMax3d::Zero(dim);
    e0.x() = -10;
    VectorMax3d e1 = VectorMax3d::Zero(dim);
    e1.x() = 10;

    double distance = point_edge_distance(p, e0, e1);
    CHECK(distance == Approx(expected_distance * expected_distance));
}

TEST_CASE("Point-edge distance gradient", "[distance][point-edge][gradient]")
{
    int dim = GENERATE(2, 3);
    double expected_distance = GENERATE(-10, -1, -1e-12, 0, 1e-12, 1, 10);
    VectorMax3d p = VectorMax3d::Zero(dim);
    p.y() = expected_distance;
    VectorMax3d e0 = VectorMax3d::Zero(dim);
    e0.x() = -10;
    VectorMax3d e1 = VectorMax3d::Zero(dim);
    e1.x() = 10;

    Eigen::VectorXd grad;
    point_edge_distance_gradient(p, e0, e1, grad);

    // Compute the gradient using finite differences
    Eigen::VectorXd x(3 * dim);
    x.segment(0 * dim, dim) = p;
    x.segment(1 * dim, dim) = e0;
    x.segment(2 * dim, dim) = e1;
    auto f = [&dim](const Eigen::VectorXd& x) {
        return point_edge_distance(
            x.head(dim), x.segment(dim, dim), x.tail(dim));
    };
    Eigen::VectorXd fgrad;
    fd::finite_gradient(x, f, fgrad);

    CHECK(fd::compare_gradient(grad, fgrad));
}

TEST_CASE("Point-edge distance hessian", "[distance][point-edge][hessian]")
{
    int dim = GENERATE(2, 3);
    double expected_distance = GENERATE(-10, -1, -1e-12, 0, 1e-12, 1, 10);
    VectorMax3d p = VectorMax3d::Zero(dim);
    p.y() = expected_distance;
    VectorMax3d e0 = VectorMax3d::Zero(dim);
    e0.x() = -10;
    VectorMax3d e1 = VectorMax3d::Zero(dim);
    e1.x() = 10;

    Eigen::MatrixXd hess;
    point_edge_distance_hessian(p, e0, e1, hess);

    // Compute the gradient using finite differences
    Eigen::VectorXd x(3 * dim);
    x.segment(0 * dim, dim) = p;
    x.segment(1 * dim, dim) = e0;
    x.segment(2 * dim, dim) = e1;
    auto f = [&dim](const Eigen::VectorXd& x) {
        return point_edge_distance(
            x.head(dim), x.segment(dim, dim), x.tail(dim));
    };
    Eigen::MatrixXd fhess;
    fd::finite_hessian(x, f, fhess);

    CHECK(fd::compare_hessian(hess, fhess, 1e-2));
}


<|start_filename|>tests/broad_phase/benchmark_spatial_hash.cpp<|end_filename|>
#include <catch2/catch.hpp>

#include <Eigen/Core>

#include <igl/IO>
#include <igl/edges.h>
#include <igl/Timer.h>

#include <ipc/ipc.hpp>
#include <ipc/broad_phase/hash_grid.hpp>
#include <ipc/broad_phase/spatial_hash.hpp>
#include <ipc/broad_phase/brute_force.hpp>

using namespace ipc;

static const bool BENCHMARK_EV = false;
static const bool BENCHMARK_EE = false;
static const bool BENCHMARK_FV = true;

TEST_CASE(
    "Benchmark different spatial hashes",
    "[!benchmark][spatial_hash][hash_grid]")
{
    Eigen::MatrixXd V0, V1;
    Eigen::MatrixXi E, F;

#ifdef NDEBUG
    std::string filename =
        GENERATE(std::string("cube.obj"), std::string("bunny.obj"));
#else
    std::string filename = "cube.obj";
#endif
    std::string mesh_path = std::string(TEST_DATA_DIR) + filename;
    bool success = igl::read_triangle_mesh(mesh_path, V0, F);
    REQUIRE(success);

    VectorMax3d min_V = V0.colwise().minCoeff();
    VectorMax3d max_V = V0.colwise().maxCoeff();
    VectorMax3d center = (max_V + min_V) / 2;
    V0.rowwise() -= center.transpose();

    SECTION("Squish")
    {
        igl::edges(F, E);
        double s = GENERATE(0.1, 0.0, -0.1);
        V1 = V0;
        V1.col(0) *= s;
    }
    SECTION("Interobject")
    {
        VectorMax3d scale = max_V - min_V;
        for (int j = 0; j < V0.cols(); j++) {
            V0.col(j).array() /= scale(j);
        }

        int n = V0.rows();

        Eigen::MatrixXd V(2 * n, V0.cols());
        V.topRows(n) = V0;
        V.block(0, 0, n, 1).array() += 0.5;
        V.bottomRows(n) = V0;
        V.block(n, 0, n, 1).array() -= 0.5;
        V0 = V;

        double dx = GENERATE(0.0, 0.25, 0.75, 1.0);
        V1 = V0;
        V1.block(0, 0, n, 1).array() -= dx;
        V1.block(n, 0, n, 1).array() += dx;

        Eigen::MatrixXi F_stack(2 * F.rows(), F.cols());
        F_stack.topRows(F.rows()) = F;
        F_stack.bottomRows(F.rows()) = F.array() + n;
        F = F_stack;

        igl::edges(F, E);
    }

    double inflation_radius = 1e-2; // GENERATE(take(5, random(0.0, 0.1)));

    BENCHMARK("SpatialHash")
    {
        SpatialHash sh;
        sh.build(V0, V1, E, F, inflation_radius);

        Candidates candidates;
        sh.queryMeshForCandidates(
            V0, V1, E, F, candidates,
            /*queryEV=*/BENCHMARK_EV,
            /*queryEE=*/BENCHMARK_EE,
            /*queryFV=*/BENCHMARK_FV);
    };

    BENCHMARK("HashGrid")
    {
        HashGrid hashgrid;
        hashgrid.resize(V0, V1, E, inflation_radius);
        hashgrid.addVertices(V0, V1, inflation_radius);
        hashgrid.addEdges(V0, V1, E, inflation_radius);
        hashgrid.addFaces(V0, V1, F, inflation_radius);

        Candidates candidates;
        if (BENCHMARK_EV) {
            hashgrid.getVertexEdgePairs(E, candidates.ev_candidates);
        }
        if (BENCHMARK_EE) {
            hashgrid.getEdgeEdgePairs(E, candidates.ee_candidates);
        }
        if (BENCHMARK_FV) {
            hashgrid.getFaceVertexPairs(F, candidates.fv_candidates);
        }
    };

    BENCHMARK("BruteForce")
    {
        Candidates candidates;
        detect_collision_candidates_brute_force(
            V0, V1, E, F, candidates,
            /*detect_edge_vertex=*/BENCHMARK_EV,
            /*detect_edge_edge=*/BENCHMARK_EE,
            /*detect_face_vertex=*/BENCHMARK_FV,
            /*perform_aabb_check=*/true, inflation_radius);
    };
}

TEST_CASE(
    "Benchmark different spatial hashes on real data",
    "[!benchmark][spatial_hash][hash_grid][real_data]")
{
    Eigen::MatrixXd V0, V1;
    Eigen::MatrixXi E, F;

    std::string mesh_path =
        std::string(TEST_DATA_DIR) + "slow-broadphase-ccd/s0.obj";
    bool success = igl::read_triangle_mesh(mesh_path, V0, F);
    if (!success) {
        return; // Data is private
    }

    mesh_path = std::string(TEST_DATA_DIR) + "slow-broadphase-ccd/s1.obj";
    success = igl::read_triangle_mesh(mesh_path, V1, F);
    if (!success) {
        return; // Data is private
    }

    igl::edges(F, E);

    double inflation_radius = 1e-2; // GENERATE(take(5, random(0.0, 0.1)));

    BENCHMARK("SpatialHash")
    {
        SpatialHash sh;
        sh.build(V0, V1, E, F, inflation_radius);

        Candidates candidates;
        sh.queryMeshForCandidates(
            V0, V1, E, F, candidates,
            /*queryEV=*/BENCHMARK_EV,
            /*queryEE=*/BENCHMARK_EE,
            /*queryFV=*/BENCHMARK_FV);
    };

    BENCHMARK("HashGrid")
    {
        HashGrid hashgrid;
        hashgrid.resize(V0, V1, E, inflation_radius);
        hashgrid.addVertices(V0, V1, inflation_radius);
        hashgrid.addEdges(V0, V1, E, inflation_radius);
        hashgrid.addFaces(V0, V1, F, inflation_radius);

        Candidates candidates;
        if (BENCHMARK_EV) {
            hashgrid.getVertexEdgePairs(E, candidates.ev_candidates);
        }
        if (BENCHMARK_EE) {
            hashgrid.getEdgeEdgePairs(E, candidates.ee_candidates);
        }
        if (BENCHMARK_FV) {
            hashgrid.getFaceVertexPairs(F, candidates.fv_candidates);
        }
    };

    BENCHMARK("BruteForce")
    {
        Candidates candidates;
        detect_collision_candidates_brute_force(
            V0, V1, E, F, candidates,
            /*detect_edge_vertex=*/BENCHMARK_EV,
            /*detect_edge_edge=*/BENCHMARK_EE,
            /*detect_face_vertex=*/BENCHMARK_FV,
            /*perform_aabb_check=*/true, inflation_radius);
    };
}


<|start_filename|>python/src/distance/edge_edge_mollifier.cpp<|end_filename|>
#include <pybind11/pybind11.h>
#include <pybind11/eigen.h>

#include <ipc/distance/edge_edge_mollifier.hpp>

#include "../utils.hpp"

namespace py = pybind11;
using namespace ipc;

void define_edge_edge_mollifier_functions(py::module_& m)
{
    m.def(
        "edge_edge_cross_squarednorm",
        [](const Eigen::Vector3d& ea0, const Eigen::Vector3d& ea1,
           const Eigen::Vector3d& eb0, const Eigen::Vector3d& eb1) {
            return edge_edge_cross_squarednorm(ea0, ea1, eb0, eb1);
        },
        R"ipc_Qu8mg5v7(
        Compute the squared norm of the edge-edge cross product.

        Parameters:
            ea0: first vertex of the first edge
            ea1: second vertex of the first edge
            eb0: first vertex of the second edge
            eb1: second vertex of the second edge

        Returns:
            The squared norm of the edge-edge cross product.
        )ipc_Qu8mg5v7",
        py::arg("ea0"), py::arg("ea1"), py::arg("eb0"), py::arg("eb1"));

    m.def(
        "edge_edge_cross_squarednorm_gradient",
        [](const Eigen::Vector3d& ea0, const Eigen::Vector3d& ea1,
           const Eigen::Vector3d& eb0, const Eigen::Vector3d& eb1) {
            Vector<double, 12> grad;
            edge_edge_cross_squarednorm_gradient(ea0, ea1, eb0, eb1, grad);
            return grad;
        },
        R"ipc_Qu8mg5v7(
        Compute the gradient of the squared norm of the edge cross product.

        Parameters:
            ea0: first vertex of the first edge
            ea1: second vertex of the first edge
            eb0: first vertex of the second edge
            eb1: second vertex of the second edge

        Returns:
            The gradient of the squared norm of the edge cross product wrt ea0, ea1, eb0, and eb1.
        )ipc_Qu8mg5v7",
        py::arg("ea0"), py::arg("ea1"), py::arg("eb0"), py::arg("eb1"));

    m.def(
        "edge_edge_cross_squarednorm_hessian",
        [](const Eigen::Vector3d& ea0, const Eigen::Vector3d& ea1,
           const Eigen::Vector3d& eb0, const Eigen::Vector3d& eb1) {
            Eigen::Matrix<double, 12, 12> hess;
            edge_edge_cross_squarednorm_hessian(ea0, ea1, eb0, eb1, hess);
            return hess;
        },
        R"ipc_Qu8mg5v7(
        Compute the hessian of the squared norm of the edge cross product.

        Parameters:
            ea0: first vertex of the first edge
            ea1: second vertex of the first edge
            eb0: first vertex of the second edge
            eb1: second vertex of the second edge

        Returns:
            The hessian of the squared norm of the edge cross product wrt ea0, ea1, eb0, and eb1.
        )ipc_Qu8mg5v7",
        py::arg("ea0"), py::arg("ea1"), py::arg("eb0"), py::arg("eb1"));

    m.def(
        "edge_edge_mollifier", &edge_edge_mollifier<double>,
        R"ipc_Qu8mg5v7(
        Mollifier function for edge-edge distance.

        Parameters:
            - x: squared norm of the edge-edge cross product
            - eps_x: mollifier activation threshold

        Returns:
            The mollifier coefficient to premultiply the edge-edge distance.
        )ipc_Qu8mg5v7",
        py::arg("x"), py::arg("eps_x"));

    m.def(
        "edge_edge_mollifier_gradient", &edge_edge_mollifier_gradient<double>,
        R"ipc_Qu8mg5v7(
        The gradient of the mollifier function for edge-edge distance.

        Parameters:
            - x: squared norm of the edge-edge cross product
            - eps_x: mollifier activation threshold

        Returns:
            The gradient of the mollifier function for edge-edge distance wrt x.
        )ipc_Qu8mg5v7",
        py::arg("x"), py::arg("eps_x"));

    m.def(
        "edge_edge_mollifier_hessian", &edge_edge_mollifier_hessian<double>,
        R"ipc_Qu8mg5v7(
        The hessian of the mollifier function for edge-edge distance.

        Parameters:
            - x: squared norm of the edge-edge cross product
            - eps_x: mollifier activation threshold

        Returns:
            The hessian of the mollifier function for edge-edge distance wrt x.
        )ipc_Qu8mg5v7",
        py::arg("x"), py::arg("eps_x"));

    m.def(
        "edge_edge_mollifier",
        [](const Eigen::Vector3d& ea0, const Eigen::Vector3d& ea1,
           const Eigen::Vector3d& eb0, const Eigen::Vector3d& eb1,
           const double eps_x) {
            return edge_edge_mollifier(ea0, ea1, eb0, eb1, eps_x);
        },
        R"ipc_Qu8mg5v7(
        Compute a mollifier for the edge-edge distance.

        This helps smooth the non-smoothness at close to parallel edges.

        Parameters:
            - ea0: first vertex of the first edge
            - ea1: second vertex of the first edge
            - eb0: first vertex of the second edge
            - eb1: second vertex of the second edge
            - eps_x: mollifier activation threshold

        Returns:
            The mollifier coefficient to premultiply the edge-edge distance.
        )ipc_Qu8mg5v7",
        py::arg("ea0"), py::arg("ea1"), py::arg("eb0"), py::arg("eb1"),
        py::arg("eps_x"));

    m.def(
        "edge_edge_mollifier_gradient",
        [](const Eigen::Vector3d& ea0, const Eigen::Vector3d& ea1,
           const Eigen::Vector3d& eb0, const Eigen::Vector3d& eb1,
           const double eps_x) {
            Vector<double, 12> grad;
            edge_edge_mollifier_gradient(ea0, ea1, eb0, eb1, eps_x, grad);
            return grad;
        },
        R"ipc_Qu8mg5v7(
        Compute the gradient of the mollifier for the edge-edge distance.

        Parameters:
            - ea0: first vertex of the first edge
            - ea1: second vertex of the first edge
            - eb0: first vertex of the second edge
            - eb1: second vertex of the second edge
            - eps_x: mollifier activation threshold

        Returns:
            The gradient of the mollifier.
        )ipc_Qu8mg5v7",
        py::arg("ea0"), py::arg("ea1"), py::arg("eb0"), py::arg("eb1"),
        py::arg("eps_x"));

    m.def(
        "edge_edge_mollifier_hessian",
        [](const Eigen::Vector3d& ea0, const Eigen::Vector3d& ea1,
           const Eigen::Vector3d& eb0, const Eigen::Vector3d& eb1,
           const double eps_x) {
            Eigen::Matrix<double, 12, 12> hess;
            edge_edge_mollifier_hessian(ea0, ea1, eb0, eb1, eps_x, hess);
            return hess;
        },
        R"ipc_Qu8mg5v7(
        Compute the hessian of the mollifier for the edge-edge distance.

        Parameters:
            - ea0: first vertex of the first edge
            - ea1: second vertex of the first edge
            - eb0: first vertex of the second edge
            - eb1: second vertex of the second edge
            - eps_x: mollifier activation threshold

        Returns:
            The hessian of the mollifier.
        )ipc_Qu8mg5v7",
        py::arg("ea0"), py::arg("ea1"), py::arg("eb0"), py::arg("eb1"),
        py::arg("eps_x"));

    m.def(
        "edge_edge_mollifier_threshold",
        [](const Eigen::Vector3d& ea0_rest, const Eigen::Vector3d& ea1_rest,
           const Eigen::Vector3d& eb0_rest, const Eigen::Vector3d& eb1_rest) {
            return edge_edge_mollifier_threshold(
                ea0_rest, ea1_rest, eb0_rest, eb1_rest);
        },
        R"ipc_Qu8mg5v7(
        Compute the threshold of the mollifier edge-edge distance.

        This values is computed based on the edges at rest length.

        Parameters:
            ea0_rest: rest position of the first vertex of the first edge
            ea1_rest: rest position of the second vertex of the first edge
            eb0_rest: rest position of the first vertex of the second edge
            eb1_rest: rest position of the second vertex of the second edge

        Returns:
            Threshold for edge-edge mollification.
        )ipc_Qu8mg5v7",
        py::arg("ea0_rest"), py::arg("ea1_rest"), py::arg("eb0_rest"),
        py::arg("eb1_rest"));
}


<|start_filename|>src/utils/vertex_to_min_edge.hpp<|end_filename|>
#pragma once

#include <vector>

#include <Eigen/Core>

namespace ipc {

std::vector<size_t>
vertex_to_min_edge(size_t num_vertices, const Eigen::MatrixXi& E);

} // namespace ipc


<|start_filename|>src/broad_phase/voxel_size_heuristic.cpp<|end_filename|>
#include <ipc/broad_phase/voxel_size_heuristic.hpp>

#include <ipc/utils/logger.hpp>

namespace ipc {

double suggest_good_voxel_size(
    const Eigen::MatrixXd& V, const Eigen::MatrixXi& E, double inflation_radius)
{
    double edge_len = average_edge_length(V, V, E);
    double voxel_size = 2 * edge_len + inflation_radius;
    IPC_LOG(trace(
        "suggesting voxel size of {} (avg_edge_len={})", voxel_size, edge_len));
    return voxel_size;
}

double suggest_good_voxel_size(
    const Eigen::MatrixXd& V0,
    const Eigen::MatrixXd& V1,
    const Eigen::MatrixXi& E,
    double inflation_radius)
{
    double edge_len = average_edge_length(V0, V1, E);
    double disp_len = average_displacement_length(V1 - V0);
    double voxel_size = 2 * std::max(edge_len, disp_len) + inflation_radius;
    // double voxel_size = 2 * edge_len + inflation_radius;
    IPC_LOG(trace(
        "suggesting voxel size of {} (avg_edge_len={} avg_disp_len={})",
        voxel_size, edge_len, disp_len));
    return voxel_size;
}

/// @brief Compute the average edge length of a mesh.
double average_edge_length(
    const Eigen::MatrixXd& V0,
    const Eigen::MatrixXd& V1,
    const Eigen::MatrixXi& E)
{
    double avg = 0;
    for (unsigned i = 0; i < E.rows(); i++) {
        avg += (V0.row(E(i, 0)) - V0.row(E(i, 1))).norm();
        avg += (V1.row(E(i, 0)) - V1.row(E(i, 1))).norm();
    }
    return avg / (2 * E.rows());
}

/// @brief Compute the average displacement length.
double average_displacement_length(const Eigen::MatrixXd& U)
{
    return U.rowwise().norm().sum() / U.rows();
}

} // namespace ipc


<|start_filename|>src/distance/distance_type.hpp<|end_filename|>
#pragma once

#include <Eigen/Core>

namespace ipc {

/// @brief Closest pair between a point and edge.
enum class PointEdgeDistanceType {
    P_E0, ///< The point is closest to edge vertex zero.
    P_E1, ///< The point is closest to edge vertex one.
    P_E   ///< The point is closest to the interior of the edge.
};

/// @brief Closest pair between a point and triangle.
enum class PointTriangleDistanceType {
    P_T0, ///< The point is closest to triangle vertex zero.
    P_T1, ///< The point is closest to triangle vertex one.
    P_T2, ///< The point is closest to triangle vertex two.
    P_E0, ///< The point is closest to triangle edge zero (vertex zero to one).
    P_E1, ///< The point is closest to triangle edge one (vertex one to two).
    P_E2, ///< The point is closest to triangle edge two (vertex two to zero).
    P_T   ///< The point is closest to the interior of the triangle.
};

/// @brief Closest pair between two edges.
enum class EdgeEdgeDistanceType {
    EA0_EB0, ///< The edges are closest at vertex 0 of edge A and 0 of edge B.
    EA0_EB1, ///< The edges are closest at vertex 0 of edge A and 1 of edge B.
    EA1_EB0, ///< The edges are closest at vertex 1 of edge A and 0 of edge B.
    EA1_EB1, ///< The edges are closest at vertex 1 of edge A and 1 of edge B.
    EA_EB0, ///< The edges are closest at the interior of edge A and vertex 0 of
            ///< edge B.
    EA_EB1, ///< The edges are closest at the interior of edge A and vertex 1 of
            ///< edge B.
    EA0_EB, ///< The edges are closest at vertex 0 of edge A and the interior of
            ///< edge B.
    EA1_EB, ///< The edges are closest at vertex 1 of edge A and the interior of
            ///< edge B.
    EA_EB   ///< The edges are closest at an interior point of edge A and B.
};

/// @brief Determine the closest pair between a point and edge.
/// @param p The point.
/// @param e0 The first vertex of the edge.
/// @param e1 The second vertex of the edge.
/// @return The distance type of the point-edge pair.
template <typename DerivedP, typename DerivedE0, typename DerivedE1>
PointEdgeDistanceType point_edge_distance_type(
    const Eigen::MatrixBase<DerivedP>& p,
    const Eigen::MatrixBase<DerivedE0>& e0,
    const Eigen::MatrixBase<DerivedE1>& e1);

/// @brief Determine the closest pair between a point and triangle.
/// @param p The point.
/// @param t0 The first vertex of the triangle.
/// @param t1 The second vertex of the triangle.
/// @param t2 The third vertex of the triangle.
/// @return The distance type of the point-triangle pair.
template <
    typename DerivedP,
    typename DerivedT0,
    typename DerivedT1,
    typename DerivedT2>
PointTriangleDistanceType point_triangle_distance_type(
    const Eigen::MatrixBase<DerivedP>& p,
    const Eigen::MatrixBase<DerivedT0>& t0,
    const Eigen::MatrixBase<DerivedT1>& t1,
    const Eigen::MatrixBase<DerivedT2>& t2);

/// @brief Determine the closest pair between two edges.
/// @param ea0 The first vertex of the first edge.
/// @param ea1 The second vertex of the first edge.
/// @param eb0 The first vertex of the second edge.
/// @param eb1 The second vertex of the second edge.
/// @return The distance type of the edge-edge pair.
template <
    typename DerivedEA0,
    typename DerivedEA1,
    typename DerivedEB0,
    typename DerivedEB1>
EdgeEdgeDistanceType edge_edge_distance_type(
    const Eigen::MatrixBase<DerivedEA0>& ea0,
    const Eigen::MatrixBase<DerivedEA1>& ea1,
    const Eigen::MatrixBase<DerivedEB0>& eb0,
    const Eigen::MatrixBase<DerivedEB1>& eb1);

} // namespace ipc

#include <ipc/distance/distance_type.tpp>


<|start_filename|>cmake/recipes/tight_inclusion.cmake<|end_filename|>
if(TARGET tight_inclusion::tight_inclusion)
    return()
endif()

message(STATUS "Third-party: creating target 'tight_inclusion::tight_inclusion'")

option(TIGHT_INCLUSION_WITH_NO_ZERO_TOI "Enable refinement if CCD produces a zero ToI" ON)

if(EXISTS "${IPC_TOOLKIT_TIGHT_INCLUSION_PATH}")
    message(STATUS "Using Tight Inclusion found at: ${IPC_TOOLKIT_TIGHT_INCLUSION_PATH}")
    add_subdirectory("${IPC_TOOLKIT_TIGHT_INCLUSION_PATH}" "${PROJECT_BINARY_DIR}/tight-inclusion")
else()
    include(FetchContent)
    FetchContent_Declare(
        tight_inclusion
        GIT_REPOSITORY https://github.com/Continuous-Collision-Detection/Tight-Inclusion.git
        GIT_TAG 1ace9ff8a5d16abd19104225700ce435b191554a
        GIT_SHALLOW FALSE
    )
    FetchContent_MakeAvailable(tight_inclusion)
endif()

add_library(tight_inclusion::tight_inclusion ALIAS tight_inclusion)


<|start_filename|>src/utils/local_to_global.hpp<|end_filename|>
#pragma once

#include <vector>

#include <Eigen/Core>
#include <Eigen/Sparse>

namespace ipc {

template <typename DerivedLocalGrad, typename DerivedGrad>
void local_gradient_to_global_gradient(
    const Eigen::MatrixBase<DerivedLocalGrad>& local_grad,
    const std::vector<long>& ids,
    int dim,
    Eigen::PlainObjectBase<DerivedGrad>& grad)
{
    for (int i = 0; i < ids.size(); i++) {
        grad.segment(dim * ids[i], dim) += local_grad.segment(dim * i, dim);
    }
}

template <typename Derived>
void local_hessian_to_global_triplets(
    const Eigen::MatrixBase<Derived>& local_hessian,
    const std::vector<long>& ids,
    int dim,
    std::vector<Eigen::Triplet<double>>& triplets)
{
    for (int i = 0; i < ids.size(); i++) {
        for (int j = 0; j < ids.size(); j++) {
            for (int k = 0; k < dim; k++) {
                for (int l = 0; l < dim; l++) {
                    triplets.emplace_back(
                        dim * ids[i] + k, dim * ids[j] + l,
                        local_hessian(dim * i + k, dim * j + l));
                }
            }
        }
    }
}

} // namespace ipc


<|start_filename|>src/broad_phase/voxel_size_heuristic.hpp<|end_filename|>
#pragma once

#include <Eigen/Core>

namespace ipc {

double suggest_good_voxel_size(
    const Eigen::MatrixXd& V,
    const Eigen::MatrixXi& E,
    double inflation_radius = 0);

double suggest_good_voxel_size(
    const Eigen::MatrixXd& V0,
    const Eigen::MatrixXd& V1,
    const Eigen::MatrixXi& E,
    double inflation_radius = 0);

/// @brief Compute the average edge length of a mesh.
double average_edge_length(
    const Eigen::MatrixXd& V0,
    const Eigen::MatrixXd& V1,
    const Eigen::MatrixXi& E);

/// @brief Compute the average displacement length.
double average_displacement_length(const Eigen::MatrixXd& U);

} // namespace ipc


<|start_filename|>src/broad_phase/broad_phase.hpp<|end_filename|>
#pragma once

#include <Eigen/Core>

#include <ipc/broad_phase/collision_candidate.hpp>

namespace ipc {

enum class BroadPhaseMethod {
    BRUTE_FORCE,
    HASH_GRID,
    SPATIAL_HASH,
};

/// @brief Construct a set of discrete collision detection candidates.
///
/// All vertices in V will be considered for collisions, so V should be only the
/// surface vertices. The edges and face should be only for the surface
/// elements.
///
/// @param[in] V Vertex positions at start as rows of a matrix.
/// @param[in] E Edges as rows of indicies into V.
/// @param[in] F Triangular faces as rows of indicies into V.
/// @param[out] canidates The constructed candidate set as output.
/// @param[in] inflation_radius Amount to inflate the bounding boxes.
/// @param[in] method Broad phase method to use.
/// @param[in] can_collide
///     A function that takes two vertex IDs (row numbers in F) and returns true
///     if the vertices (and faces or edges containing the vertices) can
///     collide. By default all primitives can collide with all other
///     primitives.
void construct_collision_candidates(
    const Eigen::MatrixXd& V,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    Candidates& candidates,
    double inflation_radius = 0,
    const BroadPhaseMethod& method = BroadPhaseMethod::HASH_GRID,
    const std::function<bool(size_t, size_t)>& can_collide =
        [](size_t, size_t) { return true; });

/// @brief Construct a set of discrete collision detection candidates.
///
/// V can either be the surface vertices or the entire mesh vertices. The edges
/// and face should be only for the surface elements.
///
/// @param[in] V Vertex positions at start as rows of a matrix.
/// @param[in] codim_V Vertex indices of codimensional points. Pass
///     Eigen::VectorXi() to ignore all codimensional vertices (vertices not
///     connected to an edge).
/// @param[in] E Edges as rows of indicies into V.
/// @param[in] F Triangular faces as rows of indicies into V.
/// @param[out] canidates The constructed candidate set as output.
/// @param[in] inflation_radius Amount to inflate the bounding boxes.
/// @param[in] method Broad phase method to use.
/// @param[in] can_collide
///     A function that takes two vertex IDs (row numbers in F) and returns true
///     if the vertices (and faces or edges containing the vertices) can
///     collide. By default all primitives can collide with all other
///     primitives.
void construct_collision_candidates(
    const Eigen::MatrixXd& V,
    const Eigen::VectorXi& codim_V,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    Candidates& candidates,
    double inflation_radius = 0,
    const BroadPhaseMethod& method = BroadPhaseMethod::HASH_GRID,
    const std::function<bool(size_t, size_t)>& can_collide =
        [](size_t, size_t) { return true; });

/// @brief Construct a set of continous collision detection candidates.
///
/// All vertices in V will be considered for collisions, so V should be only the
/// surface vertices. The edges and face should be only for the surface
/// elements.
///
/// @note Assumes the trajectory is linear.
///
/// @param[in] V0 Vertex positions at start as rows of a matrix.
/// @param[in] V1 Vertex positions at end as rows of a matrix.
/// @param[in] E Edges as rows of indicies into V.
/// @param[in] F Triangular faces as rows of indicies into V.
/// @param[out] canidates The constructed candidate set as output.
/// @param[in] inflation_radius Amount to inflate the bounding boxes.
/// @param[in] method Broad phase method to use.
/// @param[in] can_collide
///     A function that takes two vertex IDs (row numbers in F) and returns true
///     if the vertices (and faces or edges containing the vertices) can
///     collide. By default all primitives can collide with all other
///     primitives.
void construct_collision_candidates(
    const Eigen::MatrixXd& V0,
    const Eigen::MatrixXd& V1,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    Candidates& candidates,
    double inflation_radius = 0,
    const BroadPhaseMethod& method = BroadPhaseMethod::HASH_GRID,
    const std::function<bool(size_t, size_t)>& can_collide =
        [](size_t, size_t) { return true; });

/// @brief Construct a set of continous collision detection candidates.
///
/// V can either be the surface vertices or the entire mesh vertices. The edges
/// and face should be only for the surface elements.
///
/// @note Assumes the trajectory is linear.
///
/// @param[in] V0 Vertex positions at start as rows of a matrix.
/// @param[in] V1 Vertex positions at end as rows of a matrix.
/// @param[in] codim_V Vertex indices of codimensional points. Pass
///     Eigen::VectorXi() to ignore all codimensional vertices (vertices not
///     connected to an edge).
/// @param[in] E Edges as rows of indicies into V.
/// @param[in] F Triangular faces as rows of indicies into V.
/// @param[out] canidates The constructed candidate set as output.
/// @param[in] inflation_radius Amount to inflate the bounding boxes.
/// @param[in] method Broad phase method to use.
/// @param[in] can_collide
///     A function that takes two vertex IDs (row numbers in F) and returns true
///     if the vertices (and faces or edges containing the vertices) can
///     collide. By default all primitives can collide with all other
///     primitives.
void construct_collision_candidates(
    const Eigen::MatrixXd& V0,
    const Eigen::MatrixXd& V1,
    const Eigen::VectorXi& codim_V,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    Candidates& candidates,
    double inflation_radius = 0,
    const BroadPhaseMethod& method = BroadPhaseMethod::HASH_GRID,
    const std::function<bool(size_t, size_t)>& can_collide =
        [](size_t, size_t) { return true; });

} // namespace ipc


<|start_filename|>python/src/broad_phase/broad_phase.cpp<|end_filename|>
#include <pybind11/pybind11.h>

#include <ipc/broad_phase/broad_phase.hpp>

namespace py = pybind11;
using namespace ipc;

void define_broad_phase_functions(py::module_& m)
{
    py::enum_<BroadPhaseMethod>(m, "BroadPhaseMethod")
        .value("BRUTE_FORCE", BroadPhaseMethod::BRUTE_FORCE)
        .value("HASH_GRID", BroadPhaseMethod::HASH_GRID)
        .value("SPATIAL_HASH", BroadPhaseMethod::SPATIAL_HASH)
        .export_values();
}


<|start_filename|>src/collision_constraint.cpp<|end_filename|>
#include <ipc/collision_constraint.hpp>

#include <stdexcept> // std::out_of_range

#include <ipc/barrier/barrier.hpp>
#include <ipc/distance/edge_edge.hpp>
#include <ipc/distance/edge_edge_mollifier.hpp>
#include <ipc/distance/point_edge.hpp>
#include <ipc/distance/point_triangle.hpp>

namespace ipc {

double CollisionConstraint::compute_potential(
    const Eigen::MatrixXd& V,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    const double dhat) const
{
    const double dhat_squared = dhat * dhat;
    const double distance = compute_distance(V, E, F); // Squared distance
    return barrier(
        distance - minimum_distance * minimum_distance,
        2 * minimum_distance * dhat + dhat_squared);
}

VectorMax12d CollisionConstraint::compute_potential_gradient(
    const Eigen::MatrixXd& V,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    const double dhat) const
{
    const double dhat_squared = dhat * dhat;

    // ∇b(d(x)) = b'(d(x)) * ∇d(x)
    const double distance = compute_distance(V, E, F);
    const VectorMax12d distance_grad = compute_distance_gradient(V, E, F);

    const double grad_b = barrier_gradient(
        distance - minimum_distance * minimum_distance,
        2 * minimum_distance * dhat + dhat_squared);
    return grad_b * distance_grad;
}

MatrixMax12d CollisionConstraint::compute_potential_hessian(
    const Eigen::MatrixXd& V,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    const double dhat,
    const bool project_hessian_to_psd) const
{
    const double dhat_squared = dhat * dhat;
    const double min_dist_squrared = minimum_distance * minimum_distance;

    // ∇²[b(d(x))] = ∇(b'(d(x)) * ∇d(x))
    //             = b"(d(x)) * ∇d(x) * ∇d(x)ᵀ + b'(d(x)) * ∇²d(x)

    const double distance = compute_distance(V, E, F);
    const VectorMax12d distance_grad = compute_distance_gradient(V, E, F);
    const MatrixMax12d distance_hess = compute_distance_hessian(V, E, F);

    const double grad_b = barrier_gradient(
        distance - min_dist_squrared,
        2 * minimum_distance * dhat + dhat_squared);
    const double hess_b = barrier_hessian(
        distance - min_dist_squrared,
        2 * minimum_distance * dhat + dhat_squared);

    // b"(x) ≥ 0 ⟹ b"(x) * ∇d(x) * ∇d(x)ᵀ is PSD
    assert(hess_b >= 0);
    return hess_b * distance_grad * distance_grad.transpose()
        + (project_hessian_to_psd
               ? project_to_psd((grad_b * distance_hess).eval())
               : (grad_b * distance_hess));
}

///////////////////////////////////////////////////////////////////////////////

VertexVertexConstraint::VertexVertexConstraint(
    long vertex0_index, long vertex1_index)
    : VertexVertexCandidate(vertex0_index, vertex1_index)
{
}

VertexVertexConstraint::VertexVertexConstraint(
    const VertexVertexCandidate& candidate)
    : VertexVertexCandidate(candidate)
{
}

double VertexVertexConstraint::compute_distance(
    const Eigen::MatrixXd& V,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F) const
{
    return point_point_distance(V.row(vertex0_index), V.row(vertex1_index));
}

VectorMax12d VertexVertexConstraint::compute_distance_gradient(
    const Eigen::MatrixXd& V,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F) const
{
    VectorMax6d distance_grad;
    point_point_distance_gradient(
        V.row(vertex0_index), V.row(vertex1_index), distance_grad);
    return distance_grad;
}

MatrixMax12d VertexVertexConstraint::compute_distance_hessian(
    const Eigen::MatrixXd& V,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F) const
{
    MatrixMax6d distance_hess;
    point_point_distance_hessian(
        V.row(vertex0_index), V.row(vertex1_index), distance_hess);
    return distance_hess;
}

double VertexVertexConstraint::compute_potential(
    const Eigen::MatrixXd& V,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    const double dhat) const
{
    return multiplicity * CollisionConstraint::compute_potential(V, E, F, dhat);
}

VectorMax12d VertexVertexConstraint::compute_potential_gradient(
    const Eigen::MatrixXd& V,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    const double dhat) const
{
    // ∇[m * b(d(x))] = m * b'(d(x)) * ∇d(x)
    return multiplicity
        * CollisionConstraint::compute_potential_gradient(V, E, F, dhat);
}

MatrixMax12d VertexVertexConstraint::compute_potential_hessian(
    const Eigen::MatrixXd& V,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    const double dhat,
    const bool project_hessian_to_psd) const
{
    // ∇²[m * b(d(x))] = m * ∇(b'(d(x)) * ∇d(x))
    //                 = m * [b"(d(x)) * ∇d(x) * ∇d(x)ᵀ + b'(d(x)) * ∇²d(x)]
    return multiplicity
        * CollisionConstraint::compute_potential_hessian(
               V, E, F, dhat, project_hessian_to_psd);
}

///////////////////////////////////////////////////////////////////////////////

EdgeVertexConstraint::EdgeVertexConstraint(long edge_index, long vertex_index)
    : EdgeVertexCandidate(edge_index, vertex_index)
{
}

EdgeVertexConstraint::EdgeVertexConstraint(const EdgeVertexCandidate& candidate)
    : EdgeVertexCandidate(candidate)
{
}

double EdgeVertexConstraint::compute_distance(
    const Eigen::MatrixXd& V,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F) const
{
    // The distance type is known because of construct_constraint_set()
    return point_edge_distance(
        V.row(vertex_index), V.row(E(edge_index, 0)), V.row(E(edge_index, 1)),
        PointEdgeDistanceType::P_E);
}

VectorMax12d EdgeVertexConstraint::compute_distance_gradient(
    const Eigen::MatrixXd& V,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F) const
{
    VectorMax9d distance_grad;
    point_edge_distance_gradient(
        V.row(vertex_index), V.row(E(edge_index, 0)), V.row(E(edge_index, 1)),
        PointEdgeDistanceType::P_E, distance_grad);
    return distance_grad;
}

MatrixMax12d EdgeVertexConstraint::compute_distance_hessian(
    const Eigen::MatrixXd& V,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F) const
{
    MatrixMax9d distance_hess;
    point_edge_distance_hessian(
        V.row(vertex_index), V.row(E(edge_index, 0)), V.row(E(edge_index, 1)),
        PointEdgeDistanceType::P_E, distance_hess);
    return distance_hess;
}

double EdgeVertexConstraint::compute_potential(
    const Eigen::MatrixXd& V,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    const double dhat) const
{
    return multiplicity * CollisionConstraint::compute_potential(V, E, F, dhat);
}

VectorMax12d EdgeVertexConstraint::compute_potential_gradient(
    const Eigen::MatrixXd& V,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    const double dhat) const
{
    // ∇[m * b(d(x))] = m * b'(d(x)) * ∇d(x)
    return multiplicity
        * CollisionConstraint::compute_potential_gradient(V, E, F, dhat);
}

MatrixMax12d EdgeVertexConstraint::compute_potential_hessian(
    const Eigen::MatrixXd& V,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    const double dhat,
    const bool project_hessian_to_psd) const
{
    // ∇²[m * b(d(x))] = m * ∇(b'(d(x)) * ∇d(x))
    //                 = m * [b"(d(x)) * ∇d(x) * ∇d(x)ᵀ + b'(d(x)) * ∇²d(x)]
    return multiplicity
        * CollisionConstraint::compute_potential_hessian(
               V, E, F, dhat, project_hessian_to_psd);
}

///////////////////////////////////////////////////////////////////////////////

EdgeEdgeConstraint::EdgeEdgeConstraint(
    long edge0_index, long edge1_index, double eps_x)
    : EdgeEdgeCandidate(edge0_index, edge1_index)
    , eps_x(eps_x)
{
}

EdgeEdgeConstraint::EdgeEdgeConstraint(
    const EdgeEdgeCandidate& candidate, double eps_x)
    : EdgeEdgeCandidate(candidate)
    , eps_x(eps_x)
{
}

double EdgeEdgeConstraint::compute_distance(
    const Eigen::MatrixXd& V,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F) const
{
    // The distance type is unknown because of mollified PP and PE
    // constraints where also added as EE constraints.
    return edge_edge_distance(
        V.row(E(edge0_index, 0)), V.row(E(edge0_index, 1)),
        V.row(E(edge1_index, 0)), V.row(E(edge1_index, 1)));
}

VectorMax12d EdgeEdgeConstraint::compute_distance_gradient(
    const Eigen::MatrixXd& V,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F) const
{
    VectorMax9d distance_grad;
    edge_edge_distance_gradient(
        V.row(E(edge0_index, 0)), V.row(E(edge0_index, 1)),
        V.row(E(edge1_index, 0)), V.row(E(edge1_index, 1)), distance_grad);
    return distance_grad;
}

MatrixMax12d EdgeEdgeConstraint::compute_distance_hessian(
    const Eigen::MatrixXd& V,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F) const
{
    MatrixMax12d distance_hess;
    edge_edge_distance_hessian(
        V.row(E(edge0_index, 0)), V.row(E(edge0_index, 1)),
        V.row(E(edge1_index, 0)), V.row(E(edge1_index, 1)), distance_hess);
    return distance_hess;
}

double EdgeEdgeConstraint::compute_potential(
    const Eigen::MatrixXd& V,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    const double dhat) const
{
    return edge_edge_mollifier(
               V.row(E(edge0_index, 0)), V.row(E(edge0_index, 1)),
               V.row(E(edge1_index, 0)), V.row(E(edge1_index, 1)), eps_x)
        * CollisionConstraint::compute_potential(V, E, F, dhat);
}

VectorMax12d EdgeEdgeConstraint::compute_potential_gradient(
    const Eigen::MatrixXd& V,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    const double dhat) const
{
    const double dhat_squared = dhat * dhat;

    // ∇[m(x) * b(d(x))] = (∇m(x)) * b(d(x)) + m(x) * b'(d(x)) * ∇d(x)
    const auto& ea0 = V.row(E(edge0_index, 0));
    const auto& ea1 = V.row(E(edge0_index, 1));
    const auto& eb0 = V.row(E(edge1_index, 0));
    const auto& eb1 = V.row(E(edge1_index, 1));

    // The distance type is unknown because of mollified PP and PE
    // constraints where also added as EE constraints.
    const EdgeEdgeDistanceType dtype =
        edge_edge_distance_type(ea0, ea1, eb0, eb1);
    const double distance = edge_edge_distance(ea0, ea1, eb0, eb1, dtype);
    VectorMax12d distance_grad;
    edge_edge_distance_gradient(ea0, ea1, eb0, eb1, dtype, distance_grad);

    // m(x)
    const double mollifier = edge_edge_mollifier(ea0, ea1, eb0, eb1, eps_x);
    // ∇m(x)
    VectorMax12d mollifier_grad;
    edge_edge_mollifier_gradient(ea0, ea1, eb0, eb1, eps_x, mollifier_grad);

    // b(d(x))
    const double b = barrier(
        distance - minimum_distance * minimum_distance,
        2 * minimum_distance * dhat + dhat_squared);
    // b'(d(x))
    const double grad_b = barrier_gradient(
        distance - minimum_distance * minimum_distance,
        2 * minimum_distance * dhat + dhat_squared);

    return mollifier_grad * b + mollifier * grad_b * distance_grad;
}

MatrixMax12d EdgeEdgeConstraint::compute_potential_hessian(
    const Eigen::MatrixXd& V,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    const double dhat,
    const bool project_hessian_to_psd) const
{
    const double dhat_squared = dhat * dhat;
    const double min_dist_squrared = minimum_distance * minimum_distance;

    // ∇²[m(x) * b(d(x))] = ∇[∇m(x) * b(d(x)) + m(x) * b'(d(x)) * ∇d(x)]
    //                    = ∇²m(x) * b(d(x)) + b'(d(x)) * ∇d(x) * ∇m(x)ᵀ
    //                      + ∇m(x) * b'(d(x)) * ∇d(x))ᵀ
    //                      + m(x) * b"(d(x)) * ∇d(x) * ∇d(x)ᵀ
    //                      + m(x) * b'(d(x)) * ∇²d(x)
    const auto& ea0 = V.row(E(edge0_index, 0));
    const auto& ea1 = V.row(E(edge0_index, 1));
    const auto& eb0 = V.row(E(edge1_index, 0));
    const auto& eb1 = V.row(E(edge1_index, 1));

    // Compute distance derivatives
    // The distance type is unknown because of mollified PP and PE
    // constraints where also added as EE constraints.
    const EdgeEdgeDistanceType dtype =
        edge_edge_distance_type(ea0, ea1, eb0, eb1);
    const double distance = edge_edge_distance(ea0, ea1, eb0, eb1, dtype);
    VectorMax12d distance_grad;
    edge_edge_distance_gradient(ea0, ea1, eb0, eb1, dtype, distance_grad);
    MatrixMax12d distance_hess;
    edge_edge_distance_hessian(ea0, ea1, eb0, eb1, dtype, distance_hess);

    // Compute mollifier derivatives
    const double mollifier = edge_edge_mollifier(ea0, ea1, eb0, eb1, eps_x);
    VectorMax12d mollifier_grad;
    edge_edge_mollifier_gradient(ea0, ea1, eb0, eb1, eps_x, mollifier_grad);
    MatrixMax12d mollifier_hess;
    edge_edge_mollifier_hessian(ea0, ea1, eb0, eb1, eps_x, mollifier_hess);

    // Compute barrier derivatives
    const double b = barrier(
        distance - min_dist_squrared,
        2 * minimum_distance * dhat + dhat_squared);
    const double grad_b = barrier_gradient(
        distance - min_dist_squrared,
        2 * minimum_distance * dhat + dhat_squared);
    const double hess_b = barrier_hessian(
        distance - min_dist_squrared,
        2 * minimum_distance * dhat + dhat_squared);

    MatrixMax12d hess = mollifier_hess * b
        + grad_b
            * (distance_grad * mollifier_grad.transpose()
               + mollifier_grad * distance_grad.transpose())
        + mollifier
            * (hess_b * distance_grad * distance_grad.transpose()
               + grad_b * distance_hess);

    if (project_hessian_to_psd) {
        hess = project_to_psd(hess);
    }

    return hess;
}

///////////////////////////////////////////////////////////////////////////////

FaceVertexConstraint::FaceVertexConstraint(long face_index, long vertex_index)
    : FaceVertexCandidate(face_index, vertex_index)
{
}

FaceVertexConstraint::FaceVertexConstraint(const FaceVertexCandidate& candidate)
    : FaceVertexCandidate(candidate)
{
}

double FaceVertexConstraint::compute_distance(
    const Eigen::MatrixXd& V,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F) const
{
    // The distance type is known because of construct_constraint_set()
    return point_triangle_distance(
        V.row(vertex_index), V.row(F(face_index, 0)), V.row(F(face_index, 1)),
        V.row(F(face_index, 2)), PointTriangleDistanceType::P_T);
}

VectorMax12d FaceVertexConstraint::compute_distance_gradient(
    const Eigen::MatrixXd& V,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F) const
{
    VectorMax12d distance_grad;
    point_triangle_distance_gradient(
        V.row(vertex_index), V.row(F(face_index, 0)), V.row(F(face_index, 1)),
        V.row(F(face_index, 2)), PointTriangleDistanceType::P_T, distance_grad);
    return distance_grad;
}

MatrixMax12d FaceVertexConstraint::compute_distance_hessian(
    const Eigen::MatrixXd& V,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F) const
{
    MatrixMax12d distance_hess;
    point_triangle_distance_hessian(
        V.row(vertex_index), V.row(F(face_index, 0)), V.row(F(face_index, 1)),
        V.row(F(face_index, 2)), PointTriangleDistanceType::P_T, distance_hess);
    return distance_hess;
}

///////////////////////////////////////////////////////////////////////////////

PlaneVertexConstraint::PlaneVertexConstraint(
    const VectorMax3d& plane_origin,
    const VectorMax3d& plane_normal,
    const long vertex_index)
    : plane_origin(plane_origin)
    , plane_normal(plane_normal)
    , vertex_index(vertex_index)
{
}

double PlaneVertexConstraint::compute_distance(
    const Eigen::MatrixXd& V,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F) const
{
    return point_plane_distance(
        V.row(vertex_index), plane_origin, plane_normal);
}

VectorMax12d PlaneVertexConstraint::compute_distance_gradient(
    const Eigen::MatrixXd& V,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F) const
{
    VectorMax3d distance_grad;
    point_plane_distance_gradient(
        V.row(vertex_index), plane_origin, plane_normal, distance_grad);
    return distance_grad;
}

MatrixMax12d PlaneVertexConstraint::compute_distance_hessian(
    const Eigen::MatrixXd& V,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F) const
{
    MatrixMax3d distance_hess;
    point_plane_distance_hessian(
        V.row(vertex_index), plane_origin, plane_normal, distance_hess);
    return distance_hess;
}

///////////////////////////////////////////////////////////////////////////////

size_t Constraints::size() const
{
    return vv_constraints.size() + ev_constraints.size() + ee_constraints.size()
        + fv_constraints.size() + pv_constraints.size();
}

size_t Constraints::num_constraints() const
{
    size_t num_constraints = 0;
    for (const auto& vv_constraint : vv_constraints) {
        num_constraints += vv_constraint.multiplicity;
    }
    for (const auto& ev_constraint : ev_constraints) {
        num_constraints += ev_constraint.multiplicity;
    }
    num_constraints +=
        ee_constraints.size() + fv_constraints.size() + pv_constraints.size();
    return num_constraints;
}

bool Constraints::empty() const
{
    return vv_constraints.empty() && ev_constraints.empty()
        && ee_constraints.empty() && fv_constraints.empty()
        && pv_constraints.empty();
}

void Constraints::clear()
{
    vv_constraints.clear();
    ev_constraints.clear();
    ee_constraints.clear();
    fv_constraints.clear();
    pv_constraints.clear();
}

CollisionConstraint& Constraints::operator[](size_t idx)
{
    if (idx < 0) {
        throw std::out_of_range("Constraint index is out of range!");
    }
    if (idx < vv_constraints.size()) {
        return vv_constraints[idx];
    }
    idx -= vv_constraints.size();
    if (idx < ev_constraints.size()) {
        return ev_constraints[idx];
    }
    idx -= ev_constraints.size();
    if (idx < ee_constraints.size()) {
        return ee_constraints[idx];
    }
    idx -= ee_constraints.size();
    if (idx < fv_constraints.size()) {
        return fv_constraints[idx];
    }
    idx -= fv_constraints.size();
    if (idx < pv_constraints.size()) {
        return pv_constraints[idx];
    }
    throw std::out_of_range("Constraint index is out of range!");
}

const CollisionConstraint& Constraints::operator[](size_t idx) const
{
    if (idx < 0) {
        throw std::out_of_range("Constraint index is out of range!");
    }
    if (idx < vv_constraints.size()) {
        return vv_constraints[idx];
    }
    idx -= vv_constraints.size();
    if (idx < ev_constraints.size()) {
        return ev_constraints[idx];
    }
    idx -= ev_constraints.size();
    if (idx < ee_constraints.size()) {
        return ee_constraints[idx];
    }
    idx -= ee_constraints.size();
    if (idx < fv_constraints.size()) {
        return fv_constraints[idx];
    }
    idx -= fv_constraints.size();
    if (idx < pv_constraints.size()) {
        return pv_constraints[idx];
    }
    throw std::out_of_range("Constraint index is out of range!");
}

} // namespace ipc


<|start_filename|>src/utils/faces_to_edges.hpp<|end_filename|>
#pragma once

#include <Eigen/Core>

namespace ipc {

/// Construct a matrix that maps from the faces' edges to rows in the edges
/// matrix
Eigen::MatrixXi
faces_to_edges(const Eigen::MatrixXi& F, const Eigen::MatrixXi& E);

} // namespace ipc


<|start_filename|>cmake/ipc_toolkit/ipc_toolkit_copy_headers.cmake<|end_filename|>
# Copy header files into the build directory
function(ipc_toolkit_copy_headers)
  foreach(filepath IN ITEMS ${ARGN})
    file(RELATIVE_PATH filename "${IPC_TOOLKIT_SOURCE_DIR}" "${filepath}")
    if(${filename} MATCHES ".*\.(hpp|h|ipp|tpp)$")
      configure_file(${filepath} ${PROJECT_BINARY_DIR}/include/ipc/${filename})
    endif()
  endforeach()
endfunction()


<|start_filename|>tests/test_has_intersections.cpp<|end_filename|>
#include <catch2/catch.hpp>

#include <Eigen/Geometry>

#include <tbb/parallel_sort.h>
#include <tbb/global_control.h>
#include <tbb/task_scheduler_init.h>

#include <igl/predicates/segment_segment_intersect.h>

#include <ipc/ipc.hpp>
#include <ipc/broad_phase/brute_force.hpp>
#include <ipc/broad_phase/collision_candidate.hpp>
#include <ipc/broad_phase/hash_grid.hpp>
#include <ipc/utils/intersection.hpp>

#include "test_utils.hpp"

using namespace ipc;

bool combine_meshes(
    const std::string& mesh1_name,
    const std::string& mesh2_name,
    const Eigen::Matrix3d& R1,
    const Eigen::Matrix3d& R2,
    int dim,
    Eigen::MatrixXd& V,
    Eigen::MatrixXi& E,
    Eigen::MatrixXi& F)
{
    Eigen::MatrixXd V1, V2;
    Eigen::MatrixXi E1, E2, F1, F2;

    bool success =
        load_mesh(mesh1_name, V1, E1, F1) && load_mesh(mesh2_name, V2, E2, F2);
    if (!success) {
        return false;
    }

    V1 = V1 * R1.transpose(); // (RVᵀ)ᵀ = VRᵀ
    V2 = V2 * R2.transpose();

    REQUIRE(dim <= V1.cols());
    REQUIRE(dim <= V2.cols());
    V = Eigen::MatrixXd(V1.rows() + V2.rows(), dim);
    V.topRows(V1.rows()) = V1.leftCols(dim);
    V.bottomRows(V2.rows()) = V2.leftCols(dim);

    REQUIRE(E1.cols() == E2.cols());
    E = Eigen::MatrixXi(E1.rows() + E2.rows(), E1.cols());
    E.topRows(E1.rows()) = E1;
    E.bottomRows(E2.rows()) = E2;
    E.bottomRows(E2.rows()).array() += V1.rows();

    REQUIRE(F1.cols() == F2.cols());
    F = Eigen::MatrixXi(F1.rows() + F2.rows(), F1.cols());
    F.topRows(F1.rows()) = F1;
    F.bottomRows(F2.rows()) = F2;
    F.bottomRows(F2.rows()).array() += V1.rows();

    return true;
}

TEST_CASE("Test HashGrid EF vs brute-force", "[intersection][brute_force]")
{
#ifdef NDEBUG
    std::string mesh1_name = GENERATE("cube.obj", "bunny.obj");
    std::string mesh2_name = GENERATE("cube.obj", "bunny.obj");
#else
    std::string mesh1_name = "cube.obj";
    std::string mesh2_name = "cube.obj";
#endif
    int dim = GENERATE(2, 3);
    bool perform_aabb_check = true;

    Eigen::Matrix3d R1 = GENERATE(take(4, RotationGenerator::create()));
    Eigen::Matrix3d R2 = GENERATE(take(4, RotationGenerator::create()));

    Eigen::MatrixXd V;
    Eigen::MatrixXi E, F;
    bool success = combine_meshes(mesh1_name, mesh2_name, R1, R2, dim, V, E, F);
    REQUIRE(success);

    HashGrid hash_grid;
    hash_grid.resize(V, E);
    hash_grid.addEdges(V, E);
    if (V.cols() == 3) {
        // These are not needed for 2D
        hash_grid.addFaces(V, F);
    }

    if (V.cols() == 2) { // Need to check segment-segment intersections in 2D
        std::vector<EdgeEdgeCandidate> hg_ee_candidates;
        hash_grid.getEdgeEdgePairs(E, hg_ee_candidates);

        std::vector<EdgeEdgeCandidate> bf_ee_candidates;
        detect_edge_edge_collision_candidates_brute_force(
            V, E, bf_ee_candidates, perform_aabb_check);

        REQUIRE(hg_ee_candidates.size() <= bf_ee_candidates.size());

        tbb::parallel_sort(hg_ee_candidates.begin(), hg_ee_candidates.end());
        tbb::parallel_sort(bf_ee_candidates.begin(), bf_ee_candidates.end());

        // narrow-phase using igl
        igl::predicates::exactinit();
        int hg_ci = 0;
        for (int bf_ci = 0; bf_ci < bf_ee_candidates.size(); bf_ci++) {
            if (hg_ee_candidates.size() <= hg_ci
                || bf_ee_candidates[bf_ci] != hg_ee_candidates[hg_ci]) {
                long eai = bf_ee_candidates[bf_ci].edge0_index;
                long ebi = bf_ee_candidates[bf_ci].edge1_index;
                bool intersects = igl::predicates::segment_segment_intersect(
                    V.row(E(eai, 0)).head<2>(), V.row(E(eai, 1)).head<2>(),
                    V.row(E(ebi, 0)).head<2>(), V.row(E(ebi, 1)).head<2>());

                CHECK(!intersects); // Check for FN

            } else {
                hg_ci++;
            }
        }
        CHECK(hg_ci >= hg_ee_candidates.size());
    } else { // Need to check segment-triangle intersections in 3D
        assert(V.cols() == 3);

        std::vector<EdgeFaceCandidate> hg_ef_candidates;
        hash_grid.getEdgeFacePairs(E, F, hg_ef_candidates);

        std::vector<EdgeFaceCandidate> bf_ef_candidates;
        detect_edge_face_collision_candidates_brute_force(
            V, E, F, bf_ef_candidates, perform_aabb_check);

        REQUIRE(hg_ef_candidates.size() <= bf_ef_candidates.size());

        tbb::parallel_sort(hg_ef_candidates.begin(), hg_ef_candidates.end());
        tbb::parallel_sort(bf_ef_candidates.begin(), bf_ef_candidates.end());

        // narrow-phase using igl
        int hg_ci = 0;
        for (int bf_ci = 0; bf_ci < bf_ef_candidates.size(); bf_ci++) {
            if (hg_ef_candidates.size() <= hg_ci
                || bf_ef_candidates[bf_ci] != hg_ef_candidates[hg_ci]) {
                long ei = bf_ef_candidates[bf_ci].edge_index;
                long fi = bf_ef_candidates[bf_ci].face_index;
                bool intersects = is_edge_intersecting_triangle(
                    V.row(E(ei, 0)), V.row(E(ei, 1)), V.row(F(fi, 0)),
                    V.row(F(fi, 1)), V.row(F(fi, 2)));

                CHECK(!intersects); // Check for FN
            } else {
                hg_ci++;
            }
        }
        CHECK(hg_ci >= hg_ef_candidates.size());
    }

    CAPTURE(mesh1_name, mesh2_name, R1, R2);
}

TEST_CASE("Test has_intersections()", "[intersection][thisone]")
{
    std::string mesh1_name = GENERATE("cube.obj", "bunny.obj");
    std::string mesh2_name = GENERATE("cube.obj", "bunny.obj");
    int dim = GENERATE(2, 3);

#ifdef NDEBUG
    Eigen::Matrix3d R1 = GENERATE(take(4, RotationGenerator::create()));
    Eigen::Matrix3d R2 = GENERATE(take(4, RotationGenerator::create()));
#else
    Eigen::Matrix3d R1 = GENERATE(take(2, RotationGenerator::create()));
    Eigen::Matrix3d R2 = GENERATE(take(2, RotationGenerator::create()));
#endif

    Eigen::MatrixXd V;
    Eigen::MatrixXi E, F;
    bool success = combine_meshes(mesh1_name, mesh2_name, R1, R2, dim, V, E, F);
    REQUIRE(success);

    CAPTURE(mesh1_name, mesh2_name, R1, R2);
    CHECK(has_intersections(V, E, F));
}


<|start_filename|>src/ccd/inexact_point_edge.cpp<|end_filename|>
#include <ipc/ccd/inexact_point_edge.hpp>

#include <ipc/distance/distance_type.hpp>

namespace ipc {

bool inexact_point_edge_ccd_2D(
    const Eigen::Vector2d& p_t0,
    const Eigen::Vector2d& e0_t0,
    const Eigen::Vector2d& e1_t0,
    const Eigen::Vector2d& p_t1,
    const Eigen::Vector2d& e0_t1,
    const Eigen::Vector2d& e1_t1,
    double& toi,
    const double conservative_rescaling)
{
    Eigen::Vector2d d0 = p_t1 - p_t0, d1 = e0_t1 - e0_t0, d2 = e1_t1 - e1_t0;

    double a = d0[0] * (d2[1] - d1[1]) + d0[1] * (d1[0] - d2[0]) + d2[0] * d1[1]
        - d2[1] * d1[0];
    double b = p_t0[0] * (d2[1] - d1[1]) + d0[0] * (e1_t0[1] - e0_t0[1])
        + d0[1] * (e0_t0[0] - e1_t0[0]) + p_t0[1] * (d1[0] - d2[0])
        + d1[1] * e1_t0[0] + d2[0] * e0_t0[1] - d1[0] * e1_t0[1]
        - d2[1] * e0_t0[0];
    double c = p_t0[0] * (e1_t0[1] - e0_t0[1]) + p_t0[1] * (e0_t0[0] - e1_t0[0])
        + e1_t0[0] * e0_t0[1] - e1_t0[1] * e0_t0[0];

    double roots[2];
    int rootAmt = 0;
    if (a == 0) {
        if (b == 0) {
            // parallel motion, only need to handle colinear case
            if (c == 0) {
                // colinear PP CCD
                if ((p_t0 - e0_t0).dot(d0 - d1) < 0) {
                    roots[rootAmt] = std::sqrt(
                        (p_t0 - e0_t0).squaredNorm() / (d0 - d1).squaredNorm());
                    if (roots[rootAmt] > 0 && roots[rootAmt] <= 1) {
                        ++rootAmt;
                    }
                }
                if ((p_t0 - e1_t0).dot(d0 - d2) < 0) {
                    roots[rootAmt] = std::sqrt(
                        (p_t0 - e1_t0).squaredNorm() / (d0 - d2).squaredNorm());
                    if (roots[rootAmt] > 0 && roots[rootAmt] <= 1) {
                        ++rootAmt;
                    }
                }

                if (rootAmt == 2) {
                    toi = std::min(roots[0], roots[1]) * conservative_rescaling;
                    return true;
                } else if (rootAmt == 1) {
                    toi = roots[0] * conservative_rescaling;
                    return true;
                } else {
                    return false;
                }
            }
        } else {
            rootAmt = 1;
            roots[0] = -c / b;
        }
    } else {
        double delta = b * b - 4 * a * c;
        if (delta == 0) {
            rootAmt = 1;
            roots[0] = -b / (2 * a);
        } else if (delta > 0) {
            rootAmt = 2;
            // accurate expression differs in b's sign
            if (b > 0) {
                roots[0] = (-b - std::sqrt(delta)) / (2 * a);
                roots[1] = 2 * c / (-b - std::sqrt(delta));
            } else {
                roots[0] = 2 * c / (-b + std::sqrt(delta));
                roots[1] = (-b + std::sqrt(delta)) / (2 * a);
            }

            if (roots[0] > roots[1]) {
                std::swap(roots[0], roots[1]);
            }
        }
    }

    for (int i = 0; i < rootAmt; ++i) {
        if (roots[i] > 0 && roots[i] <= 1) {
            // check overlap
            double ratio;
            if (point_edge_distance_type(
                    Eigen::Vector2d(p_t0 + roots[i] * d0),
                    Eigen::Vector2d(e0_t0 + roots[i] * d1),
                    Eigen::Vector2d(e1_t0 + roots[i] * d2))
                == PointEdgeDistanceType::P_E) {
                toi = roots[i] * conservative_rescaling; // TODO: distance eta
                return true;
            }
        }
    }

    return false;
}

} // namespace ipc


<|start_filename|>src/distance/point_triangle.hpp<|end_filename|>
#pragma once

#include <stdexcept> // std::invalid_argument

#include <ipc/distance/distance_type.hpp>
#include <ipc/distance/point_edge.hpp>
#include <ipc/distance/point_plane.hpp>
#include <ipc/distance/point_point.hpp>

namespace ipc {

/// @brief Compute the distance between a points and a triangle.
/// @note The distance is actually squared distance.
/// @param p The point.
/// @param t0 The first vertex of the triangle.
/// @param t1 The second vertex of the triangle.
/// @param t2 The third vertex of the triangle.
/// @return The distance between the point and triangle.
template <
    typename DerivedP,
    typename DerivedT0,
    typename DerivedT1,
    typename DerivedT2>
auto point_triangle_distance(
    const Eigen::MatrixBase<DerivedP>& p,
    const Eigen::MatrixBase<DerivedT0>& t0,
    const Eigen::MatrixBase<DerivedT1>& t1,
    const Eigen::MatrixBase<DerivedT2>& t2)
{
    assert(p.size() == 3);
    assert(t0.size() == 3);
    assert(t1.size() == 3);
    assert(t2.size() == 3);

    return point_triangle_distance(
        p, t0, t1, t2, point_triangle_distance_type(p, t0, t1, t2));
}

/// @brief Compute the distance between a points and a triangle.
/// @note The distance is actually squared distance.
/// @param p The point.
/// @param t0 The first vertex of the triangle.
/// @param t1 The second vertex of the triangle.
/// @param t2 The third vertex of the triangle.
/// @param dtype The point-triangle distance type to compute.
/// @return The distance between the point and triangle.
template <
    typename DerivedP,
    typename DerivedT0,
    typename DerivedT1,
    typename DerivedT2>
auto point_triangle_distance(
    const Eigen::MatrixBase<DerivedP>& p,
    const Eigen::MatrixBase<DerivedT0>& t0,
    const Eigen::MatrixBase<DerivedT1>& t1,
    const Eigen::MatrixBase<DerivedT2>& t2,
    const PointTriangleDistanceType dtype)
{
    assert(p.size() == 3);
    assert(t0.size() == 3);
    assert(t1.size() == 3);
    assert(t2.size() == 3);

    switch (dtype) {
    case PointTriangleDistanceType::P_T0:
        return point_point_distance(p, t0);

    case PointTriangleDistanceType::P_T1:
        return point_point_distance(p, t1);

    case PointTriangleDistanceType::P_T2:
        return point_point_distance(p, t2);

    case PointTriangleDistanceType::P_E0:
        return point_line_distance(p, t0, t1);

    case PointTriangleDistanceType::P_E1:
        return point_line_distance(p, t1, t2);

    case PointTriangleDistanceType::P_E2:
        return point_line_distance(p, t2, t0);

    case PointTriangleDistanceType::P_T:
        return point_plane_distance(p, t0, t1, t2);
    }

    throw std::invalid_argument(
        "Invalid distance type for point-triangle distance!");
}

/// @brief Compute the gradient of the distance between a points and a triangle.
/// @note The distance is actually squared distance.
/// @param[in] p The point.
/// @param[in] t0 The first vertex of the triangle.
/// @param[in] t1 The second vertex of the triangle.
/// @param[in] t2 The third vertex of the triangle.
/// @param[out] grad The gradient of the distance wrt p, t0, t1, and t2.
template <
    typename DerivedP,
    typename DerivedT0,
    typename DerivedT1,
    typename DerivedT2,
    typename DerivedGrad>
void point_triangle_distance_gradient(
    const Eigen::MatrixBase<DerivedP>& p,
    const Eigen::MatrixBase<DerivedT0>& t0,
    const Eigen::MatrixBase<DerivedT1>& t1,
    const Eigen::MatrixBase<DerivedT2>& t2,
    Eigen::PlainObjectBase<DerivedGrad>& grad)
{
    assert(p.size() == 3);
    assert(t0.size() == 3);
    assert(t1.size() == 3);
    assert(t2.size() == 3);

    return point_triangle_distance_gradient(
        p, t0, t1, t2, point_triangle_distance_type(p, t0, t1, t2), grad);
}

/// @brief Compute the gradient of the distance between a points and a triangle.
/// @note The distance is actually squared distance.
/// @param[in] p The point.
/// @param[in] t0 The first vertex of the triangle.
/// @param[in] t1 The second vertex of the triangle.
/// @param[in] t2 The third vertex of the triangle.
/// @param dtype The point-triangle distance type to compute.
/// @param[out] grad The gradient of the distance wrt p, t0, t1, and t2.
template <
    typename DerivedP,
    typename DerivedT0,
    typename DerivedT1,
    typename DerivedT2,
    typename DerivedGrad>
void point_triangle_distance_gradient(
    const Eigen::MatrixBase<DerivedP>& p,
    const Eigen::MatrixBase<DerivedT0>& t0,
    const Eigen::MatrixBase<DerivedT1>& t1,
    const Eigen::MatrixBase<DerivedT2>& t2,
    const PointTriangleDistanceType dtype,
    Eigen::PlainObjectBase<DerivedGrad>& grad)
{
    int dim = p.size();
    assert(t0.size() == dim);
    assert(t1.size() == dim);
    assert(t2.size() == dim);

    grad.resize(4 * dim);
    grad.setZero();

    VectorMax9<typename DerivedGrad::Scalar> local_grad;
    switch (dtype) {
    case PointTriangleDistanceType::P_T0:
        point_point_distance_gradient(p, t0, local_grad);
        grad.head(2 * dim) = local_grad;
        break;

    case PointTriangleDistanceType::P_T1:
        point_point_distance_gradient(p, t1, local_grad);
        grad.head(dim) = local_grad.head(dim);
        grad.segment(2 * dim, dim) = local_grad.tail(dim);
        break;

    case PointTriangleDistanceType::P_T2:
        point_point_distance_gradient(p, t2, local_grad);
        grad.head(dim) = local_grad.head(dim);
        grad.tail(dim) = local_grad.tail(dim);
        break;

    case PointTriangleDistanceType::P_E0:
        point_line_distance_gradient(p, t0, t1, local_grad);
        grad.head(3 * dim) = local_grad;
        break;

    case PointTriangleDistanceType::P_E1:
        point_line_distance_gradient(p, t1, t2, local_grad);
        grad.head(dim) = local_grad.head(dim);
        grad.tail(2 * dim) = local_grad.tail(2 * dim);
        break;

    case PointTriangleDistanceType::P_E2:
        point_line_distance_gradient(p, t2, t0, local_grad);
        grad.head(dim) = local_grad.head(dim);         // ∇_p
        grad.segment(dim, dim) = local_grad.tail(dim); // ∇_{t0}
        grad.tail(dim) = local_grad.segment(dim, dim); // ∇_{t2}
        break;

    case PointTriangleDistanceType::P_T:
        point_plane_distance_gradient(p, t0, t1, t2, grad);
        break;

    default:
        throw std::invalid_argument(
            "Invalid distance type for point-triangle distance gradient!");
    }
}

/// @brief Compute the hessian of the distance between a points and a triangle.
/// @note The distance is actually squared distance.
/// @param[in] p The point.
/// @param[in] t0 The first vertex of the triangle.
/// @param[in] t1 The second vertex of the triangle.
/// @param[in] t2 The third vertex of the triangle.
/// @param[out] grad The hessian of the distance wrt p, t0, t1, and t2.
template <
    typename DerivedP,
    typename DerivedT0,
    typename DerivedT1,
    typename DerivedT2,
    typename DerivedHess>
void point_triangle_distance_hessian(
    const Eigen::MatrixBase<DerivedP>& p,
    const Eigen::MatrixBase<DerivedT0>& t0,
    const Eigen::MatrixBase<DerivedT1>& t1,
    const Eigen::MatrixBase<DerivedT2>& t2,
    Eigen::PlainObjectBase<DerivedHess>& hess)
{
    assert(p.size() == 3);
    assert(t0.size() == 3);
    assert(t1.size() == 3);
    assert(t2.size() == 3);

    return point_triangle_distance_hessian(
        p, t0, t1, t2, point_triangle_distance_type(p, t0, t1, t2), hess);
}

/// @brief Compute the hessian of the distance between a points and a triangle.
/// @note The distance is actually squared distance.
/// @param[in] p The point.
/// @param[in] t0 The first vertex of the triangle.
/// @param[in] t1 The second vertex of the triangle.
/// @param[in] t2 The third vertex of the triangle.
/// @param dtype The point-triangle distance type to compute.
/// @param[out] grad The hessian of the distance wrt p, t0, t1, and t2.
template <
    typename DerivedP,
    typename DerivedT0,
    typename DerivedT1,
    typename DerivedT2,
    typename DerivedHess>
void point_triangle_distance_hessian(
    const Eigen::MatrixBase<DerivedP>& p,
    const Eigen::MatrixBase<DerivedT0>& t0,
    const Eigen::MatrixBase<DerivedT1>& t1,
    const Eigen::MatrixBase<DerivedT2>& t2,
    const PointTriangleDistanceType dtype,
    Eigen::PlainObjectBase<DerivedHess>& hess)
{
    int dim = p.size();
    assert(t0.size() == dim);
    assert(t1.size() == dim);
    assert(t2.size() == dim);

    hess.resize(4 * dim, 4 * dim);
    hess.setZero();

    MatrixMax9<typename DerivedHess::Scalar> local_hess;
    switch (dtype) {
    case PointTriangleDistanceType::P_T0:
        point_point_distance_hessian(p, t0, local_hess);
        hess.topLeftCorner(2 * dim, 2 * dim) = local_hess;
        break;

    case PointTriangleDistanceType::P_T1:
        point_point_distance_hessian(p, t1, local_hess);
        hess.topLeftCorner(dim, dim) = local_hess.topLeftCorner(dim, dim);
        hess.block(0, 2 * dim, dim, dim) = local_hess.topRightCorner(dim, dim);
        hess.block(2 * dim, 0, dim, dim) =
            local_hess.bottomLeftCorner(dim, dim);
        hess.block(2 * dim, 2 * dim, dim, dim) =
            local_hess.bottomRightCorner(dim, dim);
        break;

    case PointTriangleDistanceType::P_T2:
        point_point_distance_hessian(p, t2, local_hess);
        hess.topLeftCorner(dim, dim) = local_hess.topLeftCorner(dim, dim);
        hess.topRightCorner(dim, dim) = local_hess.topRightCorner(dim, dim);
        hess.bottomLeftCorner(dim, dim) = local_hess.bottomLeftCorner(dim, dim);
        hess.bottomRightCorner(dim, dim) =
            local_hess.bottomRightCorner(dim, dim);
        break;

    case PointTriangleDistanceType::P_E0:
        point_line_distance_hessian(p, t0, t1, local_hess);
        hess.topLeftCorner(3 * dim, 3 * dim) = local_hess;
        break;

    case PointTriangleDistanceType::P_E1:
        point_line_distance_hessian(p, t1, t2, local_hess);
        hess.topLeftCorner(dim, dim) = local_hess.topLeftCorner(dim, dim);
        hess.topRightCorner(dim, 2 * dim) =
            local_hess.topRightCorner(dim, 2 * dim);
        hess.bottomLeftCorner(2 * dim, dim) =
            local_hess.bottomLeftCorner(2 * dim, dim);
        hess.bottomRightCorner(2 * dim, 2 * dim) =
            local_hess.bottomRightCorner(2 * dim, 2 * dim);
        break;

    case PointTriangleDistanceType::P_E2:
        point_line_distance_hessian(p, t2, t0, local_hess);
        hess.topLeftCorner(dim, dim) = local_hess.topLeftCorner(dim, dim);
        hess.block(0, dim, dim, dim) = local_hess.topRightCorner(dim, dim);
        hess.topRightCorner(dim, dim) = local_hess.block(0, dim, dim, dim);
        hess.block(dim, 0, dim, dim) = local_hess.bottomLeftCorner(dim, dim);
        hess.block(dim, dim, dim, dim) = local_hess.bottomRightCorner(dim, dim);
        hess.block(dim, 3 * dim, dim, dim) =
            local_hess.block(2 * dim, dim, dim, dim);
        hess.bottomLeftCorner(dim, dim) = local_hess.block(dim, 0, dim, dim);
        hess.block(3 * dim, dim, dim, dim) =
            local_hess.block(dim, 2 * dim, dim, dim);
        hess.bottomRightCorner(dim, dim) = local_hess.block(dim, dim, dim, dim);
        break;

    case PointTriangleDistanceType::P_T:
        point_plane_distance_hessian(p, t0, t1, t2, hess);
        break;

    default:
        throw std::invalid_argument(
            "Invalid distance type for point-triangle distance hessian!");
    }
}

} // namespace ipc


<|start_filename|>python/src/ipc.cpp<|end_filename|>
#include <pybind11/pybind11.h>
#include <pybind11/eigen.h>
#include <pybind11/functional.h>

#include <ipc/ipc.hpp>

namespace py = pybind11;
using namespace ipc;

bool default_can_collide(size_t, size_t) { return true; }

void define_ipc_functions(py::module_& m)
{
    m.def(
        "construct_constraint_set",
        [](const Eigen::MatrixXd& V_rest, const Eigen::MatrixXd& V,
           const Eigen::MatrixXi& E, const Eigen::MatrixXi& F, double dhat,
           const Eigen::MatrixXi& F2E, double dmin,
           const BroadPhaseMethod& method,
           const std::function<bool(size_t, size_t)>& can_collide) {
            Constraints constraint_set;
            construct_constraint_set(
                V_rest, V, E, F, dhat, constraint_set, F2E, dmin, method,
                can_collide);
            return constraint_set;
        },
        R"ipc_Qu8mg5v7(
        Construct a set of constraints used to compute the barrier potential.

        All vertices in V will be considered for collisions, so V should be only the surface vertices. The edges and face should be only for the surface elements.

        Parameters:
            V: vertex positions as rows of a matrix
            E: edges as rows of indicies into V
            F: triangular faces as rows of indicies into V
            dhat: the activation distance of the barrier
            F2E: (optional) map from F edges to rows of E
            dmin: (optional) minimum distance
            method: (optional) broad-phase method to use
            can_collide: (optional) a function that takes two vertex IDs (row numbers in F) and returns true if the vertices (and faces or edges containting the edges) can collide. By default all primitives can collide with all other primitives.

        Returns:
            The constructed set of constraints.
        )ipc_Qu8mg5v7",
        py::arg("V_rest"), py::arg("V"), py::arg("E"), py::arg("F"),
        py::arg("dhat"), py::arg("F2E") = Eigen::MatrixXi(),
        py::arg("dmin") = 0, py::arg("method") = BroadPhaseMethod::HASH_GRID,
        py::arg("can_collide") =
            std::function<bool(size_t, size_t)>(default_can_collide));

    m.def(
        "compute_barrier_potential", &compute_barrier_potential,
        R"ipc_Qu8mg5v7(
        Compute the barrier potential for a given constraint set.

        Parameters:
            V: vertex positions as rows of a matrix
            E: edges as rows of indicies into V
            F: triangular faces as rows of indicies into V
            constraint_set: the set of constraints
            dhat: the activation distance of the barrier

        Returns:
            The sum of all barrier potentials (not scaled by the barrier stiffness).
        )ipc_Qu8mg5v7",
        py::arg("V"), py::arg("E"), py::arg("F"), py::arg("constraint_set"),
        py::arg("dhat"));

    m.def(
        "compute_barrier_potential_gradient",
        &compute_barrier_potential_gradient,
        R"ipc_Qu8mg5v7(
        Compute the gradient of the barrier potential.

        Parameters:
            V: vertex positions as rows of a matrix
            E: edges as rows of indicies into V
            F: triangular faces as rows of indicies into V
            constraint_set: the set of constraints
            dhat: the activation distance of the barrier

        Returns:
            The gradient of all barrier potentials (not scaled by the barrier stiffness).
        )ipc_Qu8mg5v7",
        py::arg("V"), py::arg("E"), py::arg("F"), py::arg("constraint_set"),
        py::arg("dhat"));

    m.def(
        "compute_barrier_potential_hessian", &compute_barrier_potential_hessian,
        R"ipc_Qu8mg5v7(
        Compute the hessian of the barrier potential.

        Parameters:
            V: vertex positions as rows of a matrix
            E: edges as rows of indicies into V
            F: triangular faces as rows of indicies into V
            constraint_set: the set of constraints
            dhat: the activation distance of the barrier
            project_to_psd: make sure the hessian is positive semi-definite

        Returns:
            The hessian of all barrier potentials (not scaled by the barrier stiffness).
        )ipc_Qu8mg5v7",
        py::arg("V"), py::arg("E"), py::arg("F"), py::arg("constraint_set"),
        py::arg("dhat"), py::arg("project_to_psd") = true);

    // ...

    m.def(
        "has_intersections", &has_intersections,
        R"ipc_Qu8mg5v7(

        Parameters:
            V: vertex positions as rows of a matrix
            E: edges as rows of indicies into V
            F: triangular faces as rows of indicies into V
            can_collide: (optional) a function that takes two vertex IDs (row numbers in F) and returns true if the vertices (and faces or edges containting the edges) can collide. By default all primitives can collide with all other primitives.

        Returns:
            A boolean for if the mesh has intersections.
        )ipc_Qu8mg5v7",
        py::arg("V"), py::arg("E"), py::arg("F"),
        py::arg("can_collide") =
            std::function<bool(size_t, size_t)>(default_can_collide));
}


<|start_filename|>cmake/recipes/robin_map.cmake<|end_filename|>
if(TARGET tsl::robin_map)
    return()
endif()

message(STATUS "Third-party: creating target 'tsl::robin_map'")

include(FetchContent)
FetchContent_Declare(
    robin-map
    GIT_REPOSITORY https://github.com/Tessil/robin-map.git
    GIT_TAG v0.6.3
    GIT_SHALLOW TRUE
)
FetchContent_MakeAvailable(robin-map)


<|start_filename|>src/distance/line_line.hpp<|end_filename|>
#pragma once

#include <Eigen/Core>
#include <Eigen/Geometry>

#include <ipc/utils/eigen_ext.hpp>

namespace ipc {

/// @brief Compute the distance between a two infinite lines in 3D.
/// @note The distance is actually squared distance.
/// @warning If the lines are parallel this function returns a distance of zero.
/// @param ea0 The first vertex of the edge defining the first line.
/// @param ea1 The second vertex of the edge defining the first line.
/// @param ea0 The first vertex of the edge defining the second line.
/// @param ea1 The second vertex of the edge defining the second line.
/// @return The distance between the two lines.
template <
    typename DerivedEA0,
    typename DerivedEA1,
    typename DerivedEB0,
    typename DerivedEB1>
auto line_line_distance(
    const Eigen::MatrixBase<DerivedEA0>& ea0,
    const Eigen::MatrixBase<DerivedEA1>& ea1,
    const Eigen::MatrixBase<DerivedEB0>& eb0,
    const Eigen::MatrixBase<DerivedEB1>& eb1)
{
    assert(ea0.size() == 3);
    assert(ea1.size() == 3);
    assert(eb0.size() == 3);
    assert(eb1.size() == 3);

    const auto normal = cross(ea1 - ea0, eb1 - eb0);
    const auto line_to_line = (eb0 - ea0).dot(normal);
    return line_to_line * line_to_line / normal.squaredNorm();
}

// Symbolically generated derivatives;
namespace autogen {
    void line_line_distance_gradient(
        double v01,
        double v02,
        double v03,
        double v11,
        double v12,
        double v13,
        double v21,
        double v22,
        double v23,
        double v31,
        double v32,
        double v33,
        double g[12]);

    void line_line_distance_hessian(
        double v01,
        double v02,
        double v03,
        double v11,
        double v12,
        double v13,
        double v21,
        double v22,
        double v23,
        double v31,
        double v32,
        double v33,
        double H[144]);
} // namespace autogen

/// @brief Compute the gradient of the distance between a two lines in 3D.
/// @note The distance is actually squared distance.
/// @warning If the lines are parallel this function returns a distance of zero.
/// @param[in] ea0 The first vertex of the edge defining the first line.
/// @param[in] ea1 The second vertex of the edge defining the first line.
/// @param[in] ea0 The first vertex of the edge defining the second line.
/// @param[in] ea1 The second vertex of the edge defining the second line.
/// @param[out] hess The gradient of the distance wrt ea0, ea1, eb0, and eb1.
template <
    typename DerivedEA0,
    typename DerivedEA1,
    typename DerivedEB0,
    typename DerivedEB1,
    typename DerivedGrad>
void line_line_distance_gradient(
    const Eigen::MatrixBase<DerivedEA0>& ea0,
    const Eigen::MatrixBase<DerivedEA1>& ea1,
    const Eigen::MatrixBase<DerivedEB0>& eb0,
    const Eigen::MatrixBase<DerivedEB1>& eb1,
    Eigen::PlainObjectBase<DerivedGrad>& grad)
{
    assert(ea0.size() == 3);
    assert(ea1.size() == 3);
    assert(eb0.size() == 3);
    assert(eb1.size() == 3);

    grad.resize(ea0.size() + ea1.size() + eb0.size() + eb1.size());
    autogen::line_line_distance_gradient(
        ea0[0], ea0[1], ea0[2], ea1[0], ea1[1], ea1[2], eb0[0], eb0[1], eb0[2],
        eb1[0], eb1[1], eb1[2], grad.data());
}

/// @brief Compute the hessian of the distance between a two lines in 3D.
/// @note The distance is actually squared distance.
/// @warning If the lines are parallel this function returns a distance of zero.
/// @param[in] ea0 The first vertex of the edge defining the first line.
/// @param[in] ea1 The second vertex of the edge defining the first line.
/// @param[in] ea0 The first vertex of the edge defining the second line.
/// @param[in] ea1 The second vertex of the edge defining the second line.
/// @param[out] hess The hessian of the distance wrt ea0, ea1, eb0, and eb1.
template <
    typename DerivedEA0,
    typename DerivedEA1,
    typename DerivedEB0,
    typename DerivedEB1,
    typename DerivedHess>
void line_line_distance_hessian(
    const Eigen::MatrixBase<DerivedEA0>& ea0,
    const Eigen::MatrixBase<DerivedEA1>& ea1,
    const Eigen::MatrixBase<DerivedEB0>& eb0,
    const Eigen::MatrixBase<DerivedEB1>& eb1,
    Eigen::PlainObjectBase<DerivedHess>& hess)
{
    assert(ea0.size() == 3);
    assert(ea1.size() == 3);
    assert(eb0.size() == 3);
    assert(eb1.size() == 3);

    hess.resize(
        ea0.size() + ea1.size() + eb0.size() + eb1.size(),
        ea0.size() + ea1.size() + eb0.size() + eb1.size());
    autogen::line_line_distance_hessian(
        ea0[0], ea0[1], ea0[2], ea1[0], ea1[1], ea1[2], eb0[0], eb0[1], eb0[2],
        eb1[0], eb1[1], eb1[2], hess.data());
}

} // namespace ipc


<|start_filename|>src/distance/point_point.hpp<|end_filename|>
#pragma once

#include <Eigen/Core>

#include <ipc/utils/eigen_ext.hpp>

namespace ipc {

/// @brief Compute the distance between two points.
/// @note The distance is actually squared distance.
/// @param[in] p0 The first point.
/// @param[in] p1 The second point.
/// @return The distance between p0 and p1.
template <typename DerivedP0, typename DerivedP1>
inline auto point_point_distance(
    const Eigen::MatrixBase<DerivedP0>& p0,
    const Eigen::MatrixBase<DerivedP1>& p1)
{
    return (p1 - p0).squaredNorm();
}

/// @brief Compute the gradient of the distance between two points.
/// @note The distance is actually squared distance.
/// @param[in] p0 The first point.
/// @param[in] p1 The second point.
/// @param[out] grad The computed gradient.
template <typename DerivedP0, typename DerivedP1, typename DerivedGrad>
inline void point_point_distance_gradient(
    const Eigen::MatrixBase<DerivedP0>& p0,
    const Eigen::MatrixBase<DerivedP1>& p1,
    Eigen::PlainObjectBase<DerivedGrad>& grad)
{
    assert(p0.size() == p1.size());
    grad.resize(p0.size() + p1.size());
    grad.head(p0.size()) = 2.0 * (p0 - p1);
    grad.tail(p1.size()) = -grad.head(p0.size());
}

/// @brief Compute the hessian of the distance between two points.
/// @note The distance is actually squared distance.
/// @param[in] p0 The first point.
/// @param[in] p1 The second point.
/// @param[out] hess The computed hessian.
template <typename DerivedP0, typename DerivedP1, typename DerivedHess>
inline void point_point_distance_hessian(
    const Eigen::MatrixBase<DerivedP0>& p0,
    const Eigen::MatrixBase<DerivedP1>& p1,
    Eigen::PlainObjectBase<DerivedHess>& hess)
{
    int dim = p0.size();
    assert(p1.size() == dim);

    hess.resize(2 * dim, 2 * dim);

    hess.setZero();
    hess.diagonal().setConstant(2.0);
    for (int i = 0; i < dim; i++) {
        hess(i, i + dim) = hess(i + dim, i) = -2;
    }
}

} // namespace ipc


<|start_filename|>python/src/broad_phase/collision_candidate.cpp<|end_filename|>
#include <pybind11/pybind11.h>
#include <pybind11/stl.h>
#include <pybind11/iostream.h>
#include <pybind11/operators.h>

#include <ipc/broad_phase/collision_candidate.hpp>
#include <ipc/utils/logger.hpp>

namespace py = pybind11;
using namespace ipc;

void define_collision_candidate_classes(py::module_& m)
{
    py::class_<VertexVertexCandidate>(m, "VertexVertexCandidate")
        .def(py::init<long, long>())
        .def(
            "__str__",
            [](const VertexVertexCandidate& ev) {
                return fmt::format(
                    "[{:d}, {:d}]", ev.vertex0_index, ev.vertex1_index);
            })
        .def(
            "__repr__",
            [](const VertexVertexCandidate& ev) {
                return fmt::format(
                    "VertexVertexCandidate({:d}, {:d})", ev.vertex0_index,
                    ev.vertex1_index);
            })
        .def("__eq__", &VertexVertexCandidate::operator==)
        .def("__ne__", &VertexVertexCandidate::operator!=)
        .def("__lt__", &VertexVertexCandidate::operator<)
        .def_readwrite("vertex0_index", &VertexVertexCandidate::vertex0_index)
        .def_readwrite("vertex1_index", &VertexVertexCandidate::vertex1_index);

    py::class_<EdgeVertexCandidate>(m, "EdgeVertexCandidate")
        .def(py::init<long, long>())
        .def(
            "__str__",
            [](const EdgeVertexCandidate& ev) {
                return fmt::format(
                    "[{:d}, {:d}]", ev.edge_index, ev.vertex_index);
            })
        .def(
            "__repr__",
            [](const EdgeVertexCandidate& ev) {
                return fmt::format(
                    "EdgeVertexCandidate({:d}, {:d})", ev.edge_index,
                    ev.vertex_index);
            })
        .def("__eq__", &EdgeVertexCandidate::operator==)
        .def("__ne__", &EdgeVertexCandidate::operator!=)
        .def("__lt__", &EdgeVertexCandidate::operator<)
        .def_readwrite("edge_index", &EdgeVertexCandidate::edge_index)
        .def_readwrite("vertex_index", &EdgeVertexCandidate::vertex_index);

    py::class_<EdgeEdgeCandidate>(m, "EdgeEdgeCandidate")
        .def(py::init<long, long>())
        .def(
            "__str__",
            [](const EdgeEdgeCandidate& ev) {
                return fmt::format(
                    "[{:d}, {:d}]", ev.edge0_index, ev.edge1_index);
            })
        .def(
            "__repr__",
            [](const EdgeEdgeCandidate& ev) {
                return fmt::format(
                    "EdgeEdgeCandidate({:d}, {:d})", ev.edge0_index,
                    ev.edge1_index);
            })
        .def("__eq__", &EdgeEdgeCandidate::operator==)
        .def("__ne__", &EdgeEdgeCandidate::operator!=)
        .def("__lt__", &EdgeEdgeCandidate::operator<)
        .def_readwrite("edge0_index", &EdgeEdgeCandidate::edge0_index)
        .def_readwrite("edge1_index", &EdgeEdgeCandidate::edge1_index);

    py::class_<EdgeFaceCandidate>(m, "EdgeFaceCandidate")
        .def(py::init<long, long>())
        .def(
            "__str__",
            [](const EdgeFaceCandidate& ev) {
                return fmt::format(
                    "[{:d}, {:d}]", ev.edge_index, ev.face_index);
            })
        .def(
            "__repr__",
            [](const EdgeFaceCandidate& ev) {
                return fmt::format(
                    "EdgeFaceCandidate({:d}, {:d})", ev.edge_index,
                    ev.face_index);
            })
        .def("__eq__", &EdgeFaceCandidate::operator==)
        .def("__ne__", &EdgeFaceCandidate::operator!=)
        .def("__lt__", &EdgeFaceCandidate::operator<)
        .def_readwrite("edge_index", &EdgeFaceCandidate::edge_index)
        .def_readwrite("face_index", &EdgeFaceCandidate::face_index);

    py::class_<FaceVertexCandidate>(m, "FaceVertexCandidate")
        .def(py::init<long, long>())
        .def(
            "__str__",
            [](const FaceVertexCandidate& ev) {
                return fmt::format(
                    "[{:d}, {:d}]", ev.face_index, ev.vertex_index);
            })
        .def(
            "__repr__",
            [](const FaceVertexCandidate& ev) {
                return fmt::format(
                    "FaceVertexCandidate({:d}, {:d})", ev.face_index,
                    ev.vertex_index);
            })
        .def("__eq__", &FaceVertexCandidate::operator==)
        .def("__ne__", &FaceVertexCandidate::operator!=)
        .def("__lt__", &FaceVertexCandidate::operator<)
        .def_readwrite("face_index", &FaceVertexCandidate::face_index)
        .def_readwrite("vertex_index", &FaceVertexCandidate::vertex_index);

    py::class_<Candidates>(m, "Candidates")
        .def(py::init())
        .def("size", &Candidates::size)
        .def("empty", &Candidates::empty)
        .def("clear", &Candidates::clear)
        .def_readwrite("ev_candidates", &Candidates::ev_candidates)
        .def_readwrite("ee_candidates", &Candidates::ee_candidates)
        .def_readwrite("fv_candidates", &Candidates::fv_candidates);
}


<|start_filename|>src/implicits/plane.hpp<|end_filename|>
#pragma once

#include <vector>

#include <Eigen/Core>

#include <ipc/collision_constraint.hpp>

namespace ipc {

/// @brief Construct a set of point-plane distance constraints used to compute
/// the barrier potential.
///
/// @note The given pv_constraints will be cleared.
///
/// @param[in]  V  Vertex positions as rows of a matrix.
/// @param[in]  plane_origins  Plane origins as rows of a matrix.
/// @param[in]  plane_normals  Plane normals as rows of a matrix.
/// @param[in]  dhat  The activation distance of the barrier.
/// @param[out] pv_constraints  The constructed set of constraints.
/// @param[in]  dmin  Minimum distance.
/// @param[in] can_collide
///     A function that takes a vertex ID (row numbers in V) and a plane ID (row
///     number in plane_origins) then returns true if the vertex can collide
///     with the plane. By default all points can collide with all planes.
void construct_point_plane_constraint_set(
    const Eigen::MatrixXd& V,
    const Eigen::MatrixXd& plane_origins,
    const Eigen::MatrixXd& plane_normals,
    const double dhat,
    std::vector<PlaneVertexConstraint>& pv_constraints,
    const double dmin = 0,
    const std::function<bool(size_t, size_t)>& can_collide =
        [](size_t, size_t) { return true; });

///////////////////////////////////////////////////////////////////////////////
// Collision detection

/// @brief Determine if the step is collision free.
///
/// @note Assumes the trajectory is linear.
/// @note V can either be the surface vertices or the entire mesh vertices.
///
/// @param[in] V0 Vertex positions at start as rows of a matrix.
/// @param[in] V1 Vertex positions at end as rows of a matrix.
/// @param[in] plane_origins  Plane origins as rows of a matrix.
/// @param[in] plane_normals  Plane normals as rows of a matrix.
/// @param[in] can_collide
///     A function that takes a vertex ID (row numbers in V) and a plane ID (row
///     number in plane_origins) then returns true if the vertex can collide
///     with the plane. By default all points can collide with all planes.
/// @returns True if <b>any</b> collisions occur.
bool is_step_point_plane_collision_free(
    const Eigen::MatrixXd& V0,
    const Eigen::MatrixXd& V1,
    const Eigen::MatrixXd& plane_origins,
    const Eigen::MatrixXd& plane_normals,
    const std::function<bool(size_t, size_t)>& can_collide =
        [](size_t, size_t) { return true; });

/// @brief Computes a maximal step size that is collision free.
///
/// @note Assumes V0 is intersection free.
/// @note Assumes the trajectory is linear.
/// @note A value of 1.0 if a full step and 0.0 is no step.
///
/// @param[in] V0 Vertex positions at start as rows of a matrix.
/// @param[in] V1 Vertex positions at end as rows of a matrix.
/// @param[in] plane_origins  Plane origins as rows of a matrix.
/// @param[in] plane_normals  Plane normals as rows of a matrix.
/// @param[in] can_collide
///     A function that takes a vertex ID (row numbers in V) and a plane ID (row
///     number in plane_origins) then returns true if the vertex can collide
///     with the plane. By default all points can collide with all planes.
/// @returns A step-size \f$\in [0, 1]\f$ that is collision free.
bool compute_point_plane_collision_free_stepsize(
    const Eigen::MatrixXd& V0,
    const Eigen::MatrixXd& V1,
    const Eigen::MatrixXd& plane_origins,
    const Eigen::MatrixXd& plane_normals,
    const std::function<bool(size_t, size_t)>& can_collide =
        [](size_t, size_t) { return true; });

} // namespace ipc


<|start_filename|>src/broad_phase/collision_candidate.hpp<|end_filename|>
#pragma once

#include <vector>
#include <fstream>

#include <Eigen/Core>

#include <ipc/ccd/ccd.hpp>

namespace ipc {

/// Virtual class for candidates that support CCD.
struct ContinuousCollisionCandidate {
    virtual ~ContinuousCollisionCandidate() { }

    /// Perform narrow-phase CCD on the candidate.
    /// @param[in] V0 Mesh vertex positions at the start of the time step.
    /// @param[in] V1 Mesh vertex positions at the end of the time step.
    /// @param[in] E Mesh edges as rows of indicies into V.
    /// @param[in] F Mesh triangular faces as rows of indicies into V.
    /// @param[out] toi Computed time of impact (normalized).
    /// @param[in] tmax Maximum time (normalized) to look for collisions.
    ///                 Should be in [0, 1].
    /// @param[in] tolerance CCD tolerance used by Tight-Inclusion CCD.
    /// @param[in] max_iterations Maximum iterations used by Tight-Inclusion
    ///                           CCD.
    /// @param[in] conservative_rescaling Conservative rescaling value used to
    ///                                   avoid taking steps exactly to impact.
    /// @return If the candidate had a collision over the time interval.
    virtual bool
    ccd(const Eigen::MatrixXd& V0,
        const Eigen::MatrixXd& V1,
        const Eigen::MatrixXi& E,
        const Eigen::MatrixXi& F,
        double& toi,
        const double tmax = 1.0,
        const double tolerance = DEFAULT_CCD_TOLERANCE,
        const long max_iterations = DEFAULT_CCD_MAX_ITERATIONS,
        const double conservative_rescaling =
            DEFAULT_CCD_CONSERVATIVE_RESCALING) const = 0;
};

struct VertexVertexCandidate {
    VertexVertexCandidate(long vertex0_index, long vertex1_index);

    bool operator==(const VertexVertexCandidate& other) const;
    bool operator!=(const VertexVertexCandidate& other) const
    {
        return !(*this == other);
    }

    /// @brief Compare EdgeVertexCandidates for sorting.
    bool operator<(const VertexVertexCandidate& other) const;

    long vertex0_index;
    long vertex1_index;
};

struct EdgeVertexCandidate : ContinuousCollisionCandidate {
    EdgeVertexCandidate(long edge_index, long vertex_index);

    bool operator==(const EdgeVertexCandidate& other) const;
    bool operator!=(const EdgeVertexCandidate& other) const
    {
        return !(*this == other);
    }

    /// @brief Compare EdgeVertexCandidates for sorting.
    bool operator<(const EdgeVertexCandidate& other) const;

    bool
    ccd(const Eigen::MatrixXd& V0,
        const Eigen::MatrixXd& V1,
        const Eigen::MatrixXi& E,
        const Eigen::MatrixXi& F,
        double& toi,
        const double tmax = 1.0,
        const double tolerance = DEFAULT_CCD_TOLERANCE,
        const long max_iterations = DEFAULT_CCD_MAX_ITERATIONS,
        const double conservative_rescaling =
            DEFAULT_CCD_CONSERVATIVE_RESCALING) const override;

    long edge_index;
    long vertex_index;
};

struct EdgeEdgeCandidate : ContinuousCollisionCandidate {
    EdgeEdgeCandidate(long edge0_index, long edge1_index);

    bool operator==(const EdgeEdgeCandidate& other) const;
    bool operator!=(const EdgeEdgeCandidate& other) const
    {
        return !(*this == other);
    }

    /// @brief Compare EdgeEdgeCandidates for sorting.
    bool operator<(const EdgeEdgeCandidate& other) const;

    bool
    ccd(const Eigen::MatrixXd& V0,
        const Eigen::MatrixXd& V1,
        const Eigen::MatrixXi& E,
        const Eigen::MatrixXi& F,
        double& toi,
        const double tmax = 1.0,
        const double tolerance = DEFAULT_CCD_TOLERANCE,
        const long max_iterations = DEFAULT_CCD_MAX_ITERATIONS,
        const double conservative_rescaling =
            DEFAULT_CCD_CONSERVATIVE_RESCALING) const override;

    long edge0_index;
    long edge1_index;
};

/// @brief Candidate for <b>intersection</b> between edge and face.
///
/// Not included in Candidates because it is not a collision candidate.
struct EdgeFaceCandidate {
    EdgeFaceCandidate(long edge_index, long face_index);

    bool operator==(const EdgeFaceCandidate& other) const;
    bool operator!=(const EdgeFaceCandidate& other) const
    {
        return !(*this == other);
    }

    /// @brief Compare EdgeFaceCandidate for sorting.
    bool operator<(const EdgeFaceCandidate& other) const;

    long edge_index;
    long face_index;
};

struct FaceVertexCandidate : ContinuousCollisionCandidate {
    FaceVertexCandidate(long face_index, long vertex_index);

    bool operator==(const FaceVertexCandidate& other) const;
    bool operator!=(const FaceVertexCandidate& other) const
    {
        return !(*this == other);
    }

    /// @brief Compare FaceVertexCandidate for sorting.
    bool operator<(const FaceVertexCandidate& other) const;

    bool
    ccd(const Eigen::MatrixXd& V0,
        const Eigen::MatrixXd& V1,
        const Eigen::MatrixXi& E,
        const Eigen::MatrixXi& F,
        double& toi,
        const double tmax = 1.0,
        const double tolerance = DEFAULT_CCD_TOLERANCE,
        const long max_iterations = DEFAULT_CCD_MAX_ITERATIONS,
        const double conservative_rescaling =
            DEFAULT_CCD_CONSERVATIVE_RESCALING) const override;

    long face_index;
    long vertex_index;
};

struct Candidates {
    std::vector<EdgeVertexCandidate> ev_candidates;
    std::vector<EdgeEdgeCandidate> ee_candidates;
    std::vector<FaceVertexCandidate> fv_candidates;

    size_t size() const;

    bool empty() const;

    void clear();

    ContinuousCollisionCandidate& operator[](size_t idx);
    const ContinuousCollisionCandidate& operator[](size_t idx) const;

    bool save_obj(
        const std::string& filename,
        const Eigen::MatrixXd& V,
        const Eigen::MatrixXi& E,
        const Eigen::MatrixXi& F);
};

////////////////////////////////////////////////////////////////////////////////
// Debugging functions

void save_obj(
    std::ofstream& out,
    const Eigen::MatrixXd& V,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    const std::vector<EdgeVertexCandidate>& ev_candidates);
void save_obj(
    std::ofstream& out,
    const Eigen::MatrixXd& V,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    const std::vector<EdgeEdgeCandidate>& ee_candidates);
void save_obj(
    std::ofstream& out,
    const Eigen::MatrixXd& V,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    const std::vector<FaceVertexCandidate>& fv_candidates);
void save_obj(
    std::ofstream& out,
    const Eigen::MatrixXd& V,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    const std::vector<EdgeFaceCandidate>& ef_candidates);

template <typename Candidate>
bool save_obj(
    const std::string& filename,
    const Eigen::MatrixXd& V,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    const std::vector<Candidate>& candidates)
{
    std::ofstream obj(filename);
    if (!obj.is_open()) {
        return false;
    }
    save_obj(obj, V, E, F, candidates);
    return true;
}

} // namespace ipc


<|start_filename|>src/utils/logger.cpp<|end_filename|>
#ifdef IPC_TOOLKIT_WITH_LOGGER
#include <iostream>
#include <memory>

#include <ipc/utils/logger.hpp>

#include <spdlog/details/registry.h>
#include <spdlog/details/thread_pool.h>
#include <spdlog/sinks/basic_file_sink.h>
#include <spdlog/sinks/ostream_sink.h>
#include <spdlog/sinks/stdout_color_sinks.h>

namespace ipc {

std::shared_ptr<spdlog::async_logger> Logger::_logger;

// See https://github.com/gabime/spdlog#asynchronous-logger-with-multi-sinks
void Logger::init(std::vector<spdlog::sink_ptr>& sinks)
{
    auto l = spdlog::get("ipc_toolkit");
    bool had_ipc_toolkit = l != nullptr;
    if (had_ipc_toolkit) {
        spdlog::drop("ipc_toolkit");
    }

    if (spdlog::thread_pool() == nullptr) {
        spdlog::init_thread_pool(8192, 1);
    }
    Logger::_logger = std::make_shared<spdlog::async_logger>(
        "ipc_toolkit", sinks.begin(), sinks.end(), spdlog::thread_pool(),
        spdlog::async_overflow_policy::block);
    spdlog::register_logger(_logger);

    if (had_ipc_toolkit) {
        logger().warn("Removed another ipc toolkit logger");
    }
}

void Logger::init(bool use_cout, const std::string& filename, bool truncate)
{
    std::vector<spdlog::sink_ptr> sinks;
    if (use_cout) {
        sinks.emplace_back(
            std::make_shared<spdlog::sinks::stdout_color_sink_mt>());
    }
    if (!filename.empty()) {
        sinks.emplace_back(std::make_shared<spdlog::sinks::basic_file_sink_mt>(
            filename, truncate));
    }

    init(sinks);
}

void Logger::init(std::ostream& os)
{
    std::vector<spdlog::sink_ptr> sinks;
    sinks.emplace_back(
        std::make_shared<spdlog::sinks::ostream_sink_mt>(os, false));

    init(sinks);
}

} // namespace ipc
#endif


<|start_filename|>src/ipc.hpp<|end_filename|>
#pragma once

#include <Eigen/Core>
#include <Eigen/Sparse>

#include <ipc/collision_constraint.hpp>
// NOTE: Include this so the user can just include ipc.hpp
#include <ipc/friction/friction.hpp>
#include <ipc/broad_phase/broad_phase.hpp>

/// Incremental Potential Contact functions
namespace ipc {

/// @brief Construct a set of constraints used to compute the barrier potential.
///
/// All vertices in V will be considered for collisions, so V should be only the
/// surface vertices. The edges and face should be only for the surface
/// elements.
///
/// @param[in] V Vertex positions as rows of a matrix.
/// @param[in] E Edges as rows of indicies into V.
/// @param[in] F Triangular faces as rows of indicies into V.
/// @param[in] dhat The activation distance of the barrier.
/// @param[out] constraint_set
///     The constructed set of constraints (any existing constraints will be
///     cleared).
/// @param[in] F2E Map from F edges to rows of E.
/// @param[in] dmin Minimum distance.
/// @param[in] method Broad-phase method to use.
/// @param[in] can_collide
///     A function that takes two vertex IDs (row numbers in V) and returns true
///     if the vertices (and faces or edges containing the vertices) can
///     collide. By default all primitives can collide with all other
///     primitives.
void construct_constraint_set(
    const Eigen::MatrixXd& V_rest,
    const Eigen::MatrixXd& V,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    const double dhat,
    Constraints& constraint_set,
    const Eigen::MatrixXi& F2E = Eigen::MatrixXi(),
    const double dmin = 0,
    const BroadPhaseMethod& method = BroadPhaseMethod::HASH_GRID,
    const std::function<bool(size_t, size_t)>& can_collide =
        [](size_t, size_t) { return true; });

/// @brief Construct a set of constraints used to compute the barrier potential.
///
/// V can either be the surface vertices or the entire mesh vertices. The edges
/// and face should be only for the surface elements.
///
/// @param[in] V Vertex positions as rows of a matrix.
/// @param[in] codim_V
///     Vertex indices of codimensional points. Pass `Eigen::VectorXi()` to
///     ignore all codimensional vertices (vertices not connected to an edge).
/// @param[in] E Edges as rows of indicies into V.
/// @param[in] F Triangular faces as rows of indicies into V.
/// @param[in] dhat The activation distance of the barrier.
/// @param[out] constraint_set
///     The constructed set of constraints (any existing constraints will be
///     cleared).
/// @param[in] F2E Map from F edges to rows of E.
/// @param[in] dmin Minimum distance.
/// @param[in] method Broad-phase method to use.
/// @param[in] can_collide
///     A function that takes two vertex IDs (row numbers in V) and returns true
///     if the vertices (and faces or edges containing the vertices) can
///     collide. By default all primitives can collide with all other
///     primitives.
void construct_constraint_set(
    const Eigen::MatrixXd& V_rest,
    const Eigen::MatrixXd& V,
    const Eigen::VectorXi& codim_V,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    const double dhat,
    Constraints& constraint_set,
    const Eigen::MatrixXi& F2E = Eigen::MatrixXi(),
    const double dmin = 0,
    const BroadPhaseMethod& method = BroadPhaseMethod::HASH_GRID,
    const std::function<bool(size_t, size_t)>& can_collide =
        [](size_t, size_t) { return true; });

/// @brief Construct a set of constraints used to compute the barrier potential.
///
/// V can either be the surface vertices or the entire mesh vertices. The edges
/// and face should be only for the surface elements.
///
/// @param[in]  candidates  Distance candidates from which the constraint set is
///                         built.
/// @param[in]  V  Vertex positions as rows of a matrix.
/// @param[in]  E  Edges as rows of indicies into V.
/// @param[in]  F  Triangular faces as rows of indicies into V.
/// @param[in]  dhat  The activation distance of the barrier.
/// @param[out] constraint_set
///     The constructed set of constraints (any existing constraints will be
///     cleared).
/// @param[in]  F2E  Map from F edges to rows of E.
/// @param[in]  dmin  Minimum distance.
void construct_constraint_set(
    const Candidates& candidates,
    const Eigen::MatrixXd& V_rest,
    const Eigen::MatrixXd& V,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    const double dhat,
    Constraints& constraint_set,
    const Eigen::MatrixXi& F2E = Eigen::MatrixXi(),
    const double dmin = 0);

/// @brief Compute the barrier potential for a given constraint set.
///
/// @param[in] V Vertex positions as rows of a matrix.
/// @param[in] E Edges as rows of indicies into V.
/// @param[in] F Triangular faces as rows of indicies into V.
/// @param[in] constraint_set The set of constraints.
/// @param[in] dhat The activation distance of the barrier.
/// @returns The sum of all barrier potentials (not scaled by the barrier
/// stiffness).
double compute_barrier_potential(
    const Eigen::MatrixXd& V,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    const Constraints& constraint_set,
    const double dhat);

/// @brief Compute the gradient of the barrier potential.
///
/// @param[in] V Vertex positions as rows of a matrix.
/// @param[in] E Edges as rows of indicies into V.
/// @param[in] F Triangular faces as rows of indicies into V.
/// @param[in] constraint_set The set of constraints.
/// @param[in] dhat The activation distance of the barrier.
/// @returns The gradient of all barrier potentials (not scaled by the barrier
/// stiffness).
Eigen::VectorXd compute_barrier_potential_gradient(
    const Eigen::MatrixXd& V,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    const Constraints& constraint_set,
    const double dhat);

/// @brief Compute the hessian of the barrier potential.
///
/// @param[in] V Vertex positions as rows of a matrix.
/// @param[in] E Edges as rows of indicies into V.
/// @param[in] F Triangular faces as rows of indicies into V.
/// @param[in] constraint_set The set of constraints.
/// @param[in] dhat The activation distance of the barrier.
/// @param[in] project_hessian_to_psd Make sure the hessian is positive
///            semi-definite.
/// @returns The hessian of all barrier potentials (not scaled by the barrier
/// stiffness).
Eigen::SparseMatrix<double> compute_barrier_potential_hessian(
    const Eigen::MatrixXd& V,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    const Constraints& constraint_set,
    const double dhat,
    const bool project_hessian_to_psd = true);

///////////////////////////////////////////////////////////////////////////////
// Collision detection

/// @brief Determine if the step is collision free.
///
/// All vertices in V0 and V1 will be considered for collisions, so V0 and
/// V1 should be only the surface vertices. The edges and face should be only
/// for the surface elements.
///
/// @note Assumes the trajectory is linear.
///
/// @param[in] V0 Vertex positions at start as rows of a matrix.
/// @param[in] V1 Vertex positions at end as rows of a matrix.
/// @param[in] E Edges as rows of indicies into V.
/// @param[in] F Triangular faces as rows of indicies into V.
/// @param[in] can_collide
///     A function that takes two vertex IDs (row numbers in F) and returns true
///     if the vertices (and faces or edges containing the vertices) can
///     collide. By default all primitives can collide with all other primiti
/// @returns True if <b>any</b> collisions occur.
bool is_step_collision_free(
    const Eigen::MatrixXd& V0,
    const Eigen::MatrixXd& V1,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    const BroadPhaseMethod& method = BroadPhaseMethod::HASH_GRID,
    const double tolerance = 1e-6,
    const long max_iterations = 1e7,
    const std::function<bool(size_t, size_t)>& can_collide =
        [](size_t, size_t) { return true; });

/// @brief Determine if the step is collision free.
///
/// V can either be the surface vertices or the entire mesh vertices. The edges
/// and face should be only for the surface elements.
///
/// @note Assumes the trajectory is linear.
///
/// @param[in] V0 Vertex positions at start as rows of a matrix.
/// @param[in] V1 Vertex positions at end as rows of a matrix.
/// @param[in] codim_V Vertex indices of codimensional points. Pass
///     Eigen::VectorXi() to ignore all codimensional vertices (vertices not
///     connected to an edge).
/// @param[in] E Edges as rows of indicies into V.
/// @param[in] F Triangular faces as rows of indicies into V.
/// @param[in] can_collide
///     A function that takes two vertex IDs (row numbers in F) and returns true
///     if the vertices (and faces or edges containing the vertices) can
///     collide. By default all primitives can collide with all other primiti
/// @returns True if <b>any</b> collisions occur.
bool is_step_collision_free(
    const Eigen::MatrixXd& V0,
    const Eigen::MatrixXd& V1,
    const Eigen::VectorXi& codim_V,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    const BroadPhaseMethod& method = BroadPhaseMethod::HASH_GRID,
    const double tolerance = 1e-6,
    const long max_iterations = 1e7,
    const std::function<bool(size_t, size_t)>& can_collide =
        [](size_t, size_t) { return true; });

/// @brief Determine if the step is collision free from a set of candidates.
///
/// V can either be the surface vertices or the entire mesh vertices. The edges
/// and face should be only for the surface elements.
///
/// @note Assumes the trajectory is linear.
///
/// @param[in] candidates Set of candidates to check for collisions.
/// @param[in] V0 Vertex positions at start as rows of a matrix.
/// @param[in] V1 Vertex positions at end as rows of a matrix.
/// @param[in] E Edges as rows of indicies into V.
/// @param[in] F Triangular faces as rows of indicies into V.
///
/// @returns True if <b>any</b> collisions occur.
bool is_step_collision_free(
    const Candidates& candidates,
    const Eigen::MatrixXd& V0,
    const Eigen::MatrixXd& V1,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    const double tolerance = 1e-6,
    const long max_iterations = 1e7);

/// @brief Computes a maximal step size that is collision free.
///
/// All vertices in V0 and V1 will be considered for collisions, so V0 and
/// V1 should be only the surface vertices. The edges and face should be only
/// for the surface elements.
///
/// @note Assumes the trajectory is linear.
///
/// @param[in] V0
///     Vertex positions at start as rows of a matrix. Assumes V0 is
///     intersection free.
/// @param[in] V1 Vertex positions at end as rows of a matrix.
/// @param[in] E Edges as rows of indicies into V.
/// @param[in] F Triangular faces as rows of indicies into V.
/// @param[in] can_collide
///     A function that takes two vertex IDs (row numbers in F) and returns true
///     if the vertices (and faces or edges containing the vertices) can
///     collide. By default all primitives can collide with all other
///     primitives.
/// @returns A step-size \f$\in [0, 1]\f$ that is collision free. A value of 1.0
/// if a full step and 0.0 is no step.
double compute_collision_free_stepsize(
    const Eigen::MatrixXd& V0,
    const Eigen::MatrixXd& V1,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    const BroadPhaseMethod& method = BroadPhaseMethod::HASH_GRID,
    const double tolerance = 1e-6,
    const long max_iterations = 1e7,
    const std::function<bool(size_t, size_t)>& can_collide =
        [](size_t, size_t) { return true; });

/// @brief Computes a maximal step size that is collision free.
///
/// @note Assumes the trajectory is linear.
///
/// @param[in] V0
///     Vertex positions at start as rows of a matrix. Assumes V0 is
///     intersection free.
/// @param[in] V1 Vertex positions at end as rows of a matrix.
/// @param[in] codim_V Vertex indices of codimensional points. Pass
///     Eigen::VectorXi() to ignore all codimensional vertices (vertices not
///     connected to an edge).
/// @param[in] E Edges as rows of indicies into V.
/// @param[in] F Triangular faces as rows of indicies into V.
/// @param[in] can_collide
///     A function that takes two vertex IDs (row numbers in F) and returns true
///     if the vertices (and faces or edges containing the vertices) can
///     collide. By default all primitives can collide with all other
///     primitives.
/// @returns A step-size \f$\in [0, 1]\f$ that is collision free. A value of 1.0
/// if a full step and 0.0 is no step.
double compute_collision_free_stepsize(
    const Eigen::MatrixXd& V0,
    const Eigen::MatrixXd& V1,
    const Eigen::VectorXi& codim_V,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    const BroadPhaseMethod& method = BroadPhaseMethod::HASH_GRID,
    const double tolerance = 1e-6,
    const long max_iterations = 1e7,
    const std::function<bool(size_t, size_t)>& can_collide =
        [](size_t, size_t) { return true; });

/// @brief Computes a maximal step size that is collision free using a set of
/// collision candidates.
///
/// @note Assumes the trajectory is linear.
///
/// @param[in] candidates Set of candidates to check for collisions.
/// @param[in] V0
///     Vertex positions at start as rows of a matrix. Assumes V0 is
///     intersection free.
/// @param[in] V1 Vertex positions at end as rows of a matrix.
/// @param[in] E Edges as rows of indicies into V.
/// @param[in] F Triangular faces as rows of indicies into V.
////
/// @returns A step-size \f$\in [0, 1]\f$ that is collision free. A value of 1.0
/// if a full step and 0.0 is no step.
double compute_collision_free_stepsize(
    const Candidates& candidates,
    const Eigen::MatrixXd& V0,
    const Eigen::MatrixXd& V1,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    const double tolerance = 1e-6,
    const long max_iterations = 1e7);

///////////////////////////////////////////////////////////////////////////////
// Utilities

/// @brief Computes the minimum distance between any non-adjacent elements.
///
/// @param[in] V Vertex positions as rows of a matrix.
/// @param[in] E Edges as rows of indicies into V.
/// @param[in] F Triangular faces as rows of indicies into V.
/// @returns The minimum distance between any non-adjacent elements.
double compute_minimum_distance(
    const Eigen::MatrixXd& V,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    const Constraints& constraint_set);

/// @brief Determine if the mesh has self intersections.
///
/// @param[in] V Vertex positions as rows of a matrix.
/// @param[in] E Edges as rows of indicies into V.
/// @param[in] F Triangular faces as rows of indicies into V.
/// @param[in] can_collide
///     A function that takes two vertex IDs (row numbers in F) and returns true
///     if the vertices (and faces or edges containing the vertices) can
///     collide. By default all primitives can collide with all other
///     primitives.
bool has_intersections(
    const Eigen::MatrixXd& V,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    const std::function<bool(size_t, size_t)>& can_collide =
        [](size_t, size_t) { return true; });

} // namespace ipc


<|start_filename|>docs/source/_static/css/custom.css<|end_filename|>
dl.function, dl.class, dl.enum, dl.struct {
    margin-bottom: 1.25em;
    padding-bottom: 1.25em;
    border-bottom: solid black 1px;
}

dt.py {
    margin-bottom: 0.5em;
}

/*
em.sig-param::before {
    content: "\A    ";
    white-space: pre;
}
*/

<|start_filename|>tests/distance/test_distance_type.cpp<|end_filename|>
#include <catch2/catch.hpp>

#include <ipc/distance/distance_type.hpp>
#include <ipc/distance/point_triangle.hpp>

using namespace ipc;

inline Eigen::Vector2d
edge_normal(const Eigen::Vector2d& e0, const Eigen::Vector2d& e1)
{
    Eigen::Vector2d e = e1 - e0;
    Eigen::Vector2d normal(-e.y(), e.x());
    return normal.normalized();
}

TEST_CASE("Point-edge distance type", "[distance][distance-type][point-edge]")
{
    // TODO: Implement tests
}

struct RandomBarycentricCoordGenerator
    : Catch::Generators::IGenerator<Eigen::Vector3d> {
    Eigen::Vector3d bc;

    RandomBarycentricCoordGenerator() { next(); }

    // via GeneratorUntypedBase:
    // Attempts to move the generator to the next element.
    // Returns true if successful (and thus has another element that can be
    // read)
    bool next() override
    {
        double margin = 1e-8;
        bc.head<2>() =
            (Eigen::Array2d::Random() + 1.0) / 2.0 * (1.0 - 2.0 * margin)
            + margin;
        if (bc(0) + bc(1) >= 1) {
            bc(0) = 1 - bc(0);
            bc(1) = 1 - bc(1);
        }
        bc(2) = 1 - bc(0) - bc(1);
        return true;
    }

    // Precondition:
    // The generator is either freshly constructed or the last call to next()
    // returned true
    Eigen::Vector3d const& get() const override { return bc; }
};

TEST_CASE(
    "Point-triangle distance type", "[distance][distance-type][point-triangle]")
{
    double py = GENERATE(-10, -1, -1e-12, 0, 1e-12, 1, 10);
    Eigen::Vector3d p(0, py, 0);
    Eigen::Vector3d t0(-1, 0, 1);
    Eigen::Vector3d t1(1, 0, 1);
    Eigen::Vector3d t2(0, 0, -1);

    PointTriangleDistanceType expected_dtype;

    SECTION("closest to triangle")
    {
        double pz = GENERATE(0, -1 + 1e-12, 1 - 1e-12);
        p.z() = pz;
        expected_dtype = PointTriangleDistanceType::P_T;
    }
    SECTION("random closest to triangle")
    {
        Eigen::Vector3d bc = GENERATE(take(
            100,
            Catch::Generators::GeneratorWrapper<Eigen::Vector3d>(
                std::unique_ptr<RandomBarycentricCoordGenerator>(
                    new RandomBarycentricCoordGenerator()))));
        Eigen::Vector3d point_in_plane = bc(0) * t0 + bc(1) * t1 + bc(2) * t2;
        p.x() = point_in_plane.x();
        p.z() = point_in_plane.z();
        expected_dtype = PointTriangleDistanceType::P_T;
    }
    SECTION("closest to t0")
    {
        double delta = GENERATE(1e-8, 1e-4, 0.1, 11);
        p.x() = t0.x() - delta;
        p.z() = t0.z() + delta;
        expected_dtype = PointTriangleDistanceType::P_T0;
    }
    SECTION("closest to t1")
    {
        double delta = GENERATE(1e-8, 1e-4, 0.1, 11);
        p.x() = t1.x() + delta;
        p.z() = t1.z() + delta;
        expected_dtype = PointTriangleDistanceType::P_T1;
    }
    SECTION("closest to t2")
    {
        double pz = GENERATE(-1 - 1e-12, -1.1, -11);
        p.z() = pz;
        expected_dtype = PointTriangleDistanceType::P_T2;
    }
    SECTION("closest to t0t1")
    {
        double alpha = GENERATE(1e-4, 0.5, 1.0 - 1e-4);
        Eigen::Vector3d closest_point = (t1 - t0) * alpha + t0;
        Eigen::Vector2d perp = edge_normal(
            Eigen::Vector2d(t0.x(), t0.z()), Eigen::Vector2d(t1.x(), t1.z()));
        double scale = GENERATE(1e-12, 1e-4, 1, 2, 11, 1000);
        p.x() = closest_point.x() + scale * perp.x();
        p.z() = closest_point.z() + scale * perp.y();
        expected_dtype = PointTriangleDistanceType::P_E0;
    }
    SECTION("random closest to t0t1")
    {
        double alpha = GENERATE(take(100, random(1e-8, 1 - 1e-8)));
        Eigen::Vector3d closest_point = (t1 - t0) * alpha + t0;
        Eigen::Vector2d perp = edge_normal(
            Eigen::Vector2d(t0.x(), t0.z()), Eigen::Vector2d(t1.x(), t1.z()));
        double scale = GENERATE(take(100, random(1e-12, 1e4)));
        p.x() = closest_point.x() + scale * perp.x();
        p.z() = closest_point.z() + scale * perp.y();
        expected_dtype = PointTriangleDistanceType::P_E0;
    }
    SECTION("closest to t1t2")
    {
        double alpha = GENERATE(1e-4, 0.5, 1.0 - 1e-4);
        Eigen::Vector3d closest_point = (t2 - t1) * alpha + t1;
        Eigen::Vector2d perp = edge_normal(
            Eigen::Vector2d(t1.x(), t1.z()), Eigen::Vector2d(t2.x(), t2.z()));
        double scale = GENERATE(1e-12, 1e-4, 1, 2, 11, 1000);
        p.x() = closest_point.x() + scale * perp.x();
        p.z() = closest_point.z() + scale * perp.y();
        expected_dtype = PointTriangleDistanceType::P_E1;
    }
    SECTION("random closest to t1t2")
    {
        double alpha = GENERATE(take(100, random(1e-8, 1 - 1e-8)));
        Eigen::Vector3d closest_point = (t2 - t1) * alpha + t1;
        Eigen::Vector2d perp = edge_normal(
            Eigen::Vector2d(t1.x(), t1.z()), Eigen::Vector2d(t2.x(), t2.z()));
        double scale = GENERATE(take(100, random(1e-12, 1e4)));
        p.x() = closest_point.x() + scale * perp.x();
        p.z() = closest_point.z() + scale * perp.y();
        expected_dtype = PointTriangleDistanceType::P_E1;
    }
    SECTION("closest to t2t0")
    {
        double alpha = GENERATE(1e-4, 0.5, 1.0 - 1e-4);
        Eigen::Vector3d closest_point = (t0 - t2) * alpha + t2;
        Eigen::Vector2d perp = edge_normal(
            Eigen::Vector2d(t2.x(), t2.z()), Eigen::Vector2d(t0.x(), t0.z()));
        double scale = GENERATE(1e-12, 1e-4, 1, 2, 11, 1000);
        p.x() = closest_point.x() + scale * perp.x();
        p.z() = closest_point.z() + scale * perp.y();
        expected_dtype = PointTriangleDistanceType::P_E2;
    }
    SECTION("random closest to t2t0")
    {
        double alpha = GENERATE(take(100, random(1e-8, 1 - 1e-8)));
        Eigen::Vector3d closest_point = (t0 - t2) * alpha + t2;
        Eigen::Vector2d perp = edge_normal(
            Eigen::Vector2d(t2.x(), t2.z()), Eigen::Vector2d(t0.x(), t0.z()));
        double scale = GENERATE(take(100, random(1e-12, 1e4)));
        p.x() = closest_point.x() + scale * perp.x();
        p.z() = closest_point.z() + scale * perp.y();
        expected_dtype = PointTriangleDistanceType::P_E2;
    }

    PointTriangleDistanceType dtype =
        point_triangle_distance_type(p, t0, t1, t2);
    CHECK(dtype == expected_dtype);
}

TEST_CASE(
    "Point-triangle distance type GH Issue",
    "[distance][distance-type][point-triangle][debug]")
{
    Eigen::Vector3d p(0.488166, 0.0132623, 0.289055);
    Eigen::Vector3d v0(0.456476, 0.0526442, 0.260834);
    Eigen::Vector3d v1(0.609111, 0.0595969, 0.275928);
    Eigen::Vector3d v2(0.431262, 0.0508414, 0.255831);

    CHECK(
        point_triangle_distance_type(p, v0, v1, v2)
        == PointTriangleDistanceType::P_E0);
}

TEST_CASE("Edge-edge distance type", "[distance][distance-type][edge-edge]")
{
    // TODO: Implement tests
}


<|start_filename|>tests/test_ipc.cpp<|end_filename|>
#include <catch2/catch.hpp>

#include <iostream>

#include <finitediff.hpp>

#include <ipc/ipc.hpp>

#include "test_utils.hpp"

using namespace ipc;

TEST_CASE("Dummy test for IPC compilation", "[!benchmark][ipc]")
{
    double dhat = -1;
    std::string mesh_name;

    SECTION("cube")
    {
        dhat = sqrt(2.0);
        mesh_name = "cube.obj";
    }
    SECTION("bunny")
    {
        dhat = 1e-2;
        mesh_name = "bunny.obj";
    }

    Eigen::MatrixXd V;
    Eigen::MatrixXi E, F;
    bool success = load_mesh(mesh_name, V, E, F);
    REQUIRE(success);

    Constraints constraint_set;
    construct_constraint_set(/*V_rest=*/V, V, E, F, dhat, constraint_set);
    CAPTURE(mesh_name, dhat);
    CHECK(constraint_set.num_constraints() > 0);

    BENCHMARK("Compute barrier potential")
    {
        double b =
            ipc::compute_barrier_potential(V, E, F, constraint_set, dhat);
    };
    BENCHMARK("Compute barrier potential gradient")
    {
        Eigen::VectorXd grad_b = ipc::compute_barrier_potential_gradient(
            V, E, F, constraint_set, dhat);
    };
    BENCHMARK("Compute barrier potential hessian")
    {
        Eigen::MatrixXd hess_b = ipc::compute_barrier_potential_hessian(
            V, E, F, constraint_set, dhat);
    };
    BENCHMARK("Compute compute_minimum_distance")
    {
        double min_dist =
            ipc::compute_minimum_distance(V, E, F, constraint_set);
    };
}

TEST_CASE("Test IPC full gradient", "[ipc][gradient]")
{
    double dhat = -1;
    std::string mesh_name = "";
    bool ignore_codimensional_vertices = false;

    BroadPhaseMethod method = GENERATE(
        BroadPhaseMethod::BRUTE_FORCE, BroadPhaseMethod::HASH_GRID //,
        // BroadPhaseMethod::SPATIAL_HASH
    );

    SECTION("cube")
    {
        dhat = sqrt(2.0);
        mesh_name = "cube.obj";
    }
    SECTION("two cubes far")
    {
        dhat = 1e-1;
        mesh_name = "two-cubes-far.obj";
        ignore_codimensional_vertices = true;
    }
    SECTION("two cubes close")
    {
        dhat = 1e-1;
        mesh_name = "two-cubes-close.obj";
        ignore_codimensional_vertices = true;
    }
    // SECTION("bunny")
    // {
    //     dhat = 1e-2;
    //     mesh_name = "bunny.obj";
    // }

    Eigen::MatrixXd V;
    Eigen::MatrixXi E, F;
    bool success = load_mesh(mesh_name, V, E, F);
    CAPTURE(mesh_name);
    REQUIRE(success);

    Constraints constraint_set;
    if (ignore_codimensional_vertices) {
        ipc::construct_constraint_set(
            /*V_rest=*/V, V, /*codim_V=*/Eigen::VectorXi(), E, F, dhat,
            constraint_set, /*F2E=*/Eigen::MatrixXi(), /*dmin=*/0, method);
    } else {
        ipc::construct_constraint_set(
            /*V_rest=*/V, V, E, F, dhat, constraint_set,
            /*F2E=*/Eigen::MatrixXi(), /*dmin=*/0, method);
    }
    CAPTURE(dhat, method, ignore_codimensional_vertices);
    CHECK(constraint_set.num_constraints() > 0);

    Eigen::VectorXd grad_b =
        ipc::compute_barrier_potential_gradient(V, E, F, constraint_set, dhat);

    // Compute the gradient using finite differences
    auto f = [&](const Eigen::VectorXd& x) {
        return ipc::compute_barrier_potential(
            unflatten(x, V.cols()), E, F, constraint_set, dhat);
    };
    Eigen::VectorXd fgrad_b;
    fd::finite_gradient(flatten(V), f, fgrad_b);

    REQUIRE(grad_b.squaredNorm() > 1e-8);
    CHECK(fd::compare_gradient(grad_b, fgrad_b));
}

TEST_CASE("Test IPC full hessian", "[ipc][hessian]")
{
    double dhat = -1;
    std::string mesh_name = "";
    bool ignore_codimensional_vertices = false;

    BroadPhaseMethod method = GENERATE(
        BroadPhaseMethod::BRUTE_FORCE, BroadPhaseMethod::HASH_GRID //,
        // BroadPhaseMethod::SPATIAL_HASH
    );

    // SECTION("cube")
    // {
    //     dhat = sqrt(2.0);
    //     mesh_name = "cube.obj";
    // }
    SECTION("two cubes far")
    {
        dhat = 1e-1;
        mesh_name = "two-cubes-far.obj";
        ignore_codimensional_vertices = true;
    }
    SECTION("two cubes close")
    {
        dhat = 1e-1;
        mesh_name = "two-cubes-close.obj";
        ignore_codimensional_vertices = true;
    }
    // WARNING: The bunny takes too long in debug.
    // SECTION("bunny")
    // {
    //     dhat = 1e-2;
    //     mesh_name = "bunny.obj";
    // }

    Eigen::MatrixXd V;
    Eigen::MatrixXi E, F;
    bool success = load_mesh(mesh_name, V, E, F);
    CAPTURE(mesh_name);
    REQUIRE(success);

    Constraints constraint_set;
    if (ignore_codimensional_vertices) {
        ipc::construct_constraint_set(
            /*V_rest=*/V, V, /*codim_V=*/Eigen::VectorXi(), E, F, dhat,
            constraint_set, /*F2E=*/Eigen::MatrixXi(), /*dmin=*/0, method);
    } else {
        ipc::construct_constraint_set(
            /*V_rest=*/V, V, E, F, dhat, constraint_set,
            /*F2E=*/Eigen::MatrixXi(), /*dmin=*/0, method);
    }
    CAPTURE(dhat, method, ignore_codimensional_vertices);
    CHECK(constraint_set.num_constraints() > 0);

    Eigen::MatrixXd hess_b = ipc::compute_barrier_potential_hessian(
        V, E, F, constraint_set, dhat, /*project_to_psd=*/false);

    // Compute the gradient using finite differences
    auto f = [&](const Eigen::VectorXd& x) {
        return ipc::compute_barrier_potential_gradient(
            unflatten(x, V.cols()), E, F, constraint_set, dhat);
    };
    Eigen::MatrixXd fhess_b;
    fd::finite_jacobian(flatten(V), f, fhess_b);

    REQUIRE(hess_b.squaredNorm() > 1e-3);
    CHECK(fd::compare_hessian(hess_b, fhess_b, 1e-3));
}


<|start_filename|>python/src/distance/distance_type.cpp<|end_filename|>
#include <pybind11/pybind11.h>
#include <pybind11/eigen.h>

#include <ipc/distance/distance_type.hpp>

#include "../utils.hpp"

namespace py = pybind11;
using namespace ipc;

void define_distance_type_functions(py::module_& m)
{
    py::enum_<PointEdgeDistanceType>(m, "PointEdgeDistanceType")
        .value(
            "P_E0", PointEdgeDistanceType::P_E0,
            "point is closest to edge vertex zero")
        .value(
            "P_E1", PointEdgeDistanceType::P_E1,
            "point is closest to edge vertex one")
        .value(
            "P_E", PointEdgeDistanceType::P_E,
            "point is closest to the interior of the edge")
        .export_values();

    py::enum_<PointTriangleDistanceType>(m, "PointTriangleDistanceType")
        .value(
            "P_T0", PointTriangleDistanceType::P_T0,
            "point is closest to triangle vertex zero")
        .value(
            "P_T1", PointTriangleDistanceType::P_T1,
            "point is closest to triangle vertex one")
        .value(
            "P_T2", PointTriangleDistanceType::P_T2,
            "point is closest to triangle vertex two")
        .value(
            "P_E0", PointTriangleDistanceType::P_E0,
            "point is closest to triangle edge zero (vertex zero to one)")
        .value(
            "P_E1", PointTriangleDistanceType::P_E1,
            "point is closest to triangle edge one (vertex one to two)")
        .value(
            "P_E2", PointTriangleDistanceType::P_E2,
            "point is closest to triangle edge two (vertex two to zero)")
        .value(
            "P_T", PointTriangleDistanceType::P_T,
            "point is closest to the interior of the triangle")
        .export_values();

    py::enum_<EdgeEdgeDistanceType>(m, "EdgeEdgeDistanceType")
        .value(
            "EA0_EB0", EdgeEdgeDistanceType::EA0_EB0,
            "edges are closest at vertex 0 of edge A and 0 of edge B")
        .value(
            "EA0_EB1", EdgeEdgeDistanceType::EA0_EB1,
            "edges are closest at vertex 0 of edge A and 1 of edge B")
        .value(
            "EA1_EB0", EdgeEdgeDistanceType::EA1_EB0,
            "edges are closest at vertex 1 of edge A and 0 of edge B")
        .value(
            "EA1_EB1", EdgeEdgeDistanceType::EA1_EB1,
            "edges are closest at vertex 1 of edge A and 1 of edge B")
        .value(
            "EA_EB0", EdgeEdgeDistanceType::EA_EB0,
            "edges are closest at the interior of edge A and vertex 0 "
            "of edge B")
        .value(
            "EA_EB1", EdgeEdgeDistanceType::EA_EB1,
            "edges are closest at the interior of edge A and vertex 1 "
            "of edge B")
        .value(
            "EA0_EB", EdgeEdgeDistanceType::EA0_EB,
            "edges are closest at vertex 0 of edge A and the interior "
            "of edge B")
        .value(
            "EA1_EB", EdgeEdgeDistanceType::EA1_EB,
            "edges are closest at vertex 1 of edge A and the interior "
            "of edge B")
        .value(
            "EA_EB", EdgeEdgeDistanceType::EA_EB,
            "edges are closest at an interior point of edge A and B")
        .export_values();

    m.def(
        "point_edge_distance_type",
        [](const VectorMax3d& p, const VectorMax3d& e0, const VectorMax3d& e1) {
            assert_2D_or_3D_vector(p, "p");
            assert_2D_or_3D_vector(e0, "e0");
            assert_2D_or_3D_vector(e1, "e1");
            return point_edge_distance_type(p, e0, e1);
        },
        R"ipc_Qu8mg5v7(
        Determine the closest pair between a point and edge.

        Parameters:
            p: point
            e0: first vertex of the edge
            e1: second vertex of the edge

        Returns:
            The distance type of the point-edge pair.
        )ipc_Qu8mg5v7",
        py::arg("p"), py::arg("e0"), py::arg("e1"));

    m.def(
        "point_triangle_distance_type",
        [](const Eigen::Vector3d& p, const Eigen::Vector3d& t0,
           const Eigen::Vector3d& t1, const Eigen::Vector3d& t2) {
            return point_triangle_distance_type(p, t0, t1, t2);
        },
        R"ipc_Qu8mg5v7(
        Determine the closest pair between a point and triangle.

        Parameters:
            p: point
            t0: first vertex of the triangle
            t1: second vertex of the triangle
            t2: third vertex of the triangle

        Returns:
            The distance type of the point-triangle pair.
        )ipc_Qu8mg5v7",
        py::arg("p"), py::arg("t0"), py::arg("t1"), py::arg("t2"));

    m.def(
        "edge_edge_distance_type",
        [](const Eigen::Vector3d& ea0, const Eigen::Vector3d& ea1,
           const Eigen::Vector3d& eb0, const Eigen::Vector3d& eb1) {
            return edge_edge_distance_type(ea0, ea1, eb0, eb1);
        },
        R"ipc_Qu8mg5v7(
        Determine the closest pair between two edges.

        Parameters:
            ea0: first vertex of the first edge
            ea1: second vertex of the first edge
            eb0: first vertex of the second edge
            eb1: second vertex of the second edge

        Returns:
            The distance type of the edge-edge pair.
        )ipc_Qu8mg5v7",
        py::arg("ea0"), py::arg("ea1"), py::arg("eb0"), py::arg("eb1"));
}


<|start_filename|>src/utils/rational.hpp<|end_filename|>
#pragma once

#ifdef IPC_TOOLKIT_WITH_RATIONAL_INTERSECTION

#include <gmp.h>
#include <iostream>

namespace ipc {

// https://cs.nyu.edu/acsys/cvc3/releases/1.5/doc/rational-gmp_8cpp-source.html
class Rational {
public:
    mpq_t value;
    void canonicalize() { mpq_canonicalize(value); }
    int get_sign() const { return mpq_sgn(value); }

    Rational()
    {
        mpq_init(value);
        mpq_set_d(value, 0);
    }

    Rational(double d)
    {
        mpq_init(value);
        mpq_set_d(value, d);
        // canonicalize();
    }

    Rational(const mpq_t& v_)
    {
        mpq_init(value);
        mpq_set(value, v_);
        // canonicalize();
    }

    Rational(const Rational& other)
    {
        mpq_init(value);
        mpq_set(value, other.value);
    }

    ~Rational() { mpq_clear(value); }

    friend Rational operator-(const Rational& v)
    {
        Rational r_out;
        mpq_neg(r_out.value, v.value);
        return r_out;
    }

    friend Rational operator+(const Rational& x, const Rational& y)
    {
        Rational r_out;
        mpq_add(r_out.value, x.value, y.value);
        return r_out;
    }

    friend Rational operator-(const Rational& x, const Rational& y)
    {
        Rational r_out;
        mpq_sub(r_out.value, x.value, y.value);
        return r_out;
    }

    friend Rational operator*(const Rational& x, const Rational& y)
    {
        Rational r_out;
        mpq_mul(r_out.value, x.value, y.value);
        return r_out;
    }

    friend Rational operator/(const Rational& x, const Rational& y)
    {
        Rational r_out;
        mpq_div(r_out.value, x.value, y.value);
        return r_out;
    }

    Rational& operator=(const Rational& x)
    {
        if (this == &x)
            return *this;
        mpq_set(value, x.value);
        return *this;
    }

    Rational& operator=(const double x)
    {
        mpq_set_d(value, x);
        // canonicalize();
        return *this;
    }

    friend bool operator<(const Rational& r, const Rational& r1)
    {
        return mpq_cmp(r.value, r1.value) < 0;
    }

    friend bool operator>(const Rational& r, const Rational& r1)
    {
        return mpq_cmp(r.value, r1.value) > 0;
    }

    friend bool operator<=(const Rational& r, const Rational& r1)
    {
        return mpq_cmp(r.value, r1.value) <= 0;
    }

    friend bool operator>=(const Rational& r, const Rational& r1)
    {
        return mpq_cmp(r.value, r1.value) >= 0;
    }

    friend bool operator==(const Rational& r, const Rational& r1)
    {
        return mpq_equal(r.value, r1.value);
    }

    friend bool operator!=(const Rational& r, const Rational& r1)
    {
        return !mpq_equal(r.value, r1.value);
    }

    double to_double() { return mpq_get_d(value); }

    friend std::ostream& operator<<(std::ostream& os, const Rational& r)
    {
        os << mpq_get_d(r.value);
        return os;
    }
};

} // namespace ipc

#endif


<|start_filename|>src/utils/vertex_to_min_edge.cpp<|end_filename|>
#include <ipc/utils/vertex_to_min_edge.hpp>

#include <algorithm> // std::min/max

namespace ipc {

std::vector<size_t>
vertex_to_min_edge(size_t num_vertices, const Eigen::MatrixXi& E)
{
    std::vector<size_t> V2E(num_vertices, E.rows() + 1);
    // Column first because colmajor
    for (size_t ej = 0; ej < E.cols(); ej++) {
        for (size_t ei = 0; ei < E.rows(); ei++) {
            const size_t& vi = E(ei, ej);
            V2E[vi] = std::min(V2E[vi], ei);
        }
    }
    return V2E;
}

} // namespace ipc


<|start_filename|>src/ipc.cpp<|end_filename|>
#include <ipc/ipc.hpp>

#include <stdexcept> // std::runtime_error
#include <algorithm> // std::min/max

#define IPC_EARLIEST_TOI_USE_MUTEX
#ifdef IPC_EARLIEST_TOI_USE_MUTEX
#include <mutex>
#endif
#include <tbb/parallel_for.h>
#include <tbb/blocked_range.h>
#include <tbb/enumerable_thread_specific.h>

#include <igl/predicates/segment_segment_intersect.h>

#include <ipc/ccd/ccd.hpp>
#include <ipc/distance/edge_edge.hpp>
#include <ipc/distance/edge_edge_mollifier.hpp>
#include <ipc/distance/point_edge.hpp>
#include <ipc/distance/point_triangle.hpp>
#include <ipc/broad_phase/hash_grid.hpp>
#include <ipc/utils/local_to_global.hpp>
#include <ipc/utils/intersection.hpp>
#include <ipc/utils/world_bbox_diagonal_length.hpp>
#include <ipc/utils/unordered_map_and_set.hpp>

namespace ipc {

/// Find the index of the undirected edge (e0, e1)
long find_edge(const Eigen::MatrixXi& E, const long e0, const long e1)
{
    for (long i = 0; i < E.rows(); i++) {
        if ((E(i, 0) == e0 && E(i, 1) == e1)
            || (E(i, 1) == e0 && E(i, 0) == e1)) {
            return i;
        }
    }
    throw std::runtime_error("Unable to find edge!");
}

template <typename Hash>
void add_vertex_vertex_constraint(
    std::vector<VertexVertexConstraint>& vv_constraints,
    unordered_map<VertexVertexConstraint, long, Hash>& vv_to_index,
    const long v0i,
    const long v1i)
{
    VertexVertexConstraint vv_constraint(v0i, v1i);
    auto found_item = vv_to_index.find(vv_constraint);
    if (found_item != vv_to_index.end()) {
        // Constraint already exists, so increase multiplicity
        vv_constraints[found_item->second].multiplicity++;
    } else {
        // New constraint, so add it to the end of vv_constraints
        vv_to_index.emplace(vv_constraint, vv_constraints.size());
        vv_constraints.push_back(vv_constraint);
    }
}

template <typename Hash>
void add_edge_vertex_constraint(
    std::vector<EdgeVertexConstraint>& ev_constraints,
    unordered_map<EdgeVertexConstraint, long, Hash>& ev_to_index,
    const long ei,
    const long vi)
{
    EdgeVertexConstraint ev_constraint(ei, vi);
    auto found_item = ev_to_index.find(ev_constraint);
    if (found_item != ev_to_index.end()) {
        // Constraint already exists, so increase multiplicity
        ev_constraints[found_item->second].multiplicity++;
    } else {
        // New constraint, so add it to the end of vv_constraints
        ev_to_index.emplace(ev_constraint, ev_constraints.size());
        ev_constraints.push_back(ev_constraint);
    }
}

void construct_constraint_set(
    const Eigen::MatrixXd& V_rest,
    const Eigen::MatrixXd& V,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    const double dhat,
    Constraints& constraint_set,
    const Eigen::MatrixXi& F2E,
    const double dmin,
    const BroadPhaseMethod& method,
    const std::function<bool(size_t, size_t)>& can_collide)
{
    double inflation_radius = (dhat + dmin) / 1.99; // Conservative inflation

    Candidates candidates;
    construct_collision_candidates(
        V, E, F, candidates, inflation_radius, method, can_collide);

    construct_constraint_set(
        candidates, V_rest, V, E, F, dhat, constraint_set, F2E, dmin);
}

void construct_constraint_set(
    const Eigen::MatrixXd& V_rest,
    const Eigen::MatrixXd& V,
    const Eigen::VectorXi& codim_V,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    const double dhat,
    Constraints& constraint_set,
    const Eigen::MatrixXi& F2E,
    const double dmin,
    const BroadPhaseMethod& method,
    const std::function<bool(size_t, size_t)>& can_collide)
{
    double inflation_radius = (dhat + dmin) / 1.99; // Conservative inflation

    Candidates candidates;
    construct_collision_candidates(
        V, codim_V, E, F, candidates, inflation_radius, method, can_collide);

    construct_constraint_set(
        candidates, V_rest, V, E, F, dhat, constraint_set, F2E, dmin);
}

void construct_constraint_set(
    const Candidates& candidates,
    const Eigen::MatrixXd& V_rest,
    const Eigen::MatrixXd& V,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    const double dhat,
    Constraints& constraint_set,
    const Eigen::MatrixXi& F2E,
    const double dmin)
{
    constraint_set.clear();

    double dhat_squared = dhat * dhat;
    double dmin_squared = dmin * dmin;

    // Cull the candidates by measuring the distance and dropping those that are
    // greater than dhat.

    // Store the indices to VV and EV pairs to avoid duplicates.
    auto vv_hash = [&V](const VertexVertexConstraint& vv_constraint) -> size_t {
        // The vertex-vertex pair should be order independent
        long min_vi =
            std::min(vv_constraint.vertex0_index, vv_constraint.vertex1_index);
        long max_vi =
            std::max(vv_constraint.vertex0_index, vv_constraint.vertex1_index);
        return size_t(max_vi * V.rows() + min_vi);
    };
    unordered_map<VertexVertexConstraint, long, decltype(vv_hash)> vv_to_index(
        /*min_buckets=*/candidates.size(), vv_hash);

    auto ev_hash = [&V](const EdgeVertexConstraint& ev_constraint) -> size_t {
        // There are max E.rows() * V.rows() constraints
        return size_t(
            ev_constraint.edge_index * V.rows() + ev_constraint.vertex_index);
    };
    unordered_map<EdgeVertexConstraint, long, decltype(ev_hash)> ev_to_index(
        /*min_buckets=*/candidates.size(), ev_hash);

    for (const auto& ev_candidate : candidates.ev_candidates) {
        long vi = ev_candidate.vertex_index;
        long e0i = E(ev_candidate.edge_index, 0);
        long e1i = E(ev_candidate.edge_index, 1);

        PointEdgeDistanceType dtype =
            point_edge_distance_type(V.row(vi), V.row(e0i), V.row(e1i));

        double distance_sqr =
            point_edge_distance(V.row(vi), V.row(e0i), V.row(e1i), dtype);

        if (distance_sqr - dmin_squared < 2 * dmin * dhat + dhat_squared) {
            switch (dtype) {
            case PointEdgeDistanceType::P_E0:
                add_vertex_vertex_constraint(
                    constraint_set.vv_constraints, vv_to_index, vi, e0i);
                break;

            case PointEdgeDistanceType::P_E1:
                add_vertex_vertex_constraint(
                    constraint_set.vv_constraints, vv_to_index, vi, e1i);
                break;

            case PointEdgeDistanceType::P_E:
                // ev_candidates is a set, so no duplicate EV constraints
                constraint_set.ev_constraints.emplace_back(ev_candidate);
                ev_to_index.emplace(
                    constraint_set.ev_constraints.back(),
                    constraint_set.ev_constraints.size() - 1);
                break;
            }
        }
    }

    for (const auto& ee_candidate : candidates.ee_candidates) {
        long eai = ee_candidate.edge0_index, ebi = ee_candidate.edge1_index;
        long ea0i = E(eai, 0), ea1i = E(eai, 1);
        long eb0i = E(ebi, 0), eb1i = E(ebi, 1);

        EdgeEdgeDistanceType dtype = edge_edge_distance_type(
            V.row(ea0i), V.row(ea1i), V.row(eb0i), V.row(eb1i));

        double distance_sqr = edge_edge_distance(
            V.row(ea0i), V.row(ea1i), V.row(eb0i), V.row(eb1i), dtype);

        if (distance_sqr - dmin_squared < 2 * dmin * dhat + dhat_squared) {
            double eps_x = edge_edge_mollifier_threshold(
                V_rest.row(ea0i), V_rest.row(ea1i), //
                V_rest.row(eb0i), V_rest.row(eb1i));
            double ee_cross_norm_sqr = edge_edge_cross_squarednorm(
                V.row(ea0i), V.row(ea1i), V.row(eb0i), V.row(eb1i));
            if (ee_cross_norm_sqr < eps_x) {
                // NOTE: This may not actually be the distance type, but all EE
                // pairs requiring mollification must be mollified later.
                dtype = EdgeEdgeDistanceType::EA_EB;
            }

            switch (dtype) {
            case EdgeEdgeDistanceType::EA0_EB0:
                add_vertex_vertex_constraint(
                    constraint_set.vv_constraints, vv_to_index, ea0i, eb0i);
                break;

            case EdgeEdgeDistanceType::EA0_EB1:
                add_vertex_vertex_constraint(
                    constraint_set.vv_constraints, vv_to_index, ea0i, eb1i);
                break;

            case EdgeEdgeDistanceType::EA1_EB0:
                add_vertex_vertex_constraint(
                    constraint_set.vv_constraints, vv_to_index, ea1i, eb0i);
                break;

            case EdgeEdgeDistanceType::EA1_EB1:
                add_vertex_vertex_constraint(
                    constraint_set.vv_constraints, vv_to_index, ea1i, eb1i);
                break;

            case EdgeEdgeDistanceType::EA_EB0:
                add_edge_vertex_constraint(
                    constraint_set.ev_constraints, ev_to_index, eai, eb0i);
                break;

            case EdgeEdgeDistanceType::EA_EB1:
                add_edge_vertex_constraint(
                    constraint_set.ev_constraints, ev_to_index, eai, eb1i);
                break;

            case EdgeEdgeDistanceType::EA0_EB:
                add_edge_vertex_constraint(
                    constraint_set.ev_constraints, ev_to_index, ebi, ea0i);
                break;

            case EdgeEdgeDistanceType::EA1_EB:
                add_edge_vertex_constraint(
                    constraint_set.ev_constraints, ev_to_index, ebi, ea1i);
                break;

            case EdgeEdgeDistanceType::EA_EB:
                constraint_set.ee_constraints.emplace_back(ee_candidate, eps_x);
                break;
            }
        }
    }

    for (const auto& fv_candidate : candidates.fv_candidates) {
        long vi = fv_candidate.vertex_index;
        long fi = fv_candidate.face_index;
        long f0i = F(fi, 0);
        long f1i = F(fi, 1);
        long f2i = F(fi, 2);

        // Compute distance type
        PointTriangleDistanceType dtype = point_triangle_distance_type(
            V.row(fv_candidate.vertex_index), //
            V.row(f0i), V.row(f1i), V.row(f2i));

        double distance_sqr = point_triangle_distance(
            V.row(fv_candidate.vertex_index), //
            V.row(f0i), V.row(f1i), V.row(f2i), dtype);

        if (distance_sqr - dmin_squared < 2 * dmin * dhat + dhat_squared) {
            switch (dtype) {
            case PointTriangleDistanceType::P_T0:
                add_vertex_vertex_constraint(
                    constraint_set.vv_constraints, vv_to_index, vi, f0i);
                break;

            case PointTriangleDistanceType::P_T1:
                add_vertex_vertex_constraint(
                    constraint_set.vv_constraints, vv_to_index, vi, f1i);
                break;

            case PointTriangleDistanceType::P_T2:
                add_vertex_vertex_constraint(
                    constraint_set.vv_constraints, vv_to_index, vi, f2i);
                break;

            case PointTriangleDistanceType::P_E0:
                add_edge_vertex_constraint(
                    constraint_set.ev_constraints, ev_to_index,
                    F2E.rows() > fi ? F2E(fi, 0) : find_edge(E, f0i, f1i), vi);
                break;

            case PointTriangleDistanceType::P_E1:
                add_edge_vertex_constraint(
                    constraint_set.ev_constraints, ev_to_index,
                    F2E.rows() > fi ? F2E(fi, 1) : find_edge(E, f1i, f2i), vi);
                break;

            case PointTriangleDistanceType::P_E2:
                add_edge_vertex_constraint(
                    constraint_set.ev_constraints, ev_to_index,
                    F2E.rows() > fi ? F2E(fi, 2) : find_edge(E, f2i, f0i), vi);
                break;

            case PointTriangleDistanceType::P_T:
                constraint_set.fv_constraints.emplace_back(fv_candidate);
                break;
            }
        }
    }

    for (size_t ci = 0; ci < constraint_set.size(); ci++) {
        constraint_set[ci].minimum_distance = dmin;
    }
}

///////////////////////////////////////////////////////////////////////////////

double compute_barrier_potential(
    const Eigen::MatrixXd& V,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    const Constraints& constraint_set,
    const double dhat)
{
    if (constraint_set.empty()) {
        return 0;
    }

    tbb::enumerable_thread_specific<double> storage(0);

    tbb::parallel_for(
        tbb::blocked_range<size_t>(size_t(0), constraint_set.size()),
        [&](const tbb::blocked_range<size_t>& r) {
            auto& local_potential = storage.local();
            for (size_t i = r.begin(); i < r.end(); i++) {
                local_potential +=
                    constraint_set[i].compute_potential(V, E, F, dhat);
            }
        });

    double potential = 0;
    for (const auto& local_potential : storage) {
        potential += local_potential;
    }
    return potential;
}

Eigen::VectorXd compute_barrier_potential_gradient(
    const Eigen::MatrixXd& V,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    const Constraints& constraint_set,
    const double dhat)
{
    if (constraint_set.empty()) {
        return Eigen::VectorXd::Zero(V.size());
    }

    int dim = V.cols();

    tbb::enumerable_thread_specific<Eigen::VectorXd> storage(
        Eigen::VectorXd::Zero(V.size()));

    tbb::parallel_for(
        tbb::blocked_range<size_t>(size_t(0), constraint_set.size()),
        [&](const tbb::blocked_range<size_t>& r) {
            auto& local_grad = storage.local();
            for (size_t i = r.begin(); i < r.end(); i++) {
                local_gradient_to_global_gradient(
                    constraint_set[i].compute_potential_gradient(V, E, F, dhat),
                    constraint_set[i].vertex_indices(E, F), dim, local_grad);
            }
        });

    Eigen::VectorXd grad = Eigen::VectorXd::Zero(V.size());
    for (const auto& local_grad : storage) {
        grad += local_grad;
    }
    return grad;
}

Eigen::SparseMatrix<double> compute_barrier_potential_hessian(
    const Eigen::MatrixXd& V,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    const Constraints& constraint_set,
    const double dhat,
    const bool project_hessian_to_psd)
{
    if (constraint_set.empty()) {
        return Eigen::SparseMatrix<double>(V.size(), V.size());
    }

    int dim = V.cols();

    tbb::enumerable_thread_specific<std::vector<Eigen::Triplet<double>>>
        storage;

    tbb::parallel_for(
        tbb::blocked_range<size_t>(size_t(0), constraint_set.size()),
        [&](const tbb::blocked_range<size_t>& r) {
            auto& local_hess_triplets = storage.local();

            for (size_t i = r.begin(); i < r.end(); i++) {
                local_hessian_to_global_triplets(
                    constraint_set[i].compute_potential_hessian(
                        V, E, F, dhat, project_hessian_to_psd),
                    constraint_set[i].vertex_indices(E, F), dim,
                    local_hess_triplets);
            }
        });

    Eigen::SparseMatrix<double> hess(V.size(), V.size());
    for (const auto& local_hess_triplets : storage) {
        Eigen::SparseMatrix<double> local_hess(V.size(), V.size());
        local_hess.setFromTriplets(
            local_hess_triplets.begin(), local_hess_triplets.end());
        hess += local_hess;
    }
    return hess;
}

///////////////////////////////////////////////////////////////////////////////

bool is_step_collision_free(
    const Eigen::MatrixXd& V0,
    const Eigen::MatrixXd& V1,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    const BroadPhaseMethod& method,
    const double tolerance,
    const long max_iterations,
    const std::function<bool(size_t, size_t)>& can_collide)
{
    // Broad phase
    Candidates candidates;
    construct_collision_candidates(
        V0, V1, E, F, candidates, /*inflation_radius=*/0, method, can_collide);

    // Narrow phase
    return is_step_collision_free(
        candidates, V0, V1, E, F, tolerance, max_iterations);
}

bool is_step_collision_free(
    const Eigen::MatrixXd& V0,
    const Eigen::MatrixXd& V1,
    const Eigen::VectorXi& codim_V,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    const BroadPhaseMethod& method,
    const double tolerance,
    const long max_iterations,
    const std::function<bool(size_t, size_t)>& can_collide)
{
    // Broad phase
    Candidates candidates;
    construct_collision_candidates(
        V0, V1, codim_V, E, F, candidates, /*inflation_radius=*/0, method,
        can_collide);

    // Narrow phase
    return is_step_collision_free(
        candidates, V0, V1, E, F, tolerance, max_iterations);
}

bool is_step_collision_free(
    const Candidates& candidates,
    const Eigen::MatrixXd& V0,
    const Eigen::MatrixXd& V1,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    const double tolerance,
    const long max_iterations)
{
    assert(V0.cols() == V1.cols());

    // Narrow phase
    for (size_t i = 0; i < candidates.size(); i++) {
        double toi;
        bool is_collision = candidates[i].ccd(
            V0, V1, E, F, toi, /*tmax=*/1.0, tolerance, max_iterations);

        if (is_collision) {
            return false;
        }
    }

    return true;
}

///////////////////////////////////////////////////////////////////////////////

double compute_collision_free_stepsize(
    const Eigen::MatrixXd& V0,
    const Eigen::MatrixXd& V1,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    const BroadPhaseMethod& method,
    const double tolerance,
    const long max_iterations,
    const std::function<bool(size_t, size_t)>& can_collide)
{
    // Broad phase
    Candidates candidates;
    construct_collision_candidates(
        V0, V1, E, F, candidates, /*inflation_radius=*/0, method, can_collide);

    // Narrow phase
    return compute_collision_free_stepsize(
        candidates, V0, V1, E, F, tolerance, max_iterations);
}

double compute_collision_free_stepsize(
    const Eigen::MatrixXd& V0,
    const Eigen::MatrixXd& V1,
    const Eigen::VectorXi& codim_V,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    const BroadPhaseMethod& method,
    const double tolerance,
    const long max_iterations,
    const std::function<bool(size_t, size_t)>& can_collide)
{
    // Broad phase
    Candidates candidates;
    construct_collision_candidates(
        V0, V1, codim_V, E, F, candidates, /*inflation_radius=*/0, method,
        can_collide);

    // Narrow phase
    return compute_collision_free_stepsize(
        candidates, V0, V1, E, F, tolerance, max_iterations);
}

double compute_collision_free_stepsize(
    const Candidates& candidates,
    const Eigen::MatrixXd& V0,
    const Eigen::MatrixXd& V1,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    const double tolerance,
    const long max_iterations)
{
    assert(V0.cols() == V1.cols());

    if (candidates.empty()) {
        return 1; // No possible collisions, so can take full step.
    }

    // Narrow phase
#ifdef IPC_EARLIEST_TOI_USE_MUTEX
    double earliest_toi = 1;
    std::mutex earliest_toi_mutex;
#else
    tbb::enumerable_thread_specific<double> storage(1);
#endif

    tbb::parallel_for(
        tbb::blocked_range<size_t>(0, candidates.size()),
        [&](tbb::blocked_range<size_t> r) {
#ifndef IPC_EARLIEST_TOI_USE_MUTEX
            double& earliest_toi = storage.local();
#endif
            for (size_t i = r.begin(); i < r.end(); i++) {
                double toi = std::numeric_limits<double>::infinity();
                bool are_colliding = candidates[i].ccd(
                    V0, V1, E, F, toi, /*tmax=*/earliest_toi, tolerance,
                    max_iterations);

                if (are_colliding) {
#ifdef IPC_EARLIEST_TOI_USE_MUTEX
                    std::lock_guard<std::mutex> lock(earliest_toi_mutex);
#endif
                    if (toi < earliest_toi) {
                        earliest_toi = toi;
                    }
                }
            }
        });

#ifndef IPC_EARLIEST_TOI_USE_MUTEX
    double earliest_toi = 1;
    for (const auto& local_earliest_toi : storage) {
        earliest_toi = std::min(earliest_toi, local_earliest_toi);
    }
#endif
    assert(earliest_toi >= 0 && earliest_toi <= 1.0);
    return earliest_toi;
}

///////////////////////////////////////////////////////////////////////////////

// NOTE: Actually distance squared
double compute_minimum_distance(
    const Eigen::MatrixXd& V,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    const Constraints& constraint_set)
{
    if (constraint_set.empty()) {
        return std::numeric_limits<double>::infinity();
    }

    tbb::enumerable_thread_specific<double> storage(
        std::numeric_limits<double>::infinity());

    // Do a single block range over all constraint vectors
    tbb::parallel_for(
        tbb::blocked_range<size_t>(0, constraint_set.size()),
        [&](tbb::blocked_range<size_t> r) {
            double& local_min_dist = storage.local();

            for (size_t i = r.begin(); i < r.end(); i++) {
                const double dist = constraint_set[i].compute_distance(V, E, F);

                if (dist < local_min_dist) {
                    local_min_dist = dist;
                }
            }
        });

    double min_dist = std::numeric_limits<double>::infinity();
    for (const auto& local_min_dist : storage) {
        min_dist = std::min(min_dist, local_min_dist);
    }
    return min_dist;
}

///////////////////////////////////////////////////////////////////////////////

bool has_intersections(
    const Eigen::MatrixXd& V,
    const Eigen::MatrixXi& E,
    const Eigen::MatrixXi& F,
    const std::function<bool(size_t, size_t)>& can_collide)
{
    // TODO: Expose the broad-phase method
    HashGrid hash_grid;
    double conservative_inflation_radius = 1e-2 * world_bbox_diagonal_length(V);
    hash_grid.resize(V, E, conservative_inflation_radius);
    hash_grid.addEdges(V, E, conservative_inflation_radius);
    if (V.cols() == 3) {
        // These are not needed for 2D
        hash_grid.addFaces(V, F, conservative_inflation_radius);
    }

    if (V.cols() == 2) { // Need to check segment-segment intersections in 2D
        std::vector<EdgeEdgeCandidate> ee_candidates;
        hash_grid.getEdgeEdgePairs(E, ee_candidates, can_collide);

        // narrow-phase using igl
        igl::predicates::exactinit();
        for (const EdgeEdgeCandidate& ee_candidate : ee_candidates) {
            if (igl::predicates::segment_segment_intersect(
                    V.row(E(ee_candidate.edge0_index, 0)).head<2>(),
                    V.row(E(ee_candidate.edge0_index, 1)).head<2>(),
                    V.row(E(ee_candidate.edge1_index, 0)).head<2>(),
                    V.row(E(ee_candidate.edge1_index, 1)).head<2>())) {
                return true;
            }
        }
    } else { // Need to check segment-triangle intersections in 3D
        assert(V.cols() == 3);

        std::vector<EdgeFaceCandidate> ef_candidates;
        hash_grid.getEdgeFacePairs(E, F, ef_candidates, can_collide);

        for (const EdgeFaceCandidate& ef_candidate : ef_candidates) {
            if (is_edge_intersecting_triangle(
                    V.row(E(ef_candidate.edge_index, 0)),
                    V.row(E(ef_candidate.edge_index, 1)),
                    V.row(F(ef_candidate.face_index, 0)),
                    V.row(F(ef_candidate.face_index, 1)),
                    V.row(F(ef_candidate.face_index, 2)))) {
                return true;
            }
        }
    }

    return false;
}

} // namespace ipc


<|start_filename|>src/ccd/ccd.hpp<|end_filename|>
#pragma once

#include <Eigen/Core>
#include <ipc/utils/eigen_ext.hpp>

namespace ipc {

/// The default tolerance used with Tight-Inclusion CCD.
static constexpr double DEFAULT_CCD_TOLERANCE = 1e-6;
/// The default maximum number of iterations used with Tight-Inclusion CCD.
static constexpr long DEFAULT_CCD_MAX_ITERATIONS = 1e7;
/// The default conservative rescaling value used to avoid taking steps exactly
/// to impact.
static constexpr double DEFAULT_CCD_CONSERVATIVE_RESCALING = 0.8;

// 2D

bool point_edge_ccd_2D(
    const Eigen::Vector2d& p_t0,
    const Eigen::Vector2d& e0_t0,
    const Eigen::Vector2d& e1_t0,
    const Eigen::Vector2d& p_t1,
    const Eigen::Vector2d& e0_t1,
    const Eigen::Vector2d& e1_t1,
    double& toi,
    const double tmax = 1.0,
    const double tolerance = DEFAULT_CCD_TOLERANCE,
    const long max_iterations = DEFAULT_CCD_MAX_ITERATIONS,
    const double conservative_rescaling = DEFAULT_CCD_CONSERVATIVE_RESCALING);

// 3D

bool point_point_ccd(
    const Eigen::Vector3d& p0_t0,
    const Eigen::Vector3d& p1_t0,
    const Eigen::Vector3d& p0_t1,
    const Eigen::Vector3d& p1_t1,
    double& toi,
    const double tmax = 1.0,
    const double tolerance = DEFAULT_CCD_TOLERANCE,
    const long max_iterations = DEFAULT_CCD_MAX_ITERATIONS,
    const double conservative_rescaling = DEFAULT_CCD_CONSERVATIVE_RESCALING);

bool point_edge_ccd_3D(
    const Eigen::Vector3d& p_t0,
    const Eigen::Vector3d& e0_t0,
    const Eigen::Vector3d& e1_t0,
    const Eigen::Vector3d& p_t1,
    const Eigen::Vector3d& e0_t1,
    const Eigen::Vector3d& e1_t1,
    double& toi,
    const double tmax = 1.0,
    const double tolerance = DEFAULT_CCD_TOLERANCE,
    const long max_iterations = DEFAULT_CCD_MAX_ITERATIONS,
    const double conservative_rescaling = DEFAULT_CCD_CONSERVATIVE_RESCALING);

bool point_triangle_ccd(
    const Eigen::Vector3d& p_t0,
    const Eigen::Vector3d& t0_t0,
    const Eigen::Vector3d& t1_t0,
    const Eigen::Vector3d& t2_t0,
    const Eigen::Vector3d& p_t1,
    const Eigen::Vector3d& t0_t1,
    const Eigen::Vector3d& t1_t1,
    const Eigen::Vector3d& t2_t1,
    double& toi,
    const double tmax = 1.0,
    const double tolerance = DEFAULT_CCD_TOLERANCE,
    const long max_iterations = DEFAULT_CCD_MAX_ITERATIONS,
    const double conservative_rescaling = DEFAULT_CCD_CONSERVATIVE_RESCALING);

bool edge_edge_ccd(
    const Eigen::Vector3d& ea0_t0,
    const Eigen::Vector3d& ea1_t0,
    const Eigen::Vector3d& eb0_t0,
    const Eigen::Vector3d& eb1_t0,
    const Eigen::Vector3d& ea0_t1,
    const Eigen::Vector3d& ea1_t1,
    const Eigen::Vector3d& eb0_t1,
    const Eigen::Vector3d& eb1_t1,
    double& toi,
    const double tmax = 1.0,
    const double tolerance = DEFAULT_CCD_TOLERANCE,
    const long max_iterations = DEFAULT_CCD_MAX_ITERATIONS,
    const double conservative_rescaling = DEFAULT_CCD_CONSERVATIVE_RESCALING);

// 2D or 3D

bool point_edge_ccd(
    const VectorMax3d& p_t0,
    const VectorMax3d& e0_t0,
    const VectorMax3d& e1_t0,
    const VectorMax3d& p_t1,
    const VectorMax3d& e0_t1,
    const VectorMax3d& e1_t1,
    double& toi,
    const double tmax = 1.0,
    const double tolerance = DEFAULT_CCD_TOLERANCE,
    const long max_iterations = DEFAULT_CCD_MAX_ITERATIONS,
    const double conservative_rescaling = DEFAULT_CCD_CONSERVATIVE_RESCALING);

} // namespace ipc


<|start_filename|>src/ccd/ccd.cpp<|end_filename|>
#include <ipc/ccd/ccd.hpp>

#include <algorithm> // std::min/max
#include <array>

#ifdef IPC_TOOLKIT_WITH_CORRECT_CCD
#include <tight_inclusion/inclusion_ccd.hpp>
#else
#include <CTCD.h>
#endif

#include <ipc/distance/edge_edge.hpp>
#include <ipc/distance/point_triangle.hpp>

namespace ipc {

#ifdef IPC_TOOLKIT_WITH_CORRECT_CCD
static constexpr int TIGHT_INCLUSION_CCD_TYPE = 1;
static constexpr double INITIAL_DISTANCE_TOLERANCE_SCALE = 0.5;
#endif

bool ccd_strategy(
    const std::function<bool(
        double /*min_distance*/, bool /*no_zero_toi*/, double& /*toi*/)>& ccd,
    const double initial_distance,
    const double conservative_rescaling,
    double& toi)
{
    static constexpr double SMALL_TOI = 1e-6;

    if (initial_distance == 0) {
        IPC_LOG(warn("Initial distance is 0, returning toi=0!"));
        toi = 0;
        return true;
    }

    double min_distance = (1.0 - conservative_rescaling) * initial_distance;
    // #ifdef IPC_TOOLKIT_WITH_CORRECT_CCD
    // Tight Inclusion performs better when the minimum separation is small
    // min_distance = std::min(min_distance, 1e-4);
    // #endif

    assert(min_distance < initial_distance);

    // Do not use no_zero_toi because the minimum distance is arbitrary and can
    // be removed if the query is challenging (i.e., produces small ToI).
    bool is_impacting = ccd(min_distance, /*no_zero_toi=*/false, toi);

#ifdef IPC_TOOLKIT_WITH_CORRECT_CCD
    // Tight inclusion will have higher accuracy and better performance if we
    // shrink the minimum distance. The value 1e-10 is arbitrary.
    while (is_impacting && toi < SMALL_TOI && min_distance > 1e-10) {
        min_distance /= 10;
        is_impacting = ccd(min_distance, /*no_zero_toi=*/false, toi);
    }
#endif

    if (is_impacting && toi < SMALL_TOI) {
        is_impacting = ccd(/*min_distance=*/0, /*no_zero_toi=*/true, toi);

        if (is_impacting) {
            toi *= conservative_rescaling;
            assert(toi != 0);
        }
    }

    return is_impacting;
}

bool point_point_ccd(
    const Eigen::Vector3d& p0_t0,
    const Eigen::Vector3d& p1_t0,
    const Eigen::Vector3d& p0_t1,
    const Eigen::Vector3d& p1_t1,
    double& toi,
    const double tmax,
    const double tolerance,
    const long max_iterations,
    const double conservative_rescaling)
{
    assert(tmax >= 0 && tmax <= 1.0);

    double initial_distance = sqrt(point_point_distance(p0_t0, p1_t0));

    double adjusted_tolerance = std::min(
        INITIAL_DISTANCE_TOLERANCE_SCALE * initial_distance, tolerance);

    const auto ccd = [&](double min_distance, bool no_zero_toi,
                         double& toi) -> bool {
#ifdef IPC_TOOLKIT_WITH_CORRECT_CCD
        double output_tolerance;
        // NOTE: Use degenerate edge-edge
        return inclusion_ccd::edgeEdgeCCD_double(
            p0_t0, p0_t0, p1_t0, p1_t0, p0_t1, p0_t1, p1_t1, p1_t1,
            { { -1, -1, -1 } }, // rounding error (auto)
            min_distance,       // minimum separation distance
            toi,                // time of impact
            adjusted_tolerance, // delta
            tmax,               // maximum time to check
            max_iterations,     // maximum number of iterations
            output_tolerance,   // delta_actual
            TIGHT_INCLUSION_CCD_TYPE, no_zero_toi);
#else
        return CTCD::vertexVertexCTCD(
            p0_t0, p1_t0, p0_t1, p1_t1, min_distance, toi);
#endif
    };

    return ccd_strategy(ccd, initial_distance, conservative_rescaling, toi);
}

inline Eigen::Vector3d to_3D(const Eigen::Vector2d& v)
{
    return Eigen::Vector3d(v.x(), v.y(), 0);
}

bool point_edge_ccd_2D(
    const Eigen::Vector2d& p_t0,
    const Eigen::Vector2d& e0_t0,
    const Eigen::Vector2d& e1_t0,
    const Eigen::Vector2d& p_t1,
    const Eigen::Vector2d& e0_t1,
    const Eigen::Vector2d& e1_t1,
    double& toi,
    const double tmax,
    const double tolerance,
    const long max_iterations,
    const double conservative_rescaling)
{
#ifndef IPC_TOOLKIT_WITH_CORRECT_CCD
    inexact_point_edge_ccd_2D(
        p_t0, e0_t0, e1_t0, p_t1, e0_t1, e1_t1, toi, conservative_rescaling);
#else
    assert(0 <= tmax && tmax <= 1.0);

    Eigen::Vector3d p_t0_3D = to_3D(p_t0);
    Eigen::Vector3d e0_t0_3D = to_3D(e0_t0);
    Eigen::Vector3d e1_t0_3D = to_3D(e1_t0);
    Eigen::Vector3d p_t1_3D = to_3D(p_t1);
    Eigen::Vector3d e0_t1_3D = to_3D(e0_t1);
    Eigen::Vector3d e1_t1_3D = to_3D(e1_t1);

    double initial_distance = sqrt(point_edge_distance(p_t0, e0_t0, e1_t0));

    double adjusted_tolerance = std::min(
        INITIAL_DISTANCE_TOLERANCE_SCALE * initial_distance, tolerance);

    const auto ccd = [&](double min_distance, bool no_zero_toi,
                         double& toi) -> bool {
        double output_tolerance;
        // NOTE: Use degenerate edge-edge
        return inclusion_ccd::edgeEdgeCCD_double(
            p_t0_3D, p_t0_3D, e0_t0_3D, e1_t0_3D, //
            p_t1_3D, p_t1_3D, e0_t1_3D, e1_t1_3D,
            { { -1, -1, -1 } }, // rounding error (auto)
            min_distance,       // minimum separation distance
            toi,                // time of impact
            adjusted_tolerance, // delta
            tmax,               // maximum time to check
            max_iterations,     // maximum number of iterations
            output_tolerance,   // delta_actual
            TIGHT_INCLUSION_CCD_TYPE, no_zero_toi);
    };

    return ccd_strategy(ccd, initial_distance, conservative_rescaling, toi);
#endif
}

bool point_edge_ccd_3D(
    const Eigen::Vector3d& p_t0,
    const Eigen::Vector3d& e0_t0,
    const Eigen::Vector3d& e1_t0,
    const Eigen::Vector3d& p_t1,
    const Eigen::Vector3d& e0_t1,
    const Eigen::Vector3d& e1_t1,
    double& toi,
    const double tmax,
    const double tolerance,
    const long max_iterations,
    const double conservative_rescaling)
{
    assert(tmax >= 0 && tmax <= 1.0);

    double initial_distance = sqrt(point_edge_distance(p_t0, e0_t0, e1_t0));

    double adjusted_tolerance = std::min(
        INITIAL_DISTANCE_TOLERANCE_SCALE * initial_distance, tolerance);

    const auto ccd = [&](double min_distance, bool no_zero_toi,
                         double& toi) -> bool {
#ifdef IPC_TOOLKIT_WITH_CORRECT_CCD
        double output_tolerance = tolerance;
        // NOTE: Use degenerate edge-edge
        return inclusion_ccd::edgeEdgeCCD_double(
            p_t0, p_t0, e0_t0, e1_t0, p_t1, p_t1, e0_t1, e1_t1,
            { { -1, -1, -1 } }, // rounding error (auto)
            min_distance,       // minimum separation distance
            toi,                // time of impact
            adjusted_tolerance, // delta
            tmax,               // maximum time to check
            max_iterations,     // maximum number of iterations
            output_tolerance,   // delta_actual
            TIGHT_INCLUSION_CCD_TYPE, no_zero_toi);
#else
        return CTCD::vertexEdgeCTCD(
            p_t0, e0_t0, e1_t0, p_t1, e0_t1, e1_t1, min_distance, toi);
#endif
    };

    return ccd_strategy(ccd, initial_distance, conservative_rescaling, toi);
}

bool point_edge_ccd(
    const VectorMax3d& p_t0,
    const VectorMax3d& e0_t0,
    const VectorMax3d& e1_t0,
    const VectorMax3d& p_t1,
    const VectorMax3d& e0_t1,
    const VectorMax3d& e1_t1,
    double& toi,
    const double tmax,
    const double tolerance,
    const long max_iterations,
    const double conservative_rescaling)
{
    int dim = p_t0.size();
    assert(e0_t0.size() == dim);
    assert(e1_t0.size() == dim);
    assert(p_t1.size() == dim);
    assert(e0_t1.size() == dim);
    assert(e1_t1.size() == dim);
    if (dim == 2) {
        return point_edge_ccd_2D(
            p_t0, e0_t0, e1_t0, p_t1, e0_t1, e1_t1, //
            toi, tmax, tolerance, max_iterations, conservative_rescaling);
    } else {
        return point_edge_ccd_3D(
            p_t0, e0_t0, e1_t0, p_t1, e0_t1, e1_t1, //
            toi, tmax, tolerance, max_iterations, conservative_rescaling);
    }
}

bool edge_edge_ccd(
    const Eigen::Vector3d& ea0_t0,
    const Eigen::Vector3d& ea1_t0,
    const Eigen::Vector3d& eb0_t0,
    const Eigen::Vector3d& eb1_t0,
    const Eigen::Vector3d& ea0_t1,
    const Eigen::Vector3d& ea1_t1,
    const Eigen::Vector3d& eb0_t1,
    const Eigen::Vector3d& eb1_t1,
    double& toi,
    const double tmax,
    const double tolerance,
    const long max_iterations,
    const double conservative_rescaling)
{
    assert(tmax >= 0 && tmax <= 1.0);

    double initial_distance =
        sqrt(edge_edge_distance(ea0_t0, ea1_t0, eb0_t0, eb1_t0));

    double adjusted_tolerance = std::min(
        INITIAL_DISTANCE_TOLERANCE_SCALE * initial_distance, tolerance);

    const auto ccd = [&](double min_distance, bool no_zero_toi,
                         double& toi) -> bool {
#ifdef IPC_TOOLKIT_WITH_CORRECT_CCD
        double output_tolerance;
        return inclusion_ccd::edgeEdgeCCD_double(
            ea0_t0, ea1_t0, eb0_t0, eb1_t0, ea0_t1, ea1_t1, eb0_t1, eb1_t1,
            { { -1, -1, -1 } }, // rounding error (auto)
            min_distance,       // minimum separation distance
            toi,                // time of impact
            adjusted_tolerance, // delta
            tmax,               // maximum time to check
            max_iterations,     // maximum number of iterations
            output_tolerance,   // delta_actual
            TIGHT_INCLUSION_CCD_TYPE, no_zero_toi);
#else
        return CTCD::edgeEdgeCTCD(
            ea0_t0, ea1_t0, eb0_t0, eb1_t0, ea0_t1, ea1_t1, eb0_t1, eb1_t1,
            min_distance, toi);
#endif
    };

    return ccd_strategy(ccd, initial_distance, conservative_rescaling, toi);
}

bool point_triangle_ccd(
    const Eigen::Vector3d& p_t0,
    const Eigen::Vector3d& t0_t0,
    const Eigen::Vector3d& t1_t0,
    const Eigen::Vector3d& t2_t0,
    const Eigen::Vector3d& p_t1,
    const Eigen::Vector3d& t0_t1,
    const Eigen::Vector3d& t1_t1,
    const Eigen::Vector3d& t2_t1,
    double& toi,
    const double tmax,
    const double tolerance,
    const long max_iterations,
    const double conservative_rescaling)
{
    assert(tmax >= 0 && tmax <= 1.0);

    double initial_distance =
        sqrt(point_triangle_distance(p_t0, t0_t0, t1_t0, t2_t0));

    double adjusted_tolerance = std::min(
        INITIAL_DISTANCE_TOLERANCE_SCALE * initial_distance, tolerance);

    const auto ccd = [&](double min_distance, bool no_zero_toi,
                         double& toi) -> bool {
#ifdef IPC_TOOLKIT_WITH_CORRECT_CCD
        double output_tolerance;
        return inclusion_ccd::vertexFaceCCD_double(
            p_t0, t0_t0, t1_t0, t2_t0, p_t1, t0_t1, t1_t1, t2_t1,
            { { -1, -1, -1 } }, // rounding error (auto)
            min_distance,       // minimum separation distance
            toi,                // time of impact
            adjusted_tolerance, // delta
            tmax,               // maximum time to check
            max_iterations,     // maximum number of iterations
            output_tolerance,   // delta_actual
            TIGHT_INCLUSION_CCD_TYPE, no_zero_toi);
#else
        return CTCD::vertexFaceCTCD(
            p_t0, t0_t0, t1_t0, t2_t0, p_t1, t0_t1, t1_t1, t2_t1, //
            min_distance, toi);
#endif
    };

    return ccd_strategy(ccd, initial_distance, conservative_rescaling, toi);
}

} // namespace ipc


<|start_filename|>python/src/distance/point_line.cpp<|end_filename|>
#include <pybind11/pybind11.h>
#include <pybind11/eigen.h>

#include <ipc/distance/point_line.hpp>

#include "../utils.hpp"

namespace py = pybind11;
using namespace ipc;

void define_point_line_distance_functions(py::module_& m)
{
    m.def(
        "point_line_distance",
        [](const VectorMax3d& p, const VectorMax3d& e0, const VectorMax3d& e1) {
            assert_2D_or_3D_vector(p, "p");
            assert_2D_or_3D_vector(e0, "e0");
            assert_2D_or_3D_vector(e1, "e1");
            return point_line_distance(p, e0, e1);
        },
        R"ipc_Qu8mg5v7(
        Compute the distance between a point and line in 2D or 3D.

        Parameters:
            p: point
            e0: first vertex of the edge defining the line
            e1: second vertex of the edge defining the line

        Returns:
            The distance between the point and line.

        Note:
            The distance is actually squared distance.
        )ipc_Qu8mg5v7",
        py::arg("p"), py::arg("e0"), py::arg("e1"));

    m.def(
        "point_line_distance_gradient",
        [](const VectorMax3d& p, const VectorMax3d& e0, const VectorMax3d& e1) {
            assert_2D_or_3D_vector(p, "p");
            assert_2D_or_3D_vector(e0, "e0");
            assert_2D_or_3D_vector(e1, "e1");
            VectorMax9d grad;
            point_line_distance_gradient(p, e0, e1, grad);
            return grad;
        },
        R"ipc_Qu8mg5v7(
        Compute the gradient of the distance between a point and line.
        Parameters:
            p: point
            e0: first vertex of the edge defining the line.
            e1: second vertex of the edge defining the line.

        Returns:
            The gradient of the distance wrt p, e0, and e1.

        Note:
            The distance is actually squared distance.
        )ipc_Qu8mg5v7",
        py::arg("p"), py::arg("e0"), py::arg("e1"));

    m.def(
        "point_line_distance_hessian",
        [](const VectorMax3d& p, const VectorMax3d& e0, const VectorMax3d& e1) {
            assert_2D_or_3D_vector(p, "p");
            assert_2D_or_3D_vector(e0, "e0");
            assert_2D_or_3D_vector(e1, "e1");
            MatrixMax9d hess;
            point_line_distance_hessian(p, e0, e1, hess);
            return hess;
        },
        R"ipc_Qu8mg5v7(
        Compute the hessian of the distance between a point and line.
        Parameters:
            p: point
            e0: first vertex of the edge defining the line
            e1: second vertex of the edge defining the line

        Returns:
            The hessian of the distance wrt p, e0, and e1.

        Note:
            The distance is actually squared distance.
        )ipc_Qu8mg5v7",
        py::arg("p"), py::arg("e0"), py::arg("e1"));
}
