-- SPDX-License-Identifier: MPL-2.0

{- | Checked distances, containment, intersections and primitive signed distance
functions. Boundaries are included, rays return their nonnegative parameter
(direction need not be normalized), and coincident/overlapping intersections
are explicit. Metrics accept dimensions 1..16; discrete Jaccard uses sets.
-}
module HMath.Geometry.Primitives (
    Metric (..),
    distance,
    cosineDistance,
    hammingDistance,
    jaccardDistance,
    hausdorffDistance,
    mahalanobisDistance,
    pointLineDistance,
    pointSegmentDistance,
    pointPlaneDistance,
    orientation,
    incircle,
    polygonLocation,
    triangleLocation,
    sphereContains,
    boxContains,
    ellipseContains,
    triangleBarycentric,
    circumcenter,
    incenter,
    Intersection2 (..),
    segmentIntersection,
    lineIntersection,
    linePlaneIntersection,
    planeIntersection,
    rayTriangle,
    rayBox,
    raySphere,
    CircleIntersection (..),
    circleIntersection,
    triangleIntersection,
    sphereSDF,
    boxSDF,
    obbSDF,
    capsuleSDF,
    cylinderSDF,
    coneSDF,
) where

import Data.List (nub, sort)
import HMath.Error (MathError (..))
import HMath.Geometry.Internal

-- | Minkowski p must be finite and at least one.
data Metric = Euclidean | Manhattan | Chebyshev | Minkowski Double deriving (Metric -> Metric -> Bool
(Metric -> Metric -> Bool)
-> (Metric -> Metric -> Bool) -> Eq Metric
forall a. (a -> a -> Bool) -> (a -> a -> Bool) -> Eq a
$c== :: Metric -> Metric -> Bool
== :: Metric -> Metric -> Bool
$c/= :: Metric -> Metric -> Bool
/= :: Metric -> Metric -> Bool
Eq, Int -> Metric -> ShowS
[Metric] -> ShowS
Metric -> String
(Int -> Metric -> ShowS)
-> (Metric -> String) -> ([Metric] -> ShowS) -> Show Metric
forall a.
(Int -> a -> ShowS) -> (a -> String) -> ([a] -> ShowS) -> Show a
$cshowsPrec :: Int -> Metric -> ShowS
showsPrec :: Int -> Metric -> ShowS
$cshow :: Metric -> String
show :: Metric -> String
$cshowList :: [Metric] -> ShowS
showList :: [Metric] -> ShowS
Show)

-- | Euclidean, L1, Linfinity, or scaled Lp distance.
distance :: Metric -> PointND -> PointND -> Either MathError Double
distance :: Metric -> PointND -> PointND -> Either MathError Double
distance Metric
metric PointND
a PointND
b = do
    PointND -> PointND -> Either MathError ()
checkPair PointND
a PointND
b
    let xs :: [Double]
xs = (Double -> Double) -> [Double] -> [Double]
forall a b. (a -> b) -> [a] -> [b]
map Double -> Double
forall a. Num a => a -> a
abs ([Double] -> [Double] -> [Double]
sub (PointND -> [Double]
coordsND PointND
a) (PointND -> [Double]
coordsND PointND
b)); s :: Double
s = [Double] -> Double
forall a. Ord a => [a] -> a
forall (t :: * -> *) a. (Foldable t, Ord a) => t a -> a
maximum [Double]
xs
    case Metric
metric of
        Metric
Euclidean -> Double -> Either MathError Double
forall a b. b -> Either a b
Right ([Double] -> Double
norm [Double]
xs)
        Metric
Manhattan -> Double -> Either MathError Double
forall a b. b -> Either a b
Right ([Double] -> Double
forall a. Num a => [a] -> a
forall (t :: * -> *) a. (Foldable t, Num a) => t a -> a
sum [Double]
xs)
        Metric
Chebyshev -> Double -> Either MathError Double
forall a b. b -> Either a b
Right Double
s
        Minkowski Double
p ->
            Bool -> String -> Either MathError ()
require (Double -> Bool
finite Double
p Bool -> Bool -> Bool
&& Double
p Double -> Double -> Bool
forall a. Ord a => a -> a -> Bool
>= Double
1) String
"Minkowski p must be >=1"
                Either MathError ()
-> Either MathError Double -> Either MathError Double
forall a b.
Either MathError a -> Either MathError b -> Either MathError b
forall (m :: * -> *) a b. Monad m => m a -> m b -> m b
>> Double -> Either MathError Double
forall a b. b -> Either a b
Right (if Double
s Double -> Double -> Bool
forall a. Eq a => a -> a -> Bool
== Double
0 then Double
0 else Double
s Double -> Double -> Double
forall a. Num a => a -> a -> a
* ([Double] -> Double
forall a. Num a => [a] -> a
forall (t :: * -> *) a. (Foldable t, Num a) => t a -> a
sum [(Double
x Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
s) Double -> Double -> Double
forall a. Floating a => a -> a -> a
** Double
p | Double
x <- [Double]
xs]) Double -> Double -> Double
forall a. Floating a => a -> a -> a
** (Double
1 Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
p))

-- | One minus cosine similarity; zero vectors are undefined.
cosineDistance :: PointND -> PointND -> Either MathError Double
cosineDistance :: PointND -> PointND -> Either MathError Double
cosineDistance PointND
a PointND
b = do
    PointND -> PointND -> Either MathError ()
checkPair PointND
a PointND
b
    let x :: [Double]
x = PointND -> [Double]
coordsND PointND
a; y :: [Double]
y = PointND -> [Double]
coordsND PointND
b; nx :: Double
nx = [Double] -> Double
norm [Double]
x; ny :: Double
ny = [Double] -> Double
norm [Double]
y
    Bool -> String -> Either MathError ()
require (Double
nx Double -> Double -> Bool
forall a. Ord a => a -> a -> Bool
> Double
0 Bool -> Bool -> Bool
&& Double
ny Double -> Double -> Bool
forall a. Ord a => a -> a -> Bool
> Double
0) String
"cosine distance of zero vector"
    Double -> Either MathError Double
forall a b. b -> Either a b
Right (Double
1 Double -> Double -> Double
forall a. Num a => a -> a -> a
- Double -> Double -> Double
forall a. Ord a => a -> a -> a
max (-Double
1) (Double -> Double -> Double
forall a. Ord a => a -> a -> a
min Double
1 ([Double] -> [Double] -> Double
dot ((Double -> Double) -> [Double] -> [Double]
forall a b. (a -> b) -> [a] -> [b]
map (Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
nx) [Double]
x) ((Double -> Double) -> [Double] -> [Double]
forall a b. (a -> b) -> [a] -> [b]
map (Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
ny) [Double]
y))))

-- | Number of unequal positions (not normalized).
hammingDistance :: (Eq a) => [a] -> [a] -> Either MathError Int
hammingDistance :: forall a. Eq a => [a] -> [a] -> Either MathError Int
hammingDistance [a]
a [a]
b =
    Bool -> String -> Either MathError ()
require ([a] -> Int
forall a. [a] -> Int
forall (t :: * -> *) a. Foldable t => t a -> Int
length [a]
a Int -> Int -> Bool
forall a. Eq a => a -> a -> Bool
== [a] -> Int
forall a. [a] -> Int
forall (t :: * -> *) a. Foldable t => t a -> Int
length [a]
b Bool -> Bool -> Bool
&& [a] -> Int
forall a. [a] -> Int
forall (t :: * -> *) a. Foldable t => t a -> Int
length [a]
a Int -> Int -> Bool
forall a. Ord a => a -> a -> Bool
<= Int
1000000) String
"Hamming length mismatch/budget"
        Either MathError () -> Either MathError Int -> Either MathError Int
forall a b.
Either MathError a -> Either MathError b -> Either MathError b
forall (m :: * -> *) a b. Monad m => m a -> m b -> m b
>> Int -> Either MathError Int
forall a b. b -> Either a b
Right ([Bool] -> Int
forall a. [a] -> Int
forall (t :: * -> *) a. Foldable t => t a -> Int
length ((Bool -> Bool) -> [Bool] -> [Bool]
forall a. (a -> Bool) -> [a] -> [a]
filter Bool -> Bool
not ((a -> a -> Bool) -> [a] -> [a] -> [Bool]
forall a b c. (a -> b -> c) -> [a] -> [b] -> [c]
zipWith a -> a -> Bool
forall a. Eq a => a -> a -> Bool
(==) [a]
a [a]
b)))

-- | Set Jaccard distance; two empty sets have distance zero.
jaccardDistance :: (Eq a) => [a] -> [a] -> Either MathError Double
jaccardDistance :: forall a. Eq a => [a] -> [a] -> Either MathError Double
jaccardDistance [a]
a [a]
b = do
    Bool -> String -> Either MathError ()
require ([a] -> Int
forall a. [a] -> Int
forall (t :: * -> *) a. Foldable t => t a -> Int
length [a]
a Int -> Int -> Bool
forall a. Ord a => a -> a -> Bool
<= Int
4096 Bool -> Bool -> Bool
&& [a] -> Int
forall a. [a] -> Int
forall (t :: * -> *) a. Foldable t => t a -> Int
length [a]
b Int -> Int -> Bool
forall a. Ord a => a -> a -> Bool
<= Int
4096) String
"Jaccard reference budget exceeded"
    let x :: [a]
x = [a] -> [a]
forall a. Eq a => [a] -> [a]
nub [a]
a; y :: [a]
y = [a] -> [a]
forall a. Eq a => [a] -> [a]
nub [a]
b; union :: [a]
union = [a] -> [a]
forall a. Eq a => [a] -> [a]
nub ([a]
x [a] -> [a] -> [a]
forall a. [a] -> [a] -> [a]
++ [a]
y); common :: [a]
common = (a -> Bool) -> [a] -> [a]
forall a. (a -> Bool) -> [a] -> [a]
filter (a -> [a] -> Bool
forall a. Eq a => a -> [a] -> Bool
forall (t :: * -> *) a. (Foldable t, Eq a) => a -> t a -> Bool
`elem` [a]
y) [a]
x
    Double -> Either MathError Double
forall a b. b -> Either a b
Right (if [a] -> Bool
forall a. [a] -> Bool
forall (t :: * -> *) a. Foldable t => t a -> Bool
null [a]
union then Double
0 else Double
1 Double -> Double -> Double
forall a. Num a => a -> a -> a
- Int -> Double
forall a b. (Integral a, Num b) => a -> b
fromIntegral ([a] -> Int
forall a. [a] -> Int
forall (t :: * -> *) a. Foldable t => t a -> Int
length [a]
common) Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Int -> Double
forall a b. (Integral a, Num b) => a -> b
fromIntegral ([a] -> Int
forall a. [a] -> Int
forall (t :: * -> *) a. Foldable t => t a -> Int
length [a]
union))

-- | Symmetric Hausdorff distance on two nonempty finite clouds, at most 1024 each.
hausdorffDistance :: [PointND] -> [PointND] -> Either MathError Double
hausdorffDistance :: [PointND] -> [PointND] -> Either MathError Double
hausdorffDistance [PointND]
a [PointND]
b = do
    Bool -> String -> Either MathError ()
require
        (Bool -> Bool
not ([PointND] -> Bool
forall a. [a] -> Bool
forall (t :: * -> *) a. Foldable t => t a -> Bool
null [PointND]
a Bool -> Bool -> Bool
|| [PointND] -> Bool
forall a. [a] -> Bool
forall (t :: * -> *) a. Foldable t => t a -> Bool
null [PointND]
b) Bool -> Bool -> Bool
&& [PointND] -> Int
forall a. [a] -> Int
forall (t :: * -> *) a. Foldable t => t a -> Int
length [PointND]
a Int -> Int -> Bool
forall a. Ord a => a -> a -> Bool
<= Int
1024 Bool -> Bool -> Bool
&& [PointND] -> Int
forall a. [a] -> Int
forall (t :: * -> *) a. Foldable t => t a -> Int
length [PointND]
b Int -> Int -> Bool
forall a. Ord a => a -> a -> Bool
<= Int
1024)
        String
"Hausdorff empty cloud/budget"
    rows <- (PointND -> Either MathError [Double])
-> [PointND] -> Either MathError [[Double]]
forall (t :: * -> *) (m :: * -> *) a b.
(Traversable t, Monad m) =>
(a -> m b) -> t a -> m (t b)
forall (m :: * -> *) a b. Monad m => (a -> m b) -> [a] -> m [b]
mapM (\PointND
x -> (PointND -> Either MathError Double)
-> [PointND] -> Either MathError [Double]
forall (t :: * -> *) (m :: * -> *) a b.
(Traversable t, Monad m) =>
(a -> m b) -> t a -> m (t b)
forall (m :: * -> *) a b. Monad m => (a -> m b) -> [a] -> m [b]
mapM (Metric -> PointND -> PointND -> Either MathError Double
distance Metric
Euclidean PointND
x) [PointND]
b) [PointND]
a
    Right
        (max (maximum (map minimum rows)) (maximum [minimum [r !! j | r <- rows] | j <- [0 .. length b - 1]]))

{- | Mahalanobis distance using a checked symmetric positive-definite covariance.
A Cholesky triangular solve avoids an explicit inverse.
-}
mahalanobisDistance :: [[Double]] -> PointND -> PointND -> Either MathError Double
mahalanobisDistance :: [[Double]] -> PointND -> PointND -> Either MathError Double
mahalanobisDistance [[Double]]
covariance PointND
a PointND
b = do
    PointND -> PointND -> Either MathError ()
checkPair PointND
a PointND
b
    let n :: Int
