deal with codim vert/edge in 2D

Author: Huangzizhou

src/ipc/collision_mesh.cpp

@@ -145,17 +145,17 @@ void CollisionMesh::construct_edges_to_faces()
 
 void CollisionMesh::init_codim_vertices()
 {
-    std::vector<bool> is_codim_vertex(num_vertices(), true);
+    m_is_codim_vertex.assign(num_vertices(), true);
     for (int i : m_edges.reshaped()) {
-        is_codim_vertex[i] = false;
+        m_is_codim_vertex[i] = false;
     }
 
     m_codim_vertices.resize(
-        std::count(is_codim_vertex.begin(), is_codim_vertex.end(), true));
+        std::count(m_is_codim_vertex.begin(), m_is_codim_vertex.end(), true));
 
     int j = 0;
     for (int i = 0; i < num_vertices(); i++) {
-        if (is_codim_vertex[i]) {
+        if (m_is_codim_vertex[i]) {
             assert(j < m_codim_vertices.size());
             m_codim_vertices[j++] = i;
         }
@@ -165,17 +165,17 @@ void CollisionMesh::init_codim_vertices()
 
 void CollisionMesh::init_codim_edges()
 {
-    std::vector<bool> is_codim_edge(num_edges(), true);
+    m_is_codim_edge.assign(num_edges(), true);
     for (int i : m_faces_to_edges.reshaped()) {
-        is_codim_edge[i] = false;
+        m_is_codim_edge[i] = false;
     }
 
     m_codim_edges.resize(
-        std::count(is_codim_edge.begin(), is_codim_edge.end(), true));
+        std::count(m_is_codim_edge.begin(), m_is_codim_edge.end(), true));
 
     int j = 0;
     for (int i = 0; i < num_edges(); i++) {
-        if (is_codim_edge[i]) {
+        if (m_is_codim_edge[i]) {
             assert(j < m_codim_edges.size());
             m_codim_edges[j++] = i;
         }

src/ipc/collision_mesh.hpp

@@ -99,9 +99,13 @@ class CollisionMesh {
     /// @brief Get the indices of codimensional vertices of the collision mesh (#CV x 1).
     const Eigen::VectorXi& codim_vertices() const { return m_codim_vertices; }
 
+    bool is_codim_vertex(const long& v) const { return m_is_codim_vertex[v]; }
+
     /// @brief Get the indices of codimensional edges of the collision mesh (#CE x 1).
     const Eigen::VectorXi& codim_edges() const { return m_codim_edges; }
 
+    bool is_codim_edge(const long& e) const { return m_is_codim_edge[e]; }
+
     /// @brief Get the edges of the collision mesh (#E × 2).
     const Eigen::MatrixXi& edges() const { return m_edges; }
 
@@ -317,8 +321,12 @@ class CollisionMesh {
     Eigen::MatrixXd m_full_rest_positions;
     /// @brief The vertex positions at rest (#V × dim).
     Eigen::MatrixXd m_rest_positions;
+    /// @brief The mask of codimensional vertices (#V).
+    std::vector<bool> m_is_codim_vertex;
     /// @brief The indices of codimensional vertices (#CV x 1).
     Eigen::VectorXi m_codim_vertices;
+    /// @brief The mask of codimensional edges (#E).
+    std::vector<bool> m_is_codim_edge;
     /// @brief The indices of codimensional edges (#CE x 1).
     Eigen::VectorXi m_codim_edges;
     /// @brief Edges as rows of indicies into vertices (#E × 2).

src/ipc/smooth_contact/primitives/edge2.cpp

@@ -10,14 +10,20 @@ Edge2::Edge2(
     : Primitive(id, param)
 {
     _vert_ids = { { mesh.edges()(id, 0), mesh.edges()(id, 1) } };
-    Vector2d edge = vertices.row(_vert_ids[1]) - vertices.row(_vert_ids[0]);
-    is_active_ = Math<double>::cross2(d, edge) > 0;
+
+    is_active_ = mesh.is_codim_edge(id) || Math<double>::cross2(d, vertices.row(_vert_ids[1]) - vertices.row(_vert_ids[0])) > 0;
+}
+
+int Edge2::n_vertices() const 
+{ 
+    return n_edge_neighbors_2d; 
 }
-int Edge2::n_vertices() const { return n_edge_neighbors_2d; }
+
 double Edge2::potential(const Vector2d& d, const Vector4d& x) const
 {
     return (x.tail<2>() - x.head<2>()).norm();
 }
+
 Vector6d Edge2::grad(const Vector2d& d, const Vector4d& x) const
 {
     const Vector2d t = x.tail<2>() - x.head<2>();
@@ -28,6 +34,7 @@ Vector6d Edge2::grad(const Vector2d& d, const Vector4d& x) const
     g.segment<2>(4) =  t / len;
     return g;
 }
+
 Matrix6d Edge2::hessian(const Vector2d& d, const Vector4d& x) const
 {
     Matrix6d h;

src/ipc/smooth_contact/primitives/point2.cpp

@@ -2,98 +2,164 @@
 
 namespace ipc {
 
+namespace {
+    bool smooth_point2_term_type(
+        const Eigen::Ref<const Vector2d>& v,
+        const Eigen::Ref<const Vector2d>& direc,
+        const Eigen::Ref<const Vector2d>& e0,
+        const Eigen::Ref<const Vector2d>& e1,
+        const ParameterType& param,
+        const bool orientable)
+    {
+        const Vector2d dn = -direc.normalized();
+        const Vector2d t0 = (e0 - v).normalized(), t1 = (e1 - v).normalized();
+    
+        if (dn.dot(t0) <= -param.alpha_t || dn.dot(t1) <= -param.alpha_t)
+            return false;
+    
+        if (orientable)
+        {
+            const double tmp = Math<double>::smooth_heaviside(-Math<double>::cross2(dn, t0), param.alpha_n, param.beta_n) + 
+                                Math<double>::smooth_heaviside(Math<double>::cross2(dn, t1), param.alpha_n, param.beta_n);
+            if (tmp <= 1. - param.alpha_n)
+                return false;
+        }
+    
+        return true;
+    }
+
+    template <class scalar>
+    scalar smooth_point2_term(
+        const Eigen::Ref<const Vector2<scalar>>& v,
+        const Eigen::Ref<const Vector2<scalar>>& direc,
+        const Eigen::Ref<const Vector2<scalar>>& e0,
+        const Eigen::Ref<const Vector2<scalar>>& e1,
+        const ParameterType& param,
+        const bool orientable)
+    {
+        const Vector2<scalar> dn = -direc.normalized();
+        const Vector2<scalar> t0 = (e0 - v).normalized(), t1 = (v - e1).normalized();
+    
+        scalar val = Math<scalar>::smooth_heaviside(dn.dot(t0), param.alpha_t, param.beta_t) *
+                            Math<scalar>::smooth_heaviside(-dn.dot(t1), param.alpha_t, param.beta_t);
+    
+        if (orientable)
+        {
+            const scalar tmp = Math<scalar>::smooth_heaviside(-Math<scalar>::cross2(dn, t0), param.alpha_n, param.beta_n) + 
+                                    Math<scalar>::smooth_heaviside(-Math<scalar>::cross2(dn, t1), param.alpha_n, param.beta_n);
+            val = val * Math<scalar>::smooth_heaviside(tmp - 1., param.alpha_n, 0);
+        }
+    
+        return val * ((e0 - v).norm() + (e1 - v).norm()) / 2.;
+    }
+
+    template <class scalar>
+    scalar smooth_point2_term_one_side(
+        const Eigen::Ref<const Vector2<scalar>>& v,
+        const Eigen::Ref<const Vector2<scalar>>& direc,
+        const Eigen::Ref<const Vector2<scalar>>& e0,
+        const ParameterType& param)
+    {
+        const Vector2<scalar> dn = -direc.normalized();
+        const Vector2<scalar> t0 = e0 - v;
+    
+        const scalar tangent_term = Math<scalar>::smooth_heaviside(dn.dot(t0) / t0.norm(), param.alpha_t, param.beta_t);
+        
+        return tangent_term * t0.norm();
+    }
+}
+
 Point2::Point2(const long &id, 
     const CollisionMesh& mesh,
     const Eigen::MatrixXd& vertices,
     const VectorMax3d& d,
     const ParameterType& param)
 : Primitive(id, param)
 {
-    _vert_ids = {{-1, -1, -1}};
-    _vert_ids[0] = id;
+    orientable = !mesh.is_codim_vertex(id);
+    auto neighbor_verts = mesh.find_vertex_adjacent_vertices(id);
+    has_neighbor_1 = neighbor_verts[0] >= 0;
+    has_neighbor_2 = neighbor_verts[1] >= 0;
 
-    if (mesh.vertex_edge_adjacencies()[id].size() != 2)
-        logger().error("Invalid number of vertex neighbor in 2D! {} should be 2.", mesh.vertex_edge_adjacencies()[id].size());
-    for (long i : mesh.vertex_edge_adjacencies()[id])
+    if (has_neighbor_1 && has_neighbor_2)
     {
-        if (mesh.edges()(i, 0) == id)
-            _vert_ids[1] = mesh.edges()(i, 1);
-        else if (mesh.edges()(i, 1) == id)
-            _vert_ids[2] = mesh.edges()(i, 0);
-        else
-            logger().error("Wrong edge-vertex adjacency!");
+        _vert_ids = {{id, neighbor_verts[0], neighbor_verts[1]}};
+        is_active_ = smooth_point2_term_type(vertices.row(id), d, vertices.row(_vert_ids[1]), vertices.row(_vert_ids[2]), _param, orientable);
     }
+    else if (has_neighbor_1 || has_neighbor_2)
+    {
+        _vert_ids = {{id, has_neighbor_1 ? neighbor_verts[0] : neighbor_verts[1]}};
 
-    is_active_ = smooth_point2_term_type(vertices.row(id), d, vertices.row(_vert_ids[1]), vertices.row(_vert_ids[2]), _param);
+        const Vector2d dn = -d.normalized();
+        const Vector2d t0 = (vertices.row(_vert_ids[1]) - vertices.row(_vert_ids[0])).normalized();
+    
+        is_active_ = dn.dot(t0) > -param.alpha_t;
+    }
+    else
+    {
+        _vert_ids = {{id}};
+        is_active_ = true;
+    }
 }
 
 int Point2::n_vertices() const
 {
-    return 3;
+    return _vert_ids.size();
 }
 
 double Point2::potential(const Vector<double, dim> &d, const Vector<double, -1, max_size> &x) const
 {
-    return smooth_point2_term<double>(x.segment<dim>(0), d, x.segment<dim>(dim), x.segment<dim>(2 * dim), _param);
+    if (has_neighbor_1 && has_neighbor_2)
+        return smooth_point2_term<double>(x.segment<dim>(0), d, x.segment<dim>(dim), x.segment<dim>(2 * dim), _param, orientable);
+    else if (has_neighbor_1 || has_neighbor_2)
+        return smooth_point2_term_one_side<double>(x.segment<dim>(0), d, x.segment<dim>(dim), _param);
+    else
+        return 1.;
 }
 Vector<double, -1, Point2::max_size+Point2::dim> Point2::grad(const Vector<double, dim> &d, const Vector<double, -1, max_size> &x) const
 {
-    DiffScalarBase::setVariableCount(4*dim);
-    using T = ADGrad<4*dim>;
-    Vector<double, 4*dim> tmp;
-    tmp << d, x;
-    Eigen::Matrix<T, 4, dim> X = slice_positions<T, 4, dim>(tmp);
-    return smooth_point2_term<T>(X.row(1), X.row(0), X.row(2), X.row(3), _param).getGradient();
+    if (has_neighbor_1 && has_neighbor_2)
+    {
+        DiffScalarBase::setVariableCount(4*dim);
+        using T = ADGrad<4*dim>;
+        Vector<double, 4*dim> tmp;
+        tmp << d, x;
+        Eigen::Matrix<T, 4, dim> X = slice_positions<T, 4, dim>(tmp);
+        return smooth_point2_term<T>(X.row(1), X.row(0), X.row(2), X.row(3), _param, orientable).getGradient();
+    }
+    else if (has_neighbor_1 || has_neighbor_2)
+    {
+        DiffScalarBase::setVariableCount(3*dim);
+        using T = ADGrad<3*dim>;
+        Vector<double, 3*dim> tmp;
+        tmp << d, x;
+        Eigen::Matrix<T, 3, dim> X = slice_positions<T, 3, dim>(tmp);
+        return smooth_point2_term_one_side<T>(X.row(1), X.row(0), X.row(2), _param).getGradient();
+    }
+    else
+        return Vector<double, -1, Point2::max_size+Point2::dim>::Zero(x.size() + d.size());
 }
 MatrixMax<double, Point2::max_size+Point2::dim, Point2::max_size+Point2::dim> Point2::hessian(const Vector<double, dim> &d, const Vector<double, -1, max_size> &x) const
 {
-    DiffScalarBase::setVariableCount(4*dim);
-    using T = ADHessian<4*dim>;
-    Vector<double, 4*dim> tmp;
-    tmp << d, x;
-    Eigen::Matrix<T, 4, dim> X = slice_positions<T, 4, dim>(tmp);
-    return smooth_point2_term<T>(X.row(1), X.row(0), X.row(2), X.row(3), _param).getHessian();
-}
-
-template <class scalar>
-scalar smooth_point2_term(
-    const Eigen::Ref<const Vector2<scalar>>& v,
-    const Eigen::Ref<const Vector2<scalar>>& direc,
-    const Eigen::Ref<const Vector2<scalar>>& e0,
-    const Eigen::Ref<const Vector2<scalar>>& e1,
-    const ParameterType& param)
-{
-    const Vector2<scalar> dn = -direc.normalized();
-    const Vector2<scalar> t0 = (e0 - v).normalized(), t1 = (v - e1).normalized();
-
-    const scalar tangent_term = Math<scalar>::smooth_heaviside(dn.dot(t0), param.alpha_t, param.beta_t) *
-                        Math<scalar>::smooth_heaviside(-dn.dot(t1), param.alpha_t, param.beta_t);
-
-    const scalar tmp = Math<scalar>::smooth_heaviside(-Math<scalar>::cross2(dn, t0), param.alpha_n, param.beta_n) + 
-                         Math<scalar>::smooth_heaviside(-Math<scalar>::cross2(dn, t1), param.alpha_n, param.beta_n);
-    const scalar normal_term = Math<scalar>::smooth_heaviside(tmp - 1., param.alpha_n, 0);
-
-    return tangent_term * normal_term * ((e0 - v).norm() + (e1 - v).norm()) / 2.;
-}
-
-bool smooth_point2_term_type(
-    const Eigen::Ref<const Vector2d>& v,
-    const Eigen::Ref<const Vector2d>& direc,
-    const Eigen::Ref<const Vector2d>& e0,
-    const Eigen::Ref<const Vector2d>& e1,
-    const ParameterType& param)
-{
-    const Vector2d dn = -direc.normalized();
-    const Vector2d t0 = (e0 - v).normalized(), t1 = (v - e1).normalized();
-
-    if (dn.dot(t0) <= -param.alpha_t || -dn.dot(t1) <= -param.alpha_t)
-        return false;
-
-    const double tmp = Math<double>::smooth_heaviside(-Math<double>::cross2(dn, t0), param.alpha_n, param.beta_n) + 
-                         Math<double>::smooth_heaviside(-Math<double>::cross2(dn, t1), param.alpha_n, param.beta_n);
-    if (tmp <= 1. - param.alpha_n)
-        return false;
-
-    return true;
+    if (has_neighbor_1 && has_neighbor_2)
+    {
+        DiffScalarBase::setVariableCount(4*dim);
+        using T = ADHessian<4*dim>;
+        Vector<double, 4*dim> tmp;
+        tmp << d, x;
+        Eigen::Matrix<T, 4, dim> X = slice_positions<T, 4, dim>(tmp);
+        return smooth_point2_term<T>(X.row(1), X.row(0), X.row(2), X.row(3), _param, orientable).getHessian();
+    }
+    else if (has_neighbor_1 || has_neighbor_2)
+    {
+        DiffScalarBase::setVariableCount(3*dim);
+        using T = ADHessian<3*dim>;
+        Vector<double, 3*dim> tmp;
+        tmp << d, x;
+        Eigen::Matrix<T, 3, dim> X = slice_positions<T, 3, dim>(tmp);
+        return smooth_point2_term_one_side<T>(X.row(1), X.row(0), X.row(2), _param).getHessian();
+    }
+    else
+        return MatrixMax<double, -1, Point2::max_size+Point2::dim>::Zero(x.size() + d.size(), x.size() + d.size());
 }
 }

src/ipc/smooth_contact/primitives/point2.hpp

@@ -36,37 +36,9 @@ class Point2 : public Primitive {
     MatrixMax<double, max_size + dim, max_size + dim> hessian(
         const Vector<double, dim>& d,
         const Vector<double, -1, max_size>& x) const;
-};
-
-/// @brief
-/// @param v
-/// @param direc points from v
-/// @param e0
-/// @param e1
-/// @param alpha
-/// @param beta
-/// @return
-template <class scalar>
-scalar smooth_point2_term(
-    const Eigen::Ref<const Vector2<scalar>>& v,
-    const Eigen::Ref<const Vector2<scalar>>& direc,
-    const Eigen::Ref<const Vector2<scalar>>& e0,
-    const Eigen::Ref<const Vector2<scalar>>& e1,
-    const ParameterType& param);
-
-/// @brief
-/// @param v
-/// @param direc points from v
-/// @param e0
-/// @param e1
-/// @param alpha
-/// @param beta
-/// @return
-bool smooth_point2_term_type(
-    const Eigen::Ref<const Vector2<double>>& v,
-    const Eigen::Ref<const Vector2<double>>& direc,
-    const Eigen::Ref<const Vector2<double>>& e0,
-    const Eigen::Ref<const Vector2<double>>& e1,
-    const ParameterType& param);
 
+private:
+    bool has_neighbor_1, has_neighbor_2;
+    bool orientable;
+};
 } // namespace ipc