diff --git a/src/common/aka_types.hh b/src/common/aka_types.hh index 8f948eaa8..ce3fbf341 100644 --- a/src/common/aka_types.hh +++ b/src/common/aka_types.hh @@ -1,1916 +1,1923 @@ /** * @file aka_types.hh * * @author Nicolas Richart * * @date creation: Thu Feb 17 2011 * @date last modification: Wed Dec 09 2020 * * @brief description of the "simple" types * * * @section LICENSE * * Copyright (©) 2010-2021 EPFL (Ecole Polytechnique Fédérale de Lausanne) * Laboratory (LSMS - Laboratoire de Simulation en Mécanique des Solides) * * Akantu is free software: you can redistribute it and/or modify it under the * terms of the GNU Lesser General Public License as published by the Free * Software Foundation, either version 3 of the License, or (at your option) any * later version. * * Akantu 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 Lesser General Public License for more * details. * * You should have received a copy of the GNU Lesser General Public License * along with Akantu. If not, see . * */ /* -------------------------------------------------------------------------- */ #include "aka_common.hh" #include "aka_math.hh" /* -------------------------------------------------------------------------- */ #include #include #include /* -------------------------------------------------------------------------- */ #ifndef AKANTU_AKA_TYPES_HH_ #define AKANTU_AKA_TYPES_HH_ namespace akantu { enum NormType { L_1 = 1, L_2 = 2, L_inf = UInt(-1) }; /** * DimHelper is a class to generalize the setup of a dim array from 3 * values. This gives a common interface in the TensorStorage class * independently of its derived inheritance (Vector, Matrix, Tensor3) * @tparam dim */ template struct DimHelper { static inline void setDims(UInt m, UInt n, UInt p, std::array & dims); }; /* -------------------------------------------------------------------------- */ template <> struct DimHelper<1> { static inline void setDims(UInt m, UInt /*n*/, UInt /*p*/, std::array & dims) { dims[0] = m; } }; /* -------------------------------------------------------------------------- */ template <> struct DimHelper<2> { static inline void setDims(UInt m, UInt n, UInt /*p*/, std::array & dims) { dims[0] = m; dims[1] = n; } }; /* -------------------------------------------------------------------------- */ template <> struct DimHelper<3> { static inline void setDims(UInt m, UInt n, UInt p, std::array & dims) { dims[0] = m; dims[1] = n; dims[2] = p; } }; /* -------------------------------------------------------------------------- */ template class TensorStorage; /* -------------------------------------------------------------------------- */ /* Proxy classes */ /* -------------------------------------------------------------------------- */ namespace tensors { template struct is_copyable { enum : bool { value = false }; }; template struct is_copyable { enum : bool { value = true }; }; template struct is_copyable { enum : bool { value = true }; }; template struct is_copyable { enum : bool { value = true }; }; } // namespace tensors /* -------------------------------------------------------------------------- */ /* -------------------------------------------------------------------------- */ namespace types { namespace details { template class vector_iterator { public: using difference_type = std::ptrdiff_t; using value_type = std::decay_t; using pointer = value_type *; using reference = reference_; using iterator_category = std::input_iterator_tag; vector_iterator(pointer ptr) : ptr(ptr) {} // input iterator ++it vector_iterator & operator++() { ++ptr; return *this; } // input iterator it++ vector_iterator operator++(int) { auto cpy = *this; ++ptr; return cpy; } vector_iterator & operator+=(int n) { ptr += n; return *this; } vector_iterator operator+(int n) { vector_iterator cpy(*this); cpy += n; return cpy; } // input iterator it != other_it bool operator!=(const vector_iterator & other) const { return ptr != other.ptr; } bool operator==(const vector_iterator & other) const { return ptr == other.ptr; } difference_type operator-(const vector_iterator & other) const { return this->ptr - other.ptr; } // input iterator dereference *it reference operator*() { return *ptr; } pointer operator->() { return ptr; } private: pointer ptr; }; } // namespace details } // namespace types /** * @class TensorProxy aka_types.hh * The TensorProxy class is a proxy class to the * TensorStorage it handles the wrapped case. That is to say if an accessor * should give access to a Tensor wrapped on some data, like the * Array::iterator they can return a TensorProxy that will be automatically * transformed as a TensorStorage wrapped on the same data * @tparam T stored type * @tparam ndim order of the tensor * @tparam _RetType real derived type */ template class TensorProxy : public TensorProxyTrait { protected: using RetTypeProxy = typename RetType_::proxy; constexpr TensorProxy(T * data, UInt m, UInt n, UInt p) { DimHelper::setDims(m, n, p, this->n); this->values = data; } template ::value>> explicit TensorProxy(const Other & other) { this->values = other.storage(); for (UInt i = 0; i < ndim; ++i) { this->n[i] = other.size(i); } } public: using RetType = RetType_; UInt size(UInt i) const { AKANTU_DEBUG_ASSERT(i < ndim, "This tensor has only " << ndim << " dimensions, not " << (i + 1)); return n[i]; } inline UInt size() const { UInt _size = 1; for (UInt d = 0; d < ndim; ++d) { _size *= this->n[d]; } return _size; } T * storage() const { return values; } template ::value>> inline TensorProxy & operator=(const Other & other) { AKANTU_DEBUG_ASSERT( other.size() == this->size(), "You are trying to copy two tensors with different sizes"); std::copy_n(other.storage(), this->size(), this->values); return *this; } // template ::value>> // inline TensorProxy & operator=(const Other && other) { // AKANTU_DEBUG_ASSERT( // other.size() == this->size(), // "You are trying to copy two tensors with different sizes"); // memcpy(this->values, other.storage(), this->size() * sizeof(T)); // return *this; // } template inline RetTypeProxy & operator*=(const O & o) { RetType(*this) *= o; return static_cast(*this); } template inline RetTypeProxy & operator/=(const O & o) { RetType(*this) /= o; return static_cast(*this); } protected: T * values; std::array n; }; /* -------------------------------------------------------------------------- */ template class VectorProxy : public TensorProxy> { using parent = TensorProxy>; using type = Vector; public: constexpr VectorProxy(T * data, UInt n) : parent(data, n, 0, 0) {} template explicit VectorProxy(Other & src) : parent(src) {} /* ---------------------------------------------------------------------- */ template inline VectorProxy & operator=(const Other & other) { parent::operator=(other); return *this; } // inline VectorProxy & operator=(const VectorProxy && other) { // parent::operator=(other); // return *this; // } using iterator = types::details::vector_iterator; using const_iterator = types::details::vector_iterator; iterator begin() { return iterator(this->storage()); } iterator end() { return iterator(this->storage() + this->size()); } const_iterator begin() const { return const_iterator(this->storage()); } const_iterator end() const { return const_iterator(this->storage() + this->size()); } /* ------------------------------------------------------------------------ */ T & operator()(UInt index) { return this->values[index]; }; const T & operator()(UInt index) const { return this->values[index]; }; }; template class MatrixProxy : public TensorProxy> { using parent = TensorProxy>; using type = Matrix; public: MatrixProxy(T * data, UInt m, UInt n) : parent(data, m, n, 0) {} template explicit MatrixProxy(Other & src) : parent(src) {} /* ---------------------------------------------------------------------- */ template inline MatrixProxy & operator=(const Other & other) { parent::operator=(other); return *this; } }; template class Tensor3Proxy : public TensorProxy> { using parent = TensorProxy>; using type = Tensor3; public: Tensor3Proxy(const T * data, UInt m, UInt n, UInt k) : parent(data, m, n, k) {} Tensor3Proxy(const Tensor3Proxy & src) : parent(src) {} Tensor3Proxy(const Tensor3 & src) : parent(src) {} /* ---------------------------------------------------------------------- */ template inline Tensor3Proxy & operator=(const Other & other) { parent::operator=(other); return *this; } }; /* -------------------------------------------------------------------------- */ /* Tensor base class */ /* -------------------------------------------------------------------------- */ template class TensorStorage : public TensorTrait { public: using value_type = T; friend class Array; protected: template void copySize(const TensorType & src) { for (UInt d = 0; d < ndim; ++d) { this->n[d] = src.size(d); // NOLINT } this->_size = src.size(); } TensorStorage() = default; TensorStorage(const TensorProxy & proxy) { this->copySize(proxy); this->values = proxy.storage(); this->wrapped = true; } public: TensorStorage(const TensorStorage & src) = delete; TensorStorage(const TensorStorage & src, bool deep_copy) : values(nullptr) { if (deep_copy) { this->deepCopy(src); } else { this->shallowCopy(src); } } protected: TensorStorage(UInt m, UInt n, UInt p, const T & def) { static_assert(std::is_trivially_constructible{}, "Cannot create a tensor on non trivial types"); DimHelper::setDims(m, n, p, this->n); this->computeSize(); this->values = new T[this->_size]; // NOLINT this->set(def); this->wrapped = false; } TensorStorage(T * data, UInt m, UInt n, UInt p) { DimHelper::setDims(m, n, p, this->n); this->computeSize(); this->values = data; this->wrapped = true; } public: /* ------------------------------------------------------------------------ */ template inline void shallowCopy(const TensorType & src) { this->copySize(src); if (!this->wrapped) { delete[] this->values; } this->values = src.storage(); this->wrapped = true; } /* ------------------------------------------------------------------------ */ template inline void deepCopy(const TensorType & src) { this->copySize(src); if (!this->wrapped) { delete[] this->values; } static_assert(std::is_trivially_constructible{}, "Cannot create a tensor on non trivial types"); this->values = new T[this->_size]; // NOLINT static_assert(std::is_trivially_copyable{}, "Cannot copy a tensor on non trivial types"); std::copy_n(src.storage(), this->_size, this->values); this->wrapped = false; } virtual ~TensorStorage() { if (!this->wrapped) { delete[] this->values; } } /* ------------------------------------------------------------------------ */ inline TensorStorage & operator=(const TensorStorage & other) { if (this == &other) { return *this; } this->operator=(aka::as_type(other)); return *this; } // inline TensorStorage & operator=(TensorStorage && other) noexcept { // std::swap(n, other.n); // std::swap(_size, other._size); // std::swap(values, other.values); // std::swap(wrapped, other.wrapped); // return *this; // } /* ------------------------------------------------------------------------ */ inline TensorStorage & operator=(const RetType & src) { if (this != &src) { if (this->wrapped) { static_assert(std::is_trivially_copyable{}, "Cannot copy a tensor on non trivial types"); // this test is not sufficient for Tensor of order higher than 1 AKANTU_DEBUG_ASSERT(this->_size == src.size(), "Tensors of different size (" << this->_size << " != " << src.size() << ")"); std::copy_n(src.storage(), this->_size, this->values); } else { deepCopy(src); } } return *this; } /* ------------------------------------------------------------------------ */ template inline RetType & operator+=(const TensorStorage & other) { T * a = this->storage(); T * b = other.storage(); AKANTU_DEBUG_ASSERT( _size == other.size(), "The two tensors do not have the same size, they cannot be subtracted"); for (UInt i = 0; i < _size; ++i) { *(a++) += *(b++); } return *(static_cast(this)); } /* ------------------------------------------------------------------------ */ template inline RetType & operator-=(const TensorStorage & other) { T * a = this->storage(); T * b = other.storage(); AKANTU_DEBUG_ASSERT( _size == other.size(), "The two tensors do not have the same size, they cannot be subtracted"); for (UInt i = 0; i < _size; ++i) { *(a++) -= *(b++); } return *(static_cast(this)); } /* ------------------------------------------------------------------------ */ inline RetType & operator+=(const T & x) { T * a = this->values; for (UInt i = 0; i < _size; ++i) { *(a++) += x; } return *(static_cast(this)); } /* ------------------------------------------------------------------------ */ inline RetType & operator-=(const T & x) { T * a = this->values; for (UInt i = 0; i < _size; ++i) { *(a++) -= x; } return *(static_cast(this)); } /* ------------------------------------------------------------------------ */ inline RetType & operator*=(const T & x) { T * a = this->storage(); for (UInt i = 0; i < _size; ++i) { *(a++) *= x; } return *(static_cast(this)); } /* ---------------------------------------------------------------------- */ inline RetType & operator/=(const T & x) { T * a = this->values; for (UInt i = 0; i < _size; ++i) { *(a++) /= x; } return *(static_cast(this)); } /// \f[Y = \alpha X + Y\f] inline RetType & aXplusY(const TensorStorage & other, const T & alpha = 1.) { AKANTU_DEBUG_ASSERT( _size == other.size(), "The two tensors do not have the same size, they cannot be subtracted"); Math::aXplusY(this->_size, alpha, other.storage(), this->storage()); return *(static_cast(this)); } /* ------------------------------------------------------------------------ */ T * storage() const { return values; } UInt size() const { return _size; } UInt size(UInt i) const { AKANTU_DEBUG_ASSERT(i < ndim, "This tensor has only " << ndim << " dimensions, not " << (i + 1)); return n[i]; }; /* ------------------------------------------------------------------------ */ inline void set(const T & t) { std::fill_n(values, _size, t); }; inline void zero() { this->set(0.); }; template inline void copy(const TensorType & other) { AKANTU_DEBUG_ASSERT( _size == other.size(), "The two tensors do not have the same size, they cannot be copied"); std::copy_n(other.storage(), _size, values); } bool isWrapped() const { return this->wrapped; } protected: inline void computeSize() { _size = 1; for (UInt d = 0; d < ndim; ++d) { _size *= this->n[d]; } } protected: template struct NormHelper { template static R norm(const Ten & ten) { R _norm = 0.; R * it = ten.storage(); R * end = ten.storage() + ten.size(); for (; it < end; ++it) { _norm += std::pow(std::abs(*it), norm_type); } return std::pow(_norm, 1. / norm_type); } }; // namespace akantu template struct NormHelper { template static R norm(const Ten & ten) { R _norm = 0.; R * it = ten.storage(); R * end = ten.storage() + ten.size(); for (; it < end; ++it) { _norm += std::abs(*it); } return _norm; } }; template struct NormHelper { template static R norm(const Ten & ten) { R _norm = 0.; R * it = ten.storage(); R * end = ten.storage() + ten.size(); for (; it < end; ++it) { _norm += *it * *it; } return sqrt(_norm); } }; template struct NormHelper { template static R norm(const Ten & ten) { R _norm = 0.; R * it = ten.storage(); R * end = ten.storage() + ten.size(); for (; it < end; ++it) { _norm = std::max(std::abs(*it), _norm); } return _norm; } }; public: /*----------------------------------------------------------------------- */ /// "Entrywise" norm norm @f[ \|\boldsymbol{T}\|_p = \left( /// \sum_i^{n[0]}\sum_j^{n[1]}\sum_k^{n[2]} |T_{ijk}|^p \right)^{\frac{1}{p}} /// @f] template inline T norm() const { return NormHelper::norm(*this); } protected: std::array n{}; UInt _size{0}; T * values{nullptr}; bool wrapped{false}; }; /* -------------------------------------------------------------------------- */ /* Vector */ /* -------------------------------------------------------------------------- */ template class Vector : public TensorStorage> { using parent = TensorStorage>; public: using value_type = typename parent::value_type; using proxy = VectorProxy; public: Vector() : parent() {} explicit Vector(UInt n, const T & def = T()) : parent(n, 0, 0, def) {} Vector(T * data, UInt n) : parent(data, n, 0, 0) {} Vector(const Vector & src, bool deep_copy = true) : parent(src, deep_copy) {} Vector(const TensorProxy & src) : parent(src) {} Vector(std::initializer_list list) : parent(list.size(), 0, 0, T()) { UInt i = 0; for (auto val : list) { operator()(i++) = val; } } public: using iterator = types::details::vector_iterator; using const_iterator = types::details::vector_iterator; iterator begin() { return iterator(this->storage()); } iterator end() { return iterator(this->storage() + this->size()); } const_iterator begin() const { return const_iterator(this->storage()); } const_iterator end() const { return const_iterator(this->storage() + this->size()); } public: ~Vector() override = default; /* ------------------------------------------------------------------------ */ inline Vector & operator=(const Vector & src) { parent::operator=(src); return *this; } inline Vector & operator=(Vector && src) noexcept = default; /* ------------------------------------------------------------------------ */ inline T & operator()(UInt i) { AKANTU_DEBUG_ASSERT((i < this->n[0]), "Access out of the vector! " << "Index (" << i << ") is out of the vector of size (" << this->n[0] << ")"); return *(this->values + i); } inline const T & operator()(UInt i) const { AKANTU_DEBUG_ASSERT((i < this->n[0]), "Access out of the vector! " << "Index (" << i << ") is out of the vector of size (" << this->n[0] << ")"); return *(this->values + i); } inline T & operator[](UInt i) { return this->operator()(i); } inline const T & operator[](UInt i) const { return this->operator()(i); } /* ------------------------------------------------------------------------ */ inline Vector & operator*=(Real x) { return parent::operator*=(x); } inline Vector & operator/=(Real x) { return parent::operator/=(x); } /* ------------------------------------------------------------------------ */ inline Vector & operator*=(const Vector & vect) { AKANTU_DEBUG_ASSERT(this->_size == vect._size, "The vectors have non matching sizes"); T * a = this->storage(); T * b = vect.storage(); for (UInt i = 0; i < this->_size; ++i) { *(a++) *= *(b++); } return *this; } /* ------------------------------------------------------------------------ */ inline Real dot(const Vector & vect) const { return Math::vectorDot(this->values, vect.storage(), this->_size); } /* ------------------------------------------------------------------------ */ inline Real mean() const { Real mean = 0; T * a = this->storage(); for (UInt i = 0; i < this->_size; ++i) { mean += *(a++); } return mean / this->_size; } /* ------------------------------------------------------------------------ */ inline Vector & crossProduct(const Vector & v1, const Vector & v2) { AKANTU_DEBUG_ASSERT(this->size() == 3, "crossProduct is only defined in 3D (n=" << this->size() << ")"); AKANTU_DEBUG_ASSERT( this->size() == v1.size() && this->size() == v2.size(), "crossProduct is not a valid operation non matching size vectors"); Math::vectorProduct3(v1.storage(), v2.storage(), this->values); return *this; } inline Vector crossProduct(const Vector & v) { Vector tmp(this->size()); tmp.crossProduct(*this, v); return tmp; } /* ------------------------------------------------------------------------ */ inline void solve(const Matrix & A, const Vector & b) { AKANTU_DEBUG_ASSERT( this->size() == A.rows() && this->_size == A.cols(), "The size of the solution vector mismatches the size of the matrix"); AKANTU_DEBUG_ASSERT( this->_size == b._size, "The rhs vector has a mismatch in size with the matrix"); Math::solve(this->_size, A.storage(), this->values, b.storage()); } /* ------------------------------------------------------------------------ */ inline void skew2vec(const Matrix & mat) { AKANTU_DEBUG_ASSERT(this->size() == 3 && mat.rows() == 3 && mat.cols() == 3, "need a 3 dim matrix and vector"); (*this) = {mat(2,1), mat(0,2), mat(1,0)}; } /* ------------------------------------------------------------------------ */ inline void logMap(const Matrix &R) { AKANTU_DEBUG_ASSERT(this->size() == 3 && R.rows() == 3 && R.cols() == 3, "need a 3 dim matrix and vector"); Real theta = R.computeRotationAngle(); if (theta < 1e-8) { (*this) = {-R(1,2), R(0,2), -R(0,1)}; } auto sym = ((R - R.transpose()).template norm() / R.template norm()) < 1e-12; if (sym) { Matrix _R = (R + R.transpose()) / 2.; Matrix eig_vects(3,3); Vector eig_vals(3); _R.eig(eig_vals, eig_vects); for (UInt i = 0; i < eig_vals.size(); ++i) { auto val = eig_vals(i); Vector vect = eig_vects(i); if (std::abs(val - 1) < 1e-14) { (*this) = theta * vect / vect.norm(); } } throw std::runtime_error("Symmetric cannot log:"); } Matrix A = (R - R.transpose()) / 2.; Matrix _log = theta / std::sin(theta) * A; Vector res(3); res.skew2vec(_log); (*this) = res; } /* ------------------------------------------------------------------------ */ /** Computes the convected angle derivative . This is the closest quantity to an angle variation generalized to 3D. \f[ \Lambda^T(t)\frac{d}{dt}\Lambda(t) \f] with \f$\Lambda^T(t) = exp(\theta(t))\f$. The implementation is based on Rodrigues's formula. @param B the provided rotation vector \f$\theta(t)\f$ @param Bp the provided rotation vector derivative \f$\frac{d}{dt}\theta(t)\f$ */ inline void convectedAngleDerivative(const Vector &B, const Vector &Bp) { AKANTU_DEBUG_ASSERT(this->size() == B.size() == Bp.size() ==3, "need a 3 dim vectors"); Matrix B1(3,3); Matrix Bp1(3,3); B1.expMap(-1. * B); Bp1.expDerivative(B, Bp); Matrix exp_deriv = B1 * Bp1; Vector skew_exp_deriv(3); skew_exp_deriv.skew2vec(exp_deriv); (*this) = skew_exp_deriv; } /* ------------------------------------------------------------------------ */ inline void angleAcceleration(const Vector &B, const Vector &Bp, const Vector &Bpp) { AKANTU_DEBUG_ASSERT(this->size() == B.size() == Bp.size() == Bpp.size() == 3, "need a 3 dim vectors"); Matrix L(3,3); Matrix vel(3,3); Matrix acc(3,3); L.expMap(B); vel.expDerivative(B, Bp); acc.expAcceleration(B, Bp, Bpp); Matrix res = acc * L.transpose() + vel * vel.transpose(); Vector rev_vec(3); rev_vec.skew2vec(res); (*this) = rev_vec; } /* ------------------------------------------------------------------------ */ /** Given two vectors, representing rotations, this functions return the vector representing the composition of the two rotations.*/ inline auto composeRotations(const Vector &a1, const Vector &a2) { AKANTU_DEBUG_ASSERT(this->size() == a1.size() == a2.size() == 3, "need a 3 dim vectors"); Matrix mat1(3,3); Matrix mat2(3,3); mat1.expMap(a1); mat2.expMap(a2); Matrix mat12 = mat1 * mat2; Vector vec12(3); vec12.logMap(mat12); (*this) = vec12; } /* ------------------------------------------------------------------------ */ /* ------------------------------------------------------------------------ */ template inline void mul(const Matrix & A, const Vector & x, T alpha = T(1)); /* ------------------------------------------------------------------------ */ inline Real norm() const { return parent::template norm(); } template inline Real norm() const { return parent::template norm(); } /* ------------------------------------------------------------------------ */ inline Vector & normalize() { Real n = norm(); operator/=(n); return *this; } /* ------------------------------------------------------------------------ */ /// norm of (*this - x) inline Real distance(const Vector & y) const { Real * vx = this->values; Real * vy = y.storage(); Real sum_2 = 0; for (UInt i = 0; i < this->_size; ++i, ++vx, ++vy) { // NOLINT sum_2 += (*vx - *vy) * (*vx - *vy); } return sqrt(sum_2); } /* ------------------------------------------------------------------------ */ inline bool equal(const Vector & v, Real tolerance = Math::getTolerance()) const { T * a = this->storage(); T * b = v.storage(); UInt i = 0; while (i < this->_size && (std::abs(*(a++) - *(b++)) < tolerance)) { ++i; } return i == this->_size; } /* ------------------------------------------------------------------------ */ inline short compare(const Vector & v, Real tolerance = Math::getTolerance()) const { T * a = this->storage(); T * b = v.storage(); for (UInt i(0); i < this->_size; ++i, ++a, ++b) { if (std::abs(*a - *b) > tolerance) { return (((*a - *b) > tolerance) ? 1 : -1); } } return 0; } /* ------------------------------------------------------------------------ */ inline bool operator==(const Vector & v) const { return equal(v); } inline bool operator!=(const Vector & v) const { return !operator==(v); } inline bool operator<(const Vector & v) const { return compare(v) == -1; } inline bool operator>(const Vector & v) const { return compare(v) == 1; } template decltype(auto) accumulate(const Vector & v, Acc && accumulator, Func && func) const { T * a = this->storage(); T * b = v.storage(); for (UInt i(0); i < this->_size; ++i, ++a, ++b) { accumulator = func(*a, *b, std::forward(accumulator)); } return accumulator; } inline bool operator<=(const Vector & v) const { bool res = true; return accumulate(v, res, [](auto && a, auto && b, auto && accumulator) { return accumulator & (a <= b); }); } inline bool operator>=(const Vector & v) const { bool res = true; return accumulate(v, res, [](auto && a, auto && b, auto && accumulator) { return accumulator & (a >= b); }); } /* ------------------------------------------------------------------------ */ /// function to print the containt of the class virtual void printself(std::ostream & stream, int indent = 0) const { std::string space; for (Int i = 0; i < indent; i++, space += AKANTU_INDENT) { ; } stream << "["; for (UInt i = 0; i < this->_size; ++i) { if (i != 0) { stream << ", "; } stream << this->values[i]; } stream << "]"; } /* ---------------------------------------------------------------------- */ static inline Vector zeros(UInt n) { Vector tmp(n); tmp.set(T()); return tmp; } }; using RVector = Vector; /* ------------------------------------------------------------------------ */ template <> inline bool Vector::equal(const Vector & v, __attribute__((unused)) Real tolerance) const { UInt * a = this->storage(); UInt * b = v.storage(); UInt i = 0; while (i < this->_size && (*(a++) == *(b++))) { ++i; } return i == this->_size; } /* -------------------------------------------------------------------------- */ namespace types { namespace details { template class column_iterator { public: using difference_type = std::ptrdiff_t; using value_type = decltype(std::declval().operator()(0)); using pointer = value_type *; using reference = value_type &; using iterator_category = std::input_iterator_tag; column_iterator(Mat & mat, UInt col) : mat(mat), col(col) {} decltype(auto) operator*() { return mat(col); } decltype(auto) operator++() { ++col; AKANTU_DEBUG_ASSERT(col <= mat.cols(), "The iterator is out of bound"); return *this; } decltype(auto) operator++(int) { auto tmp = *this; ++col; AKANTU_DEBUG_ASSERT(col <= mat.cols(), "The iterator is out of bound"); return tmp; } bool operator!=(const column_iterator & other) const { return col != other.col; } bool operator==(const column_iterator & other) const { return not operator!=(other); } private: Mat & mat; UInt col; }; } // namespace details } // namespace types /* ------------------------------------------------------------------------ */ /* Matrix */ /* ------------------------------------------------------------------------ */ template class Matrix : public TensorStorage> { using parent = TensorStorage>; public: using value_type = typename parent::value_type; using proxy = MatrixProxy; public: Matrix() : parent() {} Matrix(UInt m, UInt n, const T & def = T()) : parent(m, n, 0, def) {} Matrix(T * data, UInt m, UInt n) : parent(data, m, n, 0) {} Matrix(const Matrix & src, bool deep_copy = true) : parent(src, deep_copy) {} Matrix(const MatrixProxy & src) : parent(src) {} Matrix(std::initializer_list> list) { static_assert(std::is_trivially_copyable{}, "Cannot create a tensor on non trivial types"); std::size_t n = 0; std::size_t m = list.size(); for (auto row : list) { n = std::max(n, row.size()); } DimHelper<2>::setDims(m, n, 0, this->n); this->computeSize(); this->values = new T[this->_size]; this->set(0); UInt i{0}; UInt j{0}; for (auto & row : list) { for (auto & val : row) { at(i, j++) = val; } ++i; j = 0; } } ~Matrix() override = default; /* ------------------------------------------------------------------------ */ inline Matrix & operator=(const Matrix & src) { parent::operator=(src); return *this; } inline Matrix & operator=(Matrix && src) noexcept = default; public: /* ---------------------------------------------------------------------- */ UInt rows() const { return this->n[0]; } UInt cols() const { return this->n[1]; } /* ---------------------------------------------------------------------- */ inline T & at(UInt i, UInt j) { AKANTU_DEBUG_ASSERT(((i < this->n[0]) && (j < this->n[1])), "Access out of the matrix! " << "Index (" << i << ", " << j << ") is out of the matrix of size (" << this->n[0] << ", " << this->n[1] << ")"); return *(this->values + i + j * this->n[0]); } inline const T & at(UInt i, UInt j) const { AKANTU_DEBUG_ASSERT(((i < this->n[0]) && (j < this->n[1])), "Access out of the matrix! " << "Index (" << i << ", " << j << ") is out of the matrix of size (" << this->n[0] << ", " << this->n[1] << ")"); return *(this->values + i + j * this->n[0]); } /* ------------------------------------------------------------------------ */ inline T & operator()(UInt i, UInt j) { return this->at(i, j); } inline const T & operator()(UInt i, UInt j) const { return this->at(i, j); } /// give a line vector wrapped on the column i inline VectorProxy operator()(UInt j) { AKANTU_DEBUG_ASSERT(j < this->n[1], "Access out of the matrix! " << "You are trying to access the column vector " << j << " in a matrix of size (" << this->n[0] << ", " << this->n[1] << ")"); return VectorProxy(this->values + j * this->n[0], this->n[0]); } inline VectorProxy operator()(UInt j) const { AKANTU_DEBUG_ASSERT(j < this->n[1], "Access out of the matrix! " << "You are trying to access the column vector " << j << " in a matrix of size (" << this->n[0] << ", " << this->n[1] << ")"); return VectorProxy(this->values + j * this->n[0], this->n[0]); } public: decltype(auto) begin() { return types::details::column_iterator>(*this, 0); } decltype(auto) begin() const { return types::details::column_iterator>(*this, 0); } decltype(auto) end() { return types::details::column_iterator>(*this, this->cols()); } decltype(auto) end() const { return types::details::column_iterator>(*this, this->cols()); } /* ------------------------------------------------------------------------ */ inline void block(const Matrix & block, UInt pos_i, UInt pos_j) { AKANTU_DEBUG_ASSERT(pos_i + block.rows() <= rows(), "The block size or position are not correct"); AKANTU_DEBUG_ASSERT(pos_i + block.cols() <= cols(), "The block size or position are not correct"); for (UInt i = 0; i < block.rows(); ++i) { for (UInt j = 0; j < block.cols(); ++j) { this->at(i + pos_i, j + pos_j) = block(i, j); } } } inline Matrix block(UInt pos_i, UInt pos_j, UInt block_rows, UInt block_cols) const { AKANTU_DEBUG_ASSERT(pos_i + block_rows <= rows(), "The block size or position are not correct"); AKANTU_DEBUG_ASSERT(pos_i + block_cols <= cols(), "The block size or position are not correct"); Matrix block(block_rows, block_cols); for (UInt i = 0; i < block_rows; ++i) { for (UInt j = 0; j < block_cols; ++j) { block(i, j) = this->at(i + pos_i, j + pos_j); } } return block; } inline T & operator[](UInt idx) { return *(this->values + idx); }; inline const T & operator[](UInt idx) const { return *(this->values + idx); }; /* ---------------------------------------------------------------------- */ inline Matrix operator*(const Matrix & B) const { Matrix C(this->rows(), B.cols()); C.mul(*this, B); return C; } /* ----------------------------------------------------------------------- */ inline Matrix & operator*=(const T & x) { return parent::operator*=(x); } inline Matrix & operator*=(const Matrix & B) { Matrix C(*this); this->mul(C, B); return *this; } /* ---------------------------------------------------------------------- */ template inline void mul(const Matrix & A, const Matrix & B, T alpha = 1.0) { UInt k = A.cols(); if (tr_A) { k = A.rows(); } #ifndef AKANTU_NDEBUG if (tr_B) { AKANTU_DEBUG_ASSERT(k == B.cols(), "matrices to multiply have no fit dimensions"); AKANTU_DEBUG_ASSERT(this->cols() == B.rows(), "matrices to multiply have no fit dimensions"); } else { AKANTU_DEBUG_ASSERT(k == B.rows(), "matrices to multiply have no fit dimensions"); AKANTU_DEBUG_ASSERT(this->cols() == B.cols(), "matrices to multiply have no fit dimensions"); } if (tr_A) { AKANTU_DEBUG_ASSERT(this->rows() == A.cols(), "matrices to multiply have no fit dimensions"); } else { AKANTU_DEBUG_ASSERT(this->rows() == A.rows(), "matrices to multiply have no fit dimensions"); } #endif // AKANTU_NDEBUG Math::matMul(this->rows(), this->cols(), k, alpha, A.storage(), B.storage(), 0., this->storage()); } /* ---------------------------------------------------------------------- */ /** Tensor outer product @param v1 vector @param v2 vector @return Matrix \f$M_{ij} = v^1_i v^2_j\f$ */ inline void outerProduct(const Vector & A, const Vector & B) { AKANTU_DEBUG_ASSERT( A.size() == this->rows() && B.size() == this->cols(), "A and B are not compatible with the size of the matrix"); for (UInt i = 0; i < this->rows(); ++i) { for (UInt j = 0; j < this->cols(); ++j) { this->values[i + j * this->rows()] += A[i] * B[j]; } } } /* ---------------------------------------------------------------------- */ /** From a vector, construct the matching skew matrix @param v Matrix 3x1(vector) @return Matrix 3x3 */ inline void skew(const Vector &v) { AKANTU_DEBUG_ASSERT( v.size() == 3 && this->rows() == 3 && this->cols() == 3, "v and the matrix are not in dimension 3"); (*this)(0)=Vector {0, v(2), -v(1)}; (*this)(1)=Vector {-v(2), 0, v(0)}; (*this)(2)=Vector {v(1), -v(0), 0}; } /* ---------------------------------------------------------------------- */ /** To implement the functions: - skew - expMap - expDerivative - expAcceleration - computeRotationAngle - skew2vec - logMap - convectedAngleDerivative - angleAcceleration - composeRotations in 2D see: Lie Groups for 2D and 3D Transformations https://ethaneade.com/lie.pdf https://www.cis.upenn.edu/~cis610/geombchap14.pdf */ inline void expMap(const Vector &v) { AKANTU_DEBUG_ASSERT(v.size()==3 && this->rows() ==3 && this->cols() == 3, "only for dimension 3 vector and matrix"); auto theta = v.norm(); if (Math::are_float_equal(theta, 0.)) { (*this) = Matrix::eye(3, 1.); } Matrix K(3,3); K.skew(v); K /= theta; (*this) = Matrix::eye(3, 1.) + std::sin(theta) * K + (1 - std::cos(theta)) * K * K; } /* ---------------------------------------------------------------------- */ inline Real computeRotationAngle() const { AKANTU_DEBUG_ASSERT(this->rows() == this->cols() == 3, "need a 3 dim matrix"); auto cos_val = (this->trace() -1) / 2; if (cos_val < -1) { cos_val = -1; } if (cos_val > 1) { cos_val = 1; } Real theta = std::acos(cos_val); if(std::isnan(theta)) { throw std::runtime_error( "in angle calculation theta is not a number: abort"); } return theta; } /* ---------------------------------------------------------------------- */ /** The purpose of this function is to compute the derivative of a rotation function of the kind : \f[ \frac{d}{dt} \Lambda(t) \f] with \f$\Lambda^T(t) = exp(\theta(t))\f$. The implementation is based on Rodrigues's formula. @param v the provided rotation vector \f$\theta(t)\f$ @param vp the provided rotation vector derivative \f$\frac{d}{dt} \theta(t)\f$ */ inline void expDerivative(const Vector &v, const Vector &vp) { AKANTU_DEBUG_ASSERT(v.size()==3 && vp.size() == 3 && this->rows() ==3 && this->cols() == 3, "only for dimension 3 vectors and matrix"); Real f; Real h; Real fp; Real hp; Real theta = v.norm(); if (theta < 1e-12) { f = 1.; h = 0.5; fp = 0.; hp = 0.; } else { Real thetap = v.dot(vp) / theta; f = std::sin(theta) / theta; fp = (theta * std::cos(theta) - std::sin(theta)) * thetap / (theta * theta); hp = (theta * std::sin(theta) + 2 * std::cos(theta) - 2.) * thetap / (theta * theta * theta); } Matrix _B(3,3); Matrix _Bp(3,3); _B.skew(v); _Bp.skew(vp); (*this) = fp * _B + hp * _B * _B + f * _Bp + h * (_Bp * _B + _B * _Bp); } /* ---------------------------------------------------------------------- */ inline void expAcceleration(const Vector &B, const Vector &Bp, const Vector &Bpp) { AKANTU_DEBUG_ASSERT(B.size()==3 && Bp.size() == 3 && Bpp.size() == 3 && this->rows() ==3 && this->cols() == 3, "only for dimension 3 vectors and matrix"); auto theta = B.norm(); auto s = std::sin(theta); auto c = std::cos(theta); Real f; Real h; Real fp; Real hp; Real hpp; Real fpp; if (theta == 0) { f = 1; h = 0.5; fp = 0; hp = 0; hpp = 0; fpp = 0; } else { auto thetap = B.dot(Bp) / theta; auto thetapp = -std::pow(B.dot(Bp), 2) / std::pow(theta, 3) + (Bp.dot(Bp) + B.dot(Bpp)) / theta; f = s / theta; h = (1 - c) / std::pow(theta, 2); fp = (theta * c - s) * thetap / std::pow(theta, 2); hp = (theta * s + 2 * c - 2) * thetap / std::pow(theta, 3); fpp = 1 / std::pow(theta, 3) * (std::pow(theta, 2) * (thetapp * c - s * std::pow(thetap, 2)) - theta * (s * thetapp + 2 * c * std::pow(thetap, 2)) + 2 * s * std::pow(thetap, 2)); hpp = 1 / std::pow(theta, 4) * (std::pow(theta, 2) * (s * thetapp + c * std::pow(thetap, 2)) + 2 * theta * ((c - 1) * thetapp - 2 * s * std::pow(thetap, 2)) - 6 * (c - 1) * std::pow(thetap, 2)); } Matrix Bhat(3,3); Matrix Bphat(3,3); Matrix Bpphat(3,3); Bhat.skew(B); Bphat.skew(Bp); Bpphat.skew(Bpp); (*this) = fpp * Bhat + hpp * Bhat * Bhat; (*this) += 2 * fp * Bphat + 2 * hp * (Bhat * Bphat + Bphat * Bhat); (*this) += f * Bpphat + h * (Bphat * Bphat + Bphat * Bphat + Bhat * Bpphat + Bpphat * Bhat); } /* ---------------------------------------------------------------------- */ private: class EigenSorter { public: EigenSorter(const Vector & eigs) : eigs(eigs) {} bool operator()(const UInt & a, const UInt & b) const { return (eigs(a) > eigs(b)); } private: const Vector & eigs; }; public: /* ---------------------------------------------------------------------- */ inline void eig(Vector & eigenvalues, Matrix & eigenvectors, bool sort = true) const { AKANTU_DEBUG_ASSERT(this->cols() == this->rows(), "eig is not a valid operation on a rectangular matrix"); AKANTU_DEBUG_ASSERT(eigenvalues.size() == this->cols(), "eigenvalues should be of size " << this->cols() << "."); #ifndef AKANTU_NDEBUG if (eigenvectors.storage() != nullptr) { AKANTU_DEBUG_ASSERT((eigenvectors.rows() == eigenvectors.cols()) && (eigenvectors.rows() == this->cols()), "Eigenvectors needs to be a square matrix of size " << this->cols() << " x " << this->cols() << "."); } #endif Matrix tmp = *this; Vector tmp_eigs(eigenvalues.size()); Matrix tmp_eig_vects(eigenvectors.rows(), eigenvectors.cols()); if (tmp_eig_vects.rows() == 0 || tmp_eig_vects.cols() == 0) { Math::matrixEig(tmp.cols(), tmp.storage(), tmp_eigs.storage()); } else { Math::matrixEig(tmp.cols(), tmp.storage(), tmp_eigs.storage(), tmp_eig_vects.storage()); } if (not sort) { eigenvalues = tmp_eigs; eigenvectors = tmp_eig_vects; return; } Vector perm(eigenvalues.size()); for (UInt i = 0; i < perm.size(); ++i) { perm(i) = i; } std::sort(perm.storage(), perm.storage() + perm.size(), EigenSorter(tmp_eigs)); for (UInt i = 0; i < perm.size(); ++i) { eigenvalues(i) = tmp_eigs(perm(i)); } if (tmp_eig_vects.rows() != 0 && tmp_eig_vects.cols() != 0) { for (UInt i = 0; i < perm.size(); ++i) { for (UInt j = 0; j < eigenvectors.rows(); ++j) { eigenvectors(j, i) = tmp_eig_vects(j, perm(i)); } } } } /* ---------------------------------------------------------------------- */ inline void eig(Vector & eigenvalues) const { Matrix empty; eig(eigenvalues, empty); } /* ---------------------------------------------------------------------- */ inline void eye(T alpha = 1.) { AKANTU_DEBUG_ASSERT(this->cols() == this->rows(), "eye is not a valid operation on a rectangular matrix"); this->zero(); for (UInt i = 0; i < this->cols(); ++i) { this->values[i + i * this->rows()] = alpha; } } /* ---------------------------------------------------------------------- */ static inline Matrix eye(UInt m, T alpha = 1.) { Matrix tmp(m, m); tmp.eye(alpha); return tmp; } /* ---------------------------------------------------------------------- */ inline T trace() const { AKANTU_DEBUG_ASSERT( this->cols() == this->rows(), "trace is not a valid operation on a rectangular matrix"); T trace = 0.; for (UInt i = 0; i < this->rows(); ++i) { trace += this->values[i + i * this->rows()]; } return trace; } /* ---------------------------------------------------------------------- */ inline Matrix transpose() const { Matrix tmp(this->cols(), this->rows()); for (UInt i = 0; i < this->rows(); ++i) { for (UInt j = 0; j < this->cols(); ++j) { tmp(j, i) = operator()(i, j); } } return tmp; } /* ---------------------------------------------------------------------- */ inline void inverse(const Matrix & A) { AKANTU_DEBUG_ASSERT(A.cols() == A.rows(), "inv is not a valid operation on a rectangular matrix"); AKANTU_DEBUG_ASSERT(this->cols() == A.cols(), "the matrix should have the same size as its inverse"); if (this->cols() == 1) { *this->values = 1. / *A.storage(); } else if (this->cols() == 2) { Math::inv2(A.storage(), this->values); } else if (this->cols() == 3) { Math::inv3(A.storage(), this->values); } else { Math::inv(this->cols(), A.storage(), this->values); } } inline Matrix inverse() { Matrix inv(this->rows(), this->cols()); inv.inverse(*this); return inv; } /* --------------------------------------------------------------------- */ inline T det() const { AKANTU_DEBUG_ASSERT(this->cols() == this->rows(), "inv is not a valid operation on a rectangular matrix"); if (this->cols() == 1) { return *(this->values); } if (this->cols() == 2) { return Math::det2(this->values); } if (this->cols() == 3) { return Math::det3(this->values); } return Math::det(this->cols(), this->values); } /* --------------------------------------------------------------------- */ inline T doubleDot(const Matrix & other) const { AKANTU_DEBUG_ASSERT( this->cols() == this->rows(), "doubleDot is not a valid operation on a rectangular matrix"); if (this->cols() == 1) { return *(this->values) * *(other.storage()); } if (this->cols() == 2) { return Math::matrixDoubleDot22(this->values, other.storage()); } if (this->cols() == 3) { return Math::matrixDoubleDot33(this->values, other.storage()); } AKANTU_ERROR("doubleDot is not defined for other spatial dimensions" << " than 1, 2 or 3."); } /* ---------------------------------------------------------------------- */ /// function to print the containt of the class virtual void printself(std::ostream & stream, int indent = 0) const { std::string space; for (Int i = 0; i < indent; i++, space += AKANTU_INDENT) { ; } stream << "["; for (UInt i = 0; i < this->n[0]; ++i) { if (i != 0) { stream << ", "; } stream << "["; for (UInt j = 0; j < this->n[1]; ++j) { if (j != 0) { stream << ", "; } stream << operator()(i, j); } stream << "]"; } stream << "]"; }; }; /* ------------------------------------------------------------------------ */ template template inline void Vector::mul(const Matrix & A, const Vector & x, T alpha) { #ifndef AKANTU_NDEBUG UInt n = x.size(); if (tr_A) { AKANTU_DEBUG_ASSERT(n == A.rows(), "matrix and vector to multiply have no fit dimensions"); AKANTU_DEBUG_ASSERT(this->size() == A.cols(), "matrix and vector to multiply have no fit dimensions"); } else { AKANTU_DEBUG_ASSERT(n == A.cols(), "matrix and vector to multiply have no fit dimensions"); AKANTU_DEBUG_ASSERT(this->size() == A.rows(), "matrix and vector to multiply have no fit dimensions"); } #endif Math::matVectMul(A.rows(), A.cols(), alpha, A.storage(), x.storage(), 0., this->storage()); } /* -------------------------------------------------------------------------- */ template inline std::ostream & operator<<(std::ostream & stream, const Matrix & _this) { _this.printself(stream); return stream; } /* -------------------------------------------------------------------------- */ template inline std::ostream & operator<<(std::ostream & stream, const Vector & _this) { _this.printself(stream); return stream; } /* ------------------------------------------------------------------------ */ /* Tensor3 */ /* ------------------------------------------------------------------------ */ template class Tensor3 : public TensorStorage> { using parent = TensorStorage>; public: using value_type = typename parent::value_type; using proxy = Tensor3Proxy; public: Tensor3() : parent(){}; Tensor3(UInt m, UInt n, UInt p, const T & def = T()) : parent(m, n, p, def) {} Tensor3(T * data, UInt m, UInt n, UInt p) : parent(data, m, n, p) {} Tensor3(const Tensor3 & src, bool deep_copy = true) : parent(src, deep_copy) {} Tensor3(const proxy & src) : parent(src) {} public: /* ------------------------------------------------------------------------ */ inline Tensor3 & operator=(const Tensor3 & src) { parent::operator=(src); return *this; } /* ---------------------------------------------------------------------- */ inline T & operator()(UInt i, UInt j, UInt k) { AKANTU_DEBUG_ASSERT( (i < this->n[0]) && (j < this->n[1]) && (k < this->n[2]), "Access out of the tensor3! " << "You are trying to access the element " << "(" << i << ", " << j << ", " << k << ") in a tensor of size (" << this->n[0] << ", " << this->n[1] << ", " << this->n[2] << ")"); return *(this->values + (k * this->n[0] + i) * this->n[1] + j); } inline const T & operator()(UInt i, UInt j, UInt k) const { AKANTU_DEBUG_ASSERT( (i < this->n[0]) && (j < this->n[1]) && (k < this->n[2]), "Access out of the tensor3! " << "You are trying to access the element " << "(" << i << ", " << j << ", " << k << ") in a tensor of size (" << this->n[0] << ", " << this->n[1] << ", " << this->n[2] << ")"); return *(this->values + (k * this->n[0] + i) * this->n[1] + j); } inline MatrixProxy operator()(UInt k) { AKANTU_DEBUG_ASSERT((k < this->n[2]), "Access out of the tensor3! " << "You are trying to access the slice " << k << " in a tensor3 of size (" << this->n[0] << ", " << this->n[1] << ", " << this->n[2] << ")"); return MatrixProxy(this->values + k * this->n[0] * this->n[1], this->n[0], this->n[1]); } inline MatrixProxy operator()(UInt k) const { AKANTU_DEBUG_ASSERT((k < this->n[2]), "Access out of the tensor3! " << "You are trying to access the slice " << k << " in a tensor3 of size (" << this->n[0] << ", " << this->n[1] << ", " << this->n[2] << ")"); return MatrixProxy(this->values + k * this->n[0] * this->n[1], this->n[0], this->n[1]); } inline MatrixProxy operator[](UInt k) { return MatrixProxy(this->values + k * this->n[0] * this->n[1], this->n[0], this->n[1]); } inline MatrixProxy operator[](UInt k) const { return MatrixProxy(this->values + k * this->n[0] * this->n[1], this->n[0], this->n[1]); } }; /* -------------------------------------------------------------------------- */ // support operations for the creation of other vectors /* -------------------------------------------------------------------------- */ template Vector operator*(const T & scalar, const Vector & a) { Vector r(a); r *= scalar; return r; } template Vector operator*(const Vector & a, const T & scalar) { Vector r(a); r *= scalar; return r; } template Vector operator/(const Vector & a, const T & scalar) { Vector r(a); r /= scalar; return r; } template Vector operator*(const Vector & a, const Vector & b) { Vector r(a); r *= b; return r; } template Vector operator+(const Vector & a, const Vector & b) { Vector r(a); r += b; return r; } template Vector operator-(const Vector & a, const Vector & b) { Vector r(a); r -= b; return r; } template Vector operator*(const Matrix & A, const Vector & b) { Vector r(b.size()); r.template mul(A, b); return r; } template Vector skew2vec(const Matrix & mat){ Vector v(3); v.skew2vec(mat); return v; } template Vector logMap(const Matrix & mat){ Vector v(3); v.logMap(mat); return v; } template Vector convectedAngleDerivative(const Vector & v1, const Vector & v2){ Vector v(3); v.convectedAngleDerivative(v1, v2); return v; } template Vector angleAcceleration(const Vector & v1, const Vector & v2, const Vector &v3){ Vector v(3); v.angleAcceleration(v1, v2, v3); return v; } template Vector composeRotations(const Vector & v1, const Vector & v2){ Vector v(3); v.composeRotations(v1, v2); return v; } /* -------------------------------------------------------------------------- */ template Matrix operator*(const T & scalar, const Matrix & a) { Matrix r(a); r *= scalar; return r; } template Matrix operator*(const Matrix & a, const T & scalar) { Matrix r(a); r *= scalar; return r; } template Matrix operator/(const Matrix & a, const T & scalar) { Matrix r(a); r /= scalar; return r; } template Matrix operator+(const Matrix & a, const Matrix & b) { Matrix r(a); r += b; return r; } template Matrix operator-(const Matrix & a, const Matrix & b) { Matrix r(a); r -= b; return r; } +template +Matrix outerProduct(const Vector & v1, const Vector & v2){ + Matrix s(v1.size(), v2.size()); + s.outerProduct(v1,v2); + return s; +} + template Matrix skew(const Vector & v){ Matrix s(3,3); s.skew(v); return s; } template Matrix expMap(const Vector & v){ Matrix s(3,3); s.expMap(v); return s; } template Matrix expDerivative(const Vector & v1, const Vector & v2){ Matrix s(3,3); s.expDerivative(v1, v2); return s; } template Matrix expAcceleration(const Vector & v1, const Vector & v2, const Vector & v3){ Matrix s(3,3); s.expAcceleration(v1, v2, v3); return s; } template Real computeRotationAngle(const Matrix &mat){ return mat.computeRotationAngle(); } } // namespace akantu #include namespace std { template struct iterator_traits<::akantu::types::details::vector_iterator> { protected: using iterator = ::akantu::types::details::vector_iterator; public: using iterator_category = typename iterator::iterator_category; using value_type = typename iterator::value_type; using difference_type = typename iterator::difference_type; using pointer = typename iterator::pointer; using reference = typename iterator::reference; }; template struct iterator_traits<::akantu::types::details::column_iterator> { protected: using iterator = ::akantu::types::details::column_iterator; public: using iterator_category = typename iterator::iterator_category; using value_type = typename iterator::value_type; using difference_type = typename iterator::difference_type; using pointer = typename iterator::pointer; using reference = typename iterator::reference; }; } // namespace std #endif /* AKANTU_AKA_TYPES_HH_ */ diff --git a/src/model/nonlinear_beam/nonlinear_beam_model.cc b/src/model/nonlinear_beam/nonlinear_beam_model.cc index a6cf47b77..31fa22f66 100644 --- a/src/model/nonlinear_beam/nonlinear_beam_model.cc +++ b/src/model/nonlinear_beam/nonlinear_beam_model.cc @@ -1,593 +1,825 @@ #include "nonlinear_beam_model.hh" #include "dumpable_inline_impl.hh" #include "element_synchronizer.hh" #include "fe_engine_template.hh" #include "generalized_trapezoidal.hh" #include "group_manager_inline_impl.hh" #include "integrator_gauss.hh" #include "mesh.hh" #include "parser.hh" #include "shape_lagrange.hh" #ifdef AKANTU_USE_IOHELPER #include "dumper_element_partition.hh" #include "dumper_elemental_field.hh" #include "dumper_internal_material_field.hh" #include "dumper_iohelper_paraview.hh" #endif /* -------------------------------------------------------------------------- */ namespace akantu { /* -------------------------------------------------------------------------- */ NonlinearBeamModel::NonlinearBeamModel(Mesh & mesh, UInt dim, const ID & id, std::shared_ptr dof_manager) - : Model(mesh, model_type, dof_manager, dim, id),Ns(0,2*spatial_dimension*2*spatial_dimension*Mesh::getNbNodesPerElement(_segment_3), "shape function matrix"), dNs(0,2*spatial_dimension*2*spatial_dimension*Mesh::getNbNodesPerElement(_segment_3), "derivative shape function matrix") { + : Model(mesh, model_type, dof_manager, dim, id),Ns(0,2*spatial_dimension*2*spatial_dimension*Mesh::getNbNodesPerElement(_segment_3), "shape function matrix"), dNs(0,2*spatial_dimension*2*spatial_dimension*Mesh::getNbNodesPerElement(_segment_3), "derivative shape function matrix"), M(0,2*spatial_dimension*2*spatial_dimension*Mesh::getNbNodesPerElement(_segment_3)*Mesh::getNbNodesPerElement(_segment_3), "Mass matrix") { AKANTU_DEBUG_IN(); this->registerDataAccessor(*this); auto nb_element = mesh.getNbElement(_segment_3); if (nb_element ==0){ AKANTU_EXCEPTION("Model only works with _segment_3 elements"); } this->type = _segment_3; /* if (this->mesh.isDistributed()) { auto & synchronizer = this->mesh.getElementSynchronizer(); /// Syncronisation this->registerSynchronizer(synchronizer, SynchronizationTag::_htm_temperature); this->registerSynchronizer(synchronizer, SynchronizationTag::_htm_gradient_temperature); } */ registerFEEngineObject(id + ":fem", mesh, 1); #ifdef AKANTU_USE_IOHELPER this->mesh.registerDumper("nonlinear_beam", id, true); this->mesh.addDumpMesh(mesh, spatial_dimension, _not_ghost, _ek_regular); #endif this->registerParam("E", E, _pat_parsmod); this->registerParam("nu", nu, _pat_parsmod); this->registerParam("A", A, _pat_parsmod); this->registerParam("J_11", J_11, _pat_parsmod); this->registerParam("J_22", J_22, _pat_parsmod); this->registerParam("J_33", J_33, _pat_parsmod); this->registerParam("J_12", J_12, _pat_parsmod); this->registerParam("J_13", J_13, _pat_parsmod); this->registerParam("J_23", J_23, _pat_parsmod); + this->registerParam("J_0", J_0, _pat_parsmod); this->registerParam("density", density, _pat_parsmod); AKANTU_DEBUG_OUT(); } /* -------------------------------------------------------------------------- */ NonlinearBeamModel::~NonlinearBeamModel() = default; /* -------------------------------------------------------------------------- */ void NonlinearBeamModel::setTimeStep(Real time_step, const ID & solver_id) { Model::setTimeStep(time_step, solver_id); #if defined(AKANTU_USE_IOHELPER) this->mesh.getDumper().setTimeStep(time_step); #endif } /* -------------------------------------------------------------------------- */ /* Initialization */ /* -------------------------------------------------------------------------- */ /* -------------------------------------------------------------------------- */ void NonlinearBeamModel::initFullImpl(const ModelOptions & options) { Model::initFullImpl(options); readMaterials(); } /* -------------------------------------------------------------------------- */ void NonlinearBeamModel::initModel() { auto & fem = this->getFEEngine(); fem.initShapeFunctions(_not_ghost); fem.initShapeFunctions(_ghost); Ns.resize(mesh.getNbElement(type) * fem.getNbIntegrationPoints(type)); dNs.resize(mesh.getNbElement(type) * fem.getNbIntegrationPoints(type)); + M.resize(mesh.getNbElement(type) * fem.getNbIntegrationPoints(type)); this->N_matrix(Ns); this->N_grad_matrix(dNs); } /* -------------------------------------------------------------------------- */ TimeStepSolverType NonlinearBeamModel::getDefaultSolverType() const { return TimeStepSolverType::_dynamic_lumped; } /* -------------------------------------------------------------------------- */ ModelSolverOptions NonlinearBeamModel::getDefaultSolverOptions( const TimeStepSolverType & type) const { ModelSolverOptions options; switch (type) { case TimeStepSolverType::_dynamic_lumped: { options.non_linear_solver_type = NonLinearSolverType::_lumped; options.integration_scheme_type["linear_angular_displacement"] = IntegrationSchemeType::_central_difference; options.solution_type["linear_angular_displacement"] = IntegrationScheme::_acceleration; break; } case TimeStepSolverType::_static: { options.non_linear_solver_type = NonLinearSolverType::_newton_raphson; options.integration_scheme_type["linear_angular_displacement"] = IntegrationSchemeType::_pseudo_time; options.solution_type["linear_angular_displacement"] = IntegrationScheme::_not_defined; break; } default: AKANTU_EXCEPTION(type << " is not a valid time step solver type"); } return options; } /* -------------------------------------------------------------------------- */ std::tuple NonlinearBeamModel::getDefaultSolverID(const AnalysisMethod & method) { switch (method) { case _explicit_lumped_mass: { return std::make_tuple("explicit_lumped", TimeStepSolverType::_dynamic_lumped); } case _static: { return std::make_tuple("static", TimeStepSolverType::_static); } default: return std::make_tuple("unknown", TimeStepSolverType::_not_defined); } } /* -------------------------------------------------------------------------- */ void NonlinearBeamModel::initSolver(TimeStepSolverType time_step_solver_type, NonLinearSolverType /*unused*/) { auto & dof_manager = this->getDOFManager(); // for alloc type of solvers this->allocNodalField(this->linear_angular_displacement, 2*spatial_dimension, "linear_angular_displacement"); // this->allocNodalField(this->internal_force_torque, 2*spatial_dimension, "internal_force_torque"); + this->allocNodalField(this->inertial_force_torque, 2*spatial_dimension, + "inertial_force_torque"); // this->allocNodalField(this->external_force_torque, 2*spatial_dimension, "external_force_torque"); // this->allocNodalField(this->blocked_dofs, 2*spatial_dimension, "blocked_dofs"); // this->allocNodalField(this->initial_angle, spatial_dimension, "initial_angle"); /* ------------------------------------------------------------------------ */ if (!dof_manager.hasDOFs("linear_angular_displacement")) { dof_manager.registerDOFs("linear_angular_displacement", *this->linear_angular_displacement, _dst_nodal); dof_manager.registerBlockedDOFs("linear_angular_displacement", *this->blocked_dofs); } /* ------------------------------------------------------------------------ */ // for dynamic if (time_step_solver_type == TimeStepSolverType::_dynamic_lumped) { this->allocNodalField(this->linear_angular_velocity, 2*spatial_dimension, "linear_angular_velocity"); // this->allocNodalField(this->linear_angular_acceleration, 2*spatial_dimension, "linear_angular_acceleration"); if (!dof_manager.hasDOFsDerivatives("linear_angular_displacement", 1)) { dof_manager.registerDOFsDerivative("linear_angular_displacement", 1, *this->linear_angular_velocity); dof_manager.registerDOFsDerivative("linear_angular_displacement", 2, *this->linear_angular_acceleration); } } } /* -------------------------------------------------------------------------- */ void NonlinearBeamModel::assembleResidual() { AKANTU_DEBUG_IN(); // computes the internal forces this->assembleInternalForces(); + this->computeInertialForces(); this->getDOFManager().assembleToResidual("displacement", *this->external_force_torque, 1); this->getDOFManager().assembleToResidual("displacement", - *this->internal_force_torque, 1); + *this->internal_force_torque, -1); + this->getDOFManager().assembleToResidual("displacement", + *this->inertial_force_torque, -1); AKANTU_DEBUG_OUT(); } MatrixType NonlinearBeamModel::getMatrixType(const ID & matrix_id) const { // \TODO check the materials to know what is the correct answer /* if (matrix_id == "C") { return _mt_not_defined; } if (matrix_id == "K") { auto matrix_type = _unsymmetric; for (auto & material : materials) { matrix_type = std::max(matrix_type, material->getMatrixType(matrix_id)); } } return _symmetric; */ } /* -------------------------------------------------------------------------- */ void NonlinearBeamModel::assembleMatrix(const ID & matrix_id) { if (matrix_id == "K") { - this->assembleStiffnessMatrix(); + //this->assembleStiffnessMatrix(); } else if (matrix_id == "M") { this->assembleMass(); } } /* -------------------------------------------------------------------------- */ void NonlinearBeamModel::assembleLumpedMatrix(const ID & matrix_id) { if (matrix_id == "M") { this->assembleMassLumped(); } } /* -------------------------------------------------------------------------- */ void NonlinearBeamModel::N_matrix(Array &Ns) { AKANTU_DEBUG_IN(); Ns.set(0.); UInt nb_nodes_per_element = Mesh::getNbNodesPerElement(type); const auto & shape_vector = getFEEngine().getShapes(type); for(auto && data : zip(make_view(shape_vector, nb_nodes_per_element), make_view(Ns, 2* spatial_dimension, 2*spatial_dimension*nb_nodes_per_element))) { auto & shapes = std::get<0>(data); auto & N = std::get<1>(data); for (UInt nd=0; nd < nb_nodes_per_element; ++nd) { N.block(Matrix::eye(6,1.) * shapes(nd),0, nd * 2*spatial_dimension); } } AKANTU_DEBUG_OUT(); } void NonlinearBeamModel::N_grad_matrix(Array &dNs) { AKANTU_DEBUG_IN(); dNs.set(0.); UInt nb_nodes_per_element = Mesh::getNbNodesPerElement(type); const auto & derivative_shape_vector = getFEEngine().getShapesDerivatives(type); for(auto && data : zip(make_view(derivative_shape_vector, nb_nodes_per_element), make_view(dNs, 2* spatial_dimension, 2*spatial_dimension*nb_nodes_per_element))) { auto & deri_shapes = std::get<0>(data); auto & dN = std::get<1>(data); for (UInt nd=0; nd < nb_nodes_per_element; ++nd) { dN.block(Matrix::eye(6,1.) * deri_shapes(nd),0, nd * 2*spatial_dimension); } } AKANTU_DEBUG_OUT(); } void NonlinearBeamModel::N_rotator_matrix(Array & N_rot_mat) { AKANTU_DEBUG_IN(); UInt nb_nodes_per_element = Mesh::getNbNodesPerElement(type); const auto & derivative_shape_vector = getFEEngine().getShapesDerivatives(type); const auto & shape_vector = getFEEngine().getShapes(type); UInt dim = spatial_dimension; //Array N_rot_mat(derivative_shape_vector.size(),2*spatial_dimension*2*spatial_dimension*nb_nodes_per_element); N_rot_mat.set(0.); Vector phip(3); auto conn_it = make_view(mesh.getConnectivity(type), nb_nodes_per_element).begin(); auto disp_it = make_view(*(this->linear_angular_displacement), dim, 2).begin(); auto init_pos_it = make_view(mesh.getNodes(), dim).begin(); auto nb_quad_points = getFEEngine().getNbIntegrationPoints(type); for(auto && data : enumerate(make_view(derivative_shape_vector, nb_nodes_per_element), make_view(shape_vector, nb_nodes_per_element), make_view(N_rot_mat, 2* spatial_dimension, 2*spatial_dimension*nb_nodes_per_element))) { auto & deri_shapes = std::get<1>(data); auto & shapes = std::get<2>(data); auto & N_rot = std::get<3>(data); auto elem = std::get<0>(data)/nb_quad_points; Vector conn = conn_it[elem]; phip.set(0.); for (UInt nd = 0; nd disp = Matrix (disp_it[conn(nd)])(0); Vector init = init_pos_it[conn(nd)]; phip += (disp + init) * deri_shapes(nd); } Matrix phip_mat = skew(phip); for (UInt nd=0; nd < nb_nodes_per_element; ++nd) { N_rot.block(phip_mat * shapes(nd), 0, nd * 2*spatial_dimension + 3); } } AKANTU_DEBUG_OUT(); } void NonlinearBeamModel::B_matrix(Array B_mat) { AKANTU_DEBUG_IN(); UInt nb_node_per_element = Mesh::getNbNodesPerElement(type); const auto & derivative_shape_vector = getFEEngine().getShapesDerivatives(type); Array L(derivative_shape_vector.size(), spatial_dimension*nb_node_per_element); Array N_rot_mat(derivative_shape_vector.size(),2*spatial_dimension*2*spatial_dimension*nb_node_per_element); this->get_rotation_matrix(L); this->N_rotator_matrix(N_rot_mat); Matrix Rot(6,6); for (auto && data : zip(make_view(L, spatial_dimension, nb_node_per_element), make_view(N_rot_mat, 2* spatial_dimension, 2*spatial_dimension*nb_node_per_element), make_view(dNs, 2* spatial_dimension, 2*spatial_dimension*nb_node_per_element), make_view(B_mat, 2* spatial_dimension, 2*spatial_dimension*nb_node_per_element))) { auto & LL = std::get<0>(data); auto & N_rot = std::get<1>(data); auto & dN = std::get<2>(data); auto & B = std::get<3>(data); Rot.set(0.); Rot.block(LL, 0, 0); Rot.block(LL, 3, 3); B = Rot.transpose() * (dN + N_rot); } AKANTU_DEBUG_OUT(); } void NonlinearBeamModel::get_rotation_matrix(Array L, bool origin) { AKANTU_DEBUG_IN(); const auto & shape_vector = getFEEngine().getShapes(type); UInt nb_node_per_element = Mesh::getNbNodesPerElement(type); Array inter_angle(shape_vector.size(), nb_node_per_element); Array inter_initAngle(shape_vector.size(), nb_node_per_element); Array angle(this->linear_angular_displacement->size(), spatial_dimension); for (auto && data : zip(make_view(angle, spatial_dimension), make_view(*(this->linear_angular_displacement), spatial_dimension, 2))) { std::get<0>(data) = std::get<1>(data)(1); } this->interpolate(angle, inter_angle); this->interpolate(*(this->initial_angle), inter_initAngle); for (auto && data : zip(make_view(L, spatial_dimension,nb_node_per_element), make_view(inter_angle, spatial_dimension), make_view(inter_initAngle, spatial_dimension))) { auto & LL = std::get<0>(data); ; LL = expMap(std::get<2>(data)); if (not origin) { LL = expMap(std::get<1>(data)) * LL; } } AKANTU_DEBUG_OUT(); } void NonlinearBeamModel::interpolate(Array &field, Array &interField) { AKANTU_DEBUG_IN(); const auto & shape_vector = getFEEngine().getShapes(type); UInt nb_node_per_element = Mesh::getNbNodesPerElement(type); auto conn_it = make_view(mesh.getConnectivity(type), nb_node_per_element).begin(); auto field_it = make_view(field, spatial_dimension, 1).begin(); auto nb_quad_points = getFEEngine().getNbIntegrationPoints(type); for (auto && data : enumerate(make_view(shape_vector, nb_node_per_element), make_view(interField, nb_node_per_element))) { auto & N = std::get<1>(data); auto & intF = std::get<2>(data); auto elem = std::get<0>(data)/nb_quad_points; Vector conn = conn_it[elem]; for (UInt nd = 0; nd F = Matrix (field_it[conn(nd)])(0); intF += F * N(nd); } } AKANTU_DEBUG_OUT(); } void NonlinearBeamModel::grad_interpolate(Array &field, Array &gradinterField) { AKANTU_DEBUG_IN(); const auto & derivative_shape_vector = getFEEngine().getShapesDerivatives(type); UInt nb_node_per_element = Mesh::getNbNodesPerElement(type); auto conn_it = make_view(mesh.getConnectivity(type), nb_node_per_element).begin(); auto field_it = make_view(field, spatial_dimension, 1).begin(); auto nb_quad_points = getFEEngine().getNbIntegrationPoints(type); for (auto && data : enumerate(make_view(derivative_shape_vector, nb_node_per_element), make_view(gradinterField, nb_node_per_element))) { auto & dN = std::get<1>(data); auto & gintF = std::get<2>(data); auto elem = std::get<0>(data)/nb_quad_points; Vector conn = conn_it[elem]; for (UInt nd = 0; nd F = Matrix (field_it[conn(nd)])(0); gintF += F * dN(nd); } } AKANTU_DEBUG_OUT(); } void NonlinearBeamModel::computeStrains(Array strains, bool origin) { AKANTU_DEBUG_IN(); UInt nb_node_per_element = Mesh::getNbNodesPerElement(type); const auto & shape_vector = getFEEngine().getShapes(type); Array pos(this->linear_angular_displacement->size(), spatial_dimension); for (auto && data : zip(make_view(pos, spatial_dimension), make_view(mesh.getNodes(), spatial_dimension), make_view(*(this->linear_angular_displacement), spatial_dimension, 2))) { auto P = std::get<0>(data); auto init_P = std::get<1>(data); auto lin_disp = std::get<2>(data)(0); P = init_P; if (not origin) { P = init_P + Vector (lin_disp); } } Array L(shape_vector.size(), spatial_dimension*nb_node_per_element); Array L0(shape_vector.size(), spatial_dimension*nb_node_per_element); this->get_rotation_matrix(L, origin); this->get_rotation_matrix(L0, true); Array theta(this->linear_angular_displacement->size(), spatial_dimension); for (auto && data : zip(make_view(theta, spatial_dimension), make_view(*(this->linear_angular_displacement), spatial_dimension, 2))) { std::get<0>(data) = std::get<1>(data)(1); } Array Gamma(shape_vector.size(), spatial_dimension); Array B(shape_vector.size(), spatial_dimension); Array Bp(shape_vector.size(), spatial_dimension); this->grad_interpolate(pos, Gamma); this->interpolate(theta, B); this->grad_interpolate(theta, Bp); Matrix Omega_hat(3,3); Vector e3 = {0., 0., 1.}; for (auto && data : zip(make_view(L,spatial_dimension,nb_node_per_element), make_view(L0, spatial_dimension,nb_node_per_element), make_view(Gamma, nb_node_per_element), make_view(B, nb_node_per_element), make_view(Bp, nb_node_per_element), make_view(strains, spatial_dimension, 2))) { auto & LL = std::get<0>(data); auto & LL0 = std::get<1>(data); auto & Gam = std::get<2>(data); auto & BB = std::get<3>(data); auto & BBp = std::get<4>(data); auto & strain = std::get<5>(data); Omega_hat = LL.transpose() * expDerivative(BB, BBp) * LL0; strain(1) = skew2vec(Omega_hat); strain(0) = LL.transpose() * Gam - e3; } AKANTU_DEBUG_OUT(); } -void NonlinearBeamModel::ConstitutiveC() { +void NonlinearBeamModel::constitutive_C(Array C) { + AKANTU_DEBUG_IN(); + Real E = this->E; + Real G = this->E/(2*(1+this->nu)); + Real A = this->A; + Real J_22 = this->J_22; + Real J_12 = this->J_12; + Real J_11 = this->J_11; + Real J_0 = this->J_0; + for (auto && data : zip(make_view(C, 2*spatial_dimension, 2*spatial_dimension))) { + auto & CC = std::get<0>(data); + CC(0, 0) = G * A; + CC(1, 1) = G * A; + CC(2, 2) = E * A; + CC(3, 3) = J_22 * E; + CC(3, 4) = -J_12 * E; + CC(4, 3) = -J_12 * E; + CC(4, 4) = J_11 * E; + CC(5, 5) = J_0 * G; + } + AKANTU_DEBUG_OUT(); + +} +void NonlinearBeamModel::computeJbar(Array Jbar) { + AKANTU_DEBUG_IN(); + UInt nb_node_per_element = Mesh::getNbNodesPerElement(type); + const auto & shape_vector = getFEEngine().getShapes(type); + + Array strains(shape_vector.size(), 2*spatial_dimension); + Array strains0(shape_vector.size(), 2*spatial_dimension); + this->computeStrains(strains, false); + this->computeStrains(strains0, true); + + Array L(shape_vector.size(), spatial_dimension*nb_node_per_element); + this->get_rotation_matrix(L); + + + Array ID(shape_vector.size(), spatial_dimension*nb_node_per_element); + for (auto && data : zip(make_view(ID, spatial_dimension, nb_node_per_element))){ + std::get<0>(data) = Matrix::eye(spatial_dimension,1); + } + + Array DX(shape_vector.size(), spatial_dimension*spatial_dimension); + Array DX0(shape_vector.size(), spatial_dimension*spatial_dimension); + this->computeDX(DX, strains, L, 0., 0.); + this->computeDX(DX0, strains0, ID, 0., 0.); + + Array F(shape_vector.size(), spatial_dimension*spatial_dimension); + for (auto && data : zip(make_view(F, spatial_dimension, spatial_dimension), make_view(DX, spatial_dimension, spatial_dimension), make_view(DX0, spatial_dimension, spatial_dimension), make_view(Jbar))) { + auto & FF = std::get<0>(data); + auto & D = std::get<1>(data); + auto & D0 = std::get<2>(data); + FF = D * D0.inverse(); + std::get<3>(data) = FF.det(); + } + AKANTU_DEBUG_OUT(); + } -void NonlinearBeamModel::computeJbar() { +void NonlinearBeamModel::computeDX(Array DX, Array strains, Array Lambda, Real x1, Real x2) { + AKANTU_DEBUG_IN(); + Vector e1 = {1., 0., 0.}; + Vector e2 = {0., 1., 0.}; + Vector e3 = {0., 0., 1.}; + Vector Gamma(3); + Vector Omega(3); + Vector t1(3); + Vector t2(3); + Vector vs(3); + Vector vec(3); + Vector cp(3); + Matrix op(3,3); + Matrix res(3,3); + + for (auto && data : zip(make_view(strains, spatial_dimension, 2), make_view(Lambda, spatial_dimension, spatial_dimension), make_view(DX, spatial_dimension, spatial_dimension))) { + auto & strain = std::get<0>(data); + + Gamma = strain(0); + Omega = strain(1); + + t1 = std::get<1>(data) * e1; + t2 = std::get<1>(data) * e2; + vs = t1 * x1 + t2 * x2; + vec = Gamma + cp.crossProduct(Omega, std::get<1>(data) * vs); + op.outerProduct(vec, e3); + res = Matrix::eye(3, 1.) + op; + std::get<2>(data) = std::get<1>(data) * res; + } + AKANTU_DEBUG_OUT(); } void NonlinearBeamModel::computeStresses(Array stresses) { AKANTU_DEBUG_IN(); - Array strains(dim, 2*spatial_dimension); - this->computeStrains(strains); - Array C(dim, 2*spatial_dimension*2*nb_nodes_per_element); + UInt nb_node_per_element = Mesh::getNbNodesPerElement(type); + const auto & shape_vector = getFEEngine().getShapes(type); + + Array strains(shape_vector.size(), 2*spatial_dimension); + this->computeStrains(strains, false); + Array C(shape_vector.size(), 2*spatial_dimension*2*spatial_dimension); this->constitutive_C(C); for (auto && data :zip(make_view(stresses, 2*spatial_dimension), make_view(strains, 2*spatial_dimension), make_view(C, 2*spatial_dimension, 2*nb_node_per_element))){ auto & stress = std::get<0>(data); auto & strain = std::get<1>(data); auto & CC = std::get<2>(data); stress = CC * strain; } AKANTU_DEBUG_OUT(); } +void NonlinearBeamModel::computeInertiaTensor(Array J) { + + AKANTU_DEBUG_IN(); + Real J_22 = this->J_22; + Real J_12 = this->J_12; + Real J_21 = J_12; + Real J_11 = this->J_11; + Real rho = this->density; + Matrix J1(spatial_dimension, spatial_dimension); + J1 = {{J_11, J_12, 0}, {J_21, J_22, 0}, {0, 0, 0}}; + for (auto && data : zip(make_view(J, spatial_dimension, spatial_dimension))) { + std::get<0>(data) = rho * Matrix::eye(spatial_dimension, 1.) * (J_11+J_22) - rho * J1; + } + AKANTU_DEBUG_OUT(); + +} +void NonlinearBeamModel::assembleMass() { + AKANTU_DEBUG_IN(); + UInt nb_node_per_element = Mesh::getNbNodesPerElement(type); + const auto & shape_vector = getFEEngine().getShapes(type); + + Array J(shape_vector.size(), spatial_dimension*spatial_dimension); + this->computeInertiaTensor(J); + Array L(shape_vector.size(), spatial_dimension*nb_node_per_element); + this->get_rotation_matrix(L); + Matrix R(2*spatial_dimension, 2*spatial_dimension); + R.set(0.); + Matrix Imat(2*spatial_dimension, 2*spatial_dimension); + Imat.set(0.); + Imat.block(Matrix::eye(spatial_dimension, 1.) * this->density * this->A, 0, 0); + + for (auto && data : zip(make_view(M, 2*spatial_dimension*nb_node_per_element, 2*spatial_dimension*nb_node_per_element), make_view(J, spatial_dimension, spatial_dimension), make_view(L, spatial_dimension, spatial_dimension), make_view(Ns, 2* spatial_dimension, 2*spatial_dimension*nb_node_per_element))) { + auto & Mass = std::get<0>(data); + auto & LL = std::get<2>(data); + auto & N = std::get<3>(data); + + R.block(LL, 0, 0); + R.block(LL, 3, 3); + Imat.block(std::get<1>(data), 3, 3); + + Mass = N.transpose() * R * Imat * R.transpose() * N; + //Mass /= L/2; + } + AKANTU_DEBUG_OUT(); +} +void NonlinearBeamModel::convected_angular_velocity(Array conv_rv) { + AKANTU_DEBUG_IN(); + UInt nb_node_per_element = Mesh::getNbNodesPerElement(type); + const auto & shape_vector = getFEEngine().getShapes(type); + Array L(shape_vector.size(), spatial_dimension*nb_node_per_element); + Array L0(shape_vector.size(), spatial_dimension*nb_node_per_element); + this->get_rotation_matrix(L, false); + this->get_rotation_matrix(L0, true); + Array inter_rv(shape_vector.size(), nb_node_per_element); + Array rv(this->linear_angular_displacement->size(), spatial_dimension); + Array inter_theta(shape_vector.size(), nb_node_per_element); + Array theta(this->linear_angular_displacement->size(), spatial_dimension); + + for (auto && data : zip(make_view(rv, spatial_dimension), make_view(*(this->linear_angular_velocity), spatial_dimension, 2), make_view(theta, spatial_dimension), make_view(*(this->linear_angular_displacement), spatial_dimension, 2))) { + std::get<0>(data) = std::get<1>(data)(1); + std::get<2>(data) = std::get<3>(data)(1); + } + this->interpolate(rv, inter_rv); + this->interpolate(theta, inter_theta); + Matrix W_hat(spatial_dimension, spatial_dimension); + for (auto && data : zip(make_view(L, spatial_dimension, nb_node_per_element), make_view(L0, spatial_dimension, nb_node_per_element), make_view(inter_theta, spatial_dimension), make_view(inter_rv, spatial_dimension), make_view(conv_rv, spatial_dimension))) { + auto & LL = std::get<0>(data); + auto & LL0 = std::get<1>(data); + W_hat = LL.transpose() * expDerivative(std::get<2>(data), std::get<3>(data)) * LL0; + std::get<4>(data) = skew2vec(W_hat); + } + AKANTU_DEBUG_OUT(); +} + +void NonlinearBeamModel::computeInertialBodyForce(Array IBF) { + + AKANTU_DEBUG_IN(); + UInt nb_node_per_element = Mesh::getNbNodesPerElement(type); + const auto & shape_vector = getFEEngine().getShapes(type); + Array J(shape_vector.size(), spatial_dimension*spatial_dimension); + this->computeInertiaTensor(J); + Array w(shape_vector.size(), spatial_dimension); + this->convected_angular_velocity(w); + Array L(shape_vector.size(), spatial_dimension*nb_node_per_element); + this->get_rotation_matrix(L); + Vector f(spatial_dimension); + for (auto && data : zip(make_view(IBF, spatial_dimension, 2), make_view(J, spatial_dimension, spatial_dimension), make_view(w, spatial_dimension), make_view(L, spatial_dimension, nb_node_per_element))) { + auto & LL = std::get<3>(data); + + f = skew(std::get<2>(data)) * LL * std::get<1>(data) * LL.transpose() * std::get<2>(data); + std::get<0>(data)(0) = Vector {0., 0., 0.}; + std::get<0>(data)(1) = f; + } + + AKANTU_DEBUG_OUT(); +} + + +void NonlinearBeamModel::computeInertialForces() { + AKANTU_DEBUG_IN(); + UInt nb_node_per_element = Mesh::getNbNodesPerElement(type); + const auto & shape_vector = getFEEngine().getShapes(type); + + Array IBF(shape_vector.size(), 2*spatial_dimension); + this->computeInertialBodyForce(IBF); + + for (auto && data : zip(make_view(*(this->inertial_force_torque), 2*spatial_dimension*nb_node_per_element), make_view(Ns, 2* spatial_dimension, 2*spatial_dimension*nb_node_per_element), make_view(IBF, 2*spatial_dimension))) { + auto & N = std::get<1>(data); + + std::get<0>(data) = N.transpose() * std::get<2>(data); + //std::get<0>(data) /= L/2.; + } + AKANTU_DEBUG_OUT(); +} + + +void NonlinearBeamModel::assembleInternalForces() { + AKANTU_DEBUG_IN(); + UInt nb_node_per_element = Mesh::getNbNodesPerElement(type); + const auto & shape_vector = getFEEngine().getShapes(type); + + Array B(shape_vector.size(), 2*spatial_dimension*2*spatial_dimension*nb_node_per_element); + this->B_matrix(B); + Array stresses(shape_vector.size(), 2*spatial_dimension); + this->computeStresses(stresses); + + for (auto && data : zip(make_view(*(this->internal_force_torque), 2*spatial_dimension*nb_node_per_element), make_view(B, 2*spatial_dimension, 2*spatial_dimension*nb_node_per_element), make_view(stresses, 2*spatial_dimension))) { + std::get<0>(data) = std::get<1>(data).transpose() * std::get<2>(data); + } + AKANTU_DEBUG_OUT(); +} + + + + +void NonlinearBeamModel::computeAcceleration() { + AKANTU_DEBUG_IN(); + UInt nb_node_per_element = Mesh::getNbNodesPerElement(type); + const auto & shape_vector = getFEEngine().getShapes(type); + + this->assembleMass(); + + for (auto && data : zip(make_view(M, 2*spatial_dimension*nb_node_per_element, 2*spatial_dimension*nb_node_per_element), make_view(*(this->blocked_dofs), 2*spatial_dimension*nb_node_per_element))){ + + } + + AKANTU_DEBUG_OUT(); +} + + -void NonlinearBeamModel::assembleInternalForces() { - AKANTU_DEBUG_IN(); - AKANTU_DEBUG_OUT(); -} -void NonlinearBeamModel::assembleMass() { - AKANTU_DEBUG_IN(); - AKANTU_DEBUG_OUT(); -} -void NonlinearBeamModel::assembleStiffnessMatrix() { - AKANTU_DEBUG_IN(); - AKANTU_DEBUG_OUT(); -} - } // namespace akantu diff --git a/src/model/nonlinear_beam/nonlinear_beam_model.hh b/src/model/nonlinear_beam/nonlinear_beam_model.hh index 0b2d07b82..e97cf9e3f 100644 --- a/src/model/nonlinear_beam/nonlinear_beam_model.hh +++ b/src/model/nonlinear_beam/nonlinear_beam_model.hh @@ -1,258 +1,278 @@ #include "data_accessor.hh" #include "boundary_condition.hh" #include "fe_engine.hh" #include "model.hh" #include #ifndef AKANTU_NONLINEAR_BEAM_MODEL_HH_ #define AKANTU_NONLINEAR_BEAM_MODEL_HH_ namespace akantu { class Material; class MaterialSelector; class DumperIOHelper; template class IntegratorGauss; template class ShapeLagrange; } // namespace akantu namespace akantu { class NonlinearBeamModel : public Model, public DataAccessor, public DataAccessor { /* ------------------------------------------------------------------------ */ /* Constructors/Destructors */ /* ------------------------------------------------------------------------ */ public: using FEEngineType = FEEngineTemplate; NonlinearBeamModel(Mesh & mesh, UInt dim = _all_dimensions, const ID & id = "nonlinear_beam_model", std::shared_ptr dof_manager = nullptr); ~NonlinearBeamModel() override; /* ------------------------------------------------------------------------ */ /* Methods */ /* ------------------------------------------------------------------------ */ protected: /// generic function to initialize everything ready for explicit dynamics void initFullImpl(const ModelOptions & options) override; /// read one material file to instantiate all the materials void readMaterials(); /// allocate all vectors void initSolver(TimeStepSolverType, NonLinearSolverType) override; /// initialize the model void initModel() override; //void predictor() override; void assembleResidual() override; /// get the type of matrix needed MatrixType getMatrixType(const ID & matrix_id) const override; /// callback to assemble a lumped Matrix void assembleLumpedMatrix(const ID & matrix_id) override; /// callback to assemble a Matrix void assembleMatrix(const ID & matrix_id) override; std::tuple getDefaultSolverID(const AnalysisMethod & method) override; ModelSolverOptions getDefaultSolverOptions(const TimeStepSolverType & type) const override; TimeStepSolverType getDefaultSolverType() const override; void N_matrix(Array &Ns); void N_rotator_matrix(Array &N_rot_mat); void N_grad_matrix(Array &dNs); void B_matrix(Array B_mat); void get_rotation_matrix(Array L, bool origin = false); void interpolate(Array &field, Array &interField); void grad_interpolate(Array &field, Array &interField); void computeStrains(Array strains, bool origin); + void computeStresses(Array stresses); + void constitutive_C(Array C); + void computeJbar(Array Jbar); + void computeDX(Array DX, Array strains, Array Lambda, Real x1, Real x2); + void computeInertiaTensor(Array J); + + void computeInertialBodyForce(Array IBF); + void convected_angular_velocity(Array conv_rv); + void computeInertialForces(); + void assembleInternalForces(); + void computeAcceleration(); void assembleMass(); - void assembleStiffnessMatrix(); void assembleMassLumped(); /* ------------------------------------------------------------------------ */ /* Methods for explicit */ /* ------------------------------------------------------------------------ */ public: /// compute and get the stable time step //Real getStableTimeStep(); /// set the stable timestep void setTimeStep(Real time_step, const ID & solver_id = "") override; ///RAJOUTER DES FUNCTION POUR LE CALCUL EXPLICIT /* ------------------------------------------------------------------------ */ /* Dumpable interface */ /* ------------------------------------------------------------------------ */ public: /* std::shared_ptr createNodalFieldReal(const std::string & field_name, const std::string & group_name, bool padding_flag) override; std::shared_ptr createNodalFieldBool(const std::string & field_name, const std::string & group_name, bool padding_flag) override; std::shared_ptr createElementalField(const std::string & field_name, const std::string & group_name, bool padding_flag, UInt spatial_dimension, ElementKind kind) override; */ /* ------------------------------------------------------------------------ */ /* Accessors */ /* ------------------------------------------------------------------------ */ public: /// get the current value of the time step AKANTU_GET_MACRO(TimeStep, time_step, Real); /// get the density value AKANTU_GET_MACRO(Density, density, Real); /// get the value of the Young Modulus AKANTU_GET_MACRO(E, E, Real); /// get the value of the Poisson ratio AKANTU_GET_MACRO(Nu, nu, Real); /// get the value of the section area AKANTU_GET_MACRO(A, A, Real); /// get the Inertia AKANTU_GET_MACRO(J_11, J_11, Real); AKANTU_GET_MACRO(J_22, J_22, Real); AKANTU_GET_MACRO(J_33, J_33, Real); AKANTU_GET_MACRO(J_12, J_12, Real); AKANTU_GET_MACRO(J_13, J_13, Real); AKANTU_GET_MACRO(J_23, J_23, Real); /// get the NonlinearBeamModel::linear_angular_displacement array AKANTU_GET_MACRO_DEREF_PTR_NOT_CONST(Linear_Angular_Displacement, linear_angular_displacement); /// get the NonlinearBeamModel::linear_angular_displacement array AKANTU_GET_MACRO_DEREF_PTR(Linear_Angular_Displacement, linear_angular_displacement); /// get the NonlinearBeamModel::linear_angular_velocity array AKANTU_GET_MACRO_DEREF_PTR_NOT_CONST(Linear_Angular_Velocity, linear_angular_velocity); /// get the NonlinearBeamModel::linear_angular_velocity array AKANTU_GET_MACRO_DEREF_PTR(Linear_Angular_Velocity, linear_angular_velocity); /// get the NonlinearBeamModel::linear_angular_acceleration array AKANTU_GET_MACRO_DEREF_PTR_NOT_CONST(Linear_Angular_Acceleration, linear_angular_acceleration); /// get the NonlinearBeamModel::linear_angular_acceleration array AKANTU_GET_MACRO_DEREF_PTR(Linear_Angular_Acceleration, linear_angular_acceleration); /// get the NonlinearBeamModel::external_force_torque array AKANTU_GET_MACRO_DEREF_PTR_NOT_CONST(ExternalForceTorque, external_force_torque); /// get the NonlinearBeamModel::external_force_torque array AKANTU_GET_MACRO_DEREF_PTR(ExternalForceTorque, external_force_torque); /// get the NonlinearBeamModel::internal_force_torque array AKANTU_GET_MACRO_DEREF_PTR_NOT_CONST(InternalForceTorque, internal_force_torque); /// get the NonlinearBeamModel::internal_force_torque array AKANTU_GET_MACRO_DEREF_PTR(InternalForceTorque, internal_force_torque); + /// get the NonlinearBeamModel::inertial_force_torque array + AKANTU_GET_MACRO_DEREF_PTR_NOT_CONST(InertialForceTorque, inertial_force_torque); + /// get the NonlinearBeamModel::inertial_force_torque array + AKANTU_GET_MACRO_DEREF_PTR(InertialForceTorque, inertial_force_torque); + /// get the NonlinearBeamModel::blocked_dofs array AKANTU_GET_MACRO_DEREF_PTR_NOT_CONST(BlockedDOFs, blocked_dofs); /// get the NonlinearBeamModel::blocked_dofs array AKANTU_GET_MACRO_DEREF_PTR(BlockedDOFs, blocked_dofs); /// get the NonlinearBeamModel::initial_angle array AKANTU_GET_MACRO_DEREF_PTR_NOT_CONST(Initial_Angle, initial_angle); /// get the NonlinearBeamModel::initial_angle array AKANTU_GET_MACRO_DEREF_PTR(Initial_Angle, initial_angle); protected: /* ------------------------------------------------------------------------ */ FEEngine & getFEEngineBoundary(const ID & name = "") override; /* ------------------------------------------------------------------------ */ /* Class Members */ /* ------------------------------------------------------------------------ */ private: Array Ns; Array dNs; + Array M; ElementType type; /// time step Real time_step; /// the density Real density; /// Young modulus Real E; /// poisson Ratio Real nu; /// Section Area Real A; /// Inertia Real J_11; Real J_22; Real J_33; Real J_12; Real J_13; Real J_23; + Real J_0; /// linear and angular displacement array std::unique_ptr> linear_angular_displacement; /// linear and angular velocity array std::unique_ptr> linear_angular_velocity; /// linear and angular acceleration array std::unique_ptr> linear_angular_acceleration; /// external force and torque array std::unique_ptr> external_force_torque; /// internal force and torque array std::unique_ptr> internal_force_torque; + /// inertial force and torque array + std::unique_ptr> inertial_force_torque; + /// blocked dofs array std::unique_ptr> blocked_dofs; /// initial angle std::unique_ptr> initial_angle; }; } // namespace akantu /* -------------------------------------------------------------------------- */ /* inline functions */ /* -------------------------------------------------------------------------- */ #endif /* AKANTU_NONLINEAR_BEAM_MODEL_HH_ */