SuperQuadricParticle.cc

Go to the documentation of this file.

//Copyright (c) 2013-2020, The MercuryDPM Developers Team. All rights reserved.
//For the list of developers, see <http://www.MercuryDPM.org/Team>.
//
//Redistribution and use in source and binary forms, with or without
//modification, are permitted provided that the following conditions are met:
//  * Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
//  * Redistributions in binary form must reproduce the above copyright
//    notice, this list of conditions and the following disclaimer in the
//    documentation and/or other materials provided with the distribution.
//  * Neither the name MercuryDPM nor the
//    names of its contributors may be used to endorse or promote products
//    derived from this software without specific prior written permission.
//
//THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
//ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
//WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
//DISCLAIMED. IN NO EVENT SHALL THE MERCURYDPM DEVELOPERS TEAM BE LIABLE FOR ANY
//DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
//(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
//LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
//ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
//(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
//SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

#include "DPMBase.h"
#include "BaseParticle.h"
#include <cmath>
#include "SuperQuadricParticle.h"
#include "InteractionHandler.h"
#include "ParticleHandler.h"
#include "SpeciesHandler.h"

SuperQuadricParticle::SuperQuadricParticle()
        : BaseParticle()
{
    axes_ = Vec3D(1.0, 1.0, 1.0);
    eps1_ = 1.0;
    eps2_ = 1.0;
    logger(DEBUG, "SuperQuadricParticle::SuperQuadricParticle() finished");
}

SuperQuadricParticle::SuperQuadricParticle(const SuperQuadricParticle& p)
        : BaseParticle(p)
{
    axes_ = p.axes_;
    eps1_ = p.eps1_;
    eps2_ = p.eps2_;
}


SuperQuadricParticle::SuperQuadricParticle(const BaseParticle& p) : BaseParticle(p)
{
    Mdouble radius = p.getRadius();
    axes_ = Vec3D(radius, radius, radius);
    eps1_ = 1.0;
    eps2_ = 1.0;
}

SuperQuadricParticle::~SuperQuadricParticle()
{
    if (getHandler() != nullptr)
    {
        getHandler()->checkExtremaOnDelete(this);
    }
    logger(DEBUG, "SuperQuadricParticle::SuperQuadricParticleParticle() of particle % finished.", getId());
    
}

SuperQuadricParticle* SuperQuadricParticle::copy() const
{
    return new SuperQuadricParticle(*this);
}

void SuperQuadricParticle::write(std::ostream& os) const
{
    BaseParticle::write(os);
    os << " axes " << axes_
       << " exp1 " << eps1_
       << " exp2 " << eps2_;
}

std::string SuperQuadricParticle::getName() const
{
    return "SuperQuadricParticle";
}


void SuperQuadricParticle::read(std::istream& is)
{
    BaseParticle::read(is);
    std::string dummy;
    is >> dummy >> axes_ >> dummy >> eps1_ >> dummy >> eps2_;
    logger.assert_always(eps1_ < 1 + 1e-10, "epsilon1 should be at most 1");
    logger.assert_always(eps2_ < 1 + 1e-10, "epsilon2 should be at most 1");
}

// Set and Get Functions

void SuperQuadricParticle::setAxesAndExponents(const Mdouble& a1, const Mdouble& a2, const Mdouble& a3, const Mdouble& eps1,
                                       const Mdouble& eps2)
{
    eps1_ = eps1;
    eps2_ = eps2;
    logger.assert_always(eps1_ < 1 + 1e-10, "epsilon1 should be at most 1");
    logger.assert_always(eps2_ < 1 + 1e-10, "epsilon2 should be at most 1");

    setAxes(a1,a2,a3);
}

void SuperQuadricParticle::setAxesAndExponents(const Vec3D& axes, const Mdouble& eps1, const Mdouble& eps2)
{
    eps1_ = eps1;
    eps2_ = eps2;
    logger.assert_always(eps1_ < 1 + 1e-10, "epsilon1 should be at most 1");
    logger.assert_always(eps2_ < 1 + 1e-10, "epsilon2 should be at most 1");

    setAxes(axes);
}

void SuperQuadricParticle::setAxes(const Vec3D& axes)
{
    axes_ = axes;
    setBoundingRadius();
    if (getSpecies() != nullptr)
    {
        getSpecies()->computeMass(this);
    }
}

void SuperQuadricParticle::setAxes(const Mdouble& a1, const Mdouble& a2, const Mdouble& a3)
{
    setAxes({a1,a2,a3});
}

void SuperQuadricParticle::setExponents(const Mdouble& eps1, const Mdouble& eps2)
{
    eps1_ = eps1;
    eps2_ = eps2;
    setBoundingRadius();
    logger.assert_always(eps1_ < 1 + 1e-10, "epsilon1 should be at most 1");
    logger.assert_always(eps2_ < 1 + 1e-10, "epsilon2 should be at most 1");
    if (getSpecies() != nullptr)
    {
        getSpecies()->computeMass(this);
    }
}

