185 {
187 ASSERT(dt > 0, "Timestep < 0");
188
189
197
199 for(
MInt i = 0; i < nDim; i++) {
202 }
203 }
204
205 for(
MInt i = 0; i < nDim; i++) {
207 }
209
210
211
212 const MFloat invDensityRatio =
214
216
218 array<MFloat, nDim> predictedPos{};
219 array<MFloat, nDim> predictedVel{};
220 array<MFloat, nDim> predictedAngularVel{};
221 array<MFloat, nDim + 1> predictedQuaternion{};
222
231 for(
MInt i = 0; i < 3; i++) {
233 }
234
235
236 for(
MInt i = 0; i < nDim; i++) {
239 }
240 predictedQuaternion[0] = w + F1B2 * dt * (-x * q0[0] -
y * q0[1] - z * q0[2]);
241 predictedQuaternion[1] = x + F1B2 * dt * (w * q0[0] - z * q0[1] +
y * q0[2]);
242 predictedQuaternion[2] =
y + F1B2 * dt * (z * q0[0] + w * q0[1] - x * q0[2]);
243 predictedQuaternion[3] = z + F1B2 * dt * (-
y * q0[0] + x * q0[1] + w * q0[2]);
244 for(
MInt i = 0; i < 3; i++) {
246 }
247
249
251
252 ASSERT(
m_cellId > -1,
"Invalid cell!");
253 ASSERT(predictedCellId > -1, "Invalid cell!");
254 ASSERT(
s_backPtr->c_isLeafCell(predictedCellId),
"Invalid cell!");
255
256 vector<MFloat> predictedWeights(0);
257 array<MFloat, nDim> fluidVel{};
258 array<MFloat, nDim * nDim> fluidVelGradient{};
259 array<MFloat, nDim * nDim> fluidVelGradientHat{};
260 std::fill(fluidVel.begin(), fluidVel.end(), std::numeric_limits<MFloat>::quiet_NaN());
261 std::fill(fluidVelGradient.begin(), fluidVelGradient.end(), std::numeric_limits<MFloat>::quiet_NaN());
262 std::fill(fluidVelGradientHat.begin(), fluidVelGradientHat.end(), std::numeric_limits<MFloat>::quiet_NaN());
264 predictedWeights);
265
266
269 for(
MInt i = 0; i < nDim; i++) {
271 }
272
273 m_fluidVelMag = sqrt(std::inner_product(fluidVel.begin(), fluidVel.end(), fluidVel.begin(), F0));
274
276
278
281
282 for(
MInt i = 0; i < nDim; i++) {
289 }
290 } else {
291 switch(
s_backPtr->m_motionEquationType) {
292 case 0: {
293
294 for(
MInt i = 0; i < nDim; i++) {
300 }
301 break;
302 }
303 case 1: {
304
305
307
309
311
312 for(
MInt i = 0; i < nDim; i++) {
313 m_accel[i] = CD * (fluidVel[i] - predictedVel[i]) + (1.0 - invDensityRatio) *
s_Frm[i];
316 }
317 break;
318 }
319 case 2: {
320
321 const MFloat fdtB2 = F1B2 * dt;
326
332
336
337 const MFloat linAccFac = (8.0 / 3.0) * pow(beta, F2B3) * fTauP;
338 const MFloat angAccFac = (40.0 / 9.0) * pow(beta, F2B3) * fTauP;
339
340
341 const MInt maxit = 100;
344
345
346 for(
MInt i = 0; i < 3; i++) {
349 tq[i] = tq0[i];
350 q[i] = q0[i];
351 }
352
353
354 for(
MInt i = 0; i < 3; i++) {
355 MInt id0 = (i + 1) % 3;
356 MInt id1 = (id0 + 1) % 3;
357 strain[i] = F1B2 * (fluidVelGradientHat[3 * id1 + id0] + fluidVelGradientHat[3 * id0 + id1]);
358 vort[i] = F1B2 * (fluidVelGradientHat[3 * id1 + id0] - fluidVelGradientHat[3 * id0 + id1]);
359 }
360
364 W(2, 0) = F0;
365 W(2, 1) = F0;
366
367
368 while(delta > 1e-10 && it < maxit) {
369 W(0, 1) = -fdtB2 * q[2] * (beta2 - F1) / (beta2 + F1);
370 W(0, 2) = -fdtB2 * q[1] * (beta2 - F1) / (beta2 + F1);
371 W(1, 0) = -fdtB2 * q[2] * (F1 - beta2) / (beta2 + F1);
372 W(1, 2) = -fdtB2 * q[0] * (F1 - beta2) / (beta2 + F1);
373
375
376
377 tq[0] = q[1] * q[2] * (beta2 - F1) / (beta2 + F1)
378 + angAccFac * (((F1 - beta2) / (F1 + beta2)) * strain[0] + vort[0] - q[0])
380 tq[1] = q[0] * q[2] * (F1 - beta2) / (beta2 + F1)
381 + angAccFac * (((beta2 - F1) / (F1 + beta2)) * strain[1] + vort[1] - q[1])
383 tq[2] = angAccFac * (vort[2] - q[2]) / (F2 *
m_shapeParams[1]);
384
385 for(
MInt i = 0; i < 3; i++)
386 rhs[i] = q[i] - q0[i] - fdtB2 * (tq0[i] + tq[i]);
387
388 delta = F0;
389 for(
MInt i = 0; i < 3; i++) {
391 for(
MInt j = 0; j < 3; j++) {
392 q[i] -= iW(i, j) * rhs(j);
393 }
394 delta =
mMax(delta, fabs(q[i] - qq));
395 }
396 it++;
397 }
398
399 if(it >= maxit || it <= 0) {
400 cerr <<
"Newton iterations did not converge " <<
m_partId << endl;
401 }
402
403 for(
MInt i = 0; i < 3; i++) {
406 }
407
408
413
414 W(0, 0) = F0;
415 W(0, 1) = -q[0];
416 W(0, 2) = -q[1];
417 W(0, 3) = -q[2];
418 W(1, 0) = q[0];
419 W(1, 1) = F0;
420 W(1, 2) = q[2];
421 W(1, 3) = -q[1];
422 W(2, 0) = q[1];
423 W(2, 1) = -q[2];
424 W(2, 2) = F0;
425 W(2, 3) = q[0];
426 W(3, 0) = q[2];
427 W(3, 1) = q[1];
428 W(3, 2) = -q[0];
429 W(3, 3) = F0;
430
431 for(
MInt i = 0; i < 4; i++) {
432 for(
MInt j = 0; j < 4; j++) {
433 W(i, j) = -F1B2 * fdtB2 * W(i, j);
434 }
435 W(i, i) = F1;
436 }
437
438 rhs(0) = w + F1B2 * fdtB2 * (-x * q0[0] -
y * q0[1] - z * q0[2]);
439 rhs(1) = x + F1B2 * fdtB2 * (w * q0[0] - z * q0[1] +
y * q0[2]);
440 rhs(2) =
y + F1B2 * fdtB2 * (z * q0[0] + w * q0[1] - x * q0[2]);
441 rhs(3) = z + F1B2 * fdtB2 * (-
y * q0[0] + x * q0[1] + w * q0[2]);
442
444
445 for(
MInt i = 0; i < 4; i++) {
447 for(
MInt j = 0; j < 4; j++) {
449 }
450 }
451
453 for(
MInt i = 0; i < 4; i++)
455 for(
MInt i = 0; i < 4; i++)
457
458
459 K.fill(F0);
461 K(1, 1) = K(0, 0);
464
465 for(
MInt i = 0; i < nDim; i++) {
466 vrel[i] = fluidVel[i] - predictedVel[i];
467 }
468
472
473 for(
MInt i = 0; i < nDim; i++) {
474 m_accel[i] = linAccFac * vrel[i] + (1.0 - invDensityRatio) *
s_Frm[i];
478 }
479 break;
480 }
481 case 3: {
482
483 const MFloat fdtB2 = F1B2 * dt;
488
494
501
502 const MFloat angAccFac = (40.0 / 9.0) * pow(beta, F2B3) * fTauP;
503 const MFloat pitchingTorqueFac = (15.0 / 8.0) * invDensityRatio * pow(beta, F2B3) /
POW2(eqRadius);
507
509
510 for(
MInt i = 0; i < nDim; i++) {
511 vrel[i] = fluidVel[i] - predictedVel[i];
512 }
513
517
518 const MFloat magRelVelHat = sqrt(
POW2(vrelhat[0]) +
POW2(vrelhat[1]) +
POW2(vrelhat[2]));
519
520 const auto nan = std::numeric_limits<MFloat>::quiet_NaN();
521 MFloat inclinationAngle = nan;
522 std::array<MFloat, 3> accHat = {nan, nan, nan};
523 std::array<MFloat, 3> accDragHat = {nan, nan, nan};
524 std::array<MFloat, 3> accLiftHat = {nan, nan, nan};
525 std::array<MFloat, 3> dirDHat = {nan, nan, nan};
526 std::array<MFloat, 3> dirTHat = {nan, nan, nan};
527 std::array<MFloat, 3> dirLHat = {nan, nan, nan};
528 std::array<MFloat, 3> dirDCrossZ = {nan, nan, nan};
529 std::array<MFloat, 3> dirZHat{nan, nan, nan};
530
531
532 for(
MInt i = 0; i < nDim; i++) {
534 }
535
536 for(
MInt i = 0; i < nDim; i++) {
537 dirDHat[i] = vrelhat[i] / magRelVelHat;
538 }
539
541 const MFloat dotProduct = std::inner_product(dirDHat.begin(), dirDHat.end(), dirZHat.begin(), F0);
542 inclinationAngle = acos(fabs(dotProduct));
543 MInt sgn = (dotProduct > 0 ? 1 : -1);
544 for(
MInt i = 0; i < nDim; i++) {
545 dirTHat[i] = dirDCrossZ[i] / sqrt(
POW2(dirDCrossZ[0]) +
POW2(dirDCrossZ[1]) +
POW2(dirDCrossZ[2])) *
sgn;
546 }
547
549
550
551 const MInt maxit = 100;
554
558 W(2, 0) = F0;
559 W(2, 1) = F0;
560
561
562 for(
MInt i = 0; i < 3; i++) {
565 tq[i] = tq0[i];
566 q[i] = q0[i];
567 }
568
569
571 if(
s_backPtr->m_torqueModelType > 0 && Rep > 1e-8) {
573 for(
MInt i = 0; i < 3; i++) {
574 pitch[i] = pitchingTorqueFac *
POW2(magRelVelHat) * CT / momI[i] * dirTHat[i];
575 if(pitchingTorqueFac <= F0 || CT <= F0 || momI[i] <= F0)
576 cout << "Calc pitching torque " << pitchingTorqueFac << " " << CT << " " << momI[i] << endl;
577 }
578 }
579
580
581 for(
MInt i = 0; i < 3; i++) {
582 MInt id0 = (i + 1) % 3;
583 MInt id1 = (id0 + 1) % 3;
584 strain[i] = F1B2 * (fluidVelGradientHat[3 * id1 + id0] + fluidVelGradientHat[3 * id0 + id1]);
585 vort[i] = F1B2 * (fluidVelGradientHat[3 * id1 + id0] - fluidVelGradientHat[3 * id0 + id1]);
586 }
587
588
589 while(delta > 1e-10 && it < maxit) {
590 W(0, 1) = -fdtB2 * q[2] * (beta2 - F1) / (beta2 + F1);
591 W(0, 2) = -fdtB2 * q[1] * (beta2 - F1) / (beta2 + F1);
592 W(1, 0) = -fdtB2 * q[2] * (F1 - beta2) / (beta2 + F1);
593 W(1, 2) = -fdtB2 * q[0] * (F1 - beta2) / (beta2 + F1);
594
596
597 tq[0] = q[1] * q[2] * (beta2 - F1) / (beta2 + F1)
598 + angAccFac * (((F1 - beta2) / (F1 + beta2)) * strain[0] + vort[0] - q[0])
600 + pitch[0];
601 tq[1] = q[0] * q[2] * (F1 - beta2) / (beta2 + F1)
602 + angAccFac * (((beta2 - F1) / (F1 + beta2)) * strain[1] + vort[1] - q[1])
604 + pitch[1];
605 tq[2] = angAccFac * (vort[2] - q[2]) / (F2 *
m_shapeParams[1]) + pitch[2];
606
607 for(
MInt i = 0; i < 3; i++)
608 rhs[i] = q[i] - q0[i] - fdtB2 * (tq0[i] + tq[i]);
609
610 delta = F0;
611 for(
MInt i = 0; i < 3; i++) {
613 for(
MInt j = 0; j < 3; j++) {
614 q[i] -= iW(i, j) * rhs(j);
615 }
616 delta =
mMax(delta, fabs(q[i] - qq));
617 }
618 it++;
619 }
620
621 if(it >= maxit || it <= 0) {
622 cerr <<
"Newton iterations did not converge " <<
m_partId << endl;
623 }
624
625 for(
MInt i = 0; i < 3; i++) {
628 }
629
630
635
636 W(0, 0) = F0;
637 W(0, 1) = -q[0];
638 W(0, 2) = -q[1];
639 W(0, 3) = -q[2];
640 W(1, 0) = q[0];
641 W(1, 1) = F0;
642 W(1, 2) = q[2];
643 W(1, 3) = -q[1];
644 W(2, 0) = q[1];
645 W(2, 1) = -q[2];
646 W(2, 2) = F0;
647 W(2, 3) = q[0];
648 W(3, 0) = q[2];
649 W(3, 1) = q[1];
650 W(3, 2) = -q[0];
651 W(3, 3) = F0;
652
653 for(
MInt i = 0; i < 4; i++) {
654 for(
MInt j = 0; j < 4; j++) {
655 W(i, j) = -F1B2 * fdtB2 * W(i, j);
656 }
657 W(i, i) = F1;
658 }
659
660 rhs(0) = w + F1B2 * fdtB2 * (-x * q0[0] -
y * q0[1] - z * q0[2]);
661 rhs(1) = x + F1B2 * fdtB2 * (w * q0[0] - z * q0[1] +
y * q0[2]);
662 rhs(2) =
y + F1B2 * fdtB2 * (z * q0[0] + w * q0[1] - x * q0[2]);
663 rhs(3) = z + F1B2 * fdtB2 * (-
y * q0[0] + x * q0[1] + w * q0[2]);
664
666
667 for(
MInt i = 0; i < 4; i++) {
669 for(
MInt j = 0; j < 4; j++) {
671 }
672 }
673
675 for(
MInt i = 0; i < 4; i++)
677 for(
MInt i = 0; i < 4; i++)
679
680
681 K.fill(F0);
683 K(1, 1) = K(0, 0);
685
686
688 accDrag.fill(F0);
691 for(
MInt i = 0; i < nDim; i++) {
692 accLiftHat[i] = addFacCd * facCL * magRelVelHat * dirLHat[i];
693 accDragHat[i] = addFacCd * facCD * magRelVelHat * dirDHat[i];
694 accHat[i] = accDragHat[i] + accLiftHat[i];
695 }
697
698 for(
MInt i = 0; i < nDim; i++) {
699 m_accel[i] = accDrag[i] + (1.0 - invDensityRatio) *
s_Frm[i];
703 }
704 break;
705 }
706 default: {
707 mTerm(1, AT_,
"Unknown particle corrector equation type!");
708 }
709 }
710 }
711
712#ifdef LPT_DEBUG
715 }
716#endif
717
718
720
721#ifdef LPT_DEBUG
722 for(
MInt i = 0; i < nDim; i++) {
727 << fluidVel[0] << " " << fluidVel[1] << " " << fluidVel[nDim - 1] << fluidDensity << " " << T << " "
729 }
730 }
731#endif
732}
void checkCellChange(const MFloat *oldPosition, const MBool allowHaloNonLeaf=false)
Checks whether position is within cell with number cellId if not, cellId is updated.
MFloat liftFactor(const MFloat partRe, const MFloat beta, const MFloat inclinationAngle, const MFloat K0, const MFloat K2)
Calculates the lift factor for current particle with given Re number, aspect ratio,...
static MFloat s_lengthFactor
MFloat fParticleRelTime(const MFloat dynamicViscosity)
Calculates the reciprocal of the particle relaxation time.
MFloat torqueFactor(const MFloat partRe, const MFloat beta, const MFloat inclinationAngle)
Calculates the torque factor for current particle with given Re number, aspect ratio and inclination ...
void interpolateVelocityAndVelocitySlopes(const MInt cellId, const MFloat *const position, MFloat *const velocity, MFloat *const velocityGradient, std::vector< MFloat > &weights)
MFloat m_fluidVelMag
fluid velocity magnitude
MFloat particleRe(const MFloat velocity, const MFloat density, const MFloat dynamicViscosity)
Calculates the particle Reynolds number.
MFloat magRelVel(const MFloat *const velocity1, const MFloat *const velocity2) const
Calculates the magnitude of the relative velocity of the two given velocity vectors.
MFloat dragFactor(const MFloat partRe, const MFloat beta=1, const MFloat inclinationAngle=0, const MFloat K0=0, const MFloat K2=0)
Calculates the drag factor for current particle with given Re number, aspect ratio and inclination an...
constexpr T mMax(const T &x, const T &y)
void cross(const T *const u, const T *const v, T *const c)
void matrixVectorProduct(MFloat *c, MFloatScratchSpace &A, MFloat *b)
c=A*b
void invert(MFloat *A, const MInt m, const MInt n)