1 /* ----------------------------------------------------------------------
2 This is the
3
4 ██╗ ██╗ ██████╗ ██████╗ ██████╗ ██╗ ██╗████████╗███████╗
5 ██║ ██║██╔════╝ ██╔════╝ ██╔════╝ ██║ ██║╚══██╔══╝██╔════╝
6 ██║ ██║██║ ███╗██║ ███╗██║ ███╗███████║ ██║ ███████╗
7 ██║ ██║██║ ██║██║ ██║██║ ██║██╔══██║ ██║ ╚════██║
8 ███████╗██║╚██████╔╝╚██████╔╝╚██████╔╝██║ ██║ ██║ ███████║
9 ╚══════╝╚═╝ ╚═════╝ ╚═════╝ ╚═════╝ ╚═╝ ╚═╝ ╚═╝ ╚══════╝®
10
11 DEM simulation engine, released by
12 DCS Computing Gmbh, Linz, Austria
13 http://www.dcs-computing.com, office@dcs-computing.com
14
15 LIGGGHTS® is part of CFDEM®project:
16 http://www.liggghts.com | http://www.cfdem.com
17
18 Core developer and main author:
19 Christoph Kloss, christoph.kloss@dcs-computing.com
20
21 LIGGGHTS® is open-source, distributed under the terms of the GNU Public
22 License, version 2 or later. It is distributed in the hope that it will
23 be useful, but WITHOUT ANY WARRANTY; without even the implied warranty
24 of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. You should have
25 received a copy of the GNU General Public License along with LIGGGHTS®.
26 If not, see http://www.gnu.org/licenses . See also top-level README
27 and LICENSE files.
28
29 LIGGGHTS® and CFDEM® are registered trade marks of DCS Computing GmbH,
30 the producer of the LIGGGHTS® software and the CFDEM®coupling software
31 See http://www.cfdem.com/terms-trademark-policy for details.
32
33 -------------------------------------------------------------------------
34 Contributing author and copyright for this file:
35 Alexander Podlozhnyuk (DCS Computing GmbH, Linz)
36
37 Copyright 2015- DCS Computing GmbH, Linz
38 ------------------------------------------------------------------------- */
39
40 #ifndef LMP_MATH_EXTRA_LIGGGHTS_NONSPHERICAL_H
41 #define LMP_MATH_EXTRA_LIGGGHTS_NONSPHERICAL_H
42
43 #include "pointers.h"
44 #include <cmath>
45 #include <stdio.h>
46 #include <string.h>
47 #include "vector_liggghts.h"
48 #include "math_extra.h"
49 #include "random_park.h"
50 #include "ctype.h"
51 #include "contact_interface.h"
52 #include "math_extra_liggghts.h"
53
54 #define TOLERANCE_ORTHO 1e-10
55
56 using namespace LIGGGHTS::ContactModels;
57
58 namespace MathExtraLiggghtsNonspherical {
59
60 const double scale_koef = 1.0001;
61
62 struct point {
63 double x,y,z;
pointpoint64 point():
65 x(0), y(0), z(0) {};
pointpoint66 point(double *coords):
67 x(coords[0]), y(coords[1]), z(coords[2]) {}
68 };
69
70 inline double sign(const double val);
71 inline double pow_abs(double a, double b);
72 inline double normN(const double *a, const int len);
73 inline double normNsq(const double *a, const int len);
74 inline void vectorCopyN(const double *from, const int len, double *to);
75 inline double dotN(const double *a, const double *b, const int len);
76 inline void addN(const double *a, const double *b, const int len, double *result);
77 inline void subN(const double *a, const double *b, const int len, double *result);
78 inline void mulN(const double *a, const double k, const int len, double *result);
79 inline void mulN(const double k, const double *a, const int len, double *result);
80 inline void matvecN(const double *A, const double *b, const int len, double *c);
81 inline void matmat(const double *A, double *B, const int N, const int K, const int M, double *C);
82 inline void zeros(double *a, const int len);
83 inline void tensor_quat_rotate(const double *mat, const double *quat, double *result);
84 template<int N, int m_start> void GMRES(const double *A, const double *b, const double *x0, double *x);
85 inline double distsq(const double *v1, const double *v2, int len);
86 inline double dist(const double *v1, const double *v2, int len);
87 double point_wall_projection(const double *normal_vector, const double *point_on_wall, const double *input_point, double *output_point);
88 inline void rotate_local2global(const double *quat, const double *input_coord, double *result);
89 inline void rotate_global2local(const double *quat, const double *input_coord, double *result);
90 double point_line_distance(double *pointA, double *pointB, double *input_point, double *output_point, double *delta);
91 void integrate_quat(double *quat, double *omega, double dtq);
92 void integrate_omega(double *quat, double *torque, double *inertia, double *omega, double dt);
93
94 void angmom_to_omega(const double *quat, const double *angmom, const double *inertia, double *omega);
95 void omega_to_angmom(const double *quat, const double *omega, const double *inertia, double *angmom);
96 inline void rotate_local2global(const double *mat, const double *input_coord, double *result);
97 inline void rotate_global2local(const double *mat, const double *input_coord, double *result);
98 void no_squish_rotate(int k, double *p, double *q, double *inertia, double dt);
99 void calc_conjqm(double *quat, double *angmom_body, double *conjqm);
100 inline void invquat(double *quat);
101 double clamp(double val, double min, double max);
102 void line_segments_distance(double *P1, double *Q1, double *P2, double *Q2, double *C1, double *C2);
103 double get_effective_radius(SurfacesIntersectData & sidata, double *blockiness_i, double *blockiness_j,double koefi, double koefj, double curvatureLimitFactor,LAMMPS_NS::Error *error);
104 double get_effective_radius_wall(SurfacesIntersectData & sidata, double *blockiness_i, double koefi, double curvatureLimitFactor,LAMMPS_NS::Error *error);
105 inline void yabx3D(double *a, double b, double *x, double *y);
106 inline double pow_abs_int(double a, int b);
107 inline int round_int(double x);
108 inline void matvec(const double *m, const double *v, double *ans);
109 inline void transpose_matvec(const double *m, const double *v, double *ans);
110 inline void times3(const double *m, const double *m2, double *ans);
111 inline void transpose_times3(const double *m, const double *m2, double *ans);
112 inline void times3_transpose(const double *m, const double *m2, double *ans);
113 inline void quat_to_mat(const double *quat, double *rotation_matrix);
114
115 inline bool isInteger(double x);
116 inline void surfacesIntersectNonSpherical(SurfacesIntersectData & sidata, double **x);
117 };
118
119 //rotate tensor by quaternion
tensor_quat_rotate(const double * tensor,const double * quat,double * result)120 inline void MathExtraLiggghtsNonspherical::tensor_quat_rotate(const double *tensor, const double *quat, double *result)
121 {
122 double rotation_matrix[9], temp[9];
123 MathExtraLiggghtsNonspherical::quat_to_mat(quat, rotation_matrix); //calc rotation matrix (A) from quaternion
124 MathExtraLiggghtsNonspherical::times3(rotation_matrix, tensor, temp); // calc temp = A * M
125 MathExtraLiggghtsNonspherical::times3_transpose(temp, rotation_matrix, result); //calc result = A * M * A^T
126 }
127
128 //calculation of 2-norm of the vector
normN(const double * a,const int len)129 inline double MathExtraLiggghtsNonspherical::normN(const double *a, const int len)
130 {
131 double result = 0.0;
132 for(int i = 0; i < len; i++)
133 result += a[i]*a[i];
134 return ::sqrt(result);
135 }
136
pow_abs_int(double a,int b)137 inline double MathExtraLiggghtsNonspherical::pow_abs_int(double a, int b)
138 {
139 double result;
140 a = fabs(a);
141 switch(b) {
142 case 0:
143 result = 1.0;
144 break;
145 case 1:
146 result = a;
147 break;
148 case 2:
149 result = a*a;
150 break;
151 case 3:
152 result = a*a*a;
153 break;
154 case 4: {
155 double a2 = a*a;
156 result = a2*a2;
157 break;
158 }
159 case 5: {
160 double a2 = a*a;
161 result = a2*a2*a;
162 break;
163 }
164 case 6: {
165 double a3 = a*a*a;
166 result = a3*a3;
167 break;
168 }
169 case 7: {
170 double a2 = a*a;
171 double a3 = a2*a;
172 result = a2*a2*a3;
173 break;
174 }
175 case 8: {
176 double a2 = a*a;
177 double a4 = a2*a2;
178 result = a4*a4;
179 break;
180 }
181 case 9: {
182 double a3 = a*a*a;
183 result = a3*a3*a3;
184 break;
185 }
186 default:
187 result = a;
188 for(int i = 0; i < b - 1; i++)
189 result *= a;
190 break;
191 }
192 return result;
193 }
194
pow_abs(double base,double exponent)195 inline double MathExtraLiggghtsNonspherical::pow_abs(double base, double exponent)
196 {
197 base = fabs(base);
198 double tol = 1e-12;
199 if(fabs(exponent)<tol or fabs(base - 1.0) < tol)
200 return 1.0;
201 else if(fabs(base) < tol)
202 return 0.0;
203 else
204 {
205 double n = floor(fabs(exponent));
206 int n_int = static_cast<int>(n);
207 double result = pow_abs_int(base, n_int);
208 double n_delta = fabs(exponent) - n;
209 if(n_delta > 0.0) {
210 if(fabs(2.0*n_delta - 1.0) < 2.0*tol)
211 result *= ::sqrt(base);
212 else
213 {
214 result *= ::pow(base, n_delta);
215 }
216 }
217 if(exponent > 0.0)
218 return result;
219 else
220 return 1.0 / result;
221 }
222 }
223
224 //GMRES Linear System Solver
225 //https://math.berkeley.edu/~mgu/MA228A/saad-schultz.pdf
GMRES(const double * A,const double * b,const double * x0,double * x)226 template<int N, int m_start> void MathExtraLiggghtsNonspherical::GMRES(const double *A, const double *b, const double *x0, double *x)
227 {
228 int m = m_start;
229 double res[N];
230 double Ax0[N];
231 matvecN(A, x0, N, Ax0); // calc A*x0
232 subN(b, Ax0, N, res); //res = b - A*x0
233 vectorCopyN(x0, N, x);
234 double beta_sq = normNsq(res, N); //beta = ||res||
235 if(beta_sq < 1e-20) {
236 return;
237 }
238
239 double beta = ::sqrt(beta_sq);
240
241 double y[m_start];
242 double v[(m_start+1)*N];
243 double h[(m_start+1)*m_start];
244 double w[m_start*N];
245
246 zeros(y, m);
247 zeros(v, (m+1)*N);
248 zeros(h, (m+1)*m);
249 zeros(w, m*N);
250
251 mulN(1.0/beta, res, N, v); //v0 = res / beta
252
253 for(int j = 0; j < m; j++) {
254 matvecN(A, v + j*N, N, w + j*N);
255 for(int i = 0; i < j + 1; i++) {
256 for(int ix = 0; ix < N; ix++) {
257 h[i*m+j] += w[j*N + ix]*v[i*N + ix];
258 }
259 for(int ix = 0; ix < N; ix++)
260 w[j*N + ix] -= h[i*m+j]*v[i*N + ix];
261 }
262 h[(j+1)*m + j] = normN(w + j*N, N);
263
264 if(h[(j+1)*m + j] < 1e-12) {
265 m = j + 1;
266 break;
267 } else
268 mulN(1.0/h[(j+1)*m + j], w+j*N, N, v+(j+1)*N);
269 }
270
271 double hm[(m_start+1)*m_start];
272 double g[m_start+1];
273 zeros(g, m+1);
274 g[0] = beta;
275
276 for(int i = 0; i < m + 1; i++) {
277 for(int j = 0; j < m; j++) {
278 hm[i*m_start+j] = h[i*m_start+j];
279 }
280 }
281
282 double row1[m_start];
283 double row2[m_start];
284
285 for(int j = 0; j < m; j++) {
286 double root_inv = 1.0 / sqrt(hm[j*m_start+j]*hm[j*m_start+j] + hm[(j+1)*m_start+j]*hm[(j+1)*m_start+j]);
287 double s = hm[(j+1)*m_start+j] * root_inv;
288 double c = hm[j*m_start+j] * root_inv;
289
290 for(int i = 0; i < m; i++) {
291 row1[i] = c*hm[j*m_start+i] + s*hm[(j+1)*m_start+i];
292 row2[i] = -s*hm[j*m_start+i] + c*hm[(j+1)*m_start+i];
293 }
294 for(int i = 0; i < m; i++) {
295 hm[j*m_start+i] = row1[i];
296 hm[(j+1)*m_start+i] = row2[i];
297 }
298 double g1_temp = c*g[j] + s*g[j+1];
299 double g2_temp = -s*g[j] + c*g[j+1];
300
301 g[j] = g1_temp;
302 g[j + 1] = g2_temp;
303 }
304
305 for(int j = 0; j < m; j++) {
306 int j1 = m - 1 - j;
307 double summ = 0.0;
308 for(int i = 0; i < j; i++) {
309 int i1 = m - 1 - i;
310 summ += hm[j1*m_start+i1] * y[i1];
311 }
312 y[j1] = (g[j1] - summ) / hm[j1*m_start + j1];
313 for(int i = 0; i < N; i++)
314 x[i] += y[j1] * v[j1*N+i];
315 }
316 }
317
318 //signum function
sign(const double val)319 inline double MathExtraLiggghtsNonspherical::sign(const double val)
320 {
321 if(val > 1e-14)
322 return 1.0;
323 else if (val < -1e-14)
324 return -1.0;
325 else // -1e-14 <= val <= 1e-14
326 return 0.0;
327 }
328
329 //calculation of 2-norm of the vector
normNsq(const double * a,const int len)330 inline double MathExtraLiggghtsNonspherical::normNsq(const double *a, const int len)
331 {
332 double result = 0.0;
333 for(int i = 0; i < len; i++)
334 result += a[i]*a[i];
335 return result;
336 }
337
338 //copy vector
vectorCopyN(const double * from,const int len,double * to)339 inline void MathExtraLiggghtsNonspherical::vectorCopyN(const double *from, const int len, double *to)
340 {
341 for(int i = 0; i < len; i++)
342 to[i] = from[i];
343 }
344
345 //vector addition: result = a+b
addN(const double * a,const double * b,const int len,double * result)346 inline void MathExtraLiggghtsNonspherical::addN(const double *a, const double *b, const int len, double *result)
347 {
348 for(int i = 0; i < len; i++)
349 result[i] = a[i] + b[i];
350 }
351
352 //vector substract: result = a-b
subN(const double * a,const double * b,const int len,double * result)353 inline void MathExtraLiggghtsNonspherical::subN(const double *a, const double *b, const int len, double *result)
354 {
355 for(int i = 0; i < len; i++)
356 result[i] = a[i] - b[i];
357 }
358
359 //vector-scalar multiplication
mulN(const double * a,const double k,const int len,double * result)360 inline void MathExtraLiggghtsNonspherical::mulN(const double *a, const double k, const int len, double *result)
361 {
362 for(int i = 0; i < len; i++)
363 result[i] = a[i]*k;
364 }
365
366 //vector-scalar multiplication
mulN(const double k,const double * a,const int len,double * result)367 inline void MathExtraLiggghtsNonspherical::mulN(const double k, const double *a, const int len, double *result)
368 {
369 for(int i = 0; i < len; i++)
370 result[i] = a[i]*k;
371 }
372
373 //dot product of two vectors a and b of length N
dotN(const double * a,const double * b,const int len)374 inline double MathExtraLiggghtsNonspherical::dotN(const double *a, const double *b, const int len)
375 {
376 double result = 0.0;
377 for(int i = 0; i < len; i++)
378 result += a[i]*b[i];
379 return result;
380 }
381
382 //matrix-vector multiplication, size of A is NxN, size of b is N, (N = len)
matvecN(const double * A,const double * b,const int len,double * c)383 inline void MathExtraLiggghtsNonspherical::matvecN(const double *A, const double *b, const int len, double *c)
384 {
385 for(int i = 0; i < len; i++) {
386 c[i] = 0.0;
387 for(int j = 0; j < len; j++)
388 {
389 if(A[i*len+j] != 0.0 and b[j] != 0.0)
390 c[i] += A[i*len+j]*b[j];
391 }
392 }
393 }
394 //matrix-matrix multiplication: C = A*B, A(NxK), B(KxM), C(NxM)
matmat(const double * A,double * B,const int N,const int K,const int M,double * C)395 inline void MathExtraLiggghtsNonspherical::matmat(const double *A, double *B, const int N, const int K, const int M, double *C)
396 {
397 for(int iM = 0; iM < M; iM++) {
398 for(int iN = 0; iN < N; iN ++) {
399 C[iN * M + iM] = 0.0;
400 for(int iK = 0; iK < K; iK++) {
401 if(A[iN * K + iK] != 0.0 and B[iK * M + iM] != 0.0)
402 C[iN * M + iM] += A[iN * K + iK] * B[iK * M + iM];
403 }
404 }
405 }
406 }
407
408 //fill the first <len> elements of a with zeros
zeros(double * a,const int len)409 inline void MathExtraLiggghtsNonspherical::zeros(double *a, const int len)
410 {
411 memset(a, 0.0, len*sizeof(double));
412 }
413
414 //squared distance between two points in N-dimensional space
distsq(const double * v1,const double * v2,int len)415 inline double MathExtraLiggghtsNonspherical::distsq(const double *v1, const double *v2, int len)
416 {
417 double result = 0.0;
418 for(int i = 0; i < len; i++)
419 result += (v1[i]-v2[i])*(v1[i]-v2[i]);
420 return result;
421 }
422
423 //rotate vector from local to global reference frame with a given quaternion
rotate_local2global(const double * quat,const double * input_coord,double * result)424 inline void MathExtraLiggghtsNonspherical::rotate_local2global(const double *quat, const double *input_coord, double *result) {
425 double A[9];
426 MathExtraLiggghtsNonspherical::quat_to_mat(quat, A);
427 MathExtraLiggghtsNonspherical::matvec(A, input_coord, result);
428 }
429
430 //rotate vector from global to local reference frame with a given quaternion
rotate_global2local(const double * quat,const double * input_coord,double * result)431 inline void MathExtraLiggghtsNonspherical::rotate_global2local(const double *quat, const double *input_coord, double *result) {
432 double A[9];
433 MathExtraLiggghtsNonspherical::quat_to_mat(quat, A);
434 MathExtraLiggghtsNonspherical::transpose_matvec(A, input_coord, result);
435 }
436
invquat(double * quat)437 inline void MathExtraLiggghtsNonspherical::invquat(double *quat)
438 {
439 double norm_sq_inv = 1.0 / LAMMPS_NS::vectorMag4D(quat);
440 quat[0] *= norm_sq_inv;
441 quat[1] *= -norm_sq_inv;
442 quat[2] *= -norm_sq_inv;
443 quat[3] *= -norm_sq_inv;
444 }
445
yabx3D(double * a,double b,double * x,double * y)446 inline void MathExtraLiggghtsNonspherical::yabx3D(double *a, double b, double *x, double *y)
447 {
448 y[0] = a[0] + b*x[0];
449 y[1] = a[1] + b*x[1];
450 y[2] = a[2] + b*x[2];
451 }
452
round_int(double x)453 inline int MathExtraLiggghtsNonspherical::round_int(double x)
454 {
455 return static_cast<int>(x+0.5);
456 }
457
matvec(const double * m,const double * v,double * ans)458 inline void MathExtraLiggghtsNonspherical::matvec(const double *m, const double *v, double *ans)
459 {
460 ans[0] = m[0]*v[0] + m[1]*v[1] + m[2]*v[2];
461 ans[1] = m[3]*v[0] + m[4]*v[1] + m[5]*v[2];
462 ans[2] = m[6]*v[0] + m[7]*v[1] + m[8]*v[2];
463 }
464
transpose_matvec(const double * m,const double * v,double * ans)465 inline void MathExtraLiggghtsNonspherical::transpose_matvec(const double *m, const double *v, double *ans)
466 {
467 ans[0] = m[0]*v[0] + m[3]*v[1] + m[6]*v[2];
468 ans[1] = m[1]*v[0] + m[4]*v[1] + m[7]*v[2];
469 ans[2] = m[2]*v[0] + m[5]*v[1] + m[8]*v[2];
470 }
471
times3(const double * m,const double * m2,double * ans)472 inline void MathExtraLiggghtsNonspherical::times3(const double *m, const double *m2, double *ans)
473 {
474 ans[0*3+0] = m[0*3+0]*m2[0*3+0] + m[0*3+1]*m2[1*3+0] + m[0*3+2]*m2[2*3+0];
475 ans[0*3+1] = m[0*3+0]*m2[0*3+1] + m[0*3+1]*m2[1*3+1] + m[0*3+2]*m2[2*3+1];
476 ans[0*3+2] = m[0*3+0]*m2[0*3+2] + m[0*3+1]*m2[1*3+2] + m[0*3+2]*m2[2*3+2];
477 ans[1*3+0] = m[1*3+0]*m2[0*3+0] + m[1*3+1]*m2[1*3+0] + m[1*3+2]*m2[2*3+0];
478 ans[1*3+1] = m[1*3+0]*m2[0*3+1] + m[1*3+1]*m2[1*3+1] + m[1*3+2]*m2[2*3+1];
479 ans[1*3+2] = m[1*3+0]*m2[0*3+2] + m[1*3+1]*m2[1*3+2] + m[1*3+2]*m2[2*3+2];
480 ans[2*3+0] = m[2*3+0]*m2[0*3+0] + m[2*3+1]*m2[1*3+0] + m[2*3+2]*m2[2*3+0];
481 ans[2*3+1] = m[2*3+0]*m2[0*3+1] + m[2*3+1]*m2[1*3+1] + m[2*3+2]*m2[2*3+1];
482 ans[2*3+2] = m[2*3+0]*m2[0*3+2] + m[2*3+1]*m2[1*3+2] + m[2*3+2]*m2[2*3+2];
483 }
484
transpose_times3(const double * m,const double * m2,double * ans)485 inline void MathExtraLiggghtsNonspherical::transpose_times3(const double *m, const double *m2, double *ans)
486 {
487 ans[0*3+0] = m[0*3+0]*m2[0*3+0] + m[1*3+0]*m2[1*3+0] + m[2*3+0]*m2[2*3+0];
488 ans[0*3+1] = m[0*3+0]*m2[0*3+1] + m[1*3+0]*m2[1*3+1] + m[2*3+0]*m2[2*3+1];
489 ans[0*3+2] = m[0*3+0]*m2[0*3+2] + m[1*3+0]*m2[1*3+2] + m[2*3+0]*m2[2*3+2];
490 ans[1*3+0] = m[0*3+1]*m2[0*3+0] + m[1*3+1]*m2[1*3+0] + m[2*3+1]*m2[2*3+0];
491 ans[1*3+1] = m[0*3+1]*m2[0*3+1] + m[1*3+1]*m2[1*3+1] + m[2*3+1]*m2[2*3+1];
492 ans[1*3+2] = m[0*3+1]*m2[0*3+2] + m[1*3+1]*m2[1*3+2] + m[2*3+1]*m2[2*3+2];
493 ans[2*3+0] = m[0*3+2]*m2[0*3+0] + m[1*3+2]*m2[1*3+0] + m[2*3+2]*m2[2*3+0];
494 ans[2*3+1] = m[0*3+2]*m2[0*3+1] + m[1*3+2]*m2[1*3+1] + m[2*3+2]*m2[2*3+1];
495 ans[2*3+2] = m[0*3+2]*m2[0*3+2] + m[1*3+2]*m2[1*3+2] + m[2*3+2]*m2[2*3+2];
496 }
497
times3_transpose(const double * m,const double * m2,double * ans)498 inline void MathExtraLiggghtsNonspherical::times3_transpose(const double *m, const double *m2, double *ans)
499 {
500 ans[0*3+0] = m[0*3+0]*m2[0*3+0] + m[0*3+1]*m2[0*3+1] + m[0*3+2]*m2[0*3+2];
501 ans[0*3+1] = m[0*3+0]*m2[1*3+0] + m[0*3+1]*m2[1*3+1] + m[0*3+2]*m2[1*3+2];
502 ans[0*3+2] = m[0*3+0]*m2[2*3+0] + m[0*3+1]*m2[2*3+1] + m[0*3+2]*m2[2*3+2];
503 ans[1*3+0] = m[1*3+0]*m2[0*3+0] + m[1*3+1]*m2[0*3+1] + m[1*3+2]*m2[0*3+2];
504 ans[1*3+1] = m[1*3+0]*m2[1*3+0] + m[1*3+1]*m2[1*3+1] + m[1*3+2]*m2[1*3+2];
505 ans[1*3+2] = m[1*3+0]*m2[2*3+0] + m[1*3+1]*m2[2*3+1] + m[1*3+2]*m2[2*3+2];
506 ans[2*3+0] = m[2*3+0]*m2[0*3+0] + m[2*3+1]*m2[0*3+1] + m[2*3+2]*m2[0*3+2];
507 ans[2*3+1] = m[2*3+0]*m2[1*3+0] + m[2*3+1]*m2[1*3+1] + m[2*3+2]*m2[1*3+2];
508 ans[2*3+2] = m[2*3+0]*m2[2*3+0] + m[2*3+1]*m2[2*3+1] + m[2*3+2]*m2[2*3+2];
509 }
510
quat_to_mat(const double * quat,double * rotation_matrix)511 inline void MathExtraLiggghtsNonspherical::quat_to_mat(const double *quat, double *rotation_matrix) {
512 double w2 = quat[0]*quat[0];
513 double i2 = quat[1]*quat[1];
514 double j2 = quat[2]*quat[2];
515 double k2 = quat[3]*quat[3];
516 double twoij = 2.0*quat[1]*quat[2];
517 double twoik = 2.0*quat[1]*quat[3];
518 double twojk = 2.0*quat[2]*quat[3];
519 double twoiw = 2.0*quat[1]*quat[0];
520 double twojw = 2.0*quat[2]*quat[0];
521 double twokw = 2.0*quat[3]*quat[0];
522
523 rotation_matrix[0] = w2+i2-j2-k2;
524 rotation_matrix[1] = twoij-twokw;
525 rotation_matrix[2] = twojw+twoik;
526
527 rotation_matrix[3] = twoij+twokw;
528 rotation_matrix[4] = w2-i2+j2-k2;
529 rotation_matrix[5] = twojk-twoiw;
530
531 rotation_matrix[6] = twoik-twojw;
532 rotation_matrix[7] = twojk+twoiw;
533 rotation_matrix[8] = w2-i2-j2+k2;
534 }
535
surfacesIntersectNonSpherical(SurfacesIntersectData & sidata,double ** x)536 inline void MathExtraLiggghtsNonspherical::surfacesIntersectNonSpherical(SurfacesIntersectData & sidata, double **x)
537 {
538 #ifdef NONSPHERICAL_ACTIVE_FLAG
539 double xci[3], xcj[3];
540 double v_rot_i[3], v_rot_j[3];
541 double omega_relative[3], v_relative[3], v_rot_relative[3];
542 if(sidata.is_wall) {
543 LAMMPS_NS::vectorSubtract3D(sidata.contact_point, x[sidata.i], xci);
544 LAMMPS_NS::vectorCross3D(sidata.omega_i, xci, v_rot_i);
545 LAMMPS_NS::vectorCopy3D(v_rot_i, v_rot_relative);
546 LAMMPS_NS::vectorCopy3D(sidata.omega_i, omega_relative);
547 sidata.cri = LAMMPS_NS::vectorMag3D(xci);
548 } else {
549 LAMMPS_NS::vectorSubtract3D(sidata.contact_point, x[sidata.i], xci);
550 LAMMPS_NS::vectorSubtract3D(sidata.contact_point, x[sidata.j], xcj);
551 LAMMPS_NS::vectorCross3D(sidata.omega_i, xci, v_rot_i);
552 LAMMPS_NS::vectorCross3D(sidata.omega_j, xcj, v_rot_j);
553 LAMMPS_NS::vectorSubtract3D(v_rot_i, v_rot_j, v_rot_relative);
554 LAMMPS_NS::vectorSubtract3D(sidata.omega_i, sidata.omega_j, omega_relative);
555 sidata.cri = LAMMPS_NS::vectorMag3D(xci);
556 sidata.crj = LAMMPS_NS::vectorMag3D(xcj);
557 }
558 // relative velocity
559 v_relative[0] = sidata.v_i[0] - sidata.v_j[0] + v_rot_relative[0];
560 v_relative[1] = sidata.v_i[1] - sidata.v_j[1] + v_rot_relative[1];
561 v_relative[2] = sidata.v_i[2] - sidata.v_j[2] + v_rot_relative[2];
562
563 // normal component
564 const double vn = LAMMPS_NS::vectorDot3D(v_relative, sidata.en);
565 // tangential components
566 sidata.vtr1 = v_relative[0] - vn * sidata.en[0];
567 sidata.vtr2 = v_relative[1] - vn * sidata.en[1];
568 sidata.vtr3 = v_relative[2] - vn * sidata.en[2];
569
570 sidata.wr1 = omega_relative[0];
571 sidata.wr2 = omega_relative[1];
572 sidata.wr3 = omega_relative[2];
573
574 sidata.vn = vn;
575 #endif
576 }
577
isInteger(double x)578 inline bool MathExtraLiggghtsNonspherical::isInteger(double x)
579 {
580 double x_temp = floor(x + 1e-2);
581 return MathExtraLiggghts::compDouble(x, x_temp, 1e-2);
582 }
583 #endif
584