Vec3D SuperQuadricParticle::getAxes() const
{
    return axes_;
}

Mdouble SuperQuadricParticle::getExponentEps1() const
{
    return eps1_;
}

Mdouble SuperQuadricParticle::getExponentEps2() const
{
    return eps2_;
}

Mdouble SuperQuadricParticle::getVolume() const
{
    logger.assert_debug(getHandler() != nullptr, "[SuperQuadricParticle::getVolume] no particle handler specified");

    return (2.0 * axes_.X * axes_.Y * axes_.Z * eps1_ * eps2_) * mathsFunc::beta(0.5 * eps1_ + 1.0, eps1_) *
           mathsFunc::beta(0.5 * eps2_, 0.5 * eps2_);
}

void SuperQuadricParticle::setInertia()
{
    MatrixSymmetric3D inertia;
    
    inertia.XX = getSpecies()->getDensity() * (0.5 * axes_.X * axes_.Y * axes_.Z * eps1_ * eps2_) *
                 (axes_.Y * axes_.Y * mathsFunc::beta(1.5 * eps2_, 0.5 * eps2_) *
                  mathsFunc::beta(0.5 * eps1_, 2.0 * eps1_ + 1.0)
                  + 4.0 * axes_.Z * axes_.Z * mathsFunc::beta(0.5 * eps2_, 0.5 * eps2_ + 1) *
                    mathsFunc::beta(1.5 * eps1_, eps1_ + 1.0));
    
    inertia.YY = getSpecies()->getDensity() * (0.5 * axes_.X * axes_.Y * axes_.Z * eps1_ * eps2_) *
                 (axes_.X * axes_.X * mathsFunc::beta(1.5 * eps2_, 0.5 * eps2_) *
                  mathsFunc::beta(0.5 * eps1_, 2.0 * eps1_ + 1.0)
                  + 4.0 * axes_.Z * axes_.Z * mathsFunc::beta(0.5 * eps2_, 0.5 * eps2_ + 1) *
                    mathsFunc::beta(1.5 * eps1_, eps1_ + 1.0));
    
    inertia.ZZ = getSpecies()->getDensity() * (0.5 * axes_.X * axes_.Y * axes_.Z * eps1_ * eps2_) *
                 (axes_.X * axes_.X + axes_.Y * axes_.Y) * mathsFunc::beta(1.5 * eps2_, 0.5 * eps2_) *
                 mathsFunc::beta(0.5 * eps1_, 2.0 * eps1_ + 1.0);
    
    BaseParticle::setInertia(inertia);
}

void SuperQuadricParticle::setRadius(const Mdouble radius)
{
    logger(ERROR,"This function should not be used");
}

void SuperQuadricParticle::setBoundingRadius()
{

    if(mathsFunc::isEqual(eps2_,1,std::numeric_limits<Mdouble>::epsilon()) && mathsFunc::isEqual(eps1_,1,std::numeric_limits<Mdouble>::epsilon()))
    {
        BaseParticle::setRadius(std::max(std::max(axes_.Y,axes_.X),axes_.Z));
        return;
    }else
    {

        const Mdouble axesX = std::max(axes_.X, axes_.Y);
        const Mdouble axesY = std::min(axes_.X, axes_.Y);

        Mdouble alpha;

        const Mdouble eps1 = std::min(.96, eps1_);
        const Mdouble eps2 = std::min(.96, eps2_);

        alpha = std::pow(axesY / axesX, 2.0 / (2.0 / eps2 - 2.0));

        const Mdouble help1 = std::pow(alpha, 2.0 / eps2);
        const Mdouble gamma = std::pow(1.0 + help1, eps2 / eps1 - 1.0);
        const Mdouble beta = std::pow(gamma * axes_.Z * axes_.Z / (axesX * axesX), 1.0 / (2.0 / eps1 - 2.0));
        const Mdouble xTilde = std::pow(std::pow(1 + help1, eps2 / eps1) + std::pow(beta, 2.0 / eps1),
                                        -eps1 / 2.0);
        BaseParticle::setRadius(std::sqrt(mathsFunc::square(axesX * xTilde)
                                          + mathsFunc::square(alpha * axesY * xTilde)
                                          + mathsFunc::square(beta * axes_.Z * xTilde)));

    }
}

BaseInteraction* SuperQuadricParticle::getInteractionWith(BaseParticle* const p, const unsigned timeStamp,
                                                               InteractionHandler* const interactionHandler)
{
    //get the normal (from P away from the contact)
    const LabFixedCoordinates branchVector = p->getPosition() - getPosition();
    //Get the square of the distance between particle i and particle j
    const Mdouble distanceSquared = Vec3D::getLengthSquared(branchVector);
    auto mixedSpecies = getSpecies()->getHandler()->getMixedObject(getSpecies(),p->getSpecies());

    const Mdouble sumOfInteractionRadii = getMaxInteractionRadius()+p->getMaxInteractionRadius();
    if (distanceSquared < (sumOfInteractionRadii * sumOfInteractionRadii))
    {
        if (p->isSphericalParticle())
        {
            SuperQuadricParticle* pQuad = new SuperQuadricParticle;
            pQuad->setAxes(p->getRadius(), p->getRadius(), p->getRadius());
            BaseInteraction* contacts = getInteractionWithSuperQuad(pQuad, timeStamp, interactionHandler);
            delete pQuad;
            return contacts;
        } else {
            SuperQuadricParticle* pQuad = static_cast<SuperQuadricParticle*>(p);
            return getInteractionWithSuperQuad(pQuad, timeStamp, interactionHandler);
        }
    }
    return nullptr;
}

