<commit_before>// =============================================================================
// PROJECT CHRONO - http://projectchrono.org
//
// Copyright (c) 2014 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>
// =============================================================================

#include "chrono/physics/ChLoadsBody.h"

namespace chrono {

// -----------------------------------------------------------------------------
// ChLoadBodyForce
// -----------------------------------------------------------------------------

ChLoadBodyForce::ChLoadBodyForce(std::shared_ptr<ChBody> body,
                                 const ChVector<>& force,
                                 bool local_force,
                                 const ChVector<>& point,
                                 bool local_point)
    : ChLoadCustom(body),
      m_force(force),
      m_point(point),
      m_local_force(local_force),
      m_local_point(local_point),
      m_scale(1) {
    m_modulation = chrono_types::make_shared<ChFunction_Const>(1.0);
}

void ChLoadBodyForce::ComputeQ(ChState* state_x, ChStateDelta* state_w) {
    auto mbody = std::dynamic_pointer_cast<ChBody>(this->loadable);
    double detJ;  // not used

    ChVector<> abs_force;
    if (m_local_force)
        abs_force = mbody->TransformDirectionLocalToParent(m_force);
    else
        abs_force = m_force;

    abs_force *= m_scale;

    ChVector<> abs_point;
    if (m_local_point)
        abs_point = mbody->TransformPointLocalToParent(m_point);
    else
        abs_point = m_point;

    // ChBody assumes F={force_abs, torque_abs}
    ChVectorDynamic<> mF(loadable->Get_field_ncoords());
    mF(0) = abs_force.x();
    mF(1) = abs_force.y();
    mF(2) = abs_force.z();
    mF(3) = 0;
    mF(4) = 0;
    mF(5) = 0;

    // Compute Q = N(u,v,w)'*F
    mbody->ComputeNF(abs_point.x(), abs_point.y(), abs_point.z(), load_Q, detJ, mF, state_x, state_w);
}

void ChLoadBodyForce::Update(double time) {
    m_modulation->Update(time);
    m_scale = m_modulation->Get_y(time);
    ChLoadCustom::Update(time);
}

void ChLoadBodyForce::SetForce(const ChVector<>& force, bool is_local) {
    m_force = force;
    m_local_force = is_local;
}

void ChLoadBodyForce::SetApplicationPoint(const ChVector<>& point, const bool is_local) {
    m_point = point;
    m_local_point = is_local;
}

ChVector<> ChLoadBodyForce::GetForce() const {
    return m_force * m_scale;
}

// -----------------------------------------------------------------------------
// ChLoadBodyTorque
// -----------------------------------------------------------------------------

ChLoadBodyTorque::ChLoadBodyTorque(std::shared_ptr<ChBody> body, const ChVector<>& torque, bool local_torque)
    : ChLoadCustom(body), m_torque(torque), m_local_torque(local_torque), m_scale(1) {
    m_modulation = chrono_types::make_shared<ChFunction_Const>(1.0);
}

void ChLoadBodyTorque::ComputeQ(ChState* state_x, ChStateDelta* state_w) {
    auto mbody = std::dynamic_pointer_cast<ChBody>(this->loadable);
    double detJ;  // not used

    ChVector<> abs_torque;
    if (m_local_torque)
        abs_torque = mbody->TransformDirectionLocalToParent(m_torque);
    else
        abs_torque = m_torque;

    abs_torque *= m_scale;

    // ChBody assumes F={force_abs, torque_abs}
    ChVectorDynamic<> mF(loadable->Get_field_ncoords());
    mF(1) = 0;
    mF(2) = 0;
    mF(3) = 0;
    mF(3) = abs_torque.x();
    mF(4) = abs_torque.y();
    mF(5) = abs_torque.z();

    // Compute Q = N(u,v,w)'*F
    mbody->ComputeNF(0, 0, 0, load_Q, detJ, mF, state_x, state_w);
}

void ChLoadBodyTorque::Update(double time) {
    m_modulation->Update(time);
    m_scale = m_modulation->Get_y(time);
    ChLoadCustom::Update(time);
}

void ChLoadBodyTorque::SetTorque(const ChVector<>& torque, bool is_local) {
    m_torque = torque;
    m_local_torque = is_local;
}

ChVector<> ChLoadBodyTorque::GetTorque() const {
    return m_torque * m_scale;
}

// -----------------------------------------------------------------------------
// ChLoadBodyBody
// -----------------------------------------------------------------------------

ChLoadBodyBody::ChLoadBodyBody(std::shared_ptr<ChBody> mbodyA,
                               std::shared_ptr<ChBody> mbodyB,
                               const ChFrame<>& abs_application)
    : ChLoadCustomMultiple(mbodyA, mbodyB) {
    mbodyA->ChFrame::TransformParentToLocal(abs_application, loc_application_A);
    mbodyB->ChFrame::TransformParentToLocal(abs_application, loc_application_B);
}

void ChLoadBodyBody::ComputeQ(ChState* state_x, ChStateDelta* state_w) {
    auto mbodyA = std::dynamic_pointer_cast<ChBody>(this->loadables[0]);
    auto mbodyB = std::dynamic_pointer_cast<ChBody>(this->loadables[1]);

    ChFrameMoving<> bodycoordA, bodycoordB;
    if (state_x) {
        // the numerical jacobian algo might change state_x
        bodycoordA.SetCoord(state_x->segment(0, 7));
        bodycoordB.SetCoord(state_x->segment(7, 7));
    } else {
        bodycoordA.SetCoord(mbodyA->coord);
        bodycoordB.SetCoord(mbodyB->coord);
    }

    if (state_w) {
        // the numerical jacobian algo might change state_w
        bodycoordA.SetPos_dt(state_w->segment(0, 3));
        bodycoordA.SetWvel_loc(state_w->segment(3, 3));
        bodycoordB.SetPos_dt(state_w->segment(6, 3));
        bodycoordB.SetWvel_loc(state_w->segment(9, 3));
    } else {
        bodycoordA.SetCoord_dt(mbodyA->GetCoord_dt());
        bodycoordB.SetCoord_dt(mbodyB->GetCoord_dt());
    }

    frame_Aw = ChFrameMoving<>(loc_application_A) >> bodycoordA;
    frame_Bw = ChFrameMoving<>(loc_application_B) >> bodycoordB;
    ChFrameMoving<> rel_AB = frame_Aw >> frame_Bw.GetInverse();

    // OMPUTE THE FORCE

    ComputeBodyBodyForceTorque(rel_AB, locB_force, locB_torque);

    ChVector<> abs_force = frame_Bw.TransformDirectionLocalToParent(locB_force);
    ChVector<> abs_torque = frame_Bw.TransformDirectionLocalToParent(locB_torque);

    // Compute Q

    ChVector<> loc_ftorque = bodycoordA.GetRot().RotateBack(((frame_Aw.GetPos() - bodycoordA.GetPos()) % -abs_force));
    ChVector<> loc_torque = bodycoordA.GetRot().RotateBack(-abs_torque);
    load_Q.segment(0, 3) = -abs_force.eigen();
    load_Q.segment(3, 3) = (loc_ftorque + loc_torque).eigen();

    loc_ftorque = bodycoordB.GetRot().RotateBack(((frame_Bw.GetPos() - bodycoordB.GetPos()) % abs_force));
    loc_torque = bodycoordB.GetRot().RotateBack(abs_torque);<commit_msg>Do not apply force or torque from ChLoadBodyForce or ChLoadBodyTorque if body not active<commit_after>// =============================================================================
// PROJECT CHRONO - http://projectchrono.org
//
// Copyright (c) 2014 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>
// =============================================================================

#include "chrono/physics/ChLoadsBody.h"

namespace chrono {

// -----------------------------------------------------------------------------
// ChLoadBodyForce
// -----------------------------------------------------------------------------

ChLoadBodyForce::ChLoadBodyForce(std::shared_ptr<ChBody> body,
                                 const ChVector<>& force,
                                 bool local_force,
                                 const ChVector<>& point,
                                 bool local_point)
    : ChLoadCustom(body),
      m_force(force),
      m_point(point),
      m_local_force(local_force),
      m_local_point(local_point),
      m_scale(1) {
    m_modulation = chrono_types::make_shared<ChFunction_Const>(1.0);
}

void ChLoadBodyForce::ComputeQ(ChState* state_x, ChStateDelta* state_w) {
    auto mbody = std::dynamic_pointer_cast<ChBody>(this->loadable);
    if (!mbody->Variables().IsActive())
        return;

    ChVector<> abs_force;
    if (m_local_force)
        abs_force = mbody->TransformDirectionLocalToParent(m_force);
    else
        abs_force = m_force;

    abs_force *= m_scale;

    ChVector<> abs_point;
    if (m_local_point)
        abs_point = mbody->TransformPointLocalToParent(m_point);
    else
        abs_point = m_point;

    // ChBody assumes F={force_abs, torque_abs}
    ChVectorDynamic<> mF(loadable->Get_field_ncoords());
    mF(0) = abs_force.x();
    mF(1) = abs_force.y();
    mF(2) = abs_force.z();
    mF(3) = 0;
    mF(4) = 0;
    mF(5) = 0;

    // Compute Q = N(u,v,w)'*F
    double detJ;  // not used
    mbody->ComputeNF(abs_point.x(), abs_point.y(), abs_point.z(), load_Q, detJ, mF, state_x, state_w);
}

void ChLoadBodyForce::Update(double time) {
    if (!std::dynamic_pointer_cast<ChBody>(this->loadable)->Variables().IsActive())
        return;

    m_modulation->Update(time);
    m_scale = m_modulation->Get_y(time);
    ChLoadCustom::Update(time);
}

void ChLoadBodyForce::SetForce(const ChVector<>& force, bool is_local) {
    m_force = force;
    m_local_force = is_local;
}

void ChLoadBodyForce::SetApplicationPoint(const ChVector<>& point, const bool is_local) {
    m_point = point;
    m_local_point = is_local;
}

ChVector<> ChLoadBodyForce::GetForce() const {
    return m_force * m_scale;
}

// -----------------------------------------------------------------------------
// ChLoadBodyTorque
// -----------------------------------------------------------------------------

ChLoadBodyTorque::ChLoadBodyTorque(std::shared_ptr<ChBody> body, const ChVector<>& torque, bool local_torque)
    : ChLoadCustom(body), m_torque(torque), m_local_torque(local_torque), m_scale(1) {
    m_modulation = chrono_types::make_shared<ChFunction_Const>(1.0);
}

void ChLoadBodyTorque::ComputeQ(ChState* state_x, ChStateDelta* state_w) {
    auto mbody = std::dynamic_pointer_cast<ChBody>(this->loadable);
    if (!mbody->Variables().IsActive())
        return;

    ChVector<> abs_torque;
    if (m_local_torque)
        abs_torque = mbody->TransformDirectionLocalToParent(m_torque);
    else
        abs_torque = m_torque;

    abs_torque *= m_scale;

    // ChBody assumes F={force_abs, torque_abs}
    ChVectorDynamic<> mF(loadable->Get_field_ncoords());
    mF(1) = 0;
    mF(2) = 0;
    mF(3) = 0;
    mF(3) = abs_torque.x();
    mF(4) = abs_torque.y();
    mF(5) = abs_torque.z();

    // Compute Q = N(u,v,w)'*F
    double detJ;  // not used
    mbody->ComputeNF(0, 0, 0, load_Q, detJ, mF, state_x, state_w);
}

void ChLoadBodyTorque::Update(double time) {
    if (!std::dynamic_pointer_cast<ChBody>(this->loadable)->Variables().IsActive())
        return;

    m_modulation->Update(time);
    m_scale = m_modulation->Get_y(time);
    ChLoadCustom::Update(time);
}

void ChLoadBodyTorque::SetTorque(const ChVector<>& torque, bool is_local) {
    m_torque = torque;
    m_local_torque = is_local;
}

ChVector<> ChLoadBodyTorque::GetTorque() const {
    return m_torque * m_scale;
}

// -----------------------------------------------------------------------------
// ChLoadBodyBody
// -----------------------------------------------------------------------------

ChLoadBodyBody::ChLoadBodyBody(std::shared_ptr<ChBody> mbodyA,
                               std::shared_ptr<ChBody> mbodyB,
                               const ChFrame<>& abs_application)
    : ChLoadCustomMultiple(mbodyA, mbodyB) {
    mbodyA->ChFrame::TransformParentToLocal(abs_application, loc_application_A);
    mbodyB->ChFrame::TransformParentToLocal(abs_application, loc_application_B);
}

void ChLoadBodyBody::ComputeQ(ChState* state_x, ChStateDelta* state_w) {
    auto mbodyA = std::dynamic_pointer_cast<ChBody>(this->loadables[0]);
    auto mbodyB = std::dynamic_pointer_cast<ChBody>(this->loadables[1]);

    ChFrameMoving<> bodycoordA, bodycoordB;
    if (state_x) {
        // the numerical jacobian algo might change state_x
        bodycoordA.SetCoord(state_x->segment(0, 7));
        bodycoordB.SetCoord(state_x->segment(7, 7));
    } else {
        bodycoordA.SetCoord(mbodyA->coord);
        bodycoordB.SetCoord(mbodyB->coord);
    }

    if (state_w) {
        // the numerical jacobian algo might change state_w
        bodycoordA.SetPos_dt(state_w->segment(0, 3));
        bodycoordA.SetWvel_loc(state_w->segment(3, 3));
        bodycoordB.SetPos_dt(state_w->segment(6, 3));
        bodycoordB.SetWvel_loc(state_w->segment(9, 3));
    } else {
        bodycoordA.SetCoord_dt(mbodyA->GetCoord_dt());
        bodycoordB.SetCoord_dt(mbodyB->GetCoord_dt());
    }

    frame_Aw = ChFrameMoving<>(loc_application_A) >> bodycoordA;
    frame_Bw = ChFrameMoving<>(loc_application_B) >> bodycoordB;
    ChFrameMoving<> rel_AB = frame_Aw >> frame_Bw.GetInverse();

    // COMPUTE THE FORCE

    ComputeBodyBodyForceTorque(rel_AB, locB_force, locB_torque);

    ChVector<> abs_force = frame_Bw.TransformDirectionLocalToParent(locB_force);
    ChVector<> abs_torque = frame_Bw.TransformDirectionLocalToParent(locB_torque);

    // Compute Q

    ChVector<> loc_ftorque = bodycoordA.GetRot().RotateBack(((frame_Aw.GetPos() - bodycoordA.GetPos()) % -abs_force));
    ChVector<> loc_torque = bodycoordA.GetRot().RotateBack(-abs_torque);
    load_Q.segment(0, 3) = -abs_force.eigen();
    load_Q.segment(3, 3) = (loc_ftorque + loc_torque).eigen();

    loc_ftorque = bodycoordB.GetRot().RotateBack(((frame_Bw.GetPos() - bodycoordB.GetPos()) % abs_force));
    loc_torque = bodycoordB.GetRot().RotateBack(abs_torque);
{'fraction_non_alphanumeric': 0.21965566714490675, 'fraction_numerical': 0.00703012912482066, 'mean_word_length': 2.7176, 'pattern_counts': {'":': 0, '<': 73, '<?xml version=': 0, '>': 145, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 8, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '7372141', 'n_tokens_mistral': 4599, 'n_tokens_neox': 4478, 'n_words': 746}