Frustum.hs

module Math.Frustum where

import Data.List (foldl')
import Data.Vect.Float

data Plane = Plane { plNormal :: Vec3, plDist :: Float }

newtype Frustum = Frustum { frPlanes :: [Plane] }

pointInFrustum :: Vec3 -> Frustum -> Bool
pointInFrustum p fr = foldl' (\b (Plane n d) -> b && d + n &. p >= 0) True $ frPlanes fr

sphereInFrustum :: Vec3 -> Float -> Frustum -> Bool
--sphereInFrustum p r fr = foldl' (\b (Plane n d) -> b && d + n &. p >= (-r)) True $ frPlanes fr
sphereInFrustum p r fr = all (\(Plane n d) -> d + n &. p >= (-r)) (frPlanes fr)

boxInFrustum ::Vec3 -> Vec3 -> Frustum -> Bool
boxInFrustum pp pn fr = foldl' (\b (Plane n d) -> b && d + n &. (g pp pn n) >= 0) True $ frPlanes fr
    where
        g (Vec3 px py pz) (Vec3 nx ny nz) n = Vec3 (fx px nx) (fy py ny) (fz pz nz)
            where
                [fx,fy,fz] = map (\a -> if a > 0 then max else min) $ destructVec3 [n]

frustum :: Float -> Float -> Float -> Float -> Vec3 -> Vec3 -> Vec3 -> Frustum
frustum angle ratio nearD farD p l u = Frustum [pl ntr ntl ftl, pl nbl nbr fbr, pl ntl nbl fbl,
                                                pl nbr ntr fbr, pl ntl ntr nbr, pl ftr ftl fbl]
  where
    pl a b c = Plane n d
      where
        n = normalize $ (c &- b) &^ (a &- b)
        d = -(n &. b)
    ang2rad = pi / 180
    tang    = tan $ angle * ang2rad * 0.5
    nh  = nearD * tang
    nw  = nh * ratio
    fh  = farD * tang
    fw  = fh * ratio
    z   = normalize $ p &- l
    x   = normalize $ u &^ z
    y   = z &^ x

    nc  = p &- nearD *& z
    fc  = p &- farD *& z

    ntl = nc &+ nh *& y &- nw *& x
    ntr = nc &+ nh *& y &+ nw *& x
    nbl = nc &- nh *& y &- nw *& x
    nbr = nc &- nh *& y &+ nw *& x

    ftl = fc &+ fh *& y &- fw *& x
    ftr = fc &+ fh *& y &+ fw *& x
    fbl = fc &- fh *& y &- fw *& x
    fbr = fc &- fh *& y &+ fw *& x

frustumFromMatrix :: Mat4 -> Frustum
frustumFromMatrix m = Frustum [pl plLeftN plLeftD, pl plRightN plRightD, pl plTopN plTopD, pl plBottomN plBottomD, pl plNearN plNearD, pl plFarN plFarD]
    where
        Mat4 (Vec4 m00 m01 m02 m03) (Vec4 m10 m11 m12 m13) (Vec4 m20 m21 m22 m23) (Vec4 m30 m31 m32 m33) = transpose m
        pl n d = Plane (normalize n) (d / norm n)
        plLeftN     = Vec3 (m30+m00) (m31+m01) (m32+m02)
        plLeftD     = (m33+m03)
        plRightN    = Vec3 (m30-m00) (m31-m01) (m32-m02)
        plRightD    = (m33-m03)
        plTopN      = Vec3 (m30-m10) (m31-m11) (m32-m12)
        plTopD      = (m33-m13)
        plBottomN   = Vec3 (m30+m10) (m31+m11) (m32+m12)
        plBottomD   = (m33+m13)
        plNearN     = Vec3 (m30+m20) (m31+m21) (m32+m22)
        plNearD     = (m33+m23)
        plFarN      = Vec3 (m30-m20) (m31-m21) (m32-m22)
        plFarD      = (m33-m23)