BaseInteraction* SuperQuadricParticle::getInteractionWithSuperQuad(SuperQuadricParticle* const p, const unsigned timeStamp,
                                          InteractionHandler* const interactionHandler)
{
    BaseInteraction* const C = interactionHandler->getInteraction(p, this, timeStamp);
    const SmallVector<4> approximateContactPoint = getContactPoint(p, C);
    
    //set the new contact point:
    LabFixedCoordinates contactPoint;
    contactPoint.X = approximateContactPoint[0];
    contactPoint.Y = approximateContactPoint[1];
    contactPoint.Z = approximateContactPoint[2];
    
    //if the minimum is positive, there is no contact: return empty vector
    if (computeShape(contactPoint) > -1e-10)
    {
        interactionHandler->removeObject(C->getIndex());
        return nullptr;
    }
    
    C->setContactPoint(contactPoint);
    C->setLagrangeMultiplier(approximateContactPoint[3]);
    
    SmallVector<3> gradThis = computeShapeGradientLabFixed(contactPoint).getNormalised();
    LabFixedCoordinates normal;
    normal.X = gradThis[0];
    normal.Y = gradThis[1];
    normal.Z = gradThis[2];
    C->setNormal(normal);
    
    const Mdouble alphaI = overlapFromContactPoint(contactPoint, normal);
    const Mdouble alphaJ = p->overlapFromContactPoint(contactPoint, -normal);
    C->setOverlap(alphaI + alphaJ);
    C->setDistance((getPosition() - p->getPosition()).getLength() - C->getOverlap());
    
    return C;
}

SmallVector<4> SuperQuadricParticle::getContactPoint(const SuperQuadricParticle* const p, BaseInteraction* C) const
{
    //Assuming contact between two spheres
    SmallVector<4> approximateContactPoint = getInitialGuessForContact(p, C);
    if (computeContactPoint(approximateContactPoint, this, p))
    {
        return approximateContactPoint;
    }
    else
    {
        return getContactPointPlanB(p, 4);
    }
}

Mdouble SuperQuadricParticle::overlapFromContactPoint(const LabFixedCoordinates& contactPoint,
        const LabFixedCoordinates& normal) const
{
    Mdouble alphaI = 0;
    LabFixedCoordinates xEdge = contactPoint + alphaI * normal;
    Mdouble dampingCoefficient = 1;
    while (std::abs(computeShape(xEdge)) > 1e-10)
    {
        SmallVector<3> gradientShape = computeShapeGradientLabFixed(xEdge);
        LabFixedCoordinates gradient;
        gradient.X = gradientShape(0);
        gradient.Y = gradientShape(1);
        gradient.Z = gradientShape(2);
        Mdouble newValue = alphaI - dampingCoefficient * computeShape(xEdge) / Vec3D::dot(gradient, normal);
        const LabFixedCoordinates xEdgeNew = contactPoint + newValue * normal;
        
        if (std::abs(computeShape(xEdgeNew)) < std::abs(computeShape(xEdge)))
        {
            alphaI = newValue;
            xEdge = xEdgeNew;
            dampingCoefficient = 1;
        }
        else
        {
            dampingCoefficient /= 2;
            if (dampingCoefficient < 1e-10)
            {
                logger(ERROR, "overlap detection does not converge");
            }
        }
    }
    return alphaI;
}

SmallVector<4> SuperQuadricParticle::getInitialGuessForContact(const SuperQuadricParticle* pQuad, BaseInteraction* const C) const
{
    SmallVector<4> approximateContactPoint;
    if (C == nullptr || C->getOverlap() < 1e-15)
    {
        //this is a new interaction
        const LabFixedCoordinates branchVector = pQuad->getPosition() - getPosition();
        //Get the square of the distance between particle i and particle j
        const Mdouble distanceSquared = Vec3D::getLengthSquared(branchVector);
        const Mdouble distance = sqrt(distanceSquared);
        const LabFixedCoordinates normal = (branchVector / distance);
        const Mdouble overlap = getSumOfInteractionRadii(pQuad) - distance;
        const LabFixedCoordinates contactPoint = (pQuad->getPosition() -
                                                  (pQuad->getInteractionRadius(this) - 0.5 * overlap) * normal);
        
        approximateContactPoint[0] = contactPoint.X;
        approximateContactPoint[1] = contactPoint.Y;
        approximateContactPoint[2] = contactPoint.Z;
        approximateContactPoint[3] = 1;
    }
    else
    {
        //this is an existing interaction
        const LabFixedCoordinates contactPoint = C->getContactPoint();
        const Mdouble multiplier = C->getLagrangeMultiplier();
        approximateContactPoint[0] = contactPoint.X;
        approximateContactPoint[1] = contactPoint.Y;
        approximateContactPoint[2] = contactPoint.Z;
        approximateContactPoint[3] = multiplier;
    }
    return approximateContactPoint;
}

