Branch data Line data Source code
1 : : // *****************************************************************************
2 : : /*!
3 : : \file src/Base/Vector.hpp
4 : : \copyright 2012-2015 J. Bakosi,
5 : : 2016-2018 Los Alamos National Security, LLC.,
6 : : 2019-2021 Triad National Security, LLC.
7 : : All rights reserved. See the LICENSE file for details.
8 : : \brief Vector algebra
9 : : \details Vector algebra.
10 : : */
11 : : // *****************************************************************************
12 : : #ifndef Vector_h
13 : : #define Vector_h
14 : :
15 : : #include <array>
16 : : #include <cmath>
17 : : #include <vector>
18 : : #include <cblas.h>
19 : :
20 : : #include "Types.hpp"
21 : : #include "Exception.hpp"
22 : :
23 : : // ignore old-style-casts required for lapack/blas calls
24 : : #if defined(__clang__)
25 : : #pragma clang diagnostic ignored "-Wold-style-cast"
26 : : #endif
27 : :
28 : : // Lapacke forward declarations
29 : : extern "C" {
30 : :
31 : : using lapack_int = long;
32 : :
33 : : #define LAPACK_ROW_MAJOR 101
34 : : #define LAPACK_COL_MAJOR 102
35 : :
36 : : extern lapack_int LAPACKE_dgetrf( int, lapack_int, lapack_int, double*,
37 : : lapack_int, lapack_int* );
38 : : extern lapack_int LAPACKE_dgetri( int, lapack_int, double*, lapack_int,
39 : : const lapack_int* );
40 : :
41 : : }
42 : :
43 : : namespace tk {
44 : :
45 : : //! Flip sign of vector components
46 : : //! \param[in] v Vector whose components to multiply by -1.0
47 : : inline void
48 : : flip( std::array< real, 3 >& v )
49 : : {
50 : : v[0] = -v[0];
51 : : v[1] = -v[1];
52 : : v[2] = -v[2];
53 : : }
54 : :
55 : : //! Compute the cross-product of two vectors
56 : : //! \param[in] v1x x coordinate of the 1st vector
57 : : //! \param[in] v1y y coordinate of the 1st vector
58 : : //! \param[in] v1z z coordinate of the 1st vector
59 : : //! \param[in] v2x x coordinate of the 2nd vector
60 : : //! \param[in] v2y y coordinate of the 2nd vector
61 : : //! \param[in] v2z z coordinate of the 2nd vector
62 : : //! \param[out] rx x coordinate of the product vector
63 : : //! \param[out] ry y coordinate of the product vector
64 : : //! \param[out] rz z coordinate of the product vector
65 : : #pragma omp declare simd
66 : : inline void
67 : : cross( real v1x, real v1y, real v1z,
68 : : real v2x, real v2y, real v2z,
69 : : real& rx, real& ry, real& rz )
70 : : {
71 : 956928031 : rx = v1y*v2z - v2y*v1z;
72 : 956928031 : ry = v1z*v2x - v2z*v1x;
73 : 944822746 : rz = v1x*v2y - v2x*v1y;
74 : : }
75 : :
76 : : //! Compute the cross-product of two vectors
77 : : //! \param[in] v1 1st vector
78 : : //! \param[in] v2 2nd vector
79 : : //! \return Cross-product
80 : : inline std::array< real, 3 >
81 : : cross( const std::array< real, 3 >& v1, const std::array< real, 3 >& v2 )
82 : : {
83 : : real rx, ry, rz;
84 : : cross( v1[0], v1[1], v1[2], v2[0], v2[1], v2[2], rx, ry, rz );
85 : 1 : return { std::move(rx), std::move(ry), std::move(rz) };
86 : : }
87 : :
88 : : //! Compute the cross-product of two vectors divided by a scalar
89 : : //! \param[in] v1x x coordinate of the 1st vector
90 : : //! \param[in] v1y y coordinate of the 1st vector
91 : : //! \param[in] v1z z coordinate of the 1st vector
92 : : //! \param[in] v2x x coordinate of the 2nd vector
93 : : //! \param[in] v2y y coordinate of the 2nd vector
94 : : //! \param[in] v2z z coordinate of the 2nd vector
95 : : //! \param[in] j The scalar to divide the product with
96 : : //! \param[out] rx x coordinate of the product vector
97 : : //! \param[out] ry y coordinate of the product vector
98 : : //! \param[out] rz z coordinate of the product vector
99 : : #pragma omp declare simd uniform(j)
100 : : inline void
101 : : crossdiv( real v1x, real v1y, real v1z,
102 : : real v2x, real v2y, real v2z,
103 : : real j,
104 : : real& rx, real& ry, real& rz )
105 : : {
106 : : cross( v1x, v1y, v1z, v2x, v2y, v2z, rx, ry, rz );
107 : 92125543 : rx /= j;
108 : 92125543 : ry /= j;
109 : 92125543 : rz /= j;
110 : : }
111 : :
112 : : //! Compute the cross-product of two vectors divided by a scalar
113 : : //! \param[in] v1 1st vector
114 : : //! \param[in] v2 2nd vector
115 : : //! \param[in] j Scalar to divide each component by
116 : : //! \return Cross-product divided by scalar
117 : : inline std::array< real, 3 >
118 : : crossdiv( const std::array< real, 3 >& v1,
119 : : const std::array< real, 3 >& v2,
120 : : real j )
121 : : {
122 : 90775388 : return {{ (v1[1]*v2[2] - v2[1]*v1[2]) / j,
123 : 90775388 : (v1[2]*v2[0] - v2[2]*v1[0]) / j,
124 : 90682117 : (v1[0]*v2[1] - v2[0]*v1[1]) / j }};
125 : : }
126 : :
127 : : //! Compute the dot-product of two vectors
128 : : //! \param[in] v1 1st vector
129 : : //! \param[in] v2 2nd vector
130 : : //! \return Dot-product
131 : : inline real
132 : : dot( const std::array< real, 3 >& v1, const std::array< real, 3 >& v2 )
133 : : {
134 [ + + ][ + + ]: 1058028933 : return v1[0]*v2[0] + v1[1]*v2[1] + v1[2]*v2[2];
[ + + ][ + + ]
[ + + ][ + + ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ + + ]
[ - - ][ - - ]
[ + - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
135 : : }
136 : :
137 : : //! Compute the dot-product of a matrix and a vector
138 : : //! \param[in] m Matrix
139 : : //! \param[in] v vector
140 : : //! \return Dot-product
141 : : inline std::array< real, 3 >
142 : : matvec(
143 : : const std::array< std::array< real, 3 >, 3 >& m,
144 : : const std::array< real, 3 >& v )
145 : : {
146 : 686000 : std::array< real, 3 > mv{0, 0, 0};
147 [ - - ][ - - ]: 2744000 : for (std::size_t i=0; i<3; ++i) {
[ + + ][ + + ]
148 [ - - ][ - - ]: 8232000 : for (std::size_t j=0; j<3; ++j)
[ + + ][ + + ]
149 : 6174000 : mv[i] += m[i][j]*v[j];
150 : : }
151 : :
152 : : return mv;
153 : : }
154 : :
155 : : //! Compute length of a vector
156 : : //! \param[in] x X coordinate of vector
157 : : //! \param[in] y Y coordinate of vector
158 : : //! \param[in] z Z coordinate of vector
159 : : //! \return length
160 : : #pragma omp declare simd
161 : : inline real
162 : : length( real x, real y, real z )
163 : : {
164 [ + + ][ + + ]: 18642643 : return std::sqrt( x*x + y*y + z*z );
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ]
165 : : }
166 : :
167 : : //! Compute length of a vector
168 : : //! \param[in] v vector
169 : : //! \return length
170 : : inline real
171 : : length( const std::array< real, 3 >& v )
172 : : {
173 [ + + ][ + + ]: 23692393 : return std::sqrt( dot(v,v) );
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
174 : : }
175 : :
176 : : //! Scale vector to unit length
177 : : //! \param[in,out] v Vector to normalize
178 : : inline void
179 : 2 : unit( std::array< real, 3 >& v ) noexcept(ndebug)
180 : : {
181 : : auto len = length( v );
182 : : Assert( len > std::numeric_limits< tk::real >::epsilon(), "div by zero" );
183 : 2 : v[0] /= len;
184 : 2 : v[1] /= len;
185 : 2 : v[2] /= len;
186 : 2 : }
187 : :
188 : : //! Compute the triple-product of three vectors
189 : : //! \param[in] v1x x coordinate of the 1st vector
190 : : //! \param[in] v1y y coordinate of the 1st vector
191 : : //! \param[in] v1z z coordinate of the 1st vector
192 : : //! \param[in] v2x x coordinate of the 2nd vector
193 : : //! \param[in] v2y y coordinate of the 2nd vector
194 : : //! \param[in] v2z z coordinate of the 2nd vector
195 : : //! \param[in] v3x x coordinate of the 3rd vector
196 : : //! \param[in] v3y y coordinate of the 3rd vector
197 : : //! \param[in] v3z z coordinate of the 3rd vector
198 : : //! \return Scalar value of the triple product
199 : : #pragma omp declare simd
200 : : inline tk::real
201 : : triple( real v1x, real v1y, real v1z,
202 : : real v2x, real v2y, real v2z,
203 : : real v3x, real v3y, real v3z )
204 : : {
205 : : real cx, cy, cz;
206 : : cross( v2x, v2y, v2z, v3x, v3y, v3z, cx, cy, cz );
207 [ - + ][ - + ]: 107254695 : return v1x*cx + v1y*cy + v1z*cz;
[ - + ][ - + ]
[ - + ][ - + ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ]
208 : : }
209 : :
210 : : //! Compute the triple-product of three vectors
211 : : //! \param[in] v1 1st vector
212 : : //! \param[in] v2 2nd vector
213 : : //! \param[in] v3 3rd vector
214 : : //! \return Triple-product
215 : : inline real
216 : : triple( const std::array< real, 3 >& v1,
217 : : const std::array< real, 3 >& v2,
218 : : const std::array< real, 3 >& v3 )
219 : : {
220 : : return dot( v1, cross(v2,v3) );
221 : : }
222 : :
223 : : //! Rotate vector about X axis
224 : : //! \param[in] v Vector to rotate
225 : : //! \param[in] angle Angle to use to rotate with
226 : : //! \return Rotated vector
227 : : inline std::array< real, 3 >
228 : 153116 : rotateX( const std::array< real, 3 >& v, real angle )
229 : : {
230 : : using std::cos; using std::sin;
231 : :
232 : : std::array< std::array< real, 3 >, 3 >
233 : : R{{ {{ 1.0, 0.0, 0.0 }},
234 : 153116 : {{ 0.0, cos(angle), -sin(angle) }},
235 : : {{ 0.0, sin(angle), cos(angle) }} }};
236 : :
237 : 153116 : return {{ dot(R[0],v), dot(R[1],v), dot(R[2],v) }};
238 : : }
239 : :
240 : : //! Rotate vector about Y axis
241 : : //! \param[in] v Vector to rotate
242 : : //! \param[in] angle Angle to use to rotate with
243 : : //! \return Rotated vector
244 : : inline std::array< real, 3 >
245 : 153116 : rotateY( const std::array< real, 3 >& v, real angle )
246 : : {
247 : : using std::cos; using std::sin;
248 : :
249 : : std::array< std::array< real, 3 >, 3 >
250 : 153116 : R{{ {{ cos(angle), 0.0, sin(angle) }},
251 : : {{ 0.0, 1.0, 0.0 }},
252 : 153116 : {{ -sin(angle), 0.0, cos(angle) }} }};
253 : :
254 : 153116 : return {{ dot(R[0],v), dot(R[1],v), dot(R[2],v) }};
255 : : }
256 : :
257 : : //! Rotate vector about Z axis
258 : : //! \param[in] v Vector to rotate
259 : : //! \param[in] angle Angle to use to rotate with
260 : : //! \return Rotated vector
261 : : inline std::array< real, 3 >
262 : 153116 : rotateZ( const std::array< real, 3 >& v, real angle )
263 : : {
264 : : using std::cos; using std::sin;
265 : :
266 : : std::array< std::array< real, 3 >, 3 >
267 : 153116 : R{{ {{ cos(angle), -sin(angle), 0.0 }},
268 : : {{ sin(angle), cos(angle), 0.0 }},
269 : : {{ 0.0, 0.0, 1.0 }} }};
270 : :
271 : 153116 : return {{ dot(R[0],v), dot(R[1],v), dot(R[2],v) }};
272 : : }
273 : :
274 : : //! \brief Compute the determinant of the Jacobian of a coordinate
275 : : //! transformation over a tetrahedron
276 : : //! \param[in] v1 (x,y,z) coordinates of 1st vertex of the tetrahedron
277 : : //! \param[in] v2 (x,y,z) coordinates of 2nd vertex of the tetrahedron
278 : : //! \param[in] v3 (x,y,z) coordinates of 3rd vertex of the tetrahedron
279 : : //! \param[in] v4 (x,y,z) coordinates of 4th vertex of the tetrahedron
280 : : //! \return Determinant of the Jacobian of transformation of physical
281 : : //! tetrahedron to reference (xi, eta, zeta) space
282 : : inline real
283 : 719760734 : Jacobian( const std::array< real, 3 >& v1,
284 : : const std::array< real, 3 >& v2,
285 : : const std::array< real, 3 >& v3,
286 : : const std::array< real, 3 >& v4 )
287 : : {
288 : 719760734 : std::array< real, 3 > ba{{ v2[0]-v1[0], v2[1]-v1[1], v2[2]-v1[2] }},
289 : 719760734 : ca{{ v3[0]-v1[0], v3[1]-v1[1], v3[2]-v1[2] }},
290 : 719760734 : da{{ v4[0]-v1[0], v4[1]-v1[1], v4[2]-v1[2] }};
291 : 719760734 : return triple( ba, ca, da );
292 : : }
293 : :
294 : : //! \brief Compute the inverse of the Jacobian of a coordinate transformation
295 : : //! over a tetrahedron
296 : : //! \param[in] v1 (x,y,z) coordinates of 1st vertex of the tetrahedron
297 : : //! \param[in] v2 (x,y,z) coordinates of 2nd vertex of the tetrahedron
298 : : //! \param[in] v3 (x,y,z) coordinates of 3rd vertex of the tetrahedron
299 : : //! \param[in] v4 (x,y,z) coordinates of 4th vertex of the tetrahedron
300 : : //! \return Inverse of the Jacobian of transformation of physical
301 : : //! tetrahedron to reference (xi, eta, zeta) space
302 : : inline std::array< std::array< real, 3 >, 3 >
303 : 11275991 : inverseJacobian( const std::array< real, 3 >& v1,
304 : : const std::array< real, 3 >& v2,
305 : : const std::array< real, 3 >& v3,
306 : : const std::array< real, 3 >& v4 )
307 : : {
308 : : std::array< std::array< real, 3 >, 3 > jacInv;
309 : :
310 : 11275991 : auto detJ = Jacobian( v1, v2, v3, v4 );
311 : :
312 : 11275991 : jacInv[0][0] = ( (v3[1]-v1[1])*(v4[2]-v1[2])
313 : 11275991 : - (v4[1]-v1[1])*(v3[2]-v1[2])) / detJ;
314 : 11275991 : jacInv[1][0] = -( (v2[1]-v1[1])*(v4[2]-v1[2])
315 : 11275991 : - (v4[1]-v1[1])*(v2[2]-v1[2])) / detJ;
316 : 11275991 : jacInv[2][0] = ( (v2[1]-v1[1])*(v3[2]-v1[2])
317 : 11275991 : - (v3[1]-v1[1])*(v2[2]-v1[2])) / detJ;
318 : :
319 : 11275991 : jacInv[0][1] = -( (v3[0]-v1[0])*(v4[2]-v1[2])
320 : 11275991 : - (v4[0]-v1[0])*(v3[2]-v1[2])) / detJ;
321 : 11275991 : jacInv[1][1] = ( (v2[0]-v1[0])*(v4[2]-v1[2])
322 : 11275991 : - (v4[0]-v1[0])*(v2[2]-v1[2])) / detJ;
323 : 11275991 : jacInv[2][1] = -( (v2[0]-v1[0])*(v3[2]-v1[2])
324 : 11275991 : - (v3[0]-v1[0])*(v2[2]-v1[2])) / detJ;
325 : :
326 : 11275991 : jacInv[0][2] = ( (v3[0]-v1[0])*(v4[1]-v1[1])
327 : 11275991 : - (v4[0]-v1[0])*(v3[1]-v1[1])) / detJ;
328 : 11275991 : jacInv[1][2] = -( (v2[0]-v1[0])*(v4[1]-v1[1])
329 : 11275991 : - (v4[0]-v1[0])*(v2[1]-v1[1])) / detJ;
330 : 11275991 : jacInv[2][2] = ( (v2[0]-v1[0])*(v3[1]-v1[1])
331 : 11275991 : - (v3[0]-v1[0])*(v2[1]-v1[1])) / detJ;
332 : :
333 : 11275991 : return jacInv;
334 : : }
335 : :
336 : : //! Compute the determinant of 3x3 matrix
337 : : //! \param[in] a 3x3 matrix
338 : : //! \return Determinant of the 3x3 matrix
339 : : inline tk::real
340 : 68821440 : determinant( const std::array< std::array< tk::real, 3 >, 3 >& a )
341 : : {
342 : 68821440 : return ( a[0][0] * (a[1][1]*a[2][2]-a[1][2]*a[2][1])
343 : 68821440 : - a[0][1] * (a[1][0]*a[2][2]-a[1][2]*a[2][0])
344 : 68821440 : + a[0][2] * (a[1][0]*a[2][1]-a[1][1]*a[2][0]) );
345 : : }
346 : :
347 : : //! Compute the inverse of 3x3 matrix
348 : : //! \param[in] m 3x3 matrix
349 : : //! \return Inverse of the 3x3 matrix
350 : : inline std::array< std::array< tk::real, 3 >, 3 >
351 : : inverse( const std::array< std::array< tk::real, 3 >, 3 >& m )
352 : : {
353 : : tk::real det = m[0][0] * (m[1][1] * m[2][2] - m[2][1] * m[1][2]) -
354 : : m[0][1] * (m[1][0] * m[2][2] - m[1][2] * m[2][0]) +
355 : : m[0][2] * (m[1][0] * m[2][1] - m[1][1] * m[2][0]);
356 : :
357 : : tk::real invdet = 1.0 / det;
358 : :
359 : : std::array< std::array< tk::real, 3 >, 3 > minv;
360 : : minv[0][0] = (m[1][1] * m[2][2] - m[2][1] * m[1][2]) * invdet;
361 : : minv[0][1] = (m[0][2] * m[2][1] - m[0][1] * m[2][2]) * invdet;
362 : : minv[0][2] = (m[0][1] * m[1][2] - m[0][2] * m[1][1]) * invdet;
363 : : minv[1][0] = (m[1][2] * m[2][0] - m[1][0] * m[2][2]) * invdet;
364 : : minv[1][1] = (m[0][0] * m[2][2] - m[0][2] * m[2][0]) * invdet;
365 : : minv[1][2] = (m[1][0] * m[0][2] - m[0][0] * m[1][2]) * invdet;
366 : : minv[2][0] = (m[1][0] * m[2][1] - m[2][0] * m[1][1]) * invdet;
367 : : minv[2][1] = (m[2][0] * m[0][1] - m[0][0] * m[2][1]) * invdet;
368 : : minv[2][2] = (m[0][0] * m[1][1] - m[1][0] * m[0][1]) * invdet;
369 : :
370 : : return minv;
371 : : }
372 : :
373 : : //! Solve a 3x3 system of equations using Cramer's rule
374 : : //! \param[in] a 3x3 lhs matrix
375 : : //! \param[in] b 3x1 rhs matrix
376 : : //! \return Array of solutions of the 3x3 system
377 : : inline std::array < tk::real, 3 >
378 : 17205360 : cramer( const std::array< std::array< tk::real, 3 >, 3>& a,
379 : : const std::array< tk::real, 3 >& b )
380 : : {
381 : 17205360 : auto de = determinant( a );
382 : :
383 : : auto nu(0.0);
384 : : std::array < real, 3 > x;
385 : :
386 : 17205360 : nu = determinant( {{{{b[0], a[0][1], a[0][2]}},
387 : : {{b[1], a[1][1], a[1][2]}},
388 : : {{b[2], a[2][1], a[2][2]}}}} );
389 : 17205360 : x[0] = nu/de;
390 : :
391 : 17205360 : nu = determinant( {{{{a[0][0], b[0], a[0][2]}},
392 : : {{a[1][0], b[1], a[1][2]}},
393 : : {{a[2][0], b[2], a[2][2]}}}} );
394 : 17205360 : x[1] = nu/de;
395 : :
396 : 17205360 : nu = determinant( {{{{a[0][0], a[0][1], b[0]}},
397 : : {{a[1][0], a[1][1], b[1]}},
398 : : {{a[2][0], a[2][1], b[2]}}}} );
399 : 17205360 : x[2] = nu/de;
400 : :
401 : 17205360 : return x;
402 : : }
403 : :
404 : : //! Move a point to a reference space given coordinates of origin of that space
405 : : //! \param[in] origin Origin of reference frame to which point is to be moved
406 : : //! \param[in,out] point Point that needs to be moved to reference frame
407 : : inline void
408 : : movePoint( const std::array< tk::real, 3 >& origin,
409 : : std::array< tk::real, 3 >& point )
410 : : {
411 [ + + ][ + + ]: 343914 : for (std::size_t i=0; i<3; ++i)
[ + + ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ + + ][ + + ]
[ + + ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ]
412 : 258759 : point[i] -= origin[i];
413 : : }
414 : :
415 : : //! Rotate a point in 3D space by specifying rotation angles in degrees
416 : : //! \param[in] angles Angles in 3D space by which point is to be rotated
417 : : //! \param[in,out] point Point that needs to be rotated
418 : : inline void
419 : 84057 : rotatePoint( const std::array< tk::real, 3 >& angles,
420 : : std::array< tk::real, 3 >& point )
421 : : {
422 : : // Convert angles to radian
423 : : tk::real pi = 4.0*std::atan(1.0);
424 : 84057 : auto a = angles[0] * pi/180.0;
425 : 84057 : auto b = angles[1] * pi/180.0;
426 : 84057 : auto c = angles[2] * pi/180.0;
427 : :
428 : : // Rotation matrix
429 : : std::array< std::array< tk::real, 3 >, 3 > rotMat;
430 : : {
431 : : using namespace std;
432 : 84057 : rotMat[0][0] = cos(b)*cos(c);
433 : 84057 : rotMat[0][1] = - cos(b)*sin(c);
434 : 84057 : rotMat[0][2] = sin(b);
435 : :
436 : 84057 : rotMat[1][0] = sin(a)*sin(b)*cos(c) + cos(a)*sin(c);
437 : 84057 : rotMat[1][1] = - sin(a)*sin(b)*sin(c) + cos(a)*cos(c);
438 : 84057 : rotMat[1][2] = - sin(a)*cos(b);
439 : :
440 : 84057 : rotMat[2][0] = - cos(a)*sin(b)*cos(c) + sin(a)*sin(c);
441 : 84057 : rotMat[2][1] = cos(a)*sin(b)*sin(c) + sin(a)*cos(c);
442 : 84057 : rotMat[2][2] = cos(a)*cos(b);
443 : : }
444 : :
445 : : // Apply rotation
446 : 84057 : std::array< tk::real, 3 > x{{0.0, 0.0, 0.0}};
447 [ + + ]: 336228 : for (std::size_t i=0; i<3; ++i) {
448 [ + + ]: 1008684 : for (std::size_t j=0; j<3; ++j) {
449 : 756513 : x[i] += rotMat[i][j]*point[j];
450 : : }
451 : : }
452 : 84057 : point = x;
453 : 84057 : }
454 : :
455 : : //! \brief Get the Right Cauchy-Green strain tensor from the inverse deformation
456 : : //! gradient tensor.
457 : : //! \param[in] g Inverse deformation gradient tensor
458 : : //! \return Right Cauchy-Green tensor
459 : : inline std::array< std::array< real, 3 >, 3 >
460 : 0 : getRightCauchyGreen(const std::array< std::array< real, 3 >, 3 >& g)
461 : : {
462 : : // allocate matrices
463 : : double G[9], C[9];
464 : :
465 : : // initialize c-matrices
466 [ - - ]: 0 : for (std::size_t i=0; i<3; ++i) {
467 [ - - ]: 0 : for (std::size_t j=0; j<3; ++j)
468 : 0 : G[i*3+j] = g[i][j];
469 : : }
470 : :
471 : : // get g.g^T
472 : 0 : cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasTrans,
473 : : 3, 3, 3, 1.0, G, 3, G, 3, 0.0, C, 3);
474 : :
475 : : // get inv(g.g^T)
476 : : lapack_int ipiv[3];
477 : :
478 : : #ifndef NDEBUG
479 : : lapack_int ierr =
480 : : #endif
481 : 0 : LAPACKE_dgetrf(LAPACK_ROW_MAJOR, 3, 3, C, 3, ipiv);
482 : : Assert(ierr==0, "Lapack error in LU factorization of g.g^T");
483 : :
484 : : #ifndef NDEBUG
485 : : lapack_int jerr =
486 : : #endif
487 : 0 : LAPACKE_dgetri(LAPACK_ROW_MAJOR, 3, C, 3, ipiv);
488 : : Assert(jerr==0, "Lapack error in inverting g.g^T");
489 : :
490 : : // Output C as 2D array
491 : 0 : return {{ {C[0], C[1], C[2]},
492 : 0 : {C[3], C[4], C[5]},
493 : 0 : {C[6], C[7], C[8]} }};
494 : : }
495 : :
496 : : //! \brief Get the Left Cauchy-Green strain tensor from the inverse deformation
497 : : //! gradient tensor.
498 : : //! \param[in] g Inverse deformation gradient tensor
499 : : //! \return Left Cauchy-Green tensor
500 : : inline std::array< std::array< real, 3 >, 3 >
501 : 0 : getLeftCauchyGreen(const std::array< std::array< real, 3 >, 3 >& g)
502 : : {
503 : : // allocate matrices
504 : : double G[9], b[9];
505 : :
506 : : // initialize c-matrices
507 [ - - ]: 0 : for (std::size_t i=0; i<3; ++i) {
508 [ - - ]: 0 : for (std::size_t j=0; j<3; ++j)
509 : 0 : G[i*3+j] = g[i][j];
510 : : }
511 : :
512 : : // get g^T.g
513 : 0 : cblas_dgemm(CblasRowMajor, CblasTrans, CblasNoTrans,
514 : : 3, 3, 3, 1.0, G, 3, G, 3, 0.0, b, 3);
515 : :
516 : : // get inv(g^T.g)
517 : : lapack_int ipiv[3];
518 : :
519 : : #ifndef NDEBUG
520 : : lapack_int ierr =
521 : : #endif
522 : 0 : LAPACKE_dgetrf(LAPACK_ROW_MAJOR, 3, 3, b, 3, ipiv);
523 : : Assert(ierr==0, "Lapack error in LU factorization of g^T.g");
524 : :
525 : : #ifndef NDEBUG
526 : : lapack_int jerr =
527 : : #endif
528 : 0 : LAPACKE_dgetri(LAPACK_ROW_MAJOR, 3, b, 3, ipiv);
529 : : Assert(jerr==0, "Lapack error in inverting g^T.g");
530 : :
531 : : // Output b as 2D array
532 : 0 : return {{ {b[0], b[1], b[2]},
533 : 0 : {b[3], b[4], b[5]},
534 : 0 : {b[6], b[7], b[8]} }};
535 : : }
536 : :
537 : : //! \brief Get the deviatoric Hencky strain tensor from the inverse deformation
538 : : //! gradient tensor.
539 : : //! \param[in] g Inverse deformation gradient tensor
540 : : //! \return Deviatoric Hencky strain tensor
541 : : inline std::array< std::array< real, 3 >, 3 >
542 : 0 : getDevHencky(const std::array< std::array< real, 3 >, 3 >& g)
543 : : {
544 : : // Get right Cauchy-Green strain tensor
545 : 0 : auto C = getLeftCauchyGreen(g);
546 : :
547 : 0 : std::array< std::array< real, 3 >, 3 > devH{{{0,0,0}, {0,0,0}, {0,0,0}}};
548 : :
549 : : // Compute approximation of Hencky strain tensor from section 3.4 of
550 : : // Barton, P. T. (2019). An interface-capturing Godunov method for the
551 : : // simulation of compressible solid-fluid problems. Journal of Computational
552 : : // Physics, 390, 25-50.
553 : :
554 : : // get inv(C)
555 : : double CInv[9];
556 [ - - ]: 0 : for (std::size_t i=0; i<3; ++i) {
557 [ - - ]: 0 : for (std::size_t j=0; j<3; ++j)
558 : 0 : CInv[i*3+j] = C[i][j];
559 : : }
560 : : lapack_int ipiv[3];
561 : :
562 : : #ifndef NDEBUG
563 : : lapack_int ierr =
564 : : #endif
565 : 0 : LAPACKE_dgetrf(LAPACK_ROW_MAJOR, 3, 3, CInv, 3, ipiv);
566 : : Assert(ierr==0, "Lapack error in LU factorization of C");
567 : :
568 : : #ifndef NDEBUG
569 : : lapack_int jerr =
570 : : #endif
571 : 0 : LAPACKE_dgetri(LAPACK_ROW_MAJOR, 3, CInv, 3, ipiv);
572 : : Assert(jerr==0, "Lapack error in inverting C");
573 : :
574 : : // Compute (C-CInv)/4
575 [ - - ]: 0 : for (std::size_t i=0; i<3; ++i)
576 [ - - ]: 0 : for (std::size_t j=0; j<3; ++j)
577 : 0 : devH[i][j] = 0.25*(C[i][j]-CInv[3*i+j]);
578 : :
579 : : // Compute deviatoric part
580 : 0 : tk::real trH = devH[0][0] + devH[1][1] + devH[2][2];
581 : :
582 [ - - ]: 0 : for (std::size_t i=0; i<3; ++i)
583 : 0 : devH[i][i] -= trH/3.0;
584 : :
585 : : // Output devH
586 : 0 : return devH;
587 : : }
588 : :
589 : : //! \brief Get transpose of a 3 by 3 matrix
590 : : //! \param[in] mat matrix to be transposed
591 : : //! \return transposed matrix
592 : : inline std::array< std::array< tk::real, 3 >, 3 >
593 : : transpose3by3(std::array< std::array< tk::real, 3 >, 3 > mat)
594 : : {
595 : : std::array< std::array< tk::real, 3 >, 3 > transMat;
596 : : for (size_t i=0; i<3; ++i)
597 : : for (size_t j=0; j<3; ++j)
598 : : transMat[i][j] = mat[j][i];
599 : : return transMat;
600 : : }
601 : :
602 : : //! \brief Get rotation matrix in 2D array form given a normal
603 : : //! direction. The remaining directions are given by section 5.3.1 of
604 : : //! Miller, G. H., and P. Colella. "A conservative three-dimensional
605 : : //! Eulerian method for coupled solid–fluid shock capturing."
606 : : //! Journal of Computational Physics 183.1 (2002): 26-82.
607 : : //! \param[in] r Coordinates of the first basis vector r = (rx,ry,rz).
608 : : //! \return rotation matrix
609 : : inline std::array< std::array< tk::real, 3 >, 3 >
610 : 1392147 : getRotationMatrix(const std::array< tk::real, 3 >& r)
611 : : {
612 : : std::array< std::array< tk::real, 3 >, 3 > rotMat;
613 : 1392147 : tk::real rx = r[0];
614 : 1392147 : tk::real ry = r[1];
615 : 1392147 : tk::real rz = r[2];
616 [ + + ]: 1392147 : if (std::abs(ry+rz) <= std::abs(ry-rz))
617 : : {
618 : 819426 : tk::real norm = std::sqrt(2*(1-rx*ry-rx*rz-ry*rz));
619 : 819426 : rotMat[0][0] = rx;
620 : 819426 : rotMat[0][1] = ry;
621 : 819426 : rotMat[0][2] = rz;
622 : 819426 : rotMat[1][0] = (ry-rz)/norm;
623 : 819426 : rotMat[1][1] = (rz-rx)/norm;
624 : 819426 : rotMat[1][2] = (rx-ry)/norm;
625 : 819426 : rotMat[2][0] = (rx*(ry+rz)-ry*ry-rz*rz)/norm;
626 : 819426 : rotMat[2][1] = (ry*(rx+rz)-rx*rx-rz*rz)/norm;
627 : 819426 : rotMat[2][2] = (rz*(rx+ry)-rx*rx-ry*ry)/norm;
628 : : }
629 : : else
630 : : {
631 : 572721 : tk::real norm = std::sqrt(2*(1+rz*(ry-rx)+rx*ry));
632 : 572721 : rotMat[0][0] = rx;
633 : 572721 : rotMat[0][1] = ry;
634 : 572721 : rotMat[0][2] = rz;
635 : 572721 : rotMat[1][0] = (ry+rz)/norm;
636 : 572721 : rotMat[1][1] = (rz-rx)/norm;
637 : 572721 : rotMat[1][2] = (-rx-ry)/norm;
638 : 572721 : rotMat[2][0] = (rx*(rz-ry)-ry*ry-rz*rz)/norm;
639 : 572721 : rotMat[2][1] = (ry*(rx+rz)+rx*rx+rz*rz)/norm;
640 : 572721 : rotMat[2][2] = (rz*(rx-ry)-rx*rx-ry*ry)/norm;
641 : : }
642 : 1392147 : return rotMat;
643 : : }
644 : :
645 : : //! \brief Rotate a second order tensor (e.g. a Strain/Stress matrix) from
646 : : //! the (x,y,z) to a new (r,s,t) coordinate system.
647 : : //! The directions are given by section 5.3.1 of
648 : : //! Miller, G. H., and P. Colella. "A conservative three-dimensional
649 : : //! Eulerian method for coupled solid–fluid shock capturing."
650 : : //! Journal of Computational Physics 183.1 (2002): 26-82.
651 : : //! \param[in] mat matrix to be rotated.
652 : : //! \param[in] r Coordinates of the first basis vector r = (rx,ry,rz).
653 : : //! \return rotated tensor
654 : : inline std::array< std::array< tk::real, 3 >, 3 >
655 : 1392147 : rotateTensor(const std::array< std::array< tk::real, 3 >, 3 >& mat,
656 : : const std::array< tk::real, 3 >& r )
657 : : {
658 : : // define rotation matrix
659 : 1392147 : std::array< std::array< tk::real, 3 >, 3 > rotMatrix = getRotationMatrix(r);
660 : : double rotMat[9];
661 : :
662 : : // define matrices
663 : : double matAuxIn[9], matAuxOut[9];
664 [ + + ]: 5568588 : for (std::size_t i=0; i<3; ++i)
665 [ + + ]: 16705764 : for (std::size_t j=0; j<3; ++j)
666 : : {
667 : 12529323 : matAuxIn[i*3+j] = mat[i][j];
668 : 12529323 : rotMat[i*3+j] = rotMatrix[i][j];
669 : : }
670 : :
671 : : // compute matAuxIn*rotMat and store it into matAuxOut
672 : 1392147 : cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasTrans,
673 : : 3, 3, 3, 1.0, matAuxIn, 3, rotMat, 3, 0.0, matAuxOut, 3);
674 : :
675 : : // matAuxOut -> matAuxIn
676 [ + + ]: 13921470 : for (std::size_t i=0; i<9; i++)
677 : : {
678 : 12529323 : matAuxIn[i] = matAuxOut[i];
679 : 12529323 : matAuxOut[i] = 0.0;
680 : : }
681 : :
682 : : // compute rotMat^T*matAuxIn and store it into matAuxOut
683 : 1392147 : cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasNoTrans,
684 : : 3, 3, 3, 1.0, rotMat, 3, matAuxIn, 3, 0.0, matAuxOut, 3);
685 : :
686 : : // return matAuxOut as a 2D array
687 : 1392147 : return {{ {matAuxOut[0], matAuxOut[1], matAuxOut[2]},
688 : 1392147 : {matAuxOut[3], matAuxOut[4], matAuxOut[5]},
689 : 1392147 : {matAuxOut[6], matAuxOut[7], matAuxOut[8]} }};
690 : : }
691 : :
692 : : //! \brief Rotate a second order tensor (e.g. a Strain/Stress matrix) from
693 : : //! the (x,y,z) to a new (r,s,t) coordinate system.
694 : : //! The directions are given by section 5.3.1 of
695 : : //! Miller, G. H., and P. Colella. "A conservative three-dimensional
696 : : //! Eulerian method for coupled solid–fluid shock capturing."
697 : : //! Journal of Computational Physics 183.1 (2002): 26-82.
698 : : //! \param[in] mat matrix to be rotated.
699 : : //! \param[in] r Coordinates of the first basis vector r = (rx,ry,rz).
700 : : //! \return rotated tensor
701 : : inline std::array< std::array< tk::real, 3 >, 3 >
702 : : unrotateTensor(const std::array< std::array< tk::real, 3 >, 3 >& mat,
703 : : const std::array< tk::real, 3 >& r )
704 : : {
705 : : // define rotation matrix
706 : : std::array< std::array< tk::real, 3 >, 3 > rotMatrix = getRotationMatrix(r);
707 : : double rotMat[9];
708 : :
709 : : // define matrices
710 : : double matAuxIn[9], matAuxOut[9];
711 : : for (std::size_t i=0; i<3; ++i)
712 : : for (std::size_t j=0; j<3; ++j)
713 : : {
714 : : matAuxIn[i*3+j] = mat[i][j];
715 : : rotMat[i*3+j] = rotMatrix[i][j];
716 : : }
717 : : // compute matAuxIn*rotMat and store it into matAuxOut
718 : : cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasNoTrans,
719 : : 3, 3, 3, 1.0, matAuxIn, 3, rotMat, 3, 0.0, matAuxOut, 3);
720 : : // matAuxOut -> matAuxIn
721 : : for (std::size_t i=0; i<9; i++)
722 : : {
723 : : matAuxIn[i] = matAuxOut[i];
724 : : matAuxOut[i] = 0.0;
725 : : }
726 : : // compute rotMat^T*matAuxIn and store it into matAuxOut
727 : : cblas_dgemm(CblasRowMajor, CblasTrans, CblasNoTrans,
728 : : 3, 3, 3, 1.0, rotMat, 3, matAuxIn, 3, 0.0, matAuxOut, 3);
729 : : // return matAuxOut as a 2D array
730 : : return {{ {matAuxOut[0], matAuxOut[1], matAuxOut[2]},
731 : : {matAuxOut[3], matAuxOut[4], matAuxOut[5]},
732 : : {matAuxOut[6], matAuxOut[7], matAuxOut[8]} }};
733 : : }
734 : :
735 : : //! \brief Rotate a vector (e.g. a velocity) from
736 : : //! the (x,y,z) to a new (r,s,t) coordinate system.
737 : : //! The directions are given by section 5.3.1 of
738 : : //! Miller, G. H., and P. Colella. "A conservative three-dimensional
739 : : //! Eulerian method for coupled solid–fluid shock capturing."
740 : : //! Journal of Computational Physics 183.1 (2002): 26-82.
741 : : //! \param[in] v Vector to be rotated.
742 : : //! \param[in] r Coordinates of the first basis vector r = (rx,ry,rz).
743 : : //! \return rotated vector
744 : : inline std::array< tk::real, 3 >
745 : : rotateVector( const std::array< tk::real, 3 >& v,
746 : : const std::array< tk::real, 3 >& r )
747 : : {
748 : : // define rotation matrix
749 : : std::array< std::array< tk::real, 3 >, 3 > rotMat = getRotationMatrix(r);
750 : :
751 : : // return rotMat*v
752 : : return matvec(rotMat,v);
753 : : }
754 : :
755 : : //! \brief Unrotate a vector (e.g. a velocity) from
756 : : //! the (x,y,z) to a new (r,s,t) coordinate system.
757 : : //! The directions are given by section 5.3.1 of
758 : : //! Miller, G. H., and P. Colella. "A conservative three-dimensional
759 : : //! Eulerian method for coupled solid–fluid shock capturing."
760 : : //! Journal of Computational Physics 183.1 (2002): 26-82.
761 : : //! \param[in] v Vector to be rotated.
762 : : //! \param[in] r Coordinates of the first basis vector r = (rx,ry,rz).
763 : : //! \return rotated vector
764 : : inline std::array< tk::real, 3 >
765 : : unrotateVector( const std::array< tk::real, 3 >& v,
766 : : const std::array< tk::real, 3 >& r )
767 : : {
768 : : // define rotation matrix
769 : : std::array< std::array< tk::real, 3 >, 3 > rotMat = getRotationMatrix(r);
770 : :
771 : : // return rotMat^T*v
772 : : return matvec(transpose3by3(rotMat),v);
773 : : }
774 : :
775 : : //! \brief Reflect a second order tensor (e.g. a Strain/Stress matrix)
776 : : //! \param[in] mat matrix to be rotated.
777 : : //! \param[in] reflectMat Reflection matrix
778 : : //! \return reflected tensor
779 : : inline std::array< std::array< tk::real, 3 >, 3 >
780 : 0 : reflectTensor(const std::array< std::array< tk::real, 3 >, 3 >& mat,
781 : : const std::array< std::array< tk::real, 3 >, 3 >& reflectMat)
782 : : {
783 : : // define reflection matrix
784 : : double refMat[9];
785 [ - - ]: 0 : for (std::size_t i=0; i<3; ++i)
786 [ - - ]: 0 : for (std::size_t j=0; j<3; ++j)
787 : 0 : refMat[i*3+j] = reflectMat[i][j];
788 : :
789 : : // define matAux (I need matrices as row major 1D arrays)
790 : : double matAuxIn[9], matAuxOut[9];
791 [ - - ]: 0 : for (std::size_t i=0; i<3; ++i)
792 [ - - ]: 0 : for (std::size_t j=0; j<3; ++j)
793 : 0 : matAuxIn[i*3+j] = mat[i][j];
794 : :
795 : : // compute matAuxIn*refMat and store it into matAuxOut
796 : 0 : cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasNoTrans,
797 : : 3, 3, 3, 1.0, matAuxIn, 3, refMat, 3, 0.0, matAuxOut, 3);
798 : :
799 : : // matAuxOut -> matAuxIn
800 [ - - ]: 0 : for (std::size_t i=0; i<9; i++)
801 : : {
802 : 0 : matAuxIn[i] = matAuxOut[i];
803 : 0 : matAuxOut[i] = 0.0;
804 : : }
805 : :
806 : : // compute refMat^T*matAuxIn and store it into matAuxOut
807 : 0 : cblas_dgemm(CblasRowMajor, CblasTrans, CblasNoTrans,
808 : : 3, 3, 3, 1.0, refMat, 3, matAuxIn, 3, 0.0, matAuxOut, 3);
809 : :
810 : : // return matAuxOut as a 2D array
811 : 0 : return {{ {matAuxOut[0], matAuxOut[1], matAuxOut[2]},
812 : 0 : {matAuxOut[3], matAuxOut[4], matAuxOut[5]},
813 : 0 : {matAuxOut[6], matAuxOut[7], matAuxOut[8]} }};
814 : : }
815 : :
816 : : } // tk::
817 : :
818 : : #endif // Vector_h
|
