Eigen  5.0.1
 
Loading...
Searching...
No Matches
InverseSize4.h
1// This file is part of Eigen, a lightweight C++ template library
2// for linear algebra.
3//
4// Copyright (C) 2001 Intel Corporation
5// Copyright (C) 2010 Gael Guennebaud <gael.guennebaud@inria.fr>
6// Copyright (C) 2009 Benoit Jacob <jacob.benoit.1@gmail.com>
7//
8// This Source Code Form is subject to the terms of the Mozilla
9// Public License v. 2.0. If a copy of the MPL was not distributed
10// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
11//
12// The algorithm below is a reimplementation of former \src\LU\Inverse_SSE.h using PacketMath.
13// inv(M) = M#/|M|, where inv(M), M# and |M| denote the inverse of M,
14// adjugate of M and determinant of M respectively. M# is computed block-wise
15// using specific formulae. For proof, see:
16// https://lxjk.github.io/2017/09/03/Fast-4x4-Matrix-Inverse-with-SSE-SIMD-Explained.html
17// Variable names are adopted from \src\LU\Inverse_SSE.h.
18//
19// The SSE code for the 4x4 float and double matrix inverse in former (deprecated) \src\LU\Inverse_SSE.h
20// comes from the following Intel's library:
21// http://software.intel.com/en-us/articles/optimized-matrix-library-for-use-with-the-intel-pentiumr-4-processors-sse2-instructions/
22//
23// Here is the respective copyright and license statement:
24//
25// Copyright (c) 2001 Intel Corporation.
26//
27// Permission is granted to use, copy, distribute and prepare derivative works
28// of this library for any purpose and without fee, provided, that the above
29// copyright notice and this statement appear in all copies.
30// Intel makes no representations about the suitability of this software for
31// any purpose, and specifically disclaims all warranties.
32// See LEGAL.TXT for all the legal information.
33//
34// TODO: Unify implementations of different data types (i.e. float and double).
35// SPDX-License-Identifier: MPL-2.0
36#ifndef EIGEN_INVERSE_SIZE_4_H
37#define EIGEN_INVERSE_SIZE_4_H
38
39// IWYU pragma: private
40#include "../InternalHeaderCheck.h"
41
42#if EIGEN_COMP_GNUC_STRICT
43// These routines requires bit manipulation of the sign, which is not compatible
44// with fastmath.
45#pragma GCC push_options
46#pragma GCC optimize("no-fast-math")
47#endif
48
49namespace Eigen {
50namespace internal {
51template <typename MatrixType, typename ResultType>
52struct compute_inverse_size4<Architecture::Target, float, MatrixType, ResultType> {
53 enum {
54 MatrixAlignment = traits<MatrixType>::Alignment,
55 ResultAlignment = traits<ResultType>::Alignment,
56 StorageOrdersMatch = (MatrixType::Flags & RowMajorBit) == (ResultType::Flags & RowMajorBit)
57 };
58 using ActualMatrixType =
59 std::conditional_t<(MatrixType::Flags & LinearAccessBit), const MatrixType &, typename MatrixType::PlainObject>;
60
61 static void run(const MatrixType &mat, ResultType &result) {
62 ActualMatrixType matrix(mat);
63
64 const float *data = matrix.data();
65 const Index stride = matrix.innerStride();
66 Packet4f L1 = ploadt<Packet4f, MatrixAlignment>(data);
67 Packet4f L2 = ploadt<Packet4f, MatrixAlignment>(data + stride * 4);
68 Packet4f L3 = ploadt<Packet4f, MatrixAlignment>(data + stride * 8);
69 Packet4f L4 = ploadt<Packet4f, MatrixAlignment>(data + stride * 12);
70
71 // Four 2x2 sub-matrices of the input matrix
72 // input = [[A, B],
73 // [C, D]]
74 Packet4f A, B, C, D;
75
76 EIGEN_IF_CONSTEXPR (!StorageOrdersMatch) {
77 A = vec4f_unpacklo(L1, L2);
78 B = vec4f_unpacklo(L3, L4);
79 C = vec4f_unpackhi(L1, L2);
80 D = vec4f_unpackhi(L3, L4);
81 } else {
82 A = vec4f_movelh(L1, L2);
83 B = vec4f_movehl(L2, L1);
84 C = vec4f_movelh(L3, L4);
85 D = vec4f_movehl(L4, L3);
86 }
87
88 Packet4f AB, DC;
89
90 // AB = A# * B, where A# denotes the adjugate of A, and * denotes matrix product.
91 AB = pmul(vec4f_swizzle2(A, A, 3, 3, 0, 0), B);
92 AB = psub(AB, pmul(vec4f_swizzle2(A, A, 1, 1, 2, 2), vec4f_swizzle2(B, B, 2, 3, 0, 1)));
93
94 // DC = D#*C
95 DC = pmul(vec4f_swizzle2(D, D, 3, 3, 0, 0), C);
96 DC = psub(DC, pmul(vec4f_swizzle2(D, D, 1, 1, 2, 2), vec4f_swizzle2(C, C, 2, 3, 0, 1)));
97
98 // determinants of the sub-matrices
99 Packet4f dA, dB, dC, dD;
100
101 dA = pmul(vec4f_swizzle2(A, A, 3, 3, 1, 1), A);
102 dA = psub(dA, vec4f_movehl(dA, dA));
103
104 dB = pmul(vec4f_swizzle2(B, B, 3, 3, 1, 1), B);
105 dB = psub(dB, vec4f_movehl(dB, dB));
106
107 dC = pmul(vec4f_swizzle2(C, C, 3, 3, 1, 1), C);
108 dC = psub(dC, vec4f_movehl(dC, dC));
109
110 dD = pmul(vec4f_swizzle2(D, D, 3, 3, 1, 1), D);
111 dD = psub(dD, vec4f_movehl(dD, dD));
112
113 Packet4f d, d1, d2;
114
115 d = pmul(vec4f_swizzle2(DC, DC, 0, 2, 1, 3), AB);
116 d = padd(d, vec4f_movehl(d, d));
117 d = padd(d, vec4f_swizzle2(d, d, 1, 0, 0, 0));
118 d1 = pmul(dA, dD);
119 d2 = pmul(dB, dC);
120
121 // determinant of the input matrix, det = |A||D| + |B||C| - trace(A#*B*D#*C)
122 Packet4f det = vec4f_duplane(psub(padd(d1, d2), d), 0);
123
124 // reciprocal of the determinant of the input matrix, rd = 1/det
125 Packet4f rd = preciprocal(det);
126
127 // Four sub-matrices of the inverse
128 Packet4f iA, iB, iC, iD;
129
130 // iD = D*|A| - C*A#*B
131 iD = pmul(vec4f_swizzle2(C, C, 0, 0, 2, 2), vec4f_movelh(AB, AB));
132 iD = padd(iD, pmul(vec4f_swizzle2(C, C, 1, 1, 3, 3), vec4f_movehl(AB, AB)));
133 iD = psub(pmul(D, vec4f_duplane(dA, 0)), iD);
134
135 // iA = A*|D| - B*D#*C
136 iA = pmul(vec4f_swizzle2(B, B, 0, 0, 2, 2), vec4f_movelh(DC, DC));
137 iA = padd(iA, pmul(vec4f_swizzle2(B, B, 1, 1, 3, 3), vec4f_movehl(DC, DC)));
138 iA = psub(pmul(A, vec4f_duplane(dD, 0)), iA);
139
140 // iB = C*|B| - D * (A#B)# = C*|B| - D*B#*A
141 iB = pmul(D, vec4f_swizzle2(AB, AB, 3, 0, 3, 0));
142 iB = psub(iB, pmul(vec4f_swizzle2(D, D, 1, 0, 3, 2), vec4f_swizzle2(AB, AB, 2, 1, 2, 1)));
143 iB = psub(pmul(C, vec4f_duplane(dB, 0)), iB);
144
145 // iC = B*|C| - A * (D#C)# = B*|C| - A*C#*D
146 iC = pmul(A, vec4f_swizzle2(DC, DC, 3, 0, 3, 0));
147 iC = psub(iC, pmul(vec4f_swizzle2(A, A, 1, 0, 3, 2), vec4f_swizzle2(DC, DC, 2, 1, 2, 1)));
148 iC = psub(pmul(B, vec4f_duplane(dC, 0)), iC);
149
150 // Built from integer bits: a -0.0f literal may be replaced by +0.0f under fast-math.
151 const float neg_zero = numext::bit_cast<float>(0x80000000u);
152 EIGEN_ALIGN_TO_BOUNDARY(unpacket_traits<Packet4f>::alignment)
153 const float sign_mask[4] = {0.0f, neg_zero, neg_zero, 0.0f};
154 const Packet4f p4f_sign_PNNP = pload<Packet4f>(sign_mask);
155 rd = pxor(rd, p4f_sign_PNNP);
156 iA = pmul(iA, rd);
157 iB = pmul(iB, rd);
158 iC = pmul(iC, rd);
159 iD = pmul(iD, rd);
160
161 Index res_stride = result.outerStride();
162 float *res = result.data();
163
164 pstoret<float, Packet4f, ResultAlignment>(res + 0, vec4f_swizzle2(iA, iB, 3, 1, 3, 1));
165 pstoret<float, Packet4f, ResultAlignment>(res + res_stride, vec4f_swizzle2(iA, iB, 2, 0, 2, 0));
166 pstoret<float, Packet4f, ResultAlignment>(res + 2 * res_stride, vec4f_swizzle2(iC, iD, 3, 1, 3, 1));
167 pstoret<float, Packet4f, ResultAlignment>(res + 3 * res_stride, vec4f_swizzle2(iC, iD, 2, 0, 2, 0));
168 }
169};
170
171#if !defined(EIGEN_VECTORIZE_NEON) || EIGEN_ARCH_ARM64
172// same algorithm as above, except that each operand is split into
173// halves for two registers to hold.
174template <typename MatrixType, typename ResultType>
175struct compute_inverse_size4<Architecture::Target, double, MatrixType, ResultType> {
176 enum {
177 MatrixAlignment = traits<MatrixType>::Alignment,
178 ResultAlignment = traits<ResultType>::Alignment,
179 StorageOrdersMatch = (MatrixType::Flags & RowMajorBit) == (ResultType::Flags & RowMajorBit)
180 };
181 using ActualMatrixType =
182 std::conditional_t<(MatrixType::Flags & LinearAccessBit), const MatrixType &, typename MatrixType::PlainObject>;
183
184 static void run(const MatrixType &mat, ResultType &result) {
185 ActualMatrixType matrix(mat);
186
187 // Four 2x2 sub-matrices of the input matrix, each is further divided into upper and lower
188 // row e.g. A1, upper row of A, A2, lower row of A
189 // input = [[A, B], = [[[A1, [B1,
190 // [C, D]] A2], B2]],
191 // [[C1, [D1,
192 // C2], D2]]]
193
194 Packet2d A1, A2, B1, B2, C1, C2, D1, D2;
195
196 const double *data = matrix.data();
197 const Index stride = matrix.innerStride();
198 EIGEN_IF_CONSTEXPR (StorageOrdersMatch) {
199 A1 = ploadt<Packet2d, MatrixAlignment>(data + stride * 0);
200 B1 = ploadt<Packet2d, MatrixAlignment>(data + stride * 2);
201 A2 = ploadt<Packet2d, MatrixAlignment>(data + stride * 4);
202 B2 = ploadt<Packet2d, MatrixAlignment>(data + stride * 6);
203 C1 = ploadt<Packet2d, MatrixAlignment>(data + stride * 8);
204 D1 = ploadt<Packet2d, MatrixAlignment>(data + stride * 10);
205 C2 = ploadt<Packet2d, MatrixAlignment>(data + stride * 12);
206 D2 = ploadt<Packet2d, MatrixAlignment>(data + stride * 14);
207 } else {
208 Packet2d temp;
209 A1 = ploadt<Packet2d, MatrixAlignment>(data + stride * 0);
210 C1 = ploadt<Packet2d, MatrixAlignment>(data + stride * 2);
211 A2 = ploadt<Packet2d, MatrixAlignment>(data + stride * 4);
212 C2 = ploadt<Packet2d, MatrixAlignment>(data + stride * 6);
213 temp = A1;
214 A1 = vec2d_unpacklo(A1, A2);
215 A2 = vec2d_unpackhi(temp, A2);
216
217 temp = C1;
218 C1 = vec2d_unpacklo(C1, C2);
219 C2 = vec2d_unpackhi(temp, C2);
220
221 B1 = ploadt<Packet2d, MatrixAlignment>(data + stride * 8);
222 D1 = ploadt<Packet2d, MatrixAlignment>(data + stride * 10);
223 B2 = ploadt<Packet2d, MatrixAlignment>(data + stride * 12);
224 D2 = ploadt<Packet2d, MatrixAlignment>(data + stride * 14);
225
226 temp = B1;
227 B1 = vec2d_unpacklo(B1, B2);
228 B2 = vec2d_unpackhi(temp, B2);
229
230 temp = D1;
231 D1 = vec2d_unpacklo(D1, D2);
232 D2 = vec2d_unpackhi(temp, D2);
233 }
234
235 // determinants of the sub-matrices
236 Packet2d dA, dB, dC, dD;
237
238 dA = vec2d_swizzle2(A2, A2, 1);
239 dA = pmul(A1, dA);
240 dA = psub(dA, vec2d_duplane(dA, 1));
241
242 dB = vec2d_swizzle2(B2, B2, 1);
243 dB = pmul(B1, dB);
244 dB = psub(dB, vec2d_duplane(dB, 1));
245
246 dC = vec2d_swizzle2(C2, C2, 1);
247 dC = pmul(C1, dC);
248 dC = psub(dC, vec2d_duplane(dC, 1));
249
250 dD = vec2d_swizzle2(D2, D2, 1);
251 dD = pmul(D1, dD);
252 dD = psub(dD, vec2d_duplane(dD, 1));
253
254 Packet2d DC1, DC2, AB1, AB2;
255
256 // AB = A# * B, where A# denotes the adjugate of A, and * denotes matrix product.
257 AB1 = pmul(B1, vec2d_duplane(A2, 1));
258 AB2 = pmul(B2, vec2d_duplane(A1, 0));
259 AB1 = psub(AB1, pmul(B2, vec2d_duplane(A1, 1)));
260 AB2 = psub(AB2, pmul(B1, vec2d_duplane(A2, 0)));
261
262 // DC = D#*C
263 DC1 = pmul(C1, vec2d_duplane(D2, 1));
264 DC2 = pmul(C2, vec2d_duplane(D1, 0));
265 DC1 = psub(DC1, pmul(C2, vec2d_duplane(D1, 1)));
266 DC2 = psub(DC2, pmul(C1, vec2d_duplane(D2, 0)));
267
268 Packet2d d1, d2;
269
270 // determinant of the input matrix, det = |A||D| + |B||C| - trace(A#*B*D#*C)
271 Packet2d det;
272
273 // reciprocal of the determinant of the input matrix, rd = 1/det
274 Packet2d rd;
275
276 d1 = pmul(AB1, vec2d_swizzle2(DC1, DC2, 0));
277 d2 = pmul(AB2, vec2d_swizzle2(DC1, DC2, 3));
278 rd = padd(d1, d2);
279 rd = padd(rd, vec2d_duplane(rd, 1));
280
281 d1 = pmul(dA, dD);
282 d2 = pmul(dB, dC);
283
284 det = padd(d1, d2);
285 det = psub(det, rd);
286 det = vec2d_duplane(det, 0);
287 rd = pdiv(pset1<Packet2d>(1.0), det);
288
289 // rows of four sub-matrices of the inverse
290 Packet2d iA1, iA2, iB1, iB2, iC1, iC2, iD1, iD2;
291
292 // iD = D*|A| - C*A#*B
293 iD1 = pmul(AB1, vec2d_duplane(C1, 0));
294 iD2 = pmul(AB1, vec2d_duplane(C2, 0));
295 iD1 = padd(iD1, pmul(AB2, vec2d_duplane(C1, 1)));
296 iD2 = padd(iD2, pmul(AB2, vec2d_duplane(C2, 1)));
297 dA = vec2d_duplane(dA, 0);
298 iD1 = psub(pmul(D1, dA), iD1);
299 iD2 = psub(pmul(D2, dA), iD2);
300
301 // iA = A*|D| - B*D#*C
302 iA1 = pmul(DC1, vec2d_duplane(B1, 0));
303 iA2 = pmul(DC1, vec2d_duplane(B2, 0));
304 iA1 = padd(iA1, pmul(DC2, vec2d_duplane(B1, 1)));
305 iA2 = padd(iA2, pmul(DC2, vec2d_duplane(B2, 1)));
306 dD = vec2d_duplane(dD, 0);
307 iA1 = psub(pmul(A1, dD), iA1);
308 iA2 = psub(pmul(A2, dD), iA2);
309
310 // iB = C*|B| - D * (A#B)# = C*|B| - D*B#*A
311 iB1 = pmul(D1, vec2d_swizzle2(AB2, AB1, 1));
312 iB2 = pmul(D2, vec2d_swizzle2(AB2, AB1, 1));
313 iB1 = psub(iB1, pmul(vec2d_swizzle2(D1, D1, 1), vec2d_swizzle2(AB2, AB1, 2)));
314 iB2 = psub(iB2, pmul(vec2d_swizzle2(D2, D2, 1), vec2d_swizzle2(AB2, AB1, 2)));
315 dB = vec2d_duplane(dB, 0);
316 iB1 = psub(pmul(C1, dB), iB1);
317 iB2 = psub(pmul(C2, dB), iB2);
318
319 // iC = B*|C| - A * (D#C)# = B*|C| - A*C#*D
320 iC1 = pmul(A1, vec2d_swizzle2(DC2, DC1, 1));
321 iC2 = pmul(A2, vec2d_swizzle2(DC2, DC1, 1));
322 iC1 = psub(iC1, pmul(vec2d_swizzle2(A1, A1, 1), vec2d_swizzle2(DC2, DC1, 2)));
323 iC2 = psub(iC2, pmul(vec2d_swizzle2(A2, A2, 1), vec2d_swizzle2(DC2, DC1, 2)));
324 dC = vec2d_duplane(dC, 0);
325 iC1 = psub(pmul(B1, dC), iC1);
326 iC2 = psub(pmul(B2, dC), iC2);
327
328 // Built from integer bits: a -0.0 literal may be replaced by +0.0 under fast-math.
329 const double neg_zero = numext::bit_cast<double>(0x8000000000000000ull);
330 EIGEN_ALIGN_TO_BOUNDARY(unpacket_traits<Packet2d>::alignment) const double sign_mask1[2] = {0.0, neg_zero};
331 EIGEN_ALIGN_TO_BOUNDARY(unpacket_traits<Packet2d>::alignment) const double sign_mask2[2] = {neg_zero, 0.0};
332 const Packet2d sign_PN = pload<Packet2d>(sign_mask1);
333 const Packet2d sign_NP = pload<Packet2d>(sign_mask2);
334 d1 = pxor(rd, sign_PN);
335 d2 = pxor(rd, sign_NP);
336
337 Index res_stride = result.outerStride();
338 double *res = result.data();
339 pstoret<double, Packet2d, ResultAlignment>(res + 0, pmul(vec2d_swizzle2(iA2, iA1, 3), d1));
340 pstoret<double, Packet2d, ResultAlignment>(res + res_stride, pmul(vec2d_swizzle2(iA2, iA1, 0), d2));
341 pstoret<double, Packet2d, ResultAlignment>(res + 2, pmul(vec2d_swizzle2(iB2, iB1, 3), d1));
342 pstoret<double, Packet2d, ResultAlignment>(res + res_stride + 2, pmul(vec2d_swizzle2(iB2, iB1, 0), d2));
343 pstoret<double, Packet2d, ResultAlignment>(res + 2 * res_stride, pmul(vec2d_swizzle2(iC2, iC1, 3), d1));
344 pstoret<double, Packet2d, ResultAlignment>(res + 3 * res_stride, pmul(vec2d_swizzle2(iC2, iC1, 0), d2));
345 pstoret<double, Packet2d, ResultAlignment>(res + 2 * res_stride + 2, pmul(vec2d_swizzle2(iD2, iD1, 3), d1));
346 pstoret<double, Packet2d, ResultAlignment>(res + 3 * res_stride + 2, pmul(vec2d_swizzle2(iD2, iD1, 0), d2));
347 }
348};
349#endif
350} // namespace internal
351} // namespace Eigen
352
353#if EIGEN_COMP_GNUC_STRICT
354#pragma GCC pop_options
355#endif
356
357#endif
Eigen::LinearAccessBit
constexpr unsigned int LinearAccessBit
Definition Constants.h:134
Eigen::RowMajorBit
constexpr unsigned int RowMajorBit
Definition Constants.h:71