diff --git a/.clang-tidy b/.clang-tidy index 1ccce196a..e1aa65cee 100644 --- a/.clang-tidy +++ b/.clang-tidy @@ -8,6 +8,7 @@ Checks: > # Bug Detection and Safety bugprone-*, + # misc-const-correctness, # Specific high-value checks (even if not in a group above) google-runtime-int, @@ -27,7 +28,10 @@ Checks: > -readability-isolate-declaration, -readability-magic-numbers, -readability-math-missing-parentheses, - -readability-redundant-access-specifiers + -readability-redundant-access-specifiers, + -readability-uppercase-literal-suffix, + -readability-function-size, + -readability-convert-member-functions-to-static WarningsAsErrors: '*' HeaderFilterRegex: '.*(ipc/).*' diff --git a/.github/workflows/coverage.yml b/.github/workflows/coverage.yml index 30b24d53c..71281ccd7 100644 --- a/.github/workflows/coverage.yml +++ b/.github/workflows/coverage.yml @@ -72,7 +72,7 @@ jobs: lcov --remove coverage.info --ignore-errors unused '/usr/*' "$HOME/.cache/*" "*tests/*" --output-file coverage.info - name: Upload coverage reports to Codecov - uses: codecov/codecov-action@v3.1.6 + uses: codecov/codecov-action@v5.5.1 with: fail_ci_if_error: true files: build/coverage.info diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 14f21bf7f..59e933930 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -1,11 +1,11 @@ repos: - repo: https://github.com/pre-commit/mirrors-clang-format - rev: v14.0.0 + rev: v21.1.2 hooks: - id: clang-format name: clang-format description: "Use clang-format to format C/C++ code" - entry: clang-format-20 + entry: clang-format args: - --style=file - --verbose diff --git a/CMakeLists.txt b/CMakeLists.txt index 612f1ff9e..da527286d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -176,6 +176,11 @@ target_link_libraries(ipc_toolkit PRIVATE simple_bvh::simple_bvh) include(spdlog) target_link_libraries(ipc_toolkit PUBLIC spdlog::spdlog) +# TinyAD +include(tinyad) +# TODO: Make this a private dependency once we stop exposing TinyAD types in the public API. +target_link_libraries(ipc_toolkit PUBLIC TinyAD::TinyAD) + # CCD if(IPC_TOOLKIT_WITH_INEXACT_CCD) # Etienne Vouga's CTCD Library for the floating point root finding algorithm diff --git a/cmake/ipc_toolkit/ipc_toolkit_tests_data.cmake b/cmake/ipc_toolkit/ipc_toolkit_tests_data.cmake index f51db2f7b..6ed41d69f 100644 --- a/cmake/ipc_toolkit/ipc_toolkit_tests_data.cmake +++ b/cmake/ipc_toolkit/ipc_toolkit_tests_data.cmake @@ -30,7 +30,7 @@ else() SOURCE_DIR ${IPC_TOOLKIT_TESTS_DATA_DIR} GIT_REPOSITORY https://github.com/ipc-sim/ipc-toolkit-tests-data.git - GIT_TAG 0c6ce752d020db32f62da37d7b8ca9f73c204b36 + GIT_TAG 7ca6db695adcc00d3d6d978767dfc0d81722a515 CONFIGURE_COMMAND "" BUILD_COMMAND "" diff --git a/cmake/recipes/scalable_ccd.cmake b/cmake/recipes/scalable_ccd.cmake index 5fea4ab23..074b1d355 100644 --- a/cmake/recipes/scalable_ccd.cmake +++ b/cmake/recipes/scalable_ccd.cmake @@ -8,7 +8,7 @@ message(STATUS "Third-party: creating target 'scalable_ccd::scalable_ccd'") include(CPM) CPMAddPackage( - URI "gh:continuous-collision-detection/scalable-ccd#4fa806f533b19132e696a2dddeab16537025b5f9" + URI "gh:continuous-collision-detection/scalable-ccd#c80af01cab083b3eeb8dac80312ec9cfe479a5cf" OPTIONS "SCALABLE_CCD_WITH_CUDA ${IPC_TOOLKIT_WITH_CUDA}" ) diff --git a/cmake/recipes/spdlog.cmake b/cmake/recipes/spdlog.cmake index 29016cd0b..d6bf30653 100644 --- a/cmake/recipes/spdlog.cmake +++ b/cmake/recipes/spdlog.cmake @@ -10,7 +10,7 @@ option(SPDLOG_INSTALL "Generate the install target" ON) set(CMAKE_INSTALL_DEFAULT_COMPONENT_NAME "spdlog") include(CPM) -CPMAddPackage("gh:gabime/spdlog@1.11.0") +CPMAddPackage("gh:gabime/spdlog@1.15.3") set_target_properties(spdlog PROPERTIES POSITION_INDEPENDENT_CODE ON) diff --git a/cmake/recipes/tinyad.cmake b/cmake/recipes/tinyad.cmake new file mode 100644 index 000000000..0352b088f --- /dev/null +++ b/cmake/recipes/tinyad.cmake @@ -0,0 +1,16 @@ +# TinyAD (https://github.com/patr-schm/tinyad) +# License: MIT +if(TARGET TinyAD::TinyAD) + return() +endif() + +message(STATUS "Third-party: creating target 'TinyAD::TinyAD'") + +include(CPM) +CPMAddPackage( + URI "gh:zfergus/tinyad#957ede5bcf4b8d924f27230083508c664a9f7a7d" + OPTIONS "TINYAD_PARALLEL_BACKEND onetbb" +) + +# Folder name for IDE +set_target_properties(TinyAD PROPERTIES FOLDER "ThirdParty") \ No newline at end of file diff --git a/docs/source/developers/tools.rst b/docs/source/developers/tools.rst new file mode 100644 index 000000000..925d0119f --- /dev/null +++ b/docs/source/developers/tools.rst @@ -0,0 +1,25 @@ +Tools for Developers +===================== + +Using Pre-Commit Hooks +---------------------- + +Use the ``.pre-commit-config.yaml`` file to apply clang-format on before commits. + +Steps: +1. ``pip install pre-commit`` +2. ``pre-commit install`` +3. ``git add . && git commit -m "My commit message"`` + +Hopefully, that will avoid upsetting the GitHub checks :slightly_smiling_face: + +Clang-Tidy +---------- + +To run locally: + +1. `Install clang-tidy on mac `_ +2. ``cd build; cmake -DCMAKE_EXPORT_COMPILE_COMMANDS=ON ..``: this exports a JSON describing how all the files will be compiled +3. ``cd ..; run-clang-tidy -quiet -p build $(find src -name '*.cpp')`` + +You can also tack on a ``-j `` to run-clang-tidy to make it parallel. diff --git a/python/src/bindings.cpp b/python/src/bindings.cpp index 3e5422dec..8bffaf376 100644 --- a/python/src/bindings.cpp +++ b/python/src/bindings.cpp @@ -88,6 +88,8 @@ PYBIND11_MODULE(ipctk, m) define_smooth_friction_mollifier(m); define_smooth_mu(m); + define_smooth_potential(m); + // implicits define_plane_implicit(m); diff --git a/python/src/collision_mesh.cpp b/python/src/collision_mesh.cpp index 31ccad0a4..680dbee65 100644 --- a/python/src/collision_mesh.cpp +++ b/python/src/collision_mesh.cpp @@ -151,7 +151,8 @@ void define_collision_mesh(py::module_& m) "displacement_map"_a = Eigen::SparseMatrix()) .def( py::init< - const std::vector&, Eigen::ConstRef, + const std::vector&, const std::vector&, + Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, const Eigen::SparseMatrix&>(), @@ -160,12 +161,13 @@ void define_collision_mesh(py::module_& m) Parameters: include_vertex: Vector of bools indicating whether each vertex should be included in the collision mesh. + orient_vertex: Vector of bools indicating whether each vertex is orientable. full_rest_positions: The vertices of the full mesh at rest (#V × dim). edges: The edges of the collision mesh indexed into the full mesh vertices (#E × 2). faces: The faces of the collision mesh indexed into the full mesh vertices (#F × 3). displacement_map: The displacement mapping from displacements on the full mesh to the collision mesh. )ipc_Qu8mg5v7", - "include_vertex"_a, "full_rest_positions"_a, + "include_vertex"_a, "orient_vertex"_a, "full_rest_positions"_a, "edges"_a = Eigen::MatrixXi(), "faces"_a = Eigen::MatrixXi(), "displacement_map"_a = Eigen::SparseMatrix()) .def_static( diff --git a/python/src/collisions/normal/normal_collisions.cpp b/python/src/collisions/normal/normal_collisions.cpp index fceddcc15..82019f660 100644 --- a/python/src/collisions/normal/normal_collisions.cpp +++ b/python/src/collisions/normal/normal_collisions.cpp @@ -1,9 +1,86 @@ #include #include +#include using namespace ipc; +template +void define_smooth_collision_template(py::module_& m, std::string name) +{ + py::class_(m, name.c_str()) + .def("name", &collision_type::name, "Get the type name of collision") + .def( + "num_vertices", &collision_type::num_vertices, + "Get the number of vertices"); +} + +void define_smooth_collisions(py::module_& m, std::string name) +{ + py::class_(m, name.c_str()) + .def(py::init()) + .def( + "build", + py::overload_cast< + const CollisionMesh&, Eigen::ConstRef, + const SmoothContactParameters, const bool, + const std::shared_ptr&>(&SmoothCollisions::build), + R"ipc_Qu8mg5v7( + Initialize the set of collisions used to compute the barrier potential. + + Parameters: + mesh: The collision mesh. + vertices: Vertices of the collision mesh. + param: SmoothContactParameters. + use_adaptive_dhat: If the adaptive dhat should be used. + broad_phase: Broad phase method. + )ipc_Qu8mg5v7", + py::arg("mesh"), py::arg("vertices"), py::arg("param"), + py::arg("use_adaptive_dhat") = false, + py::arg("broad_phase") = make_default_broad_phase()) + .def( + "compute_minimum_distance", + &SmoothCollisions::compute_minimum_distance, + R"ipc_Qu8mg5v7( + Computes the minimum distance between any non-adjacent elements. + + Parameters: + mesh: The collision mesh. + vertices: Vertices of the collision mesh. + + Returns: + The minimum distance between any non-adjacent elements. + )ipc_Qu8mg5v7", + py::arg("mesh"), py::arg("vertices")) + .def( + "__len__", &SmoothCollisions::size, "Get the number of collisions.") + .def( + "empty", &SmoothCollisions::empty, + "Get if the collision set is empty.") + .def("clear", &SmoothCollisions::clear, "Clear the collision set.") + .def( + "__getitem__", + [](SmoothCollisions& self, size_t i) -> + typename SmoothCollisions::value_type& { return self[i]; }, + py::return_value_policy::reference, + R"ipc_Qu8mg5v7( + Get a reference to collision at index i. + + Parameters: + i: The index of the collision. + + Returns: + A reference to the collision. + )ipc_Qu8mg5v7", + py::arg("i")) + .def( + "to_string", &SmoothCollisions::to_string, py::arg("mesh"), + py::arg("vertices"), py::arg("param")) + .def( + "n_candidates", &SmoothCollisions::n_candidates, + "Get the number of candidates."); +} + void define_normal_collisions(py::module_& m) { py::class_(m, "NormalCollisions") @@ -159,4 +236,42 @@ void define_normal_collisions(py::module_& m) .def_readwrite("ee_collisions", &NormalCollisions::ee_collisions) .def_readwrite("fv_collisions", &NormalCollisions::fv_collisions) .def_readwrite("pv_collisions", &NormalCollisions::pv_collisions); + + py::class_(m, "SmoothCollision2") + .def("n_dofs", &SmoothCollision::n_dofs, "Get the degree of freedom") + .def( + "__call__", &SmoothCollision::operator(), + R"ipc_Qu8mg5v7( + Compute the potential. + + Parameters: + positions: The vertex positions. + params: The parameters. + + Returns: + The potential (not scaled by the barrier stiffness) of this collision pair. + )ipc_Qu8mg5v7", + py::arg("positions"), py::arg("params")) + .def( + "__getitem__", + [](SmoothCollision& self, size_t i) -> long { return self[i]; }, + R"ipc_Qu8mg5v7( + Get primitive id. + + Parameters: + i: 0 or 1. + + Returns: + The index of the primitive. + )ipc_Qu8mg5v7", + py::arg("i")); + + define_smooth_collision_template< + SmoothCollisionTemplate, SmoothCollision>( + m, "Edge2Point2Collision"); + define_smooth_collision_template< + SmoothCollisionTemplate, SmoothCollision>( + m, "Point2Point2Collision"); + + define_smooth_collisions(m, "SmoothCollisions"); } diff --git a/python/src/potentials/barrier_potential.cpp b/python/src/potentials/barrier_potential.cpp index 92ff43f64..88c658665 100644 --- a/python/src/potentials/barrier_potential.cpp +++ b/python/src/potentials/barrier_potential.cpp @@ -1,6 +1,7 @@ #include #include +#include using namespace ipc; @@ -37,3 +38,153 @@ void define_barrier_potential(py::module_& m) &BarrierPotential::set_barrier, "Barrier function used to compute the potential."); } + +void define_smooth_potential(py::module_& m) +{ + py::class_(m, "SmoothContactParameters") + .def( + py::init< + const double, const double, const double, const double, + const double, const int>(), + R"ipc_Qu8mg5v7( + Construct parameter set for smooth contact. + + Parameters: + dhat, alpha_t, beta_t, alpha_n, beta_n, r + )ipc_Qu8mg5v7", + py::arg("dhat"), py::arg("alpha_t"), py::arg("beta_t"), + py::arg("alpha_n"), py::arg("beta_n"), py::arg("r")) + .def( + py::init(), + R"ipc_Qu8mg5v7( + Construct parameter set for smooth contact. + + Parameters: + dhat, alpha_t, beta_t, r + )ipc_Qu8mg5v7", + py::arg("dhat"), py::arg("alpha_t"), py::arg("beta_t"), + py::arg("r")) + .def_readonly("dhat", &SmoothContactParameters::dhat) + .def_readonly("alpha_t", &SmoothContactParameters::alpha_t) + .def_readonly("beta_t", &SmoothContactParameters::beta_t) + .def_readonly("alpha_n", &SmoothContactParameters::alpha_n) + .def_readonly("beta_n", &SmoothContactParameters::beta_n) + .def_readonly("r", &SmoothContactParameters::r); + + py::class_(m, "SmoothPotential") + .def( + py::init(), + R"ipc_Qu8mg5v7( + Construct a smooth barrier potential. + + Parameters: + param: A set of parameters. + )ipc_Qu8mg5v7", + py::arg("param")) + .def( + "__call__", + py::overload_cast< + const SmoothCollisions&, const CollisionMesh&, + Eigen::ConstRef>( + &ipc::SmoothContactPotential::operator(), py::const_), + R"ipc_Qu8mg5v7( + Compute the barrier potential for a set of collisions. + + Parameters: + collisions: The set of collisions. + mesh: The collision mesh. + vertices: Vertices of the collision mesh. + + Returns: + The sum of all barrier potentials (not scaled by the barrier stiffness). + )ipc_Qu8mg5v7", + py::arg("collisions"), py::arg("mesh"), py::arg("vertices")) + .def( + "gradient", + py::overload_cast< + const SmoothCollisions&, const CollisionMesh&, + Eigen::ConstRef>( + &ipc::SmoothContactPotential::gradient, py::const_), + R"ipc_Qu8mg5v7( + Compute the gradient of the barrier potential. + + Parameters: + collisions: The set of collisions. + mesh: The collision mesh. + vertices: Vertices of the collision mesh. + + Returns: + The gradient of all barrier potentials (not scaled by the barrier stiffness). This will have a size of |vertices|. + )ipc_Qu8mg5v7", + py::arg("collisions"), py::arg("mesh"), py::arg("vertices")) + .def( + "hessian", + py::overload_cast< + const SmoothCollisions&, const CollisionMesh&, + Eigen::ConstRef, const PSDProjectionMethod>( + &ipc::SmoothContactPotential::hessian, py::const_), + R"ipc_Qu8mg5v7( + Compute the hessian of the barrier potential. + + Parameters: + collisions: The set of collisions. + mesh: The collision mesh. + vertices: Vertices of the collision mesh. + project_hessian_to_psd: Make sure the hessian is positive semi-definite. + + Returns: + The hessian of all barrier potentials (not scaled by the barrier stiffness). This will have a size of |vertices|x|vertices|. + )ipc_Qu8mg5v7", + py::arg("collisions"), py::arg("mesh"), py::arg("vertices"), + py::arg("project_hessian_to_psd") = PSDProjectionMethod::NONE) + .def( + "__call__", + py::overload_cast< + const SmoothCollision&, Eigen::ConstRef>( + &ipc::SmoothContactPotential::operator(), py::const_), + R"ipc_Qu8mg5v7( + Compute the potential for a single collision. + + Parameters: + collision: The collision. + x: The collision stencil's degrees of freedom. + + Returns: + The potential. + )ipc_Qu8mg5v7", + py::arg("collision"), py::arg("x")) + .def( + "gradient", + py::overload_cast< + const SmoothCollision&, Eigen::ConstRef>( + &SmoothContactPotential::gradient, py::const_), + R"ipc_Qu8mg5v7( + Compute the gradient of the potential for a single collision. + + Parameters: + collision: The collision. + x: The collision stencil's degrees of freedom. + + Returns: + The gradient of the potential. + )ipc_Qu8mg5v7", + py::arg("collision"), py::arg("x")) + .def( + "hessian", + py::overload_cast< + const SmoothCollision&, Eigen::ConstRef, + const PSDProjectionMethod>( + &SmoothContactPotential::hessian, py::const_), + R"ipc_Qu8mg5v7( + Compute the hessian of the potential for a single collision. + + Parameters: + collision: The collision. + x: The collision stencil's degrees of freedom. + + Returns: + The hessian of the potential. + )ipc_Qu8mg5v7", + py::arg("collision"), py::arg("x"), + py::arg("project_hessian_to_psd") = PSDProjectionMethod::NONE); +} diff --git a/python/src/potentials/bindings.hpp b/python/src/potentials/bindings.hpp index e16d87a09..f440ee258 100644 --- a/python/src/potentials/bindings.hpp +++ b/python/src/potentials/bindings.hpp @@ -3,6 +3,7 @@ #include void define_barrier_potential(py::module_& m); +void define_smooth_potential(py::module_& m); void define_friction_potential(py::module_& m); void define_normal_adhesion_potential(py::module_& m); void define_normal_potential(py::module_& m); diff --git a/src/ipc/CMakeLists.txt b/src/ipc/CMakeLists.txt index a16cca530..95f28eb48 100644 --- a/src/ipc/CMakeLists.txt +++ b/src/ipc/CMakeLists.txt @@ -21,5 +21,6 @@ add_subdirectory(distance) add_subdirectory(friction) add_subdirectory(implicits) add_subdirectory(potentials) +add_subdirectory(smooth_contact) add_subdirectory(tangent) add_subdirectory(utils) \ No newline at end of file diff --git a/src/ipc/broad_phase/CMakeLists.txt b/src/ipc/broad_phase/CMakeLists.txt index 8fe5a051d..ff04b4cfa 100644 --- a/src/ipc/broad_phase/CMakeLists.txt +++ b/src/ipc/broad_phase/CMakeLists.txt @@ -7,6 +7,8 @@ set(SOURCES brute_force.hpp bvh.cpp bvh.hpp + create_broad_phase.cpp + create_broad_phase.hpp default_broad_phase.hpp hash_grid.cpp hash_grid.hpp diff --git a/src/ipc/broad_phase/create_broad_phase.cpp b/src/ipc/broad_phase/create_broad_phase.cpp new file mode 100644 index 000000000..1eecb5ffa --- /dev/null +++ b/src/ipc/broad_phase/create_broad_phase.cpp @@ -0,0 +1,38 @@ +#include "create_broad_phase.hpp" + +#include +#include +#include +#include +#include +#include + +namespace ipc { + +std::shared_ptr +create_broad_phase(const BroadPhaseMethod& broad_phase_method) +{ + switch (broad_phase_method) { + case BroadPhaseMethod::BRUTE_FORCE: + return std::make_shared(); + case BroadPhaseMethod::HASH_GRID: + return std::make_shared(); + case BroadPhaseMethod::SPATIAL_HASH: + return std::make_shared(); + case BroadPhaseMethod::BVH: + return std::make_shared(); + case BroadPhaseMethod::SWEEP_AND_PRUNE: + return std::make_shared(); + case BroadPhaseMethod::SWEEP_AND_TINIEST_QUEUE: +#ifdef IPC_TOOLKIT_WITH_CUDA + return std::make_shared(); +#else + log_and_throw_error( + "Sweep and Tiniest Queue broad phase requires CUDA! Enable it through CMake option IPC_TOOLKIT_WITH_CUDA."); +#endif + default: + log_and_throw_error("Unknown broad phase type!"); + } +} + +} // namespace ipc diff --git a/src/ipc/broad_phase/create_broad_phase.hpp b/src/ipc/broad_phase/create_broad_phase.hpp new file mode 100644 index 000000000..066249eb8 --- /dev/null +++ b/src/ipc/broad_phase/create_broad_phase.hpp @@ -0,0 +1,20 @@ +#pragma once + +#include +#include + +namespace ipc { + +enum class BroadPhaseMethod : uint8_t { + BRUTE_FORCE, + HASH_GRID, + SPATIAL_HASH, + BVH, + SWEEP_AND_PRUNE, + SWEEP_AND_TINIEST_QUEUE +}; + +std::shared_ptr +create_broad_phase(const BroadPhaseMethod& broad_phase_method); + +} // namespace ipc diff --git a/src/ipc/candidates/collision_stencil.hpp b/src/ipc/candidates/collision_stencil.hpp index 36d9aebcc..cec3bdd6f 100644 --- a/src/ipc/candidates/collision_stencil.hpp +++ b/src/ipc/candidates/collision_stencil.hpp @@ -13,6 +13,9 @@ namespace ipc { /// @brief A stencil representing a collision between at most four vertices. class CollisionStencil { public: + /// @brief The maximum number of vertices in a collision stencil. + static constexpr int STENCIL_SIZE = 4; + virtual ~CollisionStencil() = default; /// @brief Get the number of vertices in the collision stencil. @@ -30,8 +33,8 @@ class CollisionStencil { /// @brief Get the vertex IDs of the collision stencil. /// @param edges Collision mesh edges /// @param faces Collision mesh faces - /// @return The vertex IDs of the collision stencil. Size is always 4, but elements i > num_vertices() are -1. - virtual std::array vertex_ids( + /// @return The vertex IDs of the collision stencil. Elements i > num_vertices() are -1. + virtual std::array vertex_ids( Eigen::ConstRef edges, Eigen::ConstRef faces) const = 0; @@ -39,8 +42,8 @@ class CollisionStencil { /// @param vertices Vertex attributes /// @param edges Collision mesh edges /// @param faces Collision mesh faces - /// @return The vertex positions of the collision stencil. Size is always 4, but elements i > num_vertices() are NaN. - std::array vertices( + /// @return The vertex positions of the collision stencil. Elements i > num_vertices() are NaN. + std::array vertices( Eigen::ConstRef vertices, Eigen::ConstRef edges, Eigen::ConstRef faces) const @@ -49,8 +52,8 @@ class CollisionStencil { const auto vertex_ids = this->vertex_ids(edges, faces); - std::array stencil_vertices; - for (int i = 0; i < 4; i++) { + std::array stencil_vertices; + for (int i = 0; i < STENCIL_SIZE; i++) { if (vertex_ids[i] >= 0) { stencil_vertices[i] = vertices.row(vertex_ids[i]); } else { diff --git a/src/ipc/ccd/additive_ccd.hpp b/src/ipc/ccd/additive_ccd.hpp index b82f0d82b..123477c16 100644 --- a/src/ipc/ccd/additive_ccd.hpp +++ b/src/ipc/ccd/additive_ccd.hpp @@ -28,7 +28,7 @@ class AdditiveCCD : public NarrowPhaseCCD { /// @brief Construct a new AdditiveCCD object. /// @param max_iterations The maximum number of iterations to use for the CCD algorithm. If set to UNLIMITTED_ITERATIONS, the algorithm will run until it converges. /// @param conservative_rescaling The conservative rescaling of the time of impact. - AdditiveCCD( + explicit AdditiveCCD( const long max_iterations = UNLIMITTED_ITERATIONS, const double conservative_rescaling = DEFAULT_CONSERVATIVE_RESCALING); diff --git a/src/ipc/ccd/tight_inclusion_ccd.hpp b/src/ipc/ccd/tight_inclusion_ccd.hpp index ceeffb547..5f9637701 100644 --- a/src/ipc/ccd/tight_inclusion_ccd.hpp +++ b/src/ipc/ccd/tight_inclusion_ccd.hpp @@ -22,7 +22,7 @@ class TightInclusionCCD : public NarrowPhaseCCD { /// @param tolerance The tolerance used for the CCD algorithm. /// @param max_iterations The maximum number of iterations for the CCD algorithm. /// @param conservative_rescaling The conservative rescaling of the time of impact. - TightInclusionCCD( + explicit TightInclusionCCD( const double tolerance = DEFAULT_TOLERANCE, const long max_iterations = DEFAULT_MAX_ITERATIONS, const double conservative_rescaling = DEFAULT_CONSERVATIVE_RESCALING); diff --git a/src/ipc/collision_mesh.cpp b/src/ipc/collision_mesh.cpp index 870e9329c..66aeac933 100644 --- a/src/ipc/collision_mesh.cpp +++ b/src/ipc/collision_mesh.cpp @@ -4,6 +4,7 @@ #include #include #include +#include #include @@ -16,6 +17,7 @@ CollisionMesh::CollisionMesh( const Eigen::SparseMatrix& displacement_map) : CollisionMesh( std::vector(rest_positions.rows(), true), + std::vector(rest_positions.rows(), false), rest_positions, edges, faces, @@ -25,6 +27,7 @@ CollisionMesh::CollisionMesh( CollisionMesh::CollisionMesh( const std::vector& include_vertex, + const std::vector& orient_vertex, Eigen::ConstRef full_rest_positions, Eigen::ConstRef edges, Eigen::ConstRef faces, @@ -85,6 +88,12 @@ CollisionMesh::CollisionMesh( m_rest_positions = m_select_vertices * full_rest_positions; // m_rest_positions = vertices(full_rest_positions); + assert(orient_vertex.size() == full_rest_positions.rows()); + m_is_orient_vertex.assign(m_rest_positions.rows(), false); + for (int i = 0; i < m_is_orient_vertex.size(); i++) { + m_is_orient_vertex[i] = orient_vertex[m_vertex_to_full_vertex[i]]; + } + // Map faces and edges to only included vertices if (!include_all_vertices) { for (int i = 0; i < m_edges.rows(); i++) { @@ -106,6 +115,7 @@ CollisionMesh::CollisionMesh( m_faces_to_edges = construct_faces_to_edges(m_faces, m_edges); + init_edges_to_faces(); init_codim_vertices(); init_codim_edges(); init_areas(); @@ -116,19 +126,40 @@ CollisionMesh::CollisionMesh( // ============================================================================ +void CollisionMesh::init_edges_to_faces() +{ + if (dim() == 2) { + return; + } + + m_edges_to_faces.setOnes(num_edges(), 2); + m_edges_to_faces *= -1; + for (int f = 0; f < m_faces_to_edges.rows(); f++) { + for (int le = 0; le < 3; le++) { + if (m_edges_to_faces(m_faces_to_edges(f, le), 0) < 0) { + m_edges_to_faces(m_faces_to_edges(f, le), 0) = f; + } else if (m_edges_to_faces(m_faces_to_edges(f, le), 1) < 0) { + m_edges_to_faces(m_faces_to_edges(f, le), 1) = f; + } else { + assert(false); + } + } + } +} + void CollisionMesh::init_codim_vertices() { - std::vector is_codim_vertex(num_vertices(), true); + m_is_codim_vertex.assign(num_vertices(), true); for (int i : m_edges.reshaped()) { - is_codim_vertex[i] = false; + m_is_codim_vertex[i] = false; } m_codim_vertices.resize( - std::count(is_codim_vertex.begin(), is_codim_vertex.end(), true)); + std::count(m_is_codim_vertex.begin(), m_is_codim_vertex.end(), true)); int j = 0; for (int i = 0; i < num_vertices(); i++) { - if (is_codim_vertex[i]) { + if (m_is_codim_vertex[i]) { assert(j < m_codim_vertices.size()); m_codim_vertices[j++] = i; } @@ -138,17 +169,17 @@ void CollisionMesh::init_codim_vertices() void CollisionMesh::init_codim_edges() { - std::vector is_codim_edge(num_edges(), true); + m_is_codim_edge.assign(num_edges(), true); for (int i : m_faces_to_edges.reshaped()) { - is_codim_edge[i] = false; + m_is_codim_edge[i] = false; } m_codim_edges.resize( - std::count(is_codim_edge.begin(), is_codim_edge.end(), true)); + std::count(m_is_codim_edge.begin(), m_is_codim_edge.end(), true)); int j = 0; for (int i = 0; i < num_edges(); i++) { - if (is_codim_edge[i]) { + if (m_is_codim_edge[i]) { assert(j < m_codim_edges.size()); m_codim_edges[j++] = i; } @@ -237,19 +268,19 @@ void CollisionMesh::init_adjacencies() void CollisionMesh::init_areas() { - // m_vertices_to_edges.resize(num_vertices()); - // for (int i = 0; i < m_edges.rows(); i++) { - // for (int j = 0; j < m_edges.cols(); j++) { - // m_vertices_to_edges[m_edges(i, j)].push_back(i); - // } - // } - // - // m_vertices_to_faces.resize(num_vertices()); - // for (int i = 0; i < m_faces.rows(); i++) { - // for (int j = 0; j < m_faces.cols(); j++) { - // m_vertices_to_faces[m_faces(i, j)].push_back(i); - // } - // } + m_vertices_to_edges.resize(num_vertices()); + for (int i = 0; i < m_edges.rows(); i++) { + for (int j = 0; j < m_edges.cols(); j++) { + m_vertices_to_edges[m_edges(i, j)].push_back(i); + } + } + + m_vertices_to_faces.resize(num_vertices()); + for (int i = 0; i < m_faces.rows(); i++) { + for (int j = 0; j < m_faces.cols(); j++) { + m_vertices_to_faces[m_faces(i, j)].push_back(i); + } + } // Compute vertex areas as the sum of ½ the length of connected edges Eigen::VectorXd vertex_edge_areas = @@ -475,4 +506,19 @@ Eigen::MatrixXi CollisionMesh::construct_faces_to_edges( return faces_to_edges; } +double CollisionMesh::edge_length(const index_t edge_id) const +{ + return (m_rest_positions.row(m_edges(edge_id, 0)) + - m_rest_positions.row(m_edges(edge_id, 1))) + .norm(); +} + +double CollisionMesh::max_edge_length() const +{ + double val = 0; + for (int i = 0; i < m_edges.rows(); i++) { + val = std::max(edge_length(i), val); + } + return val; +} } // namespace ipc diff --git a/src/ipc/collision_mesh.hpp b/src/ipc/collision_mesh.hpp index 1298c0714..37fecb743 100644 --- a/src/ipc/collision_mesh.hpp +++ b/src/ipc/collision_mesh.hpp @@ -17,7 +17,7 @@ class CollisionMesh { /// @param edges The edges of the collision mesh (|E| × 2). /// @param faces The faces of the collision mesh (|F| × 3). /// @param displacement_map The displacement mapping from displacements on the full mesh to the collision mesh. - CollisionMesh( + explicit CollisionMesh( Eigen::ConstRef rest_positions, Eigen::ConstRef edges = Eigen::MatrixXi(), Eigen::ConstRef faces = Eigen::MatrixXi(), @@ -26,12 +26,14 @@ class CollisionMesh { /// @brief Construct a new Collision Mesh object from a full mesh vertices. /// @param include_vertex Vector of bools indicating whether each vertex should be included in the collision mesh. + /// @param orient_vertex Vector of bools indicating whether each vertex is orientable. /// @param full_rest_positions The vertices of the full mesh at rest (|V| × dim). /// @param edges The edges of the collision mesh indexed into the full mesh vertices (|E| × 2). /// @param faces The faces of the collision mesh indexed into the full mesh vertices (|F| × 3). /// @param displacement_map The displacement mapping from displacements on the full mesh to the collision mesh. CollisionMesh( const std::vector& include_vertex, + const std::vector& orient_vertex, Eigen::ConstRef full_rest_positions, Eigen::ConstRef edges = Eigen::MatrixXi(), Eigen::ConstRef faces = Eigen::MatrixXi(), @@ -50,6 +52,7 @@ class CollisionMesh { { return CollisionMesh( construct_is_on_surface(full_rest_positions.rows(), edges), + std::vector(full_rest_positions.rows(), false), full_rest_positions, edges, faces); } @@ -94,12 +97,24 @@ class CollisionMesh { /// @brief Get the vertices of the collision mesh at rest (|V| × dim). const Eigen::MatrixXd& rest_positions() const { return m_rest_positions; } - /// @brief Get the indices of codimensional vertices of the collision mesh (|CV| x 1). + /// @brief Get the indices of codimensional vertices of the collision mesh (|CV| × 1). const Eigen::VectorXi& codim_vertices() const { return m_codim_vertices; } - /// @brief Get the indices of codimensional edges of the collision mesh (|CE| x 1). + /// @brief Check if vertex i is codimensional. + bool is_codim_vertex(const index_t i) const { return m_is_codim_vertex[i]; } + + /// @brief Check if vertex i is orientable. + bool is_orient_vertex(const index_t i) const + { + return m_is_orient_vertex[i]; + } + + /// @brief Get the indices of codimensional edges of the collision mesh (|CE| × 1). const Eigen::VectorXi& codim_edges() const { return m_codim_edges; } + /// @brief Check if edge i is codimensional. + bool is_codim_edge(const index_t i) const { return m_is_codim_edge[i]; } + /// @brief Get the edges of the collision mesh (|E| × 2). const Eigen::MatrixXi& edges() const { return m_edges; } @@ -109,15 +124,26 @@ class CollisionMesh { /// @brief Get the mapping from faces to edges of the collision mesh (|F| × 3). const Eigen::MatrixXi& faces_to_edges() const { return m_faces_to_edges; } - // const std::vector>& vertices_to_edges() const - // { - // return m_vertices_to_edges; - // } + /// @brief Compute the rest length of an edge. + double edge_length(const index_t edge_id) const; - // const std::vector>& vertices_to_faces() const - // { - // return m_vertices_to_faces; - // } + /// @brief Compute the maximum rest length of all edges. + double max_edge_length() const; + + /// @brief Get the mapping from vertices to edges of the collision mesh. + const std::vector>& vertices_to_edges() const + { + return m_vertices_to_edges; + } + + /// @brief Get the mapping from vertices to faces of the collision mesh. + const std::vector>& vertices_to_faces() const + { + return m_vertices_to_faces; + } + + /// @brief Get the mapping from edges to faces of the collision mesh (|E| × 2). + const Eigen::MatrixXi& edges_to_faces() const { return m_edges_to_faces; } // ----------------------------------------------------------------------- @@ -264,7 +290,7 @@ class CollisionMesh { /// @brief Construct a vector of bools indicating whether each vertex is on the surface. /// @param num_vertices The number of vertices in the mesh. /// @param edges The surface edges of the mesh (|E| × 2). - /// @param codim_vertices The indices of codimensional vertices (|CV| x 1). + /// @param codim_vertices The indices of codimensional vertices (|CV| × 1). /// @return A vector of bools indicating whether each vertex is on the surface. static std::vector construct_is_on_surface( const size_t num_vertices, @@ -288,7 +314,13 @@ class CollisionMesh { // ----------------------------------------------------------------------- // Helper initialization functions + /// @brief Initialize map from edges to adjacent faces (|E| × 2). + void init_edges_to_faces(); + + /// @brief Initialize the codimensional vertices. void init_codim_vertices(); + + /// @brief Initialize the codimensional edges. void init_codim_edges(); /// @brief Initialize the selection matrix from full vertices/DOF to collision vertices/DOF. @@ -307,9 +339,15 @@ class CollisionMesh { Eigen::MatrixXd m_full_rest_positions; /// @brief The vertex positions at rest (|V| × dim). Eigen::MatrixXd m_rest_positions; - /// @brief The indices of codimensional vertices (|CV| x 1). + /// @brief The mask of codimensional vertices (|V|). + std::vector m_is_codim_vertex; + /// @brief The mask of orientable vertices (|V|). + std::vector m_is_orient_vertex; + /// @brief The indices of codimensional vertices (|CV| × 1). Eigen::VectorXi m_codim_vertices; - /// @brief The indices of codimensional edges (|CE| x 1). + /// @brief The mask of codimensional edges (|E|). + std::vector m_is_codim_edge; + /// @brief The indices of codimensional edges (|CE| × 1). Eigen::VectorXi m_codim_edges; /// @brief Edges as rows of indicies into vertices (|E| × 2). Eigen::MatrixXi m_edges; @@ -317,6 +355,8 @@ class CollisionMesh { Eigen::MatrixXi m_faces; /// @brief Map from faces edges to rows of edges (|F| × 3). Eigen::MatrixXi m_faces_to_edges; + /// @brief Map from edges to adjacent faces (|E| × 2). + Eigen::MatrixXi m_edges_to_faces; /// @brief Map from full vertices to collision vertices. /// @note Negative values indicate full vertex is dropped. @@ -343,8 +383,10 @@ class CollisionMesh { /// @brief Vertices adjacent to edges std::vector> m_edge_vertex_adjacencies; - // std::vector> m_vertices_to_faces; - // std::vector> m_vertices_to_edges; + /// @brief For each vertex, the faces adjacent to it. + std::vector> m_vertices_to_faces; + /// @brief For each vertex, the edges adjacent to it. + std::vector> m_vertices_to_edges; /// @brief Is vertex on the boundary of the triangle mesh in 3D or polyline in 2D? std::vector m_is_vertex_on_boundary; diff --git a/src/ipc/collisions/tangential/edge_edge.cpp b/src/ipc/collisions/tangential/edge_edge.cpp index f997c6315..56dfc9dd6 100644 --- a/src/ipc/collisions/tangential/edge_edge.cpp +++ b/src/ipc/collisions/tangential/edge_edge.cpp @@ -15,6 +15,15 @@ EdgeEdgeTangentialCollision::EdgeEdgeTangentialCollision( this->weight_gradient = collision.weight_gradient; } +EdgeEdgeTangentialCollision::EdgeEdgeTangentialCollision( + const EdgeEdgeNormalCollision& collision, + Eigen::ConstRef positions, + const double normal_force) + : EdgeEdgeTangentialCollision(collision) +{ + TangentialCollision::init(collision, positions, normal_force); +} + EdgeEdgeTangentialCollision::EdgeEdgeTangentialCollision( const EdgeEdgeNormalCollision& collision, Eigen::ConstRef positions, diff --git a/src/ipc/collisions/tangential/edge_edge.hpp b/src/ipc/collisions/tangential/edge_edge.hpp index 37923477e..f032face1 100644 --- a/src/ipc/collisions/tangential/edge_edge.hpp +++ b/src/ipc/collisions/tangential/edge_edge.hpp @@ -13,6 +13,11 @@ class EdgeEdgeTangentialCollision : public EdgeEdgeCandidate, EdgeEdgeTangentialCollision(const EdgeEdgeNormalCollision& collision); + EdgeEdgeTangentialCollision( + const EdgeEdgeNormalCollision& collision, + Eigen::ConstRef positions, + const double normal_force); + EdgeEdgeTangentialCollision( const EdgeEdgeNormalCollision& collision, Eigen::ConstRef positions, diff --git a/src/ipc/collisions/tangential/edge_vertex.cpp b/src/ipc/collisions/tangential/edge_vertex.cpp index da126a791..97e520abf 100644 --- a/src/ipc/collisions/tangential/edge_vertex.cpp +++ b/src/ipc/collisions/tangential/edge_vertex.cpp @@ -15,6 +15,15 @@ EdgeVertexTangentialCollision::EdgeVertexTangentialCollision( this->weight_gradient = collision.weight_gradient; } +EdgeVertexTangentialCollision::EdgeVertexTangentialCollision( + const EdgeVertexNormalCollision& collision, + Eigen::ConstRef positions, + const double normal_force) + : EdgeVertexTangentialCollision(collision) +{ + TangentialCollision::init(collision, positions, normal_force); +} + EdgeVertexTangentialCollision::EdgeVertexTangentialCollision( const EdgeVertexNormalCollision& collision, Eigen::ConstRef positions, diff --git a/src/ipc/collisions/tangential/edge_vertex.hpp b/src/ipc/collisions/tangential/edge_vertex.hpp index 9856acd8b..70bd1798d 100644 --- a/src/ipc/collisions/tangential/edge_vertex.hpp +++ b/src/ipc/collisions/tangential/edge_vertex.hpp @@ -13,6 +13,11 @@ class EdgeVertexTangentialCollision : public EdgeVertexCandidate, EdgeVertexTangentialCollision(const EdgeVertexNormalCollision& collision); + EdgeVertexTangentialCollision( + const EdgeVertexNormalCollision& collision, + Eigen::ConstRef positions, + const double normal_force); + EdgeVertexTangentialCollision( const EdgeVertexNormalCollision& collision, Eigen::ConstRef positions, diff --git a/src/ipc/collisions/tangential/face_vertex.cpp b/src/ipc/collisions/tangential/face_vertex.cpp index c5be2fd48..0f0c01cb3 100644 --- a/src/ipc/collisions/tangential/face_vertex.cpp +++ b/src/ipc/collisions/tangential/face_vertex.cpp @@ -15,6 +15,15 @@ FaceVertexTangentialCollision::FaceVertexTangentialCollision( this->weight_gradient = collision.weight_gradient; } +FaceVertexTangentialCollision::FaceVertexTangentialCollision( + const FaceVertexNormalCollision& collision, + Eigen::ConstRef positions, + const double normal_force) + : FaceVertexTangentialCollision(collision) +{ + TangentialCollision::init(collision, positions, normal_force); +} + FaceVertexTangentialCollision::FaceVertexTangentialCollision( const FaceVertexNormalCollision& collision, Eigen::ConstRef positions, diff --git a/src/ipc/collisions/tangential/face_vertex.hpp b/src/ipc/collisions/tangential/face_vertex.hpp index f27905732..f2dd9d469 100644 --- a/src/ipc/collisions/tangential/face_vertex.hpp +++ b/src/ipc/collisions/tangential/face_vertex.hpp @@ -13,6 +13,11 @@ class FaceVertexTangentialCollision : public FaceVertexCandidate, FaceVertexTangentialCollision(const FaceVertexNormalCollision& collision); + FaceVertexTangentialCollision( + const FaceVertexNormalCollision& collision, + Eigen::ConstRef positions, + const double normal_force); + FaceVertexTangentialCollision( const FaceVertexNormalCollision& collision, Eigen::ConstRef positions, diff --git a/src/ipc/collisions/tangential/tangential_collision.cpp b/src/ipc/collisions/tangential/tangential_collision.cpp index 874526229..8b840e96b 100644 --- a/src/ipc/collisions/tangential/tangential_collision.cpp +++ b/src/ipc/collisions/tangential/tangential_collision.cpp @@ -11,6 +11,17 @@ void TangentialCollision::init( Eigen::ConstRef positions, const NormalPotential& normal_potential, const double normal_stiffness) +{ + init( + collision, positions, + normal_potential.force_magnitude( + compute_distance(positions), collision.dmin, normal_stiffness)); +} + +void TangentialCollision::init( + const NormalCollision& collision, + Eigen::ConstRef positions, + const double normal_force) { // do this to initialize dim() const int dim = collision.dim(positions.size()); @@ -18,8 +29,7 @@ void TangentialCollision::init( closest_point = compute_closest_point(positions); tangent_basis = compute_tangent_basis(positions); - normal_force_magnitude = normal_potential.force_magnitude( - compute_distance(positions), collision.dmin, normal_stiffness); + normal_force_magnitude = normal_force; } } // namespace ipc diff --git a/src/ipc/collisions/tangential/tangential_collision.hpp b/src/ipc/collisions/tangential/tangential_collision.hpp index dcb71f28a..1b3d6e0da 100644 --- a/src/ipc/collisions/tangential/tangential_collision.hpp +++ b/src/ipc/collisions/tangential/tangential_collision.hpp @@ -2,6 +2,7 @@ #include #include +#include #include #include @@ -20,6 +21,15 @@ class TangentialCollision : virtual public CollisionStencil { const NormalPotential& normal_potential, const double normal_stiffness); + /// @brief Initialize the collision. + /// @param collision NormalCollision stencil. + /// @param positions Collision stencil's vertex positions. + /// @param normal_force Magnitude of the normal force. + void init( + const NormalCollision& collision, + Eigen::ConstRef positions, + const double normal_force); + public: virtual ~TangentialCollision() = default; @@ -85,6 +95,9 @@ class TangentialCollision : virtual public CollisionStencil { /// @brief Normal force magnitude double normal_force_magnitude = 0; + /// @brief SmoothCollision instance to compute normal force magnitude and its derivatives + std::shared_ptr smooth_collision; + /// @brief Ratio between normal and static tangential forces (e.g., friction coefficient) double mu_s = 0; diff --git a/src/ipc/collisions/tangential/tangential_collisions.cpp b/src/ipc/collisions/tangential/tangential_collisions.cpp index 466b32e88..6eb77115c 100644 --- a/src/ipc/collisions/tangential/tangential_collisions.cpp +++ b/src/ipc/collisions/tangential/tangential_collisions.cpp @@ -53,9 +53,9 @@ void TangentialCollisions::build( normal_stiffness); const auto& [vi, e0i, e1i, _] = FC_ev.back().vertex_ids(edges, faces); - const double edge_mu = + const double edge_mu_s = (mu_s(e1i) - mu_s(e0i)) * FC_ev.back().closest_point[0] + mu_s(e0i); - FC_ev.back().mu_s = blend_mu(edge_mu, mu_s(vi)); + FC_ev.back().mu_s = blend_mu(edge_mu_s, mu_s(vi)); const double edge_mu_k = (mu_k(e1i) - mu_k(e0i)) * FC_ev.back().closest_point[0] + mu_k(e0i); FC_ev.back().mu_k = blend_mu(edge_mu_k, mu_k(vi)); @@ -114,6 +114,193 @@ void TangentialCollisions::build( } } +void TangentialCollisions::build( + const CollisionMesh& mesh, + const Eigen::MatrixXd& vertices, + const SmoothCollisions& collisions, + const SmoothContactParameters& params, + const double normal_stiffness, + Eigen::ConstRef mu_s, + Eigen::ConstRef mu_k, + const std::function& blend_mu) +{ + assert(mu_k.size() == vertices.rows()); + assert(mu_s.size() == vertices.rows()); + + const Eigen::MatrixXi& edges = mesh.edges(); + const Eigen::MatrixXi& faces = mesh.faces(); + + clear(); + + auto& [FC_vv, FC_ev, FC_ee, FC_fv] = *this; + + // FC_vv.reserve(C_vv.size()); + for (size_t i = 0; i < collisions.size(); i++) { + const auto& cc = collisions[i]; + Eigen::VectorXd contact_potential_grad = + cc.gradient(cc.dof(vertices), params); + const double contact_force = + normal_stiffness * contact_potential_grad.norm(); + + if (mesh.dim() == 3) { + TangentialCollision* ptr = nullptr; + if (const auto* const cvv = dynamic_cast< + const SmoothCollisionTemplate*>(&cc)) { + Eigen::VectorXd collision_points = cvv->core_dof(vertices); + FC_vv.emplace_back( + VertexVertexNormalCollision( + cc[0], cc[1], 1., Eigen::SparseVector()), + collision_points, contact_force); + const auto& [v0i, v1i, _, __] = + FC_vv.back().vertex_ids(edges, faces); + + FC_vv.back().mu_k = blend_mu(mu_k(v0i), mu_k(v1i)); + FC_vv.back().mu_s = blend_mu(mu_s(v0i), mu_s(v1i)); + ptr = &(FC_vv.back()); + } else if ( + const auto* const cev = + dynamic_cast*>( + &cc)) { + Eigen::VectorXd collision_points = cev->core_dof(vertices); + collision_points = + collision_points({ 6, 7, 8, 0, 1, 2, 3, 4, 5 }) + .eval(); // {edge, point} -> {point, edge} + FC_ev.emplace_back( + EdgeVertexNormalCollision( + cc[0], cc[1], 1., Eigen::SparseVector()), + collision_points, contact_force); + const auto& [vi, e0i, e1i, _] = + FC_ev.back().vertex_ids(edges, faces); + + const double edge_mu_k = + (mu_k(e1i) - mu_k(e0i)) * FC_ev.back().closest_point[0] + + mu_k(e0i); + FC_ev.back().mu_k = blend_mu(edge_mu_k, mu_k(vi)); + + const double edge_mu_s = + (mu_s(e1i) - mu_s(e0i)) * FC_ev.back().closest_point[0] + + mu_s(e0i); + FC_ev.back().mu_s = blend_mu(edge_mu_s, mu_s(vi)); + + ptr = &(FC_ev.back()); + } else if ( + const auto* const cee = + dynamic_cast*>( + &cc)) { + Eigen::VectorXd collision_points = cee->core_dof(vertices); + const auto vert_ids = cee->core_vertex_ids(); + const Eigen::Vector3d ea0 = vertices.row(vert_ids[0]); + const Eigen::Vector3d ea1 = vertices.row(vert_ids[1]); + const Eigen::Vector3d eb0 = vertices.row(vert_ids[2]); + const Eigen::Vector3d eb1 = vertices.row(vert_ids[3]); + + // Skip EE collisions that are close to parallel + if (edge_edge_cross_squarednorm(ea0, ea1, eb0, eb1) + < edge_edge_mollifier_threshold(ea0, ea1, eb0, eb1)) { + continue; + } + + FC_ee.emplace_back( + EdgeEdgeNormalCollision( + cc[0], cc[1], 0., EdgeEdgeDistanceType::EA_EB), + collision_points, contact_force); + + double ea_mu_k = (mu_k(vert_ids[1]) - mu_k(vert_ids[0])) + * FC_ee.back().closest_point[0] + + mu_k(vert_ids[0]); + double eb_mu_k = (mu_k(vert_ids[3]) - mu_k(vert_ids[2])) + * FC_ee.back().closest_point[1] + + mu_k(vert_ids[2]); + FC_ee.back().mu_k = blend_mu(ea_mu_k, eb_mu_k); + + double ea_mu_s = (mu_s(vert_ids[1]) - mu_s(vert_ids[0])) + * FC_ee.back().closest_point[0] + + mu_s(vert_ids[0]); + double eb_mu_s = (mu_s(vert_ids[3]) - mu_s(vert_ids[2])) + * FC_ee.back().closest_point[1] + + mu_s(vert_ids[2]); + FC_ee.back().mu_s = blend_mu(ea_mu_s, eb_mu_s); + + ptr = &(FC_ee.back()); + } else if ( + const auto* const cfv = + dynamic_cast*>( + &cc)) { + Eigen::VectorXd collision_points = cfv->core_dof(vertices); + collision_points = + collision_points({ 9, 10, 11, 0, 1, 2, 3, 4, 5, 6, 7, 8 }) + .eval(); // {face, point} -> {point, face} + FC_fv.emplace_back( + FaceVertexNormalCollision( + cc[0], cc[1], 1., Eigen::SparseVector()), + collision_points, contact_force); + const auto& [vi, f0i, f1i, f2i] = + FC_fv.back().vertex_ids(edges, faces); + + double face_mu_k = mu_k(f0i) + + FC_fv.back().closest_point[0] * (mu_k(f1i) - mu_k(f0i)) + + FC_fv.back().closest_point[1] * (mu_k(f2i) - mu_k(f0i)); + FC_fv.back().mu_k = blend_mu(face_mu_k, mu_k(vi)); + + double face_mu_s = mu_s(f0i) + + FC_fv.back().closest_point[0] * (mu_s(f1i) - mu_s(f0i)) + + FC_fv.back().closest_point[1] * (mu_s(f2i) - mu_s(f0i)); + FC_fv.back().mu_s = blend_mu(face_mu_s, mu_s(vi)); + + ptr = &(FC_fv.back()); + } + if (ptr) { + ptr->smooth_collision = collisions.collisions[i]; + } + } else { + TangentialCollision* ptr = nullptr; + if (const auto* const cvv = dynamic_cast< + const SmoothCollisionTemplate*>(&cc)) { + Eigen::VectorXd collision_points = cvv->core_dof(vertices); + FC_vv.emplace_back( + VertexVertexNormalCollision( + cc[0], cc[1], 1., Eigen::SparseVector()), + collision_points, contact_force); + const auto& [v0i, v1i, _, __] = + FC_vv.back().vertex_ids(edges, faces); + + FC_vv.back().mu_s = blend_mu(mu_s(v0i), mu_s(v1i)); + FC_vv.back().mu_k = blend_mu(mu_k(v0i), mu_k(v1i)); + ptr = &(FC_vv.back()); + } else if ( + const auto* const cev = + dynamic_cast*>( + &cc)) { + Eigen::VectorXd collision_points = cev->core_dof(vertices); + collision_points = + collision_points({ 4, 5, 0, 1, 2, 3 }) + .eval(); // {edge, point} -> {point, edge} + FC_ev.emplace_back( + EdgeVertexNormalCollision( + cc[0], cc[1], 1., Eigen::SparseVector()), + collision_points, contact_force); + const auto& [vi, e0i, e1i, _] = + FC_ev.back().vertex_ids(edges, faces); + + const double edge_mu_k = + (mu_k(e1i) - mu_k(e0i)) * FC_ev.back().closest_point[0] + + mu_k(e0i); + FC_ev.back().mu_k = blend_mu(edge_mu_k, mu_k(vi)); + + const double edge_mu_s = + (mu_s(e1i) - mu_s(e0i)) * FC_ev.back().closest_point[0] + + mu_s(e0i); + FC_ev.back().mu_s = blend_mu(edge_mu_s, mu_s(vi)); + + ptr = &(FC_ev.back()); + } + if (ptr) { + ptr->smooth_collision = collisions.collisions[i]; + } + } + } +} + // ============================================================================ size_t TangentialCollisions::size() const diff --git a/src/ipc/collisions/tangential/tangential_collisions.hpp b/src/ipc/collisions/tangential/tangential_collisions.hpp index 5ecf7db22..b117fc9f4 100644 --- a/src/ipc/collisions/tangential/tangential_collisions.hpp +++ b/src/ipc/collisions/tangential/tangential_collisions.hpp @@ -7,6 +7,7 @@ #include #include #include +#include #include #include @@ -85,6 +86,26 @@ class TangentialCollisions { const std::function& blend_mu = default_blend_mu); + /// @brief Build the tangential collisions for smooth contact. + /// @param mesh The collision mesh. + /// @param vertices The vertices of the mesh. + /// @param collisions The set of normal collisions. + /// @param params Parameters of Geometric Contact Potential + /// @param normal_stiffness Stiffness of the normal potential. + /// @param mu_s The static friction coefficient per vertex. + /// @param mu_k The kinetic friction coefficient per vertex. + /// @param blend_mu Function to blend vertex-based coefficients of friction. Defaults to average. + void build( + const CollisionMesh& mesh, + const Eigen::MatrixXd& vertices, + const SmoothCollisions& collisions, + const SmoothContactParameters& params, + const double normal_stiffness, + Eigen::ConstRef mu_s, + Eigen::ConstRef mu_k, + const std::function& blend_mu = + default_blend_mu); + // ------------------------------------------------------------------------ /// @brief Get the number of friction collisions. diff --git a/src/ipc/collisions/tangential/vertex_vertex.cpp b/src/ipc/collisions/tangential/vertex_vertex.cpp index 95036f807..2075627e7 100644 --- a/src/ipc/collisions/tangential/vertex_vertex.cpp +++ b/src/ipc/collisions/tangential/vertex_vertex.cpp @@ -15,6 +15,15 @@ VertexVertexTangentialCollision::VertexVertexTangentialCollision( this->weight_gradient = collision.weight_gradient; } +VertexVertexTangentialCollision::VertexVertexTangentialCollision( + const VertexVertexNormalCollision& collision, + Eigen::ConstRef positions, + const double normal_force) + : VertexVertexTangentialCollision(collision) +{ + TangentialCollision::init(collision, positions, normal_force); +} + VertexVertexTangentialCollision::VertexVertexTangentialCollision( const VertexVertexNormalCollision& collision, Eigen::ConstRef positions, diff --git a/src/ipc/collisions/tangential/vertex_vertex.hpp b/src/ipc/collisions/tangential/vertex_vertex.hpp index 85c8fc01d..92e1f0ae3 100644 --- a/src/ipc/collisions/tangential/vertex_vertex.hpp +++ b/src/ipc/collisions/tangential/vertex_vertex.hpp @@ -14,6 +14,11 @@ class VertexVertexTangentialCollision : public VertexVertexCandidate, VertexVertexTangentialCollision( const VertexVertexNormalCollision& collision); + VertexVertexTangentialCollision( + const VertexVertexNormalCollision& collision, + Eigen::ConstRef positions, + const double normal_force); + VertexVertexTangentialCollision( const VertexVertexNormalCollision& collision, Eigen::ConstRef positions, diff --git a/src/ipc/config.hpp.in b/src/ipc/config.hpp.in index 61fb05ad6..80c9626e3 100644 --- a/src/ipc/config.hpp.in +++ b/src/ipc/config.hpp.in @@ -16,6 +16,8 @@ #cmakedefine IPC_TOOLKIT_WITH_ROBIN_MAP #cmakedefine IPC_TOOLKIT_WITH_ABSEIL #cmakedefine IPC_TOOLKIT_WITH_FILIB +#define IPC_TOOLKIT_WITH_TBB +// #define IPC_TOOLKIT_DEBUG_AUTODIFF namespace ipc { diff --git a/src/ipc/distance/distance_type.hpp b/src/ipc/distance/distance_type.hpp index 3c04c9396..3f4b877c6 100644 --- a/src/ipc/distance/distance_type.hpp +++ b/src/ipc/distance/distance_type.hpp @@ -4,6 +4,12 @@ namespace ipc { +/// @brief Closest pair between a point and point. +enum class PointPointDistanceType : uint8_t { + P_P = 0, ///< The points are closest to each other. + AUTO = 0 ///< Automatically determine the closest pair. (Same as P_P) +}; + /// @brief Closest pair between a point and edge. enum class PointEdgeDistanceType : uint8_t { P_E0, ///< The point is closest to edge vertex zero. diff --git a/src/ipc/distance/edge_edge.hpp b/src/ipc/distance/edge_edge.hpp index 0dabd4777..0f4f9bf40 100644 --- a/src/ipc/distance/edge_edge.hpp +++ b/src/ipc/distance/edge_edge.hpp @@ -4,7 +4,7 @@ namespace ipc { -/// @brief Compute the distance between a two lines segments in 3D. +/// @brief Compute the distance between two lines segments in 3D. /// @note The distance is actually squared distance. /// @param ea0 The first vertex of the first edge. /// @param ea1 The second vertex of the first edge. diff --git a/src/ipc/potentials/potential.cpp b/src/ipc/potentials/potential.cpp index 325cc2711..4b3250791 100644 --- a/src/ipc/potentials/potential.cpp +++ b/src/ipc/potentials/potential.cpp @@ -3,15 +3,36 @@ #include #include #include +#include #include #include #include #include +#include #include namespace ipc { +namespace { + void set_triplets( + const Eigen::SparseMatrix& M, + std::vector>& triplets, + const size_t start_index) + { + using InnerIterator = Eigen::SparseMatrix::InnerIterator; + assert(start_index + M.nonZeros() <= triplets.size()); + int count = 0; + for (int k = 0; k < M.outerSize(); ++k) { + for (InnerIterator it(M, k); it; ++it) { + assert(count < M.nonZeros()); + triplets[start_index + count++] = + Eigen::Triplet(it.row(), it.col(), it.value()); + } + } + } +} // namespace + template double Potential::operator()( const TCollisions& collisions, @@ -56,10 +77,10 @@ Eigen::VectorXd Potential::gradient( for (size_t i = r.begin(); i < r.end(); i++) { const TCollision& collision = collisions[i]; - const VectorMax12d local_grad = this->gradient( + const VectorMaxNd local_grad = this->gradient( collision, collision.dof(X, mesh.edges(), mesh.faces())); - const std::array vids = + const std::array vids = collision.vertex_ids(mesh.edges(), mesh.faces()); local_gradient_to_global_gradient( @@ -91,47 +112,90 @@ Eigen::SparseMatrix Potential::hessian( const int dim = X.cols(); const int ndof = X.size(); - tbb::enumerable_thread_specific>> - storage; - - tbb::parallel_for( - tbb::blocked_range(size_t(0), collisions.size()), - [&](const tbb::blocked_range& r) { - auto& hess_triplets = storage.local(); + constexpr int MAX_TRIPLETS_SIZE = 10'000'000; + const int buffer_size = std::min(MAX_TRIPLETS_SIZE, ndof); + auto storage = + create_thread_storage(LocalThreadMatStorage(buffer_size, ndof, ndof)); + maybe_parallel_for( + collisions.size(), [&](int start, int end, int thread_id) { + auto& hess_triplets = get_local_thread_storage(storage, thread_id); - for (size_t i = r.begin(); i < r.end(); i++) { + for (size_t i = start; i < end; i++) { const TCollision& collision = collisions[i]; - const MatrixMax12d local_hess = this->hessian( + const MatrixMaxNd local_hess = this->hessian( collisions[i], collisions[i].dof(X, edges, faces), project_hessian_to_psd); - const std::array vids = + const std::array vids = collision.vertex_ids(edges, faces); local_hessian_to_global_triplets( - local_hess, vids, dim, hess_triplets); + local_hess, vids, dim, *(hess_triplets.cache)); } }); - // Combine the local hessians - tbb::combinable> hess( - Eigen::SparseMatrix(ndof, ndof)); + if (storage.empty()) { + return Eigen::SparseMatrix(); + } - tbb::parallel_for( - tbb::blocked_range( - storage.begin(), storage.end()), - [&](const tbb::blocked_range& r) { - for (auto it = r.begin(); it != r.end(); ++it) { - Eigen::SparseMatrix local_hess(ndof, ndof); - local_hess.setFromTriplets(it->begin(), it->end()); - hess.local() += local_hess; - } - }); + // Assemble the stiffness matrix by concatenating the tuples in each local + // storage + + tbb::parallel_for_each( + storage.begin(), storage.end(), + [](const auto& local_storage) { local_storage.cache->prune(); }); + + // Prepares for parallel concatenation + std::vector offsets(storage.size()); + + size_t index = 0; + size_t triplet_count = 0; + for (auto& local_storage : storage) { + offsets[index++] = triplet_count; + triplet_count += local_storage.cache->triplet_count(); + } + + std::vector> triplets; + + Eigen::SparseMatrix hess(ndof, ndof); + if (triplet_count >= triplets.max_size()) { + // Serial fallback version in case the vector of triplets cannot be + // allocated + logger().warn( + "Unable to allocate sufficient memory for triplets. " + "Falling back to serial assembly, which may impact performance. " + "Consider reducing the problem size or optimizing memory usage."); + // Serially merge local storages + for (LocalThreadMatStorage& local_storage : storage) { + hess += local_storage.cache->get_matrix(false); // will also prune + } + hess.makeCompressed(); + return hess; + } + + triplets.resize(triplet_count); + + // Parallel copy into triplets + tbb::parallel_for(size_t(0), storage.size(), [&](size_t i) { + const SparseMatrixCache& cache = dynamic_cast( + *((storage.begin() + i)->cache)); + size_t offset = offsets[i]; + + std::copy( + cache.entries().begin(), cache.entries().end(), + triplets.begin() + offset); + offset += cache.entries().size(); + + if (cache.mat().nonZeros() > 0) { + set_triplets(cache.mat(), triplets, offset); + } + }); + + // Sort and assemble + hess.setFromTriplets(triplets.begin(), triplets.end()); - return hess.combine( - [](const Eigen::SparseMatrix& a, - const Eigen::SparseMatrix& b) { return a + b; }); + return hess; } template class Potential; diff --git a/src/ipc/potentials/potential.hpp b/src/ipc/potentials/potential.hpp index 23dc7744b..3b9d815a9 100644 --- a/src/ipc/potentials/potential.hpp +++ b/src/ipc/potentials/potential.hpp @@ -10,6 +10,10 @@ namespace ipc { template class Potential { protected: using TCollision = typename TCollisions::value_type; + /// @brief Maximum degrees of freedom per collision + static constexpr int STENCIL_NDOF = 3 * TCollision::STENCIL_SIZE; + using VectorMaxNd = Vector; + using MatrixMaxNd = MatrixMax; public: Potential() = default; @@ -57,23 +61,23 @@ template class Potential { /// @param x The collision stencil's degrees of freedom. /// @return The potential. virtual double operator()( - const TCollision& collision, Eigen::ConstRef x) const = 0; + const TCollision& collision, Eigen::ConstRef x) const = 0; /// @brief Compute the gradient of the potential for a single collision. /// @param collision The collision. /// @param x The collision stencil's degrees of freedom. /// @return The gradient of the potential. - virtual VectorMax12d gradient( - const TCollision& collision, Eigen::ConstRef x) const = 0; + virtual VectorMaxNd gradient( + const TCollision& collision, Eigen::ConstRef x) const = 0; /// @brief Compute the hessian of the potential for a single collision. /// @param collision The collision. /// @param x The collision stencil's degrees of freedom. /// @param project_hessian_to_psd Whether to project the hessian to the positive semi-definite cone. /// @return The hessian of the potential. - virtual MatrixMax12d hessian( + virtual MatrixMaxNd hessian( const TCollision& collision, - Eigen::ConstRef x, + Eigen::ConstRef x, const PSDProjectionMethod project_hessian_to_psd = PSDProjectionMethod::NONE) const = 0; }; diff --git a/src/ipc/potentials/tangential_potential.cpp b/src/ipc/potentials/tangential_potential.cpp index 17ed25b4a..798a9de28 100644 --- a/src/ipc/potentials/tangential_potential.cpp +++ b/src/ipc/potentials/tangential_potential.cpp @@ -494,4 +494,366 @@ MatrixMax12d TangentialPotential::force_jacobian( return J; } +Eigen::VectorXd TangentialPotential::smooth_contact_force( + const TangentialCollisions& collisions, + const CollisionMesh& mesh, + Eigen::ConstRef rest_positions, + Eigen::ConstRef lagged_displacements, + Eigen::ConstRef velocities, + const double dmin, + const bool no_mu) const +{ + if (collisions.empty()) { + return Eigen::VectorXd::Zero(velocities.size()); + } + + const int dim = velocities.cols(); + const Eigen::MatrixXi& edges = mesh.edges(); + const Eigen::MatrixXi& faces = mesh.faces(); + + tbb::enumerable_thread_specific storage( + Eigen::VectorXd::Zero(velocities.size())); + + tbb::parallel_for( + tbb::blocked_range(size_t(0), collisions.size()), + [&](const tbb::blocked_range& r) { + Eigen::VectorXd& global_force = storage.local(); + for (size_t i = r.begin(); i < r.end(); i++) { + const auto& collision = collisions[i]; + + const VectorMaxNd local_force = smooth_contact_force( + collision, collision.dof(rest_positions, edges, faces), + collision.dof(lagged_displacements, edges, faces), + collision.dof(velocities, edges, faces), no_mu); + + const auto vis = + collision.vertex_ids(mesh.edges(), mesh.faces()); + + local_gradient_to_global_gradient( + local_force, vis, dim, global_force); + } + }); + + return storage.combine([](const Eigen::VectorXd& a, + const Eigen::VectorXd& b) { return a + b; }); +} + +Eigen::SparseMatrix TangentialPotential::smooth_contact_force_jacobian( + const TangentialCollisions& collisions, + const CollisionMesh& mesh, + Eigen::ConstRef rest_positions, + Eigen::ConstRef lagged_displacements, + Eigen::ConstRef velocities, + const SmoothContactParameters& params, + const DiffWRT wrt, + const double dmin, + const bool no_mu) const +{ + if (collisions.empty()) { + return Eigen::SparseMatrix( + velocities.size(), velocities.size()); + } + + const int dim = velocities.cols(); + const Eigen::MatrixXi& edges = mesh.edges(); + const Eigen::MatrixXi& faces = mesh.faces(); + + tbb::enumerable_thread_specific>> + storage; + + const Eigen::MatrixXd lagged_positions = + rest_positions + lagged_displacements; + + tbb::parallel_for( + tbb::blocked_range(size_t(0), collisions.size()), + [&](const tbb::blocked_range& r) { + auto& jac_triplets = storage.local(); + + for (size_t i = r.begin(); i < r.end(); i++) { + const TangentialCollision& collision = collisions[i]; + + // This jacobian doesn't include the derivatives of normal + // contact force + const MatrixMaxNd local_force_jacobian = + collision.normal_force_magnitude + * smooth_contact_force_jacobian_unit( + collision, + collision.dof(lagged_positions, edges, faces), + collision.dof(velocities, edges, faces), wrt, false); + // smooth_contact_force_jacobian( + // collision, collision.dof(rest_positions, edges, faces), + // collision.dof(lagged_displacements, edges, faces), + // collision.dof(velocities, edges, faces), // + // wrt); + + const auto vis = + collision.vertex_ids(mesh.edges(), mesh.faces()); + + local_hessian_to_global_triplets( + local_force_jacobian, vis, dim, jac_triplets); + + if (wrt == DiffWRT::VELOCITIES) { + continue; + } + + // The term that includes derivatives of normal contact force + const VectorMaxNd local_force = smooth_contact_force( + collision, collision.dof(rest_positions, edges, faces), + collision.dof(lagged_displacements, edges, faces), + collision.dof(velocities, edges, faces), false, true); + + // normal_force_grad is the gradient of contact force norm + Eigen::VectorXd normal_force_grad; + std::vector cc_vert_ids; + Eigen::MatrixXd Xt = rest_positions + lagged_displacements; + auto cc = collision.smooth_collision; + const Eigen::VectorXd contact_grad = + cc->gradient(cc->dof(Xt), params); + const Eigen::MatrixXd contact_hess = + cc->hessian(cc->dof(Xt), params); + normal_force_grad = + (1 / contact_grad.norm()) * (contact_hess * contact_grad); + cc_vert_ids = cc->vertex_ids(); + + local_jacobian_to_global_triplets( + local_force * normal_force_grad.transpose(), vis, + cc_vert_ids, dim, jac_triplets); + } + }); + + Eigen::SparseMatrix jacobian(velocities.size(), velocities.size()); + for (const auto& local_jac_triplets : storage) { + Eigen::SparseMatrix local_jacobian( + velocities.size(), velocities.size()); + local_jacobian.setFromTriplets( + local_jac_triplets.begin(), local_jac_triplets.end()); + jacobian += local_jacobian; + } + + // This term is zero! + // if wrt == X then compute ∇ₓ w(x) + // if (wrt == DiffWRT::REST_POSITIONS) { + // for (int i = 0; i < collisions.size(); i++) { + // const FrictionCollision& collision = collisions[i]; + // assert(collision.weight_gradient.size() == + // rest_positions.size()); if (collision.weight_gradient.size() != + // rest_positions.size()) { + // throw std::runtime_error( + // "Shape derivative is not computed for friction + // collision!"); + // } + + // VectorMaxNd local_force + // = + // smooth_contact_force( + // collision, collision.dof(rest_positions, edges, faces), + // collision.dof(lagged_displacements, edges, faces), + // collision.dof(velocities, edges, faces), // + // dmin); + // assert(collision.weight != 0); + // local_force /= collision.weight; + + // Eigen::SparseVector force(rest_positions.size()); + // force.reserve(local_force.size()); + // local_gradient_to_global_gradient( + // local_force, collision.vertex_ids(edges, faces), dim, force); + + // jacobian += force * collision.weight_gradient.transpose(); + // } + // } + + return jacobian; +} + +TangentialPotential::VectorMaxNd TangentialPotential::smooth_contact_force( + const TangentialCollision& collision, + Eigen::ConstRef rest_positions, // = x + Eigen::ConstRef lagged_displacements, // = u + Eigen::ConstRef velocities, // = v + const bool no_mu, + const bool no_contact_force_multiplier) const +{ + // x is the rest position + // u is the displacment at the begginging of the lagged solve + // v is the current velocity + // + // τ = T(x + u)ᵀv is the tangential sliding velocity + // F(x, u, v) = -μ N(x + u) f₁(‖τ‖)/‖τ‖ T(x + u) τ + assert(rest_positions.size() == lagged_displacements.size()); + assert(rest_positions.size() == velocities.size()); + + // const VectorMaxNd x = dof(rest_positions, edges, faces); + // const Vector u = + // dof(lagged_displacements, edges, faces); + // const VectorMaxNd v = dof(velocities, edges, faces); + const VectorMaxNd lagged_positions = + rest_positions + lagged_displacements; // = x + + // Compute N(x + u) + // const double N = collision.compute_normal_force_magnitude( + // lagged_positions, barrier_potential, barrier_stiffness, dmin); + const double N = collision.normal_force_magnitude; + + // Compute P + const MatrixMax P = + collision.compute_tangent_basis(lagged_positions); + + // compute β + const VectorMax2d beta = collision.compute_closest_point(lagged_positions); + + // Compute Γ + const MatrixMax Gamma = + collision.relative_velocity_matrix(beta); + + // Compute T = ΓᵀP + const MatrixMax T = Gamma.transpose() * P; + + // Compute τ = PᵀΓv + const VectorMax2d tau = T.transpose() * velocities; + + // Compute f₁(‖τ‖)/‖τ‖ + const double mu_s = no_mu ? 1.0 : collision.mu_s; + const double mu_k = no_mu ? 1.0 : collision.mu_k; + const double mu_f1_over_norm_tau = mu_f1_over_x(tau.norm(), mu_s, mu_k); + + // F = -μ N f₁(‖τ‖)/‖τ‖ T τ + // NOTE: no_mu -> leave mu out of this function (i.e., assuming mu = 1) + return -collision.weight * (no_contact_force_multiplier ? 1.0 : N) + * mu_f1_over_norm_tau * T * tau; +} + +TangentialPotential::MatrixMaxNd +TangentialPotential::smooth_contact_force_jacobian_unit( + const TangentialCollision& collision, + Eigen::ConstRef lagged_positions, // = x + u^t + Eigen::ConstRef velocities, // = v + const DiffWRT wrt, + const bool no_mu) const +{ + // x is the rest position + // u is the displacment at the begginging of the lagged solve + // v is the current velocity + // + // τ = T(x + u)ᵀv is the tangential sliding velocity + // F(x, u, v) = -μ f₁(‖τ‖)/‖τ‖ T(x + u) τ + // + // 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 bool need_jac_N_or_T = wrt != DiffWRT::VELOCITIES; + + // Compute P + const MatrixMax P = + collision.compute_tangent_basis(lagged_positions); + + // Compute β + const VectorMax2d beta = collision.compute_closest_point(lagged_positions); + + // Compute Γ + const MatrixMax Gamma = + collision.relative_velocity_matrix(beta); + + // Compute T = ΓᵀP + const MatrixMax T = Gamma.transpose() * P; + + // Compute ∇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 = + 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); + } + + // Vertex-vertex does not have a closest point + if (beta.size()) { + // ∇Γ(β) = ∇ᵦΓ∇β ∈ ℝ^{d×n×n} ≡ ℝ^{nd×n} + const MatrixMax jac_beta = + 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); + } + } + } + } + + // Compute τ = PᵀΓv + const VectorMax2d tau = P.transpose() * Gamma * velocities; + + // Compute ∇τ = ∇T(x + u)ᵀv + T(x + u)ᵀ∇v + MatrixMax jac_tau; + if (need_jac_N_or_T) { + jac_tau.resize(dim - 1, n); + // Compute ∇T(x + u)ᵀv + for (int i = 0; i < n; i++) { + jac_tau.col(i) = + jac_T.middleRows(i * n, n).transpose() * velocities; + } + } else { + jac_tau = T.transpose(); // Tᵀ ∇ᵥv = Tᵀ + } + + // Compute f₁(‖τ‖)/‖τ‖ + const double tau_norm = tau.norm(); + const double mu_s = no_mu ? 1.0 : collision.mu_s; + const double mu_k = no_mu ? 1.0 : collision.mu_k; + const double f1_over_norm_tau = mu_f1_over_x(tau_norm, mu_s, mu_k); + + // Compute ∇(f₁(‖τ‖)/‖τ‖) + VectorMaxNd grad_f1_over_norm_tau; + if (tau_norm == 0) { + // lim_{x→0} f₂(x)x² = 0 + grad_f1_over_norm_tau.setZero(n); + } else { + // ∇ (f₁(‖τ‖)/‖τ‖) = (f₁'(‖τ‖)‖τ‖ - f₁(‖τ‖)) / ‖τ‖³ τᵀ ∇τ + double f2 = mu_f2_x_minus_mu_f1_over_x3(tau_norm, mu_s, mu_k); + assert(std::isfinite(f2)); + grad_f1_over_norm_tau = f2 * tau.transpose() * jac_tau; + } + + // Premultiplied values + const VectorMaxNd T_times_tau = T * tau; + + // ------------------------------------------------------------------------ + // Compute J = ∇F = ∇(-μ N f₁(‖τ‖)/‖τ‖ T τ) + MatrixMaxNd J = MatrixMaxNd::Zero(n, n); + + // + -μ N T τ [∇(f₁(‖τ‖)/‖τ‖)] + J += T_times_tau * grad_f1_over_norm_tau.transpose(); + + // + -μ 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++) { + // ∂J/∂xᵢ = ∂T/∂xᵢ * τ + J.col(i) += jac_T.middleRows(i * n, n) * scaled_tau; + } + } + + // + -μ N f₁(‖τ‖)/‖τ‖ T [∇τ] + 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 + // here to compute the gradient. Instead this should be handled outside of + // the function. For a simple multiplicitive model (∑ᵢ wᵢ Fᵢ) this can be + // done easily. + J *= -collision.weight; + + return J; +} + } // namespace ipc \ No newline at end of file diff --git a/src/ipc/potentials/tangential_potential.hpp b/src/ipc/potentials/tangential_potential.hpp index c0bf69527..40cbf66d6 100644 --- a/src/ipc/potentials/tangential_potential.hpp +++ b/src/ipc/potentials/tangential_potential.hpp @@ -7,7 +7,10 @@ namespace ipc { /// @brief A tangential dissipative potential. class TangentialPotential : public Potential { - using Super = Potential; + using Super = Potential; + using Super::STENCIL_NDOF; + using VectorMaxNd = Super::VectorMaxNd; + using MatrixMaxNd = Super::MatrixMaxNd; public: virtual ~TangentialPotential() { } @@ -71,6 +74,26 @@ class TangentialPotential : public Potential { const DiffWRT wrt, const double dmin = 0) const; + Eigen::VectorXd smooth_contact_force( + const TangentialCollisions& collisions, + const CollisionMesh& mesh, + Eigen::ConstRef rest_positions, + Eigen::ConstRef lagged_displacements, + Eigen::ConstRef velocities, + const double dmin = 0, + const bool no_mu = false) const; + + Eigen::SparseMatrix smooth_contact_force_jacobian( + const TangentialCollisions& collisions, + const CollisionMesh& mesh, + Eigen::ConstRef rest_positions, + Eigen::ConstRef lagged_displacements, + Eigen::ConstRef velocities, + const SmoothContactParameters& params, + const DiffWRT wrt, + const double dmin = 0, + const bool no_mu = false) const; + // -- Single collision methods --------------------------------------------- /// @brief Compute the potential for a single collision. @@ -140,6 +163,39 @@ class TangentialPotential : public Potential { const DiffWRT wrt, const double dmin = 0) const; + VectorMaxNd smooth_contact_force( + const TangentialCollision& collision, + Eigen::ConstRef rest_positions, // = x + Eigen::ConstRef lagged_displacements, // = u + Eigen::ConstRef velocities, // = v + const bool no_mu = false, + const bool no_contact_force_multiplier = false) const; + + Eigen::MatrixXd smooth_contact_force_jacobian( + const TangentialCollision& collision, + Eigen::ConstRef rest_positions, // = x + Eigen::ConstRef lagged_displacements, // = u + Eigen::ConstRef velocities, // = v + const DiffWRT wrt, + const bool no_mu) const; + + /// @brief Compute the friction force Jacobian assuming the contact force magnitude being 1. + /// @param collision The collision + /// @param rest_positions Rest positions of the vertices (rowwise). + /// @param lagged_displacements Previous displacements of the vertices (rowwise). + /// @param velocities Current displacements of the vertices (rowwise). + /// @param barrier_potential Barrier potential (used for normal force magnitude). + /// @param barrier_stiffness Barrier stiffness (used for normal force magnitude). + /// @param wrt Variable to differentiate the friction force with respect to. + /// @param dmin Minimum distance (used for normal force magnitude). + /// @return Friction force Jacobian + MatrixMaxNd smooth_contact_force_jacobian_unit( + const TangentialCollision& collision, + Eigen::ConstRef lagged_positions, + Eigen::ConstRef velocities, + const DiffWRT wrt, + const bool no_mu = false) const; + protected: /// @brief Compute the value of the ∫ μ(y) f₁(y) dy, where f₁ is the first derivative of the smooth mollifier. /// @param x The tangential relative speed. diff --git a/src/ipc/smooth_contact/CMakeLists.txt b/src/ipc/smooth_contact/CMakeLists.txt new file mode 100644 index 000000000..93ad869af --- /dev/null +++ b/src/ipc/smooth_contact/CMakeLists.txt @@ -0,0 +1,20 @@ +set(SOURCES + common.hpp + smooth_contact_potential.cpp + smooth_contact_potential.hpp + smooth_collisions.cpp + smooth_collisions.hpp + smooth_collisions_builder.cpp + smooth_collisions_builder.hpp +) + +source_group(TREE "${CMAKE_CURRENT_SOURCE_DIR}" PREFIX "Source Files" FILES ${SOURCES}) +target_sources(ipc_toolkit PRIVATE ${SOURCES}) + +################################################################################ +# Subfolders +################################################################################ + +add_subdirectory(collisions) +add_subdirectory(distance) +add_subdirectory(primitives) diff --git a/src/ipc/smooth_contact/collisions/CMakeLists.txt b/src/ipc/smooth_contact/collisions/CMakeLists.txt new file mode 100644 index 000000000..d9b91169e --- /dev/null +++ b/src/ipc/smooth_contact/collisions/CMakeLists.txt @@ -0,0 +1,7 @@ +set(SOURCES + smooth_collision.cpp + smooth_collision.hpp +) + +source_group(TREE "${CMAKE_CURRENT_SOURCE_DIR}" PREFIX "Source Files" FILES ${SOURCES}) +target_sources(ipc_toolkit PRIVATE ${SOURCES}) diff --git a/src/ipc/smooth_contact/collisions/smooth_collision.cpp b/src/ipc/smooth_contact/collisions/smooth_collision.cpp new file mode 100644 index 000000000..668cad2d7 --- /dev/null +++ b/src/ipc/smooth_contact/collisions/smooth_collision.cpp @@ -0,0 +1,501 @@ +#include "smooth_collision.hpp" + +#include + +namespace ipc { + +// clang-format off +template <> CollisionType SmoothCollisionTemplate::type() const { return CollisionType::VERTEX_VERTEX; } +template <> CollisionType SmoothCollisionTemplate::type() const { return CollisionType::VERTEX_VERTEX; } +template <> CollisionType SmoothCollisionTemplate::type() const { return CollisionType::EDGE_VERTEX; } +template <> CollisionType SmoothCollisionTemplate::type() const { return CollisionType::EDGE_VERTEX; } +template <> CollisionType SmoothCollisionTemplate::type() const { return CollisionType::FACE_VERTEX; } +template <> CollisionType SmoothCollisionTemplate::type() const { return CollisionType::EDGE_EDGE; } +// clang-format on + +// clang-format off +template <> std::string SmoothCollisionTemplate::name() const { return "vert-vert"; } +template <> std::string SmoothCollisionTemplate::name() const { return "vert-vert"; } +template <> std::string SmoothCollisionTemplate::name() const { return "edge-vert"; } +template <> std::string SmoothCollisionTemplate::name() const { return "edge-vert"; } +template <> std::string SmoothCollisionTemplate::name() const { return "face-vert"; } +template <> std::string SmoothCollisionTemplate::name() const { return "edge-edge"; } +// clang-format on + +Eigen::VectorXd SmoothCollision::dof(Eigen::ConstRef X) const +{ + const int DIM = X.cols(); + Eigen::VectorXd x(num_vertices() * DIM); + if (DIM == 2) { + for (int i = 0; i < num_vertices(); i++) { + x.segment<2>(i * 2) = X.row(m_vertex_ids[i]); + } + } else if (DIM == 3) { + for (int i = 0; i < num_vertices(); i++) { + x.segment<3>(i * 3) = X.row(m_vertex_ids[i]); + } + } else { + throw std::runtime_error("Invalid dimension!"); + } + return x; +} + +template +auto SmoothCollisionTemplate::get_core_indices() const + -> Vector +{ + Vector core_indices; + core_indices << Eigen::VectorXi::LinSpaced( + N_CORE_DOFS_A, 0, N_CORE_DOFS_A - 1), + Eigen::VectorXi::LinSpaced( + N_CORE_DOFS_B, primitive_a->n_dofs(), + primitive_a->n_dofs() + N_CORE_DOFS_B - 1); + return core_indices; +} + +template +SmoothCollisionTemplate::SmoothCollisionTemplate( + index_t _primitive0, + index_t _primitive1, + SmoothCollisionTemplate::DTYPE dtype, + const CollisionMesh& mesh, + const SmoothContactParameters& params, + const double _dhat, + const Eigen::MatrixXd& V) + : SmoothCollision(_primitive0, _primitive1, _dhat, mesh) +{ + VectorMax3d d = + PrimitiveDistance::compute_closest_direction( + mesh, V, _primitive0, _primitive1, dtype); + primitive_a = std::make_unique(_primitive0, mesh, V, d, params); + primitive_b = + std::make_unique(_primitive1, mesh, V, -d, params); + + if ((primitive_a->n_vertices() + primitive_b->n_vertices()) * DIM + > ELEMENT_SIZE) { + logger().error( + "Too many neighbors for collision pair! {} > {}! Increase MAX_VERT_3D in common.hpp", + primitive_a->n_vertices() + primitive_b->n_vertices(), MAX_VERT_3D); + } + + int i = 0; + m_vertex_ids.assign( + primitive_a->vertex_ids().size() + primitive_b->vertex_ids().size(), + -1); + for (auto& v : primitive_a->vertex_ids()) { + m_vertex_ids[i++] = v; + } + for (auto& v : primitive_b->vertex_ids()) { + m_vertex_ids[i++] = v; + } + assert(i == primitive_a->n_vertices() + primitive_b->n_vertices()); + m_is_active = (d.norm() < m_dhat) && primitive_a->is_active() + && primitive_b->is_active(); + + if (d.norm() < 1e-12) { + logger().warn( + "pair distance {}, id {} and {}, dtype {}, active {}", d.norm(), + _primitive0, _primitive1, + PrimitiveDistType::NAME, m_is_active); + + logger().warn("value {}", (*this)(this->dof(V), params)); + } +} + +template +double SmoothCollisionTemplate::operator()( + Eigen::ConstRef> positions, + const SmoothContactParameters& params) const +{ + Vector x; + x << positions.head(PrimitiveA::N_CORE_POINTS * DIM), + positions.segment( + primitive_a->n_dofs(), PrimitiveB::N_CORE_POINTS * DIM); + + // grad of "d" wrt. points + const Vector closest_direction = + PrimitiveDistanceTemplate:: + compute_closest_direction(x, DTYPE::AUTO); + const double dist = closest_direction.norm(); + + assert(positions.size() == primitive_a->n_dofs() + primitive_b->n_dofs()); + double a1 = primitive_a->potential( + closest_direction, positions.head(primitive_a->n_dofs())); + double a2 = primitive_b->potential( + -closest_direction, positions.tail(primitive_b->n_dofs())); + double a3 = Math::inv_barrier(dist / dhat(), params.r); + double a4 = + PrimitiveDistanceTemplate::mollifier( + x, dist * dist); + + if (params.r == 0) { + logger().error("Invalid params!"); + } + + if (dist < 1e-12) { + logger().warn( + "pair distance {:.3e}, dhat {:.3e}, r {}, barrier {:.3e}, mollifier {:.3e}, orient {:.3e} {:.3e}", + dist, dhat(), params.r, a3, a4, a1, a2); + } + + return a1 * a2 * a3 * a4; +} + +template +auto SmoothCollisionTemplate::gradient( + Eigen::ConstRef> positions, + const SmoothContactParameters& params) const + -> Vector +{ + const auto core_indices = get_core_indices(); + + Vector x; + x = positions(core_indices); + + const auto dtype = + PrimitiveDistance::compute_distance_type(x); + + Vector closest_direction; + Eigen::Matrix closest_direction_grad; + std::tie(closest_direction, closest_direction_grad) = PrimitiveDistance< + PrimitiveA, PrimitiveB>::compute_closest_direction_gradient(x, dtype); + + const double dist = closest_direction.norm(); + assert(dist > 0); + + // these two use autodiff with different variable count + auto gA_reduced = primitive_a->grad( + closest_direction, positions.head(primitive_a->n_dofs())); + auto gB_reduced = primitive_b->grad( + -closest_direction, positions.tail(primitive_b->n_dofs())); + + // gradient of barrier potential + double barrier = 0; + Vector gBarrier = Vector::Zero(); + { + barrier = Math::inv_barrier(dist / dhat(), params.r); + + const Vector closest_direction_normalized = + closest_direction / dist; + const double barrier_1st_deriv = + Math::inv_barrier_grad(dist / dhat(), params.r) / dhat(); + const Vector gBarrier_wrt_d = + barrier_1st_deriv * closest_direction_normalized; + gBarrier = closest_direction_grad.transpose() * gBarrier_wrt_d; + } + + // gradient of mollifier + { + double mollifier = 0; + Vector gMollifier = + Vector::Zero(); +#ifdef IPC_TOOLKIT_DEBUG_AUTODIFF + ScalarBase::setVariableCount(N_CORE_DOFS); + using T = ADGrad; + Vector xAD = slice_positions(x); + Vector closest_direction_autodiff = PrimitiveDistanceTemplate< + PrimitiveA, PrimitiveB, T>::compute_closest_direction(xAD, dtype); + const auto dist_sqr_AD = closest_direction_autodiff.squaredNorm(); + auto mollifier_autodiff = + PrimitiveDistanceTemplate::mollifier( + xAD, dist_sqr_AD); + mollifier = mollifier_autodiff.val; + gMollifier = mollifier_autodiff.grad; +#else + Vector mollifier_grad; + std::tie(mollifier, mollifier_grad) = PrimitiveDistance< + PrimitiveA, PrimitiveB>::compute_mollifier_gradient(x, dist * dist); + + const Vector dist_sqr_grad = + 2 * closest_direction_grad.transpose() * closest_direction; + mollifier_grad.head(N_CORE_DOFS) += + mollifier_grad(N_CORE_DOFS) * dist_sqr_grad; + gMollifier = mollifier_grad.head(N_CORE_DOFS); +#endif + // merge mollifier into barrier + gBarrier = gBarrier * mollifier + gMollifier * barrier; + barrier *= mollifier; + } + + // grad of tangent/normal terms + double orient = 0; + Vector gOrient; + { + Vector + gA = Vector::Zero(n_dofs()), + gB = Vector::Zero(n_dofs()); + { + gA(core_indices) = + closest_direction_grad.transpose() * gA_reduced.head(DIM); + gA.head(primitive_a->n_dofs()) += + gA_reduced.tail(primitive_a->n_dofs()); + + gB(core_indices) = + closest_direction_grad.transpose() * -gB_reduced.head(DIM); + gB.tail(primitive_b->n_dofs()) += + gB_reduced.tail(primitive_b->n_dofs()); + } + const double potential_a = primitive_a->potential( + closest_direction, positions.head(primitive_a->n_dofs())); + const double potential_b = primitive_b->potential( + -closest_direction, positions.tail(primitive_b->n_dofs())); + + orient = potential_a * potential_b; + gOrient = gA * potential_b + gB * potential_a; + } + + // merge barrier into orient + gOrient *= barrier; + gOrient(core_indices) += gBarrier * orient; + // orient *= barrier; + + return gOrient; +} + +template +auto SmoothCollisionTemplate::hessian( + Eigen::ConstRef> positions, + const SmoothContactParameters& params) const + -> MatrixMax +{ + const auto core_indices = get_core_indices(); + + Vector x; + x = positions(core_indices); + + const auto dtype = + PrimitiveDistance::compute_distance_type(x); + + Vector closest_direction; + Eigen::Matrix closest_direction_grad; + std::array, DIM> + closest_direction_hess; + std::tie( + closest_direction, closest_direction_grad, closest_direction_hess) = + PrimitiveDistance:: + compute_closest_direction_hessian(x, dtype); + + const double dist = closest_direction.norm(); + + // these two use autodiff with different variable count + auto gA_reduced = primitive_a->grad( + closest_direction, positions.head(primitive_a->n_dofs())); + auto hA_reduced = primitive_a->hessian( + closest_direction, positions.head(primitive_a->n_dofs())); + auto gB_reduced = primitive_b->grad( + -closest_direction, positions.tail(primitive_b->n_dofs())); + auto hB_reduced = primitive_b->hessian( + -closest_direction, positions.tail(primitive_b->n_dofs())); + + // hessian of barrier potential + double barrier = 0; + Vector gBarrier = Vector::Zero(); + Eigen::Matrix hBarrier = + Eigen::Matrix::Zero(); + { + barrier = Math::inv_barrier(dist / dhat(), params.r); + + const Vector closest_direction_normalized = + closest_direction / dist; + const double barrier_1st_deriv = + Math::inv_barrier_grad(dist / dhat(), params.r) / dhat(); + const Vector gBarrier_wrt_d = + barrier_1st_deriv * closest_direction_normalized; + gBarrier = closest_direction_grad.transpose() * gBarrier_wrt_d; + + const double barrier_2nd_deriv = + Math::inv_barrier_hess(dist / dhat(), params.r) / dhat() + / dhat(); + const Eigen::Matrix hBarrier_wrt_d = + (barrier_1st_deriv / dist) + * Eigen::Matrix::Identity() + + (barrier_2nd_deriv - barrier_1st_deriv / dist) + * closest_direction_normalized + * closest_direction_normalized.transpose(); + hBarrier = closest_direction_grad.transpose() * hBarrier_wrt_d + * closest_direction_grad; + for (int d = 0; d < DIM; d++) { + hBarrier += closest_direction_hess[d] * gBarrier_wrt_d(d); + } + } + + // hessian of mollifier + { + double mollifier = 0; + Vector gMollifier = + Vector::Zero(); + Eigen::Matrix hMollifier = + Eigen::Matrix::Zero(); +#ifdef IPC_TOOLKIT_DEBUG_AUTODIFF + ScalarBase::setVariableCount(N_CORE_DOFS); + using T = ADHessian; + Vector xAD = slice_positions(x); + Vector closest_direction_autodiff = PrimitiveDistanceTemplate< + PrimitiveA, PrimitiveB, T>::compute_closest_direction(xAD, dtype); + const auto dist_sqr_AD = closest_direction_autodiff.squaredNorm(); + auto mollifier_autodiff = + PrimitiveDistanceTemplate::mollifier( + xAD, dist_sqr_AD); + mollifier = mollifier_autodiff.val; + + gMollifier = mollifier_autodiff.grad; + hMollifier = mollifier_autodiff.Hess; +#else + Vector mollifier_grad; + Eigen::Matrix mollifier_hess; + std::tie(mollifier, mollifier_grad, mollifier_hess) = PrimitiveDistance< + PrimitiveA, PrimitiveB>::compute_mollifier_hessian(x, dist * dist); + + const Vector dist_sqr_grad = + 2 * closest_direction_grad.transpose() * closest_direction; + mollifier_grad.head(N_CORE_DOFS) += + mollifier_grad(N_CORE_DOFS) * dist_sqr_grad; + Eigen::Matrix dist_sqr_hess = + 2 * closest_direction_grad.transpose() * closest_direction_grad; + for (int d = 0; d < DIM; d++) { + dist_sqr_hess += + 2 * closest_direction(d) * closest_direction_hess[d]; + } + mollifier_hess.topLeftCorner(N_CORE_DOFS, N_CORE_DOFS) += + dist_sqr_hess * mollifier_grad(N_CORE_DOFS) + + dist_sqr_grad * mollifier_hess(N_CORE_DOFS, N_CORE_DOFS) + * dist_sqr_grad.transpose() + + dist_sqr_grad + * mollifier_hess.block(N_CORE_DOFS, 0, 1, N_CORE_DOFS) + + mollifier_hess.block(0, N_CORE_DOFS, N_CORE_DOFS, 1) + * dist_sqr_grad.transpose(); + + gMollifier = mollifier_grad.head(core_indices.size()); + hMollifier = mollifier_hess.topLeftCorner( + core_indices.size(), core_indices.size()); +#endif + // merge mollifier into barrier + hBarrier = hBarrier * mollifier + gBarrier * gMollifier.transpose() + + gMollifier * gBarrier.transpose() + hMollifier * barrier; + gBarrier = gBarrier * mollifier + gMollifier * barrier; + barrier *= mollifier; + } + + // grad of tangent/normal terms + double orient = 0; + Vector gOrient; + MatrixMax hOrient; + { + Vector + gA = Vector::Zero(n_dofs()), + gB = Vector::Zero(n_dofs()); + MatrixMax + hA = MatrixMax::Zero( + n_dofs(), n_dofs()), + hB = MatrixMax::Zero( + n_dofs(), n_dofs()); + { + gA(core_indices) = + closest_direction_grad.transpose() * gA_reduced.head(DIM); + gA.head(primitive_a->n_dofs()) += + gA_reduced.tail(primitive_a->n_dofs()); + + hA(core_indices, core_indices) = closest_direction_grad.transpose() + * hA_reduced.topLeftCorner(DIM, DIM) * closest_direction_grad; + for (int d = 0; d < DIM; d++) { + hA(core_indices, core_indices) += + gA_reduced(d) * closest_direction_hess[d]; + } + + hA.topLeftCorner(primitive_a->n_dofs(), primitive_a->n_dofs()) += + hA_reduced.bottomRightCorner( + primitive_a->n_dofs(), primitive_a->n_dofs()); + hA(core_indices, Eigen::seqN(0, primitive_a->n_dofs())) += + closest_direction_grad.transpose() + * hA_reduced.topRightCorner(DIM, primitive_a->n_dofs()); + hA(Eigen::seqN(0, primitive_a->n_dofs()), core_indices) += + hA_reduced.bottomLeftCorner(primitive_a->n_dofs(), DIM) + * closest_direction_grad; + + gB(core_indices) = + closest_direction_grad.transpose() * -gB_reduced.head(DIM); + gB.tail(primitive_b->n_dofs()) += + gB_reduced.tail(primitive_b->n_dofs()); + + hB(core_indices, core_indices) = closest_direction_grad.transpose() + * hB_reduced.topLeftCorner(DIM, DIM) * closest_direction_grad; + for (int d = 0; d < DIM; d++) { + hB(core_indices, core_indices) -= + gB_reduced(d) * closest_direction_hess[d]; + } + + hB.bottomRightCorner( + primitive_b->n_dofs(), primitive_b->n_dofs()) += + hB_reduced.bottomRightCorner( + primitive_b->n_dofs(), primitive_b->n_dofs()); + hB(core_indices, + Eigen::seqN(primitive_a->n_dofs(), primitive_b->n_dofs())) -= + closest_direction_grad.transpose() + * hB_reduced.topRightCorner(DIM, primitive_b->n_dofs()); + hB(Eigen::seqN(primitive_a->n_dofs(), primitive_b->n_dofs()), + core_indices) -= + hB_reduced.bottomLeftCorner(primitive_b->n_dofs(), DIM) + * closest_direction_grad; + } + const double potential_a = primitive_a->potential( + closest_direction, positions.head(primitive_a->n_dofs())); + const double potential_b = primitive_b->potential( + -closest_direction, positions.tail(primitive_b->n_dofs())); + + orient = potential_a * potential_b; + gOrient = gA * potential_b + gB * potential_a; + hOrient = (potential_a * hB + potential_b * hA) + + (gA * gB.transpose() + gB * gA.transpose()); + } + + // merge barrier into orient + hOrient *= barrier; + hOrient(core_indices, core_indices) += hBarrier * orient; + hOrient(Eigen::all, core_indices) += gOrient * gBarrier.transpose(); + hOrient(core_indices, Eigen::all) += gBarrier * gOrient.transpose(); + gOrient *= barrier; + gOrient(core_indices) += gBarrier * orient; + // orient *= barrier; + + // return hOrient + return (hOrient + hOrient.transpose()) / 2.; +} + +// ---- distance ---- + +template +double SmoothCollisionTemplate::compute_distance( + Eigen::ConstRef vertices) const +{ + Vector positions = dof(vertices); + + Vector x; + x << positions.head(PrimitiveA::N_CORE_POINTS * DIM), + positions.segment( + primitive_a->n_dofs(), PrimitiveB::N_CORE_POINTS * DIM); + + return PrimitiveDistanceTemplate< + PrimitiveA, PrimitiveB, double>::compute_distance(x, DTYPE::AUTO); +} + +template +auto SmoothCollisionTemplate::core_vertex_ids() const + -> std::array +{ + std::array vids {}; + auto ids = get_core_indices(); + for (int i = 0; i < N_CORE_DOFS; i++) { + vids[i] = m_vertex_ids[ids[i]]; + } + return vids; +} + +// Note: Primitive pair order cannot change +template class SmoothCollisionTemplate; +template class SmoothCollisionTemplate; + +template class SmoothCollisionTemplate; +template class SmoothCollisionTemplate; +template class SmoothCollisionTemplate; +template class SmoothCollisionTemplate; +} // namespace ipc \ No newline at end of file diff --git a/src/ipc/smooth_contact/collisions/smooth_collision.hpp b/src/ipc/smooth_contact/collisions/smooth_collision.hpp new file mode 100644 index 000000000..b9dd5e6ea --- /dev/null +++ b/src/ipc/smooth_contact/collisions/smooth_collision.hpp @@ -0,0 +1,221 @@ +#pragma once + +#include +#include +#include + +namespace ipc { + +enum class CollisionType : uint8_t { + EDGE_VERTEX, + VERTEX_VERTEX, + FACE_VERTEX, + EDGE_EDGE, +}; + +/// @brief Contact pair class for Geometric Contact Potential. +/// @note Unlike NormalCollision, SmoothCollision has to be reconstructed whenever vertices change position +class SmoothCollision { +public: + static constexpr int ELEMENT_SIZE = 3 * MAX_VERT_3D; + + SmoothCollision( + const index_t _primitive0, + const index_t _primitive1, + const double _dhat, + const CollisionMesh& mesh) + : primitive0(_primitive0) + , primitive1(_primitive1) + , m_dhat(_dhat) + { + } + + virtual ~SmoothCollision() = default; + + /// @brief Check if this contact pair is active (depending on both orientation and distance) + bool is_active() const { return m_is_active; } + + /// @brief dhat value for this contact pair + double dhat() const { return m_dhat; } + + /// @brief Name of the contact pair type + virtual std::string name() const = 0; + + /// @brief Number of vertices involved times the dimension + virtual int n_dofs() const = 0; + + /// @brief Contact pair type + virtual CollisionType type() const = 0; + + /// @brief Get the number of vertices in the collision stencil. + virtual int num_vertices() const = 0; + + /// @brief Get the vertex IDs of the collision stencil. + /// @return The vertex IDs of the collision stencil. Size is always 4, but elements i > num_vertices() are -1. + std::vector vertex_ids() const { return m_vertex_ids; } + + /// @brief Get the vertex attributes of the collision stencil. + /// @param vertices Vertex attributes + /// @return The vertex positions of the collision stencil. Size is always 4, but elements i > num_vertices() are NaN. + Eigen::MatrixXd vertices(Eigen::ConstRef vertices) const + { + const int DIM = vertices.cols(); + Eigen::MatrixXd stencil_vertices(vertex_ids().size(), DIM); + for (int i = 0; i < vertex_ids().size(); i++) { + stencil_vertices.row(i) = vertices.row(vertex_ids()[i]); + } + + return stencil_vertices; + } + + /// @brief Select this stencil's DOF from the full matrix of DOF. + /// @param X Full matrix of DOF (rowwise). + /// @return This stencil's DOF. + Eigen::VectorXd dof(Eigen::ConstRef X) const; + + /// @brief Compute the distance of the stencil. + /// @param vertices Collision mesh vertices + /// @return Squared distance of the stencil. + virtual double + compute_distance(Eigen::ConstRef vertices) const = 0; + + /// @brief Compute the value of the GCP potential + virtual double operator()( + Eigen::ConstRef> positions, + const SmoothContactParameters& params) const = 0; + + /// @brief Compute the gradient of the GCP potential wrt. vertices involved + virtual Vector gradient( + Eigen::ConstRef> positions, + const SmoothContactParameters& params) const = 0; + + /// @brief Compute the Hessian of the GCP potential wrt. vertices involved + virtual MatrixMax hessian( + Eigen::ConstRef> positions, + const SmoothContactParameters& params) const = 0; + + bool operator==(const SmoothCollision& other) const + { + return ( + primitive0 == other.primitive0 && primitive1 == other.primitive1); + } + + bool operator!=(const SmoothCollision& other) const + { + return !(*this == other); + } + + index_t operator[](int idx) const + { + if (idx == 0) { + return primitive0; + } else if (idx == 1) { + return primitive1; + } else { + throw std::runtime_error("Invalid index in smooth_collision!"); + } + } + + std::pair get_hash() const + { + return std::make_pair(primitive0, primitive1); + } + + double weight = 1; + +protected: + bool m_is_active = true; + index_t primitive0, primitive1; + double m_dhat; + std::vector m_vertex_ids; +}; + +/// @brief Templated class for various types of contact pairs +template +class SmoothCollisionTemplate : public SmoothCollision { +public: + using Super = SmoothCollision; + /// @brief Distance type of the contact pair + using DTYPE = typename PrimitiveDistType::type; + /// @brief Number of points needed to compute the distance between two primitives + static constexpr int N_CORE_POINTS = + PrimitiveA::N_CORE_POINTS + PrimitiveB::N_CORE_POINTS; + static constexpr int DIM = PrimitiveA::DIM; + static constexpr int N_CORE_DOFS_A = PrimitiveA::N_CORE_POINTS * DIM; + static constexpr int N_CORE_DOFS_B = PrimitiveB::N_CORE_POINTS * DIM; + static constexpr int N_CORE_DOFS = N_CORE_POINTS * DIM; + static constexpr int ELEMENT_SIZE = Super::ELEMENT_SIZE; + + SmoothCollisionTemplate( + index_t primitive0, + index_t primitive1, + DTYPE dtype, + const CollisionMesh& mesh, + const SmoothContactParameters& params, + const double dhat, + const Eigen::MatrixXd& V); + + virtual ~SmoothCollisionTemplate() = default; + + std::string name() const override; + + int n_dofs() const override + { + return primitive_a->n_dofs() + primitive_b->n_dofs(); + } + CollisionType type() const override; + + Vector get_core_indices() const; + std::array core_vertex_ids() const; + + int num_vertices() const override + { + return primitive_a->n_vertices() + primitive_b->n_vertices(); + } + + template + Vector core_dof(const Eigen::MatrixX& X) const + { + return this->dof(X)(get_core_indices()); + } + + // ---- non distance type potential ---- + + /// @brief Compute the GCP potential + /// @param positions Vertex positions + /// @param params GCP parameters + /// @return GCP potential value + double operator()( + Eigen::ConstRef> positions, + const SmoothContactParameters& params) const override; + + /// @brief Compute the potential gradient wrt. positions + /// @param positions Vertex positions + /// @param params GCP parameters + /// @return GCP potential gradient + Vector gradient( + Eigen::ConstRef> positions, + const SmoothContactParameters& params) const override; + + /// @brief Compute the potential Hessian wrt. positions + /// @param positions Vertex positions + /// @param params GCP parameters + /// @return GCP potential Hessian + MatrixMax hessian( + Eigen::ConstRef> positions, + const SmoothContactParameters& params) const override; + + // ---- distance ---- + + /// @brief Compute the minimum squared distance between two primitives + double + compute_distance(Eigen::ConstRef vertices) const override; + +private: + /// @brief The first primitive in the contact pair + std::unique_ptr primitive_a; + /// @brief The second primitive in the contact pair + std::unique_ptr primitive_b; +}; + +} // namespace ipc \ No newline at end of file diff --git a/src/ipc/smooth_contact/common.hpp b/src/ipc/smooth_contact/common.hpp new file mode 100644 index 000000000..e9f8679c7 --- /dev/null +++ b/src/ipc/smooth_contact/common.hpp @@ -0,0 +1,106 @@ +#pragma once +#include + +#include + +namespace ipc { + +static constexpr int N_VERT_NEIGHBORS_2D = 3; +static constexpr int N_EDGE_NEIGHBORS_2D = 2; +static constexpr int MAX_VERT_2D = + 2 * std::max(N_VERT_NEIGHBORS_2D, N_EDGE_NEIGHBORS_2D); +static constexpr int N_VERT_NEIGHBORS_3D = 20; // increase me if needed +static constexpr int N_EDGE_NEIGHBORS_3D = 4; +static constexpr int N_FACE_NEIGHBORS_3D = 3; +static constexpr int MAX_VERT_3D = N_VERT_NEIGHBORS_3D * 2; + +template class MaxVertices; +template <> class MaxVertices<2> { +public: + static constexpr int value = MAX_VERT_2D; // NOLINT +}; +template <> class MaxVertices<3> { +public: + static constexpr int value = MAX_VERT_3D; // NOLINT +}; + +struct SmoothContactParameters { + SmoothContactParameters() = default; + + SmoothContactParameters( + const double _dhat, + const double _alpha_t, + const double _beta_t, + const int _r) + : SmoothContactParameters(_dhat, _alpha_t, _beta_t, 0, 0.1, _r) + { + } + + SmoothContactParameters( + const double _dhat, + const double _alpha_t, + const double _beta_t, + const double _alpha_n, + const double _beta_n, + const int _r) + : dhat(_dhat) + , alpha_t(_alpha_t) + , beta_t(_beta_t) + , alpha_n(_alpha_n) + , beta_n(_beta_n) + , r(_r) + { + if (r <= 0) { + logger().error("Parameter 'r' must be greater than 0! r: {}", r); + } + if (dhat <= 0) { + logger().error( + "Parameter 'dhat' must be greater than 0! dhat: {}", dhat); + } + if (abs(alpha_t) > 1) { + logger().error( + "Parameter 'alpha_t' must be in [-1, 1]! alpha_t: {}", alpha_t); + } + if (abs(alpha_n) > 1) { + logger().error( + "Parameter 'alpha_n' must be in [-1, 1]! alpha_n: {}", alpha_n); + } + if (abs(beta_t) > 1) { + logger().error( + "Parameter 'beta_t' must be in [-1, 1]! beta_t: {}", beta_t); + } + if (abs(beta_n) > 1) { + logger().error( + "Parameter 'beta_n' must be in [-1, 1]! beta_n: {}", beta_n); + } + if (beta_t + alpha_t <= 1e-6) { + logger().error( + "Sum of 'beta_t' and 'alpha_t' must be greater than 1e-6! beta_t: {}, alpha_t: {}", + beta_t, alpha_t); + } + if (beta_n + alpha_n <= 1e-6) { + logger().error( + "Sum of 'beta_n' and 'alpha_n' must be greater than 1e-6! beta_n: {}, alpha_n: {}", + beta_n, alpha_n); + } + } + + double adaptive_dhat_ratio() const { return m_adaptive_dhat_ratio; } + + void set_adaptive_dhat_ratio(const double adaptive_dhat_ratio) + { + m_adaptive_dhat_ratio = adaptive_dhat_ratio; + } + + double dhat = 1; + double alpha_t = 1; + double beta_t = 0; + double alpha_n = 0.1; + double beta_n = 0; + int r = 2; + +private: + double m_adaptive_dhat_ratio = 0.5; +}; + +} // namespace ipc \ No newline at end of file diff --git a/src/ipc/smooth_contact/distance/CMakeLists.txt b/src/ipc/smooth_contact/distance/CMakeLists.txt new file mode 100644 index 000000000..b0b65f5df --- /dev/null +++ b/src/ipc/smooth_contact/distance/CMakeLists.txt @@ -0,0 +1,19 @@ +set(SOURCES + autogen.hpp + autogen.cpp + mollifier.cpp + mollifier.hpp + mollifier.tpp + point_edge.cpp + point_edge.hpp + point_face.cpp + point_face.hpp + edge_edge.cpp + edge_edge.hpp + primitive_distance.cpp + primitive_distance.hpp + primitive_distance.tpp +) + +source_group(TREE "${CMAKE_CURRENT_SOURCE_DIR}" PREFIX "Source Files" FILES ${SOURCES}) +target_sources(ipc_toolkit PRIVATE ${SOURCES}) diff --git a/src/ipc/smooth_contact/distance/autogen.cpp b/src/ipc/smooth_contact/distance/autogen.cpp new file mode 100644 index 000000000..79a58bf39 --- /dev/null +++ b/src/ipc/smooth_contact/distance/autogen.cpp @@ -0,0 +1,5143 @@ +#include "autogen.hpp" + +#include + +namespace ipc::autogen { + +// hess is (144×1) flattened in column-major order +void edge_edge_closest_point_hessian_a( + double ea0_x, + double ea0_y, + double ea0_z, + double ea1_x, + double ea1_y, + double ea1_z, + double eb0_x, + double eb0_y, + double eb0_z, + double eb1_x, + double eb1_y, + double eb1_z, + double hess[144]) +{ + const auto t0 = ea0_y * eb0_y; + const auto t1 = ea0_x * eb0_x; + const auto t2 = 2 * t1; + const auto t3 = ea0_y * eb1_y; + const auto t4 = ea0_x * eb1_x; + const auto t5 = 2 * t4; + const auto t6 = ea0_z * eb0_z; + const auto t7 = ea0_z * eb1_z; + const auto t8 = ea0_x * eb0_y; + const auto t9 = ea1_x * eb1_y; + const auto t10 = ea0_x * eb0_z; + const auto t11 = ea1_x * eb1_z; + const auto t12 = ea1_y * eb0_y; + const auto t13 = ea1_y * eb1_y; + const auto t14 = ea1_z * eb0_z; + const auto t15 = ea1_z * eb1_z; + const auto t16 = 2 * t0; + const auto t17 = 2 * t3; + const auto t18 = ea1_x * eb0_x; + const auto t19 = ea1_x * eb1_x; + const auto t20 = ea0_y * eb1_x; + const auto t21 = ea1_y * eb0_x; + const auto t22 = ea0_y * eb0_z; + const auto t23 = ea1_y * eb1_z; + const auto t24 = 2 * t6; + const auto t25 = 2 * t7; + const auto t26 = ea0_z * eb1_x; + const auto t27 = ea1_z * eb0_x; + const auto t28 = ea0_z * eb1_y; + const auto t29 = ea1_z * eb0_y; + const auto t30 = 2 * t18; + const auto t31 = 2 * t19; + const auto t32 = 2 * t12; + const auto t33 = 2 * t13; + const auto t34 = std::pow(ea0_x, 2); + const auto t35 = std::pow(eb0_y, 2); + const auto t36 = std::pow(eb0_z, 2); + const auto t37 = std::pow(eb1_y, 2); + const auto t38 = std::pow(eb1_z, 2); + const auto t39 = std::pow(ea0_y, 2); + const auto t40 = std::pow(eb0_x, 2); + const auto t41 = std::pow(eb1_x, 2); + const auto t42 = std::pow(ea0_z, 2); + const auto t43 = std::pow(ea1_x, 2); + const auto t44 = std::pow(ea1_y, 2); + const auto t45 = std::pow(ea1_z, 2); + const auto t46 = ea0_x * t35; + const auto t47 = 2 * ea1_x; + const auto t48 = ea0_x * t36; + const auto t49 = ea0_x * t37; + const auto t50 = ea0_x * t38; + const auto t51 = 2 * eb0_y; + const auto t52 = eb1_y * t34; + const auto t53 = 2 * eb0_z; + const auto t54 = eb1_z * t34; + const auto t55 = 2 * ea0_y; + const auto t56 = ea1_y * t40; + const auto t57 = ea1_y * t36; + const auto t58 = ea1_y * t41; + const auto t59 = ea1_y * t38; + const auto t60 = 2 * eb0_x; + const auto t61 = eb1_x * t39; + const auto t62 = eb1_z * t39; + const auto t63 = 2 * ea0_z; + const auto t64 = ea1_z * t40; + const auto t65 = ea1_z * t35; + const auto t66 = ea1_z * t41; + const auto t67 = ea1_z * t37; + const auto t68 = eb1_x * t42; + const auto t69 = eb1_y * t42; + const auto t70 = eb1_y * t43; + const auto t71 = eb1_z * t43; + const auto t72 = eb1_x * t44; + const auto t73 = eb1_z * t44; + const auto t74 = eb1_x * t45; + const auto t75 = eb1_y * t45; + const auto t76 = -t0 * t2 + t0 * t5 + 4 * t10 * t11 + t12 * t2 + t12 * t24 + - t12 * t25 - t12 * t30 + t12 * t31 - t12 * t5 - t13 * t2 - t13 * t24 + + t13 * t25 + t13 * t30 - t13 * t31 + t13 * t5 + t14 * t16 - t14 * t17 + + t14 * t2 - t14 * t30 + t14 * t31 - t14 * t32 + t14 * t33 - t14 * t5 + - t15 * t16 + t15 * t17 - t15 * t2 + t15 * t30 - t15 * t31 + t15 * t32 + - t15 * t33 + t15 * t5 + t16 * t18 - t16 * t19 - t16 * t6 + t16 * t7 + - t17 * t18 + t17 * t19 + t17 * t6 - t17 * t7 + t18 * t24 - t18 * t25 + - t19 * t24 + t19 * t25 + t2 * t3 - t2 * t6 + t2 * t7 + 4 * t20 * t21 + + 4 * t22 * t23 + 4 * t26 * t27 + 4 * t28 * t29 - t3 * t5 + t34 * t35 + + t34 * t36 + t34 * t37 + t34 * t38 + t35 * t42 + t35 * t43 + t35 * t45 + + t36 * t39 + t36 * t43 + t36 * t44 + t37 * t42 + t37 * t43 + t37 * t45 + + t38 * t39 + t38 * t43 + t38 * t44 + t39 * t40 + t39 * t41 + t40 * t42 + + t40 * t44 + t40 * t45 + t41 * t42 + t41 * t44 + t41 * t45 - t46 * t47 + - t47 * t48 - t47 * t49 - t47 * t50 + t5 * t6 - t5 * t7 - t51 * t52 + - t51 * t69 - t51 * t70 - t51 * t75 - t53 * t54 - t53 * t62 - t53 * t71 + - t53 * t73 - t55 * t56 - t55 * t57 - t55 * t58 - t55 * t59 - t60 * t61 + - t60 * t68 - t60 * t72 - t60 * t74 - t63 * t64 - t63 * t65 - t63 * t66 + - t63 * t67 + 4 * t8 * t9; + const auto t77 = 1.0 / t76; + const auto t78 = eb1_z * t53; + const auto t79 = t36 + t38 - t78; + const auto t80 = eb1_y * t51; + const auto t81 = t35 + t37 - t80; + const auto t82 = t79 + t81; + const auto t83 = ea0_x - eb0_x; + const auto t84 = eb0_x - eb1_x; + const auto t85 = t83 * t84; + const auto t86 = ea0_y - eb0_y; + const auto t87 = eb0_y - eb1_y; + const auto t88 = t86 * t87; + const auto t89 = ea0_z - eb0_z; + const auto t90 = eb0_z - eb1_z; + const auto t91 = t89 * t90; + const auto t92 = t88 + t91; + const auto t93 = t85 + t92; + const auto t94 = -t14 + t15 + t6 - t7; + const auto t95 = t0 - t12 + t13 - t3; + const auto t96 = t94 + t95; + const auto t97 = t1 - t18 + t19 - t4; + const auto t98 = t96 + t97; + const auto t99 = t93 * t98; + const auto t100 = t82 * t99; + const auto t101 = ea0_x - ea1_x; + const auto t102 = t101 * t83; + const auto t103 = ea0_y - ea1_y; + const auto t104 = t103 * t86; + const auto t105 = ea0_z - ea1_z; + const auto t106 = t105 * t89; + const auto t107 = t102 + t104 + t106; + const auto t108 = eb1_x * t60; + const auto t109 = -t108 + t40 + t41; + const auto t110 = t109 + t82; + const auto t111 = std::pow(t76, -2); + const auto t112 = 2 * ea0_x; + const auto t113 = eb1_y * t112; + const auto t114 = eb1_z * t112; + const auto t115 = -ea1_x * t35 - ea1_x * t36 - ea1_x * t37 - ea1_x * t38 + - eb0_x * t0 + eb0_x * t12 - eb0_x * t13 + eb0_x * t14 - eb0_x * t15 + + eb0_x * t3 - eb0_x * t6 + eb0_x * t7 - eb0_y * t113 - eb0_z * t114 + + eb1_x * t0 - eb1_x * t12 + eb1_x * t13 - eb1_x * t14 + eb1_x * t15 + - eb1_x * t3 + eb1_x * t6 - eb1_x * t7 + t11 * t53 + t46 + t48 + t49 + + t50 + t51 * t9; + const auto t116 = std::pow(t115, 2); + const auto t117 = t111 * t116; + const auto t118 = 4 * t117; + const auto t119 = 2 * t77; + const auto t120 = t115 * t93; + const auto t121 = t120 * t84; + const auto t122 = t107 * t110; + const auto t123 = t115 * t84; + const auto t124 = t119 * t98; + const auto t125 = t110 * t115; + const auto t126 = ea1_x + eb0_x - t112; + const auto t127 = t119 * t126; + const auto t128 = t110 + t123 * t124 + t125 * t127 - std::pow(t84, 2); + const auto t129 = t84 * t87; + const auto t130 = + eb0_x * eb0_y - eb0_x * eb1_y - eb0_y * eb1_x + eb1_x * eb1_y; + const auto t131 = -t101 * t83 - t103 * t86 - t105 * t89; + const auto t132 = t110 * t131; + const auto t133 = t132 * t77; + const auto t134 = t130 * t99; + const auto t135 = eb0_x * t55; + const auto t136 = eb1_z * t55; + const auto t137 = ea1_y * eb1_x; + const auto t138 = -ea0_y * t36 - ea0_y * t38 - ea0_y * t40 - ea0_y * t41 + + eb0_y * t1 - eb0_y * t14 + eb0_y * t15 - eb0_y * t18 + eb0_y * t19 + - eb0_y * t4 + eb0_y * t6 - eb0_y * t7 + eb0_z * t136 + eb1_x * t135 + - eb1_y * t1 + eb1_y * t14 - eb1_y * t15 + eb1_y * t18 - eb1_y * t19 + + eb1_y * t4 - eb1_y * t6 + eb1_y * t7 - t137 * t60 - t23 * t53 + t56 + + t57 + t58 + t59; + const auto t139 = t110 * t138; + const auto t140 = t126 * t77; + const auto t141 = ea1_y + eb0_y - t55; + const auto t142 = t84 * t93; + const auto t143 = t138 * t142; + const auto t144 = t77 * t98; + const auto t145 = t144 * t84; + const auto t146 = t119 + * (4 * t110 * t111 * t115 * t131 * t138 + t110 * t115 * t141 * t77 + + 4 * t111 * t115 * t138 * t93 * t98 + t115 * t77 * t87 * t93 + + t115 * t77 * t87 * t98 - t129 - t130 * t133 - t134 * t77 + - t138 * t145 - t139 * t140 - t143 * t77); + const auto t147 = t84 * t90; + const auto t148 = + eb0_x * eb0_z - eb0_x * eb1_z - eb0_z * eb1_x + eb1_x * eb1_z; + const auto t149 = t148 * t99; + const auto t150 = eb0_x * t63; + const auto t151 = eb0_y * t63; + const auto t152 = ea1_z * eb1_x; + const auto t153 = ea1_z * eb1_y; + const auto t154 = -ea0_z * t35 - ea0_z * t37 - ea0_z * t40 - ea0_z * t41 + + eb0_z * t0 + eb0_z * t1 - eb0_z * t12 + eb0_z * t13 - eb0_z * t18 + + eb0_z * t19 - eb0_z * t3 - eb0_z * t4 + eb1_x * t150 + eb1_y * t151 + - eb1_z * t0 - eb1_z * t1 + eb1_z * t12 - eb1_z * t13 + eb1_z * t18 + - eb1_z * t19 + eb1_z * t3 + eb1_z * t4 - t152 * t60 - t153 * t51 + t64 + + t65 + t66 + t67; + const auto t155 = t110 * t154; + const auto t156 = ea1_z + eb0_z - t63; + const auto t157 = t142 * t154; + const auto t158 = t119 + * (4 * t110 * t111 * t115 * t131 * t154 + t110 * t115 * t156 * t77 + + 4 * t111 * t115 * t154 * t93 * t98 + t115 * t77 * t90 * t93 + + t115 * t77 * t90 * t98 - t133 * t148 - t140 * t155 - t145 * t154 + - t147 - t149 * t77 - t157 * t77); + const auto t159 = 4 * t77; + const auto t160 = t77 + * (-t100 * t119 + 2 * t107 * t110 * t77 * t82 + + 2 * t110 * t115 * t77 * t83 + 8 * t111 * t116 * t93 * t98 + - 8 * t117 * t122 - t121 * t159 - t128); + const auto t161 = t122 * t130; + const auto t162 = t125 * t86; + const auto t163 = t120 * t87; + const auto t164 = t119 * t163; + const auto t165 = t124 * t84; + const auto t166 = 8 * t111; + const auto t167 = t138 * t166; + const auto t168 = t115 * t122; + const auto t169 = t115 * t99; + const auto t170 = t167 * t169; + const auto t171 = t77 + * (t119 * t134 + t119 * t143 - t119 * t161 + t119 * t162 + t127 * t139 + + t129 + t138 * t165 - t164 + t167 * t168 - t170); + const auto t172 = t122 * t148; + const auto t173 = t125 * t89; + const auto t174 = t120 * t90; + const auto t175 = t119 * t174; + const auto t176 = t154 * t166; + const auto t177 = t169 * t176; + const auto t178 = t77 + * (t119 * t149 + t119 * t157 - t119 * t172 + t119 * t173 + t127 * t155 + + t147 + t154 * t165 + t168 * t176 - t175 - t177); + const auto t179 = ea0_x + eb1_x - t60; + const auto t180 = t131 * t159; + const auto t181 = t101 * t119; + const auto t182 = t115 * t124; + const auto t183 = t119 * t96; + const auto t184 = t125 * t131; + const auto t185 = ea0_x * t0 - ea0_x * t12 + ea0_x * t13 - ea0_x * t14 + + ea0_x * t15 - ea0_x * t3 + ea0_x * t6 - ea0_x * t7 - ea1_x * t0 + + ea1_x * t12 - ea1_x * t13 + ea1_x * t14 - ea1_x * t15 + ea1_x * t3 + - ea1_x * t6 + ea1_x * t7 - eb0_x * t39 - eb0_x * t42 - eb0_x * t44 + - eb0_x * t45 - t137 * t55 - t152 * t63 + t21 * t55 + t27 * t63 + t61 + + t68 + t72 + t74; + const auto t186 = 8 * t185; + const auto t187 = t111 * t186; + const auto t188 = t110 - t123 * t180 - t125 * t181 + t132 * t183 + - t179 * t182 + t179 * t84 - t184 * t187; + const auto t189 = t101 * t84; + const auto t190 = 2 * t126; + const auto t191 = t110 * t127; + const auto t192 = t119 * t142; + const auto t193 = -t120 * t181 - t169 * t187 + t183 * t99 + t185 * t192; + const auto t194 = + t165 * t185 + t185 * t191 + t189 + t190 * t84 + t193 + t98; + const auto t195 = t77 * (-t188 - t194 - t93); + const auto t196 = -ea0_y * eb0_x - ea1_x * t51 + eb0_y * t112 - t113 - t137 + + t20 + t21 + 2 * t9; + const auto t197 = t119 * t196; + const auto t198 = t115 * t180; + const auto t199 = ea1_x * eb0_y; + const auto t200 = -ea0_y * t1 + ea0_y * t14 - ea0_y * t15 + ea0_y * t18 + - ea0_y * t19 + ea0_y * t4 - ea0_y * t6 + ea0_y * t7 + ea1_y * t1 + - ea1_y * t14 + ea1_y * t15 - ea1_y * t18 + ea1_y * t19 - ea1_y * t4 + + ea1_y * t6 - ea1_y * t7 + eb0_y * t34 + eb0_y * t42 + eb0_y * t43 + + eb0_y * t45 - t112 * t199 + t112 * t9 + t153 * t63 - t29 * t63 - t52 + - t69 - t70 - t75; + const auto t201 = t166 * t200; + const auto t202 = t132 * t197 - t184 * t201 + t198 * t87; + const auto t203 = t103 * t119; + const auto t204 = t120 * t203; + const auto t205 = t192 * t200; + const auto t206 = t197 * t99; + const auto t207 = -t169 * t201; + const auto t208 = ea0_y + eb1_y - t51; + const auto t209 = + t125 * t203 + t182 * t208 + t204 + t205 + t206 + t207 - t208 * t84; + const auto t210 = t103 * t84; + const auto t211 = t165 * t200 - t190 * t87 + t191 * t200 - t210; + const auto t212 = t77 * (t202 + t209 + t211); + const auto t213 = -ea0_z * eb0_x - ea1_x * t53 + eb0_z * t112 + 2 * t11 + - t114 - t152 + t26 + t27; + const auto t214 = t119 * t213; + const auto t215 = ea1_x * eb0_z; + const auto t216 = ea1_y * eb0_z; + const auto t217 = -ea0_z * t0 - ea0_z * t1 + ea0_z * t12 - ea0_z * t13 + + ea0_z * t18 - ea0_z * t19 + ea0_z * t3 + ea0_z * t4 + ea1_z * t0 + + ea1_z * t1 - ea1_z * t12 + ea1_z * t13 - ea1_z * t18 + ea1_z * t19 + - ea1_z * t3 - ea1_z * t4 + eb0_z * t34 + eb0_z * t39 + eb0_z * t43 + + eb0_z * t44 + t11 * t112 - t112 * t215 - t216 * t55 + t23 * t55 - t54 + - t62 - t71 - t73; + const auto t218 = t166 * t217; + const auto t219 = t132 * t214 - t184 * t218 + t198 * t90; + const auto t220 = t105 * t119; + const auto t221 = t120 * t220; + const auto t222 = t192 * t217; + const auto t223 = t214 * t99; + const auto t224 = -t169 * t218; + const auto t225 = ea0_z + eb1_z - t53; + const auto t226 = + t125 * t220 + t182 * t225 + t221 + t222 + t223 + t224 - t225 * t84; + const auto t227 = t105 * t84; + const auto t228 = t165 * t217 - t190 * t90 + t191 * t217 - t227; + const auto t229 = t77 * (t219 + t226 + t228); + const auto t230 = t107 * t159; + const auto t231 = -t122 * t183 + t123 * t230 + t168 * t187 - t182 * t83; + const auto t232 = t77 * (t194 + t231 + 2 * t85 + t92); + const auto t233 = t84 * t86; + const auto t234 = t182 * t86 + t204 + t205 + t206 + t207 - t233; + const auto t235 = t77 + * (2 * t107 * t110 * t196 * t77 + 4 * t107 * t115 * t77 * t87 + - t168 * t201 - t211 - t234); + const auto t236 = t84 * t89; + const auto t237 = t182 * t89 + t221 + t222 + t223 + t224 - t236; + const auto t238 = t77 + * (2 * t107 * t110 * t213 * t77 + 4 * t107 * t115 * t77 * t90 + - t168 * t218 - t228 - t237); + const auto t239 = std::pow(t87, 2); + const auto t240 = t109 + t79; + const auto t241 = t138 * t87; + const auto t242 = t124 * t241; + const auto t243 = t119 * t141; + const auto t244 = t139 * t243; + const auto t245 = std::pow(t138, 2); + const auto t246 = 4 * t111 * t245; + const auto t247 = t108 - t35 - t36 - t37 - t38 - t40 - t41 + t78 + t80; + const auto t248 = t87 * t90; + const auto t249 = + eb0_y * eb0_z - eb0_y * eb1_z - eb0_z * eb1_y + eb1_y * eb1_z; + const auto t250 = t249 * t99; + const auto t251 = t141 * t155; + const auto t252 = t139 * t156; + const auto t253 = t154 * t87; + const auto t254 = t253 * t93; + const auto t255 = t138 * t90; + const auto t256 = t255 * t93; + const auto t257 = 4 * t111 * t138; + const auto t258 = t131 * t155; + const auto t259 = t154 * t99; + const auto t260 = -t119 + * (t133 * t249 + t144 * t253 + t144 * t255 + t248 + t250 * t77 + + t251 * t77 + t252 * t77 + t254 * t77 + t256 * t77 + t257 * t258 + + t257 * t259); + const auto t261 = t110 * t83; + const auto t262 = t138 * t261; + const auto t263 = t77 + * (2 * t110 * t130 * t131 * t77 - t119 * t262 - t125 * t243 + t129 + + 2 * t130 * t77 * t93 * t98 + 2 * t138 * t77 * t84 * t93 - t164 + - t167 * t184 - t170 - t182 * t87); + const auto t264 = t139 * t86; + const auto t265 = t119 * t132; + const auto t266 = t240 * t99; + const auto t267 = t77 + * (8 * t110 * t111 * t131 * t245 - t110 + 8 * t111 * t245 * t93 * t98 + - t119 * t264 - t119 * t266 + 4 * t138 * t77 * t87 * t93 + t239 + - t240 * t265 + t242 + t244); + const auto t268 = t139 * t89; + const auto t269 = t119 * t250 + t119 * t254 + t119 * t256 + t167 * t258 + + t167 * t259 + t248 + t249 * t265; + const auto t270 = t77 * (t119 * t251 - t119 * t268 + t124 * t253 + t269); + const auto t271 = t124 * t138; + const auto t272 = -ea0_x * eb1_y + ea1_y * t60 + eb1_x * t55 - t135 + - 2 * t137 - t199 + t8 + t9; + const auto t273 = t138 * t84; + const auto t274 = t131 * t139; + const auto t275 = t139 * t181 + t179 * t271 + t179 * t87 + t180 * t273 + + t187 * t274 + t265 * t272; + const auto t276 = t101 * t87; + const auto t277 = 2 * t141; + const auto t278 = t185 * t87; + const auto t279 = t110 * t243; + const auto t280 = t138 * t93; + const auto t281 = t119 * t93; + const auto t282 = t119 * t99; + const auto t283 = t138 * t187; + const auto t284 = t181 * t280 + t272 * t282 + t278 * t281 + t283 * t99; + const auto t285 = t124 * t278 + t185 * t279 + t276 + t277 * t84 + t284; + const auto t286 = -t77 * (t275 + t285); + const auto t287 = t94 + t97; + const auto t288 = + -8 * t110 * t111 * t131 * t138 * t200 + t180 * t241 + t265 * t287; + const auto t289 = t110 + t139 * t203 + t208 * t271 + t208 * t87; + const auto t290 = t200 * t87; + const auto t291 = -t281 * t290; + const auto t292 = t203 * t280; + const auto t293 = t282 * t287; + const auto t294 = t167 * t99; + const auto t295 = -t200 * t294; + const auto t296 = t103 * t87; + const auto t297 = -t124 * t290 - t200 * t279 + t277 * t87 + t291 + t292 + + t293 + t295 + t296; + const auto t298 = t93 + t98; + const auto t299 = t77 * (-t288 - t289 - t297 - t298); + const auto t300 = t217 * t87; + const auto t301 = t281 * t300; + const auto t302 = -t301; + const auto t303 = -ea0_z * eb0_y - ea1_y * t53 + eb0_z * t55 - t136 - t153 + + 2 * t23 + t28 + t29; + const auto t304 = t282 * t303; + const auto t305 = -t304; + const auto t306 = t220 * t280; + const auto t307 = t217 * t294; + const auto t308 = -t307; + const auto t309 = t180 * t255; + const auto t310 = t139 * t220 + t225 * t271 + t225 * t87; + const auto t311 = t105 * t87; + const auto t312 = -t124 * t300 - t217 * t279 + t277 * t90 + t311; + const auto t313 = t77 + * (8 * t110 * t111 * t131 * t138 * t217 + 2 * t110 * t131 * t303 * t77 + - t302 - t305 - t306 - t308 - t309 - t310 - t312); + const auto t314 = t83 * t87; + const auto t315 = t119 * t122; + const auto t316 = + -t122 * t283 - t230 * t273 + t271 * t83 - t272 * t315 + t314; + const auto t317 = t77 * (t285 + t316); + const auto t318 = t271 * t86; + const auto t319 = t122 * t167; + const auto t320 = t200 * t319 - t230 * t241 - t287 * t315; + const auto t321 = t85 + t98; + const auto t322 = t77 * (t297 + t318 + t320 + t321 + 2 * t88 + t91); + const auto t323 = t87 * t89; + const auto t324 = t271 * t89; + const auto t325 = + t217 * t319 - t230 * t255 + t302 + t303 * t315 + t305 + t306 + t308; + const auto t326 = t77 * (t312 + t323 + t324 + t325); + const auto t327 = std::pow(t90, 2); + const auto t328 = t109 + t81; + const auto t329 = t154 * t90; + const auto t330 = t124 * t329; + const auto t331 = t119 * t156; + const auto t332 = t155 * t331; + const auto t333 = std::pow(t154, 2); + const auto t334 = 4 * t111 * t333; + const auto t335 = t154 * t261; + const auto t336 = t77 + * (2 * t110 * t131 * t148 * t77 - t119 * t335 - t125 * t331 + t147 + + 2 * t148 * t77 * t93 * t98 + 2 * t154 * t77 * t84 * t93 - t175 + - t176 * t184 - t177 - t182 * t90); + const auto t337 = t155 * t86; + const auto t338 = t77 * (t119 * t252 - t119 * t337 + t124 * t255 + t269); + const auto t339 = t155 * t89; + const auto t340 = t328 * t99; + const auto t341 = t77 + * (8 * t110 * t111 * t131 * t333 - t110 + 8 * t111 * t333 * t93 * t98 + - t119 * t339 - t119 * t340 + 4 * t154 * t77 * t90 * t93 + - t265 * t328 + t327 + t330 + t332); + const auto t342 = t124 * t154; + const auto t343 = -ea0_x * eb1_z + ea1_z * t60 + eb1_x * t63 + t10 + t11 + - t150 - 2 * t152 - t215; + const auto t344 = t154 * t84; + const auto t345 = t155 * t181 + t179 * t342 + t179 * t90 + t180 * t344 + + t187 * t258 + t265 * t343; + const auto t346 = t101 * t90; + const auto t347 = 2 * t156; + const auto t348 = t185 * t90; + const auto t349 = t110 * t331; + const auto t350 = t154 * t93; + const auto t351 = t181 * t350 + t187 * t259 + t281 * t348 + t282 * t343; + const auto t352 = t124 * t348 + t185 * t349 + t346 + t347 * t84 + t351; + const auto t353 = -t77 * (t345 + t352); + const auto t354 = -ea0_y * eb1_z + ea1_z * t51 + eb1_y * t63 - t151 + - 2 * t153 - t216 + t22 + t23; + const auto t355 = + -8 * t110 * t111 * t131 * t154 * t200 + t180 * t253 + t265 * t354; + const auto t356 = t155 * t203 + t208 * t342 + t208 * t90; + const auto t357 = t200 * t90; + const auto t358 = -t281 * t357; + const auto t359 = t203 * t350; + const auto t360 = t282 * t354; + const auto t361 = t176 * t99; + const auto t362 = -t200 * t361; + const auto t363 = t103 * t90; + const auto t364 = -t124 * t357 - t200 * t349 + t347 * t87 + t358 + t359 + + t360 + t362 + t363; + const auto t365 = t77 * (-t355 - t356 - t364); + const auto t366 = t95 + t97; + const auto t367 = + -8 * t110 * t111 * t131 * t154 * t217 + t180 * t329 + t265 * t366; + const auto t368 = t110 + t155 * t220 + t225 * t342 + t225 * t90; + const auto t369 = t217 * t90; + const auto t370 = -t281 * t369; + const auto t371 = t220 * t350; + const auto t372 = t282 * t366; + const auto t373 = -t217 * t361; + const auto t374 = t105 * t90; + const auto t375 = -t124 * t369 - t217 * t349 + t347 * t90 + t370 + t371 + + t372 + t373 + t374; + const auto t376 = t77 * (-t298 - t367 - t368 - t375); + const auto t377 = t83 * t90; + const auto t378 = t122 * t187; + const auto t379 = + -t154 * t378 - t230 * t344 - t315 * t343 + t342 * t83 + t377; + const auto t380 = t77 * (t352 + t379); + const auto t381 = t86 * t90; + const auto t382 = t342 * t86 + t381; + const auto t383 = t122 * t176; + const auto t384 = t200 * t383 - t230 * t253 - t315 * t354; + const auto t385 = t77 * (t364 + t382 + t384); + const auto t386 = t342 * t89; + const auto t387 = t217 * t383 - t230 * t329 - t315 * t366; + const auto t388 = t77 * (t321 + t375 + t386 + t387 + t88 + 2 * t91); + const auto t389 = 4 * t116; + const auto t390 = t77 * t99; + const auto t391 = 2 * t111; + const auto t392 = t122 * t159; + const auto t393 = t115 * t138; + const auto t394 = 4 * t390; + const auto t395 = t391 + * (-t134 - t143 + t161 - t162 + t163 + t262 - t392 * t393 + + t393 * t394); + const auto t396 = t115 * t154; + const auto t397 = t391 + * (-t149 - t157 + t172 - t173 + t174 + t335 - t392 * t396 + + t394 * t396); + const auto t398 = t119 * t261; + const auto t399 = -t185 * t398 + t193 + t92; + const auto t400 = t77 * (t188 + t399 - t85); + const auto t401 = -t200 * t398 + t202 + 2 * t314; + const auto t402 = t77 * (-t209 - t401); + const auto t403 = -t217 * t398 + t219 + 2 * t377; + const auto t404 = t77 * (-t226 - t403); + const auto t405 = t77 * (-t231 - t399); + const auto t406 = t77 * (t234 + t401); + const auto t407 = t77 * (t237 + t403); + const auto t408 = 2 * t93; + const auto t409 = t138 * t154; + const auto t410 = t391 + * (t122 * t249 - t250 - t254 - t256 + t268 + t337 + t392 * t409 + - t394 * t409); + const auto t411 = t110 * t119; + const auto t412 = t185 * t86; + const auto t413 = -2 * t233 + t284 - t411 * t412; + const auto t414 = t77 * (t275 + t413); + const auto t415 = t411 * t86; + const auto t416 = t200 * t415 + t291 + t292 + t293 + t295 + t85 + t91; + const auto t417 = t77 * (t289 + t320 + t416 - t88); + const auto t418 = 2 * t381; + const auto t419 = t217 * t415; + const auto t420 = t77 * (t310 + t325 - t418 + t419); + const auto t421 = t77 * (-t316 - t413); + const auto t422 = t77 * (-t288 - t318 - t416); + const auto t423 = t77 + * (t218 * t274 + t265 * t303 + t301 + t304 - t306 + t307 - t309 - t323 + - t324 + t418 - t419); + const auto t424 = t411 * t89; + const auto t425 = -t185 * t424 - 2 * t236 + t351; + const auto t426 = t77 * (t345 + t425); + const auto t427 = t200 * t424 - 2 * t323 + t358 + t359 + t360 + t362; + const auto t428 = t77 * (t356 + t384 + t427); + const auto t429 = t217 * t424 + t370 + t371 + t372 + t373 + t85 + t88; + const auto t430 = t77 * (t368 + t387 + t429 - t91); + const auto t431 = t77 * (-t379 - t425); + const auto t432 = t77 * (-t355 - t382 - t427); + const auto t433 = t77 * (-t367 - t386 - t429); + const auto t434 = -ea1_z * t63 + t42 + t45; + const auto t435 = -ea1_y * t55 + t39 + t44; + const auto t436 = t434 + t435; + const auto t437 = t181 * t185; + const auto t438 = t111 * std::pow(t185, 2); + const auto t439 = 4 * t438; + const auto t440 = -t110 * t131 * t436 * t77 + t131 + t132 * t439 + + t180 * t185 * t84 - t436 * t77 * t93 * t98 + t437 * t93 + t439 * t99; + const auto t441 = t124 * t185; + const auto t442 = + -t0 - t1 + t12 - t13 + t14 - t15 + t18 - t19 + t3 + t4 - t6 + t7; + const auto t443 = t101 * t179 + t110 * t437 + t179 * t441 + 2 * t189 + t442; + const auto t444 = + ea0_x * ea0_y - ea0_x * ea1_y - ea0_y * ea1_x + ea1_x * ea1_y; + const auto t445 = -8 * t110 * t111 * t131 * t185 * t200 + - 4 * t131 * t200 * t77 * t84 + t180 * t278 + t265 * t444; + const auto t446 = t181 * t200; + const auto t447 = t124 * t179; + const auto t448 = t103 * t179 - t110 * t446 - t200 * t447 + 2 * t276; + const auto t449 = -t446 * t93; + const auto t450 = t185 * t203; + const auto t451 = t450 * t93; + const auto t452 = t282 * t444; + const auto t453 = -t187 * t200 * t99; + const auto t454 = t101 * t208 + t110 * t450 + t208 * t441 + 2 * t210 + t449 + + t451 + t452 + t453; + const auto t455 = t77 * (-t445 - t448 - t454); + const auto t456 = + ea0_x * ea0_z - ea0_x * ea1_z - ea0_z * ea1_x + ea1_x * ea1_z; + const auto t457 = -8 * t110 * t111 * t131 * t185 * t217 + - 4 * t131 * t217 * t77 * t84 + t180 * t348 + t265 * t456; + const auto t458 = t181 * t217; + const auto t459 = t105 * t179 - t110 * t458 - t217 * t447 + 2 * t346; + const auto t460 = -t458 * t93; + const auto t461 = t185 * t220; + const auto t462 = t461 * t93; + const auto t463 = t282 * t456; + const auto t464 = -t187 * t217 * t99; + const auto t465 = t101 * t225 + t110 * t461 + t225 * t441 + 2 * t227 + t460 + + t462 + t463 + t464; + const auto t466 = t77 * (-t457 - t459 - t465); + const auto t467 = t441 * t83; + const auto t468 = 2 * t104; + const auto t469 = 2 * t106; + const auto t470 = 8 * t438; + const auto t471 = t159 * t93; + const auto t472 = t77 + * (t101 * t185 * t471 - t102 - t107 * t186 * t77 * t84 - t122 * t470 + - t282 * t436 + t315 * t436 + t443 + t467 - t468 - t469 + + t470 * t99); + const auto t473 = t101 * t86 + t124 * t412 + t449 + t451 + t452 + t453; + const auto t474 = t230 * t84; + const auto t475 = t200 * t378 + t200 * t474 - t230 * t278 - t315 * t444; + const auto t476 = t77 * (t448 + t473 + t475); + const auto t477 = t101 * t89 + t441 * t89 + t460 + t462 + t463 + t464; + const auto t478 = t217 * t378 + t217 * t474 - t230 * t348 - t315 * t456; + const auto t479 = t77 * (t459 + t477 + t478); + const auto t480 = t103 * t208; + const auto t481 = 2 * t296; + const auto t482 = t110 * t200 * t203; + const auto t483 = t124 * t208; + const auto t484 = t200 * t483; + const auto t485 = -ea1_x * t112 + t34 + t43; + const auto t486 = t434 + t485; + const auto t487 = std::pow(t200, 2); + const auto t488 = t111 * t487; + const auto t489 = 4 * t488; + const auto t490 = -2 * t103 * t200 * t77 * t93 - t110 * t131 * t486 * t77 + - 4 * t131 * t200 * t77 * t87 + t131 + t132 * t489 + - t486 * t77 * t93 * t98 + t489 * t99; + const auto t491 = + ea0_y * ea0_z - ea0_y * ea1_z - ea0_z * ea1_y + ea1_y * ea1_z; + const auto t492 = t201 * t217; + const auto t493 = -4 * t131 * t200 * t77 * t90 - 4 * t131 * t217 * t77 * t87 + + t132 * t492 + t265 * t491; + const auto t494 = t203 * t217; + const auto t495 = t105 * t208 - t110 * t494 - t217 * t483 + 2 * t363; + const auto t496 = -t494 * t93; + const auto t497 = t200 * t220; + const auto t498 = -t497 * t93; + const auto t499 = t282 * t491; + const auto t500 = t492 * t99; + const auto t501 = t124 * t200; + const auto t502 = t103 * t225 - t110 * t497 - t225 * t501 + 2 * t311 + t496 + + t498 + t499 + t500; + const auto t503 = t77 * (-t493 - t495 - t502); + const auto t504 = t103 * t83 - t501 * t83; + const auto t505 = t77 * (t454 + t475 + t504); + const auto t506 = t501 * t86; + const auto t507 = 2 * t102 + t98; + const auto t508 = t77 + * (-t103 * t200 * t471 - t104 + 2 * t107 * t110 * t486 * t77 + + 8 * t107 * t200 * t77 * t87 + 8 * t111 * t487 * t93 * t98 + - 8 * t122 * t488 - t282 * t486 - t469 + t480 + t481 - t482 - t484 + - t506 - t507); + const auto t509 = t103 * t89 + t496 + t498 + t499 + t500 - t501 * t89; + const auto t510 = -t122 * t492 + t230 * t300 + t230 * t357 - t315 * t491; + const auto t511 = t77 * (t495 + t509 + t510); + const auto t512 = t105 * t225; + const auto t513 = 2 * t374; + const auto t514 = t110 * t217 * t220; + const auto t515 = t124 * t217; + const auto t516 = t225 * t515; + const auto t517 = t435 + t485; + const auto t518 = std::pow(t217, 2); + const auto t519 = t111 * t518; + const auto t520 = 4 * t519; + const auto t521 = -2 * t105 * t217 * t77 * t93 - t110 * t131 * t517 * t77 + - 4 * t131 * t217 * t77 * t90 + t131 + t132 * t520 + - t517 * t77 * t93 * t98 + t520 * t99; + const auto t522 = t105 * t83 - t515 * t83; + const auto t523 = t77 * (t465 + t478 + t522); + const auto t524 = t105 * t86 - t515 * t86; + const auto t525 = t77 * (t502 + t510 + t524); + const auto t526 = t515 * t89; + const auto t527 = t77 + * (-t105 * t217 * t471 - t106 + 2 * t107 * t110 * t517 * t77 + + 8 * t107 * t217 * t77 * t90 + 8 * t111 * t518 * t93 * t98 + - 8 * t122 * t519 - t282 * t517 - t468 - t507 + t512 + t513 - t514 + - t516 - t526); + const auto t528 = t77 * (-t445 - t473 - t504); + const auto t529 = t77 * (-t457 - t477 - t522); + const auto t530 = t77 * (-t493 - t509 - t524); + hess[0] = t119 + * (t100 * t77 - t107 * t110 * t77 * t82 + t118 * t122 - t118 * t99 + + t119 * t121 + t128); + hess[1] = t146; + hess[2] = t158; + hess[3] = t160; + hess[4] = t171; + hess[5] = t178; + hess[6] = t195; + hess[7] = t212; + hess[8] = t229; + hess[9] = t232; + hess[10] = t235; + hess[11] = t238; + hess[12] = t146; + hess[13] = t119 + * (t110 * t131 * t240 * t77 - t119 * t241 * t93 - t132 * t246 - t239 + + t240 * t77 * t93 * t98 - t242 - t244 - t246 * t99 - t247); + hess[14] = t260; + hess[15] = t263; + hess[16] = t267; + hess[17] = t270; + hess[18] = t286; + hess[19] = t299; + hess[20] = t313; + hess[21] = t317; + hess[22] = t322; + hess[23] = t326; + hess[24] = t158; + hess[25] = t260; + hess[26] = t119 + * (t110 * t131 * t328 * t77 - t132 * t334 - t247 - t281 * t329 - t327 + + t328 * t77 * t93 * t98 - t330 - t332 - t334 * t99); + hess[27] = t336; + hess[28] = t338; + hess[29] = t341; + hess[30] = t353; + hess[31] = t365; + hess[32] = t376; + hess[33] = t380; + hess[34] = t385; + hess[35] = t388; + hess[36] = t160; + hess[37] = t263; + hess[38] = t336; + hess[39] = t391 + * (t110 * t131 * t82 - 2 * t115 * t261 + 2 * t115 * t84 * t93 + - t133 * t389 - t389 * t390 + t82 * t93 * t98); + hess[40] = t395; + hess[41] = t397; + hess[42] = t400; + hess[43] = t402; + hess[44] = t404; + hess[45] = t405; + hess[46] = t406; + hess[47] = t407; + hess[48] = t171; + hess[49] = t267; + hess[50] = t338; + hess[51] = t395; + hess[52] = t391 + * (-t122 * t240 - t241 * t408 + t245 * t392 - t245 * t394 + 2 * t264 + + t266); + hess[53] = t410; + hess[54] = t414; + hess[55] = t417; + hess[56] = t420; + hess[57] = t421; + hess[58] = t422; + hess[59] = t423; + hess[60] = t178; + hess[61] = t270; + hess[62] = t341; + hess[63] = t397; + hess[64] = t410; + hess[65] = t391 + * (-t122 * t328 - t329 * t408 + t333 * t392 - t333 * t394 + 2 * t339 + + t340); + hess[66] = t426; + hess[67] = t428; + hess[68] = t430; + hess[69] = t431; + hess[70] = t432; + hess[71] = t433; + hess[72] = t195; + hess[73] = t286; + hess[74] = t353; + hess[75] = t400; + hess[76] = t414; + hess[77] = t426; + hess[78] = t119 * (-t440 - t443); + hess[79] = t455; + hess[80] = t466; + hess[81] = t472; + hess[82] = t476; + hess[83] = t479; + hess[84] = t212; + hess[85] = t299; + hess[86] = t365; + hess[87] = t402; + hess[88] = t417; + hess[89] = t428; + hess[90] = t455; + hess[91] = t119 * (-t442 - t480 - t481 + t482 + t484 - t490); + hess[92] = t503; + hess[93] = t505; + hess[94] = t508; + hess[95] = t511; + hess[96] = t229; + hess[97] = t313; + hess[98] = t376; + hess[99] = t404; + hess[100] = t420; + hess[101] = t430; + hess[102] = t466; + hess[103] = t503; + hess[104] = t119 * (-t442 - t512 - t513 + t514 + t516 - t521); + hess[105] = t523; + hess[106] = t525; + hess[107] = t527; + hess[108] = t232; + hess[109] = t317; + hess[110] = t380; + hess[111] = t405; + hess[112] = t421; + hess[113] = t431; + hess[114] = t472; + hess[115] = t505; + hess[116] = t523; + hess[117] = t119 * (-t102 - t440 - t467); + hess[118] = t528; + hess[119] = t529; + hess[120] = t235; + hess[121] = t322; + hess[122] = t385; + hess[123] = t406; + hess[124] = t422; + hess[125] = t432; + hess[126] = t476; + hess[127] = t508; + hess[128] = t525; + hess[129] = t528; + hess[130] = t119 * (-t104 - t490 + t506); + hess[131] = t530; + hess[132] = t238; + hess[133] = t326; + hess[134] = t388; + hess[135] = t407; + hess[136] = t423; + hess[137] = t433; + hess[138] = t479; + hess[139] = t511; + hess[140] = t527; + hess[141] = t529; + hess[142] = t530; + hess[143] = t119 * (-t106 - t521 + t526); +} + +// hess is (144×1) flattened in column-major order +void edge_edge_closest_point_hessian_b( + double ea0_x, + double ea0_y, + double ea0_z, + double ea1_x, + double ea1_y, + double ea1_z, + double eb0_x, + double eb0_y, + double eb0_z, + double eb1_x, + double eb1_y, + double eb1_z, + double hess[144]) +{ + const auto t0 = ea0_z * eb0_z; + const auto t1 = ea1_z * eb1_z; + const auto t2 = ea0_z * eb1_z; + const auto t3 = ea1_z * eb0_z; + const auto t4 = t0 + t1 - t2 - t3; + const auto t5 = ea0_y * eb0_y; + const auto t6 = ea1_y * eb1_y; + const auto t7 = ea0_y * eb1_y; + const auto t8 = ea1_y * eb0_y; + const auto t9 = t5 + t6 - t7 - t8; + const auto t10 = t4 + t9; + const auto t11 = ea0_x * eb0_x; + const auto t12 = ea1_x * eb1_x; + const auto t13 = ea0_x * eb1_x; + const auto t14 = ea1_x * eb0_x; + const auto t15 = t11 + t12 - t13 - t14; + const auto t16 = t10 + t15; + const auto t17 = ea0_x - ea1_x; + const auto t18 = ea0_x - eb0_x; + const auto t19 = t17 * t18; + const auto t20 = ea0_y - ea1_y; + const auto t21 = ea0_y - eb0_y; + const auto t22 = t20 * t21; + const auto t23 = ea0_z - ea1_z; + const auto t24 = ea0_z - eb0_z; + const auto t25 = t23 * t24; + const auto t26 = t22 + t25; + const auto t27 = t19 + t26; + const auto t28 = t16 * t27; + const auto t29 = 2 * t11; + const auto t30 = 2 * t13; + const auto t31 = ea0_x * eb0_y; + const auto t32 = ea1_x * eb1_y; + const auto t33 = ea0_x * eb0_z; + const auto t34 = ea1_x * eb1_z; + const auto t35 = 2 * t5; + const auto t36 = 2 * t7; + const auto t37 = ea0_y * eb1_x; + const auto t38 = ea1_y * eb0_x; + const auto t39 = ea0_y * eb0_z; + const auto t40 = ea1_y * eb1_z; + const auto t41 = 2 * t0; + const auto t42 = 2 * t2; + const auto t43 = ea0_z * eb1_x; + const auto t44 = ea1_z * eb0_x; + const auto t45 = ea0_z * eb1_y; + const auto t46 = ea1_z * eb0_y; + const auto t47 = 2 * t14; + const auto t48 = 2 * t12; + const auto t49 = 2 * t8; + const auto t50 = 2 * t6; + const auto t51 = std::pow(ea0_x, 2); + const auto t52 = std::pow(eb0_y, 2); + const auto t53 = std::pow(eb0_z, 2); + const auto t54 = std::pow(eb1_y, 2); + const auto t55 = std::pow(eb1_z, 2); + const auto t56 = std::pow(ea0_y, 2); + const auto t57 = std::pow(eb0_x, 2); + const auto t58 = std::pow(eb1_x, 2); + const auto t59 = std::pow(ea0_z, 2); + const auto t60 = std::pow(ea1_x, 2); + const auto t61 = std::pow(ea1_y, 2); + const auto t62 = std::pow(ea1_z, 2); + const auto t63 = 2 * ea0_x; + const auto t64 = ea1_x * t52; + const auto t65 = ea1_x * t53; + const auto t66 = ea1_x * t54; + const auto t67 = ea1_x * t55; + const auto t68 = 2 * eb0_y; + const auto t69 = eb1_y * t51; + const auto t70 = 2 * eb0_z; + const auto t71 = eb1_z * t51; + const auto t72 = 2 * ea0_y; + const auto t73 = ea1_y * t57; + const auto t74 = ea1_y * t53; + const auto t75 = ea1_y * t58; + const auto t76 = ea1_y * t55; + const auto t77 = 2 * eb0_x; + const auto t78 = eb1_x * t56; + const auto t79 = eb1_z * t56; + const auto t80 = 2 * ea0_z; + const auto t81 = ea1_z * t57; + const auto t82 = ea1_z * t52; + const auto t83 = ea1_z * t58; + const auto t84 = ea1_z * t54; + const auto t85 = eb1_x * t59; + const auto t86 = eb1_y * t59; + const auto t87 = eb1_y * t60; + const auto t88 = eb1_z * t60; + const auto t89 = eb1_x * t61; + const auto t90 = eb1_z * t61; + const auto t91 = eb1_x * t62; + const auto t92 = eb1_y * t62; + const auto t93 = -t0 * t29 + t0 * t30 - t0 * t35 + t0 * t36 - t1 * t29 + + t1 * t30 - t1 * t35 + t1 * t36 + t1 * t47 - t1 * t48 + t1 * t49 + - t1 * t50 - t12 * t35 + t12 * t36 - t12 * t41 + t12 * t42 + t14 * t35 + - t14 * t36 + t14 * t41 - t14 * t42 + t2 * t29 - t2 * t30 + t2 * t35 + - t2 * t36 + t29 * t3 - t29 * t5 - t29 * t6 + t29 * t7 + t29 * t8 + - t3 * t30 + t3 * t35 - t3 * t36 - t3 * t47 + t3 * t48 - t3 * t49 + + t3 * t50 + t30 * t5 + t30 * t6 - t30 * t7 - t30 * t8 + 4 * t31 * t32 + + 4 * t33 * t34 + 4 * t37 * t38 + 4 * t39 * t40 - t41 * t6 + t41 * t8 + + t42 * t6 - t42 * t8 + 4 * t43 * t44 + 4 * t45 * t46 + t47 * t6 + - t47 * t8 - t48 * t6 + t48 * t8 + t51 * t52 + t51 * t53 + t51 * t54 + + t51 * t55 + t52 * t59 + t52 * t60 + t52 * t62 + t53 * t56 + t53 * t60 + + t53 * t61 + t54 * t59 + t54 * t60 + t54 * t62 + t55 * t56 + t55 * t60 + + t55 * t61 + t56 * t57 + t56 * t58 + t57 * t59 + t57 * t61 + t57 * t62 + + t58 * t59 + t58 * t61 + t58 * t62 - t63 * t64 - t63 * t65 - t63 * t66 + - t63 * t67 - t68 * t69 - t68 * t86 - t68 * t87 - t68 * t92 - t70 * t71 + - t70 * t79 - t70 * t88 - t70 * t90 - t72 * t73 - t72 * t74 - t72 * t75 + - t72 * t76 - t77 * t78 - t77 * t85 - t77 * t89 - t77 * t91 - t80 * t81 + - t80 * t82 - t80 * t83 - t80 * t84; + const auto t94 = 1.0 / t93; + const auto t95 = -eb1_z * t70 + t53 + t55; + const auto t96 = -eb1_y * t68 + t52 + t54; + const auto t97 = t95 + t96; + const auto t98 = ea1_z * t80; + const auto t99 = t59 + t62 - t98; + const auto t100 = ea1_y * t72; + const auto t101 = -t100 + t56 + t61; + const auto t102 = t101 + t99; + const auto t103 = ea1_x * t63; + const auto t104 = -t103 + t51 + t60; + const auto t105 = t102 + t104; + const auto t106 = eb0_x - eb1_x; + const auto t107 = t106 * t18; + const auto t108 = eb0_y - eb1_y; + const auto t109 = t108 * t21; + const auto t110 = eb0_z - eb1_z; + const auto t111 = t110 * t24; + const auto t112 = t109 + t111; + const auto t113 = t107 + t112; + const auto t114 = eb1_y * t63; + const auto t115 = eb1_z * t63; + const auto t116 = ea0_x * t52 + ea0_x * t53 + ea0_x * t54 + ea0_x * t55 + - eb0_x * t0 - eb0_x * t1 + eb0_x * t2 + eb0_x * t3 - eb0_x * t5 + - eb0_x * t6 + eb0_x * t7 + eb0_x * t8 - eb0_y * t114 - eb0_z * t115 + + eb1_x * t0 + eb1_x * t1 - eb1_x * t2 - eb1_x * t3 + eb1_x * t5 + + eb1_x * t6 - eb1_x * t7 - eb1_x * t8 + t32 * t68 + t34 * t70 - t64 + - t65 - t66 - t67; + const auto t117 = std::pow(t116, 2); + const auto t118 = std::pow(t93, -2); + const auto t119 = 2 * t94; + const auto t120 = t116 * t119; + const auto t121 = t106 * t120; + const auto t122 = t117 * t118; + const auto t123 = t105 * t113; + const auto t124 = 4 * t123; + const auto t125 = -t105 * t113 * t94 * t97 - 4 * t113 * t116 * t17 * t94 + - 4 * t117 * t118 * t16 * t27 + t121 * t27 + t122 * t124 + + t28 * t94 * t97; + const auto t126 = + -t0 - t1 - t11 - t12 + t13 + t14 + t2 + t3 - t5 - t6 + t7 + t8; + const auto t127 = t113 + t126; + const auto t128 = ea1_x + eb0_x - t63; + const auto t129 = t106 * t17; + const auto t130 = t128 * t16; + const auto t131 = -t105 * t121 + t106 * t128 - t120 * t130 + 2 * t129; + const auto t132 = ea1_y + eb0_y - t72; + const auto t133 = t106 * t132; + const auto t134 = t108 * t17; + const auto t135 = 2 * t134; + const auto t136 = -t17 * t18 - t20 * t21 - t23 * t24; + const auto t137 = t108 * t120; + const auto t138 = t136 * t137; + const auto t139 = t105 * t137; + const auto t140 = t120 * t16; + const auto t141 = t132 * t140; + const auto t142 = eb0_x * t72; + const auto t143 = eb1_z * t72; + const auto t144 = ea1_y * eb1_x; + const auto t145 = -ea0_y * t53 - ea0_y * t55 - ea0_y * t57 - ea0_y * t58 + + eb0_y * t0 + eb0_y * t1 + eb0_y * t11 + eb0_y * t12 - eb0_y * t13 + - eb0_y * t14 - eb0_y * t2 - eb0_y * t3 + eb0_z * t143 + eb1_x * t142 + - eb1_y * t0 - eb1_y * t1 - eb1_y * t11 - eb1_y * t12 + eb1_y * t13 + + eb1_y * t14 + eb1_y * t2 + eb1_y * t3 - t144 * t77 - t40 * t70 + t73 + + t74 + t75 + t76; + const auto t146 = t119 * t145; + const auto t147 = t106 * t146; + const auto t148 = t136 * t147; + const auto t149 = t136 * t16; + const auto t150 = + t119 * (eb0_x * eb0_y - eb0_x * eb1_y - eb0_y * eb1_x + eb1_x * eb1_y); + const auto t151 = t149 * t150; + const auto t152 = 8 * t116; + const auto t153 = t118 * t152; + const auto t154 = t145 * t153; + const auto t155 = t149 * t154; + const auto t156 = t106 * t20; + const auto t157 = 4 * t116; + const auto t158 = t113 * t94; + const auto t159 = t157 * t158; + const auto t160 = t159 * t20; + const auto t161 = t123 * t150; + const auto t162 = t158 * t17; + const auto t163 = 4 * t162; + const auto t164 = t145 * t163; + const auto t165 = t123 * t154; + const auto t166 = t105 * t147 + t108 * t128 + t130 * t146 + 2 * t156 - t160 + + t161 + t164 - t165; + const auto t167 = + t94 * (-t133 - t135 + t138 + t139 + t141 - t148 - t151 + t155 - t166); + const auto t168 = ea1_z + eb0_z - t80; + const auto t169 = t106 * t168; + const auto t170 = t110 * t17; + const auto t171 = 2 * t170; + const auto t172 = t110 * t120; + const auto t173 = t136 * t172; + const auto t174 = t105 * t172; + const auto t175 = t140 * t168; + const auto t176 = eb0_x * t80; + const auto t177 = eb0_y * t80; + const auto t178 = ea1_z * eb1_x; + const auto t179 = ea1_z * eb1_y; + const auto t180 = -ea0_z * t52 - ea0_z * t54 - ea0_z * t57 - ea0_z * t58 + + eb0_z * t11 + eb0_z * t12 - eb0_z * t13 - eb0_z * t14 + eb0_z * t5 + + eb0_z * t6 - eb0_z * t7 - eb0_z * t8 + eb1_x * t176 + eb1_y * t177 + - eb1_z * t11 - eb1_z * t12 + eb1_z * t13 + eb1_z * t14 - eb1_z * t5 + - eb1_z * t6 + eb1_z * t7 + eb1_z * t8 - t178 * t77 - t179 * t68 + t81 + + t82 + t83 + t84; + const auto t181 = t119 * t180; + const auto t182 = t106 * t181; + const auto t183 = t136 * t182; + const auto t184 = + t119 * (eb0_x * eb0_z - eb0_x * eb1_z - eb0_z * eb1_x + eb1_x * eb1_z); + const auto t185 = t149 * t184; + const auto t186 = t153 * t180; + const auto t187 = t149 * t186; + const auto t188 = t106 * t23; + const auto t189 = t159 * t23; + const auto t190 = t123 * t184; + const auto t191 = t163 * t180; + const auto t192 = t123 * t186; + const auto t193 = t105 * t182 + t110 * t128 + t130 * t181 + 2 * t188 - t189 + + t190 + t191 - t192; + const auto t194 = + t94 * (-t169 - t171 + t173 + t174 + t175 - t183 - t185 + t187 - t193); + const auto t195 = t140 * t18; + const auto t196 = 2 * t109; + const auto t197 = t136 * t94; + const auto t198 = t119 * t97; + const auto t199 = 8 * t122; + const auto t200 = 2 * t111 + t126; + const auto t201 = t94 + * (-t106 * t157 * t197 + t107 - t123 * t198 + t123 * t199 + t131 + - t149 * t198 + t149 * t199 - t152 * t162 + t195 + t196 + t200); + const auto t202 = t106 * t21; + const auto t203 = t147 * t27; + const auto t204 = t150 * t28; + const auto t205 = t140 * t21; + const auto t206 = t137 * t27; + const auto t207 = t154 * t28; + const auto t208 = t94 * (t166 - t202 - t203 - t204 + t205 + t206 + t207); + const auto t209 = t106 * t24; + const auto t210 = t182 * t27; + const auto t211 = t184 * t28; + const auto t212 = t140 * t24; + const auto t213 = t172 * t27; + const auto t214 = t186 * t28; + const auto t215 = t94 * (t193 - t209 - t210 - t211 + t212 + t213 + t214); + const auto t216 = ea0_x * t0 + ea0_x * t1 - ea0_x * t2 - ea0_x * t3 + + ea0_x * t5 + ea0_x * t6 - ea0_x * t7 - ea0_x * t8 - ea1_x * t0 + - ea1_x * t1 + ea1_x * t2 + ea1_x * t3 - ea1_x * t5 - ea1_x * t6 + + ea1_x * t7 + ea1_x * t8 - eb0_x * t56 - eb0_x * t59 - eb0_x * t61 + - eb0_x * t62 - t144 * t72 - t178 * t80 + t38 * t72 + t44 * t80 + t78 + + t85 + t89 + t91; + const auto t217 = t105 * t216; + const auto t218 = t106 * t119; + const auto t219 = t119 * t130; + const auto t220 = t10 * t119; + const auto t221 = t123 * t220; + const auto t222 = t163 * t216; + const auto t223 = t153 * t216; + const auto t224 = t123 * t223; + const auto t225 = t221 + t222 - t224; + const auto t226 = t128 * t17 + t216 * t219 + t217 * t218 + t225; + const auto t227 = ea0_x + eb1_x - t77; + const auto t228 = 2 * t17; + const auto t229 = t120 * t136; + const auto t230 = t105 * t120; + const auto t231 = t136 * t216; + const auto t232 = t129 + t136 - t140 * t17 + t149 * t220 - t149 * t223 + t16 + - t17 * t229 + t218 * t231 + t227 * t228 - t227 * t230; + const auto t233 = t94 * (-t105 - t226 - t232); + const auto t234 = t128 * t20; + const auto t235 = ea0_y + eb1_y - t68; + const auto t236 = t228 * t235; + const auto t237 = t140 * t20; + const auto t238 = ea1_x * eb0_y; + const auto t239 = -ea0_y * t0 - ea0_y * t1 - ea0_y * t11 - ea0_y * t12 + + ea0_y * t13 + ea0_y * t14 + ea0_y * t2 + ea0_y * t3 + ea1_y * t0 + + ea1_y * t1 + ea1_y * t11 + ea1_y * t12 - ea1_y * t13 - ea1_y * t14 + - ea1_y * t2 - ea1_y * t3 + eb0_y * t51 + eb0_y * t59 + eb0_y * t60 + + eb0_y * t62 + t179 * t80 - t238 * t63 + t32 * t63 - t46 * t80 - t69 + - t86 - t87 - t92; + const auto t240 = t105 * t239; + const auto t241 = t218 * t240; + const auto t242 = t230 * t235; + const auto t243 = t219 * t239; + const auto t244 = t17 * t239; + const auto t245 = 4 * t158; + const auto t246 = t244 * t245; + const auto t247 = t119 + * (-ea0_y * eb0_x - ea1_x * t68 + eb0_y * t63 - t114 - t144 + 2 * t32 + + t37 + t38); + const auto t248 = t123 * t247; + const auto t249 = t120 * t27; + const auto t250 = t239 * t27; + const auto t251 = t123 * t153 * t239; + const auto t252 = -8 * t116 * t118 * t16 * t239 * t27 + t20 * t249 + + t218 * t250 - t246 + t247 * t28 - t248 + t251; + const auto t253 = + t94 * (-t156 - t234 - t236 + t237 + t241 + t242 + t243 - t252); + const auto t254 = t128 * t23; + const auto t255 = ea0_z + eb1_z - t70; + const auto t256 = t228 * t255; + const auto t257 = t140 * t23; + const auto t258 = ea1_x * eb0_z; + const auto t259 = ea1_y * eb0_z; + const auto t260 = -ea0_z * t11 - ea0_z * t12 + ea0_z * t13 + ea0_z * t14 + - ea0_z * t5 - ea0_z * t6 + ea0_z * t7 + ea0_z * t8 + ea1_z * t11 + + ea1_z * t12 - ea1_z * t13 - ea1_z * t14 + ea1_z * t5 + ea1_z * t6 + - ea1_z * t7 - ea1_z * t8 + eb0_z * t51 + eb0_z * t56 + eb0_z * t60 + + eb0_z * t61 - t258 * t63 - t259 * t72 + t34 * t63 + t40 * t72 - t71 + - t79 - t88 - t90; + const auto t261 = t105 * t260; + const auto t262 = t218 * t261; + const auto t263 = t230 * t255; + const auto t264 = t219 * t260; + const auto t265 = t17 * t260; + const auto t266 = t245 * t265; + const auto t267 = t119 + * (-ea0_z * eb0_x - ea1_x * t70 + eb0_z * t63 - t115 - t178 + 2 * t34 + + t43 + t44); + const auto t268 = t123 * t267; + const auto t269 = t260 * t27; + const auto t270 = t123 * t153 * t260; + const auto t271 = -8 * t116 * t118 * t16 * t260 * t27 + t218 * t269 + + t23 * t249 - t266 + t267 * t28 - t268 + t270; + const auto t272 = + t94 * (-t188 - t254 - t256 + t257 + t262 + t263 + t264 - t271); + const auto t273 = -t22; + const auto t274 = t105 * t18; + const auto t275 = t120 * t274; + const auto t276 = t216 * t27; + const auto t277 = t218 * t276; + const auto t278 = t220 * t28; + const auto t279 = t17 * t27; + const auto t280 = t120 * t279; + const auto t281 = t16 * t276; + const auto t282 = t153 * t281; + const auto t283 = -t25; + const auto t284 = t105 + t283; + const auto t285 = + t94 * (t19 + t226 + t273 - t275 - t277 - t278 + t280 + t282 + t284); + const auto t286 = t17 * t21; + const auto t287 = 2 * t286; + const auto t288 = t21 * t230; + const auto t289 = t136 * t218; + const auto t290 = -8 * t116 * t118 * t136 * t16 * t239 + t149 * t247 + + t20 * t229 + t239 * t289 + t246 + t248 - t251; + const auto t291 = t94 * (t234 - t241 - t243 + t287 - t288 - t290); + const auto t292 = t17 * t24; + const auto t293 = 2 * t292; + const auto t294 = t230 * t24; + const auto t295 = -8 * t116 * t118 * t136 * t16 * t260 + t149 * t267 + + t229 * t23 + t260 * t289 + t266 + t268 - t270; + const auto t296 = t94 * (t254 - t262 - t264 + t293 - t294 - t295); + const auto t297 = -eb1_x * t77 + t57 + t58; + const auto t298 = t297 + t95; + const auto t299 = t108 * t146; + const auto t300 = std::pow(t145, 2); + const auto t301 = t118 * t300; + const auto t302 = t149 * t301; + const auto t303 = t20 * t245; + const auto t304 = -t105 * t113 * t298 * t94 + t124 * t301 + t145 * t303; + const auto t305 = t108 * t20; + const auto t306 = t132 * t16; + const auto t307 = t105 * t299 + t108 * t132 + t146 * t306 + 2 * t305; + const auto t308 = + t119 * (eb0_y * eb0_z - eb0_y * eb1_z - eb0_z * eb1_y + eb1_y * eb1_z); + const auto t309 = t123 * t308; + const auto t310 = t180 * t303; + const auto t311 = t23 * t245; + const auto t312 = t145 * t311; + const auto t313 = 8 * t123; + const auto t314 = t118 * t145; + const auto t315 = t180 * t314; + const auto t316 = t313 * t315; + const auto t317 = t108 * t181; + const auto t318 = t110 * t146; + const auto t319 = 8 * t149; + const auto t320 = t108 * t23; + const auto t321 = t105 * t317 + t110 * t132 + t181 * t306 + 2 * t320; + const auto t322 = t110 * t20; + const auto t323 = t146 * t16; + const auto t324 = t105 * t318 + t108 * t168 + t168 * t323 + 2 * t322; + const auto t325 = -t94 + * (t136 * t317 + t136 * t318 + t149 * t308 + t309 + t310 + t312 + + t315 * t319 + t316 + t321 + t324); + const auto t326 = t108 * t18 + t160 - t161 - t164 + t165 + t18 * t323; + const auto t327 = + t94 * (t133 + t135 - t138 - t139 - t141 + t148 + t151 - t155 - t326); + const auto t328 = -t21 * t323; + const auto t329 = 2 * t107; + const auto t330 = t119 * t298; + const auto t331 = 4 * t197; + const auto t332 = 8 * t158; + const auto t333 = t94 + * (t108 * t145 * t331 + t109 - t123 * t330 + t145 * t20 * t332 + - t149 * t330 + t200 + t301 * t313 + 8 * t302 + t307 + t328 + t329); + const auto t334 = t108 * t24; + const auto t335 = t24 * t323; + const auto t336 = t27 * t317; + const auto t337 = t27 * t318; + const auto t338 = t28 * t308; + const auto t339 = 8 * t28; + const auto t340 = t315 * t339; + const auto t341 = t309 + t310 + t312 + t316 - t336 - t337 - t338 - t340; + const auto t342 = t94 * (t321 - t334 - t335 + t341); + const auto t343 = t136 * t146; + const auto t344 = t108 * t119; + const auto t345 = t119 + * (-ea0_x * eb1_y + ea1_y * t77 + eb1_x * t72 - t142 - 2 * t144 - t238 + + t31 + t32); + const auto t346 = t216 * t314; + const auto t347 = t149 * t345 + t17 * t343 + t231 * t344 + t319 * t346; + const auto t348 = t119 * t306; + const auto t349 = t132 * t17 + t216 * t348 + t217 * t344; + const auto t350 = t123 * t345; + const auto t351 = t20 * t216; + const auto t352 = t245 * t351; + const auto t353 = t313 * t346; + const auto t354 = 2 * t20; + const auto t355 = t105 * t146; + const auto t356 = + t134 + t17 * t323 + t227 * t354 + t227 * t355 + t350 + t352 + t353; + const auto t357 = -t94 * (t347 + t349 + t356); + const auto t358 = t119 * (t15 + t4); + const auto t359 = -2 * t108 * t136 * t239 * t94 + - 8 * t118 * t136 * t145 * t16 * t239 + t136 + t149 * t358 + t20 * t343; + const auto t360 = -t239 * t303; + const auto t361 = t123 * t358; + const auto t362 = t239 * t314; + const auto t363 = -t313 * t362; + const auto t364 = + t132 * t20 - t239 * t348 - t240 * t344 + t360 + t361 + t363; + const auto t365 = t16 + t20 * t323 + t235 * t354 + t235 * t355 + t305; + const auto t366 = t94 * (-t105 - t359 - t364 - t365); + const auto t367 = t132 * t23; + const auto t368 = t255 * t354; + const auto t369 = t23 * t323; + const auto t370 = t255 * t355; + const auto t371 = t261 * t344; + const auto t372 = t260 * t348; + const auto t373 = t119 + * (-ea0_z * eb0_y - ea1_y * t70 + eb0_z * t72 - t143 - t179 + 2 * t40 + + t45 + t46); + const auto t374 = t123 * t373; + const auto t375 = t20 * t260; + const auto t376 = t245 * t375; + const auto t377 = t260 * t314; + const auto t378 = t313 * t377; + const auto t379 = t136 * t260 * t344 + t149 * t373 - t23 * t343 + + t319 * t377 + t374 + t376 + t378; + const auto t380 = + t94 * (-t320 - t367 - t368 - t369 - t370 + t371 + t372 + t379); + const auto t381 = t18 * t20; + const auto t382 = t146 * t274 + t350 + t352 + t353 + 2 * t381; + const auto t383 = 8 * t281; + const auto t384 = -t146 * t279 - t276 * t344 - t28 * t345 - t314 * t383; + const auto t385 = t94 * (t349 + t382 + t384); + const auto t386 = t21 * t355 + t22; + const auto t387 = -t19; + const auto t388 = t146 * t27; + const auto t389 = + -t20 * t388 + t250 * t344 - t28 * t358 + t339 * t362 + t387; + const auto t390 = t94 * (t284 + t364 + t386 + t389); + const auto t391 = t20 * t24; + const auto t392 = 2 * t391; + const auto t393 = t24 * t355; + const auto t394 = -t23 * t388 + t269 * t344 + t28 * t373 + t339 * t377 + - t374 - t376 - t378; + const auto t395 = t94 * (t367 - t371 - t372 + t392 + t393 + t394); + const auto t396 = t297 + t96; + const auto t397 = t110 * t181; + const auto t398 = std::pow(t180, 2); + const auto t399 = t118 * t398; + const auto t400 = t149 * t399; + const auto t401 = -t105 * t113 * t396 * t94 + t124 * t399 + t180 * t311; + const auto t402 = t110 * t23; + const auto t403 = t16 * t181; + const auto t404 = t105 * t397 + t110 * t168 + t168 * t403 + 2 * t402; + const auto t405 = t110 * t18 + t18 * t403 + t189 - t190 - t191 + t192; + const auto t406 = + t94 * (t169 + t171 - t173 - t174 - t175 + t183 + t185 - t187 - t405); + const auto t407 = t110 * t21; + const auto t408 = t21 * t403; + const auto t409 = t94 * (t324 + t341 - t407 - t408); + const auto t410 = -t24 * t403; + const auto t411 = t119 * t396; + const auto t412 = t94 + * (t110 * t180 * t331 + t111 - t123 * t411 + t126 - t149 * t411 + + t180 * t23 * t332 + t196 + t313 * t399 + t329 + 8 * t400 + t404 + + t410); + const auto t413 = t136 * t181; + const auto t414 = t110 * t119; + const auto t415 = t119 + * (-ea0_x * eb1_z + ea1_z * t77 + eb1_x * t80 - t176 - 2 * t178 - t258 + + t33 + t34); + const auto t416 = t118 * t180; + const auto t417 = t216 * t416; + const auto t418 = t149 * t415 + t17 * t413 + t231 * t414 + t319 * t417; + const auto t419 = t119 * t16; + const auto t420 = t168 * t419; + const auto t421 = t168 * t17 + t216 * t420 + t217 * t414; + const auto t422 = t123 * t415; + const auto t423 = t216 * t23; + const auto t424 = t245 * t423; + const auto t425 = t313 * t417; + const auto t426 = 2 * t23; + const auto t427 = t105 * t181; + const auto t428 = + t17 * t403 + t170 + t227 * t426 + t227 * t427 + t422 + t424 + t425; + const auto t429 = -t94 * (t418 + t421 + t428); + const auto t430 = t119 + * (-ea0_y * eb1_z + ea1_z * t68 + eb1_y * t80 - t177 - 2 * t179 - t259 + + t39 + t40); + const auto t431 = -2 * t110 * t136 * t239 * t94 + - 8 * t118 * t136 * t16 * t180 * t239 + t149 * t430 + t20 * t413; + const auto t432 = t168 * t20 - t239 * t420 - t240 * t414; + const auto t433 = t23 * t239; + const auto t434 = -t245 * t433; + const auto t435 = t123 * t430; + const auto t436 = t239 * t416; + const auto t437 = -t313 * t436; + const auto t438 = + t20 * t403 + t235 * t426 + t235 * t427 + t322 + t434 + t435 + t437; + const auto t439 = t94 * (-t431 - t432 - t438); + const auto t440 = t119 * (t15 + t9); + const auto t441 = -2 * t110 * t136 * t260 * t94 + - 8 * t118 * t136 * t16 * t180 * t260 + t136 + t149 * t440 + t23 * t413; + const auto t442 = -t260 * t311; + const auto t443 = t123 * t440; + const auto t444 = t260 * t416; + const auto t445 = -t313 * t444; + const auto t446 = + t105 + t168 * t23 - t260 * t420 - t261 * t414 + t442 + t443 + t445; + const auto t447 = t16 + t23 * t403 + t255 * t426 + t255 * t427 + t402; + const auto t448 = t94 * (-t441 - t446 - t447); + const auto t449 = t18 * t23; + const auto t450 = t181 * t274 + t422 + t424 + t425 + 2 * t449; + const auto t451 = -t181 * t279 - t276 * t414 - t28 * t415 - t383 * t416; + const auto t452 = t94 * (t421 + t450 + t451); + const auto t453 = t21 * t23; + const auto t454 = t21 * t427 + t434 + t435 + t437 + 2 * t453; + const auto t455 = t181 * t27; + const auto t456 = -t20 * t455 + t250 * t414 - t28 * t430 + t339 * t436; + const auto t457 = t94 * (t432 + t454 + t456); + const auto t458 = t24 * t427 + t25; + const auto t459 = + -t23 * t455 + t269 * t414 + t273 - t28 * t440 + t339 * t444 + t387; + const auto t460 = t94 * (t446 + t458 + t459); + const auto t461 = t94 * (t202 + t203 + t204 - t205 - t206 - t207 + t326); + const auto t462 = t94 * (t209 + t210 + t211 - t212 - t213 - t214 + t405); + const auto t463 = t18 * t419; + const auto t464 = t216 * t463; + const auto t465 = t94 * (t225 + t232 + t387 - t464); + const auto t466 = t239 * t463; + const auto t467 = t94 * (t156 + t236 - t237 - t242 - t290 - t381 + t466); + const auto t468 = t260 * t463; + const auto t469 = t94 * (t188 + t256 - t257 - t263 - t295 - t449 + t468); + const auto t470 = t94 + * (-t221 - t222 + t224 + t26 + t275 + t277 + t278 - t280 - t282 + t464); + const auto t471 = t94 * (-t252 - t287 + t288 + t381 - t466); + const auto t472 = t94 * (-t271 - t293 + t294 + t449 - t468); + const auto t473 = t94 + * (-t309 - t310 - t312 - t316 + t334 + t335 + t336 + t337 + t338 + t340 + + t407 + t408); + const auto t474 = t21 * t419; + const auto t475 = -t216 * t474 - t286; + const auto t476 = t94 * (t356 + t384 + t475); + const auto t477 = t239 * t474 + t360 + t361 + t363; + const auto t478 = t94 * (-2 * t22 + t283 + t365 + t389 + t477); + const auto t479 = t260 * t474; + const auto t480 = t94 * (t320 + t368 + t369 + t370 + t394 - t453 + t479); + const auto t481 = t94 * (-t347 - t382 - t475); + const auto t482 = t94 * (-t359 - t386 - t477); + const auto t483 = t94 * (t379 - t392 - t393 + t453 - t479); + const auto t484 = t24 * t419; + const auto t485 = -t216 * t484 - t292; + const auto t486 = t94 * (t428 + t451 + t485); + const auto t487 = t239 * t484 - t391; + const auto t488 = t94 * (t438 + t456 + t487); + const auto t489 = t260 * t484 + t442 + t443 + t445; + const auto t490 = t94 * (-2 * t25 + t447 + t459 + t489); + const auto t491 = t94 * (-t418 - t450 - t485); + const auto t492 = t94 * (-t431 - t454 - t487); + const auto t493 = t94 * (-t441 - t458 - t489); + const auto t494 = std::pow(t17, 2); + const auto t495 = t119 * t227; + const auto t496 = t217 * t495; + const auto t497 = t17 * t216 * t419; + const auto t498 = std::pow(t216, 2); + const auto t499 = t118 * t498; + const auto t500 = t17 * t20; + const auto t501 = + ea0_x * ea0_y - ea0_x * ea1_y - ea0_y * ea1_x + ea1_x * ea1_y; + const auto t502 = t123 * t501; + const auto t503 = t149 * t94; + const auto t504 = t217 * t94; + const auto t505 = t16 * t94; + const auto t506 = t119 + * (4 * t105 * t113 * t118 * t216 * t239 + t105 * t227 * t239 * t94 + + 4 * t118 * t136 * t16 * t216 * t239 + t136 * t17 * t239 * t94 + + t16 * t17 * t239 * t94 - t197 * t351 - t235 * t504 - t351 * t505 + - t500 - t501 * t503 - t502 * t94); + const auto t507 = t17 * t23; + const auto t508 = + ea0_x * ea0_z - ea0_x * ea1_z - ea0_z * ea1_x + ea1_x * ea1_z; + const auto t509 = t123 * t508; + const auto t510 = t119 + * (4 * t105 * t113 * t118 * t216 * t260 + t105 * t227 * t260 * t94 + + 4 * t118 * t136 * t16 * t216 * t260 + t136 * t17 * t260 * t94 + + t16 * t17 * t260 * t94 - t197 * t423 - t255 * t504 - t423 * t505 + - t503 * t508 - t507 - t509 * t94); + const auto t511 = 4 * t276; + const auto t512 = t94 + * (-t102 * t119 * t123 + 2 * t102 * t16 * t27 * t94 + + 8 * t105 * t113 * t118 * t498 + 2 * t105 * t18 * t216 * t94 - t105 + - t17 * t511 * t94 - t339 * t499 + t494 + t496 + t497); + const auto t513 = t21 * t217; + const auto t514 = t239 * t279; + const auto t515 = t118 * t216; + const auto t516 = t118 * t383; + const auto t517 = -t119 * t20 * t276 - t119 * t28 * t501 + t119 * t502 + + t119 * t514 - t239 * t313 * t515 + t239 * t516 + t500; + const auto t518 = t94 * (t119 * t513 - t240 * t495 - t244 * t419 + t517); + const auto t519 = t217 * t24; + const auto t520 = t260 * t279; + const auto t521 = -t119 * t23 * t276 - t119 * t28 * t508 + t119 * t509 + + t119 * t520 - t260 * t313 * t515 + t260 * t516 + t507; + const auto t522 = t94 * (t119 * t519 - t261 * t495 - t265 * t419 + t521); + const auto t523 = t104 + t99; + const auto t524 = t123 * t523; + const auto t525 = t28 * t523; + const auto t526 = std::pow(t239, 2); + const auto t527 = t118 * t526; + const auto t528 = 4 * t28; + const auto t529 = t20 * t239; + const auto t530 = t119 * t235; + const auto t531 = t105 - std::pow(t20, 2) + t240 * t530 + t419 * t529; + const auto t532 = t20 * t23; + const auto t533 = + ea0_y * ea0_z - ea0_y * ea1_z - ea0_z * ea1_y + ea1_y * ea1_z; + const auto t534 = t123 * t533; + const auto t535 = t235 * t261; + const auto t536 = t240 * t255; + const auto t537 = t27 * t375; + const auto t538 = t27 * t433; + const auto t539 = t239 * t260; + const auto t540 = t118 * t539; + const auto t541 = t119 + * (-t124 * t540 + t28 * t533 * t94 + t375 * t505 + t433 * t505 + + t528 * t540 - t532 - t534 * t94 + t535 * t94 + t536 * t94 + - t537 * t94 - t538 * t94); + const auto t542 = + t94 * (-t119 * t239 * t274 + t217 * t530 + t351 * t419 + t517); + const auto t543 = t119 * t149; + const auto t544 = t94 + * (8 * t105 * t113 * t118 * t526 + 8 * t118 * t136 * t16 * t526 + - t119 * t21 * t240 - t119 * t524 - t331 * t529 - t523 * t543 + - t531); + const auto t545 = t119 * t136; + const auto t546 = t119 * t534 + t313 * t540 + t319 * t540 - t375 * t545 + - t433 * t545 + t532 + t533 * t543; + const auto t547 = + t94 * (-t119 * t24 * t240 - t119 * t535 - t375 * t419 + t546); + const auto t548 = t101 + t104; + const auto t549 = t123 * t548; + const auto t550 = t28 * t548; + const auto t551 = std::pow(t260, 2); + const auto t552 = t118 * t551; + const auto t553 = t23 * t260; + const auto t554 = t119 * t255; + const auto t555 = t105 - std::pow(t23, 2) + t261 * t554 + t419 * t553; + const auto t556 = + t94 * (-t119 * t260 * t274 + t217 * t554 + t419 * t423 + t521); + const auto t557 = + t94 * (-t119 * t21 * t261 - t119 * t536 - t419 * t433 + t546); + const auto t558 = t94 + * (8 * t105 * t113 * t118 * t551 + 8 * t118 * t136 * t16 * t551 + - t119 * t24 * t261 - t119 * t549 - t331 * t553 - t543 * t548 + - t555); + const auto t559 = t124 * t94; + const auto t560 = 2 * t118; + const auto t561 = t505 * t511; + const auto t562 = t560 + * (4 * t105 * t113 * t216 * t239 * t94 + t105 * t18 * t239 + + t16 * t27 * t501 + t20 * t216 * t27 - t239 * t561 - t502 - t513 + - t514); + const auto t563 = t560 + * (4 * t105 * t113 * t216 * t260 * t94 + t105 * t18 * t260 + + t16 * t27 * t508 + t216 * t23 * t27 - t260 * t561 - t509 - t519 + - t520); + const auto t564 = t560 + * (t105 * t21 * t260 + t105 * t239 * t24 + + 4 * t16 * t239 * t260 * t27 * t94 + t16 * t27 * t533 - t534 - t537 + - t538 - t539 * t559); + hess[0] = t119 * (-t125 - t127 - t131); + hess[1] = t167; + hess[2] = t194; + hess[3] = t201; + hess[4] = t208; + hess[5] = t215; + hess[6] = t233; + hess[7] = t253; + hess[8] = t272; + hess[9] = t285; + hess[10] = t291; + hess[11] = t296; + hess[12] = t167; + hess[13] = t119 + * (-t127 + t136 * t16 * t298 * t94 - t136 * t299 - 4 * t302 - t304 + - t307); + hess[14] = t325; + hess[15] = t327; + hess[16] = t333; + hess[17] = t342; + hess[18] = t357; + hess[19] = t366; + hess[20] = t380; + hess[21] = t385; + hess[22] = t390; + hess[23] = t395; + hess[24] = t194; + hess[25] = t325; + hess[26] = t119 + * (-t127 + t136 * t16 * t396 * t94 - t136 * t397 - 4 * t400 - t401 + - t404); + hess[27] = t406; + hess[28] = t409; + hess[29] = t412; + hess[30] = t429; + hess[31] = t439; + hess[32] = t448; + hess[33] = t452; + hess[34] = t457; + hess[35] = t460; + hess[36] = t201; + hess[37] = t327; + hess[38] = t406; + hess[39] = t119 * (-t112 - t125 - t195); + hess[40] = t461; + hess[41] = t462; + hess[42] = t465; + hess[43] = t467; + hess[44] = t469; + hess[45] = t470; + hess[46] = t471; + hess[47] = t472; + hess[48] = t208; + hess[49] = t333; + hess[50] = t409; + hess[51] = t461; + hess[52] = t119 + * (-t107 + 2 * t108 * t145 * t27 * t94 - t111 + + 4 * t118 * t16 * t27 * t300 - t28 * t298 * t94 - t304 - t328); + hess[53] = t473; + hess[54] = t476; + hess[55] = t478; + hess[56] = t480; + hess[57] = t481; + hess[58] = t482; + hess[59] = t483; + hess[60] = t215; + hess[61] = t342; + hess[62] = t412; + hess[63] = t462; + hess[64] = t473; + hess[65] = t119 + * (-t107 - t109 + 2 * t110 * t180 * t27 * t94 + + 4 * t118 * t16 * t27 * t398 - t28 * t396 * t94 - t401 - t410); + hess[66] = t486; + hess[67] = t488; + hess[68] = t490; + hess[69] = t491; + hess[70] = t492; + hess[71] = t493; + hess[72] = t233; + hess[73] = t357; + hess[74] = t429; + hess[75] = t465; + hess[76] = t476; + hess[77] = t486; + hess[78] = t119 + * (-t100 + t102 * t105 * t113 * t94 + t102 * t136 * t16 * t94 - t103 + - t119 * t17 * t231 - t124 * t499 - 4 * t149 * t499 - t494 - t496 + - t497 + t51 + t56 + t59 + t60 + t61 + t62 - t98); + hess[79] = t506; + hess[80] = t510; + hess[81] = t512; + hess[82] = t518; + hess[83] = t522; + hess[84] = t253; + hess[85] = t366; + hess[86] = t439; + hess[87] = t467; + hess[88] = t478; + hess[89] = t488; + hess[90] = t506; + hess[91] = t119 + * (-t119 * t20 * t250 - t124 * t527 + t524 * t94 - t525 * t94 + + t527 * t528 + t531); + hess[92] = t541; + hess[93] = t542; + hess[94] = t544; + hess[95] = t547; + hess[96] = t272; + hess[97] = t380; + hess[98] = t448; + hess[99] = t469; + hess[100] = t480; + hess[101] = t490; + hess[102] = t510; + hess[103] = t541; + hess[104] = t119 + * (-t119 * t23 * t269 - t124 * t552 + t528 * t552 + t549 * t94 + - t550 * t94 + t555); + hess[105] = t556; + hess[106] = t557; + hess[107] = t558; + hess[108] = t285; + hess[109] = t385; + hess[110] = t452; + hess[111] = t470; + hess[112] = t481; + hess[113] = t491; + hess[114] = t512; + hess[115] = t542; + hess[116] = t556; + hess[117] = t560 + * (t102 * t105 * t113 + t102 * t136 * t16 - 2 * t216 * t274 + - t228 * t231 - 4 * t498 * t503 - t498 * t559); + hess[118] = t562; + hess[119] = t563; + hess[120] = t291; + hess[121] = t390; + hess[122] = t457; + hess[123] = t471; + hess[124] = t482; + hess[125] = t492; + hess[126] = t518; + hess[127] = t544; + hess[128] = t557; + hess[129] = t562; + hess[130] = t560 + * (t105 * t113 * t523 + 2 * t105 * t21 * t239 + + 4 * t16 * t27 * t526 * t94 - t250 * t354 - t525 - t526 * t559); + hess[131] = t564; + hess[132] = t296; + hess[133] = t395; + hess[134] = t460; + hess[135] = t472; + hess[136] = t483; + hess[137] = t493; + hess[138] = t522; + hess[139] = t547; + hess[140] = t558; + hess[141] = t563; + hess[142] = t564; + hess[143] = t560 + * (t105 * t113 * t548 + 2 * t105 * t24 * t260 + + 4 * t16 * t27 * t551 * t94 - t269 * t426 - t550 - t551 * t559); +} + +void face_normal_squared_norm_gradient( + double t0_x, + double t0_y, + double t0_z, + double t1_x, + double t1_y, + double t1_z, + double t2_x, + double t2_y, + double t2_z, + double grad[9]) +{ + const auto t0 = -t2_y; + const auto t1 = t0 + t1_y; + const auto t2 = t0_x - t1_x; + const auto t3 = t0 + t0_y; + const auto t4 = -t2_x; + const auto t5 = t0_x + t4; + const auto t6 = t0_y - t1_y; + const auto t7 = t2 * t3 - t5 * t6; + const auto t8 = -t2_z; + const auto t9 = t1_z + t8; + const auto t10 = t0_z + t8; + const auto t11 = t0_z - t1_z; + const auto t12 = t10 * t2 - t11 * t5; + const auto t13 = t1_x + t4; + const auto t14 = t10 * t6 - t11 * t3; + const auto t15 = t10 * t2 - t11 * t5; + grad[0] = 2 * t1 * t7 + 2 * t12 * t9; + grad[1] = -2 * t13 * t7 + 2 * t14 * t9; + grad[2] = -2 * t1 * t14 - 2 * t13 * t15; + grad[3] = -2 * t10 * t15 - 2 * t3 * t7; + grad[4] = -2 * t10 * t14 + 2 * t5 * t7; + grad[5] = 2 * t12 * t5 + 2 * t14 * t3; + grad[6] = 2 * t11 * t12 + 2 * t6 * t7; + grad[7] = 2 * t11 * t14 - 2 * t2 * t7; + grad[8] = -2 * t14 * t6 - 2 * t15 * t2; +} + +// hess is (81�1) flattened in column-major order +void face_normal_squared_norm_hessian( + double t0_x, + double t0_y, + double t0_z, + double t1_x, + double t1_y, + double t1_z, + double t2_x, + double t2_y, + double t2_z, + double hess[81]) +{ + const auto t0 = -t2_y; + const auto t1 = t0 + t1_y; + const auto t2 = std::pow(t1, 2); + const auto t3 = -t2_z; + const auto t4 = t1_z + t3; + const auto t5 = std::pow(t4, 2); + const auto t6 = -t2_x; + const auto t7 = t1_x + t6; + const auto t8 = 2 * t7; + const auto t9 = -t1 * t8; + const auto t10 = -t4 * t8; + const auto t11 = t0 + t0_y; + const auto t12 = t1 * t11; + const auto t13 = t0_z + t3; + const auto t14 = t13 * t4; + const auto t15 = -2 * t12 - 2 * t14; + const auto t16 = t0_x + t6; + const auto t17 = t0_y - t1_y; + const auto t18 = t16 * t17; + const auto t19 = -t18; + const auto t20 = t0_x - t1_x; + const auto t21 = t11 * t20; + const auto t22 = t19 + t21; + const auto t23 = 2 * t1 * t16 + 2 * t22; + const auto t24 = t0_z - t1_z; + const auto t25 = t16 * t24; + const auto t26 = -t25; + const auto t27 = t13 * t20; + const auto t28 = t26 + t27; + const auto t29 = 2 * t16 * t4 + 2 * t28; + const auto t30 = t1 * t17; + const auto t31 = t24 * t4; + const auto t32 = 2 * t30 + 2 * t31; + const auto t33 = -2 * t1 * t20 - 2 * t22; + const auto t34 = -2 * t20 * t4 - 2 * t28; + const auto t35 = std::pow(t7, 2); + const auto t36 = -2 * t1 * t4; + const auto t37 = t18 - t21; + const auto t38 = 2 * t11 * t7 + 2 * t37; + const auto t39 = t16 * t7; + const auto t40 = -2 * t14 - 2 * t39; + const auto t41 = t11 * t24; + const auto t42 = -t41; + const auto t43 = t13 * t17; + const auto t44 = t42 + t43; + const auto t45 = 2 * t11 * t4 + 2 * t44; + const auto t46 = -2 * t17 * t7 - 2 * t37; + const auto t47 = t20 * t7; + const auto t48 = 2 * t31 + 2 * t47; + const auto t49 = -2 * t17 * t4 - 2 * t44; + const auto t50 = t25 - t27; + const auto t51 = 2 * t13 * t7 + 2 * t50; + const auto t52 = t41 - t43; + const auto t53 = 2 * t1 * t13 + 2 * t52; + const auto t54 = -2 * t12 - 2 * t39; + const auto t55 = -2 * t24 * t7 - 2 * t50; + const auto t56 = -2 * t1 * t24 - 2 * t52; + const auto t57 = 2 * t30 + 2 * t47; + const auto t58 = std::pow(t11, 2); + const auto t59 = std::pow(t13, 2); + const auto t60 = 2 * t16; + const auto t61 = -t11 * t60; + const auto t62 = -t13 * t60; + const auto t63 = t11 * t17; + const auto t64 = t13 * t24; + const auto t65 = -2 * t63 - 2 * t64; + const auto t66 = 2 * t19 + 4 * t21; + const auto t67 = 2 * t26 + 4 * t27; + const auto t68 = std::pow(t16, 2); + const auto t69 = -2 * t11 * t13; + const auto t70 = 4 * t16 * t17 - 2 * t21; + const auto t71 = t16 * t20; + const auto t72 = -2 * t64 - 2 * t71; + const auto t73 = 2 * t42 + 4 * t43; + const auto t74 = 4 * t16 * t24 - 2 * t27; + const auto t75 = 4 * t11 * t24 - 2 * t43; + const auto t76 = -2 * t63 - 2 * t71; + const auto t77 = std::pow(t17, 2); + const auto t78 = std::pow(t24, 2); + const auto t79 = 2 * t20; + const auto t80 = -t17 * t79; + const auto t81 = -t24 * t79; + const auto t82 = std::pow(t20, 2); + const auto t83 = -2 * t17 * t24; + hess[0] = 2 * t2 + 2 * t5; + hess[1] = t9; + hess[2] = t10; + hess[3] = t15; + hess[4] = t23; + hess[5] = t29; + hess[6] = t32; + hess[7] = t33; + hess[8] = t34; + hess[9] = t9; + hess[10] = 2 * t35 + 2 * t5; + hess[11] = t36; + hess[12] = t38; + hess[13] = t40; + hess[14] = t45; + hess[15] = t46; + hess[16] = t48; + hess[17] = t49; + hess[18] = t10; + hess[19] = t36; + hess[20] = 2 * t2 + 2 * t35; + hess[21] = t51; + hess[22] = t53; + hess[23] = t54; + hess[24] = t55; + hess[25] = t56; + hess[26] = t57; + hess[27] = t15; + hess[28] = t38; + hess[29] = t51; + hess[30] = 2 * t58 + 2 * t59; + hess[31] = t61; + hess[32] = t62; + hess[33] = t65; + hess[34] = t66; + hess[35] = t67; + hess[36] = t23; + hess[37] = t40; + hess[38] = t53; + hess[39] = t61; + hess[40] = 2 * t59 + 2 * t68; + hess[41] = t69; + hess[42] = t70; + hess[43] = t72; + hess[44] = t73; + hess[45] = t29; + hess[46] = t45; + hess[47] = t54; + hess[48] = t62; + hess[49] = t69; + hess[50] = 2 * t58 + 2 * t68; + hess[51] = t74; + hess[52] = t75; + hess[53] = t76; + hess[54] = t32; + hess[55] = t46; + hess[56] = t55; + hess[57] = t65; + hess[58] = t70; + hess[59] = t74; + hess[60] = 2 * t77 + 2 * t78; + hess[61] = t80; + hess[62] = t81; + hess[63] = t33; + hess[64] = t48; + hess[65] = t56; + hess[66] = t66; + hess[67] = t72; + hess[68] = t75; + hess[69] = t80; + hess[70] = 2 * t78 + 2 * t82; + hess[71] = t83; + hess[72] = t34; + hess[73] = t49; + hess[74] = t57; + hess[75] = t67; + hess[76] = t73; + hess[77] = t76; + hess[78] = t81; + hess[79] = t83; + hess[80] = 2 * t77 + 2 * t82; +} + +void face_term_aux_gradient( + double t0_x, + double t0_y, + double t0_z, + double t1_x, + double t1_y, + double t1_z, + double t2_x, + double t2_y, + double t2_z, + double p1_x, + double p1_y, + double p1_z, + double p2_x, + double p2_y, + double p2_z, + double grad[15]) +{ + const auto t0 = p1_x - p2_x; + const auto t1 = p1_y - p2_y; + const auto t2 = p1_z - p2_z; + const auto t3 = std::pow(t0, 2) + std::pow(t1, 2) + std::pow(t2, 2); + const auto t4 = t0_x - t1_x; + const auto t5 = -t2_z; + const auto t6 = t0_z + t5; + const auto t7 = t4 * t6; + const auto t8 = -t2_x; + const auto t9 = t0_x + t8; + const auto t10 = t0_z - t1_z; + const auto t11 = t10 * t9; + const auto t12 = -t11 + t7; + const auto t13 = -t2_y; + const auto t14 = t0_y + t13; + const auto t15 = t14 * t4; + const auto t16 = t0_y - t1_y; + const auto t17 = t16 * t9; + const auto t18 = t15 - t17; + const auto t19 = -t10 * t14 + t16 * t6; + const auto t20 = std::pow(t18, 2) + std::pow(t19, 2); + const auto t21 = std::pow(t12, 2) + t20; + const auto t22 = std::pow(t21 * t3, -1.0 / 2.0); + const auto t23 = p2_y - t0_y; + const auto t24 = t1_z + t5; + const auto t25 = p2_z - t0_z; + const auto t26 = t13 + t1_y; + const auto t27 = p2_x - t0_x; + const auto t28 = t18 * t25 + t19 * t27; + const auto t29 = -t12 * t23 + t28; + const auto t30 = t29 / t21; + const auto t31 = t1_x + t8; + const auto t32 = t10 * t9; + const auto t33 = -t32 + t4 * t6; + const auto t34 = t20 + std::pow(t33, 2); + const auto t35 = std::pow(t3 * t34, -1.0 / 2.0); + const auto t36 = -t23 * t33 + t28; + const auto t37 = t36 / t34; + const auto t38 = 1.0 / t3; + const auto t39 = t35 * t36 * t38; + const auto t40 = t29 * t38; + grad[0] = + t22 * (-t19 - t23 * t24 + t25 * t26 - t30 * (t12 * t24 + t18 * t26)); + grad[1] = t22 + * (t24 * t27 - t25 * t31 - t30 * (-t18 * t31 + t19 * t24) - t32 + + t4 * t6); + grad[2] = t22 + * (-t15 + t17 + t23 * t31 - t26 * t27 + t30 * (t12 * t31 + t19 * t26)); + grad[3] = t35 * (-t14 * t25 + t23 * t6 + t37 * (t12 * t6 + t14 * t18)); + grad[4] = -t35 * (-t25 * t9 + t27 * t6 + t37 * (t18 * t9 - t19 * t6)); + grad[5] = t35 * (t14 * t27 - t23 * t9 - t37 * (t14 * t19 + t33 * t9)); + grad[6] = -t35 * (t10 * t23 - t16 * t25 + t37 * (t10 * t33 + t16 * t18)); + grad[7] = t35 * (t10 * t27 - t25 * t4 + t37 * (-t10 * t19 + t18 * t4)); + grad[8] = t35 * (-t16 * t27 + t23 * t4 + t37 * (t12 * t4 + t16 * t19)); + grad[9] = -t0 * t39; + grad[10] = -t1 * t39; + grad[11] = -t2 * t39; + grad[12] = t22 * (t0 * t40 + t19); + grad[13] = t22 * (t1 * t40 + t11 - t7); + grad[14] = t22 * (t18 + t2 * t40); +} + +// hess is (225�1) flattened in column-major order +void face_term_aux_hessian( + double t0_x, + double t0_y, + double t0_z, + double t1_x, + double t1_y, + double t1_z, + double t2_x, + double t2_y, + double t2_z, + double p1_x, + double p1_y, + double p1_z, + double p2_x, + double p2_y, + double p2_z, + double hess[225]) +{ + const auto t0 = -t2_y; + const auto t1 = t0 + t1_y; + const auto t2 = std::pow(t1, 2); + const auto t3 = -t2_z; + const auto t4 = t1_z + t3; + const auto t5 = std::pow(t4, 2); + const auto t6 = -t0_y; + const auto t7 = p2_y + t6; + const auto t8 = -t1_x; + const auto t9 = t0_x + t8; + const auto t10 = -t9; + const auto t11 = t0_z + t3; + const auto t12 = -t11; + const auto t13 = t10 * t12; + const auto t14 = -t2_x; + const auto t15 = t0_x + t14; + const auto t16 = -t15; + const auto t17 = -t1_z; + const auto t18 = t0_z + t17; + const auto t19 = -t18; + const auto t20 = t16 * t19; + const auto t21 = t13 - t20; + const auto t22 = t21 * t7; + const auto t23 = -t0_x; + const auto t24 = p2_x + t23; + const auto t25 = -t1_y; + const auto t26 = t0_y + t25; + const auto t27 = t11 * t26; + const auto t28 = t0 + t0_y; + const auto t29 = t18 * t28; + const auto t30 = -t29; + const auto t31 = t27 + t30; + const auto t32 = t24 * t31; + const auto t33 = -t0_z; + const auto t34 = p2_z + t33; + const auto t35 = t28 * t9; + const auto t36 = t15 * t26; + const auto t37 = -t36; + const auto t38 = t35 + t37; + const auto t39 = t34 * t38; + const auto t40 = t32 + t39; + const auto t41 = -t22 + t40; + const auto t42 = -t1 * t34 + t31 + t4 * t7; + const auto t43 = -t42; + const auto t44 = t1 * t38; + const auto t45 = t21 * t4 + t44; + const auto t46 = 2 * t45; + const auto t47 = std::pow(t31, 2) + std::pow(t38, 2); + const auto t48 = std::pow(t21, 2) + t47; + const auto t49 = 1.0 / t48; + const auto t50 = t41 * t49; + const auto t51 = -t28; + const auto t52 = -t26; + const auto t53 = t10 * t51 - t16 * t52; + const auto t54 = -t21; + const auto t55 = t1 * t53 - t4 * t54; + const auto t56 = t50 * t55; + const auto t57 = -p2_x; + const auto t58 = p1_x + t57; + const auto t59 = std::pow(t58, 2); + const auto t60 = -p2_y; + const auto t61 = p1_y + t60; + const auto t62 = std::pow(t61, 2); + const auto t63 = -p2_z; + const auto t64 = p1_z + t63; + const auto t65 = std::pow(t64, 2); + const auto t66 = t59 + t62 + t65; + const auto t67 = std::pow(t48 * t66, -1.0 / 2.0); + const auto t68 = t49 * t67; + const auto t69 = t14 + t1_x; + const auto t70 = -t31 * t4 + t38 * t69; + const auto t71 = -t70; + const auto t72 = 2 * t71; + const auto t73 = t11 * t9; + const auto t74 = t15 * t18; + const auto t75 = -t74; + const auto t76 = t73 + t75; + const auto t77 = t24 * t4 - t34 * t69 + t76; + const auto t78 = t41 * t69; + const auto t79 = t45 * t50; + const auto t80 = t1 * t78 - t43 * t71 - t45 * t77 + t71 * t79; + const auto t81 = t1 * t24; + const auto t82 = t69 * t7; + const auto t83 = t38 + t81 - t82; + const auto t84 = -t83; + const auto t85 = t45 * t84; + const auto t86 = t1 * t31 + t21 * t69; + const auto t87 = t79 * t86; + const auto t88 = 2 * t86; + const auto t89 = t11 * t7 - t28 * t34; + const auto t90 = t45 * t89; + const auto t91 = t11 * t21 + t28 * t38; + const auto t92 = t1 * t28; + const auto t93 = t11 * t4; + const auto t94 = t92 + t93; + const auto t95 = t50 * t91; + const auto t96 = t45 * t95; + const auto t97 = 2 * t91; + const auto t98 = -t11 * t31 + t15 * t38; + const auto t99 = std::pow(t48, -2); + const auto t100 = t11 * t24 - t15 * t34; + const auto t101 = -t100; + const auto t102 = t45 * t49; + const auto t103 = t43 * t49; + const auto t104 = t101 * t102 + t103 * t98 + t2_z - t41 * t45 * t98 * t99 + + t50 * (t1 * t15 + t38) + t63; + const auto t105 = t15 * t21 + t28 * t31; + const auto t106 = t24 * t28; + const auto t107 = t15 * t7; + const auto t108 = t106 - t107; + const auto t109 = p2_y + t0; + const auto t110 = t102 * t108 + t103 * t105 - t105 * t41 * t45 * t99 + t109 + + t50 * (t15 * t4 + t76); + const auto t111 = t18 * t21 + t26 * t38; + const auto t112 = t18 * t7 - t26 * t34; + const auto t113 = -t112; + const auto t114 = t1 * t26; + const auto t115 = t18 * t4; + const auto t116 = + -t111 * t41 * t45 * t49 + t111 * t43 + t113 * t45 + t41 * (t114 + t115); + const auto t117 = t18 * t31 - t38 * t9; + const auto t118 = t18 * t24; + const auto t119 = t34 * t9; + const auto t120 = t118 - t119; + const auto t121 = p2_z + t102 * t120 + t103 * t117 - t117 * t41 * t45 * t99 + + t17 + t50 * (-t1 * t9 - t38); + const auto t122 = t21 * t9 + t26 * t31; + const auto t123 = 2 * t122; + const auto t124 = t41 * t99; + const auto t125 = t24 * t26; + const auto t126 = -t125 + t7 * t9; + const auto t127 = t102 * t126 + t122 * t124 * t45 - t122 * t43 * t49 + t1_y + + t50 * (-t4 * t9 - t76) + t60; + const auto t128 = t47 + std::pow(t76, 2); + const auto t129 = 1.0 / t128; + const auto t130 = t40 - t7 * t76; + const auto t131 = t129 * t130; + const auto t132 = t131 * (t4 * t76 + t44) + t42; + const auto t133 = std::pow(t128 * t66, -1.0 / 2.0); + const auto t134 = 1.0 / t66; + const auto t135 = t133 * t134; + const auto t136 = t132 * t135; + const auto t137 = t136 * t58; + const auto t138 = t134 * t61; + const auto t139 = t133 * t138; + const auto t140 = t132 * t139; + const auto t141 = t136 * t64; + const auto t142 = t134 * t79; + const auto t143 = t67 * (-t102 * t31 + t134 * t43 * t58 - t142 * t58); + const auto t144 = t17 + t2_z; + const auto t145 = t67 * (t102 * t21 + t134 * t43 * t61 - t138 * t79 + t144); + const auto t146 = t25 + t2_y; + const auto t147 = + t67 * (-t102 * t38 + t134 * t43 * t64 - t142 * t64 - t146); + const auto t148 = t12 * t52 - t19 * t51; + const auto t149 = t148 * t4 - t53 * t69; + const auto t150 = t149 * t50; + const auto t151 = std::pow(t69, 2); + const auto t152 = t71 * t84; + const auto t153 = t50 * t71; + const auto t154 = t153 * t86; + const auto t155 = t124 * t149; + const auto t156 = t49 * t71; + const auto t157 = -t35 + t36; + const auto t158 = t124 * t71; + const auto t159 = p2_z + t156 * t89 + t158 * t91 + t3 - t49 * t77 * t91 + + t50 * (t157 + t28 * t69); + const auto t160 = 2 * t98; + const auto t161 = t15 * t69; + const auto t162 = t50 * t98; + const auto t163 = -t101 * t71 + t162 * t71 + t41 * (t161 + t93) - t77 * t98; + const auto t164 = t49 * t77; + const auto t165 = t105 * t164 - t105 * t41 * t71 * t99 + t108 * t156 + t2_x + + t50 * (t28 * t4 + t31) + t57; + const auto t166 = t111 * t164 - t111 * t41 * t71 * t99 + t113 * t156 + t1_z + + t50 * (-t157 - t26 * t69) + t63; + const auto t167 = t69 * t9; + const auto t168 = + -t117 * t41 * t49 * t71 + t117 * t77 + t120 * t71 + t41 * (t115 + t167); + const auto t169 = p2_x + t122 * t158 - t122 * t49 * t77 + t126 * t156 + + t50 * (-t26 * t4 - t31) + t8; + const auto t170 = t131 * t70 + t77; + const auto t171 = t135 * t170; + const auto t172 = -t171 * t58; + const auto t173 = -t139 * t170; + const auto t174 = -t171 * t64; + const auto t175 = t134 * t153; + const auto t176 = t67 * (t134 * t58 * t77 - t144 - t156 * t31 - t175 * t58); + const auto t177 = t67 * (t134 * t61 * t77 - t138 * t153 + t156 * t21); + const auto t178 = t67 * (t134 * t64 * t77 - t156 * t38 - t175 * t64 - t69); + const auto t179 = -t1 * t148 + t54 * t69; + const auto t180 = t179 * t50; + const auto t181 = t49 * t86; + const auto t182 = t49 * t84; + const auto t183 = -t73 + t74; + const auto t184 = t124 * t86; + const auto t185 = + t109 + t181 * t89 + t182 * t91 + t184 * t91 - t50 * (t11 * t69 + t183); + const auto t186 = -t27 + t29; + const auto t187 = p2_x - t101 * t49 * t86 + t14 + t182 * t98 + t184 * t98 + + t50 * (t1 * t11 + t186); + const auto t188 = t105 * t84; + const auto t189 = t161 + t92; + const auto t190 = 2 * t105; + const auto t191 = t105 * t50; + const auto t192 = t191 * t86; + const auto t193 = p2_y + t111 * t182 + t111 * t184 - t113 * t49 * t86 + t25 + + t50 * (-t18 * t69 - t183); + const auto t194 = t117 * t182 + t117 * t184 - t120 * t49 * t86 + t1_x + + t50 * (-t1 * t18 - t186) + t57; + const auto t195 = t122 * t50; + const auto t196 = + t122 * t84 + t126 * t86 + t195 * t86 - t41 * (t114 + t167); + const auto t197 = -t131 * t86 - t157 + t81 - t82; + const auto t198 = t135 * t197; + const auto t199 = t198 * t58; + const auto t200 = t139 * t197; + const auto t201 = t198 * t64; + const auto t202 = t134 * t84; + const auto t203 = t134 * t58; + const auto t204 = t50 * t86; + const auto t205 = t67 * (t146 + t181 * t31 + t202 * t58 + t203 * t204); + const auto t206 = t133 + * (t129 * t130 * t134 * t61 * t86 - t129 * t76 * t86 - t138 * t83 - t2_x + - t8); + const auto t207 = t134 * t64; + const auto t208 = t67 * (t181 * t38 + t202 * t64 + t204 * t207); + const auto t209 = t11 * t54 + t51 * t53; + const auto t210 = t209 * t50; + const auto t211 = t124 * t209; + const auto t212 = std::pow(t28, 2); + const auto t213 = std::pow(t11, 2); + const auto t214 = t89 * t98; + const auto t215 = t15 * t41; + const auto t216 = t95 * t98; + const auto t217 = t105 * t89; + const auto t218 = t105 * t95; + const auto t219 = t111 * t89; + const auto t220 = t26 * t28; + const auto t221 = t11 * t18; + const auto t222 = t220 + t221; + const auto t223 = 2 * t111; + const auto t224 = t111 * t95; + const auto t225 = t49 * t89; + const auto t226 = t124 * t91; + const auto t227 = t0_z + t117 * t225 + t117 * t226 - t120 * t49 * t91 + + t50 * (2 * t35 + t37) + t63; + const auto t228 = t49 * t91; + const auto t229 = t0_y + t60; + const auto t230 = + t122 * t225 + t122 * t226 + t126 * t228 + t229 - t50 * (2 * t73 + t75); + const auto t231 = t89 + t95; + const auto t232 = t134 * t67; + const auto t233 = t231 * t232; + const auto t234 = -t233 * t58; + const auto t235 = t138 * t67; + const auto t236 = -t231 * t235; + const auto t237 = -t233 * t64; + const auto t238 = t134 * t89; + const auto t239 = t67 * (t203 * t95 + t228 * t31 + t238 * t58); + const auto t240 = t67 * (t11 + t138 * t89 + t138 * t95 - t21 * t228); + const auto t241 = t2_y + t6; + const auto t242 = t67 * (t207 * t95 + t228 * t38 + t238 * t64 + t241); + const auto t243 = t12 * t148 + t15 * t53; + const auto t244 = t243 * t50; + const auto t245 = t124 * t243; + const auto t246 = std::pow(t15, 2); + const auto t247 = -t101 * t105 + t105 * t162 - t108 * t98 + t11 * t28 * t41; + const auto t248 = t49 * t98; + const auto t249 = t101 * t111 * t49 - t111 * t41 * t98 * t99 + t113 * t248 + + t34 + t50 * (2 * t15 * t26 - t35); + const auto t250 = 2 * t117; + const auto t251 = t15 * t9; + const auto t252 = + -t101 * t117 + t117 * t162 - t120 * t98 + t41 * (t221 + t251); + const auto t253 = t0_x - t101 * t122 * t49 + t122 * t124 * t98 + t126 * t248 + + t50 * (2 * t27 + t30) + t57; + const auto t254 = t100 + t162; + const auto t255 = t232 * t254; + const auto t256 = t255 * t58; + const auto t257 = t235 * t254; + const auto t258 = t255 * t64; + const auto t259 = + t67 * (t101 * t134 * t58 - t11 - t162 * t203 - t248 * t31); + const auto t260 = t67 * (t101 * t138 - t138 * t162 + t21 * t248); + const auto t261 = t23 + t2_x; + const auto t262 = + t67 * (t101 * t134 * t64 - t162 * t207 - t248 * t38 - t261); + const auto t263 = t148 * t28 + t16 * t54; + const auto t264 = t263 * t50; + const auto t265 = t108 * t49; + const auto t266 = t105 * t49; + const auto t267 = -t105 * t111 * t41 * t99 + t111 * t265 + t113 * t266 + + t229 + t50 * (2 * t15 * t18 - t73); + const auto t268 = -t105 * t117 * t41 * t99 + t117 * t265 + t120 * t266 + t24 + + t50 * (2 * t18 * t28 - t27); + const auto t269 = t105 * t126; + const auto t270 = t220 + t251; + const auto t271 = t122 * t191; + const auto t272 = -t106 + t107 + t191; + const auto t273 = t232 * t272; + const auto t274 = t273 * t58; + const auto t275 = t235 * t272; + const auto t276 = t273 * t64; + const auto t277 = + t67 * (t108 * t134 * t58 - t191 * t203 - t241 - t266 * t31); + const auto t278 = t67 * (t108 * t138 - t138 * t191 + t21 * t266 + t261); + const auto t279 = t67 * (t108 * t134 * t64 - t191 * t207 - t266 * t38); + const auto t280 = t19 * t54 + t26 * t53; + const auto t281 = t280 * t50; + const auto t282 = std::pow(t26, 2); + const auto t283 = std::pow(t18, 2); + const auto t284 = t41 * t9; + const auto t285 = t111 * t50; + const auto t286 = -t111 * t120 - t113 * t117 + t117 * t285 + t26 * t284; + const auto t287 = t111 * t126; + const auto t288 = t122 * t285; + const auto t289 = t112 + t285; + const auto t290 = t232 * t289; + const auto t291 = t290 * t58; + const auto t292 = t235 * t289; + const auto t293 = t290 * t64; + const auto t294 = t111 * t49; + const auto t295 = t67 * (t113 * t134 * t58 - t203 * t285 - t294 * t31); + const auto t296 = t1_z + t33; + const auto t297 = t67 * (t113 * t138 - t138 * t285 + t21 * t294 + t296); + const auto t298 = t1_y + t6; + const auto t299 = + t67 * (t113 * t134 * t64 - t207 * t285 - t294 * t38 - t298); + const auto t300 = t10 * t53 + t148 * t18; + const auto t301 = t124 * t300; + const auto t302 = t300 * t50; + const auto t303 = std::pow(t9, 2); + const auto t304 = t117 * t126; + const auto t305 = t117 * t50; + const auto t306 = t122 * t305; + const auto t307 = -t118 + t119 + t305; + const auto t308 = t232 * t307; + const auto t309 = t308 * t58; + const auto t310 = t235 * t307; + const auto t311 = t308 * t64; + const auto t312 = t117 * t49; + const auto t313 = + t67 * (t120 * t134 * t58 - t203 * t305 - t296 - t31 * t312); + const auto t314 = t67 * (t120 * t138 - t138 * t305 + t21 * t312); + const auto t315 = t67 * (t120 * t134 * t64 - t207 * t305 - t312 * t38 - t9); + const auto t316 = t148 * t52 + t54 * t9; + const auto t317 = t316 * t50; + const auto t318 = -t125 + t195 + t7 * t9; + const auto t319 = t232 * t318; + const auto t320 = -t319 * t58; + const auto t321 = -t235 * t318; + const auto t322 = -t319 * t64; + const auto t323 = t126 * t134; + const auto t324 = t122 * t49; + const auto t325 = t67 * (t195 * t203 + t298 + t31 * t324 + t323 * t58); + const auto t326 = t67 * (t126 * t138 + t138 * t195 - t21 * t324 + t9); + const auto t327 = t67 * (t195 * t207 + t323 * t64 + t324 * t38); + const auto t328 = 3 * t134; + const auto t329 = t328 * t59; + const auto t330 = t232 * t41; + const auto t331 = std::pow(t66, -2); + const auto t332 = 3 * t41; + const auto t333 = t331 * t332 * t58 * t67; + const auto t334 = t333 * t61; + const auto t335 = t333 * t64; + const auto t336 = t31 * t58; + const auto t337 = t22 - t32 - t39; + const auto t338 = t329 * t41 + t337; + const auto t339 = t232 * (-t336 - t338); + const auto t340 = t138 * t332; + const auto t341 = t13 - t20 - t340; + const auto t342 = t232 * t58; + const auto t343 = t341 * t342; + const auto t344 = t328 * t41; + const auto t345 = t344 * t64; + const auto t346 = t345 + t38; + const auto t347 = -t342 * t346; + const auto t348 = t328 * t62; + const auto t349 = t331 * t332 * t61 * t64 * t67; + const auto t350 = t31 + t344 * t58; + const auto t351 = -t235 * t350; + const auto t352 = t21 * t61; + const auto t353 = -t348 * t41 + t41; + const auto t354 = -t235 * t346; + const auto t355 = t328 * t65; + const auto t356 = t232 * t64; + const auto t357 = -t350 * t356; + const auto t358 = t341 * t356; + const auto t359 = t38 * t64; + const auto t360 = t337 + t355 * t41; + const auto t361 = t232 * (-t359 - t360); + const auto t362 = t232 * (-t21 * t58 + t31 * t61 + t340 * t58); + const auto t363 = t232 * (t31 * t64 + t345 * t58 + t38 * t58); + const auto t364 = t232 * (-t21 * t64 + t340 * t64 + t38 * t61); + hess[0] = t68 + * (-t41 * (t2 + t5) - t43 * t46 + std::pow(t45, 2) * t50 + t46 * t56); + hess[1] = t68 * (t56 * t72 + t80); + hess[2] = t68 * (t4 * t41 * t69 + t43 * t86 - t56 * t88 - t85 - t87); + hess[3] = t68 * (t41 * t94 + t43 * t91 - t56 * t97 - t90 - t96); + hess[4] = t67 * (-t104 + 2 * t41 * t55 * t98 * t99); + hess[5] = t67 * (2 * t105 * t41 * t55 * t99 - t110); + hess[6] = t68 * (2 * t111 * t41 * t49 * t55 - t116); + hess[7] = t67 * (2 * t117 * t41 * t55 * t99 - t121); + hess[8] = t67 * (-t123 * t124 * t55 - t127); + hess[9] = t137; + hess[10] = t140; + hess[11] = t141; + hess[12] = t143; + hess[13] = t145; + hess[14] = t147; + hess[15] = t68 * (t150 * t46 + t80); + hess[16] = t68 + * (t150 * t72 - t41 * (t151 + t5) + t50 * std::pow(t71, 2) - t72 * t77); + hess[17] = t68 * (t1 * t4 * t41 - t150 * t88 - t152 - t154 + t77 * t86); + hess[18] = t67 * (-t155 * t97 - t159); + hess[19] = t68 * (t150 * t160 + t163); + hess[20] = t67 * (2 * t105 * t149 * t41 * t99 - t165); + hess[21] = t67 * (2 * t111 * t149 * t41 * t99 - t166); + hess[22] = t68 * (2 * t117 * t149 * t41 * t49 - t168); + hess[23] = t67 * (-t123 * t155 - t169); + hess[24] = t172; + hess[25] = t173; + hess[26] = t174; + hess[27] = t176; + hess[28] = t177; + hess[29] = t178; + hess[30] = t68 * (t180 * t46 + t4 * t78 + t43 * t86 - t85 - t87); + hess[31] = t68 * (t1 * t4 * t41 - t152 - t154 + t180 * t72 + t77 * t86); + hess[32] = t68 + * (-t180 * t88 + t41 * t49 * std::pow(t86, 2) - t41 * (t151 + t2) + + 2 * t84 * t86); + hess[33] = t67 * (-t124 * t179 * t97 + t185); + hess[34] = t67 * (2 * t179 * t41 * t98 * t99 - t187); + hess[35] = t68 * (t108 * t86 + t180 * t190 - t188 + t189 * t41 - t192); + hess[36] = t67 * (2 * t111 * t179 * t41 * t99 - t193); + hess[37] = t67 * (2 * t117 * t179 * t41 * t99 - t194); + hess[38] = t68 * (-t123 * t180 + t196); + hess[39] = t199; + hess[40] = t200; + hess[41] = t201; + hess[42] = t205; + hess[43] = t206; + hess[44] = t208; + hess[45] = t68 * (t210 * t46 + t41 * t94 + t43 * t91 - t90 - t96); + hess[46] = t67 * (-t159 + 2 * t209 * t41 * t71 * t99); + hess[47] = t67 * (t185 - t211 * t88); + hess[48] = t68 + * (-t210 * t97 - t41 * (t212 + t213) + t50 * std::pow(t91, 2) + + t89 * t97); + hess[49] = t68 * (t101 * t91 + t160 * t210 - t214 + t215 * t28 - t216); + hess[50] = t68 * (t108 * t91 + t11 * t215 + t190 * t210 - t217 - t218); + hess[51] = t68 * (t113 * t91 + t210 * t223 - t219 + t222 * t41 - t224); + hess[52] = t67 * (2 * t117 * t209 * t41 * t99 - t227); + hess[53] = t67 * (-t123 * t211 + t230); + hess[54] = t234; + hess[55] = t236; + hess[56] = t237; + hess[57] = t239; + hess[58] = t240; + hess[59] = t242; + hess[60] = t67 * (-t104 + 2 * t243 * t41 * t45 * t99); + hess[61] = t68 * (t163 + t244 * t72); + hess[62] = t67 * (-t187 - t245 * t88); + hess[63] = t68 * (t101 * t91 + t15 * t28 * t41 - t214 - t216 - t244 * t97); + hess[64] = t68 + * (-t101 * t160 + 2 * t243 * t41 * t49 * t98 + + t41 * t49 * std::pow(t98, 2) - t41 * (t213 + t246)); + hess[65] = t68 * (t190 * t244 + t247); + hess[66] = t67 * (2 * t111 * t243 * t41 * t99 - t249); + hess[67] = t68 * (t244 * t250 + t252); + hess[68] = t67 * (-t123 * t245 - t253); + hess[69] = t256; + hess[70] = t257; + hess[71] = t258; + hess[72] = t259; + hess[73] = t260; + hess[74] = t262; + hess[75] = t67 * (-t110 + 2 * t263 * t41 * t45 * t99); + hess[76] = t67 * (-t165 + 2 * t263 * t41 * t71 * t99); + hess[77] = t68 * (t108 * t86 - t188 + t189 * t41 - t192 - t264 * t88); + hess[78] = t68 * (t108 * t91 + t11 * t15 * t41 - t217 - t218 - t264 * t97); + hess[79] = t68 * (t160 * t264 + t247); + hess[80] = t68 + * (std::pow(t105, 2) * t41 * t49 + 2 * t105 * t263 * t41 * t49 + - t108 * t190 - t41 * (t212 + t246)); + hess[81] = t67 * (2 * t111 * t263 * t41 * t99 - t267); + hess[82] = t67 * (2 * t117 * t263 * t41 * t99 - t268); + hess[83] = t68 * (t108 * t122 - t123 * t264 - t269 + t270 * t41 - t271); + hess[84] = t274; + hess[85] = t275; + hess[86] = t276; + hess[87] = t277; + hess[88] = t278; + hess[89] = t279; + hess[90] = t68 * (-t116 + 2 * t280 * t41 * t45 * t49); + hess[91] = t67 * (-t166 + 2 * t280 * t41 * t71 * t99); + hess[92] = t67 * (-t124 * t280 * t88 - t193); + hess[93] = t68 * (t113 * t91 - t219 + t222 * t41 - t224 - t281 * t97); + hess[94] = t67 * (-t249 + 2 * t280 * t41 * t98 * t99); + hess[95] = t67 * (2 * t105 * t280 * t41 * t99 - t267); + hess[96] = t68 + * (std::pow(t111, 2) * t41 * t49 + 2 * t111 * t280 * t41 * t49 + - t113 * t223 - t41 * (t282 + t283)); + hess[97] = t68 * (t250 * t281 + t286); + hess[98] = t68 * (t113 * t122 - t123 * t281 + t18 * t41 * t9 - t287 - t288); + hess[99] = t291; + hess[100] = t292; + hess[101] = t293; + hess[102] = t295; + hess[103] = t297; + hess[104] = t299; + hess[105] = t67 * (-t121 + 2 * t300 * t41 * t45 * t99); + hess[106] = t68 * (-t168 + 2 * t300 * t41 * t49 * t71); + hess[107] = t67 * (-t194 - t301 * t88); + hess[108] = t67 * (-t227 - t301 * t97); + hess[109] = t68 * (t160 * t302 + t252); + hess[110] = t67 * (2 * t105 * t300 * t41 * t99 - t268); + hess[111] = t68 * (t223 * t302 + t286); + hess[112] = t68 + * (std::pow(t117, 2) * t41 * t49 + 2 * t117 * t300 * t41 * t49 + - t120 * t250 - t41 * (t283 + t303)); + hess[113] = + t68 * (t120 * t122 - t123 * t302 + t18 * t26 * t41 - t304 - t306); + hess[114] = t309; + hess[115] = t310; + hess[116] = t311; + hess[117] = t313; + hess[118] = t314; + hess[119] = t315; + hess[120] = t67 * (-t127 + 2 * t316 * t41 * t45 * t99); + hess[121] = t67 * (-t169 + 2 * t316 * t41 * t71 * t99); + hess[122] = t68 * (t196 - t317 * t88); + hess[123] = t67 * (-t124 * t316 * t97 + t230); + hess[124] = t67 * (-t253 + 2 * t316 * t41 * t98 * t99); + hess[125] = t68 * (t108 * t122 + t190 * t317 - t269 + t270 * t41 - t271); + hess[126] = t68 * (t113 * t122 + t18 * t284 + t223 * t317 - t287 - t288); + hess[127] = + t68 * (t120 * t122 + t18 * t26 * t41 + t250 * t317 - t304 - t306); + hess[128] = t68 + * (std::pow(t122, 2) * t50 + t123 * t126 - t123 * t317 + - t41 * (t282 + t303)); + hess[129] = t320; + hess[130] = t321; + hess[131] = t322; + hess[132] = t325; + hess[133] = t326; + hess[134] = t327; + hess[135] = t137; + hess[136] = t172; + hess[137] = t199; + hess[138] = t234; + hess[139] = t256; + hess[140] = t274; + hess[141] = t291; + hess[142] = t309; + hess[143] = t320; + hess[144] = t330 * (t329 - 1); + hess[145] = t334; + hess[146] = t335; + hess[147] = t339; + hess[148] = t343; + hess[149] = t347; + hess[150] = t140; + hess[151] = t173; + hess[152] = t200; + hess[153] = t236; + hess[154] = t257; + hess[155] = t275; + hess[156] = t292; + hess[157] = t310; + hess[158] = t321; + hess[159] = t334; + hess[160] = t330 * (t348 - 1); + hess[161] = t349; + hess[162] = t351; + hess[163] = t232 * (t352 + t353); + hess[164] = t354; + hess[165] = t141; + hess[166] = t174; + hess[167] = t201; + hess[168] = t237; + hess[169] = t258; + hess[170] = t276; + hess[171] = t293; + hess[172] = t311; + hess[173] = t322; + hess[174] = t335; + hess[175] = t349; + hess[176] = t330 * (t355 - 1); + hess[177] = t357; + hess[178] = t358; + hess[179] = t361; + hess[180] = t143; + hess[181] = t176; + hess[182] = t205; + hess[183] = t239; + hess[184] = t259; + hess[185] = t277; + hess[186] = t295; + hess[187] = t313; + hess[188] = t325; + hess[189] = t339; + hess[190] = t351; + hess[191] = t357; + hess[192] = t232 * (2 * t336 + t338); + hess[193] = t362; + hess[194] = t363; + hess[195] = t145; + hess[196] = t177; + hess[197] = t206; + hess[198] = t240; + hess[199] = t260; + hess[200] = t278; + hess[201] = t297; + hess[202] = t314; + hess[203] = t326; + hess[204] = t343; + hess[205] = t135 * (-t130 * t348 + t130 + t61 * t76); + hess[206] = t358; + hess[207] = t362; + hess[208] = t232 * (-2 * t352 - t353); + hess[209] = t364; + hess[210] = t147; + hess[211] = t178; + hess[212] = t208; + hess[213] = t242; + hess[214] = t262; + hess[215] = t279; + hess[216] = t299; + hess[217] = t315; + hess[218] = t327; + hess[219] = t347; + hess[220] = t354; + hess[221] = t361; + hess[222] = t363; + hess[223] = t364; + hess[224] = t232 * (2 * t359 + t360); +} + +// hess is (144×1) flattened in column-major order +void triangle_closest_point_hessian_0( + double p_x, + double p_y, + double p_z, + double t0_x, + double t0_y, + double t0_z, + double t1_x, + double t1_y, + double t1_z, + double t2_x, + double t2_y, + double t2_z, + double hess[144]) +{ + const auto t0 = t0_x * t1_x; + const auto t1 = t0_y * t1_y; + const auto t2 = 2 * t1; + const auto t3 = t0_y * t2_y; + const auto t4 = 2 * t3; + const auto t5 = t0_x * t2_x; + const auto t6 = 2 * t5; + const auto t7 = t0_z * t1_z; + const auto t8 = 2 * t7; + const auto t9 = t0_z * t2_z; + const auto t10 = 2 * t9; + const auto t11 = t1_y * t2_y; + const auto t12 = 2 * t11; + const auto t13 = t1_z * t2_z; + const auto t14 = 2 * t13; + const auto t15 = t1_x * t2_x; + const auto t16 = 2 * t15; + const auto t17 = std::pow(t0_x, 2); + const auto t18 = std::pow(t1_y, 2); + const auto t19 = std::pow(t1_z, 2); + const auto t20 = std::pow(t2_y, 2); + const auto t21 = std::pow(t2_z, 2); + const auto t22 = std::pow(t0_y, 2); + const auto t23 = std::pow(t1_x, 2); + const auto t24 = std::pow(t2_x, 2); + const auto t25 = std::pow(t0_z, 2); + const auto t26 = 2 * t0; + const auto t27 = t0 * t10 + t0 * t12 + t0 * t14 - t0 * t2 + t0 * t4 + - t0 * t8 + t1 * t10 + t1 * t14 + t1 * t16 + t1 * t6 + t10 * t11 + + t10 * t15 - t10 * t18 - t10 * t23 + t11 * t6 - t12 * t13 - t12 * t15 + - t12 * t17 - t12 * t25 + t12 * t7 + t13 * t4 + t13 * t6 - t14 * t15 + - t14 * t17 - t14 * t22 + t15 * t4 - t16 * t22 - t16 * t25 + t16 * t7 + + t17 * t18 + t17 * t19 + t17 * t20 + t17 * t21 + t18 * t21 + t18 * t24 + + t18 * t25 - t18 * t6 + t19 * t20 + t19 * t22 + t19 * t24 - t19 * t4 + - t19 * t6 - t2 * t21 - t2 * t24 - t2 * t7 + t20 * t23 + t20 * t25 + - t20 * t26 - t20 * t8 + t21 * t22 + t21 * t23 - t21 * t26 + t22 * t23 + + t22 * t24 + t23 * t25 - t23 * t4 + t24 * t25 - t24 * t8 - t3 * t6 + + t4 * t7 - t4 * t9 + t6 * t7 - t6 * t9; + const auto t28 = 1.0 / t27; + const auto t29 = t0_x - t2_x; + const auto t30 = 2 * t0_x; + const auto t31 = -t30; + const auto t32 = t2_x + t31; + const auto t33 = t1_x + t32; + const auto t34 = t0_x - t1_x; + const auto t35 = t29 * t34; + const auto t36 = t20 + t21; + const auto t37 = -t10 + t25; + const auto t38 = t22 - t4; + const auto t39 = t36 + t37 + t38; + const auto t40 = t17 - t6; + const auto t41 = t24 + t39 + t40; + const auto t42 = t0_x * t18; + const auto t43 = t0_x * t19; + const auto t44 = t0_x * t20; + const auto t45 = t0_x * t21; + const auto t46 = t1_x * t20; + const auto t47 = t1_x * t21; + const auto t48 = t18 * t2_x; + const auto t49 = t19 * t2_x; + const auto t50 = t11 * t1_x; + const auto t51 = t13 * t1_x; + const auto t52 = t11 * t2_x; + const auto t53 = t13 * t2_x; + const auto t54 = t1 * t1_x; + const auto t55 = t2_x * t3; + const auto t56 = t1_x * t7; + const auto t57 = t2_x * t9; + const auto t58 = t1 * t2_x; + const auto t59 = t2_x * t7; + const auto t60 = t58 + t59; + const auto t61 = t1_x * t3; + const auto t62 = t1_x * t9; + const auto t63 = t61 + t62; + const auto t64 = -t11 * t30 - t13 * t30 + t42 + t43 + t44 + t45 - t46 - t47 + - t48 - t49 + t50 + t51 + t52 + t53 - t54 - t55 - t56 - t57 + t60 + t63; + const auto t65 = t11 + t13; + const auto t66 = -t9; + const auto t67 = t25 + t66 - t7; + const auto t68 = -t3; + const auto t69 = -t1 + t22 + t68; + const auto t70 = t65 + t67 + t69; + const auto t71 = -t5; + const auto t72 = -t0 + t17 + t71; + const auto t73 = t15 + t70 + t72; + const auto t74 = t64 * t73; + const auto t75 = 2 * t29; + const auto t76 = t28 * t75; + const auto t77 = -t13; + const auto t78 = t66 + t7; + const auto t79 = t77 + t78; + const auto t80 = -t11; + const auto t81 = t1 + t68; + const auto t82 = t80 + t81; + const auto t83 = t36 + t79 + t82; + const auto t84 = -t15; + const auto t85 = t0 + t71; + const auto t86 = t84 + t85; + const auto t87 = t24 + t83 + t86; + const auto t88 = t28 + * (2 * t28 * t34 * t41 * t64 - t29 * t33 - 2 * t35 - t74 * t76 - t87); + const auto t89 = t0_y - t2_y; + const auto t90 = t34 * t89; + const auto t91 = 2 * t90; + const auto t92 = 2 * t0_y; + const auto t93 = -t92; + const auto t94 = t2_y + t93; + const auto t95 = t1_y + t94; + const auto t96 = t0_y * t23; + const auto t97 = t0_y * t19; + const auto t98 = t0_y * t24; + const auto t99 = t0_y * t21; + const auto t100 = t1_y * t24; + const auto t101 = t1_y * t21; + const auto t102 = t23 * t2_y; + const auto t103 = t19 * t2_y; + const auto t104 = t15 * t1_y; + const auto t105 = t15 * t2_y; + const auto t106 = t13 * t1_y; + const auto t107 = t13 * t2_y; + const auto t108 = t0 * t1_y; + const auto t109 = t2_y * t5; + const auto t110 = t1_y * t7; + const auto t111 = t2_y * t9; + const auto t112 = t0 * t2_y; + const auto t113 = t2_y * t7; + const auto t114 = t112 + t113; + const auto t115 = t1_y * t5 + t1_y * t9; + const auto t116 = -t100 - t101 - t102 - t103 + t104 + t105 + t106 + t107 + - t108 - t109 - t110 - t111 + t114 + t115 - t13 * t92 - t15 * t92 + t96 + + t97 + t98 + t99; + const auto t117 = t73 * t76; + const auto t118 = + t28 * (-t116 * t117 + 2 * t116 * t28 * t34 * t41 - t29 * t95 - t91); + const auto t119 = t0_z - t2_z; + const auto t120 = t119 * t34; + const auto t121 = 2 * t120; + const auto t122 = 2 * t0_z; + const auto t123 = -t122; + const auto t124 = t123 + t2_z; + const auto t125 = t124 + t1_z; + const auto t126 = t0_z * t23; + const auto t127 = t0_z * t18; + const auto t128 = t0_z * t24; + const auto t129 = t0_z * t20; + const auto t130 = t1_z * t24; + const auto t131 = t1_z * t20; + const auto t132 = t23 * t2_z; + const auto t133 = t18 * t2_z; + const auto t134 = t15 * t1_z; + const auto t135 = t15 * t2_z; + const auto t136 = t11 * t1_z; + const auto t137 = t11 * t2_z; + const auto t138 = t0 * t1_z; + const auto t139 = t2_z * t5; + const auto t140 = t1 * t1_z; + const auto t141 = t2_z * t3; + const auto t142 = t0 * t2_z; + const auto t143 = t1 * t2_z; + const auto t144 = t142 + t143; + const auto t145 = t1_z * t3 + t1_z * t5; + const auto t146 = -t11 * t122 - t122 * t15 + t126 + t127 + t128 + t129 + - t130 - t131 - t132 - t133 + t134 + t135 + t136 + t137 - t138 - t139 + - t140 - t141 + t144 + t145; + const auto t147 = + t28 * (-t117 * t146 - t121 - t125 * t29 + 2 * t146 * t28 * t34 * t41); + const auto t148 = t1_x * t22; + const auto t149 = t1_x * t25; + const auto t150 = t22 * t2_x; + const auto t151 = t25 * t2_x; + const auto t152 = t0_x * t3; + const auto t153 = t0_x * t9; + const auto t154 = t0_x * t1; + const auto t155 = t0_x * t7; + const auto t156 = t0_x * t11 + t0_x * t13; + const auto t157 = t148 + t149 - t150 - t151 + t152 + t153 - t154 - t155 + + t156 - t44 - t45 + t46 + t47 - t52 - t53 + t55 + t57 + t60 - 2 * t61 + - 2 * t62; + const auto t158 = t157 * t41; + const auto t159 = 2 * t28; + const auto t160 = t159 * t34; + const auto t161 = + t28 * (-t117 * t157 + t158 * t160 - std::pow(t29, 2) + t41); + const auto t162 = t29 * t89; + const auto t163 = t17 * t1_y; + const auto t164 = t1_y * t25; + const auto t165 = t17 * t2_y; + const auto t166 = t25 * t2_y; + const auto t167 = t0_y * t5; + const auto t168 = t0_y * t9; + const auto t169 = t0 * t0_y; + const auto t170 = t0_y * t7; + const auto t171 = t0_y * t13 + t0_y * t15; + const auto t172 = -t10 * t1_y + t100 + t101 - t105 - t107 + t109 + t111 + + t114 + t163 + t164 - t165 - t166 + t167 + t168 - t169 - t170 + t171 + - t1_y * t6 - t98 - t99; + const auto t173 = t28 * (-t117 * t172 - t162 + 2 * t172 * t28 * t34 * t41); + const auto t174 = t119 * t29; + const auto t175 = t17 * t1_z; + const auto t176 = t1_z * t22; + const auto t177 = t17 * t2_z; + const auto t178 = t22 * t2_z; + const auto t179 = t0_z * t5; + const auto t180 = t0_z * t3; + const auto t181 = t0 * t0_z; + const auto t182 = t0_z * t1; + const auto t183 = t0_z * t11 + t0_z * t15; + const auto t184 = -t128 - t129 + t130 + t131 - t135 - t137 + t139 + t141 + + t144 + t175 + t176 - t177 - t178 + t179 + t180 - t181 - t182 + t183 + - t1_z * t4 - t1_z * t6; + const auto t185 = t28 * (-t117 * t184 - t174 + 2 * t184 * t28 * t34 * t41); + const auto t186 = -t148 - t149 + t150 + t151 - t152 - t153 + t154 + t155 + + t156 - t42 - t43 + t48 + t49 - t50 - t51 + t54 + t56 - 2 * t58 + - 2 * t59 + t63; + const auto t187 = t160 * t41; + const auto t188 = + t0 + t1 - t17 - t22 - t25 + t3 + t5 + t7 + t77 + t80 + t84 + t9; + const auto t189 = t28 * (-t117 * t186 + t186 * t187 + t188 + t35); + const auto t190 = t0_y - t1_y; + const auto t191 = t190 * t29; + const auto t192 = t102 + t103 - t104 - t106 + t108 + t110 - 2 * t112 + - 2 * t113 + t115 - t163 - t164 + t165 + t166 - t167 - t168 + t169 + + t170 + t171 - t96 - t97; + const auto t193 = t28 * (-t117 * t192 + t187 * t192 - t191 + t91); + const auto t194 = t0_z - t1_z; + const auto t195 = t194 * t29; + const auto t196 = -t126 - t127 + t132 + t133 - t134 - t136 + t138 + t140 + - 2 * t142 - 2 * t143 + t145 - t175 - t176 + t177 + t178 - t179 - t180 + + t181 + t182 + t183; + const auto t197 = t28 * (-t117 * t196 + t121 + t187 * t196 - t195); + const auto t198 = 2 * t191; + const auto t199 = t159 * t89; + const auto t200 = + t28 * (2 * t190 * t28 * t41 * t64 - t198 - t199 * t74 - t33 * t89); + const auto t201 = t190 * t89; + const auto t202 = t199 * t73; + const auto t203 = t28 + * (2 * t116 * t190 * t28 * t41 - t116 * t202 - 2 * t201 - t87 + - t89 * t95); + const auto t204 = t119 * t190; + const auto t205 = 2 * t204; + const auto t206 = + t28 * (-t125 * t89 + 2 * t146 * t190 * t28 * t41 - t146 * t202 - t205); + const auto t207 = t28 * (2 * t157 * t190 * t28 * t41 - t157 * t202 - t162); + const auto t208 = t159 * t190; + const auto t209 = t208 * t41; + const auto t210 = + t28 * (-t172 * t202 + t172 * t209 + t41 - std::pow(t89, 2)); + const auto t211 = t119 * t89; + const auto t212 = t28 * (2 * t184 * t190 * t28 * t41 - t184 * t202 - t211); + const auto t213 = + t28 * (2 * t186 * t190 * t28 * t41 - t186 * t202 + t198 - t90); + const auto t214 = t28 * (t188 - t192 * t202 + t192 * t209 + t201); + const auto t215 = t194 * t89; + const auto t216 = t28 * (-t196 * t202 + t196 * t209 + t205 - t215); + const auto t217 = 2 * t195; + const auto t218 = t119 * t159; + const auto t219 = + t28 * (-t119 * t33 + 2 * t194 * t28 * t41 * t64 - t217 - t218 * t74); + const auto t220 = 2 * t215; + const auto t221 = t218 * t73; + const auto t222 = + t28 * (2 * t116 * t194 * t28 * t41 - t116 * t221 - t119 * t95 - t220); + const auto t223 = t119 * t194; + const auto t224 = t28 + * (-t119 * t125 + 2 * t146 * t194 * t28 * t41 - t146 * t221 - 2 * t223 + - t87); + const auto t225 = t28 * (2 * t157 * t194 * t28 * t41 - t157 * t221 - t174); + const auto t226 = t28 * (2 * t172 * t194 * t28 * t41 - t172 * t221 - t211); + const auto t227 = t159 * t194; + const auto t228 = t227 * t41; + const auto t229 = + t28 * (-std::pow(t119, 2) - t184 * t221 + t184 * t228 + t41); + const auto t230 = + t28 * (-t120 + 2 * t186 * t194 * t28 * t41 - t186 * t221 + t217); + const auto t231 = + t28 * (2 * t192 * t194 * t28 * t41 - t192 * t221 - t204 + t220); + const auto t232 = t28 * (t188 - t196 * t221 + t196 * t228 + t223); + const auto t233 = p_x + t32; + const auto t234 = p_x + t1_x + t31; + const auto t235 = t234 * t75; + const auto t236 = p_x - t0_x; + const auto t237 = t236 * t34; + const auto t238 = p_y - t0_y; + const auto t239 = t190 * t238; + const auto t240 = p_z - t0_z; + const auto t241 = t194 * t240; + const auto t242 = t239 + t241; + const auto t243 = t237 + t242; + const auto t244 = t243 * t41; + const auto t245 = -t12; + const auto t246 = -t14; + const auto t247 = t18 + t19; + const auto t248 = t28 * (t245 + t246 + t247 + t36); + const auto t249 = t236 * t29; + const auto t250 = t238 * t89; + const auto t251 = t119 * t240; + const auto t252 = t250 + t251; + const auto t253 = t249 + t252; + const auto t254 = t253 * t73; + const auto t255 = std::pow(t27, -2); + const auto t256 = 4 * t255; + const auto t257 = t256 * std::pow(t64, 2); + const auto t258 = t159 * t233; + const auto t259 = t159 * t64; + const auto t260 = t234 * t41; + const auto t261 = t253 * t33; + const auto t262 = 4 * t28; + const auto t263 = t243 * t262; + const auto t264 = t263 * t29; + const auto t265 = t264 * t64; + const auto t266 = -t237 - t239 - t241 + t253 + t87; + const auto t267 = p_y + t94; + const auto t268 = p_y + t1_y + t93; + const auto t269 = t1_x * t1_y; + const auto t270 = t2_x * t2_y; + const auto t271 = t1_x * t2_y; + const auto t272 = -t271; + const auto t273 = t1_y * t2_x; + const auto t274 = -t273; + const auto t275 = t269 + t270 + t272 + t274; + const auto t276 = t258 * t73; + const auto t277 = t159 * t74; + const auto t278 = 8 * t255; + const auto t279 = t278 * t64; + const auto t280 = 2 * t234; + const auto t281 = t263 * t64; + const auto t282 = t280 * t89 - t281 * t89; + const auto t283 = -t116 * t264 + t268 * t75; + const auto t284 = t28 + * (2 * t116 * t234 * t28 * t41 - t116 * t244 * t279 + + 8 * t116 * t253 * t255 * t64 * t73 + 2 * t116 * t253 * t28 * t33 + - t116 * t276 - t159 * t244 * t275 - t233 * t95 + + 2 * t253 * t275 * t28 * t73 + 2 * t253 * t28 * t64 * t95 + - t267 * t277 - t267 * t33 + 2 * t268 * t28 * t41 * t64 - t282 + - t283); + const auto t285 = p_z + t124; + const auto t286 = p_z + t123 + t1_z; + const auto t287 = t1_x * t1_z; + const auto t288 = t2_x * t2_z; + const auto t289 = t1_x * t2_z; + const auto t290 = -t289; + const auto t291 = t1_z * t2_x; + const auto t292 = -t291; + const auto t293 = t287 + t288 + t290 + t292; + const auto t294 = t119 * t280 - t119 * t281; + const auto t295 = -t146 * t264 + t286 * t75; + const auto t296 = t28 + * (-t125 * t233 + 2 * t125 * t253 * t28 * t64 + + 2 * t146 * t234 * t28 * t41 - t146 * t244 * t279 + + 8 * t146 * t253 * t255 * t64 * t73 + 2 * t146 * t253 * t28 * t33 + - t146 * t276 - t159 * t244 * t293 + 2 * t253 * t28 * t293 * t73 + - t277 * t285 + 2 * t28 * t286 * t41 * t64 - t285 * t33 - t294 + - t295); + const auto t297 = -t249; + const auto t298 = t236 * t41; + const auto t299 = -t157 * t264 + t252; + const auto t300 = t28 + * (2 * t157 * t234 * t28 * t41 - t157 * t244 * t279 + + 8 * t157 * t253 * t255 * t64 * t73 + 2 * t157 * t253 * t28 * t33 + - t157 * t276 - t159 * t244 * t83 - t233 * t29 + + 2 * t253 * t28 * t29 * t64 + 2 * t253 * t28 * t73 * t83 + - t259 * t298 - t297 - t299 - t41); + const auto t301 = t238 * t29; + const auto t302 = 2 * t301; + const auto t303 = t0_y * t1_x; + const auto t304 = -t303; + const auto t305 = t1_y * t30; + const auto t306 = 2 * t273; + const auto t307 = t271 - t306; + const auto t308 = t0_y * t2_x; + const auto t309 = t2_y * t30; + const auto t310 = t308 - t309; + const auto t311 = t159 * (t270 + t304 + t305 + t307 + t310); + const auto t312 = t238 * t41; + const auto t313 = t159 * t260; + const auto t314 = t172 * t264; + const auto t315 = t253 * t64; + const auto t316 = t172 * t253; + const auto t317 = t159 * t33; + const auto t318 = t172 * t279; + const auto t319 = t28 + * (-t172 * t276 + t172 * t313 + t199 * t315 - t233 * t89 + t244 * t311 + - t244 * t318 - t254 * t311 + t254 * t318 - t259 * t312 + t302 + t314 + + t316 * t317); + const auto t320 = t240 * t29; + const auto t321 = 2 * t320; + const auto t322 = t0_z * t1_x; + const auto t323 = -t322; + const auto t324 = t1_z * t30; + const auto t325 = 2 * t291; + const auto t326 = t289 - t325; + const auto t327 = t0_z * t2_x; + const auto t328 = t2_z * t30; + const auto t329 = t327 - t328; + const auto t330 = t159 * (t288 + t323 + t324 + t326 + t329); + const auto t331 = t240 * t41; + const auto t332 = t184 * t264; + const auto t333 = t184 * t253; + const auto t334 = t184 * t279; + const auto t335 = t28 + * (-t119 * t233 - t184 * t276 + t184 * t313 + t218 * t315 + t244 * t330 + - t244 * t334 - t254 * t330 + t254 * t334 - t259 * t331 + t317 * t333 + + t321 + t332); + const auto t336 = t186 * t264; + const auto t337 = -t19 + t78; + const auto t338 = -t18 + t81; + const auto t339 = t159 * (t337 + t338 + t65); + const auto t340 = t159 * t261; + const auto t341 = t186 * t279; + const auto t342 = -t250; + const auto t343 = -t251; + const auto t344 = 2 * t237 + 2 * t239 + 2 * t241 + t297 + t342 + t343 + t73; + const auto t345 = t28 + * (t160 * t315 - t186 * t276 + t186 * t313 + t186 * t340 - t233 * t34 + + t235 + t236 * t277 + t236 * t33 + t244 * t339 - t244 * t341 + - t254 * t339 + t254 * t341 - t265 + t336 + t344); + const auto t346 = -t308; + const auto t347 = 2 * t271; + const auto t348 = t273 - t347; + const auto t349 = t303 - t305; + const auto t350 = t159 * (t269 + t309 + t346 + t348 + t349); + const auto t351 = t192 * t264; + const auto t352 = t192 * t279; + const auto t353 = t28 + * (-t190 * t233 - t192 * t276 + t192 * t313 + t192 * t340 + t208 * t315 + + t238 * t277 + t238 * t33 + t244 * t350 - t244 * t352 - t254 * t350 + + t254 * t352 + t282 + t351); + const auto t354 = -t327; + const auto t355 = 2 * t289; + const auto t356 = t291 - t355; + const auto t357 = t322 - t324; + const auto t358 = t159 * (t287 + t328 + t354 + t356 + t357); + const auto t359 = t196 * t264; + const auto t360 = t196 * t279; + const auto t361 = t28 + * (-t194 * t233 - t196 * t276 + t196 * t313 + t196 * t340 + t227 * t315 + + t240 * t277 + t240 * t33 + t244 * t358 - t244 * t360 - t254 * t358 + + t254 * t360 + t294 + t359); + const auto t362 = 2 * t89; + const auto t363 = t268 * t362; + const auto t364 = -t16; + const auto t365 = t21 + t24; + const auto t366 = t19 + t23; + const auto t367 = t28 * (t246 + t364 + t365 + t366); + const auto t368 = std::pow(t116, 2) * t256; + const auto t369 = t116 * t159; + const auto t370 = t267 * t73; + const auto t371 = t268 * t41; + const auto t372 = t253 * t95; + const auto t373 = t263 * t89; + const auto t374 = t116 * t373; + const auto t375 = t1_y * t1_z; + const auto t376 = t2_y * t2_z; + const auto t377 = t1_y * t2_z; + const auto t378 = -t377; + const auto t379 = t1_z * t2_y; + const auto t380 = -t379; + const auto t381 = t375 + t376 + t378 + t380; + const auto t382 = t146 * t159; + const auto t383 = t369 * t73; + const auto t384 = t116 * t278; + const auto t385 = 2 * t119; + const auto t386 = t119 * t263; + const auto t387 = -t116 * t386 + t268 * t385; + const auto t388 = -t146 * t373 + t286 * t362; + const auto t389 = t28 + * (2 * t116 * t125 * t253 * t28 + 8 * t116 * t146 * t253 * t255 * t73 + + 2 * t116 * t28 * t286 * t41 - t125 * t267 - t146 * t244 * t384 + + 2 * t146 * t253 * t28 * t95 + 2 * t146 * t268 * t28 * t41 + - t159 * t244 * t381 + 2 * t253 * t28 * t381 * t73 - t285 * t383 + - t285 * t95 - t370 * t382 - t387 - t388); + const auto t390 = t236 * t89; + const auto t391 = 2 * t390; + const auto t392 = t0_x * t1_y; + const auto t393 = -t392; + const auto t394 = t1_x * t92; + const auto t395 = t0_x * t2_y; + const auto t396 = t2_x * t92; + const auto t397 = t395 - t396; + const auto t398 = t159 * (t270 + t348 + t393 + t394 + t397); + const auto t399 = t158 * t159; + const auto t400 = t116 * t253; + const auto t401 = t157 * t373; + const auto t402 = t157 * t253; + const auto t403 = t159 * t95; + const auto t404 = t159 * t370; + const auto t405 = t157 * t384; + const auto t406 = t28 + * (-t157 * t404 + t244 * t398 - t244 * t405 - t254 * t398 + t254 * t405 + - t267 * t29 + t268 * t399 - t298 * t369 + t391 + t400 * t76 + t401 + + t402 * t403); + const auto t407 = t365 + t79 + t86; + const auto t408 = -t172 * t373 + t249 + t251; + const auto t409 = t28 + * (8 * t116 * t172 * t253 * t255 * t73 + 2 * t116 * t253 * t28 * t89 + - t159 * t244 * t407 - t172 * t244 * t384 + + 2 * t172 * t253 * t28 * t95 + 2 * t172 * t268 * t28 * t41 + - t172 * t404 + 2 * t253 * t28 * t407 * t73 - t267 * t89 + - t312 * t369 - t342 - t408 - t41); + const auto t410 = t240 * t89; + const auto t411 = 2 * t410; + const auto t412 = t0_z * t1_y; + const auto t413 = -t412; + const auto t414 = t1_z * t92; + const auto t415 = 2 * t379; + const auto t416 = t377 - t415; + const auto t417 = t0_z * t2_y; + const auto t418 = t2_z * t92; + const auto t419 = t417 - t418; + const auto t420 = t159 * (t376 + t413 + t414 + t416 + t419); + const auto t421 = t159 * t371; + const auto t422 = t184 * t373; + const auto t423 = t184 * t384; + const auto t424 = t28 + * (-t119 * t267 - t184 * t404 + t184 * t421 + t218 * t400 + t244 * t420 + - t244 * t423 - t254 * t420 + t254 * t423 - t331 * t369 + t333 * t403 + + t411 + t422); + const auto t425 = -t395; + const auto t426 = t392 - t394; + const auto t427 = t159 * (t269 + t307 + t396 + t425 + t426); + const auto t428 = t159 * t372; + const auto t429 = t186 * t373; + const auto t430 = t186 * t384; + const auto t431 = t28 + * (t160 * t400 - t186 * t404 + t186 * t421 + t186 * t428 + t236 * t383 + + t236 * t95 + t244 * t427 - t244 * t430 - t254 * t427 + t254 * t430 + - t267 * t34 + t283 + t429); + const auto t432 = t192 * t373; + const auto t433 = t13 + t15; + const auto t434 = -t23 + t85; + const auto t435 = t159 * (t337 + t433 + t434); + const auto t436 = t192 * t384; + const auto t437 = t28 + * (-t190 * t267 - t192 * t404 + t192 * t421 + t192 * t428 + t208 * t400 + + t238 * t383 + t238 * t95 + t244 * t435 - t244 * t436 - t254 * t435 + + t254 * t436 + t344 + t363 - t374 + t432); + const auto t438 = -t417; + const auto t439 = 2 * t377; + const auto t440 = t379 - t439; + const auto t441 = t412 - t414; + const auto t442 = t159 * (t375 + t418 + t438 + t440 + t441); + const auto t443 = t196 * t373; + const auto t444 = t196 * t384; + const auto t445 = t28 + * (-t194 * t267 - t196 * t404 + t196 * t421 + t196 * t428 + t227 * t400 + + t240 * t383 + t240 * t95 + t244 * t442 - t244 * t444 - t254 * t442 + + t254 * t444 + t387 + t443); + const auto t446 = t286 * t385; + const auto t447 = t20 + t24; + const auto t448 = t18 + t23; + const auto t449 = t28 * (t245 + t364 + t447 + t448); + const auto t450 = std::pow(t146, 2) * t256; + const auto t451 = t285 * t73; + const auto t452 = t286 * t41; + const auto t453 = t125 * t253; + const auto t454 = t146 * t386; + const auto t455 = t119 * t236; + const auto t456 = 2 * t455; + const auto t457 = t0_x * t1_z; + const auto t458 = -t457; + const auto t459 = t122 * t1_x; + const auto t460 = t0_x * t2_z; + const auto t461 = t122 * t2_x; + const auto t462 = t460 - t461; + const auto t463 = t159 * (t288 + t356 + t458 + t459 + t462); + const auto t464 = t146 * t253; + const auto t465 = t157 * t386; + const auto t466 = t125 * t159; + const auto t467 = t159 * t451; + const auto t468 = t146 * t278; + const auto t469 = t157 * t244; + const auto t470 = t254 * t468; + const auto t471 = t28 + * (-t157 * t467 + t157 * t470 + t244 * t463 - t254 * t463 - t285 * t29 + + t286 * t399 - t298 * t382 + t402 * t466 + t456 + t464 * t76 + t465 + - t468 * t469); + const auto t472 = t119 * t238; + const auto t473 = 2 * t472; + const auto t474 = t0_y * t1_z; + const auto t475 = -t474; + const auto t476 = t122 * t1_y; + const auto t477 = t0_y * t2_z; + const auto t478 = t122 * t2_y; + const auto t479 = t477 - t478; + const auto t480 = t159 * (t376 + t440 + t475 + t476 + t479); + const auto t481 = t159 * t452; + const auto t482 = t172 * t386; + const auto t483 = t244 * t468; + const auto t484 = t28 + * (-t172 * t467 + t172 * t470 + t172 * t481 - t172 * t483 + t199 * t464 + + t244 * t480 - t254 * t480 - t285 * t89 - t312 * t382 + t316 * t466 + + t473 + t482); + const auto t485 = t447 + t82 + t86; + const auto t486 = -t184 * t386 + t249 + t250; + const auto t487 = t28 + * (2 * t119 * t146 * t253 * t28 - t119 * t285 + + 2 * t125 * t184 * t253 * t28 + 8 * t146 * t184 * t253 * t255 * t73 + - t159 * t244 * t485 + 2 * t184 * t28 * t286 * t41 - t184 * t467 + - t184 * t483 + 2 * t253 * t28 * t485 * t73 - t331 * t382 - t343 + - t41 - t486); + const auto t488 = -t460; + const auto t489 = t457 - t459; + const auto t490 = t159 * (t287 + t326 + t461 + t488 + t489); + const auto t491 = t382 * t73; + const auto t492 = t159 * t453; + const auto t493 = t186 * t386; + const auto t494 = t28 + * (t125 * t236 + t160 * t464 - t186 * t467 + t186 * t470 + t186 * t481 + - t186 * t483 + t186 * t492 + t236 * t491 + t244 * t490 - t254 * t490 + - t285 * t34 + t295 + t493); + const auto t495 = -t477; + const auto t496 = t474 - t476; + const auto t497 = t159 * (t375 + t416 + t478 + t495 + t496); + const auto t498 = t192 * t386; + const auto t499 = t28 + * (t125 * t238 - t190 * t285 - t192 * t467 + t192 * t470 + t192 * t481 + - t192 * t483 + t192 * t492 + t208 * t464 + t238 * t491 + t244 * t497 + - t254 * t497 + t388 + t498); + const auto t500 = t196 * t386; + const auto t501 = t11 + t15; + const auto t502 = t159 * (t338 + t434 + t501); + const auto t503 = t28 + * (t125 * t240 - t194 * t285 - t196 * t467 + t196 * t470 + t196 * t481 + - t196 * t483 + t196 * t492 + t227 * t464 + t240 * t491 + t244 * t502 + - t254 * t502 + t344 + t446 - t454 + t500); + const auto t504 = std::pow(t157, 2); + const auto t505 = 2 * t255; + const auto t506 = t0_x * t0_y; + const auto t507 = t270 + t346 + t425 + t506; + const auto t508 = t172 * t262; + const auto t509 = t505 + * (4 * t157 * t172 * t253 * t28 * t73 + t157 * t253 * t89 - t158 * t238 + + t172 * t253 * t29 - t172 * t298 - t244 * t507 + t253 * t507 * t73 + - t469 * t508); + const auto t510 = t0_x * t0_z; + const auto t511 = t288 + t354 + t488 + t510; + const auto t512 = t505 + * (t119 * t157 * t253 + 4 * t157 * t184 * t253 * t28 * t73 - t158 * t240 + + t184 * t253 * t29 - t184 * t262 * t469 - t184 * t298 - t244 * t511 + + t253 * t511 * t73); + const auto t513 = t159 * t298; + const auto t514 = t159 * t70; + const auto t515 = t159 * t73; + const auto t516 = t157 * t515; + const auto t517 = t253 * t76; + const auto t518 = t186 * t278; + const auto t519 = t157 * t254; + const auto t520 = t28 + * (t160 * t402 - t186 * t513 + t186 * t517 + t236 * t516 - t244 * t514 + + t254 * t514 + t299 - t469 * t518 + t518 * t519); + const auto t521 = t159 * (t274 + t310 + t347 + t426 + t506); + const auto t522 = t192 * t278; + const auto t523 = t28 + * (-t192 * t513 + t192 * t517 + t208 * t402 + t238 * t516 + t244 * t521 + - t254 * t521 + t301 - t391 - t401 - t469 * t522 + t519 * t522); + const auto t524 = t159 * (t292 + t329 + t355 + t489 + t510); + const auto t525 = t196 * t278; + const auto t526 = t28 + * (-t196 * t513 + t196 * t517 + t227 * t402 + t240 * t516 + t244 * t524 + - t254 * t524 + t320 - t456 - t465 - t469 * t525 + t519 * t525); + const auto t527 = t365 + t37 + t40; + const auto t528 = std::pow(t172, 2); + const auto t529 = t0_y * t0_z; + const auto t530 = t376 + t438 + t495 + t529; + const auto t531 = t505 + * (t119 * t172 * t253 + 4 * t172 * t184 * t253 * t28 * t73 - t172 * t331 + - t184 * t244 * t508 + t184 * t253 * t89 - t184 * t312 - t244 * t530 + + t253 * t530 * t73); + const auto t532 = t159 * (t272 + t306 + t349 + t397 + t506); + const auto t533 = t159 * t312; + const auto t534 = t199 * t253; + const auto t535 = t172 * t515; + const auto t536 = t172 * t518; + const auto t537 = t28 + * (t160 * t316 - t186 * t533 + t186 * t534 + t236 * t535 + t244 * t532 + - t244 * t536 - t254 * t532 + t254 * t536 - t302 - t314 + t390); + const auto t538 = t159 * (t433 + t67 + t72); + const auto t539 = t172 * t522; + const auto t540 = t28 + * (-t192 * t533 + t192 * t534 + t208 * t316 + t238 * t535 - t244 * t538 + - t244 * t539 + t254 * t538 + t254 * t539 + t408); + const auto t541 = t159 * (t380 + t419 + t439 + t496 + t529); + const auto t542 = t172 * t525; + const auto t543 = t28 + * (-t196 * t533 + t196 * t534 + t227 * t316 + t240 * t535 + t244 * t541 + - t244 * t542 - t254 * t541 + t254 * t542 + t410 - t473 - t482); + const auto t544 = t38 + t40 + t447; + const auto t545 = std::pow(t184, 2); + const auto t546 = t159 * (t290 + t325 + t357 + t462 + t510); + const auto t547 = t159 * t331; + const auto t548 = t218 * t253; + const auto t549 = t184 * t515; + const auto t550 = t184 * t518; + const auto t551 = t28 + * (t160 * t333 - t186 * t547 + t186 * t548 + t236 * t549 + t244 * t546 + - t244 * t550 - t254 * t546 + t254 * t550 - t321 - t332 + t455); + const auto t552 = t159 * (t378 + t415 + t441 + t479 + t529); + const auto t553 = t184 * t522; + const auto t554 = t28 + * (-t192 * t547 + t192 * t548 + t208 * t333 + t238 * t549 + t244 * t552 + - t244 * t553 - t254 * t552 + t254 * t553 - t411 - t422 + t472); + const auto t555 = t159 * (t501 + t69 + t72); + const auto t556 = t184 * t525; + const auto t557 = t28 + * (-t196 * t547 + t196 * t548 + t227 * t333 + t240 * t549 - t244 * t555 + - t244 * t556 + t254 * t555 + t254 * t556 + t486); + const auto t558 = t25 - t8; + const auto t559 = -t2 + t22; + const auto t560 = t247 + t558 + t559; + const auto t561 = std::pow(t186, 2); + const auto t562 = t159 * (t269 + t304 + t393 + t506); + const auto t563 = t160 * t253; + const auto t564 = t186 * t253; + const auto t565 = t236 * t515; + const auto t566 = t186 * t515; + const auto t567 = t192 * t518; + const auto t568 = t28 + * (t190 * t236 + t192 * t563 + t192 * t565 + t208 * t564 + t238 * t34 + + t238 * t566 - t244 * t562 - t244 * t567 + t254 * t562 + t254 * t567 + - t351 - t429); + const auto t569 = t159 * (t287 + t323 + t458 + t510); + const auto t570 = t196 * t518; + const auto t571 = t28 + * (t194 * t236 + t196 * t563 + t196 * t565 + t227 * t564 + t240 * t34 + + t240 * t566 - t244 * t569 - t244 * t570 + t254 * t569 + t254 * t570 + - t359 - t493); + const auto t572 = t17 - t26; + const auto t573 = t366 + t558 + t572; + const auto t574 = std::pow(t192, 2); + const auto t575 = t159 * (t375 + t413 + t475 + t529); + const auto t576 = t196 * t522; + const auto t577 = t28 + * (t190 * t240 + t192 * t227 * t253 + t192 * t240 * t515 + t194 * t238 + + t196 * t208 * t253 + t196 * t238 * t515 - t244 * t575 - t244 * t576 + + t254 * t575 + t254 * t576 - t443 - t498); + const auto t578 = t448 + t559 + t572; + const auto t579 = std::pow(t196, 2); + hess[0] = 0; + hess[1] = 0; + hess[2] = 0; + hess[3] = t88; + hess[4] = t118; + hess[5] = t147; + hess[6] = t161; + hess[7] = t173; + hess[8] = t185; + hess[9] = t189; + hess[10] = t193; + hess[11] = t197; + hess[12] = 0; + hess[13] = 0; + hess[14] = 0; + hess[15] = t200; + hess[16] = t203; + hess[17] = t206; + hess[18] = t207; + hess[19] = t210; + hess[20] = t212; + hess[21] = t213; + hess[22] = t214; + hess[23] = t216; + hess[24] = 0; + hess[25] = 0; + hess[26] = 0; + hess[27] = t219; + hess[28] = t222; + hess[29] = t224; + hess[30] = t225; + hess[31] = t226; + hess[32] = t229; + hess[33] = t230; + hess[34] = t231; + hess[35] = t232; + hess[36] = t88; + hess[37] = t200; + hess[38] = t219; + hess[39] = t159 + * (-t233 * t33 - t235 + t244 * t248 - t244 * t257 - t248 * t254 + + t254 * t257 - t258 * t74 + t259 * t260 + t259 * t261 + t265 + + t266); + hess[40] = t284; + hess[41] = t296; + hess[42] = t300; + hess[43] = t319; + hess[44] = t335; + hess[45] = t345; + hess[46] = t353; + hess[47] = t361; + hess[48] = t118; + hess[49] = t203; + hess[50] = t222; + hess[51] = t284; + hess[52] = t159 + * (t244 * t367 - t244 * t368 - t254 * t367 + t254 * t368 + t266 + - t267 * t95 - t363 - t369 * t370 + t369 * t371 + t369 * t372 + + t374); + hess[53] = t389; + hess[54] = t406; + hess[55] = t409; + hess[56] = t424; + hess[57] = t431; + hess[58] = t437; + hess[59] = t445; + hess[60] = t147; + hess[61] = t206; + hess[62] = t224; + hess[63] = t296; + hess[64] = t389; + hess[65] = t159 + * (-t125 * t285 + t244 * t449 - t244 * t450 - t254 * t449 + t254 * t450 + + t266 - t382 * t451 + t382 * t452 + t382 * t453 - t446 + t454); + hess[66] = t471; + hess[67] = t484; + hess[68] = t487; + hess[69] = t494; + hess[70] = t499; + hess[71] = t503; + hess[72] = t161; + hess[73] = t207; + hess[74] = t225; + hess[75] = t300; + hess[76] = t406; + hess[77] = t471; + hess[78] = t505 + * (2 * t157 * t253 * t29 - 2 * t157 * t298 + t243 * t39 * t41 + - t244 * t262 * t504 + 4 * t253 * t28 * t504 * t73 - t254 * t39); + hess[79] = t509; + hess[80] = t512; + hess[81] = t520; + hess[82] = t523; + hess[83] = t526; + hess[84] = t173; + hess[85] = t210; + hess[86] = t226; + hess[87] = t319; + hess[88] = t409; + hess[89] = t484; + hess[90] = t509; + hess[91] = t505 + * (2 * t172 * t253 * t89 - 2 * t172 * t312 + t243 * t41 * t527 + - t244 * t262 * t528 + 4 * t253 * t28 * t528 * t73 - t254 * t527); + hess[92] = t531; + hess[93] = t537; + hess[94] = t540; + hess[95] = t543; + hess[96] = t185; + hess[97] = t212; + hess[98] = t229; + hess[99] = t335; + hess[100] = t424; + hess[101] = t487; + hess[102] = t512; + hess[103] = t531; + hess[104] = t505 + * (2 * t119 * t184 * t253 - 2 * t184 * t331 + t243 * t41 * t544 + - t244 * t262 * t545 + 4 * t253 * t28 * t545 * t73 - t254 * t544); + hess[105] = t551; + hess[106] = t554; + hess[107] = t557; + hess[108] = t189; + hess[109] = t213; + hess[110] = t230; + hess[111] = t345; + hess[112] = t431; + hess[113] = t494; + hess[114] = t520; + hess[115] = t537; + hess[116] = t551; + hess[117] = t159 + * (2 * t186 * t236 * t28 * t73 + 2 * t186 * t253 * t28 * t34 - t242 + + t243 * t28 * t41 * t560 - t244 * t256 * t561 + + 4 * t253 * t255 * t561 * t73 - t254 * t28 * t560 - t336); + hess[118] = t568; + hess[119] = t571; + hess[120] = t193; + hess[121] = t214; + hess[122] = t231; + hess[123] = t353; + hess[124] = t437; + hess[125] = t499; + hess[126] = t523; + hess[127] = t540; + hess[128] = t554; + hess[129] = t568; + hess[130] = t159 + * (2 * t190 * t192 * t253 * t28 + 2 * t192 * t238 * t28 * t73 - t237 + - t241 + t243 * t28 * t41 * t573 - t244 * t256 * t574 + + 4 * t253 * t255 * t574 * t73 - t254 * t28 * t573 - t432); + hess[131] = t577; + hess[132] = t197; + hess[133] = t216; + hess[134] = t232; + hess[135] = t361; + hess[136] = t445; + hess[137] = t503; + hess[138] = t526; + hess[139] = t543; + hess[140] = t557; + hess[141] = t571; + hess[142] = t577; + hess[143] = t159 + * (2 * t194 * t196 * t253 * t28 + 2 * t196 * t240 * t28 * t73 - t237 + - t239 + t243 * t28 * t41 * t578 - t244 * t256 * t579 + + 4 * t253 * t255 * t579 * t73 - t254 * t28 * t578 - t500); +} + +// hess is (144×1) flattened in column-major order +void triangle_closest_point_hessian_1( + double p_x, + double p_y, + double p_z, + double t0_x, + double t0_y, + double t0_z, + double t1_x, + double t1_y, + double t1_z, + double t2_x, + double t2_y, + double t2_z, + double hess[144]) +{ + const auto t0 = t0_y * t1_y; + const auto t1 = t0_x * t1_x; + const auto t2 = 2 * t1; + const auto t3 = t0_y * t2_y; + const auto t4 = t0_x * t2_x; + const auto t5 = 2 * t0; + const auto t6 = 2 * t3; + const auto t7 = t0_z * t1_z; + const auto t8 = t0_z * t2_z; + const auto t9 = 2 * t7; + const auto t10 = 2 * t8; + const auto t11 = t1_y * t2_y; + const auto t12 = t1_z * t2_z; + const auto t13 = 2 * t11; + const auto t14 = 2 * t12; + const auto t15 = t1_x * t2_x; + const auto t16 = 2 * t15; + const auto t17 = std::pow(t0_x, 2); + const auto t18 = std::pow(t1_y, 2); + const auto t19 = std::pow(t1_z, 2); + const auto t20 = std::pow(t2_y, 2); + const auto t21 = std::pow(t2_z, 2); + const auto t22 = std::pow(t0_y, 2); + const auto t23 = std::pow(t1_x, 2); + const auto t24 = std::pow(t2_x, 2); + const auto t25 = std::pow(t0_z, 2); + const auto t26 = 2 * t4; + const auto t27 = -t0 * t2 - t10 * t18 - t10 * t23 - t10 * t4 + t11 * t2 + + t11 * t9 - t12 * t13 + t12 * t2 + t12 * t5 - t13 * t15 - t13 * t17 + - t13 * t25 + t13 * t4 + t13 * t8 - t14 * t15 - t14 * t17 - t14 * t22 + + t14 * t3 + t14 * t4 + t15 * t5 + t15 * t9 - t16 * t22 - t16 * t25 + + t16 * t3 + t16 * t8 + t17 * t18 + t17 * t19 + t17 * t20 + t17 * t21 + + t18 * t21 + t18 * t24 + t18 * t25 - t18 * t26 + t19 * t20 + t19 * t22 + + t19 * t24 - t19 * t26 - t19 * t6 - t2 * t20 - t2 * t21 + t2 * t3 + - t2 * t7 + t2 * t8 + t20 * t23 + t20 * t25 - t20 * t9 + t21 * t22 + + t21 * t23 - t21 * t5 + t22 * t23 + t22 * t24 + t23 * t25 - t23 * t6 + + t24 * t25 - t24 * t5 - t24 * t9 + t3 * t9 + t4 * t5 - t4 * t6 + + t4 * t9 - t5 * t7 + t5 * t8 - t6 * t8; + const auto t28 = 1.0 / t27; + const auto t29 = t0_x - t1_x; + const auto t30 = 2 * t0_x; + const auto t31 = -t30; + const auto t32 = t1_x + t31; + const auto t33 = t2_x + t32; + const auto t34 = t0_x - t2_x; + const auto t35 = t29 * t34; + const auto t36 = t18 + t19; + const auto t37 = t25 - t9; + const auto t38 = t22 - t5; + const auto t39 = t36 + t37 + t38; + const auto t40 = t17 - t2; + const auto t41 = t23 + t39 + t40; + const auto t42 = t0_x * t18; + const auto t43 = t0_x * t19; + const auto t44 = t0_x * t20; + const auto t45 = t0_x * t21; + const auto t46 = t1_x * t20; + const auto t47 = t1_x * t21; + const auto t48 = t18 * t2_x; + const auto t49 = t19 * t2_x; + const auto t50 = t11 * t1_x; + const auto t51 = t12 * t1_x; + const auto t52 = t11 * t2_x; + const auto t53 = t12 * t2_x; + const auto t54 = t0 * t1_x; + const auto t55 = t2_x * t3; + const auto t56 = t1_x * t7; + const auto t57 = t2_x * t8; + const auto t58 = t0 * t2_x; + const auto t59 = t2_x * t7; + const auto t60 = t58 + t59; + const auto t61 = t1_x * t3; + const auto t62 = t1_x * t8; + const auto t63 = t61 + t62; + const auto t64 = -t11 * t30 - t12 * t30 + t42 + t43 + t44 + t45 - t46 - t47 + - t48 - t49 + t50 + t51 + t52 + t53 - t54 - t55 - t56 - t57 + t60 + t63; + const auto t65 = -t0; + const auto t66 = -t7; + const auto t67 = t65 + t66; + const auto t68 = -t8; + const auto t69 = t12 + t25 + t68; + const auto t70 = -t3; + const auto t71 = t11 + t22 + t70; + const auto t72 = t67 + t69 + t71; + const auto t73 = -t4; + const auto t74 = -t1; + const auto t75 = t15 + t17 + t73 + t74; + const auto t76 = t72 + t75; + const auto t77 = t64 * t76; + const auto t78 = 2 * t29; + const auto t79 = t28 * t78; + const auto t80 = -t15; + const auto t81 = -t11; + const auto t82 = -t12; + const auto t83 = t81 + t82; + const auto t84 = t3 + t4 + t8 + t80 + t83; + const auto t85 = t23 + t36 + t67 + t74 + t84; + const auto t86 = t28 + * (2 * t28 * t34 * t41 * t64 - t29 * t33 - 2 * t35 - t77 * t79 - t85); + const auto t87 = 2 * t0_y; + const auto t88 = -t87; + const auto t89 = t1_y + t88; + const auto t90 = t2_y + t89; + const auto t91 = t0_y - t1_y; + const auto t92 = t34 * t91; + const auto t93 = 2 * t92; + const auto t94 = t0_y * t23; + const auto t95 = t0_y * t19; + const auto t96 = t0_y * t24; + const auto t97 = t0_y * t21; + const auto t98 = t1_y * t24; + const auto t99 = t1_y * t21; + const auto t100 = t23 * t2_y; + const auto t101 = t19 * t2_y; + const auto t102 = t15 * t1_y; + const auto t103 = t15 * t2_y; + const auto t104 = t12 * t1_y; + const auto t105 = t12 * t2_y; + const auto t106 = t1 * t1_y; + const auto t107 = t2_y * t4; + const auto t108 = t1_y * t7; + const auto t109 = t2_y * t8; + const auto t110 = t1 * t2_y + t2_y * t7; + const auto t111 = t1_y * t4; + const auto t112 = t1_y * t8; + const auto t113 = t111 + t112; + const auto t114 = -t100 - t101 + t102 + t103 + t104 + t105 - t106 - t107 + - t108 - t109 + t110 + t113 - t12 * t87 - t15 * t87 + t94 + t95 + t96 + + t97 - t98 - t99; + const auto t115 = t76 * t79; + const auto t116 = + t28 * (-t114 * t115 + 2 * t114 * t28 * t34 * t41 - t29 * t90 - t93); + const auto t117 = 2 * t0_z; + const auto t118 = -t117; + const auto t119 = t118 + t1_z; + const auto t120 = t119 + t2_z; + const auto t121 = t0_z - t1_z; + const auto t122 = t121 * t34; + const auto t123 = 2 * t122; + const auto t124 = t0_z * t23; + const auto t125 = t0_z * t18; + const auto t126 = t0_z * t24; + const auto t127 = t0_z * t20; + const auto t128 = t1_z * t24; + const auto t129 = t1_z * t20; + const auto t130 = t23 * t2_z; + const auto t131 = t18 * t2_z; + const auto t132 = t15 * t1_z; + const auto t133 = t15 * t2_z; + const auto t134 = t11 * t1_z; + const auto t135 = t11 * t2_z; + const auto t136 = t1 * t1_z; + const auto t137 = t2_z * t4; + const auto t138 = t0 * t1_z; + const auto t139 = t2_z * t3; + const auto t140 = t0 * t2_z + t1 * t2_z; + const auto t141 = t1_z * t4; + const auto t142 = t1_z * t3; + const auto t143 = t141 + t142; + const auto t144 = -t11 * t117 - t117 * t15 + t124 + t125 + t126 + t127 + - t128 - t129 - t130 - t131 + t132 + t133 + t134 + t135 - t136 - t137 + - t138 - t139 + t140 + t143; + const auto t145 = + t28 * (-t115 * t144 - t120 * t29 - t123 + 2 * t144 * t28 * t34 * t41); + const auto t146 = t1_x * t22; + const auto t147 = t1_x * t25; + const auto t148 = t22 * t2_x; + const auto t149 = t25 * t2_x; + const auto t150 = t0_x * t3; + const auto t151 = t0_x * t8; + const auto t152 = t0 * t0_x; + const auto t153 = t0_x * t7; + const auto t154 = t0_x * t11 + t0_x * t12; + const auto t155 = t146 + t147 - t148 - t149 + t150 + t151 - t152 - t153 + + t154 - t44 - t45 + t46 + t47 - t52 - t53 + t55 + t57 + t60 - 2 * t61 + - 2 * t62; + const auto t156 = 2 * t28; + const auto t157 = t156 * t34; + const auto t158 = t0 + t1 - t17 - t22 - t25 + t7 + t84; + const auto t159 = t28 * (-t115 * t155 + t155 * t157 * t41 + t158 + t35); + const auto t160 = t0_y - t2_y; + const auto t161 = t160 * t29; + const auto t162 = t17 * t1_y; + const auto t163 = t1_y * t25; + const auto t164 = t17 * t2_y; + const auto t165 = t25 * t2_y; + const auto t166 = t0_y * t4; + const auto t167 = t0_y * t8; + const auto t168 = t0_y * t1; + const auto t169 = t0_y * t7; + const auto t170 = t0_y * t12 + t0_y * t15; + const auto t171 = -t103 - t105 + t107 + t109 + t110 - 2 * t111 - 2 * t112 + + t162 + t163 - t164 - t165 + t166 + t167 - t168 - t169 + t170 - t96 + - t97 + t98 + t99; + const auto t172 = + t28 * (-t115 * t171 - t161 + 2 * t171 * t28 * t34 * t41 + t93); + const auto t173 = t0_z - t2_z; + const auto t174 = t173 * t29; + const auto t175 = t17 * t1_z; + const auto t176 = t1_z * t22; + const auto t177 = t17 * t2_z; + const auto t178 = t22 * t2_z; + const auto t179 = t0_z * t4; + const auto t180 = t0_z * t3; + const auto t181 = t0_z * t1; + const auto t182 = t0 * t0_z; + const auto t183 = t0_z * t11 + t0_z * t15; + const auto t184 = -t126 - t127 + t128 + t129 - t133 - t135 + t137 + t139 + + t140 - 2 * t141 - 2 * t142 + t175 + t176 - t177 - t178 + t179 + t180 + - t181 - t182 + t183; + const auto t185 = + t28 * (-t115 * t184 + t123 - t174 + 2 * t184 * t28 * t34 * t41); + const auto t186 = -t146 - t147 + t148 + t149 - t150 - t151 + t152 + t153 + + t154 - t42 - t43 + t48 + t49 - t50 - t51 + t54 + t56 - 2 * t58 + - 2 * t59 + t63; + const auto t187 = t186 * t41; + const auto t188 = + t28 * (-t115 * t186 + t157 * t187 - std::pow(t29, 2) + t41); + const auto t189 = t29 * t91; + const auto t190 = t100 + t101 - t102 - t104 + t106 + t108 + t113 - t162 + - t163 + t164 + t165 - t166 - t167 + t168 + t169 + t170 - t2 * t2_y + - t2_y * t9 - t94 - t95; + const auto t191 = t28 * (-t115 * t190 - t189 + 2 * t190 * t28 * t34 * t41); + const auto t192 = t121 * t29; + const auto t193 = -t124 - t125 + t130 + t131 - t132 - t134 + t136 + t138 + + t143 - t175 - t176 + t177 + t178 - t179 - t180 + t181 + t182 + t183 + - t2 * t2_z - t2_z * t5; + const auto t194 = t28 * (-t115 * t193 - t192 + 2 * t193 * t28 * t34 * t41); + const auto t195 = 2 * t161; + const auto t196 = t156 * t91; + const auto t197 = + t28 * (2 * t160 * t28 * t41 * t64 - t195 - t196 * t77 - t33 * t91); + const auto t198 = t160 * t91; + const auto t199 = t196 * t76; + const auto t200 = t28 + * (2 * t114 * t160 * t28 * t41 - t114 * t199 - 2 * t198 - t85 + - t90 * t91); + const auto t201 = t121 * t160; + const auto t202 = 2 * t201; + const auto t203 = + t28 * (-t120 * t91 + 2 * t144 * t160 * t28 * t41 - t144 * t199 - t202); + const auto t204 = t156 * t160; + const auto t205 = t204 * t41; + const auto t206 = t28 * (-t155 * t199 + t155 * t205 + t195 - t92); + const auto t207 = t28 * (t158 - t171 * t199 + t171 * t205 + t198); + const auto t208 = t173 * t91; + const auto t209 = + t28 * (2 * t160 * t184 * t28 * t41 - t184 * t199 + t202 - t208); + const auto t210 = t28 * (2 * t160 * t186 * t28 * t41 - t186 * t199 - t189); + const auto t211 = + t28 * (-t190 * t199 + t190 * t205 + t41 - std::pow(t91, 2)); + const auto t212 = t121 * t91; + const auto t213 = t28 * (2 * t160 * t193 * t28 * t41 - t193 * t199 - t212); + const auto t214 = 2 * t174; + const auto t215 = t121 * t156; + const auto t216 = + t28 * (-t121 * t33 + 2 * t173 * t28 * t41 * t64 - t214 - t215 * t77); + const auto t217 = 2 * t208; + const auto t218 = t215 * t76; + const auto t219 = + t28 * (2 * t114 * t173 * t28 * t41 - t114 * t218 - t121 * t90 - t217); + const auto t220 = t121 * t173; + const auto t221 = t28 + * (-t120 * t121 + 2 * t144 * t173 * t28 * t41 - t144 * t218 - 2 * t220 + - t85); + const auto t222 = t156 * t173; + const auto t223 = t222 * t41; + const auto t224 = t28 * (-t122 - t155 * t218 + t155 * t223 + t214); + const auto t225 = t28 * (-t171 * t218 + t171 * t223 - t201 + t217); + const auto t226 = t28 * (t158 - t184 * t218 + t184 * t223 + t220); + const auto t227 = t28 * (2 * t173 * t186 * t28 * t41 - t186 * t218 - t192); + const auto t228 = t28 * (2 * t173 * t190 * t28 * t41 - t190 * t218 - t212); + const auto t229 = + t28 * (-std::pow(t121, 2) - t193 * t218 + t193 * t223 + t41); + const auto t230 = p_x + t32; + const auto t231 = p_x - t0_x; + const auto t232 = t231 * t29; + const auto t233 = p_y - t0_y; + const auto t234 = t233 * t91; + const auto t235 = p_z - t0_z; + const auto t236 = t121 * t235; + const auto t237 = t234 + t236; + const auto t238 = t232 + t237; + const auto t239 = t238 * t76; + const auto t240 = -t13; + const auto t241 = -t14; + const auto t242 = t20 + t21; + const auto t243 = t240 + t241 + t242 + t36; + const auto t244 = t231 * t34; + const auto t245 = t160 * t233; + const auto t246 = t173 * t235; + const auto t247 = t245 + t246; + const auto t248 = t244 + t247; + const auto t249 = std::pow(t64, 2); + const auto t250 = std::pow(t27, -2); + const auto t251 = p_x + t2_x + t31; + const auto t252 = t156 * t230; + const auto t253 = t248 * t41; + const auto t254 = 4 * t250; + const auto t255 = 4 * t28; + const auto t256 = t248 * t255; + const auto t257 = t256 * t29; + const auto t258 = -t232; + const auto t259 = -t234; + const auto t260 = -t236; + const auto t261 = t258 + t259 + t260; + const auto t262 = t251 * t78 - t257 * t64 + t261; + const auto t263 = t68 + t7; + const auto t264 = t0 + t70; + const auto t265 = t263 + t264; + const auto t266 = t12 - t19; + const auto t267 = t11 - t18; + const auto t268 = t265 + t266 + t267; + const auto t269 = t1 + t73; + const auto t270 = t15 - t23; + const auto t271 = t248 + t268 + t269 + t270; + const auto t272 = p_y + t89; + const auto t273 = p_y + t2_y + t88; + const auto t274 = t1_x * t1_y; + const auto t275 = t2_x * t2_y; + const auto t276 = t1_x * t2_y; + const auto t277 = -t276; + const auto t278 = t1_y * t2_x; + const auto t279 = -t278; + const auto t280 = t274 + t275 + t277 + t279; + const auto t281 = t252 * t76; + const auto t282 = t156 * t77; + const auto t283 = 8 * t250; + const auto t284 = t283 * t64; + const auto t285 = 2 * t251; + const auto t286 = t256 * t64; + const auto t287 = t285 * t91 - t286 * t91; + const auto t288 = -t114 * t257 + t273 * t78; + const auto t289 = t28 + * (8 * t114 * t238 * t250 * t64 * t76 + 2 * t114 * t238 * t28 * t33 + + 2 * t114 * t251 * t28 * t41 - t114 * t253 * t284 - t114 * t281 + - t156 * t253 * t280 - t230 * t90 + 2 * t238 * t28 * t280 * t76 + + 2 * t238 * t28 * t64 * t90 - t272 * t282 - t272 * t33 + + 2 * t273 * t28 * t41 * t64 - t287 - t288); + const auto t290 = p_z + t119; + const auto t291 = p_z + t118 + t2_z; + const auto t292 = t1_x * t1_z; + const auto t293 = t2_x * t2_z; + const auto t294 = t1_x * t2_z; + const auto t295 = -t294; + const auto t296 = t1_z * t2_x; + const auto t297 = -t296; + const auto t298 = t292 + t293 + t295 + t297; + const auto t299 = t121 * t285 - t121 * t286; + const auto t300 = -t144 * t257 + t291 * t78; + const auto t301 = t28 + * (-t120 * t230 + 2 * t120 * t238 * t28 * t64 + + 8 * t144 * t238 * t250 * t64 * t76 + 2 * t144 * t238 * t28 * t33 + + 2 * t144 * t251 * t28 * t41 - t144 * t253 * t284 - t144 * t281 + - t156 * t253 * t298 + 2 * t238 * t28 * t298 * t76 + + 2 * t28 * t291 * t41 * t64 - t282 * t290 - t290 * t33 - t299 + - t300); + const auto t302 = t155 * t257; + const auto t303 = t156 * (t242 + t265 + t83); + const auto t304 = t238 * t64; + const auto t305 = t251 * t41; + const auto t306 = t155 * t156; + const auto t307 = t238 * t33; + const auto t308 = t155 * t284; + const auto t309 = 2 * t244 + 2 * t245 + 2 * t246 + t76; + const auto t310 = t28 + * (-t155 * t281 + t157 * t304 - t230 * t34 + t231 * t282 + t231 * t33 + + t239 * t303 + t239 * t308 - t253 * t303 - t253 * t308 + t262 + t302 + + t305 * t306 + t306 * t307 + t309); + const auto t311 = t0_y * t1_x; + const auto t312 = -t311; + const auto t313 = t1_y * t30; + const auto t314 = 2 * t278; + const auto t315 = t276 - t314; + const auto t316 = t0_y * t2_x; + const auto t317 = t2_y * t30; + const auto t318 = t316 - t317; + const auto t319 = t156 * (t275 + t312 + t313 + t315 + t318); + const auto t320 = t156 * t171; + const auto t321 = t171 * t257; + const auto t322 = t171 * t284; + const auto t323 = t28 + * (-t160 * t230 - t171 * t281 + t204 * t304 + t233 * t282 + t233 * t33 + - t239 * t319 + t239 * t322 + t253 * t319 - t253 * t322 + t287 + + t305 * t320 + t307 * t320 + t321); + const auto t324 = t0_z * t1_x; + const auto t325 = -t324; + const auto t326 = t1_z * t30; + const auto t327 = 2 * t296; + const auto t328 = t294 - t327; + const auto t329 = t0_z * t2_x; + const auto t330 = t2_z * t30; + const auto t331 = t329 - t330; + const auto t332 = t156 * (t293 + t325 + t326 + t328 + t331); + const auto t333 = t156 * t184; + const auto t334 = t184 * t257; + const auto t335 = t184 * t284; + const auto t336 = t28 + * (-t173 * t230 - t184 * t281 + t222 * t304 + t235 * t282 + t235 * t33 + - t239 * t332 + t239 * t335 + t253 * t332 - t253 * t335 + t299 + + t305 * t333 + t307 * t333 + t334); + const auto t337 = t231 * t41; + const auto t338 = t156 * t64; + const auto t339 = -t186 * t257 + t237; + const auto t340 = t28 + * (-t156 * t239 * t268 + 8 * t186 * t238 * t250 * t64 * t76 + + 2 * t186 * t238 * t28 * t33 + 2 * t186 * t251 * t28 * t41 + - t186 * t253 * t284 - t186 * t281 - t230 * t29 + + 2 * t238 * t28 * t29 * t64 + 2 * t248 * t268 * t28 * t41 - t258 + - t337 * t338 - t339 - t41); + const auto t341 = t233 * t29; + const auto t342 = 2 * t341; + const auto t343 = -t316; + const auto t344 = 2 * t276; + const auto t345 = t278 - t344; + const auto t346 = t311 - t313; + const auto t347 = t156 * (t274 + t317 + t343 + t345 + t346); + const auto t348 = t233 * t41; + const auto t349 = t156 * t305; + const auto t350 = t190 * t257; + const auto t351 = t190 * t238; + const auto t352 = t156 * t33; + const auto t353 = t190 * t284; + const auto t354 = t28 + * (-t190 * t281 + t190 * t349 + t196 * t304 - t230 * t91 - t239 * t347 + + t239 * t353 + t253 * t347 - t253 * t353 - t338 * t348 + t342 + t350 + + t351 * t352); + const auto t355 = t235 * t29; + const auto t356 = 2 * t355; + const auto t357 = -t329; + const auto t358 = 2 * t294; + const auto t359 = t296 - t358; + const auto t360 = t324 - t326; + const auto t361 = t156 * (t292 + t330 + t357 + t359 + t360); + const auto t362 = t235 * t41; + const auto t363 = t193 * t257; + const auto t364 = t193 * t238; + const auto t365 = t193 * t284; + const auto t366 = t28 + * (-t121 * t230 - t193 * t281 + t193 * t349 + t215 * t304 - t239 * t361 + + t239 * t365 + t253 * t361 - t253 * t365 - t338 * t362 + t352 * t364 + + t356 + t363); + const auto t367 = -t16; + const auto t368 = t21 + t24; + const auto t369 = t19 + t23; + const auto t370 = t241 + t367 + t368 + t369; + const auto t371 = std::pow(t114, 2); + const auto t372 = t114 * t156; + const auto t373 = t272 * t76; + const auto t374 = t261 + t271; + const auto t375 = 2 * t91; + const auto t376 = t256 * t91; + const auto t377 = -t114 * t376 + t273 * t375; + const auto t378 = t1_y * t1_z; + const auto t379 = t2_y * t2_z; + const auto t380 = t1_y * t2_z; + const auto t381 = -t380; + const auto t382 = t1_z * t2_y; + const auto t383 = -t382; + const auto t384 = t378 + t379 + t381 + t383; + const auto t385 = t144 * t156; + const auto t386 = t372 * t76; + const auto t387 = t114 * t283; + const auto t388 = 2 * t121; + const auto t389 = t121 * t256; + const auto t390 = -t114 * t389 + t273 * t388; + const auto t391 = -t144 * t376 + t291 * t375; + const auto t392 = t28 + * (2 * t114 * t120 * t238 * t28 + 8 * t114 * t144 * t238 * t250 * t76 + + 2 * t114 * t28 * t291 * t41 - t120 * t272 + + 2 * t144 * t238 * t28 * t90 - t144 * t253 * t387 + + 2 * t144 * t273 * t28 * t41 - t156 * t253 * t384 + + 2 * t238 * t28 * t384 * t76 - t290 * t386 - t290 * t90 + - t373 * t385 - t390 - t391); + const auto t393 = t0_x * t1_y; + const auto t394 = -t393; + const auto t395 = t1_x * t87; + const auto t396 = t0_x * t2_y; + const auto t397 = t2_x * t87; + const auto t398 = t396 - t397; + const auto t399 = t156 * (t275 + t345 + t394 + t395 + t398); + const auto t400 = t114 * t238; + const auto t401 = t273 * t41; + const auto t402 = t238 * t90; + const auto t403 = t155 * t376; + const auto t404 = t155 * t387; + const auto t405 = t28 + * (t157 * t400 + t231 * t386 + t231 * t90 - t239 * t399 + t239 * t404 + + t253 * t399 - t253 * t404 - t272 * t34 + t288 - t306 * t373 + + t306 * t401 + t306 * t402 + t403); + const auto t406 = t171 * t376; + const auto t407 = t263 + t269; + const auto t408 = t156 * (t368 + t407 + t80 + t82); + const auto t409 = t171 * t387; + const auto t410 = t261 + t309; + const auto t411 = t28 + * (-t160 * t272 + t204 * t400 + t233 * t386 + t233 * t90 + t239 * t408 + + t239 * t409 - t253 * t408 - t253 * t409 - t320 * t373 + t320 * t401 + + t320 * t402 + t377 + t406 + t410); + const auto t412 = t0_z * t1_y; + const auto t413 = -t412; + const auto t414 = t1_z * t87; + const auto t415 = 2 * t382; + const auto t416 = t380 - t415; + const auto t417 = t0_z * t2_y; + const auto t418 = t2_z * t87; + const auto t419 = t417 - t418; + const auto t420 = t156 * (t379 + t413 + t414 + t416 + t419); + const auto t421 = t184 * t376; + const auto t422 = t184 * t387; + const auto t423 = t28 + * (-t173 * t272 + t222 * t400 + t235 * t386 + t235 * t90 - t239 * t420 + + t239 * t422 + t253 * t420 - t253 * t422 - t333 * t373 + t333 * t401 + + t333 * t402 + t390 + t421); + const auto t424 = t231 * t91; + const auto t425 = 2 * t424; + const auto t426 = -t396; + const auto t427 = t393 - t395; + const auto t428 = t156 * (t274 + t315 + t397 + t426 + t427); + const auto t429 = t156 * t187; + const auto t430 = t186 * t376; + const auto t431 = t186 * t238; + const auto t432 = t156 * t90; + const auto t433 = t156 * t373; + const auto t434 = t186 * t387; + const auto t435 = t28 + * (-t186 * t433 - t239 * t428 + t239 * t434 + t253 * t428 - t253 * t434 + - t272 * t29 + t273 * t429 - t337 * t372 + t400 * t79 + t425 + t430 + + t431 * t432); + const auto t436 = t266 + t270 + t407; + const auto t437 = -t190 * t376 + t232 + t236; + const auto t438 = t28 + * (8 * t114 * t190 * t238 * t250 * t76 + 2 * t114 * t238 * t28 * t91 + - t156 * t239 * t436 + 2 * t190 * t238 * t28 * t90 + - t190 * t253 * t387 + 2 * t190 * t273 * t28 * t41 - t190 * t433 + + 2 * t248 * t28 * t41 * t436 - t259 - t272 * t91 - t348 * t372 - t41 + - t437); + const auto t439 = t235 * t91; + const auto t440 = 2 * t439; + const auto t441 = -t417; + const auto t442 = 2 * t380; + const auto t443 = t382 - t442; + const auto t444 = t412 - t414; + const auto t445 = t156 * (t378 + t418 + t441 + t443 + t444); + const auto t446 = t193 * t376; + const auto t447 = t193 * t387; + const auto t448 = t28 + * (-t121 * t272 + t156 * t193 * t401 - t193 * t433 + t215 * t400 + - t239 * t445 + t239 * t447 + t253 * t445 - t253 * t447 - t362 * t372 + + t364 * t432 + t440 + t446); + const auto t449 = t20 + t24; + const auto t450 = t18 + t23; + const auto t451 = t240 + t367 + t449 + t450; + const auto t452 = std::pow(t144, 2); + const auto t453 = t290 * t76; + const auto t454 = -t144 * t389 + t291 * t388; + const auto t455 = t0_x * t1_z; + const auto t456 = -t455; + const auto t457 = t117 * t1_x; + const auto t458 = t0_x * t2_z; + const auto t459 = t117 * t2_x; + const auto t460 = t458 - t459; + const auto t461 = t156 * (t293 + t359 + t456 + t457 + t460); + const auto t462 = t385 * t76; + const auto t463 = t144 * t238; + const auto t464 = t291 * t41; + const auto t465 = t120 * t238; + const auto t466 = t155 * t389; + const auto t467 = t144 * t283; + const auto t468 = t155 * t467; + const auto t469 = t28 + * (t120 * t231 + t157 * t463 + t231 * t462 - t239 * t461 + t239 * t468 + + t253 * t461 - t253 * t468 - t290 * t34 + t300 - t306 * t453 + + t306 * t464 + t306 * t465 + t466); + const auto t470 = t0_y * t1_z; + const auto t471 = -t470; + const auto t472 = t117 * t1_y; + const auto t473 = t0_y * t2_z; + const auto t474 = t117 * t2_y; + const auto t475 = t473 - t474; + const auto t476 = t156 * (t379 + t443 + t471 + t472 + t475); + const auto t477 = t171 * t389; + const auto t478 = t171 * t467; + const auto t479 = t28 + * (t120 * t233 - t160 * t290 + t204 * t463 + t233 * t462 - t239 * t476 + + t239 * t478 + t253 * t476 - t253 * t478 - t320 * t453 + t320 * t464 + + t320 * t465 + t391 + t477); + const auto t480 = t184 * t389; + const auto t481 = t264 + t269; + const auto t482 = t156 * (t449 + t481 + t80 + t81); + const auto t483 = t184 * t467; + const auto t484 = t28 + * (t120 * t235 - t173 * t290 + t222 * t463 + t235 * t462 + t239 * t482 + + t239 * t483 - t253 * t482 - t253 * t483 - t333 * t453 + t333 * t464 + + t333 * t465 + t410 + t454 + t480); + const auto t485 = t121 * t231; + const auto t486 = 2 * t485; + const auto t487 = -t458; + const auto t488 = t455 - t457; + const auto t489 = t156 * (t292 + t328 + t459 + t487 + t488); + const auto t490 = t186 * t389; + const auto t491 = t120 * t156; + const auto t492 = t156 * t453; + const auto t493 = t186 * t467; + const auto t494 = t28 + * (-t186 * t492 - t239 * t489 + t239 * t493 + t253 * t489 - t253 * t493 + - t29 * t290 + t291 * t429 - t337 * t385 + t431 * t491 + t463 * t79 + + t486 + t490); + const auto t495 = t121 * t233; + const auto t496 = 2 * t495; + const auto t497 = -t473; + const auto t498 = t470 - t472; + const auto t499 = t156 * (t378 + t416 + t474 + t497 + t498); + const auto t500 = t190 * t389; + const auto t501 = t190 * t467; + const auto t502 = t28 + * (t156 * t190 * t464 - t190 * t492 + t196 * t463 - t239 * t499 + + t239 * t501 + t253 * t499 - t253 * t501 - t290 * t91 - t348 * t385 + + t351 * t491 + t496 + t500); + const auto t503 = t267 + t270 + t481; + const auto t504 = -t193 * t389 + t232 + t234; + const auto t505 = t28 + * (2 * t120 * t193 * t238 * t28 + 2 * t121 * t144 * t238 * t28 + - t121 * t290 + 8 * t144 * t193 * t238 * t250 * t76 + - t156 * t239 * t503 - t193 * t253 * t467 + + 2 * t193 * t28 * t291 * t41 - t193 * t492 + + 2 * t248 * t28 * t41 * t503 - t260 - t362 * t385 - t41 - t504); + const auto t506 = -t10 + t25; + const auto t507 = t22 - t6; + const auto t508 = t242 + t506 + t507; + const auto t509 = std::pow(t155, 2); + const auto t510 = t0_x * t0_y; + const auto t511 = t156 * (t275 + t343 + t426 + t510); + const auto t512 = t157 * t238; + const auto t513 = t155 * t238; + const auto t514 = t231 * t76; + const auto t515 = t306 * t76; + const auto t516 = t155 * t283; + const auto t517 = t171 * t516; + const auto t518 = t28 + * (t160 * t231 + t171 * t512 + t204 * t513 + t233 * t34 + t233 * t515 + + t239 * t511 + t239 * t517 - t253 * t511 - t253 * t517 + t320 * t514 + - t321 - t403); + const auto t519 = t0_x * t0_z; + const auto t520 = t156 * (t293 + t357 + t487 + t519); + const auto t521 = t184 * t516; + const auto t522 = t28 + * (t173 * t231 + t184 * t512 + t222 * t513 + t235 * t34 + t235 * t515 + + t239 * t520 + t239 * t521 - t253 * t520 - t253 * t521 + t333 * t514 + - t334 - t466); + const auto t523 = t156 * t72; + const auto t524 = t156 * t514; + const auto t525 = t186 * t516; + const auto t526 = t28 + * (t157 * t431 + t186 * t524 + t239 * t523 + t239 * t525 - t253 * t523 + - t253 * t525 - t306 * t337 + t339 + t513 * t79); + const auto t527 = t156 * (t279 + t318 + t344 + t427 + t510); + const auto t528 = t190 * t516; + const auto t529 = t28 + * (t157 * t351 + t190 * t524 + t196 * t513 - t239 * t527 + t239 * t528 + + t253 * t527 - t253 * t528 - t306 * t348 - t342 - t350 + t424); + const auto t530 = t156 * (t297 + t331 + t358 + t488 + t519); + const auto t531 = t193 * t516; + const auto t532 = t28 + * (t157 * t364 + t193 * t524 + t215 * t513 - t239 * t530 + t239 * t531 + + t253 * t530 - t253 * t531 - t306 * t362 - t356 - t363 + t485); + const auto t533 = t17 - t26; + const auto t534 = t368 + t506 + t533; + const auto t535 = std::pow(t171, 2); + const auto t536 = t0_y * t0_z; + const auto t537 = t156 * (t379 + t441 + t497 + t536); + const auto t538 = t171 * t238; + const auto t539 = t233 * t76; + const auto t540 = t235 * t76; + const auto t541 = t171 * t283; + const auto t542 = t184 * t541; + const auto t543 = t28 + * (t160 * t235 + t173 * t233 + t184 * t204 * t238 + t222 * t538 + + t239 * t537 + t239 * t542 - t253 * t537 - t253 * t542 + t320 * t540 + + t333 * t539 - t421 - t477); + const auto t544 = t156 * (t277 + t314 + t346 + t398 + t510); + const auto t545 = t156 * t539; + const auto t546 = t186 * t541; + const auto t547 = t28 + * (t186 * t545 + t204 * t431 - t239 * t544 + t239 * t546 + t253 * t544 + - t253 * t546 - t320 * t337 + t341 - t425 - t430 + t538 * t79); + const auto t548 = t156 * (t66 + t69 + t75); + const auto t549 = t190 * t541; + const auto t550 = t28 + * (t190 * t545 + t196 * t538 + t204 * t351 + t239 * t548 + t239 * t549 + - t253 * t548 - t253 * t549 - t320 * t348 + t437); + const auto t551 = t156 * (t383 + t419 + t442 + t498 + t536); + const auto t552 = t193 * t541; + const auto t553 = t28 + * (t193 * t545 + t204 * t364 + t215 * t538 - t239 * t551 + t239 * t552 + + t253 * t551 - t253 * t552 - t320 * t362 - t440 - t446 + t495); + const auto t554 = t449 + t507 + t533; + const auto t555 = std::pow(t184, 2); + const auto t556 = t156 * (t295 + t327 + t360 + t460 + t519); + const auto t557 = t184 * t238; + const auto t558 = t156 * t540; + const auto t559 = t184 * t283; + const auto t560 = t186 * t253; + const auto t561 = t239 * t559; + const auto t562 = t28 + * (t186 * t558 + t186 * t561 + t222 * t431 - t239 * t556 + t253 * t556 + - t333 * t337 + t355 - t486 - t490 + t557 * t79 - t559 * t560); + const auto t563 = t156 * (t381 + t415 + t444 + t475 + t536); + const auto t564 = t253 * t559; + const auto t565 = t28 + * (t190 * t558 + t190 * t561 - t190 * t564 + t196 * t557 + t222 * t351 + - t239 * t563 + t253 * t563 - t333 * t348 + t439 - t496 - t500); + const auto t566 = t156 * (t65 + t71 + t75); + const auto t567 = t28 + * (t193 * t558 + t193 * t561 - t193 * t564 + t215 * t557 + t222 * t364 + + t239 * t566 - t253 * t566 - t333 * t362 + t504); + const auto t568 = std::pow(t186, 2); + const auto t569 = 2 * t250; + const auto t570 = t274 + t312 + t394 + t510; + const auto t571 = t190 * t255; + const auto t572 = t569 + * (4 * t186 * t190 * t238 * t28 * t76 + t186 * t238 * t91 - t187 * t233 + + t190 * t238 * t29 - t190 * t337 + t238 * t570 * t76 - t253 * t570 + - t560 * t571); + const auto t573 = t292 + t325 + t456 + t519; + const auto t574 = t569 + * (t121 * t186 * t238 + 4 * t186 * t193 * t238 * t28 * t76 - t187 * t235 + + t193 * t238 * t29 - t193 * t255 * t560 - t193 * t337 + + t238 * t573 * t76 - t253 * t573); + const auto t575 = t369 + t37 + t40; + const auto t576 = std::pow(t190, 2); + const auto t577 = t378 + t413 + t471 + t536; + const auto t578 = t569 + * (t121 * t190 * t238 + 4 * t190 * t193 * t238 * t28 * t76 - t190 * t362 + + t193 * t238 * t91 - t193 * t253 * t571 - t193 * t348 + + t238 * t577 * t76 - t253 * t577); + const auto t579 = t38 + t40 + t450; + const auto t580 = std::pow(t193, 2); + hess[0] = 0; + hess[1] = 0; + hess[2] = 0; + hess[3] = t86; + hess[4] = t116; + hess[5] = t145; + hess[6] = t159; + hess[7] = t172; + hess[8] = t185; + hess[9] = t188; + hess[10] = t191; + hess[11] = t194; + hess[12] = 0; + hess[13] = 0; + hess[14] = 0; + hess[15] = t197; + hess[16] = t200; + hess[17] = t203; + hess[18] = t206; + hess[19] = t207; + hess[20] = t209; + hess[21] = t210; + hess[22] = t211; + hess[23] = t213; + hess[24] = 0; + hess[25] = 0; + hess[26] = 0; + hess[27] = t216; + hess[28] = t219; + hess[29] = t221; + hess[30] = t224; + hess[31] = t225; + hess[32] = t226; + hess[33] = t227; + hess[34] = t228; + hess[35] = t229; + hess[36] = t86; + hess[37] = t197; + hess[38] = t216; + hess[39] = t156 + * (-t230 * t33 + 4 * t238 * t249 * t250 * t76 + + 2 * t238 * t28 * t33 * t64 - t239 * t243 * t28 + + t243 * t248 * t28 * t41 - t249 * t253 * t254 + + 2 * t251 * t28 * t41 * t64 - t252 * t77 - t262 - t271); + hess[40] = t289; + hess[41] = t301; + hess[42] = t310; + hess[43] = t323; + hess[44] = t336; + hess[45] = t340; + hess[46] = t354; + hess[47] = t366; + hess[48] = t116; + hess[49] = t200; + hess[50] = t219; + hess[51] = t289; + hess[52] = t156 + * (2 * t114 * t238 * t28 * t90 + 2 * t114 * t273 * t28 * t41 + + 4 * t238 * t250 * t371 * t76 - t239 * t28 * t370 + + t248 * t28 * t370 * t41 - t253 * t254 * t371 - t272 * t90 + - t372 * t373 - t374 - t377); + hess[53] = t392; + hess[54] = t405; + hess[55] = t411; + hess[56] = t423; + hess[57] = t435; + hess[58] = t438; + hess[59] = t448; + hess[60] = t145; + hess[61] = t203; + hess[62] = t221; + hess[63] = t301; + hess[64] = t392; + hess[65] = t156 + * (2 * t120 * t144 * t238 * t28 - t120 * t290 + + 2 * t144 * t28 * t291 * t41 + 4 * t238 * t250 * t452 * t76 + - t239 * t28 * t451 + t248 * t28 * t41 * t451 - t253 * t254 * t452 + - t374 - t385 * t453 - t454); + hess[66] = t469; + hess[67] = t479; + hess[68] = t484; + hess[69] = t494; + hess[70] = t502; + hess[71] = t505; + hess[72] = t159; + hess[73] = t206; + hess[74] = t224; + hess[75] = t310; + hess[76] = t405; + hess[77] = t469; + hess[78] = t156 + * (2 * t155 * t231 * t28 * t76 + 2 * t155 * t238 * t28 * t34 + + 4 * t238 * t250 * t509 * t76 - t239 * t28 * t508 - t247 + + t248 * t28 * t41 * t508 - t253 * t254 * t509 - t302); + hess[79] = t518; + hess[80] = t522; + hess[81] = t526; + hess[82] = t529; + hess[83] = t532; + hess[84] = t172; + hess[85] = t207; + hess[86] = t225; + hess[87] = t323; + hess[88] = t411; + hess[89] = t479; + hess[90] = t518; + hess[91] = t156 + * (2 * t160 * t171 * t238 * t28 + 2 * t171 * t233 * t28 * t76 + + 4 * t238 * t250 * t535 * t76 - t239 * t28 * t534 - t244 - t246 + + t248 * t28 * t41 * t534 - t253 * t254 * t535 - t406); + hess[92] = t543; + hess[93] = t547; + hess[94] = t550; + hess[95] = t553; + hess[96] = t185; + hess[97] = t209; + hess[98] = t226; + hess[99] = t336; + hess[100] = t423; + hess[101] = t484; + hess[102] = t522; + hess[103] = t543; + hess[104] = t156 + * (2 * t173 * t184 * t238 * t28 + 2 * t184 * t235 * t28 * t76 + + 4 * t238 * t250 * t555 * t76 - t239 * t28 * t554 - t244 - t245 + + t248 * t28 * t41 * t554 - t253 * t254 * t555 - t480); + hess[105] = t562; + hess[106] = t565; + hess[107] = t567; + hess[108] = t188; + hess[109] = t210; + hess[110] = t227; + hess[111] = t340; + hess[112] = t435; + hess[113] = t494; + hess[114] = t526; + hess[115] = t547; + hess[116] = t562; + hess[117] = t569 + * (2 * t186 * t238 * t29 - 2 * t186 * t337 + 4 * t238 * t28 * t568 * t76 + - t239 * t39 + t248 * t39 * t41 - t253 * t255 * t568); + hess[118] = t572; + hess[119] = t574; + hess[120] = t191; + hess[121] = t211; + hess[122] = t228; + hess[123] = t354; + hess[124] = t438; + hess[125] = t502; + hess[126] = t529; + hess[127] = t550; + hess[128] = t565; + hess[129] = t572; + hess[130] = t569 + * (2 * t190 * t238 * t91 - 2 * t190 * t348 + 4 * t238 * t28 * t576 * t76 + - t239 * t575 + t248 * t41 * t575 - t253 * t255 * t576); + hess[131] = t578; + hess[132] = t194; + hess[133] = t213; + hess[134] = t229; + hess[135] = t366; + hess[136] = t448; + hess[137] = t505; + hess[138] = t532; + hess[139] = t553; + hess[140] = t567; + hess[141] = t574; + hess[142] = t578; + hess[143] = t569 + * (2 * t121 * t193 * t238 - 2 * t193 * t362 + + 4 * t238 * t28 * t580 * t76 - t239 * t579 + t248 * t41 * t579 + - t253 * t255 * t580); +} + +void face_term_aux_fast_gradient( + double t0_x, + double t0_y, + double t0_z, + double t1_x, + double t1_y, + double t1_z, + double t2_x, + double t2_y, + double t2_z, + double p_x, + double p_y, + double p_z, + double d, + double grad[13]) +{ + const auto t0 = t0_x - t1_x; + const auto t1 = -t2_z; + const auto t2 = t0_z + t1; + const auto t3 = -t2_x; + const auto t4 = t0_x + t3; + const auto t5 = t0_z - t1_z; + const auto t6 = t0 * t2 - t4 * t5; + const auto t7 = -t2_y; + const auto t8 = t0_y + t7; + const auto t9 = t0 * t8; + const auto t10 = t0_y - t1_y; + const auto t11 = t10 * t4; + const auto t12 = -t11 + t9; + const auto t13 = t10 * t2 - t5 * t8; + const auto t14 = std::pow(t12, 2) + std::pow(t13, 2); + const auto t15 = t14 + std::pow(t6, 2); + const auto t16 = std::pow(d * t15, -1.0 / 2.0); + const auto t17 = p_y - t0_y; + const auto t18 = t1 + t1_z; + const auto t19 = p_z - t0_z; + const auto t20 = t1_y + t7; + const auto t21 = p_x - t0_x; + const auto t22 = t12 * t19 + t13 * t21; + const auto t23 = (-t17 * t6 + t22) / t15; + const auto t24 = t1_x + t3; + const auto t25 = t4 * t5; + const auto t26 = t0 * t2 - t25; + const auto t27 = t14 + std::pow(t26, 2); + const auto t28 = std::pow(d * t27, -1.0 / 2.0); + const auto t29 = (-t17 * t26 + t22) / t27; + grad[0] = + t16 * (-t13 - t17 * t18 + t19 * t20 - t23 * (t12 * t20 + t18 * t6)); + grad[1] = t16 + * (t0 * t2 + t18 * t21 - t19 * t24 - t23 * (-t12 * t24 + t13 * t18) + - t25); + grad[2] = + t16 * (t11 + t17 * t24 - t20 * t21 + t23 * (t13 * t20 + t24 * t6) - t9); + grad[3] = t28 * (t17 * t2 - t19 * t8 + t29 * (t12 * t8 + t2 * t6)); + grad[4] = -t28 * (-t19 * t4 + t2 * t21 + t29 * (t12 * t4 - t13 * t2)); + grad[5] = t28 * (-t17 * t4 + t21 * t8 - t29 * (t13 * t8 + t26 * t4)); + grad[6] = -t28 * (-t10 * t19 + t17 * t5 + t29 * (t10 * t12 + t26 * t5)); + grad[7] = t28 * (-t0 * t19 + t21 * t5 + t29 * (t0 * t12 - t13 * t5)); + grad[8] = t28 * (t0 * t17 - t10 * t21 + t29 * (t0 * t6 + t10 * t13)); + grad[9] = t13 * t28; + grad[10] = -t26 * t28; + grad[11] = t12 * t28; + grad[12] = -1.0 / 2.0 * t15 * t28 * t29 / d; +} + +// hess is (169×1) flattened in column-major order +void face_term_aux_fast_hessian( + double t0_x, + double t0_y, + double t0_z, + double t1_x, + double t1_y, + double t1_z, + double t2_x, + double t2_y, + double t2_z, + double p_x, + double p_y, + double p_z, + double d, + double hess[169]) +{ + const auto t0 = -t2_y; + const auto t1 = t0 + t1_y; + const auto t2 = std::pow(t1, 2); + const auto t3 = -t2_z; + const auto t4 = t1_z + t3; + const auto t5 = std::pow(t4, 2); + const auto t6 = -t0_y; + const auto t7 = p_y + t6; + const auto t8 = -t1_x; + const auto t9 = t0_x + t8; + const auto t10 = -t9; + const auto t11 = t0_z + t3; + const auto t12 = -t11; + const auto t13 = -t2_x; + const auto t14 = t0_x + t13; + const auto t15 = -t14; + const auto t16 = -t1_z; + const auto t17 = t0_z + t16; + const auto t18 = -t17; + const auto t19 = t10 * t12 - t15 * t18; + const auto t20 = -t0_x; + const auto t21 = p_x + t20; + const auto t22 = -t1_y; + const auto t23 = t0_y + t22; + const auto t24 = t11 * t23; + const auto t25 = t0 + t0_y; + const auto t26 = t17 * t25; + const auto t27 = -t26; + const auto t28 = t24 + t27; + const auto t29 = -t0_z; + const auto t30 = p_z + t29; + const auto t31 = t25 * t9; + const auto t32 = t14 * t23; + const auto t33 = -t32; + const auto t34 = t31 + t33; + const auto t35 = t21 * t28 + t30 * t34; + const auto t36 = -t19 * t7 + t35; + const auto t37 = -t1 * t30 + t28 + t4 * t7; + const auto t38 = -t37; + const auto t39 = t1 * t34; + const auto t40 = t19 * t4 + t39; + const auto t41 = 2 * t40; + const auto t42 = std::pow(t28, 2) + std::pow(t34, 2); + const auto t43 = std::pow(t19, 2) + t42; + const auto t44 = 1.0 / t43; + const auto t45 = t36 * t44; + const auto t46 = -t25; + const auto t47 = -t23; + const auto t48 = t10 * t46 - t15 * t47; + const auto t49 = -t19; + const auto t50 = t1 * t48 - t4 * t49; + const auto t51 = t36 * t50; + const auto t52 = t44 * t51; + const auto t53 = std::pow(d * t43, -1.0 / 2.0); + const auto t54 = t44 * t53; + const auto t55 = t13 + t1_x; + const auto t56 = -t28 * t4 + t34 * t55; + const auto t57 = -t56; + const auto t58 = 2 * t57; + const auto t59 = t21 * t4; + const auto t60 = t30 * t55; + const auto t61 = t11 * t9; + const auto t62 = t14 * t17; + const auto t63 = -t62; + const auto t64 = t61 + t63; + const auto t65 = t59 - t60 + t64; + const auto t66 = t36 * t55; + const auto t67 = t40 * t45; + const auto t68 = t1 * t66 - t38 * t57 - t40 * t65 + t57 * t67; + const auto t69 = t1 * t21; + const auto t70 = -t34 + t55 * t7 - t69; + const auto t71 = t40 * t70; + const auto t72 = t1 * t28 + t19 * t55; + const auto t73 = t67 * t72; + const auto t74 = 2 * t72; + const auto t75 = t11 * t7 - t25 * t30; + const auto t76 = t40 * t75; + const auto t77 = t11 * t19 + t25 * t34; + const auto t78 = t1 * t25; + const auto t79 = t11 * t4; + const auto t80 = t78 + t79; + const auto t81 = t67 * t77; + const auto t82 = 2 * t77; + const auto t83 = -t11 * t28 + t14 * t34; + const auto t84 = std::pow(t43, -2); + const auto t85 = -p_z; + const auto t86 = t11 * t21 - t14 * t30; + const auto t87 = -t86; + const auto t88 = t40 * t44; + const auto t89 = t38 * t44; + const auto t90 = t2_z - t36 * t40 * t83 * t84 + t45 * (t1 * t14 + t34) + + t83 * t89 + t85 + t87 * t88; + const auto t91 = t25 * t28; + const auto t92 = t14 * t19 + t91; + const auto t93 = t21 * t25; + const auto t94 = t14 * t7; + const auto t95 = t93 - t94; + const auto t96 = p_y + t0; + const auto t97 = -t36 * t40 * t84 * t92 + t45 * (t14 * t4 + t64) + t88 * t95 + + t89 * t92 + t96; + const auto t98 = t23 * t34; + const auto t99 = t17 * t19 + t98; + const auto t100 = t17 * t7 - t23 * t30; + const auto t101 = -t100; + const auto t102 = t1 * t23; + const auto t103 = t17 * t4; + const auto t104 = + t101 * t40 - t36 * t40 * t44 * t99 + t36 * (t102 + t103) + t38 * t99; + const auto t105 = -t17 * t28 + t34 * t9; + const auto t106 = -t105; + const auto t107 = t17 * t21; + const auto t108 = t30 * t9; + const auto t109 = t107 - t108; + const auto t110 = p_z - t106 * t36 * t40 * t84 + t106 * t89 + t109 * t88 + + t16 + t45 * (-t1 * t9 - t34); + const auto t111 = t19 * t9 + t23 * t28; + const auto t112 = 2 * t111; + const auto t113 = -p_y; + const auto t114 = t21 * t23; + const auto t115 = -t114 + t7 * t9; + const auto t116 = t36 * t84; + const auto t117 = t111 * t116 * t40 - t111 * t38 * t44 + t113 + t115 * t88 + + t1_y + t45 * (-t4 * t9 - t64); + const auto t118 = t39 + t4 * t64; + const auto t119 = t42 + std::pow(t64, 2); + const auto t120 = std::pow(d * t119, -1.0 / 2.0); + const auto t121 = 1.0 / t119; + const auto t122 = t121 * t28; + const auto t123 = t120 * t122; + const auto t124 = -t118 * t123; + const auto t125 = t53 * (t16 + t19 * t88 + t2_z); + const auto t126 = t121 * t34; + const auto t127 = t22 + t2_y; + const auto t128 = t120 * (-t118 * t126 - t127); + const auto t129 = t12 * t47 - t18 * t46; + const auto t130 = std::pow(t129, 2) + std::pow(t48, 2) + std::pow(t49, 2); + const auto t131 = (1.0 / 2.0) * t130; + const auto t132 = t130 * t44; + const auto t133 = 1.0 / d; + const auto t134 = t133 * t54; + const auto t135 = t129 * t4 - t48 * t55; + const auto t136 = t135 * t36; + const auto t137 = t136 * t44; + const auto t138 = std::pow(t55, 2); + const auto t139 = t57 * t70; + const auto t140 = t45 * t57; + const auto t141 = t140 * t72; + const auto t142 = t136 * t84; + const auto t143 = t44 * t57; + const auto t144 = -t31 + t32; + const auto t145 = t116 * t57; + const auto t146 = p_z + t143 * t75 + t145 * t77 + t3 - t44 * t65 * t77 + + t45 * (t144 + t25 * t55); + const auto t147 = 2 * t83; + const auto t148 = t14 * t55; + const auto t149 = t140 * t83 + t36 * (t148 + t79) - t57 * t87 - t65 * t83; + const auto t150 = -p_x; + const auto t151 = t44 * t65; + const auto t152 = t143 * t95 + t150 + t151 * t92 + t2_x + - t36 * t57 * t84 * t92 + t45 * (t25 * t4 + t28); + const auto t153 = t101 * t143 + t151 * t99 + t1_z - t36 * t57 * t84 * t99 + + t45 * (-t144 - t23 * t55) + t85; + const auto t154 = t55 * t9; + const auto t155 = + -t106 * t36 * t44 * t57 + t106 * t65 + t109 * t57 + t36 * (t103 + t154); + const auto t156 = p_x + t111 * t145 - t111 * t44 * t65 + t115 * t143 + + t45 * (-t23 * t4 - t28) + t8; + const auto t157 = t120 * (t122 * t56 + t4); + const auto t158 = t121 * t64; + const auto t159 = t120 * t158; + const auto t160 = -t159 * t56; + const auto t161 = -t53 * (t143 * t34 + t55); + const auto t162 = -t1 * t129 + t49 * t55; + const auto t163 = t162 * t36; + const auto t164 = t163 * t44; + const auto t165 = t44 * t72; + const auto t166 = t44 * t70; + const auto t167 = -t61 + t62; + const auto t168 = t116 * t72; + const auto t169 = + t165 * t75 + t166 * t77 + t168 * t77 - t45 * (t11 * t55 + t167) + t96; + const auto t170 = -t24 + t26; + const auto t171 = p_x + t13 + t166 * t83 + t168 * t83 - t44 * t72 * t87 + + t45 * (t1 * t11 + t170); + const auto t172 = t70 * t92; + const auto t173 = t148 + t78; + const auto t174 = 2 * t92; + const auto t175 = t45 * t72; + const auto t176 = t175 * t92; + const auto t177 = p_y - t101 * t44 * t72 + t166 * t99 + t168 * t99 + t22 + + t45 * (-t167 - t17 * t55); + const auto t178 = t106 * t166 + t106 * t168 - t109 * t44 * t72 + t150 + t1_x + + t45 * (-t1 * t17 - t170); + const auto t179 = + t111 * t175 + t111 * t70 + t115 * t72 - t36 * (t102 + t154); + const auto t180 = t53 * (t127 + t165 * t28); + const auto t181 = t120 * (-t158 * t72 - t2_x - t8); + const auto t182 = t120 * t126; + const auto t183 = t182 * t72; + const auto t184 = t11 * t49 + t46 * t48; + const auto t185 = t184 * t36; + const auto t186 = t185 * t44; + const auto t187 = t185 * t84; + const auto t188 = std::pow(t25, 2); + const auto t189 = std::pow(t11, 2); + const auto t190 = t75 * t83; + const auto t191 = t14 * t36; + const auto t192 = t45 * t77; + const auto t193 = t192 * t83; + const auto t194 = t75 * t92; + const auto t195 = t192 * t92; + const auto t196 = t75 * t99; + const auto t197 = t23 * t25; + const auto t198 = t11 * t17; + const auto t199 = t197 + t198; + const auto t200 = 2 * t99; + const auto t201 = t192 * t99; + const auto t202 = t44 * t75; + const auto t203 = t116 * t77; + const auto t204 = t0_z + t106 * t202 + t106 * t203 - t109 * t44 * t77 + + t45 * (2 * t31 + t33) + t85; + const auto t205 = t44 * t77; + const auto t206 = t0_y + t113; + const auto t207 = + t111 * t202 + t111 * t203 + t115 * t205 + t206 - t45 * (2 * t61 + t63); + const auto t208 = t123 * t77; + const auto t209 = t120 * (-t158 * t77 - t29 - t2_z); + const auto t210 = t2_y + t6; + const auto t211 = t53 * (t205 * t34 + t210); + const auto t212 = t12 * t129 + t14 * t48; + const auto t213 = t212 * t36; + const auto t214 = t213 * t44; + const auto t215 = t213 * t84; + const auto t216 = std::pow(t14, 2); + const auto t217 = t45 * t83; + const auto t218 = t11 * t25 * t36 + t217 * t92 - t83 * t95 - t87 * t92; + const auto t219 = t44 * t83; + const auto t220 = t101 * t219 + t30 - t36 * t83 * t84 * t99 + + t44 * t87 * t99 + t45 * (2 * t14 * t23 - t31); + const auto t221 = 2 * t106; + const auto t222 = t14 * t9; + const auto t223 = + t106 * t217 - t106 * t87 - t109 * t83 + t36 * (t198 + t222); + const auto t224 = t0_x + t111 * t116 * t83 - t111 * t44 * t87 + t115 * t219 + + t150 + t45 * (2 * t24 + t27); + const auto t225 = -t53 * (t11 + t219 * t28); + const auto t226 = t159 * t83; + const auto t227 = t20 + t2_x; + const auto t228 = t120 * (-t126 * t83 - t227); + const auto t229 = t129 * t25 + t15 * t49; + const auto t230 = t229 * t36; + const auto t231 = t230 * t44; + const auto t232 = t44 * t95; + const auto t233 = t44 * t92; + const auto t234 = t101 * t233 + t206 + t232 * t99 - t36 * t84 * t92 * t99 + + t45 * (2 * t14 * t17 - t61); + const auto t235 = t106 * t232 - t106 * t36 * t84 * t92 + t109 * t233 + t21 + + t45 * (2 * t17 * t25 - t24); + const auto t236 = t115 * t92; + const auto t237 = t197 + t222; + const auto t238 = t45 * t92; + const auto t239 = t111 * t238; + const auto t240 = t14 * t64 + t91; + const auto t241 = t120 * (-t122 * t240 - t210); + const auto t242 = t53 * (t19 * t233 + t227); + const auto t243 = -t182 * t240; + const auto t244 = t18 * t49 + t23 * t48; + const auto t245 = t244 * t36; + const auto t246 = t245 * t44; + const auto t247 = std::pow(t23, 2); + const auto t248 = std::pow(t17, 2); + const auto t249 = t36 * t9; + const auto t250 = t45 * t99; + const auto t251 = -t101 * t106 + t106 * t250 - t109 * t99 + t23 * t249; + const auto t252 = t115 * t99; + const auto t253 = t111 * t250; + const auto t254 = t17 * t64 + t98; + const auto t255 = -t123 * t254; + const auto t256 = t53 * (t19 * t44 * t99 + t1_z + t29); + const auto t257 = t1_y + t6; + const auto t258 = t120 * (-t126 * t254 - t257); + const auto t259 = t10 * t48 + t129 * t17; + const auto t260 = t259 * t36; + const auto t261 = t260 * t84; + const auto t262 = t260 * t44; + const auto t263 = std::pow(t9, 2); + const auto t264 = t106 * t115; + const auto t265 = t106 * t45; + const auto t266 = t111 * t265; + const auto t267 = t120 * (t105 * t122 + t17); + const auto t268 = -t105 * t159; + const auto t269 = -t53 * (t106 * t34 * t44 + t9); + const auto t270 = t129 * t47 + t49 * t9; + const auto t271 = t270 * t36; + const auto t272 = t271 * t44; + const auto t273 = t53 * (t111 * t28 * t44 + t257); + const auto t274 = t120 * (-t111 * t158 - t1_x - t20); + const auto t275 = t111 * t182; + const auto t276 = t111 * t45; + const auto t277 = t121 * t43; + const auto t278 = t120 * t277; + const auto t279 = (1.0 / 2.0) * t133 * t278; + const auto t280 = -t279 * t28; + const auto t281 = t279 * t64; + const auto t282 = -t279 * t34; + const auto t283 = t131 * t134; + hess[0] = t54 + * (-t36 * (t2 + t5) - t38 * t41 + std::pow(t40, 2) * t45 + t41 * t52); + hess[1] = t54 * (t52 * t58 + t68); + hess[2] = t54 * (t36 * t4 * t55 + t38 * t72 - t52 * t74 - t71 - t73); + hess[3] = t54 * (t36 * t80 + t38 * t77 - t52 * t82 - t76 - t81); + hess[4] = t53 * (2 * t36 * t50 * t83 * t84 - t90); + hess[5] = t53 * (2 * t36 * t50 * t84 * t92 - t97); + hess[6] = t54 * (-t104 + 2 * t36 * t44 * t50 * t99); + hess[7] = t53 * (2 * t106 * t36 * t50 * t84 - t110); + hess[8] = t53 * (-t112 * t51 * t84 - t117); + hess[9] = t124; + hess[10] = t125; + hess[11] = t128; + hess[12] = t134 * (-t131 * t38 + t131 * t67 + t132 * t51 - t51); + hess[13] = t54 * (t137 * t41 + t68); + hess[14] = t54 + * (t137 * t58 - t36 * (t138 + t5) + t45 * std::pow(t57, 2) - t58 * t65); + hess[15] = t54 * (t1 * t36 * t4 - t137 * t74 - t139 - t141 + t65 * t72); + hess[16] = t53 * (-t142 * t82 - t146); + hess[17] = t54 * (t137 * t147 + t149); + hess[18] = t53 * (2 * t135 * t36 * t84 * t92 - t152); + hess[19] = t53 * (2 * t135 * t36 * t84 * t99 - t153); + hess[20] = t54 * (2 * t106 * t135 * t36 * t44 - t155); + hess[21] = t53 * (-t112 * t142 - t156); + hess[22] = t157; + hess[23] = t160; + hess[24] = t161; + hess[25] = t134 * (t131 * t140 - t131 * t65 + t132 * t136 - t136); + hess[26] = t54 * (t164 * t41 + t38 * t72 + t4 * t66 - t71 - t73); + hess[27] = t54 * (t1 * t36 * t4 - t139 - t141 + t164 * t58 + t65 * t72); + hess[28] = t54 + * (-t164 * t74 + t36 * t44 * std::pow(t72, 2) - t36 * (t138 + t2) + + 2 * t70 * t72); + hess[29] = t53 * (-t163 * t82 * t84 + t169); + hess[30] = t53 * (2 * t162 * t36 * t83 * t84 - t171); + hess[31] = t54 * (t164 * t174 - t172 + t173 * t36 - t176 + t72 * t95); + hess[32] = t53 * (2 * t162 * t36 * t84 * t99 - t177); + hess[33] = t53 * (2 * t106 * t162 * t36 * t84 - t178); + hess[34] = t54 * (-t112 * t164 + t179); + hess[35] = t180; + hess[36] = t181; + hess[37] = t183; + hess[38] = + t134 * (t130 * t162 * t36 * t44 - t131 * t175 - t131 * t70 - t163); + hess[39] = t54 * (t186 * t41 + t36 * t80 + t38 * t77 - t76 - t81); + hess[40] = t53 * (-t146 + 2 * t184 * t36 * t57 * t84); + hess[41] = t53 * (t169 - t187 * t74); + hess[42] = t54 + * (-t186 * t82 - t36 * (t188 + t189) + t45 * std::pow(t77, 2) + + t75 * t82); + hess[43] = t54 * (t147 * t186 - t190 + t191 * t25 - t193 + t77 * t87); + hess[44] = t54 * (t11 * t191 + t174 * t186 - t194 - t195 + t77 * t95); + hess[45] = t54 * (t101 * t77 + t186 * t200 - t196 + t199 * t36 - t201); + hess[46] = t53 * (2 * t106 * t184 * t36 * t84 - t204); + hess[47] = t53 * (-t112 * t187 + t207); + hess[48] = t208; + hess[49] = t209; + hess[50] = t211; + hess[51] = + t134 * (t130 * t184 * t36 * t44 - t131 * t192 - t131 * t75 - t185); + hess[52] = t53 * (2 * t212 * t36 * t40 * t84 - t90); + hess[53] = t54 * (t149 + t214 * t58); + hess[54] = t53 * (-t171 - t215 * t74); + hess[55] = t54 * (t14 * t25 * t36 - t190 - t193 - t214 * t82 + t77 * t87); + hess[56] = t54 + * (-t147 * t87 + 2 * t212 * t36 * t44 * t83 + + t36 * t44 * std::pow(t83, 2) - t36 * (t189 + t216)); + hess[57] = t54 * (t174 * t214 + t218); + hess[58] = t53 * (2 * t212 * t36 * t84 * t99 - t220); + hess[59] = t54 * (t214 * t221 + t223); + hess[60] = t53 * (-t112 * t215 - t224); + hess[61] = t225; + hess[62] = t226; + hess[63] = t228; + hess[64] = t134 + * (t130 * t212 * t36 * t44 + (1.0 / 2.0) * t130 * t36 * t44 * t83 + - t131 * t87 - t213); + hess[65] = t53 * (2 * t229 * t36 * t40 * t84 - t97); + hess[66] = t53 * (-t152 + 2 * t229 * t36 * t57 * t84); + hess[67] = t54 * (-t172 + t173 * t36 - t176 - t231 * t74 + t72 * t95); + hess[68] = t54 * (t11 * t14 * t36 - t194 - t195 - t231 * t82 + t77 * t95); + hess[69] = t54 * (t147 * t231 + t218); + hess[70] = t54 + * (-t174 * t95 + 2 * t229 * t36 * t44 * t92 + + t36 * t44 * std::pow(t92, 2) - t36 * (t188 + t216)); + hess[71] = t53 * (2 * t229 * t36 * t84 * t99 - t234); + hess[72] = t53 * (2 * t106 * t229 * t36 * t84 - t235); + hess[73] = t54 * (t111 * t95 - t112 * t231 - t236 + t237 * t36 - t239); + hess[74] = t241; + hess[75] = t242; + hess[76] = t243; + hess[77] = t134 + * (t130 * t229 * t36 * t44 + (1.0 / 2.0) * t130 * t36 * t44 * t92 + - t131 * t95 - t230); + hess[78] = t54 * (-t104 + 2 * t244 * t36 * t40 * t44); + hess[79] = t53 * (-t153 + 2 * t244 * t36 * t57 * t84); + hess[80] = t53 * (-t177 - t245 * t74 * t84); + hess[81] = t54 * (t101 * t77 - t196 + t199 * t36 - t201 - t246 * t82); + hess[82] = t53 * (-t220 + 2 * t244 * t36 * t83 * t84); + hess[83] = t53 * (-t234 + 2 * t244 * t36 * t84 * t92); + hess[84] = t54 + * (-t101 * t200 + 2 * t244 * t36 * t44 * t99 + + t36 * t44 * std::pow(t99, 2) - t36 * (t247 + t248)); + hess[85] = t54 * (t221 * t246 + t251); + hess[86] = t54 * (t101 * t111 - t112 * t246 + t17 * t36 * t9 - t252 - t253); + hess[87] = t255; + hess[88] = t256; + hess[89] = t258; + hess[90] = t134 + * (-t101 * t131 + t130 * t244 * t36 * t44 + + (1.0 / 2.0) * t130 * t36 * t44 * t99 - t245); + hess[91] = t53 * (-t110 + 2 * t259 * t36 * t40 * t84); + hess[92] = t54 * (-t155 + 2 * t259 * t36 * t44 * t57); + hess[93] = t53 * (-t178 - t261 * t74); + hess[94] = t53 * (-t204 - t261 * t82); + hess[95] = t54 * (t147 * t262 + t223); + hess[96] = t53 * (-t235 + 2 * t259 * t36 * t84 * t92); + hess[97] = t54 * (t200 * t262 + t251); + hess[98] = t54 + * (std::pow(t106, 2) * t36 * t44 + 2 * t106 * t259 * t36 * t44 + - t109 * t221 - t36 * (t248 + t263)); + hess[99] = + t54 * (t109 * t111 - t112 * t262 + t17 * t23 * t36 - t264 - t266); + hess[100] = t267; + hess[101] = t268; + hess[102] = t269; + hess[103] = t134 + * ((1.0 / 2.0) * t106 * t130 * t36 * t44 - t109 * t131 + + t130 * t259 * t36 * t44 - t260); + hess[104] = t53 * (-t117 + 2 * t270 * t36 * t40 * t84); + hess[105] = t53 * (-t156 + 2 * t270 * t36 * t57 * t84); + hess[106] = t54 * (t179 - t272 * t74); + hess[107] = t53 * (t207 - t271 * t82 * t84); + hess[108] = t53 * (-t224 + 2 * t270 * t36 * t83 * t84); + hess[109] = t54 * (t111 * t95 + t174 * t272 - t236 + t237 * t36 - t239); + hess[110] = t54 * (t101 * t111 + t17 * t249 + t200 * t272 - t252 - t253); + hess[111] = + t54 * (t109 * t111 + t17 * t23 * t36 + t221 * t272 - t264 - t266); + hess[112] = t54 + * (std::pow(t111, 2) * t45 + t112 * t115 - t112 * t272 + - t36 * (t247 + t263)); + hess[113] = t273; + hess[114] = t274; + hess[115] = t275; + hess[116] = + t134 * (-t115 * t131 + t130 * t270 * t36 * t44 - t131 * t276 - t271); + hess[117] = t124; + hess[118] = t157; + hess[119] = t180; + hess[120] = t208; + hess[121] = t225; + hess[122] = t241; + hess[123] = t255; + hess[124] = t267; + hess[125] = t273; + hess[126] = 0; + hess[127] = 0; + hess[128] = 0; + hess[129] = t280; + hess[130] = t125; + hess[131] = t160; + hess[132] = t181; + hess[133] = t209; + hess[134] = t226; + hess[135] = t242; + hess[136] = t256; + hess[137] = t268; + hess[138] = t274; + hess[139] = 0; + hess[140] = 0; + hess[141] = 0; + hess[142] = t281; + hess[143] = t128; + hess[144] = t161; + hess[145] = t183; + hess[146] = t211; + hess[147] = t228; + hess[148] = t243; + hess[149] = t258; + hess[150] = t269; + hess[151] = t275; + hess[152] = 0; + hess[153] = 0; + hess[154] = 0; + hess[155] = t282; + hess[156] = t283 * (t37 + t67); + hess[157] = t283 * (t140 + t167 - t59 + t60); + hess[158] = -t283 * (t144 + t175 + t55 * t7 - t69); + hess[159] = -t283 * (t192 + t75); + hess[160] = t283 * (t217 + t86); + hess[161] = t283 * (t238 - t93 + t94); + hess[162] = t283 * (t100 + t250); + hess[163] = t283 * (-t107 + t108 + t265); + hess[164] = -t283 * (-t114 + t276 + t7 * t9); + hess[165] = t280; + hess[166] = t281; + hess[167] = t282; + hess[168] = + (1.0 / 4.0) * t278 * (t277 + 2) * (t35 - t64 * t7) / std::pow(d, 2); +} + +} // namespace ipc::autogen diff --git a/src/ipc/smooth_contact/distance/autogen.hpp b/src/ipc/smooth_contact/distance/autogen.hpp new file mode 100644 index 000000000..cbc899013 --- /dev/null +++ b/src/ipc/smooth_contact/distance/autogen.hpp @@ -0,0 +1,163 @@ +#pragma once + +namespace ipc::autogen { + +// hess is (144×1) flattened in column-major order +void edge_edge_closest_point_hessian_a( + double ea0_x, + double ea0_y, + double ea0_z, + double ea1_x, + double ea1_y, + double ea1_z, + double eb0_x, + double eb0_y, + double eb0_z, + double eb1_x, + double eb1_y, + double eb1_z, + double hess[144]); + +// hess is (144×1) flattened in column-major order +void edge_edge_closest_point_hessian_b( + double ea0_x, + double ea0_y, + double ea0_z, + double ea1_x, + double ea1_y, + double ea1_z, + double eb0_x, + double eb0_y, + double eb0_z, + double eb1_x, + double eb1_y, + double eb1_z, + double hess[144]); + +void face_normal_squared_norm_gradient( + double t0_x, + double t0_y, + double t0_z, + double t1_x, + double t1_y, + double t1_z, + double t2_x, + double t2_y, + double t2_z, + double grad[9]); + +// hess is (81×1) flattened in column-major order +void face_normal_squared_norm_hessian( + double t0_x, + double t0_y, + double t0_z, + double t1_x, + double t1_y, + double t1_z, + double t2_x, + double t2_y, + double t2_z, + double hess[81]); + +void face_term_aux_gradient( + double t0_x, + double t0_y, + double t0_z, + double t1_x, + double t1_y, + double t1_z, + double t2_x, + double t2_y, + double t2_z, + double p1_x, + double p1_y, + double p1_z, + double p2_x, + double p2_y, + double p2_z, + double grad[15]); + +// hess is (225×1) flattened in column-major order +void face_term_aux_hessian( + double t0_x, + double t0_y, + double t0_z, + double t1_x, + double t1_y, + double t1_z, + double t2_x, + double t2_y, + double t2_z, + double p1_x, + double p1_y, + double p1_z, + double p2_x, + double p2_y, + double p2_z, + double hess[225]); + +// hess is (144×1) flattened in column-major order +void triangle_closest_point_hessian_0( + double p_x, + double p_y, + double p_z, + double t0_x, + double t0_y, + double t0_z, + double t1_x, + double t1_y, + double t1_z, + double t2_x, + double t2_y, + double t2_z, + double hess[144]); +// hess is (144×1) flattened in column-major order +void triangle_closest_point_hessian_1( + double p_x, + double p_y, + double p_z, + double t0_x, + double t0_y, + double t0_z, + double t1_x, + double t1_y, + double t1_z, + double t2_x, + double t2_y, + double t2_z, + double hess[144]); + +void face_term_aux_fast_gradient( + double t0_x, + double t0_y, + double t0_z, + double t1_x, + double t1_y, + double t1_z, + double t2_x, + double t2_y, + double t2_z, + double p_x, + double p_y, + double p_z, + double d, + double grad[13]); + +// hess is (169×1) flattened in column-major order +void face_term_aux_fast_hessian( + double t0_x, + double t0_y, + double t0_z, + double t1_x, + double t1_y, + double t1_z, + double t2_x, + double t2_y, + double t2_z, + double p_x, + double p_y, + double p_z, + double d, + double hess[169]); + +} // namespace ipc::autogen diff --git a/src/ipc/smooth_contact/distance/edge_edge.cpp b/src/ipc/smooth_contact/distance/edge_edge.cpp new file mode 100644 index 000000000..23a4cdd30 --- /dev/null +++ b/src/ipc/smooth_contact/distance/edge_edge.cpp @@ -0,0 +1,554 @@ +#include "edge_edge.hpp" + +#include "autogen.hpp" + +#include + +#include + +namespace ipc { + +std::tuple> +line_line_closest_point_direction_gradient( + Eigen::ConstRef ea0, + Eigen::ConstRef ea1, + Eigen::ConstRef eb0, + Eigen::ConstRef eb1) +{ + const Eigen::Vector3d ea = ea1 - ea0; + const Eigen::Vector3d eb = eb1 - eb0; + const Eigen::Vector3d normal = ea.cross(eb); + const Eigen::Matrix cross_grad = + cross_product_gradient(ea, eb); + + const double normal_sqr_norm = normal.squaredNorm(); + const Eigen::Vector3d t = eb0 - ea0; + const Eigen::Vector3d vec = (t.dot(normal) / normal_sqr_norm) * normal; + + Eigen::Matrix grad = Eigen::Matrix::Zero(); + // derivative wrt. normal, tempararily store here + grad(Eigen::all, { 3, 4, 5 }) = + (normal * t.transpose() + + t.transpose() * normal * Eigen::Matrix3d::Identity() + - 2 * vec * normal.transpose()) + / normal_sqr_norm; + // derivative wrt. t + grad.middleCols<3>(6) = normal * normal.transpose() / normal_sqr_norm; + grad.leftCols<3>() = -grad.middleCols<3>(6); + // contributions from n + grad(Eigen::all, { 3, 4, 5, 9, 10, 11 }) = + grad(Eigen::all, { 3, 4, 5 }) * cross_grad; + grad(Eigen::all, { 0, 1, 2, 6, 7, 8 }) -= + grad(Eigen::all, { 3, 4, 5, 9, 10, 11 }); + + return std::make_tuple(vec, grad); +} + +std::tuple< + Eigen::Vector3d, + Eigen::Matrix, + std::array> +line_line_closest_point_direction_hessian( + Eigen::ConstRef ea0, + Eigen::ConstRef ea1, + Eigen::ConstRef eb0, + Eigen::ConstRef eb1) +{ + const Eigen::Vector3d ea = ea1 - ea0; + const Eigen::Vector3d eb = eb1 - eb0; + const Eigen::Vector3d normal = ea.cross(eb); + const Eigen::Matrix cross_grad = + cross_product_gradient(ea, eb); + const std::array cross_hess = cross_product_hessian(ea, eb); + + const double normal_sqr_norm = normal.squaredNorm(); + const Eigen::Vector3d t = eb0 - ea0; + const Eigen::Vector3d vec = (t.dot(normal) / normal_sqr_norm) * normal; + + Eigen::Matrix grad = Eigen::Matrix::Zero(); + // derivative wrt. normal and t + Eigen::Matrix3d grad_normal = + (normal * t.transpose() - 2 * vec * normal.transpose()) + / normal_sqr_norm; + grad_normal.diagonal().array() += + (t.transpose() * normal)(0) / normal_sqr_norm; + Eigen::Matrix3d grad_t = normal * normal.transpose() / normal_sqr_norm; + grad.middleCols<3>(6) = grad_t; + grad.leftCols<3>() = -grad_t; + // contributions from n to ea, eb + grad(Eigen::all, { 3, 4, 5, 9, 10, 11 }) = grad_normal * cross_grad; + grad(Eigen::all, { 0, 1, 2, 6, 7, 8 }) -= + grad(Eigen::all, { 3, 4, 5, 9, 10, 11 }); + + // hessian of output wrt. normal + std::array hess_wrt_normal; + // hessian of output wrt. normal (row) and t (col) + std::array mixed_hess; + for (int d = 0; d < 3; d++) { + hess_wrt_normal[d] = -2 + * (normal * grad_normal.row(d) + + grad_normal.row(d).transpose() * normal.transpose()); + hess_wrt_normal[d].col(d) += t; + hess_wrt_normal[d].row(d) += t.transpose(); + hess_wrt_normal[d].diagonal().array() -= 2 * vec(d); + hess_wrt_normal[d] /= normal_sqr_norm; + + mixed_hess[d] = -2 * normal * grad_t.row(d); + mixed_hess[d].row(d) += normal.transpose(); + mixed_hess[d].diagonal().array() += normal(d); + mixed_hess[d] /= normal_sqr_norm; + } + + // grad and hessian of [normal, t] wrt. [ea0, ea1, eb0, eb1] + Eigen::Matrix inner_grad; + inner_grad << -cross_grad.leftCols<3>(), cross_grad.leftCols<3>(), + -cross_grad.rightCols<3>(), cross_grad.rightCols<3>(); + + std::array inner_hess; + { + Eigen::Matrix2d tmp; + tmp << 1, -1, -1, 1; + for (int d = 0; d < 3; d++) { + inner_hess[d] + << Eigen::KroneckerProduct( + tmp, cross_hess[d].block<3, 3>(0, 0)), + Eigen::KroneckerProduct( + tmp, cross_hess[d].block<3, 3>(0, 3)), + Eigen::KroneckerProduct( + tmp, cross_hess[d].block<3, 3>(3, 0)), + Eigen::KroneckerProduct( + tmp, cross_hess[d].block<3, 3>(3, 3)); + } + } + + std::array hess; + for (int d = 0; d < 3; d++) { + Eigen::Matrix tmp = + inner_grad.transpose() * mixed_hess[d]; + hess[d] = inner_grad.transpose() * hess_wrt_normal[d] * inner_grad; + hess[d].middleCols<3>(6) += tmp; + hess[d].middleCols<3>(0) -= tmp; + hess[d].middleRows<3>(6) += tmp.transpose(); + hess[d].middleRows<3>(0) -= tmp.transpose(); + for (int i = 0; i < 3; i++) { + hess[d] += inner_hess[i] * grad_normal(d, i); + } + } + + return std::make_tuple(vec, grad, hess); +} + +std::tuple> +line_line_closest_point_pairs_gradient( + Eigen::ConstRef ea0, + Eigen::ConstRef ea1, + Eigen::ConstRef eb0, + Eigen::ConstRef eb1) +{ + const auto uv = edge_edge_closest_point(ea0, ea1, eb0, eb1); + const auto J = edge_edge_closest_point_jacobian(ea0, ea1, eb0, eb1); + + Vector6d out; + out << uv(0) * (ea1 - ea0) + ea0, uv(1) * (eb1 - eb0) + eb0; + Eigen::Matrix grad = Eigen::Matrix::Zero(); + grad.topRows<3>() += (ea1 - ea0) * J.row(0); + grad.block<3, 3>(0, 3).diagonal().array() += uv(0); + grad.block<3, 3>(0, 0).diagonal().array() += (1 - uv(0)); + grad.bottomRows<3>() += (eb1 - eb0) * J.row(1); + grad.block<3, 3>(3, 9).diagonal().array() += uv(1); + grad.block<3, 3>(3, 6).diagonal().array() += (1 - uv(1)); + + return { out, grad }; +} + +std::tuple, std::array> +line_line_closest_point_pairs_hessian( + Eigen::ConstRef ea0, + Eigen::ConstRef ea1, + Eigen::ConstRef eb0, + Eigen::ConstRef eb1) +{ + const auto uv = edge_edge_closest_point(ea0, ea1, eb0, eb1); + const auto J = edge_edge_closest_point_jacobian(ea0, ea1, eb0, eb1); + + Vector6d out; + out << uv(0) * (ea1 - ea0) + ea0, uv(1) * (eb1 - eb0) + eb0; + + Eigen::Matrix grad = Eigen::Matrix::Zero(); + grad.topRows<3>() += (ea1 - ea0) * J.row(0); + grad.block<3, 3>(0, 3).diagonal().array() += uv(0); + grad.block<3, 3>(0, 0).diagonal().array() += (1 - uv(0)); + grad.bottomRows<3>() += (eb1 - eb0) * J.row(1); + grad.block<3, 3>(3, 9).diagonal().array() += uv(1); + grad.block<3, 3>(3, 6).diagonal().array() += (1 - uv(1)); + + std::array hess; + for (auto& h : hess) { + h.setZero(); + } + + { + Eigen::Matrix Ha, Hb; + autogen::edge_edge_closest_point_hessian_a( + ea0.x(), ea0.y(), ea0.z(), ea1.x(), ea1.y(), ea1.z(), eb0.x(), + eb0.y(), eb0.z(), eb1.x(), eb1.y(), eb1.z(), Ha.data()); + autogen::edge_edge_closest_point_hessian_b( + ea0.x(), ea0.y(), ea0.z(), ea1.x(), ea1.y(), ea1.z(), eb0.x(), + eb0.y(), eb0.z(), eb1.x(), eb1.y(), eb1.z(), Hb.data()); + + for (int d = 0; d < 3; d++) { + // wrt. uv + hess[d] += (ea1(d) - ea0(d)) * Ha; + hess[d + 3] += (eb1(d) - eb0(d)) * Hb; + + // wrt. ea0 & uv(0) + hess[d].row(d) -= J.row(0); + hess[d].col(d) -= J.row(0).transpose(); + + // wrt. ea1 & uv(0) + hess[d].row(d + 3) += J.row(0); + hess[d].col(d + 3) += J.row(0).transpose(); + + // wrt. eb0 & uv(1) + hess[d + 3].row(d + 6) -= J.row(1); + hess[d + 3].col(d + 6) -= J.row(1).transpose(); + + // wrt. eb1 & uv(1) + hess[d + 3].row(d + 9) += J.row(1); + hess[d + 3].col(d + 9) += J.row(1).transpose(); + } + } + + return { out, grad, hess }; +} + +template +scalar line_line_sqr_distance( + Eigen::ConstRef> ea0, + Eigen::ConstRef> ea1, + Eigen::ConstRef> eb0, + Eigen::ConstRef> eb1) +{ + const Eigen::Vector3 normal = (ea1 - ea0).cross(eb1 - eb0); + const scalar line_to_line = (eb0 - ea0).dot(normal); + return line_to_line * line_to_line / normal.squaredNorm(); +} + +template +scalar edge_edge_sqr_distance( + Eigen::ConstRef> ea0, + Eigen::ConstRef> ea1, + Eigen::ConstRef> eb0, + Eigen::ConstRef> eb1, + EdgeEdgeDistanceType dtype) +{ + if constexpr (std::is_same::value) { + if (dtype == EdgeEdgeDistanceType::AUTO) { + dtype = edge_edge_distance_type(ea0, ea1, eb0, eb1); + } + } + + switch (dtype) { + case EdgeEdgeDistanceType::EA0_EB0: + return PointEdgeDistance::point_point_sqr_distance(ea0, eb0); + + case EdgeEdgeDistanceType::EA0_EB1: + return PointEdgeDistance::point_point_sqr_distance(ea0, eb1); + + case EdgeEdgeDistanceType::EA1_EB0: + return PointEdgeDistance::point_point_sqr_distance(ea1, eb0); + + case EdgeEdgeDistanceType::EA1_EB1: + return PointEdgeDistance::point_point_sqr_distance(ea1, eb1); + + case EdgeEdgeDistanceType::EA_EB0: + return PointEdgeDistance::point_line_sqr_distance( + eb0, ea0, ea1); + + case EdgeEdgeDistanceType::EA_EB1: + return PointEdgeDistance::point_line_sqr_distance( + eb1, ea0, ea1); + + case EdgeEdgeDistanceType::EA0_EB: + return PointEdgeDistance::point_line_sqr_distance( + ea0, eb0, eb1); + + case EdgeEdgeDistanceType::EA1_EB: + return PointEdgeDistance::point_line_sqr_distance( + ea1, eb0, eb1); + + case EdgeEdgeDistanceType::EA_EB: + return line_line_sqr_distance(ea0, ea1, eb0, eb1); + + default: + throw std::invalid_argument( + "Invalid distance type for edge-edge distance!"); + } +} + +template +Eigen::Vector3 line_line_closest_point_direction( + Eigen::ConstRef> ea0, + Eigen::ConstRef> ea1, + Eigen::ConstRef> eb0, + Eigen::ConstRef> eb1) +{ + const Eigen::Vector3 normal = (ea1 - ea0).cross(eb1 - eb0); + return ((eb0 - ea0).dot(normal) / normal.squaredNorm()) * normal; +} + +template +Eigen::Matrix line_line_closest_point_pairs( + Eigen::ConstRef> ea0, + Eigen::ConstRef> ea1, + Eigen::ConstRef> eb0, + Eigen::ConstRef> eb1) +{ + const Eigen::Vector3 ta = ea1 - ea0; + const Eigen::Vector3 tb = eb1 - eb0; + const scalar la = ta.squaredNorm(); + const scalar lb = tb.squaredNorm(); + const scalar lab = ta.dot(tb); + const Eigen::Vector3 d = eb0 - ea0; + + Eigen::Matrix out; + const scalar fac = la * lb - pow(lab, 2); + out.col(0) = ea0 + (lb * ta.dot(d) - lab * tb.dot(d)) / fac * ta; + out.col(1) = eb0 + (lab * ta.dot(d) - la * tb.dot(d)) / fac * tb; + + return out; +} + +/// @brief Computes the direction of the closest point pair +/// @param ea0 Vertex 0 of edge 0 +/// @param ea1 Vertex 1 of edge 0 +/// @param eb0 Vertex 0 of edge 1 +/// @param eb1 Vertex 1 of edge 1 +/// @param dtype Edge-edge distance type +/// @return Difference of the pair of closest point, pointing from edge 0 to edge 1 +template +Eigen::Vector3 edge_edge_closest_point_direction( + Eigen::ConstRef> ea0, + Eigen::ConstRef> ea1, + Eigen::ConstRef> eb0, + Eigen::ConstRef> eb1, + EdgeEdgeDistanceType dtype) +{ + if constexpr (std::is_same::value) { + if (dtype == EdgeEdgeDistanceType::AUTO) { + dtype = edge_edge_distance_type(ea0, ea1, eb0, eb1); + } + } + + switch (dtype) { + case EdgeEdgeDistanceType::EA0_EB0: + return (eb0 - ea0); + + case EdgeEdgeDistanceType::EA0_EB1: + return (eb1 - ea0); + + case EdgeEdgeDistanceType::EA1_EB0: + return (eb0 - ea1); + + case EdgeEdgeDistanceType::EA1_EB1: + return (eb1 - ea1); + + case EdgeEdgeDistanceType::EA_EB0: + return PointEdgeDistance::point_line_closest_point_direction( + eb0, ea0, ea1); + + case EdgeEdgeDistanceType::EA_EB1: + return PointEdgeDistance::point_line_closest_point_direction( + eb1, ea0, ea1); + + case EdgeEdgeDistanceType::EA0_EB: + return -PointEdgeDistance< + scalar, 3>::point_line_closest_point_direction(ea0, eb0, eb1); + + case EdgeEdgeDistanceType::EA1_EB: + return -PointEdgeDistance< + scalar, 3>::point_line_closest_point_direction(ea1, eb0, eb1); + + case EdgeEdgeDistanceType::EA_EB: + return line_line_closest_point_direction(ea0, ea1, eb0, eb1); + + default: + throw std::invalid_argument( + "Invalid distance type for edge-edge distance!"); + } +} + +template +Eigen::Matrix edge_edge_closest_point_pairs( + Eigen::ConstRef> ea0, + Eigen::ConstRef> ea1, + Eigen::ConstRef> eb0, + Eigen::ConstRef> eb1, + EdgeEdgeDistanceType dtype) +{ + if constexpr (std::is_same::value) { + if (dtype == EdgeEdgeDistanceType::AUTO) { + dtype = edge_edge_distance_type(ea0, ea1, eb0, eb1); + } + } + + Eigen::Matrix out; + switch (dtype) { + case EdgeEdgeDistanceType::EA0_EB0: + out << ea0, eb0; + break; + + case EdgeEdgeDistanceType::EA0_EB1: + out << ea0, eb1; + break; + + case EdgeEdgeDistanceType::EA1_EB0: + out << ea1, eb0; + break; + + case EdgeEdgeDistanceType::EA1_EB1: + out << ea1, eb1; + break; + + case EdgeEdgeDistanceType::EA_EB0: + out << eb0 + - PointEdgeDistance:: + point_line_closest_point_direction(eb0, ea0, ea1), + eb0; + break; + + case EdgeEdgeDistanceType::EA_EB1: + out << eb1 + - PointEdgeDistance:: + point_line_closest_point_direction(eb1, ea0, ea1), + eb1; + break; + + case EdgeEdgeDistanceType::EA0_EB: + out << ea0, + ea0 + - PointEdgeDistance::point_line_closest_point_direction( + ea0, eb0, eb1); + break; + + case EdgeEdgeDistanceType::EA1_EB: + out << ea1, + ea1 + - PointEdgeDistance::point_line_closest_point_direction( + ea1, eb0, eb1); + break; + + case EdgeEdgeDistanceType::EA_EB: + out = line_line_closest_point_pairs(ea0, ea1, eb0, eb1); + break; + + default: + throw std::invalid_argument( + "Invalid distance type for edge-edge distance!"); + } + + return out; +} + +template Eigen::Vector3d edge_edge_closest_point_direction( + Eigen::ConstRef ea0, + Eigen::ConstRef ea1, + Eigen::ConstRef eb0, + Eigen::ConstRef eb1, + EdgeEdgeDistanceType dtype); +template Eigen::Vector3> edge_edge_closest_point_direction( + Eigen::ConstRef>> ea0, + Eigen::ConstRef>> ea1, + Eigen::ConstRef>> eb0, + Eigen::ConstRef>> eb1, + EdgeEdgeDistanceType dtype); +template Eigen::Vector3> edge_edge_closest_point_direction( + Eigen::ConstRef>> ea0, + Eigen::ConstRef>> ea1, + Eigen::ConstRef>> eb0, + Eigen::ConstRef>> eb1, + EdgeEdgeDistanceType dtype); +template Eigen::Vector3> edge_edge_closest_point_direction( + Eigen::ConstRef>> ea0, + Eigen::ConstRef>> ea1, + Eigen::ConstRef>> eb0, + Eigen::ConstRef>> eb1, + EdgeEdgeDistanceType dtype); +template Eigen::Vector3> edge_edge_closest_point_direction( + Eigen::ConstRef>> ea0, + Eigen::ConstRef>> ea1, + Eigen::ConstRef>> eb0, + Eigen::ConstRef>> eb1, + EdgeEdgeDistanceType dtype); +template Eigen::Vector3> edge_edge_closest_point_direction( + Eigen::ConstRef>> ea0, + Eigen::ConstRef>> ea1, + Eigen::ConstRef>> eb0, + Eigen::ConstRef>> eb1, + EdgeEdgeDistanceType dtype); +template Eigen::Vector3> edge_edge_closest_point_direction( + Eigen::ConstRef>> ea0, + Eigen::ConstRef>> ea1, + Eigen::ConstRef>> eb0, + Eigen::ConstRef>> eb1, + EdgeEdgeDistanceType dtype); + +template double edge_edge_sqr_distance( + Eigen::ConstRef ea0, + Eigen::ConstRef ea1, + Eigen::ConstRef eb0, + Eigen::ConstRef eb1, + EdgeEdgeDistanceType dtype); +template ADGrad<9> edge_edge_sqr_distance( + Eigen::ConstRef>> ea0, + Eigen::ConstRef>> ea1, + Eigen::ConstRef>> eb0, + Eigen::ConstRef>> eb1, + EdgeEdgeDistanceType dtype); +template ADHessian<9> edge_edge_sqr_distance( + Eigen::ConstRef>> ea0, + Eigen::ConstRef>> ea1, + Eigen::ConstRef>> eb0, + Eigen::ConstRef>> eb1, + EdgeEdgeDistanceType dtype); +template ADGrad<12> edge_edge_sqr_distance( + Eigen::ConstRef>> ea0, + Eigen::ConstRef>> ea1, + Eigen::ConstRef>> eb0, + Eigen::ConstRef>> eb1, + EdgeEdgeDistanceType dtype); +template ADHessian<12> edge_edge_sqr_distance( + Eigen::ConstRef>> ea0, + Eigen::ConstRef>> ea1, + Eigen::ConstRef>> eb0, + Eigen::ConstRef>> eb1, + EdgeEdgeDistanceType dtype); +template ADGrad<13> edge_edge_sqr_distance( + Eigen::ConstRef>> ea0, + Eigen::ConstRef>> ea1, + Eigen::ConstRef>> eb0, + Eigen::ConstRef>> eb1, + EdgeEdgeDistanceType dtype); +template ADHessian<13> edge_edge_sqr_distance( + Eigen::ConstRef>> ea0, + Eigen::ConstRef>> ea1, + Eigen::ConstRef>> eb0, + Eigen::ConstRef>> eb1, + EdgeEdgeDistanceType dtype); + +template Eigen::Matrix line_line_closest_point_pairs( + Eigen::ConstRef ea0, + Eigen::ConstRef ea1, + Eigen::ConstRef eb0, + Eigen::ConstRef eb1); +template Eigen::Matrix, 3, 2> line_line_closest_point_pairs( + Eigen::ConstRef>> ea0, + Eigen::ConstRef>> ea1, + Eigen::ConstRef>> eb0, + Eigen::ConstRef>> eb1); +template Eigen::Matrix, 3, 2> line_line_closest_point_pairs( + Eigen::ConstRef>> ea0, + Eigen::ConstRef>> ea1, + Eigen::ConstRef>> eb0, + Eigen::ConstRef>> eb1); +} // namespace ipc diff --git a/src/ipc/smooth_contact/distance/edge_edge.hpp b/src/ipc/smooth_contact/distance/edge_edge.hpp new file mode 100644 index 000000000..4162111f9 --- /dev/null +++ b/src/ipc/smooth_contact/distance/edge_edge.hpp @@ -0,0 +1,95 @@ +#pragma once + +#include "point_edge.hpp" + +namespace ipc { + +template +T line_line_sqr_distance( + Eigen::ConstRef> ea0, + Eigen::ConstRef> ea1, + Eigen::ConstRef> eb0, + Eigen::ConstRef> eb1); + +template +T edge_edge_sqr_distance( + Eigen::ConstRef> ea0, + Eigen::ConstRef> ea1, + Eigen::ConstRef> eb0, + Eigen::ConstRef> eb1, + EdgeEdgeDistanceType dtype); + +template +Eigen::Vector3 line_line_closest_point_direction( + Eigen::ConstRef> ea0, + Eigen::ConstRef> ea1, + Eigen::ConstRef> eb0, + Eigen::ConstRef> eb1); + +std::tuple> +line_line_closest_point_direction_gradient( + Eigen::ConstRef ea0, + Eigen::ConstRef ea1, + Eigen::ConstRef eb0, + Eigen::ConstRef eb1); + +std::tuple< + Eigen::Vector3d, + Eigen::Matrix, + std::array> +line_line_closest_point_direction_hessian( + Eigen::ConstRef ea0, + Eigen::ConstRef ea1, + Eigen::ConstRef eb0, + Eigen::ConstRef eb1); + +template +Eigen::Matrix line_line_closest_point_pairs( + Eigen::ConstRef> ea0, + Eigen::ConstRef> ea1, + Eigen::ConstRef> eb0, + Eigen::ConstRef> eb1); + +std::tuple> +line_line_closest_point_pairs_gradient( + Eigen::ConstRef ea0, + Eigen::ConstRef ea1, + Eigen::ConstRef eb0, + Eigen::ConstRef eb1); + +std::tuple, std::array> +line_line_closest_point_pairs_hessian( + Eigen::ConstRef ea0, + Eigen::ConstRef ea1, + Eigen::ConstRef eb0, + Eigen::ConstRef eb1); + +/// @brief Computes the direction of the closest point pair +/// @param ea0 Vertex 0 of edge 0 +/// @param ea1 Vertex 1 of edge 0 +/// @param eb0 Vertex 0 of edge 1 +/// @param eb1 Vertex 1 of edge 1 +/// @param dtype Edge-edge distance type +/// @return Difference of the pair of closest point, pointing from edge 0 to edge 1 +template +Eigen::Vector3 edge_edge_closest_point_direction( + Eigen::ConstRef> ea0, + Eigen::ConstRef> ea1, + Eigen::ConstRef> eb0, + Eigen::ConstRef> eb1, + EdgeEdgeDistanceType dtype); + +/// @brief Computes the position of two closest points on two edges +/// @param ea0 Vertex 0 of edge 0 +/// @param ea1 Vertex 1 of edge 0 +/// @param eb0 Vertex 0 of edge 1 +/// @param eb1 Vertex 1 of edge 1 +/// @param dtype Edge-edge distance type +template +Eigen::Matrix edge_edge_closest_point_pairs( + Eigen::ConstRef> ea0, + Eigen::ConstRef> ea1, + Eigen::ConstRef> eb0, + Eigen::ConstRef> eb1, + EdgeEdgeDistanceType dtype); +} // namespace ipc diff --git a/src/ipc/smooth_contact/distance/mollifier.cpp b/src/ipc/smooth_contact/distance/mollifier.cpp new file mode 100644 index 000000000..9ad370c1d --- /dev/null +++ b/src/ipc/smooth_contact/distance/mollifier.cpp @@ -0,0 +1,446 @@ +#include +#ifndef IPC_TOOLKIT_DEBUG_AUTODIFF + +#include "mollifier.hpp" + +#include +#include +#include +#include + +namespace ipc { +namespace { + // double func_aux(const double a, const double b, const double c, const + // double& eps) + // { + // return Math::mollifier((a - c) / c / eps); + // } + + Vector get_indices(int i, int j, int k) + { + Vector out; + out << 3 * i + 0, 3 * i + 1, 3 * i + 2, 3 * j + 0, 3 * j + 1, 3 * j + 2, + 3 * k + 0, 3 * k + 1, 3 * k + 2; + return out; + } + + GradType<3> func_aux_grad( + const double a, const double b, const double c, const double eps) + { + const double val = (a - c) / c / eps; + return std::make_tuple( + Math::mollifier(val), + Eigen::Vector3d(1. / c, 0., -a / c / c) + * Math::mollifier_grad(val) / eps); + } + + HessianType<3> func_aux_hess( + const double a, const double b, const double c, const double eps) + { + const double c2 = 1. / c / c; + Eigen::Matrix3d h1; + h1 << 0., 0., -c2, 0., 0., 0., -c2, 0., 2 * a * c2 / c; + Eigen::Vector3d g1; + g1 << 1. / c, 0., -a * c2; + + const double val = (a - c) / c; + const double g2 = Math::mollifier_grad(val / eps) / eps; + const double h2 = Math::mollifier_hess(val / eps) / eps / eps; + + return std::make_tuple( + Math::mollifier(val / eps), g1 * g2, + h1 * g2 + g1 * h2 * g1.transpose()); + } +} // namespace + +/// @brief Compute the gradient of the mollifier function wrt. 4 edge points and the distance squared +GradType<13> edge_edge_mollifier_gradient( + Eigen::ConstRef ea0, + Eigen::ConstRef ea1, + Eigen::ConstRef eb0, + Eigen::ConstRef eb1, + const std::array& mtypes, + const double dist_sqr) +{ + Vector input; + input << ea0, ea1, eb0, eb1, dist_sqr; + + Eigen::Matrix vert_indices; + vert_indices << 0, 2, 3, 1, 2, 3, 2, 0, 1, 3, 0, 1; + + Eigen::Matrix dof_indices; + for (int i = 0; i < 4; i++) { + dof_indices.row(i) = + get_indices( + vert_indices(i, 0), vert_indices(i, 1), vert_indices(i, 2)) + .transpose(); + } + + // derivatives of point edge distances + Eigen::Vector4d point_edge_dists; + Eigen::Matrix point_edge_dists_grad; + point_edge_dists_grad.setZero(); + for (int i = 0; i < 4; i++) { + point_edge_dists(i) = point_edge_distance( + input.segment<3>(3 * vert_indices(i, 0)), + input.segment<3>(3 * vert_indices(i, 1)), + input.segment<3>(3 * vert_indices(i, 2))); + point_edge_dists_grad(i, dof_indices.row(i)) = + point_edge_distance_gradient( + input.segment<3>(3 * vert_indices(i, 0)), + input.segment<3>(3 * vert_indices(i, 1)), + input.segment<3>(3 * vert_indices(i, 2))); + } + + // derivatives of edge lengths + Eigen::Vector2d edge_lengths; + Eigen::Matrix edge_lengths_grad; + edge_lengths_grad.setZero(); + for (int i = 0; i < 2; i++) { + Eigen::Vector3d edge = + input.segment<3>(i * 6) - input.segment<3>(i * 6 + 3); + edge_lengths(i) = edge.squaredNorm(); + edge_lengths_grad.block<1, 3>(i, 0 + 6 * i) = 2 * edge; + edge_lengths_grad.block<1, 3>(i, 3 + 6 * i) = -2 * edge; + } + + // derivatives of mollifier and ratios, input order : [dist_sqr, + // edge_lengths (1 x 2), point_edge_dists (1 x 4)] + Eigen::Vector4d mollifier; + Eigen::Matrix mollifier_grad; + mollifier_grad.setZero(); + for (int i = 0; i < 4; i++) { + const double len = edge_lengths(1 - i / 2); + const auto [val2, g2] = func_aux_grad( + point_edge_dists(i), len, input(12), MOLLIFIER_THRESHOLD_EPS); + + Eigen::Vector3i ind; + ind << 3 + i, 2 - i / 2, 0; + mollifier(i) = val2; + mollifier_grad(i, ind) = g2; + } + + // partial products of mollifiers + Eigen::Vector4d partial_products_1; + partial_products_1.setOnes(); + for (int i = 0; i < 4; i++) { + for (int j = 0; j < 4; j++) { + if (i != j) { + partial_products_1(j) *= mollifier(i); + } + } + } + + // derivatives of mollifier products, input order : [dist_sqr, edge_lengths + // (1 x 2), point_edge_dists (1 x 4)] + Vector product_grad = + mollifier_grad.transpose() * partial_products_1; + + // derivatives wrt. input : [ea0, ea1, eb0, eb1, dist_sqr] + Vector grad; + grad << edge_lengths_grad.transpose() * product_grad.segment<2>(1) + + point_edge_dists_grad.transpose() * product_grad.segment<4>(3), + product_grad(0); + + return std::make_tuple(mollifier.prod(), grad); +} + +/// @brief Compute the hessian of the mollifier function wrt. 4 edge points and the distance squared +HessianType<13> edge_edge_mollifier_hessian( + Eigen::ConstRef ea0, + Eigen::ConstRef ea1, + Eigen::ConstRef eb0, + Eigen::ConstRef eb1, + const std::array& mtypes, + const double dist_sqr) +{ + Vector input; + input << ea0, ea1, eb0, eb1, dist_sqr; + + Eigen::Matrix vert_indices; + vert_indices << 0, 2, 3, 1, 2, 3, 2, 0, 1, 3, 0, 1; + + Eigen::Matrix dof_indices; + for (int i = 0; i < 4; i++) { + dof_indices.row(i) = + get_indices( + vert_indices(i, 0), vert_indices(i, 1), vert_indices(i, 2)) + .transpose(); + } + + // derivatives of point edge distances + Eigen::Vector4d point_edge_dists; + Eigen::Matrix point_edge_dists_grad; + point_edge_dists_grad.setZero(); + std::array, 4> point_edge_dists_hess; + for (auto& mat : point_edge_dists_hess) { + mat.setZero(); + } + for (int i = 0; i < 4; i++) { + point_edge_dists(i) = point_edge_distance( + input.segment<3>(3 * vert_indices(i, 0)), + input.segment<3>(3 * vert_indices(i, 1)), + input.segment<3>(3 * vert_indices(i, 2))); + point_edge_dists_grad(i, dof_indices.row(i)) = + point_edge_distance_gradient( + input.segment<3>(3 * vert_indices(i, 0)), + input.segment<3>(3 * vert_indices(i, 1)), + input.segment<3>(3 * vert_indices(i, 2))); + point_edge_dists_hess[i](dof_indices.row(i), dof_indices.row(i)) = + point_edge_distance_hessian( + input.segment<3>(3 * vert_indices(i, 0)), + input.segment<3>(3 * vert_indices(i, 1)), + input.segment<3>(3 * vert_indices(i, 2))); + } + + // derivatives of edge lengths + Eigen::Vector2d edge_lengths; + Eigen::Matrix edge_lengths_grad; + edge_lengths_grad.setZero(); + for (int i = 0; i < 2; i++) { + const Eigen::Vector3d edge = + input.segment<3>(i * 6) - input.segment<3>(i * 6 + 3); + edge_lengths(i) = edge.squaredNorm(); + edge_lengths_grad.block<1, 3>(i, 0 + 6 * i) = 2 * edge; + edge_lengths_grad.block<1, 3>(i, 3 + 6 * i) = -2 * edge; + } + + // derivatives of mollifier and ratios, input order : [dist_sqr, + // edge_lengths (1 x 2), point_edge_dists (1 x 4)] + Eigen::Vector4d mollifier; + Eigen::Matrix mollifier_grad; + mollifier_grad.setZero(); + std::array, 4> mollifier_hess; + for (auto& mat : mollifier_hess) { + mat.setZero(); + } + for (int i = 0; i < 4; i++) { + const double len = edge_lengths(1 - i / 2); + const auto [val, g, h] = func_aux_hess( + point_edge_dists(i), len, input(12), MOLLIFIER_THRESHOLD_EPS); + + Eigen::Vector3i ind; + ind << 3 + i, 2 - i / 2, 0; + mollifier(i) = val; + mollifier_grad(i, ind) = g; + mollifier_hess[i](ind, ind) = h; + } + + // partial products of mollifiers + Eigen::Vector4d partial_products_1; + partial_products_1.setOnes(); + Eigen::Matrix partial_products_2; + partial_products_2.setOnes(); + for (int i = 0; i < 4; i++) { + for (int j = 0; j < 4; j++) { + if (i != j) { + partial_products_1(j) *= mollifier(i); + + for (int k = 0; k < 4; k++) { + if (i != k) { + partial_products_2(j, k) *= mollifier(i); + } + } + } + } + } + partial_products_2.diagonal().setZero(); + + // derivatives of mollifier products, input order : [dist_sqr, edge_lengths + // (1 x 2), point_edge_dists (1 x 4)] + Vector product_grad = + mollifier_grad.transpose() * partial_products_1; + Eigen::Matrix product_hess = + mollifier_grad.transpose() * partial_products_2 * mollifier_grad; + for (int i = 0; i < 4; i++) { + product_hess += mollifier_hess[i] * partial_products_1(i); + } + + // derivatives wrt. input : [ea0, ea1, eb0, eb1, dist_sqr] + Vector grad = Vector::Zero(); + Eigen::Matrix hess = Eigen::Matrix::Zero(); + { + Eigen::Matrix grads; + grads << edge_lengths_grad, point_edge_dists_grad; + + grad(12) = product_grad(0); + grad.head<12>() = grads.transpose() * product_grad.segment<6>(1); + + hess(12, 12) = product_hess(0, 0); + hess.block<1, 12>(12, 0) += product_hess.block<1, 6>(0, 1) * grads; + hess.block<12, 1>(0, 12) += + grads.transpose() * product_hess.block<6, 1>(1, 0); + + for (int j = 0; j < 2; j++) { + const double val = 2 * product_grad(1 + j); + hess.block<6, 6>(j * 6, j * 6).diagonal().array() += val; + hess.block<3, 3>(j * 6, j * 6 + 3).diagonal().array() -= val; + hess.block<3, 3>(j * 6 + 3, j * 6).diagonal().array() -= val; + } + for (int i = 0; i < 4; i++) { + hess.topLeftCorner<12, 12>() += + product_grad(3 + i) * point_edge_dists_hess[i]; + } + + hess.topLeftCorner<12, 12>() += + grads.transpose() * product_hess.block<6, 6>(1, 1) * grads; + } + return std::make_tuple(mollifier.prod(), grad, hess); +} + +std::array edge_edge_mollifier_type( + Eigen::ConstRef ea0, + Eigen::ConstRef ea1, + Eigen::ConstRef eb0, + Eigen::ConstRef eb1, + const double dist_sqr) +{ + std::array mtypes {}; + mtypes[0] = (point_edge_distance(ea0, eb0, eb1) - dist_sqr) + >= dist_sqr * MOLLIFIER_THRESHOLD_EPS + ? HeavisideType::ONE + : HeavisideType::VARIANT; + mtypes[1] = (point_edge_distance(ea1, eb0, eb1) - dist_sqr) + >= dist_sqr * MOLLIFIER_THRESHOLD_EPS + ? HeavisideType::ONE + : HeavisideType::VARIANT; + mtypes[2] = (point_edge_distance(eb0, ea0, ea1) - dist_sqr) + >= dist_sqr * MOLLIFIER_THRESHOLD_EPS + ? HeavisideType::ONE + : HeavisideType::VARIANT; + mtypes[3] = (point_edge_distance(eb1, ea0, ea1) - dist_sqr) + >= dist_sqr * MOLLIFIER_THRESHOLD_EPS + ? HeavisideType::ONE + : HeavisideType::VARIANT; + return mtypes; +} + +/// @brief Compute the gradient of the mollifier function wrt. 4 edge points and the distance squared +GradType<13> point_face_mollifier_gradient( + Eigen::ConstRef p, + Eigen::ConstRef e0, + Eigen::ConstRef e1, + Eigen::ConstRef e2, + const double dist_sqr) +{ + Vector12d x; + x << p, e0, e1, e2; + + Eigen::Vector3d vals; + Eigen::Matrix grads; + for (int i = 0; i < 3; i++) { + const int ei = i * 3 + 3; + const int ej = ((i + 1) % 3) * 3 + 3; + const int ek = ((i + 2) % 3) * 3 + 3; + + Vector ind; + + Eigen::Matrix dist_grad = + Eigen::Matrix::Zero(); + + const double point_edge_dist = + point_line_distance(p, x.segment<3>(ei), x.segment<3>(ej)); + ind << 0, 1, 2, ei, ei + 1, ei + 2, ej, ej + 1, ej + 2; + dist_grad(0, ind) = + point_line_distance_gradient(p, x.segment<3>(ei), x.segment<3>(ej)); + + const double vert_edge_dist = point_line_distance( + x.segment<3>(ek), x.segment<3>(ei), x.segment<3>(ej)); + ind << ek, ek + 1, ek + 2, ei, ei + 1, ei + 2, ej, ej + 1, ej + 2; + dist_grad(1, ind) = point_line_distance_gradient( + x.segment<3>(ek), x.segment<3>(ei), x.segment<3>(ej)); + + Eigen::Vector3d tmp_grad; + std::tie(vals(i), tmp_grad) = func_aux_grad( + point_edge_dist, vert_edge_dist, dist_sqr, MOLLIFIER_THRESHOLD_EPS); + + grads.row(i) = tmp_grad.head<2>().transpose() * dist_grad; + grads(i, 12) += tmp_grad(2); + } + + Vector grad = (vals(0) * vals(1)) * grads.row(2) + + (vals(0) * vals(2)) * grads.row(1) + + (vals(1) * vals(2)) * grads.row(0); + + return std::make_tuple(vals.prod(), grad); +} + +/// @brief Compute the hessian of the mollifier function wrt. 4 edge points and the distance squared +HessianType<13> point_face_mollifier_hessian( + Eigen::ConstRef p, + Eigen::ConstRef e0, + Eigen::ConstRef e1, + Eigen::ConstRef e2, + const double dist_sqr) +{ + Vector12d x; + x << p, e0, e1, e2; + + Eigen::Vector3d vals; + Eigen::Matrix grads; + std::array, 3> hesses; + for (int i = 0; i < 3; i++) { + const int ei = i * 3 + 3; + const int ej = ((i + 1) % 3) * 3 + 3; + const int ek = ((i + 2) % 3) * 3 + 3; + + Vector ind; + + Eigen::Matrix dist_grad = + Eigen::Matrix::Zero(); + Eigen::Matrix point_edge_dist_hess = + Eigen::Matrix::Zero(), + vert_edge_dist_hess = + Eigen::Matrix::Zero(); + + const double point_edge_dist = + point_line_distance(p, x.segment<3>(ei), x.segment<3>(ej)); + ind << 0, 1, 2, ei, ei + 1, ei + 2, ej, ej + 1, ej + 2; + dist_grad(0, ind) = + point_line_distance_gradient(p, x.segment<3>(ei), x.segment<3>(ej)); + point_edge_dist_hess(ind, ind) = + point_line_distance_hessian(p, x.segment<3>(ei), x.segment<3>(ej)); + + const double vert_edge_dist = point_line_distance( + x.segment<3>(ek), x.segment<3>(ei), x.segment<3>(ej)); + ind << ek, ek + 1, ek + 2, ei, ei + 1, ei + 2, ej, ej + 1, ej + 2; + dist_grad(1, ind) = point_line_distance_gradient( + x.segment<3>(ek), x.segment<3>(ei), x.segment<3>(ej)); + vert_edge_dist_hess(ind, ind) = point_line_distance_hessian( + x.segment<3>(ek), x.segment<3>(ei), x.segment<3>(ej)); + + Eigen::Vector3d tmp_grad; + Eigen::Matrix3d tmp_hess; + std::tie(vals(i), tmp_grad, tmp_hess) = func_aux_hess( + point_edge_dist, vert_edge_dist, dist_sqr, MOLLIFIER_THRESHOLD_EPS); + + grads.row(i) = tmp_grad.head<2>().transpose() * dist_grad; + grads(i, 12) += tmp_grad(2); + + hesses[i] = tmp_grad(0) * point_edge_dist_hess + + tmp_grad(1) * vert_edge_dist_hess; + hesses[i] += + dist_grad.transpose() * tmp_hess.topLeftCorner<2, 2>() * dist_grad; + hesses[i](12, 12) += tmp_hess(2, 2); + hesses[i].col(12) += dist_grad.transpose() * tmp_hess.block<2, 1>(0, 2); + hesses[i].row(12) += tmp_hess.block<1, 2>(2, 0) * dist_grad; + } + + Vector grad = (vals(0) * vals(1)) * grads.row(2) + + (vals(0) * vals(2)) * grads.row(1) + + (vals(1) * vals(2)) * grads.row(0); + Eigen::Matrix hess = (vals(0) * vals(1)) * hesses[2] + + (vals(0) * vals(2)) * hesses[1] + (vals(1) * vals(2)) * hesses[0] + + grads.row(0).transpose() * vals(2) * grads.row(1) + + grads.row(1).transpose() * vals(2) * grads.row(0) + + grads.row(0).transpose() * vals(1) * grads.row(2) + + grads.row(2).transpose() * vals(1) * grads.row(0) + + grads.row(1).transpose() * vals(0) * grads.row(2) + + grads.row(2).transpose() * vals(0) * grads.row(1); + + return std::make_tuple(vals.prod(), grad, hess); +} + +} // namespace ipc + +#endif \ No newline at end of file diff --git a/src/ipc/smooth_contact/distance/mollifier.hpp b/src/ipc/smooth_contact/distance/mollifier.hpp new file mode 100644 index 000000000..662d1cfea --- /dev/null +++ b/src/ipc/smooth_contact/distance/mollifier.hpp @@ -0,0 +1,73 @@ +#pragma once +#include "edge_edge.hpp" +#include "point_edge.hpp" +#include "point_face.hpp" + +namespace ipc { +template +scalar point_edge_mollifier( + Eigen::ConstRef> p, + Eigen::ConstRef> e0, + Eigen::ConstRef> e1, + const scalar& dist_sqr); + +std::array edge_edge_mollifier_type( + Eigen::ConstRef ea0, + Eigen::ConstRef ea1, + Eigen::ConstRef eb0, + Eigen::ConstRef eb1, + const double dist_sqr); + +template +scalar edge_edge_mollifier( + Eigen::ConstRef> ea0, + Eigen::ConstRef> ea1, + Eigen::ConstRef> eb0, + Eigen::ConstRef> eb1, + const std::array& mtypes, + const scalar& dist_sqr); + +/// @brief Compute the gradient of the mollifier function wrt. 4 edge points and the distance squared +GradType<13> edge_edge_mollifier_gradient( + Eigen::ConstRef ea0, + Eigen::ConstRef ea1, + Eigen::ConstRef eb0, + Eigen::ConstRef eb1, + const std::array& mtypes, + const double dist_sqr); + +/// @brief Compute the hessian of the mollifier function wrt. 4 edge points and the distance squared +HessianType<13> edge_edge_mollifier_hessian( + Eigen::ConstRef ea0, + Eigen::ConstRef ea1, + Eigen::ConstRef eb0, + Eigen::ConstRef eb1, + const std::array& mtypes, + const double dist_sqr); + +template +scalar point_face_mollifier( + Eigen::ConstRef> p, + Eigen::ConstRef> e0, + Eigen::ConstRef> e1, + Eigen::ConstRef> e2, + const scalar& dist_sqr); + +/// @brief Compute the gradient of the mollifier function wrt. 4 edge points and the distance squared +GradType<13> point_face_mollifier_gradient( + Eigen::ConstRef p, + Eigen::ConstRef e0, + Eigen::ConstRef e1, + Eigen::ConstRef e2, + const double dist_sqr); + +/// @brief Compute the hessian of the mollifier function wrt. 4 edge points and the distance squared +HessianType<13> point_face_mollifier_hessian( + Eigen::ConstRef p, + Eigen::ConstRef e0, + Eigen::ConstRef e1, + Eigen::ConstRef e2, + const double dist_sqr); +} // namespace ipc + +#include "mollifier.tpp" diff --git a/src/ipc/smooth_contact/distance/mollifier.tpp b/src/ipc/smooth_contact/distance/mollifier.tpp new file mode 100644 index 000000000..9718c8d72 --- /dev/null +++ b/src/ipc/smooth_contact/distance/mollifier.tpp @@ -0,0 +1,88 @@ +#pragma once + +#include "mollifier.hpp" + +namespace ipc { +template +scalar point_edge_mollifier( + Eigen::ConstRef> p, + Eigen::ConstRef> e0, + Eigen::ConstRef> e1, + const scalar& dist_sqr) +{ + const scalar denominator = dist_sqr * MOLLIFIER_THRESHOLD_EPS; + return Math::mollifier( + ((p - e0).squaredNorm() - dist_sqr) / denominator) + * Math::mollifier( + ((p - e1).squaredNorm() - dist_sqr) / denominator); +} + +template +scalar edge_edge_mollifier( + Eigen::ConstRef> ea0, + Eigen::ConstRef> ea1, + Eigen::ConstRef> eb0, + Eigen::ConstRef> eb1, + const std::array& mtypes, + const scalar& dist_sqr) +{ + const scalar da = dist_sqr * MOLLIFIER_THRESHOLD_EPS; + const scalar db = dist_sqr * MOLLIFIER_THRESHOLD_EPS; + scalar a = (mtypes[0] == HeavisideType::VARIANT) + ? Math::mollifier( + (PointEdgeDistance::point_edge_sqr_distance( + ea0, eb0, eb1) + - dist_sqr) + / db) + : scalar(1.); + scalar b = (mtypes[1] == HeavisideType::VARIANT) + ? Math::mollifier( + (PointEdgeDistance::point_edge_sqr_distance( + ea1, eb0, eb1) + - dist_sqr) + / db) + : scalar(1.); + scalar c = (mtypes[2] == HeavisideType::VARIANT) + ? Math::mollifier( + (PointEdgeDistance::point_edge_sqr_distance( + eb0, ea0, ea1) + - dist_sqr) + / da) + : scalar(1.); + scalar d = (mtypes[3] == HeavisideType::VARIANT) + ? Math::mollifier( + (PointEdgeDistance::point_edge_sqr_distance( + eb1, ea0, ea1) + - dist_sqr) + / da) + : scalar(1.); + + return a * b * c * d; +} + +template +scalar point_face_mollifier( + Eigen::ConstRef> p, + Eigen::ConstRef> e0, + Eigen::ConstRef> e1, + Eigen::ConstRef> e2, + const scalar& dist_sqr) +{ + // use point-line distance instead of point-edge distance because + // this function vanishes if the point is outside the triangle, so + // whenever this function is nonzero the point-edge distance equals + // the point-line distance + return Math::mollifier( + (PointEdgeDistance::point_line_sqr_distance(p, e0, e1) + - dist_sqr) + / MOLLIFIER_THRESHOLD_EPS / dist_sqr) + * Math::mollifier( + (PointEdgeDistance::point_line_sqr_distance(p, e2, e1) + - dist_sqr) + / MOLLIFIER_THRESHOLD_EPS / dist_sqr) + * Math::mollifier( + (PointEdgeDistance::point_line_sqr_distance(p, e0, e2) + - dist_sqr) + / MOLLIFIER_THRESHOLD_EPS / dist_sqr); +} +} // namespace ipc \ No newline at end of file diff --git a/src/ipc/smooth_contact/distance/point_edge.cpp b/src/ipc/smooth_contact/distance/point_edge.cpp new file mode 100644 index 000000000..303c98d53 --- /dev/null +++ b/src/ipc/smooth_contact/distance/point_edge.cpp @@ -0,0 +1,330 @@ +#include "point_edge.hpp" + +#include +#include + +namespace ipc { +template +scalar PointEdgeDistance::point_point_sqr_distance( + Eigen::ConstRef> a, + Eigen::ConstRef> b) +{ + return (a - b).squaredNorm(); +} + +template +scalar PointEdgeDistance::point_line_sqr_distance( + Eigen::ConstRef> p, + Eigen::ConstRef> e0, + Eigen::ConstRef> e1) +{ + if constexpr (dim == 2) { + return Math::sqr(Math::cross2(e0 - p, e1 - p)) + / (e1 - e0).squaredNorm(); + } else { + return (e0 - p).cross(e1 - p).squaredNorm() / (e1 - e0).squaredNorm(); + } +} + +template +scalar PointEdgeDistance::point_edge_sqr_distance( + Eigen::ConstRef> p, + Eigen::ConstRef> e0, + Eigen::ConstRef> e1, + const PointEdgeDistanceType dtype) +{ + switch (dtype) { + case PointEdgeDistanceType::P_E: + return point_line_sqr_distance(p, e0, e1); + case PointEdgeDistanceType::P_E0: + return point_point_sqr_distance(p, e0); + case PointEdgeDistanceType::P_E1: + return point_point_sqr_distance(p, e1); + case PointEdgeDistanceType::AUTO: + default: + const Vector t = e1 - e0; + const Vector pos = p - e0; + const scalar s = pos.dot(t) / t.squaredNorm(); + return (pos - Math::l_ns(s) * t).squaredNorm(); + } +} + +template +Vector +PointEdgeDistance::point_line_closest_point_direction( + Eigen::ConstRef> p, + Eigen::ConstRef> e0, + Eigen::ConstRef> e1) +{ + const Vector d = p - e0; + const Vector t = e1 - e0; + return d - (d.dot(t) / t.squaredNorm()) * t; +} + +template +Vector +PointEdgeDistance::point_edge_closest_point_direction( + Eigen::ConstRef> p, + Eigen::ConstRef> e0, + Eigen::ConstRef> e1, + const PointEdgeDistanceType dtype) +{ + switch (dtype) { + case PointEdgeDistanceType::P_E: + return point_line_closest_point_direction(p, e0, e1); + case PointEdgeDistanceType::P_E0: + return p - e0; + case PointEdgeDistanceType::P_E1: + return p - e1; + case PointEdgeDistanceType::AUTO: + default: + Vector t = e1 - e0; + const Vector pos = p - e0; + const scalar s = pos.dot(t) / t.squaredNorm(); + return pos - Math::l_ns(s) * t; + } +} + +template +std::tuple, Eigen::Matrix> +PointEdgeDistanceDerivatives::point_line_closest_point_direction_grad( + Eigen::ConstRef> p, + Eigen::ConstRef> e0, + Eigen::ConstRef> e1) +{ + Vector val; + Eigen::Matrix grad; +#ifdef IPC_TOOLKIT_DEBUG_AUTODIFF + using T = ADGrad<3 * dim>; + ScalarBase::setVariableCount(3 * dim); + const Vector pT = slice_positions(p); + const Vector e0T = slice_positions(e0, dim); + const Vector e1T = slice_positions(e1, 2 * dim); + + const Vector out = + PointEdgeDistance::point_line_closest_point_direction( + pT, e0T, e1T); + for (int i = 0; i < dim; i++) { + val(i) = out(i).val; + grad.row(i) = out(i).grad; + } +#else + const double uv = point_edge_closest_point(p, e0, e1); + const Vector g = + point_edge_closest_point_jacobian(p, e0, e1); + + val = (p - e0) - uv * (e1 - e0); + + grad.setZero(); + grad -= (e1 - e0) * g.transpose(); + grad.block(0, 0, dim, dim).diagonal().array() += 1.; + grad.block(0, dim, dim, dim).diagonal().array() += -1. + uv; + grad.block(0, 2 * dim, dim, dim).diagonal().array() -= uv; +#endif + + return std::make_tuple(val, grad); +} + +template +std::tuple< + Vector, + Eigen::Matrix, + std::array, dim>> +PointEdgeDistanceDerivatives::point_line_closest_point_direction_hessian( + Eigen::ConstRef> p, + Eigen::ConstRef> e0, + Eigen::ConstRef> e1) +{ + Vector val; + Eigen::Matrix grad; + std::array, dim> hess; + +#ifdef IPC_TOOLKIT_DEBUG_AUTODIFF + using T = ADHessian<3 * dim>; + ScalarBase::setVariableCount(3 * dim); + Vector pT = slice_positions(p); + Vector e0T = slice_positions(e0, dim); + Vector e1T = slice_positions(e1, 2 * dim); + + Vector out = + PointEdgeDistance::point_line_closest_point_direction( + pT, e0T, e1T); + for (int i = 0; i < dim; i++) { + val(i) = out(i).val; + grad.row(i) = out(i).grad; + hess[i] = out(i).Hess; + } +#else + const double uv = point_edge_closest_point(p, e0, e1); + const Vector g = + point_edge_closest_point_jacobian(p, e0, e1); + const Eigen::Matrix h = + point_edge_closest_point_hessian(p, e0, e1); + + val = (p - e0) - uv * (e1 - e0); + + grad.setZero(); + grad -= (e1 - e0) * g.transpose(); + grad.block(0, 0, dim, dim).diagonal().array() += 1.; + grad.block(0, dim, dim, dim).diagonal().array() += -1. + uv; + grad.block(0, 2 * dim, dim, dim).diagonal().array() -= uv; + + for (auto& hi : hess) { + hi.setZero(); + } + for (int d = 0; d < dim; d++) { + // wrt. uv + hess[d] -= (e1(d) - e0(d)) * h; + + // wrt. uv & e0 + hess[d].row(dim + d) += g.transpose(); + hess[d].col(dim + d) += g; + + // wrt. uv & e1 + hess[d].row(2 * dim + d) -= g.transpose(); + hess[d].col(2 * dim + d) -= g; + } +#endif + + return std::make_tuple(val, grad, hess); +} + +template +std::tuple, Eigen::Matrix> +PointEdgeDistanceDerivatives::point_edge_closest_point_direction_grad( + Eigen::ConstRef> p, + Eigen::ConstRef> e0, + Eigen::ConstRef> e1, + const PointEdgeDistanceType dtype) +{ + Vector vec; + Eigen::Matrix grad = + Eigen::Matrix::Zero(); +#ifdef IPC_TOOLKIT_DEBUG_AUTODIFF + using T = ADGrad<3 * dim>; + ScalarBase::setVariableCount(3 * dim); + Vector pT = slice_positions(p); + Vector e0T = slice_positions(e0, dim); + Vector e1T = slice_positions(e1, 2 * dim); + + Vector out = + PointEdgeDistance::point_edge_closest_point_direction( + pT, e0T, e1T, dtype); + for (int i = 0; i < dim; i++) { + vec(i) = out(i).val; + grad.row(i) = out(i).grad; + } +#else + switch (dtype) { + case PointEdgeDistanceType::P_E: + std::tie(vec, grad) = + point_line_closest_point_direction_grad(p, e0, e1); + break; + case PointEdgeDistanceType::P_E0: + vec = p - e0; + grad.leftCols(dim).diagonal().array() = 1.; + grad.middleCols(dim, dim).diagonal().array() = -1.; + break; + case PointEdgeDistanceType::P_E1: + vec = p - e1; + grad.leftCols(dim).diagonal().array() = 1.; + grad.middleCols(2 * dim, dim).diagonal().array() = -1.; + break; + default: + throw std::runtime_error("PointEdgeDistanceType not specified!"); + } +#endif + + return { vec, grad }; +} + +template +std::tuple< + Vector, + Eigen::Matrix, + std::array, dim>> +PointEdgeDistanceDerivatives::point_edge_closest_point_direction_hessian( + Eigen::ConstRef> p, + Eigen::ConstRef> e0, + Eigen::ConstRef> e1, + const PointEdgeDistanceType dtype) +{ + Vector vec; + Eigen::Matrix grad = + Eigen::Matrix::Zero(); + std::array, dim> hess; + for (auto& hi : hess) { + hi.setZero(); + } +#ifdef IPC_TOOLKIT_DEBUG_AUTODIFF + using T = ADHessian<3 * dim>; + ScalarBase::setVariableCount(3 * dim); + Vector pT = slice_positions(p); + Vector e0T = slice_positions(e0, dim); + Vector e1T = slice_positions(e1, 2 * dim); + + Vector out = + PointEdgeDistance::point_edge_closest_point_direction( + pT, e0T, e1T); + + for (int i = 0; i < dim; i++) { + vec(i) = out(i).val; + grad.row(i) = out(i).grad; + hess[i] = out(i).Hess; + } +#else + switch (dtype) { + case PointEdgeDistanceType::P_E: + std::tie(vec, grad, hess) = + point_line_closest_point_direction_hessian(p, e0, e1); + break; + case PointEdgeDistanceType::P_E0: + vec = p - e0; + grad.leftCols(dim).diagonal().array() = 1.; + grad.middleCols(dim, dim).diagonal().array() = -1.; + break; + case PointEdgeDistanceType::P_E1: + vec = p - e1; + grad.leftCols(dim).diagonal().array() = 1.; + grad.middleCols(2 * dim, dim).diagonal().array() = -1.; + break; + default: + throw std::runtime_error("PointEdgeDistanceType not specified!"); + } +#endif + + return { vec, grad, hess }; +} + +template class PointEdgeDistance; +template class PointEdgeDistance; + +template class PointEdgeDistance, 2>; +template class PointEdgeDistance, 2>; + +template class PointEdgeDistance, 2>; +template class PointEdgeDistance, 2>; + +template class PointEdgeDistance, 2>; +template class PointEdgeDistance, 2>; + +template class PointEdgeDistance, 3>; +template class PointEdgeDistance, 3>; + +template class PointEdgeDistance, 3>; +template class PointEdgeDistance, 3>; + +template class PointEdgeDistance, 3>; +template class PointEdgeDistance, 3>; + +#ifdef IPC_TOOLKIT_DEBUG_AUTODIFF +template class PointEdgeDistance, 3>; +template class PointEdgeDistance, 3>; + +template class PointEdgeDistance, 3>; +template class PointEdgeDistance, 3>; +#endif + +template class PointEdgeDistanceDerivatives<2>; +template class PointEdgeDistanceDerivatives<3>; +} // namespace ipc diff --git a/src/ipc/smooth_contact/distance/point_edge.hpp b/src/ipc/smooth_contact/distance/point_edge.hpp new file mode 100644 index 000000000..24181bc7e --- /dev/null +++ b/src/ipc/smooth_contact/distance/point_edge.hpp @@ -0,0 +1,84 @@ +#pragma once + +#include +#include +#include +#include +#include + +#include + +namespace ipc { +template class PointEdgeDistance { +public: + PointEdgeDistance() = delete; + PointEdgeDistance(const PointEdgeDistance&) = delete; + PointEdgeDistance& operator=(const PointEdgeDistance&) = delete; + + static scalar point_point_sqr_distance( + Eigen::ConstRef> a, + Eigen::ConstRef> b); + + static scalar point_line_sqr_distance( + Eigen::ConstRef> p, + Eigen::ConstRef> e0, + Eigen::ConstRef> e1); + + static scalar point_edge_sqr_distance( + Eigen::ConstRef> p, + Eigen::ConstRef> e0, + Eigen::ConstRef> e1, + const PointEdgeDistanceType dtype = PointEdgeDistanceType::AUTO); + + static Vector point_line_closest_point_direction( + Eigen::ConstRef> p, + Eigen::ConstRef> e0, + Eigen::ConstRef> e1); + + static Vector point_edge_closest_point_direction( + Eigen::ConstRef> p, + Eigen::ConstRef> e0, + Eigen::ConstRef> e1, + const PointEdgeDistanceType dtype = PointEdgeDistanceType::AUTO); +}; + +template class PointEdgeDistanceDerivatives { +public: + PointEdgeDistanceDerivatives() = delete; + PointEdgeDistanceDerivatives(const PointEdgeDistanceDerivatives&) = delete; + PointEdgeDistanceDerivatives& + operator=(const PointEdgeDistanceDerivatives&) = delete; + + static std::tuple, Eigen::Matrix> + point_line_closest_point_direction_grad( + Eigen::ConstRef> p, + Eigen::ConstRef> e0, + Eigen::ConstRef> e1); + + static std::tuple< + Vector, + Eigen::Matrix, + std::array, dim>> + point_line_closest_point_direction_hessian( + Eigen::ConstRef> p, + Eigen::ConstRef> e0, + Eigen::ConstRef> e1); + + static std::tuple, Eigen::Matrix> + point_edge_closest_point_direction_grad( + Eigen::ConstRef> p, + Eigen::ConstRef> e0, + Eigen::ConstRef> e1, + const PointEdgeDistanceType dtype = PointEdgeDistanceType::AUTO); + + static std::tuple< + Vector, + Eigen::Matrix, + std::array, dim>> + point_edge_closest_point_direction_hessian( + Eigen::ConstRef> p, + Eigen::ConstRef> e0, + Eigen::ConstRef> e1, + const PointEdgeDistanceType dtype = PointEdgeDistanceType::AUTO); +}; +} // namespace ipc diff --git a/src/ipc/smooth_contact/distance/point_face.cpp b/src/ipc/smooth_contact/distance/point_face.cpp new file mode 100644 index 000000000..eb529cfd6 --- /dev/null +++ b/src/ipc/smooth_contact/distance/point_face.cpp @@ -0,0 +1,429 @@ +#include "point_face.hpp" + +#include "autogen.hpp" +#include "point_edge.hpp" + +#include + +namespace ipc { + +std::tuple> +point_plane_closest_point_direction_grad( + Eigen::ConstRef p, + Eigen::ConstRef t0, + Eigen::ConstRef t1, + Eigen::ConstRef t2) +{ + const Eigen::Vector2d uv = point_triangle_closest_point(p, t0, t1, t2); + const Eigen::Matrix jac = + point_triangle_closest_point_jacobian(p, t0, t1, t2); + const Eigen::Vector3d direc = + p - (t0 * (1 - uv(0) - uv(1)) + t1 * uv(0) + t2 * uv(1)); + Eigen::Matrix grad = + -(t1 - t0) * jac.row(0) - (t2 - t0) * jac.row(1); + grad.middleCols<3>(3).diagonal().array() -= 1. - uv(0) - uv(1); + grad.middleCols<3>(6).diagonal().array() -= uv(0); + grad.middleCols<3>(9).diagonal().array() -= uv(1); + grad.leftCols<3>().diagonal().array() += 1.; + + return { direc, grad }; +} + +std::tuple< + Eigen::Vector3d, + Eigen::Matrix, + std::array> +point_plane_closest_point_direction_hessian( + Eigen::ConstRef p, + Eigen::ConstRef t0, + Eigen::ConstRef t1, + Eigen::ConstRef t2) +{ + // compute derivatives of uv + const Eigen::Vector2d uv = point_triangle_closest_point(p, t0, t1, t2); + const Eigen::Matrix jac = + point_triangle_closest_point_jacobian(p, t0, t1, t2); + std::array H; + autogen::triangle_closest_point_hessian_0( + p(0), p(1), p(2), t0(0), t0(1), t0(2), t1(0), t1(1), t1(2), t2(0), + t2(1), t2(2), H[0].data()); + autogen::triangle_closest_point_hessian_1( + p(0), p(1), p(2), t0(0), t0(1), t0(2), t1(0), t1(1), t1(2), t2(0), + t2(1), t2(2), H[1].data()); + + // compute derivatives of the closest point + const Eigen::Vector3d direc = + p - (t0 * (1 - uv(0) - uv(1)) + t1 * uv(0) + t2 * uv(1)); + + Eigen::Matrix grad = + -(t1 - t0) * jac.row(0) - (t2 - t0) * jac.row(1); + grad.middleCols<3>(3).diagonal().array() -= 1. - uv(0) - uv(1); + grad.middleCols<3>(6).diagonal().array() -= uv(0); + grad.middleCols<3>(9).diagonal().array() -= uv(1); + grad.leftCols<3>().diagonal().array() += 1.; + + std::array hess; + for (auto& h : hess) { + h.setZero(); + } + for (int d = 0; d < 3; d++) { + // wrt. uv + hess[d] -= (t1(d) - t0(d)) * H[0] + (t2(d) - t0(d)) * H[1]; + + // wrt. uv & t0 + hess[d].row(3 + d) += jac.row(0) + jac.row(1); + hess[d].col(3 + d) += (jac.row(0) + jac.row(1)).transpose(); + + // wrt. uv & t1 + hess[d].row(6 + d) -= jac.row(0); + hess[d].col(6 + d) -= jac.row(0).transpose(); + + // wrt. uv & t2 + hess[d].row(9 + d) -= jac.row(1); + hess[d].col(9 + d) -= jac.row(1).transpose(); + } + + return { direc, grad, hess }; +} + +std::tuple> +point_triangle_closest_point_direction_grad( + Eigen::ConstRef p, + Eigen::ConstRef t0, + Eigen::ConstRef t1, + Eigen::ConstRef t2, + const PointTriangleDistanceType dtype) +{ + Eigen::Vector3d pts = Eigen::Vector3d::Zero(); + Eigen::Matrix grad = Eigen::Matrix::Zero(); + switch (dtype) { + case PointTriangleDistanceType::P_T0: + pts = p - t0; + grad.leftCols<3>() = Eigen::Matrix::Identity(); + grad.middleCols<3>(3) = -Eigen::Matrix::Identity(); + break; + + case PointTriangleDistanceType::P_T1: + pts = p - t1; + grad.leftCols<3>() = Eigen::Matrix::Identity(); + grad.middleCols<3>(6) = -Eigen::Matrix::Identity(); + break; + + case PointTriangleDistanceType::P_T2: + pts = p - t2; + grad.leftCols<3>() = Eigen::Matrix::Identity(); + grad.middleCols<3>(9) = -Eigen::Matrix::Identity(); + break; + + case PointTriangleDistanceType::P_E0: { + const auto [pts_, d_grad] = PointEdgeDistanceDerivatives< + 3>::point_line_closest_point_direction_grad(p, t0, t1); + pts = pts_; + grad(Eigen::all, { 0, 1, 2, 3, 4, 5, 6, 7, 8 }) = d_grad; + break; + } + + case PointTriangleDistanceType::P_E1: { + const auto [pts_, d_grad] = PointEdgeDistanceDerivatives< + 3>::point_line_closest_point_direction_grad(p, t1, t2); + pts = pts_; + grad(Eigen::all, { 0, 1, 2, 6, 7, 8, 9, 10, 11 }) = d_grad; + break; + } + + case PointTriangleDistanceType::P_E2: { + const auto [pts_, d_grad] = PointEdgeDistanceDerivatives< + 3>::point_line_closest_point_direction_grad(p, t2, t0); + pts = pts_; + grad(Eigen::all, { 0, 1, 2, 9, 10, 11, 3, 4, 5 }) = d_grad; + break; + } + + case PointTriangleDistanceType::P_T: + std::tie(pts, grad) = + point_plane_closest_point_direction_grad(p, t0, t1, t2); + break; + + default: + throw std::invalid_argument( + "Invalid distance type for point-triangle distance!"); + } + + return { pts, grad }; +} + +std::tuple< + Eigen::Vector3d, + Eigen::Matrix, + std::array> +point_triangle_closest_point_direction_hessian( + Eigen::ConstRef p, + Eigen::ConstRef t0, + Eigen::ConstRef t1, + Eigen::ConstRef t2, + const PointTriangleDistanceType dtype) +{ + Eigen::Vector3d pts = Eigen::Vector3d::Zero(); + Eigen::Matrix grad = Eigen::Matrix::Zero(); + std::array hess; + for (auto& h : hess) { + h.setZero(); + } + switch (dtype) { + case PointTriangleDistanceType::P_T0: + pts = p - t0; + grad.leftCols<3>() = Eigen::Matrix::Identity(); + grad.middleCols<3>(3) = -Eigen::Matrix::Identity(); + break; + + case PointTriangleDistanceType::P_T1: + pts = p - t1; + grad.leftCols<3>() = Eigen::Matrix::Identity(); + grad.middleCols<3>(6) = -Eigen::Matrix::Identity(); + break; + + case PointTriangleDistanceType::P_T2: + pts = p - t2; + grad.leftCols<3>() = Eigen::Matrix::Identity(); + grad.middleCols<3>(9) = -Eigen::Matrix::Identity(); + break; + + case PointTriangleDistanceType::P_E0: { + const auto [d, d_grad, d_hess] = PointEdgeDistanceDerivatives< + 3>::point_line_closest_point_direction_hessian(p, t0, t1); + pts = d; + std::vector ind { 0, 1, 2, 3, 4, 5, 6, 7, 8 }; + grad(Eigen::all, ind) = d_grad; + for (int i = 0; i < 3; i++) { + hess[i](ind, ind) = d_hess[i]; + } + + break; + } + + case PointTriangleDistanceType::P_E1: { + const auto [d, d_grad, d_hess] = PointEdgeDistanceDerivatives< + 3>::point_line_closest_point_direction_hessian(p, t1, t2); + pts = d; + std::vector ind { 0, 1, 2, 6, 7, 8, 9, 10, 11 }; + grad(Eigen::all, ind) = d_grad; + for (int i = 0; i < 3; i++) { + hess[i](ind, ind) = d_hess[i]; + } + + break; + } + + case PointTriangleDistanceType::P_E2: { + const auto [d, d_grad, d_hess] = PointEdgeDistanceDerivatives< + 3>::point_line_closest_point_direction_hessian(p, t2, t0); + pts = d; + std::vector ind { 0, 1, 2, 9, 10, 11, 3, 4, 5 }; + grad(Eigen::all, ind) = d_grad; + for (int i = 0; i < 3; i++) { + hess[i](ind, ind) = d_hess[i]; + } + + break; + } + + case PointTriangleDistanceType::P_T: + std::tie(pts, grad, hess) = + point_plane_closest_point_direction_hessian(p, t0, t1, t2); + break; + + default: + throw std::invalid_argument( + "Invalid distance type for point-triangle distance!"); + } + + return { pts, grad, hess }; +} + +template +scalar point_plane_sqr_distance( + Eigen::ConstRef> p, + Eigen::ConstRef> f0, + Eigen::ConstRef> f1, + Eigen::ConstRef> f2) +{ + const Eigen::Vector3 normal = (f2 - f0).cross(f1 - f0); + return Math::sqr(normal.dot(p - f0)) / normal.squaredNorm(); +} + +template +scalar point_triangle_sqr_distance( + Eigen::ConstRef> p, + Eigen::ConstRef> t0, + Eigen::ConstRef> t1, + Eigen::ConstRef> t2, + PointTriangleDistanceType dtype) +{ + if constexpr (std::is_same::value) { + if (dtype == PointTriangleDistanceType::AUTO) { + dtype = point_triangle_distance_type(p, t0, t1, t2); + } + } + + switch (dtype) { + case PointTriangleDistanceType::P_T0: { + return PointEdgeDistance::point_point_sqr_distance(p, t0); + } + + case PointTriangleDistanceType::P_T1: { + return PointEdgeDistance::point_point_sqr_distance(p, t1); + } + + case PointTriangleDistanceType::P_T2: { + return PointEdgeDistance::point_point_sqr_distance(p, t2); + } + + case PointTriangleDistanceType::P_E0: { + return PointEdgeDistance::point_line_sqr_distance(p, t0, t1); + } + + case PointTriangleDistanceType::P_E1: { + return PointEdgeDistance::point_line_sqr_distance(p, t1, t2); + } + + case PointTriangleDistanceType::P_E2: { + return PointEdgeDistance::point_line_sqr_distance(p, t2, t0); + } + + case PointTriangleDistanceType::P_T: { + return point_plane_sqr_distance(p, t0, t1, t2); + } + + default: { + throw std::invalid_argument( + "Invalid distance type for point-triangle distance!"); + } + } +} + +template +Eigen::Vector3 point_plane_closest_point_direction( + Eigen::ConstRef> p, + Eigen::ConstRef> f0, + Eigen::ConstRef> f1, + Eigen::ConstRef> f2) +{ + const Eigen::Vector3 normal = (f2 - f0).cross(f1 - f0); + return (normal.dot(p - f0) / normal.squaredNorm()) * normal; +} + +template +Eigen::Vector3 point_triangle_closest_point_direction( + Eigen::ConstRef> p, + Eigen::ConstRef> t0, + Eigen::ConstRef> t1, + Eigen::ConstRef> t2, + PointTriangleDistanceType dtype) +{ + if constexpr (std::is_same::value) { + if (dtype == PointTriangleDistanceType::AUTO) { + dtype = point_triangle_distance_type(p, t0, t1, t2); + } + } + + switch (dtype) { + case PointTriangleDistanceType::P_T0: + return p - t0; + + case PointTriangleDistanceType::P_T1: { + return p - t1; + } + + case PointTriangleDistanceType::P_T2: { + return p - t2; + } + + case PointTriangleDistanceType::P_E0: { + return PointEdgeDistance::point_line_closest_point_direction( + p, t0, t1); + } + + case PointTriangleDistanceType::P_E1: { + return PointEdgeDistance::point_line_closest_point_direction( + p, t1, t2); + } + + case PointTriangleDistanceType::P_E2: { + return PointEdgeDistance::point_line_closest_point_direction( + p, t2, t0); + } + + case PointTriangleDistanceType::P_T: { + return point_plane_closest_point_direction(p, t0, t1, t2); + } + + default: { + throw std::invalid_argument( + "Invalid distance type for point-triangle distance!"); + } + } +} + +template ADGrad<12> point_triangle_sqr_distance( + Eigen::ConstRef>> p, + Eigen::ConstRef>> t0, + Eigen::ConstRef>> t1, + Eigen::ConstRef>> t2, + PointTriangleDistanceType dtype); + +template ADHessian<12> point_triangle_sqr_distance( + Eigen::ConstRef>> p, + Eigen::ConstRef>> t0, + Eigen::ConstRef>> t1, + Eigen::ConstRef>> t2, + PointTriangleDistanceType dtype); +template ADGrad<13> point_triangle_sqr_distance( + Eigen::ConstRef>> p, + Eigen::ConstRef>> t0, + Eigen::ConstRef>> t1, + Eigen::ConstRef>> t2, + PointTriangleDistanceType dtype); +template ADHessian<13> point_triangle_sqr_distance( + Eigen::ConstRef>> p, + Eigen::ConstRef>> t0, + Eigen::ConstRef>> t1, + Eigen::ConstRef>> t2, + PointTriangleDistanceType dtype); +template double point_triangle_sqr_distance( + Eigen::ConstRef p, + Eigen::ConstRef t0, + Eigen::ConstRef t1, + Eigen::ConstRef t2, + PointTriangleDistanceType dtype); + +template Eigen::Vector3> point_triangle_closest_point_direction( + Eigen::ConstRef>> p, + Eigen::ConstRef>> t0, + Eigen::ConstRef>> t1, + Eigen::ConstRef>> t2, + PointTriangleDistanceType dtype); +template Eigen::Vector3> point_triangle_closest_point_direction( + Eigen::ConstRef>> p, + Eigen::ConstRef>> t0, + Eigen::ConstRef>> t1, + Eigen::ConstRef>> t2, + PointTriangleDistanceType dtype); +template Eigen::Vector3> point_triangle_closest_point_direction( + Eigen::ConstRef>> p, + Eigen::ConstRef>> t0, + Eigen::ConstRef>> t1, + Eigen::ConstRef>> t2, + PointTriangleDistanceType dtype); +template Eigen::Vector3> point_triangle_closest_point_direction( + Eigen::ConstRef>> p, + Eigen::ConstRef>> t0, + Eigen::ConstRef>> t1, + Eigen::ConstRef>> t2, + PointTriangleDistanceType dtype); +template Eigen::Vector3d point_triangle_closest_point_direction( + Eigen::ConstRef p, + Eigen::ConstRef t0, + Eigen::ConstRef t1, + Eigen::ConstRef t2, + PointTriangleDistanceType dtype); +} // namespace ipc \ No newline at end of file diff --git a/src/ipc/smooth_contact/distance/point_face.hpp b/src/ipc/smooth_contact/distance/point_face.hpp new file mode 100644 index 000000000..99e92cd91 --- /dev/null +++ b/src/ipc/smooth_contact/distance/point_face.hpp @@ -0,0 +1,72 @@ +#pragma once + +#include "point_edge.hpp" + +namespace ipc { + +template +T point_plane_sqr_distance( + Eigen::ConstRef> p, + Eigen::ConstRef> f0, + Eigen::ConstRef> f1, + Eigen::ConstRef> f2); + +template +T point_triangle_sqr_distance( + Eigen::ConstRef> p, + Eigen::ConstRef> t0, + Eigen::ConstRef> t1, + Eigen::ConstRef> t2, + PointTriangleDistanceType dtype); + +template +Eigen::Vector3 point_plane_closest_point_direction( + Eigen::ConstRef> p, + Eigen::ConstRef> f0, + Eigen::ConstRef> f1, + Eigen::ConstRef> f2); + +std::tuple> +point_plane_closest_point_direction_grad( + Eigen::ConstRef p, + Eigen::ConstRef t0, + Eigen::ConstRef t1, + Eigen::ConstRef t2); + +std::tuple< + Eigen::Vector3d, + Eigen::Matrix, + std::array> +point_plane_closest_point_direction_hessian( + Eigen::ConstRef p, + Eigen::ConstRef t0, + Eigen::ConstRef t1, + Eigen::ConstRef t2); + +template +Eigen::Vector3 point_triangle_closest_point_direction( + Eigen::ConstRef> p, + Eigen::ConstRef> t0, + Eigen::ConstRef> t1, + Eigen::ConstRef> t2, + PointTriangleDistanceType dtype); + +std::tuple> +point_triangle_closest_point_direction_grad( + Eigen::ConstRef p, + Eigen::ConstRef t0, + Eigen::ConstRef t1, + Eigen::ConstRef t2, + const PointTriangleDistanceType dtype = PointTriangleDistanceType::AUTO); + +std::tuple< + Eigen::Vector3d, + Eigen::Matrix, + std::array> +point_triangle_closest_point_direction_hessian( + Eigen::ConstRef p, + Eigen::ConstRef t0, + Eigen::ConstRef t1, + Eigen::ConstRef t2, + const PointTriangleDistanceType dtype = PointTriangleDistanceType::AUTO); +} // namespace ipc diff --git a/src/ipc/smooth_contact/distance/primitive_distance.cpp b/src/ipc/smooth_contact/distance/primitive_distance.cpp new file mode 100644 index 000000000..0d5454003 --- /dev/null +++ b/src/ipc/smooth_contact/distance/primitive_distance.cpp @@ -0,0 +1,286 @@ +#include "primitive_distance.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace ipc { + +template <> +typename PrimitiveDistType::type +PrimitiveDistance::compute_distance_type( + const Vector& x) +{ + return point_triangle_distance_type( + x.tail(3) /* point */, x.head(3), x.segment(3, 3), + x.segment(6, 3) /* face */); +} + +template <> +typename PrimitiveDistType::type +PrimitiveDistance::compute_distance_type( + const Vector& x) +{ + return edge_edge_distance_type( + x.head(3) /* edge 0 */, x.segment(3, 3) /* edge 0 */, + x.segment(6, 3) /* edge 1 */, x.tail(3) /* edge 1 */); +} + +template <> +typename PrimitiveDistType::type +PrimitiveDistance::compute_distance_type( + const Vector& x) +{ + return point_edge_distance_type( + x.tail(3) /* point */, x.head(3) /* edge */, + x.segment(3, 3) /* edge */); +} + +template <> +typename PrimitiveDistType::type +PrimitiveDistance::compute_distance_type( + const Vector& x) +{ + return PointEdgeDistanceType::AUTO; +} + +template <> +typename PrimitiveDistType::type +PrimitiveDistance::compute_distance_type( + const Vector& x) +{ + return PointPointDistanceType::P_P; +} + +template <> +typename PrimitiveDistType::type +PrimitiveDistance::compute_distance_type( + const Vector& x) +{ + return PointPointDistanceType::P_P; +} + +template <> +Vector::DIM> +PrimitiveDistance::compute_closest_direction( + const CollisionMesh& mesh, + const Eigen::MatrixXd& V, + const index_t a, + const index_t b, + typename PrimitiveDistType::type dtype) +{ + return point_triangle_closest_point_direction( + V.row(b), V.row(mesh.faces()(a, 0)), V.row(mesh.faces()(a, 1)), + V.row(mesh.faces()(a, 2)), dtype); +} + +template <> +Vector::DIM> +PrimitiveDistance::compute_closest_direction( + const CollisionMesh& mesh, + const Eigen::MatrixXd& V, + const index_t a, + const index_t b, + typename PrimitiveDistType::type dtype) +{ + return edge_edge_closest_point_direction( + V.row(mesh.edges()(a, 0)), V.row(mesh.edges()(a, 1)), + V.row(mesh.edges()(b, 0)), V.row(mesh.edges()(b, 1)), dtype); +} + +template <> +Vector::DIM> +PrimitiveDistance::compute_closest_direction( + const CollisionMesh& mesh, + const Eigen::MatrixXd& V, + const index_t a, + const index_t b, + typename PrimitiveDistType::type dtype) +{ + return PointEdgeDistance::point_edge_closest_point_direction( + V.row(b), V.row(mesh.edges()(a, 0)), V.row(mesh.edges()(a, 1)), dtype); +} + +template <> +Vector::DIM> +PrimitiveDistance::compute_closest_direction( + const CollisionMesh& mesh, + const Eigen::MatrixXd& V, + const index_t a, + const index_t b, + typename PrimitiveDistType::type dtype) +{ + return PointEdgeDistance::point_edge_closest_point_direction( + V.row(b), V.row(mesh.edges()(a, 0)), V.row(mesh.edges()(a, 1)), dtype); +} + +template <> +Vector::DIM> +PrimitiveDistance::compute_closest_direction( + const CollisionMesh& mesh, + const Eigen::MatrixXd& V, + const index_t a, + const index_t b, + typename PrimitiveDistType::type dtype) +{ + return V.row(b) - V.row(a); +} + +template <> +Vector::DIM> +PrimitiveDistance::compute_closest_direction( + const CollisionMesh& mesh, + const Eigen::MatrixXd& V, + const index_t a, + const index_t b, + typename PrimitiveDistType::type dtype) +{ + return V.row(b) - V.row(a); +} + +#ifndef IPC_TOOLKIT_DEBUG_AUTODIFF + +template <> +std::tuple< + Vector::DIM>, + Eigen::Matrix< + double, + PrimitiveDistance::DIM, + PrimitiveDistance::N_CORE_DOFS>, + std::array< + Eigen::Matrix< + double, + PrimitiveDistance::N_CORE_DOFS, + PrimitiveDistance::N_CORE_DOFS>, + PrimitiveDistance::DIM>> +PrimitiveDistance::compute_closest_direction_hessian( + const Vector& x, + typename PrimitiveDistType::type dtype) +{ + assert(dtype == EdgeEdgeDistanceType::EA_EB); + return line_line_closest_point_direction_hessian( + x.head<3>() /* edge 0 */, x.segment<3>(3) /* edge 0 */, + x.segment<3>(6) /* edge 1 */, x.tail<3>() /* edge 1 */); +} + +template <> +std::tuple< + Vector::DIM>, + Eigen::Matrix< + double, + PrimitiveDistance::DIM, + PrimitiveDistance::N_CORE_DOFS>, + std::array< + Eigen::Matrix< + double, + PrimitiveDistance::N_CORE_DOFS, + PrimitiveDistance::N_CORE_DOFS>, + PrimitiveDistance::DIM>> +PrimitiveDistance::compute_closest_direction_hessian( + const Vector& x, + typename PrimitiveDistType::type dtype) +{ + Eigen::Vector2d out = x.tail(2) - x.head(2); + Eigen::Matrix J = Eigen::Matrix::Zero(); + J.leftCols<2>().diagonal().array() = -1; + J.rightCols<2>().diagonal().array() = 1; + std::array, 2> H; + for (auto& h : H) { + h.setZero(); + } + return std::make_tuple(out, J, H); +} + +template <> +std::tuple< + Vector::DIM>, + Eigen::Matrix< + double, + PrimitiveDistance::DIM, + PrimitiveDistance::N_CORE_DOFS>, + std::array< + Eigen::Matrix< + double, + PrimitiveDistance::N_CORE_DOFS, + PrimitiveDistance::N_CORE_DOFS>, + PrimitiveDistance::DIM>> +PrimitiveDistance::compute_closest_direction_hessian( + const Vector& x, + typename PrimitiveDistType::type dtype) +{ + Eigen::Vector3d out = x.tail(3) - x.head(3); + Eigen::Matrix J = Eigen::Matrix::Zero(); + J.leftCols<3>().diagonal().array() = -1; + J.rightCols<3>().diagonal().array() = 1; + std::array, 3> H; + for (auto& h : H) { + h.setZero(); + } + return std::make_tuple(out, J, H); +} + +template <> +GradType::N_CORE_DOFS + 1> +PrimitiveDistance::compute_mollifier_gradient( + const Vector& x, const double dist_sqr) +{ + const auto otypes = edge_edge_mollifier_type( + x.head(3), x.segment(3, 3), x.segment(6, 3), x.tail(3), dist_sqr); + + return edge_edge_mollifier_gradient( + x.head(3), x.segment(3, 3), x.segment(6, 3), x.tail(3), otypes, + dist_sqr); +} + +template <> +HessianType::N_CORE_DOFS + 1> +PrimitiveDistance::compute_mollifier_hessian( + const Vector& x, const double dist_sqr) +{ + const auto otypes = edge_edge_mollifier_type( + x.head<3>(), x.segment<3>(3), x.segment<3>(6), x.tail<3>(), dist_sqr); + + return edge_edge_mollifier_hessian( + x.head<3>(), x.segment<3>(3), x.segment<3>(6), x.tail<3>(), otypes, + dist_sqr); +} + +template <> +GradType::N_CORE_DOFS + 1> +PrimitiveDistance::compute_mollifier_gradient( + const Vector& x, const double dist_sqr) +{ + const auto [val, grad] = point_face_mollifier_gradient( + x.tail<3>(), x.head<3>(), x.segment<3>(3), x.segment<3>(6), dist_sqr); + Vector indices; + indices << 3, 4, 5, 6, 7, 8, 9, 10, 11, 0, 1, 2, 12; + return std::make_tuple(val, grad(indices)); +} + +template <> +HessianType::N_CORE_DOFS + 1> +PrimitiveDistance::compute_mollifier_hessian( + const Vector& x, const double dist_sqr) +{ + const auto [val, grad, hess] = point_face_mollifier_hessian( + x.tail<3>(), x.head<3>(), x.segment<3>(3), x.segment<3>(6), dist_sqr); + Vector indices; + indices << 3, 4, 5, 6, 7, 8, 9, 10, 11, 0, 1, 2, 12; + return std::make_tuple(val, grad(indices), hess(indices, indices)); +} + +#endif + +template class PrimitiveDistance; +template class PrimitiveDistance; + +template class PrimitiveDistance; +template class PrimitiveDistance; +template class PrimitiveDistance; +template class PrimitiveDistance; +} // namespace ipc diff --git a/src/ipc/smooth_contact/distance/primitive_distance.hpp b/src/ipc/smooth_contact/distance/primitive_distance.hpp new file mode 100644 index 000000000..e77a79f72 --- /dev/null +++ b/src/ipc/smooth_contact/distance/primitive_distance.hpp @@ -0,0 +1,205 @@ +#pragma once +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace ipc { +template +struct PrimitiveDistType { }; + +template <> struct PrimitiveDistType { + using type = PointPointDistanceType; + static constexpr std::string_view NAME = "PointPoint"; +}; + +template <> struct PrimitiveDistType { + using type = PointPointDistanceType; + static constexpr std::string_view NAME = "PointPoint"; +}; + +template <> struct PrimitiveDistType { + using type = PointEdgeDistanceType; + static constexpr std::string_view NAME = "EdgePoint"; +}; + +template <> struct PrimitiveDistType { + using type = PointEdgeDistanceType; + static constexpr std::string_view NAME = "EdgePoint"; +}; + +template <> struct PrimitiveDistType { + using type = PointTriangleDistanceType; + static constexpr std::string_view NAME = "FacePoint"; +}; + +template <> struct PrimitiveDistType { + using type = EdgeEdgeDistanceType; + static constexpr std::string_view NAME = "EDGE_EDGE"; +}; + +template +class PrimitiveDistanceTemplate { + static_assert( + PrimitiveA::DIM == PrimitiveB::DIM, + "Primitives must have the same dimension"); + static constexpr int DIM = PrimitiveA::DIM; + static constexpr int N_CORE_DOFS = + PrimitiveA::N_CORE_POINTS * PrimitiveA::DIM + + PrimitiveB::N_CORE_POINTS * PrimitiveB::DIM; + +public: + static T compute_distance( + const Vector& x, + typename PrimitiveDistType::type dtype); + static Vector compute_closest_direction( + const Vector& x, + typename PrimitiveDistType::type dtype); + static Eigen::Matrix compute_closest_point_pairs( + const Vector& x, + typename PrimitiveDistType::type dtype); + static T mollifier(const Vector& x, const T& dist_sqr); +}; + +template class PrimitiveDistance { + static_assert( + PrimitiveA::DIM == PrimitiveB::DIM, + "Primitives must have the same dimension"); + +public: + static constexpr int DIM = PrimitiveA::DIM; + static constexpr int N_CORE_DOFS = + PrimitiveA::N_CORE_POINTS * PrimitiveA::DIM + + PrimitiveB::N_CORE_POINTS * PrimitiveB::DIM; + + static typename PrimitiveDistType::type + compute_distance_type(const Vector& x); + + static double compute_distance( + const CollisionMesh& mesh, + const Eigen::MatrixXd& V, + const index_t a, + const index_t b, + typename PrimitiveDistType::type dtype); + + static GradType compute_distance_gradient( + const Vector& x, + typename PrimitiveDistType::type dtype) + { + ScalarBase::setVariableCount(N_CORE_DOFS); + using T = ADGrad; + const Vector X = slice_positions(x); + const T d = PrimitiveDistanceTemplate< + PrimitiveA, PrimitiveB, T>::compute_distance(X, dtype); + + return { d.val, d.grad }; + } + + static HessianType compute_distance_hessian( + const Vector& x, + typename PrimitiveDistType::type dtype) + { + ScalarBase::setVariableCount(N_CORE_DOFS); + using T = ADHessian; + const Vector X = slice_positions(x); + const T d = PrimitiveDistanceTemplate< + PrimitiveA, PrimitiveB, T>::compute_distance(X, dtype); + + return { d.val, d.grad, d.Hess }; + } + + // points from primitiveA to primitiveB + static Vector compute_closest_direction( + const CollisionMesh& mesh, + const Eigen::MatrixXd& V, + const index_t a, + const index_t b, + typename PrimitiveDistType::type dtype); + + static std:: + tuple, Eigen::Matrix> + compute_closest_direction_gradient( + const Vector& x, + typename PrimitiveDistType::type dtype) + { + ScalarBase::setVariableCount(N_CORE_DOFS); + using T = ADGrad; + const Vector X = slice_positions(x); + const Vector d = PrimitiveDistanceTemplate< + PrimitiveA, PrimitiveB, T>::compute_closest_direction(X, dtype); + + Vector out; + Eigen::Matrix J = + Eigen::Matrix::Zero(); + for (int i = 0; i < DIM; i++) { + out(i) = d(i).val; + J.row(i) = d(i).grad; + } + return std::make_tuple(out, J); + } + + static std::tuple< + Vector, + Eigen::Matrix, + std::array, DIM>> + compute_closest_direction_hessian( + const Vector& x, + typename PrimitiveDistType::type dtype) + { + ScalarBase::setVariableCount(N_CORE_DOFS); + using T = ADHessian; + const Vector X = slice_positions(x); + const Vector d = PrimitiveDistanceTemplate< + PrimitiveA, PrimitiveB, T>::compute_closest_direction(X, dtype); + + Vector out; + Eigen::Matrix J = + Eigen::Matrix::Zero(); + std::array, DIM> H; + for (int i = 0; i < DIM; i++) { + out(i) = d(i).val; + J.row(i) = d(i).grad; + H[i] = d(i).Hess; + } + return std::make_tuple(out, J, H); + } + + static GradType compute_mollifier_gradient( + const Vector& x, const double dist_sqr) + { + ScalarBase::setVariableCount(N_CORE_DOFS + 1); + using T = ADGrad; + const Vector X = + slice_positions( + (Vector() << x, dist_sqr).finished()); + const T out = + PrimitiveDistanceTemplate::mollifier( + X.head(N_CORE_DOFS), X(N_CORE_DOFS)); + + return std::make_tuple(out.val, out.grad); + } + + static HessianType compute_mollifier_hessian( + const Vector& x, const double dist_sqr) + { + ScalarBase::setVariableCount(N_CORE_DOFS + 1); + using T = ADHessian; + const Vector X = + slice_positions( + (Vector() << x, dist_sqr).finished()); + const T out = + PrimitiveDistanceTemplate::mollifier( + X.head(N_CORE_DOFS), X(N_CORE_DOFS)); + + return std::make_tuple(out.val, out.grad, out.Hess); + } +}; + +} // namespace ipc + +#include "primitive_distance.tpp" diff --git a/src/ipc/smooth_contact/distance/primitive_distance.tpp b/src/ipc/smooth_contact/distance/primitive_distance.tpp new file mode 100644 index 000000000..98ec97427 --- /dev/null +++ b/src/ipc/smooth_contact/distance/primitive_distance.tpp @@ -0,0 +1,206 @@ +#pragma once + +#include "primitive_distance.hpp" + +namespace ipc { + +template class PrimitiveDistanceTemplate { + static_assert( + Face::DIM == Point3::DIM, "Primitives must have the same dimension"); + static constexpr int DIM = Face::DIM; + static constexpr int N_CORE_DOFS = + Face::N_CORE_POINTS * Face::DIM + Point3::N_CORE_POINTS * Point3::DIM; + +public: + static T compute_distance( + const Vector& x, + typename PrimitiveDistType::type dtype) + { + return point_triangle_sqr_distance( + x.template tail<3>() /* point */, x.template head<3>(), + x.template segment<3>(3), x.template segment<3>(6) /* face */, + dtype); + } + + static Vector compute_closest_direction( + const Vector& x, + typename PrimitiveDistType::type dtype) + { + return point_triangle_closest_point_direction( + x.template tail<3>() /* point */, x.template head<3>(), + x.template segment<3>(3), x.template segment<3>(6) /* face */, + dtype); + } + + static T mollifier(const Vector& x, const T& dist_sqr) + { + return point_face_mollifier( + x.template tail<3>() /* point */, x.template head<3>(), + x.template segment<3>(3), x.template segment<3>(6) /* face */, + dist_sqr); + } +}; + +template class PrimitiveDistanceTemplate { + static constexpr int DIM = Edge3::DIM; + static constexpr int N_CORE_DOFS = + Edge3::N_CORE_POINTS * Edge3::DIM + Edge3::N_CORE_POINTS * Edge3::DIM; + +public: + static T compute_distance( + const Vector& x, + typename PrimitiveDistType::type dtype) + { + return edge_edge_sqr_distance( + x.template head<3>() /* edge 0 */, + x.template segment<3>(3) /* edge 0 */, + x.template segment<3>(6) /* edge 1 */, + x.template tail<3>() /* edge 1 */, dtype); + } + + static Vector compute_closest_direction( + const Vector& x, + typename PrimitiveDistType::type dtype) + { + return edge_edge_closest_point_direction( + x.template head<3>() /* edge 0 */, + x.template segment<3>(3) /* edge 0 */, + x.template segment<3>(6) /* edge 1 */, + x.template tail<3>() /* edge 1 */, dtype); + } + + static T mollifier(const Vector& x, const T& dist_sqr) + { + std::array types {}; + types.fill(HeavisideType::VARIANT); + return edge_edge_mollifier( + x.template head<3>() /* edge 0 */, + x.template segment<3>(3) /* edge 0 */, + x.template segment<3>(6) /* edge 1 */, + x.template tail<3>() /* edge 1 */, types, dist_sqr); + } +}; + +template class PrimitiveDistanceTemplate { + static_assert( + Edge2::DIM == Point2::DIM, "Primitives must have the same dimension"); + static constexpr int DIM = Point2::DIM; + static constexpr int N_CORE_DOFS = + Edge2::N_CORE_POINTS * Edge2::DIM + Point2::N_CORE_POINTS * Point2::DIM; + +public: + static T compute_distance( + const Vector& x, + typename PrimitiveDistType::type dtype) + { + return PointEdgeDistance::point_edge_sqr_distance( + x.template tail<2>() /* point */, x.template head<2>() /* edge */, + x.template segment<2>(2) /* edge */, dtype); + } + + static Vector compute_closest_direction( + const Vector& x, + typename PrimitiveDistType::type dtype) + { + return PointEdgeDistance::point_edge_closest_point_direction( + x.template tail<2>() /* point */, x.template head<2>() /* edge */, + x.template segment<2>(2) /* edge */, dtype); + } + + static T mollifier(const Vector& x, const T& dist_sqr) + { + return point_edge_mollifier( + x.template tail<2>() /* point */, + x.template segment<2>(2) /* edge */, + x.template head<2>() /* edge */, dist_sqr); + } +}; + +template class PrimitiveDistanceTemplate { + static_assert( + Edge3::DIM == Point3::DIM, "Primitives must have the same dimension"); + static constexpr int DIM = Edge3::DIM; + static constexpr int N_CORE_DOFS = + Edge3::N_CORE_POINTS * Edge3::DIM + Point3::N_CORE_POINTS * Point3::DIM; + +public: + static T compute_distance( + const Vector& x, + typename PrimitiveDistType::type dtype) + { + return PointEdgeDistance::point_edge_sqr_distance( + x.template tail<3>() /* point */, x.template head<3>() /* edge */, + x.template segment<3>(3) /* edge */, dtype); + } + + static Vector compute_closest_direction( + const Vector& x, + typename PrimitiveDistType::type dtype) + { + return PointEdgeDistance::point_edge_closest_point_direction( + x.template tail<3>() /* point */, x.template head<3>() /* edge */, + x.template segment<3>(3) /* edge */, dtype); + } + + static T mollifier(const Vector& x, const T& dist_sqr) + { + return point_edge_mollifier( + x.template tail<3>() /* point */, + x.template segment<3>(3) /* edge */, + x.template head<3>() /* edge */, dist_sqr); + } +}; + +template class PrimitiveDistanceTemplate { + static constexpr int DIM = Point2::DIM; + static constexpr int N_CORE_DOFS = Point2::N_CORE_POINTS * Point2::DIM + + Point2::N_CORE_POINTS * Point2::DIM; + +public: + static T compute_distance( + const Vector& x, + typename PrimitiveDistType::type dtype) + { + return (x.template tail<2>() - x.template head<2>()).squaredNorm(); + } + + static Vector compute_closest_direction( + const Vector& x, + typename PrimitiveDistType::type dtype) + { + return x.template tail<2>() - x.template head<2>(); + } + + static T mollifier(const Vector& x, const T& dist_sqr) + { + return T(1.); + } +}; + +template class PrimitiveDistanceTemplate { + static constexpr int DIM = Point3::DIM; + static constexpr int N_CORE_DOFS = Point3::N_CORE_POINTS * Point3::DIM + + Point3::N_CORE_POINTS * Point3::DIM; + +public: + static T compute_distance( + const Vector& x, + typename PrimitiveDistType::type dtype) + { + return (x.template tail<3>() - x.template head<3>()).squaredNorm(); + } + + static Vector compute_closest_direction( + const Vector& x, + typename PrimitiveDistType::type dtype) + { + return x.template tail<3>() - x.template head<3>(); + } + + static T mollifier(const Vector& x, const T& dist_sqr) + { + return T(1.); + } +}; + +} // namespace ipc diff --git a/src/ipc/smooth_contact/primitives/CMakeLists.txt b/src/ipc/smooth_contact/primitives/CMakeLists.txt new file mode 100644 index 000000000..950d2192d --- /dev/null +++ b/src/ipc/smooth_contact/primitives/CMakeLists.txt @@ -0,0 +1,18 @@ +set(SOURCES + autogen.cpp + autogen.hpp + edge2.cpp + edge2.hpp + edge3.cpp + edge3.hpp + face.cpp + face.hpp + primitive.hpp + point3.cpp + point3.hpp + point2.cpp + point2.hpp +) + +source_group(TREE "${CMAKE_CURRENT_SOURCE_DIR}" PREFIX "Source Files" FILES ${SOURCES}) +target_sources(ipc_toolkit PRIVATE ${SOURCES}) diff --git a/src/ipc/smooth_contact/primitives/autogen.cpp b/src/ipc/smooth_contact/primitives/autogen.cpp new file mode 100644 index 000000000..d643d6e83 --- /dev/null +++ b/src/ipc/smooth_contact/primitives/autogen.cpp @@ -0,0 +1,8588 @@ +#include "autogen.hpp" + +#include + +namespace ipc { +namespace autogen { + + void edge_normal_term_gradient( + double d_x, + double d_y, + double d_z, + double e0_x, + double e0_y, + double e0_z, + double e1_x, + double e1_y, + double e1_z, + double f0_x, + double f0_y, + double f0_z, + double f1_x, + double f1_y, + double f1_z, + double grad[15]) + { + const auto t0 = e0_y - e1_y; + const auto t1 = e0_z - e1_z; + const auto t2 = e0_x - e1_x; + const auto t3 = std::pow(t2, 2); + const auto t4 = std::pow(t0, 2); + const auto t5 = std::pow(t1, 2); + const auto t6 = t3 + t4 + t5; + const auto t7 = 1.0 / t6; + const auto t8 = d_x * t2; + const auto t9 = d_y * t0; + const auto t10 = d_z * t1; + const auto t11 = t10 + t8 + t9; + const auto t12 = t11 * t7; + const auto t13 = d_z - t1 * t12; + const auto t14 = d_x - t12 * t2; + const auto t15 = d_y - t0 * t12; + const auto t16 = std::pow(t13, 2) + std::pow(t14, 2) + std::pow(t15, 2); + const auto t17 = std::pow(t16, -1.0 / 2.0); + const auto t18 = t13 * t17; + const auto t19 = -t2; + const auto t20 = -t0; + const auto t21 = -t1; + const auto t22 = std::pow(t19, 2) + std::pow(t20, 2) + std::pow(t21, 2); + const auto t23 = 1.0 / t22; + const auto t24 = e0_x - f1_x; + const auto t25 = -t24; + const auto t26 = t2 * t25; + const auto t27 = e0_y - f1_y; + const auto t28 = -t27; + const auto t29 = t0 * t28; + const auto t30 = e0_z - f1_z; + const auto t31 = -t30; + const auto t32 = t1 * t31; + const auto t33 = t26 + t29 + t32; + const auto t34 = t23 * t33; + const auto t35 = t1 * t34 + t30; + const auto t36 = t2 * t34 + t24; + const auto t37 = t0 * t34 + t27; + const auto t38 = std::pow(t35, 2) + std::pow(t36, 2) + std::pow(t37, 2); + const auto t39 = std::pow(t38, -1.0 / 2.0); + const auto t40 = t18 + t35 * t39; + const auto t41 = 1.0 / t16; + const auto t42 = t23 * t3; + const auto t43 = t42 - 1; + const auto t44 = t2 * t7; + const auto t45 = t0 * t44; + const auto t46 = t1 * t44; + const auto t47 = t13 * t46 + t15 * t45; + const auto t48 = t41 * (t14 * t43 + t47); + const auto t49 = 1 - t42; + const auto t50 = t14 * t48 + t49; + const auto t51 = e0_x - f0_x; + const auto t52 = -t51; + const auto t53 = t2 * t52; + const auto t54 = e0_y - f0_y; + const auto t55 = -t54; + const auto t56 = t0 * t55; + const auto t57 = e0_z - f0_z; + const auto t58 = -t57; + const auto t59 = t1 * t58; + const auto t60 = t53 + t56 + t59; + const auto t61 = t23 * t60; + const auto t62 = t1 * t61 + t57; + const auto t63 = t2 * t61 + t51; + const auto t64 = t0 * t61 + t54; + const auto t65 = std::pow(t62, 2) + std::pow(t63, 2) + std::pow(t64, 2); + const auto t66 = std::pow(t65, -1.0 / 2.0); + const auto t67 = t18 + t62 * t66; + const auto t68 = t14 * t17; + const auto t69 = t63 * t66 + t68; + const auto t70 = -t46; + const auto t71 = t13 * t48 + t70; + const auto t72 = t36 * t39 + t68; + const auto t73 = t15 * t17; + const auto t74 = -e0_y; + const auto t75 = t0 * t27 + t1 * t30 + t2 * t24; + const auto t76 = t7 * t75; + const auto t77 = -f1_y - t0 * t76 - t74; + const auto t78 = -e0_x; + const auto t79 = -f1_x - t44 * t75 - t78; + const auto t80 = -e0_z; + const auto t81 = -f1_z - t1 * t76 - t80; + const auto t82 = std::pow(t77, 2) + std::pow(t79, 2) + std::pow(t81, 2); + const auto t83 = std::pow(t82, -1.0 / 2.0); + const auto t84 = t73 + t77 * t83; + const auto t85 = t3 * t7; + const auto t86 = t41 * (t14 * (t85 - 1) + t47); + const auto t87 = t14 * t86 - t85 + 1; + const auto t88 = t7 * (t0 * t54 + t1 * t57 + t2 * t51); + const auto t89 = -f0_y - t0 * t88 - t74; + const auto t90 = -f0_x - t2 * t88 - t78; + const auto t91 = -f0_z - t1 * t88 - t80; + const auto t92 = std::pow( + std::pow(t89, 2) + std::pow(t90, 2) + std::pow(t91, 2), -1.0 / 2.0); + const auto t93 = t73 + t89 * t92; + const auto t94 = t68 + t90 * t92; + const auto t95 = -t45; + const auto t96 = t15 * t86 + t95; + const auto t97 = t68 + t79 * t83; + const auto t98 = t13 * t86 + t70; + const auto t99 = t18 + t81 * t83; + const auto t100 = t18 + t91 * t92; + const auto t101 = std::pow(t6, -1.0 / 2.0); + const auto t102 = t101 * t17; + const auto t103 = t0 * t1; + const auto t104 = t103 * t7; + const auto t105 = -t104; + const auto t106 = t23 * t4; + const auto t107 = t106 - 1; + const auto t108 = t104 * t13 + t14 * t45; + const auto t109 = t41 * (t107 * t15 + t108); + const auto t110 = t105 + t109 * t13; + const auto t111 = t109 * t14 + t95; + const auto t112 = t4 * t7; + const auto t113 = t41 * (t108 + t15 * (t112 - 1)); + const auto t114 = -t112 + t113 * t15 + 1; + const auto t115 = t105 + t113 * t13; + const auto t116 = t113 * t14 + t95; + const auto t117 = t23 * t5; + const auto t118 = t117 - 1; + const auto t119 = t104 * t15 + t14 * t46; + const auto t120 = t41 * (t118 * t13 + t119); + const auto t121 = 1 - t117; + const auto t122 = t120 * t13 + t121; + const auto t123 = t120 * t14 + t70; + const auto t124 = t5 * t7; + const auto t125 = t41 * (t119 + t13 * (t124 - 1)); + const auto t126 = -t124 + t125 * t13 + 1; + const auto t127 = t105 + t125 * t15; + const auto t128 = t125 * t14 + t70; + const auto t129 = std::pow(t38, -3.0 / 2.0); + const auto t130 = -t37; + const auto t131 = t19 * t23; + const auto t132 = -2 * e0_x + e1_x; + const auto t133 = f1_x + t131 * t26 + t131 * t29 + t131 * t32 + t132; + const auto t134 = t133 + t19 * t34; + const auto t135 = t0 * t23; + const auto t136 = t134 * t135; + const auto t137 = -t35; + const auto t138 = t1 * t23; + const auto t139 = t134 * t138; + const auto t140 = -t36; + const auto t141 = t2 * t23; + const auto t142 = t19 * t2; + const auto t143 = std::pow(t22, -2); + const auto t144 = t143 * t33; + const auto t145 = t34 + 1; + const auto t146 = t133 * t141 + t142 * t144 + t145; + const auto t147 = t129 * (t130 * t136 + t137 * t139 + t140 * t146); + const auto t148 = t11 * t23; + const auto t149 = d_x + t10 * t131 + t131 * t8 + t131 * t9; + const auto t150 = t148 * t19 + t149; + const auto t151 = t0 * t148; + const auto t152 = d_y - t151; + const auto t153 = t0 * t152; + const auto t154 = t1 * t148; + const auto t155 = d_z - t154; + const auto t156 = t1 * t155; + const auto t157 = t11 * t141; + const auto t158 = d_x - t157; + const auto t159 = t11 + t149 * t2 + t157 * t19; + const auto t160 = std::pow(t16, -3.0 / 2.0); + const auto t161 = t160 * t23; + const auto t162 = t161 * (t150 * t153 + t150 * t156 + t158 * t159); + const auto t163 = -t1 * t150 * t17 * t23 + t13 * t162; + const auto t164 = t139 * t39 + t147 * t35 + t163; + const auto t165 = std::pow(t65, -3.0 / 2.0); + const auto t166 = -t64; + const auto t167 = f0_x + t131 * t53 + t131 * t56 + t131 * t59 + t132; + const auto t168 = t167 + t19 * t61; + const auto t169 = t135 * t168; + const auto t170 = -t62; + const auto t171 = t138 * t168; + const auto t172 = -t63; + const auto t173 = t143 * t60; + const auto t174 = t61 + 1; + const auto t175 = t141 * t167 + t142 * t173 + t174; + const auto t176 = t165 * (t166 * t169 + t170 * t171 + t172 * t175); + const auto t177 = t163 + t171 * t66 + t176 * t62; + const auto t178 = t14 * t162 - t159 * t17 * t23; + const auto t179 = t175 * t66 + t176 * t63 + t178; + const auto t180 = t146 * t39 + t147 * t36 + t178; + const auto t181 = -t0 * t150 * t17 * t23 + t15 * t162; + const auto t182 = -t169 * t66 - t176 * t64 - t181; + const auto t183 = -t136 * t39 - t147 * t37 - t181; + const auto t184 = t100 * t84; + const auto t185 = t93 * t99; + const auto t186 = -t184 + t185; + const auto t187 = t84 * t94; + const auto t188 = t93 * t97; + const auto t189 = t187 - t188; + const auto t190 = t40 * t69 - t67 * t72; + const auto t191 = t184 - t185 + t186 * t85 + t189 * t46 - t190 * t45; + const auto t192 = + std::pow(t152, 2) + std::pow(t155, 2) + std::pow(t158, 2); + const auto t193 = std::pow(t192, -1.0 / 2.0); + const auto t194 = t158 * t193; + const auto t195 = + std::pow(t166, 2) + std::pow(t170, 2) + std::pow(t172, 2); + const auto t196 = std::pow(t195, -1.0 / 2.0); + const auto t197 = -t172 * t196 + t194; + const auto t198 = t20 * t23; + const auto t199 = -2 * e0_y + e1_y; + const auto t200 = f1_y + t198 * t26 + t198 * t29 + t198 * t32 + t199; + const auto t201 = t20 * t34 + t200; + const auto t202 = t141 * t201; + const auto t203 = t138 * t201; + const auto t204 = t0 * t20; + const auto t205 = t135 * t200 + t144 * t204 + t145; + const auto t206 = t130 * t205 + t137 * t203 + t140 * t202; + const auto t207 = + std::pow(t130, 2) + std::pow(t137, 2) + std::pow(t140, 2); + const auto t208 = t206 / std::pow(t207, 3.0 / 2.0); + const auto t209 = std::pow(t207, -1.0 / 2.0); + const auto t210 = d_y + t10 * t198 + t198 * t8 + t198 * t9; + const auto t211 = t148 * t20 + t210; + const auto t212 = t158 * t2; + const auto t213 = t0 * t210 + t11 + t151 * t20; + const auto t214 = t152 * t213 + t156 * t211 + t211 * t212; + const auto t215 = t214 * t23 / std::pow(t192, 3.0 / 2.0); + const auto t216 = -t138 * t193 * t211 + t155 * t215; + const auto t217 = t155 * t193; + const auto t218 = -t137 * t209 + t217; + const auto t219 = f0_y + t198 * t53 + t198 * t56 + t198 * t59 + t199; + const auto t220 = t20 * t61 + t219; + const auto t221 = t141 * t220; + const auto t222 = t138 * t220; + const auto t223 = t135 * t219 + t173 * t204 + t174; + const auto t224 = t166 * t223 + t170 * t222 + t172 * t221; + const auto t225 = t224 / std::pow(t195, 3.0 / 2.0); + const auto t226 = t141 * t211; + const auto t227 = t158 * t215 - t193 * t226; + const auto t228 = -t140 * t209 + t194; + const auto t229 = -t170 * t196 + t217; + const auto t230 = t64 * t66 + t73; + const auto t231 = t129 * t206; + const auto t232 = t161 * t214; + const auto t233 = t17 * t23; + const auto t234 = -t1 * t211 * t233 + t13 * t232; + const auto t235 = t37 * t39 + t73; + const auto t236 = t165 * t224; + const auto t237 = t15 * t232 - t213 * t233; + const auto t238 = t223 * t66 + t236 * t64 + t237; + const auto t239 = t205 * t39 + t231 * t37 + t237; + const auto t240 = t14 * t232 - t17 * t226; + const auto t241 = t197 * t218 - t228 * t229; + const auto t242 = t21 * t23; + const auto t243 = -2 * e0_z + e1_z; + const auto t244 = f0_z + t242 * t53 + t242 * t56 + t242 * t59 + t243; + const auto t245 = t21 * t61 + t244; + const auto t246 = t141 * t245; + const auto t247 = t135 * t245; + const auto t248 = t1 * t21; + const auto t249 = t138 * t244 + t173 * t248 + t174; + const auto t250 = t165 * (t166 * t247 + t170 * t249 + t172 * t246); + const auto t251 = d_z + t10 * t242 + t242 * t8 + t242 * t9; + const auto t252 = t148 * t21 + t251; + const auto t253 = t1 * t251 + t11 + t154 * t21; + const auto t254 = t161 * (t153 * t252 + t155 * t253 + t212 * t252); + const auto t255 = t14 * t254 - t17 * t2 * t23 * t252; + const auto t256 = t246 * t66 + t250 * t63 + t255; + const auto t257 = f1_z + t242 * t26 + t242 * t29 + t242 * t32 + t243; + const auto t258 = t21 * t34 + t257; + const auto t259 = t141 * t258; + const auto t260 = t135 * t258; + const auto t261 = t138 * t257 + t144 * t248 + t145; + const auto t262 = t129 * (t130 * t260 + t137 * t261 + t140 * t259); + const auto t263 = t255 + t259 * t39 + t262 * t36; + const auto t264 = t13 * t254 - t17 * t23 * t253; + const auto t265 = t261 * t39 + t262 * t35 + t264; + const auto t266 = t249 * t66 + t250 * t62 + t264; + const auto t267 = -t0 * t17 * t23 * t252 + t15 * t254; + const auto t268 = -t247 * t66 - t250 * t64 - t267; + const auto t269 = -t260 * t39 - t262 * t37 - t267; + const auto t270 = -t104 * t190 + t124 * t189 + t186 * t46 - t187 + t188; + const auto t271 = t141 * t29 + t141 * t32 + t25 * t42; + const auto t272 = t271 + t36; + const auto t273 = t0 * t272; + const auto t274 = t1 * t272; + const auto t275 = -t2 * (t24 + t271) - t33 * t42 + t33; + const auto t276 = t129 * (-t130 * t273 - t137 * t274 + t140 * t275); + const auto t277 = d_x * t42 - d_x + t10 * t141 + t141 * t9; + const auto t278 = t157 + t277; + const auto t279 = -t11 * t42 + t11 - t2 * t277; + const auto t280 = t160 * (-t153 * t278 - t156 * t278 + t158 * t279); + const auto t281 = t17 * t278; + const auto t282 = t1 * t281 + t13 * t280; + const auto t283 = -t274 * t39 + t276 * t35 + t282; + const auto t284 = t141 * t56 + t141 * t59 + t42 * t52; + const auto t285 = t284 + t63; + const auto t286 = t0 * t285; + const auto t287 = t1 * t285; + const auto t288 = -t2 * (t284 + t51) - t42 * t60 + t60; + const auto t289 = t165 * (-t166 * t286 - t170 * t287 + t172 * t288); + const auto t290 = t0 * t281 + t15 * t280; + const auto t291 = -t286 * t66 + t289 * t64 + t290; + const auto t292 = t282 - t287 * t66 + t289 * t62; + const auto t293 = -t273 * t39 + t276 * t37 + t290; + const auto t294 = t14 * t280 - t17 * t279; + const auto t295 = t288 * t66 + t289 * t63 + t294; + const auto t296 = t275 * t39 + t276 * t36 + t294; + const auto t297 = t106 * t28 + t135 * t26 + t135 * t32; + const auto t298 = t297 + t37; + const auto t299 = t2 * t298; + const auto t300 = t1 * t298; + const auto t301 = -t0 * (t27 + t297) - t23 * t33 * t4 + t33; + const auto t302 = t129 * (t130 * t301 - t137 * t300 - t140 * t299); + const auto t303 = d_y * t106 - d_y + t10 * t135 + t135 * t8; + const auto t304 = t151 + t303; + const auto t305 = -t0 * t303 - t106 * t11 + t11; + const auto t306 = t160 * (t152 * t305 - t156 * t304 - t212 * t304); + const auto t307 = t17 * t304; + const auto t308 = t1 * t307 + t13 * t306; + const auto t309 = -t300 * t39 + t302 * t35 + t308; + const auto t310 = t106 * t55 + t135 * t53 + t135 * t59; + const auto t311 = t310 + t64; + const auto t312 = t2 * t311; + const auto t313 = t1 * t311; + const auto t314 = -t0 * (t310 + t54) - t106 * t60 + t60; + const auto t315 = t165 * (t166 * t314 - t170 * t313 - t172 * t312); + const auto t316 = t308 - t313 * t66 + t315 * t62; + const auto t317 = t15 * t306 - t17 * t305; + const auto t318 = t314 * t66 + t315 * t64 + t317; + const auto t319 = t301 * t39 + t302 * t37 + t317; + const auto t320 = t14 * t306 + t2 * t307; + const auto t321 = -t312 * t66 + t315 * t63 + t320; + const auto t322 = -t299 * t39 + t302 * t36 + t320; + const auto t323 = t117 * t58 + t138 * t53 + t138 * t56; + const auto t324 = t323 + t62; + const auto t325 = t2 * t324; + const auto t326 = t0 * t324; + const auto t327 = -t1 * (t323 + t57) - t23 * t5 * t60 + t60; + const auto t328 = t165 * (-t166 * t326 + t170 * t327 - t172 * t325); + const auto t329 = d_z * t117 - d_z + t138 * t8 + t138 * t9; + const auto t330 = t154 + t329; + const auto t331 = -t1 * t329 - t11 * t23 * t5 + t11; + const auto t332 = t160 * (-t153 * t330 + t155 * t331 - t212 * t330); + const auto t333 = t17 * t330; + const auto t334 = t0 * t333 + t15 * t332; + const auto t335 = -t326 * t66 + t328 * t64 + t334; + const auto t336 = t117 * t31 + t138 * t26 + t138 * t29; + const auto t337 = t336 + t35; + const auto t338 = t2 * t337; + const auto t339 = t0 * t337; + const auto t340 = -t1 * (t30 + t336) - t117 * t33 + t33; + const auto t341 = t129 * (-t130 * t339 + t137 * t340 - t140 * t338); + const auto t342 = t334 - t339 * t39 + t341 * t37; + const auto t343 = t13 * t332 - t17 * t331; + const auto t344 = t340 * t39 + t341 * t35 + t343; + const auto t345 = t327 * t66 + t328 * t62 + t343; + const auto t346 = t14 * t332 + t2 * t333; + const auto t347 = -t325 * t66 + t328 * t63 + t346; + const auto t348 = -t338 * t39 + t341 * t36 + t346; + const auto t349 = -t130 * t209 + t152 * t193; + const auto t350 = 1.0 / t195; + const auto t351 = t0 * t141; + const auto t352 = t1 * t141; + const auto t353 = -t43; + const auto t354 = t350 * (t166 * t351 + t170 * t352 - t172 * t353); + const auto t355 = t172 * t354 + t49; + const auto t356 = -t166 * t354 + t351; + const auto t357 = t1 * t196; + const auto t358 = t196 * t2; + const auto t359 = 1.0 / t65; + const auto t360 = t359 * t62; + const auto t361 = t104 * t62 + t107 * t64 + t45 * t63; + const auto t362 = t359 * t63; + const auto t363 = t103 * t23; + const auto t364 = -t107; + const auto t365 = t350 * (-t166 * t364 + t170 * t363 + t172 * t351); + const auto t366 = 1 - t106; + const auto t367 = t166 * t365 + t366; + const auto t368 = -t118; + const auto t369 = t350 * (t166 * t363 - t170 * t368 + t172 * t352); + const auto t370 = t121 + t170 * t369; + const auto t371 = -t166 * t369 + t363; + const auto t372 = t35 * t46 + t36 * t43 + t37 * t45; + const auto t373 = 1.0 / t82; + const auto t374 = t372 * t373; + const auto t375 = -t374 * t77 + t45; + const auto t376 = 1.0 / t38; + const auto t377 = t372 * t376; + const auto t378 = 1.0 / t207; + const auto t379 = t104 * t35 + t107 * t37 + t36 * t45; + const auto t380 = t376 * t379; + const auto t381 = -t36 * t380 + t45; + const auto t382 = t209 * t69; + const auto t383 = t104 * t37 + t118 * t35 + t36 * t46; + const auto t384 = t373 * t383; + const auto t385 = t376 * t383; + const auto t386 = t39 * t67; + const auto t387 = + t121 + t137 * t378 * (t130 * t363 - t137 * t368 + t140 * t352); + grad[0] = t102 + * (-t0 * (t40 * t50 - t50 * t67 + t69 * t71 - t71 * t72) + + t1 * (t84 * t87 - t87 * t93 + t94 * t96 - t96 * t97) + + t2 * (-t100 * t96 - t84 * t98 + t93 * t98 + t96 * t99)); + grad[1] = t102 + * (-t0 * (t110 * t69 - t110 * t72 + t111 * t40 - t111 * t67) + + t1 * (t114 * t94 - t114 * t97 + t116 * t84 - t116 * t93) + + t2 * (-t100 * t114 + t114 * t99 - t115 * t84 + t115 * t93)); + grad[2] = t102 + * (-t0 * (t122 * t69 - t122 * t72 + t123 * t40 - t123 * t67) + + t1 * (t127 * t94 - t127 * t97 + t128 * t84 - t128 * t93) + + t2 * (-t100 * t127 - t126 * t84 + t126 * t93 + t127 * t99)); + grad[3] = t101 + * (-t0 * (t164 * t69 - t177 * t72 + t179 * t40 - t180 * t67) + + t1 * (t179 * t84 - t180 * t93 + t182 * t97 - t183 * t94) - t191 + + t2 * (t100 * t183 + t164 * t93 - t177 * t84 - t182 * t99)); + grad[4] = t101 + * (-t0 + * (t197 * (-t137 * t208 + t203 * t209 + t216) + + t218 * (-t172 * t225 + t196 * t221 + t227) + - t228 * (-t170 * t225 + t196 * t222 + t216) + - t229 * (-t140 * t208 + t202 * t209 + t227)) + + t1 + * (-t230 * (t202 * t39 + t231 * t36 + t240) + + t235 * (t221 * t66 + t236 * t63 + t240) - t238 * t72 + + t239 * t69) + - t104 * (-t230 * t72 + t235 * t69) + + t2 + * (t230 * (t203 * t39 + t231 * t35 + t234) + - t235 * (t222 * t66 + t234 + t236 * t62) + t238 * t40 + - t239 * t67) + + t241 * t4 * t7 - t241 - t45 * (t230 * t40 - t235 * t67)); + grad[5] = t101 + * (-t0 * (t256 * t40 - t263 * t67 + t265 * t69 - t266 * t72) + + t1 * (t256 * t84 - t263 * t93 + t268 * t97 - t269 * t94) + + t2 * (t100 * t269 + t265 * t93 - t266 * t84 - t268 * t99) + - t270); + grad[6] = t101 + * (-t135 * (-t283 * t69 + t292 * t72 - t295 * t40 + t296 * t67) + + t138 * (t291 * t97 - t293 * t94 - t295 * t84 + t296 * t93) + + t141 * (t100 * t293 - t283 * t93 - t291 * t99 + t292 * t84) + + t191); + grad[7] = t101 + * (t104 * t189 - t112 * t190 + - t135 * (-t309 * t69 + t316 * t72 - t321 * t40 + t322 * t67) + + t138 * (t318 * t97 - t319 * t94 - t321 * t84 + t322 * t93) + + t141 * (t100 * t319 - t309 * t93 + t316 * t84 - t318 * t99) + + t186 * t45 + t190); + grad[8] = t101 + * (-t135 * (-t344 * t69 + t345 * t72 - t347 * t40 + t348 * t67) + + t138 * (t335 * t97 - t342 * t94 - t347 * t84 + t348 * t93) + + t141 * (t100 * t342 - t335 * t99 - t344 * t93 + t345 * t84) + + t270); + grad[9] = t101 + * (t0 + * (t196 * t355 * t40 + + t66 * t72 + * (-t360 * (t43 * t63 + t45 * t64 + t46 * t62) + t46)) + - t357 * (t228 * t356 + t349 * t355) + - t358 * (-t218 * t356 + t349 * (-t170 * t354 + t352))); + grad[10] = t101 + * (t0 * t66 + * (-t40 * (-t361 * t362 + t45) + t72 * (t104 - t360 * t361)) + + t357 * (t228 * t367 + t349 * (-t172 * t365 + t351)) + - t358 * (t218 * t367 + t349 * (-t170 * t365 + t363))); + grad[11] = t101 + * (-t0 + * (t196 * t370 * t72 + + t40 * t66 + * (-t362 * (t104 * t64 + t118 * t62 + t46 * t63) + + t46)) + + t196 * t2 * (t218 * t371 + t349 * t370) + - t357 * (t228 * t371 - t349 * (-t172 * t369 + t352))); + grad[12] = t101 + * (-t0 + * (t209 * t67 + * (t140 * t378 + * (t130 * t351 + t137 * t352 - t140 * t353) + + t49) + + t39 * t69 * (-t35 * t377 + t46)) + + t1 * (t375 * t83 * t94 + t39 * t93 * (t36 * t377 + t49)) + + t2 * t83 * (-t100 * t375 + t93 * (-t374 * t81 + t46))); + grad[13] = t101 + * (-t0 * t39 * (-t381 * t67 + t69 * (t104 - t35 * t380)) + - t1 + * (t230 * t381 * t39 + + t382 + * (t130 * t378 + * (-t130 * t364 + t137 * t363 + t140 * t351) + + t366)) + + t2 + * (t100 * t39 * (t366 + t37 * t380) + + t83 * t93 * (t104 - t373 * t379 * t81))); + grad[14] = t101 + * (t0 * (t382 * t387 + t386 * (-t36 * t385 + t46)) + + t1 * t83 + * (-t93 * (-t384 * t79 + t46) + t94 * (t104 - t384 * t77)) + - t2 * (t209 * t230 * t387 + t386 * (t104 - t37 * t385))); + } + + // hess is (225×1) flattened in column-major order + void edge_normal_term_hessian( + double d_x, + double d_y, + double d_z, + double e0_x, + double e0_y, + double e0_z, + double e1_x, + double e1_y, + double e1_z, + double f0_x, + double f0_y, + double f0_z, + double f1_x, + double f1_y, + double f1_z, + double hess[225]) + { + const auto t0 = -e1_y; + const auto t1 = e0_y + t0; + const auto t2 = -e1_x; + const auto t3 = e0_x + t2; + const auto t4 = d_x * t3; + const auto t5 = d_y * t1; + const auto t6 = -e1_z; + const auto t7 = e0_z + t6; + const auto t8 = d_z * t7; + const auto t9 = t5 + t8; + const auto t10 = t4 + t9; + const auto t11 = std::pow(t3, 2); + const auto t12 = std::pow(t1, 2); + const auto t13 = std::pow(t7, 2); + const auto t14 = t11 + t12 + t13; + const auto t15 = 1.0 / t14; + const auto t16 = t15 * t3; + const auto t17 = t10 * t16; + const auto t18 = d_x - t17; + const auto t19 = t1 * t15; + const auto t20 = t10 * t19; + const auto t21 = d_y - t20; + const auto t22 = t15 * t7; + const auto t23 = t10 * t22; + const auto t24 = d_z - t23; + const auto t25 = std::pow(t18, 2) + std::pow(t21, 2) + std::pow(t24, 2); + const auto t26 = std::pow(t25, -1.0 / 2.0); + const auto t27 = t18 * t26; + const auto t28 = -t3; + const auto t29 = std::pow(t28, 2); + const auto t30 = -t1; + const auto t31 = std::pow(t30, 2); + const auto t32 = -t7; + const auto t33 = std::pow(t32, 2); + const auto t34 = t29 + t31 + t33; + const auto t35 = 1.0 / t34; + const auto t36 = e0_x - f0_x; + const auto t37 = -t36; + const auto t38 = t3 * t37; + const auto t39 = e0_y - f0_y; + const auto t40 = -t39; + const auto t41 = t1 * t40; + const auto t42 = e0_z - f0_z; + const auto t43 = -t42; + const auto t44 = t43 * t7; + const auto t45 = t38 + t41 + t44; + const auto t46 = t35 * t45; + const auto t47 = t3 * t46; + const auto t48 = t36 + t47; + const auto t49 = t1 * t46; + const auto t50 = t39 + t49; + const auto t51 = t46 * t7; + const auto t52 = t42 + t51; + const auto t53 = std::pow(t48, 2) + std::pow(t50, 2) + std::pow(t52, 2); + const auto t54 = std::pow(t53, -1.0 / 2.0); + const auto t55 = t27 + t48 * t54; + const auto t56 = t11 * t35; + const auto t57 = t56 - 1; + const auto t58 = -t57; + const auto t59 = std::pow(t34, -2); + const auto t60 = t11 * t59; + const auto t61 = t12 * t60; + const auto t62 = t13 * t60; + const auto t63 = std::pow(t58, 2) + t61 + t62; + const auto t64 = t19 * t3; + const auto t65 = t21 * t64; + const auto t66 = t22 * t3; + const auto t67 = t24 * t66; + const auto t68 = t65 + t67; + const auto t69 = t18 * t57 + t68; + const auto t70 = t66 * t69; + const auto t71 = 3 * std::pow(t69, 2); + const auto t72 = 1.0 / t25; + const auto t73 = t24 * t72; + const auto t74 = t24 * t63 + 2 * t70 - t71 * t73; + const auto t75 = -t74; + const auto t76 = t24 * t26; + const auto t77 = e0_x - f1_x; + const auto t78 = -t77; + const auto t79 = t3 * t78; + const auto t80 = e0_y - f1_y; + const auto t81 = -t80; + const auto t82 = t1 * t81; + const auto t83 = e0_z - f1_z; + const auto t84 = -t83; + const auto t85 = t7 * t84; + const auto t86 = t79 + t82 + t85; + const auto t87 = t35 * t86; + const auto t88 = t7 * t87; + const auto t89 = t83 + t88; + const auto t90 = t3 * t87; + const auto t91 = t77 + t90; + const auto t92 = t1 * t87; + const auto t93 = t80 + t92; + const auto t94 = std::pow(t89, 2) + std::pow(t91, 2) + std::pow(t93, 2); + const auto t95 = std::pow(t94, -1.0 / 2.0); + const auto t96 = t76 + t89 * t95; + const auto t97 = 2 * t57; + const auto t98 = t18 * t72; + const auto t99 = t18 * t63 + t69 * t97 - t71 * t98; + const auto t100 = -t99; + const auto t101 = t27 + t91 * t95; + const auto t102 = t52 * t54 + t76; + const auto t103 = t21 * t26; + const auto t104 = t3 * t36; + const auto t105 = t1 * t39; + const auto t106 = t42 * t7; + const auto t107 = t104 + t105 + t106; + const auto t108 = t107 * t19; + const auto t109 = -e0_y; + const auto t110 = f0_y + t109; + const auto t111 = t108 + t110; + const auto t112 = -t111; + const auto t113 = t107 * t15; + const auto t114 = t113 * t3; + const auto t115 = -e0_x; + const auto t116 = f0_x + t115; + const auto t117 = t114 + t116; + const auto t118 = -t117; + const auto t119 = t107 * t22; + const auto t120 = -e0_z; + const auto t121 = f0_z + t120; + const auto t122 = t119 + t121; + const auto t123 = -t122; + const auto t124 = + std::pow(t112, 2) + std::pow(t118, 2) + std::pow(t123, 2); + const auto t125 = std::pow(t124, -1.0 / 2.0); + const auto t126 = t103 + t112 * t125; + const auto t127 = t3 * t77; + const auto t128 = t1 * t80; + const auto t129 = t7 * t83; + const auto t130 = t127 + t128 + t129; + const auto t131 = t130 * t22; + const auto t132 = f1_z + t120; + const auto t133 = t131 + t132; + const auto t134 = -t133; + const auto t135 = t130 * t15; + const auto t136 = t135 * t3; + const auto t137 = f1_x + t115; + const auto t138 = t136 + t137; + const auto t139 = -t138; + const auto t140 = t130 * t19; + const auto t141 = f1_y + t109; + const auto t142 = t140 + t141; + const auto t143 = -t142; + const auto t144 = + std::pow(t134, 2) + std::pow(t139, 2) + std::pow(t143, 2); + const auto t145 = std::pow(t144, -1.0 / 2.0); + const auto t146 = t134 * t145 + t76; + const auto t147 = t64 * t69; + const auto t148 = t21 * t72; + const auto t149 = 2 * t147 - t148 * t71 + t21 * t63; + const auto t150 = t103 + t143 * t145; + const auto t151 = t123 * t125 + t76; + const auto t152 = t118 * t125 + t27; + const auto t153 = t139 * t145 + t27; + const auto t154 = std::pow(t14, -1.0 / 2.0); + const auto t155 = std::pow(t25, -3.0 / 2.0); + const auto t156 = t154 * t155; + const auto t157 = t13 * t15; + const auto t158 = t12 * t35; + const auto t159 = t56 - 2; + const auto t160 = t158 + t159; + const auto t161 = t157 + t160; + const auto t162 = t161 * t64; + const auto t163 = t162 * t24; + const auto t164 = t158 - 1; + const auto t165 = t18 * t64; + const auto t166 = t1 * t22; + const auto t167 = t166 * t24; + const auto t168 = t165 + t167; + const auto t169 = t164 * t21 + t168; + const auto t170 = t169 * t66; + const auto t171 = + -t163 - t166 * t69 + 3 * t169 * t24 * t69 * t72 - t170; + const auto t172 = t161 * t165; + const auto t173 = + -t147 + 3 * t169 * t18 * t69 * t72 - t169 * t57 - t172; + const auto t174 = t12 * t15; + const auto t175 = t174 - 1; + const auto t176 = t168 + t175 * t21; + const auto t177 = t176 * t66; + const auto t178 = t11 * t15; + const auto t179 = t178 - 1; + const auto t180 = t179 * t18 + t68; + const auto t181 = t166 * t180; + const auto t182 = -t163 + 3 * t176 * t180 * t24 * t72 - t177 - t181; + const auto t183 = -t161 * t65 - t175 * t180 + + 3 * t176 * t180 * t21 * t72 - t176 * t64; + const auto t184 = + -t172 - t176 * t179 + 3 * t176 * t18 * t180 * t72 - t180 * t64; + const auto t185 = t156 + * (-t1 * (-t101 * t171 - t102 * t173 + t171 * t55 + t173 * t96) + + t3 * (t126 * t182 + t146 * t183 - t150 * t182 - t151 * t183) + + t7 * (-t126 * t184 + t150 * t184 + t152 * t183 - t153 * t183)); + const auto t186 = t13 * t35; + const auto t187 = t159 + t174 + t186; + const auto t188 = t187 * t67; + const auto t189 = t186 - 1; + const auto t190 = t18 * t66; + const auto t191 = t1 * t21; + const auto t192 = t191 * t22; + const auto t193 = t190 + t192; + const auto t194 = t189 * t24 + t193; + const auto t195 = t194 * t66; + const auto t196 = + -t188 - t189 * t69 + 3 * t194 * t24 * t69 * t72 - t195; + const auto t197 = t187 * t190; + const auto t198 = 3 * t18 * t194 * t69 * t72 - t194 * t57 - t197 - t70; + const auto t199 = t157 - 1; + const auto t200 = t193 + t199 * t24; + const auto t201 = + -t180 * t199 + 3 * t180 * t200 * t24 * t72 - t188 - t200 * t66; + const auto t202 = t187 * t66; + const auto t203 = t200 * t64; + const auto t204 = + 3 * t180 * t200 * t21 * t72 - t181 - t202 * t21 - t203; + const auto t205 = + -t179 * t200 + 3 * t18 * t180 * t200 * t72 - t180 * t66 - t197; + const auto t206 = t156 + * (-t1 * (-t101 * t196 - t102 * t198 + t196 * t55 + t198 * t96) + + t3 * (t126 * t201 + t146 * t204 - t150 * t201 - t151 * t204) + + t7 * (-t126 * t205 + t150 * t205 + t152 * t204 - t153 * t204)); + const auto t207 = t3 * t35; + const auto t208 = t207 * t28; + const auto t209 = 2 * t208; + const auto t210 = t209 + 1; + const auto t211 = t210 * t7; + const auto t212 = t10 * t35; + const auto t213 = t212 * t28; + const auto t214 = t28 * t35; + const auto t215 = t214 * t4; + const auto t216 = t214 * t5; + const auto t217 = t214 * t8; + const auto t218 = d_x + t215 + t216 + t217; + const auto t219 = t213 + t218; + const auto t220 = t69 * t72; + const auto t221 = t219 * t220; + const auto t222 = t1 * t35; + const auto t223 = t10 * t222; + const auto t224 = -t223; + const auto t225 = d_y + t224; + const auto t226 = t1 * t225; + const auto t227 = t35 * t7; + const auto t228 = t10 * t227; + const auto t229 = -t228; + const auto t230 = d_z + t229; + const auto t231 = t230 * t7; + const auto t232 = t10 * t207; + const auto t233 = -t232; + const auto t234 = d_x + t233; + const auto t235 = t218 * t3; + const auto t236 = t232 * t28; + const auto t237 = t10 + t235 + t236; + const auto t238 = t219 * t226 + t219 * t231 + t234 * t237; + const auto t239 = t238 * t72; + const auto t240 = t234 * t3; + const auto t241 = t208 + 1; + const auto t242 = 2 * t241; + const auto t243 = t1 * t210; + const auto t244 = t72 + * (t12 * t219 * t3 * t35 + t13 * t219 * t3 * t35 - t211 * t230 + - t225 * t243 - t237 * t58 - t240 * t242); + const auto t245 = std::pow(t25, -2); + const auto t246 = t24 * t245; + const auto t247 = 3 * t238; + const auto t248 = t247 * t69; + const auto t249 = + t211 + t221 * t7 + t239 * t66 + t24 * t244 - t246 * t248; + const auto t250 = -t249 * t35; + const auto t251 = 2 * t3; + const auto t252 = t220 * t237; + const auto t253 = t57 * t72; + const auto t254 = t238 * t253; + const auto t255 = t245 * t248; + const auto t256 = t18 * t244 - t18 * t255 + t241 * t251 + t252 + t254; + const auto t257 = -t256 * t35; + const auto t258 = -t66; + const auto t259 = t220 * t24 + t258; + const auto t260 = t35 * t38; + const auto t261 = t260 * t28; + const auto t262 = t35 * t41; + const auto t263 = t262 * t28; + const auto t264 = t35 * t44; + const auto t265 = t264 * t28; + const auto t266 = 2 * e0_x; + const auto t267 = -t266; + const auto t268 = e1_x + t267; + const auto t269 = f0_x + t268; + const auto t270 = t261 + t263 + t265 + t269; + const auto t271 = t207 * t270; + const auto t272 = t45 * t59; + const auto t273 = t28 * t3; + const auto t274 = t272 * t273; + const auto t275 = t46 + 1; + const auto t276 = t271 + t274 + t275; + const auto t277 = std::pow(t53, -3.0 / 2.0); + const auto t278 = t28 * t46; + const auto t279 = t270 + t278; + const auto t280 = -t50; + const auto t281 = t222 * t280; + const auto t282 = -t52; + const auto t283 = t227 * t282; + const auto t284 = -t48; + const auto t285 = t276 * t284 + t279 * t281 + t279 * t283; + const auto t286 = t277 * t285; + const auto t287 = t18 * t35; + const auto t288 = t155 * t238; + const auto t289 = -t237 * t26 * t35 + t287 * t288; + const auto t290 = t276 * t54 + t286 * t48 + t289; + const auto t291 = 1 - t56; + const auto t292 = t18 * t220 + t291; + const auto t293 = std::pow(t94, -3.0 / 2.0); + const auto t294 = t28 * t87; + const auto t295 = t35 * t79; + const auto t296 = t28 * t295; + const auto t297 = t35 * t82; + const auto t298 = t28 * t297; + const auto t299 = t35 * t85; + const auto t300 = t28 * t299; + const auto t301 = f1_x + t268; + const auto t302 = t296 + t298 + t300 + t301; + const auto t303 = t294 + t302; + const auto t304 = -t93; + const auto t305 = t222 * t304; + const auto t306 = -t89; + const auto t307 = t227 * t306; + const auto t308 = -t91; + const auto t309 = t207 * t302; + const auto t310 = t59 * t86; + const auto t311 = t273 * t310; + const auto t312 = t87 + 1; + const auto t313 = t309 + t311 + t312; + const auto t314 = t303 * t305 + t303 * t307 + t308 * t313; + const auto t315 = t293 * t314; + const auto t316 = t303 * t95; + const auto t317 = t24 * t35; + const auto t318 = -t219 * t26 * t35 * t7 + t288 * t317; + const auto t319 = t227 * t316 + t315 * t89 + t318; + const auto t320 = t289 + t313 * t95 + t315 * t91; + const auto t321 = t279 * t54; + const auto t322 = t227 * t321 + t286 * t52 + t318; + const auto t323 = t259 * t290 - t259 * t320 + t292 * t319 - t292 * t322; + const auto t324 = + t1 * t221 + t21 * t244 - t21 * t255 + t239 * t64 + t243; + const auto t325 = t324 * t35; + const auto t326 = -t64; + const auto t327 = t180 * t72; + const auto t328 = t21 * t327 + t326; + const auto t329 = -t319; + const auto t330 = t24 * t327 + t258; + const auto t331 = t21 * t35; + const auto t332 = -t1 * t219 * t26 * t35 + t288 * t331; + const auto t333 = t222 * t321 + t286 * t50 + t332; + const auto t334 = -t333; + const auto t335 = -t322; + const auto t336 = t222 * t316 + t315 * t93 + t332; + const auto t337 = -t336; + const auto t338 = t328 * t329 - t328 * t335 + t330 * t334 - t330 * t337; + const auto t339 = -t290; + const auto t340 = 1 - t178; + const auto t341 = t18 * t327 + t340; + const auto t342 = -t320; + const auto t343 = t328 * t339 - t328 * t342 - t334 * t341 + t337 * t341; + const auto t344 = t150 * t330; + const auto t345 = t151 * t328; + const auto t346 = t126 * t330; + const auto t347 = t146 * t328; + const auto t348 = -t344 - t345 + t346 + t347; + const auto t349 = t150 * t341; + const auto t350 = t126 * t341; + const auto t351 = t152 * t328; + const auto t352 = t153 * t328; + const auto t353 = t349 - t350 + t351 - t352; + const auto t354 = -t101 * t259 - t102 * t292 + t259 * t55 + t292 * t96; + const auto t355 = + t178 * t348 + t344 + t345 - t346 - t347 + t353 * t66 - t354 * t64; + const auto t356 = t154 * t26; + const auto t357 = + std::pow(t225, 2) + std::pow(t230, 2) + std::pow(t234, 2); + const auto t358 = std::pow(t357, -1.0 / 2.0); + const auto t359 = t234 * t358; + const auto t360 = + std::pow(t280, 2) + std::pow(t282, 2) + std::pow(t284, 2); + const auto t361 = std::pow(t360, -1.0 / 2.0); + const auto t362 = -t284 * t361 + t359; + const auto t363 = 1.0 / t357; + const auto t364 = t222 * t30; + const auto t365 = 2 * t364; + const auto t366 = t365 + 1; + const auto t367 = t30 * t35; + const auto t368 = t367 * t4; + const auto t369 = t367 * t5; + const auto t370 = t367 * t8; + const auto t371 = d_y + t368 + t369 + t370; + const auto t372 = t1 * t371; + const auto t373 = t223 * t30; + const auto t374 = t10 + t372 + t373; + const auto t375 = t222 * t374; + const auto t376 = t3 * t30; + const auto t377 = 2 * t376; + const auto t378 = t234 * t377; + const auto t379 = 2 * t30; + const auto t380 = t227 * t230; + const auto t381 = t35 * t378 - t375 + t379 * t380; + const auto t382 = t225 * t366 + t381; + const auto t383 = t212 * t30; + const auto t384 = t371 + t383; + const auto t385 = t186 * t384; + const auto t386 = t384 * t58 - t385; + const auto t387 = t382 + t386; + const auto t388 = t1 * t3; + const auto t389 = t35 * t388; + const auto t390 = t3 * t7; + const auto t391 = t35 * t390; + const auto t392 = t234 * t58; + const auto t393 = t225 * t389 + t230 * t391 - t392; + const auto t394 = t363 * t393; + const auto t395 = t384 * t394; + const auto t396 = std::pow(t357, -2); + const auto t397 = t225 * t374 + t231 * t384 + t240 * t384; + const auto t398 = t363 * t397; + const auto t399 = t227 * t377 + t391 * t398; + const auto t400 = -3 * t230 * t393 * t396 * t397 + t395 * t7 + t399; + const auto t401 = t35 * (t230 * t3 * t363 * t387 - t400); + const auto t402 = t230 * t358; + const auto t403 = + std::pow(t304, 2) + std::pow(t306, 2) + std::pow(t308, 2); + const auto t404 = std::pow(t403, -1.0 / 2.0); + const auto t405 = -t306 * t404 + t402; + const auto t406 = 2 * t56; + const auto t407 = t396 * t397; + const auto t408 = 3 * t234; + const auto t409 = + -t3 * t395 - t30 * t406 + t393 * t407 * t408 + t398 * t58; + const auto t410 = t35 * (t240 * t363 * t387 + t409); + const auto t411 = -t308 * t404 + t359; + const auto t412 = -t282 * t361 + t402; + const auto t413 = -t391; + const auto t414 = t230 * t394 + t413; + const auto t415 = t30 * t46; + const auto t416 = t260 * t30; + const auto t417 = t262 * t30; + const auto t418 = t264 * t30; + const auto t419 = 2 * e0_y; + const auto t420 = -t419; + const auto t421 = e1_y + t420; + const auto t422 = f0_y + t421; + const auto t423 = t416 + t417 + t418 + t422; + const auto t424 = t415 + t423; + const auto t425 = t207 * t284; + const auto t426 = t222 * t423; + const auto t427 = t1 * t30; + const auto t428 = t272 * t427; + const auto t429 = t275 + t426 + t428; + const auto t430 = t280 * t429 + t283 * t424 + t424 * t425; + const auto t431 = std::pow(t360, -3.0 / 2.0); + const auto t432 = t430 * t431; + const auto t433 = t361 * t424; + const auto t434 = std::pow(t357, -3.0 / 2.0); + const auto t435 = t35 * t397; + const auto t436 = t434 * t435; + const auto t437 = t358 * t384; + const auto t438 = -t207 * t437 + t234 * t436; + const auto t439 = t207 * t433 - t284 * t432 + t438; + const auto t440 = t234 * t394 + t291; + const auto t441 = t30 * t87; + const auto t442 = t295 * t30; + const auto t443 = t297 * t30; + const auto t444 = t299 * t30; + const auto t445 = f1_y + t421; + const auto t446 = t442 + t443 + t444 + t445; + const auto t447 = t441 + t446; + const auto t448 = t207 * t308; + const auto t449 = t222 * t446; + const auto t450 = t310 * t427; + const auto t451 = t312 + t449 + t450; + const auto t452 = t304 * t451 + t307 * t447 + t447 * t448; + const auto t453 = std::pow(t403, -3.0 / 2.0); + const auto t454 = t452 * t453; + const auto t455 = t404 * t447; + const auto t456 = -t227 * t437 + t230 * t436; + const auto t457 = t227 * t455 - t306 * t454 + t456; + const auto t458 = t207 * t455 - t308 * t454 + t438; + const auto t459 = t227 * t433 - t282 * t432 + t456; + const auto t460 = t414 * t439 - t414 * t458 + t440 * t457 - t440 * t459; + const auto t461 = t1 * t358; + const auto t462 = t103 + t50 * t54; + const auto t463 = t35 * t374; + const auto t464 = 2 * t388; + const auto t465 = std::pow(t14, -2); + const auto t466 = t18 * t465; + const auto t467 = t464 * t466; + const auto t468 = t1 * t7; + const auto t469 = 2 * t465 * t468; + const auto t470 = t24 * t469; + const auto t471 = t467 + t470; + const auto t472 = t19 * t463 + t471; + const auto t473 = -t21 * t35 * t366 + t472; + const auto t474 = t35 * t57; + const auto t475 = t157 * t35; + const auto t476 = t384 * t474 + t384 * t475; + const auto t477 = -t473 - t476; + const auto t478 = t227 * t384; + const auto t479 = 3 * t69; + const auto t480 = t245 * t479; + const auto t481 = t317 * t397; + const auto t482 = 2 * t7; + const auto t483 = t465 * t482; + const auto t484 = t388 * t483; + const auto t485 = t397 * t72; + const auto t486 = t207 * t22; + const auto t487 = t484 - t485 * t486; + const auto t488 = -t220 * t478 + t480 * t481 + t487; + const auto t489 = t3 * t477 * t73 + t488; + const auto t490 = t3 * t366; + const auto t491 = t35 * t490; + const auto t492 = t220 * t463; + const auto t493 = t19 * t207; + const auto t494 = t485 * t493; + const auto t495 = 3 * t21 * t245 * t35 * t397 * t69 + + t21 * t3 * t477 * t72 - t491 - t492 - t494; + const auto t496 = t103 + t93 * t95; + const auto t497 = t21 * t220 + t326; + const auto t498 = t293 * t452; + const auto t499 = t447 * t95; + const auto t500 = t155 * t397; + const auto t501 = t26 * t384; + const auto t502 = -t227 * t501 + t317 * t500; + const auto t503 = t227 * t499 + t498 * t89 + t502; + const auto t504 = t277 * t430; + const auto t505 = t26 * t35; + const auto t506 = t331 * t500 - t374 * t505; + const auto t507 = t429 * t54 + t50 * t504 + t506; + const auto t508 = t424 * t54; + const auto t509 = t227 * t508 + t502 + t504 * t52; + const auto t510 = t451 * t95 + t498 * t93 + t506; + const auto t511 = t259 * t507 - t259 * t510 + t497 * t503 - t497 * t509; + const auto t512 = t18 * t3; + const auto t513 = t512 * t72; + const auto t514 = 2 * t1; + const auto t515 = t11 * t465; + const auto t516 = t514 * t515; + const auto t517 = t207 * t220; + const auto t518 = t397 * t480; + const auto t519 = -t253 * t435 + t287 * t518 - t384 * t517 + t516; + const auto t520 = t477 * t513 + t519; + const auto t521 = -t207 * t501 + t287 * t500; + const auto t522 = t207 * t508 + t48 * t504 + t521; + const auto t523 = t207 * t499 + t498 * t91 + t521; + const auto t524 = + -t292 * t507 + t292 * t510 + t497 * t522 - t497 * t523; + const auto t525 = t362 * t414 + t405 * t440 - t411 * t414 - t412 * t440; + const auto t526 = t26 * t64; + const auto t527 = t1 * t26; + const auto t528 = t22 * t527; + const auto t529 = -t12 * t15 * t358 * t525 + t358 * t525 + + t526 * (-t102 * t497 + t259 * t462 - t259 * t496 + t497 * t96) + + t528 * (-t101 * t497 - t292 * t462 + t292 * t496 + t497 * t55); + const auto t530 = t227 * t32; + const auto t531 = 2 * t530; + const auto t532 = t531 + 1; + const auto t533 = t3 * t532; + const auto t534 = t35 * t533; + const auto t535 = t32 * t35; + const auto t536 = t4 * t535; + const auto t537 = t5 * t535; + const auto t538 = t535 * t8; + const auto t539 = d_z + t536 + t537 + t538; + const auto t540 = t539 * t7; + const auto t541 = t228 * t32; + const auto t542 = t10 + t540 + t541; + const auto t543 = t35 * t542; + const auto t544 = t220 * t543; + const auto t545 = t212 * t32; + const auto t546 = t539 + t545; + const auto t547 = t226 * t546 + t230 * t542 + t240 * t546; + const auto t548 = t547 * t72; + const auto t549 = t486 * t548; + const auto t550 = t480 * t547; + const auto t551 = t317 * t550; + const auto t552 = 2 * t390; + const auto t553 = t466 * t552; + const auto t554 = t21 * t469; + const auto t555 = t553 + t554; + const auto t556 = t22 * t543 + t555; + const auto t557 = -t24 * t35 * t532 + t556; + const auto t558 = t174 * t35; + const auto t559 = t474 * t546 + t546 * t558; + const auto t560 = t557 + t559; + const auto t561 = -t560; + const auto t562 = t3 * t73; + const auto t563 = t534 + t544 + t549 - t551 - t561 * t562; + const auto t564 = -t563; + const auto t565 = t482 * t515; + const auto t566 = t253 * t35; + const auto t567 = t287 * t550 - t517 * t546 - t547 * t566 + t565; + const auto t568 = t513 * t561 + t567; + const auto t569 = t32 * t46; + const auto t570 = t260 * t32; + const auto t571 = t262 * t32; + const auto t572 = t264 * t32; + const auto t573 = 2 * e0_z; + const auto t574 = -t573; + const auto t575 = e1_z + t574; + const auto t576 = f0_z + t575; + const auto t577 = t570 + t571 + t572 + t576; + const auto t578 = t569 + t577; + const auto t579 = t227 * t577; + const auto t580 = t32 * t7; + const auto t581 = t272 * t580; + const auto t582 = t275 + t579 + t581; + const auto t583 = t281 * t578 + t282 * t582 + t425 * t578; + const auto t584 = t277 * t583; + const auto t585 = t54 * t578; + const auto t586 = t155 * t547; + const auto t587 = -t26 * t3 * t35 * t546 + t287 * t586; + const auto t588 = t207 * t585 + t48 * t584 + t587; + const auto t589 = t295 * t32; + const auto t590 = t297 * t32; + const auto t591 = t299 * t32; + const auto t592 = f1_z + t575; + const auto t593 = t589 + t590 + t591 + t592; + const auto t594 = t227 * t593; + const auto t595 = t310 * t580; + const auto t596 = t312 + t594 + t595; + const auto t597 = t32 * t87; + const auto t598 = t593 + t597; + const auto t599 = t305 * t598 + t306 * t596 + t448 * t598; + const auto t600 = t293 * t599; + const auto t601 = -t26 * t35 * t542 + t317 * t586; + const auto t602 = t596 * t95 + t600 * t89 + t601; + const auto t603 = t598 * t95; + const auto t604 = t207 * t603 + t587 + t600 * t91; + const auto t605 = t52 * t584 + t54 * t582 + t601; + const auto t606 = t259 * t588 - t259 * t604 + t292 * t602 - t292 * t605; + const auto t607 = t148 * t3; + const auto t608 = -d_z; + const auto t609 = d_z * t157; + const auto t610 = t22 * t4; + const auto t611 = t22 * t5; + const auto t612 = t608 + t609 + t610 + t611; + const auto t613 = t23 + t612; + const auto t614 = -t613; + const auto t615 = t327 * t614; + const auto t616 = t191 * t546 + t24 * t542 + t512 * t546; + const auto t617 = -t484; + const auto t618 = t493 * t616 * t72 + t617; + const auto t619 = + -3 * t180 * t21 * t245 * t35 * t616 + t19 * t615 + t618; + const auto t620 = -t560 * t607 - t619; + const auto t621 = -t602; + const auto t622 = -t1 * t26 * t35 * t546 + t331 * t586; + const auto t623 = t222 * t585 + t50 * t584 + t622; + const auto t624 = -t623; + const auto t625 = -t605; + const auto t626 = t222 * t603 + t600 * t93 + t622; + const auto t627 = -t626; + const auto t628 = t328 * t621 - t328 * t625 + t330 * t624 - t330 * t627; + const auto t629 = -t565; + const auto t630 = t35 * t72; + const auto t631 = t616 * t630; + const auto t632 = t16 * t615 + t179 * t631 + - 3 * t18 * t180 * t245 * t35 * t616 + t629; + const auto t633 = -t513 * t560 - t632; + const auto t634 = -t604; + const auto t635 = -t588; + const auto t636 = t328 * t634 - t328 * t635 + t341 * t624 - t341 * t627; + const auto t637 = + t157 * t353 - t166 * t354 + t348 * t66 - t349 + t350 - t351 + t352; + const auto t638 = t207 * t82 + t207 * t85 + t56 * t78; + const auto t639 = t638 + t91; + const auto t640 = t1 * t304; + const auto t641 = t306 * t7; + const auto t642 = t638 + t77; + const auto t643 = t3 * t642; + const auto t644 = t56 * t86; + const auto t645 = -t643 - t644 + t86; + const auto t646 = -t308 * t645 + t639 * t640 + t639 * t641; + const auto t647 = -t646; + const auto t648 = t293 * t647; + const auto t649 = t639 * t95; + const auto t650 = -d_x; + const auto t651 = d_x * t56; + const auto t652 = t207 * t5; + const auto t653 = t207 * t8; + const auto t654 = t651 + t652 + t653; + const auto t655 = t650 + t654; + const auto t656 = t232 + t655; + const auto t657 = t3 * t655; + const auto t658 = t10 * t56; + const auto t659 = t10 - t657 - t658; + const auto t660 = t226 * t656 + t231 * t656 - t234 * t659; + const auto t661 = -t660; + const auto t662 = t155 * t661; + const auto t663 = t26 * t7; + const auto t664 = t24 * t662 + t656 * t663; + const auto t665 = t648 * t89 - t649 * t7 + t664; + const auto t666 = t328 * t665; + const auto t667 = t207 * t41 + t207 * t44 + t37 * t56; + const auto t668 = t48 + t667; + const auto t669 = t1 * t280; + const auto t670 = t282 * t7; + const auto t671 = t36 + t667; + const auto t672 = t3 * t671; + const auto t673 = t45 * t56; + const auto t674 = t45 - t672 - t673; + const auto t675 = -t284 * t674 + t668 * t669 + t668 * t670; + const auto t676 = -t675; + const auto t677 = t277 * t676; + const auto t678 = t54 * t668; + const auto t679 = t21 * t662 + t527 * t656; + const auto t680 = -t1 * t678 + t50 * t677 + t679; + const auto t681 = t330 * t680; + const auto t682 = t52 * t677 + t664 - t678 * t7; + const auto t683 = -t1 * t649 + t648 * t93 + t679; + const auto t684 = t406 - 1; + const auto t685 = t684 * t7; + const auto t686 = t220 * t656; + const auto t687 = t661 * t72; + const auto t688 = t1 * t684; + const auto t689 = t3 * t656; + const auto t690 = t158 * t689 + t186 * t689 + t58 * t659; + const auto t691 = + t72 * (-t225 * t688 - t230 * t685 + t251 * t392 + t690); + const auto t692 = t479 * t661; + const auto t693 = + t24 * t691 + t246 * t692 - t66 * t687 + t685 + t686 * t7; + const auto t694 = t245 * t692; + const auto t695 = + t1 * t686 + t21 * t691 + t21 * t694 - t64 * t687 + t688; + const auto t696 = t341 * t683; + const auto t697 = -t674; + const auto t698 = -t659; + const auto t699 = t18 * t662 + t26 * t698; + const auto t700 = t48 * t677 - t54 * t697 + t699; + const auto t701 = t328 * t700; + const auto t702 = -t645; + const auto t703 = t648 * t91 + t699 - t702 * t95; + const auto t704 = t3 * t97; + const auto t705 = t18 * t694 + t220 * t698 - t253 * t661; + const auto t706 = t18 * t691 + t704 + t705; + const auto t707 = -t665; + const auto t708 = t292 * t707; + const auto t709 = -t682; + const auto t710 = t292 * t709; + const auto t711 = -t700; + const auto t712 = t259 * t711; + const auto t713 = -t703; + const auto t714 = t259 * t713; + const auto t715 = -t693; + const auto t716 = -t706; + const auto t717 = -d_y; + const auto t718 = d_y * t158; + const auto t719 = t222 * t4; + const auto t720 = t222 * t8; + const auto t721 = t718 + t719 + t720; + const auto t722 = t717 + t721; + const auto t723 = t223 + t722; + const auto t724 = t474 * t723; + const auto t725 = t475 * t723; + const auto t726 = 2 * t158; + const auto t727 = t726 - 1; + const auto t728 = -t1 * t722 - t10 * t12 * t35 + t10; + const auto t729 = -t728; + const auto t730 = t35 * t729; + const auto t731 = t19 * t730; + const auto t732 = t331 * t727 + t471 - t731; + const auto t733 = -t724 - t725 + t732; + const auto t734 = t227 * t723; + const auto t735 = -t225 * t728 + t231 * t723 + t240 * t723; + const auto t736 = -t735; + const auto t737 = t480 * t736; + const auto t738 = t72 * t736; + const auto t739 = t484 - t486 * t738; + const auto t740 = t220 * t734 + t317 * t737 + t739; + const auto t741 = -t562 * t733 + t740; + const auto t742 = t3 * t727; + const auto t743 = t35 * t742; + const auto t744 = t493 * t738; + const auto t745 = -t744; + const auto t746 = t220 * t730 + t331 * t737 - t607 * t733 + t743 + t745; + const auto t747 = t222 * t79; + const auto t748 = t222 * t85; + const auto t749 = t158 * t81 + t747 + t748; + const auto t750 = t749 + t93; + const auto t751 = t3 * t308; + const auto t752 = t749 + t80; + const auto t753 = -t1 * t752 - t12 * t35 * t86 + t86; + const auto t754 = -t304 * t753 + t641 * t750 + t750 * t751; + const auto t755 = -t754; + const auto t756 = t293 * t755; + const auto t757 = t750 * t95; + const auto t758 = t155 * t736; + const auto t759 = t24 * t758 + t663 * t723; + const auto t760 = -t7 * t757 + t756 * t89 + t759; + const auto t761 = t328 * t35; + const auto t762 = t222 * t38; + const auto t763 = t222 * t44; + const auto t764 = t158 * t40 + t762 + t763; + const auto t765 = t39 + t764; + const auto t766 = t1 * t765; + const auto t767 = t158 * t45; + const auto t768 = t45 - t766 - t767; + const auto t769 = -t768; + const auto t770 = t50 + t764; + const auto t771 = t284 * t3; + const auto t772 = -t280 * t768 + t670 * t770 + t770 * t771; + const auto t773 = -t772; + const auto t774 = t277 * t773; + const auto t775 = t21 * t758 + t26 * t729; + const auto t776 = t50 * t774 - t54 * t769 + t775; + const auto t777 = t330 * t35; + const auto t778 = t54 * t770; + const auto t779 = t52 * t774 - t7 * t778 + t759; + const auto t780 = -t753; + const auto t781 = t756 * t93 + t775 - t780 * t95; + const auto t782 = + -t328 * t35 * t779 - t330 * t35 * t781 + t760 * t761 + t776 * t777; + const auto t783 = t287 * t737 + t516 + t517 * t723 - t566 * t736; + const auto t784 = -t513 * t733 + t783; + const auto t785 = t26 * t3; + const auto t786 = t18 * t758 + t723 * t785; + const auto t787 = -t3 * t778 + t48 * t774 + t786; + const auto t788 = t341 * t35; + const auto t789 = -t3 * t757 + t756 * t91 + t786; + const auto t790 = + -t328 * t35 * t789 - t341 * t35 * t776 + t761 * t787 + t781 * t788; + const auto t791 = -t741; + const auto t792 = -t784; + const auto t793 = -t787; + const auto t794 = t259 * t35; + const auto t795 = -t760; + const auto t796 = t292 * t35; + const auto t797 = -t789; + const auto t798 = -t779; + const auto t799 = t793 * t794 - t794 * t797 + t795 * t796 - t796 * t798; + const auto t800 = t166 * t353 - t174 * t354 + t348 * t64 + t354; + const auto t801 = 2 * t186; + const auto t802 = t801 - 1; + const auto t803 = t3 * t802; + const auto t804 = d_z * t186; + const auto t805 = t227 * t4; + const auto t806 = t227 * t5; + const auto t807 = t804 + t805 + t806; + const auto t808 = t608 + t807; + const auto t809 = -t10 * t13 * t35 + t10 - t7 * t808; + const auto t810 = -t809; + const auto t811 = t35 * t810; + const auto t812 = t220 * t811; + const auto t813 = t228 + t808; + const auto t814 = t226 * t813 - t230 * t809 + t240 * t813; + const auto t815 = -t814; + const auto t816 = t72 * t815; + const auto t817 = t486 * t816; + const auto t818 = -t817; + const auto t819 = t474 * t813; + const auto t820 = t558 * t813; + const auto t821 = t22 * t811; + const auto t822 = t317 * t802 + t555 - t821; + const auto t823 = -t819 - t820 + t822; + const auto t824 = t480 * t815; + const auto t825 = t317 * t824; + const auto t826 = t35 * t803 - t562 * t823 + t812 + t818 + t825; + const auto t827 = t222 * t813; + const auto t828 = t484 - t493 * t816; + const auto t829 = t220 * t827 + t331 * t824 + t828; + const auto t830 = -t607 * t823 + t829; + const auto t831 = t227 * t79; + const auto t832 = t227 * t82; + const auto t833 = t186 * t84 + t831 + t832; + const auto t834 = t83 + t833; + const auto t835 = t7 * t834; + const auto t836 = t186 * t86; + const auto t837 = -t835 - t836 + t86; + const auto t838 = -t837; + const auto t839 = t833 + t89; + const auto t840 = -t306 * t837 + t640 * t839 + t751 * t839; + const auto t841 = -t840; + const auto t842 = t293 * t841; + const auto t843 = t155 * t815; + const auto t844 = t24 * t843 + t26 * t810; + const auto t845 = -t838 * t95 + t842 * t89 + t844; + const auto t846 = t227 * t38; + const auto t847 = t227 * t41; + const auto t848 = t186 * t43 + t846 + t847; + const auto t849 = t52 + t848; + const auto t850 = t42 + t848; + const auto t851 = -t13 * t35 * t45 + t45 - t7 * t850; + const auto t852 = -t282 * t851 + t669 * t849 + t771 * t849; + const auto t853 = -t852; + const auto t854 = t277 * t853; + const auto t855 = t54 * t849; + const auto t856 = t21 * t843 + t527 * t813; + const auto t857 = -t1 * t855 + t50 * t854 + t856; + const auto t858 = -t851; + const auto t859 = t52 * t854 - t54 * t858 + t844; + const auto t860 = t839 * t95; + const auto t861 = -t1 * t860 + t842 * t93 + t856; + const auto t862 = + -t328 * t35 * t859 - t330 * t35 * t861 + t761 * t845 + t777 * t857; + const auto t863 = t287 * t824 + t517 * t813 + t565 - t566 * t815; + const auto t864 = -t513 * t823 + t863; + const auto t865 = t18 * t843 + t785 * t813; + const auto t866 = -t3 * t855 + t48 * t854 + t865; + const auto t867 = -t3 * t860 + t842 * t91 + t865; + const auto t868 = + -t328 * t35 * t867 - t341 * t35 * t857 + t761 * t866 + t788 * t861; + const auto t869 = -t826; + const auto t870 = -t864; + const auto t871 = -t866; + const auto t872 = -t845; + const auto t873 = -t867; + const auto t874 = -t859; + const auto t875 = t794 * t871 - t794 * t873 + t796 * t872 - t796 * t874; + const auto t876 = t280 * t389; + const auto t877 = t282 * t391; + const auto t878 = t284 * t58; + const auto t879 = t876 + t877 - t878; + const auto t880 = 1.0 / t360; + const auto t881 = t879 * t880; + const auto t882 = t284 * t881; + const auto t883 = t291 + t882; + const auto t884 = t282 * t881; + const auto t885 = t391 - t884; + const auto t886 = -t389; + const auto t887 = t225 * t394 + t886; + const auto t888 = t280 * t881; + const auto t889 = t389 - t888; + const auto t890 = t3 * t361; + const auto t891 = t358 * t890; + const auto t892 = 1.0 / t53; + const auto t893 = t48 * t57 + t50 * t64 + t52 * t66; + const auto t894 = t892 * t893; + const auto t895 = -t50 * t894 + t64; + const auto t896 = t54 * t895; + const auto t897 = t361 * t883; + const auto t898 = t154 + * (t1 * t358 * t361 * (t414 * t883 + t440 * t885) + - t663 * (t292 * t896 + t497 * t897) + - t891 * (-t414 * t889 + t885 * t887)); + const auto t899 = t164 * t50 + t166 * t52 + t48 * t64; + const auto t900 = 1.0 / t124; + const auto t901 = t899 * t900; + const auto t902 = -t123 * t901 + t166; + const auto t903 = -t118 * t901 + t64; + const auto t904 = t1 * t125; + const auto t905 = t892 * t899; + const auto t906 = -t48 * t905 + t64; + const auto t907 = t497 * t54; + const auto t908 = t284 * t389; + const auto t909 = t1 * t227; + const auto t910 = t282 * t909; + const auto t911 = -t164; + const auto t912 = t280 * t911; + const auto t913 = t908 + t910 - t912; + const auto t914 = t280 * t880; + const auto t915 = t913 * t914; + const auto t916 = 1 - t158; + const auto t917 = t915 + t916; + const auto t918 = t361 * t917; + const auto t919 = t166 - t52 * t905; + const auto t920 = t356 + * (-t3 * (t259 * t918 + t907 * t919) + + t7 * (t292 * t918 + t906 * t907) + + t904 * (-t330 * t903 + t341 * t902)); + const auto t921 = t284 * t391; + const auto t922 = t227 * t669; + const auto t923 = -t189; + const auto t924 = t282 * t923; + const auto t925 = t921 + t922 - t924; + const auto t926 = t880 * t925; + const auto t927 = t282 * t926; + const auto t928 = 1 - t186; + const auto t929 = t927 + t928; + const auto t930 = t909 - t914 * t925; + const auto t931 = t284 * t926; + const auto t932 = t391 - t931; + const auto t933 = t361 * t7; + const auto t934 = t358 * t933; + const auto t935 = t166 * t50 + t189 * t52 + t48 * t66; + const auto t936 = t892 * t935; + const auto t937 = -t48 * t936 + t66; + const auto t938 = t54 * t937; + const auto t939 = t361 * t929; + const auto t940 = t154 + * (t3 * t358 * t361 * (t414 * t930 + t887 * t929) + - t527 * (t259 * t938 + t292 * t939) + - t934 * (t440 * t930 - t887 * t932)); + const auto t941 = t64 * t93; + const auto t942 = t57 * t91 + t66 * t89 + t941; + const auto t943 = 1.0 / t144; + const auto t944 = t942 * t943; + const auto t945 = -t134 * t944 + t66; + const auto t946 = -t143 * t944 + t64; + const auto t947 = t145 * t3; + const auto t948 = 1.0 / t94; + const auto t949 = t942 * t948; + const auto t950 = t93 * t949; + const auto t951 = t64 - t950; + const auto t952 = t292 * t95; + const auto t953 = t304 * t389; + const auto t954 = t306 * t391; + const auto t955 = t308 * t58; + const auto t956 = t953 + t954 - t955; + const auto t957 = 1.0 / t403; + const auto t958 = t308 * t957; + const auto t959 = t956 * t958; + const auto t960 = t291 + t959; + const auto t961 = t404 * t960; + const auto t962 = t66 - t89 * t949; + const auto t963 = t356 + * (-t1 * (t259 * t961 + t952 * t962) + + t7 * (t497 * t961 + t951 * t952) + + t947 * (t328 * t945 - t330 * t946)); + const auto t964 = t308 * t389; + const auto t965 = t306 * t909; + const auto t966 = t304 * t911; + const auto t967 = t964 + t965 - t966; + const auto t968 = t957 * t967; + const auto t969 = t304 * t968; + const auto t970 = t916 + t969; + const auto t971 = t306 * t968; + const auto t972 = t909 - t971; + const auto t973 = t389 - t958 * t967; + const auto t974 = t404 * t461; + const auto t975 = t164 * t93 + t166 * t89 + t64 * t91; + const auto t976 = t91 * t948; + const auto t977 = t64 - t975 * t976; + const auto t978 = t95 * t977; + const auto t979 = t292 * t404; + const auto t980 = t154 + * (t3 * t358 * t404 * (t414 * t970 + t887 * t972) + - t663 * (t497 * t978 + t970 * t979) + - t974 * (-t414 * t973 + t440 * t972)); + const auto t981 = t66 * t91; + const auto t982 = t166 * t93 + t189 * t89 + t981; + const auto t983 = t943 * t982; + const auto t984 = -t143 * t983 + t166; + const auto t985 = -t139 * t983 + t66; + const auto t986 = t145 * t7; + const auto t987 = t66 - t976 * t982; + const auto t988 = t259 * t95; + const auto t989 = t308 * t391; + const auto t990 = t227 * t640; + const auto t991 = t306 * t923; + const auto t992 = t989 + t990 - t991; + const auto t993 = t306 * t957; + const auto t994 = t992 * t993; + const auto t995 = t928 + t994; + const auto t996 = t93 * t948; + const auto t997 = t166 - t982 * t996; + const auto t998 = t404 * t995; + const auto t999 = t356 + * (t1 * (t979 * t995 + t987 * t988) + - t3 * (t497 * t998 + t988 * t997) + + t986 * (-t328 * t985 + t341 * t984)); + const auto t1000 = t12 * t59; + const auto t1001 = t1000 * t13; + const auto t1002 = t1001 + t61 + std::pow(t911, 2); + const auto t1003 = 2 * t169; + const auto t1004 = 3 * std::pow(t169, 2); + const auto t1005 = t1002 * t24 + t1003 * t166 - t1004 * t73; + const auto t1006 = -t1005; + const auto t1007 = t1002 * t18 + t1003 * t64 - t1004 * t98; + const auto t1008 = -t1007; + const auto t1009 = 2 * t164; + const auto t1010 = t1002 * t21 - t1004 * t148 + t1009 * t169; + const auto t1011 = t158 + t178 + t186 - 2; + const auto t1012 = t1011 * t167; + const auto t1013 = t166 * t194; + const auto t1014 = + -t1012 - t1013 - t169 * t189 + 3 * t169 * t194 * t24 * t72; + const auto t1015 = t1011 * t166; + const auto t1016 = t1015 * t18; + const auto t1017 = + -t1016 + 3 * t169 * t18 * t194 * t72 - t170 - t194 * t64; + const auto t1018 = + -t1012 - t166 * t200 - t176 * t199 + 3 * t176 * t200 * t24 * t72; + const auto t1019 = -t1011 * t192 - t166 * t176 - t175 * t200 + + 3 * t176 * t200 * t21 * t72; + const auto t1020 = -t1016 + 3 * t176 * t18 * t200 * t72 - t177 - t203; + const auto t1021 = t156 + * (-t1 * (-t101 * t1014 + t1014 * t55 - t1017 * t102 + t1017 * t96) + + t3 + * (t1018 * t126 - t1018 * t150 + t1019 * t146 - t1019 * t151) + + t7 + * (t1019 * t152 - t1019 * t153 - t1020 * t126 + + t1020 * t150)); + const auto t1022 = t237 * t35; + const auto t1023 = t464 * t465; + const auto t1024 = t1023 * t21; + const auto t1025 = t465 * t552; + const auto t1026 = t1025 * t24; + const auto t1027 = t1024 + t1026; + const auto t1028 = t1022 * t16 + t1027; + const auto t1029 = t1028 - t210 * t287; + const auto t1030 = t164 * t35; + const auto t1031 = t1030 * t219 + t219 * t475; + const auto t1032 = t1029 + t1031; + const auto t1033 = -t1032; + const auto t1034 = t1 * t73; + const auto t1035 = t169 * t72; + const auto t1036 = t219 * t227; + const auto t1037 = t245 * t317; + const auto t1038 = 3 * t169; + const auto t1039 = t1038 * t238; + const auto t1040 = t22 * t222; + const auto t1041 = -t1035 * t1036 + t1037 * t1039 - t1040 * t239 + t484; + const auto t1042 = t1033 * t1034 + t1041; + const auto t1043 = t243 * t35; + const auto t1044 = t1022 * t1035; + const auto t1045 = t239 * t493; + const auto t1046 = t245 * t287; + const auto t1047 = t1039 * t1046; + const auto t1048 = t1 * t98; + const auto t1049 = -t1033 * t1048 + t1043 + t1044 + t1045 - t1047; + const auto t1050 = -t1049; + const auto t1051 = t1035 * t18 + t326; + const auto t1052 = -t166; + const auto t1053 = t1035 * t24 + t1052; + const auto t1054 = + t1051 * t319 - t1051 * t322 + t1053 * t290 - t1053 * t320; + const auto t1055 = d_x * t178; + const auto t1056 = t16 * t5; + const auto t1057 = t16 * t8; + const auto t1058 = t1055 + t1056 + t1057 + t650; + const auto t1059 = t1058 + t17; + const auto t1060 = -t1059; + const auto t1061 = t176 * t72; + const auto t1062 = t1060 * t1061; + const auto t1063 = t24 * t7; + const auto t1064 = t1063 * t219 + t18 * t237 + t191 * t219; + const auto t1065 = t1040 * t1064 * t72 + t617; + const auto t1066 = + t1062 * t22 - 3 * t1064 * t176 * t24 * t245 * t35 + t1065; + const auto t1067 = -t1032 * t1034 - t1066; + const auto t1068 = t191 * t72; + const auto t1069 = t12 * t465; + const auto t1070 = t1069 * t251; + const auto t1071 = -t1070; + const auto t1072 = t1064 * t630; + const auto t1073 = t1062 * t19 - 3 * t1064 * t176 * t21 * t245 * t35 + + t1071 + t1072 * t175; + const auto t1074 = -t1032 * t1068 - t1073; + const auto t1075 = t1052 + t1061 * t24; + const auto t1076 = 1 - t174; + const auto t1077 = t1061 * t21 + t1076; + const auto t1078 = + -t1075 * t334 + t1075 * t337 - t1077 * t329 + t1077 * t335; + const auto t1079 = t1061 * t18 + t326; + const auto t1080 = + -t1077 * t339 + t1077 * t342 + t1079 * t334 - t1079 * t337; + const auto t1081 = t1075 * t150; + const auto t1082 = t1077 * t151; + const auto t1083 = t1075 * t126; + const auto t1084 = t1077 * t146; + const auto t1085 = -t1081 - t1082 + t1083 + t1084; + const auto t1086 = t1077 * t152; + const auto t1087 = t1077 * t153; + const auto t1088 = t1079 * t150; + const auto t1089 = t1079 * t126; + const auto t1090 = t1086 - t1087 + t1088 - t1089; + const auto t1091 = + -t101 * t1053 - t102 * t1051 + t1051 * t96 + t1053 * t55; + const auto t1092 = t1081 + t1082 - t1083 - t1084 + t1085 * t178 + + t1090 * t66 - t1091 * t64; + const auto t1093 = t366 * t7; + const auto t1094 = t364 + 1; + const auto t1095 = 2 * t1094; + const auto t1096 = t384 * t56; + const auto t1097 = -t1 * t1096 - t1 * t385 + t1095 * t226 + t374 * t911; + const auto t1098 = t1093 * t230 + t1097 + t234 * t490; + const auto t1099 = t230 * t909; + const auto t1100 = t225 * t911; + const auto t1101 = t1099 - t1100 + t234 * t389; + const auto t1102 = t1101 * t363; + const auto t1103 = t1102 * t384; + const auto t1104 = + -3 * t1101 * t230 * t396 * t397 + t1103 * t7 + t398 * t909; + const auto t1105 = t35 * (-t1093 + t1098 * t230 * t363 - t1104); + const auto t1106 = + -3 * t1101 * t234 * t396 * t397 + t1103 * t3 + t389 * t398; + const auto t1107 = t35 * (t1098 * t234 * t363 - t1106 - t490); + const auto t1108 = t1102 * t234 + t886; + const auto t1109 = -t909; + const auto t1110 = t1102 * t230 + t1109; + const auto t1111 = + t1108 * t457 - t1108 * t459 + t1110 * t439 - t1110 * t458; + const auto t1112 = t1035 * t384; + const auto t1113 = -t1098 * t72; + const auto t1114 = t35 + * (-t1093 - t1112 * t7 - t1113 * t24 - t166 * t485 + + 3 * t169 * t24 * t245 * t397); + const auto t1115 = t1035 * t374; + const auto t1116 = t164 * t72; + const auto t1117 = t35 + * (-t1094 * t514 - t1113 * t21 - t1115 - t1116 * t397 + + 3 * t169 * t21 * t245 * t397); + const auto t1118 = t1035 * t21 + t916; + const auto t1119 = + t1053 * t507 - t1053 * t510 + t1118 * t503 - t1118 * t509; + const auto t1120 = t35 + * (-t1112 * t3 - t1113 * t18 + 3 * t169 * t18 * t245 * t397 + - t485 * t64 - t490); + const auto t1121 = + -t1051 * t507 + t1051 * t510 + t1118 * t522 - t1118 * t523; + const auto t1122 = + t1108 * t405 - t1108 * t412 + t1110 * t362 - t1110 * t411; + const auto t1123 = -t1122 * t12 * t15 * t358 + t1122 * t358 + + t526 * (-t102 * t1118 + t1053 * t462 - t1053 * t496 + t1118 * t96) + + t528 + * (-t101 * t1118 - t1051 * t462 + t1051 * t496 + t1118 * t55); + const auto t1124 = t1 * t532; + const auto t1125 = t1124 * t35; + const auto t1126 = t1035 * t543; + const auto t1127 = t1040 * t548; + const auto t1128 = t1038 * t547; + const auto t1129 = t1037 * t1128; + const auto t1130 = t15 * t56; + const auto t1131 = t1030 * t546 + t1130 * t546; + const auto t1132 = t1131 + t557; + const auto t1133 = -t1132; + const auto t1134 = -t1034 * t1133 + t1125 + t1126 + t1127 - t1129; + const auto t1135 = -t1134; + const auto t1136 = t1035 * t207; + const auto t1137 = t1046 * t1128 - t1136 * t546 + t484 - t493 * t548; + const auto t1138 = t1048 * t1133 + t1137; + const auto t1139 = + t1051 * t602 - t1051 * t605 + t1053 * t588 - t1053 * t604; + const auto t1140 = t1069 * t482; + const auto t1141 = -t1140; + const auto t1142 = t1061 * t614; + const auto t1143 = t1141 + t1142 * t19 + t175 * t631 + - 3 * t176 * t21 * t245 * t35 * t616; + const auto t1144 = -t1068 * t1132 - t1143; + const auto t1145 = + t1075 * t624 - t1075 * t627 + t1077 * t621 - t1077 * t625; + const auto t1146 = + t1142 * t16 - 3 * t176 * t18 * t245 * t35 * t616 + t618; + const auto t1147 = -t1048 * t1132 - t1146; + const auto t1148 = + t1077 * t634 - t1077 * t635 + t1079 * t624 - t1079 * t627; + const auto t1149 = t1085 * t66 - t1086 + t1087 - t1088 + t1089 + + t1090 * t157 - t1091 * t166; + const auto t1150 = t1030 * t656; + const auto t1151 = t475 * t656; + const auto t1152 = t35 * t698; + const auto t1153 = t1152 * t16; + const auto t1154 = t1027 - t1153 + t287 * t684; + const auto t1155 = -t1150 - t1151 + t1154; + const auto t1156 = t227 * t656; + const auto t1157 = t1038 * t661; + const auto t1158 = -t1040 * t687 + t484; + const auto t1159 = t1035 * t1156 + t1037 * t1157 + t1158; + const auto t1160 = -t1034 * t1155 + t1159; + const auto t1161 = t1030 * t72; + const auto t1162 = t222 * t656; + const auto t1163 = t245 * t331; + const auto t1164 = t1035 * t1162 + t1070 + t1157 * t1163 - t1161 * t661; + const auto t1165 = -t1068 * t1155 + t1164; + const auto t1166 = t1075 * t35; + const auto t1167 = t1077 * t35; + const auto t1168 = -t1075 * t35 * t683 - t1077 * t35 * t682 + + t1166 * t680 + t1167 * t665; + const auto t1169 = t1035 * t1152; + const auto t1170 = t493 * t687; + const auto t1171 = -t1170; + const auto t1172 = t1046 * t1157; + const auto t1173 = -t1048 * t1155 + t1169 + t1171 + t1172 + t35 * t688; + const auto t1174 = t1079 * t35; + const auto t1175 = -t1077 * t35 * t703 - t1079 * t35 * t680 + + t1167 * t700 + t1174 * t683; + const auto t1176 = -t1160; + const auto t1177 = -t1173; + const auto t1178 = t1051 * t35; + const auto t1179 = t1053 * t35; + const auto t1180 = + t1178 * t707 - t1178 * t709 + t1179 * t711 - t1179 * t713; + const auto t1181 = t1077 * t760; + const auto t1182 = t1075 * t776; + const auto t1183 = t7 * t727; + const auto t1184 = t1035 * t723; + const auto t1185 = t1 * t723; + const auto t1186 = t72 + * (t1100 * t514 - t1183 * t230 + t1185 * t186 + t1185 * t56 + - t234 * t742 + t728 * t911); + const auto t1187 = t1038 * t736; + const auto t1188 = + t1183 + t1184 * t7 + t1186 * t24 + t1187 * t246 - t166 * t738; + const auto t1189 = t1 * t1009; + const auto t1190 = t1035 * t729; + const auto t1191 = t1187 * t245; + const auto t1192 = + -t1116 * t736 + t1186 * t21 + t1189 + t1190 + t1191 * t21; + const auto t1193 = t1077 * t787; + const auto t1194 = t1079 * t781; + const auto t1195 = + t1184 * t3 + t1186 * t18 + t1191 * t18 - t64 * t738 + t742; + const auto t1196 = t1051 * t795; + const auto t1197 = t1053 * t793; + const auto t1198 = t1051 * t798; + const auto t1199 = t1053 * t797; + const auto t1200 = -t1188; + const auto t1201 = -t1195; + const auto t1202 = t1085 * t64 + t1090 * t166 - t1091 * t174 + t1091; + const auto t1203 = t1 * t802; + const auto t1204 = t1203 * t35; + const auto t1205 = t1035 * t811; + const auto t1206 = t1040 * t816; + const auto t1207 = -t1206; + const auto t1208 = t1030 * t813; + const auto t1209 = t1130 * t813; + const auto t1210 = -t1208 - t1209 + t822; + const auto t1211 = t1038 * t815; + const auto t1212 = t1037 * t1211; + const auto t1213 = -t1034 * t1210 + t1204 + t1205 + t1207 + t1212; + const auto t1214 = t1035 * t827 + t1140 - t1161 * t815 + t1163 * t1211; + const auto t1215 = -t1068 * t1210 + t1214; + const auto t1216 = -t1075 * t35 * t861 - t1077 * t35 * t859 + + t1166 * t857 + t1167 * t845; + const auto t1217 = t1046 * t1211 + t1136 * t813 + t828; + const auto t1218 = -t1048 * t1210 + t1217; + const auto t1219 = -t1077 * t35 * t867 - t1079 * t35 * t857 + + t1167 * t866 + t1174 * t861; + const auto t1220 = -t1213; + const auto t1221 = -t1218; + const auto t1222 = + t1178 * t872 - t1178 * t874 + t1179 * t871 - t1179 * t873; + const auto t1223 = t1102 * t225 + t916; + const auto t1224 = t154 + * (t1 * t358 * t361 * (t1108 * t885 + t1110 * t883) + - t663 * (t1051 * t896 + t1118 * t897) + - t891 * (-t1110 * t889 + t1223 * t885)); + const auto t1225 = t1118 * t54; + const auto t1226 = t356 + * (-t3 * (t1053 * t918 + t1225 * t919) + + t7 * (t1051 * t918 + t1225 * t906) + + t904 * (-t1075 * t903 + t1079 * t902)); + const auto t1227 = t154 + * (t3 * t358 * t361 * (t1110 * t930 + t1223 * t929) + - t527 * (t1051 * t939 + t1053 * t938) + - t934 * (t1108 * t930 - t1223 * t932)); + const auto t1228 = t1051 * t95; + const auto t1229 = t356 + * (-t1 * (t1053 * t961 + t1228 * t962) + + t7 * (t1118 * t961 + t1228 * t951) + + t947 * (-t1075 * t946 + t1077 * t945)); + const auto t1230 = t404 * t970; + const auto t1231 = t154 + * (t3 * t358 * t404 * (t1110 * t970 + t1223 * t972) + - t663 * (t1051 * t1230 + t1118 * t978) + - t974 * (t1108 * t972 - t1110 * t973)); + const auto t1232 = t1053 * t95; + const auto t1233 = t356 + * (t1 * (t1051 * t998 + t1232 * t987) + - t3 * (t1118 * t998 + t1232 * t997) + + t986 * (-t1077 * t985 + t1079 * t984)); + const auto t1234 = t1001 + t62 + std::pow(t923, 2); + const auto t1235 = 2 * t189; + const auto t1236 = 3 * std::pow(t194, 2); + const auto t1237 = t1234 * t24 + t1235 * t194 - t1236 * t73; + const auto t1238 = -t1237; + const auto t1239 = t1234 * t18 - t1236 * t98 + 2 * t195; + const auto t1240 = -t1239; + const auto t1241 = 2 * t1013 + t1234 * t21 - t1236 * t148; + const auto t1242 = t189 * t35; + const auto t1243 = t1242 * t219 + t219 * t558; + const auto t1244 = t1029 + t1243; + const auto t1245 = -t1244; + const auto t1246 = t1063 * t72; + const auto t1247 = t13 * t465; + const auto t1248 = t1247 * t251; + const auto t1249 = t1242 * t72; + const auto t1250 = t194 * t72; + const auto t1251 = 3 * t194; + const auto t1252 = t1251 * t238; + const auto t1253 = + -t1036 * t1250 + t1037 * t1252 + t1248 - t1249 * t238; + const auto t1254 = t1245 * t1246 + t1253; + const auto t1255 = t211 * t35; + const auto t1256 = t1022 * t1250; + const auto t1257 = t239 * t486; + const auto t1258 = t1046 * t1252; + const auto t1259 = t7 * t98; + const auto t1260 = -t1245 * t1259 + t1255 + t1256 + t1257 - t1258; + const auto t1261 = -t1260; + const auto t1262 = t1250 * t18 + t258; + const auto t1263 = t1250 * t24 + t928; + const auto t1264 = + t1262 * t319 - t1262 * t322 + t1263 * t290 - t1263 * t320; + const auto t1265 = -t1248; + const auto t1266 = t200 * t72; + const auto t1267 = t1060 * t1266; + const auto t1268 = -3 * t1064 * t200 * t24 * t245 * t35 + t1072 * t199 + + t1265 + t1267 * t22; + const auto t1269 = -t1244 * t1246 - t1268; + const auto t1270 = t148 * t7; + const auto t1271 = + -3 * t1064 * t200 * t21 * t245 * t35 + t1065 + t1267 * t19; + const auto t1272 = -t1244 * t1270 - t1271; + const auto t1273 = t1052 + t1266 * t21; + const auto t1274 = 1 - t157; + const auto t1275 = t1266 * t24 + t1274; + const auto t1276 = + -t1273 * t329 + t1273 * t335 - t1275 * t334 + t1275 * t337; + const auto t1277 = t1266 * t18 + t258; + const auto t1278 = + -t1273 * t339 + t1273 * t342 + t1277 * t334 - t1277 * t337; + const auto t1279 = t1275 * t150; + const auto t1280 = t1273 * t151; + const auto t1281 = t126 * t1275; + const auto t1282 = t1273 * t146; + const auto t1283 = -t1279 - t1280 + t1281 + t1282; + const auto t1284 = t1273 * t152; + const auto t1285 = t1277 * t150; + const auto t1286 = t1273 * t153; + const auto t1287 = t126 * t1277; + const auto t1288 = t1284 + t1285 - t1286 - t1287; + const auto t1289 = + -t101 * t1263 - t102 * t1262 + t1262 * t96 + t1263 * t55; + const auto t1290 = t1279 + t1280 - t1281 - t1282 + t1283 * t178 + + t1288 * t66 - t1289 * t64; + const auto t1291 = -t1096 + t384 * t923; + const auto t1292 = t1291 + t382; + const auto t1293 = t230 * t923; + const auto t1294 = -t1293 + t226 * t227 + t234 * t391; + const auto t1295 = t1294 * t363; + const auto t1296 = t1295 * t384; + const auto t1297 = 3 * t230; + const auto t1298 = + t1294 * t1297 * t407 - t1296 * t7 - t30 * t801 + t398 * t923; + const auto t1299 = t35 * (t1292 * t231 * t363 + t1298); + const auto t1300 = -3 * t1294 * t234 * t396 * t397 + t1296 * t3 + t399; + const auto t1301 = t35 * (t1292 * t234 * t363 * t7 - t1300); + const auto t1302 = t1295 * t234 + t413; + const auto t1303 = t1295 * t230 + t928; + const auto t1304 = + t1302 * t457 - t1302 * t459 + t1303 * t439 - t1303 * t458; + const auto t1305 = t1096 * t15 + t1242 * t384; + const auto t1306 = -t1305 - t473; + const auto t1307 = t1247 * t514; + const auto t1308 = + -t1249 * t397 - t1250 * t478 + t1251 * t245 * t481 + t1307; + const auto t1309 = t1246 * t1306 + t1308; + const auto t1310 = t1093 * t35; + const auto t1311 = t1250 * t463; + const auto t1312 = t1040 * t485; + const auto t1313 = t1306 * t21 * t7 * t72 - t1310 - t1311 - t1312 + + 3 * t194 * t21 * t245 * t35 * t397; + const auto t1314 = t1052 + t1250 * t21; + const auto t1315 = + t1263 * t507 - t1263 * t510 + t1314 * t503 - t1314 * t509; + const auto t1316 = t1250 * t207; + const auto t1317 = t1251 * t397; + const auto t1318 = t1046 * t1317 - t1316 * t384 + t487; + const auto t1319 = t1259 * t1306 + t1318; + const auto t1320 = + -t1262 * t507 + t1262 * t510 + t1314 * t522 - t1314 * t523; + const auto t1321 = + t1302 * t405 - t1302 * t412 + t1303 * t362 - t1303 * t411; + const auto t1322 = -t12 * t1321 * t15 * t358 + t1321 * t358 + + t526 * (-t102 * t1314 + t1263 * t462 - t1263 * t496 + t1314 * t96) + + t528 + * (-t101 * t1314 - t1262 * t462 + t1262 * t496 + t1314 * t55); + const auto t1323 = t530 + 1; + const auto t1324 = t1250 * t542; + const auto t1325 = t189 * t72; + const auto t1326 = 2 * t1323; + const auto t1327 = t72 + * (t11 * t35 * t546 * t7 - t1124 * t225 + t12 * t35 * t546 * t7 + - t1326 * t231 - t234 * t533 - t542 * t923); + const auto t1328 = t1251 * t547; + const auto t1329 = + t1323 * t482 + t1324 + t1325 * t547 + t1327 * t24 - t1328 * t246; + const auto t1330 = -t1329 * t35; + const auto t1331 = t1250 * t546; + const auto t1332 = t1328 * t245; + const auto t1333 = + t1327 * t18 + t1331 * t3 - t1332 * t18 + t533 + t548 * t66; + const auto t1334 = -t1333 * t35; + const auto t1335 = + t1262 * t602 - t1262 * t605 + t1263 * t588 - t1263 * t604; + const auto t1336 = + t1 * t1331 + t1124 + t1327 * t21 - t1332 * t21 + t166 * t548; + const auto t1337 = t1336 * t35; + const auto t1338 = + t1273 * t621 - t1273 * t625 + t1275 * t624 - t1275 * t627; + const auto t1339 = + -t1273 * t634 + t1273 * t635 - t1277 * t624 + t1277 * t627; + const auto t1340 = t1283 * t66 - t1284 - t1285 + t1286 + t1287 + + t1288 * t157 - t1289 * t166; + const auto t1341 = t1242 * t656; + const auto t1342 = t558 * t656; + const auto t1343 = t1154 - t1341 - t1342; + const auto t1344 = t1251 * t661; + const auto t1345 = t1037 * t1344 + t1156 * t1250 + t1248 - t1249 * t661; + const auto t1346 = -t1246 * t1343 + t1345; + const auto t1347 = t1158 + t1162 * t1250 + t1163 * t1344; + const auto t1348 = -t1270 * t1343 + t1347; + const auto t1349 = t1273 * t35; + const auto t1350 = t1275 * t35; + const auto t1351 = -t1273 * t35 * t682 - t1275 * t35 * t683 + + t1349 * t665 + t1350 * t680; + const auto t1352 = t1152 * t1250; + const auto t1353 = t486 * t687; + const auto t1354 = -t1353; + const auto t1355 = t1046 * t1344; + const auto t1356 = -t1259 * t1343 + t1352 + t1354 + t1355 + t35 * t685; + const auto t1357 = t1277 * t35; + const auto t1358 = -t1273 * t35 * t703 - t1277 * t35 * t680 + + t1349 * t700 + t1357 * t683; + const auto t1359 = -t1346; + const auto t1360 = -t1356; + const auto t1361 = t1262 * t35; + const auto t1362 = t1263 * t35; + const auto t1363 = + t1361 * t707 - t1361 * t709 + t1362 * t711 - t1362 * t713; + const auto t1364 = t1242 * t723; + const auto t1365 = t1130 * t723; + const auto t1366 = -t1364 - t1365 + t732; + const auto t1367 = t1251 * t736; + const auto t1368 = t1037 * t1367 - t1249 * t736 + t1250 * t734 + t1307; + const auto t1369 = -t1246 * t1366 + t1368; + const auto t1370 = t1040 * t738; + const auto t1371 = -t1370; + const auto t1372 = + t1163 * t1367 + t1183 * t35 + t1250 * t730 - t1270 * t1366 + t1371; + const auto t1373 = -t1273 * t35 * t779 - t1275 * t35 * t781 + + t1349 * t760 + t1350 * t776; + const auto t1374 = t1046 * t1367 + t1316 * t723 + t739; + const auto t1375 = -t1259 * t1366 + t1374; + const auto t1376 = -t1273 * t35 * t789 - t1277 * t35 * t776 + + t1349 * t787 + t1357 * t781; + const auto t1377 = -t1369; + const auto t1378 = -t1375; + const auto t1379 = + t1361 * t795 - t1361 * t798 + t1362 * t793 - t1362 * t797; + const auto t1380 = t1283 * t64 + t1288 * t166 - t1289 * t174 + t1289; + const auto t1381 = t1275 * t857; + const auto t1382 = t1273 * t845; + const auto t1383 = t1250 * t813; + const auto t1384 = t7 * t813; + const auto t1385 = t72 + * (-t1203 * t225 + t1293 * t482 + t1384 * t158 + t1384 * t56 + - t234 * t803 + t809 * t923); + const auto t1386 = t1251 * t815; + const auto t1387 = t1386 * t245; + const auto t1388 = + t1 * t1383 + t1203 + t1385 * t21 + t1387 * t21 - t166 * t816; + const auto t1389 = t1250 * t810; + const auto t1390 = + t1235 * t7 - t1325 * t815 + t1385 * t24 + t1386 * t246 + t1389; + const auto t1391 = t1277 * t861; + const auto t1392 = t1273 * t866; + const auto t1393 = + t1383 * t3 + t1385 * t18 + t1387 * t18 - t66 * t816 + t803; + const auto t1394 = t1263 * t871; + const auto t1395 = t1263 * t873; + const auto t1396 = t1262 * t872; + const auto t1397 = t1262 * t874; + const auto t1398 = -t1393; + const auto t1399 = -t1390; + const auto t1400 = t1109 + t1295 * t225; + const auto t1401 = t154 + * (t1 * t358 * t361 * (t1302 * t885 + t1303 * t883) + - t663 * (t1262 * t896 + t1314 * t897) + - t891 * (-t1303 * t889 + t1400 * t885)); + const auto t1402 = t1314 * t54; + const auto t1403 = t356 + * (-t3 * (t1263 * t918 + t1402 * t919) + + t7 * (t1262 * t918 + t1402 * t906) + + t904 * (-t1275 * t903 + t1277 * t902)); + const auto t1404 = t154 + * (t3 * t358 * t361 * (t1303 * t930 + t1400 * t929) + - t527 * (t1262 * t939 + t1263 * t938) + - t934 * (t1302 * t930 - t1400 * t932)); + const auto t1405 = t1262 * t95; + const auto t1406 = t356 + * (-t1 * (t1263 * t961 + t1405 * t962) + + t7 * (t1314 * t961 + t1405 * t951) + + t947 * (t1273 * t945 - t1275 * t946)); + const auto t1407 = t154 + * (t3 * t358 * t404 * (t1303 * t970 + t1400 * t972) + - t663 * (t1230 * t1262 + t1314 * t978) + - t974 * (t1302 * t972 - t1303 * t973)); + const auto t1408 = t1263 * t95; + const auto t1409 = t356 + * (t1 * (t1262 * t998 + t1408 * t987) + - t3 * (t1314 * t998 + t1408 * t997) + + t986 * (-t1273 * t985 + t1277 * t984)); + const auto t1410 = t19 * t21; + const auto t1411 = -t1410; + const auto t1412 = t22 * t24; + const auto t1413 = -t1412; + const auto t1414 = 2 * t515; + const auto t1415 = t207 * t219; + const auto t1416 = t72 + * (-t1063 * t1414 - t1411 - t1413 - t1414 * t191 - t1415 * t157 + - t1415 * t174 + 2 * t18 * t241 * t3 * t35 - t237 * t474); + const auto t1417 = -t22; + const auto t1418 = t1417 + t565; + const auto t1419 = -t1257 + t1418; + const auto t1420 = + t1416 * t24 + t1419 - t219 * t35 * t69 * t7 * t72 + t255 * t317; + const auto t1421 = t207 * t242; + const auto t1422 = + -t1416 * t18 + t1421 + t252 * t35 + t254 * t35 - t255 * t287; + const auto t1423 = -t1422; + const auto t1424 = -t1420; + const auto t1425 = -t19; + const auto t1426 = t1425 + t516; + const auto t1427 = -t1045 + t1426; + const auto t1428 = + t1 * t219 * t35 * t69 * t72 - t1416 * t21 - t1427 - t255 * t331; + const auto t1429 = t15 * t18; + const auto t1430 = t1414 * t18; + const auto t1431 = t1028 - t1429 + t1430; + const auto t1432 = t1031 + t1431; + const auto t1433 = -t1432; + const auto t1434 = t1034 * t1433 + t1041; + const auto t1435 = -t1044 + t1047 + t1048 * t1433 + t1427; + const auto t1436 = -t1034 * t1432 - t1066; + const auto t1437 = -t1068 * t1432 - t1073; + const auto t1438 = -t1435; + const auto t1439 = t1243 + t1431; + const auto t1440 = -t1439; + const auto t1441 = t1246 * t1440 + t1253; + const auto t1442 = -t1256 + t1258 + t1259 * t1440 + t1419; + const auto t1443 = -t1246 * t1439 - t1268; + const auto t1444 = -t1270 * t1439 - t1271; + const auto t1445 = -t1442; + const auto t1446 = 2 * t334; + const auto t1447 = 2 * t337; + const auto t1448 = std::pow(t144, -3.0 / 2.0); + const auto t1449 = -t90; + const auto t1450 = 2 * f1_x; + const auto t1451 = 2 * t28; + const auto t1452 = t1451 * t78; + const auto t1453 = t1452 * t35; + const auto t1454 = 3 * t29; + const auto t1455 = t1454 * t59; + const auto t1456 = t1455 * t79; + const auto t1457 = t1455 * t82; + const auto t1458 = t1455 * t85; + const auto t1459 = t209 + 2; + const auto t1460 = t297 + t299; + const auto t1461 = t1460 + t295; + const auto t1462 = -t1453 - t1456 - t1457 - t1458 + t1459 + t1461; + const auto t1463 = 2 * t294; + const auto t1464 = t1454 * t310; + const auto t1465 = 2 * e1_x; + const auto t1466 = -4 * e0_x + t1465; + const auto t1467 = t1449 + t1450 + t1451 * t295 + t1451 * t297 + + t1451 * t299 + t1451 * t309 - t1462 * t3 + t1463 + t1464 * t3 + + t1466; + const auto t1468 = t1467 * t35; + const auto t1469 = t1451 * t35; + const auto t1470 = -t209 - 2; + const auto t1471 = -t295 - t297 - t299 - t87; + const auto t1472 = t1453 + t1456 + t1457 + t1458 + t1464 + t1469 * t302 + + t1470 + t1471; + const auto t1473 = t222 * t93; + const auto t1474 = t227 * t89; + const auto t1475 = std::pow(t303, 2); + const auto t1476 = t13 * t59; + const auto t1477 = t1000 * t1475 + t1475 * t1476 + std::pow(t313, 2); + const auto t1478 = + t1448 * (t1468 * t91 + t1472 * t1473 + t1472 * t1474 + t1477); + const auto t1479 = -t130 * t465; + const auto t1480 = 3 / std::pow(t14, 3); + const auto t1481 = t11 * t1480; + const auto t1482 = t178 * t77; + const auto t1483 = t128 * t16 + t129 * t16 + t1482; + const auto t1484 = t1483 + t301; + const auto t1485 = t251 * t465; + const auto t1486 = t154 / std::sqrt(t34); + const auto t1487 = t1462 * t1486; + const auto t1488 = -t130 * t1481 - t1479 - t1484 * t1485 - t1487; + const auto t1489 = std::pow(t144, -5.0 / 2.0); + const auto t1490 = t1473 * t303 + t1474 * t303 + t313 * t91; + const auto t1491 = std::pow(t1490, 2); + const auto t1492 = t136 + t1484; + const auto t1493 = t1448 * t1490; + const auto t1494 = 2 * t1493; + const auto t1495 = t1492 * t1494; + const auto t1496 = std::pow(t219, 2); + const auto t1497 = std::pow(t237, 2); + const auto t1498 = 3 * t4; + const auto t1499 = t29 * t35; + const auto t1500 = 3 * t5; + const auto t1501 = 3 * t8; + const auto t1502 = t1498 * t1499 + t1499 * t1500 + t1499 * t1501; + const auto t1503 = 2 * d_x; + const auto t1504 = t1503 * t28; + const auto t1505 = 2 * t4; + const auto t1506 = -t1505; + const auto t1507 = 2 * t5; + const auto t1508 = -t1507; + const auto t1509 = 2 * t8; + const auto t1510 = -t1509; + const auto t1511 = t1508 + t1510; + const auto t1512 = t1506 + t1511; + const auto t1513 = t1504 + t1512; + const auto t1514 = t1451 * t218 + t1454 * t212 + t1502 + t1513; + const auto t1515 = t21 * t222; + const auto t1516 = t227 * t24; + const auto t1517 = -t4; + const auto t1518 = -t5; + const auto t1519 = -t8; + const auto t1520 = t1518 + t1519; + const auto t1521 = t1517 + t1520; + const auto t1522 = t1504 + t1521; + const auto t1523 = t10 * t59; + const auto t1524 = t1503 + 2 * t213 + t233; + const auto t1525 = t1454 * t1523 * t3 + t1469 * t235 + t1505 * t214 + + t1507 * t214 + t1509 * t214 + t1524 + t207 * (t1502 + t1522); + const auto t1526 = t155 + * (t12 * t1496 * t35 + t13 * t1496 * t35 + t1497 * t35 + - t1514 * t1515 - t1514 * t1516 - t1525 * t18); + const auto t1527 = -t1058; + const auto t1528 = t1527 * t3; + const auto t1529 = t10 * t178; + const auto t1530 = std::pow(t3, 3); + const auto t1531 = 3 * t1530; + const auto t1532 = t15 * t1531; + const auto t1533 = d_x * t1532 + t1500 * t178 + t1501 * t178; + const auto t1534 = t1511 + 3 * t1529 + t1533 - 4 * t4; + const auto t1535 = t465 * (-2 * t1528 + t1534); + const auto t1536 = std::pow(t25, -5.0 / 2.0); + const auto t1537 = std::pow(t1064, 2); + const auto t1538 = 2 * t22; + const auto t1539 = t155 * t35; + const auto t1540 = t1064 * t1539; + const auto t1541 = t1060 * t1540; + const auto t1542 = t1526 * t317 + t1535 * t663 + - 3 * t1536 * t1537 * t24 * t59 + t1538 * t1541; + const auto t1543 = std::pow(t124, -3.0 / 2.0); + const auto t1544 = -t47; + const auto t1545 = 2 * f0_x; + const auto t1546 = t1451 * t37; + const auto t1547 = t1546 * t35; + const auto t1548 = t1455 * t38; + const auto t1549 = t1455 * t41; + const auto t1550 = t1455 * t44; + const auto t1551 = t262 + t264; + const auto t1552 = t1551 + t260; + const auto t1553 = t1459 - t1547 - t1548 - t1549 - t1550 + t1552; + const auto t1554 = 2 * t278; + const auto t1555 = t1454 * t272; + const auto t1556 = t1451 * t260 + t1451 * t262 + t1451 * t264 + + t1451 * t271 + t1466 + t1544 + t1545 - t1553 * t3 + t1554 + + t1555 * t3; + const auto t1557 = t1556 * t35; + const auto t1558 = -t260 - t262 - t264 - t46; + const auto t1559 = t1469 * t270 + t1470 + t1547 + t1548 + t1549 + t1550 + + t1555 + t1558; + const auto t1560 = t222 * t50; + const auto t1561 = t227 * t52; + const auto t1562 = std::pow(t279, 2); + const auto t1563 = t1000 * t1562 + t1476 * t1562 + std::pow(t276, 2); + const auto t1564 = + t1543 * (t1557 * t48 + t1559 * t1560 + t1559 * t1561 + t1563); + const auto t1565 = -t107 * t465; + const auto t1566 = t178 * t36; + const auto t1567 = t105 * t16 + t106 * t16 + t1566; + const auto t1568 = t1567 + t269; + const auto t1569 = t1486 * t1553; + const auto t1570 = -t107 * t1481 - t1485 * t1568 - t1565 - t1569; + const auto t1571 = std::pow(t124, -5.0 / 2.0); + const auto t1572 = t1560 * t279 + t1561 * t279 + t276 * t48; + const auto t1573 = std::pow(t1572, 2); + const auto t1574 = t114 + t1568; + const auto t1575 = t1543 * t1572; + const auto t1576 = 2 * t1575; + const auto t1577 = t1574 * t1576; + const auto t1578 = 2 * t19; + const auto t1579 = t1526 * t331 + t1535 * t527 + - 3 * t1536 * t1537 * t21 * t59 + t1541 * t1578; + const auto t1580 = t1 * t125 * t1570 - t112 * t1564 + + 3 * t112 * t1571 * t1573 - t1577 * t19 - t1579; + const auto t1581 = t1 * t145 * t1488 - t143 * t1478 + + 3 * t143 * t1489 * t1491 - t1495 * t19 - t1579; + const auto t1582 = t1480 * t1530; + const auto t1583 = t107 * t465; + const auto t1584 = t11 * t1583; + const auto t1585 = t1568 * t16; + const auto t1586 = -t113; + const auto t1587 = t1586 + 1; + const auto t1588 = t1584 + t1585 + t1587; + const auto t1589 = t1527 * t178; + const auto t1590 = -t1498 + t1520 + t1533; + const auto t1591 = -t1503; + const auto t1592 = t1531 * t465; + const auto t1593 = -t10 * t1592 + t1503 * t178 + t1507 * t16 + + t1509 * t16 + t1591 + 3 * t17; + const auto t1594 = -t15 * t1590 * t3 + t1593; + const auto t1595 = t15 * t26; + const auto t1596 = t10 - t1529; + const auto t1597 = t1528 + t1596; + const auto t1598 = 2 * t15; + const auto t1599 = t1526 * t287 - 3 * t1536 * t1537 * t18 * t59 + + t1540 * t1597 * t1598 + t1595 * (-2 * t1589 - t1594); + const auto t1600 = t130 * t465; + const auto t1601 = t11 * t1600; + const auto t1602 = t1484 * t16; + const auto t1603 = -t135; + const auto t1604 = t1603 + 1; + const auto t1605 = t1601 + t1602 + t1604; + const auto t1606 = t308 * t35; + const auto t1607 = -t1472; + const auto t1608 = + t293 * (-t1467 * t1606 + t1477 + t1607 * t305 + t1607 * t307); + const auto t1609 = 3 / std::pow(t94, 5.0 / 2.0); + const auto t1610 = t1609 * std::pow(t314, 2); + const auto t1611 = t227 * t95; + const auto t1612 = 2 * t315; + const auto t1613 = t227 * t303; + const auto t1614 = -t1514; + const auto t1615 = t222 * t225; + const auto t1616 = t155 + * (t1496 * t158 + t1496 * t186 + t1497 * t35 - t1525 * t234 + + t1614 * t1615 + t1614 * t380); + const auto t1617 = t59 * t663; + const auto t1618 = 3 * t1536; + const auto t1619 = t1618 * t59; + const auto t1620 = t1619 * std::pow(t238, 2); + const auto t1621 = t288 * t59; + const auto t1622 = + -t1514 * t1617 - t1616 * t317 + t1620 * t24 - t1621 * t219 * t482; + const auto t1623 = t284 * t35; + const auto t1624 = -t1559; + const auto t1625 = + t277 * (-t1556 * t1623 + t1563 + t1624 * t281 + t1624 * t283); + const auto t1626 = 3 / std::pow(t53, 5.0 / 2.0); + const auto t1627 = t1626 * std::pow(t285, 2); + const auto t1628 = t227 * t54; + const auto t1629 = 2 * t286; + const auto t1630 = t227 * t279; + const auto t1631 = + -t1525 * t505 - t1616 * t287 + t1620 * t18 - 2 * t1621 * t237; + const auto t1632 = t126 * t329; + const auto t1633 = t146 * t334; + const auto t1634 = t150 * t335; + const auto t1635 = t151 * t337; + const auto t1636 = t1632 + t1633 - t1634 - t1635; + const auto t1637 = -t1636; + const auto t1638 = t1637 * t178; + const auto t1639 = + -t126 * t342 + t150 * t339 + t152 * t337 - t153 * t334; + const auto t1640 = -t1639; + const auto t1641 = t1640 * t66; + const auto t1642 = t319 * t55; + const auto t1643 = t101 * t322; + const auto t1644 = t290 * t96; + const auto t1645 = t102 * t320; + const auto t1646 = t1642 - t1643 + t1644 - t1645; + const auto t1647 = t1646 * t64; + const auto t1648 = -t101 * t102 + t55 * t96; + const auto t1649 = t1648 * t19; + const auto t1650 = -t126 * t153 + t150 * t152; + const auto t1651 = t1650 * t22; + const auto t1652 = -t1651; + const auto t1653 = t126 * t146 - t150 * t151; + const auto t1654 = t16 * t1653; + const auto t1655 = 3 * t1654; + const auto t1656 = t1592 * t1653; + const auto t1657 = 3 * t1; + const auto t1658 = t1657 * t515; + const auto t1659 = t1648 * t1658; + const auto t1660 = 3 * t7; + const auto t1661 = t1660 * t515; + const auto t1662 = t1650 * t1661; + const auto t1663 = t1649 + t1652 - t1655 + t1656 - t1659 + t1662; + const auto t1664 = t7 * t95; + const auto t1665 = t28 * t446; + const auto t1666 = t30 * t302; + const auto t1667 = 3 * t30; + const auto t1668 = t28 * t81; + const auto t1669 = t30 * t78; + const auto t1670 = t1 * t28; + const auto t1671 = -t1670 - t376; + const auto t1672 = + t1667 * t296 + t1667 * t298 + t1667 * t300 + t1668 + t1669 + t1671; + const auto t1673 = t1665 + t1666 + t1667 * t294 + t1672; + const auto t1674 = t1673 * t59; + const auto t1675 = t35 * t89; + const auto t1676 = 3 * t28; + const auto t1677 = t1676 * t376; + const auto t1678 = + t1665 * t207 + t1666 * t207 + t1672 * t207 + t1677 * t310 + t447; + const auto t1679 = t1667 * t1670; + const auto t1680 = + t1665 * t222 + t1666 * t222 + t1672 * t222 + t1679 * t310 + t303; + const auto t1681 = t3 * t313; + const auto t1682 = t1 * t451; + const auto t1683 = t186 * t303; + const auto t1684 = t1681 * t447 + t1682 * t303 + t1683 * t447; + const auto t1685 = -t1673 * t307 - t1678 * t308 - t1680 * t304 + t1684; + const auto t1686 = t1685 * t293; + const auto t1687 = t314 * t452; + const auto t1688 = t1609 * t89; + const auto t1689 = t315 * t447; + const auto t1690 = d_x * t30; + const auto t1691 = d_y * t28; + const auto t1692 = t214 * t30; + const auto t1693 = + t1498 * t1692 + t1500 * t1692 + t1501 * t1692 + t1690 + t1691; + const auto t1694 = t28 * t371; + const auto t1695 = + t1523 * t1677 + t1693 * t207 + t1694 * t207 + t235 * t367 + t384; + const auto t1696 = t218 * t30; + const auto t1697 = + t1523 * t1679 + t1693 * t222 + t1696 * t222 + t214 * t372 + t219; + const auto t1698 = t1667 * t213 + t1693 + t1694 + t1696; + const auto t1699 = -t1 * t219 * t35 * t374 - t13 * t219 * t35 * t384 + - t237 * t3 * t35 * t384; + const auto t1700 = t1695 * t234 + t1697 * t225 + t1698 * t380 + t1699; + const auto t1701 = -t155 * t1700; + const auto t1702 = t1621 * t384; + const auto t1703 = t59 * t7; + const auto t1704 = t219 * t500; + const auto t1705 = t247 * t397; + const auto t1706 = t1536 * t59; + const auto t1707 = t1705 * t1706; + const auto t1708 = -t1617 * t1698 - t1701 * t317 - t1702 * t7 + - t1703 * t1704 + t1707 * t24; + const auto t1709 = t1613 * t498 + t1664 * t1674 - t1675 * t1686 + + t1687 * t1688 + t1689 * t227 + t1708; + const auto t1710 = t54 * t7; + const auto t1711 = t28 * t423; + const auto t1712 = t270 * t30; + const auto t1713 = t28 * t40; + const auto t1714 = t30 * t37; + const auto t1715 = + t1667 * t261 + t1667 * t263 + t1667 * t265 + t1671 + t1713 + t1714; + const auto t1716 = t1667 * t278 + t1711 + t1712 + t1715; + const auto t1717 = t1716 * t59; + const auto t1718 = t35 * t52; + const auto t1719 = t270 * t35; + const auto t1720 = + t1677 * t272 + t1711 * t207 + t1715 * t207 + t1719 * t376 + t424; + const auto t1721 = t35 * t423; + const auto t1722 = + t1670 * t1721 + t1679 * t272 + t1712 * t222 + t1715 * t222 + t279; + const auto t1723 = t276 * t3; + const auto t1724 = t1 * t429; + const auto t1725 = t186 * t279; + const auto t1726 = t1723 * t424 + t1724 * t279 + t1725 * t424; + const auto t1727 = -t1716 * t283 - t1720 * t284 - t1722 * t280 + t1726; + const auto t1728 = t1727 * t277; + const auto t1729 = t285 * t430; + const auto t1730 = t1626 * t1729; + const auto t1731 = t286 * t424; + const auto t1732 = t1630 * t504 + t1708 + t1710 * t1717 - t1718 * t1728 + + t1730 * t52 + t1731 * t227; + const auto t1733 = t35 * t54; + const auto t1734 = t35 * t50; + const auto t1735 = t222 * t279; + const auto t1736 = t1 * t59; + const auto t1737 = -t1621 * t374 - t1697 * t505 - t1701 * t331 + - t1704 * t1736 + t1707 * t21; + const auto t1738 = t1722 * t1733 - t1728 * t1734 + t1730 * t50 + + t1735 * t504 + t1737 + t286 * t429; + const auto t1739 = t35 * t95; + const auto t1740 = t35 * t93; + const auto t1741 = t1609 * t1687; + const auto t1742 = t222 * t303; + const auto t1743 = t1680 * t1739 - t1686 * t1740 + t1737 + t1741 * t93 + + t1742 * t498 + t315 * t451; + const auto t1744 = t35 * t48; + const auto t1745 = t237 * t59; + const auto t1746 = -t1695 * t505 - t1701 * t287 - t1702 * t3 + + t1707 * t18 - t1745 * t500; + const auto t1747 = t1720 * t1733 - t1728 * t1744 + t1730 * t48 + + t1731 * t207 + t1746 + t276 * t504; + const auto t1748 = t35 * t91; + const auto t1749 = t1678 * t1739 - t1686 * t1748 + t1689 * t207 + + t1741 * t91 + t1746 + t313 * t498; + const auto t1750 = t285 * t431; + const auto t1751 = t35 * t434; + const auto t1752 = t230 * t238; + const auto t1753 = -t1036 * t358 + t1751 * t1752; + const auto t1754 = t1630 * t361 - t1750 * t282 + t1753; + const auto t1755 = t1754 * t411; + const auto t1756 = t314 * t453; + const auto t1757 = t35 * t358; + const auto t1758 = t1751 * t234; + const auto t1759 = -t1757 * t237 + t1758 * t238; + const auto t1760 = -t1756 * t308 + t1759 + t313 * t404; + const auto t1761 = t1760 * t412; + const auto t1762 = t1613 * t404 + t1753 - t1756 * t306; + const auto t1763 = -t1750 * t284 + t1759 + t276 * t361; + const auto t1764 = t404 * t7; + const auto t1765 = t306 * t35; + const auto t1766 = -t1685 * t453; + const auto t1767 = 3 / std::pow(t403, 5.0 / 2.0); + const auto t1768 = t1687 * t1767; + const auto t1769 = t1756 * t447; + const auto t1770 = t1751 * t230; + const auto t1771 = t358 * t7; + const auto t1772 = t1771 * t59; + const auto t1773 = t1703 * t434; + const auto t1774 = t238 * t384; + const auto t1775 = t1773 * t219; + const auto t1776 = std::pow(t357, -5.0 / 2.0); + const auto t1777 = t1776 * t59; + const auto t1778 = t1777 * t397; + const auto t1779 = 3 * t1752; + const auto t1780 = -t1698 * t1772 + t1700 * t1770 - t1773 * t1774 + - t1775 * t397 + t1778 * t1779; + const auto t1781 = t282 * t35; + const auto t1782 = -t1727 * t431; + const auto t1783 = 3 * t282; + const auto t1784 = std::pow(t360, -5.0 / 2.0); + const auto t1785 = t1729 * t1784; + const auto t1786 = t1750 * t424; + const auto t1787 = t35 * t361; + const auto t1788 = 3 * t284; + const auto t1789 = t1745 * t434; + const auto t1790 = t3 * t59; + const auto t1791 = t1790 * t434; + const auto t1792 = t1777 * t234; + const auto t1793 = -t1695 * t1757 + t1700 * t1758 + t1705 * t1792 + - t1774 * t1791 - t1789 * t397; + const auto t1794 = t35 * t404; + const auto t1795 = t1762 * t362; + const auto t1796 = t1763 * t405; + const auto t1797 = -t1755 - t1761 + t1795 + t1796; + const auto t1798 = + -t102 * t336 + t319 * t462 - t322 * t496 + t333 * t96; + const auto t1799 = t101 * t333; + const auto t1800 = t320 * t462; + const auto t1801 = -t1799 - t1800 + t290 * t496 + t336 * t55; + const auto t1802 = t362 * t405 - t411 * t412; + const auto t1803 = + -t102 * t510 + t462 * t503 - t496 * t509 + t507 * t96; + const auto t1804 = -t102 * t496 + t462 * t96; + const auto t1805 = t362 * t457; + const auto t1806 = t405 * t439; + const auto t1807 = t411 * t459; + const auto t1808 = t412 * t458; + const auto t1809 = t1805 + t1806 - t1807 - t1808; + const auto t1810 = + -t101 * t507 - t462 * t523 + t496 * t522 + t510 * t55; + const auto t1811 = 3 * t3; + const auto t1812 = t1069 * t1811; + const auto t1813 = -t101 * t462 + t496 * t55; + const auto t1814 = t388 * t465; + const auto t1815 = t1660 * t1814; + const auto t1816 = t16 * t1802 + t1658 * t1804 - t178 * t1803 + - t1802 * t1812 + t1803 - t1804 * t19 + t1809 * t64 - t1810 * t66 + + t1813 * t1815; + const auto t1817 = 3 * t1802; + const auto t1818 = t1817 * t7; + const auto t1819 = t102 * t626; + const auto t1820 = t496 * t605; + const auto t1821 = + -t101 * t623 - t462 * t604 + t496 * t588 + t55 * t626; + const auto t1822 = -t1819 - t1820 + t462 * t602 + t623 * t96; + const auto t1823 = t431 * t583; + const auto t1824 = t207 * t361; + const auto t1825 = t207 * t358; + const auto t1826 = t1758 * t547 - t1825 * t546; + const auto t1827 = -t1823 * t284 + t1824 * t578 + t1826; + const auto t1828 = t1827 * t405; + const auto t1829 = t453 * t599; + const auto t1830 = t207 * t404; + const auto t1831 = t1826 - t1829 * t308 + t1830 * t598; + const auto t1832 = t1831 * t412; + const auto t1833 = -t1757 * t542 + t1770 * t547; + const auto t1834 = -t1829 * t306 + t1833 + t404 * t596; + const auto t1835 = t1834 * t362; + const auto t1836 = -t1823 * t282 + t1833 + t361 * t582; + const auto t1837 = t1836 * t411; + const auto t1838 = t1828 - t1832 + t1835 - t1837; + const auto t1839 = t28 * t577; + const auto t1840 = t270 * t32; + const auto t1841 = 3 * t32; + const auto t1842 = t28 * t43; + const auto t1843 = t32 * t37; + const auto t1844 = t3 * t32; + const auto t1845 = t28 * t7; + const auto t1846 = -t1844 - t1845; + const auto t1847 = + t1841 * t261 + t1841 * t263 + t1841 * t265 + t1842 + t1843 + t1846; + const auto t1848 = t1839 + t1840 + t1841 * t278 + t1847; + const auto t1849 = t1 * t54; + const auto t1850 = t1676 * t1844; + const auto t1851 = + t1719 * t1844 + t1839 * t207 + t1847 * t207 + t1850 * t272 + t578; + const auto t1852 = t1845 * t35; + const auto t1853 = t1841 * t1845; + const auto t1854 = + t1840 * t227 + t1847 * t227 + t1852 * t577 + t1853 * t272 + t279; + const auto t1855 = t582 * t7; + const auto t1856 = t158 * t279; + const auto t1857 = t1723 * t578 + t1855 * t279 + t1856 * t578; + const auto t1858 = -t1848 * t281 - t1851 * t284 - t1854 * t282 + t1857; + const auto t1859 = t1858 * t277; + const auto t1860 = t285 * t583; + const auto t1861 = t1626 * t50; + const auto t1862 = t286 * t578; + const auto t1863 = d_x * t32; + const auto t1864 = d_z * t28; + const auto t1865 = t214 * t32; + const auto t1866 = + t1498 * t1865 + t1500 * t1865 + t1501 * t1865 + t1863 + t1864; + const auto t1867 = t28 * t539; + const auto t1868 = + t1523 * t1850 + t1866 * t207 + t1867 * t207 + t235 * t535 + t546; + const auto t1869 = t218 * t32; + const auto t1870 = + t1523 * t1853 + t1866 * t227 + t1869 * t227 + t214 * t540 + t219; + const auto t1871 = t1841 * t213 + t1866 + t1867 + t1869; + const auto t1872 = -t12 * t219 * t35 * t546 - t219 * t35 * t542 * t7 + - t237 * t3 * t35 * t546; + const auto t1873 = t1615 * t1871 + t1868 * t234 + t1870 * t230 + t1872; + const auto t1874 = -t155 * t1873; + const auto t1875 = t1621 * t546; + const auto t1876 = t586 * t59; + const auto t1877 = t1876 * t219; + const auto t1878 = t247 * t547; + const auto t1879 = t1706 * t1878; + const auto t1880 = -t1 * t1875 - t1 * t1877 - t1871 * t527 * t59 + - t1874 * t331 + t1879 * t21; + const auto t1881 = -t1734 * t1859 + t1735 * t584 + t1848 * t1849 * t59 + + t1860 * t1861 + t1862 * t222 + t1880; + const auto t1882 = t28 * t593; + const auto t1883 = t302 * t32; + const auto t1884 = t28 * t84; + const auto t1885 = t32 * t78; + const auto t1886 = + t1841 * t296 + t1841 * t298 + t1841 * t300 + t1846 + t1884 + t1885; + const auto t1887 = t1841 * t294 + t1882 + t1883 + t1886; + const auto t1888 = t1 * t95; + const auto t1889 = t1844 * t35; + const auto t1890 = + t1850 * t310 + t1882 * t207 + t1886 * t207 + t1889 * t302 + t598; + const auto t1891 = + t1852 * t593 + t1853 * t310 + t1883 * t227 + t1886 * t227 + t303; + const auto t1892 = t596 * t7; + const auto t1893 = t158 * t303; + const auto t1894 = t1681 * t598 + t1892 * t303 + t1893 * t598; + const auto t1895 = -t1887 * t305 - t1890 * t308 - t1891 * t306 + t1894; + const auto t1896 = t1895 * t293; + const auto t1897 = t314 * t599; + const auto t1898 = t1609 * t1897; + const auto t1899 = t222 * t598; + const auto t1900 = -t1740 * t1896 + t1742 * t600 + t1880 + + t1887 * t1888 * t59 + t1898 * t93 + t1899 * t315; + const auto t1901 = -t1621 * t542 - t1870 * t505 - t1874 * t317 + - t1877 * t7 + t1879 * t24; + const auto t1902 = t1613 * t600 - t1675 * t1896 + t1688 * t1897 + + t1739 * t1891 + t1901 + t315 * t596; + const auto t1903 = t1626 * t1860; + const auto t1904 = t1630 * t584 - t1718 * t1859 + t1733 * t1854 + t1901 + + t1903 * t52 + t286 * t582; + const auto t1905 = -t1745 * t586 + t18 * t1879 - t1868 * t505 + - t1874 * t287 - t1875 * t3; + const auto t1906 = t1733 * t1851 - t1744 * t1859 + t1862 * t207 + + t1903 * t48 + t1905 + t276 * t584; + const auto t1907 = t207 * t598; + const auto t1908 = t1739 * t1890 - t1748 * t1896 + t1898 * t91 + t1905 + + t1907 * t315 + t313 * t600; + const auto t1909 = -t1895 * t453; + const auto t1910 = t1767 * t1897; + const auto t1911 = t542 * t59; + const auto t1912 = t1911 * t434; + const auto t1913 = -t1757 * t1870 + t1770 * t1873 - t1775 * t547 + + t1777 * t1779 * t547 - t1912 * t238; + const auto t1914 = -t1858 * t431; + const auto t1915 = t1784 * t1860; + const auto t1916 = t207 * t578; + const auto t1917 = t1791 * t546; + const auto t1918 = -t1757 * t1868 + t1758 * t1873 - t1789 * t547 + + t1792 * t1878 - t1917 * t238; + const auto t1919 = + -t126 * t665 - t146 * t680 + t150 * t682 + t151 * t683; + const auto t1920 = t1919 * t35; + const auto t1921 = t35 * t680; + const auto t1922 = t334 * t35; + const auto t1923 = t35 * t683; + const auto t1924 = t337 * t35; + const auto t1925 = -t1450; + const auto t1926 = t1676 * t60; + const auto t1927 = 3 * t59; + const auto t1928 = t1927 * t273; + const auto t1929 = t1460 + t1926 * t78 + t1928 * t82 + t1928 * t85 + - t214 * t78 + t291 + 2 * t295; + const auto t1930 = 3 * e0_x; + const auto t1931 = t1930 + t2; + const auto t1932 = t1925 + t1926 * t86 + t1929 * t3 + t1931 + + t214 * t643 - t294 - t296 - t298 - t300 + t302 * t56 + t638 + + 2 * t90; + const auto t1933 = t1929 + t214 * t642 + t309 + 3 * t311 + t87; + const auto t1934 = t1 * t93; + const auto t1935 = t7 * t89; + const auto t1936 = t1683 * t639 + t1893 * t639; + const auto t1937 = t1448 * t35; + const auto t1938 = t1937 + * (t1932 * t91 + t1933 * t1934 + t1933 * t1935 + t1936 + + t313 * t702); + const auto t1939 = -t137 - t1483; + const auto t1940 = t1939 * t3; + const auto t1941 = 3 * t1601; + const auto t1942 = t128 * t15; + const auto t1943 = -t1942; + const auto t1944 = t129 * t15; + const auto t1945 = -t1944; + const auto t1946 = t127 * t15; + const auto t1947 = 3 * t128; + const auto t1948 = 3 * t129; + const auto t1949 = t1592 * t77 + t1943 + t1945 - 3 * t1946 + + t1947 * t515 + t1948 * t515 + t340; + const auto t1950 = -t15 * t1940 + t1602 + t1603 + t1941 + t1949; + const auto t1951 = t138 + t1483; + const auto t1952 = -t1951; + const auto t1953 = t1493 * t1952; + const auto t1954 = t1934 * t639 + t1935 * t639 + t702 * t91; + const auto t1955 = t1937 * t1954; + const auto t1956 = t1492 * t1955; + const auto t1957 = d_x * t28; + const auto t1958 = 3 * t56; + const auto t1959 = t1500 * t208 + t1501 * t208 + t1957 * t1958 - t1957; + const auto t1960 = + t1498 + t1507 + t1509 + t1959 + t235 + 3 * t236 + t28 * t655; + const auto t1961 = t10 * t60; + const auto t1962 = t1591 + t1676 * t1961 + t207 * (t1505 + t1959 + t9) + - t213 + t214 * t657 - t215 - t216 - t217 + t218 * t56 + 2 * t232 + + t654; + const auto t1963 = t155 + * (t12 * t219 * t35 * t656 + t13 * t219 * t35 * t656 - t1515 * t1960 + - t1516 * t1960 - t18 * t1962 + t237 * t35 * t698); + const auto t1964 = t1058 * t3; + const auto t1965 = t465 * (t1527 * t3 - t1534 - t1964); + const auto t1966 = t1063 * t656 + t18 * t698 + t191 * t656; + const auto t1967 = t1059 * t1540; + const auto t1968 = t1060 * t1539; + const auto t1969 = t1966 * t1968; + const auto t1970 = -3 * t1064 * t1536 * t1966 * t24 * t59 + t1963 * t317 + + t1965 * t663 + t1967 * t22 + t1969 * t22; + const auto t1971 = -t1545; + const auto t1972 = t1551 + t1926 * t37 + t1928 * t41 + t1928 * t44 + - t214 * t37 + 2 * t260 + t291; + const auto t1973 = t1926 * t45 + t1931 + t1971 + t1972 * t3 + + t214 * t672 - t261 - t263 - t265 + t270 * t56 - t278 + 2 * t47 + + t667; + const auto t1974 = t1972 + t214 * t671 + t271 + 3 * t274 + t46; + const auto t1975 = t1 * t50; + const auto t1976 = t52 * t7; + const auto t1977 = t1725 * t668 + t1856 * t668; + const auto t1978 = t1543 * t35; + const auto t1979 = t1978 + * (t1973 * t48 + t1974 * t1975 + t1974 * t1976 + t1977 + + t276 * t697); + const auto t1980 = -t116 - t1567; + const auto t1981 = t1980 * t3; + const auto t1982 = 3 * t1584; + const auto t1983 = t105 * t15; + const auto t1984 = -t1983; + const auto t1985 = t106 * t15; + const auto t1986 = -t1985; + const auto t1987 = t104 * t15; + const auto t1988 = 3 * t105; + const auto t1989 = 3 * t106; + const auto t1990 = t1592 * t36 + t1984 + t1986 - 3 * t1987 + + t1988 * t515 + t1989 * t515 + t340; + const auto t1991 = -t15 * t1981 + t1585 + t1586 + t1982 + t1990; + const auto t1992 = t117 + t1567; + const auto t1993 = -t1992; + const auto t1994 = t1575 * t1993; + const auto t1995 = t1975 * t668 + t1976 * t668 + t48 * t697; + const auto t1996 = t1978 * t1995; + const auto t1997 = t1574 * t1996; + const auto t1998 = -3 * t1064 * t1536 * t1966 * t21 * t59 + t19 * t1967 + + t19 * t1969 + t1963 * t331 + t1965 * t527; + const auto t1999 = t1 * t125 * t15 * t1991 + + 3 * t112 * t1571 * t1572 * t1995 * t35 - t112 * t1979 + - t19 * t1994 - t19 * t1997 - t1998; + const auto t2000 = t1 * t145 * t15 * t1950 + + 3 * t143 * t1489 * t1490 * t1954 * t35 - t143 * t1938 + - t19 * t1953 - t19 * t1956 - t1998; + const auto t2001 = t178 * t1980; + const auto t2002 = 2 * t105; + const auto t2003 = 2 * t106; + const auto t2004 = 3 * t114 - t1531 * t1583 + t1545 + 2 * t1566 + + t16 * t2002 + t16 * t2003; + const auto t2005 = e1_x - t1930; + const auto t2006 = t107 * t178; + const auto t2007 = t107 + t1981 - t2006; + const auto t2008 = t15 * t1575; + const auto t2009 = t1543 * t1588; + const auto t2010 = t2009 * t35; + const auto t2011 = -t1596 + t1964; + const auto t2012 = t15 * t1539; + const auto t2013 = t1064 * t2012; + const auto t2014 = t1597 * t2012; + const auto t2015 = -3 * t1064 * t1536 * t18 * t1966 * t59 + + t1595 * (-t1058 * t178 + t1589 - t1590 * t16 + t1593) + + t1963 * t287 + t1966 * t2014 + t2011 * t2013; + const auto t2016 = t178 * t1939; + const auto t2017 = 2 * t128; + const auto t2018 = 2 * t129; + const auto t2019 = 3 * t136 + t1450 + 2 * t1482 - t1531 * t1600 + + t16 * t2017 + t16 * t2018; + const auto t2020 = t130 * t178; + const auto t2021 = t130 + t1940 - t2020; + const auto t2022 = t1493 * t15; + const auto t2023 = t1448 * t1605; + const auto t2024 = t2023 * t35; + const auto t2025 = -t1649; + const auto t2026 = t293 + * (-t1932 * t308 - t1933 * t640 - t1933 * t641 + t1936 + - t313 * t645); + const auto t2027 = t314 * t647; + const auto t2028 = t155 + * (-t1022 * t659 + t158 * t219 * t656 - t1615 * t1960 + + t186 * t219 * t656 - t1960 * t380 - t1962 * t234); + const auto t2029 = t227 * t26; + const auto t2030 = t1536 * t247; + const auto t2031 = t317 * t661; + const auto t2032 = t1156 * t288 - t155 * t219 * t35 * t661 * t7 + + t1960 * t2029 + t2028 * t24 + t2030 * t2031; + const auto t2033 = t277 + * (-t1973 * t284 - t1974 * t669 - t1974 * t670 + t1977 + - t276 * t674); + const auto t2034 = t1626 * t285; + const auto t2035 = t2034 * t676; + const auto t2036 = t2030 * t287; + const auto t2037 = t1152 * t288 - t155 * t237 * t35 * t661 + t18 * t2028 + + t1962 * t26 + t2036 * t661; + const auto t2038 = t1609 * t91; + const auto t2039 = -t101 * t709 - t102 * t713 + t55 * t707 + t711 * t96; + const auto t2040 = t2039 * t493; + const auto t2041 = + t126 * t703 - t150 * t700 - t152 * t683 + t153 * t680; + const auto t2042 = t2041 * t486; + const auto t2043 = t154 + * (-t1130 * t1919 + t1636 + t1638 + t1641 - t1647 + t1651 + t1655 + - t1656 + t1659 - t1662 + t1920 + t2025 + t2040 - t2042 + - t222 + * (-t101 + * (-t1630 * t677 + t1974 * t54 * t7 - t2032 + - t2033 * t52 - t2035 * t52 + + t277 * t285 * t668 * t7) + - t102 + * (t1932 * t95 - t2026 * t91 - t2027 * t2038 - t2037 + + t293 * t314 * t702 - t313 * t648) + + t290 * t707 + t319 * t711 - t320 * t709 - t322 * t713 + + t55 + * (-t1613 * t648 - t1688 * t2027 + t1933 * t7 * t95 + - t2026 * t89 - t2032 + t293 * t314 * t639 * t7) + + t96 + * (t1973 * t54 - t2033 * t48 - t2035 * t48 - t2037 + - t276 * t677 + t277 * t285 * t697)) + + t3 + * (t126 + * (3 * t134 * t1489 * t1490 * t1954 * t35 + - t134 * t1938 + t145 * t15 * t1950 * t7 + - t1953 * t22 - t1956 * t22 - t1970) + + t146 * t1999 + - t150 + * (3 * t123 * t1571 * t1572 * t1995 * t35 + - t123 * t1979 + t125 * t15 * t1991 * t7 - t1970 + - t1994 * t22 - t1997 * t22) + - t151 * t2000 + t1921 * t329 + t1922 * t665 + - t1923 * t335 - t1924 * t682) + + t7 + * (-t126 + * (3 * t139 * t1489 * t1490 * t1954 * t35 + - t139 * t1938 + + t145 * t15 + * (t11 * t1484 * t15 + t1949 * t3 - t2005 + - t2016 - t2019) + - t1954 * t2024 - t2015 - t2021 * t2022) + + t150 + * (3 * t118 * t1571 * t1572 * t1995 * t35 + - t118 * t1979 + + t125 * t15 + * (t11 * t15 * t1568 + t1990 * t3 - t2001 + - t2004 - t2005) + - t1995 * t2010 - t2007 * t2008 - t2015) + + t152 * t2000 - t153 * t1999 - t1921 * t342 + - t1922 * t703 + t1923 * t339 + t1924 * t700)); + const auto t2044 = + -t126 * t760 - t146 * t776 + t150 * t779 + t151 * t781; + const auto t2045 = t2044 * t35; + const auto t2046 = t35 * t776; + const auto t2047 = t35 * t781; + const auto t2048 = t1 * t78; + const auto t2049 = -t388; + const auto t2050 = 3 * t158; + const auto t2051 = 3 * t1670; + const auto t2052 = + t1668 * t2050 - t1668 + t2048 + t2049 + t2051 * t295 + t2051 * t299; + const auto t2053 = t28 * t752; + const auto t2054 = t1670 * t1811; + const auto t2055 = + t2052 * t207 + t2053 * t207 + t2054 * t310 + t302 * t389 + t750; + const auto t2056 = -t1 * t2052 * t35 - t1 * t28 * t35 * t752 + - 3 * t12 * t28 * t59 * t86 - t12 * t302 * t35 + t303; + const auto t2057 = t1 * t302 + t2051 * t87 + t2052 + t2053; + const auto t2058 = t1681 * t750 + t1683 * t750; + const auto t2059 = t1937 + * (t1474 * t2057 + t1742 * t780 + t2055 * t91 - t2056 * t93 + + t2058); + const auto t2060 = t1 * t1484; + const auto t2061 = t3 * t80; + const auto t2062 = t1 * t77; + const auto t2063 = 3 * t174; + const auto t2064 = 3 * t178; + const auto t2065 = + t1948 * t64 + t2061 * t2063 - t2061 + t2062 * t2064 - t2062; + const auto t2066 = t2049 + t2065; + const auto t2067 = t174 * t80; + const auto t2068 = t127 * t19 + t129 * t19 + t2067; + const auto t2069 = -t141 - t2068; + const auto t2070 = t2069 * t3; + const auto t2071 = t140 * t1811; + const auto t2072 = -t2070 + t2071; + const auto t2073 = t142 + t2068; + const auto t2074 = -t2073; + const auto t2075 = t1493 * t2074; + const auto t2076 = t3 * t91; + const auto t2077 = t1935 * t750 + t2076 * t750 + t780 * t93; + const auto t2078 = t1937 * t2077; + const auto t2079 = t1492 * t2078; + const auto t2080 = d_x * t1; + const auto t2081 = t1670 * t35; + const auto t2082 = + t1498 * t2081 + t1501 * t2081 + t1691 * t2050 - t1691 + t2080; + const auto t2083 = t28 * t722; + const auto t2084 = + t1523 * t2054 + t207 * t2082 + t207 * t2083 + t222 * t235 + t723; + const auto t2085 = -t1 * t2082 * t35 - t1 * t28 * t35 * t722 + - 3 * t10 * t12 * t28 * t59 - t12 * t218 * t35 + t219; + const auto t2086 = t1 * t218 + t2051 * t212 + t2082 + t2083; + const auto t2087 = -t13 * t219 * t35 * t723 - t237 * t3 * t35 * t723; + const auto t2088 = t155 + * (t1 * t219 * t35 * t729 - t1516 * t2086 - t18 * t2084 + + t2085 * t21 - t2087); + const auto t2089 = d_y * t174; + const auto t2090 = t19 * t4; + const auto t2091 = t19 * t8; + const auto t2092 = t2089 + t2090 + t2091 + t717; + const auto t2093 = t2092 * t3; + const auto t2094 = t1811 * t20; + const auto t2095 = d_y * t3; + const auto t2096 = -t2080 - t2095; + const auto t2097 = t1501 * t64 + t2063 * t2095 + t2064 * t2080 + t2096; + const auto t2098 = -t1 * t1527 + t2094 + t2097; + const auto t2099 = t465 * t663; + const auto t2100 = t1063 * t723 + t21 * t729 + t512 * t723; + const auto t2101 = t20 + t2092; + const auto t2102 = t1540 * t2101; + const auto t2103 = t1968 * t2100; + const auto t2104 = -3 * t1064 * t1536 * t2100 * t24 * t59 + t2088 * t317 + + t2099 * (-t2093 - t2098) + t2102 * t22 + t2103 * t22; + const auto t2105 = t1 * t37; + const auto t2106 = + t1713 * t2050 - t1713 + t2049 + t2051 * t260 + t2051 * t264 + t2105; + const auto t2107 = t28 * t765; + const auto t2108 = + t2054 * t272 + t207 * t2106 + t207 * t2107 + t270 * t389 + t770; + const auto t2109 = -t1 * t2106 * t35 - t1 * t28 * t35 * t765 + - t12 * t270 * t35 - 3 * t12 * t28 * t45 * t59 + t279; + const auto t2110 = t222 * t769; + const auto t2111 = t1 * t270 + t2051 * t46 + t2106 + t2107; + const auto t2112 = t1723 * t770 + t1725 * t770; + const auto t2113 = t1978 + * (t1561 * t2111 + t2108 * t48 - t2109 * t50 + t2110 * t279 + + t2112); + const auto t2114 = t1 * t1568; + const auto t2115 = t3 * t39; + const auto t2116 = t1 * t36; + const auto t2117 = + t1989 * t64 + t2063 * t2115 + t2064 * t2116 - t2115 - t2116; + const auto t2118 = t2049 + t2117; + const auto t2119 = t174 * t39; + const auto t2120 = t104 * t19 + t106 * t19 + t2119; + const auto t2121 = -t110 - t2120; + const auto t2122 = t2121 * t3; + const auto t2123 = t108 * t1811; + const auto t2124 = -t2122 + t2123; + const auto t2125 = t111 + t2120; + const auto t2126 = -t2125; + const auto t2127 = t1575 * t2126; + const auto t2128 = t3 * t48; + const auto t2129 = t1976 * t770 + t2128 * t770 + t50 * t769; + const auto t2130 = t1978 * t2129; + const auto t2131 = t1574 * t2130; + const auto t2132 = t12 * t1583; + const auto t2133 = -t1811 * t2132; + const auto t2134 = t19 * t2122 + t2133; + const auto t2135 = -t1568 * t174 + t1574; + const auto t2136 = t1 * t2121; + const auto t2137 = t107 * t174; + const auto t2138 = t107 + t2136 - t2137; + const auto t2139 = -t2097; + const auto t2140 = t19 * t2093; + const auto t2141 = t10 * t1812; + const auto t2142 = t1059 + t1527 * t174 - t2141; + const auto t2143 = t1 * t2092; + const auto t2144 = t10 * t174; + const auto t2145 = t10 - t2144; + const auto t2146 = t2143 - t2145; + const auto t2147 = -3 * t1064 * t1536 * t21 * t2100 * t59 + + t1595 * (t19 * t2139 - t2140 + t2142) + t19 * t2103 + + t2013 * t2146 + t2088 * t331; + const auto t2148 = 3 * t112 * t1571 * t1572 * t2129 * t35 - t112 * t2113 + + t125 * t15 * (t1 * t15 * t2118 - t2134 - t2135) - t19 * t2131 + - t2008 * t2138 - t2147; + const auto t2149 = t12 * t1600; + const auto t2150 = -t1811 * t2149; + const auto t2151 = t19 * t2070 + t2150; + const auto t2152 = -t1484 * t174 + t1492; + const auto t2153 = t1 * t2069; + const auto t2154 = t130 * t174; + const auto t2155 = t130 + t2153 - t2154; + const auto t2156 = 3 * t143 * t1489 * t1490 * t2077 * t35 - t143 * t2059 + + t145 * t15 * (t1 * t15 * t2066 - t2151 - t2152) - t19 * t2079 + - t2022 * t2155 - t2147; + const auto t2157 = -t1 * t1982; + const auto t2158 = -t1568 * t64 + t2157; + const auto t2159 = t178 * t2121 + t2125; + const auto t2160 = t178 * t2092; + const auto t2161 = t10 * t1658; + const auto t2162 = t1528 * t19 + t2101 - t2161; + const auto t2163 = -3 * t1064 * t1536 * t18 * t2100 * t59 + + t1595 * (t16 * t2139 - t2160 + t2162) + t16 * t2102 + + t2014 * t2100 + t2088 * t287; + const auto t2164 = -t1 * t1941; + const auto t2165 = -t1484 * t64 + t2164; + const auto t2166 = t178 * t2069 + t2073; + const auto t2167 = t1653 * t19; + const auto t2168 = t1646 * t174; + const auto t2169 = t16 * t1648; + const auto t2170 = t222 * t753; + const auto t2171 = t293 + * (-t2055 * t308 + t2056 * t304 - t2057 * t307 + t2058 + - t2170 * t303); + const auto t2172 = t314 * t755; + const auto t2173 = t219 * t222; + const auto t2174 = t155 + * (-t2084 * t234 + t2085 * t225 - t2086 * t380 - t2087 + - t2173 * t728); + const auto t2175 = t2030 * t317; + const auto t2176 = -t155 * t219 * t35 * t7 * t736 + t2029 * t2086 + + t2174 * t24 + t2175 * t736 + t288 * t734; + const auto t2177 = t222 * t768; + const auto t2178 = t277 + * (-t2108 * t284 + t2109 * t280 - t2111 * t283 + t2112 + - t2177 * t279); + const auto t2179 = t2034 * t773; + const auto t2180 = t207 * t288; + const auto t2181 = -t155 * t237 * t35 * t736 + t18 * t2174 + + t2036 * t736 + t2084 * t26 + t2180 * t723; + const auto t2182 = t1637 * t64; + const auto t2183 = t1640 * t166; + const auto t2184 = t1130 * t2044; + const auto t2185 = t1653 * t1658; + const auto t2186 = t1648 * t1812; + const auto t2187 = -t101 * t798 - t102 * t797 + t55 * t795 + t793 * t96; + const auto t2188 = t2187 * t493; + const auto t2189 = + t126 * t789 - t150 * t787 - t152 * t781 + t153 * t776; + const auto t2190 = t2189 * t486; + const auto t2191 = t1650 * t1815; + const auto t2192 = t154 + * (t1646 + t2045 + t2167 - t2168 - t2169 + t2182 + t2183 - t2184 + - t2185 + t2186 + t2188 - t2190 - t2191 + - t222 + * (-t101 + * (-t1630 * t774 + t2111 * t35 * t54 * t7 - t2176 + - t2178 * t52 - t2179 * t52 + + t277 * t285 * t7 * t770) + - t102 + * (-t2038 * t2172 + t2055 * t95 - t2171 * t91 - t2181 + + t293 * t3 * t314 * t750 - t313 * t756) + + t290 * t795 + t319 * t793 - t320 * t798 - t322 * t797 + + t55 + * (-t1613 * t756 - t1688 * t2172 + + t2057 * t35 * t7 * t95 - t2171 * t89 - t2176 + + t293 * t314 * t7 * t750) + + t96 + * (t2108 * t54 - t2178 * t48 - t2179 * t48 - t2181 + - t276 * t774 + t277 * t285 * t3 * t770)) + + t3 + * (t126 + * (3 * t134 * t1489 * t1490 * t2077 * t35 + - t134 * t2059 + + t145 * t465 * t7 * (t2060 + t2066 + t2072) + - t2075 * t22 - t2079 * t22 - t2104) + + t146 * t2148 + - t150 + * (3 * t123 * t1571 * t1572 * t2129 * t35 + - t123 * t2113 + + t125 * t465 * t7 * (t2114 + t2118 + t2124) + - t2104 - t2127 * t22 - t2131 * t22) + - t151 * t2156 + t1922 * t760 - t1924 * t779 + + t2046 * t329 - t2047 * t335) + + t7 + * (-t126 + * (3 * t139 * t1489 * t1490 * t2077 * t35 + - t139 * t2059 + + t145 * t15 * (t15 * t2066 * t3 - t2165 - t2166) + - t16 * t2075 - t2024 * t2077 - t2163) + + t150 + * (3 * t118 * t1571 * t1572 * t2129 * t35 + - t118 * t2113 + + t125 * t15 * (t15 * t2118 * t3 - t2158 - t2159) + - t16 * t2127 - t2010 * t2129 - t2163) + + t152 * t2156 - t153 * t2148 - t1922 * t789 + + t1924 * t787 - t2046 * t342 + t2047 * t339)); + const auto t2193 = + -t126 * t845 - t146 * t857 + t150 * t859 + t151 * t861; + const auto t2194 = t2193 * t35; + const auto t2195 = t35 * t857; + const auto t2196 = t35 * t861; + const auto t2197 = t37 * t7; + const auto t2198 = -t390; + const auto t2199 = 3 * t186; + const auto t2200 = 3 * t1845; + const auto t2201 = + t1842 * t2199 - t1842 + t2197 + t2198 + t2200 * t260 + t2200 * t262; + const auto t2202 = t28 * t850; + const auto t2203 = t1811 * t1845; + const auto t2204 = + t207 * t2201 + t207 * t2202 + t2203 * t272 + t270 * t391 + t849; + const auto t2205 = -t13 * t270 * t35 - 3 * t13 * t28 * t45 * t59 + - t2201 * t35 * t7 + t279 - t28 * t35 * t7 * t850; + const auto t2206 = -t2205; + const auto t2207 = t2200 * t46 + t2201 + t2202 + t270 * t7; + const auto t2208 = t1723 * t849 + t1856 * t849; + const auto t2209 = t1978 + * (t1560 * t2207 + t1630 * t858 + t2204 * t48 + t2206 * t52 + + t2208); + const auto t2210 = t1568 * t7; + const auto t2211 = t3 * t42; + const auto t2212 = t36 * t7; + const auto t2213 = 3 * t157; + const auto t2214 = + t1988 * t66 + t2064 * t2212 + t2211 * t2213 - t2211 - t2212; + const auto t2215 = t2198 + t2214; + const auto t2216 = t157 * t42; + const auto t2217 = t104 * t22 + t105 * t22 + t2216; + const auto t2218 = -t121 - t2217; + const auto t2219 = t2218 * t3; + const auto t2220 = t119 * t1811; + const auto t2221 = -t2219 + t2220; + const auto t2222 = t122 + t2217; + const auto t2223 = -t2222; + const auto t2224 = t1575 * t2223; + const auto t2225 = t1975 * t849 + t2128 * t849 + t52 * t858; + const auto t2226 = t1978 * t2225; + const auto t2227 = t1574 * t2226; + const auto t2228 = d_x * t7; + const auto t2229 = + t1498 * t1852 + t1500 * t1852 + t1864 * t2199 - t1864 + t2228; + const auto t2230 = t28 * t808; + const auto t2231 = + t1523 * t2203 + t207 * t2229 + t207 * t2230 + t227 * t235 + t813; + const auto t2232 = -3 * t10 * t13 * t28 * t59 - t13 * t218 * t35 + t219 + - t2229 * t35 * t7 - t28 * t35 * t7 * t808; + const auto t2233 = -t2232; + const auto t2234 = t212 * t2200 + t218 * t7 + t2229 + t2230; + const auto t2235 = -t12 * t219 * t35 * t813 - t237 * t3 * t35 * t813; + const auto t2236 = t155 + * (-t1515 * t2234 - t18 * t2231 + t219 * t35 * t7 * t810 + - t2233 * t24 - t2235); + const auto t2237 = t3 * t612; + const auto t2238 = d_z * t3; + const auto t2239 = -t2228 - t2238; + const auto t2240 = t1500 * t66 + t2064 * t2228 + t2213 * t2238 + t2239; + const auto t2241 = t1811 * t23 + t2240; + const auto t2242 = -t1527 * t7 + t2241; + const auto t2243 = t465 * t527; + const auto t2244 = t191 * t813 + t24 * t810 + t512 * t813; + const auto t2245 = t1540 * t613; + const auto t2246 = t1968 * t2244; + const auto t2247 = -3 * t1064 * t1536 * t21 * t2244 * t59 + t19 * t2245 + + t19 * t2246 + t2236 * t331 + t2243 * (-t2237 - t2242); + const auto t2248 = t1 * t125 * t465 * (t2210 + t2215 + t2221) + + 3 * t112 * t1571 * t1572 * t2225 * t35 - t112 * t2209 + - t19 * t2224 - t19 * t2227 - t2247; + const auto t2249 = t7 * t78; + const auto t2250 = + t1884 * t2199 - t1884 + t2198 + t2200 * t295 + t2200 * t297 + t2249; + const auto t2251 = t28 * t834; + const auto t2252 = + t207 * t2250 + t207 * t2251 + t2203 * t310 + t302 * t391 + t839; + const auto t2253 = -3 * t13 * t28 * t59 * t86 - t13 * t302 * t35 + - t2250 * t35 * t7 - t28 * t35 * t7 * t834 + t303; + const auto t2254 = -t2253; + const auto t2255 = t2200 * t87 + t2250 + t2251 + t302 * t7; + const auto t2256 = t1681 * t839 + t1893 * t839; + const auto t2257 = t1937 + * (t1473 * t2255 + t1613 * t838 + t2252 * t91 + t2254 * t89 + + t2256); + const auto t2258 = t1484 * t7; + const auto t2259 = t3 * t83; + const auto t2260 = t7 * t77; + const auto t2261 = + t1947 * t66 + t2064 * t2260 + t2213 * t2259 - t2259 - t2260; + const auto t2262 = t2198 + t2261; + const auto t2263 = t157 * t83; + const auto t2264 = t127 * t22 + t128 * t22 + t2263; + const auto t2265 = -t132 - t2264; + const auto t2266 = t2265 * t3; + const auto t2267 = t131 * t1811; + const auto t2268 = -t2266 + t2267; + const auto t2269 = t133 + t2264; + const auto t2270 = -t2269; + const auto t2271 = t1493 * t2270; + const auto t2272 = t1934 * t839 + t2076 * t839 + t838 * t89; + const auto t2273 = t1937 * t2272; + const auto t2274 = t1492 * t2273; + const auto t2275 = t1 * t145 * t465 * (t2258 + t2262 + t2268) + + 3 * t143 * t1489 * t1490 * t2272 * t35 - t143 * t2257 + - t19 * t2271 - t19 * t2274 - t2247; + const auto t2276 = t22 * t2266; + const auto t2277 = t13 * t1600; + const auto t2278 = -t1811 * t2277; + const auto t2279 = -t15 * t2262 * t7 + t2278; + const auto t2280 = -t1484 * t157 + t1492; + const auto t2281 = t130 * t157; + const auto t2282 = t130 + t2265 * t7 - t2281; + const auto t2283 = -t2240; + const auto t2284 = t22 * t2283; + const auto t2285 = t22 * t2237; + const auto t2286 = t1247 * t1811; + const auto t2287 = t10 * t2286; + const auto t2288 = -t2287; + const auto t2289 = t1059 + t1527 * t157 + t2288; + const auto t2290 = t612 * t7; + const auto t2291 = t10 * t157; + const auto t2292 = t10 - t2291; + const auto t2293 = t2290 - t2292; + const auto t2294 = -3 * t1064 * t1536 * t2244 * t24 * t59 + + t1595 * (t2284 - t2285 + t2289) + t2013 * t2293 + t22 * t2246 + + t2236 * t317; + const auto t2295 = t22 * t2219; + const auto t2296 = t13 * t1583; + const auto t2297 = -t1811 * t2296; + const auto t2298 = -t15 * t2215 * t7 + t2297; + const auto t2299 = -t1568 * t157 + t1574; + const auto t2300 = t107 * t157; + const auto t2301 = t107 + t2218 * t7 - t2300; + const auto t2302 = -t15 * t2215 * t3; + const auto t2303 = -t1982 * t7; + const auto t2304 = -t1568 * t66 + t2303; + const auto t2305 = t178 * t2218 + t2222; + const auto t2306 = t16 * t2283; + const auto t2307 = t178 * t612; + const auto t2308 = t10 * t1661; + const auto t2309 = -t2308; + const auto t2310 = t1528 * t22 + t2309 + t613; + const auto t2311 = -3 * t1064 * t1536 * t18 * t2244 * t59 + + t1595 * (t2306 - t2307 + t2310) + t16 * t2245 + t2014 * t2244 + + t2236 * t287; + const auto t2312 = -t15 * t2262 * t3; + const auto t2313 = -t1941 * t7; + const auto t2314 = -t1484 * t66 + t2313; + const auto t2315 = t178 * t2265 + t2269; + const auto t2316 = t227 * t837; + const auto t2317 = t293 + * (-t2252 * t308 + t2253 * t306 - t2255 * t305 + t2256 + - t2316 * t303); + const auto t2318 = t314 * t841; + const auto t2319 = t155 + * (-t1036 * t809 - t1615 * t2234 - t2231 * t234 + t2232 * t230 + - t2235); + const auto t2320 = -t155 * t219 * t35 * t7 * t815 + t2175 * t815 + + t2233 * t26 + t2319 * t24 + t288 * t811; + const auto t2321 = t227 * t851; + const auto t2322 = t277 + * (-t2204 * t284 + t2205 * t282 - t2207 * t281 + t2208 + - t2321 * t279); + const auto t2323 = t2034 * t853; + const auto t2324 = -t155 * t237 * t35 * t815 + t18 * t2319 + + t2036 * t815 + t2180 * t813 + t2231 * t26; + const auto t2325 = t1130 * t2193; + const auto t2326 = -t101 * t874 - t102 * t873 + t55 * t872 + t871 * t96; + const auto t2327 = t2326 * t493; + const auto t2328 = + t126 * t867 - t150 * t866 - t152 * t861 + t153 * t857; + const auto t2329 = t2328 * t486; + const auto t2330 = t16 * t1650; + const auto t2331 = t1653 * t22; + const auto t2332 = t1653 * t1661; + const auto t2333 = t1650 * t2286; + const auto t2334 = t1648 * t1815; + const auto t2335 = t2330 + t2331 - t2332 - t2333 + t2334; + const auto t2336 = + t157 * t1640 + t1637 * t66 + t1639 - t1646 * t166 + t2335; + const auto t2337 = t154 + * (t2194 + - t222 + * (-t101 + * (-t1630 * t854 + t2206 * t54 - t2320 - t2322 * t52 + - t2323 * t52 + t277 * t285 * t858) + - t102 + * (-t2038 * t2318 + t2252 * t95 - t2317 * t91 - t2324 + + t293 * t3 * t314 * t839 - t313 * t842) + + t290 * t872 + t319 * t871 - t320 * t874 - t322 * t873 + + t55 + * (-t1613 * t842 - t1688 * t2318 + t2254 * t95 + - t2317 * t89 - t2320 + t293 * t314 * t838) + + t96 + * (t2204 * t54 - t2322 * t48 - t2323 * t48 - t2324 + - t276 * t854 + t277 * t285 * t3 * t849)) + - t2325 + t2327 - t2329 + t2336 + + t3 + * (t126 + * (3 * t134 * t1489 * t1490 * t2272 * t35 + - t134 * t2257 + + t145 * t15 * (-t2276 - t2279 - t2280) + - t2022 * t2282 - t22 * t2274 - t2294) + + t146 * t2248 + - t150 + * (3 * t123 * t1571 * t1572 * t2225 * t35 + - t123 * t2209 + + t125 * t15 * (-t2295 - t2298 - t2299) + - t2008 * t2301 - t22 * t2227 - t2294) + - t151 * t2275 + t1922 * t845 - t1924 * t859 + + t2195 * t329 - t2196 * t335) + + t7 + * (-t126 + * (3 * t139 * t1489 * t1490 * t2272 * t35 + - t139 * t2257 + + t145 * t15 * (-t2312 - t2314 - t2315) + - t16 * t2271 - t2024 * t2272 - t2311) + + t150 + * (3 * t118 * t1571 * t1572 * t2225 * t35 + - t118 * t2209 + + t125 * t15 * (-t2302 - t2304 - t2305) + - t16 * t2224 - t2010 * t2225 - t2311) + + t152 * t2275 - t153 * t2248 - t1922 * t867 + + t1924 * t866 - t2195 * t342 + t2196 * t339)); + const auto t2338 = t19 * t50; + const auto t2339 = t22 * t52; + const auto t2340 = t207 * t279; + const auto t2341 = std::pow(t53, -2); + const auto t2342 = 3 * t285; + const auto t2343 = t2341 * t2342; + const auto t2344 = t52 * t893; + const auto t2345 = t1418 - t15 * t285 * t3 * t7 * t892; + const auto t2346 = t101 * t54; + const auto t2347 = t251 * t59; + const auto t2348 = t1670 * t2347; + const auto t2349 = t1845 * t2347; + const auto t2350 = t279 * t3; + const auto t2351 = + -t1000 * t2350 - t1476 * t2350 + t242 * t425 + t276 * t58; + const auto t2352 = t2348 * t280 + t2349 * t282 + t2351 + t281 + t283; + const auto t2353 = t2352 * t880; + const auto t2354 = t285 * t880; + const auto t2355 = -t1421; + const auto t2356 = std::pow(t360, -2); + const auto t2357 = t2356 * t879; + const auto t2358 = t2357 * t285; + const auto t2359 = t1788 * t2358 + t2354 * t58 + t2355 - t276 * t881; + const auto t2360 = t2353 * t284 + t2359; + const auto t2361 = t361 * t96; + const auto t2362 = -t52 * t894 + t66; + const auto t2363 = t2362 * t320 * t54 + t319 * t897; + const auto t2364 = t225 * t358 - t304 * t404; + const auto t2365 = t1630 * t881 - t1783 * t2358; + const auto t2366 = t2354 * t391; + const auto t2367 = t227 + t2349 + t2366; + const auto t2368 = t2342 * t280; + const auto t2369 = t1735 * t881 - t2357 * t2368; + const auto t2370 = t2354 * t389; + const auto t2371 = t222 + t2348 + t2370; + const auto t2372 = -t2352 * t914 + t2369 + t2371; + const auto t2373 = t1751 * t225; + const auto t2374 = + t1742 * t404 - t1756 * t304 - t2173 * t358 + t2373 * t238; + const auto t2375 = -t1762 * t889 + t2374 * t885; + const auto t2376 = t1760 * t889 + t2374 * t883; + const auto t2377 = t361 * (t2364 * t885 - t405 * t889); + const auto t2378 = t2346 * t2362 + t897 * t96; + const auto t2379 = t2378 * t64; + const auto t2380 = t178 * t2377; + const auto t2381 = t361 * (t2364 * t883 + t411 * t889); + const auto t2382 = t2381 * t66; + const auto t2383 = t2377 + t2379 - t2380 - t2382; + const auto t2384 = t15 * t48; + const auto t2385 = -2 * t1 * t3 * t465 * t50 - 2 * t11 * t465 * t48 + + t15 * t1723 + t2384 - 2 * t3 * t465 * t52 * t7; + const auto t2386 = t2343 * t48; + const auto t2387 = t54 * t96; + const auto t2388 = -t1725 + t279 * t911; + const auto t2389 = t280 * t35; + const auto t2390 = 2 * t1670; + const auto t2391 = 2 * t1845; + const auto t2392 = -t1723 + t1781 * t2391 + t2389 * t2390; + const auto t2393 = t116 + t1544 + t209 * t284 + t2392; + const auto t2394 = t2388 + t2393; + const auto t2395 = t880 * t913; + const auto t2396 = t482 * t59; + const auto t2397 = t1670 * t2396; + const auto t2398 = t2354 * t909 + t2397; + const auto t2399 = + t1630 * t2395 - 3 * t2356 * t282 * t285 * t913 + t2398; + const auto t2400 = -t1 * t2394 * t282 * t35 * t880 + t2399; + const auto t2401 = t101 * t361; + const auto t2402 = t54 * t906; + const auto t2403 = t2402 * t319 - t320 * t54 * t919; + const auto t2404 = t222 * t2394; + const auto t2405 = -t1000 * t1451; + const auto t2406 = t2356 * t913; + const auto t2407 = -t1 * t279 * t35 * t880 * t913 + t2354 * t911 + + t2368 * t2406 + t2405; + const auto t2408 = -t2404 * t914 - t2407; + const auto t2409 = -t2395 * t282 + t909; + const auto t2410 = t1762 * t917 + t2374 * t2409; + const auto t2411 = t284 * t880; + const auto t2412 = -t1788 * t2406 * t285 + t2395 * t276; + const auto t2413 = -t2395 * t284 + t389; + const auto t2414 = t1760 * t917 + t2374 * t2413; + const auto t2415 = t361 * (t2364 * t2409 + t405 * t917); + const auto t2416 = t101 * t919 - t906 * t96; + const auto t2417 = t2416 * t54; + const auto t2418 = t2364 * t2413 + t411 * t917; + const auto t2419 = t2418 * t361; + const auto t2420 = -t178 * t2415 + t2415 + t2417 * t64 + t2419 * t66; + const auto t2421 = t361 * (t2364 * t929 + t405 * t930); + const auto t2422 = t178 * t2421; + const auto t2423 = t361 * (-t2364 * t932 + t411 * t930); + const auto t2424 = t2423 * t66; + const auto t2425 = t101 * t939 + t938 * t96; + const auto t2426 = t2425 * t64; + const auto t2427 = -t1856 + t279 * t923; + const auto t2428 = t2393 + t2427; + const auto t2429 = t227 * t2428; + const auto t2430 = t2356 * t925; + const auto t2431 = t1735 * t926 - t2368 * t2430 + t2398; + const auto t2432 = -t2429 * t914 + t2431; + const auto t2433 = t283 * t880; + const auto t2434 = -t1451 * t1476; + const auto t2435 = t2430 * t285; + const auto t2436 = -t1630 * t926 + t1783 * t2435 + t2354 * t923 + t2434; + const auto t2437 = t2428 * t2433 + t2436; + const auto t2438 = t1762 * t930 + t2374 * t929; + const auto t2439 = -t1788 * t2435 + t276 * t926; + const auto t2440 = t1760 * t930 - t2374 * t932; + const auto t2441 = t319 * t938 + t320 * t939; + const auto t2442 = t145 * t334; + const auto t2443 = t19 * t93; + const auto t2444 = t22 * t89; + const auto t2445 = t516 * t93; + const auto t2446 = t565 * t89; + const auto t2447 = t207 * t303; + const auto t2448 = -t1421 * t91 - t157 * t2447 - t174 * t2447 - t2443 + - t2444 + t2445 + t2446 - t313 * t57; + const auto t2449 = t89 * t948; + const auto t2450 = std::pow(t94, -2); + const auto t2451 = 3 * t314; + const auto t2452 = t2450 * t2451; + const auto t2453 = t2452 * t942; + const auto t2454 = t314 * t948; + const auto t2455 = t1418 - t2454 * t66; + const auto t2456 = t1613 * t949 - t2448 * t2449 + t2453 * t89 + t2455; + const auto t2457 = t126 * t95; + const auto t2458 = -t2454 * t64; + const auto t2459 = t1742 * t949 + t2453 * t93 + t2458; + const auto t2460 = t1426 - t2448 * t996 + t2459; + const auto t2461 = t2454 * t57; + const auto t2462 = t91 * t949; + const auto t2463 = t2462 + t291; + const auto t2464 = t2463 * t95; + const auto t2465 = t145 * t946; + const auto t2466 = t2464 * t334 + t2465 * t339; + const auto t2467 = t55 * t95; + const auto t2468 = t3 * t303; + const auto t2469 = + -t1000 * t2468 - t1476 * t2468 + t242 * t448 + t313 * t58; + const auto t2470 = t314 * t957; + const auto t2471 = t956 * t957; + const auto t2472 = std::pow(t403, -2); + const auto t2473 = t2451 * t2472; + const auto t2474 = t308 * t956; + const auto t2475 = t2355 + t2470 * t58 - t2471 * t313 + t2473 * t2474; + const auto t2476 = t102 * t404; + const auto t2477 = t290 * t95; + const auto t2478 = t2477 * t962 + t322 * t961; + const auto t2479 = t145 * (t126 * t945 - t151 * t946); + const auto t2480 = t102 * t961 + t2467 * t962; + const auto t2481 = t2480 * t64; + const auto t2482 = t178 * t2479; + const auto t2483 = t2457 * t2463; + const auto t2484 = t152 * t2465; + const auto t2485 = t2483 + t2484; + const auto t2486 = t2485 * t66; + const auto t2487 = t2479 + t2481 - t2482 - t2486; + const auto t2488 = t1023 * t93; + const auto t2489 = t1025 * t89; + const auto t2490 = t15 * t1681 - t2488 - t2489; + const auto t2491 = t1030 * t303 + t2490 + t303 * t475; + const auto t2492 = t15 * t91; + const auto t2493 = t1414 * t91; + const auto t2494 = t2492 - t2493; + const auto t2495 = -t2491 - t2494; + const auto t2496 = t1 * t2495; + const auto t2497 = t2452 * t975; + const auto t2498 = t948 * t975; + const auto t2499 = t1613 * t2498 - t166 * t2454 + t2497 * t89 + t484; + const auto t2500 = t1070 - t164 * t2454 + t1742 * t2498 + t2497 * t93; + const auto t2501 = t916 + t975 * t996; + const auto t2502 = t2501 * t95; + const auto t2503 = -t134 * t943 * t975 + t166; + const auto t2504 = t2442 * t2503 + t2502 * t335; + const auto t2505 = t313 * t948; + const auto t2506 = t95 + * (t1 * t2495 * t91 * t948 - t1426 - t2458 - t2497 * t91 + - t2505 * t975); + const auto t2507 = -t1683 + t303 * t911; + const auto t2508 = t304 * t35; + const auto t2509 = -t1681 + t1765 * t2391 + t2390 * t2508; + const auto t2510 = t137 + t1449 + t209 * t308 + t2509; + const auto t2511 = t2507 + t2510; + const auto t2512 = t2397 + t2470 * t909; + const auto t2513 = + t1613 * t968 - 3 * t2472 * t306 * t314 * t967 + t2512; + const auto t2514 = t404 * t55; + const auto t2515 = t166 - t2449 * t975; + const auto t2516 = -t2515 * t290 * t95 + t322 * t978; + const auto t2517 = t305 * t957; + const auto t2518 = + -t1742 * t968 + t2405 + t2470 * t911 + t2473 * t304 * t967; + const auto t2519 = t1230 * t290 + t333 * t978; + const auto t2520 = t145 * t2503; + const auto t2521 = t126 * t2520 + t151 * t2501 * t95; + const auto t2522 = t1230 * t55 + t462 * t978; + const auto t2523 = -t102 * t977 + t2515 * t55; + const auto t2524 = t2523 * t95; + const auto t2525 = -t178 * t2521 + t2521 + t2522 * t66 + t2524 * t64; + const auto t2526 = -t1893 + t303 * t923; + const auto t2527 = t2510 + t2526; + const auto t2528 = t957 * t992; + const auto t2529 = + -t1613 * t2528 + t2434 + t2470 * t923 + t2473 * t306 * t992; + const auto t2530 = t227 * t2527 * t993 + t2529; + const auto t2531 = t404 * t462; + const auto t2532 = + t1742 * t2528 - 3 * t2472 * t304 * t314 * t992 + t2512; + const auto t2533 = t2527 * t304 * t35 * t7 * t957 - t2532; + const auto t2534 = t322 * t95; + const auto t2535 = t2534 * t997 + t333 * t998; + const auto t2536 = t91 * t982; + const auto t2537 = t95 + * (-t2452 * t2536 - t2455 - t2505 * t982 + + t7 * t91 * t948 + * (-t1242 * t303 - t2490 - t2494 - t303 * t558)); + const auto t2538 = t95 * t987; + const auto t2539 = -t2477 * t997 + t2538 * t333; + const auto t2540 = t2534 * t987 + t290 * t998; + const auto t2541 = t102 * t95; + const auto t2542 = t2541 * t997; + const auto t2543 = t462 * t998; + const auto t2544 = t2542 + t2543; + const auto t2545 = t178 * t2544; + const auto t2546 = t102 * t2538 + t55 * t998; + const auto t2547 = t2546 * t64; + const auto t2548 = -t126 * t985 + t152 * t984; + const auto t2549 = t145 * t2548; + const auto t2550 = t2549 * t66; + const auto t2551 = t2544 - t2545 + t2547 + t2550; + const auto t2552 = t225 * t365 + t225 + t381; + const auto t2553 = t2552 + t386; + const auto t2554 = t35 * (t230 * t2553 * t3 * t363 - t400); + const auto t2555 = t35 * (t240 * t2553 * t363 + t409); + const auto t2556 = t15 * t21; + const auto t2557 = 2 * t1069; + const auto t2558 = t21 * t2557; + const auto t2559 = -t2556 + t2558 + t472; + const auto t2560 = -t2559 - t476; + const auto t2561 = t2560 * t562 + t488; + const auto t2562 = -t16; + const auto t2563 = t1070 + t2562; + const auto t2564 = t2563 - t494; + const auto t2565 = t2560 * t607 + t2564 + t331 * t518 - t492; + const auto t2566 = t2560 * t513 + t519; + const auto t2567 = t1097 + t1099 * t379 + t222 * t378 + t231 + t240; + const auto t2568 = + t35 * (-t1104 + t230 * t2567 * t363 - t379 * t909 - t7); + const auto t2569 = + t35 * (-t1106 - t222 * t377 + t234 * t2567 * t363 - t3); + const auto t2570 = t1429 * t3; + const auto t2571 = -t2570; + const auto t2572 = t1070 * t18; + const auto t2573 = t1063 * t2557; + const auto t2574 = t157 * t222; + const auto t2575 = 2 * t1 * t1094 * t21 * t35 - t1030 * t374 + - t1096 * t19 - t1413 - t2571 - t2572 - t2573 - t2574 * t384; + const auto t2576 = t2575 * t72; + const auto t2577 = t1038 * t245 * t397; + const auto t2578 = t1140 + t1417; + const auto t2579 = -t1312 + t2578; + const auto t2580 = -t1112 * t227 + t24 * t2576 + t2577 * t317 + t2579; + const auto t2581 = t1095 * t222; + const auto t2582 = -t1115 * t35 - t1161 * t397 + + 3 * t169 * t21 * t245 * t35 * t397 + t21 * t2575 * t72 - t2581; + const auto t2583 = -t1112 * t207 + t18 * t2576 + t2564 + t2577 * t287; + const auto t2584 = t1291 + t2552; + const auto t2585 = t35 * (t1298 + t231 * t2584 * t363); + const auto t2586 = t35 * (-t1300 + t234 * t2584 * t363 * t7); + const auto t2587 = -t1305 - t2559; + const auto t2588 = t1246 * t2587 + t1308; + const auto t2589 = t1163 * t1317 + t1270 * t2587 - t1311 + t2579; + const auto t2590 = t1259 * t2587 + t1318; + const auto t2591 = -t503; + const auto t2592 = -t509; + const auto t2593 = -t507; + const auto t2594 = -t510; + const auto t2595 = + t1937 * (t1474 * t1673 + t1678 * t91 + t1680 * t93 + t1684); + const auto t2596 = t2068 + t445; + const auto t2597 = t2596 * t3; + const auto t2598 = -2 * t1 * t3; + const auto t2599 = t2065 + t2598; + const auto t2600 = t207 * t91; + const auto t2601 = t1474 * t447 + t2600 * t447 + t451 * t93; + const auto t2602 = t140 + t2596; + const auto t2603 = t1493 * t2602; + const auto t2604 = t1448 * t2601; + const auto t2605 = t1492 * t2604; + const auto t2606 = + t155 * (-t1516 * t1698 - t1695 * t18 - t1697 * t21 - t1699); + const auto t2607 = -t2092; + const auto t2608 = t2607 * t3; + const auto t2609 = t1063 * t384 + t21 * t374 + t384 * t512; + const auto t2610 = -t2101; + const auto t2611 = t1540 * t2610; + const auto t2612 = t1968 * t2609; + const auto t2613 = -3 * t1064 * t1536 * t24 * t2609 * t59 + + t2099 * (t2098 - t2608) + t22 * t2611 + t22 * t2612 + + t2606 * t317; + const auto t2614 = + t1978 * (t1561 * t1716 + t1720 * t48 + t1722 * t50 + t1726); + const auto t2615 = t2120 + t422; + const auto t2616 = t2615 * t3; + const auto t2617 = t2117 + t2598; + const auto t2618 = t207 * t48; + const auto t2619 = t1561 * t424 + t2618 * t424 + t429 * t50; + const auto t2620 = t108 + t2615; + const auto t2621 = t1575 * t2620; + const auto t2622 = t1543 * t2619; + const auto t2623 = t1574 * t2622; + const auto t2624 = t19 * t2615; + const auto t2625 = t1587 + t2132 + t2624; + const auto t2626 = -t2617; + const auto t2627 = t1 * t2607; + const auto t2628 = t2145 + t2627; + const auto t2629 = -3 * t1064 * t1536 * t21 * t2609 * t59 + + t1595 * (t1 * t15 * t2097 - t19 * t2608 - t2142) + t19 * t2612 + + t2013 * t2628 + t2606 * t331; + const auto t2630 = 3 * t112 * t1571 * t1572 * t2619 - t112 * t2614 + + t125 * t15 * (-t19 * t2616 + t19 * t2626 + t2133 + t2135) + - t1575 * t2625 - t19 * t2623 - t2629; + const auto t2631 = t19 * t2596; + const auto t2632 = t1604 + t2149 + t2631; + const auto t2633 = -t2599; + const auto t2634 = 3 * t143 * t1489 * t1490 * t2601 - t143 * t2595 + + t145 * t15 * (-t19 * t2597 + t19 * t2633 + t2150 + t2152) + - t1493 * t2632 - t19 * t2605 - t2629; + const auto t2635 = -t522; + const auto t2636 = -t523; + const auto t2637 = -3 * t1064 * t1536 * t18 * t2609 * t59 + + t1595 * (t15 * t2097 * t3 - t178 * t2607 - t2162) + t16 * t2611 + + t2014 * t2609 + t2606 * t287; + const auto t2638 = t126 * t2591; + const auto t2639 = t146 * t2593; + const auto t2640 = t150 * t2592 + t151 * t2594 - t2638 - t2639; + const auto t2641 = t101 * t509; + const auto t2642 = t102 * t523; + const auto t2643 = -t2641 - t2642 + t503 * t55 + t522 * t96; + const auto t2644 = t150 * t2635; + const auto t2645 = t152 * t2594; + const auto t2646 = t126 * t2636 + t153 * t2593 - t2644 - t2645; + const auto t2647 = -t2167 + t2169 + t2185 - t2186 + t2191; + const auto t2648 = 2 * t507; + const auto t2649 = 2 * t510; + const auto t2650 = std::pow(t447, 2); + const auto t2651 = t379 * t81; + const auto t2652 = t2651 * t35; + const auto t2653 = t35 * t379; + const auto t2654 = 3 * t31; + const auto t2655 = t2654 * t310; + const auto t2656 = t2654 * t59; + const auto t2657 = t2656 * t79; + const auto t2658 = t2656 * t82; + const auto t2659 = t2656 * t85; + const auto t2660 = -t365 - 2; + const auto t2661 = t1471 + t2652 + t2653 * t446 + t2655 + t2657 + t2658 + + t2659 + t2660; + const auto t2662 = -t2661; + const auto t2663 = -t92; + const auto t2664 = 2 * f1_y; + const auto t2665 = t365 + 2; + const auto t2666 = 2 * t441; + const auto t2667 = 2 * e1_y; + const auto t2668 = -4 * e0_y + t2667; + const auto t2669 = t1 * t2655 + + t1 * (-t1461 + t2652 + t2657 + t2658 + t2659 - t2665) + t2663 + + t2664 + t2666 + t2668 + t295 * t379 + t297 * t379 + t299 * t379 + + t379 * t449; + const auto t2670 = t1476 * t2650 - t2508 * t2669 + t2650 * t60 + + t2662 * t307 + t2662 * t448 + std::pow(t451, 2); + const auto t2671 = t2670 * t293; + const auto t2672 = std::pow(t452, 2); + const auto t2673 = t1609 * t2672; + const auto t2674 = 2 * t498; + const auto t2675 = t2674 * t447; + const auto t2676 = std::pow(t384, 2); + const auto t2677 = t31 * t35; + const auto t2678 = t1498 * t2677 + t1500 * t2677 + t1501 * t2677; + const auto t2679 = 2 * d_y; + const auto t2680 = t2679 * t30; + const auto t2681 = t1512 + t2680; + const auto t2682 = t212 * t2654 + t2678 + t2681 + t371 * t379; + const auto t2683 = -t2682; + const auto t2684 = t240 * t35; + const auto t2685 = t1521 + t2680; + const auto t2686 = t224 + t2679 + 2 * t383; + const auto t2687 = t1 * t1523 * t2654 + t1505 * t367 + t1507 * t367 + + t1509 * t367 + t222 * (t2678 + t2685) + t2653 * t372 + t2686; + const auto t2688 = t186 * t2676 - t225 * t2687 + t2676 * t56 + + t2683 * t2684 + t2683 * t380 + t35 * std::pow(t374, 2); + const auto t2689 = t155 * t2688; + const auto t2690 = std::pow(t397, 2); + const auto t2691 = t1619 * t2690; + const auto t2692 = t384 * t500; + const auto t2693 = + -t1617 * t2682 - t2396 * t2692 + t24 * t2691 - t2689 * t317; + const auto t2694 = std::pow(t424, 2); + const auto t2695 = t379 * t40; + const auto t2696 = t2695 * t35; + const auto t2697 = t2654 * t272; + const auto t2698 = t2656 * t38; + const auto t2699 = t2656 * t41; + const auto t2700 = t2656 * t44; + const auto t2701 = t1558 + t1721 * t379 + t2660 + t2696 + t2697 + t2698 + + t2699 + t2700; + const auto t2702 = -t2701; + const auto t2703 = -t49; + const auto t2704 = 2 * f0_y; + const auto t2705 = 2 * t415; + const auto t2706 = t1 * t2697 + + t1 * (-t1552 - t2665 + t2696 + t2698 + t2699 + t2700) + + t260 * t379 + t262 * t379 + t264 * t379 + t2668 + t2703 + t2704 + + t2705 + t379 * t426; + const auto t2707 = t1476 * t2694 - t2389 * t2706 + t2694 * t60 + + t2702 * t283 + t2702 * t425 + std::pow(t429, 2); + const auto t2708 = t2707 * t277; + const auto t2709 = std::pow(t430, 2); + const auto t2710 = t1626 * t2709; + const auto t2711 = 2 * t504; + const auto t2712 = t2711 * t424; + const auto t2713 = + t21 * t2691 - t2687 * t505 - t2689 * t331 - 2 * t374 * t500 * t59; + const auto t2714 = + t1733 * t2706 - t2708 * t50 + t2710 * t50 + t2711 * t429 + t2713; + const auto t2715 = + t1739 * t2669 - t2671 * t93 + t2673 * t93 + t2674 * t451 + t2713; + const auto t2716 = t207 * t54; + const auto t2717 = t59 * t785; + const auto t2718 = + t18 * t2691 - t2347 * t2692 - t2682 * t2717 - t2689 * t287; + const auto t2719 = t207 * t95; + const auto t2720 = t2670 * t453; + const auto t2721 = t1767 * t2672; + const auto t2722 = t227 * t404; + const auto t2723 = 2 * t227; + const auto t2724 = t447 * t454; + const auto t2725 = t1777 * t2690; + const auto t2726 = t397 * t434; + const auto t2727 = t2726 * t384; + const auto t2728 = + t1297 * t2725 - t1770 * t2688 + t1772 * t2683 - t2396 * t2727; + const auto t2729 = t2707 * t431; + const auto t2730 = t1784 * t2709; + const auto t2731 = t424 * t432; + const auto t2732 = 2 * t207; + const auto t2733 = t3 * t358; + const auto t2734 = t2733 * t59; + const auto t2735 = + -t1758 * t2688 - t2347 * t2727 + t2683 * t2734 + t2725 * t408; + const auto t2736 = t227 * t361; + const auto t2737 = t174 * t1809; + const auto t2738 = std::pow(t1, 3); + const auto t2739 = t2738 * t465; + const auto t2740 = t1069 * t1660; + const auto t2741 = -t16 * t1804 + t1804 * t1812 - t1813 * t22 + + t1813 * t2740 + t1817 * t19 - t1817 * t2739; + const auto t2742 = t30 * t84; + const auto t2743 = t32 * t81; + const auto t2744 = t1 * t32; + const auto t2745 = t30 * t7; + const auto t2746 = -t2744 - t2745; + const auto t2747 = + t1841 * t442 + t1841 * t443 + t1841 * t444 + t2742 + t2743 + t2746; + const auto t2748 = t30 * t593; + const auto t2749 = t32 * t446; + const auto t2750 = t1841 * t2745; + const auto t2751 = + t227 * t2747 + t227 * t2748 + t227 * t2749 + t2750 * t310 + t447; + const auto t2752 = t1667 * t2744; + const auto t2753 = + t222 * t2747 + t222 * t2748 + t222 * t2749 + t2752 * t310 + t598; + const auto t2754 = t1841 * t441 + t2747 + t2748 + t2749; + const auto t2755 = t447 * t56; + const auto t2756 = t1682 * t598 + t1892 * t447 + t2755 * t598; + const auto t2757 = -t2751 * t306 - t2753 * t304 - t2754 * t448 + t2756; + const auto t2758 = t2757 * t293; + const auto t2759 = t452 * t599; + const auto t2760 = t447 * t600; + const auto t2761 = d_y * t32; + const auto t2762 = d_z * t30; + const auto t2763 = t32 * t367; + const auto t2764 = + t1498 * t2763 + t1500 * t2763 + t1501 * t2763 + t2761 + t2762; + const auto t2765 = t32 * t371; + const auto t2766 = + t1523 * t2750 + t227 * t2764 + t227 * t2765 + t367 * t540 + t384; + const auto t2767 = t30 * t539; + const auto t2768 = + t1523 * t2752 + t222 * t2764 + t222 * t2767 + t372 * t535 + t546; + const auto t2769 = t1841 * t383 + t2764 + t2765 + t2767; + const auto t2770 = -t1 * t35 * t374 * t546 - t11 * t35 * t384 * t546 + - t35 * t384 * t542 * t7; + const auto t2771 = t225 * t2768 + t230 * t2766 + t2684 * t2769 + t2770; + const auto t2772 = -t155 * t2771; + const auto t2773 = t1876 * t384; + const auto t2774 = t397 * t547; + const auto t2775 = t1619 * t24 * t2774 - t1911 * t500 - t2766 * t505 + - t2772 * t317 - t2773 * t7; + const auto t2776 = -t1675 * t2758 + t1688 * t2759 + t1739 * t2751 + + t227 * t2760 + t2775 + t498 * t596; + const auto t2777 = t30 * t43; + const auto t2778 = t32 * t40; + const auto t2779 = + t1841 * t416 + t1841 * t417 + t1841 * t418 + t2746 + t2777 + t2778; + const auto t2780 = t30 * t577; + const auto t2781 = t32 * t423; + const auto t2782 = + t222 * t2779 + t222 * t2780 + t222 * t2781 + t272 * t2752 + t578; + const auto t2783 = + t227 * t2779 + t227 * t2780 + t227 * t2781 + t272 * t2750 + t424; + const auto t2784 = t1841 * t415 + t2779 + t2780 + t2781; + const auto t2785 = t424 * t56; + const auto t2786 = t1724 * t578 + t1855 * t424 + t2785 * t578; + const auto t2787 = -t2782 * t280 - t2783 * t282 - t2784 * t425 + t2786; + const auto t2788 = t277 * t2787; + const auto t2789 = t430 * t583; + const auto t2790 = t222 * t578; + const auto t2791 = t500 * t546; + const auto t2792 = t1619 * t2774; + const auto t2793 = -t1736 * t2791 - t1876 * t374 + t21 * t2792 + - t2768 * t505 - t2772 * t331; + const auto t2794 = t1733 * t2782 - t1734 * t2788 + t1861 * t2789 + + t2790 * t504 + t2793 + t429 * t584; + const auto t2795 = t1626 * t2789; + const auto t2796 = t424 * t584; + const auto t2797 = -t1718 * t2788 + t1733 * t2783 + t227 * t2796 + t2775 + + t2795 * t52 + t504 * t582; + const auto t2798 = t1609 * t93; + const auto t2799 = t1739 * t2753 - t1740 * t2758 + t1899 * t498 + + t2759 * t2798 + t2793 + t451 * t600; + const auto t2800 = t3 * t54; + const auto t2801 = t2784 * t59; + const auto t2802 = -t1790 * t2791 + t18 * t2792 - t2717 * t2769 + - t2772 * t287 - t2773 * t3; + const auto t2803 = -t1744 * t2788 + t1916 * t504 + t207 * t2796 + + t2795 * t48 + t2800 * t2801 + t2802; + const auto t2804 = t3 * t95; + const auto t2805 = t2754 * t59; + const auto t2806 = -t1748 * t2758 + t1907 * t498 + t2038 * t2759 + + t207 * t2760 + t2802 + t2804 * t2805; + const auto t2807 = -t2787 * t431; + const auto t2808 = t1784 * t2789; + const auto t2809 = t1823 * t424; + const auto t2810 = t384 * t547; + const auto t2811 = t1778 * t547; + const auto t2812 = t1758 * t2771 - t1791 * t2810 - t1917 * t397 + - t2734 * t2769 + t2811 * t408; + const auto t2813 = t3 * t404; + const auto t2814 = -t2757 * t453; + const auto t2815 = t1767 * t2759; + const auto t2816 = t1829 * t447; + const auto t2817 = t1297 * t2811 - t1757 * t2766 + t1770 * t2771 + - t1773 * t2810 - t1912 * t397; + const auto t2818 = t1247 * t1657; + const auto t2819 = -t1069 * t1818 - t157 * t1810 + t166 * t1809 + + t1802 * t22 - t1803 * t66 + t1804 * t1815 + t1810 - t1813 * t19 + + t1813 * t2818; + const auto t2820 = t453 * t646; + const auto t2821 = t434 * t660; + const auto t2822 = -t1771 * t656 + t230 * t2821; + const auto t2823 = t1764 * t639 - t2820 * t306 + t2822; + const auto t2824 = t431 * t675; + const auto t2825 = -t282 * t2824 + t2822 + t668 * t933; + const auto t2826 = t234 * t2821 + t358 * t659; + const auto t2827 = -t2824 * t284 + t2826 - t361 * t674; + const auto t2828 = -t2820 * t308 + t2826 - t404 * t645; + const auto t2829 = + t2823 * t362 - t2825 * t411 + t2827 * t405 - t2828 * t412; + const auto t2830 = t207 * t645; + const auto t2831 = t30 * t642; + const auto t2832 = 3 * t376; + const auto t2833 = t3 * t81; + const auto t2834 = + t1669 * t1958 - t1669 + t2049 + t2832 * t297 + t2832 * t299 + t2833; + const auto t2835 = t2831 + t2832 * t87 + t2834 + t3 * t446; + const auto t2836 = t1657 * t376; + const auto t2837 = + t222 * t2831 + t222 * t2834 + t2836 * t310 + t389 * t446 + t639; + const auto t2838 = t1667 * t60; + const auto t2839 = + -t207 * t2834 - t2838 * t86 - t367 * t643 - t446 * t56 + t447; + const auto t2840 = -t1 * t451 * t639 - t13 * t35 * t447 * t639 + + t2830 * t447 + t2835 * t307 + t2837 * t304 - t2839 * t308; + const auto t2841 = t2840 * t453; + const auto t2842 = t454 * t7; + const auto t2843 = t1767 * t452; + const auto t2844 = t306 * t646; + const auto t2845 = t2820 * t447; + const auto t2846 = t207 * t659; + const auto t2847 = t30 * t655; + const auto t2848 = t35 * t376; + const auto t2849 = + t1500 * t2848 + t1501 * t2848 + t1690 * t1958 - t1690 + t2095; + const auto t2850 = t212 * t2832 + t2847 + t2849 + t3 * t371; + const auto t2851 = + -t1667 * t1961 - t207 * t2849 - t367 * t657 - t371 * t56 + t384; + const auto t2852 = + t1523 * t2836 + t207 * t372 + t222 * t2847 + t222 * t2849 + t656; + const auto t2853 = -t225 * t2852 + t234 * t2851 - t2846 * t384 + - t2850 * t380 + t375 * t656 + t385 * t656; + const auto t2854 = -t2853 * t434; + const auto t2855 = t227 * t358; + const auto t2856 = t1776 * t435; + const auto t2857 = t2856 * t660; + const auto t2858 = -t1156 * t2726 + t1297 * t2857 + t230 * t2854 + - t2821 * t478 - t2850 * t2855; + const auto t2859 = t207 * t674; + const auto t2860 = t30 * t671; + const auto t2861 = t3 * t40; + const auto t2862 = + t1714 * t1958 - t1714 + t2049 + t262 * t2832 + t264 * t2832 + t2861; + const auto t2863 = t2832 * t46 + t2860 + t2862 + t3 * t423; + const auto t2864 = + t222 * t2860 + t222 * t2862 + t272 * t2836 + t389 * t423 + t668; + const auto t2865 = + -t207 * t2862 - t2838 * t45 - t367 * t672 - t423 * t56 + t424; + const auto t2866 = -t1 * t429 * t668 - t13 * t35 * t424 * t668 + + t280 * t2864 + t283 * t2863 - t284 * t2865 + t2859 * t424; + const auto t2867 = t2866 * t431; + const auto t2868 = t432 * t7; + const auto t2869 = t1784 * t430; + const auto t2870 = t1783 * t2869; + const auto t2871 = t2824 * t424; + const auto t2872 = t1788 * t675; + const auto t2873 = t207 * t384; + const auto t2874 = t234 * t2854 - t2821 * t2873 + t2851 * t358 + + t2857 * t408 + t436 * t659; + const auto t2875 = t2843 * t308; + const auto t2876 = -t680; + const auto t2877 = -t683; + const auto t2878 = -t2840 * t293; + const auto t2879 = t452 * t647; + const auto t2880 = t447 * t648; + const auto t2881 = t155 * t2853; + const auto t2882 = t1618 * t661; + const auto t2883 = t1156 * t500 - t155 * t35 * t384 * t661 * t7 + + t2029 * t2850 + t24 * t2881 + t2882 * t481; + const auto t2884 = -t277 * t2866; + const auto t2885 = t430 * t676; + const auto t2886 = t1626 * t2885; + const auto t2887 = t424 * t677; + const auto t2888 = t2882 * t397; + const auto t2889 = t1162 * t500 - t155 * t35 * t374 * t661 + t21 * t2881 + + t26 * t2852 + t2888 * t331; + const auto t2890 = t1 * t277 * t430 * t668 - t1861 * t2885 + t2864 * t54 + - t2884 * t50 - t2889 - t429 * t677; + const auto t2891 = t1 * t293 * t452 * t639 - t2798 * t2879 + t2837 * t95 + - t2878 * t93 - t2889 - t451 * t648; + const auto t2892 = t1152 * t500 - t155 * t3 * t35 * t384 * t661 + + t18 * t2881 - t26 * t2851 + t287 * t2888; + const auto t2893 = t154 + * (-t1040 + * (-t101 * t2876 + t2877 * t55 - t462 * t713 + t496 * t711) + + t12 * t15 * t2829 * t35 - t1816 + - t222 + * (t2823 * t439 - t2825 * t458 + t2827 * t457 - t2828 * t459 + + t362 + * (t227 * t2845 + t2722 * t2835 - t2841 * t306 + + t2842 * t639 - t2843 * t2844 + t2858) + + t405 + * (t207 * t2871 - t284 * t2867 - t2865 * t361 + - t2869 * t2872 + t2874 - t432 * t674) + - t411 + * (t227 * t2871 + t2736 * t2863 - t282 * t2867 + t2858 + + t2868 * t668 - t2870 * t675) + - t412 + * (t207 * t2845 - t2839 * t404 - t2841 * t308 + t2874 + - t2875 * t646 - t454 * t645)) + - t2829 * t35 + + t3 * t35 + * (-t102 * t2891 + t2876 * t503 - t2877 * t509 + t2890 * t96 + + t462 + * (-t1688 * t2879 - t227 * t2880 + + t2835 * t35 * t7 * t95 - t2878 * t89 - t2883 + + t293 * t452 * t639 * t7) + - t496 + * (-t227 * t2887 + t277 * t430 * t668 * t7 + + t2863 * t35 * t54 * t7 - t2883 - t2884 * t52 + - t2886 * t52) + + t507 * t707 - t510 * t709) + + t35 * t7 + * (-t101 * t2890 - t2876 * t523 + t2877 * t522 + t2891 * t55 + - t462 + * (-t2038 * t2879 - t207 * t2880 - t2839 * t95 + - t2878 * t91 - t2892 + t293 * t452 * t702) + + t496 + * (-t207 * t2887 + t277 * t430 * t697 - t2865 * t54 + - t2884 * t48 - t2886 * t48 - t2892) + - t507 * t713 + t510 * t711) + - t493 + * (-t102 * t2877 + t2876 * t96 + t462 * t707 - t496 * t709)); + const auto t2894 = t453 * t754; + const auto t2895 = t434 * t735; + const auto t2896 = -t1771 * t723 + t230 * t2895; + const auto t2897 = t1764 * t750 - t2894 * t306 + t2896; + const auto t2898 = t431 * t772; + const auto t2899 = t234 * t2895 - t2733 * t723; + const auto t2900 = -t284 * t2898 + t2899 + t770 * t890; + const auto t2901 = -t282 * t2898 + t2896 + t770 * t933; + const auto t2902 = t2813 * t750 - t2894 * t308 + t2899; + const auto t2903 = + t2897 * t362 + t2900 * t405 - t2901 * t411 - t2902 * t412; + const auto t2904 = t1000 * t1667; + const auto t2905 = t1927 * t427; + const auto t2906 = t2904 * t81 + t2905 * t79 + t2905 * t85 + t295 + + 2 * t297 + t299 - t367 * t81 + t916; + const auto t2907 = t2906 + t367 * t752 + t449 + 3 * t450 + t87; + const auto t2908 = -t2664; + const auto t2909 = t1 * t752; + const auto t2910 = 3 * e0_y; + const auto t2911 = t0 + t2910; + const auto t2912 = t1 * t2906 + t158 * t446 + t2904 * t86 + t2908 + + t2909 * t367 + t2911 - t441 - t442 - t443 - t444 + t749 + 2 * t92; + const auto t2913 = t186 * t447; + const auto t2914 = t2755 * t750 + t2913 * t750; + const auto t2915 = + -t2907 * t641 - t2907 * t751 - t2912 * t304 + t2914 - t451 * t753; + const auto t2916 = -t2915 * t453; + const auto t2917 = t2843 * t306; + const auto t2918 = t2894 * t447; + const auto t2919 = -t2679; + const auto t2920 = d_y * t30; + const auto t2921 = t1498 * t364 + t1501 * t364 + t2050 * t2920 - t2920; + const auto t2922 = t1 * t722; + const auto t2923 = t10 * t2904 + t158 * t371 + + t222 * (t1507 + t2921 + t4 + t8) + 2 * t223 + t2919 + t2922 * t367 + - t368 - t369 - t370 - t383 + t721; + const auto t2924 = + t1500 + t1505 + t1509 + t2921 + t30 * t722 + t372 + 3 * t373; + const auto t2925 = -t11 * t35 * t384 * t723 - t13 * t35 * t384 * t723; + const auto t2926 = + t225 * t2923 + t2684 * t2924 + t2924 * t380 + t2925 + t463 * t728; + const auto t2927 = t2926 * t434; + const auto t2928 = t2856 * t735; + const auto t2929 = t1297 * t2928 + t230 * t2927 - t2726 * t734 + - t2855 * t2924 - t2895 * t478; + const auto t2930 = t260 + 2 * t262 + t264 + t2904 * t40 + t2905 * t38 + + t2905 * t44 - t367 * t40 + t916; + const auto t2931 = t2930 + t367 * t765 + t426 + 3 * t428 + t46; + const auto t2932 = -t2704; + const auto t2933 = t1 * t2930 + t158 * t423 + t2904 * t45 + t2911 + + t2932 + t367 * t766 - t415 - t416 - t417 - t418 + 2 * t49 + t764; + const auto t2934 = t186 * t424; + const auto t2935 = t2785 * t770 + t2934 * t770; + const auto t2936 = + -t280 * t2933 - t2931 * t670 - t2931 * t771 + t2935 - t429 * t768; + const auto t2937 = -t2936 * t431; + const auto t2938 = t3 * t432; + const auto t2939 = t1788 * t2869; + const auto t2940 = t2898 * t424; + const auto t2941 = t207 * t723; + const auto t2942 = -t1825 * t2924 + t234 * t2927 - t2726 * t2941 + - t2873 * t2895 + t2928 * t408; + const auto t2943 = t3 * t454; + const auto t2944 = -t776; + const auto t2945 = -t781; + const auto t2946 = t2915 * t293; + const auto t2947 = t452 * t755; + const auto t2948 = t447 * t756; + const auto t2949 = -t155 * t2926; + const auto t2950 = t1618 * t736; + const auto t2951 = -t155 * t35 * t384 * t7 * t736 + t2029 * t2924 + + t24 * t2949 + t2950 * t481 + t500 * t734; + const auto t2952 = -t1688 * t2947 - t227 * t2948 + t2907 * t7 * t95 + + t293 * t452 * t7 * t750 - t2946 * t89 - t2951; + const auto t2953 = t277 * t2936; + const auto t2954 = t430 * t773; + const auto t2955 = t1626 * t2954; + const auto t2956 = t424 * t774; + const auto t2957 = -t227 * t2956 + t277 * t430 * t7 * t770 + + t2931 * t54 * t7 - t2951 - t2953 * t52 - t2955 * t52; + const auto t2958 = t2950 * t397; + const auto t2959 = -t155 * t35 * t374 * t736 + t21 * t2949 + t26 * t2923 + + t2958 * t331 + t500 * t730; + const auto t2960 = -t1861 * t2954 + t277 * t430 * t769 + t2933 * t54 + - t2953 * t50 - t2959 - t429 * t774; + const auto t2961 = -t2798 * t2947 + t2912 * t95 + t293 * t452 * t780 + - t2946 * t93 - t2959 - t451 * t756; + const auto t2962 = t207 * t26; + const auto t2963 = -t155 * t3 * t35 * t384 * t736 + t18 * t2949 + + t287 * t2958 + t2924 * t2962 + t2941 * t500; + const auto t2964 = -t207 * t2956 + t277 * t3 * t430 * t770 + + t2931 * t3 * t54 - t2953 * t48 - t2955 * t48 - t2963; + const auto t2965 = -t2038 * t2947 - t207 * t2948 + t2907 * t3 * t95 + + t293 * t3 * t452 * t750 - t2946 * t91 - t2963; + const auto t2966 = t431 * t852; + const auto t2967 = t434 * t814; + const auto t2968 = t234 * t2967 - t2733 * t813; + const auto t2969 = -t284 * t2966 + t2968 + t849 * t890; + const auto t2970 = t453 * t840; + const auto t2971 = t2813 * t839 + t2968 - t2970 * t308; + const auto t2972 = t230 * t2967 + t358 * t809; + const auto t2973 = -t2970 * t306 + t2972 - t404 * t837; + const auto t2974 = -t282 * t2966 + t2972 - t361 * t851; + const auto t2975 = + t2969 * t405 - t2971 * t412 + t2973 * t362 - t2974 * t411; + const auto t2976 = t30 * t850; + const auto t2977 = 3 * t2745; + const auto t2978 = t40 * t7; + const auto t2979 = -t468; + const auto t2980 = + t1667 * t846 + t1667 * t847 + t2199 * t2777 - t2777 + t2978 + t2979; + const auto t2981 = t2976 + t2977 * t46 + t2980 + t423 * t7; + const auto t2982 = t1657 * t2745; + const auto t2983 = + t222 * t2976 + t222 * t2980 + t272 * t2982 + t423 * t909 + t849; + const auto t2984 = t7 * t850; + const auto t2985 = -t1476 * t1667 * t45 - t186 * t423 - t227 * t2980 + - t2984 * t367 + t424; + const auto t2986 = t1724 * t849 - t2321 * t424 + t2785 * t849 + - t280 * t2983 + t282 * t2985 - t2981 * t425; + const auto t2987 = -t2986 * t431; + const auto t2988 = t2966 * t424; + const auto t2989 = t30 * t808; + const auto t2990 = d_y * t7; + const auto t2991 = t227 * t30; + const auto t2992 = + t1498 * t2991 + t1500 * t2991 + t2199 * t2762 - t2762 + t2990; + const auto t2993 = t1667 * t228 + t2989 + t2992 + t371 * t7; + const auto t2994 = -3 * t10 * t13 * t30 * t59 - t13 * t35 * t371 + - t2992 * t35 * t7 - t30 * t35 * t7 * t808 + t384; + const auto t2995 = + t1523 * t2982 + t222 * t2989 + t222 * t2992 + t227 * t372 + t813; + const auto t2996 = -t1 * t35 * t374 * t813 - t11 * t35 * t384 * t813 + + t225 * t2995 - t230 * t2994 + t2684 * t2993 + t478 * t809; + const auto t2997 = t2996 * t434; + const auto t2998 = t207 * t813; + const auto t2999 = t2856 * t814; + const auto t3000 = -t1825 * t2993 + t234 * t2997 - t2726 * t2998 + - t2873 * t2967 + t2999 * t408; + const auto t3001 = t30 * t834; + const auto t3002 = t7 * t81; + const auto t3003 = + t1667 * t831 + t1667 * t832 + t2199 * t2742 - t2742 + t2979 + t3002; + const auto t3004 = t2977 * t87 + t3001 + t3003 + t446 * t7; + const auto t3005 = + t222 * t3001 + t222 * t3003 + t2982 * t310 + t446 * t909 + t839; + const auto t3006 = -3 * t13 * t30 * t59 * t86 - t13 * t35 * t446 + - t30 * t35 * t7 * t834 - t3003 * t35 * t7 + t447; + const auto t3007 = t1682 * t839 - t2316 * t447 + t2755 * t839 + - t3004 * t448 - t3005 * t304 + t3006 * t306; + const auto t3008 = -t3007 * t453; + const auto t3009 = t2970 * t447; + const auto t3010 = t1297 * t2999 + t230 * t2997 - t2967 * t478 + + t2994 * t358 + t436 * t809; + const auto t3011 = -t857; + const auto t3012 = -t861; + const auto t3013 = t293 * t3007; + const auto t3014 = t452 * t841; + const auto t3015 = t447 * t842; + const auto t3016 = -t155 * t2996; + const auto t3017 = t1618 * t815; + const auto t3018 = -t155 * t35 * t384 * t7 * t815 + t24 * t3016 + - t26 * t2994 + t3017 * t481 + t500 * t811; + const auto t3019 = t277 * t2986; + const auto t3020 = t430 * t853; + const auto t3021 = t1626 * t3020; + const auto t3022 = t424 * t854; + const auto t3023 = t3017 * t397; + const auto t3024 = -t155 * t35 * t374 * t815 + t21 * t3016 + t26 * t2995 + + t3023 * t331 + t500 * t827; + const auto t3025 = t1 * t277 * t430 * t849 - t1861 * t3020 + t2983 * t54 + - t3019 * t50 - t3024 - t429 * t854; + const auto t3026 = t1 * t293 * t452 * t839 - t2798 * t3014 + t3005 * t95 + - t3013 * t93 - t3024 - t451 * t842; + const auto t3027 = -t155 * t3 * t35 * t384 * t815 + t18 * t3016 + + t287 * t3023 + t2962 * t2993 + t2998 * t500; + const auto t3028 = t154 + * (-t1040 + * (-t101 * t3011 + t3012 * t55 - t462 * t873 + t496 * t871) + + t12 * t15 * t2975 * t35 + - t222 + * (t2969 * t457 - t2971 * t459 + t2973 * t439 - t2974 * t458 + + t362 + * (t227 * t3009 - t2917 * t840 - t3006 * t404 + - t3008 * t306 + t3010 - t454 * t837) + + t405 + * (t1824 * t2981 + t207 * t2988 - t284 * t2987 + + t2938 * t849 - t2939 * t852 + t3000) + - t411 + * (t227 * t2988 - t282 * t2987 - t2870 * t852 + - t2985 * t361 + t3010 - t432 * t851) + - t412 + * (t1830 * t3004 + t207 * t3009 - t2875 * t840 + + t2943 * t839 + t3000 - t3008 * t308)) + - t2819 - t2975 * t35 + + t3 * t35 + * (-t102 * t3026 + t3011 * t503 - t3012 * t509 + t3025 * t96 + + t462 + * (-t1688 * t3014 - t227 * t3015 + t293 * t452 * t838 + - t3006 * t95 - t3013 * t89 - t3018) + - t496 + * (-t227 * t3022 + t277 * t430 * t858 - t2985 * t54 + - t3018 - t3019 * t52 - t3021 * t52) + + t507 * t872 - t510 * t874) + + t35 * t7 + * (-t101 * t3025 - t3011 * t523 + t3012 * t522 + t3026 * t55 + - t462 + * (-t2038 * t3014 - t207 * t3015 + + t293 * t3 * t452 * t839 + t3 * t3004 * t35 * t95 + - t3013 * t91 - t3027) + + t496 + * (-t207 * t3022 + t277 * t3 * t430 * t849 + + t2981 * t3 * t35 * t54 - t3019 * t48 + - t3021 * t48 - t3027) + - t507 * t873 + t510 * t871) + - t493 + * (-t102 * t3012 + t3011 * t96 + t462 * t872 - t496 * t874)); + const auto t3029 = + -2 * t1 * t3 * t465 * t48 - 2 * t1 * t465 * t52 * t7 + t19 * t429; + const auto t3030 = t3029 + t424 * t474 + t424 * t475; + const auto t3031 = -2 * t12 * t465 * t50 + t15 * t50; + const auto t3032 = -t3030 - t3031; + const auto t3033 = t52 * t892; + const auto t3034 = t3 * t3033; + const auto t3035 = 3 * t430; + const auto t3036 = t2341 * t3035; + const auto t3037 = t424 * t894; + const auto t3038 = t430 * t892; + const auto t3039 = t227 * t3037 + t2344 * t3036 - t3038 * t66 + t484; + const auto t3040 = t153 * t54; + const auto t3041 = t3036 * t893; + const auto t3042 = t207 * t3037 - t3038 * t57 + t3041 * t48 + t516; + const auto t3043 = t291 + t48 * t894; + const auto t3044 = t893 * t900; + const auto t3045 = -t123 * t3044 + t66; + const auto t3046 = t125 * t3045; + const auto t3047 = t2591 * t3043 * t54 + t2636 * t3046; + const auto t3048 = t2563 - t3038 * t64; + const auto t3049 = + t3 * t3032 * t50 * t892 - t3041 * t50 - t3048 - t429 * t894; + const auto t3050 = -t2934 + t424 * t58; + const auto t3051 = t1623 * t377 - t1724 + t283 * t379; + const auto t3052 = t110 + t2703 + t280 * t365 + t3051; + const auto t3053 = t3050 + t3052; + const auto t3054 = t424 * t881; + const auto t3055 = t2396 * t376; + const auto t3056 = t430 * t880; + const auto t3057 = t3055 + t3056 * t391; + const auto t3058 = + t227 * t3054 - 3 * t2356 * t282 * t430 * t879 + t3057; + const auto t3059 = t361 * t496; + const auto t3060 = -t2362 * t510 * t54 + t503 * t896; + const auto t3061 = t425 * t880; + const auto t3062 = -t379 * t60; + const auto t3063 = + t1788 * t2357 * t430 - t207 * t3054 + t3056 * t58 + t3062; + const auto t3064 = t510 * t897 + t523 * t896; + const auto t3065 = t146 * t3043 * t54 + t153 * t3046; + const auto t3066 = t166 * (t101 * t896 + t496 * t897) - t174 * t3065 + + t3065 + t54 * t64 * (t2362 * t496 - t895 * t96); + const auto t3067 = -t146 * t903 + t153 * t902; + const auto t3068 = t125 * t3067; + const auto t3069 = t174 * t3068; + const auto t3070 = t496 * t54; + const auto t3071 = t3070 * t919 + t918 * t96; + const auto t3072 = t166 * (t101 * t918 + t2402 * t496); + const auto t3073 = t2384 * t3; + const auto t3074 = -t1095 * t1560 + 2 * t12 * t3 * t465 * t48 + + 2 * t12 * t465 * t52 * t7 - t164 * t429 - t19 * t2785 - t2339 + - t2574 * t424 - t3073; + const auto t3075 = t3036 * t899; + const auto t3076 = t424 * t905; + const auto t3077 = -t166 * t3038 + t2578; + const auto t3078 = t227 * t3076 - t3033 * t3074 + t3075 * t52 + t3077; + const auto t3079 = + t207 * t3076 + t3048 - t3074 * t48 * t892 + t3075 * t48; + const auto t3080 = t514 * t59; + const auto t3081 = t3080 * t376; + const auto t3082 = t2745 * t3080; + const auto t3083 = t1 * t424; + const auto t3084 = + t1095 * t281 - t1476 * t3083 - t3083 * t60 + t429 * t911; + const auto t3085 = -t2581; + const auto t3086 = t2406 * t280; + const auto t3087 = -t2395 * t429 + t3035 * t3086 + t3056 * t911 + t3085; + const auto t3088 = + t3087 + t914 * (t282 * t3082 + t283 + t284 * t3081 + t3084 + t425); + const auto t3089 = t2402 * t510 + t523 * t918; + const auto t3090 = t503 * t918 + t510 * t54 * t919; + const auto t3091 = -t2785 + t424 * t923; + const auto t3092 = t3052 + t3091; + const auto t3093 = -t1476 * t379; + const auto t3094 = t424 * t926; + const auto t3095 = + t1783 * t2430 * t430 - t227 * t3094 + t3056 * t923 + t3093; + const auto t3096 = t2433 * t3092 + t3095; + const auto t3097 = + t207 * t3094 - 3 * t2356 * t284 * t430 * t925 + t3057; + const auto t3098 = t284 * t3092 * t35 * t7 * t880 - t3097; + const auto t3099 = t503 * t938 + t523 * t939; + const auto t3100 = t166 - t50 * t936; + const auto t3101 = t3100 * t54; + const auto t3102 = t510 * t938; + const auto t3103 = -t3036 * t50 * t935 - t3077 - t429 * t936 + + t50 * t7 * t892 * (-t1242 * t424 - t15 * t2785 - t3029 - t3031); + const auto t3104 = t3101 * t503 + t510 * t939; + const auto t3105 = -t174 * t2425 + t2425; + const auto t3106 = -t1 * t15 * t54 * t7 * (t101 * t3100 - t496 * t937) + + t3105 + t64 * (t2387 * t3100 + t496 * t939); + const auto t3107 = t15 * t93; + const auto t3108 = t2557 * t93; + const auto t3109 = t1023 * t91; + const auto t3110 = t469 * t89; + const auto t3111 = t19 * t451 + t3107 - t3108 - t3109 - t3110; + const auto t3112 = 3 * t452; + const auto t3113 = t2450 * t3112; + const auto t3114 = t3113 * t93; + const auto t3115 = -t1 * t15 * t3 * t452 * t948 + t2563; + const auto t3116 = + t3 * t93 * t948 * (-t3111 - t447 * t474 - t447 * t475) + - t3114 * t942 - t3115 - t451 * t949; + const auto t3117 = -t2913 + t447 * t58; + const auto t3118 = t1606 * t377 - t1682 + t307 * t379; + const auto t3119 = t141 + t2663 + t304 * t365 + t3118; + const auto t3120 = t3117 + t3119; + const auto t3121 = t2471 * t447; + const auto t3122 = t452 * t957; + const auto t3123 = t3055 + t3122 * t391; + const auto t3124 = + t227 * t3121 - 3 * t2472 * t306 * t452 * t956 + t3123; + const auto t3125 = -t3 * t306 * t3120 * t35 * t957 + t3124; + const auto t3126 = t95 * t951; + const auto t3127 = t3126 * t509 - t507 * t95 * t962; + const auto t3128 = t2472 * t3112; + const auto t3129 = -t207 * t3121 + t2474 * t3128 + t3062 + t3122 * t58; + const auto t3130 = t207 * t3120 * t958 + t3129; + const auto t3131 = t3126 * t522 + t507 * t961; + const auto t3132 = t391 - t956 * t993; + const auto t3133 = t3132 * t439 + t459 * t960; + const auto t3134 = t1 * t404; + const auto t3135 = t3132 * t362 + t412 * t960; + const auto t3136 = -t12 * t15 * t3135 * t404 + + t166 * (t2467 * t951 + t462 * t961) + t3135 * t404 + + t64 * t95 * (-t102 * t951 + t462 * t962); + const auto t3137 = t362 * t972 - t412 * t973; + const auto t3138 = t3137 * t404; + const auto t3139 = t462 * t95; + const auto t3140 = t64 * (t102 * t1230 + t2515 * t3139); + const auto t3141 = t166 * t2522; + const auto t3142 = t3122 * t909; + const auto t3143 = t1 * t447; + const auto t3144 = + t1095 * t305 - t1476 * t3143 - t3143 * t60 + t451 * t911; + const auto t3145 = t306 * t3082 + t307 + t308 * t3081 + t3144 + t448; + const auto t3146 = t3128 * t967; + const auto t3147 = t447 * t968; + const auto t3148 = t227 * t3147 - t306 * t3146; + const auto t3149 = t3122 * t389; + const auto t3150 = t207 * t3147 - t308 * t3146; + const auto t3151 = t439 * t972 - t459 * t973; + const auto t3152 = t2492 * t3; + const auto t3153 = t1070 * t91; + const auto t3154 = t1140 * t89; + const auto t3155 = -t1095 * t1473 - t164 * t451 - t19 * t2755 - t2444 + - t2574 * t447 - t3152 + t3153 + t3154; + const auto t3156 = t3113 * t975; + const auto t3157 = t2498 * t447; + const auto t3158 = t304 * t957; + const auto t3159 = t304 * t3146 + t3085 + t3122 * t911 - t451 * t968; + const auto t3160 = t3145 * t3158 + t3159; + const auto t3161 = t1230 * t522 + t507 * t978; + const auto t3162 = -t1 * t15 * t452 * t7 * t948 + t2578; + const auto t3163 = t1230 * t509 + t2515 * t507 * t95; + const auto t3164 = t391 - t958 * t992; + const auto t3165 = t3164 * t412 + t362 * t995; + const auto t3166 = t166 * t95 * (-t462 * t987 + t55 * t997); + const auto t3167 = t3165 * t404; + const auto t3168 = t174 * t3167; + const auto t3169 = t2544 * t64; + const auto t3170 = -t2755 + t447 * t923; + const auto t3171 = t3119 + t3170; + const auto t3172 = t2528 * t447; + const auto t3173 = + t207 * t3172 - 3 * t2472 * t308 * t452 * t992 + t3123; + const auto t3174 = -t308 * t3171 * t35 * t7 * t957 + t3173; + const auto t3175 = + -t227 * t3172 + t306 * t3128 * t992 + t3093 + t3122 * t923; + const auto t3176 = t227 * t3171 * t993 + t3175; + const auto t3177 = t3164 * t459 + t439 * t995; + const auto t3178 = t95 * t997; + const auto t3179 = t2538 * t507; + const auto t3180 = t948 * t982; + const auto t3181 = -t3114 * t982 - t3162 - t3180 * t451 + + t7 * t93 * t948 * (-t1242 * t447 - t15 * t2755 - t3111); + const auto t3182 = t3178 * t509 + t507 * t998; + const auto t3183 = t15 * t24; + const auto t3184 = 2 * t1247; + const auto t3185 = t24 * t3184; + const auto t3186 = -t3183 + t3185 + t556; + const auto t3187 = t3186 + t559; + const auto t3188 = -t3187; + const auto t3189 = t1248 + t2562; + const auto t3190 = t3189 - t549; + const auto t3191 = t3188 * t562 + t3190 - t544 + t551; + const auto t3192 = t3188 * t513 + t567; + const auto t3193 = -t3191; + const auto t3194 = -t3187 * t607 - t619; + const auto t3195 = -t3187 * t513 - t632; + const auto t3196 = t1131 + t3186; + const auto t3197 = -t3196; + const auto t3198 = t1307 + t1425; + const auto t3199 = -t1127 + t3198; + const auto t3200 = t1034 * t3197 - t1126 + t1129 + t3199; + const auto t3201 = t1048 * t3197 + t1137; + const auto t3202 = -t3200; + const auto t3203 = -t1068 * t3196 - t1143; + const auto t3204 = -t1048 * t3196 - t1146; + const auto t3205 = t1326 * t227; + const auto t3206 = t1248 * t18; + const auto t3207 = t191 * t3184; + const auto t3208 = t174 * t227; + const auto t3209 = t72 + * (-t1242 * t542 + 2 * t1323 * t24 * t35 * t7 - t1411 + - t22 * t546 * t56 - t2571 - t3206 - t3207 - t3208 * t546); + const auto t3210 = + t1249 * t547 + t1324 * t35 - t1332 * t317 - t24 * t3209 + t3205; + const auto t3211 = -t3210; + const auto t3212 = + t1332 * t287 + t18 * t3209 - t194 * t3 * t35 * t546 * t72 + t3190; + const auto t3213 = + t1 * t194 * t35 * t546 * t72 - t1332 * t331 - t21 * t3209 - t3199; + const auto t3214 = -t3212; + const auto t3215 = + t1978 * (t1560 * t1848 + t1851 * t48 + t1854 * t52 + t1857); + const auto t3216 = -2 * t3 * t7; + const auto t3217 = t2214 + t3216; + const auto t3218 = t2217 + t576; + const auto t3219 = t3 * t3218; + const auto t3220 = t2220 + t3219; + const auto t3221 = t1560 * t578 + t2618 * t578 + t52 * t582; + const auto t3222 = t119 + t3218; + const auto t3223 = t1575 * t3222; + const auto t3224 = t1543 * t3221; + const auto t3225 = t1574 * t3224; + const auto t3226 = + t155 * (-t1515 * t1871 - t18 * t1868 - t1870 * t24 - t1872); + const auto t3227 = -t612; + const auto t3228 = t3 * t3227; + const auto t3229 = -t3228; + const auto t3230 = t1540 * t614; + const auto t3231 = t1968 * t616; + const auto t3232 = -3 * t1064 * t1536 * t21 * t59 * t616 + t19 * t3230 + + t19 * t3231 + t2243 * (t2242 + t3229) + t3226 * t331; + const auto t3233 = t1 * t125 * t465 * (-t2210 - t3217 - t3220) + + 3 * t112 * t1571 * t1572 * t3221 - t112 * t3215 - t19 * t3223 + - t19 * t3225 - t3232; + const auto t3234 = + t1937 * (t1473 * t1887 + t1890 * t91 + t1891 * t89 + t1894); + const auto t3235 = t2261 + t3216; + const auto t3236 = t2264 + t592; + const auto t3237 = t3 * t3236; + const auto t3238 = t2267 + t3237; + const auto t3239 = t1473 * t598 + t2600 * t598 + t596 * t89; + const auto t3240 = t131 + t3236; + const auto t3241 = t1493 * t3240; + const auto t3242 = t1448 * t3239; + const auto t3243 = t1492 * t3242; + const auto t3244 = t1 * t145 * t465 * (-t2258 - t3235 - t3238) + + 3 * t143 * t1489 * t1490 * t3239 - t143 * t3234 - t19 * t3241 + - t19 * t3243 - t3232; + const auto t3245 = t22 * t3236; + const auto t3246 = t1604 + t2277 + t3245; + const auto t3247 = -t22 * t3237; + const auto t3248 = -t3235; + const auto t3249 = t22 * t3228; + const auto t3250 = t3227 * t7; + const auto t3251 = t2292 + t3250; + const auto t3252 = -3 * t1064 * t1536 * t24 * t59 * t616 + + t1595 * (t15 * t2240 * t7 - t2289 - t3249) + t2013 * t3251 + + t22 * t3231 + t317 * t3226; + const auto t3253 = t22 * t3218; + const auto t3254 = t1587 + t2296 + t3253; + const auto t3255 = -t22 * t3219; + const auto t3256 = -t3217; + const auto t3257 = -t178 * t3218 + t3222; + const auto t3258 = t178 * t3227; + const auto t3259 = -3 * t1064 * t1536 * t18 * t59 * t616 + + t1595 * (t15 * t2240 * t3 - t2310 - t3258) + t16 * t3230 + + t2014 * t616 + t287 * t3226; + const auto t3260 = -t178 * t3236 + t3240; + const auto t3261 = + t126 * t621 + t146 * t624 - t150 * t625 - t151 * t627; + const auto t3262 = -t3261; + const auto t3263 = t152 * t627; + const auto t3264 = t150 * t635; + const auto t3265 = t153 * t624; + const auto t3266 = t126 * t634; + const auto t3267 = t3263 + t3264 - t3265 - t3266; + const auto t3268 = -t3267; + const auto t3269 = t588 * t96; + const auto t3270 = t102 * t604; + const auto t3271 = t55 * t602; + const auto t3272 = t101 * t605; + const auto t3273 = t3269 - t3270 + t3271 - t3272; + const auto t3274 = t178 * t3262 + t3261 + t3268 * t66 - t3273 * t64; + const auto t3275 = t1448 * t3246; + const auto t3276 = -2 * t1 * t7; + const auto t3277 = t1 * t83; + const auto t3278 = t7 * t80; + const auto t3279 = 3 * t127; + const auto t3280 = + t166 * t3279 + t2063 * t3278 + t2213 * t3277 - t3277 - t3278; + const auto t3281 = t3276 + t3280; + const auto t3282 = -t3281; + const auto t3283 = t1 * t3236; + const auto t3284 = 3 * t2277; + const auto t3285 = -t1 * t3284; + const auto t3286 = -t22 * t3283 + t3285; + const auto t3287 = + t1937 * (t2600 * t2754 + t2751 * t89 + t2753 * t93 + t2756); + const auto t3288 = t2602 * t3242; + const auto t3289 = d_z * t1; + const auto t3290 = -t2990 - t3289; + const auto t3291 = t1498 * t166 + t2063 * t2990 + t2213 * t3289 + t3290; + const auto t3292 = t1 * t3227; + const auto t3293 = t10 * t2818; + const auto t3294 = t2101 + t22 * t3292 - t3293; + const auto t3295 = t18 * t207; + const auto t3296 = + t155 * (-t21 * t2768 - t24 * t2766 - t2769 * t3295 - t2770); + const auto t3297 = t2012 * t3251; + const auto t3298 = t1539 * t616; + const auto t3299 = t2610 * t3298; + const auto t3300 = -3 * t1536 * t24 * t2609 * t59 * t616 + + t1595 * (t15 * t3291 * t7 - t157 * t2607 - t3294) + t22 * t3299 + + t2609 * t3297 + t317 * t3296; + const auto t3301 = -t1660 * t2132; + const auto t3302 = t1 * t42; + const auto t3303 = t39 * t7; + const auto t3304 = 3 * t104; + const auto t3305 = + t166 * t3304 + t2063 * t3303 + t2213 * t3302 - t3302 - t3303; + const auto t3306 = t3276 + t3305; + const auto t3307 = -t3306; + const auto t3308 = -t174 * t3218 + t3222; + const auto t3309 = + t1978 * (t2618 * t2784 + t2782 * t50 + t2783 * t52 + t2786); + const auto t3310 = t2622 * t3222; + const auto t3311 = t10 * t2740; + const auto t3312 = t174 * t3227 - t3311 + t613; + const auto t3313 = t2012 * t616; + const auto t3314 = t1539 * t614; + const auto t3315 = t2609 * t3314; + const auto t3316 = -3 * t1536 * t21 * t2609 * t59 * t616 + + t1595 * (t1 * t15 * t3291 - t22 * t2627 - t3312) + t19 * t3315 + + t2628 * t3313 + t3296 * t331; + const auto t3317 = 3 * t112 * t1571 * t2619 * t3221 - t112 * t3309 + + t125 * t15 * (-t166 * t2615 + t19 * t3307 + t3301 + t3308) + - t19 * t3310 - t2625 * t3224 - t3316; + const auto t3318 = t1543 * t3254; + const auto t3319 = t1 * t3218; + const auto t3320 = 3 * t2296; + const auto t3321 = -t1 * t3320; + const auto t3322 = -t22 * t3319 + t3321; + const auto t3323 = t2620 * t3224; + const auto t3324 = -t1660 * t2149; + const auto t3325 = -t174 * t3236 + t3240; + const auto t3326 = t2604 * t3240; + const auto t3327 = 3 * t143 * t1489 * t2601 * t3239 - t143 * t3287 + + t145 * t15 * (-t166 * t2596 + t19 * t3282 + t3324 + t3325) + - t19 * t3326 - t2632 * t3242 - t3316; + const auto t3328 = 3 * t119; + const auto t3329 = t1 * t3328; + const auto t3330 = 3 * t23; + const auto t3331 = t1 * t3330 + t3291; + const auto t3332 = -t3292 + t3331; + const auto t3333 = t465 * t785; + const auto t3334 = -3 * t1536 * t18 * t2609 * t59 * t616 + t16 * t3299 + + t16 * t3315 + t287 * t3296 + t3333 * (-t2607 * t7 + t3332); + const auto t3335 = 3 * t131; + const auto t3336 = t1 * t3335; + const auto t3337 = t174 * t3273; + const auto t3338 = t3262 * t64; + const auto t3339 = t166 * t3268; + const auto t3340 = t1648 * t22; + const auto t3341 = t1650 * t19; + const auto t3342 = t1648 * t2740; + const auto t3343 = t1650 * t2818; + const auto t3344 = t1653 * t1815; + const auto t3345 = t3340 - t3341 - t3342 + t3343 + t3344; + const auto t3346 = 2 * t624; + const auto t3347 = 2 * t627; + const auto t3348 = -t51; + const auto t3349 = 2 * f0_z; + const auto t3350 = 2 * t32; + const auto t3351 = t3350 * t43; + const auto t3352 = t3351 * t35; + const auto t3353 = 3 * t33; + const auto t3354 = t3353 * t59; + const auto t3355 = t3354 * t38; + const auto t3356 = t3354 * t41; + const auto t3357 = t3354 * t44; + const auto t3358 = t531 + 2; + const auto t3359 = t1552 - t3352 - t3355 - t3356 - t3357 + t3358; + const auto t3360 = 2 * t569; + const auto t3361 = t1660 * t33; + const auto t3362 = 2 * e1_z; + const auto t3363 = -4 * e0_z + t3362; + const auto t3364 = t260 * t3350 + t262 * t3350 + t264 * t3350 + + t272 * t3361 + t3348 + t3349 + t3350 * t579 - t3359 * t7 + t3360 + + t3363; + const auto t3365 = t3350 * t35; + const auto t3366 = -t531 - 2; + const auto t3367 = t1558 + t272 * t3353 + t3352 + t3355 + t3356 + t3357 + + t3365 * t577 + t3366; + const auto t3368 = std::pow(t578, 2); + const auto t3369 = t1000 * t3368 + t3368 * t60 + std::pow(t582, 2); + const auto t3370 = + t1543 * (t1560 * t3367 + t1718 * t3364 + t2618 * t3367 + t3369); + const auto t3371 = t13 * t1480; + const auto t3372 = t1486 * t3359; + const auto t3373 = -t107 * t3371 - t1565 - t3218 * t483 - t3372; + const auto t3374 = std::pow(t3221, 2); + const auto t3375 = 2 * t3224; + const auto t3376 = t3222 * t3375; + const auto t3377 = std::pow(t546, 2); + const auto t3378 = std::pow(t542, 2); + const auto t3379 = t33 * t35; + const auto t3380 = t1498 * t3379 + t1500 * t3379 + t1501 * t3379; + const auto t3381 = 2 * d_z; + const auto t3382 = t32 * t3381; + const auto t3383 = t1512 + t3382; + const auto t3384 = t212 * t3353 + t3350 * t539 + t3380 + t3383; + const auto t3385 = t1521 + t3382; + const auto t3386 = t229 + t3381 + 2 * t545; + const auto t3387 = t1505 * t535 + t1507 * t535 + t1509 * t535 + + t1523 * t3361 + t227 * (t3380 + t3385) + t3365 * t540 + t3386; + const auto t3388 = t155 + * (t11 * t3377 * t35 + t12 * t3377 * t35 - t1515 * t3384 + - t24 * t3387 - t3295 * t3384 + t3378 * t35); + const auto t3389 = std::pow(t7, 3); + const auto t3390 = 3 * t3389; + const auto t3391 = t15 * t3390; + const auto t3392 = d_z * t3391 + t1498 * t157 + t1500 * t157; + const auto t3393 = t1506 + t1508 + 3 * t2291 + t3392 - 4 * t8; + const auto t3394 = -2 * t3250 + t3393; + const auto t3395 = std::pow(t616, 2); + const auto t3396 = t3298 * t614; + const auto t3397 = -3 * t1536 * t21 * t3395 * t59 + t1578 * t3396 + + t2243 * t3394 + t331 * t3388; + const auto t3398 = t1 * t125 * t3373 + 3 * t112 * t1571 * t3374 + - t112 * t3370 - t19 * t3376 - t3397; + const auto t3399 = -t88; + const auto t3400 = 2 * f1_z; + const auto t3401 = t3350 * t84; + const auto t3402 = t3401 * t35; + const auto t3403 = t3354 * t79; + const auto t3404 = t3354 * t82; + const auto t3405 = t3354 * t85; + const auto t3406 = t1461 + t3358 - t3402 - t3403 - t3404 - t3405; + const auto t3407 = 2 * t597; + const auto t3408 = t295 * t3350 + t297 * t3350 + t299 * t3350 + + t310 * t3361 + t3350 * t594 + t3363 + t3399 + t3400 - t3406 * t7 + + t3407; + const auto t3409 = t1471 + t310 * t3353 + t3365 * t593 + t3366 + t3402 + + t3403 + t3404 + t3405; + const auto t3410 = std::pow(t598, 2); + const auto t3411 = t1000 * t3410 + t3410 * t60 + std::pow(t596, 2); + const auto t3412 = + t1448 * (t1473 * t3409 + t1675 * t3408 + t2600 * t3409 + t3411); + const auto t3413 = t1486 * t3406; + const auto t3414 = -t130 * t3371 - t1479 - t3236 * t483 - t3413; + const auto t3415 = std::pow(t3239, 2); + const auto t3416 = t19 * t3240; + const auto t3417 = 2 * t3242; + const auto t3418 = t1 * t145 * t3414 + 3 * t143 * t1489 * t3415 + - t143 * t3412 - t3397 - t3416 * t3417; + const auto t3419 = t1480 * t3389; + const auto t3420 = t157 * t3227; + const auto t3421 = -t1501 + t1517 + t1518 + t3392; + const auto t3422 = -t3381; + const auto t3423 = t3390 * t465; + const auto t3424 = -t10 * t3423 + t1505 * t22 + t1507 * t22 + + t157 * t3381 + t3330 + t3422; + const auto t3425 = -t15 * t3421 * t7 + t3424; + const auto t3426 = -3 * t1536 * t24 * t3395 * t59 + + t1595 * (-2 * t3420 - t3425) + t1598 * t3251 * t3298 + + t317 * t3388; + const auto t3427 = 2 * t16; + const auto t3428 = -3 * t1536 * t18 * t3395 * t59 + t287 * t3388 + + t3333 * t3394 + t3396 * t3427; + const auto t3429 = t16 * t3240; + const auto t3430 = -t3367; + const auto t3431 = + t277 * (-t1781 * t3364 + t281 * t3430 + t3369 + t3430 * t425); + const auto t3432 = t1626 * std::pow(t583, 2); + const auto t3433 = 2 * t584; + const auto t3434 = -t3384; + const auto t3435 = t155 + * (t158 * t3377 + t1615 * t3434 - t230 * t3387 + t2684 * t3434 + + t3377 * t56 + t3378 * t35); + const auto t3436 = t1619 * std::pow(t547, 2); + const auto t3437 = + t18 * t3436 - t1876 * t251 * t546 - t2717 * t3384 - t287 * t3435; + const auto t3438 = -t3409; + const auto t3439 = + t293 * (-t1765 * t3408 + t305 * t3438 + t3411 + t3438 * t448); + const auto t3440 = t1609 * std::pow(t599, 2); + const auto t3441 = 2 * t600; + const auto t3442 = + -2 * t1911 * t586 + t24 * t3436 - t317 * t3435 - t3387 * t505; + const auto t3443 = t157 * t3268; + const auto t3444 = t3262 * t66; + const auto t3445 = t166 * t3273; + const auto t3446 = -t1654; + const auto t3447 = 3 * t1651; + const auto t3448 = t1650 * t3423; + const auto t3449 = t1648 * t2818; + const auto t3450 = t1653 * t2286; + const auto t3451 = t1649 + t3446 - t3447 + t3448 - t3449 + t3450; + const auto t3452 = t2041 * t35; + const auto t3453 = t35 * t624; + const auto t3454 = t35 * t627; + const auto t3455 = t158 * t578; + const auto t3456 = t207 * t697; + const auto t3457 = t32 * t671; + const auto t3458 = 3 * t1844; + const auto t3459 = t3 * t43; + const auto t3460 = + t1843 * t1958 - t1843 + t2198 + t262 * t3458 + t264 * t3458 + t3459; + const auto t3461 = t3 * t577 + t3457 + t3458 * t46 + t3460; + const auto t3462 = t1660 * t1844; + const auto t3463 = + t227 * t3457 + t227 * t3460 + t272 * t3462 + t391 * t577 + t668; + const auto t3464 = -3 * t11 * t32 * t45 * t59 - t11 * t35 * t577 + - t3 * t32 * t35 * t671 - t3 * t3460 * t35 + t578; + const auto t3465 = -t3464; + const auto t3466 = t1978 + * (t1560 * t3461 + t1855 * t668 + t3455 * t668 + t3456 * t578 + + t3463 * t52 + t3465 * t48); + const auto t3467 = -t1980 * t7; + const auto t3468 = t1993 * t3224; + const auto t3469 = t1996 * t3222; + const auto t3470 = t32 * t655; + const auto t3471 = + t1500 * t1889 + t1501 * t1889 + t1863 * t1958 - t1863 + t2238; + const auto t3472 = t212 * t3458 + t3 * t539 + t3470 + t3471; + const auto t3473 = + t1523 * t3462 + t207 * t540 + t227 * t3470 + t227 * t3471 + t656; + const auto t3474 = -3 * t10 * t11 * t32 * t59 - t11 * t35 * t539 + - t3 * t32 * t35 * t655 - t3 * t3471 * t35 + t546; + const auto t3475 = -t3474; + const auto t3476 = t155 + * (t12 * t35 * t546 * t656 - t1515 * t3472 - t18 * t3475 + - t24 * t3473 + t3 * t35 * t546 * t698 + t35 * t542 * t656 * t7); + const auto t3477 = t1058 * t7 + t2241; + const auto t3478 = t1966 * t3314; + const auto t3479 = t1059 * t3298; + const auto t3480 = -3 * t1536 * t1966 * t21 * t59 * t616 + t19 * t3478 + + t19 * t3479 + t2243 * (-t3229 - t3477) + t331 * t3476; + const auto t3481 = t1 * t125 * t465 * (t2215 + t3220 + t3467) + + 3 * t112 * t1571 * t1995 * t3221 * t35 - t112 * t3466 + - t19 * t3468 - t19 * t3469 - t3480; + const auto t3482 = t158 * t598; + const auto t3483 = t207 * t702; + const auto t3484 = t32 * t642; + const auto t3485 = t3 * t84; + const auto t3486 = + t1885 * t1958 - t1885 + t2198 + t297 * t3458 + t299 * t3458 + t3485; + const auto t3487 = t3 * t593 + t3458 * t87 + t3484 + t3486; + const auto t3488 = + t227 * t3484 + t227 * t3486 + t310 * t3462 + t391 * t593 + t639; + const auto t3489 = -3 * t11 * t32 * t59 * t86 - t11 * t35 * t593 + - t3 * t32 * t35 * t642 - t3 * t3486 * t35 + t598; + const auto t3490 = -t3489; + const auto t3491 = t1937 + * (t1473 * t3487 + t1892 * t639 + t3482 * t639 + t3483 * t598 + + t3488 * t89 + t3490 * t91); + const auto t3492 = -t1939 * t7; + const auto t3493 = t1952 * t3242; + const auto t3494 = t1 * t145 * t465 * (t2262 + t3238 + t3492) + + 3 * t143 * t1489 * t1954 * t3239 * t35 - t143 * t3491 + - t19 * t3493 - t1955 * t3416 - t3480; + const auto t3495 = t157 * t1939 + t1951; + const auto t3496 = t3275 * t35; + const auto t3497 = t1058 * t157; + const auto t3498 = -3 * t1536 * t1966 * t24 * t59 * t616 + + t1595 * (t1059 + t2284 + t2288 + t3249 - t3497) + t1966 * t3297 + + t22 * t3479 + t317 * t3476; + const auto t3499 = t157 * t1980 + t1992; + const auto t3500 = t3318 * t35; + const auto t3501 = t1981 * t22 + t2303; + const auto t3502 = t15 * t3224; + const auto t3503 = t1964 * t22; + const auto t3504 = -3 * t1536 * t18 * t1966 * t59 * t616 + + t1595 * (t2306 + t2309 + t3258 - t3503 + t613) + t16 * t3478 + + t2011 * t3313 + t287 * t3476; + const auto t3505 = t1940 * t22 + t2313; + const auto t3506 = t15 * t3242; + const auto t3507 = t277 + * (t12 * t35 * t578 * t668 - t281 * t3461 - t282 * t3463 + + t284 * t3464 - t2859 * t578 + t582 * t668 * t7); + const auto t3508 = t1626 * t583; + const auto t3509 = t3508 * t676; + const auto t3510 = t155 + * (t158 * t546 * t656 - t1615 * t3472 + t227 * t542 * t656 + - t230 * t3473 + t234 * t3474 - t2846 * t546); + const auto t3511 = t287 * t547; + const auto t3512 = t1152 * t586 - t155 * t3 * t35 * t546 * t661 + + t18 * t3510 + t26 * t3475 + t2882 * t3511; + const auto t3513 = t293 + * (t12 * t35 * t598 * t639 - t2830 * t598 - t305 * t3487 + - t306 * t3488 + t308 * t3489 + t596 * t639 * t7); + const auto t3514 = t599 * t647; + const auto t3515 = t1156 * t586 - t155 * t35 * t542 * t661 + + t1618 * t2031 * t547 + t24 * t3510 + t26 * t3473; + const auto t3516 = t157 * t3452; + const auto t3517 = t1040 * t2039; + const auto t3518 = t1920 * t66; + const auto t3519 = t154 + * (-t222 + * (-t101 + * (t277 * t583 * t668 * t7 + t3463 * t54 - t3507 * t52 + - t3509 * t52 - t3515 - t582 * t677) + - t102 + * (-t1907 * t648 - t2038 * t3514 + t293 * t599 * t702 + + t3490 * t95 - t3512 - t3513 * t91) + + t55 + * (-t1688 * t3514 + t293 * t599 * t639 * t7 + + t3488 * t95 - t3513 * t89 - t3515 - t596 * t648) + + t588 * t707 + t602 * t711 - t604 * t709 - t605 * t713 + + t96 + * (-t1916 * t677 + t277 * t583 * t697 + t3465 * t54 + - t3507 * t48 - t3509 * t48 - t3512)) + + t2335 + + t3 + * (t126 + * (3 * t134 * t1489 * t1954 * t3239 * t35 + - t134 * t3491 + + t145 * t15 * (-t2279 - t3247 - t3495) + - t1954 * t3496 - t22 * t3493 - t3498) + + t146 * t3481 + - t150 + * (3 * t123 * t1571 * t1995 * t3221 * t35 + - t123 * t3466 + + t125 * t15 * (-t2298 - t3255 - t3499) + - t1995 * t3500 - t22 * t3468 - t3498) + - t151 * t3494 + t1921 * t621 - t1923 * t625 + + t3453 * t665 - t3454 * t682) + + t3274 + t3452 - t3516 + t3517 - t3518 + + t7 + * (-t126 + * (3 * t139 * t1489 * t1954 * t3239 * t35 + - t139 * t3491 + + t145 * t15 * (-t2312 - t3260 - t3505) + - t1955 * t3429 - t2021 * t3506 - t3504) + + t150 + * (3 * t118 * t1571 * t1995 * t3221 * t35 + - t118 * t3466 + + t125 * t15 * (-t2302 - t3257 - t3501) + - t16 * t3469 - t2007 * t3502 - t3504) + + t152 * t3494 - t153 * t3481 - t1921 * t634 + + t1923 * t635 - t3453 * t703 + t3454 * t700)); + const auto t3520 = t2189 * t35; + const auto t3521 = t2979 + t3305; + const auto t3522 = t2136 * t22 + t3301; + const auto t3523 = t1 * t43; + const auto t3524 = + t1841 * t762 + t1841 * t763 + t2050 * t2778 - t2778 + t2979 + t3523; + const auto t3525 = -t1 * t32 * t35 * t765 - t1 * t35 * t3524 + - 3 * t12 * t32 * t45 * t59 - t12 * t35 * t577 + t578; + const auto t3526 = t1 * t577; + const auto t3527 = t32 * t765; + const auto t3528 = t1660 * t2744; + const auto t3529 = + t227 * t3524 + t227 * t3526 + t227 * t3527 + t272 * t3528 + t770; + const auto t3530 = 3 * t2744; + const auto t3531 = t3524 + t3526 + t3527 + t3530 * t46; + const auto t3532 = t56 * t578; + const auto t3533 = t1855 * t770 + t3532 * t770; + const auto t3534 = t1978 + * (t2110 * t578 + t2618 * t3531 - t3525 * t50 + t3529 * t52 + + t3533); + const auto t3535 = t2130 * t3222; + const auto t3536 = -t3291; + const auto t3537 = t2143 * t22; + const auto t3538 = t222 * t32; + const auto t3539 = + t1498 * t3538 + t1501 * t3538 + t2050 * t2761 - t2761 + t3289; + const auto t3540 = -t1 * t32 * t35 * t722 - t1 * t35 * t3539 + - 3 * t10 * t12 * t32 * t59 - t12 * t35 * t539 + t546; + const auto t3541 = t32 * t722; + const auto t3542 = + t1523 * t3528 + t222 * t540 + t227 * t3539 + t227 * t3541 + t723; + const auto t3543 = t1 * t539 + t1841 * t223 + t3539 + t3541; + const auto t3544 = -t11 * t35 * t546 * t723 - t35 * t542 * t7 * t723; + const auto t3545 = t155 + * (t1 * t35 * t546 * t729 + t21 * t3540 - t24 * t3542 + - t3295 * t3543 - t3544); + const auto t3546 = t2100 * t3314; + const auto t3547 = -3 * t1536 * t21 * t2100 * t59 * t616 + + t1595 * (t19 * t3536 + t3312 - t3537) + t19 * t3546 + + t2146 * t3313 + t331 * t3545; + const auto t3548 = 3 * t112 * t1571 * t2129 * t3221 * t35 - t112 * t3534 + + t125 * t15 * (t1 * t15 * t3521 - t3308 - t3522) - t19 * t3535 + - t2138 * t3502 - t3547; + const auto t3549 = t2979 + t3280; + const auto t3550 = t2153 * t22 + t3324; + const auto t3551 = t1 * t84; + const auto t3552 = + t1841 * t747 + t1841 * t748 + t2050 * t2743 - t2743 + t2979 + t3551; + const auto t3553 = -t1 * t32 * t35 * t752 - t1 * t35 * t3552 + - 3 * t12 * t32 * t59 * t86 - t12 * t35 * t593 + t598; + const auto t3554 = t1 * t593; + const auto t3555 = t32 * t752; + const auto t3556 = + t227 * t3552 + t227 * t3554 + t227 * t3555 + t310 * t3528 + t750; + const auto t3557 = t3530 * t87 + t3552 + t3554 + t3555; + const auto t3558 = t56 * t598; + const auto t3559 = t1892 * t750 + t3558 * t750; + const auto t3560 = t1937 + * (t1899 * t780 + t2600 * t3557 - t3553 * t93 + t3556 * t89 + + t3559); + const auto t3561 = 3 * t143 * t1489 * t2077 * t3239 * t35 - t143 * t3560 + + t145 * t15 * (t1 * t15 * t3549 - t3325 - t3550) - t2078 * t3416 + - t2155 * t3506 - t3547; + const auto t3562 = t157 * t2069 + t2073; + const auto t3563 = t2074 * t3242; + const auto t3564 = t157 * t2092; + const auto t3565 = t2101 * t3298; + const auto t3566 = -3 * t1536 * t2100 * t24 * t59 * t616 + + t1595 * (t22 * t3536 + t3294 - t3564) + t2100 * t3297 + + t22 * t3565 + t317 * t3545; + const auto t3567 = t157 * t2121 + t2125; + const auto t3568 = t2126 * t3224; + const auto t3569 = -t2121 * t7 + t3329; + const auto t3570 = t2092 * t7; + const auto t3571 = -3 * t1536 * t18 * t2100 * t59 * t616 + t16 * t3546 + + t16 * t3565 + t287 * t3545 + t3333 * (-t3332 - t3570); + const auto t3572 = -t2069 * t7 + t3336; + const auto t3573 = t277 + * (-t2177 * t578 + t280 * t3525 - t282 * t3529 - t3531 * t425 + + t3533); + const auto t3574 = t3508 * t773; + const auto t3575 = t222 * t546; + const auto t3576 = t155 + * (t225 * t3540 - t230 * t3542 - t2684 * t3543 - t3544 + - t3575 * t728); + const auto t3577 = -t155 * t3 * t35 * t546 * t736 + t18 * t3576 + + t2941 * t586 + t2950 * t3511 + t2962 * t3543; + const auto t3578 = t293 + * (-t2170 * t598 + t304 * t3553 - t306 * t3556 - t3557 * t448 + + t3559); + const auto t3579 = t599 * t755; + const auto t3580 = t317 * t547; + const auto t3581 = -t155 * t35 * t542 * t736 + t24 * t3576 + t26 * t3542 + + t2950 * t3580 + t586 * t734; + const auto t3582 = t157 * t3520; + const auto t3583 = t1040 * t2187; + const auto t3584 = t2045 * t66; + const auto t3585 = t154 + * (-t222 + * (-t101 + * (t277 * t583 * t7 * t770 + t3529 * t54 - t3573 * t52 + - t3574 * t52 - t3581 - t582 * t774) + - t102 + * (-t1907 * t756 - t2038 * t3579 + + t293 * t3 * t599 * t750 + t3 * t35 * t3557 * t95 + - t3577 - t3578 * t91) + + t55 + * (-t1688 * t3579 + t293 * t599 * t7 * t750 + + t3556 * t95 - t3578 * t89 - t3581 - t596 * t756) + + t588 * t795 + t602 * t793 - t604 * t798 - t605 * t797 + + t96 + * (-t1916 * t774 + t277 * t3 * t583 * t770 + + t3 * t35 * t3531 * t54 - t3573 * t48 + - t3574 * t48 - t3577)) + + t3 + * (t126 + * (3 * t134 * t1489 * t2077 * t3239 * t35 + - t134 * t3560 + + t145 * t15 * (t15 * t3549 * t7 - t3286 - t3562) + - t2077 * t3496 - t22 * t3563 - t3566) + + t146 * t3548 + - t150 + * (3 * t123 * t1571 * t2129 * t3221 * t35 + - t123 * t3534 + + t125 * t15 * (t15 * t3521 * t7 - t3322 - t3567) + - t2129 * t3500 - t22 * t3568 - t3566) + - t151 * t3561 + t2046 * t621 - t2047 * t625 + + t3453 * t760 - t3454 * t779) + + t3273 - t3337 + t3338 + t3339 - t3340 + t3341 + t3342 - t3343 + - t3344 + t3520 - t3582 + t3583 - t3584 + + t7 + * (-t126 + * (3 * t139 * t1489 * t2077 * t3239 * t35 + - t139 * t3560 + + t145 * t3 * t465 * (t3283 + t3549 + t3572) + - t16 * t3563 - t2078 * t3429 - t3571) + + t150 + * (3 * t118 * t1571 * t2129 * t3221 * t35 + - t118 * t3534 + + t125 * t3 * t465 * (t3319 + t3521 + t3569) + - t16 * t3535 - t16 * t3568 - t3571) + + t152 * t3561 - t153 * t3548 - t2046 * t634 + + t2047 * t635 - t3453 * t789 + t3454 * t787)); + const auto t3586 = t2328 * t35; + const auto t3587 = -t3349; + const auto t3588 = t1476 * t1841; + const auto t3589 = t1927 * t580; + const auto t3590 = t260 + t262 + 2 * t264 + t3588 * t43 + t3589 * t38 + + t3589 * t41 - t43 * t535 + t928; + const auto t3591 = 3 * e0_z; + const auto t3592 = t3591 + t6; + const auto t3593 = t186 * t577 + t2984 * t535 + t3587 + t3588 * t45 + + t3590 * t7 + t3592 + 2 * t51 - t569 - t570 - t571 - t572 + t848; + const auto t3594 = t3590 + t46 + t535 * t850 + t579 + 3 * t581; + const auto t3595 = t3455 * t849 + t3532 * t849; + const auto t3596 = t1978 + * (t1975 * t3594 + t2128 * t3594 + t3593 * t52 + t3595 + + t582 * t858); + const auto t3597 = -t1987; + const auto t3598 = t1247 * t1988 + t1247 * t3304 + t1274 + t1984 + - 3 * t1985 + t3423 * t42 + t3597; + const auto t3599 = t1586 - t22 * t2218 + t3253 + t3320 + t3598; + const auto t3600 = t2223 * t3224; + const auto t3601 = t2226 * t3222; + const auto t3602 = d_z * t32; + const auto t3603 = t1498 * t530 + t1500 * t530 + t2199 * t3602 - t3602; + const auto t3604 = t7 * t808; + const auto t3605 = t10 * t3588 + t186 * t539 + + t227 * (t1509 + t3603 + t4 + t5) + 2 * t228 + t3422 + t3604 * t535 + - t536 - t537 - t538 - t545 + t807; + const auto t3606 = + t1501 + t1505 + t1507 + t32 * t808 + t3603 + t540 + 3 * t541; + const auto t3607 = -t11 * t35 * t546 * t813 - t12 * t35 * t546 * t813; + const auto t3608 = t155 + * (-t1515 * t3606 - t24 * t3605 - t3295 * t3606 + t35 * t542 * t810 + - t3607); + const auto t3609 = -t2290 + t3227 * t7 - t3393; + const auto t3610 = t3298 * t613; + const auto t3611 = t2244 * t3314; + const auto t3612 = -3 * t1536 * t21 * t2244 * t59 * t616 + t19 * t3610 + + t19 * t3611 + t2243 * t3609 + t331 * t3608; + const auto t3613 = t1 * t125 * t15 * t3599 + + 3 * t112 * t1571 * t2225 * t3221 * t35 - t112 * t3596 + - t19 * t3600 - t19 * t3601 - t3612; + const auto t3614 = -t3400; + const auto t3615 = t295 + t297 + 2 * t299 + t3588 * t84 + t3589 * t79 + + t3589 * t82 - t535 * t84 + t928; + const auto t3616 = t186 * t593 + t3588 * t86 + t3592 + t3614 + + t3615 * t7 + t535 * t835 - t589 - t590 - t591 - t597 + t833 + + 2 * t88; + const auto t3617 = t3615 + t535 * t834 + t594 + 3 * t595 + t87; + const auto t3618 = t3482 * t839 + t3558 * t839; + const auto t3619 = t1937 + * (t1934 * t3617 + t2076 * t3617 + t3616 * t89 + t3618 + + t596 * t838); + const auto t3620 = -t1946; + const auto t3621 = t1247 * t1947 + t1247 * t3279 + t1274 + t1943 + - 3 * t1944 + t3423 * t83 + t3620; + const auto t3622 = t1603 - t22 * t2265 + t3245 + t3284 + t3621; + const auto t3623 = t2270 * t3242; + const auto t3624 = t1 * t145 * t15 * t3622 + + 3 * t143 * t1489 * t2272 * t3239 * t35 - t143 * t3619 + - t19 * t3623 - t2273 * t3416 - t3612; + const auto t3625 = t157 * t2265; + const auto t3626 = 2 * t127; + const auto t3627 = -t1600 * t3390 + t2017 * t22 + t22 * t3626 + + 2 * t2263 + t3335 + t3400; + const auto t3628 = e1_z - t3591; + const auto t3629 = -3 * t1536 * t2244 * t24 * t59 * t616 + + t1595 * (-t157 * t612 - t22 * t3421 + t3420 + t3424) + + t2244 * t3297 + t2293 * t3313 + t317 * t3608; + const auto t3630 = t157 * t2218; + const auto t3631 = 2 * t104; + const auto t3632 = -t1583 * t3390 + t2002 * t22 + t22 * t3631 + + 2 * t2216 + t3328 + t3349; + const auto t3633 = -3 * t1536 * t18 * t2244 * t59 * t616 + t16 * t3610 + + t16 * t3611 + t287 * t3608 + t3333 * t3609; + const auto t3634 = t277 + * (-t282 * t3593 - t3594 * t669 - t3594 * t771 + t3595 + - t582 * t851); + const auto t3635 = t3508 * t853; + const auto t3636 = t155 + * (-t1615 * t3606 - t230 * t3605 - t2684 * t3606 - t3607 + - t543 * t809); + const auto t3637 = -t155 * t3 * t35 * t546 * t815 + t18 * t3636 + + t2962 * t3606 + t2998 * t586 + t3017 * t3511; + const auto t3638 = t293 + * (-t306 * t3616 - t3617 * t640 - t3617 * t751 + t3618 + - t596 * t837); + const auto t3639 = t599 * t841; + const auto t3640 = -t155 * t35 * t542 * t815 + t24 * t3636 + t26 * t3605 + + t3017 * t3580 + t586 * t811; + const auto t3641 = t1040 * t2326; + const auto t3642 = t2194 * t66; + const auto t3643 = t154 + * (-t157 * t3586 + t1654 + t2025 + - t222 + * (-t101 + * (t277 * t583 * t858 + t3593 * t54 - t3634 * t52 + - t3635 * t52 - t3640 - t582 * t854) + - t102 + * (-t1907 * t842 - t2038 * t3639 + + t293 * t3 * t599 * t839 + t3 * t3617 * t95 + - t3637 - t3638 * t91) + + t55 + * (-t1688 * t3639 + t293 * t599 * t838 + t3616 * t95 + - t3638 * t89 - t3640 - t596 * t842) + + t588 * t872 + t602 * t871 - t604 * t874 - t605 * t873 + + t96 + * (-t1916 * t854 + t277 * t3 * t583 * t849 + + t3 * t3594 * t54 - t3634 * t48 - t3635 * t48 + - t3637)) + + t3 + * (t126 + * (3 * t134 * t1489 * t2272 * t3239 * t35 + - t134 * t3619 + + t145 * t15 + * (t13 * t15 * t3236 + t3621 * t7 - t3625 + - t3627 - t3628) + - t2272 * t3496 - t2282 * t3506 - t3629) + + t146 * t3613 + - t150 + * (3 * t123 * t1571 * t2225 * t3221 * t35 + - t123 * t3596 + + t125 * t15 + * (t13 * t15 * t3218 + t3598 * t7 - t3628 + - t3630 - t3632) + - t2225 * t3500 - t2301 * t3502 - t3629) + - t151 * t3624 + t2195 * t621 - t2196 * t625 + + t3453 * t845 - t3454 * t859) + + t3267 + t3443 + t3444 - t3445 + t3447 - t3448 + t3449 - t3450 + + t3586 + t3641 - t3642 + + t7 + * (-t126 + * (3 * t139 * t1489 * t2272 * t3239 * t35 + - t139 * t3619 + t145 * t15 * t3 * t3622 + - t16 * t3623 - t2273 * t3429 - t3633) + + t150 + * (3 * t118 * t1571 * t2225 * t3221 * t35 + - t118 * t3596 + t125 * t15 * t3 * t3599 + - t16 * t3600 - t16 * t3601 - t3633) + + t152 * t3624 - t153 * t3613 - t2195 * t634 + + t2196 * t635 - t3453 * t867 + t3454 * t866)); + const auto t3644 = -2 * t1 * t465 * t50 * t7 - 2 * t13 * t465 * t52 + + t15 * t52 + t22 * t582 - 2 * t3 * t465 * t48 * t7; + const auto t3645 = 3 * t583; + const auto t3646 = t2341 * t3645; + const auto t3647 = -t15 * t3 * t583 * t7 * t892 + t3189; + const auto t3648 = -t3455 + t578 * t58; + const auto t3649 = 2 * t1844; + const auto t3650 = t1623 * t3649 - t1855 + t281 * t3350; + const auto t3651 = t121 + t282 * t531 + t3348 + t3650; + const auto t3652 = t3648 + t3651; + const auto t3653 = t583 * t880; + const auto t3654 = -t3350 * t60; + const auto t3655 = t2357 * t583; + const auto t3656 = t1788 * t3655 - t1916 * t881 + t3653 * t58 + t3654; + const auto t3657 = t3061 * t3652 + t3656; + const auto t3658 = t54 * t604; + const auto t3659 = t2362 * t3658 + t602 * t897; + const auto t3660 = t282 * t880; + const auto t3661 = -t1783 * t3655 + t582 * t881; + const auto t3662 = t3653 * t391; + const auto t3663 = t1844 * t2396; + const auto t3664 = t207 + t3662 + t3663; + const auto t3665 = t207 * t914; + const auto t3666 = t280 * t3645; + const auto t3667 = t1844 * t3080; + const auto t3668 = t3653 * t389 + t3667; + const auto t3669 = -t2357 * t3666 + t2790 * t881 + t3668; + const auto t3670 = -t3652 * t3665 + t3669; + const auto t3671 = + -t1829 * t304 + t1899 * t404 + t2373 * t547 - t3575 * t358; + const auto t3672 = -t1834 * t889 + t3671 * t885; + const auto t3673 = t1831 * t889 + t3671 * t883; + const auto t3674 = t166 * t2378; + const auto t3675 = t157 * t2381; + const auto t3676 = t2377 * t66; + const auto t3677 = t2381 + t3674 - t3675 - t3676; + const auto t3678 = t2402 * t602; + const auto t3679 = -t3532 + t578 * t911; + const auto t3680 = t3651 + t3679; + const auto t3681 = + t1916 * t2395 - 3 * t2356 * t284 * t583 * t913 + t3668; + const auto t3682 = -t1 * t284 * t35 * t3680 * t880 + t3681; + const auto t3683 = t222 * t3680; + const auto t3684 = -t1783 * t2406 * t583 + t2395 * t582; + const auto t3685 = t3653 * t909; + const auto t3686 = t2396 * t2744; + const auto t3687 = t222 + t3685 + t3686; + const auto t3688 = -t1000 * t3350; + const auto t3689 = -t1 * t35 * t578 * t880 * t913 + t3086 * t3645 + + t3653 * t911 + t3688; + const auto t3690 = -t3683 * t914 - t3689; + const auto t3691 = t1834 * t917 + t2409 * t3671; + const auto t3692 = t1831 * t917 + t2413 * t3671; + const auto t3693 = t157 * t2419; + const auto t3694 = t166 * t2417; + const auto t3695 = t2415 * t66; + const auto t3696 = -t2419 + t3693 + t3694 - t3695; + const auto t3697 = t578 * t7; + const auto t3698 = + -t1000 * t3697 + t1326 * t283 - t3697 * t60 + t582 * t923; + const auto t3699 = t280 * t3686 + t281 + t284 * t3663 + t3698 + t425; + const auto t3700 = t3699 * t880; + const auto t3701 = -t3205; + const auto t3702 = t2430 * t583; + const auto t3703 = t1783 * t3702 + t3653 * t923 + t3701 - t582 * t926; + const auto t3704 = t282 * t3700 + t3703; + const auto t3705 = t602 * t938 + t604 * t939; + const auto t3706 = -t2430 * t3666 + t2790 * t926; + const auto t3707 = t3687 - t3699 * t914 + t3706; + const auto t3708 = t1834 * t930 + t3671 * t929; + const auto t3709 = -t1788 * t3702 + t1916 * t926; + const auto t3710 = t1831 * t930 - t3671 * t932; + const auto t3711 = t157 * t2423; + const auto t3712 = t166 * t2425; + const auto t3713 = t2421 * t66; + const auto t3714 = t2423 - t3711 - t3712 + t3713; + const auto t3715 = t474 * t598 + t558 * t598; + const auto t3716 = t15 * t89; + const auto t3717 = t3184 * t89; + const auto t3718 = t1025 * t91; + const auto t3719 = t469 * t93; + const auto t3720 = t22 * t596 - t3718 - t3719; + const auto t3721 = t3716 - t3717 + t3720; + const auto t3722 = -t3715 - t3721; + const auto t3723 = t3 * t996; + const auto t3724 = 3 * t599; + const auto t3725 = t2450 * t3724; + const auto t3726 = t3725 * t942; + const auto t3727 = t599 * t948; + const auto t3728 = t1899 * t949 + t3726 * t93 - t3727 * t64 + t484; + const auto t3729 = -t3722 * t3723 + t3728; + const auto t3730 = t3 * t3722; + const auto t3731 = t3189 - t3727 * t66; + const auto t3732 = -t2449 * t3730 + t3726 * t89 + t3731 + t596 * t949; + const auto t3733 = t152 * t95; + const auto t3734 = t1907 * t949 + t3726 * t91 - t3727 * t57 + t565; + const auto t3735 = t2464 * t624 + t2465 * t635; + const auto t3736 = -t3482 + t58 * t598; + const auto t3737 = t1606 * t3649 - t1892 + t305 * t3350; + const auto t3738 = t132 + t306 * t531 + t3399 + t3737; + const auto t3739 = t207 * t958; + const auto t3740 = t599 * t957; + const auto t3741 = t2472 * t3724; + const auto t3742 = -t1907 * t2471 + t2474 * t3741 + t3654 + t3740 * t58; + const auto t3743 = t588 * t95; + const auto t3744 = t3743 * t962 + t605 * t961; + const auto t3745 = t157 * t2485; + const auto t3746 = t166 * t2480; + const auto t3747 = t2479 * t66; + const auto t3748 = t2485 - t3745 + t3746 - t3747; + const auto t3749 = t605 * t978; + const auto t3750 = -t3558 + t598 * t911; + const auto t3751 = t3738 + t3750; + const auto t3752 = t3667 + t3740 * t389; + const auto t3753 = + t1907 * t968 - 3 * t2472 * t308 * t599 * t967 + t3752; + const auto t3754 = t1 * t308 * t35 * t3751 * t957 - t3753; + const auto t3755 = t2498 * t596; + const auto t3756 = t1030 * t598 + t15 * t3558; + const auto t3757 = t1 * (-t3721 - t3756); + const auto t3758 = t3725 * t975; + const auto t3759 = t3758 * t89; + const auto t3760 = t166 * t3727; + const auto t3761 = t3198 - t3760; + const auto t3762 = -t2449 * t3757 + t3755 + t3759 + t3761; + const auto t3763 = + -t1899 * t968 + t304 * t3741 * t967 + t3688 + t3740 * t911; + const auto t3764 = t1230 * t588 + t623 * t978; + const auto t3765 = t1140 - t164 * t3727 + t1899 * t2498 + t3758 * t93; + const auto t3766 = t2502 * t625 + t2520 * t624; + const auto t3767 = -t157 * t2522 - t166 * t2524 + t2521 * t66 + t2522; + const auto t3768 = 2 * t1 * t13 * t465 * t93 + 2 * t13 * t3 * t465 * t91 + - t1326 * t1474 - t189 * t596 - t22 * t3558 - t2443 - t3152 + - t3208 * t598; + const auto t3769 = + t1907 * t3180 + t2536 * t3725 + t3731 - t3768 * t91 * t948; + const auto t3770 = t598 * t7; + const auto t3771 = + -t1000 * t3770 + t1326 * t307 - t3770 * t60 + t596 * t923; + const auto t3772 = t306 * t992; + const auto t3773 = -t2528 * t596 + t3701 + t3740 * t923 + t3741 * t3772; + const auto t3774 = + t3773 + t993 * (t304 * t3686 + t305 + t308 * t3663 + t3771 + t448); + const auto t3775 = t2538 * t605 + t588 * t998; + const auto t3776 = + t1899 * t3180 + t3725 * t93 * t982 + t3761 - t3768 * t996; + const auto t3777 = t3178 * t605 + t623 * t998; + const auto t3778 = t2544 * t66; + const auto t3779 = t157 * t2549; + const auto t3780 = t166 * t2546; + const auto t3781 = t2549 + t3778 - t3779 - t3780; + const auto t3782 = t1530 * t35; + const auto t3783 = e1_x + t115; + const auto t3784 = t3782 + t3783; + const auto t3785 = 2 * t3784; + const auto t3786 = + -t226 * t406 + t226 - t231 * t406 + t231 - t234 * t3785 + t690; + const auto t3787 = t3786 * t72; + const auto t3788 = + t1354 + t1418 + t227 * t686 + t317 * t3787 + t317 * t694; + const auto t3789 = + t1171 + t1426 + t222 * t686 + t331 * t3787 + t331 * t694; + const auto t3790 = 2 * t3782; + const auto t3791 = t1465 + t267 + t3786 * t98 + t3790 + t705; + const auto t3792 = -t35 * t3791; + const auto t3793 = -t3788; + const auto t3794 = -t1024 - t1026 + t1153 + t1429 - t1430; + const auto t3795 = -t1150 - t1151 - t3794; + const auto t3796 = -t1034 * t3795 + t1159; + const auto t3797 = -t1068 * t3795 + t1164; + const auto t3798 = -t516; + const auto t3799 = t1048 * t3795 - t1169 + t1170 - t1172 + t19 + t3798; + const auto t3800 = -t3799; + const auto t3801 = -t3796; + const auto t3802 = -t1341 - t1342 - t3794; + const auto t3803 = -t1246 * t3802 + t1345; + const auto t3804 = -t1270 * t3802 + t1347; + const auto t3805 = t1259 * t3802 - t1352 + t1353 - t1355 + t22 + t629; + const auto t3806 = -t3805; + const auto t3807 = -t3803; + const auto t3808 = t59 * t680; + const auto t3809 = 2 * t3808; + const auto t3810 = t59 * t683; + const auto t3811 = 2 * t3810; + const auto t3812 = 2 * t82; + const auto t3813 = 2 * t85; + const auto t3814 = -t79 - t82 - t85; + const auto t3815 = 3 * t3782; + const auto t3816 = t1452 + t1958 * t82 + t1958 * t85 + t3815 * t78; + const auto t3817 = -2 * t11 * t35 * t642 - t1463 - 3 * t1530 * t59 * t86 + + t1925 + t207 * t3812 + t207 * t3813 + t266 + - t3 * t35 * (t3814 + t3816) + t406 * t78 + t90; + const auto t3818 = -t3817; + const auto t3819 = 2 * t79; + const auto t3820 = -t3812 - t3813 - t3819; + const auto t3821 = t3816 + t3820 + 2 * t643 + 3 * t644; + const auto t3822 = std::pow(t639, 2); + const auto t3823 = t158 * t3822 + t186 * t3822; + const auto t3824 = t1937 + * (t1473 * t3821 + t1474 * t3821 + t35 * std::pow(t702, 2) + + t3818 * t91 + t3823); + const auto t3825 = -2 * t1 * t80; + const auto t3826 = -2 * t7 * t83; + const auto t3827 = t1532 * t77 + t178 * t1947 + t178 * t1948; + const auto t3828 = + 2 * t1939 * t3 - 3 * t2020 + 4 * t3 * t77 - t3825 - t3826 - t3827; + const auto t3829 = std::pow(t1954, 2); + const auto t3830 = t1952 * t1955; + const auto t3831 = std::pow(t656, 2); + const auto t3832 = d_x * t3815 + t1500 * t56 + t1501 * t56; + const auto t3833 = t1513 + t3832 + 2 * t657 + 3 * t658; + const auto t3834 = -t1503 * t56 - t1507 * t207 - t1509 * t207 + + t1523 * t1531 + t1524 + t207 * (t1522 + t3832) + t406 * t655; + const auto t3835 = t155 + * (t12 * t35 * t3831 + t13 * t35 * t3831 - t1515 * t3833 + - t1516 * t3833 - t18 * t3834 + t35 * std::pow(t698, 2)); + const auto t3836 = t1534 + 2 * t1964; + const auto t3837 = std::pow(t1966, 2); + const auto t3838 = t1539 * t1966; + const auto t3839 = t1059 * t3838; + const auto t3840 = -3 * t1536 * t24 * t3837 * t59 + t1538 * t3839 + + t2099 * t3836 + t317 * t3835; + const auto t3841 = 2 * t41; + const auto t3842 = 2 * t44; + const auto t3843 = -t38 - t41 - t44; + const auto t3844 = t1546 + t1958 * t41 + t1958 * t44 + t37 * t3815; + const auto t3845 = -2 * t11 * t35 * t671 - 3 * t1530 * t45 * t59 - t1554 + + t1971 + t207 * t3841 + t207 * t3842 + t266 + - t3 * t35 * (t3843 + t3844) + t37 * t406 + t47; + const auto t3846 = -t3845; + const auto t3847 = 2 * t38; + const auto t3848 = -t3841 - t3842 - t3847; + const auto t3849 = t3844 + t3848 + 2 * t672 + 3 * t673; + const auto t3850 = std::pow(t668, 2); + const auto t3851 = t158 * t3850 + t186 * t3850; + const auto t3852 = t1978 + * (t1560 * t3849 + t1561 * t3849 + t35 * std::pow(t697, 2) + + t3846 * t48 + t3851); + const auto t3853 = -2 * t1 * t39; + const auto t3854 = -2 * t42 * t7; + const auto t3855 = t1532 * t36 + t178 * t1988 + t178 * t1989; + const auto t3856 = + 2 * t1980 * t3 - 3 * t2006 + 4 * t3 * t36 - t3853 - t3854 - t3855; + const auto t3857 = std::pow(t1995, 2); + const auto t3858 = t1993 * t1996; + const auto t3859 = -3 * t1536 * t21 * t3837 * t59 + t1578 * t3839 + + t2243 * t3836 + t331 * t3835; + const auto t3860 = t1 * t125 * t3856 * t465 + + 3 * t112 * t1571 * t3857 * t59 - t112 * t3852 - t1578 * t3858 + - t3859; + const auto t3861 = t1 * t145 * t3828 * t465 + + 3 * t143 * t1489 * t3829 * t59 - t143 * t3824 - t1578 * t3830 + - t3859; + const auto t3862 = -t1 * t39; + const auto t3863 = -t42 * t7; + const auto t3864 = -3 * t1536 * t18 * t3837 * t59 + + t1595 * (2 * t1058 * t11 * t15 - t1594) + t1598 * t2011 * t3838 + + t287 * t3835; + const auto t3865 = -t1 * t80; + const auto t3866 = -t7 * t83; + const auto t3867 = t35 * t711; + const auto t3868 = t35 * t713; + const auto t3869 = -t3821; + const auto t3870 = t293 + * (t305 * t3869 + t307 * t3869 + t308 * t3817 + + t35 * std::pow(t645, 2) + t3823); + const auto t3871 = t1609 * std::pow(t647, 2); + const auto t3872 = 2 * t648; + const auto t3873 = t227 * t639; + const auto t3874 = -t3833; + const auto t3875 = t155 + * (t158 * t3831 + t1615 * t3874 + t186 * t3831 - t234 * t3834 + + t35 * std::pow(t659, 2) + t380 * t3874); + const auto t3876 = t1618 * std::pow(t661, 2); + const auto t3877 = 2 * t662; + const auto t3878 = + t1156 * t3877 - t2029 * t3833 - t24 * t3875 + t317 * t3876; + const auto t3879 = -t3849; + const auto t3880 = t277 + * (t281 * t3879 + t283 * t3879 + t284 * t3845 + + t35 * std::pow(t674, 2) + t3851); + const auto t3881 = t1626 * std::pow(t676, 2); + const auto t3882 = 2 * t677; + const auto t3883 = t227 * t668; + const auto t3884 = t35 * t697; + const auto t3885 = + t1152 * t3877 - t18 * t3875 - t26 * t3834 + t287 * t3876; + const auto t3886 = t35 * t702; + const auto t3887 = t2039 * t35; + const auto t3888 = t59 * t776; + const auto t3889 = t59 * t781; + const auto t3890 = t186 * t639; + const auto t3891 = t222 * t780; + const auto t3892 = 3 * t388; + const auto t3893 = 3 * t389; + const auto t3894 = + t1958 * t2048 - t2048 + t2050 * t2833 - t2833 + t3893 * t85; + const auto t3895 = t1 * t642 + t3 * t752 + t3892 * t87 + t3894; + const auto t3896 = -3 * t1 * t11 * t59 * t86 - t1 * t3 * t35 * t642 + - t11 * t35 * t752 - t3 * t35 * t3894 + t750; + const auto t3897 = -t3896; + const auto t3898 = -t1 * t3 * t35 * t752 - t1 * t35 * t3894 + - 3 * t12 * t3 * t59 * t86 - t12 * t35 * t642 + t639; + const auto t3899 = t1937 + * (t1474 * t3895 + t3483 * t750 + t3890 * t750 + t3891 * t639 + + t3897 * t91 - t3898 * t93); + const auto t3900 = t1955 * t2074; + const auto t3901 = t1952 * t2078; + const auto t3902 = t1501 * t389 + t1958 * t2080 + t2050 * t2095 + t2096; + const auto t3903 = t1 * t655 + t212 * t3892 + t3 * t722 + t3902; + const auto t3904 = t225 - t718 - t719 - t720; + const auto t3905 = + t1657 * t1961 + t207 * t3902 + t222 * t657 + t3904 + t56 * t722; + const auto t3906 = t234 - t651 - t652 - t653; + const auto t3907 = t10 * t1000 * t1811 + t158 * t655 + t207 * t2922 + + t222 * t3902 + t3906; + const auto t3908 = t155 + * (t1 * t35 * t656 * t729 + t13 * t35 * t656 * t723 - t1516 * t3903 + - t18 * t3905 - t21 * t3907 + t3 * t35 * t698 * t723); + const auto t3909 = t2101 * t3838; + const auto t3910 = t1059 * t1539; + const auto t3911 = t2100 * t3910; + const auto t3912 = -3 * t1536 * t1966 * t2100 * t24 * t59 + + t2099 * (t1 * t1058 + t2093 + t2094 + t2097) + t22 * t3909 + + t22 * t3911 + t317 * t3908; + const auto t3913 = t186 * t668; + const auto t3914 = + t1958 * t2105 + t2050 * t2861 - t2105 - t2861 + t3893 * t44; + const auto t3915 = t1 * t671 + t3 * t765 + t3892 * t46 + t3914; + const auto t3916 = -3 * t1 * t11 * t45 * t59 - t1 * t3 * t35 * t671 + - t11 * t35 * t765 - t3 * t35 * t3914 + t770; + const auto t3917 = -t3916; + const auto t3918 = -t1 * t3 * t35 * t765 - t1 * t35 * t3914 + - 3 * t12 * t3 * t45 * t59 - t12 * t35 * t671 + t668; + const auto t3919 = t1978 + * (t1561 * t3915 + t2110 * t668 + t3456 * t770 + t3913 * t770 + + t3917 * t48 - t3918 * t50); + const auto t3920 = t1996 * t2126; + const auto t3921 = t1993 * t2130; + const auto t3922 = -t2117; + const auto t3923 = t15 * t2138; + const auto t3924 = -t1055 - t1056 - t1057 + t18; + const auto t3925 = t1966 * t2012; + const auto t3926 = -3 * t1536 * t1966 * t21 * t2100 * t59 + + t1595 * (t1058 * t174 + t19 * t2097 + t2140 + t2141 + t3924) + + t19 * t3911 + t2146 * t3925 + t331 * t3908; + const auto t3927 = 3 * t112 * t1571 * t1995 * t2129 * t59 - t112 * t3919 + + t125 * t15 * (t174 * t1980 + t19 * t3922 + t1992 + t2134) + - t19 * t3921 - t1996 * t3923 - t3926; + const auto t3928 = -t2065; + const auto t3929 = t15 * t2155; + const auto t3930 = 3 * t143 * t1489 * t1954 * t2077 * t59 - t143 * t3899 + + t145 * t15 * (t174 * t1939 + t19 * t3928 + t1951 + t2151) + - t19 * t3901 - t1955 * t3929 - t3926; + const auto t3931 = t15 * t2007; + const auto t3932 = -t2089 - t2090 - t2091 + t21; + const auto t3933 = t2011 * t2012; + const auto t3934 = -3 * t1536 * t18 * t1966 * t2100 * t59 + + t1595 * (t16 * t2097 + t19 * t1964 + t2160 + t2161 + t3932) + + t16 * t3909 + t2100 * t3933 + t287 * t3908; + const auto t3935 = t15 * t2021; + const auto t3936 = t35 * t707; + const auto t3937 = t35 * t709; + const auto t3938 = t293 + * (t13 * t35 * t639 * t750 - t2170 * t639 - t2830 * t750 + + t304 * t3898 - t307 * t3895 + t308 * t3896); + const auto t3939 = t648 * t750; + const auto t3940 = t1609 * t647; + const auto t3941 = t3940 * t755; + const auto t3942 = t155 + * (-t1162 * t728 + t186 * t656 * t723 - t225 * t3907 - t234 * t3905 + - t2846 * t723 - t380 * t3903); + const auto t3943 = t1156 * t758 - t2029 * t3903 + t2031 * t2950 + - t24 * t3942 + t662 * t734; + const auto t3944 = t277 + * (t13 * t35 * t668 * t770 - t2177 * t668 + t280 * t3918 + - t283 * t3915 + t284 * t3916 - t2859 * t770); + const auto t3945 = t677 * t770; + const auto t3946 = t1626 * t676; + const auto t3947 = t3946 * t773; + const auto t3948 = t287 * t2882; + const auto t3949 = t1152 * t758 - t18 * t3942 - t26 * t3905 + + t2941 * t662 + t3948 * t736; + const auto t3950 = t154 + * (t1040 * t2041 - t174 * t3887 + t1920 * t64 - t2045 + t2184 + - t2188 + t2190 + - t222 + * (-t101 + * (t1628 * t3915 + t1718 * t3947 - t227 * t3945 + - t3883 * t774 + t3943 - t3944 * t52) + - t102 + * (t1748 * t3941 - t207 * t3939 - t3886 * t756 + + t3897 * t95 - t3938 * t91 + t3949) + + t3867 * t795 - t3868 * t798 + t3936 * t793 + - t3937 * t797 + + t55 + * (t1611 * t3895 + t1675 * t3941 - t227 * t3939 + - t3873 * t756 - t3938 * t89 + t3943) + + t96 + * (t1744 * t3947 - t207 * t3945 - t3884 * t774 + + t3917 * t54 - t3944 * t48 + t3949)) + + t2647 + + t3 + * (t126 + * (3 * t134 * t1489 * t1954 * t2077 * t59 + - t134 * t3899 + + t145 * t465 * t7 * (t1 * t1939 - t2065 - t2072) + - t22 * t3900 - t22 * t3901 - t3912) + + t146 * t3927 + - t150 + * (3 * t123 * t1571 * t1995 * t2129 * t59 + - t123 * t3919 + + t125 * t465 * t7 * (t1 * t1980 - t2117 - t2124) + - t22 * t3920 - t22 * t3921 - t3912) + - t151 * t3930 + t3808 * t760 - t3810 * t779 + + t3888 * t665 - t3889 * t682) + + t3887 + + t7 + * (-t126 + * (3 * t139 * t1489 * t1954 * t2077 * t59 + - t139 * t3899 + + t145 * t15 + * (t16 * t3928 + t19 * t1940 + t2164 + t2166) + - t16 * t3900 - t2078 * t3935 - t3934) + + t150 + * (3 * t118 * t1571 * t1995 * t2129 * t59 + - t118 * t3919 + + t125 * t15 + * (t16 * t3922 + t19 * t1981 + t2157 + t2159) + - t16 * t3920 - t2130 * t3931 - t3934) + + t152 * t3930 - t153 * t3927 - t3808 * t789 + + t3810 * t787 - t3888 * t703 + t3889 * t700)); + const auto t3951 = t59 * t857; + const auto t3952 = t59 * t861; + const auto t3953 = t158 * t668; + const auto t3954 = t227 * t858; + const auto t3955 = 3 * t390; + const auto t3956 = 3 * t391; + const auto t3957 = + t1958 * t2197 - t2197 + t2199 * t3459 - t3459 + t3956 * t41; + const auto t3958 = t3 * t850 + t3955 * t46 + t3957 + t671 * t7; + const auto t3959 = -t11 * t35 * t850 - 3 * t11 * t45 * t59 * t7 + - t3 * t35 * t3957 - t3 * t35 * t671 * t7 + t849; + const auto t3960 = -t3959; + const auto t3961 = -3 * t13 * t3 * t45 * t59 - t13 * t35 * t671 + - t3 * t35 * t7 * t850 - t35 * t3957 * t7 + t668; + const auto t3962 = -t3961; + const auto t3963 = t1978 + * (t1560 * t3958 + t3456 * t849 + t3953 * t849 + t3954 * t668 + + t3960 * t48 + t3962 * t52); + const auto t3964 = t1996 * t2223; + const auto t3965 = t1993 * t2226; + const auto t3966 = t1500 * t391 + t1958 * t2228 + t2199 * t2238 + t2239; + const auto t3967 = t212 * t3955 + t3 * t808 + t3966 + t655 * t7; + const auto t3968 = t230 - t804 - t805 - t806; + const auto t3969 = + t1660 * t1961 + t207 * t3966 + t227 * t657 + t3968 + t56 * t808; + const auto t3970 = t10 * t1476 * t1811 + t186 * t655 + t207 * t3604 + + t227 * t3966 + t3906; + const auto t3971 = t155 + * (t12 * t35 * t656 * t813 - t1515 * t3967 - t18 * t3969 + - t24 * t3970 + t3 * t35 * t698 * t813 + t35 * t656 * t7 * t810); + const auto t3972 = t3838 * t613; + const auto t3973 = t2244 * t3910; + const auto t3974 = -3 * t1536 * t1966 * t21 * t2244 * t59 + t19 * t3972 + + t19 * t3973 + t2243 * (t2237 + t3477) + t331 * t3971; + const auto t3975 = t1 * t125 * t465 * (-t2214 - t2221 - t3467) + + 3 * t112 * t1571 * t1995 * t2225 * t59 - t112 * t3963 + - t19 * t3964 - t19 * t3965 - t3974; + const auto t3976 = t158 * t639; + const auto t3977 = t227 * t838; + const auto t3978 = + t1958 * t2249 + t2199 * t3485 - t2249 - t3485 + t3956 * t82; + const auto t3979 = t3 * t834 + t3955 * t87 + t3978 + t642 * t7; + const auto t3980 = -t11 * t35 * t834 - 3 * t11 * t59 * t7 * t86 + - t3 * t35 * t3978 - t3 * t35 * t642 * t7 + t839; + const auto t3981 = -t3980; + const auto t3982 = -3 * t13 * t3 * t59 * t86 - t13 * t35 * t642 + - t3 * t35 * t7 * t834 - t35 * t3978 * t7 + t639; + const auto t3983 = -t3982; + const auto t3984 = t1937 + * (t1473 * t3979 + t3483 * t839 + t3976 * t839 + t3977 * t639 + + t3981 * t91 + t3983 * t89); + const auto t3985 = t1955 * t2270; + const auto t3986 = t1952 * t2273; + const auto t3987 = t1 * t145 * t465 * (-t2261 - t2268 - t3492) + + 3 * t143 * t1489 * t1954 * t2272 * t59 - t143 * t3984 + - t19 * t3985 - t19 * t3986 - t3974; + const auto t3988 = -t2261; + const auto t3989 = t15 * t2282; + const auto t3990 = -3 * t1536 * t1966 * t2244 * t24 * t59 + + t1595 * (t22 * t2240 + t2285 + t2287 + t3497 + t3924) + + t22 * t3973 + t2293 * t3925 + t317 * t3971; + const auto t3991 = -t2214; + const auto t3992 = t15 * t2301; + const auto t3993 = t24 - t609 - t610 - t611; + const auto t3994 = -3 * t1536 * t18 * t1966 * t2244 * t59 + + t1595 * (t16 * t2240 + t2307 + t2308 + t3503 + t3993) + + t16 * t3972 + t2244 * t3933 + t287 * t3971; + const auto t3995 = t293 + * (t12 * t35 * t639 * t839 - t2316 * t639 - t2830 * t839 + - t305 * t3979 + t306 * t3982 + t308 * t3980); + const auto t3996 = t35 * t838; + const auto t3997 = t3940 * t841; + const auto t3998 = t155 + * (-t1156 * t809 + t158 * t656 * t813 - t1615 * t3967 - t230 * t3970 + - t234 * t3969 - t2846 * t813); + const auto t3999 = t1156 * t843 + t2031 * t3017 - t24 * t3998 + - t26 * t3970 + t662 * t811; + const auto t4000 = t277 + * (t12 * t35 * t668 * t849 - t2321 * t668 - t281 * t3958 + + t282 * t3961 + t284 * t3959 - t2859 * t849); + const auto t4001 = t207 * t849; + const auto t4002 = t3946 * t853; + const auto t4003 = t1152 * t843 - t18 * t3998 - t26 * t3969 + + t2998 * t662 + t3948 * t815; + const auto t4004 = t35 * t858; + const auto t4005 = t207 * t839; + const auto t4006 = t154 + * (-t2194 + - t222 + * (-t101 + * (t1718 * t4002 - t3883 * t854 + t3962 * t54 + t3999 + - t4000 * t52 - t4004 * t677) + - t102 + * (t1748 * t3997 - t3886 * t842 + t3981 * t95 + - t3995 * t91 + t4003 - t4005 * t648) + + t3867 * t872 - t3868 * t874 + t3936 * t871 + - t3937 * t873 + + t55 + * (t1675 * t3997 - t3873 * t842 + t3983 * t95 + - t3995 * t89 - t3996 * t648 + t3999) + + t96 + * (t1744 * t4002 - t3884 * t854 + t3960 * t54 + - t4000 * t48 - t4001 * t677 + t4003)) + + t2325 - t2327 + t2329 - t2330 - t2331 + t2332 + t2333 - t2334 + + t3 + * (t126 + * (3 * t134 * t1489 * t1954 * t2272 * t59 + - t134 * t3984 + + t145 * t15 + * (t22 * t3988 + t2276 + t2278 + t3495) + - t1955 * t3989 - t22 * t3986 - t3990) + + t146 * t3975 + - t150 + * (3 * t123 * t1571 * t1995 * t2225 * t59 + - t123 * t3963 + + t125 * t15 + * (t22 * t3991 + t2295 + t2297 + t3499) + - t1996 * t3992 - t22 * t3965 - t3990) + - t151 * t3987 + t3808 * t845 - t3810 * t859 + + t3951 * t665 - t3952 * t682) + - t3452 + t3516 - t3517 + t3518 + + t7 + * (-t126 + * (3 * t139 * t1489 * t1954 * t2272 * t59 + - t139 * t3984 + + t145 * t15 * (t16 * t3988 + t2315 + t3505) + - t16 * t3985 - t2273 * t3935 - t3994) + + t150 + * (3 * t118 * t1571 * t1995 * t2225 * t59 + - t118 * t3963 + + t125 * t15 * (t16 * t3991 + t2305 + t3501) + - t16 * t3964 - t2226 * t3931 - t3994) + + t152 * t3987 - t153 * t3975 - t3808 * t867 + + t3810 * t866 - t3951 * t703 + t3952 * t700)); + const auto t4007 = t3 * t3913 + t3 * t3953 + t58 * t674; + const auto t4008 = 2 * t1 * t11 * t280 * t35 + 2 * t11 * t282 * t35 * t7 + + 2 * t284 * t3784 - t4007 - t669 - t670; + const auto t4009 = t675 * t880; + const auto t4010 = t2357 * t2872 + t4009 * t58 + t674 * t881; + const auto t4011 = -t1465 + t266 - t3790; + const auto t4012 = t284 * t4008 * t880 + t4010 + t4011; + const auto t4013 = t668 * t881; + const auto t4014 = t391 * t4009; + const auto t4015 = -3 * t2356 * t282 * t675 * t879 + t4013 * t7 + t4014; + const auto t4016 = t406 * t7; + const auto t4017 = e1_z + t120; + const auto t4018 = t4016 + t4017; + const auto t4019 = -t282 * t4008 * t880 + t4015 + t4018; + const auto t4020 = t2362 * t54 * t713 + t707 * t897; + const auto t4021 = 3 * t280; + const auto t4022 = t389 * t4009; + const auto t4023 = t1 * t4013 - t2357 * t4021 * t675 + t4022; + const auto t4024 = t1 * t406; + const auto t4025 = e1_y + t109; + const auto t4026 = t4024 + t4025; + const auto t4027 = -t4008 * t914 + t4023 + t4026; + const auto t4028 = + t225 * t2821 - t2820 * t304 + t3134 * t639 - t461 * t656; + const auto t4029 = -t2823 * t889 + t4028 * t885; + const auto t4030 = t2828 * t889 + t4028 * t883; + const auto t4031 = -t2377 - t2379 + t2380 + t2382; + const auto t4032 = -t3913 + t668 * t911; + const auto t4033 = t2859 + 2 * t876 + 2 * t877; + const auto t4034 = t284 * t406 + t4033 + t48; + const auto t4035 = t4032 + t4034; + const auto t4036 = t227 * t464; + const auto t4037 = t4009 * t909 + t4036; + const auto t4038 = + -3 * t2356 * t282 * t675 * t913 + t2395 * t668 * t7 + t4037; + const auto t4039 = -t1 * t282 * t4035 * t880 + t4038; + const auto t4040 = t4035 * t880; + const auto t4041 = t2395 * t674 + t2406 * t2872 - t4022; + const auto t4042 = -t4024; + const auto t4043 = t1 + t4042; + const auto t4044 = t1 * t284 * t4040 + t4041 + t4043; + const auto t4045 = t2402 * t707 - t54 * t713 * t919; + const auto t4046 = t158 * t251; + const auto t4047 = -t4046; + const auto t4048 = 3 * t3086; + const auto t4049 = + -t1 * t668 * t880 * t913 + t4009 * t911 + t4047 + t4048 * t675; + const auto t4050 = -t4040 * t669 - t4049; + const auto t4051 = t2409 * t4028 + t2823 * t917; + const auto t4052 = t2413 * t4028 + t2828 * t917; + const auto t4053 = -t3953 + t668 * t923; + const auto t4054 = t4034 + t4053; + const auto t4055 = t4054 * t880; + const auto t4056 = t186 * t251; + const auto t4057 = -t4056; + const auto t4058 = t668 * t926; + const auto t4059 = + t1783 * t2430 * t675 + t4009 * t923 + t4057 - t4058 * t7; + const auto t4060 = t4055 * t670 + t4059; + const auto t4061 = t284 * t7; + const auto t4062 = t2430 * t2872 - t4014 + t674 * t926; + const auto t4063 = -t4016; + const auto t4064 = t4063 + t7; + const auto t4065 = t4055 * t4061 + t4062 + t4064; + const auto t4066 = t707 * t938 + t713 * t939; + const auto t4067 = t7 * t914; + const auto t4068 = t2430 * t4021; + const auto t4069 = t1 * t4058 + t4037 - t4068 * t675; + const auto t4070 = -t4054 * t4067 + t4069; + const auto t4071 = t2823 * t930 + t4028 * t929; + const auto t4072 = t2828 * t930 - t4028 * t932; + const auto t4073 = t2421 - t2422 + t2424 + t2426; + const auto t4074 = t1739 * t2463; + const auto t4075 = t2465 * t700; + const auto t4076 = -t2444; + const auto t4077 = t35 * t3785; + const auto t4078 = t207 * t639; + const auto t4079 = -t157 * t4078 - t174 * t4078 + t2443 - t2445 - t2446 + - t4076 - t4077 * t91 - t474 * t702; + const auto t4080 = 3 * t2450; + const auto t4081 = t4080 * t647; + const auto t4082 = t4081 * t942; + const auto t4083 = t3 * t3890 + t3 * t3976 + t58 * t645; + const auto t4084 = 2 * t1 * t11 * t304 * t35 + 2 * t11 * t306 * t35 * t7 + + 2 * t308 * t3784 - t4083 - t640 - t641; + const auto t4085 = t2471 * t639; + const auto t4086 = t646 * t957; + const auto t4087 = t391 * t4086; + const auto t4088 = -3 * t2472 * t306 * t646 * t956 + t4085 * t7 + t4087; + const auto t4089 = t306 * t4084 * t957 - t4018 - t4088; + const auto t4090 = t389 * t4086; + const auto t4091 = t1 * t4085 - 3 * t2472 * t304 * t646 * t956 + t4090; + const auto t4092 = t95 * t962; + const auto t4093 = -t2876 * t4092 + t3126 * t709; + const auto t4094 = 3 * t2472; + const auto t4095 = t4094 * t646; + const auto t4096 = t2471 * t645 + t2474 * t4095 + t4086 * t58; + const auto t4097 = t4092 * t711 + t709 * t961; + const auto t4098 = t4081 * t975; + const auto t4099 = t1675 * t4098; + const auto t4100 = + t1030 * t639 + t16 * t3886 + t2488 + t2489 + t475 * t639; + const auto t4101 = t1 * (t2492 - t2493 - t4100); + const auto t4102 = t1740 * t4098; + const auto t4103 = t1739 * t2501; + const auto t4104 = t1921 * t2520 + t4103 * t682; + const auto t4105 = -t3890 + t639 * t911; + const auto t4106 = t2830 + 2 * t953 + 2 * t954; + const auto t4107 = t308 * t406 + t4106 + t91; + const auto t4108 = t4105 + t4107; + const auto t4109 = t639 * t968; + const auto t4110 = t4036 + t4086 * t909; + const auto t4111 = -3 * t2472 * t306 * t646 * t967 + t4109 * t7 + t4110; + const auto t4112 = t308 * t4095; + const auto t4113 = -t4090 + t4112 * t967 + t645 * t968; + const auto t4114 = t1 * t4108 * t958 + t4043 + t4113; + const auto t4115 = -t2515 * t711 * t95 + t709 * t978; + const auto t4116 = t640 * t957; + const auto t4117 = t4094 * t967; + const auto t4118 = + -t1 * t4109 + t304 * t4117 * t646 + t4047 + t4086 * t911; + const auto t4119 = t1230 * t711 + t2876 * t978; + const auto t4120 = -t3976 + t639 * t923; + const auto t4121 = t4107 + t4120; + const auto t4122 = t641 * t957; + const auto t4123 = t2528 * t639; + const auto t4124 = t4094 * t992; + const auto t4125 = t2844 * t4124 + t4057 + t4086 * t923 - t4123 * t7; + const auto t4126 = t4121 * t4122 + t4125; + const auto t4127 = t1 * t4123 - 3 * t2472 * t304 * t646 * t992 + t4110; + const auto t4128 = t304 * t4121 * t7 * t957 - t4127; + const auto t4129 = t2876 * t998 + t3178 * t709; + const auto t4130 = t2528 * t645 - t4087 + t4112 * t992; + const auto t4131 = t4064 + t4121 * t7 * t958 + t4130; + const auto t4132 = t2538 * t709 + t711 * t998; + const auto t4133 = t2538 * t2876 - t3178 * t711; + const auto t4134 = -t2542 - t2543 + t2545 - t2547 - t2550; + const auto t4135 = t2556 - t2558 - t467 - t470 + t731; + const auto t4136 = -t4135 - t724 - t725; + const auto t4137 = -t4136 * t562 + t740; + const auto t4138 = -t1071 - t16 + 3 * t21 * t245 * t35 * t69 * t736 + + t35 * t69 * t72 * t729 - t4136 * t607 - t744; + const auto t4139 = -t4136 * t513 + t783; + const auto t4140 = -t4137; + const auto t4141 = -t4139; + const auto t4142 = t2738 * t35; + const auto t4143 = t4025 + t4142; + const auto t4144 = -t1030 * t729 - t1412 - t19 * t56 * t723 + + 2 * t21 * t35 * t4143 - t2570 + t2572 + t2573 - t2574 * t723; + const auto t4145 = t4144 * t72; + const auto t4146 = + t1184 * t227 + t1191 * t317 + t1371 - t24 * t4145 + t2578; + const auto t4147 = 2 * t35 * t4143; + const auto t4148 = + -t1161 * t736 + t1190 * t35 + t1191 * t331 - t21 * t4145 + t4147; + const auto t4149 = + t1184 * t207 + t1191 * t287 + t2563 - t4144 * t98 + t745; + const auto t4150 = -t4146; + const auto t4151 = -t4149; + const auto t4152 = -t1364 - t1365 - t4135; + const auto t4153 = -t1246 * t4152 + t1368; + const auto t4154 = -t1141 - t1270 * t4152 - t1370 + + 3 * t194 * t21 * t245 * t35 * t736 + t194 * t35 * t72 * t729 + - t22; + const auto t4155 = -t1259 * t4152 + t1374; + const auto t4156 = -t4153; + const auto t4157 = -t4155; + const auto t4158 = t2187 * t35; + const auto t4159 = t2593 * t35; + const auto t4160 = t2594 * t35; + const auto t4161 = t1937 + * (t1935 * t2907 + t2076 * t2907 + t2912 * t93 + t2914 + + t451 * t780); + const auto t4162 = 3 * t2739; + const auto t4163 = t1069 * t1948 + t1069 * t3279 + t1076 - 3 * t1942 + + t1945 + t3620 + t4162 * t80; + const auto t4164 = t1603 - t19 * t2069 + 3 * t2149 + t2631 + t4163; + const auto t4165 = t2074 * t2604; + const auto t4166 = t2078 * t2602; + const auto t4167 = t155 + * (-t1516 * t2924 - t21 * t2923 - t2924 * t3295 - t2925 + + t35 * t374 * t729); + const auto t4168 = 3 * t2738; + const auto t4169 = t15 * t4168; + const auto t4170 = d_y * t4169 + t1498 * t174 + t1501 * t174; + const auto t4171 = t1506 + t1510 + 3 * t2144 + t4170 - 4 * t5; + const auto t4172 = t1 * t2607 - t2143 - t4171; + const auto t4173 = t1539 * t22; + const auto t4174 = t2101 * t2609; + const auto t4175 = t2100 * t2610; + const auto t4176 = -3 * t1536 * t2100 * t24 * t2609 * t59 + + t2099 * t4172 + t317 * t4167 + t4173 * t4174 + t4173 * t4175; + const auto t4177 = t1978 + * (t1976 * t2931 + t2128 * t2931 + t2933 * t50 + t2935 + + t429 * t769); + const auto t4178 = t1069 * t1989 + t1069 * t3304 + t1076 - 3 * t1983 + + t1986 + t3597 + t39 * t4162; + const auto t4179 = t1586 - t19 * t2121 + 3 * t2132 + t2624 + t4178; + const auto t4180 = t2126 * t2622; + const auto t4181 = t2130 * t2620; + const auto t4182 = t174 * t2121; + const auto t4183 = 3 * t108 - t1583 * t4168 + t19 * t2003 + t19 * t3631 + + 2 * t2119 + t2704; + const auto t4184 = e1_y - t2910; + const auto t4185 = -3 * d_y * t1 + t1517 + t1519 + t4170; + const auto t4186 = -t10 * t4162 + t1505 * t19 + t1509 * t19 + + t174 * t2679 + 3 * t20 + t2919; + const auto t4187 = t2012 * t2146; + const auto t4188 = t2012 * t2100; + const auto t4189 = -3 * t1536 * t21 * t2100 * t2609 * t59 + + t1595 * (-t174 * t2092 + t174 * t2607 - t19 * t4185 + t4186) + + t2609 * t4187 + t2628 * t4188 + t331 * t4167; + const auto t4190 = 3 * t112 * t1571 * t2129 * t2619 * t35 - t112 * t4177 + + t125 * t15 + * (t1 * t4178 + t12 * t15 * t2615 - t4182 - t4183 - t4184) + - t2130 * t2625 - t2622 * t3923 - t4189; + const auto t4191 = t174 * t2069; + const auto t4192 = 3 * t140 - t1600 * t4168 + t19 * t2018 + t19 * t3626 + + 2 * t2067 + t2664; + const auto t4193 = 3 * t143 * t1489 * t2077 * t2601 * t35 - t143 * t4161 + + t145 * t15 + * (t1 * t4163 + t12 * t15 * t2596 - t4184 - t4191 - t4192) + - t2078 * t2632 - t2604 * t3929 - t4189; + const auto t4194 = t1539 * t16; + const auto t4195 = -3 * t1536 * t18 * t2100 * t2609 * t59 + t287 * t4167 + + t3333 * t4172 + t4174 * t4194 + t4175 * t4194; + const auto t4196 = t2045 * t64; + const auto t4197 = t1040 * t2189; + const auto t4198 = -t1648 * t4162 + 3 * t1649 + t1650 * t2740 + t1652 + + t1653 * t1812 + t3446; + const auto t4199 = 2 * t3888; + const auto t4200 = 2 * t3889; + const auto t4201 = 3 * t4142; + const auto t4202 = t2050 * t79 + t2050 * t85 + t2651 + t4201 * t81; + const auto t4203 = -t1 * t35 * (t3814 + t4202) - 2 * t12 * t35 * t752 + + t222 * t3813 + t222 * t3819 - t2666 - 3 * t2738 * t59 * t86 + + t2908 + t419 + t726 * t81 + t92; + const auto t4204 = 3 * t158 * t86 + 2 * t2909 + t3820 + t4202; + const auto t4205 = std::pow(t750, 2); + const auto t4206 = t186 * t4205 + t4205 * t56; + const auto t4207 = t1937 + * (t1474 * t4204 + t2600 * t4204 + t35 * std::pow(t780, 2) + - t4203 * t93 + t4206); + const auto t4208 = -2 * t3 * t77; + const auto t4209 = t174 * t1948 + t174 * t3279 + t4169 * t80; + const auto t4210 = + 2 * t1 * t2069 + 4 * t1 * t80 - 3 * t2154 - t3826 - t4208 - t4209; + const auto t4211 = std::pow(t2077, 2); + const auto t4212 = t2074 * t2078; + const auto t4213 = std::pow(t723, 2); + const auto t4214 = d_y * t4201 + t1498 * t158 + t1501 * t158; + const auto t4215 = 3 * t10 * t158 + t2681 + 2 * t2922 + t4214; + const auto t4216 = -t1505 * t222 - t1509 * t222 + t1523 * t4168 + - t158 * t2679 + t222 * (t2685 + t4214) + t2686 + t722 * t726; + const auto t4217 = t155 + * (t11 * t35 * t4213 + t13 * t35 * t4213 - t1516 * t4215 + - t21 * t4216 - t3295 * t4215 + t35 * std::pow(t729, 2)); + const auto t4218 = 2 * t2143 + t4171; + const auto t4219 = std::pow(t2100, 2); + const auto t4220 = 2 * t2101; + const auto t4221 = -3 * t1536 * t24 * t4219 * t59 + t2099 * t4218 + + t2100 * t4173 * t4220 + t317 * t4217; + const auto t4222 = t2050 * t38 + t2050 * t44 + t2695 + t40 * t4201; + const auto t4223 = -t1 * t35 * (t3843 + t4222) - 2 * t12 * t35 * t765 + + t222 * t3842 + t222 * t3847 - t2705 - 3 * t2738 * t45 * t59 + + t2932 + t40 * t726 + t419 + t49; + const auto t4224 = t3848 + t4222 + 2 * t766 + 3 * t767; + const auto t4225 = std::pow(t770, 2); + const auto t4226 = t186 * t4225 + t4225 * t56; + const auto t4227 = t1978 + * (t1561 * t4224 + t2618 * t4224 + t35 * std::pow(t769, 2) + - t4223 * t50 + t4226); + const auto t4228 = -2 * t3 * t36; + const auto t4229 = t174 * t1989 + t174 * t3304 + t39 * t4169; + const auto t4230 = + 2 * t1 * t2121 + 4 * t1 * t39 - 3 * t2137 - t3854 - t4228 - t4229; + const auto t4231 = std::pow(t2129, 2); + const auto t4232 = t2126 * t2130; + const auto t4233 = -t3 * t36; + const auto t4234 = t1539 * t2100; + const auto t4235 = -3 * t1536 * t21 * t4219 * t59 + + t1595 * (t1 * t15 * t4185 + 2 * t12 * t15 * t2092 - t4186) + + t1598 * t2146 * t4234 + t331 * t4217; + const auto t4236 = 3 * t112 * t1571 * t4231 * t59 - t112 * t4227 + + t125 * t15 + * (t19 * (3 * t1 * t39 - t3863 - t4229 - t4233) + 2 * t4182 + + t4183 + t420) + - t1598 * t2130 * t2138 - t4235; + const auto t4237 = -t3 * t77; + const auto t4238 = 3 * t143 * t1489 * t4211 * t59 - t143 * t4207 + + t145 * t15 + * (t19 * (3 * t1 * t80 - t3866 - t4209 - t4237) + 2 * t4191 + + t4192 + t420) + - t1598 * t2078 * t2155 - t4235; + const auto t4239 = t2100 * t4194; + const auto t4240 = -3 * t1536 * t18 * t4219 * t59 + t287 * t4217 + + t3333 * t4218 + t4220 * t4239; + const auto t4241 = 2 * t4158; + const auto t4242 = t35 * t793; + const auto t4243 = t35 * t797; + const auto t4244 = -t4204; + const auto t4245 = t293 + * (t304 * t4203 + t307 * t4244 + t35 * std::pow(t753, 2) + t4206 + + t4244 * t448); + const auto t4246 = t1609 * std::pow(t755, 2); + const auto t4247 = -t4215; + const auto t4248 = t155 + * (t186 * t4213 - t225 * t4216 + t2684 * t4247 + + t35 * std::pow(t728, 2) + t380 * t4247 + t4213 * t56); + const auto t4249 = t1618 * std::pow(t736, 2); + const auto t4250 = 2 * t758; + const auto t4251 = + -t2029 * t4215 - t24 * t4248 + t317 * t4249 + t4250 * t734; + const auto t4252 = -t4224; + const auto t4253 = t277 + * (t280 * t4223 + t283 * t4252 + t35 * std::pow(t768, 2) + t4226 + + t425 * t4252); + const auto t4254 = t1626 * std::pow(t773, 2); + const auto t4255 = t770 * t774; + const auto t4256 = + -t18 * t4248 + t287 * t4249 + t2941 * t4250 - t2962 * t4215; + const auto t4257 = t207 * t750; + const auto t4258 = t2326 * t35; + const auto t4259 = -t3280; + const auto t4260 = t56 * t750; + const auto t4261 = t4260 * t839; + const auto t4262 = 3 * t468; + const auto t4263 = 3 * t909; + const auto t4264 = + t2050 * t3002 + t2199 * t3551 - t3002 - t3551 + t4263 * t79; + const auto t4265 = t1 * t834 + t4262 * t87 + t4264 + t7 * t752; + const auto t4266 = -t1 * t35 * t4264 - t1 * t35 * t7 * t752 + - t12 * t35 * t834 - 3 * t12 * t59 * t7 * t86 + t839; + const auto t4267 = -3 * t1 * t13 * t59 * t86 - t1 * t35 * t7 * t834 + - t13 * t35 * t752 - t35 * t4264 * t7 + t750; + const auto t4268 = -t4267; + const auto t4269 = t1937 + * (t2600 * t4265 + t3891 * t839 + t3977 * t750 + t4261 - t4266 * t93 + + t4268 * t89); + const auto t4270 = t2074 * t2273; + const auto t4271 = t1 * t612; + const auto t4272 = -t11 * t35 * t723 * t813; + const auto t4273 = t1498 * t909 + t2050 * t2990 + t2199 * t3289 + t3290; + const auto t4274 = t1 * t808 + t1657 * t228 + t4273 + t7 * t722; + const auto t4275 = t10 * t1000 * t1660 + t158 * t808 + t222 * t4273 + + t227 * t2922 + t3968; + const auto t4276 = t10 * t1476 * t1657 + t186 * t722 + t222 * t3604 + + t227 * t4273 + t3904; + const auto t4277 = t155 + * (t1 * t35 * t729 * t813 - t21 * t4275 - t24 * t4276 + - t3295 * t4274 + t35 * t7 * t723 * t810 - t4272); + const auto t4278 = t2101 * t2244; + const auto t4279 = -3 * t1536 * t2100 * t2244 * t24 * t59 + + t1595 * (t22 * t3291 + t22 * t4271 + t3293 + t3564 + t3932) + + t2293 * t4188 + t317 * t4277 + t4173 * t4278; + const auto t4280 = -t3305; + const auto t4281 = t56 * t770; + const auto t4282 = t4281 * t849; + const auto t4283 = + t2050 * t2978 + t2199 * t3523 - t2978 - t3523 + t38 * t4263; + const auto t4284 = t1 * t850 + t4262 * t46 + t4283 + t7 * t765; + const auto t4285 = -t1 * t35 * t4283 - t1 * t35 * t7 * t765 + - t12 * t35 * t850 - 3 * t12 * t45 * t59 * t7 + t849; + const auto t4286 = -3 * t1 * t13 * t45 * t59 - t1 * t35 * t7 * t850 + - t13 * t35 * t765 - t35 * t4283 * t7 + t770; + const auto t4287 = -t4286; + const auto t4288 = t1978 + * (t2110 * t849 + t2618 * t4284 + t3954 * t770 + t4282 - t4285 * t50 + + t4287 * t52); + const auto t4289 = t2130 * t2223; + const auto t4290 = t19 * t613; + const auto t4291 = -3 * t1536 * t21 * t2100 * t2244 * t59 + + t1595 * (t174 * t612 + t19 * t3291 + t3311 + t3537 + t3993) + + t2244 * t4187 + t331 * t4277 + t4234 * t4290; + const auto t4292 = 3 * t112 * t1571 * t2129 * t2225 * t59 - t112 * t4288 + + t125 * t15 * (t174 * t2218 + t19 * t4280 + t2222 + t3522) + - t19 * t4289 - t2226 * t3923 - t4291; + const auto t4293 = t2126 * t2226; + const auto t4294 = t2078 * t2270; + const auto t4295 = 3 * t143 * t1489 * t2077 * t2272 * t59 - t143 * t4269 + + t145 * t15 * (t174 * t2265 + t19 * t4259 + t2269 + t3550) + - t19 * t4294 - t2273 * t3929 - t4291; + const auto t4296 = -3 * t1536 * t18 * t2100 * t2244 * t59 + t287 * t4277 + + t3333 * (t3331 + t3570 + t4271) + t4194 * t4278 + t4239 * t613; + const auto t4297 = t35 * t871; + const auto t4298 = t35 * t873; + const auto t4299 = t277 + * (-t2177 * t849 - t2321 * t770 + t280 * t4285 + t282 * t4286 + - t425 * t4284 + t4282); + const auto t4300 = t770 * t854; + const auto t4301 = t1626 * t773 * t853; + const auto t4302 = t155 + * (-t225 * t4275 - t230 * t4276 - t2684 * t4274 - t4272 + - t728 * t827 - t734 * t809); + const auto t4303 = t2950 * t815; + const auto t4304 = -t18 * t4302 + t287 * t4303 + t2941 * t843 + - t2962 * t4274 + t2998 * t758; + const auto t4305 = t293 + * (-t2170 * t839 - t2316 * t750 + t304 * t4266 + t306 * t4267 + + t4261 - t4265 * t448); + const auto t4306 = t750 * t842; + const auto t4307 = t1609 * t755; + const auto t4308 = t1748 * t841; + const auto t4309 = t1675 * t841; + const auto t4310 = -t24 * t4302 - t26 * t4276 + t317 * t4303 + + t734 * t843 + t758 * t811; + const auto t4311 = t154 + * (t1040 * t2328 - t174 * t4258 + t2194 * t64 + - t222 + * (-t101 + * (t1718 * t4301 - t227 * t4300 - t4004 * t774 + + t4287 * t54 - t4299 * t52 + t4310) + - t102 + * (-t207 * t4306 + t2719 * t4265 - t4005 * t756 + + t4304 - t4305 * t91 + t4307 * t4308) + + t4242 * t872 - t4243 * t874 + t4297 * t795 + - t4298 * t798 + + t55 + * (-t227 * t4306 - t3996 * t756 + t4268 * t95 + - t4305 * t89 + t4307 * t4309 + t4310) + + t96 + * (t1744 * t4301 - t207 * t4300 + t2716 * t4284 + - t4001 * t774 - t4299 * t48 + t4304)) + + t3 + * (t126 + * (3 * t134 * t1489 * t2077 * t2272 * t59 + - t134 * t4269 + + t145 * t15 + * (t1 * t22 * t2265 + t22 * t4259 + t3285 + + t3562) + - t2078 * t3989 - t22 * t4270 - t4279) + + t146 * t4292 + - t150 + * (3 * t123 * t1571 * t2129 * t2225 * t59 + - t123 * t4288 + + t125 * t15 + * (t1 * t22 * t2218 + t22 * t4280 + t3321 + + t3567) + - t2130 * t3992 - t22 * t4293 - t4279) + - t151 * t4295 + t3888 * t845 - t3889 * t859 + + t3951 * t760 - t3952 * t779) + + t3345 - t3520 + t3582 - t3583 + t3584 + t4258 + + t7 + * (-t126 + * (3 * t139 * t1489 * t2077 * t2272 * t59 + - t139 * t4269 + + t145 * t3 * t465 * (t1 * t2265 - t3280 - t3572) + - t16 * t4270 - t16 * t4294 - t4296) + + t150 + * (3 * t118 * t1571 * t2129 * t2225 * t59 + - t118 * t4288 + + t125 * t3 * t465 * (t1 * t2218 - t3305 - t3569) + - t16 * t4289 - t16 * t4293 - t4296) + + t152 * t4295 - t153 * t4292 - t3888 * t867 + + t3889 * t866 - t3951 * t789 + t3952 * t787)); + const auto t4312 = t186 * t770; + const auto t4313 = -t4312 + t58 * t770; + const auto t4314 = t2177 + 2 * t908 + 2 * t910; + const auto t4315 = t280 * t726 + t4314 + t50; + const auto t4316 = t4313 + t4315; + const auto t4317 = t772 * t880; + const auto t4318 = t770 * t881; + const auto t4319 = t2357 * t772; + const auto t4320 = t1788 * t4319 - t3 * t4318 + t4042 + t4317 * t58; + const auto t4321 = t4316 * t771 * t880 + t4320; + const auto t4322 = t391 * t4317 + t4036; + const auto t4323 = -3 * t2356 * t282 * t772 * t879 + t4318 * t7 + t4322; + const auto t4324 = -t282 * t3 * t4316 * t880 + t4323; + const auto t4325 = t2362 * t54 * t797 + t795 * t897; + const auto t4326 = t3 * t914; + const auto t4327 = t389 * t4317; + const auto t4328 = t4021 * t4319 - t4327 + t768 * t881; + const auto t4329 = t3 + t4047; + const auto t4330 = -t4316 * t4326 - t4328 - t4329; + const auto t4331 = + t225 * t2895 - t2894 * t304 + t358 * t728 - t404 * t753; + const auto t4332 = -t2897 * t889 + t4331 * t885; + const auto t4333 = t2902 * t889 + t4331 * t883; + const auto t4334 = + -t12 * t15 * t2378 + t166 * t2381 + t2377 * t64 + t2378; + const auto t4335 = t2415 * t64; + const auto t4336 = t1 * t4281 + t1 * t4312 + t768 * t911; + const auto t4337 = 2 * t12 * t282 * t35 * t7 + 2 * t12 * t284 * t3 * t35 + + 2 * t280 * t4143 - t4336 - t670 - t771; + const auto t4338 = t2395 * t770; + const auto t4339 = t4317 * t909; + const auto t4340 = -3 * t2356 * t282 * t772 * t913 + t4338 * t7 + t4339; + const auto t4341 = t7 * t726; + const auto t4342 = t4017 + t4341; + const auto t4343 = -t282 * t4337 * t880 + t4340 + t4342; + const auto t4344 = t2395 * t768 + t4048 * t772 + t4317 * t911; + const auto t4345 = -t2667 - 2 * t4142 + t419; + const auto t4346 = -t4337 * t914 - t4344 - t4345; + const auto t4347 = t2409 * t4331 + t2897 * t917; + const auto t4348 = -3 * t2356 * t284 * t772 * t913 + t3 * t4338 + t4327; + const auto t4349 = t3783 + t4046; + const auto t4350 = -t284 * t4337 * t880 + t4348 + t4349; + const auto t4351 = t2413 * t4331 + t2902 * t917; + const auto t4352 = t2402 * t795 - t54 * t797 * t919; + const auto t4353 = -t4281 + t770 * t923; + const auto t4354 = t4315 + t4353; + const auto t4355 = t1 * t801; + const auto t4356 = -t4355; + const auto t4357 = t770 * t926; + const auto t4358 = + t1783 * t2430 * t772 + t4317 * t923 + t4356 - t4357 * t7; + const auto t4359 = t4354 * t670 * t880 + t4358; + const auto t4360 = -3 * t2356 * t284 * t772 * t925 + t3 * t4357 + t4322; + const auto t4361 = -t284 * t4354 * t7 * t880 + t4360; + const auto t4362 = t795 * t938 + t797 * t939; + const auto t4363 = t4068 * t772 - t4339 + t768 * t926; + const auto t4364 = -t4341; + const auto t4365 = t4364 + t7; + const auto t4366 = -t4067 * t4354 - t4363 - t4365; + const auto t4367 = t2897 * t930 + t4331 * t929; + const auto t4368 = t2902 * t930 - t4331 * t932; + const auto t4369 = -t166 * t2423 + t2421 * t64 + t3105; + const auto t4370 = t35 * t780; + const auto t4371 = + t19 * t4370 + t3109 + t3110 + t474 * t750 + t475 * t750; + const auto t4372 = t3107 - t3108 - t4371; + const auto t4373 = t4080 * t755; + const auto t4374 = t4373 * t942; + const auto t4375 = t1740 * t4374; + const auto t4376 = t1748 * t4374; + const auto t4377 = t3 * t976; + const auto t4378 = t2465 * t35; + const auto t4379 = t4074 * t776 + t4378 * t787; + const auto t4380 = t186 * t750; + const auto t4381 = -t4380 + t58 * t750; + const auto t4382 = t2170 + 2 * t964 + 2 * t965; + const auto t4383 = t304 * t726 + t4382 + t93; + const auto t4384 = t4381 + t4383; + const auto t4385 = t2471 * t750; + const auto t4386 = t754 * t957; + const auto t4387 = t391 * t4386 + t4036; + const auto t4388 = -3 * t2472 * t306 * t754 * t956 + t4385 * t7 + t4387; + const auto t4389 = t3 * t306 * t4384 * t957 - t4388; + const auto t4390 = t304 * t754; + const auto t4391 = t389 * t4386; + const auto t4392 = t2471 * t753 + t4094 * t4390 * t956 - t4391; + const auto t4393 = -t2944 * t4092 + t3126 * t798; + const auto t4394 = t751 * t957; + const auto t4395 = t4094 * t754; + const auto t4396 = t2474 * t4395 - t3 * t4385 + t4042 + t4386 * t58; + const auto t4397 = t4092 * t793 + t798 * t961; + const auto t4398 = t166 * t2485 - t174 * t2480 + t2479 * t64 + t2480; + const auto t4399 = -t1030 * t780 - t19 * t4260 - t2574 * t750 + t3152 + - t3153 - t3154 - t4076 - t4147 * t93; + const auto t4400 = t4373 * t975; + const auto t4401 = t1 * t4260 + t1 * t4380 + t753 * t911; + const auto t4402 = 2 * t12 * t3 * t308 * t35 + 2 * t12 * t306 * t35 * t7 + + 2 * t304 * t4143 - t4401 - t641 - t751; + const auto t4403 = t750 * t968; + const auto t4404 = t4386 * t909; + const auto t4405 = -3 * t2472 * t306 * t754 * t967 + t4403 * t7 + t4404; + const auto t4406 = -3 * t2472 * t308 * t754 * t967 + t3 * t4403 + t4391; + const auto t4407 = t308 * t4402 * t957 - t4349 - t4406; + const auto t4408 = -t2515 * t793 * t95 + t798 * t978; + const auto t4409 = t4117 * t4390 + t4386 * t911 + t753 * t968; + const auto t4410 = t1230 * t793 + t2944 * t978; + const auto t4411 = + -t1 * t15 * t2521 * t3 + t174 * t2524 - t2523 * t95 + t3141; + const auto t4412 = -t4260 + t750 * t923; + const auto t4413 = t4383 + t4412; + const auto t4414 = t2528 * t750; + const auto t4415 = t3772 * t4395 + t4356 + t4386 * t923 - t4414 * t7; + const auto t4416 = t4122 * t4413 + t4415; + const auto t4417 = t2528 * t753 + t4124 * t4390 - t4404; + const auto t4418 = t3158 * t4413 * t7 + t4365 + t4417; + const auto t4419 = t2944 * t998 + t3178 * t798; + const auto t4420 = -3 * t2472 * t308 * t754 * t992 + t3 * t4414 + t4387; + const auto t4421 = t308 * t4413 * t7 * t957 - t4420; + const auto t4422 = t2538 * t798 + t793 * t998; + const auto t4423 = t2538 * t2944 - t3178 * t793; + const auto t4424 = + -t1 * t145 * t15 * t2548 * t7 - t12 * t15 * t2546 + t2546 + t3169; + const auto t4425 = t3183 - t3185 - t553 - t554 + t821; + const auto t4426 = -t4425 - t819 - t820; + const auto t4427 = t1265 + t16 + t4426 * t562 - t812 + t817 - t825; + const auto t4428 = -t4427; + const auto t4429 = -t4426 * t607 + t829; + const auto t4430 = -t4426 * t513 + t863; + const auto t4431 = -t4430; + const auto t4432 = -t1208 - t1209 - t4425; + const auto t4433 = t1034 * t4432 - t1205 + t1206 - t1212 - t1307 + t19; + const auto t4434 = -t4433; + const auto t4435 = -t1068 * t4432 + t1214; + const auto t4436 = -t1048 * t4432 + t1217; + const auto t4437 = -t4436; + const auto t4438 = t3389 * t35; + const auto t4439 = t4017 + t4438; + const auto t4440 = -t1242 * t810 - t1410 - t22 * t56 * t813 + + 2 * t24 * t35 * t4439 - t2570 + t3206 + t3207 - t3208 * t813; + const auto t4441 = t4440 * t72; + const auto t4442 = + t1207 + t1383 * t222 + t1387 * t331 - t21 * t4441 + t3198; + const auto t4443 = -t1249 * t815 + t1387 * t317 + t1389 * t35 + - t24 * t4441 + 2 * t35 * t4439; + const auto t4444 = + t1383 * t207 + t1387 * t287 + t3189 - t4440 * t98 + t818; + const auto t4445 = -t4444; + const auto t4446 = -t4443; + const auto t4447 = 2 * t3951; + const auto t4448 = 2 * t3952; + const auto t4449 = 3 * t4438; + const auto t4450 = t2199 * t38 + t2199 * t41 + t3351 + t43 * t4449; + const auto t4451 = -2 * t13 * t35 * t850 + t227 * t3841 + t227 * t3847 + - t3360 - 3 * t3389 * t45 * t59 - t35 * t7 * (t3843 + t4450) + t3587 + + t43 * t801 + t51 + t573; + const auto t4452 = -t4451; + const auto t4453 = 3 * t186 * t45 + 2 * t2984 + t3848 + t4450; + const auto t4454 = std::pow(t849, 2); + const auto t4455 = t158 * t4454 + t4454 * t56; + const auto t4456 = t1978 + * (t1560 * t4453 + t2618 * t4453 + t35 * std::pow(t858, 2) + + t4452 * t52 + t4455); + const auto t4457 = t157 * t1988 + t157 * t3304 + t3391 * t42; + const auto t4458 = + 2 * t2218 * t7 - 3 * t2300 - t3853 + 4 * t42 * t7 - t4228 - t4457; + const auto t4459 = std::pow(t2225, 2); + const auto t4460 = t2223 * t2226; + const auto t4461 = std::pow(t813, 2); + const auto t4462 = d_z * t4449 + t1498 * t186 + t1500 * t186; + const auto t4463 = 3 * t10 * t186 + t3383 + 2 * t3604 + t4462; + const auto t4464 = -t1505 * t227 - t1507 * t227 + t1523 * t3390 + - t186 * t3381 + t227 * (t3385 + t4462) + t3386 + t801 * t808; + const auto t4465 = t155 + * (t11 * t35 * t4461 + t12 * t35 * t4461 - t1515 * t4463 + - t24 * t4464 - t3295 * t4463 + t35 * std::pow(t810, 2)); + const auto t4466 = 2 * t2290 + t3393; + const auto t4467 = std::pow(t2244, 2); + const auto t4468 = t1539 * t2244; + const auto t4469 = -3 * t1536 * t21 * t4467 * t59 + t2243 * t4466 + + t331 * t4465 + 2 * t4290 * t4468; + const auto t4470 = t1 * t125 * t4458 * t465 + + 3 * t112 * t1571 * t4459 * t59 - t112 * t4456 - t1578 * t4460 + - t4469; + const auto t4471 = t2199 * t79 + t2199 * t82 + t3401 + t4449 * t84; + const auto t4472 = -2 * t13 * t35 * t834 + t227 * t3812 + t227 * t3819 + - 3 * t3389 * t59 * t86 - t3407 - t35 * t7 * (t3814 + t4471) + t3614 + + t573 + t801 * t84 + t88; + const auto t4473 = -t4472; + const auto t4474 = t3820 + t4471 + 2 * t835 + 3 * t836; + const auto t4475 = std::pow(t839, 2); + const auto t4476 = t158 * t4475 + t4475 * t56; + const auto t4477 = t1937 + * (t1473 * t4474 + t2600 * t4474 + t35 * std::pow(t838, 2) + + t4473 * t89 + t4476); + const auto t4478 = t157 * t1947 + t157 * t3279 + t3391 * t83; + const auto t4479 = + 2 * t2265 * t7 - 3 * t2281 - t3825 - t4208 - t4478 + 4 * t7 * t83; + const auto t4480 = std::pow(t2272, 2); + const auto t4481 = t2270 * t2273; + const auto t4482 = t1 * t145 * t4479 * t465 + + 3 * t143 * t1489 * t4480 * t59 - t143 * t4477 - t1578 * t4481 + - t4469; + const auto t4483 = -3 * t1536 * t24 * t4467 * t59 + + t1595 * (2 * t13 * t15 * t612 - t3425) + t1598 * t2293 * t4468 + + t317 * t4465; + const auto t4484 = -3 * t1536 * t18 * t4467 * t59 + + 2 * t2244 * t4194 * t613 + t287 * t4465 + t3333 * t4466; + const auto t4485 = 2 * t3586; + const auto t4486 = -t4453; + const auto t4487 = t277 + * (t281 * t4486 + t282 * t4451 + t35 * std::pow(t851, 2) + + t425 * t4486 + t4455); + const auto t4488 = t1626 * std::pow(t853, 2); + const auto t4489 = 2 * t854; + const auto t4490 = -t4463; + const auto t4491 = t155 + * (t158 * t4461 + t1615 * t4490 - t230 * t4464 + t2684 * t4490 + + t35 * std::pow(t809, 2) + t4461 * t56); + const auto t4492 = t1618 * std::pow(t815, 2); + const auto t4493 = 2 * t843; + const auto t4494 = + -t18 * t4491 + t287 * t4492 - t2962 * t4463 + t2998 * t4493; + const auto t4495 = -t4474; + const auto t4496 = t293 + * (t305 * t4495 + t306 * t4472 + t35 * std::pow(t837, 2) + t4476 + + t448 * t4495); + const auto t4497 = t1609 * std::pow(t841, 2); + const auto t4498 = 2 * t842; + const auto t4499 = + -t24 * t4491 - t26 * t4464 + t317 * t4492 + t4493 * t811; + const auto t4500 = t158 * t849; + const auto t4501 = -t4500 + t58 * t849; + const auto t4502 = t2321 + 2 * t921 + 2 * t922; + const auto t4503 = t282 * t801 + t4502 + t52; + const auto t4504 = t4501 + t4503; + const auto t4505 = t4504 * t880; + const auto t4506 = t852 * t880; + const auto t4507 = t849 * t881; + const auto t4508 = t2357 * t852; + const auto t4509 = t1788 * t4508 - t3 * t4507 + t4063 + t4506 * t58; + const auto t4510 = t4505 * t771 + t4509; + const auto t4511 = t282 * t3; + const auto t4512 = t391 * t4506; + const auto t4513 = t1783 * t4508 - t4512 + t851 * t881; + const auto t4514 = t3 + t4057; + const auto t4515 = t4505 * t4511 + t4513 + t4514; + const auto t4516 = t2362 * t54 * t873 + t872 * t897; + const auto t4517 = t389 * t4506 + t4036; + const auto t4518 = t1 * t4507 - t4021 * t4508 + t4517; + const auto t4519 = -t4326 * t4504 + t4518; + const auto t4520 = + t225 * t2967 - t2970 * t304 + t3134 * t839 - t461 * t813; + const auto t4521 = -t2973 * t889 + t4520 * t885; + const auto t4522 = t2971 * t889 + t4520 * t883; + const auto t4523 = -t2381 - t3674 + t3675 + t3676; + const auto t4524 = t56 * t849; + const auto t4525 = -t4524 + t849 * t911; + const auto t4526 = t4503 + t4525; + const auto t4527 = t4526 * t880; + const auto t4528 = t4506 * t909; + const auto t4529 = t1783 * t2406 * t852 + t2395 * t851 - t4528; + const auto t4530 = t1 + t4356; + const auto t4531 = t1 * t282 * t4527 + t4529 + t4530; + const auto t4532 = + -3 * t2356 * t284 * t852 * t913 + t2395 * t3 * t849 + t4517; + const auto t4533 = -t1 * t284 * t4526 * t880 + t4532; + const auto t4534 = t2402 * t872 - t54 * t873 * t919; + const auto t4535 = + -t1 * t849 * t880 * t913 + t4048 * t852 + t4364 + t4506 * t911; + const auto t4536 = -t4527 * t669 - t4535; + const auto t4537 = t2409 * t4520 + t2973 * t917; + const auto t4538 = t2413 * t4520 + t2971 * t917; + const auto t4539 = t2419 - t3693 - t3694 + t3695; + const auto t4540 = t4500 * t7 + t4524 * t7 + t851 * t923; + const auto t4541 = 2 * t1 * t13 * t280 * t35 + 2 * t13 * t284 * t3 * t35 + + 2 * t282 * t4439 - t4540 - t669 - t771; + const auto t4542 = t849 * t926; + const auto t4543 = t1 * t4542 - t4068 * t852 + t4528; + const auto t4544 = t4025 + t4355; + const auto t4545 = -t4541 * t914 + t4543 + t4544; + const auto t4546 = -3 * t2356 * t284 * t852 * t925 + t3 * t4542 + t4512; + const auto t4547 = t3783 + t4056; + const auto t4548 = -t284 * t4541 * t880 + t4546 + t4547; + const auto t4549 = t2971 * t930 - t4520 * t932; + const auto t4550 = t1783 * t2430 * t852 + t4506 * t923 + t851 * t926; + const auto t4551 = -t3362 - 2 * t4438 + t573; + const auto t4552 = t282 * t4541 * t880 + t4550 + t4551; + const auto t4553 = t2973 * t930 + t4520 * t929; + const auto t4554 = t872 * t938 + t873 * t939; + const auto t4555 = t4080 * t942; + const auto t4556 = t1740 * t841; + const auto t4557 = t4555 * t4556; + const auto t4558 = t474 * t839 + t558 * t839; + const auto t4559 = t22 * t3996 + t3718 + t3719; + const auto t4560 = -t3716 + t3717 + t4559; + const auto t4561 = -t4558 - t4560; + const auto t4562 = t4308 * t4555; + const auto t4563 = t4074 * t857 + t4378 * t866; + const auto t4564 = t158 * t839; + const auto t4565 = -t4564 + t58 * t839; + const auto t4566 = t2316 + 2 * t989 + 2 * t990; + const auto t4567 = t306 * t801 + t4566 + t89; + const auto t4568 = t4565 + t4567; + const auto t4569 = t4094 * t840; + const auto t4570 = t840 * t957; + const auto t4571 = t391 * t4570; + const auto t4572 = t2471 * t837 + t306 * t4569 * t956 - t4571; + const auto t4573 = t3 * t4568 * t993 + t4514 + t4572; + const auto t4574 = t2471 * t839; + const auto t4575 = t389 * t4570 + t4036; + const auto t4576 = t1 * t4574 - 3 * t2472 * t304 * t840 * t956 + t4575; + const auto t4577 = -t3011 * t4092 + t3126 * t874; + const auto t4578 = t2474 * t4569 - t3 * t4574 + t4063 + t4570 * t58; + const auto t4579 = t4092 * t871 + t874 * t961; + const auto t4580 = t56 * t839; + const auto t4581 = t1030 * t839 + t15 * t4580; + const auto t4582 = t1 * (-t4560 - t4581); + const auto t4583 = t4080 * t975; + const auto t4584 = t4309 * t4583; + const auto t4585 = t4556 * t4583; + const auto t4586 = t2195 * t2520 + t4103 * t859; + const auto t4587 = -t4580 + t839 * t911; + const auto t4588 = t4567 + t4587; + const auto t4589 = t4570 * t909; + const auto t4590 = t306 * t4117 * t840 - t4589 + t837 * t968; + const auto t4591 = t839 * t968; + const auto t4592 = -3 * t2472 * t308 * t840 * t967 + t3 * t4591 + t4575; + const auto t4593 = t1 * t308 * t4588 * t957 - t4592; + const auto t4594 = -t2515 * t871 * t95 + t874 * t978; + const auto t4595 = + -t1 * t4591 + t304 * t4117 * t840 + t4364 + t4570 * t911; + const auto t4596 = t1230 * t871 + t3011 * t978; + const auto t4597 = t4564 * t7 + t4580 * t7 + t837 * t923; + const auto t4598 = 2 * t1 * t13 * t304 * t35 + 2 * t13 * t3 * t308 * t35 + + 2 * t306 * t4439 - t4597 - t640 - t751; + const auto t4599 = t2528 * t837 + t3772 * t4569 + t4570 * t923; + const auto t4600 = t4551 + t4598 * t993 + t4599; + const auto t4601 = t2528 * t839; + const auto t4602 = t1 * t4601 - 3 * t2472 * t304 * t840 * t992 + t4589; + const auto t4603 = t304 * t4598 * t957 - t4544 - t4602; + const auto t4604 = t3011 * t998 + t3178 * t874; + const auto t4605 = -3 * t2472 * t308 * t840 * t992 + t3 * t4601 + t4571; + const auto t4606 = t308 * t4598 * t957 - t4547 - t4605; + const auto t4607 = t2538 * t874 + t871 * t998; + const auto t4608 = t2538 * t3011 - t3178 * t871; + const auto t4609 = t1043 * t280 + t1255 * t282 + t2351; + const auto t4610 = t2359 + t284 * t4609 * t880; + const auto t4611 = t1255 + t2366; + const auto t4612 = t2365 - t282 * t4609 * t880 + t4611; + const auto t4613 = t1043 + t2370; + const auto t4614 = t2369 - t4609 * t914 + t4613; + const auto t4615 = -t1734 * t366 - t3030; + const auto t4616 = t280 * t366 + t3051; + const auto t4617 = t3050 + t4616; + const auto t4618 = t3056 * t389 + t491; + const auto t4619 = 3 * t2356 * t280 * t430 * t879 + + t280 * t3 * t35 * t4617 * t880 - t429 * t881 - t4618; + const auto t4620 = t282 * t532 + t3650; + const auto t4621 = t3648 + t4620; + const auto t4622 = t3061 * t4621 + t3656; + const auto t4623 = t3662 + t534; + const auto t4624 = -t282 * t3 * t35 * t4621 * t880 + t3661 + t4623; + const auto t4625 = -t3665 * t4621 + t3669; + const auto t4626 = t251 * t58; + const auto t4627 = + t1 * t280 * t684 - t251 * t878 + t282 * t684 * t7 - t4007; + const auto t4628 = t284 * t4627 * t880 + t4010 + t4626; + const auto t4629 = -t282 * t4627 * t880 + t4015 + t685; + const auto t4630 = t4023 - t4627 * t914 + t688; + const auto t4631 = -t727; + const auto t4632 = -t280 * t4631 + t4314; + const auto t4633 = t4313 + t4632; + const auto t4634 = t4320 + t4633 * t771 * t880; + const auto t4635 = -t282 * t3 * t4633 * t880 + t4323; + const auto t4636 = t3 * t4631; + const auto t4637 = -t4326 * t4633 - t4328 - t4636; + const auto t4638 = -t802; + const auto t4639 = -t282 * t4638 + t4502; + const auto t4640 = t4501 + t4639; + const auto t4641 = t4640 * t880; + const auto t4642 = t4509 + t4641 * t771; + const auto t4643 = t3 * t4638; + const auto t4644 = t4511 * t4641 + t4513 + t4643; + const auto t4645 = -t4326 * t4640 + t4518; + const auto t4646 = t391 * t879; + const auto t4647 = std::pow(t879, 2); + const auto t4648 = -3 * t282 * t4647 * t880 + t282 * t63 + 2 * t4646; + const auto t4649 = 2 * t58; + const auto t4650 = t1788 * t4647 * t880 - t284 * t63 + t4649 * t879; + const auto t4651 = t389 * t879; + const auto t4652 = 3 * t914; + const auto t4653 = t280 * t63 - t4647 * t4652 + 2 * t4651; + const auto t4654 = t154 * t431; + const auto t4655 = -t160 - t186; + const auto t4656 = 3 * t913; + const auto t4657 = + t1 * t3 * t35 * t913 - t4655 * t876 - t4656 * t888 - t879 * t911; + const auto t4658 = t3 * t431; + const auto t4659 = -t4651 + t4655 * t908 + t4656 * t882 + t58 * t913; + const auto t4660 = t431 * t7; + const auto t4661 = t66 * t899; + const auto t4662 = t154 + * (t1 + * (t101 * t277 + * (-t162 * t52 - t166 * t893 - t4661 + + 3 * t52 * t892 * t893 * t899) + + t431 * t4659 * t96) + - t4658 + * (t2364 + * (t1 * t35 * t7 * t879 - t282 * t389 * t4655 + + t3 * t35 * t7 * t913 - t4656 * t884) + - t405 * t4657) + - t4660 * (t2364 * t4659 + t411 * t4657)); + const auto t4663 = t391 * t925; + const auto t4664 = 3 * t925; + const auto t4665 = t4655 * t877 - t4663 + t4664 * t884 + t879 * t923; + const auto t4666 = -t4646 + t4655 * t921 + t4664 * t882 + t58 * t925; + const auto t4667 = t389 * t925; + const auto t4668 = + -t280 * t391 * t4655 - t4664 * t888 + t4667 + t879 * t909; + const auto t4669 = t4654 + * (t1 * (-t101 * t4665 + t4666 * t96) + - t3 * (-t2364 * t4665 - t405 * t4668) + - t7 * (t2364 * t4666 + t411 * t4668)); + const auto t4670 = t2362 * t54; + const auto t4671 = t154 + * (-t1 * (-t4092 * t897 + t4670 * t961) + + t125 * t145 * t3 + * (-t3045 * t946 + t945 * (-t112 * t3044 + t64)) + - t7 * (t361 * t883 * t95 * t951 - t896 * t961)); + const auto t4672 = t404 * t890; + const auto t4673 = t3134 * t361; + const auto t4674 = t154 + * (t4672 * (t885 * t970 + t889 * t972) + + t4673 * (t883 * t972 + t885 * t973) + + t7 * (t361 * t404 * t883 * t970 - t896 * t978)); + const auto t4675 = -t154 + * (t1 * (-t2538 * t4670 + t361 * t404 * t883 * t995) + + t1664 * (t896 * t987 + t897 * t997) + + t2800 * (t2362 * t3178 + t895 * t998)); + const auto t4676 = t210 * t284 + t2392; + const auto t4677 = t2388 + t4676; + const auto t4678 = -t1 * t282 * t35 * t4677 * t880 + t2399; + const auto t4679 = -t1 * t284 * t35 * t4677 * t880 + t2412 + t4613; + const auto t4680 = -t222 * t4677 * t914 - t2407; + const auto t4681 = t54 * t919; + const auto t4682 = t1310 * t282 + t284 * t491 + t3084; + const auto t4683 = t2395 * t424; + const auto t4684 = t1310 + t3056 * t909; + const auto t4685 = -t227 * t4683 + 3 * t2356 * t282 * t430 * t913 + + t282 * t4682 * t880 - t4684; + const auto t4686 = -t207 * t4683 + 3 * t2356 * t284 * t430 * t913 + + t284 * t4682 * t880 - t4618; + const auto t4687 = t3087 + t4682 * t914; + const auto t4688 = t3679 + t4620; + const auto t4689 = -t1 * t284 * t35 * t4688 * t880 + t3681; + const auto t4690 = t1125 + t3685; + const auto t4691 = -t1 * t282 * t35 * t4688 * t880 + t3684 + t4690; + const auto t4692 = -t222 * t4688 * t914 - t3689; + const auto t4693 = -t684; + const auto t4694 = -t284 * t4693 + t4033; + const auto t4695 = t4032 + t4694; + const auto t4696 = -t1 * t282 * t4695 * t880 + t4038; + const auto t4697 = t1 * t4693; + const auto t4698 = t4695 * t880; + const auto t4699 = t1 * t284 * t4698 + t4041 + t4697; + const auto t4700 = -t4049 - t4698 * t669; + const auto t4701 = t514 * t911; + const auto t4702 = + t282 * t7 * t727 + t284 * t3 * t727 - t4336 - t514 * t912; + const auto t4703 = t4344 + t4701 + t4702 * t914; + const auto t4704 = -t284 * t4702 * t880 + t4348 + t742; + const auto t4705 = t1183 - t282 * t4702 * t880 + t4340; + const auto t4706 = t1 * t4638; + const auto t4707 = t4525 + t4639; + const auto t4708 = t4707 * t880; + const auto t4709 = t1 * t282 * t4708 + t4529 + t4706; + const auto t4710 = -t1 * t284 * t4707 * t880 + t4532; + const auto t4711 = -t4535 - t4708 * t669; + const auto t4712 = std::pow(t913, 2); + const auto t4713 = + t1002 * t282 - 3 * t282 * t4712 * t880 + 2 * t909 * t913; + const auto t4714 = + t1002 * t284 - 3 * t284 * t4712 * t880 + 2 * t389 * t913; + const auto t4715 = 2 * t911; + const auto t4716 = t1002 * t280 - t4652 * t4712 - t4715 * t913; + const auto t4717 = + t1 * t35 * t7 * t913 - t4655 * t922 - t4664 * t915 - t911 * t925; + const auto t4718 = t909 * t925; + const auto t4719 = t4655 * t910 + t4656 * t927 - t4718 + t913 * t923; + const auto t4720 = -t154 + * (t1 + * (t101 * t431 * t4719 + + t277 * t96 + * (-t1015 * t48 - t4661 + 3 * t48 * t892 * t899 * t935 + - t64 * t935)) + + t4658 * (-t2364 * t4719 - t405 * t4717) + + t4660 + * (-t2364 + * (-t284 * t4655 * t909 + t391 * t913 - t4656 * t931 + + t4667) + + t411 * t4717)); + const auto t4721 = -t154 + * (t1849 * (t4092 * t906 + t919 * t961) + + t2804 * (t4681 * t951 + t918 * t962) + + t7 * (-t2402 * t3126 + t361 * t404 * t917 * t960)); + const auto t4722 = t404 * t933; + const auto t4723 = t2515 * t95; + const auto t4724 = t154 + * (t3 * (t1230 * t4681 - t4723 * t918) + - t4673 * (-t2409 * t973 + t2413 * t972) + - t4722 * (t2413 * t970 - t917 * t973)); + const auto t4725 = t154 + * (t3 * (-t3178 * t4681 + t361 * t404 * t917 * t995) + + t4673 * (t2409 * t3164 + t2413 * t995) + + t4722 * (t2413 * (-t3158 * t992 + t909) + t3164 * t917)); + const auto t4726 = t2427 + t4676; + const auto t4727 = t2433 * t4726 + t2436; + const auto t4728 = t2439 - t284 * t35 * t4726 * t7 * t880 + t4611; + const auto t4729 = -t227 * t4726 * t914 + t2431; + const auto t4730 = t3091 + t4616; + const auto t4731 = t2433 * t4730 + t3095; + const auto t4732 = 3 * t2356 * t280 * t430 * t925 + + t280 * t35 * t4730 * t7 * t880 - t429 * t926 - t4684; + const auto t4733 = t284 * t35 * t4730 * t7 * t880 - t3097; + const auto t4734 = t1125 * t280 + t284 * t534 + t3698; + const auto t4735 = t282 * t4734 * t880 + t3703; + const auto t4736 = -t284 * t4734 * t880 + t3709 + t4623; + const auto t4737 = t3706 + t4690 - t4734 * t914; + const auto t4738 = t4053 + t4694; + const auto t4739 = t4738 * t880; + const auto t4740 = t4059 + t4739 * t670; + const auto t4741 = t4693 * t7; + const auto t4742 = t4061 * t4739 + t4062 + t4741; + const auto t4743 = -t4067 * t4738 + t4069; + const auto t4744 = t4353 + t4632; + const auto t4745 = t4358 + t4744 * t670 * t880; + const auto t4746 = -t284 * t4744 * t7 * t880 + t4360; + const auto t4747 = t4631 * t7; + const auto t4748 = -t4067 * t4744 - t4363 - t4747; + const auto t4749 = t482 * t923; + const auto t4750 = + t1 * t280 * t802 + t284 * t3 * t802 - t4540 - t482 * t924; + const auto t4751 = t282 * t4750 * t880 + t4550 + t4749; + const auto t4752 = -t284 * t4750 * t880 + t4546 + t803; + const auto t4753 = t1203 + t4543 - t4750 * t914; + const auto t4754 = 2 * t923; + const auto t4755 = std::pow(t925, 2); + const auto t4756 = -t1234 * t282 + t1783 * t4755 * t880 + t4754 * t925; + const auto t4757 = t1234 * t284 - 3 * t284 * t4755 * t880 + 2 * t4663; + const auto t4758 = t1234 * t280 - t4652 * t4755 + 2 * t4718; + const auto t4759 = -t2471 * t304 + t389; + const auto t4760 = t154 + * (t1 * (t361 * t404 * t929 * t960 - t4092 * t938) + + t4672 * (t3132 * t930 + t4759 * t929) + + t4722 * (t4759 * t932 + t930 * t960)); + const auto t4761 = -t154 + * (t1710 * (t1230 * t937 + t3100 * t978) + + t1888 * (t2515 * t938 + t939 * t977) + + t3 * (-t3101 * t4723 + t361 * t404 * t929 * t970)); + const auto t4762 = t900 * t935; + const auto t4763 = t154 + * (-t1 * (t361 * t929 * t95 * t987 - t938 * t998) + + t125 * t145 * t7 + * (t984 * (-t118 * t4762 + t66) + - t985 * (-t112 * t4762 + t166)) + - t3 * (t3101 * t998 - t3178 * t939)); + const auto t4764 = t1043 * t304 + t1255 * t306 + t2469; + const auto t4765 = t1255 + t2470 * t391; + const auto t4766 = -t1613 * t2471 + 3 * t2472 * t306 * t314 * t956 + + t306 * t4764 * t957 - t4765; + const auto t4767 = t1043 + t2470 * t389; + const auto t4768 = -t4764; + const auto t4769 = t304 * t366 + t3118; + const auto t4770 = t3117 + t4769; + const auto t4771 = -t3 * t306 * t35 * t4770 * t957 + t3124; + const auto t4772 = t3149 + t491; + const auto t4773 = -t2471 * t451 + 3 * t2472 * t304 * t452 * t956 + + t3 * t304 * t35 * t4770 * t957 - t4772; + const auto t4774 = t3129 + t3739 * t4770; + const auto t4775 = t1675 * t532 + t3720; + const auto t4776 = -t3715 - t4775; + const auto t4777 = t306 * t532 + t3737; + const auto t4778 = t3736 + t4777; + const auto t4779 = t3740 * t391 + t534; + const auto t4780 = -t2471 * t596 + 3 * t2472 * t306 * t599 * t956 + + t3 * t306 * t35 * t4778 * t957 - t4779; + const auto t4781 = t251 * t955 - t304 * t688 - t306 * t685 + t4083; + const auto t4782 = -t4781; + const auto t4783 = t306 * t4782 * t957 - t4088 - t685; + const auto t4784 = t647 * t948; + const auto t4785 = t755 * t948; + const auto t4786 = t1740 * t727 + t4371; + const auto t4787 = -t304 * t4631 + t4382; + const auto t4788 = t4381 + t4787; + const auto t4789 = t3 * t306 * t4788 * t957 - t4388; + const auto t4790 = t841 * t948; + const auto t4791 = t1675 * t802 + t4559; + const auto t4792 = t4558 + t4791; + const auto t4793 = -t306 * t4638 + t4566; + const auto t4794 = t4565 + t4793; + const auto t4795 = t3 * t4794 * t993 + t4572 + t4643; + const auto t4796 = t391 * t956; + const auto t4797 = std::pow(t956, 2); + const auto t4798 = 3 * t306 * t4797 * t957 - t306 * t63 - 2 * t4796; + const auto t4799 = t389 * t956; + const auto t4800 = 3 * t304 * t4797 * t957 - t304 * t63 - 2 * t4799; + const auto t4801 = -t308 * t63 + t4649 * t956 + 3 * t4797 * t958; + const auto t4802 = t154 * t453; + const auto t4803 = t389 * t967; + const auto t4804 = 3 * t956; + const auto t4805 = 3 * t959; + const auto t4806 = t4655 * t964 - t4799 + t4805 * t967 + t58 * t967; + const auto t4807 = t66 * t975; + const auto t4808 = t166 * t942; + const auto t4809 = t4807 + t4808; + const auto t4810 = t154 + * (-t1 + * (t102 * t453 * t4806 + + t293 * t55 + * (-t162 * t89 - t4809 + + 3 * t89 * t942 * t948 * t975)) + + t3 + * (t126 * t1448 + * (-t134 * t162 + 3 * t134 * t942 * t943 * t975 + - t4809) + + t151 * t293 + * (t161 * t941 + t164 * t942 + t64 * t975 + - 3 * t950 * t975)) + - t453 * t7 + * (-t462 * t4806 + + t55 + * (t4655 * t953 - t4803 + t4804 * t969 + + t911 * t956))); + const auto t4811 = t391 * t992; + const auto t4812 = t4655 * t954 + t4804 * t994 - t4811 + t923 * t956; + const auto t4813 = t64 * t982; + const auto t4814 = t4808 + t4813; + const auto t4815 = t102 * t293; + const auto t4816 = t154 + * (-t1 * t453 + * (t102 * (t4655 * t989 - t4796 + t4805 * t992 + t58 * t992) + - t4812 * t55) + - t3 + * (t453 * t462 * t4812 + + t4815 + * (-t202 * t93 - t4814 + + 3 * t93 * t942 * t948 * t982)) + + t7 + * (t126 * t293 + * (t187 * t981 - 3 * t2462 * t982 + t57 * t982 + + t66 * t942) + + t1448 * t152 + * (-t143 * t202 + 3 * t143 * t942 * t943 * t982 + - t4814))); + const auto t4817 = t1 * (-t1748 * t210 - t2491); + const auto t4818 = t210 * t308 + t2509; + const auto t4819 = t2507 + t4818; + const auto t4820 = t1 * t308 * t35 * t4819 * t957 + + 3 * t2472 * t308 * t314 * t967 - t313 * t968 - t4767; + const auto t4821 = t1310 * t306 + t308 * t491 + t3144; + const auto t4822 = t1310 + t3142; + const auto t4823 = -t306 * t4821 * t957 + t3148 + t4822; + const auto t4824 = -t308 * t4821 * t957 + t3150 + t4772; + const auto t4825 = t3158 * t4821 + t3159; + const auto t4826 = t3750 + t4777; + const auto t4827 = t1 * t308 * t35 * t4826 * t957 - t3753; + const auto t4828 = t1125 + t3740 * t909; + const auto t4829 = t1 * (-t3756 - t4775); + const auto t4830 = t1 * (t1748 * t684 + t4100); + const auto t4831 = t222 * t2498; + const auto t4832 = -t308 * t4693 + t4106; + const auto t4833 = t4105 + t4832; + const auto t4834 = t1 * t4833 * t958 + t4113 + t4697; + const auto t4835 = -t1183 * t306 - t308 * t742 + t4401 + t514 * t966; + const auto t4836 = -t4835; + const auto t4837 = t308 * t4836 * t957 - t4406 - t742; + const auto t4838 = t1 * (t4581 + t4791); + const auto t4839 = t4587 + t4793; + const auto t4840 = t1 * t308 * t4839 * t957 - t4592; + const auto t4841 = t909 * t967; + const auto t4842 = std::pow(t967, 2); + const auto t4843 = -t1002 * t306 + 3 * t306 * t4842 * t957 - 2 * t4841; + const auto t4844 = -t1002 * t308 + 3 * t308 * t4842 * t957 - 2 * t4803; + const auto t4845 = -t1002 * t304 + 3 * t3158 * t4842 + t4715 * t967; + const auto t4846 = t909 * t992; + const auto t4847 = 3 * t992; + const auto t4848 = t4655 * t965 - t4846 + t4847 * t971 + t923 * t967; + const auto t4849 = t4655 * t990 - t4841 + t4847 * t969 + t911 * t992; + const auto t4850 = + -t1015 * t91 - t4807 - t4813 + 3 * t91 * t948 * t975 * t982; + const auto t4851 = t453 * t55; + const auto t4852 = t154 + * (t1 * (t4815 * t4850 + t4848 * t4851) + - t3 * t453 * (-t102 * t4849 + t462 * t4848) + - t7 * (t293 * t462 * t4850 + t4849 * t4851)); + const auto t4853 = t2526 + t4818; + const auto t4854 = t227 * t4853 * t993 + t2529; + const auto t4855 = -t2532 + t304 * t35 * t4853 * t7 * t957; + const auto t4856 = 3 * t2472 * t308 * t314 * t992 - t2528 * t313 + + t308 * t35 * t4853 * t7 * t957 - t4765; + const auto t4857 = t3170 + t4769; + const auto t4858 = -t308 * t35 * t4857 * t7 * t957 + t3173; + const auto t4859 = t227 * t4857 * t993 + t3175; + const auto t4860 = 3 * t2472 * t304 * t452 * t992 - t2528 * t451 + + t304 * t35 * t4857 * t7 * t957 - t4822; + const auto t4861 = t1125 * t304 + t308 * t534 + t3771; + const auto t4862 = -t1899 * t2528 + 3 * t2472 * t304 * t599 * t992 + + t304 * t4861 * t957 - t4828; + const auto t4863 = -t1907 * t2528 + 3 * t2472 * t308 * t599 * t992 + + t308 * t4861 * t957 - t4779; + const auto t4864 = t3773 + t4861 * t993; + const auto t4865 = t4120 + t4832; + const auto t4866 = t4122 * t4865 + t4125; + const auto t4867 = t304 * t4865 * t7 * t957 - t4127; + const auto t4868 = t4130 + t4741 + t4865 * t7 * t958; + const auto t4869 = t4412 + t4787; + const auto t4870 = t4122 * t4869 + t4415; + const auto t4871 = t3158 * t4869 * t7 + t4417 + t4747; + const auto t4872 = t308 * t4869 * t7 * t957 - t4420; + const auto t4873 = + t1 * t304 * t802 + t3 * t308 * t802 - t4597 - t482 * t991; + const auto t4874 = t4599 + t4749 + t4873 * t993; + const auto t4875 = -t1203 + t304 * t4873 * t957 - t4602; + const auto t4876 = t308 * t4873 * t957 - t4605 - t803; + const auto t4877 = std::pow(t992, 2); + const auto t4878 = -t1234 * t304 + 3 * t304 * t4877 * t957 - 2 * t4846; + const auto t4879 = -t1234 * t308 + 3 * t308 * t4877 * t957 - 2 * t4811; + const auto t4880 = -t1234 * t306 + t4754 * t992 + 3 * t4877 * t993; + hess[0] = t156 + * (-t1 * (-t100 * t102 + t100 * t96 - t101 * t75 + t55 * t75) + + t3 * (-t126 * t74 - t146 * t149 + t149 * t151 + t150 * t74) + + t7 * (t126 * t99 - t149 * t152 + t149 * t153 - t150 * t99)); + hess[1] = t185; + hess[2] = t206; + hess[3] = t356 + * (-t1 + * (-t101 * t250 - t102 * t257 + t250 * t55 + t257 * t96 + + t323) + + t3 + * (-t126 * t249 * t35 - t146 * t325 + t150 * t249 * t35 + + t151 * t324 * t35 - t338) + - t355 + + t7 + * (t126 * t256 * t35 - t150 * t256 * t35 - t152 * t325 + + t153 * t324 * t35 - t343)); + hess[4] = t154 + * (t26 * t3 + * (-t102 * t495 + t462 * t489 - t489 * t496 + t495 * t96 + + t511) + + t26 * t7 + * (-t101 * t495 - t462 * t520 + t495 * t55 + t496 * t520 + + t524) + - t461 + * (t362 * t401 - t401 * t411 + t405 * t410 - t410 * t412 + + t460) + - t529); + hess[5] = t356 + * (-t1 + * (-t101 * t564 - t102 * t568 + t55 * t564 + t568 * t96 + + t606) + + t3 + * (-t126 * t563 + t146 * t620 + t150 * t563 - t151 * t620 + - t628) + - t637 + + t7 + * (-t126 * t633 + t150 * t633 + t152 * t620 - t153 * t620 + + t636)); + hess[6] = t356 + * (t207 + * (-t126 * t693 - t146 * t695 + t150 * t693 + t151 * t695 + + t328 * t682 + t330 * t683 - t666 - t681) + - t222 + * (-t101 * t715 - t102 * t716 + t55 * t715 + t708 - t710 + + t712 - t714 + t716 * t96) + + t227 + * (t126 * t706 - t150 * t706 - t152 * t695 + t153 * t695 + + t328 * t703 + t341 * t680 - t696 - t701) + + t355); + hess[7] = t356 + * (-t1 + * (-t101 * t791 - t102 * t792 + t55 * t791 + t792 * t96 + + t799) + + t3 + * (-t126 * t741 - t146 * t746 + t150 * t741 + t151 * t746 + - t782) + + t7 + * (t126 * t784 - t150 * t784 - t152 * t746 + t153 * t746 + - t790) + + t800); + hess[8] = t356 + * (-t1 + * (-t101 * t869 - t102 * t870 + t55 * t869 + t870 * t96 + + t875) + + t3 + * (-t126 * t826 - t146 * t830 + t150 * t826 + t151 * t830 + - t862) + + t637 + + t7 + * (t126 * t864 - t150 * t864 - t152 * t830 + t153 * t830 + - t868)); + hess[9] = t898; + hess[10] = t920; + hess[11] = t940; + hess[12] = t963; + hess[13] = t980; + hess[14] = t999; + hess[15] = t185; + hess[16] = t156 + * (-t1 * (-t1006 * t101 + t1006 * t55 - t1008 * t102 + t1008 * t96) + + t3 + * (-t1005 * t126 + t1005 * t150 - t1010 * t146 + + t1010 * t151) + + t7 + * (t1007 * t126 - t1007 * t150 - t1010 * t152 + + t1010 * t153)); + hess[17] = t1021; + hess[18] = t356 + * (-t1 + * (-t101 * t1042 - t102 * t1050 + t1042 * t55 + t1050 * t96 + + t1054) + - t1092 + + t3 + * (t1067 * t126 - t1067 * t150 + t1074 * t146 - t1074 * t151 + + t1078) + + t7 + * (t1049 * t126 - t1049 * t150 + t1074 * t152 - t1074 * t153 + + t1080)); + hess[19] = t154 + * (-t1123 + + t26 * t3 + * (-t102 * t1117 + t1114 * t462 - t1114 * t496 + t1117 * t96 + + t1119) + + t26 * t7 + * (-t101 * t1117 + t1117 * t55 - t1120 * t462 + t1120 * t496 + + t1121) + - t461 + * (t1105 * t362 - t1105 * t411 + t1107 * t405 - t1107 * t412 + + t1111)); + hess[20] = t356 + * (-t1 + * (-t101 * t1135 - t102 * t1138 + t1135 * t55 + t1138 * t96 + + t1139) + - t1149 + + t3 + * (-t1134 * t126 + t1134 * t150 + t1144 * t146 - t1144 * t151 + - t1145) + + t7 + * (t1144 * t152 - t1144 * t153 - t1147 * t126 + t1147 * t150 + + t1148)); + hess[21] = t356 + * (-t1 + * (-t101 * t1176 - t102 * t1177 + t1176 * t55 + t1177 * t96 + + t1180) + + t1092 + + t3 + * (-t1160 * t126 + t1160 * t150 - t1165 * t146 + t1165 * t151 + - t1168) + + t7 + * (-t1165 * t152 + t1165 * t153 + t1173 * t126 - t1173 * t150 + - t1175)); + hess[22] = t356 + * (t1202 + + t207 + * (t1075 * t781 + t1077 * t779 - t1181 - t1182 - t1188 * t126 + + t1188 * t150 - t1192 * t146 + t1192 * t151) + - t222 + * (-t101 * t1200 - t102 * t1201 + t1196 + t1197 - t1198 + - t1199 + t1200 * t55 + t1201 * t96) + + t227 + * (t1077 * t789 + t1079 * t776 - t1192 * t152 + t1192 * t153 + - t1193 - t1194 + t1195 * t126 - t1195 * t150)); + hess[23] = t356 + * (-t1 + * (-t101 * t1220 - t102 * t1221 + t1220 * t55 + t1221 * t96 + + t1222) + + t1149 + + t3 + * (-t1213 * t126 + t1213 * t150 - t1215 * t146 + t1215 * t151 + - t1216) + + t7 + * (-t1215 * t152 + t1215 * t153 + t1218 * t126 - t1218 * t150 + - t1219)); + hess[24] = t1224; + hess[25] = t1226; + hess[26] = t1227; + hess[27] = t1229; + hess[28] = t1231; + hess[29] = t1233; + hess[30] = t206; + hess[31] = t1021; + hess[32] = t156 + * (-t1 * (-t101 * t1238 - t102 * t1240 + t1238 * t55 + t1240 * t96) + + t3 + * (-t1237 * t126 + t1237 * t150 - t1241 * t146 + + t1241 * t151) + + t7 + * (t1239 * t126 - t1239 * t150 - t1241 * t152 + + t1241 * t153)); + hess[33] = t356 + * (-t1 + * (-t101 * t1254 - t102 * t1261 + t1254 * t55 + t1261 * t96 + + t1264) + - t1290 + + t3 + * (t126 * t1269 - t1269 * t150 + t1272 * t146 - t1272 * t151 + + t1276) + + t7 + * (t126 * t1260 - t1260 * t150 + t1272 * t152 - t1272 * t153 + + t1278)); + hess[34] = t154 + * (-t1322 + + t26 * t3 + * (-t102 * t1313 + t1309 * t462 - t1309 * t496 + t1313 * t96 + + t1315) + + t26 * t7 + * (-t101 * t1313 + t1313 * t55 - t1319 * t462 + t1319 * t496 + + t1320) + - t461 + * (t1299 * t362 - t1299 * t411 + t1301 * t405 - t1301 * t412 + + t1304)); + hess[35] = t356 + * (-t1 + * (-t101 * t1330 - t102 * t1334 + t1330 * t55 + t1334 * t96 + + t1335) + - t1340 + + t3 + * (-t126 * t1329 * t35 + t1329 * t150 * t35 + + t1336 * t151 * t35 - t1337 * t146 - t1338) + + t7 + * (t126 * t1333 * t35 - t1333 * t150 * t35 + + t1336 * t153 * t35 - t1337 * t152 - t1339)); + hess[36] = t356 + * (-t1 + * (-t101 * t1359 - t102 * t1360 + t1359 * t55 + t1360 * t96 + + t1363) + + t1290 + + t3 + * (-t126 * t1346 + t1346 * t150 - t1348 * t146 + t1348 * t151 + - t1351) + + t7 + * (t126 * t1356 - t1348 * t152 + t1348 * t153 - t1356 * t150 + - t1358)); + hess[37] = t356 + * (-t1 + * (-t101 * t1377 - t102 * t1378 + t1377 * t55 + t1378 * t96 + + t1379) + + t1380 + + t3 + * (-t126 * t1369 + t1369 * t150 - t1372 * t146 + t1372 * t151 + - t1373) + + t7 + * (t126 * t1375 - t1372 * t152 + t1372 * t153 - t1375 * t150 + - t1376)); + hess[38] = t356 + * (t1340 + + t207 + * (-t126 * t1390 + t1273 * t859 + t1275 * t861 - t1381 + - t1382 - t1388 * t146 + t1388 * t151 + t1390 * t150) + - t222 + * (-t101 * t1399 - t102 * t1398 + t1394 - t1395 + t1396 + - t1397 + t1398 * t96 + t1399 * t55) + + t227 + * (t126 * t1393 + t1273 * t867 + t1277 * t857 - t1388 * t152 + + t1388 * t153 - t1391 - t1392 - t1393 * t150)); + hess[39] = t1401; + hess[40] = t1403; + hess[41] = t1404; + hess[42] = t1406; + hess[43] = t1407; + hess[44] = t1409; + hess[45] = t356 + * (-t1 + * (-t101 * t1420 - t102 * t1423 + t1420 * t55 + t1423 * t96 + + t323) + + t3 + * (-t126 * t1424 + t1424 * t150 - t1428 * t146 + t1428 * t151 + - t338) + - t355 + + t7 + * (t126 * t1422 - t1422 * t150 - t1428 * t152 + t1428 * t153 + - t343)); + hess[46] = t356 + * (-t1 + * (-t101 * t1434 - t102 * t1435 + t1054 + t1434 * t55 + + t1435 * t96) + - t1092 + + t3 + * (t1078 + t126 * t1436 - t1436 * t150 + t1437 * t146 + - t1437 * t151) + + t7 + * (t1080 + t126 * t1438 + t1437 * t152 - t1437 * t153 + - t1438 * t150)); + hess[47] = t356 + * (-t1 + * (-t101 * t1441 - t102 * t1442 + t1264 + t1441 * t55 + + t1442 * t96) + - t1290 + + t3 + * (t126 * t1443 + t1276 - t1443 * t150 + t1444 * t146 + - t1444 * t151) + + t7 + * (t126 * t1445 + t1278 + t1444 * t152 - t1444 * t153 + - t1445 * t150)); + hess[48] = t154 + * (-t1 + * (-t101 + * (t1559 * t1628 + t1622 - t1625 * t52 + t1627 * t52 + + t1629 * t1630) + - t102 + * (t1468 * t95 - t1608 * t91 + t1610 * t91 + + t1612 * t313 + t1631) + + 2 * t290 * t319 - 2 * t320 * t322 + + t55 + * (t1472 * t1611 - t1608 * t89 + t1610 * t89 + + t1612 * t1613 + t1622) + + t96 + * (t1557 * t54 - t1625 * t48 + t1627 * t48 + + t1629 * t276 + t1631)) + - 2 * t1632 - 2 * t1633 + 2 * t1634 + 2 * t1635 - 2 * t1638 + - 2 * t1641 + 2 * t1647 + t1663 + + t3 + * (t126 + * (-t134 * t1478 + 3 * t134 * t1489 * t1491 + + t145 * t1488 * t7 - t1495 * t22 - t1542) + + t1446 * t329 - t1447 * t335 + t146 * t1580 + - t150 + * (-t123 * t1564 + 3 * t123 * t1571 * t1573 + + t125 * t1570 * t7 - t1542 - t1577 * t22) + - t151 * t1581) + + t7 + * (-t126 + * (-t139 * t1478 + 3 * t139 * t1489 * t1491 + + t145 + * (-t130 * t1582 + 3 * t130 * t3 * t465 + - t1414 * t1484 + 2 * t1484 * t15 + - t1487 * t3) + - t1494 * t1605 - t1599) + - t1446 * t342 + t1447 * t339 + + t150 + * (-t118 * t1564 + 3 * t118 * t1571 * t1573 + + t125 + * (-t107 * t1582 + 3 * t107 * t3 * t465 + - t1414 * t1568 + 2 * t15 * t1568 + - t1569 * t3) + - t1576 * t1588 - t1599) + + t152 * t1581 - t153 * t1580)); + hess[49] = t154 + * (-t1 + * (-t1754 * t458 - t1760 * t459 + t1762 * t439 + t1763 * t457 + + t362 + * (t1613 * t454 + t1674 * t1764 - t1765 * t1766 + - t1768 * t306 + t1769 * t227 + t1780) + + t405 + * (-t1623 * t1782 + t1720 * t1787 - t1785 * t1788 + + t1786 * t207 + t1793 + t276 * t432) + - t411 + * (t1630 * t432 + t1717 * t933 + t1780 - t1781 * t1782 + - t1783 * t1785 + t1786 * t227) + - t412 + * (-t1606 * t1766 + t1678 * t1794 - t1768 * t308 + + t1769 * t207 + t1793 + t313 * t454)) + - t166 * t1801 + t174 * t1797 + t1755 + t1761 - t1795 - t1796 + - t1798 * t64 + t1816 + + t3 + * (-t102 * t1743 + t1709 * t462 - t1732 * t496 + t1738 * t96 + + t319 * t507 - t322 * t510 + t333 * t503 - t336 * t509) + + t7 + * (-t101 * t1738 + t1743 * t55 + t1747 * t496 - t1749 * t462 + + t290 * t510 - t320 * t507 - t333 * t523 + t336 * t522)); + hess[50] = t154 + * (t1 * t15 * t1797 * t7 + t1 * t15 * t1838 * t3 + - t1 + * (-t1754 * t1831 - t1760 * t1836 + t1762 * t1827 + + t1763 * t1834 + + t362 + * (t1613 * t1829 + t1756 * t596 - t1765 * t1909 + + t1794 * t1891 - t1910 * t306 + t1913) + + t405 + * (-t1623 * t1914 + t1750 * t1916 + t1787 * t1851 + - t1788 * t1915 + t1823 * t276 + t1918) + - t411 + * (t1630 * t1823 + t1750 * t582 - t1781 * t1914 + - t1783 * t1915 + t1787 * t1854 + t1913) + - t412 + * (-t1606 * t1909 + t1756 * t1907 + t1794 * t1890 + + t1829 * t313 - t1910 * t308 + t1918)) + + 3 * t11 * t1804 * t465 * t7 + 3 * t13 * t1813 * t3 * t465 + - t157 * t1801 - t16 * t1813 - t178 * t1822 - t1798 * t66 - t1799 + - t1800 - t1804 * t22 - t1814 * t1818 - t1819 - t1820 + - t1821 * t66 + t290 * t496 + + t3 + * (-t102 * t1900 + t1881 * t96 + t1902 * t462 - t1904 * t496 + + t319 * t623 - t322 * t626 + t333 * t602 - t336 * t605) + + t336 * t55 + t462 * t602 + t623 * t96 + + t7 + * (-t101 * t1881 + t1900 * t55 + t1906 * t496 - t1908 * t462 + + t290 * t626 - t320 * t623 - t333 * t604 + t336 * t588)); + hess[51] = t2043; + hess[52] = t2192; + hess[53] = t2337; + hess[54] = t154 + * (t1 + * (t2346 + * (-t1630 * t894 - t2343 * t2344 - t2345 + + t52 * t892 + * (2 * t1 * t11 * t465 * t50 + + 2 * t11 * t465 * t52 * t7 - t1421 * t48 + - t157 * t2340 - t174 * t2340 - t2338 + - t2339 - t276 * t57)) + + t2360 * t2361 + t2363) + - t2383 + - t890 + * (t2364 * (-t2353 * t282 + t2365 + t2367) - t2372 * t405 + + t2375) + - t933 * (t2360 * t2364 + t2372 * t411 + t2376)); + hess[55] = t154 + * (t1 + * (-t2387 + * (t1 * t15 * t285 * t3 * t892 + + t1 * t48 * t892 + * (-t1030 * t279 - t2385 - t279 * t475) + - t1426 - t2386 * t899 - t276 * t905) + + t2400 * t2401 - t2403) + - t2420 - t890 * (t2364 * t2400 - t2408 * t405 + t2410) + - t933 + * (-t2364 * (t2371 - t2404 * t2411 + t2412) + t2408 * t411 + - t2414)); + hess[56] = t154 + * (-t1 + * (t2387 + * (-t2345 - t2386 * t935 - t276 * t936 + + t48 * t7 * t892 + * (-t1242 * t279 - t2385 - t279 * t558)) + + t2401 * t2437 + t2441) + + t2421 - t2422 + t2424 + t2426 + - t890 * (-t2364 * t2437 - t2432 * t405 - t2438) + - t933 + * (-t2364 * (t2367 - t2411 * t2429 + t2439) + t2432 * t411 + + t2440)); + hess[57] = t154 + * (-t1 + * (-t2456 * t2467 + + t2476 + * (t2475 + + t958 + * (t2348 * t304 + t2349 * t306 + t2469 + t305 + + t307)) + + t2478) + + t2487 + + t3 + * (t145 * t335 * t946 + t151 * t2460 * t95 - t2442 * t945 + - t2456 * t2457) + + t7 + * (t126 * t95 + * (-t1421 - t2448 * t976 + + 3 * t2450 * t314 * t91 * t942 - t2461 + + t313 * t942 * t948) + - t152 * t2460 * t95 - t2466)); + hess[58] = t154 + * (-t1 + * (-t102 * t2506 + - t2514 * (t1 * t2511 * t306 * t35 * t957 - t2513) + - t2516) + + t2525 + + t3 + * (t151 * t95 * (-t2496 * t996 + t2500) + - t2457 * (-t2449 * t2496 + t2499) - t2504) + - t7 * (t2506 * t462 + t2514 * (t2511 * t2517 + t2518) + t2519)); + hess[59] = t154 + * (t1 * (t102 * t2537 + t2514 * t2530 + t2540) - t2551 + - t3 * (-t2476 * t2533 + t2530 * t2531 + t2535) + - t7 * (t2514 * t2533 + t2537 * t462 + t2539)); + hess[60] = t154 + * (t26 * t3 + * (-t102 * t2565 + t2561 * t462 - t2561 * t496 + t2565 * t96 + + t511) + + t26 * t7 + * (-t101 * t2565 + t2565 * t55 - t2566 * t462 + t2566 * t496 + + t524) + - t461 + * (t2554 * t362 - t2554 * t411 + t2555 * t405 - t2555 * t412 + + t460) + - t529); + hess[61] = t154 + * (-t1123 + + t26 * t3 + * (-t102 * t2582 + t1119 + t2580 * t462 - t2580 * t496 + + t2582 * t96) + + t26 * t7 + * (-t101 * t2582 + t1121 + t2582 * t55 - t2583 * t462 + + t2583 * t496) + - t461 + * (t1111 + t2568 * t362 - t2568 * t411 + t2569 * t405 + - t2569 * t412)); + hess[62] = t154 + * (-t1322 + + t26 * t3 + * (-t102 * t2589 + t1315 + t2588 * t462 - t2588 * t496 + + t2589 * t96) + + t26 * t7 + * (-t101 * t2589 + t1320 + t2589 * t55 - t2590 * t462 + + t2590 * t496) + - t461 + * (t1304 + t2585 * t362 - t2585 * t411 + t2586 * t405 + - t2586 * t412)); + hess[63] = t154 + * (-t1 + * (-t101 * t1732 - t102 * t1749 + t1709 * t55 + t1747 * t96 + + t290 * t503 + t319 * t522 - t320 * t509 - t322 * t523) + + t150 * t2592 + t151 * t2594 - t1642 + t1643 - t1644 + t1645 + - t178 * t2640 + t2168 - t2182 - t2183 - t2638 - t2639 + + t2643 * t64 - t2646 * t66 + t2647 + + t3 + * (t126 + * (3 * t134 * t1489 * t1490 * t2601 - t134 * t2595 + + t145 * t465 * t7 + * (-t2060 - t2071 - t2597 - t2599) + - t22 * t2603 - t22 * t2605 - t2613) + + t146 * t2630 + - t150 + * (3 * t123 * t1571 * t1572 * t2619 - t123 * t2614 + + t125 * t465 * t7 + * (-t2114 - t2123 - t2616 - t2617) + - t22 * t2621 - t22 * t2623 - t2613) + - t151 * t2634 + t2591 * t334 - t2592 * t337 + + t2593 * t329 - t2594 * t335) + + t7 + * (-t126 + * (3 * t139 * t1489 * t1490 * t2601 - t139 * t2595 + + t145 * t15 + * (t16 * t2633 - t178 * t2596 + t2165 + t2602) + - t16 * t2603 - t2023 * t2601 - t2637) + + t150 + * (3 * t118 * t1571 * t1572 * t2619 - t118 * t2614 + + t125 * t15 + * (t16 * t2626 - t178 * t2615 + t2158 + t2620) + - t16 * t2621 - t2009 * t2619 - t2637) + + t152 * t2634 - t153 * t2630 - t2593 * t342 + + t2594 * t339 + t2635 * t337 - t2636 * t334)); + hess[64] = t154 + * (-t1 + * (t362 + * (-t2662 * t2722 + t2720 * t306 - t2721 * t306 + + t2723 * t2724 + t2728) + + t405 + * (-t1788 * t2730 - t1824 * t2702 + t2729 * t284 + + t2731 * t2732 + t2735) + - t411 + * (-t1783 * t2730 - t2702 * t2736 + t2723 * t2731 + + t2728 + t2729 * t282) + - t412 + * (-t1830 * t2662 + t2720 * t308 - t2721 * t308 + + t2724 * t2732 + t2735) + + 2 * t439 * t457 - 2 * t458 * t459) + - 2 * t166 * t1810 - 2 * t1803 * t64 - 2 * t1805 - 2 * t1806 + + 2 * t1807 + 2 * t1808 + 2 * t2737 + t2741 + + t3 + * (-t102 * t2715 + t2648 * t503 - t2649 * t509 + t2714 * t96 + + t462 + * (t1611 * t2661 + t227 * t2675 - t2671 * t89 + + t2673 * t89 + t2693) + - t496 + * (t1628 * t2701 + t227 * t2712 + t2693 - t2708 * t52 + + t2710 * t52)) + + t7 + * (-t101 * t2714 - t2648 * t523 + t2649 * t522 + t2715 * t55 + - t462 + * (t207 * t2675 + t2661 * t2719 - t2671 * t91 + + t2673 * t91 + t2718) + + t496 + * (t207 * t2712 + t2701 * t2716 - t2708 * t48 + + t2710 * t48 + t2718))); + hess[65] = t154 + * (-t1 + * (t1827 * t457 - t1831 * t459 + t1834 * t439 - t1836 * t458 + + t362 + * (-t1765 * t2814 + t1794 * t2751 + t227 * t2816 + - t2815 * t306 + t2817 + t454 * t596) + + t405 + * (-t1623 * t2807 - t1788 * t2808 + t1916 * t432 + + t207 * t2809 + t2801 * t890 + t2812) + - t411 + * (-t1781 * t2807 - t1783 * t2808 + t1787 * t2783 + + t227 * t2809 + t2817 + t432 * t582) + - t412 + * (-t1606 * t2814 + t1907 * t454 + t207 * t2816 + + t2805 * t2813 + t2812 - t2815 * t308)) + - t166 * t1821 + t174 * t1838 - t1822 * t64 - t1828 + t1832 + - t1835 + t1837 + t2819 + + t3 + * (-t102 * t2799 + t2776 * t462 + t2794 * t96 - t2797 * t496 + + t503 * t623 + t507 * t602 - t509 * t626 - t510 * t605) + + t7 + * (-t101 * t2794 + t2799 * t55 + t2803 * t496 - t2806 * t462 + - t507 * t604 + t510 * t588 + t522 * t626 - t523 * t623)); + hess[66] = t2893; + hess[67] = t154 + * (t1 * t15 * t1803 * t3 + t1 * t15 * t1810 * t7 + - t1040 + * (-t101 * t2944 + t2945 * t55 - t462 * t797 + t496 * t793) + + t12 * t15 * t2903 * t35 - t1807 - t1808 + - t222 + * (t2897 * t439 + t2900 * t457 - t2901 * t458 - t2902 * t459 + + t362 + * (t1764 * t2907 + t227 * t2918 + t2842 * t750 + - t2916 * t306 - t2917 * t754 + t2929) + + t405 + * (t207 * t2940 - t284 * t2937 + t2931 * t890 + + t2938 * t770 - t2939 * t772 + t2942) + - t411 + * (t227 * t2940 - t282 * t2937 + t2868 * t770 + - t2870 * t772 + t2929 + t2931 * t933) + - t412 + * (t207 * t2918 + t2813 * t2907 - t2875 * t754 + - t2916 * t308 + t2942 + t2943 * t750)) + - t2737 - t2741 - t2903 * t35 + + t3 * t35 + * (-t102 * t2961 + t2944 * t503 - t2945 * t509 + t2952 * t462 + - t2957 * t496 + t2960 * t96 + t507 * t795 - t510 * t798) + + t35 * t7 + * (-t101 * t2960 - t2944 * t523 + t2945 * t522 + t2961 * t55 + + t2964 * t496 - t2965 * t462 - t507 * t797 + t510 * t793) + + t362 * t457 + t405 * t439 + - t493 + * (-t102 * t2945 + t2944 * t96 + t462 * t795 - t496 * t798)); + hess[68] = t3028; + hess[69] = t154 + * (t1 + * (t146 * t54 * (-t2128 * t3032 * t892 + t3042) + - t3040 * (-t3032 * t3034 + t3039) - t3047) + - t3 + * (-t2387 * t3049 + - t3059 * (t282 * t3 * t3053 * t35 * t880 - t3058) + - t3060) + + t3066 + - t7 + * (t2346 * t3049 + t3059 * (t3053 * t3061 + t3063) + t3064)); + hess[70] = t154 + * (t1 + * (t125 * t2591 * t903 - t125 * t2636 * t902 + + t146 * t3079 * t54 - t3040 * t3078) + - t3 * (t2361 * t3088 - t3070 * t3078 + t3090) + t3068 - t3069 + + t3071 * t64 - t3072 + + t7 * (t2401 * t3088 - t3070 * t3079 + t3089)); + hess[71] = t154 + * (-t1 * (-t2361 * t3098 + t2401 * t3096 + t3099) + + t3 * (t2387 * t3103 + t3059 * t3096 + t3104) - t3106 + - t7 * (t2346 * t3103 + t3059 * t3098 + t3101 * t523 - t3102)); + hess[72] = t154 + * (t3 * (t2531 * t3125 - t2541 * t3116 - t3127) + - t3134 * (t3125 * t362 + t3130 * t412 + t3133) - t3136 + + t7 * (t2467 * t3116 + t2531 * t3130 + t3131)); + hess[73] = t154 + * (t12 * t15 * t3137 * t404 + + t3 + * (t2476 * t3160 + + t3139 + * (-t227 * t3157 + t3155 * t89 * t948 - t3156 * t89 + - t3162) + + t3163) + - t3134 + * (t3151 + + t362 * (t227 + t3082 + t3142 - t3145 * t993 + t3148) + - t412 * (t207 + t3081 - t3145 * t958 + t3149 + t3150)) + - t3138 - t3140 + t3141 + - t7 + * (t2514 * t3160 + + t3139 + * (-t207 * t3157 - t3115 + t3155 * t91 * t948 + - t3156 * t91) + + t3161)); + hess[74] = t154 + * (-t3 * (t2531 * t3176 + t2541 * t3181 + t3182) + - t3134 * (-t3174 * t412 - t3176 * t362 - t3177) + t3165 * t404 + - t3166 - t3168 + t3169 + + t7 * (t2467 * t3181 - t2531 * t3174 + t3178 * t522 - t3179)); + hess[75] = t356 + * (-t1 + * (-t101 * t3191 - t102 * t3192 + t3191 * t55 + t3192 * t96 + + t606) + + t3 + * (-t126 * t3193 + t146 * t3194 + t150 * t3193 - t151 * t3194 + - t628) + - t637 + + t7 + * (-t126 * t3195 + t150 * t3195 + t152 * t3194 - t153 * t3194 + + t636)); + hess[76] = t356 + * (-t1 + * (-t101 * t3200 - t102 * t3201 + t1139 + t3200 * t55 + + t3201 * t96) + - t1149 + + t3 + * (-t1145 - t126 * t3202 + t146 * t3203 + t150 * t3202 + - t151 * t3203) + + t7 + * (t1148 - t126 * t3204 + t150 * t3204 + t152 * t3203 + - t153 * t3203)); + hess[77] = t356 + * (-t1 + * (-t101 * t3211 - t102 * t3212 + t1335 + t3211 * t55 + + t3212 * t96) + - t1340 + + t3 + * (-t126 * t3210 - t1338 - t146 * t3213 + t150 * t3210 + + t151 * t3213) + + t7 + * (t126 * t3214 - t1339 - t150 * t3214 - t152 * t3213 + + t153 * t3213)); + hess[78] = t154 + * (-t1 + * (-t101 * t1904 - t102 * t1908 + t1902 * t55 + t1906 * t96 + + t290 * t602 + t319 * t588 - t320 * t605 - t322 * t604) + - t2336 + + t3 + * (t126 + * (3 * t134 * t1489 * t1490 * t3239 - t134 * t3234 + + t145 * t15 + * (t22 * t3248 + t2278 + t2280 + t3247) + - t1493 * t3246 - t22 * t3243 - t3252) + + t146 * t3233 + - t150 + * (3 * t123 * t1571 * t1572 * t3221 - t123 * t3215 + + t125 * t15 + * (t22 * t3256 + t2297 + t2299 + t3255) + - t1575 * t3254 - t22 * t3225 - t3252) + - t151 * t3244 + t329 * t624 + t334 * t621 - t335 * t627 + - t337 * t625) + - t3274 + + t7 + * (-t126 + * (3 * t139 * t1489 * t1490 * t3239 - t139 * t3234 + + t145 * t15 * (t16 * t3248 + t2314 + t3260) + - t16 * t3241 - t2023 * t3239 - t3259) + + t150 + * (3 * t118 * t1571 * t1572 * t3221 - t118 * t3215 + + t125 * t15 * (t16 * t3256 + t2304 + t3257) + - t16 * t3223 - t2009 * t3221 - t3259) + + t152 * t3244 - t153 * t3233 - t334 * t634 + t337 * t635 + + t339 * t627 - t342 * t624)); + hess[79] = t154 + * (-t1 + * (-t101 * t2797 - t102 * t2806 + t2776 * t55 + t2803 * t96 + + t503 * t588 - t509 * t604 + t522 * t602 - t523 * t605) + + t126 * t2636 + t153 * t2593 - t157 * t2646 + t166 * t2643 + - t2640 * t66 - t2644 - t2645 + + t3 + * (t126 + * (3 * t134 * t1489 * t2601 * t3239 - t134 * t3287 + + t145 * t15 + * (-t157 * t2596 + t22 * t3282 + t2602 + t3286) + - t22 * t3288 - t2601 * t3275 - t3300) + + t146 * t3317 + - t150 + * (3 * t123 * t1571 * t2619 * t3221 - t123 * t3309 + + t125 * t15 + * (-t157 * t2615 + t22 * t3307 + t2620 + t3322) + - t22 * t3323 - t2619 * t3318 - t3300) + - t151 * t3327 + t2591 * t624 - t2592 * t627 + + t2593 * t621 - t2594 * t625) + - t3269 + t3270 - t3271 + t3272 + t3337 - t3338 - t3339 + t3345 + + t7 + * (-t126 + * (3 * t139 * t1489 * t2601 * t3239 - t139 * t3287 + + t145 * t3 * t465 + * (-t2596 * t7 - t3281 - t3283 - t3336) + - t16 * t3288 - t16 * t3326 - t3334) + + t150 + * (3 * t118 * t1571 * t2619 * t3221 - t118 * t3309 + + t125 * t3 * t465 + * (-t2615 * t7 - t3306 - t3319 - t3329) + - t16 * t3310 - t16 * t3323 - t3334) + + t152 * t3327 - t153 * t3317 - t2593 * t634 + + t2594 * t635 + t2635 * t627 - t2636 * t624)); + hess[80] = t154 + * (-t1 + * (-t101 + * (t1733 * t3364 - t3431 * t52 + t3432 * t52 + + t3433 * t582 + t3442) + - t102 + * (t1907 * t3441 + t2719 * t3409 + t3437 - t3439 * t91 + + t3440 * t91) + + t55 + * (t1739 * t3408 - t3439 * t89 + t3440 * t89 + + t3441 * t596 + t3442) + + 2 * t588 * t602 - 2 * t604 * t605 + + t96 + * (t1916 * t3433 + t2716 * t3367 - t3431 * t48 + + t3432 * t48 + t3437)) + + t3 + * (t126 + * (3 * t134 * t1489 * t3415 - t134 * t3412 + + t145 + * (-t130 * t3419 + 3 * t130 * t465 * t7 + + 2 * t15 * t3236 - t3184 * t3236 + - t3413 * t7) + - 2 * t3239 * t3275 - t3426) + + t146 * t3398 + - t150 + * (3 * t123 * t1571 * t3374 - t123 * t3370 + + t125 + * (-t107 * t3419 + 3 * t107 * t465 * t7 + + 2 * t15 * t3218 - t3184 * t3218 + - t3372 * t7) + - t3254 * t3375 - t3426) + - t151 * t3418 + t3346 * t621 - t3347 * t625) + - 2 * t3263 - 2 * t3264 + 2 * t3265 + 2 * t3266 - 2 * t3443 + - 2 * t3444 + 2 * t3445 + t3451 + + t7 + * (-t126 + * (3 * t139 * t1489 * t3415 - t139 * t3412 + + t145 * t3 * t3414 - t3417 * t3429 - t3428) + + t150 + * (3 * t118 * t1571 * t3374 - t118 * t3370 + + t125 * t3 * t3373 - t16 * t3376 - t3428) + + t152 * t3418 - t153 * t3398 - t3346 * t634 + + t3347 * t635)); + hess[81] = t3519; + hess[82] = t3585; + hess[83] = t3643; + hess[84] = t154 + * (t1 + * (t2346 + * (-t2344 * t3646 + + t3 * t52 * t892 + * (-t3644 - t474 * t578 - t558 * t578) + - t3647 - t582 * t894) + + t2361 * t3657 + t3659) + - t3677 + - t890 + * (t2364 * (-t207 * t3652 * t3660 + t3661 + t3664) + - t3670 * t405 + t3672) + - t933 * (t2364 * t3657 + t3670 * t411 + t3673)); + hess[85] = t154 + * (t1 + * (t2346 + * (t1 * t15 * t583 * t7 * t892 + + t1 * t52 * t892 + * (-t1030 * t578 - t15 * t3532 - t3644) + - t3198 - t3646 * t52 * t899 - t582 * t905) + - t2361 * t3682 + t3658 * t919 - t3678) + - t3696 + - t890 + * (t2364 * (-t3660 * t3683 + t3684 + t3687) - t3690 * t405 + + t3691) + - t933 * (-t2364 * t3682 + t3690 * t411 - t3692)); + hess[86] = t154 + * (-t1 + * (t2387 + * (-t1916 * t936 - t3646 * t48 * t935 - t3647 + + t48 * t892 + * (2 * t1 * t13 * t465 * t50 + + 2 * t13 * t3 * t465 * t48 - t1326 * t1561 + - t189 * t582 - t22 * t3532 - t2338 - t3073 + - t3208 * t578)) + + t2401 * t3704 + t3705) + - t3714 - t890 * (-t2364 * t3704 - t3707 * t405 - t3708) + - t933 + * (-t2364 * (-t284 * t3700 + t3664 + t3709) + t3707 * t411 + + t3710)); + hess[87] = t154 + * (-t1 + * (-t2467 * t3732 + t2476 * (t3739 * (t3736 + t3738) + t3742) + + t3744) + + t3 + * (-t145 * t624 * t945 + t145 * t625 * t946 + + t151 * t3729 * t95 - t2457 * t3732) + + t3748 + + t7 + * (t126 * t95 * (-t3730 * t976 + t3734) - t3729 * t3733 + - t3735)); + hess[88] = t154 + * (-t1 * (-t2467 * t3762 + t2476 * t3754 + t2515 * t3743 - t3749) + + t3 + * (t151 * t95 * (-t3757 * t996 + t3765) - t2457 * t3762 + - t3766) + - t3767 + - t7 + * (t2514 * (t2517 * t3751 + t3763) - t2531 * t3754 + t3764)); + hess[89] = t154 + * (t1 * (t2514 * t3774 - t2541 * t3769 + t3775) + - t3 * (t2531 * t3774 - t2541 * t3776 + t3777) + t3781 + + t7 + * (t126 * t3769 * t95 + t145 * t624 * t985 + - t145 * t635 * t984 - t3733 * t3776)); + hess[90] = t356 + * (-t1 + * (-t101 * t3793 - t102 * t3792 + t35 * t708 - t35 * t710 + + t35 * t712 - t35 * t714 + t3792 * t96 + t3793 * t55) + + t3 + * (-t126 * t3788 - t146 * t3789 + t150 * t3788 + t151 * t3789 + + t328 * t35 * t682 + t330 * t35 * t683 - t35 * t666 + - t35 * t681) + + t355 + + t7 + * (t126 * t35 * t3791 - t150 * t35 * t3791 - t152 * t3789 + + t153 * t3789 + t328 * t35 * t703 + t341 * t35 * t680 + - t35 * t696 - t35 * t701)); + hess[91] = t356 + * (-t1 + * (-t101 * t3801 - t102 * t3799 + t1180 + t3799 * t96 + + t3801 * t55) + + t1092 + + t3 + * (-t1168 - t126 * t3796 - t146 * t3797 + t150 * t3796 + + t151 * t3797) + + t7 + * (-t1175 + t126 * t3800 - t150 * t3800 - t152 * t3797 + + t153 * t3797)); + hess[92] = t356 + * (-t1 + * (-t101 * t3807 - t102 * t3805 + t1363 + t3805 * t96 + + t3807 * t55) + + t1290 + + t3 + * (-t126 * t3803 - t1351 - t146 * t3804 + t150 * t3803 + + t151 * t3804) + + t7 + * (t126 * t3806 - t1358 - t150 * t3806 - t152 * t3804 + + t153 * t3804)); + hess[93] = t2043; + hess[94] = t2893; + hess[95] = t3519; + hess[96] = t154 + * (t15 * t1919 * t406 + t1663 - 2 * t1920 - 2 * t2040 + 2 * t2042 + - t222 + * (-t101 + * (t1628 * t3849 + t1718 * t3881 + t3878 - t3880 * t52 + - t3882 * t3883) + - t102 + * (t1748 * t3871 + t3818 * t95 - t3870 * t91 + - t3872 * t3886 + t3885) + + 2 * t3867 * t707 - 2 * t3868 * t709 + + t55 + * (t1611 * t3821 + t1675 * t3871 - t3870 * t89 + - t3872 * t3873 + t3878) + + t96 + * (t1744 * t3881 + t3846 * t54 - t3880 * t48 + - t3882 * t3884 + t3885)) + + t3 + * (t126 + * (3 * t134 * t1489 * t3829 * t59 - t134 * t3824 + + t145 * t3828 * t465 * t7 - t1538 * t3830 - t3840) + + t146 * t3860 + - t150 + * (3 * t123 * t1571 * t3857 * t59 - t123 * t3852 + + t125 * t3856 * t465 * t7 - t1538 * t3858 - t3840) + - t151 * t3861 + t3809 * t665 - t3811 * t682) + + t7 + * (-t126 + * (3 * t139 * t1489 * t3829 * t59 - t139 * t3824 + + t145 * t15 + * (t16 * (3 * t3 * t77 - t3827 - t3865 - t3866) + + 2 * t2016 + t2019 + t267) + - t1598 * t1955 * t2021 - t3864) + + t150 + * (3 * t118 * t1571 * t3857 * t59 - t118 * t3852 + + t125 * t15 + * (t16 * (3 * t3 * t36 - t3855 - t3862 - t3863) + + 2 * t2001 + t2004 + t267) + - t1598 * t1996 * t2007 - t3864) + + t152 * t3861 - t153 * t3860 - t3809 * t703 + + t3811 * t700)); + hess[97] = t3950; + hess[98] = t4006; + hess[99] = t154 + * (t1 * t35 * (t2361 * t4012 + t2401 * t4019 + t4020) + - t1824 * (t2364 * t4019 - t4027 * t405 + t4029) + - t2736 * (t2364 * t4012 + t4027 * t411 + t4030) - t4031); + hess[100] = t154 + * (-t1824 * (t2364 * t4039 - t405 * t4050 + t4051) + + t222 * (t2401 * t4039 + t361 * t4044 * t96 - t4045) + t2420 + - t2736 * (t2364 * t4044 + t4050 * t411 - t4052)); + hess[101] = t154 + * (t1 * t35 * (-t2401 * t4060 + t361 * t4065 * t96 - t4066) + - t1824 * (-t2364 * t4060 - t405 * t4070 - t4071) + - t2736 * (t2364 * t4065 + t4070 * t411 + t4072) - t4073); + hess[102] = t154 + * (-t207 + * (-t2476 * (t304 * t4084 * t957 - t4026 - t4091) + + t2531 * t4089 + t4093) + - t222 + * (t2476 * (t4011 + t4084 * t958 + t4096) - t2514 * t4089 + + t4097) + - t2487 + + t7 + * (t126 * t95 + * (-t1748 * t4082 + t35 * t57 * t647 * t948 + + t35 * t702 * t942 * t948 - t4077 - t4079 * t976) + - t35 * t4075 + - t3733 + * (t1 * t15 * t3 * t35 * t647 * t948 + + t1 * t35 * t639 * t942 * t948 - t1426 + - t1740 * t4082 - t4079 * t996) + - t4074 * t680)); + hess[103] = t154 + * (-t222 + * (t102 * t404 * t4114 + - t2514 * (t1 * t306 * t4108 * t957 - t4111) - t4115) + - t227 + * (t2514 * (t4108 * t4116 + t4118) - t2531 * t4114 + t4119) + - t2525 + + t3 + * (t151 * t95 + * (t1 * t35 * t639 * t948 * t975 - t1070 + + t164 * t35 * t647 * t948 - t4101 * t996 - t4102) + - t2457 + * (t1 * t15 * t35 * t647 * t7 * t948 - t2449 * t4101 + + t35 * t639 * t7 * t948 * t975 - t4099 - t484) + - t4104)); + hess[104] = t154 + * (-t207 * (-t2476 * t4128 + t2531 * t4126 + t4129) + - t222 * (t102 * t404 * t4131 - t2514 * t4126 - t4132) + - t227 * (t2514 * t4128 - t2531 * t4131 + t4133) - t4134); + hess[105] = t356 + * (-t1 + * (-t101 * t4140 - t102 * t4141 + t4140 * t55 + t4141 * t96 + + t799) + + t3 + * (-t126 * t4137 - t146 * t4138 + t150 * t4137 + t151 * t4138 + - t782) + + t7 + * (t126 * t4139 - t150 * t4139 - t152 * t4138 + t153 * t4138 + - t790) + + t800); + hess[106] = t356 + * (-t1 + * (-t101 * t4150 - t102 * t4151 + t1196 * t35 + t1197 * t35 + - t1198 * t35 - t1199 * t35 + t4150 * t55 + t4151 * t96) + + t1202 + + t3 + * (t1075 * t35 * t781 + t1077 * t35 * t779 - t1181 * t35 + - t1182 * t35 - t126 * t4146 - t146 * t4148 + t150 * t4146 + + t151 * t4148) + + t7 + * (t1077 * t35 * t789 + t1079 * t35 * t776 - t1193 * t35 + - t1194 * t35 + t126 * t4149 - t150 * t4149 - t152 * t4148 + + t153 * t4148)); + hess[107] = t356 + * (-t1 + * (-t101 * t4156 - t102 * t4157 + t1379 + t4156 * t55 + + t4157 * t96) + + t1380 + + t3 + * (-t126 * t4153 - t1373 - t146 * t4154 + t150 * t4153 + + t151 * t4154) + + t7 + * (t126 * t4155 - t1376 - t150 * t4155 - t152 * t4154 + + t153 * t4154)); + hess[108] = t2192; + hess[109] = t154 + * (t1 * t15 * t2640 * t3 + t1 * t15 * t2646 * t7 + + t12 * t15 * t2187 * t35 - t174 * t2643 + - t222 + * (-t101 * t2957 - t102 * t2965 + t2952 * t55 + t2964 * t96 + + t503 * t793 - t509 * t797 + t522 * t795 - t523 * t798) + - t2641 - t2642 + + t3 + * (t126 + * (3 * t134 * t1489 * t2077 * t2601 * t35 + - t134 * t4161 + t145 * t15 * t4164 * t7 + - t22 * t4165 - t22 * t4166 - t4176) + + t146 * t4190 + - t150 + * (3 * t123 * t1571 * t2129 * t2619 * t35 + - t123 * t4177 + t125 * t15 * t4179 * t7 + - t22 * t4180 - t22 * t4181 - t4176) + - t151 * t4193 + t2046 * t2591 - t2047 * t2592 + + t4159 * t760 - t4160 * t779) + - t4158 - t4196 - t4197 - t4198 + t503 * t55 + t522 * t96 + + t7 + * (-t126 + * (3 * t139 * t1489 * t2077 * t2601 * t35 + - t139 * t4161 + t145 * t15 * t3 * t4164 + - t16 * t4165 - t16 * t4166 - t4195) + + t150 + * (3 * t118 * t1571 * t2129 * t2619 * t35 + - t118 * t4177 + t125 * t15 * t3 * t4179 + - t16 * t4180 - t16 * t4181 - t4195) + + t152 * t4193 - t153 * t4190 - t2046 * t2636 + + t2047 * t2635 - t4159 * t789 + t4160 * t787)); + hess[110] = t3585; + hess[111] = t3950; + hess[112] = t154 + * (-t174 * t4241 + - t222 + * (-t101 + * (t1628 * t4224 + t1718 * t4254 - t2723 * t4255 + + t4251 - t4253 * t52) + - t102 + * (t1748 * t4246 + t2719 * t4204 - t4245 * t91 + t4256 + - 2 * t4257 * t756) + + 2 * t4242 * t795 - 2 * t4243 * t798 + + t55 + * (t1611 * t4204 + t1675 * t4246 - t2723 * t750 * t756 + - t4245 * t89 + t4251) + + t96 + * (t1744 * t4254 + t2716 * t4224 - t2732 * t4255 + - t4253 * t48 + t4256)) + + t3 + * (t126 + * (3 * t134 * t1489 * t4211 * t59 - t134 * t4207 + + t145 * t4210 * t465 * t7 - t1538 * t4212 - t4221) + + t146 * t4236 + - t150 + * (3 * t123 * t1571 * t4231 * t59 - t123 * t4227 + + t125 * t4230 * t465 * t7 - t1538 * t4232 - t4221) + - t151 * t4238 + t4199 * t760 - t4200 * t779) + + 2 * t4196 + 2 * t4197 + t4198 + t4241 + + t7 + * (-t126 + * (3 * t139 * t1489 * t4211 * t59 - t139 * t4207 + + t145 * t3 * t4210 * t465 - t3427 * t4212 - t4240) + + t150 + * (3 * t118 * t1571 * t4231 * t59 - t118 * t4227 + + t125 * t3 * t4230 * t465 - t3427 * t4232 - t4240) + + t152 * t4238 - t153 * t4236 - t4199 * t789 + + t4200 * t787)); + hess[113] = t4311; + hess[114] = t154 + * (t1 * t35 * (t2361 * t4321 + t2401 * t4324 + t4325) + - t1824 * (t2364 * t4324 - t405 * t4330 + t4332) + - t2736 * (t2364 * t4321 + t411 * t4330 + t4333) - t4334); + hess[115] = t154 + * (t1 * t15 * t2418 * t361 * t7 + + t1 * t35 * (t2401 * t4343 - t361 * t4350 * t96 - t4352) + + t12 * t15 * t2416 * t54 + - t1824 * (t2364 * t4343 - t405 * t4346 + t4347) - t2417 + - t2736 * (-t2364 * t4350 + t411 * t4346 - t4351) - t4335); + hess[116] = t154 + * (-t1824 * (-t2364 * t4359 - t405 * t4366 - t4367) + + t222 * (-t2401 * t4359 - t361 * t4361 * t96 - t4362) + - t2736 * (-t2364 * t4361 + t411 * t4366 + t4368) + t4369); + hess[117] = t154 + * (-t207 + * (-t2476 * (t3 * t3158 * t4384 + t4329 + t4392) + + t2531 * t4389 + t4393) + - t222 + * (t2476 * (t4384 * t4394 + t4396) - t2514 * t4389 + t4397) + + t4398 + + t7 + * (t126 * t95 + * (t3 * t35 * t750 * t942 * t948 + + t35 * t57 * t755 * t948 - t4372 * t4377 - t4376 + - t516) + - t3733 + * (t1 * t15 * t3 * t35 * t755 * t948 - t2563 + + t35 * t780 * t942 * t948 - t3723 * t4372 - t4375) + - t4379)); + hess[118] = t154 + * (-t222 + * (t102 * t404 * t4407 + - t2514 * (t306 * t4402 * t957 - t4342 - t4405) - t4408) + - t227 + * (t2514 * (t3158 * t4402 + t4345 + t4409) - t2531 * t4407 + + t4410) + + t3 + * (t151 * t95 + * (t164 * t35 * t755 * t948 - t1740 * t4400 + + t35 * t780 * t948 * t975 - t4147 - t4399 * t996) + - t2046 * t2520 + - t2457 + * (t1 * t15 * t35 * t7 * t755 * t948 - t1675 * t4400 + - t2449 * t4399 - t2578 + + t35 * t7 * t750 * t948 * t975) + - t4103 * t779) + - t4411); + hess[119] = t154 + * (-t207 * (-t2476 * t4418 + t2531 * t4416 + t4419) + - t222 * (t102 * t404 * t4421 - t2514 * t4416 - t4422) + - t227 * (t2514 * t4418 - t2531 * t4421 + t4423) - t4424); + hess[120] = t356 + * (-t1 + * (-t101 * t4427 - t102 * t4431 + t4427 * t55 + t4431 * t96 + + t875) + + t3 + * (-t126 * t4428 - t146 * t4429 + t150 * t4428 + t151 * t4429 + - t862) + + t637 + + t7 + * (t126 * t4430 - t150 * t4430 - t152 * t4429 + t153 * t4429 + - t868)); + hess[121] = t356 + * (-t1 + * (-t101 * t4433 - t102 * t4437 + t1222 + t4433 * t55 + + t4437 * t96) + + t1149 + + t3 + * (-t1216 - t126 * t4434 - t146 * t4435 + t150 * t4434 + + t151 * t4435) + + t7 + * (-t1219 + t126 * t4436 - t150 * t4436 - t152 * t4435 + + t153 * t4435)); + hess[122] = t356 + * (-t1 + * (-t101 * t4446 - t102 * t4445 + t1394 * t35 - t1395 * t35 + + t1396 * t35 - t1397 * t35 + t4445 * t96 + t4446 * t55) + + t1340 + + t3 + * (-t126 * t4443 + t1273 * t35 * t859 + t1275 * t35 * t861 + - t1381 * t35 - t1382 * t35 - t146 * t4442 + t150 * t4443 + + t151 * t4442) + + t7 + * (t126 * t4444 + t1273 * t35 * t867 + t1277 * t35 * t857 + - t1391 * t35 - t1392 * t35 - t150 * t4444 - t152 * t4442 + + t153 * t4442)); + hess[123] = t2337; + hess[124] = t3028; + hess[125] = t3643; + hess[126] = t4006; + hess[127] = t4311; + hess[128] = t154 + * (t157 * t4485 + - t222 + * (-t101 + * (t1718 * t4488 - t4004 * t4489 + t4452 * t54 + - t4487 * t52 + t4499) + - t102 + * (t1748 * t4497 + t2719 * t4474 - t4005 * t4498 + + t4494 - t4496 * t91) + + 2 * t4297 * t872 - 2 * t4298 * t874 + + t55 + * (t1675 * t4497 - t3996 * t4498 + t4473 * t95 + - t4496 * t89 + t4499) + + t96 + * (t1744 * t4488 + t2716 * t4453 - t4001 * t4489 + - t4487 * t48 + t4494)) + + t3 + * (t126 + * (3 * t134 * t1489 * t4480 * t59 - t134 * t4477 + + t145 * t15 + * (t22 + * (-t3865 - t4237 - t4478 + + 3 * t7 * t83) + + 2 * t3625 + t3627 + t574) + - t1598 * t2273 * t2282 - t4483) + + t146 * t4470 + - t150 + * (3 * t123 * t1571 * t4459 * t59 - t123 * t4456 + + t125 * t15 + * (t22 + * (-t3862 + 3 * t42 * t7 - t4233 + - t4457) + + 2 * t3630 + t3632 + t574) + - t1598 * t2226 * t2301 - t4483) + - t151 * t4482 + t4447 * t845 - t4448 * t859) + + t3451 - 2 * t3641 + 2 * t3642 - t4485 + + t7 + * (-t126 + * (3 * t139 * t1489 * t4480 * t59 - t139 * t4477 + + t145 * t3 * t4479 * t465 - t3427 * t4481 - t4484) + + t150 + * (3 * t118 * t1571 * t4459 * t59 - t118 * t4456 + + t125 * t3 * t4458 * t465 - t3427 * t4460 - t4484) + + t152 * t4482 - t153 * t4470 - t4447 * t867 + + t4448 * t866)); + hess[129] = t154 + * (t1 * t35 * (t2361 * t4510 - t2401 * t4515 + t4516) + - t1824 * (-t2364 * t4515 - t405 * t4519 + t4521) + - t2736 * (t2364 * t4510 + t411 * t4519 + t4522) - t4523); + hess[130] = t154 + * (t1 * t35 * (-t2401 * t4531 - t361 * t4533 * t96 - t4534) + - t1824 * (-t2364 * t4531 - t405 * t4536 + t4537) + - t2736 * (-t2364 * t4533 + t411 * t4536 - t4538) - t4539); + hess[131] = t154 + * (t1 * t35 * (-t2401 * t4552 - t361 * t4548 * t96 - t4554) + - t1824 * (-t2364 * t4552 - t405 * t4545 - t4553) + t2423 + - t2736 * (-t2364 * t4548 + t411 * t4545 + t4549) - t3711 - t3712 + + t3713); + hess[132] = t154 + * (-t207 + * (-t2476 * (t3 * t304 * t4568 * t957 - t4576) + + t2531 * t4573 + t4577) + - t222 + * (t2476 * (t4394 * t4568 + t4578) - t2514 * t4573 + t4579) + - t3748 + + t7 + * (t126 * t95 + * (t3 * t35 * t839 * t942 * t948 + + t35 * t57 * t841 * t948 - t4377 * t4561 - t4562 + - t565) + - t3733 + * (t1 * t15 * t3 * t35 * t841 * t948 + + t1 * t35 * t839 * t942 * t948 - t3723 * t4561 + - t4557 - t484) + - t4563)); + hess[133] = t154 + * (-t222 + * (t102 * t404 * t4593 + - t2514 * (t1 * t4588 * t993 + t4530 + t4590) - t4594) + - t227 + * (t2514 * (t4116 * t4588 + t4595) - t2531 * t4593 + t4596) + + t3 + * (t151 * t95 + * (t1 * t35 * t839 * t948 * t975 - t1140 + + t164 * t35 * t841 * t948 - t4582 * t996 - t4585) + - t2457 + * (t1 * t15 * t35 * t7 * t841 * t948 - t2449 * t4582 + - t3198 + t35 * t838 * t948 * t975 - t4584) + - t4586) + + t3767); + hess[134] = t154 + * (-t207 * (-t2476 * t4603 + t2531 * t4600 + t4604) + - t222 * (t102 * t404 * t4606 - t2514 * t4600 - t4607) + - t227 * (t2514 * t4603 - t2531 * t4606 + t4608) - t3781); + hess[135] = t898; + hess[136] = t1224; + hess[137] = t1401; + hess[138] = t154 + * (t1 * (t2361 * t4610 + t2363 + t2401 * t4612) - t2383 + - t890 * (t2364 * t4612 + t2375 - t405 * t4614) + - t933 * (t2364 * t4610 + t2376 + t411 * t4614)); + hess[139] = t154 + * (t1 + * (t146 * t54 * (-t2128 * t4615 * t892 + t3042) + - t3040 * (-t3034 * t4615 + t3039) - t3047) + - t3 + * (-t3059 * (t282 * t3 * t35 * t4617 * t880 - t3058) - t3060 + + t361 * t4619 * t96) + + t3066 + - t7 + * (-t2401 * t4619 + t3059 * (t3061 * t4617 + t3063) + + t3064)); + hess[140] = t154 + * (t1 * (t2361 * t4622 + t2401 * t4624 + t3659) - t3677 + - t890 * (t2364 * t4624 + t3672 - t405 * t4625) + - t933 * (t2364 * t4622 + t3673 + t411 * t4625)); + hess[141] = t154 + * (t1 * t35 * (t2361 * t4628 + t2401 * t4629 + t4020) + - t1824 * (t2364 * t4629 + t4029 - t405 * t4630) + - t2736 * (t2364 * t4628 + t4030 + t411 * t4630) - t4031); + hess[142] = t154 + * (t1 * t35 * (t2361 * t4634 + t2401 * t4635 + t4325) + - t1824 * (t2364 * t4635 - t405 * t4637 + t4332) + - t2736 * (t2364 * t4634 + t411 * t4637 + t4333) - t4334); + hess[143] = t154 + * (t1 * t35 * (t2361 * t4642 - t2401 * t4644 + t4516) + - t1824 * (-t2364 * t4644 - t405 * t4645 + t4521) + - t2736 * (t2364 * t4642 + t411 * t4645 + t4522) - t4523); + hess[144] = t4654 + * (t1 * (t101 * t4648 + t4650 * t96) + - t3 * (t2364 * t4648 - t405 * t4653) + - t7 * (t2364 * t4650 + t411 * t4653)); + hess[145] = t4662; + hess[146] = t4669; + hess[147] = t4671; + hess[148] = t4674; + hess[149] = t4675; + hess[150] = t920; + hess[151] = t1226; + hess[152] = t1403; + hess[153] = t154 + * (t1 * (t2401 * t4678 - t2403 - t361 * t4679 * t96) - t2420 + - t890 * (t2364 * t4678 + t2410 - t405 * t4680) + - t933 * (-t2364 * t4679 - t2414 + t411 * t4680)); + hess[154] = t154 + * (t1 * t15 * t3 * t3071 + - t1 + * (-t2361 * t4686 + t2401 * t4685 + t2402 * t503 + - t4681 * t523) + + t125 * t3067 - t3 * (t2361 * t4687 - t3059 * t4685 + t3090) + - t3069 - t3072 + - t7 * (-t2401 * t4687 - t3089 + t361 * t4686 * t496)); + hess[155] = t154 + * (t1 + * (t2401 * t4691 - t361 * t4689 * t96 - t3678 + + t54 * t604 * t919) + - t3696 - t890 * (t2364 * t4691 + t3691 - t405 * t4692) + - t933 * (-t2364 * t4689 - t3692 + t411 * t4692)); + hess[156] = t154 + * (-t1824 * (t2364 * t4696 - t405 * t4700 + t4051) + + t222 * (t2401 * t4696 + t361 * t4699 * t96 - t4045) + t2420 + - t2736 * (t2364 * t4699 - t4052 + t411 * t4700)); + hess[157] = t154 + * (t166 * t2419 + t174 * t2417 + - t1824 * (t2364 * t4705 + t405 * t4703 + t4347) + + t222 * (t2401 * t4705 - t361 * t4704 * t96 - t4352) - t2417 + + t2736 * (t2364 * t4704 + t411 * t4703 + t4351) - t4335); + hess[158] = t154 + * (t1 * t35 * (-t2401 * t4709 - t361 * t4710 * t96 - t4534) + - t1824 * (-t2364 * t4709 - t405 * t4711 + t4537) + - t2736 * (-t2364 * t4710 + t411 * t4711 - t4538) - t4539); + hess[159] = t4662; + hess[160] = t4654 + * (t1 * (t101 * t4713 - t4714 * t96) + - t3 * (t2364 * t4713 - t405 * t4716) + - t7 * (-t2364 * t4714 + t411 * t4716)); + hess[161] = t4720; + hess[162] = t4721; + hess[163] = t4724; + hess[164] = t4725; + hess[165] = t940; + hess[166] = t1227; + hess[167] = t1404; + hess[168] = t154 + * (t1 * (-t2401 * t4727 - t2441 - t361 * t4728 * t96) + t4073 + - t890 * (-t2364 * t4727 - t2438 - t405 * t4729) + - t933 * (-t2364 * t4728 + t2440 + t411 * t4729)); + hess[169] = t154 + * (-t1 * (-t2361 * t4733 + t2401 * t4731 + t3099) + - t3 * (-t3059 * t4731 - t3104 + t361 * t4732 * t96) - t3106 + - t7 + * (-t2401 * t4732 + t3100 * t523 * t54 - t3102 + + t361 * t4733 * t496)); + hess[170] = t154 + * (t1 * (-t2401 * t4735 - t361 * t4736 * t96 - t3705) - t3714 + - t890 * (-t2364 * t4735 - t3708 - t405 * t4737) + - t933 * (-t2364 * t4736 + t3710 + t411 * t4737)); + hess[171] = t154 + * (t1 * t35 * (-t2401 * t4740 + t361 * t4742 * t96 - t4066) + - t1824 * (-t2364 * t4740 - t405 * t4743 - t4071) + - t2736 * (t2364 * t4742 + t4072 + t411 * t4743) - t4073); + hess[172] = t154 + * (-t1824 * (-t2364 * t4745 - t405 * t4748 - t4367) + + t222 * (-t2401 * t4745 - t361 * t4746 * t96 - t4362) + - t2736 * (-t2364 * t4746 + t411 * t4748 + t4368) + t4369); + hess[173] = t154 + * (t1824 * (t2364 * t4751 + t405 * t4753 + t4553) + + t222 * (-t2401 * t4751 - t361 * t4752 * t96 - t4554) + - t2736 * (-t2364 * t4752 + t411 * t4753 + t4549) + t3714); + hess[174] = t4669; + hess[175] = t4720; + hess[176] = t4654 + * (t1 * (-t101 * t4756 - t4757 * t96) + - t3 * (-t2364 * t4756 - t405 * t4758) + - t7 * (-t2364 * t4757 + t411 * t4758)); + hess[177] = t4760; + hess[178] = t4761; + hess[179] = t4763; + hess[180] = t963; + hess[181] = t1229; + hess[182] = t1406; + hess[183] = t154 + * (-t1 * (t2476 * (t2475 + t4764 * t958) + t2478 - t2514 * t4766) + + t2479 + t2481 - t2482 - t2486 + - t3 + * (-t2476 + * (-t1742 * t2471 + 3 * t2472 * t304 * t314 * t956 + + t304 * t4764 * t957 - t4767) + + t2531 * t4766 + t2534 * t951 - t333 * t4092) + + t7 + * (t126 * t95 + * (t2355 + t2453 * t91 - t2461 + t313 * t949 + + t4768 * t976) + - t2466 - t3733 * (-t1043 + t2459 + t4768 * t996))); + hess[184] = t154 + * (t3 * (t102 * t404 * t4773 + t2531 * t4771 - t3127) + - t3134 * (t3133 + t362 * t4771 + t412 * t4774) - t3136 + + t7 * (-t2514 * t4773 + t2531 * t4774 + t3131)); + hess[185] = t154 + * (-t1 * (t2476 * (t3739 * t4778 + t3742) - t2514 * t4780 + t3744) + + t2483 + t2484 + - t3 + * (-t2476 + * (-t1899 * t2471 + 3 * t2472 * t304 * t599 * t956 + + t3 * t304 * t35 * t4778 * t957 - t3752) + + t2531 * t4780 + t3126 * t605 - t4092 * t623) + - t3745 + t3746 - t3747 + + t7 + * (t126 * t95 * (t3734 - t4377 * t4776) + - t3733 * (-t3723 * t4776 + t3728) - t3735)); + hess[186] = t154 + * (-t207 + * (-t2476 * (t304 * t4782 * t957 - t4091 - t688) + + t2531 * t4783 + t4093) + - t222 + * (t2476 * (t4096 + t4626 + t4782 * t958) - t2514 * t4783 + + t4097) + - t2487 + + t35 * t7 + * (t126 * t95 + * (-t4082 * t91 + t4781 * t976 + t4784 * t57 + + t702 * t949 - t704) + - t2464 * t680 + - t3733 + * (t1 * t639 * t949 - t4082 * t93 + t4781 * t996 + + t4784 * t64 - t688) + - t4075)); + hess[187] = t154 + * (-t207 + * (-t2476 * (t3 * t3158 * t4788 + t4392 + t4636) + + t2531 * t4789 + t4393) + - t222 + * (t2476 * (t4394 * t4788 + t4396) - t2514 * t4789 + t4397) + + t4398 + + t7 + * (t126 * t95 + * (t3798 + t4257 * t949 - t4376 + t4377 * t4786 + + t474 * t4785) + - t3733 + * (t3723 * t4786 + t4370 * t949 - t4375 + t4785 * t493 + - t743) + - t4379)); + hess[188] = t154 + * (-t207 + * (-t2476 * (t3 * t304 * t4794 * t957 - t4576) + + t2531 * t4795 + t4577) + - t222 + * (t2476 * (t4394 * t4794 + t4578) - t2514 * t4795 + t4579) + - t3748 + + t7 + * (t126 * t95 + * (t4005 * t949 + t4377 * t4792 - t4562 + t474 * t4790 + + t629) + - t3733 + * (t222 * t839 * t949 + t3723 * t4792 - t4557 + + t4790 * t493 + t617) + - t4563)); + hess[189] = t4671; + hess[190] = t4721; + hess[191] = t4760; + hess[192] = -t4802 + * (t1 * (t102 * t4801 - t4798 * t55) + + t3 * (-t102 * t4800 + t462 * t4798) + + t7 * (-t462 * t4801 + t4800 * t55)); + hess[193] = t4810; + hess[194] = t4816; + hess[195] = t980; + hess[196] = t1231; + hess[197] = t1407; + hess[198] = t154 + * (-t1 + * (t102 * t404 * t4820 + - t2514 * (t1 * t306 * t35 * t4819 * t957 - t2513) + - t2516) + + t2525 + + t3 + * (t151 * t95 * (t2500 - t4817 * t996) + - t2457 * (-t2449 * t4817 + t2499) - t2504) + - t7 + * (t2514 * (t2517 * t4819 + t2518) + t2519 - t2531 * t4820)); + hess[199] = t154 + * (t174 * t3138 + t3 * (t2476 * t4825 + t2531 * t4823 + t3163) + - t3134 * (t3151 + t362 * t4823 - t412 * t4824) - t3138 - t3140 + + t3141 + t7 * (-t2514 * t4825 - t3161 - t404 * t462 * t4824)); + hess[200] = t154 + * (-t1 + * (t102 * t404 * t4827 + - t2514 + * (t1 * t306 * t35 * t4826 * t957 + + 3 * t2472 * t306 * t599 * t967 - t4828 + - t596 * t968) + + t2515 * t588 * t95 - t3749) + + t3 + * (t151 * t95 * (t3765 - t4829 * t996) + - t2457 * (-t1125 - t2449 * t4829 + t3755 + t3759 - t3760) + - t3766) + - t3767 + - t7 + * (t2514 * (t2517 * t4826 + t3763) - t2531 * t4827 + t3764)); + hess[201] = t154 + * (-t222 + * (t102 * t404 * t4834 + - t2514 * (t1 * t306 * t4833 * t957 - t4111) - t4115) + - t227 + * (t2514 * (t4116 * t4833 + t4118) - t2531 * t4834 + t4119) + - t2525 + + t3 + * (t151 * t95 + * (t1030 * t4784 + t1071 - t4102 + t4830 * t996 + + t4831 * t639) + - t2457 + * (t1040 * t4784 + t2449 * t4830 + t2498 * t3873 + - t4099 + t617) + - t4104)); + hess[202] = t154 + * (-t222 + * (t102 * t404 * t4837 + - t2514 * (-t1183 + t306 * t4836 * t957 - t4405) - t4408) + - t227 + * (t2514 * (t3158 * t4836 + t4409 + t4701) - t2531 * t4837 + + t4410) + + t3 * t35 + * (t151 * t95 + * (-t1189 + t164 * t4785 + t2498 * t780 - t4400 * t93 + + t4835 * t996) + - t2457 + * (-t1183 + t166 * t4785 + t2449 * t4835 + + t2498 * t7 * t750 - t4400 * t89) + - t2502 * t779 - t2520 * t776) + - t4411); + hess[203] = t154 + * (-t222 + * (t102 * t404 * t4840 + - t2514 * (t1 * t4839 * t993 + t4590 + t4706) - t4594) + - t227 + * (t2514 * (t4116 * t4839 + t4595) - t2531 * t4840 + t4596) + + t3 + * (t151 * t95 + * (t1030 * t4790 + t1141 - t4585 + t4831 * t839 + + t4838 * t996) + - t2457 + * (t1040 * t4790 - t1204 + t2449 * t4838 + + t2498 * t3996 - t4584) + - t4586) + + t3767); + hess[204] = t4674; + hess[205] = t4724; + hess[206] = t4761; + hess[207] = t4810; + hess[208] = -t4802 + * (t1 * (t102 * t4844 - t4843 * t55) + + t3 * (-t102 * t4845 + t462 * t4843) + + t7 * (-t462 * t4844 + t4845 * t55)); + hess[209] = t4852; + hess[210] = t999; + hess[211] = t1233; + hess[212] = t1409; + hess[213] = t154 + * (-t1 * (t102 * t404 * t4856 - t2514 * t4854 - t2540) - t2551 + - t3 * (-t2476 * t4855 + t2531 * t4854 + t2535) + - t7 * (t2514 * t4855 - t2531 * t4856 + t2539)); + hess[214] = t154 + * (t3 * (t102 * t404 * t4860 - t2531 * t4859 - t3182) + - t3134 * (-t3177 - t362 * t4859 - t412 * t4858) - t3166 + t3167 + - t3168 + t3169 + + t7 + * (-t2514 * t4860 - t3179 - t404 * t462 * t4858 + + t522 * t95 * t997)); + hess[215] = t154 + * (-t1 * (t102 * t404 * t4863 - t2514 * t4864 - t3775) + t2549 + - t3 * (-t2476 * t4862 + t2531 * t4864 + t3777) + t3778 - t3779 + - t3780 + - t7 + * (t2514 * t4862 - t2531 * t4863 + t2538 * t623 + - t3178 * t588)); + hess[216] = t154 + * (-t207 * (-t2476 * t4867 + t2531 * t4866 + t4129) + - t222 * (t102 * t404 * t4868 - t2514 * t4866 - t4132) + - t227 * (t2514 * t4867 - t2531 * t4868 + t4133) - t4134); + hess[217] = t154 + * (-t207 * (-t2476 * t4871 + t2531 * t4870 + t4419) + - t222 * (t102 * t404 * t4872 - t2514 * t4870 - t4422) + - t227 * (t2514 * t4871 - t2531 * t4872 + t4423) - t4424); + hess[218] = t154 + * (-t207 * (-t2476 * t4875 + t2531 * t4874 + t4604) + - t222 * (t102 * t404 * t4876 - t2514 * t4874 - t4607) + - t227 * (t2514 * t4875 - t2531 * t4876 + t4608) - t3781); + hess[219] = t4675; + hess[220] = t4725; + hess[221] = t4763; + hess[222] = t4816; + hess[223] = t4852; + hess[224] = -t4802 + * (t1 * (t102 * t4879 - t4880 * t55) + + t3 * (-t102 * t4878 + t462 * t4880) + + t7 * (-t462 * t4879 + t4878 * t55)); + } + +} // namespace autogen +} // namespace ipc diff --git a/src/ipc/smooth_contact/primitives/autogen.hpp b/src/ipc/smooth_contact/primitives/autogen.hpp new file mode 100644 index 000000000..a9a54a491 --- /dev/null +++ b/src/ipc/smooth_contact/primitives/autogen.hpp @@ -0,0 +1,42 @@ +#pragma once + +namespace ipc { +namespace autogen { + + void edge_normal_term_gradient( + double d_x, + double d_y, + double d_z, + double e0_x, + double e0_y, + double e0_z, + double e1_x, + double e1_y, + double e1_z, + double f0_x, + double f0_y, + double f0_z, + double f1_x, + double f1_y, + double f1_z, + double grad[15]); + // hess is (225×1) flattened in column-major order + void edge_normal_term_hessian( + double d_x, + double d_y, + double d_z, + double e0_x, + double e0_y, + double e0_z, + double e1_x, + double e1_y, + double e1_z, + double f0_x, + double f0_y, + double f0_z, + double f1_x, + double f1_y, + double f1_z, + double hess[225]); +} // namespace autogen +} // namespace ipc diff --git a/src/ipc/smooth_contact/primitives/edge.cpp b/src/ipc/smooth_contact/primitives/edge.cpp new file mode 100644 index 000000000..8bf800454 --- /dev/null +++ b/src/ipc/smooth_contact/primitives/edge.cpp @@ -0,0 +1,70 @@ +#include "edge.hpp" + +#include + +namespace ipc { + +template +Edge::Edge( + const index_t id, + const CollisionMesh& mesh, + const Eigen::MatrixXd& vertices, + const VectorMax3d& d, + const SmoothContactParameters& params) + : Primitive(id, params) +{ + m_vertex_ids = { { mesh.edges()(id, 0), mesh.edges()(id, 1) } }; + + m_is_active = (mesh.is_orient_vertex(m_vertex_ids[0]) + && mesh.is_orient_vertex(m_vertex_ids[1])) + || Math::cross2( + d, vertices.row(m_vertex_ids[1]) - vertices.row(m_vertex_ids[0])) + > 0; +} + +template <> int Edge<2>::n_vertices() const { return N_EDGE_NEIGHBORS_2D; } +template <> int Edge<3>::n_vertices() const { return N_EDGE_NEIGHBORS_3D; } + +template <> +double +Edge<2>::potential(Eigen::ConstRef d, Eigen::ConstRef x) const +{ + return (x.tail<2>() - x.head<2>()).norm(); +} + +template <> +Vector6d +Edge<2>::grad(Eigen::ConstRef d, Eigen::ConstRef x) const +{ + const Eigen::Vector2d t = x.tail<2>() - x.head<2>(); + const double len = t.norm(); + Vector6d g; + g.setZero(); + g.segment<2>(2) = -t / len; + g.segment<2>(4) = t / len; + return g; +} + +template <> +Matrix6d +Edge<2>::hessian(Eigen::ConstRef d, Eigen::ConstRef x) const +{ + Matrix6d h; + h.setZero(); +#ifdef IPC_TOOLKIT_DEBUG_AUTODIFF + ScalarBase::setVariableCount(4); + using T = ADHessian<4>; + auto xAD = slice_positions(x); + h.block<4, 4>(2, 2) = (xAD.row(0) - xAD.row(1)).norm().Hess; +#else + const Eigen::Vector2d t = x.tail<2>() - x.head<2>(); + const double norm = t.norm(); + h.block<2, 2>(2, 2) = + (Matrix2d::Identity() - t * (1. / norm / norm) * t.transpose()) / norm; + h.block<2, 2>(4, 4) = h.block<2, 2>(2, 2); + h.block<2, 2>(2, 4) = -h.block<2, 2>(2, 2); + h.block<2, 2>(4, 2) = -h.block<2, 2>(2, 2); +#endif + return h; +} +} // namespace ipc diff --git a/src/ipc/smooth_contact/primitives/edge.hpp b/src/ipc/smooth_contact/primitives/edge.hpp new file mode 100644 index 000000000..d2878829d --- /dev/null +++ b/src/ipc/smooth_contact/primitives/edge.hpp @@ -0,0 +1,53 @@ +#pragma once + +#include "primitive.hpp" + +namespace ipc { + +template class Edge : public Primitive { +public: + static constexpr int N_CORE_POINTS = 2; + using DVector = Eigen::Vector; + using XVector = Eigen::Vector; + using GradType = Eigen::Vector; + static constexpr int HESSIAN_ROWS = DIM == 2 ? 6 : 15; + static constexpr int HESSIAN_COLS = HESSIAN_ROWS; + using HessianType = Eigen::Matrix; + + // d is a vector from closest point on the edge to the point outside of the + // edge + Edge( + const index_t id, + const CollisionMesh& mesh, + const Eigen::MatrixXd& vertices, + const VectorMax3d& d, + const SmoothContactParameters& params); + + int n_vertices() const override; + int n_dofs() const override { return n_vertices() * DIM; } + + /// @brief Compute the product of local minimum and exteior direction terms of this edge + /// @param d Vector from closest point on the edge to the point outside of the edge + /// @param x Positions of the two vertices of this edge + /// @return Value of the product + double + potential(Eigen::ConstRef d, Eigen::ConstRef x) const; + + /// @brief Compute the gradient of potential wrt. d and x + /// @param d Vector from closest point on the edge to the point outside of the edge + /// @param x Positions of the two vertices of this edge + /// @return Gradient of the potential wrt. d and x + GradType grad(Eigen::ConstRef d, Eigen::ConstRef x) const; + + /// @brief Compute the Hessian of potential wrt. d and x + /// @param d Vector from closest point on the edge to the point outside of the edge + /// @param x Positions of the two vertices of this edge + /// @return Hessian of the potential wrt. d and x + HessianType + hessian(Eigen::ConstRef d, Eigen::ConstRef x) const; +}; + +using Edge2 = Edge<2>; +using Edge3 = Edge<3>; + +} // namespace ipc diff --git a/src/ipc/smooth_contact/primitives/edge2.cpp b/src/ipc/smooth_contact/primitives/edge2.cpp new file mode 100644 index 000000000..83fbc19a1 --- /dev/null +++ b/src/ipc/smooth_contact/primitives/edge2.cpp @@ -0,0 +1,71 @@ +#include "edge2.hpp" + +#include + +namespace ipc { +Edge2::Edge2( + const index_t id, + const CollisionMesh& mesh, + const Eigen::MatrixXd& vertices, + const VectorMax3d& d, + const SmoothContactParameters& params) + : Primitive(id, params) +{ + m_vertex_ids = { { mesh.edges()(id, 0), mesh.edges()(id, 1) } }; + + m_is_active = (mesh.is_orient_vertex(m_vertex_ids[0]) + && mesh.is_orient_vertex(m_vertex_ids[1])) + || Math::cross2( + d, vertices.row(m_vertex_ids[1]) - vertices.row(m_vertex_ids[0])) + > 0; +} + +int Edge2::n_vertices() const { return N_EDGE_NEIGHBORS_2D; } + +double Edge2::potential( + Eigen::ConstRef d, + Eigen::ConstRef x) const +{ + assert(m_is_active); + return (x.tail<2>() - x.head<2>()).norm(); +} + +Vector6d Edge2::grad( + Eigen::ConstRef d, + Eigen::ConstRef x) const +{ + assert(m_is_active); + const Eigen::Vector2d t = x.tail<2>() - x.head<2>(); + const double len = t.norm(); + Vector6d g; + g.setZero(); + g.segment<2>(2) = -t / len; + g.segment<2>(4) = t / len; + return g; +} + +Matrix6d Edge2::hessian( + Eigen::ConstRef d, + Eigen::ConstRef x) const +{ + assert(m_is_active); + Matrix6d h; + h.setZero(); +#ifdef IPC_TOOLKIT_DEBUG_AUTODIFF + ScalarBase::setVariableCount(4); + using T = ADHessian<4>; + auto xAD = slice_positions(x); + h.block<4, 4>(2, 2) = (xAD.row(0) - xAD.row(1)).norm().Hess; +#else + const Eigen::Vector2d t = x.tail<2>() - x.head<2>(); + const double norm = t.norm(); + h.block<2, 2>(2, 2) = + (Eigen::Matrix2d::Identity() - t * (1. / norm / norm) * t.transpose()) + / norm; + h.block<2, 2>(4, 4) = h.block<2, 2>(2, 2); + h.block<2, 2>(2, 4) = -h.block<2, 2>(2, 2); + h.block<2, 2>(4, 2) = -h.block<2, 2>(2, 2); +#endif + return h; +} +} // namespace ipc diff --git a/src/ipc/smooth_contact/primitives/edge2.hpp b/src/ipc/smooth_contact/primitives/edge2.hpp new file mode 100644 index 000000000..aa8dc217a --- /dev/null +++ b/src/ipc/smooth_contact/primitives/edge2.hpp @@ -0,0 +1,34 @@ +#pragma once + +#include "primitive.hpp" + +#include + +namespace ipc { +class Edge2 : public Primitive { +public: + static constexpr int N_CORE_POINTS = 2; + static constexpr int DIM = 2; + // d is a vector from closest point on the edge to the point outside of the + // edge + Edge2( + const index_t id, + const CollisionMesh& mesh, + const Eigen::MatrixXd& vertices, + const VectorMax3d& d, + const SmoothContactParameters& params); + + int n_vertices() const override; + int n_dofs() const override { return n_vertices() * DIM; } + + double potential( + Eigen::ConstRef d, + Eigen::ConstRef x) const; + Vector6d grad( + Eigen::ConstRef d, + Eigen::ConstRef x) const; + Matrix6d hessian( + Eigen::ConstRef d, + Eigen::ConstRef x) const; +}; +} // namespace ipc diff --git a/src/ipc/smooth_contact/primitives/edge3.cpp b/src/ipc/smooth_contact/primitives/edge3.cpp new file mode 100644 index 000000000..53c20974f --- /dev/null +++ b/src/ipc/smooth_contact/primitives/edge3.cpp @@ -0,0 +1,669 @@ +#include "edge3.hpp" + +#include +#include +#include + +namespace ipc { +namespace { + + template + Eigen::Vector3 project( + Eigen::ConstRef> a, + Eigen::ConstRef> b) + { + return a - a.dot(b) * b; + } + + bool smooth_edge3_term_type( + Eigen::ConstRef dn, + Eigen::ConstRef e0, + Eigen::ConstRef e1, + Eigen::ConstRef f0, + Eigen::ConstRef f1, + const SmoothContactParameters& params, + OrientationTypes& otypes, + const bool orientable) + { + otypes.set_size(2); + + const Eigen::Vector3d t0 = + PointEdgeDistance::point_line_closest_point_direction( + f0, e0, e1) + .normalized(); + const Eigen::Vector3d t1 = + PointEdgeDistance::point_line_closest_point_direction( + f1, e0, e1) + .normalized(); + + { + otypes.tangent_type(0) = OrientationTypes::compute_type( + -dn.dot(t0), params.alpha_t, params.beta_t); + otypes.tangent_type(1) = OrientationTypes::compute_type( + -dn.dot(t1), params.alpha_t, params.beta_t); + if (otypes.tangent_type(0) == HeavisideType::ZERO + || otypes.tangent_type(1) == HeavisideType::ZERO) { + return false; + } + } + + if (orientable) { + const Eigen::Vector3d edge = (e0 - e1).normalized(); + const Eigen::Vector3d d = project(dn, edge).normalized(); + otypes.normal_type(0) = OrientationTypes::compute_type( + (d - t0).cross(d - t1).dot(edge), params.alpha_n, + params.beta_n); + otypes.normal_type(1) = otypes.normal_type(0); + } + + return true; + } + + double smooth_edge3_tangent_term( + Eigen::ConstRef dn, + Eigen::ConstRef e0, + Eigen::ConstRef e1, + Eigen::ConstRef f0, + Eigen::ConstRef f1, + const double alpha, + const double beta, + const OrientationTypes& otypes) + { + double tangent_term = 1.; + if (otypes.tangent_type(0) != HeavisideType::ONE) { + const Eigen::Vector3d t0 = PointEdgeDistance< + double, 3>::point_line_closest_point_direction(f0, e0, e1); + tangent_term *= opposite_direction_penalty(t0, -dn, alpha, beta); + } + if (otypes.tangent_type(1) != HeavisideType::ONE) { + const Eigen::Vector3d t1 = PointEdgeDistance< + double, 3>::point_line_closest_point_direction(f1, e0, e1); + tangent_term *= opposite_direction_penalty(t1, -dn, alpha, beta); + } + + return tangent_term; + } + + GradType<15> smooth_edge3_tangent_term_gradient( + Eigen::ConstRef dn, + Eigen::ConstRef e0, + Eigen::ConstRef e1, + Eigen::ConstRef f0, + Eigen::ConstRef f1, + const double alpha, + const double beta, + const OrientationTypes& otypes) + { + Eigen::Vector2d vals; + vals << 1., 1.; + std::array, 2> grads; + for (auto& g : grads) { + g.setZero(); + } + + for (int d : { 0, 1 }) { + const Eigen::Ref f = d == 0 ? f0 : f1; + if (otypes.tangent_type(d) != HeavisideType::ONE) { + const auto [t, g] = PointEdgeDistanceDerivatives< + 3>::point_line_closest_point_direction_grad(f, e0, e1); + const auto [tmp_val, tmp_grad] = + opposite_direction_penalty_grad(t, -dn, alpha, beta); + + vals[d] = tmp_val; + + Vector gradient_tmp; + gradient_tmp << -tmp_grad.tail<3>(), + g.transpose() * tmp_grad.head<3>(); + + Vector indices; + indices << 0, 1, 2, 9, 10, 11, 3, 4, 5, 6, 7, 8; + if (d == 1) { + indices.segment<3>(3).array() += 3; + } + + grads[d](indices) = gradient_tmp; + } + } + + return std::make_tuple( + vals.prod(), vals[0] * grads[1] + vals[1] * grads[0]); + } + + HessianType<15> smooth_edge3_tangent_term_hessian( + Eigen::ConstRef dn, + Eigen::ConstRef e0, + Eigen::ConstRef e1, + Eigen::ConstRef f0, + Eigen::ConstRef f1, + const double alpha, + const double beta, + const OrientationTypes& otypes) + { + Eigen::Vector2d vals; + vals << 1., 1.; + std::array, 2> grads; + std::array, 2> hesses; + for (auto& g : grads) { + g.setZero(); + } + for (auto& h : hesses) { + h.setZero(); + } + + for (int d : { 0, 1 }) { + const Eigen::Ref f = d == 0 ? f0 : f1; + if (otypes.tangent_type(d) != HeavisideType::ONE) { + const auto [t, g, h] = PointEdgeDistanceDerivatives< + 3>::point_line_closest_point_direction_hessian(f, e0, e1); + auto [tmp_val, tmp_grad, tmp_hess] = + opposite_direction_penalty_hess(t, -dn, alpha, beta); + tmp_grad.tail<3>() *= -1; + tmp_hess.rightCols<3>() *= -1; + tmp_hess.bottomRows<3>() *= -1; + + vals[d] = tmp_val; + + Vector gradient_tmp; + gradient_tmp << tmp_grad.tail<3>(), + g.transpose() * tmp_grad.head<3>(); + + Eigen::Matrix hessian_tmp; + // dn + hessian_tmp.block<3, 3>(0, 0) = tmp_hess.block<3, 3>(3, 3); + // f, e0, e1 + hessian_tmp.block<9, 9>(3, 3) = + g.transpose() * tmp_hess.block<3, 3>(0, 0) * g + + tmp_grad(0) * h[0] + tmp_grad(1) * h[1] + + tmp_grad(2) * h[2]; + + // mixed 2nd derivatives + hessian_tmp.block<3, 9>(0, 3) = tmp_hess.block<3, 3>(3, 0) * g; + hessian_tmp.block<9, 3>(3, 0) = + g.transpose() * tmp_hess.block<3, 3>(0, 3); + + Vector indices; + indices << 0, 1, 2, 9, 10, 11, 3, 4, 5, 6, 7, 8; + if (d == 1) { + indices.segment<3>(3).array() += 3; + } + + hesses[d](indices, indices) = hessian_tmp; + grads[d](indices) = gradient_tmp; + } + } + + return std::make_tuple( + vals.prod(), grads[0] * vals[1] + grads[1] * vals[0], + hesses[0] * vals[1] + hesses[1] * vals[0] + + grads[0] * grads[1].transpose() + + grads[1] * grads[0].transpose()); + } + + double smooth_edge3_term( + Eigen::ConstRef direc, + Eigen::ConstRef e0, + Eigen::ConstRef e1, + Eigen::ConstRef f0, + Eigen::ConstRef f1, + const SmoothContactParameters& params, + const OrientationTypes& otypes, + const bool orientable) + { + const Eigen::Vector3d dn = direc.normalized(); + double tangent_term = smooth_edge3_tangent_term( + dn, e0, e1, f0, f1, params.alpha_t, params.beta_t, otypes); + double normal_term = !orientable + ? 1 + : smooth_edge3_normal_term( + dn, e0, e1, f0, f1, params.alpha_n, params.beta_n, otypes); + + return (e1 - e0).squaredNorm() * tangent_term * normal_term; + } + + GradType<15> smooth_edge3_term_gradient( + Eigen::ConstRef direc, + Eigen::ConstRef e0, + Eigen::ConstRef e1, + Eigen::ConstRef f0, + Eigen::ConstRef f1, + const SmoothContactParameters& params, + const OrientationTypes& otypes, + const bool orientable) + { + assert(otypes.size() == 2); + + const auto [dn, dn_grad] = normalize_vector_grad(direc); + + auto [t_term, t_grad] = smooth_edge3_tangent_term_gradient( + dn, e0, e1, f0, f1, params.alpha_t, params.beta_t, otypes); + + double n_term = 1.; + Vector15d n_grad = Vector15d::Zero(); + if (orientable) { + std::tie(n_term, n_grad) = smooth_edge3_normal_term_gradient( + dn, e0, e1, f0, f1, params.alpha_n, params.beta_n, otypes); + } + + t_grad.head<3>() = dn_grad * t_grad.head<3>(); + n_grad.head<3>() = dn_grad * n_grad.head<3>(); + + const double weight = (e1 - e0).squaredNorm(); + const double val = weight * t_term * n_term; + + Vector15d gradient = + (weight * t_term) * n_grad + (weight * n_term) * t_grad; + gradient.segment<3>(3) += (2 * t_term * n_term) * (e0 - e1); + gradient.segment<3>(6) += (2 * t_term * n_term) * (e1 - e0); + + return std::make_tuple(val, gradient); + } + + HessianType<15> smooth_edge3_term_hessian( + Eigen::ConstRef direc, + Eigen::ConstRef e0, + Eigen::ConstRef e1, + Eigen::ConstRef f0, + Eigen::ConstRef f1, + const SmoothContactParameters& params, + const OrientationTypes& otypes, + const bool orientable) + { + assert(otypes.size() == 2); + + const auto [dn, dn_grad, dn_hess] = normalize_vector_hess(direc); + + auto [t_term, t_grad, t_hess] = smooth_edge3_tangent_term_hessian( + dn, e0, e1, f0, f1, params.alpha_t, params.beta_t, otypes); + + double n_term = 1.; + Vector15d n_grad = Vector15d::Zero(); + Matrix15d n_hess = Matrix15d::Zero(); + if (orientable) { + std::tie(n_term, n_grad, n_hess) = smooth_edge3_normal_term_hessian( + dn, e0, e1, f0, f1, params.alpha_n, params.beta_n, otypes); + } + + t_hess.topRows<3>() = dn_grad * t_hess.topRows<3>(); + t_hess.leftCols<3>() = t_hess.leftCols<3>() * dn_grad; + t_hess.topLeftCorner<3, 3>() += dn_hess[0] * t_grad(0) + + dn_hess[1] * t_grad(1) + dn_hess[2] * t_grad(2); + t_grad.head<3>() = dn_grad * t_grad.head<3>(); + + n_hess.topRows<3>() = dn_grad * n_hess.topRows<3>(); + n_hess.leftCols<3>() = n_hess.leftCols<3>() * dn_grad; + n_hess.topLeftCorner<3, 3>() += dn_hess[0] * n_grad(0) + + dn_hess[1] * n_grad(1) + dn_hess[2] * n_grad(2); + n_grad.head<3>() = dn_grad * n_grad.head<3>(); + + const double weight = (e1 - e0).squaredNorm(); + + Vector15d gradient = t_term * n_grad + n_term * t_grad; + + Matrix15d hessian = (weight * t_term) * n_hess + + (weight * n_term) * t_hess + + (weight * t_grad) * n_grad.transpose() + + (weight * n_grad) * t_grad.transpose(); + + const double tmp_val = 2 * t_term * n_term; + hessian.block<6, 6>(3, 3).diagonal().array() += tmp_val; + hessian.block<3, 3>(3, 6).diagonal().array() -= tmp_val; + hessian.block<3, 3>(6, 3).diagonal().array() -= tmp_val; + + Vector6d weight_grad; + weight_grad << 2 * (e0 - e1), 2 * (e1 - e0); + hessian.middleCols<6>(3) += gradient * weight_grad.transpose(); + hessian.middleRows<6>(3) += weight_grad * gradient.transpose(); + + gradient *= weight; + gradient.segment<3>(3) += tmp_val * (e0 - e1); + gradient.segment<3>(6) += tmp_val * (e1 - e0); + + return std::make_tuple(weight * t_term * n_term, gradient, hessian); + } + + /// @brief + /// @tparam scalar + /// @param dn from edge to point outside, normalized + /// @param e0 + /// @param e1 + /// @param f0 face [f0, e0, e1] + /// @param f1 face [f1, e1, e0] + /// @param alpha + /// @return + template + scalar smooth_edge3_term_template( + Eigen::ConstRef> dn, + Eigen::ConstRef> e0, + Eigen::ConstRef> e1, + Eigen::ConstRef> f0, + Eigen::ConstRef> f1, + const SmoothContactParameters& params, + const OrientationTypes& otypes, + const bool orientable) + { + scalar tangent_term = scalar(1.); + const Eigen::Vector3 t0 = + PointEdgeDistance::point_line_closest_point_direction( + f0, e0, e1) + .normalized(); + if (otypes.tangent_type(0) != HeavisideType::ONE) { + tangent_term = tangent_term + * Math::smooth_heaviside( + -dn.dot(t0), params.alpha_t, params.beta_t); + } + + const Eigen::Vector3 t1 = + PointEdgeDistance::point_line_closest_point_direction( + f1, e0, e1) + .normalized(); + + if (otypes.tangent_type(1) != HeavisideType::ONE) { + tangent_term = tangent_term + * Math::smooth_heaviside( + -dn.dot(t1), params.alpha_t, params.beta_t); + } + + scalar normal_term = scalar(1.); + if (orientable && otypes.normal_type(0) != HeavisideType::ONE) { + const Eigen::Vector3 edge = (e0 - e1).normalized(); + const Eigen::Vector3 d = + project(dn, edge).normalized(); + normal_term = Math::smooth_heaviside( + (d - t0).cross(d - t1).dot(edge), params.alpha_n, + params.beta_n); + } + + return (e1 - e0).squaredNorm() * tangent_term * normal_term; + } + +} // namespace +// d is a vector from any point on the edge to the point outside of the edge +Edge3::Edge3( + const index_t id, + const CollisionMesh& mesh, + const Eigen::MatrixXd& vertices, + const VectorMax3d& d, + const SmoothContactParameters& params) + : Primitive(id, params) +{ + orientable = + (mesh.is_orient_vertex(mesh.edges()(id, 0)) + && mesh.is_orient_vertex(mesh.edges()(id, 0))); + + std::array neighbors { { -1, -1, -1, -1 } }; + { + std::array lf = { { mesh.edges_to_faces()(id, 0), + mesh.edges_to_faces()(id, 1) } }; + has_neighbor_1 = lf[0] >= 0; + has_neighbor_2 = lf[1] >= 0; + + if (has_neighbor_1) { + const int j = 0; + for (int i = 0; i < 3; i++) { + const auto va = mesh.faces()(lf[j], i); + + if (va != mesh.edges()(id, 0) && va != mesh.edges()(id, 1)) { + neighbors[2 + j] = va; + + // When there is a face adjacent to the edge, we orient the + // edge the same way as in the face + neighbors[0] = mesh.faces()(lf[j], (i + 1) % 3); + neighbors[1] = mesh.faces()(lf[j], (i + 2) % 3); + break; + } + } + assert(neighbors[2] >= 0 && neighbors[0] >= 0 && neighbors[1] >= 0); + } + + if (has_neighbor_2) { + const int j = 1; + for (int i = 0; i < 3; i++) { + const auto va = mesh.faces()(lf[j], i); + + if (va != mesh.edges()(id, 0) && va != mesh.edges()(id, 1)) { + neighbors[2 + j] = va; + + // When has_neighbor_1==false, we orient the edge the same + // way as in the second face. + if (!has_neighbor_1) { + neighbors[0] = mesh.faces()(lf[j], (i + 2) % 3); + neighbors[1] = mesh.faces()(lf[j], (i + 1) % 3); + } else { + assert( + neighbors[0] == mesh.faces()(lf[j], (i + 2) % 3)); + assert( + neighbors[1] == mesh.faces()(lf[j], (i + 1) % 3)); + } + break; + } + } + assert(neighbors[3] >= 0); + } + } + + if (has_neighbor_1 && has_neighbor_2) { + m_vertex_ids = std::vector( + neighbors.begin(), neighbors.begin() + neighbors.size()); + m_is_active = smooth_edge3_term_type( + d.normalized(), vertices.row(m_vertex_ids[0]), + vertices.row(m_vertex_ids[1]), vertices.row(m_vertex_ids[2]), + vertices.row(m_vertex_ids[3]), params, otypes, orientable); + } else if (has_neighbor_1 || has_neighbor_2) { + m_vertex_ids = { { neighbors[0], neighbors[1], + has_neighbor_1 ? neighbors[2] : neighbors[3] } }; + + } else { + m_vertex_ids = { { neighbors[0], neighbors[1] } }; + m_is_active = true; + } +} + +int Edge3::n_vertices() const { return N_EDGE_NEIGHBORS_3D; } + +double Edge3::potential( + Eigen::ConstRef d, Eigen::ConstRef x) const +{ +#ifdef IPC_TOOLKIT_DEBUG_AUTODIFF + return smooth_edge3_term_template( + d.normalized(), x.head<3>(), x.segment<3>(3), x.segment<3>(6), + x.tail<3>(), m_params, otypes, orientable); +#else + return smooth_edge3_term( + d, x.head<3>(), x.segment<3>(3), x.segment<3>(6), x.tail<3>(), m_params, + otypes, orientable); +#endif +} + +Vector15d Edge3::grad( + Eigen::ConstRef d, Eigen::ConstRef x) const +{ +#ifdef IPC_TOOLKIT_DEBUG_AUTODIFF + Vector15d tmp; + tmp << d, x; + ScalarBase::setVariableCount(15); + using T = ADGrad<15>; + auto X = slice_positions(tmp); + return smooth_edge3_term_template( + X.row(0) / X.row(0).norm(), X.row(1), X.row(2), X.row(3), + X.row(4), m_params, otypes, orientable) + .grad; +#else + return std::get<1>(smooth_edge3_term_gradient( + d, x.head<3>(), x.segment<3>(3), x.segment<3>(6), x.tail<3>(), m_params, + otypes, orientable)); +#endif +} + +Matrix15d Edge3::hessian( + Eigen::ConstRef d, Eigen::ConstRef x) const +{ +#ifdef IPC_TOOLKIT_DEBUG_AUTODIFF + Vector15d tmp; + tmp << d, x; + ScalarBase::setVariableCount(15); + using T = ADHessian<15>; + auto X = slice_positions(tmp); + return smooth_edge3_term_template( + X.row(0) / X.row(0).norm(), X.row(1), X.row(2), X.row(3), + X.row(4), m_params, otypes, orientable) + .Hess; +#else + return std::get<2>(smooth_edge3_term_hessian( + d, x.head<3>(), x.segment<3>(3), x.segment<3>(6), x.tail<3>(), m_params, + otypes, orientable)); +#endif +} + +double smooth_edge3_normal_term( + Eigen::ConstRef dn, + Eigen::ConstRef e0, + Eigen::ConstRef e1, + Eigen::ConstRef f0, + Eigen::ConstRef f1, + const double alpha, + const double beta, + const OrientationTypes& otypes) +{ + if (otypes.normal_type(0) == HeavisideType::ONE + || otypes.normal_type(1) == HeavisideType::ONE) { + return 1.; + } + + const Eigen::Vector3d t0 = + PointEdgeDistance::point_line_closest_point_direction( + f0, e0, e1) + .normalized(); + const Eigen::Vector3d t1 = + PointEdgeDistance::point_line_closest_point_direction( + f1, e0, e1) + .normalized(); + const Eigen::Vector3d edge = (e0 - e1).normalized(); + const Eigen::Vector3d d = project(dn, edge).normalized(); + return Math::smooth_heaviside( + (d - t0).cross(d - t1).dot(edge), alpha, beta); +} + +GradType<15> smooth_edge3_normal_term_gradient( + Eigen::ConstRef dn, + Eigen::ConstRef e0, + Eigen::ConstRef e1, + Eigen::ConstRef f0, + Eigen::ConstRef f1, + const double alpha, + const double beta, + const OrientationTypes& otypes) +{ + double val = 1.; + Vector15d gradient = Vector15d::Zero(); + + if (otypes.normal_type(0) == HeavisideType::ONE + || otypes.normal_type(1) == HeavisideType::ONE) { + return std::make_tuple(val, gradient); + } + + { + const auto t0n = + PointEdgeDistance::point_line_closest_point_direction( + f0, e0, e1) + .normalized(); + + const auto t1n = + PointEdgeDistance::point_line_closest_point_direction( + f1, e0, e1) + .normalized(); + const auto edge = (e0 - e1).normalized(); + + const Eigen::Vector3d d = project(dn, edge).normalized(); + val = (d - t0n).cross(d - t1n).dot(edge); + } + + autogen::edge_normal_term_gradient( + dn[0], dn[1], dn[2], e0[0], e0[1], e0[2], e1[0], e1[1], e1[2], f0[0], + f0[1], f0[2], f1[0], f1[1], f1[2], gradient.data()); + + gradient *= Math::smooth_heaviside_grad(val, alpha, beta); + val = Math::smooth_heaviside(val, alpha, beta); + return std::make_tuple(val, gradient); +} + +HessianType<15> smooth_edge3_normal_term_hessian( + Eigen::ConstRef dn, + Eigen::ConstRef e0, + Eigen::ConstRef e1, + Eigen::ConstRef f0, + Eigen::ConstRef f1, + const double alpha, + const double beta, + const OrientationTypes& otypes) +{ + double val = 1.; + Vector15d gradient = Vector15d::Zero(); + Matrix15d hessian = Matrix15d::Zero(); + + if (otypes.normal_type(0) == HeavisideType::ONE + || otypes.normal_type(1) == HeavisideType::ONE) { + return std::make_tuple(val, gradient, hessian); + } + + { + const auto t0n = + PointEdgeDistance::point_line_closest_point_direction( + f0, e0, e1) + .normalized(); + + const auto t1n = + PointEdgeDistance::point_line_closest_point_direction( + f1, e0, e1) + .normalized(); + const auto edge = (e0 - e1).normalized(); + + const Eigen::Vector3d d = project(dn, edge).normalized(); + val = (d - t0n).cross(d - t1n).dot(edge); + } + + autogen::edge_normal_term_gradient( + dn[0], dn[1], dn[2], e0[0], e0[1], e0[2], e1[0], e1[1], e1[2], f0[0], + f0[1], f0[2], f1[0], f1[1], f1[2], gradient.data()); + + autogen::edge_normal_term_hessian( + dn[0], dn[1], dn[2], e0[0], e0[1], e0[2], e1[0], e1[1], e1[2], f0[0], + f0[1], f0[2], f1[0], f1[1], f1[2], hessian.data()); + + const double tmp = Math::smooth_heaviside_grad(val, alpha, beta); + hessian = hessian * tmp + + gradient * Math::smooth_heaviside_hess(val, alpha, beta) + * gradient.transpose(); + gradient *= tmp; + val = Math::smooth_heaviside(val, alpha, beta); + + // verify with autodiff + // { + // ScalarBase::setVariableCount(15); + // using T = ADHessian<15>; + // Vector15d tmp; + // tmp << dn, e0, e1, f0, f1; + // auto X = slice_positions(tmp); + // const Eigen::Vector3 n0 = (X.segment<3>(3) - + // X.segment<3>(9)).cross(X.segment<3>(6) - X.segment<3>(9)); const + // Eigen::Vector3 n1 = -(X.segment<3>(3) - + // X.tail<3>()).cross(X.segment<3>(6) - X.tail<3>()); T normal_term = + // Math::smooth_heaviside( + // (Math::smooth_heaviside(X.head<3>().dot(n0) / n0.norm(), + // alpha, beta) + // + Math::smooth_heaviside(X.head<3>().dot(n1) / n1.norm(), + // alpha, beta) + // - 1), 1., 0); + + // if ((normal_term.Hess - hessian).norm() > 1e-6 * + // hessian.norm()) { + // std::cout << "gradient: \n" << + // normal_term.grad.transpose() << "\n vs \n" << + // gradient.transpose() << std::endl; std::cout << "hessian + // mismatch: \n" << normal_term.Hess << "\n vs \n" << + // hessian << std::endl; + // } + // } + + return std::make_tuple(val, gradient, hessian); +} +} // namespace ipc diff --git a/src/ipc/smooth_contact/primitives/edge3.hpp b/src/ipc/smooth_contact/primitives/edge3.hpp new file mode 100644 index 000000000..c57ddfc30 --- /dev/null +++ b/src/ipc/smooth_contact/primitives/edge3.hpp @@ -0,0 +1,67 @@ +#pragma once + +#include "primitive.hpp" + +#include + +namespace ipc { +class Edge3 : public Primitive { +public: + static constexpr int N_CORE_POINTS = 2; + static constexpr int DIM = 3; + // d is a vector from closest point on the edge to the point outside of the + // edge + Edge3( + const index_t id, + const CollisionMesh& mesh, + const Eigen::MatrixXd& vertices, + const VectorMax3d& d, + const SmoothContactParameters& params); + + int n_vertices() const override; + int n_dofs() const override { return n_vertices() * DIM; } + + double potential( + Eigen::ConstRef d, Eigen::ConstRef x) const; + Vector15d grad( + Eigen::ConstRef d, Eigen::ConstRef x) const; + Matrix15d hessian( + Eigen::ConstRef d, Eigen::ConstRef x) const; + +private: + OrientationTypes otypes; + + bool has_neighbor_1, has_neighbor_2; + bool orientable; +}; + +double smooth_edge3_normal_term( + Eigen::ConstRef dn, + Eigen::ConstRef e0, + Eigen::ConstRef e1, + Eigen::ConstRef f0, + Eigen::ConstRef f1, + const double alpha, + const double beta, + const OrientationTypes& otypes); + +GradType<15> smooth_edge3_normal_term_gradient( + Eigen::ConstRef dn, + Eigen::ConstRef e0, + Eigen::ConstRef e1, + Eigen::ConstRef f0, + Eigen::ConstRef f1, + const double alpha, + const double beta, + const OrientationTypes& otypes); + +HessianType<15> smooth_edge3_normal_term_hessian( + Eigen::ConstRef dn, + Eigen::ConstRef e0, + Eigen::ConstRef e1, + Eigen::ConstRef f0, + Eigen::ConstRef f1, + const double alpha, + const double beta, + const OrientationTypes& otypes); +} // namespace ipc \ No newline at end of file diff --git a/src/ipc/smooth_contact/primitives/face.cpp b/src/ipc/smooth_contact/primitives/face.cpp new file mode 100644 index 000000000..54b4e76fc --- /dev/null +++ b/src/ipc/smooth_contact/primitives/face.cpp @@ -0,0 +1,71 @@ +#include "face.hpp" + +#include + +namespace ipc { +// namespace { +// template +// std::array, 3> double_to_autodiff( +// Eigen::ConstRef v0, +// Eigen::ConstRef v1, +// Eigen::ConstRef v2) +// { +// Eigen::Vector3 v0_, v1_, v2_; +// for (int d = 0; d < 3; d++) +// { +// v0_(d) = T( d, v0(d)); +// v1_(d) = T(3+d, v1(d)); +// v2_(d) = T(6+d, v2(d)); +// } +// return {{v0_, v1_, v2_}}; +// } +// } +Face::Face( + const index_t id, + const CollisionMesh& mesh, + const Eigen::MatrixXd& vertices, + const VectorMax3d& d, + const SmoothContactParameters& params) + : Primitive(id, params) +{ + m_vertex_ids = { { mesh.faces()(id, 0), mesh.faces()(id, 1), + mesh.faces()(id, 2) } }; + Eigen::Vector3d a = + vertices.row(m_vertex_ids[1]) - vertices.row(m_vertex_ids[0]); + Eigen::Vector3d b = + vertices.row(m_vertex_ids[2]) - vertices.row(m_vertex_ids[0]); + + bool orientable = mesh.is_orient_vertex(m_vertex_ids[0]) + && mesh.is_orient_vertex(m_vertex_ids[1]) + && mesh.is_orient_vertex(m_vertex_ids[2]); + m_is_active = !orientable || a.cross(b).dot(d) > 0; +} +int Face::n_vertices() const { return N_FACE_NEIGHBORS_3D; } +double +Face::potential(Eigen::ConstRef d, Eigen::ConstRef x) +{ + return smooth_face_term(x.head<3>(), x.segment<3>(3), x.tail<3>()); +} +Vector12d +Face::grad(Eigen::ConstRef d, Eigen::ConstRef x) +{ + Vector12d g; + g.setZero(); + ScalarBase::setVariableCount(9); + auto X = slice_positions, 3, 3>(x); + g.tail<9>() = + smooth_face_term>(X.row(0), X.row(1), X.row(2)).grad; + return g; +} +Matrix12d +Face::hessian(Eigen::ConstRef d, Eigen::ConstRef x) +{ + Matrix12d h; + h.setZero(); + ScalarBase::setVariableCount(9); + auto X = slice_positions, 3, 3>(x); + h.bottomRightCorner<9, 9>() = + smooth_face_term>(X.row(0), X.row(1), X.row(2)).Hess; + return h; +} +} // namespace ipc diff --git a/src/ipc/smooth_contact/primitives/face.hpp b/src/ipc/smooth_contact/primitives/face.hpp new file mode 100644 index 000000000..4372f1f9a --- /dev/null +++ b/src/ipc/smooth_contact/primitives/face.hpp @@ -0,0 +1,41 @@ +#pragma once + +#include "primitive.hpp" + +#include + +namespace ipc { +class Face : public Primitive { +public: + static constexpr int N_CORE_POINTS = 3; + static constexpr int DIM = 3; + // d is a vector from closest point on the face to the point outside of the + // face + Face( + const index_t id, + const CollisionMesh& mesh, + const Eigen::MatrixXd& vertices, + const VectorMax3d& d, + const SmoothContactParameters& params); + + int n_vertices() const override; + int n_dofs() const override { return n_vertices() * DIM; } + + static double + potential(Eigen::ConstRef d, Eigen::ConstRef x); + static Vector12d + grad(Eigen::ConstRef d, Eigen::ConstRef x); + static Matrix12d + hessian(Eigen::ConstRef d, Eigen::ConstRef x); +}; + +/// @brief d points from triangle to the point +template +scalar smooth_face_term( + Eigen::ConstRef> v0, + Eigen::ConstRef> v1, + Eigen::ConstRef> v2) +{ + return 0.5 * (v1 - v0).cross(v2 - v0).norm(); // area of triangle +} +} // namespace ipc \ No newline at end of file diff --git a/src/ipc/smooth_contact/primitives/point2.cpp b/src/ipc/smooth_contact/primitives/point2.cpp new file mode 100644 index 000000000..07635d232 --- /dev/null +++ b/src/ipc/smooth_contact/primitives/point2.cpp @@ -0,0 +1,214 @@ +#include "point2.hpp" + +namespace ipc { + +namespace { + std::vector + find_vertex_adjacent_vertices(const CollisionMesh& mesh, const index_t v) + { + assert(mesh.dim() == 2); + std::vector neighbors; + neighbors.assign(2, -1); + + for (index_t i : mesh.vertex_edge_adjacencies()[v]) { + if (mesh.edges()(i, 0) == v) { + neighbors[0] = mesh.edges()(i, 1); + } else if (mesh.edges()(i, 1) == v) { + neighbors[1] = mesh.edges()(i, 0); + } else { + throw std::runtime_error("Invalid edge-vertex adjacency!"); + } + } + + return neighbors; + } + + bool smooth_point2_term_type( + Eigen::ConstRef v, + Eigen::ConstRef direc, + Eigen::ConstRef e0, + Eigen::ConstRef e1, + const SmoothContactParameters& params, + const bool orientable) + { + const Eigen::Vector2d dn = -direc.normalized(); + const Eigen::Vector2d t0 = (e0 - v).normalized(), + t1 = (e1 - v).normalized(); + + if (dn.dot(t0) <= -params.alpha_t || dn.dot(t1) <= -params.alpha_t) { + return false; + } + + if (orientable) { + const double tmp = Math::smooth_heaviside( + -Math::cross2(dn, t0), + params.alpha_n, params.beta_n) + + Math::smooth_heaviside( + Math::cross2(dn, t1), params.alpha_n, + params.beta_n); + if (tmp <= 1. - params.alpha_n) { + return false; + } + } + + return true; + } + + template + scalar smooth_point2_term( + Eigen::ConstRef> v, + Eigen::ConstRef> direc, + Eigen::ConstRef> e0, + Eigen::ConstRef> e1, + const SmoothContactParameters& params, + const bool orientable) + { + const Eigen::Vector2 dn = -direc.normalized(); + const Eigen::Vector2 t0 = (e0 - v).normalized(), + t1 = (v - e1).normalized(); + + scalar val = Math::smooth_heaviside( + dn.dot(t0), params.alpha_t, params.beta_t) + * Math::smooth_heaviside( + -dn.dot(t1), params.alpha_t, params.beta_t); + + if (orientable) { + const scalar tmp = Math::smooth_heaviside( + -Math::cross2(dn, t0), + params.alpha_n, params.beta_n) + + Math::smooth_heaviside( + -Math::cross2(dn, t1), + params.alpha_n, params.beta_n); + val = val + * Math::smooth_heaviside(tmp - 1., params.alpha_n, 0); + } + + return val * ((e0 - v).norm() + (e1 - v).norm()) / 2.; + } + + template + scalar smooth_point2_term_one_side( + Eigen::ConstRef> v, + Eigen::ConstRef> direc, + Eigen::ConstRef> e0, + const SmoothContactParameters& params) + { + const Eigen::Vector2 dn = -direc.normalized(); + const Eigen::Vector2 t0 = e0 - v; + + const scalar tangent_term = Math::smooth_heaviside( + dn.dot(t0) / t0.norm(), params.alpha_t, params.beta_t); + + return tangent_term * t0.norm(); + } +} // namespace + +Point2::Point2( + const index_t id, + const CollisionMesh& mesh, + const Eigen::MatrixXd& vertices, + const VectorMax3d& d, + const SmoothContactParameters& params) + : Primitive(id, params) +{ + orientable = mesh.is_orient_vertex(id); + auto neighbor_verts = find_vertex_adjacent_vertices(mesh, id); + has_neighbor_1 = neighbor_verts[0] >= 0; + has_neighbor_2 = neighbor_verts[1] >= 0; + + if (has_neighbor_1 && has_neighbor_2) { + m_vertex_ids = { { id, neighbor_verts[0], neighbor_verts[1] } }; + m_is_active = smooth_point2_term_type( + vertices.row(id), d, vertices.row(m_vertex_ids[1]), + vertices.row(m_vertex_ids[2]), params, orientable); + } else if (has_neighbor_1 || has_neighbor_2) { + m_vertex_ids = { + { id, has_neighbor_1 ? neighbor_verts[0] : neighbor_verts[1] } + }; + + const Eigen::Vector2d dn = -d.normalized(); + const Eigen::Vector2d t0 = + (vertices.row(m_vertex_ids[1]) - vertices.row(m_vertex_ids[0])) + .normalized(); + + m_is_active = dn.dot(t0) > -params.alpha_t; + } else { + m_vertex_ids.resize(1); + m_vertex_ids[0] = id; + m_is_active = true; + } +} + +int Point2::n_vertices() const { return m_vertex_ids.size(); } + +double Point2::potential( + const Vector& d, const Vector& x) const +{ + if (has_neighbor_1 && has_neighbor_2) { + return smooth_point2_term( + x.segment(0), d, x.segment(DIM), x.segment(2 * DIM), + m_params, orientable); + } else if (has_neighbor_1 || has_neighbor_2) { + return smooth_point2_term_one_side( + x.segment(0), d, x.segment(DIM), m_params); + } else { + return 1.; + } +} +Vector Point2::grad( + const Vector& d, const Vector& x) const +{ + if (has_neighbor_1 && has_neighbor_2) { + ScalarBase::setVariableCount(4 * DIM); + using T = ADGrad<4 * DIM>; + Vector tmp; + tmp << d, x; + Eigen::Matrix X = slice_positions(tmp); + return smooth_point2_term( + X.row(1), X.row(0), X.row(2), X.row(3), m_params, orientable) + .grad; + } else if (has_neighbor_1 || has_neighbor_2) { + ScalarBase::setVariableCount(3 * DIM); + using T = ADGrad<3 * DIM>; + Vector tmp; + tmp << d, x; + Eigen::Matrix X = slice_positions(tmp); + return smooth_point2_term_one_side( + X.row(1), X.row(0), X.row(2), m_params) + .grad; + } else { + return Vector::Zero( + x.size() + d.size()); + } +} +MatrixMax< + double, + Point2::MAX_SIZE + Point2::DIM, + Point2::MAX_SIZE + Point2::DIM> +Point2::hessian( + const Vector& d, const Vector& x) const +{ + if (has_neighbor_1 && has_neighbor_2) { + ScalarBase::setVariableCount(4 * DIM); + using T = ADHessian<4 * DIM>; + Vector tmp; + tmp << d, x; + Eigen::Matrix X = slice_positions(tmp); + return smooth_point2_term( + X.row(1), X.row(0), X.row(2), X.row(3), m_params, orientable) + .Hess; + } else if (has_neighbor_1 || has_neighbor_2) { + ScalarBase::setVariableCount(3 * DIM); + using T = ADHessian<3 * DIM>; + Vector tmp; + tmp << d, x; + Eigen::Matrix X = slice_positions(tmp); + return smooth_point2_term_one_side( + X.row(1), X.row(0), X.row(2), m_params) + .Hess; + } else { + return MatrixMax::Zero( + x.size() + d.size(), x.size() + d.size()); + } +} +} // namespace ipc \ No newline at end of file diff --git a/src/ipc/smooth_contact/primitives/point2.hpp b/src/ipc/smooth_contact/primitives/point2.hpp new file mode 100644 index 000000000..125bb4f4f --- /dev/null +++ b/src/ipc/smooth_contact/primitives/point2.hpp @@ -0,0 +1,45 @@ +#pragma once + +#include "primitive.hpp" + +#include + +namespace ipc { +class Point2 : public Primitive { +public: + static constexpr int N_CORE_POINTS = 1; + static constexpr int DIM = 2; + static constexpr int MAX_SIZE = N_VERT_NEIGHBORS_2D * DIM; + // d is a vector from this point to the other primitive + Point2( + const index_t id, + const CollisionMesh& mesh, + const Eigen::MatrixXd& vertices, + const VectorMax3d& d, + const SmoothContactParameters& params); + + Point2( + const index_t id, + const CollisionMesh& mesh, + const Eigen::MatrixXd& vertices); + + int n_vertices() const override; + int n_dofs() const override { return n_vertices() * DIM; } + + // assume the following functions are only called if active + double potential( + const Vector& d, + const Vector& x) const; + // derivatives including wrt. d (the closest direction) in front + Vector grad( + const Vector& d, + const Vector& x) const; + MatrixMax hessian( + const Vector& d, + const Vector& x) const; + +private: + bool has_neighbor_1, has_neighbor_2; + bool orientable; +}; +} // namespace ipc diff --git a/src/ipc/smooth_contact/primitives/point3.cpp b/src/ipc/smooth_contact/primitives/point3.cpp new file mode 100644 index 000000000..ddfb7e70c --- /dev/null +++ b/src/ipc/smooth_contact/primitives/point3.cpp @@ -0,0 +1,581 @@ +#include "point3.hpp" + +#include +#include + +namespace ipc { + +Point3::Point3( + const index_t id, + const CollisionMesh& mesh, + const Eigen::MatrixXd& vertices, + const VectorMax3d& d, + const SmoothContactParameters& params) + : Primitive(id, params) +{ + orientable = + mesh.is_orient_vertex(id) && !mesh.vertices_to_faces()[id].empty(); + + // Build index mapping from all vertices to one-ring neighbors + { + global_to_local_vids[id] = 0; + local_to_global_vids.push_back(id); + + int i = 0; + edges.setZero(mesh.vertices_to_edges()[id].size(), 2); + for (int eid : mesh.vertices_to_edges()[id]) { + i++; + index_t neighbor_id = mesh.edges()(eid, 0) == id + ? mesh.edges()(eid, 1) + : mesh.edges()(eid, 0); + assert( + global_to_local_vids.find(neighbor_id) + == global_to_local_vids.end()); + global_to_local_vids[neighbor_id] = i; + edges.row(i - 1) = Eigen::RowVector2i(0, i); + local_to_global_vids.push_back(neighbor_id); + } + } + + // If the vertex is on some faces, record the local indices of the face + // vertices + if (!mesh.vertices_to_faces()[id].empty()) { + int i = 0; + faces.setZero(mesh.vertices_to_faces()[id].size(), 3); + for (auto f : mesh.vertices_to_faces()[id]) { + int lv = 0; + for (; lv < 3; lv++) { + if (mesh.faces()(f, lv) == id) { + faces.row(i) << 0, + global_to_local_vids[mesh.faces()(f, (lv + 1) % 3)], + global_to_local_vids[mesh.faces()(f, (lv + 2) % 3)]; + break; + } + } + assert(lv < 3); + i++; + } + } + + n_neighbors = local_to_global_vids.size() - 1; + m_vertex_ids = local_to_global_vids; + + if (m_vertex_ids.size() > N_VERT_NEIGHBORS_3D) { + logger().error( + "Too many neighbors for point3 primitive! {} > {}! Increase N_VERT_NEIGHBORS_3D in common.hpp", + m_vertex_ids.size(), N_VERT_NEIGHBORS_3D); + } + + m_is_active = + smooth_point3_term_type(vertices(local_to_global_vids, Eigen::all), d); +} + +int Point3::n_vertices() const { return local_to_global_vids.size(); } + +double Point3::potential( + const Vector& d, const Vector& x) const +{ + const Eigen::Matrix X = + slice_positions(x); + return smooth_point3_term(X, d); +} + +Vector Point3::grad( + const Vector& d, const Vector& x) const +{ +#ifdef IPC_TOOLKIT_DEBUG_AUTODIFF + using T = ADGrad<-1>; + Eigen::VectorXd tmp(x.size() + d.size()); + tmp << d, x; + ScalarBase::setVariableCount(tmp.size()); + const Eigen::Matrix X = slice_positions(tmp); + return smooth_point3_term(X.bottomRows(X.rows() - 1), X.row(0)).grad; +#else + const Eigen::Matrix X = + slice_positions(x); + const auto [val, grad] = smooth_point3_term_gradient(d, X, m_params); + return grad; +#endif +} + +MatrixMax< + double, + Point3::MAX_SIZE + Point3::DIM, + Point3::MAX_SIZE + Point3::DIM> +Point3::hessian( + const Vector& d, const Vector& x) const +{ +#ifdef IPC_TOOLKIT_DEBUG_AUTODIFF + using T = ADHessian<-1>; + Eigen::VectorXd tmp(x.size() + d.size()); + tmp << d, x; + ScalarBase::setVariableCount(tmp.size()); + const Eigen::Matrix X = slice_positions(tmp); + return smooth_point3_term(X.bottomRows(X.rows() - 1), X.row(0)).Hess; +#else + const auto X = slice_positions(x); + const auto [val, grad, hess] = smooth_point3_term_hessian(d, X, m_params); + return hess; +#endif +} + +GradType<-1> Point3::smooth_point3_term_tangent_gradient( + Eigen::ConstRef direc, + Eigen::ConstRef> tangents, + const double alpha, + const double beta) const +{ + const int nn = tangents.rows(); + Eigen::VectorXd values = Eigen::VectorXd::Ones(nn, 1); + Eigen::VectorXd acc_val_1 = Eigen::VectorXd::Ones(nn, 1); + std::vector tmp_grad(nn); + for (int a = 0; a < nn; a++) { + if (otypes.tangent_type(a) == HeavisideType::VARIANT) { + std::tie(values(a), tmp_grad[a]) = opposite_direction_penalty_grad( + tangents.row(a), direc, alpha, beta); + for (int b = 0; b < nn; b++) { + if (otypes.tangent_type(b) == HeavisideType::VARIANT + && b != a) { + acc_val_1(b) *= values(a); + } + } + } + } + + Eigen::VectorXd tangent_grad = Eigen::VectorXd::Zero((nn + 1) * 3); + for (int a = 0; a < nn; a++) { + if (otypes.tangent_type(a) == HeavisideType::VARIANT) { + const int id = (a + 1) * 3; + tangent_grad.segment<3>(id) = tmp_grad[a].head<3>() * acc_val_1(a); + tangent_grad.segment<3>(0) += tmp_grad[a].tail<3>() * acc_val_1(a); + } + } + + return std::make_tuple(values.prod(), tangent_grad); +} + +HessianType<-1> Point3::smooth_point3_term_tangent_hessian( + Eigen::ConstRef direc, + Eigen::ConstRef> tangents, + const double alpha, + const double beta) const +{ + const int nn = tangents.rows(); + Eigen::VectorXd values = Eigen::VectorXd::Ones(nn, 1); + Eigen::VectorXd acc_val_1 = Eigen::VectorXd::Ones(nn, 1); + Eigen::MatrixXd acc_val_2 = Eigen::MatrixXd::Ones(nn, nn); + std::vector tmp_grad(nn); + std::vector tmp_hess(nn); + for (int a = 0; a < nn; a++) { + if (otypes.tangent_type(a) == HeavisideType::VARIANT) { + std::tie(values(a), tmp_grad[a], tmp_hess[a]) = + opposite_direction_penalty_hess( + tangents.row(a), direc, alpha, beta); + for (int b = 0; b < nn; b++) { + if (b != a) { + acc_val_1(b) *= values(a); + + for (int c = 0; c < nn; c++) { + if (a != c) { + acc_val_2(b, c) *= values(a); + } + } + } + } + } + } + + Eigen::VectorXd tangent_grad = Eigen::VectorXd::Zero((nn + 1) * 3); + Eigen::MatrixXd tangent_hess = + Eigen::MatrixXd::Zero(tangent_grad.size(), tangent_grad.size()); + Eigen::Vector3d tmp; + for (int a = 0; a < nn; a++) { + if (otypes.tangent_type(a) == HeavisideType::VARIANT) { + const int id = (a + 1) * 3; + tangent_grad.segment<3>(id) = tmp_grad[a].head<3>() * acc_val_1(a); + tangent_grad.segment<3>(0) += tmp_grad[a].tail<3>() * acc_val_1(a); + + tmp.setZero(); + for (int b = 0; b < nn; b++) { + if (otypes.tangent_type(b) == HeavisideType::VARIANT + && b != a) { + tmp += tmp_grad[b].tail<3>() * acc_val_2(a, b); + tangent_hess.block<3, 3>(id, (b + 1) * 3) = + tmp_grad[a].head<3>() + * tmp_grad[b].head<3>().transpose() * acc_val_2(a, b); + tangent_hess.block<3, 3>((b + 1) * 3, id) = + tmp_grad[b].head<3>() + * tmp_grad[a].head<3>().transpose() * acc_val_2(a, b); + } + } + + tangent_hess.block<3, 3>(id, id) = + tmp_hess[a].block<3, 3>(0, 0) * acc_val_1(a); + tangent_hess.block<3, 3>(0, 0) += + tmp_hess[a].block<3, 3>(3, 3) * acc_val_1(a); + tangent_hess.block<3, 3>(0, 0) += + tmp_grad[a].tail<3>() * tmp.transpose(); + + tangent_hess.block<3, 3>(id, 0) += + tmp_hess[a].block<3, 3>(0, 3) * acc_val_1(a); + tangent_hess.block<3, 3>(0, id) += + tmp_hess[a].block<3, 3>(3, 0) * acc_val_1(a); + tangent_hess.block<3, 3>(id, 0) += + tmp_grad[a].head<3>() * tmp.transpose(); + tangent_hess.block<3, 3>(0, id) += + tmp * tmp_grad[a].head<3>().transpose(); + } + } + + return std::make_tuple(values.prod(), tangent_grad, tangent_hess); +} + +GradType<-1> Point3::smooth_point3_term_normal_gradient( + Eigen::ConstRef direc, + Eigen::ConstRef> tangents, + const double alpha, + const double beta) const +{ + Eigen::VectorXd grad = + Eigen::VectorXd::Zero(tangents.size() + direc.size()); + if (!orientable || otypes.normal_type(0) == HeavisideType::ONE) { + return std::make_tuple(1., grad); + } + + double normal_term = 0.; + for (int a = 0; a < faces.rows(); a++) { + const Eigen::Ref t1 = + tangents.row(faces(a, 1) - 1); + const Eigen::Ref t2 = + tangents.row(faces(a, 2) - 1); + + if (otypes.normal_type(a) == HeavisideType::VARIANT) { + const int id1 = faces(a, 1) * 3; + const int id2 = faces(a, 2) * 3; + const auto [y, dy] = + negative_orientation_penalty_grad(t1, t2, -direc, alpha, beta); + + normal_term += y; + + grad.segment<3>(id1) += dy.head<3>(); + grad.segment<3>(id2) += dy.segment<3>(3); + grad.head<3>() -= dy.tail<3>(); + } + } + + const double val = Math::smooth_heaviside(normal_term - 1, 1., 0); + const double grad_val = + Math::smooth_heaviside_grad(normal_term - 1, 1., 0); + + return std::make_tuple(val, grad * grad_val); +} + +HessianType<-1> Point3::smooth_point3_term_normal_hessian( + Eigen::ConstRef direc, + Eigen::ConstRef> tangents, + const double alpha, + const double beta) const +{ + Eigen::VectorXd grad = Eigen::VectorXd::Zero((tangents.rows() + 1) * 3); + Eigen::MatrixXd hess = Eigen::MatrixXd::Zero(grad.size(), grad.size()); + if (!orientable || otypes.normal_type(0) == HeavisideType::ONE) { + return std::make_tuple(1., grad, hess); + } + + // Buggy + // double normal_term = 0.; + // for (int a = 0; a < faces.rows(); a++) { + // const Eigen::Ref t1 = tangents.row(faces(a, + // 1) - 1); const Eigen::Ref t2 = + // tangents.row(faces(a, 2) - 1); + + // if (otypes.normal_type(a) == HeavisideType::VARIANT) + // { + // const int id1 = faces(a, 1) * 3; + // const int id2 = faces(a, 2) * 3; + // auto [y, dy, ddy] = negative_orientation_penalty_hess( + // t1, t2, -direc, alpha, beta); + + // normal_term += y; + + // grad.segment<3>(id1) += dy.head(3); + // grad.segment<3>(id2) += dy.segment<3>(3); + // grad.head<3>() -= dy.tail<3>(); + + // hess.block<3, 3>(id1, id1) += ddy.block<3, 3>(0, 0); + // hess.block<3, 3>(id1, id2) += ddy.block<3, 3>(0, 3); + // hess.block<3, 3>(id1, 0) += ddy.block<3, 3>(0, 6); + + // hess.block<3, 3>(id2, id1) += ddy.block<3, 3>(3, 0); + // hess.block<3, 3>(id2, id2) += ddy.block<3, 3>(3, 3); + // hess.block<3, 3>(id2, 0) += ddy.block<3, 3>(3, 6); + + // hess.block<3, 3>(0, id1) += ddy.block<3, 3>(6, 0); + // hess.block<3, 3>(0, id2) += ddy.block<3, 3>(6, 3); + // hess.block<3, 3>(0, 0) -= ddy.block<3, 3>(6, 6); + // } + // } + + // autodiff + // TODO: replace with efficient code + double normal_term = 0; + { + using T = ADHessian<-1>; + Eigen::VectorXd tmp(direc.size() + tangents.size()); + tmp.head<3>() = direc; + for (int i = 0; i < tangents.rows(); i++) { + tmp.segment<3>(3 * i + 3) = tangents.row(i); + } + + ScalarBase::setVariableCount(tmp.size()); + const Eigen::Matrix X = slice_positions(tmp); + + T normal_term_ad(0.); + for (int a = 0; a < faces.rows(); a++) { + if (otypes.normal_type(a) == HeavisideType::VARIANT) { + normal_term_ad = + normal_term_ad + + Math::smooth_heaviside( + -X.row(0).dot(X.row(faces(a, 1)) + .cross(X.row(faces(a, 2))) + .normalized()), + m_params.alpha_n, m_params.beta_n); + } + } + + normal_term = normal_term_ad.val; + hess = normal_term_ad.Hess; + grad = normal_term_ad.grad; + } + + const double val = Math::smooth_heaviside(normal_term - 1, 1., 0); + const double grad_val = + Math::smooth_heaviside_grad(normal_term - 1, 1., 0); + const double hess_val = + Math::smooth_heaviside_hess(normal_term - 1, 1., 0); + + return std::make_tuple( + val, grad * grad_val, + grad * hess_val * grad.transpose() + grad_val * hess); +} + +bool Point3::smooth_point3_term_type( + const Eigen::Matrix& X, + Eigen::ConstRef direc) +{ + otypes.set_size(edges.rows()); + + const Eigen::RowVector3d dn = direc.normalized(); + for (int a = 0; a < edges.rows(); a++) { + const Eigen::RowVector3d t = X.row(edges(a, 1)) - X.row(edges(a, 0)); + otypes.tangent_type(a) = ipc::OrientationTypes::compute_type( + -dn.dot(t) / t.norm(), m_params.alpha_t, m_params.beta_t); + if (otypes.tangent_type(a) == HeavisideType::ZERO) { + return false; + } + } + + if (!orientable) { + return true; + } + + double normal_term = 0; + for (int a = 0; a < faces.rows(); a++) { + const Eigen::RowVector3d t1 = X.row(faces(a, 1)) - X.row(faces(a, 0)); + const Eigen::RowVector3d t2 = X.row(faces(a, 2)) - X.row(faces(a, 0)); + const double tmp = dn.dot(t1.cross(t2).normalized()); + otypes.normal_type(a) = ipc::OrientationTypes::compute_type( + tmp, m_params.alpha_n, m_params.beta_n); + normal_term += Math::smooth_heaviside( + tmp, m_params.alpha_n, m_params.beta_n); + } + + if (normal_term >= 1) { + for (int a = 0; a < faces.rows(); a++) { + otypes.normal_type(a) = HeavisideType::ONE; + } + } + + return normal_term > 0; +} + +GradType<-1> Point3::smooth_point3_term_gradient( + Eigen::ConstRef direc, + Eigen::ConstRef> X, + const SmoothContactParameters& params) const +{ + const int n_dofs = (X.rows() + 1) * 3; + const int n_neighbor_dofs = n_neighbors * 3; + + const Eigen::Matrix tangents = + X.bottomRows(n_neighbors).rowwise() - X.row(0); + const Eigen::VectorXd tangents_vec = + Eigen::Map(tangents.data(), tangents.size()); + + auto [dn, dn_grad] = normalize_vector_grad(direc); + dn *= -1; + dn_grad *= -1; + + const auto [tangent_term, tangent_grad] = + smooth_point3_term_tangent_gradient( + dn, tangents, params.alpha_t, params.beta_t); + + auto [normal_term, normal_grad] = smooth_point3_term_normal_gradient( + dn, tangents, params.alpha_n, params.beta_n); + + double val = tangent_term * normal_term; + + // gradient wrt. [dn, tangents] + Eigen::VectorXd grad_tmp = + tangent_grad * normal_term + normal_grad * tangent_term; + + const double weight = tangents.squaredNorm() / 3.; + grad_tmp *= weight; + grad_tmp.tail(n_neighbor_dofs) += (2. / 3. * val) * tangents_vec; + + val *= weight; + + // gradient wrt. [direc, v, neighbors] + Eigen::VectorXd grad(n_dofs); + grad.head<3>() = dn_grad * grad_tmp.head<3>(); + for (int d = 0; d < 3; d++) { + grad(d + 3) = -grad_tmp(Eigen::seqN(d + 3, n_neighbors, 3)).sum(); + } + grad.tail(n_neighbor_dofs) = grad_tmp.tail(n_neighbor_dofs); + + return std::tuple(val, grad); +} + +HessianType<-1> Point3::smooth_point3_term_hessian( + Eigen::ConstRef direc, + Eigen::ConstRef> X, + const SmoothContactParameters& params) const +{ + const int n_dofs = (X.rows() + 1) * 3; + const int n_neighbor_dofs = n_neighbors * 3; + + const Eigen::Matrix tangents = + X.bottomRows(n_neighbors).rowwise() - X.row(0); + const Eigen::VectorXd tangents_vec = + Eigen::Map(tangents.data(), tangents.size()); + + auto [dn, dn_grad, dn_hess] = normalize_vector_hess(direc); + dn *= -1; + dn_grad *= -1; + for (auto& mat : dn_hess) { + mat *= -1; + } + + const auto [tangent_term, tangent_grad, tangent_hess] = + smooth_point3_term_tangent_hessian( + dn, tangents, params.alpha_t, params.beta_t); + + auto [normal_term, normal_grad, normal_hess] = + smooth_point3_term_normal_hessian( + dn, tangents, params.alpha_n, params.beta_n); + + double val = tangent_term * normal_term; + + // gradient wrt. [dn, tangents] + Eigen::VectorXd grad_tmp = + tangent_grad * normal_term + normal_grad * tangent_term; + + // hessian wrt. [dn, tangents] + Eigen::MatrixXd hess_tmp = tangent_hess * normal_term + + normal_hess * tangent_term + tangent_grad * normal_grad.transpose() + + normal_grad * tangent_grad.transpose(); + + const double weight = tangents.squaredNorm() / 3.; + hess_tmp *= weight; + hess_tmp.bottomRightCorner(n_neighbor_dofs, n_neighbor_dofs) + .diagonal() + .array() += 2. / 3. * val; + hess_tmp.bottomRows(n_neighbor_dofs) += + 2. / 3. * tangents_vec * grad_tmp.transpose(); + hess_tmp.rightCols(n_neighbor_dofs) += + 2. / 3. * grad_tmp * tangents_vec.transpose(); + + grad_tmp *= weight; + grad_tmp.tail(n_neighbor_dofs) += (2. / 3. * val) * tangents_vec; + + val *= weight; + + // gradient wrt. [direc, v, neighbors] + Eigen::VectorXd grad(n_dofs); + grad.head<3>() = dn_grad * grad_tmp.head<3>(); + for (int d = 0; d < 3; d++) { + grad(d + 3) = -grad_tmp(Eigen::seqN(d + 3, n_neighbors, 3)).sum(); + } + grad.tail(n_neighbor_dofs) = grad_tmp.tail(n_neighbor_dofs); + + // hessian wrt. [direc, v, neighbors] + Eigen::MatrixXd hess; + hess.setZero(n_dofs, n_dofs); + + hess.topLeftCorner<3, 3>() = + dn_grad * hess_tmp.topLeftCorner<3, 3>() * dn_grad.transpose() + + dn_hess[0] * grad_tmp(0) + dn_hess[1] * grad_tmp(1) + + dn_hess[2] * grad_tmp(2); + hess.bottomRightCorner(n_neighbor_dofs, n_neighbor_dofs) = + hess_tmp.bottomRightCorner(n_neighbor_dofs, n_neighbor_dofs); + + hess.block(0, 6, 3, n_neighbor_dofs) = + dn_grad * hess_tmp.topRightCorner(3, n_neighbor_dofs); + hess.block(6, 0, n_neighbor_dofs, 3) = + hess_tmp.bottomLeftCorner(n_neighbor_dofs, 3) * dn_grad; + + for (int k = 0, id = 3; k < n_neighbors; k++, id += 3) { + hess.block<3, 3>(0, 3) -= dn_grad * hess_tmp.block<3, 3>(0, id); + hess.block<3, 3>(3, 0) -= hess_tmp.block<3, 3>(id, 0) * dn_grad; + for (int l = 0, jd = 3; l < n_neighbors; l++, jd += 3) { + hess.block<3, 3>(3, 3) += hess_tmp.block<3, 3>(id, jd); + hess.block<3, 3>(3, 3 + id) -= hess_tmp.block<3, 3>(jd, id); + hess.block<3, 3>(3 + id, 3) -= hess_tmp.block<3, 3>(id, jd); + } + } + + return std::tuple(val, grad, hess); +} + +template +scalar Point3::smooth_point3_term( + const Eigen::Matrix& X, + Eigen::ConstRef> direc) const +{ + const Eigen::RowVector3 dn = direc.normalized(); + scalar tangent_term(1.); + scalar weight(0.); + scalar normal_term(0.); + for (int a = 0; a < edges.rows(); a++) { + const Eigen::RowVector3 t = + X.row(edges(a, 1)) - X.row(edges(a, 0)); + if (otypes.tangent_type(a) == HeavisideType::VARIANT) { + tangent_term = + tangent_term + * Math::smooth_heaviside( + -dn.dot(t) / t.norm(), m_params.alpha_t, m_params.beta_t); + } + + weight = weight + t.squaredNorm(); + } + weight /= 3.; + + if (!orientable || otypes.normal_type(0) == HeavisideType::ONE) { + normal_term = scalar(1.); + } else { + for (int a = 0; a < faces.rows(); a++) { + const Eigen::RowVector3 t1 = + X.row(faces(a, 1)) - X.row(faces(a, 0)); + const Eigen::RowVector3 t2 = + X.row(faces(a, 2)) - X.row(faces(a, 0)); + normal_term = normal_term + + Math::smooth_heaviside( + dn.dot(t1.cross(t2).normalized()), + m_params.alpha_n, m_params.beta_n); + } + normal_term = Math::smooth_heaviside(normal_term - 1, 1., 0); + } + + return weight * normal_term * tangent_term; +} +} // namespace ipc diff --git a/src/ipc/smooth_contact/primitives/point3.hpp b/src/ipc/smooth_contact/primitives/point3.hpp new file mode 100644 index 000000000..424cbb960 --- /dev/null +++ b/src/ipc/smooth_contact/primitives/point3.hpp @@ -0,0 +1,105 @@ +#pragma once + +#include "primitive.hpp" + +#include + +namespace ipc { +class Point3 : public Primitive { +public: + static constexpr int N_CORE_POINTS = 1; + static constexpr int DIM = 3; + static constexpr int MAX_SIZE = N_VERT_NEIGHBORS_3D * DIM; + // d is a vector from this point to the other primitive + Point3( + const index_t id, + const CollisionMesh& mesh, + const Eigen::MatrixXd& vertices, + const VectorMax3d& d, + const SmoothContactParameters& params); + + Point3( + const index_t id, + const CollisionMesh& mesh, + const Eigen::MatrixXd& vertices); + virtual ~Point3() = default; + + int n_vertices() const override; + int n_dofs() const override { return n_vertices() * DIM; } + + // assume the following functions are only called if active + double potential( + const Vector& d, + const Vector& x) const; + // derivatives including wrt. d (the closest direction) in front + Vector grad( + const Vector& d, + const Vector& x) const; + MatrixMax hessian( + const Vector& d, + const Vector& x) const; + + /// @brief + /// @tparam scalar + /// @param direc normalized + /// @param v + /// @param direc points from v to the other point + /// @param neighbors follow counter-clockwise order + /// @param params + /// @return + template + scalar smooth_point3_term( + const Eigen::Matrix& X, + Eigen::ConstRef> direc) const; + + GradType<-1> smooth_point3_term_gradient( + Eigen::ConstRef direc, + Eigen::ConstRef> X, + const SmoothContactParameters& params) const; + + HessianType<-1> smooth_point3_term_hessian( + Eigen::ConstRef direc, + Eigen::ConstRef> X, + const SmoothContactParameters& params) const; + + GradType<-1> smooth_point3_term_tangent_gradient( + Eigen::ConstRef direc, + Eigen::ConstRef> tangents, + const double alpha, + const double beta) const; + + HessianType<-1> smooth_point3_term_tangent_hessian( + Eigen::ConstRef direc, + Eigen::ConstRef> tangents, + const double alpha, + const double beta) const; + + GradType<-1> smooth_point3_term_normal_gradient( + Eigen::ConstRef direc, + Eigen::ConstRef> tangents, + const double alpha, + const double beta) const; + + HessianType<-1> smooth_point3_term_normal_hessian( + Eigen::ConstRef direc, + Eigen::ConstRef> tangents, + const double alpha, + const double beta) const; + +private: + int n_neighbors; + OrientationTypes otypes; + + std::vector local_to_global_vids; + std::map global_to_local_vids; + + Eigen::Matrix faces; + Eigen::Matrix edges; + bool orientable; + + bool smooth_point3_term_type( + const Eigen::Matrix& X, + Eigen::ConstRef direc); +}; + +} // namespace ipc diff --git a/src/ipc/smooth_contact/primitives/primitive.hpp b/src/ipc/smooth_contact/primitives/primitive.hpp new file mode 100644 index 000000000..a85943718 --- /dev/null +++ b/src/ipc/smooth_contact/primitives/primitive.hpp @@ -0,0 +1,51 @@ +#pragma once + +#include +#include +#include + +namespace ipc { + +/* + * @brief Primitive class to be used in GCP contact pairs. + * + * Any derived class of Primitive requires not only the vertex positions on this + * primitive, but also the closest point direction from this primitive to the + * other. The constructor function will check if the primitive is active, and + * the contact pair will be active only if both primitives are active. + */ +class Primitive { +public: + Primitive(const index_t id, const SmoothContactParameters& params) + : m_params(params) + , m_id(id) + { + } + + virtual ~Primitive() = default; + + bool operator==(const Primitive& other) const { return id() == other.id(); } + + /// @brief Vertex/Edge/Face ID of this primitive + index_t id() const { return m_id; } + /// @brief Whether this primitive is active + bool is_active() const { return m_is_active; } + /// @brief Number of vertices on this primitive + virtual int n_vertices() const = 0; + /// @brief Number of vertices times the dimension + virtual int n_dofs() const = 0; + /// @brief Vertex IDs on this primitive + const std::vector& vertex_ids() const { return m_vertex_ids; } + +protected: + /// @brief GCP parameters + const SmoothContactParameters m_params; + /// @brief Vertex IDs on this primitive + std::vector m_vertex_ids; + /// @brief Vertex/Edge/Face ID of this primitive + index_t m_id; + /// @brief Whether this primitive is active + bool m_is_active = true; +}; + +} // namespace ipc diff --git a/src/ipc/smooth_contact/smooth_collisions.cpp b/src/ipc/smooth_contact/smooth_collisions.cpp new file mode 100644 index 000000000..3042138de --- /dev/null +++ b/src/ipc/smooth_contact/smooth_collisions.cpp @@ -0,0 +1,308 @@ +#include "smooth_collisions.hpp" + +#include "smooth_collisions_builder.hpp" + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include // std::out_of_range + +namespace ipc { + +void SmoothCollisions::compute_adaptive_dhat( + const CollisionMesh& mesh, + Eigen::ConstRef vertices, // set to zero for rest pose + const SmoothContactParameters params, + const std::shared_ptr& broad_phase) +{ + assert(vertices.rows() == mesh.num_vertices()); + + const double dhat = params.dhat; + double inflation_radius = dhat / 2; + + // Candidates m_candidates; + m_candidates.build(mesh, vertices, inflation_radius, broad_phase); + this->build( + m_candidates, mesh, vertices, params, + false /*disable adaptive dhat to compute true pairs*/); + + vert_adaptive_dhat.setConstant(mesh.num_vertices(), dhat); + edge_adaptive_dhat.setConstant(mesh.num_edges(), dhat); + if (mesh.dim() == 3) { + face_adaptive_dhat.setConstant(mesh.num_faces(), dhat); + } else { + face_adaptive_dhat.resize(0); + } + + auto assign_min = [](double& a, const double b) -> void { + a = std::min(a, b); + }; + + for (const auto& cc : collisions) { + const double dist = + params.adaptive_dhat_ratio() * sqrt(cc->compute_distance(vertices)); + switch (cc->type()) { + case CollisionType::EDGE_EDGE: { + assign_min(edge_adaptive_dhat((*cc)[0]), dist); + assign_min(edge_adaptive_dhat((*cc)[1]), dist); + break; + } + case CollisionType::EDGE_VERTEX: { + assign_min(edge_adaptive_dhat((*cc)[0]), dist); + assign_min(vert_adaptive_dhat((*cc)[1]), dist); + break; + } + case CollisionType::FACE_VERTEX: { + assign_min(face_adaptive_dhat((*cc)[0]), dist); + assign_min(vert_adaptive_dhat((*cc)[1]), dist); + break; + } + case CollisionType::VERTEX_VERTEX: { + assign_min(vert_adaptive_dhat((*cc)[0]), dist); + assign_min(vert_adaptive_dhat((*cc)[1]), dist); + break; + } + default: { + throw std::runtime_error("Invalid collision type!"); + } + } + } + + // face adaptive dhat should be minimum of all its adjacent vertices and + // edges + if (mesh.dim() == 3) { + for (int f = 0; f < mesh.num_faces(); f++) { + for (int lv = 0; lv < 3; lv++) { + face_adaptive_dhat(f) = std::min( + face_adaptive_dhat(f), + vert_adaptive_dhat(mesh.faces()(f, lv))); + face_adaptive_dhat(f) = std::min( + face_adaptive_dhat(f), + edge_adaptive_dhat(mesh.faces_to_edges()(f, lv))); + } + } + } + + // edge adaptive dhat should be minimum of all its adjacent vertices + for (int e = 0; e < mesh.num_edges(); e++) { + for (int lv = 0; lv < 2; lv++) { + edge_adaptive_dhat(e) = std::min( + edge_adaptive_dhat(e), vert_adaptive_dhat(mesh.edges()(e, lv))); + } + } + + logger().debug( + "Adaptive dhat: vert dhat min {:.2e}, max {:.2e}", + vert_adaptive_dhat.minCoeff(), vert_adaptive_dhat.maxCoeff()); + logger().debug( + "Adaptive dhat: edge dhat min {:.2e}, max {:.2e}", + edge_adaptive_dhat.minCoeff(), edge_adaptive_dhat.maxCoeff()); + if (mesh.dim() == 3) { + logger().debug( + "Adaptive dhat: face dhat min {:.2e}, max {:.2e}", + face_adaptive_dhat.minCoeff(), face_adaptive_dhat.maxCoeff()); + } +} + +void SmoothCollisions::build( + const CollisionMesh& mesh, + Eigen::ConstRef vertices, + const SmoothContactParameters params, + const bool use_adaptive_dhat, + const std::shared_ptr& broad_phase) +{ + assert(vertices.rows() == mesh.num_vertices()); + + double inflation_radius = params.dhat / 2; + + // Candidates m_candidates; + m_candidates.build(mesh, vertices, inflation_radius, broad_phase); + this->build(m_candidates, mesh, vertices, params, use_adaptive_dhat); +} + +void SmoothCollisions::build( + const Candidates& candidates, + const CollisionMesh& mesh, + Eigen::ConstRef vertices, + const SmoothContactParameters params, + const bool use_adaptive_dhat) +{ + assert(vertices.rows() == mesh.num_vertices()); + + clear(); + + const double dhat = params.dhat; + if (!use_adaptive_dhat) { + vert_adaptive_dhat.resize(1); + vert_adaptive_dhat(0) = dhat; + edge_adaptive_dhat.resize(1); + edge_adaptive_dhat(0) = dhat; + if (mesh.dim() == 3) { + face_adaptive_dhat.resize(1); + face_adaptive_dhat(0) = dhat; + } else { + face_adaptive_dhat.resize(0); + } + } + + auto vert_dhat = [&](const index_t v_id) { + return this->get_vert_dhat(v_id); + }; + auto edge_dhat = [&](const index_t e_id) { + return this->get_edge_dhat(e_id); + }; + auto face_dhat = [&](const index_t f_id) { + return this->get_face_dhat(f_id); + }; + + if (mesh.dim() == 2) { + auto storage = create_thread_storage>( + SmoothCollisionsBuilder<2>()); + maybe_parallel_for( + candidates.ev_candidates.size(), + [&](int start, int end, int thread_id) { + SmoothCollisionsBuilder<2>& local_storage = + get_local_thread_storage(storage, thread_id); + local_storage.add_edge_vertex_collisions( + mesh, vertices, candidates.ev_candidates, params, vert_dhat, + edge_dhat, start, end); + }); + SmoothCollisionsBuilder<2>::merge(storage, *this); + } else { + auto storage = create_thread_storage>( + SmoothCollisionsBuilder<3>()); + maybe_parallel_for( + candidates.ee_candidates.size(), + [&](int start, int end, int thread_id) { + SmoothCollisionsBuilder<3>& local_storage = + get_local_thread_storage(storage, thread_id); + local_storage.add_edge_edge_collisions( + mesh, vertices, candidates.ee_candidates, params, vert_dhat, + edge_dhat, start, end); + }); + + maybe_parallel_for( + candidates.fv_candidates.size(), + [&](int start, int end, int thread_id) { + SmoothCollisionsBuilder<3>& local_storage = + get_local_thread_storage(storage, thread_id); + local_storage.add_face_vertex_collisions( + mesh, vertices, candidates.fv_candidates, params, vert_dhat, + edge_dhat, face_dhat, start, end); + }); + SmoothCollisionsBuilder<3>::merge(storage, *this); + } + m_candidates = candidates; +} + +// ============================================================================ +size_t SmoothCollisions::size() const { return collisions.size(); } +bool SmoothCollisions::empty() const { return collisions.empty(); } +void SmoothCollisions::clear() { collisions.clear(); } + +SmoothCollision& SmoothCollisions::operator[](size_t i) +{ + if (i < collisions.size()) { + return *collisions[i]; + } + throw std::out_of_range("Collision index is out of range!"); +} + +const SmoothCollision& SmoothCollisions::operator[](size_t i) const +{ + if (i < collisions.size()) { + return *collisions[i]; + } + throw std::out_of_range("Collision index is out of range!"); +} + +std::string SmoothCollisions::to_string( + const CollisionMesh& mesh, + Eigen::ConstRef vertices, + const SmoothContactParameters& params) const +{ + std::stringstream ss; + for (const auto& cc : collisions) { + ss << "\n"; + { + ss << fmt::format( + "[{}]: ({} {}) dist {} potential {} grad {}", cc->name(), + (*cc)[0], (*cc)[1], cc->compute_distance(vertices), + (*cc)(cc->dof(vertices), params), + (*cc).gradient(cc->dof(vertices), params).norm()); + } + } + return ss.str(); +} + +// NOTE: Actually distance squared +double SmoothCollisions::compute_minimum_distance( + const CollisionMesh& mesh, Eigen::ConstRef vertices) const +{ + assert(vertices.rows() == mesh.num_vertices()); + + if (m_candidates.empty()) { + return std::numeric_limits::infinity(); + } + + const Eigen::MatrixXi& edges = mesh.edges(); + const Eigen::MatrixXi& faces = mesh.faces(); + + tbb::enumerable_thread_specific storage( + std::numeric_limits::infinity()); + + tbb::parallel_for( + tbb::blocked_range(0, m_candidates.size()), + [&](tbb::blocked_range r) { + double& local_min_dist = storage.local(); + + for (size_t i = r.begin(); i < r.end(); i++) { + const double dist = m_candidates[i].compute_distance( + m_candidates[i].dof(vertices, edges, faces)); + + local_min_dist = std::min(dist, local_min_dist); + } + }); + + return storage.combine([](double a, double b) { return std::min(a, b); }); +} + +double SmoothCollisions::compute_active_minimum_distance( + const CollisionMesh& mesh, Eigen::ConstRef vertices) const +{ + assert(vertices.rows() == mesh.num_vertices()); + + if (collisions.empty()) { + return std::numeric_limits::infinity(); + } + + tbb::enumerable_thread_specific storage( + std::numeric_limits::infinity()); + + tbb::parallel_for( + tbb::blocked_range(0, collisions.size()), + [&](tbb::blocked_range r) { + double& local_min_dist = storage.local(); + + for (size_t i = r.begin(); i < r.end(); i++) { + const double dist = collisions[i]->compute_distance(vertices); + + if (collisions[i]->is_active() && dist < local_min_dist) { + local_min_dist = dist; + } + } + }); + + return storage.combine([](double a, double b) { return std::min(a, b); }); +} + +} // namespace ipc diff --git a/src/ipc/smooth_contact/smooth_collisions.hpp b/src/ipc/smooth_contact/smooth_collisions.hpp new file mode 100644 index 000000000..6ae16e0ac --- /dev/null +++ b/src/ipc/smooth_contact/smooth_collisions.hpp @@ -0,0 +1,156 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include + +namespace ipc { +class SmoothCollisions { +public: + /// @brief The type of the collisions. + using value_type = SmoothCollision; + +public: + SmoothCollisions() = default; + virtual ~SmoothCollisions() = default; + + void compute_adaptive_dhat( + const CollisionMesh& mesh, + Eigen::ConstRef vertices, + const SmoothContactParameters params, + const std::shared_ptr& broad_phase = + make_default_broad_phase()); + + /// @brief Initialize the set of collisions used to compute the barrier potential. + /// @param mesh The collision mesh. + /// @param vertices Vertices of the collision mesh. + /// @param broad_phase_method Broad-phase method to use. + void build( + const CollisionMesh& mesh, + Eigen::ConstRef vertices, + const SmoothContactParameters params, + const bool use_adaptive_dhat = false, + const std::shared_ptr& broad_phase = + make_default_broad_phase()); + + /// @brief Initialize the set of collisions used to compute the barrier potential. + /// @param candidates Distance candidates from which the collision set is built. + /// @param mesh The collision mesh. + /// @param vertices Vertices of the collision mesh. + void build( + const Candidates& _candidates, + const CollisionMesh& mesh, + Eigen::ConstRef vertices, + const SmoothContactParameters params, + const bool use_adaptive_dhat = false); + + // ------------------------------------------------------------------------ + + /// @brief Get the number of collisions. + size_t size() const; + + /// @brief Get if the collision set are empty. + bool empty() const; + + /// @brief Clear the collision set. + void clear(); + + /// @brief Get a reference to collision at index i. + /// @param i The index of the collision. + /// @return A reference to the collision. + SmoothCollision& operator[](size_t i); + + /// @brief Get a const reference to collision at index i. + /// @param i The index of the collision. + /// @return A const reference to the collision. + const SmoothCollision& operator[](size_t i) const; + + /// @brief Compute minimum distance between all contact candidates + /// @param mesh The collision mesh. + /// @param vertices Vertices of the collision mesh. + /// @return Squared minimum distance + double compute_minimum_distance( + const CollisionMesh& mesh, + Eigen::ConstRef vertices) const; + + /// @brief Compute minimum distance between contact pairs with non-zero potential + /// @param mesh The collision mesh. + /// @param vertices Vertices of the collision mesh. + /// @return Squared minimum distance + double compute_active_minimum_distance( + const CollisionMesh& mesh, + Eigen::ConstRef vertices) const; + + /// @brief Convert contact pairs to string + std::string to_string( + const CollisionMesh& mesh, + Eigen::ConstRef vertices, + const SmoothContactParameters& params) const; + + /// @brief Get per-vertex dhat value when dhat is adaptive + double get_vert_dhat(int vert_id) const + { + if (vert_adaptive_dhat.size() > 1) { + return vert_adaptive_dhat(vert_id); + } else { + return vert_adaptive_dhat(0); + } + } + /// @brief Get per-edge dhat value when dhat is adaptive + double get_edge_dhat(int edge_id) const + { + if (edge_adaptive_dhat.size() > 1) { + return edge_adaptive_dhat(edge_id); + } else { + return edge_adaptive_dhat(0); + } + } + /// @brief Get per-face dhat value when dhat is adaptive + double get_face_dhat(int face_id) const + { + if (face_adaptive_dhat.size() > 1) { + return face_adaptive_dhat(face_id); + } else { + return face_adaptive_dhat(0); + } + } + /// @brief Get maximum dhat value when dhat is adaptive + double get_max_dhat() const + { + double out = std::max( + vert_adaptive_dhat.maxCoeff(), edge_adaptive_dhat.maxCoeff()); + if (face_adaptive_dhat.size() > 0) { + return std::max(out, face_adaptive_dhat.maxCoeff()); + } + return out; + } + + /// @brief Number of contact candidates + int n_candidates() const { return m_candidates.size(); } + +public: + /// @brief (active) collision pairs + std::vector> collisions; + + /// @brief per-vertex adaptive dhat + Eigen::VectorXd vert_adaptive_dhat; + /// @brief per-edge adaptive dhat + Eigen::VectorXd edge_adaptive_dhat; + /// @brief per-face adaptive dhat + Eigen::VectorXd face_adaptive_dhat; + + /// @brief Collision candidates + Candidates m_candidates; +}; + +} // namespace ipc diff --git a/src/ipc/smooth_contact/smooth_collisions_builder.cpp b/src/ipc/smooth_contact/smooth_collisions_builder.cpp new file mode 100644 index 000000000..531b58fd2 --- /dev/null +++ b/src/ipc/smooth_contact/smooth_collisions_builder.cpp @@ -0,0 +1,291 @@ +#include "smooth_collisions_builder.hpp" + +#include +#include +#include +#include + +#include + +namespace ipc { + +namespace { + template + void add_collision( + const std::shared_ptr& pair, + unordered_map, std::shared_ptr>& + cc_to_id, + std::vector>& collisions) + { + if (pair->is_active() + && cc_to_id.find(pair->get_hash()) == cc_to_id.end()) { + // New collision, so add it to the end of collisions + cc_to_id.emplace(pair->get_hash(), pair); + collisions.push_back(pair); + } + } + + template + void add_collision( + const std::shared_ptr& pair, + std::vector>& collisions) + { + if (pair->is_active()) { + collisions.push_back(pair); + } + } +} // namespace + +void SmoothCollisionsBuilder<2>::add_edge_vertex_collisions( + const CollisionMesh& mesh, + const Eigen::MatrixXd& vertices, + const std::vector& candidates, + const SmoothContactParameters& params, + const std::function& vert_dhat, + const std::function& edge_dhat, + const size_t start_i, + const size_t end_i) +{ + for (size_t i = start_i; i < end_i; i++) { + const auto& [ei, vi] = candidates[i]; + + const PointEdgeDistanceType pe_dtype = point_edge_distance_type( + vertices.row(vi), vertices.row(mesh.edges()(ei, 0)), + vertices.row(mesh.edges()(ei, 1))); + + if (pe_dtype == PointEdgeDistanceType::P_E) { + add_collision<2, SmoothCollisionTemplate>( + std::make_shared>( + ei, vi, pe_dtype, mesh, params, + std::min(edge_dhat(ei), vert_dhat(vi)), vertices), + vert_edge_2_to_id, collisions); + } + + for (int j : { 0, 1 }) { + const auto& vj = mesh.edges()(ei, j); + const double dhat = std::min(vert_dhat(vi), vert_dhat(vj)); + if ((vertices.row(vi) - vertices.row(vj)).norm() >= dhat) { + continue; + } + add_collision<2, SmoothCollisionTemplate>( + std::make_shared>( + std::min(vi, vj), std::max(vi, vj), + PointPointDistanceType::P_P, mesh, params, dhat, vertices), + vert_vert_2_to_id, collisions); + } + } +} + +// ============================================================================ + +void SmoothCollisionsBuilder<3>::add_edge_edge_collisions( + const CollisionMesh& mesh, + const Eigen::MatrixXd& vertices, + const std::vector& candidates, + const SmoothContactParameters& params, + const std::function& vert_dhat, + const std::function& edge_dhat, + const size_t start_i, + const size_t end_i) +{ + for (size_t i = start_i; i < end_i; i++) { + const auto& [eai, ebi] = candidates[i]; + + const auto [ea0, ea1, eb0, eb1] = + candidates[i].vertices(vertices, mesh.edges(), mesh.faces()); + + const EdgeEdgeDistanceType actual_dtype = + edge_edge_distance_type(ea0, ea1, eb0, eb1); + + const double distance = + sqrt(edge_edge_distance(ea0, ea1, eb0, eb1, actual_dtype)); + + if (actual_dtype != EdgeEdgeDistanceType::EA_EB + || distance >= params.dhat) { + continue; + } + + add_collision<3, SmoothCollisionTemplate>( + std::make_shared>( + std::min(eai, ebi), std::max(eai, ebi), actual_dtype, mesh, + params, std::min(edge_dhat(eai), edge_dhat(ebi)), vertices), + collisions); + } +} + +void SmoothCollisionsBuilder<3>::add_face_vertex_collisions( + const CollisionMesh& mesh, + const Eigen::MatrixXd& vertices, + const std::vector& candidates, + const SmoothContactParameters& params, + const std::function& vert_dhat, + const std::function& edge_dhat, + const std::function& face_dhat, + const size_t start_i, + const size_t end_i) +{ + for (size_t i = start_i; i < end_i; i++) { + const auto& [fi, vi] = candidates[i]; + + Eigen::Vector3d v = vertices.row(vi); + Eigen::Vector3d f0 = vertices.row(mesh.faces()(fi, 0)); + Eigen::Vector3d f1 = vertices.row(mesh.faces()(fi, 1)); + Eigen::Vector3d f2 = vertices.row(mesh.faces()(fi, 2)); + const PointTriangleDistanceType pt_dtype = + point_triangle_distance_type(v, f0, f1, f2); + const double distance = + sqrt(point_triangle_distance(v, f0, f1, f2, pt_dtype)); + + if (distance >= vert_dhat(vi)) { + continue; + } + + if (pt_dtype == PointTriangleDistanceType::P_T) { + add_collision<3, SmoothCollisionTemplate>( + std::make_shared>( + fi, vi, pt_dtype, mesh, params, + std::min(face_dhat(fi), vert_dhat(vi)), vertices), + collisions); + } + + for (int lv = 0; lv < 3; lv++) { + const auto& vj = mesh.faces()(fi, lv); + const double dhat = std::min(vert_dhat(vi), vert_dhat(vj)); + if ((vertices.row(vi) - vertices.row(vj)).norm() >= dhat) { + continue; + } + add_collision<3, SmoothCollisionTemplate>( + std::make_shared>( + std::min(vi, vj), std::max(vi, vj), + PointPointDistanceType::P_P, mesh, params, dhat, vertices), + vert_vert_3_to_id, collisions); + } + + for (int le = 0; le < 3; le++) { + const auto& eid = mesh.faces_to_edges()(fi, le); + const double dhat = std::min(edge_dhat(eid), vert_dhat(vi)); + + const PointEdgeDistanceType pe_dtype = point_edge_distance_type( + vertices.row(vi), vertices.row(mesh.edges()(eid, 0)), + vertices.row(mesh.edges()(eid, 1))); + const double distance_sqr = point_edge_distance( + vertices.row(vi), vertices.row(mesh.edges()(eid, 0)), + vertices.row(mesh.edges()(eid, 1)), pe_dtype); + + if (pe_dtype != PointEdgeDistanceType::P_E + || sqrt(distance_sqr) >= dhat) { + continue; + } + + add_collision<3, SmoothCollisionTemplate>( + std::make_shared>( + eid, vi, pe_dtype, mesh, params, dhat, vertices), + edge_vert_3_to_id, collisions); + } + } +} + +void SmoothCollisionsBuilder<3>::merge( + const ParallelCacheType>& local_storage, + SmoothCollisions& merged_collisions) +{ + unordered_map< + std::pair, + std::shared_ptr>> + vert_vert_3_to_id; + unordered_map< + std::pair, + std::shared_ptr>> + edge_vert_3_to_id; + + // size up the hash items + size_t total = 0; + for (const auto& storage : local_storage) { + total += storage.collisions.size(); + } + + merged_collisions.collisions.reserve(total); + + // merge + for (const auto& builder : local_storage) { + vert_vert_3_to_id.insert( + builder.vert_vert_3_to_id.begin(), builder.vert_vert_3_to_id.end()); + edge_vert_3_to_id.insert( + builder.edge_vert_3_to_id.begin(), builder.edge_vert_3_to_id.end()); + } + int edge_vert_count = edge_vert_3_to_id.size(); + int vert_vert_count = vert_vert_3_to_id.size(); + int face_vert_count = 0; + int edge_edge_count = 0; + + for (const auto& [key, val] : vert_vert_3_to_id) { + merged_collisions.collisions.push_back(val); + } + for (const auto& [key, val] : edge_vert_3_to_id) { + merged_collisions.collisions.push_back(val); + } + + for (const auto& builder : local_storage) { + for (const auto& cc : builder.collisions) { + if (cc->type() == CollisionType::FACE_VERTEX) { + face_vert_count++; + merged_collisions.collisions.push_back(cc); + } else if (cc->type() == CollisionType::EDGE_EDGE) { + edge_edge_count++; + merged_collisions.collisions.push_back(cc); + } + } + } + + logger().trace( + "edge-vert pairs {}, vert-vert pairs {}", edge_vert_count, + vert_vert_count); + logger().trace( + "face-vert pairs {}, edge-edge pairs {}", face_vert_count, + edge_edge_count); +} + +void SmoothCollisionsBuilder<2>::merge( + const ParallelCacheType>& local_storage, + SmoothCollisions& merged_collisions) +{ + unordered_map< + std::pair, + std::shared_ptr>> + vert_vert_2_to_id; + unordered_map< + std::pair, + std::shared_ptr>> + vert_edge_2_to_id; + + // size up the hash items + size_t total = 0; + for (const auto& storage : local_storage) { + total += storage.collisions.size(); + } + + merged_collisions.collisions.reserve(total); + + // merge + for (const auto& builder : local_storage) { + vert_vert_2_to_id.insert( + builder.vert_vert_2_to_id.begin(), builder.vert_vert_2_to_id.end()); + vert_edge_2_to_id.insert( + builder.vert_edge_2_to_id.begin(), builder.vert_edge_2_to_id.end()); + } + int edge_vert_count = vert_edge_2_to_id.size(); + int vert_vert_count = vert_vert_2_to_id.size(); + + for (const auto& [key, val] : vert_vert_2_to_id) { + merged_collisions.collisions.push_back(val); + } + for (const auto& [key, val] : vert_edge_2_to_id) { + merged_collisions.collisions.push_back(val); + } + + logger().trace( + "edge-vert pairs {}, vert-vert pairs {}", edge_vert_count, + vert_vert_count); +} + +} // namespace ipc \ No newline at end of file diff --git a/src/ipc/smooth_contact/smooth_collisions_builder.hpp b/src/ipc/smooth_contact/smooth_collisions_builder.hpp new file mode 100644 index 000000000..ef1804057 --- /dev/null +++ b/src/ipc/smooth_contact/smooth_collisions_builder.hpp @@ -0,0 +1,98 @@ +#pragma once + +#include "smooth_collisions.hpp" + +#include +#include + +#include + +namespace ipc { + +template class SmoothCollisionsBuilder; + +template <> class SmoothCollisionsBuilder<2> { +public: + SmoothCollisionsBuilder() { } + + void add_edge_vertex_collisions( + const CollisionMesh& mesh, + const Eigen::MatrixXd& vertices, + const std::vector& candidates, + const SmoothContactParameters& params, + const std::function& vert_dhat, + const std::function& edge_dhat, + const size_t start_i, + const size_t end_i); + + // ------------------------------------------------------------------------- + + static void merge( + const ParallelCacheType>& local_storage, + SmoothCollisions& merged_collisions); + + // Constructed collisions + std::vector> collisions; + + // ------------------------------------------------------------------------- + + // Store the indices to pairs to avoid duplicates. + unordered_map< + std::pair, + std::shared_ptr>> + vert_vert_2_to_id; + unordered_map< + std::pair, + std::shared_ptr>> + vert_edge_2_to_id; +}; + +template <> class SmoothCollisionsBuilder<3> { +public: + SmoothCollisionsBuilder() { } + + void add_edge_edge_collisions( + const CollisionMesh& mesh, + const Eigen::MatrixXd& vertices, + const std::vector& candidates, + const SmoothContactParameters& params, + const std::function& vert_dhat, + const std::function& edge_dhat, + const size_t start_i, + const size_t end_i); + + void add_face_vertex_collisions( + const CollisionMesh& mesh, + const Eigen::MatrixXd& vertices, + const std::vector& candidates, + const SmoothContactParameters& params, + const std::function& vert_dhat, + const std::function& edge_dhat, + const std::function& face_dhat, + const size_t start_i, + const size_t end_i); + + // ------------------------------------------------------------------------- + + static void merge( + const ParallelCacheType>& local_storage, + SmoothCollisions& merged_collisions); + + // Constructed collisions + std::vector> collisions; + + // ------------------------------------------------------------------------- + + // Store the indices to pairs to avoid duplicates, no need for Face-Vertex + // and Edge-Edge + unordered_map< + std::pair, + std::shared_ptr>> + vert_vert_3_to_id; + unordered_map< + std::pair, + std::shared_ptr>> + edge_vert_3_to_id; +}; + +} // namespace ipc \ No newline at end of file diff --git a/src/ipc/smooth_contact/smooth_contact_potential.cpp b/src/ipc/smooth_contact/smooth_contact_potential.cpp new file mode 100644 index 000000000..51cc65c5d --- /dev/null +++ b/src/ipc/smooth_contact/smooth_contact_potential.cpp @@ -0,0 +1,217 @@ +#include "smooth_contact_potential.hpp" + +#include +#include + +#include +#include +#include +#include + +namespace ipc { + +double SmoothContactPotential::operator()( + const SmoothCollisions& collisions, + const CollisionMesh& mesh, + Eigen::ConstRef X) const +{ + assert(X.rows() == mesh.num_vertices()); + + if (collisions.empty()) { + return 0; + } + + tbb::enumerable_thread_specific storage(0); + + tbb::parallel_for( + tbb::blocked_range(size_t(0), collisions.size()), + [&](const tbb::blocked_range& r) { + auto& local_potential = storage.local(); + for (size_t i = r.begin(); i < r.end(); i++) { + // Quadrature weight is premultiplied by local potential + local_potential += (*this)(collisions[i], collisions[i].dof(X)); + } + }); + + return storage.combine([](double a, double b) { return a + b; }); +} + +Eigen::VectorXd SmoothContactPotential::gradient( + const SmoothCollisions& collisions, + const CollisionMesh& mesh, + Eigen::ConstRef X) const +{ + assert(X.rows() == mesh.num_vertices()); + + if (collisions.empty()) { + return Eigen::VectorXd::Zero(X.size()); + } + + const int dim = X.cols(); + + auto storage = + create_thread_storage(Eigen::VectorXd::Zero(X.size())); + maybe_parallel_for( + collisions.size(), [&](int start, int end, int thread_id) { + auto& global_grad = get_local_thread_storage(storage, thread_id); + + for (size_t i = start; i < end; i++) { + const SmoothCollision& collision = collisions[i]; + + const Eigen::VectorXd local_grad = + this->gradient(collision, collision.dof(X)); + + const std::vector vids = collision.vertex_ids(); + + local_gradient_to_global_gradient( + local_grad, vids, dim, global_grad); + } + }); + + Eigen::VectorXd grad; + grad.setZero(X.size()); + for (const auto& local_storage : storage) { + grad += local_storage; + } + return grad; +} + +Eigen::SparseMatrix SmoothContactPotential::hessian( + const SmoothCollisions& collisions, + const CollisionMesh& mesh, + Eigen::ConstRef X, + const PSDProjectionMethod project_hessian_to_psd) const +{ + assert(X.rows() == mesh.num_vertices()); + + if (collisions.empty()) { + return Eigen::SparseMatrix(X.size(), X.size()); + } + + const int dim = X.cols(); + const int ndof = X.size(); + + const int max_triplets_size = int(1e7); + const int buffer_size = std::min(max_triplets_size, ndof); + auto storage = + create_thread_storage(LocalThreadMatStorage(buffer_size, ndof, ndof)); + maybe_parallel_for( + collisions.size(), [&](int start, int end, int thread_id) { + auto& hess_triplets = get_local_thread_storage(storage, thread_id); + + for (size_t i = start; i < end; i++) { + const SmoothCollision& collision = collisions[i]; + + const Eigen::MatrixXd local_hess = this->hessian( + collisions[i], collisions[i].dof(X), + project_hessian_to_psd); + + local_hessian_to_global_triplets( + local_hess, collision.vertex_ids(), dim, + *(hess_triplets.cache)); + } + }); + + Eigen::SparseMatrix hess(ndof, ndof); + + // Assemble the stiffness matrix by concatenating the tuples in each local + // storage + + // Collect thread storages + std::vector storages(storage.size()); + int index = 0; + for (auto& local_storage : storage) { + storages[index++] = &local_storage; + } + + maybe_parallel_for( + storages.size(), [&](int i) { storages[i]->cache->prune(); }); + + if (storage.empty()) { + return Eigen::SparseMatrix(); + } + + // Prepares for parallel concatenation + std::vector offsets(storage.size()); + + index = 0; + int triplet_count = 0; + for (auto& local_storage : storage) { + offsets[index++] = triplet_count; + triplet_count += local_storage.cache->triplet_count(); + } + + std::vector> triplets; + + assert(!storages.empty()); + if (triplet_count >= triplets.max_size()) { + // Serial fallback version in case the vector of triplets cannot be + // allocated + + logger().warn( + "Cannot allocate space for triplets, switching to serial assembly."); + + // Serially merge local storages + for (LocalThreadMatStorage& local_storage : storage) { + hess += local_storage.cache->get_matrix(false); // will also prune + } + hess.makeCompressed(); + } else { + triplets.resize(triplet_count); + + // Parallel copy into triplets + maybe_parallel_for(storages.size(), [&](int i) { + const SparseMatrixCache& cache = + dynamic_cast(*storages[i]->cache); + int offset = offsets[i]; + + std::copy( + cache.entries().begin(), cache.entries().end(), + triplets.begin() + offset); + offset += cache.entries().size(); + + if (cache.mat().nonZeros() > 0) { + int count = 0; + for (int k = 0; k < cache.mat().outerSize(); ++k) { + for (Eigen::SparseMatrix::InnerIterator it( + cache.mat(), k); + it; ++it) { + assert(count < cache.mat().nonZeros()); + triplets[offset + count++] = Eigen::Triplet( + it.row(), it.col(), it.value()); + } + } + } + }); + + // Sort and assemble + hess.setFromTriplets(triplets.begin(), triplets.end()); + } + + return hess; +} + +double SmoothContactPotential::operator()( + const SmoothCollision& collision, + Eigen::ConstRef positions) const +{ + return collision.weight * collision(positions, params); +} + +Eigen::VectorXd SmoothContactPotential::gradient( + const SmoothCollision& collision, + Eigen::ConstRef positions) const +{ + return collision.weight * collision.gradient(positions, params); +} + +Eigen::MatrixXd SmoothContactPotential::hessian( + const SmoothCollision& collision, + Eigen::ConstRef positions, + const PSDProjectionMethod project_hessian_to_psd) const +{ + Eigen::MatrixXd hess = + collision.weight * collision.hessian(positions, params); + return project_to_psd(hess, project_hessian_to_psd); +} +} // namespace ipc \ No newline at end of file diff --git a/src/ipc/smooth_contact/smooth_contact_potential.hpp b/src/ipc/smooth_contact/smooth_contact_potential.hpp new file mode 100644 index 000000000..5aed2700a --- /dev/null +++ b/src/ipc/smooth_contact/smooth_contact_potential.hpp @@ -0,0 +1,86 @@ +#pragma once + +#include +#include +#include + +namespace ipc { + +class SmoothContactPotential { +public: + SmoothContactPotential(const SmoothContactParameters& _params) + : params(_params) + { + } + + virtual ~SmoothContactPotential() = default; + + // -- Cumulative methods --------------------------------------------------- + + /// @brief Compute the potential for a set of collisions. + /// @param collisions The set of collisions. + /// @param mesh The collision mesh. + /// @param X Degrees of freedom of the collision mesh (e.g., vertices or velocities). + /// @returns The potential for a set of collisions. + double operator()( + const SmoothCollisions& collisions, + const CollisionMesh& mesh, + Eigen::ConstRef X) const; + + /// @brief Compute the gradient of the potential. + /// @param collisions The set of collisions. + /// @param mesh The collision mesh. + /// @param X Degrees of freedom of the collision mesh (e.g., vertices or velocities). + /// @returns The gradient of the potential w.r.t. X. This will have a size of |X|. + Eigen::VectorXd gradient( + const SmoothCollisions& collisions, + const CollisionMesh& mesh, + Eigen::ConstRef X) const; + + /// @brief Compute the hessian of the potential. + /// @param collisions The set of collisions. + /// @param mesh The collision mesh. + /// @param X Degrees of freedom of the collision mesh (e.g., vertices or velocities). + /// @param project_hessian_to_psd Make sure the hessian is positive semi-definite. + /// @returns The Hessian of the potential w.r.t. X. This will have a size of |X|×|X|. + virtual Eigen::SparseMatrix hessian( + const SmoothCollisions& collisions, + const CollisionMesh& mesh, + Eigen::ConstRef X, + const PSDProjectionMethod project_hessian_to_psd = + PSDProjectionMethod::NONE) const; + + // -- Single collision methods --------------------------------------------- + + /// @brief Compute the potential for a single collision. + /// @param collision The collision. + /// @param positions The collision stencil's positions. + /// @return The potential. + double operator()( + const SmoothCollision& collision, + Eigen::ConstRef positions) const; + + /// @brief Compute the gradient of the potential for a single collision. + /// @param collision The collision. + /// @param positions The collision stencil's positions. + /// @return The gradient of the potential. + Eigen::VectorXd gradient( + const SmoothCollision& collision, + Eigen::ConstRef positions) const; + + /// @brief Compute the hessian of the potential for a single collision. + /// @param collision The collision. + /// @param positions The collision stencil's positions. + /// @return The hessian of the potential. + Eigen::MatrixXd hessian( + const SmoothCollision& collision, + Eigen::ConstRef positions, + const PSDProjectionMethod project_hessian_to_psd = + PSDProjectionMethod::NONE) const; + +protected: + /// @brief GCP parameters for collision potential + SmoothContactParameters params; +}; + +} // namespace ipc diff --git a/src/ipc/tangent/closest_point.cpp b/src/ipc/tangent/closest_point.cpp index e16dcf4f8..55f4a4ecd 100644 --- a/src/ipc/tangent/closest_point.cpp +++ b/src/ipc/tangent/closest_point.cpp @@ -1,5 +1,7 @@ #include "closest_point.hpp" +#include + #include #include @@ -37,6 +39,28 @@ VectorMax9d point_edge_closest_point_jacobian( return J; } +MatrixMax9d point_edge_closest_point_hessian( + Eigen::ConstRef p, + Eigen::ConstRef e0, + Eigen::ConstRef e1) +{ + const int dim = p.size(); + assert(dim == 2 || dim == 3); + assert(e0.size() == dim && e1.size() == dim); + + MatrixMax9d H(3 * dim, 3 * dim); + if (dim == 2) { + autogen::point_edge_closest_point_2D_hessian( + p(0), p(1), e0(0), e0(1), e1(0), e1(1), H.data()); + } else { + autogen::point_edge_closest_point_3D_hessian( + p(0), p(1), p(2), e0(0), e0(1), e0(2), e1(0), e1(1), e1(2), + H.data()); + } + + return H; +} + // ============================================================================ // Edge - Edge @@ -115,6 +139,273 @@ Eigen::Matrix point_triangle_closest_point_jacobian( // ============================================================================ namespace autogen { + // hess is (6×6) flattened in column-major order + void point_edge_closest_point_2D_hessian( + double p_x, + double p_y, + double e0_x, + double e0_y, + double e1_x, + double e1_y, + double hess[36]) + { + const auto t0 = e0_x - e1_x; + const auto t1 = t0 * t0; + const auto t2 = e0_y - e1_y; + const auto t3 = t2 * t2; + const auto t4 = t1 + t3; + const auto t5 = 1.0 / t4; + const auto t6 = 2 * t5; + const auto t7 = t1 * t6 - 1; + const auto t8 = t5 * t7; + const auto t9 = t5 * t5; + const auto t10 = 2 * t9; + const auto t11 = t0 * t2; + const auto t12 = t10 * t11; + const auto t13 = -t5 * t7; + const auto t14 = -t12; + const auto t15 = t3 * t6 - 1; + const auto t16 = t15 * t5; + const auto t17 = -t15 * t5; + const auto t18 = -2 * e0_x + e1_x + p_x; + const auto t19 = t0 * t18 * t6; + const auto t20 = e0_x - p_x; + const auto t21 = t0 * t20; + const auto t22 = e0_y - p_y; + const auto t23 = t2 * t22; + const auto t24 = t21 + t23; + const auto t25 = t24 * t9; + const auto t26 = t24 * t5; + const auto t27 = 1 - t26; + const auto t28 = -2 * e0_y + e1_y + p_y; + const auto t29 = t0 * t28; + const auto t30 = 4 * t11 * t26; + const auto t31 = t18 * t2 + t30; + const auto t32 = t10 * (t29 + t31); + const auto t33 = 8 * t25; + const auto t34 = -2 * t24 * t5 + 1; + const auto t35 = t5 * (2 * t0 * t20 * t5 - t1 * t33 - t19 - t34); + const auto t36 = t0 * t22; + const auto t37 = t10 * (-t31 + t36); + const auto t38 = t2 * t28 * t6; + const auto t39 = t2 * t20; + const auto t40 = t10 * (-t29 - t30 + t39); + const auto t41 = t5 * (2 * t2 * t22 * t5 - t3 * t33 - t34 - t38); + const auto t42 = t10 * (t30 - t36 - t39); + hess[0] = 0; + hess[1] = 0; + hess[2] = t8; + hess[3] = t12; + hess[4] = t13; + hess[5] = t14; + hess[6] = 0; + hess[7] = 0; + hess[8] = t12; + hess[9] = t16; + hess[10] = t14; + hess[11] = t17; + hess[12] = t8; + hess[13] = t12; + hess[14] = t6 * (4 * t1 * t25 + t19 + t27); + hess[15] = t32; + hess[16] = t35; + hess[17] = t37; + hess[18] = t12; + hess[19] = t16; + hess[20] = t32; + hess[21] = t6 * (4 * t25 * t3 + t27 + t38); + hess[22] = t40; + hess[23] = t41; + hess[24] = t13; + hess[25] = t14; + hess[26] = t35; + hess[27] = t40; + hess[28] = t10 * (4 * t1 * t24 * t5 - 3 * t21 - t23); + hess[29] = t42; + hess[30] = t14; + hess[31] = t17; + hess[32] = t37; + hess[33] = t41; + hess[34] = t42; + hess[35] = t10 * (-t21 - 3 * t23 + 4 * t24 * t3 * t5); + } + + // hess is (9×9) flattened in column-major order + void point_edge_closest_point_3D_hessian( + double p_x, + double p_y, + double p_z, + double e0_x, + double e0_y, + double e0_z, + double e1_x, + double e1_y, + double e1_z, + double hess[81]) + { + const auto t0 = e0_x - e1_x; + const auto t1 = t0 * t0; + const auto t2 = e0_y - e1_y; + const auto t3 = t2 * t2; + const auto t4 = e0_z - e1_z; + const auto t5 = t4 * t4; + const auto t6 = t1 + t3 + t5; + const auto t7 = 1.0 / t6; + const auto t8 = 2 * t7; + const auto t9 = t1 * t8 - 1; + const auto t10 = t7 * t9; + const auto t11 = t7 * t7; + const auto t12 = 2 * t11; + const auto t13 = t0 * t12; + const auto t14 = t13 * t2; + const auto t15 = t13 * t4; + const auto t16 = -t7 * t9; + const auto t17 = -t14; + const auto t18 = -t15; + const auto t19 = t3 * t8 - 1; + const auto t20 = t19 * t7; + const auto t21 = t2 * t4; + const auto t22 = t12 * t21; + const auto t23 = -t19 * t7; + const auto t24 = -t22; + const auto t25 = t5 * t8 - 1; + const auto t26 = t25 * t7; + const auto t27 = -t25 * t7; + const auto t28 = -2 * e0_x + e1_x + p_x; + const auto t29 = t0 * t28 * t8; + const auto t30 = e0_x - p_x; + const auto t31 = t0 * t30; + const auto t32 = e0_y - p_y; + const auto t33 = t2 * t32; + const auto t34 = e0_z - p_z; + const auto t35 = t34 * t4; + const auto t36 = t33 + t35; + const auto t37 = t31 + t36; + const auto t38 = t11 * t37; + const auto t39 = t37 * t7; + const auto t40 = 1 - t39; + const auto t41 = -2 * e0_y + e1_y + p_y; + const auto t42 = t0 * t41; + const auto t43 = 4 * t39; + const auto t44 = t0 * t43; + const auto t45 = t2 * t44; + const auto t46 = t2 * t28 + t45; + const auto t47 = t12 * (t42 + t46); + const auto t48 = -2 * e0_z + e1_z + p_z; + const auto t49 = t0 * t48; + const auto t50 = t4 * t44; + const auto t51 = t28 * t4 + t50; + const auto t52 = t12 * (t49 + t51); + const auto t53 = 8 * t38; + const auto t54 = -2 * t37 * t7 + 1; + const auto t55 = t7 * (2 * t0 * t30 * t7 - t1 * t53 - t29 - t54); + const auto t56 = t0 * t32; + const auto t57 = t12 * (-t46 + t56); + const auto t58 = t0 * t34; + const auto t59 = t12 * (-t51 + t58); + const auto t60 = t2 * t41 * t8; + const auto t61 = 4 * t38; + const auto t62 = t2 * t48; + const auto t63 = t21 * t43; + const auto t64 = t4 * t41 + t63; + const auto t65 = t12 * (t62 + t64); + const auto t66 = t2 * t30; + const auto t67 = t12 * (-t42 - t45 + t66); + const auto t68 = t7 * (2 * t2 * t32 * t7 - t3 * t53 - t54 - t60); + const auto t69 = t2 * t34; + const auto t70 = t12 * (-t64 + t69); + const auto t71 = t4 * t48 * t8; + const auto t72 = t30 * t4; + const auto t73 = t12 * (-t49 - t50 + t72); + const auto t74 = t32 * t4; + const auto t75 = t12 * (-t62 - t63 + t74); + const auto t76 = t7 * (2 * t34 * t4 * t7 - t5 * t53 - t54 - t71); + const auto t77 = t12 * (t45 - t56 - t66); + const auto t78 = t12 * (t50 - t58 - t72); + const auto t79 = t12 * (t63 - t69 - t74); + hess[0] = 0; + hess[1] = 0; + hess[2] = 0; + hess[3] = t10; + hess[4] = t14; + hess[5] = t15; + hess[6] = t16; + hess[7] = t17; + hess[8] = t18; + hess[9] = 0; + hess[10] = 0; + hess[11] = 0; + hess[12] = t14; + hess[13] = t20; + hess[14] = t22; + hess[15] = t17; + hess[16] = t23; + hess[17] = t24; + hess[18] = 0; + hess[19] = 0; + hess[20] = 0; + hess[21] = t15; + hess[22] = t22; + hess[23] = t26; + hess[24] = t18; + hess[25] = t24; + hess[26] = t27; + hess[27] = t10; + hess[28] = t14; + hess[29] = t15; + hess[30] = t8 * (4 * t1 * t38 + t29 + t40); + hess[31] = t47; + hess[32] = t52; + hess[33] = t55; + hess[34] = t57; + hess[35] = t59; + hess[36] = t14; + hess[37] = t20; + hess[38] = t22; + hess[39] = t47; + hess[40] = t8 * (t3 * t61 + t40 + t60); + hess[41] = t65; + hess[42] = t67; + hess[43] = t68; + hess[44] = t70; + hess[45] = t15; + hess[46] = t22; + hess[47] = t26; + hess[48] = t52; + hess[49] = t65; + hess[50] = t8 * (t40 + t5 * t61 + t71); + hess[51] = t73; + hess[52] = t75; + hess[53] = t76; + hess[54] = t16; + hess[55] = t17; + hess[56] = t18; + hess[57] = t55; + hess[58] = t67; + hess[59] = t73; + hess[60] = t12 * (4 * t1 * t37 * t7 - 3 * t31 - t36); + hess[61] = t77; + hess[62] = t78; + hess[63] = t17; + hess[64] = t23; + hess[65] = t24; + hess[66] = t57; + hess[67] = t68; + hess[68] = t75; + hess[69] = t77; + hess[70] = t12 * (4 * t3 * t37 * t7 - t31 - 3 * t33 - t35); + hess[71] = t79; + hess[72] = t18; + hess[73] = t24; + hess[74] = t27; + hess[75] = t59; + hess[76] = t70; + hess[77] = t76; + hess[78] = t78; + hess[79] = t79; + hess[80] = t12 * (-t31 - t33 - 3 * t35 + 4 * t37 * t5 * t7); + } + // J is (2×12) flattened in column-major order void edge_edge_closest_point_jacobian( double ea0_x, diff --git a/src/ipc/tangent/closest_point.hpp b/src/ipc/tangent/closest_point.hpp index b633eee95..d45699be7 100644 --- a/src/ipc/tangent/closest_point.hpp +++ b/src/ipc/tangent/closest_point.hpp @@ -27,6 +27,17 @@ VectorMax9d point_edge_closest_point_jacobian( Eigen::ConstRef e0, Eigen::ConstRef e1); +/// @brief Compute the hessian of the closest point on the edge +/// @param p Point +/// @param e0 First edge point +/// @param e1 Second edge point +/// @return Hessian of the closest point +/// @todo Implement this for 2D vectors +MatrixMax9d point_edge_closest_point_hessian( + Eigen::ConstRef p, + Eigen::ConstRef e0, + Eigen::ConstRef e1); + // ============================================================================ // Edge - Edge @@ -83,6 +94,29 @@ Eigen::Matrix point_triangle_closest_point_jacobian( // ============================================================================ namespace autogen { + // hess is (6×6) flattened in column-major order + void point_edge_closest_point_2D_hessian( + double p_x, + double p_y, + double e0_x, + double e0_y, + double e1_x, + double e1_y, + double hess[36]); + + // hess is (9×9) flattened in column-major order + void point_edge_closest_point_3D_hessian( + double p_x, + double p_y, + double p_z, + double e0_x, + double e0_y, + double e0_z, + double e1_x, + double e1_y, + double e1_z, + double hess[81]); + // J is (2×12) flattened in column-major order void edge_edge_closest_point_jacobian( double ea0_x, diff --git a/src/ipc/utils/CMakeLists.txt b/src/ipc/utils/CMakeLists.txt index ce09c59a0..1a551aa87 100644 --- a/src/ipc/utils/CMakeLists.txt +++ b/src/ipc/utils/CMakeLists.txt @@ -18,6 +18,14 @@ set(SOURCES vertex_to_min_edge.cpp vertex_to_min_edge.hpp world_bbox_diagonal_length.hpp + math.cpp + math.hpp + math.tpp + autodiff_types.hpp + maybe_parallel_for.hpp + maybe_parallel_for.tpp + matrix_cache.hpp + matrix_cache.cpp ) target_sources(ipc_toolkit PRIVATE ${SOURCES}) \ No newline at end of file diff --git a/src/ipc/utils/autodiff_types.hpp b/src/ipc/utils/autodiff_types.hpp new file mode 100644 index 000000000..a1ed857dc --- /dev/null +++ b/src/ipc/utils/autodiff_types.hpp @@ -0,0 +1,29 @@ +#pragma once + +#include // define IPC_TOOLKIT_DEBUG_AUTODIFF +#include + +#include + +namespace ipc { + +using ScalarBase = TinyAD::ScalarBase; + +template using ADGrad = TinyAD::Scalar; +template using ADHessian = TinyAD::Scalar; + +template struct IsADGrad { + static constexpr bool value = false; // NOLINT +}; +template struct IsADGrad> { + static constexpr bool value = true; // NOLINT +}; + +template struct IsADHessian { + static constexpr bool value = false; // NOLINT +}; +template struct IsADHessian> { + static constexpr bool value = true; // NOLINT +}; + +} // namespace ipc diff --git a/src/ipc/utils/eigen_ext.hpp b/src/ipc/utils/eigen_ext.hpp index 9904357f2..9202ecf93 100644 --- a/src/ipc/utils/eigen_ext.hpp +++ b/src/ipc/utils/eigen_ext.hpp @@ -50,12 +50,17 @@ using Vector6d = Eigen::Vector; using Vector9d = Eigen::Vector; /// @brief A static size matrix of size of 12×1 using Vector12d = Eigen::Vector; +/// @brief A static size matrix of size of 15×1 +using Vector15d = Eigen::Vector; + /// @brief A static size matrix of size of 6×6 using Matrix6d = Eigen::Matrix; /// @brief A static size matrix of size of 9×9 using Matrix9d = Eigen::Matrix; /// @brief A static size matrix of size of 12×12 using Matrix12d = Eigen::Matrix; +/// @brief A static size matrix of size of 15×15 +using Matrix15d = Eigen::Matrix; /// @brief A dynamic size matrix with a fixed maximum size of 3×1 template using VectorMax2 = Vector; @@ -162,6 +167,11 @@ using ArrayMax4d = ArrayMax4; /// @brief A dynamic size array with a fixed maximum size of 4×1 using ArrayMax4i = ArrayMax4; +template using GradType = std::tuple>; +template +using HessianType = + std::tuple, Eigen::Matrix>; + /**@}*/ /// @brief Cross product matrix for 3D vectors. diff --git a/src/ipc/utils/local_to_global.hpp b/src/ipc/utils/local_to_global.hpp index 0c5acb760..aa1f3c74f 100644 --- a/src/ipc/utils/local_to_global.hpp +++ b/src/ipc/utils/local_to_global.hpp @@ -1,5 +1,7 @@ #pragma once +#include + #include #include @@ -63,4 +65,103 @@ void local_hessian_to_global_triplets( } } +template +void local_jacobian_to_global_triplets( + const Eigen::MatrixBase& local_jacobian, + const IDContainer1& row_ids, + const IDContainer2& col_ids, + int dim, + std::vector>& triplets) +{ + // assert(local_jacobian.rows() == local_jacobian.cols()); + assert(local_jacobian.rows() % dim == 0); + assert(local_jacobian.cols() % dim == 0); + const int n_rows = local_jacobian.rows() / dim; + const int n_cols = local_jacobian.cols() / dim; + assert(row_ids.size() >= n_rows); // Can be extra ids + assert(col_ids.size() >= n_cols); // Can be extra ids + for (int i = 0; i < n_rows; i++) { + for (int j = 0; j < n_cols; j++) { + for (int k = 0; k < dim; k++) { + for (int l = 0; l < dim; l++) { + triplets.emplace_back( + dim * row_ids[i] + k, dim * col_ids[j] + l, + local_jacobian(dim * i + k, dim * j + l)); + } + } + } + } +} + +class LocalThreadMatStorage { +public: + std::unique_ptr cache = nullptr; + + LocalThreadMatStorage() = delete; + + LocalThreadMatStorage(const int buffer_size, const int rows, const int cols) + { + init(buffer_size, rows, cols); + } + + LocalThreadMatStorage(const int buffer_size, const MatrixCache& c) + { + init(buffer_size, c); + } + + LocalThreadMatStorage(const LocalThreadMatStorage& other) + : cache(other.cache->copy()) + { + } + + LocalThreadMatStorage& operator=(const LocalThreadMatStorage& other) + { + assert(other.cache != nullptr); + cache = other.cache->copy(); + return *this; + } + + void init(const int buffer_size, const int rows, const int cols) + { + cache = std::make_unique(); + cache->reserve(buffer_size); + cache->init(rows, cols); + } + + void init(const int buffer_size, const MatrixCache& c) + { + if (cache == nullptr) { + cache = c.copy(); + } + cache->reserve(buffer_size); + cache->init(c); + } +}; + +template +void local_hessian_to_global_triplets( + const Eigen::MatrixBase& local_hessian, + const IDContainer& ids, + int dim, + MatrixCache& triplets) +{ + assert(local_hessian.rows() == local_hessian.cols()); + assert(local_hessian.rows() % dim == 0); + const int n_verts = local_hessian.rows() / dim; + assert(ids.size() >= n_verts); // Can be extra ids + for (int i = 0; i < n_verts; i++) { + for (int j = 0; j < n_verts; j++) { + for (int k = 0; k < dim; k++) { + for (int l = 0; l < dim; l++) { + const auto& val = local_hessian(dim * i + k, dim * j + l); + if (val != 0) { + triplets.add_value( + 0, dim * ids[i] + k, dim * ids[j] + l, val); + } + } + } + } + } +} + } // namespace ipc diff --git a/src/ipc/utils/math.cpp b/src/ipc/utils/math.cpp new file mode 100644 index 000000000..90ec80757 --- /dev/null +++ b/src/ipc/utils/math.cpp @@ -0,0 +1,229 @@ +#include "math.hpp" + +#include +#include + +namespace ipc { +namespace { + + /* + Mathematica script + + t1x = Table[t1[k], {k, 0, 2}]; + t2x = Table[t2[k], {k, 0, 2}]; + t = Flatten[{t1x, t2x}]; + n = Cross[t1x, t2x]; + CForm[Sum[ddy[i - 1, j - 1] Transpose[{D[n[[j]], {t}]}].{D[n[[i]], {t}]} + + dy[i - 1] D[n[[i]], {t}, {t}], {i, 1, 3}, {j, 1, 3}]] + */ + +} // namespace + +HeavisideType OrientationTypes::compute_type( + const double val, const double alpha, const double beta) +{ + if (val <= -alpha) { + return HeavisideType::ZERO; + } + if (val >= beta) { + return HeavisideType::ONE; + } + + return HeavisideType::VARIANT; +} + +void OrientationTypes::set_size(const int size) +{ + m_size = size; + tangent_types.assign(m_size, HeavisideType::VARIANT); + normal_types.assign(m_size, HeavisideType::VARIANT); +} + +std::tuple +normalize_vector_grad(Eigen::ConstRef t) +{ + double norm = t.norm(); + Eigen::Vector3d y = t / norm; + Eigen::Matrix3d grad = + (Eigen::Matrix3d::Identity() - y * y.transpose()) / norm; + return std::make_tuple(y, grad); +} + +std::tuple< + Eigen::Vector3d, + Eigen::Matrix3d, + std::array, 3>> +normalize_vector_hess(Eigen::ConstRef t) +{ + double norm = t.norm(); + Eigen::Vector3d y = t / norm; + Eigen::Matrix3d grad = + (Eigen::Matrix3d::Identity() - y * y.transpose()) / norm; + std::array, 3> hess; + for (int i = 0; i < 3; i++) { + hess[i] = -(y(i) * grad + y * grad.row(i) + grad.col(i) * y.transpose()) + / norm; + } + + return std::make_tuple(y, grad, hess); +} + +double opposite_direction_penalty( + Eigen::ConstRef t, + Eigen::ConstRef d, + const double alpha, + const double beta) +{ + return Math::smooth_heaviside(d.dot(t) / t.norm(), alpha, beta); +} + +GradType<6> opposite_direction_penalty_grad( + Eigen::ConstRef t, + Eigen::ConstRef d, + const double alpha, + const double beta) +{ + auto [tn, tn_grad] = normalize_vector_grad(t); + const double a = d.dot(tn); + const double y = Math::smooth_heaviside(a, alpha, beta); + const double dy = Math::smooth_heaviside_grad(a, alpha, beta); + + Vector6d grad; + grad << tn_grad * dy * d, dy * tn; + return std::make_tuple(y, grad); +} + +HessianType<6> opposite_direction_penalty_hess( + Eigen::ConstRef t, + Eigen::ConstRef d, + const double alpha, + const double beta) +{ + auto [tn, tn_grad, tn_hess] = normalize_vector_hess(t); + const double a = d.dot(tn); + const double y = Math::smooth_heaviside(a, alpha, beta); + const double dy = Math::smooth_heaviside_grad(a, alpha, beta); + const double ddy = Math::smooth_heaviside_hess(a, alpha, beta); + + Vector6d grad; + grad << dy * d, dy * tn; + + Matrix6d hess; + hess << d * ddy * d.transpose(), + d * ddy * tn.transpose() + Eigen::Matrix3d::Identity() * dy, + tn * ddy * d.transpose() + Eigen::Matrix3d::Identity() * dy, + tn * ddy * tn.transpose(); + + // chain rule of vector t normalize + hess.topRows(3) = tn_grad * hess.topRows(3); + hess.leftCols(3) = hess.leftCols(3) * tn_grad; + hess.topLeftCorner(3, 3) += + grad(0) * tn_hess[0] + grad(1) * tn_hess[1] + grad(2) * tn_hess[2]; + + grad.head(3) = tn_grad * grad.head(3); + + return std::make_tuple(y, grad, hess); +} + +// assume unit vector d +double negative_orientation_penalty( + Eigen::ConstRef t1, + Eigen::ConstRef t2, + Eigen::ConstRef d, + const double alpha, + const double beta) +{ + const Eigen::Vector3d n = t1.cross(t2); + return opposite_direction_penalty(n, d, alpha, beta); +} + +GradType<9> negative_orientation_penalty_grad( + Eigen::ConstRef t1, + Eigen::ConstRef t2, + Eigen::ConstRef d, + const double alpha, + const double beta) +{ + const Eigen::Vector3d n = t1.cross(t2); + auto [y, dy] = opposite_direction_penalty_grad(n, d, alpha, beta); + + /* + Mathematica script + + t1 = {t10, t11, t12}; + t2 = {t20, t21, t22}; + n = Cross[t1, t2]; + Sum[dy[k - 1] D[n[[k]], {{t1, t2}}], {k, 1, 3}] // MatrixForm + */ + + Vector9d grad; + grad << -t2(2) * dy(1) + t2(1) * dy(2), -t2(0) * dy(2) + t2(2) * dy(0), + -t2(1) * dy(0) + t2(0) * dy(1), -t1(1) * dy(2) + t1(2) * dy(1), + -t1(2) * dy(0) + t1(0) * dy(2), -t1(0) * dy(1) + t1(1) * dy(0), + dy.tail(3); + return std::make_tuple(y, grad); +} + +HessianType<9> negative_orientation_penalty_hess( + Eigen::ConstRef t1, + Eigen::ConstRef t2, + Eigen::ConstRef d, + const double alpha, + const double beta) +{ + const Eigen::Vector3d n = t1.cross(t2); + const Eigen::Matrix cross_grad = + cross_product_gradient(t1, t2); + const std::array cross_hess = cross_product_hessian(t1, t2); + auto [y, dy, ddy] = opposite_direction_penalty_hess(n, d, alpha, beta); + + Vector9d grad; + grad.tail(3) = dy.tail(3); + grad.head(6) = cross_grad.transpose() * dy.head(3); + + Matrix9d hess; + hess.bottomRightCorner(3, 3) = ddy.bottomRightCorner(3, 3); + hess.topLeftCorner(6, 6) = + cross_grad.transpose() * ddy.topLeftCorner(3, 3) * cross_grad + + cross_hess[0] * dy(0) + cross_hess[1] * dy(1) + cross_hess[2] * dy(2); + hess.topRightCorner(6, 3) = + cross_grad.transpose() * ddy.topRightCorner(3, 3); + hess.bottomLeftCorner(3, 6) = ddy.bottomLeftCorner(3, 3) * cross_grad; + + return std::make_tuple(y, grad, hess); +} + +Eigen::Matrix cross_product_gradient( + Eigen::ConstRef t1, Eigen::ConstRef t2) +{ + Eigen::Matrix grad; + grad << 0, t2(2), -t2(1), 0, -t1(2), t1(1), -t2(2), 0, t2(0), t1(2), 0, + -t1(0), t2(1), -t2(0), 0, -t1(1), t1(0), 0; + + return grad; +} + +std::array cross_product_hessian( + Eigen::ConstRef t1, Eigen::ConstRef t2) +{ + std::array hess; + hess.fill(Matrix6d::Zero()); + hess[0](1, 5) = 1; + hess[0](5, 1) = 1; + hess[0](2, 4) = -1; + hess[0](4, 2) = -1; + + hess[1](0, 5) = -1; + hess[1](5, 0) = -1; + hess[1](2, 3) = 1; + hess[1](3, 2) = 1; + + hess[2](0, 4) = 1; + hess[2](4, 0) = 1; + hess[2](1, 3) = -1; + hess[2](3, 1) = -1; + + return hess; +} + +} // namespace ipc \ No newline at end of file diff --git a/src/ipc/utils/math.hpp b/src/ipc/utils/math.hpp new file mode 100644 index 000000000..918b500a1 --- /dev/null +++ b/src/ipc/utils/math.hpp @@ -0,0 +1,169 @@ +#pragma once + +#include +#include +#include + +namespace ipc { + +enum class HeavisideType : uint8_t { ZERO = 0, ONE = 1, VARIANT = 2 }; + +struct OrientationTypes { + + static HeavisideType + compute_type(const double val, const double alpha, const double beta); + + int size() const { return m_size; } + void set_size(const int size); + const HeavisideType& tangent_type(const int i) const + { + return tangent_types[i]; + } + const HeavisideType& normal_type(const int i) const + { + return normal_types[i]; + } + HeavisideType& tangent_type(const int i) { return tangent_types[i]; } + HeavisideType& normal_type(const int i) { return normal_types[i]; } + + int m_size = 0; + std::vector tangent_types, normal_types; +}; + +constexpr double MOLLIFIER_THRESHOLD_EPS = 1e-2; + +template struct Math { + Math() = delete; + Math(const Math&) = delete; + Math& operator=(const Math&) = delete; + + static double sign(const double x); + static T abs(const T& x); + static T sqr(const T& x); + static T cubic(const T& x); + + static T cubic_spline(const T& x); + static double cubic_spline_grad(const double x); + static double cubic_spline_hess(const double x); + + /// @brief support is [-1, 1] + /// @tparam T + /// @param x + /// @return + static T quadratic_spline(const T& x); + + static T + smooth_heaviside(const T& x, const double alpha, const double beta = 0); + static double smooth_heaviside_grad( + const double x, const double alpha, const double beta = 0); + static double smooth_heaviside_hess( + const double x, const double alpha, const double beta = 0); + + static T mollifier(const T& x); + static double mollifier_grad(const double x); + static double mollifier_hess(const double x); + + // support is [0, 1] + static T inv_barrier(const T& x, const int r); + static double inv_barrier_grad(const double x, const int r); + static double inv_barrier_hess(const double x, const int r); + + static T l_ns(const T& x); + + static T cross2( + Eigen::ConstRef> a, + Eigen::ConstRef> b); +}; + +// gradient is symmetric +std::tuple +normalize_vector_grad(Eigen::ConstRef t); + +// hessian is symmetric wrt. the three dimensions +std::tuple< + Eigen::Vector3d, + Eigen::Matrix3d, + std::array, 3>> +normalize_vector_hess(Eigen::ConstRef t); + +template +inline Eigen::Matrix< + T, + rows, + cols, + (max_rows > 1 ? Eigen::ColMajor : Eigen::RowMajor), + max_rows, + cols> +slice_positions(const Eigen::VectorXd& positions, const int offset = 0) +{ + static_assert(cols > 0); + const int nrows = rows > 0 ? rows : positions.size() / cols; + Eigen::Matrix< + T, rows, cols, (max_rows > 1 ? Eigen::ColMajor : Eigen::RowMajor), + max_rows, cols> + points; + points.setZero(nrows, cols); + + for (int i = 0, id = 0; i < nrows; i++) { + for (int d = 0; d < cols; d++, id++) { + if constexpr (std::is_same::value) { + points(i, d) = positions(id); + } else { + points(i, d) = T(positions(id), id + offset); + } + } + } + + return points; +} + +Eigen::Matrix cross_product_gradient( + Eigen::ConstRef t1, Eigen::ConstRef t2); + +std::array cross_product_hessian( + Eigen::ConstRef t1, Eigen::ConstRef t2); + +// assume unit vector d +double opposite_direction_penalty( + Eigen::ConstRef t, + Eigen::ConstRef d, + const double alpha, + const double beta); + +GradType<6> opposite_direction_penalty_grad( + Eigen::ConstRef t, + Eigen::ConstRef d, + const double alpha, + const double beta); + +HessianType<6> opposite_direction_penalty_hess( + Eigen::ConstRef t, + Eigen::ConstRef d, + const double alpha, + const double beta); + +// assume unit vector d +double negative_orientation_penalty( + Eigen::ConstRef t1, + Eigen::ConstRef t2, + Eigen::ConstRef d, + const double alpha, + const double beta); + +GradType<9> negative_orientation_penalty_grad( + Eigen::ConstRef t1, + Eigen::ConstRef t2, + Eigen::ConstRef d, + const double alpha, + const double beta); + +HessianType<9> negative_orientation_penalty_hess( + Eigen::ConstRef t1, + Eigen::ConstRef t2, + Eigen::ConstRef d, + const double alpha, + const double beta); + +} // namespace ipc + +#include "math.tpp" diff --git a/src/ipc/utils/math.tpp b/src/ipc/utils/math.tpp new file mode 100644 index 000000000..fb712a9de --- /dev/null +++ b/src/ipc/utils/math.tpp @@ -0,0 +1,268 @@ +#pragma once +#include "math.hpp" + +#include + +namespace ipc { + +namespace { + /// @brief support is [0, 3] + /// @tparam T + /// @param x + /// @return + template T quadratic_spline_aux(const T& x) + { + if (x <= 0) { + return T(0.); + } + if (x <= 1) { + return Math::sqr(x) / 2.; + } + if (x <= 2) { + return (-3 + x * (6 - 2 * x)) / 2.; + } + if (x < 3) { + return Math::sqr(3. - x) / 2.; + } + return T(0.); + } + + template T smooth_heaviside_standard(const T& x) + { + if (x <= -3) { + return T(0.); + } + if (x <= -2) { + return Math::cubic(3. + x) / 6.; + } + if (x <= -1) { + return (((-2 * x - 9) * x - 9) * x + 3) / 6; + } + if (x < 0) { + return Math::cubic(x) / 6. + 1.; + } + + return T(1.); + } + + [[maybe_unused]] double smooth_heaviside_standard_grad(const double x) + { + if (x <= -3 || x >= 0) { + return 0.; + } + if (x <= -2) { + return Math::sqr(3. + x) / 2.; + } + if (x <= -1) { + return -(x * x + 3 * x + 1.5); + } + + return Math::sqr(x) / 2.; + } + + [[maybe_unused]] double smooth_heaviside_standard_hess(const double x) + { + if (x <= -3 || x >= 0) { + return 0.; + } + if (x <= -2) { + return 3. + x; + } + if (x <= -1) { + return -3 - 2 * x; + } + + return x; + } +} // namespace + +template double Math::sign(const double x) +{ + if (x > 0) { + return 1.; + } else { + return -1.; + } +} + +template T Math::abs(const T& x) +{ + if (x >= 0) { + return x; + } else { + return -x; + } +} + +template T Math::sqr(const T& x) { return x * x; } + +template T Math::cubic(const T& x) { return x * x * x; } + +template T Math::cubic_spline(const T& x) +{ + if (abs(x) >= 1) { + return T(0.); + } + if (abs(x) >= 0.5) { + return cubic(1 - abs(x)) * (4. / 3.); + } + + return 2. / 3. - 4. * (x * x) * (1 - abs(x)); +} +template double Math::cubic_spline_grad(const double x) +{ + if (Math::abs(x) >= 1) { + return 0.; + } + if (Math::abs(x) >= 0.5) { + return -4. * Math::sqr(1 - Math::abs(x)) * sign(x); + } + + return 4. * x * (3. * Math::abs(x) - 2.); +} +template double Math::cubic_spline_hess(const double x) +{ + if (Math::abs(x) >= 1) { + return 0.; + } + if (Math::abs(x) >= 0.5) { + return 8. * (1 - Math::abs(x)); + } + + return 8. * (3. * Math::abs(x) - 1.); +} + +/// @brief support is [-1, 1] +/// @tparam T +/// @param x +/// @return +template T Math::quadratic_spline(const T& x) +{ + return quadratic_spline_aux(x * 1.5 + 1.5); +} + +// template +// T smooth_heaviside_aux(const T &x) +// { +// if (x <= -2) +// return T(0.); +// if (x <= -1) +// return intpow(2. + x, 2) / 2.; +// if (x <= 0) +// return 1 - intpow(x, 2) / 2.; + +// return T(1.); +// } + +template +T Math::smooth_heaviside(const T& x, const double alpha, const double beta) +{ + return smooth_heaviside_standard((x - beta) * (3 / (alpha + beta))); +} + +template +double Math::smooth_heaviside_grad( + const double x, const double alpha, const double beta) +{ + const double s = 3 / (alpha + beta); + return smooth_heaviside_standard_grad((x - beta) * s) * s; +} + +template +double Math::smooth_heaviside_hess( + const double x, const double alpha, const double beta) +{ + const double s = 3 / (alpha + beta); + return smooth_heaviside_standard_hess((x - beta) * s) * s * s; +} + +template T Math::mollifier(const T& x) +{ + if constexpr (IsADHessian::value) { + if (x <= 0) { + return T(0.); + } else if (x < 1) { + const double deriv = 2. * (1. - x.val), hess = -2.; + return T::known_derivatives( + x.val * (2. - x.val), deriv * x.grad, + x.grad * hess * x.grad.transpose() + deriv * x.Hess); + } else { + return T(1.); + } + // return smooth_heaviside(x - 1.); + } else { + if (x <= 0) { + return T(0.); + } else if (x < 1) { + return x * (2. - x); + } else { + return T(1.); + } + // return smooth_heaviside(x - 1.); + } +} + +template double Math::mollifier_grad(const double x) +{ + if (x <= 0 || x >= 1) { + return 0.; + } else { + return 2. * (1. - x); + } +} + +template double Math::mollifier_hess(const double x) +{ + if (x <= 0 || x >= 1) { + return 0.; + } else { + return -2.; + } +} + +// support is [0, 1] +template T Math::inv_barrier(const T& x, const int r) +{ + return cubic_spline(x) / pow(x, r); + // log barrier + // if (x < 1) + // return -(1 - x) * (1 - x) * log(x); + // else + // return T(0.); +} + +template +double Math::inv_barrier_grad(const double x, const int r) +{ + return (cubic_spline_grad(x) - Math::cubic_spline(x) * r / x) + / pow(x, r); +} +template +double Math::inv_barrier_hess(const double x, const int r) +{ + return (cubic_spline_hess(x) + + (-2. * cubic_spline_grad(x) + + (r + 1.) * Math::cubic_spline(x) / x) + * r / x) + / pow(x, r); +} + +template T Math::l_ns(const T& x) +{ + if (x <= 0.) { + return T(0.); + } + if (x >= 1.) { + return T(1.); + } + return x; +} + +template +T Math::cross2( + Eigen::ConstRef> a, Eigen::ConstRef> b) +{ + return a[0] * b[1] - a[1] * b[0]; +} + +} // namespace ipc \ No newline at end of file diff --git a/src/ipc/utils/matrix_cache.cpp b/src/ipc/utils/matrix_cache.cpp new file mode 100644 index 000000000..2fceab981 --- /dev/null +++ b/src/ipc/utils/matrix_cache.cpp @@ -0,0 +1,216 @@ +#include "matrix_cache.hpp" + +#include +#include + +namespace ipc { + +SparseMatrixCache::SparseMatrixCache(const size_t size) { init(size); } + +SparseMatrixCache::SparseMatrixCache(const size_t rows, const size_t cols) +{ + init(rows, cols); +} + +SparseMatrixCache::SparseMatrixCache(const MatrixCache& other) { init(other); } + +SparseMatrixCache::SparseMatrixCache( + const SparseMatrixCache& other, const bool copy_main_cache_ptr) +{ + init(other, copy_main_cache_ptr); +} + +void SparseMatrixCache::init(const size_t size) +{ + assert(mapping().empty() || m_size == size); + + m_size = size; + m_tmp.resize(m_size, m_size); + m_mat.resize(m_size, m_size); + m_mat.setZero(); +} + +void SparseMatrixCache::init(const size_t rows, const size_t cols) +{ + assert(mapping().empty()); + + m_size = rows == cols ? rows : 0; + m_tmp.resize(rows, cols); + m_mat.resize(rows, cols); + m_mat.setZero(); +} + +void SparseMatrixCache::init(const MatrixCache& other) +{ + assert(this != &other); + assert(&other == &dynamic_cast(other)); + init(dynamic_cast(other)); +} + +void SparseMatrixCache::init( + const SparseMatrixCache& other, const bool copy_main_cache_ptr) +{ + assert(this != &other); + if (copy_main_cache_ptr) { + m_main_cache = other.m_main_cache; + } else if (m_main_cache == nullptr) { + m_main_cache = other.main_cache(); + // Only one level of cache + assert( + m_main_cache != this && m_main_cache != nullptr + && m_main_cache->m_main_cache == nullptr); + } + m_size = other.m_size; + + m_values.resize(other.m_values.size()); + + m_tmp.resize(other.m_mat.rows(), other.m_mat.cols()); + m_mat.resize(other.m_mat.rows(), other.m_mat.cols()); + m_mat.setZero(); + std::fill(m_values.begin(), m_values.end(), 0); +} + +void SparseMatrixCache::set_zero() +{ + m_tmp.setZero(); + m_mat.setZero(); + + std::fill(m_values.begin(), m_values.end(), 0); +} + +void SparseMatrixCache::add_value( + const int e, const int i, const int j, const double value) +{ + // caches have yet to be constructed (likely because the matrix has yet to + // be fully assembled) + if (mapping().empty()) { + // save entry so it can be added to the matrix later + m_entries.emplace_back(i, j, value); + + // save the index information so the cache can be built later + if (m_second_cache_entries.size() <= e) { + m_second_cache_entries.resize(e + 1); + } + m_second_cache_entries[e].emplace_back(i, j); + } else { + if (e != m_current_e) { + m_current_e = e; + m_current_e_index = 0; + } + + // save entry directly to value buffer at the proper index + m_values[second_cache()[e][m_current_e_index]] += value; + m_current_e_index++; + } +} + +void SparseMatrixCache::prune() +{ + // caches have yet to be constructed (likely because the matrix has yet to + // be fully assembled) + if (mapping().empty()) { + m_tmp.setFromTriplets(m_entries.begin(), m_entries.end()); + m_tmp.makeCompressed(); + m_mat += m_tmp; + + m_tmp.setZero(); + m_tmp.data().squeeze(); + m_mat.makeCompressed(); + + m_entries.clear(); + + m_mat.makeCompressed(); + } +} + +Eigen::SparseMatrix +SparseMatrixCache::get_matrix(const bool compute_mapping) +{ + prune(); + + // caches have yet to be constructed (likely because the matrix has yet to + // be fully assembled) + if (mapping().empty()) { + if (compute_mapping && m_size > 0) { + assert(m_main_cache == nullptr); + + m_values.resize(m_mat.nonZeros()); + m_inner_index.resize(m_mat.nonZeros()); + m_outer_index.resize(m_mat.rows() + 1); + m_mapping.resize(m_mat.rows()); + + // note: m_mat is column major + auto* const inn_ptr = m_mat.innerIndexPtr(); + auto* const out_ptr = m_mat.outerIndexPtr(); + m_inner_index.assign(inn_ptr, inn_ptr + m_inner_index.size()); + m_outer_index.assign(out_ptr, out_ptr + m_outer_index.size()); + + size_t index = 0; + // loop over columns of the matrix + for (size_t i = 0; i < m_mat.cols(); ++i) { + const auto start = m_outer_index[i]; + const auto end = m_outer_index[i + 1]; + + // loop over the nonzero elements of the given column + for (size_t ii = start; ii < end; ++ii) { + // pick out current row + const auto j = m_inner_index[ii]; + auto& map = m_mapping[j]; + map.emplace_back(i, index); + ++index; + } + } + + logger().trace("Cache computed"); + + m_second_cache.clear(); + m_second_cache.resize(m_second_cache_entries.size()); + // loop over each element + for (int e = 0; e < m_second_cache_entries.size(); ++e) { + // loop over each global index affected by the given element + for (const auto& p : m_second_cache_entries[e]) { + const int i = p.first; + const int j = p.second; + + // pick out column/sparse matrix index pairs for the given + // column + const auto& map = mapping()[i]; + int local_index = -1; + + // loop over column/sparse matrix index pairs + for (const auto& q : map) { + // match columns + if (q.first == j) { + assert(q.second < m_values.size()); + local_index = q.second; + break; + } + } + assert(local_index >= 0); + + // save the sparse matrix index used by this element + m_second_cache[e].emplace_back(local_index); + } + } + + m_second_cache_entries.resize(0); + + logger().trace("Second cache computed"); + } + } else { + assert(m_size > 0); + const auto& outer_index = main_cache()->m_outer_index; + const auto& inner_index = main_cache()->m_inner_index; + // directly write the values to the matrix + m_mat = Eigen::Map>( + m_size, m_size, m_values.size(), outer_index.data(), + inner_index.data(), m_values.data()); + + m_current_e = -1; + m_current_e_index = -1; + } + std::fill(m_values.begin(), m_values.end(), 0); + return m_mat; +} + +} // namespace ipc \ No newline at end of file diff --git a/src/ipc/utils/matrix_cache.hpp b/src/ipc/utils/matrix_cache.hpp new file mode 100644 index 000000000..ea36d33b7 --- /dev/null +++ b/src/ipc/utils/matrix_cache.hpp @@ -0,0 +1,172 @@ +#pragma once + +#include "eigen_ext.hpp" + +#include + +namespace ipc { + +/// abstract class used for caching +class MatrixCache { +public: + MatrixCache() { } + virtual ~MatrixCache() = default; + + virtual std::unique_ptr copy() const = 0; + + virtual void init(const size_t size) = 0; + virtual void init(const size_t rows, const size_t cols) = 0; + virtual void init(const MatrixCache& other) = 0; + + virtual void set_zero() = 0; + + virtual void reserve(const size_t size) = 0; + virtual size_t entries_size() const = 0; + virtual size_t capacity() const = 0; + virtual size_t non_zeros() const = 0; + virtual size_t triplet_count() const = 0; + virtual bool is_sparse() const = 0; + bool is_dense() const { return !is_sparse(); } + + virtual void + add_value(const int e, const int i, const int j, const double value) = 0; + virtual Eigen::SparseMatrix + get_matrix(const bool compute_mapping = true) = 0; + virtual void prune() = 0; +}; + +class SparseMatrixCache : public MatrixCache { +public: + // constructors (call init functions below) + SparseMatrixCache() = default; + explicit SparseMatrixCache(const size_t size); + SparseMatrixCache(const size_t rows, const size_t cols); + SparseMatrixCache(const MatrixCache& other); + SparseMatrixCache( + const SparseMatrixCache& other, const bool copy_main_cache_ptr = false); + + std::unique_ptr copy() const override + { + // just copy main cache pointer + return std::make_unique(*this, true); + } + + /// set matrix to be size x size + void init(const size_t size) override; + /// set matrix to be rows x cols + void init(const size_t rows, const size_t cols) override; + /// set matrix to be a matrix of all zeros with same size as other + void init(const MatrixCache& other) override; + /// set matrix to be a matrix of all zeros with same size as other + /// (potentially with the same main cache) + void init( + const SparseMatrixCache& other, const bool copy_main_cache_ptr = false); + + /// set matrix values to zero + /// modifies tmp_, m_mat, and values (setting all to zero) + void set_zero() override; + + void reserve(const size_t size) override { m_entries.reserve(size); } + + size_t entries_size() const override { return m_entries.size(); } + + size_t capacity() const override { return m_entries.capacity(); } + + size_t non_zeros() const override + { + return m_mapping.empty() ? m_mat.nonZeros() : m_values.size(); + } + + size_t triplet_count() const override + { + return m_entries.size() + m_mat.nonZeros(); + } + + bool is_sparse() const override { return true; } + + size_t mapping_size() const { return m_mapping.size(); } + + /// @brief Add a value to the matrix cache. + /// @param e element index + /// @param i global row index + /// @param j global column index + /// @param value value to add to matrix if the cache is yet to be constructed + void add_value( + const int e, const int i, const int j, const double value) override; + + /// if the cache is yet to be constructed, save the + /// cached (ordered) indices in inner_index_ and outer_index_ + /// then fill in map and second_cache_ + /// in this case, modifies inner_index_, outer_index_, map, and + /// second_cache_ to reflect the matrix structure also empties + /// second_cache_entries and sets values_ to zero + /// otherwise, update m_mat directly using the cached indices and values_ + /// in this case, modifies m_mat and sets values_ to zero + Eigen::SparseMatrix + get_matrix(const bool compute_mapping = true) override; + + /// if caches have yet to be constructed, add the saved triplets to m_mat + /// modifies tmp_ and m_mat, also sets entries_ to be empty after writing + /// its values to m_mat + void prune() override; ///< add saved entries to stored matrix + + const Eigen::SparseMatrix& mat() const + { + return m_mat; + } + const std::vector>& entries() const + { + return m_entries; + } + +private: + const SparseMatrixCache* main_cache() const + { + return m_main_cache == nullptr ? this : m_main_cache; + } + + const std::vector>>& mapping() const + { + return main_cache()->m_mapping; + } + + const std::vector>& second_cache() const + { + return main_cache()->m_second_cache; + } + + size_t m_size = 0; + + Eigen::SparseMatrix m_tmp; + + Eigen::SparseMatrix m_mat; + + /// contains global matrix indices and corresponding value + std::vector> m_entries; + + /// maps row indices to column index/local index pairs + std::vector>> m_mapping; + + /// saves inner indices for sparse matrix + std::vector m_inner_index; + + /// saves inner indices for sparse matrix + std::vector m_outer_index; + + /// buffer for values (corresponds to inner/outer_index_ structure for + /// sparse matrix) + std::vector m_values; + + const SparseMatrixCache* m_main_cache = nullptr; + + /// maps element index to local index + std::vector> m_second_cache; + + /// maps element indices to global matrix indices + std::vector>> m_second_cache_entries; + + int m_current_e = -1; + int m_current_e_index = -1; +}; + +} // namespace ipc diff --git a/src/ipc/utils/maybe_parallel_for.hpp b/src/ipc/utils/maybe_parallel_for.hpp new file mode 100644 index 000000000..0ce0b3a59 --- /dev/null +++ b/src/ipc/utils/maybe_parallel_for.hpp @@ -0,0 +1,74 @@ +#pragma once + +#include + +#ifdef IPC_TOOLKIT_WITH_TBB +#include +#else // Not using parallel for +#include +#endif + +/// @file maybe_parallel_for.hpp +/// @brief Utilities for conditional parallel for loops and thread-local storage. + +namespace ipc { + +/// @typedef ParallelCacheType +/// @brief Type alias for thread-local storage depending on threading library. +/// +/// If IPC_TOOLKIT_WITH_TBB is defined, uses tbb::enumerable_thread_specific. +/// Otherwise, uses std::array. +/// +/// @tparam T Type of the thread-local storage. +template +#ifdef IPC_TOOLKIT_WITH_TBB +using ParallelCacheType = tbb::enumerable_thread_specific; +#else +using ParallelCacheType = std::array; +#endif + +/// @brief Executes a (possibly parallel) for loop over a range. +/// +/// The loop runs from 0 to `size` (exclusive) with an increment of 1. +/// The provided function is called with the start, end, and thread id for each +/// partition. The parallelization depends on compile-time definitions. +/// +/// @param size Number of iterations. +/// @param partial_for Function to execute for each partition: void(int start, int end, int thread_id). +inline void maybe_parallel_for( + int size, const std::function& partial_for); + +/// @brief Executes a (possibly parallel) for loop over a range. +/// +/// The loop runs from 0 to `size` (exclusive) with an increment of 1. +/// The provided function is called for each index. +/// The parallelization depends on compile-time definitions. +/// +/// @param size Number of iterations. +/// @param body Function to execute for each index: void(int). +inline void maybe_parallel_for(int size, const std::function& body); + +/// @brief Creates thread-specific storage for use in maybe_parallel_for(). +/// +/// The return type depends on the threading library: +/// - TBB: tbb::enumerable_thread_specific +/// - None: std::array +/// +/// @tparam LocalStorage Type of the local storage. +/// @param initial_local_storage Initial value for each thread's local storage. +/// @return Thread-specific storage container. +template +inline auto create_thread_storage(const LocalStorage& initial_local_storage); + +/// @brief Retrieves the local storage for a specific thread. +/// +/// @tparam Storages Type of the storage container. +/// @param storage Thread-specific storage container. +/// @param thread_id Identifier of the thread. +/// @return Reference to the local storage for the given thread. +template +inline auto& get_local_thread_storage(Storages& storage, int thread_id); + +} // namespace ipc + +#include "maybe_parallel_for.tpp" diff --git a/src/ipc/utils/maybe_parallel_for.tpp b/src/ipc/utils/maybe_parallel_for.tpp new file mode 100644 index 000000000..b05243d1f --- /dev/null +++ b/src/ipc/utils/maybe_parallel_for.tpp @@ -0,0 +1,58 @@ +#include "maybe_parallel_for.hpp" + +#ifdef IPC_TOOLKIT_WITH_TBB +#include +#endif + +namespace ipc { + +inline void maybe_parallel_for( + int size, const std::function& partial_for) +{ +#ifdef IPC_TOOLKIT_WITH_TBB + tbb::parallel_for( + tbb::blocked_range(0, size), + [&](const tbb::blocked_range& r) { + partial_for( + r.begin(), r.end(), + tbb::this_task_arena::current_thread_index()); + }); +#else + partial_for(0, size, /*thread_id=*/0); // actually the full for loop +#endif +} + +inline void maybe_parallel_for(int size, const std::function& body) +{ +#ifdef IPC_TOOLKIT_WITH_TBB + tbb::parallel_for(0, size, body); +#else + for (int i = 0; i < size; ++i) { + body(i); + } +#endif +} + +template +inline auto create_thread_storage(const LocalStorage& initial_local_storage) +{ +#ifdef IPC_TOOLKIT_WITH_TBB + return tbb::enumerable_thread_specific(initial_local_storage); +#else + return std::array { { initial_local_storage } }; +#endif +} + +template +inline auto& get_local_thread_storage(Storages& storage, int thread_id) +{ +#ifdef IPC_TOOLKIT_WITH_TBB + return storage.local(); +#else + assert(thread_id == 0); + assert(storage.size() == 1); + return storage[0]; +#endif +} + +} // namespace ipc diff --git a/src/ipc/utils/unordered_map_and_set.cpp b/src/ipc/utils/unordered_map_and_set.cpp index f396a1eb7..7ecb2be67 100644 --- a/src/ipc/utils/unordered_map_and_set.cpp +++ b/src/ipc/utils/unordered_map_and_set.cpp @@ -13,6 +13,14 @@ AbslHashValue(Hash> h, const std::pair p) return Hash>::combine(std::move(h), p.first, p.second); } +template <> +Hash> +AbslHashValue(Hash> h, std::pair p) +{ + return Hash>::combine( + std::move(h), p.first, p.second); +} + template <> Hash AbslHashValue(Hash h, const int i) { return Hash::combine(std::move(h), i); diff --git a/tests/src/tests/barrier/test_barrier.cpp b/tests/src/tests/barrier/test_barrier.cpp index e85ab5eea..f52c2bc4a 100644 --- a/tests/src/tests/barrier/test_barrier.cpp +++ b/tests/src/tests/barrier/test_barrier.cpp @@ -5,12 +5,16 @@ #include #include +#include +#include +#include #include +#include #include -namespace ipc { +using namespace ipc; /// @warning This implementation will not work with dmin > 0 class PhysicalBarrier : public NormalizedClampedLogBarrier { @@ -39,7 +43,355 @@ class PhysicalBarrier : public NormalizedClampedLogBarrier { bool use_dist_sqr; }; -} // namespace ipc +TEST_CASE("Spline derivatives", "[deriv]") +{ + const int n_samples = 100; + ScalarBase::setVariableCount(1); + using T = ipc::ADHessian<1>; + for (int i = 0; i < n_samples; i++) { + const double x = 2. * i / static_cast(n_samples) - 1; + double deriv = ipc::Math::cubic_spline_grad(x); + double hess = ipc::Math::cubic_spline_hess(x); + T x_ad = T(x, 0); + T y_ad = ipc::Math::cubic_spline(x_ad); + double deriv_ad = y_ad.grad(0); + double hess_ad = y_ad.Hess(0); + + CHECK(abs(deriv_ad - deriv) < 1e-14 * std::max(1., abs(deriv))); + CHECK(abs(hess_ad - hess) < 1e-12 * std::max(1., abs(hess))); + } +} + +TEST_CASE("Heaviside derivatives", "[deriv]") +{ + const int n_samples = 100; + ScalarBase::setVariableCount(1); + using T = ipc::ADHessian<1>; + for (int i = 0; i < n_samples; i++) { + const double x = i / static_cast(n_samples) - 1; + double deriv = ipc::Math::smooth_heaviside_grad(x, 1., 0.); + double hess = ipc::Math::smooth_heaviside_hess(x, 1., 0.); + T x_ad = T(x, 0); + T y_ad = ipc::Math::smooth_heaviside(x_ad, 1., 0.); + double deriv_ad = y_ad.grad(0); + double hess_ad = y_ad.Hess(0); + + CHECK(abs(deriv_ad - deriv) < 1e-14 * std::max(1., abs(deriv))); + CHECK(abs(hess_ad - hess) < 1e-12 * std::max(1., abs(hess))); + } +} + +TEST_CASE("Inv barrier derivatives", "[deriv]") +{ + const int n_samples = 100; + ScalarBase::setVariableCount(1); + using T = ipc::ADHessian<1>; + const int r = 1; + const double dhat = 0.13; + for (int i = 1; i <= n_samples; i++) { + const double x = i / static_cast(n_samples); + double deriv = ipc::Math::inv_barrier_grad(x / dhat, r) / dhat; + double hess = + ipc::Math::inv_barrier_hess(x / dhat, r) / dhat / dhat; + T x_ad = T(x, 0); + T y_ad = ipc::Math::inv_barrier(x_ad / dhat, r); + double deriv_ad = y_ad.grad(0); + double hess_ad = y_ad.Hess(0); + + CHECK(abs(deriv_ad - deriv) < 1e-14 * std::max(1., abs(deriv))); + CHECK(abs(hess_ad - hess) < 1e-12 * std::max(1., abs(hess))); + } + + ScalarBase::setVariableCount(3); + using T3 = ipc::ADHessian<3>; + + for (int i = 1; i <= n_samples; i++) { + Eigen::Vector3d x = Eigen::Vector3d::Random() * dhat / 3.; + double deriv = + ipc::Math::inv_barrier_grad(x.norm() / dhat, r) / dhat; + double hess = ipc::Math::inv_barrier_hess(x.norm() / dhat, r) + / dhat / dhat; + auto x_ad = ipc::slice_positions(x); + T3 y_ad = ipc::Math::inv_barrier(x_ad.norm() / dhat, r); + Eigen::Vector3d deriv_ad = y_ad.grad; + Eigen::Matrix3d hess_ad = y_ad.Hess; + + Eigen::Vector3d xn = x / x.norm(); + Eigen::Vector3d deriv_analytic = deriv * xn; + Eigen::Matrix3d hess_analytic = + (deriv / x.norm()) * Eigen::Matrix3d::Identity() + + (hess - deriv / x.norm()) * xn * xn.transpose(); + + CHECK( + (deriv_ad - deriv_analytic).norm() + < 1e-14 * std::max(1., deriv_ad.norm())); + CHECK( + (hess_ad - hess_analytic).norm() + < 1e-12 * std::max(1., hess_ad.norm())); + } +} + +TEST_CASE("Normalize vector derivatives", "[deriv]") +{ + const int n_samples = 1000; + ScalarBase::setVariableCount(3); + using T = ipc::ADHessian<3>; + for (int i = 1; i <= n_samples; i++) { + Eigen::Vector3d x = Eigen::Vector3d::Random(); + ipc::Vector x_ad = ipc::slice_positions(x); + ipc::Vector y_ad = x_ad / x_ad.norm(); + + const auto [y, grad, hess] = ipc::normalize_vector_hess(x); + + double err_grad = 0, err_hess = 0; + for (int j = 0; j < 3; j++) { + err_grad += (grad.col(j) - y_ad(j).grad).norm(); + err_hess += (hess[j] - y_ad(j).Hess).norm(); + } + + CHECK((grad - grad.transpose()).norm() <= 1e-12); + CHECK((hess[0] - hess[0].transpose()).norm() <= 1e-12); + CHECK(err_grad <= 1e-12); + CHECK(err_hess <= 1e-12); + } +} + +TEST_CASE("line-line closest direction derivatives", "[deriv]") +{ + const int n_samples = 100; + ScalarBase::setVariableCount(12); + using T = ipc::ADHessian<12>; + for (int i = 1; i <= n_samples; i++) { + ipc::Vector6d ea = ipc::Vector6d::Random(); + ipc::Vector eaT = ipc::slice_positions(ea); + ipc::Vector6d eb = ipc::Vector6d::Random(); + ipc::Vector ebT = ipc::slice_positions(eb, 6); + + ipc::Vector dT = ipc::line_line_closest_point_direction( + eaT.head<3>(), eaT.tail<3>(), ebT.head<3>(), ebT.tail<3>()); + + const auto [d1, grad1] = + ipc::line_line_closest_point_direction_gradient( + ea.head<3>(), ea.tail<3>(), eb.head<3>(), eb.tail<3>()); + + const auto [d2, grad2, hess2] = + ipc::line_line_closest_point_direction_hessian( + ea.head<3>(), ea.tail<3>(), eb.head<3>(), eb.tail<3>()); + + double err_grad = (grad1 - grad2).norm(); + double err_hess = 0; + for (int j = 0; j < 3; j++) { + err_grad += (grad2.row(j).transpose() - dT(j).grad).norm(); + err_hess += (hess2[j] - dT(j).Hess).norm(); + } + + CHECK(err_grad <= 1e-12); + CHECK(err_hess <= 1e-10); + } +} + +TEST_CASE("opposite_direction_penalty derivatives", "[deriv]") +{ + const int n_samples = 1000; + ScalarBase::setVariableCount(6); + using T = ipc::ADHessian<6>; + const double alpha = 1; + const double beta = 1; + for (int i = 1; i <= n_samples; i++) { + ipc::Vector6d x = ipc::Vector6d::Random(); + ipc::Vector x_ad = ipc::slice_positions(x); + T y_ad = ipc::Math::smooth_heaviside( + x_ad.tail(3).dot(x_ad.head(3)) / x_ad.head(3).norm(), alpha, beta); + + const auto [y, grad, hess] = ipc::opposite_direction_penalty_hess( + x.head(3), x.tail(3), alpha, beta); + + CHECK((grad - y_ad.grad).norm() <= 1e-12); + CHECK((hess - y_ad.Hess).norm() <= 1e-12); + } +} + +TEST_CASE("negative_orientation_penalty derivatives", "[deriv]") +{ + const int n_samples = 1000; + const double alpha = 1; + const double beta = 1; + for (int i = 1; i <= n_samples; i++) { + ScalarBase::setVariableCount(9); + using T = ipc::ADHessian<9>; + ipc::Vector9d x = ipc::Vector9d::Random(); + ipc::Vector x_ad = ipc::slice_positions(x); + ipc::Vector t = x_ad.head<3>().cross(x_ad.segment<3>(3)); + T y_ad = ipc::Math::smooth_heaviside( + x_ad.tail(3).dot(t) / t.norm(), alpha, beta); + + auto [y, grad, hess] = ipc::negative_orientation_penalty_hess( + x.head(3), x.segment(3, 3), x.tail(3), alpha, beta); + + // hess.topLeftCorner(6, 6).setZero(); + // auto hess_ad = y_ad.Hess; + // hess_ad.topLeftCorner(6, 6).setZero(); + // y_ad = T(y_ad.val, y_ad.grad, hess_ad); + + CHECK(abs(y - y_ad.val) <= 1e-12); + CHECK((grad - y_ad.grad).norm() <= 1e-12); + CHECK((hess - y_ad.Hess).norm() <= 1e-10); + } +} + +TEST_CASE("point term derivatives", "[deriv]") +{ + ipc::SmoothContactParameters params(1, 1, 1, 0.01, 0, 2); + + Eigen::Matrix vectors(9, 3); + vectors << -0.696515, -0.173578, -0.696231, 0.50146, -0.0017947, 0.999718, + 0.346908, 0.152939, 1.00049, 0.208062, 0.290611, 1.00075, 0.280725, + 0.498548, 1.00069, 0.499796, 0.49999, 1.00127, 0.501162, 0.498839, + 0.779581, 0.50047, 0.290411, 0.709051, 0.500615, 0.153465, 0.846423; + + Eigen::Matrix V = vectors; + V.row(0).setZero(); + + Eigen::MatrixXi E(8, 2), F(8, 3); + E << 0, 1, 0, 2, 0, 3, 0, 4, 0, 5, 0, 6, 0, 7, 0, 8; + F << 0, 2, 1, 0, 3, 2, 0, 4, 3, 0, 5, 4, 0, 6, 5, 0, 6, 7, 0, 7, 8, 0, 8, 1; + igl::edges(F, E); + ipc::CollisionMesh mesh(V, E, F); + + auto point_term = + std::make_unique(0, mesh, V, -vectors.row(0), params); + + { + auto [y, y_grad, y_hess] = + point_term->smooth_point3_term_tangent_hessian( + vectors.row(0), V.bottomRows(8), params.alpha_t, params.beta_t); + + Eigen::VectorXd fgrad; + fd::finite_gradient( + fd::flatten(vectors), + [&](const Eigen::VectorXd& x) { + Eigen::MatrixXd V_fd = fd::unflatten(x, 3); + V_fd.row(0).setZero(); + auto point_term_fd = std::make_unique( + 0, mesh, V_fd, -x.head<3>(), params); + return std::get<0>( + point_term_fd->smooth_point3_term_tangent_gradient( + x.head(3), fd::unflatten(x.tail(x.size() - 3), 3), + params.alpha_t, params.beta_t)); + }, + fgrad); + + CHECK((y_grad - fgrad).norm() <= 1e-7 * fgrad.norm()); + + Eigen::MatrixXd fhess; + fd::finite_jacobian( + fd::flatten(vectors), + [&](const Eigen::VectorXd& x) { + Eigen::MatrixXd V_fd = fd::unflatten(x, 3); + V_fd.row(0).setZero(); + auto point_term_fd = std::make_unique( + 0, mesh, V_fd, -x.head<3>(), params); + return std::get<1>( + point_term_fd->smooth_point3_term_tangent_gradient( + x.head(3), fd::unflatten(x.tail(x.size() - 3), 3), + params.alpha_t, params.beta_t)); + }, + fhess); + + CHECK((y_hess - fhess).norm() <= 1e-7 * fhess.norm()); + } + + { + auto y_grad = point_term->grad(vectors.row(0), fd::flatten(V)); + auto y_hess = point_term->hessian(vectors.row(0), fd::flatten(V)); + + Eigen::MatrixXd X(V.rows() + 1, 3); + X << vectors.row(0), V; + + Eigen::VectorXd fgrad; + fd::finite_gradient( + fd::flatten(X), + [&](const Eigen::VectorXd& x) { + return point_term->potential(x.head(3), x.tail(x.size() - 3)); + }, + fgrad); + + CHECK((y_grad - fgrad).norm() <= 1e-7 * fgrad.norm()); + + Eigen::MatrixXd fhess; + fd::finite_jacobian( + fd::flatten(X), + [&](const Eigen::VectorXd& x) { + return point_term->grad(x.head(3), x.tail(x.size() - 3)); + }, + fhess); + + CHECK((y_hess - fhess).norm() <= 1e-7 * fhess.norm()); + } +} + +TEST_CASE("point term normal derivatives", "[deriv]") +{ + ipc::SmoothContactParameters params(1, 1, 1, 1, 0, 2); + + Eigen::Matrix vectors(9, 3); + vectors << -0.696515, -0.173578, -0.696231, 0.50146, -0.0017947, 0.999718, + 0.346908, 0.152939, 1.00049, 0.208062, 0.290611, 1.00075, 0.280725, + 0.498548, 1.00069, 0.499796, 0.49999, 1.00127, 0.501162, 0.498839, + 0.779581, 0.50047, 0.290411, 0.709051, 0.500615, 0.153465, 0.846423; + + Eigen::Matrix V = vectors; + V.row(0).setZero(); + + Eigen::MatrixXi E(3, 2), F(1, 3); + E << 0, 1, 0, 2, 1, 2; + F << 0, 1, 2; + igl::edges(F, E); + ipc::CollisionMesh mesh(V, E, F); + + auto point_term = + std::make_unique(0, mesh, V, -vectors.row(0), params); + + { + auto [y, y_grad, y_hess] = + point_term->smooth_point3_term_normal_hessian( + vectors.row(0), V.bottomRows(8), params.alpha_n, params.beta_n); + + Eigen::VectorXd fgrad; + fd::finite_gradient( + fd::flatten(vectors), + [&](const Eigen::VectorXd& x) { + Eigen::MatrixXd V_fd = fd::unflatten(x, 3); + V_fd.row(0).setZero(); + auto point_term_fd = std::make_unique( + 0, mesh, V_fd, -x.head<3>(), params); + return std::get<0>( + point_term_fd->smooth_point3_term_normal_hessian( + x.head(3), fd::unflatten(x.tail(x.size() - 3), 3), + params.alpha_n, params.beta_n)); + }, + fgrad); + + CHECK((y_grad - fgrad).norm() <= 1e-7 * fgrad.norm()); + + Eigen::MatrixXd fhess; + fd::finite_jacobian( + fd::flatten(vectors), + [&](const Eigen::VectorXd& x) { + Eigen::MatrixXd V_fd = fd::unflatten(x, 3); + V_fd.row(0).setZero(); + auto point_term_fd = std::make_unique( + 0, mesh, V_fd, -x.head<3>(), params); + return std::get<1>( + point_term_fd->smooth_point3_term_normal_hessian( + x.head(3), fd::unflatten(x.tail(x.size() - 3), 3), + params.alpha_n, params.beta_n)); + }, + fhess, fd::AccuracyOrder::FOURTH, 1e-6); + + CHECK((y_hess - fhess).norm() <= 1e-7 * fhess.norm()); + } +} TEST_CASE("Barrier derivatives", "[barrier]") { @@ -65,7 +417,7 @@ TEST_CASE("Barrier derivatives", "[barrier]") } SECTION("Physical") { - barrier = std::make_unique(use_dist_sqr); + barrier = std::make_unique(use_dist_sqr); } SECTION("ClampedLogSq") { @@ -111,7 +463,7 @@ TEST_CASE("Physical barrier", "[barrier]") const bool use_dist_sqr = GENERATE(false, true); ipc::ClampedLogBarrier original_barrier; - ipc::PhysicalBarrier new_barrier(use_dist_sqr); + PhysicalBarrier new_barrier(use_dist_sqr); const double dhat = pow(10, GENERATE_COPY(range(use_dist_sqr ? -5 : -5, 0))); diff --git a/tests/src/tests/broad_phase/test_broad_phase.cpp b/tests/src/tests/broad_phase/test_broad_phase.cpp index db7a0bec3..cea066e05 100644 --- a/tests/src/tests/broad_phase/test_broad_phase.cpp +++ b/tests/src/tests/broad_phase/test_broad_phase.cpp @@ -3,6 +3,7 @@ #include #include +#include #include #include @@ -288,4 +289,16 @@ TEST_CASE("Broad phase build from boxes", "[broad_phase]") CAPTURE(broad_phase->name()); CHECK(candidates.size() == boxes.size() - 1); +} + +TEST_CASE("Create broad phase", "[broad_phase]") +{ +#ifdef IPC_TOOLKIT_WITH_CUDA + uint8_t n_broad_phase_methods = 6; +#else + uint8_t n_broad_phase_methods = 5; +#endif + for (uint8_t i = 0; i < n_broad_phase_methods; i++) { + CHECK(create_broad_phase(static_cast(i))); + } } \ No newline at end of file diff --git a/tests/src/tests/distance/test_distance_type.cpp b/tests/src/tests/distance/test_distance_type.cpp index 1aac4190b..e5b76de80 100644 --- a/tests/src/tests/distance/test_distance_type.cpp +++ b/tests/src/tests/distance/test_distance_type.cpp @@ -227,8 +227,9 @@ TEST_CASE("Edge-edge distance type", "[distance][distance-type][edge-edge]") { double alpha = GENERATE(range(-1.0, 2.0, 0.1)); double s = GENERATE(range(-10.0, 10.0, 1.0)); - if (s == 0) + if (s == 0) { return; + } const bool swap_ea = GENERATE(false, true); const bool swap_eb = GENERATE(false, true); const bool swap_edges = GENERATE(false, true); diff --git a/tests/src/tests/distance/test_edge_edge.cpp b/tests/src/tests/distance/test_edge_edge.cpp index 098473b4b..5a9a35d0e 100644 --- a/tests/src/tests/distance/test_edge_edge.cpp +++ b/tests/src/tests/distance/test_edge_edge.cpp @@ -3,9 +3,12 @@ #include #include #include +#include #include #include +#include +#include #include #include @@ -49,6 +52,12 @@ TEST_CASE("Edge-edge distance", "[distance][edge-edge]") CAPTURE(e0x, e0y, e0z, edge_edge_distance_type(e00, e01, e10, e11)); CHECK(distance == Catch::Approx(expected_distance).margin(1e-12)); + + distance = + PrimitiveDistanceTemplate::compute_distance( + (Vector12d() << e00, e01, e10, e11).finished(), + edge_edge_distance_type(e00, e01, e10, e11)); + CHECK(distance == Catch::Approx(expected_distance)); } TEST_CASE("Edge-edge distance !EA_EB", "[distance][edge-edge]") @@ -98,11 +107,17 @@ TEST_CASE("Edge-edge distance !EA_EB", "[distance][edge-edge]") std::swap(ea1, eb1); } - const double distance = edge_edge_distance(ea0, ea1, eb0, eb1); + double distance = edge_edge_distance(ea0, ea1, eb0, eb1); CAPTURE(alpha, s, swap_ea, swap_eb, swap_edges); CHECK(distance == Catch::Approx(s * s).margin(1e-15)); + + distance = + PrimitiveDistanceTemplate::compute_distance( + (Vector12d() << ea0, ea1, eb0, eb1).finished(), + edge_edge_distance_type(ea0, ea1, eb0, eb1)); + CHECK(distance == Catch::Approx(s * s).margin(1e-15)); } } @@ -143,7 +158,13 @@ TEST_CASE("Edge-edge distance EA_EB", "[distance][edge-edge]") std::swap(ea1, eb1); } - const double distance = edge_edge_distance(ea0, ea1, eb0, eb1); + double distance = edge_edge_distance(ea0, ea1, eb0, eb1); + CHECK(distance == Catch::Approx(s * s).margin(1e-15)); + + distance = + PrimitiveDistanceTemplate::compute_distance( + (Vector12d() << ea0, ea1, eb0, eb1).finished(), + edge_edge_distance_type(ea0, ea1, eb0, eb1)); CHECK(distance == Catch::Approx(s * s).margin(1e-15)); } } @@ -272,6 +293,13 @@ TEST_CASE("Edge-edge distance gradient", "[distance][edge-edge][gradient]") CAPTURE(e0x, e0y, e0z, edge_edge_distance_type(e00, e01, e10, e11)); CHECK(fd::compare_gradient(grad, fgrad)); + + Vector, 12> X1 = slice_positions, 12, 1>( + (Vector12d() << e00, e01, e10, e11).finished()); + ADGrad<12> dist1 = + PrimitiveDistanceTemplate>::compute_distance( + X1, edge_edge_distance_type(e00, e01, e10, e11)); + CHECK(fd::compare_gradient(dist1.grad, fgrad)); } TEST_CASE( @@ -303,5 +331,243 @@ TEST_CASE( CAPTURE(angle, (grad - fgrad).squaredNorm()); CHECK(distance == Catch::Approx(1.0)); CHECK(fd::compare_gradient(grad, fgrad)); - // CHECK(distance.getHessian().squaredNorm() != Catch::Approx(0.0)); + // CHECK(distance.Hess.squaredNorm() != Catch::Approx(0.0)); +} + +TEST_CASE( + "Edge-edge closest direction gradient !EA_EB", "[distance][edge-edge]") +{ + double alpha = GENERATE(range(-1.0, 2.0, 0.1)); + double s = GENERATE(range(-10.0, 10.0, 1.0)); + if (s == 0) + return; + const bool swap_ea = GENERATE(false, true); + const bool swap_eb = GENERATE(false, true); + const bool swap_edges = GENERATE(false, true); + const int n_random_edges = 20; + + const auto sign = [](double x) { return x < 0 ? -1 : 1; }; + + for (int i = 0; i < n_random_edges; i++) { + Eigen::Vector3d ea0 = Eigen::Vector3d::Random(); + Eigen::Vector3d ea1 = Eigen::Vector3d::Random(); + Eigen::Vector3d n = + (ea1 - ea0).cross(Eigen::Vector3d::UnitX()).normalized(); + + Eigen::Vector3d eb0 = ((ea1 - ea0) * alpha + ea0) + s * n; + if (alpha < 0) { + alpha = 0; + n = eb0 - ea0; + s = n.norm(); + n /= s; + } else if (alpha > 1) { + alpha = 1; + n = eb0 - ea1; + s = n.norm(); + n /= s; + } + REQUIRE(s != 0); + + Eigen::Vector3d eb1 = + ((ea1 - ea0) * alpha + ea0) + (s + sign(s) * 1) * n; + + if (swap_ea) { + std::swap(ea0, ea1); + } + if (swap_eb) { + std::swap(eb0, eb1); + } + if (swap_edges) { + std::swap(ea0, eb0); + std::swap(ea1, eb1); + } + + double distance = edge_edge_distance(ea0, ea1, eb0, eb1); + + CAPTURE(alpha, s, swap_ea, swap_eb, swap_edges); + + CHECK(distance == Catch::Approx(s * s).margin(1e-15)); + + Vector12d x; + x << ea0, ea1, eb0, eb1; + const auto dtype = edge_edge_distance_type(ea0, ea1, eb0, eb1); + + distance = + PrimitiveDistanceTemplate::compute_distance( + x, dtype); + CHECK(distance == Catch::Approx(s * s).margin(1e-15)); + + Vector, 12> X = slice_positions, 12, 1>(x); + Vector, 3> direc = PrimitiveDistanceTemplate< + Edge3, Edge3, ADGrad<12>>::compute_closest_direction(X, dtype); + CHECK(direc.squaredNorm().val == Catch::Approx(s * s).margin(1e-15)); + + // Compute the gradient using finite differences + Eigen::MatrixXd fjac; + fd::finite_jacobian( + x, + [dtype](const Eigen::VectorXd& _x) { + return PrimitiveDistanceTemplate< + Edge3, Edge3, double>::compute_closest_direction(_x, dtype); + }, + fjac); + + for (int d = 0; d < 3; d++) { + CHECK( + fd::compare_jacobian( + fjac.row(d), direc(d).grad.transpose(), 1e-5)); + } + } } + +TEST_CASE("Edge normal term", "[distance][edge-edge][gradient]") +{ + OrientationTypes otypes; + otypes.set_size(2); + const double alpha = 0.85; + const double beta = 0.2; + SmoothContactParameters params { 1e-3, 1, 0, alpha, beta, 2 }; + + Eigen::Vector3d dn, e0, e1, f0, f1; + e0 << 0, 0, 0; + e1 << 1, 0, 0; + dn << 0, 0, 1; + f0 << 0.4, 0.3, GENERATE(take(10, random(-0.2, 0.2))); + f1 << 0.6, -0.2, GENERATE(take(10, random(-0.2, 0.2))); + + ipc::Vector15d x; + x << dn, e0, e1, f0, f1; + + const auto [val, grad, hess] = smooth_edge3_normal_term_hessian( + dn, e0, e1, f0, f1, alpha, beta, otypes); + + Eigen::VectorXd fgrad; + fd::finite_gradient( + x, + [&](const Eigen::VectorXd& y) { + return smooth_edge3_normal_term( + y.head(3), y.segment(3, 3), y.segment(6, 3), y.segment(9, 3), + y.segment(12, 3), alpha, beta, otypes); + }, + fgrad, fd::AccuracyOrder::FOURTH, 1e-5); + + CHECK((fgrad - grad).norm() / 1e-8 <= grad.norm()); + + Eigen::MatrixXd fhess; + fd::finite_jacobian( + x, + [&](const Eigen::VectorXd& y) { + return std::get<1>(smooth_edge3_normal_term_gradient( + y.head(3), y.segment(3, 3), y.segment(6, 3), y.segment(9, 3), + y.segment(12, 3), alpha, beta, otypes)); + }, + fhess, fd::AccuracyOrder::SECOND, 1e-7); + + CHECK((fhess - hess).norm() / 1e-6 <= hess.norm()); +} + +TEMPLATE_TEST_CASE_SIG( + "Edge-edge templated functions !EA_EB", + "[distance][edge-edge]", + ((int ndofs), ndofs), + 9, + 12, + 13) +{ + double alpha = GENERATE(range(-1.0, 2.0, 0.4)); + double s = GENERATE(range(-10.0, 10.0, 5.0)); + if (s == 0) + return; + const bool swap_ea = GENERATE(false, true); + const bool swap_eb = GENERATE(false, true); + const bool swap_edges = GENERATE(false, true); + const int n_random_edges = 5; + + const auto sign = [](double x) { return x < 0 ? -1 : 1; }; + + for (int i = 0; i < n_random_edges; i++) { + Eigen::Vector3d ea0 = Eigen::Vector3d::Random(); + Eigen::Vector3d ea1 = Eigen::Vector3d::Random(); + Eigen::Vector3d n = + (ea1 - ea0).cross(Eigen::Vector3d::UnitX()).normalized(); + + Eigen::Vector3d eb0 = ((ea1 - ea0) * alpha + ea0) + s * n; + if (alpha < 0) { + alpha = 0; + n = eb0 - ea0; + s = n.norm(); + n /= s; + } else if (alpha > 1) { + alpha = 1; + n = eb0 - ea1; + s = n.norm(); + n /= s; + } + REQUIRE(s != 0); + + Eigen::Vector3d eb1 = + ((ea1 - ea0) * alpha + ea0) + (s + sign(s) * 1) * n; + + if (swap_ea) { + std::swap(ea0, ea1); + } + if (swap_eb) { + std::swap(eb0, eb1); + } + if (swap_edges) { + std::swap(ea0, eb0); + std::swap(ea1, eb1); + } + + Vector12d x; + x << ea0, ea1, eb0, eb1; + const auto dtype = edge_edge_distance_type(ea0, ea1, eb0, eb1); + + auto distance1 = PrimitiveDistanceTemplate< + Edge3, Edge3, ADGrad>::compute_distance(x, dtype); + CHECK(distance1.val == Catch::Approx(s * s).margin(1e-15)); + + auto distance2 = PrimitiveDistanceTemplate< + Edge3, Edge3, ADHessian>::compute_distance(x, dtype); + CHECK(distance2.val == Catch::Approx(s * s).margin(1e-15)); + } +} + +TEMPLATE_TEST_CASE_SIG( + "Edge-edge templated functions EA_EB", + "[distance][edge-edge]", + ((int ndofs), ndofs), + 9, + 12, + 13) +{ + Eigen::Vector3d e00(-1, 0, 0); + Eigen::Vector3d e01(1, 0, 0); + Eigen::Vector3d e10(0, 0, -1); + Eigen::Vector3d e11(0, 0, 1); + + Vector12d x; + x << e00, e01, e10, e11; + const auto dtype = edge_edge_distance_type(e00, e01, e10, e11); + double expected_distance = edge_edge_distance(e00, e01, e10, e11, dtype); + + auto distance1 = PrimitiveDistanceTemplate< + Edge3, Edge3, ADGrad>::compute_distance(x, dtype); + CHECK(distance1.val == Catch::Approx(expected_distance).margin(1e-15)); + + auto distance2 = PrimitiveDistanceTemplate< + Edge3, Edge3, ADHessian>::compute_distance(x, dtype); + CHECK(distance2.val == Catch::Approx(expected_distance).margin(1e-15)); + + auto direc1 = PrimitiveDistanceTemplate< + Edge3, Edge3, ADGrad>::compute_closest_direction(x, dtype); + CHECK( + direc1.squaredNorm().val + == Catch::Approx(expected_distance).margin(1e-15)); + + auto direc2 = PrimitiveDistanceTemplate< + Edge3, Edge3, ADHessian>::compute_closest_direction(x, dtype); + CHECK( + direc2.squaredNorm().val + == Catch::Approx(expected_distance).margin(1e-15)); +} \ No newline at end of file diff --git a/tests/src/tests/distance/test_line_line.cpp b/tests/src/tests/distance/test_line_line.cpp index 53d850832..8ac7c4356 100644 --- a/tests/src/tests/distance/test_line_line.cpp +++ b/tests/src/tests/distance/test_line_line.cpp @@ -2,8 +2,10 @@ #include #include #include +#include #include +#include #include #include @@ -19,6 +21,26 @@ double line_line_distance_stacked(const Eigen::VectorXd& x) } } // namespace +TEST_CASE("Line-line closest point pair", "[distance][line-line]") +{ + double ya = GENERATE(take(5, random(-100.0, 100.0))); + double za = GENERATE(take(5, random(-0.5, 0.5))); + Eigen::Vector3d ea0(-1, ya, za), ea1(1, ya, 0); + + double yb = GENERATE(take(5, random(-100.0, 100.0))); + double zb = GENERATE(take(5, random(-0.5, 0.5))); + Eigen::Vector3d eb0(0, yb, -1 - zb), eb1(0, yb, 1); + + double expected_distance = line_line_distance(ea0, ea1, eb0, eb1); + + Eigen::Matrix closest_points = + line_line_closest_point_pairs(ea0, ea1, eb0, eb1); + double distance = + (closest_points.col(0) - closest_points.col(1)).squaredNorm(); + + CHECK(distance == Catch::Approx(expected_distance)); +} + TEST_CASE("Line-line distance", "[distance][line-line]") { double ya = GENERATE(take(10, random(-100.0, 100.0))); @@ -72,3 +94,93 @@ TEST_CASE("Line-line distance hessian", "[distance][line-line][hessian]") CAPTURE(ya, yb, (hess - expected_hess).norm()); CHECK(fd::compare_hessian(hess, expected_hess, 1e-2)); } + +TEST_CASE( + "Line-line closest direction hessian", "[distance][line-line][hessian]") +{ + double ya = GENERATE(take(2, random(-10.0, 10.0))); + Eigen::Vector3d ea0(-1, ya, 0), ea1(1, ya, 0); + + double yb = GENERATE(take(2, random(-10.0, 10.0))); + Eigen::Vector3d eb0(0, yb, -1), eb1(0, yb, 1); + + using T = ADHessian<12>; + ScalarBase::setVariableCount(12); + const auto x = slice_positions( + (Vector12d() << ea0, ea1, eb0, eb1).finished()); + BENCHMARK("AutoDiff Hessian") + { + line_line_closest_point_direction( + x.row(0), x.row(1), x.row(2), x.row(3)); + }; + BENCHMARK("Hessian") + { + line_line_closest_point_direction_hessian(ea0, ea1, eb0, eb1); + }; +} + +TEST_CASE( + "Line-line closest point pairs gradient", "[distance][line-line][gradient]") +{ + double ya = GENERATE(take(10, random(-10.0, 10.0))); + Eigen::Vector3d ea0(-1, ya, 0), ea1(1, ya, 0); + + double yb = GENERATE(take(10, random(-10.0, 10.0))); + Eigen::Vector3d eb0(0, yb, -1), eb1(0, yb, 1); + + using T = ADGrad<12>; + ScalarBase::setVariableCount(12); + const auto x = slice_positions( + (Vector12d() << ea0, ea1, eb0, eb1).finished()); + auto yAD = line_line_closest_point_pairs( + x.row(0), x.row(1), x.row(2), x.row(3)); + auto [y, grad] = line_line_closest_point_pairs_gradient(ea0, ea1, eb0, eb1); + for (int i = 0; i < yAD.size(); i++) { + REQUIRE((yAD(i).val - y(i)) < 1e-8); + REQUIRE( + (yAD(i).grad - grad.row(i).transpose()).norm() + < 1e-6 * grad.row(i).norm()); + } + + // BENCHMARK("AutoDiff Gradient") { + // line_line_closest_point_pairs(x.row(0), x.row(1), x.row(2), + // x.row(3)); + // }; + // BENCHMARK("Gradient") { + // line_line_closest_point_pairs_gradient(ea0, ea1, eb0, eb1); + // }; +} + +TEST_CASE( + "Line-line closest point pairs hessian", "[distance][line-line][hessian]") +{ + double ya = GENERATE(take(10, random(-10.0, 10.0))); + Eigen::Vector3d ea0(-1, ya, 0.05), ea1(1, ya, 0); + + double yb = GENERATE(take(10, random(-10.0, 10.0))); + Eigen::Vector3d eb0(0, yb, -1), eb1(0, yb, 1.02); + + using T = ADHessian<12>; + ScalarBase::setVariableCount(12); + const auto x = slice_positions( + (Vector12d() << ea0, ea1, eb0, eb1).finished()); + auto yAD = line_line_closest_point_pairs( + x.row(0), x.row(1), x.row(2), x.row(3)); + auto [y, grad, hess] = + line_line_closest_point_pairs_hessian(ea0, ea1, eb0, eb1); + for (int i = 0; i < yAD.size(); i++) { + REQUIRE((yAD(i).val - y(i)) < 1e-8); + REQUIRE( + (yAD(i).grad - grad.row(i).transpose()).norm() + < 1e-8 * grad.row(i).norm()); + REQUIRE((yAD(i).Hess - hess[i]).norm() < 1e-8 * hess[i].norm()); + } + + // BENCHMARK("AutoDiff Hessian") { + // line_line_closest_point_pairs(x.row(0), x.row(1), x.row(2), + // x.row(3)); + // }; + // BENCHMARK("Hessian") { + // line_line_closest_point_pairs_hessian(ea0, ea1, eb0, eb1); + // }; +} \ No newline at end of file diff --git a/tests/src/tests/distance/test_point_edge.cpp b/tests/src/tests/distance/test_point_edge.cpp index 23a78bc3a..5adc739b8 100644 --- a/tests/src/tests/distance/test_point_edge.cpp +++ b/tests/src/tests/distance/test_point_edge.cpp @@ -5,6 +5,8 @@ #include #include +#include +#include #include #include @@ -193,3 +195,127 @@ TEMPLATE_TEST_CASE_SIG( CHECK(fd::compare_hessian(hess, fhess, 1e-2)); } } + +TEMPLATE_TEST_CASE_SIG( + "Point-edge closest direction all types", + "[distance][point-edge][gradient][hessian]", + ((int dim), dim), + 2, + 3) +{ + // Note: Closest direction is not differentiable at alpha=0, 1 + + const double alpha = GENERATE(range(-1.0, 2.0, 0.12)); + const double d = GENERATE(range(-10.0, 10.0, 1.0)); + + VectorMax3d e0 = VectorMax3d::Zero(dim); + e0.x() = -10; + VectorMax3d e1 = VectorMax3d::Zero(dim); + e1.x() = 10; + + const VectorMax3d p = + ((e1 - e0) * alpha + e0) + d * Eigen::Vector3d::UnitY().head(dim); + + CAPTURE(alpha, d, dim); + + const auto dtype = point_edge_distance_type(p, e0, e1); + { // Distance type + if (alpha < -1e-8) { + CHECK(dtype == PointEdgeDistanceType::P_E0); + } else if (alpha > 1 + 1e-8) { + CHECK(dtype == PointEdgeDistanceType::P_E1); + } else if (alpha > 1e-8 && alpha < 1 - 1e-8) { + CHECK(dtype == PointEdgeDistanceType::P_E); + } + // ignore alpha ≈ 0 and alpha ≈ 1 + } + + { // Distance + const double expected_distance = alpha < 0 + ? (e0 - p).squaredNorm() + : (alpha > 1 ? (e1 - p).squaredNorm() : (d * d)); + const double distance = + PointEdgeDistance::point_edge_closest_point_direction( + p, e0, e1, dtype) + .squaredNorm(); + CHECK(distance == Catch::Approx(expected_distance).margin(1e-15)); + } + + // Gradient (skip C1 transition points) + if (abs(alpha) > 1e-5 && abs(alpha - 1.0) > 1e-5) { + const auto [vec, grad] = PointEdgeDistanceDerivatives< + dim>::point_edge_closest_point_direction_grad(p, e0, e1, dtype); + + // Compute the gradient using finite differences + VectorMax9d x(3 * dim); + x << p, e0, e1; + Eigen::MatrixXd fgrad; + fd::finite_jacobian( + x, + [](const Eigen::VectorXd& x) { + Vector p = x.segment(0); + Vector e0 = x.segment(dim); + Vector e1 = x.segment(2 * dim); + return PointEdgeDistance< + double, dim>::point_edge_closest_point_direction(p, e0, e1); + }, + fgrad); + + CHECK(fd::compare_jacobian(grad, fgrad, 1e-6)); + } + + // Gradient (skip C1 transition points) + if (abs(alpha) > 1e-5 && abs(alpha - 1.0) > 1e-5) { + VectorMax9d x(3 * dim); + x << e0, e1, p; + + Vector, 3 * dim> X = + slice_positions, 3 * dim, 1>(x); + ADGrad<3 * dim> dist; + if constexpr (dim == 2) { + dist = PrimitiveDistanceTemplate< + Edge2, Point2, ADGrad<3 * dim>>::compute_distance(X, dtype); + } else { + dist = PrimitiveDistanceTemplate< + Edge3, Point3, ADGrad<3 * dim>>::compute_distance(X, dtype); + } + + // Compute the gradient using finite differences + Eigen::VectorXd fgrad; + fd::finite_gradient( + x, + [](const Eigen::VectorXd& x) { + Vector e0 = x.segment(0); + Vector e1 = x.segment(dim); + Vector p = x.segment(2 * dim); + return PointEdgeDistance::point_edge_sqr_distance( + p, e0, e1); + }, + fgrad); + + CHECK(fd::compare_gradient(dist.grad, fgrad, 1e-6)); + } + + // Hessian (skip C1 transition points) + if (abs(alpha) > 1e-5 && abs(alpha - 1.0) > 1e-5) { + const auto [vec, grad, hess] = PointEdgeDistanceDerivatives< + dim>::point_edge_closest_point_direction_hessian(p, e0, e1, dtype); + // Compute the gradient using finite differences + VectorMax9d x(3 * dim); + x << p, e0, e1; + for (int i = 0; i < dim; ++i) { + Eigen::MatrixXd fhess; + fd::finite_hessian( + x, + [i, dtype](const Eigen::VectorXd& x) { + Vector p = x.segment(0); + Vector e0 = x.segment(dim); + Vector e1 = x.segment(2 * dim); + return PointEdgeDistance:: + point_edge_closest_point_direction(p, e0, e1, dtype)(i); + }, + fhess); + CHECK(fd::compare_hessian(hess[i], fhess, 1e-4)); + } + } +} \ No newline at end of file diff --git a/tests/src/tests/distance/test_point_line.cpp b/tests/src/tests/distance/test_point_line.cpp index c38e56b50..3d2f52a1e 100644 --- a/tests/src/tests/distance/test_point_line.cpp +++ b/tests/src/tests/distance/test_point_line.cpp @@ -5,9 +5,11 @@ #include #include #include +#include #include #include +#include #include @@ -154,3 +156,79 @@ TEST_CASE( CHECK(fd::compare_hessian(hess, fhess, 1e-2)); } + +TEST_CASE( + "Point-line closest point 3D pairs hessian", + "[distance][point-line][hessian]") +{ + double ya = GENERATE(take(2, random(-10.0, 10.0))); + Eigen::Vector3d p(1, ya, 0); + + double yb = GENERATE(take(2, random(-10.0, 10.0))); + Eigen::Vector3d eb0(0, yb, -1), eb1(0, yb, 1.02); + + using T = ADHessian<9>; + ScalarBase::setVariableCount(9); + const auto x = + slice_positions((Vector9d() << p, eb0, eb1).finished()); + auto yAD = PointEdgeDistance::point_line_closest_point_direction( + x.row(0), x.row(1), x.row(2)); + auto [y, grad, hess] = PointEdgeDistanceDerivatives< + 3>::point_line_closest_point_direction_hessian(p, eb0, eb1); + for (int i = 0; i < yAD.size(); i++) { + REQUIRE((yAD(i).val - y(i)) < 1e-8); + REQUIRE( + (yAD(i).grad - grad.row(i).transpose()).norm() + < 1e-8 * grad.row(i).norm()); + REQUIRE((yAD(i).Hess - hess[i]).norm() < 1e-8 * hess[i].norm()); + } + + BENCHMARK("AutoDiff Hessian") + { + PointEdgeDistance::point_line_closest_point_direction( + x.row(0), x.row(1), x.row(2)); + }; + BENCHMARK("Hessian") + { + PointEdgeDistanceDerivatives< + 3>::point_line_closest_point_direction_hessian(p, eb0, eb1); + }; +} + +TEST_CASE( + "Point-line closest point 2D pairs hessian", + "[distance][point-line][hessian]") +{ + double ya = GENERATE(take(2, random(-10.0, 10.0))); + Eigen::Vector2d p(ya, 0); + + double yb = GENERATE(take(2, random(-10.0, 10.0))); + Eigen::Vector2d eb0(yb, -1), eb1(yb, 1.02); + + using T = ADHessian<6>; + ScalarBase::setVariableCount(6); + const auto x = + slice_positions((Vector6d() << p, eb0, eb1).finished()); + auto yAD = PointEdgeDistance::point_line_closest_point_direction( + x.row(0), x.row(1), x.row(2)); + auto [y, grad, hess] = PointEdgeDistanceDerivatives< + 2>::point_line_closest_point_direction_hessian(p, eb0, eb1); + for (int i = 0; i < yAD.size(); i++) { + REQUIRE((yAD(i).val - y(i)) < 1e-8); + REQUIRE( + (yAD(i).grad - grad.row(i).transpose()).norm() + < 1e-8 * grad.row(i).norm()); + REQUIRE((yAD(i).Hess - hess[i]).norm() < 1e-8 * hess[i].norm()); + } + + BENCHMARK("AutoDiff Hessian") + { + PointEdgeDistance::point_line_closest_point_direction( + x.row(0), x.row(1), x.row(2)); + }; + BENCHMARK("Hessian") + { + PointEdgeDistanceDerivatives< + 2>::point_line_closest_point_direction_hessian(p, eb0, eb1); + }; +} diff --git a/tests/src/tests/distance/test_point_point.cpp b/tests/src/tests/distance/test_point_point.cpp index ec405c8e3..0fa5c34b5 100644 --- a/tests/src/tests/distance/test_point_point.cpp +++ b/tests/src/tests/distance/test_point_point.cpp @@ -4,7 +4,9 @@ #include #include +#include #include +#include #include @@ -62,6 +64,20 @@ TEMPLATE_TEST_CASE_SIG( fd::finite_gradient(x, point_point_distance_stacked, fgrad); CHECK(fd::compare_gradient(grad, fgrad)); + + constexpr int n_dofs = 2 * dim; + Vector, n_dofs> X = + slice_positions, n_dofs, 1>(x); + ADGrad dist; + if constexpr (dim == 2) { + dist = PrimitiveDistanceTemplate>:: + compute_distance(X, PointPointDistanceType::AUTO); + } else { + dist = PrimitiveDistanceTemplate>:: + compute_distance(X, PointPointDistanceType::AUTO); + } + + CHECK(fd::compare_gradient(dist.grad, fgrad)); } TEMPLATE_TEST_CASE_SIG( diff --git a/tests/src/tests/distance/test_point_triangle.cpp b/tests/src/tests/distance/test_point_triangle.cpp index 773d06b19..3ed910814 100644 --- a/tests/src/tests/distance/test_point_triangle.cpp +++ b/tests/src/tests/distance/test_point_triangle.cpp @@ -3,9 +3,13 @@ #include #include #include +#include +#include #include #include +#include +#include #include #include @@ -96,6 +100,14 @@ TEST_CASE("Point-triangle distance", "[distance][point-triangle]") CHECK( distance == Catch::Approx(point_point_distance(p, closest_point)).margin(1e-12)); + + distance = + PrimitiveDistanceTemplate::compute_distance( + (Vector12d() << t0, t1, t2, p).finished(), + point_triangle_distance_type(p, t0, t1, t2)); + CHECK( + distance + == Catch::Approx(point_point_distance(p, closest_point)).margin(1e-12)); } TEST_CASE( @@ -178,6 +190,18 @@ TEST_CASE( fgrad); CHECK(fd::compare_gradient(grad, fgrad)); + + Vector, 12> X = slice_positions, 12, 1>( + (Vector12d() << p, t0, t1, t2).finished()); + { + Vector, 3> tmp = X.head<3>(); + X.head<9>() = X.tail<9>().eval(); + X.tail<3>() = tmp; + } + ADGrad<12> dist = + PrimitiveDistanceTemplate>::compute_distance( + X, point_triangle_distance_type(p, t0, t1, t2)); + CHECK(fd::compare_gradient(dist.grad, fgrad)); } TEST_CASE("Point-triangle distance hessian", "[distance][point-triangle][hess]") @@ -263,4 +287,231 @@ TEST_CASE("Point-triangle distance hessian", "[distance][point-triangle][hess]") CAPTURE(dtype); CHECK(fd::compare_hessian(hess, fhess, 1e-2)); + + Vector, 12> X = slice_positions, 12, 1>(x); + { + Vector, 3> tmp = X.head<3>(); + X.head<9>() = X.tail<9>().eval(); + X.tail<3>() = tmp; + } + ADHessian<12> dist = + PrimitiveDistanceTemplate>:: + compute_distance(X, point_triangle_distance_type(p, t0, t1, t2)); + CHECK(fd::compare_hessian(dist.Hess, fhess, 1e-2)); } + +TEST_CASE( + "Point-triangle closest direction derivatives", + "[distance][point-triangle][grad]") +{ + double py = GENERATE(-10, -1, -1e-5, 0, 1e-5, 1, 10); + Eigen::Vector3d p(0, py, 0); + Eigen::Vector3d t0(-1, 0, 1); + Eigen::Vector3d t1(1, 0, 1); + Eigen::Vector3d t2(0, 0, -1); + + SECTION("closest to triangle") + { + double pz = GENERATE(0, -1 + 1e-5, -1, 1, 1 - 1e-5); + p.z() = pz; + } + SECTION("closest to t0") + { + double px = GENERATE(-1, -1 - 1e-5, -11); + p.x() = px; + p.z() = t0.z(); + } + SECTION("closest to t1") + { + double px = GENERATE(1, 1 + 1e-5, 11); + p.x() = px; + p.z() = t1.z(); + } + SECTION("closest to t2") + { + double pz = GENERATE(-1, -1 - 1e-5, -11); + p.z() = pz; + } + SECTION("closest to t0t1") + { + double alpha = GENERATE(0.0, 1e-4, 0.5, 1.0 - 1e-4, 1.0); + Eigen::Vector3d closest_point = (t1 - t0) * alpha + t0; + Eigen::Vector2d perp = tests::edge_normal( + Eigen::Vector2d(t0.x(), t0.z()), Eigen::Vector2d(t1.x(), t1.z())); + // double scale = GENERATE(0, 1e-5, 1e-4, 1, 2, 11, 1000); + double scale = GENERATE(0, 1e-5, 1e-4, 1, 2, 11); + p.x() = closest_point.x() + scale * perp.x(); + p.z() = closest_point.z() + scale * perp.y(); + } + SECTION("closest to t1t2") + { + double alpha = GENERATE(0.0, 1e-4, 0.5, 1.0 - 1e-4, 1.0); + Eigen::Vector3d closest_point = (t2 - t1) * alpha + t1; + Eigen::Vector2d perp = tests::edge_normal( + Eigen::Vector2d(t1.x(), t1.z()), Eigen::Vector2d(t2.x(), t2.z())); + // double scale = GENERATE(0, 1e-5, 1e-4, 1, 2, 11, 1000); + double scale = GENERATE(0, 1e-5, 1e-4, 1, 2, 11); + p.x() = closest_point.x() + scale * perp.x(); + p.z() = closest_point.z() + scale * perp.y(); + } + SECTION("closest to t2t0") + { + double alpha = GENERATE(0.0, 1e-4, 0.5, 1.0 - 1e-4, 1.0); + Eigen::Vector3d closest_point = (t0 - t2) * alpha + t2; + Eigen::Vector2d perp = tests::edge_normal( + Eigen::Vector2d(t2.x(), t2.z()), Eigen::Vector2d(t0.x(), t0.z())); + // double scale = GENERATE(0, 1e-5, 1e-4, 1, 2, 11, 1000); + double scale = GENERATE(0, 1e-5, 1e-4, 1, 2, 11); + p.x() = closest_point.x() + scale * perp.x(); + p.z() = closest_point.z() + scale * perp.y(); + } + + PointTriangleDistanceType dtype = + point_triangle_distance_type(p, t0, t1, t2); + + Vector12d x; + x << p, t0, t1, t2; + + { + const auto [vec, grad] = + point_triangle_closest_point_direction_grad(p, t0, t1, t2, dtype); + + // Compute the gradient using finite differences + Eigen::MatrixXd fjac; + fd::finite_jacobian( + x, + [dtype](const Eigen::VectorXd& _x) { + return point_triangle_closest_point_direction( + _x.segment<3>(0), _x.segment<3>(3), _x.segment<3>(6), + _x.segment<3>(9), dtype); + }, + fjac); + + CHECK(fd::compare_jacobian(fjac, grad, 1e-5)); + } + + const auto [vec, grad, hess] = + point_triangle_closest_point_direction_hessian(p, t0, t1, t2, dtype); + + // Compute the gradient using finite differences + Eigen::MatrixXd fjac; + fd::finite_jacobian( + x, + [dtype](const Eigen::VectorXd& _x) { + return point_triangle_closest_point_direction( + _x.segment<3>(0), _x.segment<3>(3), _x.segment<3>(6), + _x.segment<3>(9), dtype); + }, + fjac); + + std::array grad_fjac; + for (int i = 0; i < 3; i++) { + fd::finite_hessian( + x, + [dtype, i](const Eigen::VectorXd& _x) { + return point_triangle_closest_point_direction( + _x.segment<3>(0), _x.segment<3>(3), _x.segment<3>(6), + _x.segment<3>(9), dtype)(i); + }, + grad_fjac[i]); + + CHECK(fd::compare_jacobian(grad_fjac[i], hess[i], 1e-5)); + } + + CAPTURE(dtype); + CHECK(fd::compare_jacobian(grad, fjac, 1e-6)); +} + +TEMPLATE_TEST_CASE_SIG( + "Point-triangle distance template", + "[distance][point-triangle][hess]", + ((int ndofs), ndofs), + 12, + 13) +{ + double py = GENERATE(-10, -1, -1e-12, 0, 1e-12, 1, 10); + Eigen::Vector3d p(0, py, 0); + Eigen::Vector3d t0(-1, 0, 1); + Eigen::Vector3d t1(1, 0, 1); + Eigen::Vector3d t2(0, 0, -1); + + SECTION("closest to triangle") + { + double pz = GENERATE(0, -1 + 1e-12, -1, 1, 1 - 1e-12); + p.z() = pz; + } + SECTION("closest to t0") + { + double px = GENERATE(-1, -1 - 1e-12, -11); + p.x() = px; + p.z() = t0.z(); + } + SECTION("closest to t1") + { + double px = GENERATE(1, 1 + 1e-12, 11); + p.x() = px; + p.z() = t1.z(); + } + SECTION("closest to t2") + { + double pz = GENERATE(-1, -1 - 1e-12, -11); + p.z() = pz; + } + SECTION("closest to t0t1") + { + double alpha = GENERATE(0.0, 1e-4, 0.5, 1.0 - 1e-4, 1.0); + Eigen::Vector3d closest_point = (t1 - t0) * alpha + t0; + Eigen::Vector2d perp = tests::edge_normal( + Eigen::Vector2d(t0.x(), t0.z()), Eigen::Vector2d(t1.x(), t1.z())); + // double scale = GENERATE(0, 1e-12, 1e-4, 1, 2, 11, 1000); + double scale = GENERATE(0, 1e-12, 1e-4, 1, 2, 11); + p.x() = closest_point.x() + scale * perp.x(); + p.z() = closest_point.z() + scale * perp.y(); + } + SECTION("closest to t1t2") + { + double alpha = GENERATE(0.0, 1e-4, 0.5, 1.0 - 1e-4, 1.0); + Eigen::Vector3d closest_point = (t2 - t1) * alpha + t1; + Eigen::Vector2d perp = tests::edge_normal( + Eigen::Vector2d(t1.x(), t1.z()), Eigen::Vector2d(t2.x(), t2.z())); + // double scale = GENERATE(0, 1e-12, 1e-4, 1, 2, 11, 1000); + double scale = GENERATE(0, 1e-12, 1e-4, 1, 2, 11); + p.x() = closest_point.x() + scale * perp.x(); + p.z() = closest_point.z() + scale * perp.y(); + } + SECTION("closest to t2t0") + { + double alpha = GENERATE(0.0, 1e-4, 0.5, 1.0 - 1e-4, 1.0); + Eigen::Vector3d closest_point = (t0 - t2) * alpha + t2; + Eigen::Vector2d perp = tests::edge_normal( + Eigen::Vector2d(t2.x(), t2.z()), Eigen::Vector2d(t0.x(), t0.z())); + // double scale = GENERATE(0, 1e-12, 1e-4, 1, 2, 11, 1000); + double scale = GENERATE(0, 1e-12, 1e-4, 1, 2, 11); + p.x() = closest_point.x() + scale * perp.x(); + p.z() = closest_point.z() + scale * perp.y(); + } + + PointTriangleDistanceType dtype = + point_triangle_distance_type(p, t0, t1, t2); + + Vector12d x; + x << t0, t1, t2, p; + + const double dist = point_triangle_distance(p, t0, t1, t2, dtype); + + auto dist1 = PrimitiveDistanceTemplate< + Face, Point3, ADGrad>::compute_distance(x, dtype); + CHECK(dist1.val == Catch::Approx(dist).margin(1e-12)); + + auto dist2 = PrimitiveDistanceTemplate< + Face, Point3, ADHessian>::compute_distance(x, dtype); + CHECK(dist2.val == Catch::Approx(dist).margin(1e-12)); + + auto direc1 = PrimitiveDistanceTemplate< + Face, Point3, ADGrad>::compute_closest_direction(x, dtype); + CHECK(direc1.squaredNorm().val == Catch::Approx(dist).margin(1e-12)); + + auto direc2 = PrimitiveDistanceTemplate< + Face, Point3, ADHessian>::compute_closest_direction(x, dtype); + CHECK(direc2.squaredNorm().val == Catch::Approx(dist).margin(1e-12)); +} \ No newline at end of file diff --git a/tests/src/tests/friction/friction_data_generator.cpp b/tests/src/tests/friction/friction_data_generator.cpp index 132378734..29d4472c2 100644 --- a/tests/src/tests/friction/friction_data_generator.cpp +++ b/tests/src/tests/friction/friction_data_generator.cpp @@ -5,6 +5,8 @@ #include #include +#include +#include Eigen::VectorXd LogSpaced(int num, double start, double stop, double base) { @@ -141,5 +143,252 @@ FrictionData friction_data_generator() collisions.vv_collisions.back().weight_gradient.resize(V0.size()); } + return data; +} + +using namespace ipc; + +SmoothFrictionData smooth_friction_data_generator_3d() +{ + SmoothFrictionData data; + + auto& [V0, V1, E, F, collisions, mu, epsv_times_h, params, barrier_stiffness] = + data; + + double& dhat = params.dhat; + + mu = 1.; // GENERATE(range(0.0, 1.0, 0.2)); +#ifdef NDEBUG + epsv_times_h = pow(10, GENERATE(range(-2, 0))); + dhat = pow(10, GENERATE(range(-2, 0))); + barrier_stiffness = 1.; +#else + epsv_times_h = 1.; // pow(10, GENERATE(range(-6, 0, 2))); + dhat = 1e-2; // pow(10, GENERATE(range(-4, 0, 2))); + barrier_stiffness = 1.; // 100; +#endif + + params = SmoothContactParameters(dhat, 0.8, 0, 1, 0, 2); + const double max_d = dhat * 0.9; + const double min_d = dhat * 0.1; + const double d = GENERATE_COPY(range(min_d, max_d, max_d / 10)); + SECTION("point-triangle") + { + V0.resize(8, 3); + V0 << 0, d, 0, // point at t=0 + -1, 0, 1, // triangle vertex 0 at t=0 + 2, 0, 0, // triangle vertex 1 at t=0 + -1, 0, -1, // triangle vertex 2 at t=0 + -1, d + 2, 1, 2, d + 2, 0, -1, d + 2, -1, 0, -2, 0; + + V1 = V0; + Eigen::Vector3d disp; + disp << 0.5, 0, 0; + V1.row(0) += disp.transpose(); + V1.row(4) += disp.transpose(); + V1.row(5) += disp.transpose(); + V1.row(6) += disp.transpose(); + + F.resize(8, 3); + F << 1, 2, 3, 1, 7, 2, 2, 7, 3, 3, 7, 1, 4, 5, 6, 4, 0, 5, 5, 0, 6, 6, + 0, 4; + igl::edges(F, E); + + CollisionMesh mesh(V0, E, F); + collisions.collisions.push_back( + std::make_shared>( + 0, 0, PointTriangleDistanceType::P_T, mesh, params, dhat, V0)); + } + SECTION("edge-edge") + { + V0.resize(10, 3); + V0.row(0) << -1, d, 0; // edge a vertex 0 at t=0 + V0.row(1) << 1, d, 0; // edge a vertex 1 at t=0 + V0.row(2) << 0, 0, -1; // edge b vertex 0 at t=0 + V0.row(3) << 0, 0, 1; // edge b vertex 1 at t=0 + V0.row(4) << 0, d, -1; + V0.row(5) << 0, d, 1; + V0.row(6) << 1, 0, 0; + V0.row(7) << -1, 0, 0; + V0.row(8) << 0, 2 * d, 0; + V0.row(9) << 0, -d, 0; + + V1 = V0; + Eigen::Vector3d disp; + disp << 0.5, 0, 0; + V1.row(0) += disp.transpose(); + V1.row(1) += disp.transpose(); + V1.row(4) += disp.transpose(); + V1.row(5) += disp.transpose(); + V1.row(8) += disp.transpose(); + + F.resize(12, 3); + F << 1, 0, 4, 1, 5, 0, 1, 4, 8, 4, 0, 8, 0, 5, 8, 5, 1, 8, 2, 7, 3, 2, + 3, 6, 2, 6, 9, 6, 3, 9, 3, 7, 9, 7, 2, 9; + + igl::edges(F, E); + + int e0 = E.rows(), e1 = E.rows(); + for (int e = 0; e < E.rows(); e++) { + if (std::min(E(e, 0), E(e, 1)) == 0 + && std::max(E(e, 0), E(e, 1)) == 1) { + e0 = e; + } else if ( + std::min(E(e, 0), E(e, 1)) == 2 + && std::max(E(e, 0), E(e, 1)) == 3) { + e1 = e; + } + } + assert(e0 < E.rows()); + assert(e1 < E.rows()); + + CollisionMesh mesh(V0, E, F); + collisions.collisions.push_back( + std::make_shared>( + e0, e1, EdgeEdgeDistanceType::EA_EB, mesh, params, dhat, V0)); + } + SECTION("point-edge") + { + V0.resize(9, 3); + V0 << 0, d, 0, // point at t=0 + 0, 0, -1, // edge vertex 0 at t=0 + 0, 0, 1, // edge vertex 1 at t=0 + -1, d * 2, 1, 2, d * 2, 0, -1, d * 2, -1, 1, 0, 0, -1, 0, 0, 0, -d, + 0; + + V1 = V0; + Eigen::Vector3d disp; + disp << 0.5, 0, 0; + V1.row(0) += disp.transpose(); + V1.row(3) += disp.transpose(); + V1.row(4) += disp.transpose(); + V1.row(5) += disp.transpose(); + + F.resize(10, 3); + F << 3, 0, 4, 4, 0, 5, 5, 0, 3, 3, 4, 5, 1, 7, 2, 1, 2, 6, 2, 7, 8, 7, + 1, 8, 1, 6, 8, 6, 2, 8; + + igl::edges(F, E); + + int e = 0; + for (; e < E.rows(); e++) { + if (std::min(E(e, 0), E(e, 1)) == 1 + && std::max(E(e, 0), E(e, 1)) == 2) { + break; + } + } + assert(e < E.rows()); + + CollisionMesh mesh(V0, E, F); + collisions.collisions.push_back( + std::make_shared>( + e, 0, PointEdgeDistanceType::AUTO, mesh, params, dhat, V0)); + } + SECTION("point-point") + { + V0.resize(8, 3); + + V0 << 0, 0, 0, 0, d, 0, -1, -d, 1, 2, -d, 0, -1, -d, -1, -1, 2 * d, 1, + 2, 2 * d, 0, -1, 2 * d, -1; + + V1 = V0; + Eigen::Vector3d disp; + disp << 0.5, 0, 0; + V1.row(0) += disp.transpose(); + V1.row(2) += disp.transpose(); + V1.row(3) += disp.transpose(); + V1.row(4) += disp.transpose(); + + F.resize(8, 3); + F << 0, 2, 3, 0, 3, 4, 0, 4, 2, 3, 2, 4, 1, 6, 5, 1, 5, 7, 1, 7, 6, 5, + 6, 7; + igl::edges(F, E); + + CollisionMesh mesh(V0, E, F); + collisions.collisions.push_back( + std::make_shared>( + 0, 1, PointPointDistanceType::AUTO, mesh, params, dhat, V0)); + } + + return data; +} + +SmoothFrictionData smooth_friction_data_generator_2d() +{ + SmoothFrictionData data; + + auto& [V0, V1, E, F, collisions, mu, epsv_times_h, params, barrier_stiffness] = + data; + + double& dhat = params.dhat; + + mu = 1.; // GENERATE(range(0.0, 1.0, 0.2)); +#ifdef NDEBUG + epsv_times_h = pow(10, GENERATE(range(-2, 0))); + dhat = pow(10, GENERATE(range(-3, 0))); + barrier_stiffness = 1.; +#else + epsv_times_h = 1.; // pow(10, GENERATE(range(-6, 0, 2))); + dhat = 1e-1; // pow(10, GENERATE(range(-4, 0, 2))); + barrier_stiffness = 1.; // 100; +#endif + + params = SmoothContactParameters(dhat, 0.8, 0, 1, 0, 2); + const double max_d = dhat * 0.9; + const double min_d = dhat * 0.1; + const double d = GENERATE_COPY(range(min_d, max_d, max_d / 10)); + SECTION("point-edge 2D") + { + V0.resize(6, 2); + V0 << d, 0, // point at t=0 + 0, -1, // edge vertex 0 at t=0 + 0, 1, // edge vertex 1 at t=0 + 1 + d, -1, 1 + d, 1, -1, 0; + + V1 = V0; + Eigen::Vector2d disp; + disp << 0, 0.5; + V1.row(0) += disp.transpose(); + V1.row(3) += disp.transpose(); + V1.row(4) += disp.transpose(); + + F.resize(2, 3); + F << 0, 3, 4, 1, 2, 5; + + E.resize(6, 2); + E << 0, 3, 3, 4, 4, 0, 1, 2, 2, 5, 5, 1; + // igl::edges(F, E); + + int e = 3; + + CollisionMesh mesh(V0, E, F); + collisions.collisions.push_back( + std::make_shared>( + e, 0, PointEdgeDistanceType::AUTO, mesh, params, dhat, V0)); + } + SECTION("point-point 2D") + { + V0.resize(6, 2); + V0 << d, 0, 0, 0, 1 + d, -1, 1 + d, 1, 0, -1, 0, 1; + + V1 = V0; + Eigen::Vector2d disp; + disp << 0, 0.5; + V1.row(0) += disp.transpose(); + V1.row(2) += disp.transpose(); + V1.row(3) += disp.transpose(); + + F.resize(2, 3); + F << 0, 2, 3, 1, 5, 4; + + E.resize(6, 2); + E << 0, 2, 2, 3, 3, 0, 1, 5, 5, 4, 4, 1; + + CollisionMesh mesh(V0, E, F); + collisions.collisions.push_back( + std::make_shared>( + 0, 1, PointPointDistanceType::AUTO, mesh, params, dhat, V0)); + } + return data; } \ No newline at end of file diff --git a/tests/src/tests/friction/friction_data_generator.hpp b/tests/src/tests/friction/friction_data_generator.hpp index 42f2e017c..fe1299932 100644 --- a/tests/src/tests/friction/friction_data_generator.hpp +++ b/tests/src/tests/friction/friction_data_generator.hpp @@ -3,6 +3,7 @@ #include #include +#include struct FrictionData { Eigen::MatrixXd V0; @@ -19,4 +20,19 @@ struct FrictionData { Eigen::VectorXd LogSpaced(int num, double start, double stop, double base = 10); Eigen::VectorXd GeomSpaced(int num, double start, double stop); -FrictionData friction_data_generator(); \ No newline at end of file +FrictionData friction_data_generator(); + +struct SmoothFrictionData { + Eigen::MatrixXd V0; + Eigen::MatrixXd V1; + Eigen::MatrixXi E; + Eigen::MatrixXi F; + ipc::SmoothCollisions collisions; + double mu; + double epsv_times_h; + ipc::SmoothContactParameters p; + double barrier_stiffness; +}; + +SmoothFrictionData smooth_friction_data_generator_2d(); +SmoothFrictionData smooth_friction_data_generator_3d(); diff --git a/tests/src/tests/friction/test_force_jacobian.cpp b/tests/src/tests/friction/test_force_jacobian.cpp index 1a8e52565..ecd1b57d5 100644 --- a/tests/src/tests/friction/test_force_jacobian.cpp +++ b/tests/src/tests/friction/test_force_jacobian.cpp @@ -6,6 +6,7 @@ #include #include +#include #include #include @@ -46,7 +47,7 @@ void check_friction_force_jacobian( tangential_collisions.build( mesh, X + Ut, collisions, BarrierPotential(dhat), barrier_stiffness, mu); - CHECK(tangential_collisions.size()); + CHECK(!tangential_collisions.empty()); const FrictionPotential D(epsv_times_h); @@ -317,7 +318,8 @@ TEST_CASE( std::vector is_on_surface = CollisionMesh::construct_is_on_surface(X.rows(), E); - CollisionMesh mesh(is_on_surface, X, E, F); + CollisionMesh mesh( + is_on_surface, std::vector(X.rows(), false), X, E, F); mesh.init_area_jacobians(); X = mesh.vertices(X); @@ -342,3 +344,297 @@ TEST_CASE( check_friction_force_jacobian( mesh, Ut, U, collisions, mu, epsv_dt, dhat, kappa, true); } + +void check_smooth_friction_force_jacobian( + const CollisionMesh& mesh, + const Eigen::MatrixXd& Ut, + const Eigen::MatrixXd& U, + const SmoothCollisions& collisions, + const double mu, + const double epsv_times_h, + const SmoothContactParameters& params, + const double barrier_stiffness, + const bool recompute_collisions) +{ + const int dim = mesh.dim(); + const double dhat = params.dhat; + const Eigen::MatrixXd& X = mesh.rest_positions(); + double distance_t0 = collisions.compute_minimum_distance(mesh, X + Ut); + double distance_t1 = collisions.compute_minimum_distance(mesh, X + U); + // CHECK((distance_t0 < dhat || distance_t1 < dhat)); + if (distance_t0 == 0 || distance_t1 == 0) { + return; + } + + const Eigen::MatrixXd velocities = U - Ut; + + CAPTURE(mu, epsv_times_h, dhat, barrier_stiffness, collisions.size()); + + TangentialCollisions friction_collisions; + friction_collisions.build( + mesh, X + Ut, collisions, params, barrier_stiffness, + Eigen::VectorXd::Ones(mesh.num_vertices()) * mu, + Eigen::VectorXd::Ones(mesh.num_vertices()) * mu); + CHECK(!friction_collisions.empty()); + + const FrictionPotential D(epsv_times_h); + + /////////////////////////////////////////////////////////////////////////// + + const Eigen::VectorXd force = + D.smooth_contact_force(friction_collisions, mesh, X, Ut, velocities); + const Eigen::VectorXd grad_D = + D.gradient(friction_collisions, mesh, velocities); + CHECK((force + grad_D).norm() <= 1e-8 * force.norm()); + + /////////////////////////////////////////////////////////////////////////// + + const Eigen::MatrixXd hess_D = + D.hessian(friction_collisions, mesh, velocities); + + auto grad = [&](const Eigen::VectorXd& v) { + return D.gradient( + friction_collisions, mesh, fd::unflatten(v, velocities.cols())); + }; + Eigen::MatrixXd fd_hessian; + fd::finite_jacobian( + fd::flatten(velocities), grad, fd_hessian, fd::AccuracyOrder::FOURTH, + 1e-6 * dhat); + // CHECK(fd::compare_jacobian(hess_D, fd_hessian)); + // if (!fd::compare_jacobian(hess_D, fd_hessian)) { + // tests::print_compare_nonzero(hess_D, fd_hessian); + // } + CHECK( + (hess_D.norm() == 0 + || (hess_D - fd_hessian).norm() <= 1e-7 * hess_D.norm())); + + /////////////////////////////////////////////////////////////////////////// + + Eigen::MatrixXd jac_force = D.smooth_contact_force_jacobian( + friction_collisions, mesh, X, Ut, velocities, params, + FrictionPotential::DiffWRT::VELOCITIES); + CHECK((hess_D + jac_force).norm() <= 1e-7 * hess_D.norm()); + + /////////////////////////////////////////////////////////////////////////// + + auto create_smooth_collision = [&](const CollisionMesh& fd_mesh, + const Eigen::MatrixXd& + fd_lagged_positions) { + SmoothCollisions fd_collisions; + assert(friction_collisions.size() == 1); + + auto cc = friction_collisions[0].smooth_collision; + std::shared_ptr fd_cc; + if (dim == 3) { + if (cc->type() == CollisionType::EDGE_EDGE) { + fd_cc = std::make_shared>( + (*cc)[0], (*cc)[1], + PrimitiveDistType::type::AUTO, fd_mesh, + params, dhat, fd_lagged_positions); + } else if (cc->type() == CollisionType::EDGE_VERTEX) { + fd_cc = + std::make_shared>( + (*cc)[0], (*cc)[1], + PrimitiveDistType::type::AUTO, fd_mesh, + params, dhat, fd_lagged_positions); + } else if (cc->type() == CollisionType::VERTEX_VERTEX) { + fd_cc = + std::make_shared>( + (*cc)[0], (*cc)[1], + PrimitiveDistType::type::AUTO, fd_mesh, + params, dhat, fd_lagged_positions); + } else if (cc->type() == CollisionType::FACE_VERTEX) { + fd_cc = std::make_shared>( + (*cc)[0], (*cc)[1], + PrimitiveDistType::type::AUTO, fd_mesh, + params, dhat, fd_lagged_positions); + } + + fd_collisions.collisions.push_back(fd_cc); + } else { + if (cc->type() == CollisionType::EDGE_VERTEX) { + fd_cc = + std::make_shared>( + (*cc)[0], (*cc)[1], + PrimitiveDistType::type::AUTO, fd_mesh, + params, dhat, fd_lagged_positions); + } else if (cc->type() == CollisionType::VERTEX_VERTEX) { + fd_cc = + std::make_shared>( + (*cc)[0], (*cc)[1], + PrimitiveDistType::type::AUTO, fd_mesh, + params, dhat, fd_lagged_positions); + } + + fd_collisions.collisions.push_back(fd_cc); + } + + return fd_collisions; + }; + + /////////////////////////////////////////////////////////////////////////// + + // test contact force norm derivative + //{ + // const Eigen::MatrixXd lagged_positions = X + Ut; + // Eigen::VectorXd normal_force_jacobian = + // Eigen::VectorXd::Zero(X.size()); + // { + // auto cc = create_smooth_collision(mesh, lagged_positions); + // SmoothContactPotential potential(params); + // Eigen::VectorXd g = potential.gradient(cc, mesh, + // lagged_positions); Eigen::SparseMatrix h = + // potential.hessian(cc, mesh, lagged_positions); + // normal_force_jacobian = (h * g) / g.norm(); + // } + + // // finite difference + // auto F_X = [&](const Eigen::VectorXd& x) { + // Eigen::MatrixXd fd_X = fd::unflatten(x, dim); + // const Eigen::MatrixXd fd_lagged_positions = fd_X + Ut; + + // CollisionMesh fd_mesh(fd_X, mesh.edges(), mesh.faces()); + // auto fd_cc = create_smooth_collision(fd_mesh, + // fd_lagged_positions); + + // SmoothContactPotential potential(params); + // return potential.gradient(fd_cc, fd_mesh, + // fd_lagged_positions).norm(); + // }; + + // Eigen::VectorXd fd_normal_force_jacobian; + // fd::finite_gradient(fd::flatten(X), F_X, fd_normal_force_jacobian); + // CHECK((normal_force_jacobian - fd_normal_force_jacobian).norm() <= + // 1e-7 * std::max(normal_force_jacobian.norm(), 1e-8)); + //} + + /////////////////////////////////////////////////////////////////////////// + + Eigen::MatrixXd JF_wrt_X = D.smooth_contact_force_jacobian( + friction_collisions, mesh, X, Ut, velocities, params, + FrictionPotential::DiffWRT::REST_POSITIONS); + + auto F_X = [&](const Eigen::VectorXd& x) { + Eigen::MatrixXd fd_X = fd::unflatten(x, X.cols()); + Eigen::MatrixXd fd_lagged_positions = fd_X + Ut; + + CollisionMesh fd_mesh(fd_X, mesh.edges(), mesh.faces()); + + auto fd_collisions = + create_smooth_collision(fd_mesh, fd_lagged_positions); + + TangentialCollisions fd_friction_collisions; + fd_friction_collisions.build( + fd_mesh, fd_lagged_positions, fd_collisions, params, + barrier_stiffness, Eigen::VectorXd::Ones(mesh.num_vertices()) * mu, + Eigen::VectorXd::Ones(mesh.num_vertices()) * mu); + + return D.smooth_contact_force( + fd_friction_collisions, fd_mesh, fd_X, Ut, velocities); + }; + Eigen::MatrixXd fd_JF_wrt_X; + fd::finite_jacobian( + fd::flatten(X), F_X, fd_JF_wrt_X, fd::AccuracyOrder::FOURTH, + 1e-6 * dhat); + // CHECK(fd::compare_jacobian(JF_wrt_X, fd_JF_wrt_X)); + // if (!fd::compare_jacobian(JF_wrt_X, fd_JF_wrt_X)) { + // tests::print_compare_nonzero(JF_wrt_X, fd_JF_wrt_X); + // } + CHECK( + (JF_wrt_X - fd_JF_wrt_X).norm() + <= 1e-7 * std::max(JF_wrt_X.norm(), 1e-8)); + + /////////////////////////////////////////////////////////////////////////// + + Eigen::MatrixXd JF_wrt_Ut = D.smooth_contact_force_jacobian( + friction_collisions, mesh, X, Ut, velocities, params, + FrictionPotential::DiffWRT::LAGGED_DISPLACEMENTS); + + auto F_Ut = [&](const Eigen::VectorXd& ut) { + Eigen::MatrixXd fd_Ut = fd::unflatten(ut, Ut.cols()); + Eigen::MatrixXd fd_lagged_positions = X + fd_Ut; + + auto fd_collisions = create_smooth_collision(mesh, fd_lagged_positions); + + TangentialCollisions fd_friction_collisions; + fd_friction_collisions.build( + mesh, fd_lagged_positions, fd_collisions, params, barrier_stiffness, + Eigen::VectorXd::Ones(mesh.num_vertices()) * mu, + Eigen::VectorXd::Ones(mesh.num_vertices()) * mu); + + return D.smooth_contact_force( + fd_friction_collisions, mesh, X, fd_Ut, velocities); + }; + Eigen::MatrixXd fd_JF_wrt_Ut; + fd::finite_jacobian( + fd::flatten(Ut), F_Ut, fd_JF_wrt_Ut, fd::AccuracyOrder::FOURTH, + 1e-6 * dhat); + // CHECK(fd::compare_jacobian(JF_wrt_Ut, fd_JF_wrt_Ut)); + // if (!fd::compare_jacobian(JF_wrt_Ut, fd_JF_wrt_Ut)) { + // tests::print_compare_nonzero(JF_wrt_Ut, fd_JF_wrt_Ut); + // } + CHECK( + (JF_wrt_Ut - fd_JF_wrt_Ut).norm() + <= 1e-7 * std::max(JF_wrt_Ut.norm(), 1e-8)); + + /////////////////////////////////////////////////////////////////////////// + + Eigen::MatrixXd JF_wrt_V = D.smooth_contact_force_jacobian( + friction_collisions, mesh, X, Ut, velocities, params, + FrictionPotential::DiffWRT::VELOCITIES); + + auto F_V = [&](const Eigen::VectorXd& v) { + return D.smooth_contact_force( + friction_collisions, mesh, X, Ut, + fd::unflatten(v, velocities.cols())); + }; + Eigen::MatrixXd fd_JF_wrt_V; + fd::finite_jacobian( + fd::flatten(velocities), F_V, fd_JF_wrt_V, fd::AccuracyOrder::FOURTH, + 1e-6 * dhat); + CHECK( + (JF_wrt_V.norm() == 0 + || (fd_JF_wrt_V - JF_wrt_V).norm() <= 1e-7 * JF_wrt_V.norm())); +} + +TEST_CASE( + "Smooth friction force jacobian 2D", "[friction-smooth][force-jacobian]") +{ + SmoothFrictionData data = smooth_friction_data_generator_2d(); + const auto& [V0, V1, E, F, collisions, mu, epsv_times_h, params, barrier_stiffness] = + data; + + Eigen::MatrixXd X, Ut, U; + X = V0; + Ut = V0 - X; + U = V1 - X; + + CollisionMesh mesh(X, E, F); + + check_smooth_friction_force_jacobian( + mesh, Ut, U, collisions, mu, epsv_times_h, params, barrier_stiffness, + false); +} + +TEST_CASE( + "Smooth friction force jacobian 3D", "[friction-smooth][force-jacobian]") +{ +#if (defined(WIN32) || defined(_WIN32) || defined(__WIN32)) && !defined(NDEBUG) + SKIP("'Smooth friction force jacobian 3D' test is skipped in debug mode"); +#endif + + SmoothFrictionData data = smooth_friction_data_generator_3d(); + const auto& [V0, V1, E, F, collisions, mu, epsv_times_h, params, barrier_stiffness] = + data; + + Eigen::MatrixXd X, Ut, U; + X = V0; + Ut = V0 - X; + U = V1 - X; + + CollisionMesh mesh(X, E, F); + + check_smooth_friction_force_jacobian( + mesh, Ut, U, collisions, mu, epsv_times_h, params, barrier_stiffness, + false); +} \ No newline at end of file diff --git a/tests/src/tests/potential/CMakeLists.txt b/tests/src/tests/potential/CMakeLists.txt index 99d291bbf..03c08d97a 100644 --- a/tests/src/tests/potential/CMakeLists.txt +++ b/tests/src/tests/potential/CMakeLists.txt @@ -2,6 +2,7 @@ set(SOURCES # Tests test_adhesion_potentials.cpp test_barrier_potential.cpp + test_smooth_potential.cpp test_friction_potential.cpp # Benchmarks diff --git a/tests/src/tests/potential/test_barrier_potential.cpp b/tests/src/tests/potential/test_barrier_potential.cpp index 24375f681..9ccd8131f 100644 --- a/tests/src/tests/potential/test_barrier_potential.cpp +++ b/tests/src/tests/potential/test_barrier_potential.cpp @@ -32,7 +32,7 @@ TEST_CASE( const bool use_physical_barrier = GENERATE(true, false); double dhat = -1; - std::string mesh_name = ""; + std::string mesh_name; bool all_vertices_on_surface = true; SECTION("cube") { diff --git a/tests/src/tests/potential/test_smooth_potential.cpp b/tests/src/tests/potential/test_smooth_potential.cpp new file mode 100644 index 000000000..29890fa98 --- /dev/null +++ b/tests/src/tests/potential/test_smooth_potential.cpp @@ -0,0 +1,401 @@ +#include +#include + +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include + +using namespace ipc; + +TEST_CASE("Smooth barrier potential codim", "[smooth_potential]") +{ + const auto method = make_default_broad_phase(); + double dhat = 2; + std::string mesh_name; + + Eigen::MatrixXd vertices(4, 2); + Eigen::MatrixXi edges(2, 2), faces; + + vertices << -1, 0, 0, 0, 1, 0, 1.5, 0.2; + edges << 0, 1, 1, 2; + + CollisionMesh mesh; + + SmoothCollisions collisions; + mesh = CollisionMesh( + std::vector(vertices.rows(), true), + std::vector(vertices.rows(), false), vertices, edges, faces); + + SmoothContactParameters params(dhat, 0.85, 0.5, 0.95, 0.6, 2); + collisions.build(mesh, vertices, params, false, method); + CAPTURE(dhat, method); + CHECK(!collisions.empty()); + CHECK(!has_intersections(mesh, vertices)); + + SmoothContactPotential potential(params); + std::cout << "energy: " << potential(collisions, mesh, vertices) << "\n"; + + // ------------------------------------------------------------------------- + // Minimum distance + // ------------------------------------------------------------------------- + + CHECK( + collisions.compute_minimum_distance(mesh, vertices) + <= collisions.compute_active_minimum_distance(mesh, vertices) + * (1. + 1e-15)); + + // ------------------------------------------------------------------------- + // Gradient + // ------------------------------------------------------------------------- + + const Eigen::VectorXd grad_b = + potential.gradient(collisions, mesh, vertices); + + // Compute the gradient using finite differences + Eigen::VectorXd fgrad_b; + { + auto f = [&](const Eigen::VectorXd& x) { + return potential( + collisions, mesh, fd::unflatten(x, vertices.cols())); + }; + fd::finite_gradient( + fd::flatten(vertices), f, fgrad_b, fd::AccuracyOrder::SECOND, 1e-8); + } + + // REQUIRE(grad_b.squaredNorm() > 1e-8); + std::cout << "grad relative error " + << (grad_b - fgrad_b).norm() / grad_b.norm() << ", norms " + << grad_b.norm() << " " << fgrad_b.norm() << "\n"; + CHECK((grad_b - fgrad_b).norm() / grad_b.norm() < 1e-5); + + // ------------------------------------------------------------------------- + // Hessian + // ------------------------------------------------------------------------- + + Eigen::MatrixXd hess_b = potential.hessian(collisions, mesh, vertices); + + // Compute the gradient using finite differences + Eigen::MatrixXd fhess_b; + { + auto f = [&](const Eigen::VectorXd& x) { + return potential.gradient( + collisions, mesh, fd::unflatten(x, vertices.cols())); + }; + fd::finite_jacobian( + fd::flatten(vertices), f, fhess_b, fd::AccuracyOrder::SECOND, 1e-8); + } + + REQUIRE(hess_b.squaredNorm() > 1e-8); + std::cout << "hess relative error " + << (hess_b - fhess_b).norm() / hess_b.norm() << ", norms " + << hess_b.norm() << " " << fhess_b.norm() << "\n"; + CHECK((hess_b - fhess_b).norm() / hess_b.norm() < 1e-5); +} + +#if defined(NDEBUG) && !defined(WIN32) +std::string tagsopt = "[smooth_potential]"; +#else +std::string tagsopt = "[.][smooth_potential]"; +#endif + +TEST_CASE("Smooth barrier potential full gradient and hessian 3D", tagsopt) +{ + const auto method = make_default_broad_phase(); + const bool adaptive_dhat = GENERATE(true, false); + const bool orientable = GENERATE(true, false); + double dhat = -1; + std::string mesh_name; + bool all_vertices_on_surface = true; + + SECTION("two cubes far") + { + dhat = 1; + mesh_name = "two-cubes-far.ply"; + all_vertices_on_surface = false; + } + SECTION("two cubes close") + { + dhat = 1e-1; + mesh_name = "two-cubes-close.ply"; + all_vertices_on_surface = false; + } + + double min_dist_ratio = 1.5; + Eigen::MatrixXd vertices; + Eigen::MatrixXi edges, faces; + bool success = tests::load_mesh(mesh_name, vertices, edges, faces); + vertices += + Eigen::MatrixXd::Random(vertices.rows(), vertices.cols()) * 1e-3; + CAPTURE(mesh_name); + REQUIRE(success); + + CollisionMesh mesh; + + SmoothCollisions collisions; + if (all_vertices_on_surface) { + mesh = CollisionMesh( + std::vector(vertices.rows(), true), + std::vector(vertices.rows(), orientable), vertices, edges, + faces); + } else { + mesh = CollisionMesh( + ipc::CollisionMesh::construct_is_on_surface(vertices.rows(), edges), + std::vector(vertices.rows(), orientable), vertices, edges, + faces); + + vertices = mesh.vertices(vertices); + } + + SmoothContactParameters params(dhat, 0.85, 0.5, 0.95, 0.6, 2); + params.set_adaptive_dhat_ratio(min_dist_ratio); + collisions.compute_adaptive_dhat(mesh, vertices, params, method); + collisions.build(mesh, vertices, params, adaptive_dhat, method); + CAPTURE(dhat, method, adaptive_dhat, all_vertices_on_surface); + CHECK(!collisions.empty()); + CHECK(!has_intersections(mesh, vertices)); + + SmoothContactPotential potential(params); + std::cout << "energy: " << potential(collisions, mesh, vertices) << "\n"; + + // ------------------------------------------------------------------------- + // Minimum distance + // ------------------------------------------------------------------------- + + CHECK( + collisions.compute_minimum_distance(mesh, vertices) + <= collisions.compute_active_minimum_distance(mesh, vertices) + * (1. + 1e-15)); + + // ------------------------------------------------------------------------- + // Gradient + // ------------------------------------------------------------------------- + + const Eigen::VectorXd grad_b = + potential.gradient(collisions, mesh, vertices); + + // Compute the gradient using finite differences + Eigen::VectorXd fgrad_b; + { + auto f = [&](const Eigen::VectorXd& x) { + return potential( + collisions, mesh, fd::unflatten(x, vertices.cols())); + }; + fd::finite_gradient( + fd::flatten(vertices), f, fgrad_b, fd::AccuracyOrder::SECOND, 1e-8); + } + + // REQUIRE(grad_b.squaredNorm() > 1e-8); + std::cout << "grad relative error " + << (grad_b - fgrad_b).norm() / grad_b.norm() << ", norms " + << grad_b.norm() << " " << fgrad_b.norm() << "\n"; + CHECK((grad_b - fgrad_b).norm() / grad_b.norm() < 1e-5); + + // ------------------------------------------------------------------------- + // Hessian + // ------------------------------------------------------------------------- + + Eigen::MatrixXd hess_b = potential.hessian(collisions, mesh, vertices); + + // Compute the gradient using finite differences + Eigen::MatrixXd fhess_b; + { + auto f = [&](const Eigen::VectorXd& x) { + return potential.gradient( + collisions, mesh, fd::unflatten(x, vertices.cols())); + }; + fd::finite_jacobian( + fd::flatten(vertices), f, fhess_b, fd::AccuracyOrder::SECOND, 1e-8); + } + + REQUIRE(hess_b.squaredNorm() > 1e-8); + std::cout << "hess relative error " + << (hess_b - fhess_b).norm() / hess_b.norm() << ", norms " + << hess_b.norm() << " " << fhess_b.norm() << "\n"; + CHECK((hess_b - fhess_b).norm() / hess_b.norm() < 1e-5); +} + +TEST_CASE("Smooth barrier potential real sim 2D C^2", "[smooth_potential]") +{ + const auto method = make_default_broad_phase(); + const bool adaptive_dhat = GENERATE(true, false); + const bool orientable = GENERATE(true, false); + + double dhat = -1; + std::string mesh_name; + SECTION("debug1") + { + mesh_name = + (tests::DATA_DIR / "gcp" / "nonlinear_solve_iter020.obj").string(); + dhat = 3e-2; + } + + double min_dist_ratio = 1.5; + Eigen::MatrixXd vertices; + Eigen::MatrixXi edges, faces; + bool success = igl::readCSV(mesh_name + "-v.csv", vertices); + success = success && igl::readCSV(mesh_name + "-e.csv", edges); + CAPTURE(mesh_name); + REQUIRE(success); + + // std::cout << "\n" << vertices << "\n" << edges << "\n"; + + CollisionMesh mesh; + SmoothContactParameters params(dhat, 0.9, -0.05, 0.95, 0.05, 1); + params.set_adaptive_dhat_ratio(min_dist_ratio); + SmoothCollisions collisions; + mesh = CollisionMesh( + std::vector(vertices.rows(), true), + std::vector(vertices.rows(), orientable), vertices, edges, faces); + collisions.compute_adaptive_dhat(mesh, vertices, params, method); + collisions.build(mesh, vertices, params, adaptive_dhat, method); + CAPTURE(dhat, method, adaptive_dhat); + CHECK(!collisions.empty()); + std::cout << "smooth collision candidate size " << collisions.size() + << "\n"; + + CHECK(!has_intersections(mesh, vertices)); + + SmoothContactPotential potential(params); + std::cout << "energy: " << potential(collisions, mesh, vertices) << "\n"; + + // ------------------------------------------------------------------------- + // Gradient + // ------------------------------------------------------------------------- + + const Eigen::VectorXd grad_b = + potential.gradient(collisions, mesh, vertices); + + // Compute the gradient using finite differences + Eigen::VectorXd fgrad_b; + { + auto f = [&](const Eigen::VectorXd& x) { + return potential( + collisions, mesh, fd::unflatten(x, vertices.cols())); + }; + fd::finite_gradient( + fd::flatten(vertices), f, fgrad_b, fd::AccuracyOrder::SECOND, 1e-8); + } + + REQUIRE(grad_b.squaredNorm() > 1e-8); + std::cout << "grad relative error " + << (grad_b - fgrad_b).norm() / grad_b.norm() << "\n"; + CHECK((grad_b - fgrad_b).norm() < 1e-7 * grad_b.norm()); + // CHECK(fd::compare_gradient(grad_b, fgrad_b)); + + // ------------------------------------------------------------------------- + // Hessian + // ------------------------------------------------------------------------- + + Eigen::MatrixXd hess_b = potential.hessian(collisions, mesh, vertices); + + // Compute the gradient using finite differences + Eigen::MatrixXd fhess_b; + { + auto f = [&](const Eigen::VectorXd& x) { + return potential.gradient( + collisions, mesh, fd::unflatten(x, vertices.cols())); + }; + fd::finite_jacobian( + fd::flatten(vertices), f, fhess_b, fd::AccuracyOrder::SECOND, 1e-8); + } + + REQUIRE(hess_b.squaredNorm() > 1e-3); + std::cout << "hess relative error " + << (hess_b - fhess_b).norm() / hess_b.norm() << "\n"; + CHECK((hess_b - fhess_b).norm() < 1e-7 * hess_b.norm()); + // CHECK(fd::compare_hessian(hess_b, fhess_b, 1e-3)); +} + +TEST_CASE("Smooth barrier potential real sim 2D C^1", "[smooth_potential]") +{ + const auto method = make_default_broad_phase(); + const bool adaptive_dhat = GENERATE(true, false); + + double dhat = -1; + std::string mesh_name; + SECTION("debug2") + { + mesh_name = (tests::DATA_DIR / "gcp" / "simple_2d.obj").string(); + dhat = 0.1; + } + + double min_dist_ratio = 1.5; + Eigen::MatrixXd vertices; + Eigen::MatrixXi edges, faces; + bool success = igl::readCSV(mesh_name + "-v.csv", vertices); + success = success && igl::readCSV(mesh_name + "-e.csv", edges); + CAPTURE(mesh_name); + REQUIRE(success); + + // std::cout << "\n" << vertices << "\n" << edges << "\n"; + + CollisionMesh mesh; + SmoothContactParameters params(dhat, 0.9, -0.05, 0.95, 0.05, 1); + params.set_adaptive_dhat_ratio(min_dist_ratio); + SmoothCollisions collisions; + mesh = CollisionMesh(vertices, edges, faces); + collisions.compute_adaptive_dhat(mesh, vertices, params, method); + collisions.build(mesh, vertices, params, adaptive_dhat, method); + CAPTURE(dhat, method, adaptive_dhat); + CHECK(!collisions.empty()); + std::cout << "smooth collision candidate size " << collisions.size() + << "\n"; + std::cout << collisions.to_string(mesh, vertices, params) << "\n"; + + CHECK(!has_intersections(mesh, vertices)); + + SmoothContactPotential potential(params); + std::cout << "energy: " << potential(collisions, mesh, vertices) << "\n"; + + // ------------------------------------------------------------------------- + // Gradient + // ------------------------------------------------------------------------- + + const Eigen::VectorXd grad_b = + potential.gradient(collisions, mesh, vertices); + + // Compute the gradient using finite differences + Eigen::VectorXd fgrad_b; + { + auto f = [&](const Eigen::VectorXd& x) { + return potential( + collisions, mesh, fd::unflatten(x, vertices.cols())); + }; + fd::finite_gradient( + fd::flatten(vertices), f, fgrad_b, fd::AccuracyOrder::SECOND, 1e-8); + } + + REQUIRE(grad_b.squaredNorm() > 1e-8); + std::cout << "grad relative error " + << (grad_b - fgrad_b).norm() / grad_b.norm() << "\n"; + CHECK((grad_b - fgrad_b).norm() < 1e-7 * grad_b.norm()); + // CHECK(fd::compare_gradient(grad_b, fgrad_b)); +} + +TEST_CASE("Benchmark autogen code", "[!benchmark]") +{ + Eigen::Vector3d ea0, ea1, eb0, eb1; + ea0 << -0.9, 0, 0; + ea1 << 1.05, 0, 0; + eb0 << 0, -1.1, 1.02; + eb1 << 0, 0.99, 1.01; + + BENCHMARK("autogen") + { + ipc::line_line_distance_hessian(ea0, ea1, eb0, eb1); + }; + + BENCHMARK("manual") + { + ipc::line_line_closest_point_direction_hessian(ea0, ea1, eb0, eb1); + }; +} diff --git a/tests/src/tests/test_collision_mesh.cpp b/tests/src/tests/test_collision_mesh.cpp index 870f3994f..fa60d6328 100644 --- a/tests/src/tests/test_collision_mesh.cpp +++ b/tests/src/tests/test_collision_mesh.cpp @@ -1,5 +1,5 @@ #include - +#include #include using namespace ipc; @@ -23,6 +23,9 @@ TEST_CASE("Collision mesh", "[collision_mesh]") CollisionMesh mesh(V, E, /*F=*/Eigen::MatrixXi(), W); + CHECK(mesh.max_edge_length() == Catch::Approx(1.).margin(1e-15)); + CHECK(mesh.edge_length(0) == Catch::Approx(1.).margin(1e-15)); + Eigen::MatrixXd U(3, 2); U << 0, 0, 1, 1, 0, 0; diff --git a/tests/src/tests/utils/CMakeLists.txt b/tests/src/tests/utils/CMakeLists.txt index 0d0352934..b15998743 100644 --- a/tests/src/tests/utils/CMakeLists.txt +++ b/tests/src/tests/utils/CMakeLists.txt @@ -2,6 +2,7 @@ set(SOURCES # Tests test_interval.cpp test_utils.cpp + test_matrixcache.cpp # Benchmarks diff --git a/tests/src/tests/utils/test_matrixcache.cpp b/tests/src/tests/utils/test_matrixcache.cpp new file mode 100644 index 000000000..72eddc169 --- /dev/null +++ b/tests/src/tests/utils/test_matrixcache.cpp @@ -0,0 +1,41 @@ +#include +#include +#include +#include + +using namespace ipc; + +TEST_CASE("SparseMatrixCache", "[utils][matrix_cache]") +{ + const size_t N = 5; + + { + auto cache = std::make_shared(N); + CHECK(cache); + + cache = std::make_shared(N, 2 * N); + CHECK(cache); + + cache->init(SparseMatrixCache(N, N), true); + CHECK(cache); + + cache->set_zero(); + CHECK( + cache->get_matrix().squaredNorm() + == Catch::Approx(0.).margin(1e-15)); + + SparseMatrixCache cache2(*cache); + } + + { + SparseMatrixCache cache(N, N); + + for (size_t i = 0; i < N - 1; i++) { + cache.add_value(0, i, i + 1, 1.); + } + + CHECK( + cache.get_matrix().squaredNorm() + == Catch::Approx(N - 1.).margin(1e-15)); + } +}