From 134f9d6cf39cf3e7d3d405fd268a85b55442cc3b Mon Sep 17 00:00:00 2001
From: Marc Sunet <jeannekamikaze@gmail.com>
Date: Mon, 6 Aug 2012 13:25:57 +0200
Subject: Added physics module

---
 Spear.cabal            | 131 ++++++++++--------------------------
 Spear.lkshs            |  12 ++--
 Spear.lkshw            |   2 +-
 Spear/Physics.hs       |  12 ++++
 Spear/Physics/Rigid.hs | 122 ++++++++++++++++++++++++++++++++++
 Spear/Physics/Types.hs |  11 +++
 Spear/Physics/World.hs | 177 +++++++++++++++++++++++++++++++++++++++++++++++++
 7 files changed, 364 insertions(+), 103 deletions(-)
 create mode 100644 Spear/Physics.hs
 create mode 100644 Spear/Physics/Rigid.hs
 create mode 100644 Spear/Physics/Types.hs
 create mode 100644 Spear/Physics/World.hs

diff --git a/Spear.cabal b/Spear.cabal
index ab8f6b9..dc462ae 100644
--- a/Spear.cabal
+++ b/Spear.cabal
@@ -7,7 +7,6 @@ license-file: LICENSE
 maintainer: jeannekamikaze@gmail.com
 homepage: http://spear.shellblade.net
 synopsis: A 3D game framework.
-description:  
 category: Game
 author: Marc Sunet
 data-dir: ""
@@ -15,106 +14,46 @@ data-dir: ""
 library
     build-depends: GLFW -any, OpenGL -any, OpenGLRaw -any,
                    StateVar -any, base -any, bytestring -any, directory -any,
-                   mtl -any, transformers -any, resource-simple -any, parsec >= 3.1.3, containers,
-                   ansi-terminal, vector
-                   
-    exposed-modules:
-                     Spear.App
-                     Spear.App.Application
-                     Spear.App.Input
-                     
-                     Spear.Assets.Image
-                     Spear.Assets.Model
-                     
-                     Spear.Collision
-                     Spear.Collision.AABB
-                     Spear.Collision.Collision
-                     Spear.Collision.Collisioner
-                     Spear.Collision.Sphere
-                     Spear.Collision.Triangle
-                     Spear.Collision.Types
-                     
-                     Spear.Game
-                     
-                     Spear.GLSL
-                     Spear.GLSL.Buffer
-                     Spear.GLSL.Error
-                     Spear.GLSL.Management
-                     Spear.GLSL.Texture
-                     Spear.GLSL.Uniform
-                     Spear.GLSL.VAO
-                     
-                     Spear.Math.Camera
-                     Spear.Math.Entity
-                     Spear.Math.Matrix3
-                     Spear.Math.Matrix4
-                     Spear.Math.MatrixUtils
-                     Spear.Math.Octree
-                     Spear.Math.Plane
-                     Spear.Math.Spatial
-                     Spear.Math.Vector3
-                     Spear.Math.Vector4
-                     
+                   mtl -any, transformers -any, resource-simple -any, parsec >=3.1.3,
+                   containers -any, ansi-terminal -any, vector -any, array -any
+    exposed-modules: Spear.Physics.Types Spear.Physics.World Spear.App
+                     Spear.App.Application Spear.App.Input Spear.Assets.Image
+                     Spear.Assets.Model Spear.Collision Spear.Collision.AABB
+                     Spear.Collision.Collision Spear.Collision.Collisioner
+                     Spear.Collision.Sphere Spear.Collision.Triangle
+                     Spear.Collision.Types Spear.Game Spear.GLSL Spear.GLSL.Buffer
+                     Spear.GLSL.Error Spear.GLSL.Management Spear.GLSL.Texture
+                     Spear.GLSL.Uniform Spear.GLSL.VAO Spear.Math.Camera
+                     Spear.Math.Entity Spear.Math.Matrix3 Spear.Math.Matrix4
+                     Spear.Math.MatrixUtils Spear.Math.Octree Spear.Math.Plane
+                     Spear.Math.Quaternion Spear.Math.Spatial Spear.Math.Vector3
+                     Spear.Math.Vector4 Spear.Physics Spear.Physics.Rigid
                      Spear.Render.AnimatedModel
