diff --git a/docs/source/cpp-api/interval.rst b/docs/source/cpp-api/interval.rst new file mode 100644 index 000000000..54184f489 --- /dev/null +++ b/docs/source/cpp-api/interval.rst @@ -0,0 +1,31 @@ +Interval Arithmetic +=================== + +For interval arithmetic, we use the `Filib `_ library. + +.. doxygenclass:: filib::Interval + +Helper Functions +---------------- + +.. doxygenfunction:: ipc::norm +.. doxygenfunction:: ipc::squared_norm + +Type Definitions +---------------- + +.. doxygentypedef:: ipc::Vector2I +.. doxygentypedef:: ipc::Vector3I +.. doxygentypedef:: ipc::VectorXI + +.. doxygentypedef:: ipc::RowVector2I +.. doxygentypedef:: ipc::RowVector3I +.. doxygentypedef:: ipc::RowVectorXI + +.. doxygentypedef:: ipc::Matrix2I +.. doxygentypedef:: ipc::Matrix3I +.. doxygentypedef:: ipc::MatrixXI + +.. doxygentypedef:: ipc::VectorMax3I +.. doxygentypedef:: ipc::RowVectorMax3I +.. doxygentypedef:: ipc::MatrixMax3I \ No newline at end of file diff --git a/docs/source/cpp.rst b/docs/source/cpp.rst index 0ff2239e7..1c6ab6331 100644 --- a/docs/source/cpp.rst +++ b/docs/source/cpp.rst @@ -31,6 +31,8 @@ C++ :start-after: :end-before: +.. _filib_dependency_note: + .. warning:: ``filib`` is licensed under `LGPL-2.1 `_ and as such it is required to be dynamically linked. Doing so automatically is a challenge, so by default we use static linkage. Enabling dynaic linkage requires copying the ``.so``/``.dylib``/``.dll`` file to the binary directory or system path. To enable this, set the CMake option :cmake:`FILIB_BUILD_SHARED_LIBS` to :cmake:`ON` and add this CMake code to copy the shared libaray object to the binary directory: diff --git a/docs/source/index.rst b/docs/source/index.rst index 1e4259c7e..6146b9ec6 100644 --- a/docs/source/index.rst +++ b/docs/source/index.rst @@ -37,6 +37,7 @@ cpp-api/barrier.rst cpp-api/adhesion.rst cpp-api/intersections.rst + cpp-api/interval.rst cpp-api/utils.rst .. toctree:: @@ -57,6 +58,7 @@ python-api/barrier.rst python-api/adhesion.rst python-api/intersections.rst + python-api/interval.rst python-api/utils.rst .. toctree:: diff --git a/docs/source/python-api/interval.rst b/docs/source/python-api/interval.rst new file mode 100644 index 000000000..b5bc0289a --- /dev/null +++ b/docs/source/python-api/interval.rst @@ -0,0 +1,8 @@ +Interval Arithmetic +=================== + +.. automodule:: ipctk.filib + :members: + :undoc-members: + :show-inheritance: + :inherited-members: \ No newline at end of file diff --git a/docs/source/tutorial/adhesion.rst b/docs/source/tutorial/adhesion.rst index ab4dd3441..427df576a 100644 --- a/docs/source/tutorial/adhesion.rst +++ b/docs/source/tutorial/adhesion.rst @@ -3,7 +3,7 @@ Adhesion Adhesion simulates the sticking interaction between surfaces, such as glue bonding or material contact. -We provide functionality to compute normal adhesion (perpendicular forces) and tangential adhesion (parallel forces) using the model of :cite:`Fang2023AugmentedStickyInteractions`. +We provide functionality to compute normal adhesion (perpendicular forces) and tangential adhesion (parallel forces) using the model of :cite:t:`Fang2023AugmentedStickyInteractions`. Normal Adhesion --------------- diff --git a/docs/source/tutorial/nonlinear_ccd.rst b/docs/source/tutorial/nonlinear_ccd.rst index 354adf78f..93d69be93 100644 --- a/docs/source/tutorial/nonlinear_ccd.rst +++ b/docs/source/tutorial/nonlinear_ccd.rst @@ -1,7 +1,9 @@ Nonlinear CCD ============= -We can also perform CCD of nonlinear trajectories (of linear geometry) using the method of :cite:t:`Ferguson2021RigidIPC`. While :cite:t:`Ferguson2021RigidIPC` introduce their method in the context of rigid bodies, it can be applied to any nonlinear trajectory of linear geometry. The method works by transforming the nonlinear trajectories into (adaptive) piecewise linear trajectories with an envelope/minimum separation around each piece, enclosing the nonlinear trajectory. The method then performs CCD on the piecewise linear trajectories to find the earliest time of impact. +We also implement CCD of nonlinear trajectories (of linear geometry) using the method of :cite:t:`Ferguson2021RigidIPC`. While :cite:t:`Ferguson2021RigidIPC` introduce their method in the context of rigid bodies, it can be applied to any nonlinear trajectory of linear geometry. + +The method works by transforming the nonlinear trajectories into (adaptive) piecewise linear trajectories with an envelope/minimum separation around each piece, enclosing the nonlinear trajectory. The method then performs CCD on the piecewise linear trajectories to find the earliest time of impact. We provide the following functions to perform nonlinear CCD: @@ -14,7 +16,7 @@ We provide the following functions to perform nonlinear CCD: * :cpp:func:`ipc::edge_edge_nonlinear_ccd`, and * :cpp:func:`ipc::point_triangle_nonlinear_ccd`. - Each of these functions take as input a :cpp:func:`ipc::NonlinearTrajectory` object for the endpoints of the linear geometry. + Each of these functions take as input a :cpp:class:`ipc::NonlinearTrajectory` object for the endpoints of the linear geometry. .. md-tab-item:: Python @@ -23,7 +25,7 @@ We provide the following functions to perform nonlinear CCD: * :py:func:`ipctk.edge_edge_nonlinear_ccd`, and * :py:func:`ipctk.point_triangle_nonlinear_ccd`. - Each of these functions take as input a :py:func:`ipctk.NonlinearTrajectory` object for the endpoints of the linear geometry. + Each of these functions take as input a :py:class:`ipctk.NonlinearTrajectory` object for the endpoints of the linear geometry. For example, the following code defines a rigid trajectory in 2D in order to perform nonlinear CCD between a point and edge: @@ -47,7 +49,7 @@ For example, the following code defines a rigid trajectory in 2D in order to per Defining the Trajectory ----------------------- -Let's dive deeper by breaking down the implementation of Rigid2DTrajectory. The first function we need to implement is the call operator: +Let's dive deeper by breaking down the implementation of ``Rigid2DTrajectory``. The first function we need to implement is the call operator: .. md-tab-set:: @@ -63,7 +65,7 @@ Let's dive deeper by breaking down the implementation of Rigid2DTrajectory. The .. literalinclude:: ../../../python/tests/test_ccd.py :language: python - :lines: 91-95 + :lines: 90-94 :dedent: 8 This function computes the position of the point at a time :math:`t \in [0, 1]`. This defines the trajectory of the point. In this case, we have a rigid body with a center of mass (COM) at the origin. The trajectory of the point is given by: @@ -92,7 +94,7 @@ The second function we need to implement is ``max_distance_from_linear``. .. literalinclude:: ../../../python/tests/test_ccd.py :language: python - :lines: 97-103 + :lines: 96-102 :dedent: 8 This function computes the maximum distance over a time interval :math:`[t_0, t_1]` between the nonlinear trajectory and a line segment from :math:`x(t_0)` to :math:`x(t_1)`. Mathematically this function computes @@ -135,4 +137,96 @@ Last, we use the ``Rigid2DTrajectory`` to perform nonlinear CCD between a point :dedent: 4 .. note:: - We adjust the ``conservative_rescaling`` parameter to get a more accurate time of impact (TOI), but in practice, this is not needed as a more conservative estimate of the TOI is sufficient to avoid penetrations. \ No newline at end of file + We adjust the ``conservative_rescaling`` parameter to get a more accurate time of impact (TOI), but in practice, this is not needed as a more conservative estimate of the TOI is sufficient to avoid penetrations. + +Interval-Based Nonlinear CCD +---------------------------- + +.. warning:: + The following section requires enabling the ``filib`` interval arithmetic library. ``filib`` is licensed under L-GPL 2.1, so special care must be taken when using it. See the `filib dependency note <../../cpp.html#filib-dependency-note>`_ for more information. + +If an analytic expression for the ``max_distance_from_linear`` function is not available, we can use interval arithmetic to compute a conservative envelope. + +.. md-tab-set:: + + .. md-tab-item:: C++ + + The :cpp:class:`ipc::IntervalNonlinearTrajectory` class does this for us and all we need to provide is a function to compute the point's position over a time interval. + + .. md-tab-item:: Python + + The :py:class:`ipctk.IntervalNonlinearTrajectory` class does this for us and all we need to provide is a function to compute the point's position over a time interval. + +Conservative Envelope +~~~~~~~~~~~~~~~~~~~~~ + +Our implementation of the ``max_distance_from_linear`` function is as follows. + +Start by defining a linear interpolation function: + +.. math:: + \operatorname{lerp}(a, b, t) := (b - a) t + a, + +which interpolates between two points :math:`a` and :math:`b` at time :math:`t`. + +The exact envelope from above is bounded by a interval approximation: + +.. math:: + \begin{align} + &\max_{t \in [0, 1]} \| p(\operatorname{lerp}(t_0, t_1, t)) - \operatorname{lerp}(p(t_0), p(t_1), t) \|_2\\ + &\leq \sup(\| p_{\Box}([t_0, t_1]) - ((p(t_1) - p(t_0)) \cdot [0, 1] + p(t_0)) \|_2). + \end{align} + +Therefore, we can compute the a conservative estimate of the envelope by implementing :math:`p_{\Box}([t_0, t_1])` and :math:`p(t)`. + +.. note:: + In practice we subdivide the interval into smaller intervals and compute the envelope over each subinterval. This is done to create a more accurate estimate. + +Interval Arithmetic +~~~~~~~~~~~~~~~~~~~ + +`Interval arithmetic `_ is a method to compute bounds on the range of a function over an interval. We can construct a vector of intervals to represent the position of the point over a time interval. + +The following code snippet shows an example of how to use interval arithmetic to compute the position of a point over a time interval: + +.. md-tab-set:: + + .. md-tab-item:: C++ + + .. code-block:: cpp + + #include + + using namespace ipc; + + Vector2I position( + const VectorMax3d& center, + const VectorMax3d& point, + const double omega, + const Interval& t) + { + // 2×2 matrix of intervals representing the rotation matrix + Matrix2I R; + R << cos(omega * t), -sin(omega * t), + sin(omega * t), cos(omega * t); + return R * (point - center) + center; + } + + The full documentation for the ``Interval`` class can be found `here <../../cpp-api/interval.html>`_. + + .. md-tab-item:: Python + + .. code-block:: python + + import numpy as np + + from ipctk.filib import Interval, sin, cos + + def position(center, point, omega, t : Interval): + R = np.array([ + [cos(omega * t), -sin(omega * t)], + [sin(omega * t), cos(omega * t)] + ]) + return R @ (point - center) + center + + The full documentation for the filib python bindings can be found `here <../python-api/interval.html>`_. diff --git a/python/src/utils/interval.cpp b/python/src/utils/interval.cpp index 4e98a2c4c..11dfeb8f0 100644 --- a/python/src/utils/interval.cpp +++ b/python/src/utils/interval.cpp @@ -36,7 +36,9 @@ using namespace filib; void define_interval(py::module_& m) { #ifdef IPC_TOOLKIT_WITH_FILIB - auto m_filib = m.def_submodule("filib", "Fast Interval Library"); + auto m_filib = m.def_submodule( + "filib", + "Python bindings for the `Fast Interval Library (Filib) `_ by Werner Hofschuster and Walter Kraemer."); py::class_(m_filib, "Interval") .def(py::init()) diff --git a/src/ipc/candidates/collision_stencil.hpp b/src/ipc/candidates/collision_stencil.hpp index 8313d0223..be63ce3f1 100644 --- a/src/ipc/candidates/collision_stencil.hpp +++ b/src/ipc/candidates/collision_stencil.hpp @@ -39,7 +39,7 @@ class CollisionStencil { /// @return The vertex positions of the collision stencil. Size is always 4, but elements i > num_vertices() are NaN. template std::array, 4> vertices( - const MatrixX& vertices, + const Eigen::MatrixX& vertices, const Eigen::MatrixXi& edges, const Eigen::MatrixXi& faces) const { @@ -67,7 +67,7 @@ class CollisionStencil { /// @return This stencil's DOF. template VectorMax12 - dof(const MatrixX& X, + dof(const Eigen::MatrixX& X, const Eigen::MatrixXi& edges, const Eigen::MatrixXi& faces) const { diff --git a/src/ipc/collisions/normal/normal_collisions.cpp b/src/ipc/collisions/normal/normal_collisions.cpp index 50f765ed8..40a33f244 100644 --- a/src/ipc/collisions/normal/normal_collisions.cpp +++ b/src/ipc/collisions/normal/normal_collisions.cpp @@ -136,6 +136,8 @@ namespace { return ev_candidates; } + + inline double sqr(double x) { return x * x; } } // namespace void NormalCollisions::build( @@ -147,7 +149,7 @@ void NormalCollisions::build( { assert(vertices.rows() == mesh.num_vertices()); - double inflation_radius = (dhat + dmin) / 2; + const double inflation_radius = 0.5 * (dhat + dmin); Candidates candidates; candidates.build(mesh, vertices, inflation_radius, broad_phase_method); @@ -168,8 +170,7 @@ void NormalCollisions::build( // Cull the candidates by measuring the distance and dropping those that are // greater than dhat. - const double offset_sqr = (dmin + dhat) * (dmin + dhat); - auto is_active = [&](double distance_sqr) { + auto is_active = [offset_sqr = sqr(dmin + dhat)](double distance_sqr) { return distance_sqr < offset_sqr; }; diff --git a/src/ipc/tangent/closest_point.cpp b/src/ipc/tangent/closest_point.cpp index b6df8c1e7..7f3d05f95 100644 --- a/src/ipc/tangent/closest_point.cpp +++ b/src/ipc/tangent/closest_point.cpp @@ -40,7 +40,7 @@ VectorMax9d point_edge_closest_point_jacobian( // ============================================================================ // Edge - Edge -Vector2 edge_edge_closest_point( +Eigen::Vector2d edge_edge_closest_point( const Eigen::Ref& ea0, const Eigen::Ref& ea1, const Eigen::Ref& eb0, @@ -55,7 +55,7 @@ Vector2 edge_edge_closest_point( coefMtr(0, 1) = coefMtr(1, 0) = -eb.dot(ea); coefMtr(1, 1) = eb.squaredNorm(); - Vector2 rhs; + Eigen::Vector2d rhs; rhs[0] = -eb_to_ea.dot(ea); rhs[1] = eb_to_ea.dot(eb); @@ -82,15 +82,15 @@ Eigen::Matrix edge_edge_closest_point_jacobian( // ============================================================================ // Point - Triangle -Vector2 point_triangle_closest_point( +Eigen::Vector2d point_triangle_closest_point( const Eigen::Ref& p, const Eigen::Ref& t0, const Eigen::Ref& t1, const Eigen::Ref& t2) { Eigen::Matrix basis; - basis.row(0) = RowVector3(t1 - t0); // edge 0 - basis.row(1) = RowVector3(t2 - t0); // edge 1 + 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); diff --git a/src/ipc/utils/eigen_ext.hpp b/src/ipc/utils/eigen_ext.hpp index 040b8ab4b..16d1784d3 100644 --- a/src/ipc/utils/eigen_ext.hpp +++ b/src/ipc/utils/eigen_ext.hpp @@ -8,31 +8,20 @@ namespace ipc { // Boolean scalar -typedef Eigen::Array ArrayXb; -typedef Eigen::Matrix VectorXb; -typedef Eigen::Matrix Vector3b; -typedef Eigen::Matrix MatrixXb; +using ArrayXb = Eigen::Array; +using VectorXb = Eigen::Matrix; +using Vector3b = Eigen::Matrix; +using MatrixXb = Eigen::Matrix; template using Vector = Eigen::Matrix; template using RowVector = Eigen::Matrix; -template using Vector1 = Vector; -template using Vector2 = Vector; -template using Vector3 = Vector; -template using VectorX = Vector; -template using RowVector2 = RowVector; -template using RowVector3 = RowVector; -template using RowVectorX = RowVector; -template using Matrix2 = Eigen::Matrix; -template using Matrix3 = Eigen::Matrix; -template -using MatrixX = Eigen::Matrix; -using Vector1d = Vector1; -using Vector6d = Vector; -using Vector9d = Vector; -using Vector12d = Vector; +using Vector1d = Eigen::Vector; +using Vector6d = Eigen::Vector; +using Vector9d = Eigen::Vector; +using Vector12d = Eigen::Vector; using Matrix6d = Eigen::Matrix; using Matrix9d = Eigen::Matrix; using Matrix12d = Eigen::Matrix; diff --git a/src/ipc/utils/interval.hpp b/src/ipc/utils/interval.hpp index 2943ae8ec..4bc9c2f0f 100644 --- a/src/ipc/utils/interval.hpp +++ b/src/ipc/utils/interval.hpp @@ -10,22 +10,31 @@ #include namespace filib { +/// @brief Interval type class Interval : public interval { public: + /// @brief Construct an Interval from a filib interval + /// @param i The filib interval Interval(const interval& i) : interval(i) { } + /// @brief Construct an empty Interval Interval() { this->INF = 0; this->SUP = 0; } + /// @brief Construct an Interval from a single value + /// @param x The value explicit Interval(double x) { this->INF = x; this->SUP = x; } + /// @brief Construct an Interval from two values + /// @param x The infimum value + /// @param y The supremum value Interval(double x, double y) { this->INF = x; @@ -43,26 +52,43 @@ template <> struct fmt::formatter : ostream_formatter { }; namespace ipc { -typedef Vector2 Vector2I; -typedef Vector3 Vector3I; -typedef VectorMax3 VectorMax3I; -typedef VectorX VectorXI; -typedef RowVector2 RowVector2I; -typedef RowVector3 RowVector3I; -typedef RowVectorMax3 RowVectorMax3I; -typedef RowVectorX RowVectorXI; -typedef Matrix2 Matrix2I; -typedef Matrix3 Matrix3I; -typedef MatrixMax3 MatrixMax3I; -typedef MatrixX MatrixXI; - -/// @brief Compute the L2 norm of a 3-dimensional interval -/// @param v The 3-dimensional interval -/// @return The L2 norm of the interval +/// @brief 2D vector of intervals +using Vector2I = Eigen::Vector2; +/// @brief 3D vector of intervals +using Vector3I = Eigen::Vector3; +/// @brief Dynamic vector of intervals +using VectorXI = Eigen::VectorX; + +/// @brief 2D row vector of intervals +using RowVector2I = Eigen::RowVector2; +/// @brief 3D row vector of intervals +using RowVector3I = Eigen::RowVector3; +/// @brief Dynamic row vector of intervals +using RowVectorXI = Eigen::RowVectorX; + +/// @brief 2x2 matrix of intervals +using Matrix2I = Eigen::Matrix2; +/// @brief 3x3 matrix of intervals +using Matrix3I = Eigen::Matrix3; +/// @brief Dynamic matrix of intervals +using MatrixXI = Eigen::MatrixX; + +/// @brief Dynamic vector of intervals with a maximum size of 3 +using VectorMax3I = VectorMax3; +/// @brief Dynamic row vector of intervals with a maximum size of 3 +using RowVectorMax3I = RowVectorMax3; +/// @brief Dynamic matrix of intervals with a maximum size of 3x3 +using MatrixMax3I = MatrixMax3; + +/// @brief Compute the squared L2 norm of an n-dimensional interval +/// @note This should be used instead of the .squaredNorm() method of Eigen because it avoids negative values in intermediate computations. +/// @param v The n-dimensional interval +/// @return The squared L2 norm of the interval filib::Interval squared_norm(const Eigen::Ref& v); // L2 norm -/// @brief Compute the L2 norm of a 3-dimensional interval -/// @param v The 3-dimensional interval +/// @brief Compute the L2 norm of a n-dimensional interval +/// @note This should be used instead of the .norm() method of Eigen because it avoids negative values in intermediate computations. +/// @param v The n-dimensional interval /// @return The L2 norm of the interval filib::Interval norm(const Eigen::Ref& v); // L2 norm