DOLFINx 0.12.0.0
DOLFINx C++
Loading...
Searching...
No Matches
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 "types.h"
10#include <algorithm>
11#include <array>
12#include <basix/mdspan.hpp>
13#include <cassert>
14#include <cmath>
15#include <concepts>
16#include <format>
17#include <stdexcept>
18
19namespace dolfinx::math
20{
21
26template <typename U, typename V>
27 requires scalar<typename U::value_type>
28 && std::same_as<typename U::value_type, typename V::value_type>
29constexpr std::array<typename U::value_type, 3> cross(const U& u, const V& v)
30{
31 assert(u.size() == 3);
32 assert(v.size() == 3);
33 return {u[1] * v[2] - u[2] * v[1], u[2] * v[0] - u[0] * v[2],
34 u[0] * v[1] - u[1] * v[0]};
35}
36
39// TODO: mark constexpr with C++23 (std::fma becomes constexpr).
40template <std::floating_point T>
41T difference_of_products(T a, T b, T c, T d) noexcept
42{
43 T w = b * c;
44 T err = std::fma(-b, c, w);
45 T diff = std::fma(a, d, -w);
46 return diff + err;
47}
48
53// TODO: mark constexpr with C++23 (relies on std::fma, constexpr from C++23).
54template <typename U>
55 requires MDSpanRank2<U> && std::floating_point<typename U::value_type>
56auto det(U A)
57{
58 assert(A.extent(0) == A.extent(1));
59
60 using value_type = typename U::value_type;
61 const int nrows = A.extent(0);
62 switch (nrows)
63 {
64 case 1:
65 return A(0, 0);
66 case 2:
67 return difference_of_products(A(0, 0), A(0, 1), A(1, 0), A(1, 1));
68 case 3:
69 {
70 // Leibniz formula combined with Kahan’s method for accurate
71 // computation of 3 x 3 determinants
72 value_type w0 = difference_of_products(A(1, 1), A(1, 2), A(2, 1), A(2, 2));
73 value_type w1 = difference_of_products(A(1, 0), A(1, 2), A(2, 0), A(2, 2));
74 value_type w2 = difference_of_products(A(1, 0), A(1, 1), A(2, 0), A(2, 1));
75 value_type w3 = difference_of_products(A(0, 0), A(0, 1), w1, w0);
76 value_type w4 = std::fma(A(0, 2), w2, w3);
77 return w4;
78 }
79 default:
80 throw std::runtime_error(
81 std::format("math::det is not implemented for {}x{} matrices.",
82 A.extent(0), A.extent(1)));
83 }
84}
85
92// TODO: mark constexpr with C++23 (relies on std::fma, constexpr from C++23).
93template <typename U, typename V>
94 requires MDSpanRank2<U> && MDSpanRank2<V>
95 && std::floating_point<typename U::value_type>
96void inv(U A, V B)
97{
98 using value_type = typename U::value_type;
99 const std::size_t nrows = A.extent(0);
100 switch (nrows)
101 {
102 case 1:
103 B(0, 0) = value_type{1} / A(0, 0);
104 break;
105 case 2:
106 {
107 value_type idet = 1. / det(A);
108 B(0, 0) = idet * A(1, 1);
109 B(0, 1) = -idet * A(0, 1);
110 B(1, 0) = -idet * A(1, 0);
111 B(1, 1) = idet * A(0, 0);
112 break;
113 }
114 case 3:
115 {
116 value_type w0 = difference_of_products(A(1, 1), A(1, 2), A(2, 1), A(2, 2));
117 value_type w1 = difference_of_products(A(1, 0), A(1, 2), A(2, 0), A(2, 2));
118 value_type w2 = difference_of_products(A(1, 0), A(1, 1), A(2, 0), A(2, 1));
119 value_type w3 = difference_of_products(A(0, 0), A(0, 1), w1, w0);
120 value_type det = std::fma(A(0, 2), w2, w3);
121 assert(det != 0.);
122 value_type idet = 1 / det;
123
124 B(0, 0) = w0 * idet;
125 B(1, 0) = -w1 * idet;
126 B(2, 0) = w2 * idet;
127 B(0, 1) = difference_of_products(A(0, 2), A(0, 1), A(2, 2), A(2, 1)) * idet;
128 B(0, 2) = difference_of_products(A(0, 1), A(0, 2), A(1, 1), A(1, 2)) * idet;
129 B(1, 1) = difference_of_products(A(0, 0), A(0, 2), A(2, 0), A(2, 2)) * idet;
130 B(1, 2) = difference_of_products(A(1, 0), A(0, 0), A(1, 2), A(0, 2)) * idet;
131 B(2, 1) = difference_of_products(A(2, 0), A(0, 0), A(2, 1), A(0, 1)) * idet;
132 B(2, 2) = difference_of_products(A(0, 0), A(1, 0), A(0, 1), A(1, 1)) * idet;
133 break;
134 }
135 default:
136 throw std::runtime_error(
137 std::format("math::inv is not implemented for {}x{} matrices.",
138 A.extent(0), A.extent(1)));
139 }
140}
141
148template <typename U, typename V, typename P>
149 requires MDSpanRank2<U> && MDSpanRank2<V> && MDSpanRank2<P>
150 && scalar<typename U::value_type> && scalar<typename V::value_type>
151 && scalar<typename P::value_type>
152constexpr void dot(U A, V B, P C, bool transpose = false)
153{
154 if (transpose)
155 {
156 assert(A.extent(0) == B.extent(1));
157 for (std::size_t i = 0; i < A.extent(1); i++)
158 for (std::size_t j = 0; j < B.extent(0); j++)
159 for (std::size_t k = 0; k < A.extent(0); k++)
160 C(i, j) += A(k, i) * B(j, k);
161 }
162 else
163 {
164 assert(A.extent(1) == B.extent(0));
165 for (std::size_t i = 0; i < A.extent(0); i++)
166 for (std::size_t j = 0; j < B.extent(1); j++)
167 for (std::size_t k = 0; k < A.extent(1); k++)
168 C(i, j) += A(i, k) * B(k, j);
169 }
170}
171
178// TODO: mark constexpr with C++23 (relies on std::fma via inv, constexpr from
179// C++23).
180template <typename U, typename V>
181 requires MDSpanRank2<U> && MDSpanRank2<V>
182 && std::floating_point<typename U::value_type>
183void pinv(U A, V P)
184{
185 assert(A.extent(0) > A.extent(1));
186 assert(P.extent(1) == A.extent(0));
187 assert(P.extent(0) == A.extent(1));
188 using T = typename U::value_type;
189 if (A.extent(1) == 2)
190 {
191 // Fixed-size buffers below assume A is 3x2
192 assert(A.extent(0) == 3);
193 std::array<T, 6> ATb;
194 std::array<T, 4> ATAb, Invb;
195 md::mdspan<T, md::extents<std::size_t, 2, 3>> AT(ATb.data(), 2, 3);
196 md::mdspan<T, md::extents<std::size_t, 2, 2>> ATA(ATAb.data(), 2, 2);
197 md::mdspan<T, md::extents<std::size_t, 2, 2>> Inv(Invb.data(), 2, 2);
198
199 for (std::size_t i = 0; i < AT.extent(0); ++i)
200 for (std::size_t j = 0; j < AT.extent(1); ++j)
201 AT(i, j) = A(j, i);
202
203 std::ranges::fill(ATAb, 0.0);
204 for (std::size_t i = 0; i < P.extent(0); ++i)
205 for (std::size_t j = 0; j < P.extent(1); ++j)
206 P(i, j) = 0;
207
208 // pinv(A) = (A^T * A)^-1 * A^T
209 dot(AT, A, ATA);
210 inv(ATA, Inv);
211 dot(Inv, AT, P);
212 }
213 else if (A.extent(1) == 1)
214 {
215 T res = 0;
216 for (std::size_t i = 0; i < A.extent(0); ++i)
217 for (std::size_t j = 0; j < A.extent(1); ++j)
218 res += A(i, j) * A(i, j);
219
220 for (std::size_t i = 0; i < A.extent(0); ++i)
221 for (std::size_t j = 0; j < A.extent(1); ++j)
222 P(j, i) = (1 / res) * A(i, j);
223 }
224 else
225 {
226 throw std::runtime_error(
227 std::format("math::pinv is not implemented for {}x{} matrices.",
228 A.extent(0), A.extent(1)));
229 }
230}
231
232} // namespace dolfinx::math
Generated by doxygen 1.17.0