v0.13.0
RigidBodies.hpp
Go to the documentation of this file.
1 /* This file is part of MoFEM.
2  * MoFEM is free software: you can redistribute it and/or modify it under
3  * the terms of the GNU Lesser General Public License as published by the
4  * Free Software Foundation, either version 3 of the License, or (at your
5  * option) any later version.
6  *
7  * MoFEM is distributed in the hope that it will be useful, but WITHOUT
8  * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
9  * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public
10  * License for more details.
11  *
12  * You should have received a copy of the GNU Lesser General Public
13  * License along with MoFEM. If not, see <http://www.gnu.org/licenses/>. */
14 
15 #pragma once
16 
24  STL,
26  LASTBODY
27 };
28 
29 struct RigidBodyData {
30  const int iD;
37  double gAp;
44  Tensor2<double, 3, 3> rotationMat;
45  Tensor2<double, 3, 3> diffNormal;
46 
48 
49  int bodyType;
50  array<double, 9> dataForTags;
51 
52  RigidBodyData(VectorDouble c_coords, VectorDouble roller_disp, int id)
53  : originCoords(c_coords), rollerDisp(roller_disp), iD(id),
54  defaultOrientation(0., 0., 1.), oRientation(0., 0., 1.) {
56  BodyDispScaled.clear();
58  std::fill(dataForTags.begin(), dataForTags.end(), 0);
59  bodyType = PLANE;
60  }
61 
62  RigidBodyData() = delete;
63  virtual ~RigidBodyData() {}
64 
65  using TPack3 = PackPtr<double *, 3>; // t_coords
66  using TPack1 = PackPtr<double *, 1>; // t_disp
67 
69  Tensor1<TPack1, 3> &t_disp) = 0;
71  Tensor1<double, 3> &t_disp) = 0;
73  Tensor1<double, 3> &t_disp) = 0;
74 
75  virtual Tensor2<double, 3, 3> getDiffNormal(Tensor1<TPack3, 3> &t_coords,
76  Tensor1<TPack1, 3> &t_disp,
77  Tensor1<TPack1, 3> &t_normal) = 0;
78 
79  virtual double getGap(Tensor1<TPack3, 3> &t_coords) = 0;
81  Tensor1<TPack1, 3> &t_normal) = 0;
82 
84 
86  // https://math.stackexchange.com/questions/180418
88  oRientation.normalize();
91  const double s = v.l2();
92  if (abs(s) < 1e-16)
94  const double c = defaultOrientation(i) * oRientation(i);
95  const double coef = 1. / (1. + c);
96  Tensor2<double, 3, 3> t_v;
97  t_v(i, j) = levi_civita(i, j, k) * v(k);
98  rotationMat(i, j) += t_v(i, j) + (coef * t_v(i, k)) * t_v(k, j);
99  // Tensor1<double, 3> test;
100  // test(i) = rotationMat(j, i) * defaultOrientation(j);
101 
103  };
104 
107  return Tensor1<double, 3>(pos(0), pos(1), pos(2));
108  };
109 
110  template <typename T1, typename T2>
112  Tensor1<T2, 3> &t_disp) {
113  auto t_off = getBodyOffset();
114  pointCoords(i) = t_coords(i) - t_off(i) + t_disp(i);
115  return pointCoords;
116  };
117 
119  EntityHandle &vertex) {
121 
122  Tag tag0;
123  int def_int = 0;
124  std::array<double, 9> def_real;
125  std::fill(def_real.begin(), def_real.end(), 0);
126 
128 
129  CHKERR moab_debug.tag_get_handle("_ROLLER_ID", 1, MB_TYPE_INTEGER, tag0,
130  MB_TAG_CREAT | MB_TAG_SPARSE, &def_int);
131  CHKERR moab_debug.tag_set_data(tag0, &vertex, 1, &this->iD);
132 
133  CHKERR moab_debug.tag_get_handle("_BODY_TYPE", 1, MB_TYPE_INTEGER, tag0,
134  MB_TAG_CREAT | MB_TAG_SPARSE, &def_int);
135  CHKERR moab_debug.tag_set_data(tag0, &vertex, 1, &this->bodyType);
136 
137  CHKERR moab_debug.tag_get_handle("_ORIENTATION", 3, MB_TYPE_DOUBLE, tag0,
138  MB_TAG_CREAT | MB_TAG_SPARSE, &def_real);
139  CHKERR moab_debug.tag_set_data(tag0, &vertex, 1, &this->oRientation(0));
140 
141  CHKERR moab_debug.tag_get_handle("_ROLLER_DATA", 9, MB_TYPE_DOUBLE, tag0,
142  MB_TAG_CREAT | MB_TAG_SPARSE, &def_real);
143  CHKERR moab_debug.tag_set_data(tag0, &vertex, 1, this->dataForTags.data());
144 
145 
146  // tags
147  // int roller_id
148  // int body_type
149  // vec3 orientation
150  // tensor9 roller_data
151  // 0 radius1 = 5
152  // 1 inner_radius = 1
153  // 2 height = 1
154  // 3 angle = 45
155  // 4 angle_a = 45
156  // 5 angle_b = 45
157  //
158 
160  }
162 };
163 
164 struct PlaneRigidBody : public RigidBodyData {
165  PetscBool fLip;
166  PlaneRigidBody(VectorDouble c_coords, VectorDouble roller_disp, int id)
167  : RigidBodyData(c_coords, roller_disp, id), fLip(PETSC_FALSE) {}
168 
171  bodyType = PLANE;
173  }
174 
176  Tensor1<TPack1, 3> &t_disp) {
177  return getNormalImpl(t_coords, t_disp);
178  }
180  Tensor1<double, 3> &t_disp) {
181  return getNormalImpl(t_coords, t_disp);
182  }
184  Tensor1<double, 3> &t_disp) {
185  return getNormalImpl(t_coords, t_disp);
186  }
187 
188  Tensor2<double, 3, 3> getDiffNormal(Tensor1<TPack3, 3> &t_coords,
189  Tensor1<TPack1, 3> &t_disp,
190  Tensor1<TPack1, 3> &t_normal) {
191  return getDiffNormalImpl(t_coords, t_disp, t_normal);
192  }
193 
194  inline double getGap(Tensor1<TPack3, 3> &t_coords) {
195  return getGapImpl(t_coords);
196  }
197 
199  Tensor1<TPack1, 3> &t_normal) {
200  return getdGapImpl(t_coords, t_normal);
201  }
202 
203  template <typename T1, typename T2>
205  Tensor1<T2, 3> &t_disp) {
206  auto t_d = getPointCoords(t_coords, t_disp);
207  // FIXME:
208  // tNormal(i) = rotationMat(j, i) * defaultOrientation(j);
209  tNormal(i) = oRientation(i);
210  if (fLip)
211  tNormal(i) *= -1;
212  // rotationMat(i, j)
213  return tNormal;
214  };
215 
216  template <typename T1> double getGapImpl(Tensor1<T1, 3> &t_coords) {
217  // test(i) = rotationMat(j, i) * t_off(j);
218  // t_d2(i) = t_coords(i) - rotationMat(j, i) * t_off(j);
219  gAp = tNormal(i) * pointCoords(i);
220  return gAp;
221  };
222 
223  template <typename T1, typename T2, typename T3>
224  inline Tensor2<double, 3, 3> getDiffNormalImpl(Tensor1<T1, 3> &t_coords,
225  Tensor1<T2, 3> &t_disp,
226  Tensor1<T3, 3> &t_normal) {
227  const Tensor2<double, 3, 3> t_diff_norm(0., 0., 0., 0., 0., 0., 0., 0., 0.);
228  return t_diff_norm;
229  };
230 
231  template <typename T1, typename T2>
233  Tensor1<T2, 3> &t_normal) {
234  dGap(i) = tNormal(i);
235  return dGap;
236  };
237 
240  PetscBool rflg;
241  PetscInt nb_dirs = 3;
242  CHKERR PetscOptionsBegin(PETSC_COMM_WORLD, "", "", "");
243  string param = "-direction" + to_string(iD);
244  CHKERR PetscOptionsGetRealArray(NULL, NULL, param.c_str(), &oRientation(0),
245  &nb_dirs, &rflg);
246  param = "-flip" + to_string(iD);
247  CHKERR PetscOptionsBool(param.c_str(), "flip the plane normal", "", fLip,
248  &fLip, PETSC_NULL);
249  if (rflg)
251  ierr = PetscOptionsEnd();
252  CHKERRG(ierr);
254  }
255 };
256 
258  double rAdius;
259  SphereRigidBody(VectorDouble c_coords, VectorDouble roller_disp, int id)
260  : RigidBodyData(c_coords, roller_disp, id), rAdius(1) {}
261 
264  bodyType = SPHERE;
265  dataForTags[0] = rAdius;
267  }
268 
270  Tensor1<TPack1, 3> &t_disp) {
271  return getNormalImpl(t_coords, t_disp);
272  }
274  Tensor1<double, 3> &t_disp) {
275  return getNormalImpl(t_coords, t_disp);
276  }
278  Tensor1<double, 3> &t_disp) {
279  return getNormalImpl(t_coords, t_disp);
280  }
281 
282  Tensor2<double, 3, 3> getDiffNormal(Tensor1<TPack3, 3> &t_coords,
283  Tensor1<TPack1, 3> &t_disp,
284  Tensor1<TPack1, 3> &t_normal) {
285  return getDiffNormalImpl(t_coords, t_disp, t_normal);
286  }
287 
288  inline double getGap(Tensor1<TPack3, 3> &t_coords) {
289  return getGapImpl(t_coords);
290  }
291 
293  Tensor1<TPack1, 3> &t_normal) {
294  return getdGapImpl(t_coords, t_normal);
295  }
296 
297  template <typename T1, typename T2>
299  Tensor1<T2, 3> &t_disp) {
300  auto t_d = getPointCoords(t_coords, t_disp);
301  tNormal(i) = t_d(i);
302  tNormal.normalize();
303 
304  return tNormal;
305  };
306 
307  template <typename T1, typename T2, typename T3>
308  inline Tensor2<double, 3, 3> getDiffNormalImpl(Tensor1<T1, 3> &t_coords,
309  Tensor1<T2, 3> &t_disp,
310  Tensor1<T3, 3> &t_normal) {
311 
312  auto t_d = getPointCoords(t_coords, t_disp);
313 
314  const double norm = t_d.l2();
315  const double norm3 = norm * norm * norm;
316  diffNormal(i, j) =
317  kronecker_delta(i, j) * (1. / norm) - (1. / norm3) * t_d(i) * t_d(j);
318 
319  return diffNormal;
320  };
321 
322  template <typename T1> inline double getGapImpl(Tensor1<T1, 3> &t_coords) {
323 
324  gAp = tNormal(i) * (pointCoords(i) - tNormal(i) * rAdius);
325  return gAp;
326  };
327 
328  template <typename T1, typename T2>
330  Tensor1<T2, 3> &t_normal) {
331  // d gap / d normal
332  dGap(i) =
333  pointCoords(j) * diffNormal(i, j) +
334  tNormal(j) * (kronecker_delta(i, j) - 2. * rAdius * diffNormal(i, j));
335  return dGap;
336  };
337 
340 
341  CHKERR PetscOptionsBegin(PETSC_COMM_WORLD, "", "", "");
342 
343  string param = "-radius" + to_string(iD);
344  CHKERR PetscOptionsScalar(param.c_str(), "set roller radius", "", rAdius,
345  &rAdius, PETSC_NULL);
346  ierr = PetscOptionsEnd();
347  CHKERRG(ierr);
348 
350  }
351 };
352 
354  double rAdius;
355  double hEight;
356  CylinderRigidBody(VectorDouble c_coords, VectorDouble roller_disp, int id)
357  : RigidBodyData(c_coords, roller_disp, id), rAdius(1), hEight(INFINITY) {}
358 
361  bodyType = CYLINDER;
362  dataForTags[0] = rAdius;
363  dataForTags[2] = hEight;
365  }
366 
368  Tensor1<TPack1, 3> &t_disp) {
369  return getNormalImpl(t_coords, t_disp);
370  }
372  Tensor1<double, 3> &t_disp) {
373  return getNormalImpl(t_coords, t_disp);
374  }
376  Tensor1<double, 3> &t_disp) {
377  return getNormalImpl(t_coords, t_disp);
378  }
379 
380  Tensor2<double, 3, 3> getDiffNormal(Tensor1<TPack3, 3> &t_coords,
381  Tensor1<TPack1, 3> &t_disp,
382  Tensor1<TPack1, 3> &t_normal) {
383  return getDiffNormalImpl(t_coords, t_disp, t_normal);
384  }
385 
386  inline double getGap(Tensor1<TPack3, 3> &t_coords) {
387  return getGapImpl(t_coords);
388  }
389 
391  Tensor1<TPack1, 3> &t_normal) {
392  return getdGapImpl(t_coords, t_normal);
393  }
394 
395  template <typename T1, typename T2>
397  Tensor1<T2, 3> &t_disp) {
398  auto t_d = getPointCoords(t_coords, t_disp);
399  pointCoords(i) = rotationMat(i, j) * t_d(j);
400  Tensor1<double, 3> c_center(0., 0., 0.);
401  Tensor1<double, 3> q_minus_c(0., 0., 0.);
402  Tensor1<double, 3> K(0., 0., 0.);
403  auto Q = pointCoords;
404  auto P_Q = pointCoords;
405  auto Q_K = pointCoords;
406 
407  // https://www.geometrictools.com/Documentation/DistanceToCircle3.pdf
408  const double half_height = hEight / 2.;
409  const double rad2 = rAdius * rAdius;
410  tNormal(i) = 0.;
411 
412  if (pointCoords(2) > half_height) {
413  tNormal(2) = 1;
414  if (pow(pointCoords(1), 2) + pow(pointCoords(0), 2) < rad2) {
415  gAp = tNormal(i) * pointCoords(i) - half_height;
416  } else {
417  c_center(2) = half_height;
418  Q(2) = c_center(2); // Q
419  q_minus_c(i) = Q(i) - c_center(i);
420  q_minus_c.normalize();
421  K(i) = c_center(i) + rAdius * q_minus_c(i);
422  P_Q(i) = pointCoords(i) - Q(i);
423  Q_K(i) = Q(i) - K(i);
424  gAp = sqrt(P_Q.l2() * P_Q.l2() + Q_K.l2() * Q_K.l2());
425  }
426  } else if (pointCoords(2) < -half_height) {
427  tNormal(2) = -1;
428  if (pow(pointCoords(1), 2) + pow(pointCoords(0), 2) < rad2) {
429  gAp = tNormal(i) * pointCoords(i) - half_height;
430  } else {
431  c_center(2) = -half_height;
432  Q(2) = c_center(2); // Q
433  q_minus_c(i) = Q(i) - c_center(i);
434  q_minus_c.normalize();
435  K(i) = c_center(i) + rAdius * q_minus_c(i);
436  P_Q(i) = pointCoords(i) - Q(i);
437  Q_K(i) = Q(i) - K(i);
438  gAp = sqrt(P_Q.l2() * P_Q.l2() + Q_K.l2() * Q_K.l2());
439  }
440  } else {
441  pointCoords(2) = 0;
442  tNormal(i) = pointCoords(i);
443  tNormal.normalize();
444  gAp = tNormal(i) * (pointCoords(i) - tNormal(i) * rAdius);
445  }
446 
447  // auto length = [&](auto v) { return sqrt(v(0) * v(0) + v(1) * v(1)); };
448 
449  // auto sd_cylinder = [&](Tensor1<double, 3> p) {
450  // typedef Tensor1<double, 2> vec2;
451  // Index<'i', 2> i;
452  // vec2 d(length(p) - rAdius, abs(p(2)) - half_height);
453  // vec2 mx_d(max(d(0), 0.0), max(d(1), 0.0));
454  // return min(max(d(0), d(1)), 0.0) + length(mx_d);
455  // };
456 
457  // const double eps = 1e-6;
458  // auto cyl_distance = sd_cylinder(pointCoords);
459  // gAp = cyl_distance;
460 
461  // auto get_d = [&](int a) {
462  // auto pt = pointCoords;
463  // pt(a) += eps;
464  // return (sd_cylinder(pt) - cyl_distance) / eps;
465  // };
466 
467  // for (int dd = 0; dd != 3; ++dd)
468  // tNormal(dd) = get_d(dd);
469 
470  // for debugging
471  // auto my_rand_mn = [](double M, double N) {
472  // return M + (rand() / (RAND_MAX / (N - M)));
473  // };
474  // auto t_off = getBodyOffset();
475  // std::ofstream afile("cylinder_pts.csv", std::ios::ate);
476  // if (afile.is_open()) {
477  // afile << "X,Y,Z\n";
478  // Tensor1<double, 3> p(0, 0, 0);
479  // for (int n = 0; n != 1e6; n++) {
480  // double x = my_rand_mn(-300, 300);
481  // double y = my_rand_mn(-300, 300);
482  // double z = my_rand_mn(-300, 300);
483  // Tensor1<double, 3> coords(x, y, z);
484  // if (sd_cylinder(coords) < 0) {
485 
486  // afile << x + t_off(0) << "," << y + t_off(1) << "," << z + t_off(2)
487  // << "\n";
488  // }
489  // }
490  // afile.close();
491  // }
492  // cout << "kill process " << endl;
493 
494  tNormal.normalize();
495 
496  auto normal_copy = tNormal;
497  tNormal(i) = rotationMat(j, i) * normal_copy(j);
498 
499  return tNormal;
500  };
501 
502  template <typename T1, typename T2, typename T3>
503  inline Tensor2<double, 3, 3> getDiffNormalImpl(Tensor1<T1, 3> &t_coords,
504  Tensor1<T2, 3> &t_disp,
505  Tensor1<T3, 3> &t_normal) {
506 
507  Tensor2<double, 3, 3> diff_normal;
508  Tensor1<double, 3> norm_diff;
509  const double clean_gap = gAp;
510  const double eps = 1e-8;
511  for (int ii = 0; ii != 3; ++ii) {
512  Tensor1<double, 3> pert_disp{t_disp(0), t_disp(1), t_disp(2)};
513  pert_disp(ii) += eps;
514  auto pert_normal = getNormal(t_coords, pert_disp);
515  norm_diff(i) = (pert_normal(i) - t_normal(i)) / eps;
516  dGap(ii) = (gAp - clean_gap) / eps;
517 
518  for (int jj = 0; jj != 3; ++jj) {
519  diff_normal(jj, ii) = norm_diff(jj);
520  }
521  }
522  return diff_normal;
523  };
524 
525  template <typename T1> inline double getGapImpl(Tensor1<T1, 3> &t_coords) {
526  // gAp = tNormal(i) * (pointCoords(i) - tNormal(i) * rAdius);
527  return gAp;
528  };
529 
530  template <typename T1, typename T2>
532  Tensor1<T2, 3> &t_normal) {
533  // dGap(i) =
534  // pointCoords(j) * diffNormal(i, j) +
535  // tNormal(j) * (kronecker_delta(i, j) - 2. * rAdius * diffNormal(i,
536  // j));
537  return dGap;
538  };
539 
542  PetscBool rflg;
543  PetscInt nb_dirs = 3;
544  CHKERR PetscOptionsBegin(PETSC_COMM_WORLD, "", "", "");
545  string param = "-direction" + to_string(iD);
546  CHKERR PetscOptionsGetRealArray(NULL, NULL, param.c_str(), &oRientation(0),
547  &nb_dirs, &rflg);
548  param = "-radius" + to_string(iD);
549  CHKERR PetscOptionsScalar(param.c_str(), "set roller radius", "", rAdius,
550  &rAdius, PETSC_NULL);
551  param = "-height" + to_string(iD);
552  CHKERR PetscOptionsScalar(param.c_str(), "set cylinder height", "", hEight,
553  &hEight, PETSC_NULL);
554  if (rflg)
556  ierr = PetscOptionsEnd();
557  CHKERRG(ierr);
559  }
560 };
561 
562 struct TorusRigidBody : public RigidBodyData {
563  // shared_ptr<ArbitrarySurfaceData> cP;
564  double rAdius;
565  double innerRadius;
566  TorusRigidBody(VectorDouble c_coords, VectorDouble roller_disp, int id)
567  : RigidBodyData(c_coords, roller_disp, id), rAdius(1), innerRadius(0.5) {
568  // , cP(R, r, 1)
569  // compute rotation matrix here
570  }
571 
574  bodyType = TORUS;
575  dataForTags[0] = rAdius;
578  }
579 
581  Tensor1<TPack1, 3> &t_disp) {
582  return getNormalImpl(t_coords, t_disp);
583  }
585  Tensor1<double, 3> &t_disp) {
586  return getNormalImpl(t_coords, t_disp);
587  }
589  Tensor1<double, 3> &t_disp) {
590  return getNormalImpl(t_coords, t_disp);
591  }
592 
593  Tensor2<double, 3, 3> getDiffNormal(Tensor1<TPack3, 3> &t_coords,
594  Tensor1<TPack1, 3> &t_disp,
595  Tensor1<TPack1, 3> &t_normal) {
596  return getDiffNormalImpl(t_coords, t_disp, t_normal);
597  }
598 
599  inline double getGap(Tensor1<TPack3, 3> &t_coords) {
600  return getGapImpl(t_coords);
601  }
603  Tensor1<TPack1, 3> &t_normal) {
604  return getdGapImpl(t_coords, t_normal);
605  }
606 
607  template <typename T1, typename T2>
609  Tensor1<T2, 3> &t_disp) {
610 
611  auto t_d = getPointCoords(t_coords, t_disp);
612  pointCoords(i) = rotationMat(i, j) * t_d(j);
613  t_d(i) = pointCoords(i);
614 
615  // cP->tt = 1;
616  // cP->rr = 1.;
617  // cP->R1 = 0;
618  // cP->PrintTorusDebug();
619  // cP->PrintTorusAnimDebug();
620 
621  // auto closest_pt_toroid = cP->getClosestPointToToroid(t_d);
622  // closestPoint(i) = closest_pt_toroid(i);
623 
624  // tNormal(j) = t_d(j) - closestPoint(j);
625  // if (cP->isPointInsideTorus(t_d))
626  // tNormal(i) *= -1;
627  // tNormal.normalize();
628 
629  // gAp = tNormal(i) * (t_d(i) - closestPoint(i));
630 
631  // https: // iquilezles.org/www/articles/distfunctions/distfunctions.htm
632  const double x = t_d(0);
633  const double y = t_d(1);
634  const double z = t_d(2);
635  const double RR = rAdius - innerRadius;
636  const double prod = sqrt(pow(x, 2) + pow(y, 2));
637  const double prod2 = sqrt(pow(-RR + prod, 2) + pow(z, 2));
638  gAp = -innerRadius + prod2;
639  tNormal(0) = (x * (-RR + prod)) / (prod * prod2);
640  tNormal(1) = (y * (-RR + prod)) / (prod * prod2);
641  tNormal(2) = z / prod2;
642  tNormal.normalize();
643 
644  auto normal_copy = tNormal;
645  tNormal(i) = rotationMat(j, i) * normal_copy(j);
646 
647  return tNormal;
648  };
649 
650  template <typename T1> inline double getGapImpl(Tensor1<T1, 3> &t_coords) {
651  return gAp;
652  };
653 
654  template <typename T1, typename T2, typename T3>
655  inline Tensor2<double, 3, 3> getDiffNormalImpl(Tensor1<T1, 3> &t_coords,
656  Tensor1<T2, 3> &t_disp,
657  Tensor1<T3, 3> &t_normal) {
658  Tensor2<double, 3, 3> diff_normal;
659  Tensor1<double, 3> norm_diff;
660  const double clean_gap = gAp;
661  const double eps = 1e-8;
662  for (int ii = 0; ii != 3; ++ii) {
663  Tensor1<double, 3> pert_disp{t_disp(0), t_disp(1), t_disp(2)};
664  pert_disp(ii) += eps;
665  auto pert_normal = getNormal(t_coords, pert_disp);
666  norm_diff(i) = (pert_normal(i) - t_normal(i)) / eps;
667  dGap(ii) = (gAp - clean_gap) / eps;
668 
669  for (int jj = 0; jj != 3; ++jj) {
670  diff_normal(jj, ii) = norm_diff(jj);
671  }
672  }
673  return diff_normal;
674  };
675 
676  template <typename T1, typename T2>
678  Tensor1<T2, 3> &t_normal) {
679  return dGap;
680  };
681 
684  PetscBool rflg;
685  int nb_dirs = 3;
686  CHKERR PetscOptionsBegin(PETSC_COMM_WORLD, "", "", "");
687  string param = "-inner_radius" + to_string(iD);
688  CHKERR PetscOptionsScalar(param.c_str(), "set torus small radius", "", innerRadius,
689  &innerRadius, PETSC_NULL);
690  param = "-radius" + to_string(iD);
691  CHKERR PetscOptionsScalar(param.c_str(), "set torus large Radius", "", rAdius,
692  &rAdius, PETSC_NULL);
693  param = "-direction" + to_string(iD);
694  CHKERR PetscOptionsGetRealArray(NULL, NULL, param.c_str(), &oRientation(0),
695  &nb_dirs, &rflg);
696  ierr = PetscOptionsEnd();
697  CHKERRG(ierr);
698 
699  if (rflg)
701 
702  // cP = make_shared<ArbitrarySurfaceData>(R, r);
703 
705  }
706 };
707 
709  // shared_ptr<ArbitrarySurfaceData> cP;
710  double angleA;
711  double angleB;
712  double rAdius;
713  double fIllet;
714  double torusRadius;
715  double hEight;
716 
717  double coneRadiusA;
719  double coneOffsetA;
720 
721  double coneRadiusB;
723  double coneOffsetB;
724 
725  RollerRigidBody(VectorDouble c_coords, VectorDouble roller_disp, int id)
726  : RigidBodyData(c_coords, roller_disp, id) {}
727 
730  bodyType = ROLLER;
731  dataForTags[0] = rAdius;
732  dataForTags[1] = fIllet;
733  dataForTags[2] = hEight;
734  dataForTags[4] = angleA;
735  dataForTags[5] = angleB;
736 
738  }
739 
741  Tensor1<TPack1, 3> &t_disp) {
742  return getNormalImpl(t_coords, t_disp);
743  }
745  Tensor1<double, 3> &t_disp) {
746  return getNormalImpl(t_coords, t_disp);
747  }
749  Tensor1<double, 3> &t_disp) {
750  return getNormalImpl(t_coords, t_disp);
751  }
752 
753  Tensor2<double, 3, 3> getDiffNormal(Tensor1<TPack3, 3> &t_coords,
754  Tensor1<TPack1, 3> &t_disp,
755  Tensor1<TPack1, 3> &t_normal) {
756  return getDiffNormalImpl(t_coords, t_disp, t_normal);
757  }
758 
759  inline double getGap(Tensor1<TPack3, 3> &t_coords) {
760  return getGapImpl(t_coords);
761  }
762 
764  Tensor1<TPack1, 3> &t_normal) {
765  return getdGapImpl(t_coords, t_normal);
766  }
767 
770  PetscBool rflg;
771  int nb_dirs = 3;
772 
773  CHKERR PetscOptionsBegin(PETSC_COMM_WORLD, "", "", "");
774  string param = "-radius" + to_string(iD);
775  CHKERR PetscOptionsScalar(param.c_str(), "set roller radius", "", rAdius,
776  &rAdius, PETSC_NULL);
777  param = "-fillet" + to_string(iD);
778  CHKERR PetscOptionsScalar(param.c_str(), "set torus small radius", "",
779  fIllet, &fIllet, PETSC_NULL);
780  param = "-angle_a" + to_string(iD);
781  CHKERR PetscOptionsScalar(param.c_str(), "set roller angle of attack", "",
782  angleA, &angleA, PETSC_NULL);
783  param = "-angle_b" + to_string(iD);
784  CHKERR PetscOptionsScalar(param.c_str(), "set roller back angle", "",
785  angleB, &angleB, PETSC_NULL);
786  hEight = 3 * fIllet;
787  param = "-height" + to_string(iD);
788  CHKERR PetscOptionsScalar(param.c_str(), "set roller height (optional)", "",
789  hEight, &hEight, PETSC_NULL);
790  param = "-direction" + to_string(iD);
791  CHKERR PetscOptionsGetRealArray(NULL, NULL, param.c_str(), &oRientation(0),
792  &nb_dirs, &rflg);
793  ierr = PetscOptionsEnd();
794  CHKERRG(ierr);
795  if (rflg)
797 
798  // cP = make_shared<ArbitrarySurfaceData>(rAdius, fIllet);
799 
801  angleA = std::tan(angleA * M_PI / 180);
802  angleB = std::tan(angleB * M_PI / 180);
803 
804  coneRadiusA = torusRadius + fIllet / sqrt(1 + angleA * angleA);
806  coneOffsetA = -fIllet * angleA / sqrt(1 + angleA * angleA) - hEight / 2.;
807 
808  coneTopRadiusB = torusRadius + fIllet / sqrt(1 + angleB * angleB);
810  coneOffsetB = fIllet * angleB / sqrt(1 + angleB * angleB) + hEight / 2.;
811 
813  }
814 
815  template <typename T1, typename T2>
817  Tensor1<T2, 3> &t_disp) {
818  auto t_d = getPointCoords(t_coords, t_disp);
819  pointCoords(i) = rotationMat(i, j) * t_d(j);
820  t_d(i) = pointCoords(i);
821 
822  const double inv_eps = 1e6;
823  array<double, 3> gaps;
824  const double half_height = hEight / 2.;
825 
826  double cone_apex_offset_a = coneOffsetA + half_height;
827  double cone_apex_offset_b = coneOffsetB - half_height;
828 
829  // https://computergraphics.stackexchange.com/questions/7485/glsl-shapes-signed-distance-field-implementation-explanation
830  auto sd_infinite_cone = [&](Tensor1<double, 3> p, double slope) {
831  double c1 = 1. / sqrt(1 + (slope * slope));
832  double c2 = c1 * slope;
833  auto prod = sqrt(p(0) * p(0) + p(1) * p(1));
834  auto prod2 = sqrt(1 + slope * slope);
835  tNormal(0) = p(0) / (prod2 * prod);
836  tNormal(1) = p(1) / (prod2 * prod);
837  tNormal(2) = slope / prod2;
838  return c1 * prod + c2 * p(2);
839  };
840 
841  const double full_h_a = -cone_apex_offset_a + coneRadiusA / angleA;
842  const double full_h_b = cone_apex_offset_b + coneTopRadiusB / angleB;
843 
844  Tensor1<double, 3> t_apx_1(t_d(0), t_d(1), t_d(2) - full_h_a);
845  Tensor1<double, 3> t_apx_2(t_d(0), t_d(1), t_d(2) + full_h_b);
846 
847  auto torus_dist = [&]() {
848  const double prod = sqrt(pow(t_d(0), 2) + pow(t_d(1), 2));
849  const double prod2 = sqrt(pow(-torusRadius + prod, 2) + pow(t_d(2), 2));
850  tNormal(0) = (t_d(0) * (-torusRadius + prod)) / (prod * prod2);
851  tNormal(1) = (t_d(1) * (-torusRadius + prod)) / (prod * prod2);
852  tNormal(2) = t_d(2) / prod2;
853  return -fIllet + prod2;
854  };
855 
856  gaps[0] = torus_dist();
857  gaps[1] = t_d(2) < -cone_apex_offset_a ? inv_eps
858  : sd_infinite_cone(t_apx_1, angleA);
859  gaps[2] = t_d(2) > -cone_apex_offset_b ? inv_eps
860  : sd_infinite_cone(t_apx_2, -angleB);
861 
862  int nb = distance(gaps.begin(), std::min_element(gaps.begin(), gaps.end()));
863  gAp = gaps[nb];
864 
865  switch (nb) {
866  case 0: {
867  torus_dist();
868  break;
869  }
870  case 1: {
871  // compute normal for cone A
872  sd_infinite_cone(t_apx_1, angleA);
873  break;
874  }
875  case 2: {
876  // compute normal for cone b
877  sd_infinite_cone(t_apx_2, -angleB);
878  } break;
879  }
880 
881  // // for debugging
882  // auto my_rand_mn = [](double M, double N) {
883  // return M + (rand() / (RAND_MAX / (N - M)));
884  // };
885  // auto t_off = getBodyOffset();
886  // std::ofstream afile("cone_pts.csv", std::ios::ate);
887  // if (afile.is_open()) {
888  // afile << "x,y,z\n";
889  // Tensor1<double, 3> p(0, 0, 0);
890  // for (int n = 0; n != 1e6; n++) {
891  // double x = my_rand_mn(-1.5, 1.5);
892  // double y = my_rand_mn(-1.5, 1.5);
893  // double z = my_rand_mn(-1, 1);
894  // Tensor1<double, 3> coords(x, y, z);
895 
896  // t_d(i) = coords(i);
897  // t_dc1(i) = coords(i);
898  // t_dc2(i) = coords(i);
899  // t_dc1(2) -= full_h_a;
900  // t_dc2(2) += full_h_b;
901 
902  // gaps[0] = torus_dist();
903  // gaps[1] = t_d(2) < -cone_apex_offset_a ? inv_eps :
904  // sd_infinite_cone(t_dc1, angleA); gaps[2] = t_d(2) >
905  // -cone_apex_offset_b ? inv_eps : sd_infinite_cone(t_dc2, -angleB);
906 
907  // if (gaps[0] < 0 || gaps[1] < 0 || gaps[2] < 0) {
908 
909  // afile << x + t_off(0) << "," << y + t_off(1) << "," << z + t_off(2)
910  // << "\n";
911  // }
912  // }
913  // afile.close();
914  // }
915 
916  tNormal.normalize();
917  auto normal_copy = tNormal;
918  tNormal(i) = rotationMat(j, i) * normal_copy(j);
919 
920  return tNormal;
921  };
922 
923  template <typename T1, typename T2, typename T3>
924  inline Tensor2<double, 3, 3> getDiffNormalImpl(Tensor1<T1, 3> &t_coords,
925  Tensor1<T2, 3> &t_disp,
926  Tensor1<T3, 3> &t_normal) {
927  Tensor2<double, 3, 3> diff_normal;
928  Tensor1<double, 3> norm_diff;
929  const double clean_gap = gAp;
930  const double eps = 1e-8;
931  for (int ii = 0; ii != 3; ++ii) {
932  Tensor1<double, 3> pert_disp{t_disp(0), t_disp(1), t_disp(2)};
933  pert_disp(ii) += eps;
934  auto pert_normal = getNormal(t_coords, pert_disp);
935  norm_diff(i) = (pert_normal(i) - t_normal(i)) / eps;
936  dGap(ii) = (gAp - clean_gap) / eps;
937 
938  for (int jj = 0; jj != 3; ++jj) {
939  diff_normal(jj, ii) = norm_diff(jj);
940  }
941  }
942  return diff_normal;
943  };
944 
945  template <typename T1> inline double getGapImpl(Tensor1<T1, 3> &t_coords) {
946  return gAp;
947  };
948 
949  template <typename T1, typename T2>
951  Tensor1<T2, 3> &t_normal) {
952  return dGap;
953  };
954 };
955 
956 struct ConeRigidBody : public RigidBodyData {
957  shared_ptr<ArbitrarySurfaceData> cP;
958  double sLope;
959  double hEight;
960  double rAdius;
961  double smallRadius;
962  double oFfset;
963 
964  double aNgle;
965  ConeRigidBody(VectorDouble c_coords, VectorDouble roller_disp, int id)
966  : RigidBodyData(c_coords, roller_disp, id) {
967  // , cP(R, r, 1)
968  sLope = 1;
969  hEight = 1;
970  rAdius = 1;
971  oFfset = 0;
972  // compute rotation matrix here
973  }
974 
977  bodyType = CONE;
978  dataForTags[0] = rAdius;
979  dataForTags[2] = hEight;
980  dataForTags[3] = aNgle;
982  }
983 
985  Tensor1<TPack1, 3> &t_disp) {
986  return getNormalImpl(t_coords, t_disp);
987  }
989  Tensor1<double, 3> &t_disp) {
990  return getNormalImpl(t_coords, t_disp);
991  }
993  Tensor1<double, 3> &t_disp) {
994  return getNormalImpl(t_coords, t_disp);
995  }
996 
997  Tensor2<double, 3, 3> getDiffNormal(Tensor1<TPack3, 3> &t_coords,
998  Tensor1<TPack1, 3> &t_disp,
999  Tensor1<TPack1, 3> &t_normal) {
1000  return getDiffNormalImpl(t_coords, t_disp, t_normal);
1001  }
1002 
1003  inline double getGap(Tensor1<TPack3, 3> &t_coords) {
1004  return getGapImpl(t_coords);
1005  }
1007  Tensor1<TPack1, 3> &t_normal) {
1008  return getdGapImpl(t_coords, t_normal);
1009  }
1010 
1011  template <typename T1, typename T2>
1013  Tensor1<T2, 3> &t_disp) {
1014 
1015  auto t_d = getPointCoords(t_coords, t_disp);
1016  pointCoords(i) = rotationMat(i, j) * t_d(j);
1017  t_d(i) = pointCoords(i);
1018 
1019  // Tensor1<double, 3> c_center(0., 0., 0.);
1020  // Tensor1<double, 3> q_minus_c(0., 0., 0.);
1021  // Tensor1<double, 3> K(0., 0., 0.);
1022  // auto Q = pointCoords;
1023  // auto P_Q = pointCoords;
1024  // auto Q_K = pointCoords;
1025 
1026  const double half_height = hEight / 2.;
1027  // const double rad2 = rAdius * rAdius;
1028  // const double small_rad2 = smallRadius * smallRadius;
1029 
1030  // tNormal(i) = 0.;
1031  // pointCoords(2) += (oFfset + half_height);
1032  // t_d(2) += oFfset + half_height;
1033  // if (t_d(2) > half_height) {
1034  // tNormal(2) = 1;
1035  // if (pow(t_d(1), 2) + pow(t_d(0), 2) < rad2) {
1036  // gAp = tNormal(i) * t_d(i) - half_height;
1037  // } else {
1038  // c_center(2) = half_height;
1039  // Q(2) = c_center(2); // Q
1040  // q_minus_c(i) = Q(i) - c_center(i);
1041  // q_minus_c.normalize();
1042  // K(i) = c_center(i) + rAdius * q_minus_c(i);
1043  // P_Q(i) = t_d(i) - Q(i);
1044  // Q_K(i) = Q(i) - K(i);
1045  // gAp = sqrt(P_Q.l2() * P_Q.l2() + Q_K.l2() * Q_K.l2());
1046  // }
1047  // } else if (t_d(2) < -half_height) {
1048  // tNormal(2) = -1;
1049  // if (pow(t_d(1), 2) + pow(t_d(0), 2) < small_rad2) {
1050  // gAp = tNormal(i) * t_d(i) - half_height;
1051  // } else {
1052  // c_center(2) = -half_height;
1053  // Q(2) = c_center(2); // Q
1054  // q_minus_c(i) = Q(i) - c_center(i);
1055  // q_minus_c.normalize();
1056  // K(i) = c_center(i) + smallRadius * q_minus_c(i);
1057  // P_Q(i) = t_d(i) - Q(i);
1058  // Q_K(i) = Q(i) - K(i);
1059  // gAp = sqrt(P_Q.l2() * P_Q.l2() + Q_K.l2() * Q_K.l2());
1060  // }
1061  // } else {
1062  // auto closest_pt_cone = cP->getClosestPointToCone(t_d);
1063  // closestPoint(i) = closest_pt_cone(i);
1064 
1065  // tNormal(j) = t_d(j) - closestPoint(j);
1066  // auto test_normal = tNormal.l2();
1067  // if (cP->isPointInsideCone(t_d))
1068  // tNormal(i) *= -1;
1069  // tNormal.normalize();
1070 
1071  // gAp = tNormal(i) * (t_d(i) - closestPoint(i));
1072  // }
1073 
1074  auto clamp = [](auto x, auto min, auto max) {
1075  if (x < min)
1076  x = min;
1077  else if (x > max)
1078  x = max;
1079  return x;
1080  };
1081 
1082  auto sd_capped_cone = [&](Tensor1<double, 3> p, double h, double r1,
1083  double r2) {
1084  typedef Tensor1<double, 2> vec2;
1085  Index<'i', 2> i;
1086  vec2 cb;
1087  vec2 q(sqrt(p(i) * p(i)), p(2));
1088  vec2 k1(r2, h);
1089  vec2 k2(r2 - r1, 2.0 * h);
1090  vec2 ca(q(0) - std::min(q(0), (q(1) < 0.0) ? r1 : r2), abs(q(1)) - h);
1091  vec2 k1_q(k1(0) - q(0), k1(1) - q(1));
1092  cb(i) = q(i) - k1(i) +
1093  k2(i) * clamp((k1_q(i) * k2(i)) / (k2(i) * k2(i)), 0.0, 1.0);
1094  double s = (cb(0) < 0.0 && ca(1) < 0.0) ? -1.0 : 1.0;
1095  return s * sqrt(std::min((ca(i) * ca(i)), (cb(i) * cb(i))));
1096  };
1097 
1098  const double eps = 1e-6;
1099  auto cone_distance =
1100  sd_capped_cone(pointCoords, half_height, rAdius, smallRadius);
1101 
1102  auto get_d = [&](int a) {
1103  auto pt = pointCoords;
1104  pt(a) += eps;
1105  return (sd_capped_cone(pt, half_height, rAdius, smallRadius) -
1106  cone_distance) /
1107  eps;
1108  };
1109 
1110  // for debugging
1111  // auto my_rand_mn = [](double M, double N) {
1112  // return M + (rand() / (RAND_MAX / (N - M)));
1113  // };
1114  // auto t_off = getBodyOffset();
1115  // std::ofstream afile("cone_pts.csv", std::ios::ate);
1116  // if (afile.is_open()) {
1117  // afile << "X,Y,Z\n";
1118  // Tensor1<double, 3> p(0, 0, 0);
1119  // for (int n = 0; n != 1e6; n++) {
1120  // double x = my_rand_mn(-3, 3);
1121  // double y = my_rand_mn(-3, 3);
1122  // double z = my_rand_mn(-3, 3);
1123  // Tensor1<double, 3> coords(x, y, z);
1124  // if (sd_capped_cone(coords, half_height, rAdius, smallRadius) < 0) {
1125 
1126  // afile << x + t_off(0) << "," << y + t_off(1) << "," << z + t_off(2)
1127  // << "\n";
1128  // }
1129  // }
1130  // afile.close();
1131  // }
1132 
1133  Tensor1<double, 3> my_normal(get_d(0), get_d(1), get_d(2));
1134  my_normal.normalize();
1135 
1136  gAp = cone_distance;
1137  tNormal(i) = rotationMat(j, i) * my_normal(j);
1138 
1139  return tNormal;
1140  };
1141 
1142  template <typename T1> inline double getGapImpl(Tensor1<T1, 3> &t_coords) {
1143  return gAp;
1144  };
1145 
1146  template <typename T1, typename T2, typename T3>
1147  inline Tensor2<double, 3, 3> getDiffNormalImpl(Tensor1<T1, 3> &t_coords,
1148  Tensor1<T2, 3> &t_disp,
1149  Tensor1<T3, 3> &t_normal) {
1150  Tensor2<double, 3, 3> diff_normal;
1151  Tensor1<double, 3> norm_diff;
1152  const double clean_gap = gAp;
1153  const double eps = 1e-8;
1154  for (int ii = 0; ii != 3; ++ii) {
1155  Tensor1<double, 3> pert_disp{t_disp(0), t_disp(1), t_disp(2)};
1156  pert_disp(ii) += eps;
1157  auto pert_normal = getNormal(t_coords, pert_disp);
1158  norm_diff(i) = (pert_normal(i) - t_normal(i)) / eps;
1159  dGap(ii) = (gAp - clean_gap) / eps;
1160 
1161  for (int jj = 0; jj != 3; ++jj) {
1162  diff_normal(jj, ii) = norm_diff(jj);
1163  }
1164  }
1165  return diff_normal;
1166  };
1167 
1168  template <typename T1, typename T2>
1170  Tensor1<T2, 3> &t_normal) {
1171  return dGap;
1172  };
1173 
1176  PetscBool rflg;
1177  int nb_dirs = 3;
1178  double angle = 45;
1179  CHKERR PetscOptionsBegin(PETSC_COMM_WORLD, "", "", "");
1180  string param = "-radius" + to_string(iD);
1181  CHKERR PetscOptionsScalar(param.c_str(), "set cone radius", "", rAdius,
1182  &rAdius, PETSC_NULL);
1183  param = "-height" + to_string(iD);
1184  CHKERR PetscOptionsScalar(param.c_str(), "set cone height", "", hEight,
1185  &hEight, PETSC_NULL);
1186  param = "-angle" + to_string(iD);
1187  CHKERR PetscOptionsScalar(param.c_str(), "set cone half angle", "", angle,
1188  &angle, PETSC_NULL);
1189  param = "-direction" + to_string(iD);
1190  CHKERR PetscOptionsGetRealArray(NULL, NULL, param.c_str(), &oRientation(0),
1191  &nb_dirs, &rflg);
1192  ierr = PetscOptionsEnd();
1193  CHKERRG(ierr);
1194  aNgle = angle;
1195  sLope = std::tan(aNgle * M_PI / 180);
1196  oFfset = (rAdius / sLope) - hEight;
1198 
1199  if (rflg)
1201 
1202  cP = make_shared<ArbitrarySurfaceData>(1, sLope);
1203 
1205  }
1206 };
1207 
1208 struct STLRigidBody : public RigidBodyData {
1211  shared_ptr<OrientedBoxTreeTool> orientedBox;
1215 
1216  STLRigidBody(VectorDouble c_coords, VectorDouble roller_disp, int id)
1217  : RigidBodyData(c_coords, roller_disp, id) {
1218  bodyMesh = &mbInstance;
1219  }
1220 
1223  bodyType = STL;
1225  }
1226 
1228  Tensor1<TPack1, 3> &t_disp) {
1229  return getNormalImpl(t_coords, t_disp);
1230  }
1232  Tensor1<double, 3> &t_disp) {
1233  return getNormalImpl(t_coords, t_disp);
1234  }
1236  Tensor1<double, 3> &t_disp) {
1237  return getNormalImpl(t_coords, t_disp);
1238  }
1239 
1240  Tensor2<double, 3, 3> getDiffNormal(Tensor1<TPack3, 3> &t_coords,
1241  Tensor1<TPack1, 3> &t_disp,
1242  Tensor1<TPack1, 3> &t_normal) {
1243  return getDiffNormalImpl(t_coords, t_disp, t_normal);
1244  }
1245 
1246  inline double getGap(Tensor1<TPack3, 3> &t_coords) {
1247  return getGapImpl(t_coords);
1248  }
1249 
1251  Tensor1<TPack1, 3> &t_normal) {
1252  return getdGapImpl(t_coords, t_normal);
1253  }
1254 
1255  template <typename T1, typename T2>
1257  Tensor1<T2, 3> &t_disp) {
1258  auto t_d = getPointCoords(t_coords, t_disp);
1259 
1260  CHKERR orientedBox->closest_to_location(&t_d(0), treeMeshset,
1261  &closestPoint(0), triangle);
1262  CHKERR bodyMesh->tag_get_data(tagNormal, &triangle, 1, &tNormal(0));
1263  gAp = tNormal(i) * (pointCoords(i) - closestPoint(i));
1264  return tNormal;
1265  };
1266 
1267  template <typename T1> inline double getGapImpl(Tensor1<T1, 3> &t_coords) {
1268  return gAp;
1269  };
1270 
1271  template <typename T1, typename T2>
1273  Tensor1<T2, 3> &t_normal) {
1274  return dGap;
1275  };
1276 
1277  template <typename T1, typename T2, typename T3>
1278  inline Tensor2<double, 3, 3> getDiffNormalImpl(Tensor1<T1, 3> &t_coords,
1279  Tensor1<T2, 3> &t_disp,
1280  Tensor1<T3, 3> &t_normal) {
1281  Tensor2<double, 3, 3> diff_normal;
1282  Tensor1<double, 3> norm_diff;
1283  const double clean_gap = gAp;
1284  const double eps = 1e-8;
1285  for (int ii = 0; ii != 3; ++ii) {
1286  Tensor1<double, 3> pert_disp{t_disp(0), t_disp(1), t_disp(2)};
1287  pert_disp(ii) += eps;
1288  auto pert_normal = getNormal(t_coords, pert_disp);
1289  norm_diff(i) = (pert_normal(i) - t_normal(i)) / eps;
1290  dGap(ii) = (gAp - clean_gap) / eps;
1291 
1292  for (int jj = 0; jj != 3; ++jj) {
1293  diff_normal(jj, ii) = norm_diff(jj);
1294  }
1295  }
1296  return diff_normal;
1297  };
1298 
1301  CHKERR PetscOptionsBegin(PETSC_COMM_WORLD, "", "", "");
1302  PetscBool rflg;
1303  string param = "-body_file_name" + to_string(iD);
1304  char mesh_file_name[255];
1305  CHKERR PetscOptionsString(param.c_str(), "file name", "", "mesh.vtk",
1306  mesh_file_name, 255, &rflg);
1307  if (string(mesh_file_name).substr(string(mesh_file_name).length() - 3) !=
1308  "vtk")
1309  SETERRQ(PETSC_COMM_SELF, MOFEM_DATA_INCONSISTENCY,
1310  "body surface should be vtk surface");
1311  bodyMesh->load_file(mesh_file_name, 0, "");
1312  Range tris;
1313  CHKERR bodyMesh->get_entities_by_dimension(0, 2, tris, false);
1314  orientedBox =
1315  make_shared<OrientedBoxTreeTool>(bodyMesh, "ROOTSETSURF", true);
1316  CHKERR orientedBox->build(tris, treeMeshset);
1317  char normal_tag_name[255] = "Normals";
1318  if (bodyMesh->tag_get_handle(normal_tag_name, tagNormal) != MB_SUCCESS) {
1319  SETERRQ(PETSC_COMM_SELF, MOFEM_DATA_INCONSISTENCY,
1320  "Normal tag name %s cannot be found. Please "
1321  "provide the correct name. or run ./calculate_normals");
1322  }
1323 
1324  ierr = PetscOptionsEnd();
1325  CHKERRG(ierr);
1327  }
1328 };
1329 // struct NurbsRigidBody : public RigidBodyData {
1330 // NurbsRigidBody(VectorDouble c_coords, VectorDouble roller_disp, int id)
1331 // : RigidBodyData(c_coords, roller_disp, id) {}
1332 // };
static Index< 'p', 3 > p
static Index< 'K', 3 > K
RigidBodyTypes
Definition: RigidBodies.hpp:17
@ TORUS
Definition: RigidBodies.hpp:22
@ ROLLER
Definition: RigidBodies.hpp:23
@ STL
Definition: RigidBodies.hpp:24
@ PLANE
Definition: RigidBodies.hpp:18
@ CYLINDER
Definition: RigidBodies.hpp:20
@ NURBS
Definition: RigidBodies.hpp:25
@ CONE
Definition: RigidBodies.hpp:21
@ SPHERE
Definition: RigidBodies.hpp:19
@ LASTBODY
Definition: RigidBodies.hpp:26
constexpr double a
static const double eps
Definition: single.cpp:4
#define MoFEMFunctionReturnHot(a)
Last executable line of each PETSc function used for error handling. Replaces return()
Definition: definitions.h:460
#define MoFEMFunctionBegin
First executable line of each MoFEM function, used for error handling. Final line of MoFEM functions ...
Definition: definitions.h:359
#define CHKERRG(n)
Check error code of MoFEM/MOAB/PETSc function.
Definition: definitions.h:496
@ MOFEM_DATA_INCONSISTENCY
Definition: definitions.h:44
#define MoFEMFunctionReturn(a)
Last executable line of each PETSc function used for error handling. Replaces return()
Definition: definitions.h:429
#define CHKERR
Inline error check.
Definition: definitions.h:548
#define MoFEMFunctionBeginHot
First executable line of each MoFEM function, used for error handling. Final line of MoFEM functions ...
Definition: definitions.h:453
double v
phase velocity of light in medium (cm/ns)
char mesh_file_name[255]
Tensor2_Expr< Kronecker_Delta< T >, T, Dim0, Dim1, i, j > kronecker_delta(const Index< i, Dim0 > &, const Index< j, Dim1 > &)
Rank 2.
constexpr std::enable_if<(Dim0<=2 &&Dim1<=2), Tensor2_Expr< Levi_Civita< T >, T, Dim0, Dim1, i, j > >::type levi_civita(const Index< i, Dim0 > &, const Index< j, Dim1 > &)
levi_civita functions to make for easy adhoc use
static MoFEMErrorCodeGeneric< PetscErrorCode > ierr
Definition: Exceptions.hpp:87
PetscErrorCode MoFEMErrorCode
MoFEM/PETSc error code.
Definition: Exceptions.hpp:67
UBlasVector< double > VectorDouble
Definition: Types.hpp:79
PetscErrorCode PetscOptionsGetRealArray(PetscOptions *, const char pre[], const char name[], PetscReal dval[], PetscInt *nmax, PetscBool *set)
CoreTmp< 0 > Core
Definition: Core.hpp:1096
DeprecatedCoreInterface Interface
Definition: Interface.hpp:1965
double h
convective heat coefficient
shared_ptr< ArbitrarySurfaceData > cP
Tensor2< double, 3, 3 > getDiffNormalImpl(Tensor1< T1, 3 > &t_coords, Tensor1< T2, 3 > &t_disp, Tensor1< T3, 3 > &t_normal)
double smallRadius
Tensor1< double, 3 > getdGapImpl(Tensor1< T1, 3 > &t_coords, Tensor1< T2, 3 > &t_normal)
Tensor1< double, 3 > getNormal(Tensor1< TPack3, 3 > &t_coords, Tensor1< double, 3 > &t_disp)
Tensor1< double, 3 > & getNormalImpl(Tensor1< T1, 3 > &t_coords, Tensor1< T2, 3 > &t_disp)
ConeRigidBody(VectorDouble c_coords, VectorDouble roller_disp, int id)
Tensor1< double, 3 > getNormal(Tensor1< TPack3, 3 > &t_coords, Tensor1< TPack1, 3 > &t_disp)
double getGapImpl(Tensor1< T1, 3 > &t_coords)
MoFEMErrorCode getRollerDataForTag()
Tensor1< double, 3 > getNormal(Tensor1< double, 3 > &t_coords, Tensor1< double, 3 > &t_disp)
Tensor2< double, 3, 3 > getDiffNormal(Tensor1< TPack3, 3 > &t_coords, Tensor1< TPack1, 3 > &t_disp, Tensor1< TPack1, 3 > &t_normal)
double getGap(Tensor1< TPack3, 3 > &t_coords)
virtual MoFEMErrorCode getBodyOptions()
Tensor1< double, 3 > getdGap(Tensor1< TPack3, 3 > &t_coords, Tensor1< TPack1, 3 > &t_normal)
Tensor1< double, 3 > getNormal(Tensor1< TPack3, 3 > &t_coords, Tensor1< TPack1, 3 > &t_disp)
Tensor1< double, 3 > getNormal(Tensor1< double, 3 > &t_coords, Tensor1< double, 3 > &t_disp)
Tensor1< double, 3 > getNormal(Tensor1< TPack3, 3 > &t_coords, Tensor1< double, 3 > &t_disp)
Tensor2< double, 3, 3 > getDiffNormal(Tensor1< TPack3, 3 > &t_coords, Tensor1< TPack1, 3 > &t_disp, Tensor1< TPack1, 3 > &t_normal)
double getGapImpl(Tensor1< T1, 3 > &t_coords)
Tensor1< double, 3 > & getNormalImpl(Tensor1< T1, 3 > &t_coords, Tensor1< T2, 3 > &t_disp)
Tensor1< double, 3 > getdGapImpl(Tensor1< T1, 3 > &t_coords, Tensor1< T2, 3 > &t_normal)
MoFEMErrorCode getRollerDataForTag()
Tensor1< double, 3 > getdGap(Tensor1< TPack3, 3 > &t_coords, Tensor1< TPack1, 3 > &t_normal)
MoFEMErrorCode getBodyOptions()
Tensor2< double, 3, 3 > getDiffNormalImpl(Tensor1< T1, 3 > &t_coords, Tensor1< T2, 3 > &t_disp, Tensor1< T3, 3 > &t_normal)
double getGap(Tensor1< TPack3, 3 > &t_coords)
CylinderRigidBody(VectorDouble c_coords, VectorDouble roller_disp, int id)
double getGap(Tensor1< TPack3, 3 > &t_coords)
PlaneRigidBody(VectorDouble c_coords, VectorDouble roller_disp, int id)
Tensor1< double, 3 > getNormal(Tensor1< TPack3, 3 > &t_coords, Tensor1< TPack1, 3 > &t_disp)
Tensor1< double, 3 > getNormal(Tensor1< TPack3, 3 > &t_coords, Tensor1< double, 3 > &t_disp)
Tensor1< double, 3 > getdGap(Tensor1< TPack3, 3 > &t_coords, Tensor1< TPack1, 3 > &t_normal)
MoFEMErrorCode getRollerDataForTag()
double getGapImpl(Tensor1< T1, 3 > &t_coords)
Tensor2< double, 3, 3 > getDiffNormal(Tensor1< TPack3, 3 > &t_coords, Tensor1< TPack1, 3 > &t_disp, Tensor1< TPack1, 3 > &t_normal)
Tensor2< double, 3, 3 > getDiffNormalImpl(Tensor1< T1, 3 > &t_coords, Tensor1< T2, 3 > &t_disp, Tensor1< T3, 3 > &t_normal)
Tensor1< double, 3 > getdGapImpl(Tensor1< T1, 3 > &t_coords, Tensor1< T2, 3 > &t_normal)
MoFEMErrorCode getBodyOptions()
Tensor1< double, 3 > & getNormalImpl(Tensor1< T1, 3 > &t_coords, Tensor1< T2, 3 > &t_disp)
Tensor1< double, 3 > getNormal(Tensor1< double, 3 > &t_coords, Tensor1< double, 3 > &t_disp)
PetscBool fLip
PackPtr< double *, 1 > TPack1
Definition: RigidBodies.hpp:66
virtual ~RigidBodyData()
Definition: RigidBodies.hpp:63
virtual MoFEMErrorCode getRollerDataForTag()=0
VectorDouble rollerDisp
Definition: RigidBodies.hpp:35
Tensor1< double, 3 > pointCoords
Definition: RigidBodies.hpp:40
Tensor1< double, 3 > tNormal
Definition: RigidBodies.hpp:38
virtual Tensor2< double, 3, 3 > getDiffNormal(Tensor1< TPack3, 3 > &t_coords, Tensor1< TPack1, 3 > &t_disp, Tensor1< TPack1, 3 > &t_normal)=0
Tensor1< double, 3 > dGap
Definition: RigidBodies.hpp:39
Tensor1< double, 3 > oRientation
Definition: RigidBodies.hpp:43
string positionDataParamName
Definition: RigidBodies.hpp:47
RigidBodyData(VectorDouble c_coords, VectorDouble roller_disp, int id)
Definition: RigidBodies.hpp:52
VectorDouble BodyDispScaled
Definition: RigidBodies.hpp:36
virtual MoFEMErrorCode getBodyOptions()=0
virtual Tensor1< double, 3 > getdGap(Tensor1< TPack3, 3 > &t_coords, Tensor1< TPack1, 3 > &t_normal)=0
Index< 'k', 3 > k
Definition: RigidBodies.hpp:33
virtual Tensor1< double, 3 > getNormal(Tensor1< TPack3, 3 > &t_coords, Tensor1< double, 3 > &t_disp)=0
Tensor1< double, 3 > & getPointCoords(Tensor1< T1, 3 > &t_coords, Tensor1< T2, 3 > &t_disp)
Index< 'i', 3 > i
Definition: RigidBodies.hpp:31
Tensor2< double, 3, 3 > rotationMat
Definition: RigidBodies.hpp:44
array< double, 9 > dataForTags
Definition: RigidBodies.hpp:50
MoFEMErrorCode saveBasicDataOnTag(moab::Interface &moab_debug, EntityHandle &vertex)
VectorDouble originCoords
Definition: RigidBodies.hpp:34
PackPtr< double *, 3 > TPack3
Definition: RigidBodies.hpp:65
virtual Tensor1< double, 3 > getNormal(Tensor1< TPack3, 3 > &t_coords, Tensor1< TPack1, 3 > &t_disp)=0
MoFEMErrorCode computeRotationMatrix()
Definition: RigidBodies.hpp:85
virtual Tensor1< double, 3 > getNormal(Tensor1< double, 3 > &t_coords, Tensor1< double, 3 > &t_disp)=0
Index< 'j', 3 > j
Definition: RigidBodies.hpp:32
Tensor1< double, 3 > getBodyOffset()
virtual double getGap(Tensor1< TPack3, 3 > &t_coords)=0
Tensor1< double, 3 > closestPoint
Definition: RigidBodies.hpp:41
const int iD
Definition: RigidBodies.hpp:30
Tensor1< double, 3 > defaultOrientation
Definition: RigidBodies.hpp:42
Tensor2< double, 3, 3 > diffNormal
Definition: RigidBodies.hpp:45
RigidBodyData()=delete
Tensor1< double, 3 > getdGap(Tensor1< TPack3, 3 > &t_coords, Tensor1< TPack1, 3 > &t_normal)
Tensor1< double, 3 > & getNormalImpl(Tensor1< T1, 3 > &t_coords, Tensor1< T2, 3 > &t_disp)
Tensor2< double, 3, 3 > getDiffNormalImpl(Tensor1< T1, 3 > &t_coords, Tensor1< T2, 3 > &t_disp, Tensor1< T3, 3 > &t_normal)
Tensor1< double, 3 > getdGapImpl(Tensor1< T1, 3 > &t_coords, Tensor1< T2, 3 > &t_normal)
MoFEMErrorCode getBodyOptions()
double getGap(Tensor1< TPack3, 3 > &t_coords)
Tensor1< double, 3 > getNormal(Tensor1< TPack3, 3 > &t_coords, Tensor1< TPack1, 3 > &t_disp)
MoFEMErrorCode getRollerDataForTag()
Tensor1< double, 3 > getNormal(Tensor1< TPack3, 3 > &t_coords, Tensor1< double, 3 > &t_disp)
Tensor1< double, 3 > getNormal(Tensor1< double, 3 > &t_coords, Tensor1< double, 3 > &t_disp)
RollerRigidBody(VectorDouble c_coords, VectorDouble roller_disp, int id)
double getGapImpl(Tensor1< T1, 3 > &t_coords)
Tensor2< double, 3, 3 > getDiffNormal(Tensor1< TPack3, 3 > &t_coords, Tensor1< TPack1, 3 > &t_disp, Tensor1< TPack1, 3 > &t_normal)
Tensor1< double, 3 > getdGap(Tensor1< TPack3, 3 > &t_coords, Tensor1< TPack1, 3 > &t_normal)
double getGap(Tensor1< TPack3, 3 > &t_coords)
moab::Core mbInstance
Tensor1< double, 3 > getNormal(Tensor1< TPack3, 3 > &t_coords, Tensor1< double, 3 > &t_disp)
MoFEMErrorCode getBodyOptions()
Tensor1< double, 3 > getdGapImpl(Tensor1< T1, 3 > &t_coords, Tensor1< T2, 3 > &t_normal)
moab::Interface * bodyMesh
Tensor1< double, 3 > getNormal(Tensor1< TPack3, 3 > &t_coords, Tensor1< TPack1, 3 > &t_disp)
Tensor2< double, 3, 3 > getDiffNormal(Tensor1< TPack3, 3 > &t_coords, Tensor1< TPack1, 3 > &t_disp, Tensor1< TPack1, 3 > &t_normal)
shared_ptr< OrientedBoxTreeTool > orientedBox
Tensor1< double, 3 > getNormal(Tensor1< double, 3 > &t_coords, Tensor1< double, 3 > &t_disp)
EntityHandle triangle
Tensor2< double, 3, 3 > getDiffNormalImpl(Tensor1< T1, 3 > &t_coords, Tensor1< T2, 3 > &t_disp, Tensor1< T3, 3 > &t_normal)
MoFEMErrorCode getRollerDataForTag()
Tensor1< double, 3 > & getNormalImpl(Tensor1< T1, 3 > &t_coords, Tensor1< T2, 3 > &t_disp)
STLRigidBody(VectorDouble c_coords, VectorDouble roller_disp, int id)
double getGapImpl(Tensor1< T1, 3 > &t_coords)
EntityHandle treeMeshset
double getGap(Tensor1< TPack3, 3 > &t_coords)
SphereRigidBody(VectorDouble c_coords, VectorDouble roller_disp, int id)
Tensor1< double, 3 > getNormal(Tensor1< TPack3, 3 > &t_coords, Tensor1< TPack1, 3 > &t_disp)
MoFEMErrorCode getRollerDataForTag()
Tensor2< double, 3, 3 > getDiffNormal(Tensor1< TPack3, 3 > &t_coords, Tensor1< TPack1, 3 > &t_disp, Tensor1< TPack1, 3 > &t_normal)
MoFEMErrorCode getBodyOptions()
double getGapImpl(Tensor1< T1, 3 > &t_coords)
Tensor2< double, 3, 3 > getDiffNormalImpl(Tensor1< T1, 3 > &t_coords, Tensor1< T2, 3 > &t_disp, Tensor1< T3, 3 > &t_normal)
Tensor1< double, 3 > getdGapImpl(Tensor1< T1, 3 > &t_coords, Tensor1< T2, 3 > &t_normal)
Tensor1< double, 3 > getNormal(Tensor1< double, 3 > &t_coords, Tensor1< double, 3 > &t_disp)
Tensor1< double, 3 > & getNormalImpl(Tensor1< T1, 3 > &t_coords, Tensor1< T2, 3 > &t_disp)
Tensor1< double, 3 > getdGap(Tensor1< TPack3, 3 > &t_coords, Tensor1< TPack1, 3 > &t_normal)
Tensor1< double, 3 > getNormal(Tensor1< TPack3, 3 > &t_coords, Tensor1< double, 3 > &t_disp)
double getGap(Tensor1< TPack3, 3 > &t_coords)
MoFEMErrorCode getRollerDataForTag()
Tensor1< double, 3 > getNormal(Tensor1< double, 3 > &t_coords, Tensor1< double, 3 > &t_disp)
Tensor2< double, 3, 3 > getDiffNormalImpl(Tensor1< T1, 3 > &t_coords, Tensor1< T2, 3 > &t_disp, Tensor1< T3, 3 > &t_normal)
Tensor1< double, 3 > getdGapImpl(Tensor1< T1, 3 > &t_coords, Tensor1< T2, 3 > &t_normal)
virtual MoFEMErrorCode getBodyOptions()
double getGapImpl(Tensor1< T1, 3 > &t_coords)
TorusRigidBody(VectorDouble c_coords, VectorDouble roller_disp, int id)
Tensor2< double, 3, 3 > getDiffNormal(Tensor1< TPack3, 3 > &t_coords, Tensor1< TPack1, 3 > &t_disp, Tensor1< TPack1, 3 > &t_normal)
Tensor1< double, 3 > getNormal(Tensor1< TPack3, 3 > &t_coords, Tensor1< double, 3 > &t_disp)
Tensor1< double, 3 > & getNormalImpl(Tensor1< T1, 3 > &t_coords, Tensor1< T2, 3 > &t_disp)
Tensor1< double, 3 > getNormal(Tensor1< TPack3, 3 > &t_coords, Tensor1< TPack1, 3 > &t_disp)
Tensor1< double, 3 > getdGap(Tensor1< TPack3, 3 > &t_coords, Tensor1< TPack1, 3 > &t_normal)