Mdouble SuperQuadricParticle::computeShape(const LabFixedCoordinates& labFixedCoordinates) const
{
    BodyFixedCoordinates bodyFixedCoordinates = labFixedCoordinates - this->getPosition();
    getOrientation().rotateBack(bodyFixedCoordinates);
    
    const Mdouble n1 = 2. / eps1_;
    const Mdouble n2 = 2. / eps2_;
    
    return std::pow(std::pow(std::abs(bodyFixedCoordinates.X / axes_.X), n2)
                    + std::pow(std::abs(bodyFixedCoordinates.Y / axes_.Y), n2), n1 / n2)
           + std::pow(std::abs(bodyFixedCoordinates.Z / axes_.Z), n1) - 1.0;
}

SmallVector<3> SuperQuadricParticle::computeShapeGradientLabFixed(const LabFixedCoordinates& labFixedCoordinates) const
{
    
    BodyFixedCoordinates bodyFixedCoordinates = labFixedCoordinates - this->getPosition();
    getOrientation().rotateBack(bodyFixedCoordinates);
    
    const Mdouble n1 = 2. / eps1_;
    const Mdouble n2 = 2. / eps2_;
    
    const Mdouble absXoa = std::abs(bodyFixedCoordinates.X / axes_.X);
    const Mdouble absYob = std::abs(bodyFixedCoordinates.Y / axes_.Y);
    const Mdouble absZoc = std::abs(bodyFixedCoordinates.Z / axes_.Z);
    
    const Mdouble nu = std::pow(absXoa, n2) + std::pow(absYob, n2);
    
    const Mdouble help1 = std::pow(nu, n1 / n2 - 1.0);
    
    Vec3D gradientBodyFixed = {
            (n1 / axes_.X) * std::pow(absXoa, n2 - 1.0) * help1 * mathsFunc::sign(bodyFixedCoordinates.X),
            (n1 / axes_.Y) * std::pow(absYob, n2 - 1.0) * help1 * mathsFunc::sign(bodyFixedCoordinates.Y),
            (n1 / axes_.Z) * std::pow(absZoc, n1 - 1.0) * mathsFunc::sign(bodyFixedCoordinates.Z)};
    //we get the gradient in terms of the lab-fixed coordinates by \nabla_X F(X) = A \nabla_x f(x).
    getOrientation().rotate(gradientBodyFixed);
    return {gradientBodyFixed.X, gradientBodyFixed.Y, gradientBodyFixed.Z};
}

SmallMatrix<3, 3> SuperQuadricParticle::computeHessianLabFixed(const LabFixedCoordinates& labFixedCoordinates) const
{
    SmallMatrix<3, 3> hessian;
    BodyFixedCoordinates bodyFixedCoordinates = labFixedCoordinates - this->getPosition();
    getOrientation().rotateBack(bodyFixedCoordinates);
    
    const Mdouble n1 = 2.0 / eps1_;
    const Mdouble n2 = 2.0 / eps2_;
    
    const Mdouble absXoa = std::abs(bodyFixedCoordinates.X / axes_.X);
    const Mdouble absYob = std::abs(bodyFixedCoordinates.Y / axes_.Y);
    const Mdouble absZoc = std::abs(bodyFixedCoordinates.Z / axes_.Z);
    
    const Mdouble nu = std::pow(absXoa, n2) + std::pow(absYob, n2);
    const Mdouble help1 = std::pow(nu, n1 / n2 - 1.0);
    const Mdouble help2 = std::pow(nu, n1 / n2 - 2.0);
    
    hessian(0, 0) = n1 * (n2 - 1) / axes_.X / axes_.X * std::pow(absXoa, n2 - 2.0) * help1
                    + ((n1 - n2) * n1 / axes_.X / axes_.X) * std::pow(absXoa, 2. * n2 - 2.0) * help2;
    hessian(1, 1) = n1 * (n2 - 1) / axes_.Y / axes_.Y * std::pow(absYob, n2 - 2.0) * help1
                    + ((n1 - n2) * n1 / axes_.Y / axes_.Y) * std::pow(absYob, 2. * n2 - 2.0) * help2;
    hessian(2, 2) = n1 * (n1 - 1) / axes_.Z / axes_.Z * std::pow(absZoc, n1 - 2.0);
    hessian(1, 0) = n1 * (n1 - n2) / axes_.X / axes_.Y * std::pow(absXoa, n2 - 1.0) * std::pow(absYob, n2 - 1.0) *
                    help2 * mathsFunc::sign(bodyFixedCoordinates.X * bodyFixedCoordinates.Y);
    hessian(0, 1) = hessian(1, 0);
    SmallMatrix<3, 3> A;
    getOrientation().getRotationMatrix(A);
    return A * hessian * (A.transpose());
}

