Screw.cc

Go to the documentation of this file.

//Copyright (c) 2013-2014, 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 "Screw.h"
#include "array"
#include "InteractionHandler.h"
#include "Math/ExtendedMath.h"
#include "Particles/BaseParticle.h"
using mathsFunc::square;
Screw::Screw()
{
    start_.setZero();
    l_ = 1.0;
    maxR_ = 1.0;
    n_ = 1.0;
    omega_ = 1.0;
    offset_ = 0.0;
    thickness_ = 0.0;
    logger(DEBUG, "Screw() constructor finished.");              
}

Screw::Screw(const Screw& other)
    : BaseWall(other)
{
    start_ = other.start_;
    l_ = other.l_;
    maxR_ = other.maxR_;
    n_ = other.n_;
    omega_ = other.omega_;
    thickness_ = other.thickness_;
    offset_ = other.offset_;
    logger(DEBUG, "Screw(const Screw&) copy constructor finished.");
}

Screw::Screw(Vec3D start, Mdouble l, Mdouble r, Mdouble n, Mdouble omega, Mdouble thickness)
{
    start_ = start;
    l_ = l;
    maxR_ = r;
    n_ = n;
    omega_ = omega;
    thickness_ = thickness;
    offset_ = 0.0;
    logger(DEBUG, "Screw(Vec3D, Mdouble, Mdouble, Mdouble, Mdouble, Mdouble) constructor finished.");           
}

Screw::~Screw()
{
    logger(DEBUG, "~Screw() finished, destroyed the Screw.");
}

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

bool Screw::getDistanceAndNormal(const BaseParticle& p, Mdouble& distance, Vec3D& normal_return) const
{
    Mdouble RSquared = square(p.getPosition().X - start_.X) + square(p.getPosition().Y - start_.Y);
    Mdouble Z = p.getPosition().Z - start_.Z;
    //first do a simple check if particle is within the cylindrical hull of the screw
    if (RSquared > square(maxR_ + p.getWallInteractionRadius() + thickness_) 
        || Z > l_ + p.getWallInteractionRadius() + thickness_ 
        || Z < - p.getWallInteractionRadius() - thickness_)
    {
        //std::cout << "failed " << p.getPosition() << std::endl;
        return false;
    }

    //else:
    Mdouble R = sqrt(RSquared);
    Mdouble A = atan2(p.getPosition().Y - start_.Y, p.getPosition().X - start_.X);
    
    //after subtracting the start position and transforming the particle position from (XYZ) into (RAZ) 
    //coordinates, we compute the distance to the wall at, located at (r,a,z)=(r,2*pi*(offset+N*q+k/2),q*L), 0<q<1.

    //To find the contact point we have to minimize (with respect to r and q)
    //distance^2=(x-x0-r*cos(2*pi*(offset+N*q)))^2+(y-y0-r*sin(2*pi*(offset+N*q)))^2+(z-z0-q*L)^2
    //Using polar coordinates (i.e. x-x0=R*cos(A), y-y0=R*sin(A) and Z=z-z0)
    //distance^2=R^2+r^2-2*R*r*cos(A-2*pi*(offset+N*q))+(Z-q*L)^2
    
    //Assumption: d(distance)/dr=0 at minDistance (should there be also a q-derivative?)
    //Differentiate with respect to r and solve for zero:
    //0=2*r-2*R*cos(A-2*pi*(offset+N*q)
    //r=R*cos(A-2*pi*(offset+N*q))
    
    //Substitue back
    //distance^2=R^2+R^2*cos^2(A-2*pi*(offset+N*q))-2*R^2*cos^2(A-2*pi*(offset+N*q))+(Z-q*L)^2
    //distance^2=R^2*sin^2(A-2*pi*(offset+N*q))+(Z-q*L)^2
    
    //So we have to minimize:
    //distance^2=R^2*sin^2(A-2*pi*(offset+N*q))^2 + (Z-q*L)^2 = f(q)
    //f'(q)=(-2*pi*N)*R^2*sin(2*A-4*pi*(offset+N*q)) + 2*L*(Z-q*L)    (D[Sin[x]^2,x]=Sin[2x])
    //f''(q)=(4*pi^2*N^2)*R^2*cos(2*A-4*pi*(offset+N*q)) - 2*L*L
    //For this we use the Euler algoritm
    
    Mdouble q; //Current guess
    Mdouble dd; //Derivative at current guess
    Mdouble ddd; //Second derivative at current guess
    Mdouble q0 = Z / l_; //assume closest point q0 is at same z-location as the particle
            
    //Set initial q to the closest position on the screw with the same angle as A
    //The initial guess will be in the minimum of the sin closest to q0
    //Minima of the sin are at
    //A-2*pi*(offset+N*q)=k*pi (k=integer)
    //q=A/(2*pi*N)-k/(2*N)-offset/N (k=integer)
    
    Mdouble k = round(A / constants::pi - 2.0 * (offset_ + n_ * q0)); // k: |A-a(q0)-k*pi|=min
    q = A / (2.0 * constants::pi * n_) - k / (2.0 * n_) - offset_ / n_; // q: a(q)=A
    
    //Now apply Newton's method
    do
    {
        Mdouble arg = 2.0 * A - 4.0 * constants::pi * (n_ * q + offset_);
        dd = -2.0 * constants::pi * n_ * RSquared * sin(arg) - 2.0 * l_ * (Z - q * l_);
        ddd = 8.0 * constants::sqr_pi * n_ * n_ * RSquared * cos(arg) + 2.0 * l_ * l_;
        q -= dd / ddd;
    } while (fabs(dd / ddd) > 1e-14);
    
    //Calculate r
    Mdouble r = R * cos(2.0 * constants::pi * (offset_ + n_ * q) - A);
    
    //Check if the location is actually on the screw:
    //First posibility is that the radius is too large:
    if (fabs(r) > maxR_) //Left boundary of the coil
    {
        r = mathsFunc::sign(r) * maxR_;
        unsigned int steps = 0;
        //This case reduces to the coil problem
        do
        {
            dd = -4.0 * R * r * constants::pi * n_ * sin(A - 2.0 * constants::pi * (n_ * q + offset_)) - 2.0 * l_ * (Z - q * l_);
            ddd = 8.0 * R * r * constants::sqr_pi * n_ * n_ * cos(A - 2.0 * constants::pi * (n_ * q + offset_)) + 2.0 * l_ * l_;
            q -= dd / ddd;
            steps++;
        } while (fabs(dd / ddd) > 1e-14);
    }
    //Second possibility is that it occured before the start of after the end
    if (q < 0)
    {
        q = 0;
        r = R * cos(A - 2.0 * constants::pi * (offset_ + q * n_));
        if (fabs(r) > maxR_)
        {
            r = mathsFunc::sign(r) * maxR_;
        }
    }
    else if (q > 1)
    {
        q = 1;
        r = R * cos(A - 2.0 * constants::pi * (offset_ + q * n_));
        if (fabs(r) > maxR_)
        {
            r = mathsFunc::sign(r) * maxR_;
        }
    }
    
    Mdouble distanceSquared = R * R * pow(sin(A - 2 * constants::pi * (offset_ + n_ * q)), 2) + pow(Z - q * l_, 2);
    //If distance is too large there is no contact
    if (distanceSquared >= square(p.getWallInteractionRadius() + thickness_))
    {
        return false;
    }
    
    Vec3D ContactPoint;
    distance = sqrt(distanceSquared) - thickness_;
    ContactPoint.X = start_.X + r * cos(2.0 * constants::pi * (offset_ + n_ * q));
    ContactPoint.Y = start_.Y + r * sin(2.0 * constants::pi * (offset_ + n_ * q));
    ContactPoint.Z = start_.Z + q * l_;
    normal_return = (ContactPoint - p.getPosition());
    normal_return.normalize();
    return true;
}

