DOLFINx
DOLFINx C++ interface
math.h
1// Copyright (C) 2021 Igor Baratta
2//
3// This file is part of DOLFINx (https://www.fenicsproject.org)
4//
5// SPDX-License-Identifier: LGPL-3.0-or-later
6
7#pragma once
8
9#include <array>
10#include <cmath>
11#include <type_traits>
12#include <xtensor/xfixed.hpp>
13#include <xtensor/xtensor.hpp>
14
15namespace dolfinx::math
16{
17
22template <typename U, typename V>
23xt::xtensor_fixed<typename U::value_type, xt::xshape<3>> cross(const U& u,
24 const V& v)
25{
26 assert(u.size() == 3);
27 assert(v.size() == 3);
28 return {u[1] * v[2] - u[2] * v[1], u[2] * v[0] - u[0] * v[2],
29 u[0] * v[1] - u[1] * v[0]};
30}
31
34template <typename T>
35T difference_of_products(T a, T b, T c, T d) noexcept
36{
37 T w = b * c;
38 T err = std::fma(-b, c, w);
39 T diff = std::fma(a, d, -w);
40 return diff + err;
41}
42
47template <typename Matrix>
48auto det(const Matrix& A)
49{
50 using value_type = typename Matrix::value_type;
51 assert(A.shape(0) == A.shape(1));
52 assert(A.dimension() == 2);
53
54 const int nrows = A.shape(0);
55 switch (nrows)
56 {
57 case 1:
58 return A(0, 0);
59 case 2:
60 return difference_of_products(A(0, 0), A(0, 1), A(1, 0), A(1, 1));
61 case 3:
62 {
63 // Leibniz formula combined with Kahan’s method for accurate
64 // computation of 3 x 3 determinants
65 value_type w0 = difference_of_products(A(1, 1), A(1, 2), A(2, 1), A(2, 2));
66 value_type w1 = difference_of_products(A(1, 0), A(1, 2), A(2, 0), A(2, 2));
67 value_type w2 = difference_of_products(A(1, 0), A(1, 1), A(2, 0), A(2, 1));
68 value_type w3 = difference_of_products(A(0, 0), A(0, 1), w1, w0);
69 value_type w4 = std::fma(A(0, 2), w2, w3);
70 return w4;
71 }
72 default:
73 throw std::runtime_error("math::det is not implemented for "
74 + std::to_string(A.shape(0)) + "x"
75 + std::to_string(A.shape(1)) + " matrices.");
76 }
77}
78
85template <typename U, typename V>
86void inv(const U& A, V&& B)
87{
88 using value_type = typename U::value_type;
89 const std::size_t nrows = A.shape(0);
90 switch (nrows)
91 {
92 case 1:
93 B(0, 0) = 1 / A(0, 0);
94 break;
95 case 2:
96 {
97 value_type idet = 1. / det(A);
98 B(0, 0) = idet * A(1, 1);
99 B(0, 1) = -idet * A(0, 1);
100 B(1, 0) = -idet * A(1, 0);
101 B(1, 1) = idet * A(0, 0);
102 break;
103 }
104 case 3:
105 {
106 value_type w0 = difference_of_products(A(1, 1), A(1, 2), A(2, 1), A(2, 2));
107 value_type w1 = difference_of_products(A(1, 0), A(1, 2), A(2, 0), A(2, 2));
108 value_type w2 = difference_of_products(A(1, 0), A(1, 1), A(2, 0), A(2, 1));
109 value_type w3 = difference_of_products(A(0, 0), A(0, 1), w1, w0);
110 value_type det = std::fma(A(0, 2), w2, w3);
111 assert(det != 0.);
112 value_type idet = 1 / det;
113
114 B(0, 0) = w0 * idet;
115 B(1, 0) = -w1 * idet;
116 B(2, 0) = w2 * idet;
117 B(0, 1) = difference_of_products(A(0, 2), A(0, 1), A(2, 2), A(2, 1)) * idet;
118 B(0, 2) = difference_of_products(A(0, 1), A(0, 2), A(1, 1), A(1, 2)) * idet;
119 B(1, 1) = difference_of_products(A(0, 0), A(0, 2), A(2, 0), A(2, 2)) * idet;
120 B(1, 2) = difference_of_products(A(1, 0), A(0, 0), A(1, 2), A(0, 2)) * idet;
121 B(2, 1) = difference_of_products(A(2, 0), A(0, 0), A(2, 1), A(0, 1)) * idet;
122 B(2, 2) = difference_of_products(A(0, 0), A(1, 0), A(0, 1), A(1, 1)) * idet;
123 break;
124 }
125 default:
126 throw std::runtime_error("math::inv is not implemented for "
127 + std::to_string(A.shape(0)) + "x"
128 + std::to_string(A.shape(1)) + " matrices.");
129 }
130}
131
138template <typename U, typename V, typename P>
139void dot(const U& A, const V& B, P&& C, bool transpose = false)
140{
141 if (transpose)
142 {
143 assert(A.shape(0) == B.shape(1));
144 for (std::size_t i = 0; i < A.shape(1); i++)
145 for (std::size_t j = 0; j < B.shape(0); j++)
146 for (std::size_t k = 0; k < A.shape(0); k++)
147 C(i, j) += A(k, i) * B(j, k);
148 }
149 else
150 {
151 assert(A.shape(1) == B.shape(0));
152 for (std::size_t i = 0; i < A.shape(0); i++)
153 for (std::size_t j = 0; j < B.shape(1); j++)
154 for (std::size_t k = 0; k < A.shape(1); k++)
155 C(i, j) += A(i, k) * B(k, j);
156 }
157}
158
165template <typename U, typename V>
166void pinv(const U& A, V&& P)
167{
168 auto shape = A.shape();
169 assert(shape[0] > shape[1]);
170 assert(P.shape(1) == shape[0]);
171 assert(P.shape(0) == shape[1]);
172 using T = typename U::value_type;
173 if (shape[1] == 2)
174 {
175 xt::xtensor_fixed<T, xt::xshape<2, 3>> AT;
176 xt::xtensor_fixed<T, xt::xshape<2, 2>> ATA;
177 xt::xtensor_fixed<T, xt::xshape<2, 2>> Inv;
178 AT = xt::transpose(A);
179 std::fill(ATA.begin(), ATA.end(), 0.0);
180 std::fill(P.begin(), P.end(), 0.0);
181
182 // pinv(A) = (A^T * A)^-1 * A^T
183 dolfinx::math::dot(AT, A, ATA);
184 dolfinx::math::inv(ATA, Inv);
185 dolfinx::math::dot(Inv, AT, P);
186 }
187 else if (shape[1] == 1)
188 {
189 auto res = std::transform_reduce(A.begin(), A.end(), 0., std::plus<T>(),
190 [](const auto v) { return v * v; });
191 P = (1 / res) * xt::transpose(A);
192 }
193 else
194 {
195 throw std::runtime_error("math::pinv is not implemented for "
196 + std::to_string(A.shape(0)) + "x"
197 + std::to_string(A.shape(1)) + " matrices.");
198 }
199}
200
201} // namespace dolfinx::math