MercuryDPM  Trunk
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Pages
SuperQuadricParticle.cc
Go to the documentation of this file.
1 //Copyright (c) 2013-2020, The MercuryDPM Developers Team. All rights reserved.
2 //For the list of developers, see <http://www.MercuryDPM.org/Team>.
3 //
4 //Redistribution and use in source and binary forms, with or without
5 //modification, are permitted provided that the following conditions are met:
6 // * Redistributions of source code must retain the above copyright
7 // notice, this list of conditions and the following disclaimer.
8 // * Redistributions in binary form must reproduce the above copyright
9 // notice, this list of conditions and the following disclaimer in the
10 // documentation and/or other materials provided with the distribution.
11 // * Neither the name MercuryDPM nor the
12 // names of its contributors may be used to endorse or promote products
13 // derived from this software without specific prior written permission.
14 //
15 //THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
16 //ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
17 //WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
18 //DISCLAIMED. IN NO EVENT SHALL THE MERCURYDPM DEVELOPERS TEAM BE LIABLE FOR ANY
19 //DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
20 //(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
21 //LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
22 //ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
23 //(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
24 //SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
25 
26 #include "DPMBase.h"
27 #include "BaseParticle.h"
28 #include <cmath>
29 #include "SuperQuadricParticle.h"
30 #include "InteractionHandler.h"
31 #include "ParticleHandler.h"
32 #include "SpeciesHandler.h"
33 
39  : BaseParticle()
40 {
41  axes_ = Vec3D(1.0, 1.0, 1.0);
42  eps1_ = 1.0;
43  eps2_ = 1.0;
44  logger(DEBUG, "SuperQuadricParticle::SuperQuadricParticle() finished");
45 }
46 
56  : BaseParticle(p)
57 {
58  axes_ = p.axes_;
59  eps1_ = p.eps1_;
60  eps2_ = p.eps2_;
61 }
62 
63 
65 {
66  Mdouble radius = p.getRadius();
67  axes_ = Vec3D(radius, radius, radius);
68  eps1_ = 1.0;
69  eps2_ = 1.0;
70 }
71 
77 {
78  if (getHandler() != nullptr)
79  {
81  }
82  logger(DEBUG, "SuperQuadricParticle::SuperQuadricParticleParticle() of particle % finished.", getId());
83 
84 }
85 
91 {
92  return new SuperQuadricParticle(*this);
93 }
94 
100 void SuperQuadricParticle::write(std::ostream& os) const
101 {
103  os << " axes " << axes_
104  << " exp1 " << eps1_
105  << " exp2 " << eps2_;
106 }
107 
112 std::string SuperQuadricParticle::getName() const
113 {
114  return "SuperQuadricParticle";
115 }
116 
117 
122 void SuperQuadricParticle::read(std::istream& is)
123 {
124  BaseParticle::read(is);
125  std::string dummy;
126  is >> dummy >> axes_ >> dummy >> eps1_ >> dummy >> eps2_;
127  logger.assert_always(eps1_ < 1 + 1e-10, "epsilon1 should be at most 1");
128  logger.assert_always(eps2_ < 1 + 1e-10, "epsilon2 should be at most 1");
129 }
130 
131 // Set and Get Functions
132 
133 void SuperQuadricParticle::setAxesAndExponents(const Mdouble& a1, const Mdouble& a2, const Mdouble& a3, const Mdouble& eps1,
134  const Mdouble& eps2)
135 {
136  eps1_ = eps1;
137  eps2_ = eps2;
138  logger.assert_always(eps1_ < 1 + 1e-10, "epsilon1 should be at most 1");
139  logger.assert_always(eps2_ < 1 + 1e-10, "epsilon2 should be at most 1");
140 
141  setAxes(a1,a2,a3);
142 }
143 
144 void SuperQuadricParticle::setAxesAndExponents(const Vec3D& axes, const Mdouble& eps1, const Mdouble& eps2)
145 {
146  eps1_ = eps1;
147  eps2_ = eps2;
148  logger.assert_always(eps1_ < 1 + 1e-10, "epsilon1 should be at most 1");
149  logger.assert_always(eps2_ < 1 + 1e-10, "epsilon2 should be at most 1");
150 
151  setAxes(axes);
152 }
153 
155 {
156  axes_ = axes;
158  if (getSpecies() != nullptr)
159  {
160  getSpecies()->computeMass(this);
161  }
162 }
163 
164 void SuperQuadricParticle::setAxes(const Mdouble& a1, const Mdouble& a2, const Mdouble& a3)
165 {
166  setAxes({a1,a2,a3});
167 }
168 
169 void SuperQuadricParticle::setExponents(const Mdouble& eps1, const Mdouble& eps2)
170 {
171  eps1_ = eps1;
172  eps2_ = eps2;
174  logger.assert_always(eps1_ < 1 + 1e-10, "epsilon1 should be at most 1");
175  logger.assert_always(eps2_ < 1 + 1e-10, "epsilon2 should be at most 1");
176  if (getSpecies() != nullptr)
177  {
178  getSpecies()->computeMass(this);
179  }
180 }
181 
183 {
184  return axes_;
185 }
186 
188 {
189  return eps1_;
190 }
191 
193 {
194  return eps2_;
195 }
196 
205 {
206  logger.assert_debug(getHandler() != nullptr, "[SuperQuadricParticle::getVolume] no particle handler specified");
207 
208  return (2.0 * axes_.X * axes_.Y * axes_.Z * eps1_ * eps2_) * mathsFunc::beta(0.5 * eps1_ + 1.0, eps1_) *
209  mathsFunc::beta(0.5 * eps2_, 0.5 * eps2_);
210 }
211 
217 {
218  MatrixSymmetric3D inertia;
219 
220  inertia.XX = getSpecies()->getDensity() * (0.5 * axes_.X * axes_.Y * axes_.Z * eps1_ * eps2_) *
221  (axes_.Y * axes_.Y * mathsFunc::beta(1.5 * eps2_, 0.5 * eps2_) *
222  mathsFunc::beta(0.5 * eps1_, 2.0 * eps1_ + 1.0)
223  + 4.0 * axes_.Z * axes_.Z * mathsFunc::beta(0.5 * eps2_, 0.5 * eps2_ + 1) *
224  mathsFunc::beta(1.5 * eps1_, eps1_ + 1.0));
225 
226  inertia.YY = getSpecies()->getDensity() * (0.5 * axes_.X * axes_.Y * axes_.Z * eps1_ * eps2_) *
227  (axes_.X * axes_.X * mathsFunc::beta(1.5 * eps2_, 0.5 * eps2_) *
228  mathsFunc::beta(0.5 * eps1_, 2.0 * eps1_ + 1.0)
229  + 4.0 * axes_.Z * axes_.Z * mathsFunc::beta(0.5 * eps2_, 0.5 * eps2_ + 1) *
230  mathsFunc::beta(1.5 * eps1_, eps1_ + 1.0));
231 
232  inertia.ZZ = getSpecies()->getDensity() * (0.5 * axes_.X * axes_.Y * axes_.Z * eps1_ * eps2_) *
233  (axes_.X * axes_.X + axes_.Y * axes_.Y) * mathsFunc::beta(1.5 * eps2_, 0.5 * eps2_) *
234  mathsFunc::beta(0.5 * eps1_, 2.0 * eps1_ + 1.0);
235 
236  BaseParticle::setInertia(inertia);
237 }
238 
240 {
241  logger(ERROR,"This function should not be used");
242 }
243 
245 {
246 
247  if(mathsFunc::isEqual(eps2_,1,std::numeric_limits<Mdouble>::epsilon()) && mathsFunc::isEqual(eps1_,1,std::numeric_limits<Mdouble>::epsilon()))
248  {
249  BaseParticle::setRadius(std::max(std::max(axes_.Y,axes_.X),axes_.Z));
250  return;
251  }else
252  {
253 
254  const Mdouble axesX = std::max(axes_.X, axes_.Y);
255  const Mdouble axesY = std::min(axes_.X, axes_.Y);
256 
257  Mdouble alpha;
258 
259  const Mdouble eps1 = std::min(.96, eps1_);
260  const Mdouble eps2 = std::min(.96, eps2_);
261 
262  alpha = std::pow(axesY / axesX, 2.0 / (2.0 / eps2 - 2.0));
263 
264  const Mdouble help1 = std::pow(alpha, 2.0 / eps2);
265  const Mdouble gamma = std::pow(1.0 + help1, eps2 / eps1 - 1.0);
266  const Mdouble beta = std::pow(gamma * axes_.Z * axes_.Z / (axesX * axesX), 1.0 / (2.0 / eps1 - 2.0));
267  const Mdouble xTilde = std::pow(std::pow(1 + help1, eps2 / eps1) + std::pow(beta, 2.0 / eps1),
268  -eps1 / 2.0);
269  BaseParticle::setRadius(std::sqrt(mathsFunc::square(axesX * xTilde)
270  + mathsFunc::square(alpha * axesY * xTilde)
271  + mathsFunc::square(beta * axes_.Z * xTilde)));
272 
273  }
274 }
275 
287  InteractionHandler* const interactionHandler)
288 {
289  //get the normal (from P away from the contact)
290  const LabFixedCoordinates branchVector = p->getPosition() - getPosition();
291  //Get the square of the distance between particle i and particle j
292  const Mdouble distanceSquared = Vec3D::getLengthSquared(branchVector);
293  auto mixedSpecies = getSpecies()->getHandler()->getMixedObject(getSpecies(),p->getSpecies());
294 
295  const Mdouble sumOfInteractionRadii = getMaxInteractionRadius()+p->getMaxInteractionRadius();
296  if (distanceSquared < (sumOfInteractionRadii * sumOfInteractionRadii))
297  {
298  if (p->isSphericalParticle())
299  {
301  pQuad->setAxes(p->getRadius(), p->getRadius(), p->getRadius());
302  BaseInteraction* contacts = getInteractionWithSuperQuad(pQuad, timeStamp, interactionHandler);
303  delete pQuad;
304  return contacts;
305  } else {
306  SuperQuadricParticle* pQuad = static_cast<SuperQuadricParticle*>(p);
307  return getInteractionWithSuperQuad(pQuad, timeStamp, interactionHandler);
308  }
309  }
310  return nullptr;
311 }
312 
324  InteractionHandler* const interactionHandler)
325 {
326  BaseInteraction* const C = interactionHandler->getInteraction(p, this, timeStamp);
327  const SmallVector<4> approximateContactPoint = getContactPoint(p, C);
328 
329  //set the new contact point:
330  LabFixedCoordinates contactPoint;
331  contactPoint.X = approximateContactPoint[0];
332  contactPoint.Y = approximateContactPoint[1];
333  contactPoint.Z = approximateContactPoint[2];
334 
335  //if the minimum is positive, there is no contact: return empty vector
336  if (computeShape(contactPoint) > -1e-10)
337  {
338  interactionHandler->removeObject(C->getIndex());
339  return nullptr;
340  }
341 
342  C->setContactPoint(contactPoint);
343  C->setLagrangeMultiplier(approximateContactPoint[3]);
344 
345  SmallVector<3> gradThis = computeShapeGradientLabFixed(contactPoint).getNormalised();
346  LabFixedCoordinates normal;
347  normal.X = gradThis[0];
348  normal.Y = gradThis[1];
349  normal.Z = gradThis[2];
350  C->setNormal(normal);
351 
352  const Mdouble alphaI = overlapFromContactPoint(contactPoint, normal);
353  const Mdouble alphaJ = p->overlapFromContactPoint(contactPoint, -normal);
354  C->setOverlap(alphaI + alphaJ);
356  C->setDistance((getPosition() - p->getPosition()).getLength() - C->getOverlap());
357 
358  return C;
359 }
360 
373 {
374  //Assuming contact between two spheres
375  SmallVector<4> approximateContactPoint = getInitialGuessForContact(p, C);
376  if (computeContactPoint(approximateContactPoint, this, p))
377  {
378  return approximateContactPoint;
379  }
380  else
381  {
382  return getContactPointPlanB(p, 4);
383  }
384 }
385 
394  const LabFixedCoordinates& normal) const
395 {
396  Mdouble alphaI = 0;
397  LabFixedCoordinates xEdge = contactPoint + alphaI * normal;
398  Mdouble dampingCoefficient = 1;
399  while (std::abs(computeShape(xEdge)) > 1e-10)
400  {
401  SmallVector<3> gradientShape = computeShapeGradientLabFixed(xEdge);
402  LabFixedCoordinates gradient;
403  gradient.X = gradientShape(0);
404  gradient.Y = gradientShape(1);
405  gradient.Z = gradientShape(2);
406  Mdouble newValue = alphaI - dampingCoefficient * computeShape(xEdge) / Vec3D::dot(gradient, normal);
407  const LabFixedCoordinates xEdgeNew = contactPoint + newValue * normal;
408 
409  if (std::abs(computeShape(xEdgeNew)) < std::abs(computeShape(xEdge)))
410  {
411  alphaI = newValue;
412  xEdge = xEdgeNew;
413  dampingCoefficient = 1;
414  }
415  else
416  {
417  dampingCoefficient /= 2;
418  if (dampingCoefficient < 1e-10)
419  {
420  logger(ERROR, "overlap detection does not converge");
421  }
422  }
423  }
424  return alphaI;
425 }
426 
437 {
438  SmallVector<4> approximateContactPoint;
439  if (C == nullptr || C->getOverlap() < 1e-15)
440  {
441  //this is a new interaction
442  const LabFixedCoordinates branchVector = pQuad->getPosition() - getPosition();
443  //Get the square of the distance between particle i and particle j
444  const Mdouble distanceSquared = Vec3D::getLengthSquared(branchVector);
445  const Mdouble distance = sqrt(distanceSquared);
446  const LabFixedCoordinates normal = (branchVector / distance);
447  const Mdouble overlap = getSumOfInteractionRadii(pQuad) - distance;
448  const LabFixedCoordinates contactPoint = (pQuad->getPosition() -
449  (pQuad->getInteractionRadius(this) - 0.5 * overlap) * normal);
450 
451  approximateContactPoint[0] = contactPoint.X;
452  approximateContactPoint[1] = contactPoint.Y;
453  approximateContactPoint[2] = contactPoint.Z;
454  approximateContactPoint[3] = 1;
455  }
456  else
457  {
458  //this is an existing interaction
459  const LabFixedCoordinates contactPoint = C->getContactPoint();
460  const Mdouble multiplier = C->getLagrangeMultiplier();
461  approximateContactPoint[0] = contactPoint.X;
462  approximateContactPoint[1] = contactPoint.Y;
463  approximateContactPoint[2] = contactPoint.Z;
464  approximateContactPoint[3] = multiplier;
465  }
466  return approximateContactPoint;
467 }
468 
475 {
476  BodyFixedCoordinates bodyFixedCoordinates = labFixedCoordinates - this->getPosition();
477  getOrientation().rotateBack(bodyFixedCoordinates);
478 
479  const Mdouble n1 = 2. / eps1_;
480  const Mdouble n2 = 2. / eps2_;
481 
482  return std::pow(std::pow(std::abs(bodyFixedCoordinates.X / axes_.X), n2)
483  + std::pow(std::abs(bodyFixedCoordinates.Y / axes_.Y), n2), n1 / n2)
484  + std::pow(std::abs(bodyFixedCoordinates.Z / axes_.Z), n1) - 1.0;
485 }
486 
495 {
496 
497  BodyFixedCoordinates bodyFixedCoordinates = labFixedCoordinates - this->getPosition();
498  getOrientation().rotateBack(bodyFixedCoordinates);
499 
500  const Mdouble n1 = 2. / eps1_;
501  const Mdouble n2 = 2. / eps2_;
502 
503  const Mdouble absXoa = std::abs(bodyFixedCoordinates.X / axes_.X);
504  const Mdouble absYob = std::abs(bodyFixedCoordinates.Y / axes_.Y);
505  const Mdouble absZoc = std::abs(bodyFixedCoordinates.Z / axes_.Z);
506 
507  const Mdouble nu = std::pow(absXoa, n2) + std::pow(absYob, n2);
508 
509  const Mdouble help1 = std::pow(nu, n1 / n2 - 1.0);
510 
511  Vec3D gradientBodyFixed = {
512  (n1 / axes_.X) * std::pow(absXoa, n2 - 1.0) * help1 * mathsFunc::sign(bodyFixedCoordinates.X),
513  (n1 / axes_.Y) * std::pow(absYob, n2 - 1.0) * help1 * mathsFunc::sign(bodyFixedCoordinates.Y),
514  (n1 / axes_.Z) * std::pow(absZoc, n1 - 1.0) * mathsFunc::sign(bodyFixedCoordinates.Z)};
515  //we get the gradient in terms of the lab-fixed coordinates by \nabla_X F(X) = A \nabla_x f(x).
516  getOrientation().rotate(gradientBodyFixed);
517  return {gradientBodyFixed.X, gradientBodyFixed.Y, gradientBodyFixed.Z};
518 }
519 
528 {
529  SmallMatrix<3, 3> hessian;
530  BodyFixedCoordinates bodyFixedCoordinates = labFixedCoordinates - this->getPosition();
531  getOrientation().rotateBack(bodyFixedCoordinates);
532 
533  const Mdouble n1 = 2.0 / eps1_;
534  const Mdouble n2 = 2.0 / eps2_;
535 
536  const Mdouble absXoa = std::abs(bodyFixedCoordinates.X / axes_.X);
537  const Mdouble absYob = std::abs(bodyFixedCoordinates.Y / axes_.Y);
538  const Mdouble absZoc = std::abs(bodyFixedCoordinates.Z / axes_.Z);
539 
540  const Mdouble nu = std::pow(absXoa, n2) + std::pow(absYob, n2);
541  const Mdouble help1 = std::pow(nu, n1 / n2 - 1.0);
542  const Mdouble help2 = std::pow(nu, n1 / n2 - 2.0);
543 
544  hessian(0, 0) = n1 * (n2 - 1) / axes_.X / axes_.X * std::pow(absXoa, n2 - 2.0) * help1
545  + ((n1 - n2) * n1 / axes_.X / axes_.X) * std::pow(absXoa, 2. * n2 - 2.0) * help2;
546  hessian(1, 1) = n1 * (n2 - 1) / axes_.Y / axes_.Y * std::pow(absYob, n2 - 2.0) * help1
547  + ((n1 - n2) * n1 / axes_.Y / axes_.Y) * std::pow(absYob, 2. * n2 - 2.0) * help2;
548  hessian(2, 2) = n1 * (n1 - 1) / axes_.Z / axes_.Z * std::pow(absZoc, n1 - 2.0);
549  hessian(1, 0) = n1 * (n1 - n2) / axes_.X / axes_.Y * std::pow(absXoa, n2 - 1.0) * std::pow(absYob, n2 - 1.0) *
550  help2 * mathsFunc::sign(bodyFixedCoordinates.X * bodyFixedCoordinates.Y);
551  hessian(0, 1) = hessian(1, 0);
554  return A * hessian * (A.transpose());
555 }
556 
568  const SuperQuadricParticle* const p1,
569  const SuperQuadricParticle* const p2) const
570 {
571  LabFixedCoordinates labFixedCoordinates;
572  labFixedCoordinates.X = position[0];
573  labFixedCoordinates.Y = position[1];
574  labFixedCoordinates.Z = position[2];
575  const Mdouble lagrangeMultiplier = position[3];
576 
577  // First compute the gradient of the superquadric shape function and then rotate it to the
578  // global frame of reference
579  SmallVector<3> gradThis = p1->computeShapeGradientLabFixed(labFixedCoordinates);
580 
581  SmallVector<3> gradOther = p2->computeShapeGradientLabFixed(labFixedCoordinates);
582 
583  // Eqn. (23) in Podhlozhnyuk et al, Comp. Part. Mech. 2017.
584  // where contactPoint[3] = mu
585  SmallVector<4> toBecomeZero;
586  for (unsigned int i = 0; i < 3; ++i)
587  {
588  toBecomeZero[i] = gradThis[i] + lagrangeMultiplier * lagrangeMultiplier * gradOther[i];
589  }
590  toBecomeZero[3] = p1->computeShape(labFixedCoordinates) - p2->computeShape(labFixedCoordinates);
591 
592  return toBecomeZero;
593 }
594 
625  const SuperQuadricParticle* const p1,
626  const SuperQuadricParticle* const p2) const
627 {
628  LabFixedCoordinates labFixedCoordinates;
629  labFixedCoordinates.X = contactPoint[0];
630  labFixedCoordinates.Y = contactPoint[1];
631  labFixedCoordinates.Z = contactPoint[2];
632  const Mdouble lagrangeMultiplier = contactPoint[3];
633 
634  SmallVector<3> gradThis = p1->computeShapeGradientLabFixed(labFixedCoordinates);
635  SmallVector<3> gradOther = p2->computeShapeGradientLabFixed(labFixedCoordinates);
636 
637  SmallMatrix<3, 3> hessianThis = p1->computeHessianLabFixed(labFixedCoordinates);
638  SmallMatrix<3, 3> hessianOther = p2->computeHessianLabFixed(labFixedCoordinates);
639 
640  SmallMatrix<3, 3> upperLeftCorner = hessianThis + lagrangeMultiplier * lagrangeMultiplier * hessianOther;
641 
642  SmallMatrix<4, 4> jacobian;
643  for (unsigned int i = 0; i < 3; ++i)
644  {
645  for (unsigned int j = 0; j < 3; ++j)
646  {
647  jacobian(i, j) = upperLeftCorner(i, j);
648  }
649  jacobian(i, 3) = 2 * lagrangeMultiplier * gradOther[i];
650  }
651 
652  for (unsigned int j = 0; j < 3; ++j)
653  {
654  jacobian(3, j) = gradThis[j] - gradOther[j];
655  }
656 
657  return jacobian;
658 }
659 
666 {
667  SmallVector<3> gradientVec = computeShapeGradientLabFixed(labFixedCoordinates);
668  SmallMatrix<3, 1> gradient = gradientVec;
669  SmallMatrix<3, 3> hessian = computeHessianLabFixed(labFixedCoordinates);
670  Mdouble helper = ((gradient.transpose() * hessian) * gradient)(0, 0);
671  Mdouble gradientLength = gradientVec.length();
672  Mdouble helper2 = gradientLength * gradientLength * (hessian(0, 0) + hessian(1, 1) + hessian(2, 2));
673  Mdouble denominator = 2 * gradientLength * gradientLength * gradientLength;
674 
676  return (helper2 - helper) / denominator;
677 }
678 
687 {
688  SmallVector<4> approximateContactPoint;
689 
690  if (p->isSphericalParticle())
691  {
693  pQuad->setAxes(p->getRadius(), p->getRadius(), p->getRadius());
694  approximateContactPoint = getContactPoint(pQuad, nullptr);
695  delete pQuad;
696  } else {
697  const SuperQuadricParticle* pQuad = static_cast<const SuperQuadricParticle*>(p);
698  approximateContactPoint = getContactPoint(pQuad, nullptr);
699  }
700 
701  //set the new contact point:
702  LabFixedCoordinates contactPoint;
703  contactPoint.X = approximateContactPoint[0];
704  contactPoint.Y = approximateContactPoint[1];
705  contactPoint.Z = approximateContactPoint[2];
706 
707  //if the minimum is positive, there is no contact: return false
708  return (computeShape(contactPoint) < 0);
709 
710 }
711 
721 SmallVector<4> SuperQuadricParticle::getContactPointPlanB(const SuperQuadricParticle* const pOther, unsigned numberOfSteps) const
722 {
723  logger(VERBOSE, "Number of iterations: %", numberOfSteps);
724  // Step 1: Compute contact point for two volume equivalent spheres
725  const Mdouble interactionRadiusThis = getInteractionRadius(pOther);
726  const Mdouble interactionRadiusOther = pOther->getInteractionRadius(this);
727  const Vec3D positionThis = getPosition();
728  const Vec3D positionOther = pOther->getPosition();
729 
730  //analytical solution for contact point between bounding spheres
731  const Vec3D initialPoint = (interactionRadiusOther * positionThis + interactionRadiusThis * positionOther) /
732  (interactionRadiusThis + interactionRadiusOther);
733 
734  SmallVector<4> contactPointPlanB;
735  // Analytical solution
736  contactPointPlanB[0] = initialPoint.X;
737  contactPointPlanB[1] = initialPoint.Y;
738  contactPointPlanB[2] = initialPoint.Z;
739  contactPointPlanB[3] = 1.0; //Need to check.
740 
741  writeDebugMessageStep1(pOther, contactPointPlanB);
742 
743  //Step 2: Compute the deltas
744  const Vec3D dAxesThis = (getAxes() - interactionRadiusThis * Vec3D(1, 1, 1)) / numberOfSteps;
745  const Mdouble dn11 = (2.0 / getExponentEps1() - 2.0) / numberOfSteps;
746  const Mdouble dn12 = (2.0 / getExponentEps2() - 2.0) / numberOfSteps;
747 
748  const Vec3D dAxesOther = (pOther->getAxes() - interactionRadiusOther * Vec3D(1, 1, 1)) / numberOfSteps;
749  const Mdouble dn21 = (2.0 / pOther->getExponentEps1() - 2.0) / numberOfSteps;
750  const Mdouble dn22 = (2.0 / pOther->getExponentEps2() - 2.0) / numberOfSteps;
751 
752  writeDebugMessageStep2(pOther, dAxesThis, dn11, dn12, dAxesOther, dn21, dn22);
753 
754  // Create two superquadrics with the above parameters for incrementally computing the contact point
757 
758  p1.setPosition(getPosition());
759  p2.setPosition(pOther->getPosition());
760 
762  p2.setOrientation(pOther->getOrientation());
763  bool success = true;
764  const unsigned maxIterations = 20;
765  for (unsigned int counter = 1; counter <= numberOfSteps; ++counter)
766  {
767 
768  // Step 3: Increment the shape and blockiness parameters
769  const Vec3D axesThis = interactionRadiusThis * Vec3D(1, 1, 1) + counter * dAxesThis;
770  const Mdouble n11 = 2.0 + counter * dn11;
771  const Mdouble n12 = 2.0 + counter * dn12;
772 
773  Vec3D axesOther = interactionRadiusOther * Vec3D(1, 1, 1) + counter * dAxesOther;
774  const Mdouble n21 = 2.0 + counter * dn21;
775  const Mdouble n22 = 2.0 + counter * dn22;
776 
777  p1.setAxesAndExponents(axesThis, 2.0 / n11, 2.0 / n12);
778  p2.setAxesAndExponents(axesOther, 2.0 / n21, 2.0 / n22);
779 
780  writeDebugMessageStep3(axesThis, n11, n12, axesOther, n21, n22);
781 
782  writeDebugMessageMiddleOfLoop(p1, p2, contactPointPlanB, counter);
783 
784  //compute the contact point of the new particles
785  success = computeContactPoint(contactPointPlanB, &p1, &p2);
786  if (!success)
787  {
788  if (numberOfSteps > maxIterations)
789  {
790  write(std::cout);
791  pOther->write(std::cout);
792  logger(ERROR, "Plan B fails even with more than 20 intermediate steps");
793  }
794  return getContactPointPlanB(pOther, 2 * numberOfSteps);
795  }
796  }
797  logger.assert_debug(p1.getAxes().X == getAxes().X, "In getContactPointPlanB, final particle for contact-detection not "
798  "the same as original particle");
799 
800  logger(VERBOSE, "Plan B contact point: %", contactPointPlanB);
801 
802  return contactPointPlanB;
803 }
804 
818  const SuperQuadricParticle* const p2) const
819 {
820  // Damped Newton's method: (dampingFactor 1 is undamped)
821  Mdouble dampingFactor = 1;
822  int iteration = 1;
823  const Mdouble tolerance = 1e-5;
824  const unsigned maxIterations = 100;
825 
826  logger(VERBOSE, "func to be zero value: % \n",
827  computeResidualContactDetection(contactPoint, p1, p2));
828 
829  while (computeResidualContactDetection(contactPoint, p1, p2).length() > tolerance && iteration < maxIterations)
830  {
831  logger(VERBOSE, "Iteration: %\n", iteration);
832 
833  SmallMatrix<4, 4> jacobian = getJacobianOfContactDetectionObjective(contactPoint, p1, p2);
834  SmallVector<4> func = -computeResidualContactDetection(contactPoint, p1, p2);
835  jacobian.solve(func); //note: solve is in place
836 
837  SmallVector<4> newPoint = contactPoint + dampingFactor * func;
838  const auto residueOld = computeResidualContactDetection(contactPoint, p1, p2);
839  const auto residueNew = computeResidualContactDetection(newPoint, p1, p2);
840  logger(VERBOSE, "Old value: % (%) new value: % (%)",
841  computeResidualContactDetection(contactPoint, p1, p2),
842  computeResidualContactDetection(contactPoint, p1, p2).length(),
843  computeResidualContactDetection(newPoint, p1, p2),
844  computeResidualContactDetection(newPoint, p1, p2).length());
845 
846  logger(VERBOSE, "current contact point: %, new contact point: %\n", contactPoint, newPoint);
847  logger(VERBOSE, "damping factor: %", dampingFactor);
848 
849  if (residueNew.length()
850  < residueOld.length())
851  {
852  contactPoint = newPoint;
853  dampingFactor = 1;
854  }
855  else
856  {
857  dampingFactor /= 2;
858 
859  if (dampingFactor < 1e-10)
860  {
861  return false;
862  }
863  }
864 
865  iteration++;
866  }
867  return (iteration != maxIterations);
868 }
869 
871  SmallVector<4>& contactPointPlanB, const unsigned int& counter) const
872 {
873  logger(VERBOSE, "Position of particle 1 (p1): % \nPosition of particle 2 (p2): % \n",
874  p1.getPosition(), p2.getPosition());
875  logger(VERBOSE, "Orientation of particle 1 (p1): % \nOrientation of particle 2 (p2): %\n",
876  p1.getOrientation(), p2.getOrientation());
877 
878  // Step 4: Calculate the contact point for the updated shape parameters
879 
880  logger(VERBOSE, "----------------------------------------");
881  logger(VERBOSE, "STEP 4: Compute contact point");
882  logger(VERBOSE, "----------------------------------------");
883  logger(VERBOSE, " ");
884  logger(VERBOSE, "Counter: %", counter);
885  logger(VERBOSE, " ");
886 
887  logger(VERBOSE, "Analytical solution Contact Point: % ", contactPointPlanB);
888  logger(VERBOSE, " ");
889 }
890 
891 void
892 SuperQuadricParticle::writeDebugMessageStep3(const Vec3D& axesThis, const Mdouble& n11, const Mdouble& n12,
893  const Vec3D& axesOther, const Mdouble& n21, const Mdouble& n22) const
894 {
895  logger(VERBOSE, "-----------------------------------");
896  logger(VERBOSE, "STEP 3: First increment");
897  logger(VERBOSE, "-----------------------------------");
898  logger(VERBOSE, " ");
899 
900  logger(VERBOSE, "Particle 1 x-scale after increment: %", axesThis.X);
901  logger(VERBOSE, "Particle 1 y-scale after increment: %", axesThis.Y);
902  logger(VERBOSE, "Particle 1 z-scale after increment: %", axesThis.Z);
903  logger(VERBOSE, "Particle 1 n1 after increment: %", n11);
904  logger(VERBOSE, "Particle 1 n2 after increment: %", n12);
905  logger(VERBOSE, " ");
906 
907  logger(VERBOSE, "Particle 2 x-scale after increment: %", axesOther.X);
908  logger(VERBOSE, "Particle 2 y-scale after increment: %", axesOther.Y);
909  logger(VERBOSE, "Particle 2 z-scale after increment: %", axesOther.Z);
910  logger(VERBOSE, "Particle 2 n1 after increment: %", n21);
911  logger(VERBOSE, "Particle 2 n2 after increment: %", n22);
912  logger(VERBOSE, " ");
913 }
914 
915 void
917  const Mdouble& dn12, const Vec3D& dAxesOther, const Mdouble& dn21,
918  const Mdouble& dn22) const
919 {
920  logger(VERBOSE, "---------------------");
921  logger(VERBOSE, "STEP 2");
922  logger(VERBOSE, "---------------------");
923  logger(VERBOSE, " ");
924 
925  logger(VERBOSE, "Particle 1 x-scale: %", pQuad->getAxes().X);
926  logger(VERBOSE, "Particle 1 y-scale: %", pQuad->getAxes().Y);
927  logger(VERBOSE, "Particle 1 z-scale: %", pQuad->getAxes().Z);
928  logger(VERBOSE, "Particle 1 n1: %", 2. / pQuad->getExponentEps1());
929  logger(VERBOSE, "Particle 1 n2: %", 2. / pQuad->getExponentEps2());
930  logger(VERBOSE, " ");
931 
932  logger(VERBOSE, "Particle 1 x-scale increment: %", dAxesThis.X);
933  logger(VERBOSE, "Particle 1 y-scale increment: %", dAxesThis.Y);
934  logger(VERBOSE, "Particle 1 z-scale increment: %", dAxesThis.Z);
935  logger(VERBOSE, "Particle 1 n1 increment: %", dn11);
936  logger(VERBOSE, "Particle 1 n2 increment: %", dn12);
937  logger(VERBOSE, " ");
938 
939  logger(VERBOSE, "Particle 2 x-scale: %", getAxes().X);
940  logger(VERBOSE, "Particle 2 y-scale: %", getAxes().Y);
941  logger(VERBOSE, "Particle 2 z-scale: %", getAxes().Z);
942  logger(VERBOSE, "Particle 2 n1: %", 2. / getExponentEps1());
943  logger(VERBOSE, "Particle 2 n2: %", 2. / getExponentEps2());
944  logger(VERBOSE, " ");
945 
946  logger(VERBOSE, "Particle 2 x-scale increment: %", dAxesOther.X);
947  logger(VERBOSE, "Particle 2 y-scale increment: %", dAxesOther.Y);
948  logger(VERBOSE, "Particle 2 z-scale increment: %", dAxesOther.Z);
949  logger(VERBOSE, "Particle 2 n1 increment: %", dn21);
950  logger(VERBOSE, "Particle 2 n2 increment: %", dn22);
951  logger(VERBOSE, " ");
952 }
953 
954 void SuperQuadricParticle::writeDebugMessageStep1(const SuperQuadricParticle* pQuad, const SmallVector<4>& contactPointPlanB) const
955 {
956  logger(VERBOSE, "---------------------");
957  logger(VERBOSE, "STEP 1");
958  logger(VERBOSE, "---------------------\n");
959 
960  logger(VERBOSE, "Position of particle 1: % ", getPosition());
961  logger(VERBOSE, "Position of particle 2 (pQuad): % ", pQuad->getPosition());
962  logger(VERBOSE, " ");
963 
964  logger(VERBOSE, "Radius of particle 1: % ", getInteractionRadius(pQuad));
965  logger(VERBOSE, "Radius of particle 2 (pQuad): % \n", pQuad->getInteractionRadius(this));
966 
967  logger(VERBOSE, "Orientation of particle 1: % ", getOrientation());
968  logger(VERBOSE, "Orientation of particle 2 (pQuad): % ", pQuad->getOrientation());
969  logger(VERBOSE, " ");
970 
971  logger(VERBOSE, "Particle 1 axes: %", getAxes());
972  logger(VERBOSE, "Particle 2 axes (pQuad): % \n", pQuad->getAxes());
973 
974  logger(VERBOSE, "Analytical solution for two equivalent spheres in contact: % \n", contactPointPlanB);
975 }
976 
978 {
979  const auto mixedSpecies = getSpecies()->getHandler()->getMixedObject(getSpecies(),particle->getSpecies());
980  return getRadius() + 0.5 * mixedSpecies->getInteractionDistance();
981 }
982 
985  if (isFixed()) return;
986  if (getParticleDimensions()==3) {
987  Mdouble volume = getVolume();
988 
989  Mdouble help1 = mathsFunc::beta(1.5 * eps2_, 0.5 * eps2_);
990  Mdouble help2 = mathsFunc::beta(0.5 * eps1_, 2.0 * eps1_ + 1.0);
991  Mdouble help3 = mathsFunc::beta(0.5 * eps2_, 0.5 * eps2_ + 1);
992  Mdouble help4 = mathsFunc::beta(1.5 * eps1_, eps1_ + 1.0);
993 
994  invMass_ = 1.0 / (volume * s.getDensity());
995  invInertia_.XX = 1.0 / (s.getDensity() * (0.5 * axes_.X * axes_.Y * axes_.Z * eps1_ * eps2_) *
996  (axes_.Y * axes_.Y * help1 * help2
997  + 4.0 * axes_.Z * axes_.Z * help3 * help4));
998  invInertia_.XY = 0.0;
999  invInertia_.XZ = 0.0;
1000  invInertia_.YY = 1.0 / (s.getDensity() * (0.5 * axes_.X * axes_.Y * axes_.Z * eps1_ * eps2_) *
1001  (axes_.X * axes_.X * help1 * help2
1002  + 4.0 * axes_.Z * axes_.Z * help3 * help4));
1003  invInertia_.YZ = 0.0;
1004  invInertia_.ZZ = 1.0 / (s.getDensity() * (0.5 * axes_.X * axes_.Y * axes_.Z * eps1_ * eps2_) *
1005  (axes_.X * axes_.X + axes_.Y * axes_.Y) * help1 * help2);
1006  } else {
1007  logger(ERROR, "[SuperQuadricParticle::computeMass] SuperQuadricParticle cannot be two-dimensional (yet)");
1008  }
1009 };
BaseInteraction * getInteractionWith(BaseParticle *P, unsigned timeStamp, InteractionHandler *interactionHandler) override
Checks if this superquadric is in interaction with the given particle, and if so, returns vector of p...
Mdouble getCurvature(const LabFixedCoordinates &labFixedCoordinates) const override
Get the mean curvature of this superquadric at the given (lab-fixed) position, see Podlozhyuk et al...
void read(std::istream &is) override
Particle read function, which accepts an std::istream as input.
void computeMass(const ParticleSpecies &s) override
Computes the particle's (inverse) mass and inertia.
SuperQuadricParticle * copy() const override
Copy method. It calls to copy constructor of this superquadric, useful for polymorphism.
unsigned int getId() const
Returns the unique identifier of any particular object.
Definition: BaseObject.h:125
unsigned int getIndex() const
Returns the index of the object in the handler.
Definition: BaseObject.h:118
std::string getName() const override
Returns the name of the class, here "SuperQuadricParticle".
bool computeContactPoint(SmallVector< 4 > &contactPoint, const SuperQuadricParticle *p1, const SuperQuadricParticle *p2) const
Perform the actual Newton-iterations to find the contact point. Note, that it is given back as a para...
std::enable_if<!std::is_pointer< typename U::MixedSpeciesType >::value, typename U::MixedSpeciesType * >::type getMixedObject(const U *S, const U *T)
Mdouble getInteractionRadius(const BaseParticle *particle) const
returns the radius plus half the interactionDistance of the mixed species
void checkExtremaOnDelete(BaseParticle *P)
Checks if the extrema of this ParticleHandler needs updating when a particle is deleted.
const Vec3D & getPosition() const
Returns the position of this BaseInteractable.
void setNormal(Vec3D normal)
Sets the normal vector between the two interacting objects.
Mdouble X
the vector components
Definition: Vector.h:65
void setBoundingRadius()
Get the radius of the sphere that fits precisely around the particle.
void read(std::istream &is) override
Read function: read in the information for this superquadric from the given input-stream, for example a restart file.
virtual void setInertia()
virtual bool isSphericalParticle() const
Definition: BaseParticle.h:645
void setRadius(const Mdouble radius) override
Sets the particle's radius_ (and adjusts the mass_ accordingly, based on the particle's species) ...
void rotateBack(Vec3D &position) const
Applies the inverse rotation to a position.
Definition: Quaternion.cc:592
Logger< MERCURY_LOGLEVEL > logger("MercuryKernel")
Definition of different loggers with certain modules. A user can define its own custom logger here...
void setOverlap(Mdouble overlap)
Set the overlap between the two interacting object.
double Mdouble
Definition: GeneralDefine.h:34
const ParticleSpecies * getSpecies() const
Returns a pointer to the species of this BaseInteractable.
SuperQuadricParticle()
Basic Particle constructor, creates a superquadric with axes (1,1,1) and exponents (2...
virtual void setRadius(Mdouble radius)
Sets the particle's radius_ (and adjusts the mass_ accordingly, based on the particle's species) ...
Mdouble getMaxInteractionRadius() const
Returns the particle's interaction radius, which might be different from radius_ (e.g., when dealing with wet particles)
Definition: BaseParticle.h:362
BaseInteraction * getInteractionWithSuperQuad(SuperQuadricParticle *p, unsigned timeStamp, InteractionHandler *interactionHandler)
Checks if this superquadric is in interaction with the given superquadric, and if so...
SmallMatrix< numberOfColumns, numberOfRows > transpose() const
Definition: SmallMatrix.h:330
Mdouble beta(Mdouble z, Mdouble w)
This is the beta function, returns the approximation based on cmath's implementation of ln(gamma) ...
const std::complex< Mdouble > i
Definition: ExtendedMath.h:51
bool isInContactWith(const BaseParticle *p) const override
Get whether or not this superquadric is in contact with the given particle.
~SuperQuadricParticle() override
Destructor, needs to be implemented and checked to see if it is the largest or smallest particle curr...
Mdouble invMass_
Particle radius_.
Definition: BaseParticle.h:656
Mdouble getSumOfInteractionRadii(const BaseParticle *particle) const
returns the sum of the radii plus the interactionDistance
Definition: BaseParticle.h:379
Implementation of a 3D vector (by Vitaliy).
Definition: SmallVector.h:61
void setContactPoint(Vec3D contactPoint)
Set the location of the contact point between the two interacting objects.
Mdouble length() const
Definition: SmallVector.h:253
void computeMass(BaseParticle *p) const
Compute Particle mass function, which required a reference to the Species vector. It computes the Par...
MatrixSymmetric3D invInertia_
Inverse Particle mass (for computation optimization)
Definition: BaseParticle.h:657
void setLagrangeMultiplier(Mdouble multiplier)
const Vec3D & getContactPoint() const
Gets constant reference to contact point (vector).
int sign(T val)
This is a sign function, it returns -1 for negative numbers, 1 for positive numbers and 0 for 0...
Definition: ExtendedMath.h:97
static Mdouble dot(const Vec3D &a, const Vec3D &b)
Calculates the dot product of two Vec3D: .
Definition: Vector.cc:76
void rotate(Vec3D &position) const
Applies the rotation to a position.
Definition: Quaternion.cc:563
ParticleHandler * getHandler() const
Returns pointer to the particle's ParticleHandler.
Vec3D getAxes() const override
Get the axes-lengths of this superquadric. We use the super-ellipsoid definition stated in Chapter 2 ...
void setDistance(Mdouble distance)
Sets the interaction distance between the two interacting objects.
SmallVector< 4 > getContactPoint(const SuperQuadricParticle *p, BaseInteraction *C) const
Compute the contact point between this and the given superquadric particle.
bool isEqual(Mdouble v1, Mdouble v2, Mdouble absError)
Compares the difference of two Mdouble with an absolute error, useful in UnitTests.
SmallVector< 4 > computeResidualContactDetection(const SmallVector< 4 > &position, const SuperQuadricParticle *p1, const SuperQuadricParticle *p2) const
Objective function for contact detection between the two given superquadrics. See Podlozhyuk et al...
Mdouble getLengthSquared() const
Calculates the squared length of this Vec3D: .
Definition: Vector.cc:184
Stores information about interactions between two interactable objects; often particles but could be ...
void setExponents(const Mdouble &eps1, const Mdouble &eps2) override
Set the exponents to eps1 and eps2 for this superquadric. We use the super-ellipsoid definition state...
SpeciesHandler * getHandler() const
Returns the pointer to the handler to which this species belongs.
Definition: BaseSpecies.cc:99
SmallVector< 4 > getContactPointPlanB(const SuperQuadricParticle *pOther, unsigned numberOfSteps) const
If the "normal" procedure fails to find a contact point, use an alternative approach that involves st...
Mdouble getVolume() const override
Vec3D axes_
Lengths of principal axes (a1, a2, a3).
BaseInteraction * getInteraction(BaseInteractable *P, BaseInteractable *I, unsigned timeStamp)
Returns the Interaction between the BaseInteractable's P and I.
SmallVector< 4 > getInitialGuessForContact(const SuperQuadricParticle *pQuad, BaseInteraction *C) const
Get an initial guess for the contact-point between this particle and the given particle.
virtual void removeObject(unsigned const int index)
Removes an Object from the BaseHandler.
Definition: BaseHandler.h:472
void write(std::ostream &os) const override
Particle print function, which accepts an std::ostream as input.
Container to store Interaction objects.
Mdouble eps1_
Blockiness parameters.
void solve(SmallMatrix< numberOfRows, numberOfRightHandSideColumns > &B) const
solves Ax=B where A is the current matrix and B is passed in. The result is returned in B...
Mdouble getLagrangeMultiplier()
Mdouble computeShape(const LabFixedCoordinates &labFixedCoordinates) const
Compute and get the shape-functiion at the given (lab-fixed) position.
void setInertia() override
Compute and set the inertia-tensor for this superquadric. For internal use only.
Mdouble getRadius() const
Returns the particle's radius.
Definition: BaseParticle.h:348
bool isFixed() const override
Is fixed Particle function. It returns whether a Particle is fixed or not, by checking its inverse Ma...
Definition: BaseParticle.h:93
Mdouble getOverlap() const
Returns a Mdouble with the current overlap between the two interacting objects.
Mdouble getExponentEps1() const override
Get the first exponent of this superquadric. We use the super-ellipsoid definition stated in Chapter ...
void writeDebugMessageStep1(const SuperQuadricParticle *pQuad, const SmallVector< 4 > &contactPointPlanB) const
SmallMatrix< 4, 4 > getJacobianOfContactDetectionObjective(const SmallVector< 4 > &contactPoint, const SuperQuadricParticle *p1, const SuperQuadricParticle *p2) const
Compute and return the derivative of functionThatShouldBecomeZeroForContactDetection, both to the position and the Lagrange multiplier, and evaluated at the contact point.
void setAxes(const Mdouble &a1, const Mdouble &a2, const Mdouble &a3)
Set the axes-lengths to a1, a2 and a3 for this superquadric. We use the super-ellipsoid definition st...
Mdouble Y
Definition: Vector.h:65
void setPosition(const Vec3D &position)
Sets the position of this BaseInteractable.
void getRotationMatrix(SmallMatrix< 3, 3 > &A) const
Retrieves the rotation matrix.
Definition: Quaternion.cc:508
void writeDebugMessageStep3(const Vec3D &axesThis, const Mdouble &n11, const Mdouble &n12, const Vec3D &axesOther, const Mdouble &n21, const Mdouble &n22) const
Mdouble gamma(Mdouble gamma_in)
This is the gamma function returns the true value for the half integer value.
void setAxesAndExponents(const Mdouble &a1, const Mdouble &a2, const Mdouble &a3, const Mdouble &eps1, const Mdouble &eps2)
Set the geometrical properties of the superquadrics, namely the axes-lengths a1, a2 and a3...
Mdouble getDensity() const
Allows density_ to be accessed.
void setOrientation(const Quaternion &orientation)
Sets the orientation of this BaseInteractable.
Definition: Vector.h:49
Mdouble overlapFromContactPoint(const LabFixedCoordinates &contactPoint, const LabFixedCoordinates &normal) const
Compute the distance between the contact-point and surface of this superquadric particle.
Data type for small dense matrix.
Definition: SmallMatrix.h:67
T square(const T val)
squares a number
Definition: ExtendedMath.h:106
unsigned int getParticleDimensions() const
Returns the particle's dimensions (either 2 or 3).
Mdouble getExponentEps2() const override
Get the second exponent of this superquadric. We use the super-ellipsoid definition stated in Chapter...
const Quaternion & getOrientation() const
Returns the orientation of this BaseInteractable.
Mdouble Z
Definition: Vector.h:65
SmallVector< 3 > computeShapeGradientLabFixed(const LabFixedCoordinates &labFixedCoordinates) const
Compute and get the gradient of the shape-function at the given (lab-fixed) position.
void writeDebugMessageStep2(const SuperQuadricParticle *pQuad, const Vec3D &dAxesThis, const Mdouble &dn11, const Mdouble &dn12, const Vec3D &dAxesOther, const Mdouble &dn21, const Mdouble &dn22) const
void writeDebugMessageMiddleOfLoop(const SuperQuadricParticle &p1, const SuperQuadricParticle &p2, SmallVector< 4 > &contactPointPlanB, const unsigned int &counter) const
Implementation of a 3D symmetric matrix.
void write(std::ostream &os) const override
Write function: write this superquadric to the given output-stream, for example a restart-file...
Mdouble XX
The six distinctive matrix elements.
SmallMatrix< 3, 3 > computeHessianLabFixed(const LabFixedCoordinates &labFixedCoordinates) const
Compute and get the hessian ("second derivative") of the shape-function at the given (lab-fixed) posi...