-- |
-- Module: M.Collision.Internal.March
-- Description: Ray marching implementation for grid-based collision detection
-- Copyright: (c) axionbuster, 2025
-- License: BSD-3-Clause
--
-- Implements ray marching algorithm for finding intersections with grid points.
-- Used for precise collision detection in voxel-based environments.
module M.Collision.Internal.March (March (..), march) where

import Control.Lens hiding (index)
import Control.Monad.Fix
import Control.Monad.ST.Lazy
import Data.Foldable
import Data.Functor
import Data.Functor.Rep
import Data.STRef.Lazy
import Linear hiding (trace)
import Prelude hiding (read)

-- | convert negative zero to positive zero
nonegzero :: (RealFloat a) => a -> a
nonegzero :: forall a. RealFloat a => a -> a
nonegzero a
x | a -> Bool
forall a. RealFloat a => a -> Bool
isNegativeZero a
x = a
0 -- positive zero
nonegzero a
x = a
x

isfinite :: (RealFloat a) => a -> Bool
isfinite :: forall a. RealFloat a => a -> Bool
isfinite a
x = Bool -> Bool
not (a -> Bool
forall a. RealFloat a => a -> Bool
isNaN a
x Bool -> Bool -> Bool
|| a -> Bool
forall a. RealFloat a => a -> Bool
isInfinite a
x)

-- | apply Kahan's compensated sum to two numbers
add ::
  (Num a) =>
  -- | x
  a ->
  -- | y
  a ->
  -- | compensator
  a ->
  -- | (x + y, compensator)
  (a, a)
add :: forall a. Num a => a -> a -> a -> (a, a)
add a
x a
y a
c =
  let y' :: a
y' = a
y a -> a -> a
forall a. Num a => a -> a -> a
- a
c
      u :: a
u = a
x a -> a -> a
forall a. Num a => a -> a -> a
+ a
y'
      c' :: a
c' = a
u a -> a -> a
forall a. Num a => a -> a -> a
- a
x a -> a -> a
forall a. Num a => a -> a -> a
- a
y'
   in (a
u, a
c')

-- | intermediate data structure for 'march'
data I f a = I
  { -- | delta time to next intersection
    forall (f :: * -> *) a. I f a -> a
itim :: !a,
    -- | new current position
    forall (f :: * -> *) a. I f a -> f a
icur :: !(f a),
    -- | compensator for 'icur'
    forall (f :: * -> *) a. I f a -> f a
icom :: !(f a),
    -- | grid points intersected
    forall (f :: * -> *) a. I f a -> [f Int]
igrid :: ![f Int]
  }

-- | 'march' data structure
data March f a = March
  { -- | total time
    forall (f :: * -> *) a. March f a -> a
mtot :: a,
    -- | grid intersection (lies on boundaries of grid cells)
    --
    -- \'pct\' is for \'punctum\', which is Latin for point
    forall (f :: * -> *) a. March f a -> f a
mpct :: f a,
    -- | grid points (e.g., cubes, squares) intersected
    forall (f :: * -> *) a. March f a -> [f Int]
mict :: [f Int]
  }

-- | march along a line segment, finding all intersections
-- with grid squares or cubes (depending on the dimensionality)
-- as well as the time it takes to reach each intersection
-- and the cubes that are intersected
--
-- the cubes are represented by their low corner coordinates
--
-- in 2D, when a point is intersected, the two squares about
-- the point that the line (that extends rhe ray) does NOT
-- intersect will be included. it's because this routine is used
-- for collision detection
--
-- in 3D, there are many edge cases, but generally only the cubes
-- needed for collision detection are returned. so about
-- a corner, three cubes will be returned; abour an edge,
-- two (assuming ray is not parallel to a coordinate plane)
--
-- a compensated sum is used to reduce floating point error.
-- the compensation applies to the coordinates and times
--
-- the returned list being infinite, it is recommended to
-- use 'take' to limit the number of points to be computed
--
-- the starting point is not included in the list unless it
-- happens to be a grid intersection
--
-- if the direction is (near) zero, or if any component of the
-- direction is not finite, then the function will return an empty list
march ::
  forall f a.
  ( Foldable f,
    Representable f,
    Rep f ~ E f,
    RealFloat a,
    Epsilon a
  ) =>
  -- | starting point. use either f ~ 'V2' or f ~ 'V3' or other 'Representable'
  -- vector types where 'fmap f x' agrees with
  --
  -- @'tabulate' \\i -> f ('index' x i))@
  f a ->
  -- | direction (no need to be normalized)
  f a ->
  -- | list of (total time, point, [grid point]) pairs
  [March f a]
