diff --git a/src/ipc/CMakeLists.txt b/src/ipc/CMakeLists.txt index 95f28eb48..75397d903 100644 --- a/src/ipc/CMakeLists.txt +++ b/src/ipc/CMakeLists.txt @@ -19,6 +19,7 @@ add_subdirectory(ccd) add_subdirectory(collisions) add_subdirectory(distance) add_subdirectory(friction) +add_subdirectory(geometry) add_subdirectory(implicits) add_subdirectory(potentials) add_subdirectory(smooth_contact) diff --git a/src/ipc/candidates/collision_stencil.cpp b/src/ipc/candidates/collision_stencil.cpp index 53582d7fd..ff2cceef9 100644 --- a/src/ipc/candidates/collision_stencil.cpp +++ b/src/ipc/candidates/collision_stencil.cpp @@ -1,5 +1,7 @@ #include "collision_stencil.hpp" +#include + namespace ipc { VectorMax3d CollisionStencil::compute_normal( @@ -30,19 +32,10 @@ MatrixMax CollisionStencil::compute_normal_jacobian( { const int dim = this->dim(positions.size()); - VectorMax3d n = compute_unnormalized_normal(positions); - - MatrixMax dn = - compute_unnormalized_normal_jacobian(positions); - - // Derivative of normalization (n̂ = n / ‖n‖) - const double n_norm = n.norm(); - n /= n_norm; // n̂ - - MatrixMax3d A = - (MatrixMax3d::Identity(dim, dim) - n * n.transpose()) / n_norm; + const VectorMax3d n = compute_unnormalized_normal(positions); - dn = A * dn; + MatrixMax dn = normalization_jacobian(n) + * compute_unnormalized_normal_jacobian(positions); if (flip_if_negative && (positions.head(dim) - positions.tail(dim)).dot(n) < 0) { diff --git a/src/ipc/candidates/edge_edge.cpp b/src/ipc/candidates/edge_edge.cpp index d3620bc10..5438878e6 100644 --- a/src/ipc/candidates/edge_edge.cpp +++ b/src/ipc/candidates/edge_edge.cpp @@ -1,6 +1,7 @@ #include "edge_edge.hpp" #include +#include #include namespace ipc { @@ -99,8 +100,9 @@ VectorMax3d EdgeEdgeCandidate::compute_unnormalized_normal( Eigen::ConstRef positions) const { assert(positions.size() == 12); - return (positions.segment<3>(3) - positions.head<3>()) - .cross(positions.tail<3>() - positions.segment<3>(6)); + return edge_edge_unnormalized_normal( + positions.head<3>(), positions.segment<3>(3), positions.segment<3>(6), + positions.tail<3>()); } MatrixMax @@ -108,16 +110,9 @@ EdgeEdgeCandidate::compute_unnormalized_normal_jacobian( Eigen::ConstRef positions) const { assert(positions.size() == 12); - MatrixMax dn(3, 12); - dn.leftCols<3>() = - cross_product_matrix(positions.tail<3>() - positions.segment<3>(6)); - dn.middleCols<3>(3) = - cross_product_matrix(positions.segment<3>(6) - positions.tail<3>()); - dn.middleCols<3>(6) = - cross_product_matrix(positions.head<3>() - positions.segment<3>(3)); - dn.rightCols<3>() = - cross_product_matrix(positions.segment<3>(3) - positions.head<3>()); - return dn; + return edge_edge_unnormalized_normal_jacobian( + positions.head<3>(), positions.segment<3>(3), positions.segment<3>(6), + positions.tail<3>()); } bool EdgeEdgeCandidate::ccd( diff --git a/src/ipc/candidates/edge_vertex.cpp b/src/ipc/candidates/edge_vertex.cpp index 00b08e8b9..382d7c936 100644 --- a/src/ipc/candidates/edge_vertex.cpp +++ b/src/ipc/candidates/edge_vertex.cpp @@ -1,6 +1,7 @@ #include "edge_vertex.hpp" #include +#include #include #include @@ -81,20 +82,8 @@ VectorMax3d EdgeVertexCandidate::compute_unnormalized_normal( Eigen::ConstRef positions) const { const int dim = this->dim(positions.size()); - - if (dim == 2) { - // In 2D, the normal is simply the perpendicular vector to the edge - const Eigen::Vector2d e = positions.tail<2>() - positions.segment<2>(2); - return Eigen::Vector2d(-e.y(), e.x()); - } - - // 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. - assert(dim == 3); - const Eigen::Vector3d e = positions.tail<3>() - positions.segment<3>(3); - const Eigen::Vector3d d = positions.head<3>() - positions.segment<3>(3); - return d * e.dot(e) - e * e.dot(d); + return edge_vertex_unnormalized_normal( + positions.head(dim), positions.segment(dim, dim), positions.tail(dim)); } MatrixMax @@ -102,30 +91,8 @@ EdgeVertexCandidate::compute_unnormalized_normal_jacobian( Eigen::ConstRef positions) const { const int dim = this->dim(positions.size()); - if (dim == 2) { - // In 2D, the normal is simply the perpendicular vector to the edge - MatrixMax dn(2, 6); - dn.leftCols<2>().setZero(); - dn.middleCols<2>(2) << 0, 1, -1, 0; - dn.rightCols<2>() << 0, -1, 1, 0; - return dn; - } - - assert(dim == 3); - const Eigen::Vector3d e = positions.tail<3>() - positions.segment<3>(3); - const Eigen::Vector3d d = positions.head<3>() - positions.segment<3>(3); - - const auto I = Eigen::Matrix3d::Identity(); - - MatrixMax dn(3, 9); - // ∂n/∂x0 - dn.leftCols<3>() = e.dot(e) * I - e * e.transpose(); - // ∂n/∂x2 - dn.rightCols<3>() = - -e.dot(d) * I - e * d.transpose() + (2 * d) * e.transpose(); - // ∂n/∂x1 - dn.middleCols<3>(3) = -dn.leftCols<3>() - dn.rightCols<3>(); - return dn; + return edge_vertex_unnormalized_normal_jacobian( + positions.head(dim), positions.segment(dim, dim), positions.tail(dim)); } bool EdgeVertexCandidate::ccd( diff --git a/src/ipc/candidates/face_vertex.cpp b/src/ipc/candidates/face_vertex.cpp index ae98045dc..3c26b25c2 100644 --- a/src/ipc/candidates/face_vertex.cpp +++ b/src/ipc/candidates/face_vertex.cpp @@ -1,6 +1,7 @@ #include "face_vertex.hpp" #include +#include #include #include @@ -94,8 +95,8 @@ VectorMax3d FaceVertexCandidate::compute_unnormalized_normal( Eigen::ConstRef positions) const { assert(positions.size() == 12); - return (positions.segment<3>(6) - positions.segment<3>(3)) - .cross(positions.tail<3>() - positions.segment<3>(3)); + return triangle_unnormalized_normal( + positions.segment<3>(3), positions.segment<3>(6), positions.tail<3>()); } MatrixMax @@ -105,12 +106,8 @@ FaceVertexCandidate::compute_unnormalized_normal_jacobian( assert(positions.size() == 12); MatrixMax dn(3, 12); dn.leftCols<3>().setZero(); - dn.middleCols<3>(3) = - cross_product_matrix(positions.tail<3>() - positions.segment<3>(6)); - dn.middleCols<3>(6) = - cross_product_matrix(positions.segment<3>(3) - positions.tail<3>()); - dn.rightCols<3>() = - cross_product_matrix(positions.segment<3>(6) - positions.segment<3>(3)); + dn.rightCols<9>() = triangle_unnormalized_normal_jacobian( + positions.segment<3>(3), positions.segment<3>(6), positions.tail<3>()); return dn; } diff --git a/src/ipc/geometry/CMakeLists.txt b/src/ipc/geometry/CMakeLists.txt new file mode 100644 index 000000000..b3fccc83d --- /dev/null +++ b/src/ipc/geometry/CMakeLists.txt @@ -0,0 +1,10 @@ +set(SOURCES + normal.cpp + normal.hpp +) + +target_sources(ipc_toolkit PRIVATE ${SOURCES}) + +################################################################################ +# Subfolders +################################################################################ \ No newline at end of file diff --git a/src/ipc/geometry/normal.cpp b/src/ipc/geometry/normal.cpp new file mode 100644 index 000000000..088e0788e --- /dev/null +++ b/src/ipc/geometry/normal.cpp @@ -0,0 +1,90 @@ +#include "ipc/geometry/normal.hpp" + +namespace ipc { + +std::tuple +normalization_and_jacobian(Eigen::ConstRef x) +{ + const double norm = x.norm(); + const VectorMax3d xhat = x / norm; + const auto I = MatrixMax3d::Identity(x.size(), x.size()); + const MatrixMax3d J = (I - (xhat * xhat.transpose())) / norm; + return std::make_tuple(xhat, J); +} + +std::tuple> +normalization_and_jacobian_and_hessian(Eigen::ConstRef x) +{ + // const auto [xhat, J] = normalization_and_jacobian(x); + const double norm = x.norm(); + const VectorMax3d xhat = x / norm; + const auto I = MatrixMax3d::Identity(x.size(), x.size()); + const MatrixMax3d J = (I - (xhat * xhat.transpose())) / norm; + + std::array H; + for (int i = 0; i < x.size(); i++) { + H[i] = (xhat(i) * J + xhat * J.row(i) + J.col(i) * xhat.transpose()) + / (-norm); + } + + return std::make_tuple(xhat, J, H); +} + +// --- edge-vertex normal functions ------------------------------------------- + +VectorMax3d edge_vertex_unnormalized_normal( + Eigen::ConstRef v, + Eigen::ConstRef e0, + Eigen::ConstRef e1) +{ + assert(v.size() == e0.size() && v.size() == e1.size()); + assert(v.size() == 2 || v.size() == 3); + if (v.size() == 2) { + // In 2D, the normal is simply the perpendicular vector to the edge + const Eigen::Vector2d e = e1.tail<2>() - e0.tail<2>(); + return Eigen::Vector2d(-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 = v - e0; + return d * e.dot(e) - e * e.dot(d); + } +} + +MatrixMax edge_vertex_unnormalized_normal_jacobian( + Eigen::ConstRef v, + Eigen::ConstRef e0, + Eigen::ConstRef e1) +{ + assert(v.size() == e0.size() && v.size() == e1.size()); + assert(v.size() == 2 || v.size() == 3); + + MatrixMax dn(v.size(), 3 * v.size()); + + if (v.size() == 2) { + // In 2D, the normal is simply the perpendicular vector to the edge + dn.leftCols<2>().setZero(); + dn.middleCols<2>(2) << 0, 1, -1, 0; + dn.rightCols<2>() << 0, -1, 1, 0; + return dn; + } else { + const Eigen::Vector3d e = e1 - e0; + const Eigen::Vector3d d = v - e0; + + const auto I = Eigen::Matrix3d::Identity(); + + // ∂n/∂v + dn.leftCols<3>() = e.dot(e) * I - e * e.transpose(); + // ∂n/∂e1 + dn.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>(); + } + + return dn; +} + +} // namespace ipc \ No newline at end of file diff --git a/src/ipc/geometry/normal.hpp b/src/ipc/geometry/normal.hpp new file mode 100644 index 000000000..d4867e5f3 --- /dev/null +++ b/src/ipc/geometry/normal.hpp @@ -0,0 +1,229 @@ +#pragma once + +#include + +#include + +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); + +/// @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) +{ + 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); + +/// @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) +{ + 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. +inline Eigen::Matrix3d 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; + return m; +} + +/** + * \defgroup geometry Edge-vertex normal + * \brief Functions for computing an edge-vertex normal and resp. Jacobians. + * @{ + */ + +/// @brief Computes the unnormalized normal vector of an edge-vertex pair. +/// @param v The vertex position. +/// @param e0 The start position of the edge. +/// @param e1 The end position of the edge. +/// @return The unnormalized normal vector. +VectorMax3d edge_vertex_unnormalized_normal( + Eigen::ConstRef v, + Eigen::ConstRef e0, + Eigen::ConstRef e1); + +/// @brief Computes the normal vector of an edge-vertex pair. +/// @param v The vertex position. +/// @param e0 The start position of the edge. +/// @param e1 The end position of the edge. +/// @return The normal vector. +inline VectorMax3d edge_vertex_normal( + Eigen::ConstRef v, + Eigen::ConstRef e0, + Eigen::ConstRef e1) +{ + return edge_vertex_unnormalized_normal(v, e0, e1).normalized(); +} + +/// @brief Computes the Jacobian of the unnormalized normal vector of an edge-vertex pair. +/// @param v The vertex position. +/// @param e0 The start position of the edge. +/// @param e1 The end position of the edge. +/// @return The Jacobian of the unnormalized normal vector. +MatrixMax edge_vertex_unnormalized_normal_jacobian( + Eigen::ConstRef v, + 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. +inline Eigen::Vector3d 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. +inline Eigen::Vector3d 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. +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); + J.middleCols<3>(3) = cross_product_matrix(a - c); + J.middleCols<3>(6) = cross_product_matrix(b - a); + return J; +} + +/// @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) +{ + // ∂n̂/∂x = ∂n̂/∂n * ∂n/∂x + return normalization_jacobian(triangle_unnormalized_normal(a, b, c)) + * triangle_unnormalized_normal_jacobian(a, b, c); +} + +/** @} */ + +/** + * \defgroup geometry Edge-edge normal + * \brief Functions for computing an edge-edge normal and resp. Jacobians. + * @{ + */ + +/// @brief Computes the unnormalized normal vector of 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 unnormalized normal vector of the two edges. +inline Eigen::Vector3d edge_edge_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 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 normal vector of the two edges. +inline Eigen::Vector3d edge_edge_normal( + Eigen::ConstRef ea0, + Eigen::ConstRef ea1, + Eigen::ConstRef eb0, + Eigen::ConstRef eb1) +{ + return edge_edge_unnormalized_normal(ea0, ea1, eb0, eb1).normalized(); +} + +/// @brief Computes the Jacobian of the unnormalized normal vector of 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 Jacobian of the unnormalized normal vector of the two edges. +inline Eigen::Matrix edge_edge_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); + return J; +} + +/// @brief Computes the Jacobian of the normal vector of 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 Jacobian of the normal vector of the two edges. +inline Eigen::Matrix edge_edge_normal_jacobian( + Eigen::ConstRef ea0, + Eigen::ConstRef ea1, + Eigen::ConstRef eb0, + Eigen::ConstRef eb1) +{ + // ∂n̂/∂x = ∂n̂/∂n * ∂n/∂x + return normalization_jacobian( + edge_edge_unnormalized_normal(ea0, ea1, eb0, eb1)) + * edge_edge_unnormalized_normal_jacobian(ea0, ea1, eb0, eb1); +} + +} // namespace ipc \ No newline at end of file diff --git a/src/ipc/smooth_contact/primitives/edge3.cpp b/src/ipc/smooth_contact/primitives/edge3.cpp index 53c20974f..36357286d 100644 --- a/src/ipc/smooth_contact/primitives/edge3.cpp +++ b/src/ipc/smooth_contact/primitives/edge3.cpp @@ -1,6 +1,7 @@ #include "edge3.hpp" #include +#include #include #include @@ -232,7 +233,7 @@ namespace { { assert(otypes.size() == 2); - const auto [dn, dn_grad] = normalize_vector_grad(direc); + const auto [dn, dn_grad] = normalization_and_jacobian(direc); auto [t_term, t_grad] = smooth_edge3_tangent_term_gradient( dn, e0, e1, f0, f1, params.alpha_t, params.beta_t, otypes); @@ -270,7 +271,8 @@ namespace { { assert(otypes.size() == 2); - const auto [dn, dn_grad, dn_hess] = normalize_vector_hess(direc); + const auto [dn, dn_grad, dn_hess] = + normalization_and_jacobian_and_hessian(direc); auto [t_term, t_grad, t_hess] = smooth_edge3_tangent_term_hessian( dn, e0, e1, f0, f1, params.alpha_t, params.beta_t, otypes); diff --git a/src/ipc/smooth_contact/primitives/point3.cpp b/src/ipc/smooth_contact/primitives/point3.cpp index ddfb7e70c..e9a6099bc 100644 --- a/src/ipc/smooth_contact/primitives/point3.cpp +++ b/src/ipc/smooth_contact/primitives/point3.cpp @@ -1,6 +1,7 @@ #include "point3.hpp" #include +#include #include namespace ipc { @@ -412,7 +413,7 @@ GradType<-1> Point3::smooth_point3_term_gradient( const Eigen::VectorXd tangents_vec = Eigen::Map(tangents.data(), tangents.size()); - auto [dn, dn_grad] = normalize_vector_grad(direc); + auto [dn, dn_grad] = normalization_and_jacobian(direc); dn *= -1; dn_grad *= -1; @@ -459,7 +460,7 @@ HessianType<-1> Point3::smooth_point3_term_hessian( const Eigen::VectorXd tangents_vec = Eigen::Map(tangents.data(), tangents.size()); - auto [dn, dn_grad, dn_hess] = normalize_vector_hess(direc); + auto [dn, dn_grad, dn_hess] = normalization_and_jacobian_and_hessian(direc); dn *= -1; dn_grad *= -1; for (auto& mat : dn_hess) { diff --git a/src/ipc/utils/eigen_ext.hpp b/src/ipc/utils/eigen_ext.hpp index 9202ecf93..e923e4093 100644 --- a/src/ipc/utils/eigen_ext.hpp +++ b/src/ipc/utils/eigen_ext.hpp @@ -174,18 +174,6 @@ using HessianType = /**@}*/ -/// @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) -{ - Eigen::Matrix3d m; - m << 0, -v(2), v(1), // - v(2), 0, -v(0), // - -v(1), v(0), 0; - return m; -} - /// @brief Matrix projection onto positive definite cone /// @param A Symmetric matrix to project /// @param eps Minimum eigenvalue threshold diff --git a/src/ipc/utils/math.cpp b/src/ipc/utils/math.cpp index 90ec80757..3d7954bbf 100644 --- a/src/ipc/utils/math.cpp +++ b/src/ipc/utils/math.cpp @@ -1,5 +1,6 @@ #include "math.hpp" +#include #include #include @@ -39,35 +40,6 @@ void OrientationTypes::set_size(const int size) normal_types.assign(m_size, HeavisideType::VARIANT); } -std::tuple -normalize_vector_grad(Eigen::ConstRef t) -{ - double norm = t.norm(); - Eigen::Vector3d y = t / norm; - Eigen::Matrix3d grad = - (Eigen::Matrix3d::Identity() - y * y.transpose()) / norm; - return std::make_tuple(y, grad); -} - -std::tuple< - Eigen::Vector3d, - Eigen::Matrix3d, - std::array, 3>> -normalize_vector_hess(Eigen::ConstRef t) -{ - double norm = t.norm(); - Eigen::Vector3d y = t / norm; - Eigen::Matrix3d grad = - (Eigen::Matrix3d::Identity() - y * y.transpose()) / norm; - std::array, 3> hess; - for (int i = 0; i < 3; i++) { - hess[i] = -(y(i) * grad + y * grad.row(i) + grad.col(i) * y.transpose()) - / norm; - } - - return std::make_tuple(y, grad, hess); -} - double opposite_direction_penalty( Eigen::ConstRef t, Eigen::ConstRef d, @@ -83,7 +55,7 @@ GradType<6> opposite_direction_penalty_grad( const double alpha, const double beta) { - auto [tn, tn_grad] = normalize_vector_grad(t); + auto [tn, tn_grad] = normalization_and_jacobian(t); const double a = d.dot(tn); const double y = Math::smooth_heaviside(a, alpha, beta); const double dy = Math::smooth_heaviside_grad(a, alpha, beta); @@ -99,7 +71,7 @@ HessianType<6> opposite_direction_penalty_hess( const double alpha, const double beta) { - auto [tn, tn_grad, tn_hess] = normalize_vector_hess(t); + auto [tn, tn_grad, tn_hess] = normalization_and_jacobian_and_hessian(t); const double a = d.dot(tn); const double y = Math::smooth_heaviside(a, alpha, beta); const double dy = Math::smooth_heaviside_grad(a, alpha, beta); diff --git a/src/ipc/utils/math.hpp b/src/ipc/utils/math.hpp index 918b500a1..9ff0ce769 100644 --- a/src/ipc/utils/math.hpp +++ b/src/ipc/utils/math.hpp @@ -75,17 +75,6 @@ template struct Math { Eigen::ConstRef> b); }; -// gradient is symmetric -std::tuple -normalize_vector_grad(Eigen::ConstRef t); - -// hessian is symmetric wrt. the three dimensions -std::tuple< - Eigen::Vector3d, - Eigen::Matrix3d, - std::array, 3>> -normalize_vector_hess(Eigen::ConstRef t); - template inline Eigen::Matrix< T, diff --git a/tests/src/tests/barrier/test_barrier.cpp b/tests/src/tests/barrier/test_barrier.cpp index f52c2bc4a..3e7a2da9e 100644 --- a/tests/src/tests/barrier/test_barrier.cpp +++ b/tests/src/tests/barrier/test_barrier.cpp @@ -5,6 +5,7 @@ #include #include +#include #include #include #include @@ -141,7 +142,8 @@ TEST_CASE("Normalize vector derivatives", "[deriv]") ipc::Vector x_ad = ipc::slice_positions(x); ipc::Vector y_ad = x_ad / x_ad.norm(); - const auto [y, grad, hess] = ipc::normalize_vector_hess(x); + const auto [y, grad, hess] = + ipc::normalization_and_jacobian_and_hessian(x); double err_grad = 0, err_hess = 0; for (int j = 0; j < 3; j++) {