1 // clang-format off
2 /* ----------------------------------------------------------------------
3 *
4 * *** Smooth Mach Dynamics ***
5 *
6 * This file is part of the MACHDYN package for LAMMPS.
7 * Copyright (2014) Georg C. Ganzenmueller, georg.ganzenmueller@emi.fhg.de
8 * Fraunhofer Ernst-Mach Institute for High-Speed Dynamics, EMI,
9 * Eckerstrasse 4, D-79104 Freiburg i.Br, Germany.
10 *
11 * ----------------------------------------------------------------------- */
12
13 /* ----------------------------------------------------------------------
14 LAMMPS - Large-scale Atomic/Molecular Massively Parallel Simulator
15 https://www.lammps.org/, Sandia National Laboratories
16 Steve Plimpton, sjplimp@sandia.gov
17
18 Copyright (2003) Sandia Corporation. Under the terms of Contract
19 DE-AC04-94AL85000 with Sandia Corporation, the U.S. Government retains
20 certain rights in this software. This software is distributed under
21 the GNU General Public License.
22
23 See the README file in the top-level LAMMPS directory.
24 ------------------------------------------------------------------------- */
25
26 /* ----------------------------------------------------------------------
27 Contributing author: Mike Parks (SNL)
28 ------------------------------------------------------------------------- */
29
30 #include "pair_smd_triangulated_surface.h"
31
32 #include <cmath>
33
34 #include <cstring>
35 #include <Eigen/Eigen>
36 #include "atom.h"
37 #include "domain.h"
38 #include "force.h"
39 #include "comm.h"
40 #include "neighbor.h"
41 #include "neigh_list.h"
42 #include "neigh_request.h"
43 #include "memory.h"
44 #include "error.h"
45
46
47 using namespace std;
48 using namespace LAMMPS_NS;
49 using namespace Eigen;
50
51 #define SQRT2 1.414213562e0
52
53 /* ---------------------------------------------------------------------- */
54
PairTriSurf(LAMMPS * lmp)55 PairTriSurf::PairTriSurf(LAMMPS *lmp) :
56 Pair(lmp) {
57
58 onerad_dynamic = onerad_frozen = maxrad_dynamic = maxrad_frozen = nullptr;
59 bulkmodulus = nullptr;
60 kn = nullptr;
61 scale = 1.0;
62 }
63
64 /* ---------------------------------------------------------------------- */
65
~PairTriSurf()66 PairTriSurf::~PairTriSurf() {
67
68 if (allocated) {
69 memory->destroy(setflag);
70 memory->destroy(cutsq);
71 memory->destroy(bulkmodulus);
72 memory->destroy(kn);
73
74 delete[] onerad_dynamic;
75 delete[] onerad_frozen;
76 delete[] maxrad_dynamic;
77 delete[] maxrad_frozen;
78 }
79 }
80
81 /* ---------------------------------------------------------------------- */
82
compute(int eflag,int vflag)83 void PairTriSurf::compute(int eflag, int vflag) {
84 int i, j, ii, jj, inum, jnum, itype, jtype;
85 double rsq, r, evdwl, fpair;
86 int *ilist, *jlist, *numneigh, **firstneigh;
87 double rcut, r_geom, delta, r_tri, r_particle, touch_distance, dt_crit;
88 int tri, particle;
89 Vector3d normal, x1, x2, x3, x4, x13, x23, x43, w, cp, x4cp, vnew, v_old;
90 ;
91 Vector3d xi, x_center, dx;
92 Matrix2d C;
93 Vector2d w2d, rhs;
94
95 evdwl = 0.0;
96 ev_init(eflag, vflag);
97
98 tagint *mol = atom->molecule;
99 double **f = atom->f;
100 double **smd_data_9 = atom->smd_data_9;
101 double **x = atom->x;
102 double **x0 = atom->x0;
103 double **v = atom->v;
104 double *rmass = atom->rmass;
105 int *type = atom->type;
106 int nlocal = atom->nlocal;
107 double *radius = atom->contact_radius;
108 double rcutSq;
109 Vector3d offset;
110
111 int newton_pair = force->newton_pair;
112 int periodic = (domain->xperiodic || domain->yperiodic || domain->zperiodic);
113
114 inum = list->inum;
115 ilist = list->ilist;
116 numneigh = list->numneigh;
117 firstneigh = list->firstneigh;
118
119 int max_neighs = 0;
120 stable_time_increment = 1.0e22;
121
122 // loop over neighbors of my atoms using a half neighbor list
123 for (ii = 0; ii < inum; ii++) {
124 i = ilist[ii];
125 itype = type[i];
126 jlist = firstneigh[i];
127 jnum = numneigh[i];
128 max_neighs = MAX(max_neighs, jnum);
129
130 for (jj = 0; jj < jnum; jj++) {
131 j = jlist[jj];
132
133 j &= NEIGHMASK;
134
135 jtype = type[j];
136
137 /*
138 * decide which one of i, j is triangle and which is particle
139 */
140 if ((mol[i] < 65535) && (mol[j] >= 65535)) {
141 particle = i;
142 tri = j;
143 } else if ((mol[j] < 65535) && (mol[i] >= 65535)) {
144 particle = j;
145 tri = i;
146 } else {
147 error->one(FLERR, "unknown case");
148 }
149
150 //x_center << x[tri][0], x[tri][1], x[tri][2]; // center of triangle
151 x_center(0) = x[tri][0];
152 x_center(1) = x[tri][1];
153 x_center(2) = x[tri][2];
154 //x4 << x[particle][0], x[particle][1], x[particle][2];
155 x4(0) = x[particle][0];
156 x4(1) = x[particle][1];
157 x4(2) = x[particle][2];
158 dx = x_center - x4; //
159 if (periodic) {
160 domain->minimum_image(dx(0), dx(1), dx(2));
161 }
162 rsq = dx.squaredNorm();
163
164 r_tri = scale * radius[tri];
165 r_particle = scale * radius[particle];
166 rcut = r_tri + r_particle;
167 rcutSq = rcut * rcut;
168
169 //printf("type i=%d, type j=%d, r=%f, ri=%f, rj=%f\n", itype, jtype, sqrt(rsq), ri, rj);
170
171 if (rsq < rcutSq) {
172
173 /*
174 * gather triangle information
175 */
176 normal(0) = x0[tri][0];
177 normal(1) = x0[tri][1];
178 normal(2) = x0[tri][2];
179
180 /*
181 * distance check: is particle closer than its radius to the triangle plane?
182 */
183 if (fabs(dx.dot(normal)) < radius[particle]) {
184 /*
185 * get other two triangle vertices
186 */
187 x1(0) = smd_data_9[tri][0];
188 x1(1) = smd_data_9[tri][1];
189 x1(2) = smd_data_9[tri][2];
190 x2(0) = smd_data_9[tri][3];
191 x2(1) = smd_data_9[tri][4];
192 x2(2) = smd_data_9[tri][5];
193 x3(0) = smd_data_9[tri][6];
194 x3(1) = smd_data_9[tri][7];
195 x3(2) = smd_data_9[tri][8];
196
197 PointTriangleDistance(x4, x1, x2, x3, cp, r);
198
199 /*
200 * distance to closest point
201 */
202 x4cp = x4 - cp;
203
204 /*
205 * flip normal to point in direction of x4cp
206 */
207
208 if (x4cp.dot(normal) < 0.0) {
209 normal *= -1.0;
210 }
211
212 /*
213 * penalty force pushes particle away from triangle
214 */
215 if (r < 1.0 * radius[particle]) {
216
217 delta = radius[particle] - r; // overlap distance
218 r_geom = radius[particle];
219 fpair = 1.066666667e0 * bulkmodulus[itype][jtype] * delta * sqrt(delta * r_geom);
220 dt_crit = 3.14 * sqrt(rmass[particle] / (fpair / delta));
221 stable_time_increment = MIN(stable_time_increment, dt_crit);
222
223 evdwl = r * fpair * 0.4e0 * delta; // GCG 25 April: this expression conserves total energy
224
225 fpair /= (r + 1.0e-2 * radius[particle]); // divide by r + softening and multiply with non-normalized distance vector
226
227 if (particle < nlocal) {
228 f[particle][0] += x4cp(0) * fpair;
229 f[particle][1] += x4cp(1) * fpair;
230 f[particle][2] += x4cp(2) * fpair;
231 }
232
233 if (tri < nlocal) {
234 f[tri][0] -= x4cp(0) * fpair;
235 f[tri][1] -= x4cp(1) * fpair;
236 f[tri][2] -= x4cp(2) * fpair;
237 }
238
239 if (evflag) {
240 ev_tally(i, j, nlocal, newton_pair, evdwl, 0.0, fpair, x4cp(0), x4cp(1), x4cp(2));
241 }
242
243 }
244
245 /*
246 * if particle comes too close to triangle, reflect its velocity and explicitly move it away
247 */
248
249 touch_distance = 1.0 * radius[particle];
250 if (r < touch_distance) {
251
252 /*
253 * reflect velocity if it points toward triangle
254 */
255
256 normal = x4cp / r;
257
258 //v_old << v[particle][0], v[particle][1], v[particle][2];
259 v_old(0) = v[particle][0];
260 v_old(1) = v[particle][1];
261 v_old(2) = v[particle][2];
262 if (v_old.dot(normal) < 0.0) {
263 //printf("flipping velocity\n");
264 vnew = 1.0 * (-2.0 * v_old.dot(normal) * normal + v_old);
265 v[particle][0] = vnew(0);
266 v[particle][1] = vnew(1);
267 v[particle][2] = vnew(2);
268 }
269
270 //printf("moving particle on top of triangle\n");
271 x[particle][0] = cp(0) + touch_distance * normal(0);
272 x[particle][1] = cp(1) + touch_distance * normal(1);
273 x[particle][2] = cp(2) + touch_distance * normal(2);
274 }
275
276 }
277 }
278 }
279 }
280
281 // int max_neighs_all = 0;
282 // MPI_Allreduce(&max_neighs, &max_neighs_all, 1, MPI_INT, MPI_MAX, world);
283 // if (comm->me == 0) {
284 // printf("max. neighs in tri pair is %d\n", max_neighs_all);
285 // }
286 //
287 // double stable_time_increment_all = 0.0;
288 // MPI_Allreduce(&stable_time_increment, &stable_time_increment_all, 1, MPI_DOUBLE, MPI_MIN, world);
289 // if (comm->me == 0) {
290 // printf("stable time step tri pair is %f\n", stable_time_increment_all);
291 // }
292 }
293
294 /* ----------------------------------------------------------------------
295 allocate all arrays
296 ------------------------------------------------------------------------- */
297
allocate()298 void PairTriSurf::allocate() {
299 allocated = 1;
300 int n = atom->ntypes;
301
302 memory->create(setflag, n + 1, n + 1, "pair:setflag");
303 for (int i = 1; i <= n; i++)
304 for (int j = i; j <= n; j++)
305 setflag[i][j] = 0;
306
307 memory->create(bulkmodulus, n + 1, n + 1, "pair:kspring");
308 memory->create(kn, n + 1, n + 1, "pair:kn");
309
310 memory->create(cutsq, n + 1, n + 1, "pair:cutsq"); // always needs to be allocated, even with granular neighborlist
311
312 onerad_dynamic = new double[n + 1];
313 onerad_frozen = new double[n + 1];
314 maxrad_dynamic = new double[n + 1];
315 maxrad_frozen = new double[n + 1];
316 }
317
318 /* ----------------------------------------------------------------------
319 global settings
320 ------------------------------------------------------------------------- */
321
settings(int narg,char ** arg)322 void PairTriSurf::settings(int narg, char **arg) {
323 if (narg != 1)
324 error->all(FLERR, "Illegal number of args for pair_style smd/tri_surface");
325
326 scale = utils::numeric(FLERR, arg[0],false,lmp);
327 if (comm->me == 0) {
328 printf("\n>>========>>========>>========>>========>>========>>========>>========>>========\n");
329 printf("SMD/TRI_SURFACE CONTACT SETTINGS:\n");
330 printf("... effective contact radius is scaled by %f\n", scale);
331 printf(">>========>>========>>========>>========>>========>>========>>========>>========\n");
332 }
333
334 }
335
336 /* ----------------------------------------------------------------------
337 set coeffs for one or more type pairs
338 ------------------------------------------------------------------------- */
339
coeff(int narg,char ** arg)340 void PairTriSurf::coeff(int narg, char **arg) {
341 if (narg != 3)
342 error->all(FLERR, "Incorrect args for pair coefficients");
343 if (!allocated)
344 allocate();
345
346 int ilo, ihi, jlo, jhi;
347 utils::bounds(FLERR,arg[0], 1,atom->ntypes, ilo, ihi, error);
348 utils::bounds(FLERR,arg[1], 1,atom->ntypes, jlo, jhi, error);
349
350 double bulkmodulus_one = atof(arg[2]);
351
352 // set short-range force constant
353 double kn_one = 0.0;
354 if (domain->dimension == 3) {
355 kn_one = (16. / 15.) * bulkmodulus_one; //assuming poisson ratio = 1/4 for 3d
356 } else {
357 kn_one = 0.251856195 * (2. / 3.) * bulkmodulus_one; //assuming poisson ratio = 1/3 for 2d
358 }
359
360 int count = 0;
361 for (int i = ilo; i <= ihi; i++) {
362 for (int j = MAX(jlo, i); j <= jhi; j++) {
363 bulkmodulus[i][j] = bulkmodulus_one;
364 kn[i][j] = kn_one;
365 setflag[i][j] = 1;
366 count++;
367 }
368 }
369
370 if (count == 0)
371 error->all(FLERR, "Incorrect args for pair coefficients");
372 }
373
374 /* ----------------------------------------------------------------------
375 init for one type pair i,j and corresponding j,i
376 ------------------------------------------------------------------------- */
377
init_one(int i,int j)378 double PairTriSurf::init_one(int i, int j) {
379
380 if (!allocated)
381 allocate();
382
383 if (setflag[i][j] == 0)
384 error->all(FLERR, "All pair coeffs are not set");
385
386 bulkmodulus[j][i] = bulkmodulus[i][j];
387 kn[j][i] = kn[i][j];
388
389 // cutoff = sum of max I,J radii for
390 // dynamic/dynamic & dynamic/frozen interactions, but not frozen/frozen
391
392 double cutoff = maxrad_dynamic[i] + maxrad_dynamic[j];
393 cutoff = MAX(cutoff, maxrad_frozen[i] + maxrad_dynamic[j]);
394 cutoff = MAX(cutoff, maxrad_dynamic[i] + maxrad_frozen[j]);
395
396 if (comm->me == 0) {
397 printf("cutoff for pair smd/smd/tri_surface = %f\n", cutoff);
398 }
399 return cutoff;
400 }
401
402 /* ----------------------------------------------------------------------
403 init specific to this pair style
404 ------------------------------------------------------------------------- */
405
init_style()406 void PairTriSurf::init_style() {
407 int i;
408
409 // error checks
410
411 if (!atom->contact_radius_flag)
412 error->all(FLERR, "Pair style smd/smd/tri_surface requires atom style with contact_radius");
413
414 // old: half list
415 int irequest = neighbor->request(this);
416 neighbor->requests[irequest]->size = 1;
417
418 // need a full neighbor list
419 // int irequest = neighbor->request(this);
420 // neighbor->requests[irequest]->half = 0;
421 // neighbor->requests[irequest]->full = 1;
422
423 // set maxrad_dynamic and maxrad_frozen for each type
424 // include future Fix pour particles as dynamic
425
426 for (i = 1; i <= atom->ntypes; i++)
427 onerad_dynamic[i] = onerad_frozen[i] = 0.0;
428
429 double *radius = atom->radius;
430 int *type = atom->type;
431 int nlocal = atom->nlocal;
432
433 for (i = 0; i < nlocal; i++) {
434 onerad_dynamic[type[i]] = MAX(onerad_dynamic[type[i]], radius[i]);
435 }
436
437 MPI_Allreduce(&onerad_dynamic[1], &maxrad_dynamic[1], atom->ntypes, MPI_DOUBLE, MPI_MAX, world);
438 MPI_Allreduce(&onerad_frozen[1], &maxrad_frozen[1], atom->ntypes, MPI_DOUBLE, MPI_MAX, world);
439 }
440
441 /* ----------------------------------------------------------------------
442 neighbor callback to inform pair style of neighbor list to use
443 optional granular history list
444 ------------------------------------------------------------------------- */
445
init_list(int id,NeighList * ptr)446 void PairTriSurf::init_list(int id, NeighList *ptr) {
447 if (id == 0)
448 list = ptr;
449 }
450
451 /* ----------------------------------------------------------------------
452 memory usage of local atom-based arrays
453 ------------------------------------------------------------------------- */
454
memory_usage()455 double PairTriSurf::memory_usage() {
456
457 return 0.0;
458 }
459
460 /*
461 * distance between triangle and point
462 */
463 /*
464 function [dist,PP0] = pointTriangleDistance(TRI,P)
465 % calculate distance between a point and a triangle in 3D
466 % SYNTAX
467 % dist = pointTriangleDistance(TRI,P)
468 % [dist,PP0] = pointTriangleDistance(TRI,P)
469 %
470 % DESCRIPTION
471 % Calculate the distance of a given point P from a triangle TRI.
472 % Point P is a row vector of the form 1x3. The triangle is a matrix
473 % formed by three rows of points TRI = [P1;P2;P3] each of size 1x3.
474 % dist = pointTriangleDistance(TRI,P) returns the distance of the point P
475 % to the triangle TRI.
476 % [dist,PP0] = pointTriangleDistance(TRI,P) additionally returns the
477 % closest point PP0 to P on the triangle TRI.
478 %
479 % Author: Gwendolyn Fischer
480 % Release: 1.0
481 % Release date: 09/02/02
482 % Release: 1.1 Fixed Bug because of normalization
483 % Release: 1.2 Fixed Bug because of typo in region 5 20101013
484 % Release: 1.3 Fixed Bug because of typo in region 2 20101014
485
486 % Possible extension could be a version tailored not to return the distance
487 % and additionally the closest point, but instead return only the closest
488 % point. Could lead to a small speed gain.
489
490 % Example:
491 % %% The Problem
492 % P0 = [0.5 -0.3 0.5];
493 %
494 % P1 = [0 -1 0];
495 % P2 = [1 0 0];
496 % P3 = [0 0 0];
497 %
498 % vertices = [P1; P2; P3];
499 % faces = [1 2 3];
500 %
501 % %% The Engine
502 % [dist,PP0] = pointTriangleDistance([P1;P2;P3],P0);
503 %
504 % %% Visualization
505 % [x,y,z] = sphere(20);
506 % x = dist*x+P0(1);
507 % y = dist*y+P0(2);
508 % z = dist*z+P0(3);
509 %
510 % figure
511 % hold all
512 % patch('Vertices',vertices,'Faces',faces,'FaceColor','r','FaceAlpha',0.8);
513 % plot3(P0(1),P0(2),P0(3),'b*');
514 % plot3(PP0(1),PP0(2),PP0(3),'*g')
515 % surf(x,y,z,'FaceColor','b','FaceAlpha',0.3)
516 % view(3)
517
518 % The algorithm is based on
519 % "David Eberly, 'Distance Between Point and Triangle in 3D',
520 % Geometric Tools, LLC, (1999)"
521 % https://www.geometrictools.com/Documentation/DistancePoint3Triangle3.pdf
522 %
523 % ^t
524 % \ |
525 % \reg2|
526 % \ |
527 % \ |
528 % \ |
529 % \|
530 % *P2
531 % |\
532 % | \
533 % reg3 | \ reg1
534 % | \
535 % |reg0\
536 % | \
537 % | \ P1
538 % -------*-------*------->s
539 % |P0 \
540 % reg4 | reg5 \ reg6
541 */
542
543 //void PairTriSurf::PointTriangleDistance(const Vector3d P, const Vector3d TRI1, const Vector3d TRI2, const Vector3d TRI3,
544 // Vector3d &CP, double &dist) {
545 //
546 // Vector3d B, E0, E1, D;
547 // double a, b, c, d, e, f;
548 // double det, s, t, sqrDistance, tmp0, tmp1, numer, denom, invDet;
549 //
550 // // rewrite triangle in normal form
551 // B = TRI1;
552 // E0 = TRI2 - B;
553 // E1 = TRI3 - B;
554 //
555 // D = B - P;
556 // a = E0.dot(E0);
557 // b = E0.dot(E1);
558 // c = E1.dot(E1);
559 // d = E0.dot(D);
560 // e = E1.dot(D);
561 // f = D.dot(D);
562 //
563 // det = a * c - b * b;
564 // //% do we have to use abs here?
565 // s = b * e - c * d;
566 // t = b * d - a * e;
567 //
568 // //% Terible tree of conditionals to determine in which region of the diagram
569 // //% shown above the projection of the point into the triangle-plane lies.
570 // if ((s + t) <= det) {
571 // if (s < 0) {
572 // if (t < 0) {
573 // // %region4
574 // if (d < 0) {
575 // t = 0;
576 // if (-d >= a) {
577 // s = 1;
578 // sqrDistance = a + 2 * d + f;
579 // } else {
580 // s = -d / a;
581 // sqrDistance = d * s + f;
582 // }
583 // } else {
584 // s = 0;
585 // if (e >= 0) {
586 // t = 0;
587 // sqrDistance = f;
588 // } else {
589 // if (-e >= c) {
590 // t = 1;
591 // sqrDistance = c + 2 * e + f;
592 // } else {
593 // t = -e / c;
594 // sqrDistance = e * t + f;
595 // }
596 // }
597 // }
598 // // end % of region 4
599 // } else {
600 // // % region 3
601 // s = 0;
602 // if (e >= 0) {
603 // t = 0;
604 // sqrDistance = f;
605 // } else {
606 // if (-e >= c) {
607 // t = 1;
608 // sqrDistance = c + 2 * e + f;
609 // } else {
610 // t = -e / c;
611 // sqrDistance = e * t + f;
612 // }
613 // }
614 // }
615 // // end of region 3
616 // } else {
617 // if (t < 0) {
618 // //% region 5
619 // t = 0;
620 // if (d >= 0) {
621 // s = 0;
622 // sqrDistance = f;
623 // } else {
624 // if (-d >= a) {
625 // s = 1;
626 // sqrDistance = a + 2 * d + f;
627 // } else {
628 // s = -d / a;
629 // sqrDistance = d * s + f;
630 // }
631 // }
632 // } else {
633 // // region 0
634 // invDet = 1 / det;
635 // s = s * invDet;
636 // t = t * invDet;
637 // sqrDistance = s * (a * s + b * t + 2 * d) + t * (b * s + c * t + 2 * e) + f;
638 // }
639 // }
640 // } else {
641 // if (s < 0) {
642 // // % region 2
643 // tmp0 = b + d;
644 // tmp1 = c + e;
645 // if (tmp1 > tmp0) { //% minimum on edge s+t=1
646 // numer = tmp1 - tmp0;
647 // denom = a - 2 * b + c;
648 // if (numer >= denom) {
649 // s = 1;
650 // t = 0;
651 // sqrDistance = a + 2 * d + f;
652 // } else {
653 // s = numer / denom;
654 // t = 1 - s;
655 // sqrDistance = s * (a * s + b * t + 2 * d) + t * (b * s + c * t + 2 * e) + f;
656 // }
657 // } else
658 // // % minimum on edge s=0
659 // s = 0;
660 // if (tmp1 <= 0) {
661 // t = 1;
662 // sqrDistance = c + 2 * e + f;
663 // } else {
664 // if (e >= 0) {
665 // t = 0;
666 // sqrDistance = f;
667 // } else {
668 // t = -e / c;
669 // sqrDistance = e * t + f;
670 // }
671 // }
672 // } //end % of region 2
673 // else {
674 // if (t < 0) {
675 // // %region6
676 // tmp0 = b + e;
677 // tmp1 = a + d;
678 // if (tmp1 > tmp0) {
679 // numer = tmp1 - tmp0;
680 // denom = a - 2 * b + c;
681 // if (numer >= denom) {
682 // t = 1;
683 // s = 0;
684 // sqrDistance = c + 2 * e + f;
685 // } else {
686 // t = numer / denom;
687 // s = 1 - t;
688 // sqrDistance = s * (a * s + b * t + 2 * d) + t * (b * s + c * t + 2 * e) + f;
689 // }
690 // } else {
691 // t = 0;
692 // if (tmp1 <= 0) {
693 // s = 1;
694 // sqrDistance = a + 2 * d + f;
695 // } else {
696 // if (d >= 0) {
697 // s = 0;
698 // sqrDistance = f;
699 // } else {
700 // s = -d / a;
701 // sqrDistance = d * s + f;
702 // }
703 // }
704 // } // % end region 6
705 // } else {
706 // //% region 1
707 // numer = c + e - b - d;
708 // if (numer <= 0) {
709 // s = 0;
710 // t = 1;
711 // sqrDistance = c + 2 * e + f;
712 // } else {
713 // denom = a - 2 * b + c;
714 // if (numer >= denom) {
715 // s = 1;
716 // t = 0;
717 // sqrDistance = a + 2 * d + f;
718 // } else {
719 // s = numer / denom;
720 // t = 1 - s;
721 // sqrDistance = s * (a * s + b * t + 2 * d) + t * (b * s + c * t + 2 * e) + f;
722 // }
723 // } //% end of region 1
724 // }
725 // }
726 // }
727 //
728 // // % account for numerical round-off error
729 // if (sqrDistance < 0) {
730 // sqrDistance = 0;
731 // }
732 //
733 // dist = sqrt(sqrDistance);
734 //
735 // // closest point
736 // CP = B + s * E0 + t * E1;
737 //
738 //}
739 /*
740 * % The algorithm is based on
741 % "David Eberly, 'Distance Between Point and Triangle in 3D',
742 % Geometric Tools, LLC, (1999)"
743 % https://www.geometrictools.com/Documentation/DistancePoint3Triangle3.pdf
744 */
745
PointTriangleDistance(const Vector3d sourcePosition,const Vector3d TRI0,const Vector3d TRI1,const Vector3d TRI2,Vector3d & CP,double & dist)746 void PairTriSurf::PointTriangleDistance(const Vector3d sourcePosition, const Vector3d TRI0, const Vector3d TRI1,
747 const Vector3d TRI2, Vector3d &CP, double &dist) {
748
749 Vector3d edge0 = TRI1 - TRI0;
750 Vector3d edge1 = TRI2 - TRI0;
751 Vector3d v0 = TRI0 - sourcePosition;
752
753 double a = edge0.dot(edge0);
754 double b = edge0.dot(edge1);
755 double c = edge1.dot(edge1);
756 double d = edge0.dot(v0);
757 double e = edge1.dot(v0);
758
759 double det = a * c - b * b;
760 double s = b * e - c * d;
761 double t = b * d - a * e;
762
763 if (s + t < det) {
764 if (s < 0.f) {
765 if (t < 0.f) {
766 if (d < 0.f) {
767 s = clamp(-d / a, 0.f, 1.f);
768 t = 0.f;
769 } else {
770 s = 0.f;
771 t = clamp(-e / c, 0.f, 1.f);
772 }
773 } else {
774 s = 0.f;
775 t = clamp(-e / c, 0.f, 1.f);
776 }
777 } else if (t < 0.f) {
778 s = clamp(-d / a, 0.f, 1.f);
779 t = 0.f;
780 } else {
781 float invDet = 1.f / det;
782 s *= invDet;
783 t *= invDet;
784 }
785 } else {
786 if (s < 0.f) {
787 float tmp0 = b + d;
788 float tmp1 = c + e;
789 if (tmp1 > tmp0) {
790 float numer = tmp1 - tmp0;
791 float denom = a - 2 * b + c;
792 s = clamp(numer / denom, 0.f, 1.f);
793 t = 1 - s;
794 } else {
795 t = clamp(-e / c, 0.f, 1.f);
796 s = 0.f;
797 }
798 } else if (t < 0.f) {
799 if (a + d > b + e) {
800 float numer = c + e - b - d;
801 float denom = a - 2 * b + c;
802 s = clamp(numer / denom, 0.f, 1.f);
803 t = 1 - s;
804 } else {
805 s = clamp(-e / c, 0.f, 1.f);
806 t = 0.f;
807 }
808 } else {
809 float numer = c + e - b - d;
810 float denom = a - 2 * b + c;
811 s = clamp(numer / denom, 0.f, 1.f);
812 t = 1.f - s;
813 }
814 }
815
816 CP = TRI0 + s * edge0 + t * edge1;
817 dist = (CP - sourcePosition).norm();
818
819 }
820
clamp(const double a,const double min,const double max)821 double PairTriSurf::clamp(const double a, const double min, const double max) {
822 if (a < min) {
823 return min;
824 } else if (a > max) {
825 return max;
826 } else {
827 return a;
828 }
829 }
830
extract(const char * str,int &)831 void *PairTriSurf::extract(const char *str, int &/*i*/) {
832 //printf("in PairTriSurf::extract\n");
833 if (strcmp(str, "smd/tri_surface/stable_time_increment_ptr") == 0) {
834 return (void *) &stable_time_increment;
835 }
836
837 return nullptr;
838
839 }
840