p2p_cylinder_collisions.hpp

/******************************************************************************
*                                                                             *
*  Copyright 2019 Max Planck Institute for Dynamics and Self-Organization     *
*                                                                             *
*  This file is part of TurTLE.                                               *
*                                                                             *
*  TurTLE is free software: you can redistribute it and/or modify             *
*  it under the terms of the GNU General Public License as published          *
*  by the Free Software Foundation, either version 3 of the License,          *
*  or (at your option) any later version.                                     *
*                                                                             *
*  TurTLE is distributed in the hope that it will be useful,                  *
*  but WITHOUT ANY WARRANTY; without even the implied warranty of             *
*  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the              *
*  GNU General Public License for more details.                               *
*                                                                             *
*  You should have received a copy of the GNU General Public License          *
*  along with TurTLE.  If not, see <http://www.gnu.org/licenses/>             *
*                                                                             *
* Contact: Cristian.Lalescu@ds.mpg.de                                         *
*                                                                             *
******************************************************************************/
#ifndef P2P_CYLINDER_GHOST_COLLISIONS_HPP
#define P2P_CYLINDER_GHOST_COLLISIONS_HPP

#include <cstring>
#include <math.h>       /* for sqrt, abs */

#include <set>
#include <utility>
#include <vector>
#include "particles/p2p_ghost_collision_base.hpp"


template <class real_number, class partsize_t>
class p2p_cylinder_ghost_collisions: public p2p_ghost_collision_base<real_number, partsize_t>
{
    private:
        // Following doubles are needed for the collision computation
        double width;
        double length;

public:
    p2p_cylinder_ghost_collisions() : width(1.),length(1.){}

    // Copy constructor use a counter set to zero
    p2p_cylinder_ghost_collisions(const p2p_cylinder_ghost_collisions&){}

