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 Arno Mayrhofer (DCS Computing GmbH, Linz)
37
38 Copyright 2015- DCS Computing GmbH, Linz
39 ------------------------------------------------------------------------- */
40
41 #include "math_extra_liggghts_superquadric.h"
42 #include "math_extra_liggghts_nonspherical.h"
43
44 #ifdef SUPERQUADRIC_ACTIVE_FLAG
45
46 #include <cmath>
47 #include <algorithm>
48 #if __STDCPP_MATH_SPEC_FUNCS__ >= 201003L
49 #define BETA_NAMESPACE std
50 #elif defined(HAVE_TR1_CMATH)
51 #include <tr1/cmath>
52 #define BETA_NAMESPACE std::tr1
53 #elif defined(BOOST_INCLUDED)
54 #include "boost/math/special_functions/beta.hpp"
55 #define BETA_NAMESPACE boost::math
56 #else
57
58 #include "boost/math/special_functions/beta.hpp"
59 #define BETA_NAMESPACE boost::math
60
61 #endif
62
63 #include <map>
64 #include "math_const.h"
65
66 #define PHI_INV 0.61803398874989479
67
68 namespace MathExtraLiggghtsNonspherical {
69
70 double J4[16];
71
check_inequalities(const int i,const double * delta,const double * a,const double * b,const double * A_delta,const double * B_delta,const double * C)72 double check_inequalities(const int i, const double *delta, const double *a, const double *b,
73 const double *A_delta, const double *B_delta, const double *C) {
74 double R = 0.0, R0 = 0.0, R1 = 0.0;
75 switch (i) {
76 case 0: //check A0
77 R0 = a[0];
78 R1 = b[0]*fabs(C[0]) + b[1]*fabs(C[1])+b[2]*fabs(C[2]);
79 R = fabs(A_delta[0]);
80 break;
81 case 1: //check A1
82 R0 = a[1];
83 R1 = b[0]*fabs(C[3]) + b[1]*fabs(C[4])+b[2]*fabs(C[5]);
84 R = fabs(A_delta[1]);
85 break;
86 case 2: //check A2
87 R0 = a[2];
88 R1 = b[0]*fabs(C[6]) + b[1]*fabs(C[7])+b[2]*fabs(C[8]);
89 R = fabs(A_delta[2]);
90 break;
91 case 3: //check B0
92 R0 = a[0]*fabs(C[0]) + a[1]*fabs(C[3]) + a[2]*fabs(C[6]);
93 R1 = b[0];
94 R = fabs(B_delta[0]);
95 break;
96 case 4: //check B1
97 R0 = a[0]*fabs(C[1]) + a[1]*fabs(C[4]) + a[2]*fabs(C[7]);
98 R1 = b[1];
99 R = fabs(B_delta[1]);
100 break;
101 case 5: //check B2
102 R0 = a[0]*fabs(C[2]) + a[1]*fabs(C[5]) + a[2]*fabs(C[8]);
103 R1 = b[2];
104 R = fabs(B_delta[2]);
105 break;
106 case 6: //check //A0xB0
107 R0 = a[1]*fabs(C[6]) + a[2]*fabs(C[3]);
108 R1 = b[1]*fabs(C[2]) + b[2]*fabs(C[1]);
109 R = fabs(C[3]*A_delta[2] - C[6]*A_delta[1]);
110 break;
111 case 7: //check A0xB1
112 R0 = a[1]*fabs(C[7]) + a[2]*fabs(C[4]);
113 R1 = b[0]*fabs(C[2]) + b[2]*fabs(C[0]);
114 R = fabs(C[4]*A_delta[2] - C[7]*A_delta[1]);
115 break;
116 case 8: //check A0xB2
117 R0 = a[1]*fabs(C[8]) + a[2]*fabs(C[5]);
118 R1 = b[0]*fabs(C[1]) + b[1]*fabs(C[0]);
119 R = fabs(C[5]*A_delta[2] - C[8]*A_delta[1]);
120 break;
121 case 9: //check A1xB0
122 R0 = a[0]*fabs(C[6]) + a[2]*fabs(C[0]);
123 R1 = b[1]*fabs(C[5]) + b[2]*fabs(C[4]);
124 R = fabs(C[6]*A_delta[0] - C[0]*A_delta[2]);
125 break;
126 case 10: //A1xB1
127 R0 = a[0]*fabs(C[7]) + a[2]*fabs(C[1]);
128 R1 = b[0]*fabs(C[5]) + b[2]*fabs(C[3]);
129 R = fabs(C[7]*A_delta[0] - C[1]*A_delta[2]);
130 break;
131 case 11: //A1xB2
132 R0 = a[0]*fabs(C[8]) + a[2]*fabs(C[2]);
133 R1 = b[0]*fabs(C[4]) + b[1]*fabs(C[3]);
134 R = fabs(C[8]*A_delta[0] - C[2]*A_delta[2]);
135 break;
136 case 12: //A2xB0
137 R0 = a[0]*fabs(C[3]) + a[1]*fabs(C[0]);
138 R1 = b[1]*fabs(C[8]) + b[2]*fabs(C[7]);
139 R = fabs(C[0]*A_delta[1] - C[3]*A_delta[0]);
140 break;
141 case 13: //A2xB1
142 R0 = a[0]*fabs(C[4]) + a[1]*fabs(C[1]);
143 R1 = b[0]*fabs(C[8]) + b[2]*fabs(C[6]);
144 R = fabs(C[1]*A_delta[1] - C[4]*A_delta[0]);
145 break;
146 case 14: //A2xB2
147 R0 = a[0]*fabs(C[5]) + a[1]*fabs(C[2]);
148 R1 = b[0]*fabs(C[7]) + b[1]*fabs(C[6]);
149 R = fabs(C[2]*A_delta[1] - C[5]*A_delta[0]);
150 break;
151 }
152 if(fabs(R0+R1) < 1e-14 and fabs(R) < 1e-14)
153 return false;
154 else
155 return (R > R0+R1);
156 }
157
obb_intersect(Superquadric * particleA,Superquadric * particleB)158 bool obb_intersect(Superquadric *particleA, Superquadric *particleB)
159 {
160 unsigned int inequality_to_start = 0;
161 return obb_intersect(particleA, particleB, inequality_to_start);
162 }
163
164 //checking whether bounding boxes intersect or not
165 // Source: http://www.geometrictools.com/Documentation/DynamicCollisionDetection.pdf
obb_intersect(Superquadric * particleA,Superquadric * particleB,unsigned int & inequality_to_start)166 bool obb_intersect(Superquadric *particleA, Superquadric *particleB, unsigned int &inequality_to_start)
167 {
168 double delta[3];
169 LAMMPS_NS::vectorSubtract3D(particleA->center, particleB->center, delta);
170 double shape_A[3], shape_B[3];
171
172 for(int k = 0; k < 3; k++) {
173 shape_A[k] = particleA->shape[k];
174 shape_B[k] = particleB->shape[k];
175 particleA->shape[k] *= scale_koef;
176 particleB->shape[k] *= scale_koef;
177 }
178
179 double C[9];
180 MathExtraLiggghtsNonspherical::transpose_times3(particleA->rotation_matrix, particleB->rotation_matrix, C); //C = A^T * B
181 //check max. 15 inequalities
182 double A_delta[3], B_delta[3];
183 MathExtraLiggghtsNonspherical::transpose_matvec(particleA->rotation_matrix, delta, A_delta);
184 MathExtraLiggghtsNonspherical::transpose_matvec(particleB->rotation_matrix, delta, B_delta);
185 bool obb_intersect = true;
186 for(unsigned int i = 0; i < 15; i++) {
187 int inequality_to_check;
188 if(i == 0)
189 inequality_to_check = inequality_to_start;
190 else if(i == inequality_to_start)
191 inequality_to_check = 0;
192 else
193 inequality_to_check = i;
194 if(check_inequalities(inequality_to_check, delta, particleA->shape, particleB->shape, A_delta, B_delta, C)) {
195 inequality_to_start = inequality_to_check;
196 obb_intersect = false;
197 break;
198 }
199 }
200 LAMMPS_NS::vectorCopy3D(shape_A, particleA->shape);
201 LAMMPS_NS::vectorCopy3D(shape_B, particleB->shape);
202 return obb_intersect;
203 }
204
205 //minimal bounding sphere radius calculation
bounding_sphere_radius_superquadric(const double * shape,const double * blockiness,double * radius)206 void bounding_sphere_radius_superquadric(const double *shape, const double *blockiness, double *radius)
207 {
208 if(MathExtraLiggghts::compDouble(blockiness[0],2.0) and MathExtraLiggghts::compDouble(blockiness[1],2.0))
209 *radius = std::max(std::max(shape[0], shape[1]), shape[2]);
210 else {
211 const double a = std::max(shape[0], shape[1]);
212 const double b = std::min(shape[0], shape[1]);
213 const double c = shape[2];
214 if(MathExtraLiggghts::compDouble(blockiness[1],2.0))
215 *radius = sqrt(a*a+c*c);
216 else {
217 const double n1 = std::max(2.1, blockiness[0]);
218 const double n2 = std::max(2.1, blockiness[1]);
219 double alpha;
220 if(!MathExtraLiggghts::compDouble(blockiness[1],2.0))
221 alpha = MathExtraLiggghtsNonspherical::pow_abs(b/a, 2.0 / (n2 - 2.0));
222 else
223 alpha = 0.0;
224 const double gamma = pow_abs(1.0 + pow_abs(alpha, n2), n1/n2 - 1.0);
225 const double beta = pow_abs(c * c / (a * a) * gamma, 1.0 / (n1 - 2.0));
226 const double x0 = 1.0 / pow_abs(pow_abs(1.0 + pow_abs(alpha, n2), n1/n2) + pow_abs(beta, n1), 1.0 / n1);
227 const double xtmp = x0 * a;
228 const double ytmp = alpha * x0 * b;
229 const double ztmp = beta * x0 * c;
230 *radius = sqrt(xtmp*xtmp + ytmp*ytmp + ztmp*ztmp);
231 }
232 }
233 *radius *= scale_koef;
234 //*radius = LAMMPS_NS::vectorMag3D(shape);
235 /*int index;
236
237 double u_[] = {1.0, 0.0, 0.0};
238 for(int i = 0; i <= 90; i = i + 1) {
239 double angle = static_cast<double>(i)/180*M_PI;
240 double quat_[] = {cos(0.5*angle), 0.0, sin(0.5*angle), 0.0 };
241 double shape_[] = {0.354561735, 0.354561735, 0.709123471};
242 double aa = shape_[0];
243 double bb = shape_[1];
244 double cc = shape_[2];
245 double blockiness_[] = {6.0, 2.0};
246 double p = 1.61;
247 double s_ellipsoid = 4.0*M_PI*pow((pow(aa*bb,p) + pow(bb*cc,p) + pow(cc*aa,p))/3.0, 1.0/p);
248 double s_cylinder = 2.0 * M_PI*aa*bb + 4.0*M_PI*aa*cc;
249 double s_parallelepiped = 8.0*(aa*bb + bb*cc + cc*aa);
250
251 double s_particle;
252 area_superquadric(shape_, blockiness_, &s_particle);
253 double vol;
254 volume_superquadric(shape_, blockiness_, &vol);
255 double r = cbrt(0.75/M_PI*vol);
256 double s_particle_ortho = crossSectionalArea(u_, shape_, blockiness_, quat_, index);
257 double s_sphere = 4.0*M_PI*r*r;
258 double s_sphere_ortho = M_PI*r*r;
259 double psi = s_sphere / s_particle;
260 double psi_ortho = s_sphere_ortho / s_particle_ortho;
261 //printf("%3.16e\n", s_particle_ortho);
262 printf("%3.16e\n", psi_ortho);
263 }
264 printf("!");*/
265 }
266
267 // tensor of inertia in particle based coordinate system (diagonal)
268 //source: http://lrv.fri.uni-lj.si/~franc/SRSbook/geometry.pdf
inertia_superquadric(const double * shape,const double * blockiness,const double dens,double * ans)269 void inertia_superquadric(const double *shape, const double *blockiness, const double dens, double *ans)
270 {
271 const double eps1 = 2.0 / blockiness[0];
272 const double eps2 = 2.0 / blockiness[1];
273
274 const double a1 = shape[0];
275 const double a2 = shape[1];
276 const double a3 = shape[2];
277 const double I_xx = 0.5*a1*a2*a3*eps1*eps2*(a2*a2*BETA_NAMESPACE::beta(1.5*eps2, 0.5*eps2)*BETA_NAMESPACE::beta(0.5*eps1, 2.0*eps1+1.0)+
278 4.0*a3*a3*BETA_NAMESPACE::beta(0.5*eps2, 0.5*eps2+1.0)*BETA_NAMESPACE::beta(1.5*eps1, eps1+1.0)) * dens;
279 const double I_yy = 0.5*a1*a2*a3*eps1*eps2*(a1*a1*BETA_NAMESPACE::beta(1.5*eps2, 0.5*eps2)*BETA_NAMESPACE::beta(0.5*eps1, 2.0*eps1+1.0)+
280 4.0*a3*a3*BETA_NAMESPACE::beta(0.5*eps2, 0.5*eps2+1.0)*BETA_NAMESPACE::beta(1.5*eps1, eps1+1.0)) * dens;
281 const double I_zz = 0.5*a1*a2*a3*eps1*eps2*(a1*a1 + a2*a2)*
282 BETA_NAMESPACE::beta(1.5*eps2, 0.5*eps2)*BETA_NAMESPACE::beta(0.5*eps1, 2.0*eps1+1.0) * dens;
283 ans[0] = I_xx;
284 ans[1] = I_yy;
285 ans[2] = I_zz;
286 }
287
288 //volume of superquadric particle
289 //source: http://lrv.fri.uni-lj.si/~franc/SRSbook/geometry.pdf
volume_superquadric(const double * shape,const double * blockiness,double * result)290 void volume_superquadric(const double *shape, const double *blockiness, double *result)
291 {
292 const double eps1 = 2.0 / blockiness[0];
293 const double eps2 = 2.0 / blockiness[1];
294 *result =2.0*shape[0]*shape[1]*shape[2]*eps1*eps2*
295 BETA_NAMESPACE::beta(0.5*eps1, eps1 + 1.0)*
296 BETA_NAMESPACE::beta(0.5*eps2, 0.5*eps2 + 1.0);
297 }
298
area_superquadric(const double * shape,const double * blockiness,double * result)299 void area_superquadric(const double *shape, const double *blockiness, double *result)
300 {
301 double n1 = blockiness[0];
302 double n2 = blockiness[1];
303 double aa = shape[0];
304 double bb = shape[1];
305 double cc = shape[2];
306 *result = 0.0;
307
308 const int N = 10;
309 double sin_angle[N + 1];
310 double cos_angle[N + 1];
311 double dangle = 0.5*M_PI / static_cast<double>(N); //angle step
312
313 for(int in = 0; in <= N; in++) {
314 double angle;
315 if(in < N)
316 angle = dangle*static_cast<double>(in);
317 else
318 angle = dangle*(static_cast<double>(in) - 0.01);
319 sin_angle[in] = sin(angle); //precompute sinus
320 cos_angle[in] = cos(angle); //precompute cosinus
321 }
322
323 for(int itheta = 0; itheta < N; itheta++) {
324 double sin_theta1 = sin_angle[itheta];
325 double cos_theta1 = cos_angle[itheta];
326 double sin_theta2 = sin_angle[itheta+1];
327 double cos_theta2 = cos_angle[itheta+1];
328 double z1 = fabs(sin_theta1);
329 double z2 = fabs(sin_theta2);
330 for(int iphi = 0; iphi < N; iphi++) {
331 double sin_phi1 = sin_angle[iphi];
332 double cos_phi1 = cos_angle[iphi];
333 double p11[3];
334 p11[0] = fabs(cos_phi1)*fabs(cos_theta1);
335 p11[1] = fabs(sin_phi1)*fabs(cos_theta1);
336 p11[2] = z1;
337
338 double p12[3];
339 p12[0] = fabs(cos_phi1)*fabs(cos_theta2);
340 p12[1] = fabs(sin_phi1)*fabs(cos_theta2);
341 p12[2] = z2;
342
343 double sin_phi2 = sin_angle[iphi+1];
344 double cos_phi2 = cos_angle[iphi+1];
345 double p21[3];
346 p21[0] = fabs(cos_phi2)*fabs(cos_theta1);
347 p21[1] = fabs(sin_phi2)*fabs(cos_theta1);
348 p21[2] = z1;
349
350 double p22[3];
351 p22[0] = fabs(cos_phi2)*fabs(cos_theta2);
352 p22[1] = fabs(sin_phi2)*fabs(cos_theta2);
353 p22[2] = z2;
354
355 double center_[] = {0,0,0};
356 double quat_[] = {1,0,0,0};
357 double shape_[] = {aa, bb, cc};
358 double blockiness_[] = {n1, n2};
359 Superquadric particle(center_, quat_, shape_, blockiness_);
360 double f, alpha;
361
362 f = particle.shape_function_local(p11);
363 alpha = 1.0 / MathExtraLiggghtsNonspherical::pow_abs(f/particle.koef + 1.0, 1.0/blockiness[0]);
364 for(int i = 0; i < 3; i++)
365 p11[i] *= alpha;
366
367 f = particle.shape_function_local(p12);
368 alpha = 1.0 / MathExtraLiggghtsNonspherical::pow_abs(f/particle.koef + 1.0, 1.0/blockiness[0]);
369 for(int i = 0; i < 3; i++)
370 p12[i] *= alpha;
371
372 f = particle.shape_function_local(p21);
373 alpha = 1.0 / MathExtraLiggghtsNonspherical::pow_abs(f/particle.koef + 1.0, 1.0/blockiness[0]);
374 for(int i = 0; i < 3; i++)
375 p21[i] *= alpha;
376
377 f = particle.shape_function_local(p22);
378 alpha = 1.0 / MathExtraLiggghtsNonspherical::pow_abs(f/particle.koef + 1.0, 1.0/blockiness[0]);
379 for(int i = 0; i < 3; i++)
380 p22[i] *= alpha;
381
382 double e1[3], e2[3], e3[4];
383 LAMMPS_NS::vectorSubtract3D(p22, p11, e1);
384 LAMMPS_NS::vectorSubtract3D(p12, p11, e2);
385 LAMMPS_NS::vectorSubtract3D(p21, p11, e3);
386
387 double s1 = LAMMPS_NS::vectorCrossMag3D(e1, e2);
388 double s2 = LAMMPS_NS::vectorCrossMag3D(e1, e3);
389
390 *result += (s1 + s2)*4.0;
391 }
392 }
393 /*double result_check1 = 4.0*M_PI*aa*bb; //sphere surface area
394 double result_check2 = 8*(aa*bb + bb*cc + cc*aa); //box surface area
395 double ecc = sqrt(1.0 - std::min(aa*aa,bb*bb)/std::max(aa*aa,bb*bb));
396 double len = 4.0*std::max(aa,bb)*boost::math::ellint_2(ecc);
397 double result_check3 = 2.0*M_PI*aa*bb + len*2.0*cc; //cylinder surface area
398 printf("%e %e \n", *result, result_check1);*/
399 }
400
crossSectionalArea(const double * u_,const double * shape,const double * blockiness,const double * quat,int & index)401 double crossSectionalArea(const double* u_, const double* shape, const double *blockiness, const double* quat, int& index)
402 {
403 double eps1 = 2.0 / blockiness[0];
404 double eps2 = 2.0 / blockiness[1];
405 double rot[9];
406 MathExtraLiggghtsNonspherical::quat_to_mat(quat, rot);
407 double u[3];
408 MathExtraLiggghtsNonspherical::transpose_matvec(rot, u_, u);
409 LAMMPS_NS::vectorNormalize3D(u);
410
411 if(fabs(u[2]) < 1e-8) {
412 u[2] = 0.0;
413 LAMMPS_NS::vectorNormalize3D(u);
414 }
415
416 double a = shape[0];
417 double b = shape[1];
418 double c = shape[2];
419
420 double ux = u[0];
421 double uy = u[1];
422 double uz = u[2];
423
424 double result = 0.0;
425 int N = 2000;
426
427 if(fabs(uz) > 1e-12) {
428 double dphi = M_PI / static_cast<double>(N);
429 for(int iphi = 0; iphi < N; iphi ++) {
430 double phi = static_cast<double>(iphi)*dphi;
431 double phi_next = static_cast<double>(iphi+1)*dphi;
432
433 double cos_phi = cos(phi);
434 double sin_phi = sin(phi);
435
436 double cos_phi_next = cos(phi_next);
437 double sin_phi_next = sin(phi_next);
438
439 double tg_theta = -(ux * pow_abs(cos_phi, 2.0 - eps2) * sign(cos_phi) * (c/a) +
440 uy * pow_abs(sin_phi, 2.0 - eps2) * sign(sin_phi) * (c/b)) / uz;
441 double tg_theta_next = -(ux * pow_abs(cos_phi_next, 2.0 - eps2) * sign(cos_phi_next) * (c/a) +
442 uy * pow_abs(sin_phi_next, 2.0 - eps2) * sign(sin_phi_next) * (c/b)) / uz;
443
444 tg_theta = pow_abs(tg_theta, 1.0 / (2.0 - eps1))*sign(tg_theta);
445 tg_theta_next = pow_abs(tg_theta_next, 1.0 / (2.0 - eps1))*sign(tg_theta_next);
446
447 double cos_theta2 = 1.0 / (1.0 + tg_theta*tg_theta);
448 double sin_theta2 = 1.0 - cos_theta2;
449 double cos_theta = sqrt(cos_theta2);
450 double sin_theta = sqrt(sin_theta2) * sign(tg_theta);
451
452 double cos_theta_next2 = 1.0 / (1.0 + tg_theta_next*tg_theta_next);
453 double sin_theta_next2 = 1.0 - cos_theta_next2;
454 double cos_theta_next = sqrt(cos_theta_next2);
455 double sin_theta_next = sqrt(sin_theta_next2) * sign(tg_theta_next);
456
457 double p[] = { a*pow_abs(cos_theta, eps1) * pow_abs(cos_phi, eps2) * sign(cos_theta) * sign(cos_phi),
458 b*pow_abs(cos_theta, eps1) * pow_abs(sin_phi, eps2) * sign(cos_theta) * sign(sin_phi),
459 c*pow_abs(sin_theta, eps1) * sign(sin_theta)};
460 double p_next[] = { a*pow_abs(cos_theta_next, eps1) * pow_abs(cos_phi_next, eps2) * sign(cos_theta_next) * sign(cos_phi_next),
461 b*pow_abs(cos_theta_next, eps1) * pow_abs(sin_phi_next, eps2) * sign(cos_theta_next) * sign(sin_phi_next),
462 c*pow_abs(sin_theta_next, eps1) * sign(sin_theta_next)};
463
464 double s[3];
465 LAMMPS_NS::vectorCross3D(p, p_next,s);
466 double s_ = fabs(LAMMPS_NS::vectorDot3D(s,u));
467 result += s_;
468 }
469 index = 0;
470 } else {
471 double cos_phi, sin_phi;
472 if(fabs(uy) > fabs(ux)) {
473 double tg_phi = -ux / uy * b / a;
474 tg_phi = pow_abs(tg_phi, 2.0 - eps2) * sign(tg_phi);
475 double cos_phi2 = 1.0 / (1.0 + tg_phi*tg_phi);
476 cos_phi = sqrt(cos_phi2);
477 sin_phi = sqrt(1.0 - cos_phi2)*sign(tg_phi);
478 index = 1;
479 } else {
480 double ctg_phi = -uy / ux * a / b;
481 ctg_phi = pow_abs(ctg_phi, 2.0 - eps2) * sign(ctg_phi);
482 double sin_phi2 = 1.0 / (1.0 + ctg_phi*ctg_phi);
483 sin_phi = sqrt(sin_phi2);
484 cos_phi = sqrt(1.0 - sin_phi2)*sign(ctg_phi);
485 index = 2;
486 }
487 double dtheta = M_PI / static_cast<double>(N);
488 for(int itheta = 0; itheta < N; itheta++) {
489 double theta = dtheta*static_cast<double>(itheta);
490 double theta_next = dtheta*static_cast<double>(itheta+1);
491 double cos_theta = cos(theta);
492 double sin_theta = sin(theta);
493
494 double cos_theta_next = cos(theta_next);
495 double sin_theta_next = sin(theta_next);
496
497 double p[] = { a * pow_abs(sin_theta, eps1) * pow_abs(cos_phi,eps2) * sign(sin_theta) * sign(cos_phi),
498 b * pow_abs(sin_theta, eps1) * pow_abs(sin_phi,eps2) * sign(sin_theta) * sign(sin_phi),
499 c * pow_abs(cos_theta, eps1) * sign(cos_theta)};
500 double p_next[] = { a * pow_abs(sin_theta_next, eps1) * pow_abs(cos_phi,eps2) * sign(sin_theta_next) * sign(cos_phi),
501 b * pow_abs(sin_theta_next, eps1) * pow_abs(sin_phi,eps2) * sign(sin_theta_next) * sign(sin_phi),
502 c * pow_abs(cos_theta_next, eps1) * sign(cos_theta_next)};
503
504 double s[3];
505 LAMMPS_NS::vectorCross3D(p, p_next,s);
506 double s_ = fabs(LAMMPS_NS::vectorDot3D(s,u));
507 result += s_;
508 }
509 }
510 return result;
511 }
512
get_point(int iphi,int nphi,int itheta,int ntheta,Superquadric & particle,std::map<std::pair<int,int>,point> & point_map,double * p)513 void get_point(int iphi, int nphi, int itheta, int ntheta, Superquadric &particle, std::map<std::pair<int,int>, point> &point_map, double *p) {
514 std::pair<int, int> pair;
515 pair = std::make_pair(iphi, itheta);
516 std::map<std::pair<int,int>, point>::iterator it = point_map.find(pair);
517 if(it == point_map.end()) {
518 particle.reference_point(iphi, nphi, itheta, ntheta, p);
519 point_map[pair] = point(p);
520 } else {
521 point cur_map_element = it->second;
522 p[0] = cur_map_element.x;
523 p[1] = cur_map_element.y;
524 p[2] = cur_map_element.z;
525 }
526 }
527
528 /* OBSOLETE CODE
529 TODO: remove?
530 void initial_estimate(SurfacesIntersectData &stdata, int *nphi_start, int *ntheta_start,
531 const double *point_i_estimate, const double *point_j_estimate, double *result_pointA, double *result_pointB)
532 {
533 #ifdef SUPERQUADRIC_ACTIVE_FLAG
534
535 std::map<std::pair<int,int>, point> map1;
536 std::map<std::pair<int,int>, point> map2;
537
538 double p1[3], p2[3];
539 double p1_[3], p2_[3];
540 int nphi = *nphi_start;
541 int ntheta = *ntheta_start;
542 double distance_first_estimate = LAMMPS_NS::pointDistance(point_i_estimate, point_j_estimate);
543
544 int iphi1, itheta1, iphi2, itheta2;
545 Superquadric particleA(stdata.pos_i, stdata.quat_i, stdata.shape_i, stdata.blockiness_i);
546 Superquadric particleB(stdata.pos_j, stdata.quat_j, stdata.shape_j, stdata.blockiness_j);
547
548 particleA.pre_initial_estimate(point_i_estimate, nphi, ntheta, &iphi1, &itheta1);
549 particleB.pre_initial_estimate(point_j_estimate, nphi, ntheta, &iphi2, &itheta2);
550
551 get_point(iphi1, nphi, itheta1, ntheta, particleA, map1, p1);
552 get_point(iphi2, nphi, itheta2, ntheta, particleB, map2, p2);
553
554 bool next_iter = true;
555 int it = 0;
556 while(next_iter) {
557 next_iter = false;
558 get_point(iphi1, nphi, itheta1, ntheta, particleA, map1, p1);
559 get_point(iphi2, nphi, itheta2, ntheta, particleB, map2, p2);
560 it ++;
561 int ir = 0;
562 int shifts[8][2] = {{ 0, 1},
563 { 0, -1},
564 { 1, 0},
565 {-1, 0},
566 { 1, 1},
567 { 1, -1},
568 {-1, 1},
569 {-1, -1}};
570 double rsq = LAMMPS_NS::pointDistance(p1, p2);
571 int iphi1_ = iphi1;
572 int itheta1_ = itheta1;
573 int iphi2_ = iphi2;
574 int itheta2_ = itheta2;
575 for(int ishift1 = 0; ishift1 < 8; ishift1++) {
576 double p1_new[3], p2_new[3];
577 int iphi1_new = (iphi1 + shifts[ishift1][0] + nphi - 1) % (nphi - 1);
578 int itheta1_new = (itheta1 + shifts[ishift1][1] + ntheta - 1) % (ntheta - 1);
579 get_point(iphi1_new, nphi, itheta1_new, ntheta, particleA, map1, p1_new);
580 for(int ishift2 = 0; ishift2 < 8; ishift2++) {
581 ir ++;
582 int iphi2_new = (iphi2 + shifts[ishift2][0] + nphi - 1) % (nphi - 1);
583 int itheta2_new = (itheta2 + shifts[ishift2][1] + ntheta - 1) % (ntheta - 1);
584 get_point(iphi2_new, nphi, itheta2_new, ntheta, particleB, map2, p2_new);
585 double rsq_ = LAMMPS_NS::pointDistance(p1_new, p2_new);
586 if(rsq_ < rsq) {
587 rsq = rsq_;
588 iphi1_ = iphi1_new;
589 itheta1_ = itheta1_new;
590 iphi2_ = iphi2_new;
591 itheta2_ = itheta2_new;
592 LAMMPS_NS::vectorCopy3D(p1_new, p1_);
593 LAMMPS_NS::vectorCopy3D(p2_new, p2_);
594 next_iter = true;
595 }
596 }
597 }
598 iphi1 = iphi1_;
599 itheta1 = itheta1_;
600 iphi2 = iphi2_;
601 itheta2 = itheta2_;
602 LAMMPS_NS::vectorCopy3D(p1_, p1);
603 LAMMPS_NS::vectorCopy3D(p2_, p2);
604 }
605
606 double rsq = LAMMPS_NS::pointDistance(p1, p2);
607 //double r = sqrt(rsq);
608 if(distance_first_estimate < rsq) {
609 LAMMPS_NS::vectorCopy3D(point_i_estimate, result_pointA);
610 LAMMPS_NS::vectorCopy3D(point_j_estimate, result_pointB);
611 } else {
612 LAMMPS_NS::vectorCopy3D(p1, result_pointA);
613 LAMMPS_NS::vectorCopy3D(p2, result_pointB);
614 }
615
616 #endif
617 }*/
618
calc_F(Superquadric * particleA,Superquadric * particleB,double & f1,double & f2,double * gradA,double * gradB,double * hess1,double * hess2,const double * point,double mu,double * F,double * merit)619 double calc_F(Superquadric *particleA, Superquadric *particleB, double &f1, double &f2, double *gradA, double *gradB, double *hess1, double *hess2, const double *point, double mu, double *F, double *merit)
620 {
621 /*particleA->shape_function_gradient_global(point, gradA);
622 particleB->shape_function_gradient_global(point, gradB);
623 f1 = particleA->shape_function_global(point);
624 f2 = particleB->shape_function_global(point);*/
625
626 particleA->shape_function_props_global(point, &f1, gradA, hess1);
627 particleB->shape_function_props_global(point, &f2, gradB, hess2);
628
629 double mu_sq = mu*mu;
630 F[0] = gradA[0] + mu_sq*gradB[0];
631 F[1] = gradA[1] + mu_sq*gradB[1];
632 F[2] = gradA[2] + mu_sq*gradB[2];
633 F[3] = f1 - f2;
634
635 double v[3];
636 MathExtra::cross3(gradA, gradB, v);
637 double sine = (fabs(LAMMPS_NS::vectorDot3D(v,v) / (LAMMPS_NS::vectorDot3D(gradA, gradA) * LAMMPS_NS::vectorDot3D(gradB, gradB))));
638 double aaa = fabs(f1 - f2)/(std::max(fabs(f1),fabs(f2))+1e-16);
639 *merit = std::max(sine, aaa*aaa);
640 return F[0]*F[0] + F[1]*F[1] + F[2]*F[2] + F[3]*F[3];
641 }
642
calc_F(Superquadric * particleA,Superquadric * particleB,double & f1,double & f2,double * gradA,double * gradB,double * hess1,double * hess2,const double * point,double * mu,double * F,double * merit)643 double calc_F(Superquadric *particleA, Superquadric *particleB, double &f1, double &f2, double *gradA, double *gradB, double *hess1, double *hess2, const double *point, double *mu, double *F, double *merit)
644 {
645 /*particleA->shape_function_gradient_global(point, gradA);
646 particleB->shape_function_gradient_global(point, gradB);
647 f1 = particleA->shape_function_global(point);
648 f2 = particleB->shape_function_global(point);*/
649
650 particleA->shape_function_props_global(point, &f1, gradA, hess1);
651 particleB->shape_function_props_global(point, &f2, gradB, hess2);
652
653 double mu_sq = -LAMMPS_NS::vectorDot3D(gradA,gradB) / LAMMPS_NS::vectorDot3D(gradB,gradB);
654 *mu = sqrt(fabs(mu_sq));
655 F[0] = gradA[0] + mu_sq*gradB[0];
656 F[1] = gradA[1] + mu_sq*gradB[1];
657 F[2] = gradA[2] + mu_sq*gradB[2];
658 F[3] = f1 - f2;
659
660 double v[3];
661 MathExtra::cross3(gradA, gradB, v);
662 double sine = (fabs(LAMMPS_NS::vectorDot3D(v,v) / (LAMMPS_NS::vectorDot3D(gradA, gradA) * LAMMPS_NS::vectorDot3D(gradB, gradB))));
663 double aaa = fabs(f1 - f2)/(std::max(fabs(f1),fabs(f2))+1e-16);
664 *merit = std::max(sine, aaa*aaa);
665 return F[0]*F[0] + F[1]*F[1] + F[2]*F[2] + F[3]*F[3];
666 }
667
calc_F_new(Superquadric * particle1,Superquadric * particle2,const double * x2,double * gradA,double * gradB,const double alpha,const double beta,double * F,double * merit)668 double calc_F_new(Superquadric *particle1, Superquadric *particle2, const double *x2, double *gradA, double *gradB, const double alpha, const double beta, double *F, double *merit)
669 {
670 double x1[3];
671 double f1, f2;
672
673 particle2->shape_function_props_global(x2, &f2, gradB, NULL);
674 x1[0] = x2[0] + beta*gradB[0];
675 x1[1] = x2[1] + beta*gradB[1];
676 x1[2] = x2[2] + beta*gradB[2];
677 particle1->shape_function_props_global(x1, &f1, gradA, NULL);
678
679 F[0] = gradA[0]+alpha*gradB[0];
680 F[1] = gradA[1]+alpha*gradB[1];
681 F[2] = gradA[2]+alpha*gradB[2];
682 F[3] = f1;
683 F[4] = f2;
684
685 double v[3];
686 MathExtra::cross3(gradA, gradB, v);
687 double sin_ = sqrt(fabs(LAMMPS_NS::vectorDot3D(v,v) / (LAMMPS_NS::vectorDot3D(gradA, gradA) * LAMMPS_NS::vectorDot3D(gradB, gradB))));
688 *merit = fabs(sin_) + fabs(f1) + fabs(f2);
689
690 return sqrt(F[0]*F[0] + F[1]*F[1] + F[2]*F[2] + F[3]*F[3] + F[4]*F[4]);
691 }
692
calc_F_new(Superquadric * particle1,Superquadric * particle2,double * x1,const double * x2,double * gradA,double * gradB,const double alpha,const double beta,double * F,double * merit)693 double calc_F_new(Superquadric *particle1, Superquadric *particle2, double *x1, const double *x2, double *gradA, double *gradB, const double alpha, const double beta, double *F, double *merit)
694 {
695 double f1, f2;
696 particle2->shape_function_props_global(x2, &f2, gradB, NULL);
697 x1[0] = x2[0] + beta*gradB[0];
698 x1[1] = x2[1] + beta*gradB[1];
699 x1[2] = x2[2] + beta*gradB[2];
700 particle1->shape_function_props_global(x1, &f1, gradA, NULL);
701
702 F[0] = gradA[0]+alpha*gradB[0];
703 F[1] = gradA[1]+alpha*gradB[1];
704 F[2] = gradA[2]+alpha*gradB[2];
705 F[3] = f1;
706 F[4] = f2;
707
708 double v[3];
709 MathExtra::cross3(gradA, gradB, v);
710 double sin_ = sqrt(fabs(LAMMPS_NS::vectorDot3D(v,v) / (LAMMPS_NS::vectorDot3D(gradA, gradA) * LAMMPS_NS::vectorDot3D(gradB, gradB))));
711 *merit = fabs(sin_) + fabs(f1) + fabs(f2);
712
713 return sqrt(F[0]*F[0] + F[1]*F[1] + F[2]*F[2] + F[3]*F[3] + F[4]*F[4]);
714 }
715
invf(int i,int j,const double * m)716 double invf(int i,int j,const double* m){
717
718 int o = 2+(j-i);
719
720 i += 4+o;
721 j += 4-o;
722
723 #define e(a,b) m[ ((j+b)%4)*4 + ((i+a)%4) ]
724
725 double inv =
726 + e(+1,-1)*e(+0,+0)*e(-1,+1)
727 + e(+1,+1)*e(+0,-1)*e(-1,+0)
728 + e(-1,-1)*e(+1,+0)*e(+0,+1)
729 - e(-1,-1)*e(+0,+0)*e(+1,+1)
730 - e(-1,+1)*e(+0,-1)*e(+1,+0)
731 - e(+1,-1)*e(-1,+0)*e(+0,+1);
732
733 return (o%2)?inv : -inv;
734
735 #undef e
736 }
737
inverseMatrix4x4(const double * m,double * out)738 double inverseMatrix4x4(const double *m, double *out)
739 {
740
741 double inv[16];
742
743 for(int i = 0;i < 4; i++)
744 for(int j = 0; j < 4; j++)
745 inv[j*4+i] = invf(i,j,m);
746
747 double D = 0;
748
749 for(int k = 0; k < 4; k++) D += m[k] * inv[k*4];
750
751 if (D == 0) return D;
752
753 double D_inv = 1.0 / D;
754
755 for (int i = 0; i < 16; i++)
756 out[i] = inv[i] * D_inv;
757
758 return D;
759 }
760
determinant_4x4(double * mat)761 double determinant_4x4(double *mat)
762 {
763 const double m00 = mat[0]; const double m01 = mat[1]; const double m02 = mat[2]; const double m03 = mat[3];
764 const double m10 = mat[4]; const double m11 = mat[5]; const double m12 = mat[6]; const double m13 = mat[7];
765 const double m20 = mat[8]; const double m21 = mat[9]; const double m22 = mat[10]; const double m23 = mat[11];
766 const double m30 = mat[12]; const double m31 = mat[13]; const double m32 = mat[14]; const double m33 = mat[15];
767
768 double value =
769 m03*m12*m21*m30 - m02*m13*m21*m30 - m03*m11*m22*m30 + m01*m13*m22*m30+
770 m02*m11*m23*m30 - m01*m12*m23*m30 - m03*m12*m20*m31 + m02*m13*m20*m31+
771 m03*m10*m22*m31 - m00*m13*m22*m31 - m02*m10*m23*m31 + m00*m12*m23*m31+
772 m03*m11*m20*m32 - m01*m13*m20*m32 - m03*m10*m21*m32 + m00*m13*m21*m32+
773 m01*m10*m23*m32 - m00*m11*m23*m32 - m02*m11*m20*m33 + m01*m12*m20*m33+
774 m02*m10*m21*m33 - m00*m12*m21*m33 - m01*m10*m22*m33 + m00*m11*m22*m33;
775 return value;
776 }
777
calc_contact_point(Superquadric * particleA,Superquadric * particleB,double ratio,const double * initial_point1,double * result_point,double & fi,double & fj,bool * fail,LAMMPS_NS::Error * error)778 void calc_contact_point(Superquadric *particleA, Superquadric *particleB,
779 double ratio, const double *initial_point1, double *result_point, double &fi, double &fj, bool *fail, LAMMPS_NS::Error *error)
780 {
781 const double tol1 = 1e-10; //tolerance
782 const double tol2 = 1e-12;
783 *fail = false;
784
785 double mu, mu_sq;
786 double F[4], F1[4], F2[4];
787 double point[3];
788
789 double mu1, mu2;
790 double gradA1[3], gradB1[3];
791
792 double fi1, fj1, fi2, fj2;
793
794 double merit01, merit02, merit0;
795
796 double res01 = calc_F(particleA, particleB, fi1, fj1, gradA1, gradB1, NULL, NULL, initial_point1, &mu1, F1, &merit01);
797 if(merit01 < tol1) {
798 LAMMPS_NS::vectorCopy3D(initial_point1, result_point);
799 LAMMPS_NS::vectorCopy3D(gradA1, particleA->gradient);
800 LAMMPS_NS::vectorCopy3D(gradB1, particleB->gradient);
801 fi = fi1;
802 fj = fj1;
803 return; //finish if initial guess is good enough
804 }
805
806 double initial_point2[3];
807 for(int k = 0; k < 3; k++)
808 initial_point2[k] = ratio*particleB->center[k] + (1.0 - ratio)*particleA->center[k]; //solution for spheres
809 double gradA2[3], gradB2[3];
810 double res02 = calc_F(particleA, particleB, fi2, fj2, gradA2, gradB2, NULL, NULL, initial_point2, &mu2, F2, &merit02);
811 double res0;
812 if(res01 < res02) {
813 res0 = res01; //solution from previous step is better than for spheres
814 LAMMPS_NS::vectorCopy3D(gradA1, particleA->gradient);
815 LAMMPS_NS::vectorCopy3D(gradB1, particleB->gradient);
816 LAMMPS_NS::vectorCopy3D(initial_point1, point);
817 LAMMPS_NS::vectorCopy4D(F1, F);
818 fi = fi1;
819 fj = fj1;
820 mu = mu1;
821 merit0 = merit01;
822 } else { //solution for spheres better than from the previous step one
823 res0 = res02;
824 LAMMPS_NS::vectorCopy3D(gradA2, particleA->gradient);
825 LAMMPS_NS::vectorCopy3D(gradB2, particleB->gradient);
826 LAMMPS_NS::vectorCopy3D(initial_point2, point);
827 LAMMPS_NS::vectorCopy4D(F2, F);
828 fi = fi2;
829 fj = fj2;
830 mu = mu2;
831 merit0 = merit02;
832 }
833
834 double point0[3];
835 LAMMPS_NS::vectorCopy3D(point, point0);
836
837 if(merit0 < tol1) {
838 LAMMPS_NS::vectorCopy3D(point, result_point);
839 return; //finish if the solution for spheres is good enough
840 }
841
842 double merit1 = merit0;
843 double merit2 = merit0;
844
845 double res1 = res0;
846 double res2 = res1;
847
848 double size_i = MathExtraLiggghts::min(particleA->shape[0],particleA->shape[1],particleA->shape[2]);
849 double size_j = MathExtraLiggghts::min(particleB->shape[0],particleB->shape[1],particleB->shape[2]);
850
851 double size = std::min(size_i, size_j);
852
853 J4[15] = 0.0;
854 double delta[4], delta_0[4];
855 zeros(delta, 4);
856 zeros(delta_0, 4);
857 const int Niter = 100000;
858 double pointb[3], pointa[3];
859 double J4_inv[16];
860
861 for(int iter = 0; iter < Niter; iter++) {
862
863 merit2 = merit1;
864 res2 = res1;
865
866 particleA->shape_function_hessian_global(point, particleA->hessian);
867 particleB->shape_function_hessian_global(point, particleB->hessian);
868
869 mu_sq = mu*mu;
870 for(int i = 0; i < 3; i++) { //construct Jacobian
871 for(int j = 0; j < 3; j++) {
872 J4[4*i + j] = particleA->hessian[i*3+j] + mu_sq*particleB->hessian[i*3+j];
873 }
874 J4[3+4*i] = 2.0*mu*particleB->gradient[i];
875 J4[i+4*3] = particleA->gradient[i] - particleB->gradient[i];
876 }
877
878 double det = determinant_4x4(J4);
879 if(fabs(det) > 1.0) {
880 inverseMatrix4x4(J4, J4_inv);
881 MathExtraLiggghtsNonspherical::matvecN(J4_inv, F, 4, delta);
882 } else {
883 vectorCopyN(delta, 4, delta_0);
884 GMRES<4,4>(J4, F, delta_0, delta); //solve linear system
885 }
886
887 bool nan_found = false;
888 for(unsigned int i = 0; i < 4; i++) {
889 if(std::isnan(delta[i])) {
890 nan_found = true; //check if there are NaNs in the solution
891 #ifdef LIGGGHTS_DEBUG
892 error->warning(FLERR,"NaN found in calc_contact_point while solving linear system!\n");
893 printf_debug_data(particleA, particleB, point0, error);
894 #endif
895 *fail = true;
896 break;
897 }
898 }
899
900 bool converged = false;
901 double deltax = MathExtraLiggghts::max(fabs(delta[0]),fabs(delta[1]),fabs(delta[2]));
902 if(deltax == 0.0) {
903 LAMMPS_NS::vectorCopy3D(point, result_point);
904 LAMMPS_NS::vectorCopy3D(point, result_point);
905 converged = true;
906 } else {
907 if(nan_found)
908 break;
909 else {
910 double point_[3], mu_;
911 point_[0] = point[0] - delta[0];
912 point_[1] = point[1] - delta[1];
913 point_[2] = point[2] - delta[2];
914 mu_ = mu - delta[3];
915 double fi_, fj_;
916 double merit2_;
917 double res2_ = calc_F(particleA, particleB, fi_, fj_, particleA->gradient, particleB->gradient, NULL, NULL, point_, mu_, F, &merit2_);
918 if(res2_ < res1 or merit2_ < tol1 or deltax < tol2 * size) {
919 merit2 = merit2_;
920 res2 = res2_;
921 mu = mu_;
922 fi = fi_;
923 fj = fj_;
924 LAMMPS_NS::vectorCopy3D(point_, point);
925 if(merit2 < tol1 or deltax < tol2 * size)
926 converged = true;
927 } else { //make the steepest descent of the residual
928 double a = 0.0;
929 double b = 1.0;
930 double eps = fabs(b-a);
931
932 double res2a, res2b;
933 double merit2a, merit2b;
934
935 while(eps > 1e-8) {
936 double alpha1 = b - (b - a)*PHI_INV;
937 double alpha2 = a + (b - a)*PHI_INV;
938
939 yabx3D(point, -alpha1, delta, pointa);
940 mu1 = mu - alpha1*delta[3];
941 res2a = calc_F(particleA, particleB, fi1, fj1, gradA1, gradB1, NULL, NULL, pointa, mu1, F1, &merit2a);
942
943 yabx3D(point, -alpha2, delta, pointb);
944 mu2 = mu - alpha2*delta[3];
945 res2b = calc_F(particleA, particleB, fi2, fj2, gradA2, gradB2, NULL, NULL, pointb, mu2, F2, &merit2b);
946 if(std::min(res2a, res2b) < res1) {
947 if(res2a < res2b) {
948 res2 = res2a;
949 LAMMPS_NS::vectorCopy4D(F1, F);
950 mu = mu1;
951 LAMMPS_NS::vectorCopy3D(gradA1, particleA->gradient);
952 LAMMPS_NS::vectorCopy3D(gradB1, particleB->gradient);
953 LAMMPS_NS::vectorCopy3D(pointa, point);
954 fi = fi1;
955 fj = fj1;
956 merit2 = merit2a;
957 } else {
958 res2 = res2b;
959 LAMMPS_NS::vectorCopy4D(F2, F);
960 mu = mu2;
961 LAMMPS_NS::vectorCopy3D(gradA2, particleA->gradient);
962 LAMMPS_NS::vectorCopy3D(gradB2, particleB->gradient);
963 LAMMPS_NS::vectorCopy3D(pointb, point);
964 fi = fi2;
965 fj = fj2;
966 merit2 = merit2b;
967 }
968 if(merit2 < tol1)
969 converged = true;
970 eps = 0.0;
971 } else {
972 if(res2a > res2b)
973 a = alpha1;
974 else
975 b = alpha2;
976 eps = fabs(b-a);
977 }
978 }
979 if(eps != 0.0) {
980 LAMMPS_NS::vectorCopy4D(F2, F);
981 mu = mu2;
982 LAMMPS_NS::vectorCopy3D(gradA2, particleA->gradient);
983 LAMMPS_NS::vectorCopy3D(gradB2, particleB->gradient);
984 LAMMPS_NS::vectorCopy3D(pointb, point);
985 fi = fi2;
986 fj = fj2;
987 #ifdef LIGGGHTS_DEBUG
988 if(std::min(merit2a,merit2b)>merit1) {
989 error->warning(FLERR, "Could not find optimal step by Golden section algorithm, epsilon<1e-6");
990 printf_debug_data(particleA, particleB, point0, error);
991 printf("res2b: %e merit: %e\n",res2b, merit2b);
992 }
993 #endif
994 res2 = res2b;
995 merit2 = merit2b;
996 if(merit2 < tol1)
997 converged = true;
998 }
999 }
1000 }
1001
1002 if(fabs(res1-res2)/std::max(res1, res2) < 1e-3)
1003 converged = true;
1004 }
1005
1006 merit1 = merit2;
1007 res1 = res2;
1008 if(iter >= Niter-1) {
1009 *fail = true;
1010 #ifdef LIGGGHTS_DEBUG
1011 error->warning(FLERR, "Contact detection algorithm could not converge to desired tolerance in desired number of iterations\n");
1012 printf_debug_data(particleA, particleB, point0, error);
1013 #endif
1014 }
1015 if(converged || *fail) {
1016 LAMMPS_NS::vectorCopy3D(point, result_point);
1017 break;
1018 }
1019 }
1020 }
1021
1022 const int ndim = 8;
1023
calc_F_IP(Superquadric * particleA,Superquadric * particleB,double * pointA,double * pointB,double * gradA,double * gradB,double lA,double lB,double * F,double * merit)1024 double calc_F_IP(Superquadric *particleA, Superquadric *particleB,
1025 double *pointA, double *pointB,
1026 double *gradA, double *gradB,
1027 double lA, double lB,
1028 double *F, double *merit)
1029 {
1030 double fA, fB;
1031
1032 particleA->shape_function_props_global(pointA, &fA, gradA, NULL);
1033 particleB->shape_function_props_global(pointB, &fB, gradB, NULL);
1034
1035 double delta[3];
1036 LAMMPS_NS::vectorSubtract3D(pointA, pointB, delta);
1037
1038 F[0] = delta[0] + lA*gradA[0];
1039 F[1] = delta[1] + lA*gradA[1];
1040 F[2] = delta[2] + lA*gradA[2];
1041
1042 F[3] = gradA[0] + lB*gradB[0];
1043 F[4] = gradA[1] + lB*gradB[1];
1044 F[5] = gradA[2] + lB*gradB[2];
1045
1046 F[6] = fA;
1047 F[7] = fB;
1048
1049 double vA[3], vB[3];
1050 MathExtra::cross3(gradA, delta, vA);
1051 MathExtra::cross3(gradB, delta, vB);
1052 double sin2A = fabs(LAMMPS_NS::vectorDot3D(vA,vA)) / (LAMMPS_NS::vectorDot3D(gradA, gradA) * (LAMMPS_NS::vectorDot3D(delta, delta)+1e-16));
1053 double sin2B = fabs(LAMMPS_NS::vectorDot3D(vB,vB)) / (LAMMPS_NS::vectorDot3D(gradB, gradB) * (LAMMPS_NS::vectorDot3D(delta, delta)+1e-16));
1054
1055 *merit = fabs(sin2A) + fabs(sin2B) + fabs(fA) + fabs(fB);
1056 double res = 0.0;
1057 for(int i = 0; i < ndim; i++)
1058 res += F[i]*F[i];
1059 return res;
1060 }
1061
calc_F_IP(Superquadric * particleA,Superquadric * particleB,const double * pointA,const double * pointB,double * gradA,double * gradB,double * lA_,double * lB_,double * F,double * merit,double * sine)1062 double calc_F_IP(Superquadric *particleA, Superquadric *particleB,
1063 const double *pointA, const double *pointB,
1064 double *gradA, double *gradB,
1065 double *lA_, double *lB_, double *F, double *merit, double *sine)
1066 {
1067 double fA, fB;
1068
1069 particleA->shape_function_props_global(pointA, &fA, gradA, NULL);
1070 particleB->shape_function_props_global(pointB, &fB, gradB, NULL);
1071
1072 double delta[3];
1073 LAMMPS_NS::vectorSubtract3D(pointA, pointB, delta);
1074
1075 *lA_ = -MathExtra::dot3(gradA, delta) / MathExtra::dot3(gradA, gradA);
1076 *lB_ = -MathExtra::dot3(gradB, gradA) / MathExtra::dot3(gradB, gradB);
1077
1078 double lA = *lA_;
1079 double lB = *lB_;
1080
1081 F[0] = delta[0] + lA*gradA[0];
1082 F[1] = delta[1] + lA*gradA[1];
1083 F[2] = delta[2] + lA*gradA[2];
1084
1085 F[3] = gradA[0] + lB*gradB[0];
1086 F[4] = gradA[1] + lB*gradB[1];
1087 F[5] = gradA[2] + lB*gradB[2];
1088
1089 F[6] = fA;
1090 F[7] = fB;
1091
1092 double vA[3], vB[3];
1093 MathExtra::cross3(gradA, delta, vA);
1094 MathExtra::cross3(gradB, delta, vB);
1095
1096 double sin2A = fabs(LAMMPS_NS::vectorDot3D(vA,vA)) / (LAMMPS_NS::vectorDot3D(gradA, gradA) * (LAMMPS_NS::vectorDot3D(delta, delta)+1e-16));
1097 double sin2B = fabs(LAMMPS_NS::vectorDot3D(vB,vB)) / (LAMMPS_NS::vectorDot3D(gradB, gradB) * (LAMMPS_NS::vectorDot3D(delta, delta)+1e-16));
1098
1099 *merit = fabs(sin2A) + fabs(sin2B) + fabs(fA) + fabs(fB);
1100 *sine = 0.5*sqrt(fabs(sin2A) + fabs(sin2B));
1101
1102 double res = 0.0;
1103 for(int i = 0; i < ndim; i++)
1104 res += F[i]*F[i];
1105 return res;
1106 }
1107
1108 //a minimal distance algorithm based on a common normal concept
minimal_distance(Superquadric * particleA,Superquadric * particleB,const double * initial_pointA_1,const double * initial_pointB_1,double * result_pointA,double * result_pointB,bool * fail)1109 double minimal_distance(Superquadric *particleA, Superquadric *particleB, const double *initial_pointA_1, const double *initial_pointB_1,
1110 double *result_pointA, double *result_pointB,
1111 bool *fail)
1112 {
1113 *fail = false;
1114
1115 double tol0 = 1e-26;
1116 double tol1 = 1e-12;
1117 double tol2 = 1e-16;
1118 double pointA[3], pointB[3];
1119
1120 double F[ndim];
1121 double lA, lB;
1122
1123 double F1[ndim], gradA1[3], gradB1[3];
1124 double lA1, lB1, merit1;
1125
1126 double F2[ndim], gradA2[3], gradB2[3];
1127 double lA2, lB2, merit2;
1128
1129 double merit0;
1130
1131 double sine1;
1132
1133 //calculate the residual vector for the first pair candidate
1134 double res0;
1135 double merit01;
1136 //calculate the residual vector for the first pair candidate
1137 double res01 = calc_F_IP(particleA, particleB, initial_pointA_1, initial_pointB_1, particleA->gradient, particleB->gradient, &lA1, &lB1, F1, &merit01, &sine1);
1138 if(merit01 < tol1 or res01 < tol0) {
1139 LAMMPS_NS::vectorCopy3D(initial_pointA_1, result_pointA);
1140 LAMMPS_NS::vectorCopy3D(initial_pointB_1, result_pointB);
1141 return merit01;
1142 }
1143
1144 vectorCopyN(F1, ndim, F);
1145 LAMMPS_NS::vectorCopy3D(initial_pointA_1, pointA);
1146 LAMMPS_NS::vectorCopy3D(initial_pointB_1, pointB);
1147 lA = lA1;
1148 lB = lB1;
1149 res0 = res01;
1150 merit0 = merit01;
1151
1152 merit1 = merit0;
1153 merit2 = merit0;
1154
1155 double pointA0[3],pointB0[3];
1156 LAMMPS_NS::vectorCopy3D(pointA, pointA0);
1157 LAMMPS_NS::vectorCopy3D(pointB, pointB0);
1158
1159 double delta[ndim], delta_0[ndim];
1160 zeros(delta, ndim);
1161 for(int i = 0; i < ndim; i++)
1162 delta_0[i] = 1.0;
1163 double hessA[9], hessB[9];
1164
1165 double res1 = res0;
1166 double res2 = res1;
1167
1168 bool nan_found = false;
1169
1170 bool converged = false;
1171
1172 int Niter = 10000;
1173
1174 double sizeA = MathExtraLiggghts::min(particleA->shape[0],particleA->shape[1],particleA->shape[2]);
1175 double sizeB = MathExtraLiggghts::min(particleB->shape[0],particleB->shape[1],particleB->shape[2]);
1176
1177 double size = std::min(sizeA, sizeB);
1178
1179 for(int iter = 0; iter < Niter; iter ++)
1180 {
1181
1182 res2 = res1;
1183 merit2 = merit1;
1184
1185 particleA->shape_function_hessian_global(pointA, hessA);
1186 particleB->shape_function_hessian_global(pointB, hessB);
1187
1188 double Jacobian[] =
1189 {
1190 1.0+lA*hessA[0], lA*hessA[1], lA*hessA[2],-1.0, 0.0, 0.0, particleA->gradient[0], 0,
1191 lA*hessA[3], 1.0+lA*hessA[4], lA*hessA[5], 0.0, -1.0, 0.0, particleA->gradient[1], 0,
1192 lA*hessA[6], lA*hessA[7],1.0+lA*hessA[8], 0.0, 0.0, -1.0, particleA->gradient[2], 0,
1193
1194 hessA[0], hessA[1], hessA[2], lB*hessB[0], lB*hessB[1], lB*hessB[2], 0.0, particleB->gradient[0],
1195 hessA[3], hessA[4], hessA[5], lB*hessB[3], lB*hessB[4], lB*hessB[5], 0.0, particleB->gradient[1],
1196 hessA[6], hessA[7], hessA[8], lB*hessB[6], lB*hessB[7], lB*hessB[8], 0.0, particleB->gradient[2],
1197
1198 particleA->gradient[0], particleA->gradient[1], particleA->gradient[2], 0.0, 0.0, 0.0, 0.0, 0.0,
1199 0.0, 0.0, 0.0, particleB->gradient[0], particleB->gradient[1], particleB->gradient[2], 0.0, 0.0
1200
1201 };
1202
1203 vectorCopyN(delta, ndim, delta_0);
1204 GMRES<ndim,ndim>(Jacobian, F, delta_0, delta); //solve linear system
1205
1206 double deltax1 = MathExtraLiggghts::max(fabs(delta[0]),fabs(delta[1]),fabs(delta[2]));
1207 double deltax2 = MathExtraLiggghts::max(fabs(delta[3]),fabs(delta[4]),fabs(delta[5]));
1208
1209 double deltax = std::max(deltax1, deltax2);
1210 if(deltax == 0.0) {
1211 LAMMPS_NS::vectorCopy3D(pointA, result_pointA);
1212 LAMMPS_NS::vectorCopy3D(pointB, result_pointB);
1213
1214 converged = true;
1215 } else {
1216
1217 for(int i = 0; i < ndim; i++) {
1218 if(std::isnan(delta[i])) {
1219 nan_found = true;
1220 break;
1221 }
1222 }
1223 if(nan_found)
1224 break;
1225 if(deltax > 0.01*size) {
1226 double mag = normN(delta, ndim);
1227 for(int i = 0; i < ndim; i++)
1228 delta[i] *= 0.01*size/mag;
1229 }
1230
1231 double lA_, lB_;
1232 double pointA_[3],pointB_[3];
1233 double pointA1[3], pointB1[3];
1234 double pointA2[3], pointB2[3];
1235 LAMMPS_NS::vectorSubtract3D(pointA, delta, pointA_);
1236 LAMMPS_NS::vectorSubtract3D(pointB, delta+3, pointB_);
1237 lA_ = lA - delta[6];
1238 lB_ = lB - delta[7];
1239
1240 double merit2_;
1241 double res2_ = calc_F_IP(particleA, particleB, pointA_, pointB_, particleA->gradient, particleB->gradient, lA_, lB_, F, &merit2_);
1242 if(res2_ < res1 or (res2_ < tol0 or merit2_ < tol1) or deltax < tol2 * size) {
1243 res2 = res2_;
1244 merit2 = merit2_;
1245 LAMMPS_NS::vectorCopy3D(pointA_, pointA);
1246 LAMMPS_NS::vectorCopy3D(pointB_, pointB);
1247
1248 lA = lA_;
1249 lB = lB_;
1250
1251 if((res2_ < tol0 or merit2_ < tol1) || deltax < tol2 * size)
1252 converged = true;
1253 } else { //steepest descent
1254 double a = 0.0;
1255 double b = 1.0;
1256 double eps = fabs(b-a);
1257 double res2a, res2b;
1258 double merit2a, merit2b;
1259
1260 while(eps > 1e-12) { //the golden section algorithm
1261 double alpha1 = b - (b - a)*PHI_INV;
1262 double alpha2 = a + (b - a)*PHI_INV;
1263 yabx3D(pointA, -alpha1, delta, pointA1);
1264 yabx3D(pointB, -alpha1, delta+3, pointB1);
1265 lA1 = lA - alpha1 * delta[6];
1266 lB1 = lB - alpha1 * delta[7];
1267
1268 res2a = calc_F_IP(particleA, particleB, pointA1, pointB1, gradA1, gradB1, lA1, lB1, F1, &merit2a);
1269
1270 yabx3D(pointA, -alpha2, delta, pointA2);
1271 yabx3D(pointB, -alpha2, delta+3, pointB2);
1272 lA2 = lA - alpha2 * delta[6];
1273 lB2 = lB - alpha2 * delta[7];
1274
1275 res2b = calc_F_IP(particleA, particleB, pointA2, pointB2, gradA2, gradB2, lA2, lB2, F2, &merit2b);
1276
1277 if(std::min(res2a, res2b) < res1) {
1278 if(res2a < res2b) {
1279 res2 = res2a;
1280 vectorCopyN(F1, ndim, F);
1281 LAMMPS_NS::vectorCopy3D(gradA1, particleA->gradient);
1282 LAMMPS_NS::vectorCopy3D(gradB1, particleB->gradient);
1283 LAMMPS_NS::vectorCopy3D(pointA1, pointA);
1284 LAMMPS_NS::vectorCopy3D(pointB1, pointB);
1285 lA = lA1;
1286 lB = lB1;
1287
1288 merit2 = merit2a;
1289 } else {
1290 res2 = res2b;
1291 vectorCopyN(F2, ndim, F);
1292 LAMMPS_NS::vectorCopy3D(gradA2, particleA->gradient);
1293 LAMMPS_NS::vectorCopy3D(gradB2, particleB->gradient);
1294 LAMMPS_NS::vectorCopy3D(pointA2, pointA);
1295 LAMMPS_NS::vectorCopy3D(pointB2, pointB);
1296 lA = lA2;
1297 lB = lB2;
1298
1299 merit2 = merit2b;
1300 }
1301 if(res2b < tol0 or merit1 < tol1)
1302 converged = true;
1303 eps = 0.0;
1304 } else {
1305 if(res2a > res2b)
1306 a = alpha1;
1307 else
1308 b = alpha2;
1309 eps = fabs(b-a);
1310 }
1311 }
1312 if(eps != 0.0)
1313 converged = true;
1314 }
1315 }
1316
1317 if(fabs(res1 - res2)/std::max(res1, res2) < 1e-3)
1318 converged = true;
1319
1320 res1 = res2;
1321 merit1 = merit2;
1322
1323 if(converged || iter == Niter - 1) {
1324 LAMMPS_NS::vectorCopy3D(pointA, result_pointA);
1325 LAMMPS_NS::vectorCopy3D(pointB, result_pointB);
1326 break;
1327 }
1328 }
1329 return merit1;
1330 }
1331
capsules_intersect(Superquadric * particleA,Superquadric * particleB,double * capsule_contact_point)1332 bool capsules_intersect(Superquadric *particleA, Superquadric *particleB, double *capsule_contact_point) {
1333
1334 int main_axis_i = 2, main_axis_j = 2; //z-axis is main axis for cylinders
1335 if(!particleA->isCylinder) {
1336 if(particleA->shape[0] >= std::max(particleA->shape[1], particleA->shape[2]))
1337 main_axis_i = 0;
1338 else if(particleA->shape[1] >= std::max(particleA->shape[0], particleA->shape[2]))
1339 main_axis_i = 1;
1340 else
1341 main_axis_i = 2;
1342 //printf("!");
1343 }
1344
1345 if(!particleB->isCylinder) {
1346 if(particleB->shape[0] >= std::max(particleB->shape[1], particleB->shape[2]))
1347 main_axis_j = 0;
1348 else if(particleB->shape[1] >= std::max(particleB->shape[0], particleB->shape[2]))
1349 main_axis_j = 1;
1350 else
1351 main_axis_j = 2;
1352 //printf("!");
1353 }
1354
1355 const double l_i = particleA->shape[main_axis_i];
1356 const double l_j = particleB->shape[main_axis_j];
1357
1358 const double r1_i = particleA->shape[(main_axis_i+1)%3];
1359 const double r2_i = particleA->shape[(main_axis_i+2)%3];
1360 const double r1_j = particleB->shape[(main_axis_j+1)%3];
1361 const double r2_j = particleB->shape[(main_axis_j+2)%3];
1362
1363 double r_i = std::max(r1_i,r2_i);
1364 double r_j = std::max(r1_j,r2_j);
1365
1366 if(!particleA->isCylinder)
1367 r_i = sqrt(r1_i*r1_i + r2_i*r2_i);
1368 if(!particleB->isCylinder)
1369 r_j = sqrt(r1_j*r1_j + r2_j*r2_j);
1370
1371 const double radsum = r_i + r_j;
1372
1373 double Pi[3], Qi[3], Pj[3], Qj[3];
1374 for(int k = 0; k < 3; k++) {
1375 Pi[k] = -particleA->rotation_matrix[k*3+main_axis_i]*l_i + particleA->center[k];
1376 Qi[k] = particleA->rotation_matrix[k*3+main_axis_i]*l_i + particleA->center[k];
1377 Pj[k] = -particleB->rotation_matrix[k*3+main_axis_j]*l_j + particleB->center[k];
1378 Qj[k] = particleB->rotation_matrix[k*3+main_axis_j]*l_j + particleB->center[k];
1379 }
1380
1381 double C1[3], C2[3];
1382 MathExtraLiggghtsNonspherical::line_segments_distance(Pi, Qi, Pj, Qj, C1, C2);
1383 double alpha = r_i / (r_i + r_j);
1384 for(int k = 0; k < 3; k++)
1385 capsule_contact_point[k] = alpha*particleB->center[k] + (1.0-alpha)*particleA->center[k];
1386
1387 return LAMMPS_NS::pointDistance(C1, C2) < radsum;
1388 }
1389
1390 //calculate the contact point if no information about from the previous step available
calc_contact_point_if_no_previous_point_avaialable(SurfacesIntersectData & sidata,Superquadric * particleA,Superquadric * particleB,double * contact_point,double & fi,double & fj,LAMMPS_NS::Error * error)1391 bool calc_contact_point_if_no_previous_point_avaialable(SurfacesIntersectData & sidata, Superquadric *particleA,
1392 Superquadric *particleB, double *contact_point, double &fi, double &fj, LAMMPS_NS::Error *error)
1393 {
1394 bool fail_flag = false;
1395 const double ai = particleA->shape[0];
1396 const double bi = particleA->shape[1];
1397 const double ci = particleA->shape[2];
1398
1399 const double aj = particleB->shape[0];
1400 const double bj = particleB->shape[1];
1401 const double cj = particleB->shape[2];
1402
1403 const double ri = cbrt(ai*bi*ci); // "effective" radius of the particle i
1404 const double rj = cbrt(aj*bj*cj); // "effective" radius of the particle j
1405 double ratio = ri / (ri + rj);
1406 const double ni0 = particleA->blockiness[0];
1407 const double ni1 = particleA->blockiness[1];
1408 const double nj0 = particleB->blockiness[0];
1409 const double nj1 = particleB->blockiness[1];
1410
1411 double pre_estimation[3];
1412 double nmax = std::max(std::max(std::max(ni0, ni1), nj0), nj1);
1413 double step = 1.0;
1414 int N1 = (nmax - 2.0)/step + 1;
1415 int N = std::max(N1, 10);
1416 double N_inv = 1.0/static_cast<double>(N);
1417 for(int i = 0; i <= N; i++) { //iterate from spheres to actual shaped particles
1418 particleA->set_blockiness(2.0 + (ni0 - 2.0)*static_cast<double>(i) * N_inv, 2.0 + (ni1 - 2.0)*static_cast<double>(i) * N_inv);
1419 particleB->set_blockiness(2.0 + (nj0 - 2.0)*static_cast<double>(i) * N_inv, 2.0 + (nj1 - 2.0)*static_cast<double>(i) * N_inv);
1420 particleA->set_shape(ri + (ai - ri)*static_cast<double>(i) * N_inv, ri + (bi - ri)*static_cast<double>(i) * N_inv, ri + (ci - ri)*static_cast<double>(i) * N_inv);
1421 particleB->set_shape(rj + (aj - rj)*static_cast<double>(i) * N_inv, rj + (bj - rj)*static_cast<double>(i) * N_inv, rj + (cj - rj)*static_cast<double>(i) * N_inv);
1422
1423 if(i == 0) {
1424 for(int k = 0; k < 3; k++)
1425 contact_point[k] = ratio*particleB->center[k] + (1.0 - ratio)*particleA->center[k]; //contact point solution for spheres
1426 }
1427 LAMMPS_NS::vectorCopy3D(contact_point, pre_estimation);
1428 MathExtraLiggghtsNonspherical::calc_contact_point(particleA, particleB,
1429 ratio, pre_estimation, contact_point, fi, fj, &fail_flag, error);
1430
1431 particleA->set_shape(ai, bi, ci); //set actual shape parameters back
1432 particleB->set_shape(aj, bj, cj);
1433 particleA->set_blockiness(ni0, ni1);
1434 particleB->set_blockiness(nj0, nj1);
1435 }
1436 return fail_flag;
1437 }
1438
1439 //calculate the contact point using information from previous step
calc_contact_point_using_prev_step(SurfacesIntersectData & sidata,Superquadric * particleA,Superquadric * particleB,double ratio,double dt,double * prev_step_point,double * contact_point,double & fi,double & fj,LAMMPS_NS::Error * error)1440 bool calc_contact_point_using_prev_step(SurfacesIntersectData & sidata, Superquadric *particleA, Superquadric *particleB,
1441 double ratio, double dt, double *prev_step_point, double *contact_point, double &fi, double &fj, LAMMPS_NS::Error *error)
1442 {
1443 bool fail_flag = false;
1444
1445 #ifdef SUPERQUADRIC_ACTIVE_FLAG
1446
1447 double estimation[3];
1448 double xci[3], xcj[3], v_rot_i[3], v_rot_j[3], vi[3], vj[3];
1449 LAMMPS_NS::vectorSubtract3D(prev_step_point, particleA->center, xci);
1450 LAMMPS_NS::vectorSubtract3D(prev_step_point, particleB->center, xcj);
1451 LAMMPS_NS::vectorCross3D(sidata.omega_i, xci, v_rot_i);
1452 LAMMPS_NS::vectorCross3D(sidata.omega_j, xcj, v_rot_j);
1453
1454 for(int k = 0; k < 3; k++) {
1455 vi[k] = sidata.v_i[k] + v_rot_i[k]; //velocity of the contact point with respect to particle i
1456 vj[k] = sidata.v_j[k] + v_rot_j[k]; //velocity of the contact point with respect to particle j
1457 }
1458 double v_eff[3];
1459 for(int k = 0; k < 3; k++)
1460 v_eff[k] = (vi[k]*sidata.mi + vj[k]*sidata.mj) / (sidata.mi+sidata.mj); //average velocity of the contact point
1461
1462 for(int k = 0; k < 3; k++)
1463 estimation[k] = prev_step_point[k] + v_eff[k]*dt; //contact point estimation
1464
1465 MathExtraLiggghtsNonspherical::calc_contact_point(particleA, particleB, ratio, estimation, contact_point, fi, fj, &fail_flag, error);
1466 #endif
1467
1468 return fail_flag;
1469 }
1470
1471 //basic estimation of the overlap magnitude
basic_overlap_algorithm(SurfacesIntersectData & sidata,Superquadric * particleA,Superquadric * particleB,double & alphaA,double & alphaB,const double * contact_point,double * contact_pointA,double * contact_pointB)1472 void basic_overlap_algorithm(SurfacesIntersectData & sidata, Superquadric *particleA, Superquadric *particleB,
1473 double &alphaA, double &alphaB, const double *contact_point, double *contact_pointA, double *contact_pointB)
1474 {
1475 particleA->map_point(contact_point, contact_pointA);
1476 particleB->map_point(contact_point, contact_pointB);
1477 double deltaA[3], deltaB[3];
1478 LAMMPS_NS::vectorSubtract3D(contact_pointA, contact_point, deltaA);
1479 LAMMPS_NS::vectorSubtract3D(contact_pointB, contact_point, deltaB);
1480 alphaA = LAMMPS_NS::vectorDot3D(deltaA, sidata.en);
1481 alphaB = LAMMPS_NS::vectorDot3D(deltaB, sidata.en);
1482
1483 sidata.deltan = fabs(alphaA) + fabs(alphaB);
1484
1485 LAMMPS_NS::vectorScalarMult3D(sidata.en, sidata.deltan, sidata.delta);
1486 }
1487
1488 //more sophisticated overlap algorithm, uses the same overlap direction
extended_overlap_algorithm(Superquadric * particleA,Superquadric * particleB,double * en,double * const alpha_A,double * const alpha_B,const double * contact_point,double * contact_pointA,double * contact_pointB,double * delta)1489 double extended_overlap_algorithm(Superquadric *particleA, Superquadric *particleB, double *en,
1490 double *const alpha_A, double *const alpha_B,
1491 const double *contact_point, double *contact_pointA, double *contact_pointB, double *delta)
1492 {
1493 #ifdef SUPERQUADRIC_ACTIVE_FLAG
1494 bool use_alpha_ellipsoid = false;
1495 LAMMPS_NS::vectorNegate3D(en);
1496 *alpha_A = particleA->surface_line_intersection(use_alpha_ellipsoid, contact_point, en, *alpha_A, contact_pointA);
1497 LAMMPS_NS::vectorNegate3D(en);
1498 *alpha_B = particleB->surface_line_intersection(use_alpha_ellipsoid, contact_point, en, *alpha_B, contact_pointB);
1499
1500 LAMMPS_NS::vectorSubtract3D(contact_pointB, contact_pointA, delta);
1501 return fabs(*alpha_A) + fabs(*alpha_B); //overlap magnitude
1502 #else
1503 return 0.0;
1504 #endif
1505 }
1506
1507 //common normal algorithm
common_normal(SurfacesIntersectData & sidata,Superquadric * particleA,Superquadric * particleB,bool particles_were_in_contact_flag,double * const contact_point_A_history,double * const contact_point_B_history,double * contact_point_A,double * contact_point_B)1508 double common_normal(SurfacesIntersectData & sidata, Superquadric *particleA, Superquadric *particleB, bool particles_were_in_contact_flag,
1509 double *const contact_point_A_history, double *const contact_point_B_history, //from previous timestep
1510 double *contact_point_A, double *contact_point_B) //from line-surface intersection algorithm
1511 {
1512 bool fail_flag;
1513 double contact_point_A_temp1[3], contact_point_B_temp1[3];
1514 double contact_point_A_temp2[3], contact_point_B_temp2[3];
1515
1516 LAMMPS_NS::vectorCopy3D(contact_point_A, contact_point_A_temp2);
1517 LAMMPS_NS::vectorCopy3D(contact_point_B, contact_point_B_temp2);
1518
1519 if(!particles_were_in_contact_flag) {
1520 LAMMPS_NS::vectorCopy3D(contact_point_A_temp2, contact_point_A_temp1); //use solution from overlap model 1 as initial estimation
1521 LAMMPS_NS::vectorCopy3D(contact_point_B_temp2, contact_point_B_temp1);
1522 } else {
1523 LAMMPS_NS::vectorCopy3D(contact_point_A_history, contact_point_A_temp1); //use pair of contact points from previous step as initial estimation
1524 LAMMPS_NS::vectorCopy3D(contact_point_B_history, contact_point_B_temp1);
1525 }
1526
1527 double result_pointA1[3],result_pointB1[3];
1528 double result_pointA2[3],result_pointB2[3];
1529
1530 double res1 = MathExtraLiggghtsNonspherical::minimal_distance(particleA, particleB, contact_point_A_temp1, contact_point_B_temp1,
1531 result_pointA1, result_pointB1, &fail_flag);
1532 double res = res1;
1533
1534 if(res > 1e-12) {
1535 double res2 = MathExtraLiggghtsNonspherical::minimal_distance(particleA, particleB, contact_point_A_temp2, contact_point_B_temp2,
1536 result_pointA2, result_pointB2, &fail_flag);
1537
1538 if(res2 > res1) {
1539 LAMMPS_NS::vectorCopy3D(result_pointA1, contact_point_A);
1540 LAMMPS_NS::vectorCopy3D(result_pointB1, contact_point_B);
1541 res = res1;
1542 } else {
1543 LAMMPS_NS::vectorCopy3D(result_pointA2, contact_point_A);
1544 LAMMPS_NS::vectorCopy3D(result_pointB2, contact_point_B);
1545 res = res2;
1546 }
1547 } else {
1548 LAMMPS_NS::vectorCopy3D(result_pointA1, contact_point_A);
1549 LAMMPS_NS::vectorCopy3D(result_pointB1, contact_point_B);
1550 res = res1;
1551 }
1552
1553 LAMMPS_NS::vectorSubtract3D(contact_point_B, contact_point_A, sidata.delta);
1554 LAMMPS_NS::vectorCopy3D(sidata.delta, sidata.en); // overlap direction
1555 LAMMPS_NS::vectorNormalize3D(sidata.en);
1556
1557 LAMMPS_NS::vectorCopy3D(contact_point_A, contact_point_A_history); //store pair of contact points in local coordinates for each particles
1558 LAMMPS_NS::vectorCopy3D(contact_point_B, contact_point_B_history);
1559
1560 sidata.contact_point[0] = 0.5*(contact_point_A[0] + contact_point_B[0]);
1561 sidata.contact_point[1] = 0.5*(contact_point_A[1] + contact_point_B[1]);
1562 sidata.contact_point[2] = 0.5*(contact_point_A[2] + contact_point_B[2]);
1563
1564 return LAMMPS_NS::vectorMag3D(sidata.delta); //overlap magnitude
1565 }
1566
1567 #ifdef LIGGGHTS_DEBUG
printf_debug_data(Superquadric * particleA,Superquadric * particleB,double * initial_guess,LAMMPS_NS::Error * error)1568 void printf_debug_data(Superquadric *particleA, Superquadric *particleB, double *initial_guess ,LAMMPS_NS::Error *error) //TODO: implement as warnings
1569 {
1570 printf("Initial guess in Newton method: %1.13f %1.13f %1.13f\n", initial_guess[0], initial_guess[1], initial_guess[2]);
1571 printf("Particle 1: %1.13f, %1.13f, %1.13f, %1.13f, %1.13f, %1.13f, %1.13f, %1.13f, %1.13f, %1.13f, %1.13f, %1.13f\n",
1572 particleA->center[0], particleA->center[1], particleA->center[2],
1573 particleA->shape[0], particleA->shape[1], particleA->shape[2],
1574 particleA->blockiness[0], particleA->blockiness[1],
1575 particleA->quat[0], particleA->quat[1], particleA->quat[2], particleA->quat[3]);
1576 printf("Particle 2: %1.13f, %1.13f, %1.13f, %1.13f, %1.13f, %1.13f, %1.13f, %1.13f, %1.13f, %1.13f, %1.13f, %1.13f\n",
1577 particleB->center[0], particleB->center[1], particleB->center[2],
1578 particleB->shape[0], particleB->shape[1], particleB->shape[2],
1579 particleB->blockiness[0], particleB->blockiness[1],
1580 particleB->quat[0], particleB->quat[1], particleB->quat[2], particleB->quat[3]);
1581 }
1582 #endif
1583
1584 }
1585 #endif
1586