diff --git a/src/common/aka_types.hh b/src/common/aka_types.hh index d7fc69df8..204c185bb 100644 --- a/src/common/aka_types.hh +++ b/src/common/aka_types.hh @@ -1,1847 +1,1903 @@ /** * @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 log_map(const Matrix &R) { + inline void logMap(const Matrix &R) { /// A discuter avec Nico AKANTU_DEBUG_ASSERT(this->size() == 3 && R.rows() == 3 && R.cols() == 3, "need a 3 dim matrix and vector"); - auto theta = compute_rotation_angle(R); + Real theta = R.computeRotationAngle(); if (theta < 1e-8) { (*this) = {-R(1,2), R(0,2), -R(0,1)}; } - auto sym = double((R - R.transpose()).norm() / R.norm()) < 1e-12; + auto sym = ((R - R.transpose()).template norm() / R.template norm()) < 1e-12; if (sym) { - auto _R = 0.5 * (R + R.transpose); - Eigen::SelfAdjointEigenSolver s(_R); - auto eig_vals = s.eigenvalues(); - auto eig_vects = s.eigenvectors(); + Matrix _R = 0.5 * (R + R.transpose()); + Matrix eig_vects(3,3); + Vector eig_vals(3); + _R.eig(eig_vects, eig_vals); for (UInt i = 0; i < eig_vals.size(); ++i) { auto val = eig_vals(i); auto vect = eig_vects(i); if (std::abs(val - 1) < 1e-14) { (*this) = theta * vect / vect.norm(); } } throw std::runtime_error("Symmetric cannot log:"); } - auto A = (R - R.transpose()) / 2; - auto _log = theta / std::sin(theta) * A; - (*this) = skew2vec(_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 convected_angle_derivative(const Vector &B, + inline void convectedAngleDerivative(const Vector &B, const Vector &Bp) { AKANTU_DEBUG_ASSERT(this->size() == B.size() == Bp.size() ==3, "need a 3 dim vectors"); - auto exp_deriv = exp_map(-B) * exp_derivative(B, Bp); - (*this) = skew2vec(exp_deriv); + Matrix B1(3,3); + Matrix Bp1(3,3); + Matrix BB = -B; + B1.expMap(BB); + 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 angle_acceleration(const Vector &B, const Vector &Bp, + 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"); - auto L = exp_map(B); - auto vel = exp_derivative(B, Bp); - auto acc = exp_acceleration(B, Bp, Bpp); - (*this) = acc * L.transpose() + vel * vel.transpose(); + 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 compose_rotations(const Vector &a1, const Vector &a2) { + inline auto composeRotations(const Vector &a1, const Vector &a2) { AKANTU_DEBUG_ASSERT(this->size() == a1.size() == a2.size() == 3, "need a 3 dim vectors"); - (*this) = log_map(exp_map(a1) * exp_map(a2)); + 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(); } - template - Vector skew2vec(const Matrix & mat){ - Vector v(3); - v.skew2vec(mat); - return v; - } - - template - Vector log_map(const Matrix & mat){ - Vector v(3); - v.log_map(mat); - return v; - } - - template - Vector convected_angle_derivative(const Vector & v1, const Vector & v2){ - Vector v(3); - v.convected_angle_derivative(v1, v2); - return v; - } - - template - Vector angle_acceleration(const Vector & v1, const Vector & v2, const Vector &v3){ - Vector v(3); - v.angle_acceleration(v1, v2, v3); - return v; - } + - template - Vector compose_rotations(const Vector & v1, const Vector & v2){ - Vector v(3); - v.compose_rotations(v1, v2); - return v; - } + /* ------------------------------------------------------------------------ */ 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}; + (*this)(0)=Vector {0, v(2), -v(1)}; + (*this)(1)=Vector {-v(2), 0, v(0)}; + (*this)(2)=Vector {v(1), -v(0), 0}; } /* ---------------------------------------------------------------------- */ - inline void exp_map(const Vector &v) { + 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); + (*this) = Matrix::eye(3, 1.); } - auto K = skew(v) / theta; - (*this) = Matrix::eye(3) + std::sin(theta) * K + (1 - std::cos(theta)) * K * K; + 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() { + /// demander à Nico car soirtie réel et non vecteur + 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 exp_derivative(const Vector &v, const Vector &vp) { + 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); } - auto _B = skew(v); - auto _Bp = skew(vp); + 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 exp_acceleration(const Vector &B, const Vector &Bp, + inline void expAcceleration(const Vector &B, const Vector &Bp, const Vector &Bpp) { - /// demander à Nico pour le pow 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)); } - auto Bhat = skew(B); - auto Bphat = skew(Bp); - auto Bpphat = skew(Bpp); + 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; - res += 2 * fp * Bphat + 2 * hp * (Bhat * Bphat + Bphat * Bhat); - res += f * Bpphat + h * (2 * Bphat * Bphat + Bhat * Bpphat + Bpphat * 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 skew(const Vector & v){ Matrix s(3,3); s.skew(v); return s; } template -Matrix exp_map(const Vector & v){ +Matrix expMap(const Vector & v){ Matrix s(3,3); - s.exp_map(v); + s.expMap(v); return s; } template -Matrix exp_derivative(const Vector & v1, const Vector & v2){ +Matrix expDerivative(const Vector & v1, const Vector & v2){ Matrix s(3,3); - s.exp_derivative(v1, v2); + s.expDerivative(v1, v2); return s; } template -Matrix exp_acceleration(const Vector & v1, const Vector & v2, const Vector & v3){ +Matrix expAcceleration(const Vector & v1, const Vector & v2, const Vector & v3){ Matrix s(3,3); - s.exp_acceleration(v1, v2, v3); + 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/newmark-beta-NL-beam.cc b/src/model/nonlinear_beam/newmark-beta-NL-beam.cc index db2ae8d51..2cb66419e 100644 --- a/src/model/nonlinear_beam/newmark-beta-NL-beam.cc +++ b/src/model/nonlinear_beam/newmark-beta-NL-beam.cc @@ -1,122 +1,134 @@ /** * @file newmark-beta.cc * * @author David Simon Kammer * @author Nicolas Richart * * @date creation: Fri Oct 23 2015 * @date last modification: Wed Mar 27 2019 * * @brief implementation of the newmark-@f$\beta@f$ integration scheme. This * implementation is taken from Méthodes numériques en mécanique des solides by * Alain Curnier \note{ISBN: 2-88074-247-1} * * * @section LICENSE * * Copyright (©) 2015-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 "newmark-beta-NL-beam.hh" #include "dof_manager.hh" #include "sparse_matrix.hh" -// #include "angle_tool.hh" +#include "aka_types.hh" /* -------------------------------------------------------------------------- */ namespace akantu { /* -------------------------------------------------------------------------- */ NewmarkBetaNLBeam::NewmarkBetaNLBeam(DOFManager & dof_manager, const ID & dof_id) : CentralDifference(dof_manager, dof_id) { } /* -------------------------------------------------------------------------- */ void NewmarkBetaNLBeam::predictor(Real delta_t, Array & u, Array & u_dot, Array & u_dot_dot, const Array & blocked_dofs) const { AKANTU_DEBUG_IN(); UInt nb_nodes = u.size(); UInt nb_degree_of_freedom = u.getNbComponent() * nb_nodes; Real * u_dot_val = u_dot.storage(); Real * u_dot_dot_val = u_dot_dot.storage(); bool * blocked_dofs_val = blocked_dofs.storage(); for (UInt d = 0; d < nb_degree_of_freedom; d++) { if (!(*blocked_dofs_val)) { *u_dot_val = *u_dot_val + delta_t / 2 * *u_dot_dot_val; } u_dot_val++; u_dot_dot_val++; blocked_dofs_val++; } UInt dim = u.getNbComponent() / 2; for(auto && data : zip(make_view(blocked_dofs, dim, 2), make_view(u,dim, 2), make_view(u_dot,dim,2))) { Vector blocked_u = std::get<0>(data)(0); Vector blocked_theta = std::get<0>(data)(1); Vector disp = std::get<1>(data)(0); Vector theta = std::get<1>(data)(1); Vector vel = std::get<2>(data)(0); Vector rv = std::get<2>(data)(1); disp += delta_t * vel; - //theta = log_map(exp_map(delta_t * this->rv[nd]) * exp_map(this->theta[nd])); + Matrix Sk = skew(rv); + Matrix T = expMap(rv); + Matrix TT = expDerivative(rv,theta); + Matrix AC = expAcceleration(rv, theta, disp); + Real f = computeRotationAngle(T); + + Vector Vec = skew2vec(Sk); + Vector T1 = logMap(T); + Vector T2 = convectedAngleDerivative(rv, theta); + Vector T3 = angleAcceleration(rv, disp, theta); + Vector T4 = composeRotations(rv, theta); + + //theta = logMap(expMap(delta_t * rv) * expMap(theta)); } AKANTU_DEBUG_OUT(); } /* -------------------------------------------------------------------------- */ void NewmarkBetaNLBeam::corrector(Real delta_t, Array & u, Array & u_dot, Array & u_dot_dot, const Array & blocked_dofs) const { AKANTU_DEBUG_IN(); UInt nb_nodes = u.size(); UInt nb_degree_of_freedom = u.getNbComponent() * nb_nodes; Real * u_dot_val = u_dot.storage(); Real * u_dot_dot_val = u_dot_dot.storage(); bool * blocked_dofs_val = blocked_dofs.storage(); for (UInt d = 0; d < nb_degree_of_freedom; d++) { if (!(*blocked_dofs_val)) { *u_dot_val = *u_dot_val + delta_t / 2 * *u_dot_dot_val; } u_dot_val++; u_dot_dot_val++; blocked_dofs_val++; } AKANTU_DEBUG_OUT(); } } // namespace akantu diff --git a/src/model/nonlinear_beam/newmark-beta-NL-beam.hh b/src/model/nonlinear_beam/newmark-beta-NL-beam.hh index 64ad0e03b..b7fec2abc 100644 --- a/src/model/nonlinear_beam/newmark-beta-NL-beam.hh +++ b/src/model/nonlinear_beam/newmark-beta-NL-beam.hh @@ -1,75 +1,75 @@ /** * @file newmark-beta.hh * * @author David Simon Kammer * @author Nicolas Richart * * @date creation: Tue Oct 05 2010 * @date last modification: Sat Sep 12 2020 * * @brief implementation of the newmark-@f$\beta@f$ integration scheme. This * implementation is taken from Méthodes numériques en mécanique des solides by * Alain Curnier \note{ISBN: 2-88074-247-1} * * * @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 "newmark-beta.hh" /* -------------------------------------------------------------------------- */ -#ifndef AKANTU_NEWMARK_BETA_NL_BEEAM_HH_ +#ifndef AKANTU_NEWMARK_BETA_NL_BEAM_HH_ #define AKANTU_NEWMARK_BETA_NL_BEAM_HH_ /* -------------------------------------------------------------------------- */ namespace akantu { class NewmarkBetaNLBeam : public CentralDifference { /* ------------------------------------------------------------------------ */ /* Constructors/Destructors */ /* ------------------------------------------------------------------------ */ public: NewmarkBetaNLBeam(DOFManager & dof_manager, const ID & dof_id); /* ------------------------------------------------------------------------ */ /* Methods */ /* ------------------------------------------------------------------------ */ public: void predictor(Real delta_t, Array & u, Array & u_dot, Array & u_dot_dot, const Array & blocked_dofs) const override; void corrector(Real delta_t, Array & u, Array & u_dot, Array & u_dot_dot, const Array & blocked_dofs) const; }; /* -------------------------------------------------------------------------- */ } // namespace akantu #endif /* AKANTU_NEWMARK_BETA_NL_BEAM_HH_ */ diff --git a/src/model/nonlinear_beam/nonlinear_beam_model.cc b/src/model/nonlinear_beam/nonlinear_beam_model.cc index 5b387215f..ff8420087 100644 --- a/src/model/nonlinear_beam/nonlinear_beam_model.cc +++ b/src/model/nonlinear_beam/nonlinear_beam_model.cc @@ -1,290 +1,292 @@ #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){ AKANTU_DEBUG_IN(); this->registerDataAccessor(*this); /* 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, spatial_dimension); #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("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); } /* -------------------------------------------------------------------------- */ 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->external_force_torque, 2*spatial_dimension, "external_force_torque"); // this->allocNodalField(this->blocked_dofs, 2*spatial_dimension, "blocked_dofs"); /* ------------------------------------------------------------------------ */ 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->getDOFManager().assembleToResidual("displacement", *this->external_force_torque, 1); this->getDOFManager().assembleToResidual("displacement", *this->internal_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(); } else if (matrix_id == "M") { this->assembleMass(); } } /* -------------------------------------------------------------------------- */ void NonlinearBeamModel::assembleLumpedMatrix(const ID & matrix_id) { if (matrix_id == "M") { this->assembleMassLumped(); } } /* -------------------------------------------------------------------------- */ Matrix NonlinearBeamModel::N_matrix() { + /* AKANTU_DEBUG_IN(); Matrix N_mat(2*spatial_dimension, this->nb_nodes_per_element*2*spatial_dimension,0.); auto _N = computeShapes(const vector_type & natural_coords, vector_type & N); for (UInt nd=0; nd < this->nb_nodes_per_element; ++nd) { N_mat.block<6, 2*spatial_dimension>(0, nd * 2*spatial_dimension) = Matrix(6, 6).Identity() * _N[nd]; } return N_mat; 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(); } }