n = [Double] -> Int
forall a. [a] -> Int
forall (t :: * -> *) a. Foldable t => t a -> Int
length (PointND -> [Double]
coordsND PointND
a)
    Bool -> String -> Either MathError ()
require
        ([[Double]] -> Int
forall a. [a] -> Int
forall (t :: * -> *) a. Foldable t => t a -> Int
length [[Double]]
covariance Int -> Int -> Bool
forall a. Eq a => a -> a -> Bool
== Int
n Bool -> Bool -> Bool
&& ([Double] -> Bool) -> [[Double]] -> Bool
forall (t :: * -> *) a. Foldable t => (a -> Bool) -> t a -> Bool
all ((Int -> Int -> Bool
forall a. Eq a => a -> a -> Bool
== Int
n) (Int -> Bool) -> ([Double] -> Int) -> [Double] -> Bool
forall b c a. (b -> c) -> (a -> b) -> a -> c
. [Double] -> Int
forall a. [a] -> Int
forall (t :: * -> *) a. Foldable t => t a -> Int
length) [[Double]]
covariance Bool -> Bool -> Bool
&& (Double -> Bool) -> [Double] -> Bool
forall (t :: * -> *) a. Foldable t => (a -> Bool) -> t a -> Bool
all Double -> Bool
finite ([[Double]] -> [Double]
forall (t :: * -> *) a. Foldable t => t [a] -> [a]
concat [[Double]]
covariance))
        String
"invalid covariance shape/entries"
    Bool -> String -> Either MathError ()
require
        ( [Bool] -> Bool
forall (t :: * -> *). Foldable t => t Bool -> Bool
and
            [ Double -> Double
forall a. Num a => a -> a
abs ([[Double]]
covariance [[Double]] -> Int -> [Double]
forall a. HasCallStack => [a] -> Int -> a
!! Int
i [Double] -> Int -> Double
forall a. HasCallStack => [a] -> Int -> a
!! Int
j Double -> Double -> Double
forall a. Num a => a -> a -> a
- [[Double]]
covariance [[Double]] -> Int -> [Double]
forall a. HasCallStack => [a] -> Int -> a
!! Int
j [Double] -> Int -> Double
forall a. HasCallStack => [a] -> Int -> a
!! Int
i) Double -> Double -> Bool
forall a. Ord a => a -> a -> Bool
<= Double
1e-12 Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double -> Double -> Double
forall a. Ord a => a -> a -> a
max Double
1 (Double -> Double
forall a. Num a => a -> a
abs ([[Double]]
covariance [[Double]] -> Int -> [Double]
forall a. HasCallStack => [a] -> Int -> a
!! Int
i [Double] -> Int -> Double
forall a. HasCallStack => [a] -> Int -> a
!! Int
j))
            | Int
i <- [Int
0 .. Int
n Int -> Int -> Int
forall a. Num a => a -> a -> a
- Int
1]
            , Int
j <- [Int
0 .. Int
n Int -> Int -> Int
forall a. Num a => a -> a -> a
- Int
1]
            ]
        )
        String
"covariance is not symmetric"
    l <- Int -> [[Double]] -> Int -> Either MathError [[Double]]
build Int
n [] Int
0
    let delta = [Double] -> [Double] -> [Double]
sub (PointND -> [Double]
coordsND PointND
a) (PointND -> [Double]
coordsND PointND
b)
        solve [Double]
ys Int
i
            | Int
i Int -> Int -> Bool
forall a. Eq a => a -> a -> Bool
== Int
n = [Double]
ys
            | Bool
otherwise =
                [Double] -> Int -> [Double]
solve
                    ([Double]
ys [Double] -> [Double] -> [Double]
forall a. [a] -> [a] -> [a]
++ [([Double]
delta [Double] -> Int -> Double
forall a. HasCallStack => [a] -> Int -> a
!! Int
i Double -> Double -> Double
forall a. Num a => a -> a -> a
- [Double] -> Double
forall a. Num a => [a] -> a
forall (t :: * -> *) a. (Foldable t, Num a) => t a -> a
sum [[[Double]]
l [[Double]] -> Int -> [Double]
forall a. HasCallStack => [a] -> Int -> a
!! Int
i [Double] -> Int -> Double
forall a. HasCallStack => [a] -> Int -> a
!! Int
j Double -> Double -> Double
forall a. Num a => a -> a -> a
* [Double]
ys [Double] -> Int -> Double
forall a. HasCallStack => [a] -> Int -> a
!! Int
j | Int
j <- [Int
0 .. Int
i Int -> Int -> Int
forall a. Num a => a -> a -> a
- Int
1]]) Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ ([[Double]]
l [[Double]] -> Int -> [Double]
forall a. HasCallStack => [a] -> Int -> a
!! Int
i [Double] -> Int -> Double
forall a. HasCallStack => [a] -> Int -> a
!! Int
i)])
                    (Int
i Int -> Int -> Int
forall a. Num a => a -> a -> a
+ Int
1)
        answer = [Double] -> Double
norm ([Double] -> Int -> [Double]
solve [] Int
0)
    if finite answer then Right answer else Left NonFiniteResult
  where
    build :: Int -> [[Double]] -> Int -> Either MathError [[Double]]
build Int
n [[Double]]
rows Int
i
        | Int
i Int -> Int -> Bool
forall a. Eq a => a -> a -> Bool
== Int
n = [[Double]] -> Either MathError [[Double]]
forall a b. b -> Either a b
Right [[Double]]
rows
        | Bool
otherwise = do
            row <- [[Double]] -> Int -> [Double] -> Int -> Either MathError [Double]
columns [[Double]]
rows Int
i [] Int
0
            build n (rows ++ [row ++ replicate (n - i - 1) 0]) (i + 1)
    columns :: [[Double]] -> Int -> [Double] -> Int -> Either MathError [Double]
columns [[Double]]
rows Int
i [Double]
vals Int
j
        | Int
j Int -> Int -> Bool
forall a. Ord a => a -> a -> Bool
> Int
i = [Double] -> Either MathError [Double]
forall a b. b -> Either a b
Right [Double]
vals
        | Bool
otherwise =
            let s :: Double
s =
                    [[Double]]
covariance [[Double]] -> Int -> [Double]
forall a. HasCallStack => [a] -> Int -> a
!! Int
i [Double] -> Int -> Double
forall a. HasCallStack => [a] -> Int -> a
!! Int
j
                        Double -> Double -> Double
forall a. Num a => a -> a -> a
- [Double] -> Double
forall a. Num a => [a] -> a
forall (t :: * -> *) a. (Foldable t, Num a) => t a -> a
sum [[Double]
vals [Double] -> Int -> Double
forall a. HasCallStack => [a] -> Int -> a
!! Int
k Double -> Double -> Double
forall a. Num a => a -> a -> a
* (if Int
i Int -> Int -> Bool
forall a. Eq a => a -> a -> Bool
== Int
j then [Double]
vals [Double] -> Int -> Double
forall a. HasCallStack => [a] -> Int -> a
!! Int
k else [[Double]]
rows [[Double]] -> Int -> [Double]
forall a. HasCallStack => [a] -> Int -> a
!! Int
j [Double] -> Int -> Double
forall a. HasCallStack => [a] -> Int -> a
!! Int
k) | Int
k <- [Int
0 .. Int
j Int -> Int -> Int
forall a. Num a => a -> a -> a
- Int
1]]
             in if Int
i Int -> Int -> Bool
forall a. Eq a => a -> a -> Bool
== Int
j
                    then
                        if Double
s Double -> Double -> Bool
forall a. Ord a => a -> a -> Bool
<= Double
0 Bool -> Bool -> Bool
|| Bool -> Bool
not (Double -> Bool
finite Double
s)
                            then MathError -> Either MathError [Double]
forall a b. a -> Either a b
Left (Int -> MathError
NotPositiveDefinite Int
i)
                            else [[Double]] -> Int -> [Double] -> Int -> Either MathError [Double]
columns [[Double]]
rows Int
i ([Double]
vals [Double] -> [Double] -> [Double]
forall a. [a] -> [a] -> [a]
++ [Double -> Double
forall a. Floating a => a -> a
sqrt Double
s]) (Int
j Int -> Int -> Int
forall a. Num a => a -> a -> a
+ Int
1)
                    else [[Double]] -> Int -> [Double] -> Int -> Either MathError [Double]
columns [[Double]]
rows Int
i ([Double]
vals [Double] -> [Double] -> [Double]
forall a. [a] -> [a] -> [a]
++ [Double
s Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ ([[Double]]
rows [[Double]] -> Int -> [Double]
forall a. HasCallStack => [a] -> Int -> a
!! Int
j [Double] -> Int -> Double
forall a. HasCallStack => [a] -> Int -> a
!! Int
j)]) (Int
j Int -> Int -> Int
forall a. Num a => a -> a -> a
+ Int
1)

-- | Distance to an infinite line; a zero direction is an error.
pointLineDistance :: PointND -> Line -> Either MathError Double
pointLineDistance :: PointND -> Line -> Either MathError Double
pointLineDistance PointND
p (Line PointND
a PointND
direction) = do
    PointND -> PointND -> Either MathError ()
checkPair PointND
p PointND
a
    PointND -> PointND -> Either MathError ()
checkPair PointND
p PointND
direction
    let directionValues :: [Double]
directionValues = PointND -> [Double]
coordsND PointND
direction; q :: [Double]
q = [Double] -> [Double] -> [Double]
sub (PointND -> [Double]
coordsND PointND
p) (PointND -> [Double]
coordsND PointND
a); size :: Double
size = [Double] -> Double
norm [Double]
directionValues
    Bool -> String -> Either MathError ()
require (Double
size Double -> Double -> Bool
forall a. Ord a => a -> a -> Bool
> Double
0) String
"zero line direction"
    let unit :: [Double]
unit = (Double -> Double) -> [Double] -> [Double]
forall a b. (a -> b) -> [a] -> [b]
map (Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
size) [Double]
directionValues
    Double -> Either MathError Double
forall a b. b -> Either a b
Right ([Double] -> Double
norm ([Double] -> [Double] -> [Double]
sub [Double]
q (Double -> [Double] -> [Double]
scale ([Double] -> [Double] -> Double
dot [Double]
q [Double]
unit) [Double]
unit)))

-- | Distance to a closed segment, including a zero-length segment.
pointSegmentDistance :: PointND -> Segment -> Either MathError Double
pointSegmentDistance :: PointND -> Segment -> Either MathError Double
pointSegmentDistance PointND
p (Segment PointND
a PointND
b) = do
    PointND -> PointND -> Either MathError ()
checkPair PointND
p PointND
a
    PointND -> PointND -> Either MathError ()
checkPair PointND
a PointND
b
    let d :: [Double]
d = [Double] -> [Double] -> [Double]
sub (PointND -> [Double]
coordsND PointND
b) (PointND -> [Double]
coordsND PointND
a)
        q :: [Double]
q = [Double] -> [Double] -> [Double]
sub (PointND -> [Double]
coordsND PointND
p) (PointND -> [Double]
coordsND PointND
a)
        size :: Double
size = [Double] -> Double
norm [Double]
d
        unit :: [Double]
unit = if Double
size Double -> Double -> Bool
forall a. Eq a => a -> a -> Bool
== Double
0 then Int -> Double -> [Double]
forall a. Int -> a -> [a]
replicate ([Double] -> Int
forall a. [a] -> Int
forall (t :: * -> *) a. Foldable t => t a -> Int
length [Double]
d) Double
0 else (Double -> Double) -> [Double] -> [Double]
forall a b. (a -> b) -> [a] -> [b]
map (Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
size) [Double]
d
        t :: Double
t = Double -> Double -> Double
forall a. Ord a => a -> a -> a
max Double
0 (Double -> Double -> Double
forall a. Ord a => a -> a -> a
min Double
size ([Double] -> [Double] -> Double
dot [Double]
q [Double]
unit))
    Double -> Either MathError Double
forall a b. b -> Either a b
Right ([Double] -> Double
norm ([Double] -> [Double] -> [Double]
sub [Double]
q (Double -> [Double] -> [Double]
scale Double
t [Double]
unit)))

checkPlane :: Plane -> Either MathError ()
checkPlane :: Plane -> Either MathError ()
checkPlane (Plane Point3
n Double
h) = Point3 -> Either MathError ()
check3 Point3
n Either MathError () -> Either MathError () -> Either MathError ()
forall a b.
Either MathError a -> Either MathError b -> Either MathError b
forall (m :: * -> *) a b. Monad m => m a -> m b -> m b
>> Bool -> String -> Either MathError ()
require (Double -> Bool
coord Double
h Bool -> Bool -> Bool
&& [Double] -> Double
norm (Point3 -> [Double]
coords3 Point3
n) Double -> Double -> Bool
forall a. Ord a => a -> a -> Bool
> Double
0) String
"invalid plane normal/offset"

-- | Unsigned perpendicular plane distance.
pointPlaneDistance :: Point3 -> Plane -> Either MathError Double
pointPlaneDistance :: Point3 -> Plane -> Either MathError Double
pointPlaneDistance Point3
p plane :: Plane
plane@(Plane Point3
n Double
h) = do
    Point3 -> Either MathError ()
check3 Point3
p
    Plane -> Either MathError ()
checkPlane Plane
plane
    let value :: Double
value = Rational -> Double
forall a. Fractional a => Rational -> a
fromRational (Rational -> Rational
forall a. Num a => a -> a
abs ([Rational] -> [Rational] -> Rational
rDot (Point3 -> [Rational]
r3 Point3
p) (Point3 -> [Rational]
r3 Point3
n) Rational -> Rational -> Rational
forall a. Num a => a -> a -> a
- Double -> Rational
forall a. Real a => a -> Rational
toRational Double
h) Rational -> Rational -> Rational
forall a. Fractional a => a -> a -> a
/ Double -> Rational
forall a. Real a => a -> Rational
toRational ([Double] -> Double
norm (Point3 -> [Double]
coords3 Point3
n)))
    _ <- [Double] -> Either MathError [Double]
