44 logger(
DEBUG,
"SuperQuadricParticle::SuperQuadricParticle() finished");
82 logger(
DEBUG,
"SuperQuadricParticle::SuperQuadricParticleParticle() of particle % finished.",
getId());
103 os <<
" axes " <<
axes_
105 <<
" exp2 " <<
eps2_;
114 return "SuperQuadricParticle";
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");
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");
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");
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");
206 logger.assert_debug(
getHandler() !=
nullptr,
"[SuperQuadricParticle::getVolume] no particle handler specified");
262 alpha = std::pow(axesY / axesX, 2.0 / (2.0 / eps2 - 2.0));
264 const Mdouble help1 = std::pow(alpha, 2.0 / eps2);
265 const Mdouble gamma = std::pow(1.0 + help1, eps2 / eps1 - 1.0);
267 const Mdouble xTilde = std::pow(std::pow(1 + help1, eps2 / eps1) + std::pow(beta, 2.0 / eps1),
296 if (distanceSquared < (sumOfInteractionRadii * sumOfInteractionRadii))
331 contactPoint.
X = approximateContactPoint[0];
332 contactPoint.
Y = approximateContactPoint[1];
333 contactPoint.
Z = approximateContactPoint[2];
347 normal.
X = gradThis[0];
348 normal.
Y = gradThis[1];
349 normal.
Z = gradThis[2];
378 return approximateContactPoint;
398 Mdouble dampingCoefficient = 1;
403 gradient.
X = gradientShape(0);
404 gradient.
Y = gradientShape(1);
405 gradient.
Z = gradientShape(2);
413 dampingCoefficient = 1;
417 dampingCoefficient /= 2;
418 if (dampingCoefficient < 1e-10)
420 logger(
ERROR,
"overlap detection does not converge");
445 const Mdouble distance = sqrt(distanceSquared);
451 approximateContactPoint[0] = contactPoint.
X;
452 approximateContactPoint[1] = contactPoint.
Y;
453 approximateContactPoint[2] = contactPoint.
Z;
454 approximateContactPoint[3] = 1;
461 approximateContactPoint[0] = contactPoint.
X;
462 approximateContactPoint[1] = contactPoint.
Y;
463 approximateContactPoint[2] = contactPoint.
Z;
464 approximateContactPoint[3] = multiplier;
466 return approximateContactPoint;
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;
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);
507 const Mdouble nu = std::pow(absXoa, n2) + std::pow(absYob, n2);
509 const Mdouble help1 = std::pow(nu, n1 / n2 - 1.0);
511 Vec3D gradientBodyFixed = {
517 return {gradientBodyFixed.
X, gradientBodyFixed.
Y, gradientBodyFixed.
Z};
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);
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);
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) *
551 hessian(0, 1) = hessian(1, 0);
572 labFixedCoordinates.
X = position[0];
573 labFixedCoordinates.
Y = position[1];
574 labFixedCoordinates.
Z = position[2];
575 const Mdouble lagrangeMultiplier = position[3];
586 for (
unsigned int i = 0;
i < 3; ++
i)
588 toBecomeZero[
i] = gradThis[
i] + lagrangeMultiplier * lagrangeMultiplier * gradOther[
i];
629 labFixedCoordinates.
X = contactPoint[0];
630 labFixedCoordinates.
Y = contactPoint[1];
631 labFixedCoordinates.
Z = contactPoint[2];
632 const Mdouble lagrangeMultiplier = contactPoint[3];
640 SmallMatrix<3, 3> upperLeftCorner = hessianThis + lagrangeMultiplier * lagrangeMultiplier * hessianOther;
643 for (
unsigned int i = 0;
i < 3; ++
i)
645 for (
unsigned int j = 0; j < 3; ++j)
647 jacobian(
i, j) = upperLeftCorner(
i, j);
649 jacobian(
i, 3) = 2 * lagrangeMultiplier * gradOther[
i];
652 for (
unsigned int j = 0; j < 3; ++j)
654 jacobian(3, j) = gradThis[j] - gradOther[j];
672 Mdouble helper2 = gradientLength * gradientLength * (hessian(0, 0) + hessian(1, 1) + hessian(2, 2));
673 Mdouble denominator = 2 * gradientLength * gradientLength * gradientLength;
676 return (helper2 - helper) / denominator;
703 contactPoint.
X = approximateContactPoint[0];
704 contactPoint.
Y = approximateContactPoint[1];
705 contactPoint.
Z = approximateContactPoint[2];
731 const Vec3D initialPoint = (interactionRadiusOther * positionThis + interactionRadiusThis * positionOther) /
732 (interactionRadiusThis + interactionRadiusOther);
736 contactPointPlanB[0] = initialPoint.
X;
737 contactPointPlanB[1] = initialPoint.
Y;
738 contactPointPlanB[2] = initialPoint.
Z;
739 contactPointPlanB[3] = 1.0;
744 const Vec3D dAxesThis = (
getAxes() - interactionRadiusThis *
Vec3D(1, 1, 1)) / numberOfSteps;
748 const Vec3D dAxesOther = (pOther->
getAxes() - interactionRadiusOther *
Vec3D(1, 1, 1)) / numberOfSteps;
764 const unsigned maxIterations = 20;
765 for (
unsigned int counter = 1; counter <= numberOfSteps; ++counter)
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;
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;
788 if (numberOfSteps > maxIterations)
791 pOther->
write(std::cout);
792 logger(
ERROR,
"Plan B fails even with more than 20 intermediate steps");
797 logger.assert_debug(p1.
getAxes().
X ==
getAxes().
X,
"In getContactPointPlanB, final particle for contact-detection not "
798 "the same as original particle");
800 logger(
VERBOSE,
"Plan B contact point: %", contactPointPlanB);
802 return contactPointPlanB;
823 const Mdouble tolerance = 1e-5;
824 const unsigned maxIterations = 100;
835 jacobian.
solve(func);
846 logger(
VERBOSE,
"current contact point: %, new contact point: %\n", contactPoint, newPoint);
849 if (residueNew.length()
850 < residueOld.length())
852 contactPoint = newPoint;
859 if (dampingFactor < 1e-10)
867 return (iteration != maxIterations);
871 SmallVector<4>& contactPointPlanB,
const unsigned int& counter)
const
873 logger(
VERBOSE,
"Position of particle 1 (p1): % \nPosition of particle 2 (p2): % \n",
875 logger(
VERBOSE,
"Orientation of particle 1 (p1): % \nOrientation of particle 2 (p2): %\n",
887 logger(
VERBOSE,
"Analytical solution Contact Point: % ", contactPointPlanB);
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);
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);
974 logger(
VERBOSE,
"Analytical solution for two equivalent spheres in contact: % \n", contactPointPlanB);
980 return getRadius() + 0.5 * mixedSpecies->getInteractionDistance();
1007 logger(
ERROR,
"[SuperQuadricParticle::computeMass] SuperQuadricParticle cannot be two-dimensional (yet)");
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.
unsigned int getIndex() const
Returns the index of the object in the handler.
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
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
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.
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.
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)
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
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
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_.
Mdouble getSumOfInteractionRadii(const BaseParticle *particle) const
returns the sum of the radii plus the interactionDistance
Implementation of a 3D vector (by Vitaliy).
void setContactPoint(Vec3D contactPoint)
Set the location of the contact point between the two interacting objects.
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)
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...
static Mdouble dot(const Vec3D &a, const Vec3D &b)
Calculates the dot product of two Vec3D: .
void rotate(Vec3D &position) const
Applies the rotation to a position.
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: .
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.
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.
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.
bool isFixed() const override
Is fixed Particle function. It returns whether a Particle is fixed or not, by checking its inverse Ma...
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...
void setPosition(const Vec3D &position)
Sets the position of this BaseInteractable.
void getRotationMatrix(SmallMatrix< 3, 3 > &A) const
Retrieves the rotation matrix.
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.
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.
T square(const T val)
squares a number
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.
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...