{-# 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.Constraint
  (
  ) where

import           Apecs
import           Apecs.Core
import           Control.Monad
import           Control.Monad.IO.Class (liftIO, MonadIO)
import qualified Data.IntMap         as M
import qualified Data.IntSet         as S
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.Space ()
import           Apecs.Physics.Types

C.context phycsCtx
C.include "<chipmunk.h>"

-- Constraint
newConstraint :: SpacePtr -> Ptr Body -> Ptr Body -> Int -> ConstraintType -> IO (Ptr Constraint)
newConstraint :: SpacePtr
-> Ptr Body
-> Ptr Body
-> Int
-> ConstraintType
-> IO (Ptr Constraint)
newConstraint SpacePtr
spacePtr Ptr Body
bodyA Ptr Body
bodyB (Int -> CIntPtr
forall a b. (Integral a, Num b) => a -> b
fromIntegral -> CIntPtr
ety)
              (PinJoint ((Double -> CDouble) -> BVec -> V2 CDouble
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
fmap Double -> CDouble
forall a b. (Real a, Fractional b) => a -> b
realToFrac -> V2 CDouble
ax CDouble
ay) ((Double -> CDouble) -> BVec -> V2 CDouble
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
fmap Double -> CDouble
forall a b. (Real a, Fractional b) => a -> b
realToFrac -> V2 CDouble
bx CDouble
by)) =
  SpacePtr
-> (Ptr FrnSpace -> IO (Ptr Constraint)) -> IO (Ptr Constraint)
forall a b. ForeignPtr a -> (Ptr a -> IO b) -> IO b
withForeignPtr SpacePtr
spacePtr ((Ptr FrnSpace -> IO (Ptr Constraint)) -> IO (Ptr Constraint))
-> (Ptr FrnSpace -> IO (Ptr Constraint)) -> IO (Ptr Constraint)
forall a b. (a -> b) -> a -> b
$ \Ptr FrnSpace
space -> [C.block| cpConstraint* {
    cpVect anchorA = cpv( $(double ax), $(double ay) );
    cpVect anchorB = cpv( $(double bx), $(double by) );
    cpConstraint* constraint = cpPinJointNew($(cpBody* bodyA), $(cpBody* bodyB),anchorA,anchorB);
    cpConstraintSetUserData(constraint, (void*) $(intptr_t ety));
    return cpSpaceAddConstraint($(cpSpace* space), constraint);
    } |]

newConstraint SpacePtr
spacePtr Ptr Body
bodyA Ptr Body
bodyB (Int -> CIntPtr
forall a b. (Integral a, Num b) => a -> b
fromIntegral -> CIntPtr
ety)
              (SlideJoint ((Double -> CDouble) -> BVec -> V2 CDouble
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
fmap Double -> CDouble
forall a b. (Real a, Fractional b) => a -> b
realToFrac -> V2 CDouble
ax CDouble
ay) ((Double -> CDouble) -> BVec -> V2 CDouble
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
fmap Double -> CDouble
forall a b. (Real a, Fractional b) => a -> b
realToFrac -> V2 CDouble
bx CDouble
by) (Double -> CDouble
forall a b. (Real a, Fractional b) => a -> b
realToFrac -> CDouble
min) (Double -> CDouble
forall a b. (Real a, Fractional b) => a -> b
realToFrac -> CDouble
max)) =
  SpacePtr
-> (Ptr FrnSpace -> IO (Ptr Constraint)) -> IO (Ptr Constraint)
forall a b. ForeignPtr a -> (Ptr a -> IO b) -> IO b
withForeignPtr SpacePtr
spacePtr ((Ptr FrnSpace -> IO (Ptr Constraint)) -> IO (Ptr Constraint))
-> (Ptr FrnSpace -> IO (Ptr Constraint)) -> IO (Ptr Constraint)
forall a b. (a -> b) -> a -> b
$ \Ptr FrnSpace
space -> [C.block| cpConstraint* {
    cpVect anchorA = cpv( $(double ax), $(double ay) );
    cpVect anchorB = cpv( $(double bx), $(double by) );
    cpConstraint* constraint = cpSlideJointNew($(cpBody* bodyA), $(cpBody* bodyB),anchorA,anchorB,$(double min),$(double max));
    cpConstraintSetUserData(constraint, (void*) $(intptr_t ety));
    return cpSpaceAddConstraint($(cpSpace* space), constraint);
    } |]

newConstraint SpacePtr
spacePtr Ptr Body
bodyA Ptr Body
bodyB (Int -> CIntPtr
forall a b. (Integral a, Num b) => a -> b
fromIntegral -> CIntPtr
ety)
              (PivotJoint ((Double -> CDouble) -> BVec -> V2 CDouble
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
fmap Double -> CDouble
forall a b. (Real a, Fractional b) => a -> b
realToFrac -> V2 CDouble
x CDouble
y)) =
  SpacePtr
-> (Ptr FrnSpace -> IO (Ptr Constraint)) -> IO (Ptr Constraint)
forall a b. ForeignPtr a -> (Ptr a -> IO b) -> IO b
withForeignPtr SpacePtr
spacePtr ((Ptr FrnSpace -> IO (Ptr Constraint)) -> IO (Ptr Constraint))
-> (Ptr FrnSpace -> IO (Ptr Constraint)) -> IO (Ptr Constraint)
forall a b. (a -> b) -> a -> b
$ \Ptr FrnSpace
space -> [C.block| cpConstraint* {
    cpVect anchor = cpv( $(double x), $(double y) );
    cpConstraint* constraint = cpPivotJointNew($(cpBody* bodyA), $(cpBody* bodyB), anchor);
    cpConstraintSetUserData(constraint, (void*) $(intptr_t ety));
    return cpSpaceAddConstraint($(cpSpace* space), constraint);
    } |]

newConstraint SpacePtr
spacePtr Ptr Body
bodyA Ptr Body
bodyB (Int -> CIntPtr
forall a b. (Integral a, Num b) => a -> b
fromIntegral -> CIntPtr
ety)
              (PivotJoint2 ((Double -> CDouble) -> BVec -> V2 CDouble
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
fmap Double -> CDouble
forall a b. (Real a, Fractional b) => a -> b
realToFrac -> V2 CDouble
ax CDouble
ay) ((Double -> CDouble) -> BVec -> V2 CDouble
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
fmap Double -> CDouble
forall a b. (Real a, Fractional b) => a -> b
realToFrac -> V2 CDouble
bx CDouble
by)) =
  SpacePtr
-> (Ptr FrnSpace -> IO (Ptr Constraint)) -> IO (Ptr Constraint)
forall a b. ForeignPtr a -> (Ptr a -> IO b) -> IO b
withForeignPtr SpacePtr
spacePtr ((Ptr FrnSpace -> IO (Ptr Constraint)) -> IO (Ptr Constraint))
-> (Ptr FrnSpace -> IO (Ptr Constraint)) -> IO (Ptr Constraint)
forall a b. (a -> b) -> a -> b
$ \Ptr FrnSpace
space -> [C.block| cpConstraint* {
    cpVect va = cpv( $(double ax), $(double ay) );
    cpVect vb = cpv( $(double bx), $(double by) );
    cpConstraint* constraint = cpPivotJointNew2($(cpBody* bodyA), $(cpBody* bodyB), va, vb);
    cpConstraintSetUserData(constraint, (void*) $(intptr_t ety));
    return cpSpaceAddConstraint($(cpSpace* space), constraint);
    } |]

newConstraint SpacePtr
spacePtr Ptr Body
bodyA Ptr Body
bodyB (Int -> CIntPtr
forall a b. (Integral a, Num b) => a -> b
fromIntegral -> CIntPtr
ety)
              (GrooveJoint ((Double -> CDouble) -> BVec -> V2 CDouble
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
fmap Double -> CDouble
forall a b. (Real a, Fractional b) => a -> b
realToFrac -> V2 CDouble
ax CDouble
ay) ((Double -> CDouble) -> BVec -> V2 CDouble
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
fmap Double -> CDouble
forall a b. (Real a, Fractional b) => a -> b
realToFrac -> V2 CDouble
bx CDouble
by) ((Double -> CDouble) -> BVec -> V2 CDouble
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
fmap Double -> CDouble
forall a b. (Real a, Fractional b) => a -> b
realToFrac -> V2 CDouble
ancx CDouble
ancy)) =
  SpacePtr
-> (Ptr FrnSpace -> IO (Ptr Constraint)) -> IO (Ptr Constraint)
forall a b. ForeignPtr a -> (Ptr a -> IO b) -> IO b
withForeignPtr SpacePtr
spacePtr ((Ptr FrnSpace -> IO (Ptr Constraint)) -> IO (Ptr Constraint))
-> (Ptr FrnSpace -> IO (Ptr Constraint)) -> IO (Ptr Constraint)
forall a b. (a -> b) -> a -> b
$ \Ptr FrnSpace
space -> [C.block| cpConstraint* {
    cpVect va = cpv( $(double ax), $(double ay) );
    cpVect vb = cpv( $(double bx), $(double by) );
    cpVect anchor = cpv( $(double ancx), $(double ancy) );
    cpConstraint* constraint = cpGrooveJointNew($(cpBody* bodyA), $(cpBody* bodyB), va, vb, anchor);
    cpConstraintSetUserData(constraint, (void*) $(intptr_t ety));
    return cpSpaceAddConstraint($(cpSpace* space), constraint);
    } |]

newConstraint SpacePtr
spacePtr Ptr Body
bodyA Ptr Body
bodyB (Int -> CIntPtr
forall a b. (Integral a, Num b) => a -> b
fromIntegral -> CIntPtr
ety)
              (DampedSpring ((Double -> CDouble) -> BVec -> V2 CDouble
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
fmap Double -> CDouble
forall a b. (Real a, Fractional b) => a -> b
realToFrac -> V2 CDouble
ax CDouble
ay) ((Double -> CDouble) -> BVec -> V2 CDouble
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
fmap Double -> CDouble
forall a b. (Real a, Fractional b) => a -> b
realToFrac -> V2 CDouble
bx CDouble
by) (Double -> CDouble
forall a b. (Real a, Fractional b) => a -> b
realToFrac -> CDouble
rl) (Double -> CDouble
forall a b. (Real a, Fractional b) => a -> b
realToFrac -> CDouble
stf) (Double -> CDouble
forall a b. (Real a, Fractional b) => a -> b
realToFrac -> CDouble
damping)) =
  SpacePtr
-> (Ptr FrnSpace -> IO (Ptr Constraint)) -> IO (Ptr Constraint)
forall a b. ForeignPtr a -> (Ptr a -> IO b) -> IO b
withForeignPtr SpacePtr
spacePtr ((Ptr FrnSpace -> IO (Ptr Constraint)) -> IO (Ptr Constraint))
-> (Ptr FrnSpace -> IO (Ptr Constraint)) -> IO (Ptr Constraint)
forall a b. (a -> b) -> a -> b
$ \Ptr FrnSpace
space -> [C.block| cpConstraint* {
    cpVect va = cpv( $(double ax), $(double ay) );
    cpVect vb = cpv( $(double bx), $(double by) );
    cpConstraint* constraint = cpDampedSpringNew($(cpBody* bodyA), $(cpBody* bodyB), va, vb, $(double rl), $(double stf), $(double damping));
    cpConstraintSetUserData(constraint, (void*) $(intptr_t ety));
    return cpSpaceAddConstraint($(cpSpace* space), constraint);
    } |]

newConstraint SpacePtr
spacePtr Ptr Body
bodyA Ptr Body
bodyB (Int -> CIntPtr
forall a b. (Integral a, Num b) => a -> b
fromIntegral -> CIntPtr
ety)
              (DampedRotarySpring (Double -> CDouble
forall a b. (Real a, Fractional b) => a -> b
realToFrac -> CDouble
ra) (Double -> CDouble
forall a b. (Real a, Fractional b) => a -> b
realToFrac -> CDouble
stf) (Double -> CDouble
forall a b. (Real a, Fractional b) => a -> b
realToFrac -> CDouble
damping)) =
  SpacePtr
-> (Ptr FrnSpace -> IO (Ptr Constraint)) -> IO (Ptr Constraint)
forall a b. ForeignPtr a -> (Ptr a -> IO b) -> IO b
withForeignPtr SpacePtr
spacePtr ((Ptr FrnSpace -> IO (Ptr Constraint)) -> IO (Ptr Constraint))
-> (Ptr FrnSpace -> IO (Ptr Constraint)) -> IO (Ptr Constraint)
forall a b. (a -> b) -> a -> b
$ \Ptr FrnSpace
space -> [C.block| cpConstraint* {
    cpConstraint* constraint = cpDampedRotarySpringNew($(cpBody* bodyA), $(cpBody* bodyB), $(double ra), $(double stf), $(double damping));
    cpConstraintSetUserData(constraint, (void*) $(intptr_t ety));
    return cpSpaceAddConstraint($(cpSpace* space), constraint);
    } |]

newConstraint SpacePtr
spacePtr Ptr Body
bodyA Ptr Body
bodyB (Int -> CIntPtr
forall a b. (Integral a, Num b) => a -> b
fromIntegral -> CIntPtr
ety)
              (RotaryLimitJoint  (Double -> CDouble
forall a b. (Real a, Fractional b) => a -> b
realToFrac -> CDouble
min) (Double -> CDouble
forall a b. (Real a, Fractional b) => a -> b
realToFrac -> CDouble
max)) =
  SpacePtr
-> (Ptr FrnSpace -> IO (Ptr Constraint)) -> IO (Ptr Constraint)
forall a b. ForeignPtr a -> (Ptr a -> IO b) -> IO b
withForeignPtr SpacePtr
spacePtr ((Ptr FrnSpace -> IO (Ptr Constraint)) -> IO (Ptr Constraint))
-> (Ptr FrnSpace -> IO (Ptr Constraint)) -> IO (Ptr Constraint)
forall a b. (a -> b) -> a -> b
$ \Ptr FrnSpace
space -> [C.block| cpConstraint* {
    cpConstraint* constraint = cpRotaryLimitJointNew($(cpBody* bodyA), $(cpBody* bodyB), $(double min), $(double max));
    cpConstraintSetUserData(constraint, (void*) $(intptr_t ety));
    return cpSpaceAddConstraint($(cpSpace* space), constraint);
    } |]

newConstraint SpacePtr
spacePtr Ptr Body
bodyA Ptr Body
bodyB (Int -> CIntPtr
forall a b. (Integral a, Num b) => a -> b
fromIntegral -> CIntPtr
ety)
              (RatchetJoint (Double -> CDouble
forall a b. (Real a, Fractional b) => a -> b
realToFrac -> CDouble
phase) (Double -> CDouble
forall a b. (Real a, Fractional b) => a -> b
realToFrac -> CDouble
ratchet)) =
  SpacePtr
-> (Ptr FrnSpace -> IO (Ptr Constraint)) -> IO (Ptr Constraint)
forall a b. ForeignPtr a -> (Ptr a -> IO b) -> IO b
withForeignPtr SpacePtr
spacePtr ((Ptr FrnSpace -> IO (Ptr Constraint)) -> IO (Ptr Constraint))
-> (Ptr FrnSpace -> IO (Ptr Constraint)) -> IO (Ptr Constraint)
forall a b. (a -> b) -> a -> b
$ \Ptr FrnSpace
space -> [C.block| cpConstraint* {
    cpConstraint* constraint = cpRatchetJointNew($(cpBody* bodyA), $(cpBody* bodyB), $(double phase), $(double ratchet));
    cpConstraintSetUserData(constraint, (void*) $(intptr_t ety));
    return cpSpaceAddConstraint($(cpSpace* space), constraint);
    } |]

newConstraint SpacePtr
spacePtr Ptr Body
bodyA Ptr Body
bodyB (Int -> CIntPtr
forall a b. (Integral a, Num b) => a -> b
fromIntegral -> CIntPtr
ety)
              (GearJoint (Double -> CDouble
forall a b. (Real a, Fractional b) => a -> b
realToFrac -> CDouble
phase) (Double -> CDouble
forall a b. (Real a, Fractional b) => a -> b
realToFrac -> CDouble
ratio)) =
  SpacePtr
-> (Ptr FrnSpace -> IO (Ptr Constraint)) -> IO (Ptr Constraint)
forall a b. ForeignPtr a -> (Ptr a -> IO b) -> IO b
withForeignPtr SpacePtr
spacePtr ((Ptr FrnSpace -> IO (Ptr Constraint)) -> IO (Ptr Constraint))
-> (Ptr FrnSpace -> IO (Ptr Constraint)) -> IO (Ptr Constraint)
forall a b. (a -> b) -> a -> b
$ \Ptr FrnSpace
space -> [C.block| cpConstraint* {
    cpConstraint* constraint = cpGearJointNew($(cpBody* bodyA), $(cpBody* bodyB), $(double phase), $(double ratio));
    cpConstraintSetUserData(constraint, (void*) $(intptr_t ety));
    return cpSpaceAddConstraint($(cpSpace* space), constraint);
    } |]

newConstraint SpacePtr
spacePtr Ptr Body
bodyA Ptr Body
bodyB (Int -> CIntPtr
forall a b. (Integral a, Num b) => a -> b
fromIntegral -> CIntPtr
ety)
              (SimpleMotor (Double -> CDouble
forall a b. (Real a, Fractional b) => a -> b
realToFrac -> CDouble
rate)) =
  SpacePtr
-> (Ptr FrnSpace -> IO (Ptr Constraint)) -> IO (Ptr Constraint)
forall a b. ForeignPtr a -> (Ptr a -> IO b) -> IO b
withForeignPtr SpacePtr
spacePtr ((Ptr FrnSpace -> IO (Ptr Constraint)) -> IO (Ptr Constraint))
-> (Ptr FrnSpace -> IO (Ptr Constraint)) -> IO (Ptr Constraint)
forall a b. (a -> b) -> a -> b
$ \Ptr FrnSpace
space -> [C.block| cpConstraint* {
    cpConstraint* constraint = cpSimpleMotorNew($(cpBody* bodyA), $(cpBody* bodyB), $(double rate));
    cpConstraintSetUserData(constraint, (void*) $(intptr_t ety));
    return cpSpaceAddConstraint($(cpSpace* space), constraint);
    } |]

destroyConstraint :: SpacePtr -> Ptr Constraint -> IO ()
destroyConstraint :: SpacePtr -> Ptr Constraint -> IO ()
destroyConstraint SpacePtr
spacePtr Ptr Constraint
constraintPtr = SpacePtr -> (Ptr FrnSpace -> IO ()) -> IO ()
forall a b. ForeignPtr a -> (Ptr a -> IO b) -> IO b
withForeignPtr SpacePtr
spacePtr ((Ptr FrnSpace -> IO ()) -> IO ())
-> (Ptr FrnSpace -> IO ()) -> IO ()
forall a b. (a -> b) -> a -> b
$ \Ptr FrnSpace
space -> [C.block| void {
  cpConstraint *constraint = $(cpConstraint* constraintPtr);
  cpSpaceRemoveConstraint($(cpSpace* space), constraint);
  cpConstraintFree(constraint); }|]

instance Component Constraint where
  type Storage Constraint = Space Constraint

instance (MonadIO m, Has w m Physics) => Has w m Constraint where
  getStore :: SystemT w m (Storage Constraint)
getStore = (Space Physics -> Space Constraint
forall a b. Space a -> Space b
cast :: Space Physics -> Space Constraint) (Space Physics -> Space Constraint)
-> SystemT w m (Space Physics) -> SystemT w m (Space Constraint)
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
<$> SystemT w m (Space Physics)
forall w (m :: * -> *) c. Has w m c => SystemT w m (Storage c)
getStore

instance MonadIO m => ExplSet m (Space Constraint) where
  explSet :: Space Constraint -> Int -> Elem (Space Constraint) -> m ()
explSet sp :: Space Constraint
sp@(Space IOMap BodyRecord
bMap IOMap (Record Shape)
_ IOMap (Record Constraint)
cMap IOMap (Record CollisionHandler)
_ SpacePtr
spcPtr) Int
cEty cons :: Elem (Space Constraint)
cons@(Constraint (Entity bEtyA) (Entity bEtyB) ctype) = IO () -> m ()
forall (m :: * -> *) a. MonadIO m => IO a -> m a
liftIO (IO () -> m ()) -> IO () -> m ()
forall a b. (a -> b) -> a -> b
$ do
    Space Constraint -> Int -> IO ()
forall (m :: * -> *) s. ExplDestroy m s => s -> Int -> m ()
explDestroy Space Constraint
sp Int
cEty
    Maybe BodyRecord
mBrA <- Int -> IntMap BodyRecord -> Maybe BodyRecord
forall a. Int -> IntMap a -> Maybe a
M.lookup Int
bEtyA (IntMap BodyRecord -> Maybe BodyRecord)
-> IO (IntMap BodyRecord) -> IO (Maybe BodyRecord)
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
<$> IOMap BodyRecord -> IO (IntMap BodyRecord)
forall a. IORef a -> IO a
readIORef IOMap BodyRecord
bMap
    Maybe BodyRecord
mBrB <- Int -> IntMap BodyRecord -> Maybe BodyRecord
forall a. Int -> IntMap a -> Maybe a
M.lookup Int
bEtyB (IntMap BodyRecord -> Maybe BodyRecord)
-> IO (IntMap BodyRecord) -> IO (Maybe BodyRecord)
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
<$> IOMap BodyRecord -> IO (IntMap BodyRecord)
forall a. IORef a -> IO a
readIORef IOMap BodyRecord
bMap
    case (Maybe BodyRecord
mBrA,Maybe BodyRecord
mBrB) of
      (Just BodyRecord
brA, Just BodyRecord
brB) -> do
        Ptr Constraint
cPtr <- SpacePtr
-> Ptr Body
-> Ptr Body
-> Int
-> ConstraintType
-> IO (Ptr Constraint)
newConstraint SpacePtr
spcPtr (BodyRecord -> Ptr Body
brPtr BodyRecord
brA) (BodyRecord -> Ptr Body
brPtr BodyRecord
brB) Int
cEty ConstraintType
ctype

        IOMap (Record Constraint)
-> (IntMap (Record Constraint) -> IntMap (Record Constraint))
-> IO ()
forall a. IORef a -> (a -> a) -> IO ()
modifyIORef' IOMap (Record Constraint)
cMap (Int
-> Record Constraint
-> IntMap (Record Constraint)
-> IntMap (Record Constraint)
forall a. Int -> a -> IntMap a -> IntMap a
M.insert Int
cEty (Ptr Constraint -> Constraint -> Record Constraint
forall a. Ptr a -> a -> Record a
Record Ptr Constraint
cPtr Elem (Space Constraint)
Constraint
cons))
        IORef IntSet -> (IntSet -> IntSet) -> IO ()
forall a. IORef a -> (a -> a) -> IO ()
modifyIORef' (BodyRecord -> IORef IntSet
brConstraints BodyRecord
brA) (Int -> IntSet -> IntSet
S.insert Int
cEty)
        IORef IntSet -> (IntSet -> IntSet) -> IO ()
