-- SPDX-License-Identifier: MPL-2.0

{- | Column-vector homogeneous transforms. The 3D camera convention is
right-handed, looks along negative camera z, and maps the visible depth range
to OpenGL NDC [-1,1]. Positive y points up. Composition a*b applies b first.
Affine ND transforms accept dimensions 1..16; projective 3D transforms reject
points at infinity. Frustum normals point inward and culling retains tangency.
-}
module HMath.Geometry.Transform (
    affineTransform,
    applyTransform,
    composeTransforms,
    inverseTransform,
    perspective,
    orthographic,
    lookAt,
    extractFrustum,
    cullSphere,
    cullBox,
) where

import Data.List (maximumBy)
import Data.Ord (comparing)
import HMath.Error (MathError (..))
import HMath.Geometry.Internal
import qualified HMath.Matrix as M

validate :: M.Matrix Double -> Either MathError Int
validate :: Matrix Double -> Either MathError Int
validate Matrix Double
matrix =
    let (Int
r, Int
c) = Matrix Double -> (Int, Int)
forall a. Matrix a -> (Int, Int)
M.shape Matrix Double
matrix
     in Bool -> String -> Either MathError ()
require
            (Int
r Int -> Int -> Bool
forall a. Eq a => a -> a -> Bool
== Int
c Bool -> Bool -> Bool
&& Int
r Int -> Int -> Bool
forall a. Ord a => a -> a -> Bool
>= Int
2 Bool -> Bool -> Bool
&& Int
r Int -> Int -> Bool
forall a. Ord a => a -> a -> Bool
<= Int
17 Bool -> Bool -> Bool
&& (Double -> Bool) -> [Double] -> Bool
forall (t :: * -> *) a. Foldable t => (a -> Bool) -> t a -> Bool
all Double -> Bool
coord (Matrix Double -> [Double]
forall a. Matrix a -> [a]
M.toList Matrix Double
matrix))
            String
"transform must be finite square 2..17"
            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 (Int
r Int -> Int -> Int
forall a. Num a => a -> a -> a
- Int
1)

-- | Construct an affine matrix [linear,translation;0,1].
affineTransform :: M.Matrix Double -> PointND -> Either MathError (M.Matrix Double)
affineTransform :: Matrix Double -> PointND -> Either MathError (Matrix Double)
affineTransform Matrix Double
linear PointND
translation = do
    PointND -> Either MathError ()
checkND PointND
translation
    let d :: Int
d = [Double] -> Int
forall a. [a] -> Int
forall (t :: * -> *) a. Foldable t => t a -> Int
length (PointND -> [Double]
coordsND PointND
translation)
    Bool -> String -> Either MathError ()
require (Matrix Double -> (Int, Int)
forall a. Matrix a -> (Int, Int)
M.shape Matrix Double
linear (Int, Int) -> (Int, Int) -> Bool
forall a. Eq a => a -> a -> Bool
== (Int
d, Int
d) Bool -> Bool -> Bool
&& (Double -> Bool) -> [Double] -> Bool
forall (t :: * -> *) a. Foldable t => (a -> Bool) -> t a -> Bool
all Double -> Bool
coord (Matrix Double -> [Double]
forall a. Matrix a -> [a]
M.toList Matrix Double
linear)) String
"invalid affine linear part"
    Int -> Int -> [Double] -> Either MathError (Matrix Double)
