hack

Author: Huangzizhou

src/ipc/smooth_contact/collisions/smooth_collision.cpp

@@ -146,22 +146,45 @@ double SmoothCollisionTemplate<PrimitiveA, PrimitiveB>::operator()(
     const double dist = closest_direction.norm();
 
     assert(positions.size() == pA->n_dofs() + pB->n_dofs());
-    double a1 = pA->potential(closest_direction, positions.head(pA->n_dofs()));
-    double a2 = pB->potential(-closest_direction, positions.tail(pB->n_dofs()));
-    double a3 = Math<double>::inv_barrier(dist / Super::dhat(), params.r);
-    double a4 =
-        PrimitiveDistanceTemplate<PrimitiveA, PrimitiveB, double>::mollifier(
-            x, dist * dist);
 
-    if (params.r == 0)
-        logger().error("Invalid param!");
+    if constexpr (std::is_same_v<PrimitiveA, Edge2> && std::is_same_v<PrimitiveB, Point2> && dim == 2)
+    {
+        double a1 = pA->potential(closest_direction, positions.head(pA->n_dofs()));
+        double a2 = pB->potential(-closest_direction, positions.tail(pB->n_dofs()));
+        double a3 = Math<double>::inv_barrier(dist / Super::dhat(), params.r);
+        
+        {
+            const Vector2<double> p = positions.template segment<2>(4);
+            const Vector2<double> e0 = positions.template segment<2>(0);
+            const Vector2<double> e1 = positions.template segment<2>(2);
 
-    if (dist < 1e-12)
-        logger().warn(
-            "pair distance {:.3e}, dhat {:.3e}, r {}, barrier {:.3e}, mollifier {:.3e}, orient {:.3e} {:.3e}",
-            dist, Super::dhat(), params.r, a3, a4, a1, a2);
+            double tangent_term = Math<double>::smooth_heaviside(-(p - e0).dot(e1 - e0) / (p - e0).norm() / (e1 - e0).norm(), params.alpha_t, params.beta_t);
+            double b0 = Math<double>::inv_barrier((p - e0).norm() / Super::dhat(), params.r);
+            a3 -= tangent_term * b0;
+
+            tangent_term = Math<double>::smooth_heaviside(-(p - e1).dot(e0 - e1) / (p - e1).norm() / (e0 - e1).norm(), params.alpha_t, params.beta_t);
+            b0 = Math<double>::inv_barrier((p - e1).norm() / Super::dhat(), params.r);
+            a3 -= tangent_term * b0;
+        }
 
-    return a1 * a2 * a3 * a4;
+        return a1 * a2 * a3;
+    }
+    else
+    {
+        double a1 = pA->potential(closest_direction, positions.head(pA->n_dofs()));
+        double a2 = pB->potential(-closest_direction, positions.tail(pB->n_dofs()));
+        double a3 = Math<double>::inv_barrier(dist / Super::dhat(), params.r);
+        double a4 =
+            PrimitiveDistanceTemplate<PrimitiveA, PrimitiveB, double>::mollifier(
+                x, dist * dist);
+
+        if (dist < 1e-12)
+            logger().warn(
+                "pair distance {:.3e}, barrier {:.3e}, mollifier {:.3e}, orient {:.3e} {:.3e}",
+                dist, a3, a4, a1, a2);
+
+        return a1 * a2 * a3 * a4;
+    }
 }
 
 template <typename PrimitiveA, typename PrimitiveB>

src/ipc/smooth_contact/primitives/edge2.cpp

@@ -22,38 +22,39 @@ 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();
+    return 1.;
+    // 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>();
-    const double len = t.norm();
-    Vector6d g;
-    g.setZero();
-    g.segment<2>(2) = -t / len;
-    g.segment<2>(4) = t / len;
-    return g;
+    return Vector6d::Zero();
+    // const Vector2d t = x.tail<2>() - x.head<2>();
+    // const double len = t.norm();
+    // Vector6d g;
+    // g.setZero();
+    // g.segment<2>(2) = -t / len;
+    // g.segment<2>(4) =  t / len;
+    // return g;
 }
 
 Matrix6d Edge2::hessian(const Vector2d& d, const Vector4d& x) const
 {
     Matrix6d h;
     h.setZero();
-#ifdef DERIVATIVES_WITH_AUTODIFF
-    DiffScalarBase::setVariableCount(4);
-    using T = ADHessian<4>;
-    auto xAD = slice_positions<T, 2, 2>(x);
-    h.block<4, 4>(2, 2) = (xAD.row(0) - xAD.row(1)).norm().getHessian();
-#else
-    const Vector2d t = x.tail<2>() - x.head<2>();
-    const double norm = t.norm();
-    h.block<2, 2>(2, 2) =
-        (Matrix2d::Identity() - t * (1. / norm / norm) * t.transpose()) / norm;
-    h.block<2, 2>(4, 4) = h.block<2, 2>(2, 2);
-    h.block<2, 2>(2, 4) = -h.block<2, 2>(2, 2);
-    h.block<2, 2>(4, 2) = -h.block<2, 2>(2, 2);
-#endif
+// #ifdef DERIVATIVES_WITH_AUTODIFF
+//     DiffScalarBase::setVariableCount(4);
+//     using T = ADHessian<4>;
+//     auto xAD = slice_positions<T, 2, 2>(x);
+//     h.block<4, 4>(2, 2) = (xAD.row(0) - xAD.row(1)).norm().getHessian();
+// #else
+//     const Vector2d t = x.tail<2>() - x.head<2>();
+//     const double norm = t.norm();
+//     h.block<2, 2>(2, 2) = (Matrix2d::Identity() - t * (1. / norm / norm) * t.transpose()) / norm;
+//     h.block<2, 2>(4, 4) = h.block<2, 2>(2, 2);
+//     h.block<2, 2>(2, 4) = -h.block<2, 2>(2, 2);
+//     h.block<2, 2>(4, 2) = -h.block<2, 2>(2, 2);
+// #endif
     return h;
 }
 } // namespace ipc

src/ipc/smooth_contact/primitives/point2.cpp

@@ -43,7 +43,7 @@ namespace {
         const scalar tangent_term = Math<scalar>::smooth_heaviside(
             dn.dot(t0) / t0.norm(), param.alpha_t, param.beta_t);
 
-        return tangent_term * t0.norm();
+        return tangent_term; // * t0.norm();
     }
 } // namespace
 
@@ -75,7 +75,7 @@ scalar smooth_point2_term(
             * Math<scalar>::smooth_heaviside(tmp - 1., param.alpha_n, 0);
     }
 
-    return val * ((e0 - v).norm() + (e1 - v).norm()) / 2.;
+    return val; // * ((e0 - v).norm() + (e1 - v).norm()) / 2.;
 }
 
 Point2::Point2(