march :: forall (f :: * -> *) a.
(Foldable f, Representable f, Rep f ~ E f, RealFloat a,
 Epsilon a) =>
f a -> f a -> [March f a]
march f a
_ f a
direction | (Bool -> Bool
not (Bool -> Bool) -> (f a -> Bool) -> f a -> Bool
forall b c a. (b -> c) -> (a -> b) -> a -> c
. (a -> Bool) -> f a -> Bool
forall (t :: * -> *) a. Foldable t => (a -> Bool) -> t a -> Bool
all a -> Bool
forall a. RealFloat a => a -> Bool
isfinite) f a
direction = []
march f a
_ f a
direction | (a -> Bool) -> f a -> Bool
forall (t :: * -> *) a. Foldable t => (a -> Bool) -> t a -> Bool
all a -> Bool
forall a. Epsilon a => a -> Bool
nearZero f a
direction = []
march f a
start ((a -> a) -> f a -> f a
forall a b. (a -> b) -> f a -> f b
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
fmap a -> a
forall a. RealFloat a => a -> a
nonegzero -> f a
direction) = (forall s. ST s [March f a]) -> [March f a]
forall a. (forall s. ST s a) -> a
runST do
  let fi :: Int -> a
fi = Int -> a
forall a b. (Integral a, Num b) => a -> b
fromIntegral :: Int -> a
      ! :: f a -> Rep f -> a
(!) = f a -> Rep f -> a
forall a. f a -> Rep f -> a
forall (f :: * -> *) a. Representable f => f a -> Rep f -> a
index
      new :: a -> ST s (STRef s a)
new = a -> ST s (STRef s a)
forall a s. a -> ST s (STRef s a)
newSTRef
      read :: STRef s a -> ST s a
read = STRef s a -> ST s a
forall s a. STRef s a -> ST s a
readSTRef
      write :: STRef s a -> a -> ST s ()
write = STRef s a -> a -> ST s ()
forall s a. STRef s a -> a -> ST s ()
writeSTRef
      modify :: STRef s a -> (a -> a) -> ST s ()
modify = STRef s a -> (a -> a) -> ST s ()
forall s a. STRef s a -> (a -> a) -> ST s ()
modifySTRef
      lift2 :: (Int -> Int -> Int) -> f Int -> f Int -> f Int
lift2 Int -> Int -> Int
f f Int
x f Int
y = forall (f :: * -> *) a. Representable f => (Rep f -> a) -> f a
tabulate @f \Rep f
i -> Int -> Int -> Int
f (f Int
x f Int -> Rep f -> Int
forall a. f a -> Rep f -> a
! Rep f
i) (f Int
y f Int -> Rep f -> Int
forall a. f a -> Rep f -> a
! Rep f
i)
      minimum_ :: [a] -> a
minimum_ = (a -> a -> a) -> [a] -> a
forall a. (a -> a -> a) -> [a] -> a
forall (t :: * -> *) a. Foldable t => (a -> a -> a) -> t a -> a
foldr1 \a
a a
b ->
        if
          | a -> Bool
forall a. RealFloat a => a -> Bool
isNaN a
a -> a
b
          | a -> Bool
forall a. RealFloat a => a -> Bool
isNaN a
b -> a
a
          | Bool
otherwise -> a -> a -> a
forall a. Ord a => a -> a -> a
min a
a a
b -- if both are NaN, then pick either
      computesig :: f b -> f b
computesig f b
d = b -> b
forall {a}. (Eq a, Num a) => a -> a
f (b -> b) -> (b -> b) -> b -> b
forall b c a. (b -> c) -> (a -> b) -> a -> c
. b -> b
forall b. Integral b => b -> b
forall a b. (RealFrac a, Integral b) => a -> b
floor (b -> b) -> (b -> b) -> b -> b
forall b c a. (b -> c) -> (a -> b) -> a -> c
. b -> b
forall a. Num a => a -> a
signum (b -> b) -> f b -> f b
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
<$> f b
d
        where
          -- for difficult-to-explain reasons, you need to give a stationary (0)
          -- displacement component a fake signum of +1 in order to avoid
          -- getting -Infinity when it shouldn't
          -- \^ FIXME: this explanation could be outdated
          f :: a -> a
f a
0 = a
1
          f a
x = a
x
      -- round toward opposite direction of a signum component
      round_ :: a -> a -> b
