Skip to content

Instantly share code, notes, and snippets.

@CannibalVox
Created December 15, 2021 19:36
Show Gist options
  • Save CannibalVox/c2f9cbe1628f05a7c4b3f1ca819f53b8 to your computer and use it in GitHub Desktop.
Save CannibalVox/c2f9cbe1628f05a7c4b3f1ca819f53b8 to your computer and use it in GitHub Desktop.
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