forall a. Int -> Int -> [a] -> Either MathError (Matrix a)
M.fromListChecked
        (Int
d Int -> Int -> Int
forall a. Num a => a -> a -> a
+ Int
1)
        (Int
d Int -> Int -> Int
forall a. Num a => a -> a -> a
+ Int
1)
        ( [[Double]] -> [Double]
forall (t :: * -> *) a. Foldable t => t [a] -> [a]
concat [Int -> [Double] -> [Double]
forall a. Int -> [a] -> [a]
take Int
d (Int -> [Double] -> [Double]
forall a. Int -> [a] -> [a]
drop (Int
i Int -> Int -> Int
forall a. Num a => a -> a -> a
* Int
d) (Matrix Double -> [Double]
forall a. Matrix a -> [a]
M.toList Matrix Double
linear)) [Double] -> [Double] -> [Double]
forall a. [a] -> [a] -> [a]
++ [PointND -> [Double]
coordsND PointND
translation [Double] -> Int -> Double
forall a. HasCallStack => [a] -> Int -> a
!! Int
i] | Int
i <- [Int
0 .. Int
d Int -> Int -> Int
forall a. Num a => a -> a -> a
- Int
1]]
            [Double] -> [Double] -> [Double]
forall a. [a] -> [a] -> [a]
++ Int -> Double -> [Double]
forall a. Int -> a -> [a]
replicate Int
d Double
0
            [Double] -> [Double] -> [Double]
forall a. [a] -> [a] -> [a]
++ [Double
1]
        )

-- | Apply an affine or projective matrix and divide by homogeneous w.
applyTransform :: M.Matrix Double -> PointND -> Either MathError PointND
applyTransform :: Matrix Double -> PointND -> Either MathError PointND
applyTransform Matrix Double
matrix PointND
p = do
    d <- Matrix Double -> Either MathError Int
validate Matrix Double
matrix
    checkND p
    require (length (coordsND p) == d) "transform dimension mismatch"
    let input = PointND -> [Double]
coordsND PointND
p [Double] -> [Double] -> [Double]
forall a. [a] -> [a] -> [a]
++ [Double
1]
        a = Matrix Double -> [Double]
forall a. Matrix a -> [a]
M.toList Matrix Double
matrix
        raw = [[Double] -> [Double] -> Double
dot (Int -> [Double] -> [Double]
forall a. Int -> [a] -> [a]
take (Int
d Int -> Int -> Int
forall a. Num a => a -> a -> a
+ Int
1) (Int -> [Double] -> [Double]
forall a. Int -> [a] -> [a]
drop (Int
i Int -> Int -> Int
forall a. Num a => a -> a -> a
* (Int
d Int -> Int -> Int
forall a. Num a => a -> a -> a
+ Int
1)) [Double]
a)) [Double]
input | Int
i <- [Int
0 .. Int
d]]
        w = [Double] -> Double
forall a. HasCallStack => [a] -> a
last [Double]
raw
    require (w /= 0 && finite w) "projective point lies at infinity"
    pointND (map (/ w) (take d raw))

-- | Checked matrix composition; projective and affine matrices may be mixed.
composeTransforms :: M.Matrix Double -> M.Matrix Double -> Either MathError (M.Matrix Double)
composeTransforms :: Matrix Double -> Matrix Double -> Either MathError (Matrix Double)
composeTransforms Matrix Double
a Matrix Double
b = do
    da <- Matrix Double -> Either MathError Int
validate Matrix Double
a
    db <- validate b
    require (da == db) "transform dimensions differ"
    out <- M.multiplyChecked a b
    _ <- validate out
    Right out

-- | Gauss-Jordan inverse with partial pivoting, bounded to matrices of size 17.
inverseTransform :: M.Matrix Double -> Either MathError (M.Matrix Double)
inverseTransform :: Matrix Double -> Either MathError (Matrix Double)
inverseTransform Matrix Double
matrix = do
    d <- Matrix Double -> Either MathError Int
validate Matrix Double
matrix
    let n = Int
d Int -> Int -> Int
forall a. Num a => a -> a -> a
+ Int
1
        a = Matrix Double -> [Double]
