def apply_normal_force_numba_rigid_body(
plane_origin,
plane_normal,
surface_tol,
k,
nu,
length,
position_collection,
velocity_collection,
external_forces,
):
# Compute plane response force
# total_forces = system.internal_forces + system.external_forces
total_forces = external_forces
force_component_along_normal_direction = _batch_product_i_ik_to_k(
plane_normal, 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 = position_collection
distance_from_plane = _batch_product_i_ik_to_k(
plane_normal, (element_position - plane_origin))
plane_penetration = np.minimum(distance_from_plane - length / 2, 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 = 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 rigid body is in contact with plane.
no_contact_point_idx = np.where(
(distance_from_plane - length / 2) > 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
external_forces += plane_response_force_total
return (_batch_norm(plane_response_force), no_contact_point_idx)
def test_batch_norm(blocksize):
input_first_vector_collection = np.random.randn(3, blocksize)
test_vector_collection = _batch_norm(input_first_vector_collection)
correct_vector_collection = np.sqrt(
np.einsum("ij,ij->j", input_first_vector_collection,
input_first_vector_collection))
assert_allclose(test_vector_collection, correct_vector_collection)
def _compute_geometry_from_state(position_collection, volume, lengths,
tangents, radius):
"""
Returns
-------
"""
# Compute eq (3.3) from 2018 RSOS paper
# Note : we can use the two-point difference kernel, but it needs unnecessary padding
# and hence will always be slower
position_diff = position_difference_kernel(position_collection)
# FIXME: Here 1E-14 is added to fix ghost lengths, which is 0, and causes division by zero error!
lengths[:] = _batch_norm(position_diff) + 1e-14
# _reset_scalar_ghost(lengths, ghost_elems_idx, 1.0)
for k in range(lengths.shape[0]):
tangents[0, k] = position_diff[0, k] / lengths[k]
tangents[1, k] = position_diff[1, k] / lengths[k]
tangents[2, k] = position_diff[2, k] / lengths[k]
# resize based on volume conservation
radius[k] = np.sqrt(volume[k] / lengths[k] / np.pi)
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))
return slip_function
def allocate(n_elements,
start,
direction,
normal,
base_length,
base_radius,
density,
nu,
youngs_modulus,
poisson_ratio,
alpha_c=4.0 / 3.0,
*args,
**kwargs):
# sanity checks here
assert n_elements > 1
assert base_length > Tolerance.atol()
assert np.sqrt(np.dot(normal, normal)) > Tolerance.atol()
assert np.sqrt(np.dot(direction, direction)) > Tolerance.atol()
# Set the position array
position = np.zeros((MaxDimension.value(), n_elements + 1))
# check if position is in kwargs, if it is use user defined position otherwise generate position
if kwargs.__contains__("position"):
position_temp = np.array(kwargs["position"])
# Check the shape of the input position
assert position_temp.shape == (MaxDimension.value(), n_elements + 1), (
"Given position shape is not correct, it should be " +
str(position.shape) + " but instead " + str(position_temp.shape))
# Check if the start position of the rod and first entry of position array are the same
assert_allclose(
position_temp[..., 0],
start,
atol=Tolerance.atol(),
err_msg=str("First entry of position" + " (" +
str(position_temp[..., 0]) + " ) "
" is different than start " + " (" + str(start) +
" ) "),
)
position = position_temp.copy()
else:
end = start + direction * base_length
for i in range(0, 3):
position[i, ...] = np.linspace(start[i], end[i], n_elements + 1)
# Compute rest lengths and tangents
position_diff = position[..., 1:] - position[..., :-1]
rest_lengths = _batch_norm(position_diff)
tangents = position_diff / rest_lengths
normal /= np.linalg.norm(normal)
# Set the directors matrix
directors = np.zeros(
(MaxDimension.value(), MaxDimension.value(), n_elements))
# check if directors is in kwargs, if it use user defined directors otherwise generate directors
if kwargs.__contains__("directors"):
directors_temp = np.array(kwargs["directors"])
# Check the shape of input directors
assert directors_temp.shape == (
MaxDimension.value(),
MaxDimension.value(),
n_elements,
), (" Given directors shape is not correct, it should be " +
str(directors.shape) + " but instead " + str(directors_temp.shape))
# Check if d1, d2, d3 are unit vectors
d1 = directors_temp[0, ...]
d2 = directors_temp[1, ...]
d3 = directors_temp[2, ...]
assert_allclose(
_batch_norm(d1),
np.ones((n_elements)),
atol=Tolerance.atol(),
err_msg=(
" d1 vector of input director matrix is not unit vector "),
)
assert_allclose(
_batch_norm(d2),
np.ones((n_elements)),
atol=Tolerance.atol(),
err_msg=(
" d2 vector of input director matrix is not unit vector "),
)
assert_allclose(
_batch_norm(d3),
np.ones((n_elements)),
atol=Tolerance.atol(),
err_msg=(
" d3 vector of input director matrix is not unit vector "),
)
# Check if d3xd1 = d2
assert_allclose(
_batch_cross(d3, d1),
d2,
atol=Tolerance.atol(),
err_msg=(" d3 x d1 != d2 of input director matrix"),
)
# Check if computed tangents from position is the same with d3
assert_allclose(
tangents,
d3,
atol=Tolerance.atol(),
err_msg=
" Tangent vector computed using node positions is different than d3 vector of input directors",
)
directors[:] = directors_temp[:]
else:
# Construct directors using tangents and normal
normal_collection = np.repeat(normal[:, np.newaxis],
n_elements,
axis=1)
# Check if rod normal and rod tangent are perpendicular to each other otherwise
# directors will be wrong!!
assert_allclose(
_batch_dot(normal_collection, tangents),
0,
atol=Tolerance.atol(),
err_msg=(
" Rod normal and tangent are not perpendicular to each other!"
),
)
directors[0, ...] = normal_collection
directors[1, ...] = _batch_cross(tangents, normal_collection)
directors[2, ...] = tangents
# Set radius array
radius = np.zeros((n_elements))
# Check if the user input radius is valid
radius_temp = np.array(base_radius)
assert radius_temp.ndim < 2, ("Input radius shape is not correct " +
str(radius_temp.shape) + " It should be " +
str(radius.shape) +
" or single floating number ")
radius[:] = radius_temp
# Check if the elements of radius are greater than tolerance
for k in range(n_elements):
assert radius[k] > Tolerance.atol(), (
" Radius has to be greater than 0" + " Check you radius input!")
# Set density array
density_array = np.zeros((n_elements))
# Check if the user input density is valid
density_temp = np.array(density)
assert density_temp.ndim < 2, ("Input density shape is not correct " +
str(density_temp.shape) + " It should be " +
str(density_array.shape) +
" or single floating number ")
density_array[:] = density_temp
# Check if the elements of density are greater than tolerance
for k in range(n_elements):
assert density_array[k] > Tolerance.atol(), (
" Density has to be greater than 0" + " Check you density input!")
# Second moment of inertia
A0 = np.pi * radius * radius
I0_1 = A0 * A0 / (4.0 * np.pi)
I0_2 = I0_1
I0_3 = 2.0 * I0_2
I0 = np.array([I0_1, I0_2, I0_3]).transpose()
# Mass second moment of inertia for disk cross-section
mass_second_moment_of_inertia = np.zeros(
(MaxDimension.value(), MaxDimension.value(), n_elements), np.float64)
mass_second_moment_of_inertia_temp = np.einsum("ij,i->ij", I0,
density * rest_lengths)
for i in range(n_elements):
np.fill_diagonal(
mass_second_moment_of_inertia[..., i],
mass_second_moment_of_inertia_temp[i, :],
)
# sanity check of mass second moment of inertia
for k in range(n_elements):
for i in range(0, MaxDimension.value()):
assert mass_second_moment_of_inertia[i, i, k] > Tolerance.atol()
# Inverse of second moment of inertia
inv_mass_second_moment_of_inertia = np.zeros(
(MaxDimension.value(), MaxDimension.value(), n_elements))
for i in range(n_elements):
# Check rank of mass moment of inertia matrix to see if it is invertible
assert (np.linalg.matrix_rank(
mass_second_moment_of_inertia[..., i]) == MaxDimension.value())
inv_mass_second_moment_of_inertia[..., i] = np.linalg.inv(
mass_second_moment_of_inertia[..., i])
# Shear/Stretch matrix
shear_modulus = youngs_modulus / (poisson_ratio + 1.0)
shear_matrix = np.zeros(
(MaxDimension.value(), MaxDimension.value(), n_elements), np.float64)
for i in range(n_elements):
np.fill_diagonal(
shear_matrix[..., i],
[
alpha_c * shear_modulus * A0[i],
alpha_c * shear_modulus * A0[i],
youngs_modulus * A0[i],
],
)
# Bend/Twist matrix
bend_matrix = np.zeros(
(MaxDimension.value(), MaxDimension.value(), n_elements), np.float64)
for i in range(n_elements):
np.fill_diagonal(
bend_matrix[..., i],
[
youngs_modulus * I0_1[i],
youngs_modulus * I0_2[i],
shear_modulus * I0_3[i],
],
)
for k in range(n_elements):
for i in range(0, MaxDimension.value()):
assert bend_matrix[i, i, k] > Tolerance.atol()
# Compute bend matrix in Voronoi Domain
bend_matrix = (bend_matrix[..., 1:] * rest_lengths[1:] +
bend_matrix[..., :-1] * rest_lengths[0:-1]) / (
rest_lengths[1:] + rest_lengths[:-1])
# Compute volume of elements
volume = np.pi * radius**2 * rest_lengths
# Compute mass of elements
mass = np.zeros(n_elements + 1)
mass[:-1] += 0.5 * density * volume
mass[1:] += 0.5 * density * volume
# Set dissipation constant or nu array
dissipation_constant_for_forces = np.zeros((n_elements))
# Check if the user input nu is valid
nu_temp = np.array(nu)
assert nu_temp.ndim < 2, (
"Input dissipation constant(nu) for forces shape is not correct " +
str(nu_temp.shape) + " It should be " +
str(dissipation_constant_for_forces.shape) +
" or single floating number ")
dissipation_constant_for_forces[:] = nu
# Check if the elements of dissipation constant greater than tolerance
for k in range(n_elements):
assert dissipation_constant_for_forces[k] >= 0.0, (
" Dissipation constant has to be equal or greater than 0 " +
" Check your dissipation constant(nu) input!")
dissipation_constant_for_torques = np.zeros((n_elements))
if kwargs.__contains__("nu_for_torques"):
temp_nu_for_torques = np.array(kwargs["nu_for_torques"])
assert temp_nu_for_torques.ndim < 2, (
"Input dissipation constant(nu) for torques shape is not correct "
+ str(temp_nu_for_torques.shape) + " It should be " +
str(dissipation_constant_for_torques.shape) +
" or single floating number ")
dissipation_constant_for_torques[:] = temp_nu_for_torques
else:
dissipation_constant_for_torques[:] = dissipation_constant_for_forces
# Generate rest sigma and rest kappa, use user input if defined
# set rest strains and curvature to be zero at start
# if found in kwargs modify (say for curved rod)
rest_sigma = np.zeros((MaxDimension.value(), n_elements))
if kwargs.__contains__("rest_sigma"):
temp_rest_sigma = np.array(kwargs["rest_sigma"])
assert temp_rest_sigma.shape == rest_sigma.shape, (
"Input rest sigma shape is not correct " +
str(temp_rest_sigma.shape) + " It should be " +
str(rest_sigma.shape))
rest_sigma[:] = temp_rest_sigma
rest_kappa = np.zeros((MaxDimension.value(), n_elements - 1))
if kwargs.__contains__("rest_kappa"):
temp_rest_kappa = np.array(kwargs["rest_kappa"])
assert temp_rest_kappa.shape == rest_kappa.shape, (
"Input rest kappa shape is not correct " +
str(temp_rest_kappa.shape) + " It should be " +
str(rest_kappa.shape))
rest_kappa[:] = temp_rest_kappa
# Compute rest voronoi length
rest_voronoi_lengths = 0.5 * (rest_lengths[1:] + rest_lengths[:-1])
# Allocate arrays for Cosserat Rod equations
velocities = np.zeros((MaxDimension.value(), n_elements + 1))
omegas = np.zeros((MaxDimension.value(), n_elements))
accelerations = 0.0 * velocities
angular_accelerations = 0.0 * omegas
_vector_states = np.hstack(
(position, velocities, omegas, accelerations, angular_accelerations))
_matrix_states = directors.copy()
internal_forces = 0.0 * accelerations
internal_torques = 0.0 * angular_accelerations
external_forces = 0.0 * accelerations
external_torques = 0.0 * angular_accelerations
lengths = np.zeros((n_elements))
tangents = np.zeros((3, n_elements))
dilatation = np.zeros((n_elements))
voronoi_dilatation = np.zeros((n_elements - 1))
dilatation_rate = np.zeros((n_elements))
sigma = np.zeros((3, n_elements))
kappa = np.zeros((3, n_elements - 1))
internal_stress = np.zeros((3, n_elements))
internal_couple = np.zeros((3, n_elements - 1))
damping_forces = np.zeros((3, n_elements + 1))
damping_torques = np.zeros((3, n_elements))
return (
n_elements,
_vector_states,
_matrix_states,
radius,
mass_second_moment_of_inertia,
inv_mass_second_moment_of_inertia,
shear_matrix,
bend_matrix,
density,
volume,
mass,
dissipation_constant_for_forces,
dissipation_constant_for_torques,
internal_forces,
internal_torques,
external_forces,
external_torques,
lengths,
rest_lengths,
tangents,
dilatation,
dilatation_rate,
voronoi_dilatation,
rest_voronoi_lengths,
sigma,
kappa,
rest_sigma,
rest_kappa,
internal_stress,
internal_couple,
damping_forces,
damping_torques,
)
def 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,
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 = apply_normal_force_numba(
plane_origin,
plane_normal,
surface_tol,
k,
nu,
radius,
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_pos_or_vel(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 = nodes_to_elements(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),
)
def apply_normal_force_numba(
plane_origin,
plane_normal,
surface_tol,
k,
nu,
radius,
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 = nodes_to_elements(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_pos_or_vel(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_pos_or_vel(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)