round_ (-1) = a -> b
forall b. Integral b => a -> b
forall a b. (RealFrac a, Integral b) => a -> b
ceiling
      round_ a
1 = a -> b
forall b. Integral b => a -> b
forall a b. (RealFrac a, Integral b) => a -> b
floor
      round_ a
_ = [Char] -> a -> b
forall a. HasCallStack => [Char] -> a
error [Char]
"signum neither -1 nor 1"
      -- vector of functions that each generate a grid point
      -- (specialized for each dimension). confused yet? yeah, it's
      -- really hard to explain
      gengridpoints :: f (f Int -> f a -> f Int)
gengridpoints = (Rep f -> f Int -> f a -> f Int) -> f (f Int -> f a -> f Int)
forall a. (Rep f -> a) -> f a
forall (f :: * -> *) a. Representable f => (Rep f -> a) -> f a
tabulate \Rep f
i f Int
sig f a
v ->
        -- grid point (compute from later-determined cur value)
        -- this was the hardest part to figure out
        --
        -- 1
        -- on (round_ (-(sig ! j))) ... you need a greatest integer
        -- less than (or least integer greater than) the current
        -- coordinate, which is either ((subtract 1) . ceiling)
        -- or ((+ 1) . floor), depending on the OPPOSITE side of the
        -- signum of the direction. the subtract 1 vs. + 1 will be done
        -- when we subtract the sig component from the grid point
        -- later on
        --
        -- 2
        -- on (max 0 <$> sig) ... if you flip the coordinate system
        -- by some axis, the grid points are still numbered by the
        -- bottom left (& etc) corner, so you need to subtract 1
        -- from the grid point if the direction is negative. this
        -- means to add 1 to the sig component -> hence max 0
        let roundedv :: f Int
roundedv = (Rep f -> Int) -> f Int
forall a. (Rep f -> a) -> f a
forall (f :: * -> *) a. Representable f => (Rep f -> a) -> f a
tabulate \Rep f
j -> Int -> a -> Int
forall {a} {b} {a}.
(RealFrac a, Integral b, Eq a, Num a) =>
a -> a -> b
round_ (-(f Int
sig f Int -> Rep f -> Int
forall a. f a -> Rep f -> a
! Rep f
j)) (f a
v f a -> Rep f -> a
forall a. f a -> Rep f -> a
! Rep f
j)
         in (Int -> Int -> Int) -> f Int -> f Int -> f Int
lift2 (-) f Int
roundedv (Int -> Int -> Int
forall a. Ord a => a -> a -> a
max Int
0 (Int -> Int) -> f Int -> f Int
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
<$> f Int
sig) f Int -> (f Int -> f Int) -> f Int
forall a b. a -> (a -> b) -> b
& E f -> forall x. Lens' (f x) x
forall (t :: * -> *). E t -> forall x. Lens' (t x) x
el Rep f
E f
i ((Int -> Identity Int) -> f Int -> Identity (f Int))
-> Int -> f Int -> f Int
forall a s t. Num a => ASetter s t a a -> a -> s -> t
+~ f Int
sig f Int -> Rep f -> Int
forall a. f a -> Rep f -> a
! Rep f
i
      -- 'inter' is the main loop logic
      inter :: f Int -> f a -> f a -> f a -> I f a
inter f Int
sig f a
dir f a
com f a
cur =
        -- mechanism:
        -- using the parametric equation of the line
        -- find the closest intersection with the grid -> get 'time' value
        -- then use the 'time' to get the coordinates of the intersection
        let times :: f (a, f a -> f Int)
times = (Rep f -> (a, f a -> f Int)) -> f (a, f a -> f Int)
forall a. (Rep f -> a) -> f a
forall (f :: * -> *) a. Representable f => (Rep f -> a) -> f a
tabulate \Rep f
i ->
              let r :: a -> Int
r = Int -> a -> Int
forall {a} {b} {a}.
(RealFrac a, Integral b, Eq a, Num a) =>
a -> a -> b
round_ (Int -> a -> Int) -> Int -> a -> Int
forall a b. (a -> b) -> a -> b
$ f Int
sig f Int -> Rep f -> Int
forall a. f a -> Rep f -> a
! Rep f
i
                  u :: a
