diff --git a/cmake/recipes/finite_diff.cmake b/cmake/recipes/finite_diff.cmake index bf2ee6ab6..34edda78b 100644 --- a/cmake/recipes/finite_diff.cmake +++ b/cmake/recipes/finite_diff.cmake @@ -7,7 +7,7 @@ endif() message(STATUS "Third-party: creating target 'finitediff::finitediff'") include(CPM) -CPMAddPackage("gh:zfergus/finite-diff@1.0.3") +CPMAddPackage("gh:zfergus/finite-diff@1.0.4") # Folder name for IDE set_target_properties(finitediff_finitediff PROPERTIES FOLDER "ThirdParty") \ No newline at end of file diff --git a/docs/source/cpp-api/tangent.rst b/docs/source/cpp-api/tangent.rst index 843d3df11..e3a842a37 100644 --- a/docs/source/cpp-api/tangent.rst +++ b/docs/source/cpp-api/tangent.rst @@ -28,18 +28,18 @@ Relative Velocity Relative Velocity as Multiplier Matricies ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -.. doxygenfunction:: ipc::point_point_relative_velocity_matrix -.. doxygenfunction:: ipc::point_edge_relative_velocity_matrix -.. doxygenfunction:: ipc::edge_edge_relative_velocity_matrix -.. doxygenfunction:: ipc::point_triangle_relative_velocity_matrix +.. doxygenfunction:: ipc::point_point_relative_velocity_jacobian +.. doxygenfunction:: ipc::point_edge_relative_velocity_jacobian +.. doxygenfunction:: ipc::edge_edge_relative_velocity_jacobian +.. doxygenfunction:: ipc::point_triangle_relative_velocity_jacobian Relative Velocity Matrix Jacobians ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -.. doxygenfunction:: ipc::point_point_relative_velocity_matrix_jacobian -.. doxygenfunction:: ipc::point_edge_relative_velocity_matrix_jacobian -.. doxygenfunction:: ipc::edge_edge_relative_velocity_matrix_jacobian -.. doxygenfunction:: ipc::point_triangle_relative_velocity_matrix_jacobian +.. doxygenfunction:: ipc::point_point_relative_velocity_dx_dbeta +.. doxygenfunction:: ipc::point_edge_relative_velocity_dx_dbeta +.. doxygenfunction:: ipc::edge_edge_relative_velocity_dx_dbeta +.. doxygenfunction:: ipc::point_triangle_relative_velocity_dx_dbeta Closet Points ------------- diff --git a/docs/source/python-api/tangent.rst b/docs/source/python-api/tangent.rst index 569107703..01832ee7c 100644 --- a/docs/source/python-api/tangent.rst +++ b/docs/source/python-api/tangent.rst @@ -28,18 +28,18 @@ Relative Velocity Relative Velocity as Multiplier Matricies ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -.. autofunction:: ipctk.point_point_relative_velocity_matrix -.. autofunction:: ipctk.point_edge_relative_velocity_matrix -.. autofunction:: ipctk.edge_edge_relative_velocity_matrix -.. autofunction:: ipctk.point_triangle_relative_velocity_matrix +.. autofunction:: ipctk.point_point_relative_velocity_jacobian +.. autofunction:: ipctk.point_edge_relative_velocity_jacobian +.. autofunction:: ipctk.edge_edge_relative_velocity_jacobian +.. autofunction:: ipctk.point_triangle_relative_velocity_jacobian Relative Velocity Matrix Jacobians ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -.. autofunction:: ipctk.point_point_relative_velocity_matrix_jacobian -.. autofunction:: ipctk.point_edge_relative_velocity_matrix_jacobian -.. autofunction:: ipctk.edge_edge_relative_velocity_matrix_jacobian -.. autofunction:: ipctk.point_triangle_relative_velocity_matrix_jacobian +.. autofunction:: ipctk.point_point_relative_velocity_dx_dbeta +.. autofunction:: ipctk.point_edge_relative_velocity_dx_dbeta +.. autofunction:: ipctk.edge_edge_relative_velocity_dx_dbeta +.. autofunction:: ipctk.point_triangle_relative_velocity_dx_dbeta Closet Points ------------- diff --git a/notebooks/generate_cpp_code.py b/notebooks/generate_cpp_code.py index b668f5999..1cbd05d0e 100644 --- a/notebooks/generate_cpp_code.py +++ b/notebooks/generate_cpp_code.py @@ -1,23 +1,36 @@ import numpy as np import sympy from sympy import Matrix, MatrixSymbol +from sympy.core.relational import Relational +from sympy.logic.boolalg import Boolean from sympy.printing import ccode import subprocess +import re from utils import jacobian def generate_code(expr, out_var_name=None): - CSE_results = sympy.cse( - expr, sympy.numbered_symbols("t"), optimizations='basic') + CSE_results = sympy.cse(expr, sympy.numbered_symbols("t"), optimizations="basic") lines = [] for helper in CSE_results[0]: + var_name = sympy.cxxcode(helper[0]) + value = helper[1] + + # 1. Determine the Correct C++ Type if isinstance(helper[1], MatrixSymbol): - lines.append(f'const auto {helper[0]}[{helper[1].size}];') - lines.append(sympy.cxxcode(helper[1], helper[0])) + cpp_type = "const double" + lines.append(f"{cpp_type} {var_name}[{value.size}];") + lines.append(sympy.cxxcode(value, var_name)) + continue # Skip the standard assignment line below + + if isinstance(value, (Relational, Boolean)) or value.is_Boolean: + cpp_type = "const bool" else: - lines.append( - f'const auto {sympy.cxxcode(helper[0])} = {sympy.cxxcode(helper[1])};') + cpp_type = "const double" + + # 2. Append the formatted assignment + lines.append(f"{cpp_type} {var_name} = {sympy.cxxcode(value)};") if out_var_name != None: for i, result in enumerate(CSE_results[1]): @@ -26,7 +39,18 @@ def generate_code(expr, out_var_name=None): for i, result in enumerate(CSE_results[1]): lines.append(f"return {sympy.cxxcode(result)};") - return '\n'.join(lines) + code = "\n".join(lines) + + # Regex replacements for better C++ code + code = re.sub(r"std::pow\(\s*([^,]+),\s*2\s*\)", r"((\1) * (\1))", code) + code = re.sub( + r"std::pow\(\s*([^,]+),\s*3\.0\s*/\s*2\.0\s*\)", r"((\1) * std::sqrt(\1))", code + ) + code = re.sub( + r"std::pow\(\s*([^,]+),\s*-1\.0\s*/\s*2\.0\s*\)", r"(1.0 / std::sqrt(\1))", code + ) + + return code class CXXFunctionGenerator: @@ -79,26 +103,30 @@ def __init__(self, expr, params, name, out_param_name="hess"): def generate_hpp_file(code_generators, file_name, transformer=None): newline = "\n" - with open(file_name, 'w') as f: - f.write(f"""\ + with open(file_name, "w") as f: + f.write( + f"""\ #pragma once namespace ipc::autogen{{ {newline.join(code_generator.signature() for code_generator in code_generators)} }} -""") +""" + ) subprocess.run(["clang-format", str(file_name), "-i"]) def generate_cpp_file(code_generators, file_name): newline = "\n" - with open(file_name, 'w') as f: - f.write(f"""\ + with open(file_name, "w") as f: + f.write( + f"""\ #include <{file_name[:-4]}.hpp> namespace ipc::autogen{{ {newline.join(code_generator() for code_generator in code_generators)} }} -""") +""" + ) subprocess.run(["clang-format", str(file_name), "-i"]) diff --git a/notebooks/tangent_basis_grad.ipynb b/notebooks/tangent_basis_grad.ipynb index 3290a6580..b68e47d1f 100644 --- a/notebooks/tangent_basis_grad.ipynb +++ b/notebooks/tangent_basis_grad.ipynb @@ -112,14 +112,17 @@ "cross_x = numpy.cross(numpy.array([1, 0, 0]), p0_to_p1)\n", "cross_y = numpy.cross(numpy.array([0, 1, 0]), p0_to_p1)\n", "\n", - "cross = numpy.array([\n", - " sympy.Piecewise(\n", - " (cross_x[i], sq_norm(cross_x) > sq_norm(cross_y)), \n", - " (cross_y[i], True))\n", - " for i in range(3)])\n", + "cross = numpy.array(\n", + " [\n", + " sympy.Piecewise(\n", + " (cross_x[i], sq_norm(cross_x) > sq_norm(cross_y)), (cross_y[i], True)\n", + " )\n", + " for i in range(3)\n", + " ]\n", + ")\n", "\n", - "T[:,0] = normalize(cross)\n", - "T[:,1] = normalize(numpy.cross(p0_to_p1, cross))\n", + "T[:, 0] = normalize(cross)\n", + "T[:, 1] = normalize(numpy.cross(p0_to_p1, cross))\n", "\n", "sympy.Matrix(T).diff(x).shape" ] @@ -250,8 +253,8 @@ "ea = ea1 - ea0\n", "T[:, 0] = normalize(ea)\n", "\n", - "normal = numpy.cross(ea, eb1 - eb0);\n", - "T[:, 1] = normalize(numpy.cross(normal, ea));\n", + "normal = numpy.cross(ea, eb1 - eb0)\n", + "T[:, 1] = normalize(numpy.cross(normal, ea))\n", "\n", "generators.append(CXXJacobianGenerator(T, x, \"edge_edge_tangent_basis_jacobian\"))" ] @@ -328,7 +331,7 @@ "name": "python", "nbconvert_exporter": "python", "pygments_lexer": "ipython3", - "version": "3.11.1" + "version": "3.13.9" }, "varInspector": { "cols": { diff --git a/notebooks/utils.py b/notebooks/utils.py index 42cc29077..8243197cf 100644 --- a/notebooks/utils.py +++ b/notebooks/utils.py @@ -18,9 +18,9 @@ def normalize(x): def jacobian(F, x): - J = np.empty((x.size * F.shape[0], F.shape[1]), dtype=object) + """Compute the Jacobian of a matrix-valued function F with respect to vector x.""" + J = np.empty((F.size, x.size), dtype=object) for xi in range(x.size): - for Fi in range(F.shape[0]): - for Fj in range(F.shape[1]): - J[xi * F.shape[0] + Fi, Fj] = F[Fi, Fj].diff(x[xi]) + # Flatten column-major order + J[:, xi] = np.array(sympy.Matrix(F).diff(x[xi])).reshape(F.size, order="F") return J diff --git a/python/src/collisions/tangential/tangential_collision.cpp b/python/src/collisions/tangential/tangential_collision.cpp index 0292088ab..cdb7753fe 100644 --- a/python/src/collisions/tangential/tangential_collision.cpp +++ b/python/src/collisions/tangential/tangential_collision.cpp @@ -78,9 +78,9 @@ void define_tangential_collision(py::module_& m) )ipc_Qu8mg5v7", "velocities"_a) .def( - "relative_velocity_matrix", + "relative_velocity_jacobian", py::overload_cast<>( - &TangentialCollision::relative_velocity_matrix, py::const_), + &TangentialCollision::relative_velocity_jacobian, py::const_), R"ipc_Qu8mg5v7( Construct the premultiplier matrix for the relative velocity. @@ -91,9 +91,9 @@ void define_tangential_collision(py::module_& m) A matrix M such that `relative_velocity = M * velocities`. )ipc_Qu8mg5v7") .def( - "relative_velocity_matrix", + "relative_velocity_jacobian", py::overload_cast>( - &TangentialCollision::relative_velocity_matrix, py::const_), + &TangentialCollision::relative_velocity_jacobian, py::const_), R"ipc_Qu8mg5v7( Construct the premultiplier matrix for the relative velocity. @@ -105,8 +105,8 @@ void define_tangential_collision(py::module_& m) )ipc_Qu8mg5v7", "closest_point"_a) .def( - "relative_velocity_matrix_jacobian", - &TangentialCollision::relative_velocity_matrix_jacobian, + "relative_velocity_dx_dbeta", + &TangentialCollision::relative_velocity_dx_dbeta, R"ipc_Qu8mg5v7( Construct the Jacobian of the relative velocity premultiplier wrt the closest points. diff --git a/python/src/tangent/relative_velocity.cpp b/python/src/tangent/relative_velocity.cpp index 591b23a18..6574ec52c 100644 --- a/python/src/tangent/relative_velocity.cpp +++ b/python/src/tangent/relative_velocity.cpp @@ -21,8 +21,8 @@ void define_relative_velocity(py::module_& m) "dp0"_a, "dp1"_a); m.def( - "point_point_relative_velocity_matrix", - &point_point_relative_velocity_matrix, + "point_point_relative_velocity_jacobian", + &point_point_relative_velocity_jacobian, R"ipc_Qu8mg5v7( Compute the point-point relative velocity premultiplier matrix @@ -35,8 +35,8 @@ void define_relative_velocity(py::module_& m) "dim"_a); m.def( - "point_point_relative_velocity_matrix_jacobian", - &point_point_relative_velocity_matrix_jacobian, + "point_point_relative_velocity_dx_dbeta", + &point_point_relative_velocity_dx_dbeta, R"ipc_Qu8mg5v7( Compute the Jacobian of the relative velocity premultiplier matrix @@ -65,8 +65,8 @@ void define_relative_velocity(py::module_& m) "dp"_a, "de0"_a, "de1"_a, "alpha"_a); m.def( - "point_edge_relative_velocity_matrix", - &point_edge_relative_velocity_matrix, + "point_edge_relative_velocity_jacobian", + &point_edge_relative_velocity_jacobian, R"ipc_Qu8mg5v7( Compute the point-edge relative velocity premultiplier matrix @@ -80,8 +80,8 @@ void define_relative_velocity(py::module_& m) "dim"_a, "alpha"_a); m.def( - "point_edge_relative_velocity_matrix_jacobian", - &point_edge_relative_velocity_matrix_jacobian, + "point_edge_relative_velocity_dx_dbeta", + &point_edge_relative_velocity_dx_dbeta, R"ipc_Qu8mg5v7( Compute the Jacobian of the relative velocity premultiplier matrix @@ -112,34 +112,32 @@ void define_relative_velocity(py::module_& m) "dea0"_a, "dea1"_a, "deb0"_a, "deb1"_a, "coords"_a); m.def( - "edge_edge_relative_velocity_matrix", - &edge_edge_relative_velocity_matrix, + "edge_edge_relative_velocity_jacobian", + &edge_edge_relative_velocity_jacobian, R"ipc_Qu8mg5v7( Compute the edge-edge relative velocity matrix. Parameters: - dim: Dimension (2 or 3) coords: Two parametric coordinates of the closest points on the edges Returns: The relative velocity matrix )ipc_Qu8mg5v7", - "dim"_a, "coords"_a); + "coords"_a); m.def( - "edge_edge_relative_velocity_matrix_jacobian", - &edge_edge_relative_velocity_matrix_jacobian, + "edge_edge_relative_velocity_dx_dbeta", + &edge_edge_relative_velocity_dx_dbeta, R"ipc_Qu8mg5v7( Compute the Jacobian of the edge-edge relative velocity matrix. Parameters: - dim: Dimension (2 or 3) coords: Two parametric coordinates of the closest points on the edges Returns: The Jacobian of the relative velocity matrix )ipc_Qu8mg5v7", - "dim"_a, "coords"_a); + "coords"_a); m.def( "point_triangle_relative_velocity", &point_triangle_relative_velocity, @@ -159,32 +157,30 @@ void define_relative_velocity(py::module_& m) "dp"_a, "dt0"_a, "dt1"_a, "dt2"_a, "coords"_a); m.def( - "point_triangle_relative_velocity_matrix", - &point_triangle_relative_velocity_matrix, + "point_triangle_relative_velocity_jacobian", + &point_triangle_relative_velocity_jacobian, R"ipc_Qu8mg5v7( Compute the point-triangle relative velocity matrix. Parameters: - dim: Dimension (2 or 3) coords: Barycentric coordinates of the closest point on the triangle Returns: The relative velocity matrix )ipc_Qu8mg5v7", - "dim"_a, "coords"_a); + "coords"_a); m.def( - "point_triangle_relative_velocity_matrix_jacobian", - &point_triangle_relative_velocity_matrix_jacobian, + "point_triangle_relative_velocity_dx_dbeta", + &point_triangle_relative_velocity_dx_dbeta, R"ipc_Qu8mg5v7( Compute the Jacobian of the point-triangle relative velocity matrix. Parameters: - dim: Dimension (2 or 3) coords: Barycentric coordinates of the closest point on the triangle Returns: The Jacobian of the relative velocity matrix )ipc_Qu8mg5v7", - "dim"_a, "coords"_a); + "coords"_a); } diff --git a/src/ipc/collisions/tangential/edge_edge.cpp b/src/ipc/collisions/tangential/edge_edge.cpp index 6c49bfc81..64b609360 100644 --- a/src/ipc/collisions/tangential/edge_edge.cpp +++ b/src/ipc/collisions/tangential/edge_edge.cpp @@ -45,7 +45,7 @@ MatrixMax EdgeEdgeTangentialCollision::compute_tangent_basis( positions.segment(2 * dim(), dim()), positions.tail(dim())); } -MatrixMax +MatrixMax EdgeEdgeTangentialCollision::compute_tangent_basis_jacobian( Eigen::ConstRef positions) const { @@ -87,19 +87,20 @@ VectorMax3d EdgeEdgeTangentialCollision::relative_velocity( velocities.segment<3>(2 * dim()), velocities.tail<3>(), closest_point); } -MatrixMax EdgeEdgeTangentialCollision::relative_velocity_matrix( +MatrixMax +EdgeEdgeTangentialCollision::relative_velocity_jacobian( Eigen::ConstRef _closest_point) const { assert(_closest_point.size() == 2); - return edge_edge_relative_velocity_matrix(dim(), _closest_point); + return edge_edge_relative_velocity_jacobian(_closest_point); } -MatrixMax -EdgeEdgeTangentialCollision::relative_velocity_matrix_jacobian( +MatrixMax +EdgeEdgeTangentialCollision::relative_velocity_dx_dbeta( Eigen::ConstRef _closest_point) const { assert(_closest_point.size() == 2); - return edge_edge_relative_velocity_matrix_jacobian(dim(), _closest_point); + return edge_edge_relative_velocity_dx_dbeta(_closest_point); } } // namespace ipc diff --git a/src/ipc/collisions/tangential/edge_edge.hpp b/src/ipc/collisions/tangential/edge_edge.hpp index 88fbd8ba8..bdee6e2bb 100644 --- a/src/ipc/collisions/tangential/edge_edge.hpp +++ b/src/ipc/collisions/tangential/edge_edge.hpp @@ -33,7 +33,7 @@ class EdgeEdgeTangentialCollision : public EdgeEdgeCandidate, MatrixMax compute_tangent_basis( Eigen::ConstRef positions) const override; - MatrixMax compute_tangent_basis_jacobian( + MatrixMax compute_tangent_basis_jacobian( Eigen::ConstRef positions) const override; VectorMax2d compute_closest_point( @@ -45,12 +45,12 @@ class EdgeEdgeTangentialCollision : public EdgeEdgeCandidate, VectorMax3d relative_velocity(Eigen::ConstRef velocities) const override; - using TangentialCollision::relative_velocity_matrix; + using TangentialCollision::relative_velocity_jacobian; - MatrixMax relative_velocity_matrix( + MatrixMax relative_velocity_jacobian( Eigen::ConstRef closest_point) const override; - MatrixMax relative_velocity_matrix_jacobian( + MatrixMax relative_velocity_dx_dbeta( Eigen::ConstRef closest_point) const override; }; diff --git a/src/ipc/collisions/tangential/edge_vertex.cpp b/src/ipc/collisions/tangential/edge_vertex.cpp index 602fa81dc..97ad3129a 100644 --- a/src/ipc/collisions/tangential/edge_vertex.cpp +++ b/src/ipc/collisions/tangential/edge_vertex.cpp @@ -44,7 +44,7 @@ MatrixMax EdgeVertexTangentialCollision::compute_tangent_basis( positions.tail(dim())); } -MatrixMax +MatrixMax EdgeVertexTangentialCollision::compute_tangent_basis_jacobian( Eigen::ConstRef positions) const { @@ -90,20 +90,19 @@ VectorMax3d EdgeVertexTangentialCollision::relative_velocity( } MatrixMax -EdgeVertexTangentialCollision::relative_velocity_matrix( +EdgeVertexTangentialCollision::relative_velocity_jacobian( Eigen::ConstRef _closest_point) const { assert(_closest_point.size() == 1); - return point_edge_relative_velocity_matrix(dim(), _closest_point[0]); + return point_edge_relative_velocity_jacobian(dim(), _closest_point[0]); } -MatrixMax -EdgeVertexTangentialCollision::relative_velocity_matrix_jacobian( +MatrixMax +EdgeVertexTangentialCollision::relative_velocity_dx_dbeta( Eigen::ConstRef _closest_point) const { assert(_closest_point.size() == 1); - return point_edge_relative_velocity_matrix_jacobian( - dim(), _closest_point[0]); + return point_edge_relative_velocity_dx_dbeta(dim(), _closest_point[0]); } } // namespace ipc diff --git a/src/ipc/collisions/tangential/edge_vertex.hpp b/src/ipc/collisions/tangential/edge_vertex.hpp index 2f8e0d9f2..48d280339 100644 --- a/src/ipc/collisions/tangential/edge_vertex.hpp +++ b/src/ipc/collisions/tangential/edge_vertex.hpp @@ -27,7 +27,7 @@ class EdgeVertexTangentialCollision : public EdgeVertexCandidate, MatrixMax compute_tangent_basis( Eigen::ConstRef positions) const override; - MatrixMax compute_tangent_basis_jacobian( + MatrixMax compute_tangent_basis_jacobian( Eigen::ConstRef positions) const override; VectorMax2d compute_closest_point( @@ -39,12 +39,12 @@ class EdgeVertexTangentialCollision : public EdgeVertexCandidate, VectorMax3d relative_velocity(Eigen::ConstRef velocities) const override; - using TangentialCollision::relative_velocity_matrix; + using TangentialCollision::relative_velocity_jacobian; - MatrixMax relative_velocity_matrix( + MatrixMax relative_velocity_jacobian( Eigen::ConstRef closest_point) const override; - MatrixMax relative_velocity_matrix_jacobian( + MatrixMax relative_velocity_dx_dbeta( Eigen::ConstRef closest_point) const override; }; diff --git a/src/ipc/collisions/tangential/face_vertex.cpp b/src/ipc/collisions/tangential/face_vertex.cpp index 82c935cdd..4fbd45d41 100644 --- a/src/ipc/collisions/tangential/face_vertex.cpp +++ b/src/ipc/collisions/tangential/face_vertex.cpp @@ -44,7 +44,7 @@ MatrixMax FaceVertexTangentialCollision::compute_tangent_basis( positions.segment(2 * dim(), dim()), positions.tail(dim())); } -MatrixMax +MatrixMax FaceVertexTangentialCollision::compute_tangent_basis_jacobian( Eigen::ConstRef positions) const { @@ -87,20 +87,19 @@ VectorMax3d FaceVertexTangentialCollision::relative_velocity( } MatrixMax -FaceVertexTangentialCollision::relative_velocity_matrix( +FaceVertexTangentialCollision::relative_velocity_jacobian( Eigen::ConstRef _closest_point) const { assert(_closest_point.size() == 2); - return point_triangle_relative_velocity_matrix(dim(), _closest_point); + return point_triangle_relative_velocity_jacobian(_closest_point); } -MatrixMax -FaceVertexTangentialCollision::relative_velocity_matrix_jacobian( +MatrixMax +FaceVertexTangentialCollision::relative_velocity_dx_dbeta( Eigen::ConstRef _closest_point) const { assert(_closest_point.size() == 2); - return point_triangle_relative_velocity_matrix_jacobian( - dim(), _closest_point); + return point_triangle_relative_velocity_dx_dbeta(_closest_point); } } // namespace ipc diff --git a/src/ipc/collisions/tangential/face_vertex.hpp b/src/ipc/collisions/tangential/face_vertex.hpp index 34f11e83a..3f9e0b48a 100644 --- a/src/ipc/collisions/tangential/face_vertex.hpp +++ b/src/ipc/collisions/tangential/face_vertex.hpp @@ -27,7 +27,7 @@ class FaceVertexTangentialCollision : public FaceVertexCandidate, MatrixMax compute_tangent_basis( Eigen::ConstRef positions) const override; - MatrixMax compute_tangent_basis_jacobian( + MatrixMax compute_tangent_basis_jacobian( Eigen::ConstRef positions) const override; VectorMax2d compute_closest_point( @@ -39,12 +39,12 @@ class FaceVertexTangentialCollision : public FaceVertexCandidate, VectorMax3d relative_velocity(Eigen::ConstRef velocities) const override; - using TangentialCollision::relative_velocity_matrix; + using TangentialCollision::relative_velocity_jacobian; - MatrixMax relative_velocity_matrix( + MatrixMax relative_velocity_jacobian( Eigen::ConstRef closest_point) const override; - MatrixMax relative_velocity_matrix_jacobian( + MatrixMax relative_velocity_dx_dbeta( Eigen::ConstRef closest_point) const override; }; diff --git a/src/ipc/collisions/tangential/plane_vertex.cpp b/src/ipc/collisions/tangential/plane_vertex.cpp index 09f08ba91..a7a0bc49a 100644 --- a/src/ipc/collisions/tangential/plane_vertex.cpp +++ b/src/ipc/collisions/tangential/plane_vertex.cpp @@ -45,13 +45,13 @@ MatrixMax PlaneVertexTangentialCollision::compute_tangent_basis( p0.head(positions.size()), p1.head(positions.size())); } -MatrixMax +MatrixMax PlaneVertexTangentialCollision::compute_tangent_basis_jacobian( Eigen::ConstRef positions) const { assert(positions.size() == ndof()); - return MatrixMax::Zero( - ndof() * ndof(), positions.size() == 2 ? 1 : 2); + return MatrixMax::Zero( + positions.size() == 2 ? 2 : 6, ndof()); } // ============================================================================ @@ -79,18 +79,18 @@ VectorMax3d PlaneVertexTangentialCollision::relative_velocity( } MatrixMax -PlaneVertexTangentialCollision::relative_velocity_matrix( +PlaneVertexTangentialCollision::relative_velocity_jacobian( Eigen::ConstRef _closest_point) const { return MatrixMax::Identity(ndof(), ndof()); } -MatrixMax -PlaneVertexTangentialCollision::relative_velocity_matrix_jacobian( +MatrixMax +PlaneVertexTangentialCollision::relative_velocity_dx_dbeta( Eigen::ConstRef _closest_point) const { - return MatrixMax::Zero( - _closest_point.size() * ndof(), ndof()); + return MatrixMax::Zero( + dim() * ndof(), _closest_point.size()); } } // namespace ipc \ No newline at end of file diff --git a/src/ipc/collisions/tangential/plane_vertex.hpp b/src/ipc/collisions/tangential/plane_vertex.hpp index 20f2a6ef3..c87c76fb1 100644 --- a/src/ipc/collisions/tangential/plane_vertex.hpp +++ b/src/ipc/collisions/tangential/plane_vertex.hpp @@ -28,7 +28,7 @@ class PlaneVertexTangentialCollision : public PlaneVertexCandidate, MatrixMax compute_tangent_basis( Eigen::ConstRef positions) const override; - MatrixMax compute_tangent_basis_jacobian( + MatrixMax compute_tangent_basis_jacobian( Eigen::ConstRef positions) const override; VectorMax2d compute_closest_point( @@ -40,12 +40,12 @@ class PlaneVertexTangentialCollision : public PlaneVertexCandidate, VectorMax3d relative_velocity(Eigen::ConstRef velocities) const override; - using TangentialCollision::relative_velocity_matrix; + using TangentialCollision::relative_velocity_jacobian; - MatrixMax relative_velocity_matrix( + MatrixMax relative_velocity_jacobian( Eigen::ConstRef closest_point) const override; - MatrixMax relative_velocity_matrix_jacobian( + MatrixMax relative_velocity_dx_dbeta( Eigen::ConstRef closest_point) const override; }; diff --git a/src/ipc/collisions/tangential/tangential_collision.hpp b/src/ipc/collisions/tangential/tangential_collision.hpp index 8b34edecb..0d3ee3ddb 100644 --- a/src/ipc/collisions/tangential/tangential_collision.hpp +++ b/src/ipc/collisions/tangential/tangential_collision.hpp @@ -48,7 +48,7 @@ class TangentialCollision : virtual public CollisionStencil { /// @brief Compute the Jacobian of the tangent basis of the collision. /// @param positions Collision stencil's vertex positions. /// @return Jacobian of the tangent basis of the collision. - virtual MatrixMax compute_tangent_basis_jacobian( + virtual MatrixMax compute_tangent_basis_jacobian( Eigen::ConstRef positions) const = 0; /// @brief Compute the barycentric coordinates of the closest point. @@ -72,21 +72,21 @@ class TangentialCollision : virtual public CollisionStencil { /// @brief Construct the premultiplier matrix for the relative velocity. /// @note Uses the cached closest point. /// @return A matrix M such that `relative_velocity = M * velocities`. - virtual MatrixMax relative_velocity_matrix() const + virtual MatrixMax relative_velocity_jacobian() const { - return relative_velocity_matrix(closest_point); + return relative_velocity_jacobian(closest_point); } /// @brief Construct the premultiplier matrix for the relative velocity. /// @param closest_point Barycentric coordinates of the closest point. /// @return A matrix M such that `relative_velocity = M * velocities`. - virtual MatrixMax relative_velocity_matrix( + virtual MatrixMax relative_velocity_jacobian( Eigen::ConstRef closest_point) const = 0; /// @brief Construct the Jacobian of the relative velocity premultiplier wrt the closest points. /// @param closest_point Barycentric coordinates of the closest point. /// @return Jacobian of the relative velocity premultiplier wrt the closest points. - virtual MatrixMax relative_velocity_matrix_jacobian( + virtual MatrixMax relative_velocity_dx_dbeta( Eigen::ConstRef closest_point) const = 0; public: diff --git a/src/ipc/collisions/tangential/vertex_vertex.cpp b/src/ipc/collisions/tangential/vertex_vertex.cpp index 975a05da8..1c9e9fd2e 100644 --- a/src/ipc/collisions/tangential/vertex_vertex.cpp +++ b/src/ipc/collisions/tangential/vertex_vertex.cpp @@ -43,7 +43,7 @@ MatrixMax VertexVertexTangentialCollision::compute_tangent_basis( positions.head(dim()), positions.tail(dim())); } -MatrixMax +MatrixMax VertexVertexTangentialCollision::compute_tangent_basis_jacobian( Eigen::ConstRef positions) const { @@ -78,19 +78,19 @@ VectorMax3d VertexVertexTangentialCollision::relative_velocity( } MatrixMax -VertexVertexTangentialCollision::relative_velocity_matrix( +VertexVertexTangentialCollision::relative_velocity_jacobian( Eigen::ConstRef _closest_point) const { assert(_closest_point.size() == 0); - return point_point_relative_velocity_matrix(dim()); + return point_point_relative_velocity_jacobian(dim()); } -MatrixMax -VertexVertexTangentialCollision::relative_velocity_matrix_jacobian( +MatrixMax +VertexVertexTangentialCollision::relative_velocity_dx_dbeta( Eigen::ConstRef _closest_point) const { assert(_closest_point.size() == 0); - return point_point_relative_velocity_matrix_jacobian(dim()); + return point_point_relative_velocity_dx_dbeta(dim()); } } // namespace ipc diff --git a/src/ipc/collisions/tangential/vertex_vertex.hpp b/src/ipc/collisions/tangential/vertex_vertex.hpp index 92f7133bf..3aebec36f 100644 --- a/src/ipc/collisions/tangential/vertex_vertex.hpp +++ b/src/ipc/collisions/tangential/vertex_vertex.hpp @@ -28,7 +28,7 @@ class VertexVertexTangentialCollision : public VertexVertexCandidate, MatrixMax compute_tangent_basis( Eigen::ConstRef positions) const override; - MatrixMax compute_tangent_basis_jacobian( + MatrixMax compute_tangent_basis_jacobian( Eigen::ConstRef positions) const override; VectorMax2d compute_closest_point( @@ -40,12 +40,12 @@ class VertexVertexTangentialCollision : public VertexVertexCandidate, VectorMax3d relative_velocity(Eigen::ConstRef velocities) const override; - using TangentialCollision::relative_velocity_matrix; + using TangentialCollision::relative_velocity_jacobian; - MatrixMax relative_velocity_matrix( + MatrixMax relative_velocity_jacobian( Eigen::ConstRef closest_point) const override; - MatrixMax relative_velocity_matrix_jacobian( + MatrixMax relative_velocity_dx_dbeta( Eigen::ConstRef closest_point) const override; }; diff --git a/src/ipc/distance/signed/line_line.cpp b/src/ipc/distance/signed/line_line.cpp index bff298a4a..0ec94fd71 100644 --- a/src/ipc/distance/signed/line_line.cpp +++ b/src/ipc/distance/signed/line_line.cpp @@ -28,7 +28,7 @@ Matrix12d line_line_signed_distance_hessian( // Precompute normal's Jacobian and Hessian const Eigen::Matrix jac_n = line_line_normal_jacobian(ea0, ea1, eb0, eb1); - const Eigen::Matrix hess_n = + const Eigen::Matrix hess_n = line_line_normal_hessian(ea0, ea1, eb0, eb1); // Vector from eb0 to ea0 (Distance vector) @@ -39,10 +39,10 @@ Matrix12d line_line_signed_distance_hessian( // --------------------------------------------------------- // 1. Tensor Contraction (Curvature Term) // --------------------------------------------------------- - // Contract the normal Hessian (3x144) with vector v (3x1). + // Contract the normal Hessian (3x12x12) with vector v (3x1). // This computes (v ⋅ d²n/dx²). - // The result is a 1x144 vector, which maps to the 12x12 Hessian matrix. - hess = (v.transpose() * hess_n).reshaped(12, 12); + // The result is a 1x12x12 vector, which maps to the 12x12 Hessian matrix. + hess = (hess_n.reshaped(3, 144).transpose() * v).reshaped(12, 12); // --------------------------------------------------------- // 2. Add Jacobian Terms (Product Rule Corrections) diff --git a/src/ipc/distance/signed/point_line.cpp b/src/ipc/distance/signed/point_line.cpp index 1863bdaa2..354d8d8cc 100644 --- a/src/ipc/distance/signed/point_line.cpp +++ b/src/ipc/distance/signed/point_line.cpp @@ -26,7 +26,7 @@ Matrix6d point_line_signed_distance_hessian( // Precompute normal's Jacobian and Hessian const Eigen::Matrix jac_n = point_line_normal_jacobian(p, e0, e1); - const Eigen::Matrix hess_n = + const Eigen::Matrix hess_n = point_line_normal_hessian(p, e0, e1); // Vector from e0 to p @@ -39,7 +39,7 @@ Matrix6d point_line_signed_distance_hessian( // --------------------------------------------------------- // Contract the normal Hessian (2x36) with vector v (2x1). // Result is 1x36, mapped to 6x6. - hess = (v.transpose() * hess_n).reshaped(6, 6); + hess = (hess_n.reshaped(2, 36).transpose() * v).reshaped(6, 6); // --------------------------------------------------------- // 2. Add Jacobian Terms (Product Rule Corrections) diff --git a/src/ipc/distance/signed/point_plane.cpp b/src/ipc/distance/signed/point_plane.cpp index 4b9c19f4d..6692143b3 100644 --- a/src/ipc/distance/signed/point_plane.cpp +++ b/src/ipc/distance/signed/point_plane.cpp @@ -30,7 +30,7 @@ Matrix12d point_plane_signed_distance_hessian( // Precompute normal's Jacobian and Hessian const Eigen::Matrix jac_n = triangle_normal_jacobian(t0, t1, t2); - const Eigen::Matrix hess_n = + const Eigen::Matrix hess_n = triangle_normal_hessian(t0, t1, t2); // Vector from t0 to p @@ -60,7 +60,8 @@ Matrix12d point_plane_signed_distance_hessian( // A. Contraction of the normal Hessian tensor with vector v // hess_n is 3x81. v is 3x1. Result is 1x81, which maps to 9x9. - hess.block<9, 9>(3, 3) = (v.transpose() * hess_n).reshaped(9, 9); + hess.block<9, 9>(3, 3) = + (hess_n.reshaped(3, 81).transpose() * v).reshaped(9, 9); // B. Subtract first derivative terms (Product Rule corrections) // Extract 3x3 Jacobian blocks for t0, t1, t2 diff --git a/src/ipc/geometry/normal.cpp b/src/ipc/geometry/normal.cpp index c082fea97..b1abcb0ae 100644 --- a/src/ipc/geometry/normal.cpp +++ b/src/ipc/geometry/normal.cpp @@ -87,7 +87,7 @@ MatrixMax point_line_unnormalized_normal_jacobian( return dn; } -MatrixMax point_line_unnormalized_normal_hessian( +MatrixMax point_line_unnormalized_normal_hessian( Eigen::ConstRef p, Eigen::ConstRef e0, Eigen::ConstRef e1) @@ -96,15 +96,14 @@ MatrixMax point_line_unnormalized_normal_hessian( assert(p.size() == 2 || p.size() == 3); if (p.size() == 2) { // In 2D, the normal is simply the perpendicular vector to the line - return MatrixMax::Zero(2, 36); + return MatrixMax::Zero(12, 6); } else { const Eigen::Vector3d e = e1 - e0; const Eigen::Vector3d d = p - e0; const auto I = Eigen::Matrix3d::Identity(); - // 3 components (rows), 81 entries (flattened 9x9 Hessian) - MatrixMax hess(3, 81); - hess.setZero(); + // The full Hessian is 27x9 (3 components of n, each with 9 variables) + MatrixMax hess = MatrixMax::Zero(27, 9); // Compute the Hessian for each component k of the normal vector n for (int k = 0; k < 3; ++k) { @@ -114,8 +113,7 @@ MatrixMax point_line_unnormalized_normal_hessian( // The full 9x9 Hessian for component n[k] // Order of variables: p (0-2), e0 (3-5), e1 (6-8) - Matrix9d Hk; - Hk.setZero(); + Matrix9d Hk = Matrix9d::Zero(); // ------------------------------------------------------------- // Block (p, e1): Derivative of ∂n/∂p w.r.t e1 @@ -180,15 +178,17 @@ MatrixMax point_line_unnormalized_normal_hessian( // = H_pe1 - H_e1e0 Hk.block<3, 3>(3, 3) = H_pe1 - H_e1e0; - // Flatten 9x9 (Column-Major) and assign to row k - hess.row(k) = Hk.reshaped(); + // Assign Hₖ to the strided rows of the full Hessian + for (int i = 0; i < 9; ++i) { + hess.row(3 * i + k) = Hk.row(i); + } } return hess; } } -MatrixMax point_line_normal_hessian( +MatrixMax point_line_normal_hessian( Eigen::ConstRef p, Eigen::ConstRef e0, Eigen::ConstRef e1) @@ -201,26 +201,28 @@ MatrixMax point_line_normal_hessian( const double z_norm = std::sqrt(z_norm2); const double z_norm3 = z_norm2 * z_norm; - const int DOF = 3 * z.size(); // total dof (6 or 9) - const int ROWS = z.size(); // dimension (2 or 3) - const int COLS = DOF * DOF; // (dof per variable)² + const int DIM = z.size(); // dimension (2 or 3) + const int DOF = 3 * DIM; // total dof (6 or 9) const auto dz_dx = point_line_unnormalized_normal_jacobian(p, e0, e1); const auto d2z_dx2 = point_line_unnormalized_normal_hessian(p, e0, e1); - MatrixMax d2n_dx2(ROWS, COLS); - for (int k = 0; k < COLS; ++k) { + MatrixMax d2n_dx2(DOF * DIM, DOF); + for (int k = 0; k < DOF * DOF; ++k) { const int i = k / DOF, j = k % DOF; const double alpha = z.dot(dz_dx.col(i)) / z_norm3; + const VectorMax3d d2z_dxidxj = d2z_dx2.block(DIM * i, j, DIM, 1); + const double dalpha_dxj = - (dz_dx.col(j).dot(dz_dx.col(i)) + z.dot(d2z_dx2.col(k)) + (dz_dx.col(j).dot(dz_dx.col(i)) + z.dot(d2z_dxidxj) - 3 * z.dot(dz_dx.col(i)) * dz_dx.col(j).dot(z) / z_norm2) / z_norm3; - d2n_dx2.col(k) = -dz_dx.col(i) * z.transpose() * dz_dx.col(j) / z_norm3 - + d2z_dx2.col(k) / z_norm - alpha * dz_dx.col(j) - dalpha_dxj * z; + d2n_dx2.block(DIM * i, j, DIM, 1) = + -dz_dx.col(i) * z.transpose() * dz_dx.col(j) / z_norm3 + + d2z_dxidxj / z_norm - alpha * dz_dx.col(j) - dalpha_dxj * z; } return d2n_dx2; @@ -237,14 +239,14 @@ namespace { } } // namespace -Eigen::Matrix cross_product_matrix_jacobian() +Eigen::Matrix cross_product_matrix_jacobian() { Eigen::Matrix J = Eigen::Matrix::Zero(); set_cross_product_matrix_jacobian(J); - return J.reshaped(3, 9); + return J; } -Eigen::Matrix triangle_unnormalized_normal_hessian( +Eigen::Matrix triangle_unnormalized_normal_hessian( Eigen::ConstRef a, Eigen::ConstRef b, Eigen::ConstRef c) @@ -263,10 +265,10 @@ Eigen::Matrix triangle_unnormalized_normal_hessian( set_cross_product_matrix_jacobian(H.block<9, 3>(18, 3), 1.0); // ∂²n/∂c∂b // ∂²n/∂c² = 0 - return H.reshaped(3, 81); + return H; } -Eigen::Matrix triangle_normal_hessian( +Eigen::Matrix triangle_normal_hessian( Eigen::ConstRef a, Eigen::ConstRef b, Eigen::ConstRef c) @@ -279,19 +281,22 @@ Eigen::Matrix triangle_normal_hessian( const auto dz_dx = triangle_unnormalized_normal_jacobian(a, b, c); const auto d2z_dx2 = triangle_unnormalized_normal_hessian(a, b, c); - Eigen::Matrix d2n_dx2; + Eigen::Matrix d2n_dx2; for (int k = 0; k < 81; ++k) { const int i = k / 9, j = k % 9; const double alpha = z.dot(dz_dx.col(i)) / z_norm3; + const auto d2z_dxidxj = d2z_dx2.block<3, 1>(3 * i, j); + const double dalpha_dxj = - (dz_dx.col(j).dot(dz_dx.col(i)) + z.dot(d2z_dx2.col(k)) + (dz_dx.col(j).dot(dz_dx.col(i)) + z.dot(d2z_dxidxj) - 3 * z.dot(dz_dx.col(i)) * dz_dx.col(j).dot(z) / z_norm2) / z_norm3; - d2n_dx2.col(k) = -dz_dx.col(i) * z.transpose() * dz_dx.col(j) / z_norm3 - + d2z_dx2.col(k) / z_norm - alpha * dz_dx.col(j) - dalpha_dxj * z; + d2n_dx2.block<3, 1>(3 * i, j) = + -dz_dx.col(i) * z.transpose() * dz_dx.col(j) / z_norm3 + + d2z_dxidxj / z_norm - alpha * dz_dx.col(j) - dalpha_dxj * z; } return d2n_dx2; @@ -299,7 +304,7 @@ Eigen::Matrix triangle_normal_hessian( // --- line-line normal functions --------------------------------------------- -Eigen::Matrix line_line_unnormalized_normal_hessian( +Eigen::Matrix line_line_unnormalized_normal_hessian( Eigen::ConstRef ea0, Eigen::ConstRef ea1, Eigen::ConstRef eb0, @@ -327,10 +332,10 @@ Eigen::Matrix line_line_unnormalized_normal_hessian( // ∂²n/∂d∂c = 0 // ∂²n/∂d² = 0 - return H.reshaped(3, 144); + return H; } -Eigen::Matrix line_line_normal_hessian( +Eigen::Matrix line_line_normal_hessian( Eigen::ConstRef ea0, Eigen::ConstRef ea1, Eigen::ConstRef eb0, @@ -341,24 +346,27 @@ Eigen::Matrix line_line_normal_hessian( const double z_norm = std::sqrt(z_norm2); const double z_norm3 = z_norm2 * z_norm; - const auto dz_dx = + const Eigen::Matrix dz_dx = line_line_unnormalized_normal_jacobian(ea0, ea1, eb0, eb1); - const auto d2z_dx2 = + const Eigen::Matrix d2z_dx2 = line_line_unnormalized_normal_hessian(ea0, ea1, eb0, eb1); - Eigen::Matrix d2n_dx2; + Eigen::Matrix d2n_dx2; for (int k = 0; k < 144; ++k) { const int i = k / 12, j = k % 12; const double alpha = z.dot(dz_dx.col(i)) / z_norm3; + const auto d2z_dxidxj = d2z_dx2.block<3, 1>(3 * i, j); + const double dalpha_dxj = - (dz_dx.col(j).dot(dz_dx.col(i)) + z.dot(d2z_dx2.col(k)) + (dz_dx.col(j).dot(dz_dx.col(i)) + z.dot(d2z_dxidxj) - 3 * z.dot(dz_dx.col(i)) * dz_dx.col(j).dot(z) / z_norm2) / z_norm3; - d2n_dx2.col(k) = -dz_dx.col(i) * z.transpose() * dz_dx.col(j) / z_norm3 - + d2z_dx2.col(k) / z_norm - alpha * dz_dx.col(j) - dalpha_dxj * z; + d2n_dx2.block<3, 1>(3 * i, j) = + -dz_dx.col(i) * z.transpose() * dz_dx.col(j) / z_norm3 + + d2z_dxidxj / z_norm - alpha * dz_dx.col(j) - dalpha_dxj * z; } return d2n_dx2; diff --git a/src/ipc/geometry/normal.hpp b/src/ipc/geometry/normal.hpp index c3b6e2248..35db43c19 100644 --- a/src/ipc/geometry/normal.hpp +++ b/src/ipc/geometry/normal.hpp @@ -53,7 +53,7 @@ inline Eigen::Matrix3d cross_product_matrix(Eigen::ConstRef v) /// @brief Computes the Jacobian of the cross product matrix. /// @return The Jacobian of the cross product matrix. -Eigen::Matrix cross_product_matrix_jacobian(); +Eigen::Matrix cross_product_matrix_jacobian(); // ============================================================================= @@ -101,7 +101,7 @@ MatrixMax point_line_unnormalized_normal_jacobian( /// @param e0 The start position of the line. /// @param e1 The end position of the line. /// @return The Hessian of the unnormalized normal vector of the point-line pair. -MatrixMax point_line_unnormalized_normal_hessian( +MatrixMax point_line_unnormalized_normal_hessian( Eigen::ConstRef p, Eigen::ConstRef e0, Eigen::ConstRef e1); @@ -126,7 +126,7 @@ inline MatrixMax point_line_normal_jacobian( /// @param e0 The start position of the line. /// @param e1 The end position of the line. /// @return The Hessian of the normal vector. -MatrixMax point_line_normal_hessian( +MatrixMax point_line_normal_hessian( Eigen::ConstRef p, Eigen::ConstRef e0, Eigen::ConstRef e1); @@ -189,7 +189,7 @@ inline Eigen::Matrix triangle_unnormalized_normal_jacobian( /// @param b The second vertex of the triangle. /// @param c The third vertex of the triangle. /// @return The Hessian of the unnormalized normal vector of the triangle. -Eigen::Matrix triangle_unnormalized_normal_hessian( +Eigen::Matrix triangle_unnormalized_normal_hessian( Eigen::ConstRef a, Eigen::ConstRef b, Eigen::ConstRef c); @@ -214,7 +214,7 @@ inline Eigen::Matrix triangle_normal_jacobian( /// @param b The second vertex of the triangle. /// @param c The third vertex of the triangle. /// @return The Hessian of the normal vector of the triangle. -Eigen::Matrix triangle_normal_hessian( +Eigen::Matrix triangle_normal_hessian( Eigen::ConstRef a, Eigen::ConstRef b, Eigen::ConstRef c); @@ -303,7 +303,7 @@ inline Eigen::Matrix line_line_normal_jacobian( /// @param eb0 The first vertex of the second line. /// @param eb1 The second vertex of the second line. /// @return The Hessian of the unnormalized normal vector of the two lines. -Eigen::Matrix line_line_unnormalized_normal_hessian( +Eigen::Matrix line_line_unnormalized_normal_hessian( Eigen::ConstRef ea0, Eigen::ConstRef ea1, Eigen::ConstRef eb0, @@ -315,7 +315,7 @@ Eigen::Matrix line_line_unnormalized_normal_hessian( /// @param eb0 The first vertex of the second line. /// @param eb1 The second vertex of the second line. /// @return The Hessian of the normal vector of the two lines. -Eigen::Matrix line_line_normal_hessian( +Eigen::Matrix line_line_normal_hessian( Eigen::ConstRef ea0, Eigen::ConstRef ea1, Eigen::ConstRef eb0, diff --git a/src/ipc/potentials/tangential_potential.cpp b/src/ipc/potentials/tangential_potential.cpp index 7020a3ee7..f5b8aad78 100644 --- a/src/ipc/potentials/tangential_potential.cpp +++ b/src/ipc/potentials/tangential_potential.cpp @@ -179,7 +179,7 @@ VectorMax12d TangentialPotential::gradient( // Compute T = ΓᵀP const MatrixMax T = - collision.relative_velocity_matrix().transpose() + collision.relative_velocity_jacobian().transpose() * collision.tangent_basis; // Compute μ(‖u‖) f₁(‖u‖)/‖u‖ @@ -214,7 +214,7 @@ MatrixMax12d TangentialPotential::hessian( // Compute T = ΓᵀP const MatrixMax T = - collision.relative_velocity_matrix().transpose() + collision.relative_velocity_jacobian().transpose() * collision.tangent_basis; // Compute ‖u‖ @@ -311,7 +311,7 @@ VectorMax12d TangentialPotential::force( // Compute Γ const MatrixMax Gamma = - collision.relative_velocity_matrix(beta); + collision.relative_velocity_jacobian(beta); // Compute T = ΓᵀP const MatrixMax T = Gamma.transpose() * P; @@ -348,9 +348,9 @@ MatrixMax12d TangentialPotential::force_jacobian( // Compute ∇F assert(rest_positions.size() == lagged_displacements.size()); assert(lagged_displacements.size() == velocities.size()); - const int n = rest_positions.size(); - const int dim = n / collision.num_vertices(); - assert(n % collision.num_vertices() == 0); + const int ndof = rest_positions.size(); + const int dim = ndof / collision.num_vertices(); + assert(ndof % collision.num_vertices() == 0); // const VectorMax12d x = dof(rest_positions, edges, faces); // const VectorMax12d u = dof(lagged_displacements, edges, faces); @@ -383,39 +383,40 @@ MatrixMax12d TangentialPotential::force_jacobian( // Compute Γ const MatrixMax Gamma = - collision.relative_velocity_matrix(beta); + collision.relative_velocity_jacobian(beta); // Compute T = ΓᵀP const MatrixMax T = Gamma.transpose() * P; // Compute ∇T - MatrixMax jac_T; + MatrixMax jac_T; if (need_jac_N_or_T) { - jac_T.resize(n * n, dim - 1); // ∇T = ∇(ΓᵀP) = ∇ΓᵀP + Γᵀ∇P - const MatrixMax jac_P = + + // Compute Γᵀ∇P + const MatrixMax jac_P = collision.compute_tangent_basis_jacobian(lagged_positions); - for (int i = 0; i < n; i++) { - // ∂T/∂xᵢ += Γᵀ ∂P/∂xᵢ - jac_T.middleRows(i * n, n) = - Gamma.transpose() * jac_P.middleRows(i * dim, dim); - } + jac_T = (Gamma.transpose() * jac_P.reshaped(P.rows(), P.cols() * ndof)) + .reshaped(T.size(), ndof); // Vertex-vertex does not have a closest point if (beta.size() != 0) { - // ∇Γ(β) = ∇ᵦΓ∇β ∈ ℝ^{d×n×n} ≡ ℝ^{nd×n} - const MatrixMax jac_beta = + // Compute: ∇ΓᵀP = (∇ᵦΓ ∇β)ᵀ P + const MatrixMax dbeta_dx = // ∇β collision.compute_closest_point_jacobian(lagged_positions); - const MatrixMax jac_Gamma_wrt_beta = - collision.relative_velocity_matrix_jacobian(beta); - - for (int k = 0; k < n; k++) { - for (int b = 0; b < beta.size(); b++) { - jac_T.middleRows(k * n, n) += - jac_Gamma_wrt_beta.transpose().middleCols(b * dim, dim) - * (jac_beta(b, k) * P); - } + const MatrixMax dGamma_dbeta = // ∇ᵦΓ + collision.relative_velocity_dx_dbeta(beta); + + // 1. Precompute dT/dβ = [dΓ/dβ]ᵀ P + MatrixMax dT_dbeta(T.size(), beta.size()); + for (int b = 0; b < beta.size(); ++b) { + dT_dbeta.col(b) = + (dGamma_dbeta.col(b).reshaped(dim, ndof).transpose() * P) + .reshaped(); } + + // 2. Apply chain rule: dT/dβ * dβ/dx + jac_T += dT_dbeta * dbeta_dx; } } @@ -425,12 +426,10 @@ MatrixMax12d TangentialPotential::force_jacobian( // Compute ∇τ = ∇T(x + u)ᵀv + T(x + u)ᵀ∇v MatrixMax jac_tau; if (need_jac_N_or_T) { - jac_tau.resize(dim - 1, n); + jac_tau.resize(dim - 1, ndof); // Compute ∇T(x + u)ᵀv - for (int i = 0; i < n; i++) { - jac_tau.col(i) = - jac_T.middleRows(i * n, n).transpose() * velocities; - } + jac_tau = (jac_T.reshaped(ndof, T.size()).transpose() * velocities) + .reshaped(jac_tau.rows(), jac_tau.cols()); } else { jac_tau = T.transpose(); // Tᵀ ∇ᵥv = Tᵀ } @@ -444,7 +443,7 @@ MatrixMax12d TangentialPotential::force_jacobian( VectorMax12d grad_mu_f1_over_norm_tau; if (tau_norm == 0) { // lim_{x→0} f₂(x)x² = 0 - grad_mu_f1_over_norm_tau.setZero(n); + grad_mu_f1_over_norm_tau.setZero(ndof); } else { // ∇ (f₁(‖τ‖)/‖τ‖) = (f₂(‖τ‖)‖τ‖ - f₁(‖τ‖)) / ‖τ‖³ τᵀ ∇τ double f2 = mu_f2_x_minus_mu_f1_over_x3( @@ -458,7 +457,7 @@ MatrixMax12d TangentialPotential::force_jacobian( // ------------------------------------------------------------------------ // Compute J = ∇F = ∇(-μ N f₁(‖τ‖)/‖τ‖ T τ) - MatrixMax12d J = MatrixMax12d::Zero(n, n); + MatrixMax12d J = MatrixMax12d::Zero(ndof, ndof); // = -μ f₁(‖τ‖)/‖τ‖ (T τ) [∇N]ᵀ if (need_jac_N_or_T) { @@ -471,9 +470,9 @@ MatrixMax12d TangentialPotential::force_jacobian( // + -μ N f₁(‖τ‖)/‖τ‖ [∇T] τ if (need_jac_N_or_T) { const VectorMax2d scaled_tau = N * mu_f1_over_norm_tau * tau; - for (int i = 0; i < n; i++) { + for (int i = 0; i < ndof; i++) { // ∂J/∂xᵢ = ∂T/∂xᵢ * τ - J.col(i) += jac_T.middleRows(i * n, n) * scaled_tau; + J.col(i) += jac_T.col(i).reshaped(T.rows(), T.cols()) * scaled_tau; } } @@ -701,7 +700,7 @@ TangentialPotential::VectorMaxNd TangentialPotential::smooth_contact_force( // Compute Γ const MatrixMax Gamma = - collision.relative_velocity_matrix(beta); + collision.relative_velocity_jacobian(beta); // Compute T = ΓᵀP const MatrixMax T = Gamma.transpose() * P; @@ -737,9 +736,9 @@ TangentialPotential::smooth_contact_force_jacobian_unit( // // Compute ∇F assert(lagged_positions.size() == velocities.size()); - const int n = lagged_positions.size(); - const int dim = n / collision.num_vertices(); - assert(n % collision.num_vertices() == 0); + const int ndof = lagged_positions.size(); + const int dim = ndof / collision.num_vertices(); + assert(ndof % collision.num_vertices() == 0); const bool need_jac_N_or_T = wrt != DiffWRT::VELOCITIES; @@ -752,39 +751,41 @@ TangentialPotential::smooth_contact_force_jacobian_unit( // Compute Γ const MatrixMax Gamma = - collision.relative_velocity_matrix(beta); + collision.relative_velocity_jacobian(beta); // Compute T = ΓᵀP const MatrixMax T = Gamma.transpose() * P; // Compute ∇T - MatrixMax jac_T; + MatrixMax jac_T; if (need_jac_N_or_T) { - jac_T.resize(n * n, dim - 1); // ∇T = ∇(ΓᵀP) = ∇ΓᵀP + Γᵀ∇P - const MatrixMax jac_P = + + // Compute Γᵀ∇P + const MatrixMax jac_P = collision.compute_tangent_basis_jacobian(lagged_positions); - for (int i = 0; i < n; i++) { - // ∂T/∂xᵢ += Γᵀ ∂P/∂xᵢ - jac_T.middleRows(i * n, n) = - Gamma.transpose() * jac_P.middleRows(i * dim, dim); - } + jac_T = (Gamma.transpose() * jac_P.reshaped(P.rows(), P.cols() * ndof)) + .reshaped(T.size(), ndof); // Vertex-vertex does not have a closest point if (beta.size()) { - // ∇Γ(β) = ∇ᵦΓ∇β ∈ ℝ^{d×n×n} ≡ ℝ^{nd×n} - const MatrixMax jac_beta = + // Compute: ∇ΓᵀP = (∇ᵦΓ ∇β)ᵀ P + const MatrixMax dbeta_dx = // ∇β collision.compute_closest_point_jacobian(lagged_positions); - const MatrixMax jac_Gamma_wrt_beta = - collision.relative_velocity_matrix_jacobian(beta); - - for (int k = 0; k < n; k++) { - for (int b = 0; b < beta.size(); b++) { - jac_T.middleRows(k * n, n) += - jac_Gamma_wrt_beta.transpose().middleCols(b * dim, dim) - * (jac_beta(b, k) * P); - } + const MatrixMax dGamma_dbeta = // ∇ᵦΓ + collision.relative_velocity_dx_dbeta(beta); + + // 1. Precompute dT/dβ = [dΓ/dβ]ᵀ P + MatrixMax dT_dbeta( + T.size(), beta.size()); + for (int b = 0; b < beta.size(); ++b) { + dT_dbeta.col(b) = + (dGamma_dbeta.col(b).reshaped(dim, ndof).transpose() * P) + .reshaped(); } + + // 2. Apply chain rule: dT/dβ * dβ/dx + jac_T += dT_dbeta * dbeta_dx; } } @@ -794,12 +795,10 @@ TangentialPotential::smooth_contact_force_jacobian_unit( // Compute ∇τ = ∇T(x + u)ᵀv + T(x + u)ᵀ∇v MatrixMax jac_tau; if (need_jac_N_or_T) { - jac_tau.resize(dim - 1, n); + jac_tau.resize(dim - 1, ndof); // Compute ∇T(x + u)ᵀv - for (int i = 0; i < n; i++) { - jac_tau.col(i) = - jac_T.middleRows(i * n, n).transpose() * velocities; - } + jac_tau = (jac_T.reshaped(ndof, T.size()).transpose() * velocities) + .reshaped(jac_tau.rows(), jac_tau.cols()); } else { jac_tau = T.transpose(); // Tᵀ ∇ᵥv = Tᵀ } @@ -814,7 +813,7 @@ TangentialPotential::smooth_contact_force_jacobian_unit( VectorMaxNd grad_f1_over_norm_tau; if (tau_norm == 0) { // lim_{x→0} f₂(x)x² = 0 - grad_f1_over_norm_tau.setZero(n); + grad_f1_over_norm_tau.setZero(ndof); } else { // ∇ (f₁(‖τ‖)/‖τ‖) = (f₁'(‖τ‖)‖τ‖ - f₁(‖τ‖)) / ‖τ‖³ τᵀ ∇τ double f2 = mu_f2_x_minus_mu_f1_over_x3(tau_norm, mu_s, mu_k); @@ -827,7 +826,7 @@ TangentialPotential::smooth_contact_force_jacobian_unit( // ------------------------------------------------------------------------ // Compute J = ∇F = ∇(-μ N f₁(‖τ‖)/‖τ‖ T τ) - MatrixMaxNd J = MatrixMaxNd::Zero(n, n); + MatrixMaxNd J = MatrixMaxNd::Zero(ndof, ndof); // + -μ N T τ [∇(f₁(‖τ‖)/‖τ‖)] J += T_times_tau * grad_f1_over_norm_tau.transpose(); @@ -835,9 +834,9 @@ TangentialPotential::smooth_contact_force_jacobian_unit( // + -μ N f₁(‖τ‖)/‖τ‖ [∇T] τ if (need_jac_N_or_T) { const VectorMax2d scaled_tau = f1_over_norm_tau * tau; - for (int i = 0; i < n; i++) { + for (int i = 0; i < ndof; i++) { // ∂J/∂xᵢ = ∂T/∂xᵢ * τ - J.col(i) += jac_T.middleRows(i * n, n) * scaled_tau; + J.col(i) += jac_T.col(i).reshaped(T.rows(), T.cols()) * scaled_tau; } } @@ -845,9 +844,9 @@ TangentialPotential::smooth_contact_force_jacobian_unit( J += f1_over_norm_tau * T * jac_tau; // NOTE: ∇ₓw(x) is not local to the collision pair (i.e., it involves more - // than the 4 collisioning vertices), so we do not have enough information + // than the 4 colliding vertices), so we do not have enough information // here to compute the gradient. Instead this should be handled outside of - // the function. For a simple multiplicitive model (∑ᵢ wᵢ Fᵢ) this can be + // the function. For a simple multiplicative model (∑ᵢ wᵢ Fᵢ) this can be // done easily. J *= -collision.weight; diff --git a/src/ipc/tangent/relative_velocity.cpp b/src/ipc/tangent/relative_velocity.cpp index 28e69df03..f020a50e4 100644 --- a/src/ipc/tangent/relative_velocity.cpp +++ b/src/ipc/tangent/relative_velocity.cpp @@ -11,7 +11,7 @@ VectorMax3d point_point_relative_velocity( return dp0 - dp1; } -MatrixMax point_point_relative_velocity_matrix(const int dim) +MatrixMax point_point_relative_velocity_jacobian(const int dim) { MatrixMax J(dim, 2 * dim); J.leftCols(dim) = MatrixMax3d::Identity(dim, dim); @@ -19,10 +19,10 @@ MatrixMax point_point_relative_velocity_matrix(const int dim) return J; } -MatrixMax -point_point_relative_velocity_matrix_jacobian(const int dim) +VectorMax point_point_relative_velocity_dx_dbeta(const int dim) { - return MatrixMax::Zero(dim, 2 * dim); + // Γ is constant (does not depend on β), so the derivative is zero. + return VectorMax::Zero(2 * dim * dim); } // ============================================================================ @@ -38,7 +38,7 @@ VectorMax3d point_edge_relative_velocity( } MatrixMax -point_edge_relative_velocity_matrix(const int dim, const double alpha) +point_edge_relative_velocity_jacobian(const int dim, const double alpha) { MatrixMax J = MatrixMax::Zero(dim, 3 * dim); J.leftCols(dim).diagonal().setOnes(); @@ -47,12 +47,23 @@ point_edge_relative_velocity_matrix(const int dim, const double alpha) return J; } -MatrixMax -point_edge_relative_velocity_matrix_jacobian(const int dim, const double alpha) +// Γ(α) = [I, (α-1)I, -αI] (dim × 3·dim) +// ∂Γ/∂α = [0, I, -I] +// +// Stored as vec(∂Γ/∂α) in column-major order (3rd-order convention). +// Result is a column vector of size dim × 3·dim = 3·dim². +// For a (dim × ndof) matrix M, element M(r,c) maps to vec index c·dim + r. +VectorMax +point_edge_relative_velocity_dx_dbeta(const int dim, const double alpha) { - MatrixMax J = MatrixMax::Zero(dim, 3 * dim); - J.middleCols(dim, dim).diagonal().setConstant(1); - J.rightCols(dim).diagonal().setConstant(-1); + const int ndof = 3 * dim; + VectorMax J = VectorMax::Zero(dim * ndof); + for (int i = 0; i < dim; ++i) { + // I block at cols [dim, 2·dim) + J[(dim + i) * dim + i] = 1; + // -I block at cols [2·dim, 3·dim) + J[(2 * dim + i) * dim + i] = -1; + } return J; } @@ -71,28 +82,37 @@ Eigen::Vector3d edge_edge_relative_velocity( - ((deb1 - deb0) * coords[1] + deb0); } -MatrixMax edge_edge_relative_velocity_matrix( - const int dim, Eigen::ConstRef coords) +Eigen::Matrix +edge_edge_relative_velocity_jacobian(Eigen::ConstRef coords) { - MatrixMax J = MatrixMax::Zero(dim, 4 * dim); - J.leftCols(dim).diagonal().setConstant(1 - coords[0]); - J.middleCols(dim, dim).diagonal().setConstant(coords[0]); - J.middleCols(2 * dim, dim).diagonal().setConstant(coords[1] - 1); - J.rightCols(dim).diagonal().setConstant(-coords[1]); + Eigen::Matrix J = Eigen::Matrix::Zero(); + J.leftCols<3>().diagonal().setConstant(1 - coords[0]); + J.middleCols<3>(3).diagonal().setConstant(coords[0]); + J.middleCols<3>(6).diagonal().setConstant(coords[1] - 1); + J.rightCols<3>().diagonal().setConstant(-coords[1]); return J; } -MatrixMax edge_edge_relative_velocity_matrix_jacobian( - const int dim, Eigen::ConstRef coords) +// Γ(β₁,β₂) = [(1-β₁)I, β₁I, (β₂-1)I, -β₂I] (3 × 12) +// ∂Γ/∂β₁ = [-I, I, 0, 0] +// ∂Γ/∂β₂ = [ 0, 0, I,-I] +// +// Stored as [vec(∂Γ/∂β₁) | vec(∂Γ/∂β₂)] in column-major order (3rd-order +// convention). Result shape: (36, 2). For a (3 × 12) matrix M, element M(r,c) +// maps to vec index c·3 + r. +Eigen::Matrix +edge_edge_relative_velocity_dx_dbeta(Eigen::ConstRef coords) { - MatrixMax J = - MatrixMax::Zero(2 * dim, 4 * dim); - // wrt β₁ - J.block(0, 0, dim, dim).diagonal().setConstant(-1); - J.block(0, dim, dim, dim).diagonal().setConstant(1); - // wrt β₂ - J.block(dim, 2 * dim, dim, dim).diagonal().setConstant(1); - J.block(dim, 3 * dim, dim, dim).diagonal().setConstant(-1); + constexpr int dim = 3; + Eigen::Matrix J = Eigen::Matrix::Zero(); + for (int i = 0; i < dim; ++i) { + // wrt β₁: -I at cols [0,3), I at cols [3,6) + J((0 + i) * dim + i, 0) = -1; + J((3 + i) * dim + i, 0) = 1; + // wrt β₂: I at cols [6,9), -I at cols [9,12) + J((6 + i) * dim + i, 1) = 1; + J((9 + i) * dim + i, 1) = -1; + } return J; } @@ -111,29 +131,38 @@ Eigen::Vector3d point_triangle_relative_velocity( return dp - (dt0 + coords[0] * (dt1 - dt0) + coords[1] * (dt2 - dt0)); } -MatrixMax point_triangle_relative_velocity_matrix( - const int dim, Eigen::ConstRef coords) +Eigen::Matrix point_triangle_relative_velocity_jacobian( + Eigen::ConstRef coords) { - MatrixMax J = MatrixMax::Zero(dim, 4 * dim); - J.leftCols(dim).diagonal().setOnes(); - J.middleCols(dim, dim).diagonal().setConstant(coords[0] + coords[1] - 1); - J.middleCols(2 * dim, dim).diagonal().setConstant(-coords[0]); - J.rightCols(dim).diagonal().setConstant(-coords[1]); + Eigen::Matrix J = Eigen::Matrix::Zero(); + J.leftCols<3>().diagonal().setOnes(); + J.middleCols<3>(3).diagonal().setConstant(coords[0] + coords[1] - 1); + J.middleCols<3>(6).diagonal().setConstant(-coords[0]); + J.rightCols<3>().diagonal().setConstant(-coords[1]); return J; } -MatrixMax point_triangle_relative_velocity_matrix_jacobian( - const int dim, Eigen::ConstRef coords) +// Γ(β₁,β₂) = [I, (β₁+β₂-1)I, -β₁I, -β₂I] (3 × 12) +// ∂Γ/∂β₁ = [0, I, -I, 0] +// ∂Γ/∂β₂ = [0, I, 0,-I] +// +// Stored as [vec(∂Γ/∂β₁) | vec(∂Γ/∂β₂)] in column-major order (3rd-order +// convention). Result shape: (36, 2). For a (3 × 12) matrix M, element M(r,c) +// maps to vec index c·3 + r. +Eigen::Matrix point_triangle_relative_velocity_dx_dbeta( + Eigen::ConstRef coords) { - MatrixMax J = - MatrixMax::Zero(2 * dim, 4 * dim); - // wrt β₁ - J.block(0, dim, dim, dim).diagonal().setConstant(1); - J.block(0, 2 * dim, dim, dim).diagonal().setConstant(-1); - // wrt β₂ - J.block(dim, dim, dim, dim).diagonal().setConstant(1); - J.block(dim, 3 * dim, dim, dim).diagonal().setConstant(-1); + constexpr int dim = 3; + Eigen::Matrix J = Eigen::Matrix::Zero(); + for (int i = 0; i < dim; ++i) { + // wrt β₁: I at cols [3,6), -I at cols [6,9) + J((3 + i) * dim + i, 0) = 1; + J((6 + i) * dim + i, 0) = -1; + // wrt β₂: I at cols [3,6), -I at cols [9,12) + J((3 + i) * dim + i, 1) = 1; + J((9 + i) * dim + i, 1) = -1; + } return J; } -} // namespace ipc +} // namespace ipc \ No newline at end of file diff --git a/src/ipc/tangent/relative_velocity.hpp b/src/ipc/tangent/relative_velocity.hpp index 0c1eafe2b..f2cad8eca 100644 --- a/src/ipc/tangent/relative_velocity.hpp +++ b/src/ipc/tangent/relative_velocity.hpp @@ -14,16 +14,15 @@ namespace ipc { VectorMax3d point_point_relative_velocity( Eigen::ConstRef dp0, Eigen::ConstRef dp1); -/// @brief Compute the point-point relative velocity premultiplier matrix +/// @brief Compute du/dx where u is the relative velocity of two points /// @param dim Dimension (2 or 3) -/// @return The relative velocity premultiplier matrix -MatrixMax point_point_relative_velocity_matrix(const int dim); +/// @return The relative velocity Jacobian du/dx +MatrixMax point_point_relative_velocity_jacobian(const int dim); -/// @brief Compute the Jacobian of the relative velocity premultiplier matrix +/// @brief Compute d²u/dxdβ where u is the relative velocity of two points /// @param dim Dimension (2 or 3) -/// @return The Jacobian of the relative velocity premultiplier matrix -MatrixMax -point_point_relative_velocity_matrix_jacobian(const int dim); +/// @return The vectorized tensor of d²u/dxdβ +VectorMax point_point_relative_velocity_dx_dbeta(const int dim); // ============================================================================ // Point - Edge @@ -40,19 +39,19 @@ VectorMax3d point_edge_relative_velocity( Eigen::ConstRef de1, const double alpha); -/// @brief Compute the point-edge relative velocity premultiplier matrix +/// @brief Compute du/dx where u is the relative velocity of a point and an edge /// @param dim Dimension (2 or 3) /// @param alpha Parametric coordinate of the closest point on the edge -/// @return The relative velocity premultiplier matrix +/// @return The relative velocity Jacobian du/dx MatrixMax -point_edge_relative_velocity_matrix(const int dim, const double alpha); +point_edge_relative_velocity_jacobian(const int dim, const double alpha); -/// @brief Compute the Jacobian of the relative velocity premultiplier matrix +/// @brief Compute d²u/dxdα where u is the relative velocity of a point and an edge /// @param dim Dimension (2 or 3) /// @param alpha Parametric coordinate of the closest point on the edge -/// @return The Jacobian of the relative velocity premultiplier matrix -MatrixMax -point_edge_relative_velocity_matrix_jacobian(const int dim, const double alpha); +/// @return The vectorized tensor of d²u/dxdα +VectorMax +point_edge_relative_velocity_dx_dbeta(const int dim, const double alpha); // ============================================================================ // Edge - Edge @@ -71,19 +70,18 @@ Eigen::Vector3d edge_edge_relative_velocity( Eigen::ConstRef deb1, Eigen::ConstRef coords); -/// @brief Compute the edge-edge relative velocity matrix. +/// @brief Compute du/dx where u is the relative velocity of two edges /// @param dim Dimension (2 or 3) /// @param coords Two parametric coordinates of the closest points on the edges -/// @return The relative velocity matrix -MatrixMax edge_edge_relative_velocity_matrix( - const int dim, Eigen::ConstRef coords); +/// @return The relative velocity Jacobian du/dx +Eigen::Matrix +edge_edge_relative_velocity_jacobian(Eigen::ConstRef coords); -/// @brief Compute the Jacobian of the edge-edge relative velocity matrix. -/// @param dim Dimension (2 or 3) +/// @brief Compute d²u/dxdβ where u is the relative velocity of two edges /// @param coords Two parametric coordinates of the closest points on the edges -/// @return The Jacobian of the relative velocity matrix -MatrixMax edge_edge_relative_velocity_matrix_jacobian( - const int dim, Eigen::ConstRef coords); +/// @return The vectorized tensor of d²u/dxdβ +Eigen::Matrix +edge_edge_relative_velocity_dx_dbeta(Eigen::ConstRef coords); // ============================================================================ // Point - Triangle @@ -102,18 +100,16 @@ Eigen::Vector3d point_triangle_relative_velocity( Eigen::ConstRef dt2, Eigen::ConstRef coords); -/// @brief Compute the point-triangle relative velocity matrix. -/// @param dim Dimension (2 or 3) +/// @brief Compute du/dx where u is the relative velocity of a point and a triangle /// @param coords Barycentric coordinates of the closest point on the triangle -/// @return The relative velocity matrix -MatrixMax point_triangle_relative_velocity_matrix( - const int dim, Eigen::ConstRef coords); +/// @return The relative velocity Jacobian du/dx +Eigen::Matrix point_triangle_relative_velocity_jacobian( + Eigen::ConstRef coords); -/// @brief Compute the Jacobian of the point-triangle relative velocity matrix. -/// @param dim Dimension (2 or 3) +/// @brief Compute d²u/dxdβ where u is the relative velocity of a point and a triangle /// @param coords Baricentric coordinates of the closest point on the triangle -/// @return The Jacobian of the relative velocity matrix -MatrixMax point_triangle_relative_velocity_matrix_jacobian( - const int dim, Eigen::ConstRef coords); +/// @return The vectorized tensor of d²u/dxdβ +Eigen::Matrix point_triangle_relative_velocity_dx_dbeta( + Eigen::ConstRef coords); } // namespace ipc diff --git a/src/ipc/tangent/tangent_basis.cpp b/src/ipc/tangent/tangent_basis.cpp index 49d266570..bfbbcda0b 100644 --- a/src/ipc/tangent/tangent_basis.cpp +++ b/src/ipc/tangent/tangent_basis.cpp @@ -46,20 +46,20 @@ MatrixMax point_point_tangent_basis( } } -MatrixMax point_point_tangent_basis_jacobian( +MatrixMax point_point_tangent_basis_jacobian( Eigen::ConstRef p0, Eigen::ConstRef p1) { const int dim = p0.size(); assert(dim == p1.size()); - MatrixMax J; + MatrixMax J; if (dim == 2) { - J.resize(8, 1); + J.resize(2, 4); autogen::point_point_tangent_basis_2D_jacobian( p0[0], p0[1], p1[0], p1[1], J.data()); } else { assert(dim == 3); - J.resize(18, 2); + J.resize(6, 6); autogen::point_point_tangent_basis_3D_jacobian( p0[0], p0[1], p0[2], p1[0], p1[1], p1[2], J.data()); } @@ -91,7 +91,7 @@ MatrixMax point_edge_tangent_basis( } } -MatrixMax point_edge_tangent_basis_jacobian( +MatrixMax point_edge_tangent_basis_jacobian( Eigen::ConstRef p, Eigen::ConstRef e0, Eigen::ConstRef e1) @@ -99,14 +99,14 @@ MatrixMax point_edge_tangent_basis_jacobian( const int dim = p.size(); assert(dim == e0.size() && dim == e1.size()); - MatrixMax J; + MatrixMax J; if (dim == 2) { - J.resize(12, 1); + J.resize(2, 6); autogen::point_edge_tangent_basis_2D_jacobian( p[0], p[1], e0[0], e0[1], e1[0], e1[1], J.data()); } else { assert(dim == 3); - J.resize(27, 2); + J.resize(6, 9); autogen::point_edge_tangent_basis_3D_jacobian( p[0], p[1], p[2], e0[0], e0[1], e0[2], e1[0], e1[1], e1[2], J.data()); @@ -137,13 +137,13 @@ Eigen::Matrix edge_edge_tangent_basis( return basis; } -Eigen::Matrix edge_edge_tangent_basis_jacobian( +Eigen::Matrix edge_edge_tangent_basis_jacobian( Eigen::ConstRef ea0, Eigen::ConstRef ea1, Eigen::ConstRef eb0, Eigen::ConstRef eb1) { - Eigen::Matrix J; + Eigen::Matrix J; autogen::edge_edge_tangent_basis_jacobian( ea0[0], ea0[1], ea0[2], ea1[0], ea1[1], ea1[2], eb0[0], eb0[1], eb0[2], eb1[0], eb1[1], eb1[2], J.data()); @@ -174,13 +174,13 @@ Eigen::Matrix point_triangle_tangent_basis( return basis; } -Eigen::Matrix point_triangle_tangent_basis_jacobian( +Eigen::Matrix point_triangle_tangent_basis_jacobian( Eigen::ConstRef p, Eigen::ConstRef t0, Eigen::ConstRef t1, Eigen::ConstRef t2) { - Eigen::Matrix J; + Eigen::Matrix J; autogen::point_triangle_tangent_basis_jacobian( p[0], p[1], p[2], t0[0], t0[1], t0[2], t1[0], t1[1], t1[2], t2[0], t2[1], t2[2], J.data()); @@ -190,8 +190,7 @@ Eigen::Matrix point_triangle_tangent_basis_jacobian( // ============================================================================ namespace autogen { - - // J is (8×1) flattened in column-major order + // J is (2×4) flattened in column-major order void point_point_tangent_basis_2D_jacobian( double p0_x, double p0_y, double p1_x, double p1_y, double J[8]) { @@ -204,19 +203,19 @@ namespace autogen { const double t6 = -t5; const double t7 = 1.0 / std::sqrt(t4); const double t8 = 1.0 / t4; - const double t9 = t2 * t8; - const double t10 = t3 * t8; + const double t9 = t2 * t8 - 1; + const double t10 = t3 * t8 - 1; J[0] = t6; - J[1] = t7 * (t9 - 1); - J[2] = t7 * (1 - t10); + J[1] = t7 * t9; + J[2] = -t10 * t7; J[3] = t5; J[4] = t5; - J[5] = t7 * (1 - t9); - J[6] = t7 * (t10 - 1); + J[5] = -t7 * t9; + J[6] = t10 * t7; J[7] = t6; } - // J is (18×2) flattened in column-major order + // J is (6×6) flattened in column-major order void point_point_tangent_basis_3D_jacobian( double p0_x, double p0_y, @@ -233,144 +232,133 @@ namespace autogen { const double t4 = p0_y - p1_y; const double t5 = t4 * t4; const bool t6 = (t1 + t3) < (t3 + t5); - const double t7 = -p0_z + p1_z; + const double t7 = -t2; const double t8 = t6 ? 0 : t7; - const double t9 = t3 + (t6 ? t5 : t1); + const double t9 = (t6 ? 0 : t3) + (t6 ? t3 : 0) + (t6 ? t5 : t1); const double t10 = 1.0 / pow_1_5(t9); - const double t11 = t6 ? 0 : t0; + const double t11 = 0.5 * (t6 ? 0 : (2 * t0)); const double t12 = t10 * t11; const double t13 = t6 ? t2 : 0; - const double t14 = 1 / std::sqrt(t9); - const double t15 = int(!t6); - const double t16 = -p0_y + p1_y; + const double t14 = 1.0 / std::sqrt(t9); + const double t15 = t6 ? 0 : 1; + const double t16 = -t4; const double t17 = t6 ? t16 : t0; const double t18 = 1.0 / t9; const double t19 = t17 * t18; - const double t20 = t6 ? t4 : 0; - const double t21 = t10 * t20; - const double t22 = -int(t6); - const double t23 = -int(!t6); - const double t24 = 0.5 * t8; - const bool t25 = t1 + (t7 * t7) < (t16 * t16) + t3; - const double t26 = 2 * t2; - const double t27 = t18 * t26; - const double t28 = int(t6); - const double t29 = 0.5 * t13; - const double t30 = 0.5 * t10 * t17; - const double t31 = -p0_x + p1_x; - const double t32 = t6 ? 0 : (2 * t31); - const double t33 = t10 * t32; - const double t34 = 0.5 * t19; - const double t35 = t6 ? (2 * t16) : 0; - const double t36 = t10 * t35; - const double t37 = 2 * t7; - const double t38 = t18 * t37; - const double t39 = t4 * t8; - const double t40 = t0 * t13; - const double t41 = t39 - t40; - const double t42 = t25 ? 0 : t7; - const double t43 = t42 * t7; - const double t44 = t25 ? t16 : t0; - const double t45 = t31 * t44; - const double t46 = -t43 + t45; - const double t47 = t13 * t2; - const double t48 = t17 * t4; - const double t49 = t47 - t48; - const double t50 = t41 * t41 + t46 * t46 + t49 * t49; - const double t51 = 1.0 / std::sqrt(t50); - const double t52 = t15 * t4; - const double t53 = 1.0 / t50; - const double t54 = t25 ? t2 : 0; - const double t55 = -t16 * t42 + t31 * t54; - const double t56 = t54 * t55; - const double t57 = int(!t25); - const double t58 = t16 * t44 - t54 * t7; - const double t59 = t16 * t58; - const double t60 = t43 - t45; - const double t61 = t53 * (-t56 + t57 * t59 + t60 * (t0 * t57 + t44)); - const double t62 = -t39 + t40; - const double t63 = -t0 * t17 + t2 * t8; - const double t64 = -t47 + t48; - const double t65 = t62 * t62 + t63 * t63 + t64 * t64; - const double t66 = 1.0 / std::sqrt(t65); - const double t67 = t13 * t41; - const double t68 = t0 * t15 + t17; - const double t69 = 1.0 / t65; - const double t70 = t63 * t69; - const double t71 = t42 * t55; - const double t72 = -int(t25); - const double t73 = t0 * t60; - const double t74 = -t44; - const double t75 = t53 * (t58 * (t16 * t72 + t74) + t71 + t72 * t73); - const double t76 = t17 + t22 * t4; - const double t77 = t0 * t22; - const double t78 = t41 * t8; - const double t79 = int(t25); - const double t80 = -int(!t25); - const double t81 = t49 * t53; - const double t82 = t13 + t2 * t28; - const double t83 = t2 * t23; - const double t84 = t23 * t4; - const double t85 = t0 * t28; - const double t86 = t84 - t85; - const double t87 = t41 * t86 + t46 * (t8 + t83) + t49 * t82; - const double t88 = t62 * t69; - const double t89 = t56 + t59 * t80 + t60 * (t0 * t80 + t74); - const double t90 = t0 * t23 - t17; - const double t91 = t41 * t53; - const double t92 = t17 - t28 * t4; - const double t93 = -t46 * t85 + t49 * t92 - t78; - const double t94 = -t13 + t2 * t22; - const double t95 = t52 - t77; - const double t96 = -t15 * t2 + t8; - const double t97 = t41 * t95 - t46 * t96 + t49 * t94; + const double t20 = t0 * t13 - t4 * t8; + const double t21 = -t20; + const bool t22 = t1 + (t7 * t7) < (t16 * t16) + t3; + const double t23 = t22 ? 0 : t7; + const double t24 = -t0; + const double t25 = t22 ? t16 : t0; + const double t26 = -t23 * t7 + t24 * t25; + const double t27 = -t13 * t2 + t17 * t4; + const double t28 = -t27; + const double t29 = t21 * t21 + t26 * t26 + t28 * t28; + const double t30 = 1.0 / std::sqrt(t29); + const double t31 = t15 * t4; + const double t32 = 1.0 / t29; + const double t33 = t22 ? t2 : 0; + const double t34 = -t16 * t23 + t24 * t33; + const double t35 = t33 * t34; + const double t36 = t16 * t25 - t33 * t7; + const double t37 = t22 ? 0 : 1; + const double t38 = t16 * t37; + const double t39 = t24 * t37; + const double t40 = -t25; + const double t41 = -t26; + const double t42 = t32 * (-t35 + t36 * t38 + t41 * (-t39 - t40)); + const double t43 = 0.5 * (t6 ? (2 * t4) : 0); + const double t44 = t10 * t43; + const double t45 = t6 ? -1 : 0; + const double t46 = -t0 * t17 + t2 * t8; + const double t47 = t20 * t20 + t27 * t27 + t46 * t46; + const double t48 = 1.0 / std::sqrt(t47); + const double t49 = 1.0 / t47; + const double t50 = t0 * t45; + const double t51 = t17 + t4 * t45; + const double t52 = t22 ? (-1) : 0; + const double t53 = t24 * t52; + const double t54 = + t32 * (t23 * t34 + t36 * (t16 * t52 + t40) - t41 * t53); + const double t55 = -t8; + const double t56 = t6 ? 0 : -1; + const double t57 = 0.5 * t8; + const double t58 = 2 * t2; + const double t59 = t18 * ((t22 ? 0 : t58) + (t22 ? t58 : 0)); + const double t60 = t6 ? 1 : 0; + const double t61 = 0.5 * t13; + const double t62 = 0.5 * t10 * t17; + const double t63 = t22 ? 1 : 0; + const double t64 = t22 ? 0 : -1; + const double t65 = t16 * t64; + const double t66 = -t33 + t63 * t7; + const double t67 = t28 * t32; + const double t68 = t0 * t60; + const double t69 = t2 * t56 + t8; + const double t70 = t21 * (t4 * t56 - t68) + t26 * t69 - t28 * t66; + const double t71 = t4 * t56; + const double t72 = t6 ? 0 : (2 * t24); + const double t73 = t10 * t72; + const double t74 = 0.5 * t19; + const double t75 = t24 * t64 + t25; + const double t76 = t35 + t36 * t65 - t41 * t75; + const double t77 = t6 ? (2 * t16) : 0; + const double t78 = t10 * t77; + const double t79 = -t17 + t4 * t60; + const double t80 = t0 * t46 * t60 - t20 * t8 - t27 * t79; + const double t81 = t20 * t49; + const double t82 = 2 * t7; + const double t83 = t18 * ((t22 ? 0 : t82) + (t22 ? t82 : 0)); + const double t84 = t33 + t52 * t7; + const double t85 = t15 * t2; + const double t86 = + t21 * (t15 * t4 - t50) - t26 * (-t55 - t85) - t28 * t84; J[0] = -t12 * t8; J[1] = -t12 * t13; J[2] = t14 * (-t11 * t19 + t15); - J[3] = -t21 * t8; - J[4] = -t13 * t21; - J[5] = t14 * (-t19 * t20 + t22); - J[6] = t14 * (t23 - t24 * t27); - J[7] = t14 * (-t27 * t29 + t28); - J[8] = -t30 * t26; - J[9] = -t24 * t33; - J[10] = -t29 * t33; - J[11] = t14 * (t23 - t32 * t34); - J[12] = -t24 * t36; - J[13] = -t29 * t36; - J[14] = t14 * (t28 - t34 * t35); - J[15] = t14 * (t15 - t24 * t38); - J[16] = t14 * (t22 - t29 * t38); - J[17] = -t30 * t37; - J[18] = -t51 * (t49 * t61 + t52); - J[19] = t66 * (t68 - t70 * (t46 * t68 + t49 * t52 + t67)); - J[20] = -t51 * (t13 + t41 * t61); - J[21] = -t51 * (t49 * t75 + t76); - J[22] = t66 * (t70 * (-t46 * t77 - t49 * t76 + t78) + t77); - J[23] = t51 * (-t41 * t75 + t8); - J[24] = t51 - * (-t81 - * (t55 * (t31 * t79 + t4 * t80) + t58 * (t2 * t79 + t54) - + t60 * (-t42 + t7 * t80)) - + t82); - J[25] = t66 * (t70 * t87 - t8 - t83); - J[26] = t66 * (t86 + t87 * t88); - J[27] = -t51 * (t81 * t89 + t84); - J[28] = t66 * (t70 * (-t46 * t90 - t49 * t84 + t67) + t90); - J[29] = t51 * (t13 - t89 * t91); - J[30] = t66 * (t64 * t69 * t93 + t92); - J[31] = t66 * (t70 * t93 + t85); - J[32] = -t51 * (t8 + t91 * (t58 * (t16 * t79 + t44) - t71 + t73 * t79)); - J[33] = t51 - * (-t81 - * (t55 * (t31 * t72 + t4 * t57) + t58 * (t2 * t72 - t54) - + t60 * (t42 + t57 * t7)) - + t94); - J[34] = t66 * (t70 * t97 + t96); - J[35] = t66 * (t88 * t97 + t95); + J[3] = -t30 * (t28 * t42 + t31); + J[4] = t30 * (t25 + t26 * t42 - t39); + J[5] = -t30 * (t13 + t21 * t42); + J[6] = -t44 * t8; + J[7] = -t13 * t44; + J[8] = t14 * (-t19 * t43 + t45); + J[9] = t48 * (t27 * t49 * (t21 * t8 - t26 * t50 - t28 * t51) - t51); + J[10] = t30 * (t26 * t54 + t50); + J[11] = t30 * (-t21 * t54 - t55); + J[12] = t14 * (t56 - t57 * t59); + J[13] = t14 * (-t59 * t61 + t60); + J[14] = -t62 * ((t6 ? 0 : t58) + (t6 ? t58 : 0)); + J[15] = -t30 + * (t66 + + t67 + * (t34 * (t24 * t63 - t65) - t36 * t66 + + t41 * (-t23 + t64 * t7))); + J[16] = t48 * (t46 * t49 * t70 - t69); + J[17] = t48 * (t20 * t49 * t70 - t68 + t71); + J[18] = -t57 * t73; + J[19] = -t61 * t73; + J[20] = t14 * (t56 - t72 * t74); + J[21] = -t30 * (t67 * t76 + t71); + J[22] = t30 * (t26 * t32 * t76 - t75); + J[23] = t30 * (t13 - t21 * t32 * t76); + J[24] = -t57 * t78; + J[25] = -t61 * t78; + J[26] = t14 * (t60 - t74 * t77); + J[27] = -t48 * (t27 * t49 * t80 + t79); + J[28] = t30 * (-t26 * t32 * (t21 * t8 + t26 * t68 + t28 * t79) + t68); + J[29] = -t48 * (t8 + t80 * t81); + J[30] = t14 * (t15 - t57 * t83); + J[31] = t14 * (t45 - t61 * t83); + J[32] = -t62 * ((t6 ? 0 : t82) + (t6 ? t82 : 0)); + J[33] = -t30 + * (t67 * (t34 * (-t38 + t53) - t36 * t84 + t41 * (t23 + t37 * t7)) + + t84); + J[34] = t48 * (t46 * t49 * t86 + t8 - t85); + J[35] = t48 * (t31 - t50 + t81 * t86); } - // J is (12×1) flattened in column-major order + // J is (2×6) flattened in column-major order void point_edge_tangent_basis_2D_jacobian( double p_x, double p_y, @@ -387,25 +375,25 @@ namespace autogen { const double t4 = t1 + t3; const double t5 = 1.0 / std::sqrt(t4); const double t6 = 1.0 / t4; - const double t7 = t1 * t6; - const double t8 = t0 * t2 / pow_1_5(t4); - const double t9 = t3 * t6; + const double t7 = t1 * t6 - 1; + const double t8 = t0 * t2 / (t4 * std::sqrt(t4)); + const double t9 = t3 * t6 - 1; const double t10 = -t8; J[0] = 0; J[1] = 0; J[2] = 0; J[3] = 0; - J[4] = t5 * (t7 - 1); + J[4] = t5 * t7; J[5] = t8; J[6] = t8; - J[7] = t5 * (t9 - 1); - J[8] = t5 * (1 - t7); + J[7] = t5 * t9; + J[8] = -t5 * t7; J[9] = t10; J[10] = t10; - J[11] = t5 * (1 - t9); + J[11] = -t5 * t9; } - // J is (27×2) flattened in column-major order + // J is (6×9) flattened in column-major order void point_edge_tangent_basis_3D_jacobian( double p_x, double p_y, @@ -418,140 +406,140 @@ namespace autogen { double e1_z, double J[54]) { - const double t0 = -e1_x; - const double t1 = e0_x + t0; - const double t2 = t1 * t1; - const double t3 = -e1_y; - const double t4 = e0_y + t3; - const double t5 = t4 * t4; - const double t6 = -e1_z; - const double t7 = e0_z + t6; - const double t8 = t7 * t7; - const double t9 = t2 + t5 + t8; - const double t10 = 1.0 / std::sqrt(t9); - const double t11 = 1.0 / t9; - const double t12 = t11 * t2; - const double t13 = 1.0 / pow_1_5(t9); - const double t14 = t1 * t13; - const double t15 = t14 * t4; - const double t16 = t14 * t7; - const double t17 = t11 * t5; - const double t18 = t13 * t4 * t7; - const double t19 = t11 * t8; - const double t20 = -t15; - const double t21 = -t16; - const double t22 = -t18; - const double t23 = -p_y; - const double t24 = e0_y + t23; - const double t25 = -p_x; - const double t26 = e0_x + t25; - const double t27 = t1 * t24 - t26 * t4; - const double t28 = -e0_x; - const double t29 = e1_x + t28; - const double t30 = -e0_z; - const double t31 = p_z + t30; - const double t32 = t29 * t31; - const double t33 = p_x + t28; - const double t34 = e1_z + t30; - const double t35 = t33 * t34; - const double t36 = t32 - t35; - const double t37 = t27 * t4 + t36 * t7; - const double t38 = -p_z; - const double t39 = e0_z + t38; - const double t40 = -t24 * t7 + t39 * t4; - const double t41 = t1 * t39 - t26 * t7; - const double t42 = t27 * t27 + t40 * t40; - const double t43 = t41 * t41 + t42; - const double t44 = 1.0 / pow_1_5(t43); - const double t45 = t40 * t44; - const double t46 = t36 * t36 + t42; - const double t47 = 1 / std::sqrt(t46); - const double t48 = -e0_y; - const double t49 = p_y + t48; - const double t50 = e1_y + t48; - const double t51 = t29 * t49 - t33 * t50; - const double t52 = -t32 + t35; - const double t53 = 1.0 / t46; - const double t54 = t36 * t53; - const double t55 = 1.0 / std::sqrt(t43); - const double t56 = 1.0 / t43; - const double t57 = t27 * t56; - const double t58 = -t1 * t27 + t40 * t7; - const double t59 = t40 * t56; - const double t60 = t41 * t44; - const double t61 = t31 * t50 - t34 * t49; - const double t62 = t27 * t53; - const double t63 = t40 * t53; - const double t64 = t1 * t36 + t4 * t40; - const double t65 = t41 * t56; - const double t66 = t27 * t44; - const double t67 = e1_y + t23; - const double t68 = e1_z + t38; - const double t69 = t27 * t67 + t36 * t68; - const double t70 = p_z + t6; - const double t71 = e1_x + t25; - const double t72 = -t27 * t71 + t40 * t68; - const double t73 = t36 * t71 + t40 * t67; - const double t74 = t24 * t27 + t36 * t39; - const double t75 = t26 * t27 - t39 * t40; - const double t76 = t24 * t40 + t26 * t36; + const double t0 = -e1_y; + const double t1 = e0_y + t0; + const double t2 = -e1_x; + const double t3 = e0_x + t2; + const double t4 = -p_y; + const double t5 = e0_y + t4; + const double t6 = -p_x; + const double t7 = e0_x + t6; + const double t8 = -t1 * t7 + t3 * t5; + const double t9 = -e1_z; + const double t10 = e0_z + t9; + const double t11 = -t3; + const double t12 = -p_z; + const double t13 = e0_z + t12; + const double t14 = -t13; + const double t15 = -t7; + const double t16 = -t10; + const double t17 = t11 * t14 - t15 * t16; + const double t18 = t1 * t8 + t10 * t17; + const double t19 = t1 * t13 - t10 * t5; + const double t20 = -t10 * t7 + t13 * t3; + const double t21 = t19 * t19 + t8 * t8; + const double t22 = t20 * t20 + t21; + const double t23 = 1.0 / pow_1_5(t22); + const double t24 = t19 * t23; + const double t25 = t17 * t17 + t21; + const double t26 = 1.0 / std::sqrt(t25); + const double t27 = -t5; + const double t28 = -t1; + const double t29 = t11 * t27 - t15 * t28; + const double t30 = -t17; + const double t31 = 1.0 / t25; + const double t32 = t17 * t31; + const double t33 = -e0_z; + const double t34 = e1_z + t33; + const double t35 = 1.0 / std::sqrt(t22); + const double t36 = -e0_y; + const double t37 = 1.0 / t22; + const double t38 = t37 * t8; + const double t39 = t10 * t19 - t3 * t8; + const double t40 = t19 * t37; + const double t41 = t20 * t23; + const double t42 = t14 * t28 - t16 * t27; + const double t43 = t31 * t8; + const double t44 = t19 * t31; + const double t45 = -e0_x; + const double t46 = t1 * t19 + t17 * t3; + const double t47 = t20 * t37; + const double t48 = t23 * t8; + const double t49 = t3 * t3; + const double t50 = t1 * t1; + const double t51 = t10 * t10; + const double t52 = t49 + t50 + t51; + const double t53 = 1.0 / std::sqrt(t52); + const double t54 = 1.0 / t52; + const double t55 = t49 * t54 - 1; + const double t56 = 1.0 / pow_1_5(t52); + const double t57 = t3 * t56; + const double t58 = t1 * t57; + const double t59 = t10 * t57; + const double t60 = e1_y + t4; + const double t61 = e1_z + t12; + const double t62 = t17 * t61 + t60 * t8; + const double t63 = p_z + t9; + const double t64 = t50 * t54 - 1; + const double t65 = t1 * t10 * t56; + const double t66 = e1_x + t6; + const double t67 = t19 * t61 - t66 * t8; + const double t68 = t51 * t54 - 1; + const double t69 = t17 * t66 + t19 * t60; + const double t70 = -t58; + const double t71 = -t59; + const double t72 = t13 * t17 + t5 * t8; + const double t73 = -t65; + const double t74 = -t13 * t19 + t7 * t8; + const double t75 = p_x + t45; + const double t76 = t17 * t7 + t19 * t5; J[0] = 0; J[1] = 0; J[2] = 0; - J[3] = 0; - J[4] = 0; - J[5] = 0; + J[3] = -t18 * t24; + J[4] = t26 * (t32 * (t1 * t29 + t16 * t30) + t34); + J[5] = t35 * (-e1_y - t18 * t38 - t36); J[6] = 0; J[7] = 0; J[8] = 0; - J[9] = t10 * (t12 - 1); - J[10] = t15; - J[11] = t16; - J[12] = t15; - J[13] = t10 * (t17 - 1); - J[14] = t18; - J[15] = t16; - J[16] = t18; - J[17] = t10 * (t19 - 1); - J[18] = t10 * (1 - t12); - J[19] = t20; - J[20] = t21; - J[21] = t20; - J[22] = t10 * (1 - t17); - J[23] = t22; - J[24] = t21; - J[25] = t22; - J[26] = t10 * (1 - t19); - J[27] = -t37 * t45; - J[28] = t47 * (t34 + t54 * (t34 * t52 + t4 * t51)); - J[29] = t55 * (-t37 * t57 + t4); - J[30] = t55 * (-t58 * t59 + t7); - J[31] = t58 * t60; - J[32] = -t47 * (t1 + t62 * (t29 * t51 + t61 * t7)); - J[33] = -t47 * (t4 + t63 * (t1 * t52 + t50 * t61)); - J[34] = t55 * (t1 - t64 * t65); - J[35] = t64 * t66; - J[36] = -t45 * t69; - J[37] = t47 * (t54 * (t51 * t67 + t52 * t70) + t70); - J[38] = t55 * (-t57 * t69 + t67); - J[39] = t55 * (-t59 * t72 + t68); - J[40] = t60 * t72; - J[41] = -t47 * (t62 * (t51 * (p_x + t0) + t61 * t68) + t71); - J[42] = -t47 * (t63 * (t52 * t71 + t61 * (p_y + t3)) + t67); - J[43] = t55 * (-t65 * t73 + t71); - J[44] = t66 * t73; - J[45] = t45 * t74; - J[46] = t55 * (t39 - t65 * t74); - J[47] = -t47 * (t24 + t62 * (t39 * t52 + t49 * t51)); - J[48] = -t47 * (t39 + t63 * (t26 * t51 + t31 * t61)); - J[49] = t60 * t75; - J[50] = t55 * (t26 - t57 * t75); - J[51] = t55 * (t24 - t59 * t76); - J[52] = t47 * (t33 + t54 * (t24 * t61 + t33 * t52)); - J[53] = -t66 * t76; + J[9] = t35 * (-t34 - t39 * t40); + J[10] = t39 * t41; + J[11] = -t26 * (t3 + t43 * (t10 * t42 + t11 * t29)); + J[12] = 0; + J[13] = 0; + J[14] = 0; + J[15] = -t26 * (t1 + t44 * (t28 * t42 + t3 * t30)); + J[16] = t35 * (-e1_x - t45 - t46 * t47); + J[17] = t46 * t48; + J[18] = t53 * t55; + J[19] = t58; + J[20] = t59; + J[21] = -t24 * t62; + J[22] = t26 * (t32 * (t29 * t60 - t30 * t61) + t63); + J[23] = t35 * (-p_y - t0 - t38 * t62); + J[24] = t58; + J[25] = t53 * t64; + J[26] = t65; + J[27] = t35 * (-t40 * t67 - t63); + J[28] = t41 * t67; + J[29] = -t26 * (t43 * (-t29 * t66 + t42 * t61) + t66); + J[30] = t59; + J[31] = t65; + J[32] = t53 * t68; + J[33] = -t26 * (t44 * (t30 * t66 - t42 * t60) + t60); + J[34] = t35 * (-p_x - t2 - t47 * t69); + J[35] = t48 * t69; + J[36] = -t53 * t55; + J[37] = t70; + J[38] = t71; + J[39] = t24 * t72; + J[40] = t35 * (-p_z - t33 - t47 * t72); + J[41] = -t26 * (t43 * (t13 * t30 + t27 * t29) + t5); + J[42] = t70; + J[43] = -t53 * t64; + J[44] = t73; + J[45] = -t26 * (t13 + t44 * (t14 * t42 + t29 * t7)); + J[46] = t41 * t74; + J[47] = t35 * (-t38 * t74 - t75); + J[48] = t71; + J[49] = t73; + J[50] = -t53 * t68; + J[51] = t35 * (-p_y - t36 - t40 * t76); + J[52] = t26 * (t32 * (t15 * t30 + t42 * t5) + t75); + J[53] = -t48 * t76; } - // J is (36×2) flattened in column-major order + // J is (6×12) flattened in column-major order void edge_edge_tangent_basis_jacobian( double ea0_x, double ea0_y, @@ -567,226 +555,217 @@ namespace autogen { double eb1_z, double J[72]) { - const double t0 = ea0_x - ea1_x; + const double t0 = ea0_y - ea1_y; const double t1 = t0 * t0; - const double t2 = ea0_y - ea1_y; + const double t2 = ea0_x - ea1_x; const double t3 = t2 * t2; const double t4 = ea0_z - ea1_z; const double t5 = t4 * t4; const double t6 = t3 + t5; const double t7 = t1 + t6; - const double t8 = 1 / std::sqrt(t7); + const double t8 = 1.0 / std::sqrt(t7); const double t9 = 1.0 / t7; - const double t10 = t1 * t9; + const double t10 = t3 * t9 - 1; const double t11 = 1.0 / pow_1_5(t7); const double t12 = t0 * t2; const double t13 = t11 * t12; - const double t14 = t0 * t4; + const double t14 = t2 * t4; const double t15 = t11 * t14; - const double t16 = t3 * t9; - const double t17 = t2 * t4; - const double t18 = t11 * t17; - const double t19 = t5 * t9; - const double t20 = -t13; - const double t21 = -t15; - const double t22 = -t18; - const double t23 = -ea0_x + ea1_x; - const double t24 = -eb0_z + eb1_z; - const double t25 = t23 * t24; - const double t26 = -ea0_z + ea1_z; - const double t27 = -eb0_x + eb1_x; - const double t28 = t26 * t27; - const double t29 = t25 - t28; - const double t30 = eb0_z - eb1_z; - const double t31 = t2 * t30; - const double t32 = eb0_y - eb1_y; - const double t33 = t32 * t4; - const double t34 = -t33; - const double t35 = t31 + t34; - const double t36 = t0 * t29 + t2 * t35; - const double t37 = t0 * t32; - const double t38 = eb0_x - eb1_x; - const double t39 = t2 * t38; + const double t16 = -t2; + const double t17 = eb0_z - eb1_z; + const double t18 = -t17; + const double t19 = t16 * t18; + const double t20 = -t4; + const double t21 = eb0_x - eb1_x; + const double t22 = -t21; + const double t23 = t20 * t22; + const double t24 = -t23; + const double t25 = t19 + t24; + const double t26 = -t25; + const double t27 = -t0; + const double t28 = t18 * t27; + const double t29 = eb0_y - eb1_y; + const double t30 = -t29; + const double t31 = t20 * t30; + const double t32 = t28 - t31; + const double t33 = t16 * t26 - t27 * t32; + const double t34 = t2 * t29; + const double t35 = t0 * t21; + const double t36 = -t35; + const double t37 = t34 + t36; + const double t38 = t0 * t17; + const double t39 = t29 * t4; const double t40 = -t39; - const double t41 = t37 + t40; - const double t42 = t2 * t41 + t29 * t4; - const double t43 = t0 * t41; - const double t44 = t35 * t4; - const double t45 = t43 - t44; - const double t46 = t36 * t36 + t42 * t42 + t45 * t45; - const double t47 = 1 / std::sqrt(t46); - const double t48 = 1.0 / t46; - const double t49 = 2 * t37; - const double t50 = t39 - t49; - const double t51 = -t43 + t44; - const double t52 = t2 * t32; - const double t53 = t30 * t4; - const double t54 = t52 + t53; - const double t55 = -ea0_y + ea1_y; - const double t56 = -eb0_y + eb1_y; - const double t57 = t23 * t56; - const double t58 = t27 * t55; - const double t59 = t57 - t58; - const double t60 = t55 * t59; - const double t61 = -t25; - const double t62 = t28 + t61; - const double t63 = t26 * t62; - const double t64 = t60 - t63; - const double t65 = t54 * t64; - const double t66 = t38 * t4; - const double t67 = t0 * t30; - const double t68 = 2 * t67; - const double t69 = t66 - t68; - const double t70 = t23 * t62; - const double t71 = t24 * t55; - const double t72 = t26 * t56; - const double t73 = -t72; - const double t74 = t71 + t73; - const double t75 = t55 * t74; - const double t76 = t70 - t75; - const double t77 = t48 * (t50 * t51 - t65 - t69 * t76); - const double t78 = t0 * t38; - const double t79 = t53 + t78; - const double t80 = t51 * t79; - const double t81 = 2 * t31; - const double t82 = t33 - t81; - const double t83 = 2 * t39; - const double t84 = t37 - t83; - const double t85 = t48 * (-t64 * t84 - t76 * t82 + t80); - const double t86 = t52 + t78; - const double t87 = t76 * t86; - const double t88 = 2 * t33; - const double t89 = t31 - t88; - const double t90 = -2 * t66 + t67; - const double t91 = t48 * (t51 * t89 - t64 * t90 - t87); - const double t92 = t51 * t51 + t64 * t64 + t76 * t76; - const double t93 = 1.0 / std::sqrt(t92); - const double t94 = -t70 + t75; - const double t95 = t23 * t59 - t26 * t74; - const double t96 = -t60 + t63; - const double t97 = 1.0 / t92; - const double t98 = t51 * t97; - const double t99 = t40 + t49; - const double t100 = -t66 + t68; - const double t101 = t48 * (-t100 * t76 + t51 * t99 + t65); - const double t102 = t34 + t81; - const double t103 = -t37 + t83; - const double t104 = t48 * (t102 * t76 + t103 * t64 + t80); - const double t105 = 2 * t28; - const double t106 = t105 + t61; - const double t107 = - t106 * t96 + t94 * (t0 * t27 + t32 * t55) + t95 * (t33 - t71 + t72); - const double t108 = t64 * t97; - const double t109 = -t31 + t88; - const double t110 = t36 * t48; - const double t111 = t12 * t51; - const double t112 = t14 * t76; - const double t113 = t6 * t64; - const double t114 = t111 - t112 + t113; - const double t115 = t114 * t48; - const double t116 = t1 + t5; - const double t117 = t116 * t51 + t12 * t64 + t17 * t76; - const double t118 = t42 * t48; - const double t119 = t26 * t94; - const double t120 = t17 * t51; - const double t121 = t14 * t64; - const double t122 = t1 + t3; - const double t123 = t122 * t76; - const double t124 = t120 - t121 + t123; - const double t125 = t2 * t23; - const double t126 = t0 * t119 + t125 * t95 + t96 * (t26 * t26 + t3); - const double t127 = -t14; - const double t128 = t76 * t97; - const double t129 = t4 * t55; - const double t130 = t125 * t96 + t129 * t94 + t95 * (t23 * t23 + t5); - const double t131 = - t0 * t26 * t96 + t129 * t95 + t94 * (t1 + t55 * t55); - J[0] = t8 * (t10 - 1); + const double t41 = t38 + t40; + const double t42 = t2 * t37 - t4 * t41; + const double t43 = -t42; + const double t44 = t16 * t30; + const double t45 = t22 * t27; + const double t46 = -t45; + const double t47 = t44 + t46; + const double t48 = -t20 * t26 + t27 * t47; + const double t49 = t33 * t33 + t43 * t43 + t48 * t48; + const double t50 = 1.0 / std::sqrt(t49); + const double t51 = t27 * t29; + const double t52 = -t51; + const double t53 = 1.0 / t49; + const double t54 = 2 * t19; + const double t55 = t24 + t54; + const double t56 = -t33; + const double t57 = -t18 * t20 + t51; + const double t58 = -t48; + const double t59 = t16 * t47 - t20 * t32; + const double t60 = + t53 * (-t55 * t56 - t57 * t58 + t59 * (t16 * t29 - t44 + t45)); + const double t61 = t0 * t41 + t2 * t25; + const double t62 = t0 * t37 + t25 * t4; + const double t63 = t42 * t42 + t61 * t61 + t62 * t62; + const double t64 = 1.0 / std::sqrt(t63); + const double t65 = 2 * t34; + const double t66 = t36 + t65; + const double t67 = 1.0 / t63; + const double t68 = t42 * t67; + const double t69 = t1 * t9 - 1; + const double t70 = t0 * t4; + const double t71 = t11 * t70; + const double t72 = 2 * t35; + const double t73 = 2 * t38; + const double t74 = t40 + t73; + const double t75 = t17 * t4 + t2 * t21; + const double t76 = t43 * t75; + const double t77 = t34 - t72; + const double t78 = t33 * t74 - t48 * t77 + t76; + const double t79 = t67 * t78; + const double t80 = t5 * t9 - 1; + const double t81 = t21 * t4; + const double t82 = 2 * t81; + const double t83 = t16 * t21; + const double t84 = t27 * t30; + const double t85 = -t84; + const double t86 = t83 + t85; + const double t87 = 2 * t39; + const double t88 = t17 * t2; + const double t89 = t62 * t67; + const double t90 = t53 + * (-t56 * t86 + t58 * (t20 * t21 + t25) + t59 * (t28 - 2 * t31)); + const double t91 = -t13; + const double t92 = -t15; + const double t93 = -t17 * t20 + t84; + const double t94 = -t19; + const double t95 = t16 * t17 + t23 + t94; + const double t96 = t48 * t53; + const double t97 = t33 * t95 + t43 * t66 + t48 * t93; + const double t98 = t61 * t67; + const double t99 = -t71; + const double t100 = -t33 * t74 + t48 * (t21 * t27 + t47) - t76; + const double t101 = t16 * t22; + const double t102 = t20 * t29 + t32; + const double t103 = + -t102 * t59 + t56 * (-t101 - t52) + t58 * (-t19 + 2 * t20 * t22); + const double t104 = t103 * t53; + const double t105 = t27 * t27; + const double t106 = -t20 * t4; + const double t107 = t105 + t106; + const double t108 = t107 * t48 + t12 * t43 - t14 * t33; + const double t109 = t16 * t59; + const double t110 = t16 * t56; + const double t111 = t33 * t53; + const double t112 = t27 * t56; + const double t113 = t27 * t58; + const double t114 = t20 * t20; + const double t115 = -t114; + const double t116 = t16 * t2; + const double t117 = t112 * t20 - t113 * t2 + t59 * (t115 + t116); + const double t118 = t43 * t53; + const double t119 = t16 * t16; + const double t120 = t0 * t27; + const double t121 = -t120; + const double t122 = t119 + t121; + const double t123 = t122 * t33 - t14 * t48 + t43 * t70; + const double t124 = t20 * t58; + const double t125 = t20 * t59; + const double t126 = t0 * t109 - t110 * t20 + t58 * (-t115 - t120); + const double t127 = t112 * t4 - t113 * t16 + t59 * (t106 + t119); + const double t128 = t124 * t2 - t125 * t27 + t56 * (t105 - t116); + J[0] = t10 * t8; J[1] = t13; J[2] = t15; - J[3] = t13; - J[4] = t8 * (t16 - 1); - J[5] = t18; - J[6] = t15; - J[7] = t18; - J[8] = t8 * (t19 - 1); - J[9] = t8 * (1 - t10); - J[10] = t20; - J[11] = t21; - J[12] = t20; - J[13] = t8 * (1 - t16); - J[14] = t22; - J[15] = t21; - J[16] = t22; - J[17] = t8 * (1 - t19); - J[18] = 0; - J[19] = 0; - J[20] = 0; - J[21] = 0; - J[22] = 0; - J[23] = 0; - J[24] = 0; - J[25] = 0; - J[26] = 0; - J[27] = 0; - J[28] = 0; - J[29] = 0; - J[30] = 0; - J[31] = 0; - J[32] = 0; - J[33] = 0; - J[34] = 0; - J[35] = 0; - J[36] = t47 * (-t42 * t77 + t54); - J[37] = t47 * (t45 * t77 + t50); - J[38] = t47 * (t36 * t77 + t69); - J[39] = t47 * (-t42 * t85 + t84); - J[40] = t47 * (t45 * t85 + t79); - J[41] = t47 * (t36 * t85 + t82); - J[42] = t47 * (-t42 * t91 + t90); - J[43] = t93 - * (t89 - - t98 - * (t94 * (t55 * t56 + t78) + t95 * (t4 * t56 + t74) - + t96 * (t26 * t38 + t29))); - J[44] = t47 * (t36 * t91 + t86); - J[45] = -t47 * (t101 * t42 + t54); - J[46] = t47 * (t101 * t45 + t99); - J[47] = t47 * (t100 + t101 * t36); - J[48] = t47 * (t103 + t104 * t42); - J[49] = -t93 - * (t79 - + t98 - * (t94 * (2 * t71 + t73) + t95 * (t23 * t38 + t24 * t4) - + t96 * (t39 - t57 + t58))); - J[50] = t47 * (t102 - t104 * t36); - J[51] = t93 * (t106 + t107 * t108); - J[52] = t93 * (-t107 * t98 + t109); - J[53] = - t47 * (t110 * (t109 * t51 + t64 * (-t105 + t25) + t87) - t52 - t78); - J[54] = -t47 * (t115 * t42 + t6); - J[55] = t47 * (t115 * t45 + t12); - J[56] = t47 * (t110 * t114 + t14); - J[57] = t47 * (t117 * t118 + t12); - J[58] = -t93 - * (t116 - + t98 * (t119 * t55 + t12 * t96 + t95 * (t0 * t23 + t26 * t4))); - J[59] = t47 * (-t110 * t117 + t17); - J[60] = t47 * (-t118 * t124 + t14); - J[61] = t47 * (t124 * t45 * t48 + t17); - J[62] = t47 * (-t1 + t110 * t124 - t3); - J[63] = t47 * (-t118 * (-t111 + t112 - t113) + t6); - J[64] = -t93 * (t12 + t126 * t98); - J[65] = t93 * (t126 * t128 + t127); - J[66] = t93 * (t108 * t130 - t12); - J[67] = t93 * (t116 - t130 * t98); - J[68] = t93 * (t128 * t130 - t17); - J[69] = t93 * (t108 * t131 + t127); - J[70] = -t93 * (t131 * t98 + t17); - J[71] = t47 * (t110 * (-t120 + t121 - t123) + t122); + J[3] = t50 * (t18 * t20 + t48 * t60 + t52); + J[4] = t64 * (t35 - t65 + t68 * (t33 * t55 - t43 * t66 + t48 * t57)); + J[5] = t50 * (t23 + t33 * t60 - t54); + J[6] = t13; + J[7] = t69 * t8; + J[8] = t71; + J[9] = t64 * (t2 * t29 - t62 * t79 - t72); + J[10] = t64 * (t68 * t78 + t75); + J[11] = t64 * (t39 + t61 * t79 - t73); + J[12] = t15; + J[13] = t71; + J[14] = t8 * t80; + J[15] = t64 + * (t17 * t2 - t82 + - t89 * (t33 * t86 + t43 * (t38 - t87) - t48 * (-t82 + t88))); + J[16] = t50 * (t0 * t17 - t43 * t90 - t87); + J[17] = t50 * (t33 * t90 - t83 + t84); + J[18] = -t10 * t8; + J[19] = t91; + J[20] = t92; + J[21] = t50 + * (t17 * t20 + t85 + + t96 * (-t56 * t95 - t58 * t93 + t59 * (2 * t44 + t46))); + J[22] = t64 * (t66 + t68 * t97); + J[23] = t64 * (-t81 + 2 * t88 + t97 * t98); + J[24] = t91; + J[25] = -t69 * t8; + J[26] = t99; + J[27] = -t64 * (t100 * t89 + t77); + J[28] = t64 * (t100 * t42 * t67 - t75); + J[29] = t64 * (t100 * t98 + t74); + J[30] = t92; + J[31] = t99; + J[32] = -t8 * t80; + J[33] = t50 * (t103 * t96 + 2 * t23 + t94); + J[34] = -t50 * (t102 + t104 * t43); + J[35] = t50 * (-t101 + t104 * t33 + t51); + J[36] = 0; + J[37] = 0; + J[38] = 0; + J[39] = -t64 * (t1 + t108 * t89 + t5); + J[40] = t64 * (t108 * t68 + t12); + J[41] = t50 * (t111 * (-t107 * t58 + t109 * t27 - t110 * t4) + t14); + J[42] = 0; + J[43] = 0; + J[44] = 0; + J[45] = t50 * (t117 * t96 + t12); + J[46] = -t50 * (t117 * t118 + t6); + J[47] = t64 * (t70 - t98 * (t12 * t48 + t33 * t70 + t43 * t6)); + J[48] = 0; + J[49] = 0; + J[50] = 0; + J[51] = t64 * (-t123 * t89 + t14); + J[52] = t50 * (-t118 * (-t0 * t125 - t122 * t56 + t124 * t16) + t70); + J[53] = t64 * (-t1 + t123 * t61 * t67 - t3); + J[54] = 0; + J[55] = 0; + J[56] = 0; + J[57] = t50 * (t114 + t121 + t126 * t96); + J[58] = -t50 * (t118 * t126 + t12); + J[59] = t50 * (t126 * t33 * t53 - t14); + J[60] = 0; + J[61] = 0; + J[62] = 0; + J[63] = t50 * (-t12 + t127 * t48 * t53); + J[64] = t50 * (-t118 * t127 + t6); + J[65] = t50 * (t127 * t33 * t53 - t70); + J[66] = 0; + J[67] = 0; + J[68] = 0; + J[69] = t50 * (t128 * t48 * t53 - t14); + J[70] = -t50 * (t118 * t128 + t70); + J[71] = t50 * (t105 + t111 * t128 - t116); } - // J is (36×2) flattened in column-major order + // J is (6×12) flattened in column-major order void point_triangle_tangent_basis_jacobian( double p_x, double p_y, @@ -802,144 +781,128 @@ namespace autogen { double t2_z, double J[72]) { - const double t0 = t0_x - t1_x; + const double t0 = t0_y - t1_y; const double t1 = t0 * t0; - const double t2 = -t1_y; - const double t3 = t0_y + t2; - const double t4 = t3 * t3; - const double t5 = -t1_z; - const double t6 = t0_z + t5; - const double t7 = t6 * t6; - const double t8 = t4 + t7; - const double t9 = t1 + t8; - const double t10 = 1.0 / std::sqrt(t9); - const double t11 = 1.0 / t9; - const double t12 = t1 * t11; - const double t13 = 1.0 / pow_1_5(t9); - const double t14 = t0 * t3; - const double t15 = t13 * t14; - const double t16 = t0 * t6; - const double t17 = t13 * t16; - const double t18 = t11 * t4; - const double t19 = t3 * t6; - const double t20 = t13 * t19; - const double t21 = t11 * t7; - const double t22 = -t15; - const double t23 = -t17; - const double t24 = -t20; - const double t25 = -t0_x; - const double t26 = t1_x + t25; - const double t27 = -t0_z; - const double t28 = t27 + t2_z; - const double t29 = t26 * t28; - const double t30 = t25 + t2_x; - const double t31 = t1_z + t27; - const double t32 = t30 * t31; - const double t33 = t29 - t32; - const double t34 = -t2_z; - const double t35 = t0_z + t34; - const double t36 = t3 * t35; - const double t37 = -t2_y; - const double t38 = t0_y + t37; - const double t39 = t38 * t6; - const double t40 = -t39; - const double t41 = t36 + t40; - const double t42 = t0 * t33 + t3 * t41; - const double t43 = t0 * t38; - const double t44 = -t2_x; - const double t45 = t0_x + t44; - const double t46 = t3 * t45; + const double t2 = t0_x - t1_x; + const double t3 = t2 * t2; + const double t4 = t0_z - t1_z; + const double t5 = t4 * t4; + const double t6 = t3 + t5; + const double t7 = t1 + t6; + const double t8 = (1.0 / std::sqrt(t7)); + const double t9 = 1.0 / t7; + const double t10 = t3 * t9 - 1; + const double t11 = 1.0 / pow_1_5(t7); + const double t12 = t0 * t2; + const double t13 = t11 * t12; + const double t14 = t2 * t4; + const double t15 = t11 * t14; + const double t16 = -t2; + const double t17 = -t2_z; + const double t18 = t0_z + t17; + const double t19 = -t18; + const double t20 = t16 * t19; + const double t21 = -t2_x; + const double t22 = t0_x + t21; + const double t23 = -t22; + const double t24 = -t4; + const double t25 = t23 * t24; + const double t26 = t20 - t25; + const double t27 = -t26; + const double t28 = -t0; + const double t29 = t19 * t28; + const double t30 = -t2_y; + const double t31 = t0_y + t30; + const double t32 = -t31; + const double t33 = t24 * t32; + const double t34 = t29 - t33; + const double t35 = t16 * t27 - t28 * t34; + const double t36 = t2 * t31; + const double t37 = t0 * t22; + const double t38 = -t37; + const double t39 = t36 + t38; + const double t40 = t0 * t18; + const double t41 = -t31 * t4; + const double t42 = t40 + t41; + const double t43 = t2 * t39 - t4 * t42; + const double t44 = -t43; + const double t45 = t16 * t32; + const double t46 = t23 * t28; const double t47 = -t46; - const double t48 = t43 + t47; - const double t49 = t3 * t48 + t33 * t6; - const double t50 = t0 * t48; - const double t51 = t41 * t6; - const double t52 = t50 - t51; - const double t53 = t42 * t42 + t49 * t49 + t52 * t52; - const double t54 = 1.0 / std::sqrt(t53); - const double t55 = 1.0 / t53; - const double t56 = t1_y + t37; - const double t57 = t3 * t56; - const double t58 = t1_z + t34; - const double t59 = t58 * t6; - const double t60 = t57 + t59; - const double t61 = -t0_y; - const double t62 = t1_y + t61; - const double t63 = t2_y + t61; - const double t64 = t26 * t63; - const double t65 = t30 * t62; - const double t66 = t64 - t65; - const double t67 = t62 * t66; - const double t68 = -t29; - const double t69 = t32 + t68; - const double t70 = t31 * t69; - const double t71 = t67 - t70; - const double t72 = -t43; - const double t73 = t46 + t72; - const double t74 = -t0 * t56 + t73; - const double t75 = -t50 + t51; - const double t76 = t45 * t6; - const double t77 = t0 * t35; - const double t78 = t76 - t77; - const double t79 = -t0 * t58 + t78; - const double t80 = t26 * t69; - const double t81 = t28 * t62; - const double t82 = t31 * t63; - const double t83 = -t82; - const double t84 = t81 + t83; - const double t85 = t62 * t84; - const double t86 = t80 - t85; - const double t87 = t55 * (-t60 * t71 + t74 * t75 - t79 * t86); - const double t88 = t71 * t71 + t75 * t75 + t86 * t86; - const double t89 = 1.0 / std::sqrt(t88); - const double t90 = t2_z + t5; - const double t91 = -t67 + t70; - const double t92 = -t64 + t65; - const double t93 = t26 * t66 - t31 * t84; - const double t94 = -t80 + t85; - const double t95 = 1.0 / t88; - const double t96 = t75 * t95; - const double t97 = t1_x + t44; - const double t98 = t3 * t97; - const double t99 = t0 * t97; - const double t100 = t59 + t99; - const double t101 = -t36; - const double t102 = t101 - t3 * t58 + t39; - const double t103 = t55 * (t100 * t75 - t102 * t86 - t71 * (t48 - t98)); - const double t104 = t6 * t97; - const double t105 = t57 + t99; - const double t106 = t41 - t56 * t6; - const double t107 = -t76; - const double t108 = - t55 * (-t105 * t86 + t106 * t75 - t71 * (-t104 + t107 + t77)); - const double t109 = t2 + t2_y; - const double t110 = t3 * t38; - const double t111 = t35 * t6; - const double t112 = t110 + t111; - const double t113 = 2 * t43 + t47; - const double t114 = t107 + 2 * t77; - const double t115 = t55 * (t112 * t71 + t113 * t75 - t114 * t86); - const double t116 = t0 * t45; - const double t117 = t111 + t116; - const double t118 = 2 * t36 + t40; - const double t119 = 2 * t46 + t72; - const double t120 = t55 * (t117 * t75 + t118 * t86 + t119 * t71); - const double t121 = 2 * t32; - const double t122 = t121 + t68; - const double t123 = - t122 * t91 + t93 * (t39 - t81 + t82) + t94 * (t0 * t30 + t38 * t62); - const double t124 = t71 * t95; - const double t125 = t101 + 2 * t39; - const double t126 = t42 * t55; - const double t127 = t26 * t3; - const double t128 = t0 * t31; - const double t129 = t127 * t93 + t128 * t94 + t91 * (t31 * t31 + t4); - const double t130 = -t16; - const double t131 = t86 * t95; - const double t132 = t6 * t62; - const double t133 = t127 * t91 + t132 * t94 + t93 * (t26 * t26 + t7); - const double t134 = t128 * t91 + t132 * t93 + t94 * (t1 + t62 * t62); - const double t135 = t1 + t4; + const double t48 = t45 + t47; + const double t49 = -t24 * t27 + t28 * t48; + const double t50 = t35 * t35 + t44 * t44 + t49 * t49; + const double t51 = 1.0 / std::sqrt(t50); + const double t52 = t17 + t1_z; + const double t53 = -t52; + const double t54 = t1_y + t30; + const double t55 = t28 * t54; + const double t56 = 1.0 / t50; + const double t57 = -t24 * t53 + t55; + const double t58 = -t49; + const double t59 = -t45 + t46; + const double t60 = t16 * t48 - t24 * t34; + const double t61 = t16 * t53 + t26; + const double t62 = -t35; + const double t63 = + t56 * (-t57 * t58 + t60 * (t16 * t54 + t59) - t61 * t62); + const double t64 = t2 * t54 + t39; + const double t65 = t0 * t42 + t2 * t26; + const double t66 = t0 * t39 + t26 * t4; + const double t67 = t43 * t43 + t65 * t65 + t66 * t66; + const double t68 = 1.0 / std::sqrt(t67); + const double t69 = t18 * t2; + const double t70 = t22 * t4; + const double t71 = -t70; + const double t72 = 1.0 / t67; + const double t73 = t1 * t9 - 1; + const double t74 = t0 * t4; + const double t75 = t11 * t74; + const double t76 = t1_x + t21; + const double t77 = t2 * t76 + t4 * t52; + const double t78 = t0 * t52 + t42; + const double t79 = t35 * t78 + t44 * t77 + t49 * (-t28 * t76 + t59); + const double t80 = t72 * t79; + const double t81 = t5 * t9 - 1; + const double t82 = t16 * t76; + const double t83 = -t54; + const double t84 = t28 * t83; + const double t85 = t82 - t84; + const double t86 = t24 * t83 - t29 + t33; + const double t87 = t4 * t76 - t69 + t70; + const double t88 = t66 * t72; + const double t89 = + t56 * (t58 * (t24 * t76 + t26) - t60 * t86 - t62 * t85); + const double t90 = -t13; + const double t91 = -t15; + const double t92 = t28 * t32; + const double t93 = -t18 * t24 + t92; + const double t94 = -t20; + const double t95 = t16 * t18 + t25 + t94; + const double t96 = t49 * t56; + const double t97 = 2 * t36 + t38; + const double t98 = t35 * t95 + t44 * t97 + t49 * t93; + const double t99 = t65 * t72; + const double t100 = -t75; + const double t101 = t18 * t4 + t2 * t22; + const double t102 = 2 * t40 + t41; + const double t103 = -t101 * t44 - t102 * t35 + t49 * (t22 * t28 + t48); + const double t104 = t16 * t23; + const double t105 = t24 * t31 + t34; + const double t106 = -t105 * t60 + t58 * (-t20 + 2 * t23 * t24) + + t62 * (-t104 + t28 * t31); + const double t107 = t106 * t56; + const double t108 = t24 * t24; + const double t109 = t0 * t28; + const double t110 = + t0 * t16 * t60 - t16 * t24 * t62 + t58 * (t108 - t109); + const double t111 = + -t16 * t28 * t58 + t28 * t4 * t62 + t60 * (t16 * t16 - t24 * t4); + const double t112 = t28 * t28; + const double t113 = t16 * t2; + const double t114 = + t2 * t24 * t58 - t24 * t28 * t60 + t62 * (t112 - t113); + const double t115 = t114 * t56; J[0] = 0; J[1] = 0; J[2] = 0; @@ -949,85 +912,73 @@ namespace autogen { J[6] = 0; J[7] = 0; J[8] = 0; - J[9] = t10 * (t12 - 1); - J[10] = t15; - J[11] = t17; - J[12] = t15; - J[13] = t10 * (t18 - 1); - J[14] = t20; - J[15] = t17; - J[16] = t20; - J[17] = t10 * (t21 - 1); - J[18] = t10 * (1 - t12); - J[19] = t22; - J[20] = t23; - J[21] = t22; - J[22] = t10 * (1 - t18); - J[23] = t24; - J[24] = t23; - J[25] = t24; - J[26] = t10 * (1 - t21); - J[27] = 0; - J[28] = 0; - J[29] = 0; - J[30] = 0; - J[31] = 0; - J[32] = 0; - J[33] = 0; - J[34] = 0; - J[35] = 0; - J[36] = 0; - J[37] = 0; - J[38] = 0; - J[39] = 0; - J[40] = 0; - J[41] = 0; - J[42] = 0; - J[43] = 0; - J[44] = 0; - J[45] = t54 * (-t49 * t87 + t60); - J[46] = t89 - * (t74 - - t96 - * (t91 * (t31 * t90 + t57) + t93 * (t26 * t56 + t92) - + t94 * (t0 * t90 + t69))); - J[47] = t54 * (t42 * t87 + t79); - J[48] = -t54 * (t103 * t49 + t73 + t98); - J[49] = t54 * (t100 + t103 * t52); - J[50] = t54 * (t102 + t103 * t42); - J[51] = -t54 * (t104 + t108 * t49 + t78); - J[52] = t89 - * (t106 - - t96 - * (t91 * (t31 * t97 + t33) + t93 * (t109 * t6 + t84) - + t94 * (t109 * t62 + t99))); - J[53] = t54 * (t105 + t108 * t42); - J[54] = -t54 * (t112 + t115 * t49); - J[55] = t54 * (t113 + t115 * t52); - J[56] = t54 * (t114 + t115 * t42); - J[57] = t54 * (t119 + t120 * t49); - J[58] = -t89 - * (t117 - + t96 - * (t91 * (t46 + t92) + t93 * (t26 * t45 + t28 * t6) - + t94 * (2 * t81 + t83))); - J[59] = t54 * (t118 - t120 * t42); - J[60] = t89 * (t122 + t123 * t124); - J[61] = t89 * (-t123 * t96 + t125); - J[62] = t54 - * (-t110 - t116 - + t126 - * (t125 * t75 + t71 * (-t121 + t29) + t86 * (t110 + t116))); - J[63] = t54 * (-t49 * t55 * (-t14 * t75 + t16 * t86 - t71 * t8) + t8); - J[64] = -t89 * (t129 * t96 + t14); - J[65] = t89 * (t129 * t131 + t130); - J[66] = t89 * (t124 * t133 - t14); - J[67] = t89 * (t1 - t133 * t96 + t7); - J[68] = t89 * (t131 * t133 - t19); - J[69] = t89 * (t124 * t134 + t130); - J[70] = -t89 * (t134 * t96 + t19); - J[71] = t54 * (t126 * (-t135 * t86 + t16 * t71 - t19 * t75) + t135); + J[9] = 0; + J[10] = 0; + J[11] = 0; + J[12] = 0; + J[13] = 0; + J[14] = 0; + J[15] = 0; + J[16] = 0; + J[17] = 0; + J[18] = t10 * t8; + J[19] = t13; + J[20] = t15; + J[21] = t51 * (t24 * t53 + t49 * t63 - t55); + J[22] = t51 * (-t44 * t63 - t64); + J[23] = t68 + * (-t2 * t52 + t65 * t72 * (t35 * t61 - t44 * t64 + t49 * t57) - t69 + - t71); + J[24] = t13; + J[25] = t73 * t8; + J[26] = t75; + J[27] = -t68 * (t0 * t76 - t36 + t37 + t66 * t80); + J[28] = t68 * (t43 * t80 + t77); + J[29] = t68 * (t65 * t72 * t79 - t78); + J[30] = t15; + J[31] = t75; + J[32] = t8 * t81; + J[33] = -t68 * (t87 + t88 * (t35 * t85 - t44 * t86 + t49 * t87)); + J[34] = -t51 * (t44 * t89 + t86); + J[35] = t51 * (t35 * t89 - t82 + t84); + J[36] = -t10 * t8; + J[37] = t90; + J[38] = t91; + J[39] = t51 + * (t18 * t24 - t92 + + t96 * (-t58 * t93 + t60 * (2 * t45 + t47) - t62 * t95)); + J[40] = t68 * (t43 * t72 * t98 + t97); + J[41] = t68 * (2 * t69 + t71 + t98 * t99); + J[42] = t90; + J[43] = -t73 * t8; + J[44] = t100; + J[45] = -t68 * (t103 * t88 + t36 - 2 * t37); + J[46] = t68 * (-t101 + t103 * t43 * t72); + J[47] = t68 * (t102 + t103 * t99); + J[48] = t91; + J[49] = t100; + J[50] = -t8 * t81; + J[51] = t51 * (t106 * t96 + 2 * t25 + t94); + J[52] = -t51 * (t105 + t107 * t44); + J[53] = t51 * (-t104 + t107 * t35 + t28 * t31); + J[54] = 0; + J[55] = 0; + J[56] = 0; + J[57] = t51 * (t108 - t109 + t110 * t96); + J[58] = -t51 * (t110 * t44 * t56 + t12); + J[59] = t51 * (t110 * t35 * t56 - t14); + J[60] = 0; + J[61] = 0; + J[62] = 0; + J[63] = t51 * (t111 * t49 * t56 - t12); + J[64] = t51 * (-t111 * t44 * t56 + t6); + J[65] = t51 * (t111 * t35 * t56 - t74); + J[66] = 0; + J[67] = 0; + J[68] = 0; + J[69] = t51 * (t114 * t49 * t56 - t14); + J[70] = -t51 * (t115 * t44 + t74); + J[71] = t51 * (t112 - t113 + t115 * t35); } - } // namespace autogen } // namespace ipc diff --git a/src/ipc/tangent/tangent_basis.hpp b/src/ipc/tangent/tangent_basis.hpp index 601b3f972..7c0b181c4 100644 --- a/src/ipc/tangent/tangent_basis.hpp +++ b/src/ipc/tangent/tangent_basis.hpp @@ -17,8 +17,8 @@ MatrixMax point_point_tangent_basis( /// @brief Compute the Jacobian of the tangent basis for the point-point pair. /// @param p0 First point /// @param p1 Second point -/// @return A 6*3x2 matrix whose columns are the basis vectors. -MatrixMax point_point_tangent_basis_jacobian( +/// @return A (3*2)x6 matrix whose columns are the basis vectors. +MatrixMax point_point_tangent_basis_jacobian( Eigen::ConstRef p0, Eigen::ConstRef p1); // ============================================================================ @@ -38,8 +38,8 @@ MatrixMax point_edge_tangent_basis( /// @param p Point /// @param e0 First edge point /// @param e1 Second edge point -/// @return A 9*3x2 matrix whose columns are the basis vectors. -MatrixMax point_edge_tangent_basis_jacobian( +/// @return A (3*2)x9 matrix whose columns are the basis vectors. +MatrixMax point_edge_tangent_basis_jacobian( Eigen::ConstRef p, Eigen::ConstRef e0, Eigen::ConstRef e1); @@ -64,8 +64,8 @@ Eigen::Matrix edge_edge_tangent_basis( /// @param ea1 Second point of the first edge /// @param eb0 First point of the second edge /// @param eb1 Second point of the second edge -/// @return A 12*3x2 matrix whose columns are the basis vectors. -Eigen::Matrix edge_edge_tangent_basis_jacobian( +/// @return A (3*2)x12 matrix whose columns are the basis vectors. +Eigen::Matrix edge_edge_tangent_basis_jacobian( Eigen::ConstRef ea0, Eigen::ConstRef ea1, Eigen::ConstRef eb0, @@ -99,8 +99,8 @@ Eigen::Matrix point_triangle_tangent_basis( /// @param t0 Triangle's first vertex /// @param t1 Triangle's second vertex /// @param t2 Triangle's third vertex -/// @return A 12*3x2 matrix whose columns are the basis vectors. -Eigen::Matrix point_triangle_tangent_basis_jacobian( +/// @return A (3*2)x12 matrix whose columns are the basis vectors. +Eigen::Matrix point_triangle_tangent_basis_jacobian( Eigen::ConstRef p, Eigen::ConstRef t0, Eigen::ConstRef t1, @@ -109,11 +109,11 @@ Eigen::Matrix point_triangle_tangent_basis_jacobian( // ============================================================================ namespace autogen { - // J is (8×1) flattened in column-major order + // J is (2×4) flattened in column-major order void point_point_tangent_basis_2D_jacobian( double p0_x, double p0_y, double p1_x, double p1_y, double J[8]); - // J is (18×2) flattened in column-major order + // J is (6×6) flattened in column-major order void point_point_tangent_basis_3D_jacobian( double p0_x, double p0_y, @@ -123,7 +123,7 @@ namespace autogen { double p1_z, double J[36]); - // J is (12×1) flattened in column-major order + // J is (2×6) flattened in column-major order void point_edge_tangent_basis_2D_jacobian( double p_x, double p_y, @@ -133,7 +133,7 @@ namespace autogen { double e1_y, double J[12]); - // J is (27×2) flattened in column-major order + // J is (6×9) flattened in column-major order void point_edge_tangent_basis_3D_jacobian( double p_x, double p_y, @@ -146,7 +146,7 @@ namespace autogen { double e1_z, double J[54]); - // J is (36×2) flattened in column-major order + // J is (6×12) flattened in column-major order void edge_edge_tangent_basis_jacobian( double ea0_x, double ea0_y, @@ -162,7 +162,7 @@ namespace autogen { double eb1_z, double J[72]); - // J is (36×2) flattened in column-major order + // J is (6×12) flattened in column-major order void point_triangle_tangent_basis_jacobian( double p_x, double p_y, diff --git a/tests/src/tests/candidates/test_normals.cpp b/tests/src/tests/candidates/test_normals.cpp index b47b340bd..672259a59 100644 --- a/tests/src/tests/candidates/test_normals.cpp +++ b/tests/src/tests/candidates/test_normals.cpp @@ -80,8 +80,8 @@ TEST_CASE("Edge-vertex collision normal", "[ev][normal]") expected_normal(1) = V(2, 1) < 0 ? -1 : 1; CHECK(normal.isApprox(expected_normal)); if (!normal.isApprox(expected_normal)) { - std::cout << "Normal: " << normal.transpose() << std::endl; - std::cout << "Expected: " << expected_normal.transpose() << std::endl; + std::cout << "Normal: " << normal.transpose() << "\n"; + std::cout << "Expected: " << expected_normal.transpose() << "\n"; } // Check jacobian using finite differences @@ -93,14 +93,15 @@ TEST_CASE("Edge-vertex collision normal", "[ev][normal]") fd_jacobian); CHECK(fd::compare_jacobian(jacobian, fd_jacobian)); if (!fd::compare_jacobian(jacobian, fd_jacobian)) { - std::cout << "Jacobian:\n" << jacobian << std::endl; - std::cout << "FD Jacobian:\n" << fd_jacobian << std::endl; + std::cout << "Jacobian:\n" << jacobian << "\n"; + std::cout << "FD Jacobian:\n" << fd_jacobian << "\n"; } } TEST_CASE("Point-line normal hessian", "[pl][normal]") { const int DIM = GENERATE(2, 3); + CAPTURE(DIM); VectorMax3d p(DIM); VectorMax3d e0(DIM); @@ -117,7 +118,7 @@ TEST_CASE("Point-line normal hessian", "[pl][normal]") // Check hessian using finite differences Eigen::MatrixXd hessian = point_line_unnormalized_normal_hessian(p, e0, e1); Eigen::MatrixXd fd_hessian; - fd::finite_jacobian( + fd::finite_jacobian_tensor<3>( x, [DIM](const Eigen::VectorXd& x_fd) -> Eigen::MatrixXd { return point_line_unnormalized_normal_jacobian( @@ -127,13 +128,13 @@ TEST_CASE("Point-line normal hessian", "[pl][normal]") fd_hessian); CHECK(fd::compare_jacobian(hessian, fd_hessian, 1e-6)); if (!fd::compare_jacobian(hessian, fd_hessian, 1e-6)) { - std::cout << "Hessian:\n" << hessian << std::endl; - std::cout << "FD Hessian:\n" << fd_hessian << std::endl; + std::cout << "Hessian:\n" << hessian << "\n"; + std::cout << "FD Hessian:\n" << fd_hessian << "\n"; } // Check hessian using finite differences hessian = point_line_normal_hessian(p, e0, e1); - fd::finite_jacobian( + fd::finite_jacobian_tensor<3>( x, [DIM](const Eigen::VectorXd& x_fd) -> Eigen::MatrixXd { return point_line_normal_jacobian( @@ -143,8 +144,8 @@ TEST_CASE("Point-line normal hessian", "[pl][normal]") fd_hessian); CHECK(fd::compare_jacobian(hessian, fd_hessian, 1e-6)); if (!fd::compare_jacobian(hessian, fd_hessian, 1e-6)) { - std::cout << "Hessian:\n" << hessian << std::endl; - std::cout << "FD Hessian:\n" << fd_hessian << std::endl; + std::cout << "Hessian:\n" << hessian << "\n"; + std::cout << "FD Hessian:\n" << fd_hessian << "\n"; } } @@ -188,8 +189,8 @@ TEST_CASE("Edge-edge collision normal", "[ee][normal]") fd_jacobian); CHECK(fd::compare_jacobian(jacobian, fd_jacobian)); if (!fd::compare_jacobian(jacobian, fd_jacobian)) { - std::cout << "Jacobian:\n" << jacobian << std::endl; - std::cout << "FD Jacobian:\n" << fd_jacobian << std::endl; + std::cout << "Jacobian:\n" << jacobian << "\n"; + std::cout << "FD Jacobian:\n" << fd_jacobian << "\n"; } } @@ -214,7 +215,7 @@ TEST_CASE("Line-line normal hessian", "[ee][normal][hessian]") // Check hessian using finite differences Eigen::MatrixXd hessian = line_line_unnormalized_normal_hessian(a, b, c, d); Eigen::MatrixXd fd_hessian; - fd::finite_jacobian( + fd::finite_jacobian_tensor<3>( x, [](const Eigen::VectorXd& x_fd) -> Eigen::MatrixXd { return line_line_unnormalized_normal_jacobian( @@ -224,13 +225,13 @@ TEST_CASE("Line-line normal hessian", "[ee][normal][hessian]") fd_hessian); CHECK(fd::compare_jacobian(hessian, fd_hessian, 1e-6)); if (!fd::compare_jacobian(hessian, fd_hessian, 1e-6)) { - std::cout << "Hessian:\n" << hessian << std::endl; - std::cout << "FD Hessian:\n" << fd_hessian << std::endl; + std::cout << "Hessian:\n" << hessian << "\n"; + std::cout << "FD Hessian:\n" << fd_hessian << "\n"; } // Check hessian using finite differences hessian = line_line_normal_hessian(a, b, c, d); - fd::finite_jacobian( + fd::finite_jacobian_tensor<3>( x, [](const Eigen::VectorXd& x_fd) -> Eigen::MatrixXd { return line_line_normal_jacobian( @@ -240,8 +241,8 @@ TEST_CASE("Line-line normal hessian", "[ee][normal][hessian]") fd_hessian); CHECK(fd::compare_jacobian(hessian, fd_hessian, 1e-6)); if (!fd::compare_jacobian(hessian, fd_hessian, 1e-6)) { - std::cout << "Hessian:\n" << hessian << std::endl; - std::cout << "FD Hessian:\n" << fd_hessian << std::endl; + std::cout << "Hessian:\n" << hessian << "\n"; + std::cout << "FD Hessian:\n" << fd_hessian << "\n"; } } @@ -288,8 +289,8 @@ TEST_CASE("Face-vertex collision normal", "[fv][normal]") fd_jacobian); CHECK(fd::compare_jacobian(jacobian, fd_jacobian)); if (!fd::compare_jacobian(jacobian, fd_jacobian)) { - std::cout << "Jacobian:\n" << jacobian << std::endl; - std::cout << "FD Jacobian:\n" << fd_jacobian << std::endl; + std::cout << "Jacobian:\n" << jacobian << "\n"; + std::cout << "FD Jacobian:\n" << fd_jacobian << "\n"; } } @@ -312,20 +313,20 @@ TEST_CASE("Triangle normal hessian", "[normal]") // Cross product matrix jacobian Eigen::MatrixXd J_cross = cross_product_matrix_jacobian(); Eigen::MatrixXd fd_J_cross; - fd::finite_jacobian( + fd::finite_jacobian_tensor<3>( a, [](const Eigen::Vector3d& a_fd) { return cross_product_matrix(a_fd); }, fd_J_cross); CHECK(fd::compare_jacobian(J_cross, fd_J_cross, 1e-6)); if (!fd::compare_jacobian(J_cross, fd_J_cross, 1e-6)) { - std::cout << "Hessian:\n" << J_cross << std::endl; - std::cout << "FD Hessian:\n" << fd_J_cross << std::endl; + std::cout << "Hessian:\n" << J_cross << "\n"; + std::cout << "FD Hessian:\n" << fd_J_cross << "\n"; } // Check hessian using finite differences Eigen::MatrixXd hessian = triangle_unnormalized_normal_hessian(a, b, c); Eigen::MatrixXd fd_hessian; - fd::finite_jacobian( + fd::finite_jacobian_tensor<3>( x, [](const Eigen::VectorXd& x_fd) -> Eigen::MatrixXd { return triangle_unnormalized_normal_jacobian( @@ -334,13 +335,13 @@ TEST_CASE("Triangle normal hessian", "[normal]") fd_hessian); CHECK(fd::compare_jacobian(hessian, fd_hessian, 1e-6)); if (!fd::compare_jacobian(hessian, fd_hessian, 1e-6)) { - std::cout << "Hessian:\n" << hessian << std::endl; - std::cout << "FD Hessian:\n" << fd_hessian << std::endl; + std::cout << "Hessian:\n" << hessian << "\n"; + std::cout << "FD Hessian:\n" << fd_hessian << "\n"; } // Check hessian using finite differences hessian = triangle_normal_hessian(a, b, c); - fd::finite_jacobian( + fd::finite_jacobian_tensor<3>( x, [](const Eigen::VectorXd& x_fd) -> Eigen::MatrixXd { return triangle_normal_jacobian( @@ -349,8 +350,8 @@ TEST_CASE("Triangle normal hessian", "[normal]") fd_hessian); CHECK(fd::compare_jacobian(hessian, fd_hessian, 1e-6)); if (!fd::compare_jacobian(hessian, fd_hessian, 1e-6)) { - std::cout << "Hessian:\n" << hessian << std::endl; - std::cout << "FD Hessian:\n" << fd_hessian << std::endl; + std::cout << "Hessian:\n" << hessian << "\n"; + std::cout << "FD Hessian:\n" << fd_hessian << "\n"; } } @@ -381,7 +382,7 @@ TEST_CASE("Plane-vertex collision normal", "[pv][normal]") fd_jacobian); CHECK(fd::compare_jacobian(jacobian, fd_jacobian)); if (!fd::compare_jacobian(jacobian, fd_jacobian)) { - std::cout << "Jacobian:\n" << jacobian << std::endl; - std::cout << "FD Jacobian:\n" << fd_jacobian << std::endl; + std::cout << "Jacobian:\n" << jacobian << "\n"; + std::cout << "FD Jacobian:\n" << fd_jacobian << "\n"; } } diff --git a/tests/src/tests/collisions/test_plane_vertex_collisions.cpp b/tests/src/tests/collisions/test_plane_vertex_collisions.cpp index 3cd826b1d..258791319 100644 --- a/tests/src/tests/collisions/test_plane_vertex_collisions.cpp +++ b/tests/src/tests/collisions/test_plane_vertex_collisions.cpp @@ -228,7 +228,7 @@ TEST_CASE( VectorMax2d cp; // empty closest point for pv PlaneVertexTangentialCollision tc(normal_collision, pos, normal_force); TangentialCollision& tc_base = tc; - auto mat = tc_base.relative_velocity_matrix(cp); + auto mat = tc_base.relative_velocity_jacobian(cp); CHECK(mat.isApprox(MatrixMax::Identity(3, 3))); } @@ -239,7 +239,7 @@ TEST_CASE( VectorMax2d cp; // empty closest point for pv PlaneVertexTangentialCollision tc(normal_collision, pos, normal_force); TangentialCollision& tc_base = tc; - auto jac = tc_base.relative_velocity_matrix_jacobian(cp); + auto jac = tc_base.relative_velocity_dx_dbeta(cp); CHECK(jac.isZero()); } } diff --git a/tests/src/tests/dof_layout.hpp b/tests/src/tests/dof_layout.hpp index 8dbb26fa1..689969b51 100644 --- a/tests/src/tests/dof_layout.hpp +++ b/tests/src/tests/dof_layout.hpp @@ -71,6 +71,28 @@ reorder_gradient(const Eigen::VectorXd& g_rowmajor, int n_verts, int dim) } } +/// Reorder a dense Hessian from RowMajor DOF ordering to +/// VERTEX_DERIVATIVE_LAYOUT DOF ordering. +inline auto +reorder_hessian(const Eigen::MatrixXd& H_rowmajor, int n_verts, int dim) +{ + if constexpr (VERTEX_DERIVATIVE_LAYOUT == Eigen::RowMajor) { + return H_rowmajor; // Already in the correct order, no copy needed. + } else { + const int n = n_verts * dim; + assert(H_rowmajor.rows() == n && H_rowmajor.cols() == n); + Eigen::MatrixXd H(n, n); + for (int i = 0; i < n; ++i) { + const int pi = (i % dim) * n_verts + i / dim; + for (int j = 0; j < n; ++j) { + const int pj = (j % dim) * n_verts + j / dim; + H(pi, pj) = H_rowmajor(i, j); + } + } + return H; + } +} + /// Reorder a sparse Hessian from RowMajor DOF ordering to /// VERTEX_DERIVATIVE_LAYOUT DOF ordering. inline auto reorder_hessian( diff --git a/tests/src/tests/tangent/test_relative_velocity.cpp b/tests/src/tests/tangent/test_relative_velocity.cpp index 5cea203aa..d4dd8a8be 100644 --- a/tests/src/tests/tangent/test_relative_velocity.cpp +++ b/tests/src/tests/tangent/test_relative_velocity.cpp @@ -1,5 +1,9 @@ #include #include +#include + +#include +#include #include #include @@ -85,3 +89,101 @@ TEST_CASE( Eigen::Vector2d relative_velocity = point_point_relative_velocity(dp0, dp1); CHECK((dp0 - relative_velocity).norm() == Catch::Approx(0).margin(1e-12)); } + +TEST_CASE( + "Point-point relative velocity matrix Jacobians", + "[friction][point-point][relative_velocity][jacobian]") +{ + const int dim = GENERATE(2, 3); + + auto J_analytical = point_point_relative_velocity_dx_dbeta(dim); + + Eigen::VectorXd x = Eigen::VectorXd::Zero(1); + Eigen::MatrixXd J_numerical; + fd::finite_jacobian_tensor<3>( + x, + [&dim](const Eigen::VectorXd&) -> Eigen::MatrixXd { + return point_point_relative_velocity_jacobian(dim); + }, + J_numerical); + + CHECK(fd::compare_jacobian(J_analytical, J_numerical)); + if (!fd::compare_jacobian(J_analytical, J_numerical)) { + std::cout << "Analytical:\n" + << J_analytical << "\nNumerical:\n" + << J_numerical << '\n'; + } +} + +TEST_CASE( + "Point-edge relative velocity matrix Jacobians", + "[friction][point-edge][relative_velocity][jacobian]") +{ + const int dim = GENERATE(2, 3); + Eigen::VectorXd x = Eigen::VectorXd::Random(1); + + auto J_analytical = point_edge_relative_velocity_dx_dbeta(dim, x[0]); + + Eigen::MatrixXd J_numerical; + fd::finite_jacobian_tensor<3>( + x, + [&dim](const Eigen::VectorXd& _x) -> Eigen::MatrixXd { + return point_edge_relative_velocity_jacobian(dim, _x[0]); + }, + J_numerical); + + CHECK(fd::compare_jacobian(J_analytical, J_numerical)); + if (!fd::compare_jacobian(J_analytical, J_numerical)) { + std::cout << "Analytical:\n" + << J_analytical << "\nNumerical:\n" + << J_numerical << '\n'; + } +} + +TEST_CASE( + "Edge-edge relative velocity matrix Jacobians", + "[friction][edge-edge][relative_velocity][jacobian]") +{ + Eigen::VectorXd x = Eigen::VectorXd::Random(2); + + auto J_analytical = edge_edge_relative_velocity_dx_dbeta(x); + + Eigen::MatrixXd J_numerical; + fd::finite_jacobian_tensor<3>( + x, + [](const Eigen::VectorXd& _x) -> Eigen::MatrixXd { + return edge_edge_relative_velocity_jacobian(_x); + }, + J_numerical); + + CHECK(fd::compare_jacobian(J_analytical, J_numerical)); + if (!fd::compare_jacobian(J_analytical, J_numerical)) { + std::cout << "Analytical:\n" + << J_analytical << "\nNumerical:\n" + << J_numerical << '\n'; + } +} + +TEST_CASE( + "Point-triangle relative velocity matrix Jacobians", + "[friction][point-triangle][relative_velocity][jacobian]") +{ + Eigen::VectorXd x = Eigen::VectorXd::Random(2); + + auto J_analytical = point_triangle_relative_velocity_dx_dbeta(x); + + Eigen::MatrixXd J_numerical; + fd::finite_jacobian_tensor<3>( + x, + [](const Eigen::VectorXd& _x) -> Eigen::MatrixXd { + return point_triangle_relative_velocity_jacobian(_x); + }, + J_numerical); + + CHECK(fd::compare_jacobian(J_analytical, J_numerical)); + if (!fd::compare_jacobian(J_analytical, J_numerical)) { + std::cout << "Analytical:\n" + << J_analytical << "\nNumerical:\n" + << J_numerical << '\n'; + } +} \ No newline at end of file diff --git a/tests/src/tests/tangent/test_tangent_basis.cpp b/tests/src/tests/tangent/test_tangent_basis.cpp index 53d8d8b9c..c095718e5 100644 --- a/tests/src/tests/tangent/test_tangent_basis.cpp +++ b/tests/src/tests/tangent/test_tangent_basis.cpp @@ -5,6 +5,8 @@ #include +#include + using namespace ipc; TEST_CASE( @@ -24,28 +26,26 @@ TEST_CASE( == Catch::Approx(1)); // Jacobian - const Eigen::Matrix J = - point_triangle_tangent_basis_jacobian(p, t0, t1, t2); + const auto J = point_triangle_tangent_basis_jacobian(p, t0, t1, t2); Vector12d x; x << p, t0, t1, t2; - Eigen::MatrixXd tmp; - fd::finite_jacobian( + Eigen::MatrixXd J_fd; + fd::finite_jacobian_tensor<3>( x, - [](const Eigen::VectorXd& _x) -> Eigen::VectorXd { + [](const Eigen::VectorXd& _x) -> Eigen::MatrixXd { return point_triangle_tangent_basis( - _x.segment<3>(0), _x.segment<3>(3), _x.segment<3>(6), - _x.segment<3>(9)) - .reshaped(); + _x.segment<3>(0), _x.segment<3>(3), _x.segment<3>(6), + _x.segment<3>(9)); }, - tmp); - - Eigen::Matrix J_fd; - J_fd.col(0) = tmp.topRows(3).reshaped(); - J_fd.col(1) = tmp.bottomRows(3).reshaped(); + J_fd); CHECK(fd::compare_jacobian(J, J_fd)); + if (!fd::compare_jacobian(J, J_fd)) { + std::cout << "J_analytic:\n" << J << "\n\n"; + std::cout << "J_numerical:\n" << J_fd << "\n\n"; + } } TEST_CASE( @@ -65,26 +65,20 @@ TEST_CASE( == Catch::Approx(1)); // Jacobian - const Eigen::Matrix J = - edge_edge_tangent_basis_jacobian(ea0, ea1, eb0, eb1); + const auto J = edge_edge_tangent_basis_jacobian(ea0, ea1, eb0, eb1); Vector12d x; x << ea0, ea1, eb0, eb1; - Eigen::MatrixXd tmp; - fd::finite_jacobian( + Eigen::MatrixXd J_fd; + fd::finite_jacobian_tensor<3>( x, - [](const Eigen::VectorXd& _x) -> Eigen::VectorXd { + [](const Eigen::VectorXd& _x) -> Eigen::MatrixXd { return edge_edge_tangent_basis( - _x.segment<3>(0), _x.segment<3>(3), _x.segment<3>(6), - _x.segment<3>(9)) - .reshaped(); + _x.segment<3>(0), _x.segment<3>(3), _x.segment<3>(6), + _x.segment<3>(9)); }, - tmp); - - Eigen::Matrix J_fd; - J_fd.col(0) = tmp.topRows(3).reshaped(); - J_fd.col(1) = tmp.bottomRows(3).reshaped(); + J_fd); CHECK(fd::compare_jacobian(J, J_fd)); } @@ -105,25 +99,19 @@ TEST_CASE( == Catch::Approx(1)); // Jacobian - const Eigen::Matrix J = - point_edge_tangent_basis_jacobian(p, e0, e1); + const auto J = point_edge_tangent_basis_jacobian(p, e0, e1); Vector9d x; x << p, e0, e1; - Eigen::MatrixXd tmp; - fd::finite_jacobian( + Eigen::MatrixXd J_fd; + fd::finite_jacobian_tensor<3>( x, - [](const Eigen::VectorXd& _x) -> Eigen::VectorXd { + [](const Eigen::VectorXd& _x) -> Eigen::MatrixXd { return point_edge_tangent_basis( - _x.segment<3>(0), _x.segment<3>(3), _x.segment<3>(6)) - .reshaped(); + _x.segment<3>(0), _x.segment<3>(3), _x.segment<3>(6)); }, - tmp); - - Eigen::Matrix J_fd; - J_fd.col(0) = tmp.topRows(3).reshaped(); - J_fd.col(1) = tmp.bottomRows(3).reshaped(); + J_fd); CHECK(fd::compare_jacobian(J, J_fd)); } @@ -144,24 +132,18 @@ TEST_CASE( == Catch::Approx(1)); // Jacobian - const Eigen::Matrix J = - point_point_tangent_basis_jacobian(p0, p1); + const auto J = point_point_tangent_basis_jacobian(p0, p1); Vector6d x; x << p0, p1; - Eigen::MatrixXd tmp; - fd::finite_jacobian( + Eigen::MatrixXd J_fd; + fd::finite_jacobian_tensor<3>( x, - [](const Eigen::VectorXd& _x) -> Eigen::VectorXd { - return point_point_tangent_basis(_x.head<3>(), _x.tail<3>()) - .reshaped(); + [](const Eigen::VectorXd& _x) -> Eigen::MatrixXd { + return point_point_tangent_basis(_x.head<3>(), _x.tail<3>()); }, - tmp); - - Eigen::Matrix J_fd; - J_fd.col(0) = tmp.topRows(3).reshaped(); - J_fd.col(1) = tmp.bottomRows(3).reshaped(); + J_fd); CHECK(fd::compare_jacobian(J, J_fd)); } @@ -177,20 +159,19 @@ TEST_CASE( CHECK(std::abs(basis.dot(Eigen::Vector2d::UnitX())) == Catch::Approx(1)); // Jacobian - const Vector12d J = point_edge_tangent_basis_jacobian(p, e0, e1); + const auto J = point_edge_tangent_basis_jacobian(p, e0, e1); Vector6d x; x << p, e0, e1; Eigen::MatrixXd J_fd; - fd::finite_jacobian( + fd::finite_jacobian_tensor<3>( x, [](const Eigen::VectorXd& _x) -> Eigen::VectorXd { return point_edge_tangent_basis( _x.segment<2>(0), _x.segment<2>(2), _x.segment<2>(4)); }, J_fd); - J_fd = J_fd.reshaped(); CHECK(fd::compare_jacobian(J, J_fd)); } @@ -206,20 +187,18 @@ TEST_CASE( CHECK(std::abs(basis.dot(Eigen::Vector2d::UnitX())) == Catch::Approx(1)); // Jacobian - const Eigen::Vector J = - point_point_tangent_basis_jacobian(p0, p1); + const auto J = point_point_tangent_basis_jacobian(p0, p1); Eigen::Vector4d x; x << p0, p1; Eigen::MatrixXd J_fd; - fd::finite_jacobian( + fd::finite_jacobian_tensor<3>( x, [](const Eigen::VectorXd& _x) -> Eigen::VectorXd { return point_point_tangent_basis(_x.head<2>(), _x.tail<2>()); }, J_fd); - J_fd = J_fd.reshaped(); CHECK(fd::compare_jacobian(J, J_fd)); } diff --git a/tests/src/tests/test_collision_mesh.cpp b/tests/src/tests/test_collision_mesh.cpp index 2077cd74e..d71654db2 100644 --- a/tests/src/tests/test_collision_mesh.cpp +++ b/tests/src/tests/test_collision_mesh.cpp @@ -1,3 +1,4 @@ +#include #include #include #include @@ -43,9 +44,11 @@ TEST_CASE("Collision mesh", "[collision_mesh]") Eigen::VectorXd g(8); g << 1, 1, -1, 1, 1, -1, -1, -1; + g = tests::reorder_gradient(g, mesh.num_vertices(), V.cols()); Eigen::VectorXd gf = mesh.to_full_dof(g); Eigen::VectorXd expected_gf(6); expected_gf << 1, 1, -2, 0, 0, -2; + expected_gf = tests::reorder_gradient(expected_gf, U.rows(), U.cols()); CHECK(gf == expected_gf); Eigen::MatrixXd H = Eigen::MatrixXd::Identity(8, 8); @@ -59,6 +62,7 @@ TEST_CASE("Collision mesh", "[collision_mesh]") 0, 0, 0, 2, 0, 1, // 0, 0, 1, 0, 2, 0, // 0, 0, 0, 1, 0, 2; // + expected_Hf = tests::reorder_hessian(expected_Hf, U.rows(), U.cols()); CHECK(Hf == expected_Hf); }