-                     Spear.Render.Material
-                     Spear.Render.Model
-                     Spear.Render.Program
-                     Spear.Render.Renderable
-                     Spear.Render.StaticModel
-                     Spear.Render.Texture
-                     
-                     Spear.Scene.Graph
-                     Spear.Scene.Light
-                     Spear.Scene.Loader
-                     Spear.Scene.Scene
-                     Spear.Scene.SceneResources
-                     
-                     Spear.Setup
-                     
-                     Spear.Sys.Timer
-                     
-                     Spear.Updatable
+                     Spear.Render.Material Spear.Render.Model Spear.Render.Program
+                     Spear.Render.Renderable Spear.Render.StaticModel
+                     Spear.Render.Texture Spear.Scene.Graph Spear.Scene.Light
+                     Spear.Scene.Loader Spear.Scene.Scene Spear.Scene.SceneResources
+                     Spear.Setup Spear.Sys.Timer Spear.Updatable
     exposed: True
-    
     buildable: True
-    
     build-tools: hsc2hs -any
-    
-    c-sources:
-                Spear/Assets/Image/Image.c
-                Spear/Assets/Image/BMP/BMP_load.c
-                Spear/Assets/Model/Model.c
-                Spear/Assets/Model/MD2/MD2_load.c
-                Spear/Assets/Model/OBJ/OBJ_load.cc
-                Spear/Render/RenderModel.c
-                Spear/Sys/Timer/ctimer.c
-               
+    cc-options: -O2 -g -Wno-unused-result
+    c-sources: Spear/Assets/Image/Image.c
+               Spear/Assets/Image/BMP/BMP_load.c Spear/Assets/Model/Model.c
+               Spear/Assets/Model/MD2/MD2_load.c
+               Spear/Assets/Model/OBJ/OBJ_load.cc Spear/Render/RenderModel.c
+               Spear/Sys/Timer/ctimer.c
     extensions: TypeFamilies
-    
-    includes:
-                Spear/Assets/Image/BMP/BMP_load.h
-                Spear/Assets/Image/Image.h
-                Spear/Assets/Image/Image_error_code.h
-                Spear/Assets/Image/sys_types.h
-                Spear/Assets/Model/MD2/MD2_load.h
-                Spear/Assets/Model/OBJ/OBJ_load.h
-                Spear/Assets/Model/Model.h
-                Spear/Assets/Model/Model_error_code.h
-                Spear/Assets/Model/sys_types.h
-                Spear/Render/RenderModel.h
-                Timer/Timer.h
-              
-    include-dirs:
-                Spear/Assets/Image
-                Spear/Assets/Model
-                Spear/Render
-                Spear/Sys
-    
+    extra-libraries: stdc++
+    includes: Spear/Assets/Image/BMP/BMP_load.h
+              Spear/Assets/Image/Image.h Spear/Assets/Image/Image_error_code.h
+              Spear/Assets/Image/sys_types.h Spear/Assets/Model/MD2/MD2_load.h
+              Spear/Assets/Model/OBJ/OBJ_load.h Spear/Assets/Model/Model.h
+              Spear/Assets/Model/Model_error_code.h
+              Spear/Assets/Model/sys_types.h Spear/Render/RenderModel.h
+              Timer/Timer.h
+    include-dirs: Spear/Assets/Image Spear/Assets/Model Spear/Render
+                  Spear/Sys
     hs-source-dirs: .
-    
     ghc-options: -O2 -rtsopts
-    
-    cc-options:  -O2 -g -Wno-unused-result
-    
-    extra-libraries: stdc++
  
diff --git a/Spear.lkshs b/Spear.lkshs
index 1427d7f..9fbb082 100644
--- a/Spear.lkshs
+++ b/Spear.lkshs
@@ -1,18 +1,18 @@
 Version of session file format:
                1
 Time of storage:
