<commit_before>// =============================================================================
// PROJECT CHRONO - http://projectchrono.org
//
// Copyright (c) 2016 projectchrono.org
// All rights 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>
// =============================================================================
//
// Handling of rigid contact and calculation of corrections and Jacobians
//
// =============================================================================

#include <algorithm>
#include <limits>

#include "chrono_parallel/ChConfigParallel.h"
#include "chrono_parallel/constraints/ChConstraintRigidRigid.h"
#include "chrono_parallel/constraints/ChConstraintUtils.h"

#include <thrust/iterator/constant_iterator.h>

using namespace chrono;

// -----------------------------------------------------------------------------

ChConstraintRigidRigid::ChConstraintRigidRigid()
    : data_manager(nullptr), offset(3), inv_h(0), inv_hpa(0), inv_hhpa(0) {}

void ChConstraintRigidRigid::func_Project_normal(int index, const vec2* ids, const real* cohesion, real* gamma) {
    real gamma_x = gamma[index * 1 + 0];
    vec2 body_id = ids[index];
    real coh = cohesion[index];

    gamma_x += coh;
    gamma_x = gamma_x < 0 ? 0 : gamma_x - coh;
    gamma[index * 1 + 0] = gamma_x;
    if (data_manager->settings.solver.solver_mode == SolverMode::SLIDING) {
        gamma[data_manager->num_rigid_contacts + index * 2 + 0] = 0;
        gamma[data_manager->num_rigid_contacts + index * 2 + 1] = 0;
    }
    if (data_manager->settings.solver.solver_mode == SolverMode::SPINNING) {
        gamma[3 * data_manager->num_rigid_contacts + index * 3 + 0] = 0;
        gamma[3 * data_manager->num_rigid_contacts + index * 3 + 1] = 0;
        gamma[3 * data_manager->num_rigid_contacts + index * 3 + 2] = 0;
    }
}