checked [Double
value]
    Right value

r3 :: Point3 -> [Rational]
r3 :: Point3 -> [Rational]
r3 = (Double -> Rational) -> [Double] -> [Rational]
forall a b. (a -> b) -> [a] -> [b]
map Double -> Rational
forall a. Real a => a -> Rational
toRational ([Double] -> [Rational])
-> (Point3 -> [Double]) -> Point3 -> [Rational]
forall b c a. (b -> c) -> (a -> b) -> a -> c
. Point3 -> [Double]
coords3
rDot :: [Rational] -> [Rational] -> Rational
rDot :: [Rational] -> [Rational] -> Rational
rDot [Rational]
a [Rational]
b = [Rational] -> Rational
forall a. Num a => [a] -> a
forall (t :: * -> *) a. (Foldable t, Num a) => t a -> a
sum ((Rational -> Rational -> Rational)
-> [Rational] -> [Rational] -> [Rational]
forall a b c. (a -> b -> c) -> [a] -> [b] -> [c]
zipWith Rational -> Rational -> Rational
forall a. Num a => a -> a -> a
(*) [Rational]
a [Rational]
b)
rCross :: [Rational] -> [Rational] -> [Rational]
rCross :: [Rational] -> [Rational] -> [Rational]
rCross [Rational
a, Rational
b, Rational
c] [Rational
x, Rational
y, Rational
z] = [Rational
b Rational -> Rational -> Rational
forall a. Num a => a -> a -> a
* Rational
z Rational -> Rational -> Rational
forall a. Num a => a -> a -> a
- Rational
c Rational -> Rational -> Rational
forall a. Num a => a -> a -> a
* Rational
y, Rational
c Rational -> Rational -> Rational
forall a. Num a => a -> a -> a
* Rational
x Rational -> Rational -> Rational
forall a. Num a => a -> a -> a
- Rational
a Rational -> Rational -> Rational
forall a. Num a => a -> a -> a
* Rational
z, Rational
a Rational -> Rational -> Rational
forall a. Num a => a -> a -> a
* Rational
y Rational -> Rational -> Rational
forall a. Num a => a -> a -> a
- Rational
b Rational -> Rational -> Rational
forall a. Num a => a -> a -> a
* Rational
x]
rCross [Rational]
_ [Rational]
_ = []

-- | Exact orientation of the represented finite Double coordinates.
orientation :: Point2 -> Point2 -> Point2 -> Either MathError Orientation
orientation :: Point2 -> Point2 -> Point2 -> Either MathError Orientation
orientation Point2
a Point2
b Point2
c = do
    (Point2 -> Either MathError ()) -> [Point2] -> Either MathError ()
forall (t :: * -> *) (m :: * -> *) a b.
(Foldable t, Monad m) =>
(a -> m b) -> t a -> m ()
mapM_ Point2 -> Either MathError ()
check2 [Point2
a, Point2
b, Point2
c]
    Orientation -> Either MathError Orientation
forall a b. b -> Either a b
Right (case Point2 -> Point2 -> Point2 -> Ordering
orient Point2
a Point2
b Point2
c of Ordering
LT -> Orientation
Clockwise; Ordering
EQ -> Orientation
Collinear; Ordering
GT -> Orientation
Counterclockwise)

{- | Exact in-circle sign, orientation-independent: positive means inside.
A collinear defining triangle is rejected.
-}
incircle :: Point2 -> Point2 -> Point2 -> Point2 -> Either MathError Ordering
incircle :: Point2 -> Point2 -> Point2 -> Point2 -> Either MathError Ordering
incircle Point2
a Point2
b Point2
c Point2
p = do
    (Point2 -> Either MathError ()) -> [Point2] -> Either MathError ()
forall (t :: * -> *) (m :: * -> *) a b.
(Foldable t, Monad m) =>
(a -> m b) -> t a -> m ()
mapM_ Point2 -> Either MathError ()
check2 [Point2
a, Point2
b, Point2
c, Point2
p]
    Bool -> String -> Either MathError ()
require (Point2 -> Point2 -> Point2 -> Ordering
orient Point2
a Point2
b Point2
c Ordering -> Ordering -> Bool
forall a. Eq a => a -> a -> Bool
/= Ordering
EQ) String
"incircle triangle is collinear"
    let rational :: Point2 -> (Rational, Rational)
rational (P2 Double
x Double
y) = (Double -> Rational
forall a. Real a => a -> Rational
toRational Double
x, Double -> Rational
forall a. Real a => a -> Rational
toRational Double
y)
        delta :: Point2 -> Point2 -> (Rational, Rational)
delta Point2
u Point2
v = let (Rational
x, Rational
y) = Point2 -> (Rational, Rational)
rational Point2
u; (Rational
z, Rational
w) = Point2 -> (Rational, Rational)
rational Point2
v in (Rational
x Rational -> Rational -> Rational
forall a. Num a => a -> a -> a
- Rational
z, Rational
y Rational -> Rational -> Rational
forall a. Num a => a -> a -> a
- Rational
w)
        (Rational
ax, Rational
ay) = Point2 -> Point2 -> (Rational, Rational)
delta Point2
a Point2
p
        (Rational
bx, Rational
by) = Point2 -> Point2 -> (Rational, Rational)
delta Point2
b Point2
p
        (Rational
cx, Rational
cy) = Point2 -> Point2 -> (Rational, Rational)
delta Point2
c Point2
p
        determinant :: Rational
determinant =
            (Rational
ax Rational -> Rational -> Rational
forall a. Num a => a -> a -> a
* Rational
ax Rational -> Rational -> Rational
forall a. Num a => a -> a -> a
+ Rational
ay Rational -> Rational -> Rational
forall a. Num a => a -> a -> a
* Rational
ay) Rational -> Rational -> Rational
forall a. Num a => a -> a -> a
* (Rational
bx Rational -> Rational -> Rational
forall a. Num a => a -> a -> a
* Rational
cy Rational -> Rational -> Rational
forall a. Num a => a -> a -> a
- Rational
by Rational -> Rational -> Rational
forall a. Num a => a -> a -> a
* Rational
cx)
                Rational -> Rational -> Rational
forall a. Num a => a -> a -> a
- (Rational
bx Rational -> Rational -> Rational
forall a. Num a => a -> a -> a
* Rational
bx Rational -> Rational -> Rational
forall a. Num a => a -> a -> a
+ Rational
by Rational -> Rational -> Rational
forall a. Num a => a -> a -> a
* Rational
by) Rational -> Rational -> Rational
forall a. Num a => a -> a -> a
* (Rational
ax Rational -> Rational -> Rational
forall a. Num a => a -> a -> a
* Rational
cy Rational -> Rational -> Rational
forall a. Num a => a -> a -> a
- Rational
ay Rational -> Rational -> Rational
forall a. Num a => a -> a -> a
* Rational
cx)
                Rational -> Rational -> Rational
forall a. Num a => a -> a -> a
+ (Rational
cx Rational -> Rational -> Rational
forall a. Num a => a -> a -> a
* Rational
cx Rational -> Rational -> Rational
forall a. Num a => a -> a -> a
+ Rational
cy Rational -> Rational -> Rational
forall a. Num a => a -> a -> a
* Rational
cy) Rational -> Rational -> Rational
forall a. Num a => a -> a -> a
* (Rational
ax Rational -> Rational -> Rational
forall a. Num a => a -> a -> a
* Rational
by Rational -> Rational -> Rational
forall a. Num a => a -> a -> a
- Rational
ay Rational -> Rational -> Rational
forall a. Num a => a -> a -> a
* Rational
bx)
    Ordering -> Either MathError Ordering
forall a b. b -> Either a b
Right (Rational -> Rational -> Ordering
forall a. Ord a => a -> a -> Ordering
compare (if Point2 -> Point2 -> Point2 -> Ordering
orient Point2
a Point2
b Point2
c Ordering -> Ordering -> Bool
forall a. Eq a => a -> a -> Bool
== Ordering
GT then Rational
determinant else -Rational
determinant) Rational
0)

-- | Simple polygon position, with boundary separate from interior.
polygonLocation :: Polygon -> Point2 -> Either MathError Location
polygonLocation :: Polygon -> Point2 -> Either MathError Location
polygonLocation Polygon
poly Point2
p = do ps <- Polygon -> Either MathError [Point2]
checkPolygon Polygon
poly; check2 p; Right (locateUnchecked ps p)

-- | Triangle position using exact orientation signs.
triangleLocation :: Triangle -> Point2 -> Either MathError Location
triangleLocation :: Triangle -> Point2 -> Either MathError Location
triangleLocation (Triangle Point2
a Point2
b Point2
c) Point2
p = do
    (Point2 -> Either MathError ()) -> [Point2] -> Either MathError ()
forall (t :: * -> *) (m :: * -> *) a b.
(Foldable t, Monad m) =>
(a -> m b) -> t a -> m ()
mapM_ Point2 -> Either MathError ()
check2 [Point2
a, Point2
b, Point2
c, Point2
p]
    Bool -> String -> Either MathError ()
require (Point2 -> Point2 -> Point2 -> Ordering
orient Point2
a Point2
b Point2
c Ordering -> Ordering -> Bool
forall a. Eq a => a -> a -> Bool
/= Ordering
EQ) String
"degenerate triangle"
    let signs :: [Ordering]
signs = [Point2 -> Point2 -> Point2 -> Ordering
orient Point2
a Point2
b Point2
p, Point2 -> Point2 -> Point2 -> Ordering
orient Point2
b Point2
c Point2
p, Point2 -> Point2 -> Point2 -> Ordering
orient Point2
c Point2
a Point2
p]
    Location -> Either MathError Location
forall a b. b -> Either a b
Right
        (if Ordering
LT Ordering -> [Ordering] -> Bool
forall a. Eq a => a -> [a] -> Bool
forall (t :: * -> *) a. (Foldable t, Eq a) => a -> t a -> Bool
`elem` [Ordering]
signs Bool -> Bool -> Bool
&& Ordering
GT Ordering -> [Ordering] -> Bool
forall a. Eq a => a -> [a] -> Bool
forall (t :: * -> *) a. (Foldable t, Eq a) => a -> t a -> Bool
`elem` [Ordering]
signs then Location
Outside else if Ordering
EQ Ordering -> [Ordering] -> Bool
forall a. Eq a => a -> [a] -> Bool
forall (t :: * -> *) a. (Foldable t, Eq a) => a -> t a -> Bool
`elem` [Ordering]
signs then Location
Boundary else Location
Inside)

-- | Closed sphere containment.
sphereContains :: Sphere -> Point3 -> Either MathError Bool
sphereContains :: Sphere -> Point3 -> Either MathError Bool
sphereContains Sphere
sphere Point3
p = (Double -> Double -> Bool
forall a. Ord a => a -> a -> Bool
<= Double
0) (Double -> Bool)
-> Either MathError Double -> Either MathError Bool
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
<$> Sphere -> Point3 -> Either MathError Double
sphereSDF Sphere
sphere Point3
p

checkBox :: AABB -> Either MathError ()
checkBox :: AABB -> Either MathError ()
checkBox (AABB PointND
a PointND
b) = PointND -> PointND -> Either MathError ()
checkPair PointND
a PointND
b Either MathError () -> Either MathError () -> Either MathError ()
forall a b.
Either MathError a -> Either MathError b -> Either MathError b
forall (m :: * -> *) a b. Monad m => m a -> m b -> m b
>> Bool -> String -> Either MathError ()
require ([Bool] -> Bool
forall (t :: * -> *). Foldable t => t Bool -> Bool
and ((Double -> Double -> Bool) -> [Double] -> [Double] -> [Bool]
forall a b c. (a -> b -> c) -> [a] -> [b] -> [c]
zipWith Double -> Double -> Bool
forall a. Ord a => a -> a -> Bool
(<=) (PointND -> [Double]
coordsND PointND
a) (PointND -> [Double]
coordsND PointND
b))) String
"unordered box bounds"

-- | Closed axis-aligned box containment.
boxContains :: AABB -> PointND -> Either MathError Bool
boxContains :: AABB -> PointND -> Either MathError Bool
boxContains box :: AABB
box@(AABB PointND
a PointND
b) PointND
p =
    AABB -> Either MathError ()
checkBox AABB
box
        Either MathError () -> Either MathError () -> Either MathError ()
forall a b.
Either MathError a -> Either MathError b -> Either MathError b
forall (m :: * -> *) a b. Monad m => m a -> m b -> m b
>> PointND -> PointND -> Either MathError ()
checkPair PointND
a PointND
p
        Either MathError ()
-> Either MathError Bool -> Either MathError Bool
forall a b.
Either MathError a -> Either MathError b -> Either MathError b
forall (m :: * -> *) a b. Monad m => m a -> m b -> m b
>> Bool -> Either MathError Bool
forall a b. b -> Either a b
Right ([Bool] -> Bool
forall (t :: * -> *). Foldable t => t Bool -> Bool
and ((Double -> Double -> Double -> Bool)
-> [Double] -> [Double] -> [Double] -> [Bool]
forall a b c d. (a -> b -> c -> d) -> [a] -> [b] -> [c] -> [d]
zipWith3 (\Double
x Double
l Double
h -> Double
x Double -> Double -> Bool
forall a. Ord a => a -> a -> Bool
>= Double
l Bool -> Bool -> Bool
&& Double
x Double -> Double -> Bool
forall a. Ord a => a -> a -> Bool
<= Double
h) (PointND -> [Double]
coordsND PointND
p) (PointND -> [Double]
coordsND PointND
a) (PointND -> [Double]
coordsND PointND
b)))

