1 // clang-format off
2 /* ----------------------------------------------------------------------
3 LAMMPS - Large-scale Atomic/Molecular Massively Parallel Simulator
4 https://www.lammps.org/, Sandia National Laboratories
5 Steve Plimpton, sjplimp@sandia.gov
6
7 Copyright (2003) Sandia Corporation. Under the terms of Contract
8 DE-AC04-94AL85000 with Sandia Corporation, the U.S. Government retains
9 certain rights in this software. This software is distributed under
10 the GNU General Public License.
11
12 See the README file in the top-level LAMMPS directory.
13 ------------------------------------------------------------------------- */
14
15 /* ----------------------------------------------------------------------
16 Contributing author: Trung Dac Nguyen (ndactrung@gmail.com)
17 Ref: Wang, Yu, Langston, Fraige, Particle shape effects in discrete
18 element modelling of cohesive angular particles, Granular Matter 2011,
19 13:1-12.
20 Note: The current implementation has not taken into account
21 the contact history for friction forces.
22 ------------------------------------------------------------------------- */
23
24 #include "pair_body_rounded_polyhedron.h"
25
26 #include "atom.h"
27 #include "atom_vec_body.h"
28 #include "body_rounded_polyhedron.h"
29 #include "comm.h"
30 #include "error.h"
31 #include "fix.h"
32 #include "force.h"
33 #include "math_const.h"
34 #include "math_extra.h"
35 #include "memory.h"
36 #include "modify.h"
37 #include "neigh_list.h"
38 #include "neighbor.h"
39
40 #include <cmath>
41 #include <cstring>
42
43 using namespace LAMMPS_NS;
44 using namespace MathConst;
45
46 #define DELTA 10000
47 #define EPSILON 1e-3
48 #define MAX_FACE_SIZE 4 // maximum number of vertices per face (same as BodyRoundedPolyhedron)
49 #define MAX_CONTACTS 32 // for 3D models (including duplicated counts)
50
51 //#define _POLYHEDRON_DEBUG
52
53 enum {EE_INVALID=0,EE_NONE,EE_INTERACT};
54 enum {EF_INVALID=0,EF_NONE,EF_PARALLEL,EF_SAME_SIDE_OF_FACE,
55 EF_INTERSECT_INSIDE,EF_INTERSECT_OUTSIDE};
56
57 /* ---------------------------------------------------------------------- */
58
PairBodyRoundedPolyhedron(LAMMPS * lmp)59 PairBodyRoundedPolyhedron::PairBodyRoundedPolyhedron(LAMMPS *lmp) : Pair(lmp)
60 {
61 dmax = nmax = 0;
62 discrete = nullptr;
63 dnum = dfirst = nullptr;
64
65 edmax = ednummax = 0;
66 edge = nullptr;
67 ednum = edfirst = nullptr;
68
69 facmax = facnummax = 0;
70 face = nullptr;
71 facnum = facfirst = nullptr;
72
73 enclosing_radius = nullptr;
74 rounded_radius = nullptr;
75 maxerad = nullptr;
76
77 single_enable = 0;
78 restartinfo = 0;
79
80 c_n = 0.1;
81 c_t = 0.2;
82 mu = 0.0;
83 A_ua = 1.0;
84
85 k_n = nullptr;
86 k_na = nullptr;
87 }
88
89 /* ---------------------------------------------------------------------- */
90
~PairBodyRoundedPolyhedron()91 PairBodyRoundedPolyhedron::~PairBodyRoundedPolyhedron()
92 {
93 memory->destroy(discrete);
94 memory->destroy(dnum);
95 memory->destroy(dfirst);
96
97 memory->destroy(edge);
98 memory->destroy(ednum);
99 memory->destroy(edfirst);
100
101 memory->destroy(face);
102 memory->destroy(facnum);
103 memory->destroy(facfirst);
104
105 memory->destroy(enclosing_radius);
106 memory->destroy(rounded_radius);
107 memory->destroy(maxerad);
108
109 if (allocated) {
110 memory->destroy(setflag);
111 memory->destroy(cutsq);
112
113 memory->destroy(k_n);
114 memory->destroy(k_na);
115 }
116 }
117
118 /* ---------------------------------------------------------------------- */
119
compute(int eflag,int vflag)120 void PairBodyRoundedPolyhedron::compute(int eflag, int vflag)
121 {
122 int i,j,ii,jj,inum,jnum,itype,jtype;
123 int ni,nj,npi,npj,ifirst,jfirst,nei,nej,iefirst,jefirst;
124 double xtmp,ytmp,ztmp,delx,dely,delz,evdwl,facc[3];
125 double rsq,eradi,eradj;
126 int *ilist,*jlist,*numneigh,**firstneigh;
127
128 evdwl = 0.0;
129 ev_init(eflag,vflag);
130
131 double **x = atom->x;
132 double **v = atom->v;
133 double **f = atom->f;
134 double **torque = atom->torque;
135 double **angmom = atom->angmom;
136 int *body = atom->body;
137 int *type = atom->type;
138 int nlocal = atom->nlocal;
139 int nall = nlocal + atom->nghost;
140 int newton_pair = force->newton_pair;
141
142 inum = list->inum;
143 ilist = list->ilist;
144 numneigh = list->numneigh;
145 firstneigh = list->firstneigh;
146
147 // grow the per-atom lists if necessary and initialize
148
149 if (atom->nmax > nmax) {
150 memory->destroy(dnum);
151 memory->destroy(dfirst);
152 memory->destroy(ednum);
153 memory->destroy(edfirst);
154 memory->destroy(facnum);
155 memory->destroy(facfirst);
156 memory->destroy(enclosing_radius);
157 memory->destroy(rounded_radius);
158 nmax = atom->nmax;
159 memory->create(dnum,nmax,"pair:dnum");
160 memory->create(dfirst,nmax,"pair:dfirst");
161 memory->create(ednum,nmax,"pair:ednum");
162 memory->create(edfirst,nmax,"pair:edfirst");
163 memory->create(facnum,nmax,"pair:facnum");
164 memory->create(facfirst,nmax,"pair:facfirst");
165 memory->create(enclosing_radius,nmax,"pair:enclosing_radius");
166 memory->create(rounded_radius,nmax,"pair:rounded_radius");
167 }
168
169 ndiscrete = nedge = nface = 0;
170 for (i = 0; i < nall; i++)
171 dnum[i] = ednum[i] = facnum[i] = 0;
172
173 // loop over neighbors of my atoms
174
175 for (ii = 0; ii < inum; ii++) {
176 i = ilist[ii];
177 xtmp = x[i][0];
178 ytmp = x[i][1];
179 ztmp = x[i][2];
180 itype = type[i];
181 jlist = firstneigh[i];
182 jnum = numneigh[i];
183
184 if (body[i] >= 0) {
185 if (dnum[i] == 0) body2space(i);
186 npi = dnum[i];
187 ifirst = dfirst[i];
188 nei = ednum[i];
189 iefirst = edfirst[i];
190 eradi = enclosing_radius[i];
191 }
192
193 for (jj = 0; jj < jnum; jj++) {
194 j = jlist[jj];
195 j &= NEIGHMASK;
196
197 delx = xtmp - x[j][0];
198 dely = ytmp - x[j][1];
199 delz = ztmp - x[j][2];
200 rsq = delx*delx + dely*dely + delz*delz;
201 jtype = type[j];
202
203 // body/body interactions
204
205 evdwl = 0.0;
206 facc[0] = facc[1] = facc[2] = 0;
207
208 if (body[i] < 0 || body[j] < 0) continue;
209
210 if (dnum[j] == 0) body2space(j);
211 npj = dnum[j];
212 jfirst = dfirst[j];
213 nej = ednum[j];
214 jefirst = edfirst[j];
215 eradj = enclosing_radius[j];
216
217 // no interaction
218
219 double r = sqrt(rsq);
220 if (r > eradi + eradj + cut_inner) continue;
221
222 // sphere-sphere interaction
223
224 if (npi == 1 && npj == 1) {
225 sphere_against_sphere(i, j, itype, jtype, delx, dely, delz,
226 rsq, v, f, evflag);
227 continue;
228 }
229
230 // reset vertex and edge forces
231
232 for (ni = 0; ni < npi; ni++) {
233 discrete[ifirst+ni][3] = 0;
234 discrete[ifirst+ni][4] = 0;
235 discrete[ifirst+ni][5] = 0;
236 discrete[ifirst+ni][6] = 0;
237 }
238
239 for (nj = 0; nj < npj; nj++) {
240 discrete[jfirst+nj][3] = 0;
241 discrete[jfirst+nj][4] = 0;
242 discrete[jfirst+nj][5] = 0;
243 discrete[jfirst+nj][6] = 0;
244 }
245
246 for (ni = 0; ni < nei; ni++) {
247 edge[iefirst+ni][2] = 0;
248 edge[iefirst+ni][3] = 0;
249 edge[iefirst+ni][4] = 0;
250 edge[iefirst+ni][5] = 0;
251 }
252
253 for (nj = 0; nj < nej; nj++) {
254 edge[jefirst+nj][2] = 0;
255 edge[jefirst+nj][3] = 0;
256 edge[jefirst+nj][4] = 0;
257 edge[jefirst+nj][5] = 0;
258 }
259
260 // one of the two bodies is a sphere
261
262 if (npj == 1) {
263 sphere_against_face(i, j, itype, jtype, x, v, f, torque,
264 angmom, evflag);
265 sphere_against_edge(i, j, itype, jtype, x, v, f, torque,
266 angmom, evflag);
267 continue;
268 } else if (npi == 1) {
269 sphere_against_face(j, i, jtype, itype, x, v, f, torque,
270 angmom, evflag);
271 sphere_against_edge(j, i, jtype, itype, x, v, f, torque,
272 angmom, evflag);
273 continue;
274 }
275
276 int num_contacts;
277 Contact contact_list[MAX_CONTACTS];
278
279 num_contacts = 0;
280
281 // check interaction between i's edges and j' faces
282 #ifdef _POLYHEDRON_DEBUG
283 printf("INTERACTION between edges of %d vs. faces of %d:\n", i, j);
284 #endif
285 edge_against_face(i, j, itype, jtype, x, contact_list,
286 num_contacts, evdwl, facc);
287
288 // check interaction between j's edges and i' faces
289 #ifdef _POLYHEDRON_DEBUG
290 printf("\nINTERACTION between edges of %d vs. faces of %d:\n", j, i);
291 #endif
292 edge_against_face(j, i, jtype, itype, x, contact_list,
293 num_contacts, evdwl, facc);
294
295 // check interaction between i's edges and j' edges
296 #ifdef _POLYHEDRON_DEBUG
297 printf("INTERACTION between edges of %d vs. edges of %d:\n", i, j);
298 #endif
299 edge_against_edge(i, j, itype, jtype, x, contact_list,
300 num_contacts, evdwl, facc);
301
302 // estimate the contact area
303 // also consider point contacts and line contacts
304
305 if (num_contacts > 0) {
306 rescale_cohesive_forces(x, f, torque, contact_list, num_contacts,
307 itype, jtype, facc);
308 }
309
310 if (evflag) ev_tally_xyz(i,j,nlocal,newton_pair,evdwl,0.0,
311 facc[0],facc[1],facc[2],delx,dely,delz);
312
313 } // end for jj
314 }
315
316 if (vflag_fdotr) virial_fdotr_compute();
317 }
318
319 /* ----------------------------------------------------------------------
320 allocate all arrays
321 ------------------------------------------------------------------------- */
322
allocate()323 void PairBodyRoundedPolyhedron::allocate()
324 {
325 allocated = 1;
326 int n = atom->ntypes;
327
328 memory->create(setflag,n+1,n+1,"pair:setflag");
329 for (int i = 1; i <= n; i++)
330 for (int j = i; j <= n; j++)
331 setflag[i][j] = 0;
332
333 memory->create(cutsq,n+1,n+1,"pair:cutsq");
334
335 memory->create(k_n,n+1,n+1,"pair:k_n");
336 memory->create(k_na,n+1,n+1,"pair:k_na");
337 memory->create(maxerad,n+1,"pair:maxerad");
338 }
339
340 /* ----------------------------------------------------------------------
341 global settings
342 ------------------------------------------------------------------------- */
343
settings(int narg,char ** arg)344 void PairBodyRoundedPolyhedron::settings(int narg, char **arg)
345 {
346 if (narg < 5) error->all(FLERR,"Illegal pair_style command");
347
348 c_n = utils::numeric(FLERR,arg[0],false,lmp);
349 c_t = utils::numeric(FLERR,arg[1],false,lmp);
350 mu = utils::numeric(FLERR,arg[2],false,lmp);
351 A_ua = utils::numeric(FLERR,arg[3],false,lmp);
352 cut_inner = utils::numeric(FLERR,arg[4],false,lmp);
353
354 if (A_ua < 0) A_ua = 1;
355 }
356
357 /* ----------------------------------------------------------------------
358 set coeffs for one or more type pairs
359 ------------------------------------------------------------------------- */
360
coeff(int narg,char ** arg)361 void PairBodyRoundedPolyhedron::coeff(int narg, char **arg)
362 {
363 if (narg < 4 || narg > 5)
364 error->all(FLERR,"Incorrect args for pair coefficients");
365 if (!allocated) allocate();
366
367 int ilo,ihi,jlo,jhi;
368 utils::bounds(FLERR,arg[0],1,atom->ntypes,ilo,ihi,error);
369 utils::bounds(FLERR,arg[1],1,atom->ntypes,jlo,jhi,error);
370
371 double k_n_one = utils::numeric(FLERR,arg[2],false,lmp);
372 double k_na_one = utils::numeric(FLERR,arg[3],false,lmp);
373
374 int count = 0;
375 for (int i = ilo; i <= ihi; i++) {
376 for (int j = MAX(jlo,i); j <= jhi; j++) {
377 k_n[i][j] = k_n_one;
378 k_na[i][j] = k_na_one;
379 setflag[i][j] = 1;
380 count++;
381 }
382 }
383
384 if (count == 0) error->all(FLERR,"Incorrect args for pair coefficients");
385 }
386
387 /* ----------------------------------------------------------------------
388 init specific to this pair style
389 ------------------------------------------------------------------------- */
390
init_style()391 void PairBodyRoundedPolyhedron::init_style()
392 {
393 avec = (AtomVecBody *) atom->style_match("body");
394 if (!avec) error->all(FLERR,"Pair body/rounded/polyhedron requires "
395 "atom style body");
396 if (strcmp(avec->bptr->style,"rounded/polyhedron") != 0)
397 error->all(FLERR,"Pair body/rounded/polyhedron requires "
398 "body style rounded/polyhedron");
399 bptr = (BodyRoundedPolyhedron *) avec->bptr;
400
401 if (force->newton_pair == 0)
402 error->all(FLERR,"Pair style body/rounded/polyhedron requires "
403 "newton pair on");
404
405 if (comm->ghost_velocity == 0)
406 error->all(FLERR,"Pair body/rounded/polyhedron requires "
407 "ghost atoms store velocity");
408
409 neighbor->request(this);
410
411 // find the maximum enclosing radius for each atom type
412
413 int i, itype;
414 double eradi;
415 int* body = atom->body;
416 int* type = atom->type;
417 int ntypes = atom->ntypes;
418 int nlocal = atom->nlocal;
419
420 if (atom->nmax > nmax) {
421 memory->destroy(dnum);
422 memory->destroy(dfirst);
423 memory->destroy(ednum);
424 memory->destroy(edfirst);
425 memory->destroy(facnum);
426 memory->destroy(facfirst);
427 memory->destroy(enclosing_radius);
428 memory->destroy(rounded_radius);
429 nmax = atom->nmax;
430 memory->create(dnum,nmax,"pair:dnum");
431 memory->create(dfirst,nmax,"pair:dfirst");
432 memory->create(ednum,nmax,"pair:ednum");
433 memory->create(edfirst,nmax,"pair:edfirst");
434 memory->create(facnum,nmax,"pair:facnum");
435 memory->create(facfirst,nmax,"pair:facfirst");
436 memory->create(enclosing_radius,nmax,"pair:enclosing_radius");
437 memory->create(rounded_radius,nmax,"pair:rounded_radius");
438 }
439
440 ndiscrete = nedge = nface = 0;
441 for (i = 0; i < nlocal; i++)
442 dnum[i] = ednum[i] = facnum[i] = 0;
443
444 double *merad = nullptr;
445 memory->create(merad,ntypes+1,"pair:merad");
446 for (i = 1; i <= ntypes; i++)
447 maxerad[i] = merad[i] = 0;
448
449 int ipour;
450 for (ipour = 0; ipour < modify->nfix; ipour++)
451 if (strcmp(modify->fix[ipour]->style,"pour") == 0) break;
452 if (ipour == modify->nfix) ipour = -1;
453
454 int idep;
455 for (idep = 0; idep < modify->nfix; idep++)
456 if (strcmp(modify->fix[idep]->style,"deposit") == 0) break;
457 if (idep == modify->nfix) idep = -1;
458
459 for (i = 1; i <= ntypes; i++) {
460 merad[i] = 0.0;
461 if (ipour >= 0) {
462 itype = i;
463 merad[i] =
464 *((double *) modify->fix[ipour]->extract("radius",itype));
465 }
466 if (idep >= 0) {
467 itype = i;
468 merad[i] =
469 *((double *) modify->fix[idep]->extract("radius",itype));
470 }
471 }
472
473 for (i = 0; i < nlocal; i++) {
474 itype = type[i];
475 if (body[i] >= 0) {
476 if (dnum[i] == 0) body2space(i);
477 eradi = enclosing_radius[i];
478 if (eradi > merad[itype]) merad[itype] = eradi;
479 } else
480 merad[itype] = 0;
481 }
482
483 MPI_Allreduce(&merad[1],&maxerad[1],ntypes,MPI_DOUBLE,MPI_MAX,world);
484
485 memory->destroy(merad);
486
487 sanity_check();
488 }
489
490 /* ----------------------------------------------------------------------
491 init for one type pair i,j and corresponding j,i
492 ------------------------------------------------------------------------- */
493
init_one(int i,int j)494 double PairBodyRoundedPolyhedron::init_one(int i, int j)
495 {
496 k_n[j][i] = k_n[i][j];
497 k_na[j][i] = k_na[i][j];
498
499 return (maxerad[i]+maxerad[j]);
500 }
501
502 /* ----------------------------------------------------------------------
503 convert N sub-particles in body I to space frame using current quaternion
504 store sub-particle space-frame displacements from COM in discrete list
505 ------------------------------------------------------------------------- */
506
body2space(int i)507 void PairBodyRoundedPolyhedron::body2space(int i)
508 {
509 int ibonus = atom->body[i];
510 AtomVecBody::Bonus *bonus = &avec->bonus[ibonus];
511 int nsub = bptr->nsub(bonus);
512 double *coords = bptr->coords(bonus);
513 int body_num_edges = bptr->nedges(bonus);
514 double* edge_ends = bptr->edges(bonus);
515 int body_num_faces = bptr->nfaces(bonus);
516 double* face_pts = bptr->faces(bonus);
517 double eradius = bptr->enclosing_radius(bonus);
518 double rradius = bptr->rounded_radius(bonus);
519
520 // get the number of sub-particles (vertices)
521 // and the index of the first vertex of my body in the list
522
523 dnum[i] = nsub;
524 dfirst[i] = ndiscrete;
525
526 // grow the vertex list if necessary
527 // the first 3 columns are for coords, the last 3 for forces
528
529 if (ndiscrete + nsub > dmax) {
530 dmax += DELTA;
531 memory->grow(discrete,dmax,7,"pair:discrete");
532 }
533
534 double p[3][3];
535 MathExtra::quat_to_mat(bonus->quat,p);
536
537 for (int m = 0; m < nsub; m++) {
538 MathExtra::matvec(p,&coords[3*m],discrete[ndiscrete]);
539 discrete[ndiscrete][3] = 0;
540 discrete[ndiscrete][4] = 0;
541 discrete[ndiscrete][5] = 0;
542 discrete[ndiscrete][6] = 0;
543 ndiscrete++;
544 }
545
546 // get the number of edges (vertices)
547 // and the index of the first edge of my body in the list
548
549 ednum[i] = body_num_edges;
550 edfirst[i] = nedge;
551
552 // grow the edge list if necessary
553 // the first 2 columns are for vertex indices within body, the last 3 for forces
554
555 if (nedge + body_num_edges > edmax) {
556 edmax += DELTA;
557 memory->grow(edge,edmax,6,"pair:edge");
558 }
559
560 if ((body_num_edges > 0) && (edge_ends == nullptr))
561 error->one(FLERR,"Inconsistent edge data for body of atom {}",
562 atom->tag[i]);
563
564 for (int m = 0; m < body_num_edges; m++) {
565 edge[nedge][0] = static_cast<int>(edge_ends[2*m+0]);
566 edge[nedge][1] = static_cast<int>(edge_ends[2*m+1]);
567 edge[nedge][2] = 0;
568 edge[nedge][3] = 0;
569 edge[nedge][4] = 0;
570 edge[nedge][5] = 0;
571 nedge++;
572 }
573
574 // get the number of faces and the index of the first face
575
576 facnum[i] = body_num_faces;
577 facfirst[i] = nface;
578
579 // grow the face list if necessary
580 // the first 3 columns are for vertex indices within body, the last 3 for forces
581
582 if (nface + body_num_faces > facmax) {
583 facmax += DELTA;
584 memory->grow(face,facmax,MAX_FACE_SIZE,"pair:face");
585 }
586
587 if ((body_num_faces > 0) && (face_pts == nullptr))
588 error->one(FLERR,"Inconsistent face data for body of atom {}",
589 atom->tag[i]);
590
591 for (int m = 0; m < body_num_faces; m++) {
592 for (int k = 0; k < MAX_FACE_SIZE; k++)
593 face[nface][k] = static_cast<int>(face_pts[MAX_FACE_SIZE*m+k]);
594 nface++;
595 }
596
597 enclosing_radius[i] = eradius;
598 rounded_radius[i] = rradius;
599 }
600
601 /* ----------------------------------------------------------------------
602 Interaction between two spheres with different radii
603 according to the 2D model from Fraige et al.
604 ---------------------------------------------------------------------- */
605
sphere_against_sphere(int ibody,int jbody,int itype,int jtype,double delx,double dely,double delz,double rsq,double ** v,double ** f,int evflag)606 void PairBodyRoundedPolyhedron::sphere_against_sphere(int ibody, int jbody,
607 int itype, int jtype, double delx, double dely, double delz, double rsq,
608 double** v, double** f, int evflag)
609 {
610 double rradi,rradj,contact_dist;
611 double vr1,vr2,vr3,vnnr,vn1,vn2,vn3,vt1,vt2,vt3;
612 double rij,rsqinv,R,fx,fy,fz,fn[3],ft[3],fpair,energy;
613 int nlocal = atom->nlocal;
614 int newton_pair = force->newton_pair;
615
616 rradi = rounded_radius[ibody];
617 rradj = rounded_radius[jbody];
618 contact_dist = rradi + rradj;
619
620 rij = sqrt(rsq);
621 R = rij - contact_dist;
622
623 energy = 0;
624 kernel_force(R, itype, jtype, energy, fpair);
625
626 fx = delx*fpair/rij;
627 fy = dely*fpair/rij;
628 fz = delz*fpair/rij;
629
630 if (R <= 0) { // in contact
631
632 // relative translational velocity
633
634 vr1 = v[ibody][0] - v[jbody][0];
635 vr2 = v[ibody][1] - v[jbody][1];
636 vr3 = v[ibody][2] - v[jbody][2];
637
638 // normal component
639
640 rsqinv = 1.0/rsq;
641 vnnr = vr1*delx + vr2*dely + vr3*delz;
642 vn1 = delx*vnnr * rsqinv;
643 vn2 = dely*vnnr * rsqinv;
644 vn3 = delz*vnnr * rsqinv;
645
646 // tangential component
647
648 vt1 = vr1 - vn1;
649 vt2 = vr2 - vn2;
650 vt3 = vr3 - vn3;
651
652 // normal friction term at contact
653
654 fn[0] = -c_n * vn1;
655 fn[1] = -c_n * vn2;
656 fn[2] = -c_n * vn3;
657
658 // tangential friction term at contact,
659 // excluding the tangential deformation term for now
660
661 ft[0] = -c_t * vt1;
662 ft[1] = -c_t * vt2;
663 ft[2] = -c_t * vt3;
664
665 fx += fn[0] + ft[0];
666 fy += fn[1] + ft[1];
667 fz += fn[2] + ft[2];
668 }
669
670 f[ibody][0] += fx;
671 f[ibody][1] += fy;
672 f[ibody][2] += fz;
673
674 if (newton_pair || jbody < nlocal) {
675 f[jbody][0] -= fx;
676 f[jbody][1] -= fy;
677 f[jbody][2] -= fz;
678 }
679
680 if (evflag) ev_tally_xyz(ibody,jbody,nlocal,newton_pair,
681 energy,0.0,fx,fy,fz,delx,dely,delz);
682 }
683
684 /* ----------------------------------------------------------------------
685 Interaction bt the edges of a polyhedron (ibody) and a sphere (jbody)
686 ---------------------------------------------------------------------- */
687
sphere_against_edge(int ibody,int jbody,int itype,int jtype,double ** x,double ** v,double ** f,double ** torque,double ** angmom,int evflag)688 void PairBodyRoundedPolyhedron::sphere_against_edge(int ibody, int jbody,
689 int itype, int jtype, double** x, double** v, double** f, double** torque,
690 double** angmom, int evflag)
691 {
692 int ni,nei,ifirst,iefirst,npi1,npi2,ibonus;
693 double xi1[3],xi2[3],vti[3],h[3],fn[3],ft[3],d,t;
694 double delx,dely,delz,rsq,rij,rsqinv,R,fx,fy,fz,fpair,energy;
695 double rradi,rradj,contact_dist;
696 double vr1,vr2,vr3,vnnr,vn1,vn2,vn3,vt1,vt2,vt3;
697 double *quat, *inertia;
698 AtomVecBody::Bonus *bonus;
699
700 int nlocal = atom->nlocal;
701 int newton_pair = force->newton_pair;
702
703 ifirst = dfirst[ibody];
704 iefirst = edfirst[ibody];
705 nei = ednum[ibody];
706
707 rradi = rounded_radius[ibody];
708 rradj = rounded_radius[jbody];
709 contact_dist = rradi + rradj;
710
711 for (ni = 0; ni < nei; ni++) {
712
713 npi1 = static_cast<int>(edge[iefirst+ni][0]);
714 npi2 = static_cast<int>(edge[iefirst+ni][1]);
715
716 // compute the space-fixed coordinates for the vertices of the face
717
718 xi1[0] = x[ibody][0] + discrete[ifirst+npi1][0];
719 xi1[1] = x[ibody][1] + discrete[ifirst+npi1][1];
720 xi1[2] = x[ibody][2] + discrete[ifirst+npi1][2];
721
722 xi2[0] = x[ibody][0] + discrete[ifirst+npi2][0];
723 xi2[1] = x[ibody][1] + discrete[ifirst+npi2][1];
724 xi2[2] = x[ibody][2] + discrete[ifirst+npi2][2];
725
726 // find the projection of the jbody's COM on the edge
727
728 project_pt_line(x[jbody], xi1, xi2, h, d, t);
729
730 if (d > contact_dist + cut_inner) continue;
731 if (t < 0 || t > 1) continue;
732
733 if (fabs(t) < EPSILON) {
734 if (static_cast<int>(discrete[ifirst+npi1][6]) == 1)
735 continue;
736 else {
737 h[0] = xi1[0];
738 h[1] = xi1[1];
739 h[2] = xi1[2];
740 discrete[ifirst+npi1][6] = 1;
741 }
742 }
743
744 if (fabs(t-1) < EPSILON) {
745 if (static_cast<int>(discrete[ifirst+npi2][6]) == 1)
746 continue;
747 else {
748 h[0] = xi2[0];
749 h[1] = xi2[1];
750 h[2] = xi2[2];
751 discrete[ifirst+npi2][6] = 1;
752 }
753 }
754
755 delx = h[0] - x[jbody][0];
756 dely = h[1] - x[jbody][1];
757 delz = h[2] - x[jbody][2];
758 rsq = delx*delx + dely*dely + delz*delz;
759 rsqinv = (rsq == 0.0) ? 0.0 : 1.0/rsq;
760 rij = sqrt(rsq);
761 R = rij - contact_dist;
762
763 energy = 0;
764 kernel_force(R, itype, jtype, energy, fpair);
765
766 fx = delx*fpair/rij;
767 fy = dely*fpair/rij;
768 fz = delz*fpair/rij;
769
770 if (R <= 0) { // in contact
771
772 // compute the velocity of the vertex in the space-fixed frame
773
774 ibonus = atom->body[ibody];
775 bonus = &avec->bonus[ibonus];
776 quat = bonus->quat;
777 inertia = bonus->inertia;
778 total_velocity(h, x[ibody], v[ibody], angmom[ibody],
779 inertia, quat, vti);
780
781 // relative translational velocity
782
783 vr1 = vti[0] - v[jbody][0];
784 vr2 = vti[1] - v[jbody][1];
785 vr3 = vti[2] - v[jbody][2];
786
787 // normal component
788
789 vnnr = vr1*delx + vr2*dely + vr3*delz;
790 vn1 = delx*vnnr * rsqinv;
791 vn2 = dely*vnnr * rsqinv;
792 vn3 = delz*vnnr * rsqinv;
793
794 // tangential component
795
796 vt1 = vr1 - vn1;
797 vt2 = vr2 - vn2;
798 vt3 = vr3 - vn3;
799
800 // normal friction term at contact
801
802 fn[0] = -c_n * vn1;
803 fn[1] = -c_n * vn2;
804 fn[2] = -c_n * vn3;
805
806 // tangential friction term at contact,
807 // excluding the tangential deformation term
808
809 ft[0] = -c_t * vt1;
810 ft[1] = -c_t * vt2;
811 ft[2] = -c_t * vt3;
812
813 fx += fn[0] + ft[0];
814 fy += fn[1] + ft[1];
815 fz += fn[2] + ft[2];
816 }
817
818 f[ibody][0] += fx;
819 f[ibody][1] += fy;
820 f[ibody][2] += fz;
821 sum_torque(x[ibody], h, fx, fy, fz, torque[ibody]);
822
823 if (newton_pair || jbody < nlocal) {
824 f[jbody][0] -= fx;
825 f[jbody][1] -= fy;
826 f[jbody][2] -= fz;
827 }
828
829 if (evflag) ev_tally_xyz(ibody,jbody,nlocal,newton_pair,
830 energy,0.0,fx,fy,fz,delx,dely,delz);
831 }
832 }
833
834 /* ----------------------------------------------------------------------
835 Interaction bt the faces of a polyhedron (ibody) and a sphere (jbody)
836 ---------------------------------------------------------------------- */
837
sphere_against_face(int ibody,int jbody,int itype,int jtype,double ** x,double ** v,double ** f,double ** torque,double ** angmom,int evflag)838 void PairBodyRoundedPolyhedron::sphere_against_face(int ibody, int jbody,
839 int itype, int jtype, double** x, double** v, double** f, double** torque,
840 double** angmom, int evflag)
841 {
842 int ni,nfi,inside,ifirst,iffirst,npi1,npi2,npi3,ibonus,tmp;
843 double xi1[3],xi2[3],xi3[3],ui[3],vi[3],vti[3],n[3],h[3],fn[3],ft[3],d;
844 double delx,dely,delz,rsq,rij,rsqinv,R,fx,fy,fz,fpair,energy;
845 double rradi,rradj,contact_dist;
846 double vr1,vr2,vr3,vnnr,vn1,vn2,vn3,vt1,vt2,vt3;
847 double *quat, *inertia;
848 AtomVecBody::Bonus *bonus;
849
850 int nlocal = atom->nlocal;
851 int newton_pair = force->newton_pair;
852
853 ifirst = dfirst[ibody];
854 iffirst = facfirst[ibody];
855 nfi = facnum[ibody];
856
857 rradi = rounded_radius[ibody];
858 rradj = rounded_radius[jbody];
859 contact_dist = rradi + rradj;
860
861 for (ni = 0; ni < nfi; ni++) {
862
863 npi1 = static_cast<int>(face[iffirst+ni][0]);
864 npi2 = static_cast<int>(face[iffirst+ni][1]);
865 npi3 = static_cast<int>(face[iffirst+ni][2]);
866
867 // compute the space-fixed coordinates for the vertices of the face
868
869 xi1[0] = x[ibody][0] + discrete[ifirst+npi1][0];
870 xi1[1] = x[ibody][1] + discrete[ifirst+npi1][1];
871 xi1[2] = x[ibody][2] + discrete[ifirst+npi1][2];
872
873 xi2[0] = x[ibody][0] + discrete[ifirst+npi2][0];
874 xi2[1] = x[ibody][1] + discrete[ifirst+npi2][1];
875 xi2[2] = x[ibody][2] + discrete[ifirst+npi2][2];
876
877 xi3[0] = x[ibody][0] + discrete[ifirst+npi3][0];
878 xi3[1] = x[ibody][1] + discrete[ifirst+npi3][1];
879 xi3[2] = x[ibody][2] + discrete[ifirst+npi3][2];
880
881 // find the normal unit vector of the face
882
883 MathExtra::sub3(xi2, xi1, ui);
884 MathExtra::sub3(xi3, xi1, vi);
885 MathExtra::cross3(ui, vi, n);
886 MathExtra::norm3(n);
887
888 // skip if the COM of the two bodies are in the same side of the face
889
890 if (opposite_sides(n, xi1, x[ibody], x[jbody]) == 0) continue;
891
892 // find the projection of the sphere on the face
893
894 project_pt_plane(x[jbody], xi1, xi2, xi3, h, d, inside);
895
896 inside_polygon(ibody, ni, x[ibody], h, nullptr, inside, tmp);
897 if (inside == 0) continue;
898
899 delx = h[0] - x[jbody][0];
900 dely = h[1] - x[jbody][1];
901 delz = h[2] - x[jbody][2];
902 rsq = delx*delx + dely*dely + delz*delz;
903 rij = sqrt(rsq);
904 R = rij - contact_dist;
905
906 energy = 0;
907 kernel_force(R, itype, jtype, energy, fpair);
908
909 fx = delx*fpair/rij;
910 fy = dely*fpair/rij;
911 fz = delz*fpair/rij;
912
913 if (R <= 0) { // in contact
914
915 // compute the velocity of the vertex in the space-fixed frame
916
917 ibonus = atom->body[ibody];
918 bonus = &avec->bonus[ibonus];
919 quat = bonus->quat;
920 inertia = bonus->inertia;
921 total_velocity(h, x[ibody], v[ibody], angmom[ibody],
922 inertia, quat, vti);
923
924 // relative translational velocity
925
926 vr1 = vti[0] - v[jbody][0];
927 vr2 = vti[1] - v[jbody][1];
928 vr3 = vti[2] - v[jbody][2];
929
930 // normal component
931
932 rsqinv = 1.0/rsq;
933 vnnr = vr1*delx + vr2*dely + vr3*delz;
934 vn1 = delx*vnnr * rsqinv;
935 vn2 = dely*vnnr * rsqinv;
936 vn3 = delz*vnnr * rsqinv;
937
938 // tangential component
939
940 vt1 = vr1 - vn1;
941 vt2 = vr2 - vn2;
942 vt3 = vr3 - vn3;
943
944 // normal friction term at contact
945
946 fn[0] = -c_n * vn1;
947 fn[1] = -c_n * vn2;
948 fn[2] = -c_n * vn3;
949
950 // tangential friction term at contact,
951 // excluding the tangential deformation term for now
952
953 ft[0] = -c_t * vt1;
954 ft[1] = -c_t * vt2;
955 ft[2] = -c_t * vt3;
956
957 fx += fn[0] + ft[0];
958 fy += fn[1] + ft[1];
959 fz += fn[2] + ft[2];
960 }
961
962 f[ibody][0] += fx;
963 f[ibody][1] += fy;
964 f[ibody][2] += fz;
965 sum_torque(x[ibody], h, fx, fy, fz, torque[ibody]);
966
967 if (newton_pair || jbody < nlocal) {
968 f[jbody][0] -= fx;
969 f[jbody][1] -= fy;
970 f[jbody][2] -= fz;
971 }
972
973 if (evflag) ev_tally_xyz(ibody,jbody,nlocal,newton_pair,
974 energy,0.0,fx,fy,fz,delx,dely,delz);
975 }
976 }
977
978 /* ----------------------------------------------------------------------
979 Determine the interaction mode between i's edges against j's edges
980
981 i = atom i (body i)
982 j = atom j (body j)
983 x = atoms' coordinates
984 f = atoms' forces
985 torque = atoms' torques
986 tag = atoms' tags
987 contact_list = list of contacts
988 num_contacts = number of contacts between i's edges and j's edges
989 Return:
990
991 ---------------------------------------------------------------------- */
992
edge_against_edge(int ibody,int jbody,int itype,int jtype,double ** x,Contact * contact_list,int & num_contacts,double & evdwl,double * facc)993 int PairBodyRoundedPolyhedron::edge_against_edge(int ibody, int jbody,
994 int itype, int jtype, double** x, Contact* contact_list, int &num_contacts,
995 double &evdwl, double* facc)
996 {
997 int ni,nei,nj,nej,interact;
998 double rradi,rradj,energy;
999
1000 nei = ednum[ibody];
1001 rradi = rounded_radius[ibody];
1002 nej = ednum[jbody];
1003 rradj = rounded_radius[jbody];
1004
1005 energy = 0;
1006 interact = EE_NONE;
1007
1008 // loop through body i's edges
1009
1010 for (ni = 0; ni < nei; ni++) {
1011
1012 for (nj = 0; nj < nej; nj++) {
1013
1014 // compute the distance between the edge nj to the edge ni
1015 #ifdef _POLYHEDRON_DEBUG
1016 printf("Compute interaction between edge %d of body %d "
1017 "with edge %d of body %d:\n",
1018 nj, jbody, ni, ibody);
1019 #endif
1020
1021 interact = interaction_edge_to_edge(ibody, ni, x[ibody], rradi,
1022 jbody, nj, x[jbody], rradj,
1023 itype, jtype, cut_inner,
1024 contact_list, num_contacts,
1025 energy, facc);
1026 }
1027
1028 } // end for looping through the edges of body i
1029
1030 evdwl += energy;
1031
1032 return interact;
1033 }
1034
1035 /* ----------------------------------------------------------------------
1036 Determine the interaction mode between i's edges against j's faces
1037
1038 i = atom i (body i)
1039 j = atom j (body j)
1040 x = atoms' coordinates
1041 f = atoms' forces
1042 torque = atoms' torques
1043 tag = atoms' tags
1044 contact_list = list of contacts
1045 num_contacts = number of contacts between i's edges and j's faces
1046 Return:
1047
1048 ---------------------------------------------------------------------- */
1049
edge_against_face(int ibody,int jbody,int itype,int jtype,double ** x,Contact * contact_list,int & num_contacts,double & evdwl,double * facc)1050 int PairBodyRoundedPolyhedron::edge_against_face(int ibody, int jbody,
1051 int itype, int jtype, double** x, Contact* contact_list, int &num_contacts,
1052 double &evdwl, double* facc)
1053 {
1054 int ni,nei,nj,nfj,interact;
1055 double rradi,rradj,energy;
1056
1057 nei = ednum[ibody];
1058 rradi = rounded_radius[ibody];
1059 nfj = facnum[jbody];
1060 rradj = rounded_radius[jbody];
1061
1062 energy = 0;
1063 interact = EF_NONE;
1064
1065 // loop through body i's edges
1066
1067 for (ni = 0; ni < nei; ni++) {
1068
1069 // loop through body j's faces
1070
1071 for (nj = 0; nj < nfj; nj++) {
1072
1073 // compute the distance between the face nj to the edge ni
1074 #ifdef _POLYHEDRON_DEBUG
1075 printf("Compute interaction between face %d of body %d with "
1076 "edge %d of body %d:\n",
1077 nj, jbody, ni, ibody);
1078 #endif
1079
1080 interact = interaction_face_to_edge(jbody, nj, x[jbody], rradj,
1081 ibody, ni, x[ibody], rradi,
1082 itype, jtype, cut_inner,
1083 contact_list, num_contacts,
1084 energy, facc);
1085 }
1086
1087 } // end for looping through the edges of body i
1088
1089 evdwl += energy;
1090
1091 return interact;
1092 }
1093
1094 /* -------------------------------------------------------------------------
1095 Compute the distance between an edge of body i and an edge from
1096 another body
1097 Input:
1098 ibody = body i (i.e. atom i)
1099 face_index = face index of body i
1100 xmi = atom i's coordinates (body i's center of mass)
1101 rounded_radius_i = rounded radius of the body i
1102 jbody = body i (i.e. atom j)
1103 edge_index = coordinate of the tested edge from another body
1104 xmj = atom j's coordinates (body j's center of mass)
1105 rounded_radius_j = rounded radius of the body j
1106 cut_inner = cutoff for vertex-vertex and vertex-edge interaction
1107 Output:
1108 d = Distance from a point x0 to an edge
1109 hi = coordinates of the projection of x0 on the edge
1110
1111 contact = 0 no contact between the queried edge and the face
1112 1 contact detected
1113 return
1114 INVALID if the face index is invalid
1115 NONE if there is no interaction
1116 ------------------------------------------------------------------------- */
1117
interaction_edge_to_edge(int ibody,int edge_index_i,double * xmi,double rounded_radius_i,int jbody,int edge_index_j,double * xmj,double rounded_radius_j,int itype,int jtype,double cut_inner,Contact * contact_list,int & num_contacts,double & energy,double * facc)1118 int PairBodyRoundedPolyhedron::interaction_edge_to_edge(int ibody,
1119 int edge_index_i, double *xmi, double rounded_radius_i,
1120 int jbody, int edge_index_j, double *xmj, double rounded_radius_j,
1121 int itype, int jtype, double cut_inner,
1122 Contact* contact_list, int &num_contacts, double &energy, double* facc)
1123 {
1124 int ifirst,iefirst,jfirst,jefirst,npi1,npi2,npj1,npj2,interact;
1125 double xi1[3],xi2[3],xpj1[3],xpj2[3];
1126 double r,t1,t2,h1[3],h2[3];
1127 double contact_dist;
1128
1129 double** x = atom->x;
1130 double** v = atom->v;
1131 double** f = atom->f;
1132 double** torque = atom->torque;
1133 double** angmom = atom->angmom;
1134
1135 ifirst = dfirst[ibody];
1136 iefirst = edfirst[ibody];
1137 npi1 = static_cast<int>(edge[iefirst+edge_index_i][0]);
1138 npi2 = static_cast<int>(edge[iefirst+edge_index_i][1]);
1139
1140 // compute the space-fixed coordinates for the edge ends
1141
1142 xi1[0] = xmi[0] + discrete[ifirst+npi1][0];
1143 xi1[1] = xmi[1] + discrete[ifirst+npi1][1];
1144 xi1[2] = xmi[2] + discrete[ifirst+npi1][2];
1145
1146 xi2[0] = xmi[0] + discrete[ifirst+npi2][0];
1147 xi2[1] = xmi[1] + discrete[ifirst+npi2][1];
1148 xi2[2] = xmi[2] + discrete[ifirst+npi2][2];
1149
1150 // two ends of the edge from body j
1151
1152 jfirst = dfirst[jbody];
1153 jefirst = edfirst[jbody];
1154 npj1 = static_cast<int>(edge[jefirst+edge_index_j][0]);
1155 npj2 = static_cast<int>(edge[jefirst+edge_index_j][1]);
1156
1157 xpj1[0] = xmj[0] + discrete[jfirst+npj1][0];
1158 xpj1[1] = xmj[1] + discrete[jfirst+npj1][1];
1159 xpj1[2] = xmj[2] + discrete[jfirst+npj1][2];
1160
1161 xpj2[0] = xmj[0] + discrete[jfirst+npj2][0];
1162 xpj2[1] = xmj[1] + discrete[jfirst+npj2][1];
1163 xpj2[2] = xmj[2] + discrete[jfirst+npj2][2];
1164
1165 contact_dist = rounded_radius_i + rounded_radius_j;
1166
1167 int jflag = 1;
1168 distance_bt_edges(xpj1, xpj2, xi1, xi2, h1, h2, t1, t2, r);
1169
1170 #ifdef _POLYHEDRON_DEBUG
1171 double ui[3],uj[3];
1172 MathExtra::sub3(xi1,xi2,ui);
1173 MathExtra::norm3(ui);
1174 MathExtra::sub3(xpj1,xpj2,uj);
1175 MathExtra::norm3(uj);
1176 double dot = MathExtra::dot3(ui, uj);
1177 printf(" edge npi1 = %d (%f %f %f); npi2 = %d (%f %f %f) vs."
1178 " edge npj1 = %d (%f %f %f); npj2 = %d (%f %f %f): "
1179 "t1 = %f; t2 = %f; r = %f; dot = %f\n",
1180 npi1, xi1[0], xi1[1], xi1[2], npi2, xi2[0], xi2[1], xi2[2],
1181 npj1, xpj1[0], xpj1[1], xpj1[2], npj2, xpj2[0], xpj2[1], xpj2[2],
1182 t1, t2, r, dot);
1183 #endif
1184
1185 interact = EE_NONE;
1186
1187 // singularity case, ignore interactions
1188
1189 if (r < EPSILON) return interact;
1190
1191 // include the vertices for interactions
1192
1193 if (t1 >= 0 && t1 <= 1 && t2 >= 0 && t2 <= 1 &&
1194 r < contact_dist + cut_inner) {
1195 pair_force_and_torque(jbody, ibody, h1, h2, r, contact_dist,
1196 jtype, itype, x, v, f, torque, angmom,
1197 jflag, energy, facc);
1198
1199 interact = EE_INTERACT;
1200 if (r <= contact_dist) {
1201 // store the contact info
1202 contact_list[num_contacts].ibody = ibody;
1203 contact_list[num_contacts].jbody = jbody;
1204 contact_list[num_contacts].xi[0] = h2[0];
1205 contact_list[num_contacts].xi[1] = h2[1];
1206 contact_list[num_contacts].xi[2] = h2[2];
1207 contact_list[num_contacts].xj[0] = h1[0];
1208 contact_list[num_contacts].xj[1] = h1[1];
1209 contact_list[num_contacts].xj[2] = h1[2];
1210 contact_list[num_contacts].type = 1;
1211 contact_list[num_contacts].separation = r - contact_dist;
1212 contact_list[num_contacts].unique = 1;
1213 num_contacts++;
1214 }
1215 }
1216 return interact;
1217 }
1218
1219 /* -------------------------------------------------------------------------
1220 Compute the interaction between a face of body i and an edge from
1221 another body
1222 Input:
1223 ibody = body i (i.e. atom i)
1224 face_index = face index of body i
1225 xmi = atom i's coordinates (body i's center of mass)
1226 rounded_radius_i = rounded radius of the body i
1227 jbody = body i (i.e. atom j)
1228 edge_index = coordinate of the tested edge from another body
1229 xmj = atom j's coordinates (body j's center of mass)
1230 rounded_radius_j = rounded radius of the body j
1231 cut_inner = cutoff for vertex-vertex and vertex-edge interaction
1232 Output:
1233 d = Distance from a point x0 to an edge
1234 hi = coordinates of the projection of x0 on the edge
1235
1236 contact = 0 no contact between the queried edge and the face
1237 1 contact detected
1238 return
1239 INVALID if the face index is invalid
1240 NONE if there is no interaction
1241 ------------------------------------------------------------------------- */
1242
interaction_face_to_edge(int ibody,int face_index,double * xmi,double rounded_radius_i,int jbody,int edge_index,double * xmj,double rounded_radius_j,int itype,int jtype,double cut_inner,Contact * contact_list,int & num_contacts,double & energy,double * facc)1243 int PairBodyRoundedPolyhedron::interaction_face_to_edge(int ibody,
1244 int face_index, double *xmi, double rounded_radius_i,
1245 int jbody, int edge_index, double *xmj, double rounded_radius_j,
1246 int itype, int jtype, double cut_inner,
1247 Contact* contact_list, int &num_contacts, double &energy, double* facc)
1248 {
1249 if (face_index >= facnum[ibody]) return EF_INVALID;
1250
1251 int ifirst,iffirst,jfirst,npi1,npi2,npi3;
1252 int jefirst,npj1,npj2;
1253 double xi1[3],xi2[3],xi3[3],xpj1[3],xpj2[3],ui[3],vi[3],n[3];
1254
1255 double** x = atom->x;
1256 double** v = atom->v;
1257 double** f = atom->f;
1258 double** torque = atom->torque;
1259 double** angmom = atom->angmom;
1260
1261 ifirst = dfirst[ibody];
1262 iffirst = facfirst[ibody];
1263 npi1 = static_cast<int>(face[iffirst+face_index][0]);
1264 npi2 = static_cast<int>(face[iffirst+face_index][1]);
1265 npi3 = static_cast<int>(face[iffirst+face_index][2]);
1266
1267 // compute the space-fixed coordinates for the vertices of the face
1268
1269 xi1[0] = xmi[0] + discrete[ifirst+npi1][0];
1270 xi1[1] = xmi[1] + discrete[ifirst+npi1][1];
1271 xi1[2] = xmi[2] + discrete[ifirst+npi1][2];
1272
1273 xi2[0] = xmi[0] + discrete[ifirst+npi2][0];
1274 xi2[1] = xmi[1] + discrete[ifirst+npi2][1];
1275 xi2[2] = xmi[2] + discrete[ifirst+npi2][2];
1276
1277 xi3[0] = xmi[0] + discrete[ifirst+npi3][0];
1278 xi3[1] = xmi[1] + discrete[ifirst+npi3][1];
1279 xi3[2] = xmi[2] + discrete[ifirst+npi3][2];
1280
1281 // find the normal unit vector of the face, ensure it point outward of the body
1282
1283 MathExtra::sub3(xi2, xi1, ui);
1284 MathExtra::sub3(xi3, xi1, vi);
1285 MathExtra::cross3(ui, vi, n);
1286 MathExtra::norm3(n);
1287
1288 double xc[3], dot, ans[3];
1289 xc[0] = (xi1[0] + xi2[0] + xi3[0])/3.0;
1290 xc[1] = (xi1[1] + xi2[1] + xi3[1])/3.0;
1291 xc[2] = (xi1[2] + xi2[2] + xi3[2])/3.0;
1292 MathExtra::sub3(xc, xmi, ans);
1293 dot = MathExtra::dot3(ans, n);
1294 if (dot < 0) MathExtra::negate3(n);
1295
1296 // two ends of the edge from body j
1297
1298 jfirst = dfirst[jbody];
1299 jefirst = edfirst[jbody];
1300 npj1 = static_cast<int>(edge[jefirst+edge_index][0]);
1301 npj2 = static_cast<int>(edge[jefirst+edge_index][1]);
1302
1303 xpj1[0] = xmj[0] + discrete[jfirst+npj1][0];
1304 xpj1[1] = xmj[1] + discrete[jfirst+npj1][1];
1305 xpj1[2] = xmj[2] + discrete[jfirst+npj1][2];
1306
1307 xpj2[0] = xmj[0] + discrete[jfirst+npj2][0];
1308 xpj2[1] = xmj[1] + discrete[jfirst+npj2][1];
1309 xpj2[2] = xmj[2] + discrete[jfirst+npj2][2];
1310
1311 // no interaction if two ends of the edge
1312 // are on the same side with the COM wrt the face
1313
1314 if (opposite_sides(n, xi1, xmi, xpj1) == 0 &&
1315 opposite_sides(n, xi1, xmi, xpj2) == 0)
1316 return EF_NONE;
1317
1318 // determine the intersection of the edge to the face
1319
1320 double hi1[3], hi2[3], d1, d2, contact_dist;
1321 int inside1 = 0;
1322 int inside2 = 0;
1323
1324 // enum {EF_PARALLEL=0,EF_SAME_SIDE_OF_FACE,
1325 // EF_INTERSECT_INSIDE,EF_INTERSECT_OUTSIDE};
1326
1327 int interact = edge_face_intersect(xi1, xi2, xi3, xpj1, xpj2,
1328 hi1, hi2, d1, d2, inside1, inside2);
1329
1330 inside_polygon(ibody, face_index, xmi, hi1, hi2, inside1, inside2);
1331
1332 contact_dist = rounded_radius_i + rounded_radius_j;
1333
1334 // both endpoints are on the same side of, or parallel to, the face
1335 // and both are out of the interaction zone
1336
1337 if (interact == EF_SAME_SIDE_OF_FACE || interact == EF_PARALLEL) {
1338
1339 if (d1 > contact_dist + cut_inner && d2 > contact_dist + cut_inner)
1340 return EF_NONE;
1341
1342 int num_outside = 0;
1343 int jflag = 1;
1344
1345 #ifdef _POLYHEDRON_DEBUG
1346 if (interact == EF_SAME_SIDE_OF_FACE)
1347 printf(" - same side of face\n");
1348 else if (interact == EF_PARALLEL)
1349 printf(" - parallel\n");
1350 printf(" face: xi1 (%f %f %f) xi2 (%f %f %f) xi3 (%f %f %f)\n",
1351 xi1[0], xi1[1], xi1[2], xi2[0], xi2[1], xi2[2], xi3[0], xi3[1], xi3[2]);
1352 printf(" edge: xpj1 (%f %f %f) xpj2 (%f %f %f)\n",
1353 xpj1[0], xpj1[1], xpj1[2], xpj2[0], xpj2[1], xpj2[2]);
1354 #endif
1355
1356 // xpj1 is in the interaction zone
1357 // and its projection on the face is inside the triangle
1358 // compute vertex-face interaction and accumulate force/torque to both bodies
1359
1360 if (d1 <= contact_dist + cut_inner) {
1361 if (inside1) {
1362 if (static_cast<int>(discrete[jfirst+npj1][6]) == 0) {
1363 pair_force_and_torque(jbody, ibody, xpj1, hi1, d1, contact_dist,
1364 jtype, itype, x, v, f, torque, angmom,
1365 jflag, energy, facc);
1366 #ifdef _POLYHEDRON_DEBUG
1367 printf(" - compute pair force between vertex %d from edge %d of body %d "
1368 "with face %d of body %d: d1 = %f\n",
1369 npj1, edge_index, jbody, face_index, ibody, d1);
1370 #endif
1371
1372 if (d1 <= contact_dist) {
1373 // store the contact info
1374 contact_list[num_contacts].ibody = ibody;
1375 contact_list[num_contacts].jbody = jbody;
1376 contact_list[num_contacts].xi[0] = hi1[0];
1377 contact_list[num_contacts].xi[1] = hi1[1];
1378 contact_list[num_contacts].xi[2] = hi1[2];
1379 contact_list[num_contacts].xj[0] = xpj1[0];
1380 contact_list[num_contacts].xj[1] = xpj1[1];
1381 contact_list[num_contacts].xj[2] = xpj1[2];
1382 contact_list[num_contacts].type = 0;
1383 contact_list[num_contacts].separation = d1 - contact_dist;
1384 contact_list[num_contacts].unique = 1;
1385 num_contacts++;
1386 }
1387
1388 discrete[jfirst+npj1][6] = 1;
1389 }
1390 } else {
1391 num_outside++;
1392 }
1393 }
1394
1395 // xpj2 is in the interaction zone
1396 // and its projection on the face is inside the triangle
1397 // compute vertex-face interaction and accumulate force/torque to both bodies
1398
1399 if (d2 <= contact_dist + cut_inner) {
1400 if (inside2) {
1401 if (static_cast<int>(discrete[jfirst+npj2][6]) == 0) {
1402 pair_force_and_torque(jbody, ibody, xpj2, hi2, d2, contact_dist,
1403 jtype, itype, x, v, f, torque, angmom,
1404 jflag, energy, facc);
1405 #ifdef _POLYHEDRON_DEBUG
1406 printf(" - compute pair force between vertex %d from edge %d of body %d "
1407 "with face %d of body %d: d2 = %f\n",
1408 npj2, edge_index, jbody, face_index, ibody, d2);
1409 #endif
1410
1411 if (d2 <= contact_dist) {
1412 // store the contact info
1413 contact_list[num_contacts].ibody = ibody;
1414 contact_list[num_contacts].jbody = jbody;
1415 contact_list[num_contacts].xi[0] = hi2[0];
1416 contact_list[num_contacts].xi[1] = hi2[1];
1417 contact_list[num_contacts].xi[2] = hi2[2];
1418 contact_list[num_contacts].xj[0] = xpj2[0];
1419 contact_list[num_contacts].xj[1] = xpj2[1];
1420 contact_list[num_contacts].xj[2] = xpj2[2];
1421 contact_list[num_contacts].type = 0;
1422 contact_list[num_contacts].separation = d2 - contact_dist;
1423 contact_list[num_contacts].unique = 1;
1424 num_contacts++;
1425 }
1426 discrete[jfirst+npj2][6] = 1;
1427 }
1428 } else {
1429 num_outside++;
1430 }
1431 }
1432
1433 // both ends have projection outside of the face
1434 // compute interaction between the edge with the three edges of the face
1435
1436 if (num_outside == 2) {
1437
1438 #ifdef _POLYHEDRON_DEBUG
1439 printf(" - outside = 2\n");
1440 printf(" - compute pair force between edge %d of body %d "
1441 "with 3 edges of face %d of body %d\n",
1442 edge_index, jbody, face_index, ibody);
1443 #endif
1444
1445 interact = EF_INTERSECT_OUTSIDE;
1446
1447 }
1448
1449 } else if (interact == EF_INTERSECT_OUTSIDE) {
1450
1451 // compute interaction between the edge with the three edges of the face
1452
1453 #ifdef _POLYHEDRON_DEBUG
1454 printf(" - intersect outside triangle\n");
1455 printf(" - compute pair force between edge %d of body %d "
1456 "with face %d of body %d\n", edge_index, jbody, face_index, ibody);
1457 printf(" face: xi1 (%f %f %f) xi2 (%f %f %f) xi3 (%f %f %f)\n",
1458 xi1[0], xi1[1], xi1[2], xi2[0], xi2[1], xi2[2], xi3[0], xi3[1], xi3[2]);
1459 printf(" edge: xpj1 (%f %f %f) xpj2 (%f %f %f)\n",
1460 xpj1[0], xpj1[1], xpj1[2], xpj2[0], xpj2[1], xpj2[2]);
1461
1462 #endif
1463 } else if (interact == EF_INTERSECT_INSIDE) {
1464 // need to do something here to resolve overlap!!
1465 // p is the intersection between the edge and the face
1466 int jflag = 1;
1467 if (d1 < d2)
1468 pair_force_and_torque(jbody, ibody, xpj1, hi1, d1, contact_dist,
1469 jtype, itype, x, v, f, torque, angmom,
1470 jflag, energy, facc);
1471 else
1472 pair_force_and_torque(jbody, ibody, xpj2, hi2, d2, contact_dist,
1473 jtype, itype, x, v, f, torque, angmom,
1474 jflag, energy, facc);
1475 }
1476
1477 return interact;
1478 }
1479
1480 /* ----------------------------------------------------------------------
1481 Compute forces and torques between two bodies caused by the interaction
1482 between a pair of points on either bodies (similar to sphere-sphere)
1483 ------------------------------------------------------------------------- */
1484
pair_force_and_torque(int ibody,int jbody,double * pi,double * pj,double r,double contact_dist,int itype,int jtype,double ** x,double ** v,double ** f,double ** torque,double ** angmom,int jflag,double & energy,double * facc)1485 void PairBodyRoundedPolyhedron::pair_force_and_torque(int ibody, int jbody,
1486 double* pi, double* pj, double r, double contact_dist,
1487 int itype, int jtype, double** x,
1488 double** v, double** f, double** torque, double** angmom,
1489 int jflag, double& energy, double* facc)
1490 {
1491 double delx,dely,delz,R,fx,fy,fz,fpair;
1492
1493 delx = pi[0] - pj[0];
1494 dely = pi[1] - pj[1];
1495 delz = pi[2] - pj[2];
1496 R = r - contact_dist;
1497
1498 kernel_force(R, itype, jtype, energy, fpair);
1499
1500 fx = delx*fpair/r;
1501 fy = dely*fpair/r;
1502 fz = delz*fpair/r;
1503
1504 #ifdef _POLYHEDRON_DEBUG
1505 printf(" - R = %f; r = %f; k_na = %f; shift = %f; fpair = %f;"
1506 " energy = %f; jflag = %d\n", R, r, k_na, shift, fpair,
1507 energy, jflag);
1508 #endif
1509
1510 if (R <= 0) {
1511
1512 // contact: accumulate normal and tangential contact force components
1513
1514 contact_forces(ibody, jbody, pi, pj, delx, dely, delz, fx, fy, fz,
1515 x, v, angmom, f, torque, facc);
1516 } else {
1517
1518 // accumulate force and torque to both bodies directly
1519
1520 f[ibody][0] += fx;
1521 f[ibody][1] += fy;
1522 f[ibody][2] += fz;
1523 sum_torque(x[ibody], pi, fx, fy, fz, torque[ibody]);
1524
1525 facc[0] += fx; facc[1] += fy; facc[2] += fz;
1526
1527 if (jflag) {
1528 f[jbody][0] -= fx;
1529 f[jbody][1] -= fy;
1530 f[jbody][2] -= fz;
1531 sum_torque(x[jbody], pj, -fx, -fy, -fz, torque[jbody]);
1532 }
1533 }
1534 }
1535
1536 /* ----------------------------------------------------------------------
1537 Kernel force is model-dependent and can be derived for other styles
1538 here is the harmonic potential (linear piece-wise forces) in Wang et al.
1539 ------------------------------------------------------------------------- */
1540
kernel_force(double R,int itype,int jtype,double & energy,double & fpair)1541 void PairBodyRoundedPolyhedron::kernel_force(double R, int itype, int jtype,
1542 double& energy, double& fpair)
1543 {
1544 double kn = k_n[itype][jtype];
1545 double kna = k_na[itype][jtype];
1546 double shift = kna * cut_inner;
1547 double e = 0;
1548 if (R <= 0) { // deformation occurs
1549 fpair = -kn * R - shift;
1550 e = (0.5 * kn * R + shift) * R;
1551 } else if (R <= cut_inner) { // not deforming but cohesive ranges overlap
1552 fpair = kna * R - shift;
1553 e = (-0.5 * kna * R + shift) * R;
1554 } else fpair = 0.0;
1555 energy += e;
1556 }
1557
1558 /* ----------------------------------------------------------------------
1559 Compute contact forces between two bodies
1560 modify the force stored at the vertex and edge in contact by j_a
1561 sum forces and torque to the corresponding bodies
1562 fx,fy,fz = unscaled cohesive forces
1563 fn = normal friction component
1564 ft = tangential friction component (-c_t * v_t)
1565 ------------------------------------------------------------------------- */
1566
contact_forces(int ibody,int jbody,double * xi,double * xj,double delx,double dely,double delz,double fx,double fy,double fz,double ** x,double ** v,double ** angmom,double ** f,double ** torque,double * facc)1567 void PairBodyRoundedPolyhedron::contact_forces(int ibody, int jbody,
1568 double *xi, double *xj, double delx, double dely, double delz,
1569 double fx, double fy, double fz, double** x, double** v, double** angmom,
1570 double** f, double** torque, double* facc)
1571 {
1572 int ibonus,jbonus;
1573 double rsq,rsqinv,vr1,vr2,vr3,vnnr,vn1,vn2,vn3,vt1,vt2,vt3;
1574 double fn[3],ft[3],vi[3],vj[3];
1575 double *quat, *inertia;
1576 AtomVecBody::Bonus *bonus;
1577
1578 // compute the velocity of the vertex in the space-fixed frame
1579
1580 ibonus = atom->body[ibody];
1581 bonus = &avec->bonus[ibonus];
1582 quat = bonus->quat;
1583 inertia = bonus->inertia;
1584 total_velocity(xi, x[ibody], v[ibody], angmom[ibody],
1585 inertia, quat, vi);
1586
1587 // compute the velocity of the point on the edge in the space-fixed frame
1588
1589 jbonus = atom->body[jbody];
1590 bonus = &avec->bonus[jbonus];
1591 quat = bonus->quat;
1592 inertia = bonus->inertia;
1593 total_velocity(xj, x[jbody], v[jbody], angmom[jbody],
1594 inertia, quat, vj);
1595
1596 // vector pointing from the contact point on ibody to that on jbody
1597
1598 rsq = delx*delx + dely*dely + delz*delz;
1599 rsqinv = 1.0/rsq;
1600
1601 // relative translational velocity
1602
1603 vr1 = vi[0] - vj[0];
1604 vr2 = vi[1] - vj[1];
1605 vr3 = vi[2] - vj[2];
1606
1607 // normal component
1608
1609 vnnr = vr1*delx + vr2*dely + vr3*delz;
1610 vn1 = delx*vnnr * rsqinv;
1611 vn2 = dely*vnnr * rsqinv;
1612 vn3 = delz*vnnr * rsqinv;
1613
1614 // tangential component
1615
1616 vt1 = vr1 - vn1;
1617 vt2 = vr2 - vn2;
1618 vt3 = vr3 - vn3;
1619
1620 // normal friction term at contact
1621
1622 fn[0] = -c_n * vn1;
1623 fn[1] = -c_n * vn2;
1624 fn[2] = -c_n * vn3;
1625
1626 // tangential friction term at contact
1627 // excluding the tangential deformation term for now
1628
1629 ft[0] = -c_t * vt1;
1630 ft[1] = -c_t * vt2;
1631 ft[2] = -c_t * vt3;
1632
1633 // these are contact forces (F_n, F_t and F_ne) only
1634 // cohesive forces will be scaled by j_a after contact area is computed
1635 // mu * fne = tangential friction deformation during gross sliding
1636 // see Eq. 4, Fraige et al.
1637
1638 fx = fn[0] + ft[0] + mu * fx;
1639 fy = fn[1] + ft[1] + mu * fy;
1640 fz = fn[2] + ft[2] + mu * fz;
1641
1642 f[ibody][0] += fx;
1643 f[ibody][1] += fy;
1644 f[ibody][2] += fz;
1645 sum_torque(x[ibody], xi, fx, fy, fz, torque[ibody]);
1646
1647 f[jbody][0] -= fx;
1648 f[jbody][1] -= fy;
1649 f[jbody][2] -= fz;
1650 sum_torque(x[jbody], xj, -fx, -fy, -fz, torque[jbody]);
1651
1652 facc[0] += fx; facc[1] += fy; facc[2] += fz;
1653
1654 #ifdef _POLYHEDRON_DEBUG
1655 printf("contact ibody = %d: f = %f %f %f; torque = %f %f %f\n", ibody,
1656 f[ibody][0], f[ibody][1], f[ibody][2],
1657 torque[ibody][0], torque[ibody][1], torque[ibody][2]);
1658 printf("contact jbody = %d: f = %f %f %f; torque = %f %f %f\n", jbody,
1659 f[jbody][0], f[jbody][1], f[jbody][2],
1660 torque[jbody][0], torque[jbody][1], torque[jbody][2]);
1661 #endif
1662 }
1663
1664 /* ----------------------------------------------------------------------
1665 Rescale the forces and torques for all the contacts
1666 ------------------------------------------------------------------------- */
1667
rescale_cohesive_forces(double ** x,double ** f,double ** torque,Contact * contact_list,int & num_contacts,int itype,int jtype,double * facc)1668 void PairBodyRoundedPolyhedron::rescale_cohesive_forces(double** x,
1669 double** f, double** torque, Contact* contact_list, int &num_contacts,
1670 int itype, int jtype, double* facc)
1671 {
1672 int m,ibody,jbody;
1673 double delx,dely,delz,fx,fy,fz,R,fpair,r,contact_area;
1674
1675 int num_unique_contacts = 0;
1676 if (num_contacts == 1) {
1677 num_unique_contacts = 1;
1678 contact_area = 0;
1679 } else if (num_contacts == 2) {
1680 num_unique_contacts = 2;
1681 contact_area = num_contacts * A_ua;
1682 } else {
1683 find_unique_contacts(contact_list, num_contacts);
1684
1685 double xc[3],dx,dy,dz;
1686 xc[0] = xc[1] = xc[2] = 0;
1687 num_unique_contacts = 0;
1688 for (int m = 0; m < num_contacts; m++) {
1689 if (contact_list[m].unique == 0) continue;
1690 xc[0] += contact_list[m].xi[0];
1691 xc[1] += contact_list[m].xi[1];
1692 xc[2] += contact_list[m].xi[2];
1693 num_unique_contacts++;
1694 }
1695
1696 xc[0] /= (double)num_unique_contacts;
1697 xc[1] /= (double)num_unique_contacts;
1698 xc[2] /= (double)num_unique_contacts;
1699
1700 contact_area = 0.0;
1701 for (int m = 0; m < num_contacts; m++) {
1702 if (contact_list[m].unique == 0) continue;
1703 dx = contact_list[m].xi[0] - xc[0];
1704 dy = contact_list[m].xi[1] - xc[1];
1705 dz = contact_list[m].xi[2] - xc[2];
1706 contact_area += (dx*dx + dy*dy + dz*dz);
1707 }
1708 contact_area *= (MY_PI/(double)num_unique_contacts);
1709 }
1710
1711 double j_a = contact_area / (num_unique_contacts * A_ua);
1712 if (j_a < 1.0) j_a = 1.0;
1713 for (m = 0; m < num_contacts; m++) {
1714 if (contact_list[m].unique == 0) continue;
1715
1716 ibody = contact_list[m].ibody;
1717 jbody = contact_list[m].jbody;
1718
1719 delx = contact_list[m].xi[0] - contact_list[m].xj[0];
1720 dely = contact_list[m].xi[1] - contact_list[m].xj[1];
1721 delz = contact_list[m].xi[2] - contact_list[m].xj[2];
1722 r = sqrt(delx*delx + dely*dely + delz*delz);
1723 R = contact_list[m].separation;
1724
1725 double energy = 0;
1726 kernel_force(R, itype, jtype, energy, fpair);
1727
1728 fpair *= j_a;
1729 fx = delx*fpair/r;
1730 fy = dely*fpair/r;
1731 fz = delz*fpair/r;
1732
1733 f[ibody][0] += fx;
1734 f[ibody][1] += fy;
1735 f[ibody][2] += fz;
1736 sum_torque(x[ibody], contact_list[m].xi, fx, fy, fz, torque[ibody]);
1737
1738 f[jbody][0] -= fx;
1739 f[jbody][1] -= fy;
1740 f[jbody][2] -= fz;
1741 sum_torque(x[jbody], contact_list[m].xj, -fx, -fy, -fz, torque[jbody]);
1742
1743 facc[0] += fx; facc[1] += fy; facc[2] += fz;
1744 }
1745 }
1746
1747 /* ----------------------------------------------------------------------
1748 Accumulate torque to body from the force f=(fx,fy,fz) acting at point x
1749 ------------------------------------------------------------------------- */
1750
sum_torque(double * xm,double * x,double fx,double fy,double fz,double * torque)1751 void PairBodyRoundedPolyhedron::sum_torque(double* xm, double *x, double fx,
1752 double fy, double fz, double* torque)
1753 {
1754 double rx = x[0] - xm[0];
1755 double ry = x[1] - xm[1];
1756 double rz = x[2] - xm[2];
1757 double tx = ry * fz - rz * fy;
1758 double ty = rz * fx - rx * fz;
1759 double tz = rx * fy - ry * fx;
1760 torque[0] += tx;
1761 torque[1] += ty;
1762 torque[2] += tz;
1763 }
1764
1765 /* ----------------------------------------------------------------------
1766 Test if two points a and b are in opposite sides of a plane defined by
1767 a normal vector n and a point x0
1768 ------------------------------------------------------------------------- */
1769
opposite_sides(double * n,double * x0,double * a,double * b)1770 int PairBodyRoundedPolyhedron::opposite_sides(double* n, double* x0,
1771 double* a, double* b)
1772 {
1773 double m_a = n[0]*(a[0] - x0[0])+n[1]*(a[1] - x0[1])+n[2]*(a[2] - x0[2]);
1774 double m_b = n[0]*(b[0] - x0[0])+n[1]*(b[1] - x0[1])+n[2]*(b[2] - x0[2]);
1775 // equal to zero when either a or b is on the plane
1776 if (m_a * m_b <= 0)
1777 return 1;
1778 else
1779 return 0;
1780 }
1781
1782 /* ----------------------------------------------------------------------
1783 Test if a line segment defined by two points a and b intersects with
1784 a triangle defined by three points x1, x2 and x3
1785 ------------------------------------------------------------------------- */
1786
edge_face_intersect(double * x1,double * x2,double * x3,double * a,double * b,double * h_a,double * h_b,double & d_a,double & d_b,int & inside_a,int & inside_b)1787 int PairBodyRoundedPolyhedron::edge_face_intersect(double* x1, double* x2,
1788 double* x3, double* a, double* b, double* h_a, double* h_b,
1789 double& d_a, double& d_b, int& inside_a, int& inside_b)
1790 {
1791 double s[3], u[3], v[3], n[3];
1792
1793 // line director
1794
1795 MathExtra::sub3(b, a, s);
1796
1797 // plane normal vector
1798
1799 MathExtra::sub3(x2, x1, u);
1800 MathExtra::sub3(x3, x1, v);
1801 MathExtra::cross3(u, v, n);
1802 MathExtra::norm3(n);
1803
1804 // find the projection of a and b to the plane and the corresponding distances
1805
1806 project_pt_plane(a, x1, x2, x3, h_a, d_a, inside_a);
1807
1808 project_pt_plane(b, x1, x2, x3, h_b, d_b, inside_b);
1809
1810 // check if the line segment is parallel to the plane
1811
1812 double dot = MathExtra::dot3(s, n);
1813 if (fabs(dot) < EPSILON) return EF_PARALLEL;
1814
1815 // solve for the intersection between the line and the plane
1816
1817 double m[3][3], invm[3][3], p[3], ans[3];
1818 m[0][0] = -s[0];
1819 m[0][1] = u[0];
1820 m[0][2] = v[0];
1821
1822 m[1][0] = -s[1];
1823 m[1][1] = u[1];
1824 m[1][2] = v[1];
1825
1826 m[2][0] = -s[2];
1827 m[2][1] = u[2];
1828 m[2][2] = v[2];
1829
1830 MathExtra::sub3(a, x1, p);
1831 MathExtra::invert3(m, invm);
1832 MathExtra::matvec(invm, p, ans);
1833
1834 // p is reused for the intersection point
1835 // s = b - a
1836
1837 double t = ans[0];
1838 p[0] = a[0] + s[0] * t;
1839 p[1] = a[1] + s[1] * t;
1840 p[2] = a[2] + s[2] * t;
1841
1842 // check if p is inside the triangle, excluding the edges and vertices
1843 // the edge-edge and edge-vertices are handled separately
1844
1845 int inside = 0;
1846 if (ans[1] > 0 && ans[2] > 0 && ans[1] + ans[2] < 1)
1847 inside = 1;
1848
1849 int interact;
1850 if (t < 0 || t > 1) {
1851 interact = EF_SAME_SIDE_OF_FACE;
1852 } else {
1853 if (inside == 1)
1854 interact = EF_INTERSECT_INSIDE;
1855 else
1856 interact = EF_INTERSECT_OUTSIDE;
1857 }
1858
1859 return interact;
1860 }
1861
1862 /* ----------------------------------------------------------------------
1863 Find the projection of q on the plane defined by point p and the normal
1864 unit vector n: q_proj = q - dot(q - p, n) * n
1865 and the distance d from q to the plane
1866 ------------------------------------------------------------------------- */
1867
project_pt_plane(const double * q,const double * p,const double * n,double * q_proj,double & d)1868 void PairBodyRoundedPolyhedron::project_pt_plane(const double* q,
1869 const double* p, const double* n,
1870 double* q_proj, double &d)
1871 {
1872 double dot, ans[3], n_p[3];
1873 n_p[0] = n[0]; n_p[1] = n[1]; n_p[2] = n[2];
1874 MathExtra::sub3(q, p, ans);
1875 dot = MathExtra::dot3(ans, n_p);
1876 MathExtra::scale3(dot, n_p);
1877 MathExtra::sub3(q, n_p, q_proj);
1878 MathExtra::sub3(q, q_proj, ans);
1879 d = MathExtra::len3(ans);
1880 }
1881
1882 /* ----------------------------------------------------------------------
1883 Check if points q1 and q2 are inside a convex polygon, i.e. a face of
1884 a polyhedron
1885 ibody = atom i's index
1886 face_index = face index of the body
1887 xmi = atom i's coordinates
1888 q1 = tested point on the face (e.g. the projection of a point)
1889 q2 = another point (can be a null pointer) for face-edge intersection
1890 Output:
1891 inside1 = 1 if q1 is inside the polygon, 0 otherwise
1892 inside2 = 1 if q2 is inside the polygon, 0 otherwise
1893 ------------------------------------------------------------------------- */
1894
inside_polygon(int ibody,int face_index,double * xmi,const double * q1,const double * q2,int & inside1,int & inside2)1895 void PairBodyRoundedPolyhedron::inside_polygon(int ibody, int face_index,
1896 double* xmi, const double* q1, const double* q2,
1897 int& inside1, int& inside2)
1898
1899 {
1900 int i,n,ifirst,iffirst,npi1,npi2;
1901 double xi1[3],xi2[3],u[3],v[3],costheta,anglesum1,anglesum2,magu,magv;
1902
1903 ifirst = dfirst[ibody];
1904 iffirst = facfirst[ibody];
1905 anglesum1 = anglesum2 = 0;;
1906 for (i = 0; i < MAX_FACE_SIZE; i++) {
1907 npi1 = static_cast<int>(face[iffirst+face_index][i]);
1908 if (npi1 < 0) break;
1909 n = i + 1;
1910 if (n <= MAX_FACE_SIZE - 1) {
1911 npi2 = static_cast<int>(face[iffirst+face_index][n]);
1912 if (npi2 < 0) npi2 = static_cast<int>(face[iffirst+face_index][0]);
1913 } else {
1914 npi2 = static_cast<int>(face[iffirst+face_index][0]);
1915 }
1916
1917 xi1[0] = xmi[0] + discrete[ifirst+npi1][0];
1918 xi1[1] = xmi[1] + discrete[ifirst+npi1][1];
1919 xi1[2] = xmi[2] + discrete[ifirst+npi1][2];
1920
1921 xi2[0] = xmi[0] + discrete[ifirst+npi2][0];
1922 xi2[1] = xmi[1] + discrete[ifirst+npi2][1];
1923 xi2[2] = xmi[2] + discrete[ifirst+npi2][2];
1924
1925 MathExtra::sub3(xi1,q1,u);
1926 MathExtra::sub3(xi2,q1,v);
1927 magu = MathExtra::len3(u);
1928 magv = MathExtra::len3(v);
1929
1930 // the point is at either vertices
1931
1932 if (magu * magv < EPSILON) inside1 = 1;
1933 else {
1934 costheta = MathExtra::dot3(u,v)/(magu*magv);
1935 anglesum1 += acos(costheta);
1936 }
1937
1938 if (q2 != nullptr) {
1939 MathExtra::sub3(xi1,q2,u);
1940 MathExtra::sub3(xi2,q2,v);
1941 magu = MathExtra::len3(u);
1942 magv = MathExtra::len3(v);
1943 if (magu * magv < EPSILON) inside2 = 1;
1944 else {
1945 costheta = MathExtra::dot3(u,v)/(magu*magv);
1946 anglesum2 += acos(costheta);
1947 }
1948 }
1949 }
1950
1951 if (fabs(anglesum1 - MY_2PI) < EPSILON) inside1 = 1;
1952 else inside1 = 0;
1953
1954 if (q2 != nullptr) {
1955 if (fabs(anglesum2 - MY_2PI) < EPSILON) inside2 = 1;
1956 else inside2 = 0;
1957 }
1958 }
1959
1960 /* ----------------------------------------------------------------------
1961 Find the projection of q on the plane defined by 3 points x1, x2 and x3
1962 returns the distance d from q to the plane and whether the projected
1963 point is inside the triangle defined by (x1, x2, x3)
1964 ------------------------------------------------------------------------- */
1965
project_pt_plane(const double * q,const double * x1,const double * x2,const double * x3,double * q_proj,double & d,int & inside)1966 void PairBodyRoundedPolyhedron::project_pt_plane(const double* q,
1967 const double* x1, const double* x2, const double* x3, double* q_proj,
1968 double &d, int& inside)
1969 {
1970 double u[3],v[3],n[3];
1971
1972 // plane normal vector
1973
1974 MathExtra::sub3(x2, x1, u);
1975 MathExtra::sub3(x3, x1, v);
1976 MathExtra::cross3(u, v, n);
1977 MathExtra::norm3(n);
1978
1979 // solve for the intersection between the line and the plane
1980
1981 double m[3][3], invm[3][3], p[3], ans[3];
1982 m[0][0] = -n[0];
1983 m[0][1] = u[0];
1984 m[0][2] = v[0];
1985
1986 m[1][0] = -n[1];
1987 m[1][1] = u[1];
1988 m[1][2] = v[1];
1989
1990 m[2][0] = -n[2];
1991 m[2][1] = u[2];
1992 m[2][2] = v[2];
1993
1994 MathExtra::sub3(q, x1, p);
1995 MathExtra::invert3(m, invm);
1996 MathExtra::matvec(invm, p, ans);
1997
1998 double t = ans[0];
1999 q_proj[0] = q[0] + n[0] * t;
2000 q_proj[1] = q[1] + n[1] * t;
2001 q_proj[2] = q[2] + n[2] * t;
2002
2003 // check if the projection point is inside the triangle
2004 // exclude the edges and vertices
2005 // edge-sphere and sphere-sphere interactions are handled separately
2006
2007 inside = 0;
2008 if (ans[1] > 0 && ans[2] > 0 && ans[1] + ans[2] < 1) {
2009 inside = 1;
2010 }
2011
2012 // distance from q to q_proj
2013
2014 MathExtra::sub3(q, q_proj, ans);
2015 d = MathExtra::len3(ans);
2016 }
2017
2018 /* ---------------------------------------------------------------------- */
2019
project_pt_line(const double * q,const double * xi1,const double * xi2,double * h,double & d,double & t)2020 void PairBodyRoundedPolyhedron::project_pt_line(const double* q,
2021 const double* xi1, const double* xi2, double* h, double& d, double& t)
2022 {
2023 double u[3],v[3],r[3],s;
2024
2025 MathExtra::sub3(xi2, xi1, u);
2026 MathExtra::norm3(u);
2027 MathExtra::sub3(q, xi1, v);
2028
2029 s = MathExtra::dot3(u, v);
2030 h[0] = xi1[0] + s * u[0];
2031 h[1] = xi1[1] + s * u[1];
2032 h[2] = xi1[2] + s * u[2];
2033
2034 MathExtra::sub3(q, h, r);
2035 d = MathExtra::len3(r);
2036
2037 if (fabs(xi2[0] - xi1[0]) > 0)
2038 t = (h[0] - xi1[0])/(xi2[0] - xi1[0]);
2039 else if (fabs(xi2[1] - xi1[1]) > 0)
2040 t = (h[1] - xi1[1])/(xi2[1] - xi1[1]);
2041 else if (fabs(xi2[2] - xi1[2]) > 0)
2042 t = (h[2] - xi1[2])/(xi2[2] - xi1[2]);
2043 }
2044
2045 /* ----------------------------------------------------------------------
2046 compute the shortest distance between two edges (line segments)
2047 x1, x2: two endpoints of the first edge
2048 x3, x4: two endpoints of the second edge
2049 h1: the end point of the shortest segment perpendicular to both edges
2050 on the line (x1;x2)
2051 h2: the end point of the shortest segment perpendicular to both edges
2052 on the line (x3;x4)
2053 t1: fraction of h1 in the segment (x1,x2)
2054 t2: fraction of h2 in the segment (x3,x4)
2055 ------------------------------------------------------------------------- */
2056
distance_bt_edges(const double * x1,const double * x2,const double * x3,const double * x4,double * h1,double * h2,double & t1,double & t2,double & r)2057 void PairBodyRoundedPolyhedron::distance_bt_edges(const double* x1,
2058 const double* x2, const double* x3, const double* x4,
2059 double* h1, double* h2, double& t1, double& t2, double& r)
2060 {
2061 double u[3],v[3],n[3],dot;
2062
2063 // set the default returned values
2064
2065 t1 = -2;
2066 t2 = 2;
2067 r = 0;
2068
2069 // find the edge unit directors and their dot product
2070
2071 MathExtra::sub3(x2, x1, u);
2072 MathExtra::norm3(u);
2073 MathExtra::sub3(x4, x3, v);
2074 MathExtra::norm3(v);
2075 dot = MathExtra::dot3(u,v);
2076 dot = fabs(dot);
2077
2078 // check if two edges are parallel
2079 // find the two ends of the overlapping segment, if any
2080
2081 if (fabs(dot - 1.0) < EPSILON) {
2082
2083 double s1,s2,x13[3],x23[3],x13h[3];
2084 double t13,t23,t31,t41,x31[3],x41[3];
2085 t13=t23=t31=t41=0.0;
2086
2087 MathExtra::sub3(x1,x3,x13); // x13 = x1 - x3
2088 MathExtra::sub3(x2,x3,x23); // x23 = x2 - x3
2089
2090 s1 = MathExtra::dot3(x13,v);
2091 x13h[0] = x13[0] - s1*v[0];
2092 x13h[1] = x13[1] - s1*v[1];
2093 x13h[2] = x13[2] - s1*v[2];
2094 r = MathExtra::len3(x13h);
2095
2096 // x13 is the projection of x1 on x3-x4
2097
2098 x13[0] = x3[0] + s1*v[0];
2099 x13[1] = x3[1] + s1*v[1];
2100 x13[2] = x3[2] + s1*v[2];
2101
2102 // x23 is the projection of x2 on x3-x4
2103
2104 s2 = MathExtra::dot3(x23,v);
2105 x23[0] = x3[0] + s2*v[0];
2106 x23[1] = x3[1] + s2*v[1];
2107 x23[2] = x3[2] + s2*v[2];
2108
2109 // find the fraction of the projection points on the edges
2110
2111 if (fabs(x4[0] - x3[0]) > 0)
2112 t13 = (x13[0] - x3[0])/(x4[0] - x3[0]);
2113 else if (fabs(x4[1] - x3[1]) > 0)
2114 t13 = (x13[1] - x3[1])/(x4[1] - x3[1]);
2115 else if (fabs(x4[2] - x3[2]) > 0)
2116 t13 = (x13[2] - x3[2])/(x4[2] - x3[2]);
2117
2118 if (fabs(x4[0] - x3[0]) > 0)
2119 t23 = (x23[0] - x3[0])/(x4[0] - x3[0]);
2120 else if (fabs(x4[1] - x3[1]) > 0)
2121 t23 = (x23[1] - x3[1])/(x4[1] - x3[1]);
2122 else if (fabs(x4[2] - x3[2]) > 0)
2123 t23 = (x23[2] - x3[2])/(x4[2] - x3[2]);
2124
2125 if (fabs(x23[0] - x13[0]) > 0)
2126 t31 = (x3[0] - x13[0])/(x23[0] - x13[0]);
2127 else if (fabs(x23[1] - x13[1]) > 0)
2128 t31 = (x3[1] - x13[1])/(x23[1] - x13[1]);
2129 else if (fabs(x23[2] - x13[2]) > 0)
2130 t31 = (x3[2] - x13[2])/(x23[2] - x13[2]);
2131
2132 // x31 is the projection of x3 on x1-x2
2133
2134 x31[0] = x1[0] + t31*(x2[0] - x1[0]);
2135 x31[1] = x1[1] + t31*(x2[1] - x1[1]);
2136 x31[2] = x1[2] + t31*(x2[2] - x1[2]);
2137
2138 if (fabs(x23[0] - x13[0]) > 0)
2139 t41 = (x4[0] - x13[0])/(x23[0] - x13[0]);
2140 else if (fabs(x23[1] - x13[1]) > 0)
2141 t41 = (x4[1] - x13[1])/(x23[1] - x13[1]);
2142 else if (fabs(x23[2] - x13[2]) > 0)
2143 t41 = (x4[2] - x13[2])/(x23[2] - x13[2]);
2144
2145 // x41 is the projection of x4 on x1-x2
2146
2147 x41[0] = x1[0] + t41*(x2[0] - x1[0]);
2148 x41[1] = x1[1] + t41*(x2[1] - x1[1]);
2149 x41[2] = x1[2] + t41*(x2[2] - x1[2]);
2150
2151 // determine two ends from the overlapping segments
2152
2153 int n1 = 0;
2154 int n2 = 0;
2155 if (t13 >= 0 && t13 <= 1) {
2156 h1[0] = x1[0];
2157 h1[1] = x1[1];
2158 h1[2] = x1[2];
2159 h2[0] = x13[0];
2160 h2[1] = x13[1];
2161 h2[2] = x13[2];
2162 t1 = 0;
2163 t2 = t13;
2164 n1++;
2165 n2++;
2166 }
2167 if (t23 >= 0 && t23 <= 1) {
2168 if (n1 == 0) {
2169 h1[0] = x2[0];
2170 h1[1] = x2[1];
2171 h1[2] = x2[2];
2172 h2[0] = x23[0];
2173 h2[1] = x23[1];
2174 h2[2] = x23[2];
2175 t1 = 1;
2176 t2 = t23;
2177 n1++;
2178 n2++;
2179 } else {
2180 h1[0] = (x1[0]+x2[0])/2;
2181 h1[1] = (x1[1]+x2[1])/2;
2182 h1[2] = (x1[2]+x2[2])/2;
2183 h2[0] = (x13[0]+x23[0])/2;
2184 h2[1] = (x13[1]+x23[1])/2;
2185 h2[2] = (x13[2]+x23[2])/2;
2186 t1 = 0.5;
2187 t2 = (t13+t23)/2;
2188 n1++;
2189 n2++;
2190 }
2191 }
2192
2193 if (n1 == 0 && n2 == 0) {
2194 if (t31 >= 0 && t31 <= 1) {
2195 h1[0] = x31[0];
2196 h1[1] = x31[1];
2197 h1[2] = x31[2];
2198 h2[0] = x3[0];
2199 h2[1] = x3[1];
2200 h2[2] = x3[2];
2201 t1 = t31;
2202 t2 = 0;
2203 n1++;
2204 n2++;
2205 }
2206 if (t41 >= 0 && t41 <= 1) {
2207 if (n1 == 0) {
2208 h1[0] = x41[0];
2209 h1[1] = x41[1];
2210 h1[2] = x41[2];
2211 h2[0] = x4[0];
2212 h2[1] = x4[1];
2213 h2[2] = x4[2];
2214 t1 = t41;
2215 t2 = 1;
2216 n1++;
2217 n2++;
2218 } else {
2219 h1[0] = (x31[0]+x41[0])/2;
2220 h1[1] = (x31[1]+x41[1])/2;
2221 h1[2] = (x31[2]+x41[2])/2;
2222 h2[0] = (x3[0]+x4[0])/2;
2223 h2[1] = (x3[1]+x4[1])/2;
2224 h2[2] = (x3[2]+x4[2])/2;
2225 t1 = (t31+t41)/2;
2226 t2 = 0.5;
2227 n1++;
2228 n2++;
2229 }
2230 }
2231 }
2232
2233 // if n1 == 0 and n2 == 0 at this point,
2234 // which means no overlapping segments bt two parallel edges,
2235 // return the default values of t1 and t2
2236
2237 return;
2238
2239 }
2240
2241 // find the vector n perpendicular to both edges
2242
2243 MathExtra::cross3(u, v, n);
2244 MathExtra::norm3(n);
2245
2246 // find the intersection of the line (x3,x4) and the plane (x1,x2,n)
2247 // s = director of the line (x3,x4)
2248 // n_p = plane normal vector of the plane (x1,x2,n)
2249
2250 double s[3], n_p[3];
2251 MathExtra::sub3(x4, x3, s);
2252 MathExtra::sub3(x2, x1, u);
2253 MathExtra::cross3(u, n, n_p);
2254 MathExtra::norm3(n_p);
2255
2256 // solve for the intersection between the line and the plane
2257
2258 double m[3][3], invm[3][3], p[3], ans[3];
2259 m[0][0] = -s[0];
2260 m[0][1] = u[0];
2261 m[0][2] = n[0];
2262
2263 m[1][0] = -s[1];
2264 m[1][1] = u[1];
2265 m[1][2] = n[1];
2266
2267 m[2][0] = -s[2];
2268 m[2][1] = u[2];
2269 m[2][2] = n[2];
2270
2271 MathExtra::sub3(x3, x1, p);
2272 MathExtra::invert3(m, invm);
2273 MathExtra::matvec(invm, p, ans);
2274
2275 t2 = ans[0];
2276 h2[0] = x3[0] + s[0] * t2;
2277 h2[1] = x3[1] + s[1] * t2;
2278 h2[2] = x3[2] + s[2] * t2;
2279
2280 project_pt_plane(h2, x1, n, h1, r);
2281
2282 if (fabs(x2[0] - x1[0]) > 0)
2283 t1 = (h1[0] - x1[0])/(x2[0] - x1[0]);
2284 else if (fabs(x2[1] - x1[1]) > 0)
2285 t1 = (h1[1] - x1[1])/(x2[1] - x1[1]);
2286 else if (fabs(x2[2] - x1[2]) > 0)
2287 t1 = (h1[2] - x1[2])/(x2[2] - x1[2]);
2288 }
2289
2290 /* ----------------------------------------------------------------------
2291 Calculate the total velocity of a point (vertex, a point on an edge):
2292 vi = vcm + omega ^ (p - xcm)
2293 ------------------------------------------------------------------------- */
2294
total_velocity(double * p,double * xcm,double * vcm,double * angmom,double * inertia,double * quat,double * vi)2295 void PairBodyRoundedPolyhedron::total_velocity(double* p, double *xcm,
2296 double* vcm, double *angmom, double *inertia, double *quat, double* vi)
2297 {
2298 double r[3],omega[3],ex_space[3],ey_space[3],ez_space[3];
2299 r[0] = p[0] - xcm[0];
2300 r[1] = p[1] - xcm[1];
2301 r[2] = p[2] - xcm[2];
2302 MathExtra::q_to_exyz(quat,ex_space,ey_space,ez_space);
2303 MathExtra::angmom_to_omega(angmom,ex_space,ey_space,ez_space,
2304 inertia,omega);
2305 vi[0] = omega[1]*r[2] - omega[2]*r[1] + vcm[0];
2306 vi[1] = omega[2]*r[0] - omega[0]*r[2] + vcm[1];
2307 vi[2] = omega[0]*r[1] - omega[1]*r[0] + vcm[2];
2308 }
2309
2310 /* ----------------------------------------------------------------------
2311 Determine the length of the contact segment, i.e. the separation between
2312 2 contacts, should be extended for 3D models.
2313 ------------------------------------------------------------------------- */
2314
contact_separation(const Contact & c1,const Contact & c2)2315 double PairBodyRoundedPolyhedron::contact_separation(const Contact& c1,
2316 const Contact& c2)
2317 {
2318 double x1 = 0.5*(c1.xi[0] + c1.xj[0]);
2319 double y1 = 0.5*(c1.xi[1] + c1.xj[1]);
2320 double z1 = 0.5*(c1.xi[2] + c1.xj[2]);
2321 double x2 = 0.5*(c2.xi[0] + c2.xj[0]);
2322 double y2 = 0.5*(c2.xi[1] + c2.xj[1]);
2323 double z2 = 0.5*(c2.xi[2] + c2.xj[2]);
2324 double rsq = (x2 - x1)*(x2 - x1) + (y2 - y1)*(y2 - y1) + (z2 - z1)*(z2 - z1);
2325 return rsq;
2326 }
2327
2328 /* ----------------------------------------------------------------------
2329 find the number of unique contacts
2330 ------------------------------------------------------------------------- */
2331
find_unique_contacts(Contact * contact_list,int & num_contacts)2332 void PairBodyRoundedPolyhedron::find_unique_contacts(Contact* contact_list,
2333 int& num_contacts)
2334 {
2335 int n = num_contacts;
2336 for (int i = 0; i < n - 1; i++) {
2337
2338 for (int j = i + 1; j < n; j++) {
2339 if (contact_list[i].unique == 0) continue;
2340 double d = contact_separation(contact_list[i], contact_list[j]);
2341 if (d < EPSILON) contact_list[j].unique = 0;
2342 }
2343 }
2344 }
2345
2346 /* ---------------------------------------------------------------------- */
2347
sanity_check()2348 void PairBodyRoundedPolyhedron::sanity_check()
2349 {
2350
2351 double x1[3],x2[3],h_a[3],h_b[3],d_a,d_b;
2352 double a[3],b[3],t_a,t_b;
2353
2354 x1[0] = 0; x1[1] = 3; x1[2] = 0;
2355 x2[0] = 3; x2[1] = 0; x2[2] = 0;
2356
2357 a[0] = 0; a[1] = 0; a[2] = 0;
2358 b[0] = 4; b[1] = 0; b[2] = 0;
2359
2360 project_pt_line(a, x1, x2, h_a, d_a, t_a);
2361 project_pt_line(b, x1, x2, h_b, d_b, t_b);
2362 /*
2363 printf("h_a: %f %f %f; h_b: %f %f %f; t_a = %f; t_b = %f; d = %f; d_b = %f\n",
2364 h_a[0], h_a[1], h_a[2], h_b[0], h_b[1], h_b[2], t_a, t_b, d_a, d_b);
2365 */
2366 /*
2367 int inside_a, inside_b;
2368 int mode = edge_face_intersect(x1, x2, x3, a, b, h_a, h_b, d_a, d_b,
2369 inside_a, inside_b);
2370
2371 double u[3],v[3],n[3];
2372 MathExtra::sub3(x2, x1, u);
2373 MathExtra::sub3(x3, x1, v);
2374 MathExtra::cross3(u, v, n);
2375 MathExtra::norm3(n);
2376 */
2377 /*
2378 project_pt_plane(a, x1, x2, x3, h_a, d_a, inside_a);
2379 printf("h_a: %f %f %f; d = %f: inside %d\n",
2380 h_a[0], h_a[1], h_a[2], d_a, inside_a);
2381 project_pt_plane(b, x1, x2, x3, h_b, d_b, inside_b);
2382 printf("h_b: %f %f %f; d = %f: inside %d\n",
2383 h_b[0], h_b[1], h_b[2], d_b, inside_b);
2384 */
2385 /*
2386 distance_bt_edges(x1, x2, x3, x4, h_a, h_b, t_a, t_b, d_a);
2387 printf("h_a: %f %f %f; h_b: %f %f %f; t_a = %f; t_b = %f; d = %f\n",
2388 h_a[0], h_a[1], h_a[2], h_b[0], h_b[1], h_b[2], t_a, t_b, d_a);
2389 */
2390 }
2391
2392