Constraints.h

#ifndef PBAT_SIM_CONTACT_CONSTRAINTS_H
#define PBAT_SIM_CONTACT_CONSTRAINTS_H
 
#include "pbat/HostDevice.h"
#include "pbat/common/ConstexprFor.h"
#include "pbat/math/linalg/mini/BinaryOperations.h"
#include "pbat/math/linalg/mini/Matrix.h"
 
namespace pbat::sim::contact {
template <
    math::linalg::mini::CMatrix TMatrixP,
    math::linalg::mini::CMatrix TMatrixNTB,
    math::linalg::mini::CMatrix TMatrixX1,
    math::linalg::mini::CMatrix TMatrixX2,
    math::linalg::mini::CMatrix TMatrixX3,
    math::linalg::mini::CMatrix TMatrixBary,
    math::linalg::mini::CMatrix TMatrixC>
PBAT_HOST_DEVICE void TriangleEnvironmentConstraint(
    TMatrixP const& P,
    TMatrixNTB const& NTB,
    TMatrixX1 const& X1,
    TMatrixX2 const& X2,
    TMatrixX3 const& X3,
    TMatrixBary const& bary,
    TMatrixC& C)
{
    static_assert(TMatrixP::kRows == 3 and TMatrixP::kCols == 1, "P must be 3x1");
    static_assert(TMatrixNTB::kRows == 3 and TMatrixNTB::kCols == 3, "NTB must be 3x3");
    static_assert(TMatrixX1::kRows == 3 and TMatrixX1::kCols == 1, "X1 must be 3x1");
    static_assert(TMatrixX2::kRows == 3 and TMatrixX2::kCols == 1, "X2 must be 3x1");
    static_assert(TMatrixX3::kRows == 3 and TMatrixX3::kCols == 1, "X3 must be 3x1");
    static_assert(TMatrixBary::kRows == 3 and TMatrixBary::kCols == 1, "bary must be 3x1");
    static_assert(TMatrixC::kRows == 3 and TMatrixC::kCols == 1, "C must be 3x1");
    using ScalarType = typename TMatrixP::ScalarType;
    using Vec3       = math::linalg::mini::SVector<ScalarType, 3>;
    // C(x) = [N T B]^T ([x_i x_j x_k] bary - P)
    Vec3 X = bary(0) * X1 + bary(1) * X2 + bary(2) * X3;
    C      = NTB.Transpose() * (X - P);
}

template <
    math::linalg::mini::CMatrix TMatrixP,
    math::linalg::mini::CMatrix TMatrixNTB,
    math::linalg::mini::CMatrix TMatrixX1,
    math::linalg::mini::CMatrix TMatrixX2,
    math::linalg::mini::CMatrix TMatrixX3,
    math::linalg::mini::CMatrix TMatrixBary,
    math::linalg::mini::CMatrix TMatrixC,
    math::linalg::mini::CMatrix TMatrixGradCT>
PBAT_HOST_DEVICE void TriangleEnvironmentConstraintAndGradient(
    TMatrixP const& P,
    TMatrixNTB const& NTB,
    TMatrixX1 const& X1,
    TMatrixX2 const& X2,
    TMatrixX3 const& X3,
    TMatrixBary const& bary,
    TMatrixC& C,
    TMatrixGradCT& gradCT)
{
    static_assert(TMatrixGradCT::kRows == 9 and TMatrixGradCT::kCols == 3, "gradCT must be 9x3");
    using math::linalg::mini::SMatrix;
    TriangleEnvironmentConstraint(P, NTB, X1, X2, X3, bary, C);
    // \nabla_{x_i,x_j,x_k} C_c = \beta \otimes NTB[:,c]
    common::ForRange<0, 3>(
        [&]<auto c>() {
            common::ForRange<0, 3>(
                [&]<auto i>() {
                    gradCT.Col(c).template Slice<3, 1>(i * 3, 0) = bary(i) * NTB.Col(c);
                });
        });
}
} // namespace pbat::sim::contact
 
#endif // PBAT_SIM_CONTACT_CONSTRAINTS_H