diff --git a/.clang-format b/.clang-format index 357149634..0a6eb9f48 100644 --- a/.clang-format +++ b/.clang-format @@ -9,7 +9,7 @@ BreakArrays: false BreakBeforeBinaryOperators: NonAssignment BreakStringLiterals: false ColumnLimit: 80 -CommentPragmas: '^@.+' +CommentPragmas: '^(@|NOTE|TODO|WARNING|FIXME).+' FixNamespaceComments: true # Regroup includes with a priority system IncludeBlocks: Regroup diff --git a/.clang-tidy b/.clang-tidy index 8a34a02d4..5ca9eafd3 100644 --- a/.clang-tidy +++ b/.clang-tidy @@ -45,6 +45,9 @@ CheckOptions: readability-identifier-naming.FunctionCase: lower_case readability-identifier-naming.FunctionIgnoredRegexp: '^(AbslHashValue|.*[123][Ddf].*)$' readability-identifier-naming.GlobalConstantCase: UPPER_CASE + # Variable templates follow the standard library's `_v` suffix convention + # (e.g. std::is_same_v), not the UPPER_CASE global-constant convention. + readability-identifier-naming.GlobalConstantIgnoredRegexp: '^(.*_v)$' readability-identifier-naming.MemberCase: lower_case readability-identifier-naming.MemberIgnoredRegexp: '^([A-Z])$' readability-identifier-naming.MethodCase: lower_case diff --git a/.github/workflows/clang-format-check.yml b/.github/workflows/clang-format-check.yml index fb8995342..f3a5dd499 100644 --- a/.github/workflows/clang-format-check.yml +++ b/.github/workflows/clang-format-check.yml @@ -29,5 +29,5 @@ jobs: - name: clang-format style check uses: jidicula/clang-format-action@v4.17.0 with: - clang-format-version: '20' + clang-format-version: '21' check-path: ${{ matrix.path }} \ No newline at end of file diff --git a/docs/source/_static/css/custom.css b/docs/source/_static/css/custom.css index c2937a2d0..79ca3f6b4 100644 --- a/docs/source/_static/css/custom.css +++ b/docs/source/_static/css/custom.css @@ -76,6 +76,14 @@ table.autosummary { font-size: 0.75rem !important; } +/* Compress the padding inside table cells for the custom class */ +.tight-table th, +.tight-table td { + padding-top: 2px !important; + padding-bottom: 2px !important; + line-height: 1.2 !important; +} + /* ── Gallery Grid ── */ .gallery-grid { display: grid; diff --git a/docs/source/about/release_notes.rst b/docs/source/about/release_notes.rst index 67ef88601..062018a69 100644 --- a/docs/source/about/release_notes.rst +++ b/docs/source/about/release_notes.rst @@ -6,6 +6,212 @@ Release Notes .. role:: cmake(code) :language: cmake +v2.0.0 (alpha) +-------------- + +.. warning:: + This is an in-development alpha release. The API is not yet stable. + +Highlights +~~~~~~~~~~ + +- Contact Hessian assembly is now block-accelerated. Local per-collision Hessians scatter straight into MeshFEMSparse :cite:p:`Mohammadian2026MeshFEM` block-CSC storage, and the full-mesh DOF map is folded into that scatter instead of being applied afterwards. Callers do not have to change anything to benefit: on Puffer-Ball (512k collisions) assembling a Hessian goes from 918 ms to 46 ms (`#246 `_). +- Templated distance, barrier, normal, tangent-basis, closest-point, relative-velocity, and area functions on the scalar type and dimension. This adds ``float`` support alongside ``double`` (and autodiff scalars where already supported). This is 1.5–4.8× faster for select functons; see Performance below (`#249 `_). +- Update Tight Inclusion to ``1.1.0``, which adds a bucket depth-first-search root finder and makes it the default (`#248 `_). +- Update the tutorials to match the current API, and fill the gaps in the Python bindings they depend on (`#247 `_). + +New Features |:rocket:| +~~~~~~~~~~~~~~~~~~~~~~~ + +- Expose the intersection coordinates of an edge–triangle intersection through a new :cpp:func:`ipc::edge_triangle_intersection` overload, which reports the barycentric coordinates :math:`(u, v)` on the triangle and the parameter :math:`t` along the edge (`#245 `_). +- Add :cpp:func:`ipc::CollisionMesh::face_normals`, computing the unit normal of each face for a given set of vertex positions (3D only) (`#245 `_). + +API Changes |:wrench:| +~~~~~~~~~~~~~~~~~~~~~~ + +- Update Tight Inclusion from ``1.0.6`` to ``1.1.0`` (`#248 `_). + + - Adds a ``BUCKET_DEPTH_FIRST_SEARCH`` root-finding method, which upstream makes the default for ``edgeEdgeCCD`` and ``vertexFaceCCD``. + - The method is exposed in the Python ``CCDRootFindingMethod`` enum, and ``ipctk.tight_inclusion.edge_edge_ccd`` and ``point_triangle_ccd`` now default to it so the bindings match the C++ default. + +- ``Potential::gradient`` and ``Potential::hessian`` take a new ``in_full_dof`` flag, folding the full-mesh DOF map into assembly (`#246 `_). Non-selection DOF maps fall back internally, so the flag is always safe to pass. +- Add a ``HessianAssembler`` interface separating local derivative evaluation from global matrix construction (`#246 `_). + + - The previous triplet path lives on unchanged as ``TripletHessianAssembler`` and remains the fallback when the option is off. + - Hold a ``MeshFEMHessianAssembler`` across ``Potential::hessian`` calls to get sparsity-pattern reuse, or call ``block_matrix()`` for the native block-CSC matrix if your solver can consume it. + +- Move gradient assembly into a shared ``ipc::assemble_gradient`` (``ipc/utils/gradient_assembler.hpp``) that all five gradient-producing potentials route through (`#246 `_). + +- Templatize the distance functions on the scalar type. + + - The free functions now take a scalar template parameter ``T`` and are instantiated for both ``float`` and ``double``. + - The distance functions are now a two-layer API. + + - The concrete kernels moved into ``ipc::detail`` and are templated on the scalar and the dimension (``template ``, or just ```` for the 3D-only). + - The public names in ``ipc`` are thin front ends templated on the argument expression types. They deduce ``T``, dispatch on the compile-time dimension when the arguments know it, and fall back to a single runtime branch on ``size()`` otherwise. Existing calls are unaffected. + + - Autodiff scalars are supported for the value functions only; passing one to a ``*_gradient``/``*_hessian`` or to a ``*_distance_type`` predicate is a compile-time or ``AUTO``-dispatch error rather than silently wrong output. + - 💥 **[Breaking]** Mixed-precision calls no longer deduce: ``barrier(float_d, 0.001)`` must become ``barrier(float_d, 0.001f)``. + - 💥 **[Breaking]** The gradients/Hessians of the point-point, point-line, and point-edge distances and the ``normalization_*`` functions now return **fixed-size** Eigen types (e.g. ``Eigen::Vector``) when the argument type knows its dimension at compile time, and the previous ``VectorMax``/``MatrixMax`` types otherwise. + +- Templatize the barrier classes on the scalar type. + + - ``ipc::Barrier`` is now an alias for the class template ``ipc::BarrierBase`` (defaulting to ``double``). + - 💥 **[Breaking]** The concrete barriers are now class templates and must be spelled with explicit template arguments (e.g., ``ipc::ClampedLogBarrier<>``). + - The free functions ``ipc::barrier``, ``ipc::barrier_first_derivative``, and ``ipc::barrier_second_derivative`` are now templates defaulting to ``double``. + +- Templatize the tangent functions + + - Follow the distance family: fixed-size kernels in ``ipc::detail`` templated on ```` (or ```` where the function is 3D-only), behind front ends that deduce both from the argument expressions. + - Return types are fixed-size when the argument type knows its dimension and the previous ``VectorMax``/``MatrixMax`` types otherwise. + +Performance |:zap:| +~~~~~~~~~~~~~~~~~~~ + +- Assemble the contact Hessian into MeshFEMSparse :cite:p:`Mohammadian2026MeshFEM` block-CSC storage rather than triplets, and fold the mesh's DOF map into that scatter (`#246 `_). + + Evaluating the local per-collision Hessians was only 2–8% of ``Potential::hessian`` across the eight benchmark scenes; the rest was building triplets, sorting them inside ``setFromTriplets``, and multiplying by the selection matrix twice in ``to_full_dof``. + + .. figure:: https://github.com/user-attachments/assets/fdfb0d8f-e3ad-459f-88ae-bd2fea9b64a8 + :align: center + + Hessian assembly by stage. ``to_full_dof`` is absent from every MeshFEM bar because it is folded into the scatter, and local evaluation grows from a sliver to roughly half the bar — on the largest scenes the arithmetic is now the majority of the cost. Benchmarked on Apple Silicon, AppleClang 21, Release; median of three runs. + +- The speed-up comes from four independent steps, each measured against the triplet baseline in the same run (`#246 `_). + + .. figure:: https://github.com/user-attachments/assets/32709ba8-4365-4826-a8e8-17d4abf7d71d + :align: center + + Ablation of the four steps, applied successively. Benchmarked on Apple Silicon, AppleClang 21, Release; median of three runs. + + 1. Fold ``to_full_dof`` into assembly (1.2–1.8×). When the DOF map is a plain selection matrix — which holds unless a custom displacement map was supplied — stencil vertex IDs are remapped during assembly and the two sparse products disappear. Non-selection maps fall back internally, so passing the new ``in_full_dof`` flag is always safe. + 2. Assemble into block-CSC rather than triplets (2.6–15.3×). A block sparsity pattern is built from the collision stencils, then local Hessians scatter into the value array through a sorted column-merge with per-column locks — no triplet construction and no ``setFromTriplets`` sort. + 3. Reuse the pattern across assemblies (3.2–24.8×). One assembler held across a Newton solve detects contact-set changes and rebuilds only when it must. + 4. Skip change detection when the caller asserts the set is unchanged (3.8–30.9×). + + Most of the win lands before any reuse. Reuse pays where pattern construction dominates and adds almost nothing on Rod-Twist, where detection costs about what a rebuild does — which is what the opt-in fast path in step 4 exists for. + +- Share one gradient-assembly routine across all vector-assembly paths, picking its strategy per call from the problem shape (`#246 `_). + + Sparse contact buffers the local gradients in parallel and scatters them serially, costing one add per stencil slot; dense contact keeps per-thread accumulators and reduces them in parallel over DOF blocks. The crossover sits at roughly ``out_ndof`` > 4 × collisions. + + .. figure:: https://github.com/user-attachments/assets/aceb3cb7-367f-4266-8493-7b6a4685ed01 + :align: center + + Gradient assembly, before and after, with the strategy selected per scene. Benchmarked on Apple Silicon, AppleClang 21, Release; median of three runs. + +- Replace the three 2×2 LDLT solves in ``ipc::point_triangle_distance_type`` with a closed form (`#249 `_). + + - Each edge lies in the triangle's plane, so the 2×2 Gram matrix is diagonal and the solve collapses to two guarded divisions. + - Measured 10–13× faster standalone (104 → 8.6 ns) and 7.3× on the full AUTO distance query (93 → 12.8 ns). + - An error study over 10.8 million configurations (uniform, needle, cap, near-collinear, degenerate, in-plane, and coordinate scales from 1e-50 to 1e+50) found zero classification changes outside triangles collinear to within 1e-11 of their own edge length. + +- Dim-templated fixed-size kernels behind expression-templated front ends for the point-point, point-line, and point-edge distance functions and their gradients/Hessians, and for the ``normalization_*`` family (`#249 `_). + + - ``point_line_distance`` 8.2 → 2.3 ns (3.5×), ``point_edge_distance`` 18.1 → 5.2 ns (3.5×), ``point_point_distance_hessian`` up to 5×, ``normalization_and_jacobian`` 3.4×. + - Mark small functions inline in the the header. + +- Recovering the compile-time dimension in the tangent, closest-point, relative-velocity, and area kernels is worth 1.5–4.8× on the call site that dominates in practice (`#249 `_): + + .. list-table:: + :widths: 40 15 15 15 + :header-rows: 1 + :align: center + :class: tight-table + + * - Benchmark + - Before + - After + - Speedup + * - point_edge_closest_point + - 8.7 ns + - 1.8 ns + - 4.8x + * - point_edge_closest_point, Vector3d + - 4.7 ns + - 1.7 ns + - 2.8x + * - point_edge_closest_point_jacobian + - 14.9 ns + - 6.2 ns + - 2.4x + * - point_point_relative_velocity + - 5.5 ns + - 1.4 ns + - 3.9x + * - point_point_relative_velocity_jacobian + - 6.8 ns + - 1.8 ns + - 3.9x + * - edge_length + - 4.1 ns + - 1.3 ns + - 3.1x + * - point_edge_tangent_basis + - 7.1 ns + - 4.2 ns + - 1.7x + * - point_triangle_tangent_basis + - 7.7 ns + - 5.0 ns + - 1.5x + +- Branch once on the dimension in ``EdgeVertexCandidate::compute_distance_gradient``/``_hessian`` so the statically sized kernels are selected (3.1× on the gradient). +- Move all cold ``throw`` bodies in the distance dispatch functions out of line behind ``[[noreturn]]`` helpers; constructing the exception inline consumed the caller's inlining budget (the single largest lever found: up to 2× by itself). + +Bug Fixes |:bug:| +~~~~~~~~~~~~~~~~~ + +- PSD-project the mollified Hessian block when the mollifier is zero (`#244 `_). + + - ``NormalPotential::hessian()`` early-returned the block :math:`(\text{weight} \cdot f)\nabla^2 m` for exactly parallel edges (:math:`m = 0`) *without* projection, while every other path projects. + - Positive weights leave the block PSD, so this was harmless until ``IMPROVED_MAX_APPROX`` introduced negative-weight collisions, which made the block negative-(semi)definite and the assembled "PSD-projected" Hessian non-PSD. + - Because :math:`m = 0` is a global minimum of the mollifier, :math:`\nabla^2 m` is PSD and :math:`f(d) > 0`, so projecting the scalar weight is sufficient — no eigendecomposition needed. + +Python |:snake:| +~~~~~~~~~~~~~~~~ + +- 💥 **[Breaking]** Rename the ``SmoothPotential`` class to ``SmoothContactPotential`` to match the C++ name (`#247 `_). +- Fill gaps that made the GCP and convergent-formulation tutorials impossible to follow from Python (`#247 `_): + + - Add ``SmoothCollisions.compute_adaptive_dhat``. Without it, adaptive dhat was unreachable even though ``build()`` accepts ``use_adaptive_dhat=True`` and requires this to be called first. + - Add the ``SmoothContactParameters.adaptive_dhat_ratio`` property. + - Add the ``BarrierPotential.stiffness`` and ``.use_physical_barrier`` properties, mirroring the C++ setters. + +- Validate preconditions in the bindings instead of relying on the C++ ``assert``\ s, which are compiled out under ``NDEBUG`` and would let a release build silently accept a bad value (`#247 `_). ``BarrierPotential`` now raises ``ValueError`` for a non-positive or NaN ``dhat``/``stiffness`` and for a null barrier. +- Bind ``edge_triangle_intersection()``, returning an ``(intersects, u, v, t)`` tuple since Python has no out-parameters, and ``CollisionMesh.face_normals()``, returning an (#F × 3) array to match the other per-element accessors (`#245 `_). ``face_normals()`` raises ``ValueError`` on a 2D mesh rather than invoking undefined behavior. + +Documentation +~~~~~~~~~~~~~ + +- Update the tutorials to match the current API (`#247 `_). Every snippet is now verified: the C++ is extracted into a compile harness checked against the real headers, and the Python is run against a built ``ipctk``. + + - ``ipc::point_triangle_ccd`` and the other free narrow-phase functions are now methods on :cpp:class:`ipc::NarrowPhaseCCD` subclasses; ```` no longer exists. + - The four ``*_nonlinear_ccd`` free functions are now :cpp:class:`ipc::NonlinearCCD` methods. + - ``CollisionStencil::ccd`` takes stencil vertices rather than ``(vertices, edges, faces)``; use ``dof()`` to gather them. + - ``TangentialCollisions::build`` no longer takes ``barrier_stiffness`` — stiffness now comes from the normal potential. The stale call still compiled in C++, silently binding ``barrier_stiffness`` to ``mu_s`` and ``mu`` to ``mu_k``. + +- Correct the note on conservative CCD (`#247 `_). :cpp:class:`ipc::TightInclusionCCD` does not scale the returned time of impact in the normal path; it inflates the minimum separation the query stops at, capped at ``1e-4``, and only scales the TOI in the fallback taken when that query returns a TOI below ``SMALL_TOI``. Because the cap usually binds, changing ``conservative_rescaling`` often has no effect at all. +- Describe the narrow-phase alternatives accurately (`#247 `_). + + - ``InexactCCD`` is behind ``IPC_TOOLKIT_WITH_INEXACT_CCD``, which is off by default, so it is now marked opt-in. + - All three methods compute their margin as :math:`d_\min + (1 - r)(d_0 - d_\min)`; only :cpp:class:`ipc::TightInclusionCCD` caps the second term, which is why it reports a time of impact closer to the exact one. + - :cpp:class:`ipc::AdditiveCCD` is over 100× faster and reliable in practice; it does not account for rounding error in its distance computations, but the default 10% margin is large enough to avoid false negatives, at the cost of a less accurate time of impact and more false positives. + +- Add MeshFEM :cite:p:`Mohammadian2026MeshFEM` to the gallery. + +Refactor +~~~~~~~~ + +- Replace the duplicate squared-distance implementations in ``ipc/smooth_contact/distance/`` (``point_point_sqr_distance``, ``point_line_sqr_distance``, ``line_line_sqr_distance``, ``edge_edge_sqr_distance``, ``point_plane_sqr_distance``, ``point_triangle_sqr_distance``) with the now-templated functions from ``ipc/distance/``. + +Miscellaneous +~~~~~~~~~~~~~ + +- Add ``MeshFEMCore`` and ``MeshFEMSparse`` :cite:p:`Mohammadian2026MeshFEM` as dependencies, fetched with CPM (`#246 `_). Both are MIT licensed and build as small static targets — matrix data structures and assembly routines, without sparse direct solvers. +- Take the ``distance_squared`` functor of the private ``AdditiveCCD::additive_ccd`` helper as a template parameter rather than a ``std::function`` (`#247 `_). +- Fix stale ``IPC_TOOLKIT_CCD_BENCHMARK_DIR`` and ``IPC_TOOLKIT_CCD_NEW_BENCHMARK_DIR`` references in the CMake status messages; the cache variables are ``IPC_TOOLKIT_TESTS_CCD_BENCHMARK_DIR`` and ``IPC_TOOLKIT_TESTS_NEW_CCD_BENCHMARK_DIR`` (`#248 `_). + v1.6.0 (July 14, 2026) ---------------------- diff --git a/docs/source/cpp-api/barrier/Barrier.rst b/docs/source/cpp-api/barrier/Barrier.rst index 599ab5db3..bbd9cb261 100644 --- a/docs/source/cpp-api/barrier/Barrier.rst +++ b/docs/source/cpp-api/barrier/Barrier.rst @@ -1,5 +1,5 @@ Barrier ======= -.. doxygenclass:: ipc::Barrier +.. doxygenclass:: ipc::BarrierBase :allow-dot-graphs: \ No newline at end of file diff --git a/docs/source/cpp-api/barrier/index.rst b/docs/source/cpp-api/barrier/index.rst index 4bd29c798..78ccbcd5b 100644 --- a/docs/source/cpp-api/barrier/index.rst +++ b/docs/source/cpp-api/barrier/index.rst @@ -64,9 +64,9 @@ Functions Function Details ---------------- -.. doxygenfunction:: barrier(const double, const double) -.. doxygenfunction:: barrier_first_derivative -.. doxygenfunction:: barrier_second_derivative +.. doxygenfunction:: barrier(const T d, const T dhat) +.. doxygenfunction:: barrier_first_derivative(const T d, const T dhat) +.. doxygenfunction:: barrier_second_derivative(const T d, const T dhat) Barrier Force Magnitude ~~~~~~~~~~~~~~~~~~~~~~~ diff --git a/docs/source/cpp-api/distance.rst b/docs/source/cpp-api/distance.rst index 381a00dac..bbb08a0e1 100644 --- a/docs/source/cpp-api/distance.rst +++ b/docs/source/cpp-api/distance.rst @@ -8,72 +8,72 @@ Distance Type .. doxygenenum:: EdgeEdgeDistanceType .. doxygenenum:: PointTriangleDistanceType -.. doxygenfunction:: point_edge_distance_type -.. doxygenfunction:: edge_edge_distance_type -.. doxygenfunction:: point_triangle_distance_type +.. doxygenfunction:: ipc::point_edge_distance_type +.. doxygenfunction:: ipc::edge_edge_distance_type +.. doxygenfunction:: ipc::point_triangle_distance_type Edge-Edge Mollifier ------------------- -.. doxygenfunction:: edge_edge_mollifier_threshold -.. doxygenfunction:: edge_edge_cross_squarednorm +.. doxygenfunction:: ipc::edge_edge_mollifier_threshold +.. doxygenfunction:: ipc::edge_edge_cross_squarednorm .. doxygenfunction:: ipc::edge_edge_cross_squarednorm_gradient .. doxygenfunction:: ipc::edge_edge_cross_squarednorm_hessian -.. doxygenfunction:: edge_edge_mollifier(Eigen::ConstRef ea0, Eigen::ConstRef ea1, Eigen::ConstRef eb0, Eigen::ConstRef eb1, const double eps_x) -.. doxygenfunction:: edge_edge_mollifier(const double x, const double eps_x) -.. doxygenfunction:: edge_edge_mollifier_gradient(Eigen::ConstRef ea0, Eigen::ConstRef ea1, Eigen::ConstRef eb0, Eigen::ConstRef eb1, const double eps_x) -.. doxygenfunction:: edge_edge_mollifier_gradient(const double x, const double eps_x) -.. doxygenfunction:: edge_edge_mollifier_hessian(Eigen::ConstRef ea0, Eigen::ConstRef ea1, Eigen::ConstRef eb0, Eigen::ConstRef eb1, const double eps_x) -.. doxygenfunction:: edge_edge_mollifier_hessian(const double x, const double eps_x) +.. doxygenfunction:: ipc::edge_edge_mollifier(const Eigen::MatrixBase& ea0, const Eigen::MatrixBase& ea1, const Eigen::MatrixBase& eb0, const Eigen::MatrixBase& eb1, const typename DerivedEA0::Scalar eps_x) +.. doxygenfunction:: ipc::edge_edge_mollifier(const T x, const T eps_x) +.. doxygenfunction:: ipc::edge_edge_mollifier_gradient(const Eigen::MatrixBase& ea0, const Eigen::MatrixBase& ea1, const Eigen::MatrixBase& eb0, const Eigen::MatrixBase& eb1, const typename DerivedEA0::Scalar eps_x) +.. doxygenfunction:: ipc::edge_edge_mollifier_gradient(const T x, const T eps_x) +.. doxygenfunction:: ipc::edge_edge_mollifier_hessian(const Eigen::MatrixBase& ea0, const Eigen::MatrixBase& ea1, const Eigen::MatrixBase& eb0, const Eigen::MatrixBase& eb1, const typename DerivedEA0::Scalar eps_x) +.. doxygenfunction:: ipc::edge_edge_mollifier_hessian(const T x, const T eps_x) Edge-Edge --------- -.. doxygenfunction:: edge_edge_distance -.. doxygenfunction:: edge_edge_distance_gradient -.. doxygenfunction:: edge_edge_distance_hessian +.. doxygenfunction:: ipc::edge_edge_distance +.. doxygenfunction:: ipc::edge_edge_distance_gradient +.. doxygenfunction:: ipc::edge_edge_distance_hessian Line-Line --------- -.. doxygenfunction:: line_line_distance +.. doxygenfunction:: ipc::line_line_distance .. doxygenfunction:: ipc::line_line_distance_gradient .. doxygenfunction:: ipc::line_line_distance_hessian Point-Edge ---------- -.. doxygenfunction:: point_edge_distance -.. doxygenfunction:: point_edge_distance_gradient -.. doxygenfunction:: point_edge_distance_hessian +.. doxygenfunction:: ipc::point_edge_distance +.. doxygenfunction:: ipc::point_edge_distance_gradient +.. doxygenfunction:: ipc::point_edge_distance_hessian Point-Line ---------- -.. doxygenfunction:: point_line_distance -.. doxygenfunction:: point_line_distance_gradient -.. doxygenfunction:: point_line_distance_hessian +.. doxygenfunction:: ipc::point_line_distance +.. doxygenfunction:: ipc::point_line_distance_gradient +.. doxygenfunction:: ipc::point_line_distance_hessian Point-Plane ----------- -.. doxygenfunction:: point_plane_distance(Eigen::ConstRef p, Eigen::ConstRef origin, Eigen::ConstRef normal) -.. doxygenfunction:: point_plane_distance(Eigen::ConstRef p, Eigen::ConstRef t0, Eigen::ConstRef t1, Eigen::ConstRef t2) -.. doxygenfunction:: point_plane_distance_gradient(Eigen::ConstRef p, Eigen::ConstRef origin, Eigen::ConstRef normal) -.. doxygenfunction:: point_plane_distance_gradient(Eigen::ConstRef p, Eigen::ConstRef t0, Eigen::ConstRef t1, Eigen::ConstRef t2) -.. doxygenfunction:: point_plane_distance_hessian(Eigen::ConstRef p, Eigen::ConstRef origin, Eigen::ConstRef normal) -.. doxygenfunction:: point_plane_distance_hessian(Eigen::ConstRef p, Eigen::ConstRef t0, Eigen::ConstRef t1, Eigen::ConstRef t2) +.. doxygenfunction:: point_plane_distance(const Eigen::MatrixBase& p, const Eigen::MatrixBase& origin, const Eigen::MatrixBase& normal) +.. doxygenfunction:: point_plane_distance(const Eigen::MatrixBase& p, const Eigen::MatrixBase& t0, const Eigen::MatrixBase& t1, const Eigen::MatrixBase& t2) +.. doxygenfunction:: point_plane_distance_gradient(const Eigen::MatrixBase& p, const Eigen::MatrixBase& origin, const Eigen::MatrixBase& normal) +.. doxygenfunction:: point_plane_distance_gradient(const Eigen::MatrixBase& p, const Eigen::MatrixBase& t0, const Eigen::MatrixBase& t1, const Eigen::MatrixBase& t2) +.. doxygenfunction:: point_plane_distance_hessian(const Eigen::MatrixBase& p, const Eigen::MatrixBase& origin, const Eigen::MatrixBase& normal) +.. doxygenfunction:: point_plane_distance_hessian(const Eigen::MatrixBase& p, const Eigen::MatrixBase& t0, const Eigen::MatrixBase& t1, const Eigen::MatrixBase& t2) Point-Point ----------- -.. doxygenfunction:: point_point_distance -.. doxygenfunction:: point_point_distance_gradient -.. doxygenfunction:: point_point_distance_hessian +.. doxygenfunction:: ipc::point_point_distance +.. doxygenfunction:: ipc::point_point_distance_gradient +.. doxygenfunction:: ipc::point_point_distance_hessian Point-Triangle -------------- -.. doxygenfunction:: point_triangle_distance -.. doxygenfunction:: point_triangle_distance_gradient -.. doxygenfunction:: point_triangle_distance_hessian \ No newline at end of file +.. doxygenfunction:: ipc::point_triangle_distance +.. doxygenfunction:: ipc::point_triangle_distance_gradient +.. doxygenfunction:: ipc::point_triangle_distance_hessian \ No newline at end of file diff --git a/python/src/barrier/barrier.cpp b/python/src/barrier/barrier.cpp index b0744430d..d6abf56da 100644 --- a/python/src/barrier/barrier.cpp +++ b/python/src/barrier/barrier.cpp @@ -67,7 +67,8 @@ void define_barrier(py::module_& m) The units of the barrier function. )ipc_Qu8mg5v7"); - py::class_>( + py::class_< + ClampedLogBarrier<>, Barrier, std::shared_ptr>>( m, "ClampedLogBarrier", R"ipc_Qu8mg5v7( Smoothly clamped log barrier functions from [Li et al. 2020]. @@ -80,8 +81,8 @@ void define_barrier(py::module_& m) .def(py::init()); py::class_< - NormalizedClampedLogBarrier, ClampedLogBarrier, - std::shared_ptr>( + NormalizedClampedLogBarrier<>, ClampedLogBarrier<>, + std::shared_ptr>>( m, "NormalizedClampedLogBarrier", R"ipc_Qu8mg5v7( Normalized barrier function from [Li et al. 2023]. @@ -94,7 +95,7 @@ void define_barrier(py::module_& m) .def(py::init()); py::class_< - ClampedLogSqBarrier, Barrier, std::shared_ptr>( + ClampedLogSqBarrier<>, Barrier, std::shared_ptr>>( m, "ClampedLogSqBarrier", R"ipc_Qu8mg5v7( Clamped log barrier with a quadratic log term from [Huang et al. 2024]. @@ -106,7 +107,7 @@ void define_barrier(py::module_& m) )ipc_Qu8mg5v7") .def(py::init()); - py::class_>( + py::class_, Barrier, std::shared_ptr>>( m, "CubicBarrier", R"ipc_Qu8mg5v7( Cubic barrier function from [Ando 2024]. @@ -117,7 +118,7 @@ void define_barrier(py::module_& m) )ipc_Qu8mg5v7") .def(py::init()); - py::class_>( + py::class_, Barrier, std::shared_ptr>>( m, "TwoStageBarrier", R"ipc_Qu8mg5v7( Two-stage activation barrier from [Chen et al. 2025]. @@ -135,7 +136,7 @@ void define_barrier(py::module_& m) .def(py::init()); m.def( - "barrier", &barrier, "d"_a, "dhat"_a, + "barrier", &barrier<>, "d"_a, "dhat"_a, R"ipc_Qu8mg5v7( Function that grows to infinity as d approaches 0 from the right. @@ -152,7 +153,8 @@ void define_barrier(py::module_& m) )ipc_Qu8mg5v7"); m.def( - "barrier_first_derivative", &barrier_first_derivative, "d"_a, "dhat"_a, + "barrier_first_derivative", &barrier_first_derivative<>, "d"_a, + "dhat"_a, R"ipc_Qu8mg5v7( Derivative of the barrier function. @@ -170,7 +172,7 @@ void define_barrier(py::module_& m) )ipc_Qu8mg5v7"); m.def( - "barrier_second_derivative", &barrier_second_derivative, "d"_a, + "barrier_second_derivative", &barrier_second_derivative<>, "d"_a, "dhat"_a, R"ipc_Qu8mg5v7( Second derivative of the barrier function. diff --git a/python/src/distance/distance_type.cpp b/python/src/distance/distance_type.cpp index 2a1259ff6..08cca965c 100644 --- a/python/src/distance/distance_type.cpp +++ b/python/src/distance/distance_type.cpp @@ -86,7 +86,11 @@ void define_distance_type(py::module_& m) .export_values(); m.def( - "point_edge_distance_type", &point_edge_distance_type, + "point_edge_distance_type", + [](Eigen::ConstRef p, Eigen::ConstRef e0, + Eigen::ConstRef e1) { + return point_edge_distance_type(p, e0, e1); + }, R"ipc_Qu8mg5v7( Determine the closest pair between a point and edge. @@ -101,7 +105,8 @@ void define_distance_type(py::module_& m) "p"_a, "e0"_a, "e1"_a); m.def( - "point_triangle_distance_type", &point_triangle_distance_type, + "point_triangle_distance_type", + &detail::point_triangle_distance_type, R"ipc_Qu8mg5v7( Determine the closest pair between a point and triangle. @@ -117,7 +122,7 @@ void define_distance_type(py::module_& m) "p"_a, "t0"_a, "t1"_a, "t2"_a); m.def( - "edge_edge_distance_type", &edge_edge_distance_type, + "edge_edge_distance_type", &detail::edge_edge_distance_type, R"ipc_Qu8mg5v7( Determine the closest pair between two edges. @@ -133,7 +138,8 @@ void define_distance_type(py::module_& m) "ea0"_a, "ea1"_a, "eb0"_a, "eb1"_a); m.def( - "edge_edge_parallel_distance_type", &edge_edge_parallel_distance_type, + "edge_edge_parallel_distance_type", + &detail::edge_edge_parallel_distance_type, R"ipc_Qu8mg5v7( Determine the closest pair between two parallel edges. diff --git a/python/src/distance/edge_edge.cpp b/python/src/distance/edge_edge.cpp index 8db1ef867..ec03e64f6 100644 --- a/python/src/distance/edge_edge.cpp +++ b/python/src/distance/edge_edge.cpp @@ -8,7 +8,7 @@ using namespace ipc; void define_edge_edge_distance(py::module_& m) { m.def( - "edge_edge_distance", &edge_edge_distance, + "edge_edge_distance", &detail::edge_edge_distance, R"ipc_Qu8mg5v7( Compute the distance between a two lines segments in 3D. @@ -29,7 +29,8 @@ void define_edge_edge_distance(py::module_& m) "dtype"_a = EdgeEdgeDistanceType::AUTO); m.def( - "edge_edge_distance_gradient", &edge_edge_distance_gradient, + "edge_edge_distance_gradient", + &detail::edge_edge_distance_gradient, R"ipc_Qu8mg5v7( Compute the gradient of the distance between a two lines segments. @@ -50,7 +51,8 @@ void define_edge_edge_distance(py::module_& m) "dtype"_a = EdgeEdgeDistanceType::AUTO); m.def( - "edge_edge_distance_hessian", &edge_edge_distance_hessian, + "edge_edge_distance_hessian", + &detail::edge_edge_distance_hessian, R"ipc_Qu8mg5v7( Compute the hessian of the distance between a two lines segments. diff --git a/python/src/distance/edge_edge_mollifier.cpp b/python/src/distance/edge_edge_mollifier.cpp index be58b4445..eef2b0ab4 100644 --- a/python/src/distance/edge_edge_mollifier.cpp +++ b/python/src/distance/edge_edge_mollifier.cpp @@ -7,7 +7,8 @@ using namespace ipc; void define_edge_edge_mollifier(py::module_& m) { m.def( - "edge_edge_cross_squarednorm", &edge_edge_cross_squarednorm, + "edge_edge_cross_squarednorm", + &detail::edge_edge_cross_squarednorm, R"ipc_Qu8mg5v7( Compute the squared norm of the edge-edge cross product. @@ -24,7 +25,7 @@ void define_edge_edge_mollifier(py::module_& m) m.def( "edge_edge_cross_squarednorm_gradient", - &edge_edge_cross_squarednorm_gradient, + &detail::edge_edge_cross_squarednorm_gradient, R"ipc_Qu8mg5v7( Compute the gradient of the squared norm of the edge cross product. @@ -41,7 +42,7 @@ void define_edge_edge_mollifier(py::module_& m) m.def( "edge_edge_cross_squarednorm_hessian", - &edge_edge_cross_squarednorm_hessian, + &detail::edge_edge_cross_squarednorm_hessian, R"ipc_Qu8mg5v7( Compute the hessian of the squared norm of the edge cross product. @@ -58,7 +59,8 @@ void define_edge_edge_mollifier(py::module_& m) m.def( "edge_edge_mollifier", - py::overload_cast(&edge_edge_mollifier), + py::overload_cast( + &edge_edge_mollifier), R"ipc_Qu8mg5v7( Mollifier function for edge-edge distance. @@ -74,7 +76,7 @@ void define_edge_edge_mollifier(py::module_& m) m.def( "edge_edge_mollifier_gradient", py::overload_cast( - &edge_edge_mollifier_gradient), + &edge_edge_mollifier_gradient), R"ipc_Qu8mg5v7( The gradient of the mollifier function for edge-edge distance. @@ -89,7 +91,7 @@ void define_edge_edge_mollifier(py::module_& m) m.def( "edge_edge_mollifier_derivative_wrt_eps_x", - &edge_edge_mollifier_derivative_wrt_eps_x, + &edge_edge_mollifier_derivative_wrt_eps_x, R"ipc_Qu8mg5v7( The derivative of the mollifier function for edge-edge distance wrt eps_x. @@ -105,7 +107,7 @@ void define_edge_edge_mollifier(py::module_& m) m.def( "edge_edge_mollifier_hessian", py::overload_cast( - &edge_edge_mollifier_hessian), + &edge_edge_mollifier_hessian), R"ipc_Qu8mg5v7( The hessian of the mollifier function for edge-edge distance. @@ -120,7 +122,7 @@ void define_edge_edge_mollifier(py::module_& m) m.def( "edge_edge_mollifier_gradient_derivative_wrt_eps_x", - &edge_edge_mollifier_gradient_derivative_wrt_eps_x, + &edge_edge_mollifier_gradient_derivative_wrt_eps_x, R"ipc_Qu8mg5v7( The derivative of the gradient of the mollifier function for edge-edge distance wrt eps_x. @@ -134,13 +136,7 @@ void define_edge_edge_mollifier(py::module_& m) "x"_a, "eps_x"_a); m.def( - "edge_edge_mollifier", - py::overload_cast< - Eigen::ConstRef, - Eigen::ConstRef, - Eigen::ConstRef, - Eigen::ConstRef, const double>( - &edge_edge_mollifier), + "edge_edge_mollifier", &detail::edge_edge_mollifier, R"ipc_Qu8mg5v7( Compute a mollifier for the edge-edge distance. @@ -160,12 +156,7 @@ void define_edge_edge_mollifier(py::module_& m) m.def( "edge_edge_mollifier_gradient", - py::overload_cast< - Eigen::ConstRef, - Eigen::ConstRef, - Eigen::ConstRef, - Eigen::ConstRef, const double>( - &edge_edge_mollifier_gradient), + &detail::edge_edge_mollifier_gradient, R"ipc_Qu8mg5v7( Compute the gradient of the mollifier for the edge-edge distance. @@ -183,12 +174,7 @@ void define_edge_edge_mollifier(py::module_& m) m.def( "edge_edge_mollifier_hessian", - py::overload_cast< - Eigen::ConstRef, - Eigen::ConstRef, - Eigen::ConstRef, - Eigen::ConstRef, const double>( - &edge_edge_mollifier_hessian), + &detail::edge_edge_mollifier_hessian, R"ipc_Qu8mg5v7( Compute the hessian of the mollifier for the edge-edge distance. @@ -206,7 +192,7 @@ void define_edge_edge_mollifier(py::module_& m) m.def( "edge_edge_mollifier_gradient_wrt_x", - &edge_edge_mollifier_gradient_wrt_x, + &detail::edge_edge_mollifier_gradient_wrt_x, R"ipc_Qu8mg5v7( Compute the gradient of the mollifier for the edge-edge distance wrt rest positions. @@ -228,7 +214,7 @@ void define_edge_edge_mollifier(py::module_& m) m.def( "edge_edge_mollifier_gradient_jacobian_wrt_x", - &edge_edge_mollifier_gradient_jacobian_wrt_x, + &detail::edge_edge_mollifier_gradient_jacobian_wrt_x, R"ipc_Qu8mg5v7( Compute the jacobian of the edge-edge distance mollifier's gradient wrt rest positions. @@ -252,7 +238,8 @@ void define_edge_edge_mollifier(py::module_& m) "ea1"_a, "eb0"_a, "eb1"_a); m.def( - "edge_edge_mollifier_threshold", &edge_edge_mollifier_threshold, + "edge_edge_mollifier_threshold", + &detail::edge_edge_mollifier_threshold, R"ipc_Qu8mg5v7( Compute the threshold of the mollifier edge-edge distance. @@ -271,7 +258,7 @@ void define_edge_edge_mollifier(py::module_& m) m.def( "edge_edge_mollifier_threshold_gradient", - &edge_edge_mollifier_threshold_gradient, + &detail::edge_edge_mollifier_threshold_gradient, R"ipc_Qu8mg5v7( Compute the gradient of the threshold of the mollifier edge-edge distance. diff --git a/python/src/distance/line_line.cpp b/python/src/distance/line_line.cpp index e95970052..684bdee28 100644 --- a/python/src/distance/line_line.cpp +++ b/python/src/distance/line_line.cpp @@ -7,7 +7,7 @@ using namespace ipc; void define_line_line_distance(py::module_& m) { m.def( - "line_line_distance", &line_line_distance, + "line_line_distance", &detail::line_line_distance, R"ipc_Qu8mg5v7( Compute the distance between a two infinite lines in 3D. @@ -29,7 +29,13 @@ void define_line_line_distance(py::module_& m) "ea0"_a, "ea1"_a, "eb0"_a, "eb1"_a); m.def( - "line_line_distance_gradient", &line_line_distance_gradient, + "line_line_distance_gradient", + [](Eigen::ConstRef ea0, + Eigen::ConstRef ea1, + Eigen::ConstRef eb0, + Eigen::ConstRef eb1) { + return line_line_distance_gradient(ea0, ea1, eb0, eb1); + }, R"ipc_Qu8mg5v7( Compute the gradient of the distance between a two lines in 3D. @@ -51,7 +57,13 @@ void define_line_line_distance(py::module_& m) "ea0"_a, "ea1"_a, "eb0"_a, "eb1"_a); m.def( - "line_line_distance_hessian", &line_line_distance_hessian, + "line_line_distance_hessian", + [](Eigen::ConstRef ea0, + Eigen::ConstRef ea1, + Eigen::ConstRef eb0, + Eigen::ConstRef eb1) { + return line_line_distance_hessian(ea0, ea1, eb0, eb1); + }, R"ipc_Qu8mg5v7( Compute the hessian of the distance between a two lines in 3D. diff --git a/python/src/distance/point_edge.cpp b/python/src/distance/point_edge.cpp index 4b0a0fa8f..83b7115c0 100644 --- a/python/src/distance/point_edge.cpp +++ b/python/src/distance/point_edge.cpp @@ -8,7 +8,11 @@ using namespace ipc; void define_point_edge_distance(py::module_& m) { m.def( - "point_edge_distance", &point_edge_distance, + "point_edge_distance", + [](Eigen::ConstRef p, Eigen::ConstRef e0, + Eigen::ConstRef e1, PointEdgeDistanceType dtype) { + return point_edge_distance(p, e0, e1, dtype); + }, R"ipc_Qu8mg5v7( Compute the distance between a point and edge in 2D or 3D. @@ -27,7 +31,11 @@ void define_point_edge_distance(py::module_& m) "p"_a, "e0"_a, "e1"_a, "dtype"_a = PointEdgeDistanceType::AUTO); m.def( - "point_edge_distance_gradient", &point_edge_distance_gradient, + "point_edge_distance_gradient", + [](Eigen::ConstRef p, Eigen::ConstRef e0, + Eigen::ConstRef e1, PointEdgeDistanceType dtype) { + return point_edge_distance_gradient(p, e0, e1, dtype); + }, R"ipc_Qu8mg5v7( Compute the gradient of the distance between a point and edge. @@ -46,7 +54,11 @@ void define_point_edge_distance(py::module_& m) "p"_a, "e0"_a, "e1"_a, "dtype"_a = PointEdgeDistanceType::AUTO); m.def( - "point_edge_distance_hessian", &point_edge_distance_hessian, + "point_edge_distance_hessian", + [](Eigen::ConstRef p, Eigen::ConstRef e0, + Eigen::ConstRef e1, PointEdgeDistanceType dtype) { + return point_edge_distance_hessian(p, e0, e1, dtype); + }, R"ipc_Qu8mg5v7( Compute the hessian of the distance between a point and edge. diff --git a/python/src/distance/point_line.cpp b/python/src/distance/point_line.cpp index c500f3661..0e01eb6d1 100644 --- a/python/src/distance/point_line.cpp +++ b/python/src/distance/point_line.cpp @@ -7,7 +7,11 @@ using namespace ipc; void define_point_line_distance(py::module_& m) { m.def( - "point_line_distance", &point_line_distance, + "point_line_distance", + [](Eigen::ConstRef p, Eigen::ConstRef e0, + Eigen::ConstRef e1) { + return point_line_distance(p, e0, e1); + }, R"ipc_Qu8mg5v7( Compute the distance between a point and line in 2D or 3D. @@ -25,7 +29,11 @@ void define_point_line_distance(py::module_& m) "p"_a, "e0"_a, "e1"_a); m.def( - "point_line_distance_gradient", &point_line_distance_gradient, + "point_line_distance_gradient", + [](Eigen::ConstRef p, Eigen::ConstRef e0, + Eigen::ConstRef e1) { + return point_line_distance_gradient(p, e0, e1); + }, R"ipc_Qu8mg5v7( Compute the gradient of the distance between a point and line. @@ -43,7 +51,11 @@ void define_point_line_distance(py::module_& m) "p"_a, "e0"_a, "e1"_a); m.def( - "point_line_distance_hessian", &point_line_distance_hessian, + "point_line_distance_hessian", + [](Eigen::ConstRef p, Eigen::ConstRef e0, + Eigen::ConstRef e1) { + return point_line_distance_hessian(p, e0, e1); + }, R"ipc_Qu8mg5v7( Compute the hessian of the distance between a point and line. diff --git a/python/src/distance/point_plane.cpp b/python/src/distance/point_plane.cpp index 48073a1ce..9039eb0e6 100644 --- a/python/src/distance/point_plane.cpp +++ b/python/src/distance/point_plane.cpp @@ -9,9 +9,9 @@ void define_point_plane_distance(py::module_& m) m.def( "point_plane_distance", py::overload_cast< - Eigen::ConstRef, - Eigen::ConstRef, - Eigen::ConstRef>(&point_plane_distance), + Eigen::ConstRef, Eigen::ConstRef, + Eigen::ConstRef>( + &detail::point_plane_distance), R"ipc_Qu8mg5v7( Compute the distance between a point and a plane. @@ -31,10 +31,9 @@ void define_point_plane_distance(py::module_& m) m.def( "point_plane_distance", py::overload_cast< - Eigen::ConstRef, - Eigen::ConstRef, - Eigen::ConstRef, - Eigen::ConstRef>(&point_plane_distance), + Eigen::ConstRef, Eigen::ConstRef, + Eigen::ConstRef, Eigen::ConstRef>( + &detail::point_plane_distance), R"ipc_Qu8mg5v7( Compute the distance between a point and a plane. @@ -55,10 +54,9 @@ void define_point_plane_distance(py::module_& m) m.def( "point_plane_distance_gradient", py::overload_cast< - Eigen::ConstRef, - Eigen::ConstRef, - Eigen::ConstRef>( - &point_plane_distance_gradient), + Eigen::ConstRef, Eigen::ConstRef, + Eigen::ConstRef>( + &detail::point_plane_distance_gradient), R"ipc_Qu8mg5v7( Compute the gradient of the distance between a point and a plane. @@ -78,11 +76,9 @@ void define_point_plane_distance(py::module_& m) m.def( "point_plane_distance_gradient", py::overload_cast< - Eigen::ConstRef, - Eigen::ConstRef, - Eigen::ConstRef, - Eigen::ConstRef>( - &point_plane_distance_gradient), + Eigen::ConstRef, Eigen::ConstRef, + Eigen::ConstRef, Eigen::ConstRef>( + &detail::point_plane_distance_gradient), R"ipc_Qu8mg5v7( Compute the gradient of the distance between a point and a plane. @@ -103,10 +99,9 @@ void define_point_plane_distance(py::module_& m) m.def( "point_plane_distance_hessian", py::overload_cast< - Eigen::ConstRef, - Eigen::ConstRef, - Eigen::ConstRef>( - &point_plane_distance_hessian), + Eigen::ConstRef, Eigen::ConstRef, + Eigen::ConstRef>( + &detail::point_plane_distance_hessian), R"ipc_Qu8mg5v7( Compute the hessian of the distance between a point and a plane. @@ -126,11 +121,9 @@ void define_point_plane_distance(py::module_& m) m.def( "point_plane_distance_hessian", py::overload_cast< - Eigen::ConstRef, - Eigen::ConstRef, - Eigen::ConstRef, - Eigen::ConstRef>( - &point_plane_distance_hessian), + Eigen::ConstRef, Eigen::ConstRef, + Eigen::ConstRef, Eigen::ConstRef>( + &detail::point_plane_distance_hessian), R"ipc_Qu8mg5v7( Compute the hessian of the distance between a point and a plane. diff --git a/python/src/distance/point_point.cpp b/python/src/distance/point_point.cpp index 401d161f8..27bc3ce7a 100644 --- a/python/src/distance/point_point.cpp +++ b/python/src/distance/point_point.cpp @@ -8,7 +8,10 @@ using namespace ipc; void define_point_point_distance(py::module_& m) { m.def( - "point_point_distance", &point_point_distance, + "point_point_distance", + [](Eigen::ConstRef p0, Eigen::ConstRef p1) { + return point_point_distance(p0, p1); + }, R"ipc_Qu8mg5v7( Compute the distance between two points. @@ -25,7 +28,10 @@ void define_point_point_distance(py::module_& m) "p0"_a, "p1"_a); m.def( - "point_point_distance_gradient", &point_point_distance_gradient, + "point_point_distance_gradient", + [](Eigen::ConstRef p0, Eigen::ConstRef p1) { + return point_point_distance_gradient(p0, p1); + }, R"ipc_Qu8mg5v7( Compute the gradient of the distance between two points. @@ -42,7 +48,10 @@ void define_point_point_distance(py::module_& m) "p0"_a, "p1"_a); m.def( - "point_point_distance_hessian", &point_point_distance_hessian, + "point_point_distance_hessian", + [](Eigen::ConstRef p0, Eigen::ConstRef p1) { + return point_point_distance_hessian(p0, p1); + }, R"ipc_Qu8mg5v7( Compute the hessian of the distance between two points. diff --git a/python/src/distance/point_triangle.cpp b/python/src/distance/point_triangle.cpp index eb3419a3b..a51ab75e6 100644 --- a/python/src/distance/point_triangle.cpp +++ b/python/src/distance/point_triangle.cpp @@ -8,7 +8,7 @@ using namespace ipc; void define_point_triangle_distance(py::module_& m) { m.def( - "point_triangle_distance", &point_triangle_distance, + "point_triangle_distance", &detail::point_triangle_distance, R"ipc_Qu8mg5v7( Compute the distance between a points and a triangle. @@ -29,7 +29,8 @@ void define_point_triangle_distance(py::module_& m) "dtype"_a = PointTriangleDistanceType::AUTO); m.def( - "point_triangle_distance_gradient", &point_triangle_distance_gradient, + "point_triangle_distance_gradient", + &detail::point_triangle_distance_gradient, R"ipc_Qu8mg5v7( Compute the gradient of the distance between a points and a triangle. @@ -50,7 +51,8 @@ void define_point_triangle_distance(py::module_& m) "dtype"_a = PointTriangleDistanceType::AUTO); m.def( - "point_triangle_distance_hessian", &point_triangle_distance_hessian, + "point_triangle_distance_hessian", + &detail::point_triangle_distance_hessian, R"ipc_Qu8mg5v7( Compute the hessian of the distance between a points and a triangle. diff --git a/python/src/distance/signed_distance.cpp b/python/src/distance/signed_distance.cpp index 3fe174c67..f37297b06 100644 --- a/python/src/distance/signed_distance.cpp +++ b/python/src/distance/signed_distance.cpp @@ -10,7 +10,8 @@ void define_signed_distance(py::module_& m) { // Point-line (2D) signed distance m.def( - "point_line_signed_distance", point_line_signed_distance, + "point_line_signed_distance", + &detail::point_line_signed_distance, R"ipc_Qu8mg5v7( Compute the signed distance from a point to a directed line segment (2D). @@ -26,7 +27,7 @@ void define_signed_distance(py::module_& m) m.def( "point_line_signed_distance_gradient", - point_line_signed_distance_gradient, + &detail::point_line_signed_distance_gradient, R"ipc_Qu8mg5v7( Compute the gradient of the signed point-to-line distance (2D). @@ -36,7 +37,7 @@ void define_signed_distance(py::module_& m) m.def( "point_line_signed_distance_hessian", - point_line_signed_distance_hessian, + &detail::point_line_signed_distance_hessian, R"ipc_Qu8mg5v7( Compute the Hessian of the signed point-to-line distance (2D). @@ -46,7 +47,8 @@ void define_signed_distance(py::module_& m) // Point-plane (3D) signed distance m.def( - "point_plane_signed_distance", point_plane_signed_distance, + "point_plane_signed_distance", + &detail::point_plane_signed_distance, R"ipc_Qu8mg5v7( Compute the signed distance from a point to the plane of a triangle (3D). @@ -63,7 +65,7 @@ void define_signed_distance(py::module_& m) m.def( "point_plane_signed_distance_gradient", - point_plane_signed_distance_gradient, + &detail::point_plane_signed_distance_gradient, R"ipc_Qu8mg5v7( Compute the gradient of the signed point-to-plane distance (3D). @@ -73,7 +75,7 @@ void define_signed_distance(py::module_& m) m.def( "point_plane_signed_distance_hessian", - point_plane_signed_distance_hessian, + &detail::point_plane_signed_distance_hessian, R"ipc_Qu8mg5v7( Compute the Hessian of the signed point-to-plane distance (3D). @@ -83,7 +85,7 @@ void define_signed_distance(py::module_& m) // Line-line (3D) signed distance m.def( - "line_line_signed_distance", line_line_signed_distance, + "line_line_signed_distance", &detail::line_line_signed_distance, R"ipc_Qu8mg5v7( Compute the signed distance between two lines in 3D. @@ -98,7 +100,7 @@ void define_signed_distance(py::module_& m) m.def( "line_line_signed_distance_gradient", - line_line_signed_distance_gradient, + &detail::line_line_signed_distance_gradient, R"ipc_Qu8mg5v7( Compute the gradient of the signed line-line distance (3D). @@ -107,7 +109,8 @@ void define_signed_distance(py::module_& m) "ea0"_a, "ea1"_a, "eb0"_a, "eb1"_a); m.def( - "line_line_signed_distance_hessian", line_line_signed_distance_hessian, + "line_line_signed_distance_hessian", + &detail::line_line_signed_distance_hessian, R"ipc_Qu8mg5v7( Compute the Hessian of the signed line-line distance (3D). diff --git a/python/src/geometry/area.cpp b/python/src/geometry/area.cpp index ff917c2cb..ec9b3bb06 100644 --- a/python/src/geometry/area.cpp +++ b/python/src/geometry/area.cpp @@ -8,7 +8,10 @@ using namespace ipc; void define_area(py::module_& m) { m.def( - "edge_length_gradient", &edge_length_gradient, + "edge_length_gradient", + [](Eigen::ConstRef e0, Eigen::ConstRef e1) { + return edge_length_gradient(e0, e1); + }, R"ipc_Qu8mg5v7( Compute the gradient of an edge's length. @@ -22,7 +25,7 @@ void define_area(py::module_& m) "e0"_a, "e1"_a); m.def( - "triangle_area_gradient", &triangle_area_gradient, + "triangle_area_gradient", &detail::triangle_area_gradient, R"ipc_Qu8mg5v7( Compute the gradient of the area of a triangle. diff --git a/python/src/geometry/normal.cpp b/python/src/geometry/normal.cpp index b6cd14079..2defaf4fc 100644 --- a/python/src/geometry/normal.cpp +++ b/python/src/geometry/normal.cpp @@ -7,7 +7,10 @@ using namespace ipc; void define_normal(py::module_& m) { m.def( - "normalization_and_jacobian", &normalization_and_jacobian, + "normalization_and_jacobian", + [](Eigen::ConstRef x) { + return normalization_and_jacobian(x); + }, R"ipc_qu8mg5v7( Computes the normalization and Jacobian of a vector. @@ -22,7 +25,10 @@ void define_normal(py::module_& m) "x"_a); m.def( - "normalization_jacobian", &normalization_jacobian, + "normalization_jacobian", + [](Eigen::ConstRef x) { + return normalization_jacobian(x); + }, R"ipc_qu8mg5v7( Computes the Jacobian of the normalization operation. @@ -38,7 +44,9 @@ void define_normal(py::module_& m) m.def( "normalization_and_jacobian_and_hessian", - &normalization_and_jacobian_and_hessian, + [](Eigen::ConstRef x) { + return normalization_and_jacobian_and_hessian(x); + }, R"ipc_qu8mg5v7( Computes the normalization, Jacobian, and Hessian of a vector. @@ -53,7 +61,8 @@ void define_normal(py::module_& m) "x"_a); m.def( - "normalization_hessian", &normalization_hessian, + "normalization_hessian", + [](Eigen::ConstRef x) { return normalization_hessian(x); }, R"ipc_qu8mg5v7( Computes the Hessian of the normalization operation. @@ -68,7 +77,7 @@ void define_normal(py::module_& m) "x"_a); m.def( - "cross_product_matrix", &cross_product_matrix, + "cross_product_matrix", &detail::cross_product_matrix, R"ipc_qu8mg5v7( Cross product matrix for 3D vectors. @@ -83,7 +92,8 @@ void define_normal(py::module_& m) "v"_a); m.def( - "cross_product_matrix_jacobian", &cross_product_matrix_jacobian, + "cross_product_matrix_jacobian", + &detail::cross_product_matrix_jacobian, R"ipc_qu8mg5v7( Computes the Jacobian of the cross product matrix. Returns @@ -92,7 +102,8 @@ void define_normal(py::module_& m) )ipc_qu8mg5v7"); m.def( - "point_line_unnormalized_normal", &point_line_unnormalized_normal, + "point_line_unnormalized_normal", + &detail::point_line_unnormalized_normal, R"ipc_qu8mg5v7( Computes the unnormalized normal vector of a point-line pair. @@ -109,7 +120,7 @@ void define_normal(py::module_& m) "p"_a, "e0"_a, "e1"_a); m.def( - "point_line_normal", &point_line_normal, + "point_line_normal", &detail::point_line_normal, R"ipc_qu8mg5v7( Computes the normal vector of a point-line pair. @@ -127,7 +138,7 @@ void define_normal(py::module_& m) m.def( "point_line_unnormalized_normal_jacobian", - &point_line_unnormalized_normal_jacobian, + &detail::point_line_unnormalized_normal_jacobian, R"ipc_qu8mg5v7( Computes the Jacobian of the unnormalized normal vector of a point-line pair. @@ -144,7 +155,8 @@ void define_normal(py::module_& m) "p"_a, "e0"_a, "e1"_a); m.def( - "triangle_unnormalized_normal", &triangle_unnormalized_normal, + "triangle_unnormalized_normal", + &detail::triangle_unnormalized_normal, R"ipc_qu8mg5v7( Computes the unnormalized normal vector of a triangle. @@ -161,7 +173,7 @@ void define_normal(py::module_& m) "a"_a, "b"_a, "c"_a); m.def( - "triangle_normal", &triangle_normal, + "triangle_normal", &detail::triangle_normal, R"ipc_qu8mg5v7( Computes the normal vector of a triangle. @@ -179,7 +191,7 @@ void define_normal(py::module_& m) m.def( "triangle_unnormalized_normal_jacobian", - &triangle_unnormalized_normal_jacobian, + &detail::triangle_unnormalized_normal_jacobian, R"ipc_qu8mg5v7( Computes the Jacobian of the unnormalized normal vector of a triangle. @@ -197,7 +209,7 @@ void define_normal(py::module_& m) m.def( "triangle_unnormalized_normal_hessian", - &triangle_unnormalized_normal_hessian, + &detail::triangle_unnormalized_normal_hessian, R"ipc_qu8mg5v7( Computes the Hessian of the unnormalized normal vector of a triangle. @@ -214,7 +226,7 @@ void define_normal(py::module_& m) "a"_a, "b"_a, "c"_a); m.def( - "triangle_normal_jacobian", &triangle_normal_jacobian, + "triangle_normal_jacobian", &detail::triangle_normal_jacobian, R"ipc_qu8mg5v7( Computes the Jacobian of the normal vector of a triangle. @@ -231,7 +243,7 @@ void define_normal(py::module_& m) "a"_a, "b"_a, "c"_a); m.def( - "triangle_normal_hessian", &triangle_normal_hessian, + "triangle_normal_hessian", &detail::triangle_normal_hessian, R"ipc_qu8mg5v7( Computes the Hessian of the normal vector of a triangle. @@ -248,7 +260,8 @@ void define_normal(py::module_& m) "a"_a, "b"_a, "c"_a); m.def( - "line_line_unnormalized_normal", &line_line_unnormalized_normal, + "line_line_unnormalized_normal", + &detail::line_line_unnormalized_normal, R"ipc_qu8mg5v7( Computes the unnormalized normal vector of two lines. @@ -266,7 +279,7 @@ void define_normal(py::module_& m) "ea0"_a, "ea1"_a, "eb0"_a, "eb1"_a); m.def( - "line_line_normal", &line_line_normal, + "line_line_normal", &detail::line_line_normal, R"ipc_qu8mg5v7( Computes the normal vector of two lines. @@ -285,7 +298,7 @@ void define_normal(py::module_& m) m.def( "line_line_unnormalized_normal_jacobian", - &line_line_unnormalized_normal_jacobian, + &detail::line_line_unnormalized_normal_jacobian, R"ipc_qu8mg5v7( Computes the Jacobian of the unnormalized normal vector of two lines. @@ -303,7 +316,7 @@ void define_normal(py::module_& m) "ea0"_a, "ea1"_a, "eb0"_a, "eb1"_a); m.def( - "line_line_normal_jacobian", &line_line_normal_jacobian, + "line_line_normal_jacobian", &detail::line_line_normal_jacobian, R"ipc_qu8mg5v7( Computes the Jacobian of the normal vector of two lines. @@ -322,7 +335,7 @@ void define_normal(py::module_& m) m.def( "line_line_unnormalized_normal_hessian", - &line_line_unnormalized_normal_hessian, + &detail::line_line_unnormalized_normal_hessian, R"ipc_qu8mg5v7( Computes the Hessian of the unnormalized normal vector of two lines. @@ -340,7 +353,7 @@ void define_normal(py::module_& m) "ea0"_a, "ea1"_a, "eb0"_a, "eb1"_a); m.def( - "line_line_normal_hessian", &line_line_normal_hessian, + "line_line_normal_hessian", &detail::line_line_normal_hessian, R"ipc_qu8mg5v7( Computes the Hessian of the normal vector of two lines. diff --git a/python/src/tangent/relative_velocity.cpp b/python/src/tangent/relative_velocity.cpp index 6574ec52c..7a45dbc35 100644 --- a/python/src/tangent/relative_velocity.cpp +++ b/python/src/tangent/relative_velocity.cpp @@ -7,7 +7,10 @@ using namespace ipc; void define_relative_velocity(py::module_& m) { m.def( - "point_point_relative_velocity", &point_point_relative_velocity, + "point_point_relative_velocity", + [](Eigen::ConstRef dp0, Eigen::ConstRef dp1) { + return point_point_relative_velocity(dp0, dp1); + }, R"ipc_Qu8mg5v7( Compute the relative velocity of two points @@ -22,7 +25,7 @@ void define_relative_velocity(py::module_& m) m.def( "point_point_relative_velocity_jacobian", - &point_point_relative_velocity_jacobian, + &point_point_relative_velocity_jacobian, R"ipc_Qu8mg5v7( Compute the point-point relative velocity premultiplier matrix @@ -36,7 +39,7 @@ void define_relative_velocity(py::module_& m) m.def( "point_point_relative_velocity_dx_dbeta", - &point_point_relative_velocity_dx_dbeta, + &point_point_relative_velocity_dx_dbeta, R"ipc_Qu8mg5v7( Compute the Jacobian of the relative velocity premultiplier matrix @@ -49,7 +52,11 @@ void define_relative_velocity(py::module_& m) "dim"_a); m.def( - "point_edge_relative_velocity", &point_edge_relative_velocity, + "point_edge_relative_velocity", + [](Eigen::ConstRef dp, Eigen::ConstRef de0, + Eigen::ConstRef de1, const double alpha) { + return point_edge_relative_velocity(dp, de0, de1, alpha); + }, R"ipc_Qu8mg5v7( Compute the relative velocity of a point and an edge @@ -66,7 +73,7 @@ void define_relative_velocity(py::module_& m) m.def( "point_edge_relative_velocity_jacobian", - &point_edge_relative_velocity_jacobian, + &point_edge_relative_velocity_jacobian, R"ipc_Qu8mg5v7( Compute the point-edge relative velocity premultiplier matrix @@ -81,7 +88,7 @@ void define_relative_velocity(py::module_& m) m.def( "point_edge_relative_velocity_dx_dbeta", - &point_edge_relative_velocity_dx_dbeta, + &point_edge_relative_velocity_dx_dbeta, R"ipc_Qu8mg5v7( Compute the Jacobian of the relative velocity premultiplier matrix @@ -95,7 +102,8 @@ void define_relative_velocity(py::module_& m) "dim"_a, "alpha"_a); m.def( - "edge_edge_relative_velocity", &edge_edge_relative_velocity, + "edge_edge_relative_velocity", + &detail::edge_edge_relative_velocity, R"ipc_Qu8mg5v7( Compute the relative velocity of the edges. @@ -113,7 +121,7 @@ void define_relative_velocity(py::module_& m) m.def( "edge_edge_relative_velocity_jacobian", - &edge_edge_relative_velocity_jacobian, + &detail::edge_edge_relative_velocity_jacobian, R"ipc_Qu8mg5v7( Compute the edge-edge relative velocity matrix. @@ -127,7 +135,7 @@ void define_relative_velocity(py::module_& m) m.def( "edge_edge_relative_velocity_dx_dbeta", - &edge_edge_relative_velocity_dx_dbeta, + &detail::edge_edge_relative_velocity_dx_dbeta, R"ipc_Qu8mg5v7( Compute the Jacobian of the edge-edge relative velocity matrix. @@ -140,7 +148,8 @@ void define_relative_velocity(py::module_& m) "coords"_a); m.def( - "point_triangle_relative_velocity", &point_triangle_relative_velocity, + "point_triangle_relative_velocity", + &detail::point_triangle_relative_velocity, R"ipc_Qu8mg5v7( Compute the relative velocity of the point to the triangle. @@ -158,7 +167,7 @@ void define_relative_velocity(py::module_& m) m.def( "point_triangle_relative_velocity_jacobian", - &point_triangle_relative_velocity_jacobian, + &detail::point_triangle_relative_velocity_jacobian, R"ipc_Qu8mg5v7( Compute the point-triangle relative velocity matrix. @@ -172,7 +181,7 @@ void define_relative_velocity(py::module_& m) m.def( "point_triangle_relative_velocity_dx_dbeta", - &point_triangle_relative_velocity_dx_dbeta, + &detail::point_triangle_relative_velocity_dx_dbeta, R"ipc_Qu8mg5v7( Compute the Jacobian of the point-triangle relative velocity matrix. diff --git a/src/ipc/barrier/barrier.cpp b/src/ipc/barrier/barrier.cpp index 0d5013247..742f1712f 100644 --- a/src/ipc/barrier/barrier.cpp +++ b/src/ipc/barrier/barrier.cpp @@ -1,3 +1,5 @@ +#include + // Barrier functions that grow to infinity as x -> 0+. Includes gradient and // hessian functions, too. These barrier functions can be used to impose // inequality constraints on a function. @@ -8,23 +10,23 @@ namespace ipc { -double barrier(const double d, const double dhat) +template T barrier(const T d, const T dhat) { - if (d <= 0.0) { - return std::numeric_limits::infinity(); + if (d <= T(0)) { + return std::numeric_limits::infinity(); } if (d >= dhat) { - return 0; + return T(0); } // b(d) = -(d-d̂)²ln(d / d̂) - const double d_minus_dhat = (d - dhat); + const T d_minus_dhat = (d - dhat); return -d_minus_dhat * d_minus_dhat * log(d / dhat); } -double barrier_first_derivative(const double d, const double dhat) +template T barrier_first_derivative(const T d, const T dhat) { - if (d <= 0.0 || d >= dhat) { - return 0.0; + if (d <= T(0) || d >= dhat) { + return T(0); } // b(d) = -(d - d̂)²ln(d / d̂) // b'(d) = -2(d - d̂)ln(d / d̂) - (d-d̂)²(1 / d) @@ -33,124 +35,151 @@ double barrier_first_derivative(const double d, const double dhat) return (dhat - d) * (2 * log(d / dhat) - dhat / d + 1); } -double barrier_second_derivative(const double d, const double dhat) +template T barrier_second_derivative(const T d, const T dhat) { - if (d <= 0.0 || d >= dhat) { - return 0.0; + if (d <= T(0) || d >= dhat) { + return T(0); } - const double dhat_d = dhat / d; + const T dhat_d = dhat / d; return (dhat_d + 2) * dhat_d - 2 * log(d / dhat) - 3; } // ============================================================================ -double ClampedLogSqBarrier::operator()(const double d, const double dhat) const +template +T ClampedLogSqBarrier::operator()(const T d, const T dhat) const { - if (d <= 0.0) { - return std::numeric_limits::infinity(); + if (d <= T(0)) { + return std::numeric_limits::infinity(); } if (d >= dhat) { - return 0; + return T(0); } // b(d) = (d-d̂)²ln²(d / d̂) - const double d_minus_dhat = (d - dhat); - const double log_d_dhat = log(d / dhat); + const T d_minus_dhat = (d - dhat); + const T log_d_dhat = log(d / dhat); return d_minus_dhat * d_minus_dhat * log_d_dhat * log_d_dhat; } -double -ClampedLogSqBarrier::first_derivative(const double d, const double dhat) const +template +T ClampedLogSqBarrier::first_derivative(const T d, const T dhat) const { - if (d <= 0.0 || d >= dhat) { - return 0.0; + if (d <= T(0) || d >= dhat) { + return T(0); } // b(d) = (d - d̂)²ln²(d / d̂) // b'(d) = 2 (d - d̂) ln²(d / d̂) + 2 (d - d̂)² ln(d / d̂) / d // = 2 (d - d̂) ln(d / d̂) [ln(d / d̂) + (d - d̂) / d] - const double d_minus_dhat = (d - dhat); - const double log_d_dhat = log(d / dhat); - return 2 * d_minus_dhat * log_d_dhat * (log_d_dhat + d_minus_dhat / d); + const T d_minus_dhat = (d - dhat); + const T log_d_dhat = log(d / dhat); + return T(2) * d_minus_dhat * log_d_dhat * (log_d_dhat + d_minus_dhat / d); } -double -ClampedLogSqBarrier::second_derivative(const double d, const double dhat) const +template +T ClampedLogSqBarrier::second_derivative(const T d, const T dhat) const { - if (d <= 0.0 || d >= dhat) { - return 0.0; + if (d <= T(0) || d >= dhat) { + return T(0); } - const double t0 = dhat - d; - const double t1 = log(d / dhat); - const double t2 = (t0 * t0) / (d * d); - return 2 * ((t1 * t1) - (t1 - 1) * t2 - 4 * t1 * t0 / d); + const T t0 = dhat - d; + const T t1 = log(d / dhat); + const T t2 = (t0 * t0) / (d * d); + return T(2) * ((t1 * t1) - (t1 - T(1)) * t2 - T(4) * t1 * t0 / d); } // ============================================================================ -double CubicBarrier::operator()(const double d, const double dhat) const +template +T CubicBarrier::operator()(const T d, const T dhat) const { if (d < dhat) { // b(d) = (d - d̂)³ - const double d_minus_dhat = (d - dhat); - return -2.0 / 3.0 / dhat * d_minus_dhat * d_minus_dhat * d_minus_dhat; + const T d_minus_dhat = (d - dhat); + return -T(2) / T(3) / dhat * d_minus_dhat * d_minus_dhat * d_minus_dhat; } else { - return 0; + return T(0); } } -double CubicBarrier::first_derivative(const double d, const double dhat) const +template +T CubicBarrier::first_derivative(const T d, const T dhat) const { if (d < dhat) { - const double d_minus_dhat = (d - dhat); - return -2 / dhat * d_minus_dhat * d_minus_dhat; + const T d_minus_dhat = (d - dhat); + return T(-2) / dhat * d_minus_dhat * d_minus_dhat; } else { - return 0; + return T(0); } } -double CubicBarrier::second_derivative(const double d, const double dhat) const +template +T CubicBarrier::second_derivative(const T d, const T dhat) const { if (d < dhat) { - return 4 * (1 - d / dhat); + return T(4) * (T(1) - d / dhat); } else { - return 0; + return T(0); } } // ============================================================================ -double TwoStageBarrier::operator()(const double d, const double dhat) const +template +T TwoStageBarrier::operator()(const T d, const T dhat) const { if (d >= dhat) { - return 0.0; - } else if (d >= 0.5 * dhat) { - return 0.5 * (dhat - d) * (dhat - d); + return T(0); + } else if (d >= T(0.5) * dhat) { + return T(0.5) * (dhat - d) * (dhat - d); } else { - return -0.25 * dhat * dhat * (std::log(2 * d / dhat) - 0.5); + return T(-0.25) * dhat * dhat * (std::log(T(2) * d / dhat) - T(0.5)); } } -double -TwoStageBarrier::first_derivative(const double d, const double dhat) const +template +T TwoStageBarrier::first_derivative(const T d, const T dhat) const { if (d >= dhat) { - return 0.0; - } else if (d >= 0.5 * dhat) { + return T(0); + } else if (d >= T(0.5) * dhat) { return d - dhat; } else { - return -0.25 * dhat * dhat / d; + return T(-0.25) * dhat * dhat / d; } } -double -TwoStageBarrier::second_derivative(const double d, const double dhat) const +template +T TwoStageBarrier::second_derivative(const T d, const T dhat) const { if (d >= dhat) { - return 0.0; - } else if (d >= 0.5 * dhat) { - return 1.0; + return T(0); + } else if (d >= T(0.5) * dhat) { + return T(1); } else { - return (0.25 * dhat * dhat) / (d * d); + return (T(0.25) * dhat * dhat) / (d * d); } } +// ============================================================================ +// Explicit template instantiations +/// @cond DOXYGEN_SKIP +template class BarrierBase; +template class BarrierBase; +template class ClampedLogBarrier; +template class ClampedLogBarrier; +template class ClampedLogSqBarrier; +template class ClampedLogSqBarrier; +template class CubicBarrier; +template class CubicBarrier; +template class TwoStageBarrier; +template class TwoStageBarrier; +template float barrier(const float d, const float dhat); +template double barrier(const double d, const double dhat); +template float barrier_first_derivative(const float d, const float dhat); +template double barrier_first_derivative(const double d, const double dhat); +template float barrier_second_derivative(const float d, const float dhat); +template double barrier_second_derivative(const double d, const double dhat); +/// @endcond +// ============================================================================ + } // namespace ipc diff --git a/src/ipc/barrier/barrier.hpp b/src/ipc/barrier/barrier.hpp index e6e3a30c9..105705140 100644 --- a/src/ipc/barrier/barrier.hpp +++ b/src/ipc/barrier/barrier.hpp @@ -4,41 +4,47 @@ #pragma once +#include + namespace ipc { /// Base class for barrier functions. -class Barrier { +template class BarrierBase { +protected: + using value_type = T; + public: - Barrier() = default; - virtual ~Barrier() = default; + BarrierBase() = default; + virtual ~BarrierBase() = default; /// @brief Evaluate the barrier function. /// @param d Distance. /// @param dhat Activation distance of the barrier. /// @return The value of the barrier function at d. - virtual double operator()(const double d, const double dhat) const = 0; + virtual T operator()(const T d, const T dhat) const = 0; /// @brief Evaluate the first derivative of the barrier function wrt d. /// @param d Distance. /// @param dhat Activation distance of the barrier. /// @return The value of the first derivative of the barrier function at d. - virtual double - first_derivative(const double d, const double dhat) const = 0; + virtual T first_derivative(const T d, const T dhat) const = 0; /// @brief Evaluate the second derivative of the barrier function wrt d. /// @param d Distance. /// @param dhat Activation distance of the barrier. /// @return The value of the second derivative of the barrier function at d. - virtual double - second_derivative(const double d, const double dhat) const = 0; + virtual T second_derivative(const T d, const T dhat) const = 0; /// @brief Get the units of the barrier function. /// Essentially, barrier(d, d̂) / units(d̂) should be dimensionless. /// @param dhat The activation distance of the barrier. /// @return The units of the barrier function. - virtual double units(const double dhat) const = 0; + virtual T units(const T dhat) const = 0; }; +/// @brief Default barrier type. +using Barrier = BarrierBase<>; + // ============================================================================ // Barrier functions from [Li et al. 2020] // ============================================================================ @@ -52,7 +58,7 @@ class Barrier { /// @param d The distance. /// @param dhat Activation distance of the barrier. /// @return The value of the barrier function at d. -double barrier(const double d, const double dhat); +template T barrier(const T d, const T dhat); /// @brief Derivative of the barrier function. /// @@ -64,7 +70,8 @@ double barrier(const double d, const double dhat); /// @param d The distance. /// @param dhat Activation distance of the barrier. /// @return The derivative of the barrier wrt d. -double barrier_first_derivative(const double d, const double dhat); +template +T barrier_first_derivative(const T d, const T dhat); /// @brief Second derivative of the barrier function. /// @@ -76,10 +83,11 @@ double barrier_first_derivative(const double d, const double dhat); /// @param d The distance. /// @param dhat Activation distance of the barrier. /// @return The second derivative of the barrier wrt d. -double barrier_second_derivative(const double d, const double dhat); +template +T barrier_second_derivative(const T d, const T dhat); /// @brief Smoothly clamped log barrier functions from [Li et al. 2020]. -class ClampedLogBarrier : public Barrier { +template class ClampedLogBarrier : public BarrierBase { public: ClampedLogBarrier() = default; @@ -92,7 +100,7 @@ class ClampedLogBarrier : public Barrier { /// @param d The distance. /// @param dhat Activation distance of the barrier. /// @return The value of the barrier function at d. - double operator()(const double d, const double dhat) const override + T operator()(const T d, const T dhat) const override { return barrier(d, dhat); } @@ -107,7 +115,7 @@ class ClampedLogBarrier : public Barrier { /// @param d The distance. /// @param dhat Activation distance of the barrier. /// @return The derivative of the barrier wrt d. - double first_derivative(const double d, const double dhat) const override + T first_derivative(const T d, const T dhat) const override { return barrier_first_derivative(d, dhat); } @@ -122,7 +130,7 @@ class ClampedLogBarrier : public Barrier { /// @param d The distance. /// @param dhat Activation distance of the barrier. /// @return The second derivative of the barrier wrt d. - double second_derivative(const double d, const double dhat) const override + T second_derivative(const T d, const T dhat) const override { return barrier_second_derivative(d, dhat); } @@ -130,7 +138,7 @@ class ClampedLogBarrier : public Barrier { /// @brief Get the units of the barrier function. /// @param dhat The activation distance of the barrier. /// @return The units of the barrier function. - double units(const double dhat) const override + T units(const T dhat) const override { // (d - d̂)² = d̂² (d/d̂ - 1)² return dhat * dhat; @@ -143,6 +151,8 @@ class ClampedLogBarrier : public Barrier { /// @brief Normalized barrier function from [Li et al. 2023]. template class NormalizedBarrier : public BarrierT { + using Scalar = typename BarrierT::value_type; + public: NormalizedBarrier() = default; @@ -156,9 +166,9 @@ template class NormalizedBarrier : public BarrierT { /// @param d The distance. /// @param dhat Activation distance of the barrier. /// @return The value of the barrier function at d. - double operator()(const double d, const double dhat) const override + Scalar operator()(const Scalar d, const Scalar dhat) const override { - return BarrierT::operator()(d / dhat, 1.0); + return BarrierT::operator()(d / dhat, Scalar(1)); } /// @brief Derivative of the barrier function. @@ -172,9 +182,9 @@ template class NormalizedBarrier : public BarrierT { /// @param d The distance. /// @param dhat Activation distance of the barrier. /// @return The derivative of the barrier wrt d. - double first_derivative(const double d, const double dhat) const override + Scalar first_derivative(const Scalar d, const Scalar dhat) const override { - return BarrierT::first_derivative(d / dhat, 1.0) / dhat; + return BarrierT::first_derivative(d / dhat, Scalar(1)) / dhat; } /// @brief Second derivative of the barrier function. @@ -187,28 +197,30 @@ template class NormalizedBarrier : public BarrierT { /// @param d The distance. /// @param dhat Activation distance of the barrier. /// @return The second derivative of the barrier wrt d. - double second_derivative(const double d, const double dhat) const override + Scalar second_derivative(const Scalar d, const Scalar dhat) const override { - return BarrierT::second_derivative(d / dhat, 1.0) / (dhat * dhat); + return BarrierT::second_derivative(d / dhat, Scalar(1)) / (dhat * dhat); } /// @brief Get the units of the barrier function. /// @param dhat The activation distance of the barrier. /// @return The units of the barrier function. - double units(const double dhat) const override + Scalar units(const Scalar dhat) const override { - return 1.0; // The normalized barrier is dimensionless. + return Scalar(1); // The normalized barrier is dimensionless. } }; -using NormalizedClampedLogBarrier = NormalizedBarrier; +template +using NormalizedClampedLogBarrier = NormalizedBarrier>; // ============================================================================ // Quadratic log barrier functions from [Huang et al. 2024] // ============================================================================ /// @brief Clamped log barrier with a quadratic log term from [Huang et al. 2024]. -class ClampedLogSqBarrier : public Barrier { +template +class ClampedLogSqBarrier : public BarrierBase { public: ClampedLogSqBarrier() = default; @@ -221,7 +233,7 @@ class ClampedLogSqBarrier : public Barrier { /// @param d The distance. /// @param dhat Activation distance of the barrier. /// @return The value of the barrier function at d. - double operator()(const double d, const double dhat) const override; + T operator()(const T d, const T dhat) const override; /// @brief Derivative of the barrier function. /// @@ -234,7 +246,7 @@ class ClampedLogSqBarrier : public Barrier { /// @param d The distance. /// @param dhat Activation distance of the barrier. /// @return The derivative of the barrier wrt d. - double first_derivative(const double d, const double dhat) const override; + T first_derivative(const T d, const T dhat) const override; /// @brief Second derivative of the barrier function. /// @@ -248,12 +260,12 @@ class ClampedLogSqBarrier : public Barrier { /// @param d The distance. /// @param dhat Activation distance of the barrier. /// @return The second derivative of the barrier wrt d. - double second_derivative(const double d, const double dhat) const override; + T second_derivative(const T d, const T dhat) const override; /// @brief Get the units of the barrier function. /// @param dhat The activation distance of the barrier. /// @return The units of the barrier function. - double units(const double dhat) const override + T units(const T dhat) const override { // (d - d̂)² = d̂² (d/d̂ - 1)² return dhat * dhat; @@ -265,7 +277,7 @@ class ClampedLogSqBarrier : public Barrier { // ============================================================================ /// @brief Cubic barrier function from [Ando 2024]. -class CubicBarrier : public Barrier { +template class CubicBarrier : public BarrierBase { public: CubicBarrier() = default; @@ -278,7 +290,7 @@ class CubicBarrier : public Barrier { /// @param d The distance. /// @param dhat Activation distance of the barrier. /// @return The value of the barrier function at d. - double operator()(const double d, const double dhat) const override; + T operator()(const T d, const T dhat) const override; /// @brief Derivative of the barrier function. /// @@ -289,7 +301,7 @@ class CubicBarrier : public Barrier { /// @param d The distance. /// @param dhat Activation distance of the barrier. /// @return The derivative of the barrier wrt d. - double first_derivative(const double d, const double dhat) const override; + T first_derivative(const T d, const T dhat) const override; /// @brief Second derivative of the barrier function. /// @@ -300,12 +312,12 @@ class CubicBarrier : public Barrier { /// @param d The distance. /// @param dhat Activation distance of the barrier. /// @return The second derivative of the barrier wrt d. - double second_derivative(const double d, const double dhat) const override; + T second_derivative(const T d, const T dhat) const override; /// @brief Get the units of the barrier function. /// @param dhat The activation distance of the barrier. /// @return The units of the barrier function. - double units(const double dhat) const override + T units(const T dhat) const override { // (d - d̂)² = d̂² (d/d̂ - 1)² return dhat * dhat; @@ -317,7 +329,7 @@ class CubicBarrier : public Barrier { // ============================================================================ /// @brief 2-Stage activation function from [Chen et al. 2025]. -class TwoStageBarrier : public Barrier { +template class TwoStageBarrier : public BarrierBase { public: TwoStageBarrier() = default; @@ -337,7 +349,7 @@ class TwoStageBarrier : public Barrier { * @param dhat Activation distance of the barrier. * @return The value of the barrier function at d. */ - double operator()(const double d, const double dhat) const override; + T operator()(const T d, const T dhat) const override; /** * @brief Derivative of the barrier function. @@ -354,7 +366,7 @@ class TwoStageBarrier : public Barrier { * @param dhat Activation distance of the barrier. * @return The derivative of the barrier wrt d. */ - double first_derivative(const double d, const double dhat) const override; + T first_derivative(const T d, const T dhat) const override; /** * @brief Second derivative of the barrier function. @@ -371,12 +383,12 @@ class TwoStageBarrier : public Barrier { * @param dhat Activation distance of the barrier. * @return The second derivative of the barrier wrt d. */ - double second_derivative(const double d, const double dhat) const override; + T second_derivative(const T d, const T dhat) const override; /// @brief Get the units of the barrier function. /// @param dhat The activation distance of the barrier. /// @return The units of the barrier function. - double units(const double dhat) const override + T units(const T dhat) const override { // (d - d̂)² = d̂² (d/d̂ - 1)² return dhat * dhat; diff --git a/src/ipc/candidates/edge_vertex.cpp b/src/ipc/candidates/edge_vertex.cpp index 5384488d7..c02edd6cb 100644 --- a/src/ipc/candidates/edge_vertex.cpp +++ b/src/ipc/candidates/edge_vertex.cpp @@ -26,20 +26,32 @@ VectorMax12d EdgeVertexCandidate::compute_distance_gradient( Eigen::ConstRef positions) const { assert(positions.size() == 6 || positions.size() == 9); - const int dim = this->dim(positions.size()); - return point_edge_distance_gradient( - positions.head(dim), positions.segment(dim, dim), positions.tail(dim), - known_dtype()); + // Branching here is faster (3.1x) than passing dynamic slices. + if (positions.size() == 6) { + return point_edge_distance_gradient( + positions.head<2>(), positions.segment<2>(2), positions.tail<2>(), + known_dtype()); + } else { + return point_edge_distance_gradient( + positions.head<3>(), positions.segment<3>(3), positions.tail<3>(), + known_dtype()); + } } MatrixMax12d EdgeVertexCandidate::compute_distance_hessian( Eigen::ConstRef positions) const { assert(positions.size() == 6 || positions.size() == 9); - const int dim = this->dim(positions.size()); - return point_edge_distance_hessian( - positions.head(dim), positions.segment(dim, dim), positions.tail(dim), - known_dtype()); + // Branching here is faster than passing dynamic slices. + if (positions.size() == 6) { + return point_edge_distance_hessian( + positions.head<2>(), positions.segment<2>(2), positions.tail<2>(), + known_dtype()); + } else { + return point_edge_distance_hessian( + positions.head<3>(), positions.segment<3>(3), positions.tail<3>(), + known_dtype()); + } } VectorMax4d EdgeVertexCandidate::compute_coefficients( diff --git a/src/ipc/distance/CMakeLists.txt b/src/ipc/distance/CMakeLists.txt index 846253b06..1897af881 100644 --- a/src/ipc/distance/CMakeLists.txt +++ b/src/ipc/distance/CMakeLists.txt @@ -13,7 +13,6 @@ set(SOURCES point_line.hpp point_plane.cpp point_plane.hpp - point_point.cpp point_point.hpp point_triangle.cpp point_triangle.hpp diff --git a/src/ipc/distance/distance_type.cpp b/src/ipc/distance/distance_type.cpp index aa9b145b9..f9b281e42 100644 --- a/src/ipc/distance/distance_type.cpp +++ b/src/ipc/distance/distance_type.cpp @@ -4,116 +4,141 @@ #include #include +#include -namespace ipc { +#include +#include -PointEdgeDistanceType point_edge_distance_type( - Eigen::ConstRef p, - Eigen::ConstRef e0, - Eigen::ConstRef e1) -{ - assert(p.size() == 2 || p.size() == 3); - assert(e0.size() == 2 || e0.size() == 3); - assert(e1.size() == 2 || e1.size() == 3); - - const VectorMax3d e = e1 - e0; - const double e_length_sqr = e.squaredNorm(); - if (e_length_sqr == 0) { - logger().warn("Degenerate edge in point_edge_distance_type!"); - return PointEdgeDistanceType::P_E0; // WARNING: use arbitrary end-point - } +namespace ipc::detail { - const double ratio = e.dot(p - e0) / e_length_sqr; - if (ratio < 0) { - return PointEdgeDistanceType::P_E0; // PP (p-e0) - } else if (ratio > 1) { - return PointEdgeDistanceType::P_E1; // PP (p-e1) - } else { - return PointEdgeDistanceType::P_E; // PE - } +void warn_degenerate_point_edge() noexcept +{ + logger().warn("Degenerate edge in point_edge_distance_type!"); } -PointTriangleDistanceType point_triangle_distance_type( - Eigen::ConstRef p, - Eigen::ConstRef t0, - Eigen::ConstRef t1, - Eigen::ConstRef t2) +void throw_invalid_distance_type(const char* function) { - const Eigen::Vector3d normal = (t1 - t0).cross(t2 - t0); + throw std::invalid_argument( + fmt::format("{}: invalid distance type", function)); +} - Eigen::Matrix basis, param; +void throw_auto_requires_explicit_dtype(const char* function) +{ + throw std::invalid_argument( + fmt::format( + "{}: an explicit distance type is required for non-floating-point " + "scalars; resolving AUTO means comparing single ordered values, " + "which an autodiff, SIMD batch, or interval scalar does not " + "provide", + function)); +} - basis.row(0) = t1 - t0; - basis.row(1) = basis.row(0).cross(normal); - param.col(0) = (basis * basis.transpose()).ldlt().solve(basis * (p - t0)); - if (param(0, 0) > 0.0 && param(0, 0) < 1.0 && param(1, 0) >= 0.0) { +template +PointTriangleDistanceType point_triangle_distance_type( + Eigen::ConstRef> p, + Eigen::ConstRef> t0, + Eigen::ConstRef> t1, + Eigen::ConstRef> t2) +{ + const Eigen::Vector3 normal = (t1 - t0).cross(t2 - t0); + + // Closed form for the per-edge solve + // param.col(k) = (basis * basisᵀ).ldlt().solve(basis * (p - tₖ)) + // with basis = [eₖ; eₖ×n]. Every edge eₖ lies in the triangle's plane, so + // n ⊥ eₖ and the two rows of basis are orthogonal: the 2×2 Gram matrix + // basis·basisᵀ is diagonal, diag(eₖ·eₖ, (eₖ×n)·(eₖ×n)), and the solve + // collapses to two divisions. (The off-diagonal eₖ·(eₖ×n) is symbolically + // zero; in floating point it is O(ε)‖eₖ‖²‖n‖, and by Cramer's rule keeping + // it would move the solution by only O(ε) relative — below the rounding of + // the divisions themselves.) + // + // Eigen's LDLT solves with the pseudo-inverse of D, zeroing any component + // whose diagonal entry vanishes; the `> 0` guards reproduce that, so a + // degenerate edge (‖eₖ‖ = 0) or a degenerate triangle (n = 0) classifies as + // before instead of dividing by zero. + T along[3]; // parameter along each edge; reused by the vertex tests below + + // NOTE: eₖ×n points out of the triangle, so off ≥ 0 means p is on the + // outer side of edge k. + const auto closest_to_edge = [&normal, &along]( + const Eigen::Vector3& e, + const Eigen::Vector3& d, + const int k) -> bool { + const Eigen::Vector3 perp = e.cross(normal); + const T a = e.squaredNorm(); + const T c = perp.squaredNorm(); + along[k] = a > T(0) ? T(e.dot(d) / a) : T(0); + const T off = c > T(0) ? T(perp.dot(d) / c) : T(0); + return along[k] > T(0) && along[k] < T(1) && off >= T(0); + }; + + if (closest_to_edge(t1 - t0, p - t0, 0)) { return PointTriangleDistanceType::P_E0; // edge 0 is the closest } - - basis.row(0) = t2 - t1; - basis.row(1) = basis.row(0).cross(normal); - param.col(1) = (basis * basis.transpose()).ldlt().solve(basis * (p - t1)); - if (param(0, 1) > 0.0 && param(0, 1) < 1.0 && param(1, 1) >= 0.0) { + if (closest_to_edge(t2 - t1, p - t1, 1)) { return PointTriangleDistanceType::P_E1; // edge 1 is the closest } - - basis.row(0) = t0 - t2; - basis.row(1) = basis.row(0).cross(normal); - param.col(2) = (basis * basis.transpose()).ldlt().solve(basis * (p - t2)); - if (param(0, 2) > 0.0 && param(0, 2) < 1.0 && param(1, 2) >= 0.0) { + if (closest_to_edge(t0 - t2, p - t2, 2)) { return PointTriangleDistanceType::P_E2; // edge 2 is the closest } - if (param(0, 0) <= 0.0 && param(0, 2) >= 1.0) { - // vertex 0 is the closest - return PointTriangleDistanceType::P_T0; - } else if (param(0, 1) <= 0.0 && param(0, 0) >= 1.0) { - // vertex 1 is the closest - return PointTriangleDistanceType::P_T1; - } else if (param(0, 2) <= 0.0 && param(0, 1) >= 1.0) { - // vertex 2 is the closest - return PointTriangleDistanceType::P_T2; + if (along[0] <= 0 && along[2] >= 1) { + return PointTriangleDistanceType::P_T0; // vertex 0 is the closest + } else if (along[1] <= 0 && along[0] >= 1) { + return PointTriangleDistanceType::P_T1; // vertex 1 is the closest + } else if (along[2] <= 0 && along[1] >= 1) { + return PointTriangleDistanceType::P_T2; // vertex 2 is the closest } else { return PointTriangleDistanceType::P_T; } } // A more robust implementation of http://geomalgorithms.com/a07-_distance.html +template EdgeEdgeDistanceType edge_edge_distance_type( - Eigen::ConstRef ea0, - Eigen::ConstRef ea1, - Eigen::ConstRef eb0, - Eigen::ConstRef eb1) + Eigen::ConstRef> ea0, + Eigen::ConstRef> ea1, + Eigen::ConstRef> eb0, + Eigen::ConstRef> eb1) { // Relative sin² threshold for parallelism: treat edges as parallel when // sin²(θ) < PARALLEL_THRESHOLD, scaled by a*c. This avoids misclassifying - // short nearly-collinear coplanar segments (which the old absolute - // threshold could not handle) and routes such cases to + // short nearly-collinear coplanar segments (which an absolute threshold + // could not handle) and routes such cases to // edge_edge_parallel_distance_type. - constexpr double PARALLEL_THRESHOLD = 2.5e-16; - - const Eigen::Vector3d u = ea1 - ea0; - const Eigen::Vector3d v = eb1 - eb0; - const Eigen::Vector3d w = ea0 - eb0; - - const double a = u.squaredNorm(); // always ≥ 0 - const double b = u.dot(v); - const double c = v.squaredNorm(); // always ≥ 0 - const double d = u.dot(w); - const double e = v.dot(w); - const double D = a * c - b * b; // always ≥ 0 + // + // NOTE: u×v cancels for nearly parallel edges, leaving an absolute error of + // ≈ε‖u‖‖v‖ per component, so sin²(θ) cannot be resolved below ≈ε². The + // threshold must therefore scale with the precision of T: 2.5e-16 is tuned + // for double (≈1.13ε), but in single precision it sits ~57× *below* the + // noise floor, which would make this branch unreachable. + constexpr T PARALLEL_THRESHOLD = static_cast( + 2.5e-16 + * (static_cast(std::numeric_limits::epsilon()) + / std::numeric_limits::epsilon())); + + const Eigen::Vector3 u = ea1 - ea0; + const Eigen::Vector3 v = eb1 - eb0; + const Eigen::Vector3 w = ea0 - eb0; + + const T a = u.squaredNorm(); // always ≥ 0 + const T b = u.dot(v); + const T c = v.squaredNorm(); // always ≥ 0 + const T d = u.dot(w); + const T e = v.dot(w); + const T D = a * c - b * b; // always ≥ 0 // Degenerate cases should not happen in practice, but we handle them - if (a == 0.0 && c == 0.0) { + if (a == 0 && c == 0) { return EdgeEdgeDistanceType::EA0_EB0; - } else if (a == 0.0) { + } else if (a == 0) { return EdgeEdgeDistanceType::EA0_EB; - } else if (c == 0.0) { + } else if (c == 0) { return EdgeEdgeDistanceType::EA_EB0; } // Special handling for parallel edges - const double parallel_tolerance = PARALLEL_THRESHOLD * a * c; + const T parallel_tolerance = PARALLEL_THRESHOLD * a * c; if (u.cross(v).squaredNorm() < parallel_tolerance) { return edge_edge_parallel_distance_type(ea0, ea1, eb0, eb1); } @@ -121,9 +146,9 @@ EdgeEdgeDistanceType edge_edge_distance_type( EdgeEdgeDistanceType default_case = EdgeEdgeDistanceType::EA_EB; // compute the line parameters of the two closest points - const double sN = (b * e - c * d); - double tN, tD; // tc = tN / tD - if (sN <= 0.0) { // sc < 0 ⟹ the s=0 edge is visible + const T sN = (b * e - c * d); + T tN, tD; // tc = tN / tD + if (sN <= 0) { // sc < 0 ⟹ the s=0 edge is visible tN = e; tD = c; default_case = EdgeEdgeDistanceType::EA0_EB; @@ -134,7 +159,7 @@ EdgeEdgeDistanceType edge_edge_distance_type( } else { tN = (a * e - b * d); tD = D; // default tD = D ≥ 0 - if (tN > 0.0 && tN < tD + if (tN > 0 && tN < tD && u.cross(v).squaredNorm() < parallel_tolerance) { // avoid coplanar or nearly parallel EE if (sN < D / 2) { @@ -150,9 +175,9 @@ EdgeEdgeDistanceType edge_edge_distance_type( // else default_case stays EdgeEdgeDistanceType::EA_EB } - if (tN <= 0.0) { // tc < 0 ⟹ the t=0 edge is visible + if (tN <= 0) { // tc < 0 ⟹ the t=0 edge is visible // recompute sc for this edge - if (-d <= 0.0) { + if (-d <= 0) { return EdgeEdgeDistanceType::EA0_EB0; } else if (-d >= a) { return EdgeEdgeDistanceType::EA1_EB0; @@ -161,7 +186,7 @@ EdgeEdgeDistanceType edge_edge_distance_type( } } else if (tN >= tD) { // tc > 1 ⟹ the t=1 edge is visible // recompute sc for this edge - if ((-d + b) <= 0.0) { + if ((-d + b) <= 0) { return EdgeEdgeDistanceType::EA0_EB1; } else if ((-d + b) >= a) { return EdgeEdgeDistanceType::EA1_EB1; @@ -173,15 +198,16 @@ EdgeEdgeDistanceType edge_edge_distance_type( return default_case; } +template EdgeEdgeDistanceType edge_edge_parallel_distance_type( - Eigen::ConstRef ea0, - Eigen::ConstRef ea1, - Eigen::ConstRef eb0, - Eigen::ConstRef eb1) + Eigen::ConstRef> ea0, + Eigen::ConstRef> ea1, + Eigen::ConstRef> eb0, + Eigen::ConstRef> eb1) { - const Eigen::Vector3d ea = ea1 - ea0; - const double alpha = (eb0 - ea0).dot(ea) / ea.squaredNorm(); - const double beta = (eb1 - ea0).dot(ea) / ea.squaredNorm(); + const Eigen::Vector3 ea = ea1 - ea0; + const T alpha = (eb0 - ea0).dot(ea) / ea.squaredNorm(); + const T beta = (eb1 - ea0).dot(ea) / ea.squaredNorm(); uint8_t eac; // 0: EA0, 1: EA1, 2: EA uint8_t ebc; // 0: EB0, 1: EB1, 2: EB @@ -210,4 +236,13 @@ EdgeEdgeDistanceType edge_edge_parallel_distance_type( return EdgeEdgeDistanceType(ebc < 2 ? (eac << 1 | ebc) : (6 + eac)); } -} // namespace ipc +// clang-format off +template PointTriangleDistanceType point_triangle_distance_type(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef); +template PointTriangleDistanceType point_triangle_distance_type(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef); +template EdgeEdgeDistanceType edge_edge_distance_type(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef); +template EdgeEdgeDistanceType edge_edge_distance_type(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef); +template EdgeEdgeDistanceType edge_edge_parallel_distance_type(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef); +template EdgeEdgeDistanceType edge_edge_parallel_distance_type(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef); +// clang-format on + +} // namespace ipc::detail diff --git a/src/ipc/distance/distance_type.hpp b/src/ipc/distance/distance_type.hpp index 3f4b877c6..786d38ec2 100644 --- a/src/ipc/distance/distance_type.hpp +++ b/src/ipc/distance/distance_type.hpp @@ -2,6 +2,9 @@ #include +#include +#include + namespace ipc { /// @brief Closest pair between a point and point. @@ -54,15 +57,126 @@ enum class EdgeEdgeDistanceType : uint8_t { AUTO }; +namespace detail { + /// @brief Warn about a degenerate edge. + /// @note Out of line to keep the logger (and spdlog) out of this header. + void warn_degenerate_point_edge() noexcept; + + /// @brief Throw for an invalid distance type. + /// @note Out of line and [[noreturn]] so that constructing the exception does not consume the caller's inlining budget on the hot path. + [[noreturn]] void throw_invalid_distance_type(const char* function); + + /// @brief Throw when AUTO is requested for a scalar that cannot resolve it. + [[noreturn]] void throw_auto_requires_explicit_dtype(const char* function); + + /// @brief Determine the closest pair between a point and edge. + /// @note Prefer the ipc::point_edge_distance_type front end below, which deduces both the scalar type and the dimension. + /// @tparam T The scalar type. + /// @tparam dim The dimension (2 or 3). + /// @param p The point. + /// @param e0 The first vertex of the edge. + /// @param e1 The second vertex of the edge. + /// @return The distance type of the point-edge pair. + template + inline PointEdgeDistanceType point_edge_distance_type( + const Eigen::Vector& p, + const Eigen::Vector& e0, + const Eigen::Vector& e1) + { + static_assert(dim == 2 || dim == 3, "point-edge is only 2D or 3D"); + + const Eigen::Vector e = e1 - e0; + const T e_length_sqr = e.squaredNorm(); + if (e_length_sqr == 0) { + warn_degenerate_point_edge(); + // WARNING: use an arbitrary end-point + return PointEdgeDistanceType::P_E0; + } + + const T ratio = e.dot(p - e0) / e_length_sqr; + if (ratio < 0) { + return PointEdgeDistanceType::P_E0; // PP (p-e0) + } else if (ratio > 1) { + return PointEdgeDistanceType::P_E1; // PP (p-e1) + } else { + return PointEdgeDistanceType::P_E; // PE + } + } + + /// @brief Determine the closest pair between a point and triangle. + /// @param p The point. + /// @param t0 The first vertex of the triangle. + /// @param t1 The second vertex of the triangle. + /// @param t2 The third vertex of the triangle. + /// @return The distance type of the point-triangle pair. + template + PointTriangleDistanceType point_triangle_distance_type( + Eigen::ConstRef> p, + Eigen::ConstRef> t0, + Eigen::ConstRef> t1, + Eigen::ConstRef> t2); + + /// @brief Determine the closest pair between two edges. + /// @param ea0 The first vertex of the first edge. + /// @param ea1 The second vertex of the first edge. + /// @param eb0 The first vertex of the second edge. + /// @param eb1 The second vertex of the second edge. + /// @return The distance type of the edge-edge pair. + template + EdgeEdgeDistanceType edge_edge_distance_type( + Eigen::ConstRef> ea0, + Eigen::ConstRef> ea1, + Eigen::ConstRef> eb0, + Eigen::ConstRef> eb1); + + /// @brief Determine the closest pair between two parallel edges. + /// @param ea0 The first vertex of the first edge. + /// @param ea1 The second vertex of the first edge. + /// @param eb0 The first vertex of the second edge. + /// @param eb1 The second vertex of the second edge. + /// @return The distance type of the edge-edge pair. + template + EdgeEdgeDistanceType edge_edge_parallel_distance_type( + Eigen::ConstRef> ea0, + Eigen::ConstRef> ea1, + Eigen::ConstRef> eb0, + Eigen::ConstRef> eb1); +} // namespace detail + +// --- EigenExpression wrappers --- +// +// NOTE: The std::is_class_v guard is required. Unlike the wrappers whose +// trailing return type mentions typename DerivedX::Scalar, these return a +// non-dependent enum, so nothing would reject the candidate when the first +// template argument is given explicitly as a scalar (e.g. +// edge_edge_distance_type(...)). Substitution would then form +// Eigen::MatrixBase, a hard error inside Eigen rather than a SFINAE +// failure. + /// @brief Determine the closest pair between a point and edge. /// @param p The point. /// @param e0 The first vertex of the edge. /// @param e1 The second vertex of the edge. /// @return The distance type of the point-edge pair. -PointEdgeDistanceType point_edge_distance_type( - Eigen::ConstRef p, - Eigen::ConstRef e0, - Eigen::ConstRef e1); +template +inline PointEdgeDistanceType point_edge_distance_type( + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& e0, + const Eigen::MatrixBase& e1) +{ + using T = typename DerivedP::Scalar; + + if constexpr (dim_v == 2) { + return detail::point_edge_distance_type(p, e0, e1); + } else if constexpr (dim_v == 3) { + return detail::point_edge_distance_type(p, e0, e1); + } else if (p.size() == 2) { + return detail::point_edge_distance_type(p, e0, e1); + } else { + assert(p.size() == 3); + return detail::point_edge_distance_type(p, e0, e1); + } +} /// @brief Determine the closest pair between a point and triangle. /// @param p The point. @@ -70,11 +184,21 @@ PointEdgeDistanceType point_edge_distance_type( /// @param t1 The second vertex of the triangle. /// @param t2 The third vertex of the triangle. /// @return The distance type of the point-triangle pair. -PointTriangleDistanceType point_triangle_distance_type( - Eigen::ConstRef p, - Eigen::ConstRef t0, - Eigen::ConstRef t1, - Eigen::ConstRef t2); +template < + typename DerivedP, + typename DerivedT0, + typename DerivedT1, + typename DerivedT2, + std::enable_if_t, int> = 0> +inline PointTriangleDistanceType point_triangle_distance_type( + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& t0, + const Eigen::MatrixBase& t1, + const Eigen::MatrixBase& t2) +{ + using T = typename DerivedP::Scalar; + return detail::point_triangle_distance_type(p, t0, t1, t2); +} /// @brief Determine the closest pair between two edges. /// @param ea0 The first vertex of the first edge. @@ -82,11 +206,21 @@ PointTriangleDistanceType point_triangle_distance_type( /// @param eb0 The first vertex of the second edge. /// @param eb1 The second vertex of the second edge. /// @return The distance type of the edge-edge pair. -EdgeEdgeDistanceType edge_edge_distance_type( - Eigen::ConstRef ea0, - Eigen::ConstRef ea1, - Eigen::ConstRef eb0, - Eigen::ConstRef eb1); +template < + typename DerivedEA0, + typename DerivedEA1, + typename DerivedEB0, + typename DerivedEB1, + std::enable_if_t, int> = 0> +inline EdgeEdgeDistanceType edge_edge_distance_type( + const Eigen::MatrixBase& ea0, + const Eigen::MatrixBase& ea1, + const Eigen::MatrixBase& eb0, + const Eigen::MatrixBase& eb1) +{ + using T = typename DerivedEA0::Scalar; + return detail::edge_edge_distance_type(ea0, ea1, eb0, eb1); +} /// @brief Determine the closest pair between two parallel edges. /// @param ea0 The first vertex of the first edge. @@ -94,10 +228,20 @@ EdgeEdgeDistanceType edge_edge_distance_type( /// @param eb0 The first vertex of the second edge. /// @param eb1 The second vertex of the second edge. /// @return The distance type of the edge-edge pair. -EdgeEdgeDistanceType edge_edge_parallel_distance_type( - Eigen::ConstRef ea0, - Eigen::ConstRef ea1, - Eigen::ConstRef eb0, - Eigen::ConstRef eb1); +template < + typename DerivedEA0, + typename DerivedEA1, + typename DerivedEB0, + typename DerivedEB1, + std::enable_if_t, int> = 0> +inline EdgeEdgeDistanceType edge_edge_parallel_distance_type( + const Eigen::MatrixBase& ea0, + const Eigen::MatrixBase& ea1, + const Eigen::MatrixBase& eb0, + const Eigen::MatrixBase& eb1) +{ + using T = typename DerivedEA0::Scalar; + return detail::edge_edge_parallel_distance_type(ea0, ea1, eb0, eb1); +} } // namespace ipc diff --git a/src/ipc/distance/edge_edge.cpp b/src/ipc/distance/edge_edge.cpp index b8c0395fa..fd2df2aa9 100644 --- a/src/ipc/distance/edge_edge.cpp +++ b/src/ipc/distance/edge_edge.cpp @@ -1,24 +1,27 @@ - - #include "edge_edge.hpp" +#include #include #include #include +#include -#include // std::invalid_argument - -namespace ipc { +namespace ipc::detail { -double edge_edge_distance( - Eigen::ConstRef ea0, - Eigen::ConstRef ea1, - Eigen::ConstRef eb0, - Eigen::ConstRef eb1, +template +T edge_edge_distance( + Eigen::ConstRef> ea0, + Eigen::ConstRef> ea1, + Eigen::ConstRef> eb0, + Eigen::ConstRef> eb1, EdgeEdgeDistanceType dtype) { - if (dtype == EdgeEdgeDistanceType::AUTO) { - dtype = edge_edge_distance_type(ea0, ea1, eb0, eb1); + if constexpr (std::is_floating_point_v) { + if (dtype == EdgeEdgeDistanceType::AUTO) { + dtype = edge_edge_distance_type(ea0, ea1, eb0, eb1); + } + } else if (dtype == EdgeEdgeDistanceType::AUTO) { + throw_auto_requires_explicit_dtype("edge_edge_distance"); } switch (dtype) { @@ -50,73 +53,81 @@ double edge_edge_distance( return line_line_distance(ea0, ea1, eb0, eb1); default: - throw std::invalid_argument( - "Invalid distance type for edge-edge distance!"); + throw_invalid_distance_type("edge_edge_distance"); } } -Vector12d edge_edge_distance_gradient( - Eigen::ConstRef ea0, - Eigen::ConstRef ea1, - Eigen::ConstRef eb0, - Eigen::ConstRef eb1, +template +Eigen::Vector edge_edge_distance_gradient( + Eigen::ConstRef> ea0, + Eigen::ConstRef> ea1, + Eigen::ConstRef> eb0, + Eigen::ConstRef> eb1, EdgeEdgeDistanceType dtype) { - if (dtype == EdgeEdgeDistanceType::AUTO) { - dtype = edge_edge_distance_type(ea0, ea1, eb0, eb1); + using Vector6T = Eigen::Vector; + using Vector9T = Eigen::Vector; + using Vector12T = Eigen::Vector; + + if constexpr (std::is_floating_point_v) { + if (dtype == EdgeEdgeDistanceType::AUTO) { + dtype = edge_edge_distance_type(ea0, ea1, eb0, eb1); + } + } else if (dtype == EdgeEdgeDistanceType::AUTO) { + throw_auto_requires_explicit_dtype("edge_edge_distance_gradient"); } - Vector12d grad = Vector12d::Zero(); + Vector12T grad = Vector12T::Zero(); switch (dtype) { case EdgeEdgeDistanceType::EA0_EB0: { - const Vector6d local_grad = point_point_distance_gradient(ea0, eb0); - grad.head<3>() = local_grad.head<3>(); - grad.segment<3>(6) = local_grad.tail<3>(); + const Vector6T local_grad = point_point_distance_gradient(ea0, eb0); + grad.template head<3>() = local_grad.template head<3>(); + grad.template segment<3>(6) = local_grad.template tail<3>(); break; } case EdgeEdgeDistanceType::EA0_EB1: { - const Vector6d local_grad = point_point_distance_gradient(ea0, eb1); - grad.head<3>() = local_grad.head<3>(); - grad.tail<3>() = local_grad.tail<3>(); + const Vector6T local_grad = point_point_distance_gradient(ea0, eb1); + grad.template head<3>() = local_grad.template head<3>(); + grad.template tail<3>() = local_grad.template tail<3>(); break; } case EdgeEdgeDistanceType::EA1_EB0: - grad.segment<6>(3) = point_point_distance_gradient(ea1, eb0); + grad.template segment<6>(3) = point_point_distance_gradient(ea1, eb0); break; case EdgeEdgeDistanceType::EA1_EB1: { - const Vector6d local_grad = point_point_distance_gradient(ea1, eb1); - grad.segment<3>(3) = local_grad.head<3>(); - grad.tail<3>() = local_grad.tail<3>(); + const Vector6T local_grad = point_point_distance_gradient(ea1, eb1); + grad.template segment<3>(3) = local_grad.template head<3>(); + grad.template tail<3>() = local_grad.template tail<3>(); break; } case EdgeEdgeDistanceType::EA_EB0: { - const Vector9d local_grad = point_line_distance_gradient(eb0, ea0, ea1); - grad.head<6>() = local_grad.tail<6>(); - grad.segment<3>(6) = local_grad.head<3>(); + const Vector9T local_grad = point_line_distance_gradient(eb0, ea0, ea1); + grad.template head<6>() = local_grad.template tail<6>(); + grad.template segment<3>(6) = local_grad.template head<3>(); break; } case EdgeEdgeDistanceType::EA_EB1: { - const Vector9d local_grad = point_line_distance_gradient(eb1, ea0, ea1); - grad.head<6>() = local_grad.tail<6>(); - grad.tail<3>() = local_grad.head<3>(); + const Vector9T local_grad = point_line_distance_gradient(eb1, ea0, ea1); + grad.template head<6>() = local_grad.template tail<6>(); + grad.template tail<3>() = local_grad.template head<3>(); break; } case EdgeEdgeDistanceType::EA0_EB: { - const Vector9d local_grad = point_line_distance_gradient(ea0, eb0, eb1); - grad.head<3>() = local_grad.head<3>(); - grad.tail<6>() = local_grad.tail<6>(); + const Vector9T local_grad = point_line_distance_gradient(ea0, eb0, eb1); + grad.template head<3>() = local_grad.template head<3>(); + grad.template tail<6>() = local_grad.template tail<6>(); break; } case EdgeEdgeDistanceType::EA1_EB: - grad.tail<9>() = point_line_distance_gradient(ea1, eb0, eb1); + grad.template tail<9>() = point_line_distance_gradient(ea1, eb0, eb1); break; case EdgeEdgeDistanceType::EA_EB: @@ -124,87 +135,120 @@ Vector12d edge_edge_distance_gradient( break; default: - throw std::invalid_argument( - "Invalid distance type for edge-edge distance gradient!"); + throw_invalid_distance_type("edge_edge_distance_gradient"); } return grad; } -Matrix12d edge_edge_distance_hessian( - Eigen::ConstRef ea0, - Eigen::ConstRef ea1, - Eigen::ConstRef eb0, - Eigen::ConstRef eb1, +template +Eigen::Matrix edge_edge_distance_hessian( + Eigen::ConstRef> ea0, + Eigen::ConstRef> ea1, + Eigen::ConstRef> eb0, + Eigen::ConstRef> eb1, EdgeEdgeDistanceType dtype) { - if (dtype == EdgeEdgeDistanceType::AUTO) { - dtype = edge_edge_distance_type(ea0, ea1, eb0, eb1); + using Matrix6T = Eigen::Matrix; + using Matrix9T = Eigen::Matrix; + using Matrix12T = Eigen::Matrix; + + if constexpr (std::is_floating_point_v) { + if (dtype == EdgeEdgeDistanceType::AUTO) { + dtype = edge_edge_distance_type(ea0, ea1, eb0, eb1); + } + } else if (dtype == EdgeEdgeDistanceType::AUTO) { + throw_auto_requires_explicit_dtype("edge_edge_distance_hessian"); } - Matrix12d hess = Matrix12d::Zero(); + Matrix12T hess = Matrix12T::Zero(); switch (dtype) { case EdgeEdgeDistanceType::EA0_EB0: { - const Matrix6d local_hess = point_point_distance_hessian(ea0, eb0); - hess.topLeftCorner<3, 3>() = local_hess.topLeftCorner<3, 3>(); - hess.block<3, 3>(0, 6) = local_hess.topRightCorner<3, 3>(); - hess.block<3, 3>(6, 0) = local_hess.bottomLeftCorner<3, 3>(); - hess.block<3, 3>(6, 6) = local_hess.bottomRightCorner<3, 3>(); + const Matrix6T local_hess = point_point_distance_hessian(ea0, eb0); + hess.template topLeftCorner<3, 3>() = + local_hess.template topLeftCorner<3, 3>(); + hess.template block<3, 3>(0, 6) = + local_hess.template topRightCorner<3, 3>(); + hess.template block<3, 3>(6, 0) = + local_hess.template bottomLeftCorner<3, 3>(); + hess.template block<3, 3>(6, 6) = + local_hess.template bottomRightCorner<3, 3>(); break; } case EdgeEdgeDistanceType::EA0_EB1: { - const Matrix6d local_hess = point_point_distance_hessian(ea0, eb1); - hess.topLeftCorner<3, 3>() = local_hess.topLeftCorner<3, 3>(); - hess.topRightCorner<3, 3>() = local_hess.topRightCorner<3, 3>(); - hess.bottomLeftCorner<3, 3>() = local_hess.bottomLeftCorner<3, 3>(); - hess.bottomRightCorner<3, 3>() = local_hess.bottomRightCorner<3, 3>(); + const Matrix6T local_hess = point_point_distance_hessian(ea0, eb1); + hess.template topLeftCorner<3, 3>() = + local_hess.template topLeftCorner<3, 3>(); + hess.template topRightCorner<3, 3>() = + local_hess.template topRightCorner<3, 3>(); + hess.template bottomLeftCorner<3, 3>() = + local_hess.template bottomLeftCorner<3, 3>(); + hess.template bottomRightCorner<3, 3>() = + local_hess.template bottomRightCorner<3, 3>(); break; } case EdgeEdgeDistanceType::EA1_EB0: - hess.block<6, 6>(3, 3) = point_point_distance_hessian(ea1, eb0); + hess.template block<6, 6>(3, 3) = + point_point_distance_hessian(ea1, eb0); break; case EdgeEdgeDistanceType::EA1_EB1: { - const Matrix6d local_hess = point_point_distance_hessian(ea1, eb1); - hess.block<3, 3>(3, 3) = local_hess.topLeftCorner<3, 3>(); - hess.block<3, 3>(3, 9) = local_hess.topRightCorner<3, 3>(); - hess.block<3, 3>(9, 3) = local_hess.bottomLeftCorner<3, 3>(); - hess.bottomRightCorner<3, 3>() = local_hess.bottomRightCorner<3, 3>(); + const Matrix6T local_hess = point_point_distance_hessian(ea1, eb1); + hess.template block<3, 3>(3, 3) = + local_hess.template topLeftCorner<3, 3>(); + hess.template block<3, 3>(3, 9) = + local_hess.template topRightCorner<3, 3>(); + hess.template block<3, 3>(9, 3) = + local_hess.template bottomLeftCorner<3, 3>(); + hess.template bottomRightCorner<3, 3>() = + local_hess.template bottomRightCorner<3, 3>(); break; } case EdgeEdgeDistanceType::EA_EB0: { - const Matrix9d local_hess = point_line_distance_hessian(eb0, ea0, ea1); - hess.topLeftCorner<6, 6>() = local_hess.bottomRightCorner<6, 6>(); - hess.block<3, 6>(6, 0) = local_hess.topRightCorner<3, 6>(); - hess.block<6, 3>(0, 6) = local_hess.bottomLeftCorner<6, 3>(); - hess.block<3, 3>(6, 6) = local_hess.topLeftCorner<3, 3>(); + const Matrix9T local_hess = point_line_distance_hessian(eb0, ea0, ea1); + hess.template topLeftCorner<6, 6>() = + local_hess.template bottomRightCorner<6, 6>(); + hess.template block<3, 6>(6, 0) = + local_hess.template topRightCorner<3, 6>(); + hess.template block<6, 3>(0, 6) = + local_hess.template bottomLeftCorner<6, 3>(); + hess.template block<3, 3>(6, 6) = + local_hess.template topLeftCorner<3, 3>(); break; } case EdgeEdgeDistanceType::EA_EB1: { - const Matrix9d local_hess = point_line_distance_hessian(eb1, ea0, ea1); - hess.topLeftCorner<6, 6>() = local_hess.bottomRightCorner<6, 6>(); - hess.topRightCorner<6, 3>() = local_hess.bottomLeftCorner<6, 3>(); - hess.bottomLeftCorner<3, 6>() = local_hess.topRightCorner<3, 6>(); - hess.bottomRightCorner<3, 3>() = local_hess.topLeftCorner<3, 3>(); + const Matrix9T local_hess = point_line_distance_hessian(eb1, ea0, ea1); + hess.template topLeftCorner<6, 6>() = + local_hess.template bottomRightCorner<6, 6>(); + hess.template topRightCorner<6, 3>() = + local_hess.template bottomLeftCorner<6, 3>(); + hess.template bottomLeftCorner<3, 6>() = + local_hess.template topRightCorner<3, 6>(); + hess.template bottomRightCorner<3, 3>() = + local_hess.template topLeftCorner<3, 3>(); break; } case EdgeEdgeDistanceType::EA0_EB: { - const Matrix9d local_hess = point_line_distance_hessian(ea0, eb0, eb1); - hess.topLeftCorner<3, 3>() = local_hess.topLeftCorner<3, 3>(); - hess.topRightCorner<3, 6>() = local_hess.topRightCorner<3, 6>(); - hess.bottomLeftCorner<6, 3>() = local_hess.bottomLeftCorner<6, 3>(); - hess.bottomRightCorner<6, 6>() = local_hess.bottomRightCorner<6, 6>(); + const Matrix9T local_hess = point_line_distance_hessian(ea0, eb0, eb1); + hess.template topLeftCorner<3, 3>() = + local_hess.template topLeftCorner<3, 3>(); + hess.template topRightCorner<3, 6>() = + local_hess.template topRightCorner<3, 6>(); + hess.template bottomLeftCorner<6, 3>() = + local_hess.template bottomLeftCorner<6, 3>(); + hess.template bottomRightCorner<6, 6>() = + local_hess.template bottomRightCorner<6, 6>(); break; } case EdgeEdgeDistanceType::EA1_EB: - hess.bottomRightCorner<9, 9>() = + hess.template bottomRightCorner<9, 9>() = point_line_distance_hessian(ea1, eb0, eb1); break; @@ -213,11 +257,47 @@ Matrix12d edge_edge_distance_hessian( break; default: - throw std::invalid_argument( - "Invalid distance type for edge-edge distance hessian!"); + throw_invalid_distance_type("edge_edge_distance_hessian"); } return hess; } -} // namespace ipc +#define IPC_INSTANTIATE_EDGE_EDGE(T) \ + template T edge_edge_distance( \ + Eigen::ConstRef>, \ + Eigen::ConstRef>, \ + Eigen::ConstRef>, \ + Eigen::ConstRef>, EdgeEdgeDistanceType); \ + template Eigen::Vector edge_edge_distance_gradient( \ + Eigen::ConstRef>, \ + Eigen::ConstRef>, \ + Eigen::ConstRef>, \ + Eigen::ConstRef>, EdgeEdgeDistanceType); \ + template Eigen::Matrix edge_edge_distance_hessian( \ + Eigen::ConstRef>, \ + Eigen::ConstRef>, \ + Eigen::ConstRef>, \ + Eigen::ConstRef>, EdgeEdgeDistanceType) + +// Autodiff scalars: value only. Gradients/Hessians are intentionally not +// supported for them. +#define IPC_INSTANTIATE_EDGE_EDGE_VALUE(T) \ + template T edge_edge_distance( \ + Eigen::ConstRef>, \ + Eigen::ConstRef>, \ + Eigen::ConstRef>, \ + Eigen::ConstRef>, EdgeEdgeDistanceType) + +IPC_INSTANTIATE_EDGE_EDGE(float); +IPC_INSTANTIATE_EDGE_EDGE(double); +IPC_INSTANTIATE_EDGE_EDGE_VALUE(ADGrad<9>); +IPC_INSTANTIATE_EDGE_EDGE_VALUE(ADHessian<9>); +IPC_INSTANTIATE_EDGE_EDGE_VALUE(ADGrad<12>); +IPC_INSTANTIATE_EDGE_EDGE_VALUE(ADHessian<12>); +IPC_INSTANTIATE_EDGE_EDGE_VALUE(ADGrad<13>); +IPC_INSTANTIATE_EDGE_EDGE_VALUE(ADHessian<13>); +#undef IPC_INSTANTIATE_EDGE_EDGE +#undef IPC_INSTANTIATE_EDGE_EDGE_VALUE + +} // namespace ipc::detail diff --git a/src/ipc/distance/edge_edge.hpp b/src/ipc/distance/edge_edge.hpp index 0f4f9bf40..d07e3b464 100644 --- a/src/ipc/distance/edge_edge.hpp +++ b/src/ipc/distance/edge_edge.hpp @@ -1,9 +1,62 @@ #pragma once #include +#include namespace ipc { +namespace detail { + /// @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. + /// @param eb0 The first vertex of the second edge. + /// @param eb1 The second vertex of the second edge. + /// @param dtype The point edge distance type to compute. + /// @return The distance between the two edges. + template + T edge_edge_distance( + Eigen::ConstRef> ea0, + Eigen::ConstRef> ea1, + Eigen::ConstRef> eb0, + Eigen::ConstRef> eb1, + EdgeEdgeDistanceType dtype = EdgeEdgeDistanceType::AUTO); + + /// @brief Compute the gradient of the distance between a two lines segments. + /// @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. + /// @param eb0 The first vertex of the second edge. + /// @param eb1 The second vertex of the second edge. + /// @param dtype The point edge distance type to compute. + /// @return The gradient of the distance wrt ea0, ea1, eb0, and eb1. + template + Eigen::Vector edge_edge_distance_gradient( + Eigen::ConstRef> ea0, + Eigen::ConstRef> ea1, + Eigen::ConstRef> eb0, + Eigen::ConstRef> eb1, + EdgeEdgeDistanceType dtype = EdgeEdgeDistanceType::AUTO); + + /// @brief Compute the hessian of the distance between a two lines segments. + /// @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. + /// @param eb0 The first vertex of the second edge. + /// @param eb1 The second vertex of the second edge. + /// @param dtype The point edge distance type to compute. + /// @return The hessian of the distance wrt ea0, ea1, eb0, and eb1. + template + Eigen::Matrix edge_edge_distance_hessian( + Eigen::ConstRef> ea0, + Eigen::ConstRef> ea1, + Eigen::ConstRef> eb0, + Eigen::ConstRef> eb1, + EdgeEdgeDistanceType dtype = EdgeEdgeDistanceType::AUTO); +} // namespace detail + +// --- EigenExpression wrappers --- + /// @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. @@ -12,12 +65,21 @@ namespace ipc { /// @param eb1 The second vertex of the second edge. /// @param dtype The point edge distance type to compute. /// @return The distance between the two edges. -double edge_edge_distance( - Eigen::ConstRef ea0, - Eigen::ConstRef ea1, - Eigen::ConstRef eb0, - Eigen::ConstRef eb1, - EdgeEdgeDistanceType dtype = EdgeEdgeDistanceType::AUTO); +template < + typename DerivedEA0, + typename DerivedEA1, + typename DerivedEB0, + typename DerivedEB1> +inline auto edge_edge_distance( + const Eigen::MatrixBase& ea0, + const Eigen::MatrixBase& ea1, + const Eigen::MatrixBase& eb0, + const Eigen::MatrixBase& eb1, + EdgeEdgeDistanceType dtype = EdgeEdgeDistanceType::AUTO) +{ + using T = typename DerivedEA0::Scalar; + return detail::edge_edge_distance(ea0, ea1, eb0, eb1, dtype); +} /// @brief Compute the gradient of the distance between a two lines segments. /// @note The distance is actually squared distance. @@ -27,12 +89,21 @@ double edge_edge_distance( /// @param eb1 The second vertex of the second edge. /// @param dtype The point edge distance type to compute. /// @return The gradient of the distance wrt ea0, ea1, eb0, and eb1. -Vector12d edge_edge_distance_gradient( - Eigen::ConstRef ea0, - Eigen::ConstRef ea1, - Eigen::ConstRef eb0, - Eigen::ConstRef eb1, - EdgeEdgeDistanceType dtype = EdgeEdgeDistanceType::AUTO); +template < + typename DerivedEA0, + typename DerivedEA1, + typename DerivedEB0, + typename DerivedEB1> +inline auto edge_edge_distance_gradient( + const Eigen::MatrixBase& ea0, + const Eigen::MatrixBase& ea1, + const Eigen::MatrixBase& eb0, + const Eigen::MatrixBase& eb1, + EdgeEdgeDistanceType dtype = EdgeEdgeDistanceType::AUTO) +{ + using T = typename DerivedEA0::Scalar; + return detail::edge_edge_distance_gradient(ea0, ea1, eb0, eb1, dtype); +} /// @brief Compute the hessian of the distance between a two lines segments. /// @note The distance is actually squared distance. @@ -42,11 +113,20 @@ Vector12d edge_edge_distance_gradient( /// @param eb1 The second vertex of the second edge. /// @param dtype The point edge distance type to compute. /// @return The hessian of the distance wrt ea0, ea1, eb0, and eb1. -Matrix12d edge_edge_distance_hessian( - Eigen::ConstRef ea0, - Eigen::ConstRef ea1, - Eigen::ConstRef eb0, - Eigen::ConstRef eb1, - EdgeEdgeDistanceType dtype = EdgeEdgeDistanceType::AUTO); +template < + typename DerivedEA0, + typename DerivedEA1, + typename DerivedEB0, + typename DerivedEB1> +inline auto edge_edge_distance_hessian( + const Eigen::MatrixBase& ea0, + const Eigen::MatrixBase& ea1, + const Eigen::MatrixBase& eb0, + const Eigen::MatrixBase& eb1, + EdgeEdgeDistanceType dtype = EdgeEdgeDistanceType::AUTO) +{ + using T = typename DerivedEA0::Scalar; + return detail::edge_edge_distance_hessian(ea0, ea1, eb0, eb1, dtype); +} } // namespace ipc diff --git a/src/ipc/distance/edge_edge_mollifier.cpp b/src/ipc/distance/edge_edge_mollifier.cpp index f3672af61..88c632ca8 100644 --- a/src/ipc/distance/edge_edge_mollifier.cpp +++ b/src/ipc/distance/edge_edge_mollifier.cpp @@ -3,239 +3,114 @@ #include namespace ipc { +namespace detail { -double edge_edge_cross_squarednorm( - Eigen::ConstRef ea0, - Eigen::ConstRef ea1, - Eigen::ConstRef eb0, - Eigen::ConstRef eb1) -{ - return (ea1 - ea0).cross(eb1 - eb0).squaredNorm(); -} - -Vector12d edge_edge_cross_squarednorm_gradient( - Eigen::ConstRef ea0, - Eigen::ConstRef ea1, - Eigen::ConstRef eb0, - Eigen::ConstRef eb1) -{ - Vector12d grad; - autogen::edge_edge_cross_squarednorm_gradient( - ea0[0], ea0[1], ea0[2], ea1[0], ea1[1], ea1[2], eb0[0], eb0[1], eb0[2], - eb1[0], eb1[1], eb1[2], grad.data()); - return grad; -} - -Matrix12d edge_edge_cross_squarednorm_hessian( - Eigen::ConstRef ea0, - Eigen::ConstRef ea1, - Eigen::ConstRef eb0, - Eigen::ConstRef eb1) -{ - Matrix12d hess; - autogen::edge_edge_cross_squarednorm_hessian( - ea0[0], ea0[1], ea0[2], ea1[0], ea1[1], ea1[2], eb0[0], eb0[1], eb0[2], - eb1[0], eb1[1], eb1[2], hess.data()); - return hess; -} - -double edge_edge_mollifier(const double x, const double eps_x) -{ - if (x < eps_x) { - const double x_div_eps_x = x / eps_x; - return (-x_div_eps_x + 2) * x_div_eps_x; - } else { - return 1; - } -} - -double edge_edge_mollifier_gradient(const double x, const double eps_x) -{ - if (x < eps_x) { - const double one_div_eps_x = 1 / eps_x; - return 2 * one_div_eps_x * fma(-one_div_eps_x, x, 1); - } else { - return 0; - } -} - -double edge_edge_mollifier_hessian(const double x, const double eps_x) -{ - if (x < eps_x) { - return -2 / (eps_x * eps_x); - } else { - return 0; - } -} - -double -edge_edge_mollifier_derivative_wrt_eps_x(const double x, const double eps_x) -{ - return x < eps_x ? (2 * x * (-eps_x + x) / (eps_x * eps_x * eps_x)) : 0.0; -} - -double edge_edge_mollifier_gradient_derivative_wrt_eps_x( - const double x, const double eps_x) -{ - return x < eps_x ? (2 * (-eps_x + 2 * x) / (eps_x * eps_x * eps_x)) : 0.0; -} - -double edge_edge_mollifier( - Eigen::ConstRef ea0, - Eigen::ConstRef ea1, - Eigen::ConstRef eb0, - Eigen::ConstRef eb1, - const double eps_x) -{ - const double ee_cross_norm_sqr = - edge_edge_cross_squarednorm(ea0, ea1, eb0, eb1); - if (ee_cross_norm_sqr < eps_x) { - return edge_edge_mollifier(ee_cross_norm_sqr, eps_x); - } else { - return 1; - } -} - -Vector12d edge_edge_mollifier_gradient( - Eigen::ConstRef ea0, - Eigen::ConstRef ea1, - Eigen::ConstRef eb0, - Eigen::ConstRef eb1, - const double eps_x) -{ - const double ee_cross_norm_sqr = - edge_edge_cross_squarednorm(ea0, ea1, eb0, eb1); - if (ee_cross_norm_sqr < eps_x) { - return edge_edge_mollifier_gradient(ee_cross_norm_sqr, eps_x) - * edge_edge_cross_squarednorm_gradient(ea0, ea1, eb0, eb1); - } else { - return Vector12d::Zero(); - } -} - -Matrix12d edge_edge_mollifier_hessian( - Eigen::ConstRef ea0, - Eigen::ConstRef ea1, - Eigen::ConstRef eb0, - Eigen::ConstRef eb1, - const double eps_x) -{ - const double ee_cross_norm_sqr = - edge_edge_cross_squarednorm(ea0, ea1, eb0, eb1); - if (ee_cross_norm_sqr < eps_x) { - const Vector12d grad = - edge_edge_cross_squarednorm_gradient(ea0, ea1, eb0, eb1); - - return (edge_edge_mollifier_gradient(ee_cross_norm_sqr, eps_x) - * edge_edge_cross_squarednorm_hessian(ea0, ea1, eb0, eb1)) - + ((edge_edge_mollifier_hessian(ee_cross_norm_sqr, eps_x) * grad) - * grad.transpose()); - } else { - return Matrix12d::Zero(); + template + Eigen::Vector edge_edge_mollifier_gradient_wrt_x( + Eigen::ConstRef> ea0_rest, + Eigen::ConstRef> ea1_rest, + Eigen::ConstRef> eb0_rest, + Eigen::ConstRef> eb1_rest, + Eigen::ConstRef> ea0, + Eigen::ConstRef> ea1, + Eigen::ConstRef> eb0, + Eigen::ConstRef> eb1) + { + const T eps_x = edge_edge_mollifier_threshold( + ea0_rest, ea1_rest, eb0_rest, eb1_rest); + const T ee_cross_norm_sqr = + edge_edge_cross_squarednorm(ea0, ea1, eb0, eb1); + if (ee_cross_norm_sqr < eps_x) { + // ∇ₓ m = ∂m/∂ε · ∇ₓε + // (m depends on rest positions only through eps_x, since the + // cross-squarednorm s is a function of POSITIONS only) + return edge_edge_mollifier_derivative_wrt_eps_x( + ee_cross_norm_sqr, eps_x) + * edge_edge_mollifier_threshold_gradient( + ea0_rest, ea1_rest, eb0_rest, eb1_rest); + } else { + return Eigen::Vector::Zero(); + } } -} -Vector12d edge_edge_mollifier_gradient_wrt_x( - Eigen::ConstRef ea0_rest, - Eigen::ConstRef ea1_rest, - Eigen::ConstRef eb0_rest, - Eigen::ConstRef eb1_rest, - Eigen::ConstRef ea0, - Eigen::ConstRef ea1, - Eigen::ConstRef eb0, - Eigen::ConstRef eb1) -{ - const double eps_x = - edge_edge_mollifier_threshold(ea0_rest, ea1_rest, eb0_rest, eb1_rest); - const double ee_cross_norm_sqr = - edge_edge_cross_squarednorm(ea0, ea1, eb0, eb1); - if (ee_cross_norm_sqr < eps_x) { - // ∇ₓ m = ∂m/∂ε · ∇ₓε - // (m depends on rest positions only through eps_x, since the - // cross-squarednorm s is a function of POSITIONS only) - return edge_edge_mollifier_derivative_wrt_eps_x( - ee_cross_norm_sqr, eps_x) - * edge_edge_mollifier_threshold_gradient( - ea0_rest, ea1_rest, eb0_rest, eb1_rest); - } else { - return Vector12d::Zero(); + template + Eigen::Matrix edge_edge_mollifier_gradient_jacobian_wrt_x( + Eigen::ConstRef> ea0_rest, + Eigen::ConstRef> ea1_rest, + Eigen::ConstRef> eb0_rest, + Eigen::ConstRef> eb1_rest, + Eigen::ConstRef> ea0, + Eigen::ConstRef> ea1, + Eigen::ConstRef> eb0, + Eigen::ConstRef> eb1) + { + const T eps_x = edge_edge_mollifier_threshold( + ea0_rest, ea1_rest, eb0_rest, eb1_rest); + const T ee_cross_norm_sqr = + edge_edge_cross_squarednorm(ea0, ea1, eb0, eb1); + if (ee_cross_norm_sqr < eps_x) { + // ∂²m/∂ε∂s (∇ₓε)(∇ᵤs(x+u))ᵀ + ∂m/∂s ∇ᵤ²s(x+u) + return edge_edge_mollifier_gradient_derivative_wrt_eps_x( + ee_cross_norm_sqr, eps_x) + * edge_edge_mollifier_threshold_gradient( + ea0_rest, ea1_rest, eb0_rest, eb1_rest) + * edge_edge_cross_squarednorm_gradient(ea0, ea1, eb0, eb1) + .transpose() + + ipc::edge_edge_mollifier_gradient(ee_cross_norm_sqr, eps_x) + * edge_edge_cross_squarednorm_hessian(ea0, ea1, eb0, eb1); + } else { + return Eigen::Matrix::Zero(); + } } -} -Matrix12d edge_edge_mollifier_gradient_jacobian_wrt_x( - Eigen::ConstRef ea0_rest, - Eigen::ConstRef ea1_rest, - Eigen::ConstRef eb0_rest, - Eigen::ConstRef eb1_rest, - Eigen::ConstRef ea0, - Eigen::ConstRef ea1, - Eigen::ConstRef eb0, - Eigen::ConstRef eb1) -{ - const double eps_x = - edge_edge_mollifier_threshold(ea0_rest, ea1_rest, eb0_rest, eb1_rest); - const double ee_cross_norm_sqr = - edge_edge_cross_squarednorm(ea0, ea1, eb0, eb1); - if (ee_cross_norm_sqr < eps_x) { - // ∂²m/∂ε∂s (∇ₓε)(∇ᵤs(x+u))ᵀ + ∂m/∂s ∇ᵤ²s(x+u) - return edge_edge_mollifier_gradient_derivative_wrt_eps_x( - ee_cross_norm_sqr, eps_x) - * edge_edge_mollifier_threshold_gradient( - ea0_rest, ea1_rest, eb0_rest, eb1_rest) - * edge_edge_cross_squarednorm_gradient(ea0, ea1, eb0, eb1) - .transpose() - + edge_edge_mollifier_gradient(ee_cross_norm_sqr, eps_x) - * edge_edge_cross_squarednorm_hessian(ea0, ea1, eb0, eb1); - } else { - return Matrix12d::Zero(); - } -} +#define IPC_INSTANTIATE_EDGE_EDGE_MOLLIFIER(T) \ + template Eigen::Vector edge_edge_mollifier_gradient_wrt_x( \ + Eigen::ConstRef>, \ + Eigen::ConstRef>, \ + Eigen::ConstRef>, \ + Eigen::ConstRef>, \ + Eigen::ConstRef>, \ + Eigen::ConstRef>, \ + Eigen::ConstRef>, \ + Eigen::ConstRef>); \ + template Eigen::Matrix \ + edge_edge_mollifier_gradient_jacobian_wrt_x( \ + Eigen::ConstRef>, \ + Eigen::ConstRef>, \ + Eigen::ConstRef>, \ + Eigen::ConstRef>, \ + Eigen::ConstRef>, \ + Eigen::ConstRef>, \ + Eigen::ConstRef>, \ + Eigen::ConstRef>) -double edge_edge_mollifier_threshold( - Eigen::ConstRef ea0_rest, - Eigen::ConstRef ea1_rest, - Eigen::ConstRef eb0_rest, - Eigen::ConstRef eb1_rest) -{ - return 1e-3 * (ea0_rest - ea1_rest).squaredNorm() - * (eb0_rest - eb1_rest).squaredNorm(); -} + IPC_INSTANTIATE_EDGE_EDGE_MOLLIFIER(float); + IPC_INSTANTIATE_EDGE_EDGE_MOLLIFIER(double); +#undef IPC_INSTANTIATE_EDGE_EDGE_MOLLIFIER -Vector12d edge_edge_mollifier_threshold_gradient( - Eigen::ConstRef ea0_rest, - Eigen::ConstRef ea1_rest, - Eigen::ConstRef eb0_rest, - Eigen::ConstRef eb1_rest) -{ - Vector12d grad; - autogen::edge_edge_mollifier_threshold_gradient( - ea0_rest[0], ea0_rest[1], ea0_rest[2], ea1_rest[0], ea1_rest[1], - ea1_rest[2], eb0_rest[0], eb0_rest[1], eb0_rest[2], eb1_rest[0], - eb1_rest[1], eb1_rest[2], grad.data(), /*scale=*/1e-3); - return grad; -} +} // namespace detail namespace autogen { - // This function was generated by the Symbolic Math Toolbox version 8.3. // 01-Nov-2019 16:54:23 + template void edge_edge_cross_squarednorm_gradient( - double v01, - double v02, - double v03, - double v11, - double v12, - double v13, - double v21, - double v22, - double v23, - double v31, - double v32, - double v33, - double g[12]) + T v01, + T v02, + T v03, + T v11, + T v12, + T v13, + T v21, + T v22, + T v23, + T v31, + T v32, + T v33, + T g[12]) { - double t8, t9, t10, t11, t12, t13, t23, t24, t25, t26, t27, t28, t29, - t30, t31, t32, t33; + T t8, t9, t10, t11, t12, t13, t23, t24, t25, t26, t27, t28, t29, t30, + t31, t32, t33; t8 = -v11 + v01; t9 = -v12 + v02; @@ -246,18 +121,18 @@ namespace autogen { t23 = t8 * t12 + -(t9 * t11); t24 = t8 * t13 + -(t10 * t11); t25 = t9 * t13 + -(t10 * t12); - t26 = t8 * t23 * 2.0; - t27 = t9 * t23 * 2.0; - t28 = t8 * t24 * 2.0; - t29 = t10 * t24 * 2.0; - t30 = t9 * t25 * 2.0; - t31 = t10 * t25 * 2.0; - t32 = t11 * t23 * 2.0; - t33 = t12 * t23 * 2.0; - t23 = t11 * t24 * 2.0; - t10 = t13 * t24 * 2.0; - t9 = t12 * t25 * 2.0; - t8 = t13 * t25 * 2.0; + t26 = t8 * t23 * T(2); + t27 = t9 * t23 * T(2); + t28 = t8 * t24 * T(2); + t29 = t10 * t24 * T(2); + t30 = t9 * t25 * T(2); + t31 = t10 * t25 * T(2); + t32 = t11 * t23 * T(2); + t33 = t12 * t23 * T(2); + t23 = t11 * t24 * T(2); + t10 = t13 * t24 * T(2); + t9 = t12 * t25 * T(2); + t8 = t13 * t25 * T(2); g[0] = t33 + t10; g[1] = -t32 + t8; g[2] = -t23 - t9; @@ -274,25 +149,26 @@ namespace autogen { // This function was generated by the Symbolic Math Toolbox version 8.3. // 01-Nov-2019 16:54:23 + template void edge_edge_cross_squarednorm_hessian( - double v01, - double v02, - double v03, - double v11, - double v12, - double v13, - double v21, - double v22, - double v23, - double v31, - double v32, - double v33, - double H[144]) + T v01, + T v02, + T v03, + T v11, + T v12, + T v13, + T v21, + T v22, + T v23, + T v31, + T v32, + T v33, + T H[144]) { - double t8, t9, t10, t11, t12, t13, t32, t33, t34, t35, t48, t36, t49, - t37, t38, t39, t40, t41, t42, t43, t44, t45, t46, t47, t50, t51, - t52, t20, t23, t24, t25, t86, t87, t88, t74, t75, t76, t77, t78, - t79, t89, t90, t91, t92, t93, t94, t95; + T t8, t9, t10, t11, t12, t13, t32, t33, t34, t35, t48, t36, t49, t37, + t38, t39, t40, t41, t42, t43, t44, t45, t46, t47, t50, t51, t52, + t20, t23, t24, t25, t86, t87, t88, t74, t75, t76, t77, t78, t79, + t89, t90, t91, t92, t93, t94, t95; t8 = -v11 + v01; t9 = -v12 + v02; @@ -300,39 +176,39 @@ namespace autogen { t11 = -v31 + v21; t12 = -v32 + v22; t13 = -v33 + v23; - t32 = t8 * t9 * 2.0; - t33 = t8 * t10 * 2.0; - t34 = t9 * t10 * 2.0; - t35 = t8 * t11 * 2.0; + t32 = t8 * t9 * T(2); + t33 = t8 * t10 * T(2); + t34 = t9 * t10 * T(2); + t35 = t8 * t11 * T(2); t48 = t8 * t12; - t36 = t48 * 2.0; + t36 = t48 * T(2); t49 = t9 * t11; - t37 = t49 * 2.0; - t38 = t48 * 4.0; + t37 = t49 * T(2); + t38 = t48 * T(4); t48 = t8 * t13; - t39 = t48 * 2.0; - t40 = t49 * 4.0; - t41 = t9 * t12 * 2.0; + t39 = t48 * T(2); + t40 = t49 * T(4); + t41 = t9 * t12 * T(2); t49 = t10 * t11; - t42 = t49 * 2.0; - t43 = t48 * 4.0; + t42 = t49 * T(2); + t43 = t48 * T(4); t48 = t9 * t13; - t44 = t48 * 2.0; - t45 = t49 * 4.0; + t44 = t48 * T(2); + t45 = t49 * T(4); t49 = t10 * t12; - t46 = t49 * 2.0; - t47 = t48 * 4.0; - t48 = t49 * 4.0; - t49 = t10 * t13 * 2.0; - t50 = t11 * t12 * 2.0; - t51 = t11 * t13 * 2.0; - t52 = t12 * t13 * 2.0; - t20 = t8 * t8 * 2.0; - t9 = t9 * t9 * 2.0; - t8 = t10 * t10 * 2.0; - t23 = t11 * t11 * 2.0; - t24 = t12 * t12 * 2.0; - t25 = t13 * t13 * 2.0; + t46 = t49 * T(2); + t47 = t48 * T(4); + t48 = t49 * T(4); + t49 = t10 * t13 * T(2); + t50 = t11 * t12 * T(2); + t51 = t11 * t13 * T(2); + t52 = t12 * t13 * T(2); + t20 = t8 * t8 * T(2); + t9 = t9 * t9 * T(2); + t8 = t10 * t10 * T(2); + t23 = t11 * t11 * T(2); + t24 = t12 * t12 * T(2); + t25 = t13 * t13 * T(2); t86 = t35 + t41; t87 = t35 + t49; t88 = t41 + t49; @@ -509,21 +385,22 @@ namespace autogen { H[143] = t74; } + template void edge_edge_mollifier_threshold_gradient( - double ea0x, - double ea0y, - double ea0z, - double ea1x, - double ea1y, - double ea1z, - double eb0x, - double eb0y, - double eb0z, - double eb1x, - double eb1y, - double eb1z, - double grad[12], - double scale) + T ea0x, + T ea0y, + T ea0z, + T ea1x, + T ea1y, + T ea1z, + T eb0x, + T eb0y, + T eb0z, + T eb1x, + T eb1y, + T eb1z, + T grad[12], + T scale) { const auto t0 = ea0x - ea1x; const auto t1 = eb0x - eb1x; @@ -554,5 +431,14 @@ namespace autogen { grad[11] = -t14; } + // clang-format off + template void edge_edge_cross_squarednorm_gradient(float, float, float, float, float, float, float, float, float, float, float, float, float[12]); + template void edge_edge_cross_squarednorm_gradient(double, double, double, double, double, double, double, double, double, double, double, double, double[12]); + template void edge_edge_cross_squarednorm_hessian(float, float, float, float, float, float, float, float, float, float, float, float, float[144]); + template void edge_edge_cross_squarednorm_hessian(double, double, double, double, double, double, double, double, double, double, double, double, double[144]); + template void edge_edge_mollifier_threshold_gradient(float, float, float, float, float, float, float, float, float, float, float, float, float[12], float); + template void edge_edge_mollifier_threshold_gradient(double, double, double, double, double, double, double, double, double, double, double, double, double[12], double); + // clang-format on + } // namespace autogen -} // namespace ipc +} // namespace ipc \ No newline at end of file diff --git a/src/ipc/distance/edge_edge_mollifier.hpp b/src/ipc/distance/edge_edge_mollifier.hpp index edf0d28c4..56e5f3cc0 100644 --- a/src/ipc/distance/edge_edge_mollifier.hpp +++ b/src/ipc/distance/edge_edge_mollifier.hpp @@ -4,73 +4,323 @@ namespace ipc { -/// @brief Compute the squared norm of the edge-edge cross product. -/// @param ea0 The first vertex of the first edge. -/// @param ea1 The second vertex of the first edge. -/// @param eb0 The first vertex of the second edge. -/// @param eb1 The second vertex of the second edge. -/// @return The squared norm of the edge-edge cross product. -double edge_edge_cross_squarednorm( - Eigen::ConstRef ea0, - Eigen::ConstRef ea1, - Eigen::ConstRef eb0, - Eigen::ConstRef eb1); - -/// @brief Compute the gradient of the squared norm of the edge cross product. -/// @param ea0 The first vertex of the first edge. -/// @param ea1 The second vertex of the first edge. -/// @param eb0 The first vertex of the second edge. -/// @param eb1 The second vertex of the second edge. -/// @return The gradient of the squared norm of the edge cross product wrt ea0, ea1, eb0, and eb1. -Vector12d edge_edge_cross_squarednorm_gradient( - Eigen::ConstRef ea0, - Eigen::ConstRef ea1, - Eigen::ConstRef eb0, - Eigen::ConstRef eb1); +// Symbolically generated derivatives +namespace autogen { + // clang-format off + template + void edge_edge_cross_squarednorm_gradient( + T v01, T v02, T v03, T v11, T v12, T v13, T v21, T v22, T v23, T v31, T v32, T v33, T g[12]); + template + void edge_edge_cross_squarednorm_hessian( + T v01, T v02, T v03, T v11, T v12, T v13, T v21, T v22, T v23, T v31, T v32, T v33, T H[144]); + template + void edge_edge_mollifier_threshold_gradient( + T ea0x, T ea0y, T ea0z, T ea1x, T ea1y, T ea1z, T eb0x, T eb0y, T eb0z, T eb1x, T eb1y, T eb1z, T grad[12], T scale = T(1e-3)); + // clang-format on +} // namespace autogen -/// @brief Compute the hessian of the squared norm of the edge cross product. -/// @param ea0 The first vertex of the first edge. -/// @param ea1 The second vertex of the first edge. -/// @param eb0 The first vertex of the second edge. -/// @param eb1 The second vertex of the second edge. -/// @return The hessian of the squared norm of the edge cross product wrt ea0, ea1, eb0, and eb1. -Matrix12d edge_edge_cross_squarednorm_hessian( - Eigen::ConstRef ea0, - Eigen::ConstRef ea1, - Eigen::ConstRef eb0, - Eigen::ConstRef eb1); +// --- Scalar mollifier functions ------------------------------------------ /// @brief Mollifier function for edge-edge distance. /// @param x Squared norm of the edge-edge cross product. /// @param eps_x Mollifier activation threshold. /// @return The mollifier coefficient to premultiply the edge-edge distance. -double edge_edge_mollifier(const double x, const double eps_x); +template inline T edge_edge_mollifier(const T x, const T eps_x) +{ + if (x < eps_x) { + const T x_div_eps_x = x / eps_x; + return (-x_div_eps_x + T(2)) * x_div_eps_x; + } else { + return T(1); + } +} /// @brief The gradient of the mollifier function for edge-edge distance. /// @param x Squared norm of the edge-edge cross product. /// @param eps_x Mollifier activation threshold. /// @return The gradient of the mollifier function for edge-edge distance wrt x. -double edge_edge_mollifier_gradient(const double x, const double eps_x); +template +inline T edge_edge_mollifier_gradient(const T x, const T eps_x) +{ + if (x < eps_x) { + const T one_div_eps_x = T(1) / eps_x; + return T(2) * one_div_eps_x * fma(-one_div_eps_x, x, T(1)); + } else { + return T(0); + } +} -/// @brief The derivative of the mollifier function for edge-edge distance wrt eps_x. +/// @brief The derivative of the mollifier function for edge-edge distance wrt +/// eps_x. /// @param x Squared norm of the edge-edge cross product. /// @param eps_x Mollifier activation threshold. -/// @return The derivative of the mollifier function for edge-edge distance wrt eps_x. -double -edge_edge_mollifier_derivative_wrt_eps_x(const double x, const double eps_x); +/// @return The derivative of the mollifier function for edge-edge distance wrt +/// eps_x. +template +inline T edge_edge_mollifier_derivative_wrt_eps_x(const T x, const T eps_x) +{ + return x < eps_x ? (T(2) * x * (-eps_x + x) / (eps_x * eps_x * eps_x)) + : T(0); +} /// @brief The hessian of the mollifier function for edge-edge distance. /// @param x Squared norm of the edge-edge cross product. /// @param eps_x Mollifier activation threshold. /// @return The hessian of the mollifier function for edge-edge distance wrt x. -double edge_edge_mollifier_hessian(const double x, const double eps_x); +template +inline T edge_edge_mollifier_hessian(const T x, const T eps_x) +{ + if (x < eps_x) { + return T(-2) / (eps_x * eps_x); + } else { + return T(0); + } +} -/// @brief The derivative of the gradient of the mollifier function for edge-edge distance wrt eps_x. +/// @brief The derivative of the gradient of the mollifier function for +/// edge-edge distance wrt eps_x. /// @param x Squared norm of the edge-edge cross product. /// @param eps_x Mollifier activation threshold. -/// @return The derivative of the gradient of the mollifier function for edge-edge distance wrt eps_x. -double edge_edge_mollifier_gradient_derivative_wrt_eps_x( - const double x, const double eps_x); +/// @return The derivative of the gradient of the mollifier function for +/// edge-edge distance wrt eps_x. +template +inline T +edge_edge_mollifier_gradient_derivative_wrt_eps_x(const T x, const T eps_x) +{ + return x < eps_x ? (T(2) * (-eps_x + T(2) * x) / (eps_x * eps_x * eps_x)) + : T(0); +} + +// --- Fixed-size kernels --------------------------------------------------- + +namespace detail { + /// @note Prefer the ipc::edge_edge_cross_squarednorm front end. + template + inline T edge_edge_cross_squarednorm( + Eigen::ConstRef> ea0, + Eigen::ConstRef> ea1, + Eigen::ConstRef> eb0, + Eigen::ConstRef> eb1) + { + return (ea1 - ea0).cross(eb1 - eb0).squaredNorm(); + } + + /// @note Prefer the ipc::edge_edge_cross_squarednorm_gradient front end. + template + inline Eigen::Vector edge_edge_cross_squarednorm_gradient( + Eigen::ConstRef> ea0, + Eigen::ConstRef> ea1, + Eigen::ConstRef> eb0, + Eigen::ConstRef> eb1) + { + Eigen::Vector grad; + autogen::edge_edge_cross_squarednorm_gradient( + ea0[0], ea0[1], ea0[2], ea1[0], ea1[1], ea1[2], eb0[0], eb0[1], + eb0[2], eb1[0], eb1[1], eb1[2], grad.data()); + return grad; + } + + /// @note Prefer the ipc::edge_edge_cross_squarednorm_hessian front end. + template + inline Eigen::Matrix edge_edge_cross_squarednorm_hessian( + Eigen::ConstRef> ea0, + Eigen::ConstRef> ea1, + Eigen::ConstRef> eb0, + Eigen::ConstRef> eb1) + { + Eigen::Matrix hess; + autogen::edge_edge_cross_squarednorm_hessian( + ea0[0], ea0[1], ea0[2], ea1[0], ea1[1], ea1[2], eb0[0], eb0[1], + eb0[2], eb1[0], eb1[1], eb1[2], hess.data()); + return hess; + } + + /// @note Prefer the ipc::edge_edge_mollifier front end. + template + inline T edge_edge_mollifier( + Eigen::ConstRef> ea0, + Eigen::ConstRef> ea1, + Eigen::ConstRef> eb0, + Eigen::ConstRef> eb1, + const T eps_x) + { + const T ee_cross_norm_sqr = + edge_edge_cross_squarednorm(ea0, ea1, eb0, eb1); + if (ee_cross_norm_sqr < eps_x) { + // NOTE: ipc:: qualification: the scalar overload is hidden by the + // same-named kernel in this namespace. + return ipc::edge_edge_mollifier(ee_cross_norm_sqr, eps_x); + } else { + return T(1); + } + } + + /// @note Prefer the ipc::edge_edge_mollifier_gradient front end. + template + inline Eigen::Vector edge_edge_mollifier_gradient( + Eigen::ConstRef> ea0, + Eigen::ConstRef> ea1, + Eigen::ConstRef> eb0, + Eigen::ConstRef> eb1, + const T eps_x) + { + const T ee_cross_norm_sqr = + edge_edge_cross_squarednorm(ea0, ea1, eb0, eb1); + if (ee_cross_norm_sqr < eps_x) { + return ipc::edge_edge_mollifier_gradient(ee_cross_norm_sqr, eps_x) + * edge_edge_cross_squarednorm_gradient(ea0, ea1, eb0, eb1); + } else { + return Eigen::Vector::Zero(); + } + } + + /// @note Prefer the ipc::edge_edge_mollifier_hessian front end. + template + inline Eigen::Matrix edge_edge_mollifier_hessian( + Eigen::ConstRef> ea0, + Eigen::ConstRef> ea1, + Eigen::ConstRef> eb0, + Eigen::ConstRef> eb1, + const T eps_x) + { + const T ee_cross_norm_sqr = + edge_edge_cross_squarednorm(ea0, ea1, eb0, eb1); + if (ee_cross_norm_sqr < eps_x) { + const Eigen::Vector grad = + edge_edge_cross_squarednorm_gradient(ea0, ea1, eb0, eb1); + + return (ipc::edge_edge_mollifier_gradient(ee_cross_norm_sqr, eps_x) + * edge_edge_cross_squarednorm_hessian(ea0, ea1, eb0, eb1)) + + ((ipc::edge_edge_mollifier_hessian(ee_cross_norm_sqr, eps_x) + * grad) + * grad.transpose()); + } else { + return Eigen::Matrix::Zero(); + } + } + + /// @note Prefer the ipc::edge_edge_mollifier_gradient_wrt_x front end. + template + Eigen::Vector edge_edge_mollifier_gradient_wrt_x( + Eigen::ConstRef> ea0_rest, + Eigen::ConstRef> ea1_rest, + Eigen::ConstRef> eb0_rest, + Eigen::ConstRef> eb1_rest, + Eigen::ConstRef> ea0, + Eigen::ConstRef> ea1, + Eigen::ConstRef> eb0, + Eigen::ConstRef> eb1); + + /// @note Prefer the ipc::edge_edge_mollifier_gradient_jacobian_wrt_x front + /// end. + template + Eigen::Matrix edge_edge_mollifier_gradient_jacobian_wrt_x( + Eigen::ConstRef> ea0_rest, + Eigen::ConstRef> ea1_rest, + Eigen::ConstRef> eb0_rest, + Eigen::ConstRef> eb1_rest, + Eigen::ConstRef> ea0, + Eigen::ConstRef> ea1, + Eigen::ConstRef> eb0, + Eigen::ConstRef> eb1); + + /// @note Prefer the ipc::edge_edge_mollifier_threshold front end. + template + T edge_edge_mollifier_threshold( + Eigen::ConstRef> ea0_rest, + Eigen::ConstRef> ea1_rest, + Eigen::ConstRef> eb0_rest, + Eigen::ConstRef> eb1_rest) + { + return T(1e-3) * (ea0_rest - ea1_rest).squaredNorm() + * (eb0_rest - eb1_rest).squaredNorm(); + } + + /// @note Prefer the ipc::edge_edge_mollifier_threshold_gradient front end. + template + Eigen::Vector edge_edge_mollifier_threshold_gradient( + Eigen::ConstRef> ea0_rest, + Eigen::ConstRef> ea1_rest, + Eigen::ConstRef> eb0_rest, + Eigen::ConstRef> eb1_rest) + { + Eigen::Vector grad; + autogen::edge_edge_mollifier_threshold_gradient( + ea0_rest[0], ea0_rest[1], ea0_rest[2], ea1_rest[0], ea1_rest[1], + ea1_rest[2], eb0_rest[0], eb0_rest[1], eb0_rest[2], eb1_rest[0], + eb1_rest[1], eb1_rest[2], grad.data(), /*scale=*/T(1e-3)); + return grad; + } +} // namespace detail + +// --- EigenExpression wrappers --------------------------------------------- + +/// @brief Compute the squared norm of the edge-edge cross product. +/// @param ea0 The first vertex of the first edge. +/// @param ea1 The second vertex of the first edge. +/// @param eb0 The first vertex of the second edge. +/// @param eb1 The second vertex of the second edge. +/// @return The squared norm of the edge-edge cross product. +template < + typename DerivedEA0, + typename DerivedEA1, + typename DerivedEB0, + typename DerivedEB1> +inline auto edge_edge_cross_squarednorm( + const Eigen::MatrixBase& ea0, + const Eigen::MatrixBase& ea1, + const Eigen::MatrixBase& eb0, + const Eigen::MatrixBase& eb1) +{ + using T = typename DerivedEA0::Scalar; + // NOTE: explicit : the detail overload cannot deduce T from an + // arbitrary Eigen expression. + return detail::edge_edge_cross_squarednorm(ea0, ea1, eb0, eb1); +} + +/// @brief Compute the gradient of the squared norm of the edge cross product. +/// @param ea0 The first vertex of the first edge. +/// @param ea1 The second vertex of the first edge. +/// @param eb0 The first vertex of the second edge. +/// @param eb1 The second vertex of the second edge. +/// @return The gradient of the squared norm of the edge cross product wrt ea0, +/// ea1, eb0, and eb1. +template < + typename DerivedEA0, + typename DerivedEA1, + typename DerivedEB0, + typename DerivedEB1> +inline auto edge_edge_cross_squarednorm_gradient( + const Eigen::MatrixBase& ea0, + const Eigen::MatrixBase& ea1, + const Eigen::MatrixBase& eb0, + const Eigen::MatrixBase& eb1) +{ + using T = typename DerivedEA0::Scalar; + return detail::edge_edge_cross_squarednorm_gradient(ea0, ea1, eb0, eb1); +} + +/// @brief Compute the hessian of the squared norm of the edge cross product. +/// @param ea0 The first vertex of the first edge. +/// @param ea1 The second vertex of the first edge. +/// @param eb0 The first vertex of the second edge. +/// @param eb1 The second vertex of the second edge. +/// @return The hessian of the squared norm of the edge cross product wrt ea0, +/// ea1, eb0, and eb1. +template < + typename DerivedEA0, + typename DerivedEA1, + typename DerivedEB0, + typename DerivedEB1> +inline auto edge_edge_cross_squarednorm_hessian( + const Eigen::MatrixBase& ea0, + const Eigen::MatrixBase& ea1, + const Eigen::MatrixBase& eb0, + const Eigen::MatrixBase& eb1) +{ + using T = typename DerivedEA0::Scalar; + return detail::edge_edge_cross_squarednorm_hessian(ea0, ea1, eb0, eb1); +} /// @brief Compute a mollifier for the edge-edge distance. /// @@ -82,12 +332,21 @@ double edge_edge_mollifier_gradient_derivative_wrt_eps_x( /// @param eb1 The second vertex of the second edge. /// @param eps_x Mollifier activation threshold. /// @return The mollifier coefficient to premultiply the edge-edge distance. -double edge_edge_mollifier( - Eigen::ConstRef ea0, - Eigen::ConstRef ea1, - Eigen::ConstRef eb0, - Eigen::ConstRef eb1, - const double eps_x); +template < + typename DerivedEA0, + typename DerivedEA1, + typename DerivedEB0, + typename DerivedEB1> +inline auto edge_edge_mollifier( + const Eigen::MatrixBase& ea0, + const Eigen::MatrixBase& ea1, + const Eigen::MatrixBase& eb0, + const Eigen::MatrixBase& eb1, + const typename DerivedEA0::Scalar eps_x) +{ + using T = typename DerivedEA0::Scalar; + return detail::edge_edge_mollifier(ea0, ea1, eb0, eb1, eps_x); +} /// @brief Compute the gradient of the mollifier for the edge-edge distance. /// @param ea0 The first vertex of the first edge. @@ -96,12 +355,21 @@ double edge_edge_mollifier( /// @param eb1 The second vertex of the second edge. /// @param eps_x Mollifier activation threshold. /// @return The gradient of the mollifier. -Vector12d edge_edge_mollifier_gradient( - Eigen::ConstRef ea0, - Eigen::ConstRef ea1, - Eigen::ConstRef eb0, - Eigen::ConstRef eb1, - const double eps_x); +template < + typename DerivedEA0, + typename DerivedEA1, + typename DerivedEB0, + typename DerivedEB1> +inline auto edge_edge_mollifier_gradient( + const Eigen::MatrixBase& ea0, + const Eigen::MatrixBase& ea1, + const Eigen::MatrixBase& eb0, + const Eigen::MatrixBase& eb1, + const typename DerivedEA0::Scalar eps_x) +{ + using T = typename DerivedEA0::Scalar; + return detail::edge_edge_mollifier_gradient(ea0, ea1, eb0, eb1, eps_x); +} /// @brief Compute the hessian of the mollifier for the edge-edge distance. /// @param ea0 The first vertex of the first edge. @@ -110,14 +378,24 @@ Vector12d edge_edge_mollifier_gradient( /// @param eb1 The second vertex of the second edge. /// @param eps_x Mollifier activation threshold. /// @return The hessian of the mollifier. -Matrix12d edge_edge_mollifier_hessian( - Eigen::ConstRef ea0, - Eigen::ConstRef ea1, - Eigen::ConstRef eb0, - Eigen::ConstRef eb1, - const double eps_x); - -/// @brief Compute the gradient of the mollifier for the edge-edge distance wrt rest positions. +template < + typename DerivedEA0, + typename DerivedEA1, + typename DerivedEB0, + typename DerivedEB1> +inline auto edge_edge_mollifier_hessian( + const Eigen::MatrixBase& ea0, + const Eigen::MatrixBase& ea1, + const Eigen::MatrixBase& eb0, + const Eigen::MatrixBase& eb1, + const typename DerivedEA0::Scalar eps_x) +{ + using T = typename DerivedEA0::Scalar; + return detail::edge_edge_mollifier_hessian(ea0, ea1, eb0, eb1, eps_x); +} + +/// @brief Compute the gradient of the mollifier for the edge-edge distance wrt +/// rest positions. /// @param ea0_rest The rest position of the first vertex of the first edge. /// @param ea1_rest The rest position of the second vertex of the first edge. /// @param eb0_rest The rest position of the first vertex of the second edge. @@ -127,18 +405,34 @@ Matrix12d edge_edge_mollifier_hessian( /// @param eb0 The first vertex of the second edge. /// @param eb1 The second vertex of the second edge. /// @return The derivative of the mollifier wrt rest positions. -Vector12d edge_edge_mollifier_gradient_wrt_x( - Eigen::ConstRef ea0_rest, - Eigen::ConstRef ea1_rest, - Eigen::ConstRef eb0_rest, - Eigen::ConstRef eb1_rest, - Eigen::ConstRef ea0, - Eigen::ConstRef ea1, - Eigen::ConstRef eb0, - Eigen::ConstRef eb1); - -/// @brief Compute the jacobian of the edge-edge distance mollifier's gradient wrt rest positions. -/// @note This is not the hessian of the mollifier wrt rest positions, but the jacobian wrt rest positions of the mollifier's gradient wrt positions. +template < + typename DerivedEA0Rest, + typename DerivedEA1Rest, + typename DerivedEB0Rest, + typename DerivedEB1Rest, + typename DerivedEA0, + typename DerivedEA1, + typename DerivedEB0, + typename DerivedEB1> +inline auto edge_edge_mollifier_gradient_wrt_x( + const Eigen::MatrixBase& ea0_rest, + const Eigen::MatrixBase& ea1_rest, + const Eigen::MatrixBase& eb0_rest, + const Eigen::MatrixBase& eb1_rest, + const Eigen::MatrixBase& ea0, + const Eigen::MatrixBase& ea1, + const Eigen::MatrixBase& eb0, + const Eigen::MatrixBase& eb1) +{ + using T = typename DerivedEA0Rest::Scalar; + return detail::edge_edge_mollifier_gradient_wrt_x( + ea0_rest, ea1_rest, eb0_rest, eb1_rest, ea0, ea1, eb0, eb1); +} + +/// @brief Compute the jacobian of the edge-edge distance mollifier's gradient +/// wrt rest positions. +/// @note This is not the hessian of the mollifier wrt rest positions, but the +/// jacobian wrt rest positions of the mollifier's gradient wrt positions. /// @param ea0_rest The rest position of the first vertex of the first edge. /// @param ea1_rest The rest position of the second vertex of the first edge. /// @param eb0_rest The rest position of the first vertex of the second edge. @@ -148,15 +442,29 @@ Vector12d edge_edge_mollifier_gradient_wrt_x( /// @param eb0 The first vertex of the second edge. /// @param eb1 The second vertex of the second edge. /// @return The jacobian of the mollifier's gradient wrt rest positions. -Matrix12d edge_edge_mollifier_gradient_jacobian_wrt_x( - Eigen::ConstRef ea0_rest, - Eigen::ConstRef ea1_rest, - Eigen::ConstRef eb0_rest, - Eigen::ConstRef eb1_rest, - Eigen::ConstRef ea0, - Eigen::ConstRef ea1, - Eigen::ConstRef eb0, - Eigen::ConstRef eb1); +template < + typename DerivedEA0Rest, + typename DerivedEA1Rest, + typename DerivedEB0Rest, + typename DerivedEB1Rest, + typename DerivedEA0, + typename DerivedEA1, + typename DerivedEB0, + typename DerivedEB1> +inline auto edge_edge_mollifier_gradient_jacobian_wrt_x( + const Eigen::MatrixBase& ea0_rest, + const Eigen::MatrixBase& ea1_rest, + const Eigen::MatrixBase& eb0_rest, + const Eigen::MatrixBase& eb1_rest, + const Eigen::MatrixBase& ea0, + const Eigen::MatrixBase& ea1, + const Eigen::MatrixBase& eb0, + const Eigen::MatrixBase& eb1) +{ + using T = typename DerivedEA0Rest::Scalar; + return detail::edge_edge_mollifier_gradient_jacobian_wrt_x( + ea0_rest, ea1_rest, eb0_rest, eb1_rest, ea0, ea1, eb0, eb1); +} /// @brief Compute the threshold of the mollifier edge-edge distance. /// @@ -167,13 +475,24 @@ Matrix12d edge_edge_mollifier_gradient_jacobian_wrt_x( /// @param eb0_rest The rest position of the first vertex of the second edge. /// @param eb1_rest The rest position of the second vertex of the second edge. /// @return Threshold for edge-edge mollification. -double edge_edge_mollifier_threshold( - Eigen::ConstRef ea0_rest, - Eigen::ConstRef ea1_rest, - Eigen::ConstRef eb0_rest, - Eigen::ConstRef eb1_rest); +template < + typename DerivedEA0Rest, + typename DerivedEA1Rest, + typename DerivedEB0Rest, + typename DerivedEB1Rest> +inline auto edge_edge_mollifier_threshold( + const Eigen::MatrixBase& ea0_rest, + const Eigen::MatrixBase& ea1_rest, + const Eigen::MatrixBase& eb0_rest, + const Eigen::MatrixBase& eb1_rest) +{ + using T = typename DerivedEA0Rest::Scalar; + return detail::edge_edge_mollifier_threshold( + ea0_rest, ea1_rest, eb0_rest, eb1_rest); +} -/// @brief Compute the gradient of the threshold of the mollifier edge-edge distance. +/// @brief Compute the gradient of the threshold of the mollifier edge-edge +/// distance. /// /// This values is computed based on the edges at rest length. /// @@ -182,58 +501,20 @@ double edge_edge_mollifier_threshold( /// @param eb0_rest The rest position of the first vertex of the second edge. /// @param eb1_rest The rest position of the second vertex of the second edge. /// @return Gradient of the threshold for edge-edge mollification. -Vector12d edge_edge_mollifier_threshold_gradient( - Eigen::ConstRef ea0_rest, - Eigen::ConstRef ea1_rest, - Eigen::ConstRef eb0_rest, - Eigen::ConstRef eb1_rest); +template < + typename DerivedEA0Rest, + typename DerivedEA1Rest, + typename DerivedEB0Rest, + typename DerivedEB1Rest> +inline auto edge_edge_mollifier_threshold_gradient( + const Eigen::MatrixBase& ea0_rest, + const Eigen::MatrixBase& ea1_rest, + const Eigen::MatrixBase& eb0_rest, + const Eigen::MatrixBase& eb1_rest) +{ + using T = typename DerivedEA0Rest::Scalar; + return detail::edge_edge_mollifier_threshold_gradient( + ea0_rest, ea1_rest, eb0_rest, eb1_rest); +} -// Symbolically generated derivatives; -namespace autogen { - void edge_edge_cross_squarednorm_gradient( - double v01, - double v02, - double v03, - double v11, - double v12, - double v13, - double v21, - double v22, - double v23, - double v31, - double v32, - double v33, - double g[12]); - - void edge_edge_cross_squarednorm_hessian( - double v01, - double v02, - double v03, - double v11, - double v12, - double v13, - double v21, - double v22, - double v23, - double v31, - double v32, - double v33, - double H[144]); - - void edge_edge_mollifier_threshold_gradient( - double ea0x, - double ea0y, - double ea0z, - double ea1x, - double ea1y, - double ea1z, - double eb0x, - double eb0y, - double eb0z, - double eb1x, - double eb1y, - double eb1z, - double grad[12], - double scale = 1e-3); -} // namespace autogen } // namespace ipc diff --git a/src/ipc/distance/line_line.cpp b/src/ipc/distance/line_line.cpp index ec2bbf8a1..4af226bb4 100644 --- a/src/ipc/distance/line_line.cpp +++ b/src/ipc/distance/line_line.cpp @@ -1,610 +1,571 @@ #include "line_line.hpp" -#include +#include -namespace ipc { +namespace ipc::autogen { -double line_line_distance( - Eigen::ConstRef ea0, - Eigen::ConstRef ea1, - Eigen::ConstRef eb0, - Eigen::ConstRef eb1) +// This function was generated by the Symbolic Math Toolbox version 8.3. +// 14-Jun-2019 13:58:25 +template +void line_line_distance_gradient( + T v01, + T v02, + T v03, + T v11, + T v12, + T v13, + T v21, + T v22, + T v23, + T v31, + T v32, + T v33, + T g[12]) { - const Eigen::Vector3d normal = (ea1 - ea0).cross(eb1 - eb0); - const double line_to_line = (eb0 - ea0).dot(normal); - return line_to_line * line_to_line / normal.squaredNorm(); -} + T t11, t12, t13, t14, t15, t16, t17, t18, t19, t32, t33, t34, t35, t36, t37, + t44, t45, t46, t75, t77, t76, t78, t79, t80, t81, t83; -Vector12d line_line_distance_gradient( - Eigen::ConstRef ea0, - Eigen::ConstRef ea1, - Eigen::ConstRef eb0, - Eigen::ConstRef eb1) -{ - Vector12d grad; - autogen::line_line_distance_gradient( - ea0[0], ea0[1], ea0[2], ea1[0], ea1[1], ea1[2], eb0[0], eb0[1], eb0[2], - eb1[0], eb1[1], eb1[2], grad.data()); - return grad; + t11 = -v11 + v01; + t12 = -v12 + v02; + t13 = -v13 + v03; + t14 = -v21 + v01; + t15 = -v22 + v02; + t16 = -v23 + v03; + t17 = -v31 + v21; + t18 = -v32 + v22; + t19 = -v33 + v23; + t32 = t14 * t18; + t33 = t15 * t17; + t34 = t14 * t19; + t35 = t16 * t17; + t36 = t15 * t19; + t37 = t16 * t18; + t44 = t11 * t18 + -(t12 * t17); + t45 = t11 * t19 + -(t13 * t17); + t46 = t12 * t19 + -(t13 * t18); + t75 = T(1) / ((t44 * t44 + t45 * t45) + t46 * t46); + t77 = (t16 * t44 + t14 * t46) + -(t15 * t45); + t76 = t75 * t75; + t78 = t77 * t77; + t79 = (t12 * t44 * T(2) + t13 * t45 * T(2)) * t76 * t78; + t80 = (t11 * t45 * T(2) + t12 * t46 * T(2)) * t76 * t78; + t81 = (t18 * t44 * T(2) + t19 * t45 * T(2)) * t76 * t78; + t18 = (t17 * t45 * T(2) + t18 * t46 * T(2)) * t76 * t78; + t83 = (t11 * t44 * T(2) + -(t13 * t46 * T(2))) * t76 * t78; + t19 = (t17 * t44 * T(2) + -(t19 * t46 * T(2))) * t76 * t78; + t76 = t75 * t77; + g[0] = -t81 + t76 * ((-t36 + t37) + t46) * T(2); + g[1] = t19 - t76 * ((-t34 + t35) + t45) * T(2); + g[2] = t18 + t76 * ((-t32 + t33) + t44) * T(2); + g[3] = t81 + t76 * (t36 - t37) * T(2); + g[4] = -t19 - t76 * (t34 - t35) * T(2); + g[5] = -t18 + t76 * (t32 - t33) * T(2); + t17 = t12 * t16 + -(t13 * t15); + g[6] = t79 - t76 * (t17 + t46) * T(2); + t18 = t11 * t16 + -(t13 * t14); + g[7] = -t83 + t76 * (t18 + t45) * T(2); + t19 = t11 * t15 + -(t12 * t14); + g[8] = -t80 - t76 * (t19 + t44) * T(2); + g[9] = -t79 + t76 * t17 * T(2); + g[10] = t83 - t76 * t18 * T(2); + g[11] = t80 + t76 * t19 * T(2); } -Matrix12d line_line_distance_hessian( - Eigen::ConstRef ea0, - Eigen::ConstRef ea1, - Eigen::ConstRef eb0, - Eigen::ConstRef eb1) +// This function was generated by the Symbolic Math Toolbox version 8.3. +// 14-Jun-2019 13:58:38 +template +void line_line_distance_hessian( + T v01, + T v02, + T v03, + T v11, + T v12, + T v13, + T v21, + T v22, + T v23, + T v31, + T v32, + T v33, + T H[144]) { - Matrix12d hess; - autogen::line_line_distance_hessian( - ea0[0], ea0[1], ea0[2], ea1[0], ea1[1], ea1[2], eb0[0], eb0[1], eb0[2], - eb1[0], eb1[1], eb1[2], hess.data()); - return hess; -} + T t11, t12, t13, t14, t15, t16, t26, t27, t28, t47, t48, t49, t50, t51, t52, + t53, t54, t55, t56, t57, t58, t59, t65, t73, t35, t36, t37, t38, t39, + t40, t98, t99, t100, t101, t103, t105, t107, t108, t109, t137, t138, + t139, t140, t141, t142, t143, t144, t145, t146, t147, t148, t156, t159, + t157, t262, t263, t264, t265, t266, t267, t268, t269, t270, t271, t272, + t273, t274, t275, t276, t277, t278, t279, t298, t299, t300, t301, t302, + t303, t310, t311, t312, t313, t314, t315, t322, t323, t325, t326, t327, + t328, t329, t330, t335, t337, t339, t340, t341, t342, t343, t345, t348, + t353, t356, t358, t359, t360, t362, t367, t368, t369, t371, t374, t377, + t382, t386, t387, t398, t399, t403, t408, t423, t424, t427, t428, t431, + t432, t433, t434, t437, t438, t441, t442, t446, t451, t455, t456, t467, + t468, t472, t477, t491, t492, t495, t497, t499, t500, t503, t504, t506, + t508, t550, t568, t519_tmp, b_t519_tmp, t519, t520_tmp, b_t520_tmp, + t520, t521_tmp, b_t521_tmp, t521, t522_tmp, b_t522_tmp, t522, t523_tmp, + b_t523_tmp, t523, t524_tmp, b_t524_tmp, t524, t525, t526, t527, t528, + t529, t530, t531, t532, t533, t534, t535, t536, t537, t538, t539, t540, + t542, t543, t544; -namespace autogen { - - // This function was generated by the Symbolic Math Toolbox version 8.3. - // 14-Jun-2019 13:58:25 - void line_line_distance_gradient( - double v01, - double v02, - double v03, - double v11, - double v12, - double v13, - double v21, - double v22, - double v23, - double v31, - double v32, - double v33, - double g[12]) - { - double t11, t12, t13, t14, t15, t16, t17, t18, t19, t32, t33, t34, t35, - t36, t37, t44, t45, t46, t75, t77, t76, t78, t79, t80, t81, t83; - - t11 = -v11 + v01; - t12 = -v12 + v02; - t13 = -v13 + v03; - t14 = -v21 + v01; - t15 = -v22 + v02; - t16 = -v23 + v03; - t17 = -v31 + v21; - t18 = -v32 + v22; - t19 = -v33 + v23; - t32 = t14 * t18; - t33 = t15 * t17; - t34 = t14 * t19; - t35 = t16 * t17; - t36 = t15 * t19; - t37 = t16 * t18; - t44 = t11 * t18 + -(t12 * t17); - t45 = t11 * t19 + -(t13 * t17); - t46 = t12 * t19 + -(t13 * t18); - t75 = 1.0 / ((t44 * t44 + t45 * t45) + t46 * t46); - t77 = (t16 * t44 + t14 * t46) + -(t15 * t45); - t76 = t75 * t75; - t78 = t77 * t77; - t79 = (t12 * t44 * 2.0 + t13 * t45 * 2.0) * t76 * t78; - t80 = (t11 * t45 * 2.0 + t12 * t46 * 2.0) * t76 * t78; - t81 = (t18 * t44 * 2.0 + t19 * t45 * 2.0) * t76 * t78; - t18 = (t17 * t45 * 2.0 + t18 * t46 * 2.0) * t76 * t78; - t83 = (t11 * t44 * 2.0 + -(t13 * t46 * 2.0)) * t76 * t78; - t19 = (t17 * t44 * 2.0 + -(t19 * t46 * 2.0)) * t76 * t78; - t76 = t75 * t77; - g[0] = -t81 + t76 * ((-t36 + t37) + t46) * 2.0; - g[1] = t19 - t76 * ((-t34 + t35) + t45) * 2.0; - g[2] = t18 + t76 * ((-t32 + t33) + t44) * 2.0; - g[3] = t81 + t76 * (t36 - t37) * 2.0; - g[4] = -t19 - t76 * (t34 - t35) * 2.0; - g[5] = -t18 + t76 * (t32 - t33) * 2.0; - t17 = t12 * t16 + -(t13 * t15); - g[6] = t79 - t76 * (t17 + t46) * 2.0; - t18 = t11 * t16 + -(t13 * t14); - g[7] = -t83 + t76 * (t18 + t45) * 2.0; - t19 = t11 * t15 + -(t12 * t14); - g[8] = -t80 - t76 * (t19 + t44) * 2.0; - g[9] = -t79 + t76 * t17 * 2.0; - g[10] = t83 - t76 * t18 * 2.0; - g[11] = t80 + t76 * t19 * 2.0; - } + t11 = -v11 + v01; + t12 = -v12 + v02; + t13 = -v13 + v03; + t14 = -v21 + v01; + t15 = -v22 + v02; + t16 = -v23 + v03; + t26 = -v31 + v21; + t27 = -v32 + v22; + t28 = -v33 + v23; + t47 = t11 * t27; + t48 = t12 * t26; + t49 = t11 * t28; + t50 = t13 * t26; + t51 = t12 * t28; + t52 = t13 * t27; + t53 = t14 * t27; + t54 = t15 * t26; + t55 = t14 * t28; + t56 = t16 * t26; + t57 = t15 * t28; + t58 = t16 * t27; + t59 = t11 * t26 * T(2); + t65 = t12 * t27 * T(2); + t73 = t13 * t28 * T(2); + t35 = t11 * t11 * T(2); + t36 = t12 * t12 * T(2); + t37 = t13 * t13 * T(2); + t38 = t26 * t26 * T(2); + t39 = t27 * t27 * T(2); + t40 = t28 * t28 * T(2); + t98 = t11 * t15 + -(t12 * t14); + t99 = t11 * t16 + -(t13 * t14); + t100 = t12 * t16 + -(t13 * t15); + t101 = t47 + -t48; + t103 = t49 + -t50; + t105 = t51 + -t52; + t107 = t53 + -t54; + t108 = t55 + -t56; + t109 = t57 + -t58; + t137 = t98 + t101; + t138 = t99 + t103; + t139 = t100 + t105; + t140 = (t54 + -t53) + t101; + t141 = (t56 + -t55) + t103; + t142 = (t58 + -t57) + t105; + t143 = t12 * t101 * T(2) + t13 * t103 * T(2); + t144 = t11 * t103 * T(2) + t12 * t105 * T(2); + t145 = t27 * t101 * T(2) + t28 * t103 * T(2); + t146 = t26 * t103 * T(2) + t27 * t105 * T(2); + t147 = t11 * t101 * T(2) + -(t13 * t105 * T(2)); + t148 = t26 * t101 * T(2) + -(t28 * t105 * T(2)); + t156 = T(1) / ((t101 * t101 + t103 * t103) + t105 * t105); + t159 = (t16 * t101 + t14 * t105) + -(t15 * t103); + t157 = t156 * t156; + t57 = t156 * t156 * t156; + t58 = t159 * t159; + t262 = t11 * t156 * t159 * T(2); + t263 = t12 * t156 * t159 * T(2); + t264 = t13 * t156 * t159 * T(2); + t265 = t14 * t156 * t159 * T(2); + t266 = t15 * t156 * t159 * T(2); + t267 = t16 * t156 * t159 * T(2); + t268 = (-v31 + v01) * t156 * t159 * T(2); + t269 = (-v21 + v11) * t156 * t159 * T(2); + t270 = (-v32 + v02) * t156 * t159 * T(2); + t271 = (-v22 + v12) * t156 * t159 * T(2); + t272 = (-v33 + v03) * t156 * t159 * T(2); + t273 = (-v23 + v13) * t156 * t159 * T(2); + t274 = (-v31 + v11) * t156 * t159 * T(2); + t275 = (-v32 + v12) * t156 * t159 * T(2); + t276 = (-v33 + v13) * t156 * t159 * T(2); + t277 = t26 * t156 * t159 * T(2); + t278 = t27 * t156 * t159 * T(2); + t279 = t28 * t156 * t159 * T(2); + t298 = t11 * t12 * t157 * t58 * T(2); + t299 = t11 * t13 * t157 * t58 * T(2); + t300 = t12 * t13 * t157 * t58 * T(2); + t301 = t26 * t27 * t157 * t58 * T(2); + t302 = t26 * t28 * t157 * t58 * T(2); + t303 = t27 * t28 * t157 * t58 * T(2); + t310 = (t35 + t36) * t157 * t58; + t311 = (t35 + t37) * t157 * t58; + t312 = (t36 + t37) * t157 * t58; + t313 = (t38 + t39) * t157 * t58; + t314 = (t38 + t40) * t157 * t58; + t315 = (t39 + t40) * t157 * t58; + t322 = (t59 + t65) * t157 * t58; + t323 = (t59 + t73) * t157 * t58; + t59 = (t65 + t73) * t157 * t58; + t325 = (t47 * T(2) + -(t48 * T(4))) * t157 * t58; + t53 = -t157 * t58; + t56 = t48 * T(2) - t47 * T(4); + t326 = t53 * t56; + t327 = (t49 * T(2) + -(t50 * T(4))) * t157 * t58; + t55 = t50 * T(2) - t49 * T(4); + t328 = t53 * t55; + t329 = (t51 * T(2) + -(t52 * T(4))) * t157 * t58; + t54 = t52 * T(2) - t51 * T(4); + t330 = t53 * t54; + t53 = t157 * t58; + t335 = t53 * t56; + t337 = t53 * t55; + t339 = t53 * t54; + t340 = t143 * t143 * t57 * t58 * T(2); + t341 = t144 * t144 * t57 * t58 * T(2); + t342 = t145 * t145 * t57 * t58 * T(2); + t343 = t146 * t146 * t57 * t58 * T(2); + t345 = t147 * t147 * t57 * t58 * T(2); + t348 = t148 * t148 * t57 * t58 * T(2); + t36 = t98 * t143 * t157 * t159 * T(2); + t353 = t99 * t143 * t157 * t159 * T(2); + t356 = t99 * t144 * t157 * t159 * T(2); + t65 = t100 * t144 * t157 * t159 * T(2); + t358 = t107 * t143 * t157 * t159 * T(2); + t359 = t98 * t145 * t157 * t159 * T(2); + t360 = t108 * t143 * t157 * t159 * T(2); + t54 = t107 * t144 * t157 * t159 * T(2); + t362 = t99 * t145 * t157 * t159 * T(2); + t53 = t98 * t146 * t157 * t159 * T(2); + t56 = t109 * t143 * t157 * t159 * T(2); + t27 = t108 * t144 * t157 * t159 * T(2); + t55 = t100 * t145 * t157 * t159 * T(2); + t367 = t99 * t146 * t157 * t159 * T(2); + t368 = t109 * t144 * t157 * t159 * T(2); + t369 = t100 * t146 * t157 * t159 * T(2); + t38 = t107 * t145 * t157 * t159 * T(2); + t371 = t108 * t145 * t157 * t159 * T(2); + t374 = t108 * t146 * t157 * t159 * T(2); + t28 = t109 * t146 * t157 * t159 * T(2); + t377 = t98 * t147 * t157 * t159 * T(2); + t382 = t100 * t147 * t157 * t159 * T(2); + t386 = t107 * t147 * t157 * t159 * T(2); + t387 = t98 * t148 * t157 * t159 * T(2); + t103 = t108 * t147 * t157 * t159 * T(2); + t101 = t99 * t148 * t157 * t159 * T(2); + t398 = t109 * t147 * t157 * t159 * T(2); + t399 = t100 * t148 * t157 * t159 * T(2); + t403 = t107 * t148 * t157 * t159 * T(2); + t408 = t109 * t148 * t157 * t159 * T(2); + t73 = t137 * t143 * t157 * t159 * T(2); + t423 = t138 * t143 * t157 * t159 * T(2); + t424 = t138 * t144 * t157 * t159 * T(2); + t37 = t139 * t144 * t157 * t159 * T(2); + t427 = t140 * t143 * t157 * t159 * T(2); + t428 = t137 * t145 * t157 * t159 * T(2); + t16 = t140 * t144 * t157 * t159 * T(2); + t11 = t137 * t146 * t157 * t159 * T(2); + t431 = t141 * t143 * t157 * t159 * T(2); + t432 = t138 * t145 * t157 * t159 * T(2); + t433 = t141 * t144 * t157 * t159 * T(2); + t434 = t138 * t146 * t157 * t159 * T(2); + t105 = t142 * t143 * t157 * t159 * T(2); + t14 = t139 * t145 * t157 * t159 * T(2); + t437 = t142 * t144 * t157 * t159 * T(2); + t438 = t139 * t146 * t157 * t159 * T(2); + t35 = t140 * t145 * t157 * t159 * T(2); + t441 = t141 * t145 * t157 * t159 * T(2); + t442 = t141 * t146 * t157 * t159 * T(2); + t39 = t142 * t146 * t157 * t159 * T(2); + t446 = t137 * t147 * t157 * t159 * T(2); + t451 = t139 * t147 * t157 * t159 * T(2); + t455 = t140 * t147 * t157 * t159 * T(2); + t456 = t137 * t148 * t157 * t159 * T(2); + t13 = t141 * t147 * t157 * t159 * T(2); + t26 = t138 * t148 * t157 * t159 * T(2); + t467 = t142 * t147 * t157 * t159 * T(2); + t468 = t139 * t148 * t157 * t159 * T(2); + t472 = t140 * t148 * t157 * t159 * T(2); + t477 = t142 * t148 * t157 * t159 * T(2); + t47 = t143 * t144 * t57 * t58 * T(2); + t15 = t143 * t145 * t57 * t58 * T(2); + t491 = t143 * t146 * t57 * t58 * T(2); + t492 = t144 * t145 * t57 * t58 * T(2); + t12 = t144 * t146 * t57 * t58 * T(2); + t40 = t145 * t146 * t57 * t58 * T(2); + t495 = t143 * t147 * t57 * t58 * T(2); + t497 = t144 * t147 * t57 * t58 * T(2); + t499 = t143 * t148 * t57 * t58 * T(2); + t500 = t145 * t147 * t57 * t58 * T(2); + t503 = t146 * t147 * t57 * t58 * T(2); + t504 = t144 * t148 * t57 * t58 * T(2); + t506 = t145 * t148 * t57 * t58 * T(2); + t508 = t146 * t148 * t57 * t58 * T(2); + t57 = t147 * t148 * t57 * t58 * T(2); + t550 = ((((t98 * t109 * t156 * T(2) + -t266) + t337) + t359) + t368) + t492; + t568 = ((((t108 * t137 * t156 * T(2) + -t268) + t330) + t27) + t456) + t504; + t519_tmp = t139 * t143 * t157 * t159; + b_t519_tmp = t100 * t143 * t157 * t159; + t519 = (((-(t100 * t139 * t156 * T(2)) + t312) + -t340) + b_t519_tmp * T(2)) + + t519_tmp * T(2); + t520_tmp = t140 * t146 * t157 * t159; + b_t520_tmp = t107 * t146 * t157 * t159; + t520 = (((t107 * t140 * t156 * T(2) + t313) + -t343) + b_t520_tmp * T(2)) + + -(t520_tmp * T(2)); + t521_tmp = t142 * t145 * t157 * t159; + b_t521_tmp = t109 * t145 * t157 * t159; + t521 = (((t109 * t142 * t156 * T(2) + t315) + -t342) + -(b_t521_tmp * T(2))) + + t521_tmp * T(2); + t522_tmp = t137 * t144 * t157 * t159; + b_t522_tmp = t98 * t144 * t157 * t159; + t522 = + (((-(t98 * t137 * t156 * T(2)) + t310) + -t341) + -(b_t522_tmp * T(2))) + + -(t522_tmp * T(2)); + t523_tmp = t138 * t147 * t157 * t159; + b_t523_tmp = t99 * t147 * t157 * t159; + t523 = (((-(t99 * t138 * t156 * T(2)) + t311) + -t345) + b_t523_tmp * T(2)) + + t523_tmp * T(2); + t524_tmp = t141 * t148 * t157 * t159; + b_t524_tmp = t108 * t148 * t157 * t159; + t524 = (((t108 * t141 * t156 * T(2) + t314) + -t348) + -(b_t524_tmp * T(2))) + + t524_tmp * T(2); + t525 = (((t98 * t100 * t156 * T(2) + t299) + t65) + -t36) + -t47; + t526 = (((t107 * t109 * t156 * T(2) + t302) + t38) + -t28) + -t40; + t527 = (((-(t98 * t99 * t156 * T(2)) + t300) + t377) + -t356) + t497; + t528 = (((-(t99 * t100 * t156 * T(2)) + t298) + t353) + t382) + -t495; + t529 = (((-(t107 * t108 * t156 * T(2)) + t303) + t374) + -t403) + t508; + t530 = (((-(t108 * t109 * t156 * T(2)) + t301) + -t371) + -t408) + -t506; + t531 = (((t98 * t107 * t156 * T(2) + t322) + t54) + -t53) + -t12; + t532 = (((t100 * t109 * t156 * T(2) + t59) + t55) + -t56) + -t15; + t533 = (((t99 * t108 * t156 * T(2) + t323) + t101) + -t103) + -t57; + t534 = (((t98 * t140 * t156 * T(2) + -t322) + t53) + t16) + t12; + t535 = (((-(t107 * t137 * t156 * T(2)) + -t322) + -t54) + t11) + t12; + t536 = (((t100 * t142 * t156 * T(2) + -t59) + -t55) + -t105) + t15; + t537 = (((-(t109 * t139 * t156 * T(2)) + -t59) + t56) + -t14) + t15; + t538 = (((t99 * t141 * t156 * T(2) + -t323) + -t101) + -t13) + t57; + t539 = (((-(t108 * t138 * t156 * T(2)) + -t323) + t103) + -t26) + t57; + t540 = (((t137 * t139 * t156 * T(2) + t299) + t37) + -t73) + -t47; + t148 = (((t140 * t142 * t156 * T(2) + t302) + t39) + -t35) + -t40; + t542 = (((-(t137 * t138 * t156 * T(2)) + t300) + t446) + -t424) + t497; + t543 = (((-(t138 * t139 * t156 * T(2)) + t298) + t423) + t451) + -t495; + t544 = (((-(t140 * t141 * t156 * T(2)) + t303) + t472) + -t442) + t508; + t53 = (((-(t141 * t142 * t156 * T(2)) + t301) + t441) + t477) + -t506; + t157 = (((-(t139 * t142 * t156 * T(2)) + t59) + t105) + t14) + -t15; + t159 = (((-(t137 * t140 * t156 * T(2)) + t322) + -t16) + -t11) + -t12; + t147 = (((-(t138 * t141 * t156 * T(2)) + t323) + t13) + t26) + -t57; + t146 = + ((((t100 * t107 * t156 * T(2) + t266) + t327) + -t358) + -t369) + t491; + t145 = ((((-(t99 * t107 * t156 * T(2)) + -t265) + t329) + t367) + t386) + + -t503; + t144 = ((((-(t100 * t108 * t156 * T(2)) + -t267) + t325) + t360) + -t399) + + t499; + t143 = + ((((-(t99 * t109 * t156 * T(2)) + t267) + t335) + -t362) + t398) + t500; + t52 = ((((-(t98 * t108 * t156 * T(2)) + t265) + t339) + -t27) + -t387) + + -t504; + t51 = ((((t109 * t140 * t156 * T(2) + -t278) + -t302) + t28) + t35) + t40; + t50 = ((((-(t98 * t139 * t156 * T(2)) + t263) + -t299) + t36) + -t37) + t47; + t49 = ((((t107 * t142 * t156 * T(2) + t278) + -t302) + -t38) + -t39) + t40; + t48 = + ((((-(t100 * t137 * t156 * T(2)) + -t263) + -t299) + -t65) + t73) + t47; + t47 = + ((((t99 * t137 * t156 * T(2) + t262) + -t300) + t356) + -t446) + -t497; + t73 = + ((((t100 * t138 * t156 * T(2) + t264) + -t298) + -t382) + -t423) + t495; + t65 = ((((-(t109 * t141 * t156 * T(2)) + t279) + -t301) + t408) + -t441) + + t506; + t59 = + ((((t98 * t138 * t156 * T(2) + -t262) + -t300) + -t377) + t424) + -t497; + t40 = + ((((t99 * t139 * t156 * T(2) + -t264) + -t298) + -t353) + -t451) + t495; + t39 = ((((-(t107 * t141 * t156 * T(2)) + -t277) + -t303) + t403) + t442) + + -t508; + t38 = ((((-(t108 * t142 * t156 * T(2)) + -t279) + -t301) + t371) + -t477) + + t506; + t37 = ((((-(t108 * t140 * t156 * T(2)) + t277) + -t303) + -t374) + -t472) + + -t508; + t36 = ((((t98 * t142 * t156 * T(2) + t271) + t328) + -t359) + t437) + -t492; + t35 = ((((-(t109 * t137 * t156 * T(2)) + t270) + t328) + -t368) + -t428) + + -t492; + t28 = ((((t100 * t140 * t156 * T(2) + -t271) + -t327) + t369) + -t427) + + -t491; + t27 = ((((-(t98 * t141 * t156 * T(2)) + -t269) + t330) + t387) + -t433) + + t504; + t26 = + ((((t109 * t138 * t156 * T(2) + -t272) + t326) + -t398) + t432) + -t500; + t13 = ((((-(t107 * t139 * t156 * T(2)) + -t270) + -t327) + t358) + t438) + + -t491; + t12 = ((((-(t99 * t142 * t156 * T(2)) + -t273) + t326) + t362) + t467) + + -t500; + t11 = ((((-(t99 * t140 * t156 * T(2)) + t269) + -t329) + -t367) + t455) + + t503; + t16 = + ((((t107 * t138 * t156 * T(2) + t268) + -t329) + -t386) + -t434) + t503; + t15 = ((((-(t100 * t141 * t156 * T(2)) + t273) + -t325) + t399) + t431) + + -t499; + t14 = + ((((t108 * t139 * t156 * T(2) + t272) + -t325) + -t360) + t468) + -t499; + t105 = ((((-(t139 * t140 * t156 * T(2)) + t275) + t327) + t427) + -t438) + + t491; + t103 = + ((((t138 * t140 * t156 * T(2) + -t274) + t329) + t434) + -t455) + -t503; + t101 = ((((-(t137 * t142 * t156 * T(2)) + -t275) + t337) + t428) + -t437) + + t492; + t58 = + ((((t139 * t141 * t156 * T(2) + -t276) + t325) + -t431) + -t468) + t499; + t57 = + ((((t137 * t141 * t156 * T(2) + t274) + t339) + t433) + -t456) + -t504; + t56 = + ((((t138 * t142 * t156 * T(2) + t276) + t335) + -t432) + -t467) + t500; + t55 = -t315 + t342; + H[0] = (t55 + t142 * t142 * t156 * T(2)) - t521_tmp * T(4); + H[1] = t53; + H[2] = t148; + H[3] = t521; + H[4] = t38; + H[5] = t49; + H[6] = t157; + H[7] = t56; + H[8] = t101; + H[9] = t536; + H[10] = t12; + H[11] = t36; + H[12] = t53; + t54 = -t314 + t348; + H[13] = (t54 + t141 * t141 * t156 * T(2)) - t524_tmp * T(4); + H[14] = t544; + H[15] = t65; + H[16] = t524; + H[17] = t39; + H[18] = t58; + H[19] = t147; + H[20] = t57; + H[21] = t15; + H[22] = t538; + H[23] = t27; + H[24] = t148; + H[25] = t544; + t53 = -t313 + t343; + H[26] = (t53 + t140 * t140 * t156 * T(2)) + t520_tmp * T(4); + H[27] = t51; + H[28] = t37; + H[29] = t520; + H[30] = t105; + H[31] = t103; + H[32] = t159; + H[33] = t28; + H[34] = t11; + H[35] = t534; + H[36] = t521; + H[37] = t65; + H[38] = t51; + H[39] = (t55 + t109 * t109 * t156 * T(2)) + b_t521_tmp * T(4); + H[40] = t530; + H[41] = t526; + H[42] = t537; + H[43] = t26; + H[44] = t35; + H[45] = t532; + H[46] = t143; + H[47] = t550; + H[48] = t38; + H[49] = t524; + H[50] = t37; + H[51] = t530; + H[52] = (t54 + t108 * t108 * t156 * T(2)) + b_t524_tmp * T(4); + H[53] = t529; + H[54] = t14; + H[55] = t539; + H[56] = t568; + H[57] = t144; + H[58] = t533; + H[59] = t52; + H[60] = t49; + H[61] = t39; + H[62] = t520; + H[63] = t526; + H[64] = t529; + H[65] = (t53 + t107 * t107 * t156 * T(2)) - b_t520_tmp * T(4); + H[66] = t13; + H[67] = t16; + H[68] = t535; + H[69] = t146; + H[70] = t145; + H[71] = t531; + H[72] = t157; + H[73] = t58; + H[74] = t105; + H[75] = t537; + H[76] = t14; + H[77] = t13; + t55 = -t312 + t340; + H[78] = (t55 + t139 * t139 * t156 * T(2)) - t519_tmp * T(4); + H[79] = t543; + H[80] = t540; + H[81] = t519; + H[82] = t40; + H[83] = t50; + H[84] = t56; + H[85] = t147; + H[86] = t103; + H[87] = t26; + H[88] = t539; + H[89] = t16; + H[90] = t543; + t54 = -t311 + t345; + H[91] = (t54 + t138 * t138 * t156 * T(2)) - t523_tmp * T(4); + H[92] = t542; + H[93] = t73; + H[94] = t523; + H[95] = t59; + H[96] = t101; + H[97] = t57; + H[98] = t159; + H[99] = t35; + H[100] = t568; + H[101] = t535; + H[102] = t540; + H[103] = t542; + t53 = -t310 + t341; + H[104] = (t53 + t137 * t137 * t156 * T(2)) + t522_tmp * T(4); + H[105] = t48; + H[106] = t47; + H[107] = t522; + H[108] = t536; + H[109] = t15; + H[110] = t28; + H[111] = t532; + H[112] = t144; + H[113] = t146; + H[114] = t519; + H[115] = t73; + H[116] = t48; + H[117] = (t55 + t100 * t100 * t156 * T(2)) - b_t519_tmp * T(4); + H[118] = t528; + H[119] = t525; + H[120] = t12; + H[121] = t538; + H[122] = t11; + H[123] = t143; + H[124] = t533; + H[125] = t145; + H[126] = t40; + H[127] = t523; + H[128] = t47; + H[129] = t528; + H[130] = (t54 + t99 * t99 * t156 * T(2)) - b_t523_tmp * T(4); + H[131] = t527; + H[132] = t36; + H[133] = t27; + H[134] = t534; + H[135] = t550; + H[136] = t52; + H[137] = t531; + H[138] = t50; + H[139] = t59; + H[140] = t522; + H[141] = t525; + H[142] = t527; + H[143] = (t53 + t98 * t98 * t156 * T(2)) + b_t522_tmp * T(4); +} - // This function was generated by the Symbolic Math Toolbox version 8.3. - // 14-Jun-2019 13:58:38 - void line_line_distance_hessian( - double v01, - double v02, - double v03, - double v11, - double v12, - double v13, - double v21, - double v22, - double v23, - double v31, - double v32, - double v33, - double H[144]) - { - double t11, t12, t13, t14, t15, t16, t26, t27, t28, t47, t48, t49, t50, - t51, t52, t53, t54, t55, t56, t57, t58, t59, t65, t73, t35, t36, - t37, t38, t39, t40, t98, t99, t100, t101, t103, t105, t107, t108, - t109, t137, t138, t139, t140, t141, t142, t143, t144, t145, t146, - t147, t148, t156, t159, t157, t262, t263, t264, t265, t266, t267, - t268, t269, t270, t271, t272, t273, t274, t275, t276, t277, t278, - t279, t298, t299, t300, t301, t302, t303, t310, t311, t312, t313, - t314, t315, t322, t323, t325, t326, t327, t328, t329, t330, t335, - t337, t339, t340, t341, t342, t343, t345, t348, t353, t356, t358, - t359, t360, t362, t367, t368, t369, t371, t374, t377, t382, t386, - t387, t398, t399, t403, t408, t423, t424, t427, t428, t431, t432, - t433, t434, t437, t438, t441, t442, t446, t451, t455, t456, t467, - t468, t472, t477, t491, t492, t495, t497, t499, t500, t503, t504, - t506, t508, t550, t568, t519_tmp, b_t519_tmp, t519, t520_tmp, - b_t520_tmp, t520, t521_tmp, b_t521_tmp, t521, t522_tmp, b_t522_tmp, - t522, t523_tmp, b_t523_tmp, t523, t524_tmp, b_t524_tmp, t524, t525, - t526, t527, t528, t529, t530, t531, t532, t533, t534, t535, t536, - t537, t538, t539, t540, t542, t543, t544; +#define IPC_INSTANTIATE_LINE_LINE_AUTOGEN(T) \ + template void line_line_distance_gradient( \ + T, T, T, T, T, T, T, T, T, T, T, T, T[12]); \ + template void line_line_distance_hessian( \ + T, T, T, T, T, T, T, T, T, T, T, T, T[144]) - t11 = -v11 + v01; - t12 = -v12 + v02; - t13 = -v13 + v03; - t14 = -v21 + v01; - t15 = -v22 + v02; - t16 = -v23 + v03; - t26 = -v31 + v21; - t27 = -v32 + v22; - t28 = -v33 + v23; - t47 = t11 * t27; - t48 = t12 * t26; - t49 = t11 * t28; - t50 = t13 * t26; - t51 = t12 * t28; - t52 = t13 * t27; - t53 = t14 * t27; - t54 = t15 * t26; - t55 = t14 * t28; - t56 = t16 * t26; - t57 = t15 * t28; - t58 = t16 * t27; - t59 = t11 * t26 * 2.0; - t65 = t12 * t27 * 2.0; - t73 = t13 * t28 * 2.0; - t35 = t11 * t11 * 2.0; - t36 = t12 * t12 * 2.0; - t37 = t13 * t13 * 2.0; - t38 = t26 * t26 * 2.0; - t39 = t27 * t27 * 2.0; - t40 = t28 * t28 * 2.0; - t98 = t11 * t15 + -(t12 * t14); - t99 = t11 * t16 + -(t13 * t14); - t100 = t12 * t16 + -(t13 * t15); - t101 = t47 + -t48; - t103 = t49 + -t50; - t105 = t51 + -t52; - t107 = t53 + -t54; - t108 = t55 + -t56; - t109 = t57 + -t58; - t137 = t98 + t101; - t138 = t99 + t103; - t139 = t100 + t105; - t140 = (t54 + -t53) + t101; - t141 = (t56 + -t55) + t103; - t142 = (t58 + -t57) + t105; - t143 = t12 * t101 * 2.0 + t13 * t103 * 2.0; - t144 = t11 * t103 * 2.0 + t12 * t105 * 2.0; - t145 = t27 * t101 * 2.0 + t28 * t103 * 2.0; - t146 = t26 * t103 * 2.0 + t27 * t105 * 2.0; - t147 = t11 * t101 * 2.0 + -(t13 * t105 * 2.0); - t148 = t26 * t101 * 2.0 + -(t28 * t105 * 2.0); - t156 = 1.0 / ((t101 * t101 + t103 * t103) + t105 * t105); - t159 = (t16 * t101 + t14 * t105) + -(t15 * t103); - t157 = t156 * t156; - t57 = pow(t156, 3.0); - t58 = t159 * t159; - t262 = t11 * t156 * t159 * 2.0; - t263 = t12 * t156 * t159 * 2.0; - t264 = t13 * t156 * t159 * 2.0; - t265 = t14 * t156 * t159 * 2.0; - t266 = t15 * t156 * t159 * 2.0; - t267 = t16 * t156 * t159 * 2.0; - t268 = (-v31 + v01) * t156 * t159 * 2.0; - t269 = (-v21 + v11) * t156 * t159 * 2.0; - t270 = (-v32 + v02) * t156 * t159 * 2.0; - t271 = (-v22 + v12) * t156 * t159 * 2.0; - t272 = (-v33 + v03) * t156 * t159 * 2.0; - t273 = (-v23 + v13) * t156 * t159 * 2.0; - t274 = (-v31 + v11) * t156 * t159 * 2.0; - t275 = (-v32 + v12) * t156 * t159 * 2.0; - t276 = (-v33 + v13) * t156 * t159 * 2.0; - t277 = t26 * t156 * t159 * 2.0; - t278 = t27 * t156 * t159 * 2.0; - t279 = t28 * t156 * t159 * 2.0; - t298 = t11 * t12 * t157 * t58 * 2.0; - t299 = t11 * t13 * t157 * t58 * 2.0; - t300 = t12 * t13 * t157 * t58 * 2.0; - t301 = t26 * t27 * t157 * t58 * 2.0; - t302 = t26 * t28 * t157 * t58 * 2.0; - t303 = t27 * t28 * t157 * t58 * 2.0; - t310 = (t35 + t36) * t157 * t58; - t311 = (t35 + t37) * t157 * t58; - t312 = (t36 + t37) * t157 * t58; - t313 = (t38 + t39) * t157 * t58; - t314 = (t38 + t40) * t157 * t58; - t315 = (t39 + t40) * t157 * t58; - t322 = (t59 + t65) * t157 * t58; - t323 = (t59 + t73) * t157 * t58; - t59 = (t65 + t73) * t157 * t58; - t325 = (t47 * 2.0 + -(t48 * 4.0)) * t157 * t58; - t53 = -t157 * t58; - t56 = t48 * 2.0 - t47 * 4.0; - t326 = t53 * t56; - t327 = (t49 * 2.0 + -(t50 * 4.0)) * t157 * t58; - t55 = t50 * 2.0 - t49 * 4.0; - t328 = t53 * t55; - t329 = (t51 * 2.0 + -(t52 * 4.0)) * t157 * t58; - t54 = t52 * 2.0 - t51 * 4.0; - t330 = t53 * t54; - t53 = t157 * t58; - t335 = t53 * t56; - t337 = t53 * t55; - t339 = t53 * t54; - t340 = t143 * t143 * t57 * t58 * 2.0; - t341 = t144 * t144 * t57 * t58 * 2.0; - t342 = t145 * t145 * t57 * t58 * 2.0; - t343 = t146 * t146 * t57 * t58 * 2.0; - t345 = t147 * t147 * t57 * t58 * 2.0; - t348 = t148 * t148 * t57 * t58 * 2.0; - t36 = t98 * t143 * t157 * t159 * 2.0; - t353 = t99 * t143 * t157 * t159 * 2.0; - t356 = t99 * t144 * t157 * t159 * 2.0; - t65 = t100 * t144 * t157 * t159 * 2.0; - t358 = t107 * t143 * t157 * t159 * 2.0; - t359 = t98 * t145 * t157 * t159 * 2.0; - t360 = t108 * t143 * t157 * t159 * 2.0; - t54 = t107 * t144 * t157 * t159 * 2.0; - t362 = t99 * t145 * t157 * t159 * 2.0; - t53 = t98 * t146 * t157 * t159 * 2.0; - t56 = t109 * t143 * t157 * t159 * 2.0; - t27 = t108 * t144 * t157 * t159 * 2.0; - t55 = t100 * t145 * t157 * t159 * 2.0; - t367 = t99 * t146 * t157 * t159 * 2.0; - t368 = t109 * t144 * t157 * t159 * 2.0; - t369 = t100 * t146 * t157 * t159 * 2.0; - t38 = t107 * t145 * t157 * t159 * 2.0; - t371 = t108 * t145 * t157 * t159 * 2.0; - t374 = t108 * t146 * t157 * t159 * 2.0; - t28 = t109 * t146 * t157 * t159 * 2.0; - t377 = t98 * t147 * t157 * t159 * 2.0; - t382 = t100 * t147 * t157 * t159 * 2.0; - t386 = t107 * t147 * t157 * t159 * 2.0; - t387 = t98 * t148 * t157 * t159 * 2.0; - t103 = t108 * t147 * t157 * t159 * 2.0; - t101 = t99 * t148 * t157 * t159 * 2.0; - t398 = t109 * t147 * t157 * t159 * 2.0; - t399 = t100 * t148 * t157 * t159 * 2.0; - t403 = t107 * t148 * t157 * t159 * 2.0; - t408 = t109 * t148 * t157 * t159 * 2.0; - t73 = t137 * t143 * t157 * t159 * 2.0; - t423 = t138 * t143 * t157 * t159 * 2.0; - t424 = t138 * t144 * t157 * t159 * 2.0; - t37 = t139 * t144 * t157 * t159 * 2.0; - t427 = t140 * t143 * t157 * t159 * 2.0; - t428 = t137 * t145 * t157 * t159 * 2.0; - t16 = t140 * t144 * t157 * t159 * 2.0; - t11 = t137 * t146 * t157 * t159 * 2.0; - t431 = t141 * t143 * t157 * t159 * 2.0; - t432 = t138 * t145 * t157 * t159 * 2.0; - t433 = t141 * t144 * t157 * t159 * 2.0; - t434 = t138 * t146 * t157 * t159 * 2.0; - t105 = t142 * t143 * t157 * t159 * 2.0; - t14 = t139 * t145 * t157 * t159 * 2.0; - t437 = t142 * t144 * t157 * t159 * 2.0; - t438 = t139 * t146 * t157 * t159 * 2.0; - t35 = t140 * t145 * t157 * t159 * 2.0; - t441 = t141 * t145 * t157 * t159 * 2.0; - t442 = t141 * t146 * t157 * t159 * 2.0; - t39 = t142 * t146 * t157 * t159 * 2.0; - t446 = t137 * t147 * t157 * t159 * 2.0; - t451 = t139 * t147 * t157 * t159 * 2.0; - t455 = t140 * t147 * t157 * t159 * 2.0; - t456 = t137 * t148 * t157 * t159 * 2.0; - t13 = t141 * t147 * t157 * t159 * 2.0; - t26 = t138 * t148 * t157 * t159 * 2.0; - t467 = t142 * t147 * t157 * t159 * 2.0; - t468 = t139 * t148 * t157 * t159 * 2.0; - t472 = t140 * t148 * t157 * t159 * 2.0; - t477 = t142 * t148 * t157 * t159 * 2.0; - t47 = t143 * t144 * t57 * t58 * 2.0; - t15 = t143 * t145 * t57 * t58 * 2.0; - t491 = t143 * t146 * t57 * t58 * 2.0; - t492 = t144 * t145 * t57 * t58 * 2.0; - t12 = t144 * t146 * t57 * t58 * 2.0; - t40 = t145 * t146 * t57 * t58 * 2.0; - t495 = t143 * t147 * t57 * t58 * 2.0; - t497 = t144 * t147 * t57 * t58 * 2.0; - t499 = t143 * t148 * t57 * t58 * 2.0; - t500 = t145 * t147 * t57 * t58 * 2.0; - t503 = t146 * t147 * t57 * t58 * 2.0; - t504 = t144 * t148 * t57 * t58 * 2.0; - t506 = t145 * t148 * t57 * t58 * 2.0; - t508 = t146 * t148 * t57 * t58 * 2.0; - t57 = t147 * t148 * t57 * t58 * 2.0; - t550 = - ((((t98 * t109 * t156 * 2.0 + -t266) + t337) + t359) + t368) + t492; - t568 = - ((((t108 * t137 * t156 * 2.0 + -t268) + t330) + t27) + t456) + t504; - t519_tmp = t139 * t143 * t157 * t159; - b_t519_tmp = t100 * t143 * t157 * t159; - t519 = - (((-(t100 * t139 * t156 * 2.0) + t312) + -t340) + b_t519_tmp * 2.0) - + t519_tmp * 2.0; - t520_tmp = t140 * t146 * t157 * t159; - b_t520_tmp = t107 * t146 * t157 * t159; - t520 = (((t107 * t140 * t156 * 2.0 + t313) + -t343) + b_t520_tmp * 2.0) - + -(t520_tmp * 2.0); - t521_tmp = t142 * t145 * t157 * t159; - b_t521_tmp = t109 * t145 * t157 * t159; - t521 = - (((t109 * t142 * t156 * 2.0 + t315) + -t342) + -(b_t521_tmp * 2.0)) - + t521_tmp * 2.0; - t522_tmp = t137 * t144 * t157 * t159; - b_t522_tmp = t98 * t144 * t157 * t159; - t522 = (((-(t98 * t137 * t156 * 2.0) + t310) + -t341) - + -(b_t522_tmp * 2.0)) - + -(t522_tmp * 2.0); - t523_tmp = t138 * t147 * t157 * t159; - b_t523_tmp = t99 * t147 * t157 * t159; - t523 = - (((-(t99 * t138 * t156 * 2.0) + t311) + -t345) + b_t523_tmp * 2.0) - + t523_tmp * 2.0; - t524_tmp = t141 * t148 * t157 * t159; - b_t524_tmp = t108 * t148 * t157 * t159; - t524 = - (((t108 * t141 * t156 * 2.0 + t314) + -t348) + -(b_t524_tmp * 2.0)) - + t524_tmp * 2.0; - t525 = (((t98 * t100 * t156 * 2.0 + t299) + t65) + -t36) + -t47; - t526 = (((t107 * t109 * t156 * 2.0 + t302) + t38) + -t28) + -t40; - t527 = (((-(t98 * t99 * t156 * 2.0) + t300) + t377) + -t356) + t497; - t528 = (((-(t99 * t100 * t156 * 2.0) + t298) + t353) + t382) + -t495; - t529 = (((-(t107 * t108 * t156 * 2.0) + t303) + t374) + -t403) + t508; - t530 = (((-(t108 * t109 * t156 * 2.0) + t301) + -t371) + -t408) + -t506; - t531 = (((t98 * t107 * t156 * 2.0 + t322) + t54) + -t53) + -t12; - t532 = (((t100 * t109 * t156 * 2.0 + t59) + t55) + -t56) + -t15; - t533 = (((t99 * t108 * t156 * 2.0 + t323) + t101) + -t103) + -t57; - t534 = (((t98 * t140 * t156 * 2.0 + -t322) + t53) + t16) + t12; - t535 = (((-(t107 * t137 * t156 * 2.0) + -t322) + -t54) + t11) + t12; - t536 = (((t100 * t142 * t156 * 2.0 + -t59) + -t55) + -t105) + t15; - t537 = (((-(t109 * t139 * t156 * 2.0) + -t59) + t56) + -t14) + t15; - t538 = (((t99 * t141 * t156 * 2.0 + -t323) + -t101) + -t13) + t57; - t539 = (((-(t108 * t138 * t156 * 2.0) + -t323) + t103) + -t26) + t57; - t540 = (((t137 * t139 * t156 * 2.0 + t299) + t37) + -t73) + -t47; - t148 = (((t140 * t142 * t156 * 2.0 + t302) + t39) + -t35) + -t40; - t542 = (((-(t137 * t138 * t156 * 2.0) + t300) + t446) + -t424) + t497; - t543 = (((-(t138 * t139 * t156 * 2.0) + t298) + t423) + t451) + -t495; - t544 = (((-(t140 * t141 * t156 * 2.0) + t303) + t472) + -t442) + t508; - t53 = (((-(t141 * t142 * t156 * 2.0) + t301) + t441) + t477) + -t506; - t157 = (((-(t139 * t142 * t156 * 2.0) + t59) + t105) + t14) + -t15; - t159 = (((-(t137 * t140 * t156 * 2.0) + t322) + -t16) + -t11) + -t12; - t147 = (((-(t138 * t141 * t156 * 2.0) + t323) + t13) + t26) + -t57; - t146 = ((((t100 * t107 * t156 * 2.0 + t266) + t327) + -t358) + -t369) - + t491; - t145 = ((((-(t99 * t107 * t156 * 2.0) + -t265) + t329) + t367) + t386) - + -t503; - t144 = ((((-(t100 * t108 * t156 * 2.0) + -t267) + t325) + t360) + -t399) - + t499; - t143 = ((((-(t99 * t109 * t156 * 2.0) + t267) + t335) + -t362) + t398) - + t500; - t52 = ((((-(t98 * t108 * t156 * 2.0) + t265) + t339) + -t27) + -t387) - + -t504; - t51 = - ((((t109 * t140 * t156 * 2.0 + -t278) + -t302) + t28) + t35) + t40; - t50 = ((((-(t98 * t139 * t156 * 2.0) + t263) + -t299) + t36) + -t37) - + t47; - t49 = - ((((t107 * t142 * t156 * 2.0 + t278) + -t302) + -t38) + -t39) + t40; - t48 = ((((-(t100 * t137 * t156 * 2.0) + -t263) + -t299) + -t65) + t73) - + t47; - t47 = ((((t99 * t137 * t156 * 2.0 + t262) + -t300) + t356) + -t446) - + -t497; - t73 = ((((t100 * t138 * t156 * 2.0 + t264) + -t298) + -t382) + -t423) - + t495; - t65 = ((((-(t109 * t141 * t156 * 2.0) + t279) + -t301) + t408) + -t441) - + t506; - t59 = ((((t98 * t138 * t156 * 2.0 + -t262) + -t300) + -t377) + t424) - + -t497; - t40 = ((((t99 * t139 * t156 * 2.0 + -t264) + -t298) + -t353) + -t451) - + t495; - t39 = ((((-(t107 * t141 * t156 * 2.0) + -t277) + -t303) + t403) + t442) - + -t508; - t38 = ((((-(t108 * t142 * t156 * 2.0) + -t279) + -t301) + t371) + -t477) - + t506; - t37 = ((((-(t108 * t140 * t156 * 2.0) + t277) + -t303) + -t374) + -t472) - + -t508; - t36 = ((((t98 * t142 * t156 * 2.0 + t271) + t328) + -t359) + t437) - + -t492; - t35 = ((((-(t109 * t137 * t156 * 2.0) + t270) + t328) + -t368) + -t428) - + -t492; - t28 = ((((t100 * t140 * t156 * 2.0 + -t271) + -t327) + t369) + -t427) - + -t491; - t27 = ((((-(t98 * t141 * t156 * 2.0) + -t269) + t330) + t387) + -t433) - + t504; - t26 = ((((t109 * t138 * t156 * 2.0 + -t272) + t326) + -t398) + t432) - + -t500; - t13 = ((((-(t107 * t139 * t156 * 2.0) + -t270) + -t327) + t358) + t438) - + -t491; - t12 = ((((-(t99 * t142 * t156 * 2.0) + -t273) + t326) + t362) + t467) - + -t500; - t11 = ((((-(t99 * t140 * t156 * 2.0) + t269) + -t329) + -t367) + t455) - + t503; - t16 = ((((t107 * t138 * t156 * 2.0 + t268) + -t329) + -t386) + -t434) - + t503; - t15 = ((((-(t100 * t141 * t156 * 2.0) + t273) + -t325) + t399) + t431) - + -t499; - t14 = ((((t108 * t139 * t156 * 2.0 + t272) + -t325) + -t360) + t468) - + -t499; - t105 = ((((-(t139 * t140 * t156 * 2.0) + t275) + t327) + t427) + -t438) - + t491; - t103 = ((((t138 * t140 * t156 * 2.0 + -t274) + t329) + t434) + -t455) - + -t503; - t101 = ((((-(t137 * t142 * t156 * 2.0) + -t275) + t337) + t428) + -t437) - + t492; - t58 = ((((t139 * t141 * t156 * 2.0 + -t276) + t325) + -t431) + -t468) - + t499; - t57 = ((((t137 * t141 * t156 * 2.0 + t274) + t339) + t433) + -t456) - + -t504; - t56 = ((((t138 * t142 * t156 * 2.0 + t276) + t335) + -t432) + -t467) - + t500; - t55 = -t315 + t342; - H[0] = (t55 + t142 * t142 * t156 * 2.0) - t521_tmp * 4.0; - H[1] = t53; - H[2] = t148; - H[3] = t521; - H[4] = t38; - H[5] = t49; - H[6] = t157; - H[7] = t56; - H[8] = t101; - H[9] = t536; - H[10] = t12; - H[11] = t36; - H[12] = t53; - t54 = -t314 + t348; - H[13] = (t54 + t141 * t141 * t156 * 2.0) - t524_tmp * 4.0; - H[14] = t544; - H[15] = t65; - H[16] = t524; - H[17] = t39; - H[18] = t58; - H[19] = t147; - H[20] = t57; - H[21] = t15; - H[22] = t538; - H[23] = t27; - H[24] = t148; - H[25] = t544; - t53 = -t313 + t343; - H[26] = (t53 + t140 * t140 * t156 * 2.0) + t520_tmp * 4.0; - H[27] = t51; - H[28] = t37; - H[29] = t520; - H[30] = t105; - H[31] = t103; - H[32] = t159; - H[33] = t28; - H[34] = t11; - H[35] = t534; - H[36] = t521; - H[37] = t65; - H[38] = t51; - H[39] = (t55 + t109 * t109 * t156 * 2.0) + b_t521_tmp * 4.0; - H[40] = t530; - H[41] = t526; - H[42] = t537; - H[43] = t26; - H[44] = t35; - H[45] = t532; - H[46] = t143; - H[47] = t550; - H[48] = t38; - H[49] = t524; - H[50] = t37; - H[51] = t530; - H[52] = (t54 + t108 * t108 * t156 * 2.0) + b_t524_tmp * 4.0; - H[53] = t529; - H[54] = t14; - H[55] = t539; - H[56] = t568; - H[57] = t144; - H[58] = t533; - H[59] = t52; - H[60] = t49; - H[61] = t39; - H[62] = t520; - H[63] = t526; - H[64] = t529; - H[65] = (t53 + t107 * t107 * t156 * 2.0) - b_t520_tmp * 4.0; - H[66] = t13; - H[67] = t16; - H[68] = t535; - H[69] = t146; - H[70] = t145; - H[71] = t531; - H[72] = t157; - H[73] = t58; - H[74] = t105; - H[75] = t537; - H[76] = t14; - H[77] = t13; - t55 = -t312 + t340; - H[78] = (t55 + t139 * t139 * t156 * 2.0) - t519_tmp * 4.0; - H[79] = t543; - H[80] = t540; - H[81] = t519; - H[82] = t40; - H[83] = t50; - H[84] = t56; - H[85] = t147; - H[86] = t103; - H[87] = t26; - H[88] = t539; - H[89] = t16; - H[90] = t543; - t54 = -t311 + t345; - H[91] = (t54 + t138 * t138 * t156 * 2.0) - t523_tmp * 4.0; - H[92] = t542; - H[93] = t73; - H[94] = t523; - H[95] = t59; - H[96] = t101; - H[97] = t57; - H[98] = t159; - H[99] = t35; - H[100] = t568; - H[101] = t535; - H[102] = t540; - H[103] = t542; - t53 = -t310 + t341; - H[104] = (t53 + t137 * t137 * t156 * 2.0) + t522_tmp * 4.0; - H[105] = t48; - H[106] = t47; - H[107] = t522; - H[108] = t536; - H[109] = t15; - H[110] = t28; - H[111] = t532; - H[112] = t144; - H[113] = t146; - H[114] = t519; - H[115] = t73; - H[116] = t48; - H[117] = (t55 + t100 * t100 * t156 * 2.0) - b_t519_tmp * 4.0; - H[118] = t528; - H[119] = t525; - H[120] = t12; - H[121] = t538; - H[122] = t11; - H[123] = t143; - H[124] = t533; - H[125] = t145; - H[126] = t40; - H[127] = t523; - H[128] = t47; - H[129] = t528; - H[130] = (t54 + t99 * t99 * t156 * 2.0) - b_t523_tmp * 4.0; - H[131] = t527; - H[132] = t36; - H[133] = t27; - H[134] = t534; - H[135] = t550; - H[136] = t52; - H[137] = t531; - H[138] = t50; - H[139] = t59; - H[140] = t522; - H[141] = t525; - H[142] = t527; - H[143] = (t53 + t98 * t98 * t156 * 2.0) + b_t522_tmp * 4.0; - } +IPC_INSTANTIATE_LINE_LINE_AUTOGEN(float); +IPC_INSTANTIATE_LINE_LINE_AUTOGEN(double); +#undef IPC_INSTANTIATE_LINE_LINE_AUTOGEN -} // namespace autogen -} // namespace ipc +} // namespace ipc::autogen diff --git a/src/ipc/distance/line_line.hpp b/src/ipc/distance/line_line.hpp index e519d478b..cb0b35f34 100644 --- a/src/ipc/distance/line_line.hpp +++ b/src/ipc/distance/line_line.hpp @@ -2,81 +2,103 @@ #include +#include + namespace ipc { -/// @brief Compute the distance between a two infinite lines in 3D. -/// @note The distance is actually squared distance. -/// @warning If the lines are parallel this function returns a distance of zero. -/// @param ea0 The first vertex of the edge defining the first line. -/// @param ea1 The second vertex of the edge defining the first line. -/// @param eb0 The first vertex of the edge defining the second line. -/// @param eb1 The second vertex of the edge defining the second line. -/// @return The distance between the two lines. -double line_line_distance( - Eigen::ConstRef ea0, - Eigen::ConstRef ea1, - Eigen::ConstRef eb0, - Eigen::ConstRef eb1); +// Symbolically generated derivatives +namespace autogen { + // clang-format off + template + void line_line_distance_gradient( + T v01, T v02, T v03, T v11, T v12, T v13, T v21, T v22, T v23, T v31, T v32, T v33, T g[12]); + template + void line_line_distance_hessian( + T v01, T v02, T v03, T v11, T v12, T v13, T v21, T v22, T v23, T v31, T v32, T v33, T H[144]); + // clang-format on +} // namespace autogen -/// @brief Compute the gradient of the distance between a two lines in 3D. -/// @note The distance is actually squared distance. -/// @warning If the lines are parallel this function returns a distance of zero. -/// @param ea0 The first vertex of the edge defining the first line. -/// @param ea1 The second vertex of the edge defining the first line. -/// @param eb0 The first vertex of the edge defining the second line. -/// @param eb1 The second vertex of the edge defining the second line. -/// @return The gradient of the distance wrt ea0, ea1, eb0, and eb1. -Vector12d line_line_distance_gradient( - Eigen::ConstRef ea0, - Eigen::ConstRef ea1, - Eigen::ConstRef eb0, - Eigen::ConstRef eb1); +namespace detail { + /// @brief Compute the distance between a two infinite lines in 3D. + /// @note The distance is actually squared distance. + /// @warning If the lines are parallel this function returns a distance of zero. + /// @param ea0 The first vertex of the edge defining the first line. + /// @param ea1 The second vertex of the edge defining the first line. + /// @param eb0 The first vertex of the edge defining the second line. + /// @param eb1 The second vertex of the edge defining the second line. + /// @return The distance between the two lines. + template + inline T line_line_distance( + Eigen::ConstRef> ea0, + Eigen::ConstRef> ea1, + Eigen::ConstRef> eb0, + Eigen::ConstRef> eb1) + { + const Eigen::Vector3 normal = (ea1 - ea0).cross(eb1 - eb0); + const T line_to_line = (eb0 - ea0).dot(normal); + return line_to_line * line_to_line / normal.squaredNorm(); + } +} // namespace detail -/// @brief Compute the hessian of the distance between a two lines in 3D. +/// @brief Compute the distance between a two infinite lines in 3D. /// @note The distance is actually squared distance. /// @warning If the lines are parallel this function returns a distance of zero. /// @param ea0 The first vertex of the edge defining the first line. /// @param ea1 The second vertex of the edge defining the first line. /// @param eb0 The first vertex of the edge defining the second line. /// @param eb1 The second vertex of the edge defining the second line. -/// @return The hessian of the distance wrt ea0, ea1, eb0, and eb1. -Matrix12d line_line_distance_hessian( - Eigen::ConstRef ea0, - Eigen::ConstRef ea1, - Eigen::ConstRef eb0, - Eigen::ConstRef eb1); +/// @return The distance between the two lines. +template < + typename DerivedEA0, + typename DerivedEA1, + typename DerivedEB0, + typename DerivedEB1> +inline auto line_line_distance( + const Eigen::MatrixBase& ea0, + const Eigen::MatrixBase& ea1, + const Eigen::MatrixBase& eb0, + const Eigen::MatrixBase& eb1) +{ + using T = typename DerivedEA0::Scalar; + // NOTE: explicit : the detail overload cannot deduce T from an + // arbitrary Eigen expression. + return detail::line_line_distance(ea0, ea1, eb0, eb1); +} -// Symbolically generated derivatives; -namespace autogen { - void line_line_distance_gradient( - double v01, - double v02, - double v03, - double v11, - double v12, - double v13, - double v21, - double v22, - double v23, - double v31, - double v32, - double v33, - double g[12]); +template < + typename DerivedEA0, + typename DerivedEA1, + typename DerivedEB0, + typename DerivedEB1> +inline auto line_line_distance_gradient( + const Eigen::MatrixBase& ea0, + const Eigen::MatrixBase& ea1, + const Eigen::MatrixBase& eb0, + const Eigen::MatrixBase& eb1) +{ + Eigen::Vector grad; + autogen::line_line_distance_gradient( + ea0[0], ea0[1], ea0[2], ea1[0], ea1[1], ea1[2], eb0[0], eb0[1], eb0[2], + eb1[0], eb1[1], eb1[2], grad.data()); + return grad; +} - void line_line_distance_hessian( - double v01, - double v02, - double v03, - double v11, - double v12, - double v13, - double v21, - double v22, - double v23, - double v31, - double v32, - double v33, - double H[144]); -} // namespace autogen +template < + typename DerivedEA0, + typename DerivedEA1, + typename DerivedEB0, + typename DerivedEB1> +inline auto line_line_distance_hessian( + const Eigen::MatrixBase& ea0, + const Eigen::MatrixBase& ea1, + const Eigen::MatrixBase& eb0, + const Eigen::MatrixBase& eb1) +{ + Eigen::Matrix hess; + autogen::line_line_distance_hessian( + ea0[0], ea0[1], ea0[2], ea1[0], ea1[1], ea1[2], eb0[0], eb0[1], eb0[2], + eb1[0], eb1[1], eb1[2], hess.data()); + return hess; +} } // namespace ipc diff --git a/src/ipc/distance/point_edge.cpp b/src/ipc/distance/point_edge.cpp index ac53965ec..8fbce7b1c 100644 --- a/src/ipc/distance/point_edge.cpp +++ b/src/ipc/distance/point_edge.cpp @@ -1,5 +1,6 @@ #include "point_edge.hpp" +#include #include #include @@ -7,118 +8,72 @@ namespace ipc { -double point_edge_distance( - Eigen::ConstRef p, - Eigen::ConstRef e0, - Eigen::ConstRef e1, - PointEdgeDistanceType dtype) -{ - assert(p.size() == 2 || p.size() == 3); - assert(e0.size() == 2 || e0.size() == 3); - assert(e1.size() == 2 || e1.size() == 3); - - if (dtype == PointEdgeDistanceType::AUTO) { - dtype = point_edge_distance_type(p, e0, e1); +namespace detail { + + template + Eigen::Matrix point_edge_distance_hessian( + Eigen::ConstRef> p, + Eigen::ConstRef> e0, + Eigen::ConstRef> e1, + PointEdgeDistanceType dtype) + { + static_assert(dim == 2 || dim == 3, "point-edge is only 2D or 3D"); + + if constexpr (std::is_floating_point_v) { + if (dtype == PointEdgeDistanceType::AUTO) { + dtype = point_edge_distance_type(p, e0, e1); + } + } else if (dtype == PointEdgeDistanceType::AUTO) { + throw_auto_requires_explicit_dtype("point_edge_distance_hessian"); + } + + Eigen::Matrix hess = + Eigen::Matrix::Zero(); + + switch (dtype) { + case PointEdgeDistanceType::P_E0: + hess.template topLeftCorner<2 * dim, 2 * dim>() = + point_point_distance_hessian(p, e0); + break; + + case PointEdgeDistanceType::P_E1: { + const Eigen::Matrix local_hess = + point_point_distance_hessian(p, e1); + hess.template topLeftCorner() = + local_hess.template topLeftCorner(); + hess.template topRightCorner() = + local_hess.template topRightCorner(); + hess.template bottomLeftCorner() = + local_hess.template bottomLeftCorner(); + hess.template bottomRightCorner() = + local_hess.template bottomRightCorner(); + break; + } + + case PointEdgeDistanceType::P_E: + hess = point_line_distance_hessian(p, e0, e1); + break; + + default: + throw_invalid_distance_type("point_edge_distance_hessian"); + } + return hess; } - switch (dtype) { - case PointEdgeDistanceType::P_E0: - return point_point_distance(p, e0); +#define IPC_INSTANTIATE_POINT_EDGE_DISTANCE_HESSIAN(T, dim) \ + template Eigen::Matrix \ + point_edge_distance_hessian( \ + Eigen::ConstRef>, \ + Eigen::ConstRef>, \ + Eigen::ConstRef>, PointEdgeDistanceType) - case PointEdgeDistanceType::P_E1: - return point_point_distance(p, e1); + IPC_INSTANTIATE_POINT_EDGE_DISTANCE_HESSIAN(float, 2); + IPC_INSTANTIATE_POINT_EDGE_DISTANCE_HESSIAN(float, 3); + IPC_INSTANTIATE_POINT_EDGE_DISTANCE_HESSIAN(double, 2); + IPC_INSTANTIATE_POINT_EDGE_DISTANCE_HESSIAN(double, 3); - case PointEdgeDistanceType::P_E: - return point_line_distance(p, e0, e1); +#undef IPC_INSTANTIATE_POINT_EDGE_DISTANCE_HESSIAN - default: - throw std::invalid_argument( - "Invalid distance type for point-edge distance!"); - } -} - -VectorMax9d point_edge_distance_gradient( - Eigen::ConstRef p, - Eigen::ConstRef e0, - Eigen::ConstRef e1, - PointEdgeDistanceType dtype) -{ - const int dim = p.size(); - assert(e0.size() == dim); - assert(e1.size() == dim); - - if (dtype == PointEdgeDistanceType::AUTO) { - dtype = point_edge_distance_type(p, e0, e1); - } - - VectorMax9d grad = VectorMax9d::Zero(3 * dim); - - switch (dtype) { - case PointEdgeDistanceType::P_E0: - grad.head(2 * dim) = point_point_distance_gradient(p, e0); - break; - - case PointEdgeDistanceType::P_E1: { - const VectorMax6d local_grad = point_point_distance_gradient(p, e1); - grad.head(dim) = local_grad.head(dim); - grad.tail(dim) = local_grad.tail(dim); - break; - } - - case PointEdgeDistanceType::P_E: - grad = point_line_distance_gradient(p, e0, e1); - break; - - default: - throw std::invalid_argument( - "Invalid distance type for point-edge distance gradient!"); - } - - return grad; -} - -MatrixMax9d point_edge_distance_hessian( - Eigen::ConstRef p, - Eigen::ConstRef e0, - Eigen::ConstRef e1, - PointEdgeDistanceType dtype) -{ - const int dim = p.size(); - assert(e0.size() == dim); - assert(e1.size() == dim); - - if (dtype == PointEdgeDistanceType::AUTO) { - dtype = point_edge_distance_type(p, e0, e1); - } - - MatrixMax9d hess = MatrixMax9d::Zero(3 * dim, 3 * dim); - - switch (dtype) { - case PointEdgeDistanceType::P_E0: - hess.topLeftCorner(2 * dim, 2 * dim) = - point_point_distance_hessian(p, e0); - break; - - case PointEdgeDistanceType::P_E1: { - const MatrixMax6d local_hess = point_point_distance_hessian(p, e1); - hess.topLeftCorner(dim, dim) = local_hess.topLeftCorner(dim, dim); - hess.topRightCorner(dim, dim) = local_hess.topRightCorner(dim, dim); - hess.bottomLeftCorner(dim, dim) = local_hess.bottomLeftCorner(dim, dim); - hess.bottomRightCorner(dim, dim) = - local_hess.bottomRightCorner(dim, dim); - break; - } - - case PointEdgeDistanceType::P_E: - hess = point_line_distance_hessian(p, e0, e1); - break; - - default: - throw std::invalid_argument( - "Invalid distance type for point-edge distance hessian!"); - } - - return hess; -} +} // namespace detail } // namespace ipc diff --git a/src/ipc/distance/point_edge.hpp b/src/ipc/distance/point_edge.hpp index c2596c8a7..80875cec6 100644 --- a/src/ipc/distance/point_edge.hpp +++ b/src/ipc/distance/point_edge.hpp @@ -1,9 +1,126 @@ #pragma once #include +#include +#include +#include + +#include +#include namespace ipc { +namespace detail { + /// @brief Compute the distance between a point and edge in 2D or 3D. + /// @note The distance is actually squared distance. + /// @tparam T The scalar type. + /// @tparam dim The dimension (2 or 3). + /// @param p The point. + /// @param e0 The first vertex of the edge. + /// @param e1 The second vertex of the edge. + /// @param dtype The point edge distance type to compute. + /// @return The distance between the point and edge. + template + inline T point_edge_distance( + Eigen::ConstRef> p, + Eigen::ConstRef> e0, + Eigen::ConstRef> e1, + PointEdgeDistanceType dtype = PointEdgeDistanceType::AUTO) + { + static_assert(dim == 2 || dim == 3, "point-edge is only 2D or 3D"); + + if constexpr (std::is_floating_point_v) { + if (dtype == PointEdgeDistanceType::AUTO) { + dtype = point_edge_distance_type(p, e0, e1); + } + } else if (dtype == PointEdgeDistanceType::AUTO) { + throw_auto_requires_explicit_dtype("point_edge_distance"); + } + + switch (dtype) { + case PointEdgeDistanceType::P_E0: + return point_point_distance(p, e0); + + case PointEdgeDistanceType::P_E1: + return point_point_distance(p, e1); + + case PointEdgeDistanceType::P_E: + return point_line_distance(p, e0, e1); + + default: + throw_invalid_distance_type("point_edge_distance"); + } + } + + /// @brief Compute the gradient of the distance between a point and edge. + /// @note The distance is actually squared distance. + /// @tparam T The scalar type. + /// @tparam dim The dimension (2 or 3). + /// @param p The point. + /// @param e0 The first vertex of the edge. + /// @param e1 The second vertex of the edge. + /// @param dtype The point edge distance type to compute. + /// @return The gradient of the distance wrt p, e0, and e1. + template + inline Eigen::Vector point_edge_distance_gradient( + Eigen::ConstRef> p, + Eigen::ConstRef> e0, + Eigen::ConstRef> e1, + PointEdgeDistanceType dtype = PointEdgeDistanceType::AUTO) + { + static_assert(dim == 2 || dim == 3, "point-edge is only 2D or 3D"); + + if constexpr (std::is_floating_point_v) { + if (dtype == PointEdgeDistanceType::AUTO) { + dtype = point_edge_distance_type(p, e0, e1); + } + } else if (dtype == PointEdgeDistanceType::AUTO) { + throw_auto_requires_explicit_dtype("point_edge_distance_gradient"); + } + + Eigen::Vector grad = Eigen::Vector::Zero(); + + switch (dtype) { + case PointEdgeDistanceType::P_E0: + grad.template head<2 * dim>() = + point_point_distance_gradient(p, e0); + break; + + case PointEdgeDistanceType::P_E1: { + const Eigen::Vector local_grad = + point_point_distance_gradient(p, e1); + grad.template head() = local_grad.template head(); + grad.template tail() = local_grad.template tail(); + break; + } + + case PointEdgeDistanceType::P_E: + grad = point_line_distance_gradient(p, e0, e1); + break; + + default: + throw_invalid_distance_type("point_edge_distance_gradient"); + } + return grad; + } + + /// @brief Compute the hessian of the distance between a point and edge. + /// @note The distance is actually squared distance. + /// @tparam T The scalar type. + /// @tparam dim The dimension (2 or 3). + /// @param p The point. + /// @param e0 The first vertex of the edge. + /// @param e1 The second vertex of the edge. + /// @param dtype The point edge distance type to compute. + /// @return The hessian of the distance wrt p, e0, and e1. + template + Eigen::Matrix point_edge_distance_hessian( + Eigen::ConstRef> p, + Eigen::ConstRef> e0, + Eigen::ConstRef> e1, + PointEdgeDistanceType dtype = PointEdgeDistanceType::AUTO); +} // namespace detail + /// @brief Compute the distance between a point and edge in 2D or 3D. /// @note The distance is actually squared distance. /// @param p The point. @@ -11,11 +128,26 @@ namespace ipc { /// @param e1 The second vertex of the edge. /// @param dtype The point edge distance type to compute. /// @return The distance between the point and edge. -double point_edge_distance( - Eigen::ConstRef p, - Eigen::ConstRef e0, - Eigen::ConstRef e1, - PointEdgeDistanceType dtype = PointEdgeDistanceType::AUTO); +template +inline auto point_edge_distance( + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& e0, + const Eigen::MatrixBase& e1, + PointEdgeDistanceType dtype = PointEdgeDistanceType::AUTO) +{ + using T = typename DerivedP::Scalar; + + if constexpr (dim_v == 2) { + return detail::point_edge_distance(p, e0, e1, dtype); + } else if constexpr (dim_v == 3) { + return detail::point_edge_distance(p, e0, e1, dtype); + } else if (p.size() == 2) { + return detail::point_edge_distance(p, e0, e1, dtype); + } else { + assert(p.size() == 3); + return detail::point_edge_distance(p, e0, e1, dtype); + } +} /// @brief Compute the gradient of the distance between a point and edge. /// @note The distance is actually squared distance. @@ -23,12 +155,28 @@ double point_edge_distance( /// @param e0 The first vertex of the edge. /// @param e1 The second vertex of the edge. /// @param dtype The point edge distance type to compute. -/// @return grad The gradient of the distance wrt p, e0, and e1. -VectorMax9d point_edge_distance_gradient( - Eigen::ConstRef p, - Eigen::ConstRef e0, - Eigen::ConstRef e1, - PointEdgeDistanceType dtype = PointEdgeDistanceType::AUTO); +/// @return The gradient of the distance wrt p, e0, and e1. +template +inline auto point_edge_distance_gradient( + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& e0, + const Eigen::MatrixBase& e1, + PointEdgeDistanceType dtype = PointEdgeDistanceType::AUTO) +{ + using T = typename DerivedP::Scalar; + if constexpr (dim_v == 2) { + return detail::point_edge_distance_gradient(p, e0, e1, dtype); + } else if constexpr (dim_v == 3) { + return detail::point_edge_distance_gradient(p, e0, e1, dtype); + } else if (p.size() == 2) { + return VectorMax9( + detail::point_edge_distance_gradient(p, e0, e1, dtype)); + } else { + assert(p.size() == 3); + return VectorMax9( + detail::point_edge_distance_gradient(p, e0, e1, dtype)); + } +} /// @brief Compute the hessian of the distance between a point and edge. /// @note The distance is actually squared distance. @@ -36,11 +184,27 @@ VectorMax9d point_edge_distance_gradient( /// @param e0 The first vertex of the edge. /// @param e1 The second vertex of the edge. /// @param dtype The point edge distance type to compute. -/// @return hess The hessian of the distance wrt p, e0, and e1. -MatrixMax9d point_edge_distance_hessian( - Eigen::ConstRef p, - Eigen::ConstRef e0, - Eigen::ConstRef e1, - PointEdgeDistanceType dtype = PointEdgeDistanceType::AUTO); +/// @return The hessian of the distance wrt p, e0, and e1. +template +inline auto point_edge_distance_hessian( + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& e0, + const Eigen::MatrixBase& e1, + PointEdgeDistanceType dtype = PointEdgeDistanceType::AUTO) +{ + using T = typename DerivedP::Scalar; + if constexpr (dim_v == 2) { + return detail::point_edge_distance_hessian(p, e0, e1, dtype); + } else if constexpr (dim_v == 3) { + return detail::point_edge_distance_hessian(p, e0, e1, dtype); + } else if (p.size() == 2) { + return MatrixMax9( + detail::point_edge_distance_hessian(p, e0, e1, dtype)); + } else { + assert(p.size() == 3); + return MatrixMax9( + detail::point_edge_distance_hessian(p, e0, e1, dtype)); + } +} } // namespace ipc diff --git a/src/ipc/distance/point_line.cpp b/src/ipc/distance/point_line.cpp index f053da115..2956b55a0 100644 --- a/src/ipc/distance/point_line.cpp +++ b/src/ipc/distance/point_line.cpp @@ -1,491 +1,413 @@ #include "point_line.hpp" -namespace ipc { +#include -double point_line_distance( - Eigen::ConstRef p, - Eigen::ConstRef e0, - Eigen::ConstRef e1) +namespace ipc::autogen { + +// This function was generated by the Symbolic Math Toolbox version 8.3. +// 16-Mar-2020 15:56:29 +template +void point_line_distance_gradient_2D( + T v01, T v02, T v11, T v12, T v21, T v22, T g[6]) { - assert(p.size() == 2 || p.size() == 3); - assert(e0.size() == 2 || e0.size() == 3); - assert(e1.size() == 2 || e1.size() == 3); + T t13, t14, t23, t25, t24, t26, t27; - if (p.size() == 2) { - const Eigen::Vector2d e = e1 - e0; - const double numerator = - (e[1] * p[0] - e[0] * p[1] + e1[0] * e0[1] - e1[1] * e0[0]); - return numerator * numerator / e.squaredNorm(); - } else { - return (e0 - p).head<3>().cross((e1 - p).head<3>()).squaredNorm() - / (e1 - e0).squaredNorm(); - } + t13 = -v21 + v11; + t14 = -v22 + v12; + t23 = T(1) / (t13 * t13 + t14 * t14); + t25 = ((v11 * v22 + -(v12 * v21)) + t14 * v01) + -(t13 * v02); + t24 = t23 * t23; + t26 = t25 * t25; + t27 = (v11 * T(2) + -(v21 * T(2))) * t24 * t26; + t26 *= (v12 * T(2) + -(v22 * T(2))) * t24; + g[0] = t14 * t23 * t25 * T(2); + g[1] = t13 * t23 * t25 * -T(2); + t24 = t23 * t25; + g[2] = -t27 - t24 * (-v22 + v02) * T(2); + g[3] = -t26 + t24 * (-v21 + v01) * T(2); + g[4] = t27 + t24 * (v02 - v12) * T(2); + g[5] = t26 - t24 * (v01 - v11) * T(2); } -VectorMax9d point_line_distance_gradient( - Eigen::ConstRef p, - Eigen::ConstRef e0, - Eigen::ConstRef e1) +// This function was generated by the Symbolic Math Toolbox version 8.3. +// 10-Jun-2019 18:02:37 +template +void point_line_distance_gradient_3D( + T v01, T v02, T v03, T v11, T v12, T v13, T v21, T v22, T v23, T g[9]) { - const int dim = p.size(); - assert(e0.size() == dim); - assert(e1.size() == dim); + T t17, t18, t19, t20, t21, t22, t23, t24, t25, t42, t44, t45, t46, t43, t50, + t51, t52; - VectorMax9d grad(3 * dim); - if (dim == 2) { - autogen::point_line_distance_gradient_2D( - p[0], p[1], e0[0], e0[1], e1[0], e1[1], grad.data()); - } else { - autogen::point_line_distance_gradient_3D( - p[0], p[1], p[2], e0[0], e0[1], e0[2], e1[0], e1[1], e1[2], - grad.data()); - } - return grad; + t17 = -v11 + v01; + t18 = -v12 + v02; + t19 = -v13 + v03; + t20 = -v21 + v01; + t21 = -v22 + v02; + t22 = -v23 + v03; + t23 = -v21 + v11; + t24 = -v22 + v12; + t25 = -v23 + v13; + t42 = T(1) / ((t23 * t23 + t24 * t24) + t25 * t25); + t44 = t17 * t21 + -(t18 * t20); + t45 = t17 * t22 + -(t19 * t20); + t46 = t18 * t22 + -(t19 * t21); + t43 = t42 * t42; + t50 = (t44 * t44 + t45 * t45) + t46 * t46; + t51 = (v11 * T(2) + -(v21 * T(2))) * t43 * t50; + t52 = (v12 * T(2) + -(v22 * T(2))) * t43 * t50; + t43 = (v13 * T(2) + -(v23 * T(2))) * t43 * t50; + g[0] = t42 * (t24 * t44 * T(2) + t25 * t45 * T(2)); + g[1] = -t42 * (t23 * t44 * T(2) - t25 * t46 * T(2)); + g[2] = -t42 * (t23 * t45 * T(2) + t24 * t46 * T(2)); + g[3] = -t51 - t42 * (t21 * t44 * T(2) + t22 * t45 * T(2)); + g[4] = -t52 + t42 * (t20 * t44 * T(2) - t22 * t46 * T(2)); + g[5] = -t43 + t42 * (t20 * t45 * T(2) + t21 * t46 * T(2)); + g[6] = t51 + t42 * (t18 * t44 * T(2) + t19 * t45 * T(2)); + g[7] = t52 - t42 * (t17 * t44 * T(2) - t19 * t46 * T(2)); + g[8] = t43 - t42 * (t17 * t45 * T(2) + t18 * t46 * T(2)); } -MatrixMax9d point_line_distance_hessian( - Eigen::ConstRef p, - Eigen::ConstRef e0, - Eigen::ConstRef e1) +// This function was generated by the Symbolic Math Toolbox version 8.3. +// 16-Mar-2020 15:56:29 +template +void point_line_distance_hessian_2D( + T v01, T v02, T v11, T v12, T v21, T v22, T H[36]) { - const int dim = p.size(); - assert(e0.size() == dim); - assert(e1.size() == dim); + T t15, t16, t17, t18, t19, t20, t21, t22, t23, t24, t31, t34, t32, t33, t35, + t60, t59, t62, t64, t65, t68, t71, t72, t75, t76, t77, t78, t79, t90, + t92, t94, t96, t99, t93, t97, t98, t100, t102_tmp; - MatrixMax9d hess(3 * dim, 3 * dim); - if (dim == 2) { - autogen::point_line_distance_hessian_2D( - p[0], p[1], e0[0], e0[1], e1[0], e1[1], hess.data()); - } else { - autogen::point_line_distance_hessian_3D( - p[0], p[1], p[2], e0[0], e0[1], e0[2], e1[0], e1[1], e1[2], - hess.data()); - } - return hess; + t15 = -v11 + v01; + t16 = -v12 + v02; + t17 = -v21 + v01; + t18 = -v22 + v02; + t19 = -v21 + v11; + t20 = -v22 + v12; + t21 = v11 * T(2) + -(v21 * T(2)); + t22 = v12 * T(2) + -(v22 * T(2)); + t23 = t19 * t19; + t24 = t20 * t20; + t31 = T(1) / (t23 + t24); + t34 = ((v11 * v22 + -(v12 * v21)) + t20 * v01) + -(t19 * v02); + t32 = t31 * t31; + t33 = t31 * t31 * t31; + t35 = t34 * t34; + t60 = t31 * t34 * T(2); + t59 = -(t19 * t20 * t31 * T(2)); + t62 = t32 * t35 * T(2); + t64 = t21 * t21 * t33 * t35 * T(2); + t65 = t22 * t22 * t33 * t35 * T(2); + t68 = t15 * t21 * t32 * t34 * T(2); + t71 = t16 * t22 * t32 * t34 * T(2); + t72 = t17 * t21 * t32 * t34 * T(2); + t75 = t18 * t22 * t32 * t34 * T(2); + t76 = t19 * t21 * t32 * t34 * T(2); + t77 = t20 * t21 * t32 * t34 * T(2); + t78 = t19 * t22 * t32 * t34 * T(2); + t79 = t20 * t22 * t32 * t34 * T(2); + t90 = t21 * t22 * t33 * t35 * T(2); + t92 = t16 * t20 * t31 * T(2) + t77; + t94 = -(t17 * t19 * t31 * T(2)) + t78; + t96 = (t18 * t19 * t31 * T(2) + -t60) + t76; + t99 = (-(t15 * t20 * t31 * T(2)) + -t60) + t79; + t93 = t15 * t19 * t31 * T(2) + -t78; + t35 = -(t18 * t20 * t31 * T(2)) + -t77; + t97 = (t17 * t20 * t31 * T(2) + t60) + -t79; + t98 = (-(t16 * t19 * t31 * T(2)) + t60) + -t76; + t100 = ((-(t15 * t16 * t31 * T(2)) + t71) + -t68) + t90; + t19 = ((-(t17 * t18 * t31 * T(2)) + t75) + -t72) + t90; + t102_tmp = t17 * t22 * t32 * t34; + t76 = t15 * t22 * t32 * t34; + t22 = (((-(t15 * t17 * t31 * T(2)) + t62) + -t65) + t76 * T(2)) + + t102_tmp * T(2); + t33 = t18 * t21 * t32 * t34; + t20 = t16 * t21 * t32 * t34; + t79 = (((-(t16 * t18 * t31 * T(2)) + t62) + -t64) + -(t20 * T(2))) + + -(t33 * T(2)); + t77 = (((t15 * t18 * t31 * T(2) + t60) + t68) + -t75) + -t90; + t78 = (((t16 * t17 * t31 * T(2) + -t60) + t72) + -t71) + -t90; + H[0] = t24 * t31 * T(2); + H[1] = t59; + H[2] = t35; + H[3] = t97; + H[4] = t92; + H[5] = t99; + H[6] = t59; + H[7] = t23 * t31 * T(2); + H[8] = t96; + H[9] = t94; + H[10] = t98; + H[11] = t93; + H[12] = t35; + H[13] = t96; + t35 = -t62 + t64; + H[14] = (t35 + t18 * t18 * t31 * T(2)) + t33 * T(4); + H[15] = t19; + H[16] = t79; + H[17] = t77; + H[18] = t97; + H[19] = t94; + H[20] = t19; + t33 = -t62 + t65; + H[21] = (t33 + t17 * t17 * t31 * T(2)) - t102_tmp * T(4); + H[22] = t78; + H[23] = t22; + H[24] = t92; + H[25] = t98; + H[26] = t79; + H[27] = t78; + H[28] = (t35 + t16 * t16 * t31 * T(2)) + t20 * T(4); + H[29] = t100; + H[30] = t99; + H[31] = t93; + H[32] = t77; + H[33] = t22; + H[34] = t100; + H[35] = (t33 + t15 * t15 * t31 * T(2)) - t76 * T(4); } -namespace autogen { - - // This function was generated by the Symbolic Math Toolbox version 8.3. - // 16-Mar-2020 15:56:29 - void point_line_distance_gradient_2D( - double v01, - double v02, - double v11, - double v12, - double v21, - double v22, - double g[6]) - { - double t13, t14, t23, t25, t24, t26, t27; - - t13 = -v21 + v11; - t14 = -v22 + v12; - t23 = 1.0 / (t13 * t13 + t14 * t14); - t25 = ((v11 * v22 + -(v12 * v21)) + t14 * v01) + -(t13 * v02); - t24 = t23 * t23; - t26 = t25 * t25; - t27 = (v11 * 2.0 + -(v21 * 2.0)) * t24 * t26; - t26 *= (v12 * 2.0 + -(v22 * 2.0)) * t24; - g[0] = t14 * t23 * t25 * 2.0; - g[1] = t13 * t23 * t25 * -2.0; - t24 = t23 * t25; - g[2] = -t27 - t24 * (-v22 + v02) * 2.0; - g[3] = -t26 + t24 * (-v21 + v01) * 2.0; - g[4] = t27 + t24 * (v02 - v12) * 2.0; - g[5] = t26 - t24 * (v01 - v11) * 2.0; - } - - // This function was generated by the Symbolic Math Toolbox version 8.3. - // 10-Jun-2019 18:02:37 - void point_line_distance_gradient_3D( - double v01, - double v02, - double v03, - double v11, - double v12, - double v13, - double v21, - double v22, - double v23, - double g[9]) - { - double t17, t18, t19, t20, t21, t22, t23, t24, t25, t42, t44, t45, t46, - t43, t50, t51, t52; - - t17 = -v11 + v01; - t18 = -v12 + v02; - t19 = -v13 + v03; - t20 = -v21 + v01; - t21 = -v22 + v02; - t22 = -v23 + v03; - t23 = -v21 + v11; - t24 = -v22 + v12; - t25 = -v23 + v13; - t42 = 1.0 / ((t23 * t23 + t24 * t24) + t25 * t25); - t44 = t17 * t21 + -(t18 * t20); - t45 = t17 * t22 + -(t19 * t20); - t46 = t18 * t22 + -(t19 * t21); - t43 = t42 * t42; - t50 = (t44 * t44 + t45 * t45) + t46 * t46; - t51 = (v11 * 2.0 + -(v21 * 2.0)) * t43 * t50; - t52 = (v12 * 2.0 + -(v22 * 2.0)) * t43 * t50; - t43 = (v13 * 2.0 + -(v23 * 2.0)) * t43 * t50; - g[0] = t42 * (t24 * t44 * 2.0 + t25 * t45 * 2.0); - g[1] = -t42 * (t23 * t44 * 2.0 - t25 * t46 * 2.0); - g[2] = -t42 * (t23 * t45 * 2.0 + t24 * t46 * 2.0); - g[3] = -t51 - t42 * (t21 * t44 * 2.0 + t22 * t45 * 2.0); - g[4] = -t52 + t42 * (t20 * t44 * 2.0 - t22 * t46 * 2.0); - g[5] = -t43 + t42 * (t20 * t45 * 2.0 + t21 * t46 * 2.0); - g[6] = t51 + t42 * (t18 * t44 * 2.0 + t19 * t45 * 2.0); - g[7] = t52 - t42 * (t17 * t44 * 2.0 - t19 * t46 * 2.0); - g[8] = t43 - t42 * (t17 * t45 * 2.0 + t18 * t46 * 2.0); - } - - // This function was generated by the Symbolic Math Toolbox version 8.3. - // 16-Mar-2020 15:56:29 - void point_line_distance_hessian_2D( - double v01, - double v02, - double v11, - double v12, - double v21, - double v22, - double H[36]) - { - double t15, t16, t17, t18, t19, t20, t21, t22, t23, t24, t31, t34, t32, - t33, t35, t60, t59, t62, t64, t65, t68, t71, t72, t75, t76, t77, - t78, t79, t90, t92, t94, t96, t99, t93, t97, t98, t100, t102_tmp; +// This function was generated by the Symbolic Math Toolbox version 8.3. +// 10-Jun-2019 18:02:39 +template +void point_line_distance_hessian_3D( + T v01, T v02, T v03, T v11, T v12, T v13, T v21, T v22, T v23, T H[81]) +{ + T t17, t18, t19, t20, t21, t22, t23, t24, t25, t26, t27, t28, t35, t36, t37, + t50, t51, t52, t53, t54, t55, t56, t62, t70, t71, t75, t79, t80, t84, + t88, t38, t39, t40, t41, t42, t43, t44, t46, t48, t57, t58, t60, t63, + t65, t67, t102, t103, t104, t162, t163, t164, t213, t214, t215, t216, + t217, t218, t225, t226, t227, t229, t230, t311, t231, t232, t233, t234, + t235, t236, t237, t238, t239, t240, t245, t279, t281, t282, t283, t287, + t289, t247, t248, t249, t250, t251, t252, t253, t293, t295, t299, t300, + t303, t304, t294, t297, t301, t302; - t15 = -v11 + v01; - t16 = -v12 + v02; - t17 = -v21 + v01; - t18 = -v22 + v02; - t19 = -v21 + v11; - t20 = -v22 + v12; - t21 = v11 * 2.0 + -(v21 * 2.0); - t22 = v12 * 2.0 + -(v22 * 2.0); - t23 = t19 * t19; - t24 = t20 * t20; - t31 = 1.0 / (t23 + t24); - t34 = ((v11 * v22 + -(v12 * v21)) + t20 * v01) + -(t19 * v02); - t32 = t31 * t31; - t33 = pow(t31, 3.0); - t35 = t34 * t34; - t60 = t31 * t34 * 2.0; - t59 = -(t19 * t20 * t31 * 2.0); - t62 = t32 * t35 * 2.0; - t64 = t21 * t21 * t33 * t35 * 2.0; - t65 = t22 * t22 * t33 * t35 * 2.0; - t68 = t15 * t21 * t32 * t34 * 2.0; - t71 = t16 * t22 * t32 * t34 * 2.0; - t72 = t17 * t21 * t32 * t34 * 2.0; - t75 = t18 * t22 * t32 * t34 * 2.0; - t76 = t19 * t21 * t32 * t34 * 2.0; - t77 = t20 * t21 * t32 * t34 * 2.0; - t78 = t19 * t22 * t32 * t34 * 2.0; - t79 = t20 * t22 * t32 * t34 * 2.0; - t90 = t21 * t22 * t33 * t35 * 2.0; - t92 = t16 * t20 * t31 * 2.0 + t77; - t94 = -(t17 * t19 * t31 * 2.0) + t78; - t96 = (t18 * t19 * t31 * 2.0 + -t60) + t76; - t99 = (-(t15 * t20 * t31 * 2.0) + -t60) + t79; - t93 = t15 * t19 * t31 * 2.0 + -t78; - t35 = -(t18 * t20 * t31 * 2.0) + -t77; - t97 = (t17 * t20 * t31 * 2.0 + t60) + -t79; - t98 = (-(t16 * t19 * t31 * 2.0) + t60) + -t76; - t100 = ((-(t15 * t16 * t31 * 2.0) + t71) + -t68) + t90; - t19 = ((-(t17 * t18 * t31 * 2.0) + t75) + -t72) + t90; - t102_tmp = t17 * t22 * t32 * t34; - t76 = t15 * t22 * t32 * t34; - t22 = (((-(t15 * t17 * t31 * 2.0) + t62) + -t65) + t76 * 2.0) - + t102_tmp * 2.0; - t33 = t18 * t21 * t32 * t34; - t20 = t16 * t21 * t32 * t34; - t79 = (((-(t16 * t18 * t31 * 2.0) + t62) + -t64) + -(t20 * 2.0)) - + -(t33 * 2.0); - t77 = (((t15 * t18 * t31 * 2.0 + t60) + t68) + -t75) + -t90; - t78 = (((t16 * t17 * t31 * 2.0 + -t60) + t72) + -t71) + -t90; - H[0] = t24 * t31 * 2.0; - H[1] = t59; - H[2] = t35; - H[3] = t97; - H[4] = t92; - H[5] = t99; - H[6] = t59; - H[7] = t23 * t31 * 2.0; - H[8] = t96; - H[9] = t94; - H[10] = t98; - H[11] = t93; - H[12] = t35; - H[13] = t96; - t35 = -t62 + t64; - H[14] = (t35 + t18 * t18 * t31 * 2.0) + t33 * 4.0; - H[15] = t19; - H[16] = t79; - H[17] = t77; - H[18] = t97; - H[19] = t94; - H[20] = t19; - t33 = -t62 + t65; - H[21] = (t33 + t17 * t17 * t31 * 2.0) - t102_tmp * 4.0; - H[22] = t78; - H[23] = t22; - H[24] = t92; - H[25] = t98; - H[26] = t79; - H[27] = t78; - H[28] = (t35 + t16 * t16 * t31 * 2.0) + t20 * 4.0; - H[29] = t100; - H[30] = t99; - H[31] = t93; - H[32] = t77; - H[33] = t22; - H[34] = t100; - H[35] = (t33 + t15 * t15 * t31 * 2.0) - t76 * 4.0; - } + t17 = -v11 + v01; + t18 = -v12 + v02; + t19 = -v13 + v03; + t20 = -v21 + v01; + t21 = -v22 + v02; + t22 = -v23 + v03; + t23 = -v21 + v11; + t24 = -v22 + v12; + t25 = -v23 + v13; + t26 = v11 * T(2) + -(v21 * T(2)); + t27 = v12 * T(2) + -(v22 * T(2)); + t28 = v13 * T(2) + -(v23 * T(2)); + t35 = t23 * t23; + t36 = t24 * t24; + t37 = t25 * t25; + t50 = t17 * t21; + t51 = t18 * t20; + t52 = t17 * t22; + t53 = t19 * t20; + t54 = t18 * t22; + t55 = t19 * t21; + t56 = t17 * t20 * T(2); + t62 = t18 * t21 * T(2); + t70 = t19 * t22 * T(2); + t71 = t17 * t23 * T(2); + t75 = t18 * t24 * T(2); + t79 = t19 * t25 * T(2); + t80 = t20 * t23 * T(2); + t84 = t21 * t24 * T(2); + t88 = t22 * t25 * T(2); + t38 = t17 * t17 * T(2); + t39 = t18 * t18 * T(2); + t40 = t19 * t19 * T(2); + t41 = t20 * t20 * T(2); + t42 = t21 * t21 * T(2); + t43 = t22 * t22 * T(2); + t44 = t35 * T(2); + t46 = t36 * T(2); + t48 = t37 * T(2); + t57 = t50 * T(2); + t58 = t51 * T(2); + t60 = t52 * T(2); + t63 = t53 * T(2); + t65 = t54 * T(2); + t67 = t55 * T(2); + t102 = T(1) / ((t35 + t36) + t37); + t36 = t50 + -t51; + t35 = t52 + -t53; + t37 = t54 + -t55; + t103 = t102 * t102; + t104 = t102 * t102 * t102; + t162 = -(t23 * t24 * t102 * T(2)); + t163 = -(t23 * t25 * t102 * T(2)); + t164 = -(t24 * t25 * t102 * T(2)); + t213 = t18 * t36 * T(2) + t19 * t35 * T(2); + t214 = t17 * t35 * T(2) + t18 * t37 * T(2); + t215 = t21 * t36 * T(2) + t22 * t35 * T(2); + t216 = t20 * t35 * T(2) + t21 * t37 * T(2); + t217 = t24 * t36 * T(2) + t25 * t35 * T(2); + t218 = t23 * t35 * T(2) + t24 * t37 * T(2); + t35 = (t36 * t36 + t35 * t35) + t37 * t37; + t225 = t17 * t36 * T(2) + -(t19 * t37 * T(2)); + t226 = t20 * t36 * T(2) + -(t22 * t37 * T(2)); + t227 = t23 * t36 * T(2) + -(t25 * t37 * T(2)); + t36 = t26 * t103; + t229 = t36 * t213; + t37 = t27 * t103; + t230 = t37 * t213; + t311 = t28 * t103; + t231 = t311 * t213; + t232 = t36 * t214; + t233 = t37 * t214; + t234 = t311 * t214; + t235 = t36 * t215; + t236 = t37 * t215; + t237 = t311 * t215; + t238 = t36 * t216; + t239 = t37 * t216; + t240 = t311 * t216; + t214 = t36 * t217; + t215 = t37 * t217; + t216 = t311 * t217; + t217 = t36 * t218; + t245 = t37 * t218; + t213 = t311 * t218; + t279 = t103 * t35 * T(2); + t281 = t26 * t26 * t104 * t35 * T(2); + t282 = t27 * t27 * t104 * t35 * T(2); + t283 = t28 * t28 * t104 * t35 * T(2); + t287 = t26 * t27 * t104 * t35 * T(2); + t218 = t26 * t28 * t104 * t35 * T(2); + t289 = t27 * t28 * t104 * t35 * T(2); + t247 = t36 * t225; + t248 = t37 * t225; + t249 = t311 * t225; + t250 = t36 * t226; + t251 = t37 * t226; + t252 = t311 * t226; + t253 = t36 * t227; + t35 = t37 * t227; + t36 = t311 * t227; + t293 = t102 * (t75 + t79) + t214; + t295 = -(t102 * (t80 + t84)) + t213; + t299 = t102 * ((t63 + t22 * t23 * T(2)) + -t60) + t217; + t300 = t102 * ((t67 + t22 * t24 * T(2)) + -t65) + t245; + t303 = -(t102 * ((t57 + t17 * t24 * T(2)) + -t58)) + t215; + t304 = -(t102 * ((t60 + t17 * t25 * T(2)) + -t63)) + t216; + t294 = t102 * (t71 + t75) + -t213; + t297 = -(t102 * (t80 + t88)) + t35; + t88 = -(t102 * (t84 + t88)) + -t214; + t301 = t102 * ((t58 + t21 * t23 * T(2)) + -t57) + t253; + t302 = t102 * ((t65 + t21 * t25 * T(2)) + -t67) + t36; + t84 = t102 * ((t57 + t20 * t24 * T(2)) + -t58) + -t215; + t80 = t102 * ((t60 + t20 * t25 * T(2)) + -t63) + -t216; + t75 = -(t102 * ((t63 + t19 * t23 * T(2)) + -t60)) + -t217; + t227 = -(t102 * ((t67 + t19 * t24 * T(2)) + -t65)) + -t245; + t311 = ((-(t17 * t19 * t102 * T(2)) + t231) + -t232) + t218; + t245 = ((-(t20 * t22 * t102 * T(2)) + t237) + -t238) + t218; + t226 = ((-t102 * (t67 - t54 * T(4)) + t233) + t252) + -t289; + t28 = ((-t102 * (t63 - t52 * T(4)) + t232) + -t237) + -t218; + t27 = ((-t102 * (t58 - t50 * T(4)) + t247) + -t236) + -t287; + t225 = ((-(t102 * (t65 + -(t55 * T(4)))) + t239) + t249) + -t289; + t26 = ((-(t102 * (t60 + -(t53 * T(4)))) + t238) + -t231) + -t218; + t103 = ((-(t102 * (t57 + -(t51 * T(4)))) + t250) + -t230) + -t287; + t104 = (((-(t102 * (t56 + t62)) + t234) + t240) + t279) + -t283; + t218 = (((-(t102 * (t56 + t70)) + t248) + t251) + t279) + -t282; + t217 = (((-(t102 * (t62 + t70)) + -t229) + -t235) + t279) + -t281; + t216 = t102 * (t71 + t79) + -t35; + t215 = -(t102 * ((t58 + t18 * t23 * T(2)) + -t57)) + -t253; + t214 = -(t102 * ((t65 + t18 * t25 * T(2)) + -t67)) + -t36; + t213 = ((-(t17 * t18 * t102 * T(2)) + t230) + -t247) + t287; + t37 = ((-(t20 * t21 * t102 * T(2)) + t236) + -t250) + t287; + t36 = ((-(t18 * t19 * t102 * T(2)) + -t233) + -t249) + t289; + t35 = ((-(t21 * t22 * t102 * T(2)) + -t239) + -t252) + t289; + H[0] = t102 * (t46 + t48); + H[1] = t162; + H[2] = t163; + H[3] = t88; + H[4] = t84; + H[5] = t80; + H[6] = t293; + H[7] = t303; + H[8] = t304; + H[9] = t162; + H[10] = t102 * (t44 + t48); + H[11] = t164; + H[12] = t301; + H[13] = t297; + H[14] = t302; + H[15] = t215; + H[16] = t216; + H[17] = t214; + H[18] = t163; + H[19] = t164; + H[20] = t102 * (t44 + t46); + H[21] = t299; + H[22] = t300; + H[23] = t295; + H[24] = t75; + H[25] = t227; + H[26] = t294; + H[27] = t88; + H[28] = t301; + H[29] = t299; + H[30] = ((t235 * T(2) + -t279) + t281) + t102 * (t42 + t43); + H[31] = t37; + H[32] = t245; + H[33] = t217; + H[34] = t27; + H[35] = t28; + H[36] = t84; + H[37] = t297; + H[38] = t300; + H[39] = t37; + H[40] = ((t251 * -T(2) + -t279) + t282) + t102 * (t41 + t43); + H[41] = t35; + H[42] = t103; + H[43] = t218; + H[44] = t226; + H[45] = t80; + H[46] = t302; + H[47] = t295; + H[48] = t245; + H[49] = t35; + H[50] = ((t240 * -T(2) + -t279) + t283) + t102 * (t41 + t42); + H[51] = t26; + H[52] = t225; + H[53] = t104; + H[54] = t293; + H[55] = t215; + H[56] = t75; + H[57] = t217; + H[58] = t103; + H[59] = t26; + H[60] = ((t229 * T(2) + -t279) + t281) + t102 * (t39 + t40); + H[61] = t213; + H[62] = t311; + H[63] = t303; + H[64] = t216; + H[65] = t227; + H[66] = t27; + H[67] = t218; + H[68] = t225; + H[69] = t213; + H[70] = ((t248 * -T(2) + -t279) + t282) + t102 * (t38 + t40); + H[71] = t36; + H[72] = t304; + H[73] = t214; + H[74] = t294; + H[75] = t28; + H[76] = t226; + H[77] = t104; + H[78] = t311; + H[79] = t36; + H[80] = ((t234 * -T(2) + -t279) + t283) + t102 * (t38 + t39); +} - // This function was generated by the Symbolic Math Toolbox version 8.3. - // 10-Jun-2019 18:02:39 - void point_line_distance_hessian_3D( - double v01, - double v02, - double v03, - double v11, - double v12, - double v13, - double v21, - double v22, - double v23, - double H[81]) - { - double t17, t18, t19, t20, t21, t22, t23, t24, t25, t26, t27, t28, t35, - t36, t37, t50, t51, t52, t53, t54, t55, t56, t62, t70, t71, t75, - t79, t80, t84, t88, t38, t39, t40, t41, t42, t43, t44, t46, t48, - t57, t58, t60, t63, t65, t67, t102, t103, t104, t162, t163, t164, - t213, t214, t215, t216, t217, t218, t225, t226, t227, t229, t230, - t311, t231, t232, t233, t234, t235, t236, t237, t238, t239, t240, - t245, t279, t281, t282, t283, t287, t289, t247, t248, t249, t250, - t251, t252, t253, t293, t295, t299, t300, t303, t304, t294, t297, - t301, t302; +#define IPC_INSTANTIATE_POINT_LINE_AUTOGEN(T) \ + template void point_line_distance_gradient_2D(T, T, T, T, T, T, T[6]); \ + template void point_line_distance_gradient_3D( \ + T, T, T, T, T, T, T, T, T, T[9]); \ + template void point_line_distance_hessian_2D(T, T, T, T, T, T, T[36]); \ + template void point_line_distance_hessian_3D( \ + T, T, T, T, T, T, T, T, T, T[81]) - t17 = -v11 + v01; - t18 = -v12 + v02; - t19 = -v13 + v03; - t20 = -v21 + v01; - t21 = -v22 + v02; - t22 = -v23 + v03; - t23 = -v21 + v11; - t24 = -v22 + v12; - t25 = -v23 + v13; - t26 = v11 * 2.0 + -(v21 * 2.0); - t27 = v12 * 2.0 + -(v22 * 2.0); - t28 = v13 * 2.0 + -(v23 * 2.0); - t35 = t23 * t23; - t36 = t24 * t24; - t37 = t25 * t25; - t50 = t17 * t21; - t51 = t18 * t20; - t52 = t17 * t22; - t53 = t19 * t20; - t54 = t18 * t22; - t55 = t19 * t21; - t56 = t17 * t20 * 2.0; - t62 = t18 * t21 * 2.0; - t70 = t19 * t22 * 2.0; - t71 = t17 * t23 * 2.0; - t75 = t18 * t24 * 2.0; - t79 = t19 * t25 * 2.0; - t80 = t20 * t23 * 2.0; - t84 = t21 * t24 * 2.0; - t88 = t22 * t25 * 2.0; - t38 = t17 * t17 * 2.0; - t39 = t18 * t18 * 2.0; - t40 = t19 * t19 * 2.0; - t41 = t20 * t20 * 2.0; - t42 = t21 * t21 * 2.0; - t43 = t22 * t22 * 2.0; - t44 = t35 * 2.0; - t46 = t36 * 2.0; - t48 = t37 * 2.0; - t57 = t50 * 2.0; - t58 = t51 * 2.0; - t60 = t52 * 2.0; - t63 = t53 * 2.0; - t65 = t54 * 2.0; - t67 = t55 * 2.0; - t102 = 1.0 / ((t35 + t36) + t37); - t36 = t50 + -t51; - t35 = t52 + -t53; - t37 = t54 + -t55; - t103 = t102 * t102; - t104 = pow(t102, 3.0); - t162 = -(t23 * t24 * t102 * 2.0); - t163 = -(t23 * t25 * t102 * 2.0); - t164 = -(t24 * t25 * t102 * 2.0); - t213 = t18 * t36 * 2.0 + t19 * t35 * 2.0; - t214 = t17 * t35 * 2.0 + t18 * t37 * 2.0; - t215 = t21 * t36 * 2.0 + t22 * t35 * 2.0; - t216 = t20 * t35 * 2.0 + t21 * t37 * 2.0; - t217 = t24 * t36 * 2.0 + t25 * t35 * 2.0; - t218 = t23 * t35 * 2.0 + t24 * t37 * 2.0; - t35 = (t36 * t36 + t35 * t35) + t37 * t37; - t225 = t17 * t36 * 2.0 + -(t19 * t37 * 2.0); - t226 = t20 * t36 * 2.0 + -(t22 * t37 * 2.0); - t227 = t23 * t36 * 2.0 + -(t25 * t37 * 2.0); - t36 = t26 * t103; - t229 = t36 * t213; - t37 = t27 * t103; - t230 = t37 * t213; - t311 = t28 * t103; - t231 = t311 * t213; - t232 = t36 * t214; - t233 = t37 * t214; - t234 = t311 * t214; - t235 = t36 * t215; - t236 = t37 * t215; - t237 = t311 * t215; - t238 = t36 * t216; - t239 = t37 * t216; - t240 = t311 * t216; - t214 = t36 * t217; - t215 = t37 * t217; - t216 = t311 * t217; - t217 = t36 * t218; - t245 = t37 * t218; - t213 = t311 * t218; - t279 = t103 * t35 * 2.0; - t281 = t26 * t26 * t104 * t35 * 2.0; - t282 = t27 * t27 * t104 * t35 * 2.0; - t283 = t28 * t28 * t104 * t35 * 2.0; - t287 = t26 * t27 * t104 * t35 * 2.0; - t218 = t26 * t28 * t104 * t35 * 2.0; - t289 = t27 * t28 * t104 * t35 * 2.0; - t247 = t36 * t225; - t248 = t37 * t225; - t249 = t311 * t225; - t250 = t36 * t226; - t251 = t37 * t226; - t252 = t311 * t226; - t253 = t36 * t227; - t35 = t37 * t227; - t36 = t311 * t227; - t293 = t102 * (t75 + t79) + t214; - t295 = -(t102 * (t80 + t84)) + t213; - t299 = t102 * ((t63 + t22 * t23 * 2.0) + -t60) + t217; - t300 = t102 * ((t67 + t22 * t24 * 2.0) + -t65) + t245; - t303 = -(t102 * ((t57 + t17 * t24 * 2.0) + -t58)) + t215; - t304 = -(t102 * ((t60 + t17 * t25 * 2.0) + -t63)) + t216; - t294 = t102 * (t71 + t75) + -t213; - t297 = -(t102 * (t80 + t88)) + t35; - t88 = -(t102 * (t84 + t88)) + -t214; - t301 = t102 * ((t58 + t21 * t23 * 2.0) + -t57) + t253; - t302 = t102 * ((t65 + t21 * t25 * 2.0) + -t67) + t36; - t84 = t102 * ((t57 + t20 * t24 * 2.0) + -t58) + -t215; - t80 = t102 * ((t60 + t20 * t25 * 2.0) + -t63) + -t216; - t75 = -(t102 * ((t63 + t19 * t23 * 2.0) + -t60)) + -t217; - t227 = -(t102 * ((t67 + t19 * t24 * 2.0) + -t65)) + -t245; - t311 = ((-(t17 * t19 * t102 * 2.0) + t231) + -t232) + t218; - t245 = ((-(t20 * t22 * t102 * 2.0) + t237) + -t238) + t218; - t226 = ((-t102 * (t67 - t54 * 4.0) + t233) + t252) + -t289; - t28 = ((-t102 * (t63 - t52 * 4.0) + t232) + -t237) + -t218; - t27 = ((-t102 * (t58 - t50 * 4.0) + t247) + -t236) + -t287; - t225 = ((-(t102 * (t65 + -(t55 * 4.0))) + t239) + t249) + -t289; - t26 = ((-(t102 * (t60 + -(t53 * 4.0))) + t238) + -t231) + -t218; - t103 = ((-(t102 * (t57 + -(t51 * 4.0))) + t250) + -t230) + -t287; - t104 = (((-(t102 * (t56 + t62)) + t234) + t240) + t279) + -t283; - t218 = (((-(t102 * (t56 + t70)) + t248) + t251) + t279) + -t282; - t217 = (((-(t102 * (t62 + t70)) + -t229) + -t235) + t279) + -t281; - t216 = t102 * (t71 + t79) + -t35; - t215 = -(t102 * ((t58 + t18 * t23 * 2.0) + -t57)) + -t253; - t214 = -(t102 * ((t65 + t18 * t25 * 2.0) + -t67)) + -t36; - t213 = ((-(t17 * t18 * t102 * 2.0) + t230) + -t247) + t287; - t37 = ((-(t20 * t21 * t102 * 2.0) + t236) + -t250) + t287; - t36 = ((-(t18 * t19 * t102 * 2.0) + -t233) + -t249) + t289; - t35 = ((-(t21 * t22 * t102 * 2.0) + -t239) + -t252) + t289; - H[0] = t102 * (t46 + t48); - H[1] = t162; - H[2] = t163; - H[3] = t88; - H[4] = t84; - H[5] = t80; - H[6] = t293; - H[7] = t303; - H[8] = t304; - H[9] = t162; - H[10] = t102 * (t44 + t48); - H[11] = t164; - H[12] = t301; - H[13] = t297; - H[14] = t302; - H[15] = t215; - H[16] = t216; - H[17] = t214; - H[18] = t163; - H[19] = t164; - H[20] = t102 * (t44 + t46); - H[21] = t299; - H[22] = t300; - H[23] = t295; - H[24] = t75; - H[25] = t227; - H[26] = t294; - H[27] = t88; - H[28] = t301; - H[29] = t299; - H[30] = ((t235 * 2.0 + -t279) + t281) + t102 * (t42 + t43); - H[31] = t37; - H[32] = t245; - H[33] = t217; - H[34] = t27; - H[35] = t28; - H[36] = t84; - H[37] = t297; - H[38] = t300; - H[39] = t37; - H[40] = ((t251 * -2.0 + -t279) + t282) + t102 * (t41 + t43); - H[41] = t35; - H[42] = t103; - H[43] = t218; - H[44] = t226; - H[45] = t80; - H[46] = t302; - H[47] = t295; - H[48] = t245; - H[49] = t35; - H[50] = ((t240 * -2.0 + -t279) + t283) + t102 * (t41 + t42); - H[51] = t26; - H[52] = t225; - H[53] = t104; - H[54] = t293; - H[55] = t215; - H[56] = t75; - H[57] = t217; - H[58] = t103; - H[59] = t26; - H[60] = ((t229 * 2.0 + -t279) + t281) + t102 * (t39 + t40); - H[61] = t213; - H[62] = t311; - H[63] = t303; - H[64] = t216; - H[65] = t227; - H[66] = t27; - H[67] = t218; - H[68] = t225; - H[69] = t213; - H[70] = ((t248 * -2.0 + -t279) + t282) + t102 * (t38 + t40); - H[71] = t36; - H[72] = t304; - H[73] = t214; - H[74] = t294; - H[75] = t28; - H[76] = t226; - H[77] = t104; - H[78] = t311; - H[79] = t36; - H[80] = ((t234 * -2.0 + -t279) + t283) + t102 * (t38 + t39); - } +IPC_INSTANTIATE_POINT_LINE_AUTOGEN(float); +IPC_INSTANTIATE_POINT_LINE_AUTOGEN(double); +#undef IPC_INSTANTIATE_POINT_LINE_AUTOGEN -} // namespace autogen -} // namespace ipc +} // namespace ipc::autogen diff --git a/src/ipc/distance/point_line.hpp b/src/ipc/distance/point_line.hpp index 11d5ad162..988f53bfb 100644 --- a/src/ipc/distance/point_line.hpp +++ b/src/ipc/distance/point_line.hpp @@ -4,16 +4,131 @@ namespace ipc { +// Symbolically generated derivatives +namespace autogen { + template + void point_line_distance_gradient_2D( + T v01, T v02, T v11, T v12, T v21, T v22, T g[6]); + template + void point_line_distance_gradient_3D( + T v01, T v02, T v03, T v11, T v12, T v13, T v21, T v22, T v23, T g[9]); + template + void point_line_distance_hessian_2D( + T v01, T v02, T v11, T v12, T v21, T v22, T H[36]); + template + void point_line_distance_hessian_3D( + T v01, T v02, T v03, T v11, T v12, T v13, T v21, T v22, T v23, T H[81]); +} // namespace autogen + +namespace detail { + /// @brief Compute the distance between a point and line in 2D or 3D. + /// @note The distance is actually squared distance. + /// @tparam T The scalar type. + /// @tparam dim The dimension (2 or 3). + /// @param p The point. + /// @param e0 The first vertex of the edge defining the line. + /// @param e1 The second vertex of the edge defining the line. + /// @return The distance between the point and line. + template + inline T point_line_distance( + Eigen::ConstRef> p, + Eigen::ConstRef> e0, + Eigen::ConstRef> e1) + { + static_assert(dim == 2 || dim == 3, "point-line is only 2D or 3D"); + if constexpr (dim == 2) { + const Eigen::Vector2 e = e1 - e0; + const T numerator = + (e[1] * p[0] - e[0] * p[1] + e1[0] * e0[1] - e1[1] * e0[0]); + return numerator * numerator / e.squaredNorm(); + } else { + const Eigen::Vector3 p_to_e0 = e0 - p; + const Eigen::Vector3 p_to_e1 = e1 - p; + return p_to_e0.cross(p_to_e1).squaredNorm() + / (e1 - e0).squaredNorm(); + } + } + + /// @brief Compute the gradient of the distance between a point and line. + /// @note The distance is actually squared distance. + /// @tparam T The scalar type. + /// @tparam dim The dimension (2 or 3). + /// @param p The point. + /// @param e0 The first vertex of the edge defining the line. + /// @param e1 The second vertex of the edge defining the line. + /// @return The gradient of the distance wrt p, e0, and e1. + template + inline Eigen::Vector point_line_distance_gradient( + Eigen::ConstRef> p, + Eigen::ConstRef> e0, + Eigen::ConstRef> e1) + { + static_assert(dim == 2 || dim == 3, "point-line is only 2D or 3D"); + Eigen::Vector grad; + if constexpr (dim == 2) { + autogen::point_line_distance_gradient_2D( + p[0], p[1], e0[0], e0[1], e1[0], e1[1], grad.data()); + } else { + autogen::point_line_distance_gradient_3D( + p[0], p[1], p[2], e0[0], e0[1], e0[2], e1[0], e1[1], e1[2], + grad.data()); + } + return grad; + } + + /// @brief Compute the hessian of the distance between a point and line. + /// @note The distance is actually squared distance. + /// @tparam T The scalar type. + /// @tparam dim The dimension (2 or 3). + /// @param p The point. + /// @param e0 The first vertex of the edge defining the line. + /// @param e1 The second vertex of the edge defining the line. + /// @return The hessian of the distance wrt p, e0, and e1. + template + inline Eigen::Matrix point_line_distance_hessian( + Eigen::ConstRef> p, + Eigen::ConstRef> e0, + Eigen::ConstRef> e1) + { + static_assert(dim == 2 || dim == 3, "point-line is only 2D or 3D"); + Eigen::Matrix hess; + if constexpr (dim == 2) { + autogen::point_line_distance_hessian_2D( + p[0], p[1], e0[0], e0[1], e1[0], e1[1], hess.data()); + } else { + autogen::point_line_distance_hessian_3D( + p[0], p[1], p[2], e0[0], e0[1], e0[2], e1[0], e1[1], e1[2], + hess.data()); + } + return hess; + } +} // namespace detail + /// @brief Compute the distance between a point and line in 2D or 3D. /// @note The distance is actually squared distance. /// @param p The point. /// @param e0 The first vertex of the edge defining the line. /// @param e1 The second vertex of the edge defining the line. /// @return The distance between the point and line. -double point_line_distance( - Eigen::ConstRef p, - Eigen::ConstRef e0, - Eigen::ConstRef e1); +template +inline auto point_line_distance( + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& e0, + const Eigen::MatrixBase& e1) +{ + using T = typename DerivedP::Scalar; + + if constexpr (dim_v == 2) { + return detail::point_line_distance(p, e0, e1); + } else if constexpr (dim_v == 3) { + return detail::point_line_distance(p, e0, e1); + } else if (p.size() == 2) { + return detail::point_line_distance(p, e0, e1); + } else { + assert(p.size() == 3); + return detail::point_line_distance(p, e0, e1); + } +} /// @brief Compute the gradient of the distance between a point and line. /// @note The distance is actually squared distance. @@ -21,64 +136,63 @@ double point_line_distance( /// @param e0 The first vertex of the edge defining the line. /// @param e1 The second vertex of the edge defining the line. /// @return The gradient of the distance wrt p, e0, and e1. -VectorMax9d point_line_distance_gradient( - Eigen::ConstRef p, - Eigen::ConstRef e0, - Eigen::ConstRef e1); +template +inline auto point_line_distance_gradient( + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& e0, + const Eigen::MatrixBase& e1) +{ + using T = typename DerivedP::Scalar; + if constexpr (dim_v == 2) { + return detail::point_line_distance_gradient(p, e0, e1); + } else if constexpr (dim_v == 3) { + return detail::point_line_distance_gradient(p, e0, e1); + } else if (p.size() == 2) { + VectorMax9 grad(6); + autogen::point_line_distance_gradient_2D( + p[0], p[1], e0[0], e0[1], e1[0], e1[1], grad.data()); + return grad; + } else { + assert(p.size() == 3); + VectorMax9 grad(9); + autogen::point_line_distance_gradient_3D( + p[0], p[1], p[2], e0[0], e0[1], e0[2], e1[0], e1[1], e1[2], + grad.data()); + return grad; + } +} /// @brief Compute the hessian of the distance between a point and line. +/// /// @note The distance is actually squared distance. /// @param p The point. /// @param e0 The first vertex of the edge defining the line. /// @param e1 The second vertex of the edge defining the line. /// @return The hessian of the distance wrt p, e0, and e1. -MatrixMax9d point_line_distance_hessian( - Eigen::ConstRef p, - Eigen::ConstRef e0, - Eigen::ConstRef e1); - -// Symbolically generated derivatives; -namespace autogen { - void point_line_distance_gradient_2D( - double v01, - double v02, - double v11, - double v12, - double v21, - double v22, - double g[6]); - - void point_line_distance_gradient_3D( - double v01, - double v02, - double v03, - double v11, - double v12, - double v13, - double v21, - double v22, - double v23, - double g[9]); - - void point_line_distance_hessian_2D( - double v01, - double v02, - double v11, - double v12, - double v21, - double v22, - double H[36]); +template +inline auto point_line_distance_hessian( + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& e0, + const Eigen::MatrixBase& e1) +{ + using T = typename DerivedP::Scalar; + if constexpr (dim_v == 2) { + return detail::point_line_distance_hessian(p, e0, e1); + } else if constexpr (dim_v == 3) { + return detail::point_line_distance_hessian(p, e0, e1); + } else if (p.size() == 2) { + MatrixMax9 hess(6, 6); + autogen::point_line_distance_hessian_2D( + p[0], p[1], e0[0], e0[1], e1[0], e1[1], hess.data()); + return hess; + } else { + assert(p.size() == 3); + MatrixMax9 hess(9, 9); + autogen::point_line_distance_hessian_3D( + p[0], p[1], p[2], e0[0], e0[1], e0[2], e1[0], e1[1], e1[2], + hess.data()); + return hess; + } +} - void point_line_distance_hessian_3D( - double v01, - double v02, - double v03, - double v11, - double v12, - double v13, - double v21, - double v22, - double v23, - double H[81]); -} // namespace autogen } // namespace ipc diff --git a/src/ipc/distance/point_plane.cpp b/src/ipc/distance/point_plane.cpp index 5e77342e6..fb250630c 100644 --- a/src/ipc/distance/point_plane.cpp +++ b/src/ipc/distance/point_plane.cpp @@ -1,631 +1,571 @@ #include "point_plane.hpp" -#include -#include +namespace ipc::autogen { -#include - -namespace ipc { - -double point_plane_distance( - Eigen::ConstRef p, - Eigen::ConstRef origin, - Eigen::ConstRef normal) +// This function was generated by the Symbolic Math Toolbox version 8.3. +// 10-Jun-2019 17:42:16 +template +void point_plane_distance_gradient( + T v01, + T v02, + T v03, + T v11, + T v12, + T v13, + T v21, + T v22, + T v23, + T v31, + T v32, + T v33, + T g[12]) { - const double point_to_plane = (p - origin).dot(normal); - return point_to_plane * point_to_plane / normal.squaredNorm(); -} + T t11, t12, t13, t14, t15, t16, t17, t18, t19, t20, t21, t22, t32, t33, t34, + t43, t45, t44, t46; -double point_plane_distance( - Eigen::ConstRef p, - Eigen::ConstRef t0, - Eigen::ConstRef t1, - Eigen::ConstRef t2) -{ - return point_plane_distance(p, t0, triangle_normal(t0, t1, t2)); + t11 = -v11 + v01; + t12 = -v12 + v02; + t13 = -v13 + v03; + t14 = -v21 + v11; + t15 = -v22 + v12; + t16 = -v23 + v13; + t17 = -v31 + v11; + t18 = -v32 + v12; + t19 = -v33 + v13; + t20 = -v31 + v21; + t21 = -v32 + v22; + t22 = -v33 + v23; + t32 = t14 * t18 + -(t15 * t17); + t33 = t14 * t19 + -(t16 * t17); + t34 = t15 * t19 + -(t16 * t18); + t43 = T(1) / ((t32 * t32 + t33 * t33) + t34 * t34); + t45 = (t13 * t32 + t11 * t34) + -(t12 * t33); + t44 = t43 * t43; + t46 = t45 * t45; + g[0] = t34 * t43 * t45 * T(2); + g[1] = t33 * t43 * t45 * -T(2); + g[2] = t32 * t43 * t45 * T(2); + t45 *= t43; + g[3] = -t44 * t46 * (t21 * t32 * T(2) + t22 * t33 * T(2)) + - t45 * ((t34 + t12 * t22) - t13 * t21) * T(2); + t43 = t44 * t46; + g[4] = t43 * (t20 * t32 * T(2) - t22 * t34 * T(2)) + + t45 * ((t33 + t11 * t22) - t13 * t20) * T(2); + g[5] = t43 * (t20 * t33 * T(2) + t21 * t34 * T(2)) + - t45 * ((t32 + t11 * t21) - t12 * t20) * T(2); + g[6] = t45 * (t12 * t19 - t13 * t18) * T(2) + + t43 * (t18 * t32 * T(2) + t19 * t33 * T(2)); + g[7] = t45 * (t11 * t19 - t13 * t17) * -T(2) + - t43 * (t17 * t32 * T(2) - t19 * t34 * T(2)); + g[8] = t45 * (t11 * t18 - t12 * t17) * T(2) + - t43 * (t17 * t33 * T(2) + t18 * t34 * T(2)); + g[9] = t45 * (t12 * t16 - t13 * t15) * -T(2) + - t43 * (t15 * t32 * T(2) + t16 * t33 * T(2)); + g[10] = t45 * (t11 * t16 - t13 * t14) * T(2) + + t43 * (t14 * t32 * T(2) - t16 * t34 * T(2)); + g[11] = t45 * (t11 * t15 - t12 * t14) * -T(2) + + t43 * (t14 * t33 * T(2) + t15 * t34 * T(2)); } -Eigen::Vector3d point_plane_distance_gradient( - Eigen::ConstRef p, - Eigen::ConstRef origin, - Eigen::ConstRef normal) +// This function was generated by the Symbolic Math Toolbox version 8.3. +// 10-Jun-2019 17:42:25 +template +void point_plane_distance_hessian( + T v01, + T v02, + T v03, + T v11, + T v12, + T v13, + T v21, + T v22, + T v23, + T v31, + T v32, + T v33, + T H[144]) { - return (2 / normal.squaredNorm()) * (p - origin).dot(normal) * normal; -} + T t11, t12, t13, t18, t20, t22, t23, t24, t25, t26, t27, t28, t65, t66, t67, + t68, t69, t70, t71, t77, t85, t86, t90, t94, t95, t99, t103, t38, t39, + t40, t41, t42, t43, t44, t45, t46, t72, t73, t75, t78, t80, t82, t125, + t126, t127, t128, t129, t130, t131, t133, t135, t149, t150, t151, t189, + t190, t191, t192, t193, t194, t195, t196, t197, t198, t199, t200, t202, + t205, t203, t204, t206, t241, t309, t310, t312, t313, t314, t315, t316, + t317, t318, t319, t321, t322, t323, t324, t325, t261, t262, t599, t600, + t602, t605, t609, t610, t611, t613, t615, t616, t621, t622, t623, t625, + t645, t646_tmp, t646, t601, t603, t604, t606, t607, t608, t612, t614, + t617, t618, t619, t620, t624, t626, t627, t628, t629, t630, t631, t632, + t633, t634, t635, t636, t637, t638; -Vector12d point_plane_distance_gradient( - Eigen::ConstRef p, - Eigen::ConstRef t0, - Eigen::ConstRef t1, - Eigen::ConstRef t2) -{ - Vector12d grad; - autogen::point_plane_distance_gradient( - p[0], p[1], p[2], t0[0], t0[1], t0[2], t1[0], t1[1], t1[2], t2[0], - t2[1], t2[2], grad.data()); - return grad; + t11 = -v11 + v01; + t12 = -v12 + v02; + t13 = -v13 + v03; + t18 = -v21 + v11; + t20 = -v22 + v12; + t22 = -v23 + v13; + t23 = -v31 + v11; + t24 = -v32 + v12; + t25 = -v33 + v13; + t26 = -v31 + v21; + t27 = -v32 + v22; + t28 = -v33 + v23; + t65 = t18 * t24; + t66 = t20 * t23; + t67 = t18 * t25; + t68 = t22 * t23; + t69 = t20 * t25; + t70 = t22 * t24; + t71 = t18 * t23 * T(2); + t77 = t20 * t24 * T(2); + t85 = t22 * t25 * T(2); + t86 = t18 * t26 * T(2); + t90 = t20 * t27 * T(2); + t94 = t22 * t28 * T(2); + t95 = t23 * t26 * T(2); + t99 = t24 * t27 * T(2); + t103 = t25 * t28 * T(2); + t38 = t18 * t18 * T(2); + t39 = t20 * t20 * T(2); + t40 = t22 * t22 * T(2); + t41 = t23 * t23 * T(2); + t42 = t24 * t24 * T(2); + t43 = t25 * t25 * T(2); + t44 = t26 * t26 * T(2); + t45 = t27 * t27 * T(2); + t46 = t28 * t28 * T(2); + t72 = t65 * T(2); + t73 = t66 * T(2); + t75 = t67 * T(2); + t78 = t68 * T(2); + t80 = t69 * T(2); + t82 = t70 * T(2); + t125 = t11 * t20 + -(t12 * t18); + t126 = t11 * t22 + -(t13 * t18); + t127 = t12 * t22 + -(t13 * t20); + t128 = t11 * t24 + -(t12 * t23); + t129 = t11 * t25 + -(t13 * t23); + t130 = t12 * t25 + -(t13 * t24); + t131 = t65 + -t66; + t133 = t67 + -t68; + t135 = t69 + -t70; + t149 = t131 * t131; + t150 = t133 * t133; + t151 = t135 * t135; + t189 = (t11 * t27 + -(t12 * t26)) + t131; + t190 = (t11 * t28 + -(t13 * t26)) + t133; + t191 = (t12 * t28 + -(t13 * t27)) + t135; + t192 = t20 * t131 * T(2) + t22 * t133 * T(2); + t193 = t18 * t133 * T(2) + t20 * t135 * T(2); + t194 = t24 * t131 * T(2) + t25 * t133 * T(2); + t195 = t23 * t133 * T(2) + t24 * t135 * T(2); + t196 = t27 * t131 * T(2) + t28 * t133 * T(2); + t197 = t26 * t133 * T(2) + t27 * t135 * T(2); + t198 = t18 * t131 * T(2) + -(t22 * t135 * T(2)); + t199 = t23 * t131 * T(2) + -(t25 * t135 * T(2)); + t200 = t26 * t131 * T(2) + -(t28 * t135 * T(2)); + t202 = T(1) / ((t149 + t150) + t151); + t205 = (t13 * t131 + t11 * t135) + -(t12 * t133); + t203 = t202 * t202; + t204 = t202 * t202 * t202; + t206 = t205 * t205; + t241 = t131 * t135 * t202 * T(2); + t309 = t11 * t202 * t205 * T(2); + t310 = t12 * t202 * t205 * T(2); + t13 = t13 * t202 * t205 * T(2); + t312 = (-v21 + v01) * t202 * t205 * T(2); + t313 = (-v22 + v02) * t202 * t205 * T(2); + t314 = (-v23 + v03) * t202 * t205 * T(2); + t315 = (-v31 + v01) * t202 * t205 * T(2); + t316 = t18 * t202 * t205 * T(2); + t317 = (-v32 + v02) * t202 * t205 * T(2); + t318 = t20 * t202 * t205 * T(2); + t319 = (-v33 + v03) * t202 * t205 * T(2); + t11 = t22 * t202 * t205 * T(2); + t321 = t23 * t202 * t205 * T(2); + t322 = t24 * t202 * t205 * T(2); + t323 = t25 * t202 * t205 * T(2); + t324 = t26 * t202 * t205 * T(2); + t325 = t27 * t202 * t205 * T(2); + t12 = t28 * t202 * t205 * T(2); + t261 = -(t131 * t133 * t202 * T(2)); + t262 = -(t133 * t135 * t202 * T(2)); + t599 = t130 * t135 * t202 * T(2) + t135 * t194 * t203 * t205 * T(2); + t600 = -(t125 * t131 * t202 * T(2)) + t131 * t193 * t203 * t205 * T(2); + t602 = t129 * t133 * t202 * T(2) + t133 * t199 * t203 * t205 * T(2); + t605 = -(t131 * t189 * t202 * T(2)) + t131 * t197 * t203 * t205 * T(2); + t609 = + (t127 * t133 * t202 * T(2) + -t11) + t133 * t192 * t203 * t205 * T(2); + t610 = (t126 * t135 * t202 * T(2) + t11) + t135 * t198 * t203 * t205 * T(2); + t611 = + (t130 * t131 * t202 * T(2) + -t322) + t131 * t194 * t203 * t205 * T(2); + t613 = + (t126 * t131 * t202 * T(2) + -t316) + t131 * t198 * t203 * t205 * T(2); + t615 = (-(t125 * t135 * t202 * T(2)) + -t318) + + t135 * t193 * t203 * t205 * T(2); + t616 = (-(t128 * t133 * t202 * T(2)) + -t321) + + t133 * t195 * t203 * t205 * T(2); + t621 = + (t133 * t191 * t202 * T(2) + -t12) + t133 * t196 * t203 * t205 * T(2); + t622 = (t135 * t190 * t202 * T(2) + t12) + t135 * t200 * t203 * t205 * T(2); + t623 = + (t131 * t190 * t202 * T(2) + -t324) + t131 * t200 * t203 * t205 * T(2); + t625 = (-(t135 * t189 * t202 * T(2)) + -t325) + + t135 * t197 * t203 * t205 * T(2); + t645 = ((((t127 * t129 * t202 * T(2) + -t13) + + (t72 + -(t66 * T(4))) * t203 * t206) + + t129 * t192 * t203 * t205 * T(2)) + + t127 * t199 * t203 * t205 * T(2)) + + t192 * t199 * t204 * t206 * T(2); + t646_tmp = t203 * t206; + t646 = ((((t126 * t130 * t202 * T(2) + t13) + t646_tmp * (t73 - t65 * T(4))) + + t126 * t194 * t203 * t205 * T(2)) + + t130 * t198 * t203 * t205 * T(2)) + + t194 * t198 * t204 * t206 * T(2); + t601 = t128 * t131 * t202 * T(2) + -(t131 * t195 * t203 * t205 * T(2)); + t603 = -(t127 * t135 * t202 * T(2)) + -(t135 * t192 * t203 * t205 * T(2)); + t604 = -(t126 * t133 * t202 * T(2)) + -(t133 * t198 * t203 * t205 * T(2)); + t606 = -(t135 * t191 * t202 * T(2)) + -(t135 * t196 * t203 * t205 * T(2)); + t607 = -(t133 * t190 * t202 * T(2)) + -(t133 * t200 * t203 * t205 * T(2)); + t608 = (t125 * t133 * t202 * T(2) + t316) + + -(t133 * t193 * t203 * t205 * T(2)); + t612 = (t128 * t135 * t202 * T(2) + t322) + + -(t135 * t195 * t203 * t205 * T(2)); + t614 = (-(t127 * t131 * t202 * T(2)) + t318) + + -(t131 * t192 * t203 * t205 * T(2)); + t617 = (-(t130 * t133 * t202 * T(2)) + t323) + + -(t133 * t194 * t203 * t205 * T(2)); + t618 = (-(t129 * t131 * t202 * T(2)) + t321) + + -(t131 * t199 * t203 * t205 * T(2)); + t619 = (-(t129 * t135 * t202 * T(2)) + -t323) + + -(t135 * t199 * t203 * t205 * T(2)); + t620 = (t133 * t189 * t202 * T(2) + t324) + + -(t133 * t197 * t203 * t205 * T(2)); + t624 = (-(t131 * t191 * t202 * T(2)) + t325) + + -(t131 * t196 * t203 * t205 * T(2)); + t626 = (((t125 * t127 * t202 * T(2) + t18 * t22 * t203 * t206 * T(2)) + + t125 * t192 * t203 * t205 * T(2)) + + -(t127 * t193 * t203 * t205 * T(2))) + + -(t192 * t193 * t204 * t206 * T(2)); + t627 = (((t128 * t130 * t202 * T(2) + t23 * t25 * t203 * t206 * T(2)) + + t128 * t194 * t203 * t205 * T(2)) + + -(t130 * t195 * t203 * t205 * T(2))) + + -(t194 * t195 * t204 * t206 * T(2)); + t628 = (((-(t125 * t126 * t202 * T(2)) + t20 * t22 * t203 * t206 * T(2)) + + t126 * t193 * t203 * t205 * T(2)) + + -(t125 * t198 * t203 * t205 * T(2))) + + t193 * t198 * t204 * t206 * T(2); + t629 = (((-(t128 * t129 * t202 * T(2)) + t24 * t25 * t203 * t206 * T(2)) + + t129 * t195 * t203 * t205 * T(2)) + + -(t128 * t199 * t203 * t205 * T(2))) + + t195 * t199 * t204 * t206 * T(2); + t630 = (((-(t126 * t127 * t202 * T(2)) + t18 * t20 * t203 * t206 * T(2)) + + -(t126 * t192 * t203 * t205 * T(2))) + + -(t127 * t198 * t203 * t205 * T(2))) + + -(t192 * t198 * t204 * t206 * T(2)); + t631 = (((-(t129 * t130 * t202 * T(2)) + t23 * t24 * t203 * t206 * T(2)) + + -(t129 * t194 * t203 * t205 * T(2))) + + -(t130 * t199 * t203 * t205 * T(2))) + + -(t194 * t199 * t204 * t206 * T(2)); + t632 = (((-(t125 * t128 * t202 * T(2)) + (t71 + t77) * t203 * t206) + + t128 * t193 * t203 * t205 * T(2)) + + t125 * t195 * t203 * t205 * T(2)) + + -(t193 * t195 * t204 * t206 * T(2)); + t633 = (((-(t127 * t130 * t202 * T(2)) + (t77 + t85) * t203 * t206) + + -(t130 * t192 * t203 * t205 * T(2))) + + -(t127 * t194 * t203 * t205 * T(2))) + + -(t192 * t194 * t204 * t206 * T(2)); + t634 = (((-(t126 * t129 * t202 * T(2)) + (t71 + t85) * t203 * t206) + + -(t129 * t198 * t203 * t205 * T(2))) + + -(t126 * t199 * t203 * t205 * T(2))) + + -(t198 * t199 * t204 * t206 * T(2)); + t635 = (((t127 * t191 * t202 * T(2) + -((t90 + t94) * t203 * t206)) + + t127 * t196 * t203 * t205 * T(2)) + + t191 * t192 * t203 * t205 * T(2)) + + t192 * t196 * t204 * t206 * T(2); + t636 = (((-(t128 * t189 * t202 * T(2)) + (t95 + t99) * t203 * t206) + + t128 * t197 * t203 * t205 * T(2)) + + t189 * t195 * t203 * t205 * T(2)) + + -(t195 * t197 * t204 * t206 * T(2)); + t637 = (((t125 * t189 * t202 * T(2) + -((t86 + t90) * t203 * t206)) + + -(t125 * t197 * t203 * t205 * T(2))) + + -(t189 * t193 * t203 * t205 * T(2))) + + t193 * t197 * t204 * t206 * T(2); + t638 = (((-(t130 * t191 * t202 * T(2)) + (t99 + t103) * t203 * t206) + + -(t130 * t196 * t203 * t205 * T(2))) + + -(t191 * t194 * t203 * t205 * T(2))) + + -(t194 * t196 * t204 * t206 * T(2)); + t86 = (((t126 * t190 * t202 * T(2) + -((t86 + t94) * t203 * t206)) + + t126 * t200 * t203 * t205 * T(2)) + + t190 * t198 * t203 * t205 * T(2)) + + t198 * t200 * t204 * t206 * T(2); + t71 = (((-(t129 * t190 * t202 * T(2)) + (t95 + t103) * t203 * t206) + + -(t129 * t200 * t203 * t205 * T(2))) + + -(t190 * t199 * t203 * t205 * T(2))) + + -(t199 * t200 * t204 * t206 * T(2)); + t85 = (((t189 * t191 * t202 * T(2) + t26 * t28 * t203 * t206 * T(2)) + + t189 * t196 * t203 * t205 * T(2)) + + -(t191 * t197 * t203 * t205 * T(2))) + + -(t196 * t197 * t204 * t206 * T(2)); + t90 = (((-(t189 * t190 * t202 * T(2)) + t27 * t28 * t203 * t206 * T(2)) + + t190 * t197 * t203 * t205 * T(2)) + + -(t189 * t200 * t203 * t205 * T(2))) + + t197 * t200 * t204 * t206 * T(2); + t99 = (((-(t190 * t191 * t202 * T(2)) + t26 * t27 * t203 * t206 * T(2)) + + -(t190 * t196 * t203 * t205 * T(2))) + + -(t191 * t200 * t203 * t205 * T(2))) + + -(t196 * t200 * t204 * t206 * T(2)); + t77 = ((((-(t127 * t128 * t202 * T(2)) + t310) + + (t75 + -(t68 * T(4))) * t203 * t206) + + t127 * t195 * t203 * t205 * T(2)) + + -(t128 * t192 * t203 * t205 * T(2))) + + t192 * t195 * t204 * t206 * T(2); + t131 = ((((t126 * t128 * t202 * T(2) + -t309) + + (t80 + -(t70 * T(4))) * t203 * t206) + + t128 * t198 * t203 * t205 * T(2)) + + -(t126 * t195 * t203 * t205 * T(2))) + + -(t195 * t198 * t204 * t206 * T(2)); + t133 = ((((-(t125 * t130 * t202 * T(2)) + -t310) + + t646_tmp * (t78 - t67 * T(4))) + + t130 * t193 * t203 * t205 * T(2)) + + -(t125 * t194 * t203 * t205 * T(2))) + + t193 * t194 * t204 * t206 * T(2); + t325 = + ((((t125 * t129 * t202 * T(2) + t309) + t646_tmp * (t82 - t69 * T(4))) + + t125 * t199 * t203 * t205 * T(2)) + + -(t129 * t193 * t203 * t205 * T(2))) + + -(t193 * t199 * t204 * t206 * T(2)); + t135 = ((((t125 * t191 * t202 * T(2) + t313) + + ((t75 + t18 * t28 * T(2)) + -t78) * t203 * t206) + + t125 * t196 * t203 * t205 * T(2)) + + -(t191 * t193 * t203 * t205 * T(2))) + + -(t193 * t196 * t204 * t206 * T(2)); + t324 = ((((t127 * t189 * t202 * T(2) + -t313) + + ((t78 + t22 * t26 * T(2)) + -t75) * t203 * t206) + + -(t127 * t197 * t203 * t205 * T(2))) + + t189 * t192 * t203 * t205 * T(2)) + + -(t192 * t197 * t204 * t206 * T(2)); + t318 = ((((-(t126 * t189 * t202 * T(2)) + t312) + + ((t82 + t22 * t27 * T(2)) + -t80) * t203 * t206) + + t126 * t197 * t203 * t205 * T(2)) + + -(t189 * t198 * t203 * t205 * T(2))) + + t197 * t198 * t204 * t206 * T(2); + t321 = ((((-(t130 * t189 * t202 * T(2)) + t317) + + -(((t78 + t25 * t26 * T(2)) + -t75) * t203 * t206)) + + t130 * t197 * t203 * t205 * T(2)) + + -(t189 * t194 * t203 * t205 * T(2))) + + t194 * t197 * t204 * t206 * T(2); + t323 = ((((t129 * t191 * t202 * T(2) + t319) + + -(((t72 + t23 * t27 * T(2)) + -t73) * t203 * t206)) + + t129 * t196 * t203 * t205 * T(2)) + + t191 * t199 * t203 * t205 * T(2)) + + t196 * t199 * t204 * t206 * T(2); + t322 = ((((-(t125 * t190 * t202 * T(2)) + -t312) + + ((t80 + t20 * t28 * T(2)) + -t82) * t203 * t206) + + -(t125 * t200 * t203 * t205 * T(2))) + + t190 * t193 * t203 * t205 * T(2)) + + t193 * t200 * t204 * t206 * T(2); + t316 = ((((t130 * t190 * t202 * T(2) + -t319) + + -(((t73 + t24 * t26 * T(2)) + -t72) * t203 * t206)) + + t130 * t200 * t203 * t205 * T(2)) + + t190 * t194 * t203 * t205 * T(2)) + + t194 * t200 * t204 * t206 * T(2); + t65 = ((((-(t128 * t191 * t202 * T(2)) + -t317) + + -(((t75 + t23 * t28 * T(2)) + -t78) * t203 * t206)) + + -(t128 * t196 * t203 * t205 * T(2))) + + t191 * t195 * t203 * t205 * T(2)) + + t195 * t196 * t204 * t206 * T(2); + t66 = ((((-(t127 * t190 * t202 * T(2)) + t314) + + ((t73 + t20 * t26 * T(2)) + -t72) * t203 * t206) + + -(t127 * t200 * t203 * t205 * T(2))) + + -(t190 * t192 * t203 * t205 * T(2))) + + -(t192 * t200 * t204 * t206 * T(2)); + t13 = ((((t128 * t190 * t202 * T(2) + t315) + + -(((t80 + t24 * t28 * T(2)) + -t82) * t203 * t206)) + + t128 * t200 * t203 * t205 * T(2)) + + -(t190 * t195 * t203 * t205 * T(2))) + + -(t195 * t200 * t204 * t206 * T(2)); + t12 = ((((-(t126 * t191 * t202 * T(2)) + -t314) + + ((t72 + t18 * t27 * T(2)) + -t73) * t203 * t206) + + -(t126 * t196 * t203 * t205 * T(2))) + + -(t191 * t198 * t203 * t205 * T(2))) + + -(t196 * t198 * t204 * t206 * T(2)); + t11 = ((((t129 * t189 * t202 * T(2) + -t315) + + -(((t82 + t25 * t27 * T(2)) + -t80) * t203 * t206)) + + -(t129 * t197 * t203 * t205 * T(2))) + + t189 * t199 * t203 * t205 * T(2)) + + -(t197 * t199 * t204 * t206 * T(2)); + H[0] = t151 * t202 * T(2); + H[1] = t262; + H[2] = t241; + H[3] = t606; + H[4] = t622; + H[5] = t625; + H[6] = t599; + H[7] = t619; + H[8] = t612; + H[9] = t603; + H[10] = t610; + H[11] = t615; + H[12] = t262; + H[13] = t150 * t202 * T(2); + H[14] = t261; + H[15] = t621; + H[16] = t607; + H[17] = t620; + H[18] = t617; + H[19] = t602; + H[20] = t616; + H[21] = t609; + H[22] = t604; + H[23] = t608; + H[24] = t241; + H[25] = t261; + H[26] = t149 * t202 * T(2); + H[27] = t624; + H[28] = t623; + H[29] = t605; + H[30] = t611; + H[31] = t618; + H[32] = t601; + H[33] = t614; + H[34] = t613; + H[35] = t600; + H[36] = t606; + H[37] = t621; + H[38] = t624; + H[39] = ((t191 * t191 * t202 * T(2) + t196 * t196 * t204 * t206 * T(2)) + - t646_tmp * (t45 + t46)) + + t191 * t196 * t203 * t205 * T(4); + H[40] = t99; + H[41] = t85; + H[42] = t638; + H[43] = t323; + H[44] = t65; + H[45] = t635; + H[46] = t12; + H[47] = t135; + H[48] = t622; + H[49] = t607; + H[50] = t623; + H[51] = t99; + H[52] = ((t190 * t190 * t202 * T(2) + t200 * t200 * t204 * t206 * T(2)) + - t646_tmp * (t44 + t46)) + + t190 * t200 * t203 * t205 * T(4); + H[53] = t90; + H[54] = t316; + H[55] = t71; + H[56] = t13; + H[57] = t66; + H[58] = t86; + H[59] = t322; + H[60] = t625; + H[61] = t620; + H[62] = t605; + H[63] = t85; + H[64] = t90; + H[65] = ((t189 * t189 * t202 * T(2) + t197 * t197 * t204 * t206 * T(2)) + - t646_tmp * (t44 + t45)) + - t189 * t197 * t203 * t205 * T(4); + H[66] = t321; + H[67] = t11; + H[68] = t636; + H[69] = t324; + H[70] = t318; + H[71] = t637; + H[72] = t599; + H[73] = t617; + H[74] = t611; + H[75] = t638; + H[76] = t316; + H[77] = t321; + H[78] = ((t130 * t130 * t202 * T(2) + t194 * t194 * t204 * t206 * T(2)) + - t646_tmp * (t42 + t43)) + + t130 * t194 * t203 * t205 * T(4); + H[79] = t631; + H[80] = t627; + H[81] = t633; + H[82] = t646; + H[83] = t133; + H[84] = t619; + H[85] = t602; + H[86] = t618; + H[87] = t323; + H[88] = t71; + H[89] = t11; + H[90] = t631; + H[91] = ((t129 * t129 * t202 * T(2) + t199 * t199 * t204 * t206 * T(2)) + - t646_tmp * (t41 + t43)) + + t129 * t199 * t203 * t205 * T(4); + H[92] = t629; + H[93] = t645; + H[94] = t634; + H[95] = t325; + H[96] = t612; + H[97] = t616; + H[98] = t601; + H[99] = t65; + H[100] = t13; + H[101] = t636; + H[102] = t627; + H[103] = t629; + H[104] = ((t128 * t128 * t202 * T(2) + t195 * t195 * t204 * t206 * T(2)) + - t646_tmp * (t41 + t42)) + - t128 * t195 * t203 * t205 * T(4); + H[105] = t77; + H[106] = t131; + H[107] = t632; + H[108] = t603; + H[109] = t609; + H[110] = t614; + H[111] = t635; + H[112] = t66; + H[113] = t324; + H[114] = t633; + H[115] = t645; + H[116] = t77; + H[117] = ((t127 * t127 * t202 * T(2) + t192 * t192 * t204 * t206 * T(2)) + - t646_tmp * (t39 + t40)) + + t127 * t192 * t203 * t205 * T(4); + H[118] = t630; + H[119] = t626; + H[120] = t610; + H[121] = t604; + H[122] = t613; + H[123] = t12; + H[124] = t86; + H[125] = t318; + H[126] = t646; + H[127] = t634; + H[128] = t131; + H[129] = t630; + H[130] = ((t126 * t126 * t202 * T(2) + t198 * t198 * t204 * t206 * T(2)) + - t646_tmp * (t38 + t40)) + + t126 * t198 * t203 * t205 * T(4); + H[131] = t628; + H[132] = t615; + H[133] = t608; + H[134] = t600; + H[135] = t135; + H[136] = t322; + H[137] = t637; + H[138] = t133; + H[139] = t325; + H[140] = t632; + H[141] = t626; + H[142] = t628; + H[143] = ((t125 * t125 * t202 * T(2) + t193 * t193 * t204 * t206 * T(2)) + - t646_tmp * (t38 + t39)) + - t125 * t193 * t203 * t205 * T(4); } -Eigen::Matrix3d point_plane_distance_hessian( - Eigen::ConstRef p, - Eigen::ConstRef origin, - Eigen::ConstRef normal) -{ - return 2 / normal.squaredNorm() * normal * normal.transpose(); -} - -Matrix12d point_plane_distance_hessian( - Eigen::ConstRef p, - Eigen::ConstRef t0, - Eigen::ConstRef t1, - Eigen::ConstRef t2) -{ - Matrix12d hess; - autogen::point_plane_distance_hessian( - p[0], p[1], p[2], t0[0], t0[1], t0[2], t1[0], t1[1], t1[2], t2[0], - t2[1], t2[2], hess.data()); - return hess; -} - -namespace autogen { - - // This function was generated by the Symbolic Math Toolbox version 8.3. - // 10-Jun-2019 17:42:16 - void point_plane_distance_gradient( - double v01, - double v02, - double v03, - double v11, - double v12, - double v13, - double v21, - double v22, - double v23, - double v31, - double v32, - double v33, - double g[12]) - { - double t11, t12, t13, t14, t15, t16, t17, t18, t19, t20, t21, t22, t32, - t33, t34, t43, t45, t44, t46; - - t11 = -v11 + v01; - t12 = -v12 + v02; - t13 = -v13 + v03; - t14 = -v21 + v11; - t15 = -v22 + v12; - t16 = -v23 + v13; - t17 = -v31 + v11; - t18 = -v32 + v12; - t19 = -v33 + v13; - t20 = -v31 + v21; - t21 = -v32 + v22; - t22 = -v33 + v23; - t32 = t14 * t18 + -(t15 * t17); - t33 = t14 * t19 + -(t16 * t17); - t34 = t15 * t19 + -(t16 * t18); - t43 = 1.0 / ((t32 * t32 + t33 * t33) + t34 * t34); - t45 = (t13 * t32 + t11 * t34) + -(t12 * t33); - t44 = t43 * t43; - t46 = t45 * t45; - g[0] = t34 * t43 * t45 * 2.0; - g[1] = t33 * t43 * t45 * -2.0; - g[2] = t32 * t43 * t45 * 2.0; - t45 *= t43; - g[3] = -t44 * t46 * (t21 * t32 * 2.0 + t22 * t33 * 2.0) - - t45 * ((t34 + t12 * t22) - t13 * t21) * 2.0; - t43 = t44 * t46; - g[4] = t43 * (t20 * t32 * 2.0 - t22 * t34 * 2.0) - + t45 * ((t33 + t11 * t22) - t13 * t20) * 2.0; - g[5] = t43 * (t20 * t33 * 2.0 + t21 * t34 * 2.0) - - t45 * ((t32 + t11 * t21) - t12 * t20) * 2.0; - g[6] = t45 * (t12 * t19 - t13 * t18) * 2.0 - + t43 * (t18 * t32 * 2.0 + t19 * t33 * 2.0); - g[7] = t45 * (t11 * t19 - t13 * t17) * -2.0 - - t43 * (t17 * t32 * 2.0 - t19 * t34 * 2.0); - g[8] = t45 * (t11 * t18 - t12 * t17) * 2.0 - - t43 * (t17 * t33 * 2.0 + t18 * t34 * 2.0); - g[9] = t45 * (t12 * t16 - t13 * t15) * -2.0 - - t43 * (t15 * t32 * 2.0 + t16 * t33 * 2.0); - g[10] = t45 * (t11 * t16 - t13 * t14) * 2.0 - + t43 * (t14 * t32 * 2.0 - t16 * t34 * 2.0); - g[11] = t45 * (t11 * t15 - t12 * t14) * -2.0 - + t43 * (t14 * t33 * 2.0 + t15 * t34 * 2.0); - } - - // This function was generated by the Symbolic Math Toolbox version 8.3. - // 10-Jun-2019 17:42:25 - void point_plane_distance_hessian( - double v01, - double v02, - double v03, - double v11, - double v12, - double v13, - double v21, - double v22, - double v23, - double v31, - double v32, - double v33, - double H[144]) - { - double t11, t12, t13, t18, t20, t22, t23, t24, t25, t26, t27, t28, t65, - t66, t67, t68, t69, t70, t71, t77, t85, t86, t90, t94, t95, t99, - t103, t38, t39, t40, t41, t42, t43, t44, t45, t46, t72, t73, t75, - t78, t80, t82, t125, t126, t127, t128, t129, t130, t131, t133, t135, - t149, t150, t151, t189, t190, t191, t192, t193, t194, t195, t196, - t197, t198, t199, t200, t202, t205, t203, t204, t206, t241, t309, - t310, t312, t313, t314, t315, t316, t317, t318, t319, t321, t322, - t323, t324, t325, t261, t262, t599, t600, t602, t605, t609, t610, - t611, t613, t615, t616, t621, t622, t623, t625, t645, t646_tmp, - t646, t601, t603, t604, t606, t607, t608, t612, t614, t617, t618, - t619, t620, t624, t626, t627, t628, t629, t630, t631, t632, t633, - t634, t635, t636, t637, t638; +#define IPC_INSTANTIATE_POINT_PLANE_AUTOGEN(T) \ + template void point_plane_distance_gradient( \ + T, T, T, T, T, T, T, T, T, T, T, T, T[12]); \ + template void point_plane_distance_hessian( \ + T, T, T, T, T, T, T, T, T, T, T, T, T[144]) - t11 = -v11 + v01; - t12 = -v12 + v02; - t13 = -v13 + v03; - t18 = -v21 + v11; - t20 = -v22 + v12; - t22 = -v23 + v13; - t23 = -v31 + v11; - t24 = -v32 + v12; - t25 = -v33 + v13; - t26 = -v31 + v21; - t27 = -v32 + v22; - t28 = -v33 + v23; - t65 = t18 * t24; - t66 = t20 * t23; - t67 = t18 * t25; - t68 = t22 * t23; - t69 = t20 * t25; - t70 = t22 * t24; - t71 = t18 * t23 * 2.0; - t77 = t20 * t24 * 2.0; - t85 = t22 * t25 * 2.0; - t86 = t18 * t26 * 2.0; - t90 = t20 * t27 * 2.0; - t94 = t22 * t28 * 2.0; - t95 = t23 * t26 * 2.0; - t99 = t24 * t27 * 2.0; - t103 = t25 * t28 * 2.0; - t38 = t18 * t18 * 2.0; - t39 = t20 * t20 * 2.0; - t40 = t22 * t22 * 2.0; - t41 = t23 * t23 * 2.0; - t42 = t24 * t24 * 2.0; - t43 = t25 * t25 * 2.0; - t44 = t26 * t26 * 2.0; - t45 = t27 * t27 * 2.0; - t46 = t28 * t28 * 2.0; - t72 = t65 * 2.0; - t73 = t66 * 2.0; - t75 = t67 * 2.0; - t78 = t68 * 2.0; - t80 = t69 * 2.0; - t82 = t70 * 2.0; - t125 = t11 * t20 + -(t12 * t18); - t126 = t11 * t22 + -(t13 * t18); - t127 = t12 * t22 + -(t13 * t20); - t128 = t11 * t24 + -(t12 * t23); - t129 = t11 * t25 + -(t13 * t23); - t130 = t12 * t25 + -(t13 * t24); - t131 = t65 + -t66; - t133 = t67 + -t68; - t135 = t69 + -t70; - t149 = t131 * t131; - t150 = t133 * t133; - t151 = t135 * t135; - t189 = (t11 * t27 + -(t12 * t26)) + t131; - t190 = (t11 * t28 + -(t13 * t26)) + t133; - t191 = (t12 * t28 + -(t13 * t27)) + t135; - t192 = t20 * t131 * 2.0 + t22 * t133 * 2.0; - t193 = t18 * t133 * 2.0 + t20 * t135 * 2.0; - t194 = t24 * t131 * 2.0 + t25 * t133 * 2.0; - t195 = t23 * t133 * 2.0 + t24 * t135 * 2.0; - t196 = t27 * t131 * 2.0 + t28 * t133 * 2.0; - t197 = t26 * t133 * 2.0 + t27 * t135 * 2.0; - t198 = t18 * t131 * 2.0 + -(t22 * t135 * 2.0); - t199 = t23 * t131 * 2.0 + -(t25 * t135 * 2.0); - t200 = t26 * t131 * 2.0 + -(t28 * t135 * 2.0); - t202 = 1.0 / ((t149 + t150) + t151); - t205 = (t13 * t131 + t11 * t135) + -(t12 * t133); - t203 = t202 * t202; - t204 = pow(t202, 3.0); - t206 = t205 * t205; - t241 = t131 * t135 * t202 * 2.0; - t309 = t11 * t202 * t205 * 2.0; - t310 = t12 * t202 * t205 * 2.0; - t13 = t13 * t202 * t205 * 2.0; - t312 = (-v21 + v01) * t202 * t205 * 2.0; - t313 = (-v22 + v02) * t202 * t205 * 2.0; - t314 = (-v23 + v03) * t202 * t205 * 2.0; - t315 = (-v31 + v01) * t202 * t205 * 2.0; - t316 = t18 * t202 * t205 * 2.0; - t317 = (-v32 + v02) * t202 * t205 * 2.0; - t318 = t20 * t202 * t205 * 2.0; - t319 = (-v33 + v03) * t202 * t205 * 2.0; - t11 = t22 * t202 * t205 * 2.0; - t321 = t23 * t202 * t205 * 2.0; - t322 = t24 * t202 * t205 * 2.0; - t323 = t25 * t202 * t205 * 2.0; - t324 = t26 * t202 * t205 * 2.0; - t325 = t27 * t202 * t205 * 2.0; - t12 = t28 * t202 * t205 * 2.0; - t261 = -(t131 * t133 * t202 * 2.0); - t262 = -(t133 * t135 * t202 * 2.0); - t599 = t130 * t135 * t202 * 2.0 + t135 * t194 * t203 * t205 * 2.0; - t600 = -(t125 * t131 * t202 * 2.0) + t131 * t193 * t203 * t205 * 2.0; - t602 = t129 * t133 * t202 * 2.0 + t133 * t199 * t203 * t205 * 2.0; - t605 = -(t131 * t189 * t202 * 2.0) + t131 * t197 * t203 * t205 * 2.0; - t609 = - (t127 * t133 * t202 * 2.0 + -t11) + t133 * t192 * t203 * t205 * 2.0; - t610 = - (t126 * t135 * t202 * 2.0 + t11) + t135 * t198 * t203 * t205 * 2.0; - t611 = (t130 * t131 * t202 * 2.0 + -t322) - + t131 * t194 * t203 * t205 * 2.0; - t613 = (t126 * t131 * t202 * 2.0 + -t316) - + t131 * t198 * t203 * t205 * 2.0; - t615 = (-(t125 * t135 * t202 * 2.0) + -t318) - + t135 * t193 * t203 * t205 * 2.0; - t616 = (-(t128 * t133 * t202 * 2.0) + -t321) - + t133 * t195 * t203 * t205 * 2.0; - t621 = - (t133 * t191 * t202 * 2.0 + -t12) + t133 * t196 * t203 * t205 * 2.0; - t622 = - (t135 * t190 * t202 * 2.0 + t12) + t135 * t200 * t203 * t205 * 2.0; - t623 = (t131 * t190 * t202 * 2.0 + -t324) - + t131 * t200 * t203 * t205 * 2.0; - t625 = (-(t135 * t189 * t202 * 2.0) + -t325) - + t135 * t197 * t203 * t205 * 2.0; - t645 = ((((t127 * t129 * t202 * 2.0 + -t13) - + (t72 + -(t66 * 4.0)) * t203 * t206) - + t129 * t192 * t203 * t205 * 2.0) - + t127 * t199 * t203 * t205 * 2.0) - + t192 * t199 * t204 * t206 * 2.0; - t646_tmp = t203 * t206; - t646 = - ((((t126 * t130 * t202 * 2.0 + t13) + t646_tmp * (t73 - t65 * 4.0)) - + t126 * t194 * t203 * t205 * 2.0) - + t130 * t198 * t203 * t205 * 2.0) - + t194 * t198 * t204 * t206 * 2.0; - t601 = t128 * t131 * t202 * 2.0 + -(t131 * t195 * t203 * t205 * 2.0); - t603 = -(t127 * t135 * t202 * 2.0) + -(t135 * t192 * t203 * t205 * 2.0); - t604 = -(t126 * t133 * t202 * 2.0) + -(t133 * t198 * t203 * t205 * 2.0); - t606 = -(t135 * t191 * t202 * 2.0) + -(t135 * t196 * t203 * t205 * 2.0); - t607 = -(t133 * t190 * t202 * 2.0) + -(t133 * t200 * t203 * t205 * 2.0); - t608 = (t125 * t133 * t202 * 2.0 + t316) - + -(t133 * t193 * t203 * t205 * 2.0); - t612 = (t128 * t135 * t202 * 2.0 + t322) - + -(t135 * t195 * t203 * t205 * 2.0); - t614 = (-(t127 * t131 * t202 * 2.0) + t318) - + -(t131 * t192 * t203 * t205 * 2.0); - t617 = (-(t130 * t133 * t202 * 2.0) + t323) - + -(t133 * t194 * t203 * t205 * 2.0); - t618 = (-(t129 * t131 * t202 * 2.0) + t321) - + -(t131 * t199 * t203 * t205 * 2.0); - t619 = (-(t129 * t135 * t202 * 2.0) + -t323) - + -(t135 * t199 * t203 * t205 * 2.0); - t620 = (t133 * t189 * t202 * 2.0 + t324) - + -(t133 * t197 * t203 * t205 * 2.0); - t624 = (-(t131 * t191 * t202 * 2.0) + t325) - + -(t131 * t196 * t203 * t205 * 2.0); - t626 = (((t125 * t127 * t202 * 2.0 + t18 * t22 * t203 * t206 * 2.0) - + t125 * t192 * t203 * t205 * 2.0) - + -(t127 * t193 * t203 * t205 * 2.0)) - + -(t192 * t193 * t204 * t206 * 2.0); - t627 = (((t128 * t130 * t202 * 2.0 + t23 * t25 * t203 * t206 * 2.0) - + t128 * t194 * t203 * t205 * 2.0) - + -(t130 * t195 * t203 * t205 * 2.0)) - + -(t194 * t195 * t204 * t206 * 2.0); - t628 = (((-(t125 * t126 * t202 * 2.0) + t20 * t22 * t203 * t206 * 2.0) - + t126 * t193 * t203 * t205 * 2.0) - + -(t125 * t198 * t203 * t205 * 2.0)) - + t193 * t198 * t204 * t206 * 2.0; - t629 = (((-(t128 * t129 * t202 * 2.0) + t24 * t25 * t203 * t206 * 2.0) - + t129 * t195 * t203 * t205 * 2.0) - + -(t128 * t199 * t203 * t205 * 2.0)) - + t195 * t199 * t204 * t206 * 2.0; - t630 = (((-(t126 * t127 * t202 * 2.0) + t18 * t20 * t203 * t206 * 2.0) - + -(t126 * t192 * t203 * t205 * 2.0)) - + -(t127 * t198 * t203 * t205 * 2.0)) - + -(t192 * t198 * t204 * t206 * 2.0); - t631 = (((-(t129 * t130 * t202 * 2.0) + t23 * t24 * t203 * t206 * 2.0) - + -(t129 * t194 * t203 * t205 * 2.0)) - + -(t130 * t199 * t203 * t205 * 2.0)) - + -(t194 * t199 * t204 * t206 * 2.0); - t632 = (((-(t125 * t128 * t202 * 2.0) + (t71 + t77) * t203 * t206) - + t128 * t193 * t203 * t205 * 2.0) - + t125 * t195 * t203 * t205 * 2.0) - + -(t193 * t195 * t204 * t206 * 2.0); - t633 = (((-(t127 * t130 * t202 * 2.0) + (t77 + t85) * t203 * t206) - + -(t130 * t192 * t203 * t205 * 2.0)) - + -(t127 * t194 * t203 * t205 * 2.0)) - + -(t192 * t194 * t204 * t206 * 2.0); - t634 = (((-(t126 * t129 * t202 * 2.0) + (t71 + t85) * t203 * t206) - + -(t129 * t198 * t203 * t205 * 2.0)) - + -(t126 * t199 * t203 * t205 * 2.0)) - + -(t198 * t199 * t204 * t206 * 2.0); - t635 = (((t127 * t191 * t202 * 2.0 + -((t90 + t94) * t203 * t206)) - + t127 * t196 * t203 * t205 * 2.0) - + t191 * t192 * t203 * t205 * 2.0) - + t192 * t196 * t204 * t206 * 2.0; - t636 = (((-(t128 * t189 * t202 * 2.0) + (t95 + t99) * t203 * t206) - + t128 * t197 * t203 * t205 * 2.0) - + t189 * t195 * t203 * t205 * 2.0) - + -(t195 * t197 * t204 * t206 * 2.0); - t637 = (((t125 * t189 * t202 * 2.0 + -((t86 + t90) * t203 * t206)) - + -(t125 * t197 * t203 * t205 * 2.0)) - + -(t189 * t193 * t203 * t205 * 2.0)) - + t193 * t197 * t204 * t206 * 2.0; - t638 = (((-(t130 * t191 * t202 * 2.0) + (t99 + t103) * t203 * t206) - + -(t130 * t196 * t203 * t205 * 2.0)) - + -(t191 * t194 * t203 * t205 * 2.0)) - + -(t194 * t196 * t204 * t206 * 2.0); - t86 = (((t126 * t190 * t202 * 2.0 + -((t86 + t94) * t203 * t206)) - + t126 * t200 * t203 * t205 * 2.0) - + t190 * t198 * t203 * t205 * 2.0) - + t198 * t200 * t204 * t206 * 2.0; - t71 = (((-(t129 * t190 * t202 * 2.0) + (t95 + t103) * t203 * t206) - + -(t129 * t200 * t203 * t205 * 2.0)) - + -(t190 * t199 * t203 * t205 * 2.0)) - + -(t199 * t200 * t204 * t206 * 2.0); - t85 = (((t189 * t191 * t202 * 2.0 + t26 * t28 * t203 * t206 * 2.0) - + t189 * t196 * t203 * t205 * 2.0) - + -(t191 * t197 * t203 * t205 * 2.0)) - + -(t196 * t197 * t204 * t206 * 2.0); - t90 = (((-(t189 * t190 * t202 * 2.0) + t27 * t28 * t203 * t206 * 2.0) - + t190 * t197 * t203 * t205 * 2.0) - + -(t189 * t200 * t203 * t205 * 2.0)) - + t197 * t200 * t204 * t206 * 2.0; - t99 = (((-(t190 * t191 * t202 * 2.0) + t26 * t27 * t203 * t206 * 2.0) - + -(t190 * t196 * t203 * t205 * 2.0)) - + -(t191 * t200 * t203 * t205 * 2.0)) - + -(t196 * t200 * t204 * t206 * 2.0); - t77 = ((((-(t127 * t128 * t202 * 2.0) + t310) - + (t75 + -(t68 * 4.0)) * t203 * t206) - + t127 * t195 * t203 * t205 * 2.0) - + -(t128 * t192 * t203 * t205 * 2.0)) - + t192 * t195 * t204 * t206 * 2.0; - t131 = ((((t126 * t128 * t202 * 2.0 + -t309) - + (t80 + -(t70 * 4.0)) * t203 * t206) - + t128 * t198 * t203 * t205 * 2.0) - + -(t126 * t195 * t203 * t205 * 2.0)) - + -(t195 * t198 * t204 * t206 * 2.0); - t133 = ((((-(t125 * t130 * t202 * 2.0) + -t310) - + t646_tmp * (t78 - t67 * 4.0)) - + t130 * t193 * t203 * t205 * 2.0) - + -(t125 * t194 * t203 * t205 * 2.0)) - + t193 * t194 * t204 * t206 * 2.0; - t325 = - ((((t125 * t129 * t202 * 2.0 + t309) + t646_tmp * (t82 - t69 * 4.0)) - + t125 * t199 * t203 * t205 * 2.0) - + -(t129 * t193 * t203 * t205 * 2.0)) - + -(t193 * t199 * t204 * t206 * 2.0); - t135 = ((((t125 * t191 * t202 * 2.0 + t313) - + ((t75 + t18 * t28 * 2.0) + -t78) * t203 * t206) - + t125 * t196 * t203 * t205 * 2.0) - + -(t191 * t193 * t203 * t205 * 2.0)) - + -(t193 * t196 * t204 * t206 * 2.0); - t324 = ((((t127 * t189 * t202 * 2.0 + -t313) - + ((t78 + t22 * t26 * 2.0) + -t75) * t203 * t206) - + -(t127 * t197 * t203 * t205 * 2.0)) - + t189 * t192 * t203 * t205 * 2.0) - + -(t192 * t197 * t204 * t206 * 2.0); - t318 = ((((-(t126 * t189 * t202 * 2.0) + t312) - + ((t82 + t22 * t27 * 2.0) + -t80) * t203 * t206) - + t126 * t197 * t203 * t205 * 2.0) - + -(t189 * t198 * t203 * t205 * 2.0)) - + t197 * t198 * t204 * t206 * 2.0; - t321 = ((((-(t130 * t189 * t202 * 2.0) + t317) - + -(((t78 + t25 * t26 * 2.0) + -t75) * t203 * t206)) - + t130 * t197 * t203 * t205 * 2.0) - + -(t189 * t194 * t203 * t205 * 2.0)) - + t194 * t197 * t204 * t206 * 2.0; - t323 = ((((t129 * t191 * t202 * 2.0 + t319) - + -(((t72 + t23 * t27 * 2.0) + -t73) * t203 * t206)) - + t129 * t196 * t203 * t205 * 2.0) - + t191 * t199 * t203 * t205 * 2.0) - + t196 * t199 * t204 * t206 * 2.0; - t322 = ((((-(t125 * t190 * t202 * 2.0) + -t312) - + ((t80 + t20 * t28 * 2.0) + -t82) * t203 * t206) - + -(t125 * t200 * t203 * t205 * 2.0)) - + t190 * t193 * t203 * t205 * 2.0) - + t193 * t200 * t204 * t206 * 2.0; - t316 = ((((t130 * t190 * t202 * 2.0 + -t319) - + -(((t73 + t24 * t26 * 2.0) + -t72) * t203 * t206)) - + t130 * t200 * t203 * t205 * 2.0) - + t190 * t194 * t203 * t205 * 2.0) - + t194 * t200 * t204 * t206 * 2.0; - t65 = ((((-(t128 * t191 * t202 * 2.0) + -t317) - + -(((t75 + t23 * t28 * 2.0) + -t78) * t203 * t206)) - + -(t128 * t196 * t203 * t205 * 2.0)) - + t191 * t195 * t203 * t205 * 2.0) - + t195 * t196 * t204 * t206 * 2.0; - t66 = ((((-(t127 * t190 * t202 * 2.0) + t314) - + ((t73 + t20 * t26 * 2.0) + -t72) * t203 * t206) - + -(t127 * t200 * t203 * t205 * 2.0)) - + -(t190 * t192 * t203 * t205 * 2.0)) - + -(t192 * t200 * t204 * t206 * 2.0); - t13 = ((((t128 * t190 * t202 * 2.0 + t315) - + -(((t80 + t24 * t28 * 2.0) + -t82) * t203 * t206)) - + t128 * t200 * t203 * t205 * 2.0) - + -(t190 * t195 * t203 * t205 * 2.0)) - + -(t195 * t200 * t204 * t206 * 2.0); - t12 = ((((-(t126 * t191 * t202 * 2.0) + -t314) - + ((t72 + t18 * t27 * 2.0) + -t73) * t203 * t206) - + -(t126 * t196 * t203 * t205 * 2.0)) - + -(t191 * t198 * t203 * t205 * 2.0)) - + -(t196 * t198 * t204 * t206 * 2.0); - t11 = ((((t129 * t189 * t202 * 2.0 + -t315) - + -(((t82 + t25 * t27 * 2.0) + -t80) * t203 * t206)) - + -(t129 * t197 * t203 * t205 * 2.0)) - + t189 * t199 * t203 * t205 * 2.0) - + -(t197 * t199 * t204 * t206 * 2.0); - H[0] = t151 * t202 * 2.0; - H[1] = t262; - H[2] = t241; - H[3] = t606; - H[4] = t622; - H[5] = t625; - H[6] = t599; - H[7] = t619; - H[8] = t612; - H[9] = t603; - H[10] = t610; - H[11] = t615; - H[12] = t262; - H[13] = t150 * t202 * 2.0; - H[14] = t261; - H[15] = t621; - H[16] = t607; - H[17] = t620; - H[18] = t617; - H[19] = t602; - H[20] = t616; - H[21] = t609; - H[22] = t604; - H[23] = t608; - H[24] = t241; - H[25] = t261; - H[26] = t149 * t202 * 2.0; - H[27] = t624; - H[28] = t623; - H[29] = t605; - H[30] = t611; - H[31] = t618; - H[32] = t601; - H[33] = t614; - H[34] = t613; - H[35] = t600; - H[36] = t606; - H[37] = t621; - H[38] = t624; - H[39] = ((t191 * t191 * t202 * 2.0 + t196 * t196 * t204 * t206 * 2.0) - - t646_tmp * (t45 + t46)) - + t191 * t196 * t203 * t205 * 4.0; - H[40] = t99; - H[41] = t85; - H[42] = t638; - H[43] = t323; - H[44] = t65; - H[45] = t635; - H[46] = t12; - H[47] = t135; - H[48] = t622; - H[49] = t607; - H[50] = t623; - H[51] = t99; - H[52] = ((t190 * t190 * t202 * 2.0 + t200 * t200 * t204 * t206 * 2.0) - - t646_tmp * (t44 + t46)) - + t190 * t200 * t203 * t205 * 4.0; - H[53] = t90; - H[54] = t316; - H[55] = t71; - H[56] = t13; - H[57] = t66; - H[58] = t86; - H[59] = t322; - H[60] = t625; - H[61] = t620; - H[62] = t605; - H[63] = t85; - H[64] = t90; - H[65] = ((t189 * t189 * t202 * 2.0 + t197 * t197 * t204 * t206 * 2.0) - - t646_tmp * (t44 + t45)) - - t189 * t197 * t203 * t205 * 4.0; - H[66] = t321; - H[67] = t11; - H[68] = t636; - H[69] = t324; - H[70] = t318; - H[71] = t637; - H[72] = t599; - H[73] = t617; - H[74] = t611; - H[75] = t638; - H[76] = t316; - H[77] = t321; - H[78] = ((t130 * t130 * t202 * 2.0 + t194 * t194 * t204 * t206 * 2.0) - - t646_tmp * (t42 + t43)) - + t130 * t194 * t203 * t205 * 4.0; - H[79] = t631; - H[80] = t627; - H[81] = t633; - H[82] = t646; - H[83] = t133; - H[84] = t619; - H[85] = t602; - H[86] = t618; - H[87] = t323; - H[88] = t71; - H[89] = t11; - H[90] = t631; - H[91] = ((t129 * t129 * t202 * 2.0 + t199 * t199 * t204 * t206 * 2.0) - - t646_tmp * (t41 + t43)) - + t129 * t199 * t203 * t205 * 4.0; - H[92] = t629; - H[93] = t645; - H[94] = t634; - H[95] = t325; - H[96] = t612; - H[97] = t616; - H[98] = t601; - H[99] = t65; - H[100] = t13; - H[101] = t636; - H[102] = t627; - H[103] = t629; - H[104] = ((t128 * t128 * t202 * 2.0 + t195 * t195 * t204 * t206 * 2.0) - - t646_tmp * (t41 + t42)) - - t128 * t195 * t203 * t205 * 4.0; - H[105] = t77; - H[106] = t131; - H[107] = t632; - H[108] = t603; - H[109] = t609; - H[110] = t614; - H[111] = t635; - H[112] = t66; - H[113] = t324; - H[114] = t633; - H[115] = t645; - H[116] = t77; - H[117] = ((t127 * t127 * t202 * 2.0 + t192 * t192 * t204 * t206 * 2.0) - - t646_tmp * (t39 + t40)) - + t127 * t192 * t203 * t205 * 4.0; - H[118] = t630; - H[119] = t626; - H[120] = t610; - H[121] = t604; - H[122] = t613; - H[123] = t12; - H[124] = t86; - H[125] = t318; - H[126] = t646; - H[127] = t634; - H[128] = t131; - H[129] = t630; - H[130] = ((t126 * t126 * t202 * 2.0 + t198 * t198 * t204 * t206 * 2.0) - - t646_tmp * (t38 + t40)) - + t126 * t198 * t203 * t205 * 4.0; - H[131] = t628; - H[132] = t615; - H[133] = t608; - H[134] = t600; - H[135] = t135; - H[136] = t322; - H[137] = t637; - H[138] = t133; - H[139] = t325; - H[140] = t632; - H[141] = t626; - H[142] = t628; - H[143] = ((t125 * t125 * t202 * 2.0 + t193 * t193 * t204 * t206 * 2.0) - - t646_tmp * (t38 + t39)) - - t125 * t193 * t203 * t205 * 4.0; - } +IPC_INSTANTIATE_POINT_PLANE_AUTOGEN(float); +IPC_INSTANTIATE_POINT_PLANE_AUTOGEN(double); +#undef IPC_INSTANTIATE_POINT_PLANE_AUTOGEN -} // namespace autogen -} // namespace ipc +} // namespace ipc::autogen diff --git a/src/ipc/distance/point_plane.hpp b/src/ipc/distance/point_plane.hpp index 677feae2f..9a2416acb 100644 --- a/src/ipc/distance/point_plane.hpp +++ b/src/ipc/distance/point_plane.hpp @@ -1,19 +1,150 @@ #pragma once +#include #include namespace ipc { +// Symbolically generated derivatives +namespace autogen { + // clang-format off + template + void point_plane_distance_gradient( + T v01, T v02, T v03, T v11, T v12, T v13, T v21, T v22, T v23, T v31, T v32, T v33, T g[12]); + template + void point_plane_distance_hessian( + T v01, T v02, T v03, T v11, T v12, T v13, T v21, T v22, T v23, T v31, T v32, T v33, T H[144]); + // clang-format on +} // namespace autogen + +namespace detail { + /// @brief Compute the distance between a point and a plane. + /// @note The distance is actually squared distance. + /// @param p The point. + /// @param origin The origin of the plane. + /// @param normal The normal of the plane. + /// @return The distance between the point and plane. + template + inline T point_plane_distance( + Eigen::ConstRef> p, + Eigen::ConstRef> origin, + Eigen::ConstRef> normal) + { + const T point_to_plane = (p - origin).dot(normal); + return point_to_plane * point_to_plane / normal.squaredNorm(); + } + + /// @brief Compute the distance between a point and a plane. + /// @note The distance is actually squared distance. + /// @param p The point. + /// @param t0 The first vertex of the triangle. + /// @param t1 The second vertex of the triangle. + /// @param t2 The third vertex of the triangle. + /// @return The distance between the point and plane. + template + inline T point_plane_distance( + Eigen::ConstRef> p, + Eigen::ConstRef> t0, + Eigen::ConstRef> t1, + Eigen::ConstRef> t2) + { + // Inline the (p, origin, normal) overload to avoid two-phase lookup + // issues with dependent argument types. + return point_plane_distance( + p, t0, triangle_unnormalized_normal(t0, t1, t2)); + } + + /// @brief Compute the gradient of the distance between a point and a plane. + /// @note The distance is actually squared distance. + /// @param p The point. + /// @param origin The origin of the plane. + /// @param normal The normal of the plane. + /// @return The gradient of the distance wrt p. + template + inline Eigen::Vector3 point_plane_distance_gradient( + Eigen::ConstRef> p, + Eigen::ConstRef> origin, + Eigen::ConstRef> normal) + { + return (T(2) / normal.squaredNorm()) * (p - origin).dot(normal) + * normal; + } + + /// @brief Compute the gradient of the distance between a point and a plane. + /// @note The distance is actually squared distance. + /// @param p The point. + /// @param t0 The first vertex of the triangle. + /// @param t1 The second vertex of the triangle. + /// @param t2 The third vertex of the triangle. + /// @return The gradient of the distance wrt p, t0, t1, and t2. + template + inline Eigen::Vector point_plane_distance_gradient( + Eigen::ConstRef> p, + Eigen::ConstRef> t0, + Eigen::ConstRef> t1, + Eigen::ConstRef> t2) + { + Eigen::Vector grad; + autogen::point_plane_distance_gradient( + p[0], p[1], p[2], t0[0], t0[1], t0[2], t1[0], t1[1], t1[2], t2[0], + t2[1], t2[2], grad.data()); + return grad; + } + + /// @brief Compute the hessian of the distance between a point and a plane. + /// @note The distance is actually squared distance. + /// @param p The point. + /// @param origin The origin of the plane. + /// @param normal The normal of the plane. + /// @return The hessian of the distance wrt p. + template + inline Eigen::Matrix3 point_plane_distance_hessian( + Eigen::ConstRef> p, + Eigen::ConstRef> origin, + Eigen::ConstRef> normal) + { + return ((T(2) / normal.squaredNorm()) * normal) * normal.transpose(); + } + + /// @brief Compute the hessian of the distance between a point and a plane. + /// @note The distance is actually squared distance. + /// @param p The point. + /// @param t0 The first vertex of the triangle. + /// @param t1 The second vertex of the triangle. + /// @param t2 The third vertex of the triangle. + /// @return The hessian of the distance wrt p, t0, t1, and t2. + template + inline Eigen::Matrix point_plane_distance_hessian( + Eigen::ConstRef> p, + Eigen::ConstRef> t0, + Eigen::ConstRef> t1, + Eigen::ConstRef> t2) + { + Eigen::Matrix hess; + autogen::point_plane_distance_hessian( + p[0], p[1], p[2], t0[0], t0[1], t0[2], t1[0], t1[1], t1[2], t2[0], + t2[1], t2[2], hess.data()); + return hess; + } +} // namespace detail + /// @brief Compute the distance between a point and a plane. /// @note The distance is actually squared distance. /// @param p The point. /// @param origin The origin of the plane. /// @param normal The normal of the plane. /// @return The distance between the point and plane. -double point_plane_distance( - Eigen::ConstRef p, - Eigen::ConstRef origin, - Eigen::ConstRef normal); +template +inline auto point_plane_distance( + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& origin, + const Eigen::MatrixBase& normal) +{ + using T = typename DerivedP::Scalar; + // NOTE: explicit : the detail overload cannot deduce T from an + // arbitrary Eigen expression. + return detail::point_plane_distance(p, origin, normal); +} /// @brief Compute the distance between a point and a plane. /// @note The distance is actually squared distance. @@ -22,11 +153,20 @@ double point_plane_distance( /// @param t1 The second vertex of the triangle. /// @param t2 The third vertex of the triangle. /// @return The distance between the point and plane. -double point_plane_distance( - Eigen::ConstRef p, - Eigen::ConstRef t0, - Eigen::ConstRef t1, - Eigen::ConstRef t2); +template < + typename DerivedP, + typename DerivedT0, + typename DerivedT1, + typename DerivedT2> +inline auto point_plane_distance( + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& t0, + const Eigen::MatrixBase& t1, + const Eigen::MatrixBase& t2) +{ + using T = typename DerivedP::Scalar; + return detail::point_plane_distance(p, t0, t1, t2); +} /// @brief Compute the gradient of the distance between a point and a plane. /// @note The distance is actually squared distance. @@ -34,10 +174,15 @@ double point_plane_distance( /// @param origin The origin of the plane. /// @param normal The normal of the plane. /// @return The gradient of the distance wrt p. -Eigen::Vector3d point_plane_distance_gradient( - Eigen::ConstRef p, - Eigen::ConstRef origin, - Eigen::ConstRef normal); +template +inline auto point_plane_distance_gradient( + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& origin, + const Eigen::MatrixBase& normal) +{ + using T = typename DerivedP::Scalar; + return detail::point_plane_distance_gradient(p, origin, normal); +} /// @brief Compute the gradient of the distance between a point and a plane. /// @note The distance is actually squared distance. @@ -46,11 +191,20 @@ Eigen::Vector3d point_plane_distance_gradient( /// @param t1 The second vertex of the triangle. /// @param t2 The third vertex of the triangle. /// @return The gradient of the distance wrt p, t0, t1, and t2. -Vector12d point_plane_distance_gradient( - Eigen::ConstRef p, - Eigen::ConstRef t0, - Eigen::ConstRef t1, - Eigen::ConstRef t2); +template < + typename DerivedP, + typename DerivedT0, + typename DerivedT1, + typename DerivedT2> +inline auto point_plane_distance_gradient( + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& t0, + const Eigen::MatrixBase& t1, + const Eigen::MatrixBase& t2) +{ + using T = typename DerivedP::Scalar; + return detail::point_plane_distance_gradient(p, t0, t1, t2); +} /// @brief Compute the hessian of the distance between a point and a plane. /// @note The distance is actually squared distance. @@ -58,10 +212,15 @@ Vector12d point_plane_distance_gradient( /// @param origin The origin of the plane. /// @param normal The normal of the plane. /// @return The hessian of the distance wrt p. -Eigen::Matrix3d point_plane_distance_hessian( - Eigen::ConstRef p, - Eigen::ConstRef origin, - Eigen::ConstRef normal); +template +inline auto point_plane_distance_hessian( + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& origin, + const Eigen::MatrixBase& normal) +{ + using T = typename DerivedP::Scalar; + return detail::point_plane_distance_hessian(p, origin, normal); +} /// @brief Compute the hessian of the distance between a point and a plane. /// @note The distance is actually squared distance. @@ -70,43 +229,19 @@ Eigen::Matrix3d point_plane_distance_hessian( /// @param t1 The second vertex of the triangle. /// @param t2 The third vertex of the triangle. /// @return The hessian of the distance wrt p, t0, t1, and t2. -Matrix12d point_plane_distance_hessian( - Eigen::ConstRef p, - Eigen::ConstRef t0, - Eigen::ConstRef t1, - Eigen::ConstRef t2); - -// Symbolically generated derivatives; -namespace autogen { - void point_plane_distance_gradient( - double v01, - double v02, - double v03, - double v11, - double v12, - double v13, - double v21, - double v22, - double v23, - double v31, - double v32, - double v33, - double g[12]); - - void point_plane_distance_hessian( - double v01, - double v02, - double v03, - double v11, - double v12, - double v13, - double v21, - double v22, - double v23, - double v31, - double v32, - double v33, - double H[144]); -} // namespace autogen +template < + typename DerivedP, + typename DerivedT0, + typename DerivedT1, + typename DerivedT2> +inline auto point_plane_distance_hessian( + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& t0, + const Eigen::MatrixBase& t1, + const Eigen::MatrixBase& t2) +{ + using T = typename DerivedP::Scalar; + return detail::point_plane_distance_hessian(p, t0, t1, t2); +} } // namespace ipc diff --git a/src/ipc/distance/point_point.cpp b/src/ipc/distance/point_point.cpp deleted file mode 100644 index aab817ee9..000000000 --- a/src/ipc/distance/point_point.cpp +++ /dev/null @@ -1,42 +0,0 @@ -#include "point_point.hpp" - -namespace ipc { - -double point_point_distance( - Eigen::ConstRef p0, Eigen::ConstRef p1) -{ - return (p1 - p0).squaredNorm(); -} - -VectorMax6d point_point_distance_gradient( - Eigen::ConstRef p0, Eigen::ConstRef p1) -{ - int dim = p0.size(); - assert(p1.size() == dim); - - VectorMax6d grad(2 * dim); - - grad.head(dim) = 2.0 * (p0 - p1); - grad.tail(dim) = -grad.head(dim); - - return grad; -} - -MatrixMax6d point_point_distance_hessian( - Eigen::ConstRef p0, Eigen::ConstRef p1) -{ - int dim = p0.size(); - assert(p1.size() == dim); - - MatrixMax6d hess(2 * dim, 2 * dim); - - hess.setZero(); - hess.diagonal().setConstant(2.0); - for (int i = 0; i < dim; i++) { - hess(i, i + dim) = hess(i + dim, i) = -2; - } - - return hess; -} - -} // namespace ipc diff --git a/src/ipc/distance/point_point.hpp b/src/ipc/distance/point_point.hpp index f64c73784..af6676327 100644 --- a/src/ipc/distance/point_point.hpp +++ b/src/ipc/distance/point_point.hpp @@ -2,30 +2,145 @@ #include +#include + namespace ipc { +namespace detail { + /// @brief Compute the distance between two points. + /// @note The distance is actually squared distance. + /// @tparam T The scalar type. + /// @tparam dim The dimension (2 or 3). + /// @param p0 The first point. + /// @param p1 The second point. + /// @return The distance between p0 and p1. + template + inline T point_point_distance( + Eigen::ConstRef> p0, + Eigen::ConstRef> p1) + { + static_assert(dim == 2 || dim == 3, "point-point is only 2D or 3D"); + return (p1 - p0).squaredNorm(); + } + + /// @brief Compute the gradient of the distance between two points. + /// @note The distance is actually squared distance. + /// @tparam T The scalar type. + /// @tparam dim The dimension (2 or 3). + /// @param p0 The first point. + /// @param p1 The second point. + /// @return The computed gradient. + template + inline Eigen::Vector point_point_distance_gradient( + Eigen::ConstRef> p0, + Eigen::ConstRef> p1) + { + static_assert(dim == 2 || dim == 3, "point-point is only 2D or 3D"); + Eigen::Vector grad; + grad.template head() = T(2) * (p0 - p1); + grad.template tail() = -grad.template head(); + return grad; + } + + /// @brief Compute the hessian of the distance between two points. + /// @note The distance is actually squared distance. + /// @tparam T The scalar type. + /// @tparam dim The dimension (2 or 3). + /// @param p0 The first point. + /// @param p1 The second point. + /// @return The computed hessian. + template + inline Eigen::Matrix point_point_distance_hessian( + Eigen::ConstRef> /*p0*/, + Eigen::ConstRef> /*p1*/) + { + static_assert(dim == 2 || dim == 3, "point-point is only 2D or 3D"); + const Eigen::Matrix I2 = + T(2) * Eigen::Matrix::Identity(); + Eigen::Matrix hess; + hess.template topLeftCorner() = I2; + hess.template bottomRightCorner() = I2; + hess.template topRightCorner() = -I2; + hess.template bottomLeftCorner() = -I2; + return hess; + } +} // namespace detail + /// @brief Compute the distance between two points. +/// /// @note The distance is actually squared distance. /// @param p0 The first point. /// @param p1 The second point. /// @return The distance between p0 and p1. -double point_point_distance( - Eigen::ConstRef p0, Eigen::ConstRef p1); +template +inline auto point_point_distance( + const Eigen::MatrixBase& p0, + const Eigen::MatrixBase& p1) +{ + using T = typename DerivedP0::Scalar; + + if constexpr (dim_v == 2) { + return detail::point_point_distance(p0, p1); + } else if constexpr (dim_v == 3) { + return detail::point_point_distance(p0, p1); + } else if (p0.size() == 2) { + return detail::point_point_distance(p0, p1); + } else { + assert(p0.size() == 3); + return detail::point_point_distance(p0, p1); + } +} /// @brief Compute the gradient of the distance between two points. +/// /// @note The distance is actually squared distance. /// @param p0 The first point. /// @param p1 The second point. /// @return The computed gradient. -VectorMax6d point_point_distance_gradient( - Eigen::ConstRef p0, Eigen::ConstRef p1); +template +inline auto point_point_distance_gradient( + const Eigen::MatrixBase& p0, + const Eigen::MatrixBase& p1) +{ + using T = typename DerivedP0::Scalar; + if constexpr (dim_v == 2) { + return detail::point_point_distance_gradient(p0, p1); + } else if constexpr (dim_v == 3) { + return detail::point_point_distance_gradient(p0, p1); + } else if (p0.size() == 2) { + return VectorMax6( + detail::point_point_distance_gradient(p0, p1)); + } else { + assert(p0.size() == 3); + return VectorMax6( + detail::point_point_distance_gradient(p0, p1)); + } +} /// @brief Compute the hessian of the distance between two points. +/// /// @note The distance is actually squared distance. /// @param p0 The first point. /// @param p1 The second point. /// @return The computed hessian. -MatrixMax6d point_point_distance_hessian( - Eigen::ConstRef p0, Eigen::ConstRef p1); +template +inline auto point_point_distance_hessian( + const Eigen::MatrixBase& p0, + const Eigen::MatrixBase& p1) +{ + using T = typename DerivedP0::Scalar; + if constexpr (dim_v == 2) { + return detail::point_point_distance_hessian(p0, p1); + } else if constexpr (dim_v == 3) { + return detail::point_point_distance_hessian(p0, p1); + } else if (p0.size() == 2) { + return MatrixMax6( + detail::point_point_distance_hessian(p0, p1)); + } else { + assert(p0.size() == 3); + return MatrixMax6( + detail::point_point_distance_hessian(p0, p1)); + } +} } // namespace ipc diff --git a/src/ipc/distance/point_triangle.cpp b/src/ipc/distance/point_triangle.cpp index eac1ff64f..27d5863ba 100644 --- a/src/ipc/distance/point_triangle.cpp +++ b/src/ipc/distance/point_triangle.cpp @@ -1,22 +1,27 @@ #include "point_triangle.hpp" +#include #include #include #include +#include -#include // std::invalid_argument +namespace ipc::detail { -namespace ipc { - -double point_triangle_distance( - Eigen::ConstRef p, - Eigen::ConstRef t0, - Eigen::ConstRef t1, - Eigen::ConstRef t2, +template +T point_triangle_distance( + Eigen::ConstRef> p, + Eigen::ConstRef> t0, + Eigen::ConstRef> t1, + Eigen::ConstRef> t2, PointTriangleDistanceType dtype) { - if (dtype == PointTriangleDistanceType::AUTO) { - dtype = point_triangle_distance_type(p, t0, t1, t2); + if constexpr (std::is_floating_point_v) { + if (dtype == PointTriangleDistanceType::AUTO) { + dtype = point_triangle_distance_type(p, t0, t1, t2); + } + } else if (dtype == PointTriangleDistanceType::AUTO) { + throw_auto_requires_explicit_dtype("point_triangle_distance"); } switch (dtype) { @@ -42,59 +47,67 @@ double point_triangle_distance( return point_plane_distance(p, t0, t1, t2); default: - throw std::invalid_argument( - "Invalid distance type for point-triangle distance!"); + throw_invalid_distance_type("point_triangle_distance"); } } -Vector12d point_triangle_distance_gradient( - Eigen::ConstRef p, - Eigen::ConstRef t0, - Eigen::ConstRef t1, - Eigen::ConstRef t2, +template +Eigen::Vector point_triangle_distance_gradient( + Eigen::ConstRef> p, + Eigen::ConstRef> t0, + Eigen::ConstRef> t1, + Eigen::ConstRef> t2, PointTriangleDistanceType dtype) { - if (dtype == PointTriangleDistanceType::AUTO) { - dtype = point_triangle_distance_type(p, t0, t1, t2); + if constexpr (std::is_floating_point_v) { + if (dtype == PointTriangleDistanceType::AUTO) { + dtype = point_triangle_distance_type(p, t0, t1, t2); + } + } else if (dtype == PointTriangleDistanceType::AUTO) { + throw_auto_requires_explicit_dtype("point_triangle_distance_gradient"); } - Vector12d grad = Vector12d::Zero(); + Eigen::Vector grad = Eigen::Vector::Zero(); switch (dtype) { case PointTriangleDistanceType::P_T0: - grad.head<6>() = point_point_distance_gradient(p, t0); + grad.template head<6>() = point_point_distance_gradient(p, t0); break; case PointTriangleDistanceType::P_T1: { - const Vector6d local_grad = point_point_distance_gradient(p, t1); - grad.head<3>() = local_grad.head<3>(); - grad.segment<3>(6) = local_grad.tail<3>(); + const Eigen::Vector local_grad = + point_point_distance_gradient(p, t1); + grad.template head<3>() = local_grad.template head<3>(); + grad.template segment<3>(6) = local_grad.template tail<3>(); break; } case PointTriangleDistanceType::P_T2: { - const Vector6d local_grad = point_point_distance_gradient(p, t2); - grad.head<3>() = local_grad.head<3>(); - grad.tail<3>() = local_grad.tail<3>(); + const Eigen::Vector local_grad = + point_point_distance_gradient(p, t2); + grad.template head<3>() = local_grad.template head<3>(); + grad.template tail<3>() = local_grad.template tail<3>(); break; } case PointTriangleDistanceType::P_E0: - grad.head<9>() = point_line_distance_gradient(p, t0, t1); + grad.template head<9>() = point_line_distance_gradient(p, t0, t1); break; case PointTriangleDistanceType::P_E1: { - const Vector9d local_grad = point_line_distance_gradient(p, t1, t2); - grad.head<3>() = local_grad.head<3>(); - grad.tail<6>() = local_grad.tail<6>(); + const Eigen::Vector local_grad = + point_line_distance_gradient(p, t1, t2); + grad.template head<3>() = local_grad.template head<3>(); + grad.template tail<6>() = local_grad.template tail<6>(); break; } case PointTriangleDistanceType::P_E2: { - const Vector9d local_grad = point_line_distance_gradient(p, t2, t0); - grad.head<3>() = local_grad.head<3>(); // ∇_p - grad.segment<3>(3) = local_grad.tail<3>(); // ∇_{t0} - grad.tail<3>() = local_grad.segment<3>(3); // ∇_{t2} + const Eigen::Vector local_grad = + point_line_distance_gradient(p, t2, t0); + grad.template head<3>() = local_grad.template head<3>(); // ∇_p + grad.template segment<3>(3) = local_grad.template tail<3>(); // ∇_{t0} + grad.template tail<3>() = local_grad.template segment<3>(3); // ∇_{t2} break; } @@ -103,73 +116,102 @@ Vector12d point_triangle_distance_gradient( break; default: - throw std::invalid_argument( - "Invalid distance type for point-triangle distance gradient!"); + throw_invalid_distance_type("point_triangle_distance_gradient"); } return grad; } -Matrix12d point_triangle_distance_hessian( - Eigen::ConstRef p, - Eigen::ConstRef t0, - Eigen::ConstRef t1, - Eigen::ConstRef t2, +template +Eigen::Matrix point_triangle_distance_hessian( + Eigen::ConstRef> p, + Eigen::ConstRef> t0, + Eigen::ConstRef> t1, + Eigen::ConstRef> t2, PointTriangleDistanceType dtype) { - if (dtype == PointTriangleDistanceType::AUTO) { - dtype = point_triangle_distance_type(p, t0, t1, t2); + if constexpr (std::is_floating_point_v) { + if (dtype == PointTriangleDistanceType::AUTO) { + dtype = point_triangle_distance_type(p, t0, t1, t2); + } + } else if (dtype == PointTriangleDistanceType::AUTO) { + throw_auto_requires_explicit_dtype("point_triangle_distance_hessian"); } - Matrix12d hess = Matrix12d::Zero(); + Eigen::Matrix hess = Eigen::Matrix::Zero(); switch (dtype) { case PointTriangleDistanceType::P_T0: - hess.topLeftCorner<6, 6>() = point_point_distance_hessian(p, t0); + hess.template topLeftCorner<6, 6>() = + point_point_distance_hessian(p, t0); break; case PointTriangleDistanceType::P_T1: { - const Matrix6d local_hess = point_point_distance_hessian(p, t1); - hess.topLeftCorner<3, 3>() = local_hess.topLeftCorner<3, 3>(); - hess.block<3, 3>(0, 6) = local_hess.topRightCorner<3, 3>(); - hess.block<3, 3>(6, 0) = local_hess.bottomLeftCorner<3, 3>(); - hess.block<3, 3>(6, 6) = local_hess.bottomRightCorner<3, 3>(); + const Eigen::Matrix local_hess = + point_point_distance_hessian(p, t1); + hess.template topLeftCorner<3, 3>() = + local_hess.template topLeftCorner<3, 3>(); + hess.template block<3, 3>(0, 6) = + local_hess.template topRightCorner<3, 3>(); + hess.template block<3, 3>(6, 0) = + local_hess.template bottomLeftCorner<3, 3>(); + hess.template block<3, 3>(6, 6) = + local_hess.template bottomRightCorner<3, 3>(); break; } case PointTriangleDistanceType::P_T2: { - const Matrix6d local_hess = point_point_distance_hessian(p, t2); - hess.topLeftCorner<3, 3>() = local_hess.topLeftCorner<3, 3>(); - hess.topRightCorner<3, 3>() = local_hess.topRightCorner<3, 3>(); - hess.bottomLeftCorner<3, 3>() = local_hess.bottomLeftCorner<3, 3>(); - hess.bottomRightCorner<3, 3>() = local_hess.bottomRightCorner<3, 3>(); + const Eigen::Matrix local_hess = + point_point_distance_hessian(p, t2); + hess.template topLeftCorner<3, 3>() = + local_hess.template topLeftCorner<3, 3>(); + hess.template topRightCorner<3, 3>() = + local_hess.template topRightCorner<3, 3>(); + hess.template bottomLeftCorner<3, 3>() = + local_hess.template bottomLeftCorner<3, 3>(); + hess.template bottomRightCorner<3, 3>() = + local_hess.template bottomRightCorner<3, 3>(); break; } case PointTriangleDistanceType::P_E0: - hess.topLeftCorner<9, 9>() = point_line_distance_hessian(p, t0, t1); + hess.template topLeftCorner<9, 9>() = + point_line_distance_hessian(p, t0, t1); break; case PointTriangleDistanceType::P_E1: { - const Matrix9d local_hess = point_line_distance_hessian(p, t1, t2); - hess.topLeftCorner<3, 3>() = local_hess.topLeftCorner<3, 3>(); - hess.topRightCorner<3, 6>() = local_hess.topRightCorner<3, 6>(); - hess.bottomLeftCorner<6, 3>() = local_hess.bottomLeftCorner<6, 3>(); - hess.bottomRightCorner<6, 6>() = local_hess.bottomRightCorner<6, 6>(); + const Eigen::Matrix local_hess = + point_line_distance_hessian(p, t1, t2); + hess.template topLeftCorner<3, 3>() = + local_hess.template topLeftCorner<3, 3>(); + hess.template topRightCorner<3, 6>() = + local_hess.template topRightCorner<3, 6>(); + hess.template bottomLeftCorner<6, 3>() = + local_hess.template bottomLeftCorner<6, 3>(); + hess.template bottomRightCorner<6, 6>() = + local_hess.template bottomRightCorner<6, 6>(); break; } case PointTriangleDistanceType::P_E2: { - const Matrix9d local_hess = point_line_distance_hessian(p, t2, t0); - hess.topLeftCorner<3, 3>() = local_hess.topLeftCorner<3, 3>(); - hess.block<3, 3>(0, 3) = local_hess.topRightCorner<3, 3>(); - hess.topRightCorner<3, 3>() = local_hess.block<3, 3>(0, 3); - hess.block<3, 3>(3, 0) = local_hess.bottomLeftCorner<3, 3>(); - hess.block<3, 3>(3, 3) = local_hess.bottomRightCorner<3, 3>(); - hess.block<3, 3>(3, 9) = local_hess.block<3, 3>(6, 3); - hess.bottomLeftCorner<3, 3>() = local_hess.block<3, 3>(3, 0); - hess.block<3, 3>(9, 3) = local_hess.block<3, 3>(3, 6); - hess.bottomRightCorner<3, 3>() = local_hess.block<3, 3>(3, 3); + const Eigen::Matrix local_hess = + point_line_distance_hessian(p, t2, t0); + hess.template topLeftCorner<3, 3>() = + local_hess.template topLeftCorner<3, 3>(); + hess.template block<3, 3>(0, 3) = + local_hess.template topRightCorner<3, 3>(); + hess.template topRightCorner<3, 3>() = + local_hess.template block<3, 3>(0, 3); + hess.template block<3, 3>(3, 0) = + local_hess.template bottomLeftCorner<3, 3>(); + hess.template block<3, 3>(3, 3) = + local_hess.template bottomRightCorner<3, 3>(); + hess.template block<3, 3>(3, 9) = local_hess.template block<3, 3>(6, 3); + hess.template bottomLeftCorner<3, 3>() = + local_hess.template block<3, 3>(3, 0); + hess.template block<3, 3>(9, 3) = local_hess.template block<3, 3>(3, 6); + hess.template bottomRightCorner<3, 3>() = + local_hess.template block<3, 3>(3, 3); break; } @@ -178,11 +220,46 @@ Matrix12d point_triangle_distance_hessian( break; default: - throw std::invalid_argument( - "Invalid distance type for point-triangle distance hessian!"); + throw_invalid_distance_type("point_triangle_distance_hessian"); } return hess; } -} // namespace ipc +#define IPC_INSTANTIATE_POINT_TRIANGLE(T) \ + template T point_triangle_distance( \ + Eigen::ConstRef>, \ + Eigen::ConstRef>, \ + Eigen::ConstRef>, \ + Eigen::ConstRef>, PointTriangleDistanceType); \ + template Eigen::Vector point_triangle_distance_gradient( \ + Eigen::ConstRef>, \ + Eigen::ConstRef>, \ + Eigen::ConstRef>, \ + Eigen::ConstRef>, PointTriangleDistanceType); \ + template Eigen::Matrix point_triangle_distance_hessian( \ + Eigen::ConstRef>, \ + Eigen::ConstRef>, \ + Eigen::ConstRef>, \ + Eigen::ConstRef>, PointTriangleDistanceType) + +// Autodiff scalars: value only. Gradients/Hessians are intentionally not +// supported for them. +#define IPC_INSTANTIATE_POINT_TRIANGLE_VALUE(T) \ + template T point_triangle_distance( \ + Eigen::ConstRef>, \ + Eigen::ConstRef>, \ + Eigen::ConstRef>, \ + Eigen::ConstRef>, PointTriangleDistanceType) + +IPC_INSTANTIATE_POINT_TRIANGLE(float); +IPC_INSTANTIATE_POINT_TRIANGLE(double); +IPC_INSTANTIATE_POINT_TRIANGLE_VALUE(ADGrad<12>); +IPC_INSTANTIATE_POINT_TRIANGLE_VALUE(ADHessian<12>); +IPC_INSTANTIATE_POINT_TRIANGLE_VALUE(ADGrad<13>); +IPC_INSTANTIATE_POINT_TRIANGLE_VALUE(ADHessian<13>); + +#undef IPC_INSTANTIATE_POINT_TRIANGLE +#undef IPC_INSTANTIATE_POINT_TRIANGLE_VALUE + +} // namespace ipc::detail diff --git a/src/ipc/distance/point_triangle.hpp b/src/ipc/distance/point_triangle.hpp index a7bbd0642..1ae668e7e 100644 --- a/src/ipc/distance/point_triangle.hpp +++ b/src/ipc/distance/point_triangle.hpp @@ -1,9 +1,62 @@ #pragma once #include +#include namespace ipc { +namespace detail { + /// @brief Compute the distance between a points and a triangle. + /// @note The distance is actually squared distance. + /// @param p The point. + /// @param t0 The first vertex of the triangle. + /// @param t1 The second vertex of the triangle. + /// @param t2 The third vertex of the triangle. + /// @param dtype The point-triangle distance type to compute. + /// @return The distance between the point and triangle. + template + T point_triangle_distance( + Eigen::ConstRef> p, + Eigen::ConstRef> t0, + Eigen::ConstRef> t1, + Eigen::ConstRef> t2, + PointTriangleDistanceType dtype = PointTriangleDistanceType::AUTO); + + /// @brief Compute the gradient of the distance between a points and a triangle. + /// @note The distance is actually squared distance. + /// @param p The point. + /// @param t0 The first vertex of the triangle. + /// @param t1 The second vertex of the triangle. + /// @param t2 The third vertex of the triangle. + /// @param dtype The point-triangle distance type to compute. + /// @return The gradient of the distance wrt p, t0, t1, and t2. + template + Eigen::Vector point_triangle_distance_gradient( + Eigen::ConstRef> p, + Eigen::ConstRef> t0, + Eigen::ConstRef> t1, + Eigen::ConstRef> t2, + PointTriangleDistanceType dtype = PointTriangleDistanceType::AUTO); + + /// @brief Compute the hessian of the distance between a points and a triangle. + /// @note The distance is actually squared distance. + /// @param p The point. + /// @param t0 The first vertex of the triangle. + /// @param t1 The second vertex of the triangle. + /// @param t2 The third vertex of the triangle. + /// @param dtype The point-triangle distance type to compute. + /// @return The hessian of the distance wrt p, t0, t1, and t2. + template + Eigen::Matrix point_triangle_distance_hessian( + Eigen::ConstRef> p, + Eigen::ConstRef> t0, + Eigen::ConstRef> t1, + Eigen::ConstRef> t2, + PointTriangleDistanceType dtype = PointTriangleDistanceType::AUTO); +} // namespace detail + +// --- EigenExpression wrappers --- + /// @brief Compute the distance between a points and a triangle. /// @note The distance is actually squared distance. /// @param p The point. @@ -12,12 +65,21 @@ namespace ipc { /// @param t2 The third vertex of the triangle. /// @param dtype The point-triangle distance type to compute. /// @return The distance between the point and triangle. -double point_triangle_distance( - Eigen::ConstRef p, - Eigen::ConstRef t0, - Eigen::ConstRef t1, - Eigen::ConstRef t2, - PointTriangleDistanceType dtype = PointTriangleDistanceType::AUTO); +template < + typename DerivedP, + typename DerivedT0, + typename DerivedT1, + typename DerivedT2> +inline auto point_triangle_distance( + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& t0, + const Eigen::MatrixBase& t1, + const Eigen::MatrixBase& t2, + PointTriangleDistanceType dtype = PointTriangleDistanceType::AUTO) +{ + using T = typename DerivedP::Scalar; + return detail::point_triangle_distance(p, t0, t1, t2, dtype); +} /// @brief Compute the gradient of the distance between a points and a triangle. /// @note The distance is actually squared distance. @@ -27,12 +89,21 @@ double point_triangle_distance( /// @param t2 The third vertex of the triangle. /// @param dtype The point-triangle distance type to compute. /// @return The gradient of the distance wrt p, t0, t1, and t2. -Vector12d point_triangle_distance_gradient( - Eigen::ConstRef p, - Eigen::ConstRef t0, - Eigen::ConstRef t1, - Eigen::ConstRef t2, - PointTriangleDistanceType dtype = PointTriangleDistanceType::AUTO); +template < + typename DerivedP, + typename DerivedT0, + typename DerivedT1, + typename DerivedT2> +inline auto point_triangle_distance_gradient( + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& t0, + const Eigen::MatrixBase& t1, + const Eigen::MatrixBase& t2, + PointTriangleDistanceType dtype = PointTriangleDistanceType::AUTO) +{ + using T = typename DerivedP::Scalar; + return detail::point_triangle_distance_gradient(p, t0, t1, t2, dtype); +} /// @brief Compute the hessian of the distance between a points and a triangle. /// @note The distance is actually squared distance. @@ -42,11 +113,20 @@ Vector12d point_triangle_distance_gradient( /// @param t2 The third vertex of the triangle. /// @param dtype The point-triangle distance type to compute. /// @return The hessian of the distance wrt p, t0, t1, and t2. -Matrix12d point_triangle_distance_hessian( - Eigen::ConstRef p, - Eigen::ConstRef t0, - Eigen::ConstRef t1, - Eigen::ConstRef t2, - PointTriangleDistanceType dtype = PointTriangleDistanceType::AUTO); +template < + typename DerivedP, + typename DerivedT0, + typename DerivedT1, + typename DerivedT2> +inline auto point_triangle_distance_hessian( + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& t0, + const Eigen::MatrixBase& t1, + const Eigen::MatrixBase& t2, + PointTriangleDistanceType dtype = PointTriangleDistanceType::AUTO) +{ + using T = typename DerivedP::Scalar; + return detail::point_triangle_distance_hessian(p, t0, t1, t2, dtype); +} } // namespace ipc diff --git a/src/ipc/distance/signed/line_line.cpp b/src/ipc/distance/signed/line_line.cpp index 0ec94fd71..b010cad45 100644 --- a/src/ipc/distance/signed/line_line.cpp +++ b/src/ipc/distance/signed/line_line.cpp @@ -1,40 +1,24 @@ #include "line_line.hpp" -namespace ipc { - -Vector12d line_line_signed_distance_gradient( - Eigen::ConstRef ea0, - Eigen::ConstRef ea1, - Eigen::ConstRef eb0, - Eigen::ConstRef eb1) -{ - const Eigen::Vector3d n = line_line_normal(ea0, ea1, eb0, eb1); - const Eigen::Matrix jac_n = - line_line_normal_jacobian(ea0, ea1, eb0, eb1); - - Vector12d grad = jac_n.transpose() * (ea0 - eb0); - grad.segment<3>(0) += n; - grad.segment<3>(6) -= n; - - return grad; -} - -Matrix12d line_line_signed_distance_hessian( - Eigen::ConstRef ea0, - Eigen::ConstRef ea1, - Eigen::ConstRef eb0, - Eigen::ConstRef eb1) +namespace ipc::detail { + +template +Eigen::Matrix line_line_signed_distance_hessian( + Eigen::ConstRef> ea0, + Eigen::ConstRef> ea1, + Eigen::ConstRef> eb0, + Eigen::ConstRef> eb1) { // Precompute normal's Jacobian and Hessian - const Eigen::Matrix jac_n = + const Eigen::Matrix jac_n = line_line_normal_jacobian(ea0, ea1, eb0, eb1); - const Eigen::Matrix hess_n = + const Eigen::Matrix hess_n = line_line_normal_hessian(ea0, ea1, eb0, eb1); // Vector from eb0 to ea0 (Distance vector) - const Eigen::Vector3d v = ea0 - eb0; + const Eigen::Vector3 v = ea0 - eb0; - Matrix12d hess; + Eigen::Matrix hess; // --------------------------------------------------------- // 1. Tensor Contraction (Curvature Term) @@ -54,36 +38,41 @@ Matrix12d line_line_signed_distance_hessian( // Extract 3x3 sub-blocks for cleaner code // Indices: 0->ea0, 1->ea1, 2->eb0, 3->eb1 - const auto J_a0 = jac_n.leftCols<3>(); - const auto J_a1 = jac_n.middleCols<3>(3); - const auto J_b0 = jac_n.middleCols<3>(6); - const auto J_b1 = jac_n.rightCols<3>(); + const auto J_a0 = jac_n.template leftCols<3>(); + const auto J_a1 = jac_n.template middleCols<3>(3); + const auto J_b0 = jac_n.template middleCols<3>(6); + const auto J_b1 = jac_n.template rightCols<3>(); // --- Block Row 0 (ea0) --- // d/d(ea0) [Jₙᵀ v + n] -> adds J + Jᵀ terms - hess.block<3, 3>(0, 0) += J_a0 + J_a0.transpose(); - hess.block<3, 3>(0, 3) += J_a1; - hess.block<3, 3>(0, 6) += J_b0 - J_a0.transpose(); - hess.block<3, 3>(0, 9) += J_b1; + hess.template block<3, 3>(0, 0) += J_a0 + J_a0.transpose(); + hess.template block<3, 3>(0, 3) += J_a1; + hess.template block<3, 3>(0, 6) += J_b0 - J_a0.transpose(); + hess.template block<3, 3>(0, 9) += J_b1; // --- Block Row 1 (ea1) --- // d/d(ea1) [Jₙᵀ v] -> adds Jᵀ terms - hess.block<3, 3>(3, 0) += J_a1.transpose(); - hess.block<3, 3>(3, 6) -= J_a1.transpose(); + hess.template block<3, 3>(3, 0) += J_a1.transpose(); + hess.template block<3, 3>(3, 6) -= J_a1.transpose(); // --- Block Row 2 (eb0) --- // d/d(eb0) [Jₙᵀ v - n] -> adds -J - Jᵀ terms - hess.block<3, 3>(6, 0) += J_b0.transpose() - J_a0; - hess.block<3, 3>(6, 3) -= J_a1; - hess.block<3, 3>(6, 6) -= J_b0 + J_b0.transpose(); - hess.block<3, 3>(6, 9) -= J_b1; + hess.template block<3, 3>(6, 0) += J_b0.transpose() - J_a0; + hess.template block<3, 3>(6, 3) -= J_a1; + hess.template block<3, 3>(6, 6) -= J_b0 + J_b0.transpose(); + hess.template block<3, 3>(6, 9) -= J_b1; // --- Block Row 3 (eb1) --- // d/d(eb1) [Jₙᵀ v] -> adds Jᵀ terms - hess.block<3, 3>(9, 0) += J_b1.transpose(); - hess.block<3, 3>(9, 6) -= J_b1.transpose(); + hess.template block<3, 3>(9, 0) += J_b1.transpose(); + hess.template block<3, 3>(9, 6) -= J_b1.transpose(); return hess; } -} // namespace ipc \ No newline at end of file +// clang-format off +template Matrix12f line_line_signed_distance_hessian(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef); +template Matrix12d line_line_signed_distance_hessian(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef); +// clang-format on + +} // namespace ipc::detail diff --git a/src/ipc/distance/signed/line_line.hpp b/src/ipc/distance/signed/line_line.hpp index f60ce5fa7..34fa41a86 100644 --- a/src/ipc/distance/signed/line_line.hpp +++ b/src/ipc/distance/signed/line_line.hpp @@ -1,9 +1,102 @@ #pragma once #include +#include namespace ipc { +namespace detail { + /// Compute the signed distance between two lines in 3D. + /// + /// The two lines are specified by points (ea0, ea1) and (eb0, eb1). + /// The returned value is the scalar projection of the vector (ea0 - eb0) + /// onto the unit normal that is perpendicular to both lines (as produced by + /// line_line_normal). In other words, this is the signed distance along the + /// common normal between the two lines; the sign follows the orientation of + /// the normal returned by line_line_normal. + /// + /// @param ea0 First endpoint (or a point) on the first line. + /// @param ea1 Second endpoint (or a direction-defining point) on the first line. + /// @param eb0 First endpoint (or a point) on the second line. + /// @param eb1 Second endpoint (or a direction-defining point) on the second line. + /// @return Signed distance between the two lines along the common normal. If + /// the lines are parallel (no unique common normal), the result may be + /// ill-conditioned or implementation-defined. + /// @note The points may be any two distinct points on each line (they need not + /// represent segment endpoints). Behavior is undefined if ea0 == ea1 or eb0 + /// == eb1. + template + inline T line_line_signed_distance( + Eigen::ConstRef> ea0, + Eigen::ConstRef> ea1, + Eigen::ConstRef> eb0, + Eigen::ConstRef> eb1) + { + return line_line_normal(ea0, ea1, eb0, eb1).dot(ea0 - eb0); + } + + /// Compute the gradient of the signed line-line distance with respect to + /// the four 3D input points: ea0, ea1, eb0, eb1. + /// + /// The returned gradient is a 12-vector ordered as [d/d(ea0); d/d(ea1); + /// d/d(eb0); d/d(eb1)], where each block is a 3-vector. This function + /// differentiates the signed distance produced by + /// line_line_signed_distance. The gradient may be undefined or numerically + /// unstable when the two lines are parallel or when the direction-defining + /// points coincide. + /// + /// @param ea0 First point on the first line (as in line_line_signed_distance). + /// @param ea1 Second point on the first line. + /// @param eb0 First point on the second line. + /// @param eb1 Second point on the second line. + /// @return 12-dimensional gradient of the signed distance with respect to + /// [ea0, ea1, eb0, eb1]. + /// @pre ea0 != ea1 and eb0 != eb1. For near-parallel lines, results may be unstable. + /// + /// @see line_line_signed_distance, line_line_normal + template + inline Eigen::Vector line_line_signed_distance_gradient( + Eigen::ConstRef> ea0, + Eigen::ConstRef> ea1, + Eigen::ConstRef> eb0, + Eigen::ConstRef> eb1) + { + const Eigen::Vector3 n = line_line_normal(ea0, ea1, eb0, eb1); + const Eigen::Matrix jac_n = + line_line_normal_jacobian(ea0, ea1, eb0, eb1); + Eigen::Vector grad = jac_n.transpose() * (ea0 - eb0); + grad.template segment<3>(0) += n; + grad.template segment<3>(6) -= n; + return grad; + } + + /// Compute the Hessian (second derivative) of the signed line-line distance + /// with respect to the four 3D input points: ea0, ea1, eb0, eb1. + /// + /// The returned matrix is 12x12 and corresponds to the second derivatives + /// of the scalar signed distance with respect to the stacked variable + /// [ea0; ea1; eb0; eb1]. The Hessian captures curvature information and is + /// typically required for second-order optimization or sensitivity + /// analysis. As with the gradient, the Hessian is undefined or numerically + /// unstable if either input line is degenerate (coincident points) or if + /// the lines are parallel (no unique perpendicular direction). + /// + /// @param ea0 First point on the first line. + /// @param ea1 Second point on the first line. + /// @param eb0 First point on the second line. + /// @param eb1 Second point on the second line. + /// @return 12x12 Hessian matrix of second derivatives of the signed distance. + /// @pre ea0 != ea1 and eb0 != eb1. Results may be invalid for parallel lines. + /// + /// @see line_line_signed_distance, line_line_signed_distance_gradient + template + Eigen::Matrix line_line_signed_distance_hessian( + Eigen::ConstRef> ea0, + Eigen::ConstRef> ea1, + Eigen::ConstRef> eb0, + Eigen::ConstRef> eb1); +} // namespace detail + /// Compute the signed distance between two lines in 3D. /// /// The two lines are specified by points (ea0, ea1) and (eb0, eb1). @@ -21,15 +114,23 @@ namespace ipc { /// the lines are parallel (no unique common normal), the result may be /// ill-conditioned or implementation-defined. /// @note The points may be any two distinct points on each line (they need not -/// represent segment endpoints). Behavior is undefined if ea0 == ea1 or eb0 == -/// eb1. -inline double line_line_signed_distance( - Eigen::ConstRef ea0, - Eigen::ConstRef ea1, - Eigen::ConstRef eb0, - Eigen::ConstRef eb1) +/// represent segment endpoints). Behavior is undefined if ea0 == ea1 or eb0 +/// == eb1. +template < + typename DerivedEA0, + typename DerivedEA1, + typename DerivedEB0, + typename DerivedEB1> +inline auto line_line_signed_distance( + const Eigen::MatrixBase& ea0, + const Eigen::MatrixBase& ea1, + const Eigen::MatrixBase& eb0, + const Eigen::MatrixBase& eb1) { - return line_line_normal(ea0, ea1, eb0, eb1).dot(ea0 - eb0); + using T = typename DerivedEA0::Scalar; + // NOTE: explicit : the detail overload cannot deduce T from an + // arbitrary Eigen expression. + return detail::line_line_signed_distance(ea0, ea1, eb0, eb1); } /// Compute the gradient of the signed line-line distance with respect to the @@ -50,20 +151,29 @@ inline double line_line_signed_distance( /// @pre ea0 != ea1 and eb0 != eb1. For near-parallel lines, results may be unstable. /// /// @see line_line_signed_distance, line_line_normal -Vector12d line_line_signed_distance_gradient( - Eigen::ConstRef ea0, - Eigen::ConstRef ea1, - Eigen::ConstRef eb0, - Eigen::ConstRef eb1); +template < + typename DerivedEA0, + typename DerivedEA1, + typename DerivedEB0, + typename DerivedEB1> +inline auto line_line_signed_distance_gradient( + const Eigen::MatrixBase& ea0, + const Eigen::MatrixBase& ea1, + const Eigen::MatrixBase& eb0, + const Eigen::MatrixBase& eb1) +{ + using T = typename DerivedEA0::Scalar; + return detail::line_line_signed_distance_gradient(ea0, ea1, eb0, eb1); +} /// Compute the Hessian (second derivative) of the signed line-line distance /// with respect to the four 3D input points: ea0, ea1, eb0, eb1. /// -/// The returned matrix is 12x12 and corresponds to the second derivatives -/// of the scalar signed distance with respect to the stacked variable +/// The returned matrix is 12x12 and corresponds to the second derivatives of +/// the scalar signed distance with respect to the stacked variable /// [ea0; ea1; eb0; eb1]. The Hessian captures curvature information and is -/// typically required for second-order optimization or sensitivity analysis. -/// As with the gradient, the Hessian is undefined or numerically unstable if +/// typically required for second-order optimization or sensitivity analysis. As +/// with the gradient, the Hessian is undefined or numerically unstable if /// either input line is degenerate (coincident points) or if the lines are /// parallel (no unique perpendicular direction). /// @@ -75,10 +185,19 @@ Vector12d line_line_signed_distance_gradient( /// @pre ea0 != ea1 and eb0 != eb1. Results may be invalid for parallel lines. /// /// @see line_line_signed_distance, line_line_signed_distance_gradient -Matrix12d line_line_signed_distance_hessian( - Eigen::ConstRef ea0, - Eigen::ConstRef ea1, - Eigen::ConstRef eb0, - Eigen::ConstRef eb1); +template < + typename DerivedEA0, + typename DerivedEA1, + typename DerivedEB0, + typename DerivedEB1> +inline auto line_line_signed_distance_hessian( + const Eigen::MatrixBase& ea0, + const Eigen::MatrixBase& ea1, + const Eigen::MatrixBase& eb0, + const Eigen::MatrixBase& eb1) +{ + using T = typename DerivedEA0::Scalar; + return detail::line_line_signed_distance_hessian(ea0, ea1, eb0, eb1); +} -} // namespace ipc \ No newline at end of file +} // namespace ipc diff --git a/src/ipc/distance/signed/point_line.cpp b/src/ipc/distance/signed/point_line.cpp index 354d8d8cc..7ad051ca4 100644 --- a/src/ipc/distance/signed/point_line.cpp +++ b/src/ipc/distance/signed/point_line.cpp @@ -1,38 +1,22 @@ #include "point_line.hpp" -namespace ipc { +namespace ipc::detail { -Vector6d point_line_signed_distance_gradient( - Eigen::ConstRef p, - Eigen::ConstRef e0, - Eigen::ConstRef e1) -{ - const Eigen::Vector2d n = point_line_normal(p, e0, e1); - const Eigen::Matrix jac_n = - point_line_normal_jacobian(p, e0, e1); - - Vector6d grad = jac_n.transpose() * (p - e0); - grad.segment<2>(0) += n; - grad.segment<2>(2) -= n; - - return grad; -} - -Matrix6d point_line_signed_distance_hessian( - Eigen::ConstRef p, - Eigen::ConstRef e0, - Eigen::ConstRef e1) +template +Eigen::Matrix point_line_signed_distance_hessian( + Eigen::ConstRef> p, + Eigen::ConstRef> e0, + Eigen::ConstRef> e1) { // Precompute normal's Jacobian and Hessian - const Eigen::Matrix jac_n = - point_line_normal_jacobian(p, e0, e1); - const Eigen::Matrix hess_n = - point_line_normal_hessian(p, e0, e1); - + const Eigen::Matrix jac_n = + point_line_normal_jacobian(p, e0, e1); + const Eigen::Matrix hess_n = + point_line_normal_hessian(p, e0, e1); // Vector from e0 to p - const Eigen::Vector2d v = p - e0; + const Eigen::Vector2 v = p - e0; - Matrix6d hess; + Eigen::Matrix hess; // --------------------------------------------------------- // 1. Tensor Contraction (Curvature Term) @@ -50,39 +34,44 @@ Matrix6d point_line_signed_distance_hessian( // Extract 2x2 Jacobian blocks for e₀ and e₁ // Note: ∇ n has columns 0-1 (p), 2-3 (e₀), 4-5 (e₁). // Normal usually doesn't depend on p, so block(0,0) is zero. - assert(bool(jac_n.leftCols<2>().isZero())); - const auto J_e0 = jac_n.middleCols<2>(2); - const auto J_e1 = jac_n.rightCols<2>(); + assert(bool(jac_n.template leftCols<2>().isZero())); + const auto J_e0 = jac_n.template middleCols<2>(2); + const auto J_e1 = jac_n.template rightCols<2>(); // --- Block Row 0 (p) --- // d/dp [Jₙᵀ v] -> Jₙᵀ => Adds Jᵀ to the row // (p, e0) += J_e0 - hess.block<2, 2>(0, 2) += J_e0; + hess.template block<2, 2>(0, 2) += J_e0; // (p, e1) += J_e1 - hess.block<2, 2>(0, 4) += J_e1; + hess.template block<2, 2>(0, 4) += J_e1; // --- Block Row 1 (e0) --- // d/de0 [Jₙᵀ * v + n] -> Jₙᵀ * (-I) + (-I)ᵀ * Jₙ // (e0, p) += J_e0^T - hess.block<2, 2>(2, 0) += J_e0.transpose(); + hess.template block<2, 2>(2, 0) += J_e0.transpose(); // (e0, e0) -= (J_e0 + J_e0^T) - hess.block<2, 2>(2, 2) -= (J_e0 + J_e0.transpose()); + hess.template block<2, 2>(2, 2) -= (J_e0 + J_e0.transpose()); // (e0, e1) -= J_e1 - hess.block<2, 2>(2, 4) -= J_e1; + hess.template block<2, 2>(2, 4) -= J_e1; // --- Block Row 2 (e1) --- // d/de1 [Jₙᵀ * v] -> Jₙᵀ (-I) // (e1, p) += J_e1^T - hess.block<2, 2>(4, 0) += J_e1.transpose(); + hess.template block<2, 2>(4, 0) += J_e1.transpose(); // (e1, e0) -= J_e1^T - hess.block<2, 2>(4, 2) -= J_e1.transpose(); + hess.template block<2, 2>(4, 2) -= J_e1.transpose(); return hess; } -} // namespace ipc \ No newline at end of file +// clang-format off +template Eigen::Matrix point_line_signed_distance_hessian(Eigen::ConstRef>, Eigen::ConstRef>, Eigen::ConstRef>); +template Eigen::Matrix point_line_signed_distance_hessian(Eigen::ConstRef>, Eigen::ConstRef>, Eigen::ConstRef>); +// clang-format on + +} // namespace ipc::detail diff --git a/src/ipc/distance/signed/point_line.hpp b/src/ipc/distance/signed/point_line.hpp index a9be7434f..fc5556e7f 100644 --- a/src/ipc/distance/signed/point_line.hpp +++ b/src/ipc/distance/signed/point_line.hpp @@ -1,32 +1,108 @@ #pragma once #include +#include namespace ipc { +namespace detail { + /// Compute the signed distance from a point to a directed line segment. + /// + /// The signed distance is computed as d = n · (p - e0), + /// where n = point_line_normal(p, e0, e1) is the unit normal associated + /// with the directed edge (e0 -> e1) chosen consistently for the point p. + /// Positive/negative sign indicates which side of the directed edge the + /// point lies on. + /// + /// @param p The query point (2D). + /// @param e0 The first endpoint of the directed edge (2D). + /// @param e1 The second endpoint of the directed edge (2D). + /// @return The signed scalar distance from p to the infinite line through e0 and e1. + /// @note The edge must be non-degenerate (e0 != e1). + template + inline T point_line_signed_distance( + Eigen::ConstRef> p, + Eigen::ConstRef> e0, + Eigen::ConstRef> e1) + { + return point_line_normal(p, e0, e1).dot(p - e0); + } + + /// Compute the gradient of the signed point-to-line distance with respect + /// to all input coordinates packed as [p_x, p_y, e0_x, e0_y, e1_x, e1_y]^T. + /// + /// The returned Vector6d contains partial derivatives of the scalar signed + /// distance d with respect to each coordinate in the above ordering: + /// grad = [∂d/∂p, ∂d/∂e0, ∂d/∂e1]^T. + /// + /// @param p The query point (2D). + /// @param e0 The first endpoint of the directed edge (2D). + /// @param e1 The second endpoint of the directed edge (2D). + /// @return A 6-vector containing the gradient of the signed distance. + /// @note The edge must be non-degenerate (e0 != e1). + template + inline Eigen::Vector point_line_signed_distance_gradient( + Eigen::ConstRef> p, + Eigen::ConstRef> e0, + Eigen::ConstRef> e1) + { + const Eigen::Vector2 n = point_line_normal(p, e0, e1); + const Eigen::Matrix jac_n = + point_line_normal_jacobian(p, e0, e1); + Eigen::Vector grad = jac_n.transpose() * (p - e0); + grad.template segment<2>(0) += n; + grad.template segment<2>(2) -= n; + return grad; + } + + /// Compute the Hessian (second derivatives) of the signed point-to-line + /// distance with respect to all input coordinates packed as [p_x, p_y, + /// e0_x, e0_y, e1_x, e1_y]^T. + /// + /// The returned Matrix6d is the symmetric 6x6 matrix of second partial + /// derivatives: + /// H_ij = ∂^2 d / (∂x_i ∂x_j), + /// with the same coordinate ordering as in the gradient. + /// + /// @param p The query point (2D). + /// @param e0 The first endpoint of the directed edge (2D). + /// @param e1 The second endpoint of the directed edge (2D). + /// @return A 6x6 Hessian matrix of the signed distance. + /// @note The edge must be non-degenerate (e0 != e1). + template + Eigen::Matrix point_line_signed_distance_hessian( + Eigen::ConstRef> p, + Eigen::ConstRef> e0, + Eigen::ConstRef> e1); +} // namespace detail + /// Compute the signed distance from a point to a directed line segment. /// /// The signed distance is computed as d = n · (p - e0), -/// where n = point_line_normal(p, e0, e1) is the unit normal associated with -/// the directed edge (e0 -> e1) chosen consistently for the point p. -/// Positive/negative sign indicates which side of the directed edge the point -/// lies on. +/// where n = point_line_normal(p, e0, e1) is the unit normal associated +/// with the directed edge (e0 -> e1) chosen consistently for the point p. +/// Positive/negative sign indicates which side of the directed edge the +/// point lies on. /// /// @param p The query point (2D). /// @param e0 The first endpoint of the directed edge (2D). /// @param e1 The second endpoint of the directed edge (2D). /// @return The signed scalar distance from p to the infinite line through e0 and e1. /// @note The edge must be non-degenerate (e0 != e1). -inline double point_line_signed_distance( - Eigen::ConstRef p, - Eigen::ConstRef e0, - Eigen::ConstRef e1) +template +inline auto point_line_signed_distance( + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& e0, + const Eigen::MatrixBase& e1) { - return point_line_normal(p, e0, e1).dot(p - e0); + using T = typename DerivedP::Scalar; + // NOTE: explicit : the detail overload cannot deduce T from an + // arbitrary Eigen expression. + return detail::point_line_signed_distance(p, e0, e1); } -/// Compute the gradient of the signed point-to-line distance with respect to -/// all input coordinates packed as [p_x, p_y, e0_x, e0_y, e1_x, e1_y]^T. +/// Compute the gradient of the signed point-to-line distance with respect +/// to all input coordinates packed as [p_x, p_y, e0_x, e0_y, e1_x, e1_y]^T. /// /// The returned Vector6d contains partial derivatives of the scalar signed /// distance d with respect to each coordinate in the above ordering: @@ -37,14 +113,19 @@ inline double point_line_signed_distance( /// @param e1 The second endpoint of the directed edge (2D). /// @return A 6-vector containing the gradient of the signed distance. /// @note The edge must be non-degenerate (e0 != e1). -Vector6d point_line_signed_distance_gradient( - Eigen::ConstRef p, - Eigen::ConstRef e0, - Eigen::ConstRef e1); +template +inline auto point_line_signed_distance_gradient( + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& e0, + const Eigen::MatrixBase& e1) +{ + using T = typename DerivedP::Scalar; + return detail::point_line_signed_distance_gradient(p, e0, e1); +} /// Compute the Hessian (second derivatives) of the signed point-to-line -/// distance with respect to all input coordinates packed as [p_x, p_y, e0_x, -/// e0_y, e1_x, e1_y]^T. +/// distance with respect to all input coordinates packed as [p_x, p_y, +/// e0_x, e0_y, e1_x, e1_y]^T. /// /// The returned Matrix6d is the symmetric 6x6 matrix of second partial /// derivatives: @@ -56,9 +137,14 @@ Vector6d point_line_signed_distance_gradient( /// @param e1 The second endpoint of the directed edge (2D). /// @return A 6x6 Hessian matrix of the signed distance. /// @note The edge must be non-degenerate (e0 != e1). -Matrix6d point_line_signed_distance_hessian( - Eigen::ConstRef p, - Eigen::ConstRef e0, - Eigen::ConstRef e1); +template +inline auto point_line_signed_distance_hessian( + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& e0, + const Eigen::MatrixBase& e1) +{ + using T = typename DerivedP::Scalar; + return detail::point_line_signed_distance_hessian(p, e0, e1); +} -} // namespace ipc \ No newline at end of file +} // namespace ipc diff --git a/src/ipc/distance/signed/point_plane.cpp b/src/ipc/distance/signed/point_plane.cpp index 6692143b3..268b5355a 100644 --- a/src/ipc/distance/signed/point_plane.cpp +++ b/src/ipc/distance/signed/point_plane.cpp @@ -1,57 +1,37 @@ #include "point_plane.hpp" -namespace ipc { - -Vector12d point_plane_signed_distance_gradient( - Eigen::ConstRef p, - Eigen::ConstRef t0, - Eigen::ConstRef t1, - Eigen::ConstRef t2) -{ - const Eigen::Vector3d n = triangle_normal(t0, t1, t2); - const Eigen::Matrix jac_n = - triangle_normal_jacobian(t0, t1, t2); - - Vector12d grad; - grad.segment<3>(0) = n; - grad.segment<3>(3) = jac_n.leftCols<3>().transpose() * (p - t0) - n; - grad.segment<3>(6) = jac_n.middleCols<3>(3).transpose() * (p - t0); - grad.segment<3>(9) = jac_n.rightCols<3>().transpose() * (p - t0); - - return grad; -} - -Matrix12d point_plane_signed_distance_hessian( - Eigen::ConstRef p, - Eigen::ConstRef t0, - Eigen::ConstRef t1, - Eigen::ConstRef t2) +namespace ipc::detail { + +template +Eigen::Matrix point_plane_signed_distance_hessian( + Eigen::ConstRef> p, + Eigen::ConstRef> t0, + Eigen::ConstRef> t1, + Eigen::ConstRef> t2) { // Precompute normal's Jacobian and Hessian - const Eigen::Matrix jac_n = - triangle_normal_jacobian(t0, t1, t2); - const Eigen::Matrix hess_n = - triangle_normal_hessian(t0, t1, t2); + const Eigen::Matrix jac_n = triangle_normal_jacobian(t0, t1, t2); + const Eigen::Matrix hess_n = triangle_normal_hessian(t0, t1, t2); // Vector from t0 to p - const Eigen::Vector3d v = p - t0; + const Eigen::Vector3 v = p - t0; - Matrix12d hess; + Eigen::Matrix hess; // --------------------------------------------------------- // 0. Fill Point-Point Block (p, p) // --------------------------------------------------------- // The second derivative w.r.t p is zero since the normal is constant w.r.t // p. - hess.block<3, 3>(0, 0).setZero(); + hess.template block<3, 3>(0, 0).setZero(); // --------------------------------------------------------- // 1. Fill Mixed Derivatives (p, t) and (t, p) // --------------------------------------------------------- // The gradient w.r.t p is n. // The mixed derivative is the Jacobian of n w.r.t t. - hess.block<3, 9>(0, 3) = jac_n; - hess.block<9, 3>(3, 0) = jac_n.transpose(); + hess.template block<3, 9>(0, 3) = jac_n; + hess.template block<9, 3>(3, 0) = jac_n.transpose(); // --------------------------------------------------------- // 2. Fill Triangle-Triangle Block (t, t) @@ -60,33 +40,38 @@ Matrix12d point_plane_signed_distance_hessian( // A. Contraction of the normal Hessian tensor with vector v // hess_n is 3x81. v is 3x1. Result is 1x81, which maps to 9x9. - hess.block<9, 9>(3, 3) = + hess.template block<9, 9>(3, 3) = (hess_n.reshaped(3, 81).transpose() * v).reshaped(9, 9); // B. Subtract first derivative terms (Product Rule corrections) // Extract 3x3 Jacobian blocks for t0, t1, t2 - const auto J0 = jac_n.leftCols<3>(); - const auto J1 = jac_n.middleCols<3>(3); - const auto J2 = jac_n.rightCols<3>(); + const auto J0 = jac_n.template leftCols<3>(); + const auto J1 = jac_n.template middleCols<3>(3); + const auto J2 = jac_n.template rightCols<3>(); // Apply corrections for terms involving t0 (index 0) // Block (t0, t0): i=0, j=0. Subtract J0 + J0^T - hess.block<3, 3>(3, 3) -= (J0 + J0.transpose()); + hess.template block<3, 3>(3, 3) -= (J0 + J0.transpose()); // Block (t0, t1): i=0, j=1. Subtract J1 - hess.block<3, 3>(3, 6) -= J1; + hess.template block<3, 3>(3, 6) -= J1; // Block (t0, t2): i=0, j=2. Subtract J2 - hess.block<3, 3>(3, 9) -= J2; + hess.template block<3, 3>(3, 9) -= J2; // Block (t1, t0): i=1, j=0. Subtract J1^T - hess.block<3, 3>(6, 3) -= J1.transpose(); + hess.template block<3, 3>(6, 3) -= J1.transpose(); // Block (t2, t0): i=2, j=0. Subtract J2^T - hess.block<3, 3>(9, 3) -= J2.transpose(); + hess.template block<3, 3>(9, 3) -= J2.transpose(); return hess; } -} // namespace ipc \ No newline at end of file +// clang-format off +template Matrix12f point_plane_signed_distance_hessian(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef); +template Matrix12d point_plane_signed_distance_hessian(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef); +// clang-format on + +} // namespace ipc::detail diff --git a/src/ipc/distance/signed/point_plane.hpp b/src/ipc/distance/signed/point_plane.hpp index 04fd90f34..645646f18 100644 --- a/src/ipc/distance/signed/point_plane.hpp +++ b/src/ipc/distance/signed/point_plane.hpp @@ -1,9 +1,82 @@ #pragma once #include +#include namespace ipc { +namespace detail { + /// Compute the signed distance from a point to the plane of a triangle. + /// + /// The signed distance is computed as: + /// d = triangle_normal(t0, t1, t2) · (p - t0) + /// The sign corresponds to the orientation of the triangle normal. + /// + /// @param p The query point (3D). + /// @param t0 First vertex of the triangle (3D), used as a point on the plane. + /// @param t1 Second vertex of the triangle (3D). + /// @param t2 Third vertex of the triangle (3D). + /// @return The signed distance from p to the plane of the triangle. + template + inline T point_plane_signed_distance( + Eigen::ConstRef> p, + Eigen::ConstRef> t0, + Eigen::ConstRef> t1, + Eigen::ConstRef> t2) + { + return triangle_normal(t0, t1, t2).dot(p - t0); + } + + /// Compute the gradient of the signed point-to-plane distance. + /// + /// The returned vector contains derivatives in the order: + /// [ ∂d/∂p (3), ∂d/∂t0 (3), ∂d/∂t1 (3), ∂d/∂t2 (3) ] + /// + /// @param p The query point (3D). + /// @param t0 First vertex of the triangle (3D). + /// @param t1 Second vertex of the triangle (3D). + /// @param t2 Third vertex of the triangle (3D). + /// @return A 12-vector containing the gradient of the signed distance. + template + inline Eigen::Vector point_plane_signed_distance_gradient( + Eigen::ConstRef> p, + Eigen::ConstRef> t0, + Eigen::ConstRef> t1, + Eigen::ConstRef> t2) + { + const Eigen::Vector3 n = triangle_normal(t0, t1, t2); + const Eigen::Matrix jac_n = + triangle_normal_jacobian(t0, t1, t2); + Eigen::Vector grad; + grad.template segment<3>(0) = n; + grad.template segment<3>(3) = + jac_n.template leftCols<3>().transpose() * (p - t0) - n; + grad.template segment<3>(6) = + jac_n.template middleCols<3>(3).transpose() * (p - t0); + grad.template segment<3>(9) = + jac_n.template rightCols<3>().transpose() * (p - t0); + return grad; + } + + /// Compute the Hessian (matrix of second derivatives) of the signed + /// distance. + /// + /// The returned matrix is 12x12 with variables ordered as: + /// [ p (3), t0 (3), t1 (3), t2 (3) ]. + /// + /// @param p The query point (3D). + /// @param t0 First vertex of the triangle (3D). + /// @param t1 Second vertex of the triangle (3D). + /// @param t2 Third vertex of the triangle (3D). + /// @return A 12x12 Hessian matrix of the signed distance. + template + Eigen::Matrix point_plane_signed_distance_hessian( + Eigen::ConstRef> p, + Eigen::ConstRef> t0, + Eigen::ConstRef> t1, + Eigen::ConstRef> t2); +} // namespace detail + /// Compute the signed distance from a point to the plane of a triangle. /// /// The signed distance is computed as: @@ -15,45 +88,72 @@ namespace ipc { /// @param t1 Second vertex of the triangle (3D). /// @param t2 Third vertex of the triangle (3D). /// @return The signed distance from p to the plane of the triangle. -inline double point_plane_signed_distance( - Eigen::ConstRef p, - Eigen::ConstRef t0, - Eigen::ConstRef t1, - Eigen::ConstRef t2) +template < + typename DerivedP, + typename DerivedT0, + typename DerivedT1, + typename DerivedT2> +inline auto point_plane_signed_distance( + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& t0, + const Eigen::MatrixBase& t1, + const Eigen::MatrixBase& t2) { - return triangle_normal(t0, t1, t2).dot(p - t0); + using T = typename DerivedP::Scalar; + // NOTE: explicit : the detail overload cannot deduce T from an + // arbitrary Eigen expression. + return detail::point_plane_signed_distance(p, t0, t1, t2); } /// Compute the gradient of the signed point-to-plane distance. /// -/// The returned Vector12d contains derivatives in the order: +/// The returned vector contains derivatives in the order: /// [ ∂d/∂p (3), ∂d/∂t0 (3), ∂d/∂t1 (3), ∂d/∂t2 (3) ] /// /// @param p The query point (3D). /// @param t0 First vertex of the triangle (3D). /// @param t1 Second vertex of the triangle (3D). /// @param t2 Third vertex of the triangle (3D). -/// @return A Vector12d containing the gradient of the signed distance. -Vector12d point_plane_signed_distance_gradient( - Eigen::ConstRef p, - Eigen::ConstRef t0, - Eigen::ConstRef t1, - Eigen::ConstRef t2); - -/// Compute the Hessian (matrix of second derivatives) of the signed distance. +/// @return A 12-vector containing the gradient of the signed distance. +template < + typename DerivedP, + typename DerivedT0, + typename DerivedT1, + typename DerivedT2> +inline auto point_plane_signed_distance_gradient( + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& t0, + const Eigen::MatrixBase& t1, + const Eigen::MatrixBase& t2) +{ + using T = typename DerivedP::Scalar; + return detail::point_plane_signed_distance_gradient(p, t0, t1, t2); +} + +/// Compute the Hessian (matrix of second derivatives) of the signed +/// distance. /// -/// The returned Matrix12d is the 12x12 Hessian with variables ordered as: +/// The returned matrix is 12x12 with variables ordered as: /// [ p (3), t0 (3), t1 (3), t2 (3) ]. /// /// @param p The query point (3D). /// @param t0 First vertex of the triangle (3D). /// @param t1 Second vertex of the triangle (3D). /// @param t2 Third vertex of the triangle (3D). -/// @return A Matrix12d representing the Hessian of the signed distance. -Matrix12d point_plane_signed_distance_hessian( - Eigen::ConstRef p, - Eigen::ConstRef t0, - Eigen::ConstRef t1, - Eigen::ConstRef t2); - -} // namespace ipc \ No newline at end of file +/// @return A 12x12 Hessian matrix of the signed distance. +template < + typename DerivedP, + typename DerivedT0, + typename DerivedT1, + typename DerivedT2> +inline auto point_plane_signed_distance_hessian( + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& t0, + const Eigen::MatrixBase& t1, + const Eigen::MatrixBase& t2) +{ + using T = typename DerivedP::Scalar; + return detail::point_plane_signed_distance_hessian(p, t0, t1, t2); +} + +} // namespace ipc diff --git a/src/ipc/geometry/area.cpp b/src/ipc/geometry/area.cpp index cbb6e223e..99a54fcbe 100644 --- a/src/ipc/geometry/area.cpp +++ b/src/ipc/geometry/area.cpp @@ -2,75 +2,54 @@ #include -namespace ipc { - -VectorMax6d edge_length_gradient( - Eigen::ConstRef e0, Eigen::ConstRef e1) -{ - assert(e0.size() == 2 || e0.size() == 3); - assert(e1.size() == 2 || e1.size() == 3); - assert((e1 - e0).norm() != 0); - - // ∇ ‖e₁ - e₀‖ - VectorMax6d grad(e0.size() + e1.size()); - grad.head(e0.size()) = (e0 - e1) / (e1 - e0).norm(); - grad.tail(e1.size()) = -grad.head(e0.size()); - return grad; -} - -Vector9d triangle_area_gradient( - Eigen::ConstRef t0, - Eigen::ConstRef t1, - Eigen::ConstRef t2) +namespace ipc::autogen { + +// dA is (9×1) flattened in column-major order +template +void triangle_area_gradient( + T t0_x, + T t0_y, + T t0_z, + T t1_x, + T t1_y, + T t1_z, + T t2_x, + T t2_y, + T t2_z, + T dA[9]) { - Vector9d grad; - autogen::triangle_area_gradient( - t0[0], t0[1], t0[2], t1[0], t1[1], t1[2], t2[0], t2[1], t2[2], - grad.data()); - return grad; + const T t0 = -t2_y; + const T t1 = t0 + t1_y; + const T t2 = t0_x - t1_x; + const T t3 = t0 + t0_y; + const T t4 = -t2_x; + const T t5 = t0_x + t4; + const T t6 = t0_y - t1_y; + const T t7 = t2 * t3 - t5 * t6; + const T t8 = -t2_z; + const T t9 = t1_z + t8; + const T t10 = t0_z + t8; + const T t11 = t0_z - t1_z; + const T t12 = t10 * t2 - t11 * t5; + const T t13 = t10 * t6 - t11 * t3; + const T t14 = T(0.5) / std::sqrt(t12 * t12 + t13 * t13 + t7 * t7); + const T t15 = t1_x + t4; + dA[0] = t14 * (t1 * t7 + t12 * t9); + dA[1] = -t14 * (-t13 * t9 + t15 * t7); + dA[2] = -t14 * (t1 * t13 + t12 * t15); + dA[3] = -t14 * (t10 * t12 + t3 * t7); + dA[4] = t14 * (-t10 * t13 + t5 * t7); + dA[5] = t14 * (t12 * t5 + t13 * t3); + dA[6] = t14 * (t11 * t12 + t6 * t7); + dA[7] = -t14 * (-t11 * t13 + t2 * t7); + dA[8] = -t14 * (t12 * t2 + t13 * t6); } -namespace autogen { +#define IPC_INSTANTIATE_AREA_AUTOGEN(T) \ + template void triangle_area_gradient(T, T, T, T, T, T, T, T, T, T[9]) - // dA is (9×1) flattened in column-major order - void triangle_area_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 dA[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 = t10 * t6 - t11 * t3; - const auto t14 = 0.5 / std::sqrt(t12 * t12 + t13 * t13 + t7 * t7); - const auto t15 = t1_x + t4; - dA[0] = t14 * (t1 * t7 + t12 * t9); - dA[1] = -t14 * (-t13 * t9 + t15 * t7); - dA[2] = -t14 * (t1 * t13 + t12 * t15); - dA[3] = -t14 * (t10 * t12 + t3 * t7); - dA[4] = t14 * (-t10 * t13 + t5 * t7); - dA[5] = t14 * (t12 * t5 + t13 * t3); - dA[6] = t14 * (t11 * t12 + t6 * t7); - dA[7] = -t14 * (-t11 * t13 + t2 * t7); - dA[8] = -t14 * (t12 * t2 + t13 * t6); - } +IPC_INSTANTIATE_AREA_AUTOGEN(float); +IPC_INSTANTIATE_AREA_AUTOGEN(double); +#undef IPC_INSTANTIATE_AREA_AUTOGEN -} // namespace autogen -} // namespace ipc +} // namespace ipc::autogen diff --git a/src/ipc/geometry/area.hpp b/src/ipc/geometry/area.hpp index 6700a7e48..24fc22d35 100644 --- a/src/ipc/geometry/area.hpp +++ b/src/ipc/geometry/area.hpp @@ -2,63 +2,178 @@ #include +#include + +#include + namespace ipc { +// Symbolically generated derivatives +namespace autogen { + + // clang-format off + /// dA is (9×1) flattened in column-major order + template + void triangle_area_gradient( + T t0_x, T t0_y, T t0_z, T t1_x, T t1_y, T t1_z, T t2_x, T t2_y, T t2_z, T dA[9]); + // clang-format on + +} // namespace autogen + +namespace detail { + /// @brief Compute the length of an edge. + /// @tparam T The scalar type. + /// @tparam dim The dimension (2 or 3). + /// @param e0 The first vertex of the edge. + /// @param e1 The second vertex of the edge. + /// @return The length of the edge. + template + inline T edge_length( + Eigen::ConstRef> e0, + Eigen::ConstRef> e1) + { + static_assert(dim == 2 || dim == 3, "edges are only 2D or 3D"); + return (e1 - e0).norm(); + } + + /// @brief Compute the gradient of an edge's length. + /// @tparam T The scalar type. + /// @tparam dim The dimension (2 or 3). + /// @param e0 The first vertex of the edge. + /// @param e1 The second vertex of the edge. + /// @return The gradient of the edge's length wrt e0, and e1. + template + inline Eigen::Vector edge_length_gradient( + Eigen::ConstRef> e0, + Eigen::ConstRef> e1) + { + static_assert(dim == 2 || dim == 3, "edges are only 2D or 3D"); + assert((e1 - e0).norm() != 0); + + // ∇ ‖e₁ - e₀‖ + Eigen::Vector grad; + grad.template head() = (e0 - e1) / (e1 - e0).norm(); + grad.template tail() = -grad.template head(); + return grad; + } + + /// @brief Compute the area of a triangle. + /// @tparam T The scalar type. + /// @param t0 The first vertex of the triangle. + /// @param t1 The second vertex of the triangle. + /// @param t2 The third vertex of the triangle. + /// @return The area of the triangle. + template + inline T triangle_area( + Eigen::ConstRef> t0, + Eigen::ConstRef> t1, + Eigen::ConstRef> t2) + { + return T(0.5) * (t1 - t0).cross(t2 - t0).norm(); + } + + /// @brief Compute the gradient of the area of a triangle. + /// @tparam T The scalar type. + /// @param t0 The first vertex of the triangle. + /// @param t1 The second vertex of the triangle. + /// @param t2 The third vertex of the triangle. + /// @return The gradient of the triangle's area t0, t1, and t2. + template + inline Eigen::Vector triangle_area_gradient( + Eigen::ConstRef> t0, + Eigen::ConstRef> t1, + Eigen::ConstRef> t2) + { + Eigen::Vector grad; + autogen::triangle_area_gradient( + t0[0], t0[1], t0[2], t1[0], t1[1], t1[2], t2[0], t2[1], t2[2], + grad.data()); + return grad; + } +} // namespace detail + /// @brief Compute the length of an edge. /// @param e0 The first vertex of the edge. /// @param e1 The second vertex of the edge. /// @return The length of the edge. -inline double -edge_length(Eigen::ConstRef e0, Eigen::ConstRef e1) +template +inline auto edge_length( + const Eigen::MatrixBase& e0, + const Eigen::MatrixBase& e1) { - return (e1 - e0).norm(); + using T = typename DerivedE0::Scalar; + + if constexpr (dim_v == 2) { + return detail::edge_length(e0, e1); + } else if constexpr (dim_v == 3) { + return detail::edge_length(e0, e1); + } else if (e0.size() == 2) { + return detail::edge_length(e0, e1); + } else { + assert(e0.size() == 3); + return detail::edge_length(e0, e1); + } } /// @brief Compute the gradient of an edge's length. /// @param e0 The first vertex of the edge. /// @param e1 The second vertex of the edge. /// @return The gradient of the edge's length wrt e0, and e1. -VectorMax6d edge_length_gradient( - Eigen::ConstRef e0, Eigen::ConstRef e1); +template +inline auto edge_length_gradient( + const Eigen::MatrixBase& e0, + const Eigen::MatrixBase& e1) +{ + using T = typename DerivedE0::Scalar; + + if constexpr (dim_v == 2) { + return detail::edge_length_gradient(e0, e1); + } else if constexpr (dim_v == 3) { + return detail::edge_length_gradient(e0, e1); + } else if (e0.size() == 2) { + return VectorMax6(detail::edge_length_gradient(e0, e1)); + } else { + assert(e0.size() == 3); + return VectorMax6(detail::edge_length_gradient(e0, e1)); + } +} /// @brief Compute the area of a triangle. +/// +/// Accepts any 3D Eigen expression; the scalar type is deduced. +/// /// @param t0 The first vertex of the triangle. /// @param t1 The second vertex of the triangle. /// @param t2 The third vertex of the triangle. /// @return The area of the triangle. -inline double triangle_area( - Eigen::ConstRef t0, - Eigen::ConstRef t1, - Eigen::ConstRef t2) +template +inline auto triangle_area( + const Eigen::MatrixBase& t0, + const Eigen::MatrixBase& t1, + const Eigen::MatrixBase& t2) { - return 0.5 * (t1 - t0).cross(t2 - t0).norm(); + // NOTE: explicit : the detail overload cannot deduce T from an + // arbitrary Eigen expression. + using T = typename DerivedT0::Scalar; + return detail::triangle_area(t0, t1, t2); } /// @brief Compute the gradient of the area of a triangle. +/// +/// Accepts any 3D Eigen expression; the scalar type is deduced. +/// /// @param t0 The first vertex of the triangle. /// @param t1 The second vertex of the triangle. /// @param t2 The third vertex of the triangle. /// @return The gradient of the triangle's area t0, t1, and t2. -Vector9d triangle_area_gradient( - Eigen::ConstRef t0, - Eigen::ConstRef t1, - Eigen::ConstRef t2); - -namespace autogen { - - // dA is (9×1) flattened in column-major order - void triangle_area_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 dA[9]); - -} // namespace autogen +template +inline auto triangle_area_gradient( + const Eigen::MatrixBase& t0, + const Eigen::MatrixBase& t1, + const Eigen::MatrixBase& t2) +{ + using T = typename DerivedT0::Scalar; + return detail::triangle_area_gradient(t0, t1, t2); +} } // namespace ipc diff --git a/src/ipc/geometry/normal.cpp b/src/ipc/geometry/normal.cpp index b1abcb0ae..3c669194c 100644 --- a/src/ipc/geometry/normal.cpp +++ b/src/ipc/geometry/normal.cpp @@ -1,119 +1,104 @@ #include "ipc/geometry/normal.hpp" -namespace ipc { +#include -std::tuple -normalization_and_jacobian(Eigen::ConstRef x) -{ - const double norm = x.norm(); - const VectorMax3d xhat = x / norm; - const auto I = MatrixMax3d::Identity(x.size(), x.size()); - const MatrixMax3d J = (I - (xhat * xhat.transpose())) / norm; - return std::make_tuple(xhat, J); -} +#include -std::tuple> -normalization_and_jacobian_and_hessian(Eigen::ConstRef x) -{ - // const auto [xhat, J] = normalization_and_jacobian(x); - const double norm = x.norm(); - const VectorMax3d xhat = x / norm; - const auto I = MatrixMax3d::Identity(x.size(), x.size()); - const MatrixMax3d J = (I - (xhat * xhat.transpose())) / norm; - - std::array H; - for (int i = 0; i < x.size(); i++) { - H[i] = (xhat(i) * J + xhat * J.row(i) + J.col(i) * xhat.transpose()) - / (-norm); - } - - return std::make_tuple(xhat, J, H); -} +namespace ipc::detail { // --- point-line normal functions ------------------------------------------- -VectorMax3d point_line_unnormalized_normal( - Eigen::ConstRef p, - Eigen::ConstRef e0, - Eigen::ConstRef e1) +template +VectorMax3 point_line_unnormalized_normal( + Eigen::ConstRef> p, + Eigen::ConstRef> e0, + Eigen::ConstRef> e1) { assert(p.size() == e0.size() && p.size() == e1.size()); assert(p.size() == 2 || p.size() == 3); if (p.size() == 2) { // In 2D, the normal is simply the perpendicular vector to the line - const Eigen::Vector2d e = e1.head<2>() - e0.head<2>(); - return Eigen::Vector2d(-e.y(), e.x()); + const Eigen::Vector2 e = + e1.template head<2>() - e0.template head<2>(); + return Eigen::Vector2(-e.y(), e.x()); } else { // Use triple product expansion of the cross product -e × (e × d) // (https://en.wikipedia.org/wiki/Cross_product#Triple_product_expansion) // NOTE: This would work in 2D as well, but we handle that case above. - const Eigen::Vector3d e = e1 - e0; - const Eigen::Vector3d d = p - e0; + const Eigen::Vector3 e = e1 - e0; + const Eigen::Vector3 d = p - e0; return d * e.dot(e) - e * e.dot(d); } } -MatrixMax point_line_unnormalized_normal_jacobian( - Eigen::ConstRef p, - Eigen::ConstRef e0, - Eigen::ConstRef e1) +template +MatrixMax point_line_unnormalized_normal_jacobian( + Eigen::ConstRef> p, + Eigen::ConstRef> e0, + Eigen::ConstRef> e1) { assert(p.size() == e0.size() && p.size() == e1.size()); assert(p.size() == 2 || p.size() == 3); - MatrixMax dn(p.size(), 3 * p.size()); + MatrixMax dn(p.size(), 3 * p.size()); if (p.size() == 2) { // In 2D, the normal is simply the perpendicular vector to the line - dn.leftCols<2>().setZero(); - dn.middleCols<2>(2) << 0, 1, -1, 0; - dn.rightCols<2>() << 0, -1, 1, 0; + dn.template leftCols<2>().setZero(); + dn.template middleCols<2>(2) << 0, 1, -1, 0; + dn.template rightCols<2>() << 0, -1, 1, 0; return dn; } else { - const Eigen::Vector3d e = e1 - e0; - const Eigen::Vector3d d = p - e0; + const Eigen::Vector3 e = e1 - e0; + const Eigen::Vector3 d = p - e0; - const auto I = Eigen::Matrix3d::Identity(); + const auto I = Eigen::Matrix3::Identity(); // ∂n/∂v - dn.leftCols<3>() = e.dot(e) * I - e * e.transpose(); + dn.template leftCols<3>() = e.dot(e) * I - e * e.transpose(); // ∂n/∂e1 - dn.rightCols<3>() = + dn.template rightCols<3>() = -e.dot(d) * I - e * d.transpose() + (2 * d) * e.transpose(); // ∂n/∂e0 - dn.middleCols<3>(3) = -dn.leftCols<3>() - dn.rightCols<3>(); + dn.template middleCols<3>(3) = + -dn.template leftCols<3>() - dn.template rightCols<3>(); } return dn; } -MatrixMax point_line_unnormalized_normal_hessian( - Eigen::ConstRef p, - Eigen::ConstRef e0, - Eigen::ConstRef e1) +template +MatrixMax point_line_unnormalized_normal_hessian( + Eigen::ConstRef> p, + Eigen::ConstRef> e0, + Eigen::ConstRef> e1) { + using Vector3T = Eigen::Vector3; + using Matrix3T = Eigen::Matrix3; + using Matrix9T = Eigen::Matrix; + assert(p.size() == e0.size() && p.size() == e1.size()); assert(p.size() == 2 || p.size() == 3); if (p.size() == 2) { // In 2D, the normal is simply the perpendicular vector to the line - return MatrixMax::Zero(12, 6); + return MatrixMax::Zero(12, 6); } else { - const Eigen::Vector3d e = e1 - e0; - const Eigen::Vector3d d = p - e0; - const auto I = Eigen::Matrix3d::Identity(); + const Vector3T e = e1 - e0; + const Vector3T d = p - e0; + const auto I = Matrix3T::Identity(); // The full Hessian is 27x9 (3 components of n, each with 9 variables) - MatrixMax hess = MatrixMax::Zero(27, 9); + MatrixMax hess = MatrixMax::Zero(27, 9); // Compute the Hessian for each component k of the normal vector n for (int k = 0; k < 3; ++k) { // Standard basis vector for component k - Eigen::Vector3d ek = Eigen::Vector3d::Zero(); + Vector3T ek = Vector3T::Zero(); ek(k) = 1.0; // The full 9x9 Hessian for component n[k] // Order of variables: p (0-2), e0 (3-5), e1 (6-8) - Matrix9d Hk = Matrix9d::Zero(); + Matrix9T Hk = Matrix9T::Zero(); // ------------------------------------------------------------- // Block (p, e1): Derivative of ∂n/∂p w.r.t e1 @@ -121,7 +106,7 @@ MatrixMax point_line_unnormalized_normal_hessian( // ∂n/∂p = ||e||^2 * I - e * e^T // Differentiating row k w.r.t e: // H_pe1 = 2 * ek * e^T - e * ek^T - e[k] * I - Eigen::Matrix3d H_pe1 = + Matrix3T H_pe1 = 2.0 * ek * e.transpose() - e * ek.transpose() - e(k) * I; // ------------------------------------------------------------- @@ -130,7 +115,7 @@ MatrixMax point_line_unnormalized_normal_hessian( // ∂n/∂e1 = -(e.d)I - e*d^T + 2d*e^T // Differentiating row k w.r.t e (keeping d constant): // H_e1e1 = -ek * d^T - d * ek^T + 2 * d[k] * I - Eigen::Matrix3d H_e1e1 = + Matrix3T H_e1e1 = -ek * d.transpose() - d * ek.transpose() + 2.0 * d(k) * I; // ------------------------------------------------------------- @@ -143,11 +128,11 @@ MatrixMax point_line_unnormalized_normal_hessian( // Part 2: Contribution from d // Differentiating row k w.r.t d (keeping e constant): // M = -ek * e^T - e[k] * I + 2 * e * ek^T - Eigen::Matrix3d term_d = + Matrix3T term_d = -ek * e.transpose() - e(k) * I + 2.0 * e * ek.transpose(); // Combine: -1 * H_e1e1 - 1 * term_d - Eigen::Matrix3d H_e1e0 = -H_e1e1 - term_d; + Matrix3T H_e1e0 = -H_e1e1 - term_d; // ------------------------------------------------------------- // Assemble 9x9 Matrix (Symmetry & Translation Invariance) @@ -156,27 +141,27 @@ MatrixMax point_line_unnormalized_normal_hessian( // (p, p) is Zero because ∂n/∂p is linear in p // (p, e1) and (e1, p) - Hk.block<3, 3>(0, 6) = H_pe1; - Hk.block<3, 3>(6, 0) = H_pe1.transpose(); + Hk.template block<3, 3>(0, 6) = H_pe1; + Hk.template block<3, 3>(6, 0) = H_pe1.transpose(); // (p, e0) = - (p, e1) // Because ∂e/∂e0 = -∂e/∂e1, and ∂n/∂p depends only on e - Hk.block<3, 3>(0, 3) = -H_pe1; - Hk.block<3, 3>(3, 0) = -H_pe1.transpose(); + Hk.template block<3, 3>(0, 3) = -H_pe1; + Hk.template block<3, 3>(3, 0) = -H_pe1.transpose(); // (e1, e1) - Hk.block<3, 3>(6, 6) = H_e1e1; + Hk.template block<3, 3>(6, 6) = H_e1e1; // (e1, e0) and (e0, e1) - Hk.block<3, 3>(6, 3) = H_e1e0; - Hk.block<3, 3>(3, 6) = H_e1e0.transpose(); + Hk.template block<3, 3>(6, 3) = H_e1e0; + Hk.template block<3, 3>(3, 6) = H_e1e0.transpose(); // (e0, e0) // Translation invariance: Sum of rows (and cols) must be zero. // H_e0e0 = -H_pe0 - H_e1e0 // = -(-H_pe1) - H_e1e0 // = H_pe1 - H_e1e0 - Hk.block<3, 3>(3, 3) = H_pe1 - H_e1e0; + Hk.template block<3, 3>(3, 3) = H_pe1 - H_e1e0; // Assign Hₖ to the strided rows of the full Hessian for (int i = 0; i < 9; ++i) { @@ -188,18 +173,19 @@ MatrixMax point_line_unnormalized_normal_hessian( } } -MatrixMax point_line_normal_hessian( - Eigen::ConstRef p, - Eigen::ConstRef e0, - Eigen::ConstRef e1) +template +MatrixMax point_line_normal_hessian( + Eigen::ConstRef> p, + Eigen::ConstRef> e0, + Eigen::ConstRef> e1) { assert(p.size() == e0.size() && p.size() == e1.size()); assert(p.size() == 2 || p.size() == 3); - const VectorMax3d z = point_line_unnormalized_normal(p, e0, e1); - const double z_norm2 = z.squaredNorm(); - const double z_norm = std::sqrt(z_norm2); - const double z_norm3 = z_norm2 * z_norm; + const VectorMax3 z = point_line_unnormalized_normal(p, e0, e1); + const T z_norm2 = z.squaredNorm(); + const T z_norm = std::sqrt(z_norm2); + const T z_norm3 = z_norm2 * z_norm; const int DIM = z.size(); // dimension (2 or 3) const int DOF = 3 * DIM; // total dof (6 or 9) @@ -207,15 +193,15 @@ MatrixMax point_line_normal_hessian( const auto dz_dx = point_line_unnormalized_normal_jacobian(p, e0, e1); const auto d2z_dx2 = point_line_unnormalized_normal_hessian(p, e0, e1); - MatrixMax d2n_dx2(DOF * DIM, DOF); + MatrixMax d2n_dx2(DOF * DIM, DOF); for (int k = 0; k < DOF * DOF; ++k) { const int i = k / DOF, j = k % DOF; - const double alpha = z.dot(dz_dx.col(i)) / z_norm3; + const T alpha = z.dot(dz_dx.col(i)) / z_norm3; - const VectorMax3d d2z_dxidxj = d2z_dx2.block(DIM * i, j, DIM, 1); + const VectorMax3 d2z_dxidxj = d2z_dx2.block(DIM * i, j, DIM, 1); - const double dalpha_dxj = + const T dalpha_dxj = (dz_dx.col(j).dot(dz_dx.col(i)) + z.dot(d2z_dxidxj) - 3 * z.dot(dz_dx.col(i)) * dz_dx.col(j).dot(z) / z_norm2) / z_norm3; @@ -231,70 +217,80 @@ MatrixMax point_line_normal_hessian( // --- triangle normal functions ---------------------------------------------- namespace { + template void set_cross_product_matrix_jacobian( - Eigen::Ref> Jx, double chain_rule = 1.0) + Eigen::Ref> Jx, double chain_rule = 1.0) { Jx(2, 1) = Jx(3, 2) = Jx(7, 0) = -chain_rule; Jx(1, 2) = Jx(5, 0) = Jx(6, 1) = chain_rule; } } // namespace -Eigen::Matrix cross_product_matrix_jacobian() +template Eigen::Matrix cross_product_matrix_jacobian() { - Eigen::Matrix J = Eigen::Matrix::Zero(); - set_cross_product_matrix_jacobian(J); + Eigen::Matrix J = Eigen::Matrix::Zero(); + J(2, 1) = J(3, 2) = J(7, 0) = T(-1); + J(1, 2) = J(5, 0) = J(6, 1) = T(1); return J; } -Eigen::Matrix triangle_unnormalized_normal_hessian( - Eigen::ConstRef a, - Eigen::ConstRef b, - Eigen::ConstRef c) +template +Eigen::Matrix triangle_unnormalized_normal_hessian( + Eigen::ConstRef> a, + Eigen::ConstRef> b, + Eigen::ConstRef> c) { - Eigen::Matrix H = Eigen::Matrix::Zero(); + Eigen::Matrix H = Eigen::Matrix::Zero(); // ∂²n/∂a² = 0 - set_cross_product_matrix_jacobian(H.block<9, 3>(0, 3), -1.0); // ∂²n/∂a∂b - set_cross_product_matrix_jacobian(H.block<9, 3>(0, 6), 1.0); // ∂²n/∂a∂c + set_cross_product_matrix_jacobian( + H.template block<9, 3>(0, 3), -1.0); // ∂²n/∂a∂b + set_cross_product_matrix_jacobian( + H.template block<9, 3>(0, 6), 1.0); // ∂²n/∂a∂c /**/ - set_cross_product_matrix_jacobian(H.block<9, 3>(9, 0), 1.0); // ∂²n/∂b∂a + set_cross_product_matrix_jacobian( + H.template block<9, 3>(9, 0), 1.0); // ∂²n/∂b∂a // ∂²n/∂b² = 0 - set_cross_product_matrix_jacobian(H.block<9, 3>(9, 6), -1.0); // ∂²n/∂b∂c + set_cross_product_matrix_jacobian( + H.template block<9, 3>(9, 6), -1.0); // ∂²n/∂b∂c /**/ - set_cross_product_matrix_jacobian(H.block<9, 3>(18, 0), -1.0); // ∂²n/∂c∂a - set_cross_product_matrix_jacobian(H.block<9, 3>(18, 3), 1.0); // ∂²n/∂c∂b + set_cross_product_matrix_jacobian( + H.template block<9, 3>(18, 0), -1.0); // ∂²n/∂c∂a + set_cross_product_matrix_jacobian( + H.template block<9, 3>(18, 3), 1.0); // ∂²n/∂c∂b // ∂²n/∂c² = 0 return H; } -Eigen::Matrix triangle_normal_hessian( - Eigen::ConstRef a, - Eigen::ConstRef b, - Eigen::ConstRef c) +template +Eigen::Matrix triangle_normal_hessian( + Eigen::ConstRef> a, + Eigen::ConstRef> b, + Eigen::ConstRef> c) { - const Eigen::Vector3d z = triangle_unnormalized_normal(a, b, c); - const double z_norm2 = z.squaredNorm(); - const double z_norm = std::sqrt(z_norm2); - const double z_norm3 = z_norm2 * z_norm; + const Eigen::Vector3 z = triangle_unnormalized_normal(a, b, c); + const T z_norm2 = z.squaredNorm(); + const T z_norm = std::sqrt(z_norm2); + const T z_norm3 = z_norm2 * z_norm; const auto dz_dx = triangle_unnormalized_normal_jacobian(a, b, c); const auto d2z_dx2 = triangle_unnormalized_normal_hessian(a, b, c); - Eigen::Matrix d2n_dx2; + Eigen::Matrix d2n_dx2; for (int k = 0; k < 81; ++k) { const int i = k / 9, j = k % 9; - const double alpha = z.dot(dz_dx.col(i)) / z_norm3; + const T alpha = z.dot(dz_dx.col(i)) / z_norm3; - const auto d2z_dxidxj = d2z_dx2.block<3, 1>(3 * i, j); + const auto d2z_dxidxj = d2z_dx2.template block<3, 1>(3 * i, j); - const double dalpha_dxj = + const T dalpha_dxj = (dz_dx.col(j).dot(dz_dx.col(i)) + z.dot(d2z_dxidxj) - 3 * z.dot(dz_dx.col(i)) * dz_dx.col(j).dot(z) / z_norm2) / z_norm3; - d2n_dx2.block<3, 1>(3 * i, j) = + d2n_dx2.template block<3, 1>(3 * i, j) = -dz_dx.col(i) * z.transpose() * dz_dx.col(j) / z_norm3 + d2z_dxidxj / z_norm - alpha * dz_dx.col(j) - dalpha_dxj * z; } @@ -304,67 +300,78 @@ Eigen::Matrix triangle_normal_hessian( // --- line-line normal functions --------------------------------------------- -Eigen::Matrix line_line_unnormalized_normal_hessian( - Eigen::ConstRef ea0, - Eigen::ConstRef ea1, - Eigen::ConstRef eb0, - Eigen::ConstRef eb1) +template +Eigen::Matrix line_line_unnormalized_normal_hessian( + Eigen::ConstRef> ea0, + Eigen::ConstRef> ea1, + Eigen::ConstRef> eb0, + Eigen::ConstRef> eb1) { - Eigen::Matrix H = Eigen::Matrix::Zero(); + Eigen::Matrix H = Eigen::Matrix::Zero(); // ∂²n/∂a² = 0 // ∂²n/∂a∂b = 0 - set_cross_product_matrix_jacobian(H.block<9, 3>(0, 6), -1.0); // ∂²n/∂a∂c - set_cross_product_matrix_jacobian(H.block<9, 3>(0, 9), 1.0); // ∂²n/∂a∂d + set_cross_product_matrix_jacobian( + H.template block<9, 3>(0, 6), -1.0); // ∂²n/∂a∂c + set_cross_product_matrix_jacobian( + H.template block<9, 3>(0, 9), 1.0); // ∂²n/∂a∂d /**/ // ∂²n/∂b∂a = 0 // ∂²n/∂b² = 0 - set_cross_product_matrix_jacobian(H.block<9, 3>(9, 6), 1.0); // ∂²n/∂b∂c - set_cross_product_matrix_jacobian(H.block<9, 3>(9, 9), -1.0); // ∂²n/∂b∂d + set_cross_product_matrix_jacobian( + H.template block<9, 3>(9, 6), 1.0); // ∂²n/∂b∂c + set_cross_product_matrix_jacobian( + H.template block<9, 3>(9, 9), -1.0); // ∂²n/∂b∂d /**/ - set_cross_product_matrix_jacobian(H.block<9, 3>(18, 0), 1.0); // ∂²n/∂c∂a - set_cross_product_matrix_jacobian(H.block<9, 3>(18, 3), -1.0); // ∂²n/∂c∂b + set_cross_product_matrix_jacobian( + H.template block<9, 3>(18, 0), 1.0); // ∂²n/∂c∂a + set_cross_product_matrix_jacobian( + H.template block<9, 3>(18, 3), -1.0); // ∂²n/∂c∂b // ∂²n/∂c² = 0 // ∂²n/∂d² = 0 /**/ - set_cross_product_matrix_jacobian(H.block<9, 3>(27, 0), -1.0); // ∂²n/∂d∂a - set_cross_product_matrix_jacobian(H.block<9, 3>(27, 3), 1.0); // ∂²n/∂d∂b + set_cross_product_matrix_jacobian( + H.template block<9, 3>(27, 0), -1.0); // ∂²n/∂d∂a + set_cross_product_matrix_jacobian( + H.template block<9, 3>(27, 3), 1.0); // ∂²n/∂d∂b // ∂²n/∂d∂c = 0 // ∂²n/∂d² = 0 return H; } -Eigen::Matrix line_line_normal_hessian( - Eigen::ConstRef ea0, - Eigen::ConstRef ea1, - Eigen::ConstRef eb0, - Eigen::ConstRef eb1) +template +Eigen::Matrix line_line_normal_hessian( + Eigen::ConstRef> ea0, + Eigen::ConstRef> ea1, + Eigen::ConstRef> eb0, + Eigen::ConstRef> eb1) { - const Eigen::Vector3d z = line_line_unnormalized_normal(ea0, ea1, eb0, eb1); - const double z_norm2 = z.squaredNorm(); - const double z_norm = std::sqrt(z_norm2); - const double z_norm3 = z_norm2 * z_norm; + const Eigen::Vector3 z = + line_line_unnormalized_normal(ea0, ea1, eb0, eb1); + const T z_norm2 = z.squaredNorm(); + const T z_norm = std::sqrt(z_norm2); + const T z_norm3 = z_norm2 * z_norm; - const Eigen::Matrix dz_dx = + const Eigen::Matrix dz_dx = line_line_unnormalized_normal_jacobian(ea0, ea1, eb0, eb1); - const Eigen::Matrix d2z_dx2 = + const Eigen::Matrix d2z_dx2 = line_line_unnormalized_normal_hessian(ea0, ea1, eb0, eb1); - Eigen::Matrix d2n_dx2; + Eigen::Matrix d2n_dx2; for (int k = 0; k < 144; ++k) { const int i = k / 12, j = k % 12; - const double alpha = z.dot(dz_dx.col(i)) / z_norm3; + const T alpha = z.dot(dz_dx.col(i)) / z_norm3; - const auto d2z_dxidxj = d2z_dx2.block<3, 1>(3 * i, j); + const auto d2z_dxidxj = d2z_dx2.template block<3, 1>(3 * i, j); - const double dalpha_dxj = + const T dalpha_dxj = (dz_dx.col(j).dot(dz_dx.col(i)) + z.dot(d2z_dxidxj) - 3 * z.dot(dz_dx.col(i)) * dz_dx.col(j).dot(z) / z_norm2) / z_norm3; - d2n_dx2.block<3, 1>(3 * i, j) = + d2n_dx2.template block<3, 1>(3 * i, j) = -dz_dx.col(i) * z.transpose() * dz_dx.col(j) / z_norm3 + d2z_dxidxj / z_norm - alpha * dz_dx.col(j) - dalpha_dxj * z; } @@ -372,4 +379,25 @@ Eigen::Matrix line_line_normal_hessian( return d2n_dx2; } -} // namespace ipc \ No newline at end of file +// clang-format off +template VectorMax3f point_line_unnormalized_normal(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef); +template VectorMax3d point_line_unnormalized_normal(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef); +template MatrixMax point_line_unnormalized_normal_jacobian(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef); +template MatrixMax point_line_unnormalized_normal_jacobian(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef); +template MatrixMax point_line_unnormalized_normal_hessian(Eigen::ConstRef,Eigen::ConstRef,Eigen::ConstRef); +template MatrixMax point_line_unnormalized_normal_hessian(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef); +template MatrixMax point_line_normal_hessian(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef); +template MatrixMax point_line_normal_hessian(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef); +template Eigen::Matrix cross_product_matrix_jacobian(); +template Eigen::Matrix cross_product_matrix_jacobian(); +template Eigen::Matrix triangle_unnormalized_normal_hessian(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef); +template Eigen::Matrix triangle_unnormalized_normal_hessian(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef); +template Eigen::Matrix triangle_normal_hessian(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef); +template Eigen::Matrix triangle_normal_hessian(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef); +template Eigen::Matrix line_line_unnormalized_normal_hessian(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef); +template Eigen::Matrix line_line_unnormalized_normal_hessian(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef); +template Eigen::Matrix line_line_normal_hessian(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef); +template Eigen::Matrix line_line_normal_hessian(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef); +// clang-format on + +} // namespace ipc::detail diff --git a/src/ipc/geometry/normal.hpp b/src/ipc/geometry/normal.hpp index 35db43c19..df4da5755 100644 --- a/src/ipc/geometry/normal.hpp +++ b/src/ipc/geometry/normal.hpp @@ -2,123 +2,578 @@ #include +#include #include namespace ipc { // ============================================================================= +namespace detail { + + /// @brief Computes the normalization and Jacobian of a vector. + /// @note Prefer the ipc::normalization_and_jacobian front end below, which + /// deduces both the scalar type and the dimension. + /// @tparam T The scalar type. + /// @tparam dim The dimension (2 or 3). + /// @param x The input vector. + /// @return A tuple containing the normalized vector and its Jacobian. + template + inline std::tuple, Eigen::Matrix> + normalization_and_jacobian(const Eigen::Vector& x) + { + static_assert(dim == 2 || dim == 3, "normalization is only 2D or 3D"); + const T norm = x.norm(); + const Eigen::Vector xhat = x / norm; + return { xhat, + (Eigen::Matrix::Identity() + - (xhat * xhat.transpose())) + / norm }; + } + + /// @brief Computes the normalization, Jacobian, and Hessian of a vector. + /// @tparam T The scalar type. + /// @tparam dim The dimension (2 or 3). + /// @param x The input vector. + /// @return A tuple of the normalized vector, its Jacobian, and its Hessian. + template + inline std::tuple< + Eigen::Vector, + Eigen::Matrix, + std::array, dim>> + normalization_and_jacobian_and_hessian(const Eigen::Vector& x) + { + static_assert(dim == 2 || dim == 3, "normalization is only 2D or 3D"); + const T norm = x.norm(); + const Eigen::Vector xhat = x / norm; + const Eigen::Matrix J = + (Eigen::Matrix::Identity() - (xhat * xhat.transpose())) + / norm; + + std::array, dim> H; + for (int i = 0; i < dim; i++) { + H[i] = (xhat(i) * J + xhat * J.row(i) + J.col(i) * xhat.transpose()) + / (-norm); + } + return { xhat, J, H }; + } + +} // namespace detail + /// @brief Computes the normalization and Jacobian of a vector. +/// +/// Accepts any Eigen vector expression (row or column). The dimension is +/// resolved at compile time when the argument type knows it and with a single +/// branch otherwise. When the dimension is known the return type is +/// std::tuple, Eigen::Matrix>; otherwise it +/// is std::tuple, MatrixMax3>. +/// /// @param x The input vector. /// @return A tuple containing the normalized vector and its Jacobian. -std::tuple -normalization_and_jacobian(Eigen::ConstRef x); +template +inline auto normalization_and_jacobian(const Eigen::MatrixBase& x) +{ + using T = typename DerivedX::Scalar; + + if constexpr (dim_v == 2) { + return detail::normalization_and_jacobian(x); + } else if constexpr (dim_v == 3) { + return detail::normalization_and_jacobian(x); + } else if (x.size() == 2) { + const auto [xhat, J] = detail::normalization_and_jacobian(x); + return std::tuple, MatrixMax3>(xhat, J); + } else { + assert(x.size() == 3); + const auto [xhat, J] = detail::normalization_and_jacobian(x); + return std::tuple, MatrixMax3>(xhat, J); + } +} /// @brief Computes the Jacobian of the normalization operation. /// @param x The input vector. /// @return The Jacobian of the normalization operation. -inline MatrixMax3d normalization_jacobian(Eigen::ConstRef x) +template +inline auto normalization_jacobian(const Eigen::MatrixBase& x) { - return std::get<1>(normalization_and_jacobian(x)); + using T = typename DerivedX::Scalar; + + if constexpr (dim_v == 2) { + return std::get<1>(detail::normalization_and_jacobian(x)); + } else if constexpr (dim_v == 3) { + return std::get<1>(detail::normalization_and_jacobian(x)); + } else if (x.size() == 2) { + return MatrixMax3( + std::get<1>(detail::normalization_and_jacobian(x))); + } else { + assert(x.size() == 3); + return MatrixMax3( + std::get<1>(detail::normalization_and_jacobian(x))); + } } /// @brief Computes the normalization, Jacobian, and Hessian of a vector. -/// @param t The input vector. -/// @return A tuple containing the normalized vector, its Jacobian, and its Hessian. -std::tuple> -normalization_and_jacobian_and_hessian(Eigen::ConstRef x); +/// @param x The input vector. +/// @return A tuple of the normalized vector, its Jacobian, and its Hessian. +template +inline auto +normalization_and_jacobian_and_hessian(const Eigen::MatrixBase& x) +{ + using T = typename DerivedX::Scalar; + using Ret = + std::tuple, MatrixMax3, std::array, 3>>; + + if constexpr (dim_v == 2) { + return detail::normalization_and_jacobian_and_hessian(x); + } else if constexpr (dim_v == 3) { + return detail::normalization_and_jacobian_and_hessian(x); + } else if (x.size() == 2) { + const auto [xhat, J, H] = + detail::normalization_and_jacobian_and_hessian(x); + return Ret( + xhat, J, + std::array, 3> { + MatrixMax3(H[0]), + MatrixMax3(H[1]), + MatrixMax3(), // H[2] is empty in 2D + }); + } else { + assert(x.size() == 3); + const auto [xhat, J, H] = + detail::normalization_and_jacobian_and_hessian(x); + return Ret( + xhat, J, + std::array, 3> { + MatrixMax3(H[0]), + MatrixMax3(H[1]), + MatrixMax3(H[2]), + }); + } +} /// @brief Computes the Hessian of the normalization operation. /// @param x The input vector. /// @return The Hessian of the normalization operation. -inline std::array -normalization_hessian(Eigen::ConstRef x) +template +inline auto normalization_hessian(const Eigen::MatrixBase& x) { return std::get<2>(normalization_and_jacobian_and_hessian(x)); } -// ============================================================================= +namespace detail { + + // ========================================================================= + + /// @brief Cross product matrix for 3D vectors. + /// @param v Vector to create the cross product matrix for. + /// @return The cross product matrix of the vector. + template + inline Eigen::Matrix3 + cross_product_matrix(Eigen::ConstRef> v) + { + Eigen::Matrix3 m; + // clang-format off + m << T(0), -v(2), v(1), + v(2), T(0), -v(0), + -v(1), v(0), T(0); + // clang-format on + return m; + } + + /// @brief Computes the Jacobian of the cross product matrix. + /// @return The Jacobian of the cross product matrix. + template + Eigen::Matrix cross_product_matrix_jacobian(); + + // ========================================================================= + + /** + * \defgroup geometry Point-line normal + * \brief Functions for computing a point-line normal and resp. Jacobians. + * @{ + */ + + /// @brief Computes the unnormalized normal vector of a point-line pair. + /// @param p The vertex position. + /// @param e0 The start position of the line. + /// @param e1 The end position of the line. + /// @return The unnormalized normal vector. + template + VectorMax3 point_line_unnormalized_normal( + Eigen::ConstRef> p, + Eigen::ConstRef> e0, + Eigen::ConstRef> e1); + + /// @brief Computes the normal vector of a point-line pair. + /// @param p The vertex position. + /// @param e0 The start position of the line. + /// @param e1 The end position of the line. + /// @return The normal vector. + template + inline VectorMax3 point_line_normal( + Eigen::ConstRef> p, + Eigen::ConstRef> e0, + Eigen::ConstRef> e1) + { + return point_line_unnormalized_normal(p, e0, e1).normalized(); + } + + /// @brief Computes the Jacobian of the unnormalized normal vector of a + /// point-line pair. + /// @param p The vertex position. + /// @param e0 The start position of the line. + /// @param e1 The end position of the line. + /// @return The Jacobian of the unnormalized normal vector. + template + MatrixMax point_line_unnormalized_normal_jacobian( + Eigen::ConstRef> p, + Eigen::ConstRef> e0, + Eigen::ConstRef> e1); + /// @brief Computes the Hessian of the unnormalized normal vector of a + /// point-line pair. + /// @param p The vertex position. + /// @param e0 The start position of the line. + /// @param e1 The end position of the line. + /// @return The Hessian of the unnormalized normal vector of the point-line + /// pair. + template + MatrixMax point_line_unnormalized_normal_hessian( + Eigen::ConstRef> p, + Eigen::ConstRef> e0, + Eigen::ConstRef> e1); + + /// @brief Computes the Jacobian of the normal vector of a point-line pair. + /// @param p The vertex position. + /// @param e0 The start position of the line. + /// @param e1 The end position of the line. + /// @return The Jacobian of the normal vector. + template + inline MatrixMax point_line_normal_jacobian( + Eigen::ConstRef> p, + Eigen::ConstRef> e0, + Eigen::ConstRef> e1) + { + // ∂n̂/∂x = ∂n̂/∂n * ∂n/∂x + return normalization_jacobian(point_line_unnormalized_normal(p, e0, e1)) + * point_line_unnormalized_normal_jacobian(p, e0, e1); + } + + /// @brief Computes the Hessian of the normal vector of a point-line pair. + /// @param p The vertex position. + /// @param e0 The start position of the line. + /// @param e1 The end position of the line. + /// @return The Hessian of the normal vector. + template + MatrixMax point_line_normal_hessian( + Eigen::ConstRef> p, + Eigen::ConstRef> e0, + Eigen::ConstRef> e1); + + /** @} */ + + // ========================================================================= + + /** + * \defgroup geometry Triangle normal + * \brief Functions for computing a triangle's normal and resp. Jacobians. + * @{ + */ + + /// @brief Computes the unnormalized normal vector of a triangle. + /// @param a The first vertex of the triangle. + /// @param b The second vertex of the triangle. + /// @param c The third vertex of the triangle. + /// @return The unnormalized normal vector of the triangle. + template + inline Eigen::Vector3 triangle_unnormalized_normal( + Eigen::ConstRef> a, + Eigen::ConstRef> b, + Eigen::ConstRef> c) + { + return (b - a).cross(c - a); + } + + /// @brief Computes the normal vector of a triangle. + /// @param a The first vertex of the triangle. + /// @param b The second vertex of the triangle. + /// @param c The third vertex of the triangle. + /// @return The normal vector of the triangle. + template + inline Eigen::Vector3 triangle_normal( + Eigen::ConstRef> a, + Eigen::ConstRef> b, + Eigen::ConstRef> c) + { + return triangle_unnormalized_normal(a, b, c).normalized(); + } + + /// @brief Computes the Jacobian of the unnormalized normal vector of a + /// triangle. + /// @param a The first vertex of the triangle. + /// @param b The second vertex of the triangle. + /// @param c The third vertex of the triangle. + /// @return The Jacobian of the unnormalized normal vector of the triangle. + template + inline Eigen::Matrix triangle_unnormalized_normal_jacobian( + Eigen::ConstRef> a, + Eigen::ConstRef> b, + Eigen::ConstRef> c) + { + Eigen::Matrix J; + J.template middleCols<3>(0) = cross_product_matrix(c - b); // ∂n/∂a + J.template middleCols<3>(3) = cross_product_matrix(a - c); // ∂n/∂b + J.template middleCols<3>(6) = cross_product_matrix(b - a); // ∂n/∂c + return J; + } + + /// @brief Computes the Hessian of the unnormalized normal vector of a + /// triangle. + /// @param a The first vertex of the triangle. + /// @param b The second vertex of the triangle. + /// @param c The third vertex of the triangle. + /// @return The Hessian of the unnormalized normal vector of the triangle. + template + Eigen::Matrix triangle_unnormalized_normal_hessian( + Eigen::ConstRef> a, + Eigen::ConstRef> b, + Eigen::ConstRef> c); + + /// @brief Computes the Jacobian of the normal vector of a triangle. + /// @param a The first vertex of the triangle. + /// @param b The second vertex of the triangle. + /// @param c The third vertex of the triangle. + /// @return The Jacobian of the normal vector of the triangle. + template + inline Eigen::Matrix triangle_normal_jacobian( + Eigen::ConstRef> a, + Eigen::ConstRef> b, + Eigen::ConstRef> c) + { + // ∂n̂/∂x = ∂n̂/∂n * ∂n/∂x + return normalization_jacobian(triangle_unnormalized_normal(a, b, c)) + * triangle_unnormalized_normal_jacobian(a, b, c); + } + + /// @brief Computes the Hessian of the normal vector of a triangle. + /// @param a The first vertex of the triangle. + /// @param b The second vertex of the triangle. + /// @param c The third vertex of the triangle. + /// @return The Hessian of the normal vector of the triangle. + template + Eigen::Matrix triangle_normal_hessian( + Eigen::ConstRef> a, + Eigen::ConstRef> b, + Eigen::ConstRef> c); + + /** @} */ + + // ========================================================================= + + /** + * \defgroup geometry Line-line normal + * \brief Functions for computing a line-line normal and resp. Jacobians. + * @{ + */ + + /// @brief Computes the unnormalized normal vector of two lines. + /// @param ea0 The first vertex of the first line. + /// @param ea1 The second vertex of the first line. + /// @param eb0 The first vertex of the second line. + /// @param eb1 The second vertex of the second line. + /// @return The unnormalized normal vector of the two lines. + template + inline Eigen::Vector3 line_line_unnormalized_normal( + Eigen::ConstRef> ea0, + Eigen::ConstRef> ea1, + Eigen::ConstRef> eb0, + Eigen::ConstRef> eb1) + { + return (ea1 - ea0).cross(eb1 - eb0); + } + + /// @brief Computes the normal vector of two lines. + /// @param ea0 The first vertex of the first line. + /// @param ea1 The second vertex of the first line. + /// @param eb0 The first vertex of the second line. + /// @param eb1 The second vertex of the second line. + /// @return The normal vector of the two lines. + template + inline Eigen::Vector3 line_line_normal( + Eigen::ConstRef> ea0, + Eigen::ConstRef> ea1, + Eigen::ConstRef> eb0, + Eigen::ConstRef> eb1) + { + return line_line_unnormalized_normal(ea0, ea1, eb0, eb1).normalized(); + } + + /// @brief Computes the Jacobian of the unnormalized normal vector of two + /// lines. + /// @param ea0 The first vertex of the first line. + /// @param ea1 The second vertex of the first line. + /// @param eb0 The first vertex of the second line. + /// @param eb1 The second vertex of the second line. + /// @return The Jacobian of the unnormalized normal vector of the two lines. + template + inline Eigen::Matrix line_line_unnormalized_normal_jacobian( + Eigen::ConstRef> ea0, + Eigen::ConstRef> ea1, + Eigen::ConstRef> eb0, + Eigen::ConstRef> eb1) + { + Eigen::Matrix J; + J.template middleCols<3>(0) = cross_product_matrix(eb1 - eb0); + J.template middleCols<3>(3) = cross_product_matrix(eb0 - eb1); + J.template middleCols<3>(6) = cross_product_matrix(ea0 - ea1); + J.template middleCols<3>(9) = cross_product_matrix(ea1 - ea0); + return J; + } + + /// @brief Computes the Jacobian of the normal vector of two lines. + /// @param ea0 The first vertex of the first line. + /// @param ea1 The second vertex of the first line. + /// @param eb0 The first vertex of the second line. + /// @param eb1 The second vertex of the second line. + /// @return The Jacobian of the normal vector of the two lines. + template + inline Eigen::Matrix line_line_normal_jacobian( + Eigen::ConstRef> ea0, + Eigen::ConstRef> ea1, + Eigen::ConstRef> eb0, + Eigen::ConstRef> eb1) + { + // ∂n̂/∂x = ∂n̂/∂n * ∂n/∂x + return normalization_jacobian( + line_line_unnormalized_normal(ea0, ea1, eb0, eb1)) + * line_line_unnormalized_normal_jacobian(ea0, ea1, eb0, eb1); + } + + /// @brief Computes the Hessian of the unnormalized normal vector of two + /// lines. + /// @param ea0 The first vertex of the first line. + /// @param ea1 The second vertex of the first line. + /// @param eb0 The first vertex of the second line. + /// @param eb1 The second vertex of the second line. + /// @return The Hessian of the unnormalized normal vector of the two lines. + template + Eigen::Matrix line_line_unnormalized_normal_hessian( + Eigen::ConstRef> ea0, + Eigen::ConstRef> ea1, + Eigen::ConstRef> eb0, + Eigen::ConstRef> eb1); + + /// @brief Computes the Hessian of the normal vector of two lines. + /// @param ea0 The first vertex of the first line. + /// @param ea1 The second vertex of the first line. + /// @param eb0 The first vertex of the second line. + /// @param eb1 The second vertex of the second line. + /// @return The Hessian of the normal vector of the two lines. + template + Eigen::Matrix line_line_normal_hessian( + Eigen::ConstRef> ea0, + Eigen::ConstRef> ea1, + Eigen::ConstRef> eb0, + Eigen::ConstRef> eb1); + +} // namespace detail + +/** @} */ + +// --- EigenExpression wrappers --- + +/// @brief Computes the Jacobian of the cross product matrix. +/// @return The Jacobian of the cross product matrix. +template +inline Eigen::Matrix cross_product_matrix_jacobian() +{ + return detail::cross_product_matrix_jacobian(); +} /// @brief Cross product matrix for 3D vectors. /// @param v Vector to create the cross product matrix for. /// @return The cross product matrix of the vector. -inline Eigen::Matrix3d cross_product_matrix(Eigen::ConstRef v) +template +inline auto cross_product_matrix(const Eigen::MatrixBase& v) { - Eigen::Matrix3d m; - m << 0, -v(2), v(1), // - v(2), 0, -v(0), // - -v(1), v(0), 0; - return m; + using T = typename DerivedX::Scalar; + return detail::cross_product_matrix(v); } -/// @brief Computes the Jacobian of the cross product matrix. -/// @return The Jacobian of the cross product matrix. -Eigen::Matrix cross_product_matrix_jacobian(); - -// ============================================================================= - -/** - * \defgroup geometry Point-line normal - * \brief Functions for computing a point-line normal and resp. Jacobians. - * @{ - */ - /// @brief Computes the unnormalized normal vector of a point-line pair. /// @param p The vertex position. /// @param e0 The start position of the line. /// @param e1 The end position of the line. /// @return The unnormalized normal vector. -VectorMax3d point_line_unnormalized_normal( - Eigen::ConstRef p, - Eigen::ConstRef e0, - Eigen::ConstRef e1); +template +inline auto point_line_unnormalized_normal( + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& e0, + const Eigen::MatrixBase& e1) +{ + using T = typename DerivedP::Scalar; + return detail::point_line_unnormalized_normal(p, e0, e1); +} /// @brief Computes the normal vector of a point-line pair. /// @param p The vertex position. /// @param e0 The start position of the line. /// @param e1 The end position of the line. /// @return The normal vector. -inline VectorMax3d point_line_normal( - Eigen::ConstRef p, - Eigen::ConstRef e0, - Eigen::ConstRef e1) +template +inline auto point_line_normal( + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& e0, + const Eigen::MatrixBase& e1) { - return point_line_unnormalized_normal(p, e0, e1).normalized(); + using T = typename DerivedP::Scalar; + return detail::point_line_normal(p, e0, e1); } -/// @brief Computes the Jacobian of the unnormalized normal vector of a point-line pair. +/// @brief Computes the Jacobian of the unnormalized normal vector of a +/// point-line pair. /// @param p The vertex position. /// @param e0 The start position of the line. /// @param e1 The end position of the line. /// @return The Jacobian of the unnormalized normal vector. -MatrixMax point_line_unnormalized_normal_jacobian( - Eigen::ConstRef p, - Eigen::ConstRef e0, - Eigen::ConstRef e1); +template +inline auto point_line_unnormalized_normal_jacobian( + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& e0, + const Eigen::MatrixBase& e1) +{ + using T = typename DerivedP::Scalar; + return detail::point_line_unnormalized_normal_jacobian(p, e0, e1); +} -/// @brief Computes the Hessian of the unnormalized normal vector of a point-line pair. +/// @brief Computes the Hessian of the unnormalized normal vector of a +/// point-line pair. /// @param p The vertex position. /// @param e0 The start position of the line. /// @param e1 The end position of the line. -/// @return The Hessian of the unnormalized normal vector of the point-line pair. -MatrixMax point_line_unnormalized_normal_hessian( - Eigen::ConstRef p, - Eigen::ConstRef e0, - Eigen::ConstRef e1); +/// @return The Hessian of the unnormalized normal vector of the point-line +/// pair. +template +inline auto point_line_unnormalized_normal_hessian( + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& e0, + const Eigen::MatrixBase& e1) +{ + using T = typename DerivedP::Scalar; + return detail::point_line_unnormalized_normal_hessian(p, e0, e1); +} /// @brief Computes the Jacobian of the normal vector of a point-line pair. /// @param p The vertex position. /// @param e0 The start position of the line. /// @param e1 The end position of the line. /// @return The Jacobian of the normal vector. -inline MatrixMax point_line_normal_jacobian( - Eigen::ConstRef p, - Eigen::ConstRef e0, - Eigen::ConstRef e1) +template +inline auto point_line_normal_jacobian( + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& e0, + const Eigen::MatrixBase& e1) { - // ∂n̂/∂x = ∂n̂/∂n * ∂n/∂x - return normalization_jacobian(point_line_unnormalized_normal(p, e0, e1)) - * point_line_unnormalized_normal_jacobian(p, e0, e1); + using T = typename DerivedP::Scalar; + return detail::point_line_normal_jacobian(p, e0, e1); } /// @brief Computes the Hessian of the normal vector of a point-line pair. @@ -126,32 +581,29 @@ inline MatrixMax point_line_normal_jacobian( /// @param e0 The start position of the line. /// @param e1 The end position of the line. /// @return The Hessian of the normal vector. -MatrixMax point_line_normal_hessian( - Eigen::ConstRef p, - Eigen::ConstRef e0, - Eigen::ConstRef e1); - -/** @} */ - -// ============================================================================= - -/** - * \defgroup geometry Triangle normal - * \brief Functions for computing a triangle's normal and resp. Jacobians. - * @{ - */ +template +inline auto point_line_normal_hessian( + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& e0, + const Eigen::MatrixBase& e1) +{ + using T = typename DerivedP::Scalar; + return detail::point_line_normal_hessian(p, e0, e1); +} /// @brief Computes the unnormalized normal vector of a triangle. /// @param a The first vertex of the triangle. /// @param b The second vertex of the triangle. /// @param c The third vertex of the triangle. /// @return The unnormalized normal vector of the triangle. -inline Eigen::Vector3d triangle_unnormalized_normal( - Eigen::ConstRef a, - Eigen::ConstRef b, - Eigen::ConstRef c) +template +inline auto triangle_unnormalized_normal( + const Eigen::MatrixBase& a, + const Eigen::MatrixBase& b, + const Eigen::MatrixBase& c) { - return (b - a).cross(c - a); + using T = typename DerivedA::Scalar; + return detail::triangle_unnormalized_normal(a, b, c); } /// @brief Computes the normal vector of a triangle. @@ -159,54 +611,61 @@ inline Eigen::Vector3d triangle_unnormalized_normal( /// @param b The second vertex of the triangle. /// @param c The third vertex of the triangle. /// @return The normal vector of the triangle. -inline Eigen::Vector3d triangle_normal( - Eigen::ConstRef a, - Eigen::ConstRef b, - Eigen::ConstRef c) +template +inline auto triangle_normal( + const Eigen::MatrixBase& a, + const Eigen::MatrixBase& b, + const Eigen::MatrixBase& c) { - return triangle_unnormalized_normal(a, b, c).normalized(); + using T = typename DerivedA::Scalar; + return detail::triangle_normal(a, b, c); } -/// @brief Computes the Jacobian of the unnormalized normal vector of a triangle. +/// @brief Computes the Jacobian of the unnormalized normal vector of a +/// triangle. /// @param a The first vertex of the triangle. /// @param b The second vertex of the triangle. /// @param c The third vertex of the triangle. /// @return The Jacobian of the unnormalized normal vector of the triangle. -inline Eigen::Matrix triangle_unnormalized_normal_jacobian( - Eigen::ConstRef a, - Eigen::ConstRef b, - Eigen::ConstRef c) +template +inline auto triangle_unnormalized_normal_jacobian( + const Eigen::MatrixBase& a, + const Eigen::MatrixBase& b, + const Eigen::MatrixBase& c) { - Eigen::Matrix J; - J.middleCols<3>(0) = cross_product_matrix(c - b); // ∂n/∂a - J.middleCols<3>(3) = cross_product_matrix(a - c); // ∂n/∂b - J.middleCols<3>(6) = cross_product_matrix(b - a); // ∂n/∂c - return J; + using T = typename DerivedA::Scalar; + return detail::triangle_unnormalized_normal_jacobian(a, b, c); } -/// @brief Computes the Hessian of the unnormalized normal vector of a triangle. +/// @brief Computes the Hessian of the unnormalized normal vector of a +/// triangle. /// @param a The first vertex of the triangle. /// @param b The second vertex of the triangle. /// @param c The third vertex of the triangle. /// @return The Hessian of the unnormalized normal vector of the triangle. -Eigen::Matrix triangle_unnormalized_normal_hessian( - Eigen::ConstRef a, - Eigen::ConstRef b, - Eigen::ConstRef c); +template +inline auto triangle_unnormalized_normal_hessian( + const Eigen::MatrixBase& a, + const Eigen::MatrixBase& b, + const Eigen::MatrixBase& c) +{ + using T = typename DerivedA::Scalar; + return detail::triangle_unnormalized_normal_hessian(a, b, c); +} /// @brief Computes the Jacobian of the normal vector of a triangle. /// @param a The first vertex of the triangle. /// @param b The second vertex of the triangle. /// @param c The third vertex of the triangle. /// @return The Jacobian of the normal vector of the triangle. -inline Eigen::Matrix triangle_normal_jacobian( - Eigen::ConstRef a, - Eigen::ConstRef b, - Eigen::ConstRef c) +template +inline auto triangle_normal_jacobian( + const Eigen::MatrixBase& a, + const Eigen::MatrixBase& b, + const Eigen::MatrixBase& c) { - // ∂n̂/∂x = ∂n̂/∂n * ∂n/∂x - return normalization_jacobian(triangle_unnormalized_normal(a, b, c)) - * triangle_unnormalized_normal_jacobian(a, b, c); + using T = typename DerivedA::Scalar; + return detail::triangle_normal_jacobian(a, b, c); } /// @brief Computes the Hessian of the normal vector of a triangle. @@ -214,20 +673,15 @@ inline Eigen::Matrix triangle_normal_jacobian( /// @param b The second vertex of the triangle. /// @param c The third vertex of the triangle. /// @return The Hessian of the normal vector of the triangle. -Eigen::Matrix triangle_normal_hessian( - Eigen::ConstRef a, - Eigen::ConstRef b, - Eigen::ConstRef c); - -/** @} */ - -// ============================================================================= - -/** - * \defgroup geometry Line-line normal - * \brief Functions for computing a line-line normal and resp. Jacobians. - * @{ - */ +template +inline auto triangle_normal_hessian( + const Eigen::MatrixBase& a, + const Eigen::MatrixBase& b, + const Eigen::MatrixBase& c) +{ + using T = typename DerivedA::Scalar; + return detail::triangle_normal_hessian(a, b, c); +} /// @brief Computes the unnormalized normal vector of two lines. /// @param ea0 The first vertex of the first line. @@ -235,13 +689,19 @@ Eigen::Matrix triangle_normal_hessian( /// @param eb0 The first vertex of the second line. /// @param eb1 The second vertex of the second line. /// @return The unnormalized normal vector of the two lines. -inline Eigen::Vector3d line_line_unnormalized_normal( - Eigen::ConstRef ea0, - Eigen::ConstRef ea1, - Eigen::ConstRef eb0, - Eigen::ConstRef eb1) +template < + typename DerivedEA0, + typename DerivedEA1, + typename DerivedEB0, + typename DerivedEB1> +inline auto line_line_unnormalized_normal( + const Eigen::MatrixBase& ea0, + const Eigen::MatrixBase& ea1, + const Eigen::MatrixBase& eb0, + const Eigen::MatrixBase& eb1) { - return (ea1 - ea0).cross(eb1 - eb0); + using T = typename DerivedEA0::Scalar; + return detail::line_line_unnormalized_normal(ea0, ea1, eb0, eb1); } /// @brief Computes the normal vector of two lines. @@ -250,33 +710,42 @@ inline Eigen::Vector3d line_line_unnormalized_normal( /// @param eb0 The first vertex of the second line. /// @param eb1 The second vertex of the second line. /// @return The normal vector of the two lines. -inline Eigen::Vector3d line_line_normal( - Eigen::ConstRef ea0, - Eigen::ConstRef ea1, - Eigen::ConstRef eb0, - Eigen::ConstRef eb1) +template < + typename DerivedEA0, + typename DerivedEA1, + typename DerivedEB0, + typename DerivedEB1> +inline auto line_line_normal( + const Eigen::MatrixBase& ea0, + const Eigen::MatrixBase& ea1, + const Eigen::MatrixBase& eb0, + const Eigen::MatrixBase& eb1) { - return line_line_unnormalized_normal(ea0, ea1, eb0, eb1).normalized(); + using T = typename DerivedEA0::Scalar; + return detail::line_line_normal(ea0, ea1, eb0, eb1); } -/// @brief Computes the Jacobian of the unnormalized normal vector of two lines. +/// @brief Computes the Jacobian of the unnormalized normal vector of two +/// lines. /// @param ea0 The first vertex of the first line. /// @param ea1 The second vertex of the first line. /// @param eb0 The first vertex of the second line. /// @param eb1 The second vertex of the second line. /// @return The Jacobian of the unnormalized normal vector of the two lines. -inline Eigen::Matrix line_line_unnormalized_normal_jacobian( - Eigen::ConstRef ea0, - Eigen::ConstRef ea1, - Eigen::ConstRef eb0, - Eigen::ConstRef eb1) +template < + typename DerivedEA0, + typename DerivedEA1, + typename DerivedEB0, + typename DerivedEB1> +inline auto line_line_unnormalized_normal_jacobian( + const Eigen::MatrixBase& ea0, + const Eigen::MatrixBase& ea1, + const Eigen::MatrixBase& eb0, + const Eigen::MatrixBase& eb1) { - Eigen::Matrix J; - J.middleCols<3>(0) = cross_product_matrix(eb1 - eb0); - J.middleCols<3>(3) = cross_product_matrix(eb0 - eb1); - J.middleCols<3>(6) = cross_product_matrix(ea0 - ea1); - J.middleCols<3>(9) = cross_product_matrix(ea1 - ea0); - return J; + using T = typename DerivedEA0::Scalar; + return detail::line_line_unnormalized_normal_jacobian( + ea0, ea1, eb0, eb1); } /// @brief Computes the Jacobian of the normal vector of two lines. @@ -285,29 +754,42 @@ inline Eigen::Matrix line_line_unnormalized_normal_jacobian( /// @param eb0 The first vertex of the second line. /// @param eb1 The second vertex of the second line. /// @return The Jacobian of the normal vector of the two lines. -inline Eigen::Matrix line_line_normal_jacobian( - Eigen::ConstRef ea0, - Eigen::ConstRef ea1, - Eigen::ConstRef eb0, - Eigen::ConstRef eb1) +template < + typename DerivedEA0, + typename DerivedEA1, + typename DerivedEB0, + typename DerivedEB1> +inline auto line_line_normal_jacobian( + const Eigen::MatrixBase& ea0, + const Eigen::MatrixBase& ea1, + const Eigen::MatrixBase& eb0, + const Eigen::MatrixBase& eb1) { - // ∂n̂/∂x = ∂n̂/∂n * ∂n/∂x - return normalization_jacobian( - line_line_unnormalized_normal(ea0, ea1, eb0, eb1)) - * line_line_unnormalized_normal_jacobian(ea0, ea1, eb0, eb1); + using T = typename DerivedEA0::Scalar; + return detail::line_line_normal_jacobian(ea0, ea1, eb0, eb1); } -/// @brief Computes the Hessian of the unnormalized normal vector of two lines. +/// @brief Computes the Hessian of the unnormalized normal vector of two +/// lines. /// @param ea0 The first vertex of the first line. /// @param ea1 The second vertex of the first line. /// @param eb0 The first vertex of the second line. /// @param eb1 The second vertex of the second line. /// @return The Hessian of the unnormalized normal vector of the two lines. -Eigen::Matrix line_line_unnormalized_normal_hessian( - Eigen::ConstRef ea0, - Eigen::ConstRef ea1, - Eigen::ConstRef eb0, - Eigen::ConstRef eb1); +template < + typename DerivedEA0, + typename DerivedEA1, + typename DerivedEB0, + typename DerivedEB1> +inline auto line_line_unnormalized_normal_hessian( + const Eigen::MatrixBase& ea0, + const Eigen::MatrixBase& ea1, + const Eigen::MatrixBase& eb0, + const Eigen::MatrixBase& eb1) +{ + using T = typename DerivedEA0::Scalar; + return detail::line_line_unnormalized_normal_hessian(ea0, ea1, eb0, eb1); +} /// @brief Computes the Hessian of the normal vector of two lines. /// @param ea0 The first vertex of the first line. @@ -315,12 +797,19 @@ Eigen::Matrix line_line_unnormalized_normal_hessian( /// @param eb0 The first vertex of the second line. /// @param eb1 The second vertex of the second line. /// @return The Hessian of the normal vector of the two lines. -Eigen::Matrix line_line_normal_hessian( - Eigen::ConstRef ea0, - Eigen::ConstRef ea1, - Eigen::ConstRef eb0, - Eigen::ConstRef eb1); - -/** @} */ +template < + typename DerivedEA0, + typename DerivedEA1, + typename DerivedEB0, + typename DerivedEB1> +inline auto line_line_normal_hessian( + const Eigen::MatrixBase& ea0, + const Eigen::MatrixBase& ea1, + const Eigen::MatrixBase& eb0, + const Eigen::MatrixBase& eb1) +{ + using T = typename DerivedEA0::Scalar; + return detail::line_line_normal_hessian(ea0, ea1, eb0, eb1); +} -} // namespace ipc \ No newline at end of file +} // namespace ipc diff --git a/src/ipc/potentials/barrier_potential.cpp b/src/ipc/potentials/barrier_potential.cpp index 47bb32dbb..5a0ef8004 100644 --- a/src/ipc/potentials/barrier_potential.cpp +++ b/src/ipc/potentials/barrier_potential.cpp @@ -8,7 +8,7 @@ namespace ipc { BarrierPotential::BarrierPotential( const double dhat, const double stiffness, const bool use_physical_barrier) : BarrierPotential( - std::make_shared(), + std::make_shared>(), dhat, stiffness, use_physical_barrier) diff --git a/src/ipc/potentials/barrier_potential.hpp b/src/ipc/potentials/barrier_potential.hpp index bf5e23a70..3c780c23b 100644 --- a/src/ipc/potentials/barrier_potential.hpp +++ b/src/ipc/potentials/barrier_potential.hpp @@ -138,7 +138,8 @@ class BarrierPotential : public NormalPotential { const double distance_squared, const double dmin = 0) const override; /// @brief The barrier function used to compute the potential. - std::shared_ptr m_barrier = std::make_shared(); + std::shared_ptr m_barrier = + std::make_shared>(); /// @brief The activation distance of the barrier. double m_dhat; diff --git a/src/ipc/smooth_contact/distance/edge_edge.cpp b/src/ipc/smooth_contact/distance/edge_edge.cpp index b01a13466..134193ddb 100644 --- a/src/ipc/smooth_contact/distance/edge_edge.cpp +++ b/src/ipc/smooth_contact/distance/edge_edge.cpp @@ -1,5 +1,8 @@ #include "edge_edge.hpp" +#include +#include +#include #include #include @@ -215,70 +218,6 @@ line_line_closest_point_pairs_hessian( 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, @@ -371,6 +310,12 @@ Eigen::Vector3 edge_edge_closest_point_direction( } } +/// @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, @@ -486,49 +431,6 @@ template Eigen::Vector3> edge_edge_closest_point_direction( 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, diff --git a/src/ipc/smooth_contact/distance/edge_edge.hpp b/src/ipc/smooth_contact/distance/edge_edge.hpp index 4162111f9..ba880cce7 100644 --- a/src/ipc/smooth_contact/distance/edge_edge.hpp +++ b/src/ipc/smooth_contact/distance/edge_edge.hpp @@ -4,21 +4,6 @@ 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, diff --git a/src/ipc/smooth_contact/distance/mollifier.tpp b/src/ipc/smooth_contact/distance/mollifier.tpp index 701f1de12..353b9e2e9 100644 --- a/src/ipc/smooth_contact/distance/mollifier.tpp +++ b/src/ipc/smooth_contact/distance/mollifier.tpp @@ -2,6 +2,8 @@ #include "mollifier.hpp" +#include + namespace ipc { template scalar point_edge_mollifier( @@ -73,16 +75,13 @@ scalar point_face_mollifier( // 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) + (point_line_distance(p, e0, e1) - dist_sqr) / MOLLIFIER_THRESHOLD_EPS / dist_sqr) * Math::mollifier( - (PointEdgeDistance::point_line_sqr_distance(p, e2, e1) - - dist_sqr) + (point_line_distance(p, e2, e1) - dist_sqr) / MOLLIFIER_THRESHOLD_EPS / dist_sqr) * Math::mollifier( - (PointEdgeDistance::point_line_sqr_distance(p, e0, e2) - - dist_sqr) + (point_line_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 index 0a840d6bc..b000fdcec 100644 --- a/src/ipc/smooth_contact/distance/point_edge.cpp +++ b/src/ipc/smooth_contact/distance/point_edge.cpp @@ -4,28 +4,6 @@ #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, @@ -35,11 +13,11 @@ scalar PointEdgeDistance::point_edge_sqr_distance( { switch (dtype) { case PointEdgeDistanceType::P_E: - return point_line_sqr_distance(p, e0, e1); + return point_line_distance(p, e0, e1); case PointEdgeDistanceType::P_E0: - return point_point_sqr_distance(p, e0); + return point_point_distance(p, e0); case PointEdgeDistanceType::P_E1: - return point_point_sqr_distance(p, e1); + return point_point_distance(p, e1); case PointEdgeDistanceType::AUTO: default: const Eigen::Vector t = e1 - e0; diff --git a/src/ipc/smooth_contact/distance/point_edge.hpp b/src/ipc/smooth_contact/distance/point_edge.hpp index 11bade859..f8ae36b79 100644 --- a/src/ipc/smooth_contact/distance/point_edge.hpp +++ b/src/ipc/smooth_contact/distance/point_edge.hpp @@ -2,12 +2,12 @@ #include #include +#include +#include #include #include #include -#include - namespace ipc { template class PointEdgeDistance { public: @@ -17,14 +17,6 @@ template class PointEdgeDistance { PointEdgeDistance(const PointEdgeDistance&) = delete; PointEdgeDistance& operator=(const PointEdgeDistance&) = delete; - static T point_point_sqr_distance( - Eigen::ConstRef a, Eigen::ConstRef b); - - static T point_line_sqr_distance( - Eigen::ConstRef p, - Eigen::ConstRef e0, - Eigen::ConstRef e1); - static T point_edge_sqr_distance( Eigen::ConstRef p, Eigen::ConstRef e0, diff --git a/src/ipc/smooth_contact/distance/point_face.cpp b/src/ipc/smooth_contact/distance/point_face.cpp index ea653ee2d..bd57c17b5 100644 --- a/src/ipc/smooth_contact/distance/point_face.cpp +++ b/src/ipc/smooth_contact/distance/point_face.cpp @@ -234,67 +234,6 @@ point_triangle_closest_point_direction_hessian( 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, @@ -358,38 +297,6 @@ Eigen::Vector3 point_triangle_closest_point_direction( } } -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, @@ -420,4 +327,4 @@ template Eigen::Vector3d point_triangle_closest_point_direction( Eigen::ConstRef t1, Eigen::ConstRef t2, PointTriangleDistanceType dtype); -} // namespace ipc \ No newline at end of file +} // namespace ipc diff --git a/src/ipc/smooth_contact/distance/point_face.hpp b/src/ipc/smooth_contact/distance/point_face.hpp index 99e92cd91..992ca8b1f 100644 --- a/src/ipc/smooth_contact/distance/point_face.hpp +++ b/src/ipc/smooth_contact/distance/point_face.hpp @@ -4,21 +4,6 @@ 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, diff --git a/src/ipc/smooth_contact/distance/primitive_distance.cpp b/src/ipc/smooth_contact/distance/primitive_distance.cpp index b27ff7919..28d670a99 100644 --- a/src/ipc/smooth_contact/distance/primitive_distance.cpp +++ b/src/ipc/smooth_contact/distance/primitive_distance.cpp @@ -5,7 +5,6 @@ #include #include #include -#include #include #include diff --git a/src/ipc/smooth_contact/distance/primitive_distance.tpp b/src/ipc/smooth_contact/distance/primitive_distance.tpp index 0fe0f1604..700b91187 100644 --- a/src/ipc/smooth_contact/distance/primitive_distance.tpp +++ b/src/ipc/smooth_contact/distance/primitive_distance.tpp @@ -2,6 +2,9 @@ #include "primitive_distance.hpp" +#include +#include + namespace ipc { template class PrimitiveDistanceTemplate { @@ -16,7 +19,7 @@ public: const Eigen::Vector& x, typename PrimitiveDistType::type dtype) { - return point_triangle_sqr_distance( + return point_triangle_distance( x.template tail<3>() /* point */, x.template head<3>(), x.template segment<3>(3), x.template segment<3>(6) /* face */, dtype); @@ -52,7 +55,7 @@ public: const Eigen::Vector& x, typename PrimitiveDistType::type dtype) { - return edge_edge_sqr_distance( + return edge_edge_distance( x.template head<3>() /* edge 0 */, x.template segment<3>(3) /* edge 0 */, x.template segment<3>(6) /* edge 1 */, diff --git a/src/ipc/tangent/CMakeLists.txt b/src/ipc/tangent/CMakeLists.txt index bdd862fa1..a2d198d00 100644 --- a/src/ipc/tangent/CMakeLists.txt +++ b/src/ipc/tangent/CMakeLists.txt @@ -1,7 +1,6 @@ set(SOURCES closest_point.cpp closest_point.hpp - relative_velocity.cpp relative_velocity.hpp tangent_basis.cpp tangent_basis.hpp diff --git a/src/ipc/tangent/closest_point.cpp b/src/ipc/tangent/closest_point.cpp index 5f20d3191..7919d9b79 100644 --- a/src/ipc/tangent/closest_point.cpp +++ b/src/ipc/tangent/closest_point.cpp @@ -1,4643 +1,4392 @@ #include "closest_point.hpp" -#include -#include - -namespace ipc { - -// ============================================================================ -// Point - Edge - -double point_edge_closest_point( - Eigen::ConstRef p, - Eigen::ConstRef e0, - Eigen::ConstRef e1) +namespace ipc::autogen { +// hess is (6×6) flattened in column-major order +template +void point_edge_closest_point_2D_hessian( + T p_x, T p_y, T e0_x, T e0_y, T e1_x, T e1_y, T hess[36]) { - const VectorMax3d e = e1 - e0; - return (p - e0).dot(e) / e.squaredNorm(); + const T t0 = e0_x - e1_x; + const T t1 = t0 * t0; + const T t2 = e0_y - e1_y; + const T t3 = t2 * t2; + const T t4 = t1 + t3; + const T t5 = T(1.0) / t4; + const T t6 = 2 * t5; + const T t7 = t1 * t6 - 1; + const T t8 = t5 * t7; + const T t9 = t5 * t5; + const T t10 = 2 * t9; + const T t11 = t0 * t2; + const T t12 = t10 * t11; + const T t13 = -t5 * t7; + const T t14 = -t12; + const T t15 = t3 * t6 - 1; + const T t16 = t15 * t5; + const T t17 = -t15 * t5; + const T t18 = -2 * e0_x + e1_x + p_x; + const T t19 = t0 * t18 * t6; + const T t20 = e0_x - p_x; + const T t21 = t0 * t20; + const T t22 = e0_y - p_y; + const T t23 = t2 * t22; + const T t24 = t21 + t23; + const T t25 = t24 * t9; + const T t26 = t24 * t5; + const T t27 = 1 - t26; + const T t28 = -2 * e0_y + e1_y + p_y; + const T t29 = t0 * t28; + const T t30 = 4 * t11 * t26; + const T t31 = t18 * t2 + t30; + const T t32 = t10 * (t29 + t31); + const T t33 = 8 * t25; + const T t34 = -2 * t24 * t5 + 1; + const T t35 = t5 * (2 * t0 * t20 * t5 - t1 * t33 - t19 - t34); + const T t36 = t0 * t22; + const T t37 = t10 * (-t31 + t36); + const T t38 = t2 * t28 * t6; + const T t39 = t2 * t20; + const T t40 = t10 * (-t29 - t30 + t39); + const T t41 = t5 * (2 * t2 * t22 * t5 - t3 * t33 - t34 - t38); + const T 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); } -VectorMax9d point_edge_closest_point_jacobian( - Eigen::ConstRef p, - Eigen::ConstRef e0, - Eigen::ConstRef e1) +// hess is (9×9) flattened in column-major order +template +void point_edge_closest_point_3D_hessian( + T p_x, + T p_y, + T p_z, + T e0_x, + T e0_y, + T e0_z, + T e1_x, + T e1_y, + T e1_z, + T hess[81]) { - const int dim = p.size(); - assert(dim == 2 || dim == 3); - assert(e0.size() == dim && e1.size() == dim); - - const VectorMax3d e = e1 - e0; - const VectorMax3d e2p = p - e0; - const double e_sqnorm = e.squaredNorm(); - - VectorMax9d J(3 * dim); - J.head(dim) = e / e_sqnorm; - J.segment(dim, dim) = (2 / e_sqnorm * e.dot(e2p) * e - e - e2p) / e_sqnorm; - J.tail(dim) = (e2p - 2 / e_sqnorm * e.dot(e2p) * e) / e_sqnorm; - return J; + const T t0 = e0_x - e1_x; + const T t1 = t0 * t0; + const T t2 = e0_y - e1_y; + const T t3 = t2 * t2; + const T t4 = e0_z - e1_z; + const T t5 = t4 * t4; + const T t6 = t1 + t3 + t5; + const T t7 = T(1.0) / t6; + const T t8 = 2 * t7; + const T t9 = t1 * t8 - 1; + const T t10 = t7 * t9; + const T t11 = t7 * t7; + const T t12 = 2 * t11; + const T t13 = t0 * t12; + const T t14 = t13 * t2; + const T t15 = t13 * t4; + const T t16 = -t7 * t9; + const T t17 = -t14; + const T t18 = -t15; + const T t19 = t3 * t8 - 1; + const T t20 = t19 * t7; + const T t21 = t2 * t4; + const T t22 = t12 * t21; + const T t23 = -t19 * t7; + const T t24 = -t22; + const T t25 = t5 * t8 - 1; + const T t26 = t25 * t7; + const T t27 = -t25 * t7; + const T t28 = -2 * e0_x + e1_x + p_x; + const T t29 = t0 * t28 * t8; + const T t30 = e0_x - p_x; + const T t31 = t0 * t30; + const T t32 = e0_y - p_y; + const T t33 = t2 * t32; + const T t34 = e0_z - p_z; + const T t35 = t34 * t4; + const T t36 = t33 + t35; + const T t37 = t31 + t36; + const T t38 = t11 * t37; + const T t39 = t37 * t7; + const T t40 = 1 - t39; + const T t41 = -2 * e0_y + e1_y + p_y; + const T t42 = t0 * t41; + const T t43 = 4 * t39; + const T t44 = t0 * t43; + const T t45 = t2 * t44; + const T t46 = t2 * t28 + t45; + const T t47 = t12 * (t42 + t46); + const T t48 = -2 * e0_z + e1_z + p_z; + const T t49 = t0 * t48; + const T t50 = t4 * t44; + const T t51 = t28 * t4 + t50; + const T t52 = t12 * (t49 + t51); + const T t53 = 8 * t38; + const T t54 = -2 * t37 * t7 + 1; + const T t55 = t7 * (2 * t0 * t30 * t7 - t1 * t53 - t29 - t54); + const T t56 = t0 * t32; + const T t57 = t12 * (-t46 + t56); + const T t58 = t0 * t34; + const T t59 = t12 * (-t51 + t58); + const T t60 = t2 * t41 * t8; + const T t61 = 4 * t38; + const T t62 = t2 * t48; + const T t63 = t21 * t43; + const T t64 = t4 * t41 + t63; + const T t65 = t12 * (t62 + t64); + const T t66 = t2 * t30; + const T t67 = t12 * (-t42 - t45 + t66); + const T t68 = t7 * (2 * t2 * t32 * t7 - t3 * t53 - t54 - t60); + const T t69 = t2 * t34; + const T t70 = t12 * (-t64 + t69); + const T t71 = t4 * t48 * t8; + const T t72 = t30 * t4; + const T t73 = t12 * (-t49 - t50 + t72); + const T t74 = t32 * t4; + const T t75 = t12 * (-t62 - t63 + t74); + const T t76 = t7 * (2 * t34 * t4 * t7 - t5 * t53 - t54 - t71); + const T t77 = t12 * (t45 - t56 - t66); + const T t78 = t12 * (t50 - t58 - t72); + const T 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); } -MatrixMax9d point_edge_closest_point_hessian( - Eigen::ConstRef p, - Eigen::ConstRef e0, - Eigen::ConstRef e1) +// J is (2×12) flattened in column-major order +template +void edge_edge_closest_point_jacobian( + T ea0_x, + T ea0_y, + T ea0_z, + T ea1_x, + T ea1_y, + T ea1_z, + T eb0_x, + T eb0_y, + T eb0_z, + T eb1_x, + T eb1_y, + T eb1_z, + T J[24]) { - 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; + const T t0 = ea0_y * eb0_y; + const T t1 = ea0_x * eb0_x; + const T t2 = 2 * t1; + const T t3 = ea0_y * eb1_y; + const T t4 = ea0_x * eb1_x; + const T t5 = 2 * t4; + const T t6 = ea0_z * eb0_z; + const T t7 = ea0_z * eb1_z; + const T t8 = eb0_y * eb1_y; + const T t9 = 4 * t8; + const T t10 = ea0_x * ea1_x; + const T t11 = eb0_z * eb1_z; + const T t12 = 4 * t11; + const T t13 = ea1_y * eb0_y; + const T t14 = ea1_y * eb1_y; + const T t15 = ea1_z * eb0_z; + const T t16 = ea1_z * eb1_z; + const T t17 = 2 * t0; + const T t18 = 2 * t3; + const T t19 = ea1_x * eb0_x; + const T t20 = ea1_x * eb1_x; + const T t21 = ea0_y * ea1_y; + const T t22 = eb0_x * eb1_x; + const T t23 = 4 * t22; + const T t24 = 2 * t6; + const T t25 = 2 * t7; + const T t26 = ea0_z * ea1_z; + const T t27 = 2 * t19; + const T t28 = 2 * t20; + const T t29 = 2 * t13; + const T t30 = 2 * t14; + const T t31 = ea0_x * ea0_x; + const T t32 = eb0_y * eb0_y; + const T t33 = eb0_z * eb0_z; + const T t34 = eb1_y * eb1_y; + const T t35 = eb1_z * eb1_z; + const T t36 = ea0_y * ea0_y; + const T t37 = eb0_x * eb0_x; + const T t38 = eb1_x * eb1_x; + const T t39 = ea0_z * ea0_z; + const T t40 = ea1_x * ea1_x; + const T t41 = ea1_y * ea1_y; + const T t42 = ea1_z * ea1_z; + const T t43 = 2 * ea0_x; + const T t44 = ea1_x * t32; + const T t45 = ea1_x * t33; + const T t46 = ea1_x * t34; + const T t47 = ea1_x * t35; + const T t48 = 2 * eb0_y; + const T t49 = eb1_y * t31; + const T t50 = 2 * eb0_z; + const T t51 = eb1_z * t31; + const T t52 = 2 * ea0_y; + const T t53 = ea1_y * t37; + const T t54 = ea1_y * t33; + const T t55 = ea1_y * t38; + const T t56 = ea1_y * t35; + const T t57 = 2 * eb0_x; + const T t58 = eb1_x * t36; + const T t59 = eb1_z * t36; + const T t60 = 2 * ea0_z; + const T t61 = ea1_z * t37; + const T t62 = ea1_z * t32; + const T t63 = ea1_z * t38; + const T t64 = ea1_z * t34; + const T t65 = eb1_x * t39; + const T t66 = eb1_y * t39; + const T t67 = eb1_y * t40; + const T t68 = eb1_z * t40; + const T t69 = eb1_x * t41; + const T t70 = eb1_z * t41; + const T t71 = eb1_x * t42; + const T t72 = eb1_y * t42; + const T t73 = T(1.0) + / (-t0 * t2 + t0 * t5 + t10 * t12 + t10 * t9 + t12 * t21 + t13 * t2 + + t13 * t24 - t13 * t25 - t13 * t27 + t13 * t28 - t13 * t5 - t14 * t2 + - t14 * t24 + t14 * t25 + t14 * t27 - t14 * t28 + t14 * t5 + + t15 * t17 - t15 * t18 + t15 * t2 - t15 * t27 + t15 * t28 + - t15 * t29 + t15 * t30 - t15 * t5 - t16 * t17 + t16 * t18 - t16 * t2 + + t16 * t27 - t16 * t28 + t16 * t29 - t16 * t30 + t16 * t5 + + t17 * t19 - t17 * t20 - t17 * t6 + t17 * t7 - t18 * t19 + t18 * t20 + + t18 * t6 - t18 * t7 + t19 * t24 - t19 * t25 + t2 * t3 - t2 * t6 + + t2 * t7 - t20 * t24 + t20 * t25 + t21 * t23 + t23 * t26 + t26 * t9 + - t3 * t5 + t31 * t32 + t31 * t33 + t31 * t34 + t31 * t35 + t32 * t39 + + t32 * t40 + t32 * t42 + t33 * t36 + t33 * t40 + t33 * t41 + + t34 * t39 + t34 * t40 + t34 * t42 + t35 * t36 + t35 * t40 + + t35 * t41 + t36 * t37 + t36 * t38 + t37 * t39 + t37 * t41 + + t37 * t42 + t38 * t39 + t38 * t41 + t38 * t42 - t43 * t44 + - t43 * t45 - t43 * t46 - t43 * t47 - t48 * t49 - t48 * t66 + - t48 * t67 - t48 * t72 + t5 * t6 - t5 * t7 - t50 * t51 - t50 * t59 + - t50 * t68 - t50 * t70 - t52 * t53 - t52 * t54 - t52 * t55 + - t52 * t56 - t57 * t58 - t57 * t65 - t57 * t69 - t57 * t71 + - t60 * t61 - t60 * t62 - t60 * t63 - t60 * t64); + const T t74 = ea1_x + eb0_x - t43; + const T t75 = eb1_x * t57; + const T t76 = eb1_y * t48; + const T t77 = eb1_z * t50; + const T t78 = t32 + t33 + t34 + t35 + t37 + t38 - t75 - t76 - t77; + const T t79 = eb0_x - eb1_x; + const T t80 = ea0_x - eb0_x; + const T t81 = ea0_y - eb0_y; + const T t82 = eb0_y - eb1_y; + const T t83 = ea0_z - eb0_z; + const T t84 = eb0_z - eb1_z; + const T t85 = t79 * t80 + t81 * t82 + t83 * t84; + const T t86 = t79 * t85; + const T t87 = + t0 + t1 - t13 + t14 - t15 + t16 - t19 + t20 - t3 - t4 + t6 - t7; + const T t88 = t80 * (-ea0_x + ea1_x) + t81 * (-ea0_y + ea1_y) + + t83 * (-ea0_z + ea1_z); + const T t89 = 2 * t73; + const T t90 = t89 + * (ea0_x * t32 + ea0_x * t33 + ea0_x * t34 + ea0_x * t35 + ea1_x * t76 + + ea1_x * t77 - eb0_x * t0 + eb0_x * t13 - eb0_x * t14 + eb0_x * t15 + - eb0_x * t16 + eb0_x * t3 - eb0_x * t6 + eb0_x * t7 + eb1_x * t0 + - eb1_x * t13 + eb1_x * t14 - eb1_x * t15 + eb1_x * t16 - eb1_x * t3 + + eb1_x * t6 - eb1_x * t7 - t11 * t43 - t43 * t8 - t44 - t45 - t46 + - t47); + const T t91 = t88 * t90; + const T t92 = t78 * t91; + const T t93 = t85 * t90; + const T t94 = t87 * t93; + const T t95 = ea1_x * t43; + const T t96 = ea1_y * t52; + const T t97 = ea1_z * t60; + const T t98 = t31 + t36 + t39 + t40 + t41 + t42 - t95 - t96 - t97; + const T t99 = ea0_x - ea1_x; + const T t100 = t85 * t99; + const T t101 = 2 * t100; + const T t102 = t79 * t88; + const T t103 = t93 * t98; + const T t104 = t87 * t91; + const T t105 = ea1_y + eb0_y - t52; + const T t106 = -ea0_y * t33 - ea0_y * t35 - ea0_y * t37 - ea0_y * t38 + - ea1_y * t75 - ea1_y * t77 + eb0_y * t1 - eb0_y * t15 + eb0_y * t16 + - eb0_y * t19 + eb0_y * t20 - eb0_y * t4 + eb0_y * t6 - eb0_y * t7 + - eb1_y * t1 + eb1_y * t15 - eb1_y * t16 + eb1_y * t19 - eb1_y * t20 + + eb1_y * t4 - eb1_y * t6 + eb1_y * t7 + t11 * t52 + t22 * t52 + t53 + + t54 + t55 + t56; + const T t107 = t106 * t89; + const T t108 = t107 * t88; + const T t109 = t107 * t85; + const T t110 = t109 * t87 + t82 * t85; + const T t111 = t82 * t88; + const T t112 = ea0_y - ea1_y; + const T t113 = t112 * t85; + const T t114 = t109 * t98 + 2 * t113; + const T t115 = ea1_z + eb0_z - t60; + const T t116 = -ea0_z * t32 - ea0_z * t34 - ea0_z * t37 - ea0_z * t38 + - ea1_z * t75 - ea1_z * t76 + eb0_z * t0 + eb0_z * t1 - eb0_z * t13 + + eb0_z * t14 - eb0_z * t19 + eb0_z * t20 - eb0_z * t3 - eb0_z * t4 + - eb1_z * t0 - eb1_z * t1 + eb1_z * t13 - eb1_z * t14 + eb1_z * t19 + - eb1_z * t20 + eb1_z * t3 + eb1_z * t4 + t22 * t60 + t60 * t8 + t61 + + t62 + t63 + t64; + const T t117 = t116 * t89; + const T t118 = t117 * t88; + const T t119 = t117 * t85; + const T t120 = t119 * t87 + t84 * t85; + const T t121 = t84 * t88; + const T t122 = ea0_z - ea1_z; + const T t123 = t122 * t85; + const T t124 = t119 * t98 + 2 * t123; + const T t125 = t80 * t87; + const T t126 = t112 * t81 + t122 * t83 + t80 * t99; + const T t127 = 2 * t126; + const T t128 = t127 * t73; + const T t129 = t106 * t128; + const T t130 = t81 * t87; + const T t131 = t126 * t82; + const T t132 = t116 * t128; + const T t133 = t83 * t87; + const T t134 = t126 * t84; + const T t135 = ea0_x + eb1_x - t57; + const T t136 = ea0_x * t0 - ea0_x * t13 + ea0_x * t14 - ea0_x * t15 + + ea0_x * t16 - ea0_x * t3 + ea0_x * t6 - ea0_x * t7 - ea1_x * t0 + + ea1_x * t13 - ea1_x * t14 + ea1_x * t15 - ea1_x * t16 + ea1_x * t3 + - ea1_x * t6 + ea1_x * t7 - eb0_x * t36 - eb0_x * t39 - eb0_x * t41 + - eb0_x * t42 + eb0_x * t96 + eb0_x * t97 - eb1_x * t96 - eb1_x * t97 + + t58 + t65 + t69 + t71; + const T t137 = t136 * t89; + const T t138 = t137 * t88; + const T t139 = t137 * t85; + const T t140 = t100 + t139 * t87; + const T t141 = t139 * t98; + const T t142 = ea0_y + eb1_y - t48; + const T t143 = -ea0_y * t1 + ea0_y * t15 - ea0_y * t16 + ea0_y * t19 + - ea0_y * t20 + ea0_y * t4 - ea0_y * t6 + ea0_y * t7 + ea1_y * t1 + - ea1_y * t15 + ea1_y * t16 - ea1_y * t19 + ea1_y * t20 - ea1_y * t4 + + ea1_y * t6 - ea1_y * t7 + eb0_y * t31 + eb0_y * t39 + eb0_y * t40 + + eb0_y * t42 - eb0_y * t95 - eb0_y * t97 + eb1_y * t95 + eb1_y * t97 + - t49 - t66 - t67 - t72; + const T t144 = t143 * t89; + const T t145 = t144 * t88; + const T t146 = t144 * t85; + const T t147 = t146 * t87; + const T t148 = t146 * t98; + const T t149 = ea0_z + eb1_z - t50; + const T t150 = -ea0_z * t0 - ea0_z * t1 + ea0_z * t13 - ea0_z * t14 + + ea0_z * t19 - ea0_z * t20 + ea0_z * t3 + ea0_z * t4 + ea1_z * t0 + + ea1_z * t1 - ea1_z * t13 + ea1_z * t14 - ea1_z * t19 + ea1_z * t20 + - ea1_z * t3 - ea1_z * t4 + eb0_z * t31 + eb0_z * t36 + eb0_z * t40 + + eb0_z * t41 - eb0_z * t95 - eb0_z * t96 + eb1_z * t95 + eb1_z * t96 + - t51 - t59 - t68 - t70; + const T t151 = t150 * t89; + const T t152 = t151 * t88; + const T t153 = t151 * t85; + const T t154 = t153 * t87; + const T t155 = t153 * t98; + const T t156 = t128 * t136; + const T t157 = t128 * t143; + const T t158 = t128 * t150; + J[0] = t73 * (-t74 * t78 - t79 * t87 - t86 + t92 + t94); + J[1] = t73 * (-t101 - t102 + t103 + t104 - t74 * t87 - t79 * t98); + J[2] = -t73 * (t105 * t78 + t108 * t78 + t110 + t82 * t87); + J[3] = -t73 * (t105 * t87 + t108 * t87 + t111 + t114 + t82 * t98); + J[4] = -t73 * (t115 * t78 + t118 * t78 + t120 + t84 * t87); + J[5] = -t73 * (t115 * t87 + t118 * t87 + t121 + t124 + t84 * t98); + J[6] = t73 * (-t78 * t80 + t86 - t92 - t94); + J[7] = t73 * (t101 + t102 - t103 - t104 - t125); + J[8] = t73 * (t110 - t129 * t78 - t78 * t81); + J[9] = t73 * (t114 - t129 * t87 - t130 - t131); + J[10] = t73 * (t120 - t132 * t78 - t78 * t83); + J[11] = t73 * (t124 - t132 * t87 - t133 - t134); + J[12] = -t73 * (2 * t102 + t135 * t87 + t138 * t78 + t140 + t78 * t99); + J[13] = -t73 * (t135 * t98 + t138 * t87 + t141 + t87 * t99 + t88 * t99); + J[14] = + t73 * (-2 * t111 - t112 * t78 - t113 - t142 * t87 + t145 * t78 + t147); + J[15] = t73 * (-t112 * t87 - t112 * t88 - t142 * t98 + t145 * t87 + t148); + J[16] = + t73 * (-2 * t121 - t122 * t78 - t123 - t149 * t87 + t152 * t78 + t154); + J[17] = t73 * (-t122 * t87 - t122 * t88 - t149 * t98 + t152 * t87 + t155); + J[18] = t73 * (t125 - t127 * t79 + t140 - t156 * t78); + J[19] = t73 * (-t126 * t99 + t141 - t156 * t87 + t80 * t98); + J[20] = t73 * (t113 + t130 - 2 * t131 - t147 + t157 * t78); + J[21] = t73 * (-t112 * t126 - t148 + t157 * t87 + t81 * t98); + J[22] = t73 * (t123 + t133 - 2 * t134 - t154 + t158 * t78); + J[23] = t73 * (-t122 * t126 - t155 + t158 * t87 + t83 * t98); } -// ============================================================================ -// Edge - Edge - -Eigen::Vector2d edge_edge_closest_point( - Eigen::ConstRef ea0, - Eigen::ConstRef ea1, - Eigen::ConstRef eb0, - Eigen::ConstRef eb1) +// hess is (144×1) flattened in column-major order +template +void edge_edge_closest_point_hessian_a( + T ea0_x, + T ea0_y, + T ea0_z, + T ea1_x, + T ea1_y, + T ea1_z, + T eb0_x, + T eb0_y, + T eb0_z, + T eb1_x, + T eb1_y, + T eb1_z, + T hess[144]) { - const Eigen::Vector3d eb_to_ea = ea0 - eb0; - const Eigen::Vector3d ea = ea1 - ea0; - const Eigen::Vector3d eb = eb1 - eb0; - - Eigen::Matrix2d A; - A(0, 0) = ea.squaredNorm(); - A(0, 1) = A(1, 0) = -eb.dot(ea); - A(1, 1) = eb.squaredNorm(); - - Eigen::Vector2d rhs; - rhs[0] = -eb_to_ea.dot(ea); - rhs[1] = eb_to_ea.dot(eb); - - const Eigen::Vector2d x = A.ldlt().solve(rhs); - assert((A * x - rhs).norm() < 1e-10); - return x; + const T t0 = ea0_y * eb0_y; + const T t1 = ea0_x * eb0_x; + const T t2 = 2 * t1; + const T t3 = ea0_y * eb1_y; + const T t4 = ea0_x * eb1_x; + const T t5 = 2 * t4; + const T t6 = ea0_z * eb0_z; + const T t7 = ea0_z * eb1_z; + const T t8 = ea0_x * eb0_y; + const T t9 = ea1_x * eb1_y; + const T t10 = ea0_x * eb0_z; + const T t11 = ea1_x * eb1_z; + const T t12 = ea1_y * eb0_y; + const T t13 = ea1_y * eb1_y; + const T t14 = ea1_z * eb0_z; + const T t15 = ea1_z * eb1_z; + const T t16 = 2 * t0; + const T t17 = 2 * t3; + const T t18 = ea1_x * eb0_x; + const T t19 = ea1_x * eb1_x; + const T t20 = ea0_y * eb1_x; + const T t21 = ea1_y * eb0_x; + const T t22 = ea0_y * eb0_z; + const T t23 = ea1_y * eb1_z; + const T t24 = 2 * t6; + const T t25 = 2 * t7; + const T t26 = ea0_z * eb1_x; + const T t27 = ea1_z * eb0_x; + const T t28 = ea0_z * eb1_y; + const T t29 = ea1_z * eb0_y; + const T t30 = 2 * t18; + const T t31 = 2 * t19; + const T t32 = 2 * t12; + const T t33 = 2 * t13; + const T t34 = (ea0_x * ea0_x); + const T t35 = (eb0_y * eb0_y); + const T t36 = (eb0_z * eb0_z); + const T t37 = (eb1_y * eb1_y); + const T t38 = (eb1_z * eb1_z); + const T t39 = (ea0_y * ea0_y); + const T t40 = (eb0_x * eb0_x); + const T t41 = (eb1_x * eb1_x); + const T t42 = (ea0_z * ea0_z); + const T t43 = (ea1_x * ea1_x); + const T t44 = (ea1_y * ea1_y); + const T t45 = (ea1_z * ea1_z); + const T t46 = ea0_x * t35; + const T t47 = 2 * ea1_x; + const T t48 = ea0_x * t36; + const T t49 = ea0_x * t37; + const T t50 = ea0_x * t38; + const T t51 = 2 * eb0_y; + const T t52 = eb1_y * t34; + const T t53 = 2 * eb0_z; + const T t54 = eb1_z * t34; + const T t55 = 2 * ea0_y; + const T t56 = ea1_y * t40; + const T t57 = ea1_y * t36; + const T t58 = ea1_y * t41; + const T t59 = ea1_y * t38; + const T t60 = 2 * eb0_x; + const T t61 = eb1_x * t39; + const T t62 = eb1_z * t39; + const T t63 = 2 * ea0_z; + const T t64 = ea1_z * t40; + const T t65 = ea1_z * t35; + const T t66 = ea1_z * t41; + const T t67 = ea1_z * t37; + const T t68 = eb1_x * t42; + const T t69 = eb1_y * t42; + const T t70 = eb1_y * t43; + const T t71 = eb1_z * t43; + const T t72 = eb1_x * t44; + const T t73 = eb1_z * t44; + const T t74 = eb1_x * t45; + const T t75 = eb1_y * t45; + const T 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 T t77 = T(1.0) / t76; + const T t78 = eb1_z * t53; + const T t79 = t36 + t38 - t78; + const T t80 = eb1_y * t51; + const T t81 = t35 + t37 - t80; + const T t82 = t79 + t81; + const T t83 = ea0_x - eb0_x; + const T t84 = eb0_x - eb1_x; + const T t85 = t83 * t84; + const T t86 = ea0_y - eb0_y; + const T t87 = eb0_y - eb1_y; + const T t88 = t86 * t87; + const T t89 = ea0_z - eb0_z; + const T t90 = eb0_z - eb1_z; + const T t91 = t89 * t90; + const T t92 = t88 + t91; + const T t93 = t85 + t92; + const T t94 = -t14 + t15 + t6 - t7; + const T t95 = t0 - t12 + t13 - t3; + const T t96 = t94 + t95; + const T t97 = t1 - t18 + t19 - t4; + const T t98 = t96 + t97; + const T t99 = t93 * t98; + const T t100 = t82 * t99; + const T t101 = ea0_x - ea1_x; + const T t102 = t101 * t83; + const T t103 = ea0_y - ea1_y; + const T t104 = t103 * t86; + const T t105 = ea0_z - ea1_z; + const T t106 = t105 * t89; + const T t107 = t102 + t104 + t106; + const T t108 = eb1_x * t60; + const T t109 = -t108 + t40 + t41; + const T t110 = t109 + t82; + const T t111 = 1 / (t76 * t76); + const T t112 = 2 * ea0_x; + const T t113 = eb1_y * t112; + const T t114 = eb1_z * t112; + const T 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 T t116 = (t115 * t115); + const T t117 = t111 * t116; + const T t118 = 4 * t117; + const T t119 = 2 * t77; + const T t120 = t115 * t93; + const T t121 = t120 * t84; + const T t122 = t107 * t110; + const T t123 = t115 * t84; + const T t124 = t119 * t98; + const T t125 = t110 * t115; + const T t126 = ea1_x + eb0_x - t112; + const T t127 = t119 * t126; + const T t128 = t110 + t123 * t124 + t125 * t127 - (t84 * t84); + const T t129 = t84 * t87; + const T t130 = + eb0_x * eb0_y - eb0_x * eb1_y - eb0_y * eb1_x + eb1_x * eb1_y; + const T t131 = -t101 * t83 - t103 * t86 - t105 * t89; + const T t132 = t110 * t131; + const T t133 = t132 * t77; + const T t134 = t130 * t99; + const T t135 = eb0_x * t55; + const T t136 = eb1_z * t55; + const T t137 = ea1_y * eb1_x; + const T 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 T t139 = t110 * t138; + const T t140 = t126 * t77; + const T t141 = ea1_y + eb0_y - t55; + const T t142 = t84 * t93; + const T t143 = t138 * t142; + const T t144 = t77 * t98; + const T t145 = t144 * t84; + const T 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 T t147 = t84 * t90; + const T t148 = + eb0_x * eb0_z - eb0_x * eb1_z - eb0_z * eb1_x + eb1_x * eb1_z; + const T t149 = t148 * t99; + const T t150 = eb0_x * t63; + const T t151 = eb0_y * t63; + const T t152 = ea1_z * eb1_x; + const T t153 = ea1_z * eb1_y; + const T 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 T t155 = t110 * t154; + const T t156 = ea1_z + eb0_z - t63; + const T t157 = t142 * t154; + const T 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 T t159 = 4 * t77; + const T 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 T t161 = t122 * t130; + const T t162 = t125 * t86; + const T t163 = t120 * t87; + const T t164 = t119 * t163; + const T t165 = t124 * t84; + const T t166 = 8 * t111; + const T t167 = t138 * t166; + const T t168 = t115 * t122; + const T t169 = t115 * t99; + const T t170 = t167 * t169; + const T t171 = t77 + * (t119 * t134 + t119 * t143 - t119 * t161 + t119 * t162 + t127 * t139 + + t129 + t138 * t165 - t164 + t167 * t168 - t170); + const T t172 = t122 * t148; + const T t173 = t125 * t89; + const T t174 = t120 * t90; + const T t175 = t119 * t174; + const T t176 = t154 * t166; + const T t177 = t169 * t176; + const T t178 = t77 + * (t119 * t149 + t119 * t157 - t119 * t172 + t119 * t173 + t127 * t155 + + t147 + t154 * t165 + t168 * t176 - t175 - t177); + const T t179 = ea0_x + eb1_x - t60; + const T t180 = t131 * t159; + const T t181 = t101 * t119; + const T t182 = t115 * t124; + const T t183 = t119 * t96; + const T t184 = t125 * t131; + const T 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 T t186 = 8 * t185; + const T t187 = t111 * t186; + const T t188 = t110 - t123 * t180 - t125 * t181 + t132 * t183 - t179 * t182 + + t179 * t84 - t184 * t187; + const T t189 = t101 * t84; + const T t190 = 2 * t126; + const T t191 = t110 * t127; + const T t192 = t119 * t142; + const T t193 = -t120 * t181 - t169 * t187 + t183 * t99 + t185 * t192; + const T t194 = t165 * t185 + t185 * t191 + t189 + t190 * t84 + t193 + t98; + const T t195 = t77 * (-t188 - t194 - t93); + const T t196 = -ea0_y * eb0_x - ea1_x * t51 + eb0_y * t112 - t113 - t137 + + t20 + t21 + 2 * t9; + const T t197 = t119 * t196; + const T t198 = t115 * t180; + const T t199 = ea1_x * eb0_y; + const T 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 T t201 = t166 * t200; + const T t202 = t132 * t197 - t184 * t201 + t198 * t87; + const T t203 = t103 * t119; + const T t204 = t120 * t203; + const T t205 = t192 * t200; + const T t206 = t197 * t99; + const T t207 = -t169 * t201; + const T t208 = ea0_y + eb1_y - t51; + const T t209 = + t125 * t203 + t182 * t208 + t204 + t205 + t206 + t207 - t208 * t84; + const T t210 = t103 * t84; + const T t211 = t165 * t200 - t190 * t87 + t191 * t200 - t210; + const T t212 = t77 * (t202 + t209 + t211); + const T t213 = -ea0_z * eb0_x - ea1_x * t53 + eb0_z * t112 + 2 * t11 - t114 + - t152 + t26 + t27; + const T t214 = t119 * t213; + const T t215 = ea1_x * eb0_z; + const T t216 = ea1_y * eb0_z; + const T 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 T t218 = t166 * t217; + const T t219 = t132 * t214 - t184 * t218 + t198 * t90; + const T t220 = t105 * t119; + const T t221 = t120 * t220; + const T t222 = t192 * t217; + const T t223 = t214 * t99; + const T t224 = -t169 * t218; + const T t225 = ea0_z + eb1_z - t53; + const T t226 = + t125 * t220 + t182 * t225 + t221 + t222 + t223 + t224 - t225 * t84; + const T t227 = t105 * t84; + const T t228 = t165 * t217 - t190 * t90 + t191 * t217 - t227; + const T t229 = t77 * (t219 + t226 + t228); + const T t230 = t107 * t159; + const T t231 = -t122 * t183 + t123 * t230 + t168 * t187 - t182 * t83; + const T t232 = t77 * (t194 + t231 + 2 * t85 + t92); + const T t233 = t84 * t86; + const T t234 = t182 * t86 + t204 + t205 + t206 + t207 - t233; + const T t235 = t77 + * (2 * t107 * t110 * t196 * t77 + 4 * t107 * t115 * t77 * t87 + - t168 * t201 - t211 - t234); + const T t236 = t84 * t89; + const T t237 = t182 * t89 + t221 + t222 + t223 + t224 - t236; + const T t238 = t77 + * (2 * t107 * t110 * t213 * t77 + 4 * t107 * t115 * t77 * t90 + - t168 * t218 - t228 - t237); + const T t239 = (t87 * t87); + const T t240 = t109 + t79; + const T t241 = t138 * t87; + const T t242 = t124 * t241; + const T t243 = t119 * t141; + const T t244 = t139 * t243; + const T t245 = (t138 * t138); + const T t246 = 4 * t111 * t245; + const T t247 = t108 - t35 - t36 - t37 - t38 - t40 - t41 + t78 + t80; + const T t248 = t87 * t90; + const T t249 = + eb0_y * eb0_z - eb0_y * eb1_z - eb0_z * eb1_y + eb1_y * eb1_z; + const T t250 = t249 * t99; + const T t251 = t141 * t155; + const T t252 = t139 * t156; + const T t253 = t154 * t87; + const T t254 = t253 * t93; + const T t255 = t138 * t90; + const T t256 = t255 * t93; + const T t257 = 4 * t111 * t138; + const T t258 = t131 * t155; + const T t259 = t154 * t99; + const T t260 = -t119 + * (t133 * t249 + t144 * t253 + t144 * t255 + t248 + t250 * t77 + + t251 * t77 + t252 * t77 + t254 * t77 + t256 * t77 + t257 * t258 + + t257 * t259); + const T t261 = t110 * t83; + const T t262 = t138 * t261; + const T 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 T t264 = t139 * t86; + const T t265 = t119 * t132; + const T t266 = t240 * t99; + const T 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 T t268 = t139 * t89; + const T t269 = t119 * t250 + t119 * t254 + t119 * t256 + t167 * t258 + + t167 * t259 + t248 + t249 * t265; + const T t270 = t77 * (t119 * t251 - t119 * t268 + t124 * t253 + t269); + const T t271 = t124 * t138; + const T t272 = -ea0_x * eb1_y + ea1_y * t60 + eb1_x * t55 - t135 - 2 * t137 + - t199 + t8 + t9; + const T t273 = t138 * t84; + const T t274 = t131 * t139; + const T t275 = t139 * t181 + t179 * t271 + t179 * t87 + t180 * t273 + + t187 * t274 + t265 * t272; + const T t276 = t101 * t87; + const T t277 = 2 * t141; + const T t278 = t185 * t87; + const T t279 = t110 * t243; + const T t280 = t138 * t93; + const T t281 = t119 * t93; + const T t282 = t119 * t99; + const T t283 = t138 * t187; + const T t284 = t181 * t280 + t272 * t282 + t278 * t281 + t283 * t99; + const T t285 = t124 * t278 + t185 * t279 + t276 + t277 * t84 + t284; + const T t286 = -t77 * (t275 + t285); + const T t287 = t94 + t97; + const T t288 = + -8 * t110 * t111 * t131 * t138 * t200 + t180 * t241 + t265 * t287; + const T t289 = t110 + t139 * t203 + t208 * t271 + t208 * t87; + const T t290 = t200 * t87; + const T t291 = -t281 * t290; + const T t292 = t203 * t280; + const T t293 = t282 * t287; + const T t294 = t167 * t99; + const T t295 = -t200 * t294; + const T t296 = t103 * t87; + const T t297 = -t124 * t290 - t200 * t279 + t277 * t87 + t291 + t292 + t293 + + t295 + t296; + const T t298 = t93 + t98; + const T t299 = t77 * (-t288 - t289 - t297 - t298); + const T t300 = t217 * t87; + const T t301 = t281 * t300; + const T t302 = -t301; + const T t303 = -ea0_z * eb0_y - ea1_y * t53 + eb0_z * t55 - t136 - t153 + + 2 * t23 + t28 + t29; + const T t304 = t282 * t303; + const T t305 = -t304; + const T t306 = t220 * t280; + const T t307 = t217 * t294; + const T t308 = -t307; + const T t309 = t180 * t255; + const T t310 = t139 * t220 + t225 * t271 + t225 * t87; + const T t311 = t105 * t87; + const T t312 = -t124 * t300 - t217 * t279 + t277 * t90 + t311; + const T t313 = t77 + * (8 * t110 * t111 * t131 * t138 * t217 + 2 * t110 * t131 * t303 * t77 + - t302 - t305 - t306 - t308 - t309 - t310 - t312); + const T t314 = t83 * t87; + const T t315 = t119 * t122; + const T t316 = -t122 * t283 - t230 * t273 + t271 * t83 - t272 * t315 + t314; + const T t317 = t77 * (t285 + t316); + const T t318 = t271 * t86; + const T t319 = t122 * t167; + const T t320 = t200 * t319 - t230 * t241 - t287 * t315; + const T t321 = t85 + t98; + const T t322 = t77 * (t297 + t318 + t320 + t321 + 2 * t88 + t91); + const T t323 = t87 * t89; + const T t324 = t271 * t89; + const T t325 = + t217 * t319 - t230 * t255 + t302 + t303 * t315 + t305 + t306 + t308; + const T t326 = t77 * (t312 + t323 + t324 + t325); + const T t327 = (t90 * t90); + const T t328 = t109 + t81; + const T t329 = t154 * t90; + const T t330 = t124 * t329; + const T t331 = t119 * t156; + const T t332 = t155 * t331; + const T t333 = (t154 * t154); + const T t334 = 4 * t111 * t333; + const T t335 = t154 * t261; + const T 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 T t337 = t155 * t86; + const T t338 = t77 * (t119 * t252 - t119 * t337 + t124 * t255 + t269); + const T t339 = t155 * t89; + const T t340 = t328 * t99; + const T 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 T t342 = t124 * t154; + const T t343 = -ea0_x * eb1_z + ea1_z * t60 + eb1_x * t63 + t10 + t11 - t150 + - 2 * t152 - t215; + const T t344 = t154 * t84; + const T t345 = t155 * t181 + t179 * t342 + t179 * t90 + t180 * t344 + + t187 * t258 + t265 * t343; + const T t346 = t101 * t90; + const T t347 = 2 * t156; + const T t348 = t185 * t90; + const T t349 = t110 * t331; + const T t350 = t154 * t93; + const T t351 = t181 * t350 + t187 * t259 + t281 * t348 + t282 * t343; + const T t352 = t124 * t348 + t185 * t349 + t346 + t347 * t84 + t351; + const T t353 = -t77 * (t345 + t352); + const T t354 = -ea0_y * eb1_z + ea1_z * t51 + eb1_y * t63 - t151 - 2 * t153 + - t216 + t22 + t23; + const T t355 = + -8 * t110 * t111 * t131 * t154 * t200 + t180 * t253 + t265 * t354; + const T t356 = t155 * t203 + t208 * t342 + t208 * t90; + const T t357 = t200 * t90; + const T t358 = -t281 * t357; + const T t359 = t203 * t350; + const T t360 = t282 * t354; + const T t361 = t176 * t99; + const T t362 = -t200 * t361; + const T t363 = t103 * t90; + const T t364 = -t124 * t357 - t200 * t349 + t347 * t87 + t358 + t359 + t360 + + t362 + t363; + const T t365 = t77 * (-t355 - t356 - t364); + const T t366 = t95 + t97; + const T t367 = + -8 * t110 * t111 * t131 * t154 * t217 + t180 * t329 + t265 * t366; + const T t368 = t110 + t155 * t220 + t225 * t342 + t225 * t90; + const T t369 = t217 * t90; + const T t370 = -t281 * t369; + const T t371 = t220 * t350; + const T t372 = t282 * t366; + const T t373 = -t217 * t361; + const T t374 = t105 * t90; + const T t375 = -t124 * t369 - t217 * t349 + t347 * t90 + t370 + t371 + t372 + + t373 + t374; + const T t376 = t77 * (-t298 - t367 - t368 - t375); + const T t377 = t83 * t90; + const T t378 = t122 * t187; + const T t379 = -t154 * t378 - t230 * t344 - t315 * t343 + t342 * t83 + t377; + const T t380 = t77 * (t352 + t379); + const T t381 = t86 * t90; + const T t382 = t342 * t86 + t381; + const T t383 = t122 * t176; + const T t384 = t200 * t383 - t230 * t253 - t315 * t354; + const T t385 = t77 * (t364 + t382 + t384); + const T t386 = t342 * t89; + const T t387 = t217 * t383 - t230 * t329 - t315 * t366; + const T t388 = t77 * (t321 + t375 + t386 + t387 + t88 + 2 * t91); + const T t389 = 4 * t116; + const T t390 = t77 * t99; + const T t391 = 2 * t111; + const T t392 = t122 * t159; + const T t393 = t115 * t138; + const T t394 = 4 * t390; + const T t395 = t391 + * (-t134 - t143 + t161 - t162 + t163 + t262 - t392 * t393 + + t393 * t394); + const T t396 = t115 * t154; + const T t397 = t391 + * (-t149 - t157 + t172 - t173 + t174 + t335 - t392 * t396 + + t394 * t396); + const T t398 = t119 * t261; + const T t399 = -t185 * t398 + t193 + t92; + const T t400 = t77 * (t188 + t399 - t85); + const T t401 = -t200 * t398 + t202 + 2 * t314; + const T t402 = t77 * (-t209 - t401); + const T t403 = -t217 * t398 + t219 + 2 * t377; + const T t404 = t77 * (-t226 - t403); + const T t405 = t77 * (-t231 - t399); + const T t406 = t77 * (t234 + t401); + const T t407 = t77 * (t237 + t403); + const T t408 = 2 * t93; + const T t409 = t138 * t154; + const T t410 = t391 + * (t122 * t249 - t250 - t254 - t256 + t268 + t337 + t392 * t409 + - t394 * t409); + const T t411 = t110 * t119; + const T t412 = t185 * t86; + const T t413 = -2 * t233 + t284 - t411 * t412; + const T t414 = t77 * (t275 + t413); + const T t415 = t411 * t86; + const T t416 = t200 * t415 + t291 + t292 + t293 + t295 + t85 + t91; + const T t417 = t77 * (t289 + t320 + t416 - t88); + const T t418 = 2 * t381; + const T t419 = t217 * t415; + const T t420 = t77 * (t310 + t325 - t418 + t419); + const T t421 = t77 * (-t316 - t413); + const T t422 = t77 * (-t288 - t318 - t416); + const T t423 = t77 + * (t218 * t274 + t265 * t303 + t301 + t304 - t306 + t307 - t309 - t323 + - t324 + t418 - t419); + const T t424 = t411 * t89; + const T t425 = -t185 * t424 - 2 * t236 + t351; + const T t426 = t77 * (t345 + t425); + const T t427 = t200 * t424 - 2 * t323 + t358 + t359 + t360 + t362; + const T t428 = t77 * (t356 + t384 + t427); + const T t429 = t217 * t424 + t370 + t371 + t372 + t373 + t85 + t88; + const T t430 = t77 * (t368 + t387 + t429 - t91); + const T t431 = t77 * (-t379 - t425); + const T t432 = t77 * (-t355 - t382 - t427); + const T t433 = t77 * (-t367 - t386 - t429); + const T t434 = -ea1_z * t63 + t42 + t45; + const T t435 = -ea1_y * t55 + t39 + t44; + const T t436 = t434 + t435; + const T t437 = t181 * t185; + const T t438 = t111 * (t185 * t185); + const T t439 = 4 * t438; + const T t440 = -t110 * t131 * t436 * t77 + t131 + t132 * t439 + + t180 * t185 * t84 - t436 * t77 * t93 * t98 + t437 * t93 + t439 * t99; + const T t441 = t124 * t185; + const T t442 = + -t0 - t1 + t12 - t13 + t14 - t15 + t18 - t19 + t3 + t4 - t6 + t7; + const T t443 = t101 * t179 + t110 * t437 + t179 * t441 + 2 * t189 + t442; + const T t444 = + ea0_x * ea0_y - ea0_x * ea1_y - ea0_y * ea1_x + ea1_x * ea1_y; + const T t445 = -8 * t110 * t111 * t131 * t185 * t200 + - 4 * t131 * t200 * t77 * t84 + t180 * t278 + t265 * t444; + const T t446 = t181 * t200; + const T t447 = t124 * t179; + const T t448 = t103 * t179 - t110 * t446 - t200 * t447 + 2 * t276; + const T t449 = -t446 * t93; + const T t450 = t185 * t203; + const T t451 = t450 * t93; + const T t452 = t282 * t444; + const T t453 = -t187 * t200 * t99; + const T t454 = t101 * t208 + t110 * t450 + t208 * t441 + 2 * t210 + t449 + + t451 + t452 + t453; + const T t455 = t77 * (-t445 - t448 - t454); + const T t456 = + ea0_x * ea0_z - ea0_x * ea1_z - ea0_z * ea1_x + ea1_x * ea1_z; + const T t457 = -8 * t110 * t111 * t131 * t185 * t217 + - 4 * t131 * t217 * t77 * t84 + t180 * t348 + t265 * t456; + const T t458 = t181 * t217; + const T t459 = t105 * t179 - t110 * t458 - t217 * t447 + 2 * t346; + const T t460 = -t458 * t93; + const T t461 = t185 * t220; + const T t462 = t461 * t93; + const T t463 = t282 * t456; + const T t464 = -t187 * t217 * t99; + const T t465 = t101 * t225 + t110 * t461 + t225 * t441 + 2 * t227 + t460 + + t462 + t463 + t464; + const T t466 = t77 * (-t457 - t459 - t465); + const T t467 = t441 * t83; + const T t468 = 2 * t104; + const T t469 = 2 * t106; + const T t470 = 8 * t438; + const T t471 = t159 * t93; + const T t472 = t77 + * (t101 * t185 * t471 - t102 - t107 * t186 * t77 * t84 - t122 * t470 + - t282 * t436 + t315 * t436 + t443 + t467 - t468 - t469 + + t470 * t99); + const T t473 = t101 * t86 + t124 * t412 + t449 + t451 + t452 + t453; + const T t474 = t230 * t84; + const T t475 = t200 * t378 + t200 * t474 - t230 * t278 - t315 * t444; + const T t476 = t77 * (t448 + t473 + t475); + const T t477 = t101 * t89 + t441 * t89 + t460 + t462 + t463 + t464; + const T t478 = t217 * t378 + t217 * t474 - t230 * t348 - t315 * t456; + const T t479 = t77 * (t459 + t477 + t478); + const T t480 = t103 * t208; + const T t481 = 2 * t296; + const T t482 = t110 * t200 * t203; + const T t483 = t124 * t208; + const T t484 = t200 * t483; + const T t485 = -ea1_x * t112 + t34 + t43; + const T t486 = t434 + t485; + const T t487 = (t200 * t200); + const T t488 = t111 * t487; + const T t489 = 4 * t488; + const T 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 T t491 = + ea0_y * ea0_z - ea0_y * ea1_z - ea0_z * ea1_y + ea1_y * ea1_z; + const T t492 = t201 * t217; + const T t493 = -4 * t131 * t200 * t77 * t90 - 4 * t131 * t217 * t77 * t87 + + t132 * t492 + t265 * t491; + const T t494 = t203 * t217; + const T t495 = t105 * t208 - t110 * t494 - t217 * t483 + 2 * t363; + const T t496 = -t494 * t93; + const T t497 = t200 * t220; + const T t498 = -t497 * t93; + const T t499 = t282 * t491; + const T t500 = t492 * t99; + const T t501 = t124 * t200; + const T t502 = t103 * t225 - t110 * t497 - t225 * t501 + 2 * t311 + t496 + + t498 + t499 + t500; + const T t503 = t77 * (-t493 - t495 - t502); + const T t504 = t103 * t83 - t501 * t83; + const T t505 = t77 * (t454 + t475 + t504); + const T t506 = t501 * t86; + const T t507 = 2 * t102 + t98; + const T 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 T t509 = t103 * t89 + t496 + t498 + t499 + t500 - t501 * t89; + const T t510 = -t122 * t492 + t230 * t300 + t230 * t357 - t315 * t491; + const T t511 = t77 * (t495 + t509 + t510); + const T t512 = t105 * t225; + const T t513 = 2 * t374; + const T t514 = t110 * t217 * t220; + const T t515 = t124 * t217; + const T t516 = t225 * t515; + const T t517 = t435 + t485; + const T t518 = (t217 * t217); + const T t519 = t111 * t518; + const T t520 = 4 * t519; + const T 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 T t522 = t105 * t83 - t515 * t83; + const T t523 = t77 * (t465 + t478 + t522); + const T t524 = t105 * t86 - t515 * t86; + const T t525 = t77 * (t502 + t510 + t524); + const T t526 = t515 * t89; + const T 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 T t528 = t77 * (-t445 - t473 - t504); + const T t529 = t77 * (-t457 - t477 - t522); + const T 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); } -Eigen::Matrix edge_edge_closest_point_jacobian( - Eigen::ConstRef ea0, - Eigen::ConstRef ea1, - Eigen::ConstRef eb0, - Eigen::ConstRef eb1) +// hess is (144×1) flattened in column-major order +template +void edge_edge_closest_point_hessian_b( + T ea0_x, + T ea0_y, + T ea0_z, + T ea1_x, + T ea1_y, + T ea1_z, + T eb0_x, + T eb0_y, + T eb0_z, + T eb1_x, + T eb1_y, + T eb1_z, + T hess[144]) { - Eigen::Matrix J; - - autogen::edge_edge_closest_point_jacobian( - ea0[0], ea0[1], ea0[2], ea1[0], ea1[1], ea1[2], eb0[0], eb0[1], eb0[2], - eb1[0], eb1[1], eb1[2], J.data()); - - return J; + const T t0 = ea0_z * eb0_z; + const T t1 = ea1_z * eb1_z; + const T t2 = ea0_z * eb1_z; + const T t3 = ea1_z * eb0_z; + const T t4 = t0 + t1 - t2 - t3; + const T t5 = ea0_y * eb0_y; + const T t6 = ea1_y * eb1_y; + const T t7 = ea0_y * eb1_y; + const T t8 = ea1_y * eb0_y; + const T t9 = t5 + t6 - t7 - t8; + const T t10 = t4 + t9; + const T t11 = ea0_x * eb0_x; + const T t12 = ea1_x * eb1_x; + const T t13 = ea0_x * eb1_x; + const T t14 = ea1_x * eb0_x; + const T t15 = t11 + t12 - t13 - t14; + const T t16 = t10 + t15; + const T t17 = ea0_x - ea1_x; + const T t18 = ea0_x - eb0_x; + const T t19 = t17 * t18; + const T t20 = ea0_y - ea1_y; + const T t21 = ea0_y - eb0_y; + const T t22 = t20 * t21; + const T t23 = ea0_z - ea1_z; + const T t24 = ea0_z - eb0_z; + const T t25 = t23 * t24; + const T t26 = t22 + t25; + const T t27 = t19 + t26; + const T t28 = t16 * t27; + const T t29 = 2 * t11; + const T t30 = 2 * t13; + const T t31 = ea0_x * eb0_y; + const T t32 = ea1_x * eb1_y; + const T t33 = ea0_x * eb0_z; + const T t34 = ea1_x * eb1_z; + const T t35 = 2 * t5; + const T t36 = 2 * t7; + const T t37 = ea0_y * eb1_x; + const T t38 = ea1_y * eb0_x; + const T t39 = ea0_y * eb0_z; + const T t40 = ea1_y * eb1_z; + const T t41 = 2 * t0; + const T t42 = 2 * t2; + const T t43 = ea0_z * eb1_x; + const T t44 = ea1_z * eb0_x; + const T t45 = ea0_z * eb1_y; + const T t46 = ea1_z * eb0_y; + const T t47 = 2 * t14; + const T t48 = 2 * t12; + const T t49 = 2 * t8; + const T t50 = 2 * t6; + const T t51 = (ea0_x * ea0_x); + const T t52 = (eb0_y * eb0_y); + const T t53 = (eb0_z * eb0_z); + const T t54 = (eb1_y * eb1_y); + const T t55 = (eb1_z * eb1_z); + const T t56 = (ea0_y * ea0_y); + const T t57 = (eb0_x * eb0_x); + const T t58 = (eb1_x * eb1_x); + const T t59 = (ea0_z * ea0_z); + const T t60 = (ea1_x * ea1_x); + const T t61 = (ea1_y * ea1_y); + const T t62 = (ea1_z * ea1_z); + const T t63 = 2 * ea0_x; + const T t64 = ea1_x * t52; + const T t65 = ea1_x * t53; + const T t66 = ea1_x * t54; + const T t67 = ea1_x * t55; + const T t68 = 2 * eb0_y; + const T t69 = eb1_y * t51; + const T t70 = 2 * eb0_z; + const T t71 = eb1_z * t51; + const T t72 = 2 * ea0_y; + const T t73 = ea1_y * t57; + const T t74 = ea1_y * t53; + const T t75 = ea1_y * t58; + const T t76 = ea1_y * t55; + const T t77 = 2 * eb0_x; + const T t78 = eb1_x * t56; + const T t79 = eb1_z * t56; + const T t80 = 2 * ea0_z; + const T t81 = ea1_z * t57; + const T t82 = ea1_z * t52; + const T t83 = ea1_z * t58; + const T t84 = ea1_z * t54; + const T t85 = eb1_x * t59; + const T t86 = eb1_y * t59; + const T t87 = eb1_y * t60; + const T t88 = eb1_z * t60; + const T t89 = eb1_x * t61; + const T t90 = eb1_z * t61; + const T t91 = eb1_x * t62; + const T t92 = eb1_y * t62; + const T 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 T t94 = T(1.0) / t93; + const T t95 = -eb1_z * t70 + t53 + t55; + const T t96 = -eb1_y * t68 + t52 + t54; + const T t97 = t95 + t96; + const T t98 = ea1_z * t80; + const T t99 = t59 + t62 - t98; + const T t100 = ea1_y * t72; + const T t101 = -t100 + t56 + t61; + const T t102 = t101 + t99; + const T t103 = ea1_x * t63; + const T t104 = -t103 + t51 + t60; + const T t105 = t102 + t104; + const T t106 = eb0_x - eb1_x; + const T t107 = t106 * t18; + const T t108 = eb0_y - eb1_y; + const T t109 = t108 * t21; + const T t110 = eb0_z - eb1_z; + const T t111 = t110 * t24; + const T t112 = t109 + t111; + const T t113 = t107 + t112; + const T t114 = eb1_y * t63; + const T t115 = eb1_z * t63; + const T 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 T t117 = (t116 * t116); + const T t118 = 1 / (t93 * t93); + const T t119 = 2 * t94; + const T t120 = t116 * t119; + const T t121 = t106 * t120; + const T t122 = t117 * t118; + const T t123 = t105 * t113; + const T t124 = 4 * t123; + const T t125 = -t105 * t113 * t94 * t97 - 4 * t113 * t116 * t17 * t94 + - 4 * t117 * t118 * t16 * t27 + t121 * t27 + t122 * t124 + + t28 * t94 * t97; + const T t126 = + -t0 - t1 - t11 - t12 + t13 + t14 + t2 + t3 - t5 - t6 + t7 + t8; + const T t127 = t113 + t126; + const T t128 = ea1_x + eb0_x - t63; + const T t129 = t106 * t17; + const T t130 = t128 * t16; + const T t131 = -t105 * t121 + t106 * t128 - t120 * t130 + 2 * t129; + const T t132 = ea1_y + eb0_y - t72; + const T t133 = t106 * t132; + const T t134 = t108 * t17; + const T t135 = 2 * t134; + const T t136 = -t17 * t18 - t20 * t21 - t23 * t24; + const T t137 = t108 * t120; + const T t138 = t136 * t137; + const T t139 = t105 * t137; + const T t140 = t120 * t16; + const T t141 = t132 * t140; + const T t142 = eb0_x * t72; + const T t143 = eb1_z * t72; + const T t144 = ea1_y * eb1_x; + const T 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 T t146 = t119 * t145; + const T t147 = t106 * t146; + const T t148 = t136 * t147; + const T t149 = t136 * t16; + const T t150 = + t119 * (eb0_x * eb0_y - eb0_x * eb1_y - eb0_y * eb1_x + eb1_x * eb1_y); + const T t151 = t149 * t150; + const T t152 = 8 * t116; + const T t153 = t118 * t152; + const T t154 = t145 * t153; + const T t155 = t149 * t154; + const T t156 = t106 * t20; + const T t157 = 4 * t116; + const T t158 = t113 * t94; + const T t159 = t157 * t158; + const T t160 = t159 * t20; + const T t161 = t123 * t150; + const T t162 = t158 * t17; + const T t163 = 4 * t162; + const T t164 = t145 * t163; + const T t165 = t123 * t154; + const T t166 = t105 * t147 + t108 * t128 + t130 * t146 + 2 * t156 - t160 + + t161 + t164 - t165; + const T t167 = + t94 * (-t133 - t135 + t138 + t139 + t141 - t148 - t151 + t155 - t166); + const T t168 = ea1_z + eb0_z - t80; + const T t169 = t106 * t168; + const T t170 = t110 * t17; + const T t171 = 2 * t170; + const T t172 = t110 * t120; + const T t173 = t136 * t172; + const T t174 = t105 * t172; + const T t175 = t140 * t168; + const T t176 = eb0_x * t80; + const T t177 = eb0_y * t80; + const T t178 = ea1_z * eb1_x; + const T t179 = ea1_z * eb1_y; + const T 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 T t181 = t119 * t180; + const T t182 = t106 * t181; + const T t183 = t136 * t182; + const T t184 = + t119 * (eb0_x * eb0_z - eb0_x * eb1_z - eb0_z * eb1_x + eb1_x * eb1_z); + const T t185 = t149 * t184; + const T t186 = t153 * t180; + const T t187 = t149 * t186; + const T t188 = t106 * t23; + const T t189 = t159 * t23; + const T t190 = t123 * t184; + const T t191 = t163 * t180; + const T t192 = t123 * t186; + const T t193 = t105 * t182 + t110 * t128 + t130 * t181 + 2 * t188 - t189 + + t190 + t191 - t192; + const T t194 = + t94 * (-t169 - t171 + t173 + t174 + t175 - t183 - t185 + t187 - t193); + const T t195 = t140 * t18; + const T t196 = 2 * t109; + const T t197 = t136 * t94; + const T t198 = t119 * t97; + const T t199 = 8 * t122; + const T t200 = 2 * t111 + t126; + const T t201 = t94 + * (-t106 * t157 * t197 + t107 - t123 * t198 + t123 * t199 + t131 + - t149 * t198 + t149 * t199 - t152 * t162 + t195 + t196 + t200); + const T t202 = t106 * t21; + const T t203 = t147 * t27; + const T t204 = t150 * t28; + const T t205 = t140 * t21; + const T t206 = t137 * t27; + const T t207 = t154 * t28; + const T t208 = t94 * (t166 - t202 - t203 - t204 + t205 + t206 + t207); + const T t209 = t106 * t24; + const T t210 = t182 * t27; + const T t211 = t184 * t28; + const T t212 = t140 * t24; + const T t213 = t172 * t27; + const T t214 = t186 * t28; + const T t215 = t94 * (t193 - t209 - t210 - t211 + t212 + t213 + t214); + const T 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 T t217 = t105 * t216; + const T t218 = t106 * t119; + const T t219 = t119 * t130; + const T t220 = t10 * t119; + const T t221 = t123 * t220; + const T t222 = t163 * t216; + const T t223 = t153 * t216; + const T t224 = t123 * t223; + const T t225 = t221 + t222 - t224; + const T t226 = t128 * t17 + t216 * t219 + t217 * t218 + t225; + const T t227 = ea0_x + eb1_x - t77; + const T t228 = 2 * t17; + const T t229 = t120 * t136; + const T t230 = t105 * t120; + const T t231 = t136 * t216; + const T t232 = t129 + t136 - t140 * t17 + t149 * t220 - t149 * t223 + t16 + - t17 * t229 + t218 * t231 + t227 * t228 - t227 * t230; + const T t233 = t94 * (-t105 - t226 - t232); + const T t234 = t128 * t20; + const T t235 = ea0_y + eb1_y - t68; + const T t236 = t228 * t235; + const T t237 = t140 * t20; + const T t238 = ea1_x * eb0_y; + const T 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 T t240 = t105 * t239; + const T t241 = t218 * t240; + const T t242 = t230 * t235; + const T t243 = t219 * t239; + const T t244 = t17 * t239; + const T t245 = 4 * t158; + const T t246 = t244 * t245; + const T t247 = t119 + * (-ea0_y * eb0_x - ea1_x * t68 + eb0_y * t63 - t114 - t144 + 2 * t32 + + t37 + t38); + const T t248 = t123 * t247; + const T t249 = t120 * t27; + const T t250 = t239 * t27; + const T t251 = t123 * t153 * t239; + const T t252 = -8 * t116 * t118 * t16 * t239 * t27 + t20 * t249 + + t218 * t250 - t246 + t247 * t28 - t248 + t251; + const T t253 = + t94 * (-t156 - t234 - t236 + t237 + t241 + t242 + t243 - t252); + const T t254 = t128 * t23; + const T t255 = ea0_z + eb1_z - t70; + const T t256 = t228 * t255; + const T t257 = t140 * t23; + const T t258 = ea1_x * eb0_z; + const T t259 = ea1_y * eb0_z; + const T 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 T t261 = t105 * t260; + const T t262 = t218 * t261; + const T t263 = t230 * t255; + const T t264 = t219 * t260; + const T t265 = t17 * t260; + const T t266 = t245 * t265; + const T t267 = t119 + * (-ea0_z * eb0_x - ea1_x * t70 + eb0_z * t63 - t115 - t178 + 2 * t34 + + t43 + t44); + const T t268 = t123 * t267; + const T t269 = t260 * t27; + const T t270 = t123 * t153 * t260; + const T t271 = -8 * t116 * t118 * t16 * t260 * t27 + t218 * t269 + + t23 * t249 - t266 + t267 * t28 - t268 + t270; + const T t272 = + t94 * (-t188 - t254 - t256 + t257 + t262 + t263 + t264 - t271); + const T t273 = -t22; + const T t274 = t105 * t18; + const T t275 = t120 * t274; + const T t276 = t216 * t27; + const T t277 = t218 * t276; + const T t278 = t220 * t28; + const T t279 = t17 * t27; + const T t280 = t120 * t279; + const T t281 = t16 * t276; + const T t282 = t153 * t281; + const T t283 = -t25; + const T t284 = t105 + t283; + const T t285 = + t94 * (t19 + t226 + t273 - t275 - t277 - t278 + t280 + t282 + t284); + const T t286 = t17 * t21; + const T t287 = 2 * t286; + const T t288 = t21 * t230; + const T t289 = t136 * t218; + const T t290 = -8 * t116 * t118 * t136 * t16 * t239 + t149 * t247 + + t20 * t229 + t239 * t289 + t246 + t248 - t251; + const T t291 = t94 * (t234 - t241 - t243 + t287 - t288 - t290); + const T t292 = t17 * t24; + const T t293 = 2 * t292; + const T t294 = t230 * t24; + const T t295 = -8 * t116 * t118 * t136 * t16 * t260 + t149 * t267 + + t229 * t23 + t260 * t289 + t266 + t268 - t270; + const T t296 = t94 * (t254 - t262 - t264 + t293 - t294 - t295); + const T t297 = -eb1_x * t77 + t57 + t58; + const T t298 = t297 + t95; + const T t299 = t108 * t146; + const T t300 = (t145 * t145); + const T t301 = t118 * t300; + const T t302 = t149 * t301; + const T t303 = t20 * t245; + const T t304 = -t105 * t113 * t298 * t94 + t124 * t301 + t145 * t303; + const T t305 = t108 * t20; + const T t306 = t132 * t16; + const T t307 = t105 * t299 + t108 * t132 + t146 * t306 + 2 * t305; + const T t308 = + t119 * (eb0_y * eb0_z - eb0_y * eb1_z - eb0_z * eb1_y + eb1_y * eb1_z); + const T t309 = t123 * t308; + const T t310 = t180 * t303; + const T t311 = t23 * t245; + const T t312 = t145 * t311; + const T t313 = 8 * t123; + const T t314 = t118 * t145; + const T t315 = t180 * t314; + const T t316 = t313 * t315; + const T t317 = t108 * t181; + const T t318 = t110 * t146; + const T t319 = 8 * t149; + const T t320 = t108 * t23; + const T t321 = t105 * t317 + t110 * t132 + t181 * t306 + 2 * t320; + const T t322 = t110 * t20; + const T t323 = t146 * t16; + const T t324 = t105 * t318 + t108 * t168 + t168 * t323 + 2 * t322; + const T t325 = -t94 + * (t136 * t317 + t136 * t318 + t149 * t308 + t309 + t310 + t312 + + t315 * t319 + t316 + t321 + t324); + const T t326 = t108 * t18 + t160 - t161 - t164 + t165 + t18 * t323; + const T t327 = + t94 * (t133 + t135 - t138 - t139 - t141 + t148 + t151 - t155 - t326); + const T t328 = -t21 * t323; + const T t329 = 2 * t107; + const T t330 = t119 * t298; + const T t331 = 4 * t197; + const T t332 = 8 * t158; + const T t333 = t94 + * (t108 * t145 * t331 + t109 - t123 * t330 + t145 * t20 * t332 + - t149 * t330 + t200 + t301 * t313 + 8 * t302 + t307 + t328 + t329); + const T t334 = t108 * t24; + const T t335 = t24 * t323; + const T t336 = t27 * t317; + const T t337 = t27 * t318; + const T t338 = t28 * t308; + const T t339 = 8 * t28; + const T t340 = t315 * t339; + const T t341 = t309 + t310 + t312 + t316 - t336 - t337 - t338 - t340; + const T t342 = t94 * (t321 - t334 - t335 + t341); + const T t343 = t136 * t146; + const T t344 = t108 * t119; + const T t345 = t119 + * (-ea0_x * eb1_y + ea1_y * t77 + eb1_x * t72 - t142 - 2 * t144 - t238 + + t31 + t32); + const T t346 = t216 * t314; + const T t347 = t149 * t345 + t17 * t343 + t231 * t344 + t319 * t346; + const T t348 = t119 * t306; + const T t349 = t132 * t17 + t216 * t348 + t217 * t344; + const T t350 = t123 * t345; + const T t351 = t20 * t216; + const T t352 = t245 * t351; + const T t353 = t313 * t346; + const T t354 = 2 * t20; + const T t355 = t105 * t146; + const T t356 = + t134 + t17 * t323 + t227 * t354 + t227 * t355 + t350 + t352 + t353; + const T t357 = -t94 * (t347 + t349 + t356); + const T t358 = t119 * (t15 + t4); + const T t359 = -2 * t108 * t136 * t239 * t94 + - 8 * t118 * t136 * t145 * t16 * t239 + t136 + t149 * t358 + t20 * t343; + const T t360 = -t239 * t303; + const T t361 = t123 * t358; + const T t362 = t239 * t314; + const T t363 = -t313 * t362; + const T t364 = t132 * t20 - t239 * t348 - t240 * t344 + t360 + t361 + t363; + const T t365 = t16 + t20 * t323 + t235 * t354 + t235 * t355 + t305; + const T t366 = t94 * (-t105 - t359 - t364 - t365); + const T t367 = t132 * t23; + const T t368 = t255 * t354; + const T t369 = t23 * t323; + const T t370 = t255 * t355; + const T t371 = t261 * t344; + const T t372 = t260 * t348; + const T t373 = t119 + * (-ea0_z * eb0_y - ea1_y * t70 + eb0_z * t72 - t143 - t179 + 2 * t40 + + t45 + t46); + const T t374 = t123 * t373; + const T t375 = t20 * t260; + const T t376 = t245 * t375; + const T t377 = t260 * t314; + const T t378 = t313 * t377; + const T t379 = t136 * t260 * t344 + t149 * t373 - t23 * t343 + t319 * t377 + + t374 + t376 + t378; + const T t380 = + t94 * (-t320 - t367 - t368 - t369 - t370 + t371 + t372 + t379); + const T t381 = t18 * t20; + const T t382 = t146 * t274 + t350 + t352 + t353 + 2 * t381; + const T t383 = 8 * t281; + const T t384 = -t146 * t279 - t276 * t344 - t28 * t345 - t314 * t383; + const T t385 = t94 * (t349 + t382 + t384); + const T t386 = t21 * t355 + t22; + const T t387 = -t19; + const T t388 = t146 * t27; + const T t389 = -t20 * t388 + t250 * t344 - t28 * t358 + t339 * t362 + t387; + const T t390 = t94 * (t284 + t364 + t386 + t389); + const T t391 = t20 * t24; + const T t392 = 2 * t391; + const T t393 = t24 * t355; + const T t394 = -t23 * t388 + t269 * t344 + t28 * t373 + t339 * t377 - t374 + - t376 - t378; + const T t395 = t94 * (t367 - t371 - t372 + t392 + t393 + t394); + const T t396 = t297 + t96; + const T t397 = t110 * t181; + const T t398 = (t180 * t180); + const T t399 = t118 * t398; + const T t400 = t149 * t399; + const T t401 = -t105 * t113 * t396 * t94 + t124 * t399 + t180 * t311; + const T t402 = t110 * t23; + const T t403 = t16 * t181; + const T t404 = t105 * t397 + t110 * t168 + t168 * t403 + 2 * t402; + const T t405 = t110 * t18 + t18 * t403 + t189 - t190 - t191 + t192; + const T t406 = + t94 * (t169 + t171 - t173 - t174 - t175 + t183 + t185 - t187 - t405); + const T t407 = t110 * t21; + const T t408 = t21 * t403; + const T t409 = t94 * (t324 + t341 - t407 - t408); + const T t410 = -t24 * t403; + const T t411 = t119 * t396; + const T t412 = t94 + * (t110 * t180 * t331 + t111 - t123 * t411 + t126 - t149 * t411 + + t180 * t23 * t332 + t196 + t313 * t399 + t329 + 8 * t400 + t404 + + t410); + const T t413 = t136 * t181; + const T t414 = t110 * t119; + const T t415 = t119 + * (-ea0_x * eb1_z + ea1_z * t77 + eb1_x * t80 - t176 - 2 * t178 - t258 + + t33 + t34); + const T t416 = t118 * t180; + const T t417 = t216 * t416; + const T t418 = t149 * t415 + t17 * t413 + t231 * t414 + t319 * t417; + const T t419 = t119 * t16; + const T t420 = t168 * t419; + const T t421 = t168 * t17 + t216 * t420 + t217 * t414; + const T t422 = t123 * t415; + const T t423 = t216 * t23; + const T t424 = t245 * t423; + const T t425 = t313 * t417; + const T t426 = 2 * t23; + const T t427 = t105 * t181; + const T t428 = + t17 * t403 + t170 + t227 * t426 + t227 * t427 + t422 + t424 + t425; + const T t429 = -t94 * (t418 + t421 + t428); + const T t430 = t119 + * (-ea0_y * eb1_z + ea1_z * t68 + eb1_y * t80 - t177 - 2 * t179 - t259 + + t39 + t40); + const T t431 = -2 * t110 * t136 * t239 * t94 + - 8 * t118 * t136 * t16 * t180 * t239 + t149 * t430 + t20 * t413; + const T t432 = t168 * t20 - t239 * t420 - t240 * t414; + const T t433 = t23 * t239; + const T t434 = -t245 * t433; + const T t435 = t123 * t430; + const T t436 = t239 * t416; + const T t437 = -t313 * t436; + const T t438 = + t20 * t403 + t235 * t426 + t235 * t427 + t322 + t434 + t435 + t437; + const T t439 = t94 * (-t431 - t432 - t438); + const T t440 = t119 * (t15 + t9); + const T t441 = -2 * t110 * t136 * t260 * t94 + - 8 * t118 * t136 * t16 * t180 * t260 + t136 + t149 * t440 + t23 * t413; + const T t442 = -t260 * t311; + const T t443 = t123 * t440; + const T t444 = t260 * t416; + const T t445 = -t313 * t444; + const T t446 = + t105 + t168 * t23 - t260 * t420 - t261 * t414 + t442 + t443 + t445; + const T t447 = t16 + t23 * t403 + t255 * t426 + t255 * t427 + t402; + const T t448 = t94 * (-t441 - t446 - t447); + const T t449 = t18 * t23; + const T t450 = t181 * t274 + t422 + t424 + t425 + 2 * t449; + const T t451 = -t181 * t279 - t276 * t414 - t28 * t415 - t383 * t416; + const T t452 = t94 * (t421 + t450 + t451); + const T t453 = t21 * t23; + const T t454 = t21 * t427 + t434 + t435 + t437 + 2 * t453; + const T t455 = t181 * t27; + const T t456 = -t20 * t455 + t250 * t414 - t28 * t430 + t339 * t436; + const T t457 = t94 * (t432 + t454 + t456); + const T t458 = t24 * t427 + t25; + const T t459 = + -t23 * t455 + t269 * t414 + t273 - t28 * t440 + t339 * t444 + t387; + const T t460 = t94 * (t446 + t458 + t459); + const T t461 = t94 * (t202 + t203 + t204 - t205 - t206 - t207 + t326); + const T t462 = t94 * (t209 + t210 + t211 - t212 - t213 - t214 + t405); + const T t463 = t18 * t419; + const T t464 = t216 * t463; + const T t465 = t94 * (t225 + t232 + t387 - t464); + const T t466 = t239 * t463; + const T t467 = t94 * (t156 + t236 - t237 - t242 - t290 - t381 + t466); + const T t468 = t260 * t463; + const T t469 = t94 * (t188 + t256 - t257 - t263 - t295 - t449 + t468); + const T t470 = t94 + * (-t221 - t222 + t224 + t26 + t275 + t277 + t278 - t280 - t282 + t464); + const T t471 = t94 * (-t252 - t287 + t288 + t381 - t466); + const T t472 = t94 * (-t271 - t293 + t294 + t449 - t468); + const T t473 = t94 + * (-t309 - t310 - t312 - t316 + t334 + t335 + t336 + t337 + t338 + t340 + + t407 + t408); + const T t474 = t21 * t419; + const T t475 = -t216 * t474 - t286; + const T t476 = t94 * (t356 + t384 + t475); + const T t477 = t239 * t474 + t360 + t361 + t363; + const T t478 = t94 * (-2 * t22 + t283 + t365 + t389 + t477); + const T t479 = t260 * t474; + const T t480 = t94 * (t320 + t368 + t369 + t370 + t394 - t453 + t479); + const T t481 = t94 * (-t347 - t382 - t475); + const T t482 = t94 * (-t359 - t386 - t477); + const T t483 = t94 * (t379 - t392 - t393 + t453 - t479); + const T t484 = t24 * t419; + const T t485 = -t216 * t484 - t292; + const T t486 = t94 * (t428 + t451 + t485); + const T t487 = t239 * t484 - t391; + const T t488 = t94 * (t438 + t456 + t487); + const T t489 = t260 * t484 + t442 + t443 + t445; + const T t490 = t94 * (-2 * t25 + t447 + t459 + t489); + const T t491 = t94 * (-t418 - t450 - t485); + const T t492 = t94 * (-t431 - t454 - t487); + const T t493 = t94 * (-t441 - t458 - t489); + const T t494 = (t17 * t17); + const T t495 = t119 * t227; + const T t496 = t217 * t495; + const T t497 = t17 * t216 * t419; + const T t498 = (t216 * t216); + const T t499 = t118 * t498; + const T t500 = t17 * t20; + const T t501 = + ea0_x * ea0_y - ea0_x * ea1_y - ea0_y * ea1_x + ea1_x * ea1_y; + const T t502 = t123 * t501; + const T t503 = t149 * t94; + const T t504 = t217 * t94; + const T t505 = t16 * t94; + const T 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 T t507 = t17 * t23; + const T t508 = + ea0_x * ea0_z - ea0_x * ea1_z - ea0_z * ea1_x + ea1_x * ea1_z; + const T t509 = t123 * t508; + const T 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 T t511 = 4 * t276; + const T 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 T t513 = t21 * t217; + const T t514 = t239 * t279; + const T t515 = t118 * t216; + const T t516 = t118 * t383; + const T t517 = -t119 * t20 * t276 - t119 * t28 * t501 + t119 * t502 + + t119 * t514 - t239 * t313 * t515 + t239 * t516 + t500; + const T t518 = t94 * (t119 * t513 - t240 * t495 - t244 * t419 + t517); + const T t519 = t217 * t24; + const T t520 = t260 * t279; + const T t521 = -t119 * t23 * t276 - t119 * t28 * t508 + t119 * t509 + + t119 * t520 - t260 * t313 * t515 + t260 * t516 + t507; + const T t522 = t94 * (t119 * t519 - t261 * t495 - t265 * t419 + t521); + const T t523 = t104 + t99; + const T t524 = t123 * t523; + const T t525 = t28 * t523; + const T t526 = (t239 * t239); + const T t527 = t118 * t526; + const T t528 = 4 * t28; + const T t529 = t20 * t239; + const T t530 = t119 * t235; + const T t531 = t105 - (t20 * t20) + t240 * t530 + t419 * t529; + const T t532 = t20 * t23; + const T t533 = + ea0_y * ea0_z - ea0_y * ea1_z - ea0_z * ea1_y + ea1_y * ea1_z; + const T t534 = t123 * t533; + const T t535 = t235 * t261; + const T t536 = t240 * t255; + const T t537 = t27 * t375; + const T t538 = t27 * t433; + const T t539 = t239 * t260; + const T t540 = t118 * t539; + const T 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 T t542 = + t94 * (-t119 * t239 * t274 + t217 * t530 + t351 * t419 + t517); + const T t543 = t119 * t149; + const T t544 = t94 + * (8 * t105 * t113 * t118 * t526 + 8 * t118 * t136 * t16 * t526 + - t119 * t21 * t240 - t119 * t524 - t331 * t529 - t523 * t543 + - t531); + const T t545 = t119 * t136; + const T t546 = t119 * t534 + t313 * t540 + t319 * t540 - t375 * t545 + - t433 * t545 + t532 + t533 * t543; + const T t547 = + t94 * (-t119 * t24 * t240 - t119 * t535 - t375 * t419 + t546); + const T t548 = t101 + t104; + const T t549 = t123 * t548; + const T t550 = t28 * t548; + const T t551 = (t260 * t260); + const T t552 = t118 * t551; + const T t553 = t23 * t260; + const T t554 = t119 * t255; + const T t555 = t105 - (t23 * t23) + t261 * t554 + t419 * t553; + const T t556 = + t94 * (-t119 * t260 * t274 + t217 * t554 + t419 * t423 + t521); + const T t557 = + t94 * (-t119 * t21 * t261 - t119 * t536 - t419 * t433 + t546); + const T t558 = t94 + * (8 * t105 * t113 * t118 * t551 + 8 * t118 * t136 * t16 * t551 + - t119 * t24 * t261 - t119 * t549 - t331 * t553 - t543 * t548 + - t555); + const T t559 = t124 * t94; + const T t560 = 2 * t118; + const T t561 = t505 * t511; + const T t562 = t560 + * (4 * t105 * t113 * t216 * t239 * t94 + t105 * t18 * t239 + + t16 * t27 * t501 + t20 * t216 * t27 - t239 * t561 - t502 - t513 + - t514); + const T t563 = t560 + * (4 * t105 * t113 * t216 * t260 * t94 + t105 * t18 * t260 + + t16 * t27 * t508 + t216 * t23 * t27 - t260 * t561 - t509 - t519 + - t520); + const T 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); } -std::array edge_edge_closest_point_hessian( - Eigen::ConstRef ea0, - Eigen::ConstRef ea1, - Eigen::ConstRef eb0, - Eigen::ConstRef eb1) +// J is (2×12) flattened in column-major order +template +void point_triangle_closest_point_jacobian( + T p_x, + T p_y, + T p_z, + T t0_x, + T t0_y, + T t0_z, + T t1_x, + T t1_y, + T t1_z, + T t2_x, + T t2_y, + T t2_z, + T J[24]) { - std::array H; - autogen::edge_edge_closest_point_hessian_a( - ea0[0], ea0[1], ea0[2], ea1[0], ea1[1], ea1[2], eb0[0], eb0[1], eb0[2], - eb1[0], eb1[1], eb1[2], H[0].data()); - autogen::edge_edge_closest_point_hessian_b( - ea0[0], ea0[1], ea0[2], ea1[0], ea1[1], ea1[2], eb0[0], eb0[1], eb0[2], - eb1[0], eb1[1], eb1[2], H[1].data()); - return H; + const T t0 = t0_x - t1_x; + const T t1 = t2_x * t2_x; + const T t2 = t2_y * t2_y; + const T t3 = t2_z * t2_z; + const T t4 = t0_x * t2_x; + const T t5 = 2 * t4; + const T t6 = t0_y * t2_y; + const T t7 = 2 * t6; + const T t8 = t0_z * t2_z; + const T t9 = 2 * t8; + const T t10 = t0_x * t0_x; + const T t11 = t0_y * t0_y; + const T t12 = t0_z * t0_z; + const T t13 = t10 + t11 + t12; + const T t14 = t1 + t13 + t2 + t3 - t5 - t7 - t9; + const T t15 = t0_x - t2_x; + const T t16 = t1_x * t2_x; + const T t17 = t1_y * t2_y; + const T t18 = t1_z * t2_z; + const T t19 = t0_x * t1_x; + const T t20 = t0_y * t1_y; + const T t21 = t0_z * t1_z; + const T t22 = t13 + t16 + t17 + t18 - t19 - t20 - t21 - t4 - t6 - t8; + const T t23 = 2 * t19; + const T t24 = 2 * t20; + const T t25 = 2 * t21; + const T t26 = 2 * t16; + const T t27 = 2 * t17; + const T t28 = t1_y * t1_y; + const T t29 = t1_z * t1_z; + const T t30 = t1_x * t1_x; + const T t31 = 2 * t18; + const T t32 = T(1.0) + / (t1 * t11 + t1 * t12 - t1 * t24 - t1 * t25 + t1 * t28 + t1 * t29 + + t10 * t2 - t10 * t27 + t10 * t28 + t10 * t29 + t10 * t3 - t10 * t31 + - t11 * t26 + t11 * t29 + t11 * t3 + t11 * t30 - t11 * t31 + t12 * t2 + - t12 * t26 - t12 * t27 + t12 * t28 + t12 * t30 + t16 * t24 + + t16 * t25 + t16 * t7 + t16 * t9 + t17 * t23 + t17 * t25 - t17 * t26 + + t17 * t5 + t17 * t9 + t18 * t23 + t18 * t24 - t18 * t26 - t18 * t27 + + t18 * t5 + t18 * t7 + t19 * t7 + t19 * t9 - t2 * t23 - t2 * t25 + + t2 * t29 + t2 * t30 - t20 * t23 + t20 * t5 + t20 * t9 - t21 * t23 + - t21 * t24 + t21 * t5 + t21 * t7 - t23 * t3 - t24 * t3 + t28 * t3 + - t28 * t5 - t28 * t9 - t29 * t5 - t29 * t7 + t3 * t30 - t30 * t7 + - t30 * t9 - t5 * t6 - t5 * t8 - t7 * t8); + const T t33 = t13 - t23 - t24 - t25 + t28 + t29 + t30; + const T t34 = t0_y - t1_y; + const T t35 = t0_y - t2_y; + const T t36 = t0_z - t1_z; + const T t37 = t0_z - t2_z; + const T t38 = 2 * t0_x; + const T t39 = -t38; + const T t40 = p_x + t39; + const T t41 = t1_x + t40; + const T t42 = p_x - t0_x; + const T t43 = p_y - t0_y; + const T t44 = p_z - t0_z; + const T t45 = t0 * t42 + t34 * t43 + t36 * t44; + const T t46 = t15 * t45; + const T t47 = 2 * t46; + const T t48 = t1_x + t2_x + t39; + const T t49 = t15 * t42 + t35 * t43 + t37 * t44; + const T t50 = t2_x + t40; + const T t51 = t0_x * t28; + const T t52 = t0_x * t29; + const T t53 = t0_x * t2; + const T t54 = t0_x * t3; + const T t55 = t1_x * t2; + const T t56 = t1_x * t3; + const T t57 = t28 * t2_x; + const T t58 = t29 * t2_x; + const T t59 = t17 * t1_x; + const T t60 = t18 * t1_x; + const T t61 = t17 * t2_x; + const T t62 = t18 * t2_x; + const T t63 = t1_x * t20; + const T t64 = t2_x * t6; + const T t65 = t1_x * t21; + const T t66 = t2_x * t8; + const T t67 = t20 * t2_x + t21 * t2_x; + const T t68 = t1_x * t6 + t1_x * t8; + const T t69 = 2 * t32; + const T t70 = t69 + * (-t17 * t38 - t18 * t38 + t51 + t52 + t53 + t54 - t55 - t56 - t57 + - t58 + t59 + t60 + t61 + t62 - t63 - t64 - t65 - t66 + t67 + t68); + const T t71 = t14 * t45; + const T t72 = t22 * t70; + const T t73 = t0 * t49; + const T t74 = 2 * t73; + const T t75 = t33 * t49; + const T t76 = 2 * t0_y; + const T t77 = -t76; + const T t78 = p_y + t77; + const T t79 = t1_y + t78; + const T t80 = t35 * t45; + const T t81 = 2 * t80; + const T t82 = t1_y + t2_y + t77; + const T t83 = t2_y + t78; + const T t84 = t0_y * t30; + const T t85 = t0_y * t29; + const T t86 = t0_y * t1; + const T t87 = t0_y * t3; + const T t88 = t1 * t1_y; + const T t89 = t1_y * t3; + const T t90 = t2_y * t30; + const T t91 = t29 * t2_y; + const T t92 = t16 * t1_y; + const T t93 = t16 * t2_y; + const T t94 = t18 * t1_y; + const T t95 = t18 * t2_y; + const T t96 = t19 * t1_y; + const T t97 = t2_y * t4; + const T t98 = t1_y * t21; + const T t99 = t2_y * t8; + const T t100 = t19 * t2_y + t21 * t2_y; + const T t101 = t1_y * t4 + t1_y * t8; + const T t102 = t69 + * (t100 + t101 - t16 * t76 - t18 * t76 + t84 + t85 + t86 + t87 - t88 + - t89 - t90 - t91 + t92 + t93 + t94 + t95 - t96 - t97 - t98 - t99); + const T t103 = t102 * t22; + const T t104 = t34 * t49; + const T t105 = 2 * t104; + const T t106 = 2 * t0_z; + const T t107 = -t106; + const T t108 = p_z + t107; + const T t109 = t108 + t1_z; + const T t110 = t37 * t45; + const T t111 = 2 * t110; + const T t112 = t107 + t1_z + t2_z; + const T t113 = t108 + t2_z; + const T t114 = t0_z * t30; + const T t115 = t0_z * t28; + const T t116 = t0_z * t1; + const T t117 = t0_z * t2; + const T t118 = t1 * t1_z; + const T t119 = t1_z * t2; + const T t120 = t2_z * t30; + const T t121 = t28 * t2_z; + const T t122 = t16 * t1_z; + const T t123 = t16 * t2_z; + const T t124 = t17 * t1_z; + const T t125 = t17 * t2_z; + const T t126 = t19 * t1_z; + const T t127 = t2_z * t4; + const T t128 = t1_z * t20; + const T t129 = t2_z * t6; + const T t130 = t19 * t2_z + t20 * t2_z; + const T t131 = t1_z * t4 + t1_z * t6; + const T t132 = t69 + * (-t106 * t16 - t106 * t17 + t114 + t115 + t116 + t117 - t118 - t119 + - t120 - t121 + t122 + t123 + t124 + t125 - t126 - t127 - t128 - t129 + + t130 + t131); + const T t133 = t132 * t22; + const T t134 = t36 * t49; + const T t135 = 2 * t134; + const T t136 = t11 * t1_x; + const T t137 = t12 * t1_x; + const T t138 = t11 * t2_x; + const T t139 = t12 * t2_x; + const T t140 = t0_x * t6; + const T t141 = t0_x * t8; + const T t142 = t0_x * t20; + const T t143 = t0_x * t21; + const T t144 = t0_x * t17 + t0_x * t18; + const T t145 = t69 + * (t136 + t137 - t138 - t139 + t140 + t141 - t142 - t143 + t144 + - t1_x * t7 - t1_x * t9 - t53 - t54 + t55 + t56 - t61 - t62 + t64 + + t66 + t67); + const T t146 = t145 * t22; + const T t147 = -t22 * t42; + const T t148 = t10 * t1_y; + const T t149 = t12 * t1_y; + const T t150 = t10 * t2_y; + const T t151 = t12 * t2_y; + const T t152 = t0_y * t4; + const T t153 = t0_y * t8; + const T t154 = t0_y * t19; + const T t155 = t0_y * t21; + const T t156 = t0_y * t16 + t0_y * t18; + const T t157 = t69 + * (t100 + t148 + t149 - t150 - t151 + t152 + t153 - t154 - t155 + t156 + - t1_y * t5 - t1_y * t9 - t86 - t87 + t88 + t89 - t93 - t95 + t97 + + t99); + const T t158 = t157 * t22; + const T t159 = -t22 * t43; + const T t160 = t10 * t1_z; + const T t161 = t11 * t1_z; + const T t162 = t10 * t2_z; + const T t163 = t11 * t2_z; + const T t164 = t0_z * t4; + const T t165 = t0_z * t6; + const T t166 = t0_z * t19; + const T t167 = t0_z * t20; + const T t168 = t0_z * t16 + t0_z * t17; + const T t169 = t69 + * (-t116 - t117 + t118 + t119 - t123 - t125 + t127 + t129 + t130 + t160 + + t161 - t162 - t163 + t164 + t165 - t166 - t167 + t168 - t1_z * t5 + - t1_z * t7); + const T t170 = t169 * t22; + const T t171 = -t22 * t44; + const T t172 = t69 + * (-t136 - t137 + t138 + t139 - t140 - t141 + t142 + t143 + t144 + - t24 * t2_x - t25 * t2_x - t51 - t52 + t57 + t58 - t59 - t60 + t63 + + t65 + t68); + const T t173 = t172 * t22; + const T t174 = t69 + * (t101 - t148 - t149 + t150 + t151 - t152 - t153 + t154 + t155 + t156 + - t23 * t2_y - t25 * t2_y - t84 - t85 + t90 + t91 - t92 - t94 + t96 + + t98); + const T t175 = t174 * t22; + const T t176 = t69 + * (-t114 - t115 + t120 + t121 - t122 - t124 + t126 + t128 + t131 - t160 + - t161 + t162 + t163 - t164 - t165 + t166 + t167 + t168 - t23 * t2_z + - t24 * t2_z); + const T t177 = t176 * t22; + J[0] = t32 * (-t0 * t14 + t15 * t22); + J[1] = t32 * (t0 * t22 - t15 * t33); + J[2] = t32 * (-t14 * t34 + t22 * t35); + J[3] = t32 * (t22 * t34 - t33 * t35); + J[4] = t32 * (-t14 * t36 + t22 * t37); + J[5] = t32 * (t22 * t36 - t33 * t37); + J[6] = t32 + * (-t14 * t41 + t22 * t50 - t47 - t48 * t49 - t49 * t72 + t70 * t71); + J[7] = + t32 * (t22 * t41 - t33 * t50 - t45 * t48 - t45 * t72 + t70 * t75 - t74); + J[8] = t32 + * (t102 * t71 - t103 * t49 - t14 * t79 + t22 * t83 - t49 * t82 - t81); + J[9] = t32 + * (t102 * t75 - t103 * t45 - t105 + t22 * t79 - t33 * t83 - t45 * t82); + J[10] = t32 + * (-t109 * t14 - t111 - t112 * t49 + t113 * t22 + t132 * t71 + - t133 * t49); + J[11] = t32 + * (t109 * t22 - t112 * t45 - t113 * t33 + t132 * t75 - t133 * t45 + - t135); + J[12] = t32 * (t14 * t42 + t145 * t71 - t146 * t49 - t15 * t49); + J[13] = t32 * (t145 * t75 - t146 * t45 + t147 - t46 + t74); + J[14] = t32 * (t14 * t43 + t157 * t71 - t158 * t49 - t35 * t49); + J[15] = t32 * (t105 + t157 * t75 - t158 * t45 + t159 - t80); + J[16] = t32 * (t14 * t44 + t169 * t71 - t170 * t49 - t37 * t49); + J[17] = t32 * (-t110 + t135 + t169 * t75 - t170 * t45 + t171); + J[18] = t32 * (t147 + t172 * t71 - t173 * t49 + t47 - t73); + J[19] = t32 * (-t0 * t45 + t172 * t75 - t173 * t45 + t33 * t42); + J[20] = t32 * (-t104 + t159 + t174 * t71 - t175 * t49 + t81); + J[21] = t32 * (t174 * t75 - t175 * t45 + t33 * t43 - t34 * t45); + J[22] = t32 * (t111 - t134 + t171 + t176 * t71 - t177 * t49); + J[23] = t32 * (t176 * t75 - t177 * t45 + t33 * t44 - t36 * t45); } -// ============================================================================ -// Point - Triangle - -Eigen::Vector2d point_triangle_closest_point( - Eigen::ConstRef p, - Eigen::ConstRef t0, - Eigen::ConstRef t1, - Eigen::ConstRef t2) -{ - Eigen::Matrix basis; - basis.row(0) = Eigen::RowVector3d(t1 - t0); // edge 0 - basis.row(1) = Eigen::RowVector3d(t2 - t0); // edge 1 - const Eigen::Matrix2d A = basis * basis.transpose(); - const Eigen::Vector2d b = basis * (p - t0); - const Eigen::Vector2d x = A.ldlt().solve(b); - assert((A * x - b).norm() < 1e-10); - return x; -} - -Eigen::Matrix point_triangle_closest_point_jacobian( - Eigen::ConstRef p, - Eigen::ConstRef t0, - Eigen::ConstRef t1, - Eigen::ConstRef t2) +// hess is (144×1) flattened in column-major order +template +void point_triangle_closest_point_hessian_0( + T p_x, + T p_y, + T p_z, + T t0_x, + T t0_y, + T t0_z, + T t1_x, + T t1_y, + T t1_z, + T t2_x, + T t2_y, + T t2_z, + T hess[144]) { - Eigen::Matrix J; - autogen::point_triangle_closest_point_jacobian( - p[0], p[1], p[2], t0[0], t0[1], t0[2], t1[0], t1[1], t1[2], t2[0], - t2[1], t2[2], J.data()); - - return J; + const T t0 = t0_x * t1_x; + const T t1 = t0_y * t1_y; + const T t2 = 2 * t1; + const T t3 = t0_y * t2_y; + const T t4 = 2 * t3; + const T t5 = t0_x * t2_x; + const T t6 = 2 * t5; + const T t7 = t0_z * t1_z; + const T t8 = 2 * t7; + const T t9 = t0_z * t2_z; + const T t10 = 2 * t9; + const T t11 = t1_y * t2_y; + const T t12 = 2 * t11; + const T t13 = t1_z * t2_z; + const T t14 = 2 * t13; + const T t15 = t1_x * t2_x; + const T t16 = 2 * t15; + const T t17 = (t0_x * t0_x); + const T t18 = (t1_y * t1_y); + const T t19 = (t1_z * t1_z); + const T t20 = (t2_y * t2_y); + const T t21 = (t2_z * t2_z); + const T t22 = (t0_y * t0_y); + const T t23 = (t1_x * t1_x); + const T t24 = (t2_x * t2_x); + const T t25 = (t0_z * t0_z); + const T t26 = 2 * t0; + const T 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 T t28 = T(1.0) / t27; + const T t29 = t0_x - t2_x; + const T t30 = 2 * t0_x; + const T t31 = -t30; + const T t32 = t2_x + t31; + const T t33 = t1_x + t32; + const T t34 = t0_x - t1_x; + const T t35 = t29 * t34; + const T t36 = t20 + t21; + const T t37 = -t10 + t25; + const T t38 = t22 - t4; + const T t39 = t36 + t37 + t38; + const T t40 = t17 - t6; + const T t41 = t24 + t39 + t40; + const T t42 = t0_x * t18; + const T t43 = t0_x * t19; + const T t44 = t0_x * t20; + const T t45 = t0_x * t21; + const T t46 = t1_x * t20; + const T t47 = t1_x * t21; + const T t48 = t18 * t2_x; + const T t49 = t19 * t2_x; + const T t50 = t11 * t1_x; + const T t51 = t13 * t1_x; + const T t52 = t11 * t2_x; + const T t53 = t13 * t2_x; + const T t54 = t1 * t1_x; + const T t55 = t2_x * t3; + const T t56 = t1_x * t7; + const T t57 = t2_x * t9; + const T t58 = t1 * t2_x; + const T t59 = t2_x * t7; + const T t60 = t58 + t59; + const T t61 = t1_x * t3; + const T t62 = t1_x * t9; + const T t63 = t61 + t62; + const T t64 = -t11 * t30 - t13 * t30 + t42 + t43 + t44 + t45 - t46 - t47 + - t48 - t49 + t50 + t51 + t52 + t53 - t54 - t55 - t56 - t57 + t60 + t63; + const T t65 = t11 + t13; + const T t66 = -t9; + const T t67 = t25 + t66 - t7; + const T t68 = -t3; + const T t69 = -t1 + t22 + t68; + const T t70 = t65 + t67 + t69; + const T t71 = -t5; + const T t72 = -t0 + t17 + t71; + const T t73 = t15 + t70 + t72; + const T t74 = t64 * t73; + const T t75 = 2 * t29; + const T t76 = t28 * t75; + const T t77 = -t13; + const T t78 = t66 + t7; + const T t79 = t77 + t78; + const T t80 = -t11; + const T t81 = t1 + t68; + const T t82 = t80 + t81; + const T t83 = t36 + t79 + t82; + const T t84 = -t15; + const T t85 = t0 + t71; + const T t86 = t84 + t85; + const T t87 = t24 + t83 + t86; + const T t88 = t28 + * (2 * t28 * t34 * t41 * t64 - t29 * t33 - 2 * t35 - t74 * t76 - t87); + const T t89 = t0_y - t2_y; + const T t90 = t34 * t89; + const T t91 = 2 * t90; + const T t92 = 2 * t0_y; + const T t93 = -t92; + const T t94 = t2_y + t93; + const T t95 = t1_y + t94; + const T t96 = t0_y * t23; + const T t97 = t0_y * t19; + const T t98 = t0_y * t24; + const T t99 = t0_y * t21; + const T t100 = t1_y * t24; + const T t101 = t1_y * t21; + const T t102 = t23 * t2_y; + const T t103 = t19 * t2_y; + const T t104 = t15 * t1_y; + const T t105 = t15 * t2_y; + const T t106 = t13 * t1_y; + const T t107 = t13 * t2_y; + const T t108 = t0 * t1_y; + const T t109 = t2_y * t5; + const T t110 = t1_y * t7; + const T t111 = t2_y * t9; + const T t112 = t0 * t2_y; + const T t113 = t2_y * t7; + const T t114 = t112 + t113; + const T t115 = t1_y * t5 + t1_y * t9; + const T t116 = -t100 - t101 - t102 - t103 + t104 + t105 + t106 + t107 - t108 + - t109 - t110 - t111 + t114 + t115 - t13 * t92 - t15 * t92 + t96 + t97 + + t98 + t99; + const T t117 = t73 * t76; + const T t118 = + t28 * (-t116 * t117 + 2 * t116 * t28 * t34 * t41 - t29 * t95 - t91); + const T t119 = t0_z - t2_z; + const T t120 = t119 * t34; + const T t121 = 2 * t120; + const T t122 = 2 * t0_z; + const T t123 = -t122; + const T t124 = t123 + t2_z; + const T t125 = t124 + t1_z; + const T t126 = t0_z * t23; + const T t127 = t0_z * t18; + const T t128 = t0_z * t24; + const T t129 = t0_z * t20; + const T t130 = t1_z * t24; + const T t131 = t1_z * t20; + const T t132 = t23 * t2_z; + const T t133 = t18 * t2_z; + const T t134 = t15 * t1_z; + const T t135 = t15 * t2_z; + const T t136 = t11 * t1_z; + const T t137 = t11 * t2_z; + const T t138 = t0 * t1_z; + const T t139 = t2_z * t5; + const T t140 = t1 * t1_z; + const T t141 = t2_z * t3; + const T t142 = t0 * t2_z; + const T t143 = t1 * t2_z; + const T t144 = t142 + t143; + const T t145 = t1_z * t3 + t1_z * t5; + const T t146 = -t11 * t122 - t122 * t15 + t126 + t127 + t128 + t129 - t130 + - t131 - t132 - t133 + t134 + t135 + t136 + t137 - t138 - t139 - t140 + - t141 + t144 + t145; + const T t147 = + t28 * (-t117 * t146 - t121 - t125 * t29 + 2 * t146 * t28 * t34 * t41); + const T t148 = t1_x * t22; + const T t149 = t1_x * t25; + const T t150 = t22 * t2_x; + const T t151 = t25 * t2_x; + const T t152 = t0_x * t3; + const T t153 = t0_x * t9; + const T t154 = t0_x * t1; + const T t155 = t0_x * t7; + const T t156 = t0_x * t11 + t0_x * t13; + const T t157 = t148 + t149 - t150 - t151 + t152 + t153 - t154 - t155 + t156 + - t44 - t45 + t46 + t47 - t52 - t53 + t55 + t57 + t60 - 2 * t61 + - 2 * t62; + const T t158 = t157 * t41; + const T t159 = 2 * t28; + const T t160 = t159 * t34; + const T t161 = t28 * (-t117 * t157 + t158 * t160 - (t29 * t29) + t41); + const T t162 = t29 * t89; + const T t163 = t17 * t1_y; + const T t164 = t1_y * t25; + const T t165 = t17 * t2_y; + const T t166 = t25 * t2_y; + const T t167 = t0_y * t5; + const T t168 = t0_y * t9; + const T t169 = t0 * t0_y; + const T t170 = t0_y * t7; + const T t171 = t0_y * t13 + t0_y * t15; + const T 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 T t173 = t28 * (-t117 * t172 - t162 + 2 * t172 * t28 * t34 * t41); + const T t174 = t119 * t29; + const T t175 = t17 * t1_z; + const T t176 = t1_z * t22; + const T t177 = t17 * t2_z; + const T t178 = t22 * t2_z; + const T t179 = t0_z * t5; + const T t180 = t0_z * t3; + const T t181 = t0 * t0_z; + const T t182 = t0_z * t1; + const T t183 = t0_z * t11 + t0_z * t15; + const T 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 T t185 = t28 * (-t117 * t184 - t174 + 2 * t184 * t28 * t34 * t41); + const T t186 = -t148 - t149 + t150 + t151 - t152 - t153 + t154 + t155 + t156 + - t42 - t43 + t48 + t49 - t50 - t51 + t54 + t56 - 2 * t58 - 2 * t59 + + t63; + const T t187 = t160 * t41; + const T t188 = + t0 + t1 - t17 - t22 - t25 + t3 + t5 + t7 + t77 + t80 + t84 + t9; + const T t189 = t28 * (-t117 * t186 + t186 * t187 + t188 + t35); + const T t190 = t0_y - t1_y; + const T t191 = t190 * t29; + const T t192 = t102 + t103 - t104 - t106 + t108 + t110 - 2 * t112 - 2 * t113 + + t115 - t163 - t164 + t165 + t166 - t167 - t168 + t169 + t170 + t171 + - t96 - t97; + const T t193 = t28 * (-t117 * t192 + t187 * t192 - t191 + t91); + const T t194 = t0_z - t1_z; + const T t195 = t194 * t29; + const T t196 = -t126 - t127 + t132 + t133 - t134 - t136 + t138 + t140 + - 2 * t142 - 2 * t143 + t145 - t175 - t176 + t177 + t178 - t179 - t180 + + t181 + t182 + t183; + const T t197 = t28 * (-t117 * t196 + t121 + t187 * t196 - t195); + const T t198 = 2 * t191; + const T t199 = t159 * t89; + const T t200 = + t28 * (2 * t190 * t28 * t41 * t64 - t198 - t199 * t74 - t33 * t89); + const T t201 = t190 * t89; + const T t202 = t199 * t73; + const T t203 = t28 + * (2 * t116 * t190 * t28 * t41 - t116 * t202 - 2 * t201 - t87 + - t89 * t95); + const T t204 = t119 * t190; + const T t205 = 2 * t204; + const T t206 = + t28 * (-t125 * t89 + 2 * t146 * t190 * t28 * t41 - t146 * t202 - t205); + const T t207 = t28 * (2 * t157 * t190 * t28 * t41 - t157 * t202 - t162); + const T t208 = t159 * t190; + const T t209 = t208 * t41; + const T t210 = t28 * (-t172 * t202 + t172 * t209 + t41 - (t89 * t89)); + const T t211 = t119 * t89; + const T t212 = t28 * (2 * t184 * t190 * t28 * t41 - t184 * t202 - t211); + const T t213 = + t28 * (2 * t186 * t190 * t28 * t41 - t186 * t202 + t198 - t90); + const T t214 = t28 * (t188 - t192 * t202 + t192 * t209 + t201); + const T t215 = t194 * t89; + const T t216 = t28 * (-t196 * t202 + t196 * t209 + t205 - t215); + const T t217 = 2 * t195; + const T t218 = t119 * t159; + const T t219 = + t28 * (-t119 * t33 + 2 * t194 * t28 * t41 * t64 - t217 - t218 * t74); + const T t220 = 2 * t215; + const T t221 = t218 * t73; + const T t222 = + t28 * (2 * t116 * t194 * t28 * t41 - t116 * t221 - t119 * t95 - t220); + const T t223 = t119 * t194; + const T t224 = t28 + * (-t119 * t125 + 2 * t146 * t194 * t28 * t41 - t146 * t221 - 2 * t223 + - t87); + const T t225 = t28 * (2 * t157 * t194 * t28 * t41 - t157 * t221 - t174); + const T t226 = t28 * (2 * t172 * t194 * t28 * t41 - t172 * t221 - t211); + const T t227 = t159 * t194; + const T t228 = t227 * t41; + const T t229 = t28 * (-(t119 * t119) - t184 * t221 + t184 * t228 + t41); + const T t230 = + t28 * (-t120 + 2 * t186 * t194 * t28 * t41 - t186 * t221 + t217); + const T t231 = + t28 * (2 * t192 * t194 * t28 * t41 - t192 * t221 - t204 + t220); + const T t232 = t28 * (t188 - t196 * t221 + t196 * t228 + t223); + const T t233 = p_x + t32; + const T t234 = p_x + t1_x + t31; + const T t235 = t234 * t75; + const T t236 = p_x - t0_x; + const T t237 = t236 * t34; + const T t238 = p_y - t0_y; + const T t239 = t190 * t238; + const T t240 = p_z - t0_z; + const T t241 = t194 * t240; + const T t242 = t239 + t241; + const T t243 = t237 + t242; + const T t244 = t243 * t41; + const T t245 = -t12; + const T t246 = -t14; + const T t247 = t18 + t19; + const T t248 = t28 * (t245 + t246 + t247 + t36); + const T t249 = t236 * t29; + const T t250 = t238 * t89; + const T t251 = t119 * t240; + const T t252 = t250 + t251; + const T t253 = t249 + t252; + const T t254 = t253 * t73; + const T t255 = T(1) / (t27 * t27); + const T t256 = 4 * t255; + const T t257 = t256 * (t64 * t64); + const T t258 = t159 * t233; + const T t259 = t159 * t64; + const T t260 = t234 * t41; + const T t261 = t253 * t33; + const T t262 = 4 * t28; + const T t263 = t243 * t262; + const T t264 = t263 * t29; + const T t265 = t264 * t64; + const T t266 = -t237 - t239 - t241 + t253 + t87; + const T t267 = p_y + t94; + const T t268 = p_y + t1_y + t93; + const T t269 = t1_x * t1_y; + const T t270 = t2_x * t2_y; + const T t271 = t1_x * t2_y; + const T t272 = -t271; + const T t273 = t1_y * t2_x; + const T t274 = -t273; + const T t275 = t269 + t270 + t272 + t274; + const T t276 = t258 * t73; + const T t277 = t159 * t74; + const T t278 = 8 * t255; + const T t279 = t278 * t64; + const T t280 = 2 * t234; + const T t281 = t263 * t64; + const T t282 = t280 * t89 - t281 * t89; + const T t283 = -t116 * t264 + t268 * t75; + const T 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 T t285 = p_z + t124; + const T t286 = p_z + t123 + t1_z; + const T t287 = t1_x * t1_z; + const T t288 = t2_x * t2_z; + const T t289 = t1_x * t2_z; + const T t290 = -t289; + const T t291 = t1_z * t2_x; + const T t292 = -t291; + const T t293 = t287 + t288 + t290 + t292; + const T t294 = t119 * t280 - t119 * t281; + const T t295 = -t146 * t264 + t286 * t75; + const T 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 T t297 = -t249; + const T t298 = t236 * t41; + const T t299 = -t157 * t264 + t252; + const T 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 T t301 = t238 * t29; + const T t302 = 2 * t301; + const T t303 = t0_y * t1_x; + const T t304 = -t303; + const T t305 = t1_y * t30; + const T t306 = 2 * t273; + const T t307 = t271 - t306; + const T t308 = t0_y * t2_x; + const T t309 = t2_y * t30; + const T t310 = t308 - t309; + const T t311 = t159 * (t270 + t304 + t305 + t307 + t310); + const T t312 = t238 * t41; + const T t313 = t159 * t260; + const T t314 = t172 * t264; + const T t315 = t253 * t64; + const T t316 = t172 * t253; + const T t317 = t159 * t33; + const T t318 = t172 * t279; + const T 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 T t320 = t240 * t29; + const T t321 = 2 * t320; + const T t322 = t0_z * t1_x; + const T t323 = -t322; + const T t324 = t1_z * t30; + const T t325 = 2 * t291; + const T t326 = t289 - t325; + const T t327 = t0_z * t2_x; + const T t328 = t2_z * t30; + const T t329 = t327 - t328; + const T t330 = t159 * (t288 + t323 + t324 + t326 + t329); + const T t331 = t240 * t41; + const T t332 = t184 * t264; + const T t333 = t184 * t253; + const T t334 = t184 * t279; + const T 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 T t336 = t186 * t264; + const T t337 = -t19 + t78; + const T t338 = -t18 + t81; + const T t339 = t159 * (t337 + t338 + t65); + const T t340 = t159 * t261; + const T t341 = t186 * t279; + const T t342 = -t250; + const T t343 = -t251; + const T t344 = 2 * t237 + 2 * t239 + 2 * t241 + t297 + t342 + t343 + t73; + const T 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 T t346 = -t308; + const T t347 = 2 * t271; + const T t348 = t273 - t347; + const T t349 = t303 - t305; + const T t350 = t159 * (t269 + t309 + t346 + t348 + t349); + const T t351 = t192 * t264; + const T t352 = t192 * t279; + const T 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 T t354 = -t327; + const T t355 = 2 * t289; + const T t356 = t291 - t355; + const T t357 = t322 - t324; + const T t358 = t159 * (t287 + t328 + t354 + t356 + t357); + const T t359 = t196 * t264; + const T t360 = t196 * t279; + const T 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 T t362 = 2 * t89; + const T t363 = t268 * t362; + const T t364 = -t16; + const T t365 = t21 + t24; + const T t366 = t19 + t23; + const T t367 = t28 * (t246 + t364 + t365 + t366); + const T t368 = (t116 * t116) * t256; + const T t369 = t116 * t159; + const T t370 = t267 * t73; + const T t371 = t268 * t41; + const T t372 = t253 * t95; + const T t373 = t263 * t89; + const T t374 = t116 * t373; + const T t375 = t1_y * t1_z; + const T t376 = t2_y * t2_z; + const T t377 = t1_y * t2_z; + const T t378 = -t377; + const T t379 = t1_z * t2_y; + const T t380 = -t379; + const T t381 = t375 + t376 + t378 + t380; + const T t382 = t146 * t159; + const T t383 = t369 * t73; + const T t384 = t116 * t278; + const T t385 = 2 * t119; + const T t386 = t119 * t263; + const T t387 = -t116 * t386 + t268 * t385; + const T t388 = -t146 * t373 + t286 * t362; + const T 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 T t390 = t236 * t89; + const T t391 = 2 * t390; + const T t392 = t0_x * t1_y; + const T t393 = -t392; + const T t394 = t1_x * t92; + const T t395 = t0_x * t2_y; + const T t396 = t2_x * t92; + const T t397 = t395 - t396; + const T t398 = t159 * (t270 + t348 + t393 + t394 + t397); + const T t399 = t158 * t159; + const T t400 = t116 * t253; + const T t401 = t157 * t373; + const T t402 = t157 * t253; + const T t403 = t159 * t95; + const T t404 = t159 * t370; + const T t405 = t157 * t384; + const T 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 T t407 = t365 + t79 + t86; + const T t408 = -t172 * t373 + t249 + t251; + const T 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 T t410 = t240 * t89; + const T t411 = 2 * t410; + const T t412 = t0_z * t1_y; + const T t413 = -t412; + const T t414 = t1_z * t92; + const T t415 = 2 * t379; + const T t416 = t377 - t415; + const T t417 = t0_z * t2_y; + const T t418 = t2_z * t92; + const T t419 = t417 - t418; + const T t420 = t159 * (t376 + t413 + t414 + t416 + t419); + const T t421 = t159 * t371; + const T t422 = t184 * t373; + const T t423 = t184 * t384; + const T 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 T t425 = -t395; + const T t426 = t392 - t394; + const T t427 = t159 * (t269 + t307 + t396 + t425 + t426); + const T t428 = t159 * t372; + const T t429 = t186 * t373; + const T t430 = t186 * t384; + const T 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 T t432 = t192 * t373; + const T t433 = t13 + t15; + const T t434 = -t23 + t85; + const T t435 = t159 * (t337 + t433 + t434); + const T t436 = t192 * t384; + const T 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 T t438 = -t417; + const T t439 = 2 * t377; + const T t440 = t379 - t439; + const T t441 = t412 - t414; + const T t442 = t159 * (t375 + t418 + t438 + t440 + t441); + const T t443 = t196 * t373; + const T t444 = t196 * t384; + const T 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 T t446 = t286 * t385; + const T t447 = t20 + t24; + const T t448 = t18 + t23; + const T t449 = t28 * (t245 + t364 + t447 + t448); + const T t450 = (t146 * t146) * t256; + const T t451 = t285 * t73; + const T t452 = t286 * t41; + const T t453 = t125 * t253; + const T t454 = t146 * t386; + const T t455 = t119 * t236; + const T t456 = 2 * t455; + const T t457 = t0_x * t1_z; + const T t458 = -t457; + const T t459 = t122 * t1_x; + const T t460 = t0_x * t2_z; + const T t461 = t122 * t2_x; + const T t462 = t460 - t461; + const T t463 = t159 * (t288 + t356 + t458 + t459 + t462); + const T t464 = t146 * t253; + const T t465 = t157 * t386; + const T t466 = t125 * t159; + const T t467 = t159 * t451; + const T t468 = t146 * t278; + const T t469 = t157 * t244; + const T t470 = t254 * t468; + const T 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 T t472 = t119 * t238; + const T t473 = 2 * t472; + const T t474 = t0_y * t1_z; + const T t475 = -t474; + const T t476 = t122 * t1_y; + const T t477 = t0_y * t2_z; + const T t478 = t122 * t2_y; + const T t479 = t477 - t478; + const T t480 = t159 * (t376 + t440 + t475 + t476 + t479); + const T t481 = t159 * t452; + const T t482 = t172 * t386; + const T t483 = t244 * t468; + const T 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 T t485 = t447 + t82 + t86; + const T t486 = -t184 * t386 + t249 + t250; + const T 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 T t488 = -t460; + const T t489 = t457 - t459; + const T t490 = t159 * (t287 + t326 + t461 + t488 + t489); + const T t491 = t382 * t73; + const T t492 = t159 * t453; + const T t493 = t186 * t386; + const T 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 T t495 = -t477; + const T t496 = t474 - t476; + const T t497 = t159 * (t375 + t416 + t478 + t495 + t496); + const T t498 = t192 * t386; + const T 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 T t500 = t196 * t386; + const T t501 = t11 + t15; + const T t502 = t159 * (t338 + t434 + t501); + const T 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 T t504 = (t157 * t157); + const T t505 = 2 * t255; + const T t506 = t0_x * t0_y; + const T t507 = t270 + t346 + t425 + t506; + const T t508 = t172 * t262; + const T 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 T t510 = t0_x * t0_z; + const T t511 = t288 + t354 + t488 + t510; + const T 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 T t513 = t159 * t298; + const T t514 = t159 * t70; + const T t515 = t159 * t73; + const T t516 = t157 * t515; + const T t517 = t253 * t76; + const T t518 = t186 * t278; + const T t519 = t157 * t254; + const T t520 = t28 + * (t160 * t402 - t186 * t513 + t186 * t517 + t236 * t516 - t244 * t514 + + t254 * t514 + t299 - t469 * t518 + t518 * t519); + const T t521 = t159 * (t274 + t310 + t347 + t426 + t506); + const T t522 = t192 * t278; + const T t523 = t28 + * (-t192 * t513 + t192 * t517 + t208 * t402 + t238 * t516 + t244 * t521 + - t254 * t521 + t301 - t391 - t401 - t469 * t522 + t519 * t522); + const T t524 = t159 * (t292 + t329 + t355 + t489 + t510); + const T t525 = t196 * t278; + const T t526 = t28 + * (-t196 * t513 + t196 * t517 + t227 * t402 + t240 * t516 + t244 * t524 + - t254 * t524 + t320 - t456 - t465 - t469 * t525 + t519 * t525); + const T t527 = t365 + t37 + t40; + const T t528 = (t172 * t172); + const T t529 = t0_y * t0_z; + const T t530 = t376 + t438 + t495 + t529; + const T 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 T t532 = t159 * (t272 + t306 + t349 + t397 + t506); + const T t533 = t159 * t312; + const T t534 = t199 * t253; + const T t535 = t172 * t515; + const T t536 = t172 * t518; + const T t537 = t28 + * (t160 * t316 - t186 * t533 + t186 * t534 + t236 * t535 + t244 * t532 + - t244 * t536 - t254 * t532 + t254 * t536 - t302 - t314 + t390); + const T t538 = t159 * (t433 + t67 + t72); + const T t539 = t172 * t522; + const T t540 = t28 + * (-t192 * t533 + t192 * t534 + t208 * t316 + t238 * t535 - t244 * t538 + - t244 * t539 + t254 * t538 + t254 * t539 + t408); + const T t541 = t159 * (t380 + t419 + t439 + t496 + t529); + const T t542 = t172 * t525; + const T t543 = t28 + * (-t196 * t533 + t196 * t534 + t227 * t316 + t240 * t535 + t244 * t541 + - t244 * t542 - t254 * t541 + t254 * t542 + t410 - t473 - t482); + const T t544 = t38 + t40 + t447; + const T t545 = (t184 * t184); + const T t546 = t159 * (t290 + t325 + t357 + t462 + t510); + const T t547 = t159 * t331; + const T t548 = t218 * t253; + const T t549 = t184 * t515; + const T t550 = t184 * t518; + const T t551 = t28 + * (t160 * t333 - t186 * t547 + t186 * t548 + t236 * t549 + t244 * t546 + - t244 * t550 - t254 * t546 + t254 * t550 - t321 - t332 + t455); + const T t552 = t159 * (t378 + t415 + t441 + t479 + t529); + const T t553 = t184 * t522; + const T t554 = t28 + * (-t192 * t547 + t192 * t548 + t208 * t333 + t238 * t549 + t244 * t552 + - t244 * t553 - t254 * t552 + t254 * t553 - t411 - t422 + t472); + const T t555 = t159 * (t501 + t69 + t72); + const T t556 = t184 * t525; + const T t557 = t28 + * (-t196 * t547 + t196 * t548 + t227 * t333 + t240 * t549 - t244 * t555 + - t244 * t556 + t254 * t555 + t254 * t556 + t486); + const T t558 = t25 - t8; + const T t559 = -t2 + t22; + const T t560 = t247 + t558 + t559; + const T t561 = (t186 * t186); + const T t562 = t159 * (t269 + t304 + t393 + t506); + const T t563 = t160 * t253; + const T t564 = t186 * t253; + const T t565 = t236 * t515; + const T t566 = t186 * t515; + const T t567 = t192 * t518; + const T 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 T t569 = t159 * (t287 + t323 + t458 + t510); + const T t570 = t196 * t518; + const T 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 T t572 = t17 - t26; + const T t573 = t366 + t558 + t572; + const T t574 = (t192 * t192); + const T t575 = t159 * (t375 + t413 + t475 + t529); + const T t576 = t196 * t522; + const T 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 T t578 = t448 + t559 + t572; + const T t579 = (t196 * t196); + 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); } -std::array point_triangle_closest_point_hessian( - Eigen::ConstRef p, - Eigen::ConstRef t0, - Eigen::ConstRef t1, - Eigen::ConstRef t2) +// hess is (144×1) flattened in column-major order +template +void point_triangle_closest_point_hessian_1( + T p_x, + T p_y, + T p_z, + T t0_x, + T t0_y, + T t0_z, + T t1_x, + T t1_y, + T t1_z, + T t2_x, + T t2_y, + T t2_z, + T hess[144]) { - std::array H; - autogen::point_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::point_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()); - return H; + const T t0 = t0_y * t1_y; + const T t1 = t0_x * t1_x; + const T t2 = 2 * t1; + const T t3 = t0_y * t2_y; + const T t4 = t0_x * t2_x; + const T t5 = 2 * t0; + const T t6 = 2 * t3; + const T t7 = t0_z * t1_z; + const T t8 = t0_z * t2_z; + const T t9 = 2 * t7; + const T t10 = 2 * t8; + const T t11 = t1_y * t2_y; + const T t12 = t1_z * t2_z; + const T t13 = 2 * t11; + const T t14 = 2 * t12; + const T t15 = t1_x * t2_x; + const T t16 = 2 * t15; + const T t17 = (t0_x * t0_x); + const T t18 = (t1_y * t1_y); + const T t19 = (t1_z * t1_z); + const T t20 = (t2_y * t2_y); + const T t21 = (t2_z * t2_z); + const T t22 = (t0_y * t0_y); + const T t23 = (t1_x * t1_x); + const T t24 = (t2_x * t2_x); + const T t25 = (t0_z * t0_z); + const T t26 = 2 * t4; + const T 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 T t28 = T(1.0) / t27; + const T t29 = t0_x - t1_x; + const T t30 = 2 * t0_x; + const T t31 = -t30; + const T t32 = t1_x + t31; + const T t33 = t2_x + t32; + const T t34 = t0_x - t2_x; + const T t35 = t29 * t34; + const T t36 = t18 + t19; + const T t37 = t25 - t9; + const T t38 = t22 - t5; + const T t39 = t36 + t37 + t38; + const T t40 = t17 - t2; + const T t41 = t23 + t39 + t40; + const T t42 = t0_x * t18; + const T t43 = t0_x * t19; + const T t44 = t0_x * t20; + const T t45 = t0_x * t21; + const T t46 = t1_x * t20; + const T t47 = t1_x * t21; + const T t48 = t18 * t2_x; + const T t49 = t19 * t2_x; + const T t50 = t11 * t1_x; + const T t51 = t12 * t1_x; + const T t52 = t11 * t2_x; + const T t53 = t12 * t2_x; + const T t54 = t0 * t1_x; + const T t55 = t2_x * t3; + const T t56 = t1_x * t7; + const T t57 = t2_x * t8; + const T t58 = t0 * t2_x; + const T t59 = t2_x * t7; + const T t60 = t58 + t59; + const T t61 = t1_x * t3; + const T t62 = t1_x * t8; + const T t63 = t61 + t62; + const T t64 = -t11 * t30 - t12 * t30 + t42 + t43 + t44 + t45 - t46 - t47 + - t48 - t49 + t50 + t51 + t52 + t53 - t54 - t55 - t56 - t57 + t60 + t63; + const T t65 = -t0; + const T t66 = -t7; + const T t67 = t65 + t66; + const T t68 = -t8; + const T t69 = t12 + t25 + t68; + const T t70 = -t3; + const T t71 = t11 + t22 + t70; + const T t72 = t67 + t69 + t71; + const T t73 = -t4; + const T t74 = -t1; + const T t75 = t15 + t17 + t73 + t74; + const T t76 = t72 + t75; + const T t77 = t64 * t76; + const T t78 = 2 * t29; + const T t79 = t28 * t78; + const T t80 = -t15; + const T t81 = -t11; + const T t82 = -t12; + const T t83 = t81 + t82; + const T t84 = t3 + t4 + t8 + t80 + t83; + const T t85 = t23 + t36 + t67 + t74 + t84; + const T t86 = t28 + * (2 * t28 * t34 * t41 * t64 - t29 * t33 - 2 * t35 - t77 * t79 - t85); + const T t87 = 2 * t0_y; + const T t88 = -t87; + const T t89 = t1_y + t88; + const T t90 = t2_y + t89; + const T t91 = t0_y - t1_y; + const T t92 = t34 * t91; + const T t93 = 2 * t92; + const T t94 = t0_y * t23; + const T t95 = t0_y * t19; + const T t96 = t0_y * t24; + const T t97 = t0_y * t21; + const T t98 = t1_y * t24; + const T t99 = t1_y * t21; + const T t100 = t23 * t2_y; + const T t101 = t19 * t2_y; + const T t102 = t15 * t1_y; + const T t103 = t15 * t2_y; + const T t104 = t12 * t1_y; + const T t105 = t12 * t2_y; + const T t106 = t1 * t1_y; + const T t107 = t2_y * t4; + const T t108 = t1_y * t7; + const T t109 = t2_y * t8; + const T t110 = t1 * t2_y + t2_y * t7; + const T t111 = t1_y * t4; + const T t112 = t1_y * t8; + const T t113 = t111 + t112; + const T t114 = -t100 - t101 + t102 + t103 + t104 + t105 - t106 - t107 - t108 + - t109 + t110 + t113 - t12 * t87 - t15 * t87 + t94 + t95 + t96 + t97 + - t98 - t99; + const T t115 = t76 * t79; + const T t116 = + t28 * (-t114 * t115 + 2 * t114 * t28 * t34 * t41 - t29 * t90 - t93); + const T t117 = 2 * t0_z; + const T t118 = -t117; + const T t119 = t118 + t1_z; + const T t120 = t119 + t2_z; + const T t121 = t0_z - t1_z; + const T t122 = t121 * t34; + const T t123 = 2 * t122; + const T t124 = t0_z * t23; + const T t125 = t0_z * t18; + const T t126 = t0_z * t24; + const T t127 = t0_z * t20; + const T t128 = t1_z * t24; + const T t129 = t1_z * t20; + const T t130 = t23 * t2_z; + const T t131 = t18 * t2_z; + const T t132 = t15 * t1_z; + const T t133 = t15 * t2_z; + const T t134 = t11 * t1_z; + const T t135 = t11 * t2_z; + const T t136 = t1 * t1_z; + const T t137 = t2_z * t4; + const T t138 = t0 * t1_z; + const T t139 = t2_z * t3; + const T t140 = t0 * t2_z + t1 * t2_z; + const T t141 = t1_z * t4; + const T t142 = t1_z * t3; + const T t143 = t141 + t142; + const T t144 = -t11 * t117 - t117 * t15 + t124 + t125 + t126 + t127 - t128 + - t129 - t130 - t131 + t132 + t133 + t134 + t135 - t136 - t137 - t138 + - t139 + t140 + t143; + const T t145 = + t28 * (-t115 * t144 - t120 * t29 - t123 + 2 * t144 * t28 * t34 * t41); + const T t146 = t1_x * t22; + const T t147 = t1_x * t25; + const T t148 = t22 * t2_x; + const T t149 = t25 * t2_x; + const T t150 = t0_x * t3; + const T t151 = t0_x * t8; + const T t152 = t0 * t0_x; + const T t153 = t0_x * t7; + const T t154 = t0_x * t11 + t0_x * t12; + const T t155 = t146 + t147 - t148 - t149 + t150 + t151 - t152 - t153 + t154 + - t44 - t45 + t46 + t47 - t52 - t53 + t55 + t57 + t60 - 2 * t61 + - 2 * t62; + const T t156 = 2 * t28; + const T t157 = t156 * t34; + const T t158 = t0 + t1 - t17 - t22 - t25 + t7 + t84; + const T t159 = t28 * (-t115 * t155 + t155 * t157 * t41 + t158 + t35); + const T t160 = t0_y - t2_y; + const T t161 = t160 * t29; + const T t162 = t17 * t1_y; + const T t163 = t1_y * t25; + const T t164 = t17 * t2_y; + const T t165 = t25 * t2_y; + const T t166 = t0_y * t4; + const T t167 = t0_y * t8; + const T t168 = t0_y * t1; + const T t169 = t0_y * t7; + const T t170 = t0_y * t12 + t0_y * t15; + const T t171 = -t103 - t105 + t107 + t109 + t110 - 2 * t111 - 2 * t112 + + t162 + t163 - t164 - t165 + t166 + t167 - t168 - t169 + t170 - t96 + - t97 + t98 + t99; + const T t172 = + t28 * (-t115 * t171 - t161 + 2 * t171 * t28 * t34 * t41 + t93); + const T t173 = t0_z - t2_z; + const T t174 = t173 * t29; + const T t175 = t17 * t1_z; + const T t176 = t1_z * t22; + const T t177 = t17 * t2_z; + const T t178 = t22 * t2_z; + const T t179 = t0_z * t4; + const T t180 = t0_z * t3; + const T t181 = t0_z * t1; + const T t182 = t0 * t0_z; + const T t183 = t0_z * t11 + t0_z * t15; + const T t184 = -t126 - t127 + t128 + t129 - t133 - t135 + t137 + t139 + t140 + - 2 * t141 - 2 * t142 + t175 + t176 - t177 - t178 + t179 + t180 - t181 + - t182 + t183; + const T t185 = + t28 * (-t115 * t184 + t123 - t174 + 2 * t184 * t28 * t34 * t41); + const T t186 = -t146 - t147 + t148 + t149 - t150 - t151 + t152 + t153 + t154 + - t42 - t43 + t48 + t49 - t50 - t51 + t54 + t56 - 2 * t58 - 2 * t59 + + t63; + const T t187 = t186 * t41; + const T t188 = t28 * (-t115 * t186 + t157 * t187 - (t29 * t29) + t41); + const T t189 = t29 * t91; + const T 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 T t191 = t28 * (-t115 * t190 - t189 + 2 * t190 * t28 * t34 * t41); + const T t192 = t121 * t29; + const T 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 T t194 = t28 * (-t115 * t193 - t192 + 2 * t193 * t28 * t34 * t41); + const T t195 = 2 * t161; + const T t196 = t156 * t91; + const T t197 = + t28 * (2 * t160 * t28 * t41 * t64 - t195 - t196 * t77 - t33 * t91); + const T t198 = t160 * t91; + const T t199 = t196 * t76; + const T t200 = t28 + * (2 * t114 * t160 * t28 * t41 - t114 * t199 - 2 * t198 - t85 + - t90 * t91); + const T t201 = t121 * t160; + const T t202 = 2 * t201; + const T t203 = + t28 * (-t120 * t91 + 2 * t144 * t160 * t28 * t41 - t144 * t199 - t202); + const T t204 = t156 * t160; + const T t205 = t204 * t41; + const T t206 = t28 * (-t155 * t199 + t155 * t205 + t195 - t92); + const T t207 = t28 * (t158 - t171 * t199 + t171 * t205 + t198); + const T t208 = t173 * t91; + const T t209 = + t28 * (2 * t160 * t184 * t28 * t41 - t184 * t199 + t202 - t208); + const T t210 = t28 * (2 * t160 * t186 * t28 * t41 - t186 * t199 - t189); + const T t211 = t28 * (-t190 * t199 + t190 * t205 + t41 - (t91 * t91)); + const T t212 = t121 * t91; + const T t213 = t28 * (2 * t160 * t193 * t28 * t41 - t193 * t199 - t212); + const T t214 = 2 * t174; + const T t215 = t121 * t156; + const T t216 = + t28 * (-t121 * t33 + 2 * t173 * t28 * t41 * t64 - t214 - t215 * t77); + const T t217 = 2 * t208; + const T t218 = t215 * t76; + const T t219 = + t28 * (2 * t114 * t173 * t28 * t41 - t114 * t218 - t121 * t90 - t217); + const T t220 = t121 * t173; + const T t221 = t28 + * (-t120 * t121 + 2 * t144 * t173 * t28 * t41 - t144 * t218 - 2 * t220 + - t85); + const T t222 = t156 * t173; + const T t223 = t222 * t41; + const T t224 = t28 * (-t122 - t155 * t218 + t155 * t223 + t214); + const T t225 = t28 * (-t171 * t218 + t171 * t223 - t201 + t217); + const T t226 = t28 * (t158 - t184 * t218 + t184 * t223 + t220); + const T t227 = t28 * (2 * t173 * t186 * t28 * t41 - t186 * t218 - t192); + const T t228 = t28 * (2 * t173 * t190 * t28 * t41 - t190 * t218 - t212); + const T t229 = t28 * (-(t121 * t121) - t193 * t218 + t193 * t223 + t41); + const T t230 = p_x + t32; + const T t231 = p_x - t0_x; + const T t232 = t231 * t29; + const T t233 = p_y - t0_y; + const T t234 = t233 * t91; + const T t235 = p_z - t0_z; + const T t236 = t121 * t235; + const T t237 = t234 + t236; + const T t238 = t232 + t237; + const T t239 = t238 * t76; + const T t240 = -t13; + const T t241 = -t14; + const T t242 = t20 + t21; + const T t243 = t240 + t241 + t242 + t36; + const T t244 = t231 * t34; + const T t245 = t160 * t233; + const T t246 = t173 * t235; + const T t247 = t245 + t246; + const T t248 = t244 + t247; + const T t249 = (t64 * t64); + const T t250 = 1 / (t27 * t27); + const T t251 = p_x + t2_x + t31; + const T t252 = t156 * t230; + const T t253 = t248 * t41; + const T t254 = 4 * t250; + const T t255 = 4 * t28; + const T t256 = t248 * t255; + const T t257 = t256 * t29; + const T t258 = -t232; + const T t259 = -t234; + const T t260 = -t236; + const T t261 = t258 + t259 + t260; + const T t262 = t251 * t78 - t257 * t64 + t261; + const T t263 = t68 + t7; + const T t264 = t0 + t70; + const T t265 = t263 + t264; + const T t266 = t12 - t19; + const T t267 = t11 - t18; + const T t268 = t265 + t266 + t267; + const T t269 = t1 + t73; + const T t270 = t15 - t23; + const T t271 = t248 + t268 + t269 + t270; + const T t272 = p_y + t89; + const T t273 = p_y + t2_y + t88; + const T t274 = t1_x * t1_y; + const T t275 = t2_x * t2_y; + const T t276 = t1_x * t2_y; + const T t277 = -t276; + const T t278 = t1_y * t2_x; + const T t279 = -t278; + const T t280 = t274 + t275 + t277 + t279; + const T t281 = t252 * t76; + const T t282 = t156 * t77; + const T t283 = 8 * t250; + const T t284 = t283 * t64; + const T t285 = 2 * t251; + const T t286 = t256 * t64; + const T t287 = t285 * t91 - t286 * t91; + const T t288 = -t114 * t257 + t273 * t78; + const T 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 T t290 = p_z + t119; + const T t291 = p_z + t118 + t2_z; + const T t292 = t1_x * t1_z; + const T t293 = t2_x * t2_z; + const T t294 = t1_x * t2_z; + const T t295 = -t294; + const T t296 = t1_z * t2_x; + const T t297 = -t296; + const T t298 = t292 + t293 + t295 + t297; + const T t299 = t121 * t285 - t121 * t286; + const T t300 = -t144 * t257 + t291 * t78; + const T 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 T t302 = t155 * t257; + const T t303 = t156 * (t242 + t265 + t83); + const T t304 = t238 * t64; + const T t305 = t251 * t41; + const T t306 = t155 * t156; + const T t307 = t238 * t33; + const T t308 = t155 * t284; + const T t309 = 2 * t244 + 2 * t245 + 2 * t246 + t76; + const T 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 T t311 = t0_y * t1_x; + const T t312 = -t311; + const T t313 = t1_y * t30; + const T t314 = 2 * t278; + const T t315 = t276 - t314; + const T t316 = t0_y * t2_x; + const T t317 = t2_y * t30; + const T t318 = t316 - t317; + const T t319 = t156 * (t275 + t312 + t313 + t315 + t318); + const T t320 = t156 * t171; + const T t321 = t171 * t257; + const T t322 = t171 * t284; + const T 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 T t324 = t0_z * t1_x; + const T t325 = -t324; + const T t326 = t1_z * t30; + const T t327 = 2 * t296; + const T t328 = t294 - t327; + const T t329 = t0_z * t2_x; + const T t330 = t2_z * t30; + const T t331 = t329 - t330; + const T t332 = t156 * (t293 + t325 + t326 + t328 + t331); + const T t333 = t156 * t184; + const T t334 = t184 * t257; + const T t335 = t184 * t284; + const T 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 T t337 = t231 * t41; + const T t338 = t156 * t64; + const T t339 = -t186 * t257 + t237; + const T 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 T t341 = t233 * t29; + const T t342 = 2 * t341; + const T t343 = -t316; + const T t344 = 2 * t276; + const T t345 = t278 - t344; + const T t346 = t311 - t313; + const T t347 = t156 * (t274 + t317 + t343 + t345 + t346); + const T t348 = t233 * t41; + const T t349 = t156 * t305; + const T t350 = t190 * t257; + const T t351 = t190 * t238; + const T t352 = t156 * t33; + const T t353 = t190 * t284; + const T 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 T t355 = t235 * t29; + const T t356 = 2 * t355; + const T t357 = -t329; + const T t358 = 2 * t294; + const T t359 = t296 - t358; + const T t360 = t324 - t326; + const T t361 = t156 * (t292 + t330 + t357 + t359 + t360); + const T t362 = t235 * t41; + const T t363 = t193 * t257; + const T t364 = t193 * t238; + const T t365 = t193 * t284; + const T 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 T t367 = -t16; + const T t368 = t21 + t24; + const T t369 = t19 + t23; + const T t370 = t241 + t367 + t368 + t369; + const T t371 = (t114 * t114); + const T t372 = t114 * t156; + const T t373 = t272 * t76; + const T t374 = t261 + t271; + const T t375 = 2 * t91; + const T t376 = t256 * t91; + const T t377 = -t114 * t376 + t273 * t375; + const T t378 = t1_y * t1_z; + const T t379 = t2_y * t2_z; + const T t380 = t1_y * t2_z; + const T t381 = -t380; + const T t382 = t1_z * t2_y; + const T t383 = -t382; + const T t384 = t378 + t379 + t381 + t383; + const T t385 = t144 * t156; + const T t386 = t372 * t76; + const T t387 = t114 * t283; + const T t388 = 2 * t121; + const T t389 = t121 * t256; + const T t390 = -t114 * t389 + t273 * t388; + const T t391 = -t144 * t376 + t291 * t375; + const T 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 T t393 = t0_x * t1_y; + const T t394 = -t393; + const T t395 = t1_x * t87; + const T t396 = t0_x * t2_y; + const T t397 = t2_x * t87; + const T t398 = t396 - t397; + const T t399 = t156 * (t275 + t345 + t394 + t395 + t398); + const T t400 = t114 * t238; + const T t401 = t273 * t41; + const T t402 = t238 * t90; + const T t403 = t155 * t376; + const T t404 = t155 * t387; + const T 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 T t406 = t171 * t376; + const T t407 = t263 + t269; + const T t408 = t156 * (t368 + t407 + t80 + t82); + const T t409 = t171 * t387; + const T t410 = t261 + t309; + const T 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 T t412 = t0_z * t1_y; + const T t413 = -t412; + const T t414 = t1_z * t87; + const T t415 = 2 * t382; + const T t416 = t380 - t415; + const T t417 = t0_z * t2_y; + const T t418 = t2_z * t87; + const T t419 = t417 - t418; + const T t420 = t156 * (t379 + t413 + t414 + t416 + t419); + const T t421 = t184 * t376; + const T t422 = t184 * t387; + const T 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 T t424 = t231 * t91; + const T t425 = 2 * t424; + const T t426 = -t396; + const T t427 = t393 - t395; + const T t428 = t156 * (t274 + t315 + t397 + t426 + t427); + const T t429 = t156 * t187; + const T t430 = t186 * t376; + const T t431 = t186 * t238; + const T t432 = t156 * t90; + const T t433 = t156 * t373; + const T t434 = t186 * t387; + const T 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 T t436 = t266 + t270 + t407; + const T t437 = -t190 * t376 + t232 + t236; + const T 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 T t439 = t235 * t91; + const T t440 = 2 * t439; + const T t441 = -t417; + const T t442 = 2 * t380; + const T t443 = t382 - t442; + const T t444 = t412 - t414; + const T t445 = t156 * (t378 + t418 + t441 + t443 + t444); + const T t446 = t193 * t376; + const T t447 = t193 * t387; + const T 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 T t449 = t20 + t24; + const T t450 = t18 + t23; + const T t451 = t240 + t367 + t449 + t450; + const T t452 = (t144 * t144); + const T t453 = t290 * t76; + const T t454 = -t144 * t389 + t291 * t388; + const T t455 = t0_x * t1_z; + const T t456 = -t455; + const T t457 = t117 * t1_x; + const T t458 = t0_x * t2_z; + const T t459 = t117 * t2_x; + const T t460 = t458 - t459; + const T t461 = t156 * (t293 + t359 + t456 + t457 + t460); + const T t462 = t385 * t76; + const T t463 = t144 * t238; + const T t464 = t291 * t41; + const T t465 = t120 * t238; + const T t466 = t155 * t389; + const T t467 = t144 * t283; + const T t468 = t155 * t467; + const T 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 T t470 = t0_y * t1_z; + const T t471 = -t470; + const T t472 = t117 * t1_y; + const T t473 = t0_y * t2_z; + const T t474 = t117 * t2_y; + const T t475 = t473 - t474; + const T t476 = t156 * (t379 + t443 + t471 + t472 + t475); + const T t477 = t171 * t389; + const T t478 = t171 * t467; + const T 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 T t480 = t184 * t389; + const T t481 = t264 + t269; + const T t482 = t156 * (t449 + t481 + t80 + t81); + const T t483 = t184 * t467; + const T 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 T t485 = t121 * t231; + const T t486 = 2 * t485; + const T t487 = -t458; + const T t488 = t455 - t457; + const T t489 = t156 * (t292 + t328 + t459 + t487 + t488); + const T t490 = t186 * t389; + const T t491 = t120 * t156; + const T t492 = t156 * t453; + const T t493 = t186 * t467; + const T 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 T t495 = t121 * t233; + const T t496 = 2 * t495; + const T t497 = -t473; + const T t498 = t470 - t472; + const T t499 = t156 * (t378 + t416 + t474 + t497 + t498); + const T t500 = t190 * t389; + const T t501 = t190 * t467; + const T 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 T t503 = t267 + t270 + t481; + const T t504 = -t193 * t389 + t232 + t234; + const T 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 T t506 = -t10 + t25; + const T t507 = t22 - t6; + const T t508 = t242 + t506 + t507; + const T t509 = (t155 * t155); + const T t510 = t0_x * t0_y; + const T t511 = t156 * (t275 + t343 + t426 + t510); + const T t512 = t157 * t238; + const T t513 = t155 * t238; + const T t514 = t231 * t76; + const T t515 = t306 * t76; + const T t516 = t155 * t283; + const T t517 = t171 * t516; + const T 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 T t519 = t0_x * t0_z; + const T t520 = t156 * (t293 + t357 + t487 + t519); + const T t521 = t184 * t516; + const T 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 T t523 = t156 * t72; + const T t524 = t156 * t514; + const T t525 = t186 * t516; + const T t526 = t28 + * (t157 * t431 + t186 * t524 + t239 * t523 + t239 * t525 - t253 * t523 + - t253 * t525 - t306 * t337 + t339 + t513 * t79); + const T t527 = t156 * (t279 + t318 + t344 + t427 + t510); + const T t528 = t190 * t516; + const T t529 = t28 + * (t157 * t351 + t190 * t524 + t196 * t513 - t239 * t527 + t239 * t528 + + t253 * t527 - t253 * t528 - t306 * t348 - t342 - t350 + t424); + const T t530 = t156 * (t297 + t331 + t358 + t488 + t519); + const T t531 = t193 * t516; + const T t532 = t28 + * (t157 * t364 + t193 * t524 + t215 * t513 - t239 * t530 + t239 * t531 + + t253 * t530 - t253 * t531 - t306 * t362 - t356 - t363 + t485); + const T t533 = t17 - t26; + const T t534 = t368 + t506 + t533; + const T t535 = (t171 * t171); + const T t536 = t0_y * t0_z; + const T t537 = t156 * (t379 + t441 + t497 + t536); + const T t538 = t171 * t238; + const T t539 = t233 * t76; + const T t540 = t235 * t76; + const T t541 = t171 * t283; + const T t542 = t184 * t541; + const T 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 T t544 = t156 * (t277 + t314 + t346 + t398 + t510); + const T t545 = t156 * t539; + const T t546 = t186 * t541; + const T t547 = t28 + * (t186 * t545 + t204 * t431 - t239 * t544 + t239 * t546 + t253 * t544 + - t253 * t546 - t320 * t337 + t341 - t425 - t430 + t538 * t79); + const T t548 = t156 * (t66 + t69 + t75); + const T t549 = t190 * t541; + const T t550 = t28 + * (t190 * t545 + t196 * t538 + t204 * t351 + t239 * t548 + t239 * t549 + - t253 * t548 - t253 * t549 - t320 * t348 + t437); + const T t551 = t156 * (t383 + t419 + t442 + t498 + t536); + const T t552 = t193 * t541; + const T t553 = t28 + * (t193 * t545 + t204 * t364 + t215 * t538 - t239 * t551 + t239 * t552 + + t253 * t551 - t253 * t552 - t320 * t362 - t440 - t446 + t495); + const T t554 = t449 + t507 + t533; + const T t555 = (t184 * t184); + const T t556 = t156 * (t295 + t327 + t360 + t460 + t519); + const T t557 = t184 * t238; + const T t558 = t156 * t540; + const T t559 = t184 * t283; + const T t560 = t186 * t253; + const T t561 = t239 * t559; + const T t562 = t28 + * (t186 * t558 + t186 * t561 + t222 * t431 - t239 * t556 + t253 * t556 + - t333 * t337 + t355 - t486 - t490 + t557 * t79 - t559 * t560); + const T t563 = t156 * (t381 + t415 + t444 + t475 + t536); + const T t564 = t253 * t559; + const T t565 = t28 + * (t190 * t558 + t190 * t561 - t190 * t564 + t196 * t557 + t222 * t351 + - t239 * t563 + t253 * t563 - t333 * t348 + t439 - t496 - t500); + const T t566 = t156 * (t65 + t71 + t75); + const T t567 = t28 + * (t193 * t558 + t193 * t561 - t193 * t564 + t215 * t557 + t222 * t364 + + t239 * t566 - t253 * t566 - t333 * t362 + t504); + const T t568 = (t186 * t186); + const T t569 = 2 * t250; + const T t570 = t274 + t312 + t394 + t510; + const T t571 = t190 * t255; + const T 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 T t573 = t292 + t325 + t456 + t519; + const T 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 T t575 = t369 + t37 + t40; + const T t576 = (t190 * t190); + const T t577 = t378 + t413 + t471 + t536; + const T 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 T t579 = t38 + t40 + t450; + const T t580 = (t193 * t193); + 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); } -// ============================================================================ - -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 double t0 = e0_x - e1_x; - const double t1 = t0 * t0; - const double t2 = e0_y - e1_y; - const double t3 = t2 * t2; - const double t4 = t1 + t3; - const double t5 = 1.0 / t4; - const double t6 = 2 * t5; - const double t7 = t1 * t6 - 1; - const double t8 = t5 * t7; - const double t9 = t5 * t5; - const double t10 = 2 * t9; - const double t11 = t0 * t2; - const double t12 = t10 * t11; - const double t13 = -t5 * t7; - const double t14 = -t12; - const double t15 = t3 * t6 - 1; - const double t16 = t15 * t5; - const double t17 = -t15 * t5; - const double t18 = -2 * e0_x + e1_x + p_x; - const double t19 = t0 * t18 * t6; - const double t20 = e0_x - p_x; - const double t21 = t0 * t20; - const double t22 = e0_y - p_y; - const double t23 = t2 * t22; - const double t24 = t21 + t23; - const double t25 = t24 * t9; - const double t26 = t24 * t5; - const double t27 = 1 - t26; - const double t28 = -2 * e0_y + e1_y + p_y; - const double t29 = t0 * t28; - const double t30 = 4 * t11 * t26; - const double t31 = t18 * t2 + t30; - const double t32 = t10 * (t29 + t31); - const double t33 = 8 * t25; - const double t34 = -2 * t24 * t5 + 1; - const double t35 = t5 * (2 * t0 * t20 * t5 - t1 * t33 - t19 - t34); - const double t36 = t0 * t22; - const double t37 = t10 * (-t31 + t36); - const double t38 = t2 * t28 * t6; - const double t39 = t2 * t20; - const double t40 = t10 * (-t29 - t30 + t39); - const double t41 = t5 * (2 * t2 * t22 * t5 - t3 * t33 - t34 - t38); - const double 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 double t0 = e0_x - e1_x; - const double t1 = t0 * t0; - const double t2 = e0_y - e1_y; - const double t3 = t2 * t2; - const double t4 = e0_z - e1_z; - const double t5 = t4 * t4; - const double t6 = t1 + t3 + t5; - const double t7 = 1.0 / t6; - const double t8 = 2 * t7; - const double t9 = t1 * t8 - 1; - const double t10 = t7 * t9; - const double t11 = t7 * t7; - const double t12 = 2 * t11; - const double t13 = t0 * t12; - const double t14 = t13 * t2; - const double t15 = t13 * t4; - const double t16 = -t7 * t9; - const double t17 = -t14; - const double t18 = -t15; - const double t19 = t3 * t8 - 1; - const double t20 = t19 * t7; - const double t21 = t2 * t4; - const double t22 = t12 * t21; - const double t23 = -t19 * t7; - const double t24 = -t22; - const double t25 = t5 * t8 - 1; - const double t26 = t25 * t7; - const double t27 = -t25 * t7; - const double t28 = -2 * e0_x + e1_x + p_x; - const double t29 = t0 * t28 * t8; - const double t30 = e0_x - p_x; - const double t31 = t0 * t30; - const double t32 = e0_y - p_y; - const double t33 = t2 * t32; - const double t34 = e0_z - p_z; - const double t35 = t34 * t4; - const double t36 = t33 + t35; - const double t37 = t31 + t36; - const double t38 = t11 * t37; - const double t39 = t37 * t7; - const double t40 = 1 - t39; - const double t41 = -2 * e0_y + e1_y + p_y; - const double t42 = t0 * t41; - const double t43 = 4 * t39; - const double t44 = t0 * t43; - const double t45 = t2 * t44; - const double t46 = t2 * t28 + t45; - const double t47 = t12 * (t42 + t46); - const double t48 = -2 * e0_z + e1_z + p_z; - const double t49 = t0 * t48; - const double t50 = t4 * t44; - const double t51 = t28 * t4 + t50; - const double t52 = t12 * (t49 + t51); - const double t53 = 8 * t38; - const double t54 = -2 * t37 * t7 + 1; - const double t55 = t7 * (2 * t0 * t30 * t7 - t1 * t53 - t29 - t54); - const double t56 = t0 * t32; - const double t57 = t12 * (-t46 + t56); - const double t58 = t0 * t34; - const double t59 = t12 * (-t51 + t58); - const double t60 = t2 * t41 * t8; - const double t61 = 4 * t38; - const double t62 = t2 * t48; - const double t63 = t21 * t43; - const double t64 = t4 * t41 + t63; - const double t65 = t12 * (t62 + t64); - const double t66 = t2 * t30; - const double t67 = t12 * (-t42 - t45 + t66); - const double t68 = t7 * (2 * t2 * t32 * t7 - t3 * t53 - t54 - t60); - const double t69 = t2 * t34; - const double t70 = t12 * (-t64 + t69); - const double t71 = t4 * t48 * t8; - const double t72 = t30 * t4; - const double t73 = t12 * (-t49 - t50 + t72); - const double t74 = t32 * t4; - const double t75 = t12 * (-t62 - t63 + t74); - const double t76 = t7 * (2 * t34 * t4 * t7 - t5 * t53 - t54 - t71); - const double t77 = t12 * (t45 - t56 - t66); - const double t78 = t12 * (t50 - t58 - t72); - const double 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, - 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 J[24]) - { - const double t0 = ea0_y * eb0_y; - const double t1 = ea0_x * eb0_x; - const double t2 = 2 * t1; - const double t3 = ea0_y * eb1_y; - const double t4 = ea0_x * eb1_x; - const double t5 = 2 * t4; - const double t6 = ea0_z * eb0_z; - const double t7 = ea0_z * eb1_z; - const double t8 = eb0_y * eb1_y; - const double t9 = 4 * t8; - const double t10 = ea0_x * ea1_x; - const double t11 = eb0_z * eb1_z; - const double t12 = 4 * t11; - const double t13 = ea1_y * eb0_y; - const double t14 = ea1_y * eb1_y; - const double t15 = ea1_z * eb0_z; - const double t16 = ea1_z * eb1_z; - const double t17 = 2 * t0; - const double t18 = 2 * t3; - const double t19 = ea1_x * eb0_x; - const double t20 = ea1_x * eb1_x; - const double t21 = ea0_y * ea1_y; - const double t22 = eb0_x * eb1_x; - const double t23 = 4 * t22; - const double t24 = 2 * t6; - const double t25 = 2 * t7; - const double t26 = ea0_z * ea1_z; - const double t27 = 2 * t19; - const double t28 = 2 * t20; - const double t29 = 2 * t13; - const double t30 = 2 * t14; - const double t31 = ea0_x * ea0_x; - const double t32 = eb0_y * eb0_y; - const double t33 = eb0_z * eb0_z; - const double t34 = eb1_y * eb1_y; - const double t35 = eb1_z * eb1_z; - const double t36 = ea0_y * ea0_y; - const double t37 = eb0_x * eb0_x; - const double t38 = eb1_x * eb1_x; - const double t39 = ea0_z * ea0_z; - const double t40 = ea1_x * ea1_x; - const double t41 = ea1_y * ea1_y; - const double t42 = ea1_z * ea1_z; - const double t43 = 2 * ea0_x; - const double t44 = ea1_x * t32; - const double t45 = ea1_x * t33; - const double t46 = ea1_x * t34; - const double t47 = ea1_x * t35; - const double t48 = 2 * eb0_y; - const double t49 = eb1_y * t31; - const double t50 = 2 * eb0_z; - const double t51 = eb1_z * t31; - const double t52 = 2 * ea0_y; - const double t53 = ea1_y * t37; - const double t54 = ea1_y * t33; - const double t55 = ea1_y * t38; - const double t56 = ea1_y * t35; - const double t57 = 2 * eb0_x; - const double t58 = eb1_x * t36; - const double t59 = eb1_z * t36; - const double t60 = 2 * ea0_z; - const double t61 = ea1_z * t37; - const double t62 = ea1_z * t32; - const double t63 = ea1_z * t38; - const double t64 = ea1_z * t34; - const double t65 = eb1_x * t39; - const double t66 = eb1_y * t39; - const double t67 = eb1_y * t40; - const double t68 = eb1_z * t40; - const double t69 = eb1_x * t41; - const double t70 = eb1_z * t41; - const double t71 = eb1_x * t42; - const double t72 = eb1_y * t42; - const double t73 = 1.0 - / (-t0 * t2 + t0 * t5 + t10 * t12 + t10 * t9 + t12 * t21 + t13 * t2 - + t13 * t24 - t13 * t25 - t13 * t27 + t13 * t28 - t13 * t5 - - t14 * t2 - t14 * t24 + t14 * t25 + t14 * t27 - t14 * t28 - + t14 * t5 + t15 * t17 - t15 * t18 + t15 * t2 - t15 * t27 - + t15 * t28 - t15 * t29 + t15 * t30 - t15 * t5 - t16 * t17 - + t16 * t18 - t16 * t2 + t16 * t27 - t16 * t28 + t16 * t29 - - t16 * t30 + t16 * t5 + t17 * t19 - t17 * t20 - t17 * t6 - + t17 * t7 - t18 * t19 + t18 * t20 + t18 * t6 - t18 * t7 - + t19 * t24 - t19 * t25 + t2 * t3 - t2 * t6 + t2 * t7 - t20 * t24 - + t20 * t25 + t21 * t23 + t23 * t26 + t26 * t9 - t3 * t5 - + t31 * t32 + t31 * t33 + t31 * t34 + t31 * t35 + t32 * t39 - + t32 * t40 + t32 * t42 + t33 * t36 + t33 * t40 + t33 * t41 - + t34 * t39 + t34 * t40 + t34 * t42 + t35 * t36 + t35 * t40 - + t35 * t41 + t36 * t37 + t36 * t38 + t37 * t39 + t37 * t41 - + t37 * t42 + t38 * t39 + t38 * t41 + t38 * t42 - t43 * t44 - - t43 * t45 - t43 * t46 - t43 * t47 - t48 * t49 - t48 * t66 - - t48 * t67 - t48 * t72 + t5 * t6 - t5 * t7 - t50 * t51 - - t50 * t59 - t50 * t68 - t50 * t70 - t52 * t53 - t52 * t54 - - t52 * t55 - t52 * t56 - t57 * t58 - t57 * t65 - t57 * t69 - - t57 * t71 - t60 * t61 - t60 * t62 - t60 * t63 - t60 * t64); - const double t74 = ea1_x + eb0_x - t43; - const double t75 = eb1_x * t57; - const double t76 = eb1_y * t48; - const double t77 = eb1_z * t50; - const double t78 = t32 + t33 + t34 + t35 + t37 + t38 - t75 - t76 - t77; - const double t79 = eb0_x - eb1_x; - const double t80 = ea0_x - eb0_x; - const double t81 = ea0_y - eb0_y; - const double t82 = eb0_y - eb1_y; - const double t83 = ea0_z - eb0_z; - const double t84 = eb0_z - eb1_z; - const double t85 = t79 * t80 + t81 * t82 + t83 * t84; - const double t86 = t79 * t85; - const double t87 = - t0 + t1 - t13 + t14 - t15 + t16 - t19 + t20 - t3 - t4 + t6 - t7; - const double t88 = t80 * (-ea0_x + ea1_x) + t81 * (-ea0_y + ea1_y) - + t83 * (-ea0_z + ea1_z); - const double t89 = 2 * t73; - const double t90 = t89 - * (ea0_x * t32 + ea0_x * t33 + ea0_x * t34 + ea0_x * t35 - + ea1_x * t76 + ea1_x * t77 - eb0_x * t0 + eb0_x * t13 - - eb0_x * t14 + eb0_x * t15 - eb0_x * t16 + eb0_x * t3 - - eb0_x * t6 + eb0_x * t7 + eb1_x * t0 - eb1_x * t13 - + eb1_x * t14 - eb1_x * t15 + eb1_x * t16 - eb1_x * t3 - + eb1_x * t6 - eb1_x * t7 - t11 * t43 - t43 * t8 - t44 - t45 - - t46 - t47); - const double t91 = t88 * t90; - const double t92 = t78 * t91; - const double t93 = t85 * t90; - const double t94 = t87 * t93; - const double t95 = ea1_x * t43; - const double t96 = ea1_y * t52; - const double t97 = ea1_z * t60; - const double t98 = t31 + t36 + t39 + t40 + t41 + t42 - t95 - t96 - t97; - const double t99 = ea0_x - ea1_x; - const double t100 = t85 * t99; - const double t101 = 2 * t100; - const double t102 = t79 * t88; - const double t103 = t93 * t98; - const double t104 = t87 * t91; - const double t105 = ea1_y + eb0_y - t52; - const double t106 = -ea0_y * t33 - ea0_y * t35 - ea0_y * t37 - - ea0_y * t38 - ea1_y * t75 - ea1_y * t77 + eb0_y * t1 - eb0_y * t15 - + eb0_y * t16 - eb0_y * t19 + eb0_y * t20 - eb0_y * t4 + eb0_y * t6 - - eb0_y * t7 - eb1_y * t1 + eb1_y * t15 - eb1_y * t16 + eb1_y * t19 - - eb1_y * t20 + eb1_y * t4 - eb1_y * t6 + eb1_y * t7 + t11 * t52 - + t22 * t52 + t53 + t54 + t55 + t56; - const double t107 = t106 * t89; - const double t108 = t107 * t88; - const double t109 = t107 * t85; - const double t110 = t109 * t87 + t82 * t85; - const double t111 = t82 * t88; - const double t112 = ea0_y - ea1_y; - const double t113 = t112 * t85; - const double t114 = t109 * t98 + 2 * t113; - const double t115 = ea1_z + eb0_z - t60; - const double t116 = -ea0_z * t32 - ea0_z * t34 - ea0_z * t37 - - ea0_z * t38 - ea1_z * t75 - ea1_z * t76 + eb0_z * t0 + eb0_z * t1 - - eb0_z * t13 + eb0_z * t14 - eb0_z * t19 + eb0_z * t20 - eb0_z * t3 - - eb0_z * t4 - eb1_z * t0 - eb1_z * t1 + eb1_z * t13 - eb1_z * t14 - + eb1_z * t19 - eb1_z * t20 + eb1_z * t3 + eb1_z * t4 + t22 * t60 - + t60 * t8 + t61 + t62 + t63 + t64; - const double t117 = t116 * t89; - const double t118 = t117 * t88; - const double t119 = t117 * t85; - const double t120 = t119 * t87 + t84 * t85; - const double t121 = t84 * t88; - const double t122 = ea0_z - ea1_z; - const double t123 = t122 * t85; - const double t124 = t119 * t98 + 2 * t123; - const double t125 = t80 * t87; - const double t126 = t112 * t81 + t122 * t83 + t80 * t99; - const double t127 = 2 * t126; - const double t128 = t127 * t73; - const double t129 = t106 * t128; - const double t130 = t81 * t87; - const double t131 = t126 * t82; - const double t132 = t116 * t128; - const double t133 = t83 * t87; - const double t134 = t126 * t84; - const double t135 = ea0_x + eb1_x - t57; - const double t136 = ea0_x * t0 - ea0_x * t13 + ea0_x * t14 - ea0_x * t15 - + ea0_x * t16 - ea0_x * t3 + ea0_x * t6 - ea0_x * t7 - ea1_x * t0 - + ea1_x * t13 - ea1_x * t14 + ea1_x * t15 - ea1_x * t16 + ea1_x * t3 - - ea1_x * t6 + ea1_x * t7 - eb0_x * t36 - eb0_x * t39 - eb0_x * t41 - - eb0_x * t42 + eb0_x * t96 + eb0_x * t97 - eb1_x * t96 - - eb1_x * t97 + t58 + t65 + t69 + t71; - const double t137 = t136 * t89; - const double t138 = t137 * t88; - const double t139 = t137 * t85; - const double t140 = t100 + t139 * t87; - const double t141 = t139 * t98; - const double t142 = ea0_y + eb1_y - t48; - const double t143 = -ea0_y * t1 + ea0_y * t15 - ea0_y * t16 - + ea0_y * t19 - ea0_y * t20 + ea0_y * t4 - ea0_y * t6 + ea0_y * t7 - + ea1_y * t1 - ea1_y * t15 + ea1_y * t16 - ea1_y * t19 + ea1_y * t20 - - ea1_y * t4 + ea1_y * t6 - ea1_y * t7 + eb0_y * t31 + eb0_y * t39 - + eb0_y * t40 + eb0_y * t42 - eb0_y * t95 - eb0_y * t97 - + eb1_y * t95 + eb1_y * t97 - t49 - t66 - t67 - t72; - const double t144 = t143 * t89; - const double t145 = t144 * t88; - const double t146 = t144 * t85; - const double t147 = t146 * t87; - const double t148 = t146 * t98; - const double t149 = ea0_z + eb1_z - t50; - const double t150 = -ea0_z * t0 - ea0_z * t1 + ea0_z * t13 - ea0_z * t14 - + ea0_z * t19 - ea0_z * t20 + ea0_z * t3 + ea0_z * t4 + ea1_z * t0 - + ea1_z * t1 - ea1_z * t13 + ea1_z * t14 - ea1_z * t19 + ea1_z * t20 - - ea1_z * t3 - ea1_z * t4 + eb0_z * t31 + eb0_z * t36 + eb0_z * t40 - + eb0_z * t41 - eb0_z * t95 - eb0_z * t96 + eb1_z * t95 - + eb1_z * t96 - t51 - t59 - t68 - t70; - const double t151 = t150 * t89; - const double t152 = t151 * t88; - const double t153 = t151 * t85; - const double t154 = t153 * t87; - const double t155 = t153 * t98; - const double t156 = t128 * t136; - const double t157 = t128 * t143; - const double t158 = t128 * t150; - J[0] = t73 * (-t74 * t78 - t79 * t87 - t86 + t92 + t94); - J[1] = t73 * (-t101 - t102 + t103 + t104 - t74 * t87 - t79 * t98); - J[2] = -t73 * (t105 * t78 + t108 * t78 + t110 + t82 * t87); - J[3] = -t73 * (t105 * t87 + t108 * t87 + t111 + t114 + t82 * t98); - J[4] = -t73 * (t115 * t78 + t118 * t78 + t120 + t84 * t87); - J[5] = -t73 * (t115 * t87 + t118 * t87 + t121 + t124 + t84 * t98); - J[6] = t73 * (-t78 * t80 + t86 - t92 - t94); - J[7] = t73 * (t101 + t102 - t103 - t104 - t125); - J[8] = t73 * (t110 - t129 * t78 - t78 * t81); - J[9] = t73 * (t114 - t129 * t87 - t130 - t131); - J[10] = t73 * (t120 - t132 * t78 - t78 * t83); - J[11] = t73 * (t124 - t132 * t87 - t133 - t134); - J[12] = -t73 * (2 * t102 + t135 * t87 + t138 * t78 + t140 + t78 * t99); - J[13] = -t73 * (t135 * t98 + t138 * t87 + t141 + t87 * t99 + t88 * t99); - J[14] = t73 - * (-2 * t111 - t112 * t78 - t113 - t142 * t87 + t145 * t78 + t147); - J[15] = - t73 * (-t112 * t87 - t112 * t88 - t142 * t98 + t145 * t87 + t148); - J[16] = t73 - * (-2 * t121 - t122 * t78 - t123 - t149 * t87 + t152 * t78 + t154); - J[17] = - t73 * (-t122 * t87 - t122 * t88 - t149 * t98 + t152 * t87 + t155); - J[18] = t73 * (t125 - t127 * t79 + t140 - t156 * t78); - J[19] = t73 * (-t126 * t99 + t141 - t156 * t87 + t80 * t98); - J[20] = t73 * (t113 + t130 - 2 * t131 - t147 + t157 * t78); - J[21] = t73 * (-t112 * t126 - t148 + t157 * t87 + t81 * t98); - J[22] = t73 * (t123 + t133 - 2 * t134 - t154 + t158 * t78); - J[23] = t73 * (-t122 * t126 - t155 + t158 * t87 + t83 * t98); - } - - // 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 double t0 = ea0_y * eb0_y; - const double t1 = ea0_x * eb0_x; - const double t2 = 2 * t1; - const double t3 = ea0_y * eb1_y; - const double t4 = ea0_x * eb1_x; - const double t5 = 2 * t4; - const double t6 = ea0_z * eb0_z; - const double t7 = ea0_z * eb1_z; - const double t8 = ea0_x * eb0_y; - const double t9 = ea1_x * eb1_y; - const double t10 = ea0_x * eb0_z; - const double t11 = ea1_x * eb1_z; - const double t12 = ea1_y * eb0_y; - const double t13 = ea1_y * eb1_y; - const double t14 = ea1_z * eb0_z; - const double t15 = ea1_z * eb1_z; - const double t16 = 2 * t0; - const double t17 = 2 * t3; - const double t18 = ea1_x * eb0_x; - const double t19 = ea1_x * eb1_x; - const double t20 = ea0_y * eb1_x; - const double t21 = ea1_y * eb0_x; - const double t22 = ea0_y * eb0_z; - const double t23 = ea1_y * eb1_z; - const double t24 = 2 * t6; - const double t25 = 2 * t7; - const double t26 = ea0_z * eb1_x; - const double t27 = ea1_z * eb0_x; - const double t28 = ea0_z * eb1_y; - const double t29 = ea1_z * eb0_y; - const double t30 = 2 * t18; - const double t31 = 2 * t19; - const double t32 = 2 * t12; - const double t33 = 2 * t13; - const double t34 = (ea0_x * ea0_x); - const double t35 = (eb0_y * eb0_y); - const double t36 = (eb0_z * eb0_z); - const double t37 = (eb1_y * eb1_y); - const double t38 = (eb1_z * eb1_z); - const double t39 = (ea0_y * ea0_y); - const double t40 = (eb0_x * eb0_x); - const double t41 = (eb1_x * eb1_x); - const double t42 = (ea0_z * ea0_z); - const double t43 = (ea1_x * ea1_x); - const double t44 = (ea1_y * ea1_y); - const double t45 = (ea1_z * ea1_z); - const double t46 = ea0_x * t35; - const double t47 = 2 * ea1_x; - const double t48 = ea0_x * t36; - const double t49 = ea0_x * t37; - const double t50 = ea0_x * t38; - const double t51 = 2 * eb0_y; - const double t52 = eb1_y * t34; - const double t53 = 2 * eb0_z; - const double t54 = eb1_z * t34; - const double t55 = 2 * ea0_y; - const double t56 = ea1_y * t40; - const double t57 = ea1_y * t36; - const double t58 = ea1_y * t41; - const double t59 = ea1_y * t38; - const double t60 = 2 * eb0_x; - const double t61 = eb1_x * t39; - const double t62 = eb1_z * t39; - const double t63 = 2 * ea0_z; - const double t64 = ea1_z * t40; - const double t65 = ea1_z * t35; - const double t66 = ea1_z * t41; - const double t67 = ea1_z * t37; - const double t68 = eb1_x * t42; - const double t69 = eb1_y * t42; - const double t70 = eb1_y * t43; - const double t71 = eb1_z * t43; - const double t72 = eb1_x * t44; - const double t73 = eb1_z * t44; - const double t74 = eb1_x * t45; - const double t75 = eb1_y * t45; - const double 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 double t77 = 1.0 / t76; - const double t78 = eb1_z * t53; - const double t79 = t36 + t38 - t78; - const double t80 = eb1_y * t51; - const double t81 = t35 + t37 - t80; - const double t82 = t79 + t81; - const double t83 = ea0_x - eb0_x; - const double t84 = eb0_x - eb1_x; - const double t85 = t83 * t84; - const double t86 = ea0_y - eb0_y; - const double t87 = eb0_y - eb1_y; - const double t88 = t86 * t87; - const double t89 = ea0_z - eb0_z; - const double t90 = eb0_z - eb1_z; - const double t91 = t89 * t90; - const double t92 = t88 + t91; - const double t93 = t85 + t92; - const double t94 = -t14 + t15 + t6 - t7; - const double t95 = t0 - t12 + t13 - t3; - const double t96 = t94 + t95; - const double t97 = t1 - t18 + t19 - t4; - const double t98 = t96 + t97; - const double t99 = t93 * t98; - const double t100 = t82 * t99; - const double t101 = ea0_x - ea1_x; - const double t102 = t101 * t83; - const double t103 = ea0_y - ea1_y; - const double t104 = t103 * t86; - const double t105 = ea0_z - ea1_z; - const double t106 = t105 * t89; - const double t107 = t102 + t104 + t106; - const double t108 = eb1_x * t60; - const double t109 = -t108 + t40 + t41; - const double t110 = t109 + t82; - const double t111 = 1 / (t76 * t76); - const double t112 = 2 * ea0_x; - const double t113 = eb1_y * t112; - const double t114 = eb1_z * t112; - const double 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 double t116 = (t115 * t115); - const double t117 = t111 * t116; - const double t118 = 4 * t117; - const double t119 = 2 * t77; - const double t120 = t115 * t93; - const double t121 = t120 * t84; - const double t122 = t107 * t110; - const double t123 = t115 * t84; - const double t124 = t119 * t98; - const double t125 = t110 * t115; - const double t126 = ea1_x + eb0_x - t112; - const double t127 = t119 * t126; - const double t128 = t110 + t123 * t124 + t125 * t127 - (t84 * t84); - const double t129 = t84 * t87; - const double t130 = - eb0_x * eb0_y - eb0_x * eb1_y - eb0_y * eb1_x + eb1_x * eb1_y; - const double t131 = -t101 * t83 - t103 * t86 - t105 * t89; - const double t132 = t110 * t131; - const double t133 = t132 * t77; - const double t134 = t130 * t99; - const double t135 = eb0_x * t55; - const double t136 = eb1_z * t55; - const double t137 = ea1_y * eb1_x; - const double 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 double t139 = t110 * t138; - const double t140 = t126 * t77; - const double t141 = ea1_y + eb0_y - t55; - const double t142 = t84 * t93; - const double t143 = t138 * t142; - const double t144 = t77 * t98; - const double t145 = t144 * t84; - const double 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 double t147 = t84 * t90; - const double t148 = - eb0_x * eb0_z - eb0_x * eb1_z - eb0_z * eb1_x + eb1_x * eb1_z; - const double t149 = t148 * t99; - const double t150 = eb0_x * t63; - const double t151 = eb0_y * t63; - const double t152 = ea1_z * eb1_x; - const double t153 = ea1_z * eb1_y; - const double 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 double t155 = t110 * t154; - const double t156 = ea1_z + eb0_z - t63; - const double t157 = t142 * t154; - const double 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 double t159 = 4 * t77; - const double 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 double t161 = t122 * t130; - const double t162 = t125 * t86; - const double t163 = t120 * t87; - const double t164 = t119 * t163; - const double t165 = t124 * t84; - const double t166 = 8 * t111; - const double t167 = t138 * t166; - const double t168 = t115 * t122; - const double t169 = t115 * t99; - const double t170 = t167 * t169; - const double t171 = t77 - * (t119 * t134 + t119 * t143 - t119 * t161 + t119 * t162 - + t127 * t139 + t129 + t138 * t165 - t164 + t167 * t168 - t170); - const double t172 = t122 * t148; - const double t173 = t125 * t89; - const double t174 = t120 * t90; - const double t175 = t119 * t174; - const double t176 = t154 * t166; - const double t177 = t169 * t176; - const double t178 = t77 - * (t119 * t149 + t119 * t157 - t119 * t172 + t119 * t173 - + t127 * t155 + t147 + t154 * t165 + t168 * t176 - t175 - t177); - const double t179 = ea0_x + eb1_x - t60; - const double t180 = t131 * t159; - const double t181 = t101 * t119; - const double t182 = t115 * t124; - const double t183 = t119 * t96; - const double t184 = t125 * t131; - const double 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 double t186 = 8 * t185; - const double t187 = t111 * t186; - const double t188 = t110 - t123 * t180 - t125 * t181 + t132 * t183 - - t179 * t182 + t179 * t84 - t184 * t187; - const double t189 = t101 * t84; - const double t190 = 2 * t126; - const double t191 = t110 * t127; - const double t192 = t119 * t142; - const double t193 = - -t120 * t181 - t169 * t187 + t183 * t99 + t185 * t192; - const double t194 = - t165 * t185 + t185 * t191 + t189 + t190 * t84 + t193 + t98; - const double t195 = t77 * (-t188 - t194 - t93); - const double t196 = -ea0_y * eb0_x - ea1_x * t51 + eb0_y * t112 - t113 - - t137 + t20 + t21 + 2 * t9; - const double t197 = t119 * t196; - const double t198 = t115 * t180; - const double t199 = ea1_x * eb0_y; - const double 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 double t201 = t166 * t200; - const double t202 = t132 * t197 - t184 * t201 + t198 * t87; - const double t203 = t103 * t119; - const double t204 = t120 * t203; - const double t205 = t192 * t200; - const double t206 = t197 * t99; - const double t207 = -t169 * t201; - const double t208 = ea0_y + eb1_y - t51; - const double t209 = - t125 * t203 + t182 * t208 + t204 + t205 + t206 + t207 - t208 * t84; - const double t210 = t103 * t84; - const double t211 = t165 * t200 - t190 * t87 + t191 * t200 - t210; - const double t212 = t77 * (t202 + t209 + t211); - const double t213 = -ea0_z * eb0_x - ea1_x * t53 + eb0_z * t112 - + 2 * t11 - t114 - t152 + t26 + t27; - const double t214 = t119 * t213; - const double t215 = ea1_x * eb0_z; - const double t216 = ea1_y * eb0_z; - const double 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 double t218 = t166 * t217; - const double t219 = t132 * t214 - t184 * t218 + t198 * t90; - const double t220 = t105 * t119; - const double t221 = t120 * t220; - const double t222 = t192 * t217; - const double t223 = t214 * t99; - const double t224 = -t169 * t218; - const double t225 = ea0_z + eb1_z - t53; - const double t226 = - t125 * t220 + t182 * t225 + t221 + t222 + t223 + t224 - t225 * t84; - const double t227 = t105 * t84; - const double t228 = t165 * t217 - t190 * t90 + t191 * t217 - t227; - const double t229 = t77 * (t219 + t226 + t228); - const double t230 = t107 * t159; - const double t231 = - -t122 * t183 + t123 * t230 + t168 * t187 - t182 * t83; - const double t232 = t77 * (t194 + t231 + 2 * t85 + t92); - const double t233 = t84 * t86; - const double t234 = t182 * t86 + t204 + t205 + t206 + t207 - t233; - const double t235 = t77 - * (2 * t107 * t110 * t196 * t77 + 4 * t107 * t115 * t77 * t87 - - t168 * t201 - t211 - t234); - const double t236 = t84 * t89; - const double t237 = t182 * t89 + t221 + t222 + t223 + t224 - t236; - const double t238 = t77 - * (2 * t107 * t110 * t213 * t77 + 4 * t107 * t115 * t77 * t90 - - t168 * t218 - t228 - t237); - const double t239 = (t87 * t87); - const double t240 = t109 + t79; - const double t241 = t138 * t87; - const double t242 = t124 * t241; - const double t243 = t119 * t141; - const double t244 = t139 * t243; - const double t245 = (t138 * t138); - const double t246 = 4 * t111 * t245; - const double t247 = - t108 - t35 - t36 - t37 - t38 - t40 - t41 + t78 + t80; - const double t248 = t87 * t90; - const double t249 = - eb0_y * eb0_z - eb0_y * eb1_z - eb0_z * eb1_y + eb1_y * eb1_z; - const double t250 = t249 * t99; - const double t251 = t141 * t155; - const double t252 = t139 * t156; - const double t253 = t154 * t87; - const double t254 = t253 * t93; - const double t255 = t138 * t90; - const double t256 = t255 * t93; - const double t257 = 4 * t111 * t138; - const double t258 = t131 * t155; - const double t259 = t154 * t99; - const double t260 = -t119 - * (t133 * t249 + t144 * t253 + t144 * t255 + t248 + t250 * t77 - + t251 * t77 + t252 * t77 + t254 * t77 + t256 * t77 + t257 * t258 - + t257 * t259); - const double t261 = t110 * t83; - const double t262 = t138 * t261; - const double 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 double t264 = t139 * t86; - const double t265 = t119 * t132; - const double t266 = t240 * t99; - const double 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 double t268 = t139 * t89; - const double t269 = t119 * t250 + t119 * t254 + t119 * t256 - + t167 * t258 + t167 * t259 + t248 + t249 * t265; - const double t270 = - t77 * (t119 * t251 - t119 * t268 + t124 * t253 + t269); - const double t271 = t124 * t138; - const double t272 = -ea0_x * eb1_y + ea1_y * t60 + eb1_x * t55 - t135 - - 2 * t137 - t199 + t8 + t9; - const double t273 = t138 * t84; - const double t274 = t131 * t139; - const double t275 = t139 * t181 + t179 * t271 + t179 * t87 + t180 * t273 - + t187 * t274 + t265 * t272; - const double t276 = t101 * t87; - const double t277 = 2 * t141; - const double t278 = t185 * t87; - const double t279 = t110 * t243; - const double t280 = t138 * t93; - const double t281 = t119 * t93; - const double t282 = t119 * t99; - const double t283 = t138 * t187; - const double t284 = - t181 * t280 + t272 * t282 + t278 * t281 + t283 * t99; - const double t285 = - t124 * t278 + t185 * t279 + t276 + t277 * t84 + t284; - const double t286 = -t77 * (t275 + t285); - const double t287 = t94 + t97; - const double t288 = - -8 * t110 * t111 * t131 * t138 * t200 + t180 * t241 + t265 * t287; - const double t289 = t110 + t139 * t203 + t208 * t271 + t208 * t87; - const double t290 = t200 * t87; - const double t291 = -t281 * t290; - const double t292 = t203 * t280; - const double t293 = t282 * t287; - const double t294 = t167 * t99; - const double t295 = -t200 * t294; - const double t296 = t103 * t87; - const double t297 = -t124 * t290 - t200 * t279 + t277 * t87 + t291 - + t292 + t293 + t295 + t296; - const double t298 = t93 + t98; - const double t299 = t77 * (-t288 - t289 - t297 - t298); - const double t300 = t217 * t87; - const double t301 = t281 * t300; - const double t302 = -t301; - const double t303 = -ea0_z * eb0_y - ea1_y * t53 + eb0_z * t55 - t136 - - t153 + 2 * t23 + t28 + t29; - const double t304 = t282 * t303; - const double t305 = -t304; - const double t306 = t220 * t280; - const double t307 = t217 * t294; - const double t308 = -t307; - const double t309 = t180 * t255; - const double t310 = t139 * t220 + t225 * t271 + t225 * t87; - const double t311 = t105 * t87; - const double t312 = -t124 * t300 - t217 * t279 + t277 * t90 + t311; - const double t313 = t77 - * (8 * t110 * t111 * t131 * t138 * t217 - + 2 * t110 * t131 * t303 * t77 - t302 - t305 - t306 - t308 - t309 - - t310 - t312); - const double t314 = t83 * t87; - const double t315 = t119 * t122; - const double t316 = - -t122 * t283 - t230 * t273 + t271 * t83 - t272 * t315 + t314; - const double t317 = t77 * (t285 + t316); - const double t318 = t271 * t86; - const double t319 = t122 * t167; - const double t320 = t200 * t319 - t230 * t241 - t287 * t315; - const double t321 = t85 + t98; - const double t322 = t77 * (t297 + t318 + t320 + t321 + 2 * t88 + t91); - const double t323 = t87 * t89; - const double t324 = t271 * t89; - const double t325 = - t217 * t319 - t230 * t255 + t302 + t303 * t315 + t305 + t306 + t308; - const double t326 = t77 * (t312 + t323 + t324 + t325); - const double t327 = (t90 * t90); - const double t328 = t109 + t81; - const double t329 = t154 * t90; - const double t330 = t124 * t329; - const double t331 = t119 * t156; - const double t332 = t155 * t331; - const double t333 = (t154 * t154); - const double t334 = 4 * t111 * t333; - const double t335 = t154 * t261; - const double 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 double t337 = t155 * t86; - const double t338 = - t77 * (t119 * t252 - t119 * t337 + t124 * t255 + t269); - const double t339 = t155 * t89; - const double t340 = t328 * t99; - const double 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 double t342 = t124 * t154; - const double t343 = -ea0_x * eb1_z + ea1_z * t60 + eb1_x * t63 + t10 - + t11 - t150 - 2 * t152 - t215; - const double t344 = t154 * t84; - const double t345 = t155 * t181 + t179 * t342 + t179 * t90 + t180 * t344 - + t187 * t258 + t265 * t343; - const double t346 = t101 * t90; - const double t347 = 2 * t156; - const double t348 = t185 * t90; - const double t349 = t110 * t331; - const double t350 = t154 * t93; - const double t351 = - t181 * t350 + t187 * t259 + t281 * t348 + t282 * t343; - const double t352 = - t124 * t348 + t185 * t349 + t346 + t347 * t84 + t351; - const double t353 = -t77 * (t345 + t352); - const double t354 = -ea0_y * eb1_z + ea1_z * t51 + eb1_y * t63 - t151 - - 2 * t153 - t216 + t22 + t23; - const double t355 = - -8 * t110 * t111 * t131 * t154 * t200 + t180 * t253 + t265 * t354; - const double t356 = t155 * t203 + t208 * t342 + t208 * t90; - const double t357 = t200 * t90; - const double t358 = -t281 * t357; - const double t359 = t203 * t350; - const double t360 = t282 * t354; - const double t361 = t176 * t99; - const double t362 = -t200 * t361; - const double t363 = t103 * t90; - const double t364 = -t124 * t357 - t200 * t349 + t347 * t87 + t358 - + t359 + t360 + t362 + t363; - const double t365 = t77 * (-t355 - t356 - t364); - const double t366 = t95 + t97; - const double t367 = - -8 * t110 * t111 * t131 * t154 * t217 + t180 * t329 + t265 * t366; - const double t368 = t110 + t155 * t220 + t225 * t342 + t225 * t90; - const double t369 = t217 * t90; - const double t370 = -t281 * t369; - const double t371 = t220 * t350; - const double t372 = t282 * t366; - const double t373 = -t217 * t361; - const double t374 = t105 * t90; - const double t375 = -t124 * t369 - t217 * t349 + t347 * t90 + t370 - + t371 + t372 + t373 + t374; - const double t376 = t77 * (-t298 - t367 - t368 - t375); - const double t377 = t83 * t90; - const double t378 = t122 * t187; - const double t379 = - -t154 * t378 - t230 * t344 - t315 * t343 + t342 * t83 + t377; - const double t380 = t77 * (t352 + t379); - const double t381 = t86 * t90; - const double t382 = t342 * t86 + t381; - const double t383 = t122 * t176; - const double t384 = t200 * t383 - t230 * t253 - t315 * t354; - const double t385 = t77 * (t364 + t382 + t384); - const double t386 = t342 * t89; - const double t387 = t217 * t383 - t230 * t329 - t315 * t366; - const double t388 = t77 * (t321 + t375 + t386 + t387 + t88 + 2 * t91); - const double t389 = 4 * t116; - const double t390 = t77 * t99; - const double t391 = 2 * t111; - const double t392 = t122 * t159; - const double t393 = t115 * t138; - const double t394 = 4 * t390; - const double t395 = t391 - * (-t134 - t143 + t161 - t162 + t163 + t262 - t392 * t393 - + t393 * t394); - const double t396 = t115 * t154; - const double t397 = t391 - * (-t149 - t157 + t172 - t173 + t174 + t335 - t392 * t396 - + t394 * t396); - const double t398 = t119 * t261; - const double t399 = -t185 * t398 + t193 + t92; - const double t400 = t77 * (t188 + t399 - t85); - const double t401 = -t200 * t398 + t202 + 2 * t314; - const double t402 = t77 * (-t209 - t401); - const double t403 = -t217 * t398 + t219 + 2 * t377; - const double t404 = t77 * (-t226 - t403); - const double t405 = t77 * (-t231 - t399); - const double t406 = t77 * (t234 + t401); - const double t407 = t77 * (t237 + t403); - const double t408 = 2 * t93; - const double t409 = t138 * t154; - const double t410 = t391 - * (t122 * t249 - t250 - t254 - t256 + t268 + t337 + t392 * t409 - - t394 * t409); - const double t411 = t110 * t119; - const double t412 = t185 * t86; - const double t413 = -2 * t233 + t284 - t411 * t412; - const double t414 = t77 * (t275 + t413); - const double t415 = t411 * t86; - const double t416 = t200 * t415 + t291 + t292 + t293 + t295 + t85 + t91; - const double t417 = t77 * (t289 + t320 + t416 - t88); - const double t418 = 2 * t381; - const double t419 = t217 * t415; - const double t420 = t77 * (t310 + t325 - t418 + t419); - const double t421 = t77 * (-t316 - t413); - const double t422 = t77 * (-t288 - t318 - t416); - const double t423 = t77 - * (t218 * t274 + t265 * t303 + t301 + t304 - t306 + t307 - t309 - - t323 - t324 + t418 - t419); - const double t424 = t411 * t89; - const double t425 = -t185 * t424 - 2 * t236 + t351; - const double t426 = t77 * (t345 + t425); - const double t427 = t200 * t424 - 2 * t323 + t358 + t359 + t360 + t362; - const double t428 = t77 * (t356 + t384 + t427); - const double t429 = t217 * t424 + t370 + t371 + t372 + t373 + t85 + t88; - const double t430 = t77 * (t368 + t387 + t429 - t91); - const double t431 = t77 * (-t379 - t425); - const double t432 = t77 * (-t355 - t382 - t427); - const double t433 = t77 * (-t367 - t386 - t429); - const double t434 = -ea1_z * t63 + t42 + t45; - const double t435 = -ea1_y * t55 + t39 + t44; - const double t436 = t434 + t435; - const double t437 = t181 * t185; - const double t438 = t111 * (t185 * t185); - const double t439 = 4 * t438; - const double t440 = -t110 * t131 * t436 * t77 + t131 + t132 * t439 - + t180 * t185 * t84 - t436 * t77 * t93 * t98 + t437 * t93 - + t439 * t99; - const double t441 = t124 * t185; - const double t442 = - -t0 - t1 + t12 - t13 + t14 - t15 + t18 - t19 + t3 + t4 - t6 + t7; - const double t443 = - t101 * t179 + t110 * t437 + t179 * t441 + 2 * t189 + t442; - const double t444 = - ea0_x * ea0_y - ea0_x * ea1_y - ea0_y * ea1_x + ea1_x * ea1_y; - const double t445 = -8 * t110 * t111 * t131 * t185 * t200 - - 4 * t131 * t200 * t77 * t84 + t180 * t278 + t265 * t444; - const double t446 = t181 * t200; - const double t447 = t124 * t179; - const double t448 = t103 * t179 - t110 * t446 - t200 * t447 + 2 * t276; - const double t449 = -t446 * t93; - const double t450 = t185 * t203; - const double t451 = t450 * t93; - const double t452 = t282 * t444; - const double t453 = -t187 * t200 * t99; - const double t454 = t101 * t208 + t110 * t450 + t208 * t441 + 2 * t210 - + t449 + t451 + t452 + t453; - const double t455 = t77 * (-t445 - t448 - t454); - const double t456 = - ea0_x * ea0_z - ea0_x * ea1_z - ea0_z * ea1_x + ea1_x * ea1_z; - const double t457 = -8 * t110 * t111 * t131 * t185 * t217 - - 4 * t131 * t217 * t77 * t84 + t180 * t348 + t265 * t456; - const double t458 = t181 * t217; - const double t459 = t105 * t179 - t110 * t458 - t217 * t447 + 2 * t346; - const double t460 = -t458 * t93; - const double t461 = t185 * t220; - const double t462 = t461 * t93; - const double t463 = t282 * t456; - const double t464 = -t187 * t217 * t99; - const double t465 = t101 * t225 + t110 * t461 + t225 * t441 + 2 * t227 - + t460 + t462 + t463 + t464; - const double t466 = t77 * (-t457 - t459 - t465); - const double t467 = t441 * t83; - const double t468 = 2 * t104; - const double t469 = 2 * t106; - const double t470 = 8 * t438; - const double t471 = t159 * t93; - const double t472 = t77 - * (t101 * t185 * t471 - t102 - t107 * t186 * t77 * t84 - t122 * t470 - - t282 * t436 + t315 * t436 + t443 + t467 - t468 - t469 - + t470 * t99); - const double t473 = - t101 * t86 + t124 * t412 + t449 + t451 + t452 + t453; - const double t474 = t230 * t84; - const double t475 = - t200 * t378 + t200 * t474 - t230 * t278 - t315 * t444; - const double t476 = t77 * (t448 + t473 + t475); - const double t477 = t101 * t89 + t441 * t89 + t460 + t462 + t463 + t464; - const double t478 = - t217 * t378 + t217 * t474 - t230 * t348 - t315 * t456; - const double t479 = t77 * (t459 + t477 + t478); - const double t480 = t103 * t208; - const double t481 = 2 * t296; - const double t482 = t110 * t200 * t203; - const double t483 = t124 * t208; - const double t484 = t200 * t483; - const double t485 = -ea1_x * t112 + t34 + t43; - const double t486 = t434 + t485; - const double t487 = (t200 * t200); - const double t488 = t111 * t487; - const double t489 = 4 * t488; - const double 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 double t491 = - ea0_y * ea0_z - ea0_y * ea1_z - ea0_z * ea1_y + ea1_y * ea1_z; - const double t492 = t201 * t217; - const double t493 = -4 * t131 * t200 * t77 * t90 - - 4 * t131 * t217 * t77 * t87 + t132 * t492 + t265 * t491; - const double t494 = t203 * t217; - const double t495 = t105 * t208 - t110 * t494 - t217 * t483 + 2 * t363; - const double t496 = -t494 * t93; - const double t497 = t200 * t220; - const double t498 = -t497 * t93; - const double t499 = t282 * t491; - const double t500 = t492 * t99; - const double t501 = t124 * t200; - const double t502 = t103 * t225 - t110 * t497 - t225 * t501 + 2 * t311 - + t496 + t498 + t499 + t500; - const double t503 = t77 * (-t493 - t495 - t502); - const double t504 = t103 * t83 - t501 * t83; - const double t505 = t77 * (t454 + t475 + t504); - const double t506 = t501 * t86; - const double t507 = 2 * t102 + t98; - const double 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 double t509 = t103 * t89 + t496 + t498 + t499 + t500 - t501 * t89; - const double t510 = - -t122 * t492 + t230 * t300 + t230 * t357 - t315 * t491; - const double t511 = t77 * (t495 + t509 + t510); - const double t512 = t105 * t225; - const double t513 = 2 * t374; - const double t514 = t110 * t217 * t220; - const double t515 = t124 * t217; - const double t516 = t225 * t515; - const double t517 = t435 + t485; - const double t518 = (t217 * t217); - const double t519 = t111 * t518; - const double t520 = 4 * t519; - const double 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 double t522 = t105 * t83 - t515 * t83; - const double t523 = t77 * (t465 + t478 + t522); - const double t524 = t105 * t86 - t515 * t86; - const double t525 = t77 * (t502 + t510 + t524); - const double t526 = t515 * t89; - const double 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 double t528 = t77 * (-t445 - t473 - t504); - const double t529 = t77 * (-t457 - t477 - t522); - const double 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 double t0 = ea0_z * eb0_z; - const double t1 = ea1_z * eb1_z; - const double t2 = ea0_z * eb1_z; - const double t3 = ea1_z * eb0_z; - const double t4 = t0 + t1 - t2 - t3; - const double t5 = ea0_y * eb0_y; - const double t6 = ea1_y * eb1_y; - const double t7 = ea0_y * eb1_y; - const double t8 = ea1_y * eb0_y; - const double t9 = t5 + t6 - t7 - t8; - const double t10 = t4 + t9; - const double t11 = ea0_x * eb0_x; - const double t12 = ea1_x * eb1_x; - const double t13 = ea0_x * eb1_x; - const double t14 = ea1_x * eb0_x; - const double t15 = t11 + t12 - t13 - t14; - const double t16 = t10 + t15; - const double t17 = ea0_x - ea1_x; - const double t18 = ea0_x - eb0_x; - const double t19 = t17 * t18; - const double t20 = ea0_y - ea1_y; - const double t21 = ea0_y - eb0_y; - const double t22 = t20 * t21; - const double t23 = ea0_z - ea1_z; - const double t24 = ea0_z - eb0_z; - const double t25 = t23 * t24; - const double t26 = t22 + t25; - const double t27 = t19 + t26; - const double t28 = t16 * t27; - const double t29 = 2 * t11; - const double t30 = 2 * t13; - const double t31 = ea0_x * eb0_y; - const double t32 = ea1_x * eb1_y; - const double t33 = ea0_x * eb0_z; - const double t34 = ea1_x * eb1_z; - const double t35 = 2 * t5; - const double t36 = 2 * t7; - const double t37 = ea0_y * eb1_x; - const double t38 = ea1_y * eb0_x; - const double t39 = ea0_y * eb0_z; - const double t40 = ea1_y * eb1_z; - const double t41 = 2 * t0; - const double t42 = 2 * t2; - const double t43 = ea0_z * eb1_x; - const double t44 = ea1_z * eb0_x; - const double t45 = ea0_z * eb1_y; - const double t46 = ea1_z * eb0_y; - const double t47 = 2 * t14; - const double t48 = 2 * t12; - const double t49 = 2 * t8; - const double t50 = 2 * t6; - const double t51 = (ea0_x * ea0_x); - const double t52 = (eb0_y * eb0_y); - const double t53 = (eb0_z * eb0_z); - const double t54 = (eb1_y * eb1_y); - const double t55 = (eb1_z * eb1_z); - const double t56 = (ea0_y * ea0_y); - const double t57 = (eb0_x * eb0_x); - const double t58 = (eb1_x * eb1_x); - const double t59 = (ea0_z * ea0_z); - const double t60 = (ea1_x * ea1_x); - const double t61 = (ea1_y * ea1_y); - const double t62 = (ea1_z * ea1_z); - const double t63 = 2 * ea0_x; - const double t64 = ea1_x * t52; - const double t65 = ea1_x * t53; - const double t66 = ea1_x * t54; - const double t67 = ea1_x * t55; - const double t68 = 2 * eb0_y; - const double t69 = eb1_y * t51; - const double t70 = 2 * eb0_z; - const double t71 = eb1_z * t51; - const double t72 = 2 * ea0_y; - const double t73 = ea1_y * t57; - const double t74 = ea1_y * t53; - const double t75 = ea1_y * t58; - const double t76 = ea1_y * t55; - const double t77 = 2 * eb0_x; - const double t78 = eb1_x * t56; - const double t79 = eb1_z * t56; - const double t80 = 2 * ea0_z; - const double t81 = ea1_z * t57; - const double t82 = ea1_z * t52; - const double t83 = ea1_z * t58; - const double t84 = ea1_z * t54; - const double t85 = eb1_x * t59; - const double t86 = eb1_y * t59; - const double t87 = eb1_y * t60; - const double t88 = eb1_z * t60; - const double t89 = eb1_x * t61; - const double t90 = eb1_z * t61; - const double t91 = eb1_x * t62; - const double t92 = eb1_y * t62; - const double 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 double t94 = 1.0 / t93; - const double t95 = -eb1_z * t70 + t53 + t55; - const double t96 = -eb1_y * t68 + t52 + t54; - const double t97 = t95 + t96; - const double t98 = ea1_z * t80; - const double t99 = t59 + t62 - t98; - const double t100 = ea1_y * t72; - const double t101 = -t100 + t56 + t61; - const double t102 = t101 + t99; - const double t103 = ea1_x * t63; - const double t104 = -t103 + t51 + t60; - const double t105 = t102 + t104; - const double t106 = eb0_x - eb1_x; - const double t107 = t106 * t18; - const double t108 = eb0_y - eb1_y; - const double t109 = t108 * t21; - const double t110 = eb0_z - eb1_z; - const double t111 = t110 * t24; - const double t112 = t109 + t111; - const double t113 = t107 + t112; - const double t114 = eb1_y * t63; - const double t115 = eb1_z * t63; - const double 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 double t117 = (t116 * t116); - const double t118 = 1 / (t93 * t93); - const double t119 = 2 * t94; - const double t120 = t116 * t119; - const double t121 = t106 * t120; - const double t122 = t117 * t118; - const double t123 = t105 * t113; - const double t124 = 4 * t123; - const double t125 = -t105 * t113 * t94 * t97 - - 4 * t113 * t116 * t17 * t94 - 4 * t117 * t118 * t16 * t27 - + t121 * t27 + t122 * t124 + t28 * t94 * t97; - const double t126 = - -t0 - t1 - t11 - t12 + t13 + t14 + t2 + t3 - t5 - t6 + t7 + t8; - const double t127 = t113 + t126; - const double t128 = ea1_x + eb0_x - t63; - const double t129 = t106 * t17; - const double t130 = t128 * t16; - const double t131 = -t105 * t121 + t106 * t128 - t120 * t130 + 2 * t129; - const double t132 = ea1_y + eb0_y - t72; - const double t133 = t106 * t132; - const double t134 = t108 * t17; - const double t135 = 2 * t134; - const double t136 = -t17 * t18 - t20 * t21 - t23 * t24; - const double t137 = t108 * t120; - const double t138 = t136 * t137; - const double t139 = t105 * t137; - const double t140 = t120 * t16; - const double t141 = t132 * t140; - const double t142 = eb0_x * t72; - const double t143 = eb1_z * t72; - const double t144 = ea1_y * eb1_x; - const double 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 double t146 = t119 * t145; - const double t147 = t106 * t146; - const double t148 = t136 * t147; - const double t149 = t136 * t16; - const double t150 = t119 - * (eb0_x * eb0_y - eb0_x * eb1_y - eb0_y * eb1_x + eb1_x * eb1_y); - const double t151 = t149 * t150; - const double t152 = 8 * t116; - const double t153 = t118 * t152; - const double t154 = t145 * t153; - const double t155 = t149 * t154; - const double t156 = t106 * t20; - const double t157 = 4 * t116; - const double t158 = t113 * t94; - const double t159 = t157 * t158; - const double t160 = t159 * t20; - const double t161 = t123 * t150; - const double t162 = t158 * t17; - const double t163 = 4 * t162; - const double t164 = t145 * t163; - const double t165 = t123 * t154; - const double t166 = t105 * t147 + t108 * t128 + t130 * t146 + 2 * t156 - - t160 + t161 + t164 - t165; - const double t167 = t94 - * (-t133 - t135 + t138 + t139 + t141 - t148 - t151 + t155 - t166); - const double t168 = ea1_z + eb0_z - t80; - const double t169 = t106 * t168; - const double t170 = t110 * t17; - const double t171 = 2 * t170; - const double t172 = t110 * t120; - const double t173 = t136 * t172; - const double t174 = t105 * t172; - const double t175 = t140 * t168; - const double t176 = eb0_x * t80; - const double t177 = eb0_y * t80; - const double t178 = ea1_z * eb1_x; - const double t179 = ea1_z * eb1_y; - const double 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 double t181 = t119 * t180; - const double t182 = t106 * t181; - const double t183 = t136 * t182; - const double t184 = t119 - * (eb0_x * eb0_z - eb0_x * eb1_z - eb0_z * eb1_x + eb1_x * eb1_z); - const double t185 = t149 * t184; - const double t186 = t153 * t180; - const double t187 = t149 * t186; - const double t188 = t106 * t23; - const double t189 = t159 * t23; - const double t190 = t123 * t184; - const double t191 = t163 * t180; - const double t192 = t123 * t186; - const double t193 = t105 * t182 + t110 * t128 + t130 * t181 + 2 * t188 - - t189 + t190 + t191 - t192; - const double t194 = t94 - * (-t169 - t171 + t173 + t174 + t175 - t183 - t185 + t187 - t193); - const double t195 = t140 * t18; - const double t196 = 2 * t109; - const double t197 = t136 * t94; - const double t198 = t119 * t97; - const double t199 = 8 * t122; - const double t200 = 2 * t111 + t126; - const double t201 = t94 - * (-t106 * t157 * t197 + t107 - t123 * t198 + t123 * t199 + t131 - - t149 * t198 + t149 * t199 - t152 * t162 + t195 + t196 + t200); - const double t202 = t106 * t21; - const double t203 = t147 * t27; - const double t204 = t150 * t28; - const double t205 = t140 * t21; - const double t206 = t137 * t27; - const double t207 = t154 * t28; - const double t208 = - t94 * (t166 - t202 - t203 - t204 + t205 + t206 + t207); - const double t209 = t106 * t24; - const double t210 = t182 * t27; - const double t211 = t184 * t28; - const double t212 = t140 * t24; - const double t213 = t172 * t27; - const double t214 = t186 * t28; - const double t215 = - t94 * (t193 - t209 - t210 - t211 + t212 + t213 + t214); - const double 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 double t217 = t105 * t216; - const double t218 = t106 * t119; - const double t219 = t119 * t130; - const double t220 = t10 * t119; - const double t221 = t123 * t220; - const double t222 = t163 * t216; - const double t223 = t153 * t216; - const double t224 = t123 * t223; - const double t225 = t221 + t222 - t224; - const double t226 = t128 * t17 + t216 * t219 + t217 * t218 + t225; - const double t227 = ea0_x + eb1_x - t77; - const double t228 = 2 * t17; - const double t229 = t120 * t136; - const double t230 = t105 * t120; - const double t231 = t136 * t216; - const double t232 = t129 + t136 - t140 * t17 + t149 * t220 - t149 * t223 - + t16 - t17 * t229 + t218 * t231 + t227 * t228 - t227 * t230; - const double t233 = t94 * (-t105 - t226 - t232); - const double t234 = t128 * t20; - const double t235 = ea0_y + eb1_y - t68; - const double t236 = t228 * t235; - const double t237 = t140 * t20; - const double t238 = ea1_x * eb0_y; - const double 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 double t240 = t105 * t239; - const double t241 = t218 * t240; - const double t242 = t230 * t235; - const double t243 = t219 * t239; - const double t244 = t17 * t239; - const double t245 = 4 * t158; - const double t246 = t244 * t245; - const double t247 = t119 - * (-ea0_y * eb0_x - ea1_x * t68 + eb0_y * t63 - t114 - t144 - + 2 * t32 + t37 + t38); - const double t248 = t123 * t247; - const double t249 = t120 * t27; - const double t250 = t239 * t27; - const double t251 = t123 * t153 * t239; - const double t252 = -8 * t116 * t118 * t16 * t239 * t27 + t20 * t249 - + t218 * t250 - t246 + t247 * t28 - t248 + t251; - const double t253 = - t94 * (-t156 - t234 - t236 + t237 + t241 + t242 + t243 - t252); - const double t254 = t128 * t23; - const double t255 = ea0_z + eb1_z - t70; - const double t256 = t228 * t255; - const double t257 = t140 * t23; - const double t258 = ea1_x * eb0_z; - const double t259 = ea1_y * eb0_z; - const double 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 double t261 = t105 * t260; - const double t262 = t218 * t261; - const double t263 = t230 * t255; - const double t264 = t219 * t260; - const double t265 = t17 * t260; - const double t266 = t245 * t265; - const double t267 = t119 - * (-ea0_z * eb0_x - ea1_x * t70 + eb0_z * t63 - t115 - t178 - + 2 * t34 + t43 + t44); - const double t268 = t123 * t267; - const double t269 = t260 * t27; - const double t270 = t123 * t153 * t260; - const double t271 = -8 * t116 * t118 * t16 * t260 * t27 + t218 * t269 - + t23 * t249 - t266 + t267 * t28 - t268 + t270; - const double t272 = - t94 * (-t188 - t254 - t256 + t257 + t262 + t263 + t264 - t271); - const double t273 = -t22; - const double t274 = t105 * t18; - const double t275 = t120 * t274; - const double t276 = t216 * t27; - const double t277 = t218 * t276; - const double t278 = t220 * t28; - const double t279 = t17 * t27; - const double t280 = t120 * t279; - const double t281 = t16 * t276; - const double t282 = t153 * t281; - const double t283 = -t25; - const double t284 = t105 + t283; - const double t285 = - t94 * (t19 + t226 + t273 - t275 - t277 - t278 + t280 + t282 + t284); - const double t286 = t17 * t21; - const double t287 = 2 * t286; - const double t288 = t21 * t230; - const double t289 = t136 * t218; - const double t290 = -8 * t116 * t118 * t136 * t16 * t239 + t149 * t247 - + t20 * t229 + t239 * t289 + t246 + t248 - t251; - const double t291 = t94 * (t234 - t241 - t243 + t287 - t288 - t290); - const double t292 = t17 * t24; - const double t293 = 2 * t292; - const double t294 = t230 * t24; - const double t295 = -8 * t116 * t118 * t136 * t16 * t260 + t149 * t267 - + t229 * t23 + t260 * t289 + t266 + t268 - t270; - const double t296 = t94 * (t254 - t262 - t264 + t293 - t294 - t295); - const double t297 = -eb1_x * t77 + t57 + t58; - const double t298 = t297 + t95; - const double t299 = t108 * t146; - const double t300 = (t145 * t145); - const double t301 = t118 * t300; - const double t302 = t149 * t301; - const double t303 = t20 * t245; - const double t304 = - -t105 * t113 * t298 * t94 + t124 * t301 + t145 * t303; - const double t305 = t108 * t20; - const double t306 = t132 * t16; - const double t307 = t105 * t299 + t108 * t132 + t146 * t306 + 2 * t305; - const double t308 = t119 - * (eb0_y * eb0_z - eb0_y * eb1_z - eb0_z * eb1_y + eb1_y * eb1_z); - const double t309 = t123 * t308; - const double t310 = t180 * t303; - const double t311 = t23 * t245; - const double t312 = t145 * t311; - const double t313 = 8 * t123; - const double t314 = t118 * t145; - const double t315 = t180 * t314; - const double t316 = t313 * t315; - const double t317 = t108 * t181; - const double t318 = t110 * t146; - const double t319 = 8 * t149; - const double t320 = t108 * t23; - const double t321 = t105 * t317 + t110 * t132 + t181 * t306 + 2 * t320; - const double t322 = t110 * t20; - const double t323 = t146 * t16; - const double t324 = t105 * t318 + t108 * t168 + t168 * t323 + 2 * t322; - const double t325 = -t94 - * (t136 * t317 + t136 * t318 + t149 * t308 + t309 + t310 + t312 - + t315 * t319 + t316 + t321 + t324); - const double t326 = t108 * t18 + t160 - t161 - t164 + t165 + t18 * t323; - const double t327 = t94 - * (t133 + t135 - t138 - t139 - t141 + t148 + t151 - t155 - t326); - const double t328 = -t21 * t323; - const double t329 = 2 * t107; - const double t330 = t119 * t298; - const double t331 = 4 * t197; - const double t332 = 8 * t158; - const double t333 = t94 - * (t108 * t145 * t331 + t109 - t123 * t330 + t145 * t20 * t332 - - t149 * t330 + t200 + t301 * t313 + 8 * t302 + t307 + t328 - + t329); - const double t334 = t108 * t24; - const double t335 = t24 * t323; - const double t336 = t27 * t317; - const double t337 = t27 * t318; - const double t338 = t28 * t308; - const double t339 = 8 * t28; - const double t340 = t315 * t339; - const double t341 = - t309 + t310 + t312 + t316 - t336 - t337 - t338 - t340; - const double t342 = t94 * (t321 - t334 - t335 + t341); - const double t343 = t136 * t146; - const double t344 = t108 * t119; - const double t345 = t119 - * (-ea0_x * eb1_y + ea1_y * t77 + eb1_x * t72 - t142 - 2 * t144 - - t238 + t31 + t32); - const double t346 = t216 * t314; - const double t347 = - t149 * t345 + t17 * t343 + t231 * t344 + t319 * t346; - const double t348 = t119 * t306; - const double t349 = t132 * t17 + t216 * t348 + t217 * t344; - const double t350 = t123 * t345; - const double t351 = t20 * t216; - const double t352 = t245 * t351; - const double t353 = t313 * t346; - const double t354 = 2 * t20; - const double t355 = t105 * t146; - const double t356 = - t134 + t17 * t323 + t227 * t354 + t227 * t355 + t350 + t352 + t353; - const double t357 = -t94 * (t347 + t349 + t356); - const double t358 = t119 * (t15 + t4); - const double t359 = -2 * t108 * t136 * t239 * t94 - - 8 * t118 * t136 * t145 * t16 * t239 + t136 + t149 * t358 - + t20 * t343; - const double t360 = -t239 * t303; - const double t361 = t123 * t358; - const double t362 = t239 * t314; - const double t363 = -t313 * t362; - const double t364 = - t132 * t20 - t239 * t348 - t240 * t344 + t360 + t361 + t363; - const double t365 = t16 + t20 * t323 + t235 * t354 + t235 * t355 + t305; - const double t366 = t94 * (-t105 - t359 - t364 - t365); - const double t367 = t132 * t23; - const double t368 = t255 * t354; - const double t369 = t23 * t323; - const double t370 = t255 * t355; - const double t371 = t261 * t344; - const double t372 = t260 * t348; - const double t373 = t119 - * (-ea0_z * eb0_y - ea1_y * t70 + eb0_z * t72 - t143 - t179 - + 2 * t40 + t45 + t46); - const double t374 = t123 * t373; - const double t375 = t20 * t260; - const double t376 = t245 * t375; - const double t377 = t260 * t314; - const double t378 = t313 * t377; - const double t379 = t136 * t260 * t344 + t149 * t373 - t23 * t343 - + t319 * t377 + t374 + t376 + t378; - const double t380 = - t94 * (-t320 - t367 - t368 - t369 - t370 + t371 + t372 + t379); - const double t381 = t18 * t20; - const double t382 = t146 * t274 + t350 + t352 + t353 + 2 * t381; - const double t383 = 8 * t281; - const double t384 = - -t146 * t279 - t276 * t344 - t28 * t345 - t314 * t383; - const double t385 = t94 * (t349 + t382 + t384); - const double t386 = t21 * t355 + t22; - const double t387 = -t19; - const double t388 = t146 * t27; - const double t389 = - -t20 * t388 + t250 * t344 - t28 * t358 + t339 * t362 + t387; - const double t390 = t94 * (t284 + t364 + t386 + t389); - const double t391 = t20 * t24; - const double t392 = 2 * t391; - const double t393 = t24 * t355; - const double t394 = -t23 * t388 + t269 * t344 + t28 * t373 + t339 * t377 - - t374 - t376 - t378; - const double t395 = t94 * (t367 - t371 - t372 + t392 + t393 + t394); - const double t396 = t297 + t96; - const double t397 = t110 * t181; - const double t398 = (t180 * t180); - const double t399 = t118 * t398; - const double t400 = t149 * t399; - const double t401 = - -t105 * t113 * t396 * t94 + t124 * t399 + t180 * t311; - const double t402 = t110 * t23; - const double t403 = t16 * t181; - const double t404 = t105 * t397 + t110 * t168 + t168 * t403 + 2 * t402; - const double t405 = t110 * t18 + t18 * t403 + t189 - t190 - t191 + t192; - const double t406 = t94 - * (t169 + t171 - t173 - t174 - t175 + t183 + t185 - t187 - t405); - const double t407 = t110 * t21; - const double t408 = t21 * t403; - const double t409 = t94 * (t324 + t341 - t407 - t408); - const double t410 = -t24 * t403; - const double t411 = t119 * t396; - const double t412 = t94 - * (t110 * t180 * t331 + t111 - t123 * t411 + t126 - t149 * t411 - + t180 * t23 * t332 + t196 + t313 * t399 + t329 + 8 * t400 + t404 - + t410); - const double t413 = t136 * t181; - const double t414 = t110 * t119; - const double t415 = t119 - * (-ea0_x * eb1_z + ea1_z * t77 + eb1_x * t80 - t176 - 2 * t178 - - t258 + t33 + t34); - const double t416 = t118 * t180; - const double t417 = t216 * t416; - const double t418 = - t149 * t415 + t17 * t413 + t231 * t414 + t319 * t417; - const double t419 = t119 * t16; - const double t420 = t168 * t419; - const double t421 = t168 * t17 + t216 * t420 + t217 * t414; - const double t422 = t123 * t415; - const double t423 = t216 * t23; - const double t424 = t245 * t423; - const double t425 = t313 * t417; - const double t426 = 2 * t23; - const double t427 = t105 * t181; - const double t428 = - t17 * t403 + t170 + t227 * t426 + t227 * t427 + t422 + t424 + t425; - const double t429 = -t94 * (t418 + t421 + t428); - const double t430 = t119 - * (-ea0_y * eb1_z + ea1_z * t68 + eb1_y * t80 - t177 - 2 * t179 - - t259 + t39 + t40); - const double t431 = -2 * t110 * t136 * t239 * t94 - - 8 * t118 * t136 * t16 * t180 * t239 + t149 * t430 + t20 * t413; - const double t432 = t168 * t20 - t239 * t420 - t240 * t414; - const double t433 = t23 * t239; - const double t434 = -t245 * t433; - const double t435 = t123 * t430; - const double t436 = t239 * t416; - const double t437 = -t313 * t436; - const double t438 = - t20 * t403 + t235 * t426 + t235 * t427 + t322 + t434 + t435 + t437; - const double t439 = t94 * (-t431 - t432 - t438); - const double t440 = t119 * (t15 + t9); - const double t441 = -2 * t110 * t136 * t260 * t94 - - 8 * t118 * t136 * t16 * t180 * t260 + t136 + t149 * t440 - + t23 * t413; - const double t442 = -t260 * t311; - const double t443 = t123 * t440; - const double t444 = t260 * t416; - const double t445 = -t313 * t444; - const double t446 = - t105 + t168 * t23 - t260 * t420 - t261 * t414 + t442 + t443 + t445; - const double t447 = t16 + t23 * t403 + t255 * t426 + t255 * t427 + t402; - const double t448 = t94 * (-t441 - t446 - t447); - const double t449 = t18 * t23; - const double t450 = t181 * t274 + t422 + t424 + t425 + 2 * t449; - const double t451 = - -t181 * t279 - t276 * t414 - t28 * t415 - t383 * t416; - const double t452 = t94 * (t421 + t450 + t451); - const double t453 = t21 * t23; - const double t454 = t21 * t427 + t434 + t435 + t437 + 2 * t453; - const double t455 = t181 * t27; - const double t456 = - -t20 * t455 + t250 * t414 - t28 * t430 + t339 * t436; - const double t457 = t94 * (t432 + t454 + t456); - const double t458 = t24 * t427 + t25; - const double t459 = - -t23 * t455 + t269 * t414 + t273 - t28 * t440 + t339 * t444 + t387; - const double t460 = t94 * (t446 + t458 + t459); - const double t461 = - t94 * (t202 + t203 + t204 - t205 - t206 - t207 + t326); - const double t462 = - t94 * (t209 + t210 + t211 - t212 - t213 - t214 + t405); - const double t463 = t18 * t419; - const double t464 = t216 * t463; - const double t465 = t94 * (t225 + t232 + t387 - t464); - const double t466 = t239 * t463; - const double t467 = - t94 * (t156 + t236 - t237 - t242 - t290 - t381 + t466); - const double t468 = t260 * t463; - const double t469 = - t94 * (t188 + t256 - t257 - t263 - t295 - t449 + t468); - const double t470 = t94 - * (-t221 - t222 + t224 + t26 + t275 + t277 + t278 - t280 - t282 - + t464); - const double t471 = t94 * (-t252 - t287 + t288 + t381 - t466); - const double t472 = t94 * (-t271 - t293 + t294 + t449 - t468); - const double t473 = t94 - * (-t309 - t310 - t312 - t316 + t334 + t335 + t336 + t337 + t338 - + t340 + t407 + t408); - const double t474 = t21 * t419; - const double t475 = -t216 * t474 - t286; - const double t476 = t94 * (t356 + t384 + t475); - const double t477 = t239 * t474 + t360 + t361 + t363; - const double t478 = t94 * (-2 * t22 + t283 + t365 + t389 + t477); - const double t479 = t260 * t474; - const double t480 = - t94 * (t320 + t368 + t369 + t370 + t394 - t453 + t479); - const double t481 = t94 * (-t347 - t382 - t475); - const double t482 = t94 * (-t359 - t386 - t477); - const double t483 = t94 * (t379 - t392 - t393 + t453 - t479); - const double t484 = t24 * t419; - const double t485 = -t216 * t484 - t292; - const double t486 = t94 * (t428 + t451 + t485); - const double t487 = t239 * t484 - t391; - const double t488 = t94 * (t438 + t456 + t487); - const double t489 = t260 * t484 + t442 + t443 + t445; - const double t490 = t94 * (-2 * t25 + t447 + t459 + t489); - const double t491 = t94 * (-t418 - t450 - t485); - const double t492 = t94 * (-t431 - t454 - t487); - const double t493 = t94 * (-t441 - t458 - t489); - const double t494 = (t17 * t17); - const double t495 = t119 * t227; - const double t496 = t217 * t495; - const double t497 = t17 * t216 * t419; - const double t498 = (t216 * t216); - const double t499 = t118 * t498; - const double t500 = t17 * t20; - const double t501 = - ea0_x * ea0_y - ea0_x * ea1_y - ea0_y * ea1_x + ea1_x * ea1_y; - const double t502 = t123 * t501; - const double t503 = t149 * t94; - const double t504 = t217 * t94; - const double t505 = t16 * t94; - const double 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 double t507 = t17 * t23; - const double t508 = - ea0_x * ea0_z - ea0_x * ea1_z - ea0_z * ea1_x + ea1_x * ea1_z; - const double t509 = t123 * t508; - const double 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 double t511 = 4 * t276; - const double 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 double t513 = t21 * t217; - const double t514 = t239 * t279; - const double t515 = t118 * t216; - const double t516 = t118 * t383; - const double t517 = -t119 * t20 * t276 - t119 * t28 * t501 + t119 * t502 - + t119 * t514 - t239 * t313 * t515 + t239 * t516 + t500; - const double t518 = - t94 * (t119 * t513 - t240 * t495 - t244 * t419 + t517); - const double t519 = t217 * t24; - const double t520 = t260 * t279; - const double t521 = -t119 * t23 * t276 - t119 * t28 * t508 + t119 * t509 - + t119 * t520 - t260 * t313 * t515 + t260 * t516 + t507; - const double t522 = - t94 * (t119 * t519 - t261 * t495 - t265 * t419 + t521); - const double t523 = t104 + t99; - const double t524 = t123 * t523; - const double t525 = t28 * t523; - const double t526 = (t239 * t239); - const double t527 = t118 * t526; - const double t528 = 4 * t28; - const double t529 = t20 * t239; - const double t530 = t119 * t235; - const double t531 = t105 - (t20 * t20) + t240 * t530 + t419 * t529; - const double t532 = t20 * t23; - const double t533 = - ea0_y * ea0_z - ea0_y * ea1_z - ea0_z * ea1_y + ea1_y * ea1_z; - const double t534 = t123 * t533; - const double t535 = t235 * t261; - const double t536 = t240 * t255; - const double t537 = t27 * t375; - const double t538 = t27 * t433; - const double t539 = t239 * t260; - const double t540 = t118 * t539; - const double 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 double t542 = - t94 * (-t119 * t239 * t274 + t217 * t530 + t351 * t419 + t517); - const double t543 = t119 * t149; - const double t544 = t94 - * (8 * t105 * t113 * t118 * t526 + 8 * t118 * t136 * t16 * t526 - - t119 * t21 * t240 - t119 * t524 - t331 * t529 - t523 * t543 - - t531); - const double t545 = t119 * t136; - const double t546 = t119 * t534 + t313 * t540 + t319 * t540 - - t375 * t545 - t433 * t545 + t532 + t533 * t543; - const double t547 = - t94 * (-t119 * t24 * t240 - t119 * t535 - t375 * t419 + t546); - const double t548 = t101 + t104; - const double t549 = t123 * t548; - const double t550 = t28 * t548; - const double t551 = (t260 * t260); - const double t552 = t118 * t551; - const double t553 = t23 * t260; - const double t554 = t119 * t255; - const double t555 = t105 - (t23 * t23) + t261 * t554 + t419 * t553; - const double t556 = - t94 * (-t119 * t260 * t274 + t217 * t554 + t419 * t423 + t521); - const double t557 = - t94 * (-t119 * t21 * t261 - t119 * t536 - t419 * t433 + t546); - const double t558 = t94 - * (8 * t105 * t113 * t118 * t551 + 8 * t118 * t136 * t16 * t551 - - t119 * t24 * t261 - t119 * t549 - t331 * t553 - t543 * t548 - - t555); - const double t559 = t124 * t94; - const double t560 = 2 * t118; - const double t561 = t505 * t511; - const double t562 = t560 - * (4 * t105 * t113 * t216 * t239 * t94 + t105 * t18 * t239 - + t16 * t27 * t501 + t20 * t216 * t27 - t239 * t561 - t502 - t513 - - t514); - const double t563 = t560 - * (4 * t105 * t113 * t216 * t260 * t94 + t105 * t18 * t260 - + t16 * t27 * t508 + t216 * t23 * t27 - t260 * t561 - t509 - t519 - - t520); - const double 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); - } - - // J is (2×12) flattened in column-major order - void point_triangle_closest_point_jacobian( - 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 J[24]) - { - const double t0 = t0_x - t1_x; - const double t1 = t2_x * t2_x; - const double t2 = t2_y * t2_y; - const double t3 = t2_z * t2_z; - const double t4 = t0_x * t2_x; - const double t5 = 2 * t4; - const double t6 = t0_y * t2_y; - const double t7 = 2 * t6; - const double t8 = t0_z * t2_z; - const double t9 = 2 * t8; - const double t10 = t0_x * t0_x; - const double t11 = t0_y * t0_y; - const double t12 = t0_z * t0_z; - const double t13 = t10 + t11 + t12; - const double t14 = t1 + t13 + t2 + t3 - t5 - t7 - t9; - const double t15 = t0_x - t2_x; - const double t16 = t1_x * t2_x; - const double t17 = t1_y * t2_y; - const double t18 = t1_z * t2_z; - const double t19 = t0_x * t1_x; - const double t20 = t0_y * t1_y; - const double t21 = t0_z * t1_z; - const double t22 = - t13 + t16 + t17 + t18 - t19 - t20 - t21 - t4 - t6 - t8; - const double t23 = 2 * t19; - const double t24 = 2 * t20; - const double t25 = 2 * t21; - const double t26 = 2 * t16; - const double t27 = 2 * t17; - const double t28 = t1_y * t1_y; - const double t29 = t1_z * t1_z; - const double t30 = t1_x * t1_x; - const double t31 = 2 * t18; - const double t32 = 1.0 - / (t1 * t11 + t1 * t12 - t1 * t24 - t1 * t25 + t1 * t28 + t1 * t29 - + t10 * t2 - t10 * t27 + t10 * t28 + t10 * t29 + t10 * t3 - - t10 * t31 - t11 * t26 + t11 * t29 + t11 * t3 + t11 * t30 - - t11 * t31 + t12 * t2 - t12 * t26 - t12 * t27 + t12 * t28 - + t12 * t30 + t16 * t24 + t16 * t25 + t16 * t7 + t16 * t9 - + t17 * t23 + t17 * t25 - t17 * t26 + t17 * t5 + t17 * t9 - + t18 * t23 + t18 * t24 - t18 * t26 - t18 * t27 + t18 * t5 - + t18 * t7 + t19 * t7 + t19 * t9 - t2 * t23 - t2 * t25 + t2 * t29 - + t2 * t30 - t20 * t23 + t20 * t5 + t20 * t9 - t21 * t23 - - t21 * t24 + t21 * t5 + t21 * t7 - t23 * t3 - t24 * t3 - + t28 * t3 - t28 * t5 - t28 * t9 - t29 * t5 - t29 * t7 + t3 * t30 - - t30 * t7 - t30 * t9 - t5 * t6 - t5 * t8 - t7 * t8); - const double t33 = t13 - t23 - t24 - t25 + t28 + t29 + t30; - const double t34 = t0_y - t1_y; - const double t35 = t0_y - t2_y; - const double t36 = t0_z - t1_z; - const double t37 = t0_z - t2_z; - const double t38 = 2 * t0_x; - const double t39 = -t38; - const double t40 = p_x + t39; - const double t41 = t1_x + t40; - const double t42 = p_x - t0_x; - const double t43 = p_y - t0_y; - const double t44 = p_z - t0_z; - const double t45 = t0 * t42 + t34 * t43 + t36 * t44; - const double t46 = t15 * t45; - const double t47 = 2 * t46; - const double t48 = t1_x + t2_x + t39; - const double t49 = t15 * t42 + t35 * t43 + t37 * t44; - const double t50 = t2_x + t40; - const double t51 = t0_x * t28; - const double t52 = t0_x * t29; - const double t53 = t0_x * t2; - const double t54 = t0_x * t3; - const double t55 = t1_x * t2; - const double t56 = t1_x * t3; - const double t57 = t28 * t2_x; - const double t58 = t29 * t2_x; - const double t59 = t17 * t1_x; - const double t60 = t18 * t1_x; - const double t61 = t17 * t2_x; - const double t62 = t18 * t2_x; - const double t63 = t1_x * t20; - const double t64 = t2_x * t6; - const double t65 = t1_x * t21; - const double t66 = t2_x * t8; - const double t67 = t20 * t2_x + t21 * t2_x; - const double t68 = t1_x * t6 + t1_x * t8; - const double t69 = 2 * t32; - const double t70 = t69 - * (-t17 * t38 - t18 * t38 + t51 + t52 + t53 + t54 - t55 - t56 - t57 - - t58 + t59 + t60 + t61 + t62 - t63 - t64 - t65 - t66 + t67 - + t68); - const double t71 = t14 * t45; - const double t72 = t22 * t70; - const double t73 = t0 * t49; - const double t74 = 2 * t73; - const double t75 = t33 * t49; - const double t76 = 2 * t0_y; - const double t77 = -t76; - const double t78 = p_y + t77; - const double t79 = t1_y + t78; - const double t80 = t35 * t45; - const double t81 = 2 * t80; - const double t82 = t1_y + t2_y + t77; - const double t83 = t2_y + t78; - const double t84 = t0_y * t30; - const double t85 = t0_y * t29; - const double t86 = t0_y * t1; - const double t87 = t0_y * t3; - const double t88 = t1 * t1_y; - const double t89 = t1_y * t3; - const double t90 = t2_y * t30; - const double t91 = t29 * t2_y; - const double t92 = t16 * t1_y; - const double t93 = t16 * t2_y; - const double t94 = t18 * t1_y; - const double t95 = t18 * t2_y; - const double t96 = t19 * t1_y; - const double t97 = t2_y * t4; - const double t98 = t1_y * t21; - const double t99 = t2_y * t8; - const double t100 = t19 * t2_y + t21 * t2_y; - const double t101 = t1_y * t4 + t1_y * t8; - const double t102 = t69 - * (t100 + t101 - t16 * t76 - t18 * t76 + t84 + t85 + t86 + t87 - t88 - - t89 - t90 - t91 + t92 + t93 + t94 + t95 - t96 - t97 - t98 - - t99); - const double t103 = t102 * t22; - const double t104 = t34 * t49; - const double t105 = 2 * t104; - const double t106 = 2 * t0_z; - const double t107 = -t106; - const double t108 = p_z + t107; - const double t109 = t108 + t1_z; - const double t110 = t37 * t45; - const double t111 = 2 * t110; - const double t112 = t107 + t1_z + t2_z; - const double t113 = t108 + t2_z; - const double t114 = t0_z * t30; - const double t115 = t0_z * t28; - const double t116 = t0_z * t1; - const double t117 = t0_z * t2; - const double t118 = t1 * t1_z; - const double t119 = t1_z * t2; - const double t120 = t2_z * t30; - const double t121 = t28 * t2_z; - const double t122 = t16 * t1_z; - const double t123 = t16 * t2_z; - const double t124 = t17 * t1_z; - const double t125 = t17 * t2_z; - const double t126 = t19 * t1_z; - const double t127 = t2_z * t4; - const double t128 = t1_z * t20; - const double t129 = t2_z * t6; - const double t130 = t19 * t2_z + t20 * t2_z; - const double t131 = t1_z * t4 + t1_z * t6; - const double t132 = t69 - * (-t106 * t16 - t106 * t17 + t114 + t115 + t116 + t117 - t118 - - t119 - t120 - t121 + t122 + t123 + t124 + t125 - t126 - t127 - - t128 - t129 + t130 + t131); - const double t133 = t132 * t22; - const double t134 = t36 * t49; - const double t135 = 2 * t134; - const double t136 = t11 * t1_x; - const double t137 = t12 * t1_x; - const double t138 = t11 * t2_x; - const double t139 = t12 * t2_x; - const double t140 = t0_x * t6; - const double t141 = t0_x * t8; - const double t142 = t0_x * t20; - const double t143 = t0_x * t21; - const double t144 = t0_x * t17 + t0_x * t18; - const double t145 = t69 - * (t136 + t137 - t138 - t139 + t140 + t141 - t142 - t143 + t144 - - t1_x * t7 - t1_x * t9 - t53 - t54 + t55 + t56 - t61 - t62 + t64 - + t66 + t67); - const double t146 = t145 * t22; - const double t147 = -t22 * t42; - const double t148 = t10 * t1_y; - const double t149 = t12 * t1_y; - const double t150 = t10 * t2_y; - const double t151 = t12 * t2_y; - const double t152 = t0_y * t4; - const double t153 = t0_y * t8; - const double t154 = t0_y * t19; - const double t155 = t0_y * t21; - const double t156 = t0_y * t16 + t0_y * t18; - const double t157 = t69 - * (t100 + t148 + t149 - t150 - t151 + t152 + t153 - t154 - t155 - + t156 - t1_y * t5 - t1_y * t9 - t86 - t87 + t88 + t89 - t93 - - t95 + t97 + t99); - const double t158 = t157 * t22; - const double t159 = -t22 * t43; - const double t160 = t10 * t1_z; - const double t161 = t11 * t1_z; - const double t162 = t10 * t2_z; - const double t163 = t11 * t2_z; - const double t164 = t0_z * t4; - const double t165 = t0_z * t6; - const double t166 = t0_z * t19; - const double t167 = t0_z * t20; - const double t168 = t0_z * t16 + t0_z * t17; - const double t169 = t69 - * (-t116 - t117 + t118 + t119 - t123 - t125 + t127 + t129 + t130 - + t160 + t161 - t162 - t163 + t164 + t165 - t166 - t167 + t168 - - t1_z * t5 - t1_z * t7); - const double t170 = t169 * t22; - const double t171 = -t22 * t44; - const double t172 = t69 - * (-t136 - t137 + t138 + t139 - t140 - t141 + t142 + t143 + t144 - - t24 * t2_x - t25 * t2_x - t51 - t52 + t57 + t58 - t59 - t60 - + t63 + t65 + t68); - const double t173 = t172 * t22; - const double t174 = t69 - * (t101 - t148 - t149 + t150 + t151 - t152 - t153 + t154 + t155 - + t156 - t23 * t2_y - t25 * t2_y - t84 - t85 + t90 + t91 - t92 - - t94 + t96 + t98); - const double t175 = t174 * t22; - const double t176 = t69 - * (-t114 - t115 + t120 + t121 - t122 - t124 + t126 + t128 + t131 - - t160 - t161 + t162 + t163 - t164 - t165 + t166 + t167 + t168 - - t23 * t2_z - t24 * t2_z); - const double t177 = t176 * t22; - J[0] = t32 * (-t0 * t14 + t15 * t22); - J[1] = t32 * (t0 * t22 - t15 * t33); - J[2] = t32 * (-t14 * t34 + t22 * t35); - J[3] = t32 * (t22 * t34 - t33 * t35); - J[4] = t32 * (-t14 * t36 + t22 * t37); - J[5] = t32 * (t22 * t36 - t33 * t37); - J[6] = t32 - * (-t14 * t41 + t22 * t50 - t47 - t48 * t49 - t49 * t72 - + t70 * t71); - J[7] = t32 - * (t22 * t41 - t33 * t50 - t45 * t48 - t45 * t72 + t70 * t75 - t74); - J[8] = t32 - * (t102 * t71 - t103 * t49 - t14 * t79 + t22 * t83 - t49 * t82 - - t81); - J[9] = t32 - * (t102 * t75 - t103 * t45 - t105 + t22 * t79 - t33 * t83 - - t45 * t82); - J[10] = t32 - * (-t109 * t14 - t111 - t112 * t49 + t113 * t22 + t132 * t71 - - t133 * t49); - J[11] = t32 - * (t109 * t22 - t112 * t45 - t113 * t33 + t132 * t75 - t133 * t45 - - t135); - J[12] = t32 * (t14 * t42 + t145 * t71 - t146 * t49 - t15 * t49); - J[13] = t32 * (t145 * t75 - t146 * t45 + t147 - t46 + t74); - J[14] = t32 * (t14 * t43 + t157 * t71 - t158 * t49 - t35 * t49); - J[15] = t32 * (t105 + t157 * t75 - t158 * t45 + t159 - t80); - J[16] = t32 * (t14 * t44 + t169 * t71 - t170 * t49 - t37 * t49); - J[17] = t32 * (-t110 + t135 + t169 * t75 - t170 * t45 + t171); - J[18] = t32 * (t147 + t172 * t71 - t173 * t49 + t47 - t73); - J[19] = t32 * (-t0 * t45 + t172 * t75 - t173 * t45 + t33 * t42); - J[20] = t32 * (-t104 + t159 + t174 * t71 - t175 * t49 + t81); - J[21] = t32 * (t174 * t75 - t175 * t45 + t33 * t43 - t34 * t45); - J[22] = t32 * (t111 - t134 + t171 + t176 * t71 - t177 * t49); - J[23] = t32 * (t176 * t75 - t177 * t45 + t33 * t44 - t36 * t45); - } - - // hess is (144×1) flattened in column-major order - void point_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 double t0 = t0_x * t1_x; - const double t1 = t0_y * t1_y; - const double t2 = 2 * t1; - const double t3 = t0_y * t2_y; - const double t4 = 2 * t3; - const double t5 = t0_x * t2_x; - const double t6 = 2 * t5; - const double t7 = t0_z * t1_z; - const double t8 = 2 * t7; - const double t9 = t0_z * t2_z; - const double t10 = 2 * t9; - const double t11 = t1_y * t2_y; - const double t12 = 2 * t11; - const double t13 = t1_z * t2_z; - const double t14 = 2 * t13; - const double t15 = t1_x * t2_x; - const double t16 = 2 * t15; - const double t17 = (t0_x * t0_x); - const double t18 = (t1_y * t1_y); - const double t19 = (t1_z * t1_z); - const double t20 = (t2_y * t2_y); - const double t21 = (t2_z * t2_z); - const double t22 = (t0_y * t0_y); - const double t23 = (t1_x * t1_x); - const double t24 = (t2_x * t2_x); - const double t25 = (t0_z * t0_z); - const double t26 = 2 * t0; - const double 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 double t28 = 1.0 / t27; - const double t29 = t0_x - t2_x; - const double t30 = 2 * t0_x; - const double t31 = -t30; - const double t32 = t2_x + t31; - const double t33 = t1_x + t32; - const double t34 = t0_x - t1_x; - const double t35 = t29 * t34; - const double t36 = t20 + t21; - const double t37 = -t10 + t25; - const double t38 = t22 - t4; - const double t39 = t36 + t37 + t38; - const double t40 = t17 - t6; - const double t41 = t24 + t39 + t40; - const double t42 = t0_x * t18; - const double t43 = t0_x * t19; - const double t44 = t0_x * t20; - const double t45 = t0_x * t21; - const double t46 = t1_x * t20; - const double t47 = t1_x * t21; - const double t48 = t18 * t2_x; - const double t49 = t19 * t2_x; - const double t50 = t11 * t1_x; - const double t51 = t13 * t1_x; - const double t52 = t11 * t2_x; - const double t53 = t13 * t2_x; - const double t54 = t1 * t1_x; - const double t55 = t2_x * t3; - const double t56 = t1_x * t7; - const double t57 = t2_x * t9; - const double t58 = t1 * t2_x; - const double t59 = t2_x * t7; - const double t60 = t58 + t59; - const double t61 = t1_x * t3; - const double t62 = t1_x * t9; - const double t63 = t61 + t62; - const double t64 = -t11 * t30 - t13 * t30 + t42 + t43 + t44 + t45 - t46 - - t47 - t48 - t49 + t50 + t51 + t52 + t53 - t54 - t55 - t56 - t57 - + t60 + t63; - const double t65 = t11 + t13; - const double t66 = -t9; - const double t67 = t25 + t66 - t7; - const double t68 = -t3; - const double t69 = -t1 + t22 + t68; - const double t70 = t65 + t67 + t69; - const double t71 = -t5; - const double t72 = -t0 + t17 + t71; - const double t73 = t15 + t70 + t72; - const double t74 = t64 * t73; - const double t75 = 2 * t29; - const double t76 = t28 * t75; - const double t77 = -t13; - const double t78 = t66 + t7; - const double t79 = t77 + t78; - const double t80 = -t11; - const double t81 = t1 + t68; - const double t82 = t80 + t81; - const double t83 = t36 + t79 + t82; - const double t84 = -t15; - const double t85 = t0 + t71; - const double t86 = t84 + t85; - const double t87 = t24 + t83 + t86; - const double t88 = t28 - * (2 * t28 * t34 * t41 * t64 - t29 * t33 - 2 * t35 - t74 * t76 - - t87); - const double t89 = t0_y - t2_y; - const double t90 = t34 * t89; - const double t91 = 2 * t90; - const double t92 = 2 * t0_y; - const double t93 = -t92; - const double t94 = t2_y + t93; - const double t95 = t1_y + t94; - const double t96 = t0_y * t23; - const double t97 = t0_y * t19; - const double t98 = t0_y * t24; - const double t99 = t0_y * t21; - const double t100 = t1_y * t24; - const double t101 = t1_y * t21; - const double t102 = t23 * t2_y; - const double t103 = t19 * t2_y; - const double t104 = t15 * t1_y; - const double t105 = t15 * t2_y; - const double t106 = t13 * t1_y; - const double t107 = t13 * t2_y; - const double t108 = t0 * t1_y; - const double t109 = t2_y * t5; - const double t110 = t1_y * t7; - const double t111 = t2_y * t9; - const double t112 = t0 * t2_y; - const double t113 = t2_y * t7; - const double t114 = t112 + t113; - const double t115 = t1_y * t5 + t1_y * t9; - const double t116 = -t100 - t101 - t102 - t103 + t104 + t105 + t106 - + t107 - t108 - t109 - t110 - t111 + t114 + t115 - t13 * t92 - - t15 * t92 + t96 + t97 + t98 + t99; - const double t117 = t73 * t76; - const double t118 = - t28 * (-t116 * t117 + 2 * t116 * t28 * t34 * t41 - t29 * t95 - t91); - const double t119 = t0_z - t2_z; - const double t120 = t119 * t34; - const double t121 = 2 * t120; - const double t122 = 2 * t0_z; - const double t123 = -t122; - const double t124 = t123 + t2_z; - const double t125 = t124 + t1_z; - const double t126 = t0_z * t23; - const double t127 = t0_z * t18; - const double t128 = t0_z * t24; - const double t129 = t0_z * t20; - const double t130 = t1_z * t24; - const double t131 = t1_z * t20; - const double t132 = t23 * t2_z; - const double t133 = t18 * t2_z; - const double t134 = t15 * t1_z; - const double t135 = t15 * t2_z; - const double t136 = t11 * t1_z; - const double t137 = t11 * t2_z; - const double t138 = t0 * t1_z; - const double t139 = t2_z * t5; - const double t140 = t1 * t1_z; - const double t141 = t2_z * t3; - const double t142 = t0 * t2_z; - const double t143 = t1 * t2_z; - const double t144 = t142 + t143; - const double t145 = t1_z * t3 + t1_z * t5; - const double t146 = -t11 * t122 - t122 * t15 + t126 + t127 + t128 + t129 - - t130 - t131 - t132 - t133 + t134 + t135 + t136 + t137 - t138 - - t139 - t140 - t141 + t144 + t145; - const double t147 = t28 - * (-t117 * t146 - t121 - t125 * t29 + 2 * t146 * t28 * t34 * t41); - const double t148 = t1_x * t22; - const double t149 = t1_x * t25; - const double t150 = t22 * t2_x; - const double t151 = t25 * t2_x; - const double t152 = t0_x * t3; - const double t153 = t0_x * t9; - const double t154 = t0_x * t1; - const double t155 = t0_x * t7; - const double t156 = t0_x * t11 + t0_x * t13; - const double t157 = t148 + t149 - t150 - t151 + t152 + t153 - t154 - - t155 + t156 - t44 - t45 + t46 + t47 - t52 - t53 + t55 + t57 + t60 - - 2 * t61 - 2 * t62; - const double t158 = t157 * t41; - const double t159 = 2 * t28; - const double t160 = t159 * t34; - const double t161 = - t28 * (-t117 * t157 + t158 * t160 - (t29 * t29) + t41); - const double t162 = t29 * t89; - const double t163 = t17 * t1_y; - const double t164 = t1_y * t25; - const double t165 = t17 * t2_y; - const double t166 = t25 * t2_y; - const double t167 = t0_y * t5; - const double t168 = t0_y * t9; - const double t169 = t0 * t0_y; - const double t170 = t0_y * t7; - const double t171 = t0_y * t13 + t0_y * t15; - const double 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 double t173 = - t28 * (-t117 * t172 - t162 + 2 * t172 * t28 * t34 * t41); - const double t174 = t119 * t29; - const double t175 = t17 * t1_z; - const double t176 = t1_z * t22; - const double t177 = t17 * t2_z; - const double t178 = t22 * t2_z; - const double t179 = t0_z * t5; - const double t180 = t0_z * t3; - const double t181 = t0 * t0_z; - const double t182 = t0_z * t1; - const double t183 = t0_z * t11 + t0_z * t15; - const double 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 double t185 = - t28 * (-t117 * t184 - t174 + 2 * t184 * t28 * t34 * t41); - const double t186 = -t148 - t149 + t150 + t151 - t152 - t153 + t154 - + t155 + t156 - t42 - t43 + t48 + t49 - t50 - t51 + t54 + t56 - - 2 * t58 - 2 * t59 + t63; - const double t187 = t160 * t41; - const double t188 = - t0 + t1 - t17 - t22 - t25 + t3 + t5 + t7 + t77 + t80 + t84 + t9; - const double t189 = t28 * (-t117 * t186 + t186 * t187 + t188 + t35); - const double t190 = t0_y - t1_y; - const double t191 = t190 * t29; - const double t192 = t102 + t103 - t104 - t106 + t108 + t110 - 2 * t112 - - 2 * t113 + t115 - t163 - t164 + t165 + t166 - t167 - t168 + t169 - + t170 + t171 - t96 - t97; - const double t193 = t28 * (-t117 * t192 + t187 * t192 - t191 + t91); - const double t194 = t0_z - t1_z; - const double t195 = t194 * t29; - const double t196 = -t126 - t127 + t132 + t133 - t134 - t136 + t138 - + t140 - 2 * t142 - 2 * t143 + t145 - t175 - t176 + t177 + t178 - - t179 - t180 + t181 + t182 + t183; - const double t197 = t28 * (-t117 * t196 + t121 + t187 * t196 - t195); - const double t198 = 2 * t191; - const double t199 = t159 * t89; - const double t200 = - t28 * (2 * t190 * t28 * t41 * t64 - t198 - t199 * t74 - t33 * t89); - const double t201 = t190 * t89; - const double t202 = t199 * t73; - const double t203 = t28 - * (2 * t116 * t190 * t28 * t41 - t116 * t202 - 2 * t201 - t87 - - t89 * t95); - const double t204 = t119 * t190; - const double t205 = 2 * t204; - const double t206 = t28 - * (-t125 * t89 + 2 * t146 * t190 * t28 * t41 - t146 * t202 - t205); - const double t207 = - t28 * (2 * t157 * t190 * t28 * t41 - t157 * t202 - t162); - const double t208 = t159 * t190; - const double t209 = t208 * t41; - const double t210 = - t28 * (-t172 * t202 + t172 * t209 + t41 - (t89 * t89)); - const double t211 = t119 * t89; - const double t212 = - t28 * (2 * t184 * t190 * t28 * t41 - t184 * t202 - t211); - const double t213 = - t28 * (2 * t186 * t190 * t28 * t41 - t186 * t202 + t198 - t90); - const double t214 = t28 * (t188 - t192 * t202 + t192 * t209 + t201); - const double t215 = t194 * t89; - const double t216 = t28 * (-t196 * t202 + t196 * t209 + t205 - t215); - const double t217 = 2 * t195; - const double t218 = t119 * t159; - const double t219 = t28 - * (-t119 * t33 + 2 * t194 * t28 * t41 * t64 - t217 - t218 * t74); - const double t220 = 2 * t215; - const double t221 = t218 * t73; - const double t222 = t28 - * (2 * t116 * t194 * t28 * t41 - t116 * t221 - t119 * t95 - t220); - const double t223 = t119 * t194; - const double t224 = t28 - * (-t119 * t125 + 2 * t146 * t194 * t28 * t41 - t146 * t221 - - 2 * t223 - t87); - const double t225 = - t28 * (2 * t157 * t194 * t28 * t41 - t157 * t221 - t174); - const double t226 = - t28 * (2 * t172 * t194 * t28 * t41 - t172 * t221 - t211); - const double t227 = t159 * t194; - const double t228 = t227 * t41; - const double t229 = - t28 * (-(t119 * t119) - t184 * t221 + t184 * t228 + t41); - const double t230 = - t28 * (-t120 + 2 * t186 * t194 * t28 * t41 - t186 * t221 + t217); - const double t231 = - t28 * (2 * t192 * t194 * t28 * t41 - t192 * t221 - t204 + t220); - const double t232 = t28 * (t188 - t196 * t221 + t196 * t228 + t223); - const double t233 = p_x + t32; - const double t234 = p_x + t1_x + t31; - const double t235 = t234 * t75; - const double t236 = p_x - t0_x; - const double t237 = t236 * t34; - const double t238 = p_y - t0_y; - const double t239 = t190 * t238; - const double t240 = p_z - t0_z; - const double t241 = t194 * t240; - const double t242 = t239 + t241; - const double t243 = t237 + t242; - const double t244 = t243 * t41; - const double t245 = -t12; - const double t246 = -t14; - const double t247 = t18 + t19; - const double t248 = t28 * (t245 + t246 + t247 + t36); - const double t249 = t236 * t29; - const double t250 = t238 * t89; - const double t251 = t119 * t240; - const double t252 = t250 + t251; - const double t253 = t249 + t252; - const double t254 = t253 * t73; - const double t255 = std::pow(t27, -2); - const double t256 = 4 * t255; - const double t257 = t256 * (t64 * t64); - const double t258 = t159 * t233; - const double t259 = t159 * t64; - const double t260 = t234 * t41; - const double t261 = t253 * t33; - const double t262 = 4 * t28; - const double t263 = t243 * t262; - const double t264 = t263 * t29; - const double t265 = t264 * t64; - const double t266 = -t237 - t239 - t241 + t253 + t87; - const double t267 = p_y + t94; - const double t268 = p_y + t1_y + t93; - const double t269 = t1_x * t1_y; - const double t270 = t2_x * t2_y; - const double t271 = t1_x * t2_y; - const double t272 = -t271; - const double t273 = t1_y * t2_x; - const double t274 = -t273; - const double t275 = t269 + t270 + t272 + t274; - const double t276 = t258 * t73; - const double t277 = t159 * t74; - const double t278 = 8 * t255; - const double t279 = t278 * t64; - const double t280 = 2 * t234; - const double t281 = t263 * t64; - const double t282 = t280 * t89 - t281 * t89; - const double t283 = -t116 * t264 + t268 * t75; - const double 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 double t285 = p_z + t124; - const double t286 = p_z + t123 + t1_z; - const double t287 = t1_x * t1_z; - const double t288 = t2_x * t2_z; - const double t289 = t1_x * t2_z; - const double t290 = -t289; - const double t291 = t1_z * t2_x; - const double t292 = -t291; - const double t293 = t287 + t288 + t290 + t292; - const double t294 = t119 * t280 - t119 * t281; - const double t295 = -t146 * t264 + t286 * t75; - const double 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 double t297 = -t249; - const double t298 = t236 * t41; - const double t299 = -t157 * t264 + t252; - const double 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 double t301 = t238 * t29; - const double t302 = 2 * t301; - const double t303 = t0_y * t1_x; - const double t304 = -t303; - const double t305 = t1_y * t30; - const double t306 = 2 * t273; - const double t307 = t271 - t306; - const double t308 = t0_y * t2_x; - const double t309 = t2_y * t30; - const double t310 = t308 - t309; - const double t311 = t159 * (t270 + t304 + t305 + t307 + t310); - const double t312 = t238 * t41; - const double t313 = t159 * t260; - const double t314 = t172 * t264; - const double t315 = t253 * t64; - const double t316 = t172 * t253; - const double t317 = t159 * t33; - const double t318 = t172 * t279; - const double 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 double t320 = t240 * t29; - const double t321 = 2 * t320; - const double t322 = t0_z * t1_x; - const double t323 = -t322; - const double t324 = t1_z * t30; - const double t325 = 2 * t291; - const double t326 = t289 - t325; - const double t327 = t0_z * t2_x; - const double t328 = t2_z * t30; - const double t329 = t327 - t328; - const double t330 = t159 * (t288 + t323 + t324 + t326 + t329); - const double t331 = t240 * t41; - const double t332 = t184 * t264; - const double t333 = t184 * t253; - const double t334 = t184 * t279; - const double 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 double t336 = t186 * t264; - const double t337 = -t19 + t78; - const double t338 = -t18 + t81; - const double t339 = t159 * (t337 + t338 + t65); - const double t340 = t159 * t261; - const double t341 = t186 * t279; - const double t342 = -t250; - const double t343 = -t251; - const double t344 = - 2 * t237 + 2 * t239 + 2 * t241 + t297 + t342 + t343 + t73; - const double 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 double t346 = -t308; - const double t347 = 2 * t271; - const double t348 = t273 - t347; - const double t349 = t303 - t305; - const double t350 = t159 * (t269 + t309 + t346 + t348 + t349); - const double t351 = t192 * t264; - const double t352 = t192 * t279; - const double 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 double t354 = -t327; - const double t355 = 2 * t289; - const double t356 = t291 - t355; - const double t357 = t322 - t324; - const double t358 = t159 * (t287 + t328 + t354 + t356 + t357); - const double t359 = t196 * t264; - const double t360 = t196 * t279; - const double 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 double t362 = 2 * t89; - const double t363 = t268 * t362; - const double t364 = -t16; - const double t365 = t21 + t24; - const double t366 = t19 + t23; - const double t367 = t28 * (t246 + t364 + t365 + t366); - const double t368 = (t116 * t116) * t256; - const double t369 = t116 * t159; - const double t370 = t267 * t73; - const double t371 = t268 * t41; - const double t372 = t253 * t95; - const double t373 = t263 * t89; - const double t374 = t116 * t373; - const double t375 = t1_y * t1_z; - const double t376 = t2_y * t2_z; - const double t377 = t1_y * t2_z; - const double t378 = -t377; - const double t379 = t1_z * t2_y; - const double t380 = -t379; - const double t381 = t375 + t376 + t378 + t380; - const double t382 = t146 * t159; - const double t383 = t369 * t73; - const double t384 = t116 * t278; - const double t385 = 2 * t119; - const double t386 = t119 * t263; - const double t387 = -t116 * t386 + t268 * t385; - const double t388 = -t146 * t373 + t286 * t362; - const double 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 double t390 = t236 * t89; - const double t391 = 2 * t390; - const double t392 = t0_x * t1_y; - const double t393 = -t392; - const double t394 = t1_x * t92; - const double t395 = t0_x * t2_y; - const double t396 = t2_x * t92; - const double t397 = t395 - t396; - const double t398 = t159 * (t270 + t348 + t393 + t394 + t397); - const double t399 = t158 * t159; - const double t400 = t116 * t253; - const double t401 = t157 * t373; - const double t402 = t157 * t253; - const double t403 = t159 * t95; - const double t404 = t159 * t370; - const double t405 = t157 * t384; - const double 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 double t407 = t365 + t79 + t86; - const double t408 = -t172 * t373 + t249 + t251; - const double 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 double t410 = t240 * t89; - const double t411 = 2 * t410; - const double t412 = t0_z * t1_y; - const double t413 = -t412; - const double t414 = t1_z * t92; - const double t415 = 2 * t379; - const double t416 = t377 - t415; - const double t417 = t0_z * t2_y; - const double t418 = t2_z * t92; - const double t419 = t417 - t418; - const double t420 = t159 * (t376 + t413 + t414 + t416 + t419); - const double t421 = t159 * t371; - const double t422 = t184 * t373; - const double t423 = t184 * t384; - const double 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 double t425 = -t395; - const double t426 = t392 - t394; - const double t427 = t159 * (t269 + t307 + t396 + t425 + t426); - const double t428 = t159 * t372; - const double t429 = t186 * t373; - const double t430 = t186 * t384; - const double 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 double t432 = t192 * t373; - const double t433 = t13 + t15; - const double t434 = -t23 + t85; - const double t435 = t159 * (t337 + t433 + t434); - const double t436 = t192 * t384; - const double 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 double t438 = -t417; - const double t439 = 2 * t377; - const double t440 = t379 - t439; - const double t441 = t412 - t414; - const double t442 = t159 * (t375 + t418 + t438 + t440 + t441); - const double t443 = t196 * t373; - const double t444 = t196 * t384; - const double 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 double t446 = t286 * t385; - const double t447 = t20 + t24; - const double t448 = t18 + t23; - const double t449 = t28 * (t245 + t364 + t447 + t448); - const double t450 = (t146 * t146) * t256; - const double t451 = t285 * t73; - const double t452 = t286 * t41; - const double t453 = t125 * t253; - const double t454 = t146 * t386; - const double t455 = t119 * t236; - const double t456 = 2 * t455; - const double t457 = t0_x * t1_z; - const double t458 = -t457; - const double t459 = t122 * t1_x; - const double t460 = t0_x * t2_z; - const double t461 = t122 * t2_x; - const double t462 = t460 - t461; - const double t463 = t159 * (t288 + t356 + t458 + t459 + t462); - const double t464 = t146 * t253; - const double t465 = t157 * t386; - const double t466 = t125 * t159; - const double t467 = t159 * t451; - const double t468 = t146 * t278; - const double t469 = t157 * t244; - const double t470 = t254 * t468; - const double 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 double t472 = t119 * t238; - const double t473 = 2 * t472; - const double t474 = t0_y * t1_z; - const double t475 = -t474; - const double t476 = t122 * t1_y; - const double t477 = t0_y * t2_z; - const double t478 = t122 * t2_y; - const double t479 = t477 - t478; - const double t480 = t159 * (t376 + t440 + t475 + t476 + t479); - const double t481 = t159 * t452; - const double t482 = t172 * t386; - const double t483 = t244 * t468; - const double 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 double t485 = t447 + t82 + t86; - const double t486 = -t184 * t386 + t249 + t250; - const double 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 double t488 = -t460; - const double t489 = t457 - t459; - const double t490 = t159 * (t287 + t326 + t461 + t488 + t489); - const double t491 = t382 * t73; - const double t492 = t159 * t453; - const double t493 = t186 * t386; - const double 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 double t495 = -t477; - const double t496 = t474 - t476; - const double t497 = t159 * (t375 + t416 + t478 + t495 + t496); - const double t498 = t192 * t386; - const double 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 double t500 = t196 * t386; - const double t501 = t11 + t15; - const double t502 = t159 * (t338 + t434 + t501); - const double 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 double t504 = (t157 * t157); - const double t505 = 2 * t255; - const double t506 = t0_x * t0_y; - const double t507 = t270 + t346 + t425 + t506; - const double t508 = t172 * t262; - const double 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 double t510 = t0_x * t0_z; - const double t511 = t288 + t354 + t488 + t510; - const double 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 double t513 = t159 * t298; - const double t514 = t159 * t70; - const double t515 = t159 * t73; - const double t516 = t157 * t515; - const double t517 = t253 * t76; - const double t518 = t186 * t278; - const double t519 = t157 * t254; - const double t520 = t28 - * (t160 * t402 - t186 * t513 + t186 * t517 + t236 * t516 - - t244 * t514 + t254 * t514 + t299 - t469 * t518 + t518 * t519); - const double t521 = t159 * (t274 + t310 + t347 + t426 + t506); - const double t522 = t192 * t278; - const double t523 = t28 - * (-t192 * t513 + t192 * t517 + t208 * t402 + t238 * t516 - + t244 * t521 - t254 * t521 + t301 - t391 - t401 - t469 * t522 - + t519 * t522); - const double t524 = t159 * (t292 + t329 + t355 + t489 + t510); - const double t525 = t196 * t278; - const double t526 = t28 - * (-t196 * t513 + t196 * t517 + t227 * t402 + t240 * t516 - + t244 * t524 - t254 * t524 + t320 - t456 - t465 - t469 * t525 - + t519 * t525); - const double t527 = t365 + t37 + t40; - const double t528 = (t172 * t172); - const double t529 = t0_y * t0_z; - const double t530 = t376 + t438 + t495 + t529; - const double 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 double t532 = t159 * (t272 + t306 + t349 + t397 + t506); - const double t533 = t159 * t312; - const double t534 = t199 * t253; - const double t535 = t172 * t515; - const double t536 = t172 * t518; - const double t537 = t28 - * (t160 * t316 - t186 * t533 + t186 * t534 + t236 * t535 - + t244 * t532 - t244 * t536 - t254 * t532 + t254 * t536 - t302 - - t314 + t390); - const double t538 = t159 * (t433 + t67 + t72); - const double t539 = t172 * t522; - const double t540 = t28 - * (-t192 * t533 + t192 * t534 + t208 * t316 + t238 * t535 - - t244 * t538 - t244 * t539 + t254 * t538 + t254 * t539 + t408); - const double t541 = t159 * (t380 + t419 + t439 + t496 + t529); - const double t542 = t172 * t525; - const double t543 = t28 - * (-t196 * t533 + t196 * t534 + t227 * t316 + t240 * t535 - + t244 * t541 - t244 * t542 - t254 * t541 + t254 * t542 + t410 - - t473 - t482); - const double t544 = t38 + t40 + t447; - const double t545 = (t184 * t184); - const double t546 = t159 * (t290 + t325 + t357 + t462 + t510); - const double t547 = t159 * t331; - const double t548 = t218 * t253; - const double t549 = t184 * t515; - const double t550 = t184 * t518; - const double t551 = t28 - * (t160 * t333 - t186 * t547 + t186 * t548 + t236 * t549 - + t244 * t546 - t244 * t550 - t254 * t546 + t254 * t550 - t321 - - t332 + t455); - const double t552 = t159 * (t378 + t415 + t441 + t479 + t529); - const double t553 = t184 * t522; - const double t554 = t28 - * (-t192 * t547 + t192 * t548 + t208 * t333 + t238 * t549 - + t244 * t552 - t244 * t553 - t254 * t552 + t254 * t553 - t411 - - t422 + t472); - const double t555 = t159 * (t501 + t69 + t72); - const double t556 = t184 * t525; - const double t557 = t28 - * (-t196 * t547 + t196 * t548 + t227 * t333 + t240 * t549 - - t244 * t555 - t244 * t556 + t254 * t555 + t254 * t556 + t486); - const double t558 = t25 - t8; - const double t559 = -t2 + t22; - const double t560 = t247 + t558 + t559; - const double t561 = (t186 * t186); - const double t562 = t159 * (t269 + t304 + t393 + t506); - const double t563 = t160 * t253; - const double t564 = t186 * t253; - const double t565 = t236 * t515; - const double t566 = t186 * t515; - const double t567 = t192 * t518; - const double 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 double t569 = t159 * (t287 + t323 + t458 + t510); - const double t570 = t196 * t518; - const double 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 double t572 = t17 - t26; - const double t573 = t366 + t558 + t572; - const double t574 = (t192 * t192); - const double t575 = t159 * (t375 + t413 + t475 + t529); - const double t576 = t196 * t522; - const double 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 double t578 = t448 + t559 + t572; - const double t579 = (t196 * t196); - 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); - } +#define IPC_INSTANTIATE_CLOSEST_POINT_AUTOGEN(T) \ + template void point_edge_closest_point_2D_hessian( \ + T, T, T, T, T, T, T[36]); \ + template void point_edge_closest_point_3D_hessian( \ + T, T, T, T, T, T, T, T, T, T[81]); \ + template void edge_edge_closest_point_jacobian( \ + T, T, T, T, T, T, T, T, T, T, T, T, T[24]); \ + template void edge_edge_closest_point_hessian_a( \ + T, T, T, T, T, T, T, T, T, T, T, T, T[144]); \ + template void edge_edge_closest_point_hessian_b( \ + T, T, T, T, T, T, T, T, T, T, T, T, T[144]); \ + template void point_triangle_closest_point_jacobian( \ + T, T, T, T, T, T, T, T, T, T, T, T, T[24]); \ + template void point_triangle_closest_point_hessian_0( \ + T, T, T, T, T, T, T, T, T, T, T, T, T[144]); \ + template void point_triangle_closest_point_hessian_1( \ + T, T, T, T, T, T, T, T, T, T, T, T, T[144]) - // hess is (144×1) flattened in column-major order - void point_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 double t0 = t0_y * t1_y; - const double t1 = t0_x * t1_x; - const double t2 = 2 * t1; - const double t3 = t0_y * t2_y; - const double t4 = t0_x * t2_x; - const double t5 = 2 * t0; - const double t6 = 2 * t3; - const double t7 = t0_z * t1_z; - const double t8 = t0_z * t2_z; - const double t9 = 2 * t7; - const double t10 = 2 * t8; - const double t11 = t1_y * t2_y; - const double t12 = t1_z * t2_z; - const double t13 = 2 * t11; - const double t14 = 2 * t12; - const double t15 = t1_x * t2_x; - const double t16 = 2 * t15; - const double t17 = (t0_x * t0_x); - const double t18 = (t1_y * t1_y); - const double t19 = (t1_z * t1_z); - const double t20 = (t2_y * t2_y); - const double t21 = (t2_z * t2_z); - const double t22 = (t0_y * t0_y); - const double t23 = (t1_x * t1_x); - const double t24 = (t2_x * t2_x); - const double t25 = (t0_z * t0_z); - const double t26 = 2 * t4; - const double 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 double t28 = 1.0 / t27; - const double t29 = t0_x - t1_x; - const double t30 = 2 * t0_x; - const double t31 = -t30; - const double t32 = t1_x + t31; - const double t33 = t2_x + t32; - const double t34 = t0_x - t2_x; - const double t35 = t29 * t34; - const double t36 = t18 + t19; - const double t37 = t25 - t9; - const double t38 = t22 - t5; - const double t39 = t36 + t37 + t38; - const double t40 = t17 - t2; - const double t41 = t23 + t39 + t40; - const double t42 = t0_x * t18; - const double t43 = t0_x * t19; - const double t44 = t0_x * t20; - const double t45 = t0_x * t21; - const double t46 = t1_x * t20; - const double t47 = t1_x * t21; - const double t48 = t18 * t2_x; - const double t49 = t19 * t2_x; - const double t50 = t11 * t1_x; - const double t51 = t12 * t1_x; - const double t52 = t11 * t2_x; - const double t53 = t12 * t2_x; - const double t54 = t0 * t1_x; - const double t55 = t2_x * t3; - const double t56 = t1_x * t7; - const double t57 = t2_x * t8; - const double t58 = t0 * t2_x; - const double t59 = t2_x * t7; - const double t60 = t58 + t59; - const double t61 = t1_x * t3; - const double t62 = t1_x * t8; - const double t63 = t61 + t62; - const double t64 = -t11 * t30 - t12 * t30 + t42 + t43 + t44 + t45 - t46 - - t47 - t48 - t49 + t50 + t51 + t52 + t53 - t54 - t55 - t56 - t57 - + t60 + t63; - const double t65 = -t0; - const double t66 = -t7; - const double t67 = t65 + t66; - const double t68 = -t8; - const double t69 = t12 + t25 + t68; - const double t70 = -t3; - const double t71 = t11 + t22 + t70; - const double t72 = t67 + t69 + t71; - const double t73 = -t4; - const double t74 = -t1; - const double t75 = t15 + t17 + t73 + t74; - const double t76 = t72 + t75; - const double t77 = t64 * t76; - const double t78 = 2 * t29; - const double t79 = t28 * t78; - const double t80 = -t15; - const double t81 = -t11; - const double t82 = -t12; - const double t83 = t81 + t82; - const double t84 = t3 + t4 + t8 + t80 + t83; - const double t85 = t23 + t36 + t67 + t74 + t84; - const double t86 = t28 - * (2 * t28 * t34 * t41 * t64 - t29 * t33 - 2 * t35 - t77 * t79 - - t85); - const double t87 = 2 * t0_y; - const double t88 = -t87; - const double t89 = t1_y + t88; - const double t90 = t2_y + t89; - const double t91 = t0_y - t1_y; - const double t92 = t34 * t91; - const double t93 = 2 * t92; - const double t94 = t0_y * t23; - const double t95 = t0_y * t19; - const double t96 = t0_y * t24; - const double t97 = t0_y * t21; - const double t98 = t1_y * t24; - const double t99 = t1_y * t21; - const double t100 = t23 * t2_y; - const double t101 = t19 * t2_y; - const double t102 = t15 * t1_y; - const double t103 = t15 * t2_y; - const double t104 = t12 * t1_y; - const double t105 = t12 * t2_y; - const double t106 = t1 * t1_y; - const double t107 = t2_y * t4; - const double t108 = t1_y * t7; - const double t109 = t2_y * t8; - const double t110 = t1 * t2_y + t2_y * t7; - const double t111 = t1_y * t4; - const double t112 = t1_y * t8; - const double t113 = t111 + t112; - const double t114 = -t100 - t101 + t102 + t103 + t104 + t105 - t106 - - t107 - t108 - t109 + t110 + t113 - t12 * t87 - t15 * t87 + t94 - + t95 + t96 + t97 - t98 - t99; - const double t115 = t76 * t79; - const double t116 = - t28 * (-t114 * t115 + 2 * t114 * t28 * t34 * t41 - t29 * t90 - t93); - const double t117 = 2 * t0_z; - const double t118 = -t117; - const double t119 = t118 + t1_z; - const double t120 = t119 + t2_z; - const double t121 = t0_z - t1_z; - const double t122 = t121 * t34; - const double t123 = 2 * t122; - const double t124 = t0_z * t23; - const double t125 = t0_z * t18; - const double t126 = t0_z * t24; - const double t127 = t0_z * t20; - const double t128 = t1_z * t24; - const double t129 = t1_z * t20; - const double t130 = t23 * t2_z; - const double t131 = t18 * t2_z; - const double t132 = t15 * t1_z; - const double t133 = t15 * t2_z; - const double t134 = t11 * t1_z; - const double t135 = t11 * t2_z; - const double t136 = t1 * t1_z; - const double t137 = t2_z * t4; - const double t138 = t0 * t1_z; - const double t139 = t2_z * t3; - const double t140 = t0 * t2_z + t1 * t2_z; - const double t141 = t1_z * t4; - const double t142 = t1_z * t3; - const double t143 = t141 + t142; - const double t144 = -t11 * t117 - t117 * t15 + t124 + t125 + t126 + t127 - - t128 - t129 - t130 - t131 + t132 + t133 + t134 + t135 - t136 - - t137 - t138 - t139 + t140 + t143; - const double t145 = t28 - * (-t115 * t144 - t120 * t29 - t123 + 2 * t144 * t28 * t34 * t41); - const double t146 = t1_x * t22; - const double t147 = t1_x * t25; - const double t148 = t22 * t2_x; - const double t149 = t25 * t2_x; - const double t150 = t0_x * t3; - const double t151 = t0_x * t8; - const double t152 = t0 * t0_x; - const double t153 = t0_x * t7; - const double t154 = t0_x * t11 + t0_x * t12; - const double t155 = t146 + t147 - t148 - t149 + t150 + t151 - t152 - - t153 + t154 - t44 - t45 + t46 + t47 - t52 - t53 + t55 + t57 + t60 - - 2 * t61 - 2 * t62; - const double t156 = 2 * t28; - const double t157 = t156 * t34; - const double t158 = t0 + t1 - t17 - t22 - t25 + t7 + t84; - const double t159 = - t28 * (-t115 * t155 + t155 * t157 * t41 + t158 + t35); - const double t160 = t0_y - t2_y; - const double t161 = t160 * t29; - const double t162 = t17 * t1_y; - const double t163 = t1_y * t25; - const double t164 = t17 * t2_y; - const double t165 = t25 * t2_y; - const double t166 = t0_y * t4; - const double t167 = t0_y * t8; - const double t168 = t0_y * t1; - const double t169 = t0_y * t7; - const double t170 = t0_y * t12 + t0_y * t15; - const double t171 = -t103 - t105 + t107 + t109 + t110 - 2 * t111 - - 2 * t112 + t162 + t163 - t164 - t165 + t166 + t167 - t168 - t169 - + t170 - t96 - t97 + t98 + t99; - const double t172 = - t28 * (-t115 * t171 - t161 + 2 * t171 * t28 * t34 * t41 + t93); - const double t173 = t0_z - t2_z; - const double t174 = t173 * t29; - const double t175 = t17 * t1_z; - const double t176 = t1_z * t22; - const double t177 = t17 * t2_z; - const double t178 = t22 * t2_z; - const double t179 = t0_z * t4; - const double t180 = t0_z * t3; - const double t181 = t0_z * t1; - const double t182 = t0 * t0_z; - const double t183 = t0_z * t11 + t0_z * t15; - const double t184 = -t126 - t127 + t128 + t129 - t133 - t135 + t137 - + t139 + t140 - 2 * t141 - 2 * t142 + t175 + t176 - t177 - t178 - + t179 + t180 - t181 - t182 + t183; - const double t185 = - t28 * (-t115 * t184 + t123 - t174 + 2 * t184 * t28 * t34 * t41); - const double t186 = -t146 - t147 + t148 + t149 - t150 - t151 + t152 - + t153 + t154 - t42 - t43 + t48 + t49 - t50 - t51 + t54 + t56 - - 2 * t58 - 2 * t59 + t63; - const double t187 = t186 * t41; - const double t188 = - t28 * (-t115 * t186 + t157 * t187 - (t29 * t29) + t41); - const double t189 = t29 * t91; - const double 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 double t191 = - t28 * (-t115 * t190 - t189 + 2 * t190 * t28 * t34 * t41); - const double t192 = t121 * t29; - const double 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 double t194 = - t28 * (-t115 * t193 - t192 + 2 * t193 * t28 * t34 * t41); - const double t195 = 2 * t161; - const double t196 = t156 * t91; - const double t197 = - t28 * (2 * t160 * t28 * t41 * t64 - t195 - t196 * t77 - t33 * t91); - const double t198 = t160 * t91; - const double t199 = t196 * t76; - const double t200 = t28 - * (2 * t114 * t160 * t28 * t41 - t114 * t199 - 2 * t198 - t85 - - t90 * t91); - const double t201 = t121 * t160; - const double t202 = 2 * t201; - const double t203 = t28 - * (-t120 * t91 + 2 * t144 * t160 * t28 * t41 - t144 * t199 - t202); - const double t204 = t156 * t160; - const double t205 = t204 * t41; - const double t206 = t28 * (-t155 * t199 + t155 * t205 + t195 - t92); - const double t207 = t28 * (t158 - t171 * t199 + t171 * t205 + t198); - const double t208 = t173 * t91; - const double t209 = - t28 * (2 * t160 * t184 * t28 * t41 - t184 * t199 + t202 - t208); - const double t210 = - t28 * (2 * t160 * t186 * t28 * t41 - t186 * t199 - t189); - const double t211 = - t28 * (-t190 * t199 + t190 * t205 + t41 - (t91 * t91)); - const double t212 = t121 * t91; - const double t213 = - t28 * (2 * t160 * t193 * t28 * t41 - t193 * t199 - t212); - const double t214 = 2 * t174; - const double t215 = t121 * t156; - const double t216 = t28 - * (-t121 * t33 + 2 * t173 * t28 * t41 * t64 - t214 - t215 * t77); - const double t217 = 2 * t208; - const double t218 = t215 * t76; - const double t219 = t28 - * (2 * t114 * t173 * t28 * t41 - t114 * t218 - t121 * t90 - t217); - const double t220 = t121 * t173; - const double t221 = t28 - * (-t120 * t121 + 2 * t144 * t173 * t28 * t41 - t144 * t218 - - 2 * t220 - t85); - const double t222 = t156 * t173; - const double t223 = t222 * t41; - const double t224 = t28 * (-t122 - t155 * t218 + t155 * t223 + t214); - const double t225 = t28 * (-t171 * t218 + t171 * t223 - t201 + t217); - const double t226 = t28 * (t158 - t184 * t218 + t184 * t223 + t220); - const double t227 = - t28 * (2 * t173 * t186 * t28 * t41 - t186 * t218 - t192); - const double t228 = - t28 * (2 * t173 * t190 * t28 * t41 - t190 * t218 - t212); - const double t229 = - t28 * (-(t121 * t121) - t193 * t218 + t193 * t223 + t41); - const double t230 = p_x + t32; - const double t231 = p_x - t0_x; - const double t232 = t231 * t29; - const double t233 = p_y - t0_y; - const double t234 = t233 * t91; - const double t235 = p_z - t0_z; - const double t236 = t121 * t235; - const double t237 = t234 + t236; - const double t238 = t232 + t237; - const double t239 = t238 * t76; - const double t240 = -t13; - const double t241 = -t14; - const double t242 = t20 + t21; - const double t243 = t240 + t241 + t242 + t36; - const double t244 = t231 * t34; - const double t245 = t160 * t233; - const double t246 = t173 * t235; - const double t247 = t245 + t246; - const double t248 = t244 + t247; - const double t249 = (t64 * t64); - const double t250 = 1 / (t27 * t27); - const double t251 = p_x + t2_x + t31; - const double t252 = t156 * t230; - const double t253 = t248 * t41; - const double t254 = 4 * t250; - const double t255 = 4 * t28; - const double t256 = t248 * t255; - const double t257 = t256 * t29; - const double t258 = -t232; - const double t259 = -t234; - const double t260 = -t236; - const double t261 = t258 + t259 + t260; - const double t262 = t251 * t78 - t257 * t64 + t261; - const double t263 = t68 + t7; - const double t264 = t0 + t70; - const double t265 = t263 + t264; - const double t266 = t12 - t19; - const double t267 = t11 - t18; - const double t268 = t265 + t266 + t267; - const double t269 = t1 + t73; - const double t270 = t15 - t23; - const double t271 = t248 + t268 + t269 + t270; - const double t272 = p_y + t89; - const double t273 = p_y + t2_y + t88; - const double t274 = t1_x * t1_y; - const double t275 = t2_x * t2_y; - const double t276 = t1_x * t2_y; - const double t277 = -t276; - const double t278 = t1_y * t2_x; - const double t279 = -t278; - const double t280 = t274 + t275 + t277 + t279; - const double t281 = t252 * t76; - const double t282 = t156 * t77; - const double t283 = 8 * t250; - const double t284 = t283 * t64; - const double t285 = 2 * t251; - const double t286 = t256 * t64; - const double t287 = t285 * t91 - t286 * t91; - const double t288 = -t114 * t257 + t273 * t78; - const double 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 double t290 = p_z + t119; - const double t291 = p_z + t118 + t2_z; - const double t292 = t1_x * t1_z; - const double t293 = t2_x * t2_z; - const double t294 = t1_x * t2_z; - const double t295 = -t294; - const double t296 = t1_z * t2_x; - const double t297 = -t296; - const double t298 = t292 + t293 + t295 + t297; - const double t299 = t121 * t285 - t121 * t286; - const double t300 = -t144 * t257 + t291 * t78; - const double 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 double t302 = t155 * t257; - const double t303 = t156 * (t242 + t265 + t83); - const double t304 = t238 * t64; - const double t305 = t251 * t41; - const double t306 = t155 * t156; - const double t307 = t238 * t33; - const double t308 = t155 * t284; - const double t309 = 2 * t244 + 2 * t245 + 2 * t246 + t76; - const double 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 double t311 = t0_y * t1_x; - const double t312 = -t311; - const double t313 = t1_y * t30; - const double t314 = 2 * t278; - const double t315 = t276 - t314; - const double t316 = t0_y * t2_x; - const double t317 = t2_y * t30; - const double t318 = t316 - t317; - const double t319 = t156 * (t275 + t312 + t313 + t315 + t318); - const double t320 = t156 * t171; - const double t321 = t171 * t257; - const double t322 = t171 * t284; - const double 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 double t324 = t0_z * t1_x; - const double t325 = -t324; - const double t326 = t1_z * t30; - const double t327 = 2 * t296; - const double t328 = t294 - t327; - const double t329 = t0_z * t2_x; - const double t330 = t2_z * t30; - const double t331 = t329 - t330; - const double t332 = t156 * (t293 + t325 + t326 + t328 + t331); - const double t333 = t156 * t184; - const double t334 = t184 * t257; - const double t335 = t184 * t284; - const double 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 double t337 = t231 * t41; - const double t338 = t156 * t64; - const double t339 = -t186 * t257 + t237; - const double 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 double t341 = t233 * t29; - const double t342 = 2 * t341; - const double t343 = -t316; - const double t344 = 2 * t276; - const double t345 = t278 - t344; - const double t346 = t311 - t313; - const double t347 = t156 * (t274 + t317 + t343 + t345 + t346); - const double t348 = t233 * t41; - const double t349 = t156 * t305; - const double t350 = t190 * t257; - const double t351 = t190 * t238; - const double t352 = t156 * t33; - const double t353 = t190 * t284; - const double 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 double t355 = t235 * t29; - const double t356 = 2 * t355; - const double t357 = -t329; - const double t358 = 2 * t294; - const double t359 = t296 - t358; - const double t360 = t324 - t326; - const double t361 = t156 * (t292 + t330 + t357 + t359 + t360); - const double t362 = t235 * t41; - const double t363 = t193 * t257; - const double t364 = t193 * t238; - const double t365 = t193 * t284; - const double 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 double t367 = -t16; - const double t368 = t21 + t24; - const double t369 = t19 + t23; - const double t370 = t241 + t367 + t368 + t369; - const double t371 = (t114 * t114); - const double t372 = t114 * t156; - const double t373 = t272 * t76; - const double t374 = t261 + t271; - const double t375 = 2 * t91; - const double t376 = t256 * t91; - const double t377 = -t114 * t376 + t273 * t375; - const double t378 = t1_y * t1_z; - const double t379 = t2_y * t2_z; - const double t380 = t1_y * t2_z; - const double t381 = -t380; - const double t382 = t1_z * t2_y; - const double t383 = -t382; - const double t384 = t378 + t379 + t381 + t383; - const double t385 = t144 * t156; - const double t386 = t372 * t76; - const double t387 = t114 * t283; - const double t388 = 2 * t121; - const double t389 = t121 * t256; - const double t390 = -t114 * t389 + t273 * t388; - const double t391 = -t144 * t376 + t291 * t375; - const double 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 double t393 = t0_x * t1_y; - const double t394 = -t393; - const double t395 = t1_x * t87; - const double t396 = t0_x * t2_y; - const double t397 = t2_x * t87; - const double t398 = t396 - t397; - const double t399 = t156 * (t275 + t345 + t394 + t395 + t398); - const double t400 = t114 * t238; - const double t401 = t273 * t41; - const double t402 = t238 * t90; - const double t403 = t155 * t376; - const double t404 = t155 * t387; - const double 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 double t406 = t171 * t376; - const double t407 = t263 + t269; - const double t408 = t156 * (t368 + t407 + t80 + t82); - const double t409 = t171 * t387; - const double t410 = t261 + t309; - const double 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 double t412 = t0_z * t1_y; - const double t413 = -t412; - const double t414 = t1_z * t87; - const double t415 = 2 * t382; - const double t416 = t380 - t415; - const double t417 = t0_z * t2_y; - const double t418 = t2_z * t87; - const double t419 = t417 - t418; - const double t420 = t156 * (t379 + t413 + t414 + t416 + t419); - const double t421 = t184 * t376; - const double t422 = t184 * t387; - const double 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 double t424 = t231 * t91; - const double t425 = 2 * t424; - const double t426 = -t396; - const double t427 = t393 - t395; - const double t428 = t156 * (t274 + t315 + t397 + t426 + t427); - const double t429 = t156 * t187; - const double t430 = t186 * t376; - const double t431 = t186 * t238; - const double t432 = t156 * t90; - const double t433 = t156 * t373; - const double t434 = t186 * t387; - const double 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 double t436 = t266 + t270 + t407; - const double t437 = -t190 * t376 + t232 + t236; - const double 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 double t439 = t235 * t91; - const double t440 = 2 * t439; - const double t441 = -t417; - const double t442 = 2 * t380; - const double t443 = t382 - t442; - const double t444 = t412 - t414; - const double t445 = t156 * (t378 + t418 + t441 + t443 + t444); - const double t446 = t193 * t376; - const double t447 = t193 * t387; - const double 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 double t449 = t20 + t24; - const double t450 = t18 + t23; - const double t451 = t240 + t367 + t449 + t450; - const double t452 = (t144 * t144); - const double t453 = t290 * t76; - const double t454 = -t144 * t389 + t291 * t388; - const double t455 = t0_x * t1_z; - const double t456 = -t455; - const double t457 = t117 * t1_x; - const double t458 = t0_x * t2_z; - const double t459 = t117 * t2_x; - const double t460 = t458 - t459; - const double t461 = t156 * (t293 + t359 + t456 + t457 + t460); - const double t462 = t385 * t76; - const double t463 = t144 * t238; - const double t464 = t291 * t41; - const double t465 = t120 * t238; - const double t466 = t155 * t389; - const double t467 = t144 * t283; - const double t468 = t155 * t467; - const double 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 double t470 = t0_y * t1_z; - const double t471 = -t470; - const double t472 = t117 * t1_y; - const double t473 = t0_y * t2_z; - const double t474 = t117 * t2_y; - const double t475 = t473 - t474; - const double t476 = t156 * (t379 + t443 + t471 + t472 + t475); - const double t477 = t171 * t389; - const double t478 = t171 * t467; - const double 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 double t480 = t184 * t389; - const double t481 = t264 + t269; - const double t482 = t156 * (t449 + t481 + t80 + t81); - const double t483 = t184 * t467; - const double 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 double t485 = t121 * t231; - const double t486 = 2 * t485; - const double t487 = -t458; - const double t488 = t455 - t457; - const double t489 = t156 * (t292 + t328 + t459 + t487 + t488); - const double t490 = t186 * t389; - const double t491 = t120 * t156; - const double t492 = t156 * t453; - const double t493 = t186 * t467; - const double 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 double t495 = t121 * t233; - const double t496 = 2 * t495; - const double t497 = -t473; - const double t498 = t470 - t472; - const double t499 = t156 * (t378 + t416 + t474 + t497 + t498); - const double t500 = t190 * t389; - const double t501 = t190 * t467; - const double 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 double t503 = t267 + t270 + t481; - const double t504 = -t193 * t389 + t232 + t234; - const double 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 double t506 = -t10 + t25; - const double t507 = t22 - t6; - const double t508 = t242 + t506 + t507; - const double t509 = (t155 * t155); - const double t510 = t0_x * t0_y; - const double t511 = t156 * (t275 + t343 + t426 + t510); - const double t512 = t157 * t238; - const double t513 = t155 * t238; - const double t514 = t231 * t76; - const double t515 = t306 * t76; - const double t516 = t155 * t283; - const double t517 = t171 * t516; - const double 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 double t519 = t0_x * t0_z; - const double t520 = t156 * (t293 + t357 + t487 + t519); - const double t521 = t184 * t516; - const double 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 double t523 = t156 * t72; - const double t524 = t156 * t514; - const double t525 = t186 * t516; - const double t526 = t28 - * (t157 * t431 + t186 * t524 + t239 * t523 + t239 * t525 - - t253 * t523 - t253 * t525 - t306 * t337 + t339 + t513 * t79); - const double t527 = t156 * (t279 + t318 + t344 + t427 + t510); - const double t528 = t190 * t516; - const double t529 = t28 - * (t157 * t351 + t190 * t524 + t196 * t513 - t239 * t527 - + t239 * t528 + t253 * t527 - t253 * t528 - t306 * t348 - t342 - - t350 + t424); - const double t530 = t156 * (t297 + t331 + t358 + t488 + t519); - const double t531 = t193 * t516; - const double t532 = t28 - * (t157 * t364 + t193 * t524 + t215 * t513 - t239 * t530 - + t239 * t531 + t253 * t530 - t253 * t531 - t306 * t362 - t356 - - t363 + t485); - const double t533 = t17 - t26; - const double t534 = t368 + t506 + t533; - const double t535 = (t171 * t171); - const double t536 = t0_y * t0_z; - const double t537 = t156 * (t379 + t441 + t497 + t536); - const double t538 = t171 * t238; - const double t539 = t233 * t76; - const double t540 = t235 * t76; - const double t541 = t171 * t283; - const double t542 = t184 * t541; - const double 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 double t544 = t156 * (t277 + t314 + t346 + t398 + t510); - const double t545 = t156 * t539; - const double t546 = t186 * t541; - const double t547 = t28 - * (t186 * t545 + t204 * t431 - t239 * t544 + t239 * t546 - + t253 * t544 - t253 * t546 - t320 * t337 + t341 - t425 - t430 - + t538 * t79); - const double t548 = t156 * (t66 + t69 + t75); - const double t549 = t190 * t541; - const double t550 = t28 - * (t190 * t545 + t196 * t538 + t204 * t351 + t239 * t548 - + t239 * t549 - t253 * t548 - t253 * t549 - t320 * t348 + t437); - const double t551 = t156 * (t383 + t419 + t442 + t498 + t536); - const double t552 = t193 * t541; - const double t553 = t28 - * (t193 * t545 + t204 * t364 + t215 * t538 - t239 * t551 - + t239 * t552 + t253 * t551 - t253 * t552 - t320 * t362 - t440 - - t446 + t495); - const double t554 = t449 + t507 + t533; - const double t555 = (t184 * t184); - const double t556 = t156 * (t295 + t327 + t360 + t460 + t519); - const double t557 = t184 * t238; - const double t558 = t156 * t540; - const double t559 = t184 * t283; - const double t560 = t186 * t253; - const double t561 = t239 * t559; - const double t562 = t28 - * (t186 * t558 + t186 * t561 + t222 * t431 - t239 * t556 - + t253 * t556 - t333 * t337 + t355 - t486 - t490 + t557 * t79 - - t559 * t560); - const double t563 = t156 * (t381 + t415 + t444 + t475 + t536); - const double t564 = t253 * t559; - const double t565 = t28 - * (t190 * t558 + t190 * t561 - t190 * t564 + t196 * t557 - + t222 * t351 - t239 * t563 + t253 * t563 - t333 * t348 + t439 - - t496 - t500); - const double t566 = t156 * (t65 + t71 + t75); - const double t567 = t28 - * (t193 * t558 + t193 * t561 - t193 * t564 + t215 * t557 - + t222 * t364 + t239 * t566 - t253 * t566 - t333 * t362 + t504); - const double t568 = (t186 * t186); - const double t569 = 2 * t250; - const double t570 = t274 + t312 + t394 + t510; - const double t571 = t190 * t255; - const double 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 double t573 = t292 + t325 + t456 + t519; - const double 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 double t575 = t369 + t37 + t40; - const double t576 = (t190 * t190); - const double t577 = t378 + t413 + t471 + t536; - const double 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 double t579 = t38 + t40 + t450; - const double t580 = (t193 * t193); - 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); - } +IPC_INSTANTIATE_CLOSEST_POINT_AUTOGEN(float); +IPC_INSTANTIATE_CLOSEST_POINT_AUTOGEN(double); +#undef IPC_INSTANTIATE_CLOSEST_POINT_AUTOGEN -} // namespace autogen -} // namespace ipc +} // namespace ipc::autogen diff --git a/src/ipc/tangent/closest_point.hpp b/src/ipc/tangent/closest_point.hpp index 946b3b023..2fa7570d6 100644 --- a/src/ipc/tangent/closest_point.hpp +++ b/src/ipc/tangent/closest_point.hpp @@ -2,8 +2,380 @@ #include +#include + +#include +#include +#include + namespace ipc { +// Symbolically generated derivatives +namespace autogen { + /// hess is (6×6) flattened in column-major order + template + void point_edge_closest_point_2D_hessian( + T p_x, T p_y, T e0_x, T e0_y, T e1_x, T e1_y, T hess[36]); + + /// hess is (9×9) flattened in column-major order + template + void point_edge_closest_point_3D_hessian( + T p_x, + T p_y, + T p_z, + T e0_x, + T e0_y, + T e0_z, + T e1_x, + T e1_y, + T e1_z, + T hess[81]); + + /// J is (2×12) flattened in column-major order + template + void edge_edge_closest_point_jacobian( + T ea0_x, + T ea0_y, + T ea0_z, + T ea1_x, + T ea1_y, + T ea1_z, + T eb0_x, + T eb0_y, + T eb0_z, + T eb1_x, + T eb1_y, + T eb1_z, + T J[24]); + + /// hess is (144×1) flattened in column-major order + template + void edge_edge_closest_point_hessian_a( + T ea0_x, + T ea0_y, + T ea0_z, + T ea1_x, + T ea1_y, + T ea1_z, + T eb0_x, + T eb0_y, + T eb0_z, + T eb1_x, + T eb1_y, + T eb1_z, + T hess[144]); + + /// hess is (144×1) flattened in column-major order + template + void edge_edge_closest_point_hessian_b( + T ea0_x, + T ea0_y, + T ea0_z, + T ea1_x, + T ea1_y, + T ea1_z, + T eb0_x, + T eb0_y, + T eb0_z, + T eb1_x, + T eb1_y, + T eb1_z, + T hess[144]); + + /// J is (2×12) flattened in column-major order + template + void point_triangle_closest_point_jacobian( + T p_x, + T p_y, + T p_z, + T t0_x, + T t0_y, + T t0_z, + T t1_x, + T t1_y, + T t1_z, + T t2_x, + T t2_y, + T t2_z, + T J[24]); + + /// hess is (144×1) flattened in column-major order + template + void point_triangle_closest_point_hessian_0( + T p_x, + T p_y, + T p_z, + T t0_x, + T t0_y, + T t0_z, + T t1_x, + T t1_y, + T t1_z, + T t2_x, + T t2_y, + T t2_z, + T hess[144]); + + /// hess is (144×1) flattened in column-major order + template + void point_triangle_closest_point_hessian_1( + T p_x, + T p_y, + T p_z, + T t0_x, + T t0_y, + T t0_z, + T t1_x, + T t1_y, + T t1_z, + T t2_x, + T t2_y, + T t2_z, + T hess[144]); +} // namespace autogen + +namespace detail { + /// @brief Residual bound for the 2×2 solves below. + /// + /// The 1e-10 bound is tuned for double, so scale it by the relative + /// precision of T; otherwise the float instantiation would be held to a + /// double-precision bound. + template + inline constexpr double CLOSEST_POINT_RESIDUAL_TOL = 1e-10 + * (static_cast(std::numeric_limits::epsilon()) + / std::numeric_limits::epsilon()); + + // ======================================================================== + // Point - Edge + + /// @brief Compute the barycentric coordinate of the closest point on the edge. + /// @tparam T The scalar type. + /// @tparam dim The dimension (2 or 3). + /// @param p Point + /// @param e0 First edge point + /// @param e1 Second edge point + /// @return Barycentric coordinate of the closest point + template + inline T point_edge_closest_point( + Eigen::ConstRef> p, + Eigen::ConstRef> e0, + Eigen::ConstRef> e1) + { + static_assert(dim == 2 || dim == 3, "point-edge is only 2D or 3D"); + const Eigen::Vector e = e1 - e0; + return (p - e0).dot(e) / e.squaredNorm(); + } + + /// @brief Compute the Jacobian of the closest point on the edge. + /// @tparam T The scalar type. + /// @tparam dim The dimension (2 or 3). + /// @param p Point + /// @param e0 First edge point + /// @param e1 Second edge point + /// @return Jacobian of the closest point + template + inline Eigen::Vector point_edge_closest_point_jacobian( + Eigen::ConstRef> p, + Eigen::ConstRef> e0, + Eigen::ConstRef> e1) + { + static_assert(dim == 2 || dim == 3, "point-edge is only 2D or 3D"); + + const Eigen::Vector e = e1 - e0; + const Eigen::Vector e2p = p - e0; + const T e_sqnorm = e.squaredNorm(); + + Eigen::Vector J; + J.template head() = e / e_sqnorm; + J.template segment(dim) = + (T(2) / e_sqnorm * e.dot(e2p) * e - e - e2p) / e_sqnorm; + J.template tail() = + (e2p - T(2) / e_sqnorm * e.dot(e2p) * e) / e_sqnorm; + return J; + } + + /// @brief Compute the Hessian of the closest point on the edge. + /// @tparam T The scalar type. + /// @tparam dim The dimension (2 or 3). + /// @param p Point + /// @param e0 First edge point + /// @param e1 Second edge point + /// @return Hessian of the closest point + template + inline Eigen::Matrix point_edge_closest_point_hessian( + Eigen::ConstRef> p, + Eigen::ConstRef> e0, + Eigen::ConstRef> e1) + { + static_assert(dim == 2 || dim == 3, "point-edge is only 2D or 3D"); + + Eigen::Matrix H; + if constexpr (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 + + /// @brief Compute the barycentric coordinates of the closest points between two edges. + /// @tparam T The scalar type. + /// @param ea0 First point of the first edge + /// @param ea1 Second point of the first edge + /// @param eb0 First point of the second edge + /// @param eb1 Second point of the second edge + /// @return Barycentric coordinates of the closest points + template + inline Eigen::Vector2 edge_edge_closest_point( + Eigen::ConstRef> ea0, + Eigen::ConstRef> ea1, + Eigen::ConstRef> eb0, + Eigen::ConstRef> eb1) + { + const Eigen::Vector3 eb_to_ea = ea0 - eb0; + const Eigen::Vector3 ea = ea1 - ea0; + const Eigen::Vector3 eb = eb1 - eb0; + + Eigen::Matrix2 A; + A(0, 0) = ea.squaredNorm(); + A(0, 1) = A(1, 0) = -eb.dot(ea); + A(1, 1) = eb.squaredNorm(); + + Eigen::Vector2 rhs; + rhs[0] = -eb_to_ea.dot(ea); + rhs[1] = eb_to_ea.dot(eb); + + const Eigen::Vector2 x = A.ldlt().solve(rhs); + assert((A * x - rhs).norm() < CLOSEST_POINT_RESIDUAL_TOL); + return x; + } + + /// @brief Compute the Jacobian of the closest points between two edges. + /// @tparam T The scalar type. + /// @param ea0 First point of the first edge + /// @param ea1 Second point of the first edge + /// @param eb0 First point of the second edge + /// @param eb1 Second point of the second edge + /// @return Jacobian of the closest points + template + inline Eigen::Matrix edge_edge_closest_point_jacobian( + Eigen::ConstRef> ea0, + Eigen::ConstRef> ea1, + Eigen::ConstRef> eb0, + Eigen::ConstRef> eb1) + { + Eigen::Matrix J; + autogen::edge_edge_closest_point_jacobian( + ea0[0], ea0[1], ea0[2], ea1[0], ea1[1], ea1[2], eb0[0], eb0[1], + eb0[2], eb1[0], eb1[1], eb1[2], J.data()); + return J; + } + + /// @brief Compute the Hessian of the closest points between two edges. + /// @tparam T The scalar type. + /// @param ea0 First point of the first edge + /// @param ea1 Second point of the first edge + /// @param eb0 First point of the second edge + /// @param eb1 Second point of the second edge + /// @return Hessian of the closest points (2x12x12 tensor) + template + inline std::array, 2> + edge_edge_closest_point_hessian( + Eigen::ConstRef> ea0, + Eigen::ConstRef> ea1, + Eigen::ConstRef> eb0, + Eigen::ConstRef> eb1) + { + std::array, 2> H; + autogen::edge_edge_closest_point_hessian_a( + ea0[0], ea0[1], ea0[2], ea1[0], ea1[1], ea1[2], eb0[0], eb0[1], + eb0[2], eb1[0], eb1[1], eb1[2], H[0].data()); + autogen::edge_edge_closest_point_hessian_b( + ea0[0], ea0[1], ea0[2], ea1[0], ea1[1], ea1[2], eb0[0], eb0[1], + eb0[2], eb1[0], eb1[1], eb1[2], H[1].data()); + return H; + } + + // ======================================================================== + // Point - Triangle + + /// @brief Compute the barycentric coordinates of the closest point on the triangle. + /// @tparam T The scalar type. + /// @param p Point + /// @param t0 Triangle's first vertex + /// @param t1 Triangle's second vertex + /// @param t2 Triangle's third vertex + /// @return Barycentric coordinates of the closest point + template + inline Eigen::Vector2 point_triangle_closest_point( + Eigen::ConstRef> p, + Eigen::ConstRef> t0, + Eigen::ConstRef> t1, + Eigen::ConstRef> t2) + { + Eigen::Matrix basis; + basis.row(0) = Eigen::RowVector3(t1 - t0); // edge 0 + basis.row(1) = Eigen::RowVector3(t2 - t0); // edge 1 + const Eigen::Matrix2 A = basis * basis.transpose(); + const Eigen::Vector2 b = basis * (p - t0); + const Eigen::Vector2 x = A.ldlt().solve(b); + assert((A * x - b).norm() < CLOSEST_POINT_RESIDUAL_TOL); + return x; + } + + /// @brief Compute the Jacobian of the closest point on the triangle. + /// @tparam T The scalar type. + /// @param p Point + /// @param t0 Triangle's first vertex + /// @param t1 Triangle's second vertex + /// @param t2 Triangle's third vertex + /// @return Jacobian of the closest point + template + inline Eigen::Matrix point_triangle_closest_point_jacobian( + Eigen::ConstRef> p, + Eigen::ConstRef> t0, + Eigen::ConstRef> t1, + Eigen::ConstRef> t2) + { + Eigen::Matrix J; + autogen::point_triangle_closest_point_jacobian( + p[0], p[1], p[2], t0[0], t0[1], t0[2], t1[0], t1[1], t1[2], t2[0], + t2[1], t2[2], J.data()); + return J; + } + + /// @brief Compute the Hessian of the closest point on the triangle. + /// @tparam T The scalar type. + /// @param p Point + /// @param t0 Triangle's first vertex + /// @param t1 Triangle's second vertex + /// @param t2 Triangle's third vertex + /// @return Hessian of the closest point (2x12x12 tensor) + template + inline std::array, 2> + point_triangle_closest_point_hessian( + Eigen::ConstRef> p, + Eigen::ConstRef> t0, + Eigen::ConstRef> t1, + Eigen::ConstRef> t2) + { + std::array, 2> H; + autogen::point_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::point_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()); + return H; + } +} // namespace detail + // ============================================================================ // Point - Edge @@ -12,231 +384,233 @@ namespace ipc { /// @param e0 First edge point /// @param e1 Second edge point /// @return barycentric coordinates of the closest point -double point_edge_closest_point( - Eigen::ConstRef p, - Eigen::ConstRef e0, - Eigen::ConstRef e1); +template +inline auto point_edge_closest_point( + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& e0, + const Eigen::MatrixBase& e1) +{ + using T = typename DerivedP::Scalar; + + if constexpr (dim_v == 2) { + return detail::point_edge_closest_point(p, e0, e1); + } else if constexpr (dim_v == 3) { + return detail::point_edge_closest_point(p, e0, e1); + } else if (p.size() == 2) { + return detail::point_edge_closest_point(p, e0, e1); + } else { + assert(p.size() == 3); + return detail::point_edge_closest_point(p, e0, e1); + } +} /// @brief Compute the Jacobian of the closest point on the edge. /// @param p Point /// @param e0 First edge point /// @param e1 Second edge point /// @return Jacobian of the closest point -VectorMax9d point_edge_closest_point_jacobian( - Eigen::ConstRef p, - Eigen::ConstRef e0, - Eigen::ConstRef e1); +template +inline auto point_edge_closest_point_jacobian( + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& e0, + const Eigen::MatrixBase& e1) +{ + using T = typename DerivedP::Scalar; + + if constexpr (dim_v == 2) { + return detail::point_edge_closest_point_jacobian(p, e0, e1); + } else if constexpr (dim_v == 3) { + return detail::point_edge_closest_point_jacobian(p, e0, e1); + } else if (p.size() == 2) { + return VectorMax9( + detail::point_edge_closest_point_jacobian(p, e0, e1)); + } else { + assert(p.size() == 3); + return VectorMax9( + detail::point_edge_closest_point_jacobian(p, e0, 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); +template +inline auto point_edge_closest_point_hessian( + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& e0, + const Eigen::MatrixBase& e1) +{ + using T = typename DerivedP::Scalar; + + if constexpr (dim_v == 2) { + return detail::point_edge_closest_point_hessian(p, e0, e1); + } else if constexpr (dim_v == 3) { + return detail::point_edge_closest_point_hessian(p, e0, e1); + } else if (p.size() == 2) { + MatrixMax9 H(6, 6); + autogen::point_edge_closest_point_2D_hessian( + p[0], p[1], e0[0], e0[1], e1[0], e1[1], H.data()); + return H; + } else { + assert(p.size() == 3); + MatrixMax9 H(9, 9); + 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 /// @brief Compute the barycentric coordinates of the closest points between two edges. +/// +/// Accepts any 3D Eigen expression; the scalar type is deduced. +/// /// @param ea0 First point of the first edge /// @param ea1 Second point of the first edge /// @param eb0 First point of the second edge /// @param eb1 Second point of the second edge /// @return Barycentric coordinates of the closest points -Eigen::Vector2d edge_edge_closest_point( - Eigen::ConstRef ea0, - Eigen::ConstRef ea1, - Eigen::ConstRef eb0, - Eigen::ConstRef eb1); +template < + typename DerivedEA0, + typename DerivedEA1, + typename DerivedEB0, + typename DerivedEB1> +inline auto edge_edge_closest_point( + const Eigen::MatrixBase& ea0, + const Eigen::MatrixBase& ea1, + const Eigen::MatrixBase& eb0, + const Eigen::MatrixBase& eb1) +{ + using T = typename DerivedEA0::Scalar; + return detail::edge_edge_closest_point(ea0, ea1, eb0, eb1); +} /// @brief Compute the Jacobian of the closest points between two edges. +/// +/// Accepts any 3D Eigen expression; the scalar type is deduced. +/// /// @param ea0 First point of the first edge /// @param ea1 Second point of the first edge /// @param eb0 First point of the second edge /// @param eb1 Second point of the second edge /// @return Jacobian of the closest points -Eigen::Matrix edge_edge_closest_point_jacobian( - Eigen::ConstRef ea0, - Eigen::ConstRef ea1, - Eigen::ConstRef eb0, - Eigen::ConstRef eb1); +template < + typename DerivedEA0, + typename DerivedEA1, + typename DerivedEB0, + typename DerivedEB1> +inline auto edge_edge_closest_point_jacobian( + const Eigen::MatrixBase& ea0, + const Eigen::MatrixBase& ea1, + const Eigen::MatrixBase& eb0, + const Eigen::MatrixBase& eb1) +{ + using T = typename DerivedEA0::Scalar; + return detail::edge_edge_closest_point_jacobian(ea0, ea1, eb0, eb1); +} /// @brief Compute the Hessian of the closest points between two edges. +/// +/// Accepts any 3D Eigen expression; the scalar type is deduced. +/// /// @param ea0 First point of the first edge /// @param ea1 Second point of the first edge /// @param eb0 First point of the second edge /// @param eb1 Second point of the second edge /// @return Hessian of the closest points (2x12x12 tensor) -std::array edge_edge_closest_point_hessian( - Eigen::ConstRef ea0, - Eigen::ConstRef ea1, - Eigen::ConstRef eb0, - Eigen::ConstRef eb1); +template < + typename DerivedEA0, + typename DerivedEA1, + typename DerivedEB0, + typename DerivedEB1> +inline auto edge_edge_closest_point_hessian( + const Eigen::MatrixBase& ea0, + const Eigen::MatrixBase& ea1, + const Eigen::MatrixBase& eb0, + const Eigen::MatrixBase& eb1) +{ + using T = typename DerivedEA0::Scalar; + return detail::edge_edge_closest_point_hessian(ea0, ea1, eb0, eb1); +} // ============================================================================ // Point - Triangle /// @brief Compute the barycentric coordinates of the closest point on the triangle. +/// +/// Accepts any 3D Eigen expression; the scalar type is deduced. +/// /// @param p Point /// @param t0 Triangle's first vertex /// @param t1 Triangle's second vertex /// @param t2 Triangle's third vertex /// @return Barycentric coordinates of the closest point -Eigen::Vector2d point_triangle_closest_point( - Eigen::ConstRef p, - Eigen::ConstRef t0, - Eigen::ConstRef t1, - Eigen::ConstRef t2); +template < + typename DerivedP, + typename DerivedT0, + typename DerivedT1, + typename DerivedT2> +inline auto point_triangle_closest_point( + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& t0, + const Eigen::MatrixBase& t1, + const Eigen::MatrixBase& t2) +{ + using T = typename DerivedP::Scalar; + return detail::point_triangle_closest_point(p, t0, t1, t2); +} /// @brief Compute the Jacobian of the closest point on the triangle. +/// +/// Accepts any 3D Eigen expression; the scalar type is deduced. +/// /// @param p Point /// @param t0 Triangle's first vertex /// @param t1 Triangle's second vertex /// @param t2 Triangle's third vertex /// @return Jacobian of the closest point -Eigen::Matrix point_triangle_closest_point_jacobian( - Eigen::ConstRef p, - Eigen::ConstRef t0, - Eigen::ConstRef t1, - Eigen::ConstRef t2); +template < + typename DerivedP, + typename DerivedT0, + typename DerivedT1, + typename DerivedT2> +inline auto point_triangle_closest_point_jacobian( + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& t0, + const Eigen::MatrixBase& t1, + const Eigen::MatrixBase& t2) +{ + using T = typename DerivedP::Scalar; + return detail::point_triangle_closest_point_jacobian(p, t0, t1, t2); +} /// @brief Compute the Hessian of the closest point on the triangle. +/// +/// Accepts any 3D Eigen expression; the scalar type is deduced. +/// /// @param p Point /// @param t0 Triangle's first vertex /// @param t1 Triangle's second vertex /// @param t2 Triangle's third vertex /// @return Hessian of the closest point (2x12x12 tensor) -std::array point_triangle_closest_point_hessian( - Eigen::ConstRef p, - Eigen::ConstRef t0, - Eigen::ConstRef t1, - Eigen::ConstRef t2); - -// ============================================================================ - -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, - 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 J[24]); - - // 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]); - - // J is (2×12) flattened in column-major order - void point_triangle_closest_point_jacobian( - 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 J[24]); - - // hess is (144×1) flattened in column-major order - void point_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 point_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]); -} // namespace autogen +template < + typename DerivedP, + typename DerivedT0, + typename DerivedT1, + typename DerivedT2> +inline auto point_triangle_closest_point_hessian( + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& t0, + const Eigen::MatrixBase& t1, + const Eigen::MatrixBase& t2) +{ + using T = typename DerivedP::Scalar; + return detail::point_triangle_closest_point_hessian(p, t0, t1, t2); +} } // namespace ipc diff --git a/src/ipc/tangent/relative_velocity.cpp b/src/ipc/tangent/relative_velocity.cpp deleted file mode 100644 index f020a50e4..000000000 --- a/src/ipc/tangent/relative_velocity.cpp +++ /dev/null @@ -1,168 +0,0 @@ -#include "relative_velocity.hpp" - -namespace ipc { - -// ============================================================================ -// Point - Point - -VectorMax3d point_point_relative_velocity( - Eigen::ConstRef dp0, Eigen::ConstRef dp1) -{ - return dp0 - dp1; -} - -MatrixMax point_point_relative_velocity_jacobian(const int dim) -{ - MatrixMax J(dim, 2 * dim); - J.leftCols(dim) = MatrixMax3d::Identity(dim, dim); - J.rightCols(dim) = -MatrixMax3d::Identity(dim, dim); - return J; -} - -VectorMax point_point_relative_velocity_dx_dbeta(const int dim) -{ - // Γ is constant (does not depend on β), so the derivative is zero. - return VectorMax::Zero(2 * dim * dim); -} - -// ============================================================================ -// Point - Edge - -VectorMax3d point_edge_relative_velocity( - Eigen::ConstRef dp, - Eigen::ConstRef de0, - Eigen::ConstRef de1, - const double alpha) -{ - return dp - ((de1 - de0) * alpha + de0); -} - -MatrixMax -point_edge_relative_velocity_jacobian(const int dim, const double alpha) -{ - MatrixMax J = MatrixMax::Zero(dim, 3 * dim); - J.leftCols(dim).diagonal().setOnes(); - J.middleCols(dim, dim).diagonal().setConstant(alpha - 1); - J.rightCols(dim).diagonal().setConstant(-alpha); - return J; -} - -// Γ(α) = [I, (α-1)I, -αI] (dim × 3·dim) -// ∂Γ/∂α = [0, I, -I] -// -// Stored as vec(∂Γ/∂α) in column-major order (3rd-order convention). -// Result is a column vector of size dim × 3·dim = 3·dim². -// For a (dim × ndof) matrix M, element M(r,c) maps to vec index c·dim + r. -VectorMax -point_edge_relative_velocity_dx_dbeta(const int dim, const double alpha) -{ - const int ndof = 3 * dim; - VectorMax J = VectorMax::Zero(dim * ndof); - for (int i = 0; i < dim; ++i) { - // I block at cols [dim, 2·dim) - J[(dim + i) * dim + i] = 1; - // -I block at cols [2·dim, 3·dim) - J[(2 * dim + i) * dim + i] = -1; - } - return J; -} - -// ============================================================================ -// Edge - Edge - -Eigen::Vector3d edge_edge_relative_velocity( - Eigen::ConstRef dea0, - Eigen::ConstRef dea1, - Eigen::ConstRef deb0, - Eigen::ConstRef deb1, - Eigen::ConstRef coords) -{ - // closest_point_a_velocity - closest_point_b_velocity - return ((dea1 - dea0) * coords[0] + dea0) - - ((deb1 - deb0) * coords[1] + deb0); -} - -Eigen::Matrix -edge_edge_relative_velocity_jacobian(Eigen::ConstRef coords) -{ - Eigen::Matrix J = Eigen::Matrix::Zero(); - J.leftCols<3>().diagonal().setConstant(1 - coords[0]); - J.middleCols<3>(3).diagonal().setConstant(coords[0]); - J.middleCols<3>(6).diagonal().setConstant(coords[1] - 1); - J.rightCols<3>().diagonal().setConstant(-coords[1]); - return J; -} - -// Γ(β₁,β₂) = [(1-β₁)I, β₁I, (β₂-1)I, -β₂I] (3 × 12) -// ∂Γ/∂β₁ = [-I, I, 0, 0] -// ∂Γ/∂β₂ = [ 0, 0, I,-I] -// -// Stored as [vec(∂Γ/∂β₁) | vec(∂Γ/∂β₂)] in column-major order (3rd-order -// convention). Result shape: (36, 2). For a (3 × 12) matrix M, element M(r,c) -// maps to vec index c·3 + r. -Eigen::Matrix -edge_edge_relative_velocity_dx_dbeta(Eigen::ConstRef coords) -{ - constexpr int dim = 3; - Eigen::Matrix J = Eigen::Matrix::Zero(); - for (int i = 0; i < dim; ++i) { - // wrt β₁: -I at cols [0,3), I at cols [3,6) - J((0 + i) * dim + i, 0) = -1; - J((3 + i) * dim + i, 0) = 1; - // wrt β₂: I at cols [6,9), -I at cols [9,12) - J((6 + i) * dim + i, 1) = 1; - J((9 + i) * dim + i, 1) = -1; - } - return J; -} - -// ============================================================================ -// Point - Triangle - -Eigen::Vector3d point_triangle_relative_velocity( - Eigen::ConstRef dp, - Eigen::ConstRef dt0, - Eigen::ConstRef dt1, - Eigen::ConstRef dt2, - Eigen::ConstRef coords) -{ - // Compute the velocity of the closest point and subtract it from the - // points velocity. - return dp - (dt0 + coords[0] * (dt1 - dt0) + coords[1] * (dt2 - dt0)); -} - -Eigen::Matrix point_triangle_relative_velocity_jacobian( - Eigen::ConstRef coords) -{ - Eigen::Matrix J = Eigen::Matrix::Zero(); - J.leftCols<3>().diagonal().setOnes(); - J.middleCols<3>(3).diagonal().setConstant(coords[0] + coords[1] - 1); - J.middleCols<3>(6).diagonal().setConstant(-coords[0]); - J.rightCols<3>().diagonal().setConstant(-coords[1]); - return J; -} - -// Γ(β₁,β₂) = [I, (β₁+β₂-1)I, -β₁I, -β₂I] (3 × 12) -// ∂Γ/∂β₁ = [0, I, -I, 0] -// ∂Γ/∂β₂ = [0, I, 0,-I] -// -// Stored as [vec(∂Γ/∂β₁) | vec(∂Γ/∂β₂)] in column-major order (3rd-order -// convention). Result shape: (36, 2). For a (3 × 12) matrix M, element M(r,c) -// maps to vec index c·3 + r. -Eigen::Matrix point_triangle_relative_velocity_dx_dbeta( - Eigen::ConstRef coords) -{ - constexpr int dim = 3; - Eigen::Matrix J = Eigen::Matrix::Zero(); - for (int i = 0; i < dim; ++i) { - // wrt β₁: I at cols [3,6), -I at cols [6,9) - J((3 + i) * dim + i, 0) = 1; - J((6 + i) * dim + i, 0) = -1; - // wrt β₂: I at cols [3,6), -I at cols [9,12) - J((3 + i) * dim + i, 1) = 1; - J((9 + i) * dim + i, 1) = -1; - } - return J; -} - -} // namespace ipc \ No newline at end of file diff --git a/src/ipc/tangent/relative_velocity.hpp b/src/ipc/tangent/relative_velocity.hpp index 8795a2606..b540ea852 100644 --- a/src/ipc/tangent/relative_velocity.hpp +++ b/src/ipc/tangent/relative_velocity.hpp @@ -2,8 +2,252 @@ #include +#include + namespace ipc { +namespace detail { + // ======================================================================== + // Point - Point + + /// @brief Compute the relative velocity of two points + /// @tparam T The scalar type. + /// @tparam dim The dimension (2 or 3). + /// @param dp0 Velocity of the first point + /// @param dp1 Velocity of the second point + /// @return The relative velocity of the two points + template + inline Eigen::Vector point_point_relative_velocity( + Eigen::ConstRef> dp0, + Eigen::ConstRef> dp1) + { + static_assert( + dim == 2 || dim == 3, "point-point velocity is only 2D or 3D"); + return dp0 - dp1; + } + + /// @brief Compute du/dx where u is the relative velocity of two points + /// @tparam T The scalar type. + /// @tparam dim The dimension (2 or 3). + /// @return The relative velocity Jacobian du/dx + template + inline Eigen::Matrix + point_point_relative_velocity_jacobian() + { + static_assert( + dim == 2 || dim == 3, "point-point velocity is only 2D or 3D"); + Eigen::Matrix J; + J.template leftCols() = Eigen::Matrix::Identity(); + J.template rightCols() = -Eigen::Matrix::Identity(); + return J; + } + + /// @brief Compute d²u/dxdβ where u is the relative velocity of two points + /// @tparam T The scalar type. + /// @tparam dim The dimension (2 or 3). + /// @return The vectorized tensor of d²u/dxdβ + template + inline Eigen::Vector + point_point_relative_velocity_dx_dbeta() + { + static_assert( + dim == 2 || dim == 3, "point-point velocity is only 2D or 3D"); + // Γ is constant (does not depend on β), so the derivative is zero. + return Eigen::Vector::Zero(); + } + + // ======================================================================== + // Point - Edge + + /// @brief Compute the relative velocity of a point and an edge + /// @tparam T The scalar type. + /// @tparam dim The dimension (2 or 3). + /// @param dp Velocity of the point + /// @param de0 Velocity of the first endpoint of the edge + /// @param de1 Velocity of the second endpoint of the edge + /// @param alpha Parametric coordinate of the closest point on the edge + /// @return The relative velocity of the point and the edge + template + inline Eigen::Vector point_edge_relative_velocity( + Eigen::ConstRef> dp, + Eigen::ConstRef> de0, + Eigen::ConstRef> de1, + const T alpha) + { + static_assert( + dim == 2 || dim == 3, "point-edge velocity is only 2D or 3D"); + return dp - ((de1 - de0) * alpha + de0); + } + + /// @brief Compute du/dx where u is the relative velocity of a point and an + /// edge + /// @tparam T The scalar type. + /// @tparam dim The dimension (2 or 3). + /// @param alpha Parametric coordinate of the closest point on the edge + /// @return The relative velocity Jacobian du/dx + template + inline Eigen::Matrix + point_edge_relative_velocity_jacobian(const T alpha) + { + static_assert( + dim == 2 || dim == 3, "point-edge velocity is only 2D or 3D"); + Eigen::Matrix J = + Eigen::Matrix::Zero(); + J.template leftCols().diagonal().setOnes(); + J.template middleCols(dim).diagonal().setConstant(alpha - T(1)); + J.template rightCols().diagonal().setConstant(-alpha); + return J; + } + + /// @brief Compute d²u/dxdα where u is the relative velocity of a point and + /// an edge + /// @tparam T The scalar type. + /// @tparam dim The dimension (2 or 3). + /// @param alpha Parametric coordinate of the closest point on the edge + /// @return The vectorized tensor of d²u/dxdα + template + inline Eigen::Vector + point_edge_relative_velocity_dx_dbeta(const T alpha) + { + static_assert( + dim == 2 || dim == 3, "point-edge velocity is only 2D or 3D"); + Eigen::Vector J = + Eigen::Vector::Zero(); + for (int i = 0; i < dim; ++i) { + // I block at cols [dim, 2·dim) + J[(dim + i) * dim + i] = T(1); + // -I block at cols [2·dim, 3·dim) + J[(2 * dim + i) * dim + i] = T(-1); + } + return J; + } + + // ======================================================================== + // Edge - Edge + + /// @brief Compute the relative velocity of the edges. + /// @tparam T The scalar type. + /// @param dea0 Velocity of the first endpoint of the first edge + /// @param dea1 Velocity of the second endpoint of the first edge + /// @param deb0 Velocity of the first endpoint of the second edge + /// @param deb1 Velocity of the second endpoint of the second edge + /// @param coords Two parametric coordinates of the closest points + /// @return The relative velocity of the edges + template + inline Eigen::Vector3 edge_edge_relative_velocity( + Eigen::ConstRef> dea0, + Eigen::ConstRef> dea1, + Eigen::ConstRef> deb0, + Eigen::ConstRef> deb1, + Eigen::ConstRef> coords) + { + // closest_point_a_velocity - closest_point_b_velocity + return ((dea1 - dea0) * coords[0] + dea0) + - ((deb1 - deb0) * coords[1] + deb0); + } + + /// @brief Compute du/dx where u is the relative velocity of two edges + /// @tparam T The scalar type. + /// @param coords Two parametric coordinates of the closest points + /// @return The relative velocity Jacobian du/dx + template + inline Eigen::Matrix edge_edge_relative_velocity_jacobian( + Eigen::ConstRef> coords) + { + Eigen::Matrix J = Eigen::Matrix::Zero(); + J.template leftCols<3>().diagonal().setConstant(T(1) - coords[0]); + J.template middleCols<3>(3).diagonal().setConstant(coords[0]); + J.template middleCols<3>(6).diagonal().setConstant(coords[1] - T(1)); + J.template rightCols<3>().diagonal().setConstant(-coords[1]); + return J; + } + + /// @brief Compute d²u/dxdβ where u is the relative velocity of two edges + /// @tparam T The scalar type. + /// @param coords Two parametric coordinates of the closest points + /// @return The vectorized tensor of d²u/dxdβ + template + inline Eigen::Matrix edge_edge_relative_velocity_dx_dbeta( + Eigen::ConstRef> coords) + { + constexpr int dim = 3; + Eigen::Matrix J = Eigen::Matrix::Zero(); + for (int i = 0; i < dim; ++i) { + // wrt β₁: -I at cols [0,3), I at cols [3,6) + J((0 + i) * dim + i, 0) = T(-1); + J((3 + i) * dim + i, 0) = T(1); + // wrt β₂: I at cols [6,9), -I at cols [9,12) + J((6 + i) * dim + i, 1) = T(1); + J((9 + i) * dim + i, 1) = T(-1); + } + return J; + } + + // ======================================================================== + // Point - Triangle + + /// @brief Compute the relative velocity of the point to the triangle. + /// @tparam T The scalar type. + /// @param dp Velocity of the point + /// @param dt0 Velocity of the first vertex of the triangle + /// @param dt1 Velocity of the second vertex of the triangle + /// @param dt2 Velocity of the third vertex of the triangle + /// @param coords Baricentric coordinates of the closest point + /// @return The relative velocity of the point to the triangle + template + inline Eigen::Vector3 point_triangle_relative_velocity( + Eigen::ConstRef> dp, + Eigen::ConstRef> dt0, + Eigen::ConstRef> dt1, + Eigen::ConstRef> dt2, + Eigen::ConstRef> coords) + { + // Compute the velocity of the closest point and subtract it from the + // points velocity. + return dp - (dt0 + coords[0] * (dt1 - dt0) + coords[1] * (dt2 - dt0)); + } + + /// @brief Compute du/dx where u is the relative velocity of a point and a + /// triangle + /// @tparam T The scalar type. + /// @param coords Barycentric coordinates of the closest point + /// @return The relative velocity Jacobian du/dx + template + inline Eigen::Matrix point_triangle_relative_velocity_jacobian( + Eigen::ConstRef> coords) + { + Eigen::Matrix J = Eigen::Matrix::Zero(); + J.template leftCols<3>().diagonal().setOnes(); + J.template middleCols<3>(3).diagonal().setConstant( + coords[0] + coords[1] - T(1)); + J.template middleCols<3>(6).diagonal().setConstant(-coords[0]); + J.template rightCols<3>().diagonal().setConstant(-coords[1]); + return J; + } + + /// @brief Compute d²u/dxdβ where u is the relative velocity of a point and + /// a triangle + /// @tparam T The scalar type. + /// @param coords Baricentric coordinates of the closest point + /// @return The vectorized tensor of d²u/dxdβ + template + inline Eigen::Matrix point_triangle_relative_velocity_dx_dbeta( + Eigen::ConstRef> coords) + { + constexpr int dim = 3; + Eigen::Matrix J = Eigen::Matrix::Zero(); + for (int i = 0; i < dim; ++i) { + // wrt β₁: I at cols [3,6), -I at cols [6,9) + J((3 + i) * dim + i, 0) = T(1); + J((6 + i) * dim + i, 0) = T(-1); + // wrt β₂: I at cols [3,6), -I at cols [9,12) + J((3 + i) * dim + i, 1) = T(1); + J((9 + i) * dim + i, 1) = T(-1); + } + return J; + } +} // namespace detail + // ============================================================================ // Point - Point @@ -11,18 +255,61 @@ namespace ipc { /// @param dp0 Velocity of the first point /// @param dp1 Velocity of the second point /// @return The relative velocity of the two points -VectorMax3d point_point_relative_velocity( - Eigen::ConstRef dp0, Eigen::ConstRef dp1); +template +inline auto point_point_relative_velocity( + const Eigen::MatrixBase& dp0, + const Eigen::MatrixBase& dp1) +{ + using T = typename DerivedDP0::Scalar; + assert(dp0.size() == dp1.size()); + + if constexpr (dim_v == 2) { + return detail::point_point_relative_velocity(dp0, dp1); + } else if constexpr (dim_v == 3) { + return detail::point_point_relative_velocity(dp0, dp1); + } else if (dp0.size() == 2) { + return VectorMax3( + detail::point_point_relative_velocity(dp0, dp1)); + } else { + assert(dp0.size() == 3); + return VectorMax3( + detail::point_point_relative_velocity(dp0, dp1)); + } +} /// @brief Compute du/dx where u is the relative velocity of two points +/// @tparam T The scalar type (defaults to double). /// @param dim Dimension (2 or 3) /// @return The relative velocity Jacobian du/dx -MatrixMax point_point_relative_velocity_jacobian(const int dim); +template +inline MatrixMax point_point_relative_velocity_jacobian(const int dim) +{ + if (dim == 2) { + return MatrixMax( + detail::point_point_relative_velocity_jacobian()); + } else { + assert(dim == 3); + return MatrixMax( + detail::point_point_relative_velocity_jacobian()); + } +} /// @brief Compute d²u/dxdβ where u is the relative velocity of two points +/// @tparam T The scalar type (defaults to double). /// @param dim Dimension (2 or 3) /// @return The vectorized tensor of d²u/dxdβ -VectorMax point_point_relative_velocity_dx_dbeta(const int dim); +template +inline VectorMax point_point_relative_velocity_dx_dbeta(const int dim) +{ + if (dim == 2) { + return VectorMax( + detail::point_point_relative_velocity_dx_dbeta()); + } else { + assert(dim == 3); + return VectorMax( + detail::point_point_relative_velocity_dx_dbeta()); + } +} // ============================================================================ // Point - Edge @@ -33,25 +320,73 @@ VectorMax point_point_relative_velocity_dx_dbeta(const int dim); /// @param de1 Velocity of the second endpoint of the edge /// @param alpha Parametric coordinate of the closest point on the edge /// @return The relative velocity of the point and the edge -VectorMax3d point_edge_relative_velocity( - Eigen::ConstRef dp, - Eigen::ConstRef de0, - Eigen::ConstRef de1, - const double alpha); +template +inline auto point_edge_relative_velocity( + const Eigen::MatrixBase& dp, + const Eigen::MatrixBase& de0, + const Eigen::MatrixBase& de1, + const typename DerivedDP::Scalar alpha) +{ + using T = typename DerivedDP::Scalar; + assert(dp.size() == de0.size() && dp.size() == de1.size()); + + if constexpr (dim_v == 2) { + return detail::point_edge_relative_velocity(dp, de0, de1, alpha); + } else if constexpr (dim_v == 3) { + return detail::point_edge_relative_velocity(dp, de0, de1, alpha); + } else if (dp.size() == 2) { + return VectorMax3( + detail::point_edge_relative_velocity(dp, de0, de1, alpha)); + } else { + assert(dp.size() == 3); + return VectorMax3( + detail::point_edge_relative_velocity(dp, de0, de1, alpha)); + } +} /// @brief Compute du/dx where u is the relative velocity of a point and an edge +/// @tparam T The scalar type (deduced from alpha). /// @param dim Dimension (2 or 3) /// @param alpha Parametric coordinate of the closest point on the edge /// @return The relative velocity Jacobian du/dx -MatrixMax -point_edge_relative_velocity_jacobian(const int dim, const double alpha); +template +inline MatrixMax +point_edge_relative_velocity_jacobian(const int dim, const T alpha) +{ + if (dim == 2) { + return MatrixMax( + detail::point_edge_relative_velocity_jacobian(alpha)); + } else { + assert(dim == 3); + return MatrixMax( + detail::point_edge_relative_velocity_jacobian(alpha)); + } +} /// @brief Compute d²u/dxdα where u is the relative velocity of a point and an edge +/// @tparam T The scalar type (deduced from alpha). +/// +/// Γ(α) = [I, (α-1)I, -αI] (dim × 3·dim) +/// ∂Γ/∂α = [0, I, -I] +/// +/// Stored as vec(∂Γ/∂α) in column-major order (3rd-order convention). +/// Result is a column vector of size dim × 3·dim = 3·dim². /// @param dim Dimension (2 or 3) /// @param alpha Parametric coordinate of the closest point on the edge /// @return The vectorized tensor of d²u/dxdα -VectorMax -point_edge_relative_velocity_dx_dbeta(const int dim, const double alpha); +template +inline VectorMax +point_edge_relative_velocity_dx_dbeta(const int dim, const T alpha) +{ + if (dim == 2) { + return VectorMax( + detail::point_edge_relative_velocity_dx_dbeta(alpha)); + } else { + assert(dim == 3); + return VectorMax( + detail::point_edge_relative_velocity_dx_dbeta(alpha)); + } +} // ============================================================================ // Edge - Edge @@ -63,24 +398,55 @@ point_edge_relative_velocity_dx_dbeta(const int dim, const double alpha); /// @param deb1 Velocity of the second endpoint of the second edge /// @param coords Two parametric coordinates of the closest points on the edges /// @return The relative velocity of the edges -Eigen::Vector3d edge_edge_relative_velocity( - Eigen::ConstRef dea0, - Eigen::ConstRef dea1, - Eigen::ConstRef deb0, - Eigen::ConstRef deb1, - Eigen::ConstRef coords); +template < + typename DerivedDEA0, + typename DerivedDEA1, + typename DerivedDEB0, + typename DerivedDEB1, + typename DerivedCoords> +inline auto edge_edge_relative_velocity( + const Eigen::MatrixBase& dea0, + const Eigen::MatrixBase& dea1, + const Eigen::MatrixBase& deb0, + const Eigen::MatrixBase& deb1, + const Eigen::MatrixBase& coords) +{ + using T = typename DerivedDEA0::Scalar; + // NOTE: explicit : the detail overload cannot deduce T from an + // arbitrary Eigen expression. + return detail::edge_edge_relative_velocity( + dea0, dea1, deb0, deb1, coords); +} /// @brief Compute du/dx where u is the relative velocity of two edges /// @param coords Two parametric coordinates of the closest points on the edges /// @return The relative velocity Jacobian du/dx -Eigen::Matrix -edge_edge_relative_velocity_jacobian(Eigen::ConstRef coords); +template +inline auto edge_edge_relative_velocity_jacobian( + const Eigen::MatrixBase& coords) +{ + using T = typename DerivedCoords::Scalar; + return detail::edge_edge_relative_velocity_jacobian(coords); +} /// @brief Compute d²u/dxdβ where u is the relative velocity of two edges +/// +/// Γ(β₁,β₂) = [(1-β₁)I, β₁I, (β₂-1)I, -β₂I] (3 × 12) +/// ∂Γ/∂β₁ = [-I, I, 0, 0] +/// ∂Γ/∂β₂ = [ 0, 0, I,-I] +/// +/// Stored as [vec(∂Γ/∂β₁) | vec(∂Γ/∂β₂)] in column-major order (3rd-order +/// convention). Result shape: (36, 2). For a (3 × 12) matrix M, element +/// M(r,c) maps to vec index c·3 + r. /// @param coords Two parametric coordinates of the closest points on the edges /// @return The vectorized tensor of d²u/dxdβ -Eigen::Matrix -edge_edge_relative_velocity_dx_dbeta(Eigen::ConstRef coords); +template +inline auto edge_edge_relative_velocity_dx_dbeta( + const Eigen::MatrixBase& coords) +{ + using T = typename DerivedCoords::Scalar; + return detail::edge_edge_relative_velocity_dx_dbeta(coords); +} // ============================================================================ // Point - Triangle @@ -92,23 +458,52 @@ edge_edge_relative_velocity_dx_dbeta(Eigen::ConstRef coords); /// @param dt2 Velocity of the third vertex of the triangle /// @param coords Baricentric coordinates of the closest point on the triangle /// @return The relative velocity of the point to the triangle -Eigen::Vector3d point_triangle_relative_velocity( - Eigen::ConstRef dp, - Eigen::ConstRef dt0, - Eigen::ConstRef dt1, - Eigen::ConstRef dt2, - Eigen::ConstRef coords); +template < + typename DerivedDP, + typename DerivedDT0, + typename DerivedDT1, + typename DerivedDT2, + typename DerivedCoords> +inline auto point_triangle_relative_velocity( + const Eigen::MatrixBase& dp, + const Eigen::MatrixBase& dt0, + const Eigen::MatrixBase& dt1, + const Eigen::MatrixBase& dt2, + const Eigen::MatrixBase& coords) +{ + using T = typename DerivedDP::Scalar; + return detail::point_triangle_relative_velocity( + dp, dt0, dt1, dt2, coords); +} /// @brief Compute du/dx where u is the relative velocity of a point and a triangle /// @param coords Barycentric coordinates of the closest point on the triangle /// @return The relative velocity Jacobian du/dx -Eigen::Matrix point_triangle_relative_velocity_jacobian( - Eigen::ConstRef coords); +template +inline auto point_triangle_relative_velocity_jacobian( + const Eigen::MatrixBase& coords) +{ + using T = typename DerivedCoords::Scalar; + return detail::point_triangle_relative_velocity_jacobian(coords); +} /// @brief Compute d²u/dxdβ where u is the relative velocity of a point and a triangle +/// +/// Γ(β₁,β₂) = [I, (β₁+β₂-1)I, -β₁I, -β₂I] (3 × 12) +/// ∂Γ/∂β₁ = [0, I, -I, 0] +/// ∂Γ/∂β₂ = [0, I, 0,-I] +/// +/// Stored as [vec(∂Γ/∂β₁) | vec(∂Γ/∂β₂)] in column-major order (3rd-order +/// convention). Result shape: (36, 2). For a (3 × 12) matrix M, element +/// M(r,c) maps to vec index c·3 + r. /// @param coords Baricentric coordinates of the closest point on the triangle /// @return The vectorized tensor of d²u/dxdβ -Eigen::Matrix point_triangle_relative_velocity_dx_dbeta( - Eigen::ConstRef coords); +template +inline auto point_triangle_relative_velocity_dx_dbeta( + const Eigen::MatrixBase& coords) +{ + using T = typename DerivedCoords::Scalar; + return detail::point_triangle_relative_velocity_dx_dbeta(coords); +} } // namespace ipc diff --git a/src/ipc/tangent/tangent_basis.cpp b/src/ipc/tangent/tangent_basis.cpp index bfbbcda0b..762117cd5 100644 --- a/src/ipc/tangent/tangent_basis.cpp +++ b/src/ipc/tangent/tangent_basis.cpp @@ -2,983 +2,951 @@ #include -namespace ipc { +#include -namespace { - /// @brief Compute the power of 1.5 of a number. - /// @param x Number to compute the power of 1.5 - /// @return x^(1.5) - inline double pow_1_5(double x) { return x * std::sqrt(x); } -} // namespace +namespace ipc::detail { -// ============================================================================ +// ======================================================================== // Point - Point -MatrixMax point_point_tangent_basis( - Eigen::ConstRef p0, Eigen::ConstRef p1) +template +Eigen::Matrix point_point_tangent_basis_jacobian( + Eigen::ConstRef> p0, + Eigen::ConstRef> p1) { - const int dim = p0.size(); - assert(dim == p1.size()); - - if (dim == 2) { - const Eigen::Vector2d p0_to_p1 = (p1 - p0).normalized(); - return Eigen::Vector2d(-p0_to_p1.y(), p0_to_p1.x()); - } else { - assert(dim == 3); + static_assert( + dim == 2 || dim == 3, "point-point tangent basis is only 2D or 3D"); - const Eigen::Vector3d p0_to_p1 = p1 - p0; - - const Eigen::Vector3d cross_x = - Eigen::Vector3d::UnitX().cross(p0_to_p1); - const Eigen::Vector3d cross_y = - Eigen::Vector3d::UnitY().cross(p0_to_p1); - - Eigen::Matrix basis; - if (cross_x.squaredNorm() > cross_y.squaredNorm()) { - basis.col(0) = cross_x.normalized(); - basis.col(1) = p0_to_p1.cross(cross_x).normalized(); - } else { - basis.col(0) = cross_y.normalized(); - basis.col(1) = p0_to_p1.cross(cross_y).normalized(); - } - - return basis; - } -} - -MatrixMax point_point_tangent_basis_jacobian( - Eigen::ConstRef p0, Eigen::ConstRef p1) -{ - const int dim = p0.size(); - assert(dim == p1.size()); - - MatrixMax J; - if (dim == 2) { - J.resize(2, 4); - autogen::point_point_tangent_basis_2D_jacobian( + Eigen::Matrix J; + if constexpr (dim == 2) { + autogen::point_point_tangent_basis_2D_jacobian( p0[0], p0[1], p1[0], p1[1], J.data()); } else { - assert(dim == 3); - J.resize(6, 6); - autogen::point_point_tangent_basis_3D_jacobian( + autogen::point_point_tangent_basis_3D_jacobian( p0[0], p0[1], p0[2], p1[0], p1[1], p1[2], J.data()); } return J; } -// ============================================================================ +// ======================================================================== // Point - Edge -MatrixMax point_edge_tangent_basis( - Eigen::ConstRef p, - Eigen::ConstRef e0, - Eigen::ConstRef e1) +template +Eigen::Matrix point_edge_tangent_basis_jacobian( + Eigen::ConstRef> p, + Eigen::ConstRef> e0, + Eigen::ConstRef> e1) { - const int dim = p.size(); - assert(dim == e0.size() && dim == e1.size()); - - if (dim == 2) { - return (e1 - e0).normalized(); - } else { - assert(dim == 3); - - const Eigen::Vector3d e = e1 - e0; - - Eigen::Matrix basis; - basis.col(0) = e.normalized(); - basis.col(1) = e.cross(Eigen::Vector3d(p - e0)).normalized(); - return basis; - } -} + static_assert( + dim == 2 || dim == 3, "point-edge tangent basis is only 2D or 3D"); -MatrixMax point_edge_tangent_basis_jacobian( - Eigen::ConstRef p, - Eigen::ConstRef e0, - Eigen::ConstRef e1) -{ - const int dim = p.size(); - assert(dim == e0.size() && dim == e1.size()); - - MatrixMax J; - if (dim == 2) { - J.resize(2, 6); - autogen::point_edge_tangent_basis_2D_jacobian( + Eigen::Matrix J; + if constexpr (dim == 2) { + autogen::point_edge_tangent_basis_2D_jacobian( p[0], p[1], e0[0], e0[1], e1[0], e1[1], J.data()); } else { - assert(dim == 3); - J.resize(6, 9); - autogen::point_edge_tangent_basis_3D_jacobian( + autogen::point_edge_tangent_basis_3D_jacobian( p[0], p[1], p[2], e0[0], e0[1], e0[2], e1[0], e1[1], e1[2], J.data()); } return J; } -// ============================================================================ +// ======================================================================== // Edge - Edge -Eigen::Matrix edge_edge_tangent_basis( - Eigen::ConstRef ea0, - Eigen::ConstRef ea1, - Eigen::ConstRef eb0, - Eigen::ConstRef eb1) -{ - const Eigen::Vector3d ea = ea1 - ea0; // Edge A direction - const Eigen::Vector3d normal = ea.cross(eb1 - eb0); - // The normal will be zero if the edges are parallel (i.e. coplanar). - assert(normal.norm() != 0); - - Eigen::Matrix basis; - // The first basis vector is along edge A. - basis.col(0) = ea.normalized(); - // The second basis vector is orthogonal to the first and the edge-edge - // normal. - basis.col(1) = normal.cross(ea).normalized(); - return basis; -} - -Eigen::Matrix edge_edge_tangent_basis_jacobian( - Eigen::ConstRef ea0, - Eigen::ConstRef ea1, - Eigen::ConstRef eb0, - Eigen::ConstRef eb1) +template +Eigen::Matrix edge_edge_tangent_basis_jacobian( + Eigen::ConstRef> ea0, + Eigen::ConstRef> ea1, + Eigen::ConstRef> eb0, + Eigen::ConstRef> eb1) { - Eigen::Matrix J; - autogen::edge_edge_tangent_basis_jacobian( + Eigen::Matrix J; + autogen::edge_edge_tangent_basis_jacobian( ea0[0], ea0[1], ea0[2], ea1[0], ea1[1], ea1[2], eb0[0], eb0[1], eb0[2], eb1[0], eb1[1], eb1[2], J.data()); return J; } -// ============================================================================ +// ======================================================================== // Point - Triangle -Eigen::Matrix point_triangle_tangent_basis( - Eigen::ConstRef p, - Eigen::ConstRef t0, - Eigen::ConstRef t1, - Eigen::ConstRef t2) +template +Eigen::Matrix point_triangle_tangent_basis_jacobian( + Eigen::ConstRef> p, + Eigen::ConstRef> t0, + Eigen::ConstRef> t1, + Eigen::ConstRef> t2) { - const Eigen::Vector3d e0 = t1 - t0; - const Eigen::Vector3d normal = e0.cross(t2 - t0); - assert(normal.norm() != 0); + Eigen::Matrix J; + autogen::point_triangle_tangent_basis_jacobian( + p[0], p[1], p[2], t0[0], t0[1], t0[2], t1[0], t1[1], t1[2], t2[0], + t2[1], t2[2], J.data()); + return J; +} - Eigen::Matrix basis; +// ======================================================================== +// Explicit instantiations + +#define IPC_INSTANTIATE_TANGENT_BASIS_ND(T, dim) \ + template Eigen::Matrix point_point_tangent_basis( \ + Eigen::ConstRef>, \ + Eigen::ConstRef>); \ + template Eigen::Matrix \ + point_point_tangent_basis_jacobian( \ + Eigen::ConstRef>, \ + Eigen::ConstRef>); \ + template Eigen::Matrix point_edge_tangent_basis( \ + Eigen::ConstRef>, \ + Eigen::ConstRef>, \ + Eigen::ConstRef>); \ + template Eigen::Matrix \ + point_edge_tangent_basis_jacobian( \ + Eigen::ConstRef>, \ + Eigen::ConstRef>, \ + Eigen::ConstRef>) + +IPC_INSTANTIATE_TANGENT_BASIS_ND(float, 2); +IPC_INSTANTIATE_TANGENT_BASIS_ND(float, 3); +IPC_INSTANTIATE_TANGENT_BASIS_ND(double, 2); +IPC_INSTANTIATE_TANGENT_BASIS_ND(double, 3); + +#undef IPC_INSTANTIATE_TANGENT_BASIS_ND + +#define IPC_INSTANTIATE_TANGENT_BASIS_3D(T) \ + template Eigen::Matrix edge_edge_tangent_basis( \ + Eigen::ConstRef>, \ + Eigen::ConstRef>, \ + Eigen::ConstRef>, \ + Eigen::ConstRef>); \ + template Eigen::Matrix edge_edge_tangent_basis_jacobian( \ + Eigen::ConstRef>, \ + Eigen::ConstRef>, \ + Eigen::ConstRef>, \ + Eigen::ConstRef>); \ + template Eigen::Matrix point_triangle_tangent_basis( \ + Eigen::ConstRef>, \ + Eigen::ConstRef>, \ + Eigen::ConstRef>, \ + Eigen::ConstRef>); \ + template Eigen::Matrix point_triangle_tangent_basis_jacobian( \ + Eigen::ConstRef>, \ + Eigen::ConstRef>, \ + Eigen::ConstRef>, \ + Eigen::ConstRef>) + +IPC_INSTANTIATE_TANGENT_BASIS_3D(float); +IPC_INSTANTIATE_TANGENT_BASIS_3D(double); + +#undef IPC_INSTANTIATE_TANGENT_BASIS_3D + +} // namespace ipc::detail + +namespace ipc::autogen { - // The first basis vector is along first edge of the triangle. - basis.col(0) = e0.normalized(); - // The second basis vector is orthogonal to the first and the triangle - // normal. - basis.col(1) = normal.cross(e0).normalized(); +namespace { + /// @brief Compute the power of 1.5 of a number. + /// @param x Number to compute the power of 1.5 + /// @return x^(1.5) + template inline T pow_1_5(T x) { return x * std::sqrt(x); } +} // namespace - return basis; +// J is (2×4) flattened in column-major order +template +void point_point_tangent_basis_2D_jacobian( + T p0_x, T p0_y, T p1_x, T p1_y, T J[8]) +{ + const T t0 = p0_x - p1_x; + const T t1 = p0_y - p1_y; + const T t2 = t0 * t0; + const T t3 = t1 * t1; + const T t4 = t2 + t3; + const T t5 = t0 * t1 / pow_1_5(t4); + const T t6 = -t5; + const T t7 = T(1) / std::sqrt(t4); + const T t8 = T(1) / t4; + const T t9 = t2 * t8 - 1; + const T t10 = t3 * t8 - 1; + J[0] = t6; + J[1] = t7 * t9; + J[2] = -t10 * t7; + J[3] = t5; + J[4] = t5; + J[5] = -t7 * t9; + J[6] = t10 * t7; + J[7] = t6; } -Eigen::Matrix point_triangle_tangent_basis_jacobian( - Eigen::ConstRef p, - Eigen::ConstRef t0, - Eigen::ConstRef t1, - Eigen::ConstRef t2) +// J is (6×6) flattened in column-major order +template +void point_point_tangent_basis_3D_jacobian( + T p0_x, T p0_y, T p0_z, T p1_x, T p1_y, T p1_z, T J[36]) { - Eigen::Matrix J; - autogen::point_triangle_tangent_basis_jacobian( - p[0], p[1], p[2], t0[0], t0[1], t0[2], t1[0], t1[1], t1[2], t2[0], - t2[1], t2[2], J.data()); - return J; + const T t0 = p0_x - p1_x; + const T t1 = t0 * t0; + const T t2 = p0_z - p1_z; + const T t3 = t2 * t2; + const T t4 = p0_y - p1_y; + const T t5 = t4 * t4; + const bool t6 = (t1 + t3) < (t3 + t5); + const T t7 = -t2; + const T t8 = t6 ? 0 : t7; + const T t9 = (t6 ? 0 : t3) + (t6 ? t3 : 0) + (t6 ? t5 : t1); + const T t10 = T(1) / pow_1_5(t9); + const T t11 = T(0.5) * (t6 ? 0 : (2 * t0)); + const T t12 = t10 * t11; + const T t13 = t6 ? t2 : 0; + const T t14 = T(1) / std::sqrt(t9); + const T t15 = t6 ? 0 : 1; + const T t16 = -t4; + const T t17 = t6 ? t16 : t0; + const T t18 = T(1) / t9; + const T t19 = t17 * t18; + const T t20 = t0 * t13 - t4 * t8; + const T t21 = -t20; + const bool t22 = t1 + (t7 * t7) < (t16 * t16) + t3; + const T t23 = t22 ? 0 : t7; + const T t24 = -t0; + const T t25 = t22 ? t16 : t0; + const T t26 = -t23 * t7 + t24 * t25; + const T t27 = -t13 * t2 + t17 * t4; + const T t28 = -t27; + const T t29 = t21 * t21 + t26 * t26 + t28 * t28; + const T t30 = T(1) / std::sqrt(t29); + const T t31 = t15 * t4; + const T t32 = T(1) / t29; + const T t33 = t22 ? t2 : 0; + const T t34 = -t16 * t23 + t24 * t33; + const T t35 = t33 * t34; + const T t36 = t16 * t25 - t33 * t7; + const T t37 = t22 ? 0 : 1; + const T t38 = t16 * t37; + const T t39 = t24 * t37; + const T t40 = -t25; + const T t41 = -t26; + const T t42 = t32 * (-t35 + t36 * t38 + t41 * (-t39 - t40)); + const T t43 = T(0.5) * (t6 ? (2 * t4) : 0); + const T t44 = t10 * t43; + const T t45 = t6 ? -1 : 0; + const T t46 = -t0 * t17 + t2 * t8; + const T t47 = t20 * t20 + t27 * t27 + t46 * t46; + const T t48 = T(1) / std::sqrt(t47); + const T t49 = T(1) / t47; + const T t50 = t0 * t45; + const T t51 = t17 + t4 * t45; + const T t52 = t22 ? (-1) : 0; + const T t53 = t24 * t52; + const T t54 = t32 * (t23 * t34 + t36 * (t16 * t52 + t40) - t41 * t53); + const T t55 = -t8; + const T t56 = t6 ? 0 : -1; + const T t57 = T(0.5) * t8; + const T t58 = 2 * t2; + const T t59 = t18 * ((t22 ? 0 : t58) + (t22 ? t58 : 0)); + const T t60 = t6 ? 1 : 0; + const T t61 = T(0.5) * t13; + const T t62 = T(0.5) * t10 * t17; + const T t63 = t22 ? 1 : 0; + const T t64 = t22 ? 0 : -1; + const T t65 = t16 * t64; + const T t66 = -t33 + t63 * t7; + const T t67 = t28 * t32; + const T t68 = t0 * t60; + const T t69 = t2 * t56 + t8; + const T t70 = t21 * (t4 * t56 - t68) + t26 * t69 - t28 * t66; + const T t71 = t4 * t56; + const T t72 = t6 ? 0 : (2 * t24); + const T t73 = t10 * t72; + const T t74 = T(0.5) * t19; + const T t75 = t24 * t64 + t25; + const T t76 = t35 + t36 * t65 - t41 * t75; + const T t77 = t6 ? (2 * t16) : 0; + const T t78 = t10 * t77; + const T t79 = -t17 + t4 * t60; + const T t80 = t0 * t46 * t60 - t20 * t8 - t27 * t79; + const T t81 = t20 * t49; + const T t82 = 2 * t7; + const T t83 = t18 * ((t22 ? 0 : t82) + (t22 ? t82 : 0)); + const T t84 = t33 + t52 * t7; + const T t85 = t15 * t2; + const T t86 = t21 * (t15 * t4 - t50) - t26 * (-t55 - t85) - t28 * t84; + J[0] = -t12 * t8; + J[1] = -t12 * t13; + J[2] = t14 * (-t11 * t19 + t15); + J[3] = -t30 * (t28 * t42 + t31); + J[4] = t30 * (t25 + t26 * t42 - t39); + J[5] = -t30 * (t13 + t21 * t42); + J[6] = -t44 * t8; + J[7] = -t13 * t44; + J[8] = t14 * (-t19 * t43 + t45); + J[9] = t48 * (t27 * t49 * (t21 * t8 - t26 * t50 - t28 * t51) - t51); + J[10] = t30 * (t26 * t54 + t50); + J[11] = t30 * (-t21 * t54 - t55); + J[12] = t14 * (t56 - t57 * t59); + J[13] = t14 * (-t59 * t61 + t60); + J[14] = -t62 * ((t6 ? 0 : t58) + (t6 ? t58 : 0)); + J[15] = -t30 + * (t66 + + t67 + * (t34 * (t24 * t63 - t65) - t36 * t66 + + t41 * (-t23 + t64 * t7))); + J[16] = t48 * (t46 * t49 * t70 - t69); + J[17] = t48 * (t20 * t49 * t70 - t68 + t71); + J[18] = -t57 * t73; + J[19] = -t61 * t73; + J[20] = t14 * (t56 - t72 * t74); + J[21] = -t30 * (t67 * t76 + t71); + J[22] = t30 * (t26 * t32 * t76 - t75); + J[23] = t30 * (t13 - t21 * t32 * t76); + J[24] = -t57 * t78; + J[25] = -t61 * t78; + J[26] = t14 * (t60 - t74 * t77); + J[27] = -t48 * (t27 * t49 * t80 + t79); + J[28] = t30 * (-t26 * t32 * (t21 * t8 + t26 * t68 + t28 * t79) + t68); + J[29] = -t48 * (t8 + t80 * t81); + J[30] = t14 * (t15 - t57 * t83); + J[31] = t14 * (t45 - t61 * t83); + J[32] = -t62 * ((t6 ? 0 : t82) + (t6 ? t82 : 0)); + J[33] = -t30 + * (t67 * (t34 * (-t38 + t53) - t36 * t84 + t41 * (t23 + t37 * t7)) + + t84); + J[34] = t48 * (t46 * t49 * t86 + t8 - t85); + J[35] = t48 * (t31 - t50 + t81 * t86); } -// ============================================================================ - -namespace autogen { - // J is (2×4) flattened in column-major order - void point_point_tangent_basis_2D_jacobian( - double p0_x, double p0_y, double p1_x, double p1_y, double J[8]) - { - const double t0 = p0_x - p1_x; - const double t1 = p0_y - p1_y; - const double t2 = t0 * t0; - const double t3 = t1 * t1; - const double t4 = t2 + t3; - const double t5 = t0 * t1 / pow_1_5(t4); - const double t6 = -t5; - const double t7 = 1.0 / std::sqrt(t4); - const double t8 = 1.0 / t4; - const double t9 = t2 * t8 - 1; - const double t10 = t3 * t8 - 1; - J[0] = t6; - J[1] = t7 * t9; - J[2] = -t10 * t7; - J[3] = t5; - J[4] = t5; - J[5] = -t7 * t9; - J[6] = t10 * t7; - J[7] = t6; - } - - // J is (6×6) flattened in column-major order - void point_point_tangent_basis_3D_jacobian( - double p0_x, - double p0_y, - double p0_z, - double p1_x, - double p1_y, - double p1_z, - double J[36]) - { - const double t0 = p0_x - p1_x; - const double t1 = t0 * t0; - const double t2 = p0_z - p1_z; - const double t3 = t2 * t2; - const double t4 = p0_y - p1_y; - const double t5 = t4 * t4; - const bool t6 = (t1 + t3) < (t3 + t5); - const double t7 = -t2; - const double t8 = t6 ? 0 : t7; - const double t9 = (t6 ? 0 : t3) + (t6 ? t3 : 0) + (t6 ? t5 : t1); - const double t10 = 1.0 / pow_1_5(t9); - const double t11 = 0.5 * (t6 ? 0 : (2 * t0)); - const double t12 = t10 * t11; - const double t13 = t6 ? t2 : 0; - const double t14 = 1.0 / std::sqrt(t9); - const double t15 = t6 ? 0 : 1; - const double t16 = -t4; - const double t17 = t6 ? t16 : t0; - const double t18 = 1.0 / t9; - const double t19 = t17 * t18; - const double t20 = t0 * t13 - t4 * t8; - const double t21 = -t20; - const bool t22 = t1 + (t7 * t7) < (t16 * t16) + t3; - const double t23 = t22 ? 0 : t7; - const double t24 = -t0; - const double t25 = t22 ? t16 : t0; - const double t26 = -t23 * t7 + t24 * t25; - const double t27 = -t13 * t2 + t17 * t4; - const double t28 = -t27; - const double t29 = t21 * t21 + t26 * t26 + t28 * t28; - const double t30 = 1.0 / std::sqrt(t29); - const double t31 = t15 * t4; - const double t32 = 1.0 / t29; - const double t33 = t22 ? t2 : 0; - const double t34 = -t16 * t23 + t24 * t33; - const double t35 = t33 * t34; - const double t36 = t16 * t25 - t33 * t7; - const double t37 = t22 ? 0 : 1; - const double t38 = t16 * t37; - const double t39 = t24 * t37; - const double t40 = -t25; - const double t41 = -t26; - const double t42 = t32 * (-t35 + t36 * t38 + t41 * (-t39 - t40)); - const double t43 = 0.5 * (t6 ? (2 * t4) : 0); - const double t44 = t10 * t43; - const double t45 = t6 ? -1 : 0; - const double t46 = -t0 * t17 + t2 * t8; - const double t47 = t20 * t20 + t27 * t27 + t46 * t46; - const double t48 = 1.0 / std::sqrt(t47); - const double t49 = 1.0 / t47; - const double t50 = t0 * t45; - const double t51 = t17 + t4 * t45; - const double t52 = t22 ? (-1) : 0; - const double t53 = t24 * t52; - const double t54 = - t32 * (t23 * t34 + t36 * (t16 * t52 + t40) - t41 * t53); - const double t55 = -t8; - const double t56 = t6 ? 0 : -1; - const double t57 = 0.5 * t8; - const double t58 = 2 * t2; - const double t59 = t18 * ((t22 ? 0 : t58) + (t22 ? t58 : 0)); - const double t60 = t6 ? 1 : 0; - const double t61 = 0.5 * t13; - const double t62 = 0.5 * t10 * t17; - const double t63 = t22 ? 1 : 0; - const double t64 = t22 ? 0 : -1; - const double t65 = t16 * t64; - const double t66 = -t33 + t63 * t7; - const double t67 = t28 * t32; - const double t68 = t0 * t60; - const double t69 = t2 * t56 + t8; - const double t70 = t21 * (t4 * t56 - t68) + t26 * t69 - t28 * t66; - const double t71 = t4 * t56; - const double t72 = t6 ? 0 : (2 * t24); - const double t73 = t10 * t72; - const double t74 = 0.5 * t19; - const double t75 = t24 * t64 + t25; - const double t76 = t35 + t36 * t65 - t41 * t75; - const double t77 = t6 ? (2 * t16) : 0; - const double t78 = t10 * t77; - const double t79 = -t17 + t4 * t60; - const double t80 = t0 * t46 * t60 - t20 * t8 - t27 * t79; - const double t81 = t20 * t49; - const double t82 = 2 * t7; - const double t83 = t18 * ((t22 ? 0 : t82) + (t22 ? t82 : 0)); - const double t84 = t33 + t52 * t7; - const double t85 = t15 * t2; - const double t86 = - t21 * (t15 * t4 - t50) - t26 * (-t55 - t85) - t28 * t84; - J[0] = -t12 * t8; - J[1] = -t12 * t13; - J[2] = t14 * (-t11 * t19 + t15); - J[3] = -t30 * (t28 * t42 + t31); - J[4] = t30 * (t25 + t26 * t42 - t39); - J[5] = -t30 * (t13 + t21 * t42); - J[6] = -t44 * t8; - J[7] = -t13 * t44; - J[8] = t14 * (-t19 * t43 + t45); - J[9] = t48 * (t27 * t49 * (t21 * t8 - t26 * t50 - t28 * t51) - t51); - J[10] = t30 * (t26 * t54 + t50); - J[11] = t30 * (-t21 * t54 - t55); - J[12] = t14 * (t56 - t57 * t59); - J[13] = t14 * (-t59 * t61 + t60); - J[14] = -t62 * ((t6 ? 0 : t58) + (t6 ? t58 : 0)); - J[15] = -t30 - * (t66 - + t67 - * (t34 * (t24 * t63 - t65) - t36 * t66 - + t41 * (-t23 + t64 * t7))); - J[16] = t48 * (t46 * t49 * t70 - t69); - J[17] = t48 * (t20 * t49 * t70 - t68 + t71); - J[18] = -t57 * t73; - J[19] = -t61 * t73; - J[20] = t14 * (t56 - t72 * t74); - J[21] = -t30 * (t67 * t76 + t71); - J[22] = t30 * (t26 * t32 * t76 - t75); - J[23] = t30 * (t13 - t21 * t32 * t76); - J[24] = -t57 * t78; - J[25] = -t61 * t78; - J[26] = t14 * (t60 - t74 * t77); - J[27] = -t48 * (t27 * t49 * t80 + t79); - J[28] = t30 * (-t26 * t32 * (t21 * t8 + t26 * t68 + t28 * t79) + t68); - J[29] = -t48 * (t8 + t80 * t81); - J[30] = t14 * (t15 - t57 * t83); - J[31] = t14 * (t45 - t61 * t83); - J[32] = -t62 * ((t6 ? 0 : t82) + (t6 ? t82 : 0)); - J[33] = -t30 - * (t67 * (t34 * (-t38 + t53) - t36 * t84 + t41 * (t23 + t37 * t7)) - + t84); - J[34] = t48 * (t46 * t49 * t86 + t8 - t85); - J[35] = t48 * (t31 - t50 + t81 * t86); - } +// J is (2×6) flattened in column-major order +template +void point_edge_tangent_basis_2D_jacobian( + T p_x, T p_y, T e0_x, T e0_y, T e1_x, T e1_y, T J[12]) +{ + const T t0 = e0_x - e1_x; + const T t1 = t0 * t0; + const T t2 = e0_y - e1_y; + const T t3 = t2 * t2; + const T t4 = t1 + t3; + const T t5 = T(1) / std::sqrt(t4); + const T t6 = T(1) / t4; + const T t7 = t1 * t6 - 1; + const T t8 = t0 * t2 / (t4 * std::sqrt(t4)); + const T t9 = t3 * t6 - 1; + const T t10 = -t8; + J[0] = 0; + J[1] = 0; + J[2] = 0; + J[3] = 0; + J[4] = t5 * t7; + J[5] = t8; + J[6] = t8; + J[7] = t5 * t9; + J[8] = -t5 * t7; + J[9] = t10; + J[10] = t10; + J[11] = -t5 * t9; +} - // J is (2×6) flattened in column-major order - void point_edge_tangent_basis_2D_jacobian( - double p_x, - double p_y, - double e0_x, - double e0_y, - double e1_x, - double e1_y, - double J[12]) - { - const double t0 = e0_x - e1_x; - const double t1 = t0 * t0; - const double t2 = e0_y - e1_y; - const double t3 = t2 * t2; - const double t4 = t1 + t3; - const double t5 = 1.0 / std::sqrt(t4); - const double t6 = 1.0 / t4; - const double t7 = t1 * t6 - 1; - const double t8 = t0 * t2 / (t4 * std::sqrt(t4)); - const double t9 = t3 * t6 - 1; - const double t10 = -t8; - J[0] = 0; - J[1] = 0; - J[2] = 0; - J[3] = 0; - J[4] = t5 * t7; - J[5] = t8; - J[6] = t8; - J[7] = t5 * t9; - J[8] = -t5 * t7; - J[9] = t10; - J[10] = t10; - J[11] = -t5 * t9; - } +// J is (6×9) flattened in column-major order +template +void point_edge_tangent_basis_3D_jacobian( + T p_x, + T p_y, + T p_z, + T e0_x, + T e0_y, + T e0_z, + T e1_x, + T e1_y, + T e1_z, + T J[54]) +{ + const T t0 = -e1_y; + const T t1 = e0_y + t0; + const T t2 = -e1_x; + const T t3 = e0_x + t2; + const T t4 = -p_y; + const T t5 = e0_y + t4; + const T t6 = -p_x; + const T t7 = e0_x + t6; + const T t8 = -t1 * t7 + t3 * t5; + const T t9 = -e1_z; + const T t10 = e0_z + t9; + const T t11 = -t3; + const T t12 = -p_z; + const T t13 = e0_z + t12; + const T t14 = -t13; + const T t15 = -t7; + const T t16 = -t10; + const T t17 = t11 * t14 - t15 * t16; + const T t18 = t1 * t8 + t10 * t17; + const T t19 = t1 * t13 - t10 * t5; + const T t20 = -t10 * t7 + t13 * t3; + const T t21 = t19 * t19 + t8 * t8; + const T t22 = t20 * t20 + t21; + const T t23 = T(1) / pow_1_5(t22); + const T t24 = t19 * t23; + const T t25 = t17 * t17 + t21; + const T t26 = T(1) / std::sqrt(t25); + const T t27 = -t5; + const T t28 = -t1; + const T t29 = t11 * t27 - t15 * t28; + const T t30 = -t17; + const T t31 = T(1) / t25; + const T t32 = t17 * t31; + const T t33 = -e0_z; + const T t34 = e1_z + t33; + const T t35 = T(1) / std::sqrt(t22); + const T t36 = -e0_y; + const T t37 = T(1) / t22; + const T t38 = t37 * t8; + const T t39 = t10 * t19 - t3 * t8; + const T t40 = t19 * t37; + const T t41 = t20 * t23; + const T t42 = t14 * t28 - t16 * t27; + const T t43 = t31 * t8; + const T t44 = t19 * t31; + const T t45 = -e0_x; + const T t46 = t1 * t19 + t17 * t3; + const T t47 = t20 * t37; + const T t48 = t23 * t8; + const T t49 = t3 * t3; + const T t50 = t1 * t1; + const T t51 = t10 * t10; + const T t52 = t49 + t50 + t51; + const T t53 = T(1) / std::sqrt(t52); + const T t54 = T(1) / t52; + const T t55 = t49 * t54 - 1; + const T t56 = T(1) / pow_1_5(t52); + const T t57 = t3 * t56; + const T t58 = t1 * t57; + const T t59 = t10 * t57; + const T t60 = e1_y + t4; + const T t61 = e1_z + t12; + const T t62 = t17 * t61 + t60 * t8; + const T t63 = p_z + t9; + const T t64 = t50 * t54 - 1; + const T t65 = t1 * t10 * t56; + const T t66 = e1_x + t6; + const T t67 = t19 * t61 - t66 * t8; + const T t68 = t51 * t54 - 1; + const T t69 = t17 * t66 + t19 * t60; + const T t70 = -t58; + const T t71 = -t59; + const T t72 = t13 * t17 + t5 * t8; + const T t73 = -t65; + const T t74 = -t13 * t19 + t7 * t8; + const T t75 = p_x + t45; + const T t76 = t17 * t7 + t19 * t5; + J[0] = 0; + J[1] = 0; + J[2] = 0; + J[3] = -t18 * t24; + J[4] = t26 * (t32 * (t1 * t29 + t16 * t30) + t34); + J[5] = t35 * (-e1_y - t18 * t38 - t36); + J[6] = 0; + J[7] = 0; + J[8] = 0; + J[9] = t35 * (-t34 - t39 * t40); + J[10] = t39 * t41; + J[11] = -t26 * (t3 + t43 * (t10 * t42 + t11 * t29)); + J[12] = 0; + J[13] = 0; + J[14] = 0; + J[15] = -t26 * (t1 + t44 * (t28 * t42 + t3 * t30)); + J[16] = t35 * (-e1_x - t45 - t46 * t47); + J[17] = t46 * t48; + J[18] = t53 * t55; + J[19] = t58; + J[20] = t59; + J[21] = -t24 * t62; + J[22] = t26 * (t32 * (t29 * t60 - t30 * t61) + t63); + J[23] = t35 * (-p_y - t0 - t38 * t62); + J[24] = t58; + J[25] = t53 * t64; + J[26] = t65; + J[27] = t35 * (-t40 * t67 - t63); + J[28] = t41 * t67; + J[29] = -t26 * (t43 * (-t29 * t66 + t42 * t61) + t66); + J[30] = t59; + J[31] = t65; + J[32] = t53 * t68; + J[33] = -t26 * (t44 * (t30 * t66 - t42 * t60) + t60); + J[34] = t35 * (-p_x - t2 - t47 * t69); + J[35] = t48 * t69; + J[36] = -t53 * t55; + J[37] = t70; + J[38] = t71; + J[39] = t24 * t72; + J[40] = t35 * (-p_z - t33 - t47 * t72); + J[41] = -t26 * (t43 * (t13 * t30 + t27 * t29) + t5); + J[42] = t70; + J[43] = -t53 * t64; + J[44] = t73; + J[45] = -t26 * (t13 + t44 * (t14 * t42 + t29 * t7)); + J[46] = t41 * t74; + J[47] = t35 * (-t38 * t74 - t75); + J[48] = t71; + J[49] = t73; + J[50] = -t53 * t68; + J[51] = t35 * (-p_y - t36 - t40 * t76); + J[52] = t26 * (t32 * (t15 * t30 + t42 * t5) + t75); + J[53] = -t48 * t76; +} - // J is (6×9) flattened in column-major order - void point_edge_tangent_basis_3D_jacobian( - 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 J[54]) - { - const double t0 = -e1_y; - const double t1 = e0_y + t0; - const double t2 = -e1_x; - const double t3 = e0_x + t2; - const double t4 = -p_y; - const double t5 = e0_y + t4; - const double t6 = -p_x; - const double t7 = e0_x + t6; - const double t8 = -t1 * t7 + t3 * t5; - const double t9 = -e1_z; - const double t10 = e0_z + t9; - const double t11 = -t3; - const double t12 = -p_z; - const double t13 = e0_z + t12; - const double t14 = -t13; - const double t15 = -t7; - const double t16 = -t10; - const double t17 = t11 * t14 - t15 * t16; - const double t18 = t1 * t8 + t10 * t17; - const double t19 = t1 * t13 - t10 * t5; - const double t20 = -t10 * t7 + t13 * t3; - const double t21 = t19 * t19 + t8 * t8; - const double t22 = t20 * t20 + t21; - const double t23 = 1.0 / pow_1_5(t22); - const double t24 = t19 * t23; - const double t25 = t17 * t17 + t21; - const double t26 = 1.0 / std::sqrt(t25); - const double t27 = -t5; - const double t28 = -t1; - const double t29 = t11 * t27 - t15 * t28; - const double t30 = -t17; - const double t31 = 1.0 / t25; - const double t32 = t17 * t31; - const double t33 = -e0_z; - const double t34 = e1_z + t33; - const double t35 = 1.0 / std::sqrt(t22); - const double t36 = -e0_y; - const double t37 = 1.0 / t22; - const double t38 = t37 * t8; - const double t39 = t10 * t19 - t3 * t8; - const double t40 = t19 * t37; - const double t41 = t20 * t23; - const double t42 = t14 * t28 - t16 * t27; - const double t43 = t31 * t8; - const double t44 = t19 * t31; - const double t45 = -e0_x; - const double t46 = t1 * t19 + t17 * t3; - const double t47 = t20 * t37; - const double t48 = t23 * t8; - const double t49 = t3 * t3; - const double t50 = t1 * t1; - const double t51 = t10 * t10; - const double t52 = t49 + t50 + t51; - const double t53 = 1.0 / std::sqrt(t52); - const double t54 = 1.0 / t52; - const double t55 = t49 * t54 - 1; - const double t56 = 1.0 / pow_1_5(t52); - const double t57 = t3 * t56; - const double t58 = t1 * t57; - const double t59 = t10 * t57; - const double t60 = e1_y + t4; - const double t61 = e1_z + t12; - const double t62 = t17 * t61 + t60 * t8; - const double t63 = p_z + t9; - const double t64 = t50 * t54 - 1; - const double t65 = t1 * t10 * t56; - const double t66 = e1_x + t6; - const double t67 = t19 * t61 - t66 * t8; - const double t68 = t51 * t54 - 1; - const double t69 = t17 * t66 + t19 * t60; - const double t70 = -t58; - const double t71 = -t59; - const double t72 = t13 * t17 + t5 * t8; - const double t73 = -t65; - const double t74 = -t13 * t19 + t7 * t8; - const double t75 = p_x + t45; - const double t76 = t17 * t7 + t19 * t5; - J[0] = 0; - J[1] = 0; - J[2] = 0; - J[3] = -t18 * t24; - J[4] = t26 * (t32 * (t1 * t29 + t16 * t30) + t34); - J[5] = t35 * (-e1_y - t18 * t38 - t36); - J[6] = 0; - J[7] = 0; - J[8] = 0; - J[9] = t35 * (-t34 - t39 * t40); - J[10] = t39 * t41; - J[11] = -t26 * (t3 + t43 * (t10 * t42 + t11 * t29)); - J[12] = 0; - J[13] = 0; - J[14] = 0; - J[15] = -t26 * (t1 + t44 * (t28 * t42 + t3 * t30)); - J[16] = t35 * (-e1_x - t45 - t46 * t47); - J[17] = t46 * t48; - J[18] = t53 * t55; - J[19] = t58; - J[20] = t59; - J[21] = -t24 * t62; - J[22] = t26 * (t32 * (t29 * t60 - t30 * t61) + t63); - J[23] = t35 * (-p_y - t0 - t38 * t62); - J[24] = t58; - J[25] = t53 * t64; - J[26] = t65; - J[27] = t35 * (-t40 * t67 - t63); - J[28] = t41 * t67; - J[29] = -t26 * (t43 * (-t29 * t66 + t42 * t61) + t66); - J[30] = t59; - J[31] = t65; - J[32] = t53 * t68; - J[33] = -t26 * (t44 * (t30 * t66 - t42 * t60) + t60); - J[34] = t35 * (-p_x - t2 - t47 * t69); - J[35] = t48 * t69; - J[36] = -t53 * t55; - J[37] = t70; - J[38] = t71; - J[39] = t24 * t72; - J[40] = t35 * (-p_z - t33 - t47 * t72); - J[41] = -t26 * (t43 * (t13 * t30 + t27 * t29) + t5); - J[42] = t70; - J[43] = -t53 * t64; - J[44] = t73; - J[45] = -t26 * (t13 + t44 * (t14 * t42 + t29 * t7)); - J[46] = t41 * t74; - J[47] = t35 * (-t38 * t74 - t75); - J[48] = t71; - J[49] = t73; - J[50] = -t53 * t68; - J[51] = t35 * (-p_y - t36 - t40 * t76); - J[52] = t26 * (t32 * (t15 * t30 + t42 * t5) + t75); - J[53] = -t48 * t76; - } +// J is (6×12) flattened in column-major order +template +void edge_edge_tangent_basis_jacobian( + T ea0_x, + T ea0_y, + T ea0_z, + T ea1_x, + T ea1_y, + T ea1_z, + T eb0_x, + T eb0_y, + T eb0_z, + T eb1_x, + T eb1_y, + T eb1_z, + T J[72]) +{ + const T t0 = ea0_y - ea1_y; + const T t1 = t0 * t0; + const T t2 = ea0_x - ea1_x; + const T t3 = t2 * t2; + const T t4 = ea0_z - ea1_z; + const T t5 = t4 * t4; + const T t6 = t3 + t5; + const T t7 = t1 + t6; + const T t8 = T(1) / std::sqrt(t7); + const T t9 = T(1) / t7; + const T t10 = t3 * t9 - 1; + const T t11 = T(1) / pow_1_5(t7); + const T t12 = t0 * t2; + const T t13 = t11 * t12; + const T t14 = t2 * t4; + const T t15 = t11 * t14; + const T t16 = -t2; + const T t17 = eb0_z - eb1_z; + const T t18 = -t17; + const T t19 = t16 * t18; + const T t20 = -t4; + const T t21 = eb0_x - eb1_x; + const T t22 = -t21; + const T t23 = t20 * t22; + const T t24 = -t23; + const T t25 = t19 + t24; + const T t26 = -t25; + const T t27 = -t0; + const T t28 = t18 * t27; + const T t29 = eb0_y - eb1_y; + const T t30 = -t29; + const T t31 = t20 * t30; + const T t32 = t28 - t31; + const T t33 = t16 * t26 - t27 * t32; + const T t34 = t2 * t29; + const T t35 = t0 * t21; + const T t36 = -t35; + const T t37 = t34 + t36; + const T t38 = t0 * t17; + const T t39 = t29 * t4; + const T t40 = -t39; + const T t41 = t38 + t40; + const T t42 = t2 * t37 - t4 * t41; + const T t43 = -t42; + const T t44 = t16 * t30; + const T t45 = t22 * t27; + const T t46 = -t45; + const T t47 = t44 + t46; + const T t48 = -t20 * t26 + t27 * t47; + const T t49 = t33 * t33 + t43 * t43 + t48 * t48; + const T t50 = T(1) / std::sqrt(t49); + const T t51 = t27 * t29; + const T t52 = -t51; + const T t53 = T(1) / t49; + const T t54 = 2 * t19; + const T t55 = t24 + t54; + const T t56 = -t33; + const T t57 = -t18 * t20 + t51; + const T t58 = -t48; + const T t59 = t16 * t47 - t20 * t32; + const T t60 = + t53 * (-t55 * t56 - t57 * t58 + t59 * (t16 * t29 - t44 + t45)); + const T t61 = t0 * t41 + t2 * t25; + const T t62 = t0 * t37 + t25 * t4; + const T t63 = t42 * t42 + t61 * t61 + t62 * t62; + const T t64 = T(1) / std::sqrt(t63); + const T t65 = 2 * t34; + const T t66 = t36 + t65; + const T t67 = T(1) / t63; + const T t68 = t42 * t67; + const T t69 = t1 * t9 - 1; + const T t70 = t0 * t4; + const T t71 = t11 * t70; + const T t72 = 2 * t35; + const T t73 = 2 * t38; + const T t74 = t40 + t73; + const T t75 = t17 * t4 + t2 * t21; + const T t76 = t43 * t75; + const T t77 = t34 - t72; + const T t78 = t33 * t74 - t48 * t77 + t76; + const T t79 = t67 * t78; + const T t80 = t5 * t9 - 1; + const T t81 = t21 * t4; + const T t82 = 2 * t81; + const T t83 = t16 * t21; + const T t84 = t27 * t30; + const T t85 = -t84; + const T t86 = t83 + t85; + const T t87 = 2 * t39; + const T t88 = t17 * t2; + const T t89 = t62 * t67; + const T t90 = + t53 * (-t56 * t86 + t58 * (t20 * t21 + t25) + t59 * (t28 - 2 * t31)); + const T t91 = -t13; + const T t92 = -t15; + const T t93 = -t17 * t20 + t84; + const T t94 = -t19; + const T t95 = t16 * t17 + t23 + t94; + const T t96 = t48 * t53; + const T t97 = t33 * t95 + t43 * t66 + t48 * t93; + const T t98 = t61 * t67; + const T t99 = -t71; + const T t100 = -t33 * t74 + t48 * (t21 * t27 + t47) - t76; + const T t101 = t16 * t22; + const T t102 = t20 * t29 + t32; + const T t103 = + -t102 * t59 + t56 * (-t101 - t52) + t58 * (-t19 + 2 * t20 * t22); + const T t104 = t103 * t53; + const T t105 = t27 * t27; + const T t106 = -t20 * t4; + const T t107 = t105 + t106; + const T t108 = t107 * t48 + t12 * t43 - t14 * t33; + const T t109 = t16 * t59; + const T t110 = t16 * t56; + const T t111 = t33 * t53; + const T t112 = t27 * t56; + const T t113 = t27 * t58; + const T t114 = t20 * t20; + const T t115 = -t114; + const T t116 = t16 * t2; + const T t117 = t112 * t20 - t113 * t2 + t59 * (t115 + t116); + const T t118 = t43 * t53; + const T t119 = t16 * t16; + const T t120 = t0 * t27; + const T t121 = -t120; + const T t122 = t119 + t121; + const T t123 = t122 * t33 - t14 * t48 + t43 * t70; + const T t124 = t20 * t58; + const T t125 = t20 * t59; + const T t126 = t0 * t109 - t110 * t20 + t58 * (-t115 - t120); + const T t127 = t112 * t4 - t113 * t16 + t59 * (t106 + t119); + const T t128 = t124 * t2 - t125 * t27 + t56 * (t105 - t116); + J[0] = t10 * t8; + J[1] = t13; + J[2] = t15; + J[3] = t50 * (t18 * t20 + t48 * t60 + t52); + J[4] = t64 * (t35 - t65 + t68 * (t33 * t55 - t43 * t66 + t48 * t57)); + J[5] = t50 * (t23 + t33 * t60 - t54); + J[6] = t13; + J[7] = t69 * t8; + J[8] = t71; + J[9] = t64 * (t2 * t29 - t62 * t79 - t72); + J[10] = t64 * (t68 * t78 + t75); + J[11] = t64 * (t39 + t61 * t79 - t73); + J[12] = t15; + J[13] = t71; + J[14] = t8 * t80; + J[15] = t64 + * (t17 * t2 - t82 + - t89 * (t33 * t86 + t43 * (t38 - t87) - t48 * (-t82 + t88))); + J[16] = t50 * (t0 * t17 - t43 * t90 - t87); + J[17] = t50 * (t33 * t90 - t83 + t84); + J[18] = -t10 * t8; + J[19] = t91; + J[20] = t92; + J[21] = t50 + * (t17 * t20 + t85 + + t96 * (-t56 * t95 - t58 * t93 + t59 * (2 * t44 + t46))); + J[22] = t64 * (t66 + t68 * t97); + J[23] = t64 * (-t81 + 2 * t88 + t97 * t98); + J[24] = t91; + J[25] = -t69 * t8; + J[26] = t99; + J[27] = -t64 * (t100 * t89 + t77); + J[28] = t64 * (t100 * t42 * t67 - t75); + J[29] = t64 * (t100 * t98 + t74); + J[30] = t92; + J[31] = t99; + J[32] = -t8 * t80; + J[33] = t50 * (t103 * t96 + 2 * t23 + t94); + J[34] = -t50 * (t102 + t104 * t43); + J[35] = t50 * (-t101 + t104 * t33 + t51); + J[36] = 0; + J[37] = 0; + J[38] = 0; + J[39] = -t64 * (t1 + t108 * t89 + t5); + J[40] = t64 * (t108 * t68 + t12); + J[41] = t50 * (t111 * (-t107 * t58 + t109 * t27 - t110 * t4) + t14); + J[42] = 0; + J[43] = 0; + J[44] = 0; + J[45] = t50 * (t117 * t96 + t12); + J[46] = -t50 * (t117 * t118 + t6); + J[47] = t64 * (t70 - t98 * (t12 * t48 + t33 * t70 + t43 * t6)); + J[48] = 0; + J[49] = 0; + J[50] = 0; + J[51] = t64 * (-t123 * t89 + t14); + J[52] = t50 * (-t118 * (-t0 * t125 - t122 * t56 + t124 * t16) + t70); + J[53] = t64 * (-t1 + t123 * t61 * t67 - t3); + J[54] = 0; + J[55] = 0; + J[56] = 0; + J[57] = t50 * (t114 + t121 + t126 * t96); + J[58] = -t50 * (t118 * t126 + t12); + J[59] = t50 * (t126 * t33 * t53 - t14); + J[60] = 0; + J[61] = 0; + J[62] = 0; + J[63] = t50 * (-t12 + t127 * t48 * t53); + J[64] = t50 * (-t118 * t127 + t6); + J[65] = t50 * (t127 * t33 * t53 - t70); + J[66] = 0; + J[67] = 0; + J[68] = 0; + J[69] = t50 * (t128 * t48 * t53 - t14); + J[70] = -t50 * (t118 * t128 + t70); + J[71] = t50 * (t105 + t111 * t128 - t116); +} - // J is (6×12) flattened in column-major order - void edge_edge_tangent_basis_jacobian( - 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 J[72]) - { - const double t0 = ea0_y - ea1_y; - const double t1 = t0 * t0; - const double t2 = ea0_x - ea1_x; - const double t3 = t2 * t2; - const double t4 = ea0_z - ea1_z; - const double t5 = t4 * t4; - const double t6 = t3 + t5; - const double t7 = t1 + t6; - const double t8 = 1.0 / std::sqrt(t7); - const double t9 = 1.0 / t7; - const double t10 = t3 * t9 - 1; - const double t11 = 1.0 / pow_1_5(t7); - const double t12 = t0 * t2; - const double t13 = t11 * t12; - const double t14 = t2 * t4; - const double t15 = t11 * t14; - const double t16 = -t2; - const double t17 = eb0_z - eb1_z; - const double t18 = -t17; - const double t19 = t16 * t18; - const double t20 = -t4; - const double t21 = eb0_x - eb1_x; - const double t22 = -t21; - const double t23 = t20 * t22; - const double t24 = -t23; - const double t25 = t19 + t24; - const double t26 = -t25; - const double t27 = -t0; - const double t28 = t18 * t27; - const double t29 = eb0_y - eb1_y; - const double t30 = -t29; - const double t31 = t20 * t30; - const double t32 = t28 - t31; - const double t33 = t16 * t26 - t27 * t32; - const double t34 = t2 * t29; - const double t35 = t0 * t21; - const double t36 = -t35; - const double t37 = t34 + t36; - const double t38 = t0 * t17; - const double t39 = t29 * t4; - const double t40 = -t39; - const double t41 = t38 + t40; - const double t42 = t2 * t37 - t4 * t41; - const double t43 = -t42; - const double t44 = t16 * t30; - const double t45 = t22 * t27; - const double t46 = -t45; - const double t47 = t44 + t46; - const double t48 = -t20 * t26 + t27 * t47; - const double t49 = t33 * t33 + t43 * t43 + t48 * t48; - const double t50 = 1.0 / std::sqrt(t49); - const double t51 = t27 * t29; - const double t52 = -t51; - const double t53 = 1.0 / t49; - const double t54 = 2 * t19; - const double t55 = t24 + t54; - const double t56 = -t33; - const double t57 = -t18 * t20 + t51; - const double t58 = -t48; - const double t59 = t16 * t47 - t20 * t32; - const double t60 = - t53 * (-t55 * t56 - t57 * t58 + t59 * (t16 * t29 - t44 + t45)); - const double t61 = t0 * t41 + t2 * t25; - const double t62 = t0 * t37 + t25 * t4; - const double t63 = t42 * t42 + t61 * t61 + t62 * t62; - const double t64 = 1.0 / std::sqrt(t63); - const double t65 = 2 * t34; - const double t66 = t36 + t65; - const double t67 = 1.0 / t63; - const double t68 = t42 * t67; - const double t69 = t1 * t9 - 1; - const double t70 = t0 * t4; - const double t71 = t11 * t70; - const double t72 = 2 * t35; - const double t73 = 2 * t38; - const double t74 = t40 + t73; - const double t75 = t17 * t4 + t2 * t21; - const double t76 = t43 * t75; - const double t77 = t34 - t72; - const double t78 = t33 * t74 - t48 * t77 + t76; - const double t79 = t67 * t78; - const double t80 = t5 * t9 - 1; - const double t81 = t21 * t4; - const double t82 = 2 * t81; - const double t83 = t16 * t21; - const double t84 = t27 * t30; - const double t85 = -t84; - const double t86 = t83 + t85; - const double t87 = 2 * t39; - const double t88 = t17 * t2; - const double t89 = t62 * t67; - const double t90 = t53 - * (-t56 * t86 + t58 * (t20 * t21 + t25) + t59 * (t28 - 2 * t31)); - const double t91 = -t13; - const double t92 = -t15; - const double t93 = -t17 * t20 + t84; - const double t94 = -t19; - const double t95 = t16 * t17 + t23 + t94; - const double t96 = t48 * t53; - const double t97 = t33 * t95 + t43 * t66 + t48 * t93; - const double t98 = t61 * t67; - const double t99 = -t71; - const double t100 = -t33 * t74 + t48 * (t21 * t27 + t47) - t76; - const double t101 = t16 * t22; - const double t102 = t20 * t29 + t32; - const double t103 = - -t102 * t59 + t56 * (-t101 - t52) + t58 * (-t19 + 2 * t20 * t22); - const double t104 = t103 * t53; - const double t105 = t27 * t27; - const double t106 = -t20 * t4; - const double t107 = t105 + t106; - const double t108 = t107 * t48 + t12 * t43 - t14 * t33; - const double t109 = t16 * t59; - const double t110 = t16 * t56; - const double t111 = t33 * t53; - const double t112 = t27 * t56; - const double t113 = t27 * t58; - const double t114 = t20 * t20; - const double t115 = -t114; - const double t116 = t16 * t2; - const double t117 = t112 * t20 - t113 * t2 + t59 * (t115 + t116); - const double t118 = t43 * t53; - const double t119 = t16 * t16; - const double t120 = t0 * t27; - const double t121 = -t120; - const double t122 = t119 + t121; - const double t123 = t122 * t33 - t14 * t48 + t43 * t70; - const double t124 = t20 * t58; - const double t125 = t20 * t59; - const double t126 = t0 * t109 - t110 * t20 + t58 * (-t115 - t120); - const double t127 = t112 * t4 - t113 * t16 + t59 * (t106 + t119); - const double t128 = t124 * t2 - t125 * t27 + t56 * (t105 - t116); - J[0] = t10 * t8; - J[1] = t13; - J[2] = t15; - J[3] = t50 * (t18 * t20 + t48 * t60 + t52); - J[4] = t64 * (t35 - t65 + t68 * (t33 * t55 - t43 * t66 + t48 * t57)); - J[5] = t50 * (t23 + t33 * t60 - t54); - J[6] = t13; - J[7] = t69 * t8; - J[8] = t71; - J[9] = t64 * (t2 * t29 - t62 * t79 - t72); - J[10] = t64 * (t68 * t78 + t75); - J[11] = t64 * (t39 + t61 * t79 - t73); - J[12] = t15; - J[13] = t71; - J[14] = t8 * t80; - J[15] = t64 - * (t17 * t2 - t82 - - t89 * (t33 * t86 + t43 * (t38 - t87) - t48 * (-t82 + t88))); - J[16] = t50 * (t0 * t17 - t43 * t90 - t87); - J[17] = t50 * (t33 * t90 - t83 + t84); - J[18] = -t10 * t8; - J[19] = t91; - J[20] = t92; - J[21] = t50 - * (t17 * t20 + t85 - + t96 * (-t56 * t95 - t58 * t93 + t59 * (2 * t44 + t46))); - J[22] = t64 * (t66 + t68 * t97); - J[23] = t64 * (-t81 + 2 * t88 + t97 * t98); - J[24] = t91; - J[25] = -t69 * t8; - J[26] = t99; - J[27] = -t64 * (t100 * t89 + t77); - J[28] = t64 * (t100 * t42 * t67 - t75); - J[29] = t64 * (t100 * t98 + t74); - J[30] = t92; - J[31] = t99; - J[32] = -t8 * t80; - J[33] = t50 * (t103 * t96 + 2 * t23 + t94); - J[34] = -t50 * (t102 + t104 * t43); - J[35] = t50 * (-t101 + t104 * t33 + t51); - J[36] = 0; - J[37] = 0; - J[38] = 0; - J[39] = -t64 * (t1 + t108 * t89 + t5); - J[40] = t64 * (t108 * t68 + t12); - J[41] = t50 * (t111 * (-t107 * t58 + t109 * t27 - t110 * t4) + t14); - J[42] = 0; - J[43] = 0; - J[44] = 0; - J[45] = t50 * (t117 * t96 + t12); - J[46] = -t50 * (t117 * t118 + t6); - J[47] = t64 * (t70 - t98 * (t12 * t48 + t33 * t70 + t43 * t6)); - J[48] = 0; - J[49] = 0; - J[50] = 0; - J[51] = t64 * (-t123 * t89 + t14); - J[52] = t50 * (-t118 * (-t0 * t125 - t122 * t56 + t124 * t16) + t70); - J[53] = t64 * (-t1 + t123 * t61 * t67 - t3); - J[54] = 0; - J[55] = 0; - J[56] = 0; - J[57] = t50 * (t114 + t121 + t126 * t96); - J[58] = -t50 * (t118 * t126 + t12); - J[59] = t50 * (t126 * t33 * t53 - t14); - J[60] = 0; - J[61] = 0; - J[62] = 0; - J[63] = t50 * (-t12 + t127 * t48 * t53); - J[64] = t50 * (-t118 * t127 + t6); - J[65] = t50 * (t127 * t33 * t53 - t70); - J[66] = 0; - J[67] = 0; - J[68] = 0; - J[69] = t50 * (t128 * t48 * t53 - t14); - J[70] = -t50 * (t118 * t128 + t70); - J[71] = t50 * (t105 + t111 * t128 - t116); - } +// J is (6×12) flattened in column-major order +template +void point_triangle_tangent_basis_jacobian( + T p_x, + T p_y, + T p_z, + T t0_x, + T t0_y, + T t0_z, + T t1_x, + T t1_y, + T t1_z, + T t2_x, + T t2_y, + T t2_z, + T J[72]) +{ + const T t0 = t0_y - t1_y; + const T t1 = t0 * t0; + const T t2 = t0_x - t1_x; + const T t3 = t2 * t2; + const T t4 = t0_z - t1_z; + const T t5 = t4 * t4; + const T t6 = t3 + t5; + const T t7 = t1 + t6; + const T t8 = (T(1) / std::sqrt(t7)); + const T t9 = T(1) / t7; + const T t10 = t3 * t9 - 1; + const T t11 = T(1) / pow_1_5(t7); + const T t12 = t0 * t2; + const T t13 = t11 * t12; + const T t14 = t2 * t4; + const T t15 = t11 * t14; + const T t16 = -t2; + const T t17 = -t2_z; + const T t18 = t0_z + t17; + const T t19 = -t18; + const T t20 = t16 * t19; + const T t21 = -t2_x; + const T t22 = t0_x + t21; + const T t23 = -t22; + const T t24 = -t4; + const T t25 = t23 * t24; + const T t26 = t20 - t25; + const T t27 = -t26; + const T t28 = -t0; + const T t29 = t19 * t28; + const T t30 = -t2_y; + const T t31 = t0_y + t30; + const T t32 = -t31; + const T t33 = t24 * t32; + const T t34 = t29 - t33; + const T t35 = t16 * t27 - t28 * t34; + const T t36 = t2 * t31; + const T t37 = t0 * t22; + const T t38 = -t37; + const T t39 = t36 + t38; + const T t40 = t0 * t18; + const T t41 = -t31 * t4; + const T t42 = t40 + t41; + const T t43 = t2 * t39 - t4 * t42; + const T t44 = -t43; + const T t45 = t16 * t32; + const T t46 = t23 * t28; + const T t47 = -t46; + const T t48 = t45 + t47; + const T t49 = -t24 * t27 + t28 * t48; + const T t50 = t35 * t35 + t44 * t44 + t49 * t49; + const T t51 = T(1) / std::sqrt(t50); + const T t52 = t17 + t1_z; + const T t53 = -t52; + const T t54 = t1_y + t30; + const T t55 = t28 * t54; + const T t56 = T(1) / t50; + const T t57 = -t24 * t53 + t55; + const T t58 = -t49; + const T t59 = -t45 + t46; + const T t60 = t16 * t48 - t24 * t34; + const T t61 = t16 * t53 + t26; + const T t62 = -t35; + const T t63 = t56 * (-t57 * t58 + t60 * (t16 * t54 + t59) - t61 * t62); + const T t64 = t2 * t54 + t39; + const T t65 = t0 * t42 + t2 * t26; + const T t66 = t0 * t39 + t26 * t4; + const T t67 = t43 * t43 + t65 * t65 + t66 * t66; + const T t68 = T(1) / std::sqrt(t67); + const T t69 = t18 * t2; + const T t70 = t22 * t4; + const T t71 = -t70; + const T t72 = T(1) / t67; + const T t73 = t1 * t9 - 1; + const T t74 = t0 * t4; + const T t75 = t11 * t74; + const T t76 = t1_x + t21; + const T t77 = t2 * t76 + t4 * t52; + const T t78 = t0 * t52 + t42; + const T t79 = t35 * t78 + t44 * t77 + t49 * (-t28 * t76 + t59); + const T t80 = t72 * t79; + const T t81 = t5 * t9 - 1; + const T t82 = t16 * t76; + const T t83 = -t54; + const T t84 = t28 * t83; + const T t85 = t82 - t84; + const T t86 = t24 * t83 - t29 + t33; + const T t87 = t4 * t76 - t69 + t70; + const T t88 = t66 * t72; + const T t89 = t56 * (t58 * (t24 * t76 + t26) - t60 * t86 - t62 * t85); + const T t90 = -t13; + const T t91 = -t15; + const T t92 = t28 * t32; + const T t93 = -t18 * t24 + t92; + const T t94 = -t20; + const T t95 = t16 * t18 + t25 + t94; + const T t96 = t49 * t56; + const T t97 = 2 * t36 + t38; + const T t98 = t35 * t95 + t44 * t97 + t49 * t93; + const T t99 = t65 * t72; + const T t100 = -t75; + const T t101 = t18 * t4 + t2 * t22; + const T t102 = 2 * t40 + t41; + const T t103 = -t101 * t44 - t102 * t35 + t49 * (t22 * t28 + t48); + const T t104 = t16 * t23; + const T t105 = t24 * t31 + t34; + const T t106 = + -t105 * t60 + t58 * (-t20 + 2 * t23 * t24) + t62 * (-t104 + t28 * t31); + const T t107 = t106 * t56; + const T t108 = t24 * t24; + const T t109 = t0 * t28; + const T t110 = t0 * t16 * t60 - t16 * t24 * t62 + t58 * (t108 - t109); + const T t111 = + -t16 * t28 * t58 + t28 * t4 * t62 + t60 * (t16 * t16 - t24 * t4); + const T t112 = t28 * t28; + const T t113 = t16 * t2; + const T t114 = t2 * t24 * t58 - t24 * t28 * t60 + t62 * (t112 - t113); + const T t115 = t114 * t56; + J[0] = 0; + J[1] = 0; + J[2] = 0; + J[3] = 0; + J[4] = 0; + J[5] = 0; + J[6] = 0; + J[7] = 0; + J[8] = 0; + J[9] = 0; + J[10] = 0; + J[11] = 0; + J[12] = 0; + J[13] = 0; + J[14] = 0; + J[15] = 0; + J[16] = 0; + J[17] = 0; + J[18] = t10 * t8; + J[19] = t13; + J[20] = t15; + J[21] = t51 * (t24 * t53 + t49 * t63 - t55); + J[22] = t51 * (-t44 * t63 - t64); + J[23] = t68 + * (-t2 * t52 + t65 * t72 * (t35 * t61 - t44 * t64 + t49 * t57) - t69 + - t71); + J[24] = t13; + J[25] = t73 * t8; + J[26] = t75; + J[27] = -t68 * (t0 * t76 - t36 + t37 + t66 * t80); + J[28] = t68 * (t43 * t80 + t77); + J[29] = t68 * (t65 * t72 * t79 - t78); + J[30] = t15; + J[31] = t75; + J[32] = t8 * t81; + J[33] = -t68 * (t87 + t88 * (t35 * t85 - t44 * t86 + t49 * t87)); + J[34] = -t51 * (t44 * t89 + t86); + J[35] = t51 * (t35 * t89 - t82 + t84); + J[36] = -t10 * t8; + J[37] = t90; + J[38] = t91; + J[39] = t51 + * (t18 * t24 - t92 + + t96 * (-t58 * t93 + t60 * (2 * t45 + t47) - t62 * t95)); + J[40] = t68 * (t43 * t72 * t98 + t97); + J[41] = t68 * (2 * t69 + t71 + t98 * t99); + J[42] = t90; + J[43] = -t73 * t8; + J[44] = t100; + J[45] = -t68 * (t103 * t88 + t36 - 2 * t37); + J[46] = t68 * (-t101 + t103 * t43 * t72); + J[47] = t68 * (t102 + t103 * t99); + J[48] = t91; + J[49] = t100; + J[50] = -t8 * t81; + J[51] = t51 * (t106 * t96 + 2 * t25 + t94); + J[52] = -t51 * (t105 + t107 * t44); + J[53] = t51 * (-t104 + t107 * t35 + t28 * t31); + J[54] = 0; + J[55] = 0; + J[56] = 0; + J[57] = t51 * (t108 - t109 + t110 * t96); + J[58] = -t51 * (t110 * t44 * t56 + t12); + J[59] = t51 * (t110 * t35 * t56 - t14); + J[60] = 0; + J[61] = 0; + J[62] = 0; + J[63] = t51 * (t111 * t49 * t56 - t12); + J[64] = t51 * (-t111 * t44 * t56 + t6); + J[65] = t51 * (t111 * t35 * t56 - t74); + J[66] = 0; + J[67] = 0; + J[68] = 0; + J[69] = t51 * (t114 * t49 * t56 - t14); + J[70] = -t51 * (t115 * t44 + t74); + J[71] = t51 * (t112 - t113 + t115 * t35); +} - // J is (6×12) flattened in column-major order - void point_triangle_tangent_basis_jacobian( - 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 J[72]) - { - const double t0 = t0_y - t1_y; - const double t1 = t0 * t0; - const double t2 = t0_x - t1_x; - const double t3 = t2 * t2; - const double t4 = t0_z - t1_z; - const double t5 = t4 * t4; - const double t6 = t3 + t5; - const double t7 = t1 + t6; - const double t8 = (1.0 / std::sqrt(t7)); - const double t9 = 1.0 / t7; - const double t10 = t3 * t9 - 1; - const double t11 = 1.0 / pow_1_5(t7); - const double t12 = t0 * t2; - const double t13 = t11 * t12; - const double t14 = t2 * t4; - const double t15 = t11 * t14; - const double t16 = -t2; - const double t17 = -t2_z; - const double t18 = t0_z + t17; - const double t19 = -t18; - const double t20 = t16 * t19; - const double t21 = -t2_x; - const double t22 = t0_x + t21; - const double t23 = -t22; - const double t24 = -t4; - const double t25 = t23 * t24; - const double t26 = t20 - t25; - const double t27 = -t26; - const double t28 = -t0; - const double t29 = t19 * t28; - const double t30 = -t2_y; - const double t31 = t0_y + t30; - const double t32 = -t31; - const double t33 = t24 * t32; - const double t34 = t29 - t33; - const double t35 = t16 * t27 - t28 * t34; - const double t36 = t2 * t31; - const double t37 = t0 * t22; - const double t38 = -t37; - const double t39 = t36 + t38; - const double t40 = t0 * t18; - const double t41 = -t31 * t4; - const double t42 = t40 + t41; - const double t43 = t2 * t39 - t4 * t42; - const double t44 = -t43; - const double t45 = t16 * t32; - const double t46 = t23 * t28; - const double t47 = -t46; - const double t48 = t45 + t47; - const double t49 = -t24 * t27 + t28 * t48; - const double t50 = t35 * t35 + t44 * t44 + t49 * t49; - const double t51 = 1.0 / std::sqrt(t50); - const double t52 = t17 + t1_z; - const double t53 = -t52; - const double t54 = t1_y + t30; - const double t55 = t28 * t54; - const double t56 = 1.0 / t50; - const double t57 = -t24 * t53 + t55; - const double t58 = -t49; - const double t59 = -t45 + t46; - const double t60 = t16 * t48 - t24 * t34; - const double t61 = t16 * t53 + t26; - const double t62 = -t35; - const double t63 = - t56 * (-t57 * t58 + t60 * (t16 * t54 + t59) - t61 * t62); - const double t64 = t2 * t54 + t39; - const double t65 = t0 * t42 + t2 * t26; - const double t66 = t0 * t39 + t26 * t4; - const double t67 = t43 * t43 + t65 * t65 + t66 * t66; - const double t68 = 1.0 / std::sqrt(t67); - const double t69 = t18 * t2; - const double t70 = t22 * t4; - const double t71 = -t70; - const double t72 = 1.0 / t67; - const double t73 = t1 * t9 - 1; - const double t74 = t0 * t4; - const double t75 = t11 * t74; - const double t76 = t1_x + t21; - const double t77 = t2 * t76 + t4 * t52; - const double t78 = t0 * t52 + t42; - const double t79 = t35 * t78 + t44 * t77 + t49 * (-t28 * t76 + t59); - const double t80 = t72 * t79; - const double t81 = t5 * t9 - 1; - const double t82 = t16 * t76; - const double t83 = -t54; - const double t84 = t28 * t83; - const double t85 = t82 - t84; - const double t86 = t24 * t83 - t29 + t33; - const double t87 = t4 * t76 - t69 + t70; - const double t88 = t66 * t72; - const double t89 = - t56 * (t58 * (t24 * t76 + t26) - t60 * t86 - t62 * t85); - const double t90 = -t13; - const double t91 = -t15; - const double t92 = t28 * t32; - const double t93 = -t18 * t24 + t92; - const double t94 = -t20; - const double t95 = t16 * t18 + t25 + t94; - const double t96 = t49 * t56; - const double t97 = 2 * t36 + t38; - const double t98 = t35 * t95 + t44 * t97 + t49 * t93; - const double t99 = t65 * t72; - const double t100 = -t75; - const double t101 = t18 * t4 + t2 * t22; - const double t102 = 2 * t40 + t41; - const double t103 = -t101 * t44 - t102 * t35 + t49 * (t22 * t28 + t48); - const double t104 = t16 * t23; - const double t105 = t24 * t31 + t34; - const double t106 = -t105 * t60 + t58 * (-t20 + 2 * t23 * t24) - + t62 * (-t104 + t28 * t31); - const double t107 = t106 * t56; - const double t108 = t24 * t24; - const double t109 = t0 * t28; - const double t110 = - t0 * t16 * t60 - t16 * t24 * t62 + t58 * (t108 - t109); - const double t111 = - -t16 * t28 * t58 + t28 * t4 * t62 + t60 * (t16 * t16 - t24 * t4); - const double t112 = t28 * t28; - const double t113 = t16 * t2; - const double t114 = - t2 * t24 * t58 - t24 * t28 * t60 + t62 * (t112 - t113); - const double t115 = t114 * t56; - J[0] = 0; - J[1] = 0; - J[2] = 0; - J[3] = 0; - J[4] = 0; - J[5] = 0; - J[6] = 0; - J[7] = 0; - J[8] = 0; - J[9] = 0; - J[10] = 0; - J[11] = 0; - J[12] = 0; - J[13] = 0; - J[14] = 0; - J[15] = 0; - J[16] = 0; - J[17] = 0; - J[18] = t10 * t8; - J[19] = t13; - J[20] = t15; - J[21] = t51 * (t24 * t53 + t49 * t63 - t55); - J[22] = t51 * (-t44 * t63 - t64); - J[23] = t68 - * (-t2 * t52 + t65 * t72 * (t35 * t61 - t44 * t64 + t49 * t57) - t69 - - t71); - J[24] = t13; - J[25] = t73 * t8; - J[26] = t75; - J[27] = -t68 * (t0 * t76 - t36 + t37 + t66 * t80); - J[28] = t68 * (t43 * t80 + t77); - J[29] = t68 * (t65 * t72 * t79 - t78); - J[30] = t15; - J[31] = t75; - J[32] = t8 * t81; - J[33] = -t68 * (t87 + t88 * (t35 * t85 - t44 * t86 + t49 * t87)); - J[34] = -t51 * (t44 * t89 + t86); - J[35] = t51 * (t35 * t89 - t82 + t84); - J[36] = -t10 * t8; - J[37] = t90; - J[38] = t91; - J[39] = t51 - * (t18 * t24 - t92 - + t96 * (-t58 * t93 + t60 * (2 * t45 + t47) - t62 * t95)); - J[40] = t68 * (t43 * t72 * t98 + t97); - J[41] = t68 * (2 * t69 + t71 + t98 * t99); - J[42] = t90; - J[43] = -t73 * t8; - J[44] = t100; - J[45] = -t68 * (t103 * t88 + t36 - 2 * t37); - J[46] = t68 * (-t101 + t103 * t43 * t72); - J[47] = t68 * (t102 + t103 * t99); - J[48] = t91; - J[49] = t100; - J[50] = -t8 * t81; - J[51] = t51 * (t106 * t96 + 2 * t25 + t94); - J[52] = -t51 * (t105 + t107 * t44); - J[53] = t51 * (-t104 + t107 * t35 + t28 * t31); - J[54] = 0; - J[55] = 0; - J[56] = 0; - J[57] = t51 * (t108 - t109 + t110 * t96); - J[58] = -t51 * (t110 * t44 * t56 + t12); - J[59] = t51 * (t110 * t35 * t56 - t14); - J[60] = 0; - J[61] = 0; - J[62] = 0; - J[63] = t51 * (t111 * t49 * t56 - t12); - J[64] = t51 * (-t111 * t44 * t56 + t6); - J[65] = t51 * (t111 * t35 * t56 - t74); - J[66] = 0; - J[67] = 0; - J[68] = 0; - J[69] = t51 * (t114 * t49 * t56 - t14); - J[70] = -t51 * (t115 * t44 + t74); - J[71] = t51 * (t112 - t113 + t115 * t35); - } -} // namespace autogen -} // namespace ipc +#define IPC_INSTANTIATE_TANGENT_BASIS_AUTOGEN(T) \ + template void point_point_tangent_basis_2D_jacobian(T, T, T, T, T[8]); \ + template void point_point_tangent_basis_3D_jacobian( \ + T, T, T, T, T, T, T[36]); \ + template void point_edge_tangent_basis_2D_jacobian( \ + T, T, T, T, T, T, T[12]); \ + template void point_edge_tangent_basis_3D_jacobian( \ + T, T, T, T, T, T, T, T, T, T[54]); \ + template void edge_edge_tangent_basis_jacobian( \ + T, T, T, T, T, T, T, T, T, T, T, T, T[72]); \ + template void point_triangle_tangent_basis_jacobian( \ + T, T, T, T, T, T, T, T, T, T, T, T, T[72]) + +IPC_INSTANTIATE_TANGENT_BASIS_AUTOGEN(float); +IPC_INSTANTIATE_TANGENT_BASIS_AUTOGEN(double); + +#undef IPC_INSTANTIATE_TANGENT_BASIS_AUTOGEN + +} // namespace ipc::autogen diff --git a/src/ipc/tangent/tangent_basis.hpp b/src/ipc/tangent/tangent_basis.hpp index 7c0b181c4..8454bb4f4 100644 --- a/src/ipc/tangent/tangent_basis.hpp +++ b/src/ipc/tangent/tangent_basis.hpp @@ -2,8 +2,200 @@ #include +#include + namespace ipc { +namespace detail { + // ======================================================================== + // Point - Point + + /// @brief Compute a basis for the space tangent to the point-point pair. + /// @tparam T The scalar type. + /// @tparam dim The dimension (2 or 3). + /// @param p0 First point + /// @param p1 Second point + /// @return A dim×(dim-1) matrix whose columns are the basis vectors. + template + inline Eigen::Matrix point_point_tangent_basis( + Eigen::ConstRef> p0, + Eigen::ConstRef> p1) + { + static_assert( + dim == 2 || dim == 3, "point-point tangent basis is only 2D or 3D"); + + if constexpr (dim == 2) { + const Eigen::Vector2 p0_to_p1 = (p1 - p0).normalized(); + return Eigen::Vector2(-p0_to_p1.y(), p0_to_p1.x()); + } else { + const Eigen::Vector3 p0_to_p1 = p1 - p0; + + const Eigen::Vector3 cross_x = + Eigen::Vector3::UnitX().cross(p0_to_p1); + const Eigen::Vector3 cross_y = + Eigen::Vector3::UnitY().cross(p0_to_p1); + + Eigen::Matrix basis; + if (cross_x.squaredNorm() > cross_y.squaredNorm()) { + basis.col(0) = cross_x.normalized(); + basis.col(1) = p0_to_p1.cross(cross_x).normalized(); + } else { + basis.col(0) = cross_y.normalized(); + basis.col(1) = p0_to_p1.cross(cross_y).normalized(); + } + + return basis; + } + } + + /// @brief Compute the Jacobian of the tangent basis for the point-point + /// pair. + /// @tparam T The scalar type. + /// @tparam dim The dimension (2 or 3). + /// @param p0 First point + /// @param p1 Second point + /// @return A (dim*(dim-1))×(2*dim) matrix. + template + Eigen::Matrix point_point_tangent_basis_jacobian( + Eigen::ConstRef> p0, + Eigen::ConstRef> p1); + + // ======================================================================== + // Point - Edge + + /// @brief Compute a basis for the space tangent to the point-edge pair. + /// @tparam T The scalar type. + /// @tparam dim The dimension (2 or 3). + /// @param p Point + /// @param e0 First edge point + /// @param e1 Second edge point + /// @return A dim×(dim-1) matrix whose columns are the basis vectors. + template + inline Eigen::Matrix point_edge_tangent_basis( + Eigen::ConstRef> p, + Eigen::ConstRef> e0, + Eigen::ConstRef> e1) + { + static_assert( + dim == 2 || dim == 3, "point-edge tangent basis is only 2D or 3D"); + + if constexpr (dim == 2) { + return (e1 - e0).normalized(); + } else { + const Eigen::Vector3 e = e1 - e0; + + Eigen::Matrix basis; + basis.col(0) = e.normalized(); + basis.col(1) = e.cross(Eigen::Vector3(p - e0)).normalized(); + return basis; + } + } + + /// @brief Compute the Jacobian of the tangent basis for the point-edge pair. + /// @tparam T The scalar type. + /// @tparam dim The dimension (2 or 3). + /// @param p Point + /// @param e0 First edge point + /// @param e1 Second edge point + /// @return A (dim*(dim-1))×(3*dim) matrix. + template + Eigen::Matrix point_edge_tangent_basis_jacobian( + Eigen::ConstRef> p, + Eigen::ConstRef> e0, + Eigen::ConstRef> e1); + + // ======================================================================== + // Edge - Edge + + /// @brief Compute a basis for the space tangent to the edge-edge pair. + /// @tparam T The scalar type. + /// @param ea0 First point of the first edge + /// @param ea1 Second point of the first edge + /// @param eb0 First point of the second edge + /// @param eb1 Second point of the second edge + /// @return A 3x2 matrix whose columns are the basis vectors. + template + inline Eigen::Matrix edge_edge_tangent_basis( + Eigen::ConstRef> ea0, + Eigen::ConstRef> ea1, + Eigen::ConstRef> eb0, + Eigen::ConstRef> eb1) + { + const Eigen::Vector3 ea = ea1 - ea0; // Edge A direction + const Eigen::Vector3 normal = ea.cross(eb1 - eb0); + // The normal will be zero if the edges are parallel (i.e. coplanar). + assert(normal.norm() != 0); + + Eigen::Matrix basis; + // The first basis vector is along edge A. + basis.col(0) = ea.normalized(); + // The second basis vector is orthogonal to the first and the edge-edge + // normal. + basis.col(1) = normal.cross(ea).normalized(); + return basis; + } + + /// @brief Compute the Jacobian of the tangent basis for the edge-edge pair. + /// @tparam T The scalar type. + /// @param ea0 First point of the first edge + /// @param ea1 Second point of the first edge + /// @param eb0 First point of the second edge + /// @param eb1 Second point of the second edge + /// @return A (3*2)x12 matrix whose columns are the basis vectors. + template + Eigen::Matrix edge_edge_tangent_basis_jacobian( + Eigen::ConstRef> ea0, + Eigen::ConstRef> ea1, + Eigen::ConstRef> eb0, + Eigen::ConstRef> eb1); + + // ======================================================================== + // Point - Triangle + + /// @brief Compute a basis for the space tangent to the point-triangle pair. + /// @tparam T The scalar type. + /// @param p Point + /// @param t0 Triangle's first vertex + /// @param t1 Triangle's second vertex + /// @param t2 Triangle's third vertex + /// @return A 3x2 matrix whose columns are the basis vectors. + template + inline Eigen::Matrix point_triangle_tangent_basis( + Eigen::ConstRef> p, + Eigen::ConstRef> t0, + Eigen::ConstRef> t1, + Eigen::ConstRef> t2) + { + const Eigen::Vector3 e0 = t1 - t0; + const Eigen::Vector3 normal = e0.cross(t2 - t0); + assert(normal.norm() != 0); + + Eigen::Matrix basis; + + // The first basis vector is along first edge of the triangle. + basis.col(0) = e0.normalized(); + // The second basis vector is orthogonal to the first and the triangle + // normal. + basis.col(1) = normal.cross(e0).normalized(); + + return basis; + } + + /// @brief Compute the Jacobian of the tangent basis for the point-triangle pair. + /// @tparam T The scalar type. + /// @param p Point + /// @param t0 Triangle's first vertex + /// @param t1 Triangle's second vertex + /// @param t2 Triangle's third vertex + /// @return A (3*2)x12 matrix whose columns are the basis vectors. + template + Eigen::Matrix point_triangle_tangent_basis_jacobian( + Eigen::ConstRef> p, + Eigen::ConstRef> t0, + Eigen::ConstRef> t1, + Eigen::ConstRef> t2); +} // namespace detail + // ============================================================================ // Point - Point @@ -11,15 +203,53 @@ namespace ipc { /// @param p0 First point /// @param p1 Second point /// @return A 3x2 matrix whose columns are the basis vectors. -MatrixMax point_point_tangent_basis( - Eigen::ConstRef p0, Eigen::ConstRef p1); +template +inline auto point_point_tangent_basis( + const Eigen::MatrixBase& p0, + const Eigen::MatrixBase& p1) +{ + using T = typename DerivedP0::Scalar; + assert(p0.size() == p1.size()); + + if constexpr (dim_v == 2) { + return detail::point_point_tangent_basis(p0, p1); + } else if constexpr (dim_v == 3) { + return detail::point_point_tangent_basis(p0, p1); + } else if (p0.size() == 2) { + return MatrixMax( + detail::point_point_tangent_basis(p0, p1)); + } else { + assert(p0.size() == 3); + return MatrixMax( + detail::point_point_tangent_basis(p0, p1)); + } +} /// @brief Compute the Jacobian of the tangent basis for the point-point pair. /// @param p0 First point /// @param p1 Second point /// @return A (3*2)x6 matrix whose columns are the basis vectors. -MatrixMax point_point_tangent_basis_jacobian( - Eigen::ConstRef p0, Eigen::ConstRef p1); +template +inline auto point_point_tangent_basis_jacobian( + const Eigen::MatrixBase& p0, + const Eigen::MatrixBase& p1) +{ + using T = typename DerivedP0::Scalar; + assert(p0.size() == p1.size()); + + if constexpr (dim_v == 2) { + return detail::point_point_tangent_basis_jacobian(p0, p1); + } else if constexpr (dim_v == 3) { + return detail::point_point_tangent_basis_jacobian(p0, p1); + } else if (p0.size() == 2) { + return MatrixMax( + detail::point_point_tangent_basis_jacobian(p0, p1)); + } else { + assert(p0.size() == 3); + return MatrixMax( + detail::point_point_tangent_basis_jacobian(p0, p1)); + } +} // ============================================================================ // Point - Edge @@ -29,20 +259,56 @@ MatrixMax point_point_tangent_basis_jacobian( /// @param e0 First edge point /// @param e1 Second edge point /// @return A 3x2 matrix whose columns are the basis vectors. -MatrixMax point_edge_tangent_basis( - Eigen::ConstRef p, - Eigen::ConstRef e0, - Eigen::ConstRef e1); +template +inline auto point_edge_tangent_basis( + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& e0, + const Eigen::MatrixBase& e1) +{ + using T = typename DerivedP::Scalar; + assert(p.size() == e0.size() && p.size() == e1.size()); + + if constexpr (dim_v == 2) { + return detail::point_edge_tangent_basis(p, e0, e1); + } else if constexpr (dim_v == 3) { + return detail::point_edge_tangent_basis(p, e0, e1); + } else if (p.size() == 2) { + return MatrixMax( + detail::point_edge_tangent_basis(p, e0, e1)); + } else { + assert(p.size() == 3); + return MatrixMax( + detail::point_edge_tangent_basis(p, e0, e1)); + } +} /// @brief Compute the Jacobian of the tangent basis for the point-edge pair. /// @param p Point /// @param e0 First edge point /// @param e1 Second edge point /// @return A (3*2)x9 matrix whose columns are the basis vectors. -MatrixMax point_edge_tangent_basis_jacobian( - Eigen::ConstRef p, - Eigen::ConstRef e0, - Eigen::ConstRef e1); +template +inline auto point_edge_tangent_basis_jacobian( + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& e0, + const Eigen::MatrixBase& e1) +{ + using T = typename DerivedP::Scalar; + assert(p.size() == e0.size() && p.size() == e1.size()); + + if constexpr (dim_v == 2) { + return detail::point_edge_tangent_basis_jacobian(p, e0, e1); + } else if constexpr (dim_v == 3) { + return detail::point_edge_tangent_basis_jacobian(p, e0, e1); + } else if (p.size() == 2) { + return MatrixMax( + detail::point_edge_tangent_basis_jacobian(p, e0, e1)); + } else { + assert(p.size() == 3); + return MatrixMax( + detail::point_edge_tangent_basis_jacobian(p, e0, e1)); + } +} // ============================================================================ // Edge - Edge @@ -53,11 +319,22 @@ MatrixMax point_edge_tangent_basis_jacobian( /// @param eb0 First point of the second edge /// @param eb1 Second point of the second edge /// @return A 3x2 matrix whose columns are the basis vectors. -Eigen::Matrix edge_edge_tangent_basis( - Eigen::ConstRef ea0, - Eigen::ConstRef ea1, - Eigen::ConstRef eb0, - Eigen::ConstRef eb1); +template < + typename DerivedEA0, + typename DerivedEA1, + typename DerivedEB0, + typename DerivedEB1> +inline auto edge_edge_tangent_basis( + const Eigen::MatrixBase& ea0, + const Eigen::MatrixBase& ea1, + const Eigen::MatrixBase& eb0, + const Eigen::MatrixBase& eb1) +{ + using T = typename DerivedEA0::Scalar; + // NOTE: explicit : the detail overload cannot deduce T from an + // arbitrary Eigen expression. + return detail::edge_edge_tangent_basis(ea0, ea1, eb0, eb1); +} /// @brief Compute the Jacobian of the tangent basis for the edge-edge pair. /// @param ea0 First point of the first edge @@ -65,11 +342,20 @@ Eigen::Matrix edge_edge_tangent_basis( /// @param eb0 First point of the second edge /// @param eb1 Second point of the second edge /// @return A (3*2)x12 matrix whose columns are the basis vectors. -Eigen::Matrix edge_edge_tangent_basis_jacobian( - Eigen::ConstRef ea0, - Eigen::ConstRef ea1, - Eigen::ConstRef eb0, - Eigen::ConstRef eb1); +template < + typename DerivedEA0, + typename DerivedEA1, + typename DerivedEB0, + typename DerivedEB1> +inline auto edge_edge_tangent_basis_jacobian( + const Eigen::MatrixBase& ea0, + const Eigen::MatrixBase& ea1, + const Eigen::MatrixBase& eb0, + const Eigen::MatrixBase& eb1) +{ + using T = typename DerivedEA0::Scalar; + return detail::edge_edge_tangent_basis_jacobian(ea0, ea1, eb0, eb1); +} // ============================================================================ // Point - Triangle @@ -88,11 +374,20 @@ Eigen::Matrix edge_edge_tangent_basis_jacobian( /// @param t1 Triangle's second vertex /// @param t2 Triangle's third vertex /// @return A 3x2 matrix whose columns are the basis vectors. -Eigen::Matrix point_triangle_tangent_basis( - Eigen::ConstRef p, - Eigen::ConstRef t0, - Eigen::ConstRef t1, - Eigen::ConstRef t2); +template < + typename DerivedP, + typename DerivedT0, + typename DerivedT1, + typename DerivedT2> +inline auto point_triangle_tangent_basis( + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& t0, + const Eigen::MatrixBase& t1, + const Eigen::MatrixBase& t2) +{ + using T = typename DerivedP::Scalar; + return detail::point_triangle_tangent_basis(p, t0, t1, t2); +} /// @brief Compute the Jacobian of the tangent basis for the point-triangle pair. /// @param p Point @@ -100,83 +395,88 @@ Eigen::Matrix point_triangle_tangent_basis( /// @param t1 Triangle's second vertex /// @param t2 Triangle's third vertex /// @return A (3*2)x12 matrix whose columns are the basis vectors. -Eigen::Matrix point_triangle_tangent_basis_jacobian( - Eigen::ConstRef p, - Eigen::ConstRef t0, - Eigen::ConstRef t1, - Eigen::ConstRef t2); +template < + typename DerivedP, + typename DerivedT0, + typename DerivedT1, + typename DerivedT2> +inline auto point_triangle_tangent_basis_jacobian( + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& t0, + const Eigen::MatrixBase& t1, + const Eigen::MatrixBase& t2) +{ + using T = typename DerivedP::Scalar; + return detail::point_triangle_tangent_basis_jacobian(p, t0, t1, t2); +} // ============================================================================ +// Symbolically generated derivatives namespace autogen { + // J is (2×4) flattened in column-major order + template void point_point_tangent_basis_2D_jacobian( - double p0_x, double p0_y, double p1_x, double p1_y, double J[8]); + T p0_x, T p0_y, T p1_x, T p1_y, T J[8]); // J is (6×6) flattened in column-major order + template void point_point_tangent_basis_3D_jacobian( - double p0_x, - double p0_y, - double p0_z, - double p1_x, - double p1_y, - double p1_z, - double J[36]); + T p0_x, T p0_y, T p0_z, T p1_x, T p1_y, T p1_z, T J[36]); // J is (2×6) flattened in column-major order + template void point_edge_tangent_basis_2D_jacobian( - double p_x, - double p_y, - double e0_x, - double e0_y, - double e1_x, - double e1_y, - double J[12]); + T p_x, T p_y, T e0_x, T e0_y, T e1_x, T e1_y, T J[12]); // J is (6×9) flattened in column-major order + template void point_edge_tangent_basis_3D_jacobian( - 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 J[54]); + T p_x, + T p_y, + T p_z, + T e0_x, + T e0_y, + T e0_z, + T e1_x, + T e1_y, + T e1_z, + T J[54]); // J is (6×12) flattened in column-major order + template void edge_edge_tangent_basis_jacobian( - 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 J[72]); + T ea0_x, + T ea0_y, + T ea0_z, + T ea1_x, + T ea1_y, + T ea1_z, + T eb0_x, + T eb0_y, + T eb0_z, + T eb1_x, + T eb1_y, + T eb1_z, + T J[72]); // J is (6×12) flattened in column-major order + template void point_triangle_tangent_basis_jacobian( - 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 J[72]); + T p_x, + T p_y, + T p_z, + T t0_x, + T t0_y, + T t0_z, + T t1_x, + T t1_y, + T t1_z, + T t2_x, + T t2_y, + T t2_z, + T J[72]); } // namespace autogen } // namespace ipc diff --git a/src/ipc/utils/eigen_ext.hpp b/src/ipc/utils/eigen_ext.hpp index 2dd00f79b..5f6a194b6 100644 --- a/src/ipc/utils/eigen_ext.hpp +++ b/src/ipc/utils/eigen_ext.hpp @@ -4,6 +4,7 @@ #include #include +#include #ifdef EIGEN_DONT_VECTORIZE // NOTE: Avoid error about abs casting double to int. Eigen does this @@ -49,24 +50,42 @@ template using RowVectorMax = Eigen::Matrix; +/// @brief A static size matrix of size of 1×1 +using Vector1f = Eigen::Vector; /// @brief A static size matrix of size of 1×1 using Vector1d = Eigen::Vector; /// @brief A static size matrix of size of 6×1 +using Vector6f = Eigen::Vector; +/// @brief A static size matrix of size of 6×1 using Vector6d = Eigen::Vector; /// @brief A static size matrix of size of 9×1 +using Vector9f = Eigen::Vector; +/// @brief A static size matrix of size of 9×1 using Vector9d = Eigen::Vector; /// @brief A static size matrix of size of 12×1 +using Vector12f = 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 Vector15f = 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 Matrix6f = Eigen::Matrix; /// @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 Matrix9f = 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 Matrix12f = 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 Matrix15f = 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 @@ -82,23 +101,35 @@ template using VectorMax9 = VectorMax; /// @brief A dynamic size matrix with a fixed maximum size of 12×1 template using VectorMax12 = VectorMax; +/// @brief A dynamic size matrix with a fixed maximum size of 2×1 +using VectorMax2f = VectorMax2; /// @brief A dynamic size matrix with a fixed maximum size of 2×1 using VectorMax2d = VectorMax2; /// @brief A dynamic size matrix with a fixed maximum size of 3×1 +using VectorMax3f = VectorMax3; +/// @brief A dynamic size matrix with a fixed maximum size of 3×1 using VectorMax3d = VectorMax3; /// @brief A dynamic size matrix with a fixed maximum size of 3×1 using VectorMax3i = VectorMax3; /// @brief A dynamic size matrix with a fixed maximum size of 4×1 +using VectorMax4f = VectorMax4; +/// @brief A dynamic size matrix with a fixed maximum size of 4×1 using VectorMax4d = VectorMax4; /// @brief A dynamic size matrix with a fixed maximum size of 4×1 using VectorMax4i = VectorMax4; /// @brief A dynamic size matrix with a fixed maximum size of 6×1 +using VectorMax6f = VectorMax6; +/// @brief A dynamic size matrix with a fixed maximum size of 6×1 using VectorMax6d = VectorMax6; /// @brief A dynamic size matrix with a fixed maximum size of 6×1 using VectorMax6b = VectorMax6; /// @brief A dynamic size matrix with a fixed maximum size of 9×1 +using VectorMax9f = VectorMax9; +/// @brief A dynamic size matrix with a fixed maximum size of 9×1 using VectorMax9d = VectorMax9; /// @brief A dynamic size matrix with a fixed maximum size of 12×1 +using VectorMax12f = VectorMax12; +/// @brief A dynamic size matrix with a fixed maximum size of 12×1 using VectorMax12d = VectorMax12; /// @brief A dynamic size matrix with a fixed maximum size of 1×2 @@ -137,15 +168,25 @@ template using MatrixMax9 = MatrixMax; /// @brief A dynamic size matrix with a fixed maximum size of 12×12 template using MatrixMax12 = MatrixMax; -/// @brief A dynamic size matrix with a fixed maximum size of 3×3 +/// @brief A dynamic size matrix with a fixed maximum size of 2×2 +using MatrixMax2f = MatrixMax2; +/// @brief A dynamic size matrix with a fixed maximum size of 2×2 using MatrixMax2d = MatrixMax2; /// @brief A dynamic size matrix with a fixed maximum size of 3×3 +using MatrixMax3f = MatrixMax3; +/// @brief A dynamic size matrix with a fixed maximum size of 3×3 using MatrixMax3d = MatrixMax3; /// @brief A dynamic size matrix with a fixed maximum size of 6×6 +using MatrixMax6f = MatrixMax6; +/// @brief A dynamic size matrix with a fixed maximum size of 6×6 using MatrixMax6d = MatrixMax6; -/// @brief A dynamic size matrix with a fixed maximum size of 12×12 +/// @brief A dynamic size matrix with a fixed maximum size of 9×9 +using MatrixMax9f = MatrixMax9; +/// @brief A dynamic size matrix with a fixed maximum size of 9×9 using MatrixMax9d = MatrixMax9; /// @brief A dynamic size matrix with a fixed maximum size of 12×12 +using MatrixMax12f = MatrixMax12; +/// @brief A dynamic size matrix with a fixed maximum size of 12×12 using MatrixMax12d = MatrixMax12; /// @brief A dynamic size diagonal matrix @@ -182,6 +223,15 @@ using HessianType = std:: /**@}*/ +/// @brief Compile-time dimension of an Eigen vector expression, or +/// `Eigen::Dynamic` if it is not known until run time. +/// +/// Handles row vectors (e.g. `V.row(i)`) as well as column vectors. +template +inline constexpr int dim_v = + Derived::RowsAtCompileTime == 1 ? int(Derived::ColsAtCompileTime) + : int(Derived::RowsAtCompileTime); + /// @brief Matrix projection onto positive definite cone /// @param A Symmetric matrix to project /// @param eps Minimum eigenvalue threshold diff --git a/tests/src/tests/barrier/test_barrier.cpp b/tests/src/tests/barrier/test_barrier.cpp index b95950191..01e155d68 100644 --- a/tests/src/tests/barrier/test_barrier.cpp +++ b/tests/src/tests/barrier/test_barrier.cpp @@ -18,26 +18,27 @@ using namespace ipc; /// @warning This implementation will not work with dmin > 0 -class PhysicalBarrier : public NormalizedClampedLogBarrier { +template +class PhysicalBarrier : public NormalizedClampedLogBarrier { public: PhysicalBarrier(const bool _use_dist_sqr) : use_dist_sqr(_use_dist_sqr) { } double operator()(const double d, const double dhat) const override { return (use_dist_sqr ? sqrt(dhat) : dhat) - * NormalizedClampedLogBarrier::operator()(d, dhat); + * NormalizedClampedLogBarrier::operator()(d, dhat); } double first_derivative(const double d, const double dhat) const override { return (use_dist_sqr ? sqrt(dhat) : dhat) - * NormalizedClampedLogBarrier::first_derivative(d, dhat); + * NormalizedClampedLogBarrier::first_derivative(d, dhat); } double second_derivative(const double d, const double dhat) const override { return (use_dist_sqr ? sqrt(dhat) : dhat) - * NormalizedClampedLogBarrier::second_derivative(d, dhat); + * NormalizedClampedLogBarrier::second_derivative(d, dhat); } private: @@ -411,22 +412,25 @@ TEST_CASE("Barrier derivatives", "[barrier]") std::unique_ptr barrier; SECTION("Original") { - barrier = std::make_unique(); + barrier = std::make_unique>(); } SECTION("Normalized") { - barrier = std::make_unique(); + barrier = std::make_unique>(); } SECTION("Physical") { - barrier = std::make_unique(use_dist_sqr); + barrier = std::make_unique>(use_dist_sqr); } SECTION("ClampedLogSq") { - barrier = std::make_unique(); + barrier = std::make_unique>(); + } + SECTION("Cubic") { barrier = std::make_unique>(); } + SECTION("TwoStage") + { + barrier = std::make_unique>(); } - SECTION("Cubic") { barrier = std::make_unique(); } - SECTION("TwoStage") { barrier = std::make_unique(); } if (use_dist_sqr) { d_vec *= d; diff --git a/tests/src/tests/benchmark_eigen.cpp b/tests/src/tests/benchmark_eigen.cpp index ac1562c99..2a18c0a12 100644 --- a/tests/src/tests/benchmark_eigen.cpp +++ b/tests/src/tests/benchmark_eigen.cpp @@ -1,102 +1,258 @@ #include #include -#include +#include +#include #include +#include +#include +#include #include +#include +#include +#include +#include +#include +#include #include -using namespace ipc; - -TEST_CASE("Template dynamic vs static", "[!benchmark][eigen]") -{ - const Eigen::MatrixXd V = Eigen::MatrixXd::Random(100, 3); +#include - int vi = 0, t0i = 50, t1i = 75, t2i = 99; +// ============================================================================= +// How the distance functions pass and bind their arguments. +// +// Every benchmark here compares the real library function ("Lib") against a +// same-TU reference implementation with the same body but a chosen parameter +// type. The reference rows double as a noise floor: they are unaffected by +// changes under src/, so if a "Lib" row moves and its control moves with it, +// the machine drifted, not the code. +// +// These are hidden ([!benchmark]); run them by name, e.g. +// ./ipc_toolkit_tests "Parameter type (PE)" +// +// -- Method ------------------------------------------------------------------ +// +// Hand-written stand-ins overstated the win three separate times during the +// v2.0.0 templating work. Only two methods proved trustworthy: +// * stash-compare: build the same benchmark against src/ with and without +// the change, and +// * interleaved A/B: alternate the two binaries A/B/A/B... and take medians. +// Always report the control rows alongside the result. On a loaded machine the +// controls routinely drift 3-6%, which is larger than most effects here. +// +// Three traps this file exists to keep measured: +// +// 1. LITERAL DISTANCE TYPES. Passing EdgeEdgeDistanceType::EA_EB as a +// literal lets the compiler fold the switch to one case after inlining, +// which flattered the inlined version by 2-4x. The "runtime dtype" rows +// read the type from a heap array, which is what a real caller does. +// Measure those. +// 2. LICM. Catch2 has no DoNotOptimize, so the indices come from a +// heap-allocated array of prime length (IDX_M): i % IDX_M has no +// power-of-2 period, and the heap pointer stops the compiler tracing +// provenance back to V. +// +// -- What was learned -------------------------------------------------------- +// +// Final measured state of the v2.0.0 distance work, on an Apple M-series, -O3, +// call site V.row(i) of a column-major MatrixXd (the dominant in-tree +// pattern): +// +// before after +// point_line_distance 8.2 ns 2.3 ns 3.5x +// point_edge_distance, AUTO 18.1 ns 5.2 ns 3.5x +// point_triangle_distance, AUTO 93.3 ns 12.8 ns 7.3x +// edge_edge_distance, runtime dtype 6.4 ns 6.2 ns 1.0x +// +// 1. The cost was the DYNAMIC SIZE, not the copy. Binding a Vector3d lvalue +// by Ref or by copy is a wash (2.02 vs 2.18 ns), but a VectorMax3d +// parameter costs 2.4x a Vector3d one. Hence the dim-templated kernels. +// 2. Never materialize into a dynamically sized intermediate. A VectorMax3d +// local costs 5.9 ns (10.9 ns from a VectorMax3d caller). Branch on +// size() FIRST -- which does not evaluate a lazy expression -- then copy +// into a fixed-size local. +// 3. Moving the cold `throw` bodies out of line (detail::throw_* in +// distance_type.hpp) was the single biggest lever, worth up to 2x by +// itself. Constructing a std::invalid_argument inline blows the inliner's +// budget and pushes the whole function out of line. +// 4. DO NOT INLINE A WIDE DISPATCH. Moving edge_edge_distance and +// point_triangle_distance into their headers was tried and reverted: +// their switches have 9 and 7 cases, and inlining them at every call site +// loses whenever the distance type is a runtime value (6.4 -> 7.1 ns, and +// AUTO 9.7 -> 10.5 ns). point_edge_distance has 3 cases and wins. +// 5. Eigen::ConstRef is the right default for a kernel that reads its +// arguments more than a few times -- it guarantees a single evaluation +// and measured 1.25-1.38x better than `const&` on point_edge's AUTO path. +// But it is not free: binding an expression to a Ref materializes it into +// the Ref's buffer. For line_line's gradient/Hessian, which read three +// coefficients per argument and hand them straight to generated code, +// that copy is pure cost -- Eigen::MatrixBase measured 1.16x +// faster on an expression argument and no worse anywhere else. +// 6. point_triangle_distance_type formerly cost ~87 ns, 27x the distance it +// selects for, because it ran three 2x2 LDLT solves. It is now a closed +// form (the Gram matrix is diagonal, since each edge is perpendicular to +// the normal). The error study is summarized in the v2.0.0 release notes. +// ============================================================================= - BENCHMARK("Static") - { - Eigen::Vector3d p = V.row(vi); - Eigen::Vector3d t0 = V.row(t0i); - Eigen::Vector3d t1 = V.row(t1i); - Eigen::Vector3d t2 = V.row(t2i); +using namespace ipc; - return point_triangle_distance_hessian(p, t0, t1, t2); - }; +namespace { - BENCHMARK("Dynamic") - { - return point_triangle_distance_hessian( - V.row(vi), V.row(t0i), V.row(t1i), V.row(t2i)); - }; +/// Random indices on the heap, to defeat LICM. Prime length, see trap 2 above. +constexpr int IDX_M = 997; +Eigen::ArrayXi random_indices(const int n) +{ + return Eigen::ArrayXi::Random(IDX_M).abs().unaryExpr( + [n](int x) { return x % n; }); } -// ============================================================================= +// --- point-point ------------------------------------------------------------- -double point_point_distance_ref( - const Eigen::Ref& p0, - const Eigen::Ref& p1) +double pp_ref_3d( + Eigen::ConstRef p0, Eigen::ConstRef p1) { return (p1 - p0).squaredNorm(); } -double -point_point_distance_copy(const Eigen::Vector3d& p0, const Eigen::Vector3d& p1) +double pp_copy_3d(const Eigen::Vector3d& p0, const Eigen::Vector3d& p1) { return (p1 - p0).squaredNorm(); } double -point_point_distance_copy_Nd(const VectorMax3d& p0, const VectorMax3d& p1) +pp_ref_maxN(Eigen::ConstRef p0, Eigen::ConstRef p1) { return (p1 - p0).squaredNorm(); } -TEST_CASE("Templated vs Eigen::Ref (PP)", "[!benchmark][eigen][pp][ref]") +// --- point-line -------------------------------------------------------------- + +/// @brief The shared fixed-size body. Always inlined, so any difference +/// measured below comes from the parameter type, not from this. +template +inline double pl_body( + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& e0, + const Eigen::MatrixBase& e1) { - Eigen::MatrixXd V = Eigen::MatrixXd::Random(100, 3); + if constexpr (N == 2) { + const Eigen::Vector2d e = e1 - e0; + const double numerator = + e[1] * p[0] - e[0] * p[1] + e1[0] * e0[1] - e1[1] * e0[0]; + return numerator * numerator / e.squaredNorm(); + } else { + const Eigen::Vector3d p_to_e0 = e0 - p, p_to_e1 = e1 - p; + return p_to_e0.cross(p_to_e1).squaredNorm() / (e1 - e0).squaredNorm(); + } +} - const int p0i = 0, p1i = V.rows() - 1; +/// The pre-v2.0.0 signature for the dimension-agnostic functions: dynamic Ref. +double pl_ref_maxN( + Eigen::ConstRef p, + Eigen::ConstRef e0, + Eigen::ConstRef e1) +{ + if (p.size() == 2) { + return pl_body<2>(p.head<2>(), e0.head<2>(), e1.head<2>()); + } + return pl_body<3>(p.head<3>(), e0.head<3>(), e1.head<3>()); +} - BENCHMARK("Ref") - { - return point_point_distance_ref(V.row(p0i), V.row(p1i)); - }; +/// The best this body can do: a statically sized Ref. The bar for "Lib". +double pl_ref_3d( + Eigen::ConstRef p, + Eigen::ConstRef e0, + Eigen::ConstRef e1) +{ + return pl_body<3>(p, e0, e1); +} - BENCHMARK("Ref Copied 3D") - { - const Eigen::Vector3d p0 = V.row(p0i); - const Eigen::Vector3d p1 = V.row(p1i); - return point_point_distance_ref(p0, p1); - }; +// --- point-edge: the same question, with the distance-type dispatch on top --- - BENCHMARK("Ref Copied ND") - { - const VectorMax3d p0_ND = V.row(p0i); - const VectorMax3d p1_ND = V.row(p1i); - return point_point_distance_ref(p0_ND, p1_ND); - }; +/// @brief Fixed-size stand-in for point_edge_distance_type. Matches the +/// library apart from the degenerate-edge logger call, which never fires here. +template +inline PointEdgeDistanceType pe_type_body( + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& e0, + const Eigen::MatrixBase& e1) +{ + const Eigen::Vector e = e1 - e0; + const double e_length_sqr = e.squaredNorm(); + if (e_length_sqr == 0) { + return PointEdgeDistanceType::P_E0; + } + const double ratio = e.dot(p - e0) / e_length_sqr; + if (ratio < 0) { + return PointEdgeDistanceType::P_E0; + } else if (ratio > 1) { + return PointEdgeDistanceType::P_E1; + } + return PointEdgeDistanceType::P_E; +} - BENCHMARK("Ref Nd") - { - return point_point_distance(V.row(p0i), V.row(p1i)); - }; +/// @brief The shared fixed-size body, as pl_body but with the dtype dispatch. +template +inline double pe_body( + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& e0, + const Eigen::MatrixBase& e1, + PointEdgeDistanceType dtype) +{ + if (dtype == PointEdgeDistanceType::AUTO) { + dtype = pe_type_body(p, e0, e1); + } + switch (dtype) { + case PointEdgeDistanceType::P_E0: + return (p - e0).squaredNorm(); + case PointEdgeDistanceType::P_E1: + return (p - e1).squaredNorm(); + default: + return pl_body(p, e0, e1); + } +} - BENCHMARK("Copy") - { - return point_point_distance_copy(V.row(p0i), V.row(p1i)); - }; +/// The pre-v2.0.0 signature: a dynamic-size Ref. +double pe_ref_maxN( + Eigen::ConstRef p, + Eigen::ConstRef e0, + Eigen::ConstRef e1, + PointEdgeDistanceType dtype) +{ + if (p.size() == 2) { + return pe_body<2>(p.head<2>(), e0.head<2>(), e1.head<2>(), dtype); + } + return pe_body<3>(p.head<3>(), e0.head<3>(), e1.head<3>(), dtype); +} - BENCHMARK("Copy Nd") - { - return point_point_distance_copy_Nd(V.row(p0i), V.row(p1i)); - }; +/// The bar for "Lib": a statically sized Ref. +double pe_ref_3d( + Eigen::ConstRef p, + Eigen::ConstRef e0, + Eigen::ConstRef e1, + PointEdgeDistanceType dtype) +{ + return pe_body<3>(p, e0, e1, dtype); } -// ============================================================================= +// --- line-line --------------------------------------------------------------- + +double ll_ref_3d( + Eigen::ConstRef ea0, + Eigen::ConstRef ea1, + Eigen::ConstRef eb0, + Eigen::ConstRef eb1) +{ + const Eigen::Vector3d normal = (ea1 - ea0).cross(eb1 - eb0); + const double line_to_line = (eb0 - ea0).dot(normal); + return line_to_line * line_to_line / normal.squaredNorm(); +} -double line_line_distance_ref( +/// A Ref that can bind a row of a column-major matrix WITHOUT copying, by +/// admitting a runtime inner stride. The plain Ref above +/// cannot, so it materializes the row into its own buffer first. +double ll_row_ref_3d( const Eigen::Ref>& ea0, const Eigen::Ref>& ea1, const Eigen::Ref>& eb0, @@ -107,7 +263,7 @@ double line_line_distance_ref( return line_to_line * line_to_line / normal.squaredNorm(); } -double line_line_distance_copy( +double ll_copy_3d( const Eigen::Vector3d& ea0, const Eigen::Vector3d& ea1, const Eigen::Vector3d& eb0, @@ -118,63 +274,656 @@ double line_line_distance_copy( return line_to_line * line_to_line / normal.squaredNorm(); } -TEST_CASE("Templated vs Eigen::Ref (LL)", "[!benchmark][eigen][ll][ref]") +Eigen::Vector ll_grad_ref_3d( + Eigen::ConstRef ea0, + Eigen::ConstRef ea1, + Eigen::ConstRef eb0, + Eigen::ConstRef eb1) { - const Eigen::MatrixXd V = Eigen::MatrixXd::Random(100, 3); - - int ea0i = 0, ea1i = 50, eb0i = 75, eb1i = 99; - - const auto ea0 = V.row(ea0i); - const auto ea1 = V.row(ea1i); - const auto eb0 = V.row(eb0i); - const auto eb1 = V.row(eb1i); - - BENCHMARK("Templated") { return line_line_distance(ea0, ea1, eb0, eb1); }; - - BENCHMARK("Ref") { return line_line_distance_ref(ea0, ea1, eb0, eb1); }; - - BENCHMARK("Copy") { return line_line_distance_copy(ea0, ea1, eb0, eb1); }; + Eigen::Vector grad; + autogen::line_line_distance_gradient( + ea0[0], ea0[1], ea0[2], ea1[0], ea1[1], ea1[2], eb0[0], eb0[1], eb0[2], + eb1[0], eb1[1], eb1[2], grad.data()); + return grad; } -// ============================================================================= +Eigen::Matrix ll_hess_ref_3d( + Eigen::ConstRef ea0, + Eigen::ConstRef ea1, + Eigen::ConstRef eb0, + Eigen::ConstRef eb1) +{ + Eigen::Matrix hess; + autogen::line_line_distance_hessian( + ea0[0], ea0[1], ea0[2], ea1[0], ea1[1], ea1[2], eb0[0], eb0[1], eb0[2], + eb1[0], eb1[1], eb1[2], hess.data()); + return hess; +} +/// Writes through an out-parameter instead of returning. Paired with the +/// returning form above to check that NRVO makes the two equivalent. template -void line_line_distance_hessian( - const Eigen::Ref& ea0, - const Eigen::Ref& ea1, - const Eigen::Ref& eb0, - const Eigen::Ref& eb1, +void ll_hess_out_param( + Eigen::ConstRef ea0, + Eigen::ConstRef ea1, + Eigen::ConstRef eb0, + Eigen::ConstRef eb1, Eigen::PlainObjectBase& hess) { - hess.resize( - ea0.size() + ea1.size() + eb0.size() + eb1.size(), - ea0.size() + ea1.size() + eb0.size() + eb1.size()); + hess.resize(12, 12); autogen::line_line_distance_hessian( ea0[0], ea0[1], ea0[2], ea1[0], ea1[1], ea1[2], eb0[0], eb0[1], eb0[2], eb1[0], eb1[1], eb1[2], hess.data()); } -TEST_CASE("Return type", "[!benchmark][eigen][return]") +// --- edge-edge --------------------------------------------------------------- + +/// A same-TU, statically sized reference implementation: the best this body +/// can do, against which the library function is judged. +double ee_ref_3d( + Eigen::ConstRef ea0, + Eigen::ConstRef ea1, + Eigen::ConstRef eb0, + Eigen::ConstRef eb1, + EdgeEdgeDistanceType dtype) { - const Eigen::MatrixXd V = Eigen::MatrixXd::Random(100, 3); + if (dtype == EdgeEdgeDistanceType::AUTO) { + dtype = edge_edge_distance_type(ea0, ea1, eb0, eb1); + } + switch (dtype) { + case EdgeEdgeDistanceType::EA0_EB0: + return (ea0 - eb0).squaredNorm(); + case EdgeEdgeDistanceType::EA0_EB1: + return (ea0 - eb1).squaredNorm(); + case EdgeEdgeDistanceType::EA1_EB0: + return (ea1 - eb0).squaredNorm(); + case EdgeEdgeDistanceType::EA1_EB1: + return (ea1 - eb1).squaredNorm(); + case EdgeEdgeDistanceType::EA_EB0: + return pl_body<3>(eb0, ea0, ea1); + case EdgeEdgeDistanceType::EA_EB1: + return pl_body<3>(eb1, ea0, ea1); + case EdgeEdgeDistanceType::EA0_EB: + return pl_body<3>(ea0, eb0, eb1); + case EdgeEdgeDistanceType::EA1_EB: + return pl_body<3>(ea1, eb0, eb1); + default: { + const Eigen::Vector3d n = (ea1 - ea0).cross(eb1 - eb0); + const double ltl = (eb0 - ea0).dot(n); + return ltl * ltl / n.squaredNorm(); + } + } +} - int ea0i = 0, ea1i = 50, eb0i = 75, eb1i = 99; +} // namespace + +// Rows of a column-major matrix: the dominant in-tree call site. The _D +// variants take a trailing distance type. (No __VA_OPT__ -- this is C++17.) +#define ROWS3(f) \ + f(V.row(idx[i % IDX_M]), V.row(idx[(i + 1) % IDX_M]), \ + V.row(idx[(i + 2) % IDX_M])) +#define ROWS3_D(f, dt) \ + f(V.row(idx[i % IDX_M]), V.row(idx[(i + 1) % IDX_M]), \ + V.row(idx[(i + 2) % IDX_M]), dt) +#define ROWS4(f) \ + f(V.row(idx[i % IDX_M]), V.row(idx[(i + 1) % IDX_M]), \ + V.row(idx[(i + 2) % IDX_M]), V.row(idx[(i + 3) % IDX_M])) +#define ROWS4_D(f, dt) \ + f(V.row(idx[i % IDX_M]), V.row(idx[(i + 1) % IDX_M]), \ + V.row(idx[(i + 2) % IDX_M]), V.row(idx[(i + 3) % IDX_M]), dt) + +// The same call site, but from storage whose type already knows the dimension. +#define AT3(f, src) \ + f(src[idx[i % IDX_M]], src[idx[(i + 1) % IDX_M]], src[idx[(i + 2) % IDX_M]]) +#define AT3_D(f, src, dt) \ + f(src[idx[i % IDX_M]], src[idx[(i + 1) % IDX_M]], \ + src[idx[(i + 2) % IDX_M]], dt) +#define AT4(f, src) \ + f(src[idx[i % IDX_M]], src[idx[(i + 1) % IDX_M]], \ + src[idx[(i + 2) % IDX_M]], src[idx[(i + 3) % IDX_M]]) +#define AT4_D(f, src, dt) \ + f(src[idx[i % IDX_M]], src[idx[(i + 1) % IDX_M]], \ + src[idx[(i + 2) % IDX_M]], src[idx[(i + 3) % IDX_M]], dt) + +TEST_CASE("Parameter type (PP)", "[!benchmark][eigen][pp][param]") +{ + constexpr int N = 100; + const Eigen::MatrixXd V = Eigen::MatrixXd::Random(N, 3); + const Eigen::ArrayXi idx = random_indices(N); - const Eigen::Vector3d ea0 = V.row(ea0i); - const Eigen::Vector3d ea1 = V.row(ea1i); - const Eigen::Vector3d eb0 = V.row(eb0i); - const Eigen::Vector3d eb1 = V.row(eb1i); +#define PP_ROWS(f) f(V.row(idx[i % IDX_M]), V.row(idx[(i + 1) % IDX_M])) - BENCHMARK("Explicit") + BENCHMARK("rows: Lib", i) { return PP_ROWS(point_point_distance); }; + BENCHMARK("rows: Ref", i) { return PP_ROWS(pp_ref_3d); }; + BENCHMARK("rows: Ref", i) { return PP_ROWS(pp_ref_maxN); }; + BENCHMARK("rows: by-value Vector3d", i) { return PP_ROWS(pp_copy_3d); }; + +#undef PP_ROWS +} + +TEST_CASE("Parameter type (PL)", "[!benchmark][eigen][pl][param]") +{ + constexpr int N = 100; + const Eigen::MatrixXd V = Eigen::MatrixXd::Random(N, 3); + const Eigen::ArrayXi idx = random_indices(N); + + // Call sites that already know the dimension statically, and ones that + // reach the front end's runtime branch. + std::vector P3(N); + std::vector PN(N); + for (int k = 0; k < N; ++k) { + P3[k] = V.row(k); + PN[k] = V.row(k); + } + + BENCHMARK("rows: Lib", i) { return ROWS3(point_line_distance); }; + BENCHMARK("rows: Ref", i) { return ROWS3(pl_ref_3d); }; + BENCHMARK("rows: Ref", i) { return ROWS3(pl_ref_maxN); }; + + BENCHMARK("Vector3d: Lib", i) { return AT3(point_line_distance, P3); }; + BENCHMARK("Vector3d: Ref", i) { return AT3(pl_ref_3d, P3); }; + BENCHMARK("VectorMax3d: Lib", i) { return AT3(point_line_distance, PN); }; + BENCHMARK("VectorMax3d: Ref", i) { - const Matrix12d hess = line_line_distance_hessian(ea0, ea1, eb0, eb1); - return hess; + return AT3(pl_ref_maxN, PN); + }; +} + +TEST_CASE("Parameter type (PE)", "[!benchmark][eigen][pe][param]") +{ + constexpr int N = 100; + const Eigen::MatrixXd V = Eigen::MatrixXd::Random(N, 3); + const Eigen::ArrayXi idx = random_indices(N); + + std::vector P3(N); + std::vector PN(N); + for (int k = 0; k < N; ++k) { + P3[k] = V.row(k); + PN[k] = V.row(k); + } + + // A runtime-varying dtype: a real caller gets this from a stencil, so the + // switch cannot fold to one case the way a literal lets it. See trap 1. + std::vector dts(IDX_M); + for (int k = 0; k < IDX_M; ++k) { + dts[k] = static_cast(k % 3); + } + + // AUTO: the distance type is resolved inside, as in normal use. This pays + // the parameter-type penalty twice -- once in point_edge_distance_type and + // once in the distance itself -- so it is the strongest signal here. + BENCHMARK("AUTO rows: Lib", i) + { + return ROWS3_D(point_edge_distance, PointEdgeDistanceType::AUTO); + }; + BENCHMARK("AUTO rows: Ref", i) + { + return ROWS3_D(pe_ref_3d, PointEdgeDistanceType::AUTO); + }; + BENCHMARK("AUTO rows: Ref", i) + { + return ROWS3_D(pe_ref_maxN, PointEdgeDistanceType::AUTO); + }; + + BENCHMARK("runtime dtype rows: Lib", i) + { + return ROWS3_D(point_edge_distance, dts[i % IDX_M]); + }; + BENCHMARK("runtime dtype rows: Ref", i) + { + return ROWS3_D(pe_ref_3d, dts[i % IDX_M]); + }; + + BENCHMARK("AUTO Vector3d: Lib", i) + { + return AT3_D(point_edge_distance, P3, PointEdgeDistanceType::AUTO); + }; + BENCHMARK("AUTO VectorMax3d: Lib", i) + { + return AT3_D(point_edge_distance, PN, PointEdgeDistanceType::AUTO); + }; + BENCHMARK("AUTO VectorMax3d: Ref", i) + { + return AT3_D(pe_ref_maxN, PN, PointEdgeDistanceType::AUTO); + }; + + // An argument the front end must not evaluate more than once. + BENCHMARK("AUTO lazy expr: Lib", i) + { + return point_edge_distance( + V.row(idx[i % IDX_M]) - V.row(idx[(i + 3) % IDX_M]), + V.row(idx[(i + 1) % IDX_M]), V.row(idx[(i + 2) % IDX_M]), + PointEdgeDistanceType::AUTO); + }; +} + +TEST_CASE("Parameter type (LL)", "[!benchmark][eigen][ll][param]") +{ + constexpr int N = 100; + const Eigen::MatrixXd V = Eigen::MatrixXd::Random(N, 3); + const Eigen::Matrix VR = V; + const Eigen::ArrayXi idx = random_indices(N); + + std::vector P3(N); + for (int k = 0; k < N; ++k) { + P3[k] = V.row(k); + } + + BENCHMARK("rows: Lib", i) { return ROWS4(line_line_distance); }; + BENCHMARK("rows: Ref", i) { return ROWS4(ll_ref_3d); }; + BENCHMARK("rows: Ref", i) + { + return ROWS4(ll_row_ref_3d); + }; + BENCHMARK("rows: by-value Vector3d", i) { return ROWS4(ll_copy_3d); }; + + // A row-major matrix makes each row contiguous, so Ref can + // bind it without materializing. + BENCHMARK("row-major rows: Lib", i) + { + return line_line_distance( + VR.row(idx[i % IDX_M]), VR.row(idx[(i + 1) % IDX_M]), + VR.row(idx[(i + 2) % IDX_M]), VR.row(idx[(i + 3) % IDX_M])); + }; + + BENCHMARK("Vector3d: Lib", i) { return AT4(line_line_distance, P3); }; + BENCHMARK("Vector3d: Ref", i) { return AT4(ll_ref_3d, P3); }; +} + +TEST_CASE("Parameter type (EE)", "[!benchmark][eigen][ee][param]") +{ + constexpr int N = 100; + const Eigen::MatrixXd V = Eigen::MatrixXd::Random(N, 3); + const Eigen::ArrayXi idx = random_indices(N); + + std::vector P3(N); + for (int k = 0; k < N; ++k) { + P3[k] = V.row(k); + } + + // See trap 1: a literal dtype folds the switch, a runtime one does not. + std::vector dts(IDX_M); + for (int k = 0; k < IDX_M; ++k) { + dts[k] = static_cast(k % 9); + } + + BENCHMARK("AUTO rows: Lib", i) + { + return ROWS4_D(edge_edge_distance, EdgeEdgeDistanceType::AUTO); + }; + BENCHMARK("AUTO rows: Ref", i) + { + return ROWS4_D(ee_ref_3d, EdgeEdgeDistanceType::AUTO); + }; + BENCHMARK("AUTO Vector3d: Lib", i) + { + return AT4_D(edge_edge_distance, P3, EdgeEdgeDistanceType::AUTO); + }; + BENCHMARK("AUTO Vector3d: Ref", i) + { + return AT4_D(ee_ref_3d, P3, EdgeEdgeDistanceType::AUTO); + }; + + BENCHMARK("runtime dtype rows: Lib", i) + { + return ROWS4_D(edge_edge_distance, dts[i % IDX_M]); + }; + BENCHMARK("runtime dtype rows: Ref", i) + { + return ROWS4_D(ee_ref_3d, dts[i % IDX_M]); + }; + + // Explicit dtypes isolate the parameter/inlining question from the + // distance-type resolution. EA_EB is the interior-interior case. + BENCHMARK("EA_EB rows: Lib", i) + { + return ROWS4_D(edge_edge_distance, EdgeEdgeDistanceType::EA_EB); + }; + BENCHMARK("EA_EB rows: Ref", i) + { + return ROWS4_D(ee_ref_3d, EdgeEdgeDistanceType::EA_EB); + }; + BENCHMARK("EA0_EB0 rows: Lib", i) + { + return ROWS4_D(edge_edge_distance, EdgeEdgeDistanceType::EA0_EB0); + }; + BENCHMARK("EA_EB0 rows: Lib", i) + { + return ROWS4_D(edge_edge_distance, EdgeEdgeDistanceType::EA_EB0); + }; +} + +TEST_CASE("Parameter type (PT)", "[!benchmark][eigen][pt][param]") +{ + constexpr int N = 100; + const Eigen::MatrixXd V = Eigen::MatrixXd::Random(N, 3); + const Eigen::ArrayXi idx = random_indices(N); + + std::vector P3(N); + for (int k = 0; k < N; ++k) { + P3[k] = V.row(k); + } + + BENCHMARK("AUTO rows: Lib", i) + { + return ROWS4_D( + point_triangle_distance, PointTriangleDistanceType::AUTO); + }; + BENCHMARK("P_T rows: Lib", i) + { + return ROWS4_D(point_triangle_distance, PointTriangleDistanceType::P_T); + }; + BENCHMARK("P_E0 rows: Lib", i) + { + return ROWS4_D( + point_triangle_distance, PointTriangleDistanceType::P_E0); + }; + BENCHMARK("AUTO Vector3d: Lib", i) + { + return AT4_D( + point_triangle_distance, P3, PointTriangleDistanceType::AUTO); + }; + + // Static vs dynamic argument type on the Hessian, which is the heaviest + // consumer of the fixed-size return types. + BENCHMARK("hessian rows: Lib", i) + { + return ROWS4(point_triangle_distance_hessian); + }; + BENCHMARK("hessian Vector3d: Lib", i) + { + return AT4(point_triangle_distance_hessian, P3); + }; +} + +TEST_CASE("Parameter type (LL derivatives)", "[!benchmark][eigen][ll][deriv]") +{ + constexpr int N = 100; + const Eigen::MatrixXd V = Eigen::MatrixXd::Random(N, 3); + const Eigen::ArrayXi idx = random_indices(N); + + std::vector P3(N); + for (int k = 0; k < N; ++k) { + P3[k] = V.row(k); + } + + BENCHMARK("grad rows: Lib", i) + { + return ROWS4(line_line_distance_gradient); + }; + BENCHMARK("grad rows: Ref", i) { return ROWS4(ll_grad_ref_3d); }; + BENCHMARK("grad Vector3d: Lib", i) + { + return AT4(line_line_distance_gradient, P3); + }; + BENCHMARK("grad Vector3d: Ref", i) + { + return AT4(ll_grad_ref_3d, P3); }; - BENCHMARK("Implicit") + BENCHMARK("hess rows: Lib", i) + { + return ROWS4(line_line_distance_hessian); + }; + BENCHMARK("hess rows: Ref", i) { return ROWS4(ll_hess_ref_3d); }; + BENCHMARK("hess Vector3d: Lib", i) + { + return AT4(line_line_distance_hessian, P3); + }; + BENCHMARK("hess Vector3d: Ref", i) + { + return AT4(ll_hess_ref_3d, P3); + }; + BENCHMARK("hess Vector3d: out-parameter", i) { Matrix12d hess; - line_line_distance_hessian(ea0, ea1, eb0, eb1, hess); + ll_hess_out_param( + P3[idx[i % IDX_M]], P3[idx[(i + 1) % IDX_M]], + P3[idx[(i + 2) % IDX_M]], P3[idx[(i + 3) % IDX_M]], hess); return hess; }; + + // The real in-library caller: edge_edge_distance_{gradient,hessian} routes + // the EA_EB (interior-interior) case straight to line_line. + BENCHMARK("EE grad EA_EB rows: Lib", i) + { + return ROWS4_D( + edge_edge_distance_gradient, EdgeEdgeDistanceType::EA_EB); + }; + BENCHMARK("EE hess EA_EB rows: Lib", i) + { + return ROWS4_D(edge_edge_distance_hessian, EdgeEdgeDistanceType::EA_EB); + }; + + // These read three coefficients per argument, so a non-trivial expression + // is re-evaluated three times rather than materialized once. Measured: a + // cheap elementwise expression costs nothing, and even a 3x3 product is + // within noise of materializing first -- which is why these two take + // Eigen::MatrixBase rather than Eigen::ConstRef. See finding 5. + const Eigen::Matrix3d M = Eigen::Matrix3d::Random(); + + BENCHMARK("grad expr diff: Lib", i) + { + return line_line_distance_gradient( + V.row(idx[i % IDX_M]) - V.row(idx[(i + 4) % IDX_M]), + V.row(idx[(i + 1) % IDX_M]), V.row(idx[(i + 2) % IDX_M]), + V.row(idx[(i + 3) % IDX_M])); + }; + BENCHMARK("grad expr diff: materialized", i) + { + const Eigen::Vector3d d = + V.row(idx[i % IDX_M]) - V.row(idx[(i + 4) % IDX_M]); + return line_line_distance_gradient( + d, V.row(idx[(i + 1) % IDX_M]), V.row(idx[(i + 2) % IDX_M]), + V.row(idx[(i + 3) % IDX_M])); + }; + BENCHMARK("grad expr product: Lib", i) + { + return line_line_distance_gradient( + M * P3[idx[i % IDX_M]], P3[idx[(i + 1) % IDX_M]], + P3[idx[(i + 2) % IDX_M]], P3[idx[(i + 3) % IDX_M]]); + }; + BENCHMARK("grad expr product: materialized", i) + { + const Eigen::Vector3d d = M * P3[idx[i % IDX_M]]; + return line_line_distance_gradient( + d, P3[idx[(i + 1) % IDX_M]], P3[idx[(i + 2) % IDX_M]], + P3[idx[(i + 3) % IDX_M]]); + }; +} + +TEST_CASE("Float vs double", "[!benchmark][eigen][float]") +{ + constexpr int N = 100; + const Eigen::MatrixXf Vf = Eigen::MatrixXf::Random(N, 3); + const Eigen::MatrixXd Vd = Vf.cast(); + const Eigen::ArrayXi idx = random_indices(N); + +#define EE_ROWS_OF(Mat) \ + Mat.row(idx[i % IDX_M]), Mat.row(idx[(i + 1) % IDX_M]), \ + Mat.row(idx[(i + 2) % IDX_M]), Mat.row(idx[(i + 3) % IDX_M]) + + BENCHMARK("distance: float", i) + { + return barrier(edge_edge_distance(EE_ROWS_OF(Vf)), 1.0f); + }; + BENCHMARK("distance: double", i) + { + return barrier(edge_edge_distance(EE_ROWS_OF(Vd)), 1.0); + }; + + // The full barrier Hessian assembly, which is what a simulator actually + // spends its time on. + BENCHMARK("barrier hessian: float", i) + { + const auto d = edge_edge_distance(EE_ROWS_OF(Vf)); + const auto grad_d = edge_edge_distance_gradient(EE_ROWS_OF(Vf)); + const auto hess_d = edge_edge_distance_hessian(EE_ROWS_OF(Vf)); + const float db = barrier_first_derivative(d, 1.0f); + const float d2b = barrier_second_derivative(d, 1.0f); + return (db * hess_d + (d2b * grad_d) * grad_d.transpose()).eval(); + }; + BENCHMARK("barrier hessian: double", i) + { + const auto d = edge_edge_distance(EE_ROWS_OF(Vd)); + const auto grad_d = edge_edge_distance_gradient(EE_ROWS_OF(Vd)); + const auto hess_d = edge_edge_distance_hessian(EE_ROWS_OF(Vd)); + const double db = barrier_first_derivative(d, 1.0); + const double d2b = barrier_second_derivative(d, 1.0); + return (db * hess_d + (d2b * grad_d) * grad_d.transpose()).eval(); + }; + +#undef EE_ROWS_OF +} + +#undef ROWS3 +#undef ROWS3_D +#undef ROWS4 +#undef ROWS4_D +#undef AT3 +#undef AT3_D +#undef AT4 +#undef AT4_D + +// ============================================================================= +// Converted families: tangent basis, closest point, relative velocity, area, +// edge-edge mollifier, point-plane. Measured old (dim-erased, double-only) +// vs. new (two-layer, dim-templated) via interleaved A/B of two binaries. +// The same-TU rows are identical in both binaries: the noise-floor control. + +namespace { + +double pe_cp_ref_3d( + Eigen::ConstRef p, + Eigen::ConstRef e0, + Eigen::ConstRef e1) +{ + const Eigen::Vector3d e = e1 - e0; + return e.dot(p - e0) / e.squaredNorm(); +} + +double el_ref_3d( + Eigen::ConstRef e0, Eigen::ConstRef e1) +{ + return (e1 - e0).norm(); +} + +} // namespace + +TEST_CASE("Converted families", "[!benchmark][eigen][converted]") +{ + constexpr int N = 100; + const Eigen::MatrixXd V = Eigen::MatrixXd::Random(N, 3); + const Eigen::ArrayXi idx = random_indices(N); + + std::vector P3(N); + std::vector PN(N); + for (int k = 0; k < N; ++k) { + P3[k] = V.row(k); + PN[k] = V.row(k); + } + +#define R2(f) f(V.row(idx[i % IDX_M]), V.row(idx[(i + 1) % IDX_M])) +#define R3(f) \ + f(V.row(idx[i % IDX_M]), V.row(idx[(i + 1) % IDX_M]), \ + V.row(idx[(i + 2) % IDX_M])) +#define R4(f) \ + f(V.row(idx[i % IDX_M]), V.row(idx[(i + 1) % IDX_M]), \ + V.row(idx[(i + 2) % IDX_M]), V.row(idx[(i + 3) % IDX_M])) +#define M3(f, src) \ + f(src[idx[i % IDX_M]], src[idx[(i + 1) % IDX_M]], src[idx[(i + 2) % IDX_M]]) + + // --- controls (same code in both binaries) --- + BENCHMARK("ctl pe_closest_point Ref", i) + { + return R3(pe_cp_ref_3d); + }; + BENCHMARK("ctl edge_length Ref", i) { return R2(el_ref_3d); }; + + // --- closest point --- + BENCHMARK("pe_closest_point rows", i) + { + return R3(point_edge_closest_point); + }; + BENCHMARK("pe_closest_point Vector3d", i) + { + return M3(point_edge_closest_point, P3); + }; + BENCHMARK("pe_closest_point VectorMax3d", i) + { + return M3(point_edge_closest_point, PN); + }; + BENCHMARK("pe_closest_point_jacobian rows", i) + { + return R3(point_edge_closest_point_jacobian); + }; + BENCHMARK("ee_closest_point rows", i) + { + return R4(edge_edge_closest_point); + }; + BENCHMARK("pt_closest_point rows", i) + { + return R4(point_triangle_closest_point); + }; + + // --- tangent basis --- + BENCHMARK("pp_tangent_basis rows", i) + { + return R2(point_point_tangent_basis); + }; + BENCHMARK("pp_tangent_basis Vector3d", i) + { + return point_point_tangent_basis( + P3[idx[i % IDX_M]], P3[idx[(i + 1) % IDX_M]]); + }; + BENCHMARK("pe_tangent_basis rows", i) + { + return R3(point_edge_tangent_basis); + }; + BENCHMARK("ee_tangent_basis rows", i) + { + return R4(edge_edge_tangent_basis); + }; + BENCHMARK("pt_tangent_basis rows", i) + { + return R4(point_triangle_tangent_basis); + }; + + // --- relative velocity --- + BENCHMARK("pp_relative_velocity rows", i) + { + return R2(point_point_relative_velocity); + }; + BENCHMARK("pp_relative_velocity VectorMax3d", i) + { + return point_point_relative_velocity( + PN[idx[i % IDX_M]], PN[idx[(i + 1) % IDX_M]]); + }; + BENCHMARK("pp_rel_vel_jacobian(dim=3)", i) + { + return point_point_relative_velocity_jacobian(2 + (idx[i % IDX_M] & 1)); + }; + + // --- area --- + BENCHMARK("edge_length rows", i) { return R2(edge_length); }; + BENCHMARK("edge_length VectorMax3d", i) + { + return edge_length(PN[idx[i % IDX_M]], PN[idx[(i + 1) % IDX_M]]); + }; + BENCHMARK("triangle_area rows", i) { return R3(triangle_area); }; + + // --- mollifier / point-plane --- + BENCHMARK("ee_cross_squarednorm rows", i) + { + return R4(edge_edge_cross_squarednorm); + }; + BENCHMARK("point_plane_distance rows", i) + { + return R3(point_plane_distance); + }; + +#undef R2 +#undef R3 +#undef R4 +#undef M3 } diff --git a/tests/src/tests/candidates/test_normals.cpp b/tests/src/tests/candidates/test_normals.cpp index b242151ba..b4fedc456 100644 --- a/tests/src/tests/candidates/test_normals.cpp +++ b/tests/src/tests/candidates/test_normals.cpp @@ -311,7 +311,7 @@ TEST_CASE("Triangle normal hessian", "[normal]") x << a, b, c; // Cross product matrix jacobian - Eigen::MatrixXd J_cross = cross_product_matrix_jacobian(); + Eigen::MatrixXd J_cross = cross_product_matrix_jacobian(); Eigen::MatrixXd fd_J_cross; fd::finite_jacobian_tensor<3>( a, diff --git a/tests/src/tests/ccd/test_ccd_benchmark.cpp b/tests/src/tests/ccd/test_ccd_benchmark.cpp index b19afee71..3d9d657b3 100644 --- a/tests/src/tests/ccd/test_ccd_benchmark.cpp +++ b/tests/src/tests/ccd/test_ccd_benchmark.cpp @@ -7,7 +7,7 @@ #include #include -#include +#include #include #include diff --git a/tests/src/tests/distance/test_distance_type.cpp b/tests/src/tests/distance/test_distance_type.cpp index 23f993195..3b5121613 100644 --- a/tests/src/tests/distance/test_distance_type.cpp +++ b/tests/src/tests/distance/test_distance_type.cpp @@ -508,4 +508,61 @@ TEST_CASE( const double expected = (ea1 - eb0).squaredNorm(); CHECK(dist == Catch::Approx(expected).margin(1e-10)); } -} \ No newline at end of file +} +TEST_CASE( + "Degenerate point-triangle distance types", + "[distance][distance-type][point-triangle][degenerate]") +{ + // Pins the degenerate-triangle behavior of point_triangle_distance_type. + // The closed-form solve guards its divisions with `a > 0` / `c > 0`, + // reproducing Eigen's LDLT pseudo-inverse handling of zero diagonal + // entries; dropping those guards would make every case below produce NaN + // parameters and silently fall through to P_T. + using PTDT = PointTriangleDistanceType; + const Eigen::Vector3d o(0, 0, 0), x(1, 0, 0), y(0, 1, 0); + + struct Case { + const char* name; + Eigen::Vector3d p, t0, t1, t2; + PTDT expected; + }; + const Case cases[] = { + { "all vertices coincide", { 0.5, 0.5, 0.5 }, o, o, o, PTDT::P_T }, + { "edge 0 zero (t0 == t1)", { 0.5, 0.5, 0 }, o, o, y, PTDT::P_E1 }, + { "edge 1 zero (t1 == t2)", { 0.5, 0.5, 0 }, o, x, x, PTDT::P_E0 }, + { "edge 2 zero (t2 == t0)", { 0.5, 0.5, 0 }, o, x, o, PTDT::P_E0 }, + { "collinear, p over edge 0", + { 0.5, 1, 0 }, + o, + x, + { 2, 0, 0 }, + PTDT::P_E0 }, + { "collinear, p past t2", { 3, 1, 0 }, o, x, { 2, 0, 0 }, PTDT::P_T2 }, + { "collinear, p before t0", + { -1, 1, 0 }, + o, + x, + { 2, 0, 0 }, + PTDT::P_T0 }, + { "p exactly on t0", o, o, x, y, PTDT::P_T0 }, + { "p exactly at centroid", + { 1.0 / 3.0, 1.0 / 3.0, 0 }, + o, + x, + y, + PTDT::P_T }, + { "p exactly on edge 0 midpoint", { 0.5, 0, 0 }, o, x, y, PTDT::P_E0 }, + { "interior at scale 1e-20", + Eigen::Vector3d(1.0 / 3.0, 1.0 / 3.0, 0) * 1e-20, o, x * 1e-20, + y * 1e-20, PTDT::P_T }, + { "interior at scale 1e+20", + Eigen::Vector3d(1.0 / 3.0, 1.0 / 3.0, 0) * 1e20, o, x * 1e20, + y * 1e20, PTDT::P_T }, + }; + + for (const Case& c : cases) { + CAPTURE(c.name); + CHECK( + point_triangle_distance_type(c.p, c.t0, c.t1, c.t2) == c.expected); + } +}