ASDEICR.h

/* ****************************************************************** **
**    OpenSees - Open System for Earthquake Engineering Simulation    **
**          Pacific Earthquake Engineering Research Center            **
**                                                                    **
**                                                                    **
** (C) Copyright 1999, The Regents of the University of California    **
** All Rights Reserved.                                               **
**                                                                    **
** Commercial use of this program without express permission of the   **
** University of California, Berkeley, is strictly prohibited.  See   **
** file 'COPYRIGHT'  in main directory for information on usage and   **
** redistribution,  and for a DISCLAIMER OF ALL WARRANTIES.           **
**                                                                    **
** Developed by:                                                      **
**   Frank McKenna (fmckenna@ce.berkeley.edu)                         **
**   Gregory L. Fenves (fenves@ce.berkeley.edu)                       **
**   Filip C. Filippou (filippou@ce.berkeley.edu)                     **
**                                                                    **
** ****************************************************************** */

// $Revision: 1.10 $
// $Date: 2020/05/18 22:51:21 $

// Original implementation: Massimo Petracca (ASDEA)
//
// Implementation of the EICR (Element Independent CoRotational) formulation
// 
// References:
//
// Felippa, Carlos A. 
// "A systematic approach to the element-independent corotational 
// dynamics of finite elements". 
// Technical Report CU-CAS-00-03, Center for Aerospace Structures, 2000.
//
// Felippa, Carlos A., and Bjorn Haugen. 
// "A unified formulation of small-strain corotational finite elements: I. Theory." 
// Computer Methods in Applied Mechanics and Engineering 194.21-24 (2005): 2285-2335.
//

#ifndef ASDEICR_h
#define ASDEICR_h

#include "ASDMath.h"
#include <vector>

namespace XC {

class EICR
  {

  public:

    typedef std::size_t size_t;

    typedef ASDVector3<double> Vector3Type;

    typedef std::vector<Vector3Type> NodeContainerType;

    typedef Vector VectorType;

    typedef Matrix MatrixType;

    typedef ASDQuaternion<double> QuaternionType;

public:

    template< class TVec, class TMat>
    inline static void Spin(const TVec& V, TMat& S)
    {
        S(0, 0) = 0.00;     S(0, 1) = -V(2);    S(0, 2) = V(1);
        S(1, 0) = V(2);     S(1, 1) = 0.00;     S(1, 2) = -V(0);
        S(2, 0) = -V(1);    S(2, 1) = V(0);     S(2, 2) = 0.00;
    }

    template< class TVec, class TMat>
    inline static void Spin_AtRow(const TVec& V, TMat& S, size_t row_index)
    {
        size_t i0 = row_index;
        size_t i1 = 1 + row_index;
        size_t i2 = 2 + row_index;
        double v0 = V(i0);
        double v1 = V(i1);
        double v2 = V(i2);
        S(i0, 0) = 0.00;    S(i0, 1) = -v2;     S(i0, 2) = v1;
        S(i1, 0) = v2;      S(i1, 1) = 0.00;    S(i1, 2) = -v0;
        S(i2, 0) = -v1;     S(i2, 1) = v0;      S(i2, 2) = 0.00;
    }

    template< class TVec, class TMat>
    inline static void Spin_AtRow(const TVec& V, TMat& S, size_t vector_index, size_t matrix_row_index)
    {
        size_t i0 = matrix_row_index;
        size_t i1 = 1 + matrix_row_index;
        size_t i2 = 2 + matrix_row_index;
        double v0 = V(vector_index);
        double v1 = V(vector_index + 1);
        double v2 = V(vector_index + 2);
        S(i0, 0) = 0.00;    S(i0, 1) = -v2;     S(i0, 2) = v1;
        S(i1, 0) = v2;      S(i1, 1) = 0.00;    S(i1, 2) = -v0;
        S(i2, 0) = -v1;     S(i2, 1) = v0;      S(i2, 2) = 0.00;
    }

    template< class TVec, class TMat>
    inline static void Spin(const TVec& V, TMat& S, double mult)
    {
        S(0, 0) = 0.00;         S(0, 1) = -mult * V(2); S(0, 2) = mult * V(1);
        S(1, 0) = mult * V(2);  S(1, 1) = 0.00;         S(1, 2) = -mult * V(0);
        S(2, 0) = -mult * V(1); S(2, 1) = mult * V(0);  S(2, 2) = 0.00;
    }

