Add LocalForce and WorldForce subcomponents to apecs-physics

jonascarpay · Feb 4, 2024 · 3adabf4 · 3adabf4
1 parent ebc5937
commit 3adabf4
Show file tree

Hide file tree

Showing 3 changed files with 72 additions and 8 deletions.
diff --git a/apecs-physics/src/Apecs/Physics.hs b/apecs-physics/src/Apecs/Physics.hs
@@ -12,7 +12,7 @@ module Apecs.Physics (
 
   -- * Body
   -- $BODY
-  Body (..), Position (..), Velocity (..), Angle (..), AngularVelocity (..), Force (..),
+  Body (..), Position (..), Velocity (..), Angle (..), AngularVelocity (..), Force (..), LocalForce(..), WorldForce(..),
   BodyMass (..), Moment (..), CenterOfGravity (..), Torque (..), ShapeList (..), ConstraintList (..),
 
   -- * Shape

diff --git a/apecs-physics/src/Apecs/Physics/Body.hs b/apecs-physics/src/Apecs/Physics/Body.hs
@@ -266,6 +266,50 @@ instance MonadIO m => ExplGet m (Space Force) where
     Just (BodyRecord b _ _ _) <- M.lookup ety <$> readIORef bMap
     Force <$> getForce b
 
+-- LocalForce
+applyForceAtLocalPoint :: Ptr Body -> Vec -> BVec -> IO ()
+applyForceAtLocalPoint bodyPtr force bPos  = [C.block| void {
+  const cpVect frc = { $(double fx), $(double fy) };
+  const cpVect pos = { $(double px), $(double py) };
+  cpBodyApplyForceAtLocalPoint($(cpBody* bodyPtr), frc, pos);
+  } |]
+  where
+  V2 (realToFrac -> fx) (realToFrac -> fy) = force
+  V2 (realToFrac -> px) (realToFrac -> py) = bPos
+
+instance Component LocalForce where
+  type Storage LocalForce = Space LocalForce
+
+instance (MonadIO m, Has w m Physics) => Has w m LocalForce where
+  getStore = (cast :: Space Physics -> Space LocalForce) <$> getStore
+
+instance MonadIO m => ExplSet m (Space LocalForce) where
+  explSet (Space bMap _ _ _ _) ety (LocalForce frc bPos) = liftIO $ do
+    rd <- M.lookup ety <$> readIORef bMap
+    forM_ rd$ \(BodyRecord b _ _ _) -> applyForceAtLocalPoint b frc bPos
+
+-- WorldForce
+applyForceAtWorldPoint :: Ptr Body -> Vec -> WVec -> IO ()
+applyForceAtWorldPoint bodyPtr force wPos  = [C.block| void {
+  const cpVect frc = { $(double fx), $(double fy) };
+  const cpVect pos = { $(double px), $(double py) };
+  cpBodyApplyForceAtWorldPoint($(cpBody* bodyPtr), frc, pos);
+  } |]
+  where
+  V2 (realToFrac -> fx) (realToFrac -> fy) = force
+  V2 (realToFrac -> px) (realToFrac -> py) = wPos
+
+instance Component WorldForce where
+  type Storage WorldForce = Space WorldForce
+
+instance (MonadIO m, Has w m Physics) => Has w m WorldForce where
+  getStore = (cast :: Space Physics -> Space WorldForce) <$> getStore
+
+instance MonadIO m => ExplSet m (Space WorldForce) where
+  explSet (Space bMap _ _ _ _) ety (WorldForce frc wPos) = liftIO $ do
+    rd <- M.lookup ety <$> readIORef bMap
+    forM_ rd$ \(BodyRecord b _ _ _) -> applyForceAtWorldPoint b frc wPos
+
 -- BodyMass
 getBodyMass :: Ptr Body -> IO Double
 getBodyMass bodyPtr = do

diff --git a/apecs-physics/src/Apecs/Physics/Types.hs b/apecs-physics/src/Apecs/Physics/Types.hs
@@ -69,11 +69,31 @@ data Body = DynamicBody | KinematicBody | StaticBody deriving (Eq, Ord, Enum)
 newtype Position        = Position WVec
 -- | A subcomponent of @Body@ representing where it is going in world coordinates
 newtype Velocity        = Velocity WVec
--- | A component used to apply a force to a @Body@.
+-- | A component used to /set/ a force on a @Body@.
 -- The force is applied to the body's center of gravity.
--- This component is reset to @ Vec 0 0 @ after every stimulation step, 
--- so it is mainly used to apply a force as opposed to being read.
+-- This component is reset to @ Vec 0 0 @ after every simulation step,
+-- so it is mainly used to set a "lump-sum" force as opposed to being read.
+--
+-- Setting the force on a body via this component is /not/ additive. If you
+-- need to add multiple forces to a body within a single step of your game
+-- logic, use 'LocalForce' or 'WorldForce' instead.
 newtype Force           = Force Vec
+-- | A component used to /add/ a local force to a @Body@ as if applied from the
+-- provided body-local point.
+--
+-- Applying a force to a body via this component is additive. You may use this
+-- component (and 'WorldForce') to add multiple forces to a body between
+-- simulation steps. This component may not be read, but you may read the
+-- 'Force' component to get the total force on the body.
+data LocalForce         = LocalForce !Vec !BVec
+-- | A component used to /add/ a force to a @Body@ as if applied from the provided
+-- world point.
+--
+-- Applying a force to a body via this component is additive. You may use this
+-- component (and 'LocalForce') to add multiple forces to a body between
+-- simulation steps. This component may not be read, but you may read the
+-- 'Force' component to get the total force on the body.
+data WorldForce         = WorldForce !Vec !WVec
 -- | A component used to apply a torque to a @Body@.
 -- The torque is applied to the entire body at once.
 -- This component is reset to @ 0 @ after every simulation step, so it
@@ -105,7 +125,7 @@ data Convex = Convex [BVec] Double deriving (Eq, Show)
 
 -- | If a body is a 'Sensor', it exists only to trigger collision responses.
 -- It won't phyiscally interact with other bodies in any way, but it __will__
--- cause collision handlers to run. 
+-- cause collision handlers to run.
 newtype Sensor          = Sensor          Bool       deriving (Eq, Show)
 -- | The elasticity of a shape. Higher elasticities will create more
 -- elastic collisions, IE, will be bouncier.
@@ -116,14 +136,14 @@ newtype Elasticity      = Elasticity      Double     deriving (Eq, Show)
 -- being accelerated, but it's generally easier to understand it as being how "heavy" something is.
 --
 -- The physics engine lets you set this, and it will calculate the 'Density' and other components
--- for you. 
+-- for you.
 --
 -- See <https://en.wikipedia.org/wiki/Mass> for more information.
 newtype Mass            = Mass            Double     deriving (Eq, Show)
 -- | The density of a shape is a measure of how much mass an object has in a given volume.
--- 
+--
 -- The physics engine lets you set this, and it will calculate the 'Mass' and other components for you.
--- 
+--
 -- See <https://en.wikipedia.org/wiki/Density> for more information.
 newtype Density         = Density         Double     deriving (Eq, Show)
 -- | The friction of an object is a measure of how much it resists movement.