-- | Closed ellipse containment after rotation to its principal axes.
ellipseContains :: Ellipse -> Point2 -> Either MathError Bool
ellipseContains :: Ellipse -> Point2 -> Either MathError Bool
ellipseContains (Ellipse Point2
c Double
a Double
b Double
theta) Point2
p = do
    (Point2 -> Either MathError ()) -> [Point2] -> Either MathError ()
forall (t :: * -> *) (m :: * -> *) a b.
(Foldable t, Monad m) =>
(a -> m b) -> t a -> m ()
mapM_ Point2 -> Either MathError ()
check2 [Point2
c, Point2
p]
    Bool -> String -> Either MathError ()
require (Double -> Bool
coord Double
a Bool -> Bool -> Bool
&& Double -> Bool
coord Double
b Bool -> Bool -> Bool
&& Double
a Double -> Double -> Bool
forall a. Ord a => a -> a -> Bool
> Double
0 Bool -> Bool -> Bool
&& Double
b Double -> Double -> Bool
forall a. Ord a => a -> a -> Bool
> Double
0 Bool -> Bool -> Bool
&& Double -> Bool
finite Double
theta) String
"invalid ellipse"
    let P2 Double
x Double
y = Point2 -> Point2 -> Point2
sub2 Point2
p Point2
c; u :: Double
u = (Double -> Double
forall a. Floating a => a -> a
cos Double
theta Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
x Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double -> Double
forall a. Floating a => a -> a
sin Double
theta Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
y) Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
a; v :: Double
v = (-Double -> Double
forall a. Floating a => a -> a
sin Double
theta Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
x Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double -> Double
forall a. Floating a => a -> a
cos Double
theta Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
y) Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
b
    Bool -> Either MathError Bool
forall a b. b -> Either a b
Right (Double
u Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
u Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
v Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
v Double -> Double -> Bool
forall a. Ord a => a -> a -> Bool
<= Double
1)

-- | Weights (a,b,c) summing to one; valid outside the triangle as well.
triangleBarycentric :: Triangle -> Point2 -> Either MathError [Double]
triangleBarycentric :: Triangle -> Point2 -> Either MathError [Double]
triangleBarycentric (Triangle Point2
a Point2
b Point2
c) Point2
p = do
    (Point2 -> Either MathError ()) -> [Point2] -> Either MathError ()
forall (t :: * -> *) (m :: * -> *) a b.
(Foldable t, Monad m) =>
(a -> m b) -> t a -> m ()
mapM_ Point2 -> Either MathError ()
check2 [Point2
a, Point2
b, Point2
c, Point2
p]
    Bool -> String -> Either MathError ()
require (Point2 -> Point2 -> Point2 -> Ordering
orient Point2
a Point2
b Point2
c Ordering -> Ordering -> Bool
forall a. Eq a => a -> a -> Bool
/= Ordering
EQ) String
"degenerate barycentric triangle"
    let d :: Rational
d = Point2 -> Point2 -> Point2 -> Rational
orientExact Point2
a Point2
b Point2
c
    [Double] -> Either MathError [Double]
checked ((Rational -> Double) -> [Rational] -> [Double]
forall a b. (a -> b) -> [a] -> [b]
map Rational -> Double
forall a. Fractional a => Rational -> a
fromRational [Point2 -> Point2 -> Point2 -> Rational
orientExact Point2
p Point2
b Point2
c Rational -> Rational -> Rational
forall a. Fractional a => a -> a -> a
/ Rational
d, Point2 -> Point2 -> Point2 -> Rational
orientExact Point2
a Point2
p Point2
c Rational -> Rational -> Rational
forall a. Fractional a => a -> a -> a
/ Rational
d, Point2 -> Point2 -> Point2 -> Rational
orientExact Point2
a Point2
b Point2
p Rational -> Rational -> Rational
forall a. Fractional a => a -> a -> a
/ Rational
d])

-- | Circumcenter, obtained from exact rational perpendicular-bisector equations.
circumcenter :: Triangle -> Either MathError Point2
circumcenter :: Triangle -> Either MathError Point2
circumcenter (Triangle Point2
a Point2
b Point2
c) = do
    (Point2 -> Either MathError ()) -> [Point2] -> Either MathError ()
forall (t :: * -> *) (m :: * -> *) a b.
(Foldable t, Monad m) =>
(a -> m b) -> t a -> m ()
mapM_ Point2 -> Either MathError ()
check2 [Point2
a, Point2
b, Point2
c]
    Bool -> String -> Either MathError ()
require (Point2 -> Point2 -> Point2 -> Ordering
orient Point2
a Point2
b Point2
c Ordering -> Ordering -> Bool
forall a. Eq a => a -> a -> Bool
/= Ordering
EQ) String
"collinear circumcenter"
    let rational :: Point2 -> (Rational, Rational)
rational (P2 Double
x Double
y) = (Double -> Rational
forall a. Real a => a -> Rational
toRational Double
x, Double -> Rational
forall a. Real a => a -> Rational
toRational Double
y)
        (Rational
ax, Rational
ay) = Point2 -> (Rational, Rational)
rational Point2
a
        (Rational
bx, Rational
by) = Point2 -> (Rational, Rational)
rational Point2
b
        (Rational
cx, Rational
cy) = Point2 -> (Rational, Rational)
rational Point2
c
        u :: Rational
u = Rational
bx Rational -> Rational -> Rational
forall a. Num a => a -> a -> a
- Rational
ax
        v :: Rational
v = Rational
by Rational -> Rational -> Rational
forall a. Num a => a -> a -> a
- Rational
ay
        s :: Rational
s = Rational
cx Rational -> Rational -> Rational
forall a. Num a => a -> a -> a
- Rational
ax
        t :: Rational
t = Rational
cy Rational -> Rational -> Rational
forall a. Num a => a -> a -> a
- Rational
ay
        h :: Rational
h = (Rational
u Rational -> Rational -> Rational
forall a. Num a => a -> a -> a
* Rational
u Rational -> Rational -> Rational
forall a. Num a => a -> a -> a
+ Rational
v Rational -> Rational -> Rational
forall a. Num a => a -> a -> a
* Rational
v) Rational -> Rational -> Rational
forall a. Fractional a => a -> a -> a
/ Rational
2
        k :: Rational
k = (Rational
s Rational -> Rational -> Rational
forall a. Num a => a -> a -> a
* Rational
s Rational -> Rational -> Rational
forall a. Num a => a -> a -> a
+ Rational
t Rational -> Rational -> Rational
forall a. Num a => a -> a -> a
* Rational
t) Rational -> Rational -> Rational
forall a. Fractional a => a -> a -> a
/ Rational
2
        d :: Rational
d = Rational
u Rational -> Rational -> Rational
forall a. Num a => a -> a -> a
* Rational
t Rational -> Rational -> Rational
forall a. Num a => a -> a -> a
- Rational
v Rational -> Rational -> Rational
forall a. Num a => a -> a -> a
* Rational
s
    Double -> Double -> Either MathError Point2
point2 (Rational -> Double
forall a. Fractional a => Rational -> a
fromRational (Rational
ax Rational -> Rational -> Rational
forall a. Num a => a -> a -> a
+ (Rational
h Rational -> Rational -> Rational
forall a. Num a => a -> a -> a
* Rational
t Rational -> Rational -> Rational
forall a. Num a => a -> a -> a
- Rational
v Rational -> Rational -> Rational
forall a. Num a => a -> a -> a
* Rational
k) Rational -> Rational -> Rational
forall a. Fractional a => a -> a -> a
/ Rational
d)) (Rational -> Double
forall a. Fractional a => Rational -> a
fromRational (Rational
ay Rational -> Rational -> Rational
forall a. Num a => a -> a -> a
+ (Rational
u Rational -> Rational -> Rational
forall a. Num a => a -> a -> a
* Rational
k Rational -> Rational -> Rational
forall a. Num a => a -> a -> a
- Rational
h Rational -> Rational -> Rational
forall a. Num a => a -> a -> a
* Rational
s) Rational -> Rational -> Rational
forall a. Fractional a => a -> a -> a
/ Rational
d))

-- | Incenter weighted by opposite side lengths.
incenter :: Triangle -> Either MathError Point2
incenter :: Triangle -> Either MathError Point2
incenter (Triangle Point2
a Point2
b Point2
c) = do
    (Point2 -> Either MathError ()) -> [Point2] -> Either MathError ()
forall (t :: * -> *) (m :: * -> *) a b.
(Foldable t, Monad m) =>
(a -> m b) -> t a -> m ()
mapM_ Point2 -> Either MathError ()
check2 [Point2
a, Point2
b, Point2
c]
    Bool -> String -> Either MathError ()
require (Point2 -> Point2 -> Point2 -> Ordering
orient Point2
a Point2
b Point2
c Ordering -> Ordering -> Bool
forall a. Eq a => a -> a -> Bool
/= Ordering
EQ) String
"collinear incenter"
    let x :: Double
x = Point2 -> Double
norm2 (Point2 -> Point2 -> Point2
sub2 Point2
b Point2
c)
        y :: Double
y = Point2 -> Double
norm2 (Point2 -> Point2 -> Point2
sub2 Point2
a Point2
c)
        z :: Double
z = Point2 -> Double
norm2 (Point2 -> Point2 -> Point2
sub2 Point2
a Point2
b)
        total :: Double
total = Double
x Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
y Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
z
        P2 Double
u Double
v = Point2 -> Point2 -> Point2
add2 (Double -> Point2 -> Point2
scale2 (Double
x Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
total) Point2
a) (Point2 -> Point2 -> Point2
add2 (Double -> Point2 -> Point2
scale2 (Double
y Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
total) Point2
b) (Double -> Point2 -> Point2
scale2 (Double
z Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
total) Point2
c))
    Double -> Double -> Either MathError Point2
point2 Double
u Double
v

-- | Two-dimensional intersection, with a coincident line or overlapping segment explicit.
data Intersection2
    = NoIntersection
    | IntersectionPoint Point2
    | OverlapSegment Point2 Point2
    | CoincidentLine
    deriving (Intersection2 -> Intersection2 -> Bool
(Intersection2 -> Intersection2 -> Bool)
-> (Intersection2 -> Intersection2 -> Bool) -> Eq Intersection2
forall a. (a -> a -> Bool) -> (a -> a -> Bool) -> Eq a
$c== :: Intersection2 -> Intersection2 -> Bool
== :: Intersection2 -> Intersection2 -> Bool
$c/= :: Intersection2 -> Intersection2 -> Bool
/= :: Intersection2 -> Intersection2 -> Bool
Eq, Int -> Intersection2 -> ShowS
[Intersection2] -> ShowS
Intersection2 -> String
(Int -> Intersection2 -> ShowS)
-> (Intersection2 -> String)
-> ([Intersection2] -> ShowS)
-> Show Intersection2
forall a.
(Int -> a -> ShowS) -> (a -> String) -> ([a] -> ShowS) -> Show a
$cshowsPrec :: Int -> Intersection2 -> ShowS
showsPrec :: Int -> Intersection2 -> ShowS
$cshow :: Intersection2 -> String
show :: Intersection2 -> String
$cshowList :: [Intersection2] -> ShowS
showList :: [Intersection2] -> ShowS
Show)

-- | Infinite-line intersection from origins and nonzero direction vectors.
lineIntersection :: Point2 -> Point2 -> Point2 -> Point2 -> Either MathError Intersection2
lineIntersection :: Point2
-> Point2 -> Point2 -> Point2 -> Either MathError Intersection2
lineIntersection Point2
a Point2
u Point2
b Point2
v = do
    (Point2 -> Either MathError ()) -> [Point2] -> Either MathError ()
forall (t :: * -> *) (m :: * -> *) a b.
(Foldable t, Monad m) =>
(a -> m b) -> t a -> m ()
mapM_ Point2 -> Either MathError ()
check2 [Point2
a, Point2
u, Point2
b, Point2
v]
    Bool -> String -> Either MathError ()
require (Point2
u Point2 -> Point2 -> Bool
forall a. Eq a => a -> a -> Bool
/= Double -> Double -> Point2
P2 Double
0 Double
0 Bool -> Bool -> Bool
&& Point2
v Point2 -> Point2 -> Bool
forall a. Eq a => a -> a -> Bool
/= Double -> Double -> Point2
P2 Double
0 Double
0) String
"zero line direction"
    let rational :: Point2 -> (Rational, Rational)
rational (P2 Double
x Double
y) = (Double -> Rational
forall a. Real a => a -> Rational
toRational Double
x, Double -> Rational
forall a. Real a => a -> Rational
toRational Double
y)
        (Rational
ax, Rational
ay) = Point2 -> (Rational, Rational)
rational Point2
a
        (Rational
bx, Rational
by) = Point2 -> (Rational, Rational)
rational Point2
b
        (Rational
ux, Rational
uy) = Point2 -> (Rational, Rational)
rational Point2
u
        (Rational
vx, Rational
vy) = Point2 -> (Rational, Rational)
rational Point2
v
        dx :: Rational
dx = Rational
bx Rational -> Rational -> Rational
forall a. Num a => a -> a -> a
- Rational
ax
        dy :: Rational
dy = Rational
by Rational -> Rational -> Rational
forall a. Num a => a -> a -> a
- Rational
ay
        denominator :: Rational