forall a. IORef a -> (a -> a) -> IO ()
modifyIORef' (BodyRecord -> IORef IntSet
brConstraints BodyRecord
brB) (Int -> IntSet -> IntSet
S.insert Int
cEty)

      (Maybe BodyRecord, Maybe BodyRecord)
_ -> () -> IO ()
forall (m :: * -> *) a. Monad m => a -> m a
return ()

instance MonadIO m => ExplDestroy m (Space Constraint) where
  explDestroy :: Space Constraint -> Int -> m ()
explDestroy (Space IOMap BodyRecord
bMap IOMap (Record Shape)
_ IOMap (Record Constraint)
cMap IOMap (Record CollisionHandler)
_ SpacePtr
spc) Int
cEty = IO () -> m ()
forall (m :: * -> *) a. MonadIO m => IO a -> m a
liftIO (IO () -> m ()) -> IO () -> m ()
forall a b. (a -> b) -> a -> b
$ do
    Maybe (Record Constraint)
rd <- Int -> IntMap (Record Constraint) -> Maybe (Record Constraint)
forall a. Int -> IntMap a -> Maybe a
M.lookup Int
cEty (IntMap (Record Constraint) -> Maybe (Record Constraint))
-> IO (IntMap (Record Constraint))
-> IO (Maybe (Record Constraint))
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
<$> IOMap (Record Constraint) -> IO (IntMap (Record Constraint))
forall a. IORef a -> IO a
readIORef IOMap (Record Constraint)
cMap
    Maybe (Record Constraint) -> (Record Constraint -> IO ()) -> IO ()