SmallVector<4> SuperQuadricParticle::computeResidualContactDetection(const SmallVector<4>& position,
                                                             const SuperQuadricParticle* const p1,
                                                             const SuperQuadricParticle* const p2) const
{
    LabFixedCoordinates labFixedCoordinates;
    labFixedCoordinates.X = position[0];
    labFixedCoordinates.Y = position[1];
    labFixedCoordinates.Z = position[2];
    const Mdouble lagrangeMultiplier = position[3];
    
    // First compute the gradient of the superquadric shape function and then rotate it to the
    // global frame of reference
    SmallVector<3> gradThis = p1->computeShapeGradientLabFixed(labFixedCoordinates);
    
    SmallVector<3> gradOther = p2->computeShapeGradientLabFixed(labFixedCoordinates);
    
    // Eqn. (23) in Podhlozhnyuk et al, Comp. Part. Mech. 2017.
    // where contactPoint[3] = mu
    SmallVector<4> toBecomeZero;
    for (unsigned int i = 0; i < 3; ++i)
    {
        toBecomeZero[i] = gradThis[i] + lagrangeMultiplier * lagrangeMultiplier * gradOther[i];
    }
    toBecomeZero[3] = p1->computeShape(labFixedCoordinates) - p2->computeShape(labFixedCoordinates);
    
    return toBecomeZero;
}

SmallMatrix<4, 4>
SuperQuadricParticle::getJacobianOfContactDetectionObjective(const SmallVector<4>& contactPoint,
                                                     const SuperQuadricParticle* const p1,
                                                     const SuperQuadricParticle* const p2) const
{
    LabFixedCoordinates labFixedCoordinates;
    labFixedCoordinates.X = contactPoint[0];
    labFixedCoordinates.Y = contactPoint[1];
    labFixedCoordinates.Z = contactPoint[2];
    const Mdouble lagrangeMultiplier = contactPoint[3];
    
    SmallVector<3> gradThis = p1->computeShapeGradientLabFixed(labFixedCoordinates);
    SmallVector<3> gradOther = p2->computeShapeGradientLabFixed(labFixedCoordinates);
    
    SmallMatrix<3, 3> hessianThis = p1->computeHessianLabFixed(labFixedCoordinates);
    SmallMatrix<3, 3> hessianOther = p2->computeHessianLabFixed(labFixedCoordinates);
    
    SmallMatrix<3, 3> upperLeftCorner = hessianThis + lagrangeMultiplier * lagrangeMultiplier * hessianOther;
    
    SmallMatrix<4, 4> jacobian;
    for (unsigned int i = 0; i < 3; ++i)
    {
        for (unsigned int j = 0; j < 3; ++j)
        {
            jacobian(i, j) = upperLeftCorner(i, j);
        }
        jacobian(i, 3) = 2 * lagrangeMultiplier * gradOther[i];
    }
    
    for (unsigned int j = 0; j < 3; ++j)
    {
        jacobian(3, j) = gradThis[j] - gradOther[j];
    }
    
    return jacobian;
}

Mdouble SuperQuadricParticle::getCurvature(const LabFixedCoordinates& labFixedCoordinates) const
{
    SmallVector<3> gradientVec = computeShapeGradientLabFixed(labFixedCoordinates);
    SmallMatrix<3, 1> gradient = gradientVec;
    SmallMatrix<3, 3> hessian = computeHessianLabFixed(labFixedCoordinates);
    Mdouble helper = ((gradient.transpose() * hessian) * gradient)(0, 0);
    Mdouble gradientLength = gradientVec.length();
    Mdouble helper2 = gradientLength * gradientLength * (hessian(0, 0) + hessian(1, 1) + hessian(2, 2));
    Mdouble denominator = 2 * gradientLength * gradientLength * gradientLength;
    
    return (helper2 - helper) / denominator;
}

bool SuperQuadricParticle::isInContactWith(const BaseParticle* const p) const
{
    SmallVector<4> approximateContactPoint;

    if (p->isSphericalParticle())
    {
        SuperQuadricParticle* pQuad = new SuperQuadricParticle;
        pQuad->setAxes(p->getRadius(), p->getRadius(), p->getRadius());
        approximateContactPoint = getContactPoint(pQuad, nullptr);
        delete pQuad;
    } else {
        const SuperQuadricParticle* pQuad = static_cast<const SuperQuadricParticle*>(p);
        approximateContactPoint = getContactPoint(pQuad, nullptr);
    }

    //set the new contact point:
    LabFixedCoordinates contactPoint;
    contactPoint.X = approximateContactPoint[0];
    contactPoint.Y = approximateContactPoint[1];
    contactPoint.Z = approximateContactPoint[2];
    
    //if the minimum is positive, there is no contact: return false
    return (computeShape(contactPoint) < 0);
    
}