denominator = Rational
ux Rational -> Rational -> Rational
forall a. Num a => a -> a -> a
* Rational
vy Rational -> Rational -> Rational
forall a. Num a => a -> a -> a
- Rational
uy Rational -> Rational -> Rational
forall a. Num a => a -> a -> a
* Rational
vx
    if Rational
denominator Rational -> Rational -> Bool
forall a. Eq a => a -> a -> Bool
== Rational
0
        then Intersection2 -> Either MathError Intersection2
forall a b. b -> Either a b
Right (if Rational
ux Rational -> Rational -> Rational
forall a. Num a => a -> a -> a
* Rational
dy Rational -> Rational -> Rational
forall a. Num a => a -> a -> a
- Rational
uy Rational -> Rational -> Rational
forall a. Num a => a -> a -> a
* Rational
dx Rational -> Rational -> Bool
forall a. Eq a => a -> a -> Bool
== Rational
0 then Intersection2
CoincidentLine else Intersection2
NoIntersection)
        else
            let t :: Rational
t = (Rational
dx Rational -> Rational -> Rational
forall a. Num a => a -> a -> a
* Rational
vy Rational -> Rational -> Rational
forall a. Num a => a -> a -> a
- Rational
dy Rational -> Rational -> Rational
forall a. Num a => a -> a -> a
* Rational
vx) Rational -> Rational -> Rational
forall a. Fractional a => a -> a -> a
/ Rational
denominator
             in Point2 -> Intersection2
IntersectionPoint (Point2 -> Intersection2)
-> Either MathError Point2 -> Either MathError Intersection2
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
<$> Double -> Double -> Either MathError Point2
point2 (Rational -> Double
forall a. Fractional a => Rational -> a
fromRational (Rational
ax Rational -> Rational -> Rational
forall a. Num a => a -> a -> a
+ Rational
t Rational -> Rational -> Rational
forall a. Num a => a -> a -> a
* Rational
ux)) (Rational -> Double
forall a. Fractional a => Rational -> a
fromRational (Rational
ay Rational -> Rational -> Rational
forall a. Num a => a -> a -> a
+ Rational
t Rational -> Rational -> Rational
forall a. Num a => a -> a -> a
* Rational
uy))

-- | Closed segment intersection with exact degeneracy predicates.
segmentIntersection :: Point2 -> Point2 -> Point2 -> Point2 -> Either MathError Intersection2
segmentIntersection :: Point2
-> Point2 -> Point2 -> Point2 -> Either MathError Intersection2
segmentIntersection Point2
a Point2
b Point2
c Point2
d = do
    (Point2 -> Either MathError ()) -> [Point2] -> Either MathError ()
forall (t :: * -> *) (m :: * -> *) a b.
(Foldable t, Monad m) =>
(a -> m b) -> t a -> m ()
mapM_ Point2 -> Either MathError ()
check2 [Point2
a, Point2
b, Point2
c, Point2
d]
    if Point2
a Point2 -> Point2 -> Bool
forall a. Eq a => a -> a -> Bool
== Point2
b
        then Intersection2 -> Either MathError Intersection2
forall a b. b -> Either a b
Right (if Point2 -> Point2 -> Point2 -> Bool
onSegment Point2
c Point2
d Point2
a then Point2 -> Intersection2
IntersectionPoint Point2
a else Intersection2
NoIntersection)
        else
            if Point2
c Point2 -> Point2 -> Bool
forall a. Eq a => a -> a -> Bool
== Point2
d
                then Intersection2 -> Either MathError Intersection2
forall a b. b -> Either a b
Right (if Point2 -> Point2 -> Point2 -> Bool
onSegment Point2
a Point2
b Point2
c then Point2 -> Intersection2
IntersectionPoint Point2
c else Intersection2
NoIntersection)
                else
                    if Point2 -> Point2 -> Point2 -> Ordering
orient Point2
a Point2
b Point2
c Ordering -> Ordering -> Bool
forall a. Eq a => a -> a -> Bool
== Ordering
EQ Bool -> Bool -> Bool
&& Point2 -> Point2 -> Point2 -> Ordering
orient Point2
a Point2
b Point2
d Ordering -> Ordering -> Bool
forall a. Eq a => a -> a -> Bool
== Ordering
EQ
                        then
                            let pts :: [Point2]
pts = [Point2] -> [Point2]
forall a. Eq a => [a] -> [a]
nub [Point2
p | Point2
p <- [Point2
a, Point2
b, Point2
c, Point2
d], Point2 -> Point2 -> Point2 -> Bool
onSegment Point2
a Point2
b Point2
p Bool -> Bool -> Bool
&& Point2 -> Point2 -> Point2 -> Bool
onSegment Point2
c Point2
d Point2
p]
                                ordered :: (Point2, Point2)
ordered = if Point2
a Point2 -> Point2 -> Bool
forall a. Ord a => a -> a -> Bool
<= Point2
b then ([Point2] -> Point2
forall a. Ord a => [a] -> a
forall (t :: * -> *) a. (Foldable t, Ord a) => t a -> a
minimum [Point2]
pts, [Point2] -> Point2
forall a. Ord a => [a] -> a
forall (t :: * -> *) a. (Foldable t, Ord a) => t a -> a
maximum [Point2]
pts) else ([Point2] -> Point2
forall a. Ord a => [a] -> a
forall (t :: * -> *) a. (Foldable t, Ord a) => t a -> a
maximum [Point2]
pts, [Point2] -> Point2
forall a. Ord a => [a] -> a
forall (t :: * -> *) a. (Foldable t, Ord a) => t a -> a
minimum [Point2]
pts)
                             in Intersection2 -> Either MathError Intersection2
forall a b. b -> Either a b
Right
                                    ( if [Point2] -> Bool
forall a. [a] -> Bool
forall (t :: * -> *) a. Foldable t => t a -> Bool
null [Point2]
pts
                                        then Intersection2
NoIntersection
                                        else
                                            if [Point2] -> Int
forall a. [a] -> Int
forall (t :: * -> *) a. Foldable t => t a -> Int
length [Point2]
pts Int -> Int -> Bool
forall a. Eq a => a -> a -> Bool
== Int
1 then Point2 -> Intersection2
IntersectionPoint (Point2 -> [Point2] -> Point2
forall a. a -> [a] -> a
firstOr (Double -> Double -> Point2
P2 Double
0 Double
0) [Point2]
pts) else (Point2 -> Point2 -> Intersection2)
-> (Point2, Point2) -> Intersection2
forall a b c. (a -> b -> c) -> (a, b) -> c
uncurry Point2 -> Point2 -> Intersection2
OverlapSegment (Point2, Point2)
ordered
                                    )
                        else
                            if (Point2 -> Point2 -> Point2 -> Ordering
orient Point2
a Point2
b Point2
c Ordering -> Ordering -> Bool
forall a. Eq a => a -> a -> Bool
== Ordering
EQ Bool -> Bool -> Bool
|| Point2 -> Point2 -> Point2 -> Ordering
orient Point2
a Point2
b Point2
d Ordering -> Ordering -> Bool
forall a. Eq a => a -> a -> Bool
== Ordering
EQ Bool -> Bool -> Bool
|| Point2 -> Point2 -> Point2 -> Ordering
orient Point2
a Point2
b Point2
c Ordering -> Ordering -> Bool
forall a. Eq a => a -> a -> Bool
/= Point2 -> Point2 -> Point2 -> Ordering
orient Point2
a Point2
b Point2
d)
                                Bool -> Bool -> Bool
&& (Point2 -> Point2 -> Point2 -> Ordering
orient Point2
c Point2
d Point2
a Ordering -> Ordering -> Bool
forall a. Eq a => a -> a -> Bool
== Ordering
EQ Bool -> Bool -> Bool
|| Point2 -> Point2 -> Point2 -> Ordering
orient Point2
c Point2
d Point2
b Ordering -> Ordering -> Bool
forall a. Eq a => a -> a -> Bool
== Ordering
EQ Bool -> Bool -> Bool
|| Point2 -> Point2 -> Point2 -> Ordering
orient Point2
c Point2
d Point2
a Ordering -> Ordering -> Bool
forall a. Eq a => a -> a -> Bool
/= Point2 -> Point2 -> Point2 -> Ordering
orient Point2
c Point2
d Point2
b)
                                then Point2
-> Point2 -> Point2 -> Point2 -> Either MathError Intersection2
lineIntersection Point2
a (Point2 -> Point2 -> Point2
sub2 Point2
b Point2
a) Point2
c (Point2 -> Point2 -> Point2
sub2 Point2
d Point2
c)
                                else Intersection2 -> Either MathError Intersection2
forall a b. b -> Either a b
Right Intersection2
NoIntersection

{- | Line/plane result: Nothing parallel/disjoint, Just Nothing coincident,
Just (Just p) a unique intersection. The line must have dimension three.
-}
linePlaneIntersection :: Line -> Plane -> Either MathError (Maybe (Maybe Point3))
linePlaneIntersection :: Line -> Plane -> Either MathError (Maybe (Maybe Point3))
linePlaneIntersection (Line PointND
a PointND
d) plane :: Plane
plane@(Plane Point3
normal Double
h) = do
    PointND -> PointND -> Either MathError ()
checkPair PointND
a PointND
d
    Bool -> String -> Either MathError ()
require ([Double] -> Int
forall a. [a] -> Int
forall (t :: * -> *) a. Foldable t => t a -> Int
length (PointND -> [Double]
coordsND PointND
a) Int -> Int -> Bool
forall a. Eq a => a -> a -> Bool
== Int
3 Bool -> Bool -> Bool
&& [Double] -> Double
norm (PointND -> [Double]
coordsND PointND
d) Double -> Double -> Bool
forall a. Ord a => a -> a -> Bool
> Double
0) String
"invalid 3D line"
    Plane -> Either MathError ()
checkPlane Plane
plane
    let ar :: [Rational]
ar = (Double -> Rational) -> [Double] -> [Rational]
forall a b. (a -> b) -> [a] -> [b]
map Double -> Rational
forall a. Real a => a -> Rational
toRational (PointND -> [Double]
coordsND PointND
a)
        dr :: [Rational]
dr = (Double -> Rational) -> [Double] -> [Rational]
forall a b. (a -> b) -> [a] -> [b]
map Double -> Rational
forall a. Real a => a -> Rational
toRational (PointND -> [Double]
coordsND PointND
d)
        nr :: [Rational]
nr = Point3 -> [Rational]
r3 Point3
normal
        den :: Rational
den = [Rational] -> [Rational] -> Rational
rDot [Rational]
dr [Rational]
nr
        delta :: Rational
delta = Double -> Rational
forall a. Real a => a -> Rational
toRational Double
h Rational -> Rational -> Rational
forall a. Num a => a -> a -> a
- [Rational] -> [Rational] -> Rational
rDot [Rational]
ar [Rational]
nr
    if Rational
den Rational -> Rational -> Bool
forall a. Eq a => a -> a -> Bool
== Rational
0
        then Maybe (Maybe Point3) -> Either MathError (Maybe (Maybe Point3))
forall a b. b -> Either a b
Right (if Rational
delta Rational -> Rational -> Bool
forall a. Eq a => a -> a -> Bool
== Rational
0 then Maybe Point3 -> Maybe (Maybe Point3)
forall a. a -> Maybe a
Just Maybe Point3
forall a. Maybe a
Nothing else Maybe (Maybe Point3)
forall a. Maybe a
Nothing)
        else do
            xs <- [Double] -> Either MathError [Double]
checked ((Rational -> Double) -> [Rational] -> [Double]
forall a b. (a -> b) -> [a] -> [b]
map Rational -> Double
forall a. Fractional a => Rational -> a
fromRational ((Rational -> Rational -> Rational)
-> [Rational] -> [Rational] -> [Rational]
forall a b c. (a -> b -> c) -> [a] -> [b] -> [c]
zipWith (\Rational
x Rational
v -> Rational
x Rational -> Rational -> Rational
forall a. Num a => a -> a -> a
+ Rational
delta Rational -> Rational -> Rational
forall a. Fractional a => a -> a -> a
/ Rational
den Rational -> Rational -> Rational
forall a. Num a => a -> a -> a
* Rational
v) [Rational]
ar [Rational]
dr))
            check3 (from3 xs)
            Right (Just (Just (from3 xs)))

{- | Plane intersection: same nested convention as linePlaneIntersection,
with the unique intersection represented by a line.
-}
planeIntersection :: Plane -> Plane -> Either MathError (Maybe (Maybe Line))
planeIntersection :: Plane -> Plane -> Either MathError (Maybe (Maybe Line))
planeIntersection p :: Plane
p@(Plane Point3
a Double
h) q :: Plane
q@(Plane Point3
b Double
k) = do
    Plane -> Either MathError ()
checkPlane Plane
p
    Plane -> Either MathError ()
checkPlane Plane
q
    let ar :: [Rational]
ar = Point3 -> [Rational]
r3 Point3
a; br :: [Rational]
br = Point3 -> [Rational]
r3 Point3
b; direction :: [Rational]
direction = [Rational] -> [Rational] -> [Rational]
rCross [Rational]
ar [Rational]
br; dd :: Rational
dd = [Rational] -> [Rational] -> Rational
rDot [Rational]
direction [Rational]
direction
    if Rational
dd Rational -> Rational -> Bool
forall a. Eq a => a -> a -> Bool
== Rational
0
        then
            Maybe (Maybe Line) -> Either MathError (Maybe (Maybe Line))