forall (t :: * -> *) (m :: * -> *) a b.
(Foldable t, Monad m) =>
t a -> (a -> m b) -> m ()
forM_ Maybe (Record Constraint)
rd ((Record Constraint -> IO ()) -> IO ())
-> (Record Constraint -> IO ()) -> IO ()
forall a b. (a -> b) -> a -> b
$ \(Record Ptr Constraint
cPtr (Constraint (Entity Int
bEtyA) (Entity Int
bEtyB) ConstraintType
_)) -> do
      IntMap BodyRecord
bMap' <- IOMap BodyRecord -> IO (IntMap BodyRecord)
forall a. IORef a -> IO a
readIORef IOMap BodyRecord
bMap
      let rmConstraint :: BodyRecord -> IO ()
rmConstraint BodyRecord
ref = IORef IntSet -> (IntSet -> IntSet) -> IO ()
forall a. IORef a -> (a -> a) -> IO ()
modifyIORef' (BodyRecord -> IORef IntSet
brConstraints BodyRecord
ref) (Int -> IntSet -> IntSet
S.delete Int
cEty)
      (BodyRecord -> IO ()) -> Maybe BodyRecord -> IO ()
forall (t :: * -> *) (m :: * -> *) a b.
(Foldable t, Monad m) =>
(a -> m b) -> t a -> m ()
mapM_ BodyRecord -> IO ()
rmConstraint (Int -> IntMap BodyRecord -> Maybe BodyRecord
forall a. Int -> IntMap a -> Maybe a
M.lookup Int
bEtyA IntMap BodyRecord
bMap')
      (BodyRecord -> IO ()) -> Maybe BodyRecord -> IO ()
forall (t :: * -> *) (m :: * -> *) a b.
(Foldable t, Monad m) =>
(a -> m b) -> t a -> m ()
mapM_ BodyRecord -> IO ()
rmConstraint (Int -> IntMap BodyRecord -> Maybe BodyRecord
forall a. Int -> IntMap a -> Maybe a
M.lookup Int
bEtyB IntMap BodyRecord
bMap')
      IOMap (Record Constraint)
-> (IntMap (Record Constraint) -> IntMap (Record Constraint))
-> IO ()
forall a. IORef a -> (a -> a) -> IO ()
modifyIORef' IOMap (Record Constraint)
cMap ((IntMap (Record Constraint) -> IntMap (Record Constraint))
 -> IO ())
-> (IntMap (Record Constraint) -> IntMap (Record Constraint))
-> IO ()
forall a b. (a -> b) -> a -> b
$ Int -> IntMap (Record Constraint) -> IntMap (Record Constraint)
forall a. Int -> IntMap a -> IntMap a
M.delete Int
cEty
      SpacePtr -> Ptr Constraint -> IO ()
destroyConstraint SpacePtr
spc Ptr Constraint
cPtr

instance MonadIO m => ExplMembers m (Space Constraint) where
  explMembers :: Space Constraint -> m (Vector Int)
explMembers (Space IOMap BodyRecord
_ IOMap (Record Shape)
_ IOMap (Record Constraint)
cMap IOMap (Record CollisionHandler)
_ SpacePtr
_) = IO (Vector Int) -> m (Vector Int)
forall (m :: * -> *) a. MonadIO m => IO a -> m a
liftIO (IO (Vector Int) -> m (Vector Int))
-> IO (Vector Int) -> m (Vector Int)
forall a b. (a -> b) -> a -> b
$ [Int] -> Vector Int
forall a. Unbox a => [a] -> Vector a
U.fromList ([Int] -> Vector Int)
-> (IntMap (Record Constraint) -> [Int])
-> IntMap (Record Constraint)
-> Vector Int
forall b c a. (b -> c) -> (a -> b) -> a -> c
. IntMap (Record Constraint) -> [Int]
forall a. IntMap a -> [Int]
M.keys (IntMap (Record Constraint) -> Vector Int)
-> IO (IntMap (Record Constraint)) -> IO (Vector Int)
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
<$> IOMap (Record Constraint) -> IO (IntMap (Record Constraint))
forall a. IORef a -> IO a
readIORef IOMap (Record Constraint)
cMap

instance MonadIO m => ExplGet m (Space Constraint) where
  explExists :: Space Constraint -> Int -> m Bool
explExists (Space IOMap BodyRecord
_ IOMap (Record Shape)
_ IOMap (Record Constraint)
cMap IOMap (Record CollisionHandler)
_ SpacePtr
_) Int
ety = IO Bool -> m Bool
forall (m :: * -> *) a. MonadIO m => IO a -> m a
liftIO (IO Bool -> m Bool) -> IO Bool -> m Bool
forall a b. (a -> b) -> a -> b
$ Int -> IntMap (Record Constraint) -> Bool
forall a. Int -> IntMap a -> Bool
M.member Int
ety (IntMap (Record Constraint) -> Bool)
-> IO (IntMap (Record Constraint)) -> IO Bool
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
<$> IOMap (Record Constraint) -> IO (IntMap (Record Constraint))
forall a. IORef a -> IO a
readIORef IOMap (Record Constraint)
cMap
  explGet :: Space Constraint -> Int -> m (Elem (Space Constraint))
explGet    (Space IOMap BodyRecord
_ IOMap (Record Shape)
_ IOMap (Record Constraint)
cMap IOMap (Record CollisionHandler)
_ SpacePtr
_) Int
ety = IO Constraint -> m Constraint
forall (m :: * -> *) a. MonadIO m => IO a -> m a
liftIO (IO Constraint -> m Constraint) -> IO Constraint -> m Constraint
forall a b. (a -> b) -> a -> b
$ do
    Just (Record Ptr Constraint
_ Constraint
cons)  <- Int -> IntMap (Record Constraint) -> Maybe (Record Constraint)
forall a. Int -> IntMap a -> Maybe a
M.lookup Int
ety (IntMap (Record Constraint) -> Maybe (Record Constraint))
-> IO (IntMap (Record Constraint))
-> IO (Maybe (Record Constraint))
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
<$> IOMap (Record Constraint) -> IO (IntMap (Record Constraint))
forall a. IORef a -> IO a
readIORef IOMap (Record Constraint)
cMap
    Constraint -> IO Constraint
forall (m :: * -> *) a. Monad m => a -> m a
return Constraint
cons

-- MaxForce
getMaxForce :: Ptr Constraint -> IO Double
getMaxForce :: Ptr Constraint -> IO Double
getMaxForce Ptr Constraint
c = CDouble -> Double
forall a b. (Real a, Fractional b) => a -> b
realToFrac (CDouble -> Double) -> IO CDouble -> IO Double
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
<$> [C.exp| double { cpConstraintGetMaxForce ($(cpConstraint* c)) } |]

setMaxForce :: Ptr Constraint -> Double -> IO ()
setMaxForce :: Ptr Constraint -> Double -> IO ()
setMaxForce Ptr Constraint
c (Double -> CDouble
forall a b. (Real a, Fractional b) => a -> b
realToFrac -> CDouble
maxForce) = [C.exp| void { cpConstraintSetMaxForce($(cpConstraint* c), $(double maxForce)); } |]

instance Component MaxForce where
  type Storage MaxForce = Space MaxForce

instance (MonadIO m, Has w m Physics) => Has w m MaxForce where
  getStore :: SystemT w m (Storage MaxForce)
getStore = (Space Physics -> Space MaxForce
forall a b. Space a -> Space b
cast :: Space Physics -> Space MaxForce) (Space Physics -> Space MaxForce)
-> SystemT w m (Space Physics) -> SystemT w m (Space MaxForce)
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
<$> SystemT w m (Space Physics)
forall w (m :: * -> *) c. Has w m c => SystemT w m (Storage c)
getStore

instance MonadIO m => ExplMembers m (Space MaxForce) where
  explMembers :: Space MaxForce -> m (Vector Int)
explMembers Space MaxForce
s = Space Constraint -> m (Vector Int)
forall (m :: * -> *) s. ExplMembers m s => s -> m (Vector Int)
explMembers (Space MaxForce -> Space Constraint
forall a b. Space a -> Space b
cast Space MaxForce
s :: Space Constraint)

instance MonadIO m => ExplSet m (Space MaxForce) where
  explSet :: Space MaxForce -> Int -> Elem (Space MaxForce) -> m ()
explSet (Space IOMap BodyRecord
_ IOMap (Record Shape)
_ IOMap (Record Constraint)
cMap IOMap (Record CollisionHandler)
_ SpacePtr
_) Int
ety (MaxForce vec) = IO () -> m ()
forall (m :: * -> *) a. MonadIO m => IO a -> m a
liftIO (IO () -> m ()) -> IO () -> m ()
forall a b. (a -> b) -> a -> b
$ do
    Maybe (Record Constraint)
rd <- Int -> IntMap (Record Constraint) -> Maybe (Record Constraint)
forall a. Int -> IntMap a -> Maybe a
M.lookup Int
ety (IntMap (Record Constraint) -> Maybe (Record Constraint))
-> IO (IntMap (Record Constraint))
-> IO (Maybe (Record Constraint))
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
<$> IOMap (Record Constraint) -> IO (IntMap (Record Constraint))
forall a. IORef a -> IO a
readIORef IOMap (Record Constraint)
cMap
    case Maybe (Record Constraint)
rd of
      Maybe (Record Constraint)
Nothing -> () -> IO ()
forall (m :: * -> *) a. Monad m => a -> m a
return ()
      Just (Record Ptr Constraint
c Constraint
_) -> Ptr Constraint -> Double -> IO ()
setMaxForce Ptr Constraint
c Double
vec

instance MonadIO m => ExplGet m (Space MaxForce) where
  explExists :: Space MaxForce -> Int -> m Bool
explExists Space MaxForce
s Int
ety = Space Constraint -> Int -> m Bool
forall (m :: * -> *) s. ExplGet m s => s -> Int -> m Bool
explExists (Space MaxForce -> Space Constraint
forall a b. Space a -> Space b
cast Space MaxForce
s :: Space Constraint) Int
ety
  explGet :: Space MaxForce -> Int -> m (Elem (Space MaxForce))
explGet (Space IOMap BodyRecord
_ IOMap (Record Shape)
_ IOMap (Record Constraint)
cMap IOMap (Record CollisionHandler)
_ SpacePtr
_) Int
ety = IO MaxForce -> m MaxForce
forall (m :: * -> *) a. MonadIO m => IO a -> m a
liftIO (IO MaxForce -> m MaxForce) -> IO MaxForce -> m MaxForce
forall a b. (a -> b) -> a -> b
$ do
    Just (Record Ptr Constraint
c Constraint
_) <- Int -> IntMap (Record Constraint) -> Maybe (Record Constraint)
forall a. Int -> IntMap a -> Maybe a
M.lookup Int
ety (IntMap (Record Constraint) -> Maybe (Record Constraint))
-> IO (IntMap (Record Constraint))
-> IO (Maybe (Record Constraint))
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
<$> IOMap (Record Constraint) -> IO (IntMap (Record Constraint))
forall a. IORef a -> IO a
readIORef IOMap (Record Constraint)
cMap
    Double -> MaxForce
MaxForce (Double -> MaxForce) -> IO Double -> IO MaxForce
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
<$> Ptr Constraint -> IO Double
getMaxForce Ptr Constraint
c

-- MaxBias
getMaxBias :: Ptr Constraint -> IO Double
getMaxBias :: Ptr Constraint -> IO Double
getMaxBias Ptr Constraint
c = do
  CDouble
maxBias <- [C.exp| double { cpConstraintGetMaxBias ($(cpConstraint* c)) } |]
  Double -> IO Double
forall (m :: * -> *) a. Monad m => a -> m a
return (CDouble -> Double
forall a b. (Real a, Fractional b) => a -> b
realToFrac CDouble
maxBias)

setMaxBias :: Ptr Constraint -> Double -> IO ()
setMaxBias :: Ptr Constraint -> Double -> IO ()
setMaxBias Ptr Constraint
c (Double -> CDouble
forall a b. (Real a, Fractional b) => a -> b
realToFrac -> CDouble
maxBias) = [C.exp| void { cpConstraintSetMaxBias($(cpConstraint* c), $(double maxBias)); } |]

instance Component MaxBias where
  type Storage MaxBias = Space MaxBias

instance (MonadIO m, Has w m Physics) => Has w m MaxBias where
  getStore :: SystemT w m (Storage MaxBias)
getStore = (Space Physics -> Space MaxBias
forall a b. Space a -> Space b
cast :: Space Physics -> Space MaxBias) (Space Physics -> Space MaxBias)
-> SystemT w m (Space Physics) -> SystemT w m (Space MaxBias)
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
<$> SystemT w m (Space Physics)
forall w (m :: * -> *) c. Has w m c => SystemT w m (Storage c)
getStore

instance MonadIO m => ExplMembers m (Space MaxBias) where
  explMembers :: Space MaxBias -> m (Vector Int)
explMembers Space MaxBias
s = Space Constraint -> m (Vector Int)
forall (m :: * -> *) s. ExplMembers m s => s -> m (Vector Int)
explMembers (Space MaxBias -> Space Constraint
forall a b. Space a -> Space b
cast Space MaxBias
s :: Space Constraint)

instance MonadIO m => ExplSet m (Space MaxBias) where
  explSet :: Space MaxBias -> Int -> Elem (Space MaxBias) -> m ()
explSet (Space IOMap BodyRecord
_ IOMap (Record Shape)
_ IOMap (Record Constraint)
cMap IOMap (Record CollisionHandler)
_ SpacePtr
_) Int
ety (MaxBias vec) = IO () -> m ()
forall (m :: * -> *) a. MonadIO m => IO a -> m a
liftIO (IO () -> m ()) -> IO () -> m ()
forall a b. (a -> b) -> a -> b
$ do
    Maybe (Record Constraint)
rd <- Int -> IntMap (Record Constraint) -> Maybe (Record Constraint)
forall a. Int -> IntMap a -> Maybe a
M.lookup Int
ety (IntMap (Record Constraint) -> Maybe (Record Constraint))
-> IO (IntMap (Record Constraint))
-> IO (Maybe (Record Constraint))
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
<$> IOMap (Record Constraint) -> IO (IntMap (Record Constraint))
forall a. IORef a -> IO a
readIORef IOMap (Record Constraint)
cMap
    case Maybe (Record Constraint)
rd of
      Maybe (Record Constraint)
Nothing -> () -> IO ()
forall (m :: * -> *) a. Monad m => a -> m a
return ()
      Just (Record Ptr Constraint
c Constraint
_) -> Ptr Constraint -> Double -> IO ()
setMaxBias Ptr Constraint
c Double
vec

instance MonadIO m => ExplGet m (Space MaxBias) where
  explGet :: Space MaxBias -> Int -> m (Elem (Space MaxBias))
explGet (Space IOMap BodyRecord
_ IOMap (Record Shape)
_ IOMap (Record Constraint)
cMap IOMap (Record CollisionHandler)
_ SpacePtr
_) Int
ety = IO MaxBias -> m MaxBias
forall (m :: * -> *) a. MonadIO m => IO a -> m a
liftIO (IO MaxBias -> m MaxBias) -> IO MaxBias -> m MaxBias
forall a b. (a -> b) -> a -> b
$ do
    Just (Record Ptr Constraint
c Constraint
_) <- Int -> IntMap (Record Constraint) -> Maybe (Record Constraint)
forall a. Int -> IntMap a -> Maybe a
M.lookup Int
ety (IntMap (Record Constraint) -> Maybe (Record Constraint))
-> IO (IntMap (Record Constraint))
-> IO (Maybe (Record Constraint))
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
<$> IOMap (Record Constraint) -> IO (IntMap (Record Constraint))
forall a. IORef a -> IO a
readIORef IOMap (Record Constraint)
cMap
    Double -> MaxBias
MaxBias (Double -> MaxBias) -> IO Double -> IO MaxBias
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
<$> Ptr Constraint -> IO Double
getMaxBias Ptr Constraint
c
  explExists :: Space MaxBias -> Int -> m Bool
explExists Space MaxBias
s Int
ety = Space Constraint -> Int -> m Bool
forall (m :: * -> *) s. ExplGet m s => s -> Int -> m Bool
explExists (Space MaxBias -> Space Constraint
forall a b. Space a -> Space b
cast Space MaxBias
s :: Space Constraint) Int
ety

-- ErrorBias
getErrorBias :: Ptr Constraint -> IO Double
getErrorBias :: Ptr Constraint -> IO Double
getErrorBias Ptr Constraint
c = IO Double -> IO Double
forall (m :: * -> *) a. MonadIO m => IO a -> m a
liftIO (IO Double -> IO Double) -> IO Double -> IO Double
forall a b. (a -> b) -> a -> b
$ do
  CDouble
errorBias <- [C.exp| double { cpConstraintGetErrorBias ($(cpConstraint* c)) } |]
  Double -> IO Double
forall (m :: * -> *) a. Monad m => a -> m a
return (CDouble -> Double
forall a b. (Real a, Fractional b) => a -> b
realToFrac CDouble
errorBias)

setErrorBias :: Ptr Constraint -> Double -> IO ()
setErrorBias :: Ptr Constraint -> Double -> IO ()
setErrorBias Ptr Constraint
c (Double -> CDouble
forall a b. (Real a, Fractional b) => a -> b
realToFrac -> CDouble
errorBias) = [C.exp| void { cpConstraintSetErrorBias($(cpConstraint* c), $(double errorBias)); } |]

instance Component ErrorBias where
  type Storage ErrorBias = Space ErrorBias

instance (MonadIO m, Has w m Physics) => Has w m ErrorBias where
  getStore :: SystemT w m (Storage ErrorBias)
getStore = (Space Physics -> Space ErrorBias
forall a b. Space a -> Space b
cast :: Space Physics -> Space ErrorBias) (Space Physics -> Space ErrorBias)
-> SystemT w m (Space Physics) -> SystemT w m (Space ErrorBias)
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
<$> SystemT w m (Space Physics)
forall w (m :: * -> *) c. Has w m c => SystemT w m (Storage c)
getStore

instance MonadIO m => ExplMembers m (Space ErrorBias) where
  explMembers :: Space ErrorBias -> m (Vector Int)
explMembers Space ErrorBias
s = Space Constraint -> m (Vector Int)
forall (m :: * -> *) s. ExplMembers m s => s -> m (Vector Int)
explMembers (Space ErrorBias -> Space Constraint
forall a b. Space a -> Space b
cast Space ErrorBias
s :: Space Constraint)

instance MonadIO m => ExplSet m (Space ErrorBias) where
  explSet :: Space ErrorBias -> Int -> Elem (Space ErrorBias) -> m ()
explSet (Space IOMap BodyRecord
_ IOMap (Record Shape)
_ IOMap (Record Constraint)
cMap IOMap (Record CollisionHandler)
_ SpacePtr
_) Int
ety (ErrorBias vec) = IO () -> m ()
forall (m :: * -> *) a. MonadIO m => IO a -> m a
liftIO (IO () -> m ()) -> IO () -> m ()
forall a b. (a -> b) -> a -> b
$ do
    Maybe (Record Constraint)
rd <- Int -> IntMap (Record Constraint) -> Maybe (Record Constraint)
forall a. Int -> IntMap a -> Maybe a
M.lookup Int
ety (IntMap (Record Constraint) -> Maybe (Record Constraint))
-> IO (IntMap (Record Constraint))
-> IO (Maybe (Record Constraint))
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
<$> IOMap (Record Constraint) -> IO (IntMap (Record Constraint))
forall a. IORef a -> IO a
readIORef IOMap (Record Constraint)
cMap
    case Maybe (Record Constraint)
rd of
      Maybe (Record Constraint)
Nothing -> () -> IO ()
forall (m :: * -> *) a. Monad m => a -> m a
return ()
      Just (Record Ptr Constraint
c Constraint
_) -> Ptr Constraint -> Double -> IO ()
setErrorBias Ptr Constraint
c Double
vec

instance MonadIO m => ExplGet m (Space ErrorBias) where
  explExists :: Space ErrorBias -> Int -> m Bool
explExists Space ErrorBias
s Int
ety = Space Constraint -> Int -> m Bool
forall (m :: * -> *) s. ExplGet m s => s -> Int -> m Bool
explExists (Space ErrorBias -> Space Constraint
forall a b. Space a -> Space b
cast Space ErrorBias
s :: Space Constraint) Int
ety
  explGet :: Space ErrorBias -> Int -> m (Elem (Space ErrorBias))
explGet (Space IOMap BodyRecord
_ IOMap (Record Shape)
_ IOMap (Record Constraint)
cMap IOMap (Record CollisionHandler)
_ SpacePtr
_) Int
ety = IO ErrorBias -> m ErrorBias
forall (m :: * -> *) a. MonadIO m => IO a -> m a
liftIO (IO ErrorBias -> m ErrorBias) -> IO ErrorBias -> m ErrorBias
forall a b. (a -> b) -> a -> b
$ do
    Just (Record Ptr Constraint
c Constraint
_) <- Int -> IntMap (Record Constraint) -> Maybe (Record Constraint)
forall a. Int -> IntMap a -> Maybe a
M.lookup Int
ety (IntMap (Record Constraint) -> Maybe (Record Constraint))
-> IO (IntMap (Record Constraint))
-> IO (Maybe (Record Constraint))
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
<$> IOMap (Record Constraint) -> IO (IntMap (Record Constraint))
forall a. IORef a -> IO a
readIORef IOMap (Record Constraint)
cMap
    Double -> ErrorBias
ErrorBias (Double -> ErrorBias) -> IO Double -> IO ErrorBias
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
<$> Ptr Constraint -> IO Double
getErrorBias Ptr Constraint
c

-- Impulse
getImpulse :: Ptr Constraint -> IO Double
getImpulse :: Ptr Constraint -> IO Double
getImpulse Ptr Constraint
c = CDouble -> Double
forall a b. (Real a, Fractional b) => a -> b
realToFrac (CDouble -> Double) -> IO CDouble -> IO Double
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
<$> [C.exp| double { cpConstraintGetImpulse ($(cpConstraint* c)) } |]

instance Component Impulse where
  type Storage Impulse = Space Impulse

instance (MonadIO m, Has w m Physics) => Has w m Impulse where
  getStore :: SystemT w m (Storage Impulse)
getStore = (Space Physics -> Space Impulse
forall a b. Space a -> Space b
cast :: Space Physics -> Space Impulse) (Space Physics -> Space Impulse)
-> SystemT w m (Space Physics) -> SystemT w m (Space Impulse)
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
<$> SystemT w m (Space Physics)
forall w (m :: * -> *) c. Has w m c => SystemT w m (Storage c)
getStore

instance MonadIO m => ExplMembers m (Space Impulse) where
  explMembers :: Space Impulse -> m (Vector Int)
explMembers Space Impulse
s = Space Constraint -> m (Vector Int)
forall (m :: * -> *) s. ExplMembers m s => s -> m (Vector Int)
explMembers (Space Impulse -> Space Constraint
forall a b. Space a -> Space b
cast Space Impulse
s :: Space Constraint)

instance MonadIO m => ExplGet m (Space Impulse) where
  explExists :: Space Impulse -> Int -> m Bool
explExists Space Impulse
s Int
ety = Space Constraint -> Int -> m Bool
forall (m :: * -> *) s. ExplGet m s => s -> Int -> m Bool
explExists (Space Impulse -> Space Constraint
forall a b. Space a -> Space b
cast Space Impulse
s :: Space Constraint) Int
ety
  explGet :: Space Impulse -> Int -> m (Elem (Space Impulse))
explGet (Space IOMap BodyRecord
_ IOMap (Record Shape)
_ IOMap (Record Constraint)
cMap IOMap (Record CollisionHandler)
_ SpacePtr
_) Int
ety = IO Impulse -> m Impulse
forall (m :: * -> *) a. MonadIO m => IO a -> m a
liftIO (IO Impulse -> m Impulse) -> IO Impulse -> m Impulse
forall a b. (a -> b) -> a -> b
$ do
    Just (Record Ptr Constraint
c Constraint
_) <- Int -> IntMap (Record Constraint) -> Maybe (Record Constraint)
forall a. Int -> IntMap a -> Maybe a
M.lookup Int
ety (IntMap (Record Constraint) -> Maybe (Record Constraint))
-> IO (IntMap (Record Constraint))
-> IO (Maybe (Record Constraint))
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
<$> IOMap (Record Constraint) -> IO (IntMap (Record Constraint))
forall a. IORef a -> IO a
readIORef IOMap (Record Constraint)
cMap
    Double -> Impulse
Impulse (Double -> Impulse) -> IO Double -> IO Impulse
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
<$> Ptr Constraint -> IO Double
getImpulse Ptr Constraint
c

-- CollideBodies
getCollideBodies :: Ptr Constraint -> IO Bool
getCollideBodies :: Ptr Constraint -> IO Bool
getCollideBodies Ptr Constraint
c = do
  CInt
collide <- [C.exp| int { cpConstraintGetCollideBodies ($(cpConstraint* c)) } |]
  Bool -> IO Bool
forall (m :: * -> *) a. Monad m => a -> m a
return (Bool -> IO Bool) -> (CInt -> Bool) -> CInt -> IO Bool
forall b c a. (b -> c) -> (a -> b) -> a -> c
. Int -> Bool
forall a. Enum a => Int -> a
toEnum (Int -> Bool) -> (CInt -> Int) -> CInt -> Bool
forall b c a. (b -> c) -> (a -> b) -> a -> c
. CInt -> Int
forall a b. (Integral a, Num b) => a -> b
fromIntegral (CInt -> IO Bool) -> CInt -> IO Bool
forall a b. (a -> b) -> a -> b
$ CInt
collide

setCollideBodies :: Ptr Constraint -> Bool -> IO ()
setCollideBodies :: Ptr Constraint -> Bool -> IO ()
setCollideBodies Ptr Constraint
c (Int -> CInt
forall a b. (Integral a, Num b) => a -> b
fromIntegral (Int -> CInt) -> (Bool -> Int) -> Bool -> CInt
forall b c a. (b -> c) -> (a -> b) -> a -> c
. Bool -> Int
forall a. Enum a => a -> Int
fromEnum -> CInt
collide) = [C.exp| void { cpConstraintSetCollideBodies($(cpConstraint* c), $(int collide)); } |]

instance Component CollideBodies where
  type Storage CollideBodies = Space CollideBodies

instance (MonadIO m, Has w m Physics) => Has w m CollideBodies where
  getStore :: SystemT w m (Storage CollideBodies)
getStore = (Space Physics -> Space CollideBodies
forall a b. Space a -> Space b
cast :: Space Physics -> Space CollideBodies) (Space Physics -> Space CollideBodies)
-> SystemT w m (Space Physics) -> SystemT w m (Space CollideBodies)
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
<$> SystemT w m (Space Physics)
forall w (m :: * -> *) c. Has w m c => SystemT w m (Storage c)
getStore

instance MonadIO m => ExplMembers m (Space CollideBodies) where
  explMembers :: Space CollideBodies -> m (Vector Int)
explMembers Space CollideBodies
s = Space Constraint -> m (Vector Int)
forall (m :: * -> *) s. ExplMembers m s => s -> m (Vector Int)
explMembers (Space CollideBodies -> Space Constraint
forall a b. Space a -> Space b
cast Space CollideBodies
s :: Space Constraint)

instance MonadIO m => ExplSet m (Space CollideBodies) where
  explSet :: Space CollideBodies -> Int -> Elem (Space CollideBodies) -> m ()
explSet (Space IOMap BodyRecord
_ IOMap (Record Shape)
_ IOMap (Record Constraint)
cMap IOMap (Record CollisionHandler)
_ SpacePtr
_) Int
ety (CollideBodies vec) = IO () -> m ()
forall (m :: * -> *) a. MonadIO m => IO a -> m a
liftIO (IO () -> m ()) -> IO () -> m ()
forall a b. (a -> b) -> a -> b
$ do
    Maybe (Record Constraint)
rd <- Int -> IntMap (Record Constraint) -> Maybe (Record Constraint)
forall a. Int -> IntMap a -> Maybe a
M.lookup Int
ety (IntMap (Record Constraint) -> Maybe (Record Constraint))
-> IO (IntMap (Record Constraint))
-> IO (Maybe (Record Constraint))
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
<$> IOMap (Record Constraint) -> IO (IntMap (Record Constraint))
forall a. IORef a -> IO a
readIORef IOMap (Record Constraint)
cMap
    case Maybe (Record Constraint)
rd of
      Maybe (Record Constraint)
Nothing -> () -> IO ()
forall (m :: * -> *) a. Monad m => a -> m a
return ()
      Just (Record Ptr Constraint
c Constraint
_) -> Ptr Constraint -> Bool -> IO ()
setCollideBodies Ptr Constraint
c Bool
vec

instance MonadIO m => ExplGet m (Space CollideBodies) where
  explExists :: Space CollideBodies -> Int -> m Bool
explExists Space CollideBodies
s Int
ety = Space Constraint -> Int -> m Bool
forall (m :: * -> *) s. ExplGet m s => s -> Int -> m Bool
explExists (Space CollideBodies -> Space Constraint
forall a b. Space a -> Space b
cast Space CollideBodies
s :: Space Constraint) Int
ety
  explGet :: Space CollideBodies -> Int -> m (Elem (Space CollideBodies))
explGet (Space IOMap BodyRecord
_ IOMap (Record Shape)
_ IOMap (Record Constraint)
cMap IOMap (Record CollisionHandler)
_ SpacePtr
_) Int
ety = IO CollideBodies -> m CollideBodies
forall (m :: * -> *) a. MonadIO m => IO a -> m a
liftIO (IO CollideBodies -> m CollideBodies)
-> IO CollideBodies -> m CollideBodies
forall a b. (a -> b) -> a -> b
$ do
    Just (Record Ptr Constraint
c Constraint
_) <- Int -> IntMap (Record Constraint) -> Maybe (Record Constraint)
forall a. Int -> IntMap a -> Maybe a
M.lookup Int
ety (IntMap (Record Constraint) -> Maybe (Record Constraint))
-> IO (IntMap (Record Constraint))
-> IO (Maybe (Record Constraint))
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
<$> IOMap (Record Constraint) -> IO (IntMap (Record Constraint))
forall a. IORef a -> IO a
readIORef IOMap (Record Constraint)
cMap
    Bool -> CollideBodies
CollideBodies (Bool -> CollideBodies) -> IO Bool -> IO CollideBodies
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
<$> Ptr Constraint -> IO Bool
getCollideBodies Ptr Constraint
c