fork(1) download
copy 
  1. #include "stdio.h"
  2. #include <cmath>
  3.  
  4. double dot(double * u, double * v);
  5. double abs(double * u);
  6. double min(double a, double b, double c, double d);
  7. void prod(double k, double * u, double * v);
  8. void diff(double * a, double * b, double * c);
  9. void summ(double * a, double * b, double * c);
  10. bool in(double s);
  11. bool in(double s, double t);
  12. double sd(double s, double t);
  13. double _(double s);
  14.  
  15. double P0[4],P1[4],Q0[4],Q1[4];
  16. double u[4],v[4],w0[4];
  17.  
  18. double InterSegmentDistance4d()
  19. //input: four 4d points P0, P1, Q0, Q1
  20. // output: r = minimal distance between lines segments P0P1 and Q0Q1
  21.     {
  22.       double r = 0;
  23.       //calculation of three vectors
  24. 	  //u = P1 - P0, v = Q1 - Q0, w0 = Q0 - P0
  25.           diff(P1, P0, u);  diff(Q1, Q0, v);  diff(Q0, P0, w0);
  26. 	   //calculation of supporting scalar variables
  27. 	    //a = u*u, b = u*v, c = v*v, d = w0*u, e = w0*v
  28. 	    double  a, b, c, d, e;
  29.     	        a = dot(u,u);  // a=u^2;
  30. 	            b = dot(u,v);
  31. 	            c = dot(v,v);  // c=v^2;
  32. 	            d = dot(u,w0);
  33. 	            e = dot(v,w0);
  34. 	 //calculation of main determinant
  35. 	     // D = b^2 - a*c
  36. 	      double D = b*b - a*c;
  37. 	      if (D == 0) //lines are parallel
  38.             {
  39.              double t0=-e/c; double t1=-e/c; double s0 = (-e+b)/c ; double s1= (e-c)*b/(c*a);
  40.             if (in(t0) || in(t1) || in(s0) || in(s1))
  41.                {
  42.                    double wq[4];
  43.                   prod(e/c,v,  wq);  //projection w0 on line q
  44.                   double wd[4];
  45.                    diff(w0,wq,  wd);
  46.                     r = abs(wd);
  47.                      return r ; //r = |w0 - wq|
  48.                 }
  49.                //else minimal distance between segments equals minimal distance between ends of segments
  50.                      r = min(sd(0,0), sd(0,1), sd(1,0), sd(1,1));
  51. 		             return r;
  52.             }
  53. 	       // else lines are not parallel
  54. 	        {
  55.                 //calculation of determinants
  56. 		    double D1 = b*e - c*d,  D2 = a*e - b*d;
  57. 		    //calculation of parameters for minimal segment between lines
  58.             double sc = D1/D, tc = D2/D;
  59.              if(in(sc,tc)) //minimal distance between segments equals minimal distance between lines
  60.                 {
  61.                    return r = sd(sc,tc); // r = |wc|
  62.                 }
  63.              //else minimal segment is defined by some parameter point (s,t) on edge of square [0,1]x[0,1]
  64.                  {
  65.                     double t1 = 0,   s1=_(d/a);
  66.                     double t2 = _(-2*e/c), s2=0;
  67.                     double t3 = 1,   s3=_(2*(b+d)/a);
  68.                     double t4 = _(2*(b+e)/c), s4=0;
  69.                     return min(sd(s1,t1), sd(s2,t2),sd(s3,t3), sd(s4,t4));
  70.                  }
  71. 	        }
  72.     }
  73.  
  74. int main()
  75. {
  76. 	scanf("%lf %lf %lf %lf",&P0[0], &P0[1], &P0[2],&P0[3]);
  77. 	scanf("%lf %lf %lf %lf",&P1[0], &P1[1], &P1[2],&P1[3]);
  78. scanf("%lf %lf %lf %lf",&Q0[0], &Q0[1], &Q0[2],&Q0[3]);
  79. 	scanf("%lf %lf %lf %lf",&Q1[0], &Q1[1], &Q1[2],&Q1[3]);
  80.         printf("%lf\n", InterSegmentDistance4d());
  81.         return 0;
  82. }
  83.  
  84. double dot(double * u, double * v)//calculing the scalar production of two vectors
  85. {
  86.     return u[0]*v[0]+u[1]*v[1]+u[2]*v[2]+u[3]*v[3];
  87. }
  88.  
  89. double abs(double * u)//finding the length of vector
  90. {
  91.     return sqrt(dot(u,u));
  92. }
  93.  
  94. double min(double a, double b, double c, double d)//finding the minimal value
  95. {
  96.     double m = a;
  97.      if(m>b) m = b;
  98.       if(m>c) m = c;
  99.        if(m>d) m = d;
  100.         return m;
  101. }
  102.  
  103. void diff(double * a, double * b, double * c)//finding the difference of two 4d objects(dots or vectors)
  104. {
  105. 	c[0]=a[0]-b[0];
  106. 	c[1]=a[1]-b[1];
  107. 	c[2]=a[2]-b[2];
  108. 	c[3]=a[3]-b[3];
  109. }
  110.  
  111. void summ(double * a, double * b, double * c)//finding the sum of two 4d objects(dots or vecrors)
  112. {
  113. 	c[0]=a[0]+b[0];
  114. 	c[1]=a[1]+b[1];
  115. 	c[2]=a[2]+b[2];
  116. 	c[3]=a[3]+b[3];
  117. }
  118.  
  119. void prod(double k, double * u, double * v)//calculing the production of vector and number
  120. {
  121.  v[0] = k * u[0];
  122.  v[1] = k * u[1];
  123.  v[2] = k * u[2];
  124.  v[3] = k * u[3];
  125. }
  126.  
  127. bool in(double s)//checking if the vatue is between 0 and 1
  128.  {
  129.     return 0 <= s && s <= 1 ;
  130.  }
  131.  
  132. bool in(double s, double t)//variation of the previous function for two values
  133. {
  134.     return  in(t) && in(s);
  135. }
  136.  
  137. double sd(double s, double t)//finding the distance between two segments using two parameters s and t
  138. {
  139.     double Pc[4], Qc[4], W[4];
  140.      prod(s,u, Pc);
  141.       summ(Pc,P0, Pc);
  142.      prod(t,v, Qc);
  143.       summ(Qc,Q0, Qc);
  144.        diff(Qc, Pc, W);
  145.         return  abs(W);
  146. }
  147.  
  148. double _(double s)//restriction the value for the interval [0,1]
  149. {
  150.    double s1 = s;
  151.    if(s > 1)s1 = 1;
  152.    if(s < 0)s1 = 0;
  153.     return s1;
  154. }
Success #stdin #stdout 0s 3464KB
comments (?)
stdin
copy 
Standard input is empty
stdout
copy 
0.000000
https://ideone.com/BfftDH
right version mif 15
language:
C++14 (clang 8.0)
created:
9 years ago
visibility:
secret

Share or Embed source code

Discover > Sphere Engine API

The brand new service which powers Ideone!

Discover > IDE Widget

Widget for compiling and running the source code in a web browser!