forall a. Matrix a -> [a]
M.toList Matrix Double
matrix
        rows = [Int -> [Double] -> [Double]
forall a. Int -> [a] -> [a]
take Int
n (Int -> [Double] -> [Double]
forall a. Int -> [a] -> [a]
drop (Int
i Int -> Int -> Int
forall a. Num a => a -> a -> a
* Int
n) [Double]
a) [Double] -> [Double] -> [Double]
forall 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 | Int
j <- [Int
0 .. Int
n Int -> Int -> Int
forall a. Num a => a -> a -> a
- Int
1]] | Int
i <- [Int
0 .. Int
n Int -> Int -> Int
forall a. Num a => a -> a -> a
- Int
1]]
    out <- eliminate n rows 0
    result <- M.fromListChecked n n (concatMap (drop n) out)
    _ <- validate result
    Right result
  where
    eliminate :: Int -> [[Double]] -> Int -> Either MathError [[Double]]
eliminate Int
n [[Double]]
rows Int
col
        | Int
col 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 =
            let pivot :: Int
pivot = (Int -> Int -> Ordering) -> [Int] -> Int
forall (t :: * -> *) a.
Foldable t =>
(a -> a -> Ordering) -> t a -> a
maximumBy ((Int -> Double) -> Int -> Int -> Ordering
forall a b. Ord a => (b -> a) -> b -> b -> Ordering
comparing (\Int
i -> Double -> Double
forall a. Num a => a -> a
abs ([[Double]]
rows [[Double]] -> Int -> [Double]
forall a. HasCallStack => [a] -> Int -> a
!! Int
i [Double] -> Int -> Double
forall a. HasCallStack => [a] -> Int -> a
!! Int
col))) [Int
col .. Int
n Int -> Int -> Int
forall a. Num a => a -> a -> a
- Int
1]
                exchanged :: [[Double]]
exchanged =
                    [ if Int
i Int -> Int -> Bool
forall a. Eq a => a -> a -> Bool
== Int
col then [[Double]]
rows [[Double]] -> Int -> [Double]
forall a. HasCallStack => [a] -> Int -> a
!! Int
pivot else if Int
i Int -> Int -> Bool
forall a. Eq a => a -> a -> Bool
== Int
pivot then [[Double]]
rows [[Double]] -> Int -> [Double]
forall a. HasCallStack => [a] -> Int -> a
!! Int
col else [[Double]]
rows [[Double]] -> Int -> [Double]
forall a. HasCallStack => [a] -> Int -> a
!! Int
i
                    | Int
i <- [Int
0 .. Int
n Int -> Int -> Int
forall a. Num a => a -> a -> a
- Int
1]
                    ]
                value :: Double
value = [[Double]]
exchanged [[Double]] -> Int -> [Double]
forall a. HasCallStack => [a] -> Int -> a
!! Int
col [Double] -> Int -> Double
forall a. HasCallStack => [a] -> Int -> a
!! Int
col
             in if Double
value Double -> Double -> Bool
forall a. Eq a => a -> a -> Bool
== Double
0
                    then MathError -> Either MathError [[Double]]
forall a b. a -> Either a b
Left (Int -> MathError
SingularMatrix Int
col)
                    else
                        let prow :: [Double]
prow = (Double -> Double) -> [Double] -> [Double]
forall a b. (a -> b) -> [a] -> [b]
map (Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
value) ([[Double]]
exchanged [[Double]] -> Int -> [Double]
forall a. HasCallStack => [a] -> Int -> a
!! Int
col)
                            output :: [[Double]]
output =
                                [ if Int
i Int -> Int -> Bool
forall a. Eq a => a -> a -> Bool
== Int
col then [Double]
prow else (Double -> Double -> Double) -> [Double] -> [Double] -> [Double]
forall a b c. (a -> b -> c) -> [a] -> [b] -> [c]
zipWith (-) ([[Double]]
exchanged [[Double]] -> Int -> [Double]
forall a. HasCallStack => [a] -> Int -> a
!! Int
i) (Double -> [Double] -> [Double]
scale ([[Double]]
exchanged [[Double]] -> Int -> [Double]
forall a. HasCallStack => [a] -> Int -> a
!! Int
i [Double] -> Int -> Double
forall a. HasCallStack => [a] -> Int -> a
!! Int
col) [Double]
prow)
                                | Int
i <- [Int
0 .. Int
n Int -> Int -> Int
forall a. Num a => a -> a -> a
- Int
1]
                                ]
                         in if (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]]
