{-# LANGUAGE FlexibleContexts #-}
{-# LANGUAGE FlexibleInstances #-}
{-# LANGUAGE GeneralizedNewtypeDeriving #-}
{-# LANGUAGE MultiParamTypeClasses #-}
{-# LANGUAGE OverloadedStrings #-}
{-# LANGUAGE QuasiQuotes #-}
{-# LANGUAGE RecordWildCards #-}
{-# LANGUAGE ScopedTypeVariables #-}
{-# LANGUAGE Strict #-}
{-# LANGUAGE TemplateHaskell #-}
{-# LANGUAGE TypeApplications #-}
{-# LANGUAGE TypeFamilies #-}
{-# LANGUAGE UndecidableInstances #-}
{-# LANGUAGE ViewPatterns #-}
module Apecs.Physics.Collision
( defaultHandler
, mkBeginCB, mkSeparateCB, mkPreSolveCB, mkPostSolveCB
, addPostStepCallback
) where
import Apecs
import Apecs.Core
import Control.Monad
import Control.Monad.IO.Class (MonadIO, liftIO)
import qualified Data.IntMap as M
import Data.IORef
import qualified Data.Vector.Unboxed as U
import Foreign.ForeignPtr (withForeignPtr)
import Foreign.Ptr
import qualified Language.C.Inline as C
import Linear.V2
import Apecs.Physics.Body ()
import Apecs.Physics.Space ()
import Apecs.Physics.Types
C.context (phycsCtx `mappend` C.funCtx)
C.include "<chipmunk.h>"
C.include "<chipmunk_structs.h>"
defaultHandler :: CollisionHandler
defaultHandler :: CollisionHandler
defaultHandler = CollisionSource
-> Maybe BeginCB
-> Maybe SeparateCB
-> Maybe PreSolveCB
-> Maybe PostSolveCB
-> CollisionHandler
CollisionHandler (CollisionType -> CollisionSource
Wildcard CollisionType
0) forall a. Maybe a
Nothing forall a. Maybe a
Nothing forall a. Maybe a
Nothing forall a. Maybe a
Nothing
mkCollision :: Ptr Collision -> IO Collision
mkCollision :: Ptr Collision -> IO Collision
mkCollision Ptr Collision
arb = do
Double
nx <- forall a b. (Real a, Fractional b) => a -> b
realToFrac forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
<$> [C.exp| double { cpArbiterGetNormal($(cpArbiter* arb)).x } |]
Double
ny <- forall a b. (Real a, Fractional b) => a -> b
realToFrac forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
<$> [C.exp| double { cpArbiterGetNormal($(cpArbiter* arb)).y } |]
Int
ba <- forall a b. (Integral a, Num b) => a -> b
fromIntegral forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
<$> [C.block| unsigned int { CP_ARBITER_GET_BODIES($(cpArbiter* arb), ba, bb); return (intptr_t) (ba->userData); } |]
Int
bb <- forall a b. (Integral a, Num b) => a -> b
fromIntegral forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
<$> [C.block| unsigned int { CP_ARBITER_GET_BODIES($(cpArbiter* arb), ba, bb); return (intptr_t) (bb->userData); } |]
Int
sa <- forall a b. (Integral a, Num b) => a -> b
fromIntegral forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
<$> [C.block| unsigned int { CP_ARBITER_GET_SHAPES($(cpArbiter* arb), sa, sb); return (intptr_t) (sa->userData); } |]
Int
sb <- forall a b. (Integral a, Num b) => a -> b
fromIntegral forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
<$> [C.block| unsigned int { CP_ARBITER_GET_SHAPES($(cpArbiter* arb), sa, sb); return (intptr_t) (sb->userData); } |]
forall (m :: * -> *) a. Monad m => a -> m a
return forall a b. (a -> b) -> a -> b
$ Vec -> Entity -> Entity -> Entity -> Entity -> Collision
Collision (forall a. a -> a -> V2 a
V2 Double
nx Double
ny) (Int -> Entity
Entity Int
ba) (Int -> Entity
Entity Int
bb) (Int -> Entity
Entity Int
sa) (Int -> Entity
Entity Int
sb)
mkBeginCB :: MonadIO m => (Collision -> SystemT w IO Bool) -> SystemT w m BeginCB
mkBeginCB :: forall (m :: * -> *) w.
MonadIO m =>
(Collision -> SystemT w IO Bool) -> SystemT w m BeginCB
mkBeginCB Collision -> SystemT w IO Bool
sys = do
w
w <- forall r (m :: * -> *). MonadReader r m => m r
ask
let cb :: Ptr Collision -> Ptr FrnSpace -> CUInt -> IO CUChar
cb Ptr Collision
arb Ptr FrnSpace
_ CUInt
_ = do
Collision
col <- forall (m :: * -> *) a. MonadIO m => IO a -> m a
liftIO forall a b. (a -> b) -> a -> b
$ Ptr Collision -> IO Collision
mkCollision Ptr Collision
arb
Bool
r <- forall w (m :: * -> *) a. SystemT w m a -> w -> m a
runSystem (Collision -> SystemT w IO Bool
sys Collision
col) w
w
forall (m :: * -> *) a. Monad m => a -> m a
return forall b c a. (b -> c) -> (a -> b) -> a -> c
. forall a b. (Integral a, Num b) => a -> b
fromIntegral forall b c a. (b -> c) -> (a -> b) -> a -> c
. forall a. Enum a => a -> Int
fromEnum forall a b. (a -> b) -> a -> b
$ Bool
r
forall (m :: * -> *) a. Monad m => a -> m a
return ((Ptr Collision -> Ptr FrnSpace -> CUInt -> IO CUChar) -> BeginCB
BeginCB Ptr Collision -> Ptr FrnSpace -> CUInt -> IO CUChar
cb)
mkSeparateCB :: MonadIO m => (Collision -> SystemT w IO ()) -> SystemT w m SeparateCB
mkSeparateCB :: forall (m :: * -> *) w.
MonadIO m =>
(Collision -> SystemT w IO ()) -> SystemT w m SeparateCB
mkSeparateCB Collision -> SystemT w IO ()
sys = do
w
w <- forall r (m :: * -> *). MonadReader r m => m r
ask
let cb :: Ptr Collision -> Ptr FrnSpace -> CUInt -> IO ()
cb Ptr Collision
arb Ptr FrnSpace
_ CUInt
_ = do
Collision
col <- forall (m :: * -> *) a. MonadIO m => IO a -> m a
liftIO forall a b. (a -> b) -> a -> b
$ Ptr Collision -> IO Collision
mkCollision Ptr Collision
arb
forall w (m :: * -> *) a. SystemT w m a -> w -> m a
runSystem (Collision -> SystemT w IO ()
sys Collision
col) w
w
forall (m :: * -> *) a. Monad m => a -> m a
return ((Ptr Collision -> Ptr FrnSpace -> CUInt -> IO ()) -> SeparateCB
SeparateCB Ptr Collision -> Ptr FrnSpace -> CUInt -> IO ()
cb)
mkPreSolveCB :: MonadIO m => (Collision -> SystemT w IO Bool) -> SystemT w m PreSolveCB
mkPreSolveCB :: forall (m :: * -> *) w.
MonadIO m =>
(Collision -> SystemT w IO Bool) -> SystemT w m PreSolveCB
mkPreSolveCB Collision -> SystemT w IO Bool
sys = (\(BeginCB Ptr Collision -> Ptr FrnSpace -> CUInt -> IO CUChar
cb) -> (Ptr Collision -> Ptr FrnSpace -> CUInt -> IO CUChar) -> PreSolveCB
PreSolveCB Ptr Collision -> Ptr FrnSpace -> CUInt -> IO CUChar
cb) forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
<$> forall (m :: * -> *) w.
MonadIO m =>
(Collision -> SystemT w IO Bool) -> SystemT w m BeginCB
mkBeginCB Collision -> SystemT w IO Bool
sys
mkPostSolveCB :: MonadIO m => (Collision -> SystemT w IO ()) -> SystemT w m PostSolveCB
mkPostSolveCB :: forall (m :: * -> *) w.
MonadIO m =>
(Collision -> SystemT w IO ()) -> SystemT w m PostSolveCB
mkPostSolveCB Collision -> SystemT w IO ()
sys = (\(SeparateCB Ptr Collision -> Ptr FrnSpace -> CUInt -> IO ()
cb) -> (Ptr Collision -> Ptr FrnSpace -> CUInt -> IO ()) -> PostSolveCB
PostSolveCB Ptr Collision -> Ptr FrnSpace -> CUInt -> IO ()
cb) forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
<$> forall (m :: * -> *) w.
MonadIO m =>
(Collision -> SystemT w IO ()) -> SystemT w m SeparateCB
mkSeparateCB Collision -> SystemT w IO ()
sys
newCollisionHandler :: SpacePtr -> CollisionHandler -> Int -> IO (Ptr CollisionHandler)
newCollisionHandler :: SpacePtr -> CollisionHandler -> Int -> IO (Ptr CollisionHandler)
newCollisionHandler SpacePtr
spcPtr (CollisionHandler CollisionSource
source Maybe BeginCB
begin Maybe SeparateCB
separate Maybe PreSolveCB
presolve Maybe PostSolveCB
postsolve) (forall a b. (Integral a, Num b) => a -> b
fromIntegral -> CIntPtr
ety) =
forall a b. ForeignPtr a -> (Ptr a -> IO b) -> IO b
withForeignPtr SpacePtr
spcPtr forall a b. (a -> b) -> a -> b
$ \Ptr FrnSpace
space -> do
Ptr CollisionHandler
handler <- case CollisionSource
source of
Between (CollisionType CUIntPtr
cta) (CollisionType CUIntPtr
ctb)
-> [C.exp| cpCollisionHandler* {cpSpaceAddCollisionHandler($(cpSpace* space), $(uintptr_t cta), $(uintptr_t ctb))}|]
Wildcard (CollisionType CUIntPtr
ct)
-> [C.exp| cpCollisionHandler* {cpSpaceAddWildcardHandler($(cpSpace* space), $(uintptr_t ct))}|]
[C.exp| void { $(cpCollisionHandler* handler)->userData = (void*) $(intptr_t ety) }|]
forall (t :: * -> *) (m :: * -> *) a b.
(Foldable t, Monad m) =>
t a -> (a -> m b) -> m ()
forM_ Maybe BeginCB
beginforall a b. (a -> b) -> a -> b
$ \(BeginCB Ptr Collision -> Ptr FrnSpace -> CUInt -> IO CUChar
cb) -> do
FunPtr (Ptr Collision -> Ptr FrnSpace -> CUInt -> IO CUChar)
funPtr <- forall (m :: * -> *) a. MonadIO m => IO a -> m a
liftIOforall a b. (a -> b) -> a -> b
$ $(C.mkFunPtr [t| BeginFunc |]) Ptr Collision -> Ptr FrnSpace -> CUInt -> IO CUChar
cb
let fn :: Ptr ()
fn = forall a b. FunPtr a -> Ptr b
castFunPtrToPtr FunPtr (Ptr Collision -> Ptr FrnSpace -> CUInt -> IO CUChar)
funPtr
[C.exp| void { $(cpCollisionHandler* handler)->beginFunc = $(void* fn) }|]
forall (t :: * -> *) (m :: * -> *) a b.
(Foldable t, Monad m) =>
t a -> (a -> m b) -> m ()
forM_ Maybe SeparateCB
separateforall a b. (a -> b) -> a -> b
$ \(SeparateCB Ptr Collision -> Ptr FrnSpace -> CUInt -> IO ()
cb) -> do
FunPtr (Ptr Collision -> Ptr FrnSpace -> CUInt -> IO ())
funPtr <- forall (m :: * -> *) a. MonadIO m => IO a -> m a
liftIOforall a b. (a -> b) -> a -> b
$ $(C.mkFunPtr [t| SeparateFunc |]) Ptr Collision -> Ptr FrnSpace -> CUInt -> IO ()
cb
let fn :: Ptr ()
fn = forall a b. FunPtr a -> Ptr b
castFunPtrToPtr FunPtr (Ptr Collision -> Ptr FrnSpace -> CUInt -> IO ())
funPtr
[C.exp| void { $(cpCollisionHandler* handler)->separateFunc = $(void* fn) }|]
forall (t :: * -> *) (m :: * -> *) a b.
(Foldable t, Monad m) =>
t a -> (a -> m b) -> m ()
forM_ Maybe PreSolveCB
presolveforall a b. (a -> b) -> a -> b
$ \(PreSolveCB Ptr Collision -> Ptr FrnSpace -> CUInt -> IO CUChar
cb) -> do
FunPtr (Ptr Collision -> Ptr FrnSpace -> CUInt -> IO CUChar)
funPtr <- forall (m :: * -> *) a. MonadIO m => IO a -> m a
liftIOforall a b. (a -> b) -> a -> b
$ $(C.mkFunPtr [t| PreSolveFunc |]) Ptr Collision -> Ptr FrnSpace -> CUInt -> IO CUChar
cb
let fn :: Ptr ()
fn = forall a b. FunPtr a -> Ptr b
castFunPtrToPtr FunPtr (Ptr Collision -> Ptr FrnSpace -> CUInt -> IO CUChar)
funPtr
[C.exp| void { $(cpCollisionHandler* handler)->preSolveFunc = $(void* fn) }|]
forall (t :: * -> *) (m :: * -> *) a b.
(Foldable t, Monad m) =>
t a -> (a -> m b) -> m ()
forM_ Maybe PostSolveCB
postsolveforall a b. (a -> b) -> a -> b
$ \(PostSolveCB Ptr Collision -> Ptr FrnSpace -> CUInt -> IO ()
cb) -> do
FunPtr (Ptr Collision -> Ptr FrnSpace -> CUInt -> IO ())
funPtr <- forall (m :: * -> *) a. MonadIO m => IO a -> m a
liftIOforall a b. (a -> b) -> a -> b
$ $(C.mkFunPtr [t| PostSolveFunc |]) Ptr Collision -> Ptr FrnSpace -> CUInt -> IO ()
cb
let fn :: Ptr ()
fn = forall a b. FunPtr a -> Ptr b
castFunPtrToPtr FunPtr (Ptr Collision -> Ptr FrnSpace -> CUInt -> IO ())
funPtr
[C.exp| void { $(cpCollisionHandler* handler)->postSolveFunc = $(void* fn) }|]
forall (m :: * -> *) a. Monad m => a -> m a
return Ptr CollisionHandler
handler
destroyCollisionHandler :: Ptr CollisionHandler -> IO ()
destroyCollisionHandler :: Ptr CollisionHandler -> IO ()
destroyCollisionHandler = forall a. HasCallStack => [Char] -> a
error [Char]
"Destroy CollisionHandler not yet implemented"
instance Component CollisionHandler where
type Storage CollisionHandler = Space CollisionHandler
instance (MonadIO m, Has w m Physics) => Has w m CollisionHandler where
getStore :: SystemT w m (Storage CollisionHandler)
getStore = (forall a b. Space a -> Space b
cast :: Space Physics -> Space CollisionHandler) forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
<$> forall w (m :: * -> *) c. Has w m c => SystemT w m (Storage c)
getStore
instance (MonadIO m) => ExplSet m (Space CollisionHandler) where
explSet :: Space CollisionHandler
-> Int -> Elem (Space CollisionHandler) -> m ()
explSet sp :: Space CollisionHandler
sp@(Space IOMap BodyRecord
_ IOMap (Record Shape)
_ IOMap (Record Constraint)
_ IOMap (Record CollisionHandler)
hMap SpacePtr
spcPtr) Int
ety Elem (Space CollisionHandler)
handler = forall (m :: * -> *) a. MonadIO m => IO a -> m a
liftIO forall a b. (a -> b) -> a -> b
$ do
forall (m :: * -> *) s. ExplDestroy m s => s -> Int -> m ()
explDestroy Space CollisionHandler
sp Int
ety
Ptr CollisionHandler
hPtr <- SpacePtr -> CollisionHandler -> Int -> IO (Ptr CollisionHandler)
newCollisionHandler SpacePtr
spcPtr Elem (Space CollisionHandler)
handler Int
ety
forall a. IORef a -> (a -> a) -> IO ()
modifyIORef' IOMap (Record CollisionHandler)
hMap (forall a. Int -> a -> IntMap a -> IntMap a
M.insert Int
ety (forall a. Ptr a -> a -> Record a
Record Ptr CollisionHandler
hPtr Elem (Space CollisionHandler)
handler))
instance (MonadIO m) => ExplDestroy m (Space CollisionHandler) where
explDestroy :: Space CollisionHandler -> Int -> m ()
explDestroy (Space IOMap BodyRecord
_ IOMap (Record Shape)
_ IOMap (Record Constraint)
_ IOMap (Record CollisionHandler)
hMap SpacePtr
_) Int
ety = forall (m :: * -> *) a. MonadIO m => IO a -> m a
liftIO forall a b. (a -> b) -> a -> b
$ do
Maybe (Record CollisionHandler)
rd <- forall a. Int -> IntMap a -> Maybe a
M.lookup Int
ety forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
<$> forall a. IORef a -> IO a
readIORef IOMap (Record CollisionHandler)
hMap
forall (t :: * -> *) (m :: * -> *) a b.
(Foldable t, Monad m) =>
t a -> (a -> m b) -> m ()
forM_ Maybe (Record CollisionHandler)
rdforall a b. (a -> b) -> a -> b
$ \(Record Ptr CollisionHandler
c CollisionHandler
_) -> Ptr CollisionHandler -> IO ()
destroyCollisionHandler Ptr CollisionHandler
c forall (m :: * -> *) a b. Monad m => m a -> m b -> m b
>> forall a. IORef a -> (a -> a) -> IO ()
modifyIORef' IOMap (Record CollisionHandler)
hMap (forall a. Int -> IntMap a -> IntMap a
M.delete Int
ety)
instance (MonadIO m) => ExplMembers m (Space CollisionHandler) where
explMembers :: Space CollisionHandler -> m (Vector Int)
explMembers (Space IOMap BodyRecord
_ IOMap (Record Shape)
_ IOMap (Record Constraint)
_ IOMap (Record CollisionHandler)
hMap SpacePtr
_) = forall (m :: * -> *) a. MonadIO m => IO a -> m a
liftIO forall a b. (a -> b) -> a -> b
$ forall a. Unbox a => [a] -> Vector a
U.fromList forall b c a. (b -> c) -> (a -> b) -> a -> c
. forall a. IntMap a -> [Int]
M.keys forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
<$> forall a. IORef a -> IO a
readIORef IOMap (Record CollisionHandler)
hMap
instance (MonadIO m) => ExplGet m (Space CollisionHandler) where
explExists :: Space CollisionHandler -> Int -> m Bool
explExists (Space IOMap BodyRecord
_ IOMap (Record Shape)
_ IOMap (Record Constraint)
_ IOMap (Record CollisionHandler)
hMap SpacePtr
_) Int
ety = forall (m :: * -> *) a. MonadIO m => IO a -> m a
liftIO forall a b. (a -> b) -> a -> b
$ forall a. Int -> IntMap a -> Bool
M.member Int
ety forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
<$> forall a. IORef a -> IO a
readIORef IOMap (Record CollisionHandler)
hMap
explGet :: Space CollisionHandler -> Int -> m (Elem (Space CollisionHandler))
explGet (Space IOMap BodyRecord
_ IOMap (Record Shape)
_ IOMap (Record Constraint)
_ IOMap (Record CollisionHandler)
hMap SpacePtr
_) Int
ety = forall (m :: * -> *) a. MonadIO m => IO a -> m a
liftIO forall a b. (a -> b) -> a -> b
$ do
Just (Record Ptr CollisionHandler
_ CollisionHandler
handler) <- forall a. Int -> IntMap a -> Maybe a
M.lookup Int
ety forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
<$> forall a. IORef a -> IO a
readIORef IOMap (Record CollisionHandler)
hMap
forall (m :: * -> *) a. Monad m => a -> m a
return CollisionHandler
handler
addPostStepCallback :: (Has w m Physics, MonadIO m) => Int -> SystemT w IO () -> SystemT w m ()
addPostStepCallback :: forall w (m :: * -> *).
(Has w m Physics, MonadIO m) =>
Int -> SystemT w IO () -> SystemT w m ()
addPostStepCallback (forall a. Enum a => Int -> a
toEnum -> CInt
k) SystemT w IO ()
systemCallback= do
w
w <- forall r (m :: * -> *). MonadReader r m => m r
ask
let callback :: Ptr FrnSpace -> Ptr () -> Ptr () -> IO ()
callback Ptr FrnSpace
_ Ptr ()
_ Ptr ()
_ = forall w (m :: * -> *) a. SystemT w m a -> w -> m a
runSystem SystemT w IO ()
systemCallback w
w
(Space IOMap BodyRecord
_ IOMap (Record Shape)
_ IOMap (Record Constraint)
_ IOMap (Record CollisionHandler)
_ SpacePtr
spcPtr) :: Space Physics <- forall w (m :: * -> *) c. Has w m c => SystemT w m (Storage c)
getStore
forall (m :: * -> *) a. MonadIO m => IO a -> m a
liftIO forall a b. (a -> b) -> a -> b
$ forall a b. ForeignPtr a -> (Ptr a -> IO b) -> IO b
withForeignPtr SpacePtr
spcPtr forall a b. (a -> b) -> a -> b
$ \Ptr FrnSpace
space -> do
FunPtr (Ptr FrnSpace -> Ptr () -> Ptr () -> IO ())
funPtr <- forall (m :: * -> *) a. MonadIO m => IO a -> m a
liftIOforall a b. (a -> b) -> a -> b
$ $(C.mkFunPtr [t| Ptr FrnSpace -> Ptr () -> Ptr () -> IO () |]) Ptr FrnSpace -> Ptr () -> Ptr () -> IO ()
callback
let fn :: Ptr ()
fn = forall a b. FunPtr a -> Ptr b
castFunPtrToPtr FunPtr (Ptr FrnSpace -> Ptr () -> Ptr () -> IO ())
funPtr
[C.block| void {
int *data = 0;
cpSpaceAddPostStepCallback($(cpSpace *space), $(void*fn),(void*) (uintptr_t) $(int k), &data);
} |]
forall (f :: * -> *) a. Applicative f => a -> f a
pure ()