void ChConstraintRigidRigid::func_Project_sliding(int index,
                                                  const vec2* ids,
                                                  const real3* fric,
                                                  const real* cohesion,
                                                  real* gam) {
    real3 gamma;
    gamma.x = gam[index * 1 + 0];
    gamma.y = gam[data_manager->num_rigid_contacts + index * 2 + 0];
    gamma.z = gam[data_manager->num_rigid_contacts + index * 2 + 1];

    //  if (data_manager->settings.solver.solver_mode == SPINNING) {
    //    gam[3 * data_manager->num_contacts + index * 3 + 0] = 0;
    //    gam[3 * data_manager->num_contacts + index * 3 + 1] = 0;
    //    gam[3 * data_manager->num_contacts + index * 3 + 2] = 0;
    //  }

    real coh = cohesion[index];
    gamma.x += coh;

    real mu = fric[index].x;
    if (mu == 0) {
        gamma.x = gamma.x < 0 ? 0 : gamma.x - coh;
        gamma.y = gamma.z = 0;

        gam[index * 1 + 0] = gamma.x;
        gam[data_manager->num_rigid_contacts + index * 2 + 0] = gamma.y;
        gam[data_manager->num_rigid_contacts + index * 2 + 1] = gamma.z;

        return;
    }

    Cone_generalized_rigid(gamma.x, gamma.y, gamma.z, mu);

    gam[index * 1 + 0] = gamma.x - coh;
    gam[data_manager->num_rigid_contacts + index * 2 + 0] = gamma.y;
    gam[data_manager->num_rigid_contacts + index * 2 + 1] = gamma.z;
}
void ChConstraintRigidRigid::func_Project_spinning(int index, const vec2* ids, const real3* fric, real* gam) {
    real rollingfriction = fric[index].y;
    real spinningfriction = fric[index].z;

    real f_n = gam[index * 1 + 0];
    real t_n = gam[3 * data_manager->num_rigid_contacts + index * 3 + 0];
    real t_u = gam[3 * data_manager->num_rigid_contacts + index * 3 + 1];
    real t_v = gam[3 * data_manager->num_rigid_contacts + index * 3 + 2];

	real t_tang = sqrt(t_v * t_v + t_u * t_u);
	real t_sptang = fabs(t_n);  // = sqrt(t_n*t_n);

	if (spinningfriction) {
		if (t_sptang < spinningfriction * f_n) {
			// inside upper cone? keep untouched!
		}<commit_msg>Fix bug in projection of gamma in the frictionless case<commit_after>// =============================================================================
// PROJECT CHRONO - http://projectchrono.org
//
// Copyright (c) 2016 projectchrono.org
// All rights 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>
// =============================================================================
//
// Handling of rigid contact and calculation of corrections and Jacobians
//
// =============================================================================

#include <algorithm>
#include <limits>

#include "chrono_parallel/ChConfigParallel.h"
#include "chrono_parallel/constraints/ChConstraintRigidRigid.h"
#include "chrono_parallel/constraints/ChConstraintUtils.h"

#include <thrust/iterator/constant_iterator.h>

using namespace chrono;

// -----------------------------------------------------------------------------

ChConstraintRigidRigid::ChConstraintRigidRigid()
    : data_manager(nullptr), offset(3), inv_h(0), inv_hpa(0), inv_hhpa(0) {}

void ChConstraintRigidRigid::func_Project_normal(int index, const vec2* ids, const real* cohesion, real* gamma) {
    real gamma_x = gamma[index * 1 + 0];
    real coh = cohesion[index];

    gamma[index * 1 + 0] = (gamma_x < -coh) ? -coh : gamma_x;
    switch (data_manager->settings.solver.solver_mode) {
        case SolverMode::SLIDING:
            gamma[data_manager->num_rigid_contacts + index * 2 + 0] = 0;
            gamma[data_manager->num_rigid_contacts + index * 2 + 1] = 0;
            break;
        case SolverMode::SPINNING:
            gamma[3 * data_manager->num_rigid_contacts + index * 3 + 0] = 0;
            gamma[3 * data_manager->num_rigid_contacts + index * 3 + 1] = 0;
            gamma[3 * data_manager->num_rigid_contacts + index * 3 + 2] = 0;
            break;
    }
}

void ChConstraintRigidRigid::func_Project_sliding(int index,
                                                  const vec2* ids,
                                                  const real3* fric,
                                                  const real* cohesion,
                                                  real* gam) {
    real3 gamma;
    gamma.x = gam[index * 1 + 0];
    gamma.y = gam[data_manager->num_rigid_contacts + index * 2 + 0];
    gamma.z = gam[data_manager->num_rigid_contacts + index * 2 + 1];

    real coh = cohesion[index];
    real mu = fric[index].x;

    if (mu == 0) {
        gam[index * 1 + 0] = (gamma.x < -coh) ? -coh : gamma.x;
        gam[data_manager->num_rigid_contacts + index * 2 + 0] = 0;
        gam[data_manager->num_rigid_contacts + index * 2 + 1] = 0;
        return;
    }

    gamma.x += coh;
    Cone_generalized_rigid(gamma.x, gamma.y, gamma.z, mu);
    gam[index * 1 + 0] = gamma.x - coh;
    gam[data_manager->num_rigid_contacts + index * 2 + 0] = gamma.y;
    gam[data_manager->num_rigid_contacts + index * 2 + 1] = gamma.z;
}

void ChConstraintRigidRigid::func_Project_spinning(int index, const vec2* ids, const real3* fric, real* gam) {
    real rollingfriction = fric[index].y;
    real spinningfriction = fric[index].z;

    real f_n = gam[index * 1 + 0];
    real t_n = gam[3 * data_manager->num_rigid_contacts + index * 3 + 0];
    real t_u = gam[3 * data_manager->num_rigid_contacts + index * 3 + 1];
    real t_v = gam[3 * data_manager->num_rigid_contacts + index * 3 + 2];

	real t_tang = sqrt(t_v * t_v + t_u * t_u);
	real t_sptang = fabs(t_n);  // = sqrt(t_n*t_n);

	if (spinningfriction) {
		if (t_sptang < spinningfriction * f_n) {
			// inside upper cone? keep untouched!
		}
{'fraction_non_alphanumeric': 0.2119632663191859, 'fraction_numerical': 0.018863241499131298, 'mean_word_length': 2.625281151596941, 'pattern_counts': {'":': 0, '<': 19, '<?xml version=': 0, '>': 48, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 16, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '3312783', 'n_tokens_mistral': 2794, 'n_tokens_neox': 2552, 'n_words': 598}