u = Int -> a
fi (a -> Int
r (f a
cur f a -> Rep f -> a
forall a. f a -> Rep f -> a
! Rep f
i) Int -> Int -> Int
forall a. Num a => a -> a -> a
+ f Int
sig f Int -> Rep f -> Int
forall a. f a -> Rep f -> a
! Rep f
i) a -> a -> a
forall a. Num a => a -> a -> a
- f a
cur f a -> Rep f -> a
forall a. f a -> Rep f -> a
! Rep f
i
               in (a
u a -> a -> a
forall a. Fractional a => a -> a -> a
/ f a
dir f a -> Rep f -> a
forall a. f a -> Rep f -> a
! Rep f
i, (f (f Int -> f a -> f Int)
gengridpoints f (f Int -> f a -> f Int) -> Rep f -> f Int -> f a -> f Int
forall a. f a -> Rep f -> a
! Rep f
i) f Int
sig)
            t :: a
t = [a] -> a
minimum_ ([a] -> a) -> [a] -> a
forall a b. (a -> b) -> a -> b
$ (a -> Bool) -> [a] -> [a]
forall a. (a -> Bool) -> [a] -> [a]
filter (a -> a -> Bool
forall a. Ord a => a -> a -> Bool
> a
0) ([a] -> [a]) -> [a] -> [a]
forall a b. (a -> b) -> a -> b
$ ((a, f a -> f Int) -> a) -> [(a, f a -> f Int)] -> [a]
forall a b. (a -> b) -> [a] -> [b]
map (a, f a -> f Int) -> a
forall a b. (a, b) -> a
fst ([(a, f a -> f Int)] -> [a]) -> [(a, f a -> f Int)] -> [a]
forall a b. (a -> b) -> a -> b
$ f (a, f a -> f Int) -> [(a, f a -> f Int)]
forall a. f a -> [a]
forall (t :: * -> *) a. Foldable t => t a -> [a]
toList f (a, f a -> f Int)
times
            -- funcs to compute grid coordinates at the time of intersection
            -- if many intersected simultaneously, return all of them
            eqtim :: a -> Bool
eqtim = a -> Bool
forall a. Epsilon a => a -> Bool
nearZero (a -> Bool) -> (a -> a) -> a -> Bool
forall b c a. (b -> c) -> (a -> b) -> a -> c
. a -> a -> a
forall a. Num a => a -> a -> a
subtract a
t
            gridcoordsf :: [f a -> f Int]
gridcoordsf = ((a, f a -> f Int) -> f a -> f Int)
-> [(a, f a -> f Int)] -> [f a -> f Int]
forall a b. (a -> b) -> [a] -> [b]
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
fmap (a, f a -> f Int) -> f a -> f Int
forall a b. (a, b) -> b
snd ([(a, f a -> f Int)] -> [f a -> f Int])
-> [(a, f a -> f Int)] -> [f a -> f Int]
forall a b. (a -> b) -> a -> b
$ ((a, f a -> f Int) -> Bool)
-> [(a, f a -> f Int)] -> [(a, f a -> f Int)]
forall a. (a -> Bool) -> [a] -> [a]
filter (a -> Bool
eqtim (a -> Bool)
-> ((a, f a -> f Int) -> a) -> (a, f a -> f Int) -> Bool
forall b c a. (b -> c) -> (a -> b) -> a -> c
. (a, f a -> f Int) -> a
forall a b. (a, b) -> a
fst) ([(a, f a -> f Int)] -> [(a, f a -> f Int)])
-> [(a, f a -> f Int)] -> [(a, f a -> f Int)]
forall a b. (a -> b) -> a -> b
$ f (a, f a -> f Int) -> [(a, f a -> f Int)]
forall a. f a -> [a]
forall (t :: * -> *) a. Foldable t => t a -> [a]
toList f (a, f a -> f Int)
times
            -- elementwise error-compensated vector addition
            vadd :: f a -> f a -> f (a, a)
vadd f a
v f a
w = (Rep f -> (a, a)) -> f (a, a)
forall a. (Rep f -> a) -> f a
forall (f :: * -> *) a. Representable f => (Rep f -> a) -> f a
tabulate \Rep f
i -> a -> a -> a -> (a, a)
forall a. Num a => a -> a -> a -> (a, a)
add (f a
v f a -> Rep f -> a
forall a. f a -> Rep f -> a
! Rep f
i) (f a
w f a -> Rep f -> a
forall a. f a -> Rep f -> a
! Rep f
i) (f a
com f a -> Rep f -> a
forall a. f a -> Rep f -> a
! Rep f
i)
            -- update current position and compensator
            s :: f (a, a)
s = f a -> f a -> f (a, a)
vadd f a
cur (f a -> f (a, a)) -> f a -> f (a, a)
forall a b. (a -> b) -> a -> b
$ f a
dir f a -> (a -> a) -> f a
forall (f :: * -> *) a b. Functor f => f a -> (a -> b) -> f b
<&> (a -> a -> a
forall a. Num a => a -> a -> a
* a
t)
            icur_ :: f a