SmallVector<4> SuperQuadricParticle::getContactPointPlanB(const SuperQuadricParticle* const pOther, unsigned numberOfSteps) const
{
    logger(VERBOSE, "Number of iterations: %", numberOfSteps);
    // Step 1: Compute contact point for two volume equivalent spheres
    const Mdouble interactionRadiusThis = getInteractionRadius(pOther);
    const Mdouble interactionRadiusOther = pOther->getInteractionRadius(this);
    const Vec3D positionThis = getPosition();
    const Vec3D positionOther = pOther->getPosition();
    
    //analytical solution for contact point between bounding spheres
    const Vec3D initialPoint = (interactionRadiusOther * positionThis + interactionRadiusThis * positionOther) /
                               (interactionRadiusThis + interactionRadiusOther);
    
    SmallVector<4> contactPointPlanB;
    // Analytical solution
    contactPointPlanB[0] = initialPoint.X;
    contactPointPlanB[1] = initialPoint.Y;
    contactPointPlanB[2] = initialPoint.Z;
    contactPointPlanB[3] = 1.0; //Need to check.
    
    writeDebugMessageStep1(pOther, contactPointPlanB);
    
    //Step 2: Compute the deltas
    const Vec3D dAxesThis = (getAxes() - interactionRadiusThis * Vec3D(1, 1, 1)) / numberOfSteps;
    const Mdouble dn11 = (2.0 / getExponentEps1() - 2.0) / numberOfSteps;
    const Mdouble dn12 = (2.0 / getExponentEps2() - 2.0) / numberOfSteps;
    
    const Vec3D dAxesOther = (pOther->getAxes() - interactionRadiusOther * Vec3D(1, 1, 1)) / numberOfSteps;
    const Mdouble dn21 = (2.0 / pOther->getExponentEps1() - 2.0) / numberOfSteps;
    const Mdouble dn22 = (2.0 / pOther->getExponentEps2() - 2.0) / numberOfSteps;
    
    writeDebugMessageStep2(pOther, dAxesThis, dn11, dn12, dAxesOther, dn21, dn22);
    
    // Create two superquadrics with the above parameters for incrementally computing the contact point
    SuperQuadricParticle p1;
    SuperQuadricParticle p2;
    
    p1.setPosition(getPosition());
    p2.setPosition(pOther->getPosition());
    
    p1.setOrientation(getOrientation());
    p2.setOrientation(pOther->getOrientation());
    bool success = true;
    const unsigned maxIterations = 20;
    for (unsigned int counter = 1; counter <= numberOfSteps; ++counter)
    {
        
        // Step 3: Increment the shape and blockiness parameters
        const Vec3D axesThis = interactionRadiusThis * Vec3D(1, 1, 1) + counter * dAxesThis;
        const Mdouble n11 = 2.0 + counter * dn11;
        const Mdouble n12 = 2.0 + counter * dn12;
        
        Vec3D axesOther = interactionRadiusOther * Vec3D(1, 1, 1) + counter * dAxesOther;
        const Mdouble n21 = 2.0 + counter * dn21;
        const Mdouble n22 = 2.0 + counter * dn22;
        
        p1.setAxesAndExponents(axesThis, 2.0 / n11, 2.0 / n12);
        p2.setAxesAndExponents(axesOther, 2.0 / n21, 2.0 / n22);
        
        writeDebugMessageStep3(axesThis, n11, n12, axesOther, n21, n22);
        
        writeDebugMessageMiddleOfLoop(p1, p2, contactPointPlanB, counter);
        
        //compute the contact point of the new particles
        success = computeContactPoint(contactPointPlanB, &p1, &p2);
        if (!success)
        {
            if (numberOfSteps > maxIterations)
            {
                write(std::cout);
                pOther->write(std::cout);
                logger(ERROR, "Plan B fails even with more than 20 intermediate steps");
            }
            return getContactPointPlanB(pOther, 2 * numberOfSteps);
        }
    }
    logger.assert_debug(p1.getAxes().X == getAxes().X, "In getContactPointPlanB, final particle for contact-detection not "
            "the same as original particle");
    
    logger(VERBOSE, "Plan B contact point: %", contactPointPlanB);
    
    return contactPointPlanB;
}

