Skip to content

Instantly share code, notes, and snippets.

@Janiczek

Janiczek/Common.elm

Last active September 15, 2023 21:37
Show Gist options
  • Save Janiczek/77ca84fbeaaa4a0a6897c0cea3c25366 to your computer and use it in GitHub Desktop.
Save Janiczek/77ca84fbeaaa4a0a6897c0cea3c25366 to your computer and use it in GitHub Desktop.
Download ZIP
Elm implementations of shortest distance between two 3D line segments
Raw
Common.elm
module Common exposing
( Point
, Segment
, add
, addK
, distance
, dot
, magnitude
, mid
, scale
, sub
)
type alias Segment =
( Point, Point )
type alias Point =
( Float, Float, Float )
add : Point -> Point -> Point
add ( x0, y0, z0 ) ( x1, y1, z1 ) =
( x0 + x1, y0 + y1, z0 + z1 )
addK : Point -> Point -> Float -> Point
addK ( x0, y0, z0 ) ( x1, y1, z1 ) k =
( x0 + k * x1, y0 + k * y1, z0 + k * z1 )
sub : Point -> Point -> Point
sub ( x0, y0, z0 ) ( x1, y1, z1 ) =
( x0 - x1, y0 - y1, z0 - z1 )
scale : Float -> Point -> Point
scale k ( x, y, z ) =
( k * x, k * y, k * z )
dot : Point -> Point -> Float
dot ( x0, y0, z0 ) ( x1, y1, z1 ) =
x0 * x1 + y0 * y1 + z0 * z1
mid : Point -> Point -> Point
mid a b =
add a (scale 0.5 (sub b a))
distance : Point -> Point -> Float
distance ( x0, y0, z0 ) ( x1, y1, z1 ) =
sqrt <|
((x1 - x0) ^ 2)
+ ((y1 - y0) ^ 2)
+ ((z1 - z0) ^ 2)
magnitude : Point -> Float
magnitude ( x, y, z ) =
sqrt (x * x + y * y + z * z)
Raw
First.elm
module First exposing (shortestDistance)
{-| Hallucinated by ChatGPT :shrug:
-}
import Common exposing (..)
shortestDistance : Segment -> Segment -> Float
shortestDistance ( p1, p2 ) ( p3, p4 ) =
let
u =
sub p2 p1
v =
sub p4 p3
w =
sub p1 p3
--
a =
dot u u
b =
dot u v
c =
dot v v
d =
dot u w
e =
dot v w
--
s =
(b * e) - (c * d) / (a * c - b * b)
t =
(a * e) - (b * d) / (a * c - b * b)
--
inBounds n =
0 <= n && n <= 1
in
if inBounds s && inBounds t then
distance
(addK p1 u s)
(addK p3 v t)
else
List.minimum
[ distance p1 p3
, distance p1 p4
, distance p2 p3
, distance p2 p4
]
|> Maybe.withDefault 0
Raw
Second.elm
module Second exposing (shortestDistance)
{-| Ported from: <https://www.geometrictools.com/Documentation/DistanceLine3Line3.pdf>
-}
import Common exposing (..)
getClampedRoot : Float -> Float -> Float -> Float
getClampedRoot slope h0 h1 =
if h0 < 0 then
if h1 > 0 then
let
r =
-h0 / slope
in
if r > 1 then
0.5
else
r
else
1
else
0
classify : Float -> Float
classify n =
if n <= 0 then
-1
else if n >= 1 then
1
else
0
computeMinimumParameters :
{ edge0 : Float
, edge1 : Float
, end00 : Float
, end01 : Float
, end10 : Float
, end11 : Float
, b : Float
, c : Float
, e : Float
, g00 : Float
, g10 : Float
, g01 : Float
, g11 : Float
}
->
{ param0 : Float
, param1 : Float
}
computeMinimumParameters cfg =
let
( x0, y0 ) =
( cfg.end00, cfg.end01 )
( x1, y1 ) =
( cfg.end10, cfg.end11 )
delta =
y1 - y0
h0 =
delta * (-cfg.b * x0 + cfg.c * y0 - cfg.e)
in
if h0 >= 0 then
if cfg.edge0 == 0 then
{ param0 = 0
, param1 = getClampedRoot cfg.c cfg.g00 cfg.g01
}
else if cfg.edge0 == 1 then
{ param0 = 1
, param1 = getClampedRoot cfg.c cfg.g10 cfg.g11
}
else
{ param0 = x0
, param1 = y0
}
else
let
h1 =
delta * (-cfg.b * x1 + cfg.c * y1 - cfg.e)
in
if h1 <= 0 then
if cfg.edge1 == 0 then
{ param0 = 0
, param1 = getClampedRoot cfg.c cfg.g00 cfg.g01
}
else if cfg.edge1 == 1 then
{ param0 = 1
, param1 = getClampedRoot cfg.c cfg.g10 cfg.g11
}
else
{ param0 = x1
, param1 = y1
}
else
let
z =
min (max (h0 / (h0 - h1)) 0) 1
omz =
1 - z
in
{ param0 = omz * x0 + z * x1
, param1 = omz * y0 + z * y1
}
midIfOutOfBounds : Float -> Float
midIfOutOfBounds n =
if n < 0 || n > 1 then
0.5
else
n
computeIntersection :
{ sValue0 : Float
, sValue1 : Float
, classify0 : Float
, classify1 : Float
, b : Float
, f00 : Float
, f10 : Float
}
->
{ edge0 : Float
, edge1 : Float
, end00 : Float
, end01 : Float
, end10 : Float
, end11 : Float
}
computeIntersection cfg =
if cfg.classify0 < 0 then
let
( edge1, end10, end11 ) =
if cfg.classify1 == 0 then
( 3, cfg.sValue1, 1 )
else
( 1, 1, midIfOutOfBounds (cfg.f10 / cfg.b) )
in
{ edge0 = 0
, edge1 = edge1
, end00 = 0
, end01 = midIfOutOfBounds (cfg.f00 / cfg.b)
, end10 = end10
, end11 = end11
}
else if cfg.classify0 == 0 then
let
( edge1, end10, end11 ) =
if cfg.classify1 < 0 then
( 0, 0, midIfOutOfBounds (cfg.f00 / cfg.b) )
else if cfg.classify1 == 0 then
( 3, cfg.sValue1, 1 )
else
( 1, 1, midIfOutOfBounds (cfg.f10 / cfg.b) )
in
{ edge0 = 2
, edge1 = edge1
, end00 = cfg.sValue0
, end01 = 0
, end10 = end10
, end11 = end11
}
else
let
( edge1, end10, end11 ) =
if cfg.classify1 == 0 then
( 3, cfg.sValue1, 1 )
else
( 0, 0, midIfOutOfBounds (cfg.f00 / cfg.b) )
in
{ edge0 = 1
, edge1 = edge1
, end00 = 1
, end01 = midIfOutOfBounds (cfg.f10 / cfg.b)
, end10 = end10
, end11 = end11
}
shortestDistance : Segment -> Segment -> Float
shortestDistance ( p0, p1 ) ( q0, q1 ) =
let
p1mp0 =
sub p1 p0
q1mq0 =
sub q1 q0
p0mq0 =
sub p0 q0
a =
dot p1mp0 p1mp0
b =
dot p1mp0 q1mq0
c =
dot q1mq0 q1mq0
d =
dot p1mp0 p0mq0
e =
dot q1mq0 p0mq0
f00 =
d
f10 =
f00 + a
f01 =
f00 - b
f11 =
f10 - b
g00 =
-e
g10 =
g00 - b
g01 =
g00 + c
g11 =
g10 + c
{ param0, param1 } =
if a > 0 && c > 0 then
let
sValue0 =
getClampedRoot a f00 f10
sValue1 =
getClampedRoot a f01 f11
classify0 =
classify sValue0
classify1 =
classify sValue1
in
if classify0 == -1 && classify1 == -1 then
{ param0 = 0
, param1 = getClampedRoot c g00 g01
}
else if classify0 == 1 && classify1 == 1 then
{ param0 = 1
, param1 = getClampedRoot c g10 g11
}
else
let
i =
computeIntersection
{ sValue0 = sValue0
, sValue1 = sValue1
, classify0 = classify0
, classify1 = classify1
, b = b
, f00 = f00
, f10 = f10
}
min =
computeMinimumParameters
{ edge0 = i.edge0
, edge1 = i.edge1
, end00 = i.end00
, end01 = i.end01
, end10 = i.end10
, end11 = i.end11
, b = b
, c = c
, e = e
, g00 = g00
, g10 = g10
, g01 = g01
, g11 = g11
}
in
{ param0 = min.param0
, param1 = min.param1
}
else if a > 0 then
{ param0 = getClampedRoot a f00 f10
, param1 = 0
}
else if c > 0 then
{ param0 = 0
, param1 = getClampedRoot c g00 g01
}
else
{ param0 = 0
, param1 = 0
}
closest0 =
add
(scale (1 - param0) p0)
(scale param0 p1)
closest1 =
add
(scale (1 - param1) q0)
(scale param1 q1)
diff =
sub closest0 closest1
sqrDistance =
dot diff diff
in
sqrt sqrDistance
Raw
Tests.elm
module Tests exposing (..)
import Common exposing (..)
import Expect exposing (Expectation)
import First
import Fuzz exposing (Fuzzer)
import Second
import Test exposing (Test)
import Third
suite : Test
suite =
Test.describe "Various implementations"
[ testImpl "first" First.shortestDistance
, testImpl "second" Second.shortestDistance
, testImpl "third" Third.shortestDistance
]
testImpl : String -> (Segment -> Segment -> Float) -> Test
testImpl name impl =
Test.describe name
[ Test.fuzz line "identical lines" <|
\a ->
impl a a
|> Expect.within (Expect.Absolute 0) 0
, Test.fuzz2 line line "never negative" <|
\a b ->
impl a b
|> Expect.greaterThan 0
, Test.fuzz line "if line included in other line, d=0" <|
\(( p1, p2 ) as a) ->
impl a ( p1, mid p1 p2 )
|> Expect.within (Expect.Absolute 0) 0
, Test.fuzz2 line line "symmetrical" <|
\a b ->
impl a b
|> Expect.within (Expect.Absolute 0) (impl b a)
, Test.test "example #1" <|
\() ->
-- Taken from: https://mathematica.stackexchange.com/a/136418
impl
( ( 24.134, -147.93, 193.37 ), ( -9.1854, -85.421, 96.284 ) )
( ( -135.82, -104.67, 38.896 ), ( -177.11, -32.589, -74.348 ) )
|> Expect.within (Expect.Absolute 0) 140.3575023365691
, Test.test "robustness #1" <|
\() ->
-- Taken from: https://www.geometrictools.com/Documentation/DistanceLine3Line3.pdf
let
a : Segment
a =
( ( -1.0264718499965966, 9.61633410071954e-7, 0.0 )
, ( 0.9195080803241581, -1.0094441192690283e-6, 0.0 )
)
b : Segment
b =
( ( -1.062944738380611, 9.270954008214175e-7, 0.0 )
, ( 1.08115838682279, -1.0670017179567367e-6, 0.0 )
)
in
impl a b
|> Expect.within (Expect.Absolute 0) 0
, Test.test "robustness #2" <|
\() ->
-- Taken from: https://www.geometrictools.com/Documentation/DistanceLine3Line3.pdf
let
a : Segment
a =
( ( -1.08962174737826, 9.72361455950886e-7, 0.0 )
, ( 0.9122057859785855, -9.436982943210751e-7, 0.0 )
)
b : Segment
b =
( ( -0.9001044750213624, 9.067144635133444e-7, 0.0 )
, ( 1.073087717872113, -9.818578763399274e-7, 0.0 )
)
in
impl a b
|> Expect.within (Expect.Absolute 0) 1.1575046138574105e-7
, Test.test "robustness #3" <|
\() ->
-- Taken from: https://www.geometrictools.com/Documentation/DistanceLine3Line3.pdf
let
a : Segment
a =
( ( 0.7799899009987712, 0.6119250236079097, -0.22703111823648214 )
, ( 0.5321534452959895, 0.8572458550333977, -0.10102437809109688 )
)
b : Segment
b =
( ( -0.21277333982288837, 0.3509154808707535, -0.49557160679250956 )
, ( 0.11881479667499661, 0.022494725417345762, -0.6642662095837295 )
)
in
impl a b
|> Expect.within (Expect.Absolute 0) 0.9829239711648874
, Test.test "robustness #4" <|
\() ->
-- Taken from: https://www.geometrictools.com/Documentation/DistanceLine3Line3.pdf
let
delta =
0.25 * 1.0e-4
epsilon =
sqrt delta
phi =
1.0e-5
a : Segment
a =
( ( 0, 0, 0 )
, ( 1, 1, 1 )
)
b : Segment
b =
( ( negate epsilon, phi + delta, 0 )
, ( epsilon, phi - delta, 0 )
)
in
impl a b
|> Expect.within (Expect.Absolute 0) 0
]
line : Fuzzer Segment
line =
Fuzz.map2 Tuple.pair
point
point
point : Fuzzer Point
point =
Fuzz.map3 (\x y z -> ( x, y, z ))
Fuzz.niceFloat
Fuzz.niceFloat
Fuzz.niceFloat
Raw
Third.elm
module Third exposing (shortestDistance)
{-| Ported from <https://mathematica.stackexchange.com/a/136418>
-}
import Common exposing (..)
shortestDistance : Segment -> Segment -> Float
shortestDistance ( p0, p1 ) ( q0, q1 ) =
let
small =
1.0e-14
u =
sub p1 p0
v =
sub q1 q0
w =
sub p0 q0
a =
dot u u
b =
dot u v
c =
dot v v
d =
dot u w
e =
dot v w
det =
a * c - b * b
{ sn0, sd0, tn0, td0 } =
if det < small then
{ sn0 = 0
, sd0 = 1
, tn0 = e
, td0 = c
}
else
let
sn =
b * e - c * d
tn =
a * e - b * d
in
if sn < 0 then
{ sn0 = 0
, sd0 = det
, tn0 = e
, td0 = c
}
else if sn > det then
{ sn0 = det
, sd0 = det
, tn0 = e + b
, td0 = c
}
else
{ sn0 = det
, sd0 = det
, tn0 = det
, td0 = det
}
{ tn1, sn1, sd1 } =
if tn0 < 0 then
{ tn1 = 0
, sn1 =
if -d < 0 then
0
else if -d > a then
sd0
else
-d
, sd1 =
if -d < 0 || -d > a then
sd0
else
a
}
else if tn0 > td0 then
{ tn1 = td0
, sn1 =
if -d + b < 0 then
0
else if -d + b > a then
sd0
else
-d + b
, sd1 =
if -d + b < 0 || -d + b > a then
sd0
else
a
}
else
{ sn1 = sn0
, sd1 = sd0
, tn1 = tn0
}
sc =
if abs sn1 < small then
0
else
sn1 / sd1
tc =
if abs tn1 < small then
0
else
tn1 / td0
in
magnitude (add w (sub (scale sc u) (scale tc v)))
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment