<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>, <NAME>
// =============================================================================

//// TODO
////    Serialization/deserialization of unique_ptr members is currently commented
////    out until support for unique_ptr is implemented in ChArchive.

#include "chrono/physics/ChLinkLock.h"

namespace chrono {

// Register into the object factory, to enable run-time dynamic creation and persistence
CH_FACTORY_REGISTER(ChLinkLock)

ChLinkLock::ChLinkLock()
    : type(LinkType::SPHERICAL),
      ndoc_d(0),
      ndoc_c(0),
      ndoc(0),
      d_restlength(0),
      mask(nullptr) {

    // Need to zero out the bottom-right 4x3 block
    Cq1_temp.setZero();
    Cq2_temp.setZero();

    // Default type: spherical link
    // Sets the mask, all the matrices, and number of DOC and DOF
    BuildLinkType(LinkType::SPHERICAL);
}

ChLinkLock::ChLinkLock(const ChLinkLock& other) : ChLinkMarkers(other) {
    DestroyLink();

    mask = other.mask->Clone();

    force_D.reset(other.force_D->Clone());
    force_R.reset(other.force_R->Clone());
    force_X.reset(other.force_X->Clone());
    force_Y.reset(other.force_Y->Clone());
    force_Z.reset(other.force_Z->Clone());
    force_Rx.reset(other.force_Rx->Clone());
    force_Ry.reset(other.force_Ry->Clone());
    force_Rz.reset(other.force_Rz->Clone());

    d_restlength = other.d_restlength;

    type = other.type;

    limit_X.reset(other.limit_X->Clone());
    limit_Y.reset(other.limit_Y->Clone());
    limit_Z.reset(other.limit_Z->Clone());
    limit_Rx.reset(other.limit_Rx->Clone());
    limit_Ry.reset(other.limit_Ry->Clone());
    limit_Rz.reset(other.limit_Rz->Clone());
    limit_Rp.reset(other.limit_Rp->Clone());
    limit_D.reset(other.limit_D->Clone());

    Ct_temp = other.Ct_temp;

    BuildLinkType(other.type);
}

ChLinkLock::~ChLinkLock() {
    DestroyLink();

    delete mask;
    mask = nullptr;
}

//// Note: ability to explicitly provide joint forces was removed.
//// If ever needed, these functions can be re-enabled. In that case,
//// a typical user call would be:
////         auto my_force = std::make_unique<ChLinkForce>();
////         my_joint->SetForce_X(std::move(my_force));

/*
void ChLinkLock::SetForce_D(std::unique_ptr<ChLinkForce>&& force) {
    force_D = std::move(force);
}
void ChLinkLock::SetForce_R(std::unique_ptr<ChLinkForce>&& force) {
    force_R = std::move(force);
}
void ChLinkLock::SetForce_X(std::unique_ptr<ChLinkForce>&& force) {
    force_X = std::move(force);
}
void ChLinkLock::SetForce_Y(std::unique_ptr<ChLinkForce>&& force) {
    force_Y = std::move(force);
}
void ChLinkLock::SetForce_Z(std::unique_ptr<ChLinkForce>&& force) {
    force_Z = std::move(force);
}
void ChLinkLock::SetForce_Rx(std::unique_ptr<ChLinkForce>&& force) {
    force_Rx = std::move(force);
}
void ChLinkLock::SetForce_Ry(std::unique_ptr<ChLinkForce>&& force) {
    force_Ry = std::move(force);
}
void ChLinkLock::SetForce_Rz(std::unique_ptr<ChLinkForce>&& force) {
    force_Rz = std::move(force);
}
*/

ChLinkForce& ChLinkLock::GetForce_D() {
    if (!force_D)
        force_D = std::make_unique<ChLinkForce>();
    return *force_D;
}
ChLinkForce& ChLinkLock::GetForce_R() {
    if (!force_R)
        force_R = std::make_unique<ChLinkForce>();
    return *force_R;
}
ChLinkForce& ChLinkLock::GetForce_X() {
    if (!force_X)
        force_X = std::make_unique<ChLinkForce>();
    return *force_X;
}
ChLinkForce& ChLinkLock::GetForce_Y() {
    if (!force_Y)
        force_Y = std::make_unique<ChLinkForce>();
    return *force_Y;
}
ChLinkForce& ChLinkLock::GetForce_Z() {
    if (!force_Z)
        force_Z = std::make_unique<ChLinkForce>();
    return *force_Z;
}
ChLinkForce& ChLinkLock::GetForce_Rx() {
    if (!force_Rx)
        force_Rx = std::make_unique<ChLinkForce>();
    return *force_Rx;
}
ChLinkForce& ChLinkLock::GetForce_Ry() {
    if (!force_Ry)
        force_Ry = std::make_unique<ChLinkForce>();
    return *force_Ry;
}
ChLinkForce& ChLinkLock::GetForce_Rz() {
    if (!force_Rz)
        force_Rz = std::make_unique<ChLinkForce>();
    return *force_Rz;
}

//// Note: ability to explicitly provide limits was removed.
//// If ever needed, these functions can be re-enabled. In that case,
//// a typical user call would be:
////         auto my_limit = std::make_unique<ChLinkLimit>();
////         my_joint->SetLimit_X(std::move(my_force));

/*
void ChLinkLock::SetLimit_X(std::unique_ptr<ChLinkLimit>&& limit) {
    limit_X = std::move(limit);
}
void ChLinkLock::SetLimit_Y(std::unique_ptr<ChLinkLimit>&& limit) {
    limit_Y = std::move(limit);
}
void ChLinkLock::SetLimit_Z(std::unique_ptr<ChLinkLimit>&& limit) {
    limit_Z = std::move(limit);
}
void ChLinkLock::SetLimit_Rx(std::unique_ptr<ChLinkLimit>&& limit) {
    limit_Rx = std::move(limit);
}
void ChLinkLock::SetLimit_Ry(std::unique_ptr<ChLinkLimit>&& limit) {
    limit_Ry = std::move(limit);
}
void ChLinkLock::SetLimit_Rz(std::unique_ptr<ChLinkLimit>&& limit) {
    limit_Rz = std::move(limit);
}
void ChLinkLock::SetLimit_Rp(std::unique_ptr<ChLinkLimit>&& limit) {
    limit_Rp = std::move(limit);
}
void ChLinkLock::SetLimit_D(std::unique_ptr<ChLinkLimit>&& limit) {
    limit_D = std::move(limit);
}
*/

ChLinkLimit& ChLinkLock::GetLimit_X() {
    if (!limit_X)
        limit_X = std::make_unique<ChLinkLimit>();
    return *limit_X;
}
ChLinkLimit& ChLinkLock::GetLimit_Y() {
    if (!limit_Y)
        limit_Y = std::make_unique<ChLinkLimit>();
    return *limit_Y;
}
ChLinkLimit& ChLinkLock::GetLimit_Z() {
    if (!limit_Z)
        limit_Z = std::make_unique<ChLinkLimit>();
    return *limit_Z;
}
ChLinkLimit& ChLinkLock::GetLimit_Rx() {
    if (!limit_Rx)
        limit_Rx = std::make_unique<ChLinkLimit>();
    return *limit_Rx;
}
ChLinkLimit& ChLinkLock::GetLimit_Ry() {
    if (!limit_Ry)
        limit_Ry = std::make_unique<ChLinkLimit>();
    return *limit_Ry;
}
ChLinkLimit& ChLinkLock::GetLimit_Rz() {
    if (!limit_Rz)
        limit_Rz = std::make_unique<ChLinkLimit>();
    return *limit_Rz;
}
ChLinkLimit& ChLinkLock::GetLimit_Rp() {
    if (!limit_Rp)
        limit_Rp = std::make_unique<ChLinkLimit>();
    return *limit_Rp;
}
ChLinkLimit& ChLinkLock::GetLimit_D() {
    if (!limit_D)
        limit_D = std::make_unique<ChLinkLimit>();
    return *limit_D;
}

void ChLinkLock::ChangeLinkMask(ChLinkMask* new_mask) {
    DestroyLink();
    BuildLink(new_mask);
}

void ChLinkLock::ChangedLinkMask() {
    DestroyLink();
    BuildLink();
}

void ChLinkLock::SetDisabled(bool mdis) {
    ChLinkMarkers::SetDisabled(mdis);

    if (mask->SetAllDisabled(mdis) > 0)
        ChangedLinkMask();
}

void ChLinkLock::SetBroken(bool mbro) {
    ChLinkMarkers::SetBroken(mbro);

    if (mask->SetAllBroken(mbro) > 0)
        ChangedLinkMask();
}

int ChLinkLock::RestoreRedundant() {
    int mchanges = mask->RestoreRedundant();
    if (mchanges)
        ChangedLinkMask();
    return mchanges;
}

void ChLinkLock::SetUpMarkers(ChMarker* mark1, ChMarker* mark2) {
    ChLinkMarkers::SetUpMarkers(mark1, mark2);

    // could the line below be:     assert(this->Body1 && this->Body2); ?
    if (this->Body1 && this->Body2) {
        ((ChLinkMaskLF*)this->mask)->SetTwoBodiesVariables(&Body1->Variables(), &Body2->Variables());
        // This is needed because only if all constraints in mask are now active, and C,Ct,etc.
        // matrices must be allocated accordingly, otherwise are null.
        DestroyLink();
        BuildLink();
    }
}

void ChLinkLock::BuildLink() {
    // set ndoc by counting non-dofs
    ndoc = mask->GetMaskDoc();
    ndoc_c = mask->GetMaskDoc_c();
    ndoc_d = mask->GetMaskDoc_d();

    // create matrices
    if (ndoc > 0) {
        C.resize(ndoc);
        C_dt.resize(ndoc);
        C_dtdt.resize(ndoc);
        react.resize(ndoc);
        Qc.resize(ndoc);
        Ct.resize(ndoc);
        Cq1.resize(ndoc, BODY_QDOF);
        Cq2.resize(ndoc, BODY_QDOF);
        Cqw1.resize(ndoc, BODY_DOF);
        Cqw2.resize(ndoc, BODY_DOF);

        // Zero out vectors of constraint violations
        C.setZero();
        C_dt.setZero();
        C_dtdt.setZero();
        react.setZero();

        // Need to zero out the first 3 entries in rows corrsponding to rotation constraints
        Cq1.setZero();
        Cq2.setZero();
    }
}
<commit_msg>Resize ChLinkLock member matrices even when there are zero constraints.<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>, <NAME>
// =============================================================================

//// TODO
////    Serialization/deserialization of unique_ptr members is currently commented
////    out until support for unique_ptr is implemented in ChArchive.

#include "chrono/physics/ChLinkLock.h"

namespace chrono {

// Register into the object factory, to enable run-time dynamic creation and persistence
CH_FACTORY_REGISTER(ChLinkLock)

ChLinkLock::ChLinkLock()
    : type(LinkType::SPHERICAL),
      ndoc_d(0),
      ndoc_c(0),
      ndoc(0),
      d_restlength(0),
      mask(nullptr) {

    // Need to zero out the bottom-right 4x3 block
    Cq1_temp.setZero();
    Cq2_temp.setZero();

    // Default type: spherical link
    // Sets the mask, all the matrices, and number of DOC and DOF
    BuildLinkType(LinkType::SPHERICAL);
}

ChLinkLock::ChLinkLock(const ChLinkLock& other) : ChLinkMarkers(other) {
    DestroyLink();

    mask = other.mask->Clone();

    force_D.reset(other.force_D->Clone());
    force_R.reset(other.force_R->Clone());
    force_X.reset(other.force_X->Clone());
    force_Y.reset(other.force_Y->Clone());
    force_Z.reset(other.force_Z->Clone());
    force_Rx.reset(other.force_Rx->Clone());
    force_Ry.reset(other.force_Ry->Clone());
    force_Rz.reset(other.force_Rz->Clone());

    d_restlength = other.d_restlength;

    type = other.type;

    limit_X.reset(other.limit_X->Clone());
    limit_Y.reset(other.limit_Y->Clone());
    limit_Z.reset(other.limit_Z->Clone());
    limit_Rx.reset(other.limit_Rx->Clone());
    limit_Ry.reset(other.limit_Ry->Clone());
    limit_Rz.reset(other.limit_Rz->Clone());
    limit_Rp.reset(other.limit_Rp->Clone());
    limit_D.reset(other.limit_D->Clone());

    Ct_temp = other.Ct_temp;

    BuildLinkType(other.type);
}

ChLinkLock::~ChLinkLock() {
    DestroyLink();

    delete mask;
    mask = nullptr;
}

//// Note: ability to explicitly provide joint forces was removed.
//// If ever needed, these functions can be re-enabled. In that case,
//// a typical user call would be:
////         auto my_force = std::make_unique<ChLinkForce>();
////         my_joint->SetForce_X(std::move(my_force));

/*
void ChLinkLock::SetForce_D(std::unique_ptr<ChLinkForce>&& force) {
    force_D = std::move(force);
}
void ChLinkLock::SetForce_R(std::unique_ptr<ChLinkForce>&& force) {
    force_R = std::move(force);
}
void ChLinkLock::SetForce_X(std::unique_ptr<ChLinkForce>&& force) {
    force_X = std::move(force);
}
void ChLinkLock::SetForce_Y(std::unique_ptr<ChLinkForce>&& force) {
    force_Y = std::move(force);
}
void ChLinkLock::SetForce_Z(std::unique_ptr<ChLinkForce>&& force) {
    force_Z = std::move(force);
}
void ChLinkLock::SetForce_Rx(std::unique_ptr<ChLinkForce>&& force) {
    force_Rx = std::move(force);
}
void ChLinkLock::SetForce_Ry(std::unique_ptr<ChLinkForce>&& force) {
    force_Ry = std::move(force);
}
void ChLinkLock::SetForce_Rz(std::unique_ptr<ChLinkForce>&& force) {
    force_Rz = std::move(force);
}
*/

ChLinkForce& ChLinkLock::GetForce_D() {
    if (!force_D)
        force_D = std::make_unique<ChLinkForce>();
    return *force_D;
}
ChLinkForce& ChLinkLock::GetForce_R() {
    if (!force_R)
        force_R = std::make_unique<ChLinkForce>();
    return *force_R;
}
ChLinkForce& ChLinkLock::GetForce_X() {
    if (!force_X)
        force_X = std::make_unique<ChLinkForce>();
    return *force_X;
}
ChLinkForce& ChLinkLock::GetForce_Y() {
    if (!force_Y)
        force_Y = std::make_unique<ChLinkForce>();
    return *force_Y;
}
ChLinkForce& ChLinkLock::GetForce_Z() {
    if (!force_Z)
        force_Z = std::make_unique<ChLinkForce>();
    return *force_Z;
}
ChLinkForce& ChLinkLock::GetForce_Rx() {
    if (!force_Rx)
        force_Rx = std::make_unique<ChLinkForce>();
    return *force_Rx;
}
ChLinkForce& ChLinkLock::GetForce_Ry() {
    if (!force_Ry)
        force_Ry = std::make_unique<ChLinkForce>();
    return *force_Ry;
}
ChLinkForce& ChLinkLock::GetForce_Rz() {
    if (!force_Rz)
        force_Rz = std::make_unique<ChLinkForce>();
    return *force_Rz;
}

//// Note: ability to explicitly provide limits was removed.
//// If ever needed, these functions can be re-enabled. In that case,
//// a typical user call would be:
////         auto my_limit = std::make_unique<ChLinkLimit>();
////         my_joint->SetLimit_X(std::move(my_force));

/*
void ChLinkLock::SetLimit_X(std::unique_ptr<ChLinkLimit>&& limit) {
    limit_X = std::move(limit);
}
void ChLinkLock::SetLimit_Y(std::unique_ptr<ChLinkLimit>&& limit) {
    limit_Y = std::move(limit);
}
void ChLinkLock::SetLimit_Z(std::unique_ptr<ChLinkLimit>&& limit) {
    limit_Z = std::move(limit);
}
void ChLinkLock::SetLimit_Rx(std::unique_ptr<ChLinkLimit>&& limit) {
    limit_Rx = std::move(limit);
}
void ChLinkLock::SetLimit_Ry(std::unique_ptr<ChLinkLimit>&& limit) {
    limit_Ry = std::move(limit);
}
void ChLinkLock::SetLimit_Rz(std::unique_ptr<ChLinkLimit>&& limit) {
    limit_Rz = std::move(limit);
}
void ChLinkLock::SetLimit_Rp(std::unique_ptr<ChLinkLimit>&& limit) {
    limit_Rp = std::move(limit);
}
void ChLinkLock::SetLimit_D(std::unique_ptr<ChLinkLimit>&& limit) {
    limit_D = std::move(limit);
}
*/

ChLinkLimit& ChLinkLock::GetLimit_X() {
    if (!limit_X)
        limit_X = std::make_unique<ChLinkLimit>();
    return *limit_X;
}
ChLinkLimit& ChLinkLock::GetLimit_Y() {
    if (!limit_Y)
        limit_Y = std::make_unique<ChLinkLimit>();
    return *limit_Y;
}
ChLinkLimit& ChLinkLock::GetLimit_Z() {
    if (!limit_Z)
        limit_Z = std::make_unique<ChLinkLimit>();
    return *limit_Z;
}
ChLinkLimit& ChLinkLock::GetLimit_Rx() {
    if (!limit_Rx)
        limit_Rx = std::make_unique<ChLinkLimit>();
    return *limit_Rx;
}
ChLinkLimit& ChLinkLock::GetLimit_Ry() {
    if (!limit_Ry)
        limit_Ry = std::make_unique<ChLinkLimit>();
    return *limit_Ry;
}
ChLinkLimit& ChLinkLock::GetLimit_Rz() {
    if (!limit_Rz)
        limit_Rz = std::make_unique<ChLinkLimit>();
    return *limit_Rz;
}
ChLinkLimit& ChLinkLock::GetLimit_Rp() {
    if (!limit_Rp)
        limit_Rp = std::make_unique<ChLinkLimit>();
    return *limit_Rp;
}
ChLinkLimit& ChLinkLock::GetLimit_D() {
    if (!limit_D)
        limit_D = std::make_unique<ChLinkLimit>();
    return *limit_D;
}

void ChLinkLock::ChangeLinkMask(ChLinkMask* new_mask) {
    DestroyLink();
    BuildLink(new_mask);
}

void ChLinkLock::ChangedLinkMask() {
    DestroyLink();
    BuildLink();
}

void ChLinkLock::SetDisabled(bool mdis) {
    ChLinkMarkers::SetDisabled(mdis);

    if (mask->SetAllDisabled(mdis) > 0)
        ChangedLinkMask();
}

void ChLinkLock::SetBroken(bool mbro) {
    ChLinkMarkers::SetBroken(mbro);

    if (mask->SetAllBroken(mbro) > 0)
        ChangedLinkMask();
}

int ChLinkLock::RestoreRedundant() {
    int mchanges = mask->RestoreRedundant();
    if (mchanges)
        ChangedLinkMask();
    return mchanges;
}

void ChLinkLock::SetUpMarkers(ChMarker* mark1, ChMarker* mark2) {
    ChLinkMarkers::SetUpMarkers(mark1, mark2);

    // could the line below be:     assert(this->Body1 && this->Body2); ?
    if (this->Body1 && this->Body2) {
        ((ChLinkMaskLF*)this->mask)->SetTwoBodiesVariables(&Body1->Variables(), &Body2->Variables());
        // This is needed because only if all constraints in mask are now active, and C,Ct,etc.
        // matrices must be allocated accordingly, otherwise are null.
        DestroyLink();
        BuildLink();
    }
}

void ChLinkLock::BuildLink() {
    // set ndoc by counting non-dofs
    ndoc = mask->GetMaskDoc();
    ndoc_c = mask->GetMaskDoc_c();
    ndoc_d = mask->GetMaskDoc_d();

    // create matrices
    C.resize(ndoc);
    C_dt.resize(ndoc);
    C_dtdt.resize(ndoc);
    react.resize(ndoc);
    Qc.resize(ndoc);
    Ct.resize(ndoc);
    Cq1.resize(ndoc, BODY_QDOF);
    Cq2.resize(ndoc, BODY_QDOF);
    Cqw1.resize(ndoc, BODY_DOF);
    Cqw2.resize(ndoc, BODY_DOF);

    // Zero out vectors of constraint violations
    C.setZero();
    C_dt.setZero();
    C_dtdt.setZero();
    react.setZero();

    // Need to zero out the first 3 entries in rows corrsponding to rotation constraints
    Cq1.setZero();
    Cq2.setZero();
}

{'fraction_non_alphanumeric': 0.17389815026261704, 'fraction_numerical': 0.0035967115779858413, 'mean_word_length': 3.9413258110014104, 'pattern_counts': {'":': 0, '<': 77, '<?xml version=': 0, '>': 148, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '7537309', 'n_tokens_mistral': 6162, 'n_tokens_neox': 6077, 'n_words': 1110}