Skip to content
Snippets Groups Projects
Commit c2241871 authored by Cristian Lalescu's avatar Cristian Lalescu
Browse files

add sanity check to p2p_cylinder_collision calculation

parent aed4d6f6
No related branches found
No related tags found
No related merge requests found
......@@ -67,44 +67,45 @@ public:
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: */
t = 2.0/(length*(pq*pq-1.0))*(-xp+pq*xq);
s = 2.0/(length*(pq*pq-1.0))*(-pq*xp+xq);
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 = length*0.5*t*pos_part1[3]-length*0.5*s*pos_part2[3]-x;
y_dist = length*0.5*t*pos_part1[4]-length*0.5*s*pos_part2[4]-y;
z_dist = length*0.5*t*pos_part1[5]-length*0.5*s*pos_part2[5]-z;
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 = length;
min_distance = this->length;
/* t fixed at 1, find min along s */
t = 1.0;
s = t*pq-2.0/length*xq;
x_dist = length*0.5*t*pos_part1[3]-length*0.5*s*pos_part2[3]-x;
y_dist = length*0.5*t*pos_part1[4]-length*0.5*s*pos_part2[4]-y;
z_dist = length*0.5*t*pos_part1[5]-length*0.5*s*pos_part2[5]-z;
s = t*pq-2.0/this->length*xq;
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/length*xq;
x_dist = length*0.5*t*pos_part1[3]-length*0.5*s*pos_part2[3]-x;
y_dist = length*0.5*t*pos_part1[4]-length*0.5*s*pos_part2[4]-y;
z_dist = length*0.5*t*pos_part1[5]-length*0.5*s*pos_part2[5]-z;
s = t*pq-2.0/this->length*xq;
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/length*xp;
x_dist = length*0.5*t*pos_part1[3]-length*0.5*s*pos_part2[3]-x;
y_dist = length*0.5*t*pos_part1[4]-length*0.5*s*pos_part2[4]-y;
z_dist = length*0.5*t*pos_part1[5]-length*0.5*s*pos_part2[5]-z;
t = s*pq+2.0/this->length*xp;
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/length*xp;
x_dist = length*0.5*t*pos_part1[3]-length*0.5*s*pos_part2[3]-x;
y_dist = length*0.5*t*pos_part1[4]-length*0.5*s*pos_part2[4]-y;
z_dist = length*0.5*t*pos_part1[5]-length*0.5*s*pos_part2[5]-z;
t = s*pq+2.0/this->length*xp;
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 */
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment