diff --git a/elastica/_contact_functions.py b/elastica/_contact_functions.py new file mode 100644 index 000000000..7e40a7f98 --- /dev/null +++ b/elastica/_contact_functions.py @@ -0,0 +1,782 @@ +__doc__ = """ Numba implementation module containing contact force calculation functions between rods and rigid bodies and other rods, rigid bodies or surfaces.""" + +from elastica.contact_utils import ( + _dot_product, + _norm, + _find_min_dist, + _find_slipping_elements, + _node_to_element_mass_or_force, + _elements_to_nodes_inplace, + _node_to_element_position, + _node_to_element_velocity, +) +from elastica._linalg import ( + _batch_matvec, + _batch_cross, + _batch_norm, + _batch_dot, + _batch_product_i_k_to_ik, + _batch_product_i_ik_to_k, + _batch_product_k_ik_to_ik, + _batch_vector_sum, + _batch_matrix_transpose, + _batch_vec_oneD_vec_cross, +) +import numba +import numpy as np + + +@numba.njit(cache=True) +def _calculate_contact_forces_rod_cylinder( + x_collection_rod, + edge_collection_rod, + x_cylinder_center, + x_cylinder_tip, + edge_cylinder, + radii_sum, + length_sum, + internal_forces_rod, + external_forces_rod, + external_forces_cylinder, + external_torques_cylinder, + cylinder_director_collection, + velocity_rod, + velocity_cylinder, + contact_k, + contact_nu, + velocity_damping_coefficient, + friction_coefficient, +) -> None: + # We already pass in only the first n_elem x + n_points = x_collection_rod.shape[1] + cylinder_total_contact_forces = np.zeros((3)) + cylinder_total_contact_torques = np.zeros((3)) + for i in range(n_points): + # Element-wise bounding box + x_selected = x_collection_rod[..., i] + # x_cylinder is already a (,) array from outised + del_x = x_selected - x_cylinder_tip + 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 + # segments : differs from normal cylinder-cylinder intersection + distance_vector, x_cylinder_contact_point, _ = _find_min_dist( + x_selected, edge_collection_rod[..., i], x_cylinder_tip, edge_cylinder + ) + distance_vector_length = _norm(distance_vector) + distance_vector /= distance_vector_length + + gamma = radii_sum[i] - distance_vector_length + + # If distance is large, don't worry about it + if gamma < -1e-5: + continue + + # CHECK FOR GAMMA > 0.0, heaviside but we need to overload it in numba + # As a quick fix, use this instead + mask = (gamma > 0.0) * 1.0 + + # Compute contact spring force + contact_force = contact_k * gamma * distance_vector + interpenetration_velocity = velocity_cylinder[..., 0] - 0.5 * ( + velocity_rod[..., i] + velocity_rod[..., i + 1] + ) + # Compute contact damping + normal_interpenetration_velocity = ( + _dot_product(interpenetration_velocity, distance_vector) * distance_vector + ) + contact_damping_force = -contact_nu * normal_interpenetration_velocity + + # magnitude* direction + net_contact_force = 0.5 * mask * (contact_damping_force + contact_force) + + # Compute friction + slip_interpenetration_velocity = ( + interpenetration_velocity - normal_interpenetration_velocity + ) + slip_interpenetration_velocity_mag = np.linalg.norm( + slip_interpenetration_velocity + ) + slip_interpenetration_velocity_unitized = slip_interpenetration_velocity / ( + slip_interpenetration_velocity_mag + 1e-14 + ) + # Compute friction force in the slip direction. + damping_force_in_slip_direction = ( + velocity_damping_coefficient * slip_interpenetration_velocity_mag + ) + # Compute Coulombic friction + coulombic_friction_force = friction_coefficient * np.linalg.norm( + net_contact_force + ) + # Compare damping force in slip direction and kinetic friction and minimum is the friction force. + friction_force = ( + -min(damping_force_in_slip_direction, coulombic_friction_force) + * slip_interpenetration_velocity_unitized + ) + # Update contact force + net_contact_force += friction_force + + # Torques acting on the cylinder + moment_arm = x_cylinder_contact_point - x_cylinder_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 + cylinder_total_contact_forces += 2.0 * net_contact_force + cylinder_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 + cylinder_total_contact_forces += 2.0 * net_contact_force + cylinder_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 + cylinder_total_contact_forces += 2.0 * net_contact_force + cylinder_total_contact_torques += np.cross( + moment_arm, 2.0 * net_contact_force + ) + + # Update the cylinder external forces and torques + external_forces_cylinder[..., 0] += cylinder_total_contact_forces + external_torques_cylinder[..., 0] += ( + cylinder_director_collection @ cylinder_total_contact_torques + ) + + +@numba.njit(cache=True) +def _calculate_contact_forces_rod_rod( + x_collection_rod_one, + radius_rod_one, + length_rod_one, + tangent_rod_one, + velocity_rod_one, + internal_forces_rod_one, + external_forces_rod_one, + x_collection_rod_two, + radius_rod_two, + length_rod_two, + tangent_rod_two, + velocity_rod_two, + internal_forces_rod_two, + external_forces_rod_two, + contact_k, + contact_nu, +) -> None: + # 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] + edge_collection_rod_one = _batch_product_k_ik_to_ik(length_rod_one, tangent_rod_one) + edge_collection_rod_two = _batch_product_k_ik_to_ik(length_rod_two, tangent_rod_two) + + for i in range(n_points_rod_one): + for j in range(n_points_rod_two): + radii_sum = radius_rod_one[i] + radius_rod_two[j] + length_sum = length_rod_one[i] + length_rod_two[j] + # Element-wise bounding box + x_selected_rod_one = x_collection_rod_one[..., i] + x_selected_rod_two = x_collection_rod_two[..., j] + + del_x = x_selected_rod_one - x_selected_rod_two + norm_del_x = _norm(del_x) + + # If outside then don't process + if norm_del_x >= (radii_sum + length_sum): + continue + + # find the shortest line segment between the two centerline + # segments : differs from normal cylinder-cylinder intersection + distance_vector, _, _ = _find_min_dist( + x_selected_rod_one, + edge_collection_rod_one[..., i], + x_selected_rod_two, + edge_collection_rod_two[..., j], + ) + distance_vector_length = _norm(distance_vector) + distance_vector /= distance_vector_length + gamma = radii_sum - distance_vector_length + + # If distance is large, don't worry about it + if gamma < -1e-5: + continue + + rod_one_elemental_forces = 0.5 * ( + external_forces_rod_one[..., i] + + external_forces_rod_one[..., i + 1] + + internal_forces_rod_one[..., i] + + internal_forces_rod_one[..., i + 1] + ) + + rod_two_elemental_forces = 0.5 * ( + external_forces_rod_two[..., j] + + external_forces_rod_two[..., j + 1] + + internal_forces_rod_two[..., j] + + internal_forces_rod_two[..., j + 1] + ) + + equilibrium_forces = -rod_one_elemental_forces + rod_two_elemental_forces + + """FIX ME: Remove normal force and tune rod-rod contact example""" + normal_force = _dot_product(equilibrium_forces, distance_vector) + # Following line same as np.where(normal_force < 0.0, -normal_force, 0.0) + normal_force = abs(min(normal_force, 0.0)) + + # CHECK FOR GAMMA > 0.0, heaviside but we need to overload it in numba + # As a quick fix, use this instead + mask = (gamma > 0.0) * 1.0 + + contact_force = contact_k * gamma + interpenetration_velocity = 0.5 * ( + (velocity_rod_one[..., i] + velocity_rod_one[..., i + 1]) + - (velocity_rod_two[..., j] + velocity_rod_two[..., j + 1]) + ) + contact_damping_force = contact_nu * _dot_product( + interpenetration_velocity, distance_vector + ) + + # magnitude* direction + net_contact_force = ( + normal_force + 0.5 * mask * (contact_damping_force + contact_force) + ) * distance_vector + + # Add it to the rods at the end of the day + if i == 0: + external_forces_rod_one[..., i] -= net_contact_force * 2 / 3 + external_forces_rod_one[..., i + 1] -= net_contact_force * 4 / 3 + elif i == n_points_rod_one - 1: + external_forces_rod_one[..., i] -= net_contact_force * 4 / 3 + external_forces_rod_one[..., i + 1] -= net_contact_force * 2 / 3 + else: + external_forces_rod_one[..., i] -= net_contact_force + external_forces_rod_one[..., i + 1] -= net_contact_force + + if j == 0: + external_forces_rod_two[..., j] += net_contact_force * 2 / 3 + external_forces_rod_two[..., j + 1] += net_contact_force * 4 / 3 + elif j == n_points_rod_two - 1: + external_forces_rod_two[..., j] += net_contact_force * 4 / 3 + external_forces_rod_two[..., j + 1] += net_contact_force * 2 / 3 + else: + external_forces_rod_two[..., j] += net_contact_force + external_forces_rod_two[..., j + 1] += net_contact_force + + +@numba.njit(cache=True) +def _calculate_contact_forces_self_rod( + x_collection_rod, + radius_rod, + length_rod, + tangent_rod, + velocity_rod, + external_forces_rod, + contact_k, + contact_nu, +) -> None: + # 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) + + for i in range(n_points_rod): + skip = int(1 + np.ceil(0.8 * np.pi * radius_rod[i] / length_rod[i])) + for j in range(i - skip, -1, -1): + radii_sum = radius_rod[i] + radius_rod[j] + length_sum = length_rod[i] + length_rod[j] + # Element-wise bounding box + x_selected_rod_index_i = x_collection_rod[..., i] + x_selected_rod_index_j = x_collection_rod[..., j] + + del_x = x_selected_rod_index_i - x_selected_rod_index_j + norm_del_x = _norm(del_x) + + # If outside then don't process + if norm_del_x >= (radii_sum + length_sum): + continue + + # find the shortest line segment between the two centerline + # segments : differs from normal cylinder-cylinder intersection + distance_vector, _, _ = _find_min_dist( + x_selected_rod_index_i, + edge_collection_rod_one[..., i], + x_selected_rod_index_j, + edge_collection_rod_one[..., j], + ) + distance_vector_length = _norm(distance_vector) + distance_vector /= distance_vector_length + + gamma = radii_sum - distance_vector_length + + # If distance is large, don't worry about it + if gamma < -1e-5: + continue + + # CHECK FOR GAMMA > 0.0, heaviside but we need to overload it in numba + # As a quick fix, use this instead + mask = (gamma > 0.0) * 1.0 + + contact_force = contact_k * gamma + interpenetration_velocity = 0.5 * ( + (velocity_rod[..., i] + velocity_rod[..., i + 1]) + - (velocity_rod[..., j] + velocity_rod[..., j + 1]) + ) + contact_damping_force = contact_nu * _dot_product( + interpenetration_velocity, distance_vector + ) + + # magnitude* direction + net_contact_force = ( + 0.5 * mask * (contact_damping_force + contact_force) + ) * distance_vector + + # Add it to the rods at the end of the day + # if i == 0: + # external_forces_rod[...,i] -= net_contact_force *2/3 + # external_forces_rod[...,i+1] -= net_contact_force * 4/3 + if i == n_points_rod - 1: + external_forces_rod[..., i] -= net_contact_force * 4 / 3 + external_forces_rod[..., i + 1] -= net_contact_force * 2 / 3 + else: + external_forces_rod[..., i] -= net_contact_force + external_forces_rod[..., i + 1] -= net_contact_force + + if j == 0: + external_forces_rod[..., j] += net_contact_force * 2 / 3 + external_forces_rod[..., j + 1] += net_contact_force * 4 / 3 + # elif j == n_points_rod: + # external_forces_rod[..., j] += net_contact_force * 4/3 + # external_forces_rod[..., j+1] += net_contact_force * 2/3 + else: + external_forces_rod[..., j] += net_contact_force + external_forces_rod[..., j + 1] += net_contact_force + + +@numba.njit(cache=True) +def _calculate_contact_forces_rod_sphere( + x_collection_rod, + edge_collection_rod, + x_sphere_center, + x_sphere_tip, + edge_sphere, + radii_sum, + length_sum, + internal_forces_rod, + external_forces_rod, + external_forces_sphere, + external_torques_sphere, + sphere_director_collection, + velocity_rod, + velocity_sphere, + contact_k, + contact_nu, + velocity_damping_coefficient, + friction_coefficient, +) -> None: + # 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 + 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_length = _norm(distance_vector) + distance_vector /= distance_vector_length + + gamma = radii_sum[i] - distance_vector_length + + # If distance is large, don't worry about it + if gamma < -1e-5: + continue + + # CHECK FOR GAMMA > 0.0, heaviside but we need to overload it in numba + # As a quick fix, use this instead + mask = (gamma > 0.0) * 1.0 + + # Compute contact spring force + contact_force = contact_k * gamma * distance_vector + interpenetration_velocity = velocity_sphere[..., 0] - 0.5 * ( + velocity_rod[..., i] + velocity_rod[..., i + 1] + ) + # Compute contact damping + normal_interpenetration_velocity = ( + _dot_product(interpenetration_velocity, distance_vector) * distance_vector + ) + contact_damping_force = -contact_nu * normal_interpenetration_velocity + + # magnitude* direction + net_contact_force = 0.5 * mask * (contact_damping_force + contact_force) + + # Compute friction + slip_interpenetration_velocity = ( + interpenetration_velocity - normal_interpenetration_velocity + ) + slip_interpenetration_velocity_mag = np.linalg.norm( + slip_interpenetration_velocity + ) + slip_interpenetration_velocity_unitized = slip_interpenetration_velocity / ( + slip_interpenetration_velocity_mag + 1e-14 + ) + # Compute friction force in the slip direction. + damping_force_in_slip_direction = ( + velocity_damping_coefficient * slip_interpenetration_velocity_mag + ) + # Compute Coulombic friction + coulombic_friction_force = friction_coefficient * np.linalg.norm( + net_contact_force + ) + # Compare damping force in slip direction and kinetic friction and minimum is the friction force. + friction_force = ( + -min(damping_force_in_slip_direction, coulombic_friction_force) + * slip_interpenetration_velocity_unitized + ) + # 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 + ) + + +@numba.njit(cache=True) +def _calculate_contact_forces_rod_plane( + plane_origin, + plane_normal, + surface_tol, + k, + nu, + radius, + mass, + position_collection, + velocity_collection, + internal_forces, + external_forces, +): + """ + 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. + + Parameters + ---------- + system + + Returns + ------- + magnitude of the plane response + """ + + # Compute plane response force + nodal_total_forces = _batch_vector_sum(internal_forces, external_forces) + element_total_forces = _node_to_element_mass_or_force(nodal_total_forces) + + force_component_along_normal_direction = _batch_product_i_ik_to_k( + plane_normal, element_total_forces + ) + forces_along_normal_direction = _batch_product_i_k_to_ik( + plane_normal, force_component_along_normal_direction + ) + + # If the total force component along the plane normal direction is greater than zero that means, + # total force is pushing rod away from the plane not towards the plane. Thus, response force + # applied by the surface has to be zero. + forces_along_normal_direction[ + ..., np.where(force_component_along_normal_direction > 0)[0] + ] = 0.0 + # Compute response force on the element. Plane response force + # has to be away from the surface and towards the element. Thus + # multiply forces along normal direction with negative sign. + plane_response_force = -forces_along_normal_direction + + # Elastic force response due to penetration + element_position = _node_to_element_position(position_collection) + distance_from_plane = _batch_product_i_ik_to_k( + plane_normal, (element_position - plane_origin) + ) + plane_penetration = np.minimum(distance_from_plane - radius, 0.0) + elastic_force = -k * _batch_product_i_k_to_ik(plane_normal, plane_penetration) + + # Damping force response due to velocity towards the plane + element_velocity = _node_to_element_velocity( + mass=mass, node_velocity_collection=velocity_collection + ) + normal_component_of_element_velocity = _batch_product_i_ik_to_k( + plane_normal, element_velocity + ) + damping_force = -nu * _batch_product_i_k_to_ik( + plane_normal, normal_component_of_element_velocity + ) + + # Compute total plane response force + plane_response_force_total = plane_response_force + elastic_force + damping_force + + # Check if the rod elements are in contact with plane. + no_contact_point_idx = np.where((distance_from_plane - radius) > surface_tol)[0] + # If rod element does not have any contact with plane, plane cannot apply response + # force on the element. Thus lets set plane response force to 0.0 for the no contact points. + plane_response_force[..., no_contact_point_idx] = 0.0 + plane_response_force_total[..., no_contact_point_idx] = 0.0 + + # Update the external forces + _elements_to_nodes_inplace(plane_response_force_total, external_forces) + + return (_batch_norm(plane_response_force), no_contact_point_idx) + + +@numba.njit(cache=True) +def _calculate_contact_forces_rod_plane_with_anisotropic_friction( + plane_origin, + plane_normal, + surface_tol, + slip_velocity_tol, + k, + nu, + kinetic_mu_forward, + kinetic_mu_backward, + kinetic_mu_sideways, + static_mu_forward, + static_mu_backward, + static_mu_sideways, + radius, + mass, + tangents, + position_collection, + director_collection, + velocity_collection, + omega_collection, + internal_forces, + external_forces, + internal_torques, + external_torques, +): + ( + plane_response_force_mag, + no_contact_point_idx, + ) = _calculate_contact_forces_rod_plane( + plane_origin, + plane_normal, + surface_tol, + k, + nu, + radius, + mass, + position_collection, + velocity_collection, + internal_forces, + external_forces, + ) + + # First compute component of rod tangent in plane. Because friction forces acts in plane not out of plane. Thus + # axial direction has to be in plane, it cannot be out of plane. We are projecting rod element tangent vector in + # to the plane. So friction forces can only be in plane forces and not out of plane. + tangent_along_normal_direction = _batch_product_i_ik_to_k(plane_normal, tangents) + tangent_perpendicular_to_normal_direction = tangents - _batch_product_i_k_to_ik( + plane_normal, tangent_along_normal_direction + ) + tangent_perpendicular_to_normal_direction_mag = _batch_norm( + tangent_perpendicular_to_normal_direction + ) + # Normalize tangent_perpendicular_to_normal_direction. This is axial direction for plane. Here we are adding + # small tolerance (1e-10) for normalization, in order to prevent division by 0. + axial_direction = _batch_product_k_ik_to_ik( + 1 / (tangent_perpendicular_to_normal_direction_mag + 1e-14), + tangent_perpendicular_to_normal_direction, + ) + element_velocity = _node_to_element_velocity( + mass=mass, node_velocity_collection=velocity_collection + ) + # first apply axial kinetic friction + velocity_mag_along_axial_direction = _batch_dot(element_velocity, axial_direction) + velocity_along_axial_direction = _batch_product_k_ik_to_ik( + velocity_mag_along_axial_direction, axial_direction + ) + + # Friction forces depends on the direction of velocity, in other words sign + # of the velocity vector. + velocity_sign_along_axial_direction = np.sign(velocity_mag_along_axial_direction) + # Check top for sign convention + kinetic_mu = 0.5 * ( + kinetic_mu_forward * (1 + velocity_sign_along_axial_direction) + + kinetic_mu_backward * (1 - velocity_sign_along_axial_direction) + ) + # Call slip function to check if elements slipping or not + slip_function_along_axial_direction = _find_slipping_elements( + velocity_along_axial_direction, slip_velocity_tol + ) + # Now rolling kinetic friction + rolling_direction = _batch_vec_oneD_vec_cross(axial_direction, plane_normal) + torque_arm = _batch_product_i_k_to_ik(-plane_normal, radius) + velocity_along_rolling_direction = _batch_dot(element_velocity, rolling_direction) + directors_transpose = _batch_matrix_transpose(director_collection) + # w_rot = Q.T @ omega @ Q @ r + rotation_velocity = _batch_matvec( + directors_transpose, + _batch_cross(omega_collection, _batch_matvec(director_collection, torque_arm)), + ) + rotation_velocity_along_rolling_direction = _batch_dot( + rotation_velocity, rolling_direction + ) + slip_velocity_mag_along_rolling_direction = ( + velocity_along_rolling_direction + rotation_velocity_along_rolling_direction + ) + slip_velocity_along_rolling_direction = _batch_product_k_ik_to_ik( + slip_velocity_mag_along_rolling_direction, rolling_direction + ) + slip_function_along_rolling_direction = _find_slipping_elements( + slip_velocity_along_rolling_direction, slip_velocity_tol + ) + # Compute unitized total slip velocity vector. We will use this to distribute the weight of the rod in axial + # and rolling directions. + unitized_total_velocity = ( + slip_velocity_along_rolling_direction + velocity_along_axial_direction + ) + unitized_total_velocity /= _batch_norm(unitized_total_velocity + 1e-14) + # Apply kinetic friction in axial direction. + kinetic_friction_force_along_axial_direction = -( + (1.0 - slip_function_along_axial_direction) + * kinetic_mu + * plane_response_force_mag + * _batch_dot(unitized_total_velocity, axial_direction) + * axial_direction + ) + # If rod element does not have any contact with plane, plane cannot apply friction + # force on the element. Thus lets set kinetic friction force to 0.0 for the no contact points. + kinetic_friction_force_along_axial_direction[..., no_contact_point_idx] = 0.0 + _elements_to_nodes_inplace( + kinetic_friction_force_along_axial_direction, external_forces + ) + # Apply kinetic friction in rolling direction. + kinetic_friction_force_along_rolling_direction = -( + (1.0 - slip_function_along_rolling_direction) + * kinetic_mu_sideways + * plane_response_force_mag + * _batch_dot(unitized_total_velocity, rolling_direction) + * rolling_direction + ) + # If rod element does not have any contact with plane, plane cannot apply friction + # force on the element. Thus lets set kinetic friction force to 0.0 for the no contact points. + kinetic_friction_force_along_rolling_direction[..., no_contact_point_idx] = 0.0 + _elements_to_nodes_inplace( + kinetic_friction_force_along_rolling_direction, external_forces + ) + # torque = Q @ r @ Fr + external_torques += _batch_matvec( + director_collection, + _batch_cross(torque_arm, kinetic_friction_force_along_rolling_direction), + ) + + # now axial static friction + nodal_total_forces = _batch_vector_sum(internal_forces, external_forces) + element_total_forces = _node_to_element_mass_or_force(nodal_total_forces) + force_component_along_axial_direction = _batch_dot( + element_total_forces, axial_direction + ) + force_component_sign_along_axial_direction = np.sign( + force_component_along_axial_direction + ) + # check top for sign convention + static_mu = 0.5 * ( + static_mu_forward * (1 + force_component_sign_along_axial_direction) + + static_mu_backward * (1 - force_component_sign_along_axial_direction) + ) + max_friction_force = ( + slip_function_along_axial_direction * static_mu * plane_response_force_mag + ) + # friction = min(mu N, pushing force) + static_friction_force_along_axial_direction = -( + np.minimum(np.fabs(force_component_along_axial_direction), max_friction_force) + * force_component_sign_along_axial_direction + * axial_direction + ) + # If rod element does not have any contact with plane, plane cannot apply friction + # force on the element. Thus lets set static friction force to 0.0 for the no contact points. + static_friction_force_along_axial_direction[..., no_contact_point_idx] = 0.0 + _elements_to_nodes_inplace( + static_friction_force_along_axial_direction, external_forces + ) + + # now rolling static friction + # there is some normal, tangent and rolling directions inconsitency from Elastica + total_torques = _batch_matvec( + directors_transpose, (internal_torques + external_torques) + ) + # Elastica has opposite defs of tangents in interaction.h and rod.cpp + total_torques_along_axial_direction = _batch_dot(total_torques, axial_direction) + force_component_along_rolling_direction = _batch_dot( + element_total_forces, rolling_direction + ) + noslip_force = -( + ( + radius * force_component_along_rolling_direction + - 2.0 * total_torques_along_axial_direction + ) + / 3.0 + / radius + ) + max_friction_force = ( + slip_function_along_rolling_direction + * static_mu_sideways + * plane_response_force_mag + ) + noslip_force_sign = np.sign(noslip_force) + static_friction_force_along_rolling_direction = ( + np.minimum(np.fabs(noslip_force), max_friction_force) + * noslip_force_sign + * rolling_direction + ) + # If rod element does not have any contact with plane, plane cannot apply friction + # force on the element. Thus lets set plane static friction force to 0.0 for the no contact points. + static_friction_force_along_rolling_direction[..., no_contact_point_idx] = 0.0 + _elements_to_nodes_inplace( + static_friction_force_along_rolling_direction, external_forces + ) + external_torques += _batch_matvec( + director_collection, + _batch_cross(torque_arm, static_friction_force_along_rolling_direction), + ) diff --git a/elastica/contact_forces.py b/elastica/contact_forces.py index 80caa522c..83657ce65 100644 --- a/elastica/contact_forces.py +++ b/elastica/contact_forces.py @@ -5,31 +5,18 @@ from elastica.rigidbody import Cylinder, Sphere from elastica.surface import Plane from elastica.contact_utils import ( - _dot_product, - _norm, - _find_min_dist, _prune_using_aabbs_rod_cylinder, _prune_using_aabbs_rod_rod, _prune_using_aabbs_rod_sphere, - _find_slipping_elements, - _node_to_element_mass_or_force, - _elements_to_nodes_inplace, - _node_to_element_position, - _node_to_element_velocity, ) -from elastica._linalg import ( - _batch_matvec, - _batch_cross, - _batch_norm, - _batch_dot, - _batch_product_i_k_to_ik, - _batch_product_i_ik_to_k, - _batch_product_k_ik_to_ik, - _batch_vector_sum, - _batch_matrix_transpose, - _batch_vec_oneD_vec_cross, +from elastica._contact_functions import ( + _calculate_contact_forces_rod_cylinder, + _calculate_contact_forces_rod_rod, + _calculate_contact_forces_self_rod, + _calculate_contact_forces_rod_sphere, + _calculate_contact_forces_rod_plane, + _calculate_contact_forces_rod_plane_with_anisotropic_friction, ) -import numba import numpy as np @@ -97,762 +84,6 @@ def apply_contact( pass -@numba.njit(cache=True) -def _calculate_contact_forces_rod_cylinder( - x_collection_rod, - edge_collection_rod, - x_cylinder_center, - x_cylinder_tip, - edge_cylinder, - radii_sum, - length_sum, - internal_forces_rod, - external_forces_rod, - external_forces_cylinder, - external_torques_cylinder, - cylinder_director_collection, - velocity_rod, - velocity_cylinder, - contact_k, - contact_nu, - velocity_damping_coefficient, - friction_coefficient, -) -> None: - # We already pass in only the first n_elem x - n_points = x_collection_rod.shape[1] - cylinder_total_contact_forces = np.zeros((3)) - cylinder_total_contact_torques = np.zeros((3)) - for i in range(n_points): - # Element-wise bounding box - x_selected = x_collection_rod[..., i] - # x_cylinder is already a (,) array from outised - del_x = x_selected - x_cylinder_tip - 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 - # segments : differs from normal cylinder-cylinder intersection - distance_vector, x_cylinder_contact_point, _ = _find_min_dist( - x_selected, edge_collection_rod[..., i], x_cylinder_tip, edge_cylinder - ) - distance_vector_length = _norm(distance_vector) - distance_vector /= distance_vector_length - - gamma = radii_sum[i] - distance_vector_length - - # If distance is large, don't worry about it - if gamma < -1e-5: - continue - - # CHECK FOR GAMMA > 0.0, heaviside but we need to overload it in numba - # As a quick fix, use this instead - mask = (gamma > 0.0) * 1.0 - - # Compute contact spring force - contact_force = contact_k * gamma * distance_vector - interpenetration_velocity = velocity_cylinder[..., 0] - 0.5 * ( - velocity_rod[..., i] + velocity_rod[..., i + 1] - ) - # Compute contact damping - normal_interpenetration_velocity = ( - _dot_product(interpenetration_velocity, distance_vector) * distance_vector - ) - contact_damping_force = -contact_nu * normal_interpenetration_velocity - - # magnitude* direction - net_contact_force = 0.5 * mask * (contact_damping_force + contact_force) - - # Compute friction - slip_interpenetration_velocity = ( - interpenetration_velocity - normal_interpenetration_velocity - ) - slip_interpenetration_velocity_mag = np.linalg.norm( - slip_interpenetration_velocity - ) - slip_interpenetration_velocity_unitized = slip_interpenetration_velocity / ( - slip_interpenetration_velocity_mag + 1e-14 - ) - # Compute friction force in the slip direction. - damping_force_in_slip_direction = ( - velocity_damping_coefficient * slip_interpenetration_velocity_mag - ) - # Compute Coulombic friction - coulombic_friction_force = friction_coefficient * np.linalg.norm( - net_contact_force - ) - # Compare damping force in slip direction and kinetic friction and minimum is the friction force. - friction_force = ( - -min(damping_force_in_slip_direction, coulombic_friction_force) - * slip_interpenetration_velocity_unitized - ) - # Update contact force - net_contact_force += friction_force - - # Torques acting on the cylinder - moment_arm = x_cylinder_contact_point - x_cylinder_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 - cylinder_total_contact_forces += 2.0 * net_contact_force - cylinder_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 - cylinder_total_contact_forces += 2.0 * net_contact_force - cylinder_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 - cylinder_total_contact_forces += 2.0 * net_contact_force - cylinder_total_contact_torques += np.cross( - moment_arm, 2.0 * net_contact_force - ) - - # Update the cylinder external forces and torques - external_forces_cylinder[..., 0] += cylinder_total_contact_forces - external_torques_cylinder[..., 0] += ( - cylinder_director_collection @ cylinder_total_contact_torques - ) - - -@numba.njit(cache=True) -def _calculate_contact_forces_rod_rod( - x_collection_rod_one, - radius_rod_one, - length_rod_one, - tangent_rod_one, - velocity_rod_one, - internal_forces_rod_one, - external_forces_rod_one, - x_collection_rod_two, - radius_rod_two, - length_rod_two, - tangent_rod_two, - velocity_rod_two, - internal_forces_rod_two, - external_forces_rod_two, - contact_k, - contact_nu, -) -> None: - # 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] - edge_collection_rod_one = _batch_product_k_ik_to_ik(length_rod_one, tangent_rod_one) - edge_collection_rod_two = _batch_product_k_ik_to_ik(length_rod_two, tangent_rod_two) - - for i in range(n_points_rod_one): - for j in range(n_points_rod_two): - radii_sum = radius_rod_one[i] + radius_rod_two[j] - length_sum = length_rod_one[i] + length_rod_two[j] - # Element-wise bounding box - x_selected_rod_one = x_collection_rod_one[..., i] - x_selected_rod_two = x_collection_rod_two[..., j] - - del_x = x_selected_rod_one - x_selected_rod_two - norm_del_x = _norm(del_x) - - # If outside then don't process - if norm_del_x >= (radii_sum + length_sum): - continue - - # find the shortest line segment between the two centerline - # segments : differs from normal cylinder-cylinder intersection - distance_vector, _, _ = _find_min_dist( - x_selected_rod_one, - edge_collection_rod_one[..., i], - x_selected_rod_two, - edge_collection_rod_two[..., j], - ) - distance_vector_length = _norm(distance_vector) - distance_vector /= distance_vector_length - gamma = radii_sum - distance_vector_length - - # If distance is large, don't worry about it - if gamma < -1e-5: - continue - - rod_one_elemental_forces = 0.5 * ( - external_forces_rod_one[..., i] - + external_forces_rod_one[..., i + 1] - + internal_forces_rod_one[..., i] - + internal_forces_rod_one[..., i + 1] - ) - - rod_two_elemental_forces = 0.5 * ( - external_forces_rod_two[..., j] - + external_forces_rod_two[..., j + 1] - + internal_forces_rod_two[..., j] - + internal_forces_rod_two[..., j + 1] - ) - - equilibrium_forces = -rod_one_elemental_forces + rod_two_elemental_forces - - """FIX ME: Remove normal force and tune rod-rod contact example""" - normal_force = _dot_product(equilibrium_forces, distance_vector) - # Following line same as np.where(normal_force < 0.0, -normal_force, 0.0) - normal_force = abs(min(normal_force, 0.0)) - - # CHECK FOR GAMMA > 0.0, heaviside but we need to overload it in numba - # As a quick fix, use this instead - mask = (gamma > 0.0) * 1.0 - - contact_force = contact_k * gamma - interpenetration_velocity = 0.5 * ( - (velocity_rod_one[..., i] + velocity_rod_one[..., i + 1]) - - (velocity_rod_two[..., j] + velocity_rod_two[..., j + 1]) - ) - contact_damping_force = contact_nu * _dot_product( - interpenetration_velocity, distance_vector - ) - - # magnitude* direction - net_contact_force = ( - normal_force + 0.5 * mask * (contact_damping_force + contact_force) - ) * distance_vector - - # Add it to the rods at the end of the day - if i == 0: - external_forces_rod_one[..., i] -= net_contact_force * 2 / 3 - external_forces_rod_one[..., i + 1] -= net_contact_force * 4 / 3 - elif i == n_points_rod_one - 1: - external_forces_rod_one[..., i] -= net_contact_force * 4 / 3 - external_forces_rod_one[..., i + 1] -= net_contact_force * 2 / 3 - else: - external_forces_rod_one[..., i] -= net_contact_force - external_forces_rod_one[..., i + 1] -= net_contact_force - - if j == 0: - external_forces_rod_two[..., j] += net_contact_force * 2 / 3 - external_forces_rod_two[..., j + 1] += net_contact_force * 4 / 3 - elif j == n_points_rod_two - 1: - external_forces_rod_two[..., j] += net_contact_force * 4 / 3 - external_forces_rod_two[..., j + 1] += net_contact_force * 2 / 3 - else: - external_forces_rod_two[..., j] += net_contact_force - external_forces_rod_two[..., j + 1] += net_contact_force - - -@numba.njit(cache=True) -def _calculate_contact_forces_self_rod( - x_collection_rod, - radius_rod, - length_rod, - tangent_rod, - velocity_rod, - external_forces_rod, - contact_k, - contact_nu, -) -> None: - # 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) - - for i in range(n_points_rod): - skip = int(1 + np.ceil(0.8 * np.pi * radius_rod[i] / length_rod[i])) - for j in range(i - skip, -1, -1): - radii_sum = radius_rod[i] + radius_rod[j] - length_sum = length_rod[i] + length_rod[j] - # Element-wise bounding box - x_selected_rod_index_i = x_collection_rod[..., i] - x_selected_rod_index_j = x_collection_rod[..., j] - - del_x = x_selected_rod_index_i - x_selected_rod_index_j - norm_del_x = _norm(del_x) - - # If outside then don't process - if norm_del_x >= (radii_sum + length_sum): - continue - - # find the shortest line segment between the two centerline - # segments : differs from normal cylinder-cylinder intersection - distance_vector, _, _ = _find_min_dist( - x_selected_rod_index_i, - edge_collection_rod_one[..., i], - x_selected_rod_index_j, - edge_collection_rod_one[..., j], - ) - distance_vector_length = _norm(distance_vector) - distance_vector /= distance_vector_length - - gamma = radii_sum - distance_vector_length - - # If distance is large, don't worry about it - if gamma < -1e-5: - continue - - # CHECK FOR GAMMA > 0.0, heaviside but we need to overload it in numba - # As a quick fix, use this instead - mask = (gamma > 0.0) * 1.0 - - contact_force = contact_k * gamma - interpenetration_velocity = 0.5 * ( - (velocity_rod[..., i] + velocity_rod[..., i + 1]) - - (velocity_rod[..., j] + velocity_rod[..., j + 1]) - ) - contact_damping_force = contact_nu * _dot_product( - interpenetration_velocity, distance_vector - ) - - # magnitude* direction - net_contact_force = ( - 0.5 * mask * (contact_damping_force + contact_force) - ) * distance_vector - - # Add it to the rods at the end of the day - # if i == 0: - # external_forces_rod[...,i] -= net_contact_force *2/3 - # external_forces_rod[...,i+1] -= net_contact_force * 4/3 - if i == n_points_rod - 1: - external_forces_rod[..., i] -= net_contact_force * 4 / 3 - external_forces_rod[..., i + 1] -= net_contact_force * 2 / 3 - else: - external_forces_rod[..., i] -= net_contact_force - external_forces_rod[..., i + 1] -= net_contact_force - - if j == 0: - external_forces_rod[..., j] += net_contact_force * 2 / 3 - external_forces_rod[..., j + 1] += net_contact_force * 4 / 3 - # elif j == n_points_rod: - # external_forces_rod[..., j] += net_contact_force * 4/3 - # external_forces_rod[..., j+1] += net_contact_force * 2/3 - else: - external_forces_rod[..., j] += net_contact_force - external_forces_rod[..., j + 1] += net_contact_force - - -@numba.njit(cache=True) -def _calculate_contact_forces_rod_sphere( - x_collection_rod, - edge_collection_rod, - x_sphere_center, - x_sphere_tip, - edge_sphere, - radii_sum, - length_sum, - internal_forces_rod, - external_forces_rod, - external_forces_sphere, - external_torques_sphere, - sphere_director_collection, - velocity_rod, - velocity_sphere, - contact_k, - contact_nu, - velocity_damping_coefficient, - friction_coefficient, -) -> None: - # 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 - 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_length = _norm(distance_vector) - distance_vector /= distance_vector_length - - gamma = radii_sum[i] - distance_vector_length - - # If distance is large, don't worry about it - if gamma < -1e-5: - continue - - # CHECK FOR GAMMA > 0.0, heaviside but we need to overload it in numba - # As a quick fix, use this instead - mask = (gamma > 0.0) * 1.0 - - # Compute contact spring force - contact_force = contact_k * gamma * distance_vector - interpenetration_velocity = velocity_sphere[..., 0] - 0.5 * ( - velocity_rod[..., i] + velocity_rod[..., i + 1] - ) - # Compute contact damping - normal_interpenetration_velocity = ( - _dot_product(interpenetration_velocity, distance_vector) * distance_vector - ) - contact_damping_force = -contact_nu * normal_interpenetration_velocity - - # magnitude* direction - net_contact_force = 0.5 * mask * (contact_damping_force + contact_force) - - # Compute friction - slip_interpenetration_velocity = ( - interpenetration_velocity - normal_interpenetration_velocity - ) - slip_interpenetration_velocity_mag = np.linalg.norm( - slip_interpenetration_velocity - ) - slip_interpenetration_velocity_unitized = slip_interpenetration_velocity / ( - slip_interpenetration_velocity_mag + 1e-14 - ) - # Compute friction force in the slip direction. - damping_force_in_slip_direction = ( - velocity_damping_coefficient * slip_interpenetration_velocity_mag - ) - # Compute Coulombic friction - coulombic_friction_force = friction_coefficient * np.linalg.norm( - net_contact_force - ) - # Compare damping force in slip direction and kinetic friction and minimum is the friction force. - friction_force = ( - -min(damping_force_in_slip_direction, coulombic_friction_force) - * slip_interpenetration_velocity_unitized - ) - # 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 - ) - - -@numba.njit(cache=True) -def _calculate_contact_forces_rod_plane( - plane_origin, - plane_normal, - surface_tol, - k, - nu, - radius, - mass, - position_collection, - velocity_collection, - internal_forces, - external_forces, -): - """ - 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. - - Parameters - ---------- - system - - Returns - ------- - magnitude of the plane response - """ - - # Compute plane response force - nodal_total_forces = _batch_vector_sum(internal_forces, external_forces) - element_total_forces = _node_to_element_mass_or_force(nodal_total_forces) - - force_component_along_normal_direction = _batch_product_i_ik_to_k( - plane_normal, element_total_forces - ) - forces_along_normal_direction = _batch_product_i_k_to_ik( - plane_normal, force_component_along_normal_direction - ) - - # If the total force component along the plane normal direction is greater than zero that means, - # total force is pushing rod away from the plane not towards the plane. Thus, response force - # applied by the surface has to be zero. - forces_along_normal_direction[ - ..., np.where(force_component_along_normal_direction > 0)[0] - ] = 0.0 - # Compute response force on the element. Plane response force - # has to be away from the surface and towards the element. Thus - # multiply forces along normal direction with negative sign. - plane_response_force = -forces_along_normal_direction - - # Elastic force response due to penetration - element_position = _node_to_element_position(position_collection) - distance_from_plane = _batch_product_i_ik_to_k( - plane_normal, (element_position - plane_origin) - ) - plane_penetration = np.minimum(distance_from_plane - radius, 0.0) - elastic_force = -k * _batch_product_i_k_to_ik(plane_normal, plane_penetration) - - # Damping force response due to velocity towards the plane - element_velocity = _node_to_element_velocity( - mass=mass, node_velocity_collection=velocity_collection - ) - normal_component_of_element_velocity = _batch_product_i_ik_to_k( - plane_normal, element_velocity - ) - damping_force = -nu * _batch_product_i_k_to_ik( - plane_normal, normal_component_of_element_velocity - ) - - # Compute total plane response force - plane_response_force_total = plane_response_force + elastic_force + damping_force - - # Check if the rod elements are in contact with plane. - no_contact_point_idx = np.where((distance_from_plane - radius) > surface_tol)[0] - # If rod element does not have any contact with plane, plane cannot apply response - # force on the element. Thus lets set plane response force to 0.0 for the no contact points. - plane_response_force[..., no_contact_point_idx] = 0.0 - plane_response_force_total[..., no_contact_point_idx] = 0.0 - - # Update the external forces - _elements_to_nodes_inplace(plane_response_force_total, external_forces) - - return (_batch_norm(plane_response_force), no_contact_point_idx) - - -@numba.njit(cache=True) -def _calculate_contact_forces_rod_plane_with_anisotropic_friction( - plane_origin, - plane_normal, - surface_tol, - slip_velocity_tol, - k, - nu, - kinetic_mu_forward, - kinetic_mu_backward, - kinetic_mu_sideways, - static_mu_forward, - static_mu_backward, - static_mu_sideways, - radius, - mass, - tangents, - position_collection, - director_collection, - velocity_collection, - omega_collection, - internal_forces, - external_forces, - internal_torques, - external_torques, -): - ( - plane_response_force_mag, - no_contact_point_idx, - ) = _calculate_contact_forces_rod_plane( - plane_origin, - plane_normal, - surface_tol, - k, - nu, - radius, - mass, - position_collection, - velocity_collection, - internal_forces, - external_forces, - ) - - # First compute component of rod tangent in plane. Because friction forces acts in plane not out of plane. Thus - # axial direction has to be in plane, it cannot be out of plane. We are projecting rod element tangent vector in - # to the plane. So friction forces can only be in plane forces and not out of plane. - tangent_along_normal_direction = _batch_product_i_ik_to_k(plane_normal, tangents) - tangent_perpendicular_to_normal_direction = tangents - _batch_product_i_k_to_ik( - plane_normal, tangent_along_normal_direction - ) - tangent_perpendicular_to_normal_direction_mag = _batch_norm( - tangent_perpendicular_to_normal_direction - ) - # Normalize tangent_perpendicular_to_normal_direction. This is axial direction for plane. Here we are adding - # small tolerance (1e-10) for normalization, in order to prevent division by 0. - axial_direction = _batch_product_k_ik_to_ik( - 1 / (tangent_perpendicular_to_normal_direction_mag + 1e-14), - tangent_perpendicular_to_normal_direction, - ) - element_velocity = _node_to_element_velocity( - mass=mass, node_velocity_collection=velocity_collection - ) - # first apply axial kinetic friction - velocity_mag_along_axial_direction = _batch_dot(element_velocity, axial_direction) - velocity_along_axial_direction = _batch_product_k_ik_to_ik( - velocity_mag_along_axial_direction, axial_direction - ) - - # Friction forces depends on the direction of velocity, in other words sign - # of the velocity vector. - velocity_sign_along_axial_direction = np.sign(velocity_mag_along_axial_direction) - # Check top for sign convention - kinetic_mu = 0.5 * ( - kinetic_mu_forward * (1 + velocity_sign_along_axial_direction) - + kinetic_mu_backward * (1 - velocity_sign_along_axial_direction) - ) - # Call slip function to check if elements slipping or not - slip_function_along_axial_direction = _find_slipping_elements( - velocity_along_axial_direction, slip_velocity_tol - ) - # Now rolling kinetic friction - rolling_direction = _batch_vec_oneD_vec_cross(axial_direction, plane_normal) - torque_arm = _batch_product_i_k_to_ik(-plane_normal, radius) - velocity_along_rolling_direction = _batch_dot(element_velocity, rolling_direction) - directors_transpose = _batch_matrix_transpose(director_collection) - # w_rot = Q.T @ omega @ Q @ r - rotation_velocity = _batch_matvec( - directors_transpose, - _batch_cross(omega_collection, _batch_matvec(director_collection, torque_arm)), - ) - rotation_velocity_along_rolling_direction = _batch_dot( - rotation_velocity, rolling_direction - ) - slip_velocity_mag_along_rolling_direction = ( - velocity_along_rolling_direction + rotation_velocity_along_rolling_direction - ) - slip_velocity_along_rolling_direction = _batch_product_k_ik_to_ik( - slip_velocity_mag_along_rolling_direction, rolling_direction - ) - slip_function_along_rolling_direction = _find_slipping_elements( - slip_velocity_along_rolling_direction, slip_velocity_tol - ) - # Compute unitized total slip velocity vector. We will use this to distribute the weight of the rod in axial - # and rolling directions. - unitized_total_velocity = ( - slip_velocity_along_rolling_direction + velocity_along_axial_direction - ) - unitized_total_velocity /= _batch_norm(unitized_total_velocity + 1e-14) - # Apply kinetic friction in axial direction. - kinetic_friction_force_along_axial_direction = -( - (1.0 - slip_function_along_axial_direction) - * kinetic_mu - * plane_response_force_mag - * _batch_dot(unitized_total_velocity, axial_direction) - * axial_direction - ) - # If rod element does not have any contact with plane, plane cannot apply friction - # force on the element. Thus lets set kinetic friction force to 0.0 for the no contact points. - kinetic_friction_force_along_axial_direction[..., no_contact_point_idx] = 0.0 - _elements_to_nodes_inplace( - kinetic_friction_force_along_axial_direction, external_forces - ) - # Apply kinetic friction in rolling direction. - kinetic_friction_force_along_rolling_direction = -( - (1.0 - slip_function_along_rolling_direction) - * kinetic_mu_sideways - * plane_response_force_mag - * _batch_dot(unitized_total_velocity, rolling_direction) - * rolling_direction - ) - # If rod element does not have any contact with plane, plane cannot apply friction - # force on the element. Thus lets set kinetic friction force to 0.0 for the no contact points. - kinetic_friction_force_along_rolling_direction[..., no_contact_point_idx] = 0.0 - _elements_to_nodes_inplace( - kinetic_friction_force_along_rolling_direction, external_forces - ) - # torque = Q @ r @ Fr - external_torques += _batch_matvec( - director_collection, - _batch_cross(torque_arm, kinetic_friction_force_along_rolling_direction), - ) - - # now axial static friction - nodal_total_forces = _batch_vector_sum(internal_forces, external_forces) - element_total_forces = _node_to_element_mass_or_force(nodal_total_forces) - force_component_along_axial_direction = _batch_dot( - element_total_forces, axial_direction - ) - force_component_sign_along_axial_direction = np.sign( - force_component_along_axial_direction - ) - # check top for sign convention - static_mu = 0.5 * ( - static_mu_forward * (1 + force_component_sign_along_axial_direction) - + static_mu_backward * (1 - force_component_sign_along_axial_direction) - ) - max_friction_force = ( - slip_function_along_axial_direction * static_mu * plane_response_force_mag - ) - # friction = min(mu N, pushing force) - static_friction_force_along_axial_direction = -( - np.minimum(np.fabs(force_component_along_axial_direction), max_friction_force) - * force_component_sign_along_axial_direction - * axial_direction - ) - # If rod element does not have any contact with plane, plane cannot apply friction - # force on the element. Thus lets set static friction force to 0.0 for the no contact points. - static_friction_force_along_axial_direction[..., no_contact_point_idx] = 0.0 - _elements_to_nodes_inplace( - static_friction_force_along_axial_direction, external_forces - ) - - # now rolling static friction - # there is some normal, tangent and rolling directions inconsitency from Elastica - total_torques = _batch_matvec( - directors_transpose, (internal_torques + external_torques) - ) - # Elastica has opposite defs of tangents in interaction.h and rod.cpp - total_torques_along_axial_direction = _batch_dot(total_torques, axial_direction) - force_component_along_rolling_direction = _batch_dot( - element_total_forces, rolling_direction - ) - noslip_force = -( - ( - radius * force_component_along_rolling_direction - - 2.0 * total_torques_along_axial_direction - ) - / 3.0 - / radius - ) - max_friction_force = ( - slip_function_along_rolling_direction - * static_mu_sideways - * plane_response_force_mag - ) - noslip_force_sign = np.sign(noslip_force) - static_friction_force_along_rolling_direction = ( - np.minimum(np.fabs(noslip_force), max_friction_force) - * noslip_force_sign - * rolling_direction - ) - # If rod element does not have any contact with plane, plane cannot apply friction - # force on the element. Thus lets set plane static friction force to 0.0 for the no contact points. - static_friction_force_along_rolling_direction[..., no_contact_point_idx] = 0.0 - _elements_to_nodes_inplace( - static_friction_force_along_rolling_direction, external_forces - ) - external_torques += _batch_matvec( - director_collection, - _batch_cross(torque_arm, static_friction_force_along_rolling_direction), - ) - - class RodRodContact(NoContact): """ This class is for applying contact forces between rod-rod. diff --git a/elastica/interaction.py b/elastica/interaction.py index ff0d3f0b0..07f5f4390 100644 --- a/elastica/interaction.py +++ b/elastica/interaction.py @@ -3,100 +3,36 @@ import numpy as np from elastica.external_forces import NoForces - - from numba import njit from elastica._linalg import ( - _batch_matvec, - _batch_cross, _batch_norm, - _batch_dot, _batch_product_i_k_to_ik, _batch_product_i_ik_to_k, - _batch_product_k_ik_to_ik, - _batch_vector_sum, - _batch_matrix_transpose, - _batch_vec_oneD_vec_cross, +) +from elastica.contact_utils import ( + _elements_to_nodes_inplace, + _node_to_element_velocity, +) +from elastica._contact_functions import ( + _calculate_contact_forces_rod_plane, + _calculate_contact_forces_rod_plane_with_anisotropic_friction, ) -@njit(cache=True) def find_slipping_elements(velocity_slip, velocity_threshold): - """ - This function takes the velocity of elements and checks if they are larger than the threshold velocity. - If the velocity of elements is larger than threshold velocity, that means those elements are slipping. - In other words, kinetic friction will be acting on those elements, not static friction. - This function outputs an array called slip function, this array has a size of the number of elements. - If the velocity of the element is smaller than the threshold velocity slip function value for that element is 1, - which means static friction is acting on that element. If the velocity of the element is larger than - the threshold velocity slip function value for that element is between 0 and 1, which means kinetic friction is acting - on that element. - - Parameters - ---------- - velocity_slip : numpy.ndarray - 2D (dim, blocksize) array containing data with 'float' type. - Rod-like object element velocity. - velocity_threshold : float - Threshold velocity to determine slip. - - Returns - ------- - slip_function : numpy.ndarray - 2D (dim, blocksize) array containing data with 'float' type. - """ - """ - Developer Notes - ----- - Benchmark results, for a blocksize of 100 using timeit - python version: 18.9 µs ± 2.98 µs per loop - this version: 1.96 µs ± 58.3 ns per loop - """ - abs_velocity_slip = _batch_norm(velocity_slip) - slip_points = np.where(np.fabs(abs_velocity_slip) > velocity_threshold) - slip_function = np.ones((velocity_slip.shape[1])) - slip_function[slip_points] = np.fabs( - 1.0 - np.minimum(1.0, abs_velocity_slip[slip_points] / velocity_threshold - 1.0) + raise NotImplementedError( + "This function is removed in v0.3.2. Please use\n" + "elastica.contact_utils._find_slipping_elements()\n" + "instead for finding slipping elements." ) - return slip_function -@njit(cache=True) def node_to_element_mass_or_force(input): - """ - This function converts the mass/forces on rod nodes to - elements, where special treatment is necessary at the ends. - - Parameters - ---------- - input: numpy.ndarray - 2D (dim, blocksize) array containing nodal mass/forces - with 'float' type. - - Returns - ------- - output: numpy.ndarray - 2D (dim, blocksize) array containing elemental mass/forces - with 'float' type. - """ - """ - Developer Notes - ----- - Benchmark results, for a blocksize of 100 using timeit - Python version: 18.1 µs ± 1.03 µs per loop - This version: 1.55 µs ± 13.4 ns per loop - """ - blocksize = input.shape[1] - 1 # nelem - output = np.zeros((3, blocksize)) - for i in range(3): - for k in range(0, blocksize): - output[i, k] += 0.5 * (input[i, k] + input[i, k + 1]) - - # Put extra care for the first and last element - output[..., 0] += 0.5 * input[..., 0] - output[..., -1] += 0.5 * input[..., -1] - - return output + raise NotImplementedError( + "This function is removed in v0.3.2. Please use\n" + "elastica.contact_utils._node_to_element_mass_or_force()\n" + "instead for converting the mass/forces on rod nodes to elements." + ) def nodes_to_elements(input): @@ -110,25 +46,11 @@ def nodes_to_elements(input): @njit(cache=True) def elements_to_nodes_inplace(vector_in_element_frame, vector_in_node_frame): - """ - Updating nodal forces using the forces computed on elements - Parameters - ---------- - vector_in_element_frame - vector_in_node_frame - - Returns - ------- - Notes - ----- - Benchmark results, for a blocksize of 100 using timeit - Python version: 23.1 µs ± 7.57 µs per loop - This version: 696 ns ± 10.2 ns per loop - """ - for i in range(3): - for k in range(vector_in_element_frame.shape[1]): - vector_in_node_frame[i, k] += 0.5 * vector_in_element_frame[i, k] - vector_in_node_frame[i, k + 1] += 0.5 * vector_in_element_frame[i, k] + raise NotImplementedError( + "This function is removed in v0.3.2. Please use\n" + "elastica.contact_utils._elements_to_nodes_inplace()\n" + "instead for updating nodal forces using the forces computed on elements." + ) # base class for interaction @@ -196,7 +118,7 @@ def apply_normal_force(self, system): 1D (blocksize) array containing data with 'int' type. Index of rod-like object elements that are not in contact with the plane. """ - return apply_normal_force_numba( + return _calculate_contact_forces_rod_plane( self.plane_origin, self.plane_normal, self.surface_tol, @@ -211,7 +133,6 @@ def apply_normal_force(self, system): ) -@njit(cache=True) def apply_normal_force_numba( plane_origin, plane_normal, @@ -225,76 +146,12 @@ def apply_normal_force_numba( internal_forces, external_forces, ): - """ - 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. - - Parameters - ---------- - system - - Returns - ------- - magnitude of the plane response - """ - - # Compute plane response force - nodal_total_forces = _batch_vector_sum(internal_forces, external_forces) - element_total_forces = node_to_element_mass_or_force(nodal_total_forces) - - force_component_along_normal_direction = _batch_product_i_ik_to_k( - plane_normal, element_total_forces - ) - forces_along_normal_direction = _batch_product_i_k_to_ik( - plane_normal, force_component_along_normal_direction - ) - - # If the total force component along the plane normal direction is greater than zero that means, - # total force is pushing rod away from the plane not towards the plane. Thus, response force - # applied by the surface has to be zero. - forces_along_normal_direction[ - ..., np.where(force_component_along_normal_direction > 0)[0] - ] = 0.0 - # Compute response force on the element. Plane response force - # has to be away from the surface and towards the element. Thus - # multiply forces along normal direction with negative sign. - plane_response_force = -forces_along_normal_direction - - # Elastic force response due to penetration - element_position = node_to_element_position(position_collection) - distance_from_plane = _batch_product_i_ik_to_k( - plane_normal, (element_position - plane_origin) - ) - plane_penetration = np.minimum(distance_from_plane - radius, 0.0) - elastic_force = -k * _batch_product_i_k_to_ik(plane_normal, plane_penetration) - - # Damping force response due to velocity towards the plane - element_velocity = node_to_element_velocity( - mass=mass, node_velocity_collection=velocity_collection - ) - normal_component_of_element_velocity = _batch_product_i_ik_to_k( - plane_normal, element_velocity - ) - damping_force = -nu * _batch_product_i_k_to_ik( - plane_normal, normal_component_of_element_velocity + raise NotImplementedError( + "This function is removed in v0.3.2. For rod plane contact please use: \n" + "elastica._contact_functions._calculate_contact_forces_rod_plane() \n" + "For detail, refer to issue #113." ) - # Compute total plane response force - plane_response_force_total = plane_response_force + elastic_force + damping_force - - # Check if the rod elements are in contact with plane. - no_contact_point_idx = np.where((distance_from_plane - radius) > surface_tol)[0] - # If rod element does not have any contact with plane, plane cannot apply response - # force on the element. Thus lets set plane response force to 0.0 for the no contact points. - plane_response_force[..., no_contact_point_idx] = 0.0 - plane_response_force_total[..., no_contact_point_idx] = 0.0 - - # Update the external forces - elements_to_nodes_inplace(plane_response_force_total, external_forces) - - return (_batch_norm(plane_response_force), no_contact_point_idx) - # class for anisotropic frictional plane # NOTE: friction coefficients are passed as arrays in the order @@ -391,7 +248,7 @@ def apply_forces(self, system, time=0.0): ------- """ - anisotropic_friction( + _calculate_contact_forces_rod_plane_with_anisotropic_friction( self.plane_origin, self.plane_normal, self.surface_tol, @@ -418,7 +275,6 @@ def apply_forces(self, system, time=0.0): ) -@njit(cache=True) def anisotropic_friction( plane_origin, plane_normal, @@ -444,188 +300,10 @@ def anisotropic_friction( internal_torques, external_torques, ): - plane_response_force_mag, no_contact_point_idx = apply_normal_force_numba( - plane_origin, - plane_normal, - surface_tol, - k, - nu, - radius, - mass, - position_collection, - velocity_collection, - internal_forces, - external_forces, - ) - - # First compute component of rod tangent in plane. Because friction forces acts in plane not out of plane. Thus - # axial direction has to be in plane, it cannot be out of plane. We are projecting rod element tangent vector in - # to the plane. So friction forces can only be in plane forces and not out of plane. - tangent_along_normal_direction = _batch_product_i_ik_to_k(plane_normal, tangents) - tangent_perpendicular_to_normal_direction = tangents - _batch_product_i_k_to_ik( - plane_normal, tangent_along_normal_direction - ) - tangent_perpendicular_to_normal_direction_mag = _batch_norm( - tangent_perpendicular_to_normal_direction - ) - # Normalize tangent_perpendicular_to_normal_direction. This is axial direction for plane. Here we are adding - # small tolerance (1e-10) for normalization, in order to prevent division by 0. - axial_direction = _batch_product_k_ik_to_ik( - 1 / (tangent_perpendicular_to_normal_direction_mag + 1e-14), - tangent_perpendicular_to_normal_direction, - ) - element_velocity = node_to_element_velocity( - mass=mass, node_velocity_collection=velocity_collection - ) - # first apply axial kinetic friction - velocity_mag_along_axial_direction = _batch_dot(element_velocity, axial_direction) - velocity_along_axial_direction = _batch_product_k_ik_to_ik( - velocity_mag_along_axial_direction, axial_direction - ) - - # Friction forces depends on the direction of velocity, in other words sign - # of the velocity vector. - velocity_sign_along_axial_direction = np.sign(velocity_mag_along_axial_direction) - # Check top for sign convention - kinetic_mu = 0.5 * ( - kinetic_mu_forward * (1 + velocity_sign_along_axial_direction) - + kinetic_mu_backward * (1 - velocity_sign_along_axial_direction) - ) - # Call slip function to check if elements slipping or not - slip_function_along_axial_direction = find_slipping_elements( - velocity_along_axial_direction, slip_velocity_tol - ) - - # Now rolling kinetic friction - rolling_direction = _batch_vec_oneD_vec_cross(axial_direction, plane_normal) - torque_arm = _batch_product_i_k_to_ik(-plane_normal, radius) - velocity_along_rolling_direction = _batch_dot(element_velocity, rolling_direction) - directors_transpose = _batch_matrix_transpose(director_collection) - # w_rot = Q.T @ omega @ Q @ r - rotation_velocity = _batch_matvec( - directors_transpose, - _batch_cross(omega_collection, _batch_matvec(director_collection, torque_arm)), - ) - rotation_velocity_along_rolling_direction = _batch_dot( - rotation_velocity, rolling_direction - ) - slip_velocity_mag_along_rolling_direction = ( - velocity_along_rolling_direction + rotation_velocity_along_rolling_direction - ) - slip_velocity_along_rolling_direction = _batch_product_k_ik_to_ik( - slip_velocity_mag_along_rolling_direction, rolling_direction - ) - slip_function_along_rolling_direction = find_slipping_elements( - slip_velocity_along_rolling_direction, slip_velocity_tol - ) - # Compute unitized total slip velocity vector. We will use this to distribute the weight of the rod in axial - # and rolling directions. - unitized_total_velocity = ( - slip_velocity_along_rolling_direction + velocity_along_axial_direction - ) - unitized_total_velocity /= _batch_norm(unitized_total_velocity + 1e-14) - # Apply kinetic friction in axial direction. - kinetic_friction_force_along_axial_direction = -( - (1.0 - slip_function_along_axial_direction) - * kinetic_mu - * plane_response_force_mag - * _batch_dot(unitized_total_velocity, axial_direction) - * axial_direction - ) - # If rod element does not have any contact with plane, plane cannot apply friction - # force on the element. Thus lets set kinetic friction force to 0.0 for the no contact points. - kinetic_friction_force_along_axial_direction[..., no_contact_point_idx] = 0.0 - elements_to_nodes_inplace( - kinetic_friction_force_along_axial_direction, external_forces - ) - # Apply kinetic friction in rolling direction. - kinetic_friction_force_along_rolling_direction = -( - (1.0 - slip_function_along_rolling_direction) - * kinetic_mu_sideways - * plane_response_force_mag - * _batch_dot(unitized_total_velocity, rolling_direction) - * rolling_direction - ) - # If rod element does not have any contact with plane, plane cannot apply friction - # force on the element. Thus lets set kinetic friction force to 0.0 for the no contact points. - kinetic_friction_force_along_rolling_direction[..., no_contact_point_idx] = 0.0 - elements_to_nodes_inplace( - kinetic_friction_force_along_rolling_direction, external_forces - ) - # torque = Q @ r @ Fr - external_torques += _batch_matvec( - director_collection, - _batch_cross(torque_arm, kinetic_friction_force_along_rolling_direction), - ) - - # now axial static friction - nodal_total_forces = _batch_vector_sum(internal_forces, external_forces) - element_total_forces = node_to_element_mass_or_force(nodal_total_forces) - force_component_along_axial_direction = _batch_dot( - element_total_forces, axial_direction - ) - force_component_sign_along_axial_direction = np.sign( - force_component_along_axial_direction - ) - # check top for sign convention - static_mu = 0.5 * ( - static_mu_forward * (1 + force_component_sign_along_axial_direction) - + static_mu_backward * (1 - force_component_sign_along_axial_direction) - ) - max_friction_force = ( - slip_function_along_axial_direction * static_mu * plane_response_force_mag - ) - # friction = min(mu N, pushing force) - static_friction_force_along_axial_direction = -( - np.minimum(np.fabs(force_component_along_axial_direction), max_friction_force) - * force_component_sign_along_axial_direction - * axial_direction - ) - # If rod element does not have any contact with plane, plane cannot apply friction - # force on the element. Thus lets set static friction force to 0.0 for the no contact points. - static_friction_force_along_axial_direction[..., no_contact_point_idx] = 0.0 - elements_to_nodes_inplace( - static_friction_force_along_axial_direction, external_forces - ) - - # now rolling static friction - # there is some normal, tangent and rolling directions inconsitency from Elastica - total_torques = _batch_matvec( - directors_transpose, (internal_torques + external_torques) - ) - # Elastica has opposite defs of tangents in interaction.h and rod.cpp - total_torques_along_axial_direction = _batch_dot(total_torques, axial_direction) - force_component_along_rolling_direction = _batch_dot( - element_total_forces, rolling_direction - ) - noslip_force = -( - ( - radius * force_component_along_rolling_direction - - 2.0 * total_torques_along_axial_direction - ) - / 3.0 - / radius - ) - max_friction_force = ( - slip_function_along_rolling_direction - * static_mu_sideways - * plane_response_force_mag - ) - noslip_force_sign = np.sign(noslip_force) - static_friction_force_along_rolling_direction = ( - np.minimum(np.fabs(noslip_force), max_friction_force) - * noslip_force_sign - * rolling_direction - ) - # If rod element does not have any contact with plane, plane cannot apply friction - # force on the element. Thus lets set plane static friction force to 0.0 for the no contact points. - static_friction_force_along_rolling_direction[..., no_contact_point_idx] = 0.0 - elements_to_nodes_inplace( - static_friction_force_along_rolling_direction, external_forces - ) - external_torques += _batch_matvec( - director_collection, - _batch_cross(torque_arm, static_friction_force_along_rolling_direction), + raise NotImplementedError( + "This function is removed in v0.3.2. For anisotropic_friction please use: \n" + "elastica._contact_functions._calculate_contact_forces_rod_plane_with_anisotropic_friction() \n" + "For detail, refer to issue #113." ) @@ -670,100 +348,28 @@ def sum_over_elements(input): return output -@njit(cache=True) def node_to_element_position(node_position_collection): - """ - This function computes the position of the elements - from the nodal values. - Here we define a separate function because benchmark results - showed that using Numba, we get more than 3 times faster calculation. - - Parameters - ---------- - node_position_collection: numpy.ndarray - 2D (dim, blocksize) array containing nodal positions with - 'float' type. - - Returns - ------- - element_position_collection: numpy.ndarray - 2D (dim, blocksize) array containing elemental positions with - 'float' type. - """ - """ - Developer Notes - ----- - Benchmark results, for a blocksize of 100, - - Python version: 3.5 µs ± 149 ns per loop - - This version: 729 ns ± 14.3 ns per loop - - """ - n_elem = node_position_collection.shape[1] - 1 - element_position_collection = np.empty((3, n_elem)) - for k in range(n_elem): - element_position_collection[0, k] = 0.5 * ( - node_position_collection[0, k + 1] + node_position_collection[0, k] - ) - element_position_collection[1, k] = 0.5 * ( - node_position_collection[1, k + 1] + node_position_collection[1, k] - ) - element_position_collection[2, k] = 0.5 * ( - node_position_collection[2, k + 1] + node_position_collection[2, k] - ) - - return element_position_collection + raise NotImplementedError( + "This function is removed in v0.3.2. For node-to-element_position() interpolation please use: \n" + "elastica.contact_utils._node_to_element_position() for rod position \n" + "For detail, refer to issue #113." + ) -@njit(cache=True) def node_to_element_velocity(mass, node_velocity_collection): - """ - This function computes the velocity of the elements - from the nodal values. Uses the velocity of center of mass - in order to conserve momentum during computation. - - Parameters - ---------- - mass: numpy.ndarray - 2D (dim, blocksize) array containing nodal masses with - 'float' type. - node_velocity_collection: numpy.ndarray - 2D (dim, blocksize) array containing nodal velocities with - 'float' type. - - Returns - ------- - element_velocity_collection: numpy.ndarray - 2D (dim, blocksize) array containing elemental velocities with - 'float' type. - """ - n_elem = node_velocity_collection.shape[1] - 1 - element_velocity_collection = np.empty((3, n_elem)) - for k in range(n_elem): - element_velocity_collection[0, k] = ( - mass[k + 1] * node_velocity_collection[0, k + 1] - + mass[k] * node_velocity_collection[0, k] - ) - element_velocity_collection[1, k] = ( - mass[k + 1] * node_velocity_collection[1, k + 1] - + mass[k] * node_velocity_collection[1, k] - ) - element_velocity_collection[2, k] = ( - mass[k + 1] * node_velocity_collection[2, k + 1] - + mass[k] * node_velocity_collection[2, k] - ) - element_velocity_collection[:, k] /= mass[k + 1] + mass[k] - - return element_velocity_collection + raise NotImplementedError( + "This function is removed in v0.3.2. For node-to-element_velocity() interpolation please use: \n" + "elastica.contact_utils._node_to_element_velocity() for rod velocity. \n" + "For detail, refer to issue #113." + ) def node_to_element_pos_or_vel(vector_in_node_frame): # Remove the function beyond v0.4.0 raise NotImplementedError( "This function is removed in v0.3.0. For node-to-element interpolation please use: \n" - "elastica.interaction.node_to_element_position() for rod position \n" - "elastica.interaction.node_to_element_velocity() for rod velocity. \n" + "elastica.contact_utils._node_to_element_position() for rod position \n" + "elastica.contact_utils._node_to_element_velocity() for rod velocity. \n" "For detail, refer to issue #80." ) @@ -819,7 +425,7 @@ def slender_body_forces( f = np.empty((tangents.shape[0], tangents.shape[1])) total_length = sum_over_elements(lengths) - element_velocity = node_to_element_velocity( + element_velocity = _node_to_element_velocity( mass=mass, node_velocity_collection=velocity_collection ) @@ -916,7 +522,7 @@ def apply_forces(self, system, time=0.0): system.radius, system.mass, ) - elements_to_nodes_inplace(stokes_force, system.external_forces) + _elements_to_nodes_inplace(stokes_force, system.external_forces) # base class for interaction diff --git a/elastica/joint.py b/elastica/joint.py index cfcfa1d14..e37d6f058 100644 --- a/elastica/joint.py +++ b/elastica/joint.py @@ -1,10 +1,6 @@ __doc__ = """ Module containing joint classes to connect multiple rods together. """ - -from elastica._linalg import _batch_product_k_ik_to_ik from elastica._rotations import _inv_rotate from elastica.typing import SystemType, RodType -from math import sqrt -import numba import numpy as np import logging @@ -365,97 +361,47 @@ def get_relative_rotation_two_systems( ) -@numba.njit(cache=True) +# everything below this comment should be removed beyond v0.4.0 def _dot_product(a, b): - sum = 0.0 - for i in range(3): - sum += a[i] * b[i] - return sum + raise NotImplementedError( + "This function is removed in v0.3.2. Please use\n" + "elastica.contact_utils._dot_product()\n" + "instead for find the dot product between a and b." + ) -@numba.njit(cache=True) def _norm(a): - return sqrt(_dot_product(a, a)) + raise NotImplementedError( + "This function is removed in v0.3.2. Please use\n" + "elastica.contact_utils._norm()\n" + "instead for finding the norm of a." + ) -@numba.njit(cache=True) def _clip(x, low, high): - return max(low, min(x, high)) + raise NotImplementedError( + "This function is removed in v0.3.2. Please use\n" + "elastica.contact_utils._clip()\n" + "instead for clipping x." + ) -# Can this be made more efficient than 2 comp, 1 or? -@numba.njit(cache=True) def _out_of_bounds(x, low, high): - return (x < low) or (x > high) + raise NotImplementedError( + "This function is removed in v0.3.2. Please use\n" + "elastica.contact_utils._out_of_bounds()\n" + "instead for checking if x is out of bounds." + ) -@numba.njit(cache=True) def _find_min_dist(x1, e1, x2, e2): - e1e1 = _dot_product(e1, e1) - e1e2 = _dot_product(e1, e2) - e2e2 = _dot_product(e2, e2) - - x1e1 = _dot_product(x1, e1) - x1e2 = _dot_product(x1, e2) - x2e1 = _dot_product(e1, x2) - x2e2 = _dot_product(x2, e2) - - s = 0.0 - t = 0.0 - - parallel = abs(1.0 - e1e2 ** 2 / (e1e1 * e2e2)) < 1e-6 - if parallel: - # Some are parallel, so do processing - t = (x2e1 - x1e1) / e1e1 # Comes from taking dot of e1 with a normal - t = _clip(t, 0.0, 1.0) - s = (x1e2 + t * e1e2 - x2e2) / e2e2 # Same as before - s = _clip(s, 0.0, 1.0) - else: - # Using the Cauchy-Binet formula on eq(7) in docstring referenc - s = (e1e1 * (x1e2 - x2e2) + e1e2 * (x2e1 - x1e1)) / (e1e1 * e2e2 - (e1e2) ** 2) - t = (e1e2 * s + x2e1 - x1e1) / e1e1 - - if _out_of_bounds(s, 0.0, 1.0) or _out_of_bounds(t, 0.0, 1.0): - # potential_s = -100.0 - # potential_t = -100.0 - # potential_d = -100.0 - # overall_minimum_distance = 1e20 - - # Fill in the possibilities - potential_t = (x2e1 - x1e1) / e1e1 - s = 0.0 - t = _clip(potential_t, 0.0, 1.0) - potential_d = _norm(x1 + e1 * t - x2) - overall_minimum_distance = potential_d - - potential_t = (x2e1 + e1e2 - x1e1) / e1e1 - potential_t = _clip(potential_t, 0.0, 1.0) - potential_d = _norm(x1 + e1 * potential_t - x2 - e2) - if potential_d < overall_minimum_distance: - s = 1.0 - t = potential_t - overall_minimum_distance = potential_d - - potential_s = (x1e2 - x2e2) / e2e2 - potential_s = _clip(potential_s, 0.0, 1.0) - potential_d = _norm(x2 + potential_s * e2 - x1) - if potential_d < overall_minimum_distance: - s = potential_s - t = 0.0 - overall_minimum_distance = potential_d - - potential_s = (x1e2 + e1e2 - x2e2) / e2e2 - potential_s = _clip(potential_s, 0.0, 1.0) - potential_d = _norm(x2 + potential_s * e2 - x1 - e1) - if potential_d < overall_minimum_distance: - s = potential_s - t = 1.0 - - # Return distance, contact point of system 2, contact point of system 1 - return x2 + s * e2 - x1 - t * e1, x2 + s * e2, x1 - t * e1 - - -@numba.njit(cache=True) + raise NotImplementedError( + "This function is removed in v0.3.2. Please use\n" + "elastica.contact_utils._find_min_dist()\n" + "instead for finding minimum distance between contact points." + ) + + def _calculate_contact_forces_rod_rigid_body( x_collection_rod, edge_collection_rod, @@ -476,125 +422,13 @@ def _calculate_contact_forces_rod_rigid_body( velocity_damping_coefficient, friction_coefficient, ): - # We already pass in only the first n_elem x - n_points = x_collection_rod.shape[1] - cylinder_total_contact_forces = np.zeros((3)) - cylinder_total_contact_torques = np.zeros((3)) - for i in range(n_points): - # Element-wise bounding box - x_selected = x_collection_rod[..., i] - # x_cylinder is already a (,) array from outised - del_x = x_selected - x_cylinder_tip - 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 - # segments : differs from normal cylinder-cylinder intersection - distance_vector, x_cylinder_contact_point, _ = _find_min_dist( - x_selected, edge_collection_rod[..., i], x_cylinder_tip, edge_cylinder - ) - distance_vector_length = _norm(distance_vector) - distance_vector /= distance_vector_length - - gamma = radii_sum[i] - distance_vector_length - - # If distance is large, don't worry about it - if gamma < -1e-5: - continue - - rod_elemental_forces = 0.5 * ( - external_forces_rod[..., i] - + external_forces_rod[..., i + 1] - + internal_forces_rod[..., i] - + internal_forces_rod[..., i + 1] - ) - equilibrium_forces = -rod_elemental_forces + external_forces_cylinder[..., 0] - - normal_force = _dot_product(equilibrium_forces, distance_vector) - # Following line same as np.where(normal_force < 0.0, -normal_force, 0.0) - normal_force = abs(min(normal_force, 0.0)) - - # CHECK FOR GAMMA > 0.0, heaviside but we need to overload it in numba - # As a quick fix, use this instead - mask = (gamma > 0.0) * 1.0 - - # Compute contact spring force - contact_force = contact_k * gamma * distance_vector - interpenetration_velocity = velocity_cylinder[..., 0] - 0.5 * ( - velocity_rod[..., i] + velocity_rod[..., i + 1] - ) - # Compute contact damping - normal_interpenetration_velocity = ( - _dot_product(interpenetration_velocity, distance_vector) * distance_vector - ) - contact_damping_force = -contact_nu * normal_interpenetration_velocity - - # magnitude* direction - net_contact_force = 0.5 * mask * (contact_damping_force + contact_force) - - # Compute friction - slip_interpenetration_velocity = ( - interpenetration_velocity - normal_interpenetration_velocity - ) - slip_interpenetration_velocity_mag = np.linalg.norm( - slip_interpenetration_velocity - ) - slip_interpenetration_velocity_unitized = slip_interpenetration_velocity / ( - slip_interpenetration_velocity_mag + 1e-14 - ) - # Compute friction force in the slip direction. - damping_force_in_slip_direction = ( - velocity_damping_coefficient * slip_interpenetration_velocity_mag - ) - # Compute Coulombic friction - coulombic_friction_force = friction_coefficient * np.linalg.norm( - net_contact_force - ) - # Compare damping force in slip direction and kinetic friction and minimum is the friction force. - friction_force = ( - -min(damping_force_in_slip_direction, coulombic_friction_force) - * slip_interpenetration_velocity_unitized - ) - # Update contact force - net_contact_force += friction_force - - # Torques acting on the cylinder - moment_arm = x_cylinder_contact_point - x_cylinder_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 - cylinder_total_contact_forces += 2.0 * net_contact_force - cylinder_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 - cylinder_total_contact_forces += 2.0 * net_contact_force - cylinder_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 - cylinder_total_contact_forces += 2.0 * net_contact_force - cylinder_total_contact_torques += np.cross( - moment_arm, 2.0 * net_contact_force - ) - - # Update the cylinder external forces and torques - external_forces_cylinder[..., 0] += cylinder_total_contact_forces - external_torques_cylinder[..., 0] += ( - cylinder_director_collection @ cylinder_total_contact_torques + raise NotImplementedError( + "This function is removed in v0.3.2. Please use\n" + "elastica._contact_functions._calculate_contact_forces_rod_cylinder()\n" + "instead for calculating rod cylinder contact forces." ) -@numba.njit(cache=True) def _calculate_contact_forces_rod_rod( x_collection_rod_one, radius_rod_one, @@ -613,105 +447,13 @@ def _calculate_contact_forces_rod_rod( contact_k, contact_nu, ): - # 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] - edge_collection_rod_one = _batch_product_k_ik_to_ik(length_rod_one, tangent_rod_one) - edge_collection_rod_two = _batch_product_k_ik_to_ik(length_rod_two, tangent_rod_two) - - for i in range(n_points_rod_one): - for j in range(n_points_rod_two): - radii_sum = radius_rod_one[i] + radius_rod_two[j] - length_sum = length_rod_one[i] + length_rod_two[j] - # Element-wise bounding box - x_selected_rod_one = x_collection_rod_one[..., i] - x_selected_rod_two = x_collection_rod_two[..., j] - - del_x = x_selected_rod_one - x_selected_rod_two - norm_del_x = _norm(del_x) - - # If outside then don't process - if norm_del_x >= (radii_sum + length_sum): - continue - - # find the shortest line segment between the two centerline - # segments : differs from normal cylinder-cylinder intersection - distance_vector, _, _ = _find_min_dist( - x_selected_rod_one, - edge_collection_rod_one[..., i], - x_selected_rod_two, - edge_collection_rod_two[..., j], - ) - distance_vector_length = _norm(distance_vector) - distance_vector /= distance_vector_length - - gamma = radii_sum - distance_vector_length - - # If distance is large, don't worry about it - if gamma < -1e-5: - continue - - rod_one_elemental_forces = 0.5 * ( - external_forces_rod_one[..., i] - + external_forces_rod_one[..., i + 1] - + internal_forces_rod_one[..., i] - + internal_forces_rod_one[..., i + 1] - ) - - rod_two_elemental_forces = 0.5 * ( - external_forces_rod_two[..., j] - + external_forces_rod_two[..., j + 1] - + internal_forces_rod_two[..., j] - + internal_forces_rod_two[..., j + 1] - ) - - equilibrium_forces = -rod_one_elemental_forces + rod_two_elemental_forces - - normal_force = _dot_product(equilibrium_forces, distance_vector) - # Following line same as np.where(normal_force < 0.0, -normal_force, 0.0) - normal_force = abs(min(normal_force, 0.0)) - - # CHECK FOR GAMMA > 0.0, heaviside but we need to overload it in numba - # As a quick fix, use this instead - mask = (gamma > 0.0) * 1.0 + raise NotImplementedError( + "This function is removed in v0.3.2. Please use\n" + "elastica._contact_functions._calculate_contact_forces_rod_rod()\n" + "instead for calculating rod rod contact forces." + ) - contact_force = contact_k * gamma - interpenetration_velocity = 0.5 * ( - (velocity_rod_one[..., i] + velocity_rod_one[..., i + 1]) - - (velocity_rod_two[..., j] + velocity_rod_two[..., j + 1]) - ) - contact_damping_force = contact_nu * _dot_product( - interpenetration_velocity, distance_vector - ) - # magnitude* direction - net_contact_force = ( - normal_force + 0.5 * mask * (contact_damping_force + contact_force) - ) * distance_vector - - # Add it to the rods at the end of the day - if i == 0: - external_forces_rod_one[..., i] -= net_contact_force * 2 / 3 - external_forces_rod_one[..., i + 1] -= net_contact_force * 4 / 3 - elif i == n_points_rod_one - 1: - external_forces_rod_one[..., i] -= net_contact_force * 4 / 3 - external_forces_rod_one[..., i + 1] -= net_contact_force * 2 / 3 - else: - external_forces_rod_one[..., i] -= net_contact_force - external_forces_rod_one[..., i + 1] -= net_contact_force - - if j == 0: - external_forces_rod_two[..., j] += net_contact_force * 2 / 3 - external_forces_rod_two[..., j + 1] += net_contact_force * 4 / 3 - elif j == n_points_rod_two - 1: - external_forces_rod_two[..., j] += net_contact_force * 4 / 3 - external_forces_rod_two[..., j + 1] += net_contact_force * 2 / 3 - else: - external_forces_rod_two[..., j] += net_contact_force - external_forces_rod_two[..., j + 1] += net_contact_force - - -@numba.njit(cache=True) def _calculate_contact_forces_self_rod( x_collection_rod, radius_rod, @@ -722,97 +464,21 @@ def _calculate_contact_forces_self_rod( contact_k, contact_nu, ): - # 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) - - for i in range(n_points_rod): - skip = int(1 + np.ceil(0.8 * np.pi * radius_rod[i] / length_rod[i])) - for j in range(i - skip, -1, -1): - radii_sum = radius_rod[i] + radius_rod[j] - length_sum = length_rod[i] + length_rod[j] - # Element-wise bounding box - x_selected_rod_index_i = x_collection_rod[..., i] - x_selected_rod_index_j = x_collection_rod[..., j] - - del_x = x_selected_rod_index_i - x_selected_rod_index_j - norm_del_x = _norm(del_x) - - # If outside then don't process - if norm_del_x >= (radii_sum + length_sum): - continue - - # find the shortest line segment between the two centerline - # segments : differs from normal cylinder-cylinder intersection - distance_vector, _, _ = _find_min_dist( - x_selected_rod_index_i, - edge_collection_rod_one[..., i], - x_selected_rod_index_j, - edge_collection_rod_one[..., j], - ) - distance_vector_length = _norm(distance_vector) - distance_vector /= distance_vector_length - - gamma = radii_sum - distance_vector_length - - # If distance is large, don't worry about it - if gamma < -1e-5: - continue + raise NotImplementedError( + "This function is removed in v0.3.2. Please use\n" + "elastica._contact_functions._calculate_contact_forces_self_rod()\n" + "instead for calculating rod self-contact forces." + ) - # CHECK FOR GAMMA > 0.0, heaviside but we need to overload it in numba - # As a quick fix, use this instead - mask = (gamma > 0.0) * 1.0 - contact_force = contact_k * gamma - interpenetration_velocity = 0.5 * ( - (velocity_rod[..., i] + velocity_rod[..., i + 1]) - - (velocity_rod[..., j] + velocity_rod[..., j + 1]) - ) - contact_damping_force = contact_nu * _dot_product( - interpenetration_velocity, distance_vector - ) - - # magnitude* direction - net_contact_force = ( - 0.5 * mask * (contact_damping_force + contact_force) - ) * distance_vector - - # Add it to the rods at the end of the day - # if i == 0: - # external_forces_rod[...,i] -= net_contact_force *2/3 - # external_forces_rod[...,i+1] -= net_contact_force * 4/3 - if i == n_points_rod - 1: - external_forces_rod[..., i] -= net_contact_force * 4 / 3 - external_forces_rod[..., i + 1] -= net_contact_force * 2 / 3 - else: - external_forces_rod[..., i] -= net_contact_force - external_forces_rod[..., i + 1] -= net_contact_force - - if j == 0: - external_forces_rod[..., j] += net_contact_force * 2 / 3 - external_forces_rod[..., j + 1] += net_contact_force * 4 / 3 - # elif j == n_points_rod: - # external_forces_rod[..., j] += net_contact_force * 4/3 - # external_forces_rod[..., j+1] += net_contact_force * 2/3 - else: - external_forces_rod[..., j] += net_contact_force - external_forces_rod[..., j + 1] += net_contact_force - - -@numba.njit(cache=True) def _aabbs_not_intersecting(aabb_one, aabb_two): - """Returns true if not intersecting else false""" - 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]): - return 1 - if (aabb_one[2, 1] < aabb_two[2, 0]) | (aabb_one[2, 0] > aabb_two[2, 1]): - return 1 - - return 0 + raise NotImplementedError( + "This function is removed in v0.3.2. Please use\n" + "elastica.contact_utils._aabbs_not_intersecting()\n" + "instead for checking aabbs intersection." + ) -@numba.njit(cache=True) def _prune_using_aabbs_rod_rigid_body( rod_one_position_collection, rod_one_radius_collection, @@ -822,39 +488,13 @@ def _prune_using_aabbs_rod_rigid_body( cylinder_radius, cylinder_length, ): - max_possible_dimension = np.zeros((3,)) - aabb_rod = np.empty((3, 2)) - aabb_cylinder = np.empty((3, 2)) - max_possible_dimension[...] = np.max(rod_one_radius_collection) + np.max( - rod_one_length_collection + raise NotImplementedError( + "This function is removed in v0.3.2. Please use\n" + "elastica.contact_utils._prune_using_aabbs_rod_cylinder()\n" + "instead for checking rod cylinder intersection." ) - 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] - ) - - # Is actually Q^T * d but numba complains about performance so we do - # d^T @ Q - cylinder_dimensions_in_local_FOR = np.array( - [cylinder_radius, cylinder_radius, 0.5 * cylinder_length] - ) - cylinder_dimensions_in_world_FOR = np.zeros_like(cylinder_dimensions_in_local_FOR) - for i in range(3): - for j in range(3): - cylinder_dimensions_in_world_FOR[i] += ( - cylinder_director[j, i, 0] * cylinder_dimensions_in_local_FOR[j] - ) - max_possible_dimension = np.abs(cylinder_dimensions_in_world_FOR) - aabb_cylinder[..., 0] = cylinder_position[..., 0] - max_possible_dimension - aabb_cylinder[..., 1] = cylinder_position[..., 0] + max_possible_dimension - return _aabbs_not_intersecting(aabb_cylinder, aabb_rod) - -@numba.njit(cache=True) def _prune_using_aabbs_rod_rod( rod_one_position_collection, rod_one_radius_collection, @@ -863,33 +503,11 @@ def _prune_using_aabbs_rod_rod( rod_two_radius_collection, rod_two_length_collection, ): - max_possible_dimension = np.zeros((3,)) - aabb_rod_one = np.empty((3, 2)) - aabb_rod_two = np.empty((3, 2)) - max_possible_dimension[...] = np.max(rod_one_radius_collection) + np.max( - rod_one_length_collection + raise NotImplementedError( + "This function is removed in v0.3.2. Please use\n" + "elastica.contact_utils._prune_using_aabbs_rod_rod()\n" + "instead for checking rod rod intersection." ) - for i in range(3): - aabb_rod_one[i, 0] = ( - np.min(rod_one_position_collection[i]) - max_possible_dimension[i] - ) - aabb_rod_one[i, 1] = ( - np.max(rod_one_position_collection[i]) + max_possible_dimension[i] - ) - - max_possible_dimension[...] = np.max(rod_two_radius_collection) + np.max( - rod_two_length_collection - ) - - for i in range(3): - aabb_rod_two[i, 0] = ( - np.min(rod_two_position_collection[i]) - max_possible_dimension[i] - ) - aabb_rod_two[i, 1] = ( - np.max(rod_two_position_collection[i]) + max_possible_dimension[i] - ) - - return _aabbs_not_intersecting(aabb_rod_two, aabb_rod_one) class ExternalContact(FreeJoint): @@ -974,6 +592,14 @@ def apply_forces( index_two, ): # del index_one, index_two + from elastica.contact_utils import ( + _prune_using_aabbs_rod_cylinder, + _prune_using_aabbs_rod_rod, + ) + from elastica._contact_functions import ( + _calculate_contact_forces_rod_cylinder, + _calculate_contact_forces_rod_rod, + ) # TODO: raise error during the initialization if rod one is rigid body. @@ -982,7 +608,7 @@ def apply_forces( cylinder_two = rod_two # First, check for a global AABB bounding box, and see whether that # intersects - if _prune_using_aabbs_rod_rigid_body( + if _prune_using_aabbs_rod_cylinder( rod_one.position_collection, rod_one.radius, rod_one.lengths, @@ -1002,7 +628,7 @@ def apply_forces( rod_one.position_collection[..., 1:] + rod_one.position_collection[..., :-1] ) - _calculate_contact_forces_rod_rigid_body( + _calculate_contact_forces_rod_cylinder( rod_element_position, rod_one.lengths * rod_one.tangents, cylinder_two.position_collection[..., 0], @@ -1081,6 +707,9 @@ def __init__(self, k, nu): def apply_forces(self, rod_one: RodType, index_one, rod_two: SystemType, index_two): # del index_one, index_two + from elastica._contact_functions import ( + _calculate_contact_forces_self_rod, + ) _calculate_contact_forces_self_rod( rod_one.position_collection[ diff --git a/tests/test_contact_classes.py b/tests/test_contact_classes.py index 0c347a12a..b26932423 100644 --- a/tests/test_contact_classes.py +++ b/tests/test_contact_classes.py @@ -2,15 +2,22 @@ import numpy as np from numpy.testing import assert_allclose +from elastica.utils import Tolerance from elastica.contact_forces import ( RodRodContact, RodCylinderContact, RodSelfContact, RodSphereContact, + RodPlaneContact, + RodPlaneContactWithAnisotropicFriction, ) from elastica.typing import RodBase from elastica.rigidbody import Cylinder, Sphere +from elastica.surface import Plane import pytest +from elastica.contact_utils import ( + _node_to_element_mass_or_force, +) def mock_rod_init(self): @@ -18,8 +25,9 @@ def mock_rod_init(self): "Initializing Rod" "Details of initialization are given in test_contact_specific_functions.py" - self.n_elems = 2 + self.n_elem = 2 self.position_collection = np.array([[1, 2, 3], [0, 0, 0], [0, 0, 0]]) + self.mass = np.ones(self.n_elem + 1) self.radius = np.array([1, 1]) self.lengths = np.array([1, 1]) self.tangents = np.array([[1.0, 1.0], [0.0, 0.0], [0.0, 0.0]]) @@ -28,6 +36,9 @@ def mock_rod_init(self): self.velocity_collection = np.array( [[0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]] ) + self.omega_collection = np.array([[0.0, 0.0], [0.0, 0.0], [0.0, 0.0]]) + self.external_torques = np.array([[0.0, 0.0], [0.0, 0.0], [0.0, 0.0]]) + self.internal_torques = np.array([[0.0, 0.0], [0.0, 0.0], [0.0, 0.0]]) def mock_cylinder_init(self): @@ -63,12 +74,22 @@ def mock_sphere_init(self): self.external_torques = np.array([[0.0], [0.0], [0.0]]) +def mock_plane_init(self): + + "Initializing Plane" + + self.normal = np.asarray([1.0, 0.0, 0.0]).reshape(3) + self.origin = np.asarray([0.0, 0.0, 0.0]).reshape(3, 1) + + MockRod = type("MockRod", (RodBase,), {"__init__": mock_rod_init}) MockCylinder = type("MockCylinder", (Cylinder,), {"__init__": mock_cylinder_init}) MockSphere = type("MockSphere", (Sphere,), {"__init__": mock_sphere_init}) +MockPlane = type("MockPlane", (Plane,), {"__init__": mock_plane_init}) + class TestRodCylinderContact: def test_check_systems_validity_with_invalid_systems( @@ -559,3 +580,619 @@ def test_contact_rod_sphere_with_collision_with_k_without_nu_and_friction( np.array([[0.166666, 0.333333, 0], [0, 0, 0], [0, 0, 0]]), atol=1e-6, ) + + +class TestRodPlaneContact: + def initializer( + self, + shift=0.0, + k_w=0.0, + nu_w=0.0, + plane_normal=np.array([0.0, 1.0, 0.0]), + ): + # create rod + rod = MockRod() + start = np.array([0.0, 0.0, 0.0]) + direction = np.array([0.0, 0.0, 0.0]) + end = start + 1.0 * direction + rod.position_collection = np.zeros((3, 3)) + for i in range(0, 3): + rod.position_collection[i, ...] = np.linspace(start[i], end[i], num=3) + rod.director_collection = np.repeat(np.identity(3)[:, :, np.newaxis], 2, axis=2) + rod.lengths = np.ones(2) * 1.0 / 2 + rod.radius = np.repeat(np.array([0.25]), 2, axis=0) + rod.tangents = np.repeat(direction[:, np.newaxis], 2, axis=1) + + # create plane + plane = MockPlane() + plane.origin = np.array([0.0, -rod.radius[0] + shift, 0.0]).reshape(3, 1) + plane.normal = plane_normal.reshape( + 3, + ) + rod_plane_contact = RodPlaneContact(k_w, nu_w) + + fnormal = -10.0 * np.sign(plane_normal[1]) * np.random.random_sample(1).item() + external_forces = np.repeat( + np.array([0.0, fnormal, 0.0]).reshape(3, 1), 3, axis=1 + ) + external_forces[..., 0] *= 0.5 + external_forces[..., -1] *= 0.5 + rod.external_forces = external_forces.copy() + + return rod, plane, rod_plane_contact, external_forces + + def test_check_systems_validity_with_invalid_systems( + self, + ): + mock_rod = MockRod() + mock_plane = MockPlane() + mock_list = [1, 2, 3] + rod_plane_contact = RodPlaneContact(k=1.0, nu=0.0) + + "Testing Rod Plane Contact wrapper with incorrect type for second argument" + with pytest.raises(TypeError) as excinfo: + rod_plane_contact._check_systems_validity(mock_rod, mock_list) + assert ( + "Systems provided to the contact class have incorrect order/type. \n" + " First system is {0} and second system is {1}. \n" + " First system should be a rod, second should be a plane" + ).format(mock_rod.__class__, mock_list.__class__) == str(excinfo.value) + + "Testing Self Contact wrapper with incorrect type for first argument" + with pytest.raises(TypeError) as excinfo: + rod_plane_contact._check_systems_validity(mock_list, mock_plane) + assert ( + "Systems provided to the contact class have incorrect order/type. \n" + " First system is {0} and second system is {1}. \n" + " First system should be a rod, second should be a plane" + ).format(mock_list.__class__, mock_plane.__class__) == str(excinfo.value) + + def test_rod_plane_contact_without_contact(self): + """ + This test case tests the forces on rod, when there is no + contact between rod and the plane. + + """ + + shift = -( + (2.0 - 1.0) * np.random.random_sample(1) + 1.0 + ).item() # we move plane away from rod + print("q") + [rod, plane, rod_plane_contact, external_forces] = self.initializer(shift) + + rod_plane_contact.apply_contact(rod, plane) + correct_forces = external_forces # since no contact + assert_allclose(correct_forces, rod.external_forces, atol=Tolerance.atol()) + + def test_rod_plane_contact_without_k_and_nu(self): + """ + This function tests wall response on the rod. Here + wall stiffness coefficient and damping coefficient set + to zero to check only sum of all forces on the rod. + + """ + + [rod, plane, rod_plane_contact, external_forces] = self.initializer() + + rod_plane_contact.apply_contact(rod, plane) + + correct_forces = np.zeros((3, 3)) + assert_allclose(correct_forces, rod.external_forces, atol=Tolerance.atol()) + + @pytest.mark.parametrize("k_w", [0.1, 0.5, 1.0, 2, 10]) + def test_rod_plane_contact_with_k_without_nu(self, k_w): + """ + Here wall stiffness coefficient changed parametrically + and damping coefficient set to zero . + Parameters + ---------- + k_w + + + """ + + shift = np.random.random_sample(1).item() # we move plane towards to rod + [rod, plane, rod_plane_contact, external_forces] = self.initializer( + shift=shift, k_w=k_w + ) + correct_forces = k_w * np.repeat( + np.array([0.0, shift, 0.0]).reshape(3, 1), 3, axis=1 + ) + correct_forces[..., 0] *= 0.5 + correct_forces[..., -1] *= 0.5 + + rod_plane_contact.apply_contact(rod, plane) + + assert_allclose(correct_forces, rod.external_forces, atol=Tolerance.atol()) + + @pytest.mark.parametrize("nu_w", [0.5, 1.0, 5.0, 7.0, 12.0]) + def test_rod_plane_contact_without_k_with_nu(self, nu_w): + """ + Here wall damping coefficient are changed parametrically and + wall response functions tested. + Parameters + ---------- + nu_w + """ + + [rod, plane, rod_plane_contact, external_forces] = self.initializer(nu_w=nu_w) + + normal_velocity = np.random.random_sample(1).item() + rod.velocity_collection[..., :] += np.array( + [0.0, -normal_velocity, 0.0] + ).reshape(3, 1) + + correct_forces = np.repeat( + (nu_w * np.array([0.0, normal_velocity, 0.0])).reshape(3, 1), + 3, + axis=1, + ) + + correct_forces[..., 0] *= 0.5 + correct_forces[..., -1] *= 0.5 + + rod_plane_contact.apply_contact(rod, plane) + + assert_allclose(correct_forces, rod.external_forces, atol=Tolerance.atol()) + + def test_rod_plane_contact_when_rod_is_under_plane(self): + """ + This test case tests plane response forces on the rod + in the case rod is under the plane and pushed towards + the plane. + + """ + + # we move plane on top of the rod. Note that 0.25 is radius of the rod. + offset_of_plane_with_respect_to_rod = 2.0 * 0.25 + + # plane normal changed, it is towards the negative direction, because rod + # is under the rod. + plane_normal = np.array([0.0, -1.0, 0.0]) + + [rod, plane, rod_plane_contact, external_forces] = self.initializer( + shift=offset_of_plane_with_respect_to_rod, plane_normal=plane_normal + ) + + rod_plane_contact.apply_contact(rod, plane) + correct_forces = np.zeros((3, 3)) + assert_allclose(correct_forces, rod.external_forces, atol=Tolerance.atol()) + + @pytest.mark.parametrize("k_w", [0.1, 0.5, 1.0, 2, 10]) + def test_rod_plane_contact_when_rod_is_under_plane_with_k_without_nu(self, k_w): + """ + In this test case we move the rod under the plane. + Here wall stiffness coefficient changed parametrically + and damping coefficient set to zero . + Parameters + ---------- + k_w + + """ + # we move plane on top of the rod. Note that 0.25 is radius of the rod. + offset_of_plane_with_respect_to_rod = 2.0 * 0.25 + + # we move plane towards to rod by random distance + shift = offset_of_plane_with_respect_to_rod - np.random.random_sample(1).item() + + # plane normal changed, it is towards the negative direction, because rod + # is under the rod. + plane_normal = np.array([0.0, -1.0, 0.0]) + + [rod, plane, rod_plane_contact, external_forces] = self.initializer( + shift=shift, k_w=k_w, plane_normal=plane_normal + ) + + # we have to substract rod offset because top part + correct_forces = k_w * np.repeat( + np.array([0.0, shift - offset_of_plane_with_respect_to_rod, 0.0]).reshape( + 3, 1 + ), + 3, + axis=1, + ) + correct_forces[..., 0] *= 0.5 + correct_forces[..., -1] *= 0.5 + + rod_plane_contact.apply_contact(rod, plane) + + assert_allclose(correct_forces, rod.external_forces, atol=Tolerance.atol()) + + @pytest.mark.parametrize("nu_w", [0.5, 1.0, 5.0, 7.0, 12.0]) + def test_rod_plane_contact_when_rod_is_under_plane_without_k_with_nu(self, nu_w): + """ + In this test case we move under the plane and test damping force. + Here wall damping coefficient are changed parametrically and + wall response functions tested. + Parameters + ---------- + nu_w + + """ + # we move plane on top of the rod. Note that 0.25 is radius of the rod. + offset_of_plane_with_respect_to_rod = 2.0 * 0.25 + + # plane normal changed, it is towards the negative direction, because rod + # is under the rod. + plane_normal = np.array([0.0, -1.0, 0.0]) + + [rod, plane, rod_plane_contact, external_forces] = self.initializer( + shift=offset_of_plane_with_respect_to_rod, + nu_w=nu_w, + plane_normal=plane_normal, + ) + + normal_velocity = np.random.random_sample(1).item() + rod.velocity_collection[..., :] += np.array( + [0.0, -normal_velocity, 0.0] + ).reshape(3, 1) + + correct_forces = np.repeat( + (nu_w * np.array([0.0, normal_velocity, 0.0])).reshape(3, 1), + 3, + axis=1, + ) + + correct_forces[..., 0] *= 0.5 + correct_forces[..., -1] *= 0.5 + + rod_plane_contact.apply_contact(rod, plane) + + assert_allclose(correct_forces, rod.external_forces, atol=Tolerance.atol()) + + +class TestRodPlaneWithAnisotropicFriction: + def initializer( + self, + static_mu_array=np.array([0.0, 0.0, 0.0]), + kinetic_mu_array=np.array([0.0, 0.0, 0.0]), + force_mag_long=0.0, # forces along the rod + force_mag_side=0.0, # side forces on the rod + ): + + # create rod + rod = MockRod() + start = np.array([0.0, 0.0, 0.0]) + direction = np.array([0.0, 0.0, 1.0]) + end = start + 1.0 * direction + rod.position_collection = np.zeros((3, 3)) + for i in range(0, 3): + rod.position_collection[i, ...] = np.linspace(start[i], end[i], num=3) + rod.director_collection = np.repeat(np.identity(3)[:, :, np.newaxis], 2, axis=2) + rod.lengths = np.ones(2) * 1.0 / 2 + rod.radius = np.repeat(np.array([0.25]), 2, axis=0) + rod.tangents = np.repeat(direction[:, np.newaxis], 2, axis=1) + + # create plane + plane = MockPlane() + plane.origin = np.array([0.0, -rod.radius[0], 0.0]).reshape(3, 1) + plane.normal = np.array([0.0, 1.0, 0.0]).reshape( + 3, + ) + slip_velocity_tol = 1e-2 + rod_plane_contact = RodPlaneContactWithAnisotropicFriction( + 0.0, + 0.0, + slip_velocity_tol, + static_mu_array, # forward, backward, sideways + kinetic_mu_array, # forward, backward, sideways + ) + + fnormal = (10.0 - 5.0) * np.random.random_sample( + 1 + ).item() + 5.0 # generates random numbers [5.0,10) + external_forces = np.array([force_mag_side, -fnormal, force_mag_long]) + + external_forces_collection = np.repeat(external_forces.reshape(3, 1), 3, axis=1) + external_forces_collection[..., 0] *= 0.5 + external_forces_collection[..., -1] *= 0.5 + rod.external_forces = external_forces_collection.copy() + + return rod, plane, rod_plane_contact, external_forces_collection + + def test_check_systems_validity_with_invalid_systems( + self, + ): + mock_rod = MockRod() + mock_plane = MockPlane() + mock_list = [1, 2, 3] + rod_plane_contact = RodPlaneContactWithAnisotropicFriction( + 0.0, + 0.0, + 1e-2, + np.array([0.0, 0.0, 0.0]), # forward, backward, sideways + np.array([0.0, 0.0, 0.0]), # forward, backward, sideways + ) + + "Testing Rod Plane Contact wrapper with incorrect type for second argument" + with pytest.raises(TypeError) as excinfo: + rod_plane_contact._check_systems_validity(mock_rod, mock_list) + assert ( + "Systems provided to the contact class have incorrect order/type. \n" + " First system is {0} and second system is {1}. \n" + " First system should be a rod, second should be a plane" + ).format(mock_rod.__class__, mock_list.__class__) == str(excinfo.value) + + "Testing Self Contact wrapper with incorrect type for first argument" + with pytest.raises(TypeError) as excinfo: + rod_plane_contact._check_systems_validity(mock_list, mock_plane) + assert ( + "Systems provided to the contact class have incorrect order/type. \n" + " First system is {0} and second system is {1}. \n" + " First system should be a rod, second should be a plane" + ).format(mock_list.__class__, mock_plane.__class__) == str(excinfo.value) + + @pytest.mark.parametrize("velocity", [-1.0, -3.0, 1.0, 5.0, 2.0]) + def test_axial_kinetic_friction(self, velocity): + """ + This function tests kinetic friction in forward and backward direction. + All other friction coefficients set to zero. + Parameters + ---------- + velocity + + + + """ + + [rod, plane, rod_plane_contact, external_forces_collection] = self.initializer( + kinetic_mu_array=np.array([1.0, 1.0, 0.0]) + ) + + rod.velocity_collection += np.array([0.0, 0.0, velocity]).reshape(3, 1) + + rod_plane_contact.apply_contact(rod, plane) + + direction_collection = np.repeat( + np.array([0.0, 0.0, 1.0]).reshape(3, 1), 3, axis=1 + ) + correct_forces = ( + -1.0 + * np.sign(velocity) + * np.linalg.norm(external_forces_collection, axis=0) + * direction_collection + ) + assert_allclose(correct_forces, rod.external_forces, atol=Tolerance.atol()) + + @pytest.mark.parametrize("force_mag", [-1.0, -3.0, 1.0, 5.0, 2.0]) + def test_axial_static_friction_total_force_smaller_than_static_friction_force( + self, force_mag + ): + """ + This test is for static friction when total forces applied + on the rod is smaller than the static friction force. + Fx < F_normal*mu_s + Parameters + ---------- + force_mag + """ + [rod, plane, rod_plane_contact, external_forces_collection] = self.initializer( + static_mu_array=np.array([1.0, 1.0, 0.0]), force_mag_long=force_mag + ) + + rod_plane_contact.apply_contact(rod, plane) + correct_forces = np.zeros((3, 3)) + assert_allclose(correct_forces, rod.external_forces, atol=Tolerance.atol()) + + @pytest.mark.parametrize("force_mag", [-20.0, -15.0, 15.0, 20.0]) + def test_axial_static_friction_total_force_larger_than_static_friction_force( + self, force_mag + ): + """ + This test is for static friction when total forces applied + on the rod is larger than the static friction force. + Fx > F_normal*mu_s + Parameters + ---------- + force_mag + + + """ + + [rod, plane, rod_plane_contact, external_forces_collection] = self.initializer( + static_mu_array=np.array([1.0, 1.0, 0.0]), force_mag_long=force_mag + ) + + rod_plane_contact.apply_contact(rod, plane) + correct_forces = np.zeros((3, 3)) + if np.sign(force_mag) < 0: + correct_forces[2] = ( + external_forces_collection[2] + ) - 1.0 * external_forces_collection[1] + else: + correct_forces[2] = ( + external_forces_collection[2] + ) + 1.0 * external_forces_collection[1] + + assert_allclose(correct_forces, rod.external_forces, atol=Tolerance.atol()) + + @pytest.mark.parametrize("velocity", [-1.0, -3.0, 1.0, 2.0, 5.0]) + @pytest.mark.parametrize("omega", [-5.0, -2.0, 0.0, 4.0, 6.0]) + def test_kinetic_rolling_friction(self, velocity, omega): + """ + This test is for testing kinetic rolling friction, + for different translational and angular velocities, + we compute the final external forces and torques on the rod + using apply friction function and compare results with + analytical solutions. + Parameters + ---------- + velocity + omega + + """ + [rod, plane, rod_plane_contact, external_forces_collection] = self.initializer( + kinetic_mu_array=np.array([0.0, 0.0, 1.0]) + ) + + rod.velocity_collection += np.array([velocity, 0.0, 0.0]).reshape(3, 1) + rod.omega_collection += np.array([0.0, 0.0, omega]).reshape(3, 1) + + rod_plane_contact.apply_contact(rod, plane) + + correct_forces = np.zeros((3, 3)) + correct_forces[0] = ( + -1.0 + * np.sign(velocity + omega * rod.radius[0]) + * np.fabs(external_forces_collection[1]) + ) + + assert_allclose(correct_forces, rod.external_forces, atol=Tolerance.atol()) + forces_on_elements = _node_to_element_mass_or_force(external_forces_collection) + correct_torques = np.zeros((3, 2)) + correct_torques[2] += ( + -1.0 + * np.sign(velocity + omega * rod.radius[0]) + * np.fabs(forces_on_elements[1]) + * rod.radius + ) + + assert_allclose(correct_torques, rod.external_torques, atol=Tolerance.atol()) + + @pytest.mark.parametrize("force_mag", [-20.0, -15.0, 15.0, 20.0]) + def test_static_rolling_friction_total_force_smaller_than_static_friction_force( + self, force_mag + ): + """ + In this test case static rolling friction force is tested. We set external and internal torques to + zero and only changed the force in rolling direction. In this test case, total force in rolling direction + is smaller than static friction force in rolling direction. Next test case will check what happens if + total forces in rolling direction larger than static friction force. + Parameters + ---------- + force_mag + + + """ + + [rod, plane, rod_plane_contact, external_forces_collection] = self.initializer( + static_mu_array=np.array([0.0, 0.0, 10.0]), force_mag_side=force_mag + ) + + rod_plane_contact.apply_contact(rod, plane) + + correct_forces = np.zeros((3, 3)) + correct_forces[0] = 2.0 / 3.0 * external_forces_collection[0] + assert_allclose(correct_forces, rod.external_forces, atol=Tolerance.atol()) + + forces_on_elements = _node_to_element_mass_or_force(external_forces_collection) + correct_torques = np.zeros((3, 2)) + correct_torques[2] += ( + -1.0 + * np.sign(forces_on_elements[0]) + * np.fabs(forces_on_elements[0]) + * rod.radius + / 3.0 + ) + + assert_allclose(correct_torques, rod.external_torques, atol=Tolerance.atol()) + + @pytest.mark.parametrize("force_mag", [-100.0, -80.0, 65.0, 95.0]) + def test_static_rolling_friction_total_force_larger_than_static_friction_force( + self, force_mag + ): + """ + In this test case static rolling friction force is tested. We set external and internal torques to + zero and only changed the force in rolling direction. In this test case, total force in rolling direction + is larger than static friction force in rolling direction. + Parameters + ---------- + force_mag + + + """ + + [rod, plane, rod_plane_contact, external_forces_collection] = self.initializer( + static_mu_array=np.array([0.0, 0.0, 1.0]), force_mag_side=force_mag + ) + + rod_plane_contact.apply_contact(rod, plane) + + correct_forces = np.zeros((3, 3)) + correct_forces[0] = external_forces_collection[0] - np.sign( + external_forces_collection[0] + ) * np.fabs(external_forces_collection[1]) + assert_allclose(correct_forces, rod.external_forces, atol=Tolerance.atol()) + + forces_on_elements = _node_to_element_mass_or_force(external_forces_collection) + correct_torques = np.zeros((3, 2)) + correct_torques[2] += ( + -1.0 + * np.sign(forces_on_elements[0]) + * np.fabs(forces_on_elements[1]) + * rod.radius + ) + + assert_allclose(correct_torques, rod.external_torques, atol=Tolerance.atol()) + + @pytest.mark.parametrize("torque_mag", [-3.0, -1.0, 2.0, 3.5]) + def test_static_rolling_friction_total_torque_smaller_than_static_friction_force( + self, torque_mag + ): + """ + In this test case, static rolling friction force tested with zero internal and external force and + with non-zero external torque. Here torque magnitude chosen such that total rolling force is + always smaller than the static friction force. + Parameters + ---------- + torque_mag + """ + + [rod, plane, rod_plane_contact, external_forces_collection] = self.initializer( + static_mu_array=np.array([0.0, 0.0, 10.0]) + ) + + external_torques = np.zeros((3, 2)) + external_torques[2] = torque_mag + rod.external_torques = external_torques.copy() + + rod_plane_contact.apply_contact(rod, plane) + + correct_forces = np.zeros((3, 3)) + correct_forces[0, :-1] -= external_torques[2] / (3.0 * rod.radius) + correct_forces[0, 1:] -= external_torques[2] / (3.0 * rod.radius) + assert_allclose(correct_forces, rod.external_forces, atol=Tolerance.atol()) + + correct_torques = np.zeros((3, 2)) + correct_torques[2] += external_torques[2] - 2.0 / 3.0 * external_torques[2] + + assert_allclose(correct_torques, rod.external_torques, atol=Tolerance.atol()) + + @pytest.mark.parametrize("torque_mag", [-10.0, -5.0, 6.0, 7.5]) + def test_static_rolling_friction_total_torque_larger_than_static_friction_force( + self, torque_mag + ): + """ + In this test case, static rolling friction force tested with zero internal and external force and + with non-zero external torque. Here torque magnitude chosen such that total rolling force is + always larger than the static friction force. Thus, lateral friction force will be equal to static + friction force. + Parameters + ---------- + torque_mag + + """ + + [rod, plane, rod_plane_contact, external_forces_collection] = self.initializer( + static_mu_array=np.array([0.0, 0.0, 1.0]) + ) + + external_torques = np.zeros((3, 2)) + external_torques[2] = torque_mag + rod.external_torques = external_torques.copy() + + rod_plane_contact.apply_contact(rod, plane) + + correct_forces = np.zeros((3, 3)) + correct_forces[0] = ( + -1.0 * np.sign(torque_mag) * np.fabs(external_forces_collection[1]) + ) + assert_allclose(correct_forces, rod.external_forces, atol=Tolerance.atol()) + + forces_on_elements = _node_to_element_mass_or_force(external_forces_collection) + correct_torques = external_torques + correct_torques[2] += -( + np.sign(torque_mag) * np.fabs(forces_on_elements[1]) * rod.radius + ) + + assert_allclose(correct_torques, rod.external_torques, atol=Tolerance.atol()) diff --git a/tests/test_contact_common_functions.py b/tests/test_contact_common_functions.py deleted file mode 100644 index ba4147a15..000000000 --- a/tests/test_contact_common_functions.py +++ /dev/null @@ -1,218 +0,0 @@ -__doc__ = """ Test common functions used in contact in Elastica.joint implementation, should be removed along with contact in joint""" - -import pytest -import numpy as np -from numpy.testing import assert_allclose -from elastica.joint import ( - _dot_product, - _norm, - _clip, - _out_of_bounds, - _find_min_dist, - _aabbs_not_intersecting, -) - - -class TestDotProduct: - "class to test the dot product function" - - @pytest.mark.parametrize("ndim", [3]) - def test_dot_product_using_numpy(self, ndim): - """ - This method was generated by "copilot" in VS code; - This method uses numpy dot product to compare with the output of our function, - numpy dot product uses an optimized implementation that takes advantage of - hardware-specific optimizations such as SIMD. - """ - vector1 = np.random.randn(ndim) - vector2 = np.random.randn(ndim) - dot_product = _dot_product(vector1, vector2) - assert_allclose(dot_product, np.dot(vector1, vector2)) - - def test_dot_product_with_verified_values(self): - "Testing function with analytically verified values" - - "test for parallel vectors" - vec1 = [1, 0, 0] - vec2 = [2, 0, 0] - dot_product = _dot_product(vec1, vec2) - assert_allclose(dot_product, 2) - """Calcutlations : vec1 . vect2 - [1, 0, 0] . [2, 0, 0] - 1*2 + 0*0 + 0*0 - 2 (Answer)""" - - "test for perpendicular vectors" - vec1 = [1, 0, 0] - vec2 = [0, 1, 0] - dot_product = _dot_product(vec1, vec2) - assert_allclose(dot_product, 0) - """Calcutlations : vec1 . vect2 - [1, 0, 0] . [0, 1, 0] - 1*0 + 0*1 + 0*0 - 0 (Answer)""" - - "test for opposite vectors" - vec1 = [1, 0, 0] - vec2 = [-1, 0, 0] - dot_product = _dot_product(vec1, vec2) - assert_allclose(dot_product, -1) - """Calcutlations : vec1 . vect2 - [1, 0, 0] . [-1, 0, 0] - 1*-1 + 0*0 + 0*0 - -1 (Answer)""" - - "test for arbitrary vectors" - vec1 = [1, -2, 3] - vec2 = [-2, 1, 3] - dot_product = _dot_product(vec1, vec2) - assert_allclose(dot_product, 5) - """Calcutlations : vec1 . vect2 - [1, -2, 3] . [-2, 1, 3] - 1*-2 + -2*1 + 3*3 - -2 - 2 + 9 - 5 (Answer)""" - - -def test_norm_with_verified_values(): - "Function to test the _norm function" - - "Testing function with analytically verified values" - - "test for null vector" - vec1 = [0, 0, 0] - norm = _norm(vec1) - assert_allclose(norm, 0) - """Calcutlations : sqrt(0^2 + 0^2 + 0^2) - sqrt(0 + 0 + 0) - 0 (Answer)""" - - "test for unit vector" - vec1 = [1, 1, 1] - norm = _norm(vec1) - assert_allclose(norm, 1.7320508075688772) - """Calcutlations : sqrt(1^2 + 1^2 + 1^2) - sqrt(1 + 1 + 1) - sqrt(3) - 1.7320508075688772 (Answer)""" - - "test for arbitrary natural number vector" - vec1 = [1, 2, 3] - norm = _norm(vec1) - assert_allclose(norm, 3.7416573867739413) - """Calcutlations : sqrt(1^2 + 2^2 + 3^2) - sqrt(1 + 4 + 9) - sqrt(14) - 3.7416573867739413 (Answer)""" - - "test for decimal values vector" - vec1 = [0.001, 0.002, 0.003] - norm = _norm(vec1) - assert_allclose(norm, 0.0037416573867739412) - """Calcutlations : sqrt(0.001^2 + 0.002^2 + 0.003^2) - sqrt(0.000001 + 0.000004 + 0.000009) - sqrt(0.000014) - 0.0037416573867739412 (Answer)""" - - -@pytest.mark.parametrize( - "x, result", - [(0.5, 1), (1.5, 1.5), (2.5, 2)], -) -def test_clip_with_verified_values(x, result): - - "Function to test the _clip function" - - """_clip is a simple comparison function; - The tests are made to get _clip to return 'low', 'x', 'high' in the respective order""" - - low = 1.0 - high = 2.0 - assert _clip(x, low, high) == result - - -@pytest.mark.parametrize( - "x, result", - [(0.5, 1), (1.5, 0), (2.5, 1)], -) -def test_out_of_bounds_with_verified_values(x, result): - - "Function to test the _out_of_bounds function" - - """_out_of_bounds returns 1 if x < low or x > high, else returns 0""" - - low = 1.0 - high = 2.0 - assert _out_of_bounds(x, low, high) == result - - -def test_find_min_dist(): - "Function to test the _find_min_dist function" - - "testing function with analytically verified values" - - "intersecting lines" - x1 = np.array([0, 0, 0]) - e1 = np.array([1, 1, 1]) - # line 1 starts along origin and points towards (1,1,1) - x2 = np.array([0, 1, 0]) - e2 = np.array([1, 0, 1]) - # line 2 starts along (0,1,0) and points towards (1,0,1) - min_dist_vec, contact_point_of_system2, contact_point_of_system1 = _find_min_dist( - x1, e1, x2, e2 - ) - assert_allclose(min_dist_vec, [0, 0, 0]) - # since the lines intersect, the minimum distance is 0. - assert_allclose(contact_point_of_system2, [1, 1, 1]) - # the contact point of line 2 and line 1 is (1,1,1) - assert_allclose(contact_point_of_system1, [-1, -1, -1]) - # the function returns -1 for contact point of system 1 in case of intersecting lines - - "non intersecting lines" - x1 = np.array([0, 0, 0]) - e1 = np.array([1, 0, 0]) - # line 1 starts along origin and points towards (1,0,0) - x2 = np.array([0, 1, 0]) - e2 = np.array([0, 0, 1]) - # line 2 starts along (0,1,0) and points towards (0,0,1) - min_dist_vec, contact_point_of_system2, contact_point_of_system1 = _find_min_dist( - x1, e1, x2, e2 - ) - assert_allclose(min_dist_vec, [0, 1, 0]) - # minimum distance is 1 unit(verified using GeoGebra 3D calculator visualisation) - assert_allclose(contact_point_of_system2, [0, 1, 0]) - assert_allclose(contact_point_of_system1, [0, 0, 0]) - - "parallel lines" - x1 = np.array([0, 0, 0]) - e1 = np.array([1, 0, 0]) - # line 1 starts along origin and points towards (1,0,0) - x2 = np.array([0, 1, 0]) - e2 = np.array([1, 1, 0]) - # line 2 starts along (0,1,0) and points towards (1,1,0) - min_dist_vec, contact_point_of_system2, contact_point_of_system1 = _find_min_dist( - x1, e1, x2, e2 - ) - assert_allclose(min_dist_vec, [0, 1, 0]) - # minimum distance is 1 unit(verified using GeoGebra 3D calculator visualisation) - assert_allclose(contact_point_of_system2, [0, 1, 0]) - assert_allclose(contact_point_of_system1, [0, 0, 0]) - - -def test_aabbs_not_intersectin(): - "Function to test the _aabb_intersecting function" - - "testing function with analytically verified values" - - " intersecting boxes" - aabb_one = np.array([[0, 0], [0, 0], [0, 0]]) - aabb_two = np.array([[0, 0], [0, 0], [0, 0]]) - "both boxes are overlapping perfectly thus intersecting" - assert _aabbs_not_intersecting(aabb_one, aabb_two) == 0 - - " non Intersecting boxes" - aabb_one = np.array([[0, 1], [0, 1], [0, 1]]) - "box one is a unit size cube in the first quadrant with origin as one of its vertices" - aabb_two = np.array([[2, 3], [2, 3], [2, 3]]) - "box two is a unit size cube in the first quadrant with (2,2,2) as the closest vertex to box 1" - assert _aabbs_not_intersecting(aabb_one, aabb_two) == 1 diff --git a/tests/test_contact_functions.py b/tests/test_contact_functions.py index bf41feba8..f25d5830b 100644 --- a/tests/test_contact_functions.py +++ b/tests/test_contact_functions.py @@ -5,7 +5,7 @@ from elastica.typing import RodBase from elastica.rigidbody import Cylinder, Sphere -from elastica.contact_forces import ( +from elastica._contact_functions import ( _calculate_contact_forces_rod_cylinder, _calculate_contact_forces_rod_rod, _calculate_contact_forces_self_rod, diff --git a/tests/test_contact_specific_functions.py b/tests/test_contact_specific_functions.py deleted file mode 100644 index 4e5df1596..000000000 --- a/tests/test_contact_specific_functions.py +++ /dev/null @@ -1,680 +0,0 @@ -__doc__ = """ Test specific functions used in contact in Elastica.joint implementation, should be removed along with contact in joint""" - -import numpy as np -from numpy.testing import assert_allclose -from elastica.typing import RodBase, RigidBodyBase -from elastica.joint import ( - _prune_using_aabbs_rod_rigid_body, - _prune_using_aabbs_rod_rod, - _calculate_contact_forces_rod_rigid_body, - _calculate_contact_forces_rod_rod, - _calculate_contact_forces_self_rod, -) - - -def mock_rod_init(self): - - "Initializing Rod" - - """ - This is a small rod with 2 elements; - Initial Parameters: - element's radius = 1, length = 1, - tangent vector for both elements is (1, 0, 0), - stationary rod i.e velocity vector of each node is (0, 0, 0), - internal/external forces vectors are also (0, 0, 0) - """ - - self.n_elems = 2 - self.position_collection = np.array([[1, 2, 3], [0, 0, 0], [0, 0, 0]]) - self.radius_collection = np.array([1, 1]) - self.length_collection = np.array([1, 1]) - self.tangent_collection = np.array([[1.0, 1.0], [0.0, 0.0], [0.0, 0.0]]) - self.velocity_collection = np.array( - [[0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]] - ) - self.internal_forces = np.array([[0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]]) - self.external_forces = np.array([[0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]]) - - -def mock_rigid_body_init(self): - - "Initializing Rigid Body as a cylinder" - - """ - This is a rigid body cylinder;, - Initial Parameters: - radius = 1, length = 2, - center positioned at origin i.e (0, 0, 0), - cylinder's upright in x,y,z plane thus the director array, - stationary cylinder i.e velocity vector is (0, 0, 0), - external forces and torques vectors are also (0, 0, 0) - """ - - self.n_elems = 1 - self.position = np.array([[0], [0], [0]]) - self.director = np.array( - [[[1.0], [0.0], [0.0]], [[0.0], [1.0], [0.0]], [[0.0], [0.0], [1.0]]] - ) - self.radius = 1.0 - self.length = 2.0 - self.velocity_collection = np.array([[0.0], [0.0], [0.0]]) - self.external_forces = np.array([[0.0], [0.0], [0.0]]) - self.external_torques = np.array([[0.0], [0.0], [0.0]]) - - -MockRod = type("MockRod", (RodBase,), {"__init__": mock_rod_init}) - -MockRigidBody = type( - "MockRigidBody", (RigidBodyBase,), {"__init__": mock_rigid_body_init} -) - - -def test_prune_using_aabbs_rod_rigid_body(): - "Function to test the prune using aabbs rod rigid body function" - - "Testing function with analytically verified values" - - "Intersecting rod and cylinder" - - """ - Since both the initialized rod and cylinder are overlapping in 3D space at (1, 1, 1); - Hence they are intersectiong and the function should return 0 - """ - rod = MockRod() - cylinder = MockRigidBody() - assert ( - _prune_using_aabbs_rod_rigid_body( - rod.position_collection, - rod.radius_collection, - rod.length_collection, - cylinder.position, - cylinder.director, - cylinder.radius, - cylinder.length, - ) - == 0 - ) - - "Non - Intersecting rod and cylinder" - rod = MockRod() - cylinder = MockRigidBody() - - """ - Changing the position of cylinder in 3D space so the rod and cylinder don't overlap/intersect. - """ - cylinder.position = np.array([[20], [3], [4]]) - assert ( - _prune_using_aabbs_rod_rigid_body( - rod.position_collection, - rod.radius_collection, - rod.length_collection, - cylinder.position, - cylinder.director, - cylinder.radius, - cylinder.length, - ) - == 1 - ) - - -def test_prune_using_aabbs_rod_rod(): - "Function to test the prune using aabbs rod rod function" - - "Testing function with analytically verified values" - - "Intersecting rod and rod" - """ - Since both the rods have same position, node's radius and length, they are overlapping/intersecting in 3D space. - """ - rod_one = MockRod() - rod_two = MockRod() - assert ( - _prune_using_aabbs_rod_rod( - rod_one.position_collection, - rod_one.radius_collection, - rod_one.length_collection, - rod_two.position_collection, - rod_two.radius_collection, - rod_two.length_collection, - ) - == 0 - ) - - "Non - Intersecting rod and rod" - """ - Changing the position of rod_two in 3D space so the rod_one and rod_two don't overlap/intersect. - """ - rod_two.position_collection = np.array([[20, 21, 22], [0, 0, 0], [0, 0, 0]]) - assert ( - _prune_using_aabbs_rod_rod( - rod_one.position_collection, - rod_one.radius_collection, - rod_one.length_collection, - rod_two.position_collection, - rod_two.radius_collection, - rod_two.length_collection, - ) - == 1 - ) - - -class TestCalculateContactForcesRodRigidBody: - "Class to test the calculate contact forces rod rigid body function" - - "Testing function with handcrafted/calculated values" - - def test_calculate_contact_forces_rod_rigid_body_with_k_without_nu_and_friction( - self, - ): - - "initializing rod parameters" - rod = MockRod() - rod_element_position = 0.5 * ( - rod.position_collection[..., 1:] + rod.position_collection[..., :-1] - ) - - "initializing cylinder parameters" - cylinder = MockRigidBody() - x_cyl = ( - cylinder.position[..., 0] - - 0.5 * cylinder.length * cylinder.director[2, :, 0] - ) - - "initializing constants" - """ - Setting contact_k = 1 and other parameters to 0, - so the net forces becomes a function of contact forces only. - """ - k = 1.0 - nu = 0 - velocity_damping_coefficient = 0 - friction_coefficient = 0 - - "Function call" - _calculate_contact_forces_rod_rigid_body( - rod_element_position, - rod.length_collection * rod.tangent_collection, - cylinder.position[..., 0], - x_cyl, - cylinder.length * cylinder.director[2, :, 0], - rod.radius_collection + cylinder.radius, - rod.length_collection + cylinder.length, - rod.internal_forces, - rod.external_forces, - cylinder.external_forces, - cylinder.external_torques, - cylinder.director[:, :, 0], - rod.velocity_collection, - cylinder.velocity_collection, - k, - nu, - velocity_damping_coefficient, - friction_coefficient, - ) - - "Test values" - """ - The two systems were placed such that they are penetrating by 0.5 units and - resulting forces act along the x-axis only. - The net force was calculated by halving the contact force i.e - net force = 0.5 * contact force = -0.25; - where, contact force = k(1) * min distance between colliding elements(-1) * gamma(0.5) = -0.5 - The net force is then divided to the nodes of the rod and the cylinder as per indices. - """ - assert_allclose( - cylinder.external_forces, np.array([[-0.5], [0], [0]]), atol=1e-6 - ) - assert_allclose(cylinder.external_torques, np.array([[0], [0], [0]]), atol=1e-6) - assert_allclose( - rod.external_forces, - np.array([[0.166666, 0.333333, 0], [0, 0, 0], [0, 0, 0]]), - atol=1e-6, - ) - - def test_calculate_contact_forces_rod_rigid_body_with_nu_without_k_and_friction( - self, - ): - - "initializing rod parameters" - rod = MockRod() - "Moving rod towards the cylinder with a velocity of -1 in x-axis" - rod.velocity_collection = np.array([[-1, 0, 0], [-1, 0, 0], [-1, 0, 0]]) - rod_element_position = 0.5 * ( - rod.position_collection[..., 1:] + rod.position_collection[..., :-1] - ) - - "initializing cylinder parameters" - cylinder = MockRigidBody() - "Moving cylinder towards the rod with a velocity of 1 in x-axis" - cylinder.velocity_collection = np.array([[1], [0], [0]]) - x_cyl = ( - cylinder.position[..., 0] - - 0.5 * cylinder.length * cylinder.director[2, :, 0] - ) - - "initializing constants" - """ - Setting contact_nu = 1 and other parameters to 0, - so the net forces becomes a function of contact damping forces only. - """ - k = 0.0 - nu = 1.0 - velocity_damping_coefficient = 0 - friction_coefficient = 0 - - "Function call" - _calculate_contact_forces_rod_rigid_body( - rod_element_position, - rod.length_collection * rod.tangent_collection, - cylinder.position[..., 0], - x_cyl, - cylinder.length * cylinder.director[2, :, 0], - rod.radius_collection + cylinder.radius, - rod.length_collection + cylinder.length, - rod.internal_forces, - rod.external_forces, - cylinder.external_forces, - cylinder.external_torques, - cylinder.director[:, :, 0], - rod.velocity_collection, - cylinder.velocity_collection, - k, - nu, - velocity_damping_coefficient, - friction_coefficient, - ) - - "Test values" - """ - The two systems were placed such that they are penetrating by 0.5 units and - resulting forces act along the x-axis only. - The net force was calculated by halving the contact damping force i.e - net force = 0.5 * contact damping force = -0.75; - where, contact damping force = -nu(1) * penetration velocity(1.5)[x-axis] = -1.5 - The net force is then divided to the nodes of the rod and the cylinder as per indices. - """ - assert_allclose( - cylinder.external_forces, np.array([[-1.5], [0], [0]]), atol=1e-6 - ) - assert_allclose(cylinder.external_torques, np.array([[0], [0], [0]]), atol=1e-6) - assert_allclose( - rod.external_forces, - np.array([[0.5, 1, 0], [0, 0, 0], [0, 0, 0]]), - atol=1e-6, - ) - - def test_calculate_contact_forces_rod_rigid_body_with_k_and_nu_without_friction( - self, - ): - - "initializing rod parameters" - rod = MockRod() - "Moving rod towards the cylinder with a velocity of -1 in x-axis" - rod.velocity_collection = np.array([[-1, 0, 0], [-1, 0, 0], [-1, 0, 0]]) - rod_element_position = 0.5 * ( - rod.position_collection[..., 1:] + rod.position_collection[..., :-1] - ) - - "initializing cylinder parameters" - cylinder = MockRigidBody() - "Moving cylinder towards the rod with a velocity of 1 in x-axis" - cylinder.velocity_collection = np.array([[1], [0], [0]]) - x_cyl = ( - cylinder.position[..., 0] - - 0.5 * cylinder.length * cylinder.director[2, :, 0] - ) - - "initializing constants" - """ - Setting contact_nu = 1 and contact_k = 1, - so the net forces becomes a function of contact damping and contact forces. - """ - k = 1.0 - nu = 1.0 - velocity_damping_coefficient = 0 - friction_coefficient = 0 - - "Function call" - _calculate_contact_forces_rod_rigid_body( - rod_element_position, - rod.length_collection * rod.tangent_collection, - cylinder.position[..., 0], - x_cyl, - cylinder.length * cylinder.director[2, :, 0], - rod.radius_collection + cylinder.radius, - rod.length_collection + cylinder.length, - rod.internal_forces, - rod.external_forces, - cylinder.external_forces, - cylinder.external_torques, - cylinder.director[:, :, 0], - rod.velocity_collection, - cylinder.velocity_collection, - k, - nu, - velocity_damping_coefficient, - friction_coefficient, - ) - - "Test values" - """ - For nu and k dependent case, we just have to add both the forces that were generated above. - """ - assert_allclose(cylinder.external_forces, np.array([[-2], [0], [0]]), atol=1e-6) - assert_allclose(cylinder.external_torques, np.array([[0], [0], [0]]), atol=1e-6) - assert_allclose( - rod.external_forces, - np.array([[0.666666, 1.333333, 0], [0, 0, 0], [0, 0, 0]]), - atol=1e-6, - ) - - def test_calculate_contact_forces_rod_rigid_body_with_k_and_nu_and_friction(self): - - "initializing rod parameters" - rod = MockRod() - "Moving rod towards the cylinder with a velocity of -1 in x-axis" - rod.velocity_collection = np.array([[-1, 0, 0], [-1, 0, 0], [-1, 0, 0]]) - rod_element_position = 0.5 * ( - rod.position_collection[..., 1:] + rod.position_collection[..., :-1] - ) - - "initializing cylinder parameters" - cylinder = MockRigidBody() - "Moving cylinder towards the rod with a velocity of 1 in x-axis" - cylinder.velocity_collection = np.array([[1], [0], [0]]) - x_cyl = ( - cylinder.position[..., 0] - - 0.5 * cylinder.length * cylinder.director[2, :, 0] - ) - - "initializing constants" - k = 1.0 - nu = 1.0 - velocity_damping_coefficient = 0.1 - friction_coefficient = 0.1 - - "Function call" - _calculate_contact_forces_rod_rigid_body( - rod_element_position, - rod.length_collection * rod.tangent_collection, - cylinder.position[..., 0], - x_cyl, - cylinder.length * cylinder.director[2, :, 0], - rod.radius_collection + cylinder.radius, - rod.length_collection + cylinder.length, - rod.internal_forces, - rod.external_forces, - cylinder.external_forces, - cylinder.external_torques, - cylinder.director[:, :, 0], - rod.velocity_collection, - cylinder.velocity_collection, - k, - nu, - velocity_damping_coefficient, - friction_coefficient, - ) - - "Test values" - """ - With friction, we have to subtract the frictional forces from the net contact forces. - from above values the frictional forces are calculated as: - coulombic friction force = friction coefficient(0.1) * net contact force before friction(1) * slip_direction_velocity_unitized(0.5^-2) = 0.07071... (for y and z axis) - slip direction friction = velocity damping coefficient(0.1) * slip_direction_velocity(0.5^-2) * slip_direction_velocity_unitized(0.5^-2) = 0.05 (for y and z axis) - the minimum of the two is slip direction friciton, so the frictional force is that only: - friction force = slip direction friction = 0.05 - after applying sign convention and dividing the force among the nodes of rod and cylinder, - we get the following values. - """ - assert_allclose( - cylinder.external_forces, np.array([[-2], [-0.1], [-0.1]]), atol=1e-6 - ) - assert_allclose(cylinder.external_torques, np.array([[0], [0], [0]]), atol=1e-6) - assert_allclose( - rod.external_forces, - np.array( - [ - [0.666666, 1.333333, 0], - [0.033333, 0.066666, 0], - [0.033333, 0.066666, 0], - ] - ), - atol=1e-6, - ) - - -class TestCalculateContactForcesRodRod: - "Function to test the calculate contact forces rod rod function" - - "Testing function with handcrafted/calculated values" - - def test_calculate_contact_forces_rod_rod_with_k_without_nu(self): - - rod_one = MockRod() - rod_two = MockRod() - """Placing rod two such that its first element just touches the last element of rod one.""" - rod_two.position_collection = np.array([[4, 5, 6], [0, 0, 0], [0, 0, 0]]) - - "initializing constants" - """ - Setting contact_k = 1 and nu to 0, - so the net forces becomes a function of contact forces only. - """ - k = 1.0 - nu = 0.0 - - "Function call" - _calculate_contact_forces_rod_rod( - rod_one.position_collection[..., :-1], - rod_one.radius_collection, - rod_one.length_collection, - rod_one.tangent_collection, - rod_one.velocity_collection, - rod_one.internal_forces, - rod_one.external_forces, - rod_two.position_collection[..., :-1], - rod_two.radius_collection, - rod_two.length_collection, - rod_two.tangent_collection, - rod_two.velocity_collection, - rod_two.internal_forces, - rod_two.external_forces, - k, - nu, - ) - - "Test values" - """ - Resulting forces act along the x-axis only. - The net force was calculated by halving the contact force i.e - net force = 0.5 * contact force = 0.5; - where, contact force = k(1) * min distance between colliding elements(1) * gamma(1) = 1 - The net force is then divided to the nodes of the two rods as per indices. - """ - assert_allclose( - rod_one.external_forces, - np.array( - [[0, -0.666666, -0.333333], [0, 0, 0], [0, 0, 0]], - ), - atol=1e-6, - ) - assert_allclose( - rod_two.external_forces, - np.array([[0.333333, 0.666666, 0], [0, 0, 0], [0, 0, 0]]), - atol=1e-6, - ) - - def test_calculate_contact_forces_rod_rod_without_k_with_nu(self): - - rod_one = MockRod() - rod_two = MockRod() - """Placing rod two such that its first element just touches the last element of rod one.""" - rod_two.position_collection = np.array([[4, 5, 6], [0, 0, 0], [0, 0, 0]]) - - """Moving the rods towards each other with a velocity of 1 along the x-axis.""" - rod_one.velocity_collection = np.array([[1, 0, 0], [1, 0, 0], [1, 0, 0]]) - rod_two.velocity_collection = np.array([[-1, 0, 0], [-1, 0, 0], [-1, 0, 0]]) - - "initializing constants" - """ - Setting contact_nu = 1 and nu to 0, - so the net forces becomes a function of contact damping forces only. - """ - k = 0.0 - nu = 1.0 - - "Function call" - _calculate_contact_forces_rod_rod( - rod_one.position_collection[..., :-1], - rod_one.radius_collection, - rod_one.length_collection, - rod_one.tangent_collection, - rod_one.velocity_collection, - rod_one.internal_forces, - rod_one.external_forces, - rod_two.position_collection[..., :-1], - rod_two.radius_collection, - rod_two.length_collection, - rod_two.tangent_collection, - rod_two.velocity_collection, - rod_two.internal_forces, - rod_two.external_forces, - k, - nu, - ) - - "Test values" - """ - Resulting forces act along the x-axis only. - The net force was calculated by halving the contact damping force i.e - net force = 0.5 * contact damping force = 0.25; - where, contact damping force = nu(1) * penetration velocity(0.5)[x-axis] = 0.5 - The net force is then divided to the nodes of the two rods as per indices. - """ - assert_allclose( - rod_one.external_forces, - np.array( - [[0, -0.333333, -0.166666], [0, 0, 0], [0, 0, 0]], - ), - atol=1e-6, - ) - assert_allclose( - rod_two.external_forces, - np.array([[0.166666, 0.333333, 0], [0, 0, 0], [0, 0, 0]]), - atol=1e-6, - ) - - def test_calculate_contact_forces_rod_rod_with_k_and_nu(self): - - rod_one = MockRod() - rod_two = MockRod() - """Placing rod two such that its first element just touches the last element of rod one.""" - rod_two.position_collection = np.array([[4, 5, 6], [0, 0, 0], [0, 0, 0]]) - - """Moving the rods towards each other with a velocity of 1 along the x-axis.""" - rod_one.velocity_collection = np.array([[1, 0, 0], [1, 0, 0], [1, 0, 0]]) - rod_two.velocity_collection = np.array([[-1, 0, 0], [-1, 0, 0], [-1, 0, 0]]) - - "initializing constants" - """ - Setting contact_nu = 1 and contact_k = 1, - so the net forces becomes a function of contact damping and contact forces. - """ - k = 1.0 - nu = 1.0 - - "Function call" - _calculate_contact_forces_rod_rod( - rod_one.position_collection[..., :-1], - rod_one.radius_collection, - rod_one.length_collection, - rod_one.tangent_collection, - rod_one.velocity_collection, - rod_one.internal_forces, - rod_one.external_forces, - rod_two.position_collection[..., :-1], - rod_two.radius_collection, - rod_two.length_collection, - rod_two.tangent_collection, - rod_two.velocity_collection, - rod_two.internal_forces, - rod_two.external_forces, - k, - nu, - ) - - "Test values" - """ - For nu and k dependent case, we just have to add both the forces that were generated above. - """ - assert_allclose( - rod_one.external_forces, - np.array( - [[0, -1, -0.5], [0, 0, 0], [0, 0, 0]], - ), - atol=1e-6, - ) - assert_allclose( - rod_two.external_forces, - np.array([[0.5, 1, 0], [0, 0, 0], [0, 0, 0]]), - atol=1e-6, - ) - - -def test_calculate_contact_forces_self_rod(): - "Function to test the calculate contact forces self rod function" - - "Testing function with handcrafted/calculated values" - - rod = MockRod() - """Changing rod parameters to establish self contact in rod; - elements are placed such that the a 'U' rod is formed in the x-y plane, - where the rod is penetrating itself by 0.5 units by radius.""" - rod.n_elems = 3 - rod.position_collection = np.array([[1, 4, 4, 1], [0, 0, 1, 1], [0, 0, 0, 0]]) - rod.radius_collection = np.array([1, 1, 1]) - rod.length_collection = np.array([3, 1, 3]) - rod.tangent_collection = np.array( - [[1.0, 0.0, -1.0], [0.0, 1.0, 0.0], [0.0, 0.0, 0.0]] - ) - rod.velocity_collection = np.array( - [[0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0]] - ) - rod.internal_forces = np.array( - [[0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0]] - ) - rod.external_forces = np.array( - [[0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0]] - ) - - "initializing constants" - k = 1.0 - nu = 1.0 - - "Function call" - _calculate_contact_forces_self_rod( - rod.position_collection[..., :-1], - rod.radius_collection, - rod.length_collection, - rod.tangent_collection, - rod.velocity_collection, - rod.external_forces, - k, - nu, - ) - - "Test values" - """Resulting forces act along the y-axis only. - Since the rod is stationary i.e velocity = 0, the net force is a function of contact force only. - The net force was calculated by halving the contact force i.e - net force = 0.5 * contact force = -0.5; - where, contact force = k(1) * minimum distance between colliding elements centres(-1) * gamma(1) = -1 - The net force is then divided to the nodes of the rod as per indices.""" - assert_allclose( - rod.external_forces, - np.array( - [[0, 0, 0, 0], [-0.333333, -0.666666, 0.666666, 0.333333], [0, 0, 0, 0]] - ), - atol=1e-6, - ) diff --git a/tests/test_contact_utils.py b/tests/test_contact_utils.py index 392fe28a5..e459d8154 100644 --- a/tests/test_contact_utils.py +++ b/tests/test_contact_utils.py @@ -3,6 +3,7 @@ import pytest import numpy as np from numpy.testing import assert_allclose +from elastica.utils import Tolerance from elastica.typing import RodBase from elastica.rigidbody import Cylinder, Sphere from elastica.contact_utils import ( @@ -15,6 +16,11 @@ _prune_using_aabbs_rod_cylinder, _prune_using_aabbs_rod_rod, _prune_using_aabbs_rod_sphere, + _find_slipping_elements, + _node_to_element_mass_or_force, + _elements_to_nodes_inplace, + _node_to_element_position, + _node_to_element_velocity, ) @@ -204,7 +210,7 @@ def test_find_min_dist(): assert_allclose(contact_point_of_system1, [0, 0, 0]) -def test_aabbs_not_intersectin(): +def test_aabbs_not_intersecting(): "Function to test the _aabb_intersecting function" "testing function with analytically verified values" @@ -438,3 +444,138 @@ def test_prune_using_aabbs_rod_sphere(): ) == 1 ) + + +class TestRodPlaneAuxiliaryFunctions: + @pytest.mark.parametrize("n_elem", [2, 3, 5, 10, 20]) + def test_linear_interpolation_slip(self, n_elem): + velocity_threshold = 1.0 + + # if slip velocity larger than threshold + velocity_slip = np.repeat( + np.array([0.0, 0.0, 2.0]).reshape(3, 1), n_elem, axis=1 + ) + slip_function = _find_slipping_elements(velocity_slip, velocity_threshold) + correct_slip_function = np.zeros(n_elem) + assert_allclose(correct_slip_function, slip_function, atol=Tolerance.atol()) + + # if slip velocity smaller than threshold + velocity_slip = np.repeat( + np.array([0.0, 0.0, 0.0]).reshape(3, 1), n_elem, axis=1 + ) + slip_function = _find_slipping_elements(velocity_slip, velocity_threshold) + correct_slip_function = np.ones(n_elem) + assert_allclose(correct_slip_function, slip_function, atol=Tolerance.atol()) + + # if slip velocity smaller than threshold but very close to threshold + velocity_slip = np.repeat( + np.array([0.0, 0.0, 1.0 - 1e-6]).reshape(3, 1), n_elem, axis=1 + ) + slip_function = _find_slipping_elements(velocity_slip, velocity_threshold) + correct_slip_function = np.ones(n_elem) + assert_allclose(correct_slip_function, slip_function, atol=Tolerance.atol()) + + # if slip velocity larger than threshold but very close to threshold + velocity_slip = np.repeat( + np.array([0.0, 0.0, 1.0 + 1e-6]).reshape(3, 1), n_elem, axis=1 + ) + slip_function = _find_slipping_elements(velocity_slip, velocity_threshold) + correct_slip_function = np.ones(n_elem) - 1e-6 + assert_allclose(correct_slip_function, slip_function, atol=Tolerance.atol()) + + # if half of the array slip velocity is larger than threshold and half of it + # smaller than threshold + velocity_slip = np.hstack( + ( + np.repeat(np.array([0.0, 0.0, 2.0]).reshape(3, 1), n_elem, axis=1), + np.repeat(np.array([0.0, 0.0, 0.0]).reshape(3, 1), n_elem, axis=1), + ) + ) + slip_function = _find_slipping_elements(velocity_slip, velocity_threshold) + correct_slip_function = np.hstack((np.zeros(n_elem), np.ones(n_elem))) + assert_allclose(correct_slip_function, slip_function, atol=Tolerance.atol()) + + @pytest.mark.parametrize("n_elem", [2, 3, 5, 10, 20]) + def test_node_to_element_mass_or_force(self, n_elem): + random_vector = np.random.rand(3).reshape(3, 1) + input = np.repeat(random_vector, n_elem + 1, axis=1) + input[..., 0] *= 0.5 + input[..., -1] *= 0.5 + correct_output = np.repeat(random_vector, n_elem, axis=1) + output = _node_to_element_mass_or_force(input) + assert_allclose(correct_output, output, atol=Tolerance.atol()) + assert_allclose(np.sum(input), np.sum(output), atol=Tolerance.atol()) + + # These functions are used in the case if Numba is available + class TestGeneralAuxiliaryFunctions: + @pytest.mark.parametrize("n_elem", [2, 3, 5, 10, 20]) + def test_node_to_element_position(self, n_elem): + """ + This function tests _node_to_element_position function. We are + converting node positions to element positions. Here also + we are using numba to speed up the process. + + Parameters + ---------- + n_elem + + Returns + ------- + + """ + random = np.random.rand() # Adding some random numbers + input_position = random * np.ones((3, n_elem + 1)) + correct_output = random * np.ones((3, n_elem)) + + output = _node_to_element_position(input_position) + assert_allclose(correct_output, output, atol=Tolerance.atol()) + + @pytest.mark.parametrize("n_elem", [2, 3, 5, 10, 20]) + def test_node_to_element_velocity(self, n_elem): + """ + This function tests _node_to_element_velocity function. We are + converting node velocities to element velocities. Here also + we are using numba to speed up the process. + + Parameters + ---------- + n_elem + + Returns + ------- + + """ + random = np.random.rand() # Adding some random numbers + input_velocity = random * np.ones((3, n_elem + 1)) + input_mass = 2.0 * random * np.ones(n_elem + 1) + correct_output = random * np.ones((3, n_elem)) + + output = _node_to_element_velocity( + mass=input_mass, node_velocity_collection=input_velocity + ) + assert_allclose(correct_output, output, atol=Tolerance.atol()) + + @pytest.mark.parametrize("n_elem", [2, 3, 5, 10, 20]) + def test_elements_to_nodes_inplace(self, n_elem): + """ + This function _elements_to_nodes_inplace. We are + converting node velocities to element velocities. Here also + we are using numba to speed up the process. + + Parameters + ---------- + n_elem + + Returns + ------- + + """ + random = np.random.rand() # Adding some random numbers + vector_in_element_frame = random * np.ones((3, n_elem)) + vector_in_node_frame = np.zeros((3, n_elem + 1)) + correct_output = vector_in_node_frame.copy() + correct_output[:, :n_elem] += 0.5 * vector_in_element_frame + correct_output[:, 1 : n_elem + 1] += 0.5 * vector_in_element_frame + + _elements_to_nodes_inplace(vector_in_element_frame, vector_in_node_frame) + assert_allclose(correct_output, vector_in_node_frame, atol=Tolerance.atol()) diff --git a/tests/test_contact_wrapper_classes.py b/tests/test_contact_wrapper_classes.py deleted file mode 100644 index 0d595bfe6..000000000 --- a/tests/test_contact_wrapper_classes.py +++ /dev/null @@ -1,366 +0,0 @@ -__doc__ = """ Test Wrapper Classes used in contact in Elastica.joint implementation, should be removed along with contact in joint""" - -import numpy as np -from numpy.testing import assert_allclose -from elastica.joint import ExternalContact, SelfContact -from elastica.typing import RodBase, RigidBodyBase - - -def mock_rod_init(self): - - "Initializing Rod" - "Details of initialization are given in test_contact_specific_functions.py" - - self.n_elems = 2 - self.position_collection = np.array([[1, 2, 3], [0, 0, 0], [0, 0, 0]]) - self.radius = np.array([1, 1]) - self.lengths = np.array([1, 1]) - self.tangents = np.array([[1.0, 1.0], [0.0, 0.0], [0.0, 0.0]]) - self.internal_forces = np.array([[0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]]) - self.external_forces = np.array([[0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]]) - self.velocity_collection = np.array( - [[0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]] - ) - - -def mock_rigid_body_init(self): - - "Initializing Rigid Body" - "Details of initialization are given in test_contact_specific_functions.py" - - self.n_elems = 1 - self.position_collection = np.array([[0], [0], [0]]) - self.director_collection = np.array( - [[[1.0], [0.0], [0.0]], [[0.0], [1.0], [0.0]], [[0.0], [0.0], [1.0]]] - ) - self.radius = np.array([1.0]) - self.length = np.array([2.0]) - self.external_forces = np.array([[0.0], [0.0], [0.0]]) - self.external_torques = np.array([[0.0], [0.0], [0.0]]) - self.velocity_collection = np.array([[0.0], [0.0], [0.0]]) - - -MockRod = type("MockRod", (RodBase,), {"__init__": mock_rod_init}) - -MockRigidBody = type( - "MockRigidBody", (RigidBodyBase,), {"__init__": mock_rigid_body_init} -) - - -class TestExternalContact: - def test_external_contact_rod_rigid_body_with_collision_with_k_without_nu_and_friction( - self, - ): - - "Testing External Contact wrapper with Collision with analytical verified values" - - mock_rod = MockRod() - mock_rigid_body = MockRigidBody() - ext_contact = ExternalContact(k=1.0, nu=0.0) - ext_contact.apply_forces(mock_rod, 0, mock_rigid_body, 1) - - """Details and reasoning about the values are given in 'test_contact_specific_functions.py/test_claculate_contact_forces_rod_rigid_body()'""" - assert_allclose( - mock_rod.external_forces, - np.array([[0.166666, 0.333333, 0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]]), - atol=1e-6, - ) - - assert_allclose( - mock_rigid_body.external_forces, np.array([[-0.5], [0.0], [0.0]]), atol=1e-6 - ) - - assert_allclose( - mock_rigid_body.external_torques, np.array([[0.0], [0.0], [0.0]]), atol=1e-6 - ) - - def test_external_contact_rod_rigid_body_with_collision_with_nu_without_k_and_friction( - self, - ): - - "Testing External Contact wrapper with Collision with analytical verified values" - - mock_rod = MockRod() - "Moving rod towards the cylinder with a velocity of -1 in x-axis" - mock_rod.velocity_collection = np.array([[-1, 0, 0], [-1, 0, 0], [-1, 0, 0]]) - mock_rigid_body = MockRigidBody() - "Moving cylinder towards the rod with a velocity of 1 in x-axis" - mock_rigid_body.velocity_collection = np.array([[1], [0], [0]]) - ext_contact = ExternalContact(k=0.0, nu=1.0) - ext_contact.apply_forces(mock_rod, 0, mock_rigid_body, 1) - - """Details and reasoning about the values are given in 'test_contact_specific_functions.py/test_claculate_contact_forces_rod_rigid_body()'""" - assert_allclose( - mock_rod.external_forces, - np.array([[0.5, 1, 0], [0, 0, 0], [0, 0, 0]]), - atol=1e-6, - ) - - assert_allclose( - mock_rigid_body.external_forces, np.array([[-1.5], [0], [0]]), atol=1e-6 - ) - - assert_allclose( - mock_rigid_body.external_torques, np.array([[0.0], [0.0], [0.0]]), atol=1e-6 - ) - - def test_external_contact_rod_rigid_body_with_collision_with_k_and_nu_without_friction( - self, - ): - - "Testing External Contact wrapper with Collision with analytical verified values" - - mock_rod = MockRod() - "Moving rod towards the cylinder with a velocity of -1 in x-axis" - mock_rod.velocity_collection = np.array([[-1, 0, 0], [-1, 0, 0], [-1, 0, 0]]) - mock_rigid_body = MockRigidBody() - "Moving cylinder towards the rod with a velocity of 1 in x-axis" - mock_rigid_body.velocity_collection = np.array([[1], [0], [0]]) - ext_contact = ExternalContact(k=1.0, nu=1.0) - ext_contact.apply_forces(mock_rod, 0, mock_rigid_body, 1) - - """Details and reasoning about the values are given in 'test_contact_specific_functions.py/test_claculate_contact_forces_rod_rigid_body()'""" - assert_allclose( - mock_rod.external_forces, - np.array([[0.666666, 1.333333, 0], [0, 0, 0], [0, 0, 0]]), - atol=1e-6, - ) - - assert_allclose( - mock_rigid_body.external_forces, np.array([[-2], [0], [0]]), atol=1e-6 - ) - - assert_allclose( - mock_rigid_body.external_torques, np.array([[0.0], [0.0], [0.0]]), atol=1e-6 - ) - - def test_external_contact_rod_rigid_body_with_collision_with_k_and_nu_and_friction( - self, - ): - - "Testing External Contact wrapper with Collision with analytical verified values" - - mock_rod = MockRod() - "Moving rod towards the cylinder with a velocity of -1 in x-axis" - mock_rod.velocity_collection = np.array([[-1, 0, 0], [-1, 0, 0], [-1, 0, 0]]) - mock_rigid_body = MockRigidBody() - "Moving cylinder towards the rod with a velocity of 1 in x-axis" - mock_rigid_body.velocity_collection = np.array([[1], [0], [0]]) - ext_contact = ExternalContact( - k=1.0, nu=1.0, velocity_damping_coefficient=0.1, friction_coefficient=0.1 - ) - ext_contact.apply_forces(mock_rod, 0, mock_rigid_body, 1) - - """Details and reasoning about the values are given in 'test_contact_specific_functions.py/test_claculate_contact_forces_rod_rigid_body()'""" - assert_allclose( - mock_rod.external_forces, - np.array( - [ - [0.666666, 1.333333, 0], - [0.033333, 0.066666, 0], - [0.033333, 0.066666, 0], - ] - ), - atol=1e-6, - ) - - assert_allclose( - mock_rigid_body.external_forces, np.array([[-2], [-0.1], [-0.1]]), atol=1e-6 - ) - - assert_allclose( - mock_rigid_body.external_torques, np.array([[0.0], [0.0], [0.0]]), atol=1e-6 - ) - - def test_external_contact_rod_rigid_body_without_collision(self): - - "Testing External Contact wrapper without Collision with analytical verified values" - - mock_rod = MockRod() - mock_rigid_body = MockRigidBody() - ext_contact = ExternalContact(k=1.0, nu=0.5) - - """Setting rigid body position such that there is no collision""" - mock_rigid_body.position_collection = np.array([[400], [500], [600]]) - mock_rod_external_forces_before_execution = mock_rod.external_forces.copy() - mock_rigid_body_external_forces_before_execution = ( - mock_rigid_body.external_forces.copy() - ) - mock_rigid_body_external_torques_before_execution = ( - mock_rigid_body.external_torques.copy() - ) - ext_contact.apply_forces(mock_rod, 0, mock_rigid_body, 1) - - assert_allclose( - mock_rod.external_forces, mock_rod_external_forces_before_execution - ) - assert_allclose( - mock_rigid_body.external_forces, - mock_rigid_body_external_forces_before_execution, - ) - assert_allclose( - mock_rigid_body.external_torques, - mock_rigid_body_external_torques_before_execution, - ) - - def test_external_contact_with_two_rods_with_collision_with_k_without_nu(self): - - "Testing External Contact wrapper with two rods with analytical verified values" - "Test values have been copied from 'test_contact_specific_functions.py/test_calculate_contact_forces_rod_rod()'" - - mock_rod_one = MockRod() - mock_rod_two = MockRod() - mock_rod_two.position_collection = np.array([[4, 5, 6], [0, 0, 0], [0, 0, 0]]) - ext_contact = ExternalContact(k=1.0, nu=0.0) - ext_contact.apply_forces(mock_rod_one, 0, mock_rod_two, 0) - - assert_allclose( - mock_rod_one.external_forces, - np.array([[0, -0.666666, -0.333333], [0, 0, 0], [0, 0, 0]]), - atol=1e-6, - ) - assert_allclose( - mock_rod_two.external_forces, - np.array([[0.333333, 0.666666, 0], [0, 0, 0], [0, 0, 0]]), - atol=1e-6, - ) - - def test_external_contact_with_two_rods_with_collision_without_k_with_nu(self): - - "Testing External Contact wrapper with two rods with analytical verified values" - "Test values have been copied from 'test_contact_specific_functions.py/test_calculate_contact_forces_rod_rod()'" - - mock_rod_one = MockRod() - mock_rod_two = MockRod() - - """Moving the rods towards each other with a velocity of 1 along the x-axis.""" - mock_rod_one.velocity_collection = np.array([[1, 0, 0], [1, 0, 0], [1, 0, 0]]) - mock_rod_two.velocity_collection = np.array( - [[-1, 0, 0], [-1, 0, 0], [-1, 0, 0]] - ) - mock_rod_two.position_collection = np.array([[4, 5, 6], [0, 0, 0], [0, 0, 0]]) - ext_contact = ExternalContact(k=0.0, nu=1.0) - ext_contact.apply_forces(mock_rod_one, 0, mock_rod_two, 0) - - assert_allclose( - mock_rod_one.external_forces, - np.array( - [[0, -0.333333, -0.166666], [0, 0, 0], [0, 0, 0]], - ), - atol=1e-6, - ) - assert_allclose( - mock_rod_two.external_forces, - np.array([[0.166666, 0.333333, 0], [0, 0, 0], [0, 0, 0]]), - atol=1e-6, - ) - - def test_external_contact_with_two_rods_with_collision_with_k_and_nu(self): - - "Testing External Contact wrapper with two rods with analytical verified values" - "Test values have been copied from 'test_contact_specific_functions.py/test_calculate_contact_forces_rod_rod()'" - - mock_rod_one = MockRod() - mock_rod_two = MockRod() - - """Moving the rods towards each other with a velocity of 1 along the x-axis.""" - mock_rod_one.velocity_collection = np.array([[1, 0, 0], [1, 0, 0], [1, 0, 0]]) - mock_rod_two.velocity_collection = np.array( - [[-1, 0, 0], [-1, 0, 0], [-1, 0, 0]] - ) - mock_rod_two.position_collection = np.array([[4, 5, 6], [0, 0, 0], [0, 0, 0]]) - ext_contact = ExternalContact(k=1.0, nu=1.0) - ext_contact.apply_forces(mock_rod_one, 0, mock_rod_two, 0) - - assert_allclose( - mock_rod_one.external_forces, - np.array( - [[0, -1, -0.5], [0, 0, 0], [0, 0, 0]], - ), - atol=1e-6, - ) - assert_allclose( - mock_rod_two.external_forces, - np.array([[0.5, 1, 0], [0, 0, 0], [0, 0, 0]]), - atol=1e-6, - ) - - def test_external_contact_with_two_rods_without_collision(self): - - "Testing External Contact wrapper with two rods with analytical verified values" - - mock_rod_one = MockRod() - mock_rod_two = MockRod() - - "Setting rod two position such that there is no collision" - mock_rod_two.position_collection = np.array( - [[100, 101, 102], [0, 0, 0], [0, 0, 0]] - ) - ext_contact = ExternalContact(k=1.0, nu=1.0) - mock_rod_one_external_forces_before_execution = ( - mock_rod_one.external_forces.copy() - ) - mock_rod_two_external_forces_before_execution = ( - mock_rod_two.external_forces.copy() - ) - ext_contact.apply_forces(mock_rod_one, 0, mock_rod_two, 0) - - assert_allclose( - mock_rod_one.external_forces, mock_rod_one_external_forces_before_execution - ) - assert_allclose( - mock_rod_two.external_forces, mock_rod_two_external_forces_before_execution - ) - - -class TestSelfContact: - def test_self_contact_with_rod_self_collision(self): - - "Testing Self Contact wrapper rod self collision with analytical verified values" - - mock_rod = MockRod() - - "Test values have been copied from 'test_contact_specific_functions.py/test_calculate_contact_forces_self_rod()'" - mock_rod.n_elems = 3 - mock_rod.position_collection = np.array( - [[1, 4, 4, 1], [0, 0, 1, 1], [0, 0, 0, 0]] - ) - mock_rod.radius = np.array([1, 1, 1]) - mock_rod.lengths = np.array([3, 1, 3]) - mock_rod.tangents = np.array( - [[1.0, 0.0, -1.0], [0.0, 1.0, 0.0], [0.0, 0.0, 0.0]] - ) - mock_rod.velocity_collection = np.array( - [[0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0]] - ) - mock_rod.internal_forces = np.array( - [[0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0]] - ) - mock_rod.external_forces = np.array( - [[0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0]] - ) - sel_contact = SelfContact(k=1.0, nu=0.0) - sel_contact.apply_forces(mock_rod, 0, mock_rod, 0) - - assert_allclose( - mock_rod.external_forces, - np.array( - [[0, 0, 0, 0], [-0.333333, -0.666666, 0.666666, 0.333333], [0, 0, 0, 0]] - ), - atol=1e-6, - ) - - def test_self_contact_with_rod_no_self_collision(self): - - "Testing Self Contact wrapper rod no self collision with analytical verified values" - - mock_rod = MockRod() - - "the initially set rod does not have self collision" - mock_rod_external_forces_before_execution = mock_rod.external_forces.copy() - sel_contact = SelfContact(k=1.0, nu=1.0) - sel_contact.apply_forces(mock_rod, 0, mock_rod, 0) - - assert_allclose( - mock_rod.external_forces, mock_rod_external_forces_before_execution - ) diff --git a/tests/test_interaction.py b/tests/test_interaction.py index 307c143bc..6a4f48f28 100644 --- a/tests/test_interaction.py +++ b/tests/test_interaction.py @@ -9,8 +9,12 @@ find_slipping_elements, AnisotropicFrictionalPlane, node_to_element_mass_or_force, - nodes_to_elements, SlenderBodyTheory, + nodes_to_elements, + elements_to_nodes_inplace, +) +from elastica.contact_utils import ( + _node_to_element_mass_or_force, ) from tests.test_rod.mock_rod import MockTestRod @@ -319,63 +323,36 @@ def test_interaction_when_rod_is_under_plane_without_k_with_nu(self, n_elem, nu_ class TestAuxiliaryFunctions: @pytest.mark.parametrize("n_elem", [2, 3, 5, 10, 20]) - def test_linear_interpolation_slip(self, n_elem): + def test_linear_interpolation_slip_error_message(self, n_elem): velocity_threshold = 1.0 # if slip velocity larger than threshold velocity_slip = np.repeat( np.array([0.0, 0.0, 2.0]).reshape(3, 1), n_elem, axis=1 ) - slip_function = find_slipping_elements(velocity_slip, velocity_threshold) - correct_slip_function = np.zeros(n_elem) - assert_allclose(correct_slip_function, slip_function, atol=Tolerance.atol()) - - # if slip velocity smaller than threshold - velocity_slip = np.repeat( - np.array([0.0, 0.0, 0.0]).reshape(3, 1), n_elem, axis=1 - ) - slip_function = find_slipping_elements(velocity_slip, velocity_threshold) - correct_slip_function = np.ones(n_elem) - assert_allclose(correct_slip_function, slip_function, atol=Tolerance.atol()) - - # if slip velocity smaller than threshold but very close to threshold - velocity_slip = np.repeat( - np.array([0.0, 0.0, 1.0 - 1e-6]).reshape(3, 1), n_elem, axis=1 - ) - slip_function = find_slipping_elements(velocity_slip, velocity_threshold) - correct_slip_function = np.ones(n_elem) - assert_allclose(correct_slip_function, slip_function, atol=Tolerance.atol()) - - # if slip velocity larger than threshold but very close to threshold - velocity_slip = np.repeat( - np.array([0.0, 0.0, 1.0 + 1e-6]).reshape(3, 1), n_elem, axis=1 - ) - slip_function = find_slipping_elements(velocity_slip, velocity_threshold) - correct_slip_function = np.ones(n_elem) - 1e-6 - assert_allclose(correct_slip_function, slip_function, atol=Tolerance.atol()) - - # if half of the array slip velocity is larger than threshold and half of it - # smaller than threshold - velocity_slip = np.hstack( - ( - np.repeat(np.array([0.0, 0.0, 2.0]).reshape(3, 1), n_elem, axis=1), - np.repeat(np.array([0.0, 0.0, 0.0]).reshape(3, 1), n_elem, axis=1), - ) + error_message = ( + "This function is removed in v0.3.2. Please use\n" + "elastica.contact_utils._find_slipping_elements()\n" + "instead for finding slipping elements." ) - slip_function = find_slipping_elements(velocity_slip, velocity_threshold) - correct_slip_function = np.hstack((np.zeros(n_elem), np.ones(n_elem))) - assert_allclose(correct_slip_function, slip_function, atol=Tolerance.atol()) + with pytest.raises(NotImplementedError) as error_info: + slip_function = find_slipping_elements(velocity_slip, velocity_threshold) + assert error_info.value.args[0] == error_message @pytest.mark.parametrize("n_elem", [2, 3, 5, 10, 20]) - def test_node_to_element_mass_or_force(self, n_elem): + def test_node_to_element_mass_or_force_error_message(self, n_elem): random_vector = np.random.rand(3).reshape(3, 1) input = np.repeat(random_vector, n_elem + 1, axis=1) input[..., 0] *= 0.5 input[..., -1] *= 0.5 - correct_output = np.repeat(random_vector, n_elem, axis=1) - output = node_to_element_mass_or_force(input) - assert_allclose(correct_output, output, atol=Tolerance.atol()) - assert_allclose(np.sum(input), np.sum(output), atol=Tolerance.atol()) + error_message = ( + "This function is removed in v0.3.2. Please use\n" + "elastica.contact_utils._node_to_element_mass_or_force()\n" + "instead for converting the mass/forces on rod nodes to elements." + ) + with pytest.raises(NotImplementedError) as error_info: + output = node_to_element_mass_or_force(input) + assert error_info.value.args[0] == error_message @pytest.mark.parametrize("n_elem", [2, 10]) def test_not_impl_error_for_nodes_to_elements(self, n_elem): @@ -581,7 +558,7 @@ def test_kinetic_rolling_friction(self, n_elem, velocity, omega): ) assert_allclose(correct_forces, rod.external_forces, atol=Tolerance.atol()) - forces_on_elements = node_to_element_mass_or_force(external_forces_collection) + forces_on_elements = _node_to_element_mass_or_force(external_forces_collection) correct_torques = np.zeros((3, n_elem)) correct_torques[2] += ( -1.0 @@ -622,7 +599,7 @@ def test_static_rolling_friction_total_force_smaller_than_static_friction_force( correct_forces[0] = 2.0 / 3.0 * external_forces_collection[0] assert_allclose(correct_forces, rod.external_forces, atol=Tolerance.atol()) - forces_on_elements = node_to_element_mass_or_force(external_forces_collection) + forces_on_elements = _node_to_element_mass_or_force(external_forces_collection) correct_torques = np.zeros((3, n_elem)) correct_torques[2] += ( -1.0 @@ -665,7 +642,7 @@ def test_static_rolling_friction_total_force_larger_than_static_friction_force( ) * np.fabs(external_forces_collection[1]) assert_allclose(correct_forces, rod.external_forces, atol=Tolerance.atol()) - forces_on_elements = node_to_element_mass_or_force(external_forces_collection) + forces_on_elements = _node_to_element_mass_or_force(external_forces_collection) correct_torques = np.zeros((3, n_elem)) correct_torques[2] += ( -1.0 @@ -751,7 +728,7 @@ def test_static_rolling_friction_total_torque_larger_than_static_friction_force( ) assert_allclose(correct_forces, rod.external_forces, atol=Tolerance.atol()) - forces_on_elements = node_to_element_mass_or_force(external_forces_collection) + forces_on_elements = _node_to_element_mass_or_force(external_forces_collection) correct_torques = external_torques correct_torques[2] += -( np.sign(torque_mag) * np.fabs(forces_on_elements[1]) * rod.radius @@ -794,7 +771,7 @@ def test_sum_over_elements(self, n_elem): assert_allclose(correct_output, output, atol=Tolerance.atol()) @pytest.mark.parametrize("n_elem", [2, 3, 5, 10, 20]) - def test_node_to_element_position(self, n_elem): + def test_node_to_element_position_error_message(self, n_elem): """ This function tests node_to_element_position function. We are converting node positions to element positions. Here also @@ -812,11 +789,17 @@ def test_node_to_element_position(self, n_elem): input_position = random * np.ones((3, n_elem + 1)) correct_output = random * np.ones((3, n_elem)) - output = node_to_element_position(input_position) - assert_allclose(correct_output, output, atol=Tolerance.atol()) + error_message = ( + "This function is removed in v0.3.2. For node-to-element_position() interpolation please use: \n" + "elastica.contact_utils._node_to_element_position() for rod position \n" + "For detail, refer to issue #113." + ) + with pytest.raises(NotImplementedError) as error_info: + output = node_to_element_position(input_position) + assert error_info.value.args[0] == error_message @pytest.mark.parametrize("n_elem", [2, 3, 5, 10, 20]) - def test_node_to_element_velocity(self, n_elem): + def test_node_to_element_velocity_error_message(self, n_elem): """ This function tests node_to_element_velocity function. We are converting node velocities to element velocities. Here also @@ -835,10 +818,16 @@ def test_node_to_element_velocity(self, n_elem): input_mass = 2.0 * random * np.ones(n_elem + 1) correct_output = random * np.ones((3, n_elem)) - output = node_to_element_velocity( - mass=input_mass, node_velocity_collection=input_velocity + error_message = ( + "This function is removed in v0.3.2. For node-to-element_velocity() interpolation please use: \n" + "elastica.contact_utils._node_to_element_velocity() for rod velocity. \n" + "For detail, refer to issue #113." ) - assert_allclose(correct_output, output, atol=Tolerance.atol()) + with pytest.raises(NotImplementedError) as error_info: + output = node_to_element_velocity( + mass=input_mass, node_velocity_collection=input_velocity + ) + assert error_info.value.args[0] == error_message @pytest.mark.parametrize("n_elem", [2, 3, 5, 10, 20]) def test_not_impl_error_for_node_to_element_pos_or_vel(self, n_elem): @@ -846,14 +835,41 @@ def test_not_impl_error_for_node_to_element_pos_or_vel(self, n_elem): input_velocity = random * np.ones((3, n_elem + 1)) error_message = ( "This function is removed in v0.3.0. For node-to-element interpolation please use: \n" - "elastica.interaction.node_to_element_position() for rod position \n" - "elastica.interaction.node_to_element_velocity() for rod velocity. \n" + "elastica.contact_utils._node_to_element_position() for rod position \n" + "elastica.contact_utils._node_to_element_velocity() for rod velocity. \n" "For detail, refer to issue #80." ) with pytest.raises(NotImplementedError) as error_info: node_to_element_pos_or_vel(input_velocity) assert error_info.value.args[0] == error_message + @pytest.mark.parametrize("n_elem", [2, 3, 5, 10, 20]) + def test_elements_to_nodes_inplace_error_message(self, n_elem): + """ + This function _elements_to_nodes_inplace. We are + converting node velocities to element velocities. Here also + we are using numba to speed up the process. + + Parameters + ---------- + n_elem + + Returns + ------- + + """ + random = np.random.rand() # Adding some random numbers + vector_in_element_frame = random * np.ones((3, n_elem)) + vector_in_node_frame = np.zeros((3, n_elem + 1)) + error_message = ( + "This function is removed in v0.3.2. Please use\n" + "elastica.contact_utils._elements_to_nodes_inplace()\n" + "instead for updating nodal forces using the forces computed on elements." + ) + with pytest.raises(NotImplementedError) as error_info: + elements_to_nodes_inplace(vector_in_element_frame, vector_in_node_frame) + assert error_info.value.args[0] == error_message + except ImportError: pass diff --git a/tests/test_joint.py b/tests/test_joint.py index aa4b9579e..d016226a2 100644 --- a/tests/test_joint.py +++ b/tests/test_joint.py @@ -13,6 +13,8 @@ import numpy as np import pytest from scipy.spatial.transform import Rotation +from elastica.joint import ExternalContact, SelfContact + # TODO: change tests and made them independent of rod, at least assigin hardcoded values for forces and torques @@ -372,3 +374,650 @@ def test_fixedjoint(rest_euler_angle): assert_allclose( rod2.external_torques[..., rod2_index], torque_rod2, atol=Tolerance.atol() ) + + +from elastica.joint import ( + _dot_product, + _norm, + _clip, + _out_of_bounds, + _find_min_dist, + _aabbs_not_intersecting, +) + + +@pytest.mark.parametrize("ndim", [2, 3, 5, 10, 20]) +def test_dot_product_error_message(ndim): + vector1 = np.random.randn(ndim) + vector2 = np.random.randn(ndim) + error_message = ( + "This function is removed in v0.3.2. Please use\n" + "elastica.contact_utils._dot_product()\n" + "instead for find the dot product between a and b." + ) + with pytest.raises(NotImplementedError) as error_info: + dot_product = _dot_product(vector1, vector2) + assert error_info.value.args[0] == error_message + + +@pytest.mark.parametrize("ndim", [2, 3, 5, 10, 20]) +def test_norm_error_message(ndim): + vec1 = np.random.randn(ndim) + error_message = ( + "This function is removed in v0.3.2. Please use\n" + "elastica.contact_utils._norm()\n" + "instead for finding the norm of a." + ) + with pytest.raises(NotImplementedError) as error_info: + norm = _norm(vec1) + assert error_info.value.args[0] == error_message + + +@pytest.mark.parametrize( + "x, result", + [(0.5, 1), (1.5, 1.5), (2.5, 2)], +) +def test_clip_error_message(x, result): + low = 1.0 + high = 2.0 + error_message = ( + "This function is removed in v0.3.2. Please use\n" + "elastica.contact_utils._clip()\n" + "instead for clipping x." + ) + with pytest.raises(NotImplementedError) as error_info: + _clip(x, low, high) + assert error_info.value.args[0] == error_message + + +@pytest.mark.parametrize( + "x, result", + [(0.5, 1), (1.5, 1.5), (2.5, 2)], +) +def test_out_of_bounds_error_message(x, result): + low = 1.0 + high = 2.0 + error_message = ( + "This function is removed in v0.3.2. Please use\n" + "elastica.contact_utils._out_of_bounds()\n" + "instead for checking if x is out of bounds." + ) + with pytest.raises(NotImplementedError) as error_info: + _out_of_bounds(x, low, high) + assert error_info.value.args[0] == error_message + + +def test_find_min_dist_error_message(): + x1 = np.array([0, 0, 0]) + e1 = np.array([1, 1, 1]) + x2 = np.array([0, 1, 0]) + e2 = np.array([1, 0, 1]) + + error_message = ( + "This function is removed in v0.3.2. Please use\n" + "elastica.contact_utils._find_min_dist()\n" + "instead for finding minimum distance between contact points." + ) + with pytest.raises(NotImplementedError) as error_info: + ( + min_dist_vec, + contact_point_of_system2, + contact_point_of_system1, + ) = _find_min_dist(x1, e1, x2, e2) + assert error_info.value.args[0] == error_message + + +def test_aabbs_not_intersecting_error_message(): + aabb_one = np.array([[0, 0], [0, 0], [0, 0]]) + aabb_two = np.array([[0, 0], [0, 0], [0, 0]]) + error_message = ( + "This function is removed in v0.3.2. Please use\n" + "elastica.contact_utils._aabbs_not_intersecting()\n" + "instead for checking aabbs intersection." + ) + with pytest.raises(NotImplementedError) as error_info: + _aabbs_not_intersecting(aabb_one, aabb_two) + assert error_info.value.args[0] == error_message + + +from elastica.typing import RodBase, RigidBodyBase +from elastica.joint import ( + _prune_using_aabbs_rod_rigid_body, + _prune_using_aabbs_rod_rod, + _calculate_contact_forces_rod_rigid_body, + _calculate_contact_forces_rod_rod, + _calculate_contact_forces_self_rod, +) + + +def mock_rod_init(self): + + "Initializing Rod" + "Details of initialization are given in test_contact_specific_functions.py" + + self.n_elems = 2 + self.position_collection = np.array([[1, 2, 3], [0, 0, 0], [0, 0, 0]]) + self.radius = np.array([1, 1]) + self.lengths = np.array([1, 1]) + self.tangents = np.array([[1.0, 1.0], [0.0, 0.0], [0.0, 0.0]]) + self.internal_forces = np.array([[0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]]) + self.external_forces = np.array([[0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]]) + self.velocity_collection = np.array( + [[0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]] + ) + + +def mock_rigid_body_init(self): + + "Initializing Rigid Body" + "Details of initialization are given in test_contact_specific_functions.py" + + self.n_elems = 1 + self.position_collection = np.array([[0], [0], [0]]) + self.director_collection = np.array( + [[[1.0], [0.0], [0.0]], [[0.0], [1.0], [0.0]], [[0.0], [0.0], [1.0]]] + ) + self.radius = np.array([1.0]) + self.length = np.array([2.0]) + self.external_forces = np.array([[0.0], [0.0], [0.0]]) + self.external_torques = np.array([[0.0], [0.0], [0.0]]) + self.velocity_collection = np.array([[0.0], [0.0], [0.0]]) + + +MockRod = type("MockRod", (RodBase,), {"__init__": mock_rod_init}) + +MockRigidBody = type( + "MockRigidBody", (RigidBodyBase,), {"__init__": mock_rigid_body_init} +) + + +def test_prune_using_aabbs_rod_rigid_body_error_message(): + rod = MockRod() + cylinder = MockRigidBody() + error_message = ( + "This function is removed in v0.3.2. Please use\n" + "elastica.contact_utils._prune_using_aabbs_rod_cylinder()\n" + "instead for checking rod cylinder intersection." + ) + with pytest.raises(NotImplementedError) as error_info: + _prune_using_aabbs_rod_rigid_body( + rod.position_collection, + rod.radius, + rod.lengths, + cylinder.position_collection, + cylinder.director_collection, + cylinder.radius, + cylinder.length, + ) + assert error_info.value.args[0] == error_message + + +def test_prune_using_aabbs_rod_rod_error_message(): + rod_one = MockRod() + rod_two = MockRod() + error_message = ( + "This function is removed in v0.3.2. Please use\n" + "elastica.contact_utils._prune_using_aabbs_rod_rod()\n" + "instead for checking rod rod intersection." + ) + with pytest.raises(NotImplementedError) as error_info: + _prune_using_aabbs_rod_rod( + rod_one.position_collection, + rod_one.radius, + rod_one.lengths, + rod_two.position_collection, + rod_two.radius, + rod_two.lengths, + ) + assert error_info.value.args[0] == error_message + + +def test_calculate_contact_forces_rod_rigid_body_error_message(): + + "initializing rod parameters" + rod = MockRod() + rod_element_position = 0.5 * ( + rod.position_collection[..., 1:] + rod.position_collection[..., :-1] + ) + + "initializing cylinder parameters" + cylinder = MockRigidBody() + x_cyl = ( + cylinder.position_collection[..., 0] + - 0.5 * cylinder.length * cylinder.director_collection[2, :, 0] + ) + + "initializing constants" + """ + Setting contact_k = 1 and other parameters to 0, + so the net forces becomes a function of contact forces only. + """ + k = 1.0 + nu = 0 + velocity_damping_coefficient = 0 + friction_coefficient = 0 + + error_message = ( + "This function is removed in v0.3.2. Please use\n" + "elastica._contact_functions._calculate_contact_forces_rod_cylinder()\n" + "instead for calculating rod cylinder contact forces." + ) + + "Function call" + with pytest.raises(NotImplementedError) as error_info: + _calculate_contact_forces_rod_rigid_body( + rod_element_position, + rod.lengths * rod.tangents, + cylinder.position_collection[..., 0], + x_cyl, + cylinder.length * cylinder.director_collection[2, :, 0], + rod.radius + cylinder.radius, + rod.lengths + cylinder.length, + rod.internal_forces, + rod.external_forces, + cylinder.external_forces, + cylinder.external_torques, + cylinder.director_collection[:, :, 0], + rod.velocity_collection, + cylinder.velocity_collection, + k, + nu, + velocity_damping_coefficient, + friction_coefficient, + ) + assert error_info.value.args[0] == error_message + + +def test_calculate_contact_forces_rod_rod_error_message(): + + rod_one = MockRod() + rod_two = MockRod() + """Placing rod two such that its first element just touches the last element of rod one.""" + rod_two.position_collection = np.array([[4, 5, 6], [0, 0, 0], [0, 0, 0]]) + + "initializing constants" + """ + Setting contact_k = 1 and nu to 0, + so the net forces becomes a function of contact forces only. + """ + k = 1.0 + nu = 0.0 + + error_message = ( + "This function is removed in v0.3.2. Please use\n" + "elastica._contact_functions._calculate_contact_forces_rod_rod()\n" + "instead for calculating rod rod contact forces." + ) + + "Function call" + with pytest.raises(NotImplementedError) as error_info: + _calculate_contact_forces_rod_rod( + rod_one.position_collection[..., :-1], + rod_one.radius, + rod_one.lengths, + rod_one.tangents, + rod_one.velocity_collection, + rod_one.internal_forces, + rod_one.external_forces, + rod_two.position_collection[..., :-1], + rod_two.radius, + rod_two.lengths, + rod_two.tangents, + rod_two.velocity_collection, + rod_two.internal_forces, + rod_two.external_forces, + k, + nu, + ) + assert error_info.value.args[0] == error_message + + +def test_calculate_contact_forces_self_rod_error_message(): + "Function to test the calculate contact forces self rod function" + + "Testing function with handcrafted/calculated values" + + rod = MockRod() + + "initializing constants" + k = 1.0 + nu = 1.0 + + error_message = ( + "This function is removed in v0.3.2. Please use\n" + "elastica._contact_functions._calculate_contact_forces_self_rod()\n" + "instead for calculating rod self-contact forces." + ) + + "Function call" + with pytest.raises(NotImplementedError) as error_info: + _calculate_contact_forces_self_rod( + rod.position_collection[..., :-1], + rod.radius, + rod.lengths, + rod.tangents, + rod.velocity_collection, + rod.external_forces, + k, + nu, + ) + assert error_info.value.args[0] == error_message + + +class TestExternalContact: + def test_external_contact_rod_rigid_body_with_collision_with_k_without_nu_and_friction( + self, + ): + + "Testing External Contact wrapper with Collision with analytical verified values" + + mock_rod = MockRod() + mock_rigid_body = MockRigidBody() + ext_contact = ExternalContact(k=1.0, nu=0.0) + ext_contact.apply_forces(mock_rod, 0, mock_rigid_body, 1) + + """Details and reasoning about the values are given in 'test_contact_specific_functions.py/test_claculate_contact_forces_rod_rigid_body()'""" + assert_allclose( + mock_rod.external_forces, + np.array([[0.166666, 0.333333, 0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]]), + atol=1e-6, + ) + + assert_allclose( + mock_rigid_body.external_forces, np.array([[-0.5], [0.0], [0.0]]), atol=1e-6 + ) + + assert_allclose( + mock_rigid_body.external_torques, np.array([[0.0], [0.0], [0.0]]), atol=1e-6 + ) + + def test_external_contact_rod_rigid_body_with_collision_with_nu_without_k_and_friction( + self, + ): + + "Testing External Contact wrapper with Collision with analytical verified values" + + mock_rod = MockRod() + "Moving rod towards the cylinder with a velocity of -1 in x-axis" + mock_rod.velocity_collection = np.array([[-1, 0, 0], [-1, 0, 0], [-1, 0, 0]]) + mock_rigid_body = MockRigidBody() + "Moving cylinder towards the rod with a velocity of 1 in x-axis" + mock_rigid_body.velocity_collection = np.array([[1], [0], [0]]) + ext_contact = ExternalContact(k=0.0, nu=1.0) + ext_contact.apply_forces(mock_rod, 0, mock_rigid_body, 1) + + """Details and reasoning about the values are given in 'test_contact_specific_functions.py/test_claculate_contact_forces_rod_rigid_body()'""" + assert_allclose( + mock_rod.external_forces, + np.array([[0.5, 1, 0], [0, 0, 0], [0, 0, 0]]), + atol=1e-6, + ) + + assert_allclose( + mock_rigid_body.external_forces, np.array([[-1.5], [0], [0]]), atol=1e-6 + ) + + assert_allclose( + mock_rigid_body.external_torques, np.array([[0.0], [0.0], [0.0]]), atol=1e-6 + ) + + def test_external_contact_rod_rigid_body_with_collision_with_k_and_nu_without_friction( + self, + ): + + "Testing External Contact wrapper with Collision with analytical verified values" + + mock_rod = MockRod() + "Moving rod towards the cylinder with a velocity of -1 in x-axis" + mock_rod.velocity_collection = np.array([[-1, 0, 0], [-1, 0, 0], [-1, 0, 0]]) + mock_rigid_body = MockRigidBody() + "Moving cylinder towards the rod with a velocity of 1 in x-axis" + mock_rigid_body.velocity_collection = np.array([[1], [0], [0]]) + ext_contact = ExternalContact(k=1.0, nu=1.0) + ext_contact.apply_forces(mock_rod, 0, mock_rigid_body, 1) + + """Details and reasoning about the values are given in 'test_contact_specific_functions.py/test_claculate_contact_forces_rod_rigid_body()'""" + assert_allclose( + mock_rod.external_forces, + np.array([[0.666666, 1.333333, 0], [0, 0, 0], [0, 0, 0]]), + atol=1e-6, + ) + + assert_allclose( + mock_rigid_body.external_forces, np.array([[-2], [0], [0]]), atol=1e-6 + ) + + assert_allclose( + mock_rigid_body.external_torques, np.array([[0.0], [0.0], [0.0]]), atol=1e-6 + ) + + def test_external_contact_rod_rigid_body_with_collision_with_k_and_nu_and_friction( + self, + ): + + "Testing External Contact wrapper with Collision with analytical verified values" + + mock_rod = MockRod() + "Moving rod towards the cylinder with a velocity of -1 in x-axis" + mock_rod.velocity_collection = np.array([[-1, 0, 0], [-1, 0, 0], [-1, 0, 0]]) + mock_rigid_body = MockRigidBody() + "Moving cylinder towards the rod with a velocity of 1 in x-axis" + mock_rigid_body.velocity_collection = np.array([[1], [0], [0]]) + ext_contact = ExternalContact( + k=1.0, nu=1.0, velocity_damping_coefficient=0.1, friction_coefficient=0.1 + ) + ext_contact.apply_forces(mock_rod, 0, mock_rigid_body, 1) + + """Details and reasoning about the values are given in 'test_contact_specific_functions.py/test_claculate_contact_forces_rod_rigid_body()'""" + assert_allclose( + mock_rod.external_forces, + np.array( + [ + [0.666666, 1.333333, 0], + [0.033333, 0.066666, 0], + [0.033333, 0.066666, 0], + ] + ), + atol=1e-6, + ) + + assert_allclose( + mock_rigid_body.external_forces, np.array([[-2], [-0.1], [-0.1]]), atol=1e-6 + ) + + assert_allclose( + mock_rigid_body.external_torques, np.array([[0.0], [0.0], [0.0]]), atol=1e-6 + ) + + def test_external_contact_rod_rigid_body_without_collision(self): + + "Testing External Contact wrapper without Collision with analytical verified values" + + mock_rod = MockRod() + mock_rigid_body = MockRigidBody() + ext_contact = ExternalContact(k=1.0, nu=0.5) + + """Setting rigid body position such that there is no collision""" + mock_rigid_body.position_collection = np.array([[400], [500], [600]]) + mock_rod_external_forces_before_execution = mock_rod.external_forces.copy() + mock_rigid_body_external_forces_before_execution = ( + mock_rigid_body.external_forces.copy() + ) + mock_rigid_body_external_torques_before_execution = ( + mock_rigid_body.external_torques.copy() + ) + ext_contact.apply_forces(mock_rod, 0, mock_rigid_body, 1) + + assert_allclose( + mock_rod.external_forces, mock_rod_external_forces_before_execution + ) + assert_allclose( + mock_rigid_body.external_forces, + mock_rigid_body_external_forces_before_execution, + ) + assert_allclose( + mock_rigid_body.external_torques, + mock_rigid_body_external_torques_before_execution, + ) + + def test_external_contact_with_two_rods_with_collision_with_k_without_nu(self): + + "Testing External Contact wrapper with two rods with analytical verified values" + "Test values have been copied from 'test_contact_specific_functions.py/test_calculate_contact_forces_rod_rod()'" + + mock_rod_one = MockRod() + mock_rod_two = MockRod() + mock_rod_two.position_collection = np.array([[4, 5, 6], [0, 0, 0], [0, 0, 0]]) + ext_contact = ExternalContact(k=1.0, nu=0.0) + ext_contact.apply_forces(mock_rod_one, 0, mock_rod_two, 0) + + assert_allclose( + mock_rod_one.external_forces, + np.array([[0, -0.666666, -0.333333], [0, 0, 0], [0, 0, 0]]), + atol=1e-6, + ) + assert_allclose( + mock_rod_two.external_forces, + np.array([[0.333333, 0.666666, 0], [0, 0, 0], [0, 0, 0]]), + atol=1e-6, + ) + + def test_external_contact_with_two_rods_with_collision_without_k_with_nu(self): + + "Testing External Contact wrapper with two rods with analytical verified values" + "Test values have been copied from 'test_contact_specific_functions.py/test_calculate_contact_forces_rod_rod()'" + + mock_rod_one = MockRod() + mock_rod_two = MockRod() + + """Moving the rods towards each other with a velocity of 1 along the x-axis.""" + mock_rod_one.velocity_collection = np.array([[1, 0, 0], [1, 0, 0], [1, 0, 0]]) + mock_rod_two.velocity_collection = np.array( + [[-1, 0, 0], [-1, 0, 0], [-1, 0, 0]] + ) + mock_rod_two.position_collection = np.array([[4, 5, 6], [0, 0, 0], [0, 0, 0]]) + ext_contact = ExternalContact(k=0.0, nu=1.0) + ext_contact.apply_forces(mock_rod_one, 0, mock_rod_two, 0) + + assert_allclose( + mock_rod_one.external_forces, + np.array( + [[0, -0.333333, -0.166666], [0, 0, 0], [0, 0, 0]], + ), + atol=1e-6, + ) + assert_allclose( + mock_rod_two.external_forces, + np.array([[0.166666, 0.333333, 0], [0, 0, 0], [0, 0, 0]]), + atol=1e-6, + ) + + def test_external_contact_with_two_rods_with_collision_with_k_and_nu(self): + + "Testing External Contact wrapper with two rods with analytical verified values" + "Test values have been copied from 'test_contact_specific_functions.py/test_calculate_contact_forces_rod_rod()'" + + mock_rod_one = MockRod() + mock_rod_two = MockRod() + + """Moving the rods towards each other with a velocity of 1 along the x-axis.""" + mock_rod_one.velocity_collection = np.array([[1, 0, 0], [1, 0, 0], [1, 0, 0]]) + mock_rod_two.velocity_collection = np.array( + [[-1, 0, 0], [-1, 0, 0], [-1, 0, 0]] + ) + mock_rod_two.position_collection = np.array([[4, 5, 6], [0, 0, 0], [0, 0, 0]]) + ext_contact = ExternalContact(k=1.0, nu=1.0) + ext_contact.apply_forces(mock_rod_one, 0, mock_rod_two, 0) + + assert_allclose( + mock_rod_one.external_forces, + np.array( + [[0, -1, -0.5], [0, 0, 0], [0, 0, 0]], + ), + atol=1e-6, + ) + assert_allclose( + mock_rod_two.external_forces, + np.array([[0.5, 1, 0], [0, 0, 0], [0, 0, 0]]), + atol=1e-6, + ) + + def test_external_contact_with_two_rods_without_collision(self): + + "Testing External Contact wrapper with two rods with analytical verified values" + + mock_rod_one = MockRod() + mock_rod_two = MockRod() + + "Setting rod two position such that there is no collision" + mock_rod_two.position_collection = np.array( + [[100, 101, 102], [0, 0, 0], [0, 0, 0]] + ) + ext_contact = ExternalContact(k=1.0, nu=1.0) + mock_rod_one_external_forces_before_execution = ( + mock_rod_one.external_forces.copy() + ) + mock_rod_two_external_forces_before_execution = ( + mock_rod_two.external_forces.copy() + ) + ext_contact.apply_forces(mock_rod_one, 0, mock_rod_two, 0) + + assert_allclose( + mock_rod_one.external_forces, mock_rod_one_external_forces_before_execution + ) + assert_allclose( + mock_rod_two.external_forces, mock_rod_two_external_forces_before_execution + ) + + +class TestSelfContact: + def test_self_contact_with_rod_self_collision(self): + + "Testing Self Contact wrapper rod self collision with analytical verified values" + + mock_rod = MockRod() + + "Test values have been copied from 'test_contact_specific_functions.py/test_calculate_contact_forces_self_rod()'" + mock_rod.n_elems = 3 + mock_rod.position_collection = np.array( + [[1, 4, 4, 1], [0, 0, 1, 1], [0, 0, 0, 0]] + ) + mock_rod.radius = np.array([1, 1, 1]) + mock_rod.lengths = np.array([3, 1, 3]) + mock_rod.tangents = np.array( + [[1.0, 0.0, -1.0], [0.0, 1.0, 0.0], [0.0, 0.0, 0.0]] + ) + mock_rod.velocity_collection = np.array( + [[0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0]] + ) + mock_rod.internal_forces = np.array( + [[0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0]] + ) + mock_rod.external_forces = np.array( + [[0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0]] + ) + sel_contact = SelfContact(k=1.0, nu=0.0) + sel_contact.apply_forces(mock_rod, 0, mock_rod, 0) + + assert_allclose( + mock_rod.external_forces, + np.array( + [[0, 0, 0, 0], [-0.333333, -0.666666, 0.666666, 0.333333], [0, 0, 0, 0]] + ), + atol=1e-6, + ) + + def test_self_contact_with_rod_no_self_collision(self): + + "Testing Self Contact wrapper rod no self collision with analytical verified values" + + mock_rod = MockRod() + + "the initially set rod does not have self collision" + mock_rod_external_forces_before_execution = mock_rod.external_forces.copy() + sel_contact = SelfContact(k=1.0, nu=1.0) + sel_contact.apply_forces(mock_rod, 0, mock_rod, 0) + + assert_allclose( + mock_rod.external_forces, mock_rod_external_forces_before_execution + )