-               "Thu Aug  2 15:35:02 CEST 2012"
-Layout:        VerticalP (TerminalP {paneGroups = fromList [], paneTabs = Just TopP, currentPage = 4, detachedId = Nothing, detachedSize = Nothing}) (HorizontalP (TerminalP {paneGroups = fromList [("Browser",HorizontalP (TerminalP {paneGroups = fromList [], paneTabs = Nothing, currentPage = 0, detachedId = Nothing, detachedSize = Nothing}) (HorizontalP (TerminalP {paneGroups = fromList [], paneTabs = Nothing, currentPage = 0, detachedId = Nothing, detachedSize = Nothing}) (TerminalP {paneGroups = fromList [], paneTabs = Nothing, currentPage = 0, detachedId = Nothing, detachedSize = Nothing}) 308) 219)], paneTabs = Just BottomP, currentPage = 0, detachedId = Nothing, detachedSize = Nothing}) (TerminalP {paneGroups = fromList [], paneTabs = Nothing, currentPage = 0, detachedId = Nothing, detachedSize = Nothing}) 756) 953
-Population:    [(Just (BufferSt (BufferState "/home/jeanne/programming/haskell/Spear/Spear/Collision/Collision.hs" 61)),[SplitP LeftP]),(Just (ErrorsSt ErrorsState),[SplitP RightP,SplitP TopP]),(Just (FilesSt FilesState),[SplitP RightP,SplitP TopP]),(Just (BufferSt (BufferState "/home/jeanne/programming/haskell/Spear/demos/simple-scene/GameObject.hs" 2483)),[SplitP LeftP]),(Just (BufferSt (BufferState "/home/jeanne/programming/haskell/Spear/demos/simple-scene/GameState.hs" 893)),[SplitP LeftP]),(Just (InfoSt (InfoState Nothing)),[SplitP RightP,SplitP TopP,GroupP "Browser",SplitP BottomP,SplitP BottomP]),(Just (BufferSt (BufferState "/home/jeanne/programming/haskell/Spear/Spear/Scene/Loader.hs" 10609)),[SplitP LeftP]),(Just (LogSt LogState),[SplitP RightP,SplitP BottomP]),(Just (BufferSt (BufferState "/home/jeanne/programming/haskell/Spear/Spear/Assets/Model/Model.c" 1772)),[SplitP LeftP]),(Just (BufferSt (BufferState "/home/jeanne/programming/haskell/Spear/Spear/Assets/Model/Model.h" 0)),[SplitP LeftP]),(Just (BufferSt (BufferState "/home/jeanne/programming/haskell/Spear/Spear/Assets/Model.hsc" 10563)),[SplitP LeftP]),(Just (ModulesSt (ModulesState 286 (PackageScope False,False) (Nothing,Nothing) (ExpanderState {packageExp = ([],[]), packageExpNoBlack = ([],[]), packageDExp = ([],[]), packageDExpNoBlack = ([],[]), workspaceExp = ([],[]), workspaceExpNoBlack = ([],[]), workspaceDExp = ([],[]), workspaceDExpNoBlack = ([],[]), systemExp = ([],[]), systemExpNoBlack = ([],[])}))),[SplitP RightP,SplitP TopP,GroupP "Browser",SplitP BottomP,SplitP TopP]),(Just (WorkspaceSt WorkspaceState),[SplitP RightP,SplitP TopP,GroupP "Browser",SplitP TopP]),(Just (BufferSt (BufferState "/home/jeanne/programming/haskell/Spear/demos/simple-scene/main.hs" 1249)),[SplitP LeftP])]
+               "Mon Aug  6 13:19:58 CEST 2012"
+Layout:        VerticalP (TerminalP {paneGroups = fromList [], paneTabs = Just TopP, currentPage = 7, detachedId = Nothing, detachedSize = Nothing}) (HorizontalP (TerminalP {paneGroups = fromList [("Browser",HorizontalP (TerminalP {paneGroups = fromList [], paneTabs = Nothing, currentPage = 0, detachedId = Nothing, detachedSize = Nothing}) (HorizontalP (TerminalP {paneGroups = fromList [], paneTabs = Nothing, currentPage = 0, detachedId = Nothing, detachedSize = Nothing}) (TerminalP {paneGroups = fromList [], paneTabs = Nothing, currentPage = 0, detachedId = Nothing, detachedSize = Nothing}) 289) 214)], paneTabs = Just BottomP, currentPage = 0, detachedId = Nothing, detachedSize = Nothing}) (TerminalP {paneGroups = fromList [], paneTabs = Nothing, currentPage = 0, detachedId = Nothing, detachedSize = Nothing}) 738) 954
+Population:    [(Just (BufferSt (BufferState "/home/jeanne/programming/haskell/Spear/Spear/Collision/Collision.hs" 75)),[SplitP LeftP]),(Just (BufferSt (BufferState "/home/jeanne/programming/haskell/Spear/Spear/Collision/Collisioner.hs" 551)),[SplitP LeftP]),(Just (ErrorsSt ErrorsState),[SplitP RightP,SplitP TopP]),(Just (FilesSt FilesState),[SplitP RightP,SplitP TopP]),(Just (BufferSt (BufferState "/home/jeanne/programming/haskell/Spear/demos/simple-scene/GameObject.hs" 1411)),[SplitP LeftP]),(Just (InfoSt (InfoState Nothing)),[SplitP RightP,SplitP TopP,GroupP "Browser",SplitP BottomP,SplitP BottomP]),(Just (LogSt LogState),[SplitP RightP,SplitP BottomP]),(Just (ModulesSt (ModulesState 286 (PackageScope False,False) (Nothing,Nothing) (ExpanderState {packageExp = ([],[]), packageExpNoBlack = ([],[]), packageDExp = ([],[]), packageDExpNoBlack = ([],[]), workspaceExp = ([],[]), workspaceExpNoBlack = ([],[]), workspaceDExp = ([],[]), workspaceDExpNoBlack = ([],[]), systemExp = ([],[]), systemExpNoBlack = ([],[])}))),[SplitP RightP,SplitP TopP,GroupP "Browser",SplitP BottomP,SplitP TopP]),(Just (BufferSt (BufferState "/home/jeanne/programming/haskell/Spear/Spear/Physics.hs" 133)),[SplitP LeftP]),(Just (BufferSt (BufferState "/home/jeanne/programming/haskell/Spear/Spear/Physics/Rigid.hs" 447)),[SplitP LeftP]),(Just (BufferSt (BufferState "/home/jeanne/programming/haskell/Spear/Spear/Math/Spatial.hs" 0)),[SplitP LeftP]),(Just (BufferSt (BufferState "/home/jeanne/programming/haskell/Spear/Spear/Physics/Types.hs" 142)),[SplitP LeftP]),(Just (WorkspaceSt WorkspaceState),[SplitP RightP,SplitP TopP,GroupP "Browser",SplitP TopP]),(Just (BufferSt (BufferState "/home/jeanne/programming/haskell/Spear/Spear/Physics/World.hs" 196)),[SplitP LeftP]),(Just (BufferSt (BufferState "/home/jeanne/programming/haskell/Spear/demos/simple-scene/main.hs" 1603)),[SplitP LeftP])]
 Window size:   (1796,979)
 Completion size:
                (750,400)
 Workspace:     Just "/home/jeanne/programming/haskell/Spear/Spear.lkshw"