output) then Int -> [[Double]] -> Int -> Either MathError [[Double]]
eliminate Int
n [[Double]]
output (Int
col Int -> Int -> Int
forall a. Num a => a -> a -> a
+ Int
1) else MathError -> Either MathError [[Double]]
forall a b. a -> Either a b
Left MathError
NonFiniteResult

-- | Vertical field of view in (0,pi), positive aspect, 0<near<far.
perspective :: Double -> Double -> Double -> Double -> Either MathError (M.Matrix Double)
perspective :: Double
-> Double -> Double -> Double -> Either MathError (Matrix Double)
perspective Double
fov Double
aspect Double
near Double
far = do
    Bool -> String -> Either MathError ()
require
        ((Double -> Bool) -> [Double] -> Bool
forall (t :: * -> *) a. Foldable t => (a -> Bool) -> t a -> Bool
all Double -> Bool
coord [Double
fov, Double
aspect, Double
near, Double
far] Bool -> Bool -> Bool
&& Double
fov Double -> Double -> Bool
forall a. Ord a => a -> a -> Bool
> Double
0 Bool -> Bool -> Bool
&& Double
fov Double -> Double -> Bool
forall a. Ord a => a -> a -> Bool
< Double
forall a. Floating a => a
pi Bool -> Bool -> Bool
&& Double
aspect Double -> Double -> Bool
forall a. Ord a => a -> a -> Bool
> Double
0 Bool -> Bool -> Bool
&& Double
near Double -> Double -> Bool
forall a. Ord a => a -> a -> Bool
> Double
0 Bool -> Bool -> Bool
&& Double
far Double -> Double -> Bool
forall a. Ord a => a -> a -> Bool
> Double
near)
        String
"invalid perspective parameters"
    let f :: Double
f = Double
1 Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double -> Double
forall a. Floating a => a -> a
tan (Double
fov Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
2)
    out <-
        Int -> Int -> [Double] -> Either MathError (Matrix Double)
forall a. Int -> Int -> [a] -> Either MathError (Matrix a)
M.fromListChecked
            Int
4
            Int
4
            [ Double
f Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
aspect
            , Double
0
            , Double
0
            , Double
0
            , Double
0
            , Double
f
            , Double
0
            , Double
0
            , Double
0
            , Double
0
            , (Double
far Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
near) Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ (Double
near Double -> Double -> Double
forall a. Num a => a -> a -> a
- Double
far)
            , Double
2 Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
far Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
near Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ (Double
near Double -> Double -> Double
forall a. Num a => a -> a -> a
- Double
far)
            , Double
0
            , Double
0
            , -Double
1
            , Double
0
            ]
    _ <- validate out
    Right out

-- | Orthographic bounds with left<right, bottom<top and 0<=near<far.
orthographic ::
    Double -> Double -> Double -> Double -> Double -> Double -> Either MathError (M.Matrix Double)
orthographic :: Double
-> Double
-> Double
-> Double
-> Double
-> Double
-> Either MathError (Matrix Double)
orthographic Double
left Double
right Double
bottom Double
top Double
near Double
far = do
    Bool -> String -> Either MathError ()