forall a b. b -> Either a b
Right
                ( if [Bool] -> Bool
forall (t :: * -> *). Foldable t => t Bool -> Bool
and ((Rational -> Rational -> Bool)
-> [Rational] -> [Rational] -> [Bool]
forall a b c. (a -> b -> c) -> [a] -> [b] -> [c]
zipWith Rational -> Rational -> Bool
forall a. Eq a => a -> a -> Bool
(==) ((Rational -> Rational) -> [Rational] -> [Rational]
forall a b. (a -> b) -> [a] -> [b]
map (Rational -> Rational -> Rational
forall a. Num a => a -> a -> a
* Double -> Rational
forall a. Real a => a -> Rational
toRational Double
h) [Rational]
br) ((Rational -> Rational) -> [Rational] -> [Rational]
forall a b. (a -> b) -> [a] -> [b]
map (Rational -> Rational -> Rational
forall a. Num a => a -> a -> a
* Double -> Rational
forall a. Real a => a -> Rational
toRational Double
k) [Rational]
ar))
                    then Maybe Line -> Maybe (Maybe Line)
forall a. a -> Maybe a
Just Maybe Line
forall a. Maybe a
Nothing
                    else Maybe (Maybe Line)
forall a. Maybe a
Nothing
                )
        else do
            let values :: [Rational]
values =
                    (Rational -> Rational -> Rational)
-> [Rational] -> [Rational] -> [Rational]
forall a b c. (a -> b -> c) -> [a] -> [b] -> [c]
zipWith
                        (\Rational
x Rational
y -> (Double -> Rational
forall a. Real a => a -> Rational
toRational Double
h Rational -> Rational -> Rational
forall a. Num a => a -> a -> a
* Rational
x Rational -> Rational -> Rational
forall a. Num a => a -> a -> a
+ Double -> Rational
forall a. Real a => a -> Rational
toRational Double
k Rational -> Rational -> Rational
forall a. Num a => a -> a -> a
* Rational
y) Rational -> Rational -> Rational
forall a. Fractional a => a -> a -> a
/ Rational
dd)
                        ([Rational] -> [Rational] -> [Rational]
rCross [Rational]
br [Rational]
direction)
                        ([Rational] -> [Rational] -> [Rational]
rCross [Rational]
direction [Rational]
ar)
                size :: Rational
size = [Rational] -> Rational
forall a. Ord a => [a] -> a
forall (t :: * -> *) a. (Foldable t, Ord a) => t a -> a
maximum ((Rational -> Rational) -> [Rational] -> [Rational]
forall a b. (a -> b) -> [a] -> [b]
map Rational -> Rational
forall a. Num a => a -> a
abs [Rational]
direction)
            position <- [Double] -> Either MathError PointND
pointND ((Rational -> Double) -> [Rational] -> [Double]
forall a b. (a -> b) -> [a] -> [b]
map Rational -> Double
forall a. Fractional a => Rational -> a
fromRational [Rational]
values)
            unit <- pointND (map (\Rational
x -> Rational -> Double
forall a. Fractional a => Rational -> a
fromRational (Rational
x Rational -> Rational -> Rational
forall a. Fractional a => a -> a -> a
/ Rational
size)) direction)
            Right (Just (Just (Line position unit)))

checkRay :: Ray -> Either MathError ()
checkRay :: Ray -> Either MathError ()
checkRay (Ray Point3
a Point3
d) = Point3 -> Either MathError ()
check3 Point3
a Either MathError () -> Either MathError () -> Either MathError ()
forall a b.
Either MathError a -> Either MathError b -> Either MathError b
forall (m :: * -> *) a b. Monad m => m a -> m b -> m b
>> Point3 -> Either MathError ()
check3 Point3
d Either MathError () -> Either MathError () -> Either MathError ()
forall a b.
Either MathError a -> Either MathError b -> Either MathError b
forall (m :: * -> *) a b. Monad m => m a -> m b -> m b
>> Bool -> String -> Either MathError ()
require ([Double] -> Double
norm (Point3 -> [Double]
coords3 Point3
d) Double -> Double -> Bool
forall a. Ord a => a -> a -> Bool
> Double
0) String
"zero ray direction"

{- | Moller-Trumbore ray intersection; coplanar rays return Nothing because
their hit set is not a unique transverse surface crossing. Degenerate triangles fail.
-}
rayTriangle :: Ray -> (Point3, Point3, Point3) -> Either MathError (Maybe Double)
rayTriangle :: Ray -> (Point3, Point3, Point3) -> Either MathError (Maybe Double)
rayTriangle ray :: Ray
ray@(Ray Point3
o Point3
d) (Point3
a, Point3
b, Point3
c) = do
    Ray -> Either MathError ()
checkRay Ray
ray
    (Point3 -> Either MathError ()) -> [Point3] -> Either MathError ()
forall (t :: * -> *) (m :: * -> *) a b.
(Foldable t, Monad m) =>
(a -> m b) -> t a -> m ()
mapM_ Point3 -> Either MathError ()
check3 [Point3
a, Point3
b, Point3
c]
    let e1 :: [Rational]
e1 = (Rational -> Rational -> Rational)
-> [Rational] -> [Rational] -> [Rational]
forall a b c. (a -> b -> c) -> [a] -> [b] -> [c]
zipWith (-) (Point3 -> [Rational]
r3 Point3
b) (Point3 -> [Rational]
r3 Point3
a)
        e2 :: [Rational]
e2 = (Rational -> Rational -> Rational)
-> [Rational] -> [Rational] -> [Rational]
forall a b c. (a -> b -> c) -> [a] -> [b] -> [c]
zipWith (-) (Point3 -> [Rational]
r3 Point3
c) (Point3 -> [Rational]
r3 Point3
a)
        p :: [Rational]
p = [Rational] -> [Rational] -> [Rational]
rCross (Point3 -> [Rational]
r3 Point3
d) [Rational]
e2
        det :: Rational
det = [Rational] -> [Rational] -> Rational
rDot [Rational]
e1 [Rational]
p
    Bool -> String -> Either MathError ()
require ((Rational -> Bool) -> [Rational] -> Bool
forall (t :: * -> *) a. Foldable t => (a -> Bool) -> t a -> Bool
any (Rational -> Rational -> Bool
forall a. Eq a => a -> a -> Bool
/= Rational
0) ([Rational] -> [Rational] -> [Rational]
rCross [Rational]
e1 [Rational]
e2)) String
"degenerate ray triangle"
    if Rational
det Rational -> Rational -> Bool
forall a. Eq a => a -> a -> Bool
== Rational
0
        then Maybe Double -> Either MathError (Maybe Double)
forall a b. b -> Either a b
Right Maybe Double
forall a. Maybe a
Nothing
        else
            let relative :: [Rational]
relative = (Rational -> Rational -> Rational)
-> [Rational] -> [Rational] -> [Rational]
forall a b c. (a -> b -> c) -> [a] -> [b] -> [c]
zipWith (-) (Point3 -> [Rational]
r3 Point3
o) (Point3 -> [Rational]
r3 Point3
a)
                u :: Rational
u = [Rational] -> [Rational] -> Rational
rDot [Rational]
relative [Rational]
p Rational -> Rational -> Rational
forall a. Fractional a => a -> a -> a
/ Rational
det
                q :: [Rational]
q = [Rational] -> [Rational] -> [Rational]
rCross [Rational]
relative [Rational]
e1
                v :: Rational
v = [Rational] -> [Rational] -> Rational
rDot (Point3 -> [Rational]
r3 Point3
d) [Rational]
q Rational -> Rational -> Rational
forall a. Fractional a => a -> a -> a
/ Rational
det
                t :: Rational
t = [Rational] -> [Rational] -> Rational
rDot [Rational]
e2 [Rational]
q Rational -> Rational -> Rational
forall a. Fractional a => a -> a -> a
/ Rational
det
             in if Rational
u Rational -> Rational -> Bool
forall a. Ord a => a -> a -> Bool
< Rational
0 Bool -> Bool -> Bool
|| Rational
v Rational -> Rational -> Bool
forall a. Ord a => a -> a -> Bool
< Rational
0 Bool -> Bool -> Bool
|| Rational
u Rational -> Rational -> Rational
forall a. Num a => a -> a -> a
+ Rational
v Rational -> Rational -> Bool
forall a. Ord a => a -> a -> Bool
> Rational
1 Bool -> Bool -> Bool
|| Rational
t Rational -> Rational -> Bool
forall a. Ord a => a -> a -> Bool
< Rational
0
                    then Maybe Double -> Either MathError (Maybe Double)
forall a b. b -> Either a b
Right Maybe Double
forall a. Maybe a
Nothing
                    else do
                        let parameter :: Double
parameter = Rational -> Double
forall a. Fractional a => Rational -> a
fromRational Rational
t
                        _ <- [Double] -> Either MathError [Double]
checked [Double
parameter]
                        Right (Just parameter)

-- | Slab intersection interval [entry,exit], clipped to the ray t>=0.
rayBox :: Ray -> AABB -> Either MathError (Maybe (Double, Double))
rayBox :: Ray -> AABB -> Either MathError (Maybe (Double, Double))
rayBox ray :: Ray
ray@(Ray Point3
o Point3
d) box :: AABB
box@(AABB PointND
lo PointND
hi) = do
    Ray -> Either MathError ()
checkRay Ray
ray
    AABB -> Either MathError ()
checkBox AABB
box
    Bool -> String -> Either MathError ()
require ([Double] -> Int
forall a. [a] -> Int
forall (t :: * -> *) a. Foldable t => t a -> Int
length (PointND -> [Double]
coordsND PointND
lo) Int -> Int -> Bool
forall a. Eq a => a -> a -> Bool
== Int
3) String
"ray box must be 3D"
    let slabs :: [Maybe (Double, Double)]
slabs =
            [ if Double
v Double -> Double -> Bool
forall a. Eq a => a -> a -> Bool
== Double
0
                then if Double
x Double -> Double -> Bool
forall a. Ord a => a -> a -> Bool
< Double
l Bool -> Bool -> Bool
|| Double
x Double -> Double -> Bool
forall a. Ord a => a -> a -> Bool
> Double
h then Maybe (Double, Double)
forall a. Maybe a
Nothing else (Double, Double) -> Maybe (Double, Double)
forall a. a -> Maybe a
Just (Double
0, Double
1 Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
0)
                else (Double, Double) -> Maybe (Double, Double)
forall a. a -> Maybe a
Just (Double -> Double -> Double
forall a. Ord a => a -> a -> a
min ((Double
l Double -> Double -> Double
forall a. Num a => a -> a -> a
- Double
x) Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
v) ((Double
h Double -> Double -> Double
forall a. Num a => a -> a -> a
- Double
x) Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
v), Double -> Double -> Double
forall a. Ord a => a -> a -> a
max ((Double
l Double -> Double -> Double
forall a. Num a => a -> a -> a
- Double
x) Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
v) ((Double
h Double -> Double -> Double
forall a. Num a => a -> a -> a
- Double
x) Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
v))
            | (Double
x, Double
v, Double
l, Double
h) <- [Double]
-> [Double]
-> [Double]
-> [Double]
-> [(Double, Double, Double, Double)]
forall {a} {b} {c} {d}. [a] -> [b] -> [c] -> [d] -> [(a, b, c, d)]
zip4 (Point3 -> [Double]
coords3 Point3
o) (Point3 -> [Double]
coords3 Point3
d) (PointND -> [Double]
coordsND PointND
lo) (PointND -> [Double]
coordsND PointND
hi)
            ]
    case [Maybe (Double, Double)] -> Maybe [(Double, Double)]
forall (t :: * -> *) (m :: * -> *) a.
(Traversable t, Monad m) =>
t (m a) -> m (t a)
forall (m :: * -> *) a. Monad m => [m a] -> m [a]
sequence [Maybe (Double, Double)]
slabs of
        Maybe [(Double, Double)]
Nothing -> Maybe (Double, Double) -> Either MathError (Maybe (Double, Double))
forall a b. b -> Either a b
Right Maybe (Double, Double)
forall a. Maybe a
Nothing
        Just [(Double, Double)]
intervals ->
            let a :: Double
a = [Double] -> Double
forall a. Ord a => [a] -> a
forall (t :: * -> *) a. (Foldable t, Ord a) => t a -> a
maximum (Double
0 Double -> [Double] -> [Double]
forall a. a -> [a] -> [a]
: ((Double, Double) -> Double) -> [(Double, Double)] -> [Double]
forall a b. (a -> b) -> [a] -> [b]
map (Double, Double) -> Double
forall a b. (a, b) -> a
fst [(Double, Double)]
intervals); b :: Double
b = [Double] -> Double
forall a. Ord a => [a] -> a
forall (t :: * -> *) a. (Foldable t, Ord a) => t a -> a
minimum (((Double, Double) -> Double) -> [(Double, Double)] -> [Double]
forall a b. (a -> b) -> [a] -> [b]
map (Double, Double) -> Double
forall a b. (a, b) -> b
snd [(Double, Double)]
intervals)
             in if Double
a Double -> Double -> Bool
forall a. Ord a => a -> a -> Bool
> Double
b
                    then Maybe (Double, Double) -> Either MathError (Maybe (Double, Double))
forall a b. b -> Either a b
Right Maybe (Double, Double)
forall a. Maybe a
Nothing
                    else do
                        _ <- [Double] -> Either MathError [Double]
checked [Double
a, Double
b]
                        Right (Just (a, b))
  where
    zip4 :: [a] -> [b] -> [c] -> [d] -> [(a, b, c, d)]
