From d21d296f4db420127862ff4af1e446f9458e55cc Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Fri, 27 Oct 2023 14:10:09 -0400 Subject: [PATCH 1/5] Fix shape derivative of mollified edge-edge contact --- notebooks/ee-mollifier-shape-derivative.ipynb | 268 ++++++++++++++++ notebooks/generate_cpp_code.py | 2 +- src/ipc/collisions/collision_constraint.cpp | 62 ++++ src/ipc/collisions/collision_constraint.hpp | 27 ++ src/ipc/collisions/collision_constraints.cpp | 50 +-- src/ipc/collisions/edge_edge.cpp | 47 +++ src/ipc/collisions/edge_edge.hpp | 7 + src/ipc/distance/edge_edge_mollifier.cpp | 140 ++++++++- src/ipc/distance/edge_edge_mollifier.hpp | 86 ++++++ tests/src/tests/distance/CMakeLists.txt | 1 + .../distance/test_edge_edge_mollifier.cpp | 286 ++++++++++++++++++ 11 files changed, 927 insertions(+), 49 deletions(-) create mode 100644 notebooks/ee-mollifier-shape-derivative.ipynb create mode 100644 tests/src/tests/distance/test_edge_edge_mollifier.cpp diff --git a/notebooks/ee-mollifier-shape-derivative.ipynb b/notebooks/ee-mollifier-shape-derivative.ipynb new file mode 100644 index 00000000..36c245fd --- /dev/null +++ b/notebooks/ee-mollifier-shape-derivative.ipynb @@ -0,0 +1,268 @@ +{ + "cells": [ + { + "cell_type": "code", + "execution_count": 1, + "metadata": {}, + "outputs": [], + "source": [ + "import sympy\n", + "\n", + "from generate_cpp_code import *" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# Mollifier" + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": {}, + "outputs": [ + { + "data": { + "text/latex": [ + "$\\displaystyle \\begin{cases} \\frac{x \\left(2 eps_{x} - x\\right)}{eps_{x}^{2}} & \\text{for}\\: eps_{x} > x \\\\1.0 & \\text{otherwise} \\end{cases}$" + ], + "text/plain": [ + "Piecewise((x*(2*eps_x - x)/eps_x**2, eps_x > x), (1.0, True))" + ] + }, + "execution_count": 2, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "x = sympy.Symbol('x')\n", + "eps_x = sympy.Symbol('eps_x')\n", + "\n", + "m = sympy.Piecewise(\n", + " ((-x / eps_x + 2) * x / eps_x, x < eps_x),\n", + " (1.0, True)\n", + ")\n", + "\n", + "m.simplify()" + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "metadata": {}, + "outputs": [ + { + "data": { + "text/latex": [ + "$\\displaystyle \\begin{cases} \\frac{2 \\left(eps_{x} - x\\right)}{eps_{x}^{2}} & \\text{for}\\: eps_{x} > x \\\\0 & \\text{otherwise} \\end{cases}$" + ], + "text/plain": [ + "Piecewise((2*(eps_x - x)/eps_x**2, eps_x > x), (0, True))" + ] + }, + "execution_count": 3, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "m.diff(x).simplify()" + ] + }, + { + "cell_type": "code", + "execution_count": 4, + "metadata": {}, + "outputs": [ + { + "data": { + "text/latex": [ + "$\\displaystyle \\begin{cases} - \\frac{2}{eps_{x}^{2}} & \\text{for}\\: eps_{x} > x \\\\0 & \\text{otherwise} \\end{cases}$" + ], + "text/plain": [ + "Piecewise((-2/eps_x**2, eps_x > x), (0, True))" + ] + }, + "execution_count": 4, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "m.diff(x).diff(x).simplify()" + ] + }, + { + "cell_type": "code", + "execution_count": 5, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "return ((eps_x > x) ? (\n", + " -2*x*(eps_x - x)/std::pow(eps_x, 3)\n", + ")\n", + ": (\n", + " 0\n", + "));\n" + ] + }, + { + "data": { + "text/latex": [ + "$\\displaystyle \\begin{cases} \\frac{2 x \\left(- eps_{x} + x\\right)}{eps_{x}^{3}} & \\text{for}\\: eps_{x} > x \\\\0 & \\text{otherwise} \\end{cases}$" + ], + "text/plain": [ + "Piecewise((2*x*(-eps_x + x)/eps_x**3, eps_x > x), (0, True))" + ] + }, + "execution_count": 5, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "print(generate_code(m.diff(eps_x).simplify()))\n", + "m.diff(eps_x).simplify()" + ] + }, + { + "cell_type": "code", + "execution_count": 6, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "return ((eps_x > x) ? (\n", + " -2*(eps_x - 2*x)/std::pow(eps_x, 3)\n", + ")\n", + ": (\n", + " 0\n", + "));\n" + ] + } + ], + "source": [ + "print(generate_code(m.diff(x).diff(eps_x).simplify()))" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# $\\nabla_x \\varepsilon(x)$" + ] + }, + { + "cell_type": "code", + "execution_count": 7, + "metadata": {}, + "outputs": [ + { + "data": { + "text/latex": [ + "$\\displaystyle scale \\left(\\left(ea0x - ea1x\\right)^{2} + \\left(ea0y - ea1y\\right)^{2} + \\left(ea0z - ea1z\\right)^{2}\\right) \\left(\\left(eb0x - eb1x\\right)^{2} + \\left(eb0y - eb1y\\right)^{2} + \\left(eb0z - eb1z\\right)^{2}\\right)$" + ], + "text/plain": [ + "scale*((ea0x - ea1x)**2 + (ea0y - ea1y)**2 + (ea0z - ea1z)**2)*((eb0x - eb1x)**2 + (eb0y - eb1y)**2 + (eb0z - eb1z)**2)" + ] + }, + "execution_count": 7, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "ea0 = sympy.Matrix(sympy.symbols('ea0x ea0y ea0z'))\n", + "ea1 = sympy.Matrix(sympy.symbols('ea1x ea1y ea1z'))\n", + "eb0 = sympy.Matrix(sympy.symbols('eb0x eb0y eb0z'))\n", + "eb1 = sympy.Matrix(sympy.symbols('eb1x eb1y eb1z'))\n", + "\n", + "scale = sympy.Symbol('scale')\n", + "\n", + "eps_x = (scale * (ea0 - ea1).dot(ea0 - ea1) * (eb0 - eb1).dot(eb0 - eb1)).simplify()\n", + "eps_x" + ] + }, + { + "cell_type": "code", + "execution_count": 8, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "\n", + "\n", + "void edge_edge_mollifier_threshold_gradient(double ea0x, double ea0y, double ea0z, double ea1x, double ea1y, double ea1z, double eb0x, double eb0y, double eb0z, double eb1x, double eb1y, double eb1z, double grad[12]){\n", + "const auto t0 = ea0x - ea1x;\n", + "const auto t1 = eb0x - eb1x;\n", + "const auto t2 = eb0y - eb1y;\n", + "const auto t3 = eb0z - eb1z;\n", + "const auto t4 = 2*scale;\n", + "const auto t5 = t4*(std::pow(t1, 2) + std::pow(t2, 2) + std::pow(t3, 2));\n", + "const auto t6 = t0*t5;\n", + "const auto t7 = ea0y - ea1y;\n", + "const auto t8 = t5*t7;\n", + "const auto t9 = ea0z - ea1z;\n", + "const auto t10 = t5*t9;\n", + "const auto t11 = t4*(std::pow(t0, 2) + std::pow(t7, 2) + std::pow(t9, 2));\n", + "const auto t12 = t1*t11;\n", + "const auto t13 = t11*t2;\n", + "const auto t14 = t11*t3;\n", + "grad[0] = t6;\n", + "grad[1] = t8;\n", + "grad[2] = t10;\n", + "grad[3] = -t6;\n", + "grad[4] = -t8;\n", + "grad[5] = -t10;\n", + "grad[6] = t12;\n", + "grad[7] = t13;\n", + "grad[8] = t14;\n", + "grad[9] = -t12;\n", + "grad[10] = -t13;\n", + "grad[11] = -t14;\n", + "}\n", + "\n" + ] + } + ], + "source": [ + "x = sympy.Matrix([ea0, ea1, eb0, eb1])\n", + "\n", + "g = CXXGradientGenerator(eps_x, x, 'edge_edge_mollifier_threshold_gradient')\n", + "\n", + "print(g())" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "Python 3", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.11.6" + } + }, + "nbformat": 4, + "nbformat_minor": 2 +} diff --git a/notebooks/generate_cpp_code.py b/notebooks/generate_cpp_code.py index 1d5e4775..b0720a9e 100644 --- a/notebooks/generate_cpp_code.py +++ b/notebooks/generate_cpp_code.py @@ -7,7 +7,7 @@ from utils import jacobian -def generate_code(expr, out_var_name="J"): +def generate_code(expr, out_var_name=None): CSE_results = sympy.cse( expr, sympy.numbered_symbols("t"), optimizations='basic') lines = [] diff --git a/src/ipc/collisions/collision_constraint.cpp b/src/ipc/collisions/collision_constraint.cpp index bb485480..5eb1dfaa 100644 --- a/src/ipc/collisions/collision_constraint.cpp +++ b/src/ipc/collisions/collision_constraint.cpp @@ -1,6 +1,7 @@ #include "collision_constraint.hpp" #include +#include namespace ipc { @@ -70,4 +71,65 @@ MatrixMax12d CollisionConstraint::compute_potential_hessian( return weight * (term1 + term2); } +void CollisionConstraint::compute_shape_derivative( + const Eigen::MatrixXd& rest_positions, + const Eigen::MatrixXd& vertices, + const Eigen::MatrixXi& edges, + const Eigen::MatrixXi& faces, + const double dhat, + std::vector>& triplets) const +{ + compute_shape_derivative_first_term( + rest_positions, vertices, edges, faces, dhat, triplets); + + local_hessian_to_global_triplets( + compute_shape_derivative_second_term( + rest_positions, vertices, edges, faces, dhat), + vertex_ids(edges, faces), vertices.cols(), triplets); +} + +void CollisionConstraint::compute_shape_derivative_first_term( + const Eigen::MatrixXd& rest_positions, + const Eigen::MatrixXd& vertices, + const Eigen::MatrixXi& edges, + const Eigen::MatrixXi& faces, + const double dhat, + std::vector>& triplets) const +{ + if (weight_gradient.size() != vertices.size()) { + throw std::runtime_error( + "Shape derivative is not computed for contact constraint!"); + } + + const int dim = vertices.cols(); + + VectorMax12d grad_b = + compute_potential_gradient(vertices, edges, faces, dhat); + assert(weight != 0); + grad_b.array() /= weight; // remove weight + + const std::array ids = vertex_ids(edges, faces); + for (int i = 0; i < num_vertices(); i++) { + for (int d = 0; d < dim; d++) { + using Itr = Eigen::SparseVector::InnerIterator; + for (Itr j(weight_gradient); j; ++j) { + triplets.emplace_back( + ids[i] * dim + d, j.index(), + grad_b[dim * i + d] * j.value()); + } + } + } +} + +MatrixMax12d CollisionConstraint::compute_shape_derivative_second_term( + const Eigen::MatrixXd& rest_positions, + const Eigen::MatrixXd& vertices, + const Eigen::MatrixXi& edges, + const Eigen::MatrixXi& faces, + const double dhat) const +{ + // w ∇ₓ∇ᵤb = w ∇ᵤ²b + return compute_potential_hessian(vertices, edges, faces, dhat, false); +} + } // namespace ipc diff --git a/src/ipc/collisions/collision_constraint.hpp b/src/ipc/collisions/collision_constraint.hpp index 708ce181..66d56cda 100644 --- a/src/ipc/collisions/collision_constraint.hpp +++ b/src/ipc/collisions/collision_constraint.hpp @@ -38,9 +38,36 @@ class CollisionConstraint : virtual public CollisionStencil { const double dhat, const bool project_hessian_to_psd) const; + /// Compute the derivative of the potential gradient wrt the shape. + virtual void compute_shape_derivative( + const Eigen::MatrixXd& rest_positions, + const Eigen::MatrixXd& vertices, + const Eigen::MatrixXi& edges, + const Eigen::MatrixXi& faces, + const double dhat, + std::vector>& triplets) const; + double dmin = 0; double weight = 1; Eigen::SparseVector weight_gradient; + +protected: + /// Compute (∇ₓw)(∇ᵤb)ᵀ + virtual void compute_shape_derivative_first_term( + const Eigen::MatrixXd& rest_positions, + const Eigen::MatrixXd& vertices, + const Eigen::MatrixXi& edges, + const Eigen::MatrixXi& faces, + const double dhat, + std::vector>& triplets) const; + + /// Compute w ∇ₓ∇ᵤb + virtual MatrixMax12d compute_shape_derivative_second_term( + const Eigen::MatrixXd& rest_positions, + const Eigen::MatrixXd& vertices, + const Eigen::MatrixXi& edges, + const Eigen::MatrixXi& faces, + const double dhat) const; }; } // namespace ipc diff --git a/src/ipc/collisions/collision_constraints.cpp b/src/ipc/collisions/collision_constraints.cpp index 4de5ecd8..216bd541 100644 --- a/src/ipc/collisions/collision_constraints.cpp +++ b/src/ipc/collisions/collision_constraints.cpp @@ -439,13 +439,11 @@ Eigen::SparseMatrix CollisionConstraints::compute_shape_derivative( const Eigen::MatrixXd& vertices, const double dhat) const { - Eigen::SparseMatrix shape_derivative = - compute_potential_hessian(mesh, vertices, dhat, false); - - const Eigen::MatrixXi& edges = mesh.edges(); - const Eigen::MatrixXi& faces = mesh.faces(); + assert(vertices.rows() == mesh.num_vertices()); - const int dim = vertices.cols(); + if (empty()) { + return Eigen::SparseMatrix(vertices.size(), vertices.size()); + } tbb::enumerable_thread_specific>> storage; @@ -455,42 +453,15 @@ Eigen::SparseMatrix CollisionConstraints::compute_shape_derivative( [&](const tbb::blocked_range& r) { auto& local_triplets = storage.local(); - // for (size_t ci = 0; ci < constraint_set.size(); ci++) { - for (size_t ci = r.begin(); ci < r.end(); ci++) { - - const CollisionConstraint& constraint = (*this)[ci]; - const Eigen::SparseVector& weight_gradient = - constraint.weight_gradient; - if (weight_gradient.size() != vertices.size()) { - throw std::runtime_error( - "Shape derivative is not computed for contact constraint!"); - } - - VectorMax12d local_barrier_grad = - constraint.compute_potential_gradient( - vertices, edges, faces, dhat); - assert(constraint.weight != 0); - local_barrier_grad.array() /= constraint.weight; - - const std::array ids = - constraint.vertex_ids(edges, faces); - assert(local_barrier_grad.size() % dim == 0); - const int n_verts = local_barrier_grad.size() / dim; - assert(ids.size() >= n_verts); // Can be extra ids - - for (int i = 0; i < n_verts; i++) { - for (int d = 0; d < dim; d++) { - using Itr = Eigen::SparseVector::InnerIterator; - for (Itr j(weight_gradient); j; ++j) { - local_triplets.emplace_back( - ids[i] * dim + d, j.index(), - local_barrier_grad[dim * i + d] * j.value()); - } - } - } + for (size_t i = r.begin(); i < r.end(); i++) { + (*this)[i].compute_shape_derivative( + mesh.rest_positions(), vertices, mesh.edges(), mesh.faces(), + dhat, local_triplets); } }); + Eigen::SparseMatrix shape_derivative( + vertices.size(), vertices.size()); for (const auto& local_triplets : storage) { Eigen::SparseMatrix local_shape_derivative( vertices.size(), vertices.size()); @@ -498,7 +469,6 @@ Eigen::SparseMatrix CollisionConstraints::compute_shape_derivative( local_triplets.begin(), local_triplets.end()); shape_derivative += local_shape_derivative; } - return shape_derivative; } diff --git a/src/ipc/collisions/edge_edge.cpp b/src/ipc/collisions/edge_edge.cpp index c9614b06..148ffdf3 100644 --- a/src/ipc/collisions/edge_edge.cpp +++ b/src/ipc/collisions/edge_edge.cpp @@ -122,6 +122,53 @@ MatrixMax12d EdgeEdgeConstraint::compute_potential_hessian( return project_hessian_to_psd ? project_to_psd(hess) : hess; } +MatrixMax12d EdgeEdgeConstraint::compute_shape_derivative_second_term( + const Eigen::MatrixXd& rest_positions, // = x + const Eigen::MatrixXd& vertices, // = x + u + const Eigen::MatrixXi& edges, + const Eigen::MatrixXi& faces, + const double dhat) const +{ + const auto& [ea0_rest, ea1_rest, eb0_rest, eb1_rest] = + this->vertices(rest_positions, edges, faces); + const auto& [ea0, ea1, eb0, eb1] = this->vertices(vertices, edges, faces); + + // b(d(x+u)) + const double barrier = + CollisionConstraint::compute_potential(vertices, edges, faces, dhat); + // ∇ᵤ b(d(x+u)) + const Vector12d barrier_grad = + CollisionConstraint::compute_potential_gradient( + vertices, edges, faces, dhat); + // ∇ᵤ² b(d(x+u)) + const Matrix12d barrier_hess = + CollisionConstraint::compute_potential_hessian( + vertices, edges, faces, dhat, /*project_hessian_to_psd=*/false); + + // ε(x) + const double _eps_x = + edge_edge_mollifier_threshold(ea1_rest, ea0_rest, eb0_rest, eb1_rest); + + // m(x,u) + const double mollifier = edge_edge_mollifier(ea0, ea1, eb0, eb1, _eps_x); + // ∇ᵤ m(x,u) + const Vector12d mollifier_gradu = + edge_edge_mollifier_gradient(ea0, ea1, eb0, eb1, _eps_x); + // ∇ₓ m(x,u) + const Vector12d mollifier_gradx = edge_edge_mollifier_gradient_wrt_x( + ea0_rest, ea1_rest, eb0_rest, eb1_rest, ea0, ea1, eb0, eb1); + // ∇ₓ∇ᵤ m(x,u) + const Matrix12d mollifier_jacobian = + edge_edge_mollifier_gradient_jacobian_wrt_x( + ea0_rest, ea1_rest, eb0_rest, eb1_rest, ea0, ea1, eb0, eb1); + + // Only compute the second term of the shape derivative + // ∇ₓ (b ∇ᵤm + m ∇ᵤb) = b ∇ₓ∇ᵤm + (∇ₓm)(∇ᵤb)ᵀ + (∇ᵤb)(∇ᵤm)ᵀ + m ∇ᵤ²b + return barrier * mollifier_jacobian + + mollifier_gradx * barrier_grad.transpose() + + barrier_grad * mollifier_gradu.transpose() + mollifier * barrier_hess; +} + bool EdgeEdgeConstraint::operator==(const EdgeEdgeConstraint& other) const { return EdgeEdgeCandidate::operator==(other) && dtype == other.dtype; diff --git a/src/ipc/collisions/edge_edge.hpp b/src/ipc/collisions/edge_edge.hpp index 5e68d20f..7498a9c4 100644 --- a/src/ipc/collisions/edge_edge.hpp +++ b/src/ipc/collisions/edge_edge.hpp @@ -72,6 +72,13 @@ class EdgeEdgeConstraint : public EdgeEdgeCandidate, protected: virtual EdgeEdgeDistanceType known_dtype() const override { return dtype; } + + MatrixMax12d compute_shape_derivative_second_term( + const Eigen::MatrixXd& rest_positions, + const Eigen::MatrixXd& vertices, + const Eigen::MatrixXi& edges, + const Eigen::MatrixXi& faces, + const double dhat) const override; }; } // namespace ipc diff --git a/src/ipc/distance/edge_edge_mollifier.cpp b/src/ipc/distance/edge_edge_mollifier.cpp index 3f9f2100..376d5b95 100644 --- a/src/ipc/distance/edge_edge_mollifier.cpp +++ b/src/ipc/distance/edge_edge_mollifier.cpp @@ -43,31 +43,43 @@ double edge_edge_mollifier(const double x, const double eps_x) { if (x < eps_x) { const double x_div_eps_x = x / eps_x; - return (-x_div_eps_x + 2.0) * x_div_eps_x; + return (-x_div_eps_x + 2) * x_div_eps_x; } else { - return 1.0; + return 1; } } double edge_edge_mollifier_gradient(const double x, const double eps_x) { if (x < eps_x) { - const double one_div_eps_x = 1.0 / eps_x; - return 2.0 * one_div_eps_x * (-one_div_eps_x * x + 1.0); + const double one_div_eps_x = 1 / eps_x; + return 2 * one_div_eps_x * (-one_div_eps_x * x + 1); } else { - return 0.0; + return 0; } } double edge_edge_mollifier_hessian(const double x, const double eps_x) { if (x < eps_x) { - return -2.0 / (eps_x * eps_x); + return -2 / (eps_x * eps_x); } else { - return 0.0; + return 0; } } +double +edge_edge_mollifier_derivative_wrt_eps_x(const double x, const double eps_x) +{ + return x < eps_x ? (2 * x * (-eps_x + x) / (eps_x * eps_x * eps_x)) : 0.0; +} + +double edge_edge_mollifier_gradient_derivative_wrt_eps_x( + const double x, const double eps_x) +{ + return x < eps_x ? (2 * (-eps_x + 2 * x) / (eps_x * eps_x * eps_x)) : 0.0; +} + double edge_edge_mollifier( const Eigen::Ref& ea0, const Eigen::Ref& ea1, @@ -80,7 +92,7 @@ double edge_edge_mollifier( if (ee_cross_norm_sqr < eps_x) { return edge_edge_mollifier(ee_cross_norm_sqr, eps_x); } else { - return 1.0; + return 1; } } @@ -123,6 +135,59 @@ Matrix12d edge_edge_mollifier_hessian( } } +Vector12d edge_edge_mollifier_gradient_wrt_x( + const Eigen::Ref& ea0_rest, + const Eigen::Ref& ea1_rest, + const Eigen::Ref& eb0_rest, + const Eigen::Ref& eb1_rest, + const Eigen::Ref& ea0, + const Eigen::Ref& ea1, + const Eigen::Ref& eb0, + const Eigen::Ref& eb1) +{ + const double eps_x = + edge_edge_mollifier_threshold(ea0_rest, ea1_rest, eb0_rest, eb1_rest); + const double ee_cross_norm_sqr = + edge_edge_cross_squarednorm(ea0, ea1, eb0, eb1); + if (ee_cross_norm_sqr < eps_x) { + // ∇ₓ m = ∂m/∂ε ∇ₓε + return edge_edge_mollifier_derivative_wrt_eps_x( + ee_cross_norm_sqr, eps_x) + * edge_edge_mollifier_gradient(ea0, ea1, eb0, eb1, eps_x); + } else { + return Vector12d::Zero(); + } +} + +Matrix12d edge_edge_mollifier_gradient_jacobian_wrt_x( + const Eigen::Ref& ea0_rest, + const Eigen::Ref& ea1_rest, + const Eigen::Ref& eb0_rest, + const Eigen::Ref& eb1_rest, + const Eigen::Ref& ea0, + const Eigen::Ref& ea1, + const Eigen::Ref& eb0, + const Eigen::Ref& eb1) +{ + const double eps_x = + edge_edge_mollifier_threshold(ea0_rest, ea1_rest, eb0_rest, eb1_rest); + const double ee_cross_norm_sqr = + edge_edge_cross_squarednorm(ea0, ea1, eb0, eb1); + if (ee_cross_norm_sqr < eps_x) { + // ∂²m/∂ε∂s (∇ₓε)(∇ᵤs(x+u))ᵀ + ∂m/∂s ∇ᵤ²s(x+u) + return edge_edge_mollifier_gradient_derivative_wrt_eps_x( + ee_cross_norm_sqr, eps_x) + * edge_edge_mollifier_threshold_gradient( + ea0_rest, ea1_rest, eb0_rest, eb1_rest) + * edge_edge_cross_squarednorm_gradient(ea0, ea1, eb0, eb1) + .transpose() + + edge_edge_mollifier_gradient(ee_cross_norm_sqr, eps_x) + * edge_edge_cross_squarednorm_hessian(ea0, ea1, eb0, eb1); + } else { + return Matrix12d::Zero(); + } +} + double edge_edge_mollifier_threshold( const Eigen::Ref& ea0_rest, const Eigen::Ref& ea1_rest, @@ -133,6 +198,20 @@ double edge_edge_mollifier_threshold( * (eb0_rest - eb1_rest).squaredNorm(); } +Vector12d edge_edge_mollifier_threshold_gradient( + const Eigen::Ref& ea0_rest, + const Eigen::Ref& ea1_rest, + const Eigen::Ref& eb0_rest, + const Eigen::Ref& eb1_rest) +{ + Vector12d grad; + autogen::edge_edge_mollifier_threshold_gradient( + ea0_rest[0], ea0_rest[1], ea0_rest[2], ea1_rest[0], ea1_rest[1], + ea1_rest[2], eb0_rest[0], eb0_rest[1], eb0_rest[2], eb1_rest[0], + eb1_rest[1], eb1_rest[2], grad.data(), /*scale=*/1e-3); + return grad; +} + namespace autogen { // This function was generated by the Symbolic Math Toolbox version 8.3. @@ -427,5 +506,50 @@ namespace autogen { H[143] = t74; } + void edge_edge_mollifier_threshold_gradient( + double ea0x, + double ea0y, + double ea0z, + double ea1x, + double ea1y, + double ea1z, + double eb0x, + double eb0y, + double eb0z, + double eb1x, + double eb1y, + double eb1z, + double grad[12], + double scale) + { + const auto t0 = ea0x - ea1x; + const auto t1 = eb0x - eb1x; + const auto t2 = eb0y - eb1y; + const auto t3 = eb0z - eb1z; + const auto t4 = 2 * scale; + const auto t5 = t4 * ((t1 * t1) + (t2 * t2) + (t3 * t3)); + const auto t6 = t0 * t5; + const auto t7 = ea0y - ea1y; + const auto t8 = t5 * t7; + const auto t9 = ea0z - ea1z; + const auto t10 = t5 * t9; + const auto t11 = t4 * ((t0 * t0) + (t7 * t7) + (t9 * t9)); + const auto t12 = t1 * t11; + const auto t13 = t11 * t2; + const auto t14 = t11 * t3; + grad[0] = t6; + grad[1] = t8; + grad[2] = t10; + grad[3] = -t6; + grad[4] = -t8; + grad[5] = -t10; + grad[6] = t12; + grad[7] = t13; + grad[8] = t14; + grad[9] = -t12; + grad[10] = -t13; + grad[11] = -t14; + } + } // namespace autogen } // namespace ipc diff --git a/src/ipc/distance/edge_edge_mollifier.hpp b/src/ipc/distance/edge_edge_mollifier.hpp index dfdb4c18..d82737ce 100644 --- a/src/ipc/distance/edge_edge_mollifier.hpp +++ b/src/ipc/distance/edge_edge_mollifier.hpp @@ -52,12 +52,26 @@ double edge_edge_mollifier(const double x, const double eps_x); /// @return The gradient of the mollifier function for edge-edge distance wrt x. double edge_edge_mollifier_gradient(const double x, const double eps_x); +/// @brief The derivative of the mollifier function for edge-edge distance wrt eps_x. +/// @param x Squared norm of the edge-edge cross product. +/// @param eps_x Mollifier activation threshold. +/// @return The derivative of the mollifier function for edge-edge distance wrt eps_x. +double +edge_edge_mollifier_derivative_wrt_eps_x(const double x, const double eps_x); + /// @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. double edge_edge_mollifier_hessian(const double x, const double eps_x); +/// @brief The derivative of the gradient of the mollifier function for edge-edge distance wrt eps_x. +/// @param x Squared norm of the edge-edge cross product. +/// @param eps_x Mollifier activation threshold. +/// @return The derivative of the gradient of the mollifier function for edge-edge distance wrt eps_x. +double edge_edge_mollifier_gradient_derivative_wrt_eps_x( + const double x, const double eps_x); + /// @brief Compute a mollifier for the edge-edge distance. /// /// This helps smooth the non-smoothness at close to parallel edges. @@ -103,6 +117,47 @@ Matrix12d edge_edge_mollifier_hessian( const Eigen::Ref& eb1, const double eps_x); +/// @brief Compute the gradient of the mollifier for the edge-edge distance wrt rest positions. +/// @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. +/// @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 derivative of the mollifier wrt rest positions. +Vector12d edge_edge_mollifier_gradient_wrt_x( + const Eigen::Ref& ea0_rest, + const Eigen::Ref& ea1_rest, + const Eigen::Ref& eb0_rest, + const Eigen::Ref& eb1_rest, + const Eigen::Ref& ea0, + const Eigen::Ref& ea1, + const Eigen::Ref& eb0, + const Eigen::Ref& eb1); + +/// @brief Compute the jacobian of the edge-edge distance mollifier's gradient wrt rest positions. +/// @note This is not the hessian of the mollifier wrt rest positions, but the jacobian wrt rest positions of the mollifier's gradient wrt positions. +/// @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. +/// @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 jacobian of the mollifier's gradient wrt rest positions. +Matrix12d edge_edge_mollifier_gradient_jacobian_wrt_x( + const Eigen::Ref& ea0_rest, + const Eigen::Ref& ea1_rest, + const Eigen::Ref& eb0_rest, + const Eigen::Ref& eb1_rest, + const Eigen::Ref& ea0, + const Eigen::Ref& ea1, + const Eigen::Ref& eb0, + const Eigen::Ref& eb1); + /// @brief Compute the threshold of the mollifier edge-edge distance. /// /// This values is computed based on the edges at rest length. @@ -118,6 +173,21 @@ double edge_edge_mollifier_threshold( const Eigen::Ref& eb0_rest, const Eigen::Ref& eb1_rest); +/// @brief Compute the gradient of 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 Gradient of the threshold for edge-edge mollification. +Vector12d edge_edge_mollifier_threshold_gradient( + const Eigen::Ref& ea0_rest, + const Eigen::Ref& ea1_rest, + const Eigen::Ref& eb0_rest, + const Eigen::Ref& eb1_rest); + // Symbolically generated derivatives; namespace autogen { void edge_edge_cross_squarednorm_gradient( @@ -149,5 +219,21 @@ namespace autogen { double v32, double v33, double H[144]); + + void edge_edge_mollifier_threshold_gradient( + double ea0x, + double ea0y, + double ea0z, + double ea1x, + double ea1y, + double ea1z, + double eb0x, + double eb0y, + double eb0z, + double eb1x, + double eb1y, + double eb1z, + double grad[12], + double scale = 1e-3); } // namespace autogen } // namespace ipc diff --git a/tests/src/tests/distance/CMakeLists.txt b/tests/src/tests/distance/CMakeLists.txt index 0c82a14f..4b32635a 100644 --- a/tests/src/tests/distance/CMakeLists.txt +++ b/tests/src/tests/distance/CMakeLists.txt @@ -1,6 +1,7 @@ set(SOURCES # Tests test_distance_type.cpp + test_edge_edge_mollifier.cpp test_edge_edge.cpp test_line_line.cpp test_point_edge.cpp diff --git a/tests/src/tests/distance/test_edge_edge_mollifier.cpp b/tests/src/tests/distance/test_edge_edge_mollifier.cpp new file mode 100644 index 00000000..0ef04b48 --- /dev/null +++ b/tests/src/tests/distance/test_edge_edge_mollifier.cpp @@ -0,0 +1,286 @@ +#include +#include +#include + +#include + +#include + +using namespace ipc; + +std::array get_edges() +{ + Eigen::Vector3d ea0, ea1, eb0, eb1; + SECTION("Perp") + { + ea0 = Eigen::Vector3d(-1, 0, 0); + ea1 = Eigen::Vector3d(1, 0, 0); + eb0 = Eigen::Vector3d(0, -1, 0); + eb1 = Eigen::Vector3d(0, 1, 0); + CHECK( + edge_edge_cross_squarednorm(ea0, ea1, eb0, eb1) + == Catch::Approx(16)); + } + SECTION("Almost parallel") + { + ea0 = Eigen::Vector3d(-1, 0, 0); + ea1 = Eigen::Vector3d(1, 0, 0); + eb0 = Eigen::Vector3d(-1, 1e-9, 0); + eb1 = Eigen::Vector3d(1, -1e-9, 0); + CHECK( + edge_edge_cross_squarednorm(ea0, ea1, eb0, eb1) + == Catch::Approx(0).margin(1e-9)); + } + return { ea0, ea1, eb0, eb1 }; +} + +TEST_CASE("Edge-Edge Cross Squarednorm", "[distance][edge-edge][mollifier]") +{ + const auto& [ea0, ea1, eb0, eb1] = get_edges(); + + Vector12d x; + x << ea0, ea1, eb0, eb1; + + const Vector12d grad = + edge_edge_cross_squarednorm_gradient(ea0, ea1, eb0, eb1); + + // Compute the gradient using finite differences + Eigen::VectorXd fgrad; + fd::finite_gradient( + x, + [](const Eigen::VectorXd& _x) { + return edge_edge_cross_squarednorm( + _x.segment<3>(0), _x.segment<3>(3), _x.segment<3>(6), + _x.segment<3>(9)); + }, + fgrad); + + CHECK(fd::compare_gradient(grad, fgrad)); + + const Matrix12d hess = + edge_edge_cross_squarednorm_hessian(ea0, ea1, eb0, eb1); + + // Compute the hessian using finite differences + Eigen::MatrixXd fhess; + fd::finite_hessian( + x, + [](const Eigen::VectorXd& _x) { + return edge_edge_cross_squarednorm( + _x.segment<3>(0), _x.segment<3>(3), _x.segment<3>(6), + _x.segment<3>(9)); + }, + fhess); + + CHECK(fd::compare_hessian(hess, fhess)); +} + +TEST_CASE("Edge-Edge Mollifier Scalar", "[distance][edge-edge][mollifier]") +{ + const double rel_x = GENERATE(0, 0.5, 1, 2); + const double eps_x = GENERATE(1e-3, 1e-1, 1, 2); + const double x = rel_x * eps_x; + CAPTURE(rel_x, eps_x, x); + + const double m = edge_edge_mollifier(x, eps_x); + CHECK(m >= 0); + CHECK(m <= 1); + if (x > eps_x) + CHECK(m == 1); + + // --- + + const double dm_dx = edge_edge_mollifier_gradient(x, eps_x); + + // Compute the gradient using finite differences + Eigen::VectorXd f_dm_dx; + fd::finite_gradient( + Vector1d::Constant(x), + [eps_x](const Eigen::VectorXd& _x) { + return edge_edge_mollifier(_x(0), eps_x); + }, + f_dm_dx, fd::SECOND, 1e-12); + + CHECK(dm_dx == Catch::Approx(f_dm_dx(0))); + + // --- + + if (std::abs(x - eps_x) > 1e-6) { // mollifier is only C1 at x = eps_x + const double d2m_dx2 = edge_edge_mollifier_hessian(x, eps_x); + + // Compute the gradient using finite differences + Eigen::MatrixXd f_d2m_dx2; + fd::finite_hessian( + Vector1d::Constant(x), + [eps_x](const Eigen::VectorXd& _x) { + return edge_edge_mollifier(_x(0), eps_x); + }, + f_d2m_dx2); + + CHECK(d2m_dx2 == Catch::Approx(f_d2m_dx2(0))); + } + + // --- + + const double dm_deps_x = edge_edge_mollifier_derivative_wrt_eps_x(x, eps_x); + + // Compute the gradient using finite differences + Eigen::VectorXd f_dm_deps_x; + fd::finite_gradient( + Vector1d::Constant(eps_x), + [x](const Eigen::VectorXd& _eps_x) { + return edge_edge_mollifier(x, _eps_x(0)); + }, + f_dm_deps_x, fd::SECOND, 1e-12); + + CHECK(dm_deps_x == Catch::Approx(f_dm_deps_x(0))); + + // --- + + if (std::abs(x - eps_x) > 1e-6) { // mollifier is only C1 at x = eps_x + const double d2m_deps_xdx = + edge_edge_mollifier_gradient_derivative_wrt_eps_x(x, eps_x); + + // Compute the gradient using finite differences + Eigen::VectorXd f_d2m_deps_xdx; + fd::finite_gradient( + Vector1d::Constant(eps_x), + [x](const Eigen::VectorXd& _eps_x) { + return edge_edge_mollifier_gradient(x, _eps_x(0)); + }, + f_d2m_deps_xdx); + + CHECK(d2m_deps_xdx == Catch::Approx(f_d2m_deps_xdx(0))); + } +} + +TEST_CASE("Edge-Edge Mollifier", "[distance][edge-edge][mollifier]") +{ + const Eigen::Vector3d rest_ea0(0, 0, 0); + const Eigen::Vector3d rest_ea1(1, 0, 0); + const Eigen::Vector3d rest_eb0(0, 0, 0); + const Eigen::Vector3d rest_eb1(0, 1, 0); + const auto& [ea0, ea1, eb0, eb1] = get_edges(); + + const double eps_x = + edge_edge_mollifier_threshold(rest_ea0, rest_ea1, rest_eb0, rest_eb1); + + const double m = edge_edge_mollifier(ea0, ea1, eb0, eb1, eps_x); + CHECK(m >= 0); + CHECK(m <= 1); + + Vector12d x; + x << rest_ea0, rest_ea1, rest_eb0, rest_eb1; + + Vector12d v; + v << ea0, ea1, eb0, eb1; + + const Vector12d u = v - x; + + // --- + + const Vector12d grad = + edge_edge_mollifier_gradient(ea0, ea1, eb0, eb1, eps_x); + + // Compute the gradient using finite differences + Eigen::VectorXd fgrad; + fd::finite_gradient( + v, + [eps_x](const Eigen::VectorXd& _v) { + return edge_edge_mollifier( + _v.segment<3>(0), _v.segment<3>(3), _v.segment<3>(6), + _v.segment<3>(9), eps_x); + }, + fgrad); + + CHECK(fd::compare_gradient(grad, fgrad)); + + // --- + + const Matrix12d hess = + edge_edge_mollifier_hessian(ea0, ea1, eb0, eb1, eps_x); + + // Compute the hessian using finite differences + Eigen::MatrixXd fhess; + fd::finite_hessian( + v, + [eps_x](const Eigen::VectorXd& _v) { + return edge_edge_mollifier( + _v.segment<3>(0), _v.segment<3>(3), _v.segment<3>(6), + _v.segment<3>(9), eps_x); + }, + fhess); + + CHECK(fd::compare_hessian(hess, fhess)); + + // --- + + const Vector12d grad_wrt_x = edge_edge_mollifier_gradient_wrt_x( + rest_ea0, rest_ea1, rest_eb0, rest_eb1, ea0, ea1, eb0, eb1); + + // Compute the gradient using finite differences + Eigen::VectorXd fgrad_wrt_x; + fd::finite_gradient( + x, + [u](const Eigen::VectorXd& _x) { + const Eigen::VectorXd _v = _x + u; + return edge_edge_mollifier( + _v.segment<3>(0), _v.segment<3>(3), _v.segment<3>(6), + _v.segment<3>(9), + edge_edge_mollifier_threshold( + _x.segment<3>(0), _x.segment<3>(3), _x.segment<3>(6), + _x.segment<3>(9))); + }, + fgrad_wrt_x); + + CHECK(fd::compare_gradient(grad_wrt_x, fgrad_wrt_x)); + + // --- + + const Matrix12d jac_wrt_x_u = edge_edge_mollifier_gradient_jacobian_wrt_x( + rest_ea0, rest_ea1, rest_eb0, rest_eb1, ea0, ea1, eb0, eb1); + + // Compute the gradient using finite differences + Eigen::MatrixXd fjac_wrt_x_u; + fd::finite_jacobian( + x, + [u](const Eigen::VectorXd& _x) { + const Eigen::VectorXd _v = _x + u; + return edge_edge_mollifier_gradient( + _v.segment<3>(0), _v.segment<3>(3), _v.segment<3>(6), + _v.segment<3>(9), + edge_edge_mollifier_threshold( + _x.segment<3>(0), _x.segment<3>(3), _x.segment<3>(6), + _x.segment<3>(9))); + }, + fjac_wrt_x_u); + + CAPTURE(jac_wrt_x_u, fjac_wrt_x_u); + CHECK(fd::compare_jacobian(jac_wrt_x_u, fjac_wrt_x_u)); +} + +TEST_CASE("Edge-Edge Mollifier Threshold", "[distance][edge-edge][mollifier]") +{ + const auto& [ea0, ea1, eb0, eb1] = get_edges(); + + const double eps_x = edge_edge_mollifier_threshold(ea0, ea1, eb0, eb1); + CHECK(eps_x == Catch::Approx(0.016)); + + const Vector12d grad = + edge_edge_mollifier_threshold_gradient(ea0, ea1, eb0, eb1); + + Vector12d x; + x << ea0, ea1, eb0, eb1; + + // Compute the gradient using finite differences + Eigen::VectorXd fgrad; + fd::finite_gradient( + x, + [](const Eigen::VectorXd& _x) { + return edge_edge_mollifier_threshold( + _x.segment<3>(0), _x.segment<3>(3), _x.segment<3>(6), + _x.segment<3>(9)); + }, + fgrad); + + CHECK(fd::compare_gradient(grad, fgrad)); +} \ No newline at end of file From 7926548e180d35ba36638030526ff5a4b0b5515e Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Fri, 27 Oct 2023 14:20:50 -0400 Subject: [PATCH 2/5] Fix array braces --- tests/src/tests/distance/test_edge_edge_mollifier.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tests/src/tests/distance/test_edge_edge_mollifier.cpp b/tests/src/tests/distance/test_edge_edge_mollifier.cpp index 0ef04b48..3043350f 100644 --- a/tests/src/tests/distance/test_edge_edge_mollifier.cpp +++ b/tests/src/tests/distance/test_edge_edge_mollifier.cpp @@ -31,7 +31,7 @@ std::array get_edges() edge_edge_cross_squarednorm(ea0, ea1, eb0, eb1) == Catch::Approx(0).margin(1e-9)); } - return { ea0, ea1, eb0, eb1 }; + return { { ea0, ea1, eb0, eb1 } }; } TEST_CASE("Edge-Edge Cross Squarednorm", "[distance][edge-edge][mollifier]") From ed667d1df5d73287ec89abcae155e9c1d6d124cb Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Mon, 30 Oct 2023 10:20:29 -0400 Subject: [PATCH 3/5] Test shape derivative --- src/ipc/collisions/collision_constraint.cpp | 4 + src/ipc/collisions/collision_constraints.cpp | 35 +++++ src/ipc/collisions/collision_constraints.hpp | 25 ++++ .../src/tests/collisions/test_constraints.cpp | 135 ++++++++++++++++++ tests/src/tests/test_ipc.cpp | 2 +- 5 files changed, 200 insertions(+), 1 deletion(-) diff --git a/src/ipc/collisions/collision_constraint.cpp b/src/ipc/collisions/collision_constraint.cpp index 5eb1dfaa..f2ec2fdd 100644 --- a/src/ipc/collisions/collision_constraint.cpp +++ b/src/ipc/collisions/collision_constraint.cpp @@ -101,6 +101,10 @@ void CollisionConstraint::compute_shape_derivative_first_term( "Shape derivative is not computed for contact constraint!"); } + if (weight_gradient.nonZeros() == 0) { + return; + } + const int dim = vertices.cols(); VectorMax12d grad_b = diff --git a/src/ipc/collisions/collision_constraints.cpp b/src/ipc/collisions/collision_constraints.cpp index 216bd541..a4a2a8aa 100644 --- a/src/ipc/collisions/collision_constraints.cpp +++ b/src/ipc/collisions/collision_constraints.cpp @@ -581,6 +581,41 @@ const CollisionConstraint& CollisionConstraints::operator[](size_t idx) const throw std::out_of_range("Constraint index is out of range!"); } +bool CollisionConstraints::is_vertex_vertex(size_t idx) const +{ + return idx < vv_constraints.size(); +} + +bool CollisionConstraints::is_edge_vertex(size_t idx) const +{ + return idx >= vv_constraints.size() + && idx < vv_constraints.size() + ev_constraints.size(); +} + +bool CollisionConstraints::is_edge_edge(size_t idx) const +{ + return idx >= vv_constraints.size() + ev_constraints.size() + && idx + < vv_constraints.size() + ev_constraints.size() + ee_constraints.size(); +} + +bool CollisionConstraints::is_face_vertex(size_t idx) const +{ + return idx + >= vv_constraints.size() + ev_constraints.size() + ee_constraints.size() + && idx < vv_constraints.size() + ev_constraints.size() + + ee_constraints.size() + fv_constraints.size(); +} + +bool CollisionConstraints::is_plane_vertex(size_t idx) const +{ + return idx >= vv_constraints.size() + ev_constraints.size() + + ee_constraints.size() + fv_constraints.size() + && idx < vv_constraints.size() + ev_constraints.size() + + ee_constraints.size() + fv_constraints.size() + + pv_constraints.size(); +} + std::string CollisionConstraints::to_string( const CollisionMesh& mesh, const Eigen::MatrixXd& vertices) const { diff --git a/src/ipc/collisions/collision_constraints.hpp b/src/ipc/collisions/collision_constraints.hpp index a352c185..d5f27721 100644 --- a/src/ipc/collisions/collision_constraints.hpp +++ b/src/ipc/collisions/collision_constraints.hpp @@ -121,6 +121,31 @@ class CollisionConstraints { /// @return A const reference to the constraint. const CollisionConstraint& operator[](size_t idx) const; + /// @brief Get if the constraint at idx is a vertex-vertex constraint. + /// @param idx The index of the constraint. + /// @return If the constraint at idx is a vertex-vertex constraint. + bool is_vertex_vertex(size_t idx) const; + + /// @brief Get if the constraint at idx is an edge-vertex constraint. + /// @param idx The index of the constraint. + /// @return If the constraint at idx is an edge-vertex constraint. + bool is_edge_vertex(size_t idx) const; + + /// @brief Get if the constraint at idx is an edge-edge constraint. + /// @param idx The index of the constraint. + /// @return If the constraint at idx is an edge-edge constraint. + bool is_edge_edge(size_t idx) const; + + /// @brief Get if the constraint at idx is an face-vertex constraint. + /// @param idx The index of the constraint. + /// @return If the constraint at idx is an face-vertex constraint. + bool is_face_vertex(size_t idx) const; + + /// @brief Get if the constraint at idx is an plane-vertex constraint. + /// @param idx The index of the constraint. + /// @return If the constraint at idx is an plane-vertex constraint. + bool is_plane_vertex(size_t idx) const; + /// @brief Get if the collision constraints should use the convergent formulation. /// @note If not empty, this is the current value not necessarily the value used to build the constraints. /// @return If the collision constraints should use the convergent formulation. diff --git a/tests/src/tests/collisions/test_constraints.cpp b/tests/src/tests/collisions/test_constraints.cpp index 55d44616..8e1be526 100644 --- a/tests/src/tests/collisions/test_constraints.cpp +++ b/tests/src/tests/collisions/test_constraints.cpp @@ -4,9 +4,144 @@ #include #include +#include +#include + +#include using namespace ipc; +TEST_CASE("Constraint Shape Derivative", "[constraint][shape_derivative]") +{ + Eigen::MatrixXd V; + Eigen::MatrixXi E, F; + tests::load_mesh("cube.obj", V, E, F); + + const bool use_convergent_formulation = GENERATE(false); + const double dhat = 1e-1; + + // Stack cube on top of itself + E.conservativeResize(E.rows() * 2, E.cols()); + E.bottomRows(E.rows() / 2) = E.topRows(E.rows() / 2).array() + V.rows(); + + F.conservativeResize(F.rows() * 2, F.cols()); + F.bottomRows(F.rows() / 2) = F.topRows(F.rows() / 2).array() + V.rows(); + + V.conservativeResize(V.rows() * 2, V.cols()); + V.bottomRows(V.rows() / 2) = V.topRows(V.rows() / 2); + V.bottomRows(V.rows() / 2).col(1).array() += 1 + 0.1 * dhat; + + // Rest positions + Eigen::MatrixXd X = V; + X.bottomRows(V.rows() / 2).col(1).array() += 1.0; + + // Displacements + const Eigen::MatrixXd U = V - X; + + const int ndof = V.size(); + + // ------------------------------------------------------------------------ + + CollisionMesh mesh(X, E, F); + mesh.init_area_jacobians(); + + Candidates candidates; + candidates.build(mesh, V, dhat); + + CollisionConstraints constraints; + constraints.set_use_convergent_formulation(use_convergent_formulation); + constraints.set_are_shape_derivatives_enabled(true); + constraints.build(candidates, mesh, V, dhat); + + REQUIRE(constraints.ee_constraints.size() > 0); + + for (int i = 0; i < constraints.size(); i++) { + if (use_convergent_formulation) + break; + + std::vector> triplets; + constraints[i].compute_shape_derivative(X, V, E, F, dhat, triplets); + Eigen::SparseMatrix JF_wrt_X_sparse(ndof, ndof); + JF_wrt_X_sparse.setFromTriplets(triplets.begin(), triplets.end()); + const Eigen::MatrixXd JF_wrt_X = Eigen::MatrixXd(JF_wrt_X_sparse); + + auto F_X = [&](const Eigen::VectorXd& x) -> Eigen::VectorXd { + // TODO: Recompute weight based on x + assert(use_convergent_formulation == false); + // Recompute eps_x based on x + double prev_eps_x; + if (constraints.is_edge_edge(i)) { + EdgeEdgeConstraint& c = + dynamic_cast(constraints[i]); + prev_eps_x = c.eps_x; + c.eps_x = edge_edge_mollifier_threshold( + x.segment<3>(3 * E(c.edge0_id, 0)), + x.segment<3>(3 * E(c.edge0_id, 1)), + x.segment<3>(3 * E(c.edge1_id, 0)), + x.segment<3>(3 * E(c.edge1_id, 1))); + } + + Eigen::VectorXd grad = Eigen::VectorXd::Zero(ndof); + local_gradient_to_global_gradient( + constraints[i].compute_potential_gradient( + fd::unflatten(x, X.cols()) + U, E, F, dhat), + constraints[i].vertex_ids(E, F), V.cols(), grad); + + // Restore eps_x + if (constraints.is_edge_edge(i)) { + dynamic_cast(constraints[i]).eps_x = + prev_eps_x; + } + + return grad; + }; + + Eigen::MatrixXd fd_JF_wrt_X; + fd::finite_jacobian(fd::flatten(X), F_X, fd_JF_wrt_X); + CHECK(fd::compare_jacobian(JF_wrt_X, fd_JF_wrt_X)); + if (!fd::compare_jacobian(JF_wrt_X, fd_JF_wrt_X)) { + tests::print_compare_nonzero(JF_wrt_X, fd_JF_wrt_X); + } + } + + // ------------------------------------------------------------------------ + + const Eigen::MatrixXd JF_wrt_X = + constraints.compute_shape_derivative(mesh, V, dhat); + + Eigen::MatrixXd sum = Eigen::MatrixXd::Zero(ndof, ndof); + for (int i = 0; i < constraints.size(); i++) { + std::vector> triplets; + constraints[i].compute_shape_derivative(X, V, E, F, dhat, triplets); + Eigen::SparseMatrix JF_wrt_X_sparse(ndof, ndof); + JF_wrt_X_sparse.setFromTriplets(triplets.begin(), triplets.end()); + sum += Eigen::MatrixXd(JF_wrt_X_sparse); + } + CHECK(fd::compare_jacobian(JF_wrt_X, sum)); + + auto F_X = [&](const Eigen::VectorXd& x) { + const Eigen::MatrixXd fd_X = fd::unflatten(x, X.cols()); + const Eigen::MatrixXd fd_V = fd_X + U; + + CollisionMesh fd_mesh(fd_X, mesh.edges(), mesh.faces()); + + // WARNING: This breaks the tests because EE distances are C0 when edges + // are parallel + // CollisionConstraints fd_constraints; + // fd_constraints.set_use_convergent_formulation( + // constraints.use_convergent_formulation()); + // fd_constraints.build(fd_mesh, fd_V, dhat); + + return constraints.compute_potential_gradient(fd_mesh, fd_V, dhat); + }; + Eigen::MatrixXd fd_JF_wrt_X; + fd::finite_jacobian(fd::flatten(X), F_X, fd_JF_wrt_X); + CHECK(fd::compare_jacobian(JF_wrt_X, fd_JF_wrt_X)); + if (!fd::compare_jacobian(JF_wrt_X, fd_JF_wrt_X)) { + tests::print_compare_nonzero(JF_wrt_X, fd_JF_wrt_X); + } +} + TEST_CASE("Codim. Vertex-Vertex Constraints", "[constraints][codim]") { constexpr double thickness = 0.4; diff --git a/tests/src/tests/test_ipc.cpp b/tests/src/tests/test_ipc.cpp index 71dc58bf..47ca5cad 100644 --- a/tests/src/tests/test_ipc.cpp +++ b/tests/src/tests/test_ipc.cpp @@ -204,7 +204,7 @@ TEST_CASE("IPC full hessian", "[ipc][hessian]") CHECK(fd::compare_hessian(hess_b, fhess_b, 1e-3)); } -TEST_CASE("IPC shape derivative", "[ipc][shape_opt]") +TEST_CASE("IPC shape derivative", "[ipc][shape_derivative]") { nlohmann::json data; { From a2009c9e10f5d4dc219716814678e0aadd021145 Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Mon, 30 Oct 2023 10:25:27 -0400 Subject: [PATCH 4/5] Update bindings --- .../src/collisions/collision_constraints.cpp | 65 ++++++++++++++++++- 1 file changed, 64 insertions(+), 1 deletion(-) diff --git a/python/src/collisions/collision_constraints.cpp b/python/src/collisions/collision_constraints.cpp index e8850ffb..3602be7a 100644 --- a/python/src/collisions/collision_constraints.cpp +++ b/python/src/collisions/collision_constraints.cpp @@ -27,7 +27,7 @@ void define_collision_constraints(py::module_& m) )ipc_Qu8mg5v7", py::arg("mesh"), py::arg("vertices"), py::arg("dhat"), py::arg("dmin") = 0, - py::arg("broad_phase_method") = BroadPhaseMethod::HASH_GRID) + py::arg("broad_phase_method") = DEFAULT_BROAD_PHASE_METHOD) .def( "build", py::overload_cast< @@ -147,6 +147,69 @@ void define_collision_constraints(py::module_& m) A reference to the constraint. )ipc_Qu8mg5v7", py::arg("idx")) + .def( + "is_vertex_vertex", &CollisionConstraints::is_vertex_vertex, + R"ipc_Qu8mg5v7( + Get if the constraint at idx is a vertex-vertex constraint. + + Parameters: + idx: The index of the constraint. + + Returns: + If the constraint at idx is a vertex-vertex constraint. + )ipc_Qu8mg5v7", + py::arg("idx")) + .def( + "is_edge_vertex", &CollisionConstraints::is_edge_vertex, + R"ipc_Qu8mg5v7( + Get if the constraint at idx is an edge-vertex constraint. + + Parameters: + idx: The index of the constraint. + + Returns: + If the constraint at idx is an edge-vertex constraint. + )ipc_Qu8mg5v7", + py::arg("idx")) + .def( + "is_edge_edge", &CollisionConstraints::is_edge_edge, + R"ipc_Qu8mg5v7( + Get if the constraint at idx is an edge-edge constraint. + + Parameters: + idx: The index of the constraint. + + Returns: + If the constraint at idx is an edge-edge constraint. + )ipc_Qu8mg5v7", + py::arg("idx")) + .def( + "is_face_vertex", &CollisionConstraints::is_face_vertex, + R"ipc_Qu8mg5v7( + Get if the constraint at idx is an face-vertex constraint. + + Parameters: + idx: The index of the constraint. + + Returns: + If the constraint at idx is an face-vertex constraint. + )ipc_Qu8mg5v7", + py::arg("idx")) + .def( + "is_plane_vertex", &CollisionConstraints::is_plane_vertex, + R"ipc_Qu8mg5v7( + Get if the constraint at idx is an plane-vertex constraint. + + Parameters: + idx: The index of the constraint. + + Returns: + If the constraint at idx is an plane-vertex constraint. + )ipc_Qu8mg5v7", + py::arg("idx")) + .def( + "to_string", &CollisionConstraints::to_string, "", py::arg("mesh"), + py::arg("vertices")) .def_property( "use_convergent_formulation", &CollisionConstraints::use_convergent_formulation, From 739dc9769843c7207f927d2e9b8df849cd5907ba Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Mon, 30 Oct 2023 15:57:00 -0400 Subject: [PATCH 5/5] Test is_* --- .../src/tests/collisions/test_constraints.cpp | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) diff --git a/tests/src/tests/collisions/test_constraints.cpp b/tests/src/tests/collisions/test_constraints.cpp index 8e1be526..d39df18c 100644 --- a/tests/src/tests/collisions/test_constraints.cpp +++ b/tests/src/tests/collisions/test_constraints.cpp @@ -304,4 +304,23 @@ TEST_CASE("Plane-Vertex Constraint", "[constraint][plane-vertex]") CHECK( c.compute_distance_hessian(Eigen::RowVector3d(0, 2, 0), E, F) == 2 * n * n.transpose()); +} + +TEST_CASE("is_*", "[constraints]") +{ + CollisionConstraints constraints; + constraints.vv_constraints.emplace_back(0, 1); + constraints.ev_constraints.emplace_back(0, 1); + constraints.ee_constraints.emplace_back(0, 1, 0.0); + constraints.fv_constraints.emplace_back(0, 1); + constraints.pv_constraints.emplace_back( + Eigen::Vector3d(0, 0, 0), Eigen::Vector3d(0, 1, 0), 0); + + for (int i = 0; i < constraints.size(); i++) { + CHECK(constraints.is_vertex_vertex(i) == (i == 0)); + CHECK(constraints.is_edge_vertex(i) == (i == 1)); + CHECK(constraints.is_edge_edge(i) == (i == 2)); + CHECK(constraints.is_face_vertex(i) == (i == 3)); + CHECK(constraints.is_plane_vertex(i) == (i == 4)); + } } \ No newline at end of file