Created
December 15, 2021 19:36
-
-
Save CannibalVox/c2f9cbe1628f05a7c4b3f1ca819f53b8 to your computer and use it in GitHub Desktop.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
func DistanceLineToLineForCylinder(line1Start, line1End, line2Start, line2End geom.Coord, radius1, radius2 float64) float64 { | |
/** | |
* This calculation is susceptible to roundoff errors when | |
* passed large ordinate values. | |
* It may be possible to improve this by using {@link DD} arithmetic. | |
*/ | |
if Equals(line1Start, line1End) { | |
return DistancePointToLine(line1Start, line2Start, line2End) | |
} | |
if Equals(line2Start, line1End) { | |
return DistancePointToLine(line2Start, line1Start, line1End) | |
} | |
/** | |
* Algorithm derived from http://softsurfer.com/Archive/algorithm_0106/algorithm_0106.htm | |
*/ | |
a := VectorDot(line1Start, line1End, line1Start, line1End) | |
b := VectorDot(line1Start, line1End, line2Start, line2End) | |
c := VectorDot(line2Start, line2End, line2Start, line2End) | |
d := VectorDot(line1Start, line1End, line2Start, line1Start) | |
e := VectorDot(line2Start, line2End, line2Start, line1Start) | |
denom := a*c - b*b | |
if math.IsNaN(denom) { | |
panic("Ordinates must not be NaN") | |
} | |
var s, t float64 | |
if denom <= 0.0 { | |
/** | |
* The lines are parallel. | |
* In this case solve for the parameters s and t by assuming s is 0. | |
*/ | |
s = 0 | |
// choose largest denominator for optimal numeric conditioning | |
if b > c { | |
t = d / b | |
} else { | |
t = e / c | |
} | |
} else { | |
s = (b*e - c*d) / denom | |
t = (a*e - b*d) / denom | |
} | |
if s < 0 { | |
s = 0 | |
} else if s > 1 { | |
s = 1 | |
} | |
if t < 0 { | |
t = 0 | |
} else if t > 1 { | |
t = 1 | |
} | |
/** | |
* The closest points are in interiors of segments, | |
* so compute them directly | |
*/ | |
x1 := line1Start[0] + s*(line1End[0]-line1Start[0]) | |
y1 := line1Start[1] + s*(line1End[1]-line1Start[1]) | |
z1 := line1Start[2] + s*(line1End[2]-line1Start[2]) | |
x2 := line2Start[0] + t*(line2End[0]-line2Start[0]) | |
y2 := line2Start[1] + t*(line2End[1]-line2Start[1]) | |
z2 := line2Start[2] + t*(line2End[2]-line2Start[2]) | |
// length (p1-p2) | |
closest1 := geom.Coord{x1, y1, z1} | |
closest2 := geom.Coord{x2, y2, z2} | |
dist := Distance(closest1, clostest2) | |
if s <= 0.0001 && VectorDot(line1End, line1Start, closest1, closest2) > 0 { | |
radius1 = 0 | |
} else if s >= 0.999 && VectorDot(line1Start, line1End, closest1, closest2) > 0 { | |
radius1 = 0 | |
} | |
if t <= 0.0001 && VectorDot(line2End, line2Start, closest2, closest1) > 0 { | |
radius2 = 0 | |
} else if t >= 0.999 && VectorDot(line2Start, line2End, closest2, closest1) > 0 { | |
radius2 = 0 | |
} | |
return dist - radius1 - radius2 | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment