ASDMath.h

// -*-c++-*-
//----------------------------------------------------------------------------
//  XC program; finite element analysis code
//  for structural analysis and design.
//
//  Copyright (C)  Luis C. Pérez Tato
//
//  This program derives from OpenSees <http://opensees.berkeley.edu>
//  developed by the  «Pacific earthquake engineering research center».
//
//  Except for the restrictions that may arise from the copyright
//  of the original program (see copyright_opensees.txt)
//  XC is free software: you can redistribute it and/or modify
//  it under the terms of the GNU General Public License as published by
//  the Free Software Foundation, either version 3 of the License, or 
//  (at your option) any later version.
//
//  This software is distributed in the hope that it will be useful, but 
//  WITHOUT ANY WARRANTY; without even the implied warranty of
//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//  GNU General Public License for more details. 
//
//
// You should have received a copy of the GNU General Public License 
// along with this program.
// If not, see <http://www.gnu.org/licenses/>.
//----------------------------------------------------------------------------
/* ****************************************************************** **
**    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 a Quaternion for compact and robust representation
// of spatial rotations
//

#ifndef ASDMath_h
#define ASDMath_h

#include <stdint.h>
#include <math.h>
#include <cmath>
#include <limits>
#include "utility/matrix/Vector.h"
#include "utility/matrix/Matrix.h"

namespace XC {

template<class T>
class ASDVector3
  {
  private:
    T mData[3];
  public:

     ASDVector3()
     {
         mData[0] = mData[1] = mData[2] = 0.0;
     }

     ASDVector3(T x, T y, T z)
     {
         mData[0] = x;
         mData[1] = y;
         mData[2] = z;
     }

     ASDVector3(const Vector& v, size_t index = 0)
     {
         mData[0] = v(0 + index);
         mData[1] = v(1 + index);
         mData[2] = v(2 + index);
     }

     ASDVector3(const ASDVector3& other)
     {
         mData[0] = other.mData[0];
         mData[1] = other.mData[1];
         mData[2] = other.mData[2];
     }

public:

     ASDVector3& operator= (const ASDVector3& other)
     {
         if (this != &other) {
             mData[0] = other.mData[0];
             mData[1] = other.mData[1];
             mData[2] = other.mData[2];
         }
         return *this;
     }

public:

     inline const T x()const { return mData[0]; }

     inline const T y()const { return mData[1]; }

     inline const T z()const { return mData[2]; }

     inline T operator()(size_t i) const { return mData[i]; }

     inline T& operator()(size_t i) { return mData[i]; }

     inline T operator[](size_t i) const { return mData[i]; }

     inline T& operator[](size_t i) { return mData[i]; }

public:

     inline T squaredNorm() const
     {
         return 
             mData[0] * mData[0] +
             mData[1] * mData[1] +
             mData[2] * mData[2];
     }

     inline T norm() const
     {
         return std::sqrt(squaredNorm());
     }

     inline T normalize()
     {
         T n = norm();
         if (n > 0.0) {
             mData[0] /= n;
             mData[1] /= n;
             mData[2] /= n;
         }
         return n;
     }

     inline T dot(const ASDVector3& b) const
     {
         return
             mData[0] * b.mData[0] +
             mData[1] * b.mData[1] +
             mData[2] * b.mData[2];
     }

     inline ASDVector3 cross(const ASDVector3& b) const
     {
         ASDVector3 c;
         c.mData[0] = mData[1] * b.mData[2] - mData[2] * b.mData[1];
         c.mData[1] = mData[2] * b.mData[0] - mData[0] * b.mData[2];
         c.mData[2] = mData[0] * b.mData[1] - mData[1] * b.mData[0];
         return c;
     }

public:

     inline void operator += (const ASDVector3& b)
     {
         mData[0] += b.mData[0];
         mData[1] += b.mData[1];
         mData[2] += b.mData[2];
     }

     inline ASDVector3 operator + (const ASDVector3& b) const
     {
         ASDVector3 a(*this);
         a += b;
         return a;
     }

     inline void operator -= (const ASDVector3& b)
     {
         mData[0] -= b.mData[0];
         mData[1] -= b.mData[1];
         mData[2] -= b.mData[2];
     }

     inline ASDVector3 operator - (const ASDVector3& b) const
     {
         ASDVector3 a(*this);
         a -= b;
         return a;
     }

     inline void operator *= (T b)
     {
         mData[0] *= b;
         mData[1] *= b;
         mData[2] *= b;
     }

     inline ASDVector3 operator * (T b) const
     {
         ASDVector3 a(*this);
         a *= b;
         return a;
     }

     inline void operator /= (T b)
     {
         mData[0] /= b;
         mData[1] /= b;
         mData[2] /= b;
     }

     inline ASDVector3 operator / (T b) const
     {
         ASDVector3 a(*this);
         a /= b;
         return a;
     }

  };

template<class T>
inline ASDVector3<T> operator * (T a, const ASDVector3<T>& b)
  { return b * a; }

template<class TStream, class T>
inline TStream& operator << (TStream& s, const ASDVector3<T>& v)
{
    return (s << "(" << v.x() << ", " << v.y() << ", " << v.z() << ")");
}

template<class T>
class ASDQuaternion
{

public:

    ASDQuaternion()
        : mX(0.0)
        , mY(0.0)
        , mZ(0.0)
        , mW(0.0)
    {
    }

    ASDQuaternion(T w, T x, T y, T z)
        : mX(x)
        , mY(y)
        , mZ(z)
        , mW(w)
    {
    }

    ASDQuaternion(const ASDQuaternion& other)
        : mX(other.mX)
        , mY(other.mY)
        , mZ(other.mZ)
        , mW(other.mW)
    {
    }

public:

    ASDQuaternion& operator= (const ASDQuaternion& other)
    {
        if (this != &other) {
            mX = other.mX;
            mY = other.mY;
            mZ = other.mZ;
            mW = other.mW;
        }
        return *this;
    }

public:

    inline const T x()const { return mX; }

    inline const T y()const { return mY; }

    inline const T z()const { return mZ; }

    inline const T w()const { return mW; }

public:

    inline const T squaredNorm()const
    {
        return mX * mX + mY * mY + mZ * mZ + mW * mW;
    }

    inline const T norm()const
    {
        return std::sqrt(squaredNorm());
    }

    inline void normalize()
    {
        T n = squaredNorm();
        if (n > 0.0 && n != 1.0) {
            n = std::sqrt(n);
            mX /= n;
            mY /= n;
            mZ /= n;
            mW /= n;
        }
    }

    inline ASDQuaternion conjugate()const
    {
        return ASDQuaternion(mW, -mX, -mY, -mZ);
    }

    template<class TMatrix3x3>
    inline void toRotationMatrix(TMatrix3x3& R)const
    {
        R(0, 0) = 2.0 * (mW * mW + mX * mX - 0.5);
        R(0, 1) = 2.0 * (mX * mY - mW * mZ);
        R(0, 2) = 2.0 * (mX * mZ + mW * mY);

        R(1, 0) = 2.0 * (mY * mX + mW * mZ);
        R(1, 1) = 2.0 * (mW * mW + mY * mY - 0.5);
        R(1, 2) = 2.0 * (mY * mZ - mX * mW);

        R(2, 0) = 2.0 * (mZ * mX - mW * mY);
        R(2, 1) = 2.0 * (mZ * mY + mW * mX);
        R(2, 2) = 2.0 * (mW * mW + mZ * mZ - 0.5);
    }

    inline void toRotationVector(T& rx, T& ry, T& rz)const
    {
        T xx, yy, zz, ww;

        if (mW < 0.0) {
            xx = -mX;
            yy = -mY;
            zz = -mZ;
            ww = -mW;
        }
        else {
            xx = mX;
            yy = mY;
            zz = mZ;
            ww = mW;
        }

        T vNorm = xx * xx + yy * yy + zz * zz;
        if (vNorm == 0.0) {
            rx = 0.0;
            ry = 0.0;
            rz = 0.0;
            return;
        }

        if (vNorm != 1.0)
            vNorm = std::sqrt(vNorm);

        T mult = (vNorm < ww) ? (2.0 / vNorm * std::asin(vNorm)) : (2.0 / vNorm * std::acos(ww));

        rx = xx * mult;
        ry = yy * mult;
        rz = zz * mult;
    }

    template<class TVector3>
    inline void toRotationVector(TVector3& v)const
    {
        toRotationVector(v(0), v(1), v(2));
    }

    template<class TVector3_A, class TVector3_B>
    inline void rotateVector(const TVector3_A& a, TVector3_B& b)const
    {
        // b = 2.0 * cross( this->VectorialPart, a )
        b(0) = 2.0 * (mY * a(2) - mZ * a(1));
        b(1) = 2.0 * (mZ * a(0) - mX * a(2));
        b(2) = 2.0 * (mX * a(1) - mY * a(0));

        // c = cross( this->VectorialPart, b )
        T c0 = mY * b(2) - mZ * b(1);
        T c1 = mZ * b(0) - mX * b(2);
        T c2 = mX * b(1) - mY * b(0);

        // set results
        b(0) = a(0) + b(0) * mW + c0;
        b(1) = a(1) + b(1) * mW + c1;
        b(2) = a(2) + b(2) * mW + c2;
    }

    template<class TVector3>
    inline void rotateVector(TVector3& a)const
    {
        // b = 2.0 * cross( this->VectorialPart, a )
        T b0 = 2.0 * (mY * a(2) - mZ * a(1));
        T b1 = 2.0 * (mZ * a(0) - mX * a(2));
        T b2 = 2.0 * (mX * a(1) - mY * a(0));

        // c = cross( this->VectorialPart, b )
        T c0 = mY * b2 - mZ * b1;
        T c1 = mZ * b0 - mX * b2;
        T c2 = mX * b1 - mY * b0;

        // set results
        a(0) += b0 * mW + c0;
        a(1) += b1 * mW + c1;
        a(2) += b2 * mW + c2;
    }

public:

    static inline ASDQuaternion Identity()
    {
        return ASDQuaternion(1.0, 0.0, 0.0, 0.0);
    }

    static inline ASDQuaternion FromAxisAngle(T x, T y, T z, T radians)
    {
        T sqnorm = x * x + y * y + z * z;
        if (sqnorm == 0.0)
            return ASDQuaternion::Identity();

        if (sqnorm > 0.0 && sqnorm != 1.0) {
            T norm = std::sqrt(sqnorm);
            x /= norm;
            y /= norm;
            z /= norm;
        }

        T halfAngle = radians * 0.5;

        T s = std::sin(halfAngle);
        T q0 = std::cos(halfAngle);

        ASDQuaternion result(q0, s * x, s * y, s * z);
        result.normalize();

        return result;
    }

    static inline ASDQuaternion FromRotationVector(T rx, T ry, T rz)
    {
        T rModulus = rx * rx + ry * ry + rz * rz;
        if (rModulus == 0.0)
            return ASDQuaternion::Identity();

        if (rModulus != 1.0) {
            rModulus = std::sqrt(rModulus);
            rx /= rModulus;
            ry /= rModulus;
            rz /= rModulus;
        }

        T halfAngle = rModulus * 0.5;

        T q0 = std::cos(halfAngle);
        T s = std::sin(halfAngle);

        ASDQuaternion result(q0, rx * s, ry * s, rz * s);
        result.normalize();

        return result;
    }

    template<class TVector3>
    static inline ASDQuaternion FromRotationVector(const TVector3& v)
    {
        return ASDQuaternion::FromRotationVector(v(0), v(1), v(2));
    }

    template<class TMatrix3x3>
    static inline ASDQuaternion FromRotationMatrix(const TMatrix3x3& m)
    {
        T xx = m(0, 0);
        T yy = m(1, 1);
        T zz = m(2, 2);
        T tr = xx + yy + zz;
        ASDQuaternion Q;
        if ((tr > xx) && (tr > yy) && (tr > zz))
        {
            T S = std::sqrt(tr + 1.0) * 2.0;
            Q = ASDQuaternion(
                0.25 * S,
                (m(2, 1) - m(1, 2)) / S,
                (m(0, 2) - m(2, 0)) / S,
                (m(1, 0) - m(0, 1)) / S
            );
        }
        else if ((xx > yy) && (xx > zz))
        {
            T S = std::sqrt(1.0 + xx - yy - zz) * 2.0;
            Q = ASDQuaternion(
                (m(2, 1) - m(1, 2)) / S,
                0.25 * S,
                (m(0, 1) + m(1, 0)) / S,
                (m(0, 2) + m(2, 0)) / S
            );
        }
        else if (yy > zz)
        {
            T S = std::sqrt(1.0 + yy - xx - zz) * 2.0;
            Q = ASDQuaternion(
                (m(0, 2) - m(2, 0)) / S,
                (m(0, 1) + m(1, 0)) / S,
                0.25 * S,
                (m(1, 2) + m(2, 1)) / S
            );
        }
        else
        {
            T S = std::sqrt(1.0 + zz - xx - yy) * 2.0;
            Q = ASDQuaternion(
                (m(1, 0) - m(0, 1)) / S,
                (m(0, 2) + m(2, 0)) / S,
                (m(1, 2) + m(2, 1)) / S,
                0.25 * S
            );
        }


        Q.normalize();
        return Q;
    }

private:
    T mX;
    T mY;
    T mZ;
    T mW;

};

template<class T>
inline ASDQuaternion<T> operator* (const ASDQuaternion<T>& a, const ASDQuaternion<T>& b)
{
    return ASDQuaternion<T>(
        a.w() * b.w() - a.x() * b.x() - a.y() * b.y() - a.z() * b.z(),
        a.w() * b.x() + a.x() * b.w() + a.y() * b.z() - a.z() * b.y(),
        a.w() * b.y() + a.y() * b.w() + a.z() * b.x() - a.x() * b.z(),
        a.w() * b.z() + a.z() * b.w() + a.x() * b.y() - a.y() * b.x()
        );
}

template<class TStream, class T>
inline TStream& operator << (TStream& s, const ASDQuaternion<T>& q)
  {
    return (s << "[(" << q.x() << ", " << q.y() << ", " << q.z() << "), " << q.w() << "]");
  }
} // end of XC namespace

#endif // !ASDMath_h