<commit_before>// =============================================================================
// PROJECT CHRONO - http://projectchrono.org
//
// Copyright (c) 2016 projectchrono.org
// All right reserved.
//
// Use of this source code is governed by a BSD-style license that can be found
// in the LICENSE file at the top level of the distribution and at
// http://projectchrono.org/license-chrono.txt.
//
// =============================================================================
// Authors: <NAME>, <NAME>
// =============================================================================
// Implementation of methods specific to the parallel DEM solver.
//
// These functions implement the basic time update for a multibody system using
// a penalty-based approach for including frictional contact. It is assumed that
// geometric contact information has been already computed and is available.
// The current algorithm is based on a semi-implicit Euler scheme and projection
// on the velocity manifold of the bilateral constraints.
// =============================================================================

#include "chrono/physics/ChSystemDEM.h"
#include "chrono_parallel/solver/ChIterativeSolverParallel.h"
#include <thrust/sort.h>
using namespace chrono;

// -----------------------------------------------------------------------------
// Main worker function for calculating contact forces. Calculates the contact
// force and torque for the contact pair identified by 'index' and stores them
// in the 'extended' output arrays. The calculated force and torque vectors are
// therefore duplicated in the output arrays, once for each body involved in the
// contact (with opposite signs for the two bodies).
// -----------------------------------------------------------------------------
void function_CalcContactForces(
    int index,                                            // index of this contact pair
    ChSystemDEM::ContactForceModel contact_model,         // contact force model
    ChSystemDEM::AdhesionForceModel adhesion_model,       // contact force model
    ChSystemDEM::TangentialDisplacementModel displ_mode,  // type of tangential displacement history
    bool use_mat_props,                                   // flag specifying how coefficients are obtained
    real char_vel,                                        // characteristic velocity (Hooke)
    real min_slip_vel,                                    // threshold tangential velocity
    real dT,                                              // integration time step
    real* mass,                                           // body masses
    real3* pos,                                           // body positions
    quaternion* rot,                                      // body orientations
    real* vel,                                            // body linear and angular velocities
    real2* elastic_moduli,                                // Young's modulus (per body)
    real* cr,                                             // coefficient of restitution (per body)
    real4* dem_coeffs,                                    // stiffness and damping coefficients (per body)
    real* mu,                                             // coefficient of friction (per body)
    real* adhesion,                                       // constant force (per body)
    real* adhesionMultDMT,                                // Adhesion force multiplier (per body), in DMT model.
    vec2* body_id,                                        // body IDs (per contact)
    vec2* shape_id,                                       // shape IDs (per contact)
    real3* pt1,                                           // point on shape 1 (per contact)
    real3* pt2,                                           // point on shape 2 (per contact)
    real3* normal,                                        // contact normal (per contact)
    real* depth,                                          // penetration depth (per contact)
    real* eff_radius,                                     // effective contact radius (per contact)
    vec3* shear_neigh,      // neighbor list of contacting bodies and shapes (max_shear per body)
    char* shear_touch,      // flag if contact in neighbor list is persistent (max_shear per body)
    real3* shear_disp,      // accumulated shear displacement for each neighbor (max_shear per body)
    int* ext_body_id,       // [output] body IDs (two per contact)
    real3* ext_body_force,  // [output] body force (two per contact)
    real3* ext_body_torque  // [output] body torque (two per contact)
    ) {
    // Identify the two bodies in contact.
    int body1 = body_id[index].x;
    int body2 = body_id[index].y;

    // If the two contact shapes are actually separated, set zero forces and torques.
    if (depth[index] >= 0) {
        ext_body_id[2 * index] = body1;
        ext_body_id[2 * index + 1] = body2;
        ext_body_force[2 * index] = real3(0);
        ext_body_force[2 * index + 1] = real3(0);
        ext_body_torque[2 * index] = real3(0);
        ext_body_torque[2 * index + 1] = real3(0);

        return;
    }

    // Kinematic information
    // ---------------------

    // Express contact point locations in local frames
    //   s' = At * s = At * (rP - r)
    real3 pt1_loc = TransformParentToLocal(pos[body1], rot[body1], pt1[index]);
    real3 pt2_loc = TransformParentToLocal(pos[body2], rot[body2], pt2[index]);

    // Calculate velocities of the contact points (in global frame)
    //   vP = v + omg x s = v + A * (omg' x s')
    real3 v_body1 = real3(vel[body1 * 6 + 0], vel[body1 * 6 + 1], vel[body1 * 6 + 2]);
    real3 v_body2 = real3(vel[body2 * 6 + 0], vel[body2 * 6 + 1], vel[body2 * 6 + 2]);

    real3 o_body1 = real3(vel[body1 * 6 + 3], vel[body1 * 6 + 4], vel[body1 * 6 + 5]);
    real3 o_body2 = real3(vel[body2 * 6 + 3], vel[body2 * 6 + 4], vel[body2 * 6 + 5]);

    real3 vel1 = v_body1 + Rotate(Cross(o_body1, pt1_loc), rot[body1]);
    real3 vel2 = v_body2 + Rotate(Cross(o_body2, pt2_loc), rot[body2]);

    // Calculate relative velocity (in global frame)
    // Note that relvel_n_mag is a signed quantity, while relvel_t_mag is an
    // actual magnitude (always positive).
    real3 relvel = vel2 - vel1;
    real relvel_n_mag = Dot(relvel, normal[index]);
    real3 relvel_n = relvel_n_mag * normal[index];
    real3 relvel_t = relvel - relvel_n;
    real relvel_t_mag = Length(relvel_t);

    // Calculate composite material properties
    // ---------------------------------------

    real m_eff = mass[body1] * mass[body2] / (mass[body1] + mass[body2]);

    real mu_eff = Min(mu[body1], mu[body2]);
    real adhesion_eff = Min(adhesion[body1], adhesion[body2]);
    real adhesionMultDMT_eff = Min(adhesionMultDMT[body1], adhesionMultDMT[body2]);

    real E_eff, G_eff, cr_eff;
    real user_kn, user_kt, user_gn, user_gt;

    if (use_mat_props) {
        real Y1 = elastic_moduli[body1].x;
        real Y2 = elastic_moduli[body2].x;
        real nu1 = elastic_moduli[body1].y;
        real nu2 = elastic_moduli[body2].y;
        real inv_E = (1 - nu1 * nu1) / Y1 + (1 - nu2 * nu2) / Y2;
        real inv_G = 2 * (2 - nu1) * (1 + nu1) / Y1 + 2 * (2 - nu2) * (1 + nu2) / Y2;

        E_eff = 1 / inv_E;
        G_eff = 1 / inv_G;
        cr_eff = (cr[body1] + cr[body2]) / 2;
    } else {
        user_kn = (dem_coeffs[body1].w + dem_coeffs[body2].w) / 2;
        user_kt = (dem_coeffs[body1].x + dem_coeffs[body2].x) / 2;
        user_gn = (dem_coeffs[body1].y + dem_coeffs[body2].y) / 2;
        user_gt = (dem_coeffs[body1].z + dem_coeffs[body2].z) / 2;
    }

    // Contact force
    // -------------<commit_msg>fixed error in DEM code, real4 uses standard ordering (x,y,z,w) vs a quaternion (w,x,y,z). They used to be the same data type but now they are separate<commit_after>// =============================================================================
// PROJECT CHRONO - http://projectchrono.org
//
// Copyright (c) 2016 projectchrono.org
// All right reserved.
//
// Use of this source code is governed by a BSD-style license that can be found
// in the LICENSE file at the top level of the distribution and at
// http://projectchrono.org/license-chrono.txt.
//
// =============================================================================
// Authors: <NAME>, <NAME>
// =============================================================================
// Implementation of methods specific to the parallel DEM solver.
//
// These functions implement the basic time update for a multibody system using
// a penalty-based approach for including frictional contact. It is assumed that
// geometric contact information has been already computed and is available.
// The current algorithm is based on a semi-implicit Euler scheme and projection
// on the velocity manifold of the bilateral constraints.
// =============================================================================

#include "chrono/physics/ChSystemDEM.h"
#include "chrono_parallel/solver/ChIterativeSolverParallel.h"
#include <thrust/sort.h>
using namespace chrono;

// -----------------------------------------------------------------------------
// Main worker function for calculating contact forces. Calculates the contact
// force and torque for the contact pair identified by 'index' and stores them
// in the 'extended' output arrays. The calculated force and torque vectors are
// therefore duplicated in the output arrays, once for each body involved in the
// contact (with opposite signs for the two bodies).
// -----------------------------------------------------------------------------
void function_CalcContactForces(
    int index,                                            // index of this contact pair
    ChSystemDEM::ContactForceModel contact_model,         // contact force model
    ChSystemDEM::AdhesionForceModel adhesion_model,       // contact force model
    ChSystemDEM::TangentialDisplacementModel displ_mode,  // type of tangential displacement history
    bool use_mat_props,                                   // flag specifying how coefficients are obtained
    real char_vel,                                        // characteristic velocity (Hooke)
    real min_slip_vel,                                    // threshold tangential velocity
    real dT,                                              // integration time step
    real* mass,                                           // body masses
    real3* pos,                                           // body positions
    quaternion* rot,                                      // body orientations
    real* vel,                                            // body linear and angular velocities
    real2* elastic_moduli,                                // Young's modulus (per body)
    real* cr,                                             // coefficient of restitution (per body)
    real4* dem_coeffs,                                    // stiffness and damping coefficients (per body)
    real* mu,                                             // coefficient of friction (per body)
    real* adhesion,                                       // constant force (per body)
    real* adhesionMultDMT,                                // Adhesion force multiplier (per body), in DMT model.
    vec2* body_id,                                        // body IDs (per contact)
    vec2* shape_id,                                       // shape IDs (per contact)
    real3* pt1,                                           // point on shape 1 (per contact)
    real3* pt2,                                           // point on shape 2 (per contact)
    real3* normal,                                        // contact normal (per contact)
    real* depth,                                          // penetration depth (per contact)
    real* eff_radius,                                     // effective contact radius (per contact)
    vec3* shear_neigh,      // neighbor list of contacting bodies and shapes (max_shear per body)
    char* shear_touch,      // flag if contact in neighbor list is persistent (max_shear per body)
    real3* shear_disp,      // accumulated shear displacement for each neighbor (max_shear per body)
    int* ext_body_id,       // [output] body IDs (two per contact)
    real3* ext_body_force,  // [output] body force (two per contact)
    real3* ext_body_torque  // [output] body torque (two per contact)
    ) {
    // Identify the two bodies in contact.
    int body1 = body_id[index].x;
    int body2 = body_id[index].y;

    // If the two contact shapes are actually separated, set zero forces and torques.
    if (depth[index] >= 0) {
        ext_body_id[2 * index] = body1;
        ext_body_id[2 * index + 1] = body2;
        ext_body_force[2 * index] = real3(0);
        ext_body_force[2 * index + 1] = real3(0);
        ext_body_torque[2 * index] = real3(0);
        ext_body_torque[2 * index + 1] = real3(0);

        return;
    }

    // Kinematic information
    // ---------------------

    // Express contact point locations in local frames
    //   s' = At * s = At * (rP - r)
    real3 pt1_loc = TransformParentToLocal(pos[body1], rot[body1], pt1[index]);
    real3 pt2_loc = TransformParentToLocal(pos[body2], rot[body2], pt2[index]);

    // Calculate velocities of the contact points (in global frame)
    //   vP = v + omg x s = v + A * (omg' x s')
    real3 v_body1 = real3(vel[body1 * 6 + 0], vel[body1 * 6 + 1], vel[body1 * 6 + 2]);
    real3 v_body2 = real3(vel[body2 * 6 + 0], vel[body2 * 6 + 1], vel[body2 * 6 + 2]);

    real3 o_body1 = real3(vel[body1 * 6 + 3], vel[body1 * 6 + 4], vel[body1 * 6 + 5]);
    real3 o_body2 = real3(vel[body2 * 6 + 3], vel[body2 * 6 + 4], vel[body2 * 6 + 5]);

    real3 vel1 = v_body1 + Rotate(Cross(o_body1, pt1_loc), rot[body1]);
    real3 vel2 = v_body2 + Rotate(Cross(o_body2, pt2_loc), rot[body2]);

    // Calculate relative velocity (in global frame)
    // Note that relvel_n_mag is a signed quantity, while relvel_t_mag is an
    // actual magnitude (always positive).
    real3 relvel = vel2 - vel1;
    real relvel_n_mag = Dot(relvel, normal[index]);
    real3 relvel_n = relvel_n_mag * normal[index];
    real3 relvel_t = relvel - relvel_n;
    real relvel_t_mag = Length(relvel_t);

    // Calculate composite material properties
    // ---------------------------------------

    real m_eff = mass[body1] * mass[body2] / (mass[body1] + mass[body2]);

    real mu_eff = Min(mu[body1], mu[body2]);
    real adhesion_eff = Min(adhesion[body1], adhesion[body2]);
    real adhesionMultDMT_eff = Min(adhesionMultDMT[body1], adhesionMultDMT[body2]);

    real E_eff, G_eff, cr_eff;
    real user_kn, user_kt, user_gn, user_gt;

    if (use_mat_props) {
        real Y1 = elastic_moduli[body1].x;
        real Y2 = elastic_moduli[body2].x;
        real nu1 = elastic_moduli[body1].y;
        real nu2 = elastic_moduli[body2].y;
        real inv_E = (1 - nu1 * nu1) / Y1 + (1 - nu2 * nu2) / Y2;
        real inv_G = 2 * (2 - nu1) * (1 + nu1) / Y1 + 2 * (2 - nu2) * (1 + nu2) / Y2;

        E_eff = 1 / inv_E;
        G_eff = 1 / inv_G;
        cr_eff = (cr[body1] + cr[body2]) / 2;
    } else {
        user_kn = (dem_coeffs[body1].x + dem_coeffs[body2].x) / 2;
        user_kt = (dem_coeffs[body1].y + dem_coeffs[body2].y) / 2;
        user_gn = (dem_coeffs[body1].z + dem_coeffs[body2].z) / 2;
        user_gt = (dem_coeffs[body1].w + dem_coeffs[body2].w) / 2;
    }

    // Contact force
    // -------------
{'fraction_non_alphanumeric': 0.16229423868312756, 'fraction_numerical': 0.022183641975308643, 'mean_word_length': 1.8642725598526704, 'pattern_counts': {'":': 0, '<': 9, '<?xml version=': 0, '>': 11, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 78, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '529753', 'n_tokens_mistral': 4498, 'n_tokens_neox': 4207, 'n_words': 1380}