NimbleSM
NimbleSM is a solid mechanics simulation code for dynamic systems
Loading...
Searching...
No Matches
nimble_utils.h
Go to the documentation of this file.
1/*
2//@HEADER
3// ************************************************************************
4//
5// NimbleSM
6// Copyright 2018
7// National Technology & Engineering Solutions of Sandia, LLC (NTESS)
8//
9// Under the terms of Contract DE-NA0003525 with NTESS, the U.S. Government
10// retains certain rights in this software.
11//
12// Redistribution and use in source and binary forms, with or without
13// modification, are permitted provided that the following conditions are
14// met:
15//
16// 1. Redistributions of source code must retain the above copyright
17// notice, this list of conditions and the following disclaimer.
18//
19// 2. Redistributions in binary form must reproduce the above copyright
20// notice, this list of conditions and the following disclaimer in the
21// documentation and/or other materials provided with the distribution.
22//
23// 3. Neither the name of the Corporation nor the names of the
24// contributors may be used to endorse or promote products derived from
25// this software without specific prior written permission.
26//
27// THIS SOFTWARE IS PROVIDED BY NTESS "AS IS" AND ANY EXPRESS OR IMPLIED
28// WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
29// MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN
30// NO EVENT SHALL NTESS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
31// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
32// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
33// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
34// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
35// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
36// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
37//
38// Questions? Contact David Littlewood (djlittl@sandia.gov)
39//
40// ************************************************************************
41//@HEADER
42*/
43
44#ifndef NIMBLE_UTILS_H
45#define NIMBLE_UTILS_H
46
47#include <cmath>
48#include <iomanip>
49#include <iostream>
50#include <limits>
51#include <sstream>
52#include <stdexcept>
53#include <vector>
54
55#include "nimble_defs.h"
56#include "nimble_macros.h"
57#include "nimble_view.h"
58
59NIMBLE_INLINE_FUNCTION
60int
61StringLength(const char* str)
62{
63 int len = 0;
64 while (str[len] != '\0') { len++; }
65 return len;
66}
67
68NIMBLE_INLINE_FUNCTION
69bool
70StringsAreEqual(const char* str1, const char* str2)
71{
72 int len1 = StringLength(str1);
73 int len2 = StringLength(str2);
74 int len = len1 < len2 ? len1 : len2;
75 bool are_equal = true;
76 for (int i = 0; i < len; ++i) {
77 if (str1[i] != str2[i]) {
78 are_equal = false;
79 break;
80 }
81 }
82 return are_equal;
83}
84
85// Need to avoid conflicts with NimbleSMExtras definitions
86#ifndef K_X
87static constexpr int K_X = 0;
88static constexpr int K_Y = 1;
89static constexpr int K_Z = 2;
90
91static constexpr int K_S_XX = 0;
92static constexpr int K_S_YY = 1;
93static constexpr int K_S_ZZ = 2;
94static constexpr int K_S_XY = 3;
95static constexpr int K_S_YZ = 4;
96static constexpr int K_S_ZX = 5;
97static constexpr int K_S_YX = 3;
98static constexpr int K_S_ZY = 4;
99static constexpr int K_S_XZ = 5;
100
101static constexpr int K_F_XX = 0;
102static constexpr int K_F_YY = 1;
103static constexpr int K_F_ZZ = 2;
104static constexpr int K_F_XY = 3;
105static constexpr int K_F_YZ = 4;
106static constexpr int K_F_ZX = 5;
107static constexpr int K_F_YX = 6;
108static constexpr int K_F_ZY = 7;
109static constexpr int K_F_XZ = 8;
110#endif
111
112template <typename ScalarT>
113NIMBLE_INLINE_FUNCTION void
114CheckVectorSanity(int vec_length, ScalarT const* const vec, const char* label)
115{
116#ifdef NIMBLE_DEBUG
117 for (int i = 0; i < vec_length; i++) {
118#ifndef NIMBLE_HAVE_KOKKOS
119 if (!std::isfinite(vec[i])) {
120 NIMBLE_ABORT("\n**** Finite value check failed for " + std::string(label) + "!\n");
121 }
122#else
123 if (vec[i] != vec[i] || vec[i] > DBL_MAX || vec[i] < -DBL_MAX) {
124 printf("\n**** Error, finite value check failed for %s!\n", label);
125 }
126#endif
127 }
128#endif
129}
130
131//! Return x times sign of y
132template <typename ScalarT>
133NIMBLE_INLINE_FUNCTION ScalarT
134MultiplySign(const ScalarT x, const ScalarT y)
135{
136 return x * (1 - 2 * (y < 0));
137}
138
139template <typename ScalarT>
140NIMBLE_INLINE_FUNCTION ScalarT
141Minimum(const ScalarT x, const ScalarT y)
142{
143 return x < y ? x : y;
144}
145
146template <typename ScalarT>
147NIMBLE_INLINE_FUNCTION ScalarT
148if_then(const bool b, const ScalarT v1, const ScalarT v2)
149{
150 return b ? v1 : v2;
151}
152
153template <typename ScalarT>
154NIMBLE_INLINE_FUNCTION ScalarT
155if_then_else(const bool b, const ScalarT v1, const ScalarT v2)
156{
157 return b ? v1 : v2;
158}
159
160template <typename ScalarT>
161NIMBLE_INLINE_FUNCTION ScalarT
162if_then_else_zero(const bool b, const ScalarT v)
163{
164 return b ? v : ScalarT(0.0);
165}
166
167template <typename ScalarT>
168void
169Print_Full33(std::string label, const ScalarT* const mat, int precision = 2)
170{
171 std::cout << "Full33 " << label << std::endl;
172 std::cout << std::setprecision(precision) << " " << mat[K_F_XX] << ", " << mat[K_F_XY] << ", " << mat[K_F_XZ]
173 << std::endl;
174 std::cout << std::setprecision(precision) << " " << mat[K_F_YX] << ", " << mat[K_F_YY] << ", " << mat[K_F_YZ]
175 << std::endl;
176 std::cout << std::setprecision(precision) << " " << mat[K_F_ZX] << ", " << mat[K_F_ZY] << ", " << mat[K_F_ZZ]
177 << std::endl;
178}
179
180template <typename ScalarT>
181void
182Print_Sym33(std::string label, const ScalarT* const mat, int precision = 2)
183{
184 std::cout << "Sym33 " << label << std::endl;
185 std::cout << std::setprecision(precision) << " " << mat[K_S_XX] << ", " << mat[K_S_XY] << ", " << mat[K_S_XZ]
186 << std::endl;
187 std::cout << std::setprecision(precision) << " " << mat[K_S_YX] << ", " << mat[K_S_YY] << ", " << mat[K_S_YZ]
188 << std::endl;
189 std::cout << std::setprecision(precision) << " " << mat[K_S_ZX] << ", " << mat[K_S_ZY] << ", " << mat[K_S_ZZ]
190 << std::endl;
191}
192
193//! Compute result = mat^T mat
194template <typename ScalarT>
195NIMBLE_INLINE_FUNCTION void
196Square_Full33T_Full33(const ScalarT* const mat, ScalarT* const result)
197{
198 NIMBLE_DEBUG_ASSERT(result != mat);
199 result[K_S_XX] = mat[K_F_XX] * mat[K_F_XX] + mat[K_F_YX] * mat[K_F_YX] + mat[K_F_ZX] * mat[K_F_ZX];
200 result[K_S_YY] = mat[K_F_XY] * mat[K_F_XY] + mat[K_F_YY] * mat[K_F_YY] + mat[K_F_ZY] * mat[K_F_ZY];
201 result[K_S_ZZ] = mat[K_F_XZ] * mat[K_F_XZ] + mat[K_F_YZ] * mat[K_F_YZ] + mat[K_F_ZZ] * mat[K_F_ZZ];
202 result[K_S_XY] = mat[K_F_XX] * mat[K_F_XY] + mat[K_F_YX] * mat[K_F_YY] + mat[K_F_ZX] * mat[K_F_ZY];
203 result[K_S_YZ] = mat[K_F_XY] * mat[K_F_XZ] + mat[K_F_YY] * mat[K_F_YZ] + mat[K_F_ZY] * mat[K_F_ZZ];
204 result[K_S_ZX] = mat[K_F_XX] * mat[K_F_XZ] + mat[K_F_YX] * mat[K_F_YZ] + mat[K_F_ZX] * mat[K_F_ZZ];
205}
206
207//! Multiply a full tensor by a scalar
208template <typename ScalarT>
209NIMBLE_INLINE_FUNCTION void
210Mult_Scalar_Full33(ScalarT a, const ScalarT* const mat, ScalarT* const result)
211{
212 result[K_F_XX] = a * mat[K_F_XX];
213 result[K_F_XY] = a * mat[K_F_XY];
214 result[K_F_XZ] = a * mat[K_F_XZ];
215 result[K_F_YX] = a * mat[K_F_YX];
216 result[K_F_YY] = a * mat[K_F_YY];
217 result[K_F_YZ] = a * mat[K_F_YZ];
218 result[K_F_ZX] = a * mat[K_F_ZX];
219 result[K_F_ZY] = a * mat[K_F_ZY];
220 result[K_F_ZZ] = a * mat[K_F_ZZ];
221}
222
223//! Sum of a full tensor and a full tensor
224template <typename ScalarT>
225NIMBLE_INLINE_FUNCTION void
226Sum_Full33_Full33(const ScalarT* const A, const ScalarT* const B, ScalarT* const result)
227{
228 result[K_F_XX] = A[K_F_XX] + B[K_F_XX];
229 result[K_F_XY] = A[K_F_XY] + B[K_F_XY];
230 result[K_F_XZ] = A[K_F_XZ] + B[K_F_XZ];
231 result[K_F_YX] = A[K_F_YX] + B[K_F_YX];
232 result[K_F_YY] = A[K_F_YY] + B[K_F_YY];
233 result[K_F_YZ] = A[K_F_YZ] + B[K_F_YZ];
234 result[K_F_ZX] = A[K_F_ZX] + B[K_F_ZX];
235 result[K_F_ZY] = A[K_F_ZY] + B[K_F_ZY];
236 result[K_F_ZZ] = A[K_F_ZZ] + B[K_F_ZZ];
237}
238
239//! Sum of a symmetric tensor and a full tensor
240template <typename ScalarT>
241NIMBLE_INLINE_FUNCTION void
242Sum_Sym33_Full33(const ScalarT* const A, const ScalarT* const B, ScalarT* const result)
243{
244 result[K_F_XX] = A[K_S_XX] + B[K_F_XX];
245 result[K_F_XY] = A[K_S_XY] + B[K_F_XY];
246 result[K_F_XZ] = A[K_S_XZ] + B[K_F_XZ];
247 result[K_F_YX] = A[K_S_YX] + B[K_F_YX];
248 result[K_F_YY] = A[K_S_YY] + B[K_F_YY];
249 result[K_F_YZ] = A[K_S_YZ] + B[K_F_YZ];
250 result[K_F_ZX] = A[K_S_ZX] + B[K_F_ZX];
251 result[K_F_ZY] = A[K_S_ZY] + B[K_F_ZY];
252 result[K_F_ZZ] = A[K_S_ZZ] + B[K_F_ZZ];
253}
254
255//! Multiply a full tensor by a full tensor
256template <typename ScalarT>
257NIMBLE_INLINE_FUNCTION void
258Mult_Full33_Full33(const ScalarT* const A, const ScalarT* const B, ScalarT* const result)
259{
260 NIMBLE_DEBUG_ASSERT(A != result);
261 NIMBLE_DEBUG_ASSERT(B != result);
262 result[K_F_XX] = A[K_F_XX] * B[K_F_XX] + A[K_F_XY] * B[K_F_YX] + A[K_F_XZ] * B[K_F_ZX];
263 result[K_F_XY] = A[K_F_XX] * B[K_F_XY] + A[K_F_XY] * B[K_F_YY] + A[K_F_XZ] * B[K_F_ZY];
264 result[K_F_XZ] = A[K_F_XX] * B[K_F_XZ] + A[K_F_XY] * B[K_F_YZ] + A[K_F_XZ] * B[K_F_ZZ];
265 result[K_F_YX] = A[K_F_YX] * B[K_F_XX] + A[K_F_YY] * B[K_F_YX] + A[K_F_YZ] * B[K_F_ZX];
266 result[K_F_YY] = A[K_F_YX] * B[K_F_XY] + A[K_F_YY] * B[K_F_YY] + A[K_F_YZ] * B[K_F_ZY];
267 result[K_F_YZ] = A[K_F_YX] * B[K_F_XZ] + A[K_F_YY] * B[K_F_YZ] + A[K_F_YZ] * B[K_F_ZZ];
268 result[K_F_ZX] = A[K_F_ZX] * B[K_F_XX] + A[K_F_ZY] * B[K_F_YX] + A[K_F_ZZ] * B[K_F_ZX];
269 result[K_F_ZY] = A[K_F_ZX] * B[K_F_XY] + A[K_F_ZY] * B[K_F_YY] + A[K_F_ZZ] * B[K_F_ZY];
270 result[K_F_ZZ] = A[K_F_ZX] * B[K_F_XZ] + A[K_F_ZY] * B[K_F_YZ] + A[K_F_ZZ] * B[K_F_ZZ];
271}
272
273//! Multiply a symmetric tensor by a full tensor
274template <typename ScalarT>
275NIMBLE_INLINE_FUNCTION void
276Mult_Sym33_Full33(const ScalarT* const sym, const ScalarT* const full, ScalarT* const result)
277{
278 NIMBLE_DEBUG_ASSERT(sym != result);
279 NIMBLE_DEBUG_ASSERT(full != result);
280 result[K_F_XX] = sym[K_S_XX] * full[K_F_XX] + sym[K_S_XY] * full[K_F_YX] + sym[K_S_XZ] * full[K_F_ZX];
281 result[K_F_XY] = sym[K_S_XX] * full[K_F_XY] + sym[K_S_XY] * full[K_F_YY] + sym[K_S_XZ] * full[K_F_ZY];
282 result[K_F_XZ] = sym[K_S_XX] * full[K_F_XZ] + sym[K_S_XY] * full[K_F_YZ] + sym[K_S_XZ] * full[K_F_ZZ];
283 result[K_F_YX] = sym[K_S_YX] * full[K_F_XX] + sym[K_S_YY] * full[K_F_YX] + sym[K_S_YZ] * full[K_F_ZX];
284 result[K_F_YY] = sym[K_S_YX] * full[K_F_XY] + sym[K_S_YY] * full[K_F_YY] + sym[K_S_YZ] * full[K_F_ZY];
285 result[K_F_YZ] = sym[K_S_YX] * full[K_F_XZ] + sym[K_S_YY] * full[K_F_YZ] + sym[K_S_YZ] * full[K_F_ZZ];
286 result[K_F_ZX] = sym[K_S_ZX] * full[K_F_XX] + sym[K_S_ZY] * full[K_F_YX] + sym[K_S_ZZ] * full[K_F_ZX];
287 result[K_F_ZY] = sym[K_S_ZX] * full[K_F_XY] + sym[K_S_ZY] * full[K_F_YY] + sym[K_S_ZZ] * full[K_F_ZY];
288 result[K_F_ZZ] = sym[K_S_ZX] * full[K_F_XZ] + sym[K_S_ZY] * full[K_F_YZ] + sym[K_S_ZZ] * full[K_F_ZZ];
289}
290
291//! Multiply a full tensor by a full tensor and a scalar
292template <typename ScalarT>
293NIMBLE_INLINE_FUNCTION void
294Mult_Scalar_Full33_Full33(ScalarT alpha, const ScalarT* const A, const ScalarT* const B, ScalarT* const result)
295{
296 NIMBLE_DEBUG_ASSERT(A != result);
297 NIMBLE_DEBUG_ASSERT(B != result);
298 result[K_F_XX] = alpha * (A[K_F_XX] * B[K_F_XX] + A[K_F_XY] * B[K_F_YX] + A[K_F_XZ] * B[K_F_ZX]);
299 result[K_F_XY] = alpha * (A[K_F_XX] * B[K_F_XY] + A[K_F_XY] * B[K_F_YY] + A[K_F_XZ] * B[K_F_ZY]);
300 result[K_F_XZ] = alpha * (A[K_F_XX] * B[K_F_XZ] + A[K_F_XY] * B[K_F_YZ] + A[K_F_XZ] * B[K_F_ZZ]);
301 result[K_F_YX] = alpha * (A[K_F_YX] * B[K_F_XX] + A[K_F_YY] * B[K_F_YX] + A[K_F_YZ] * B[K_F_ZX]);
302 result[K_F_YY] = alpha * (A[K_F_YX] * B[K_F_XY] + A[K_F_YY] * B[K_F_YY] + A[K_F_YZ] * B[K_F_ZY]);
303 result[K_F_YZ] = alpha * (A[K_F_YX] * B[K_F_XZ] + A[K_F_YY] * B[K_F_YZ] + A[K_F_YZ] * B[K_F_ZZ]);
304 result[K_F_ZX] = alpha * (A[K_F_ZX] * B[K_F_XX] + A[K_F_ZY] * B[K_F_YX] + A[K_F_ZZ] * B[K_F_ZX]);
305 result[K_F_ZY] = alpha * (A[K_F_ZX] * B[K_F_XY] + A[K_F_ZY] * B[K_F_YY] + A[K_F_ZZ] * B[K_F_ZY]);
306 result[K_F_ZZ] = alpha * (A[K_F_ZX] * B[K_F_XZ] + A[K_F_ZY] * B[K_F_YZ] + A[K_F_ZZ] * B[K_F_ZZ]);
307}
308
309//! Compute result = mat mat^T
310template <typename ScalarT>
311NIMBLE_INLINE_FUNCTION void
312Square_Full33_Full33T(const ScalarT* const mat, ScalarT* const result)
313{
314 NIMBLE_DEBUG_ASSERT(result != mat);
315 result[K_S_XX] = mat[K_F_XX] * mat[K_F_XX] + mat[K_F_XY] * mat[K_F_XY] + mat[K_F_XZ] * mat[K_F_XZ];
316 result[K_S_YY] = mat[K_F_YX] * mat[K_F_YX] + mat[K_F_YY] * mat[K_F_YY] + mat[K_F_YZ] * mat[K_F_YZ];
317 result[K_S_ZZ] = mat[K_F_ZX] * mat[K_F_ZX] + mat[K_F_ZY] * mat[K_F_ZY] + mat[K_F_ZZ] * mat[K_F_ZZ];
318 result[K_S_XY] = mat[K_F_XX] * mat[K_F_YX] + mat[K_F_XY] * mat[K_F_YY] + mat[K_F_XZ] * mat[K_F_YZ];
319 result[K_S_YZ] = mat[K_F_YX] * mat[K_F_ZX] + mat[K_F_YY] * mat[K_F_ZY] + mat[K_F_YZ] * mat[K_F_ZZ];
320 result[K_S_ZX] = mat[K_F_ZX] * mat[K_F_XX] + mat[K_F_ZY] * mat[K_F_XY] + mat[K_F_ZZ] * mat[K_F_XZ];
321}
322
323//! Multiply a full tensor by a symmetric tensor, return transpose of resultant
324template <typename ScalarT>
325NIMBLE_INLINE_FUNCTION void
326Mult_Full33_Sym33_ReturnT(const ScalarT* const full, const ScalarT* const sym, ScalarT* const result)
327{
328 NIMBLE_DEBUG_ASSERT(full != result);
329 NIMBLE_DEBUG_ASSERT(sym != result);
330 result[K_F_XX] = full[K_F_XX] * sym[K_S_XX] + full[K_F_XY] * sym[K_S_YX] + full[K_F_XZ] * sym[K_S_ZX];
331 result[K_F_YX] = full[K_F_XX] * sym[K_S_XY] + full[K_F_XY] * sym[K_S_YY] + full[K_F_XZ] * sym[K_S_ZY];
332 result[K_F_ZX] = full[K_F_XX] * sym[K_S_XZ] + full[K_F_XY] * sym[K_S_YZ] + full[K_F_XZ] * sym[K_S_ZZ];
333 result[K_F_XY] = full[K_F_YX] * sym[K_S_XX] + full[K_F_YY] * sym[K_S_YX] + full[K_F_YZ] * sym[K_S_ZX];
334 result[K_F_YY] = full[K_F_YX] * sym[K_S_XY] + full[K_F_YY] * sym[K_S_YY] + full[K_F_YZ] * sym[K_S_ZY];
335 result[K_F_ZY] = full[K_F_YX] * sym[K_S_XZ] + full[K_F_YY] * sym[K_S_YZ] + full[K_F_YZ] * sym[K_S_ZZ];
336 result[K_F_XZ] = full[K_F_ZX] * sym[K_S_XX] + full[K_F_ZY] * sym[K_S_YX] + full[K_F_ZZ] * sym[K_S_ZX];
337 result[K_F_YZ] = full[K_F_ZX] * sym[K_S_XY] + full[K_F_ZY] * sym[K_S_YY] + full[K_F_ZZ] * sym[K_S_ZY];
338 result[K_F_ZZ] = full[K_F_ZX] * sym[K_S_XZ] + full[K_F_ZY] * sym[K_S_YZ] + full[K_F_ZZ] * sym[K_S_ZZ];
339}
340
341//! Rotate a symetric tensor, result = R^T S R
342template <typename ScalarT>
343NIMBLE_INLINE_FUNCTION void
344Rotate_Sym33_Using_Rtranspose_S_R(const ScalarT* const s, const ScalarT* const r, ScalarT* const result)
345{
346 NIMBLE_DEBUG_ASSERT(s != result);
347 NIMBLE_DEBUG_ASSERT(r != result);
348
349 ScalarT temp_xx = s[K_S_XX] * r[K_F_XX] + s[K_S_XY] * r[K_F_YX] + s[K_S_XZ] * r[K_F_ZX];
350 ScalarT temp_yx = s[K_S_YX] * r[K_F_XX] + s[K_S_YY] * r[K_F_YX] + s[K_S_YZ] * r[K_F_ZX];
351 ScalarT temp_zx = s[K_S_ZX] * r[K_F_XX] + s[K_S_ZY] * r[K_F_YX] + s[K_S_ZZ] * r[K_F_ZX];
352 ScalarT temp_xy = s[K_S_XX] * r[K_F_XY] + s[K_S_XY] * r[K_F_YY] + s[K_S_XZ] * r[K_F_ZY];
353 ScalarT temp_yy = s[K_S_YX] * r[K_F_XY] + s[K_S_YY] * r[K_F_YY] + s[K_S_YZ] * r[K_F_ZY];
354 ScalarT temp_zy = s[K_S_ZX] * r[K_F_XY] + s[K_S_ZY] * r[K_F_YY] + s[K_S_ZZ] * r[K_F_ZY];
355 ScalarT temp_xz = s[K_S_XX] * r[K_F_XZ] + s[K_S_XY] * r[K_F_YZ] + s[K_S_XZ] * r[K_F_ZZ];
356 ScalarT temp_yz = s[K_S_YX] * r[K_F_XZ] + s[K_S_YY] * r[K_F_YZ] + s[K_S_YZ] * r[K_F_ZZ];
357 ScalarT temp_zz = s[K_S_ZX] * r[K_F_XZ] + s[K_S_ZY] * r[K_F_YZ] + s[K_S_ZZ] * r[K_F_ZZ];
358
359 result[K_S_XX] = r[K_F_XX] * temp_xx + r[K_F_YX] * temp_yx + r[K_F_ZX] * temp_zx;
360 result[K_S_YY] = r[K_F_XY] * temp_xy + r[K_F_YY] * temp_yy + r[K_F_ZY] * temp_zy;
361 result[K_S_ZZ] = r[K_F_XZ] * temp_xz + r[K_F_YZ] * temp_yz + r[K_F_ZZ] * temp_zz;
362 result[K_S_XY] = r[K_F_XX] * temp_xy + r[K_F_YX] * temp_yy + r[K_F_ZX] * temp_zy;
363 result[K_S_YZ] = r[K_F_XY] * temp_xz + r[K_F_YY] * temp_yz + r[K_F_ZY] * temp_zz;
364 result[K_S_ZX] = r[K_F_XZ] * temp_xx + r[K_F_YZ] * temp_yx + r[K_F_ZZ] * temp_zx;
365}
366
367//! Unrotate a symetric tensor, result = R S R^T
368template <typename ScalarT>
369NIMBLE_INLINE_FUNCTION void
370Unrotate_Sym33_Using_R_S_Rtranspose(const ScalarT* const s, const ScalarT* const r, ScalarT* const result)
371{
372 NIMBLE_DEBUG_ASSERT(s != result);
373 NIMBLE_DEBUG_ASSERT(r != result);
374
375 ScalarT temp_xx = s[K_S_XX] * r[K_F_XX] + s[K_S_XY] * r[K_F_XY] + s[K_S_XZ] * r[K_F_XZ];
376 ScalarT temp_yx = s[K_S_YX] * r[K_F_XX] + s[K_S_YY] * r[K_F_XY] + s[K_S_YZ] * r[K_F_XZ];
377 ScalarT temp_zx = s[K_S_ZX] * r[K_F_XX] + s[K_S_ZY] * r[K_F_XY] + s[K_S_ZZ] * r[K_F_XZ];
378 ScalarT temp_xy = s[K_S_XX] * r[K_F_YX] + s[K_S_XY] * r[K_F_YY] + s[K_S_XZ] * r[K_F_YZ];
379 ScalarT temp_yy = s[K_S_YX] * r[K_F_YX] + s[K_S_YY] * r[K_F_YY] + s[K_S_YZ] * r[K_F_YZ];
380 ScalarT temp_zy = s[K_S_ZX] * r[K_F_YX] + s[K_S_ZY] * r[K_F_YY] + s[K_S_ZZ] * r[K_F_YZ];
381 ScalarT temp_xz = s[K_S_XX] * r[K_F_ZX] + s[K_S_XY] * r[K_F_ZY] + s[K_S_XZ] * r[K_F_ZZ];
382 ScalarT temp_yz = s[K_S_YX] * r[K_F_ZX] + s[K_S_YY] * r[K_F_ZY] + s[K_S_YZ] * r[K_F_ZZ];
383 ScalarT temp_zz = s[K_S_ZX] * r[K_F_ZX] + s[K_S_ZY] * r[K_F_ZY] + s[K_S_ZZ] * r[K_F_ZZ];
384
385 result[K_S_XX] = r[K_F_XX] * temp_xx + r[K_F_XY] * temp_yx + r[K_F_XZ] * temp_zx;
386 result[K_S_YY] = r[K_F_YX] * temp_xy + r[K_F_YY] * temp_yy + r[K_F_YZ] * temp_zy;
387 result[K_S_ZZ] = r[K_F_ZX] * temp_xz + r[K_F_ZY] * temp_yz + r[K_F_ZZ] * temp_zz;
388 result[K_S_XY] = r[K_F_XX] * temp_xy + r[K_F_XY] * temp_yy + r[K_F_XZ] * temp_zy;
389 result[K_S_YZ] = r[K_F_YX] * temp_xz + r[K_F_YY] * temp_yz + r[K_F_YZ] * temp_zz;
390 result[K_S_ZX] = r[K_F_ZX] * temp_xx + r[K_F_ZY] * temp_yx + r[K_F_ZZ] * temp_zx;
391}
392
393template <typename ScalarT>
394NIMBLE_INLINE_FUNCTION void
395CrossProduct(const ScalarT* const u, const ScalarT* const v, ScalarT* const result)
396{
397 result[0] = u[1] * v[2] - u[2] * v[1];
398 result[1] = u[2] * v[0] - u[0] * v[2];
399 result[2] = u[0] * v[1] - u[1] * v[0];
400}
401
402template <typename ScalarT>
403NIMBLE_INLINE_FUNCTION ScalarT
404Determinant_Full33(const ScalarT* const mat)
405{
406 ScalarT minor0 = mat[K_F_YY] * mat[K_F_ZZ] - mat[K_F_YZ] * mat[K_F_ZY];
407 ScalarT minor1 = mat[K_F_YX] * mat[K_F_ZZ] - mat[K_F_YZ] * mat[K_F_ZX];
408 ScalarT minor2 = mat[K_F_YX] * mat[K_F_ZY] - mat[K_F_YY] * mat[K_F_ZX];
409 ScalarT det = mat[K_F_XX] * minor0 - mat[K_F_XY] * minor1 + mat[K_F_XZ] * minor2;
410
411 return det;
412}
413
414template <typename ScalarT>
415NIMBLE_INLINE_FUNCTION ScalarT
416Norm_Sym33(const ScalarT* const mat)
417{
418 ScalarT norm = 0.0;
419 norm += mat[K_S_XX] * mat[K_S_XX] + mat[K_S_XY] * mat[K_S_XY] + mat[K_S_XZ] * mat[K_S_XZ];
420 norm += mat[K_S_YX] * mat[K_S_YX] + mat[K_S_YY] * mat[K_S_YY] + mat[K_S_YZ] * mat[K_S_YZ];
421 norm += mat[K_S_ZX] * mat[K_S_ZX] + mat[K_S_ZY] * mat[K_S_ZY] + mat[K_S_ZZ] * mat[K_S_ZZ];
422
423 if (norm > 0.0) { norm = std::sqrt(norm); }
424
425 return norm;
426}
427
428template <typename ScalarT>
429NIMBLE_INLINE_FUNCTION ScalarT
430Norm_Full33(const ScalarT* const mat)
431{
432 ScalarT norm = 0.0;
433 norm += mat[K_F_XX] * mat[K_F_XX] + mat[K_F_XY] * mat[K_F_XY] + mat[K_F_XZ] * mat[K_F_XZ];
434 norm += mat[K_F_YX] * mat[K_F_YX] + mat[K_F_YY] * mat[K_F_YY] + mat[K_F_YZ] * mat[K_F_YZ];
435 norm += mat[K_F_ZX] * mat[K_F_ZX] + mat[K_F_ZY] * mat[K_F_ZY] + mat[K_F_ZZ] * mat[K_F_ZZ];
436
437 if (norm > 0.0) { norm = std::sqrt(norm); }
438
439 return norm;
440}
441
442template <typename ScalarT>
443NIMBLE_INLINE_FUNCTION void
444Zero_Full33(ScalarT* const mat)
445{
446 mat[K_F_XX] = 0.0;
447 mat[K_F_XY] = 0.0;
448 mat[K_F_XZ] = 0.0;
449 mat[K_F_YX] = 0.0;
450 mat[K_F_YY] = 0.0;
451 mat[K_F_YZ] = 0.0;
452 mat[K_F_ZX] = 0.0;
453 mat[K_F_ZY] = 0.0;
454 mat[K_F_ZZ] = 0.0;
455}
456
457template <typename ScalarT>
458NIMBLE_INLINE_FUNCTION void
459SetEqual_Full33(const ScalarT* const B, ScalarT* const A)
460{
461 A[K_F_XX] = B[K_F_XX];
462 A[K_F_XY] = B[K_F_XY];
463 A[K_F_XZ] = B[K_F_XZ];
464 A[K_F_YX] = B[K_F_YX];
465 A[K_F_YY] = B[K_F_YY];
466 A[K_F_YZ] = B[K_F_YZ];
467 A[K_F_ZX] = B[K_F_ZX];
468 A[K_F_ZY] = B[K_F_ZY];
469 A[K_F_ZZ] = B[K_F_ZZ];
470}
471
472template <typename ScalarT>
473NIMBLE_INLINE_FUNCTION void
474SetEqual_Sym33(const ScalarT* const B, ScalarT* const A)
475{
476 A[K_S_XX] = B[K_S_XX];
477 A[K_S_YY] = B[K_S_YY];
478 A[K_S_ZZ] = B[K_S_ZZ];
479 A[K_S_XY] = B[K_S_XY];
480 A[K_S_YZ] = B[K_S_YZ];
481 A[K_S_ZX] = B[K_S_ZX];
482}
483
484template <typename ScalarT>
485NIMBLE_INLINE_FUNCTION void
486SymPart_Full33(const ScalarT* const mat, ScalarT* const result)
487{
488 result[K_S_XX] = mat[K_F_XX];
489 result[K_S_YY] = mat[K_F_YY];
490 result[K_S_ZZ] = mat[K_F_ZZ];
491 result[K_S_XY] = 0.5 * (mat[K_F_XY] + mat[K_F_YX]);
492 result[K_S_YZ] = 0.5 * (mat[K_F_YZ] + mat[K_F_ZY]);
493 result[K_S_ZX] = 0.5 * (mat[K_F_ZX] + mat[K_F_XZ]);
494}
495
496template <typename ScalarT>
497NIMBLE_INLINE_FUNCTION void
498SkewPart_Full33(const ScalarT* const mat, ScalarT* const result)
499{
500 result[K_F_XX] = 0.0;
501 result[K_F_XY] = 0.5 * (mat[K_F_XY] - mat[K_F_YX]);
502 result[K_F_XZ] = 0.5 * (mat[K_F_XZ] - mat[K_F_ZX]);
503 result[K_F_YX] = 0.5 * (mat[K_F_YX] - mat[K_F_XY]);
504 result[K_F_YY] = 0.0;
505 result[K_F_YZ] = 0.5 * (mat[K_F_YZ] - mat[K_F_ZY]);
506 result[K_F_ZX] = 0.5 * (mat[K_F_ZX] - mat[K_F_XZ]);
507 result[K_F_ZY] = 0.5 * (mat[K_F_ZY] - mat[K_F_YZ]);
508 result[K_F_ZZ] = 0.0;
509}
510
511/// \brief Function to invert a 3x3 matrix
512///
513/// \tparam ScalarT Scalar values
514/// \param mat Pointer to 3x3 matrix with a storage based on the indices K_F_**
515/// \param inv Pointer to 3x3 mqtrix with a storage based on the indices K_F_**
516/// \return Determinant
517/// \remark The matrix is has the pattern
518/// [ K_F_XX K_F_XY K_F_XZ ]
519/// [ K_F_YX K_F_YY K_F_YZ ]
520/// [ K_F_ZX K_F_ZY K_F_ZZ ]
521/// but note that K_F_YY = 1 and K_F_ZZ = 2
522/// The storage proceeds along diagonals.
523template <typename ScalarT>
524NIMBLE_INLINE_FUNCTION ScalarT
525Invert_Full33(const ScalarT* mat, ScalarT* inv)
526{
527 ScalarT minor0 = mat[K_F_YY] * mat[K_F_ZZ] - mat[K_F_YZ] * mat[K_F_ZY];
528 ScalarT minor1 = mat[K_F_YX] * mat[K_F_ZZ] - mat[K_F_YZ] * mat[K_F_ZX];
529 ScalarT minor2 = mat[K_F_YX] * mat[K_F_ZY] - mat[K_F_YY] * mat[K_F_ZX];
530 ScalarT minor3 = mat[K_F_XY] * mat[K_F_ZZ] - mat[K_F_XZ] * mat[K_F_ZY];
531 ScalarT minor4 = mat[K_F_XX] * mat[K_F_ZZ] - mat[K_F_ZX] * mat[K_F_XZ];
532 ScalarT minor5 = mat[K_F_XX] * mat[K_F_ZY] - mat[K_F_XY] * mat[K_F_ZX];
533 ScalarT minor6 = mat[K_F_XY] * mat[K_F_YZ] - mat[K_F_XZ] * mat[K_F_YY];
534 ScalarT minor7 = mat[K_F_XX] * mat[K_F_YZ] - mat[K_F_XZ] * mat[K_F_YX];
535 ScalarT minor8 = mat[K_F_XX] * mat[K_F_YY] - mat[K_F_XY] * mat[K_F_YX];
536 ScalarT det = mat[K_F_XX] * minor0 - mat[K_F_XY] * minor1 + mat[K_F_XZ] * minor2;
537
538 NIMBLE_ASSERT(det >= 0.0, "Error in Invert_Full33(), singular matrix");
539
540 inv[K_F_XX] = minor0 / det;
541 inv[K_F_XY] = -1.0 * minor3 / det;
542 inv[K_F_XZ] = minor6 / det;
543 inv[K_F_YX] = -1.0 * minor1 / det;
544 inv[K_F_YY] = minor4 / det;
545 inv[K_F_YZ] = -1.0 * minor7 / det;
546 inv[K_F_ZX] = minor2 / det;
547 inv[K_F_ZY] = -1.0 * minor5 / det;
548 inv[K_F_ZZ] = minor8 / det;
549
550 return det;
551}
552
553//! R^N logarithmic map using Baker-Campbell-Hausdorff (BCH) expansion (4 terms)
554// All arguments are assumed to be full tensors (9 components)
555template <typename ScalarT>
556NIMBLE_INLINE_FUNCTION void
557BCH(const ScalarT* const x, const ScalarT* const y, ScalarT* const result)
558{
559 // adapted from MiniTensor_LinearAlgebra.t.h bch()
560
561 ScalarT temp1[9], temp2[9], temp3[9];
562
563 // first order term
564 // x + y
565 Sum_Full33_Full33(x, y, result);
566
567 // second order term
568 // 0.5*(x*y - y*x)
569 Mult_Scalar_Full33_Full33(0.5, x, y, temp1);
570 Mult_Scalar_Full33_Full33(-0.5, y, x, temp2);
571 Sum_Full33_Full33(result, temp1, result);
572 Sum_Full33_Full33(result, temp2, result);
573
574 // third order term
575 // 1.0/12.0 * (x*x*y - 2.0*x*y*x + x*y*y + y*x*x - 2.0*y*x*y + y*y*x)
576 // 1/12 * x*x*y
577 Mult_Full33_Full33(x, y, temp1);
578 Mult_Scalar_Full33_Full33(1.0 / 12.0, x, temp1, temp2);
579 Sum_Full33_Full33(result, temp2, result);
580 // -1/6 * x*y*x
581 Mult_Full33_Full33(y, x, temp1);
582 Mult_Scalar_Full33_Full33(-1.0 / 6.0, x, temp1, temp2);
583 Sum_Full33_Full33(result, temp2, result);
584 // 1/12 * x*y*y
585 Mult_Full33_Full33(y, y, temp1);
586 Mult_Scalar_Full33_Full33(1.0 / 12.0, x, temp1, temp2);
587 Sum_Full33_Full33(result, temp2, result);
588 // 1/12 * y*x*x
589 Mult_Full33_Full33(x, x, temp1);
590 Mult_Scalar_Full33_Full33(1.0 / 12.0, y, temp1, temp2);
591 Sum_Full33_Full33(result, temp2, result);
592 // -1/6 * y*x*y
593 Mult_Full33_Full33(x, y, temp1);
594 Mult_Scalar_Full33_Full33(-1.0 / 6.0, y, temp1, temp2);
595 Sum_Full33_Full33(result, temp2, result);
596 // 1/12 * y*y*x
597 Mult_Full33_Full33(y, x, temp1);
598 Mult_Scalar_Full33_Full33(1.0 / 12.0, y, temp1, temp2);
599 Sum_Full33_Full33(result, temp2, result);
600
601 // fourth order term
602 // 1.0/24.0 * (x*x*y*y - 2.0*x*y*x*y + 2.0*y*x*y*x - y*y*x*x);
603 // 1.0/24.0 * x*x*y*y
604 Mult_Full33_Full33(y, y, temp1);
605 Mult_Full33_Full33(x, temp1, temp2);
606 Mult_Scalar_Full33_Full33(1.0 / 24.0, x, temp2, temp3);
607 Sum_Full33_Full33(result, temp3, result);
608 // -2.0/24.0 * x*y*x*y
609 Mult_Full33_Full33(x, y, temp1);
610 Mult_Full33_Full33(y, temp1, temp2);
611 Mult_Scalar_Full33_Full33(-2.0 / 24.0, x, temp2, temp3);
612 Sum_Full33_Full33(result, temp3, result);
613 // 2.0/24.0 * y*x*y*x
614 Mult_Full33_Full33(y, x, temp1);
615 Mult_Full33_Full33(x, temp1, temp2);
616 Mult_Scalar_Full33_Full33(2.0 / 24.0, y, temp2, temp3);
617 Sum_Full33_Full33(result, temp3, result);
618 // -1.0/24.0 * y*y*x*x
619 Mult_Full33_Full33(x, x, temp1);
620 Mult_Full33_Full33(y, temp1, temp2);
621 Mult_Scalar_Full33_Full33(-1.0 / 24.0, y, temp2, temp3);
622 Sum_Full33_Full33(result, temp3, result);
623}
624
625//! R^N logarithmic map using Baker-Campbell-Hausdorff (BCH) expansion (4 terms)
626// All arguments are assumed to be full tensors (9 components)
627template <typename ScalarT>
628NIMBLE_INLINE_FUNCTION void
629BCH_Sym33_Full33(const ScalarT* const sym, const ScalarT* const full, ScalarT* const result)
630{
631 ScalarT temp[9];
632 temp[K_F_XX] = sym[K_S_XX];
633 temp[K_F_XY] = sym[K_S_XY];
634 temp[K_F_XZ] = sym[K_S_XZ];
635 temp[K_F_YX] = sym[K_S_YX];
636 temp[K_F_YY] = sym[K_S_YY];
637 temp[K_F_YZ] = sym[K_S_YZ];
638 temp[K_F_ZX] = sym[K_S_ZX];
639 temp[K_F_ZY] = sym[K_S_ZY];
640 temp[K_F_ZZ] = sym[K_S_ZZ];
641 BCH(temp, full, result);
642}
643
644//! Pade approximation to cos( acos(x)/3 )
645// Mathematica commands:
646// Needs["FunctionApproximations`"]
647// r1 = MiniMaxApproximation[Cos[ArcCos[x]/3], {x, {0, 1}, 6, 5},
648// WorkingPrecision -> 18, MaxIterations -> 500] 6 and 5 indicate the polynomial
649// order in the numerator and denominator.
650template <typename ScalarT>
651NIMBLE_INLINE_FUNCTION ScalarT
652Cos_Of_Acos_Divided_By_3(const ScalarT x)
653{
654 // algorithm adapted from Math_Trig.h
655
656 const ScalarT x2 = x * x;
657 const ScalarT x4 = x2 * x2;
658
659 return (0.866025403784438713 + 2.12714890259493060 * x +
660 ((1.89202064815951569 + 0.739603278343401613 * x) * x2 +
661 (0.121973926953064794 + x * (0.00655637626263929360 + 0.0000390884982780803443 * x)) * x4)) /
662 (1.0 + 2.26376989330935617 * x +
663 ((1.80461009751278976 + 0.603976798217196003 * x) * x2 +
664 (0.0783255761115461708 + 0.00268525944538021629 * x) * x4));
665}
666
667template <typename ScalarT>
668NIMBLE_INLINE_FUNCTION void
669Eigen_Sym33_NonUnit(
670 const ScalarT* const tensor,
671 ScalarT& eval0,
672 ScalarT& eval1,
673 ScalarT& eval2,
674 ScalarT* const evec0,
675 ScalarT* const evec1,
676 ScalarT* const evec2)
677{
678 ScalarT cxx = tensor[K_S_XX];
679 ScalarT cyy = tensor[K_S_YY];
680 ScalarT czz = tensor[K_S_ZZ];
681 const ScalarT& cxy = tensor[K_S_XY];
682 const ScalarT& cyz = tensor[K_S_YZ];
683 const ScalarT& czx = tensor[K_S_ZX];
684
685 const ScalarT c1 = (cxx + cyy + czz) / ScalarT(3.0);
686
687 cxx -= c1;
688 cyy -= c1;
689 czz -= c1;
690
691 const ScalarT cxy_cxy = cxy * cxy;
692 const ScalarT cyz_cyz = cyz * cyz;
693 const ScalarT czx_czx = czx * czx;
694 const ScalarT cxx_cyy = cxx * cyy;
695
696 const ScalarT c2 = cxx_cyy + cyy * czz + czz * cxx - cxy_cxy - cyz_cyz - czx_czx;
697
698 const ScalarT ThreeOverA = ScalarT(-3.0) / c2;
699 const ScalarT sqrtThreeOverA = std::sqrt(ThreeOverA);
700
701 const ScalarT c3 = cxx * cyz_cyz + cyy * czx_czx - ScalarT(2.0) * cxy * cyz * czx + czz * (cxy_cxy - cxx_cyy);
702
703 const ScalarT rr = ScalarT(-0.5) * c3 * ThreeOverA * sqrtThreeOverA;
704
705 const ScalarT arg = Minimum(std::abs(rr), ScalarT(1.0));
706
707 const ScalarT cos_thd3 = Cos_Of_Acos_Divided_By_3(arg);
708
709 const ScalarT two_cos_thd3 = ScalarT(2.0) * MultiplySign(cos_thd3, rr);
710
711 eval2 = two_cos_thd3 / sqrtThreeOverA;
712
713 ScalarT crow0[3] = {cxx - eval2, cxy, czx};
714 ScalarT crow1[3] = {cxy, cyy - eval2, cyz};
715 ScalarT crow2[3] = {czx, cyz, czz - eval2};
716
717 //
718 // do QR decomposition with column pivoting
719 //
720 const ScalarT k0 = crow0[0] * crow0[0] + cxy_cxy + czx_czx;
721 const ScalarT k1 = cxy_cxy + crow1[1] * crow1[1] + cyz_cyz;
722 const ScalarT k2 = czx_czx + cyz_cyz + crow2[2] * crow2[2];
723
724 ScalarT k_row1[3];
725 ScalarT row2[3];
726 ScalarT row3[3];
727
728 // returns zero or nan
729 const bool k0gk1 = k1 <= k0;
730 const bool k0gk2 = k2 <= k0;
731 const bool k1gk2 = k2 <= k1;
732
733 const bool k0_largest = k0gk1 && k0gk2;
734 const bool k1_largest = k1gk2 && !k0gk1;
735 const bool k2_largest = !(k0_largest || k1_largest);
736
737 k_row1[0] = if_then_else_zero(k0_largest, crow0[0]) + if_then_else_zero(k1_largest, crow1[0]) +
738 if_then_else_zero(k2_largest, crow2[0]);
739
740 k_row1[1] = if_then_else_zero(k0_largest, crow0[1]) + if_then_else_zero(k1_largest, crow1[1]) +
741 if_then_else_zero(k2_largest, crow2[1]);
742
743 k_row1[2] = if_then_else_zero(k0_largest, crow0[2]) + if_then_else_zero(k1_largest, crow1[2]) +
744 if_then_else_zero(k2_largest, crow2[2]);
745
746 row2[0] = if_then_else(k0_largest, crow1[0], crow0[0]);
747 row2[1] = if_then_else(k0_largest, crow1[1], crow0[1]);
748 row2[2] = if_then_else(k0_largest, crow1[2], crow0[2]);
749
750 row3[0] = if_then_else(k2_largest, crow1[0], crow2[0]);
751 row3[1] = if_then_else(k2_largest, crow1[1], crow2[1]);
752 row3[2] = if_then_else(k2_largest, crow1[2], crow2[2]);
753
754 const ScalarT ki_ki = ScalarT(1.0) / (if_then_else_zero(k0_largest, k0) + if_then_else_zero(k1_largest, k1) +
755 if_then_else_zero(k2_largest, k2));
756
757 const ScalarT ki_dpr1 = ki_ki * (k_row1[0] * row2[0] + k_row1[1] * row2[1] + k_row1[2] * row2[2]);
758 const ScalarT ki_dpr2 = ki_ki * (k_row1[0] * row3[0] + k_row1[1] * row3[1] + k_row1[2] * row3[2]);
759
760 row2[0] -= ki_dpr1 * k_row1[0];
761 row2[1] -= ki_dpr1 * k_row1[1];
762 row2[2] -= ki_dpr1 * k_row1[2];
763
764 row3[0] -= ki_dpr2 * k_row1[0];
765 row3[1] -= ki_dpr2 * k_row1[1];
766 row3[2] -= ki_dpr2 * k_row1[2];
767
768 const ScalarT a0 = row2[0] * row2[0] + row2[1] * row2[1] + row2[2] * row2[2];
769 const ScalarT a1 = row3[0] * row3[0] + row3[1] * row3[1] + row3[2] * row3[2];
770
771 ScalarT a_row2[3];
772
773 const bool a0lea1 = a0 <= a1;
774
775 a_row2[0] = if_then_else(a0lea1, row3[0], row2[0]);
776 a_row2[1] = if_then_else(a0lea1, row3[1], row2[1]);
777 a_row2[2] = if_then_else(a0lea1, row3[2], row2[2]);
778 const ScalarT ai_ai = ScalarT(1.0) / if_then_else(a0lea1, a1, a0);
779
780 evec2[K_X] = k_row1[1] * a_row2[2] - k_row1[2] * a_row2[1];
781 evec2[K_Y] = k_row1[2] * a_row2[0] - k_row1[0] * a_row2[2];
782 evec2[K_Z] = k_row1[0] * a_row2[1] - k_row1[1] * a_row2[0];
783
784 const ScalarT k_atr11 = cxx * k_row1[0] + cxy * k_row1[1] + czx * k_row1[2];
785 const ScalarT k_atr21 = cxy * k_row1[0] + cyy * k_row1[1] + cyz * k_row1[2];
786 const ScalarT k_atr31 = czx * k_row1[0] + cyz * k_row1[1] + czz * k_row1[2];
787
788 const ScalarT a_atr12 = cxx * a_row2[0] + cxy * a_row2[1] + czx * a_row2[2];
789 const ScalarT a_atr22 = cxy * a_row2[0] + cyy * a_row2[1] + cyz * a_row2[2];
790 const ScalarT a_atr32 = czx * a_row2[0] + cyz * a_row2[1] + czz * a_row2[2];
791
792 ScalarT rm2xx = (k_row1[0] * k_atr11 + k_row1[1] * k_atr21 + k_row1[2] * k_atr31) * ki_ki;
793 const ScalarT k_a_rm2xy = (k_row1[0] * a_atr12 + k_row1[1] * a_atr22 + k_row1[2] * a_atr32);
794 ScalarT rm2yy = (a_row2[0] * a_atr12 + a_row2[1] * a_atr22 + a_row2[2] * a_atr32) * ai_ai;
795 const ScalarT rm2xy_rm2xy = k_a_rm2xy * k_a_rm2xy * ai_ai * ki_ki;
796
797 //
798 // Wilkinson shift
799 //
800 const ScalarT b = ScalarT(0.5) * (rm2xx - rm2yy);
801 const ScalarT sqrtTerm = MultiplySign(std::sqrt(b * b + rm2xy_rm2xy), b);
802 eval0 = rm2yy + b - sqrtTerm;
803
804 eval1 = rm2xx + rm2yy - eval0;
805
806 rm2xx -= eval0;
807 rm2yy -= eval0;
808
809 const ScalarT rm2xx2 = rm2xx * rm2xx;
810 const ScalarT rm2yy2 = rm2yy * rm2yy;
811
812 const ScalarT fac1 = if_then_else(rm2xx2 < rm2yy2, k_a_rm2xy * ai_ai, rm2xx);
813 const ScalarT fac2 = if_then_else(rm2xx2 < rm2yy2, rm2yy, ki_ki * k_a_rm2xy);
814
815 evec0[0] = fac1 * a_row2[0] - fac2 * k_row1[0];
816 evec0[1] = fac1 * a_row2[1] - fac2 * k_row1[1];
817 evec0[2] = fac1 * a_row2[2] - fac2 * k_row1[2];
818
819 const bool rm2xx2iszero = rm2xx2 == ScalarT(0.0);
820 const bool rm2xy_rm2xyiszero = rm2xy_rm2xy == ScalarT(0.0);
821 const bool both_zero = rm2xx2iszero && rm2xy_rm2xyiszero;
822
823 // check degeneracy
824 evec0[0] = if_then_else(both_zero, a_row2[0], evec0[0]);
825 evec0[1] = if_then_else(both_zero, a_row2[1], evec0[1]);
826 evec0[2] = if_then_else(both_zero, a_row2[2], evec0[2]);
827
828 evec1[K_X] = evec2[K_Y] * evec0[K_Z] - evec2[K_Z] * evec0[K_Y];
829 evec1[K_Y] = evec2[K_Z] * evec0[K_X] - evec2[K_X] * evec0[K_Z];
830 evec1[K_Z] = evec2[K_X] * evec0[K_Y] - evec2[K_Y] * evec0[K_X];
831
832 eval0 += c1;
833 eval1 += c1;
834 eval2 += c1;
835
836 const ScalarT c2tol = (c1 * c1) * (-1.0e-30); // DJL where does the magic number come from?
837
838 const bool c2lsmall_neg = c2 < c2tol;
839
840 eval0 = if_then_else(c2lsmall_neg, eval0, c1);
841 eval1 = if_then_else(c2lsmall_neg, eval1, c1);
842 eval2 = if_then_else(c2lsmall_neg, eval2, c1);
843
844 evec0[0] = if_then_else(c2lsmall_neg, evec0[0], ScalarT(1.0));
845 evec0[1] = if_then_else(c2lsmall_neg, evec0[1], ScalarT(0.0));
846 evec0[2] = if_then_else(c2lsmall_neg, evec0[2], ScalarT(0.0));
847
848 evec1[0] = if_then_else(c2lsmall_neg, evec1[0], ScalarT(0.0));
849 evec1[1] = if_then_else(c2lsmall_neg, evec1[1], ScalarT(1.0));
850 evec1[2] = if_then_else(c2lsmall_neg, evec1[2], ScalarT(0.0));
851
852 evec2[0] = if_then_else(c2lsmall_neg, evec2[0], ScalarT(0.0));
853 evec2[1] = if_then_else(c2lsmall_neg, evec2[1], ScalarT(0.0));
854 evec2[2] = if_then_else(c2lsmall_neg, evec2[2], ScalarT(1.0));
855
856 return;
857}
858
859template <typename ScalarT>
860NIMBLE_INLINE_FUNCTION void
861Polar_Decomp(
862 const ScalarT* const mat, /* full tensor */
863 ScalarT* const left_stretch, /* symmetric tensor */
864 ScalarT* const rotation)
865{ /* full tensor */
866
867 ScalarT mat_inv[9];
868 Invert_Full33(mat, mat_inv);
869
870 ScalarT mat_inv_squared[6];
871 Square_Full33T_Full33(mat_inv, mat_inv_squared);
872
873 ScalarT vec1[3] = {0.0, 0.0, 0.0}; // Eigen vectors of mat_inv_squared
874 ScalarT vec2[3] = {0.0, 0.0, 0.0};
875 ScalarT vec3[3] = {0.0, 0.0, 0.0};
876 ScalarT eval[3] = {0.0, 0.0, 0.0}; // Eigen values of mat_inv_squared
877
878 Eigen_Sym33_NonUnit(mat_inv_squared, eval[0], eval[1], eval[2], vec1, vec2, vec3);
879
880 const ScalarT zero = ScalarT(0.0);
881
882 eval[0] = if_then(eval[0] < zero, zero, eval[0]);
883 eval[1] = if_then(eval[1] < zero, zero, eval[1]);
884 eval[2] = if_then(eval[2] < zero, zero, eval[2]);
885
886 const ScalarT len1_sq = vec1[K_X] * vec1[K_X] + vec1[K_Y] * vec1[K_Y] + vec1[K_Z] * vec1[K_Z];
887 const ScalarT len2_sq = vec2[K_X] * vec2[K_X] + vec2[K_Y] * vec2[K_Y] + vec2[K_Z] * vec2[K_Z];
888 const ScalarT len3_sq = vec3[K_X] * vec3[K_X] + vec3[K_Y] * vec3[K_Y] + vec3[K_Z] * vec3[K_Z];
889
890 const ScalarT xlx = std::sqrt(eval[0]);
891 const ScalarT xly = std::sqrt(eval[1]);
892 const ScalarT xlz = std::sqrt(eval[2]);
893
894 const ScalarT one = ScalarT(1.0);
895
896 const ScalarT xlxi = one / (xlx * len1_sq);
897 const ScalarT xlyi = one / (xly * len2_sq);
898 const ScalarT xlzi = one / (xlz * len3_sq);
899
900 left_stretch[K_S_XX] = xlxi * vec1[K_X] * vec1[K_X] + xlyi * vec2[K_X] * vec2[K_X] + xlzi * vec3[K_X] * vec3[K_X];
901 left_stretch[K_S_YY] = xlxi * vec1[K_Y] * vec1[K_Y] + xlyi * vec2[K_Y] * vec2[K_Y] + xlzi * vec3[K_Y] * vec3[K_Y];
902 left_stretch[K_S_ZZ] = xlxi * vec1[K_Z] * vec1[K_Z] + xlyi * vec2[K_Z] * vec2[K_Z] + xlzi * vec3[K_Z] * vec3[K_Z];
903 left_stretch[K_S_XY] = xlxi * vec1[K_X] * vec1[K_Y] + xlyi * vec2[K_X] * vec2[K_Y] + xlzi * vec3[K_X] * vec3[K_Y];
904 left_stretch[K_S_YZ] = xlxi * vec1[K_Y] * vec1[K_Z] + xlyi * vec2[K_Y] * vec2[K_Z] + xlzi * vec3[K_Y] * vec3[K_Z];
905 left_stretch[K_S_ZX] = xlxi * vec1[K_Z] * vec1[K_X] + xlyi * vec2[K_Z] * vec2[K_X] + xlzi * vec3[K_Z] * vec3[K_X];
906
907 Mult_Full33_Sym33_ReturnT(mat_inv, left_stretch, rotation);
908}
909
910template <typename ScalarT>
911NIMBLE_INLINE_FUNCTION void
912Polar_Left_LogV_Lame(
913 const ScalarT* const def_grad_inc, /* full tensor */
914 ScalarT* const left_stretch_inc, /* symmetric tensor */
915 ScalarT* const rotation_inc, /* full tensor */
916 ScalarT* const log_left_stretch_inc)
917{ /* symmetric tensor */
918
919 // Adapted from MiniTensor_LinearAlgebra.t.h, polar_left_logV_lame()
920
921 // compute spd tensor
922 ScalarT b[6];
923 Square_Full33_Full33T(def_grad_inc, b);
924
925 // get eigenvalues/eigenvectors
926 ScalarT vec1[3] = {0.0, 0.0, 0.0}; // Eigen vectors of b
927 ScalarT vec2[3] = {0.0, 0.0, 0.0};
928 ScalarT vec3[3] = {0.0, 0.0, 0.0};
929 ScalarT eval[3] = {0.0, 0.0, 0.0}; // Eigen values of b
930 Eigen_Sym33_NonUnit(b, eval[0], eval[1], eval[2], vec1, vec2, vec3);
931
932 CheckVectorSanity(3, eval, "Eigenvalues in Polar_Left_LogV_Lame()");
933
934 // normalize the eigenvectors
935 ScalarT mag;
936 mag = std::sqrt(vec1[0] * vec1[0] + vec1[1] * vec1[1] + vec1[2] * vec1[2]);
937 vec1[0] /= mag;
938 vec1[1] /= mag;
939 vec1[2] /= mag;
940 mag = std::sqrt(vec2[0] * vec2[0] + vec2[1] * vec2[1] + vec2[2] * vec2[2]);
941 vec2[0] /= mag;
942 vec2[1] /= mag;
943 vec2[2] /= mag;
944 mag = std::sqrt(vec3[0] * vec3[0] + vec3[1] * vec3[1] + vec3[2] * vec3[2]);
945 vec3[0] /= mag;
946 vec3[1] /= mag;
947 vec3[2] /= mag;
948
949 // std::cout << "DJL DEBUGGING eigen values in Polar_Left_LogV_Lame " <<
950 // eval[0] << ", " << eval[1] << ", " << eval[2] << std::endl; std::cout <<
951 // "DJL DEBUGGING eigen vectors in Polor_Left_LogV_Lame (" << vec1[0] << ", "
952 // << vec1[1] << ", " << vec1[2] << ") (" << vec2[0] << ", " << vec2[1] << ",
953 // "
954 // << vec2[2] << ") (" << vec3[0] << ", " << vec3[1] << ", " << vec3[2] << ")"
955 // << std::endl;
956
957 ScalarT x[3];
958 x[0] = std::sqrt(eval[0]);
959 x[1] = std::sqrt(eval[1]);
960 x[2] = std::sqrt(eval[2]);
961
962 ScalarT xi[3];
963 xi[0] = 1.0 / x[0];
964 xi[1] = 1.0 / x[1];
965 xi[2] = 1.0 / x[2];
966
967 ScalarT lnx[3];
968 lnx[0] = std::log(x[0]);
969 lnx[1] = std::log(x[1]);
970 lnx[2] = std::log(x[2]);
971
972 ScalarT vec_transpose[9];
973 vec_transpose[K_F_XX] = vec1[0];
974 vec_transpose[K_F_XY] = vec1[1];
975 vec_transpose[K_F_XZ] = vec1[2];
976 vec_transpose[K_F_YX] = vec2[0];
977 vec_transpose[K_F_YY] = vec2[1];
978 vec_transpose[K_F_YZ] = vec2[2];
979 vec_transpose[K_F_ZX] = vec3[0];
980 vec_transpose[K_F_ZY] = vec3[1];
981 vec_transpose[K_F_ZZ] = vec3[2];
982
983 ScalarT temp[9];
984
985 // V = eVec*x*transpose(eVec)
986 temp[K_F_XX] = vec1[0] * x[0];
987 temp[K_F_XY] = vec2[0] * x[1];
988 temp[K_F_XZ] = vec3[0] * x[2];
989 temp[K_F_YX] = vec1[1] * x[0];
990 temp[K_F_YY] = vec2[1] * x[1];
991 temp[K_F_YZ] = vec3[1] * x[2];
992 temp[K_F_ZX] = vec1[2] * x[0];
993 temp[K_F_ZY] = vec2[2] * x[1];
994 temp[K_F_ZZ] = vec3[2] * x[2];
995 left_stretch_inc[K_S_XX] = temp[K_F_XX] * vec_transpose[K_F_XX] + temp[K_F_XY] * vec_transpose[K_F_YX] +
996 temp[K_F_XZ] * vec_transpose[K_F_ZX];
997 left_stretch_inc[K_S_YY] = temp[K_F_YX] * vec_transpose[K_F_XY] + temp[K_F_YY] * vec_transpose[K_F_YY] +
998 temp[K_F_YZ] * vec_transpose[K_F_ZY];
999 left_stretch_inc[K_S_ZZ] = temp[K_F_ZX] * vec_transpose[K_F_XZ] + temp[K_F_ZY] * vec_transpose[K_F_YZ] +
1000 temp[K_F_ZZ] * vec_transpose[K_F_ZZ];
1001 left_stretch_inc[K_S_XY] = temp[K_F_XX] * vec_transpose[K_F_XY] + temp[K_F_XY] * vec_transpose[K_F_YY] +
1002 temp[K_F_XZ] * vec_transpose[K_F_ZY];
1003 left_stretch_inc[K_S_YZ] = temp[K_F_YX] * vec_transpose[K_F_XZ] + temp[K_F_YY] * vec_transpose[K_F_YZ] +
1004 temp[K_F_YZ] * vec_transpose[K_F_ZZ];
1005 left_stretch_inc[K_S_ZX] = temp[K_F_ZX] * vec_transpose[K_F_XX] + temp[K_F_ZY] * vec_transpose[K_F_YX] +
1006 temp[K_F_ZZ] * vec_transpose[K_F_ZX];
1007
1008 // Vinv = eVec*xi*transpose(eVec)
1009 temp[K_F_XX] = vec1[0] * xi[0];
1010 temp[K_F_XY] = vec2[0] * xi[1];
1011 temp[K_F_XZ] = vec3[0] * xi[2];
1012 temp[K_F_YX] = vec1[1] * xi[0];
1013 temp[K_F_YY] = vec2[1] * xi[1];
1014 temp[K_F_YZ] = vec3[1] * xi[2];
1015 temp[K_F_ZX] = vec1[2] * xi[0];
1016 temp[K_F_ZY] = vec2[2] * xi[1];
1017 temp[K_F_ZZ] = vec3[2] * xi[2];
1018 ScalarT left_stretch_inc_inverse[9];
1019 Mult_Full33_Full33(temp, vec_transpose, left_stretch_inc_inverse);
1020
1021 // Vinv = eVec*lnx*transpose(eVec)
1022 temp[K_F_XX] = vec1[0] * lnx[0];
1023 temp[K_F_XY] = vec2[0] * lnx[1];
1024 temp[K_F_XZ] = vec3[0] * lnx[2];
1025 temp[K_F_YX] = vec1[1] * lnx[0];
1026 temp[K_F_YY] = vec2[1] * lnx[1];
1027 temp[K_F_YZ] = vec3[1] * lnx[2];
1028 temp[K_F_ZX] = vec1[2] * lnx[0];
1029 temp[K_F_ZY] = vec2[2] * lnx[1];
1030 temp[K_F_ZZ] = vec3[2] * lnx[2];
1031 log_left_stretch_inc[K_S_XX] = temp[K_F_XX] * vec_transpose[K_F_XX] + temp[K_F_XY] * vec_transpose[K_F_YX] +
1032 temp[K_F_XZ] * vec_transpose[K_F_ZX];
1033 log_left_stretch_inc[K_S_YY] = temp[K_F_YX] * vec_transpose[K_F_XY] + temp[K_F_YY] * vec_transpose[K_F_YY] +
1034 temp[K_F_YZ] * vec_transpose[K_F_ZY];
1035 log_left_stretch_inc[K_S_ZZ] = temp[K_F_ZX] * vec_transpose[K_F_XZ] + temp[K_F_ZY] * vec_transpose[K_F_YZ] +
1036 temp[K_F_ZZ] * vec_transpose[K_F_ZZ];
1037 log_left_stretch_inc[K_S_XY] = temp[K_F_XX] * vec_transpose[K_F_XY] + temp[K_F_XY] * vec_transpose[K_F_YY] +
1038 temp[K_F_XZ] * vec_transpose[K_F_ZY];
1039 log_left_stretch_inc[K_S_YZ] = temp[K_F_YX] * vec_transpose[K_F_XZ] + temp[K_F_YY] * vec_transpose[K_F_YZ] +
1040 temp[K_F_YZ] * vec_transpose[K_F_ZZ];
1041 log_left_stretch_inc[K_S_ZX] = temp[K_F_ZX] * vec_transpose[K_F_XX] + temp[K_F_ZY] * vec_transpose[K_F_YX] +
1042 temp[K_F_ZZ] * vec_transpose[K_F_ZX];
1043
1044 // R = Vinv*F
1045 Mult_Full33_Full33(left_stretch_inc_inverse, def_grad_inc, rotation_inc);
1046}
1047
1048template <typename ScalarT>
1049NIMBLE_INLINE_FUNCTION void
1050CheckCorrectnessOfPolarDecomp(
1051 const ScalarT* const mat,
1052 const ScalarT* const V,
1053 const ScalarT* const R,
1054 std::string const& label)
1055{
1056 ScalarT norm_tol = 1.0e-12;
1057
1058 ScalarT check[9], should_be_zero[9];
1059 Mult_Sym33_Full33(V, R, check);
1060 for (int i = 0; i < 9; i++) { should_be_zero[i] = mat[i] - check[i]; }
1061 ScalarT should_be_zero_norm = Norm_Full33(should_be_zero);
1062 if (should_be_zero_norm > norm_tol) {
1063 std::cout << "\n\nFailure in CheckCorrectnessOfPolarDecomp()!" << std::endl;
1064 Print_Full33("mat", mat);
1065 Print_Full33("V", V);
1066 Print_Full33("R", R);
1067 std::cout << "norm of error = " << should_be_zero_norm << "\n" << std::endl;
1068 // NIMBLE_ABORT("\n**** Polar decomposition correctness check
1069 // failed for " + label + "!\n");
1070 }
1071}
1072
1073template <typename ScalarT>
1074NIMBLE_INLINE_FUNCTION void
1075CheckCorrectnessOfSymSkew(
1076 const ScalarT* const mat,
1077 const ScalarT* const sym,
1078 const ScalarT* const skew,
1079 std::string const& label)
1080{
1081 ScalarT norm_tol = 1.0e-12 * Norm_Full33(mat);
1082
1083 ScalarT should_be_zero[9];
1084 should_be_zero[K_F_XX] = sym[K_S_XX] + skew[K_F_XX] - mat[K_F_XX];
1085 should_be_zero[K_F_XY] = sym[K_S_XY] + skew[K_F_XY] - mat[K_F_XY];
1086 should_be_zero[K_F_XZ] = sym[K_S_XZ] + skew[K_F_XZ] - mat[K_F_XZ];
1087 should_be_zero[K_F_YX] = sym[K_S_YX] + skew[K_F_YX] - mat[K_F_YX];
1088 should_be_zero[K_F_YY] = sym[K_S_YY] + skew[K_F_YY] - mat[K_F_YY];
1089 should_be_zero[K_F_YZ] = sym[K_S_YZ] + skew[K_F_YZ] - mat[K_F_YZ];
1090 should_be_zero[K_F_ZX] = sym[K_S_ZX] + skew[K_F_ZX] - mat[K_F_ZX];
1091 should_be_zero[K_F_ZY] = sym[K_S_ZY] + skew[K_F_ZY] - mat[K_F_ZY];
1092 should_be_zero[K_F_ZZ] = sym[K_S_ZZ] + skew[K_F_ZZ] - mat[K_F_ZZ];
1093 ScalarT should_be_zero_norm = Norm_Full33(should_be_zero);
1094 if (should_be_zero_norm > norm_tol) {
1095 std::cout << "\n\nFailure in CheckCorrectnessOfSymSkew()!" << std::endl;
1096 Print_Full33("mat", mat);
1097 Print_Sym33("sym", sym);
1098 Print_Full33("skew", skew);
1099 std::cout << "norm of error = " << should_be_zero_norm << "\n" << std::endl;
1100 // NIMBLE_ABORT("\n**** Sym-skew correctness check failed for " +
1101 // label + "!\n");
1102 }
1103}
1104
1105template <typename ScalarT>
1106NIMBLE_INLINE_FUNCTION void
1107Log_Rotation_Pi(const ScalarT* const rotation, ScalarT* const log_rotation)
1108{
1109 // adapted from MiniTensor_LinearAlgebra.t.h log_rotation_pi()
1110
1111 ScalarT machine_epsilon = std::numeric_limits<ScalarT>::epsilon();
1112
1113 // set firewall to make sure the rotation is indeed 180 degrees
1114 if (std::abs(rotation[K_F_XX] + rotation[K_F_YY] + rotation[K_F_ZZ] + 1.0) < 10.0 * machine_epsilon) {
1115 // throw std::invalid_argument("\n**** Input check failed for Log_Rotation_Pi()!\n");
1116 }
1117
1118 ScalarT B[9];
1119 B[K_F_XX] = rotation[K_F_XX] - 1.0;
1120 B[K_F_XY] = rotation[K_F_XY];
1121 B[K_F_XZ] = rotation[K_F_XZ];
1122 B[K_F_YX] = rotation[K_F_YX];
1123 B[K_F_YY] = rotation[K_F_YY] - 1.0;
1124 B[K_F_YZ] = rotation[K_F_YZ];
1125 B[K_F_ZX] = rotation[K_F_ZX];
1126 B[K_F_ZY] = rotation[K_F_ZY];
1127 B[K_F_ZZ] = rotation[K_F_ZZ] - 1.0;
1128
1129 ScalarT u[3];
1130 u[0] = B[K_F_XX];
1131 u[1] = B[K_F_XY];
1132 u[2] = B[K_F_XZ];
1133
1134 ScalarT v[3];
1135 v[0] = B[K_F_YX];
1136 v[1] = B[K_F_YY];
1137 v[2] = B[K_F_YZ];
1138
1139 ScalarT normal[3];
1140 CrossProduct(u, v, normal);
1141
1142 ScalarT norm = normal[0] * normal[0] + normal[1] * normal[1] + normal[2] * normal[2];
1143 if (norm > 0.0) { norm = std::sqrt(norm); }
1144
1145 if (norm < machine_epsilon) {
1146 ScalarT w[3];
1147 w[0] = B[K_F_ZX];
1148 w[1] = B[K_F_ZY];
1149 w[2] = B[K_F_ZZ];
1150
1151 CrossProduct(u, w, normal);
1152
1153 norm = normal[0] * normal[0] + normal[1] * normal[1] + normal[2] * normal[2];
1154 if (norm > 0.0) { norm = std::sqrt(norm); }
1155
1156 NIMBLE_ASSERT(norm >= machine_epsilon, "\n**** Error in Log_Rotation_Pi(), cannot"
1157 " determine rotation vector.");
1158 }
1159
1160 normal[0] /= norm;
1161 normal[1] /= norm;
1162 normal[2] /= norm;
1163
1164 ScalarT pi = std::acos(-1.0);
1165
1166 log_rotation[K_F_XX] = 0.0;
1167 log_rotation[K_F_XY] = -normal[2] * pi;
1168 log_rotation[K_F_XZ] = normal[1] * pi;
1169 log_rotation[K_F_YX] = normal[2] * pi;
1170 log_rotation[K_F_YY] = 0.0;
1171 log_rotation[K_F_YZ] = -normal[0] * pi;
1172 log_rotation[K_F_ZX] = -normal[1] * pi;
1173 log_rotation[K_F_ZY] = normal[0] * pi;
1174 log_rotation[K_F_ZZ] = 0.0;
1175}
1176
1177template <typename ScalarT>
1178NIMBLE_INLINE_FUNCTION void
1179Log_Rotation(const ScalarT* const rotation, ScalarT* const log_rotation)
1180{
1181 // adapted from MiniTensor_LinearAlgebra log_rotation()
1182
1183 // firewalls, make sure R \in SO(N)
1184 bool valid_input = true;
1185 ScalarT machine_epsilon = std::numeric_limits<ScalarT>::epsilon();
1186 ScalarT temp[6];
1187 Square_Full33_Full33T(rotation, temp);
1188 temp[K_S_XX] -= 1.0;
1189 temp[K_S_YY] -= 1.0;
1190 temp[K_S_ZZ] -= 1.0;
1191 ScalarT norm = Norm_Sym33(temp);
1192 if (norm > 100.0 * machine_epsilon) { valid_input = false; }
1193 ScalarT det_rotation = Determinant_Full33(rotation);
1194 if (std::abs(det_rotation - 1.0) > 100.0 * machine_epsilon) { valid_input = false; }
1195 if (!valid_input) {
1196 std::stringstream ss;
1197 ss << "\n**** Input check failed for Log_Rotation()!";
1198 ss << "\n**** norm(R - I) must be less than (100 * machine_epsilon): "
1199 "norm = "
1200 << norm << ", machine_epsilon = " << machine_epsilon;
1201 ss << "\n**** (det(R) - 1) must be less than (100 * machine_epsilon): "
1202 "det(R) = "
1203 << det_rotation << ", machine_epsilon = " << machine_epsilon << "\n";
1204 // throw std::invalid_argument(ss.str());
1205 }
1206
1207 // acos requires input between -1 and +1
1208 ScalarT cosine = 0.5 * (rotation[K_F_XX] + rotation[K_F_YY] + rotation[K_F_ZZ] - 1.0);
1209 if (cosine < -1.0) {
1210 cosine = -1.0;
1211 } else if (cosine > 1.0) {
1212 cosine = 1.0;
1213 }
1214 ScalarT theta = std::acos(cosine);
1215
1216 if (theta == 0.0) {
1217 Zero_Full33(log_rotation);
1218 } else if (std::abs(cosine + 1.0) < 10.0 * machine_epsilon) {
1219 // r = log_rotation_pi(R);
1220 } else {
1221 // r = theta / std::sin(theta) * skew(R);
1222 ScalarT a = theta / std::sin(theta);
1223 ScalarT rotation_skew[9];
1224 SkewPart_Full33(rotation, rotation_skew);
1225 Mult_Scalar_Full33(a, rotation_skew, log_rotation);
1226 }
1227}
1228
1229NIMBLE_INLINE_FUNCTION double
1230Invert3x3(const double mat[][3], double inv[][3])
1231{
1232 double minor0 = mat[1][1] * mat[2][2] - mat[1][2] * mat[2][1];
1233 double minor1 = mat[1][0] * mat[2][2] - mat[1][2] * mat[2][0];
1234 double minor2 = mat[1][0] * mat[2][1] - mat[1][1] * mat[2][0];
1235 double minor3 = mat[0][1] * mat[2][2] - mat[0][2] * mat[2][1];
1236 double minor4 = mat[0][0] * mat[2][2] - mat[2][0] * mat[0][2];
1237 double minor5 = mat[0][0] * mat[2][1] - mat[0][1] * mat[2][0];
1238 double minor6 = mat[0][1] * mat[1][2] - mat[0][2] * mat[1][1];
1239 double minor7 = mat[0][0] * mat[1][2] - mat[0][2] * mat[1][0];
1240 double minor8 = mat[0][0] * mat[1][1] - mat[0][1] * mat[1][0];
1241 double det = mat[0][0] * minor0 - mat[0][1] * minor1 + mat[0][2] * minor2;
1242
1243 if (det <= 0.0) {
1244#ifdef NIMBLE_HAVE_KOKKOS
1245 if (det == 0.0)
1246 printf("\n**** Error in Invert3x3(), singular matrix.\n");
1247 else
1248 printf(
1249 "\n**** Error in HexInvert3x3(), negative determinant "
1250 "(%e)\n",
1251 det);
1252#else
1253 NIMBLE_ASSERT(det > 0.0, "\n**** Error in Invert3x3(), singular matrix.\n");
1254#endif
1255 }
1256
1257 inv[0][0] = minor0 / det;
1258 inv[0][1] = -1.0 * minor3 / det;
1259 inv[0][2] = minor6 / det;
1260 inv[1][0] = -1.0 * minor1 / det;
1261 inv[1][1] = minor4 / det;
1262 inv[1][2] = -1.0 * minor7 / det;
1263 inv[2][0] = minor2 / det;
1264 inv[2][1] = -1.0 * minor5 / det;
1265 inv[2][2] = minor8 / det;
1266
1267 return det;
1268}
1269
1270
1271template< class Matrix >
1272void
1273LU_Decompose(int num_entries, Matrix& mat, int* index)
1274{
1275 int imax = 0;
1276 double big, temp, sum;
1277 int n = num_entries;
1278 std::vector<double> vv(n); // vv stores the implicit scaling of each row.
1279 double tiny = 1e-40;
1280
1281 // loop over rows to get the implicit scaling information
1282 // scale each row by 1/(largest element)
1283
1284 for (int i = 0; i < n; ++i) {
1285 big = 0.0;
1286 for (int j = 0; j < n; ++j) {
1287 if (std::fabs(mat(i, j)) > big) big = std::fabs(mat(i, j));
1288 }
1289 if (big < tiny) {
1290 // No nonzero largest element.
1291 NIMBLE_ABORT("Singular matrix in routine LU_Decompose()");
1292 }
1293 vv[i] = 1.0 / big;
1294 }
1295
1296 // loop over columns
1297 for (int j = 0; j < n; ++j) {
1298 for (int i = 0; i < j; ++i) {
1299 sum = mat(i, j);
1300 for (int k = 0; k < i; ++k) { sum -= mat(i, k) * mat(k, j); }
1301 mat(i, j) = sum;
1302 }
1303
1304 big = 0.0;
1305 for (int i = j; i < n; ++i) {
1306 sum = mat(i, j);
1307 for (int k = 0; k < j; ++k) { sum -= mat(i, k) * mat(k, j); }
1308 mat(i, j) = sum;
1309
1310 if (vv[i] * std::fabs(sum) > big) {
1311 // this is the biggest element in the column
1312 // save it as the pivot element
1313 big = vv[i] * std::fabs(sum);
1314 imax = i;
1315 }
1316 }
1317
1318 // interchange rows if required
1319 if (j != imax) {
1320 for (int k = 0; k < n; ++k) {
1321 temp = mat(imax, k);
1322 mat(imax, k) = mat(j, k);
1323 mat(j, k) = temp;
1324 }
1325 vv[imax] = vv[j];
1326 }
1327 index[j] = imax;
1328
1329 if (std::fabs(mat(j, j)) < tiny) {
1330 // matrix is singular and we're about to divide by zero
1331 NIMBLE_ABORT("Singular matrix in routine LU_Decompose()");
1332 }
1333
1334 // divide by the pivot element
1335 if (j != n) {
1336 temp = 1.0 / (mat(j, j));
1337 for (int i = j + 1; i < n; ++i) { mat(i, j) *= temp; }
1338 }
1339 }
1340
1341 // decomposition is returned in mat
1342}
1343
1344
1345template< class Matrix >
1346void
1347LU_Solve(int num_entries, const Matrix& mat, double* vec, const int* index)
1348{
1349 // mat holds the upper and lower triangular decomposition (obtained by
1350 // LU_Deompose) vec hold the right-hand-side vector index holds the pivoting
1351 // information (row interchanges)
1352
1353 double sum;
1354 int n = num_entries;
1355
1356 // forward substitution
1357 // resolve pivoting as we go (stored in index array)
1358 for (int i = 0; i < n; ++i) {
1359 sum = vec[index[i]];
1360 vec[index[i]] = vec[i];
1361 for (int j = 0; j < i; ++j) { sum -= mat(i, j) * vec[j]; }
1362 vec[i] = sum;
1363 }
1364
1365 // back substitution
1366 for (int i = n - 1; i >= 0; --i) {
1367 sum = vec[i];
1368 for (int j = i + 1; j < n; ++j) { sum -= mat(i, j) * vec[j]; }
1369 vec[i] = sum / mat(i, i);
1370 }
1371
1372 // solution is returned in vec
1373}
1374
1375
1376template< class Matrix >
1377void
1378LU_SolveSystem(int num_entries, Matrix& mat, double* vec, int* scratch)
1379{
1380 int* index = scratch;
1381
1382 // decompose the matrix into upper and lower triangular parts
1383 LU_Decompose(num_entries, mat, index);
1384
1385 // solve the system (forward and backward substitution)
1386 LU_Solve(num_entries, mat, vec, index);
1387}
1388
1389#endif // NIMBLE_UTILS_H
nimble_defs.h
NIMBLE_INLINE_FUNCTION
#define NIMBLE_INLINE_FUNCTION
Definition nimble_defs.h:50
nimble_macros.h
NIMBLE_DEBUG_ASSERT
#define NIMBLE_DEBUG_ASSERT(cond)
Definition nimble_macros.h:99
NIMBLE_ASSERT
#define NIMBLE_ASSERT(...)
Definition nimble_macros.h:85
NIMBLE_ABORT
#define NIMBLE_ABORT(...)
Definition nimble_macros.h:87
Print_Full33
void Print_Full33(std::string label, const ScalarT *const mat, int precision=2)
Definition nimble_utils.h:169
Cos_Of_Acos_Divided_By_3
NIMBLE_INLINE_FUNCTION ScalarT Cos_Of_Acos_Divided_By_3(const ScalarT x)
Pade approximation to cos( acos(x)/3 )
Definition nimble_utils.h:652
Unrotate_Sym33_Using_R_S_Rtranspose
NIMBLE_INLINE_FUNCTION void Unrotate_Sym33_Using_R_S_Rtranspose(const ScalarT *const s, const ScalarT *const r, ScalarT *const result)
Unrotate a symetric tensor, result = R S R^T.
Definition nimble_utils.h:370
Log_Rotation
NIMBLE_INLINE_FUNCTION void Log_Rotation(const ScalarT *const rotation, ScalarT *const log_rotation)
Definition nimble_utils.h:1179
if_then_else
NIMBLE_INLINE_FUNCTION ScalarT if_then_else(const bool b, const ScalarT v1, const ScalarT v2)
Definition nimble_utils.h:155
CheckCorrectnessOfSymSkew
NIMBLE_INLINE_FUNCTION void CheckCorrectnessOfSymSkew(const ScalarT *const mat, const ScalarT *const sym, const ScalarT *const skew, std::string const &label)
Definition nimble_utils.h:1075
Determinant_Full33
NIMBLE_INLINE_FUNCTION ScalarT Determinant_Full33(const ScalarT *const mat)
Definition nimble_utils.h:404
Square_Full33T_Full33
NIMBLE_INLINE_FUNCTION void Square_Full33T_Full33(const ScalarT *const mat, ScalarT *const result)
Compute result = mat^T mat.
Definition nimble_utils.h:196
Mult_Full33_Sym33_ReturnT
NIMBLE_INLINE_FUNCTION void Mult_Full33_Sym33_ReturnT(const ScalarT *const full, const ScalarT *const sym, ScalarT *const result)
Multiply a full tensor by a symmetric tensor, return transpose of resultant.
Definition nimble_utils.h:326
LU_SolveSystem
void LU_SolveSystem(int num_entries, Matrix &mat, double *vec, int *scratch)
Definition nimble_utils.h:1378
Sum_Sym33_Full33
NIMBLE_INLINE_FUNCTION void Sum_Sym33_Full33(const ScalarT *const A, const ScalarT *const B, ScalarT *const result)
Sum of a symmetric tensor and a full tensor.
Definition nimble_utils.h:242
Zero_Full33
NIMBLE_INLINE_FUNCTION void Zero_Full33(ScalarT *const mat)
Definition nimble_utils.h:444
Invert_Full33
NIMBLE_INLINE_FUNCTION ScalarT Invert_Full33(const ScalarT *mat, ScalarT *inv)
Function to invert a 3x3 matrix.
Definition nimble_utils.h:525
CheckCorrectnessOfPolarDecomp
NIMBLE_INLINE_FUNCTION void CheckCorrectnessOfPolarDecomp(const ScalarT *const mat, const ScalarT *const V, const ScalarT *const R, std::string const &label)
Definition nimble_utils.h:1050
LU_Decompose
void LU_Decompose(int num_entries, Matrix &mat, int *index)
Definition nimble_utils.h:1273
if_then_else_zero
NIMBLE_INLINE_FUNCTION ScalarT if_then_else_zero(const bool b, const ScalarT v)
Definition nimble_utils.h:162
Mult_Scalar_Full33
NIMBLE_INLINE_FUNCTION void Mult_Scalar_Full33(ScalarT a, const ScalarT *const mat, ScalarT *const result)
Multiply a full tensor by a scalar.
Definition nimble_utils.h:210
Minimum
NIMBLE_INLINE_FUNCTION ScalarT Minimum(const ScalarT x, const ScalarT y)
Definition nimble_utils.h:141
StringLength
NIMBLE_INLINE_FUNCTION int StringLength(const char *str)
Definition nimble_utils.h:61
StringsAreEqual
NIMBLE_INLINE_FUNCTION bool StringsAreEqual(const char *str1, const char *str2)
Definition nimble_utils.h:70
Norm_Full33
NIMBLE_INLINE_FUNCTION ScalarT Norm_Full33(const ScalarT *const mat)
Definition nimble_utils.h:430
Sum_Full33_Full33
NIMBLE_INLINE_FUNCTION void Sum_Full33_Full33(const ScalarT *const A, const ScalarT *const B, ScalarT *const result)
Sum of a full tensor and a full tensor.
Definition nimble_utils.h:226
Log_Rotation_Pi
NIMBLE_INLINE_FUNCTION void Log_Rotation_Pi(const ScalarT *const rotation, ScalarT *const log_rotation)
Definition nimble_utils.h:1107
MultiplySign
NIMBLE_INLINE_FUNCTION ScalarT MultiplySign(const ScalarT x, const ScalarT y)
Return x times sign of y.
Definition nimble_utils.h:134
CheckVectorSanity
NIMBLE_INLINE_FUNCTION void CheckVectorSanity(int vec_length, ScalarT const *const vec, const char *label)
Definition nimble_utils.h:114
Rotate_Sym33_Using_Rtranspose_S_R
NIMBLE_INLINE_FUNCTION void Rotate_Sym33_Using_Rtranspose_S_R(const ScalarT *const s, const ScalarT *const r, ScalarT *const result)
Rotate a symetric tensor, result = R^T S R.
Definition nimble_utils.h:344
Mult_Scalar_Full33_Full33
NIMBLE_INLINE_FUNCTION void Mult_Scalar_Full33_Full33(ScalarT alpha, const ScalarT *const A, const ScalarT *const B, ScalarT *const result)
Multiply a full tensor by a full tensor and a scalar.
Definition nimble_utils.h:294
SetEqual_Full33
NIMBLE_INLINE_FUNCTION void SetEqual_Full33(const ScalarT *const B, ScalarT *const A)
Definition nimble_utils.h:459
Eigen_Sym33_NonUnit
NIMBLE_INLINE_FUNCTION void Eigen_Sym33_NonUnit(const ScalarT *const tensor, ScalarT &eval0, ScalarT &eval1, ScalarT &eval2, ScalarT *const evec0, ScalarT *const evec1, ScalarT *const evec2)
Definition nimble_utils.h:669
Invert3x3
NIMBLE_INLINE_FUNCTION double Invert3x3(const double mat[][3], double inv[][3])
Definition nimble_utils.h:1230
SetEqual_Sym33
NIMBLE_INLINE_FUNCTION void SetEqual_Sym33(const ScalarT *const B, ScalarT *const A)
Definition nimble_utils.h:474
LU_Solve
void LU_Solve(int num_entries, const Matrix &mat, double *vec, const int *index)
Definition nimble_utils.h:1347
Polar_Decomp
NIMBLE_INLINE_FUNCTION void Polar_Decomp(const ScalarT *const mat, ScalarT *const left_stretch, ScalarT *const rotation)
Definition nimble_utils.h:861
Square_Full33_Full33T
NIMBLE_INLINE_FUNCTION void Square_Full33_Full33T(const ScalarT *const mat, ScalarT *const result)
Compute result = mat mat^T.
Definition nimble_utils.h:312
Polar_Left_LogV_Lame
NIMBLE_INLINE_FUNCTION void Polar_Left_LogV_Lame(const ScalarT *const def_grad_inc, ScalarT *const left_stretch_inc, ScalarT *const rotation_inc, ScalarT *const log_left_stretch_inc)
Definition nimble_utils.h:912
CrossProduct
NIMBLE_INLINE_FUNCTION void CrossProduct(const ScalarT *const u, const ScalarT *const v, ScalarT *const result)
Definition nimble_utils.h:395
BCH
NIMBLE_INLINE_FUNCTION void BCH(const ScalarT *const x, const ScalarT *const y, ScalarT *const result)
R^N logarithmic map using Baker-Campbell-Hausdorff (BCH) expansion (4 terms)
Definition nimble_utils.h:557
SymPart_Full33
NIMBLE_INLINE_FUNCTION void SymPart_Full33(const ScalarT *const mat, ScalarT *const result)
Definition nimble_utils.h:486
Mult_Sym33_Full33
NIMBLE_INLINE_FUNCTION void Mult_Sym33_Full33(const ScalarT *const sym, const ScalarT *const full, ScalarT *const result)
Multiply a symmetric tensor by a full tensor.
Definition nimble_utils.h:276
if_then
NIMBLE_INLINE_FUNCTION ScalarT if_then(const bool b, const ScalarT v1, const ScalarT v2)
Definition nimble_utils.h:148
SkewPart_Full33
NIMBLE_INLINE_FUNCTION void SkewPart_Full33(const ScalarT *const mat, ScalarT *const result)
Definition nimble_utils.h:498
Mult_Full33_Full33
NIMBLE_INLINE_FUNCTION void Mult_Full33_Full33(const ScalarT *const A, const ScalarT *const B, ScalarT *const result)
Multiply a full tensor by a full tensor.
Definition nimble_utils.h:258
Norm_Sym33
NIMBLE_INLINE_FUNCTION ScalarT Norm_Sym33(const ScalarT *const mat)
Definition nimble_utils.h:416
BCH_Sym33_Full33
NIMBLE_INLINE_FUNCTION void BCH_Sym33_Full33(const ScalarT *const sym, const ScalarT *const full, ScalarT *const result)
R^N logarithmic map using Baker-Campbell-Hausdorff (BCH) expansion (4 terms)
Definition nimble_utils.h:629
Print_Sym33
void Print_Sym33(std::string label, const ScalarT *const mat, int precision=2)
Definition nimble_utils.h:182
nimble_view.h
Generated by doxygen 1.13.2