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 (if not contributing author is listed, this file has been contributed
36 by the core developer)
37
38 Copyright 2012- DCS Computing GmbH, Linz
39 Copyright 2009-2012 JKU Linz
40 ------------------------------------------------------------------------- */
41
42 #ifndef LMP_MATH_EXTRA_LIGGGHTS_H
43 #define LMP_MATH_EXTRA_LIGGGHTS_H
44
45 #include "pointers.h"
46 #include <cmath>
47 #include <algorithm>
48 #include <stdio.h>
49 #include <string.h>
50 #include "error.h"
51 #include "vector_liggghts.h"
52 #include "math_extra.h"
53 #include "random_park.h"
54 #include "ctype.h"
55
56 #define TOLERANCE_ORTHO 1e-10
57
58 namespace MathExtraLiggghts {
59
60 inline void col_times3(const double m[3][3],const double *v, double *ans);
61
62 inline double mdet(const double m[3][3],LAMMPS_NS::Error *error);
63
64 //cubic root approx
65 inline double cbrt_5d(double d);
66 inline double cbrta_halleyd(const double a, const double R);
67 inline double halley_cbrt1d(double d);
68
69 //exp aproximation
70 inline double exp_fast(double x);
71
72 inline double min(const double a, const double b, const double c);
73 inline double max(const double a, const double b, const double c);
74 inline double min(const double a, const double b, const double c, const double d);
75 inline double max(const double a, const double b, const double c, const double d);
76 template <typename T>
77 inline T min(const T * const input, const unsigned int n, int &which);
78 template <typename T>
79 inline T max(const T * const input, const unsigned int n, int &which);
80 template <typename T>
81 inline T abs(const T a);
82
83 inline void matrix_invert_4x4_special(double matrix[4][4]);
84 inline void transpose3(const double m[3][3], double ans[3][3]);
85 inline int is_inside_tet(double *pos,double invmatrix[4][4]);
86
87 inline void local_coosys_to_cartesian(double * const global, const double * const local, const double * const ex_local, const double * const ey_local, const double * const ez_local);
88 inline void cartesian_coosys_to_local(double *local,double *global, double *ex_local, double *ey_local, double *ez_local,LAMMPS_NS::Error *error);
89 inline void cartesian_coosys_to_local_orthogonal(double *local,double *global, double *ex_local, double *ey_local, double *ez_local,LAMMPS_NS::Error *error);
90
91 // quaternion operations
92 inline bool is_unit_quat(const double *q);
93 inline void quat_normalize(double *q);
94 inline void qconjugate(const double * const q, double * const qc);
95 inline void quat_from_vec(const double *v, double *q);
96 inline void vec_from_quat(const double *q, double * const v);
97 inline void vec_quat_rotate(const double * const vec, const double * const quat, double *result);
98 inline void vec_quat_rotate(double * const vec, const double * const quat);
vec_quat_rotate(int * const vec,const double * const quat)99 inline void vec_quat_rotate(int * const vec, const double * const quat) { UNUSED(vec); UNUSED(quat); }
vec_quat_rotate(bool * const vec,const double * const quat)100 inline void vec_quat_rotate(bool * const vec, const double * const quat) { UNUSED(vec); UNUSED(quat); }
101 inline void quat_diff(double *q_new, double *q_old, double *q_diff);
102 inline void angmom_from_omega(double *w,
103 double *ex, double *ey, double *ez,
104 double *idiag, double *m);
105
106 // double comparison
107 inline bool compDouble(double const a, double const b, double const prec = 1e-13);
108
109 // calculate barycentrc coordinates of p w.r.t node
110 inline void calcBaryTriCoords(double *p, double **edgeVec, double *edgeLen, double *bary);
111 inline void calcBaryTriCoords(double *p, double *edgeVec0, double *edgeVec1, double *edgeVec2, double *edgeLen, double *bary);
112
113 inline void random_unit_quat(LAMMPS_NS::RanPark *random,double *quat);
114
115 inline bool is_int(char *str);
116 inline void generateComplementBasis(double *uVec, double *vVec, double *direction);
117
118 // template signum function
119 template <typename T>
sgn(T val)120 int sgn(T val) {
121 return (T(0) < val) - (val < T(0));
122 }
123
124 // prime number test
125 inline bool isPrime(int val);
126 };
127
128 /* ----------------------------------------------------------------------
129 matrix times col vector
130 ------------------------------------------------------------------------- */
131
col_times3(const double m[3][3],const double * v,double * ans)132 void MathExtraLiggghts::col_times3(const double m[3][3],const double *v, double *ans)
133 {
134 ans[0] = m[0][0]*v[0]+v[1]*m[1][0]+v[2]*m[2][0];
135 ans[1] = v[0]*m[0][1]+m[1][1]*v[1]+v[2]*m[2][1];
136 ans[2] = v[0]*m[0][2]+v[1]*m[1][2]+m[2][2]*v[2];
137 }
138
139 /* ----------------------------------------------------------------------
140 Matrix determinant
141 ------------------------------------------------------------------------- */
142
mdet(const double m[3][3],LAMMPS_NS::Error * error)143 double MathExtraLiggghts::mdet(const double m[3][3],LAMMPS_NS::Error *error)
144 {
145 UNUSED(error);
146 return ( -m[0][2]*m[1][1]*m[2][0] + m[0][1]*m[1][2]*m[2][0] + m[0][2]*m[1][0]*m[2][1] - m[0][0]*m[1][2]*m[2][1] - m[0][1]*m[1][0]*m[2][2] + m[0][0]*m[1][1]*m[2][2] );
147
148 }
149
150 /* ----------------------------------------------------------------------
151 Cubic root approx.
152 ------------------------------------------------------------------------- */
153
cbrt_5d(double d)154 inline double MathExtraLiggghts::cbrt_5d(double d)
155 {
156 const unsigned int B1 = 715094163;
157 double t = 0.0;
158 unsigned int* pt = (unsigned int*) &t;
159 unsigned int* px = (unsigned int*) &d;
160 pt[1]=px[1]/3+B1;
161 return t;
162 }
163
cbrta_halleyd(const double a,const double R)164 inline double MathExtraLiggghts::cbrta_halleyd(const double a, const double R)
165 {
166 const double a3 = a*a*a;
167 const double b= a * (a3 + R + R) / (a3 + a3 + R);
168 return b;
169 }
170
171 // cube root approximation using 1 iteration of Halley's method (double)
halley_cbrt1d(double d)172 inline double MathExtraLiggghts::halley_cbrt1d(double d)
173 {
174 double a = cbrt_5d(d);
175 return cbrta_halleyd(a, d);
176 }
177
178 /* ----------------------------------------------------------------------
179 exp approx
180 ------------------------------------------------------------------------- */
181
exp_fast(double x)182 inline double MathExtraLiggghts::exp_fast(double x)
183 {
184 x = 1.0 + x / 256.0;
185 x *= x; x *= x; x *= x; x *= x;
186 x *= x; x *= x; x *= x; x *= x;
187 return x;
188 }
189
190 /* ----------------------------------------------------------------------
191 min max stuff
192 ------------------------------------------------------------------------- */
193
min(const double a,const double b,const double c)194 double MathExtraLiggghts::min(const double a, const double b, const double c)
195 {
196 return std::min(std::min(a,b),c);
197 }
198
max(const double a,const double b,const double c)199 double MathExtraLiggghts::max(const double a, const double b, const double c)
200 {
201 return std::max(std::max(a,b),c);
202 }
203
min(const double a,const double b,const double c,const double d)204 double MathExtraLiggghts::min(const double a, const double b, const double c, const double d)
205 {
206 return std::min(std::min(a,b),std::min(c,d));
207 }
208
max(const double a,const double b,const double c,const double d)209 double MathExtraLiggghts::max(const double a, const double b, const double c, const double d)
210 {
211 return std::max(std::max(a,b),std::max(c,d));
212 }
213
214 template <typename T>
min(const T * const input,const unsigned int n,int & which)215 T MathExtraLiggghts::min(const T * const input, const unsigned int n, int &which)
216 {
217 T min = input[0];
218 which = 0;
219
220 for(unsigned int i = 1; i < n; i++)
221 {
222 if(input[i] < min)
223 {
224 which = i;
225 min = input[i];
226 }
227 }
228 return min;
229 }
230
231 template <typename T>
max(const T * const input,const unsigned int n,int & which)232 T MathExtraLiggghts::max(const T * const input, const unsigned int n, int &which)
233 {
234 T max = input[0];
235 which = 0;
236
237 for(unsigned int i = 1; i < n; i++)
238 {
239 if(input[i] > max)
240 {
241 which = i;
242 max = input[i];
243 }
244 }
245 return max;
246 }
247
248 template <typename T>
abs(const T a)249 T MathExtraLiggghts::abs(const T a) { return a < static_cast<T>(0) ? -a : a; }
250
251 /*----------------------------------------------------------------------
252 inverts a special 4x4 matrix that looks like this
253
254 m11 m12 m13 m14
255 m21 m22 m23 m24
256 m31 m32 m33 m34
257 1 1 1 1
258 ------------------------------------------------------------------------- */
matrix_invert_4x4_special(double matrix[4][4])259 inline void MathExtraLiggghts::matrix_invert_4x4_special(double matrix[4][4])
260 {
261 double fac,invfac,v1x,v2x,v3x,v4x,v1y,v2y,v3y,v4y,v1z,v2z,v3z,v4z;
262 v1x = matrix[0][0]; v1y = matrix[1][0]; v1z = matrix[2][0];
263 v2x = matrix[0][1]; v2y = matrix[1][1]; v2z = matrix[2][1];
264 v3x = matrix[0][2]; v3y = matrix[1][2]; v3z = matrix[2][2];
265 v4x = matrix[0][3]; v4y = matrix[1][3]; v4z = matrix[2][3];
266
267 fac = -v1x*v2z*v3y+v1x*v2y*v3z+v2z*v3y*v4x-v2y*v3z*v4x+v1x*v2z*v4y-
268 v2z*v3x*v4y-v1x*v3z*v4y+v2x*v3z*v4y+v1z*
269 (v2x*v3y-v3y*v4x+v2y*(-v3x+v4x)-v2x*v4y+v3x*v4y)-
270 v1x*v2y*v4z+v2y*v3x*v4z+v1x*v3y*v4z-v2x*v3y*v4z+v1y*
271 (v2z*v3x-v2x*v3z-v2z*v4x+v3z*v4x+v2x*v4z-v3x*v4z);
272
273 invfac = 1./fac;
274
275 matrix[0][0] = (-v3z*v4y+v2z*(-v3y+v4y)+v2y*(v3z-v4z)+v3y*v4z)*invfac;
276 matrix[1][0] = (v1z*(v3y-v4y)+v3z*v4y-v3y*v4z+v1y*(-v3z+v4z))*invfac;
277 matrix[2][0] = (-v2z*v4y+v1z*(-v2y+v4y)+v1y*(v2z-v4z)+v2y*v4z)*invfac;
278 matrix[3][0] = (v1z*(v2y-v3y)+v2z*v3y-v2y*v3z+v1y*(-v2z+v3z))*invfac;
279
280 matrix[0][1] = (v2z*(v3x-v4x)+v3z*v4x-v3x*v4z+v2x*(-v3z+v4z))*invfac;
281 matrix[1][1] = (-v3z*v4x+v1z*(-v3x+v4x)+v1x*(v3z-v4z)+v3x*v4z)*invfac;
282 matrix[2][1] = (v1z*(v2x-v4x)+v2z*v4x-v2x*v4z+v1x*(-v2z+v4z))*invfac;
283 matrix[3][1] = (-v2z*v3x+v1z*(-v2x+v3x)+v1x*(v2z-v3z)+v2x*v3z)*invfac;
284
285 matrix[0][2] = (-v3y*v4x+v2y*(-v3x+v4x)+v2x*(v3y-v4y)+v3x*v4y)*invfac;
286 matrix[1][2] = (v1y*(v3x-v4x)+v3y*v4x-v3x*v4y+v1x*(-v3y+v4y))*invfac;
287 matrix[2][2] = (-v2y*v4x+v1y*(-v2x+v4x)+v1x*(v2y-v4y)+v2x*v4y)*invfac;
288 matrix[3][2] = (v1y*(v2x-v3x)+v2y*v3x-v2x*v3y+v1x*(-v2y+v3y))*invfac;
289
290 matrix[0][2] = (v2z*v3y*v4x-v2y*v3z*v4x-v2z*v3x*v4y+v2x*v3z*v4y+v2y*v3x*v4z-v2x*v3y*v4z)*invfac;
291 matrix[1][2] = (-v1z*v3y*v4x+v1y*v3z*v4x+v1z*v3x*v4y-v1x*v3z*v4y-v1y*v3x*v4z+v1x*v3y*v4z)*invfac;
292 matrix[2][2] = (v1z*v2y*v4x-v1y*v2z*v4x-v1z*v2x*v4y+v1x*v2z*v4y+v1y*v2x*v4z-v1x*v2y*v4z)*invfac;
293 matrix[3][2] = (-v1z*v2y*v3x+v1y*v2z*v3x+v1z*v2x*v3y-v1x*v2z*v3y-v1y*v2x*v3z+v1x*v2y*v3z)*invfac;
294 }
295
296 /* ----------------------------------------------------------------------
297 transpose mat1
298 ------------------------------------------------------------------------- */
299
transpose3(const double m[3][3],double ans[3][3])300 inline void MathExtraLiggghts::transpose3(const double m[3][3], double ans[3][3])
301 {
302 ans[0][0] = m[0][0];
303 ans[0][1] = m[1][0];
304 ans[0][2] = m[2][0];
305
306 ans[1][0] = m[0][1];
307 ans[1][1] = m[1][1];
308 ans[1][2] = m[2][1];
309
310 ans[2][0] = m[0][2];
311 ans[2][1] = m[1][2];
312 ans[2][2] = m[2][2];
313 }
314
315 /*----------------------------------------------------------------------
316 checks if a point is inside a tetrader, described by an inverse matrix
317 This inverse matrix is the the inverse of a special 4x4 matrix that looks like this
318
319 m11 m12 m13 m14
320 m21 m22 m23 m24
321 m31 m32 m33 m34
322 1 1 1 1
323
324 where m11,m21,m31 is vertex 1 etc.
325 ------------------------------------------------------------------------- */
326
is_inside_tet(double * pos,double invmatrix[4][4])327 inline int MathExtraLiggghts::is_inside_tet(double *pos,double invmatrix[4][4])
328 {
329 double result[4];
330
331 result[0] = invmatrix[0][0] * pos[0] + invmatrix[0][1] * pos[1] + invmatrix[0][2] * pos[2] + invmatrix[0][3];
332 result[1] = invmatrix[1][0] * pos[0] + invmatrix[1][1] * pos[1] + invmatrix[1][2] * pos[2] + invmatrix[1][3];
333 result[2] = invmatrix[2][0] * pos[0] + invmatrix[2][1] * pos[1] + invmatrix[2][2] * pos[2] + invmatrix[2][3];
334 result[3] = invmatrix[3][0] * pos[0] + invmatrix[3][1] * pos[1] + invmatrix[3][2] * pos[2] + invmatrix[3][3];
335
336 if(max(result[0],result[1],result[2],result[3]) > 1.0) return 0;
337 return 1;
338 }
339
340 /*----------------------------------------------------------------------
341 transform from local to global coords
342 ------------------------------------------------------------------------- */
343
local_coosys_to_cartesian(double * const global,const double * const local,const double * const ex_local,const double * const ey_local,const double * const ez_local)344 void MathExtraLiggghts::local_coosys_to_cartesian(double * const global, const double * const local, const double * const ex_local, const double * const ey_local, const double * const ez_local)
345 {
346 global[0] = local[0]*ex_local[0] + local[1]*ey_local[0] + local[2]*ez_local[0];
347 global[1] = local[0]*ex_local[1] + local[1]*ey_local[1] + local[2]*ez_local[1];
348 global[2] = local[0]*ex_local[2] + local[1]*ey_local[2] + local[2]*ez_local[2];
349 }
350
351 /*----------------------------------------------------------------------
352 transform from global to local coords
353 ------------------------------------------------------------------------- */
354
cartesian_coosys_to_local(double * local,double * global,double * ex_local,double * ey_local,double * ez_local,LAMMPS_NS::Error * error)355 void MathExtraLiggghts::cartesian_coosys_to_local(double *local,double *global, double *ex_local, double *ey_local, double *ez_local,LAMMPS_NS::Error *error)
356 {
357 UNUSED(error);
358 double M[3][3] = {{0.,0.,0.},{0.,0.,0.},{0.,0.,0.}};
359 double Mt[3][3] = {{0.,0.,0.},{0.,0.,0.},{0.,0.,0.}};
360
361 // set up the matrix
362 LAMMPS_NS::vectorCopy3D(ex_local,M[0]);
363 LAMMPS_NS::vectorCopy3D(ey_local,M[1]);
364 LAMMPS_NS::vectorCopy3D(ez_local,M[2]);
365 MathExtraLiggghts::transpose3(M,Mt);
366
367 // solve
368 MathExtra::mldivide3(Mt,global,local);
369 }
370
371 /*----------------------------------------------------------------------
372 transform from global to local coords
373 faster for orthogonal matrix
374 ------------------------------------------------------------------------- */
375
cartesian_coosys_to_local_orthogonal(double * local,double * global,double * ex_local,double * ey_local,double * ez_local,LAMMPS_NS::Error * error)376 void MathExtraLiggghts::cartesian_coosys_to_local_orthogonal(double *local,double *global, double *ex_local, double *ey_local, double *ez_local,LAMMPS_NS::Error *error)
377 {
378 // check if orthogonal
379 double dot1 = LAMMPS_NS::vectorDot3D(ex_local,ey_local);
380 double dot2 = LAMMPS_NS::vectorDot3D(ey_local,ez_local);
381 double dot3 = LAMMPS_NS::vectorDot3D(ez_local,ex_local);
382 int flag = dot1 > TOLERANCE_ORTHO || dot2 > TOLERANCE_ORTHO || dot3 > TOLERANCE_ORTHO;
383 if(flag) error->one(FLERR,"Insufficient accuracy: using MathExtraLiggghts::cartesian_coosys_to_local_orthogonal() for non-orthogonal coo-sys");
384
385 // solve
386 local[0] = global[0]*ex_local[0] + global[1]*ex_local[1] + global[2]*ex_local[2];
387 local[1] = global[0]*ey_local[0] + global[1]*ey_local[1] + global[2]*ey_local[2];
388 local[2] = global[0]*ez_local[0] + global[1]*ez_local[1] + global[2]*ez_local[2];
389 }
390
391 /* ----------------------------------------------------------------------
392 conjugate of a quaternion: qc = conjugate of q
393 assume q is of unit length
394 ------------------------------------------------------------------------- */
395
qconjugate(const double * const q,double * const qc)396 void MathExtraLiggghts::qconjugate(const double * const q, double * const qc)
397 {
398 qc[0] = q[0];
399 qc[1] = -q[1];
400 qc[2] = -q[2];
401 qc[3] = -q[3];
402 }
403
404 /* ----------------------------------------------------------------------
405 construct quaternion4 from vector3
406 ------------------------------------------------------------------------- */
407
quat_from_vec(const double * v,double * q)408 void MathExtraLiggghts::quat_from_vec(const double *v, double *q)
409 {
410 q[0] = 0.;
411 q[1] = v[0];
412 q[2] = v[1];
413 q[3] = v[2];
414 }
415
416 /* ----------------------------------------------------------------------
417 construct vector3 from quaternion4
418 ------------------------------------------------------------------------- */
419
vec_from_quat(const double * q,double * const v)420 void MathExtraLiggghts::vec_from_quat(const double *q, double * const v)
421 {
422 v[0] = q[1];
423 v[1] = q[2];
424 v[2] = q[3];
425 }
426
427 /*----------------------------------------------------------------------
428 rotoate vector by quaternion
429 ------------------------------------------------------------------------- */
430
vec_quat_rotate(const double * const vec,const double * const quat,double * const result)431 void MathExtraLiggghts::vec_quat_rotate(const double * const vec, const double * const quat, double * const result)
432 {
433 double vecQ[4], resultQ[4], quatC[4], temp[4];
434
435 // construct quaternion (0,vec)
436 quat_from_vec(vec,vecQ);
437
438 // conjugate initial quaternion
439 qconjugate(quat,quatC);
440
441 // rotate by quaternion multiplications
442 MathExtra::quatquat(quat,vecQ,temp);
443 MathExtra::quatquat(temp,quatC,resultQ);
444
445 // return result
446 vec_from_quat(resultQ,result);
447 }
448
449 /*----------------------------------------------------------------------
450 rotoate vector by quaternion
451 ------------------------------------------------------------------------- */
452
vec_quat_rotate(double * const vec,const double * const quat)453 void MathExtraLiggghts::vec_quat_rotate(double * const vec, const double * const quat)
454 {
455 double vecQ[4], resultQ[4], quatC[4], temp[4], result[3];
456
457 // construct quaternion (0,vec)
458 quat_from_vec(vec,vecQ);
459
460 // conjugate initial quaternion
461 qconjugate(quat,quatC);
462
463 // rotate by quaternion multiplications
464 MathExtra::quatquat(quat,vecQ,temp);
465 MathExtra::quatquat(temp,quatC,resultQ);
466
467 // return result
468 vec_from_quat(resultQ,result);
469 LAMMPS_NS::vectorCopy3D(result,vec);
470 }
471
472 /* ----------------------------------------------------------------------
473 compute angular momentum from omega, both in space frame
474 only know Idiag so need to do M = Iw in body frame
475 ex,ey,ez are column vectors of rotation matrix P
476 wbody = P_transpose wspace
477 Mbody = Idiag wbody
478 Mspace = P Mbody
479 ------------------------------------------------------------------------- */
480
angmom_from_omega(double * w,double * ex,double * ey,double * ez,double * idiag,double * m)481 inline void MathExtraLiggghts::angmom_from_omega(double *w,
482 double *ex, double *ey, double *ez,
483 double *idiag, double *m)
484 {
485 double mbody[3];
486
487 mbody[0] = (w[0]*ex[0] + w[1]*ex[1] + w[2]*ex[2]) * idiag[0];
488 mbody[1] = (w[0]*ey[0] + w[1]*ey[1] + w[2]*ey[2]) * idiag[1];
489 mbody[2] = (w[0]*ez[0] + w[1]*ez[1] + w[2]*ez[2]) * idiag[2];
490
491 m[0] = mbody[0]*ex[0] + mbody[1]*ey[0] + mbody[2]*ez[0];
492 m[1] = mbody[0]*ex[1] + mbody[1]*ey[1] + mbody[2]*ez[1];
493 m[2] = mbody[0]*ex[2] + mbody[1]*ey[2] + mbody[2]*ez[2];
494 }
495
496 /* ----------------------------------------------------------------------
497 Check if is unit quaternion
498 ------------------------------------------------------------------------- */
499
is_unit_quat(const double * q)500 inline bool MathExtraLiggghts::is_unit_quat(const double *q)
501 {
502 return MathExtraLiggghts::compDouble(LAMMPS_NS::vectorMag4DSquared(q),1.0,1e-6);
503 }
504
505 /* ----------------------------------------------------------------------
506 normalize a quaternion
507 ------------------------------------------------------------------------- */
508
quat_normalize(double * q)509 inline void MathExtraLiggghts::quat_normalize(double *q)
510 {
511 double norm = 1.0 / ::sqrt(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]);
512 q[0] *= norm;
513 q[1] *= norm;
514 q[2] *= norm;
515 q[3] *= norm;
516 }
517
518 /* ----------------------------------------------------------------------
519 calculate the quaternion that would rotate q_old into q_new
520 ------------------------------------------------------------------------- */
521
quat_diff(double * q_new,double * q_old,double * q_diff)522 inline void MathExtraLiggghts::quat_diff(double *q_new, double *q_old, double *q_diff)
523 {
524 double q_old_c[4];
525
526 // q_diff = q_old^-1 * q_new
527 qconjugate(q_old,q_old_c);
528 MathExtra::quatquat(q_old_c,q_new,q_diff);
529 }
530
531 /* -----------------------------------------------------------------------------
532 * compare two doubles by using their integer representation
533 * source: http://www.cygnus-software.com/papers/comparingfloats/comparingfloats.htm
534 -------------------------------------------------------------------------------*/
535
compDouble(double const a,double const b,double const prec)536 bool MathExtraLiggghts::compDouble(double const a, double const b, double const prec)
537 {
538 if (a == b)
539 return true;
540
541 if (b == 0)
542 return a < prec && a > -prec;
543
544 double x = (a-b);//b;
545
546 return x < prec && x > -prec;
547 }
548
549 /* -----------------------------------------------------------------------------
550 * calculate barycentric coordinates of a given point (in the triangle plane)
551 * should work for any point, at least analytics claim this ...
552 * ap is a vector from node[0] to the point
553 * edgeVec are assumed to be unit vectors
554 * source: http://www.blackpawn.com/texts/pointinpoly/default.html
555 * hints on _which_ barycentric coordinates are computed by this method
556 * can be found on wikipedia - u_{link} = bary[1] and v_{link} = bary[2]
557 -------------------------------------------------------------------------------*/
558
calcBaryTriCoords(double * ap,double ** edgeVec,double * edgeLen,double * bary)559 void MathExtraLiggghts::calcBaryTriCoords(double *ap, double **edgeVec, double *edgeLen, double *bary)
560 {
561
562 double a = LAMMPS_NS::vectorDot3D(ap,edgeVec[0]);
563 double b = LAMMPS_NS::vectorDot3D(ap,edgeVec[2]);
564 double c = LAMMPS_NS::vectorDot3D(edgeVec[0],edgeVec[2]);
565 double oneMinCSqr = 1 - c*c;
566
567 bary[1] = (a - b*c)/(edgeLen[0] * oneMinCSqr);
568 bary[2] = (a*c - b)/(edgeLen[2] * oneMinCSqr);
569 bary[0] = 1. - bary[1] - bary[2];
570 }
571
calcBaryTriCoords(double * ap,double * edgeVec0,double * edgeVec1,double * edgeVec2,double * edgeLen,double * bary)572 void MathExtraLiggghts::calcBaryTriCoords(double *ap, double *edgeVec0, double *edgeVec1, double *edgeVec2,
573 double *edgeLen, double *bary)
574 {
575 UNUSED(edgeVec1);
576
577 double a = LAMMPS_NS::vectorDot3D(ap,edgeVec0);
578 double b = LAMMPS_NS::vectorDot3D(ap,edgeVec2);
579 double c = LAMMPS_NS::vectorDot3D(edgeVec0,edgeVec2);
580 double oneMinCSqr = 1 - c*c;
581
582 bary[1] = (a - b*c)/(edgeLen[0] * oneMinCSqr);
583 bary[2] = (a*c - b)/(edgeLen[2] * oneMinCSqr);
584 bary[0] = 1. - bary[1] - bary[2];
585 }
586
587 /* ----------------------------------------------------------------------
588 generate random unit quaternion
589 from http://planning.cs.uiuc.edu/node198.html
590 ------------------------------------------------------------------------- */
591
random_unit_quat(LAMMPS_NS::RanPark * random,double * quat)592 void MathExtraLiggghts::random_unit_quat(LAMMPS_NS::RanPark *random,double *quat)
593 {
594 double u1 = random->uniform();
595 double u2 = random->uniform();
596 double u3 = random->uniform();
597
598 double h1 = ::sqrt(1.-u1);
599 double h2 = ::sqrt(u1);
600
601 quat[0] = h1 * ::sin(2.*M_PI*u2);
602 quat[1] = h1 * ::cos(2.*M_PI*u2);
603 quat[2] = h2 * ::sin(2.*M_PI*u3);
604 quat[3] = h2 * ::cos(2.*M_PI*u3);
605 }
606
607 /* ----------------------------------------------------------------------
608 check if char * string is int
609 ------------------------------------------------------------------------- */
610
is_int(char * str)611 bool MathExtraLiggghts::is_int(char *str)
612 {
613 size_t n = strlen(str);
614 for (size_t i = 0; i < n; ++i)
615 if (isdigit(str[i]) == 0)
616 return false;
617
618 return true;
619 }
620
621 /* ----------------------------------------------------------------------
622 generate complement basis
623 ------------------------------------------------------------------------- */
generateComplementBasis(double * uVec,double * vVec,double * direction)624 void MathExtraLiggghts::generateComplementBasis(double *uVec, double *vVec, double *direction)
625 {
626
627 double invLength;
628
629 if ( abs(direction[0]) >= abs(direction[1]) )
630 {
631 // direction.x or direction.z is the largest magnitude component, swap them
632 invLength = 1.0/::sqrt ( direction[0]*direction[0]
633 +direction[2]*direction[2]
634 );
635 uVec[0] = -direction[2]*invLength;
636 uVec[1] = 0.0;
637 uVec[2] = direction[0]*invLength;
638
639 vVec[0] = direction[1]*uVec[2];
640 vVec[1] = direction[2]*uVec[0]
641 - direction[0]*uVec[2];
642 vVec[2] = -direction[1]*uVec[0];
643 }
644 else
645 {
646 // direction.y or direction.z is the largest magnitude component, swap them
647 invLength = 1.0/::sqrt ( direction[1]*direction[1]
648 +direction[2]*direction[2]
649 );
650
651 uVec[0] = 0.0;
652 uVec[1] = direction[2]*invLength;
653 uVec[2] = -direction[1]*invLength;
654
655 vVec[0] = direction[1]*uVec[2]
656 - direction[2]*uVec[1];
657 vVec[1] = -direction[0]*uVec[2];
658 vVec[2] = direction[0]*uVec[1];
659 }
660 }
661
662 /* ----------------------------------------------------------------------
663 check if integer is a prime number (primes are 6k+-1)
664 ------------------------------------------------------------------------- */
665
isPrime(int val)666 bool MathExtraLiggghts::isPrime(int val)
667 {
668 if (val < 2)
669 return false;
670 else if (val == 2)
671 return true;
672 else if (val == 3)
673 return true;
674 else if (val % 2 == 0)
675 return false;
676 else if (val % 3 == 0)
677 return false;
678
679 // max range is up to square-root
680 int testTo = static_cast<int>(floor(::sqrt(static_cast<double>(val))));
681 int test = 5;
682 int width = 2;
683
684 while ( test <= testTo )
685 {
686 if (val % test == 0)
687 return false;
688
689 test += width;
690 width = 6 - width;
691 }
692
693 return true;
694 }
695
696 #endif
697