bool SuperQuadricParticle::computeContactPoint(SmallVector<4>& contactPoint, const SuperQuadricParticle* const p1,
                                       const SuperQuadricParticle* const p2) const
{
    // Damped Newton's method: (dampingFactor 1 is undamped)
    Mdouble dampingFactor = 1;
    int iteration = 1;
    const Mdouble tolerance = 1e-5;
    const unsigned maxIterations = 100;
    
    logger(VERBOSE, "func to be zero value: % \n",
           computeResidualContactDetection(contactPoint, p1, p2));
    
    while (computeResidualContactDetection(contactPoint, p1, p2).length() > tolerance && iteration < maxIterations)
    {
        logger(VERBOSE, "Iteration: %\n", iteration);
        
        SmallMatrix<4, 4> jacobian = getJacobianOfContactDetectionObjective(contactPoint, p1, p2);
        SmallVector<4> func = -computeResidualContactDetection(contactPoint, p1, p2);
        jacobian.solve(func); //note: solve is in place
        
        SmallVector<4> newPoint = contactPoint + dampingFactor * func;
        const auto residueOld = computeResidualContactDetection(contactPoint, p1, p2);
        const auto residueNew = computeResidualContactDetection(newPoint, p1, p2);
        logger(VERBOSE, "Old value: % (%) new value: % (%)",
               computeResidualContactDetection(contactPoint, p1, p2),
               computeResidualContactDetection(contactPoint, p1, p2).length(),
               computeResidualContactDetection(newPoint, p1, p2),
               computeResidualContactDetection(newPoint, p1, p2).length());
        
        logger(VERBOSE, "current contact point: %, new contact point: %\n", contactPoint, newPoint);
        logger(VERBOSE, "damping factor: %", dampingFactor);
        
        if (residueNew.length()
            < residueOld.length())
        {
            contactPoint = newPoint;
            dampingFactor = 1;
        }
        else
        {
            dampingFactor /= 2;
            
            if (dampingFactor < 1e-10)
            {
                return false;
            }
        }
        
        iteration++;
    }
    return (iteration != maxIterations);
}

void SuperQuadricParticle::writeDebugMessageMiddleOfLoop(const SuperQuadricParticle& p1, const SuperQuadricParticle& p2,
                                                 SmallVector<4>& contactPointPlanB, const unsigned int& counter) const
{
    logger(VERBOSE, "Position of particle 1 (p1): % \nPosition of particle 2 (p2): % \n",
           p1.getPosition(), p2.getPosition());
    logger(VERBOSE, "Orientation of particle 1 (p1): % \nOrientation of particle 2 (p2): %\n",
           p1.getOrientation(), p2.getOrientation());
    
    // Step 4: Calculate the contact point for the updated shape parameters
    
    logger(VERBOSE, "----------------------------------------");
    logger(VERBOSE, "STEP 4: Compute contact point");
    logger(VERBOSE, "----------------------------------------");
    logger(VERBOSE, " ");
    logger(VERBOSE, "Counter: %", counter);
    logger(VERBOSE, " ");
    
    logger(VERBOSE, "Analytical solution Contact Point: % ", contactPointPlanB);
    logger(VERBOSE, " ");
}

void
SuperQuadricParticle::writeDebugMessageStep3(const Vec3D& axesThis, const Mdouble& n11, const Mdouble& n12,
                                     const Vec3D& axesOther, const Mdouble& n21, const Mdouble& n22) const
{
    logger(VERBOSE, "-----------------------------------");
    logger(VERBOSE, "STEP 3: First increment");
    logger(VERBOSE, "-----------------------------------");
    logger(VERBOSE, " ");
    
    logger(VERBOSE, "Particle 1 x-scale after increment: %", axesThis.X);
    logger(VERBOSE, "Particle 1 y-scale after increment: %", axesThis.Y);
    logger(VERBOSE, "Particle 1 z-scale after increment: %", axesThis.Z);
    logger(VERBOSE, "Particle 1 n1 after increment: %", n11);
    logger(VERBOSE, "Particle 1 n2 after increment: %", n12);
    logger(VERBOSE, " ");
    
    logger(VERBOSE, "Particle 2 x-scale after increment: %", axesOther.X);
    logger(VERBOSE, "Particle 2 y-scale after increment: %", axesOther.Y);
    logger(VERBOSE, "Particle 2 z-scale after increment: %", axesOther.Z);
    logger(VERBOSE, "Particle 2 n1 after increment: %", n21);
    logger(VERBOSE, "Particle 2 n2 after increment: %", n22);
    logger(VERBOSE, " ");
}

