v0.13.1
Loading...
Searching...
No Matches
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
27};
28
30 const int iD;
34 VectorDouble originCoords;
35 VectorDouble rollerDisp;
36 VectorDouble BodyDispScaled;
37 VectorDouble BodyDirectionScaled;
38 double gAp;
45 Tensor2<double, 3, 3> rotationMat;
46 Tensor2<double, 3, 3> diffNormal;
47
48 // string positionDataParamName;
49 // string directionDataParamName;
50
51 boost::shared_ptr<TimeAccelerogram> methodOpForRollerPosition;
52 boost::shared_ptr<TimeAccelerogram> methodOpForRollerDirection;
53
55 array<double, 9> dataForTags;
56
57 RigidBodyData(VectorDouble c_coords, VectorDouble roller_disp, int id)
58 : originCoords(c_coords), rollerDisp(roller_disp), iD(id),
59 defaultOrientation(0., 0., 1.), oRientation(0., 0., 1.) {
61 BodyDispScaled.clear();
62 BodyDirectionScaled = VectorDouble({0, 0, 0});
63 rotationMat(i, j) = kronecker_delta(i, j);
64 std::fill(dataForTags.begin(), dataForTags.end(), 0);
66 }
67
68 RigidBodyData() = delete;
69 virtual ~RigidBodyData() {}
70
71 using TPack3 = PackPtr<double *, 3>; // t_coords
72 using TPack1 = PackPtr<double *, 1>; // t_disp
73
75 Tensor1<TPack1, 3> &t_disp) = 0;
77 Tensor1<double, 3> &t_disp) = 0;
79 Tensor1<double, 3> &t_disp) = 0;
80
81 virtual Tensor2<double, 3, 3> getDiffNormal(Tensor1<TPack3, 3> &t_coords,
82 Tensor1<TPack1, 3> &t_disp,
83 Tensor1<TPack1, 3> &t_normal) = 0;
84
85 virtual double getGap(Tensor1<TPack3, 3> &t_coords) = 0;
87 Tensor1<TPack1, 3> &t_normal) = 0;
88
89 virtual MoFEMErrorCode getBodyOptions() = 0;
90
91 inline MoFEMErrorCode computeRotationMatrix() {
92 // https://math.stackexchange.com/questions/180418
94
95 auto compute_rot = [&]() {
97 oRientation.normalize();
99 v(i) = levi_civita(i, j, k) * defaultOrientation(j) * oRientation(k);
100 const double s = v.l2();
101 if (abs(s) < 1e-16)
103 const double c = defaultOrientation(i) * oRientation(i);
104 const double coef = 1. / (1. + c);
105 Tensor2<double, 3, 3> t_v;
106 t_v(i, j) = levi_civita(i, j, k) * v(k);
107 rotationMat(i, j) = kronecker_delta(i, j) + t_v(i, j) +
108 (coef * t_v(i, k)) * t_v(k, j);
109 // Tensor1<double, 3> test;
110 // test(i) = rotationMat(j, i) * defaultOrientation(j);
112 };
113
114 // if we have not changed the direction, compute rotation only once
115
116 if (BodyDirectionScaled.empty())
118
120 for (int i = 0; i != 3; ++i)
122 CHKERR compute_rot();
123 } else {
124 BodyDirectionScaled.resize(0);
125 CHKERR compute_rot();
126 }
127
129 };
130
132 VectorDouble pos = originCoords + BodyDispScaled;
133 return Tensor1<double, 3>(pos(0), pos(1), pos(2));
134 };
135
136 template <typename T1, typename T2>
138 Tensor1<T2, 3> &t_disp) {
139 auto t_off = getBodyOffset();
140 pointCoords(i) = t_coords(i) - t_off(i) + t_disp(i);
141 return pointCoords;
142 };
143
144 MoFEMErrorCode saveBasicDataOnTag(moab::Interface &moab_debug,
145 EntityHandle &vertex) {
147
148 Tag tag0;
149 int def_int = 0;
150 std::array<double, 9> def_real;
151 std::fill(def_real.begin(), def_real.end(), 0);
152
154
155 CHKERR moab_debug.tag_get_handle("_ROLLER_ID", 1, MB_TYPE_INTEGER, tag0,
156 MB_TAG_CREAT | MB_TAG_SPARSE, &def_int);
157 CHKERR moab_debug.tag_set_data(tag0, &vertex, 1, &this->iD);
158
159 CHKERR moab_debug.tag_get_handle("_BODY_TYPE", 1, MB_TYPE_INTEGER, tag0,
160 MB_TAG_CREAT | MB_TAG_SPARSE, &def_int);
161 CHKERR moab_debug.tag_set_data(tag0, &vertex, 1, &this->bodyType);
162
164 CHKERR moab_debug.tag_get_handle("_ORIENTATION", 3, MB_TYPE_DOUBLE, tag0,
165 MB_TAG_CREAT | MB_TAG_SPARSE, &def_real);
166 CHKERR moab_debug.tag_set_data(tag0, &vertex, 1, &this->oRientation(0));
167
168 CHKERR moab_debug.tag_get_handle("_ROLLER_DATA", 9, MB_TYPE_DOUBLE, tag0,
169 MB_TAG_CREAT | MB_TAG_SPARSE, &def_real);
170 CHKERR moab_debug.tag_set_data(tag0, &vertex, 1, this->dataForTags.data());
171
172
173 // tags
174 // int roller_id
175 // int body_type
176 // vec3 orientation
177 // tensor9 roller_data
178 // 0 radius1 = 5
179 // 1 inner_radius = 1
180 // 2 height = 1
181 // 3 angle = 45
182 // 4 angle_a = 45
183 // 5 angle_b = 45
184 //
185
187 }
188 virtual MoFEMErrorCode getRollerDataForTag() = 0;
189};
190
192 PetscBool fLip;
193 double sIze; // for printing
194 PlaneRigidBody(VectorDouble c_coords, VectorDouble roller_disp, int id)
195 : RigidBodyData(c_coords, roller_disp, id), fLip(PETSC_FALSE), sIze(0) {}
196
197 MoFEMErrorCode getRollerDataForTag() {
199 bodyType = PLANE;
201 }
202
204 Tensor1<TPack1, 3> &t_disp) {
205 return getNormalImpl(t_coords, t_disp);
206 }
208 Tensor1<double, 3> &t_disp) {
209 return getNormalImpl(t_coords, t_disp);
210 }
212 Tensor1<double, 3> &t_disp) {
213 return getNormalImpl(t_coords, t_disp);
214 }
215
216 Tensor2<double, 3, 3> getDiffNormal(Tensor1<TPack3, 3> &t_coords,
217 Tensor1<TPack1, 3> &t_disp,
218 Tensor1<TPack1, 3> &t_normal) {
219 return getDiffNormalImpl(t_coords, t_disp, t_normal);
220 }
221
222 inline double getGap(Tensor1<TPack3, 3> &t_coords) {
223 return getGapImpl(t_coords);
224 }
225
227 Tensor1<TPack1, 3> &t_normal) {
228 return getdGapImpl(t_coords, t_normal);
229 }
230
231 template <typename T1, typename T2>
233 Tensor1<T2, 3> &t_disp) {
234 auto t_d = getPointCoords(t_coords, t_disp);
235 // FIXME:
237 // tNormal(i) = rotationMat(j, i) * oRientation(j);
238 // TODO: test this
239 // tNormal(i) = rotationMat(i, j) * oRientation(j);
241 if (fLip)
242 tNormal(i) *= -1;
243 // rotationMat(i, j)
244 return tNormal;
245 };
246
247 template <typename T1> double getGapImpl(Tensor1<T1, 3> &t_coords) {
248 // test(i) = rotationMat(j, i) * t_off(j);
249 // t_d2(i) = t_coords(i) - rotationMat(j, i) * t_off(j);
250 gAp = tNormal(i) * pointCoords(i);
251 return gAp;
252 };
253
254 template <typename T1, typename T2, typename T3>
255 inline Tensor2<double, 3, 3> getDiffNormalImpl(Tensor1<T1, 3> &t_coords,
256 Tensor1<T2, 3> &t_disp,
257 Tensor1<T3, 3> &t_normal) {
258 const Tensor2<double, 3, 3> t_diff_norm(0., 0., 0., 0., 0., 0., 0., 0., 0.);
259 return t_diff_norm;
260 };
261
262 template <typename T1, typename T2>
264 Tensor1<T2, 3> &t_normal) {
265 dGap(i) = tNormal(i);
266 return dGap;
267 };
268
269 MoFEMErrorCode getBodyOptions() {
271 PetscBool rflg;
272 PetscInt nb_dirs = 3;
273 CHKERR PetscOptionsBegin(PETSC_COMM_WORLD, "", "", "");
274 string param = "-direction" + to_string(iD);
275 CHKERR PetscOptionsGetRealArray(NULL, NULL, param.c_str(), &oRientation(0),
276 &nb_dirs, &rflg);
277 param = "-flip" + to_string(iD);
278 CHKERR PetscOptionsBool(param.c_str(), "flip the plane normal", "", fLip,
279 &fLip, PETSC_NULL);
280 if (rflg)
282 ierr = PetscOptionsEnd();
283 CHKERRG(ierr);
285 }
286};
287
289 double rAdius;
290 SphereRigidBody(VectorDouble c_coords, VectorDouble roller_disp, int id)
291 : RigidBodyData(c_coords, roller_disp, id), rAdius(1) {}
292
293 MoFEMErrorCode getRollerDataForTag() {
296 dataForTags[0] = rAdius;
298 }
299
301 Tensor1<TPack1, 3> &t_disp) {
302 return getNormalImpl(t_coords, t_disp);
303 }
305 Tensor1<double, 3> &t_disp) {
306 return getNormalImpl(t_coords, t_disp);
307 }
309 Tensor1<double, 3> &t_disp) {
310 return getNormalImpl(t_coords, t_disp);
311 }
312
313 Tensor2<double, 3, 3> getDiffNormal(Tensor1<TPack3, 3> &t_coords,
314 Tensor1<TPack1, 3> &t_disp,
315 Tensor1<TPack1, 3> &t_normal) {
316 return getDiffNormalImpl(t_coords, t_disp, t_normal);
317 }
318
319 inline double getGap(Tensor1<TPack3, 3> &t_coords) {
320 return getGapImpl(t_coords);
321 }
322
324 Tensor1<TPack1, 3> &t_normal) {
325 return getdGapImpl(t_coords, t_normal);
326 }
327
328 template <typename T1, typename T2>
330 Tensor1<T2, 3> &t_disp) {
331 auto t_d = getPointCoords(t_coords, t_disp);
332 tNormal(i) = t_d(i);
333 tNormal.normalize();
334
335 return tNormal;
336 };
337
338 template <typename T1, typename T2, typename T3>
339 inline Tensor2<double, 3, 3> getDiffNormalImpl(Tensor1<T1, 3> &t_coords,
340 Tensor1<T2, 3> &t_disp,
341 Tensor1<T3, 3> &t_normal) {
342
343 auto t_d = getPointCoords(t_coords, t_disp);
344
345 const double norm = t_d.l2();
346 const double norm3 = norm * norm * norm;
347 diffNormal(i, j) =
348 kronecker_delta(i, j) * (1. / norm) - (1. / norm3) * t_d(i) * t_d(j);
349
350 return diffNormal;
351 };
352
353 template <typename T1> inline double getGapImpl(Tensor1<T1, 3> &t_coords) {
354
355 gAp = tNormal(i) * (pointCoords(i) - tNormal(i) * rAdius);
356 return gAp;
357 };
358
359 template <typename T1, typename T2>
361 Tensor1<T2, 3> &t_normal) {
362 // d gap / d normal
363 dGap(i) =
364 pointCoords(j) * diffNormal(i, j) +
365 tNormal(j) * (kronecker_delta(i, j) - 2. * rAdius * diffNormal(i, j));
366 return dGap;
367 };
368
369 MoFEMErrorCode getBodyOptions() {
371
372 CHKERR PetscOptionsBegin(PETSC_COMM_WORLD, "", "", "");
373
374 string param = "-radius" + to_string(iD);
375 CHKERR PetscOptionsScalar(param.c_str(), "set roller radius", "", rAdius,
376 &rAdius, PETSC_NULL);
377 ierr = PetscOptionsEnd();
378 CHKERRG(ierr);
379
381 }
382};
383
385 double rAdius;
386 double hEight;
387 CylinderRigidBody(VectorDouble c_coords, VectorDouble roller_disp, int id)
388 : RigidBodyData(c_coords, roller_disp, id), rAdius(1), hEight(INFINITY) {}
389
390 MoFEMErrorCode getRollerDataForTag() {
393 dataForTags[0] = rAdius;
394 dataForTags[2] = hEight;
396 }
397
399 Tensor1<TPack1, 3> &t_disp) {
400 return getNormalImpl(t_coords, t_disp);
401 }
403 Tensor1<double, 3> &t_disp) {
404 return getNormalImpl(t_coords, t_disp);
405 }
407 Tensor1<double, 3> &t_disp) {
408 return getNormalImpl(t_coords, t_disp);
409 }
410
411 Tensor2<double, 3, 3> getDiffNormal(Tensor1<TPack3, 3> &t_coords,
412 Tensor1<TPack1, 3> &t_disp,
413 Tensor1<TPack1, 3> &t_normal) {
414 return getDiffNormalImpl(t_coords, t_disp, t_normal);
415 }
416
417 inline double getGap(Tensor1<TPack3, 3> &t_coords) {
418 return getGapImpl(t_coords);
419 }
420
422 Tensor1<TPack1, 3> &t_normal) {
423 return getdGapImpl(t_coords, t_normal);
424 }
425
426 template <typename T1, typename T2>
428 Tensor1<T2, 3> &t_disp) {
429 auto t_d = getPointCoords(t_coords, t_disp);
431 pointCoords(i) = rotationMat(i, j) * t_d(j);
432 Tensor1<double, 3> c_center(0., 0., 0.);
433 Tensor1<double, 3> q_minus_c(0., 0., 0.);
434 Tensor1<double, 3> K(0., 0., 0.);
435 auto Q = pointCoords;
436 auto P_Q = pointCoords;
437 auto Q_K = pointCoords;
438
439 // https://www.geometrictools.com/Documentation/DistanceToCircle3.pdf
440 const double half_height = hEight / 2.;
441 const double rad2 = rAdius * rAdius;
442 tNormal(i) = 0.;
443
444 if (pointCoords(2) > half_height) {
445 tNormal(2) = 1;
446 if (pow(pointCoords(1), 2) + pow(pointCoords(0), 2) < rad2) {
447 gAp = tNormal(i) * pointCoords(i) - half_height;
448 } else {
449 c_center(2) = half_height;
450 Q(2) = c_center(2); // Q
451 q_minus_c(i) = Q(i) - c_center(i);
452 q_minus_c.normalize();
453 K(i) = c_center(i) + rAdius * q_minus_c(i);
454 P_Q(i) = pointCoords(i) - Q(i);
455 Q_K(i) = Q(i) - K(i);
456 gAp = sqrt(P_Q.l2() * P_Q.l2() + Q_K.l2() * Q_K.l2());
457 }
458 } else if (pointCoords(2) < -half_height) {
459 tNormal(2) = -1;
460 if (pow(pointCoords(1), 2) + pow(pointCoords(0), 2) < rad2) {
461 gAp = tNormal(i) * pointCoords(i) - half_height;
462 } else {
463 c_center(2) = -half_height;
464 Q(2) = c_center(2); // Q
465 q_minus_c(i) = Q(i) - c_center(i);
466 q_minus_c.normalize();
467 K(i) = c_center(i) + rAdius * q_minus_c(i);
468 P_Q(i) = pointCoords(i) - Q(i);
469 Q_K(i) = Q(i) - K(i);
470 gAp = sqrt(P_Q.l2() * P_Q.l2() + Q_K.l2() * Q_K.l2());
471 }
472 } else {
473 pointCoords(2) = 0;
475 tNormal.normalize();
476 gAp = tNormal(i) * (pointCoords(i) - tNormal(i) * rAdius);
477 }
478
479 // auto length = [&](auto v) { return sqrt(v(0) * v(0) + v(1) * v(1)); };
480
481 // auto sd_cylinder = [&](Tensor1<double, 3> p) {
482 // typedef Tensor1<double, 2> vec2;
483 // Index<'i', 2> i;
484 // vec2 d(length(p) - rAdius, abs(p(2)) - half_height);
485 // vec2 mx_d(max(d(0), 0.0), max(d(1), 0.0));
486 // return min(max(d(0), d(1)), 0.0) + length(mx_d);
487 // };
488
489 // const double eps = 1e-6;
490 // auto cyl_distance = sd_cylinder(pointCoords);
491 // gAp = cyl_distance;
492
493 // auto get_d = [&](int a) {
494 // auto pt = pointCoords;
495 // pt(a) += eps;
496 // return (sd_cylinder(pt) - cyl_distance) / eps;
497 // };
498
499 // for (int dd = 0; dd != 3; ++dd)
500 // tNormal(dd) = get_d(dd);
501
502 // for debugging
503 // auto my_rand_mn = [](double M, double N) {
504 // return M + (rand() / (RAND_MAX / (N - M)));
505 // };
506 // auto t_off = getBodyOffset();
507 // std::ofstream afile("cylinder_pts.csv", std::ios::ate);
508 // if (afile.is_open()) {
509 // afile << "X,Y,Z\n";
510 // Tensor1<double, 3> p(0, 0, 0);
511 // for (int n = 0; n != 1e6; n++) {
512 // double x = my_rand_mn(-300, 300);
513 // double y = my_rand_mn(-300, 300);
514 // double z = my_rand_mn(-300, 300);
515 // Tensor1<double, 3> coords(x, y, z);
516 // if (sd_cylinder(coords) < 0) {
517
518 // afile << x + t_off(0) << "," << y + t_off(1) << "," << z + t_off(2)
519 // << "\n";
520 // }
521 // }
522 // afile.close();
523 // }
524 // cout << "kill process " << endl;
525
526 tNormal.normalize();
527
528 auto normal_copy = tNormal;
529 tNormal(i) = rotationMat(j, i) * normal_copy(j);
530
531 return tNormal;
532 };
533
534 template <typename T1, typename T2, typename T3>
535 inline Tensor2<double, 3, 3> getDiffNormalImpl(Tensor1<T1, 3> &t_coords,
536 Tensor1<T2, 3> &t_disp,
537 Tensor1<T3, 3> &t_normal) {
538
539 Tensor2<double, 3, 3> diff_normal;
540 Tensor1<double, 3> norm_diff;
541 const double clean_gap = gAp;
542 const double eps = 1e-8;
543 for (int ii = 0; ii != 3; ++ii) {
544 Tensor1<double, 3> pert_disp{t_disp(0), t_disp(1), t_disp(2)};
545 pert_disp(ii) += eps;
546 auto pert_normal = getNormal(t_coords, pert_disp);
547 norm_diff(i) = (pert_normal(i) - t_normal(i)) / eps;
548 dGap(ii) = (gAp - clean_gap) / eps;
549
550 for (int jj = 0; jj != 3; ++jj) {
551 diff_normal(jj, ii) = norm_diff(jj);
552 }
553 }
554 return diff_normal;
555 };
556
557 template <typename T1> inline double getGapImpl(Tensor1<T1, 3> &t_coords) {
558 // gAp = tNormal(i) * (pointCoords(i) - tNormal(i) * rAdius);
559 return gAp;
560 };
561
562 template <typename T1, typename T2>
564 Tensor1<T2, 3> &t_normal) {
565 // dGap(i) =
566 // pointCoords(j) * diffNormal(i, j) +
567 // tNormal(j) * (kronecker_delta(i, j) - 2. * rAdius * diffNormal(i,
568 // j));
569 return dGap;
570 };
571
572 MoFEMErrorCode getBodyOptions() {
574 PetscBool rflg;
575 PetscInt nb_dirs = 3;
576 CHKERR PetscOptionsBegin(PETSC_COMM_WORLD, "", "", "");
577 string param = "-direction" + to_string(iD);
578 CHKERR PetscOptionsGetRealArray(NULL, NULL, param.c_str(), &oRientation(0),
579 &nb_dirs, &rflg);
580 param = "-radius" + to_string(iD);
581 CHKERR PetscOptionsScalar(param.c_str(), "set roller radius", "", rAdius,
582 &rAdius, PETSC_NULL);
583 param = "-height" + to_string(iD);
584 CHKERR PetscOptionsScalar(param.c_str(), "set cylinder height", "", hEight,
585 &hEight, PETSC_NULL);
586 if (rflg)
588 ierr = PetscOptionsEnd();
589 CHKERRG(ierr);
591 }
592};
593
595 // shared_ptr<ArbitrarySurfaceData> cP;
596 double rAdius;
598 TorusRigidBody(VectorDouble c_coords, VectorDouble roller_disp, int id)
599 : RigidBodyData(c_coords, roller_disp, id), rAdius(1), innerRadius(0.5) {
600 // , cP(R, r, 1)
601 // compute rotation matrix here
602 }
603
604 MoFEMErrorCode getRollerDataForTag() {
606 bodyType = TORUS;
607 dataForTags[0] = rAdius;
610 }
611
613 Tensor1<TPack1, 3> &t_disp) {
614 return getNormalImpl(t_coords, t_disp);
615 }
617 Tensor1<double, 3> &t_disp) {
618 return getNormalImpl(t_coords, t_disp);
619 }
621 Tensor1<double, 3> &t_disp) {
622 return getNormalImpl(t_coords, t_disp);
623 }
624
625 Tensor2<double, 3, 3> getDiffNormal(Tensor1<TPack3, 3> &t_coords,
626 Tensor1<TPack1, 3> &t_disp,
627 Tensor1<TPack1, 3> &t_normal) {
628 return getDiffNormalImpl(t_coords, t_disp, t_normal);
629 }
630
631 inline double getGap(Tensor1<TPack3, 3> &t_coords) {
632 return getGapImpl(t_coords);
633 }
635 Tensor1<TPack1, 3> &t_normal) {
636 return getdGapImpl(t_coords, t_normal);
637 }
638
639 template <typename T1, typename T2>
641 Tensor1<T2, 3> &t_disp) {
642
643 auto t_d = getPointCoords(t_coords, t_disp);
645 pointCoords(i) = rotationMat(i, j) * t_d(j);
646 t_d(i) = pointCoords(i);
647
648 // cP->tt = 1;
649 // cP->rr = 1.;
650 // cP->R1 = 0;
651 // cP->PrintTorusDebug();
652 // cP->PrintTorusAnimDebug();
653
654 // auto closest_pt_toroid = cP->getClosestPointToToroid(t_d);
655 // closestPoint(i) = closest_pt_toroid(i);
656
657 // tNormal(j) = t_d(j) - closestPoint(j);
658 // if (cP->isPointInsideTorus(t_d))
659 // tNormal(i) *= -1;
660 // tNormal.normalize();
661
662 // gAp = tNormal(i) * (t_d(i) - closestPoint(i));
663
664 // https: // iquilezles.org/www/articles/distfunctions/distfunctions.htm
665 const double x = t_d(0);
666 const double y = t_d(1);
667 const double z = t_d(2);
668 const double RR = rAdius - innerRadius;
669 const double prod = sqrt(pow(x, 2) + pow(y, 2));
670 const double prod2 = sqrt(pow(-RR + prod, 2) + pow(z, 2));
671 gAp = -innerRadius + prod2;
672 tNormal(0) = (x * (-RR + prod)) / (prod * prod2);
673 tNormal(1) = (y * (-RR + prod)) / (prod * prod2);
674 tNormal(2) = z / prod2;
675 tNormal.normalize();
676
677 auto normal_copy = tNormal;
678 tNormal(i) = rotationMat(j, i) * normal_copy(j);
679
680 return tNormal;
681 };
682
683 template <typename T1> inline double getGapImpl(Tensor1<T1, 3> &t_coords) {
684 return gAp;
685 };
686
687 template <typename T1, typename T2, typename T3>
688 inline Tensor2<double, 3, 3> getDiffNormalImpl(Tensor1<T1, 3> &t_coords,
689 Tensor1<T2, 3> &t_disp,
690 Tensor1<T3, 3> &t_normal) {
691 Tensor2<double, 3, 3> diff_normal;
692 Tensor1<double, 3> norm_diff;
693 const double clean_gap = gAp;
694 const double eps = 1e-8;
695 for (int ii = 0; ii != 3; ++ii) {
696 Tensor1<double, 3> pert_disp{t_disp(0), t_disp(1), t_disp(2)};
697 pert_disp(ii) += eps;
698 auto pert_normal = getNormal(t_coords, pert_disp);
699 norm_diff(i) = (pert_normal(i) - t_normal(i)) / eps;
700 dGap(ii) = (gAp - clean_gap) / eps;
701
702 for (int jj = 0; jj != 3; ++jj) {
703 diff_normal(jj, ii) = norm_diff(jj);
704 }
705 }
706 return diff_normal;
707 };
708
709 template <typename T1, typename T2>
711 Tensor1<T2, 3> &t_normal) {
712 return dGap;
713 };
714
715 virtual MoFEMErrorCode getBodyOptions() {
717 PetscBool rflg;
718 int nb_dirs = 3;
719 CHKERR PetscOptionsBegin(PETSC_COMM_WORLD, "", "", "");
720 string param = "-inner_radius" + to_string(iD);
721 CHKERR PetscOptionsScalar(param.c_str(), "set torus small radius", "", innerRadius,
722 &innerRadius, PETSC_NULL);
723 param = "-radius" + to_string(iD);
724 CHKERR PetscOptionsScalar(param.c_str(), "set torus large Radius", "", rAdius,
725 &rAdius, PETSC_NULL);
726 param = "-direction" + to_string(iD);
727 CHKERR PetscOptionsGetRealArray(NULL, NULL, param.c_str(), &oRientation(0),
728 &nb_dirs, &rflg);
729 ierr = PetscOptionsEnd();
730 CHKERRG(ierr);
731
732 if (rflg)
734
735 // cP = make_shared<ArbitrarySurfaceData>(R, r);
736
738 }
739};
740
742 // shared_ptr<ArbitrarySurfaceData> cP;
743 double angleA;
744 double angleB;
745 double rAdius;
746 double fIllet;
748 double hEight;
749
753
757
758 RollerRigidBody(VectorDouble c_coords, VectorDouble roller_disp, int id)
759 : RigidBodyData(c_coords, roller_disp, id) {}
760
761 MoFEMErrorCode getRollerDataForTag() {
764 dataForTags[0] = rAdius;
765 dataForTags[1] = fIllet;
766 dataForTags[2] = hEight;
767 dataForTags[4] = angleA;
768 dataForTags[5] = angleB;
769
771 }
772
774 Tensor1<TPack1, 3> &t_disp) {
775 return getNormalImpl(t_coords, t_disp);
776 }
778 Tensor1<double, 3> &t_disp) {
779 return getNormalImpl(t_coords, t_disp);
780 }
782 Tensor1<double, 3> &t_disp) {
783 return getNormalImpl(t_coords, t_disp);
784 }
785
786 Tensor2<double, 3, 3> getDiffNormal(Tensor1<TPack3, 3> &t_coords,
787 Tensor1<TPack1, 3> &t_disp,
788 Tensor1<TPack1, 3> &t_normal) {
789 return getDiffNormalImpl(t_coords, t_disp, t_normal);
790 }
791
792 inline double getGap(Tensor1<TPack3, 3> &t_coords) {
793 return getGapImpl(t_coords);
794 }
795
797 Tensor1<TPack1, 3> &t_normal) {
798 return getdGapImpl(t_coords, t_normal);
799 }
800
801 MoFEMErrorCode getBodyOptions() {
803 PetscBool rflg;
804 int nb_dirs = 3;
805
806 CHKERR PetscOptionsBegin(PETSC_COMM_WORLD, "", "", "");
807 string param = "-radius" + to_string(iD);
808 CHKERR PetscOptionsScalar(param.c_str(), "set roller radius", "", rAdius,
809 &rAdius, PETSC_NULL);
810 param = "-fillet" + to_string(iD);
811 CHKERR PetscOptionsScalar(param.c_str(), "set torus small radius", "",
812 fIllet, &fIllet, PETSC_NULL);
813 param = "-angle_a" + to_string(iD);
814 CHKERR PetscOptionsScalar(param.c_str(), "set roller angle of attack", "",
815 angleA, &angleA, PETSC_NULL);
816 param = "-angle_b" + to_string(iD);
817 CHKERR PetscOptionsScalar(param.c_str(), "set roller back angle", "",
818 angleB, &angleB, PETSC_NULL);
819 hEight = 3 * fIllet;
820 param = "-height" + to_string(iD);
821 CHKERR PetscOptionsScalar(param.c_str(), "set roller height (optional)", "",
822 hEight, &hEight, PETSC_NULL);
823 param = "-direction" + to_string(iD);
824 CHKERR PetscOptionsGetRealArray(NULL, NULL, param.c_str(), &oRientation(0),
825 &nb_dirs, &rflg);
826 ierr = PetscOptionsEnd();
827 CHKERRG(ierr);
828 if (rflg)
830
831 // cP = make_shared<ArbitrarySurfaceData>(rAdius, fIllet);
832
834 angleA = std::tan(angleA * M_PI / 180);
835 angleB = std::tan(angleB * M_PI / 180);
836
837 coneRadiusA = torusRadius + fIllet / sqrt(1 + angleA * angleA);
839 coneOffsetA = -fIllet * angleA / sqrt(1 + angleA * angleA) - hEight / 2.;
840
843 coneOffsetB = fIllet * angleB / sqrt(1 + angleB * angleB) + hEight / 2.;
844
846 }
847
848 template <typename T1, typename T2>
850 Tensor1<T2, 3> &t_disp) {
851 auto t_d = getPointCoords(t_coords, t_disp);
853 pointCoords(i) = rotationMat(i, j) * t_d(j);
854 t_d(i) = pointCoords(i);
855
856 const double inv_eps = 1e6;
857 array<double, 3> gaps;
858 const double half_height = hEight / 2.;
859
860 double cone_apex_offset_a = coneOffsetA + half_height;
861 double cone_apex_offset_b = coneOffsetB - half_height;
862
863 // https://computergraphics.stackexchange.com/questions/7485/glsl-shapes-signed-distance-field-implementation-explanation
864 auto sd_infinite_cone = [&](Tensor1<double, 3> p, double slope) {
865 double c1 = 1. / sqrt(1 + (slope * slope));
866 double c2 = c1 * slope;
867 auto prod = sqrt(p(0) * p(0) + p(1) * p(1));
868 auto prod2 = sqrt(1 + slope * slope);
869 tNormal(0) = p(0) / (prod2 * prod);
870 tNormal(1) = p(1) / (prod2 * prod);
871 tNormal(2) = slope / prod2;
872 return c1 * prod + c2 * p(2);
873 };
874
875 const double full_h_a = -cone_apex_offset_a + coneRadiusA / angleA;
876 const double full_h_b = cone_apex_offset_b + coneTopRadiusB / angleB;
877
878 Tensor1<double, 3> t_apx_1(t_d(0), t_d(1), t_d(2) - full_h_a);
879 Tensor1<double, 3> t_apx_2(t_d(0), t_d(1), t_d(2) + full_h_b);
880
881 auto torus_dist = [&]() {
882 const double prod = sqrt(pow(t_d(0), 2) + pow(t_d(1), 2));
883 const double prod2 = sqrt(pow(-torusRadius + prod, 2) + pow(t_d(2), 2));
884 tNormal(0) = (t_d(0) * (-torusRadius + prod)) / (prod * prod2);
885 tNormal(1) = (t_d(1) * (-torusRadius + prod)) / (prod * prod2);
886 tNormal(2) = t_d(2) / prod2;
887 return -fIllet + prod2;
888 };
889
890 gaps[0] = torus_dist();
891 gaps[1] = t_d(2) < -cone_apex_offset_a ? inv_eps
892 : sd_infinite_cone(t_apx_1, angleA);
893 gaps[2] = t_d(2) > -cone_apex_offset_b ? inv_eps
894 : sd_infinite_cone(t_apx_2, -angleB);
895
896 int nb = distance(gaps.begin(), std::min_element(gaps.begin(), gaps.end()));
897 gAp = gaps[nb];
898
899 switch (nb) {
900 case 0: {
901 torus_dist();
902 break;
903 }
904 case 1: {
905 // compute normal for cone A
906 sd_infinite_cone(t_apx_1, angleA);
907 break;
908 }
909 case 2: {
910 // compute normal for cone b
911 sd_infinite_cone(t_apx_2, -angleB);
912 } break;
913 }
914
915 // // for debugging
916 // auto my_rand_mn = [](double M, double N) {
917 // return M + (rand() / (RAND_MAX / (N - M)));
918 // };
919 // auto t_off = getBodyOffset();
920 // std::ofstream afile("cone_pts.csv", std::ios::ate);
921 // if (afile.is_open()) {
922 // afile << "x,y,z\n";
923 // Tensor1<double, 3> p(0, 0, 0);
924 // for (int n = 0; n != 1e6; n++) {
925 // double x = my_rand_mn(-1.5, 1.5);
926 // double y = my_rand_mn(-1.5, 1.5);
927 // double z = my_rand_mn(-1, 1);
928 // Tensor1<double, 3> coords(x, y, z);
929
930 // t_d(i) = coords(i);
931 // t_dc1(i) = coords(i);
932 // t_dc2(i) = coords(i);
933 // t_dc1(2) -= full_h_a;
934 // t_dc2(2) += full_h_b;
935
936 // gaps[0] = torus_dist();
937 // gaps[1] = t_d(2) < -cone_apex_offset_a ? inv_eps :
938 // sd_infinite_cone(t_dc1, angleA); gaps[2] = t_d(2) >
939 // -cone_apex_offset_b ? inv_eps : sd_infinite_cone(t_dc2, -angleB);
940
941 // if (gaps[0] < 0 || gaps[1] < 0 || gaps[2] < 0) {
942
943 // afile << x + t_off(0) << "," << y + t_off(1) << "," << z + t_off(2)
944 // << "\n";
945 // }
946 // }
947 // afile.close();
948 // }
949
950 tNormal.normalize();
951 auto normal_copy = tNormal;
952 tNormal(i) = rotationMat(j, i) * normal_copy(j);
953
954 return tNormal;
955 };
956
957 template <typename T1, typename T2, typename T3>
958 inline Tensor2<double, 3, 3> getDiffNormalImpl(Tensor1<T1, 3> &t_coords,
959 Tensor1<T2, 3> &t_disp,
960 Tensor1<T3, 3> &t_normal) {
961 Tensor2<double, 3, 3> diff_normal;
962 Tensor1<double, 3> norm_diff;
963 const double clean_gap = gAp;
964 const double eps = 1e-8;
965 for (int ii = 0; ii != 3; ++ii) {
966 Tensor1<double, 3> pert_disp{t_disp(0), t_disp(1), t_disp(2)};
967 pert_disp(ii) += eps;
968 auto pert_normal = getNormal(t_coords, pert_disp);
969 norm_diff(i) = (pert_normal(i) - t_normal(i)) / eps;
970 dGap(ii) = (gAp - clean_gap) / eps;
971
972 for (int jj = 0; jj != 3; ++jj) {
973 diff_normal(jj, ii) = norm_diff(jj);
974 }
975 }
976 return diff_normal;
977 };
978
979 template <typename T1> inline double getGapImpl(Tensor1<T1, 3> &t_coords) {
980 return gAp;
981 };
982
983 template <typename T1, typename T2>
985 Tensor1<T2, 3> &t_normal) {
986 return dGap;
987 };
988};
989
991 shared_ptr<ArbitrarySurfaceData> cP;
992 double sLope;
993 double hEight;
994 double rAdius;
996 double oFfset;
997
998 double aNgle;
999 ConeRigidBody(VectorDouble c_coords, VectorDouble roller_disp, int id)
1000 : RigidBodyData(c_coords, roller_disp, id) {
1001 // , cP(R, r, 1)
1002 sLope = 1;
1003 hEight = 1;
1004 rAdius = 1;
1005 oFfset = 0;
1006 // compute rotation matrix here
1007 }
1008
1009 MoFEMErrorCode getRollerDataForTag() {
1011 bodyType = CONE;
1012 dataForTags[0] = rAdius;
1013 dataForTags[2] = hEight;
1014 dataForTags[3] = aNgle;
1016 }
1017
1019 Tensor1<TPack1, 3> &t_disp) {
1020 return getNormalImpl(t_coords, t_disp);
1021 }
1023 Tensor1<double, 3> &t_disp) {
1024 return getNormalImpl(t_coords, t_disp);
1025 }
1027 Tensor1<double, 3> &t_disp) {
1028 return getNormalImpl(t_coords, t_disp);
1029 }
1030
1031 Tensor2<double, 3, 3> getDiffNormal(Tensor1<TPack3, 3> &t_coords,
1032 Tensor1<TPack1, 3> &t_disp,
1033 Tensor1<TPack1, 3> &t_normal) {
1034 return getDiffNormalImpl(t_coords, t_disp, t_normal);
1035 }
1036
1037 inline double getGap(Tensor1<TPack3, 3> &t_coords) {
1038 return getGapImpl(t_coords);
1039 }
1041 Tensor1<TPack1, 3> &t_normal) {
1042 return getdGapImpl(t_coords, t_normal);
1043 }
1044
1045 template <typename T1, typename T2>
1047 Tensor1<T2, 3> &t_disp) {
1048
1049 auto t_d = getPointCoords(t_coords, t_disp);
1051 pointCoords(i) = rotationMat(i, j) * t_d(j);
1052 t_d(i) = pointCoords(i);
1053
1054 // Tensor1<double, 3> c_center(0., 0., 0.);
1055 // Tensor1<double, 3> q_minus_c(0., 0., 0.);
1056 // Tensor1<double, 3> K(0., 0., 0.);
1057 // auto Q = pointCoords;
1058 // auto P_Q = pointCoords;
1059 // auto Q_K = pointCoords;
1060
1061 const double half_height = hEight / 2.;
1062 // const double rad2 = rAdius * rAdius;
1063 // const double small_rad2 = smallRadius * smallRadius;
1064
1065 // tNormal(i) = 0.;
1066 // pointCoords(2) += (oFfset + half_height);
1067 // t_d(2) += oFfset + half_height;
1068 // if (t_d(2) > half_height) {
1069 // tNormal(2) = 1;
1070 // if (pow(t_d(1), 2) + pow(t_d(0), 2) < rad2) {
1071 // gAp = tNormal(i) * t_d(i) - half_height;
1072 // } else {
1073 // c_center(2) = half_height;
1074 // Q(2) = c_center(2); // Q
1075 // q_minus_c(i) = Q(i) - c_center(i);
1076 // q_minus_c.normalize();
1077 // K(i) = c_center(i) + rAdius * q_minus_c(i);
1078 // P_Q(i) = t_d(i) - Q(i);
1079 // Q_K(i) = Q(i) - K(i);
1080 // gAp = sqrt(P_Q.l2() * P_Q.l2() + Q_K.l2() * Q_K.l2());
1081 // }
1082 // } else if (t_d(2) < -half_height) {
1083 // tNormal(2) = -1;
1084 // if (pow(t_d(1), 2) + pow(t_d(0), 2) < small_rad2) {
1085 // gAp = tNormal(i) * t_d(i) - half_height;
1086 // } else {
1087 // c_center(2) = -half_height;
1088 // Q(2) = c_center(2); // Q
1089 // q_minus_c(i) = Q(i) - c_center(i);
1090 // q_minus_c.normalize();
1091 // K(i) = c_center(i) + smallRadius * q_minus_c(i);
1092 // P_Q(i) = t_d(i) - Q(i);
1093 // Q_K(i) = Q(i) - K(i);
1094 // gAp = sqrt(P_Q.l2() * P_Q.l2() + Q_K.l2() * Q_K.l2());
1095 // }
1096 // } else {
1097 // auto closest_pt_cone = cP->getClosestPointToCone(t_d);
1098 // closestPoint(i) = closest_pt_cone(i);
1099
1100 // tNormal(j) = t_d(j) - closestPoint(j);
1101 // auto test_normal = tNormal.l2();
1102 // if (cP->isPointInsideCone(t_d))
1103 // tNormal(i) *= -1;
1104 // tNormal.normalize();
1105
1106 // gAp = tNormal(i) * (t_d(i) - closestPoint(i));
1107 // }
1108
1109 auto clamp = [](auto x, auto min, auto max) {
1110 if (x < min)
1111 x = min;
1112 else if (x > max)
1113 x = max;
1114 return x;
1115 };
1116
1117 auto sd_capped_cone = [&](Tensor1<double, 3> p, double h, double r1,
1118 double r2) {
1119 typedef Tensor1<double, 2> vec2;
1121 vec2 cb;
1122 vec2 q(sqrt(p(i) * p(i)), p(2));
1123 vec2 k1(r2, h);
1124 vec2 k2(r2 - r1, 2.0 * h);
1125 vec2 ca(q(0) - std::min(q(0), (q(1) < 0.0) ? r1 : r2), abs(q(1)) - h);
1126 vec2 k1_q(k1(0) - q(0), k1(1) - q(1));
1127 cb(i) = q(i) - k1(i) +
1128 k2(i) * clamp((k1_q(i) * k2(i)) / (k2(i) * k2(i)), 0.0, 1.0);
1129 double s = (cb(0) < 0.0 && ca(1) < 0.0) ? -1.0 : 1.0;
1130 return s * sqrt(std::min((ca(i) * ca(i)), (cb(i) * cb(i))));
1131 };
1132
1133 const double eps = 1e-6;
1134 auto cone_distance =
1135 sd_capped_cone(pointCoords, half_height, rAdius, smallRadius);
1136
1137 auto get_d = [&](int a) {
1138 auto pt = pointCoords;
1139 pt(a) += eps;
1140 return (sd_capped_cone(pt, half_height, rAdius, smallRadius) -
1141 cone_distance) /
1142 eps;
1143 };
1144
1145 // for debugging
1146 // auto my_rand_mn = [](double M, double N) {
1147 // return M + (rand() / (RAND_MAX / (N - M)));
1148 // };
1149 // auto t_off = getBodyOffset();
1150 // std::ofstream afile("cone_pts.csv", std::ios::ate);
1151 // if (afile.is_open()) {
1152 // afile << "X,Y,Z\n";
1153 // Tensor1<double, 3> p(0, 0, 0);
1154 // for (int n = 0; n != 1e6; n++) {
1155 // double x = my_rand_mn(-3, 3);
1156 // double y = my_rand_mn(-3, 3);
1157 // double z = my_rand_mn(-3, 3);
1158 // Tensor1<double, 3> coords(x, y, z);
1159 // if (sd_capped_cone(coords, half_height, rAdius, smallRadius) < 0) {
1160
1161 // afile << x + t_off(0) << "," << y + t_off(1) << "," << z + t_off(2)
1162 // << "\n";
1163 // }
1164 // }
1165 // afile.close();
1166 // }
1167
1168 Tensor1<double, 3> my_normal(get_d(0), get_d(1), get_d(2));
1169 my_normal.normalize();
1170
1171 gAp = cone_distance;
1172 tNormal(i) = rotationMat(j, i) * my_normal(j);
1173
1174 return tNormal;
1175 };
1176
1177 template <typename T1> inline double getGapImpl(Tensor1<T1, 3> &t_coords) {
1178 return gAp;
1179 };
1180
1181 template <typename T1, typename T2, typename T3>
1182 inline Tensor2<double, 3, 3> getDiffNormalImpl(Tensor1<T1, 3> &t_coords,
1183 Tensor1<T2, 3> &t_disp,
1184 Tensor1<T3, 3> &t_normal) {
1185 Tensor2<double, 3, 3> diff_normal;
1186 Tensor1<double, 3> norm_diff;
1187 const double clean_gap = gAp;
1188 const double eps = 1e-8;
1189 for (int ii = 0; ii != 3; ++ii) {
1190 Tensor1<double, 3> pert_disp{t_disp(0), t_disp(1), t_disp(2)};
1191 pert_disp(ii) += eps;
1192 auto pert_normal = getNormal(t_coords, pert_disp);
1193 norm_diff(i) = (pert_normal(i) - t_normal(i)) / eps;
1194 dGap(ii) = (gAp - clean_gap) / eps;
1195
1196 for (int jj = 0; jj != 3; ++jj) {
1197 diff_normal(jj, ii) = norm_diff(jj);
1198 }
1199 }
1200 return diff_normal;
1201 };
1202
1203 template <typename T1, typename T2>
1205 Tensor1<T2, 3> &t_normal) {
1206 return dGap;
1207 };
1208
1209 virtual MoFEMErrorCode getBodyOptions() {
1211 PetscBool rflg;
1212 int nb_dirs = 3;
1213 double angle = 45;
1214 CHKERR PetscOptionsBegin(PETSC_COMM_WORLD, "", "", "");
1215 string param = "-radius" + to_string(iD);
1216 CHKERR PetscOptionsScalar(param.c_str(), "set cone radius", "", rAdius,
1217 &rAdius, PETSC_NULL);
1218 param = "-height" + to_string(iD);
1219 CHKERR PetscOptionsScalar(param.c_str(), "set cone height", "", hEight,
1220 &hEight, PETSC_NULL);
1221 param = "-angle" + to_string(iD);
1222 CHKERR PetscOptionsScalar(param.c_str(), "set cone half angle", "", angle,
1223 &angle, PETSC_NULL);
1224 param = "-direction" + to_string(iD);
1225 CHKERR PetscOptionsGetRealArray(NULL, NULL, param.c_str(), &oRientation(0),
1226 &nb_dirs, &rflg);
1227 ierr = PetscOptionsEnd();
1228 CHKERRG(ierr);
1229 aNgle = angle;
1230 sLope = std::tan(aNgle * M_PI / 180);
1231 oFfset = (rAdius / sLope) - hEight;
1233
1234 if (rflg)
1236
1237 cP = make_shared<ArbitrarySurfaceData>(1, sLope);
1238
1240 }
1241};
1242
1244 moab::Core mbInstance;
1245 moab::Interface *bodyMesh;
1246 shared_ptr<OrientedBoxTreeTool> orientedBox;
1250
1251 STLRigidBody(VectorDouble c_coords, VectorDouble roller_disp, int id)
1252 : RigidBodyData(c_coords, roller_disp, id) {
1254 }
1255
1256 MoFEMErrorCode getRollerDataForTag() {
1258 bodyType = STL;
1260 }
1261
1263 Tensor1<TPack1, 3> &t_disp) {
1264 return getNormalImpl(t_coords, t_disp);
1265 }
1267 Tensor1<double, 3> &t_disp) {
1268 return getNormalImpl(t_coords, t_disp);
1269 }
1271 Tensor1<double, 3> &t_disp) {
1272 return getNormalImpl(t_coords, t_disp);
1273 }
1274
1275 Tensor2<double, 3, 3> getDiffNormal(Tensor1<TPack3, 3> &t_coords,
1276 Tensor1<TPack1, 3> &t_disp,
1277 Tensor1<TPack1, 3> &t_normal) {
1278 return getDiffNormalImpl(t_coords, t_disp, t_normal);
1279 }
1280
1281 inline double getGap(Tensor1<TPack3, 3> &t_coords) {
1282 return getGapImpl(t_coords);
1283 }
1284
1286 Tensor1<TPack1, 3> &t_normal) {
1287 return getdGapImpl(t_coords, t_normal);
1288 }
1289
1290 template <typename T1, typename T2>
1292 Tensor1<T2, 3> &t_disp) {
1293 auto t_d = getPointCoords(t_coords, t_disp);
1294
1295 CHKERR orientedBox->closest_to_location(&t_d(0), treeMeshset,
1296 &closestPoint(0), triangle);
1297 CHKERR bodyMesh->tag_get_data(tagNormal, &triangle, 1, &tNormal(0));
1298 gAp = tNormal(i) * (pointCoords(i) - closestPoint(i));
1299 return tNormal;
1300 };
1301
1302 template <typename T1> inline double getGapImpl(Tensor1<T1, 3> &t_coords) {
1303 return gAp;
1304 };
1305
1306 template <typename T1, typename T2>
1308 Tensor1<T2, 3> &t_normal) {
1309 return dGap;
1310 };
1311
1312 template <typename T1, typename T2, typename T3>
1313 inline Tensor2<double, 3, 3> getDiffNormalImpl(Tensor1<T1, 3> &t_coords,
1314 Tensor1<T2, 3> &t_disp,
1315 Tensor1<T3, 3> &t_normal) {
1316 Tensor2<double, 3, 3> diff_normal;
1317 Tensor1<double, 3> norm_diff;
1318 const double clean_gap = gAp;
1319 const double eps = 1e-8;
1320 for (int ii = 0; ii != 3; ++ii) {
1321 Tensor1<double, 3> pert_disp{t_disp(0), t_disp(1), t_disp(2)};
1322 pert_disp(ii) += eps;
1323 auto pert_normal = getNormal(t_coords, pert_disp);
1324 norm_diff(i) = (pert_normal(i) - t_normal(i)) / eps;
1325 dGap(ii) = (gAp - clean_gap) / eps;
1326
1327 for (int jj = 0; jj != 3; ++jj) {
1328 diff_normal(jj, ii) = norm_diff(jj);
1329 }
1330 }
1331 return diff_normal;
1332 };
1333
1334 MoFEMErrorCode getBodyOptions() {
1336 CHKERR PetscOptionsBegin(PETSC_COMM_WORLD, "", "", "");
1337 PetscBool rflg;
1338 string param = "-body_file_name" + to_string(iD);
1339 char mesh_file_name[255];
1340 CHKERR PetscOptionsString(param.c_str(), "file name", "", "mesh.vtk",
1341 mesh_file_name, 255, &rflg);
1342 if (string(mesh_file_name).substr(string(mesh_file_name).length() - 3) !=
1343 "vtk")
1344 SETERRQ(PETSC_COMM_SELF, MOFEM_DATA_INCONSISTENCY,
1345 "body surface should be vtk surface");
1346 bodyMesh->load_file(mesh_file_name, 0, "");
1347 Range tris;
1348 CHKERR bodyMesh->get_entities_by_dimension(0, 2, tris, false);
1349 orientedBox =
1350 make_shared<OrientedBoxTreeTool>(bodyMesh, "ROOTSETSURF", true);
1351 CHKERR orientedBox->build(tris, treeMeshset);
1352 char normal_tag_name[255] = "Normals";
1353 if (bodyMesh->tag_get_handle(normal_tag_name, tagNormal) != MB_SUCCESS) {
1354 SETERRQ(PETSC_COMM_SELF, MOFEM_DATA_INCONSISTENCY,
1355 "Normal tag name %s cannot be found. Please "
1356 "provide the correct name. or run ./calculate_normals");
1357 }
1358
1359 ierr = PetscOptionsEnd();
1360 CHKERRG(ierr);
1362 }
1363};
1364// struct NurbsRigidBody : public RigidBodyData {
1365// NurbsRigidBody(VectorDouble c_coords, VectorDouble roller_disp, int id)
1366// : RigidBodyData(c_coords, roller_disp, id) {}
1367// };
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 PetscErrorCode ierr
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:447
#define MoFEMFunctionBegin
First executable line of each MoFEM function, used for error handling. Final line of MoFEM functions ...
Definition: definitions.h:346
#define CHKERRG(n)
Check error code of MoFEM/MOAB/PETSc function.
Definition: definitions.h:483
@ MOFEM_DATA_INCONSISTENCY
Definition: definitions.h:31
#define MoFEMFunctionReturn(a)
Last executable line of each PETSc function used for error handling. Replaces return()
Definition: definitions.h:416
#define CHKERR
Inline error check.
Definition: definitions.h:535
#define MoFEMFunctionBeginHot
First executable line of each MoFEM function, used for error handling. Final line of MoFEM functions ...
Definition: definitions.h:440
const double c
speed of light (cm/ns)
const double v
phase velocity of light in medium (cm/ns)
char mesh_file_name[255]
double h
shared_ptr< ArbitrarySurfaceData > cP
double smallRadius
Tensor2< double, 3, 3 > getDiffNormal(Tensor1< TPack3, 3 > &t_coords, Tensor1< TPack1, 3 > &t_disp, Tensor1< TPack1, 3 > &t_normal)
ConeRigidBody(VectorDouble c_coords, VectorDouble roller_disp, int id)
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 > & getNormalImpl(Tensor1< T1, 3 > &t_coords, Tensor1< T2, 3 > &t_disp)
double getGapImpl(Tensor1< T1, 3 > &t_coords)
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)
Tensor1< double, 3 > getdGapImpl(Tensor1< T1, 3 > &t_coords, Tensor1< T2, 3 > &t_normal)
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)
virtual MoFEMErrorCode getBodyOptions()
Tensor1< double, 3 > getNormal(Tensor1< TPack3, 3 > &t_coords, Tensor1< double, 3 > &t_disp)
Tensor1< double, 3 > getNormal(Tensor1< TPack3, 3 > &t_coords, Tensor1< TPack1, 3 > &t_disp)
Tensor2< double, 3, 3 > getDiffNormalImpl(Tensor1< T1, 3 > &t_coords, Tensor1< T2, 3 > &t_disp, Tensor1< T3, 3 > &t_normal)
double getGapImpl(Tensor1< T1, 3 > &t_coords)
Tensor1< double, 3 > & getNormalImpl(Tensor1< T1, 3 > &t_coords, Tensor1< T2, 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()
Tensor1< double, 3 > getdGapImpl(Tensor1< T1, 3 > &t_coords, Tensor1< T2, 3 > &t_normal)
Tensor1< double, 3 > getdGap(Tensor1< TPack3, 3 > &t_coords, Tensor1< TPack1, 3 > &t_normal)
double getGap(Tensor1< TPack3, 3 > &t_coords)
Tensor1< double, 3 > getNormal(Tensor1< double, 3 > &t_coords, Tensor1< double, 3 > &t_disp)
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)
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< TPack1, 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 > getNormal(Tensor1< TPack3, 3 > &t_coords, Tensor1< double, 3 > &t_disp)
MoFEMErrorCode getRollerDataForTag()
double getGapImpl(Tensor1< T1, 3 > &t_coords)
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)
MoFEMErrorCode getBodyOptions()
Tensor1< double, 3 > getdGap(Tensor1< TPack3, 3 > &t_coords, Tensor1< TPack1, 3 > &t_normal)
Tensor1< double, 3 > getdGapImpl(Tensor1< T1, 3 > &t_coords, Tensor1< T2, 3 > &t_normal)
PetscBool fLip
PackPtr< double *, 1 > TPack1
Definition: RigidBodies.hpp:72
virtual Tensor1< double, 3 > getNormal(Tensor1< TPack3, 3 > &t_coords, Tensor1< TPack1, 3 > &t_disp)=0
virtual ~RigidBodyData()
Definition: RigidBodies.hpp:69
virtual MoFEMErrorCode getRollerDataForTag()=0
boost::shared_ptr< TimeAccelerogram > methodOpForRollerPosition
Definition: RigidBodies.hpp:51
VectorDouble rollerDisp
Definition: RigidBodies.hpp:35
Tensor1< double, 3 > pointCoords
Definition: RigidBodies.hpp:41
Tensor1< double, 3 > tNormal
Definition: RigidBodies.hpp:39
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:40
Tensor1< double, 3 > oRientation
Definition: RigidBodies.hpp:44
boost::shared_ptr< TimeAccelerogram > methodOpForRollerDirection
Definition: RigidBodies.hpp:52
RigidBodyData(VectorDouble c_coords, VectorDouble roller_disp, int id)
Definition: RigidBodies.hpp:57
VectorDouble BodyDispScaled
Definition: RigidBodies.hpp:36
virtual MoFEMErrorCode getBodyOptions()=0
Index< 'k', 3 > k
Definition: RigidBodies.hpp:33
virtual Tensor1< double, 3 > getdGap(Tensor1< TPack3, 3 > &t_coords, Tensor1< TPack1, 3 > &t_normal)=0
VectorDouble BodyDirectionScaled
Definition: RigidBodies.hpp:37
Index< 'i', 3 > i
Definition: RigidBodies.hpp:31
Tensor1< double, 3 > & getPointCoords(Tensor1< T1, 3 > &t_coords, Tensor1< T2, 3 > &t_disp)
Tensor2< double, 3, 3 > rotationMat
Definition: RigidBodies.hpp:45
array< double, 9 > dataForTags
Definition: RigidBodies.hpp:55
MoFEMErrorCode saveBasicDataOnTag(moab::Interface &moab_debug, EntityHandle &vertex)
VectorDouble originCoords
Definition: RigidBodies.hpp:34
PackPtr< double *, 3 > TPack3
Definition: RigidBodies.hpp:71
Tensor1< double, 3 > getBodyOffset()
MoFEMErrorCode computeRotationMatrix()
Definition: RigidBodies.hpp:91
virtual Tensor1< double, 3 > getNormal(Tensor1< double, 3 > &t_coords, Tensor1< double, 3 > &t_disp)=0
Index< 'j', 3 > j
Definition: RigidBodies.hpp:32
virtual double getGap(Tensor1< TPack3, 3 > &t_coords)=0
virtual Tensor1< double, 3 > getNormal(Tensor1< TPack3, 3 > &t_coords, Tensor1< double, 3 > &t_disp)=0
Tensor1< double, 3 > closestPoint
Definition: RigidBodies.hpp:42
const int iD
Definition: RigidBodies.hpp:30
Tensor1< double, 3 > defaultOrientation
Definition: RigidBodies.hpp:43
Tensor2< double, 3, 3 > diffNormal
Definition: RigidBodies.hpp:46
RigidBodyData()=delete
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)
MoFEMErrorCode getBodyOptions()
double getGap(Tensor1< TPack3, 3 > &t_coords)
Tensor1< double, 3 > getdGapImpl(Tensor1< T1, 3 > &t_coords, Tensor1< T2, 3 > &t_normal)
MoFEMErrorCode getRollerDataForTag()
Tensor2< double, 3, 3 > getDiffNormalImpl(Tensor1< T1, 3 > &t_coords, Tensor1< T2, 3 > &t_disp, Tensor1< T3, 3 > &t_normal)
Tensor1< double, 3 > getdGap(Tensor1< TPack3, 3 > &t_coords, Tensor1< TPack1, 3 > &t_normal)
RollerRigidBody(VectorDouble c_coords, VectorDouble roller_disp, int id)
double getGapImpl(Tensor1< T1, 3 > &t_coords)
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)
double getGap(Tensor1< TPack3, 3 > &t_coords)
moab::Core mbInstance
MoFEMErrorCode getBodyOptions()
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 > getNormal(Tensor1< double, 3 > &t_coords, Tensor1< double, 3 > &t_disp)
moab::Interface * bodyMesh
Tensor1< double, 3 > getNormal(Tensor1< TPack3, 3 > &t_coords, Tensor1< double, 3 > &t_disp)
shared_ptr< OrientedBoxTreeTool > orientedBox
Tensor2< double, 3, 3 > getDiffNormal(Tensor1< TPack3, 3 > &t_coords, Tensor1< TPack1, 3 > &t_disp, Tensor1< TPack1, 3 > &t_normal)
EntityHandle triangle
MoFEMErrorCode getRollerDataForTag()
Tensor1< double, 3 > getNormal(Tensor1< TPack3, 3 > &t_coords, Tensor1< TPack1, 3 > &t_disp)
Tensor1< double, 3 > getdGapImpl(Tensor1< T1, 3 > &t_coords, Tensor1< T2, 3 > &t_normal)
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)
Tensor2< double, 3, 3 > getDiffNormalImpl(Tensor1< T1, 3 > &t_coords, Tensor1< T2, 3 > &t_disp, Tensor1< T3, 3 > &t_normal)
Tensor1< double, 3 > & getNormalImpl(Tensor1< T1, 3 > &t_coords, Tensor1< T2, 3 > &t_disp)
MoFEMErrorCode getRollerDataForTag()
MoFEMErrorCode getBodyOptions()
Tensor1< double, 3 > getdGap(Tensor1< TPack3, 3 > &t_coords, Tensor1< TPack1, 3 > &t_normal)
double getGapImpl(Tensor1< T1, 3 > &t_coords)
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)
Tensor1< double, 3 > getNormal(Tensor1< TPack3, 3 > &t_coords, Tensor1< TPack1, 3 > &t_disp)
Tensor1< double, 3 > getdGapImpl(Tensor1< T1, 3 > &t_coords, Tensor1< T2, 3 > &t_normal)
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)
MoFEMErrorCode getRollerDataForTag()
Tensor2< double, 3, 3 > getDiffNormal(Tensor1< TPack3, 3 > &t_coords, Tensor1< TPack1, 3 > &t_disp, Tensor1< TPack1, 3 > &t_normal)
Tensor1< double, 3 > getdGapImpl(Tensor1< T1, 3 > &t_coords, Tensor1< T2, 3 > &t_normal)
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)
virtual MoFEMErrorCode getBodyOptions()
Tensor1< double, 3 > getdGap(Tensor1< TPack3, 3 > &t_coords, Tensor1< TPack1, 3 > &t_normal)
double getGapImpl(Tensor1< T1, 3 > &t_coords)
TorusRigidBody(VectorDouble c_coords, VectorDouble roller_disp, int id)
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 > getNormal(Tensor1< TPack3, 3 > &t_coords, Tensor1< double, 3 > &t_disp)