From 5620f5dfb0eba992479994a2e4926df693374d84 Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Thu, 23 Apr 2026 12:10:56 -0400 Subject: [PATCH 01/21] Template all distance functions for scalar type - Refactored all geometric distance, gradient, and hessian functions to be templated on scalar type (e.g., double, float). - Updated Python bindings to explicitly instantiate with double. - Removed point_point.cpp, moved implementation to header as inline template. - Updated all usages and tests to use templated versions. - Added Eigen expression overloads for point-point distance functions. - Updated normal and normalization utilities to be templated. - Set minimum C++ standard to C++20 in CMakeLists.txt. --- CMakeLists.txt | 2 +- python/src/distance/distance_type.cpp | 9 +- python/src/distance/edge_edge.cpp | 6 +- python/src/distance/edge_edge_mollifier.cpp | 31 +- python/src/distance/line_line.cpp | 6 +- python/src/distance/point_edge.cpp | 6 +- python/src/distance/point_line.cpp | 6 +- python/src/distance/point_plane.cpp | 42 +- python/src/distance/point_point.cpp | 6 +- python/src/distance/point_triangle.cpp | 8 +- python/src/distance/signed_distance.cpp | 19 +- python/src/geometry/normal.cpp | 43 +- src/ipc/distance/CMakeLists.txt | 1 - src/ipc/distance/distance_type.cpp | 92 +- src/ipc/distance/distance_type.hpp | 105 +- src/ipc/distance/edge_edge.cpp | 185 +-- src/ipc/distance/edge_edge.hpp | 99 +- src/ipc/distance/edge_edge_mollifier.cpp | 319 ++--- src/ipc/distance/edge_edge_mollifier.hpp | 560 +++++++-- src/ipc/distance/line_line.cpp | 141 +-- src/ipc/distance/line_line.hpp | 156 ++- src/ipc/distance/point_edge.cpp | 44 +- src/ipc/distance/point_edge.hpp | 85 +- src/ipc/distance/point_line.cpp | 867 +++++++------- src/ipc/distance/point_line.hpp | 178 ++- src/ipc/distance/point_plane.cpp | 1169 +++++++++---------- src/ipc/distance/point_plane.hpp | 244 +++- src/ipc/distance/point_point.cpp | 42 - src/ipc/distance/point_point.hpp | 76 +- src/ipc/distance/point_triangle.cpp | 159 ++- src/ipc/distance/point_triangle.hpp | 99 +- src/ipc/distance/signed/line_line.cpp | 75 +- src/ipc/distance/signed/line_line.hpp | 106 +- src/ipc/distance/signed/point_line.cpp | 63 +- src/ipc/distance/signed/point_line.hpp | 90 +- src/ipc/distance/signed/point_plane.cpp | 71 +- src/ipc/distance/signed/point_plane.hpp | 120 +- src/ipc/geometry/normal.cpp | 321 ++--- src/ipc/geometry/normal.hpp | 624 ++++++++-- src/ipc/utils/eigen_ext.hpp | 58 + tests/src/tests/candidates/test_normals.cpp | 2 +- 41 files changed, 3838 insertions(+), 2497 deletions(-) delete mode 100644 src/ipc/distance/point_point.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index d52d55134..1027fa022 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -93,7 +93,7 @@ mark_as_advanced(IPC_TOOLKIT_VERTEX_DERIVATIVE_LAYOUT) # This is an advanced opt # Set default minimum C++ standard if(IPC_TOOLKIT_TOPLEVEL_PROJECT) - set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD 20) set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_EXTENSIONS OFF) endif() diff --git a/python/src/distance/distance_type.cpp b/python/src/distance/distance_type.cpp index 2a1259ff6..3e458d71f 100644 --- a/python/src/distance/distance_type.cpp +++ b/python/src/distance/distance_type.cpp @@ -86,7 +86,7 @@ void define_distance_type(py::module_& m) .export_values(); m.def( - "point_edge_distance_type", &point_edge_distance_type, + "point_edge_distance_type", &point_edge_distance_type, R"ipc_Qu8mg5v7( Determine the closest pair between a point and edge. @@ -101,7 +101,7 @@ 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", &point_triangle_distance_type, R"ipc_Qu8mg5v7( Determine the closest pair between a point and triangle. @@ -117,7 +117,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", &edge_edge_distance_type, R"ipc_Qu8mg5v7( Determine the closest pair between two edges. @@ -133,7 +133,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", + &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..1af83a777 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", &edge_edge_distance, R"ipc_Qu8mg5v7( Compute the distance between a two lines segments in 3D. @@ -29,7 +29,7 @@ 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", &edge_edge_distance_gradient, R"ipc_Qu8mg5v7( Compute the gradient of the distance between a two lines segments. @@ -50,7 +50,7 @@ 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", &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..c1b819170 100644 --- a/python/src/distance/edge_edge_mollifier.cpp +++ b/python/src/distance/edge_edge_mollifier.cpp @@ -7,7 +7,7 @@ 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", &edge_edge_cross_squarednorm, R"ipc_Qu8mg5v7( Compute the squared norm of the edge-edge cross product. @@ -24,7 +24,7 @@ void define_edge_edge_mollifier(py::module_& m) m.def( "edge_edge_cross_squarednorm_gradient", - &edge_edge_cross_squarednorm_gradient, + &edge_edge_cross_squarednorm_gradient, R"ipc_Qu8mg5v7( Compute the gradient of the squared norm of the edge cross product. @@ -41,7 +41,7 @@ void define_edge_edge_mollifier(py::module_& m) m.def( "edge_edge_cross_squarednorm_hessian", - &edge_edge_cross_squarednorm_hessian, + &edge_edge_cross_squarednorm_hessian, R"ipc_Qu8mg5v7( Compute the hessian of the squared norm of the edge cross product. @@ -58,7 +58,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 +75,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 +90,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 +106,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 +121,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. @@ -140,7 +141,7 @@ void define_edge_edge_mollifier(py::module_& m) Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, const double>( - &edge_edge_mollifier), + &edge_edge_mollifier), R"ipc_Qu8mg5v7( Compute a mollifier for the edge-edge distance. @@ -165,7 +166,7 @@ void define_edge_edge_mollifier(py::module_& m) Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, const double>( - &edge_edge_mollifier_gradient), + &edge_edge_mollifier_gradient), R"ipc_Qu8mg5v7( Compute the gradient of the mollifier for the edge-edge distance. @@ -188,7 +189,7 @@ void define_edge_edge_mollifier(py::module_& m) Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, const double>( - &edge_edge_mollifier_hessian), + &edge_edge_mollifier_hessian), R"ipc_Qu8mg5v7( Compute the hessian of the mollifier for the edge-edge distance. @@ -206,7 +207,7 @@ void define_edge_edge_mollifier(py::module_& m) m.def( "edge_edge_mollifier_gradient_wrt_x", - &edge_edge_mollifier_gradient_wrt_x, + &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 +229,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, + &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 +253,7 @@ 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", &edge_edge_mollifier_threshold, R"ipc_Qu8mg5v7( Compute the threshold of the mollifier edge-edge distance. @@ -271,7 +272,7 @@ void define_edge_edge_mollifier(py::module_& m) m.def( "edge_edge_mollifier_threshold_gradient", - &edge_edge_mollifier_threshold_gradient, + &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..c82cf61f4 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", &line_line_distance, R"ipc_Qu8mg5v7( Compute the distance between a two infinite lines in 3D. @@ -29,7 +29,7 @@ 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", &line_line_distance_gradient, R"ipc_Qu8mg5v7( Compute the gradient of the distance between a two lines in 3D. @@ -51,7 +51,7 @@ 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", &line_line_distance_hessian, 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..51e010520 100644 --- a/python/src/distance/point_edge.cpp +++ b/python/src/distance/point_edge.cpp @@ -8,7 +8,7 @@ using namespace ipc; void define_point_edge_distance(py::module_& m) { m.def( - "point_edge_distance", &point_edge_distance, + "point_edge_distance", &point_edge_distance, R"ipc_Qu8mg5v7( Compute the distance between a point and edge in 2D or 3D. @@ -27,7 +27,7 @@ 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", &point_edge_distance_gradient, R"ipc_Qu8mg5v7( Compute the gradient of the distance between a point and edge. @@ -46,7 +46,7 @@ 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", &point_edge_distance_hessian, 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..f179ebb9e 100644 --- a/python/src/distance/point_line.cpp +++ b/python/src/distance/point_line.cpp @@ -7,7 +7,7 @@ using namespace ipc; void define_point_line_distance(py::module_& m) { m.def( - "point_line_distance", &point_line_distance, + "point_line_distance", &point_line_distance, R"ipc_Qu8mg5v7( Compute the distance between a point and line in 2D or 3D. @@ -25,7 +25,7 @@ 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", &point_line_distance_gradient, R"ipc_Qu8mg5v7( Compute the gradient of the distance between a point and line. @@ -43,7 +43,7 @@ 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", &point_line_distance_hessian, 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..c4ed14c7f 100644 --- a/python/src/distance/point_plane.cpp +++ b/python/src/distance/point_plane.cpp @@ -9,9 +9,8 @@ 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>(&point_plane_distance), R"ipc_Qu8mg5v7( Compute the distance between a point and a plane. @@ -31,10 +30,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>( + &point_plane_distance), R"ipc_Qu8mg5v7( Compute the distance between a point and a plane. @@ -55,10 +53,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>( + &point_plane_distance_gradient), R"ipc_Qu8mg5v7( Compute the gradient of the distance between a point and a plane. @@ -78,11 +75,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>( + &point_plane_distance_gradient), R"ipc_Qu8mg5v7( Compute the gradient of the distance between a point and a plane. @@ -103,10 +98,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>( + &point_plane_distance_hessian), R"ipc_Qu8mg5v7( Compute the hessian of the distance between a point and a plane. @@ -126,11 +120,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>( + &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..a7fa112bf 100644 --- a/python/src/distance/point_point.cpp +++ b/python/src/distance/point_point.cpp @@ -8,7 +8,7 @@ using namespace ipc; void define_point_point_distance(py::module_& m) { m.def( - "point_point_distance", &point_point_distance, + "point_point_distance", &point_point_distance, R"ipc_Qu8mg5v7( Compute the distance between two points. @@ -25,7 +25,7 @@ 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", &point_point_distance_gradient, R"ipc_Qu8mg5v7( Compute the gradient of the distance between two points. @@ -42,7 +42,7 @@ 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", &point_point_distance_hessian, 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..b955d9eac 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", &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", + &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", + &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..2138d6424 100644 --- a/python/src/distance/signed_distance.cpp +++ b/python/src/distance/signed_distance.cpp @@ -10,7 +10,7 @@ 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", point_line_signed_distance, R"ipc_Qu8mg5v7( Compute the signed distance from a point to a directed line segment (2D). @@ -26,7 +26,7 @@ void define_signed_distance(py::module_& m) m.def( "point_line_signed_distance_gradient", - point_line_signed_distance_gradient, + point_line_signed_distance_gradient, R"ipc_Qu8mg5v7( Compute the gradient of the signed point-to-line distance (2D). @@ -36,7 +36,7 @@ void define_signed_distance(py::module_& m) m.def( "point_line_signed_distance_hessian", - point_line_signed_distance_hessian, + point_line_signed_distance_hessian, R"ipc_Qu8mg5v7( Compute the Hessian of the signed point-to-line distance (2D). @@ -46,7 +46,7 @@ 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", point_plane_signed_distance, R"ipc_Qu8mg5v7( Compute the signed distance from a point to the plane of a triangle (3D). @@ -63,7 +63,7 @@ void define_signed_distance(py::module_& m) m.def( "point_plane_signed_distance_gradient", - point_plane_signed_distance_gradient, + point_plane_signed_distance_gradient, R"ipc_Qu8mg5v7( Compute the gradient of the signed point-to-plane distance (3D). @@ -73,7 +73,7 @@ void define_signed_distance(py::module_& m) m.def( "point_plane_signed_distance_hessian", - point_plane_signed_distance_hessian, + point_plane_signed_distance_hessian, R"ipc_Qu8mg5v7( Compute the Hessian of the signed point-to-plane distance (3D). @@ -83,7 +83,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", line_line_signed_distance, R"ipc_Qu8mg5v7( Compute the signed distance between two lines in 3D. @@ -98,7 +98,7 @@ void define_signed_distance(py::module_& m) m.def( "line_line_signed_distance_gradient", - line_line_signed_distance_gradient, + line_line_signed_distance_gradient, R"ipc_Qu8mg5v7( Compute the gradient of the signed line-line distance (3D). @@ -107,7 +107,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", + line_line_signed_distance_hessian, R"ipc_Qu8mg5v7( Compute the Hessian of the signed line-line distance (3D). diff --git a/python/src/geometry/normal.cpp b/python/src/geometry/normal.cpp index b6cd14079..9eb6a5622 100644 --- a/python/src/geometry/normal.cpp +++ b/python/src/geometry/normal.cpp @@ -7,7 +7,7 @@ using namespace ipc; void define_normal(py::module_& m) { m.def( - "normalization_and_jacobian", &normalization_and_jacobian, + "normalization_and_jacobian", &normalization_and_jacobian, R"ipc_qu8mg5v7( Computes the normalization and Jacobian of a vector. @@ -22,7 +22,7 @@ void define_normal(py::module_& m) "x"_a); m.def( - "normalization_jacobian", &normalization_jacobian, + "normalization_jacobian", &normalization_jacobian, R"ipc_qu8mg5v7( Computes the Jacobian of the normalization operation. @@ -38,7 +38,7 @@ void define_normal(py::module_& m) m.def( "normalization_and_jacobian_and_hessian", - &normalization_and_jacobian_and_hessian, + &normalization_and_jacobian_and_hessian, R"ipc_qu8mg5v7( Computes the normalization, Jacobian, and Hessian of a vector. @@ -53,7 +53,7 @@ void define_normal(py::module_& m) "x"_a); m.def( - "normalization_hessian", &normalization_hessian, + "normalization_hessian", &normalization_hessian, R"ipc_qu8mg5v7( Computes the Hessian of the normalization operation. @@ -68,7 +68,7 @@ void define_normal(py::module_& m) "x"_a); m.def( - "cross_product_matrix", &cross_product_matrix, + "cross_product_matrix", &cross_product_matrix, R"ipc_qu8mg5v7( Cross product matrix for 3D vectors. @@ -83,7 +83,7 @@ void define_normal(py::module_& m) "v"_a); m.def( - "cross_product_matrix_jacobian", &cross_product_matrix_jacobian, + "cross_product_matrix_jacobian", &cross_product_matrix_jacobian, R"ipc_qu8mg5v7( Computes the Jacobian of the cross product matrix. Returns @@ -92,7 +92,8 @@ void define_normal(py::module_& m) )ipc_qu8mg5v7"); m.def( - "point_line_unnormalized_normal", &point_line_unnormalized_normal, + "point_line_unnormalized_normal", + &point_line_unnormalized_normal, R"ipc_qu8mg5v7( Computes the unnormalized normal vector of a point-line pair. @@ -109,7 +110,7 @@ void define_normal(py::module_& m) "p"_a, "e0"_a, "e1"_a); m.def( - "point_line_normal", &point_line_normal, + "point_line_normal", &point_line_normal, R"ipc_qu8mg5v7( Computes the normal vector of a point-line pair. @@ -127,7 +128,7 @@ void define_normal(py::module_& m) m.def( "point_line_unnormalized_normal_jacobian", - &point_line_unnormalized_normal_jacobian, + &point_line_unnormalized_normal_jacobian, R"ipc_qu8mg5v7( Computes the Jacobian of the unnormalized normal vector of a point-line pair. @@ -144,7 +145,7 @@ void define_normal(py::module_& m) "p"_a, "e0"_a, "e1"_a); m.def( - "triangle_unnormalized_normal", &triangle_unnormalized_normal, + "triangle_unnormalized_normal", &triangle_unnormalized_normal, R"ipc_qu8mg5v7( Computes the unnormalized normal vector of a triangle. @@ -161,7 +162,7 @@ void define_normal(py::module_& m) "a"_a, "b"_a, "c"_a); m.def( - "triangle_normal", &triangle_normal, + "triangle_normal", &triangle_normal, R"ipc_qu8mg5v7( Computes the normal vector of a triangle. @@ -179,7 +180,7 @@ void define_normal(py::module_& m) m.def( "triangle_unnormalized_normal_jacobian", - &triangle_unnormalized_normal_jacobian, + &triangle_unnormalized_normal_jacobian, R"ipc_qu8mg5v7( Computes the Jacobian of the unnormalized normal vector of a triangle. @@ -197,7 +198,7 @@ void define_normal(py::module_& m) m.def( "triangle_unnormalized_normal_hessian", - &triangle_unnormalized_normal_hessian, + &triangle_unnormalized_normal_hessian, R"ipc_qu8mg5v7( Computes the Hessian of the unnormalized normal vector of a triangle. @@ -214,7 +215,7 @@ void define_normal(py::module_& m) "a"_a, "b"_a, "c"_a); m.def( - "triangle_normal_jacobian", &triangle_normal_jacobian, + "triangle_normal_jacobian", &triangle_normal_jacobian, R"ipc_qu8mg5v7( Computes the Jacobian of the normal vector of a triangle. @@ -231,7 +232,7 @@ void define_normal(py::module_& m) "a"_a, "b"_a, "c"_a); m.def( - "triangle_normal_hessian", &triangle_normal_hessian, + "triangle_normal_hessian", &triangle_normal_hessian, R"ipc_qu8mg5v7( Computes the Hessian of the normal vector of a triangle. @@ -248,7 +249,7 @@ 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", &line_line_unnormalized_normal, R"ipc_qu8mg5v7( Computes the unnormalized normal vector of two lines. @@ -266,7 +267,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", &line_line_normal, R"ipc_qu8mg5v7( Computes the normal vector of two lines. @@ -285,7 +286,7 @@ void define_normal(py::module_& m) m.def( "line_line_unnormalized_normal_jacobian", - &line_line_unnormalized_normal_jacobian, + &line_line_unnormalized_normal_jacobian, R"ipc_qu8mg5v7( Computes the Jacobian of the unnormalized normal vector of two lines. @@ -303,7 +304,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", &line_line_normal_jacobian, R"ipc_qu8mg5v7( Computes the Jacobian of the normal vector of two lines. @@ -322,7 +323,7 @@ void define_normal(py::module_& m) m.def( "line_line_unnormalized_normal_hessian", - &line_line_unnormalized_normal_hessian, + &line_line_unnormalized_normal_hessian, R"ipc_qu8mg5v7( Computes the Hessian of the unnormalized normal vector of two lines. @@ -340,7 +341,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", &line_line_normal_hessian, R"ipc_qu8mg5v7( Computes the Hessian of the normal vector of two lines. 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..509b9232b 100644 --- a/src/ipc/distance/distance_type.cpp +++ b/src/ipc/distance/distance_type.cpp @@ -7,23 +7,24 @@ namespace ipc { +template PointEdgeDistanceType point_edge_distance_type( - Eigen::ConstRef p, - Eigen::ConstRef e0, - Eigen::ConstRef e1) + 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(); + const VectorMax3 e = e1 - e0; + const T 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 } - const double ratio = e.dot(p - e0) / e_length_sqr; + const T ratio = e.dot(p - e0) / e_length_sqr; if (ratio < 0) { return PointEdgeDistanceType::P_E0; // PP (p-e0) } else if (ratio > 1) { @@ -33,15 +34,16 @@ PointEdgeDistanceType point_edge_distance_type( } } +template PointTriangleDistanceType point_triangle_distance_type( - Eigen::ConstRef p, - Eigen::ConstRef t0, - Eigen::ConstRef t1, - Eigen::ConstRef t2) + Eigen::ConstRef> p, + Eigen::ConstRef> t0, + Eigen::ConstRef> t1, + Eigen::ConstRef> t2) { - const Eigen::Vector3d normal = (t1 - t0).cross(t2 - t0); + const Eigen::Vector3 normal = (t1 - t0).cross(t2 - t0); - Eigen::Matrix basis, param; + Eigen::Matrix basis, param; basis.row(0) = t1 - t0; basis.row(1) = basis.row(0).cross(normal); @@ -65,25 +67,23 @@ PointTriangleDistanceType point_triangle_distance_type( } if (param(0, 0) <= 0.0 && param(0, 2) >= 1.0) { - // vertex 0 is the closest - return PointTriangleDistanceType::P_T0; + return PointTriangleDistanceType::P_T0; // vertex 0 is the closest } else if (param(0, 1) <= 0.0 && param(0, 0) >= 1.0) { - // vertex 1 is the closest - return PointTriangleDistanceType::P_T1; + return PointTriangleDistanceType::P_T1; // vertex 1 is the closest } else if (param(0, 2) <= 0.0 && param(0, 1) >= 1.0) { - // vertex 2 is the closest - return PointTriangleDistanceType::P_T2; + 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 @@ -92,16 +92,16 @@ EdgeEdgeDistanceType edge_edge_distance_type( // 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 Eigen::Vector3 u = ea1 - ea0; + const Eigen::Vector3 v = eb1 - eb0; + const Eigen::Vector3 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 + 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) { @@ -121,8 +121,8 @@ 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 + const T sN = (b * e - c * d); + T tN, tD; // tc = tN / tD if (sN <= 0.0) { // sc < 0 ⟹ the s=0 edge is visible tN = e; tD = c; @@ -173,15 +173,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 +211,15 @@ EdgeEdgeDistanceType edge_edge_parallel_distance_type( return EdgeEdgeDistanceType(ebc < 2 ? (eac << 1 | ebc) : (6 + eac)); } +// clang-format off +template PointEdgeDistanceType point_edge_distance_type(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef); +template PointEdgeDistanceType point_edge_distance_type(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef); +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 diff --git a/src/ipc/distance/distance_type.hpp b/src/ipc/distance/distance_type.hpp index 3f4b877c6..d6d680cd9 100644 --- a/src/ipc/distance/distance_type.hpp +++ b/src/ipc/distance/distance_type.hpp @@ -59,10 +59,11 @@ enum class EdgeEdgeDistanceType : uint8_t { /// @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 PointEdgeDistanceType point_edge_distance_type( - Eigen::ConstRef p, - Eigen::ConstRef e0, - Eigen::ConstRef e1); + Eigen::ConstRef> p, + Eigen::ConstRef> e0, + Eigen::ConstRef> e1); /// @brief Determine the closest pair between a point and triangle. /// @param p The point. @@ -70,11 +71,12 @@ 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. +template PointTriangleDistanceType point_triangle_distance_type( - Eigen::ConstRef p, - Eigen::ConstRef t0, - Eigen::ConstRef t1, - Eigen::ConstRef t2); + 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. @@ -82,11 +84,12 @@ 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. +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); /// @brief Determine the closest pair between two parallel edges. /// @param ea0 The first vertex of the first edge. @@ -94,10 +97,82 @@ 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. +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); + +// --- EigenExpression wrappers --- + +template +inline PointEdgeDistanceType point_edge_distance_type( + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& e0, + const Eigen::MatrixBase& e1) +{ + using T = typename D0::Scalar; + return point_edge_distance_type( + Eigen::Ref>(p), Eigen::Ref>(e0), + Eigen::Ref>(e1)); +} + +template < + EigenExpression D0, + EigenExpression D1, + EigenExpression D2, + EigenExpression D3> +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 D0::Scalar; + return point_triangle_distance_type( + Eigen::Ref>(p), + Eigen::Ref>(t0), + Eigen::Ref>(t1), + Eigen::Ref>(t2)); +} + +template < + EigenExpression D0, + EigenExpression D1, + EigenExpression D2, + EigenExpression D3> +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 D0::Scalar; + return edge_edge_distance_type( + Eigen::Ref>(ea0), + Eigen::Ref>(ea1), + Eigen::Ref>(eb0), + Eigen::Ref>(eb1)); +} + +template < + EigenExpression D0, + EigenExpression D1, + EigenExpression D2, + EigenExpression D3> +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 D0::Scalar; + return edge_edge_parallel_distance_type( + Eigen::Ref>(ea0), + Eigen::Ref>(ea1), + Eigen::Ref>(eb0), + Eigen::Ref>(eb1)); +} } // namespace ipc diff --git a/src/ipc/distance/edge_edge.cpp b/src/ipc/distance/edge_edge.cpp index b8c0395fa..ad636a574 100644 --- a/src/ipc/distance/edge_edge.cpp +++ b/src/ipc/distance/edge_edge.cpp @@ -1,5 +1,3 @@ - - #include "edge_edge.hpp" #include @@ -10,11 +8,12 @@ namespace ipc { -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) { @@ -55,68 +54,73 @@ double 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) { + using Vector6T = Eigen::Vector; + using Vector9T = Eigen::Vector; + using Vector12T = Eigen::Vector; + if (dtype == EdgeEdgeDistanceType::AUTO) { dtype = edge_edge_distance_type(ea0, ea1, eb0, eb1); } - 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: @@ -131,80 +135,110 @@ Vector12d 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) { + using Matrix6T = Eigen::Matrix; + using Matrix9T = Eigen::Matrix; + using Matrix12T = Eigen::Matrix; + if (dtype == EdgeEdgeDistanceType::AUTO) { dtype = edge_edge_distance_type(ea0, ea1, eb0, eb1); } - 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; @@ -220,4 +254,13 @@ Matrix12d edge_edge_distance_hessian( return hess; } +// clang-format off +template float edge_edge_distance(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, EdgeEdgeDistanceType); +template double edge_edge_distance(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, EdgeEdgeDistanceType); +template Vector12f edge_edge_distance_gradient(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, EdgeEdgeDistanceType); +template Vector12d edge_edge_distance_gradient(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, EdgeEdgeDistanceType); +template Matrix12f edge_edge_distance_hessian(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, EdgeEdgeDistanceType); +template Matrix12d edge_edge_distance_hessian(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, EdgeEdgeDistanceType); +// clang-format on + } // namespace ipc diff --git a/src/ipc/distance/edge_edge.hpp b/src/ipc/distance/edge_edge.hpp index 0f4f9bf40..92ad1692b 100644 --- a/src/ipc/distance/edge_edge.hpp +++ b/src/ipc/distance/edge_edge.hpp @@ -1,6 +1,7 @@ #pragma once #include +#include namespace ipc { @@ -12,11 +13,12 @@ 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, +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. @@ -27,11 +29,12 @@ 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, +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. @@ -42,11 +45,77 @@ 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, +template +Eigen::Matrix edge_edge_distance_hessian( + Eigen::ConstRef> ea0, + Eigen::ConstRef> ea1, + Eigen::ConstRef> eb0, + Eigen::ConstRef> eb1, EdgeEdgeDistanceType dtype = EdgeEdgeDistanceType::AUTO); +// --- EigenExpression wrappers --- + +template < + EigenExpression DerivedEa0, + EigenExpression DerivedEa1, + EigenExpression DerivedEb0, + EigenExpression 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) -> + typename DerivedEa0::Scalar +{ + using T = typename DerivedEa0::Scalar; + return edge_edge_distance( + Eigen::Ref>(ea0), + Eigen::Ref>(ea1), + Eigen::Ref>(eb0), + Eigen::Ref>(eb1), dtype); +} + +template < + EigenExpression DerivedEa0, + EigenExpression DerivedEa1, + EigenExpression DerivedEb0, + EigenExpression 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) + -> Eigen::Vector +{ + using T = typename DerivedEa0::Scalar; + return edge_edge_distance_gradient( + Eigen::Ref>(ea0), + Eigen::Ref>(ea1), + Eigen::Ref>(eb0), + Eigen::Ref>(eb1), dtype); +} + +template < + EigenExpression DerivedEa0, + EigenExpression DerivedEa1, + EigenExpression DerivedEb0, + EigenExpression 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) + -> Eigen::Matrix +{ + using T = typename DerivedEa0::Scalar; + return edge_edge_distance_hessian( + Eigen::Ref>(ea0), + Eigen::Ref>(ea1), + Eigen::Ref>(eb0), + Eigen::Ref>(eb1), dtype); +} + } // namespace ipc diff --git a/src/ipc/distance/edge_edge_mollifier.cpp b/src/ipc/distance/edge_edge_mollifier.cpp index c106f1dc5..735fdccee 100644 --- a/src/ipc/distance/edge_edge_mollifier.cpp +++ b/src/ipc/distance/edge_edge_mollifier.cpp @@ -4,175 +4,44 @@ namespace ipc { -double edge_edge_cross_squarednorm( - Eigen::ConstRef ea0, - Eigen::ConstRef ea1, - Eigen::ConstRef eb0, - Eigen::ConstRef eb1) +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) { - 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(); - } -} - -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 = + const T 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); + const T ee_cross_norm_sqr = edge_edge_cross_squarednorm(ea0, ea1, eb0, eb1); if (ee_cross_norm_sqr < eps_x) { // ∇ₓ m = ∂m/∂ε ∇ₓε return edge_edge_mollifier_derivative_wrt_eps_x( ee_cross_norm_sqr, eps_x) * edge_edge_mollifier_gradient(ea0, ea1, eb0, eb1, eps_x); } else { - return Vector12d::Zero(); + return Eigen::Vector::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) +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 double eps_x = + const T 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); + 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( @@ -184,55 +53,38 @@ Matrix12d edge_edge_mollifier_gradient_jacobian_wrt_x( + edge_edge_mollifier_gradient(ee_cross_norm_sqr, eps_x) * edge_edge_cross_squarednorm_hessian(ea0, ea1, eb0, eb1); } else { - return Matrix12d::Zero(); + return Eigen::Matrix::Zero(); } } -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(); -} - -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; -} +// clang-format off +template Vector12f edge_edge_mollifier_gradient_wrt_x(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef); +template Vector12d edge_edge_mollifier_gradient_wrt_x(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef); +template Matrix12f edge_edge_mollifier_gradient_jacobian_wrt_x(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef); +template Matrix12d edge_edge_mollifier_gradient_jacobian_wrt_x(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef); +// clang-format on 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; @@ -271,25 +123,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; @@ -506,21 +359,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; @@ -551,5 +405,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..419274261 100644 --- a/src/ipc/distance/edge_edge_mollifier.hpp +++ b/src/ipc/distance/edge_edge_mollifier.hpp @@ -4,73 +4,148 @@ namespace ipc { +// 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 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); +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(); +} /// @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); +/// @return The gradient of the squared norm of the edge cross product wrt ea0, +/// ea1, eb0, and eb1. +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; +} /// @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); +/// @return The hessian of the squared norm of the edge cross product wrt ea0, +/// ea1, eb0, and eb1. +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; +} /// @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); +} /// @brief Compute a mollifier for the edge-edge distance. /// @@ -82,12 +157,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 +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) { + return edge_edge_mollifier(ee_cross_norm_sqr, eps_x); + } else { + return T(1); + } +} /// @brief Compute the gradient of the mollifier for the edge-edge distance. /// @param ea0 The first vertex of the first edge. @@ -96,12 +180,22 @@ 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 +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 edge_edge_mollifier_gradient(ee_cross_norm_sqr, eps_x) + * edge_edge_cross_squarednorm_gradient(ea0, ea1, eb0, eb1); + } else { + return Eigen::Vector::Zero(); + } +} /// @brief Compute the hessian of the mollifier for the edge-edge distance. /// @param ea0 The first vertex of the first edge. @@ -110,14 +204,30 @@ 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 +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 (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 Eigen::Matrix::Zero(); + } +} + +/// @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 +237,21 @@ 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 +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); + +/// @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 +261,16 @@ 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 +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); /// @brief Compute the threshold of the mollifier edge-edge distance. /// @@ -167,13 +281,19 @@ 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 +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(); +} -/// @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 +302,246 @@ 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 +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; +} -// 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]); +// --- EigenExpression wrappers --- - 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]); +template < + EigenExpression DerivedEa0, + EigenExpression DerivedEa1, + EigenExpression DerivedEb0, + EigenExpression DerivedEb1> +inline auto edge_edge_cross_squarednorm( + const Eigen::MatrixBase& ea0, + const Eigen::MatrixBase& ea1, + const Eigen::MatrixBase& eb0, + const Eigen::MatrixBase& eb1) -> typename DerivedEa0::Scalar +{ + using T = typename DerivedEa0::Scalar; + return edge_edge_cross_squarednorm( + Eigen::Ref>(ea0), + Eigen::Ref>(ea1), + Eigen::Ref>(eb0), + Eigen::Ref>(eb1)); +} + +template < + EigenExpression DerivedEa0, + EigenExpression DerivedEa1, + EigenExpression DerivedEb0, + EigenExpression DerivedEb1> +inline auto edge_edge_cross_squarednorm_gradient( + const Eigen::MatrixBase& ea0, + const Eigen::MatrixBase& ea1, + const Eigen::MatrixBase& eb0, + const Eigen::MatrixBase& eb1) + -> Eigen::Vector +{ + using T = typename DerivedEa0::Scalar; + return edge_edge_cross_squarednorm_gradient( + Eigen::Ref>(ea0), + Eigen::Ref>(ea1), + Eigen::Ref>(eb0), + Eigen::Ref>(eb1)); +} + +template < + EigenExpression DerivedEa0, + EigenExpression DerivedEa1, + EigenExpression DerivedEb0, + EigenExpression DerivedEb1> +inline auto edge_edge_cross_squarednorm_hessian( + const Eigen::MatrixBase& ea0, + const Eigen::MatrixBase& ea1, + const Eigen::MatrixBase& eb0, + const Eigen::MatrixBase& eb1) + -> Eigen::Matrix +{ + using T = typename DerivedEa0::Scalar; + return edge_edge_cross_squarednorm_hessian( + Eigen::Ref>(ea0), + Eigen::Ref>(ea1), + Eigen::Ref>(eb0), + Eigen::Ref>(eb1)); +} + +template < + EigenExpression DerivedEa0, + EigenExpression DerivedEa1, + EigenExpression DerivedEb0, + EigenExpression 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) -> typename DerivedEa0::Scalar +{ + using T = typename DerivedEa0::Scalar; + return edge_edge_mollifier( + Eigen::Ref>(ea0), + Eigen::Ref>(ea1), + Eigen::Ref>(eb0), + Eigen::Ref>(eb1), eps_x); +} + +template < + EigenExpression DerivedEa0, + EigenExpression DerivedEa1, + EigenExpression DerivedEb0, + EigenExpression 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) + -> Eigen::Vector +{ + using T = typename DerivedEa0::Scalar; + return edge_edge_mollifier_gradient( + Eigen::Ref>(ea0), + Eigen::Ref>(ea1), + Eigen::Ref>(eb0), + Eigen::Ref>(eb1), eps_x); +} + +template < + EigenExpression DerivedEa0, + EigenExpression DerivedEa1, + EigenExpression DerivedEb0, + EigenExpression 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) + -> Eigen::Matrix +{ + using T = typename DerivedEa0::Scalar; + return edge_edge_mollifier_hessian( + Eigen::Ref>(ea0), + Eigen::Ref>(ea1), + Eigen::Ref>(eb0), + Eigen::Ref>(eb1), eps_x); +} + +template < + EigenExpression DerivedEa0Rest, + EigenExpression DerivedEa1Rest, + EigenExpression DerivedEb0Rest, + EigenExpression DerivedEb1Rest, + EigenExpression DerivedEa0, + EigenExpression DerivedEa1, + EigenExpression DerivedEb0, + EigenExpression 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) + -> Eigen::Vector +{ + using T = typename DerivedEa0Rest::Scalar; + return edge_edge_mollifier_gradient_wrt_x( + Eigen::Ref>(ea0_rest), + Eigen::Ref>(ea1_rest), + Eigen::Ref>(eb0_rest), + Eigen::Ref>(eb1_rest), + Eigen::Ref>(ea0), + Eigen::Ref>(ea1), + Eigen::Ref>(eb0), + Eigen::Ref>(eb1)); +} + +template < + EigenExpression DerivedEa0Rest, + EigenExpression DerivedEa1Rest, + EigenExpression DerivedEb0Rest, + EigenExpression DerivedEb1Rest, + EigenExpression DerivedEa0, + EigenExpression DerivedEa1, + EigenExpression DerivedEb0, + EigenExpression 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) + -> Eigen::Matrix +{ + using T = typename DerivedEa0Rest::Scalar; + return edge_edge_mollifier_gradient_jacobian_wrt_x( + Eigen::Ref>(ea0_rest), + Eigen::Ref>(ea1_rest), + Eigen::Ref>(eb0_rest), + Eigen::Ref>(eb1_rest), + Eigen::Ref>(ea0), + Eigen::Ref>(ea1), + Eigen::Ref>(eb0), + Eigen::Ref>(eb1)); +} + +template < + EigenExpression DerivedEa0Rest, + EigenExpression DerivedEa1Rest, + EigenExpression DerivedEb0Rest, + EigenExpression 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) -> + typename DerivedEa0Rest::Scalar +{ + using T = typename DerivedEa0Rest::Scalar; + return edge_edge_mollifier_threshold( + Eigen::Ref>(ea0_rest), + Eigen::Ref>(ea1_rest), + Eigen::Ref>(eb0_rest), + Eigen::Ref>(eb1_rest)); +} + +template < + EigenExpression DerivedEa0Rest, + EigenExpression DerivedEa1Rest, + EigenExpression DerivedEb0Rest, + EigenExpression 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) + -> Eigen::Vector +{ + using T = typename DerivedEa0Rest::Scalar; + return edge_edge_mollifier_threshold_gradient( + Eigen::Ref>(ea0_rest), + Eigen::Ref>(ea1_rest), + Eigen::Ref>(eb0_rest), + Eigen::Ref>(eb1_rest)); +} - 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..0d53cce25 100644 --- a/src/ipc/distance/line_line.cpp +++ b/src/ipc/distance/line_line.cpp @@ -1,67 +1,31 @@ #include "line_line.hpp" -#include +#include namespace ipc { -double line_line_distance( - 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(); -} - -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; -} - -Matrix12d line_line_distance_hessian( - Eigen::ConstRef ea0, - Eigen::ConstRef ea1, - Eigen::ConstRef eb0, - Eigen::ConstRef eb1) -{ - 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; -} - namespace autogen { // This function was generated by the Symbolic Math Toolbox version 8.3. // 14-Jun-2019 13:58:25 + template 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]) + 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 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; + 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; t11 = -v11 + v01; t12 = -v12 + v02; @@ -111,39 +75,40 @@ namespace autogen { // This function was generated by the Symbolic Math Toolbox version 8.3. // 14-Jun-2019 13:58:38 + template 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]) + 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 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; + 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; t11 = -v11 + v01; t12 = -v12 + v02; @@ -199,7 +164,7 @@ namespace autogen { t156 = 1.0 / ((t101 * t101 + t103 * t103) + t105 * t105); t159 = (t16 * t101 + t14 * t105) + -(t15 * t103); t157 = t156 * t156; - t57 = pow(t156, 3.0); + t57 = t156 * t156 * t156; t58 = t159 * t159; t262 = t11 * t156 * t159 * 2.0; t263 = t12 * t156 * t159 * 2.0; @@ -606,5 +571,11 @@ namespace autogen { H[143] = (t53 + t98 * t98 * t156 * 2.0) + b_t522_tmp * 4.0; } + // clang-format off + template void line_line_distance_gradient(float, float, float, float, float, float, float, float, float, float, float, float, float[12]); + template void line_line_distance_gradient(double, double, double, double, double, double, double, double, double, double, double, double, double[12]); + template void line_line_distance_hessian(float, float, float, float, float, float, float, float, float, float, float, float, float[144]); + template void line_line_distance_hessian(double, double, double, double, double, double, double, double, double, double, double, double, double[144]); + // clang-format on } // namespace autogen } // namespace ipc diff --git a/src/ipc/distance/line_line.hpp b/src/ipc/distance/line_line.hpp index e519d478b..e4851fa2c 100644 --- a/src/ipc/distance/line_line.hpp +++ b/src/ipc/distance/line_line.hpp @@ -2,8 +2,22 @@ #include +#include + namespace ipc { +// 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 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. @@ -12,11 +26,17 @@ namespace ipc { /// @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); +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(); +} /// @brief Compute the gradient of the distance between a two lines in 3D. /// @note The distance is actually squared distance. @@ -26,11 +46,19 @@ double line_line_distance( /// @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); +template +inline Eigen::Vector line_line_distance_gradient( + Eigen::ConstRef> ea0, + Eigen::ConstRef> ea1, + Eigen::ConstRef> eb0, + Eigen::ConstRef> 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; +} /// @brief Compute the hessian of the distance between a two lines in 3D. /// @note The distance is actually squared distance. @@ -40,43 +68,79 @@ Vector12d line_line_distance_gradient( /// @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); +template +inline Eigen::Matrix line_line_distance_hessian( + 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; +} -// 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]); +// --- EigenExpression wrappers --- - 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 < + EigenExpression DerivedEa0, + EigenExpression DerivedEa1, + EigenExpression DerivedEb0, + EigenExpression DerivedEb1> +inline auto line_line_distance( + const Eigen::MatrixBase& ea0, + const Eigen::MatrixBase& ea1, + const Eigen::MatrixBase& eb0, + const Eigen::MatrixBase& eb1) -> typename DerivedEa0::Scalar +{ + using T = typename DerivedEa0::Scalar; + return line_line_distance( + Eigen::Ref>(ea0), + Eigen::Ref>(ea1), + Eigen::Ref>(eb0), + Eigen::Ref>(eb1)); +} + +template < + EigenExpression DerivedEa0, + EigenExpression DerivedEa1, + EigenExpression DerivedEb0, + EigenExpression 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 +{ + using T = typename DerivedEa0::Scalar; + return line_line_distance_gradient( + Eigen::Ref>(ea0), + Eigen::Ref>(ea1), + Eigen::Ref>(eb0), + Eigen::Ref>(eb1)); +} + +template < + EigenExpression DerivedEa0, + EigenExpression DerivedEa1, + EigenExpression DerivedEb0, + EigenExpression 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 +{ + using T = typename DerivedEa0::Scalar; + return line_line_distance_hessian( + Eigen::Ref>(ea0), + Eigen::Ref>(ea1), + Eigen::Ref>(eb0), + Eigen::Ref>(eb1)); +} } // namespace ipc diff --git a/src/ipc/distance/point_edge.cpp b/src/ipc/distance/point_edge.cpp index ac53965ec..7c4e258be 100644 --- a/src/ipc/distance/point_edge.cpp +++ b/src/ipc/distance/point_edge.cpp @@ -7,10 +7,11 @@ namespace ipc { -double point_edge_distance( - Eigen::ConstRef p, - Eigen::ConstRef e0, - Eigen::ConstRef e1, +template +T point_edge_distance( + Eigen::ConstRef> p, + Eigen::ConstRef> e0, + Eigen::ConstRef> e1, PointEdgeDistanceType dtype) { assert(p.size() == 2 || p.size() == 3); @@ -37,10 +38,11 @@ double point_edge_distance( } } -VectorMax9d point_edge_distance_gradient( - Eigen::ConstRef p, - Eigen::ConstRef e0, - Eigen::ConstRef e1, +template +VectorMax9 point_edge_distance_gradient( + Eigen::ConstRef> p, + Eigen::ConstRef> e0, + Eigen::ConstRef> e1, PointEdgeDistanceType dtype) { const int dim = p.size(); @@ -51,7 +53,7 @@ VectorMax9d point_edge_distance_gradient( dtype = point_edge_distance_type(p, e0, e1); } - VectorMax9d grad = VectorMax9d::Zero(3 * dim); + VectorMax9 grad = VectorMax9::Zero(3 * dim); switch (dtype) { case PointEdgeDistanceType::P_E0: @@ -59,7 +61,7 @@ VectorMax9d point_edge_distance_gradient( break; case PointEdgeDistanceType::P_E1: { - const VectorMax6d local_grad = point_point_distance_gradient(p, e1); + const VectorMax6 local_grad = point_point_distance_gradient(p, e1); grad.head(dim) = local_grad.head(dim); grad.tail(dim) = local_grad.tail(dim); break; @@ -77,10 +79,11 @@ VectorMax9d point_edge_distance_gradient( return grad; } -MatrixMax9d point_edge_distance_hessian( - Eigen::ConstRef p, - Eigen::ConstRef e0, - Eigen::ConstRef e1, +template +MatrixMax9 point_edge_distance_hessian( + Eigen::ConstRef> p, + Eigen::ConstRef> e0, + Eigen::ConstRef> e1, PointEdgeDistanceType dtype) { const int dim = p.size(); @@ -91,7 +94,7 @@ MatrixMax9d point_edge_distance_hessian( dtype = point_edge_distance_type(p, e0, e1); } - MatrixMax9d hess = MatrixMax9d::Zero(3 * dim, 3 * dim); + MatrixMax9 hess = MatrixMax9::Zero(3 * dim, 3 * dim); switch (dtype) { case PointEdgeDistanceType::P_E0: @@ -100,7 +103,7 @@ MatrixMax9d point_edge_distance_hessian( break; case PointEdgeDistanceType::P_E1: { - const MatrixMax6d local_hess = point_point_distance_hessian(p, e1); + const MatrixMax6 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); @@ -121,4 +124,13 @@ MatrixMax9d point_edge_distance_hessian( return hess; } +// clang-format off +template float point_edge_distance(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, PointEdgeDistanceType); +template double point_edge_distance(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, PointEdgeDistanceType); +template VectorMax9 point_edge_distance_gradient(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, PointEdgeDistanceType); +template VectorMax9 point_edge_distance_gradient(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, PointEdgeDistanceType); +template MatrixMax9 point_edge_distance_hessian(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, PointEdgeDistanceType); +template MatrixMax9 point_edge_distance_hessian(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, PointEdgeDistanceType); +// clang-format on + } // namespace ipc diff --git a/src/ipc/distance/point_edge.hpp b/src/ipc/distance/point_edge.hpp index c2596c8a7..b053481e0 100644 --- a/src/ipc/distance/point_edge.hpp +++ b/src/ipc/distance/point_edge.hpp @@ -1,6 +1,7 @@ #pragma once #include +#include namespace ipc { @@ -11,10 +12,11 @@ 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, +template +T point_edge_distance( + Eigen::ConstRef> p, + Eigen::ConstRef> e0, + Eigen::ConstRef> e1, PointEdgeDistanceType dtype = PointEdgeDistanceType::AUTO); /// @brief Compute the gradient of the distance between a point and edge. @@ -23,11 +25,12 @@ 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, +/// @return The gradient of the distance wrt p, e0, and e1. +template +VectorMax9 point_edge_distance_gradient( + Eigen::ConstRef> p, + Eigen::ConstRef> e0, + Eigen::ConstRef> e1, PointEdgeDistanceType dtype = PointEdgeDistanceType::AUTO); /// @brief Compute the hessian of the distance between a point and edge. @@ -36,11 +39,65 @@ 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, +/// @return The hessian of the distance wrt p, e0, and e1. +template +MatrixMax9 point_edge_distance_hessian( + Eigen::ConstRef> p, + Eigen::ConstRef> e0, + Eigen::ConstRef> e1, PointEdgeDistanceType dtype = PointEdgeDistanceType::AUTO); +// --- EigenExpression wrappers --- + +template < + EigenExpression DerivedP, + EigenExpression DerivedE0, + EigenExpression DerivedE1> +inline auto point_edge_distance( + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& e0, + const Eigen::MatrixBase& e1, + PointEdgeDistanceType dtype = PointEdgeDistanceType::AUTO) -> + typename DerivedP::Scalar +{ + using T = typename DerivedP::Scalar; + return point_edge_distance( + Eigen::Ref>(p), Eigen::Ref>(e0), + Eigen::Ref>(e1), dtype); +} + +template < + EigenExpression DerivedP, + EigenExpression DerivedE0, + EigenExpression DerivedE1> +inline auto point_edge_distance_gradient( + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& e0, + const Eigen::MatrixBase& e1, + PointEdgeDistanceType dtype = PointEdgeDistanceType::AUTO) + -> VectorMax9 +{ + using T = typename DerivedP::Scalar; + return point_edge_distance_gradient( + Eigen::Ref>(p), Eigen::Ref>(e0), + Eigen::Ref>(e1), dtype); +} + +template < + EigenExpression DerivedP, + EigenExpression DerivedE0, + EigenExpression DerivedE1> +inline auto point_edge_distance_hessian( + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& e0, + const Eigen::MatrixBase& e1, + PointEdgeDistanceType dtype = PointEdgeDistanceType::AUTO) + -> MatrixMax9 +{ + using T = typename DerivedP::Scalar; + return point_edge_distance_hessian( + Eigen::Ref>(p), Eigen::Ref>(e0), + Eigen::Ref>(e1), dtype); +} + } // namespace ipc diff --git a/src/ipc/distance/point_line.cpp b/src/ipc/distance/point_line.cpp index f053da115..6b5f70094 100644 --- a/src/ipc/distance/point_line.cpp +++ b/src/ipc/distance/point_line.cpp @@ -1,491 +1,412 @@ #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 = 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; } -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 = 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); } -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 * 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 = t31 * t31 * t31; + 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; } -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; - - 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; - } +// 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; - // 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; + 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 = t102 * t102 * t102; + 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); +} - 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); - } +// clang-format off +template void point_line_distance_gradient_2D(float, float, float, float, float, float, float[6]); +template void point_line_distance_gradient_2D(double, double, double, double, double, double, double[6]); +template void point_line_distance_gradient_3D(float, float, float, float, float, float, float, float, float, float[9]); +template void point_line_distance_gradient_3D(double, double, double, double, double, double, double, double, double, double[9]); +template void point_line_distance_hessian_2D(float, float, float, float, float, float, float[36]); +template void point_line_distance_hessian_2D(double, double, double, double, double, double, double[36]); +template void point_line_distance_hessian_3D(float, float, float, float, float, float, float, float, float, float[81]); +template void point_line_distance_hessian_3D(double, double, double, double, double, double, double, double, double, double[81]); +// clang-format on -} // 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..01c26c320 100644 --- a/src/ipc/distance/point_line.hpp +++ b/src/ipc/distance/point_line.hpp @@ -4,16 +4,49 @@ 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 + /// @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 T point_line_distance( + 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); + + if (p.size() == 2) { + const Eigen::Vector 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. @@ -21,10 +54,27 @@ 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 VectorMax9 point_line_distance_gradient( + Eigen::ConstRef> p, + Eigen::ConstRef> e0, + Eigen::ConstRef> e1) +{ + const int dim = p.size(); + assert(e0.size() == dim); + assert(e1.size() == dim); + + VectorMax9 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; +} /// @brief Compute the hessian of the distance between a point and line. /// @note The distance is actually squared distance. @@ -32,53 +82,75 @@ VectorMax9d point_line_distance_gradient( /// @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); +template +inline MatrixMax9 point_line_distance_hessian( + Eigen::ConstRef> p, + Eigen::ConstRef> e0, + Eigen::ConstRef> e1) +{ + const int dim = p.size(); + assert(e0.size() == dim); + assert(e1.size() == dim); -// 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]); + MatrixMax9 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; +} - 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]); +// --- EigenExpression wrappers --- - void point_line_distance_hessian_2D( - double v01, - double v02, - double v11, - double v12, - double v21, - double v22, - double H[36]); +template < + EigenExpression DerivedP, + EigenExpression DerivedE0, + EigenExpression DerivedE1> +inline auto point_line_distance( + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& e0, + const Eigen::MatrixBase& e1) -> typename DerivedP::Scalar +{ + using T = typename DerivedP::Scalar; + return point_line_distance( + Eigen::Ref>(p), Eigen::Ref>(e0), + Eigen::Ref>(e1)); +} + +template < + EigenExpression DerivedP, + EigenExpression DerivedE0, + EigenExpression DerivedE1> +inline auto point_line_distance_gradient( + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& e0, + const Eigen::MatrixBase& e1) + -> VectorMax9 +{ + using T = typename DerivedP::Scalar; + return point_line_distance_gradient( + Eigen::Ref>(p), Eigen::Ref>(e0), + Eigen::Ref>(e1)); +} + +template < + EigenExpression DerivedP, + EigenExpression DerivedE0, + EigenExpression DerivedE1> +inline auto point_line_distance_hessian( + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& e0, + const Eigen::MatrixBase& e1) + -> MatrixMax9 +{ + using T = typename DerivedP::Scalar; + return point_line_distance_hessian( + Eigen::Ref>(p), Eigen::Ref>(e0), + Eigen::Ref>(e1)); +} - 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..2a63369bd 100644 --- a/src/ipc/distance/point_plane.cpp +++ b/src/ipc/distance/point_plane.cpp @@ -1,631 +1,562 @@ #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) -{ - const double point_to_plane = (p - origin).dot(normal); - return point_to_plane * point_to_plane / normal.squaredNorm(); -} - -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)); -} - -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: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]) { - return (2 / normal.squaredNorm()) * (p - origin).dot(normal) * normal; -} + T t11, t12, t13, t14, t15, t16, t17, t18, t19, t20, t21, t22, t32, t33, t34, + t43, t45, t44, t46; -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; + 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); } -Eigen::Matrix3d point_plane_distance_hessian( - 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() * normal * normal.transpose(); -} + 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; -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; + 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 = t202 * t202 * t202; + 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; } -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; - - 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; - } +// clang-format off +template void point_plane_distance_gradient(float, float, float, float, float, float, float, float, float, float, float, float, float[12]); +template void point_plane_distance_gradient(double, double, double, double, double, double, double, double, double, double, double, double, double[12]); +template void point_plane_distance_hessian(float, float, float, float, float, float, float, float, float, float, float, float, float[144]); +template void point_plane_distance_hessian(double, double, double, double, double, double, double, double, double, double, double, double, double[144]); +// clang-format on -} // 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..96824cd84 100644 --- a/src/ipc/distance/point_plane.hpp +++ b/src/ipc/distance/point_plane.hpp @@ -1,19 +1,37 @@ #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 + /// @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 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. @@ -22,11 +40,19 @@ 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 +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. + const Eigen::Vector3 n = triangle_normal(t0, t1, t2); + const T d = (p - t0).dot(n); + return d * d / n.squaredNorm(); +} /// @brief Compute the gradient of the distance between a point and a plane. /// @note The distance is actually squared distance. @@ -34,10 +60,14 @@ 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 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. @@ -46,11 +76,19 @@ 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 +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. @@ -58,10 +96,14 @@ 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 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. @@ -70,43 +112,129 @@ 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); +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; +} -// 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]); +// --- EigenExpression wrappers --- - 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 < + EigenExpression DerivedP, + EigenExpression DerivedOrigin, + EigenExpression DerivedNormal> +inline auto point_plane_distance( + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& origin, + const Eigen::MatrixBase& normal) -> typename DerivedP::Scalar +{ + using T = typename DerivedP::Scalar; + return point_plane_distance( + Eigen::Ref>(p), + Eigen::Ref>(origin), + Eigen::Ref>(normal)); +} + +template < + EigenExpression DerivedP, + EigenExpression DerivedT0, + EigenExpression DerivedT1, + EigenExpression DerivedT2> +inline auto point_plane_distance( + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& t0, + const Eigen::MatrixBase& t1, + const Eigen::MatrixBase& t2) -> typename DerivedP::Scalar +{ + using T = typename DerivedP::Scalar; + return point_plane_distance( + Eigen::Ref>(p), + Eigen::Ref>(t0), + Eigen::Ref>(t1), + Eigen::Ref>(t2)); +} + +template < + EigenExpression DerivedP, + EigenExpression DerivedOrigin, + EigenExpression DerivedNormal> +inline auto point_plane_distance_gradient( + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& origin, + const Eigen::MatrixBase& normal) + -> Eigen::Vector3 +{ + using T = typename DerivedP::Scalar; + return point_plane_distance_gradient( + Eigen::Ref>(p), + Eigen::Ref>(origin), + Eigen::Ref>(normal)); +} + +template < + EigenExpression DerivedP, + EigenExpression DerivedT0, + EigenExpression DerivedT1, + EigenExpression DerivedT2> +inline auto point_plane_distance_gradient( + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& t0, + const Eigen::MatrixBase& t1, + const Eigen::MatrixBase& t2) + -> Eigen::Vector +{ + using T = typename DerivedP::Scalar; + return point_plane_distance_gradient( + Eigen::Ref>(p), + Eigen::Ref>(t0), + Eigen::Ref>(t1), + Eigen::Ref>(t2)); +} + +template < + EigenExpression DerivedP, + EigenExpression DerivedOrigin, + EigenExpression DerivedNormal> +inline auto point_plane_distance_hessian( + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& origin, + const Eigen::MatrixBase& normal) + -> Eigen::Matrix +{ + using T = typename DerivedP::Scalar; + return point_plane_distance_hessian( + Eigen::Ref>(p), + Eigen::Ref>(origin), + Eigen::Ref>(normal)); +} + +template < + EigenExpression DerivedP, + EigenExpression DerivedT0, + EigenExpression DerivedT1, + EigenExpression DerivedT2> +inline auto point_plane_distance_hessian( + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& t0, + const Eigen::MatrixBase& t1, + const Eigen::MatrixBase& t2) + -> Eigen::Matrix +{ + using T = typename DerivedP::Scalar; + return point_plane_distance_hessian( + Eigen::Ref>(p), + Eigen::Ref>(t0), + Eigen::Ref>(t1), + Eigen::Ref>(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..da4b6f3ac 100644 --- a/src/ipc/distance/point_point.hpp +++ b/src/ipc/distance/point_point.hpp @@ -9,23 +9,87 @@ namespace ipc { /// @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 T point_point_distance( + Eigen::ConstRef> p0, Eigen::ConstRef> p1) +{ + return (p1 - p0).squaredNorm(); +} /// @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 VectorMax6 point_point_distance_gradient( + Eigen::ConstRef> p0, Eigen::ConstRef> p1) +{ + const int dim = p0.size(); + assert(p1.size() == dim); + + VectorMax6 grad(2 * dim); + grad.head(dim) = T(2) * (p0 - p1); + grad.tail(dim) = -grad.head(dim); + return grad; +} /// @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 MatrixMax6 point_point_distance_hessian( + Eigen::ConstRef> p0, Eigen::ConstRef> p1) +{ + const int dim = p0.size(); + assert(p1.size() == dim); + + MatrixMax6 hess(2 * dim, 2 * dim); + hess.setZero(); + hess.diagonal().setConstant(T(2)); + for (int i = 0; i < dim; i++) { + hess(i, i + dim) = hess(i + dim, i) = T(-2); + } + return hess; +} + +// --- EigenExpression wrappers --- + +template +inline auto point_point_distance( + const Eigen::MatrixBase& p0, + const Eigen::MatrixBase& p1) -> typename DerivedP0::Scalar +{ + using T = typename DerivedP0::Scalar; + return point_point_distance( + Eigen::Ref>(p0), + Eigen::Ref>(p1)); +} + +template +inline auto point_point_distance_gradient( + const Eigen::MatrixBase& p0, + const Eigen::MatrixBase& p1) + -> VectorMax6 +{ + using T = typename DerivedP0::Scalar; + return point_point_distance_gradient( + Eigen::Ref>(p0), + Eigen::Ref>(p1)); +} + +template +inline auto point_point_distance_hessian( + const Eigen::MatrixBase& p0, + const Eigen::MatrixBase& p1) + -> MatrixMax6 +{ + using T = typename DerivedP0::Scalar; + return point_point_distance_hessian( + Eigen::Ref>(p0), + Eigen::Ref>(p1)); +} } // namespace ipc diff --git a/src/ipc/distance/point_triangle.cpp b/src/ipc/distance/point_triangle.cpp index eac1ff64f..d8e88742a 100644 --- a/src/ipc/distance/point_triangle.cpp +++ b/src/ipc/distance/point_triangle.cpp @@ -8,11 +8,12 @@ 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) { @@ -47,54 +48,59 @@ double 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); } - 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; } @@ -110,66 +116,92 @@ Vector12d 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); } - 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; } @@ -185,4 +217,13 @@ Matrix12d point_triangle_distance_hessian( return hess; } +// clang-format off +template float point_triangle_distance(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, PointTriangleDistanceType); +template double point_triangle_distance(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, PointTriangleDistanceType); +template Vector12f point_triangle_distance_gradient(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, PointTriangleDistanceType); +template Vector12d point_triangle_distance_gradient(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, PointTriangleDistanceType); +template Matrix12f point_triangle_distance_hessian(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, PointTriangleDistanceType); +template Matrix12d point_triangle_distance_hessian(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, PointTriangleDistanceType); +// clang-format on + } // namespace ipc diff --git a/src/ipc/distance/point_triangle.hpp b/src/ipc/distance/point_triangle.hpp index a7bbd0642..b771c6fec 100644 --- a/src/ipc/distance/point_triangle.hpp +++ b/src/ipc/distance/point_triangle.hpp @@ -1,6 +1,7 @@ #pragma once #include +#include namespace ipc { @@ -12,11 +13,12 @@ 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, +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. @@ -27,11 +29,12 @@ 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, +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. @@ -42,11 +45,77 @@ 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, +template +Eigen::Matrix point_triangle_distance_hessian( + Eigen::ConstRef> p, + Eigen::ConstRef> t0, + Eigen::ConstRef> t1, + Eigen::ConstRef> t2, PointTriangleDistanceType dtype = PointTriangleDistanceType::AUTO); +// --- EigenExpression wrappers --- + +template < + EigenExpression DerivedP, + EigenExpression DerivedT0, + EigenExpression DerivedT1, + EigenExpression 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) -> + typename DerivedP::Scalar +{ + using T = typename DerivedP::Scalar; + return point_triangle_distance( + Eigen::Ref>(p), + Eigen::Ref>(t0), + Eigen::Ref>(t1), + Eigen::Ref>(t2), dtype); +} + +template < + EigenExpression DerivedP, + EigenExpression DerivedT0, + EigenExpression DerivedT1, + EigenExpression 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) + -> Eigen::Vector +{ + using T = typename DerivedP::Scalar; + return point_triangle_distance_gradient( + Eigen::Ref>(p), + Eigen::Ref>(t0), + Eigen::Ref>(t1), + Eigen::Ref>(t2), dtype); +} + +template < + EigenExpression DerivedP, + EigenExpression DerivedT0, + EigenExpression DerivedT1, + EigenExpression 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) + -> Eigen::Matrix +{ + using T = typename DerivedP::Scalar; + return point_triangle_distance_hessian( + Eigen::Ref>(p), + Eigen::Ref>(t0), + Eigen::Ref>(t1), + Eigen::Ref>(t2), dtype); +} + } // namespace ipc diff --git a/src/ipc/distance/signed/line_line.cpp b/src/ipc/distance/signed/line_line.cpp index 0ec94fd71..4b41ff15c 100644 --- a/src/ipc/distance/signed/line_line.cpp +++ b/src/ipc/distance/signed/line_line.cpp @@ -2,39 +2,23 @@ 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) +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 diff --git a/src/ipc/distance/signed/line_line.hpp b/src/ipc/distance/signed/line_line.hpp index f60ce5fa7..fdace1aba 100644 --- a/src/ipc/distance/signed/line_line.hpp +++ b/src/ipc/distance/signed/line_line.hpp @@ -1,6 +1,7 @@ #pragma once #include +#include namespace ipc { @@ -23,11 +24,12 @@ namespace ipc { /// @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) +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); } @@ -50,11 +52,21 @@ 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 +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. @@ -75,10 +87,72 @@ 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 +Eigen::Matrix line_line_signed_distance_hessian( + Eigen::ConstRef> ea0, + Eigen::ConstRef> ea1, + Eigen::ConstRef> eb0, + Eigen::ConstRef> eb1); + +// --- EigenExpression wrappers --- + +template < + EigenExpression DerivedEA0, + EigenExpression DerivedEA1, + EigenExpression DerivedEB0, + EigenExpression DerivedEB1> +inline auto line_line_signed_distance( + const Eigen::MatrixBase& ea0, + const Eigen::MatrixBase& ea1, + const Eigen::MatrixBase& eb0, + const Eigen::MatrixBase& eb1) -> typename DerivedEA0::Scalar +{ + using T = typename DerivedEA0::Scalar; + return line_line_signed_distance( + Eigen::Ref>(ea0), + Eigen::Ref>(ea1), + Eigen::Ref>(eb0), + Eigen::Ref>(eb1)); +} + +template < + EigenExpression DerivedEA0, + EigenExpression DerivedEA1, + EigenExpression DerivedEB0, + EigenExpression DerivedEB1> +inline auto line_line_signed_distance_gradient( + const Eigen::MatrixBase& ea0, + const Eigen::MatrixBase& ea1, + const Eigen::MatrixBase& eb0, + const Eigen::MatrixBase& eb1) + -> Eigen::Vector +{ + using T = typename DerivedEA0::Scalar; + return line_line_signed_distance_gradient( + Eigen::Ref>(ea0), + Eigen::Ref>(ea1), + Eigen::Ref>(eb0), + Eigen::Ref>(eb1)); +} + +template < + EigenExpression DerivedEA0, + EigenExpression DerivedEA1, + EigenExpression DerivedEB0, + EigenExpression DerivedEB1> +inline auto line_line_signed_distance_hessian( + const Eigen::MatrixBase& ea0, + const Eigen::MatrixBase& ea1, + const Eigen::MatrixBase& eb0, + const Eigen::MatrixBase& eb1) + -> Eigen::Matrix +{ + using T = typename DerivedEA0::Scalar; + return line_line_signed_distance_hessian( + Eigen::Ref>(ea0), + Eigen::Ref>(ea1), + Eigen::Ref>(eb0), + Eigen::Ref>(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..a09d055b0 100644 --- a/src/ipc/distance/signed/point_line.cpp +++ b/src/ipc/distance/signed/point_line.cpp @@ -2,37 +2,19 @@ namespace ipc { -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 +32,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 diff --git a/src/ipc/distance/signed/point_line.hpp b/src/ipc/distance/signed/point_line.hpp index a9be7434f..8b0fe81bc 100644 --- a/src/ipc/distance/signed/point_line.hpp +++ b/src/ipc/distance/signed/point_line.hpp @@ -1,6 +1,7 @@ #pragma once #include +#include namespace ipc { @@ -17,10 +18,11 @@ namespace ipc { /// @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 T point_line_signed_distance( + Eigen::ConstRef> p, + Eigen::ConstRef> e0, + Eigen::ConstRef> e1) { return point_line_normal(p, e0, e1).dot(p - e0); } @@ -37,10 +39,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 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, @@ -56,9 +67,62 @@ 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 +Eigen::Matrix point_line_signed_distance_hessian( + Eigen::ConstRef> p, + Eigen::ConstRef> e0, + Eigen::ConstRef> e1); + +// --- EigenExpression wrappers --- + +template < + EigenExpression DerivedP, + EigenExpression DerivedE0, + EigenExpression DerivedE1> +inline auto point_line_signed_distance( + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& e0, + const Eigen::MatrixBase& e1) -> typename DerivedP::Scalar +{ + using T = typename DerivedP::Scalar; + return point_line_signed_distance( + Eigen::Ref>(p), + Eigen::Ref>(e0), + Eigen::Ref>(e1)); +} + +template < + EigenExpression DerivedP, + EigenExpression DerivedE0, + EigenExpression DerivedE1> +inline auto point_line_signed_distance_gradient( + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& e0, + const Eigen::MatrixBase& e1) + -> Eigen::Vector +{ + using T = typename DerivedP::Scalar; + return point_line_signed_distance_gradient( + Eigen::Ref>(p), + Eigen::Ref>(e0), + Eigen::Ref>(e1)); +} + +template < + EigenExpression DerivedP, + EigenExpression DerivedE0, + EigenExpression DerivedE1> +inline auto point_line_signed_distance_hessian( + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& e0, + const Eigen::MatrixBase& e1) + -> Eigen::Matrix +{ + using T = typename DerivedP::Scalar; + return point_line_signed_distance_hessian( + Eigen::Ref>(p), + Eigen::Ref>(e0), + Eigen::Ref>(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..7b47bad9a 100644 --- a/src/ipc/distance/signed/point_plane.cpp +++ b/src/ipc/distance/signed/point_plane.cpp @@ -2,56 +2,36 @@ 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) +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 diff --git a/src/ipc/distance/signed/point_plane.hpp b/src/ipc/distance/signed/point_plane.hpp index 04fd90f34..b23b39109 100644 --- a/src/ipc/distance/signed/point_plane.hpp +++ b/src/ipc/distance/signed/point_plane.hpp @@ -1,6 +1,7 @@ #pragma once #include +#include namespace ipc { @@ -15,45 +16,122 @@ 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 +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 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); +/// @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 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 +Eigen::Matrix point_plane_signed_distance_hessian( + Eigen::ConstRef> p, + Eigen::ConstRef> t0, + Eigen::ConstRef> t1, + Eigen::ConstRef> t2); + +// --- EigenExpression wrappers --- + +template < + EigenExpression DerivedP, + EigenExpression DerivedT0, + EigenExpression DerivedT1, + EigenExpression DerivedT2> +inline auto point_plane_signed_distance( + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& t0, + const Eigen::MatrixBase& t1, + const Eigen::MatrixBase& t2) -> typename DerivedP::Scalar +{ + using T = typename DerivedP::Scalar; + return point_plane_signed_distance( + Eigen::Ref>(p), + Eigen::Ref>(t0), + Eigen::Ref>(t1), + Eigen::Ref>(t2)); +} + +template < + EigenExpression DerivedP, + EigenExpression DerivedT0, + EigenExpression DerivedT1, + EigenExpression DerivedT2> +inline auto point_plane_signed_distance_gradient( + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& t0, + const Eigen::MatrixBase& t1, + const Eigen::MatrixBase& t2) + -> Eigen::Vector +{ + using T = typename DerivedP::Scalar; + return point_plane_signed_distance_gradient( + Eigen::Ref>(p), + Eigen::Ref>(t0), + Eigen::Ref>(t1), + Eigen::Ref>(t2)); +} + +template < + EigenExpression DerivedP, + EigenExpression DerivedT0, + EigenExpression DerivedT1, + EigenExpression DerivedT2> +inline auto point_plane_signed_distance_hessian( + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& t0, + const Eigen::MatrixBase& t1, + const Eigen::MatrixBase& t2) + -> Eigen::Matrix +{ + using T = typename DerivedP::Scalar; + return point_plane_signed_distance_hessian( + Eigen::Ref>(p), + Eigen::Ref>(t0), + Eigen::Ref>(t1), + Eigen::Ref>(t2)); +} + +} // namespace ipc diff --git a/src/ipc/geometry/normal.cpp b/src/ipc/geometry/normal.cpp index b1abcb0ae..81e8d6d69 100644 --- a/src/ipc/geometry/normal.cpp +++ b/src/ipc/geometry/normal.cpp @@ -1,27 +1,32 @@ #include "ipc/geometry/normal.hpp" +#include + +#include + namespace ipc { -std::tuple -normalization_and_jacobian(Eigen::ConstRef x) +template +std::tuple, MatrixMax3> +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; + const T norm = x.norm(); + const VectorMax3 xhat = x / norm; + const auto I = MatrixMax3::Identity(x.size(), x.size()); + const MatrixMax3 J = (I - (xhat * xhat.transpose())) / norm; return std::make_tuple(xhat, J); } -std::tuple> -normalization_and_jacobian_and_hessian(Eigen::ConstRef x) +template +std::tuple, MatrixMax3, std::array, 3>> +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; + const T norm = x.norm(); + const VectorMax3 xhat = x / norm; + const auto I = MatrixMax3::Identity(x.size(), x.size()); + const MatrixMax3 J = (I - (xhat * xhat.transpose())) / norm; - std::array H; + std::array, 3> H; for (int i = 0; i < x.size(); i++) { H[i] = (xhat(i) * J + xhat * J.row(i) + J.col(i) * xhat.transpose()) / (-norm); @@ -32,88 +37,97 @@ normalization_and_jacobian_and_hessian(Eigen::ConstRef x) // --- 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 +135,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 +144,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 +157,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 +170,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 +202,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 +222,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 +246,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 +329,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 +408,29 @@ Eigen::Matrix line_line_normal_hessian( return d2n_dx2; } -} // namespace ipc \ No newline at end of file +// clang-format off +template std::tuple normalization_and_jacobian(Eigen::ConstRef); +template std::tuple normalization_and_jacobian(Eigen::ConstRef); +template std::tuple> normalization_and_jacobian_and_hessian(Eigen::ConstRef); +template std::tuple> normalization_and_jacobian_and_hessian(Eigen::ConstRef); +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 diff --git a/src/ipc/geometry/normal.hpp b/src/ipc/geometry/normal.hpp index 35db43c19..f7c1355a3 100644 --- a/src/ipc/geometry/normal.hpp +++ b/src/ipc/geometry/normal.hpp @@ -11,28 +11,32 @@ namespace ipc { /// @brief Computes the normalization and Jacobian of a vector. /// @param x The input vector. /// @return A tuple containing the normalized vector and its Jacobian. -std::tuple -normalization_and_jacobian(Eigen::ConstRef x); +template +std::tuple, MatrixMax3> +normalization_and_jacobian(Eigen::ConstRef> x); /// @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 MatrixMax3 normalization_jacobian(Eigen::ConstRef> x) { return std::get<1>(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 +std::tuple, MatrixMax3, std::array, 3>> +normalization_and_jacobian_and_hessian(Eigen::ConstRef> x); /// @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 std::array, 3> +normalization_hessian(Eigen::ConstRef> x) { return std::get<2>(normalization_and_jacobian_and_hessian(x)); } @@ -42,18 +46,22 @@ normalization_hessian(Eigen::ConstRef x) /// @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 Eigen::Matrix3 +cross_product_matrix(Eigen::ConstRef> v) { - Eigen::Matrix3d m; - m << 0, -v(2), v(1), // - v(2), 0, -v(0), // - -v(1), v(0), 0; + 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. -Eigen::Matrix cross_product_matrix_jacobian(); +template Eigen::Matrix cross_product_matrix_jacobian(); // ============================================================================= @@ -68,56 +76,63 @@ Eigen::Matrix cross_product_matrix_jacobian(); /// @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 +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. -inline VectorMax3d point_line_normal( - Eigen::ConstRef p, - Eigen::ConstRef e0, - Eigen::ConstRef e1) +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. +/// @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); - -/// @brief Computes the Hessian of the unnormalized normal vector of a point-line pair. +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. -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 +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. -inline MatrixMax point_line_normal_jacobian( - Eigen::ConstRef p, - Eigen::ConstRef e0, - Eigen::ConstRef e1) +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)) + return normalization_jacobian(point_line_unnormalized_normal(p, e0, e1)) * point_line_unnormalized_normal_jacobian(p, e0, e1); } @@ -126,10 +141,11 @@ 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); +template +MatrixMax point_line_normal_hessian( + Eigen::ConstRef> p, + Eigen::ConstRef> e0, + Eigen::ConstRef> e1); /** @} */ @@ -146,10 +162,11 @@ MatrixMax point_line_normal_hessian( /// @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 Eigen::Vector3 triangle_unnormalized_normal( + Eigen::ConstRef> a, + Eigen::ConstRef> b, + Eigen::ConstRef> c) { return (b - a).cross(c - a); } @@ -159,54 +176,60 @@ 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 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. +/// @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) -{ - 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 +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. +/// @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 +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. -inline Eigen::Matrix triangle_normal_jacobian( - Eigen::ConstRef a, - Eigen::ConstRef b, - Eigen::ConstRef c) +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); + 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. @@ -214,10 +237,11 @@ 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); +template +Eigen::Matrix triangle_normal_hessian( + Eigen::ConstRef> a, + Eigen::ConstRef> b, + Eigen::ConstRef> c); /** @} */ @@ -235,11 +259,12 @@ 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 +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); } @@ -250,32 +275,35 @@ 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 +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. +/// @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) -{ - 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); +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; } @@ -285,29 +313,32 @@ 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 +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( + 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. +/// @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 +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. @@ -315,12 +346,371 @@ 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 +Eigen::Matrix line_line_normal_hessian( + Eigen::ConstRef> ea0, + Eigen::ConstRef> ea1, + Eigen::ConstRef> eb0, + Eigen::ConstRef> eb1); /** @} */ -} // namespace ipc \ No newline at end of file +// --- EigenExpression wrappers --- + +template +inline auto normalization_and_jacobian(const Eigen::MatrixBase& x) +{ + using T = typename DerivedX::Scalar; + return normalization_and_jacobian(Eigen::Ref>(x)); +} + +template +inline auto normalization_jacobian(const Eigen::MatrixBase& x) +{ + using T = typename DerivedX::Scalar; + return normalization_jacobian(Eigen::Ref>(x)); +} + +template +inline auto +normalization_and_jacobian_and_hessian(const Eigen::MatrixBase& x) +{ + using T = typename DerivedX::Scalar; + return normalization_and_jacobian_and_hessian( + Eigen::Ref>(x)); +} + +template +inline auto normalization_hessian(const Eigen::MatrixBase& x) +{ + using T = typename DerivedX::Scalar; + return normalization_hessian(Eigen::Ref>(x)); +} + +template +inline auto cross_product_matrix(const Eigen::MatrixBase& v) + -> Eigen::Matrix +{ + using T = typename DerivedX::Scalar; + return cross_product_matrix(Eigen::Ref>(v)); +} + +template < + EigenExpression DerivedP, + EigenExpression DerivedE0, + EigenExpression DerivedE1> +inline auto point_line_unnormalized_normal( + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& e0, + const Eigen::MatrixBase& e1) + -> VectorMax3 +{ + using T = typename DerivedP::Scalar; + return point_line_unnormalized_normal( + Eigen::Ref>(p), Eigen::Ref>(e0), + Eigen::Ref>(e1)); +} + +template < + EigenExpression DerivedP, + EigenExpression DerivedE0, + EigenExpression DerivedE1> +inline auto point_line_normal( + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& e0, + const Eigen::MatrixBase& e1) + -> VectorMax3 +{ + using T = typename DerivedP::Scalar; + return point_line_normal( + Eigen::Ref>(p), Eigen::Ref>(e0), + Eigen::Ref>(e1)); +} + +template < + EigenExpression DerivedP, + EigenExpression DerivedE0, + EigenExpression DerivedE1> +inline auto point_line_unnormalized_normal_jacobian( + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& e0, + const Eigen::MatrixBase& e1) + -> MatrixMax +{ + using T = typename DerivedP::Scalar; + return point_line_unnormalized_normal_jacobian( + Eigen::Ref>(p), Eigen::Ref>(e0), + Eigen::Ref>(e1)); +} + +template < + EigenExpression DerivedP, + EigenExpression DerivedE0, + EigenExpression DerivedE1> +inline auto point_line_unnormalized_normal_hessian( + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& e0, + const Eigen::MatrixBase& e1) + -> MatrixMax +{ + using T = typename DerivedP::Scalar; + return point_line_unnormalized_normal_hessian( + Eigen::Ref>(p), Eigen::Ref>(e0), + Eigen::Ref>(e1)); +} + +template < + EigenExpression DerivedP, + EigenExpression DerivedE0, + EigenExpression DerivedE1> +inline auto point_line_normal_jacobian( + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& e0, + const Eigen::MatrixBase& e1) + -> MatrixMax +{ + using T = typename DerivedP::Scalar; + return point_line_normal_jacobian( + Eigen::Ref>(p), Eigen::Ref>(e0), + Eigen::Ref>(e1)); +} + +template < + EigenExpression DerivedP, + EigenExpression DerivedE0, + EigenExpression DerivedE1> +inline auto point_line_normal_hessian( + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& e0, + const Eigen::MatrixBase& e1) + -> MatrixMax +{ + using T = typename DerivedP::Scalar; + return point_line_normal_hessian( + Eigen::Ref>(p), Eigen::Ref>(e0), + Eigen::Ref>(e1)); +} + +template < + EigenExpression DerivedA, + EigenExpression DerivedB, + EigenExpression DerivedC> +inline auto triangle_unnormalized_normal( + const Eigen::MatrixBase& a, + const Eigen::MatrixBase& b, + const Eigen::MatrixBase& c) + -> Eigen::Vector3 +{ + using T = typename DerivedA::Scalar; + return triangle_unnormalized_normal( + Eigen::Ref>(a), + Eigen::Ref>(b), + Eigen::Ref>(c)); +} + +template < + EigenExpression DerivedA, + EigenExpression DerivedB, + EigenExpression DerivedC> +inline auto triangle_normal( + const Eigen::MatrixBase& a, + const Eigen::MatrixBase& b, + const Eigen::MatrixBase& c) + -> Eigen::Vector3 +{ + using T = typename DerivedA::Scalar; + return triangle_normal( + Eigen::Ref>(a), + Eigen::Ref>(b), + Eigen::Ref>(c)); +} + +template < + EigenExpression DerivedA, + EigenExpression DerivedB, + EigenExpression DerivedC> +inline auto triangle_unnormalized_normal_jacobian( + const Eigen::MatrixBase& a, + const Eigen::MatrixBase& b, + const Eigen::MatrixBase& c) + -> Eigen::Matrix +{ + using T = typename DerivedA::Scalar; + return triangle_unnormalized_normal_jacobian( + Eigen::Ref>(a), + Eigen::Ref>(b), + Eigen::Ref>(c)); +} + +template < + EigenExpression DerivedA, + EigenExpression DerivedB, + EigenExpression DerivedC> +inline auto triangle_unnormalized_normal_hessian( + const Eigen::MatrixBase& a, + const Eigen::MatrixBase& b, + const Eigen::MatrixBase& c) + -> Eigen::Matrix +{ + using T = typename DerivedA::Scalar; + return triangle_unnormalized_normal_hessian( + Eigen::Ref>(a), + Eigen::Ref>(b), + Eigen::Ref>(c)); +} + +template < + EigenExpression DerivedA, + EigenExpression DerivedB, + EigenExpression DerivedC> +inline auto triangle_normal_jacobian( + const Eigen::MatrixBase& a, + const Eigen::MatrixBase& b, + const Eigen::MatrixBase& c) + -> Eigen::Matrix +{ + using T = typename DerivedA::Scalar; + return triangle_normal_jacobian( + Eigen::Ref>(a), + Eigen::Ref>(b), + Eigen::Ref>(c)); +} + +template < + EigenExpression DerivedA, + EigenExpression DerivedB, + EigenExpression DerivedC> +inline auto triangle_normal_hessian( + const Eigen::MatrixBase& a, + const Eigen::MatrixBase& b, + const Eigen::MatrixBase& c) + -> Eigen::Matrix +{ + using T = typename DerivedA::Scalar; + return triangle_normal_hessian( + Eigen::Ref>(a), + Eigen::Ref>(b), + Eigen::Ref>(c)); +} + +template < + EigenExpression DerivedEA0, + EigenExpression DerivedEA1, + EigenExpression DerivedEB0, + EigenExpression DerivedEB1> +inline auto line_line_unnormalized_normal( + const Eigen::MatrixBase& ea0, + const Eigen::MatrixBase& ea1, + const Eigen::MatrixBase& eb0, + const Eigen::MatrixBase& eb1) + -> Eigen::Vector3 +{ + using T = typename DerivedEA0::Scalar; + return line_line_unnormalized_normal( + Eigen::Ref>(ea0), + Eigen::Ref>(ea1), + Eigen::Ref>(eb0), + Eigen::Ref>(eb1)); +} + +template < + EigenExpression DerivedEA0, + EigenExpression DerivedEA1, + EigenExpression DerivedEB0, + EigenExpression DerivedEB1> +inline auto line_line_normal( + const Eigen::MatrixBase& ea0, + const Eigen::MatrixBase& ea1, + const Eigen::MatrixBase& eb0, + const Eigen::MatrixBase& eb1) + -> Eigen::Vector3 +{ + using T = typename DerivedEA0::Scalar; + return line_line_normal( + Eigen::Ref>(ea0), + Eigen::Ref>(ea1), + Eigen::Ref>(eb0), + Eigen::Ref>(eb1)); +} + +template < + EigenExpression DerivedEA0, + EigenExpression DerivedEA1, + EigenExpression DerivedEB0, + EigenExpression 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 +{ + using T = typename DerivedEA0::Scalar; + return line_line_unnormalized_normal_jacobian( + Eigen::Ref>(ea0), + Eigen::Ref>(ea1), + Eigen::Ref>(eb0), + Eigen::Ref>(eb1)); +} + +template < + EigenExpression DerivedEA0, + EigenExpression DerivedEA1, + EigenExpression DerivedEB0, + EigenExpression DerivedEB1> +inline auto line_line_normal_jacobian( + const Eigen::MatrixBase& ea0, + const Eigen::MatrixBase& ea1, + const Eigen::MatrixBase& eb0, + const Eigen::MatrixBase& eb1) + -> Eigen::Matrix +{ + using T = typename DerivedEA0::Scalar; + return line_line_normal_jacobian( + Eigen::Ref>(ea0), + Eigen::Ref>(ea1), + Eigen::Ref>(eb0), + Eigen::Ref>(eb1)); +} + +template < + EigenExpression DerivedEA0, + EigenExpression DerivedEA1, + EigenExpression DerivedEB0, + EigenExpression DerivedEB1> +inline auto line_line_unnormalized_normal_hessian( + const Eigen::MatrixBase& ea0, + const Eigen::MatrixBase& ea1, + const Eigen::MatrixBase& eb0, + const Eigen::MatrixBase& eb1) + -> Eigen::Matrix +{ + using T = typename DerivedEA0::Scalar; + return line_line_unnormalized_normal_hessian( + Eigen::Ref>(ea0), + Eigen::Ref>(ea1), + Eigen::Ref>(eb0), + Eigen::Ref>(eb1)); +} + +template < + EigenExpression DerivedEA0, + EigenExpression DerivedEA1, + EigenExpression DerivedEB0, + EigenExpression DerivedEB1> +inline auto line_line_normal_hessian( + const Eigen::MatrixBase& ea0, + const Eigen::MatrixBase& ea1, + const Eigen::MatrixBase& eb0, + const Eigen::MatrixBase& eb1) + -> Eigen::Matrix +{ + using T = typename DerivedEA0::Scalar; + return line_line_normal_hessian( + Eigen::Ref>(ea0), + Eigen::Ref>(ea1), + Eigen::Ref>(eb0), + Eigen::Ref>(eb1)); +} + +} // namespace ipc diff --git a/src/ipc/utils/eigen_ext.hpp b/src/ipc/utils/eigen_ext.hpp index 2dd00f79b..daa79d531 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 +using MatrixMax2f = MatrixMax2; /// @brief A dynamic size matrix with a fixed maximum size of 3×3 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 +using MatrixMax9f = MatrixMax9; +/// @brief A dynamic size matrix with a fixed maximum size of 12×12 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,23 @@ using HessianType = std:: /**@}*/ +/// @brief Matches any Eigen expression — stored types, blocks, maps, lazy +/// expressions. Layer 1 wrappers accept any MatrixBase-derived type, deduce T, +/// and forward as Eigen::ConstRef (zero-copy for blocks/stored types; evaluates +/// lazy expressions once). Layer 1 always calls Layer 2 with explicit to +/// prevent infinite recursion (Layer 2's first template param is T, Layer 1's +/// is DerivedXxx, so explicit-T calls unambiguously route to Layer 2). +/// +/// Pure requires-expression: checks only members of T itself, so the compiler +/// never attempts to form Eigen::MatrixBase (which is ill-formed when T +/// is a scalar type like double). +template +concept EigenExpression = requires(T& t) { + typename T::Scalar; + { t.rows() } -> std::convertible_to; + { t.cols() } -> std::convertible_to; +}; + /// @brief Matrix projection onto positive definite cone /// @param A Symmetric matrix to project /// @param eps Minimum eigenvalue threshold 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, From 59969f342e46269b0c96a24d583730c9da37bcaf Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Thu, 23 Apr 2026 14:44:01 -0400 Subject: [PATCH 02/21] Refactor smooth contact distance code to use unified distance functions - Remove redundant point/edge/triangle distance implementations from smooth_contact/distance - Replace calls to local *_sqr_distance with core distance functions - Update includes to use ipc/distance/point_line.hpp and related headers - Simplify mollifier and primitive_distance to use unified API - Minor cleanup in tangential_potential and point_plane distance math --- src/ipc/distance/edge_edge.cpp | 15 ++- src/ipc/distance/point_edge.cpp | 6 +- src/ipc/distance/point_plane.hpp | 9 +- src/ipc/distance/point_triangle.cpp | 11 +- src/ipc/potentials/tangential_potential.cpp | 4 - src/ipc/smooth_contact/distance/edge_edge.cpp | 116 ++---------------- src/ipc/smooth_contact/distance/edge_edge.hpp | 15 --- src/ipc/smooth_contact/distance/mollifier.tpp | 8 +- .../smooth_contact/distance/point_edge.cpp | 28 +---- .../smooth_contact/distance/point_edge.hpp | 12 +- .../smooth_contact/distance/point_face.cpp | 95 +------------- .../smooth_contact/distance/point_face.hpp | 15 --- .../distance/primitive_distance.cpp | 1 - .../distance/primitive_distance.tpp | 7 +- 14 files changed, 54 insertions(+), 288 deletions(-) diff --git a/src/ipc/distance/edge_edge.cpp b/src/ipc/distance/edge_edge.cpp index ad636a574..34cb1fa87 100644 --- a/src/ipc/distance/edge_edge.cpp +++ b/src/ipc/distance/edge_edge.cpp @@ -3,6 +3,7 @@ #include #include #include +#include #include // std::invalid_argument @@ -16,8 +17,10 @@ T edge_edge_distance( Eigen::ConstRef> eb1, EdgeEdgeDistanceType dtype) { - if (dtype == EdgeEdgeDistanceType::AUTO) { - dtype = edge_edge_distance_type(ea0, ea1, eb0, eb1); + if constexpr (std::is_same_v || std::is_same_v) { + if (dtype == EdgeEdgeDistanceType::AUTO) { + dtype = edge_edge_distance_type(ea0, ea1, eb0, eb1); + } } switch (dtype) { @@ -62,7 +65,7 @@ Eigen::Vector edge_edge_distance_gradient( Eigen::ConstRef> eb1, EdgeEdgeDistanceType dtype) { - using Vector6T = Eigen::Vector; + using Vector6T = Eigen::Vector; using Vector9T = Eigen::Vector; using Vector12T = Eigen::Vector; @@ -257,6 +260,12 @@ Eigen::Matrix edge_edge_distance_hessian( // clang-format off template float edge_edge_distance(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, EdgeEdgeDistanceType); template double edge_edge_distance(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, EdgeEdgeDistanceType); +template ADGrad<9> edge_edge_distance>(Eigen::ConstRef>>, Eigen::ConstRef>>, Eigen::ConstRef>>, Eigen::ConstRef>>, EdgeEdgeDistanceType); +template ADHessian<9> edge_edge_distance>(Eigen::ConstRef>>, Eigen::ConstRef>>, Eigen::ConstRef>>, Eigen::ConstRef>>, EdgeEdgeDistanceType); +template ADGrad<12> edge_edge_distance>(Eigen::ConstRef>>, Eigen::ConstRef>>, Eigen::ConstRef>>, Eigen::ConstRef>>, EdgeEdgeDistanceType); +template ADHessian<12> edge_edge_distance>(Eigen::ConstRef>>, Eigen::ConstRef>>, Eigen::ConstRef>>, Eigen::ConstRef>>, EdgeEdgeDistanceType); +template ADGrad<13> edge_edge_distance>(Eigen::ConstRef>>, Eigen::ConstRef>>, Eigen::ConstRef>>, Eigen::ConstRef>>, EdgeEdgeDistanceType); +template ADHessian<13> edge_edge_distance>(Eigen::ConstRef>>, Eigen::ConstRef>>, Eigen::ConstRef>>, Eigen::ConstRef>>, EdgeEdgeDistanceType); template Vector12f edge_edge_distance_gradient(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, EdgeEdgeDistanceType); template Vector12d edge_edge_distance_gradient(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, EdgeEdgeDistanceType); template Matrix12f edge_edge_distance_hessian(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, EdgeEdgeDistanceType); diff --git a/src/ipc/distance/point_edge.cpp b/src/ipc/distance/point_edge.cpp index 7c4e258be..cb0b99b86 100644 --- a/src/ipc/distance/point_edge.cpp +++ b/src/ipc/distance/point_edge.cpp @@ -18,8 +18,10 @@ T point_edge_distance( 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); + if constexpr (std::is_same_v || std::is_same_v) { + if (dtype == PointEdgeDistanceType::AUTO) { + dtype = point_edge_distance_type(p, e0, e1); + } } switch (dtype) { diff --git a/src/ipc/distance/point_plane.hpp b/src/ipc/distance/point_plane.hpp index 96824cd84..c4102fd07 100644 --- a/src/ipc/distance/point_plane.hpp +++ b/src/ipc/distance/point_plane.hpp @@ -49,9 +49,8 @@ inline T point_plane_distance( { // Inline the (p, origin, normal) overload to avoid two-phase lookup issues // with dependent argument types. - const Eigen::Vector3 n = triangle_normal(t0, t1, t2); - const T d = (p - t0).dot(n); - return d * d / n.squaredNorm(); + 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. @@ -66,7 +65,7 @@ inline Eigen::Vector3 point_plane_distance_gradient( Eigen::ConstRef> origin, Eigen::ConstRef> normal) { - return (T(2) / normal.squaredNorm()) * (p - origin).dot(normal) * normal; + return (2.0 / normal.squaredNorm()) * (p - origin).dot(normal) * normal; } /// @brief Compute the gradient of the distance between a point and a plane. @@ -102,7 +101,7 @@ inline Eigen::Matrix3 point_plane_distance_hessian( Eigen::ConstRef> origin, Eigen::ConstRef> normal) { - return T(2) / normal.squaredNorm() * normal * normal.transpose(); + return ((2.0 / normal.squaredNorm()) * normal) * normal.transpose(); } /// @brief Compute the hessian of the distance between a point and a plane. diff --git a/src/ipc/distance/point_triangle.cpp b/src/ipc/distance/point_triangle.cpp index d8e88742a..f0fa96c07 100644 --- a/src/ipc/distance/point_triangle.cpp +++ b/src/ipc/distance/point_triangle.cpp @@ -3,6 +3,7 @@ #include #include #include +#include #include // std::invalid_argument @@ -16,8 +17,10 @@ T point_triangle_distance( Eigen::ConstRef> t2, PointTriangleDistanceType dtype) { - if (dtype == PointTriangleDistanceType::AUTO) { - dtype = point_triangle_distance_type(p, t0, t1, t2); + if constexpr (std::is_same_v || std::is_same_v) { + if (dtype == PointTriangleDistanceType::AUTO) { + dtype = point_triangle_distance_type(p, t0, t1, t2); + } } switch (dtype) { @@ -220,6 +223,10 @@ Eigen::Matrix point_triangle_distance_hessian( // clang-format off template float point_triangle_distance(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, PointTriangleDistanceType); template double point_triangle_distance(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, PointTriangleDistanceType); +template ADGrad<12> point_triangle_distance>(Eigen::ConstRef>>, Eigen::ConstRef>>, Eigen::ConstRef>>, Eigen::ConstRef>>, PointTriangleDistanceType); +template ADHessian<12> point_triangle_distance>(Eigen::ConstRef>>, Eigen::ConstRef>>, Eigen::ConstRef>>, Eigen::ConstRef>>, PointTriangleDistanceType); +template ADGrad<13> point_triangle_distance>(Eigen::ConstRef>>, Eigen::ConstRef>>, Eigen::ConstRef>>, Eigen::ConstRef>>, PointTriangleDistanceType); +template ADHessian<13> point_triangle_distance>(Eigen::ConstRef>>, Eigen::ConstRef>>, Eigen::ConstRef>>, Eigen::ConstRef>>, PointTriangleDistanceType); template Vector12f point_triangle_distance_gradient(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, PointTriangleDistanceType); template Vector12d point_triangle_distance_gradient(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, PointTriangleDistanceType); template Matrix12f point_triangle_distance_hessian(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, PointTriangleDistanceType); diff --git a/src/ipc/potentials/tangential_potential.cpp b/src/ipc/potentials/tangential_potential.cpp index a761cfb5b..33ca51fee 100644 --- a/src/ipc/potentials/tangential_potential.cpp +++ b/src/ipc/potentials/tangential_potential.cpp @@ -189,7 +189,6 @@ double TangentialPotential::operator()( const VectorMax2d u_aniso = collision.mu_aniso.head(u.size()).cwiseProduct(u); - const int tangent_dim = u.size(); double mu_s, mu_k; friction_mu_for_evaluation(collision, false, mu_s, mu_k); @@ -794,9 +793,6 @@ TangentialPotential::VectorMaxNd TangentialPotential::smooth_contact_force( const VectorMax2d tau_aniso = collision.mu_aniso.head(tau.size()).cwiseProduct(tau); - // Get tangent space dimension (1 for 2D sim, 2 for 3D sim) - const int tangent_dim = tau.size(); - double mu_s, mu_k; friction_mu_for_evaluation(collision, no_mu, mu_s, mu_k); 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..07603f0f9 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,15 +75,15 @@ 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) + (point_line_distance(p, e0, e1) - dist_sqr) / MOLLIFIER_THRESHOLD_EPS / dist_sqr) * Math::mollifier( - (PointEdgeDistance::point_line_sqr_distance(p, e2, e1) + (point_line_distance(p, e2, e1) - dist_sqr) / MOLLIFIER_THRESHOLD_EPS / dist_sqr) * Math::mollifier( - (PointEdgeDistance::point_line_sqr_distance(p, e0, e2) + (point_line_distance(p, e0, e2) - dist_sqr) / MOLLIFIER_THRESHOLD_EPS / dist_sqr); } diff --git a/src/ipc/smooth_contact/distance/point_edge.cpp b/src/ipc/smooth_contact/distance/point_edge.cpp index 0a840d6bc..7d8310357 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..618fd7253 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 */, From 4761904ab8f1d5cbf258a228396e8d02d2512a6a Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Thu, 23 Apr 2026 16:30:43 -0400 Subject: [PATCH 03/21] Ensured all arithmetic in auto-generated and hand-written code uses the templated type for numeric literals --- src/ipc/distance/distance_type.cpp | 30 +- src/ipc/distance/edge_edge_mollifier.cpp | 78 +-- src/ipc/distance/line_line.cpp | 480 +++++++++--------- src/ipc/distance/point_line.cpp | 276 +++++------ src/ipc/distance/point_plane.cpp | 598 ++++++++++++----------- src/ipc/distance/point_plane.hpp | 4 +- tests/src/tests/benchmark_eigen.cpp | 189 ++++++- 7 files changed, 919 insertions(+), 736 deletions(-) diff --git a/src/ipc/distance/distance_type.cpp b/src/ipc/distance/distance_type.cpp index 509b9232b..253221f98 100644 --- a/src/ipc/distance/distance_type.cpp +++ b/src/ipc/distance/distance_type.cpp @@ -48,29 +48,29 @@ PointTriangleDistanceType point_triangle_distance_type( 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) { + if (param(0, 0) > 0 && param(0, 0) < 1 && param(1, 0) >= 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 (param(0, 1) > 0 && param(0, 1) < 1 && param(1, 1) >= 0) { 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 (param(0, 2) > 0 && param(0, 2) < 1 && param(1, 2) >= 0) { return PointTriangleDistanceType::P_E2; // edge 2 is the closest } - if (param(0, 0) <= 0.0 && param(0, 2) >= 1.0) { + if (param(0, 0) <= 0 && param(0, 2) >= 1) { return PointTriangleDistanceType::P_T0; // vertex 0 is the closest - } else if (param(0, 1) <= 0.0 && param(0, 0) >= 1.0) { + } else if (param(0, 1) <= 0 && param(0, 0) >= 1) { return PointTriangleDistanceType::P_T1; // vertex 1 is the closest - } else if (param(0, 2) <= 0.0 && param(0, 1) >= 1.0) { + } else if (param(0, 2) <= 0 && param(0, 1) >= 1) { return PointTriangleDistanceType::P_T2; // vertex 2 is the closest } else { return PointTriangleDistanceType::P_T; @@ -104,11 +104,11 @@ EdgeEdgeDistanceType edge_edge_distance_type( 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; } @@ -122,8 +122,8 @@ EdgeEdgeDistanceType edge_edge_distance_type( // compute the line parameters of the two closest points const T sN = (b * e - c * d); - T tN, tD; // tc = tN / tD - if (sN <= 0.0) { // sc < 0 ⟹ the s=0 edge is visible + 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 +134,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 +150,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 +161,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; diff --git a/src/ipc/distance/edge_edge_mollifier.cpp b/src/ipc/distance/edge_edge_mollifier.cpp index 735fdccee..dc46737b9 100644 --- a/src/ipc/distance/edge_edge_mollifier.cpp +++ b/src/ipc/distance/edge_edge_mollifier.cpp @@ -95,18 +95,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; @@ -150,39 +150,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; diff --git a/src/ipc/distance/line_line.cpp b/src/ipc/distance/line_line.cpp index 0d53cce25..1795657e9 100644 --- a/src/ipc/distance/line_line.cpp +++ b/src/ipc/distance/line_line.cpp @@ -45,32 +45,32 @@ namespace autogen { t44 = t11 * t18 + -(t12 * t17); t45 = t11 * t19 + -(t13 * t17); t46 = t12 * t19 + -(t13 * t18); - t75 = 1.0 / ((t44 * t44 + t45 * t45) + t46 * t46); + 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 * 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; + 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) * 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; + 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) * 2.0; + g[6] = t79 - t76 * (t17 + t46) * T(2); t18 = t11 * t16 + -(t13 * t14); - g[7] = -t83 + t76 * (t18 + t45) * 2.0; + g[7] = -t83 + t76 * (t18 + t45) * T(2); 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; + 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); } // This function was generated by the Symbolic Math Toolbox version 8.3. @@ -131,15 +131,15 @@ namespace autogen { 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; + 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); @@ -155,41 +155,41 @@ namespace autogen { 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); + 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 * 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; + 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; @@ -199,228 +199,234 @@ namespace autogen { t322 = (t59 + t65) * t157 * t58; t323 = (t59 + t73) * t157 * t58; t59 = (t65 + t73) * t157 * t58; - t325 = (t47 * 2.0 + -(t48 * 4.0)) * t157 * t58; + t325 = (t47 * T(2) + -(t48 * T(4))) * t157 * t58; t53 = -t157 * t58; - t56 = t48 * 2.0 - t47 * 4.0; + t56 = t48 * T(2) - t47 * T(4); t326 = t53 * t56; - t327 = (t49 * 2.0 + -(t50 * 4.0)) * t157 * t58; - t55 = t50 * 2.0 - t49 * 4.0; + t327 = (t49 * T(2) + -(t50 * T(4))) * t157 * t58; + t55 = t50 * T(2) - t49 * T(4); t328 = t53 * t55; - t329 = (t51 * 2.0 + -(t52 * 4.0)) * t157 * t58; - t54 = t52 * 2.0 - t51 * 4.0; + 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 * 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; + 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 * 2.0) + t312) + -t340) + b_t519_tmp * 2.0) - + t519_tmp * 2.0; + 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 * 2.0 + t313) + -t343) + b_t520_tmp * 2.0) - + -(t520_tmp * 2.0); + 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 * 2.0 + t315) + -t342) + -(b_t521_tmp * 2.0)) - + t521_tmp * 2.0; + 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 * 2.0) + t310) + -t341) - + -(b_t522_tmp * 2.0)) - + -(t522_tmp * 2.0); + 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 * 2.0) + t311) + -t345) + b_t523_tmp * 2.0) - + t523_tmp * 2.0; + (((-(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 * 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) + 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 * 2.0) + -t265) + t329) + t367) + t386) + t145 = ((((-(t99 * t107 * t156 * T(2)) + -t265) + t329) + t367) + t386) + -t503; - t144 = ((((-(t100 * t108 * t156 * 2.0) + -t267) + t325) + t360) + -t399) + t144 = + ((((-(t100 * t108 * t156 * T(2)) + -t267) + t325) + t360) + -t399) + t499; - t143 = ((((-(t99 * t109 * t156 * 2.0) + t267) + t335) + -t362) + t398) + t143 = ((((-(t99 * t109 * t156 * T(2)) + t267) + t335) + -t362) + t398) + t500; - t52 = ((((-(t98 * t108 * t156 * 2.0) + t265) + t339) + -t27) + -t387) + t52 = ((((-(t98 * t108 * t156 * T(2)) + 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) + ((((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 * 2.0 + t278) + -t302) + -t38) + -t39) + t40; - t48 = ((((-(t100 * t137 * t156 * 2.0) + -t263) + -t299) + -t65) + t73) + 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 * 2.0 + t262) + -t300) + t356) + -t446) + t47 = ((((t99 * t137 * t156 * T(2) + t262) + -t300) + t356) + -t446) + -t497; - t73 = ((((t100 * t138 * t156 * 2.0 + t264) + -t298) + -t382) + -t423) + t73 = ((((t100 * t138 * t156 * T(2) + t264) + -t298) + -t382) + -t423) + t495; - t65 = ((((-(t109 * t141 * t156 * 2.0) + t279) + -t301) + t408) + -t441) + t65 = ((((-(t109 * t141 * t156 * T(2)) + t279) + -t301) + t408) + -t441) + t506; - t59 = ((((t98 * t138 * t156 * 2.0 + -t262) + -t300) + -t377) + t424) + t59 = ((((t98 * t138 * t156 * T(2) + -t262) + -t300) + -t377) + t424) + -t497; - t40 = ((((t99 * t139 * t156 * 2.0 + -t264) + -t298) + -t353) + -t451) + t40 = ((((t99 * t139 * t156 * T(2) + -t264) + -t298) + -t353) + -t451) + t495; - t39 = ((((-(t107 * t141 * t156 * 2.0) + -t277) + -t303) + t403) + t442) + t39 = ((((-(t107 * t141 * t156 * T(2)) + -t277) + -t303) + t403) + t442) + -t508; - t38 = ((((-(t108 * t142 * t156 * 2.0) + -t279) + -t301) + t371) + -t477) + t38 = + ((((-(t108 * t142 * t156 * T(2)) + -t279) + -t301) + t371) + -t477) + t506; - t37 = ((((-(t108 * t140 * t156 * 2.0) + t277) + -t303) + -t374) + -t472) + t37 = + ((((-(t108 * t140 * t156 * T(2)) + t277) + -t303) + -t374) + -t472) + -t508; - t36 = ((((t98 * t142 * t156 * 2.0 + t271) + t328) + -t359) + t437) + t36 = ((((t98 * t142 * t156 * T(2) + t271) + t328) + -t359) + t437) + -t492; - t35 = ((((-(t109 * t137 * t156 * 2.0) + t270) + t328) + -t368) + -t428) + t35 = ((((-(t109 * t137 * t156 * T(2)) + t270) + t328) + -t368) + -t428) + -t492; - t28 = ((((t100 * t140 * t156 * 2.0 + -t271) + -t327) + t369) + -t427) + t28 = ((((t100 * t140 * t156 * T(2) + -t271) + -t327) + t369) + -t427) + -t491; - t27 = ((((-(t98 * t141 * t156 * 2.0) + -t269) + t330) + t387) + -t433) + t27 = ((((-(t98 * t141 * t156 * T(2)) + -t269) + t330) + t387) + -t433) + t504; - t26 = ((((t109 * t138 * t156 * 2.0 + -t272) + t326) + -t398) + t432) + t26 = ((((t109 * t138 * t156 * T(2) + -t272) + t326) + -t398) + t432) + -t500; - t13 = ((((-(t107 * t139 * t156 * 2.0) + -t270) + -t327) + t358) + t438) + t13 = ((((-(t107 * t139 * t156 * T(2)) + -t270) + -t327) + t358) + t438) + -t491; - t12 = ((((-(t99 * t142 * t156 * 2.0) + -t273) + t326) + t362) + t467) + t12 = ((((-(t99 * t142 * t156 * T(2)) + -t273) + t326) + t362) + t467) + -t500; - t11 = ((((-(t99 * t140 * t156 * 2.0) + t269) + -t329) + -t367) + t455) + t11 = ((((-(t99 * t140 * t156 * T(2)) + t269) + -t329) + -t367) + t455) + t503; - t16 = ((((t107 * t138 * t156 * 2.0 + t268) + -t329) + -t386) + -t434) + t16 = ((((t107 * t138 * t156 * T(2) + t268) + -t329) + -t386) + -t434) + t503; - t15 = ((((-(t100 * t141 * t156 * 2.0) + t273) + -t325) + t399) + t431) + t15 = ((((-(t100 * t141 * t156 * T(2)) + t273) + -t325) + t399) + t431) + -t499; - t14 = ((((t108 * t139 * t156 * 2.0 + t272) + -t325) + -t360) + t468) + t14 = ((((t108 * t139 * t156 * T(2) + t272) + -t325) + -t360) + t468) + -t499; - t105 = ((((-(t139 * t140 * t156 * 2.0) + t275) + t327) + t427) + -t438) + t105 = ((((-(t139 * t140 * t156 * T(2)) + t275) + t327) + t427) + -t438) + t491; - t103 = ((((t138 * t140 * t156 * 2.0 + -t274) + t329) + t434) + -t455) + t103 = ((((t138 * t140 * t156 * T(2) + -t274) + t329) + t434) + -t455) + -t503; - t101 = ((((-(t137 * t142 * t156 * 2.0) + -t275) + t337) + t428) + -t437) + t101 = + ((((-(t137 * t142 * t156 * T(2)) + -t275) + t337) + t428) + -t437) + t492; - t58 = ((((t139 * t141 * t156 * 2.0 + -t276) + t325) + -t431) + -t468) + t58 = ((((t139 * t141 * t156 * T(2) + -t276) + t325) + -t431) + -t468) + t499; - t57 = ((((t137 * t141 * t156 * 2.0 + t274) + t339) + t433) + -t456) + t57 = ((((t137 * t141 * t156 * T(2) + t274) + t339) + t433) + -t456) + -t504; - t56 = ((((t138 * t142 * t156 * 2.0 + t276) + t335) + -t432) + -t467) + t56 = ((((t138 * t142 * t156 * T(2) + t276) + t335) + -t432) + -t467) + t500; t55 = -t315 + t342; - H[0] = (t55 + t142 * t142 * t156 * 2.0) - t521_tmp * 4.0; + H[0] = (t55 + t142 * t142 * t156 * T(2)) - t521_tmp * T(4); H[1] = t53; H[2] = t148; H[3] = t521; @@ -434,7 +440,7 @@ namespace autogen { H[11] = t36; H[12] = t53; t54 = -t314 + t348; - H[13] = (t54 + t141 * t141 * t156 * 2.0) - t524_tmp * 4.0; + H[13] = (t54 + t141 * t141 * t156 * T(2)) - t524_tmp * T(4); H[14] = t544; H[15] = t65; H[16] = t524; @@ -448,7 +454,7 @@ namespace autogen { H[24] = t148; H[25] = t544; t53 = -t313 + t343; - H[26] = (t53 + t140 * t140 * t156 * 2.0) + t520_tmp * 4.0; + H[26] = (t53 + t140 * t140 * t156 * T(2)) + t520_tmp * T(4); H[27] = t51; H[28] = t37; H[29] = t520; @@ -461,7 +467,7 @@ namespace autogen { H[36] = t521; H[37] = t65; H[38] = t51; - H[39] = (t55 + t109 * t109 * t156 * 2.0) + b_t521_tmp * 4.0; + H[39] = (t55 + t109 * t109 * t156 * T(2)) + b_t521_tmp * T(4); H[40] = t530; H[41] = t526; H[42] = t537; @@ -474,7 +480,7 @@ namespace autogen { H[49] = t524; H[50] = t37; H[51] = t530; - H[52] = (t54 + t108 * t108 * t156 * 2.0) + b_t524_tmp * 4.0; + H[52] = (t54 + t108 * t108 * t156 * T(2)) + b_t524_tmp * T(4); H[53] = t529; H[54] = t14; H[55] = t539; @@ -487,7 +493,7 @@ namespace autogen { H[62] = t520; H[63] = t526; H[64] = t529; - H[65] = (t53 + t107 * t107 * t156 * 2.0) - b_t520_tmp * 4.0; + H[65] = (t53 + t107 * t107 * t156 * T(2)) - b_t520_tmp * T(4); H[66] = t13; H[67] = t16; H[68] = t535; @@ -501,7 +507,7 @@ namespace autogen { H[76] = t14; H[77] = t13; t55 = -t312 + t340; - H[78] = (t55 + t139 * t139 * t156 * 2.0) - t519_tmp * 4.0; + H[78] = (t55 + t139 * t139 * t156 * T(2)) - t519_tmp * T(4); H[79] = t543; H[80] = t540; H[81] = t519; @@ -515,7 +521,7 @@ namespace autogen { H[89] = t16; H[90] = t543; t54 = -t311 + t345; - H[91] = (t54 + t138 * t138 * t156 * 2.0) - t523_tmp * 4.0; + H[91] = (t54 + t138 * t138 * t156 * T(2)) - t523_tmp * T(4); H[92] = t542; H[93] = t73; H[94] = t523; @@ -529,7 +535,7 @@ namespace autogen { H[102] = t540; H[103] = t542; t53 = -t310 + t341; - H[104] = (t53 + t137 * t137 * t156 * 2.0) + t522_tmp * 4.0; + H[104] = (t53 + t137 * t137 * t156 * T(2)) + t522_tmp * T(4); H[105] = t48; H[106] = t47; H[107] = t522; @@ -542,7 +548,7 @@ namespace autogen { H[114] = t519; H[115] = t73; H[116] = t48; - H[117] = (t55 + t100 * t100 * t156 * 2.0) - b_t519_tmp * 4.0; + H[117] = (t55 + t100 * t100 * t156 * T(2)) - b_t519_tmp * T(4); H[118] = t528; H[119] = t525; H[120] = t12; @@ -555,7 +561,7 @@ namespace autogen { H[127] = t523; H[128] = t47; H[129] = t528; - H[130] = (t54 + t99 * t99 * t156 * 2.0) - b_t523_tmp * 4.0; + H[130] = (t54 + t99 * t99 * t156 * T(2)) - b_t523_tmp * T(4); H[131] = t527; H[132] = t36; H[133] = t27; @@ -568,7 +574,7 @@ namespace autogen { H[140] = t522; H[141] = t525; H[142] = t527; - H[143] = (t53 + t98 * t98 * t156 * 2.0) + b_t522_tmp * 4.0; + H[143] = (t53 + t98 * t98 * t156 * T(2)) + b_t522_tmp * T(4); } // clang-format off diff --git a/src/ipc/distance/point_line.cpp b/src/ipc/distance/point_line.cpp index 6b5f70094..ac2ab24e5 100644 --- a/src/ipc/distance/point_line.cpp +++ b/src/ipc/distance/point_line.cpp @@ -14,19 +14,19 @@ void point_line_distance_gradient_2D( t13 = -v21 + v11; t14 = -v22 + v12; - t23 = 1.0 / (t13 * t13 + t14 * t14); + t23 = T(1) / (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; + 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) * 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; + 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); } // This function was generated by the Symbolic Math Toolbox version 8.3. @@ -47,24 +47,24 @@ void point_line_distance_gradient_3D( t23 = -v21 + v11; t24 = -v22 + v12; t25 = -v23 + v13; - t42 = 1.0 / ((t23 * t23 + t24 * t24) + t25 * t25); + 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 * 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); + 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)); } // This function was generated by the Symbolic Math Toolbox version 8.3. @@ -83,57 +83,57 @@ void point_line_distance_hessian_2D( t18 = -v22 + v02; t19 = -v21 + v11; t20 = -v22 + v12; - t21 = v11 * 2.0 + -(v21 * 2.0); - t22 = v12 * 2.0 + -(v22 * 2.0); + t21 = v11 * T(2) + -(v21 * T(2)); + t22 = v12 * T(2) + -(v22 * T(2)); t23 = t19 * t19; t24 = t20 * t20; - t31 = 1.0 / (t23 + t24); + 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 * 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; + 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 * 2.0) + t62) + -t65) + t76 * 2.0) - + t102_tmp * 2.0; + 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 * 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; + 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 * 2.0; + H[7] = t23 * t31 * T(2); H[8] = t96; H[9] = t94; H[10] = t98; @@ -141,7 +141,7 @@ void point_line_distance_hessian_2D( H[12] = t35; H[13] = t96; t35 = -t62 + t64; - H[14] = (t35 + t18 * t18 * t31 * 2.0) + t33 * 4.0; + H[14] = (t35 + t18 * t18 * t31 * T(2)) + t33 * T(4); H[15] = t19; H[16] = t79; H[17] = t77; @@ -149,21 +149,21 @@ void point_line_distance_hessian_2D( H[19] = t94; H[20] = t19; t33 = -t62 + t65; - H[21] = (t33 + t17 * t17 * t31 * 2.0) - t102_tmp * 4.0; + 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 * 2.0) + t20 * 4.0; + 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 * 2.0) - t76 * 4.0; + H[35] = (t33 + t15 * t15 * t31 * T(2)) - t76 * T(4); } // This function was generated by the Symbolic Math Toolbox version 8.3. @@ -190,9 +190,9 @@ void point_line_distance_hessian_3D( 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); + 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; @@ -202,49 +202,49 @@ void point_line_distance_hessian_3D( 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); + 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 * 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; + 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 * 2.0 + -(t19 * t37 * 2.0); - t226 = t20 * t36 * 2.0 + -(t22 * t37 * 2.0); - t227 = t23 * t36 * 2.0 + -(t25 * t37 * 2.0); + 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; @@ -266,13 +266,13 @@ void point_line_distance_hessian_3D( 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; + 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; @@ -284,37 +284,37 @@ void point_line_distance_hessian_3D( 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; + 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 * 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; + 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 * 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; + 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; @@ -345,7 +345,7 @@ void point_line_distance_hessian_3D( H[27] = t88; H[28] = t301; H[29] = t299; - H[30] = ((t235 * 2.0 + -t279) + t281) + t102 * (t42 + t43); + H[30] = ((t235 * T(2) + -t279) + t281) + t102 * (t42 + t43); H[31] = t37; H[32] = t245; H[33] = t217; @@ -355,7 +355,7 @@ void point_line_distance_hessian_3D( H[37] = t297; H[38] = t300; H[39] = t37; - H[40] = ((t251 * -2.0 + -t279) + t282) + t102 * (t41 + t43); + H[40] = ((t251 * -T(2) + -t279) + t282) + t102 * (t41 + t43); H[41] = t35; H[42] = t103; H[43] = t218; @@ -365,7 +365,7 @@ void point_line_distance_hessian_3D( H[47] = t295; H[48] = t245; H[49] = t35; - H[50] = ((t240 * -2.0 + -t279) + t283) + t102 * (t41 + t42); + H[50] = ((t240 * -T(2) + -t279) + t283) + t102 * (t41 + t42); H[51] = t26; H[52] = t225; H[53] = t104; @@ -375,7 +375,7 @@ void point_line_distance_hessian_3D( H[57] = t217; H[58] = t103; H[59] = t26; - H[60] = ((t229 * 2.0 + -t279) + t281) + t102 * (t39 + t40); + H[60] = ((t229 * T(2) + -t279) + t281) + t102 * (t39 + t40); H[61] = t213; H[62] = t311; H[63] = t303; @@ -385,7 +385,7 @@ void point_line_distance_hessian_3D( H[67] = t218; H[68] = t225; H[69] = t213; - H[70] = ((t248 * -2.0 + -t279) + t282) + t102 * (t38 + t40); + H[70] = ((t248 * -T(2) + -t279) + t282) + t102 * (t38 + t40); H[71] = t36; H[72] = t304; H[73] = t214; @@ -395,7 +395,7 @@ void point_line_distance_hessian_3D( H[77] = t104; H[78] = t311; H[79] = t36; - H[80] = ((t234 * -2.0 + -t279) + t283) + t102 * (t38 + t39); + H[80] = ((t234 * -T(2) + -t279) + t283) + t102 * (t38 + t39); } // clang-format off diff --git a/src/ipc/distance/point_plane.cpp b/src/ipc/distance/point_plane.cpp index 2a63369bd..141c48dc7 100644 --- a/src/ipc/distance/point_plane.cpp +++ b/src/ipc/distance/point_plane.cpp @@ -38,33 +38,33 @@ void point_plane_distance_gradient( t32 = t14 * t18 + -(t15 * t17); t33 = t14 * t19 + -(t16 * t17); t34 = t15 * t19 + -(t16 * t18); - t43 = 1.0 / ((t32 * t32 + t33 * t33) + t34 * t34); + 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 * 2.0; - g[1] = t33 * t43 * t45 * -2.0; - g[2] = t32 * t43 * t45 * 2.0; + 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 * 2.0 + t22 * t33 * 2.0) - - t45 * ((t34 + t12 * t22) - t13 * t21) * 2.0; + 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 * 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); + 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)); } // This function was generated by the Symbolic Math Toolbox version 8.3. @@ -115,30 +115,30 @@ void point_plane_distance_hessian( 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; + 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); @@ -154,241 +154,247 @@ void point_plane_distance_hessian( 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); + 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 * 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; + 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 * 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; + 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; @@ -401,7 +407,7 @@ void point_plane_distance_hessian( H[10] = t610; H[11] = t615; H[12] = t262; - H[13] = t150 * t202 * 2.0; + H[13] = t150 * t202 * T(2); H[14] = t261; H[15] = t621; H[16] = t607; @@ -414,7 +420,7 @@ void point_plane_distance_hessian( H[23] = t608; H[24] = t241; H[25] = t261; - H[26] = t149 * t202 * 2.0; + H[26] = t149 * t202 * T(2); H[27] = t624; H[28] = t623; H[29] = t605; @@ -427,9 +433,9 @@ void point_plane_distance_hessian( H[36] = t606; H[37] = t621; H[38] = t624; - H[39] = ((t191 * t191 * t202 * 2.0 + t196 * t196 * t204 * t206 * 2.0) + H[39] = ((t191 * t191 * t202 * T(2) + t196 * t196 * t204 * t206 * T(2)) - t646_tmp * (t45 + t46)) - + t191 * t196 * t203 * t205 * 4.0; + + t191 * t196 * t203 * t205 * T(4); H[40] = t99; H[41] = t85; H[42] = t638; @@ -442,9 +448,9 @@ void point_plane_distance_hessian( H[49] = t607; H[50] = t623; H[51] = t99; - H[52] = ((t190 * t190 * t202 * 2.0 + t200 * t200 * t204 * t206 * 2.0) + H[52] = ((t190 * t190 * t202 * T(2) + t200 * t200 * t204 * t206 * T(2)) - t646_tmp * (t44 + t46)) - + t190 * t200 * t203 * t205 * 4.0; + + t190 * t200 * t203 * t205 * T(4); H[53] = t90; H[54] = t316; H[55] = t71; @@ -457,9 +463,9 @@ void point_plane_distance_hessian( H[62] = t605; H[63] = t85; H[64] = t90; - H[65] = ((t189 * t189 * t202 * 2.0 + t197 * t197 * t204 * t206 * 2.0) + H[65] = ((t189 * t189 * t202 * T(2) + t197 * t197 * t204 * t206 * T(2)) - t646_tmp * (t44 + t45)) - - t189 * t197 * t203 * t205 * 4.0; + - t189 * t197 * t203 * t205 * T(4); H[66] = t321; H[67] = t11; H[68] = t636; @@ -472,9 +478,9 @@ void point_plane_distance_hessian( H[75] = t638; H[76] = t316; H[77] = t321; - H[78] = ((t130 * t130 * t202 * 2.0 + t194 * t194 * t204 * t206 * 2.0) + H[78] = ((t130 * t130 * t202 * T(2) + t194 * t194 * t204 * t206 * T(2)) - t646_tmp * (t42 + t43)) - + t130 * t194 * t203 * t205 * 4.0; + + t130 * t194 * t203 * t205 * T(4); H[79] = t631; H[80] = t627; H[81] = t633; @@ -487,9 +493,9 @@ void point_plane_distance_hessian( H[88] = t71; H[89] = t11; H[90] = t631; - H[91] = ((t129 * t129 * t202 * 2.0 + t199 * t199 * t204 * t206 * 2.0) + H[91] = ((t129 * t129 * t202 * T(2) + t199 * t199 * t204 * t206 * T(2)) - t646_tmp * (t41 + t43)) - + t129 * t199 * t203 * t205 * 4.0; + + t129 * t199 * t203 * t205 * T(4); H[92] = t629; H[93] = t645; H[94] = t634; @@ -502,9 +508,9 @@ void point_plane_distance_hessian( H[101] = t636; H[102] = t627; H[103] = t629; - H[104] = ((t128 * t128 * t202 * 2.0 + t195 * t195 * t204 * t206 * 2.0) + H[104] = ((t128 * t128 * t202 * T(2) + t195 * t195 * t204 * t206 * T(2)) - t646_tmp * (t41 + t42)) - - t128 * t195 * t203 * t205 * 4.0; + - t128 * t195 * t203 * t205 * T(4); H[105] = t77; H[106] = t131; H[107] = t632; @@ -517,9 +523,9 @@ void point_plane_distance_hessian( H[114] = t633; H[115] = t645; H[116] = t77; - H[117] = ((t127 * t127 * t202 * 2.0 + t192 * t192 * t204 * t206 * 2.0) + H[117] = ((t127 * t127 * t202 * T(2) + t192 * t192 * t204 * t206 * T(2)) - t646_tmp * (t39 + t40)) - + t127 * t192 * t203 * t205 * 4.0; + + t127 * t192 * t203 * t205 * T(4); H[118] = t630; H[119] = t626; H[120] = t610; @@ -532,9 +538,9 @@ void point_plane_distance_hessian( H[127] = t634; H[128] = t131; H[129] = t630; - H[130] = ((t126 * t126 * t202 * 2.0 + t198 * t198 * t204 * t206 * 2.0) + H[130] = ((t126 * t126 * t202 * T(2) + t198 * t198 * t204 * t206 * T(2)) - t646_tmp * (t38 + t40)) - + t126 * t198 * t203 * t205 * 4.0; + + t126 * t198 * t203 * t205 * T(4); H[131] = t628; H[132] = t615; H[133] = t608; @@ -547,9 +553,9 @@ void point_plane_distance_hessian( H[140] = t632; H[141] = t626; H[142] = t628; - H[143] = ((t125 * t125 * t202 * 2.0 + t193 * t193 * t204 * t206 * 2.0) + H[143] = ((t125 * t125 * t202 * T(2) + t193 * t193 * t204 * t206 * T(2)) - t646_tmp * (t38 + t39)) - - t125 * t193 * t203 * t205 * 4.0; + - t125 * t193 * t203 * t205 * T(4); } // clang-format off diff --git a/src/ipc/distance/point_plane.hpp b/src/ipc/distance/point_plane.hpp index c4102fd07..f2ca5d40f 100644 --- a/src/ipc/distance/point_plane.hpp +++ b/src/ipc/distance/point_plane.hpp @@ -65,7 +65,7 @@ inline Eigen::Vector3 point_plane_distance_gradient( Eigen::ConstRef> origin, Eigen::ConstRef> normal) { - return (2.0 / normal.squaredNorm()) * (p - origin).dot(normal) * normal; + return (T(2) / normal.squaredNorm()) * (p - origin).dot(normal) * normal; } /// @brief Compute the gradient of the distance between a point and a plane. @@ -101,7 +101,7 @@ inline Eigen::Matrix3 point_plane_distance_hessian( Eigen::ConstRef> origin, Eigen::ConstRef> normal) { - return ((2.0 / normal.squaredNorm()) * normal) * normal.transpose(); + return ((T(2) / normal.squaredNorm()) * normal) * normal.transpose(); } /// @brief Compute the hessian of the distance between a point and a plane. diff --git a/tests/src/tests/benchmark_eigen.cpp b/tests/src/tests/benchmark_eigen.cpp index ac1562c99..0c89b30c7 100644 --- a/tests/src/tests/benchmark_eigen.cpp +++ b/tests/src/tests/benchmark_eigen.cpp @@ -4,6 +4,7 @@ #include #include #include +#include #include @@ -53,6 +54,49 @@ point_point_distance_copy_Nd(const VectorMax3d& p0, const VectorMax3d& p1) return (p1 - p0).squaredNorm(); } +// 1. The fast worker (Compiler knows N) +template +inline double fast_dist( + const Eigen::MatrixBase& p0, + const Eigen::MatrixBase& p1) +{ + return (p1.template head() - p0.template head()).squaredNorm(); +} + +// 2. The dispatcher +double point_point_distance_dispatch( + Eigen::ConstRef p0, Eigen::ConstRef p1) +{ + // By the time we reach the worker, N is a constant + if (p0.size() == 3) { + return fast_dist<3>(p0, p1); + } + if (p0.size() == 2) { + return fast_dist<2>(p0, p1); + } + return (p1 - p0).squaredNorm(); +} + +// 2. The dispatcher +template +double point_point_distance_templated( + Eigen::Matrix p0, + Eigen::Matrix p1) +{ + return (p1 - p0).squaredNorm(); +} + +inline double point_point_distance_templated( + Eigen::ConstRef p0, Eigen::ConstRef p1) +{ + if (p0.size() == 3) { + return point_point_distance_templated<3, 3>(p0, p1); + } + // else if (p0.size() == 2) + return point_point_distance_templated<2, 2>(p0, p1); + // return point_point_distance_templated<-1, 3>(p0, p1); +} + TEST_CASE("Templated vs Eigen::Ref (PP)", "[!benchmark][eigen][pp][ref]") { Eigen::MatrixXd V = Eigen::MatrixXd::Random(100, 3); @@ -92,11 +136,54 @@ TEST_CASE("Templated vs Eigen::Ref (PP)", "[!benchmark][eigen][pp][ref]") { return point_point_distance_copy_Nd(V.row(p0i), V.row(p1i)); }; + + BENCHMARK("Dispatch Nd") + { + return point_point_distance_dispatch(V.row(p0i), V.row(p1i)); + }; + + BENCHMARK("Templated") + { + return point_point_distance_templated<3, 3>(V.row(p0i), V.row(p1i)); + }; + + BENCHMARK("Templated(Nd)") + { + return point_point_distance_templated(V.row(p0i), V.row(p1i)); + }; } // ============================================================================= double line_line_distance_ref( + 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(); +} + +template < + typename DerivedA, + typename DerivedB, + typename DerivedC, + typename DerivedD> +double line_line_distance_templated( + const Eigen::MatrixBase& ea0, + const Eigen::MatrixBase& ea1, + const Eigen::MatrixBase& eb0, + const Eigen::MatrixBase& eb1) +{ + const Eigen::Vector3d normal = (ea1 - ea0).template block<3, 1>(0, 0).cross( + (eb1 - eb0).template block<3, 1>(0, 0)); + const double line_to_line = (eb0 - ea0).dot(normal); + return line_to_line * line_to_line / normal.squaredNorm(); +} + +double line_line_distance_row_ref( const Eigen::Ref>& ea0, const Eigen::Ref>& ea1, const Eigen::Ref>& eb0, @@ -120,20 +207,68 @@ double line_line_distance_copy( TEST_CASE("Templated vs Eigen::Ref (LL)", "[!benchmark][eigen][ll][ref]") { - const Eigen::MatrixXd V = Eigen::MatrixXd::Random(100, 3); + const int N = 100; + const Eigen::MatrixXd V = Eigen::MatrixXd::Random(N, 3); + const Eigen::Matrix VR = V; + + // Use a heap-allocated vector of random indices to defeat LICM. The prime + // size means i % M has no power-of-2 period, and the heap pointer prevents + // the compiler from tracing provenance back to V. Catch2 has no + // DoNotOptimize, so this is the most portable approach. + constexpr int M = 997; // prime + Eigen::ArrayXi idx = + Eigen::ArrayXi::Random(M).abs().unaryExpr([&](int x) { return x % N; }); + + BENCHMARK("Templated", i) + { + return line_line_distance_templated( + V.row(idx[i % M]), V.row(idx[(i + 1) % M]), V.row(idx[(i + 2) % M]), + V.row(idx[(i + 3) % M])); + }; - int ea0i = 0, ea1i = 50, eb0i = 75, eb1i = 99; + BENCHMARK("Lib", i) + { + return line_line_distance( + V.row(idx[i % M]), V.row(idx[(i + 1) % M]), V.row(idx[(i + 2) % M]), + V.row(idx[(i + 3) % M])); + }; - 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("Ref", i) + { + return line_line_distance_ref( + V.row(idx[i % M]), V.row(idx[(i + 1) % M]), V.row(idx[(i + 2) % M]), + V.row(idx[(i + 3) % M])); + }; - BENCHMARK("Templated") { return line_line_distance(ea0, ea1, eb0, eb1); }; + BENCHMARK("Ref(Copy)", i) + { + const Eigen::Vector3d ea0 = V.row(idx[(i + 0) % M]); + const Eigen::Vector3d ea1 = V.row(idx[(i + 1) % M]); + const Eigen::Vector3d eb0 = V.row(idx[(i + 2) % M]); + const Eigen::Vector3d eb1 = V.row(idx[(i + 3) % M]); + return line_line_distance_ref(ea0, ea1, eb0, eb1); + }; - BENCHMARK("Ref") { return line_line_distance_ref(ea0, ea1, eb0, eb1); }; + BENCHMARK("Ref (VR)", i) + { + return line_line_distance( + VR.row(idx[i % M]), VR.row(idx[(i + 1) % M]), + VR.row(idx[(i + 2) % M]), VR.row(idx[(i + 3) % M])); + }; + + BENCHMARK("RowRef", i) + { + return line_line_distance_row_ref( + V.row(idx[i % M]), V.row(idx[(i + 1) % M]), V.row(idx[(i + 2) % M]), + V.row(idx[(i + 3) % M])); + }; - BENCHMARK("Copy") { return line_line_distance_copy(ea0, ea1, eb0, eb1); }; + BENCHMARK("Copy", i) + { + return line_line_distance_copy( + V.row(idx[i % M]), V.row(idx[(i + 1) % M]), V.row(idx[(i + 2) % M]), + V.row(idx[(i + 3) % M])); + }; } // ============================================================================= @@ -178,3 +313,39 @@ TEST_CASE("Return type", "[!benchmark][eigen][return]") return hess; }; } + +// ============================================================================= + +TEST_CASE("Float vs Double: Distance", "[!benchmark][eigen][float]") +{ + const Eigen::MatrixXf Vf = Eigen::MatrixXf::Random(100, 3); + const Eigen::MatrixXd Vd = Vf.cast(); + + BENCHMARK("float") + { + return edge_edge_distance( + Vf.row(0), Vf.row(50), Vf.row(75), Vf.row(99)); + }; + BENCHMARK("double") + { + return edge_edge_distance( + Vd.row(0), Vd.row(50), Vd.row(75), Vd.row(99)); + }; +} + +TEST_CASE("Float vs Double: Hessian", "[!benchmark][eigen][float]") +{ + const Eigen::MatrixXf Vf = Eigen::MatrixXf::Random(100, 3); + const Eigen::MatrixXd Vd = Vf.cast(); + + BENCHMARK("float") + { + return edge_edge_distance_hessian( + Vf.row(0), Vf.row(50), Vf.row(75), Vf.row(99)); + }; + BENCHMARK("double") + { + return edge_edge_distance_hessian( + Vd.row(0), Vd.row(50), Vd.row(75), Vd.row(99)); + }; +} From 68e77b326ec8075c07a0c1e8d3d2896461346034 Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Fri, 24 Apr 2026 11:11:00 -0400 Subject: [PATCH 04/21] Templatize barrier functions and classes for float/double support - Convert barrier functions and barrier class hierarchy to use templates - Update all usages to specify template parameters where needed - Update Python bindings and tests to construct template barrier types - Extend benchmarks to compare float and double barrier performance - Improves support for mixed-precision and SIMD optimizations --- python/src/barrier/barrier.cpp | 20 +-- src/ipc/barrier/barrier.cpp | 147 ++++++++++++++--------- src/ipc/barrier/barrier.hpp | 94 ++++++++------- src/ipc/potentials/barrier_potential.cpp | 2 +- src/ipc/potentials/barrier_potential.hpp | 3 +- tests/src/tests/barrier/test_barrier.cpp | 24 ++-- tests/src/tests/benchmark_eigen.cpp | 71 +++++++++-- 7 files changed, 229 insertions(+), 132 deletions(-) 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/src/ipc/barrier/barrier.cpp b/src/ipc/barrier/barrier.cpp index 0d5013247..7f0c4ff47 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,149 @@ 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 +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); +// ============================================================================ + } // namespace ipc diff --git a/src/ipc/barrier/barrier.hpp b/src/ipc/barrier/barrier.hpp index e6e3a30c9..d347cf026 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 T = 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 + T operator()(const T d, const T dhat) const override { - return BarrierT::operator()(d / dhat, 1.0); + return BarrierT::operator()(d / dhat, T(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 + T first_derivative(const T d, const T dhat) const override { - return BarrierT::first_derivative(d / dhat, 1.0) / dhat; + return BarrierT::first_derivative(d / dhat, T(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 + T second_derivative(const T d, const T dhat) const override { - return BarrierT::second_derivative(d / dhat, 1.0) / (dhat * dhat); + return BarrierT::second_derivative(d / dhat, T(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 + T units(const T dhat) const override { - return 1.0; // The normalized barrier is dimensionless. + return T(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/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 06611355f..0b950b8d6 100644 --- a/src/ipc/potentials/barrier_potential.hpp +++ b/src/ipc/potentials/barrier_potential.hpp @@ -136,7 +136,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/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 0c89b30c7..08681d654 100644 --- a/tests/src/tests/benchmark_eigen.cpp +++ b/tests/src/tests/benchmark_eigen.cpp @@ -4,6 +4,7 @@ #include #include #include +#include #include #include @@ -318,34 +319,84 @@ TEST_CASE("Return type", "[!benchmark][eigen][return]") TEST_CASE("Float vs Double: Distance", "[!benchmark][eigen][float]") { - const Eigen::MatrixXf Vf = Eigen::MatrixXf::Random(100, 3); + constexpr int N = 100, M = 1000; + const Eigen::MatrixXf Vf = Eigen::MatrixXf::Random(N, 3); const Eigen::MatrixXd Vd = Vf.cast(); BENCHMARK("float") { - return edge_edge_distance( - Vf.row(0), Vf.row(50), Vf.row(75), Vf.row(99)); + float sum = 0.0; + for (int i = 0; i < M; ++i) { + sum += barrier( + edge_edge_distance( + Vf.row(i % N), Vf.row((50 + i) % N), Vf.row((75 + i) % N), + Vf.row((99 + i) % N)), + 1.0f); + } + return sum; }; BENCHMARK("double") { - return edge_edge_distance( - Vd.row(0), Vd.row(50), Vd.row(75), Vd.row(99)); + double sum = 0.0; + for (int i = 0; i < M; ++i) { + sum += barrier( + edge_edge_distance( + Vd.row(i % N), Vd.row((50 + i) % N), Vd.row((75 + i) % N), + Vd.row((99 + i) % N)), + 1.0); + } + return sum; }; } TEST_CASE("Float vs Double: Hessian", "[!benchmark][eigen][float]") { - const Eigen::MatrixXf Vf = Eigen::MatrixXf::Random(100, 3); + constexpr int N = 100, M = 1000; + const Eigen::MatrixXf Vf = Eigen::MatrixXf::Random(N, 3); const Eigen::MatrixXd Vd = Vf.cast(); BENCHMARK("float") { - return edge_edge_distance_hessian( - Vf.row(0), Vf.row(50), Vf.row(75), Vf.row(99)); + Eigen::Matrix hess = + Eigen::Matrix::Zero(); + for (int i = 0; i < M; ++i) { + auto d = edge_edge_distance( + Vf.row(i % N), Vf.row((50 + i) % N), Vf.row((75 + i) % N), + Vf.row((99 + i) % N)); + auto grad_d = edge_edge_distance_gradient( + Vf.row(i % N), Vf.row((50 + i) % N), Vf.row((75 + i) % N), + Vf.row((99 + i) % N)); + auto hess_d = edge_edge_distance_hessian( + Vf.row(i % N), Vf.row((50 + i) % N), Vf.row((75 + i) % N), + Vf.row((99 + i) % N)); + + auto db = barrier_first_derivative(d, 1.0f); + auto d2b = barrier_second_derivative(d, 1.0f); + + hess += db * hess_d + (d2b * grad_d) * grad_d.transpose(); + } + return hess; }; BENCHMARK("double") { - return edge_edge_distance_hessian( - Vd.row(0), Vd.row(50), Vd.row(75), Vd.row(99)); + Eigen::Matrix hess = + Eigen::Matrix::Zero(); + for (int i = 0; i < M; ++i) { + auto d = edge_edge_distance( + Vd.row(i % N), Vd.row((50 + i) % N), Vd.row((75 + i) % N), + Vd.row((99 + i) % N)); + auto grad_d = edge_edge_distance_gradient( + Vd.row(i % N), Vd.row((50 + i) % N), Vd.row((75 + i) % N), + Vd.row((99 + i) % N)); + auto hess_d = edge_edge_distance_hessian( + Vd.row(i % N), Vd.row((50 + i) % N), Vd.row((75 + i) % N), + Vd.row((99 + i) % N)); + + auto db = barrier_first_derivative(d, 1.0); + auto d2b = barrier_second_derivative(d, 1.0); + + hess += db * hess_d + (d2b * grad_d) * grad_d.transpose(); + } + return hess; }; } From 51351b95c581deb0e722add0fc745e069da41c8a Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Mon, 4 May 2026 14:52:56 -0400 Subject: [PATCH 05/21] Replace EigenExpression concept with typename - Set default CMAKE_CXX_STANDARD to 17 for top-level projects - Comment out EigenExpression concept in utils/eigen_ext.hpp - Update Eigen-expression wrapper templates to use typename parameters and add enable_if where appropriate --- CMakeLists.txt | 2 +- src/ipc/distance/distance_type.hpp | 64 +++---- src/ipc/distance/edge_edge.hpp | 56 +++--- src/ipc/distance/edge_edge_mollifier.hpp | 230 +++++++++++------------ src/ipc/distance/line_line.hpp | 58 +++--- src/ipc/distance/point_edge.hpp | 15 +- src/ipc/distance/point_line.hpp | 15 +- src/ipc/distance/point_plane.hpp | 39 ++-- src/ipc/distance/point_point.hpp | 6 +- src/ipc/distance/point_triangle.hpp | 24 +-- src/ipc/distance/signed/line_line.hpp | 24 +-- src/ipc/distance/signed/point_line.hpp | 15 +- src/ipc/distance/signed/point_plane.hpp | 24 +-- src/ipc/geometry/normal.hpp | 128 +++++-------- src/ipc/utils/eigen_ext.hpp | 12 +- 15 files changed, 325 insertions(+), 387 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 1027fa022..d52d55134 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -93,7 +93,7 @@ mark_as_advanced(IPC_TOOLKIT_VERTEX_DERIVATIVE_LAYOUT) # This is an advanced opt # Set default minimum C++ standard if(IPC_TOOLKIT_TOPLEVEL_PROJECT) - set(CMAKE_CXX_STANDARD 20) + set(CMAKE_CXX_STANDARD 17) set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_EXTENSIONS OFF) endif() diff --git a/src/ipc/distance/distance_type.hpp b/src/ipc/distance/distance_type.hpp index d6d680cd9..e4dffec3f 100644 --- a/src/ipc/distance/distance_type.hpp +++ b/src/ipc/distance/distance_type.hpp @@ -106,30 +106,30 @@ EdgeEdgeDistanceType edge_edge_parallel_distance_type( // --- EigenExpression wrappers --- -template +template inline PointEdgeDistanceType point_edge_distance_type( - const Eigen::MatrixBase& p, - const Eigen::MatrixBase& e0, - const Eigen::MatrixBase& e1) + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& e0, + const Eigen::MatrixBase& e1) { - using T = typename D0::Scalar; + using T = typename DerivedP::Scalar; return point_edge_distance_type( Eigen::Ref>(p), Eigen::Ref>(e0), Eigen::Ref>(e1)); } template < - EigenExpression D0, - EigenExpression D1, - EigenExpression D2, - EigenExpression D3> + typename DerivedP, + typename DerivedT0, + typename DerivedT1, + typename DerivedT2> inline PointTriangleDistanceType point_triangle_distance_type( - const Eigen::MatrixBase& p, - const Eigen::MatrixBase& t0, - const Eigen::MatrixBase& t1, - const Eigen::MatrixBase& t2) + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& t0, + const Eigen::MatrixBase& t1, + const Eigen::MatrixBase& t2) { - using T = typename D0::Scalar; + using T = typename DerivedP::Scalar; return point_triangle_distance_type( Eigen::Ref>(p), Eigen::Ref>(t0), @@ -138,17 +138,17 @@ inline PointTriangleDistanceType point_triangle_distance_type( } template < - EigenExpression D0, - EigenExpression D1, - EigenExpression D2, - EigenExpression D3> + typename DerivedEA0, + typename DerivedEA1, + typename DerivedEB0, + typename DerivedEB1> inline EdgeEdgeDistanceType edge_edge_distance_type( - const Eigen::MatrixBase& ea0, - const Eigen::MatrixBase& ea1, - const Eigen::MatrixBase& eb0, - const Eigen::MatrixBase& eb1) + const Eigen::MatrixBase& ea0, + const Eigen::MatrixBase& ea1, + const Eigen::MatrixBase& eb0, + const Eigen::MatrixBase& eb1) { - using T = typename D0::Scalar; + using T = typename DerivedEA0::Scalar; return edge_edge_distance_type( Eigen::Ref>(ea0), Eigen::Ref>(ea1), @@ -157,17 +157,17 @@ inline EdgeEdgeDistanceType edge_edge_distance_type( } template < - EigenExpression D0, - EigenExpression D1, - EigenExpression D2, - EigenExpression D3> + typename DerivedEA0, + typename DerivedEA1, + typename DerivedEB0, + typename DerivedEB1> inline EdgeEdgeDistanceType edge_edge_parallel_distance_type( - const Eigen::MatrixBase& ea0, - const Eigen::MatrixBase& ea1, - const Eigen::MatrixBase& eb0, - const Eigen::MatrixBase& eb1) + const Eigen::MatrixBase& ea0, + const Eigen::MatrixBase& ea1, + const Eigen::MatrixBase& eb0, + const Eigen::MatrixBase& eb1) { - using T = typename D0::Scalar; + using T = typename DerivedEA0::Scalar; return edge_edge_parallel_distance_type( Eigen::Ref>(ea0), Eigen::Ref>(ea1), diff --git a/src/ipc/distance/edge_edge.hpp b/src/ipc/distance/edge_edge.hpp index 92ad1692b..c68ef40d1 100644 --- a/src/ipc/distance/edge_edge.hpp +++ b/src/ipc/distance/edge_edge.hpp @@ -56,19 +56,19 @@ Eigen::Matrix edge_edge_distance_hessian( // --- EigenExpression wrappers --- template < - EigenExpression DerivedEa0, - EigenExpression DerivedEa1, - EigenExpression DerivedEb0, - EigenExpression DerivedEb1> + 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, + const Eigen::MatrixBase& ea0, + const Eigen::MatrixBase& ea1, + const Eigen::MatrixBase& eb0, + const Eigen::MatrixBase& eb1, EdgeEdgeDistanceType dtype = EdgeEdgeDistanceType::AUTO) -> - typename DerivedEa0::Scalar + typename DerivedEA0::Scalar { - using T = typename DerivedEa0::Scalar; + using T = typename DerivedEA0::Scalar; return edge_edge_distance( Eigen::Ref>(ea0), Eigen::Ref>(ea1), @@ -77,17 +77,17 @@ inline auto edge_edge_distance( } template < - EigenExpression DerivedEa0, - EigenExpression DerivedEa1, - EigenExpression DerivedEb0, - EigenExpression DerivedEb1> + 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, + const Eigen::MatrixBase& ea0, + const Eigen::MatrixBase& ea1, + const Eigen::MatrixBase& eb0, + const Eigen::MatrixBase& eb1, EdgeEdgeDistanceType dtype = EdgeEdgeDistanceType::AUTO) - -> Eigen::Vector + -> Eigen::Vector { using T = typename DerivedEa0::Scalar; return edge_edge_distance_gradient( @@ -98,17 +98,17 @@ inline auto edge_edge_distance_gradient( } template < - EigenExpression DerivedEa0, - EigenExpression DerivedEa1, - EigenExpression DerivedEb0, - EigenExpression DerivedEb1> + 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, + const Eigen::MatrixBase& ea0, + const Eigen::MatrixBase& ea1, + const Eigen::MatrixBase& eb0, + const Eigen::MatrixBase& eb1, EdgeEdgeDistanceType dtype = EdgeEdgeDistanceType::AUTO) - -> Eigen::Matrix + -> Eigen::Matrix { using T = typename DerivedEa0::Scalar; return edge_edge_distance_hessian( diff --git a/src/ipc/distance/edge_edge_mollifier.hpp b/src/ipc/distance/edge_edge_mollifier.hpp index 419274261..559c60bde 100644 --- a/src/ipc/distance/edge_edge_mollifier.hpp +++ b/src/ipc/distance/edge_edge_mollifier.hpp @@ -320,17 +320,17 @@ Eigen::Vector edge_edge_mollifier_threshold_gradient( // --- EigenExpression wrappers --- template < - EigenExpression DerivedEa0, - EigenExpression DerivedEa1, - EigenExpression DerivedEb0, - EigenExpression DerivedEb1> + 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) -> typename DerivedEa0::Scalar + const Eigen::MatrixBase& ea0, + const Eigen::MatrixBase& ea1, + const Eigen::MatrixBase& eb0, + const Eigen::MatrixBase& eb1) -> typename DerivedEA0::Scalar { - using T = typename DerivedEa0::Scalar; + using T = typename DerivedEA0::Scalar; return edge_edge_cross_squarednorm( Eigen::Ref>(ea0), Eigen::Ref>(ea1), @@ -339,18 +339,18 @@ inline auto edge_edge_cross_squarednorm( } template < - EigenExpression DerivedEa0, - EigenExpression DerivedEa1, - EigenExpression DerivedEb0, - EigenExpression DerivedEb1> + 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) - -> Eigen::Vector + const Eigen::MatrixBase& ea0, + const Eigen::MatrixBase& ea1, + const Eigen::MatrixBase& eb0, + const Eigen::MatrixBase& eb1) + -> Eigen::Vector { - using T = typename DerivedEa0::Scalar; + using T = typename DerivedEA0::Scalar; return edge_edge_cross_squarednorm_gradient( Eigen::Ref>(ea0), Eigen::Ref>(ea1), @@ -359,18 +359,18 @@ inline auto edge_edge_cross_squarednorm_gradient( } template < - EigenExpression DerivedEa0, - EigenExpression DerivedEa1, - EigenExpression DerivedEb0, - EigenExpression DerivedEb1> + 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) - -> Eigen::Matrix + const Eigen::MatrixBase& ea0, + const Eigen::MatrixBase& ea1, + const Eigen::MatrixBase& eb0, + const Eigen::MatrixBase& eb1) + -> Eigen::Matrix { - using T = typename DerivedEa0::Scalar; + using T = typename DerivedEA0::Scalar; return edge_edge_cross_squarednorm_hessian( Eigen::Ref>(ea0), Eigen::Ref>(ea1), @@ -379,18 +379,18 @@ inline auto edge_edge_cross_squarednorm_hessian( } template < - EigenExpression DerivedEa0, - EigenExpression DerivedEa1, - EigenExpression DerivedEb0, - EigenExpression DerivedEb1> + 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) -> typename DerivedEa0::Scalar + const Eigen::MatrixBase& ea0, + const Eigen::MatrixBase& ea1, + const Eigen::MatrixBase& eb0, + const Eigen::MatrixBase& eb1, + const typename DerivedEA0::Scalar eps_x) -> typename DerivedEA0::Scalar { - using T = typename DerivedEa0::Scalar; + using T = typename DerivedEA0::Scalar; return edge_edge_mollifier( Eigen::Ref>(ea0), Eigen::Ref>(ea1), @@ -399,17 +399,17 @@ inline auto edge_edge_mollifier( } template < - EigenExpression DerivedEa0, - EigenExpression DerivedEa1, - EigenExpression DerivedEb0, - EigenExpression DerivedEb1> + 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) - -> Eigen::Vector + const Eigen::MatrixBase& ea0, + const Eigen::MatrixBase& ea1, + const Eigen::MatrixBase& eb0, + const Eigen::MatrixBase& eb1, + const typename DerivedEA0::Scalar eps_x) + -> Eigen::Vector { using T = typename DerivedEa0::Scalar; return edge_edge_mollifier_gradient( @@ -420,17 +420,17 @@ inline auto edge_edge_mollifier_gradient( } template < - EigenExpression DerivedEa0, - EigenExpression DerivedEa1, - EigenExpression DerivedEb0, - EigenExpression DerivedEb1> + 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) - -> Eigen::Matrix + const Eigen::MatrixBase& ea0, + const Eigen::MatrixBase& ea1, + const Eigen::MatrixBase& eb0, + const Eigen::MatrixBase& eb1, + const typename DerivedEA0::Scalar eps_x) + -> Eigen::Matrix { using T = typename DerivedEa0::Scalar; return edge_edge_mollifier_hessian( @@ -441,26 +441,26 @@ inline auto edge_edge_mollifier_hessian( } template < - EigenExpression DerivedEa0Rest, - EigenExpression DerivedEa1Rest, - EigenExpression DerivedEb0Rest, - EigenExpression DerivedEb1Rest, - EigenExpression DerivedEa0, - EigenExpression DerivedEa1, - EigenExpression DerivedEb0, - EigenExpression DerivedEb1> + 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) - -> Eigen::Vector + 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) + -> Eigen::Vector { - using T = typename DerivedEa0Rest::Scalar; + using T = typename DerivedEA0Rest::Scalar; return edge_edge_mollifier_gradient_wrt_x( Eigen::Ref>(ea0_rest), Eigen::Ref>(ea1_rest), @@ -473,26 +473,26 @@ inline auto edge_edge_mollifier_gradient_wrt_x( } template < - EigenExpression DerivedEa0Rest, - EigenExpression DerivedEa1Rest, - EigenExpression DerivedEb0Rest, - EigenExpression DerivedEb1Rest, - EigenExpression DerivedEa0, - EigenExpression DerivedEa1, - EigenExpression DerivedEb0, - EigenExpression DerivedEb1> + 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) - -> Eigen::Matrix + 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) + -> Eigen::Matrix { - using T = typename DerivedEa0Rest::Scalar; + using T = typename DerivedEA0Rest::Scalar; return edge_edge_mollifier_gradient_jacobian_wrt_x( Eigen::Ref>(ea0_rest), Eigen::Ref>(ea1_rest), @@ -505,18 +505,18 @@ inline auto edge_edge_mollifier_gradient_jacobian_wrt_x( } template < - EigenExpression DerivedEa0Rest, - EigenExpression DerivedEa1Rest, - EigenExpression DerivedEb0Rest, - EigenExpression DerivedEb1Rest> + 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) -> - typename DerivedEa0Rest::Scalar + const Eigen::MatrixBase& ea0_rest, + const Eigen::MatrixBase& ea1_rest, + const Eigen::MatrixBase& eb0_rest, + const Eigen::MatrixBase& eb1_rest) -> + typename DerivedEA0Rest::Scalar { - using T = typename DerivedEa0Rest::Scalar; + using T = typename DerivedEA0Rest::Scalar; return edge_edge_mollifier_threshold( Eigen::Ref>(ea0_rest), Eigen::Ref>(ea1_rest), @@ -525,18 +525,18 @@ inline auto edge_edge_mollifier_threshold( } template < - EigenExpression DerivedEa0Rest, - EigenExpression DerivedEa1Rest, - EigenExpression DerivedEb0Rest, - EigenExpression DerivedEb1Rest> + 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) - -> Eigen::Vector + const Eigen::MatrixBase& ea0_rest, + const Eigen::MatrixBase& ea1_rest, + const Eigen::MatrixBase& eb0_rest, + const Eigen::MatrixBase& eb1_rest) + -> Eigen::Vector { - using T = typename DerivedEa0Rest::Scalar; + using T = typename DerivedEA0Rest::Scalar; return edge_edge_mollifier_threshold_gradient( Eigen::Ref>(ea0_rest), Eigen::Ref>(ea1_rest), diff --git a/src/ipc/distance/line_line.hpp b/src/ipc/distance/line_line.hpp index e4851fa2c..065e046c3 100644 --- a/src/ipc/distance/line_line.hpp +++ b/src/ipc/distance/line_line.hpp @@ -85,17 +85,17 @@ inline Eigen::Matrix line_line_distance_hessian( // --- EigenExpression wrappers --- template < - EigenExpression DerivedEa0, - EigenExpression DerivedEa1, - EigenExpression DerivedEb0, - EigenExpression DerivedEb1> + 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) -> typename DerivedEa0::Scalar + const Eigen::MatrixBase& ea0, + const Eigen::MatrixBase& ea1, + const Eigen::MatrixBase& eb0, + const Eigen::MatrixBase& eb1) -> typename DerivedEA0::Scalar { - using T = typename DerivedEa0::Scalar; + using T = typename DerivedEA0::Scalar; return line_line_distance( Eigen::Ref>(ea0), Eigen::Ref>(ea1), @@ -104,18 +104,18 @@ inline auto line_line_distance( } template < - EigenExpression DerivedEa0, - EigenExpression DerivedEa1, - EigenExpression DerivedEb0, - EigenExpression DerivedEb1> + 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 + const Eigen::MatrixBase& ea0, + const Eigen::MatrixBase& ea1, + const Eigen::MatrixBase& eb0, + const Eigen::MatrixBase& eb1) + -> Eigen::Vector { - using T = typename DerivedEa0::Scalar; + using T = typename DerivedEA0::Scalar; return line_line_distance_gradient( Eigen::Ref>(ea0), Eigen::Ref>(ea1), @@ -124,18 +124,18 @@ inline auto line_line_distance_gradient( } template < - EigenExpression DerivedEa0, - EigenExpression DerivedEa1, - EigenExpression DerivedEb0, - EigenExpression DerivedEb1> + 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 + const Eigen::MatrixBase& ea0, + const Eigen::MatrixBase& ea1, + const Eigen::MatrixBase& eb0, + const Eigen::MatrixBase& eb1) + -> Eigen::Matrix { - using T = typename DerivedEa0::Scalar; + using T = typename DerivedEA0::Scalar; return line_line_distance_hessian( Eigen::Ref>(ea0), Eigen::Ref>(ea1), diff --git a/src/ipc/distance/point_edge.hpp b/src/ipc/distance/point_edge.hpp index b053481e0..9e20037ae 100644 --- a/src/ipc/distance/point_edge.hpp +++ b/src/ipc/distance/point_edge.hpp @@ -49,10 +49,7 @@ MatrixMax9 point_edge_distance_hessian( // --- EigenExpression wrappers --- -template < - EigenExpression DerivedP, - EigenExpression DerivedE0, - EigenExpression DerivedE1> +template inline auto point_edge_distance( const Eigen::MatrixBase& p, const Eigen::MatrixBase& e0, @@ -66,10 +63,7 @@ inline auto point_edge_distance( Eigen::Ref>(e1), dtype); } -template < - EigenExpression DerivedP, - EigenExpression DerivedE0, - EigenExpression DerivedE1> +template inline auto point_edge_distance_gradient( const Eigen::MatrixBase& p, const Eigen::MatrixBase& e0, @@ -83,10 +77,7 @@ inline auto point_edge_distance_gradient( Eigen::Ref>(e1), dtype); } -template < - EigenExpression DerivedP, - EigenExpression DerivedE0, - EigenExpression DerivedE1> +template inline auto point_edge_distance_hessian( const Eigen::MatrixBase& p, const Eigen::MatrixBase& e0, diff --git a/src/ipc/distance/point_line.hpp b/src/ipc/distance/point_line.hpp index 01c26c320..b725b708c 100644 --- a/src/ipc/distance/point_line.hpp +++ b/src/ipc/distance/point_line.hpp @@ -106,10 +106,7 @@ inline MatrixMax9 point_line_distance_hessian( // --- EigenExpression wrappers --- -template < - EigenExpression DerivedP, - EigenExpression DerivedE0, - EigenExpression DerivedE1> +template inline auto point_line_distance( const Eigen::MatrixBase& p, const Eigen::MatrixBase& e0, @@ -121,10 +118,7 @@ inline auto point_line_distance( Eigen::Ref>(e1)); } -template < - EigenExpression DerivedP, - EigenExpression DerivedE0, - EigenExpression DerivedE1> +template inline auto point_line_distance_gradient( const Eigen::MatrixBase& p, const Eigen::MatrixBase& e0, @@ -137,10 +131,7 @@ inline auto point_line_distance_gradient( Eigen::Ref>(e1)); } -template < - EigenExpression DerivedP, - EigenExpression DerivedE0, - EigenExpression DerivedE1> +template inline auto point_line_distance_hessian( const Eigen::MatrixBase& p, const Eigen::MatrixBase& e0, diff --git a/src/ipc/distance/point_plane.hpp b/src/ipc/distance/point_plane.hpp index f2ca5d40f..e5a221035 100644 --- a/src/ipc/distance/point_plane.hpp +++ b/src/ipc/distance/point_plane.hpp @@ -127,10 +127,7 @@ inline Eigen::Matrix point_plane_distance_hessian( // --- EigenExpression wrappers --- -template < - EigenExpression DerivedP, - EigenExpression DerivedOrigin, - EigenExpression DerivedNormal> +template inline auto point_plane_distance( const Eigen::MatrixBase& p, const Eigen::MatrixBase& origin, @@ -144,10 +141,10 @@ inline auto point_plane_distance( } template < - EigenExpression DerivedP, - EigenExpression DerivedT0, - EigenExpression DerivedT1, - EigenExpression DerivedT2> + typename DerivedP, + typename DerivedT0, + typename DerivedT1, + typename DerivedT2> inline auto point_plane_distance( const Eigen::MatrixBase& p, const Eigen::MatrixBase& t0, @@ -162,10 +159,7 @@ inline auto point_plane_distance( Eigen::Ref>(t2)); } -template < - EigenExpression DerivedP, - EigenExpression DerivedOrigin, - EigenExpression DerivedNormal> +template inline auto point_plane_distance_gradient( const Eigen::MatrixBase& p, const Eigen::MatrixBase& origin, @@ -180,10 +174,10 @@ inline auto point_plane_distance_gradient( } template < - EigenExpression DerivedP, - EigenExpression DerivedT0, - EigenExpression DerivedT1, - EigenExpression DerivedT2> + typename DerivedP, + typename DerivedT0, + typename DerivedT1, + typename DerivedT2> inline auto point_plane_distance_gradient( const Eigen::MatrixBase& p, const Eigen::MatrixBase& t0, @@ -199,10 +193,7 @@ inline auto point_plane_distance_gradient( Eigen::Ref>(t2)); } -template < - EigenExpression DerivedP, - EigenExpression DerivedOrigin, - EigenExpression DerivedNormal> +template inline auto point_plane_distance_hessian( const Eigen::MatrixBase& p, const Eigen::MatrixBase& origin, @@ -217,10 +208,10 @@ inline auto point_plane_distance_hessian( } template < - EigenExpression DerivedP, - EigenExpression DerivedT0, - EigenExpression DerivedT1, - EigenExpression DerivedT2> + typename DerivedP, + typename DerivedT0, + typename DerivedT1, + typename DerivedT2> inline auto point_plane_distance_hessian( const Eigen::MatrixBase& p, const Eigen::MatrixBase& t0, diff --git a/src/ipc/distance/point_point.hpp b/src/ipc/distance/point_point.hpp index da4b6f3ac..7a609c086 100644 --- a/src/ipc/distance/point_point.hpp +++ b/src/ipc/distance/point_point.hpp @@ -57,7 +57,7 @@ inline MatrixMax6 point_point_distance_hessian( // --- EigenExpression wrappers --- -template +template inline auto point_point_distance( const Eigen::MatrixBase& p0, const Eigen::MatrixBase& p1) -> typename DerivedP0::Scalar @@ -68,7 +68,7 @@ inline auto point_point_distance( Eigen::Ref>(p1)); } -template +template inline auto point_point_distance_gradient( const Eigen::MatrixBase& p0, const Eigen::MatrixBase& p1) @@ -80,7 +80,7 @@ inline auto point_point_distance_gradient( Eigen::Ref>(p1)); } -template +template inline auto point_point_distance_hessian( const Eigen::MatrixBase& p0, const Eigen::MatrixBase& p1) diff --git a/src/ipc/distance/point_triangle.hpp b/src/ipc/distance/point_triangle.hpp index b771c6fec..e2988ff10 100644 --- a/src/ipc/distance/point_triangle.hpp +++ b/src/ipc/distance/point_triangle.hpp @@ -56,10 +56,10 @@ Eigen::Matrix point_triangle_distance_hessian( // --- EigenExpression wrappers --- template < - EigenExpression DerivedP, - EigenExpression DerivedT0, - EigenExpression DerivedT1, - EigenExpression DerivedT2> + typename DerivedP, + typename DerivedT0, + typename DerivedT1, + typename DerivedT2> inline auto point_triangle_distance( const Eigen::MatrixBase& p, const Eigen::MatrixBase& t0, @@ -77,10 +77,10 @@ inline auto point_triangle_distance( } template < - EigenExpression DerivedP, - EigenExpression DerivedT0, - EigenExpression DerivedT1, - EigenExpression DerivedT2> + typename DerivedP, + typename DerivedT0, + typename DerivedT1, + typename DerivedT2> inline auto point_triangle_distance_gradient( const Eigen::MatrixBase& p, const Eigen::MatrixBase& t0, @@ -98,10 +98,10 @@ inline auto point_triangle_distance_gradient( } template < - EigenExpression DerivedP, - EigenExpression DerivedT0, - EigenExpression DerivedT1, - EigenExpression DerivedT2> + typename DerivedP, + typename DerivedT0, + typename DerivedT1, + typename DerivedT2> inline auto point_triangle_distance_hessian( const Eigen::MatrixBase& p, const Eigen::MatrixBase& t0, diff --git a/src/ipc/distance/signed/line_line.hpp b/src/ipc/distance/signed/line_line.hpp index fdace1aba..096d6ddb0 100644 --- a/src/ipc/distance/signed/line_line.hpp +++ b/src/ipc/distance/signed/line_line.hpp @@ -97,10 +97,10 @@ Eigen::Matrix line_line_signed_distance_hessian( // --- EigenExpression wrappers --- template < - EigenExpression DerivedEA0, - EigenExpression DerivedEA1, - EigenExpression DerivedEB0, - EigenExpression DerivedEB1> + typename DerivedEA0, + typename DerivedEA1, + typename DerivedEB0, + typename DerivedEB1> inline auto line_line_signed_distance( const Eigen::MatrixBase& ea0, const Eigen::MatrixBase& ea1, @@ -116,10 +116,10 @@ inline auto line_line_signed_distance( } template < - EigenExpression DerivedEA0, - EigenExpression DerivedEA1, - EigenExpression DerivedEB0, - EigenExpression DerivedEB1> + typename DerivedEA0, + typename DerivedEA1, + typename DerivedEB0, + typename DerivedEB1> inline auto line_line_signed_distance_gradient( const Eigen::MatrixBase& ea0, const Eigen::MatrixBase& ea1, @@ -136,10 +136,10 @@ inline auto line_line_signed_distance_gradient( } template < - EigenExpression DerivedEA0, - EigenExpression DerivedEA1, - EigenExpression DerivedEB0, - EigenExpression DerivedEB1> + typename DerivedEA0, + typename DerivedEA1, + typename DerivedEB0, + typename DerivedEB1> inline auto line_line_signed_distance_hessian( const Eigen::MatrixBase& ea0, const Eigen::MatrixBase& ea1, diff --git a/src/ipc/distance/signed/point_line.hpp b/src/ipc/distance/signed/point_line.hpp index 8b0fe81bc..8b5d6ac1e 100644 --- a/src/ipc/distance/signed/point_line.hpp +++ b/src/ipc/distance/signed/point_line.hpp @@ -75,10 +75,7 @@ Eigen::Matrix point_line_signed_distance_hessian( // --- EigenExpression wrappers --- -template < - EigenExpression DerivedP, - EigenExpression DerivedE0, - EigenExpression DerivedE1> +template inline auto point_line_signed_distance( const Eigen::MatrixBase& p, const Eigen::MatrixBase& e0, @@ -91,10 +88,7 @@ inline auto point_line_signed_distance( Eigen::Ref>(e1)); } -template < - EigenExpression DerivedP, - EigenExpression DerivedE0, - EigenExpression DerivedE1> +template inline auto point_line_signed_distance_gradient( const Eigen::MatrixBase& p, const Eigen::MatrixBase& e0, @@ -108,10 +102,7 @@ inline auto point_line_signed_distance_gradient( Eigen::Ref>(e1)); } -template < - EigenExpression DerivedP, - EigenExpression DerivedE0, - EigenExpression DerivedE1> +template inline auto point_line_signed_distance_hessian( const Eigen::MatrixBase& p, const Eigen::MatrixBase& e0, diff --git a/src/ipc/distance/signed/point_plane.hpp b/src/ipc/distance/signed/point_plane.hpp index b23b39109..b58ffb502 100644 --- a/src/ipc/distance/signed/point_plane.hpp +++ b/src/ipc/distance/signed/point_plane.hpp @@ -76,10 +76,10 @@ Eigen::Matrix point_plane_signed_distance_hessian( // --- EigenExpression wrappers --- template < - EigenExpression DerivedP, - EigenExpression DerivedT0, - EigenExpression DerivedT1, - EigenExpression DerivedT2> + typename DerivedP, + typename DerivedT0, + typename DerivedT1, + typename DerivedT2> inline auto point_plane_signed_distance( const Eigen::MatrixBase& p, const Eigen::MatrixBase& t0, @@ -95,10 +95,10 @@ inline auto point_plane_signed_distance( } template < - EigenExpression DerivedP, - EigenExpression DerivedT0, - EigenExpression DerivedT1, - EigenExpression DerivedT2> + typename DerivedP, + typename DerivedT0, + typename DerivedT1, + typename DerivedT2> inline auto point_plane_signed_distance_gradient( const Eigen::MatrixBase& p, const Eigen::MatrixBase& t0, @@ -115,10 +115,10 @@ inline auto point_plane_signed_distance_gradient( } template < - EigenExpression DerivedP, - EigenExpression DerivedT0, - EigenExpression DerivedT1, - EigenExpression DerivedT2> + typename DerivedP, + typename DerivedT0, + typename DerivedT1, + typename DerivedT2> inline auto point_plane_signed_distance_hessian( const Eigen::MatrixBase& p, const Eigen::MatrixBase& t0, diff --git a/src/ipc/geometry/normal.hpp b/src/ipc/geometry/normal.hpp index f7c1355a3..42e064947 100644 --- a/src/ipc/geometry/normal.hpp +++ b/src/ipc/geometry/normal.hpp @@ -357,21 +357,27 @@ Eigen::Matrix line_line_normal_hessian( // --- EigenExpression wrappers --- -template +template < + typename DerivedX, + std::enable_if_t, int> = 0> inline auto normalization_and_jacobian(const Eigen::MatrixBase& x) { using T = typename DerivedX::Scalar; return normalization_and_jacobian(Eigen::Ref>(x)); } -template +template < + typename DerivedX, + std::enable_if_t, int> = 0> inline auto normalization_jacobian(const Eigen::MatrixBase& x) { using T = typename DerivedX::Scalar; return normalization_jacobian(Eigen::Ref>(x)); } -template +template < + typename DerivedX, + std::enable_if_t, int> = 0> inline auto normalization_and_jacobian_and_hessian(const Eigen::MatrixBase& x) { @@ -380,14 +386,18 @@ normalization_and_jacobian_and_hessian(const Eigen::MatrixBase& x) Eigen::Ref>(x)); } -template +template < + typename DerivedX, + std::enable_if_t, int> = 0> inline auto normalization_hessian(const Eigen::MatrixBase& x) { using T = typename DerivedX::Scalar; return normalization_hessian(Eigen::Ref>(x)); } -template +template < + typename DerivedX, + std::enable_if_t, int> = 0> inline auto cross_product_matrix(const Eigen::MatrixBase& v) -> Eigen::Matrix { @@ -395,10 +405,7 @@ inline auto cross_product_matrix(const Eigen::MatrixBase& v) return cross_product_matrix(Eigen::Ref>(v)); } -template < - EigenExpression DerivedP, - EigenExpression DerivedE0, - EigenExpression DerivedE1> +template inline auto point_line_unnormalized_normal( const Eigen::MatrixBase& p, const Eigen::MatrixBase& e0, @@ -411,10 +418,7 @@ inline auto point_line_unnormalized_normal( Eigen::Ref>(e1)); } -template < - EigenExpression DerivedP, - EigenExpression DerivedE0, - EigenExpression DerivedE1> +template inline auto point_line_normal( const Eigen::MatrixBase& p, const Eigen::MatrixBase& e0, @@ -427,10 +431,7 @@ inline auto point_line_normal( Eigen::Ref>(e1)); } -template < - EigenExpression DerivedP, - EigenExpression DerivedE0, - EigenExpression DerivedE1> +template inline auto point_line_unnormalized_normal_jacobian( const Eigen::MatrixBase& p, const Eigen::MatrixBase& e0, @@ -443,10 +444,7 @@ inline auto point_line_unnormalized_normal_jacobian( Eigen::Ref>(e1)); } -template < - EigenExpression DerivedP, - EigenExpression DerivedE0, - EigenExpression DerivedE1> +template inline auto point_line_unnormalized_normal_hessian( const Eigen::MatrixBase& p, const Eigen::MatrixBase& e0, @@ -459,10 +457,7 @@ inline auto point_line_unnormalized_normal_hessian( Eigen::Ref>(e1)); } -template < - EigenExpression DerivedP, - EigenExpression DerivedE0, - EigenExpression DerivedE1> +template inline auto point_line_normal_jacobian( const Eigen::MatrixBase& p, const Eigen::MatrixBase& e0, @@ -475,10 +470,7 @@ inline auto point_line_normal_jacobian( Eigen::Ref>(e1)); } -template < - EigenExpression DerivedP, - EigenExpression DerivedE0, - EigenExpression DerivedE1> +template inline auto point_line_normal_hessian( const Eigen::MatrixBase& p, const Eigen::MatrixBase& e0, @@ -491,10 +483,7 @@ inline auto point_line_normal_hessian( Eigen::Ref>(e1)); } -template < - EigenExpression DerivedA, - EigenExpression DerivedB, - EigenExpression DerivedC> +template inline auto triangle_unnormalized_normal( const Eigen::MatrixBase& a, const Eigen::MatrixBase& b, @@ -508,10 +497,7 @@ inline auto triangle_unnormalized_normal( Eigen::Ref>(c)); } -template < - EigenExpression DerivedA, - EigenExpression DerivedB, - EigenExpression DerivedC> +template inline auto triangle_normal( const Eigen::MatrixBase& a, const Eigen::MatrixBase& b, @@ -525,10 +511,7 @@ inline auto triangle_normal( Eigen::Ref>(c)); } -template < - EigenExpression DerivedA, - EigenExpression DerivedB, - EigenExpression DerivedC> +template inline auto triangle_unnormalized_normal_jacobian( const Eigen::MatrixBase& a, const Eigen::MatrixBase& b, @@ -542,10 +525,7 @@ inline auto triangle_unnormalized_normal_jacobian( Eigen::Ref>(c)); } -template < - EigenExpression DerivedA, - EigenExpression DerivedB, - EigenExpression DerivedC> +template inline auto triangle_unnormalized_normal_hessian( const Eigen::MatrixBase& a, const Eigen::MatrixBase& b, @@ -559,10 +539,7 @@ inline auto triangle_unnormalized_normal_hessian( Eigen::Ref>(c)); } -template < - EigenExpression DerivedA, - EigenExpression DerivedB, - EigenExpression DerivedC> +template inline auto triangle_normal_jacobian( const Eigen::MatrixBase& a, const Eigen::MatrixBase& b, @@ -576,10 +553,7 @@ inline auto triangle_normal_jacobian( Eigen::Ref>(c)); } -template < - EigenExpression DerivedA, - EigenExpression DerivedB, - EigenExpression DerivedC> +template inline auto triangle_normal_hessian( const Eigen::MatrixBase& a, const Eigen::MatrixBase& b, @@ -594,10 +568,10 @@ inline auto triangle_normal_hessian( } template < - EigenExpression DerivedEA0, - EigenExpression DerivedEA1, - EigenExpression DerivedEB0, - EigenExpression DerivedEB1> + typename DerivedEA0, + typename DerivedEA1, + typename DerivedEB0, + typename DerivedEB1> inline auto line_line_unnormalized_normal( const Eigen::MatrixBase& ea0, const Eigen::MatrixBase& ea1, @@ -614,10 +588,10 @@ inline auto line_line_unnormalized_normal( } template < - EigenExpression DerivedEA0, - EigenExpression DerivedEA1, - EigenExpression DerivedEB0, - EigenExpression DerivedEB1> + typename DerivedEA0, + typename DerivedEA1, + typename DerivedEB0, + typename DerivedEB1> inline auto line_line_normal( const Eigen::MatrixBase& ea0, const Eigen::MatrixBase& ea1, @@ -634,10 +608,10 @@ inline auto line_line_normal( } template < - EigenExpression DerivedEA0, - EigenExpression DerivedEA1, - EigenExpression DerivedEB0, - EigenExpression DerivedEB1> + typename DerivedEA0, + typename DerivedEA1, + typename DerivedEB0, + typename DerivedEB1> inline auto line_line_unnormalized_normal_jacobian( const Eigen::MatrixBase& ea0, const Eigen::MatrixBase& ea1, @@ -654,10 +628,10 @@ inline auto line_line_unnormalized_normal_jacobian( } template < - EigenExpression DerivedEA0, - EigenExpression DerivedEA1, - EigenExpression DerivedEB0, - EigenExpression DerivedEB1> + typename DerivedEA0, + typename DerivedEA1, + typename DerivedEB0, + typename DerivedEB1> inline auto line_line_normal_jacobian( const Eigen::MatrixBase& ea0, const Eigen::MatrixBase& ea1, @@ -674,10 +648,10 @@ inline auto line_line_normal_jacobian( } template < - EigenExpression DerivedEA0, - EigenExpression DerivedEA1, - EigenExpression DerivedEB0, - EigenExpression DerivedEB1> + typename DerivedEA0, + typename DerivedEA1, + typename DerivedEB0, + typename DerivedEB1> inline auto line_line_unnormalized_normal_hessian( const Eigen::MatrixBase& ea0, const Eigen::MatrixBase& ea1, @@ -694,10 +668,10 @@ inline auto line_line_unnormalized_normal_hessian( } template < - EigenExpression DerivedEA0, - EigenExpression DerivedEA1, - EigenExpression DerivedEB0, - EigenExpression DerivedEB1> + typename DerivedEA0, + typename DerivedEA1, + typename DerivedEB0, + typename DerivedEB1> inline auto line_line_normal_hessian( const Eigen::MatrixBase& ea0, const Eigen::MatrixBase& ea1, diff --git a/src/ipc/utils/eigen_ext.hpp b/src/ipc/utils/eigen_ext.hpp index daa79d531..b77e4b18b 100644 --- a/src/ipc/utils/eigen_ext.hpp +++ b/src/ipc/utils/eigen_ext.hpp @@ -233,12 +233,12 @@ using HessianType = std:: /// Pure requires-expression: checks only members of T itself, so the compiler /// never attempts to form Eigen::MatrixBase (which is ill-formed when T /// is a scalar type like double). -template -concept EigenExpression = requires(T& t) { - typename T::Scalar; - { t.rows() } -> std::convertible_to; - { t.cols() } -> std::convertible_to; -}; +// template +// concept EigenExpression = requires(T& t) { +// typename T::Scalar; +// { t.rows() } -> std::convertible_to; +// { t.cols() } -> std::convertible_to; +// }; /// @brief Matrix projection onto positive definite cone /// @param A Symmetric matrix to project From 1d89967c70ff595a3f84070cf172b56f641a07dc Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Mon, 4 May 2026 14:55:19 -0400 Subject: [PATCH 06/21] Fix template scalar typedef in edge-edge --- src/ipc/distance/edge_edge.hpp | 4 ++-- src/ipc/distance/edge_edge_mollifier.hpp | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/ipc/distance/edge_edge.hpp b/src/ipc/distance/edge_edge.hpp index c68ef40d1..75f285c27 100644 --- a/src/ipc/distance/edge_edge.hpp +++ b/src/ipc/distance/edge_edge.hpp @@ -89,7 +89,7 @@ inline auto edge_edge_distance_gradient( EdgeEdgeDistanceType dtype = EdgeEdgeDistanceType::AUTO) -> Eigen::Vector { - using T = typename DerivedEa0::Scalar; + using T = typename DerivedEA0::Scalar; return edge_edge_distance_gradient( Eigen::Ref>(ea0), Eigen::Ref>(ea1), @@ -110,7 +110,7 @@ inline auto edge_edge_distance_hessian( EdgeEdgeDistanceType dtype = EdgeEdgeDistanceType::AUTO) -> Eigen::Matrix { - using T = typename DerivedEa0::Scalar; + using T = typename DerivedEA0::Scalar; return edge_edge_distance_hessian( Eigen::Ref>(ea0), Eigen::Ref>(ea1), diff --git a/src/ipc/distance/edge_edge_mollifier.hpp b/src/ipc/distance/edge_edge_mollifier.hpp index 559c60bde..874e0f6ef 100644 --- a/src/ipc/distance/edge_edge_mollifier.hpp +++ b/src/ipc/distance/edge_edge_mollifier.hpp @@ -411,7 +411,7 @@ inline auto edge_edge_mollifier_gradient( const typename DerivedEA0::Scalar eps_x) -> Eigen::Vector { - using T = typename DerivedEa0::Scalar; + using T = typename DerivedEA0::Scalar; return edge_edge_mollifier_gradient( Eigen::Ref>(ea0), Eigen::Ref>(ea1), @@ -432,7 +432,7 @@ inline auto edge_edge_mollifier_hessian( const typename DerivedEA0::Scalar eps_x) -> Eigen::Matrix { - using T = typename DerivedEa0::Scalar; + using T = typename DerivedEA0::Scalar; return edge_edge_mollifier_hessian( Eigen::Ref>(ea0), Eigen::Ref>(ea1), From 3b28d6e7232c0b2914a6c56f1ad76d05c254ecfa Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Sat, 29 Aug 2026 18:20:04 -0400 Subject: [PATCH 07/21] Fix float precision and explicit-scalar calls in the templated distance API Scale PARALLEL_THRESHOLD in edge_edge_distance_type with the precision of T. u x v cancels for nearly parallel edges, leaving an absolute error of about eps*|u|*|v| per component, so sin^2(theta) cannot be resolved below ~eps^2. The threshold was left as a hard-coded 2.5e-16, which is ~1.13*eps for double but sits ~57x *below* float's noise floor, making the near-parallel branch unreachable in single precision. Over 20k exactly-parallel edge pairs the float distance type disagreed with the double one 44% of the time, and edge_edge_distance differed from the double result by >0.1% relative in 14.8% of cases (worst case 24x). Both drop to 0% after the fix. The threshold is now derived from the double-tuned value by the ratio of epsilons, so the double threshold is bit-for-bit unchanged (asserted); only the newly added float instantiation changes. parallel_tolerance is typed T rather than double to match. Guard the *_distance_type EigenExpression wrappers with std::is_class_v. The other wrappers are rejected by SFINAE when the first template argument is given explicitly as a scalar, because their trailing return type mentions typename DerivedX::Scalar. These four return a non-dependent enum, so nothing rejected the candidate and substitution went on to form Eigen::MatrixBase, a hard error inside Eigen rather than a substitution failure. As a result edge_edge_distance_type(a, b, c, d) -- the natural way to pick the scalar in the new templated API -- failed to compile with 20 errors pointing into Eigen internals. The guard matches the one already used on the single-argument wrappers in geometry/normal.hpp. Drop the commented-out EigenExpression concept and its now-unused include. The doc comment left above it described a dispatch contract the shipped code does not follow ("Layer 1 always calls Layer 2 with explicit "), which is exactly the gap that produced the compile failure above. Fix the sizes in the MatrixMax alias doc comments: MatrixMax2f/MatrixMax2d were documented as 3x3 and MatrixMax9f/MatrixMax9d as 12x12. Add a v2.0.0 (alpha) section to the release notes covering the scalar templatization of the distance, barrier, and normal APIs, along with every commit since v1.6.0: the Tight Inclusion 1.1.0 update (#248), the tutorial and Python binding work (#247), the edge-triangle intersection coordinates and CollisionMesh::face_normals (#245), the mollified m == 0 Hessian PSD projection fix (#244), and the MeshFEM gallery entry. Co-Authored-By: Claude Opus 5 --- docs/source/about/release_notes.rst | 99 +++++++++++++++++++++++++++++ src/ipc/distance/distance_type.cpp | 19 ++++-- src/ipc/distance/distance_type.hpp | 25 ++++++-- src/ipc/utils/eigen_ext.hpp | 26 ++------ 4 files changed, 139 insertions(+), 30 deletions(-) diff --git a/docs/source/about/release_notes.rst b/docs/source/about/release_notes.rst index 67ef88601..d5136792f 100644 --- a/docs/source/about/release_notes.rst +++ b/docs/source/about/release_notes.rst @@ -6,6 +6,105 @@ 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 +~~~~~~~~~~ + +- 💥 **[Breaking]** The distance, barrier, and normal APIs are now templated on the scalar type, adding ``float`` support alongside ``double`` (and autodiff scalars where already supported). +- 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 `_). + +Continuous Collision Detection +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +- 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. + +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 `_). + + - The out-parameters are seeded to NaN and written only once an intersection is confirmed, so a ``false`` return never leaves them partially populated. They also stay NaN in the degenerate rational case, where the coordinates are not uniquely defined. + - Boundary hits count as intersections (the comparisons are inclusive). + +- 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:| +~~~~~~~~~~~~~~~~~~~~~~ + +- 💥 **[Breaking]** Templatize the distance functions on the scalar type. + + - The free functions in ``ipc/distance/`` (``point_point_distance``, ``point_line_distance``, ``point_edge_distance``, ``line_line_distance``, ``edge_edge_distance``, ``point_plane_distance``, ``point_triangle_distance``, the ``*_distance_type`` predicates, the edge-edge mollifier, and the signed-distance variants), together with ``ipc/geometry/normal.hpp``, now take a scalar template parameter ``T`` and are instantiated for both ``float`` and ``double``. + - Each function gained an overload accepting any Eigen expression (blocks, maps, and rows of a matrix), so call sites no longer need to materialize an ``Eigen::Vector3d`` before calling. Existing calls that let ``T`` be deduced from their arguments continue to compile unchanged. + - Taking the address of one of these functions now requires naming the scalar explicitly, e.g. ``&ipc::point_edge_distance`` instead of ``&ipc::point_edge_distance``. + - Mixed-precision calls no longer deduce: ``barrier(float_d, 0.001)`` must become ``barrier(float_d, 0.001f)``. + +- 💥 **[Breaking]** Templatize the barrier classes on the scalar type. + + - ``ipc::Barrier`` is now an alias for the class template ``ipc::BarrierBase`` (defaulting to ``double``). Deriving from ``ipc::Barrier`` and overriding ``operator()``, ``first_derivative``, ``second_derivative``, and ``units`` is unchanged. + - The concrete barriers are now class templates and must be spelled with explicit template arguments: ``ipc::ClampedLogBarrier<>``, ``ipc::NormalizedClampedLogBarrier<>``, ``ipc::ClampedLogSqBarrier<>``, ``ipc::CubicBarrier<>``, and ``ipc::TwoStageBarrier<>``. + - The free functions ``ipc::barrier``, ``ipc::barrier_first_derivative``, and ``ipc::barrier_second_derivative`` are now templates defaulting to ``double``. + + The Python API is unaffected: ``ipctk`` continues to expose the ``double`` instantiations under their existing names. + +- Add ``float`` aliases to ``ipc/utils/eigen_ext.hpp`` (``Vector1f``, ``Vector6f``, ``Matrix6f``, ``VectorMax3f``, ``MatrixMax9f``, …) mirroring the existing ``double`` ones. + +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. + +- Scale ``PARALLEL_THRESHOLD`` in ``ipc::edge_edge_distance_type`` with the precision of the scalar type. The value tuned for ``double`` (2.5e-16 ≈ 1.13ε) sits roughly 57× *below* the cancellation noise floor of :math:`u \times v` in single precision, which made the near-parallel branch unreachable for ``float``: across 20k exactly-parallel edge pairs the ``float`` distance type disagreed with the ``double`` one 44% of the time, with distance errors up to 24× relative. The ``double`` threshold is bit-for-bit unchanged. +- Guard the ``*_distance_type`` Eigen-expression overloads with ``std::is_class_v``. Unlike the other overloads, their return type is a non-dependent enum, so an explicit scalar argument (e.g. ``edge_edge_distance_type(...)``) was not rejected by SFINAE and instead formed ``Eigen::MatrixBase``, a hard error inside Eigen. + +Python |:snake:| +~~~~~~~~~~~~~~~~ + +- 💥 **[Breaking]** Rename the ``SmoothPotential`` class to ``SmoothContactPotential`` to match the C++ name (`#247 `_). It had no in-tree users and the package is still a 2.0 alpha, so this is a straight rename with no alias. +- 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 +~~~~~~~~~~~~~ + +- 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/src/ipc/distance/distance_type.cpp b/src/ipc/distance/distance_type.cpp index 253221f98..bf56a11e0 100644 --- a/src/ipc/distance/distance_type.cpp +++ b/src/ipc/distance/distance_type.cpp @@ -5,6 +5,8 @@ #include +#include + namespace ipc { template @@ -87,10 +89,19 @@ EdgeEdgeDistanceType edge_edge_distance_type( { // 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; + // + // 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; @@ -113,7 +124,7 @@ EdgeEdgeDistanceType edge_edge_distance_type( } // 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); } diff --git a/src/ipc/distance/distance_type.hpp b/src/ipc/distance/distance_type.hpp index e4dffec3f..1093b4456 100644 --- a/src/ipc/distance/distance_type.hpp +++ b/src/ipc/distance/distance_type.hpp @@ -2,6 +2,8 @@ #include +#include + namespace ipc { /// @brief Closest pair between a point and point. @@ -105,8 +107,20 @@ EdgeEdgeDistanceType edge_edge_parallel_distance_type( Eigen::ConstRef> eb1); // --- 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. -template +template < + typename DerivedP, + typename DerivedE0, + typename DerivedE1, + std::enable_if_t, int> = 0> inline PointEdgeDistanceType point_edge_distance_type( const Eigen::MatrixBase& p, const Eigen::MatrixBase& e0, @@ -122,7 +136,8 @@ template < typename DerivedP, typename DerivedT0, typename DerivedT1, - typename DerivedT2> + typename DerivedT2, + std::enable_if_t, int> = 0> inline PointTriangleDistanceType point_triangle_distance_type( const Eigen::MatrixBase& p, const Eigen::MatrixBase& t0, @@ -141,7 +156,8 @@ template < typename DerivedEA0, typename DerivedEA1, typename DerivedEB0, - typename DerivedEB1> + typename DerivedEB1, + std::enable_if_t, int> = 0> inline EdgeEdgeDistanceType edge_edge_distance_type( const Eigen::MatrixBase& ea0, const Eigen::MatrixBase& ea1, @@ -160,7 +176,8 @@ template < typename DerivedEA0, typename DerivedEA1, typename DerivedEB0, - typename DerivedEB1> + typename DerivedEB1, + std::enable_if_t, int> = 0> inline EdgeEdgeDistanceType edge_edge_parallel_distance_type( const Eigen::MatrixBase& ea0, const Eigen::MatrixBase& ea1, diff --git a/src/ipc/utils/eigen_ext.hpp b/src/ipc/utils/eigen_ext.hpp index b77e4b18b..65ad68438 100644 --- a/src/ipc/utils/eigen_ext.hpp +++ b/src/ipc/utils/eigen_ext.hpp @@ -4,7 +4,6 @@ #include #include -#include #ifdef EIGEN_DONT_VECTORIZE // NOTE: Avoid error about abs casting double to int. Eigen does this @@ -168,9 +167,9 @@ 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 3×3 +/// @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; @@ -180,9 +179,9 @@ using MatrixMax3d = MatrixMax3; 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 12×12 +/// @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; @@ -223,23 +222,6 @@ using HessianType = std:: /**@}*/ -/// @brief Matches any Eigen expression — stored types, blocks, maps, lazy -/// expressions. Layer 1 wrappers accept any MatrixBase-derived type, deduce T, -/// and forward as Eigen::ConstRef (zero-copy for blocks/stored types; evaluates -/// lazy expressions once). Layer 1 always calls Layer 2 with explicit to -/// prevent infinite recursion (Layer 2's first template param is T, Layer 1's -/// is DerivedXxx, so explicit-T calls unambiguously route to Layer 2). -/// -/// Pure requires-expression: checks only members of T itself, so the compiler -/// never attempts to form Eigen::MatrixBase (which is ill-formed when T -/// is a scalar type like double). -// template -// concept EigenExpression = requires(T& t) { -// typename T::Scalar; -// { t.rows() } -> std::convertible_to; -// { t.cols() } -> std::convertible_to; -// }; - /// @brief Matrix projection onto positive definite cone /// @param A Symmetric matrix to project /// @param eps Minimum eigenvalue threshold From c18c644a97fc137b9ab82e4c59b936159d5c9a9c Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Sun, 30 Aug 2026 23:41:11 -0400 Subject: [PATCH 08/21] Split the distance API into deducing front ends over fixed-size kernels The distance functions now have two layers. The concrete kernels moved into ipc::detail, templated on (or just for the 3D-only line_line, edge_edge, and point_triangle) and taking Eigen::ConstRef>. The public names in ipc are thin front ends templated on the argument expression types: they deduce the scalar, resolve the dimension at compile time when the caller's type knows it, and otherwise take a single runtime branch on size(). Existing calls are unaffected. The win is the dimension, not the parameter passing. Erasing a Vector3d into a VectorMax3d cost 2.4x, so the front end branches on size() before it materializes anything and the fixed-size path never forms a dynamically sized temporary. Measured on the dominant in-tree call site, a row of a column-major MatrixXd: point_line_distance 8.2 -> 2.3 ns 3.5x point_edge_distance, AUTO 18.1 -> 5.2 ns 3.5x point_triangle_distance, AUTO 93.3 -> 12.8 ns 7.3x point_point_distance_hessian 5x normalization_and_jacobian 3.4x point_triangle_distance_type is the largest single piece of that: its three 2x2 LDLT solves are replaced by a closed form, since each edge lies in the triangle's plane and the Gram matrix is therefore diagonal. An error study over 10.8M configurations found no classification changes outside triangles collinear to within 1e-11 of their own edge length, a regime where the old code's own answer flips under a one-ulp input perturbation. It also fixes two real failures: above coordinate scale ~1e+51 the LDLT returned an infinite plane distance on every query, and in float near 1e-6 Eigen's tolerance discarded denormal Gram entries (2771 misclassifications per 400k, now 0). Two smaller levers, both measured: moving the cold throw bodies out of line behind [[noreturn]] helpers (worth up to 2x on its own, since constructing a std::invalid_argument inline consumes the caller's inlining budget), and branching once on the dimension in EdgeVertexCandidate (3.1x on the gradient). edge_edge_distance and point_triangle_distance keep their out-of-line switch dispatch; inlining a 9- or 7-case switch measured as a 10% regression with runtime distance types. line_line_distance_gradient and line_line_distance_hessian are single MatrixBase templates rather than two layers. They read three coefficients per argument and hand them to generated code, so Eigen::ConstRef's guaranteed single evaluation buys nothing and its materialization of an expression argument costs 1.16x. Also: - Guard the *_distance_type Eigen-expression overloads with std::is_class_v. Their return type is a non-dependent enum, so an explicit scalar argument was not rejected by SFINAE and instead formed Eigen::MatrixBase, a hard error inside Eigen. - Scale PARALLEL_THRESHOLD in edge_edge_distance_type with the precision of the scalar type. The value tuned for double sits ~57x below the cancellation noise floor of u x v in single precision, making the near-parallel branch unreachable for float: across 20k exactly-parallel edge pairs the float distance type disagreed with the double one 44% of the time. The double threshold is bit-for-bit unchanged. - Taking the address of these functions is no longer possible, so the Python bindings wrap them in lambdas. - Rewrite tests/src/tests/benchmark_eigen.cpp around the shipped design: every row now compares the library against a same-TU reference implementation that acts as a noise-floor control, and the accumulated findings and measurement traps are consolidated into one header block. This drops a benchmark helper that asked for block<3,1> from a 1x3 row expression, reading out of bounds under NDEBUG. Co-Authored-By: Claude Opus 5 --- docs/source/about/release_notes.rst | 25 +- python/src/distance/distance_type.cpp | 6 +- python/src/distance/edge_edge.cpp | 24 +- python/src/distance/line_line.cpp | 24 +- python/src/distance/point_edge.cpp | 18 +- python/src/distance/point_line.cpp | 18 +- python/src/distance/point_point.cpp | 15 +- python/src/distance/point_triangle.cpp | 25 +- python/src/geometry/normal.cpp | 17 +- src/ipc/candidates/edge_vertex.cpp | 28 +- src/ipc/distance/distance_type.cpp | 100 +- src/ipc/distance/distance_type.hpp | 92 +- src/ipc/distance/edge_edge.cpp | 69 +- src/ipc/distance/edge_edge.hpp | 142 +-- src/ipc/distance/line_line.cpp | 1135 ++++++++--------- src/ipc/distance/line_line.hpp | 145 +-- src/ipc/distance/point_edge.cpp | 177 +-- src/ipc/distance/point_edge.hpp | 231 +++- src/ipc/distance/point_line.hpp | 215 ++-- src/ipc/distance/point_point.hpp | 189 ++- src/ipc/distance/point_triangle.cpp | 63 +- src/ipc/distance/point_triangle.hpp | 142 +-- src/ipc/geometry/normal.cpp | 33 - src/ipc/geometry/normal.hpp | 188 ++- src/ipc/smooth_contact/distance/mollifier.tpp | 9 +- .../smooth_contact/distance/point_edge.cpp | 6 +- .../distance/primitive_distance.tpp | 4 +- src/ipc/utils/eigen_ext.hpp | 43 + tests/src/tests/benchmark_eigen.cpp | 916 +++++++++---- .../src/tests/distance/test_distance_type.cpp | 59 +- 30 files changed, 2506 insertions(+), 1652 deletions(-) diff --git a/docs/source/about/release_notes.rst b/docs/source/about/release_notes.rst index d5136792f..ffa871189 100644 --- a/docs/source/about/release_notes.rst +++ b/docs/source/about/release_notes.rst @@ -43,8 +43,8 @@ API Changes |:wrench:| - 💥 **[Breaking]** Templatize the distance functions on the scalar type. - The free functions in ``ipc/distance/`` (``point_point_distance``, ``point_line_distance``, ``point_edge_distance``, ``line_line_distance``, ``edge_edge_distance``, ``point_plane_distance``, ``point_triangle_distance``, the ``*_distance_type`` predicates, the edge-edge mollifier, and the signed-distance variants), together with ``ipc/geometry/normal.hpp``, now take a scalar template parameter ``T`` and are instantiated for both ``float`` and ``double``. - - Each function gained an overload accepting any Eigen expression (blocks, maps, and rows of a matrix), so call sites no longer need to materialize an ``Eigen::Vector3d`` before calling. Existing calls that let ``T`` be deduced from their arguments continue to compile unchanged. - - Taking the address of one of these functions now requires naming the scalar explicitly, e.g. ``&ipc::point_edge_distance`` instead of ``&ipc::point_edge_distance``. + - Each function now accepts any Eigen expression (blocks, maps, and rows of a matrix), so call sites no longer need to materialize an ``Eigen::Vector3d`` before calling. Existing calls that let ``T`` be deduced from their arguments continue to compile unchanged. + - Taking the address of one of these functions is no longer possible — wrap the call in a lambda, as the Python bindings now do. See the two-layer API entry below. - Mixed-precision calls no longer deduce: ``barrier(float_d, 0.001)`` must become ``barrier(float_d, 0.001f)``. - 💥 **[Breaking]** Templatize the barrier classes on the scalar type. @@ -55,8 +55,29 @@ API Changes |:wrench:| The Python API is unaffected: ``ipctk`` continues to expose the ``double`` instantiations under their existing names. +- 💥 **[Breaking]** 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 ``line_line``, ``edge_edge``, and ``point_triangle``). They take ``Eigen::ConstRef>`` and are explicitly instantiated for ``float``, ``double``, and — for the value functions only — the autodiff scalars. + - ``line_line_distance_gradient`` and ``line_line_distance_hessian`` are the exception: they have no ``detail`` kernel and are single ``Eigen::MatrixBase`` templates in ``ipc``. They read three coefficients per argument and hand them to the generated code, so there is nothing for a second layer to protect. Note that this leaves them without the ``static_assert`` below — an explicit ``line_line_distance_gradient(...)`` fails inside Eigen with ``undefined template 'Eigen::internal::traits'`` rather than a readable message. + - 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. + - Naming the scalar explicitly on a public function (``ipc::point_edge_distance(...)``) is now rejected by a ``static_assert`` telling you to let it deduce. Previously this either failed deep inside Eigen or — for the ``*_distance_type`` predicates — silently formed ``Eigen::MatrixBase``. Call ``ipc::detail::point_edge_distance(...)`` if you genuinely need to pin both parameters. + - 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]** 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. Assignments into ``VectorMax9d``-style storage compile unchanged; code binding the result with ``auto`` may now hold a fixed-size type. Taking the address of these function templates (e.g. ``&ipc::point_edge_distance_gradient``) is no longer possible — wrap the call in a lambda, as the Python bindings now do. - Add ``float`` aliases to ``ipc/utils/eigen_ext.hpp`` (``Vector1f``, ``Vector6f``, ``Matrix6f``, ``VectorMax3f``, ``MatrixMax9f``, …) mirroring the existing ``double`` ones. +Performance |:zap:| +~~~~~~~~~~~~~~~~~~~ + +- Replace the three 2×2 LDLT solves in ``ipc::point_triangle_distance_type`` with a closed form. Each edge lies in the triangle's plane, so the 2×2 Gram matrix is diagonal and the solve collapses to two guarded divisions; the guards reproduce Eigen's pseudo-inverse handling of degenerate edges and triangles. 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 — a regime where the previous code's own answer flips under a one-ulp input perturbation more often than the two methods disagree. The closed form also fixes two real failures: at coordinate scales above ~1e+51 the LDLT collapsed to an infinite plane distance on every query, and in ``float`` at scales near 1e-6 Eigen's tolerance discarded denormal Gram entries (2 771 misclassifications per 400k vs 0). +- 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. Measured on matrix-row call sites: ``point_line_distance`` 8.2 → 2.3 ns (3.5×), ``point_edge_distance`` 18.1 → 5.2 ns AUTO (3.5×), ``point_point_distance_hessian`` up to 5×, ``normalization_and_jacobian`` 3.4× (4.9× on its hottest caller, ``opposite_direction_penalty_grad``). Erasing the dimension into a ``VectorMax3`` was the cost, not the parameter passing: the front end branches on ``size()`` *before* materializing anything, so the fixed-size path never touches a dynamic-size temporary. +- Pass the ``ipc::detail`` kernels' arguments as ``Eigen::ConstRef>`` rather than ``const Eigen::Vector&``. Measured never worse and 1.25–1.38× better on ``point_edge_distance``'s dimension-erased ``AUTO`` path; taking ``const&`` across a TU boundary forces the caller to materialize a temporary, which cost 2× on ``edge_edge_distance`` (5.7 → 12.4 ns). Because the kernels call each other, they must agree on this convention — a single ``const&`` layer breaks the direct-call chain and reintroduces the copy. + + ``Eigen::ConstRef`` is the right default because it guarantees each argument is evaluated exactly once, but it is not free: binding an expression to a ``Ref`` materializes it into the ``Ref``'s internal buffer. For the ``line_line`` gradient and Hessian, which read only three coefficients per argument, that copy is pure cost — reading the operands in place through ``Eigen::MatrixBase`` measured 1.16× faster when the argument is an expression such as ``V.row(a) - V.row(b)``, and no slower anywhere else. Re-evaluating a three-element expression three times is cheaper than materializing it once at this size; that trade would invert for an argument whose evaluation is expensive. +- ``line_line_distance`` (the value function), ``edge_edge_distance``, and ``point_triangle_distance`` were converted to the same two-layer shape. This is structural rather than a speed-up (measured within the noise floor of the unchanged control benchmarks): they keep their out-of-line switch dispatch, since inlining their 9- and 7-case switches measured as a 10% regression with runtime distance types. The payoff is that internal calls between distance functions now resolve directly to the kernel with no wrapper hop, which removed 42 redundant ``ipc::`` qualifications from the edge-edge and point-triangle sources. +- 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:| ~~~~~~~~~~~~~~~~~ diff --git a/python/src/distance/distance_type.cpp b/python/src/distance/distance_type.cpp index 3e458d71f..dec9be881 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. diff --git a/python/src/distance/edge_edge.cpp b/python/src/distance/edge_edge.cpp index 1af83a777..fcfb42b69 100644 --- a/python/src/distance/edge_edge.cpp +++ b/python/src/distance/edge_edge.cpp @@ -8,7 +8,13 @@ using namespace ipc; void define_edge_edge_distance(py::module_& m) { m.def( - "edge_edge_distance", &edge_edge_distance, + "edge_edge_distance", + [](Eigen::ConstRef ea0, + Eigen::ConstRef ea1, + Eigen::ConstRef eb0, + Eigen::ConstRef eb1, EdgeEdgeDistanceType dtype) { + return edge_edge_distance(ea0, ea1, eb0, eb1, dtype); + }, R"ipc_Qu8mg5v7( Compute the distance between a two lines segments in 3D. @@ -29,7 +35,13 @@ 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", + [](Eigen::ConstRef ea0, + Eigen::ConstRef ea1, + Eigen::ConstRef eb0, + Eigen::ConstRef eb1, EdgeEdgeDistanceType dtype) { + return edge_edge_distance_gradient(ea0, ea1, eb0, eb1, dtype); + }, R"ipc_Qu8mg5v7( Compute the gradient of the distance between a two lines segments. @@ -50,7 +62,13 @@ 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", + [](Eigen::ConstRef ea0, + Eigen::ConstRef ea1, + Eigen::ConstRef eb0, + Eigen::ConstRef eb1, EdgeEdgeDistanceType dtype) { + return edge_edge_distance_hessian(ea0, ea1, eb0, eb1, dtype); + }, R"ipc_Qu8mg5v7( Compute the hessian of the distance between a two lines segments. diff --git a/python/src/distance/line_line.cpp b/python/src/distance/line_line.cpp index c82cf61f4..77d2a9b0a 100644 --- a/python/src/distance/line_line.cpp +++ b/python/src/distance/line_line.cpp @@ -7,7 +7,13 @@ using namespace ipc; void define_line_line_distance(py::module_& m) { m.def( - "line_line_distance", &line_line_distance, + "line_line_distance", + [](Eigen::ConstRef ea0, + Eigen::ConstRef ea1, + Eigen::ConstRef eb0, + Eigen::ConstRef eb1) { + return line_line_distance(ea0, ea1, eb0, eb1); + }, R"ipc_Qu8mg5v7( Compute the distance between a two infinite lines in 3D. @@ -29,7 +35,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 +63,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 51e010520..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 f179ebb9e..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_point.cpp b/python/src/distance/point_point.cpp index a7fa112bf..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 b955d9eac..0dc47d5ef 100644 --- a/python/src/distance/point_triangle.cpp +++ b/python/src/distance/point_triangle.cpp @@ -8,7 +8,14 @@ using namespace ipc; void define_point_triangle_distance(py::module_& m) { m.def( - "point_triangle_distance", &point_triangle_distance, + "point_triangle_distance", + [](Eigen::ConstRef p, + Eigen::ConstRef t0, + Eigen::ConstRef t1, + Eigen::ConstRef t2, + PointTriangleDistanceType dtype) { + return point_triangle_distance(p, t0, t1, t2, dtype); + }, R"ipc_Qu8mg5v7( Compute the distance between a points and a triangle. @@ -30,7 +37,13 @@ void define_point_triangle_distance(py::module_& m) m.def( "point_triangle_distance_gradient", - &point_triangle_distance_gradient, + [](Eigen::ConstRef p, + Eigen::ConstRef t0, + Eigen::ConstRef t1, + Eigen::ConstRef t2, + PointTriangleDistanceType dtype) { + return point_triangle_distance_gradient(p, t0, t1, t2, dtype); + }, R"ipc_Qu8mg5v7( Compute the gradient of the distance between a points and a triangle. @@ -52,7 +65,13 @@ void define_point_triangle_distance(py::module_& m) m.def( "point_triangle_distance_hessian", - &point_triangle_distance_hessian, + [](Eigen::ConstRef p, + Eigen::ConstRef t0, + Eigen::ConstRef t1, + Eigen::ConstRef t2, + PointTriangleDistanceType dtype) { + return point_triangle_distance_hessian(p, t0, t1, t2, dtype); + }, R"ipc_Qu8mg5v7( Compute the hessian of the distance between a points and a triangle. diff --git a/python/src/geometry/normal.cpp b/python/src/geometry/normal.cpp index 9eb6a5622..d0bfa9fcb 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. 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/distance_type.cpp b/src/ipc/distance/distance_type.cpp index bf56a11e0..1c88baf5c 100644 --- a/src/ipc/distance/distance_type.cpp +++ b/src/ipc/distance/distance_type.cpp @@ -4,37 +4,38 @@ #include #include +#include #include +#include +#include namespace ipc { -template -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); +namespace detail { - const VectorMax3 e = e1 - e0; - const T e_length_sqr = e.squaredNorm(); - if (e_length_sqr == 0) { + void warn_degenerate_point_edge() noexcept + { logger().warn("Degenerate edge in point_edge_distance_type!"); - return PointEdgeDistanceType::P_E0; // WARNING: use arbitrary end-point } - 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 + void throw_invalid_distance_type(const char* function) + { + throw std::invalid_argument( + fmt::format("{}: invalid distance type", function)); + } + + 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; the distance type cannot be determined from an " + "autodiff scalar", + function)); } -} + +} // namespace detail template PointTriangleDistanceType point_triangle_distance_type( @@ -45,34 +46,51 @@ PointTriangleDistanceType point_triangle_distance_type( { const Eigen::Vector3 normal = (t1 - t0).cross(t2 - t0); - Eigen::Matrix basis, param; - - 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 && param(0, 0) < 1 && param(1, 0) >= 0) { + // 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 && param(0, 1) < 1 && param(1, 1) >= 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 && param(0, 2) < 1 && param(1, 2) >= 0) { + if (closest_to_edge(t0 - t2, p - t2, 2)) { return PointTriangleDistanceType::P_E2; // edge 2 is the closest } - if (param(0, 0) <= 0 && param(0, 2) >= 1) { + if (along[0] <= 0 && along[2] >= 1) { return PointTriangleDistanceType::P_T0; // vertex 0 is the closest - } else if (param(0, 1) <= 0 && param(0, 0) >= 1) { + } else if (along[1] <= 0 && along[0] >= 1) { return PointTriangleDistanceType::P_T1; // vertex 1 is the closest - } else if (param(0, 2) <= 0 && param(0, 1) >= 1) { + } else if (along[2] <= 0 && along[1] >= 1) { return PointTriangleDistanceType::P_T2; // vertex 2 is the closest } else { return PointTriangleDistanceType::P_T; @@ -223,8 +241,6 @@ EdgeEdgeDistanceType edge_edge_parallel_distance_type( } // clang-format off -template PointEdgeDistanceType point_edge_distance_type(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef); -template PointEdgeDistanceType point_edge_distance_type(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef); 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); diff --git a/src/ipc/distance/distance_type.hpp b/src/ipc/distance/distance_type.hpp index 1093b4456..f3046d782 100644 --- a/src/ipc/distance/distance_type.hpp +++ b/src/ipc/distance/distance_type.hpp @@ -2,6 +2,7 @@ #include +#include #include namespace ipc { @@ -56,16 +57,52 @@ enum class EdgeEdgeDistanceType : uint8_t { AUTO }; -/// @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. -template -PointEdgeDistanceType point_edge_distance_type( - Eigen::ConstRef> p, - Eigen::ConstRef> e0, - Eigen::ConstRef> e1); +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 + } + } +} // namespace detail /// @brief Determine the closest pair between a point and triangle. /// @param p The point. @@ -116,20 +153,33 @@ EdgeEdgeDistanceType edge_edge_parallel_distance_type( // Eigen::MatrixBase, a hard error inside Eigen rather than a SFINAE // failure. -template < - typename DerivedP, - typename DerivedE0, - typename DerivedE1, - std::enable_if_t, int> = 0> +/// @brief Determine the closest pair between a point and edge. +/// +/// Accepts any Eigen expression. The dimension is resolved at compile time when +/// the argument type knows it and with a single branch otherwise; each argument +/// is evaluated exactly once. +/// +/// @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::MatrixBase& p, - const Eigen::MatrixBase& e0, - const Eigen::MatrixBase& e1) + const DerivedP& p, const DerivedE0& e0, const DerivedE1& e1) { + IPC_ASSERT_EIGEN_ARGS(DerivedP, DerivedE0, DerivedE1); using T = typename DerivedP::Scalar; - return point_edge_distance_type( - Eigen::Ref>(p), Eigen::Ref>(e0), - Eigen::Ref>(e1)); + + 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); + } } template < diff --git a/src/ipc/distance/edge_edge.cpp b/src/ipc/distance/edge_edge.cpp index 34cb1fa87..583c724d4 100644 --- a/src/ipc/distance/edge_edge.cpp +++ b/src/ipc/distance/edge_edge.cpp @@ -1,13 +1,12 @@ #include "edge_edge.hpp" +#include #include #include #include #include -#include // std::invalid_argument - -namespace ipc { +namespace ipc::detail { template T edge_edge_distance( @@ -52,8 +51,7 @@ T 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"); } } @@ -131,8 +129,7 @@ Eigen::Vector 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; @@ -250,26 +247,48 @@ Eigen::Matrix 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; } -// clang-format off -template float edge_edge_distance(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, EdgeEdgeDistanceType); -template double edge_edge_distance(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, EdgeEdgeDistanceType); -template ADGrad<9> edge_edge_distance>(Eigen::ConstRef>>, Eigen::ConstRef>>, Eigen::ConstRef>>, Eigen::ConstRef>>, EdgeEdgeDistanceType); -template ADHessian<9> edge_edge_distance>(Eigen::ConstRef>>, Eigen::ConstRef>>, Eigen::ConstRef>>, Eigen::ConstRef>>, EdgeEdgeDistanceType); -template ADGrad<12> edge_edge_distance>(Eigen::ConstRef>>, Eigen::ConstRef>>, Eigen::ConstRef>>, Eigen::ConstRef>>, EdgeEdgeDistanceType); -template ADHessian<12> edge_edge_distance>(Eigen::ConstRef>>, Eigen::ConstRef>>, Eigen::ConstRef>>, Eigen::ConstRef>>, EdgeEdgeDistanceType); -template ADGrad<13> edge_edge_distance>(Eigen::ConstRef>>, Eigen::ConstRef>>, Eigen::ConstRef>>, Eigen::ConstRef>>, EdgeEdgeDistanceType); -template ADHessian<13> edge_edge_distance>(Eigen::ConstRef>>, Eigen::ConstRef>>, Eigen::ConstRef>>, Eigen::ConstRef>>, EdgeEdgeDistanceType); -template Vector12f edge_edge_distance_gradient(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, EdgeEdgeDistanceType); -template Vector12d edge_edge_distance_gradient(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, EdgeEdgeDistanceType); -template Matrix12f edge_edge_distance_hessian(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, EdgeEdgeDistanceType); -template Matrix12d edge_edge_distance_hessian(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, EdgeEdgeDistanceType); -// clang-format on - -} // 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 75f285c27..ab69992ae 100644 --- a/src/ipc/distance/edge_edge.hpp +++ b/src/ipc/distance/edge_edge.hpp @@ -5,53 +5,55 @@ namespace ipc { -/// @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); +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); -/// @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 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); -/// @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); + /// @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); +} // namespace detail // --- EigenExpression wrappers --- @@ -61,19 +63,15 @@ template < 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) -> - typename DerivedEA0::Scalar + const DerivedEA0& ea0, + const DerivedEA1& ea1, + const DerivedEB0& eb0, + const DerivedEB1& eb1, + EdgeEdgeDistanceType dtype = EdgeEdgeDistanceType::AUTO) { + IPC_ASSERT_EIGEN_ARGS(DerivedEA0, DerivedEA1, DerivedEB0, DerivedEB1); using T = typename DerivedEA0::Scalar; - return edge_edge_distance( - Eigen::Ref>(ea0), - Eigen::Ref>(ea1), - Eigen::Ref>(eb0), - Eigen::Ref>(eb1), dtype); + return detail::edge_edge_distance(ea0, ea1, eb0, eb1, dtype); } template < @@ -82,19 +80,15 @@ template < 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, + const DerivedEA0& ea0, + const DerivedEA1& ea1, + const DerivedEB0& eb0, + const DerivedEB1& eb1, EdgeEdgeDistanceType dtype = EdgeEdgeDistanceType::AUTO) - -> Eigen::Vector { + IPC_ASSERT_EIGEN_ARGS(DerivedEA0, DerivedEA1, DerivedEB0, DerivedEB1); using T = typename DerivedEA0::Scalar; - return edge_edge_distance_gradient( - Eigen::Ref>(ea0), - Eigen::Ref>(ea1), - Eigen::Ref>(eb0), - Eigen::Ref>(eb1), dtype); + return detail::edge_edge_distance_gradient(ea0, ea1, eb0, eb1, dtype); } template < @@ -103,19 +97,15 @@ template < 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, + const DerivedEA0& ea0, + const DerivedEA1& ea1, + const DerivedEB0& eb0, + const DerivedEB1& eb1, EdgeEdgeDistanceType dtype = EdgeEdgeDistanceType::AUTO) - -> Eigen::Matrix { + IPC_ASSERT_EIGEN_ARGS(DerivedEA0, DerivedEA1, DerivedEB0, DerivedEB1); using T = typename DerivedEA0::Scalar; - return edge_edge_distance_hessian( - Eigen::Ref>(ea0), - Eigen::Ref>(ea1), - Eigen::Ref>(eb0), - Eigen::Ref>(eb1), dtype); + return detail::edge_edge_distance_hessian(ea0, ea1, eb0, eb1, dtype); } } // namespace ipc diff --git a/src/ipc/distance/line_line.cpp b/src/ipc/distance/line_line.cpp index 1795657e9..636b4cb41 100644 --- a/src/ipc/distance/line_line.cpp +++ b/src/ipc/distance/line_line.cpp @@ -2,586 +2,567 @@ #include -namespace ipc { +namespace ipc::autogen { -namespace autogen { +// 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]) +{ + 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; - // 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]) - { - 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; + 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); +} - 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); - } +// 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]) +{ + 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; - // 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]) - { - 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; + 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); +} - 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); - } +// clang-format off +template void line_line_distance_gradient(float, float, float, float, float, float, float, float, float, float, float, float, float[12]); +template void line_line_distance_gradient(double, double, double, double, double, double, double, double, double, double, double, double, double[12]); +template void line_line_distance_hessian(float, float, float, float, float, float, float, float, float, float, float, float, float[144]); +template void line_line_distance_hessian(double, double, double, double, double, double, double, double, double, double, double, double, double[144]); +// clang-format on - // clang-format off - template void line_line_distance_gradient(float, float, float, float, float, float, float, float, float, float, float, float, float[12]); - template void line_line_distance_gradient(double, double, double, double, double, double, double, double, double, double, double, double, double[12]); - template void line_line_distance_hessian(float, float, float, float, float, float, float, float, float, float, float, float, float[144]); - template void line_line_distance_hessian(double, double, double, double, double, double, double, double, double, double, double, double, double[144]); - // clang-format on -} // namespace autogen -} // namespace ipc +} // namespace ipc::autogen diff --git a/src/ipc/distance/line_line.hpp b/src/ipc/distance/line_line.hpp index 065e046c3..acfdcbe47 100644 --- a/src/ipc/distance/line_line.hpp +++ b/src/ipc/distance/line_line.hpp @@ -18,71 +18,27 @@ namespace autogen { // clang-format on } // namespace autogen -/// @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(); -} - -/// @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. -template -inline Eigen::Vector line_line_distance_gradient( - Eigen::ConstRef> ea0, - Eigen::ConstRef> ea1, - Eigen::ConstRef> eb0, - Eigen::ConstRef> 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; -} - -/// @brief Compute the hessian 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 hessian of the distance wrt ea0, ea1, eb0, and eb1. -template -inline Eigen::Matrix line_line_distance_hessian( - 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; -} - -// --- EigenExpression wrappers --- +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 template < typename DerivedEA0, @@ -90,17 +46,16 @@ template < 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) -> typename DerivedEA0::Scalar + const DerivedEA0& ea0, + const DerivedEA1& ea1, + const DerivedEB0& eb0, + const DerivedEB1& eb1) { + IPC_ASSERT_EIGEN_ARGS(DerivedEA0, DerivedEA1, DerivedEB0, DerivedEB1); using T = typename DerivedEA0::Scalar; - return line_line_distance( - Eigen::Ref>(ea0), - Eigen::Ref>(ea1), - Eigen::Ref>(eb0), - Eigen::Ref>(eb1)); + // NOTE: explicit : the detail overload cannot deduce T from an + // arbitrary Eigen expression. + return detail::line_line_distance(ea0, ea1, eb0, eb1); } template < @@ -109,18 +64,17 @@ template < 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 + const DerivedEA0& ea0, + const DerivedEA1& ea1, + const DerivedEB0& eb0, + const DerivedEB1& eb1) { - using T = typename DerivedEA0::Scalar; - return line_line_distance_gradient( - Eigen::Ref>(ea0), - Eigen::Ref>(ea1), - Eigen::Ref>(eb0), - Eigen::Ref>(eb1)); + IPC_ASSERT_EIGEN_ARGS(DerivedEA0, DerivedEA1, DerivedEB0, DerivedEB1); + 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; } template < @@ -129,18 +83,17 @@ template < 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 + const DerivedEA0& ea0, + const DerivedEA1& ea1, + const DerivedEB0& eb0, + const DerivedEB1& eb1) { - using T = typename DerivedEA0::Scalar; - return line_line_distance_hessian( - Eigen::Ref>(ea0), - Eigen::Ref>(ea1), - Eigen::Ref>(eb0), - Eigen::Ref>(eb1)); + IPC_ASSERT_EIGEN_ARGS(DerivedEA0, DerivedEA1, DerivedEB0, DerivedEB1); + 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 cb0b99b86..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,132 +8,72 @@ namespace ipc { -template -T 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 constexpr (std::is_same_v || std::is_same_v) { - 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"); } - } - - 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 std::invalid_argument( - "Invalid distance type for point-edge distance!"); - } -} - -template -VectorMax9 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); - } - - VectorMax9 grad = VectorMax9::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 VectorMax6 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; + 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; + } - default: - throw std::invalid_argument( - "Invalid distance type for point-edge distance gradient!"); - } + case PointEdgeDistanceType::P_E: + hess = point_line_distance_hessian(p, e0, e1); + break; - return grad; -} - -template -MatrixMax9 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); + default: + throw_invalid_distance_type("point_edge_distance_hessian"); + } + return hess; } - MatrixMax9 hess = MatrixMax9::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 MatrixMax6 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; - } +#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_E: - hess = point_line_distance_hessian(p, e0, e1); - break; + 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); - default: - throw std::invalid_argument( - "Invalid distance type for point-edge distance hessian!"); - } +#undef IPC_INSTANTIATE_POINT_EDGE_DISTANCE_HESSIAN - return hess; -} - -// clang-format off -template float point_edge_distance(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, PointEdgeDistanceType); -template double point_edge_distance(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, PointEdgeDistanceType); -template VectorMax9 point_edge_distance_gradient(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, PointEdgeDistanceType); -template VectorMax9 point_edge_distance_gradient(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, PointEdgeDistanceType); -template MatrixMax9 point_edge_distance_hessian(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, PointEdgeDistanceType); -template MatrixMax9 point_edge_distance_hessian(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, PointEdgeDistanceType); -// clang-format on +} // namespace detail } // namespace ipc diff --git a/src/ipc/distance/point_edge.hpp b/src/ipc/distance/point_edge.hpp index 9e20037ae..30f45748b 100644 --- a/src/ipc/distance/point_edge.hpp +++ b/src/ipc/distance/point_edge.hpp @@ -1,10 +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) + { + 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) + { + 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); +} // 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. @@ -12,12 +128,27 @@ 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. -template -T point_edge_distance( - Eigen::ConstRef> p, - Eigen::ConstRef> e0, - Eigen::ConstRef> e1, - PointEdgeDistanceType dtype = PointEdgeDistanceType::AUTO); +template +inline auto point_edge_distance( + const DerivedP& p, + const DerivedE0& e0, + const DerivedE1& e1, + PointEdgeDistanceType dtype = PointEdgeDistanceType::AUTO) +{ + IPC_ASSERT_EIGEN_ARGS(DerivedP, DerivedE0, DerivedE1); + 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. @@ -26,12 +157,28 @@ T point_edge_distance( /// @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 -VectorMax9 point_edge_distance_gradient( - Eigen::ConstRef> p, - Eigen::ConstRef> e0, - Eigen::ConstRef> e1, - PointEdgeDistanceType dtype = PointEdgeDistanceType::AUTO); +template +inline auto point_edge_distance_gradient( + const DerivedP& p, + const DerivedE0& e0, + const DerivedE1& e1, + PointEdgeDistanceType dtype = PointEdgeDistanceType::AUTO) +{ + IPC_ASSERT_EIGEN_ARGS(DerivedP, DerivedE0, DerivedE1); + 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. @@ -40,55 +187,27 @@ VectorMax9 point_edge_distance_gradient( /// @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 -MatrixMax9 point_edge_distance_hessian( - Eigen::ConstRef> p, - Eigen::ConstRef> e0, - Eigen::ConstRef> e1, - PointEdgeDistanceType dtype = PointEdgeDistanceType::AUTO); - -// --- EigenExpression wrappers --- - -template -inline auto point_edge_distance( - const Eigen::MatrixBase& p, - const Eigen::MatrixBase& e0, - const Eigen::MatrixBase& e1, - PointEdgeDistanceType dtype = PointEdgeDistanceType::AUTO) -> - typename DerivedP::Scalar -{ - using T = typename DerivedP::Scalar; - return point_edge_distance( - Eigen::Ref>(p), Eigen::Ref>(e0), - Eigen::Ref>(e1), dtype); -} - -template -inline auto point_edge_distance_gradient( - const Eigen::MatrixBase& p, - const Eigen::MatrixBase& e0, - const Eigen::MatrixBase& e1, - PointEdgeDistanceType dtype = PointEdgeDistanceType::AUTO) - -> VectorMax9 -{ - using T = typename DerivedP::Scalar; - return point_edge_distance_gradient( - Eigen::Ref>(p), Eigen::Ref>(e0), - Eigen::Ref>(e1), dtype); -} - template inline auto point_edge_distance_hessian( - const Eigen::MatrixBase& p, - const Eigen::MatrixBase& e0, - const Eigen::MatrixBase& e1, + const DerivedP& p, + const DerivedE0& e0, + const DerivedE1& e1, PointEdgeDistanceType dtype = PointEdgeDistanceType::AUTO) - -> MatrixMax9 { + IPC_ASSERT_EIGEN_ARGS(DerivedP, DerivedE0, DerivedE1); using T = typename DerivedP::Scalar; - return point_edge_distance_hessian( - Eigen::Ref>(p), Eigen::Ref>(e0), - Eigen::Ref>(e1), dtype); + 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.hpp b/src/ipc/distance/point_line.hpp index b725b708c..a0a11f24a 100644 --- a/src/ipc/distance/point_line.hpp +++ b/src/ipc/distance/point_line.hpp @@ -20,31 +20,112 @@ namespace autogen { 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. -template -inline T point_line_distance( - Eigen::ConstRef> p, - Eigen::ConstRef> e0, - Eigen::ConstRef> e1) +template +inline auto +point_line_distance(const DerivedP& p, const DerivedE0& e0, const DerivedE1& e1) { - assert(p.size() == 2 || p.size() == 3); - assert(e0.size() == 2 || e0.size() == 3); - assert(e1.size() == 2 || e1.size() == 3); + IPC_ASSERT_EIGEN_ARGS(DerivedP, DerivedE0, DerivedE1); + using T = typename DerivedP::Scalar; - if (p.size() == 2) { - const Eigen::Vector 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(); + 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 { - 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(); + assert(p.size() == 3); + return detail::point_line_distance(p, e0, e1); } } @@ -54,94 +135,68 @@ inline T 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. -template -inline VectorMax9 point_line_distance_gradient( - Eigen::ConstRef> p, - Eigen::ConstRef> e0, - Eigen::ConstRef> e1) +template +inline auto point_line_distance_gradient( + const DerivedP& p, const DerivedE0& e0, const DerivedE1& e1) { - const int dim = p.size(); - assert(e0.size() == dim); - assert(e1.size() == dim); - - VectorMax9 grad(3 * dim); - if (dim == 2) { + IPC_ASSERT_EIGEN_ARGS(DerivedP, DerivedE0, DerivedE1); + 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; } - return grad; } /// @brief Compute the hessian of the distance between a point and line. +/// /// @note The distance is actually squared distance. +/// +/// Accepts any Eigen expression. When the dimension is known at compile time +/// the return type is the fixed-size Eigen::Matrix; +/// otherwise it is MatrixMax9, filled directly by the generated code +/// (wrapping the 648-byte fixed-size result instead measured as a 7-13% +/// regression). +/// /// @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 MatrixMax9 point_line_distance_hessian( - Eigen::ConstRef> p, - Eigen::ConstRef> e0, - Eigen::ConstRef> e1) +template +inline auto point_line_distance_hessian( + const DerivedP& p, const DerivedE0& e0, const DerivedE1& e1) { - const int dim = p.size(); - assert(e0.size() == dim); - assert(e1.size() == dim); - - MatrixMax9 hess(3 * dim, 3 * dim); - if (dim == 2) { + IPC_ASSERT_EIGEN_ARGS(DerivedP, DerivedE0, DerivedE1); + 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; } - return hess; -} - -// --- EigenExpression wrappers --- - -template -inline auto point_line_distance( - const Eigen::MatrixBase& p, - const Eigen::MatrixBase& e0, - const Eigen::MatrixBase& e1) -> typename DerivedP::Scalar -{ - using T = typename DerivedP::Scalar; - return point_line_distance( - Eigen::Ref>(p), Eigen::Ref>(e0), - Eigen::Ref>(e1)); -} - -template -inline auto point_line_distance_gradient( - const Eigen::MatrixBase& p, - const Eigen::MatrixBase& e0, - const Eigen::MatrixBase& e1) - -> VectorMax9 -{ - using T = typename DerivedP::Scalar; - return point_line_distance_gradient( - Eigen::Ref>(p), Eigen::Ref>(e0), - Eigen::Ref>(e1)); -} - -template -inline auto point_line_distance_hessian( - const Eigen::MatrixBase& p, - const Eigen::MatrixBase& e0, - const Eigen::MatrixBase& e1) - -> MatrixMax9 -{ - using T = typename DerivedP::Scalar; - return point_line_distance_hessian( - Eigen::Ref>(p), Eigen::Ref>(e0), - Eigen::Ref>(e1)); } } // namespace ipc diff --git a/src/ipc/distance/point_point.hpp b/src/ipc/distance/point_point.hpp index 7a609c086..d10f819ea 100644 --- a/src/ipc/distance/point_point.hpp +++ b/src/ipc/distance/point_point.hpp @@ -2,94 +2,159 @@ #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. +/// +/// Accepts any Eigen expression. The dimension is resolved at compile time when +/// the argument type knows it and with a single branch otherwise; each argument +/// is evaluated exactly once. +/// /// @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) +template +inline auto point_point_distance(const DerivedP0& p0, const DerivedP1& p1) { - return (p1 - p0).squaredNorm(); + IPC_ASSERT_EIGEN_ARGS(DerivedP0, DerivedP1); + 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. +/// +/// Accepts any Eigen expression. When the dimension is known at compile time +/// the return type is the fixed-size Eigen::Vector; otherwise it is +/// VectorMax6. +/// /// @param p0 The first point. /// @param p1 The second point. /// @return The computed gradient. -template -inline VectorMax6 point_point_distance_gradient( - Eigen::ConstRef> p0, Eigen::ConstRef> p1) +template +inline auto +point_point_distance_gradient(const DerivedP0& p0, const DerivedP1& p1) { - const int dim = p0.size(); - assert(p1.size() == dim); - - VectorMax6 grad(2 * dim); - grad.head(dim) = T(2) * (p0 - p1); - grad.tail(dim) = -grad.head(dim); - return grad; + IPC_ASSERT_EIGEN_ARGS(DerivedP0, DerivedP1); + 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. +/// +/// Accepts any Eigen expression. When the dimension is known at compile time +/// the return type is the fixed-size Eigen::Matrix; +/// otherwise it is MatrixMax6. +/// /// @param p0 The first point. /// @param p1 The second point. /// @return The computed hessian. -template -inline MatrixMax6 point_point_distance_hessian( - Eigen::ConstRef> p0, Eigen::ConstRef> p1) -{ - const int dim = p0.size(); - assert(p1.size() == dim); - - MatrixMax6 hess(2 * dim, 2 * dim); - hess.setZero(); - hess.diagonal().setConstant(T(2)); - for (int i = 0; i < dim; i++) { - hess(i, i + dim) = hess(i + dim, i) = T(-2); - } - return hess; -} - -// --- EigenExpression wrappers --- - -template -inline auto point_point_distance( - const Eigen::MatrixBase& p0, - const Eigen::MatrixBase& p1) -> typename DerivedP0::Scalar -{ - using T = typename DerivedP0::Scalar; - return point_point_distance( - Eigen::Ref>(p0), - Eigen::Ref>(p1)); -} - -template -inline auto point_point_distance_gradient( - const Eigen::MatrixBase& p0, - const Eigen::MatrixBase& p1) - -> VectorMax6 -{ - using T = typename DerivedP0::Scalar; - return point_point_distance_gradient( - Eigen::Ref>(p0), - Eigen::Ref>(p1)); -} - template -inline auto point_point_distance_hessian( - const Eigen::MatrixBase& p0, - const Eigen::MatrixBase& p1) - -> MatrixMax6 +inline auto +point_point_distance_hessian(const DerivedP0& p0, const DerivedP1& p1) { + IPC_ASSERT_EIGEN_ARGS(DerivedP0, DerivedP1); using T = typename DerivedP0::Scalar; - return point_point_distance_hessian( - Eigen::Ref>(p0), - Eigen::Ref>(p1)); + 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 f0fa96c07..74f1012b4 100644 --- a/src/ipc/distance/point_triangle.cpp +++ b/src/ipc/distance/point_triangle.cpp @@ -1,5 +1,6 @@ #include "point_triangle.hpp" +#include #include #include #include @@ -7,7 +8,7 @@ #include // std::invalid_argument -namespace ipc { +namespace ipc::detail { template T point_triangle_distance( @@ -46,8 +47,7 @@ T 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"); } } @@ -112,8 +112,7 @@ Eigen::Vector 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; @@ -213,24 +212,46 @@ Eigen::Matrix 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; } -// clang-format off -template float point_triangle_distance(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, PointTriangleDistanceType); -template double point_triangle_distance(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, PointTriangleDistanceType); -template ADGrad<12> point_triangle_distance>(Eigen::ConstRef>>, Eigen::ConstRef>>, Eigen::ConstRef>>, Eigen::ConstRef>>, PointTriangleDistanceType); -template ADHessian<12> point_triangle_distance>(Eigen::ConstRef>>, Eigen::ConstRef>>, Eigen::ConstRef>>, Eigen::ConstRef>>, PointTriangleDistanceType); -template ADGrad<13> point_triangle_distance>(Eigen::ConstRef>>, Eigen::ConstRef>>, Eigen::ConstRef>>, Eigen::ConstRef>>, PointTriangleDistanceType); -template ADHessian<13> point_triangle_distance>(Eigen::ConstRef>>, Eigen::ConstRef>>, Eigen::ConstRef>>, Eigen::ConstRef>>, PointTriangleDistanceType); -template Vector12f point_triangle_distance_gradient(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, PointTriangleDistanceType); -template Vector12d point_triangle_distance_gradient(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, PointTriangleDistanceType); -template Matrix12f point_triangle_distance_hessian(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, PointTriangleDistanceType); -template Matrix12d point_triangle_distance_hessian(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, PointTriangleDistanceType); -// clang-format on - -} // 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 e2988ff10..79a6f26cc 100644 --- a/src/ipc/distance/point_triangle.hpp +++ b/src/ipc/distance/point_triangle.hpp @@ -5,53 +5,55 @@ namespace ipc { -/// @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); +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); -/// @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 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); -/// @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); + /// @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); +} // namespace detail // --- EigenExpression wrappers --- @@ -61,19 +63,15 @@ template < 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) -> - typename DerivedP::Scalar + const DerivedP& p, + const DerivedT0& t0, + const DerivedT1& t1, + const DerivedT2& t2, + PointTriangleDistanceType dtype = PointTriangleDistanceType::AUTO) { + IPC_ASSERT_EIGEN_ARGS(DerivedP, DerivedT0, DerivedT1, DerivedT2); using T = typename DerivedP::Scalar; - return point_triangle_distance( - Eigen::Ref>(p), - Eigen::Ref>(t0), - Eigen::Ref>(t1), - Eigen::Ref>(t2), dtype); + return detail::point_triangle_distance(p, t0, t1, t2, dtype); } template < @@ -82,19 +80,15 @@ template < 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, + const DerivedP& p, + const DerivedT0& t0, + const DerivedT1& t1, + const DerivedT2& t2, PointTriangleDistanceType dtype = PointTriangleDistanceType::AUTO) - -> Eigen::Vector { + IPC_ASSERT_EIGEN_ARGS(DerivedP, DerivedT0, DerivedT1, DerivedT2); using T = typename DerivedP::Scalar; - return point_triangle_distance_gradient( - Eigen::Ref>(p), - Eigen::Ref>(t0), - Eigen::Ref>(t1), - Eigen::Ref>(t2), dtype); + return detail::point_triangle_distance_gradient(p, t0, t1, t2, dtype); } template < @@ -103,19 +97,15 @@ template < 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, + const DerivedP& p, + const DerivedT0& t0, + const DerivedT1& t1, + const DerivedT2& t2, PointTriangleDistanceType dtype = PointTriangleDistanceType::AUTO) - -> Eigen::Matrix { + IPC_ASSERT_EIGEN_ARGS(DerivedP, DerivedT0, DerivedT1, DerivedT2); using T = typename DerivedP::Scalar; - return point_triangle_distance_hessian( - Eigen::Ref>(p), - Eigen::Ref>(t0), - Eigen::Ref>(t1), - Eigen::Ref>(t2), dtype); + return detail::point_triangle_distance_hessian(p, t0, t1, t2, dtype); } } // namespace ipc diff --git a/src/ipc/geometry/normal.cpp b/src/ipc/geometry/normal.cpp index 81e8d6d69..e31ccef6f 100644 --- a/src/ipc/geometry/normal.cpp +++ b/src/ipc/geometry/normal.cpp @@ -6,35 +6,6 @@ namespace ipc { -template -std::tuple, MatrixMax3> -normalization_and_jacobian(Eigen::ConstRef> x) -{ - const T norm = x.norm(); - const VectorMax3 xhat = x / norm; - const auto I = MatrixMax3::Identity(x.size(), x.size()); - const MatrixMax3 J = (I - (xhat * xhat.transpose())) / norm; - return std::make_tuple(xhat, J); -} - -template -std::tuple, MatrixMax3, std::array, 3>> -normalization_and_jacobian_and_hessian(Eigen::ConstRef> x) -{ - const T norm = x.norm(); - const VectorMax3 xhat = x / norm; - const auto I = MatrixMax3::Identity(x.size(), x.size()); - const MatrixMax3 J = (I - (xhat * xhat.transpose())) / norm; - - std::array, 3> 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); -} - // --- point-line normal functions ------------------------------------------- template @@ -409,10 +380,6 @@ Eigen::Matrix line_line_normal_hessian( } // clang-format off -template std::tuple normalization_and_jacobian(Eigen::ConstRef); -template std::tuple normalization_and_jacobian(Eigen::ConstRef); -template std::tuple> normalization_and_jacobian_and_hessian(Eigen::ConstRef); -template std::tuple> normalization_and_jacobian_and_hessian(Eigen::ConstRef); 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); diff --git a/src/ipc/geometry/normal.hpp b/src/ipc/geometry/normal.hpp index 42e064947..5daecb98e 100644 --- a/src/ipc/geometry/normal.hpp +++ b/src/ipc/geometry/normal.hpp @@ -2,41 +2,161 @@ #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. -template -std::tuple, MatrixMax3> -normalization_and_jacobian(Eigen::ConstRef> x); +template +inline auto normalization_and_jacobian(const DerivedX& x) +{ + IPC_ASSERT_EIGEN_ARGS(DerivedX); + 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. -template -inline MatrixMax3 normalization_jacobian(Eigen::ConstRef> x) +template +inline auto normalization_jacobian(const DerivedX& x) { - return std::get<1>(normalization_and_jacobian(x)); + IPC_ASSERT_EIGEN_ARGS(DerivedX); + 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 x The input vector. /// @return A tuple of the normalized vector, its Jacobian, and its Hessian. -template -std::tuple, MatrixMax3, std::array, 3>> -normalization_and_jacobian_and_hessian(Eigen::ConstRef> x); +template +inline auto normalization_and_jacobian_and_hessian(const DerivedX& x) +{ + IPC_ASSERT_EIGEN_ARGS(DerivedX); + 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. -template -inline std::array, 3> -normalization_hessian(Eigen::ConstRef> x) +template +inline auto normalization_hessian(const DerivedX& x) { return std::get<2>(normalization_and_jacobian_and_hessian(x)); } @@ -132,7 +252,7 @@ inline MatrixMax point_line_normal_jacobian( Eigen::ConstRef> e1) { // ∂n̂/∂x = ∂n̂/∂n * ∂n/∂x - return normalization_jacobian(point_line_unnormalized_normal(p, e0, e1)) + return normalization_jacobian(point_line_unnormalized_normal(p, e0, e1)) * point_line_unnormalized_normal_jacobian(p, e0, e1); } @@ -228,7 +348,7 @@ inline Eigen::Matrix triangle_normal_jacobian( Eigen::ConstRef> c) { // ∂n̂/∂x = ∂n̂/∂n * ∂n/∂x - return normalization_jacobian(triangle_unnormalized_normal(a, b, c)) + return normalization_jacobian(triangle_unnormalized_normal(a, b, c)) * triangle_unnormalized_normal_jacobian(a, b, c); } @@ -321,7 +441,7 @@ inline Eigen::Matrix line_line_normal_jacobian( Eigen::ConstRef> eb1) { // ∂n̂/∂x = ∂n̂/∂n * ∂n/∂x - return normalization_jacobian( + return normalization_jacobian( line_line_unnormalized_normal(ea0, ea1, eb0, eb1)) * line_line_unnormalized_normal_jacobian(ea0, ea1, eb0, eb1); } @@ -357,44 +477,6 @@ Eigen::Matrix line_line_normal_hessian( // --- EigenExpression wrappers --- -template < - typename DerivedX, - std::enable_if_t, int> = 0> -inline auto normalization_and_jacobian(const Eigen::MatrixBase& x) -{ - using T = typename DerivedX::Scalar; - return normalization_and_jacobian(Eigen::Ref>(x)); -} - -template < - typename DerivedX, - std::enable_if_t, int> = 0> -inline auto normalization_jacobian(const Eigen::MatrixBase& x) -{ - using T = typename DerivedX::Scalar; - return normalization_jacobian(Eigen::Ref>(x)); -} - -template < - typename DerivedX, - std::enable_if_t, int> = 0> -inline auto -normalization_and_jacobian_and_hessian(const Eigen::MatrixBase& x) -{ - using T = typename DerivedX::Scalar; - return normalization_and_jacobian_and_hessian( - Eigen::Ref>(x)); -} - -template < - typename DerivedX, - std::enable_if_t, int> = 0> -inline auto normalization_hessian(const Eigen::MatrixBase& x) -{ - using T = typename DerivedX::Scalar; - return normalization_hessian(Eigen::Ref>(x)); -} - template < typename DerivedX, std::enable_if_t, int> = 0> diff --git a/src/ipc/smooth_contact/distance/mollifier.tpp b/src/ipc/smooth_contact/distance/mollifier.tpp index 07603f0f9..353b9e2e9 100644 --- a/src/ipc/smooth_contact/distance/mollifier.tpp +++ b/src/ipc/smooth_contact/distance/mollifier.tpp @@ -75,16 +75,13 @@ scalar point_face_mollifier( // whenever this function is nonzero the point-edge distance equals // the point-line distance return Math::mollifier( - (point_line_distance(p, e0, e1) - - dist_sqr) + (point_line_distance(p, e0, e1) - dist_sqr) / MOLLIFIER_THRESHOLD_EPS / dist_sqr) * Math::mollifier( - (point_line_distance(p, e2, e1) - - dist_sqr) + (point_line_distance(p, e2, e1) - dist_sqr) / MOLLIFIER_THRESHOLD_EPS / dist_sqr) * Math::mollifier( - (point_line_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 7d8310357..b000fdcec 100644 --- a/src/ipc/smooth_contact/distance/point_edge.cpp +++ b/src/ipc/smooth_contact/distance/point_edge.cpp @@ -13,11 +13,11 @@ scalar PointEdgeDistance::point_edge_sqr_distance( { switch (dtype) { case PointEdgeDistanceType::P_E: - return point_line_distance(p, e0, e1); + return point_line_distance(p, e0, e1); case PointEdgeDistanceType::P_E0: - return point_point_distance(p, e0); + return point_point_distance(p, e0); case PointEdgeDistanceType::P_E1: - return point_point_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/primitive_distance.tpp b/src/ipc/smooth_contact/distance/primitive_distance.tpp index 618fd7253..700b91187 100644 --- a/src/ipc/smooth_contact/distance/primitive_distance.tpp +++ b/src/ipc/smooth_contact/distance/primitive_distance.tpp @@ -19,7 +19,7 @@ public: const Eigen::Vector& x, typename PrimitiveDistType::type dtype) { - return point_triangle_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); @@ -55,7 +55,7 @@ public: const Eigen::Vector& x, typename PrimitiveDistType::type dtype) { - return edge_edge_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/utils/eigen_ext.hpp b/src/ipc/utils/eigen_ext.hpp index 65ad68438..71195d3bc 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 @@ -222,6 +223,48 @@ using HessianType = std:: /**@}*/ +/// @brief True iff `T` is an Eigen expression (a stored matrix, block, map, or +/// lazy expression) rather than a scalar. +/// +/// A pure detection trait: it inspects only members of `T` itself, so it never +/// instantiates `Eigen::MatrixBase` — which is ill-formed when `T` is a +/// scalar such as `double` or an autodiff type. +template +inline constexpr bool is_eigen_expression_v = false; + +template +inline constexpr bool is_eigen_expression_v< + T, + std::void_t< + typename T::Scalar, + decltype(std::declval().rows()), + decltype(std::declval().cols())>> = true; + +/// @brief True iff every `Ts` is an Eigen expression. +template +inline constexpr bool are_eigen_expressions_v = + (is_eigen_expression_v && ...); + +/// @brief Assert that every deduced argument type is an Eigen expression. +/// +/// Place this as the first statement of an Eigen front end so that an explicit +/// scalar template argument produces one readable diagnostic instead of a wall +/// of Eigen internals. +#define IPC_ASSERT_EIGEN_ARGS(...) \ + static_assert( \ + ::ipc::are_eigen_expressions_v<__VA_ARGS__>, \ + "Pass Eigen vectors or expressions; the scalar type is deduced from " \ + "the arguments. Write f(a, b, ...), not f(a, b, ...).") + +/// @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/benchmark_eigen.cpp b/tests/src/tests/benchmark_eigen.cpp index 08681d654..93860c8ec 100644 --- a/tests/src/tests/benchmark_eigen.cpp +++ b/tests/src/tests/benchmark_eigen.cpp @@ -1,162 +1,234 @@ #include #include -#include -#include -#include #include #include +#include +#include +#include +#include +#include #include -using namespace ipc; +#include -TEST_CASE("Template dynamic vs static", "[!benchmark][eigen]") -{ - const Eigen::MatrixXd V = Eigen::MatrixXd::Random(100, 3); - - 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(); } -// 1. The fast worker (Compiler knows N) -template -inline double fast_dist( - const Eigen::MatrixBase& p0, - const Eigen::MatrixBase& p1) +// --- 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 DerivedP& p, const DerivedE0& e0, const DerivedE1& e1) { - return (p1.template head() - p0.template head()).squaredNorm(); + 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(); + } } -// 2. The dispatcher -double point_point_distance_dispatch( - Eigen::ConstRef p0, Eigen::ConstRef p1) +/// 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) { - // By the time we reach the worker, N is a constant - if (p0.size() == 3) { - return fast_dist<3>(p0, p1); + if (p.size() == 2) { + return pl_body<2>(p.head<2>(), e0.head<2>(), e1.head<2>()); } - if (p0.size() == 2) { - return fast_dist<2>(p0, p1); - } - return (p1 - p0).squaredNorm(); + return pl_body<3>(p.head<3>(), e0.head<3>(), e1.head<3>()); } -// 2. The dispatcher -template -double point_point_distance_templated( - Eigen::Matrix p0, - Eigen::Matrix p1) +/// 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 (p1 - p0).squaredNorm(); + return pl_body<3>(p, e0, e1); } -inline double point_point_distance_templated( - Eigen::ConstRef p0, Eigen::ConstRef p1) +// --- point-edge: the same question, with the distance-type dispatch on top --- + +/// @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 DerivedP& p, const DerivedE0& e0, const DerivedE1& e1) { - if (p0.size() == 3) { - return point_point_distance_templated<3, 3>(p0, p1); + const Eigen::Vector e = e1 - e0; + const double e_length_sqr = e.squaredNorm(); + if (e_length_sqr == 0) { + return PointEdgeDistanceType::P_E0; } - // else if (p0.size() == 2) - return point_point_distance_templated<2, 2>(p0, p1); - // return point_point_distance_templated<-1, 3>(p0, p1); + 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; } -TEST_CASE("Templated vs Eigen::Ref (PP)", "[!benchmark][eigen][pp][ref]") +/// @brief The shared fixed-size body, as pl_body but with the dtype dispatch. +template +inline double pe_body( + const DerivedP& p, + const DerivedE0& e0, + const DerivedE1& e1, + PointEdgeDistanceType dtype) { - Eigen::MatrixXd V = Eigen::MatrixXd::Random(100, 3); - - const int p0i = 0, p1i = V.rows() - 1; - - BENCHMARK("Ref") - { - return point_point_distance_ref(V.row(p0i), V.row(p1i)); - }; - - 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); - }; - - 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); - }; - - BENCHMARK("Ref Nd") - { - return point_point_distance(V.row(p0i), V.row(p1i)); - }; - - BENCHMARK("Copy") - { - return point_point_distance_copy(V.row(p0i), V.row(p1i)); - }; - - BENCHMARK("Copy Nd") - { - return point_point_distance_copy_Nd(V.row(p0i), V.row(p1i)); - }; - - BENCHMARK("Dispatch Nd") - { - return point_point_distance_dispatch(V.row(p0i), V.row(p1i)); - }; + 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("Templated") - { - return point_point_distance_templated<3, 3>(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("Templated(Nd)") - { - return point_point_distance_templated(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 line_line_distance_ref( +double ll_ref_3d( Eigen::ConstRef ea0, Eigen::ConstRef ea1, Eigen::ConstRef eb0, @@ -167,24 +239,10 @@ double line_line_distance_ref( return line_to_line * line_to_line / normal.squaredNorm(); } -template < - typename DerivedA, - typename DerivedB, - typename DerivedC, - typename DerivedD> -double line_line_distance_templated( - const Eigen::MatrixBase& ea0, - const Eigen::MatrixBase& ea1, - const Eigen::MatrixBase& eb0, - const Eigen::MatrixBase& eb1) -{ - const Eigen::Vector3d normal = (ea1 - ea0).template block<3, 1>(0, 0).cross( - (eb1 - eb0).template block<3, 1>(0, 0)); - const double line_to_line = (eb0 - ea0).dot(normal); - return line_to_line * line_to_line / normal.squaredNorm(); -} - -double line_line_distance_row_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, @@ -195,7 +253,7 @@ double line_line_distance_row_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, @@ -206,197 +264,513 @@ 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) +{ + 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 ll_hess_out_param( + Eigen::ConstRef ea0, + Eigen::ConstRef ea1, + Eigen::ConstRef eb0, + Eigen::ConstRef eb1, + Eigen::PlainObjectBase& hess) { - const int N = 100; + 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()); +} + +// --- 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) +{ + 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(); + } + } +} + +} // 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::Matrix VR = V; + const Eigen::ArrayXi idx = random_indices(N); - // Use a heap-allocated vector of random indices to defeat LICM. The prime - // size means i % M has no power-of-2 period, and the heap pointer prevents - // the compiler from tracing provenance back to V. Catch2 has no - // DoNotOptimize, so this is the most portable approach. - constexpr int M = 997; // prime - Eigen::ArrayXi idx = - Eigen::ArrayXi::Random(M).abs().unaryExpr([&](int x) { return x % N; }); +#define PP_ROWS(f) f(V.row(idx[i % IDX_M]), V.row(idx[(i + 1) % IDX_M])) - BENCHMARK("Templated", i) + 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) { - return line_line_distance_templated( - V.row(idx[i % M]), V.row(idx[(i + 1) % M]), V.row(idx[(i + 2) % M]), - V.row(idx[(i + 3) % M])); + 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); + } - BENCHMARK("Lib", i) + // 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 line_line_distance( - V.row(idx[i % M]), V.row(idx[(i + 1) % M]), V.row(idx[(i + 2) % M]), - V.row(idx[(i + 3) % M])); + 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("Ref", i) + BENCHMARK("runtime dtype rows: Lib", i) { - return line_line_distance_ref( - V.row(idx[i % M]), V.row(idx[(i + 1) % M]), V.row(idx[(i + 2) % M]), - V.row(idx[(i + 3) % M])); + 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("Ref(Copy)", i) + BENCHMARK("AUTO Vector3d: Lib", i) + { + return AT3_D(point_edge_distance, P3, PointEdgeDistanceType::AUTO); + }; + BENCHMARK("AUTO VectorMax3d: Lib", i) { - const Eigen::Vector3d ea0 = V.row(idx[(i + 0) % M]); - const Eigen::Vector3d ea1 = V.row(idx[(i + 1) % M]); - const Eigen::Vector3d eb0 = V.row(idx[(i + 2) % M]); - const Eigen::Vector3d eb1 = V.row(idx[(i + 3) % M]); - return line_line_distance_ref(ea0, ea1, eb0, eb1); + return AT3_D(point_edge_distance, PN, PointEdgeDistanceType::AUTO); + }; + BENCHMARK("AUTO VectorMax3d: Ref", i) + { + return AT3_D(pe_ref_maxN, PN, PointEdgeDistanceType::AUTO); }; - BENCHMARK("Ref (VR)", i) + // An argument the front end must not evaluate more than once. + BENCHMARK("AUTO lazy expr: Lib", i) { - return line_line_distance( - VR.row(idx[i % M]), VR.row(idx[(i + 1) % M]), - VR.row(idx[(i + 2) % M]), VR.row(idx[(i + 3) % M])); + 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); }; +} - BENCHMARK("RowRef", i) +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 line_line_distance_row_ref( - V.row(idx[i % M]), V.row(idx[(i + 1) % M]), V.row(idx[(i + 2) % M]), - V.row(idx[(i + 3) % M])); + return ROWS4(ll_row_ref_3d); }; + BENCHMARK("rows: by-value Vector3d", i) { return ROWS4(ll_copy_3d); }; - BENCHMARK("Copy", i) + // 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_copy( - V.row(idx[i % M]), V.row(idx[(i + 1) % M]), V.row(idx[(i + 2) % M]), - V.row(idx[(i + 3) % M])); + 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); }; +} -template -void line_line_distance_hessian( - const Eigen::Ref& ea0, - const Eigen::Ref& ea1, - const Eigen::Ref& eb0, - const Eigen::Ref& eb1, - Eigen::PlainObjectBase& hess) +TEST_CASE("Parameter type (EE)", "[!benchmark][eigen][ee][param]") { - hess.resize( - ea0.size() + ea1.size() + eb0.size() + eb1.size(), - ea0.size() + ea1.size() + eb0.size() + eb1.size()); - 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()); + 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("Return type", "[!benchmark][eigen][return]") +TEST_CASE("Parameter type (PT)", "[!benchmark][eigen][pt][param]") { - const Eigen::MatrixXd V = Eigen::MatrixXd::Random(100, 3); + constexpr int N = 100; + const Eigen::MatrixXd V = Eigen::MatrixXd::Random(N, 3); + const Eigen::ArrayXi idx = random_indices(N); - int ea0i = 0, ea1i = 50, eb0i = 75, eb1i = 99; + std::vector P3(N); + for (int k = 0; k < N; ++k) { + P3[k] = V.row(k); + } - 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); + 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); + }; - BENCHMARK("Explicit") + // Static vs dynamic argument type on the Hessian, which is the heaviest + // consumer of the fixed-size return types. + BENCHMARK("hessian rows: Lib", i) { - const Matrix12d hess = line_line_distance_hessian(ea0, ea1, eb0, eb1); - return hess; + 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); + }; -TEST_CASE("Float vs Double: Distance", "[!benchmark][eigen][float]") -{ - constexpr int N = 100, M = 1000; - const Eigen::MatrixXf Vf = Eigen::MatrixXf::Random(N, 3); - const Eigen::MatrixXd Vd = Vf.cast(); + // 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("float") - { - float sum = 0.0; - for (int i = 0; i < M; ++i) { - sum += barrier( - edge_edge_distance( - Vf.row(i % N), Vf.row((50 + i) % N), Vf.row((75 + i) % N), - Vf.row((99 + i) % N)), - 1.0f); - } - return sum; - }; - BENCHMARK("double") - { - double sum = 0.0; - for (int i = 0; i < M; ++i) { - sum += barrier( - edge_edge_distance( - Vd.row(i % N), Vd.row((50 + i) % N), Vd.row((75 + i) % N), - Vd.row((99 + i) % N)), - 1.0); - } - return sum; + 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: Hessian", "[!benchmark][eigen][float]") +TEST_CASE("Float vs double", "[!benchmark][eigen][float]") { - constexpr int N = 100, M = 1000; + 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); - BENCHMARK("float") - { - Eigen::Matrix hess = - Eigen::Matrix::Zero(); - for (int i = 0; i < M; ++i) { - auto d = edge_edge_distance( - Vf.row(i % N), Vf.row((50 + i) % N), Vf.row((75 + i) % N), - Vf.row((99 + i) % N)); - auto grad_d = edge_edge_distance_gradient( - Vf.row(i % N), Vf.row((50 + i) % N), Vf.row((75 + i) % N), - Vf.row((99 + i) % N)); - auto hess_d = edge_edge_distance_hessian( - Vf.row(i % N), Vf.row((50 + i) % N), Vf.row((75 + i) % N), - Vf.row((99 + i) % N)); - - auto db = barrier_first_derivative(d, 1.0f); - auto d2b = barrier_second_derivative(d, 1.0f); - - hess += db * hess_d + (d2b * grad_d) * grad_d.transpose(); - } - return hess; +#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("double") - { - Eigen::Matrix hess = - Eigen::Matrix::Zero(); - for (int i = 0; i < M; ++i) { - auto d = edge_edge_distance( - Vd.row(i % N), Vd.row((50 + i) % N), Vd.row((75 + i) % N), - Vd.row((99 + i) % N)); - auto grad_d = edge_edge_distance_gradient( - Vd.row(i % N), Vd.row((50 + i) % N), Vd.row((75 + i) % N), - Vd.row((99 + i) % N)); - auto hess_d = edge_edge_distance_hessian( - Vd.row(i % N), Vd.row((50 + i) % N), Vd.row((75 + i) % N), - Vd.row((99 + i) % N)); - - auto db = barrier_first_derivative(d, 1.0); - auto d2b = barrier_second_derivative(d, 1.0); - - hess += db * hess_d + (d2b * grad_d) * grad_d.transpose(); - } - return hess; + 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 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); + } +} From aabca0048378f9d248308294a5fde1fd7d0bafd1 Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Mon, 31 Aug 2026 00:46:53 -0400 Subject: [PATCH 09/21] Unify the non-floating-point distance-type guard and the autogen instantiations Two cleanups in the templated distance API. `edge_edge` and `point_triangle` guarded their AUTO resolution with `is_same_v || is_same_v` and then let AUTO fall through to the switch, where the default case threw the generic "invalid distance type". `point_edge` instead used `is_floating_point_v` with an explicit `throw_auto_requires_explicit_dtype`. Adopt the latter everywhere, naming the function in the message, so a scalar type that cannot resolve a distance type now reports why rather than claiming its distance type is invalid: edge_edge_distance: an explicit distance type is required for non-floating-point scalars; ... This is a user-visible improvement for autodiff scalars, which reach these paths today. It also covers the gradient and Hessian kernels, whose AUTO resolution was previously unguarded, so they can now be instantiated for scalar types that have no ordering. The generated `autogen` instantiations in `line_line`, `point_line`, and `point_plane` spelled out every signature once per scalar type -- eight hand-written lines in `point_line`, whose four functions take 6, 9, 6 and 9 arguments and fill arrays of 6, 9, 36 and 81. Fold each file's list into an `IPC_INSTANTIATE_*_AUTOGEN(T)` macro, matching the convention already used for the distance kernels themselves. `nm` confirms the same sixteen `float` and `double` symbols are still emitted. Co-Authored-By: Claude Opus 5 --- src/ipc/distance/edge_edge.cpp | 21 +++++++++++++++------ src/ipc/distance/line_line.cpp | 15 +++++++++------ src/ipc/distance/point_line.cpp | 21 +++++++++++---------- src/ipc/distance/point_plane.cpp | 15 +++++++++------ src/ipc/distance/point_triangle.cpp | 22 +++++++++++++++------- 5 files changed, 59 insertions(+), 35 deletions(-) diff --git a/src/ipc/distance/edge_edge.cpp b/src/ipc/distance/edge_edge.cpp index 583c724d4..fd2df2aa9 100644 --- a/src/ipc/distance/edge_edge.cpp +++ b/src/ipc/distance/edge_edge.cpp @@ -16,10 +16,12 @@ T edge_edge_distance( Eigen::ConstRef> eb1, EdgeEdgeDistanceType dtype) { - if constexpr (std::is_same_v || std::is_same_v) { + 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) { @@ -67,8 +69,12 @@ Eigen::Vector edge_edge_distance_gradient( using Vector9T = Eigen::Vector; using Vector12T = Eigen::Vector; - 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_gradient"); } Vector12T grad = Vector12T::Zero(); @@ -147,8 +153,12 @@ Eigen::Matrix edge_edge_distance_hessian( using Matrix9T = Eigen::Matrix; using Matrix12T = Eigen::Matrix; - 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_hessian"); } Matrix12T hess = Matrix12T::Zero(); @@ -287,7 +297,6 @@ 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 diff --git a/src/ipc/distance/line_line.cpp b/src/ipc/distance/line_line.cpp index 636b4cb41..4af226bb4 100644 --- a/src/ipc/distance/line_line.cpp +++ b/src/ipc/distance/line_line.cpp @@ -558,11 +558,14 @@ void line_line_distance_hessian( H[143] = (t53 + t98 * t98 * t156 * T(2)) + b_t522_tmp * T(4); } -// clang-format off -template void line_line_distance_gradient(float, float, float, float, float, float, float, float, float, float, float, float, float[12]); -template void line_line_distance_gradient(double, double, double, double, double, double, double, double, double, double, double, double, double[12]); -template void line_line_distance_hessian(float, float, float, float, float, float, float, float, float, float, float, float, float[144]); -template void line_line_distance_hessian(double, double, double, double, double, double, double, double, double, double, double, double, double[144]); -// clang-format on +#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]) + +IPC_INSTANTIATE_LINE_LINE_AUTOGEN(float); +IPC_INSTANTIATE_LINE_LINE_AUTOGEN(double); +#undef IPC_INSTANTIATE_LINE_LINE_AUTOGEN } // namespace ipc::autogen diff --git a/src/ipc/distance/point_line.cpp b/src/ipc/distance/point_line.cpp index ac2ab24e5..2956b55a0 100644 --- a/src/ipc/distance/point_line.cpp +++ b/src/ipc/distance/point_line.cpp @@ -398,15 +398,16 @@ void point_line_distance_hessian_3D( H[80] = ((t234 * -T(2) + -t279) + t283) + t102 * (t38 + t39); } -// clang-format off -template void point_line_distance_gradient_2D(float, float, float, float, float, float, float[6]); -template void point_line_distance_gradient_2D(double, double, double, double, double, double, double[6]); -template void point_line_distance_gradient_3D(float, float, float, float, float, float, float, float, float, float[9]); -template void point_line_distance_gradient_3D(double, double, double, double, double, double, double, double, double, double[9]); -template void point_line_distance_hessian_2D(float, float, float, float, float, float, float[36]); -template void point_line_distance_hessian_2D(double, double, double, double, double, double, double[36]); -template void point_line_distance_hessian_3D(float, float, float, float, float, float, float, float, float, float[81]); -template void point_line_distance_hessian_3D(double, double, double, double, double, double, double, double, double, double[81]); -// clang-format on +#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]) + +IPC_INSTANTIATE_POINT_LINE_AUTOGEN(float); +IPC_INSTANTIATE_POINT_LINE_AUTOGEN(double); +#undef IPC_INSTANTIATE_POINT_LINE_AUTOGEN } // namespace ipc::autogen diff --git a/src/ipc/distance/point_plane.cpp b/src/ipc/distance/point_plane.cpp index 141c48dc7..fb250630c 100644 --- a/src/ipc/distance/point_plane.cpp +++ b/src/ipc/distance/point_plane.cpp @@ -558,11 +558,14 @@ void point_plane_distance_hessian( - t125 * t193 * t203 * t205 * T(4); } -// clang-format off -template void point_plane_distance_gradient(float, float, float, float, float, float, float, float, float, float, float, float, float[12]); -template void point_plane_distance_gradient(double, double, double, double, double, double, double, double, double, double, double, double, double[12]); -template void point_plane_distance_hessian(float, float, float, float, float, float, float, float, float, float, float, float, float[144]); -template void point_plane_distance_hessian(double, double, double, double, double, double, double, double, double, double, double, double, double[144]); -// clang-format on +#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]) + +IPC_INSTANTIATE_POINT_PLANE_AUTOGEN(float); +IPC_INSTANTIATE_POINT_PLANE_AUTOGEN(double); +#undef IPC_INSTANTIATE_POINT_PLANE_AUTOGEN } // namespace ipc::autogen diff --git a/src/ipc/distance/point_triangle.cpp b/src/ipc/distance/point_triangle.cpp index 74f1012b4..27d5863ba 100644 --- a/src/ipc/distance/point_triangle.cpp +++ b/src/ipc/distance/point_triangle.cpp @@ -6,8 +6,6 @@ #include #include -#include // std::invalid_argument - namespace ipc::detail { template @@ -18,10 +16,12 @@ T point_triangle_distance( Eigen::ConstRef> t2, PointTriangleDistanceType dtype) { - if constexpr (std::is_same_v || std::is_same_v) { + 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) { @@ -59,8 +59,12 @@ Eigen::Vector point_triangle_distance_gradient( 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"); } Eigen::Vector grad = Eigen::Vector::Zero(); @@ -126,8 +130,12 @@ Eigen::Matrix point_triangle_distance_hessian( 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"); } Eigen::Matrix hess = Eigen::Matrix::Zero(); From 65044f3418a3a436afc05f4144b43811f7fdfef6 Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Mon, 31 Aug 2026 09:10:11 -0400 Subject: [PATCH 10/21] Name every unsupported scalar family in the AUTO distance-type error The message claimed "the distance type cannot be determined from an autodiff scalar", which predates the other scalar types that reach this path. It is now also hit by SIMD batches and by filib::Interval, for which the old wording was simply wrong. Say what the actual obstacle is -- resolving AUTO means comparing single ordered values -- and name all three families. Co-Authored-By: Claude Opus 5 --- src/ipc/distance/distance_type.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/ipc/distance/distance_type.cpp b/src/ipc/distance/distance_type.cpp index 1c88baf5c..7f8c0fc82 100644 --- a/src/ipc/distance/distance_type.cpp +++ b/src/ipc/distance/distance_type.cpp @@ -30,8 +30,9 @@ namespace detail { throw std::invalid_argument( fmt::format( "{}: an explicit distance type is required for non-floating-point " - "scalars; the distance type cannot be determined from an " - "autodiff scalar", + "scalars; resolving AUTO means comparing single ordered values, " + "which an autodiff, SIMD batch, or interval scalar does not " + "provide", function)); } From 899b4239239c414974665c43981907cc86a663a2 Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Tue, 1 Sep 2026 10:38:57 -0400 Subject: [PATCH 11/21] Extend the two-layer template design across the remaining geometry kernels MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Convert every remaining per-collision kernel family to the shape the distance functions established: fixed-size inner kernels in ipc::detail, deducing expression-templated front ends in ipc, and float alongside double. Families that already had scalar-templated kernels but no front end (so a matrix-row argument forced a Ref materialization per call, and an explicit scalar produced an error wall inside Eigen): edge_edge_mollifier, point_plane, and the three signed distances (line_line, point_line -- a 2D family -- and point_plane). Families that were double-only with dimension-erased VectorMax/MatrixMax signatures: closest_point, tangent_basis, relative_velocity, and geometry/area. Their autogen kernels are templated with the same instantiation-macro pattern as line_line; the only edits to generated expressions are T() literal wrappers, verified byte-identical to the previous text modulo the scalar substitution. relative_velocity's runtime-dim entry points keep their exact public signatures (template over dim-templated kernels), so call sites passing int dim compile unchanged. No caller anywhere needed editing; the deducing front ends accept every argument type used in-tree. relative_velocity ends up fully header-inline, so its TU is deleted. Small hand-written kernels are header-inline; wide autogen bodies stay in their TUs. That split is load-bearing, and the new "Converted families" benchmark (tests/src/tests/benchmark_eigen.cpp) measured it both ways: with the tangent kernels TU-defined, the dimension-erased path paid the Ref copies and MatrixMax wrap at an opaque call boundary and point_edge_tangent_basis REGRESSED 2x on matrix rows; header-inlining the value kernels turned that into the wins below. Measured old-vs-new by interleaved A/B of two binaries (baseline built from a worktree at the previous commit; controls 0.99-1.00x): point_edge_closest_point, rows 8.7 -> 1.8 ns 4.8x point_edge_closest_point, Vector3d 4.7 -> 1.7 ns 2.8x point_edge_closest_point_jacobian, rows 14.9 -> 6.2 ns 2.4x point_point_relative_velocity, rows 5.5 -> 1.4 ns 3.9x pp_relative_velocity_jacobian(dim) 6.8 -> 1.8 ns 3.9x edge_length, rows 4.1 -> 1.3 ns 3.1x point_edge_tangent_basis, rows 7.1 -> 4.2 ns 1.7x point_triangle_tangent_basis, rows 7.7 -> 5.0 ns 1.5x The already-fixed-size 3D functions (edge_edge/point_triangle closest point, triangle_area, edge_edge_cross_squarednorm, point_plane_distance) measured neutral, as expected: the win was always recovering the compile-time dimension, not the templates themselves. One residual: point_point_tangent basis on matrix rows reads 0.87-0.90x (its fixed-dim path improved 1.11x); suspected branch/layout effects in its axis-picking body, unresolved. Also: move the relative-velocity Γ layout derivations from stranded TU comments into the doxygen of their public front ends, exempt NOTE/TODO/ WARNING/FIXME comments from clang-format reflow, and swap a leftover fmt include for spdlog in the CCD benchmark test. Co-Authored-By: Claude Fable 5 --- .clang-format | 2 +- src/ipc/distance/distance_type.hpp | 5 - src/ipc/distance/edge_edge_mollifier.cpp | 139 +- src/ipc/distance/edge_edge_mollifier.hpp | 655 +- src/ipc/distance/point_line.hpp | 7 - src/ipc/distance/point_plane.hpp | 300 +- src/ipc/distance/point_point.hpp | 15 - src/ipc/distance/signed/line_line.cpp | 4 +- src/ipc/distance/signed/line_line.hpp | 226 +- src/ipc/distance/signed/point_line.cpp | 4 +- src/ipc/distance/signed/point_line.hpp | 169 +- src/ipc/distance/signed/point_plane.cpp | 4 +- src/ipc/distance/signed/point_plane.hpp | 185 +- src/ipc/geometry/area.cpp | 113 +- src/ipc/geometry/area.hpp | 173 +- src/ipc/tangent/CMakeLists.txt | 1 - src/ipc/tangent/closest_point.cpp | 8979 ++++++++++---------- src/ipc/tangent/closest_point.hpp | 709 +- src/ipc/tangent/relative_velocity.cpp | 168 - src/ipc/tangent/relative_velocity.hpp | 468 +- src/ipc/tangent/tangent_basis.cpp | 1832 ++-- src/ipc/tangent/tangent_basis.hpp | 467 +- tests/src/tests/benchmark_eigen.cpp | 149 + tests/src/tests/ccd/test_ccd_benchmark.cpp | 2 +- 24 files changed, 7790 insertions(+), 6986 deletions(-) delete mode 100644 src/ipc/tangent/relative_velocity.cpp 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/src/ipc/distance/distance_type.hpp b/src/ipc/distance/distance_type.hpp index f3046d782..67a54428d 100644 --- a/src/ipc/distance/distance_type.hpp +++ b/src/ipc/distance/distance_type.hpp @@ -154,11 +154,6 @@ EdgeEdgeDistanceType edge_edge_parallel_distance_type( // failure. /// @brief Determine the closest pair between a point and edge. -/// -/// Accepts any Eigen expression. The dimension is resolved at compile time when -/// the argument type knows it and with a single branch otherwise; each argument -/// is evaluated exactly once. -/// /// @param p The point. /// @param e0 The first vertex of the edge. /// @param e1 The second vertex of the edge. diff --git a/src/ipc/distance/edge_edge_mollifier.cpp b/src/ipc/distance/edge_edge_mollifier.cpp index 1e3cfe82b..88c632ca8 100644 --- a/src/ipc/distance/edge_edge_mollifier.cpp +++ b/src/ipc/distance/edge_edge_mollifier.cpp @@ -3,69 +3,92 @@ #include namespace ipc { +namespace detail { -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(); + 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(); + } } -} -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() - + edge_edge_mollifier_gradient(ee_cross_norm_sqr, eps_x) - * edge_edge_cross_squarednorm_hessian(ea0, ea1, eb0, eb1); - } else { - return Eigen::Matrix::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(); + } } -} -// clang-format off -template Vector12f edge_edge_mollifier_gradient_wrt_x(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef); -template Vector12d edge_edge_mollifier_gradient_wrt_x(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef); -template Matrix12f edge_edge_mollifier_gradient_jacobian_wrt_x(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef); -template Matrix12d edge_edge_mollifier_gradient_jacobian_wrt_x(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef); -// clang-format on +#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>) + + IPC_INSTANTIATE_EDGE_EDGE_MOLLIFIER(float); + IPC_INSTANTIATE_EDGE_EDGE_MOLLIFIER(double); +#undef IPC_INSTANTIATE_EDGE_EDGE_MOLLIFIER + +} // namespace detail namespace autogen { // This function was generated by the Symbolic Math Toolbox version 8.3. diff --git a/src/ipc/distance/edge_edge_mollifier.hpp b/src/ipc/distance/edge_edge_mollifier.hpp index 874e0f6ef..496293798 100644 --- a/src/ipc/distance/edge_edge_mollifier.hpp +++ b/src/ipc/distance/edge_edge_mollifier.hpp @@ -19,63 +19,7 @@ namespace autogen { // clang-format on } // namespace autogen -/// @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 -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(); -} - -/// @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 -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; -} - -/// @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 -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; -} +// --- Scalar mollifier functions ------------------------------------------ /// @brief Mollifier function for edge-edge distance. /// @param x Squared norm of the edge-edge cross product. @@ -147,299 +91,326 @@ edge_edge_mollifier_gradient_derivative_wrt_eps_x(const T x, const T eps_x) : T(0); } -/// @brief Compute a mollifier for the edge-edge distance. -/// -/// This helps smooth the non-smoothness at close to 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. -/// @param eps_x Mollifier activation threshold. -/// @return The mollifier coefficient to premultiply the edge-edge distance. -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) { - return edge_edge_mollifier(ee_cross_norm_sqr, eps_x); - } else { - return T(1); +// --- 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(); } -} -/// @brief Compute the gradient of the mollifier for the edge-edge 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 eps_x Mollifier activation threshold. -/// @return The gradient of the mollifier. -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 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_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; } -} -/// @brief Compute the hessian of the mollifier for the edge-edge 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 eps_x Mollifier activation threshold. -/// @return The hessian of the mollifier. -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 (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 Eigen::Matrix::Zero(); + /// @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; } -} -/// @brief Compute the gradient of the mollifier for the edge-edge distance wrt -/// rest positions. -/// @param ea0_rest The rest position of the first vertex of the first edge. -/// @param ea1_rest The rest position of the second vertex of the first edge. -/// @param eb0_rest The rest position of the first vertex of the second edge. -/// @param eb1_rest The rest position of the second vertex of the second edge. -/// @param ea0 The first vertex of the first edge. -/// @param ea1 The second vertex of the first edge. -/// @param eb0 The first vertex of the second edge. -/// @param eb1 The second vertex of the second edge. -/// @return The derivative of the mollifier wrt rest positions. -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 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); + } + } -/// @brief Compute the jacobian of the edge-edge distance mollifier's gradient -/// wrt rest positions. -/// @note This is not the hessian of the mollifier wrt rest positions, but the -/// jacobian wrt rest positions of the mollifier's gradient wrt positions. -/// @param ea0_rest The rest position of the first vertex of the first edge. -/// @param ea1_rest The rest position of the second vertex of the first edge. -/// @param eb0_rest The rest position of the first vertex of the second edge. -/// @param eb1_rest The rest position of the second vertex of the second edge. -/// @param ea0 The first vertex of the first edge. -/// @param ea1 The second vertex of the first edge. -/// @param eb0 The first vertex of the second edge. -/// @param eb1 The second vertex of the second edge. -/// @return The jacobian of the mollifier's gradient wrt rest positions. -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_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(); + } + } -/// @brief Compute the threshold of the mollifier edge-edge distance. -/// -/// This values is computed based on the edges at rest length. -/// -/// @param ea0_rest The rest position of the first vertex of the first edge. -/// @param ea1_rest The rest position of the second vertex of the first edge. -/// @param eb0_rest The rest position of the first vertex of the second edge. -/// @param eb1_rest The rest position of the second vertex of the second edge. -/// @return Threshold for edge-edge mollification. -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_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); -/// @brief Compute the gradient of the threshold of the mollifier edge-edge -/// distance. -/// -/// This values is computed based on the edges at rest length. -/// -/// @param ea0_rest The rest position of the first vertex of the first edge. -/// @param ea1_rest The rest position of the second vertex of the first edge. -/// @param eb0_rest The rest position of the first vertex of the second edge. -/// @param eb1_rest The rest position of the second vertex of the second edge. -/// @return Gradient of the threshold for edge-edge mollification. -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; -} + 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(); + } + } -// --- EigenExpression wrappers --- + /// @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) -> typename DerivedEA0::Scalar + const DerivedEA0& ea0, + const DerivedEA1& ea1, + const DerivedEB0& eb0, + const DerivedEB1& eb1) { + IPC_ASSERT_EIGEN_ARGS(DerivedEA0, DerivedEA1, DerivedEB0, DerivedEB1); using T = typename DerivedEA0::Scalar; - return edge_edge_cross_squarednorm( - Eigen::Ref>(ea0), - Eigen::Ref>(ea1), - Eigen::Ref>(eb0), - Eigen::Ref>(eb1)); + // 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) - -> Eigen::Vector + const DerivedEA0& ea0, + const DerivedEA1& ea1, + const DerivedEB0& eb0, + const DerivedEB1& eb1) { + IPC_ASSERT_EIGEN_ARGS(DerivedEA0, DerivedEA1, DerivedEB0, DerivedEB1); using T = typename DerivedEA0::Scalar; - return edge_edge_cross_squarednorm_gradient( - Eigen::Ref>(ea0), - Eigen::Ref>(ea1), - Eigen::Ref>(eb0), - Eigen::Ref>(eb1)); + 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) - -> Eigen::Matrix + const DerivedEA0& ea0, + const DerivedEA1& ea1, + const DerivedEB0& eb0, + const DerivedEB1& eb1) { + IPC_ASSERT_EIGEN_ARGS(DerivedEA0, DerivedEA1, DerivedEB0, DerivedEB1); using T = typename DerivedEA0::Scalar; - return edge_edge_cross_squarednorm_hessian( - Eigen::Ref>(ea0), - Eigen::Ref>(ea1), - Eigen::Ref>(eb0), - Eigen::Ref>(eb1)); + return detail::edge_edge_cross_squarednorm_hessian(ea0, ea1, eb0, eb1); } +/// @brief Compute a mollifier for the edge-edge distance. +/// +/// This helps smooth the non-smoothness at close to 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. +/// @param eps_x Mollifier activation threshold. +/// @return The mollifier coefficient to premultiply the edge-edge distance. 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) -> typename DerivedEA0::Scalar + const DerivedEA0& ea0, + const DerivedEA1& ea1, + const DerivedEB0& eb0, + const DerivedEB1& eb1, + const typename DerivedEA0::Scalar eps_x) { + IPC_ASSERT_EIGEN_ARGS(DerivedEA0, DerivedEA1, DerivedEB0, DerivedEB1); using T = typename DerivedEA0::Scalar; - return edge_edge_mollifier( - Eigen::Ref>(ea0), - Eigen::Ref>(ea1), - Eigen::Ref>(eb0), - Eigen::Ref>(eb1), eps_x); + 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. +/// @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 eps_x Mollifier activation threshold. +/// @return The gradient of the mollifier. 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 DerivedEA0& ea0, + const DerivedEA1& ea1, + const DerivedEB0& eb0, + const DerivedEB1& eb1, const typename DerivedEA0::Scalar eps_x) - -> Eigen::Vector { + IPC_ASSERT_EIGEN_ARGS(DerivedEA0, DerivedEA1, DerivedEB0, DerivedEB1); using T = typename DerivedEA0::Scalar; - return edge_edge_mollifier_gradient( - Eigen::Ref>(ea0), - Eigen::Ref>(ea1), - Eigen::Ref>(eb0), - Eigen::Ref>(eb1), eps_x); + 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. +/// @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 eps_x Mollifier activation threshold. +/// @return The hessian of the mollifier. 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 DerivedEA0& ea0, + const DerivedEA1& ea1, + const DerivedEB0& eb0, + const DerivedEB1& eb1, const typename DerivedEA0::Scalar eps_x) - -> Eigen::Matrix { + IPC_ASSERT_EIGEN_ARGS(DerivedEA0, DerivedEA1, DerivedEB0, DerivedEB1); using T = typename DerivedEA0::Scalar; - return edge_edge_mollifier_hessian( - Eigen::Ref>(ea0), - Eigen::Ref>(ea1), - Eigen::Ref>(eb0), - Eigen::Ref>(eb1), eps_x); + 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. +/// @param eb1_rest The rest position of the second vertex of the second edge. +/// @param ea0 The first vertex of the first edge. +/// @param ea1 The second vertex of the first edge. +/// @param eb0 The first vertex of the second edge. +/// @param eb1 The second vertex of the second edge. +/// @return The derivative of the mollifier wrt rest positions. template < typename DerivedEA0Rest, typename DerivedEA1Rest, @@ -450,28 +421,36 @@ template < 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) - -> Eigen::Vector + const DerivedEA0Rest& ea0_rest, + const DerivedEA1Rest& ea1_rest, + const DerivedEB0Rest& eb0_rest, + const DerivedEB1Rest& eb1_rest, + const DerivedEA0& ea0, + const DerivedEA1& ea1, + const DerivedEB0& eb0, + const DerivedEB1& eb1) { + IPC_ASSERT_EIGEN_ARGS( + DerivedEA0Rest, DerivedEA1Rest, DerivedEB0Rest, DerivedEB1Rest, + DerivedEA0, DerivedEA1, DerivedEB0, DerivedEB1); using T = typename DerivedEA0Rest::Scalar; - return edge_edge_mollifier_gradient_wrt_x( - Eigen::Ref>(ea0_rest), - Eigen::Ref>(ea1_rest), - Eigen::Ref>(eb0_rest), - Eigen::Ref>(eb1_rest), - Eigen::Ref>(ea0), - Eigen::Ref>(ea1), - Eigen::Ref>(eb0), - Eigen::Ref>(eb1)); + 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. +/// @param eb1_rest The rest position of the second vertex of the second edge. +/// @param ea0 The first vertex of the first edge. +/// @param ea1 The second vertex of the first edge. +/// @param eb0 The first vertex of the second edge. +/// @param eb1 The second vertex of the second edge. +/// @return The jacobian of the mollifier's gradient wrt rest positions. template < typename DerivedEA0Rest, typename DerivedEA1Rest, @@ -482,66 +461,76 @@ template < 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) - -> Eigen::Matrix + const DerivedEA0Rest& ea0_rest, + const DerivedEA1Rest& ea1_rest, + const DerivedEB0Rest& eb0_rest, + const DerivedEB1Rest& eb1_rest, + const DerivedEA0& ea0, + const DerivedEA1& ea1, + const DerivedEB0& eb0, + const DerivedEB1& eb1) { + IPC_ASSERT_EIGEN_ARGS( + DerivedEA0Rest, DerivedEA1Rest, DerivedEB0Rest, DerivedEB1Rest, + DerivedEA0, DerivedEA1, DerivedEB0, DerivedEB1); using T = typename DerivedEA0Rest::Scalar; - return edge_edge_mollifier_gradient_jacobian_wrt_x( - Eigen::Ref>(ea0_rest), - Eigen::Ref>(ea1_rest), - Eigen::Ref>(eb0_rest), - Eigen::Ref>(eb1_rest), - Eigen::Ref>(ea0), - Eigen::Ref>(ea1), - Eigen::Ref>(eb0), - Eigen::Ref>(eb1)); + 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. +/// +/// This values is computed based on the edges at rest length. +/// +/// @param ea0_rest The rest position of the first vertex of the first edge. +/// @param ea1_rest The rest position of the second vertex of the first edge. +/// @param eb0_rest The rest position of the first vertex of the second edge. +/// @param eb1_rest The rest position of the second vertex of the second edge. +/// @return Threshold for edge-edge mollification. 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) -> - typename DerivedEA0Rest::Scalar + const DerivedEA0Rest& ea0_rest, + const DerivedEA1Rest& ea1_rest, + const DerivedEB0Rest& eb0_rest, + const DerivedEB1Rest& eb1_rest) { + IPC_ASSERT_EIGEN_ARGS( + DerivedEA0Rest, DerivedEA1Rest, DerivedEB0Rest, DerivedEB1Rest); using T = typename DerivedEA0Rest::Scalar; - return edge_edge_mollifier_threshold( - Eigen::Ref>(ea0_rest), - Eigen::Ref>(ea1_rest), - Eigen::Ref>(eb0_rest), - Eigen::Ref>(eb1_rest)); + 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. +/// +/// This values is computed based on the edges at rest length. +/// +/// @param ea0_rest The rest position of the first vertex of the first edge. +/// @param ea1_rest The rest position of the second vertex of the first edge. +/// @param eb0_rest The rest position of the first vertex of the second edge. +/// @param eb1_rest The rest position of the second vertex of the second edge. +/// @return Gradient of the threshold for edge-edge mollification. 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) - -> Eigen::Vector + const DerivedEA0Rest& ea0_rest, + const DerivedEA1Rest& ea1_rest, + const DerivedEB0Rest& eb0_rest, + const DerivedEB1Rest& eb1_rest) { + IPC_ASSERT_EIGEN_ARGS( + DerivedEA0Rest, DerivedEA1Rest, DerivedEB0Rest, DerivedEB1Rest); using T = typename DerivedEA0Rest::Scalar; - return edge_edge_mollifier_threshold_gradient( - Eigen::Ref>(ea0_rest), - Eigen::Ref>(ea1_rest), - Eigen::Ref>(eb0_rest), - Eigen::Ref>(eb1_rest)); + return detail::edge_edge_mollifier_threshold_gradient( + ea0_rest, ea1_rest, eb0_rest, eb1_rest); } } // namespace ipc diff --git a/src/ipc/distance/point_line.hpp b/src/ipc/distance/point_line.hpp index a0a11f24a..41a3680db 100644 --- a/src/ipc/distance/point_line.hpp +++ b/src/ipc/distance/point_line.hpp @@ -163,13 +163,6 @@ inline auto point_line_distance_gradient( /// @brief Compute the hessian of the distance between a point and line. /// /// @note The distance is actually squared distance. -/// -/// Accepts any Eigen expression. When the dimension is known at compile time -/// the return type is the fixed-size Eigen::Matrix; -/// otherwise it is MatrixMax9, filled directly by the generated code -/// (wrapping the 648-byte fixed-size result instead measured as a 7-13% -/// regression). -/// /// @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. diff --git a/src/ipc/distance/point_plane.hpp b/src/ipc/distance/point_plane.hpp index e5a221035..e7b58764d 100644 --- a/src/ipc/distance/point_plane.hpp +++ b/src/ipc/distance/point_plane.hpp @@ -17,127 +17,126 @@ namespace autogen { // clang-format on } // namespace autogen -/// @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; -} - -// --- EigenExpression wrappers --- +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 template inline auto point_plane_distance( - const Eigen::MatrixBase& p, - const Eigen::MatrixBase& origin, - const Eigen::MatrixBase& normal) -> typename DerivedP::Scalar + const DerivedP& p, const DerivedOrigin& origin, const DerivedNormal& normal) { + IPC_ASSERT_EIGEN_ARGS(DerivedP, DerivedOrigin, DerivedNormal); using T = typename DerivedP::Scalar; - return point_plane_distance( - Eigen::Ref>(p), - Eigen::Ref>(origin), - Eigen::Ref>(normal)); + // NOTE: explicit : the detail overload cannot deduce T from an + // arbitrary Eigen expression. + return detail::point_plane_distance(p, origin, normal); } template < @@ -146,31 +145,23 @@ template < 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) -> typename DerivedP::Scalar + const DerivedP& p, + const DerivedT0& t0, + const DerivedT1& t1, + const DerivedT2& t2) { + IPC_ASSERT_EIGEN_ARGS(DerivedP, DerivedT0, DerivedT1, DerivedT2); using T = typename DerivedP::Scalar; - return point_plane_distance( - Eigen::Ref>(p), - Eigen::Ref>(t0), - Eigen::Ref>(t1), - Eigen::Ref>(t2)); + return detail::point_plane_distance(p, t0, t1, t2); } template inline auto point_plane_distance_gradient( - const Eigen::MatrixBase& p, - const Eigen::MatrixBase& origin, - const Eigen::MatrixBase& normal) - -> Eigen::Vector3 + const DerivedP& p, const DerivedOrigin& origin, const DerivedNormal& normal) { + IPC_ASSERT_EIGEN_ARGS(DerivedP, DerivedOrigin, DerivedNormal); using T = typename DerivedP::Scalar; - return point_plane_distance_gradient( - Eigen::Ref>(p), - Eigen::Ref>(origin), - Eigen::Ref>(normal)); + return detail::point_plane_distance_gradient(p, origin, normal); } template < @@ -179,32 +170,23 @@ template < 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) - -> Eigen::Vector + const DerivedP& p, + const DerivedT0& t0, + const DerivedT1& t1, + const DerivedT2& t2) { + IPC_ASSERT_EIGEN_ARGS(DerivedP, DerivedT0, DerivedT1, DerivedT2); using T = typename DerivedP::Scalar; - return point_plane_distance_gradient( - Eigen::Ref>(p), - Eigen::Ref>(t0), - Eigen::Ref>(t1), - Eigen::Ref>(t2)); + return detail::point_plane_distance_gradient(p, t0, t1, t2); } template inline auto point_plane_distance_hessian( - const Eigen::MatrixBase& p, - const Eigen::MatrixBase& origin, - const Eigen::MatrixBase& normal) - -> Eigen::Matrix + const DerivedP& p, const DerivedOrigin& origin, const DerivedNormal& normal) { + IPC_ASSERT_EIGEN_ARGS(DerivedP, DerivedOrigin, DerivedNormal); using T = typename DerivedP::Scalar; - return point_plane_distance_hessian( - Eigen::Ref>(p), - Eigen::Ref>(origin), - Eigen::Ref>(normal)); + return detail::point_plane_distance_hessian(p, origin, normal); } template < @@ -213,18 +195,14 @@ template < 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) - -> Eigen::Matrix + const DerivedP& p, + const DerivedT0& t0, + const DerivedT1& t1, + const DerivedT2& t2) { + IPC_ASSERT_EIGEN_ARGS(DerivedP, DerivedT0, DerivedT1, DerivedT2); using T = typename DerivedP::Scalar; - return point_plane_distance_hessian( - Eigen::Ref>(p), - Eigen::Ref>(t0), - Eigen::Ref>(t1), - Eigen::Ref>(t2)); + return detail::point_plane_distance_hessian(p, t0, t1, t2); } } // namespace ipc diff --git a/src/ipc/distance/point_point.hpp b/src/ipc/distance/point_point.hpp index d10f819ea..2f39affaf 100644 --- a/src/ipc/distance/point_point.hpp +++ b/src/ipc/distance/point_point.hpp @@ -69,11 +69,6 @@ namespace detail { /// @brief Compute the distance between two points. /// /// @note The distance is actually squared distance. -/// -/// Accepts any Eigen expression. The dimension is resolved at compile time when -/// the argument type knows it and with a single branch otherwise; each argument -/// is evaluated exactly once. -/// /// @param p0 The first point. /// @param p1 The second point. /// @return The distance between p0 and p1. @@ -98,11 +93,6 @@ inline auto point_point_distance(const DerivedP0& p0, const DerivedP1& p1) /// @brief Compute the gradient of the distance between two points. /// /// @note The distance is actually squared distance. -/// -/// Accepts any Eigen expression. When the dimension is known at compile time -/// the return type is the fixed-size Eigen::Vector; otherwise it is -/// VectorMax6. -/// /// @param p0 The first point. /// @param p1 The second point. /// @return The computed gradient. @@ -129,11 +119,6 @@ point_point_distance_gradient(const DerivedP0& p0, const DerivedP1& p1) /// @brief Compute the hessian of the distance between two points. /// /// @note The distance is actually squared distance. -/// -/// Accepts any Eigen expression. When the dimension is known at compile time -/// the return type is the fixed-size Eigen::Matrix; -/// otherwise it is MatrixMax6. -/// /// @param p0 The first point. /// @param p1 The second point. /// @return The computed hessian. diff --git a/src/ipc/distance/signed/line_line.cpp b/src/ipc/distance/signed/line_line.cpp index 4b41ff15c..b010cad45 100644 --- a/src/ipc/distance/signed/line_line.cpp +++ b/src/ipc/distance/signed/line_line.cpp @@ -1,6 +1,6 @@ #include "line_line.hpp" -namespace ipc { +namespace ipc::detail { template Eigen::Matrix line_line_signed_distance_hessian( @@ -75,4 +75,4 @@ template Matrix12f line_line_signed_distance_hessian(Eigen::ConstRef(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef); // clang-format on -} // namespace ipc +} // namespace ipc::detail diff --git a/src/ipc/distance/signed/line_line.hpp b/src/ipc/distance/signed/line_line.hpp index 096d6ddb0..ad2aad538 100644 --- a/src/ipc/distance/signed/line_line.hpp +++ b/src/ipc/distance/signed/line_line.hpp @@ -5,96 +5,97 @@ namespace ipc { -/// 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; -} +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 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); + /// 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; + } -// --- EigenExpression wrappers --- + /// 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 template < typename DerivedEA0, @@ -102,17 +103,16 @@ template < 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) -> typename DerivedEA0::Scalar + const DerivedEA0& ea0, + const DerivedEA1& ea1, + const DerivedEB0& eb0, + const DerivedEB1& eb1) { + IPC_ASSERT_EIGEN_ARGS(DerivedEA0, DerivedEA1, DerivedEB0, DerivedEB1); using T = typename DerivedEA0::Scalar; - return line_line_signed_distance( - Eigen::Ref>(ea0), - Eigen::Ref>(ea1), - Eigen::Ref>(eb0), - Eigen::Ref>(eb1)); + // NOTE: explicit : the detail overload cannot deduce T from an + // arbitrary Eigen expression. + return detail::line_line_signed_distance(ea0, ea1, eb0, eb1); } template < @@ -121,18 +121,14 @@ template < 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) - -> Eigen::Vector + const DerivedEA0& ea0, + const DerivedEA1& ea1, + const DerivedEB0& eb0, + const DerivedEB1& eb1) { + IPC_ASSERT_EIGEN_ARGS(DerivedEA0, DerivedEA1, DerivedEB0, DerivedEB1); using T = typename DerivedEA0::Scalar; - return line_line_signed_distance_gradient( - Eigen::Ref>(ea0), - Eigen::Ref>(ea1), - Eigen::Ref>(eb0), - Eigen::Ref>(eb1)); + return detail::line_line_signed_distance_gradient(ea0, ea1, eb0, eb1); } template < @@ -141,18 +137,14 @@ template < 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) - -> Eigen::Matrix + const DerivedEA0& ea0, + const DerivedEA1& ea1, + const DerivedEB0& eb0, + const DerivedEB1& eb1) { + IPC_ASSERT_EIGEN_ARGS(DerivedEA0, DerivedEA1, DerivedEB0, DerivedEB1); using T = typename DerivedEA0::Scalar; - return line_line_signed_distance_hessian( - Eigen::Ref>(ea0), - Eigen::Ref>(ea1), - Eigen::Ref>(eb0), - Eigen::Ref>(eb1)); + return detail::line_line_signed_distance_hessian(ea0, ea1, eb0, eb1); } } // namespace ipc diff --git a/src/ipc/distance/signed/point_line.cpp b/src/ipc/distance/signed/point_line.cpp index a09d055b0..0b870e97e 100644 --- a/src/ipc/distance/signed/point_line.cpp +++ b/src/ipc/distance/signed/point_line.cpp @@ -1,6 +1,6 @@ #include "point_line.hpp" -namespace ipc { +namespace ipc::detail { template Eigen::Matrix point_line_signed_distance_hessian( @@ -72,4 +72,4 @@ template Eigen::Matrix point_line_signed_distance_hessian(Ei template Eigen::Matrix point_line_signed_distance_hessian(Eigen::ConstRef>, Eigen::ConstRef>, Eigen::ConstRef>); // clang-format on -} // namespace ipc +} // namespace ipc::detail diff --git a/src/ipc/distance/signed/point_line.hpp b/src/ipc/distance/signed/point_line.hpp index 8b5d6ac1e..a13c1d419 100644 --- a/src/ipc/distance/signed/point_line.hpp +++ b/src/ipc/distance/signed/point_line.hpp @@ -5,115 +5,104 @@ namespace ipc { -/// 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; -} +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 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); + /// 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; + } -// --- EigenExpression wrappers --- + /// 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 template inline auto point_line_signed_distance( - const Eigen::MatrixBase& p, - const Eigen::MatrixBase& e0, - const Eigen::MatrixBase& e1) -> typename DerivedP::Scalar + const DerivedP& p, const DerivedE0& e0, const DerivedE1& e1) { + IPC_ASSERT_EIGEN_ARGS(DerivedP, DerivedE0, DerivedE1); using T = typename DerivedP::Scalar; - return point_line_signed_distance( - Eigen::Ref>(p), - Eigen::Ref>(e0), - Eigen::Ref>(e1)); + // NOTE: explicit : the detail overload cannot deduce T from an + // arbitrary Eigen expression. + return detail::point_line_signed_distance(p, e0, e1); } template inline auto point_line_signed_distance_gradient( - const Eigen::MatrixBase& p, - const Eigen::MatrixBase& e0, - const Eigen::MatrixBase& e1) - -> Eigen::Vector + const DerivedP& p, const DerivedE0& e0, const DerivedE1& e1) { + IPC_ASSERT_EIGEN_ARGS(DerivedP, DerivedE0, DerivedE1); using T = typename DerivedP::Scalar; - return point_line_signed_distance_gradient( - Eigen::Ref>(p), - Eigen::Ref>(e0), - Eigen::Ref>(e1)); + return detail::point_line_signed_distance_gradient(p, e0, e1); } template inline auto point_line_signed_distance_hessian( - const Eigen::MatrixBase& p, - const Eigen::MatrixBase& e0, - const Eigen::MatrixBase& e1) - -> Eigen::Matrix + const DerivedP& p, const DerivedE0& e0, const DerivedE1& e1) { + IPC_ASSERT_EIGEN_ARGS(DerivedP, DerivedE0, DerivedE1); using T = typename DerivedP::Scalar; - return point_line_signed_distance_hessian( - Eigen::Ref>(p), - Eigen::Ref>(e0), - Eigen::Ref>(e1)); + return detail::point_line_signed_distance_hessian(p, e0, e1); } } // namespace ipc diff --git a/src/ipc/distance/signed/point_plane.cpp b/src/ipc/distance/signed/point_plane.cpp index 7b47bad9a..268b5355a 100644 --- a/src/ipc/distance/signed/point_plane.cpp +++ b/src/ipc/distance/signed/point_plane.cpp @@ -1,6 +1,6 @@ #include "point_plane.hpp" -namespace ipc { +namespace ipc::detail { template Eigen::Matrix point_plane_signed_distance_hessian( @@ -74,4 +74,4 @@ template Matrix12f point_plane_signed_distance_hessian(Eigen::ConstRef(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef); // clang-format on -} // namespace ipc +} // namespace ipc::detail diff --git a/src/ipc/distance/signed/point_plane.hpp b/src/ipc/distance/signed/point_plane.hpp index b58ffb502..f505426e2 100644 --- a/src/ipc/distance/signed/point_plane.hpp +++ b/src/ipc/distance/signed/point_plane.hpp @@ -5,75 +5,77 @@ namespace ipc { -/// 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; -} +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 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); + /// 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; + } -// --- EigenExpression wrappers --- + /// 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 template < typename DerivedP, @@ -81,17 +83,16 @@ template < 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) -> typename DerivedP::Scalar + const DerivedP& p, + const DerivedT0& t0, + const DerivedT1& t1, + const DerivedT2& t2) { + IPC_ASSERT_EIGEN_ARGS(DerivedP, DerivedT0, DerivedT1, DerivedT2); using T = typename DerivedP::Scalar; - return point_plane_signed_distance( - Eigen::Ref>(p), - Eigen::Ref>(t0), - Eigen::Ref>(t1), - Eigen::Ref>(t2)); + // NOTE: explicit : the detail overload cannot deduce T from an + // arbitrary Eigen expression. + return detail::point_plane_signed_distance(p, t0, t1, t2); } template < @@ -100,18 +101,14 @@ template < 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) - -> Eigen::Vector + const DerivedP& p, + const DerivedT0& t0, + const DerivedT1& t1, + const DerivedT2& t2) { + IPC_ASSERT_EIGEN_ARGS(DerivedP, DerivedT0, DerivedT1, DerivedT2); using T = typename DerivedP::Scalar; - return point_plane_signed_distance_gradient( - Eigen::Ref>(p), - Eigen::Ref>(t0), - Eigen::Ref>(t1), - Eigen::Ref>(t2)); + return detail::point_plane_signed_distance_gradient(p, t0, t1, t2); } template < @@ -120,18 +117,14 @@ template < 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) - -> Eigen::Matrix + const DerivedP& p, + const DerivedT0& t0, + const DerivedT1& t1, + const DerivedT2& t2) { + IPC_ASSERT_EIGEN_ARGS(DerivedP, DerivedT0, DerivedT1, DerivedT2); using T = typename DerivedP::Scalar; - return point_plane_signed_distance_hessian( - Eigen::Ref>(p), - Eigen::Ref>(t0), - Eigen::Ref>(t1), - Eigen::Ref>(t2)); + 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..686a5dc3a 100644 --- a/src/ipc/geometry/area.hpp +++ b/src/ipc/geometry/area.hpp @@ -2,63 +2,174 @@ #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 DerivedE0& e0, const DerivedE1& e1) { - return (e1 - e0).norm(); + IPC_ASSERT_EIGEN_ARGS(DerivedE0, DerivedE1); + 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 DerivedE0& e0, const DerivedE1& e1) +{ + IPC_ASSERT_EIGEN_ARGS(DerivedE0, DerivedE1); + 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 DerivedT0& t0, const DerivedT1& t1, const DerivedT2& t2) { - return 0.5 * (t1 - t0).cross(t2 - t0).norm(); + IPC_ASSERT_EIGEN_ARGS(DerivedT0, DerivedT1, DerivedT2); + // 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 DerivedT0& t0, const DerivedT1& t1, const DerivedT2& t2) +{ + IPC_ASSERT_EIGEN_ARGS(DerivedT0, DerivedT1, DerivedT2); + using T = typename DerivedT0::Scalar; + return detail::triangle_area_gradient(t0, t1, t2); +} } // namespace ipc 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..f9a14ed10 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,236 @@ 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 DerivedP& p, const DerivedE0& e0, const DerivedE1& e1) +{ + IPC_ASSERT_EIGEN_ARGS(DerivedP, DerivedE0, DerivedE1); + 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 DerivedP& p, const DerivedE0& e0, const DerivedE1& e1) +{ + IPC_ASSERT_EIGEN_ARGS(DerivedP, DerivedE0, DerivedE1); + 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 DerivedP& p, const DerivedE0& e0, const DerivedE1& e1) +{ + IPC_ASSERT_EIGEN_ARGS(DerivedP, DerivedE0, DerivedE1); + 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 DerivedEA0& ea0, + const DerivedEA1& ea1, + const DerivedEB0& eb0, + const DerivedEB1& eb1) +{ + IPC_ASSERT_EIGEN_ARGS(DerivedEA0, DerivedEA1, DerivedEB0, DerivedEB1); + 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 DerivedEA0& ea0, + const DerivedEA1& ea1, + const DerivedEB0& eb0, + const DerivedEB1& eb1) +{ + IPC_ASSERT_EIGEN_ARGS(DerivedEA0, DerivedEA1, DerivedEB0, DerivedEB1); + 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 DerivedEA0& ea0, + const DerivedEA1& ea1, + const DerivedEB0& eb0, + const DerivedEB1& eb1) +{ + IPC_ASSERT_EIGEN_ARGS(DerivedEA0, DerivedEA1, DerivedEB0, DerivedEB1); + 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 DerivedP& p, + const DerivedT0& t0, + const DerivedT1& t1, + const DerivedT2& t2) +{ + IPC_ASSERT_EIGEN_ARGS(DerivedP, DerivedT0, DerivedT1, DerivedT2); + 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 DerivedP& p, + const DerivedT0& t0, + const DerivedT1& t1, + const DerivedT2& t2) +{ + IPC_ASSERT_EIGEN_ARGS(DerivedP, DerivedT0, DerivedT1, DerivedT2); + 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 DerivedP& p, + const DerivedT0& t0, + const DerivedT1& t1, + const DerivedT2& t2) +{ + IPC_ASSERT_EIGEN_ARGS(DerivedP, DerivedT0, DerivedT1, DerivedT2); + 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..7de0adfec 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 DerivedDP0& dp0, const DerivedDP1& dp1) +{ + IPC_ASSERT_EIGEN_ARGS(DerivedDP0, DerivedDP1); + 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,74 @@ 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 DerivedDP& dp, + const DerivedDE0& de0, + const DerivedDE1& de1, + const typename DerivedDP::Scalar alpha) +{ + IPC_ASSERT_EIGEN_ARGS(DerivedDP, DerivedDE0, DerivedDE1); + 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 +399,57 @@ 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 DerivedDEA0& dea0, + const DerivedDEA1& dea1, + const DerivedDEB0& deb0, + const DerivedDEB1& deb1, + const DerivedCoords& coords) +{ + IPC_ASSERT_EIGEN_ARGS( + DerivedDEA0, DerivedDEA1, DerivedDEB0, DerivedDEB1, DerivedCoords); + 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 DerivedCoords& coords) +{ + IPC_ASSERT_EIGEN_ARGS(DerivedCoords); + 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 DerivedCoords& coords) +{ + IPC_ASSERT_EIGEN_ARGS(DerivedCoords); + using T = typename DerivedCoords::Scalar; + return detail::edge_edge_relative_velocity_dx_dbeta(coords); +} // ============================================================================ // Point - Triangle @@ -92,23 +461,56 @@ 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 DerivedDP& dp, + const DerivedDT0& dt0, + const DerivedDT1& dt1, + const DerivedDT2& dt2, + const DerivedCoords& coords) +{ + IPC_ASSERT_EIGEN_ARGS( + DerivedDP, DerivedDT0, DerivedDT1, DerivedDT2, DerivedCoords); + 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 DerivedCoords& coords) +{ + IPC_ASSERT_EIGEN_ARGS(DerivedCoords); + 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 DerivedCoords& coords) +{ + IPC_ASSERT_EIGEN_ARGS(DerivedCoords); + 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..eda550832 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,52 @@ 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 DerivedP0& p0, const DerivedP1& p1) +{ + IPC_ASSERT_EIGEN_ARGS(DerivedP0, DerivedP1); + 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 DerivedP0& p0, const DerivedP1& p1) +{ + IPC_ASSERT_EIGEN_ARGS(DerivedP0, DerivedP1); + 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 +258,54 @@ 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 DerivedP& p, const DerivedE0& e0, const DerivedE1& e1) +{ + IPC_ASSERT_EIGEN_ARGS(DerivedP, DerivedE0, DerivedE1); + 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 DerivedP& p, const DerivedE0& e0, const DerivedE1& e1) +{ + IPC_ASSERT_EIGEN_ARGS(DerivedP, DerivedE0, DerivedE1); + 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 +316,23 @@ 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 DerivedEA0& ea0, + const DerivedEA1& ea1, + const DerivedEB0& eb0, + const DerivedEB1& eb1) +{ + IPC_ASSERT_EIGEN_ARGS(DerivedEA0, DerivedEA1, DerivedEB0, DerivedEB1); + 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 +340,21 @@ 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 DerivedEA0& ea0, + const DerivedEA1& ea1, + const DerivedEB0& eb0, + const DerivedEB1& eb1) +{ + IPC_ASSERT_EIGEN_ARGS(DerivedEA0, DerivedEA1, DerivedEB0, DerivedEB1); + using T = typename DerivedEA0::Scalar; + return detail::edge_edge_tangent_basis_jacobian(ea0, ea1, eb0, eb1); +} // ============================================================================ // Point - Triangle @@ -88,11 +373,21 @@ 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 DerivedP& p, + const DerivedT0& t0, + const DerivedT1& t1, + const DerivedT2& t2) +{ + IPC_ASSERT_EIGEN_ARGS(DerivedP, DerivedT0, DerivedT1, DerivedT2); + 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,89 @@ 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 DerivedP& p, + const DerivedT0& t0, + const DerivedT1& t1, + const DerivedT2& t2) +{ + IPC_ASSERT_EIGEN_ARGS(DerivedP, DerivedT0, DerivedT1, DerivedT2); + 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/tests/src/tests/benchmark_eigen.cpp b/tests/src/tests/benchmark_eigen.cpp index 93860c8ec..a2398f7e8 100644 --- a/tests/src/tests/benchmark_eigen.cpp +++ b/tests/src/tests/benchmark_eigen.cpp @@ -8,6 +8,12 @@ #include #include #include +#include +#include +#include +#include +#include +#include #include @@ -774,3 +780,146 @@ TEST_CASE("Float vs double", "[!benchmark][eigen][float]") #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/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 From a75a312e7719b0fbaf61e4898c78674dcf77f8e5 Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Tue, 1 Sep 2026 10:50:38 -0400 Subject: [PATCH 12/21] Bind the distance and tangent kernels directly in Python The public front ends deduce their scalar and dimension from the argument expressions, so their address cannot be taken. Rather than wrap each one in a lambda, bind the ipc::detail kernel with its scalar named explicitly -- &detail::f -- which is a concrete function and needs no wrapper. Two cases keep something else: - point_plane_distance, _gradient and _hessian each have two arities (a plane given as origin+normal, or as three triangle vertices). Both remain bound under one Python name, disambiguated with py::overload_cast on the parameter list. - edge_length_gradient, point_point_relative_velocity and point_edge_relative_velocity keep their lambdas. Their kernels are templated on the dimension, so any address-of would pin one; the lambdas take VectorMax3d and preserve the runtime 2D/3D dispatch these functions have always offered from Python. Docstrings, argument names and Python-visible signatures are unchanged. Verified against a freshly built ipctk (the first Python build of this work): nose2 -s python/tests passes 95/95, and a targeted probe exercises all 40 converted entry points. The repo suite covers none of these functions, so the probe is what actually checks them: both point_plane arities return the right shapes (3-vector/3x3 versus 12-vector/12x12), the three retained lambdas still accept 2D and 3D (shape (4,) versus (6,) from edge_length_gradient), the runtime-dim jacobians work at dim 2 and 3, and values are correct -- signed distance -1 where the unsigned distance is +1, edge-edge cross squared norm 16 for perpendicular length-2 edges. Co-Authored-By: Claude Opus 5 --- python/src/distance/edge_edge_mollifier.cpp | 38 +++++++-------------- python/src/distance/point_plane.cpp | 13 +++---- python/src/distance/signed_distance.cpp | 20 ++++++----- python/src/geometry/area.cpp | 7 ++-- python/src/tangent/relative_velocity.cpp | 33 +++++++++++------- 5 files changed, 56 insertions(+), 55 deletions(-) diff --git a/python/src/distance/edge_edge_mollifier.cpp b/python/src/distance/edge_edge_mollifier.cpp index c1b819170..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. @@ -135,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. @@ -161,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. @@ -184,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. @@ -207,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. @@ -229,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. @@ -253,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. @@ -272,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/point_plane.cpp b/python/src/distance/point_plane.cpp index c4ed14c7f..9039eb0e6 100644 --- a/python/src/distance/point_plane.cpp +++ b/python/src/distance/point_plane.cpp @@ -10,7 +10,8 @@ void define_point_plane_distance(py::module_& m) "point_plane_distance", py::overload_cast< Eigen::ConstRef, Eigen::ConstRef, - Eigen::ConstRef>(&point_plane_distance), + Eigen::ConstRef>( + &detail::point_plane_distance), R"ipc_Qu8mg5v7( Compute the distance between a point and a plane. @@ -32,7 +33,7 @@ void define_point_plane_distance(py::module_& m) py::overload_cast< Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef>( - &point_plane_distance), + &detail::point_plane_distance), R"ipc_Qu8mg5v7( Compute the distance between a point and a plane. @@ -55,7 +56,7 @@ void define_point_plane_distance(py::module_& m) py::overload_cast< Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef>( - &point_plane_distance_gradient), + &detail::point_plane_distance_gradient), R"ipc_Qu8mg5v7( Compute the gradient of the distance between a point and a plane. @@ -77,7 +78,7 @@ void define_point_plane_distance(py::module_& m) py::overload_cast< Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef>( - &point_plane_distance_gradient), + &detail::point_plane_distance_gradient), R"ipc_Qu8mg5v7( Compute the gradient of the distance between a point and a plane. @@ -100,7 +101,7 @@ void define_point_plane_distance(py::module_& m) py::overload_cast< Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef>( - &point_plane_distance_hessian), + &detail::point_plane_distance_hessian), R"ipc_Qu8mg5v7( Compute the hessian of the distance between a point and a plane. @@ -122,7 +123,7 @@ void define_point_plane_distance(py::module_& m) py::overload_cast< Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef>( - &point_plane_distance_hessian), + &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/signed_distance.cpp b/python/src/distance/signed_distance.cpp index 2138d6424..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). @@ -108,7 +110,7 @@ void define_signed_distance(py::module_& m) m.def( "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/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. From 9e4ea42d1556d2494ddaa6759a0b69bd0f139a3f Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Tue, 1 Sep 2026 10:55:19 -0400 Subject: [PATCH 13/21] Bind the remaining 3D distance kernels directly in Python Applies the &detail::f treatment to the bindings converted to lambdas earlier on this branch: line_line, edge_edge and point_triangle. Their kernels take Eigen::Vector3, so naming the scalar is enough to get a concrete function and the wrapper is unnecessary. The dtype parameters keep their py::arg defaults, which bind fine against the kernel's required parameter. Three groups deliberately keep their lambdas: - line_line_distance_gradient and line_line_distance_hessian have no detail kernel to address. They are single-layer Eigen::MatrixBase templates, which is correct for them: they read three coefficients per argument and hand them to generated code, so there is no whole-vector operation whose size a second layer would need to recover. - point_point, point_line, point_edge and point_edge_distance_type take VectorMax3d and dispatch on the dimension at runtime. - The normalization_* family, likewise VectorMax3d. Pinning a dimension in any of those would silently drop 2D from the Python API. Verified against a rebuilt ipctk: nose2 95/95, the previous 40-point probe still green, and a new 21-point probe covering the newly converted functions and the retained lambdas. Both dtype spellings (defaulted and explicit) return the same value, and every VectorMax3d entry point still answers in 2D and 3D. Docstrings, argument names and signatures unchanged. Co-Authored-By: Claude Opus 5 --- python/src/distance/edge_edge.cpp | 22 +++------------------- python/src/distance/line_line.cpp | 8 +------- python/src/distance/point_triangle.cpp | 25 +++---------------------- 3 files changed, 7 insertions(+), 48 deletions(-) diff --git a/python/src/distance/edge_edge.cpp b/python/src/distance/edge_edge.cpp index fcfb42b69..ec03e64f6 100644 --- a/python/src/distance/edge_edge.cpp +++ b/python/src/distance/edge_edge.cpp @@ -8,13 +8,7 @@ using namespace ipc; void define_edge_edge_distance(py::module_& m) { m.def( - "edge_edge_distance", - [](Eigen::ConstRef ea0, - Eigen::ConstRef ea1, - Eigen::ConstRef eb0, - Eigen::ConstRef eb1, EdgeEdgeDistanceType dtype) { - return edge_edge_distance(ea0, ea1, eb0, eb1, dtype); - }, + "edge_edge_distance", &detail::edge_edge_distance, R"ipc_Qu8mg5v7( Compute the distance between a two lines segments in 3D. @@ -36,12 +30,7 @@ void define_edge_edge_distance(py::module_& m) m.def( "edge_edge_distance_gradient", - [](Eigen::ConstRef ea0, - Eigen::ConstRef ea1, - Eigen::ConstRef eb0, - Eigen::ConstRef eb1, EdgeEdgeDistanceType dtype) { - return edge_edge_distance_gradient(ea0, ea1, eb0, eb1, dtype); - }, + &detail::edge_edge_distance_gradient, R"ipc_Qu8mg5v7( Compute the gradient of the distance between a two lines segments. @@ -63,12 +52,7 @@ void define_edge_edge_distance(py::module_& m) m.def( "edge_edge_distance_hessian", - [](Eigen::ConstRef ea0, - Eigen::ConstRef ea1, - Eigen::ConstRef eb0, - Eigen::ConstRef eb1, EdgeEdgeDistanceType dtype) { - return edge_edge_distance_hessian(ea0, ea1, eb0, eb1, dtype); - }, + &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/line_line.cpp b/python/src/distance/line_line.cpp index 77d2a9b0a..684bdee28 100644 --- a/python/src/distance/line_line.cpp +++ b/python/src/distance/line_line.cpp @@ -7,13 +7,7 @@ using namespace ipc; void define_line_line_distance(py::module_& m) { m.def( - "line_line_distance", - [](Eigen::ConstRef ea0, - Eigen::ConstRef ea1, - Eigen::ConstRef eb0, - Eigen::ConstRef eb1) { - return line_line_distance(ea0, ea1, eb0, eb1); - }, + "line_line_distance", &detail::line_line_distance, R"ipc_Qu8mg5v7( Compute the distance between a two infinite lines in 3D. diff --git a/python/src/distance/point_triangle.cpp b/python/src/distance/point_triangle.cpp index 0dc47d5ef..a51ab75e6 100644 --- a/python/src/distance/point_triangle.cpp +++ b/python/src/distance/point_triangle.cpp @@ -8,14 +8,7 @@ using namespace ipc; void define_point_triangle_distance(py::module_& m) { m.def( - "point_triangle_distance", - [](Eigen::ConstRef p, - Eigen::ConstRef t0, - Eigen::ConstRef t1, - Eigen::ConstRef t2, - PointTriangleDistanceType dtype) { - return point_triangle_distance(p, t0, t1, t2, dtype); - }, + "point_triangle_distance", &detail::point_triangle_distance, R"ipc_Qu8mg5v7( Compute the distance between a points and a triangle. @@ -37,13 +30,7 @@ void define_point_triangle_distance(py::module_& m) m.def( "point_triangle_distance_gradient", - [](Eigen::ConstRef p, - Eigen::ConstRef t0, - Eigen::ConstRef t1, - Eigen::ConstRef t2, - PointTriangleDistanceType dtype) { - return point_triangle_distance_gradient(p, t0, t1, t2, dtype); - }, + &detail::point_triangle_distance_gradient, R"ipc_Qu8mg5v7( Compute the gradient of the distance between a points and a triangle. @@ -65,13 +52,7 @@ void define_point_triangle_distance(py::module_& m) m.def( "point_triangle_distance_hessian", - [](Eigen::ConstRef p, - Eigen::ConstRef t0, - Eigen::ConstRef t1, - Eigen::ConstRef t2, - PointTriangleDistanceType dtype) { - return point_triangle_distance_hessian(p, t0, t1, t2, dtype); - }, + &detail::point_triangle_distance_hessian, R"ipc_Qu8mg5v7( Compute the hessian of the distance between a points and a triangle. From 6fee1d199c201314e28ec5a7c07d7dc65905503e Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Tue, 1 Sep 2026 10:58:06 -0400 Subject: [PATCH 14/21] Document the geometry-kernel conversion in the release notes Covers the three preceding commits: the two-layer conversion of the tangent, closest-point, relative-velocity, area, mollifier, point-plane and signed distance families; the measured speedups and the one unresolved regression; the header-inline versus translation-unit split and why it is load-bearing; and the Python bindings moving from lambdas to &detail::f. Also corrects two claims that the intervening work invalidated: the Highlights line named only the distance, barrier and normal APIs, and the API Changes section still advised wrapping in a lambda as the only way to take a function's address. Co-Authored-By: Claude Opus 5 --- docs/source/about/release_notes.rst | 31 +++++++++++++++++++++++++++-- 1 file changed, 29 insertions(+), 2 deletions(-) diff --git a/docs/source/about/release_notes.rst b/docs/source/about/release_notes.rst index ffa871189..cb4324621 100644 --- a/docs/source/about/release_notes.rst +++ b/docs/source/about/release_notes.rst @@ -15,7 +15,7 @@ v2.0.0 (alpha) Highlights ~~~~~~~~~~ -- 💥 **[Breaking]** The distance, barrier, and normal APIs are now templated on the scalar type, adding ``float`` support alongside ``double`` (and autodiff scalars where already supported). +- 💥 **[Breaking]** The distance, barrier, normal, tangent-basis, closest-point, relative-velocity, and area APIs are now templated on the scalar type and — where they are dimension-agnostic — on the dimension, adding ``float`` support alongside ``double`` (and autodiff scalars where already supported). On the call sites that dominate in practice this is 1.5–4.8× faster; see Performance below. - 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 `_). @@ -44,7 +44,7 @@ API Changes |:wrench:| - The free functions in ``ipc/distance/`` (``point_point_distance``, ``point_line_distance``, ``point_edge_distance``, ``line_line_distance``, ``edge_edge_distance``, ``point_plane_distance``, ``point_triangle_distance``, the ``*_distance_type`` predicates, the edge-edge mollifier, and the signed-distance variants), together with ``ipc/geometry/normal.hpp``, now take a scalar template parameter ``T`` and are instantiated for both ``float`` and ``double``. - Each function now accepts any Eigen expression (blocks, maps, and rows of a matrix), so call sites no longer need to materialize an ``Eigen::Vector3d`` before calling. Existing calls that let ``T`` be deduced from their arguments continue to compile unchanged. - - Taking the address of one of these functions is no longer possible — wrap the call in a lambda, as the Python bindings now do. See the two-layer API entry below. + - Taking the address of one of these functions is no longer possible; take the address of the ``ipc::detail`` kernel with its scalar named instead (``&ipc::detail::point_plane_distance``), or wrap the call in a lambda when the kernel is templated on the dimension. See the two-layer API entry below. - Mixed-precision calls no longer deduce: ``barrier(float_d, 0.001)`` must become ``barrier(float_d, 0.001f)``. - 💥 **[Breaking]** Templatize the barrier classes on the scalar type. @@ -64,6 +64,14 @@ API Changes |:wrench:| - 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]** 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. Assignments into ``VectorMax9d``-style storage compile unchanged; code binding the result with ``auto`` may now hold a fixed-size type. Taking the address of these function templates (e.g. ``&ipc::point_edge_distance_gradient``) is no longer possible — wrap the call in a lambda, as the Python bindings now do. +- 💥 **[Breaking]** Extend the two-layer design to the remaining per-collision kernels. + + - ``ipc/tangent/closest_point.hpp``, ``ipc/tangent/tangent_basis.hpp``, ``ipc/tangent/relative_velocity.hpp``, and ``ipc/geometry/area.hpp`` were ``double``-only with dimension-erased ``VectorMax``/``MatrixMax`` parameters. They now 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. + - ``ipc/distance/edge_edge_mollifier.hpp``, ``ipc/distance/point_plane.hpp``, and the three ``ipc/distance/signed/`` headers already had scalar-templated kernels; they gained the deducing front ends, so a caller can now pass a row of a matrix without materializing an ``Eigen::Vector3d`` first. + - ``relative_velocity``'s entry points that take a runtime ``int dim`` keep that exact signature (``template `` over dimension-templated kernels), so existing call sites are unaffected and ``float`` now works. + - No in-tree caller needed changing: the deducing front ends accept every argument type already in use. ``src/ipc/tangent/relative_velocity.cpp`` is deleted — that family is now entirely header-inline. + - ``ipc::detail::point_edge_closest_point_hessian`` no longer carries a stale "not implemented for 2D" note; 2D was always implemented. + - Add ``float`` aliases to ``ipc/utils/eigen_ext.hpp`` (``Vector1f``, ``Vector6f``, ``Matrix6f``, ``VectorMax3f``, ``MatrixMax9f``, …) mirroring the existing ``double`` ones. Performance |:zap:| @@ -75,6 +83,23 @@ Performance |:zap:| ``Eigen::ConstRef`` is the right default because it guarantees each argument is evaluated exactly once, but it is not free: binding an expression to a ``Ref`` materializes it into the ``Ref``'s internal buffer. For the ``line_line`` gradient and Hessian, which read only three coefficients per argument, that copy is pure cost — reading the operands in place through ``Eigen::MatrixBase`` measured 1.16× faster when the argument is an expression such as ``V.row(a) - V.row(b)``, and no slower anywhere else. Re-evaluating a three-element expression three times is cheaper than materializing it once at this size; that trade would invert for an argument whose evaluation is expensive. - ``line_line_distance`` (the value function), ``edge_edge_distance``, and ``point_triangle_distance`` were converted to the same two-layer shape. This is structural rather than a speed-up (measured within the noise floor of the unchanged control benchmarks): they keep their out-of-line switch dispatch, since inlining their 9- and 7-case switches measured as a 10% regression with runtime distance types. The payoff is that internal calls between distance functions now resolve directly to the kernel with no wrapper hop, which removed 42 redundant ``ipc::`` qualifications from the edge-edge and point-triangle sources. +- 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 (a row of a column-major matrix, whose ``ColsAtCompileTime`` is ``Dynamic``). Measured by interleaved A/B of two binaries, the baseline built from a worktree at the preceding commit, with unchanged same-TU control rows at 0.99–1.00×: + + .. code-block:: text + + before after + 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 + + The functions that were already fixed-size and 3D-only (``edge_edge`` and ``point_triangle`` closest point, ``triangle_area``, ``edge_edge_cross_squarednorm``, ``point_plane_distance``) measured neutral, which is the expected shape: the win is recovering the dimension, not the templating itself. One case regressed and is unresolved: ``point_point_tangent_basis`` on matrix rows measures 0.87–0.90×, while its known-dimension path improves 1.11×. + +- Keep small hand-written kernels header-inline and leave the wide generated (``autogen``) bodies in their translation units. This split is load-bearing rather than stylistic: with the tangent-basis and relative-velocity kernels defined out of line, the dimension-erased path paid an ``Eigen::Ref`` copy plus the ``VectorMax``/``MatrixMax`` wrap across an opaque call boundary, and ``point_edge_tangent_basis`` measured **2× slower** than before the conversion. Inlining only the small value and Jacobian kernels turned that into the numbers above. - 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). @@ -91,6 +116,8 @@ Bug Fixes |:bug:| Python |:snake:| ~~~~~~~~~~~~~~~~ +- Bind the ``ipc::detail`` kernels directly rather than through lambdas, now that the public front ends deduce their template arguments and so have no address to take. Most registrations are ``&ipc::detail::f``; ``point_plane_distance`` and its derivatives use ``py::overload_cast`` to keep both arities (plane as origin-plus-normal, or as three triangle vertices) under one Python name. The functions whose kernels are templated on the dimension keep a lambda taking ``VectorMax3d``, preserving the runtime 2D/3D dispatch they have always offered from Python. Docstrings, argument names, and signatures are unchanged. + - 💥 **[Breaking]** Rename the ``SmoothPotential`` class to ``SmoothContactPotential`` to match the C++ name (`#247 `_). It had no in-tree users and the package is still a 2.0 alpha, so this is a straight rename with no alias. - Fill gaps that made the GCP and convergent-formulation tutorials impossible to follow from Python (`#247 `_): From f4d91bd4d6cd6b9777a9887c4bfbdf212ab98df6 Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Tue, 1 Sep 2026 18:55:18 -0400 Subject: [PATCH 15/21] Rename NormalizedBarrier's private scalar alias to avoid an MSVC clash The private 'using T = typename BarrierT::value_type' shadowed the template parameter of any derived class that also names its parameter T. MSVC resolved 'NormalizedClampedLogBarrier' inside PhysicalBarrier to the inherited private typedef and rejected it as inaccessible, while Clang and GCC resolved it to the derived class's own parameter. --- src/ipc/barrier/barrier.hpp | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/src/ipc/barrier/barrier.hpp b/src/ipc/barrier/barrier.hpp index d347cf026..105705140 100644 --- a/src/ipc/barrier/barrier.hpp +++ b/src/ipc/barrier/barrier.hpp @@ -151,7 +151,7 @@ template class ClampedLogBarrier : public BarrierBase { /// @brief Normalized barrier function from [Li et al. 2023]. template class NormalizedBarrier : public BarrierT { - using T = typename BarrierT::value_type; + using Scalar = typename BarrierT::value_type; public: NormalizedBarrier() = default; @@ -166,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. - T operator()(const T d, const T dhat) const override + Scalar operator()(const Scalar d, const Scalar dhat) const override { - return BarrierT::operator()(d / dhat, T(1)); + return BarrierT::operator()(d / dhat, Scalar(1)); } /// @brief Derivative of the barrier function. @@ -182,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. - T first_derivative(const T d, const T dhat) const override + Scalar first_derivative(const Scalar d, const Scalar dhat) const override { - return BarrierT::first_derivative(d / dhat, T(1)) / dhat; + return BarrierT::first_derivative(d / dhat, Scalar(1)) / dhat; } /// @brief Second derivative of the barrier function. @@ -197,17 +197,17 @@ 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. - T second_derivative(const T d, const T dhat) const override + Scalar second_derivative(const Scalar d, const Scalar dhat) const override { - return BarrierT::second_derivative(d / dhat, T(1)) / (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. - T units(const T dhat) const override + Scalar units(const Scalar dhat) const override { - return T(1); // The normalized barrier is dimensionless. + return Scalar(1); // The normalized barrier is dimensionless. } }; From 53978e4c71e45635394bb29b722136b991a26712 Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Tue, 1 Sep 2026 18:55:30 -0400 Subject: [PATCH 16/21] Allow the _v variable-template suffix in the clang-tidy naming check is_eigen_expression_v, are_eigen_expressions_v, and dim_v follow the standard library's convention for variable templates, which the UPPER_CASE GlobalConstantCase rule rejects. --- .clang-tidy | 3 +++ 1 file changed, 3 insertions(+) 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 From c006d9066d738105c8699133f360029df88c92fe Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Tue, 1 Sep 2026 18:57:47 -0400 Subject: [PATCH 17/21] Match the CI clang-format version to the pinned pre-commit hook .pre-commit-config.yaml pins mirrors-clang-format v21.1.2, but the format check ran clang-format 20. The two disagree on how to pack the MatrixMax3 initializers in normal.hpp, so a file formatted by the hook a contributor actually runs failed CI. The whole tracked tree is already clean under 21. --- .github/workflows/clang-format-check.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 From 4769e4b0b8a9513d9c1debd5839e1c5b0d2a5707 Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Tue, 1 Sep 2026 21:10:36 -0400 Subject: [PATCH 18/21] Key the CI build cache on the runner CPU FindSIMD compiles with -march=native, so every cached object file carries the building runner's ISA. The cache key was runner.os + config with no CPU component, so objects built on one runner model were restored onto another and the test step died with SIGILL across unrelated suites (friction, candidates, CFL, plane-vertex collisions). The run that populated the cache compiled from scratch in 4m58s and passed; the next run restored 313 MB, built in 1m22s, and failed in 29s. --- .github/workflows/continuous.yml | 30 +++++++++++++++++++++++++++++- 1 file changed, 29 insertions(+), 1 deletion(-) diff --git a/.github/workflows/continuous.yml b/.github/workflows/continuous.yml index e743a63b9..0442a24e2 100644 --- a/.github/workflows/continuous.yml +++ b/.github/workflows/continuous.yml @@ -74,12 +74,40 @@ jobs: uses: SimenB/github-actions-cpu-cores@v2 id: cpu-cores + # FindSIMD compiles with -march=native, which bakes the building runner's + # ISA into every cached object file. The cache key had no CPU component, + # so objects built on one runner model were restored onto another and + # executed instructions it does not implement, failing the test step with + # SIGILL. Key the cache on the CPU so it only restores onto matching + # hardware. + - name: CPU fingerprint (Linux/macOS) + if: runner.os != 'Windows' + run: | + if [ "$RUNNER_OS" = "Linux" ]; then + CPU=$(grep -m1 '^model name' /proc/cpuinfo | cut -d: -f2-) + KEY=$(printf '%s' "$CPU" | sha256sum | cut -c1-12) + else + CPU=$(sysctl -n machdep.cpu.brand_string) + KEY=$(printf '%s' "$CPU" | shasum -a 256 | cut -c1-12) + fi + echo "CPU: $CPU" + echo "CPU_KEY=$KEY" >> "$GITHUB_ENV" + + - name: CPU fingerprint (Windows) + if: runner.os == 'Windows' + run: | + $cpu = Get-CimInstance Win32_Processor | Select-Object -First 1 -ExpandProperty Name + Write-Host "CPU: $cpu" + $stream = [IO.MemoryStream]::new([Text.Encoding]::UTF8.GetBytes($cpu)) + $key = (Get-FileHash -InputStream $stream -Algorithm SHA256).Hash.Substring(0, 12) + "CPU_KEY=$key" | Out-File -FilePath $env:GITHUB_ENV -Append + - name: Cache Build id: cache-build uses: actions/cache@v5 with: path: ${{ env.CACHE_PATH }} - key: ${{ runner.os }}-${{ matrix.config }}-cache + key: ${{ runner.os }}-${{ matrix.config }}-${{ env.CPU_KEY }}-cache - name: Prepare ccache run: | From aeb3db04cc13743980b97153797c6f413ecf86b7 Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Wed, 2 Sep 2026 11:53:48 -0400 Subject: [PATCH 19/21] Clean up release_notes.rst. --- docs/source/_static/css/custom.css | 8 ++ docs/source/about/release_notes.rst | 183 ++++++++++++++++++---------- 2 files changed, 129 insertions(+), 62 deletions(-) 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 cb4324621..062018a69 100644 --- a/docs/source/about/release_notes.rst +++ b/docs/source/about/release_notes.rst @@ -15,91 +15,147 @@ v2.0.0 (alpha) Highlights ~~~~~~~~~~ -- 💥 **[Breaking]** The distance, barrier, normal, tangent-basis, closest-point, relative-velocity, and area APIs are now templated on the scalar type and — where they are dimension-agnostic — on the dimension, adding ``float`` support alongside ``double`` (and autodiff scalars where already supported). On the call sites that dominate in practice this is 1.5–4.8× faster; see Performance below. +- 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 `_). -Continuous Collision Detection -~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ - -- 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. - 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 `_). - - - The out-parameters are seeded to NaN and written only once an intersection is confirmed, so a ``false`` return never leaves them partially populated. They also stay NaN in the degenerate rational case, where the coordinates are not uniquely defined. - - Boundary hits count as intersections (the comparisons are inclusive). - - 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:| ~~~~~~~~~~~~~~~~~~~~~~ -- 💥 **[Breaking]** Templatize the distance functions on the scalar type. +- Update Tight Inclusion from ``1.0.6`` to ``1.1.0`` (`#248 `_). - - The free functions in ``ipc/distance/`` (``point_point_distance``, ``point_line_distance``, ``point_edge_distance``, ``line_line_distance``, ``edge_edge_distance``, ``point_plane_distance``, ``point_triangle_distance``, the ``*_distance_type`` predicates, the edge-edge mollifier, and the signed-distance variants), together with ``ipc/geometry/normal.hpp``, now take a scalar template parameter ``T`` and are instantiated for both ``float`` and ``double``. - - Each function now accepts any Eigen expression (blocks, maps, and rows of a matrix), so call sites no longer need to materialize an ``Eigen::Vector3d`` before calling. Existing calls that let ``T`` be deduced from their arguments continue to compile unchanged. - - Taking the address of one of these functions is no longer possible; take the address of the ``ipc::detail`` kernel with its scalar named instead (``&ipc::detail::point_plane_distance``), or wrap the call in a lambda when the kernel is templated on the dimension. See the two-layer API entry below. - - Mixed-precision calls no longer deduce: ``barrier(float_d, 0.001)`` must become ``barrier(float_d, 0.001f)``. + - 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. -- 💥 **[Breaking]** Templatize the barrier classes on the scalar type. +- ``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 `_). - - ``ipc::Barrier`` is now an alias for the class template ``ipc::BarrierBase`` (defaulting to ``double``). Deriving from ``ipc::Barrier`` and overriding ``operator()``, ``first_derivative``, ``second_derivative``, and ``units`` is unchanged. - - The concrete barriers are now class templates and must be spelled with explicit template arguments: ``ipc::ClampedLogBarrier<>``, ``ipc::NormalizedClampedLogBarrier<>``, ``ipc::ClampedLogSqBarrier<>``, ``ipc::CubicBarrier<>``, and ``ipc::TwoStageBarrier<>``. - - The free functions ``ipc::barrier``, ``ipc::barrier_first_derivative``, and ``ipc::barrier_second_derivative`` are now templates defaulting to ``double``. + - 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 `_). - The Python API is unaffected: ``ipctk`` continues to expose the ``double`` instantiations under their existing names. +- Templatize the distance functions on the scalar type. -- 💥 **[Breaking]** The distance functions are now a two-layer API. + - 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. - - The concrete kernels moved into ``ipc::detail`` and are templated on the scalar and the dimension (``template ``, or just ```` for the 3D-only ``line_line``, ``edge_edge``, and ``point_triangle``). They take ``Eigen::ConstRef>`` and are explicitly instantiated for ``float``, ``double``, and — for the value functions only — the autodiff scalars. - - ``line_line_distance_gradient`` and ``line_line_distance_hessian`` are the exception: they have no ``detail`` kernel and are single ``Eigen::MatrixBase`` templates in ``ipc``. They read three coefficients per argument and hand them to the generated code, so there is nothing for a second layer to protect. Note that this leaves them without the ``static_assert`` below — an explicit ``line_line_distance_gradient(...)`` fails inside Eigen with ``undefined template 'Eigen::internal::traits'`` rather than a readable message. - - 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. - - Naming the scalar explicitly on a public function (``ipc::point_edge_distance(...)``) is now rejected by a ``static_assert`` telling you to let it deduce. Previously this either failed deep inside Eigen or — for the ``*_distance_type`` predicates — silently formed ``Eigen::MatrixBase``. Call ``ipc::detail::point_edge_distance(...)`` if you genuinely need to pin both parameters. - 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. -- 💥 **[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. Assignments into ``VectorMax9d``-style storage compile unchanged; code binding the result with ``auto`` may now hold a fixed-size type. Taking the address of these function templates (e.g. ``&ipc::point_edge_distance_gradient``) is no longer possible — wrap the call in a lambda, as the Python bindings now do. -- 💥 **[Breaking]** Extend the two-layer design to the remaining per-collision kernels. +- Templatize the barrier classes on the scalar type. - - ``ipc/tangent/closest_point.hpp``, ``ipc/tangent/tangent_basis.hpp``, ``ipc/tangent/relative_velocity.hpp``, and ``ipc/geometry/area.hpp`` were ``double``-only with dimension-erased ``VectorMax``/``MatrixMax`` parameters. They now 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. - - ``ipc/distance/edge_edge_mollifier.hpp``, ``ipc/distance/point_plane.hpp``, and the three ``ipc/distance/signed/`` headers already had scalar-templated kernels; they gained the deducing front ends, so a caller can now pass a row of a matrix without materializing an ``Eigen::Vector3d`` first. - - ``relative_velocity``'s entry points that take a runtime ``int dim`` keep that exact signature (``template `` over dimension-templated kernels), so existing call sites are unaffected and ``float`` now works. - - No in-tree caller needed changing: the deducing front ends accept every argument type already in use. ``src/ipc/tangent/relative_velocity.cpp`` is deleted — that family is now entirely header-inline. - - ``ipc::detail::point_edge_closest_point_hessian`` no longer carries a stale "not implemented for 2D" note; 2D was always implemented. + - ``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``. -- Add ``float`` aliases to ``ipc/utils/eigen_ext.hpp`` (``Vector1f``, ``Vector6f``, ``Matrix6f``, ``VectorMax3f``, ``MatrixMax9f``, …) mirroring the existing ``double`` ones. +- 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:| ~~~~~~~~~~~~~~~~~~~ -- Replace the three 2×2 LDLT solves in ``ipc::point_triangle_distance_type`` with a closed form. Each edge lies in the triangle's plane, so the 2×2 Gram matrix is diagonal and the solve collapses to two guarded divisions; the guards reproduce Eigen's pseudo-inverse handling of degenerate edges and triangles. 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 — a regime where the previous code's own answer flips under a one-ulp input perturbation more often than the two methods disagree. The closed form also fixes two real failures: at coordinate scales above ~1e+51 the LDLT collapsed to an infinite plane distance on every query, and in ``float`` at scales near 1e-6 Eigen's tolerance discarded denormal Gram entries (2 771 misclassifications per 400k vs 0). -- 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. Measured on matrix-row call sites: ``point_line_distance`` 8.2 → 2.3 ns (3.5×), ``point_edge_distance`` 18.1 → 5.2 ns AUTO (3.5×), ``point_point_distance_hessian`` up to 5×, ``normalization_and_jacobian`` 3.4× (4.9× on its hottest caller, ``opposite_direction_penalty_grad``). Erasing the dimension into a ``VectorMax3`` was the cost, not the parameter passing: the front end branches on ``size()`` *before* materializing anything, so the fixed-size path never touches a dynamic-size temporary. -- Pass the ``ipc::detail`` kernels' arguments as ``Eigen::ConstRef>`` rather than ``const Eigen::Vector&``. Measured never worse and 1.25–1.38× better on ``point_edge_distance``'s dimension-erased ``AUTO`` path; taking ``const&`` across a TU boundary forces the caller to materialize a temporary, which cost 2× on ``edge_edge_distance`` (5.7 → 12.4 ns). Because the kernels call each other, they must agree on this convention — a single ``const&`` layer breaks the direct-call chain and reintroduces the copy. +- 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. - ``Eigen::ConstRef`` is the right default because it guarantees each argument is evaluated exactly once, but it is not free: binding an expression to a ``Ref`` materializes it into the ``Ref``'s internal buffer. For the ``line_line`` gradient and Hessian, which read only three coefficients per argument, that copy is pure cost — reading the operands in place through ``Eigen::MatrixBase`` measured 1.16× faster when the argument is an expression such as ``V.row(a) - V.row(b)``, and no slower anywhere else. Re-evaluating a three-element expression three times is cheaper than materializing it once at this size; that trade would invert for an argument whose evaluation is expensive. -- ``line_line_distance`` (the value function), ``edge_edge_distance``, and ``point_triangle_distance`` were converted to the same two-layer shape. This is structural rather than a speed-up (measured within the noise floor of the unchanged control benchmarks): they keep their out-of-line switch dispatch, since inlining their 9- and 7-case switches measured as a 10% regression with runtime distance types. The payoff is that internal calls between distance functions now resolve directly to the kernel with no wrapper hop, which removed 42 redundant ``ipc::`` qualifications from the edge-edge and point-triangle sources. -- 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 (a row of a column-major matrix, whose ``ColsAtCompileTime`` is ``Dynamic``). Measured by interleaved A/B of two binaries, the baseline built from a worktree at the preceding commit, with unchanged same-TU control rows at 0.99–1.00×: + 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 - .. code-block:: text + Gradient assembly, before and after, with the strategy selected per scene. Benchmarked on Apple Silicon, AppleClang 21, Release; median of three runs. - before after - 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 +- Replace the three 2×2 LDLT solves in ``ipc::point_triangle_distance_type`` with a closed form (`#249 `_). - The functions that were already fixed-size and 3D-only (``edge_edge`` and ``point_triangle`` closest point, ``triangle_area``, ``edge_edge_cross_squarednorm``, ``point_plane_distance``) measured neutral, which is the expected shape: the win is recovering the dimension, not the templating itself. One case regressed and is unresolved: ``point_point_tangent_basis`` on matrix rows measures 0.87–0.90×, while its known-dimension path improves 1.11×. + - 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 -- Keep small hand-written kernels header-inline and leave the wide generated (``autogen``) bodies in their translation units. This split is load-bearing rather than stylistic: with the tangent-basis and relative-velocity kernels defined out of line, the dimension-erased path paid an ``Eigen::Ref`` copy plus the ``VectorMax``/``MatrixMax`` wrap across an opaque call boundary, and ``point_edge_tangent_basis`` measured **2× slower** than before the conversion. Inlining only the small value and Jacobian kernels turned that into the numbers above. - 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). @@ -108,17 +164,14 @@ 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. - -- Scale ``PARALLEL_THRESHOLD`` in ``ipc::edge_edge_distance_type`` with the precision of the scalar type. The value tuned for ``double`` (2.5e-16 ≈ 1.13ε) sits roughly 57× *below* the cancellation noise floor of :math:`u \times v` in single precision, which made the near-parallel branch unreachable for ``float``: across 20k exactly-parallel edge pairs the ``float`` distance type disagreed with the ``double`` one 44% of the time, with distance errors up to 24× relative. The ``double`` threshold is bit-for-bit unchanged. -- Guard the ``*_distance_type`` Eigen-expression overloads with ``std::is_class_v``. Unlike the other overloads, their return type is a non-dependent enum, so an explicit scalar argument (e.g. ``edge_edge_distance_type(...)``) was not rejected by SFINAE and instead formed ``Eigen::MatrixBase``, a hard error inside Eigen. + - ``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:| ~~~~~~~~~~~~~~~~ -- Bind the ``ipc::detail`` kernels directly rather than through lambdas, now that the public front ends deduce their template arguments and so have no address to take. Most registrations are ``&ipc::detail::f``; ``point_plane_distance`` and its derivatives use ``py::overload_cast`` to keep both arities (plane as origin-plus-normal, or as three triangle vertices) under one Python name. The functions whose kernels are templated on the dimension keep a lambda taking ``VectorMax3d``, preserving the runtime 2D/3D dispatch they have always offered from Python. Docstrings, argument names, and signatures are unchanged. - -- 💥 **[Breaking]** Rename the ``SmoothPotential`` class to ``SmoothContactPotential`` to match the C++ name (`#247 `_). It had no in-tree users and the package is still a 2.0 alpha, so this is a straight rename with no alias. +- 💥 **[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. @@ -139,7 +192,12 @@ Documentation - ``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. +- 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 @@ -150,6 +208,7 @@ Refactor 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 `_). From 9d98b690756b9206a316caf74c4a43ff7722c381 Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Wed, 2 Sep 2026 15:06:26 -0400 Subject: [PATCH 20/21] Move low-level normal/distance templates into ipc::detail behind SFINAE-friendly front ends Push the fixed-size, per-scalar implementations (point-line/triangle/line-line normals, point/edge/triangle/plane distances, tangent bases, relative velocities) into ipc::detail, and give each a thin ipc:: front end that takes Eigen::MatrixBase so overload resolution SFINAEs away cleanly instead of hard-erroring on non-Eigen arguments. Drops the IPC_ASSERT_EIGEN_ARGS macro and its is_eigen_expression_v/are_eigen_expressions_v traits now that the MatrixBase parameter does that job directly. Copies the detail-side Doxygen comments down to the public wrappers so the documented API carries its own docs instead of pointing into an internal namespace. Co-Authored-By: Claude Sonnet 5 --- python/src/distance/distance_type.cpp | 7 +- python/src/geometry/normal.cpp | 37 +- src/ipc/distance/distance_type.cpp | 47 +- src/ipc/distance/distance_type.hpp | 119 ++-- src/ipc/distance/edge_edge.hpp | 57 +- src/ipc/distance/edge_edge_mollifier.hpp | 112 ++- src/ipc/distance/line_line.hpp | 35 +- src/ipc/distance/point_edge.hpp | 27 +- src/ipc/distance/point_line.hpp | 17 +- src/ipc/distance/point_plane.hpp | 81 ++- src/ipc/distance/point_point.hpp | 17 +- src/ipc/distance/point_triangle.hpp | 57 +- src/ipc/distance/signed/line_line.hpp | 84 ++- src/ipc/distance/signed/point_line.cpp | 6 +- src/ipc/distance/signed/point_line.hpp | 60 +- src/ipc/distance/signed/point_plane.hpp | 59 +- src/ipc/geometry/area.hpp | 22 +- src/ipc/geometry/normal.cpp | 4 +- src/ipc/geometry/normal.hpp | 835 ++++++++++++----------- src/ipc/tangent/closest_point.hpp | 69 +- src/ipc/tangent/relative_velocity.hpp | 55 +- src/ipc/tangent/tangent_basis.hpp | 57 +- src/ipc/utils/eigen_ext.hpp | 33 - tests/src/tests/benchmark_eigen.cpp | 18 +- 24 files changed, 1063 insertions(+), 852 deletions(-) diff --git a/python/src/distance/distance_type.cpp b/python/src/distance/distance_type.cpp index dec9be881..08cca965c 100644 --- a/python/src/distance/distance_type.cpp +++ b/python/src/distance/distance_type.cpp @@ -105,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. @@ -121,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. @@ -138,7 +139,7 @@ void define_distance_type(py::module_& m) m.def( "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/geometry/normal.cpp b/python/src/geometry/normal.cpp index d0bfa9fcb..2defaf4fc 100644 --- a/python/src/geometry/normal.cpp +++ b/python/src/geometry/normal.cpp @@ -77,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. @@ -92,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 @@ -102,7 +103,7 @@ void define_normal(py::module_& m) m.def( "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. @@ -119,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. @@ -137,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. @@ -154,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. @@ -171,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. @@ -189,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. @@ -207,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. @@ -224,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. @@ -241,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. @@ -258,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. @@ -276,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. @@ -295,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. @@ -313,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. @@ -332,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. @@ -350,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/src/ipc/distance/distance_type.cpp b/src/ipc/distance/distance_type.cpp index 7f8c0fc82..f9b281e42 100644 --- a/src/ipc/distance/distance_type.cpp +++ b/src/ipc/distance/distance_type.cpp @@ -8,35 +8,30 @@ #include #include -#include -namespace ipc { +namespace ipc::detail { -namespace detail { - - void warn_degenerate_point_edge() noexcept - { - logger().warn("Degenerate edge in point_edge_distance_type!"); - } - - void throw_invalid_distance_type(const char* function) - { - throw std::invalid_argument( - fmt::format("{}: invalid distance type", function)); - } +void warn_degenerate_point_edge() noexcept +{ + logger().warn("Degenerate edge in point_edge_distance_type!"); +} - 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)); - } +void throw_invalid_distance_type(const char* function) +{ + throw std::invalid_argument( + fmt::format("{}: invalid distance type", function)); +} -} // namespace detail +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)); +} template PointTriangleDistanceType point_triangle_distance_type( @@ -250,4 +245,4 @@ template EdgeEdgeDistanceType edge_edge_parallel_distance_type(Eigen::Con template EdgeEdgeDistanceType edge_edge_parallel_distance_type(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef); // clang-format on -} // namespace ipc +} // namespace ipc::detail diff --git a/src/ipc/distance/distance_type.hpp b/src/ipc/distance/distance_type.hpp index 67a54428d..786d38ec2 100644 --- a/src/ipc/distance/distance_type.hpp +++ b/src/ipc/distance/distance_type.hpp @@ -102,46 +102,46 @@ namespace detail { return PointEdgeDistanceType::P_E; // PE } } -} // namespace detail -/// @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); + /// @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 --- // @@ -160,9 +160,10 @@ EdgeEdgeDistanceType edge_edge_parallel_distance_type( /// @return The distance type of the point-edge pair. template inline PointEdgeDistanceType point_edge_distance_type( - const DerivedP& p, const DerivedE0& e0, const DerivedE1& e1) + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& e0, + const Eigen::MatrixBase& e1) { - IPC_ASSERT_EIGEN_ARGS(DerivedP, DerivedE0, DerivedE1); using T = typename DerivedP::Scalar; if constexpr (dim_v == 2) { @@ -177,6 +178,12 @@ inline PointEdgeDistanceType point_edge_distance_type( } } +/// @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 < typename DerivedP, typename DerivedT0, @@ -190,13 +197,15 @@ inline PointTriangleDistanceType point_triangle_distance_type( const Eigen::MatrixBase& t2) { using T = typename DerivedP::Scalar; - return point_triangle_distance_type( - Eigen::Ref>(p), - Eigen::Ref>(t0), - Eigen::Ref>(t1), - Eigen::Ref>(t2)); + 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. +/// @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 < typename DerivedEA0, typename DerivedEA1, @@ -210,13 +219,15 @@ inline EdgeEdgeDistanceType edge_edge_distance_type( const Eigen::MatrixBase& eb1) { using T = typename DerivedEA0::Scalar; - return edge_edge_distance_type( - Eigen::Ref>(ea0), - Eigen::Ref>(ea1), - Eigen::Ref>(eb0), - Eigen::Ref>(eb1)); + 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. +/// @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 < typename DerivedEA0, typename DerivedEA1, @@ -230,11 +241,7 @@ inline EdgeEdgeDistanceType edge_edge_parallel_distance_type( const Eigen::MatrixBase& eb1) { using T = typename DerivedEA0::Scalar; - return edge_edge_parallel_distance_type( - Eigen::Ref>(ea0), - Eigen::Ref>(ea1), - Eigen::Ref>(eb0), - Eigen::Ref>(eb1)); + return detail::edge_edge_parallel_distance_type(ea0, ea1, eb0, eb1); } } // namespace ipc diff --git a/src/ipc/distance/edge_edge.hpp b/src/ipc/distance/edge_edge.hpp index ab69992ae..d07e3b464 100644 --- a/src/ipc/distance/edge_edge.hpp +++ b/src/ipc/distance/edge_edge.hpp @@ -20,7 +20,7 @@ namespace detail { Eigen::ConstRef> ea1, Eigen::ConstRef> eb0, Eigen::ConstRef> eb1, - EdgeEdgeDistanceType dtype); + EdgeEdgeDistanceType dtype = EdgeEdgeDistanceType::AUTO); /// @brief Compute the gradient of the distance between a two lines segments. /// @note The distance is actually squared distance. @@ -36,7 +36,7 @@ namespace detail { Eigen::ConstRef> ea1, Eigen::ConstRef> eb0, Eigen::ConstRef> eb1, - EdgeEdgeDistanceType dtype); + EdgeEdgeDistanceType dtype = EdgeEdgeDistanceType::AUTO); /// @brief Compute the hessian of the distance between a two lines segments. /// @note The distance is actually squared distance. @@ -52,58 +52,79 @@ namespace detail { Eigen::ConstRef> ea1, Eigen::ConstRef> eb0, Eigen::ConstRef> eb1, - EdgeEdgeDistanceType dtype); + 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. +/// @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 < typename DerivedEA0, typename DerivedEA1, typename DerivedEB0, typename DerivedEB1> inline auto edge_edge_distance( - const DerivedEA0& ea0, - const DerivedEA1& ea1, - const DerivedEB0& eb0, - const DerivedEB1& eb1, + const Eigen::MatrixBase& ea0, + const Eigen::MatrixBase& ea1, + const Eigen::MatrixBase& eb0, + const Eigen::MatrixBase& eb1, EdgeEdgeDistanceType dtype = EdgeEdgeDistanceType::AUTO) { - IPC_ASSERT_EIGEN_ARGS(DerivedEA0, DerivedEA1, DerivedEB0, DerivedEB1); 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. +/// @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 < typename DerivedEA0, typename DerivedEA1, typename DerivedEB0, typename DerivedEB1> inline auto edge_edge_distance_gradient( - const DerivedEA0& ea0, - const DerivedEA1& ea1, - const DerivedEB0& eb0, - const DerivedEB1& eb1, + const Eigen::MatrixBase& ea0, + const Eigen::MatrixBase& ea1, + const Eigen::MatrixBase& eb0, + const Eigen::MatrixBase& eb1, EdgeEdgeDistanceType dtype = EdgeEdgeDistanceType::AUTO) { - IPC_ASSERT_EIGEN_ARGS(DerivedEA0, DerivedEA1, DerivedEB0, DerivedEB1); 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. +/// @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 < typename DerivedEA0, typename DerivedEA1, typename DerivedEB0, typename DerivedEB1> inline auto edge_edge_distance_hessian( - const DerivedEA0& ea0, - const DerivedEA1& ea1, - const DerivedEB0& eb0, - const DerivedEB1& eb1, + const Eigen::MatrixBase& ea0, + const Eigen::MatrixBase& ea1, + const Eigen::MatrixBase& eb0, + const Eigen::MatrixBase& eb1, EdgeEdgeDistanceType dtype = EdgeEdgeDistanceType::AUTO) { - IPC_ASSERT_EIGEN_ARGS(DerivedEA0, DerivedEA1, DerivedEB0, DerivedEB1); using T = typename DerivedEA0::Scalar; return detail::edge_edge_distance_hessian(ea0, ea1, eb0, eb1, dtype); } diff --git a/src/ipc/distance/edge_edge_mollifier.hpp b/src/ipc/distance/edge_edge_mollifier.hpp index 496293798..56e5f3cc0 100644 --- a/src/ipc/distance/edge_edge_mollifier.hpp +++ b/src/ipc/distance/edge_edge_mollifier.hpp @@ -267,12 +267,11 @@ template < typename DerivedEB0, typename DerivedEB1> inline auto edge_edge_cross_squarednorm( - const DerivedEA0& ea0, - const DerivedEA1& ea1, - const DerivedEB0& eb0, - const DerivedEB1& eb1) + const Eigen::MatrixBase& ea0, + const Eigen::MatrixBase& ea1, + const Eigen::MatrixBase& eb0, + const Eigen::MatrixBase& eb1) { - IPC_ASSERT_EIGEN_ARGS(DerivedEA0, DerivedEA1, DerivedEB0, DerivedEB1); using T = typename DerivedEA0::Scalar; // NOTE: explicit : the detail overload cannot deduce T from an // arbitrary Eigen expression. @@ -292,12 +291,11 @@ template < typename DerivedEB0, typename DerivedEB1> inline auto edge_edge_cross_squarednorm_gradient( - const DerivedEA0& ea0, - const DerivedEA1& ea1, - const DerivedEB0& eb0, - const DerivedEB1& eb1) + const Eigen::MatrixBase& ea0, + const Eigen::MatrixBase& ea1, + const Eigen::MatrixBase& eb0, + const Eigen::MatrixBase& eb1) { - IPC_ASSERT_EIGEN_ARGS(DerivedEA0, DerivedEA1, DerivedEB0, DerivedEB1); using T = typename DerivedEA0::Scalar; return detail::edge_edge_cross_squarednorm_gradient(ea0, ea1, eb0, eb1); } @@ -315,12 +313,11 @@ template < typename DerivedEB0, typename DerivedEB1> inline auto edge_edge_cross_squarednorm_hessian( - const DerivedEA0& ea0, - const DerivedEA1& ea1, - const DerivedEB0& eb0, - const DerivedEB1& eb1) + const Eigen::MatrixBase& ea0, + const Eigen::MatrixBase& ea1, + const Eigen::MatrixBase& eb0, + const Eigen::MatrixBase& eb1) { - IPC_ASSERT_EIGEN_ARGS(DerivedEA0, DerivedEA1, DerivedEB0, DerivedEB1); using T = typename DerivedEA0::Scalar; return detail::edge_edge_cross_squarednorm_hessian(ea0, ea1, eb0, eb1); } @@ -341,13 +338,12 @@ template < typename DerivedEB0, typename DerivedEB1> inline auto edge_edge_mollifier( - const DerivedEA0& ea0, - const DerivedEA1& ea1, - const DerivedEB0& eb0, - const DerivedEB1& eb1, + const Eigen::MatrixBase& ea0, + const Eigen::MatrixBase& ea1, + const Eigen::MatrixBase& eb0, + const Eigen::MatrixBase& eb1, const typename DerivedEA0::Scalar eps_x) { - IPC_ASSERT_EIGEN_ARGS(DerivedEA0, DerivedEA1, DerivedEB0, DerivedEB1); using T = typename DerivedEA0::Scalar; return detail::edge_edge_mollifier(ea0, ea1, eb0, eb1, eps_x); } @@ -365,13 +361,12 @@ template < typename DerivedEB0, typename DerivedEB1> inline auto edge_edge_mollifier_gradient( - const DerivedEA0& ea0, - const DerivedEA1& ea1, - const DerivedEB0& eb0, - const DerivedEB1& eb1, + const Eigen::MatrixBase& ea0, + const Eigen::MatrixBase& ea1, + const Eigen::MatrixBase& eb0, + const Eigen::MatrixBase& eb1, const typename DerivedEA0::Scalar eps_x) { - IPC_ASSERT_EIGEN_ARGS(DerivedEA0, DerivedEA1, DerivedEB0, DerivedEB1); using T = typename DerivedEA0::Scalar; return detail::edge_edge_mollifier_gradient(ea0, ea1, eb0, eb1, eps_x); } @@ -389,13 +384,12 @@ template < typename DerivedEB0, typename DerivedEB1> inline auto edge_edge_mollifier_hessian( - const DerivedEA0& ea0, - const DerivedEA1& ea1, - const DerivedEB0& eb0, - const DerivedEB1& eb1, + const Eigen::MatrixBase& ea0, + const Eigen::MatrixBase& ea1, + const Eigen::MatrixBase& eb0, + const Eigen::MatrixBase& eb1, const typename DerivedEA0::Scalar eps_x) { - IPC_ASSERT_EIGEN_ARGS(DerivedEA0, DerivedEA1, DerivedEB0, DerivedEB1); using T = typename DerivedEA0::Scalar; return detail::edge_edge_mollifier_hessian(ea0, ea1, eb0, eb1, eps_x); } @@ -421,18 +415,15 @@ template < typename DerivedEB0, typename DerivedEB1> inline auto edge_edge_mollifier_gradient_wrt_x( - const DerivedEA0Rest& ea0_rest, - const DerivedEA1Rest& ea1_rest, - const DerivedEB0Rest& eb0_rest, - const DerivedEB1Rest& eb1_rest, - const DerivedEA0& ea0, - const DerivedEA1& ea1, - const DerivedEB0& eb0, - const DerivedEB1& eb1) + 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) { - IPC_ASSERT_EIGEN_ARGS( - DerivedEA0Rest, DerivedEA1Rest, DerivedEB0Rest, DerivedEB1Rest, - DerivedEA0, DerivedEA1, DerivedEB0, DerivedEB1); 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); @@ -461,18 +452,15 @@ template < typename DerivedEB0, typename DerivedEB1> inline auto edge_edge_mollifier_gradient_jacobian_wrt_x( - const DerivedEA0Rest& ea0_rest, - const DerivedEA1Rest& ea1_rest, - const DerivedEB0Rest& eb0_rest, - const DerivedEB1Rest& eb1_rest, - const DerivedEA0& ea0, - const DerivedEA1& ea1, - const DerivedEB0& eb0, - const DerivedEB1& eb1) + 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) { - IPC_ASSERT_EIGEN_ARGS( - DerivedEA0Rest, DerivedEA1Rest, DerivedEB0Rest, DerivedEB1Rest, - DerivedEA0, DerivedEA1, DerivedEB0, DerivedEB1); 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); @@ -493,13 +481,11 @@ template < typename DerivedEB0Rest, typename DerivedEB1Rest> inline auto edge_edge_mollifier_threshold( - const DerivedEA0Rest& ea0_rest, - const DerivedEA1Rest& ea1_rest, - const DerivedEB0Rest& eb0_rest, - const DerivedEB1Rest& eb1_rest) + const Eigen::MatrixBase& ea0_rest, + const Eigen::MatrixBase& ea1_rest, + const Eigen::MatrixBase& eb0_rest, + const Eigen::MatrixBase& eb1_rest) { - IPC_ASSERT_EIGEN_ARGS( - DerivedEA0Rest, DerivedEA1Rest, DerivedEB0Rest, DerivedEB1Rest); using T = typename DerivedEA0Rest::Scalar; return detail::edge_edge_mollifier_threshold( ea0_rest, ea1_rest, eb0_rest, eb1_rest); @@ -521,13 +507,11 @@ template < typename DerivedEB0Rest, typename DerivedEB1Rest> inline auto edge_edge_mollifier_threshold_gradient( - const DerivedEA0Rest& ea0_rest, - const DerivedEA1Rest& ea1_rest, - const DerivedEB0Rest& eb0_rest, - const DerivedEB1Rest& eb1_rest) + const Eigen::MatrixBase& ea0_rest, + const Eigen::MatrixBase& ea1_rest, + const Eigen::MatrixBase& eb0_rest, + const Eigen::MatrixBase& eb1_rest) { - IPC_ASSERT_EIGEN_ARGS( - DerivedEA0Rest, DerivedEA1Rest, DerivedEB0Rest, DerivedEB1Rest); using T = typename DerivedEA0Rest::Scalar; return detail::edge_edge_mollifier_threshold_gradient( ea0_rest, ea1_rest, eb0_rest, eb1_rest); diff --git a/src/ipc/distance/line_line.hpp b/src/ipc/distance/line_line.hpp index acfdcbe47..cb0b35f34 100644 --- a/src/ipc/distance/line_line.hpp +++ b/src/ipc/distance/line_line.hpp @@ -40,18 +40,25 @@ namespace detail { } } // 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 < typename DerivedEA0, typename DerivedEA1, typename DerivedEB0, typename DerivedEB1> inline auto line_line_distance( - const DerivedEA0& ea0, - const DerivedEA1& ea1, - const DerivedEB0& eb0, - const DerivedEB1& eb1) + const Eigen::MatrixBase& ea0, + const Eigen::MatrixBase& ea1, + const Eigen::MatrixBase& eb0, + const Eigen::MatrixBase& eb1) { - IPC_ASSERT_EIGEN_ARGS(DerivedEA0, DerivedEA1, DerivedEB0, DerivedEB1); using T = typename DerivedEA0::Scalar; // NOTE: explicit : the detail overload cannot deduce T from an // arbitrary Eigen expression. @@ -64,12 +71,11 @@ template < typename DerivedEB0, typename DerivedEB1> inline auto line_line_distance_gradient( - const DerivedEA0& ea0, - const DerivedEA1& ea1, - const DerivedEB0& eb0, - const DerivedEB1& eb1) + const Eigen::MatrixBase& ea0, + const Eigen::MatrixBase& ea1, + const Eigen::MatrixBase& eb0, + const Eigen::MatrixBase& eb1) { - IPC_ASSERT_EIGEN_ARGS(DerivedEA0, DerivedEA1, DerivedEB0, DerivedEB1); 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], @@ -83,12 +89,11 @@ template < typename DerivedEB0, typename DerivedEB1> inline auto line_line_distance_hessian( - const DerivedEA0& ea0, - const DerivedEA1& ea1, - const DerivedEB0& eb0, - const DerivedEB1& eb1) + const Eigen::MatrixBase& ea0, + const Eigen::MatrixBase& ea1, + const Eigen::MatrixBase& eb0, + const Eigen::MatrixBase& eb1) { - IPC_ASSERT_EIGEN_ARGS(DerivedEA0, DerivedEA1, DerivedEB0, DerivedEB1); 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], diff --git a/src/ipc/distance/point_edge.hpp b/src/ipc/distance/point_edge.hpp index 30f45748b..80875cec6 100644 --- a/src/ipc/distance/point_edge.hpp +++ b/src/ipc/distance/point_edge.hpp @@ -25,7 +25,7 @@ namespace detail { Eigen::ConstRef> p, Eigen::ConstRef> e0, Eigen::ConstRef> e1, - PointEdgeDistanceType dtype) + PointEdgeDistanceType dtype = PointEdgeDistanceType::AUTO) { static_assert(dim == 2 || dim == 3, "point-edge is only 2D or 3D"); @@ -66,7 +66,7 @@ namespace detail { Eigen::ConstRef> p, Eigen::ConstRef> e0, Eigen::ConstRef> e1, - PointEdgeDistanceType dtype) + PointEdgeDistanceType dtype = PointEdgeDistanceType::AUTO) { static_assert(dim == 2 || dim == 3, "point-edge is only 2D or 3D"); @@ -118,7 +118,7 @@ namespace detail { Eigen::ConstRef> p, Eigen::ConstRef> e0, Eigen::ConstRef> e1, - PointEdgeDistanceType dtype); + PointEdgeDistanceType dtype = PointEdgeDistanceType::AUTO); } // namespace detail /// @brief Compute the distance between a point and edge in 2D or 3D. @@ -130,12 +130,11 @@ namespace detail { /// @return The distance between the point and edge. template inline auto point_edge_distance( - const DerivedP& p, - const DerivedE0& e0, - const DerivedE1& e1, + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& e0, + const Eigen::MatrixBase& e1, PointEdgeDistanceType dtype = PointEdgeDistanceType::AUTO) { - IPC_ASSERT_EIGEN_ARGS(DerivedP, DerivedE0, DerivedE1); using T = typename DerivedP::Scalar; if constexpr (dim_v == 2) { @@ -159,12 +158,11 @@ inline auto point_edge_distance( /// @return The gradient of the distance wrt p, e0, and e1. template inline auto point_edge_distance_gradient( - const DerivedP& p, - const DerivedE0& e0, - const DerivedE1& e1, + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& e0, + const Eigen::MatrixBase& e1, PointEdgeDistanceType dtype = PointEdgeDistanceType::AUTO) { - IPC_ASSERT_EIGEN_ARGS(DerivedP, DerivedE0, DerivedE1); using T = typename DerivedP::Scalar; if constexpr (dim_v == 2) { return detail::point_edge_distance_gradient(p, e0, e1, dtype); @@ -189,12 +187,11 @@ inline auto point_edge_distance_gradient( /// @return The hessian of the distance wrt p, e0, and e1. template inline auto point_edge_distance_hessian( - const DerivedP& p, - const DerivedE0& e0, - const DerivedE1& e1, + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& e0, + const Eigen::MatrixBase& e1, PointEdgeDistanceType dtype = PointEdgeDistanceType::AUTO) { - IPC_ASSERT_EIGEN_ARGS(DerivedP, DerivedE0, DerivedE1); using T = typename DerivedP::Scalar; if constexpr (dim_v == 2) { return detail::point_edge_distance_hessian(p, e0, e1, dtype); diff --git a/src/ipc/distance/point_line.hpp b/src/ipc/distance/point_line.hpp index 41a3680db..988f53bfb 100644 --- a/src/ipc/distance/point_line.hpp +++ b/src/ipc/distance/point_line.hpp @@ -111,10 +111,11 @@ namespace detail { /// @param e1 The second vertex of the edge defining the line. /// @return The distance between the point and line. template -inline auto -point_line_distance(const DerivedP& p, const DerivedE0& e0, const DerivedE1& e1) +inline auto point_line_distance( + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& e0, + const Eigen::MatrixBase& e1) { - IPC_ASSERT_EIGEN_ARGS(DerivedP, DerivedE0, DerivedE1); using T = typename DerivedP::Scalar; if constexpr (dim_v == 2) { @@ -137,9 +138,10 @@ point_line_distance(const DerivedP& p, const DerivedE0& e0, const DerivedE1& e1) /// @return The gradient of the distance wrt p, e0, and e1. template inline auto point_line_distance_gradient( - const DerivedP& p, const DerivedE0& e0, const DerivedE1& e1) + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& e0, + const Eigen::MatrixBase& e1) { - IPC_ASSERT_EIGEN_ARGS(DerivedP, DerivedE0, DerivedE1); using T = typename DerivedP::Scalar; if constexpr (dim_v == 2) { return detail::point_line_distance_gradient(p, e0, e1); @@ -169,9 +171,10 @@ inline auto point_line_distance_gradient( /// @return The hessian of the distance wrt p, e0, and e1. template inline auto point_line_distance_hessian( - const DerivedP& p, const DerivedE0& e0, const DerivedE1& e1) + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& e0, + const Eigen::MatrixBase& e1) { - IPC_ASSERT_EIGEN_ARGS(DerivedP, DerivedE0, DerivedE1); using T = typename DerivedP::Scalar; if constexpr (dim_v == 2) { return detail::point_line_distance_hessian(p, e0, e1); diff --git a/src/ipc/distance/point_plane.hpp b/src/ipc/distance/point_plane.hpp index e7b58764d..9a2416acb 100644 --- a/src/ipc/distance/point_plane.hpp +++ b/src/ipc/distance/point_plane.hpp @@ -128,79 +128,118 @@ namespace detail { } } // 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 auto point_plane_distance( - const DerivedP& p, const DerivedOrigin& origin, const DerivedNormal& normal) + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& origin, + const Eigen::MatrixBase& normal) { - IPC_ASSERT_EIGEN_ARGS(DerivedP, DerivedOrigin, DerivedNormal); 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. +/// @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 < typename DerivedP, typename DerivedT0, typename DerivedT1, typename DerivedT2> inline auto point_plane_distance( - const DerivedP& p, - const DerivedT0& t0, - const DerivedT1& t1, - const DerivedT2& t2) + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& t0, + const Eigen::MatrixBase& t1, + const Eigen::MatrixBase& t2) { - IPC_ASSERT_EIGEN_ARGS(DerivedP, DerivedT0, DerivedT1, DerivedT2); 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. +/// @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 auto point_plane_distance_gradient( - const DerivedP& p, const DerivedOrigin& origin, const DerivedNormal& normal) + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& origin, + const Eigen::MatrixBase& normal) { - IPC_ASSERT_EIGEN_ARGS(DerivedP, DerivedOrigin, DerivedNormal); 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. +/// @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 < typename DerivedP, typename DerivedT0, typename DerivedT1, typename DerivedT2> inline auto point_plane_distance_gradient( - const DerivedP& p, - const DerivedT0& t0, - const DerivedT1& t1, - const DerivedT2& t2) + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& t0, + const Eigen::MatrixBase& t1, + const Eigen::MatrixBase& t2) { - IPC_ASSERT_EIGEN_ARGS(DerivedP, DerivedT0, DerivedT1, DerivedT2); 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. +/// @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 auto point_plane_distance_hessian( - const DerivedP& p, const DerivedOrigin& origin, const DerivedNormal& normal) + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& origin, + const Eigen::MatrixBase& normal) { - IPC_ASSERT_EIGEN_ARGS(DerivedP, DerivedOrigin, DerivedNormal); 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. +/// @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 < typename DerivedP, typename DerivedT0, typename DerivedT1, typename DerivedT2> inline auto point_plane_distance_hessian( - const DerivedP& p, - const DerivedT0& t0, - const DerivedT1& t1, - const DerivedT2& t2) + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& t0, + const Eigen::MatrixBase& t1, + const Eigen::MatrixBase& t2) { - IPC_ASSERT_EIGEN_ARGS(DerivedP, DerivedT0, DerivedT1, DerivedT2); using T = typename DerivedP::Scalar; return detail::point_plane_distance_hessian(p, t0, t1, t2); } diff --git a/src/ipc/distance/point_point.hpp b/src/ipc/distance/point_point.hpp index 2f39affaf..af6676327 100644 --- a/src/ipc/distance/point_point.hpp +++ b/src/ipc/distance/point_point.hpp @@ -73,9 +73,10 @@ namespace detail { /// @param p1 The second point. /// @return The distance between p0 and p1. template -inline auto point_point_distance(const DerivedP0& p0, const DerivedP1& p1) +inline auto point_point_distance( + const Eigen::MatrixBase& p0, + const Eigen::MatrixBase& p1) { - IPC_ASSERT_EIGEN_ARGS(DerivedP0, DerivedP1); using T = typename DerivedP0::Scalar; if constexpr (dim_v == 2) { @@ -97,10 +98,10 @@ inline auto point_point_distance(const DerivedP0& p0, const DerivedP1& p1) /// @param p1 The second point. /// @return The computed gradient. template -inline auto -point_point_distance_gradient(const DerivedP0& p0, const DerivedP1& p1) +inline auto point_point_distance_gradient( + const Eigen::MatrixBase& p0, + const Eigen::MatrixBase& p1) { - IPC_ASSERT_EIGEN_ARGS(DerivedP0, DerivedP1); using T = typename DerivedP0::Scalar; if constexpr (dim_v == 2) { return detail::point_point_distance_gradient(p0, p1); @@ -123,10 +124,10 @@ point_point_distance_gradient(const DerivedP0& p0, const DerivedP1& p1) /// @param p1 The second point. /// @return The computed hessian. template -inline auto -point_point_distance_hessian(const DerivedP0& p0, const DerivedP1& p1) +inline auto point_point_distance_hessian( + const Eigen::MatrixBase& p0, + const Eigen::MatrixBase& p1) { - IPC_ASSERT_EIGEN_ARGS(DerivedP0, DerivedP1); using T = typename DerivedP0::Scalar; if constexpr (dim_v == 2) { return detail::point_point_distance_hessian(p0, p1); diff --git a/src/ipc/distance/point_triangle.hpp b/src/ipc/distance/point_triangle.hpp index 79a6f26cc..1ae668e7e 100644 --- a/src/ipc/distance/point_triangle.hpp +++ b/src/ipc/distance/point_triangle.hpp @@ -20,7 +20,7 @@ namespace detail { Eigen::ConstRef> t0, Eigen::ConstRef> t1, Eigen::ConstRef> t2, - PointTriangleDistanceType dtype); + PointTriangleDistanceType dtype = PointTriangleDistanceType::AUTO); /// @brief Compute the gradient of the distance between a points and a triangle. /// @note The distance is actually squared distance. @@ -36,7 +36,7 @@ namespace detail { Eigen::ConstRef> t0, Eigen::ConstRef> t1, Eigen::ConstRef> t2, - PointTriangleDistanceType dtype); + PointTriangleDistanceType dtype = PointTriangleDistanceType::AUTO); /// @brief Compute the hessian of the distance between a points and a triangle. /// @note The distance is actually squared distance. @@ -52,58 +52,79 @@ namespace detail { Eigen::ConstRef> t0, Eigen::ConstRef> t1, Eigen::ConstRef> t2, - PointTriangleDistanceType dtype); + 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. +/// @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 < typename DerivedP, typename DerivedT0, typename DerivedT1, typename DerivedT2> inline auto point_triangle_distance( - const DerivedP& p, - const DerivedT0& t0, - const DerivedT1& t1, - const DerivedT2& t2, + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& t0, + const Eigen::MatrixBase& t1, + const Eigen::MatrixBase& t2, PointTriangleDistanceType dtype = PointTriangleDistanceType::AUTO) { - IPC_ASSERT_EIGEN_ARGS(DerivedP, DerivedT0, DerivedT1, DerivedT2); 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. +/// @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 < typename DerivedP, typename DerivedT0, typename DerivedT1, typename DerivedT2> inline auto point_triangle_distance_gradient( - const DerivedP& p, - const DerivedT0& t0, - const DerivedT1& t1, - const DerivedT2& t2, + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& t0, + const Eigen::MatrixBase& t1, + const Eigen::MatrixBase& t2, PointTriangleDistanceType dtype = PointTriangleDistanceType::AUTO) { - IPC_ASSERT_EIGEN_ARGS(DerivedP, DerivedT0, DerivedT1, DerivedT2); 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. +/// @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 < typename DerivedP, typename DerivedT0, typename DerivedT1, typename DerivedT2> inline auto point_triangle_distance_hessian( - const DerivedP& p, - const DerivedT0& t0, - const DerivedT1& t1, - const DerivedT2& t2, + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& t0, + const Eigen::MatrixBase& t1, + const Eigen::MatrixBase& t2, PointTriangleDistanceType dtype = PointTriangleDistanceType::AUTO) { - IPC_ASSERT_EIGEN_ARGS(DerivedP, DerivedT0, DerivedT1, DerivedT2); using T = typename DerivedP::Scalar; return detail::point_triangle_distance_hessian(p, t0, t1, t2, dtype); } diff --git a/src/ipc/distance/signed/line_line.hpp b/src/ipc/distance/signed/line_line.hpp index ad2aad538..3d26db337 100644 --- a/src/ipc/distance/signed/line_line.hpp +++ b/src/ipc/distance/signed/line_line.hpp @@ -97,52 +97,106 @@ namespace detail { 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). +/// 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 < typename DerivedEA0, typename DerivedEA1, typename DerivedEB0, typename DerivedEB1> inline auto line_line_signed_distance( - const DerivedEA0& ea0, - const DerivedEA1& ea1, - const DerivedEB0& eb0, - const DerivedEB1& eb1) + const Eigen::MatrixBase& ea0, + const Eigen::MatrixBase& ea1, + const Eigen::MatrixBase& eb0, + const Eigen::MatrixBase& eb1) { - IPC_ASSERT_EIGEN_ARGS(DerivedEA0, DerivedEA1, DerivedEB0, DerivedEB1); 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 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 < typename DerivedEA0, typename DerivedEA1, typename DerivedEB0, typename DerivedEB1> inline auto line_line_signed_distance_gradient( - const DerivedEA0& ea0, - const DerivedEA1& ea1, - const DerivedEB0& eb0, - const DerivedEB1& eb1) + const Eigen::MatrixBase& ea0, + const Eigen::MatrixBase& ea1, + const Eigen::MatrixBase& eb0, + const Eigen::MatrixBase& eb1) { - IPC_ASSERT_EIGEN_ARGS(DerivedEA0, DerivedEA1, DerivedEB0, DerivedEB1); 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 +/// [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 < typename DerivedEA0, typename DerivedEA1, typename DerivedEB0, typename DerivedEB1> inline auto line_line_signed_distance_hessian( - const DerivedEA0& ea0, - const DerivedEA1& ea1, - const DerivedEB0& eb0, - const DerivedEB1& eb1) + const Eigen::MatrixBase& ea0, + const Eigen::MatrixBase& ea1, + const Eigen::MatrixBase& eb0, + const Eigen::MatrixBase& eb1) { - IPC_ASSERT_EIGEN_ARGS(DerivedEA0, DerivedEA1, DerivedEB0, DerivedEB1); using T = typename DerivedEA0::Scalar; return detail::line_line_signed_distance_hessian(ea0, ea1, eb0, eb1); } diff --git a/src/ipc/distance/signed/point_line.cpp b/src/ipc/distance/signed/point_line.cpp index 0b870e97e..7ad051ca4 100644 --- a/src/ipc/distance/signed/point_line.cpp +++ b/src/ipc/distance/signed/point_line.cpp @@ -9,8 +9,10 @@ Eigen::Matrix point_line_signed_distance_hessian( 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::Vector2 v = p - e0; diff --git a/src/ipc/distance/signed/point_line.hpp b/src/ipc/distance/signed/point_line.hpp index a13c1d419..fc5556e7f 100644 --- a/src/ipc/distance/signed/point_line.hpp +++ b/src/ipc/distance/signed/point_line.hpp @@ -25,7 +25,7 @@ namespace detail { Eigen::ConstRef> e0, Eigen::ConstRef> e1) { - return point_line_normal(p, e0, e1).dot(p - e0); + return point_line_normal(p, e0, e1).dot(p - e0); } /// Compute the gradient of the signed point-to-line distance with respect @@ -46,9 +46,9 @@ namespace detail { Eigen::ConstRef> e0, Eigen::ConstRef> e1) { - const Eigen::Vector2 n = point_line_normal(p, e0, e1); + const Eigen::Vector2 n = point_line_normal(p, e0, e1); const Eigen::Matrix jac_n = - point_line_normal_jacobian(p, e0, e1); + 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; @@ -76,31 +76,73 @@ namespace detail { 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. +/// +/// @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 auto point_line_signed_distance( - const DerivedP& p, const DerivedE0& e0, const DerivedE1& e1) + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& e0, + const Eigen::MatrixBase& e1) { - IPC_ASSERT_EIGEN_ARGS(DerivedP, DerivedE0, DerivedE1); 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. +/// +/// 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 auto point_line_signed_distance_gradient( - const DerivedP& p, const DerivedE0& e0, const DerivedE1& e1) + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& e0, + const Eigen::MatrixBase& e1) { - IPC_ASSERT_EIGEN_ARGS(DerivedP, DerivedE0, DerivedE1); 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. +/// +/// 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 inline auto point_line_signed_distance_hessian( - const DerivedP& p, const DerivedE0& e0, const DerivedE1& e1) + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& e0, + const Eigen::MatrixBase& e1) { - IPC_ASSERT_EIGEN_ARGS(DerivedP, DerivedE0, DerivedE1); using T = typename DerivedP::Scalar; return detail::point_line_signed_distance_hessian(p, e0, e1); } diff --git a/src/ipc/distance/signed/point_plane.hpp b/src/ipc/distance/signed/point_plane.hpp index f505426e2..645646f18 100644 --- a/src/ipc/distance/signed/point_plane.hpp +++ b/src/ipc/distance/signed/point_plane.hpp @@ -77,52 +77,81 @@ namespace detail { Eigen::ConstRef> t2); } // 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 < typename DerivedP, typename DerivedT0, typename DerivedT1, typename DerivedT2> inline auto point_plane_signed_distance( - const DerivedP& p, - const DerivedT0& t0, - const DerivedT1& t1, - const DerivedT2& t2) + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& t0, + const Eigen::MatrixBase& t1, + const Eigen::MatrixBase& t2) { - IPC_ASSERT_EIGEN_ARGS(DerivedP, DerivedT0, DerivedT1, DerivedT2); 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 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 < typename DerivedP, typename DerivedT0, typename DerivedT1, typename DerivedT2> inline auto point_plane_signed_distance_gradient( - const DerivedP& p, - const DerivedT0& t0, - const DerivedT1& t1, - const DerivedT2& t2) + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& t0, + const Eigen::MatrixBase& t1, + const Eigen::MatrixBase& t2) { - IPC_ASSERT_EIGEN_ARGS(DerivedP, DerivedT0, DerivedT1, DerivedT2); 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 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 < typename DerivedP, typename DerivedT0, typename DerivedT1, typename DerivedT2> inline auto point_plane_signed_distance_hessian( - const DerivedP& p, - const DerivedT0& t0, - const DerivedT1& t1, - const DerivedT2& t2) + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& t0, + const Eigen::MatrixBase& t1, + const Eigen::MatrixBase& t2) { - IPC_ASSERT_EIGEN_ARGS(DerivedP, DerivedT0, DerivedT1, DerivedT2); using T = typename DerivedP::Scalar; return detail::point_plane_signed_distance_hessian(p, t0, t1, t2); } diff --git a/src/ipc/geometry/area.hpp b/src/ipc/geometry/area.hpp index 686a5dc3a..24fc22d35 100644 --- a/src/ipc/geometry/area.hpp +++ b/src/ipc/geometry/area.hpp @@ -97,9 +97,10 @@ namespace detail { /// @param e1 The second vertex of the edge. /// @return The length of the edge. template -inline auto edge_length(const DerivedE0& e0, const DerivedE1& e1) +inline auto edge_length( + const Eigen::MatrixBase& e0, + const Eigen::MatrixBase& e1) { - IPC_ASSERT_EIGEN_ARGS(DerivedE0, DerivedE1); using T = typename DerivedE0::Scalar; if constexpr (dim_v == 2) { @@ -119,9 +120,10 @@ inline auto edge_length(const DerivedE0& e0, const DerivedE1& e1) /// @param e1 The second vertex of the edge. /// @return The gradient of the edge's length wrt e0, and e1. template -inline auto edge_length_gradient(const DerivedE0& e0, const DerivedE1& e1) +inline auto edge_length_gradient( + const Eigen::MatrixBase& e0, + const Eigen::MatrixBase& e1) { - IPC_ASSERT_EIGEN_ARGS(DerivedE0, DerivedE1); using T = typename DerivedE0::Scalar; if constexpr (dim_v == 2) { @@ -145,10 +147,11 @@ inline auto edge_length_gradient(const DerivedE0& e0, const DerivedE1& e1) /// @param t2 The third vertex of the triangle. /// @return The area of the triangle. template -inline auto -triangle_area(const DerivedT0& t0, const DerivedT1& t1, const DerivedT2& t2) +inline auto triangle_area( + const Eigen::MatrixBase& t0, + const Eigen::MatrixBase& t1, + const Eigen::MatrixBase& t2) { - IPC_ASSERT_EIGEN_ARGS(DerivedT0, DerivedT1, DerivedT2); // NOTE: explicit : the detail overload cannot deduce T from an // arbitrary Eigen expression. using T = typename DerivedT0::Scalar; @@ -165,9 +168,10 @@ triangle_area(const DerivedT0& t0, const DerivedT1& t1, const DerivedT2& t2) /// @return The gradient of the triangle's area t0, t1, and t2. template inline auto triangle_area_gradient( - const DerivedT0& t0, const DerivedT1& t1, const DerivedT2& t2) + const Eigen::MatrixBase& t0, + const Eigen::MatrixBase& t1, + const Eigen::MatrixBase& t2) { - IPC_ASSERT_EIGEN_ARGS(DerivedT0, DerivedT1, DerivedT2); using T = typename DerivedT0::Scalar; return detail::triangle_area_gradient(t0, t1, t2); } diff --git a/src/ipc/geometry/normal.cpp b/src/ipc/geometry/normal.cpp index e31ccef6f..3c669194c 100644 --- a/src/ipc/geometry/normal.cpp +++ b/src/ipc/geometry/normal.cpp @@ -4,7 +4,7 @@ #include -namespace ipc { +namespace ipc::detail { // --- point-line normal functions ------------------------------------------- @@ -400,4 +400,4 @@ template Eigen::Matrix line_line_normal_hessian(Eigen::Con template Eigen::Matrix line_line_normal_hessian(Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef, Eigen::ConstRef); // clang-format on -} // namespace ipc +} // namespace ipc::detail diff --git a/src/ipc/geometry/normal.hpp b/src/ipc/geometry/normal.hpp index 5daecb98e..df4da5755 100644 --- a/src/ipc/geometry/normal.hpp +++ b/src/ipc/geometry/normal.hpp @@ -71,9 +71,8 @@ namespace detail { /// @param x The input vector. /// @return A tuple containing the normalized vector and its Jacobian. template -inline auto normalization_and_jacobian(const DerivedX& x) +inline auto normalization_and_jacobian(const Eigen::MatrixBase& x) { - IPC_ASSERT_EIGEN_ARGS(DerivedX); using T = typename DerivedX::Scalar; if constexpr (dim_v == 2) { @@ -94,9 +93,8 @@ inline auto normalization_and_jacobian(const DerivedX& x) /// @param x The input vector. /// @return The Jacobian of the normalization operation. template -inline auto normalization_jacobian(const DerivedX& x) +inline auto normalization_jacobian(const Eigen::MatrixBase& x) { - IPC_ASSERT_EIGEN_ARGS(DerivedX); using T = typename DerivedX::Scalar; if constexpr (dim_v == 2) { @@ -117,9 +115,9 @@ inline auto normalization_jacobian(const DerivedX& 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 DerivedX& x) +inline auto +normalization_and_jacobian_and_hessian(const Eigen::MatrixBase& x) { - IPC_ASSERT_EIGEN_ARGS(DerivedX); using T = typename DerivedX::Scalar; using Ret = std::tuple, MatrixMax3, std::array, 3>>; @@ -156,499 +154,541 @@ inline auto normalization_and_jacobian_and_hessian(const DerivedX& x) /// @param x The input vector. /// @return The Hessian of the normalization operation. template -inline auto normalization_hessian(const DerivedX& x) +inline auto normalization_hessian(const Eigen::MatrixBase& x) { return std::get<2>(normalization_and_jacobian_and_hessian(x)); } -// ============================================================================= - -/// @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. - * @{ - */ +namespace detail { -/// @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 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 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 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 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 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); + /// @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; + } -/** - * \defgroup geometry Triangle normal - * \brief Functions for computing a triangle's normal and resp. Jacobians. - * @{ - */ + /// @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 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 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 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 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 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 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 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 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 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 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); -/// @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); +} // namespace detail /** @} */ -// ============================================================================= - -/** - * \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; -} +// --- EigenExpression wrappers --- -/// @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. +/// @brief Computes the Jacobian of the cross product matrix. +/// @return The Jacobian of the cross product matrix. template -inline Eigen::Matrix line_line_normal_jacobian( - Eigen::ConstRef> ea0, - Eigen::ConstRef> ea1, - Eigen::ConstRef> eb0, - Eigen::ConstRef> eb1) +inline Eigen::Matrix cross_product_matrix_jacobian() { - // ∂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); + return detail::cross_product_matrix_jacobian(); } -/// @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); - -/** @} */ - -// --- EigenExpression wrappers --- - -template < - typename DerivedX, - std::enable_if_t, int> = 0> +/// @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 auto cross_product_matrix(const Eigen::MatrixBase& v) - -> Eigen::Matrix { using T = typename DerivedX::Scalar; - return cross_product_matrix(Eigen::Ref>(v)); + return detail::cross_product_matrix(v); } +/// @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 inline auto point_line_unnormalized_normal( const Eigen::MatrixBase& p, const Eigen::MatrixBase& e0, const Eigen::MatrixBase& e1) - -> VectorMax3 { using T = typename DerivedP::Scalar; - return point_line_unnormalized_normal( - Eigen::Ref>(p), Eigen::Ref>(e0), - Eigen::Ref>(e1)); + 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. template inline auto point_line_normal( const Eigen::MatrixBase& p, const Eigen::MatrixBase& e0, const Eigen::MatrixBase& e1) - -> VectorMax3 { using T = typename DerivedP::Scalar; - return point_line_normal( - Eigen::Ref>(p), Eigen::Ref>(e0), - Eigen::Ref>(e1)); + return detail::point_line_normal(p, e0, e1); } +/// @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 inline auto point_line_unnormalized_normal_jacobian( const Eigen::MatrixBase& p, const Eigen::MatrixBase& e0, const Eigen::MatrixBase& e1) - -> MatrixMax { using T = typename DerivedP::Scalar; - return point_line_unnormalized_normal_jacobian( - Eigen::Ref>(p), Eigen::Ref>(e0), - Eigen::Ref>(e1)); + return detail::point_line_unnormalized_normal_jacobian(p, e0, 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 inline auto point_line_unnormalized_normal_hessian( const Eigen::MatrixBase& p, const Eigen::MatrixBase& e0, const Eigen::MatrixBase& e1) - -> MatrixMax { using T = typename DerivedP::Scalar; - return point_line_unnormalized_normal_hessian( - Eigen::Ref>(p), Eigen::Ref>(e0), - Eigen::Ref>(e1)); + 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. template inline auto point_line_normal_jacobian( const Eigen::MatrixBase& p, const Eigen::MatrixBase& e0, const Eigen::MatrixBase& e1) - -> MatrixMax { using T = typename DerivedP::Scalar; - return point_line_normal_jacobian( - Eigen::Ref>(p), Eigen::Ref>(e0), - Eigen::Ref>(e1)); + return detail::point_line_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 inline auto point_line_normal_hessian( const Eigen::MatrixBase& p, const Eigen::MatrixBase& e0, const Eigen::MatrixBase& e1) - -> MatrixMax { using T = typename DerivedP::Scalar; - return point_line_normal_hessian( - Eigen::Ref>(p), Eigen::Ref>(e0), - Eigen::Ref>(e1)); + 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. template inline auto triangle_unnormalized_normal( const Eigen::MatrixBase& a, const Eigen::MatrixBase& b, const Eigen::MatrixBase& c) - -> Eigen::Vector3 { using T = typename DerivedA::Scalar; - return triangle_unnormalized_normal( - Eigen::Ref>(a), - Eigen::Ref>(b), - Eigen::Ref>(c)); + return detail::triangle_unnormalized_normal(a, b, c); } +/// @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 auto triangle_normal( const Eigen::MatrixBase& a, const Eigen::MatrixBase& b, const Eigen::MatrixBase& c) - -> Eigen::Vector3 { using T = typename DerivedA::Scalar; - return triangle_normal( - Eigen::Ref>(a), - Eigen::Ref>(b), - Eigen::Ref>(c)); + return detail::triangle_normal(a, b, c); } +/// @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 auto triangle_unnormalized_normal_jacobian( const Eigen::MatrixBase& a, const Eigen::MatrixBase& b, const Eigen::MatrixBase& c) - -> Eigen::Matrix { using T = typename DerivedA::Scalar; - return triangle_unnormalized_normal_jacobian( - Eigen::Ref>(a), - Eigen::Ref>(b), - Eigen::Ref>(c)); + return detail::triangle_unnormalized_normal_jacobian(a, b, c); } +/// @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 inline auto triangle_unnormalized_normal_hessian( const Eigen::MatrixBase& a, const Eigen::MatrixBase& b, const Eigen::MatrixBase& c) - -> Eigen::Matrix { using T = typename DerivedA::Scalar; - return triangle_unnormalized_normal_hessian( - Eigen::Ref>(a), - Eigen::Ref>(b), - Eigen::Ref>(c)); + 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. template inline auto triangle_normal_jacobian( const Eigen::MatrixBase& a, const Eigen::MatrixBase& b, const Eigen::MatrixBase& c) - -> Eigen::Matrix { using T = typename DerivedA::Scalar; - return triangle_normal_jacobian( - Eigen::Ref>(a), - Eigen::Ref>(b), - Eigen::Ref>(c)); + return detail::triangle_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 inline auto triangle_normal_hessian( const Eigen::MatrixBase& a, const Eigen::MatrixBase& b, const Eigen::MatrixBase& c) - -> Eigen::Matrix { using T = typename DerivedA::Scalar; - return triangle_normal_hessian( - Eigen::Ref>(a), - Eigen::Ref>(b), - Eigen::Ref>(c)); + 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. +/// @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 < typename DerivedEA0, typename DerivedEA1, @@ -659,16 +699,17 @@ inline auto line_line_unnormalized_normal( const Eigen::MatrixBase& ea1, const Eigen::MatrixBase& eb0, const Eigen::MatrixBase& eb1) - -> Eigen::Vector3 { using T = typename DerivedEA0::Scalar; - return line_line_unnormalized_normal( - Eigen::Ref>(ea0), - Eigen::Ref>(ea1), - Eigen::Ref>(eb0), - Eigen::Ref>(eb1)); + return detail::line_line_unnormalized_normal(ea0, ea1, eb0, eb1); } +/// @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 < typename DerivedEA0, typename DerivedEA1, @@ -679,16 +720,18 @@ inline auto line_line_normal( const Eigen::MatrixBase& ea1, const Eigen::MatrixBase& eb0, const Eigen::MatrixBase& eb1) - -> Eigen::Vector3 { using T = typename DerivedEA0::Scalar; - return line_line_normal( - Eigen::Ref>(ea0), - Eigen::Ref>(ea1), - Eigen::Ref>(eb0), - Eigen::Ref>(eb1)); + return detail::line_line_normal(ea0, ea1, eb0, eb1); } +/// @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 < typename DerivedEA0, typename DerivedEA1, @@ -699,16 +742,18 @@ inline auto line_line_unnormalized_normal_jacobian( const Eigen::MatrixBase& ea1, const Eigen::MatrixBase& eb0, const Eigen::MatrixBase& eb1) - -> Eigen::Matrix { using T = typename DerivedEA0::Scalar; - return line_line_unnormalized_normal_jacobian( - Eigen::Ref>(ea0), - Eigen::Ref>(ea1), - Eigen::Ref>(eb0), - Eigen::Ref>(eb1)); + return detail::line_line_unnormalized_normal_jacobian( + ea0, ea1, eb0, eb1); } +/// @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 < typename DerivedEA0, typename DerivedEA1, @@ -719,16 +764,18 @@ inline auto line_line_normal_jacobian( const Eigen::MatrixBase& ea1, const Eigen::MatrixBase& eb0, const Eigen::MatrixBase& eb1) - -> Eigen::Matrix { using T = typename DerivedEA0::Scalar; - return line_line_normal_jacobian( - Eigen::Ref>(ea0), - Eigen::Ref>(ea1), - Eigen::Ref>(eb0), - Eigen::Ref>(eb1)); + return detail::line_line_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 < typename DerivedEA0, typename DerivedEA1, @@ -739,16 +786,17 @@ inline auto line_line_unnormalized_normal_hessian( const Eigen::MatrixBase& ea1, const Eigen::MatrixBase& eb0, const Eigen::MatrixBase& eb1) - -> Eigen::Matrix { using T = typename DerivedEA0::Scalar; - return line_line_unnormalized_normal_hessian( - Eigen::Ref>(ea0), - Eigen::Ref>(ea1), - Eigen::Ref>(eb0), - Eigen::Ref>(eb1)); + 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. +/// @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 < typename DerivedEA0, typename DerivedEA1, @@ -759,14 +807,9 @@ inline auto line_line_normal_hessian( const Eigen::MatrixBase& ea1, const Eigen::MatrixBase& eb0, const Eigen::MatrixBase& eb1) - -> Eigen::Matrix { using T = typename DerivedEA0::Scalar; - return line_line_normal_hessian( - Eigen::Ref>(ea0), - Eigen::Ref>(ea1), - Eigen::Ref>(eb0), - Eigen::Ref>(eb1)); + return detail::line_line_normal_hessian(ea0, ea1, eb0, eb1); } } // namespace ipc diff --git a/src/ipc/tangent/closest_point.hpp b/src/ipc/tangent/closest_point.hpp index f9a14ed10..2fa7570d6 100644 --- a/src/ipc/tangent/closest_point.hpp +++ b/src/ipc/tangent/closest_point.hpp @@ -386,9 +386,10 @@ namespace detail { /// @return barycentric coordinates of the closest point template inline auto point_edge_closest_point( - const DerivedP& p, const DerivedE0& e0, const DerivedE1& e1) + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& e0, + const Eigen::MatrixBase& e1) { - IPC_ASSERT_EIGEN_ARGS(DerivedP, DerivedE0, DerivedE1); using T = typename DerivedP::Scalar; if constexpr (dim_v == 2) { @@ -410,9 +411,10 @@ inline auto point_edge_closest_point( /// @return Jacobian of the closest point template inline auto point_edge_closest_point_jacobian( - const DerivedP& p, const DerivedE0& e0, const DerivedE1& e1) + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& e0, + const Eigen::MatrixBase& e1) { - IPC_ASSERT_EIGEN_ARGS(DerivedP, DerivedE0, DerivedE1); using T = typename DerivedP::Scalar; if constexpr (dim_v == 2) { @@ -436,9 +438,10 @@ inline auto point_edge_closest_point_jacobian( /// @return Hessian of the closest point template inline auto point_edge_closest_point_hessian( - const DerivedP& p, const DerivedE0& e0, const DerivedE1& e1) + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& e0, + const Eigen::MatrixBase& e1) { - IPC_ASSERT_EIGEN_ARGS(DerivedP, DerivedE0, DerivedE1); using T = typename DerivedP::Scalar; if constexpr (dim_v == 2) { @@ -478,12 +481,11 @@ template < typename DerivedEB0, typename DerivedEB1> inline auto edge_edge_closest_point( - const DerivedEA0& ea0, - const DerivedEA1& ea1, - const DerivedEB0& eb0, - const DerivedEB1& eb1) + const Eigen::MatrixBase& ea0, + const Eigen::MatrixBase& ea1, + const Eigen::MatrixBase& eb0, + const Eigen::MatrixBase& eb1) { - IPC_ASSERT_EIGEN_ARGS(DerivedEA0, DerivedEA1, DerivedEB0, DerivedEB1); using T = typename DerivedEA0::Scalar; return detail::edge_edge_closest_point(ea0, ea1, eb0, eb1); } @@ -503,12 +505,11 @@ template < typename DerivedEB0, typename DerivedEB1> inline auto edge_edge_closest_point_jacobian( - const DerivedEA0& ea0, - const DerivedEA1& ea1, - const DerivedEB0& eb0, - const DerivedEB1& eb1) + const Eigen::MatrixBase& ea0, + const Eigen::MatrixBase& ea1, + const Eigen::MatrixBase& eb0, + const Eigen::MatrixBase& eb1) { - IPC_ASSERT_EIGEN_ARGS(DerivedEA0, DerivedEA1, DerivedEB0, DerivedEB1); using T = typename DerivedEA0::Scalar; return detail::edge_edge_closest_point_jacobian(ea0, ea1, eb0, eb1); } @@ -528,12 +529,11 @@ template < typename DerivedEB0, typename DerivedEB1> inline auto edge_edge_closest_point_hessian( - const DerivedEA0& ea0, - const DerivedEA1& ea1, - const DerivedEB0& eb0, - const DerivedEB1& eb1) + const Eigen::MatrixBase& ea0, + const Eigen::MatrixBase& ea1, + const Eigen::MatrixBase& eb0, + const Eigen::MatrixBase& eb1) { - IPC_ASSERT_EIGEN_ARGS(DerivedEA0, DerivedEA1, DerivedEB0, DerivedEB1); using T = typename DerivedEA0::Scalar; return detail::edge_edge_closest_point_hessian(ea0, ea1, eb0, eb1); } @@ -556,12 +556,11 @@ template < typename DerivedT1, typename DerivedT2> inline auto point_triangle_closest_point( - const DerivedP& p, - const DerivedT0& t0, - const DerivedT1& t1, - const DerivedT2& t2) + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& t0, + const Eigen::MatrixBase& t1, + const Eigen::MatrixBase& t2) { - IPC_ASSERT_EIGEN_ARGS(DerivedP, DerivedT0, DerivedT1, DerivedT2); using T = typename DerivedP::Scalar; return detail::point_triangle_closest_point(p, t0, t1, t2); } @@ -581,12 +580,11 @@ template < typename DerivedT1, typename DerivedT2> inline auto point_triangle_closest_point_jacobian( - const DerivedP& p, - const DerivedT0& t0, - const DerivedT1& t1, - const DerivedT2& t2) + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& t0, + const Eigen::MatrixBase& t1, + const Eigen::MatrixBase& t2) { - IPC_ASSERT_EIGEN_ARGS(DerivedP, DerivedT0, DerivedT1, DerivedT2); using T = typename DerivedP::Scalar; return detail::point_triangle_closest_point_jacobian(p, t0, t1, t2); } @@ -606,12 +604,11 @@ template < typename DerivedT1, typename DerivedT2> inline auto point_triangle_closest_point_hessian( - const DerivedP& p, - const DerivedT0& t0, - const DerivedT1& t1, - const DerivedT2& t2) + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& t0, + const Eigen::MatrixBase& t1, + const Eigen::MatrixBase& t2) { - IPC_ASSERT_EIGEN_ARGS(DerivedP, DerivedT0, DerivedT1, DerivedT2); using T = typename DerivedP::Scalar; return detail::point_triangle_closest_point_hessian(p, t0, t1, t2); } diff --git a/src/ipc/tangent/relative_velocity.hpp b/src/ipc/tangent/relative_velocity.hpp index 7de0adfec..b540ea852 100644 --- a/src/ipc/tangent/relative_velocity.hpp +++ b/src/ipc/tangent/relative_velocity.hpp @@ -256,10 +256,10 @@ namespace detail { /// @param dp1 Velocity of the second point /// @return The relative velocity of the two points template -inline auto -point_point_relative_velocity(const DerivedDP0& dp0, const DerivedDP1& dp1) +inline auto point_point_relative_velocity( + const Eigen::MatrixBase& dp0, + const Eigen::MatrixBase& dp1) { - IPC_ASSERT_EIGEN_ARGS(DerivedDP0, DerivedDP1); using T = typename DerivedDP0::Scalar; assert(dp0.size() == dp1.size()); @@ -322,12 +322,11 @@ inline VectorMax point_point_relative_velocity_dx_dbeta(const int dim) /// @return The relative velocity of the point and the edge template inline auto point_edge_relative_velocity( - const DerivedDP& dp, - const DerivedDE0& de0, - const DerivedDE1& de1, + const Eigen::MatrixBase& dp, + const Eigen::MatrixBase& de0, + const Eigen::MatrixBase& de1, const typename DerivedDP::Scalar alpha) { - IPC_ASSERT_EIGEN_ARGS(DerivedDP, DerivedDE0, DerivedDE1); using T = typename DerivedDP::Scalar; assert(dp.size() == de0.size() && dp.size() == de1.size()); @@ -406,14 +405,12 @@ template < typename DerivedDEB1, typename DerivedCoords> inline auto edge_edge_relative_velocity( - const DerivedDEA0& dea0, - const DerivedDEA1& dea1, - const DerivedDEB0& deb0, - const DerivedDEB1& deb1, - const DerivedCoords& coords) + const Eigen::MatrixBase& dea0, + const Eigen::MatrixBase& dea1, + const Eigen::MatrixBase& deb0, + const Eigen::MatrixBase& deb1, + const Eigen::MatrixBase& coords) { - IPC_ASSERT_EIGEN_ARGS( - DerivedDEA0, DerivedDEA1, DerivedDEB0, DerivedDEB1, DerivedCoords); using T = typename DerivedDEA0::Scalar; // NOTE: explicit : the detail overload cannot deduce T from an // arbitrary Eigen expression. @@ -425,9 +422,9 @@ inline auto edge_edge_relative_velocity( /// @param coords Two parametric coordinates of the closest points on the edges /// @return The relative velocity Jacobian du/dx template -inline auto edge_edge_relative_velocity_jacobian(const DerivedCoords& coords) +inline auto edge_edge_relative_velocity_jacobian( + const Eigen::MatrixBase& coords) { - IPC_ASSERT_EIGEN_ARGS(DerivedCoords); using T = typename DerivedCoords::Scalar; return detail::edge_edge_relative_velocity_jacobian(coords); } @@ -444,9 +441,9 @@ inline auto edge_edge_relative_velocity_jacobian(const DerivedCoords& coords) /// @param coords Two parametric coordinates of the closest points on the edges /// @return The vectorized tensor of d²u/dxdβ template -inline auto edge_edge_relative_velocity_dx_dbeta(const DerivedCoords& coords) +inline auto edge_edge_relative_velocity_dx_dbeta( + const Eigen::MatrixBase& coords) { - IPC_ASSERT_EIGEN_ARGS(DerivedCoords); using T = typename DerivedCoords::Scalar; return detail::edge_edge_relative_velocity_dx_dbeta(coords); } @@ -468,14 +465,12 @@ template < typename DerivedDT2, typename DerivedCoords> inline auto point_triangle_relative_velocity( - const DerivedDP& dp, - const DerivedDT0& dt0, - const DerivedDT1& dt1, - const DerivedDT2& dt2, - const DerivedCoords& coords) + const Eigen::MatrixBase& dp, + const Eigen::MatrixBase& dt0, + const Eigen::MatrixBase& dt1, + const Eigen::MatrixBase& dt2, + const Eigen::MatrixBase& coords) { - IPC_ASSERT_EIGEN_ARGS( - DerivedDP, DerivedDT0, DerivedDT1, DerivedDT2, DerivedCoords); using T = typename DerivedDP::Scalar; return detail::point_triangle_relative_velocity( dp, dt0, dt1, dt2, coords); @@ -485,10 +480,9 @@ inline auto point_triangle_relative_velocity( /// @param coords Barycentric coordinates of the closest point on the triangle /// @return The relative velocity Jacobian du/dx template -inline auto -point_triangle_relative_velocity_jacobian(const DerivedCoords& coords) +inline auto point_triangle_relative_velocity_jacobian( + const Eigen::MatrixBase& coords) { - IPC_ASSERT_EIGEN_ARGS(DerivedCoords); using T = typename DerivedCoords::Scalar; return detail::point_triangle_relative_velocity_jacobian(coords); } @@ -505,10 +499,9 @@ point_triangle_relative_velocity_jacobian(const DerivedCoords& coords) /// @param coords Baricentric coordinates of the closest point on the triangle /// @return The vectorized tensor of d²u/dxdβ template -inline auto -point_triangle_relative_velocity_dx_dbeta(const DerivedCoords& coords) +inline auto point_triangle_relative_velocity_dx_dbeta( + const Eigen::MatrixBase& coords) { - IPC_ASSERT_EIGEN_ARGS(DerivedCoords); using T = typename DerivedCoords::Scalar; return detail::point_triangle_relative_velocity_dx_dbeta(coords); } diff --git a/src/ipc/tangent/tangent_basis.hpp b/src/ipc/tangent/tangent_basis.hpp index eda550832..8454bb4f4 100644 --- a/src/ipc/tangent/tangent_basis.hpp +++ b/src/ipc/tangent/tangent_basis.hpp @@ -204,9 +204,10 @@ namespace detail { /// @param p1 Second point /// @return A 3x2 matrix whose columns are the basis vectors. template -inline auto point_point_tangent_basis(const DerivedP0& p0, const DerivedP1& p1) +inline auto point_point_tangent_basis( + const Eigen::MatrixBase& p0, + const Eigen::MatrixBase& p1) { - IPC_ASSERT_EIGEN_ARGS(DerivedP0, DerivedP1); using T = typename DerivedP0::Scalar; assert(p0.size() == p1.size()); @@ -229,10 +230,10 @@ inline auto point_point_tangent_basis(const DerivedP0& p0, const DerivedP1& p1) /// @param p1 Second point /// @return A (3*2)x6 matrix whose columns are the basis vectors. template -inline auto -point_point_tangent_basis_jacobian(const DerivedP0& p0, const DerivedP1& p1) +inline auto point_point_tangent_basis_jacobian( + const Eigen::MatrixBase& p0, + const Eigen::MatrixBase& p1) { - IPC_ASSERT_EIGEN_ARGS(DerivedP0, DerivedP1); using T = typename DerivedP0::Scalar; assert(p0.size() == p1.size()); @@ -260,9 +261,10 @@ point_point_tangent_basis_jacobian(const DerivedP0& p0, const DerivedP1& p1) /// @return A 3x2 matrix whose columns are the basis vectors. template inline auto point_edge_tangent_basis( - const DerivedP& p, const DerivedE0& e0, const DerivedE1& e1) + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& e0, + const Eigen::MatrixBase& e1) { - IPC_ASSERT_EIGEN_ARGS(DerivedP, DerivedE0, DerivedE1); using T = typename DerivedP::Scalar; assert(p.size() == e0.size() && p.size() == e1.size()); @@ -287,9 +289,10 @@ inline auto point_edge_tangent_basis( /// @return A (3*2)x9 matrix whose columns are the basis vectors. template inline auto point_edge_tangent_basis_jacobian( - const DerivedP& p, const DerivedE0& e0, const DerivedE1& e1) + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& e0, + const Eigen::MatrixBase& e1) { - IPC_ASSERT_EIGEN_ARGS(DerivedP, DerivedE0, DerivedE1); using T = typename DerivedP::Scalar; assert(p.size() == e0.size() && p.size() == e1.size()); @@ -322,12 +325,11 @@ template < typename DerivedEB0, typename DerivedEB1> inline auto edge_edge_tangent_basis( - const DerivedEA0& ea0, - const DerivedEA1& ea1, - const DerivedEB0& eb0, - const DerivedEB1& eb1) + const Eigen::MatrixBase& ea0, + const Eigen::MatrixBase& ea1, + const Eigen::MatrixBase& eb0, + const Eigen::MatrixBase& eb1) { - IPC_ASSERT_EIGEN_ARGS(DerivedEA0, DerivedEA1, DerivedEB0, DerivedEB1); using T = typename DerivedEA0::Scalar; // NOTE: explicit : the detail overload cannot deduce T from an // arbitrary Eigen expression. @@ -346,12 +348,11 @@ template < typename DerivedEB0, typename DerivedEB1> inline auto edge_edge_tangent_basis_jacobian( - const DerivedEA0& ea0, - const DerivedEA1& ea1, - const DerivedEB0& eb0, - const DerivedEB1& eb1) + const Eigen::MatrixBase& ea0, + const Eigen::MatrixBase& ea1, + const Eigen::MatrixBase& eb0, + const Eigen::MatrixBase& eb1) { - IPC_ASSERT_EIGEN_ARGS(DerivedEA0, DerivedEA1, DerivedEB0, DerivedEB1); using T = typename DerivedEA0::Scalar; return detail::edge_edge_tangent_basis_jacobian(ea0, ea1, eb0, eb1); } @@ -379,12 +380,11 @@ template < typename DerivedT1, typename DerivedT2> inline auto point_triangle_tangent_basis( - const DerivedP& p, - const DerivedT0& t0, - const DerivedT1& t1, - const DerivedT2& t2) + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& t0, + const Eigen::MatrixBase& t1, + const Eigen::MatrixBase& t2) { - IPC_ASSERT_EIGEN_ARGS(DerivedP, DerivedT0, DerivedT1, DerivedT2); using T = typename DerivedP::Scalar; return detail::point_triangle_tangent_basis(p, t0, t1, t2); } @@ -401,12 +401,11 @@ template < typename DerivedT1, typename DerivedT2> inline auto point_triangle_tangent_basis_jacobian( - const DerivedP& p, - const DerivedT0& t0, - const DerivedT1& t1, - const DerivedT2& t2) + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& t0, + const Eigen::MatrixBase& t1, + const Eigen::MatrixBase& t2) { - IPC_ASSERT_EIGEN_ARGS(DerivedP, DerivedT0, DerivedT1, DerivedT2); using T = typename DerivedP::Scalar; return detail::point_triangle_tangent_basis_jacobian(p, t0, t1, t2); } diff --git a/src/ipc/utils/eigen_ext.hpp b/src/ipc/utils/eigen_ext.hpp index 71195d3bc..5f6a194b6 100644 --- a/src/ipc/utils/eigen_ext.hpp +++ b/src/ipc/utils/eigen_ext.hpp @@ -223,39 +223,6 @@ using HessianType = std:: /**@}*/ -/// @brief True iff `T` is an Eigen expression (a stored matrix, block, map, or -/// lazy expression) rather than a scalar. -/// -/// A pure detection trait: it inspects only members of `T` itself, so it never -/// instantiates `Eigen::MatrixBase` — which is ill-formed when `T` is a -/// scalar such as `double` or an autodiff type. -template -inline constexpr bool is_eigen_expression_v = false; - -template -inline constexpr bool is_eigen_expression_v< - T, - std::void_t< - typename T::Scalar, - decltype(std::declval().rows()), - decltype(std::declval().cols())>> = true; - -/// @brief True iff every `Ts` is an Eigen expression. -template -inline constexpr bool are_eigen_expressions_v = - (is_eigen_expression_v && ...); - -/// @brief Assert that every deduced argument type is an Eigen expression. -/// -/// Place this as the first statement of an Eigen front end so that an explicit -/// scalar template argument produces one readable diagnostic instead of a wall -/// of Eigen internals. -#define IPC_ASSERT_EIGEN_ARGS(...) \ - static_assert( \ - ::ipc::are_eigen_expressions_v<__VA_ARGS__>, \ - "Pass Eigen vectors or expressions; the scalar type is deduced from " \ - "the arguments. Write f(a, b, ...), not f(a, b, ...).") - /// @brief Compile-time dimension of an Eigen vector expression, or /// `Eigen::Dynamic` if it is not known until run time. /// diff --git a/tests/src/tests/benchmark_eigen.cpp b/tests/src/tests/benchmark_eigen.cpp index a2398f7e8..2a18c0a12 100644 --- a/tests/src/tests/benchmark_eigen.cpp +++ b/tests/src/tests/benchmark_eigen.cpp @@ -131,8 +131,10 @@ pp_ref_maxN(Eigen::ConstRef p0, Eigen::ConstRef p1) /// @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 DerivedP& p, const DerivedE0& e0, const DerivedE1& e1) +inline double pl_body( + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& e0, + const Eigen::MatrixBase& e1) { if constexpr (N == 2) { const Eigen::Vector2d e = e1 - e0; @@ -171,8 +173,10 @@ double pl_ref_3d( /// @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 DerivedP& p, const DerivedE0& e0, const DerivedE1& e1) +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(); @@ -191,9 +195,9 @@ pe_type_body(const DerivedP& p, const DerivedE0& e0, const DerivedE1& e1) /// @brief The shared fixed-size body, as pl_body but with the dtype dispatch. template inline double pe_body( - const DerivedP& p, - const DerivedE0& e0, - const DerivedE1& e1, + const Eigen::MatrixBase& p, + const Eigen::MatrixBase& e0, + const Eigen::MatrixBase& e1, PointEdgeDistanceType dtype) { if (dtype == PointEdgeDistanceType::AUTO) { From 17aa8e860adccbf8a91dc35893ec62345257f0cd Mon Sep 17 00:00:00 2001 From: Zachary Ferguson Date: Wed, 2 Sep 2026 15:39:07 -0400 Subject: [PATCH 21/21] Fix documentation - Explicitly specify the global templated version of distance functions --- docs/source/cpp-api/barrier/Barrier.rst | 2 +- docs/source/cpp-api/barrier/index.rst | 6 +-- docs/source/cpp-api/distance.rst | 66 ++++++++++++------------- src/ipc/barrier/barrier.cpp | 2 + src/ipc/distance/signed/line_line.hpp | 23 +++++---- 5 files changed, 50 insertions(+), 49 deletions(-) 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/src/ipc/barrier/barrier.cpp b/src/ipc/barrier/barrier.cpp index 7f0c4ff47..742f1712f 100644 --- a/src/ipc/barrier/barrier.cpp +++ b/src/ipc/barrier/barrier.cpp @@ -162,6 +162,7 @@ T TwoStageBarrier::second_derivative(const T d, const T dhat) const // ============================================================================ // Explicit template instantiations +/// @cond DOXYGEN_SKIP template class BarrierBase; template class BarrierBase; template class ClampedLogBarrier; @@ -178,6 +179,7 @@ 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/distance/signed/line_line.hpp b/src/ipc/distance/signed/line_line.hpp index 3d26db337..34fa41a86 100644 --- a/src/ipc/distance/signed/line_line.hpp +++ b/src/ipc/distance/signed/line_line.hpp @@ -133,15 +133,14 @@ inline auto line_line_signed_distance( return detail::line_line_signed_distance(ea0, ea1, eb0, eb1); } -/// Compute the gradient of the signed line-line distance with respect to -/// the four 3D input points: ea0, ea1, eb0, eb1. +/// 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. +/// 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. @@ -170,13 +169,13 @@ inline auto line_line_signed_distance_gradient( /// 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 either input line is degenerate (coincident points) or if -/// the lines are parallel (no unique perpendicular direction). +/// 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.