{-# LANGUAGE BangPatterns #-}
{-# LANGUAGE CPP #-}
module Data.CSG
(
Solid
, plane
, sphere
, cylinder
, cone
, cuboid
, coneFrustum
, cylinderFrustum
, intersect
, unite
, complement
, subtract
, Point
, Vec3
, Ray(..)
, HitPoint(..)
, HitSegment
, Trace
, trace
, cast
, inside
, module V3
)
where
import Prelude hiding (Just, Nothing, Maybe, subtract)
import Data.Strict.Maybe
import Data.Strict.Tuple
import Test.QuickCheck (Arbitrary(..), frequency, sized)
import Data.Vec3 hiding (Vec3, Matrix)
import qualified Data.Vec3 as V3
#ifdef WITH_TRIPLES
type Vec3 = TVec3
#else
type Vec3 = CVec3
#endif
type Point = Vec3
type Matrix = V3.Matrix Vec3
newtype Ray = Ray (Point, Vec3)
data HitPoint = HitPoint !Double (Maybe Vec3)
deriving (Eq, Show)
instance Ord HitPoint where
compare (HitPoint t1 _) (HitPoint t2 _) = compare t1 t2
type HitSegment = (Pair HitPoint HitPoint)
type Trace = [HitSegment]
infinityP :: Double
infinityP = (/) 1 0
infinityN :: Double
infinityN = -infinityP
hitN :: HitPoint
hitN = HitPoint infinityN Nothing
hitP :: HitPoint
hitP = HitPoint infinityP Nothing
data Solid = Plane !Vec3 !Double
| Sphere !Vec3 !Double
| Cylinder !Vec3 !Point !Double
| Cone !Vec3 !Point !Double !Matrix !Double !Double
| Union !Solid !Solid
| Intersection !Solid !Solid
| Complement !Solid
deriving (Eq, Show)
instance Arbitrary Solid where
arbitrary = sized $ \n ->
frequency $
[ (4, sphere <$> arbitrary <*> arbitrary)
, (4, cuboid <$> arbitrary <*> arbitrary)
, (3, cylinder <$> arbitrary <*> arbitrary <*> arbitrary)
, (3, cone <$> arbitrary <*> arbitrary <*> arbitrary)
, (3, cylinderFrustum <$> arbitrary <*> arbitrary <*> arbitrary)
, (3, coneFrustum <$> arbitrary <*> arbitrary)
] ++
if n == 0 then [] else
[ (8, unite <$> arbitrary <*> arbitrary)
, (3, intersect <$> arbitrary <*> arbitrary)
, (3, complement <$> arbitrary)
]
plane :: Point -> Vec3 -> Solid
plane p n = Plane nn (p .* nn)
where
nn = normalize n
sphere :: Vec3 -> Double -> Solid
sphere = Sphere
cuboid :: Point -> Point -> Solid
cuboid p1 p2 =
plane p1' (fromXYZ (1, 0, 0))
`intersect`
plane p1' (fromXYZ (0, 1, 0))
`intersect`
plane p1' (fromXYZ (0, 0, 1))
`intersect`
plane p2' (fromXYZ (-1, 0, 0))
`intersect`
plane p2' (fromXYZ (0, -1, 0))
`intersect`
plane p2' (fromXYZ (0, 0, -1))
where
(x1, y1, z1) = toXYZ p1
(x2, y2, z2) = toXYZ p2
p2' = fromXYZ (min x1 x2, min y1 y2, min z1 z2)
p1' = fromXYZ (max x1 x2, max y1 y2, max z1 z2)
cylinder :: Point -> Point -> Double -> Solid
cylinder p1 p2 = Cylinder (normalize $ p2 <-> p1) p1
cylinderFrustum :: Point -> Point -> Double -> Solid
cylinderFrustum pb pt r =
plane pt axis
`intersect`
plane pb (invert axis)
`intersect`
cylinder pb pt r
where
axis = pt <-> pb
cone :: Vec3 -> Point -> Double -> Solid
cone a o h =
let
rads = h * pi / 180
h' = cos rads
n = normalize $ invert a
gamma = diag (-h' * h')
m = addM (n `vxv` n) gamma
ta = tan rads
odelta = n .* o
in
Cone n o h' m ta odelta
coneFrustum :: (Point, Double) -> (Point, Double) -> Solid
coneFrustum (p1, r1) (p2, r2) =
let
(pb, rb, pt, rt) = if r1 < r2
then (p2, r2, p1, r1)
else (p1, r1, p2, r2)
gap = pt <-> pb
height = norm gap
axis = normalize gap
dist = if rt == 0
then 0
else height / (rb / rt - 1)
apex = pt <+> (axis .^ dist)
degs = atan (rb / (dist + norm (pt <-> pb))) * (180 / pi)
in
plane pt axis
`intersect`
plane pb (invert axis)
`intersect`
cone axis apex degs
intersect :: Solid -> Solid -> Solid
intersect !b1 !b2 = Intersection b1 b2
unite :: Solid -> Solid -> Solid
unite !b1 !b2 = Union b1 b2
complement :: Solid -> Solid
complement !b = Complement b
subtract :: Solid -> Solid -> Solid
subtract !b1 !b2 = intersect b1 $ complement b2
trace :: Solid -> Ray -> Trace
{-# INLINE trace #-}
trace b@(Plane n d) (Ray (pos, v)) =
let
!f = -(n .* v)
in
if f == 0
then
[hitN :!: hitP | inside pos b]
else
let
!t = (pos .* n - d) / f
in
if f > 0
then [HitPoint t (Just n) :!: hitP]
else [hitN :!: HitPoint t (Just n)]
trace (Sphere c r) (Ray (pos, v)) =
let
!d = pos <-> c
!roots = solveq (v .* v) (v .* d * 2) (d .* d - r * r)
normal !u = normalize (u <-> c)
in
case roots of
Nothing -> []
Just (t1 :!: t2) ->
[HitPoint t1 (Just $ normal $ moveBy pos v t1) :!:
HitPoint t2 (Just $ normal $ moveBy pos v t2)]
trace (Cylinder n c r) (Ray (pos, v)) =
let
d = (pos <-> c) >< n
e = v >< n
roots = solveq (e .* e) (d .* e * 2) (d .* d - r * r)
normal u = normalize $ h <-> (n .^ (h .* n))
where h = u <-> c
in
case roots of
Nothing -> []
Just (t1 :!: t2) ->
[HitPoint t1 (Just $ normal $ moveBy pos v t1) :!:
HitPoint t2 (Just $ normal $ moveBy pos v t2)]
trace (Cone n c _ m ta odelta) (Ray (pos, v)) =
let
delta = pos <-> c
c2 = dotM v v m
c1 = dotM v delta m
c0 = dotM delta delta m
roots = solveq c2 (2 * c1) c0
normal !u = normalize $ nx .^ (1 / ta) <-> ny .^ ta
where h = u <-> c
ny' = n .^ (n .* h)
ny = normalize ny'
nx = normalize $ h <-> ny'
in
case roots of
Nothing -> []
Just (t1 :!: t2) ->
let
pos1 = moveBy pos v t1
pos2 = moveBy pos v t2
in
case ((pos1 .* n - odelta) > 0, (pos2 .* n - odelta) > 0) of
(True, True) -> [HitPoint t1 (Just $ normal pos1) :!:
HitPoint t2 (Just $ normal pos2)]
(True, False) -> [hitN :!:
HitPoint t1 (Just $ normal pos1)]
(False, True) -> [HitPoint t2 (Just $ normal pos2) :!:
hitP]
(False, False) -> []
trace (Intersection b1 b2) !p =
intersectTraces tr1 tr2
where
tr1 = trace b1 p
tr2 = trace b2 p
trace (Union b1 b2) !p =
uniteTraces tr1 tr2
where
tr1 = trace b1 p
tr2 = trace b2 p
trace (Complement b) !p =
complementTrace $ trace b p
cast :: Ray -> Solid -> Maybe HitPoint
cast r b =
case intersectTraces onlyFutureHits (trace b r) of
(:!:) hp@(HitPoint _ (Just _)) _ : _ -> Just hp
(:!:) (HitPoint _ Nothing) hp@(HitPoint _ (Just _)) : _ -> Just hp
_ -> Nothing
where
onlyFutureHits = [HitPoint 0 Nothing :!: HitPoint infinityP Nothing]
uniteTraces :: Trace -> Trace -> Trace
uniteTraces u [] = u
uniteTraces u (v:t2) =
uniteTraces (unite1 u v) t2
where
merge :: HitSegment -> HitSegment -> HitSegment
merge (a1 :!: b1) (a2 :!: b2) = min a1 a2 :!: max b1 b2
{-# INLINE merge #-}
unite1 :: Trace -> HitSegment -> Trace
unite1 [] hs = [hs]
unite1 t@(hs1@(a1 :!: b1):tr') hs2@(a2 :!: b2)
| b1 < a2 = hs1:unite1 tr' hs2
| a1 > b2 = hs2:t
| otherwise = unite1 tr' (merge hs1 hs2)
{-# INLINE unite1 #-}
{-# INLINE uniteTraces #-}
intersectTraces :: Trace -> Trace -> Trace
intersectTraces tr1 tr2 =
let
overlap :: HitSegment -> HitSegment -> HitSegment
overlap (a1 :!: b1) (a2 :!: b2) = max a1 a2 :!: min b1 b2
{-# INLINE overlap #-}
in
case tr2 of
[] -> []
(hs2@(a2 :!: b2):tr2') ->
case tr1 of
[] -> []
(hs1@(a1 :!: b1):tr1') | b1 < a2 -> intersectTraces tr1' tr2
| b2 < a1 -> intersectTraces tr1 tr2'
| otherwise -> overlap hs1 hs2:intersectTraces tr1' tr2
{-# INLINE intersectTraces #-}
complementTrace :: Trace -> Trace
complementTrace ((sp@(HitPoint ts _) :!: ep):xs) =
start ++ complementTrace' ep xs
where
flipNormals :: HitSegment -> HitSegment
flipNormals (HitPoint t1 n1 :!: HitPoint t2 n2) =
HitPoint t1 (invert <$> n1) :!: HitPoint t2 (invert <$> n2)
{-# INLINE flipNormals #-}
start = if isInfinite ts
then []
else [flipNormals $ hitN :!: sp]
complementTrace' :: HitPoint -> Trace -> Trace
complementTrace' c ((a :!: b):tr) =
flipNormals (c :!: a):complementTrace' b tr
complementTrace' a@(HitPoint t _) [] =
[flipNormals (a :!: hitP) | not (isInfinite t)]
complementTrace [] = [hitN :!: hitP]
{-# INLINE complementTrace #-}
inside :: Point -> Solid -> Bool
{-# INLINE inside #-}
inside !pos (Plane n d) = (pos .* n - d) < 0
inside !pos (Sphere c r) = norm (pos <-> c) < r
inside !pos (Cylinder n c r) =
norm (h <-> (n .^ (h .* n))) < r
where
h = pos <-> c
inside !pos (Cone n c a _ _ _) =
n .* normalize (pos <-> c) > a
inside !p (Intersection b1 b2) = inside p b1 && inside p b2
inside !p (Union b1 b2) = inside p b1 || inside p b2
inside !p (Complement b) = not $ inside p b
moveBy :: Point
-> Vec3
-> Double
-> Point
moveBy !p !v !t = p <+> (v .^ t)
{-# INLINE moveBy #-}
solveq :: Double
-> Double
-> Double
-> Maybe (Pair Double Double)
solveq !a !b !c
| d > 0 = Just $ min r1 r2 :!: max r1 r2
| otherwise = Nothing
where
d = b * b - 4 * a * c
q = sqrt d
t = 2 * a
r = - b / t
s = q / t
r1 = r - s
r2 = r + s
{-# INLINE solveq #-}