    template< class TVec, class TMat>
    inline static void Spin_AtRow(const TVec& V, TMat& S, double mult, size_t row_index)
    {
        size_t i0 = row_index;
        size_t i1 = 1 + row_index;
        size_t i2 = 2 + row_index;
        double v0 = mult * V(i0);
        double v1 = mult * V(i1);
        double v2 = mult * V(i2);
        S(i0, 0) = 0.00;    S(i0, 1) = -v2;     S(i0, 2) = v1;
        S(i1, 0) = v2;      S(i1, 1) = 0.00;    S(i1, 2) = -v0;
        S(i2, 0) = -v1;     S(i2, 1) = v0;      S(i2, 2) = 0.00;
    }

    template< class TVec, class TMat>
    inline static void Spin_AtRow(const TVec& V, TMat& S, double mult, size_t vector_index, size_t matrix_row_index)
    {
        size_t i0 = matrix_row_index;
        size_t i1 = 1 + matrix_row_index;
        size_t i2 = 2 + matrix_row_index;
        double v0 = mult * V(vector_index);
        double v1 = mult * V(vector_index + 1);
        double v2 = mult * V(vector_index + 2);
        S(i0, 0) = 0.00;    S(i0, 1) = -v2;     S(i0, 2) = v1;
        S(i1, 0) = v2;      S(i1, 1) = 0.00;    S(i1, 2) = -v0;
        S(i2, 0) = -v1;     S(i2, 1) = v0;      S(i2, 2) = 0.00;
    }

    static void SetZero(size_t n, size_t m, MatrixType& I);


    inline static void SetIdentity(size_t n, MatrixType& I, double value = 1.0)
    {
        SetZero(n, n, I);
        for (size_t j = 0; j < n; ++j)
            I(j, j) = value;
    }

    inline static void GetBlock(const VectorType& A, size_t begin, size_t end, VectorType& B)
    {
        size_t n = end - begin;
        for (size_t i = 0; i < n; ++i)
            B(i) = A(i + begin);
    }

    inline static void GetBlock(const MatrixType& A, size_t begin, size_t end, MatrixType& B)
    {
        size_t n = end - begin;
        for (size_t i = 0; i < n; ++i)
            for (size_t j = 0; j < n; ++j)
                B(i, j) = A(i + begin, j + begin);
    }

    inline static void SetBlock(MatrixType& A, size_t begin, size_t end, const MatrixType& B)
    {
        size_t n = end - begin;
        for (size_t i = 0; i < n; ++i)
            for (size_t j = 0; j < n; ++j)
                A(i + begin, j + begin) = B(i, j);
    }

    static void OuterProd(const VectorType& A, const VectorType& B, MatrixType& C);

public:

    inline static void Compute_Pt(size_t num_nodes, MatrixType& P)
    {
        double a = double(num_nodes - 1) / double(num_nodes);
        double b = -1.0 / double(num_nodes);

        size_t num_dofs = num_nodes * 6;

        SetIdentity(num_dofs, P);

        for (size_t i = 0; i < num_nodes; i++)
        {
            size_t j = i * 6;

            // diagonal block
            P(j, j) = a;
            P(j + 1, j + 1) = a;
            P(j + 2, j + 2) = a;

            // out-of-diagonal block
            for (size_t k = i + 1; k < num_nodes; k++)
            {
                size_t w = k * 6;

                P(j, w) = b;
                P(j + 1, w + 1) = b;
                P(j + 2, w + 2) = b;

                P(w, j) = b;
                P(w + 1, j + 1) = b;
                P(w + 2, j + 2) = b;
            }
        }
    }

    inline static void Compute_S(const NodeContainerType& nodes, MatrixType& S)
    {
        size_t num_nodes = nodes.size();
        size_t num_dofs = num_nodes * 6;

        SetZero(num_dofs, 3, S);

        for (size_t i = 0; i < num_nodes; i++)
        {
            size_t j = i * 6;

            Spin_AtRow(nodes[i], S, -1.0, 0, j);

            S(j + 3, 0) = 1.0;
            S(j + 4, 1) = 1.0;
            S(j + 5, 2) = 1.0;
        }
    }