void Screw::move_time(Mdouble dt)
{
    offset_ += omega_ * dt;
}

void Screw::read(std::istream& is)
{
    BaseWall::read(is);
    std::string dummy;
    is >> dummy >> start_
            >> dummy >> l_
            >> dummy >> maxR_
            >> dummy >> n_
            >> dummy >> omega_
            >> dummy >> thickness_
            >> dummy >> offset_;
}

void Screw::oldRead(std::istream& is)
{
    std::string dummy;
    is >> dummy >> start_
            >> dummy >> l_
            >> dummy >> maxR_
            >> dummy >> n_
            >> dummy >> omega_
            >> dummy >> offset_;
}

void Screw::write(std::ostream& os) const
{
    BaseWall::write(os);
    os << " start " << start_
            << " Length " << l_
            << " Radius " << maxR_
            << " Revolutions " << n_
            << " Omega " << omega_
            << " Thickness " << thickness_
            << " Offset " << offset_;
}

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

void Screw::writeVTK (std::string filename) const
{
    std::vector<Vec3D> vertices;
    std::vector<std::array<unsigned,3>> faces;
    getTriangulation (vertices, faces);

    std::stringstream file;
    file << "# vtk DataFile Version 2.0\n"
     << getName() << "\n"
     "ASCII\n"
     "DATASET UNSTRUCTURED_GRID\n"
     "POINTS " << vertices.size() << " double\n";
    for (const auto& vertex : vertices)
        file << vertex << '\n';
    file << "\nCELLS " << faces.size() << ' ' << 4*faces.size() << "\n";
    for (const auto& face : faces)
        file << "3 " << face[0] << ' ' << face[1] << ' ' << face[2] << '\n';
    file << "\nCELL_TYPES " << faces.size() << "\n";
    for (const auto& face : faces)
        file << "5\n";
    helpers::writeToFile(filename,file.str());
}

void Screw::getTriangulation (std::vector<Vec3D>& vertices, std::vector<std::array<unsigned,3>>& faces, unsigned nr, unsigned nz) const
{
    //add vertices
    vertices.reserve(nr*nz);
    Vec3D vertex;
    for (Mdouble iz=0; iz<nz; iz++) {
        for (Mdouble ir=0; ir<nr; ir++) {
            Mdouble q = iz/(nz-1); //angular direction
            Mdouble r = ir/(nr-1)*maxR_;
            vertex.X = start_.X - r * cos(2.0 * constants::pi * (offset_ + n_ * q));
            vertex.Y = start_.Y - r * sin(2.0 * constants::pi * (offset_ + n_ * q));
            vertex.Z = start_.Z + q * l_;
            vertices.push_back(vertex);
        }
    }
    //reserve space for faces
    faces.reserve(2*(nr-1)*(nz-1));
    std::array<unsigned,3> face;
    for (unsigned iz=0; iz<nz-1; iz++) {
        for (unsigned ir=0; ir<nr-1; ir++) {
            //first face 0 1 nr
            faces.push_back({iz*nr+ir,iz*nr+(ir+1),(iz+1)*nr+ir});
            //second face 1 nr+1 nr
            faces.push_back({iz*nr+(ir+1),(iz+1)*nr+(ir+1),(iz+1)*nr+ir});
        }
    }
}