Merge pull request #513 from skim0119/cleanup-contact

[Bug] Fix sphere-rod contact

Author: skim0119

docs/api/contact.rst

@@ -8,6 +8,12 @@ Contact
 Description
 -----------
 
+.. note::
+   (CAUTION)
+   The contact is recommended to be added at last. This is because contact forces often includes
+   friction that may depend on other normal forces and contraints to be calculated accurately.
+   Be careful on the order of adding interactions.
+
 .. rubric:: Available Contact Classes
 
 .. autosummary::

elastica/__init__.py

@@ -28,8 +28,6 @@
     EndpointForcesSinusoidal,
 )
 from elastica.interaction import (
-    AnisotropicFrictionalPlane,
-    InteractionPlane,
     SlenderBodyTheory,
 )
 from elastica.joint import (

elastica/_contact_functions.py

@@ -4,6 +4,7 @@
     _dot_product,
     _norm,
     _find_min_dist,
+    _find_min_dist_cylinder_sphere,
     _find_slipping_elements,
     _node_to_element_mass_or_force,
     _elements_to_nodes_inplace,
@@ -49,6 +50,13 @@ def _calculate_contact_forces_rod_cylinder(
     velocity_damping_coefficient: np.float64,
     friction_coefficient: np.float64,
 ) -> None:
+    """
+    Calculates the contact forces between a rod and a cylinder.
+
+    This function computes the linear and angular contact forces acting on both the rod and the cylinder
+    when they come into contact. It considers spring-damper-based contact forces as well as friction.
+    The forces are applied to the nodes of the rod and the center of mass of the cylinder.
+    """
     # We already pass in only the first n_elem x
     n_points = x_collection_rod.shape[1]
     cylinder_total_contact_forces = np.zeros((3))
@@ -174,6 +182,9 @@ def _calculate_contact_forces_rod_rod(
     contact_k: np.float64,
     contact_nu: np.float64,
 ) -> None:
+    """
+    Calculates the contact forces between two rods.
+    """
     # We already pass in only the first n_elem x
     n_points_rod_one = x_collection_rod_one.shape[1]
     n_points_rod_two = x_collection_rod_two.shape[1]
@@ -283,6 +294,12 @@ def _calculate_contact_forces_self_rod(
     contact_k: np.float64,
     contact_nu: np.float64,
 ) -> None:
+    """
+    Calculates the self-contact forces within a single rod.
+
+    This function prevents self-penetration of a rod by calculating contact forces between different elements
+    of the same rod. A skip parameter is used to avoid checking adjacent elements.
+    """
     # We already pass in only the first n_elem x
     n_points_rod = x_collection_rod.shape[1]
     edge_collection_rod_one = _batch_product_k_ik_to_ik(length_rod, tangent_rod)
@@ -364,41 +381,44 @@ def _calculate_contact_forces_self_rod(
 def _calculate_contact_forces_rod_sphere(
     x_collection_rod: NDArray[np.float64],
     edge_collection_rod: NDArray[np.float64],
-    x_sphere_center: NDArray[np.float64],
-    x_sphere_tip: NDArray[np.float64],
-    edge_sphere: NDArray[np.float64],
+    x_sphere: NDArray[np.float64],
     radii_sum: NDArray[np.float64],
     length_sum: NDArray[np.float64],
-    internal_forces_rod: NDArray[np.float64],
     external_forces_rod: NDArray[np.float64],
     external_forces_sphere: NDArray[np.float64],
-    external_torques_sphere: NDArray[np.float64],
-    sphere_director_collection: NDArray[np.float64],
     velocity_rod: NDArray[np.float64],
     velocity_sphere: NDArray[np.float64],
     contact_k: np.float64,
     contact_nu: np.float64,
     velocity_damping_coefficient: np.float64,
     friction_coefficient: np.float64,
 ) -> None:
+    """
+    Calculates the contact forces between a rod and a sphere.
+
+    This function computes the linear and angular contact forces acting on both the rod and the sphere
+    when they come into contact. It considers spring-damper-based contact forces as well as friction.
+    The forces are applied to the nodes of the rod and the center of mass of the sphere.
+    """
     # We already pass in only the first n_elem x
     n_points = x_collection_rod.shape[1]
     sphere_total_contact_forces = np.zeros((3))
-    sphere_total_contact_torques = np.zeros((3))
     for i in range(n_points):
         # Element-wise bounding box
         x_selected = x_collection_rod[..., i]
         # x_sphere is already a (,) array from outside
-        del_x = x_selected - x_sphere_tip
+        del_x = x_selected - x_sphere
         norm_del_x = _norm(del_x)
 
         # If outside then don't process
         if norm_del_x >= (radii_sum[i] + length_sum[i]):
             continue
 
         # find the shortest line segment between the two centerline
-        distance_vector, x_sphere_contact_point, _ = _find_min_dist(
-            x_selected, edge_collection_rod[..., i], x_sphere_tip, edge_sphere
+        distance_vector, _, _ = _find_min_dist_cylinder_sphere(
+            x_selected,
+            edge_collection_rod[..., i],
+            x_sphere,
         )
         distance_vector_length = _norm(distance_vector)
         distance_vector /= distance_vector_length
@@ -453,37 +473,22 @@ def _calculate_contact_forces_rod_sphere(
         # Update contact force
         net_contact_force += friction_force
 
-        # Torques acting on the cylinder
-        moment_arm = x_sphere_contact_point - x_sphere_center
-
         # Add it to the rods at the end of the day
         if i == 0:
             external_forces_rod[..., i] -= 2 / 3 * net_contact_force
             external_forces_rod[..., i + 1] -= 4 / 3 * net_contact_force
             sphere_total_contact_forces += 2.0 * net_contact_force
-            sphere_total_contact_torques += np.cross(
-                moment_arm, 2.0 * net_contact_force
-            )
         elif i == n_points - 1:
             external_forces_rod[..., i] -= 4 / 3 * net_contact_force
             external_forces_rod[..., i + 1] -= 2 / 3 * net_contact_force
             sphere_total_contact_forces += 2.0 * net_contact_force
-            sphere_total_contact_torques += np.cross(
-                moment_arm, 2.0 * net_contact_force
-            )
         else:
             external_forces_rod[..., i] -= net_contact_force
             external_forces_rod[..., i + 1] -= net_contact_force
             sphere_total_contact_forces += 2.0 * net_contact_force
-            sphere_total_contact_torques += np.cross(
-                moment_arm, 2.0 * net_contact_force
-            )
 
     # Update the cylinder external forces and torques
     external_forces_sphere[..., 0] += sphere_total_contact_forces
-    external_torques_sphere[..., 0] += (
-        sphere_director_collection @ sphere_total_contact_torques
-    )
 
 
 @njit(cache=True)  # type: ignore
@@ -501,17 +506,16 @@ def _calculate_contact_forces_rod_plane(
     external_forces: NDArray[np.float64],
 ) -> tuple[NDArray[np.float64], NDArray[np.intp]]:
     """
-    This function computes the plane force response on the element, in the
-    case of contact. Contact model given in Eqn 4.8 Gazzola et. al. RSoS 2018 paper
-    is used.
+    Calculates the contact forces between a rod and a plane.
+
+    This function computes the contact force exerted by a flat plane on a rod.
+    It includes a linear spring-damper model for the normal force to handle penetration
+    and a response to prevent the rod from passing through the plane.
+    It returns the magnitude of the plane response force and indices of elements not in contact.
 
-    Parameters
-    ----------
-    system
+    Contact model given in Eqn 4.8 Gazzola et. al. RSoS 2018 paper
+    is used.
 
-    Returns
-    -------
-    magnitude of the plane response
     """
 
     # Compute plane response force
@@ -597,6 +601,9 @@ def _calculate_contact_forces_rod_plane_with_anisotropic_friction(
     internal_torques: NDArray[np.float64],
     external_torques: NDArray[np.float64],
 ) -> None:
+    """
+    Calculates contact forces between a rod and a plane with anisotropic friction.
+    """
     (
         plane_response_force_mag,
         no_contact_point_idx,
@@ -796,6 +803,9 @@ def _calculate_contact_forces_cylinder_plane(
     velocity_collection: NDArray[np.float64],
     external_forces: NDArray[np.float64],
 ) -> tuple[NDArray[np.float64], NDArray[np.intp]]:
+    """
+    Calculates the contact forces between a cylinder and a plane.
+    """
 
     # Compute plane response force
     # total_forces = system.internal_forces + system.external_forces

elastica/contact_forces.py

@@ -433,10 +433,7 @@ def apply_contact(
         ):
             return
 
-        x_sph = (
-            system_two.position_collection[..., 0]
-            - system_two.radius * system_two.director_collection[2, :, 0]
-        )
+        sphere_position = system_two.position_collection[..., 0]
 
         rod_element_position = 0.5 * (
             system_one.position_collection[..., 1:]
@@ -445,16 +442,11 @@ def apply_contact(
         _calculate_contact_forces_rod_sphere(
             rod_element_position,
             system_one.lengths * system_one.tangents,
-            system_two.position_collection[..., 0],
-            x_sph,
-            system_two.radius * system_two.director_collection[2, :, 0],
+            sphere_position,
             system_one.radius + system_two.radius,
             system_one.lengths + 2 * system_two.radius,
-            system_one.internal_forces,
             system_one.external_forces,
             system_two.external_forces,
-            system_two.external_torques,
-            system_two.director_collection[:, :, 0],
             system_one.velocity_collection,
             system_two.velocity_collection,
             self.k,

elastica/contact_utils.py

@@ -41,6 +41,9 @@ def _find_min_dist(
     x2: NDArray[np.float64],
     e2: NDArray[np.float64],
 ) -> tuple[NDArray[np.float64], NDArray[np.float64], NDArray[np.float64]]:
+    """
+    Find the minimum distance between two centerline segments: (x1, edge1) and (x2, edge2).
+    """
     e1e1 = _dot_product(e1, e1)  # type: ignore
     e1e2 = _dot_product(e1, e2)  # type: ignore
     e2e2 = _dot_product(e2, e2)  # type: ignore
@@ -105,11 +108,32 @@ def _find_min_dist(
     return x2 + s * e2 - x1 - t * e1, x2 + s * e2, x1 - t * e1
 
 
+@numba.njit(cache=True)  # type: ignore
+def _find_min_dist_cylinder_sphere(
+    x1: NDArray[np.float64],
+    e1: NDArray[np.float64],
+    x2: NDArray[np.float64],
+) -> tuple[NDArray[np.float64], NDArray[np.float64], NDArray[np.float64]]:
+    """
+    Find the minimum distance between centerline segment and point: (x1, edge1) and (x2).
+    """
+    e1e1 = _dot_product(e1, e1)  # type: ignore
+    x1e1 = _dot_product(x1, e1)  # type: ignore
+    x2e1 = _dot_product(e1, x2)  # type: ignore
+
+    # Parametrization
+    t = (x2e1 - x1e1) / e1e1  # Comes from taking dot of e1 with a normal
+    t = _clip(t, 0.0, 1.0)
+
+    # Return distance, contact point of system 2, contact point of system 1
+    return x2 - x1 - t * e1, x2, x1 - t * e1
+
+
 @numba.njit(cache=True)  # type: ignore
 def _aabbs_not_intersecting(
     aabb_one: NDArray[np.float64], aabb_two: NDArray[np.float64]
 ) -> Literal[1, 0]:
-    """Returns true if not intersecting else false"""
+    """Checks if two axis-aligned bounding boxes (AABBs) are not intersecting."""
     if (aabb_one[0, 1] < aabb_two[0, 0]) | (aabb_one[0, 0] > aabb_two[0, 1]):
         return 1
     if (aabb_one[1, 1] < aabb_two[1, 0]) | (aabb_one[1, 0] > aabb_two[1, 1]):
@@ -130,6 +154,13 @@ def _prune_using_aabbs_rod_cylinder(
     cylinder_radius: NDArray[np.float64],
     cylinder_length: NDArray[np.float64],
 ) -> Literal[1, 0]:
+    """
+    Prunes broad-phase collision detection between a rod and a cylinder using AABBs.
+
+    This function checks for intersection between the axis-aligned bounding boxes (AABBs)
+    of a rod and a cylinder. It's a quick way to rule out collision without performing
+    a more detailed and expensive check.
+    """
     max_possible_dimension = np.zeros((3,))
     aabb_rod = np.empty((3, 2))
     aabb_cylinder = np.empty((3, 2))
@@ -171,6 +202,13 @@ def _prune_using_aabbs_rod_rod(
     rod_two_radius_collection: NDArray[np.float64],
     rod_two_length_collection: NDArray[np.float64],
 ) -> Literal[1, 0]:
+    """
+    Prunes broad-phase collision detection between two rods using AABBs.
+
+    This function checks for intersection between the axis-aligned bounding boxes (AABBs)
+    of two rods. It's a quick way to rule out collision without performing
+    a more detailed and expensive check.
+    """
     max_possible_dimension = np.zeros((3,))
     aabb_rod_one = np.empty((3, 2))
     aabb_rod_two = np.empty((3, 2))
@@ -209,33 +247,29 @@ def _prune_using_aabbs_rod_sphere(
     sphere_director: NDArray[np.float64],
     sphere_radius: NDArray[np.float64],
 ) -> Literal[1, 0]:
-    max_possible_dimension = np.zeros((3,))
+    """
+    Prunes broad-phase collision detection between a rod and a sphere using AABBs.
+
+    This function checks for intersection between the axis-aligned bounding boxes (AABBs)
+    of a rod and a sphere. It's a quick way to rule out collision without performing
+    a more detailed and expensive check.
+    """
+    # AABB for rod
     aabb_rod = np.empty((3, 2))
-    aabb_sphere = np.empty((3, 2))
-    max_possible_dimension[...] = np.max(rod_one_radius_collection) + np.max(
-        rod_one_length_collection
-    )
+    # Taking max radius of rod
+    max_rod_radius = np.max(rod_one_radius_collection)
     for i in range(3):
-        aabb_rod[i, 0] = (
-            np.min(rod_one_position_collection[i]) - max_possible_dimension[i]
-        )
-        aabb_rod[i, 1] = (
-            np.max(rod_one_position_collection[i]) + max_possible_dimension[i]
-        )
+        aabb_rod[i, 0] = np.min(rod_one_position_collection[i]) - max_rod_radius
+        aabb_rod[i, 1] = np.max(rod_one_position_collection[i]) + max_rod_radius
 
-    sphere_dimensions_in_local_FOR = np.array(
-        [sphere_radius, sphere_radius, sphere_radius]
-    )
-    sphere_dimensions_in_world_FOR = np.zeros_like(sphere_dimensions_in_local_FOR)
+    # AABB for sphere
+    aabb_sphere = np.empty((3, 2))
+    # A sphere is symmetrical, so its AABB is easy to compute.
+    # The director is not needed.
     for i in range(3):
-        for j in range(3):
-            sphere_dimensions_in_world_FOR[i] += (
-                sphere_director[j, i, 0] * sphere_dimensions_in_local_FOR[j]
-            )
+        aabb_sphere[i, 0] = sphere_position[i, 0] - sphere_radius
+        aabb_sphere[i, 1] = sphere_position[i, 0] + sphere_radius
 
-    max_possible_dimension = np.abs(sphere_dimensions_in_world_FOR)
-    aabb_sphere[..., 0] = sphere_position[..., 0] - max_possible_dimension
-    aabb_sphere[..., 1] = sphere_position[..., 0] + max_possible_dimension
     return _aabbs_not_intersecting(aabb_sphere, aabb_rod)