require
        ( (Double -> Bool) -> [Double] -> Bool
forall (t :: * -> *) a. Foldable t => (a -> Bool) -> t a -> Bool
all Double -> Bool
coord [Double
left, Double
right, Double
bottom, Double
top, Double
near, Double
far]
            Bool -> Bool -> Bool
&& Double
left Double -> Double -> Bool
forall a. Ord a => a -> a -> Bool
< Double
right
            Bool -> Bool -> Bool
&& Double
bottom Double -> Double -> Bool
forall a. Ord a => a -> a -> Bool
< Double
top
            Bool -> Bool -> Bool
&& Double
near Double -> Double -> Bool
forall a. Ord a => a -> a -> Bool
>= Double
0
            Bool -> Bool -> Bool
&& Double
near Double -> Double -> Bool
forall a. Ord a => a -> a -> Bool
< Double
far
        )
        String
"invalid orthographic parameters"
    Int -> Int -> [Double] -> Either MathError (Matrix Double)
forall a. Int -> Int -> [a] -> Either MathError (Matrix a)
M.fromListChecked
        Int
4
        Int
4
        [ Double
2 Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ (Double
right Double -> Double -> Double
forall a. Num a => a -> a -> a
- Double
left)
        , Double
0
        , Double
0
        , -(Double
right Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
left) Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ (Double
right Double -> Double -> Double
forall a. Num a => a -> a -> a
- Double
left)
        , Double
0
        , Double
2 Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ (Double
top Double -> Double -> Double
forall a. Num a => a -> a -> a
- Double
bottom)
        , Double
0
        , -(Double
top Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
bottom) Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ (Double
top Double -> Double -> Double
forall a. Num a => a -> a -> a
- Double
bottom)
        , Double
0
        , Double
0
        , -Double
2 Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ (Double
far Double -> Double -> Double
forall a. Num a => a -> a -> a
- Double
near)
        , -(Double
far Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
near) Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ (Double
far Double -> Double -> Double
forall a. Num a => a -> a -> a
- Double
near)
        , Double
0
        , Double
0
        , Double
0
        , Double
1
        ]
        Either MathError (Matrix Double)
-> (Matrix Double -> Either MathError (Matrix Double))
-> Either MathError (Matrix Double)
forall a b.
Either MathError a
-> (a -> Either MathError b) -> Either MathError b
forall (m :: * -> *) a b. Monad m => m a -> (a -> m b) -> m b
>>= (\Matrix Double
m -> Matrix Double -> Either MathError Int
validate Matrix Double
m Either MathError Int
-> Either MathError (Matrix Double)
-> Either MathError (Matrix 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
>> Matrix Double -> Either MathError (Matrix Double)
forall a b. b -> Either a b
Right Matrix Double
m)

-- | World-to-camera transform from eye, target and nonparallel up direction.
lookAt :: Point3 -> Point3 -> Point3 -> Either MathError (M.Matrix Double)
lookAt :: Point3 -> Point3 -> Point3 -> Either MathError (Matrix Double)
lookAt Point3
eye Point3
target Point3
up = 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
eye, Point3
target, Point3
up]
    let forward :: [Double]
forward = [Double] -> [Double] -> [Double]
sub (Point3 -> [Double]
coords3 Point3
target) (Point3 -> [Double]
coords3 Point3
eye); fNorm :: Double
fNorm = [Double] -> Double
norm [Double]
forward
    Bool -> String -> Either MathError ()
require (Double
fNorm Double -> Double -> Bool
forall a. Ord a => a -> a -> Bool
> Double
0) String
"look-at eye equals target"
    let f :: [Double]
f = (Double -> Double) -> [Double] -> [Double]
forall a b. (a -> b) -> [a] -> [b]
map (Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
fNorm) [Double]
forward; sRaw :: [Double]
sRaw = Point3 -> [Double]
coords3 (Point3 -> Point3 -> Point3
cross ([Double] -> Point3
from3 [Double]
f) Point3
up); sNorm :: Double
sNorm = [Double] -> Double
norm [Double]
sRaw
    Bool -> String -> Either MathError ()
require (Double
sNorm Double -> Double -> Bool
forall a. Ord a => a -> a -> Bool
> Double
0) String
"look-at up is parallel to view direction"
    let s :: [Double]
