Commit b6201989 authored by Jose Agustin Arguedas Leiva's avatar Jose Agustin Arguedas Leiva Committed by Cristian Lalescu
Browse files

passed functionality in p2p cylinders to a function

parent b50f99c7
Pipeline #94848 failed with stages
in 65 minutes and 39 seconds
......@@ -90,6 +90,16 @@ public:
this->collision_pairs_local.reserve(src.collision_pairs_local.capacity());
}
double rod_distance(double px, double py, double pz, double qx, double qy, double qz, double t, double s, double x, double y, double z){
double x_dist, y_dist, z_dist;
double min_distance;
x_dist = this->cylinder_length*0.5*t*px-this->cylinder_length*0.5*s*qx+x;
y_dist = this->cylinder_length*0.5*t*py-this->cylinder_length*0.5*s*qy+y;
z_dist = this->cylinder_length*0.5*t*pz-this->cylinder_length*0.5*s*qz+z;
min_distance = sqrt(x_dist*x_dist+y_dist*y_dist+z_dist*z_dist);
return min_distance;
}
template <int size_particle_rhs>
void init_result_array(real_number /*rhs*/[], const partsize_t /*nbParticles*/) const{
}
......@@ -209,7 +219,8 @@ public:
break;
case CYLINDER:
{
double pq, xp, xq, t, s, x_dist, y_dist, z_dist ,min_distance;
double pq, xp, xq, t, s, min_distance, min_distance_current;
double px,py, pz, qx, qy, qz;
const double x = xseparation;
const double y = yseparation;
......@@ -218,12 +229,17 @@ public:
if( r <= this->cylinder_length )
{
/* p and q are the orientation vectors of the first and second particles. */
px = pos_part1[IDXC_X+3];
py = pos_part1[IDXC_Y+3];
pz = pos_part1[IDXC_Z+3];
qx = pos_part2[IDXC_X+3];
qy = pos_part2[IDXC_Y+3];
qz = pos_part2[IDXC_Z+3];
/* pq, xp, xq are scalar products of these vectors with x, relative position */
pq = pos_part1[IDXC_X+3] * pos_part2[IDXC_X+3] + pos_part1[IDXC_Y+3] * pos_part2[IDXC_Y+3] + pos_part1[IDXC_Z+3] * pos_part2[IDXC_Z+3];
xp = x * pos_part1[IDXC_X+3] + y * pos_part1[IDXC_Y+3] + z * pos_part1[IDXC_Z+3];
xq = x * pos_part2[IDXC_X+3] + y * pos_part2[IDXC_Y+3] + z * pos_part2[IDXC_Z+3];
pq = px * qx + py * qy + pz * qz;
xp = x * px + y * py + z * pz;
xq = x * qx + y * qy + z * qz;
/* t and s parametrize the two rods. Find min distance: */
assert(this->cylinder_length > 0);
t = 2.0/(this->cylinder_length*(pq*pq-1.0))*(xp-pq*xq);
......@@ -232,10 +248,7 @@ public:
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->cylinder_length*0.5*t*pos_part1[IDXC_X+3]-this->cylinder_length*0.5*s*pos_part2[IDXC_X+3]+x;
y_dist = this->cylinder_length*0.5*t*pos_part1[IDXC_Y+3]-this->cylinder_length*0.5*s*pos_part2[IDXC_Y+3]+y;
z_dist = this->cylinder_length*0.5*t*pos_part1[IDXC_Z+3]-this->cylinder_length*0.5*s*pos_part2[IDXC_Z+3]+z;
min_distance = sqrt(x_dist*x_dist+y_dist*y_dist+z_dist*z_dist);
min_distance = this->rod_distance(px, py, pz, qx, qy, qz, t, s, x, y, z);
}
else
{
......@@ -244,34 +257,26 @@ public:
t = 1.0;
s = t*pq+2.0/this->cylinder_length*xq;
if( abs(s)>1.0 ) { s = s / abs(s) ;}
x_dist = this->cylinder_length*0.5*t*pos_part1[IDXC_X+3]-this->cylinder_length*0.5*s*pos_part2[IDXC_X+3]+x;
y_dist = this->cylinder_length*0.5*t*pos_part1[IDXC_Y+3]-this->cylinder_length*0.5*s*pos_part2[IDXC_Y+3]+y;
z_dist = this->cylinder_length*0.5*t*pos_part1[IDXC_Z+3]-this->cylinder_length*0.5*s*pos_part2[IDXC_Z+3]+z;
min_distance = fmin( sqrt(x_dist*x_dist+y_dist*y_dist+z_dist*z_dist), min_distance );
min_distance_current = this->rod_distance(px, py, pz, qx, qy, qz, t, s, x, y, z);
min_distance = fmin( min_distance_current, min_distance );
/* t fixed at -1, find min along s */
t = -1.0;
s = t*pq+2.0/this->cylinder_length*xq;
if( abs(s)>1.0 ) { s = s / abs(s) ;}
x_dist = this->cylinder_length*0.5*t*pos_part1[IDXC_X+3]-this->cylinder_length*0.5*s*pos_part2[IDXC_X+3]+x;
y_dist = this->cylinder_length*0.5*t*pos_part1[IDXC_Y+3]-this->cylinder_length*0.5*s*pos_part2[IDXC_Y+3]+y;
z_dist = this->cylinder_length*0.5*t*pos_part1[IDXC_Z+3]-this->cylinder_length*0.5*s*pos_part2[IDXC_Z+3]+z;
min_distance = fmin( sqrt(x_dist*x_dist+y_dist*y_dist+z_dist*z_dist), min_distance );
min_distance_current = this->rod_distance(px, py, pz, qx, qy, qz, t, s, x, y, z);
min_distance = fmin( min_distance_current, min_distance );
/* s fixed at 1, find min along t */
s = 1.0;
t = s*pq-2.0/this->cylinder_length*xp;
if( abs(t)>1.0 ) { t = t / abs(t) ;}
x_dist = this->cylinder_length*0.5*t*pos_part1[IDXC_X+3]-this->cylinder_length*0.5*s*pos_part2[IDXC_X+3]+x;
y_dist = this->cylinder_length*0.5*t*pos_part1[IDXC_Y+3]-this->cylinder_length*0.5*s*pos_part2[IDXC_Y+3]+y;
z_dist = this->cylinder_length*0.5*t*pos_part1[IDXC_Z+3]-this->cylinder_length*0.5*s*pos_part2[IDXC_Z+3]+z;
min_distance = fmin( sqrt(x_dist*x_dist+y_dist*y_dist+z_dist*z_dist), min_distance );
min_distance_current = this->rod_distance(px, py, pz, qx, qy, qz, t, s, x, y, z);
min_distance = fmin( min_distance_current, min_distance );
/* s fixed at -1, find min along t */
s = -1.0;
t = s*pq-2.0/this->cylinder_length*xp;
if( abs(t)>1.0 ) { t = t / abs(t) ;}
x_dist = this->cylinder_length*0.5*t*pos_part1[IDXC_X+3]-this->cylinder_length*0.5*s*pos_part2[IDXC_X+3]+x;
y_dist = this->cylinder_length*0.5*t*pos_part1[IDXC_Y+3]-this->cylinder_length*0.5*s*pos_part2[IDXC_Y+3]+y;
z_dist = this->cylinder_length*0.5*t*pos_part1[IDXC_Z+3]-this->cylinder_length*0.5*s*pos_part2[IDXC_Z+3]+z;
min_distance = fmin( sqrt(x_dist*x_dist+y_dist*y_dist+z_dist*z_dist), min_distance );
min_distance_current = this->rod_distance(px, py, pz, qx, qy, qz, t, s, x, y, z);
min_distance = fmin( min_distance_current, min_distance );
}
/* If cylinders overlap count it as a collision */
if( min_distance<=this->cylinder_width )
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment