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 : 1041452204 : rx = v1y*v2z - v2y*v1z;
72 : 1041452204 : ry = v1z*v2x - v2z*v1x;
73 : 1029288387 : 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 [ + + ][ + + ]: 1566684370 : 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 : 675360 : std::array< real, 3 > mv{0, 0, 0};
147 [ + + ][ + + ]: 2701440 : for (std::size_t i=0; i<3; ++i) {
148 [ + + ][ + + ]: 8104320 : for (std::size_t j=0; j<3; ++j)
149 : 6078240 : 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 [ + + ][ + + ]: 18993835 : 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 : 804196661 : 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 : 804196661 : std::array< real, 3 > ba{{ v2[0]-v1[0], v2[1]-v1[1], v2[2]-v1[2] }},
289 : 804196661 : ca{{ v3[0]-v1[0], v3[1]-v1[1], v3[2]-v1[2] }},
290 : 804196661 : da{{ v4[0]-v1[0], v4[1]-v1[1], v4[2]-v1[2] }};
291 : 804196661 : 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 : 13831654 : 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 : 13831654 : auto detJ = Jacobian( v1, v2, v3, v4 );
311 : :
312 : 13831654 : jacInv[0][0] = ( (v3[1]-v1[1])*(v4[2]-v1[2])
313 : 13831654 : - (v4[1]-v1[1])*(v3[2]-v1[2])) / detJ;
314 : 13831654 : jacInv[1][0] = -( (v2[1]-v1[1])*(v4[2]-v1[2])
315 : 13831654 : - (v4[1]-v1[1])*(v2[2]-v1[2])) / detJ;
316 : 13831654 : jacInv[2][0] = ( (v2[1]-v1[1])*(v3[2]-v1[2])
317 : 13831654 : - (v3[1]-v1[1])*(v2[2]-v1[2])) / detJ;
318 : :
319 : 13831654 : jacInv[0][1] = -( (v3[0]-v1[0])*(v4[2]-v1[2])
320 : 13831654 : - (v4[0]-v1[0])*(v3[2]-v1[2])) / detJ;
321 : 13831654 : jacInv[1][1] = ( (v2[0]-v1[0])*(v4[2]-v1[2])
322 : 13831654 : - (v4[0]-v1[0])*(v2[2]-v1[2])) / detJ;
323 : 13831654 : jacInv[2][1] = -( (v2[0]-v1[0])*(v3[2]-v1[2])
324 : 13831654 : - (v3[0]-v1[0])*(v2[2]-v1[2])) / detJ;
325 : :
326 : 13831654 : jacInv[0][2] = ( (v3[0]-v1[0])*(v4[1]-v1[1])
327 : 13831654 : - (v4[0]-v1[0])*(v3[1]-v1[1])) / detJ;
328 : 13831654 : jacInv[1][2] = -( (v2[0]-v1[0])*(v4[1]-v1[1])
329 : 13831654 : - (v4[0]-v1[0])*(v2[1]-v1[1])) / detJ;
330 : 13831654 : jacInv[2][2] = ( (v2[0]-v1[0])*(v3[1]-v1[1])
331 : 13831654 : - (v3[0]-v1[0])*(v2[1]-v1[1])) / detJ;
332 : :
333 : 13831654 : 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 : 128384810 : determinant( const std::array< std::array< tk::real, 3 >, 3 >& a )
341 : : {
342 : 128384810 : return ( a[0][0] * (a[1][1]*a[2][2]-a[1][2]*a[2][1])
343 : 128384810 : - a[0][1] * (a[1][0]*a[2][2]-a[1][2]*a[2][0])
344 : 128384810 : + a[0][2] * (a[1][0]*a[2][1]-a[1][1]*a[2][0]) );
345 : : }
346 : :
347 : : //! Solve a 3x3 system of equations using Cramer's rule
348 : : //! \param[in] a 3x3 lhs matrix
349 : : //! \param[in] b 3x1 rhs matrix
350 : : //! \return Array of solutions of the 3x3 system
351 : : inline std::array < tk::real, 3 >
352 : 31854540 : cramer( const std::array< std::array< tk::real, 3 >, 3>& a,
353 : : const std::array< tk::real, 3 >& b )
354 : : {
355 : 31854540 : auto de = determinant( a );
356 : :
357 : : auto nu(0.0);
358 : : std::array < real, 3 > x;
359 : :
360 : 31854540 : nu = determinant( {{{{b[0], a[0][1], a[0][2]}},
361 : : {{b[1], a[1][1], a[1][2]}},
362 : : {{b[2], a[2][1], a[2][2]}}}} );
363 : 31854540 : x[0] = nu/de;
364 : :
365 : 31854540 : nu = determinant( {{{{a[0][0], b[0], a[0][2]}},
366 : : {{a[1][0], b[1], a[1][2]}},
367 : : {{a[2][0], b[2], a[2][2]}}}} );
368 : 31854540 : x[1] = nu/de;
369 : :
370 : 31854540 : nu = determinant( {{{{a[0][0], a[0][1], b[0]}},
371 : : {{a[1][0], a[1][1], b[1]}},
372 : : {{a[2][0], a[2][1], b[2]}}}} );
373 : 31854540 : x[2] = nu/de;
374 : :
375 : 31854540 : return x;
376 : : }
377 : :
378 : : //! Move a point to a reference space given coordinates of origin of that space
379 : : //! \param[in] origin Origin of reference frame to which point is to be moved
380 : : //! \param[in,out] point Point that needs to be moved to reference frame
381 : : inline void
382 : : movePoint( const std::array< tk::real, 3 >& origin,
383 : : std::array< tk::real, 3 >& point )
384 : : {
385 [ + + ][ + + ]: 353602 : for (std::size_t i=0; i<3; ++i)
[ + + ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ + + ]
[ + + ][ + + ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ][ - - ]
[ - - ]
386 : 266025 : point[i] -= origin[i];
387 : : }
388 : :
389 : : //! Rotate a point in 3D space by specifying rotation angles in degrees
390 : : //! \param[in] angles Angles in 3D space by which point is to be rotated
391 : : //! \param[in,out] point Point that needs to be rotated
392 : : inline void
393 : 86469 : rotatePoint( const std::array< tk::real, 3 >& angles,
394 : : std::array< tk::real, 3 >& point )
395 : : {
396 : : // Convert angles to radian
397 : : tk::real pi = 4.0*std::atan(1.0);
398 : 86469 : auto a = angles[0] * pi/180.0;
399 : 86469 : auto b = angles[1] * pi/180.0;
400 : 86469 : auto c = angles[2] * pi/180.0;
401 : :
402 : : // Rotation matrix
403 : : std::array< std::array< tk::real, 3 >, 3 > rotMat;
404 : : {
405 : : using namespace std;
406 : 86469 : rotMat[0][0] = cos(b)*cos(c);
407 : 86469 : rotMat[0][1] = - cos(b)*sin(c);
408 : 86469 : rotMat[0][2] = sin(b);
409 : :
410 : 86469 : rotMat[1][0] = sin(a)*sin(b)*cos(c) + cos(a)*sin(c);
411 : 86469 : rotMat[1][1] = - sin(a)*sin(b)*sin(c) + cos(a)*cos(c);
412 : 86469 : rotMat[1][2] = - sin(a)*cos(b);
413 : :
414 : 86469 : rotMat[2][0] = - cos(a)*sin(b)*cos(c) + sin(a)*sin(c);
415 : 86469 : rotMat[2][1] = cos(a)*sin(b)*sin(c) + sin(a)*cos(c);
416 : 86469 : rotMat[2][2] = cos(a)*cos(b);
417 : : }
418 : :
419 : : // Apply rotation
420 : 86469 : std::array< tk::real, 3 > x{{0.0, 0.0, 0.0}};
421 [ + + ]: 345876 : for (std::size_t i=0; i<3; ++i) {
422 [ + + ]: 1037628 : for (std::size_t j=0; j<3; ++j) {
423 : 778221 : x[i] += rotMat[i][j]*point[j];
424 : : }
425 : : }
426 : 86469 : point = x;
427 : 86469 : }
428 : :
429 : : //! \brief Get the Right Cauchy-Green strain tensor from the inverse deformation
430 : : //! gradient tensor.
431 : : //! \param[in] g Inverse deformation gradient tensor
432 : : //! \return Right Cauchy-Green tensor
433 : : inline std::array< std::array< real, 3 >, 3 >
434 : 864102 : getRightCauchyGreen(const std::array< std::array< real, 3 >, 3 >& g)
435 : : {
436 : : // allocate matrices
437 : : double G[9], C[9];
438 : :
439 : : // initialize c-matrices
440 [ + + ]: 3456408 : for (std::size_t i=0; i<3; ++i) {
441 [ + + ]: 10369224 : for (std::size_t j=0; j<3; ++j)
442 : 7776918 : G[i*3+j] = g[i][j];
443 : : }
444 : :
445 : : // get g.g^T
446 : 864102 : cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasTrans,
447 : : 3, 3, 3, 1.0, G, 3, G, 3, 0.0, C, 3);
448 : :
449 : : // get inv(g.g^T)
450 : : lapack_int ipiv[9];
451 : :
452 : : #ifndef NDEBUG
453 : : lapack_int ierr =
454 : : #endif
455 : 864102 : LAPACKE_dgetrf(LAPACK_ROW_MAJOR, 3, 3, C, 3, ipiv);
456 : : Assert(ierr==0, "Lapack error in LU factorization of g.g^T");
457 : :
458 : : #ifndef NDEBUG
459 : : lapack_int jerr =
460 : : #endif
461 : 864102 : LAPACKE_dgetri(LAPACK_ROW_MAJOR, 3, C, 3, ipiv);
462 : : Assert(jerr==0, "Lapack error in inverting g.g^T");
463 : :
464 : : // Output C as 2D array
465 : 864102 : return {{ {C[0], C[1], C[2]},
466 : 864102 : {C[3], C[4], C[5]},
467 : 864102 : {C[6], C[7], C[8]} }};
468 : : }
469 : :
470 : : //! \brief Get the Left Cauchy-Green strain tensor from the inverse deformation
471 : : //! gradient tensor.
472 : : //! \param[in] g Inverse deformation gradient tensor
473 : : //! \return Left Cauchy-Green tensor
474 : : inline std::array< std::array< real, 3 >, 3 >
475 : 102548 : getLeftCauchyGreen(const std::array< std::array< real, 3 >, 3 >& g)
476 : : {
477 : : // allocate matrices
478 : : double G[9], b[9];
479 : :
480 : : // initialize c-matrices
481 [ + + ]: 410192 : for (std::size_t i=0; i<3; ++i) {
482 [ + + ]: 1230576 : for (std::size_t j=0; j<3; ++j)
483 : 922932 : G[i*3+j] = g[i][j];
484 : : }
485 : :
486 : : // get g^T.g
487 : 102548 : cblas_dgemm(CblasRowMajor, CblasTrans, CblasNoTrans,
488 : : 3, 3, 3, 1.0, G, 3, G, 3, 0.0, b, 3);
489 : :
490 : : // get inv(g^T.g)
491 : : lapack_int ipiv[9];
492 : :
493 : : #ifndef NDEBUG
494 : : lapack_int ierr =
495 : : #endif
496 : 102548 : LAPACKE_dgetrf(LAPACK_ROW_MAJOR, 3, 3, b, 3, ipiv);
497 : : Assert(ierr==0, "Lapack error in LU factorization of g^T.g");
498 : :
499 : : #ifndef NDEBUG
500 : : lapack_int jerr =
501 : : #endif
502 : 102548 : LAPACKE_dgetri(LAPACK_ROW_MAJOR, 3, b, 3, ipiv);
503 : : Assert(jerr==0, "Lapack error in inverting g^T.g");
504 : :
505 : : // Output b as 2D array
506 : 102548 : return {{ {b[0], b[1], b[2]},
507 : 102548 : {b[3], b[4], b[5]},
508 : 102548 : {b[6], b[7], b[8]} }};
509 : : }
510 : :
511 : : //! \brief Rotate a second order tensor (e.g. a Strain/Stress matrix) from
512 : : //! the (x,y,z) to a new (r,s,t) coordinate system.
513 : : //! The first direction is given by a unit vector r = (rx,ry,rz).
514 : : //! Then, the second is chosen to be:
515 : : //! if |rx| > 0 or |ry| > 0:
516 : : //! - s = (ry/sqrt(rx*rx+ry*ry),-rx/sqrt(rx*rx+ry*ry),0)
517 : : //! else:
518 : : //! - s = (1,0,0)
519 : : //! Then, third basis vector is obtained from
520 : : //! the cross-product between the first two.
521 : : //! \param[in] mat matrix to be rotated.
522 : : //! \param[in] r Coordinates of the first basis vector r = (rx,ry,rz).
523 : : //! \return rotated tensor
524 : : inline std::array< std::array< tk::real, 3 >, 3 >
525 : 1643811 : rotateTensor(const std::array< std::array< tk::real, 3 >, 3 >& mat,
526 : : const std::array< tk::real, 3 >& r )
527 : : {
528 : : // define rotation matrix
529 : : tk::real eps = 1.0e-04;
530 : : double rotMat[9];
531 : 1643811 : tk::real rx = r[0];
532 : 1643811 : tk::real ry = r[1];
533 [ + + ]: 1643811 : tk::real rz = r[2];
534 [ + + ][ + + ]: 1643811 : if (std::abs(rx) > eps || std::abs(ry) > eps)
535 : : {
536 : 1452318 : tk::real rxryNorm = std::sqrt(rx*rx+ry*ry);
537 : 1452318 : rotMat[0] = rx;
538 : 1452318 : rotMat[1] = ry;
539 : 1452318 : rotMat[2] = rz;
540 : 1452318 : rotMat[3] = ry/rxryNorm;
541 : 1452318 : rotMat[4] = -rx/rxryNorm;
542 : 1452318 : rotMat[5] = 0.0;
543 : 1452318 : rotMat[6] = rx*rz/rxryNorm;
544 : 1452318 : rotMat[7] = ry*rz/rxryNorm;
545 : 1452318 : rotMat[8] = -rxryNorm;
546 : : }
547 : : else
548 : : {
549 : 191493 : rotMat[0] = rx;
550 : 191493 : rotMat[1] = ry;
551 : 191493 : rotMat[2] = rz;
552 : 191493 : rotMat[3] = 1.0;
553 : 191493 : rotMat[4] = 0.0;
554 : 191493 : rotMat[5] = 0.0;
555 : 191493 : rotMat[6] = 0.0;
556 : 191493 : rotMat[7] = 1.0;
557 : 191493 : rotMat[8] = 0.0;
558 : : }
559 : :
560 : : // define matrices
561 : : double matAuxIn[9], matAuxOut[9];
562 [ + + ]: 6575244 : for (std::size_t i=0; i<3; ++i)
563 [ + + ]: 19725732 : for (std::size_t j=0; j<3; ++j)
564 : 14794299 : matAuxIn[i*3+j] = mat[i][j];
565 : :
566 : : // compute matAuxIn*rotMat and store it into matAuxOut
567 : 1643811 : cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasNoTrans,
568 : : 3, 3, 3, 1.0, matAuxIn, 3, rotMat, 3, 0.0, matAuxOut, 3);
569 : :
570 : : // matAuxOut -> matAuxIn
571 [ + + ]: 16438110 : for (std::size_t i=0; i<9; i++)
572 : : {
573 : 14794299 : matAuxIn[i] = matAuxOut[i];
574 : 14794299 : matAuxOut[i] = 0.0;
575 : : }
576 : :
577 : : // compute rotMat^T*matAuxIn and store it into matAuxOut
578 : 1643811 : cblas_dgemm(CblasRowMajor, CblasTrans, CblasNoTrans,
579 : : 3, 3, 3, 1.0, rotMat, 3, matAuxIn, 3, 0.0, matAuxOut, 3);
580 : :
581 : : // return matAuxOut as a 2D array
582 : 1643811 : return {{ {matAuxOut[0], matAuxOut[1], matAuxOut[2]},
583 : 1643811 : {matAuxOut[3], matAuxOut[4], matAuxOut[5]},
584 : 1643811 : {matAuxOut[6], matAuxOut[7], matAuxOut[8]} }};
585 : : }
586 : :
587 : : //! \brief Reflect a second order tensor (e.g. a Strain/Stress matrix)
588 : : //! \param[in] mat matrix to be rotated.
589 : : //! \param[in] reflectMat Reflection matrix
590 : : //! \return reflected tensor
591 : : inline std::array< std::array< tk::real, 3 >, 3 >
592 : 54000 : reflectTensor(const std::array< std::array< tk::real, 3 >, 3 >& mat,
593 : : const std::array< std::array< tk::real, 3 >, 3 >& reflectMat)
594 : : {
595 : : // define reflection matrix
596 : : double refMat[9];
597 [ + + ]: 216000 : for (std::size_t i=0; i<3; ++i)
598 [ + + ]: 648000 : for (std::size_t j=0; j<3; ++j)
599 : 486000 : refMat[i*3+j] = reflectMat[i][j];
600 : :
601 : : // define matAux (I need matrices as row major 1D arrays)
602 : : double matAuxIn[9], matAuxOut[9];
603 [ + + ]: 216000 : for (std::size_t i=0; i<3; ++i)
604 [ + + ]: 648000 : for (std::size_t j=0; j<3; ++j)
605 : 486000 : matAuxIn[i*3+j] = mat[i][j];
606 : :
607 : : // compute matAuxIn*refMat and store it into matAuxOut
608 : 54000 : cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasNoTrans,
609 : : 3, 3, 3, 1.0, matAuxIn, 3, refMat, 3, 0.0, matAuxOut, 3);
610 : :
611 : : // matAuxOut -> matAuxIn
612 [ + + ]: 540000 : for (std::size_t i=0; i<9; i++)
613 : : {
614 : 486000 : matAuxIn[i] = matAuxOut[i];
615 : 486000 : matAuxOut[i] = 0.0;
616 : : }
617 : :
618 : : // compute refMat^T*matAuxIn and store it into matAuxOut
619 : 54000 : cblas_dgemm(CblasRowMajor, CblasTrans, CblasNoTrans,
620 : : 3, 3, 3, 1.0, refMat, 3, matAuxIn, 3, 0.0, matAuxOut, 3);
621 : :
622 : : // return matAuxOut as a 2D array
623 : 54000 : return {{ {matAuxOut[0], matAuxOut[1], matAuxOut[2]},
624 : 54000 : {matAuxOut[3], matAuxOut[4], matAuxOut[5]},
625 : 54000 : {matAuxOut[6], matAuxOut[7], matAuxOut[8]} }};
626 : : }
627 : :
628 : : } // tk::
629 : :
630 : : #endif // Vector_h
|