zip4 [a]
as [b]
bs [c]
cs [d]
ds = [(a
a, b
b, c
c, d
e) | (((a
a, b
b), c
c), d
e) <- [((a, b), c)] -> [d] -> [(((a, b), c), d)]
forall a b. [a] -> [b] -> [(a, b)]
zip ([(a, b)] -> [c] -> [((a, b), c)]
forall a b. [a] -> [b] -> [(a, b)]
zip ([a] -> [b] -> [(a, b)]
forall a b. [a] -> [b] -> [(a, b)]
zip [a]
as [b]
bs) [c]
cs) [d]
ds]

-- | Ray-sphere near/far nonnegative parameters; tangent gives equal roots.
raySphere :: Ray -> Sphere -> Either MathError (Maybe (Double, Double))
raySphere :: Ray -> Sphere -> Either MathError (Maybe (Double, Double))
raySphere ray :: Ray
ray@(Ray Point3
o Point3
d) sphere :: Sphere
sphere@(Sphere Point3
center Double
radius) = do
    Ray -> Either MathError ()
checkRay Ray
ray
    _ <- Sphere -> Point3 -> Either MathError Double
sphereSDF Sphere
sphere Point3
o
    let delta = [Double] -> [Double] -> [Double]
sub (Point3 -> [Double]
coords3 Point3
o) (Point3 -> [Double]
coords3 Point3
center)
        directionNorm = [Double] -> Double
norm (Point3 -> [Double]
coords3 Point3
d)
        direction = (Double -> Double) -> [Double] -> [Double]
forall a b. (a -> b) -> [a] -> [b]
map (Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
directionNorm) (Point3 -> [Double]
coords3 Point3
d)
        size = Double -> Double -> Double
forall a. Ord a => a -> a -> a
max Double
radius ([Double] -> Double
norm [Double]
delta)
    if size == 0
        then Right (Just (0, 0))
        else do
            let relative = (Double -> Double) -> [Double] -> [Double]
forall a b. (a -> b) -> [a] -> [b]
map (Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
size) [Double]
delta
                scaledRadius = Double
radius Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
size
                b = [Double] -> [Double] -> Double
dot [Double]
relative [Double]
direction
                c = [Double] -> [Double] -> Double
dot [Double]
relative [Double]
relative Double -> Double -> Double
forall a. Num a => a -> a -> a
- Double
scaledRadius Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
scaledRadius
                discriminant = Double
b Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
b Double -> Double -> Double
forall a. Num a => a -> a -> a
- Double
c
            if discriminant < 0
                then Right Nothing
                else
                    let q = -Double
b Double -> Double -> Double
forall a. Num a => a -> a -> a
- if Double
b Double -> Double -> Bool
forall a. Ord a => a -> a -> Bool
>= Double
0 then Double -> Double
forall a. Floating a => a -> a
sqrt Double
discriminant else -Double -> Double
forall a. Floating a => a -> a
sqrt Double
discriminant
                        (u, v) = if q == 0 then (0, 0) else (q, c / q)
                        lo = Double -> Double -> Double
forall a. Ord a => a -> a -> a
min Double
u Double
v
                        hi = Double -> Double -> Double
forall a. Ord a => a -> a -> a
max Double
u Double
v
                     in if hi < 0
                            then Right Nothing
                            else do
                                values <- checked [(max 0 lo * size) / directionNorm, (hi * size) / directionNorm]
                                case values of { [Double
entry, Double
exit] -> Maybe (Double, Double) -> Either MathError (Maybe (Double, Double))
forall a b. b -> Either a b
Right ((Double, Double) -> Maybe (Double, Double)
forall a. a -> Maybe a
Just (Double
entry, Double
exit)); [Double]
_ -> MathError -> Either MathError (Maybe (Double, Double))
forall a b. a -> Either a b
Left MathError
NonFiniteResult }

-- | Circle intersections: coincidence is distinct from disjointness and tangency.
data CircleIntersection = DisjointCircles | CoincidentCircles | CirclePoints [Point2]
    deriving (CircleIntersection -> CircleIntersection -> Bool
(CircleIntersection -> CircleIntersection -> Bool)
-> (CircleIntersection -> CircleIntersection -> Bool)
-> Eq CircleIntersection
forall a. (a -> a -> Bool) -> (a -> a -> Bool) -> Eq a
$c== :: CircleIntersection -> CircleIntersection -> Bool
== :: CircleIntersection -> CircleIntersection -> Bool
$c/= :: CircleIntersection -> CircleIntersection -> Bool
/= :: CircleIntersection -> CircleIntersection -> Bool
Eq, Int -> CircleIntersection -> ShowS
[CircleIntersection] -> ShowS
CircleIntersection -> String
(Int -> CircleIntersection -> ShowS)
-> (CircleIntersection -> String)
-> ([CircleIntersection] -> ShowS)
-> Show CircleIntersection
forall a.
(Int -> a -> ShowS) -> (a -> String) -> ([a] -> ShowS) -> Show a
$cshowsPrec :: Int -> CircleIntersection -> ShowS
showsPrec :: Int -> CircleIntersection -> ShowS
$cshow :: CircleIntersection -> String
show :: CircleIntersection -> String
$cshowList :: [CircleIntersection] -> ShowS
showList :: [CircleIntersection] -> ShowS
Show)

{- | Given two circles, return DisjointCircles, CoincidentCircles, or CirclePoints for their
intersection. Require valid finite centers and nonnegative bounded radii. Tangency
yields one point; identical zero-radius circles yield their single center.
-}
circleIntersection :: Circle -> Circle -> Either MathError CircleIntersection
circleIntersection :: Circle -> Circle -> Either MathError CircleIntersection
circleIntersection (Circle Point2
a Double
r) (Circle Point2
b Double
s) = do
    (Point2 -> Either MathError ()) -> [Point2] -> Either MathError ()
forall (t :: * -> *) (m :: * -> *) a b.
(Foldable t, Monad m) =>
(a -> m b) -> t a -> m ()
mapM_ Point2 -> Either MathError ()
check2 [Point2
a, Point2
b]
    Bool -> String -> Either MathError ()
require (Double -> Bool
coord Double
r Bool -> Bool -> Bool
&& Double -> Bool
coord Double
s Bool -> Bool -> Bool
&& Double
r Double -> Double -> Bool
forall a. Ord a => a -> a -> Bool
>= Double
0 Bool -> Bool -> Bool
&& Double
s Double -> Double -> Bool
forall a. Ord a => a -> a -> Bool
>= Double
0) String
"invalid circle radius"
    let d :: Double
d = Point2 -> Double
norm2 (Point2 -> Point2 -> Point2
sub2 Point2
b Point2
a)
    if Double
d Double -> Double -> Bool
forall a. Eq a => a -> a -> Bool
== Double
0
        then
            CircleIntersection -> Either MathError CircleIntersection
forall a b. b -> Either a b
Right (if Double
r Double -> Double -> Bool
forall a. Eq a => a -> a -> Bool
== Double
s then if Double
r Double -> Double -> Bool
forall a. Eq a => a -> a -> Bool
== Double
0 then [Point2] -> CircleIntersection
CirclePoints [Point2
a] else CircleIntersection
CoincidentCircles else CircleIntersection
DisjointCircles)
        else
            if Double
d Double -> Double -> Bool
forall a. Ord a => a -> a -> Bool
> Double
r Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
s Bool -> Bool -> Bool
|| Double
d Double -> Double -> Bool
forall a. Ord a => a -> a -> Bool
< Double -> Double
forall a. Num a => a -> a
abs (Double
r Double -> Double -> Double
forall a. Num a => a -> a -> a
- Double
s)
                then CircleIntersection -> Either MathError CircleIntersection
forall a b. b -> Either a b
Right CircleIntersection
DisjointCircles
                else
                    let size :: Double
size = Double -> Double -> Double
forall a. Ord a => a -> a -> a
max Double
d (Double -> Double -> Double
forall a. Ord a => a -> a -> a
max Double
r Double
s)
                        rn :: Double
rn = Double
r Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
size
                        sn :: Double
sn = Double
s Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
size
                        dn :: Double
dn = Double
d Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
size
                        xn :: Double
xn = (Double
rn Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
rn Double -> Double -> Double
forall a. Num a => a -> a -> a
- Double
sn Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
sn Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
dn Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
dn) Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ (Double
2 Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
dn)
                        x :: Double
x = Double
xn Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
size
                        h :: Double
h = Double -> Double
forall a. Floating a => a -> a
sqrt (Double -> Double -> Double
forall a. Ord a => a -> a -> a
max Double
0 (Double
rn Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
rn Double -> Double -> Double
forall a. Num a => a -> a -> a
- Double
xn Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
xn)) Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
size
                        P2 Double
dx Double
dy = Point2 -> Point2 -> Point2
sub2 Point2
b Point2
a
                        ux :: Double
ux = Double
dx Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
d
                        uy :: Double
uy = Double
dy Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
d
                        mid :: Point2
mid = Point2 -> Point2 -> Point2
add2 Point2
a (Double -> Double -> Point2
P2 (Double
x Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
ux) (Double
x Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
uy))
                        delta :: Point2
delta = Double -> Double -> Point2
P2 (-Double
h Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
uy) (Double
h Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
ux)
                     in CircleIntersection -> Either MathError CircleIntersection
forall a b. b -> Either a b
Right ([Point2] -> CircleIntersection
CirclePoints (if Double
h Double -> Double -> Bool
forall a. Eq a => a -> a -> Bool
== Double
0 then [Point2
mid] else [Point2 -> Point2 -> Point2
add2 Point2
mid Point2
delta, Point2 -> Point2 -> Point2
sub2 Point2
mid Point2
delta]))

-- | Closed triangle overlap polygon, including isolated tangencies as vertices.
triangleIntersection :: Triangle -> Triangle -> Either MathError [Point2]
triangleIntersection :: Triangle -> Triangle -> Either MathError [Point2]
triangleIntersection ta :: Triangle
ta@(Triangle Point2
a Point2
b Point2
c) tb :: Triangle
tb@(Triangle Point2
d Point2
e Point2
f) = do
    _ <- Triangle -> Point2 -> Either MathError Location
triangleLocation Triangle
ta Point2
a
    _ <- triangleLocation tb d
    crossings <-
        mapM
            (\((Point2
u, Point2
v), (Point2
w, Point2
x)) -> Point2
-> Point2 -> Point2 -> Point2 -> Either MathError Intersection2
segmentIntersection Point2
u Point2
v Point2
w Point2
x)
            [(x, y) | x <- edges [a, b, c], y <- edges [d, e, f]]
    let hit Intersection2
NoIntersection = []
        hit (IntersectionPoint Point2
p) = [Point2
p]
        hit (OverlapSegment Point2
p Point2
q) = [Point2
p, Point2
q]
        hit Intersection2
CoincidentLine = []
        points =
            [Point2] -> [Point2]
forall a. Eq a => [a] -> [a]
nub
                ([Point2
p | Point2
p <- [Point2
a, Point2
b, Point2
c], Triangle -> Point2 -> Bool
inside Triangle
tb Point2
p] [Point2] -> [Point2] -> [Point2]
forall a. [a] -> [a] -> [a]
++ [Point2
p | Point2
p <- [Point2
d, Point2
e, Point2
f], Triangle -> Point2 -> Bool
inside Triangle
ta Point2
p] [Point2] -> [Point2] -> [Point2]
forall a. [a] -> [a] -> [a]
++ (Intersection2 -> [Point2]) -> [Intersection2] -> [Point2]
forall (t :: * -> *) a b. Foldable t => (a -> [b]) -> t a -> [b]
concatMap Intersection2 -> [Point2]
hit [Intersection2]
crossings)
        inside Triangle
tri Point2
p = case Triangle -> Point2 -> Either MathError Location
triangleLocation Triangle
tri Point2
p of Right Location
Outside -> Bool
False; Right Location
_ -> Bool
True; Left MathError
_ -> Bool
False
    -- Triangle intersection is convex; order its extreme vertices so the
    -- result can be used directly as a ring (touches retain one/two points).
    let push [Point2]
stack Point2
p = case [Point2]
stack of
            Point2
y : Point2
x : [Point2]
rest | Point2 -> Point2 -> Point2 -> Ordering
orient Point2
x Point2
y Point2
p Ordering -> Ordering -> Bool
forall a. Eq a => a -> a -> Bool
/= Ordering
GT -> [Point2] -> Point2 -> [Point2]
push (Point2
x Point2 -> [Point2] -> [Point2]
forall a. a -> [a] -> [a]
: [Point2]
rest) Point2
p
            [Point2]
_ -> Point2
p Point2 -> [Point2] -> [Point2]
forall a. a -> [a] -> [a]
: [Point2]
stack
        half = [Point2] -> [Point2]
forall a. [a] -> [a]
reverse ([Point2] -> [Point2])
-> ([Point2] -> [Point2]) -> [Point2] -> [Point2]
forall b c a. (b -> c) -> (a -> b) -> a -> c
. ([Point2] -> Point2 -> [Point2])
-> [Point2] -> [Point2] -> [Point2]
forall b a. (b -> a -> b) -> b -> [a] -> b
forall (t :: * -> *) b a.
Foldable t =>
(b -> a -> b) -> b -> t a -> b
foldl [Point2] -> Point2 -> [Point2]
push []
        ordered = [Point2] -> [Point2]
forall a. Ord a => [a] -> [a]
sort [Point2]
points
        hull = if [Point2] -> Int
forall a. [a] -> Int
forall (t :: * -> *) a. Foldable t => t a -> Int
length [Point2]
ordered Int -> Int -> Bool
forall a. Ord a => a -> a -> Bool
<= Int
2 then [Point2]
ordered else [Point2] -> [Point2]
forall a. HasCallStack => [a] -> [a]
init ([Point2] -> [Point2]
half [Point2]
ordered) [Point2] -> [Point2] -> [Point2]
forall a. [a] -> [a] -> [a]
++ [Point2] -> [Point2]
forall a. HasCallStack => [a] -> [a]
init ([Point2] -> [Point2]
half ([Point2] -> [Point2]
forall a. [a] -> [a]
reverse [Point2]
ordered))
    Right hull