s = (Double -> Double) -> [Double] -> [Double]
forall a b. (a -> b) -> [a] -> [b]
map (Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
sNorm) [Double]
sRaw; u :: [Double]
u = Point3 -> [Double]
coords3 (Point3 -> Point3 -> Point3
cross ([Double] -> Point3
from3 [Double]
s) ([Double] -> Point3
from3 [Double]
f)); e :: [Double]
e = Point3 -> [Double]
coords3 Point3
eye
    Int -> Int -> [Double] -> Either MathError (Matrix Double)
forall a. Int -> Int -> [a] -> Either MathError (Matrix a)
M.fromListChecked
        Int
4
        Int
4
        ([Double]
s [Double] -> [Double] -> [Double]
forall a. [a] -> [a] -> [a]
++ [-[Double] -> [Double] -> Double
dot [Double]
s [Double]
e] [Double] -> [Double] -> [Double]
forall a. [a] -> [a] -> [a]
++ [Double]
u [Double] -> [Double] -> [Double]
forall a. [a] -> [a] -> [a]
++ [-[Double] -> [Double] -> Double
dot [Double]
u [Double]
e] [Double] -> [Double] -> [Double]
forall a. [a] -> [a] -> [a]
++ (Double -> Double) -> [Double] -> [Double]
forall a b. (a -> b) -> [a] -> [b]
map Double -> Double
forall a. Num a => a -> a
negate [Double]
f [Double] -> [Double] -> [Double]
forall a. [a] -> [a] -> [a]
++ [[Double] -> [Double] -> Double
dot [Double]
f [Double]
e] [Double] -> [Double] -> [Double]
forall a. [a] -> [a] -> [a]
++ [Double
0, Double
0, Double
0, Double
1])

{- | Extract six inward clipping planes from projection*view. Plane normals
are normalized so signed plane values are distances in world coordinates.
-}
extractFrustum :: M.Matrix Double -> Either MathError Frustum
extractFrustum :: Matrix Double -> Either MathError Frustum
extractFrustum Matrix Double
matrix = do
    d <- Matrix Double -> Either MathError Int
validate Matrix Double
matrix
    require (d == 3) "frustum matrix must be 4x4"
    let rows = [Int -> [Double] -> [Double]
forall a. Int -> [a] -> [a]
take Int
4 (Int -> [Double] -> [Double]
forall a. Int -> [a] -> [a]
drop (Int
4 Int -> Int -> Int
forall a. Num a => a -> a -> a
* Int
i) (Matrix Double -> [Double]
forall a. Matrix a -> [a]
M.toList Matrix Double
matrix)) | Int
i <- [Int
0 .. Int
3]]
    planes <-
        mapM make [zipWith (\Double
a Double
b -> Double
a Double -> Double -> Double
forall a. Num a => a -> a -> a
+ Double
sign Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
b) (rows !! 3) (rows !! i) | i <- [0 .. 2], sign <- [1, -1]]
    Right (Frustum planes)
  where
    make :: [Double] -> Either MathError Plane
make [Double
a, Double
b, Double
c, Double
h] =
        let n :: Double
n = Double -> Double
forall a. Floating a => a -> a
sqrt (Double
a Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
a Double -> Double -> Double
forall a. Num a => a -> a -> a
+ 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 Double -> Double -> Double
forall a. Num a => a -> a -> a
* Double
c)
         in Bool -> String -> Either MathError ()
require (Double
n Double -> Double -> Bool
forall a. Ord a => a -> a -> Bool
> Double
0 Bool -> Bool -> Bool
&& Double -> Bool
finite Double
n) String
"degenerate frustum plane"
                Either MathError ()