-Active pane:   Just "Model.c"
+Active pane:   Just "World.hs"
 Toolbar visible:
                True
-FindbarState:  (False,FindState {entryStr = "asd", entryHist = ["mandatory","mandao","col","forward","asd","MouseButton"], replaceStr = "mandatory'", replaceHist = [], caseSensitive = False, entireWord = False, wrapAround = False, regex = False, lineNr = 1})
+FindbarState:  (False,FindState {entryStr = "asd", entryHist = ["gravity","asdad","rotSpeed","azimuth","mandatory","mandao","col","forward","MouseButton"], replaceStr = "mandatory'", replaceHist = [], caseSensitive = False, entireWord = False, wrapAround = False, regex = False, lineNr = 1})
 Recently opened files:
-               ["/home/jeanne/programming/haskell/Spear/Spear/Scene/Loader.hs","/home/jeanne/programming/haskell/Spear/Spear/Assets/Model.hsc","/home/jeanne/programming/haskell/Spear/Spear/Assets/Model/Model.h","/home/jeanne/programming/haskell/Spear/Spear/Scene/Graph.hs","/home/jeanne/programming/haskell/Spear/Spear/Scene/SceneResources.hs","/home/jeanne/programming/haskell/Spear/Spear/Math/Matrix4.hs","/home/jeanne/programming/haskell/Spear/Spear/Math/Vector3.hs","/home/jeanne/programming/haskell/Spear/demos/simple-scene/simple.scene","/home/jeanne/programming/haskell/Spear/Spear/Assets/Model/Model.c","/home/jeanne/programming/haskell/Spear/demos/simple-scene/GameMessage.hs","/home/jeanne/programming/haskell/Spear/demos/simple-scene/GameState.hs","/home/jeanne/programming/haskell/Spear/Spear/Collision/Collision.hs"]
+               ["/home/jeanne/programming/haskell/Spear/demos/simple-scene/GameState.hs","/home/jeanne/programming/haskell/Spear/Spear/App/Input.hs","/home/jeanne/programming/haskell/Spear/Spear/Math/Spatial.hs","/home/jeanne/programming/haskell/Spear/Spear/Math/Quaternion.hs","/home/jeanne/programming/haskell/Spear/Spear/Render/AnimatedModel.hs","/home/jeanne/programming/haskell/Spear/Spear/Render/StaticModel.hs","/home/jeanne/programming/haskell/Spear/Spear/Scene/Loader.hs","/home/jeanne/programming/haskell/Spear/Spear/Assets/Model.hsc","/home/jeanne/programming/haskell/Spear/Spear/Assets/Model/Model.h","/home/jeanne/programming/haskell/Spear/Spear/Scene/Graph.hs","/home/jeanne/programming/haskell/Spear/Spear/Scene/SceneResources.hs","/home/jeanne/programming/haskell/Spear/Spear/Math/Matrix4.hs"]
 Recently opened workspaces:
                ["/home/jeanne/programming/haskell/Spear/Spear.lkshw","/home/jeanne/leksah.lkshw"]