-- | Sphere signed distance (negative inside).
sphereSDF :: Sphere -> Point3 -> Either MathError Double
sphereSDF :: Sphere -> Point3 -> Either MathError Double
sphereSDF (Sphere Point3
c Double
r) Point3
p =
    Point3 -> Either MathError ()
check3 Point3
c
        Either MathError () -> Either MathError () -> Either MathError ()
forall a b.
Either MathError a -> Either MathError b -> Either MathError b
forall (m :: * -> *) a b. Monad m => m a -> m b -> m b
>> Point3 -> Either MathError ()
check3 Point3
p
        Either MathError () -> Either MathError () -> Either MathError ()
forall a b.
Either MathError a -> Either MathError b -> Either MathError b
forall (m :: * -> *) a b. Monad m => m a -> m b -> m b
>> Bool -> String -> Either MathError ()
require (Double -> Bool
coord Double
r Bool -> Bool -> Bool
&& Double
r Double -> Double -> Bool
forall a. Ord a => a -> a -> Bool
>= Double
0) String
"invalid sphere radius"
        Either MathError ()
-> Either MathError Double -> Either MathError Double
forall a b.
Either MathError a -> Either MathError b -> Either MathError b
forall (m :: * -> *) a b. Monad m => m a -> m b -> m b
>> Double -> Either MathError Double
forall a b. b -> Either a b
Right ([Double] -> Double
norm ([Double] -> [Double] -> [Double]
sub (Point3 -> [Double]
coords3 Point3
p) (Point3 -> [Double]
coords3 Point3
c)) Double -> Double -> Double
forall a. Num a => a -> a -> a
- Double
r)

-- | Axis-aligned box exact signed Euclidean distance in 1..16 dimensions.
boxSDF :: AABB -> PointND -> Either MathError Double
boxSDF :: AABB -> PointND -> Either MathError Double
boxSDF box :: AABB
box@(AABB PointND
a PointND
b) PointND
p = do
    AABB -> Either MathError ()
checkBox AABB
box
    PointND -> PointND -> Either MathError ()
checkPair PointND
a PointND
p
    let q :: [Double]
q = (Double -> Double -> Double -> Double)
-> [Double] -> [Double] -> [Double] -> [Double]
forall a b c d. (a -> b -> c -> d) -> [a] -> [b] -> [c] -> [d]
zipWith3 (\Double
x Double
l Double
h -> Double -> Double
forall a. Num a => a -> a
abs (Double
x Double -> Double -> Double
forall a. Num a => a -> a -> a
- (Double
l Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
h) Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
2) Double -> Double -> Double
forall a. Num a => a -> a -> a
- (Double
h Double -> Double -> Double
forall a. Num a => a -> a -> a
- Double
l) Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
2) (PointND -> [Double]
coordsND PointND
p) (PointND -> [Double]
coordsND PointND
a) (PointND -> [Double]
coordsND PointND
b)
    Double -> Either MathError Double
forall a b. b -> Either a b
Right ([Double] -> Double
norm ((Double -> Double) -> [Double] -> [Double]
forall a b. (a -> b) -> [a] -> [b]
map (Double -> Double -> Double
forall a. Ord a => a -> a -> a
max Double
0) [Double]
q) Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double -> Double -> Double
forall a. Ord a => a -> a -> a
min Double
0 ([Double] -> Double
forall a. Ord a => [a] -> a
forall (t :: * -> *) a. (Foldable t, Ord a) => t a -> a
maximum [Double]
q))

-- | Oriented box distance after validating its orthonormal axes.
obbSDF :: OBB -> Point3 -> Either MathError Double
obbSDF :: OBB -> Point3 -> Either MathError Double
obbSDF (OBB Point3
c [Point3]
axes Point3
half) Point3
p = do
    (Point3 -> Either MathError ()) -> [Point3] -> Either MathError ()
forall (t :: * -> *) (m :: * -> *) a b.
(Foldable t, Monad m) =>
(a -> m b) -> t a -> m ()
mapM_ Point3 -> Either MathError ()
check3 (Point3
c Point3 -> [Point3] -> [Point3]
forall a. a -> [a] -> [a]
: Point3
half Point3 -> [Point3] -> [Point3]
forall a. a -> [a] -> [a]
: Point3
p Point3 -> [Point3] -> [Point3]
forall a. a -> [a] -> [a]
: [Point3]
axes)
    Bool -> String -> Either MathError ()
require ([Point3] -> Int
forall a. [a] -> Int
forall (t :: * -> *) a. Foldable t => t a -> Int
length [Point3]
axes Int -> Int -> Bool
forall a. Eq a => a -> a -> Bool
== Int
3 Bool -> Bool -> Bool
&& (Double -> Bool) -> [Double] -> Bool
forall (t :: * -> *) a. Foldable t => (a -> Bool) -> t a -> Bool
all (Double -> Double -> Bool
forall a. Ord a => a -> a -> Bool
>= Double
0) (Point3 -> [Double]
coords3 Point3
half)) String
"invalid OBB axes/extents"
    Bool -> String -> Either MathError ()
require
        ( [Bool] -> Bool
forall (t :: * -> *). Foldable t => t Bool -> Bool
and
            [ Double -> Double
forall a. Num a => a -> a
abs ([Double] -> [Double] -> Double
dot (Point3 -> [Double]
coords3 ([Point3]
axes [Point3] -> Int -> Point3
forall a. HasCallStack => [a] -> Int -> a
!! Int
i)) (Point3 -> [Double]
coords3 ([Point3]
axes [Point3] -> Int -> Point3
forall a. HasCallStack => [a] -> Int -> a
!! Int
j)) Double -> Double -> Double
forall a. Num a => a -> a -> a
- if Int
i Int -> Int -> Bool
forall a. Eq a => a -> a -> Bool
== Int
j then Double
1 else Double
0) Double -> Double -> Bool
forall a. Ord a => a -> a -> Bool
< Double
1e-9
            | Int
i <- [Int
0 .. Int
2]
            , Int
j <- [Int
0 .. Int
2]
            ]
        )
        String
"OBB axes are not orthonormal"
    let local :: [Double]
local = [[Double] -> [Double] -> Double
dot ([Double] -> [Double] -> [Double]
sub (Point3 -> [Double]
coords3 Point3
p) (Point3 -> [Double]
coords3 Point3
c)) (Point3 -> [Double]
coords3 Point3
a) | Point3
a <- [Point3]
axes]; h :: [Double]
h = Point3 -> [Double]
coords3 Point3
half
    AABB -> PointND -> Either MathError Double
boxSDF (PointND -> PointND -> AABB
AABB ([Double] -> PointND
PointND ((Double -> Double) -> [Double] -> [Double]
forall a b. (a -> b) -> [a] -> [b]
map Double -> Double
forall a. Num a => a -> a
negate [Double]
h)) ([Double] -> PointND
PointND [Double]
h)) ([Double] -> PointND
PointND [Double]
local)

-- | Capsule distance from its closed axis minus radius.
capsuleSDF :: Capsule -> Point3 -> Either MathError Double
capsuleSDF :: Capsule -> Point3 -> Either MathError Double
capsuleSDF (Capsule Point3
a Point3
b Double
r) Point3
p = do
    (Point3 -> Either MathError ()) -> [Point3] -> Either MathError ()
forall (t :: * -> *) (m :: * -> *) a b.
(Foldable t, Monad m) =>
(a -> m b) -> t a -> m ()
mapM_ Point3 -> Either MathError ()
check3 [Point3
a, Point3
b, Point3
p]
    Bool -> String -> Either MathError ()
require (Double -> Bool
coord Double
r Bool -> Bool -> Bool
&& Double
r Double -> Double -> Bool
forall a. Ord a => a -> a -> Bool
>= Double
0) String
"invalid capsule radius"
    d <-
        PointND -> Segment -> Either MathError Double
pointSegmentDistance ([Double] -> PointND
PointND (Point3 -> [Double]
coords3 Point3
p)) (PointND -> PointND -> Segment
Segment ([Double] -> PointND
PointND (Point3 -> [Double]
coords3 Point3
a)) ([Double] -> PointND
PointND (Point3 -> [Double]
coords3 Point3
b)))
    Right (d - r)

-- | Exact capped finite-cylinder signed distance.
cylinderSDF :: Cylinder -> Point3 -> Either MathError Double
cylinderSDF :: Cylinder -> Point3 -> Either MathError Double
cylinderSDF (Cylinder Point3
a Point3
b Double
r) Point3
p = do
    (Point3 -> Either MathError ()) -> [Point3] -> Either MathError ()
forall (t :: * -> *) (m :: * -> *) a b.
(Foldable t, Monad m) =>
(a -> m b) -> t a -> m ()
mapM_ Point3 -> Either MathError ()
check3 [Point3
a, Point3
b, Point3
p]
    Bool -> String -> Either MathError ()
require (Double -> Bool
coord Double
r Bool -> Bool -> Bool
&& Double
r Double -> Double -> Bool
forall a. Ord a => a -> a -> Bool
>= Double
0 Bool -> Bool -> Bool
&& Point3
a Point3 -> Point3 -> Bool
forall a. Eq a => a -> a -> Bool
/= Point3
b) String
"invalid cylinder axis/radius"
    let axis :: [Double]
axis = [Double] -> [Double] -> [Double]
sub (Point3 -> [Double]
coords3 Point3
b) (Point3 -> [Double]
coords3 Point3
a)
        height :: Double
height = [Double] -> Double
norm [Double]
axis
        v :: [Double]
v = [Double] -> [Double] -> [Double]
sub (Point3 -> [Double]
coords3 Point3
p) (Point3 -> [Double]
coords3 Point3
a)
        axial :: Double
axial = [Double] -> [Double] -> Double
dot [Double]
v [Double]
axis Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
height
        radial :: Double
radial = [Double] -> Double
norm ([Double] -> [Double] -> [Double]
sub [Double]
v (Double -> [Double] -> [Double]
scale (Double
axial Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
height) [Double]
axis)) Double -> Double -> Double
forall a. Num a => a -> a -> a
- Double
r
        cap :: Double
cap = Double -> Double
forall a. Num a => a -> a
abs (Double
axial Double -> Double -> Double
forall a. Num a => a -> a -> a
- Double
height Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
2) Double -> Double -> Double
forall a. Num a => a -> a -> a
- Double
height Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
2
    Double -> Either MathError Double
forall a b. b -> Either a b
Right (Double -> Double
forall a. Floating a => a -> a
sqrt (Double -> Double -> Double
forall a. Ord a => a -> a -> a
max Double
0 Double
radial Double -> Int -> Double
forall a b. (Num a, Integral b) => a -> b -> a
^ (Int
2 :: Int) Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double -> Double -> Double
forall a. Ord a => a -> a -> a
max Double
0 Double
cap Double -> Int -> Double
forall a b. (Num a, Integral b) => a -> b -> a
^ (Int
2 :: Int)) Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double -> Double -> Double
forall a. Ord a => a -> a -> a
min Double
0 (Double -> Double -> Double
forall a. Ord a => a -> a -> a
max Double
radial Double
cap))

-- | Exact finite-cone distance by reducing to its radial/axial cross-section.
coneSDF :: Cone -> Point3 -> Either MathError Double
coneSDF :: Cone -> Point3 -> Either MathError Double
coneSDF (Cone Point3
apex Point3
base Double
r) Point3
p = do
    (Point3 -> Either MathError ()) -> [Point3] -> Either MathError ()
forall (t :: * -> *) (m :: * -> *) a b.
(Foldable t, Monad m) =>
(a -> m b) -> t a -> m ()
mapM_ Point3 -> Either MathError ()
check3 [Point3
apex, Point3
base, Point3
p]
    Bool -> String -> Either MathError ()
require (Double -> Bool
coord Double
r Bool -> Bool -> Bool
&& Double
r Double -> Double -> Bool
forall a. Ord a => a -> a -> Bool
>= Double
0 Bool -> Bool -> Bool
&& Point3
apex Point3 -> Point3 -> Bool
forall a. Eq a => a -> a -> Bool
/= Point3
base) String
"invalid cone axis/radius"
    let axis :: [Double]
axis = [Double] -> [Double] -> [Double]
sub (Point3 -> [Double]
coords3 Point3
base) (Point3 -> [Double]
coords3 Point3
apex)
        h :: Double
h = [Double] -> Double
norm [Double]
axis
        v :: [Double]
v = [Double] -> [Double] -> [Double]
sub (Point3 -> [Double]
coords3 Point3
p) (Point3 -> [Double]
coords3 Point3
apex)
        y :: Double
y = [Double] -> [Double] -> Double
dot [Double]
v [Double]
axis Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
h
        x :: Double
x = [Double] -> Double
norm ([Double] -> [Double] -> [Double]
sub [Double]
v (Double -> [Double] -> [Double]
scale (Double
y Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
h) [Double]
axis))
        q :: PointND
q = [Double] -> PointND
PointND [Double
x, Double
y]
    side <- PointND -> Segment -> Either MathError Double
pointSegmentDistance PointND
q (PointND -> PointND -> Segment
Segment ([Double] -> PointND
PointND [Double
0, Double
0]) ([Double] -> PointND
PointND [Double
r, Double
h]))
    bottom <- pointSegmentDistance q (Segment (PointND [0, h]) (PointND [r, h]))
    Right ((if y >= 0 && y <= h && x <= r * y / h then -1 else 1) * min side bottom)