    template <int size_particle_positions, int size_particle_rhs>
    void compute_interaction(const partsize_t idx_part1,
                             const real_number pos_part1[], real_number /*rhs_part1*/[],
                             const partsize_t idx_part2,
                             const real_number pos_part2[], real_number /*rhs_part2*/[],
                             const real_number /*dist_pow2*/,  const real_number /*cutoff*/,
                             const real_number /*xshift_coef*/, const real_number /*yshift_coef*/, const real_number /*zshift_coef*/){
        double x, y, z, pq, xp, xq, t, s, x_dist, y_dist, z_dist ,min_distance, pi, pi2;

        pi2 = atan(1.0)*8.0;
        pi = atan(1.0)*4.0;

        /* Relative position vector of the two particles (x,y,z)^T */
        /* complicated usage of fmod, fabs and sgn because C++ fmod does not do what it should. */
        x = std::fmod(pos_part2[0],pi2)-std::fmod(pos_part1[0],pi2);
        y = std::fmod(pos_part2[1],pi2)-std::fmod(pos_part1[1],pi2);
        z = std::fmod(pos_part2[2],pi2)-std::fmod(pos_part1[2],pi2);

        x = ( std::fmod( std::fabs(x) +pi ,pi2) - pi ) * sgn(x) ;
        y = ( std::fmod( std::fabs(y) +pi ,pi2) - pi ) * sgn(y) ;
        z = ( std::fmod( std::fabs(z) +pi ,pi2) - pi ) * sgn(z) ;

        if( sqrt(x*x+y*y+z*z) <= length )
        {

        /* p and q are the orientation vectors of the first and second particles. */
        /* pq, xp, xq are scalar products of these vectors with x, relative position */
        pq = pos_part1[3] * pos_part2[3] + pos_part1[4] * pos_part2[4] + pos_part1[5] * pos_part2[5];
        xp = x * pos_part1[3] + y * pos_part1[4] + z * pos_part1[5];
        xq = x * pos_part2[3] + y * pos_part2[4] + z * pos_part2[5];
        /* t and s parametrize the two rods. Find min distance: */
        assert(this->length > 0);
        t = 2.0/(this->length*(pq*pq-1.0))*(-xp+pq*xq);
        s = 2.0/(this->length*(pq*pq-1.0))*(-pq*xp+xq);
        /* Test if -1<s<1 and -1<t<1 */
        if( abs(t)<=1.0 and abs(s)<=1.0 ) {
            /* Get minimal distance in case of both t and s in {-1,1}. Else: check edges */
            x_dist = this->length*0.5*t*pos_part1[3]-this->length*0.5*s*pos_part2[3]-x;
            y_dist = this->length*0.5*t*pos_part1[4]-this->length*0.5*s*pos_part2[4]-y;
            z_dist = this->length*0.5*t*pos_part1[5]-this->length*0.5*s*pos_part2[5]-z;
            min_distance = sqrt(x_dist*x_dist+y_dist*y_dist+z_dist*z_dist);
        }
        else
        {
            min_distance = this->length;
            /* t fixed at 1, find min along s */
            t = 1.0;
            s = t*pq-2.0/this->length*xq;
            if( abs(s)>1.0 ) { s = s / abs(s) ;}
            x_dist = this->length*0.5*t*pos_part1[3]-this->length*0.5*s*pos_part2[3]-x;
            y_dist = this->length*0.5*t*pos_part1[4]-this->length*0.5*s*pos_part2[4]-y;
            z_dist = this->length*0.5*t*pos_part1[5]-this->length*0.5*s*pos_part2[5]-z;
            min_distance = fmin( sqrt(x_dist*x_dist+y_dist*y_dist+z_dist*z_dist), min_distance );
            /* t fixed at -1, find min along s */
            t = -1.0;
            s = t*pq-2.0/this->length*xq;
            if( abs(s)>1.0 ) { s = s / abs(s) ;}
            x_dist = this->length*0.5*t*pos_part1[3]-this->length*0.5*s*pos_part2[3]-x;
            y_dist = this->length*0.5*t*pos_part1[4]-this->length*0.5*s*pos_part2[4]-y;
            z_dist = this->length*0.5*t*pos_part1[5]-this->length*0.5*s*pos_part2[5]-z;
            min_distance = fmin( sqrt(x_dist*x_dist+y_dist*y_dist+z_dist*z_dist), min_distance );
            /* s fixed at 1, find min along t */
            s = 1.0;
            t = s*pq+2.0/this->length*xp;
            if( abs(t)>1.0 ) { t = t / abs(t) ;}
            x_dist = this->length*0.5*t*pos_part1[3]-this->length*0.5*s*pos_part2[3]-x;
            y_dist = this->length*0.5*t*pos_part1[4]-this->length*0.5*s*pos_part2[4]-y;
            z_dist = this->length*0.5*t*pos_part1[5]-this->length*0.5*s*pos_part2[5]-z;
            min_distance = fmin( sqrt(x_dist*x_dist+y_dist*y_dist+z_dist*z_dist), min_distance );
            /* s fixed at -1, find min along t */
            s = -1.0;
            t = s*pq+2.0/this->length*xp;
            if( abs(t)>1.0 ) { t = t / abs(t) ;}
            x_dist = this->length*0.5*t*pos_part1[3]-this->length*0.5*s*pos_part2[3]-x;
            y_dist = this->length*0.5*t*pos_part1[4]-this->length*0.5*s*pos_part2[4]-y;
            z_dist = this->length*0.5*t*pos_part1[5]-this->length*0.5*s*pos_part2[5]-z;
            min_distance = fmin( sqrt(x_dist*x_dist+y_dist*y_dist+z_dist*z_dist), min_distance );
        }
        /* If cylinders overlap count it as a collision */
        if( min_distance<=width ){
            std::pair <partsize_t, partsize_t> single_collision_pair(idx_part1, idx_part2);
            this->collision_pairs_local.insert(single_collision_pair);
            //DEBUG_MSG("inside compute interaction idx_part1 = %ld and idx_part2 = %ld\n", idx_part1, idx_part2);
            assert(idx_part1!=idx_part2);
        }

		}
    }

    void set_width(const double WIDTH)
    {
        this->width = WIDTH;
    }

    double get_width()
    {
        return this->width;
    }

    void set_length(const double LENGTH)
    {
        this->length = LENGTH;
    }

    double get_length()
    {
        return this->length;
    }
};


#endif // P2P_CYLINDER_GHOST_COLLISIONS_HPP