icur_ = (a, a) -> a
forall a b. (a, b) -> a
fst ((a, a) -> a) -> f (a, a) -> f a
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
<$> f (a, a)
s
            icom :: f a
icom = (a, a) -> a
forall a b. (a, b) -> b
snd ((a, a) -> a) -> f (a, a) -> f a
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
<$> f (a, a)
s
            -- properly round coordinates meant to be integers
            icur :: f a
icur = (Rep f -> a) -> f a
forall a. (Rep f -> a) -> f a
forall (f :: * -> *) a. Representable f => (Rep f -> a) -> f a
tabulate \Rep f
i ->
              let n :: a
n = f a
icur_ f a -> Rep f -> a
forall a. f a -> Rep f -> a
! Rep f
i
               in if a -> Bool
eqtim (a -> Bool) -> a -> Bool
forall a b. (a -> b) -> a -> b
$ (a, f a -> f Int) -> a
forall a b. (a, b) -> a
fst ((a, f a -> f Int) -> a) -> (a, f a -> f Int) -> a
forall a b. (a -> b) -> a -> b
$ f (a, f a -> f Int)
times f (a, f a -> f Int) -> Rep f -> (a, f a -> f Int)
forall a. f a -> Rep f -> a
! Rep f
i
                    then Int -> a
fi (Int -> a) -> Int -> a
forall a b. (a -> b) -> a -> b
$ a -> Int
forall b. Integral b => a -> b
forall a b. (RealFrac a, Integral b) => a -> b
round a
n
                    else a
n
         in I {itim :: a
itim = a
t, f a
icur :: f a
icur :: f a
icur, f a
icom :: f a
icom :: f a
icom, igrid :: [f Int]
igrid = [f a -> f Int]
gridcoordsf [f a -> f Int] -> ((f a -> f Int) -> f Int) -> [f Int]
forall (f :: * -> *) a b. Functor f => f a -> (a -> b) -> f b
<&> ((f a -> f Int) -> f a -> f Int
forall a b. (a -> b) -> a -> b
$ f a
icur)}
  cur <- f a -> ST s (STRef s (f a))
forall a s. a -> ST s (STRef s a)
new f a
start -- current position
  com <- new $ tabulate $ const 0 -- Kahan sum compensator for cur
  tot <- new (0, 0) -- (total time, compensator)
  -- IMPORTANT NOTE:
  -- at start, we go BACKWARD and find the first intersection
  -- and then go forward from there to resume the normal process
  -- this extremely hacky way of doing things is necessary
  -- to generate the grid coordinates of the starting point in a
  -- way that is consistent with the rest of the function
  do
    -- go backward and set cur, com and tot, but
    -- don't append output to the returned list
    let d = (a -> a -> a
forall a. Num a => a -> a -> a
* (-a
1)) (a -> a) -> f a -> f a
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
<$> f a
direction
    I {itim, icur, icom} <- inter (computesig d) d <$> read com <*> read cur
    write tot (-itim, 0)
    write cur icur
    write com icom
  -- loop
  fix \ST s [March f a]
this -> do
    let sig :: f Int
sig = f a -> f Int
forall {f :: * -> *} {b} {b}.
(Functor f, RealFrac b, Integral b) =>
f b -> f b
computesig f a
direction
    I {itim, icur, icom, igrid} <- f Int -> f a -> f a -> f a -> I f a
inter f Int
sig f a
direction (f a -> f a -> I f a) -> ST s (f a) -> ST s (f a -> I f a)
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
<$> STRef s (f a) -> ST s (f a)
forall s a. STRef s a -> ST s a
read STRef s (f a)
com ST s (f a -> I f a) -> ST s (f a) -> ST s (I f a)
forall a b. ST s (a -> b) -> ST s a -> ST s b
forall (f :: * -> *) a b. Applicative f => f (a -> b) -> f a -> f b
<*> STRef s (f a) -> ST s (f a)
forall s a. STRef s a -> ST s a
read STRef s (f a)
cur
    (t, _) <- modify tot (uncurry (add itim)) *> read tot
    write cur icur
    write com icom
    (March t icur igrid :) <$> this
{-# INLINEABLE march #-}
{-# SPECIALIZE march :: V3 Double -> V3 Double -> [March V3 Double] #-}
{-# SPECIALIZE march :: V3 Float -> V3 Float -> [March V3 Float] #-}