\ No newline at end of file
diff --git a/Spear.lkshw b/Spear.lkshw
index 865bceb..fdfc941 100644
--- a/Spear.lkshw
+++ b/Spear.lkshw
@@ -1,7 +1,7 @@
 Version of workspace file format:
                1
 Time of storage:
-               "Wed Aug  1 18:11:40 CEST 2012"
+               "Mon Aug  6 13:19:41 CEST 2012"
 Name of the workspace:
                "Spear"
 File paths of contained packages:
diff --git a/Spear/Physics.hs b/Spear/Physics.hs
new file mode 100644
index 0000000..248d4fe
--- /dev/null
+++ b/Spear/Physics.hs
@@ -0,0 +1,12 @@
+module Spear.Physics
+(
+    module Spear.Physics.Rigid
+,   module Spear.Physics.Types
+,   module Spear.Physics.World
+)
+where
+
+
+import Spear.Physics.Rigid
+import Spear.Physics.Types
+import Spear.Physics.World
diff --git a/Spear/Physics/Rigid.hs b/Spear/Physics/Rigid.hs
new file mode 100644
index 0000000..b9c84d2
--- /dev/null
+++ b/Spear/Physics/Rigid.hs
@@ -0,0 +1,122 @@
+module Spear.Physics.Rigid
+(
+    module Spear.Physics.Types
+,   RigidBody(..)
+,   rigidBody
+,   update
+)
+where
+
+
+import qualified Spear.Math.Matrix4 as M4
+import Spear.Math.Spatial
+import Spear.Math.Vector3 as V3
+import Spear.Physics.Types
+
+import Data.List (foldl')
+import Control.Monad.State
+
+
+data RigidBody = RigidBody
+    { mass         :: Float
+    , position     :: Vector3
+    , velocity     :: Vector3
+    , acceleration :: Vector3
+    }
+
+
+instance Spatial RigidBody where
+    
+    move v body = body { position = v + position body }
+    
+    moveFwd     speed body = body { position = position body + scale (-speed) unitZ }
+    
+    moveBack    speed body = body { position = position body + scale speed unitZ }
+    
+    strafeLeft  speed body = body { position = position body + scale (-speed) unitX }
+    
+    strafeRight speed body = body { position = position body + scale speed unitX }
+    
+    pitch angle = id
+    
+    yaw angle = id
+    
+    roll angle = id
+    
+    pos = position
+    
+    fwd _ = unitZ
+    
+    up _ = unitY
+    
+    right _ = unitX
+    
+    transform body = M4.transform unitX unitY unitZ $ position body
+    
+    setTransform transf body = body { position = M4.position transf }
+    
+    setPos p body = body { position = p }
+
+
+-- | Build a 'RigidBody'.
+rigidBody :: Mass -> Position -> RigidBody
+rigidBody m x = RigidBody m x V3.zero V3.zero
+
+
+-- | Update the given 'RigidBody'.
+update :: [Force] -> Dt -> RigidBody -> RigidBody
+update forces dt body =
+    let netforce = foldl' (+) V3.zero forces
+        m  = mass body
+        r1 = position body
+        v1 = velocity body
+        a1 = acceleration body
+        r2 = r1 + scale dt v1 + scale (0.5*dt*dt) a1
+        v' = v1 + scale (0.5*dt) a1
+        a2 = a1 + scale (1/m) netforce
+        v2 = v1 + scale (dt/2) (a2+a1) + scale (0.5*dt) a2
+    in
+        RigidBody m r2 v2 a2
+
+
+-- test
+gravity = vec3 0 (-10) 0
+b0 = rigidBody 50 $ vec3 0 1000 0
+
+
+debug :: IO ()
+debug = evalStateT debug' b0
+
+
+
+debug' :: StateT RigidBody IO ()
+debug' = do
+    lift . putStrLn $ "Initial body:"
+    lift . putStrLn . show' $ b0
+    lift . putStrLn $ "Falling..."
+    step $ update [gravity*50] 1
+    step $ update [gravity*50] 1
+    step $ update [gravity*50] 1
+    lift . putStrLn $ "Jumping"
+    step $ update [gravity*50, vec3 0 9000 0] 1
+    lift . putStrLn $ "Falling..."
+    step $ update [gravity*50] 1
+    step $ update [gravity*50] 1
+    step $ update [gravity*50] 1
+
+
+step :: (RigidBody -> RigidBody) -> StateT RigidBody IO ()
+step update = do
+    modify update
+    body <- get
+    lift . putStrLn . show' $ body
+
+
+show' body =
+    "mass " ++ (show $ mass body) ++
+    ", position " ++ (showVec $ position body) ++
+    ", velocity " ++ (showVec $ velocity body) ++
+    ", acceleration " ++ (showVec $ acceleration body)
+
+
+showVec v = (show $ x v) ++ ", " ++ (show $ y v) ++ ", " ++ (show $ z v)
diff --git a/Spear/Physics/Types.hs b/Spear/Physics/Types.hs
new file mode 100644
index 0000000..5d87c47
--- /dev/null
+++ b/Spear/Physics/Types.hs
@@ -0,0 +1,11 @@
+module Spear.Physics.Types
+where
+
+
+import Spear.Math.Vector3
+
+
+type Dt = Float
+type Force = Vector3
+type Mass = Float
+type Position = Vector3
diff --git a/Spear/Physics/World.hs b/Spear/Physics/World.hs
new file mode 100644
index 0000000..4ad0191
--- /dev/null
+++ b/Spear/Physics/World.hs
@@ -0,0 +1,177 @@
+module Spear.Physics.World
+(
+    module Spear.Physics.Types
+    -- * Data types
+,   World
+,   ObjectID
+    -- * Construction
+,   emptyWorld
+    -- * World operations
+,   setGravity
+,   updateWorld
+    -- * Object operations
+,   newObject
+,   deleteObject
+,   modifyObject
+,   objectTransform
+,   objectForces
+,   setForces
+)
+where
+
+
+import Spear.Collision.AABB
+import Spear.Collision.Collisioner as C
+import Spear.Collision.Sphere
+import Spear.Math.Matrix4 (Matrix4)
+import Spear.Math.Spatial
+import Spear.Math.Vector3
+import Spear.Physics.Rigid as Rigid
+import Spear.Physics.Types
+
+import Control.Monad.ST
+import Data.Array as A
+import Data.Array.ST
+import Data.Maybe (fromJust)
+
+
+-- | Uniquely identifies an object in a 'World'.
+newtype ObjectID = ObjectID Int
+
+
+data Object = Object
+    { body        :: RigidBody
+    , collisioner :: Collisioner
+    , forces      :: [Vector3]
+    }
+
+
+-- | The world where physical bodies are simulated.
+data World = World
+    { bodies  :: Array Int (Maybe Object) -- ^ Collection of objects.
+    , gravity :: Vector3 -- ^ World gravity.
+    }
+
+
+-- | Create an empty 'World'.
+emptyWorld :: World
+emptyWorld = World emptyArray defaultGravity
+    where
+        defaultGravity = vec3 0 (-9.8) 0
+        emptyArray = listArray (0,0) []
+
+
+-- | Create a new object.
+newObject :: RigidBody -> Collisioner -> World -> (World, ObjectID)
+newObject body collisioner world =
+    let obj = (Object body collisioner [])
+    in case emptySlot world of
+        Just i  -> (insert i obj world, ObjectID i)
+        Nothing -> append obj world
+
+
+-- | Search for an empty slot in the given 'World'.
+emptySlot :: World -> Maybe Int
+emptySlot world = Nothing
+
+
+-- | Insert the given 'Object' in the given 'World' at the given position.
+insert :: Int -> Object -> World -> World
+insert i obj world = world { bodies = bodies' }
+    where
+        bodies' = runSTArray $ do
+            bs <- thaw $ bodies world
+            writeArray bs i $ Just obj
+            return bs
+
+
+-- | Append the given object to the given 'World'.
+--
+-- The world's vectors are doubled in size to make future insertions faster.
+append :: Object -> World -> (World, ObjectID)
+append obj world = (world, ObjectID 0)
+
+
+-- | Remove the object specified by the given 'ObjectID' from the given 'World'.
+deleteObject :: ObjectID -> World -> World
+deleteObject (ObjectID i) world = world { bodies = bodies' }
+    where
+        bodies' = runSTArray $ do
+            bs <- thaw $ bodies world
+            writeArray bs i Nothing
+            return bs
+
+
+-- | Modify the object identified by the given 'ObjectID' in the given 'World'.
+modifyObject :: (RigidBody -> RigidBody) -> ObjectID -> World -> World
+modifyObject f (ObjectID i) world = world { bodies = bodies' }
+    where
+        bodies' = runSTArray $ do
+            bs  <- thaw $ bodies world
+            obj <- readArray bs i
+            writeArray bs i $ fmap (\obj -> obj { body = f $ body obj }) obj
+            return bs
+
+
+-- | Get the transform of the object identified by the given 'ObjectID'.
+objectTransform :: World -> ObjectID -> Matrix4
+objectTransform world (ObjectID i) = transform . body . fromJust $ bodies world ! i
+
+
+-- | Get the forces acting on the object identified by the given 'ObjectID'.
+objectForces :: World -> ObjectID -> [Force]
+objectForces world (ObjectID i) = forces . fromJust $ bodies world ! i
+
+
+-- | Add the given force to the forces acting on the object identified by the given 'ObjectID'.
+setForces :: [Force] -> ObjectID -> World -> World
+setForces fs (ObjectID i) world = world { bodies = bodies' }
+    where
+        bodies' = runSTArray $ do
+            bs  <- thaw $ bodies world
+            obj <- readArray bs i
+            writeArray bs i $ fmap (\obj -> obj { forces = fs }) obj
+            return bs 
+
+
+-- | Set the world's gravity.
+setGravity :: Vector3 -> World -> World
+setGravity g world = world { gravity = g }
+
+
+-- | Update the 'World'.
+updateWorld :: Dt -> World -> World
+updateWorld dt world = world { bodies = bodies' }
+    where
+        bodies' = runSTArray $ do
+            bs <- thaw $ bodies world
+            mapArray updateObject bs
+            return bs
+        
+        updateObject = fmap updateObject'
+        updateObject' (Object body collisioner forces) = Object body' collisioner' forces
+            where
+                -- Forces acting on the body.
+                forces' = scale (mass body) (gravity world) : forces
+                
+                -- Updated body.
+                body' = Rigid.update forces dt body
+                
+                -- Center collisioner around the new body's center.
+                collisioner' = center (Rigid.position body') collisioner
+                
+                -- Center the collisioner around the given point.
+                center c (SphereCol (Sphere _ r)) = sphereCollisioner $ Sphere c r
+                center c (AABBCol (AABB min max)) =
+                    let v = (max - min) / 2
+                        min' = c - v
+                        max' = c + v
+                    in
+                        aabbCollisioner $ AABB min' max'
+
+
+{--- | Test for potential collisions in the given 'World'.
+--
+-- Returns a new world and a list of colliding pairs of objects.
+--testCollisions :: World -> (World, [(ObjectID, ObjectID)])-}
+
-- 
cgit v1.2.3