-> Either MathError Plane -> Either MathError Plane
forall a b.
Either MathError a -> Either MathError b -> Either MathError b
forall (m :: * -> *) a b. Monad m => m a -> m b -> m b
>> Plane -> Either MathError Plane
forall a b. b -> Either a b
Right (Point3 -> Double -> Plane
Plane (Double -> Double -> Double -> Point3
P3 (Double
a Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
n) (Double
b Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
n) (Double
c Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
n)) (-Double
h Double -> Double -> Double
forall a. Fractional a => a -> a -> a
/ Double
n))
    make [Double]
_ = MathError -> Either MathError Plane
forall a b. a -> Either a b
Left (String -> MathError
InvalidArgument String
"frustum plane length")

checkFrustum :: Frustum -> Either MathError [Plane]
checkFrustum :: Frustum -> Either MathError [Plane]
checkFrustum (Frustum [Plane]
planes) = do
    Bool -> String -> Either MathError ()
require ([Plane] -> Int
forall a. [a] -> Int
forall (t :: * -> *) a. Foldable t => t a -> Int
length [Plane]
planes Int -> Int -> Bool
forall a. Eq a => a -> a -> Bool
== Int
6) String
"frustum requires six planes"
    (Plane -> Either MathError ()) -> [Plane] -> Either MathError ()
forall (t :: * -> *) (m :: * -> *) a b.
(Foldable t, Monad m) =>
(a -> m b) -> t a -> m ()
mapM_
        (\(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 frustum plane")
        [Plane]
planes
    [Plane] -> Either MathError [Plane]
forall a b. b -> Either a b
Right [Plane]
planes

-- | True means the closed sphere may be visible (conservative plane culling).
cullSphere :: Frustum -> Sphere -> Either MathError Bool
cullSphere :: Frustum -> Sphere -> Either MathError Bool
cullSphere Frustum
frustum (Sphere Point3
center Double
radius) = do
    planes <- Frustum -> Either MathError [Plane]
checkFrustum Frustum
frustum
    check3 center
    require (coord radius && radius >= 0) "invalid culling sphere"
    Right
        (all (\(Plane Point3
n Double
h) -> [Double] -> [Double] -> Double
dot (Point3 -> [Double]
coords3 Point3
n) (Point3 -> [Double]
coords3 Point3
center) Double -> Double -> Double
forall a. Num a => a -> a -> a
- Double
h Double -> Double -> Bool
forall a. Ord a => a -> a -> Bool
>= -Double
radius Double -> Double -> Double
forall a. Num a => a -> a -> a
* [Double] -> Double
norm (Point3 -> [Double]
coords3 Point3
n)) planes)

-- | True means the closed 3D AABB may be visible; corners need not be allocated.
cullBox :: Frustum -> AABB -> Either MathError Bool
cullBox :: Frustum -> AABB -> Either MathError Bool
cullBox Frustum
frustum (AABB PointND
lo PointND
hi) = do
    planes <- Frustum -> Either MathError [Plane]
checkFrustum Frustum
frustum
    checkPair lo hi
    require
        (length (coordsND lo) == 3 && and (zipWith (<=) (coordsND lo) (coordsND hi)))
        "invalid culling box"
    Right
        ( all
            ( \(Plane Point3
n Double
h) ->
                [Double] -> [Double] -> Double
dot
                    (Point3 -> [Double]
coords3 Point3
n)
                    ((Double -> Double -> Double -> Double)
-> [Double] -> [Double] -> [Double] -> [Double]
forall a b c d. (a -> b -> c -> d) -> [a] -> [b] -> [c] -> [d]
zipWith3 (\Double
v Double
a Double
b -> if Double
v Double -> Double -> Bool
forall a. Ord a => a -> a -> Bool
>= Double
0 then Double
b else Double
a) (Point3 -> [Double]
coords3 Point3
n) (PointND -> [Double]
coordsND PointND
lo) (PointND -> [Double]
coordsND PointND
hi))
                    Double -> Double -> Bool
forall a. Ord a => a -> a -> Bool
>= Double
h
            )
            planes
        )