    inline static void Compute_H(const VectorType& displacements, MatrixType& H)
    {
        size_t num_dofs = displacements.Size();
        size_t num_nodes = num_dofs / 6;

        SetIdentity(num_dofs, H);

        static MatrixType Omega(3, 3);
        static MatrixType Omega2(3, 3);
        static MatrixType Hi(3, 3);
        static VectorType rv(3);

        for (size_t i = 0; i < num_nodes; i++)
        {
            size_t index = i * 6;

            GetBlock(displacements, index + 3, index + 6, rv);

            double angle = rv.Norm();

            if (angle >= 2.0 * M_PI)
                angle = std::fmod(angle, 2.0 * M_PI);

            double eta;
            if (angle < 0.05) {
                double angle2 = angle * angle;
                double angle4 = angle2 * angle2;
                double angle6 = angle4 * angle2;
                eta = 1.0 / 12.0 + 1.0 / 720.0 * angle2 + 1.0 / 30240.0 * angle4 + 1.0 / 1209600.0 * angle6;
            }
            else {
                eta = (1.0 - 0.5 * angle * std::tan(0.5 * M_PI - 0.5 * angle)) / (angle * angle);
            }

            Spin(rv, Omega);
            Omega2.addMatrixProduct(0.0, Omega, Omega, 1.0);

            // Hi = I - 0.5*Omega + eta*Omega*Omega
            SetIdentity(3, Hi);
            Hi.addMatrix(1.0, Omega, -0.5);
            Hi.addMatrix(1.0, Omega2, eta);

            SetBlock(H, index + 3, index + 6, Hi);
        }
    }

    inline static void Compute_L(const VectorType& displacements, const VectorType& forces, const MatrixType& H, MatrixType& L)
    {
        size_t num_dofs = displacements.Size();
        size_t num_nodes = num_dofs / 6;

        SetZero(num_dofs, num_dofs, L);

        static VectorType rotationVector(3);
        static VectorType momentVector(3);
        static MatrixType Omega(3, 3);
        static MatrixType Omega2(3, 3);
        static MatrixType Li(3, 3);
        static MatrixType LiTemp1(3, 3);
        static MatrixType MxR(3, 3);
        static MatrixType RxM(3, 3);
        static MatrixType Hi(3, 3);

        for (size_t i = 0; i < num_nodes; i++)
        {
            size_t index = i * 6;

            GetBlock(displacements, index + 3, index + 6, rotationVector);
            GetBlock(forces, index + 3, index + 6, momentVector);

            double angle = rotationVector.Norm();

            if (angle >= 2.0 * M_PI)
                angle = std::fmod(angle, 2.0 * M_PI);

            double angle2 = angle * angle;
            double angle4 = angle2 * angle2;
            double angle6 = angle4 * angle2;

            double eta;
            double mu;
            if (angle < 0.05) {
                eta = 1.0 / 12.0 + angle2 / 720.0 + angle4 / 30240.0 + angle6 / 1209600.0;
                mu = 1.0 / 360.0 + angle2 / 7560.0 + angle4 / 201600.0 + angle6 / 5987520.0;
            }
            else {
                eta = (1.0 - 0.5 * angle * std::tan(0.5 * M_PI - 0.5 * angle)) / (angle * angle);
                double sin_h_angle = std::sin(0.5 * angle);
                mu = (angle2 + 4.0 * std::cos(angle) + angle * std::sin(angle) - 4.0) / (4.0 * angle4 * sin_h_angle * sin_h_angle);
            }

            Spin(rotationVector, Omega);
            Omega2.addMatrixProduct(0.0, Omega, Omega, 1.0);

            OuterProd(momentVector, rotationVector, MxR);
            OuterProd(rotationVector, momentVector, RxM);
            
            SetIdentity(3, Li, (rotationVector ^ momentVector));
            Li.addMatrix(1.0, RxM, 1.0);
            Li.addMatrix(1.0, MxR, -1.0);

            LiTemp1.addMatrixProduct(0.0, Omega2, MxR, mu);
            Spin(momentVector, MxR, 0.5);
            LiTemp1.addMatrix(1.0, MxR, -1.0);

            LiTemp1.addMatrix(1.0, Li, eta);

            GetBlock(H, index + 3, index + 6, Hi);
            Li.addMatrixProduct(0.0, LiTemp1, Hi, 1.0);

            SetBlock(L, index + 3, index + 6, Li);
        }
    }
  };
} // end of XC namespace
#endif // !ASDEICR_h