void
SuperQuadricParticle::writeDebugMessageStep2(const SuperQuadricParticle* pQuad, const Vec3D& dAxesThis, const Mdouble& dn11,
                                     const Mdouble& dn12, const Vec3D& dAxesOther, const Mdouble& dn21,
                                     const Mdouble& dn22) const
{
    logger(VERBOSE, "---------------------");
    logger(VERBOSE, "STEP 2");
    logger(VERBOSE, "---------------------");
    logger(VERBOSE, " ");
    
    logger(VERBOSE, "Particle 1 x-scale: %", pQuad->getAxes().X);
    logger(VERBOSE, "Particle 1 y-scale: %", pQuad->getAxes().Y);
    logger(VERBOSE, "Particle 1 z-scale: %", pQuad->getAxes().Z);
    logger(VERBOSE, "Particle 1 n1: %", 2. / pQuad->getExponentEps1());
    logger(VERBOSE, "Particle 1 n2: %", 2. / pQuad->getExponentEps2());
    logger(VERBOSE, " ");
    
    logger(VERBOSE, "Particle 1 x-scale increment: %", dAxesThis.X);
    logger(VERBOSE, "Particle 1 y-scale increment: %", dAxesThis.Y);
    logger(VERBOSE, "Particle 1 z-scale increment: %", dAxesThis.Z);
    logger(VERBOSE, "Particle 1 n1 increment: %", dn11);
    logger(VERBOSE, "Particle 1 n2 increment: %", dn12);
    logger(VERBOSE, " ");
    
    logger(VERBOSE, "Particle 2 x-scale: %", getAxes().X);
    logger(VERBOSE, "Particle 2 y-scale: %", getAxes().Y);
    logger(VERBOSE, "Particle 2 z-scale: %", getAxes().Z);
    logger(VERBOSE, "Particle 2 n1: %", 2. / getExponentEps1());
    logger(VERBOSE, "Particle 2 n2: %", 2. / getExponentEps2());
    logger(VERBOSE, " ");
    
    logger(VERBOSE, "Particle 2 x-scale increment: %", dAxesOther.X);
    logger(VERBOSE, "Particle 2 y-scale increment: %", dAxesOther.Y);
    logger(VERBOSE, "Particle 2 z-scale increment: %", dAxesOther.Z);
    logger(VERBOSE, "Particle 2 n1 increment: %", dn21);
    logger(VERBOSE, "Particle 2 n2 increment: %", dn22);
    logger(VERBOSE, " ");
}

void SuperQuadricParticle::writeDebugMessageStep1(const SuperQuadricParticle* pQuad, const SmallVector<4>& contactPointPlanB) const
{
    logger(VERBOSE, "---------------------");
    logger(VERBOSE, "STEP 1");
    logger(VERBOSE, "---------------------\n");
    
    logger(VERBOSE, "Position of particle 1: % ", getPosition());
    logger(VERBOSE, "Position of particle 2 (pQuad): % ", pQuad->getPosition());
    logger(VERBOSE, " ");
    
    logger(VERBOSE, "Radius of particle 1: % ", getInteractionRadius(pQuad));
    logger(VERBOSE, "Radius of particle 2 (pQuad): % \n", pQuad->getInteractionRadius(this));
    
    logger(VERBOSE, "Orientation of particle 1: % ", getOrientation());
    logger(VERBOSE, "Orientation of particle 2 (pQuad): % ", pQuad->getOrientation());
    logger(VERBOSE, " ");
    
    logger(VERBOSE, "Particle 1 axes: %", getAxes());
    logger(VERBOSE, "Particle 2 axes (pQuad): % \n", pQuad->getAxes());
    
    logger(VERBOSE, "Analytical solution for two equivalent spheres in contact: % \n", contactPointPlanB);
}

Mdouble SuperQuadricParticle::getInteractionRadius(const BaseParticle* particle) const
{
    const auto mixedSpecies = getSpecies()->getHandler()->getMixedObject(getSpecies(),particle->getSpecies());
    return getRadius() + 0.5 * mixedSpecies->getInteractionDistance();
}

void SuperQuadricParticle::computeMass(const ParticleSpecies& s) {
    if (isFixed()) return;
    if (getParticleDimensions()==3) {
        Mdouble volume = getVolume();

        Mdouble help1 = mathsFunc::beta(1.5 * eps2_, 0.5 * eps2_);
        Mdouble help2 = mathsFunc::beta(0.5 * eps1_, 2.0 * eps1_ + 1.0);
        Mdouble help3 = mathsFunc::beta(0.5 * eps2_, 0.5 * eps2_ + 1);
        Mdouble help4 = mathsFunc::beta(1.5 * eps1_, eps1_ + 1.0);

        invMass_ = 1.0 / (volume * s.getDensity());
        invInertia_.XX = 1.0 / (s.getDensity() * (0.5 * axes_.X * axes_.Y * axes_.Z * eps1_ * eps2_) *
                                   (axes_.Y * axes_.Y * help1 * help2
                                    + 4.0 * axes_.Z * axes_.Z * help3 * help4));
        invInertia_.XY = 0.0;
        invInertia_.XZ = 0.0;
        invInertia_.YY = 1.0 / (s.getDensity() * (0.5 * axes_.X * axes_.Y * axes_.Z * eps1_ * eps2_) *
                                   (axes_.X * axes_.X * help1 * help2
                                    + 4.0 * axes_.Z * axes_.Z * help3 * help4));
        invInertia_.YZ = 0.0;
        invInertia_.ZZ = 1.0 / (s.getDensity() * (0.5 * axes_.X * axes_.Y * axes_.Z * eps1_ * eps2_) *
                                   (axes_.X * axes_.X + axes_.Y * axes_.Y) * help1 * help2);
    } else {
        logger(ERROR, "[SuperQuadricParticle::computeMass] SuperQuadricParticle cannot be two-dimensional (yet)");
    }
};