Advertisement
Guest User

Closest distance between lines

a guest
Sep 9th, 2017
1,834
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C# 3.89 KB | None | 0 0
  1.     private ClosestLinePointResult closestDistanceBetweenLines(Vector3 a0, Vector3 a1, Vector3 b0, Vector3 b1)
  2.     {
  3.         //Based on https://stackoverflow.com/questions/2824478/shortest-distance-between-two-line-segments
  4.  
  5.         // Given two lines defined by (a0,a1,b0,b1)
  6.         // Return the closest points on each segment and their distance
  7.         //
  8.  
  9.         var result = new ClosestLinePointResult();
  10.  
  11.         // Calculate denomitator
  12.         var A = a1 - a0;
  13.         var B = b1 - b0;
  14.         float magA = A.magnitude;
  15.         float magB = B.magnitude;
  16.    
  17.         var _A = A / magA;
  18.         var _B = B / magB;
  19.  
  20.         var cross = Vector3.Cross(_A, _B);
  21.         var denom = cross.magnitude * cross.magnitude;
  22.  
  23.  
  24.         // If lines are parallel (denom=0) test if lines overlap.
  25.         // If they don't overlap then there is a closest point solution.
  26.         // If they do overlap, there are infinite closest positions, but there is a closest distance
  27.         if (denom == 0)
  28.         {
  29.             var d0 = Vector3.Dot(_A, (b0 - a0));
  30.  
  31.             // Overlap only possible with clamping
  32.  
  33.             var d1 = Vector3.Dot(_A, (b1 - a0));
  34.  
  35.             // Is segment B before A?
  36.             if (d0 <= 0 && 0 >= d1)
  37.             {
  38.                 if (Mathf.Abs(d0) < Mathf.Abs(d1))
  39.                 {
  40.                     result.Line1Closest = a0;
  41.                     result.Line2Closest = b0;
  42.                     result.Distance = (a0 - b0).magnitude;
  43.  
  44.                     return result;
  45.                 }
  46.                 result.Line1Closest = a0;
  47.                 result.Line2Closest = b1;
  48.                 result.Distance = (a0 - b1).magnitude;
  49.  
  50.                 return result;
  51.             }
  52.             // Is segment B after A?
  53.             else if (d0 >= magA && magA <= d1)
  54.             {
  55.                 if (Mathf.Abs(d0) < Mathf.Abs(d1))
  56.    
  57.             {
  58.                     result.Line1Closest = a1;
  59.                     result.Line2Closest = b0;
  60.                     result.Distance = (a1 - b0).magnitude;
  61.  
  62.                     return result;
  63.                 }
  64.                 result.Line1Closest = a1;
  65.                 result.Line2Closest = b1;
  66.                 result.Distance = (a1 - b1).magnitude;
  67.  
  68.                 return result;
  69.             }
  70.  
  71.             // Segments overlap, return distance between parallel segments
  72.             result.Line1Closest = Vector3.zero;
  73.             result.Line2Closest = Vector3.zero;
  74.             result.Distance = (((d0 * _A) + a0) - b0).magnitude;
  75.             return result;
  76.         }
  77.  
  78.  
  79.         // Lines criss-cross: Calculate the projected closest points
  80.         var t = (b0 - a0);
  81.         var detA = Determinant(t, _B, cross);
  82.         var detB = Determinant(t, _A, cross);
  83.  
  84.         var t0 = detA / denom;
  85.         var t1 = detB / denom;
  86.  
  87.         var pA = a0 + (_A * t0); // Projected closest point on segment A
  88.         var pB = b0 + (_B * t1); // Projected closest point on segment B
  89.  
  90.  
  91.         // Clamp projections
  92.         if (t0 < 0)
  93.             pA = a0;
  94.         else if (t0 > magA)
  95.             pA = a1;
  96.  
  97.         if (t1 < 0)
  98.             pB = b0;
  99.         else if (t1 > magB)
  100.             pB = b1;
  101.  
  102.         float dot;
  103.         // Clamp projection A
  104.         if (t0 < 0 || t0 > magA)
  105.         {
  106.             dot = Vector3.Dot(_B, (pA - b0));
  107.             if (dot < 0)
  108.                 dot = 0;
  109.             else if (dot > magB)
  110.                 dot = magB;
  111.             pB = b0 + (_B * dot);
  112.         }
  113.         // Clamp projection B
  114.         if (t1 < 0 || t1 > magB)
  115.         {
  116.             dot = Vector3.Dot(_A, (pB - a0));
  117.             if (dot < 0)
  118.                 dot = 0;
  119.             else if (dot > magA)
  120.                 dot = magA;
  121.             pA = a0 + (_A * dot);
  122.         }
  123.  
  124.         result.Line1Closest = pA;
  125.         result.Line2Closest = pB;
  126.         result.Distance = (pA - pB).magnitude;
  127.         return result;
  128.     }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement