packages feed

shapes (empty) → 0.1.0.0

raw patch · 48 files changed

+4367/−0 lines, 48 filesdep +QuickCheckdep +arraydep +basesetup-changed

Dependencies added: QuickCheck, array, base, containers, criterion, deepseq, either, ghc-prim, hspec, lens, linear, mtl, shapes, shapes-math, transformers, vector, vector-th-unbox

Files

+ LICENSE view
@@ -0,0 +1,22 @@+The MIT License (MIT)++Copyright (c) 2015 ublubu++Permission is hereby granted, free of charge, to any person obtaining a copy+of this software and associated documentation files (the "Software"), to deal+in the Software without restriction, including without limitation the rights+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell+copies of the Software, and to permit persons to whom the Software is+furnished to do so, subject to the following conditions:++The above copyright notice and this permission notice shall be included in all+copies or substantial portions of the Software.++THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE+SOFTWARE.+
+ Setup.hs view
@@ -0,0 +1,2 @@+import Distribution.Simple+main = defaultMain
+ bench/Main.hs view
@@ -0,0 +1,39 @@+{- |+Main. I just use this for benchmarking and profiling different scenes and solver configurations.+-}+module Main where++import Control.DeepSeq+import Criterion.Main+import Data.Proxy+import Utils.Utils+import qualified Physics.Scenes.Stacks as Stacks++import qualified Physics.Constraint.Benchmark as BC+import qualified Physics.Contact.Benchmark as BC'+import qualified Physics.Broadphase.Benchmark as BB++import qualified Physics.Engine.Main as OM++benchy :: (Num n, NFData world)+       => String+       -> Proxy e+       -> (Proxy e -> scene)+       -> (scene -> SP world cache)+       -> (scene -> n -> SP world cache -> SP world cache)+       -> Benchmark+benchy prefix p sceneGen stateGen stepGen =+  bench (prefix ++ " updateWorld 10") $ nf (_spFst . f) s0+  where s0 = stepGen scene 100 $ stateGen scene+        f = stepGen scene 10+        scene = sceneGen p++-- TODO: use something other than show to ensure evaluation of the world+main :: IO ()+-- 10 frames 15x15+-- 770ms simple 3x+-- 310ms opt+hashtable 3x+--  80ms opt+vector 2x+main = defaultMain [bench "opt updateWorld 10" $ nf (OM.runWorld 0.01 (Stacks.makeScene (30, 30) 0 OM.engineP ())) 10]+--main = print . rnf $ OM.runWorld 0.01 (Stacks.makeScene (15, 15) 0 OM.engineP) 200+--main = BB.main
+ bench/Physics/Broadphase/Benchmark.hs view
@@ -0,0 +1,65 @@+{-# LANGUAGE MagicHash #-}++module Physics.Broadphase.Benchmark where++import           Criterion.Main++import qualified Physics.Broadphase.Aabb   as OB+import qualified Physics.Broadphase.Grid   as G+import qualified Physics.Contact           as OC+import           Physics.World+import           Physics.World.Object++import           Physics.Engine            (engineP)+import           Physics.Engine.Class      (makeWorld)+import           Physics.Scenes.Stacks++import           Utils.Utils++import           Physics.Contact.Benchmark (testOptBoxes)++-- TODO: report this somehow? probably doesn't segfault in GHCI or without -O.+{-+{-# LANGUAGE MagicHash #-}++import GHC.Prim+import GHC.Types++data TestS a = TestS !a deriving Show+data TestInner = TestInner Double#++instance Show TestInner where+  show _ = "(TestInner _)"++testSegfault :: TestS Bool+testSegfault =+  TestS (testWeirdCompare a b)+  where a = TestInner 1.0##+        b = TestInner 2.0##++testWeirdCompare :: TestInner -> TestInner -> Bool+testWeirdCompare (TestInner x) (TestInner y) =+  isTrue# (notI# ((x >## y) `orI#` (y >## x)))+-}++testOptAabb :: OC.Shape+            -> OC.Shape+            -> SP (SP' OB.Aabb) Bool+testOptAabb a b = SP (SP boxA boxB) (OB.aabbCheck boxA boxB)+  where boxA = OB.toAabb a+        boxB = OB.toAabb b++testWorld :: World (WorldObj ())+testWorld =+  makeWorld engineP $ stacks engineP (0.2, 0.2) (0, -4.5) (0, 0) 0 (30, 30) ()++benchy :: [Benchmark]+--benchy = [ bench "opt aabb" $ whnf (uncurry testOptAabb) testOptBoxes+benchy = [ bench "brute-force broadphase" $ nf OB.culledKeys testWorld+         , bench "grid broadphase" $ nf G.culledKeys (G.toGrid axes testWorld)+         ]+  where axes = (G.GridAxis 20 1 (-10), G.GridAxis 20 1 (-10))++main :: IO ()+main = do+  defaultMain benchy
+ bench/Physics/Constraint/Benchmark.hs view
@@ -0,0 +1,43 @@+{-# LANGUAGE MagicHash #-}++module Physics.Constraint.Benchmark where++import Criterion.Main++import Physics.Constraint+import Physics.Linear+import Utils.Utils++testConstraint :: Constraint+testConstraint = Constraint j 0+  where j = ja `join3v3` jb+        ja = negateV2 n `append2` ((xa `minusV2` p') `crossV2` n)+        jb = n `append2` ((p' `minusV2` xb) `crossV2` n)+        xa = V2 0.0## 0.0##+        xb = V2 0.0## 4.0##+        p' = V2 0.0## 2.0##+        n = V2 0.0## 1.0##++testObjPair :: (PhysicalObj, PhysicalObj)+testObjPair =+  ( PhysicalObj { _physObjVel = V2 1.0## 1.0##+                , _physObjRotVel = 0+                , _physObjPos = V2 0.0## 0.0##+                , _physObjRotPos = 0+                , _physObjInvMass = InvMass2 1.0## 2.0##+                }+  , PhysicalObj { _physObjVel = V2 1.0## (-1.0##)+                , _physObjRotVel = 0+                , _physObjPos = V2 0.0## 4.0##+                , _physObjRotPos = 0+                , _physObjInvMass = InvMass2 1.0## 2.0##+                }+  )++benchy :: Benchmark+benchy = bench "solveConstraint" $ whnf (toSP . uncurry solveConstraint) (testConstraint, testObjPair)++main :: IO ()+main = do+  print $ solveConstraint testConstraint testObjPair+  defaultMain [benchy]
+ bench/Physics/Contact/Benchmark.hs view
@@ -0,0 +1,40 @@+{-# LANGUAGE MagicHash #-}++module Physics.Contact.Benchmark where++import           GHC.Types                  (Double (D#))++import           Criterion.Main++import qualified Physics.Contact.ConvexHull as OC+import qualified Physics.Contact.SAT        as OS+import qualified Physics.Linear             as OL+import qualified Physics.Transform          as OT++import           Utils.Utils++makeOptBox ::+     Double -- ^ center x+  -> Double -- ^ center y+  -> Double -- ^ width+  -> Double -- ^ height+  -> OC.ConvexHull+makeOptBox (D# x) (D# y) (D# w) (D# h) =+  OC.listToHull $ OT.transform (OT.translateTransform (OL.V2 x y)) vertices+  where vertices = OC.rectangleVertices w h++testOptBoxes :: (OC.ConvexHull, OC.ConvexHull)+testOptBoxes = (makeOptBox 0 0 4 4, makeOptBox 1 3 2 2)++benchy' :: Benchmark+benchy' = bench "opt contact" $ whnf (evalOptContact . uncurry OS.contact) testOptBoxes++evalOptContact :: Maybe (Flipping (Either OC.Neighborhood OS.Contact )) -> OS.Contact+evalOptContact (Just (Flip (Right c))) = c+evalOptContact (Just (Same (Right c))) = c+evalOptContact _                       = error "unexpected non-contact"++main :: IO ()+main = do+  print . evalOptContact . uncurry OS.contact $ testOptBoxes+  defaultMain [benchy']
+ bench/Physics/Contact/Circle/Benchmark.hs view
@@ -0,0 +1,11 @@+{-# LANGUAGE MagicHash #-}++module Physics.Contact.Circle.Benchmark where++import Physics.Contact.Circle+import Physics.Linear++test =+  contact+    (Circle (P2 $ V2 (-1.0##) 0.0##) 1.1)+    (Circle (P2 $ V2 1.0## 0.0##) 1.1)
+ bench/Physics/Contact/GJK/Benchmark.hs view
@@ -0,0 +1,17 @@+{-# LANGUAGE MagicHash #-}++module Physics.Contact.GJK.Benchmark where++import Physics.Linear+import Physics.Contact.GJK+import Physics.Contact.ConvexHull+import Physics.Contact.Benchmark (makeOptBox)++testSimplex1 =+  closestSimplex (makeOptBox 0 0 1.9 1.9) (P2 $ V2 1.0## 1.0##)++testSimplex2 =+  closestSimplex (makeOptBox 0 0 1.9 1.9) (P2 $ V2 0.0## 1.0##)++testSimplex3 =+  closestSimplex (makeOptBox 0 0 1.9 1.9) (P2 $ V2 0.0## 0.9##)
+ shapes.cabal view
@@ -0,0 +1,139 @@+-- This file has been generated from package.yaml by hpack version 0.20.0.+--+-- see: https://github.com/sol/hpack+--+-- hash: b615f9eb72d702f005d22de824d131fac31e9486ca6dc73264dd864fb841f374++name:           shapes+version:        0.1.0.0+synopsis:       physics engine and other tools for 2D shapes+description:    Please see the README on Github at <https://github.com/ublubu/shapes#readme>+category:       Physics+homepage:       https://github.com/ublubu/shapes#readme+bug-reports:    https://github.com/ublubu/shapes/issues+author:         Kynan Rilee+maintainer:     kynan.rilee@gmail.com+copyright:      2018 Kynan Rilee+license:        BSD3+license-file:   LICENSE+build-type:     Simple+cabal-version:  >= 1.10++source-repository head+  type: git+  location: https://github.com/ublubu/shapes++library+  hs-source-dirs:+      src+  build-depends:+      array+    , base >=4.7 && <5+    , containers+    , deepseq+    , either+    , ghc-prim+    , lens+    , linear+    , mtl+    , shapes-math+    , transformers+    , vector+    , vector-th-unbox+  exposed-modules:+      Physics.Broadphase.Aabb+      Physics.Broadphase.Grid+      Physics.Constraint+      Physics.Constraint.Types+      Physics.Constraints.Contact+      Physics.Constraints.Contact.Friction+      Physics.Constraints.Contact.NonPenetration+      Physics.Constraints.SolutionProcessors+      Physics.Constraints.Types+      Physics.Contact+      Physics.Contact.Circle+      Physics.Contact.CircleVsCircle+      Physics.Contact.CircleVsHull+      Physics.Contact.ConvexHull+      Physics.Contact.GJK+      Physics.Contact.HullVsHull+      Physics.Contact.SAT+      Physics.Contact.Types+      Physics.Engine+      Physics.Engine.Class+      Physics.Engine.Main+      Physics.Linear+      Physics.Linear.Convert+      Physics.Scenes.Balls+      Physics.Scenes.FourBoxesTwoStatic+      Physics.Scenes.Rolling+      Physics.Scenes.Scene+      Physics.Scenes.Stacks+      Physics.Scenes.TwoFlyingBoxes+      Physics.Solvers.Contact+      Physics.Transform+      Physics.World+      Physics.World.Class+      Physics.World.External+      Physics.World.Object+      Utils.Descending+      Utils.Utils+  other-modules:+      Paths_shapes+  default-language: Haskell2010++executable shapes-bench+  main-is: Main.hs+  hs-source-dirs:+      bench+  build-depends:+      array+    , base >=4.7 && <5+    , containers+    , criterion+    , deepseq+    , either+    , ghc-prim+    , lens+    , linear+    , mtl+    , shapes+    , shapes-math+    , transformers+    , vector+    , vector-th-unbox+  other-modules:+      Physics.Broadphase.Benchmark+      Physics.Constraint.Benchmark+      Physics.Contact.Benchmark+      Physics.Contact.Circle.Benchmark+      Physics.Contact.GJK.Benchmark+      Paths_shapes+  default-language: Haskell2010++test-suite shapes-spec+  type: exitcode-stdio-1.0+  main-is: Spec.hs+  hs-source-dirs:+      test+  build-depends:+      QuickCheck+    , array+    , base >=4.7 && <5+    , containers+    , deepseq+    , either+    , ghc-prim+    , hspec+    , lens+    , linear+    , mtl+    , shapes+    , shapes-math+    , transformers+    , vector+    , vector-th-unbox+  other-modules:+      Physics.Broadphase.AabbSpec+      Paths_shapes+  default-language: Haskell2010
+ src/Physics/Broadphase/Aabb.hs view
@@ -0,0 +1,168 @@+{-# LANGUAGE DeriveAnyClass        #-}+{-# LANGUAGE DeriveGeneric         #-}+{-# LANGUAGE FlexibleContexts      #-}+{-# LANGUAGE MagicHash             #-}+{-# LANGUAGE MultiParamTypeClasses #-}+{-# LANGUAGE RecordWildCards       #-}+{-# LANGUAGE TemplateHaskell       #-}+{-# LANGUAGE TypeFamilies          #-}+{-# LANGUAGE TypeOperators         #-}++{- |+"Aabb" is "Axis-aligned bounding box".+The "broadphase" of collision detection is a conservative estimate of which bodies may be in contact.+-}+module Physics.Broadphase.Aabb where++import           GHC.Generics                 (Generic)+import           GHC.Prim                     (Double#, (+##), (-##), (<##),+                                               (>##))+import           GHC.Types                    (Double (D#), isTrue#)++import           Control.DeepSeq+import           Control.Lens                 (itoListOf, (^.))+import           Data.Array                   (elems)+import           Data.Maybe+import qualified Data.Vector.Unboxed          as V+import           Data.Vector.Unboxed.Deriving+import qualified Physics.Constraint           as C+import           Physics.Contact+import           Physics.Contact.Circle+import           Physics.Contact.ConvexHull+import           Physics.Linear+import           Physics.World.Class+import           Physics.World.Object++import           Utils.Descending++-- TODO: explore rewrite rules or other alternatives to manually using primops++-- | An interval, bounded above and below+data Bounds = Bounds { _bmin :: Double# -- ^ lower bound+                     , _bmax :: Double# -- ^ upper bound+                     } deriving (Eq, Generic)++instance NFData Bounds where+  rnf (Bounds _ _) = ()++derivingUnbox "Bounds"+  [t| Bounds -> (Double, Double) |]+  [| \Bounds{..} -> (D# _bmin, D# _bmax) |]+  [| \(D# bmin', D# bmax') -> Bounds bmin' bmax' |]++-- | An axis-aligned bounding box (AABB)+data Aabb = Aabb { _aabbx :: {-# UNPACK #-} !Bounds -- ^ bounds on x axis+                 , _aabby :: {-# UNPACK #-} !Bounds -- ^ bounds on y axis+                 } deriving (Eq, Generic, NFData)++derivingUnbox "Aabb"+  [t| Aabb -> (Bounds, Bounds) |]+  [| \Aabb{..} -> (_aabbx, _aabby) |]+  [| uncurry Aabb |]++instance Show Aabb where+  show (Aabb (Bounds x0 x1) (Bounds y0 y1)) =+    "Aabb " ++ show (D# x0, D# x1) ++ " " ++ show (D# y0, D# y1)++-- | Do a pair of intervals overlap?+boundsOverlap :: Bounds -> Bounds -> Bool+boundsOverlap (Bounds a b) (Bounds c d) =+  not $ isTrue# (c >## b) || isTrue# (d <## a)+{-# INLINE boundsOverlap #-}++-- | Do a pair of AABBs overlap?+aabbCheck :: Aabb -> Aabb -> Bool+aabbCheck (Aabb xBounds yBounds) (Aabb xBounds' yBounds') =+  boundsOverlap xBounds xBounds' && boundsOverlap yBounds yBounds'+{-# INLINE aabbCheck #-}++-- | Find the AABB for a convex polygon.+hullToAabb :: ConvexHull -> Aabb+hullToAabb hull = foldl1 mergeAabb aabbs+  where aabbs = fmap toAabb_ . elems . _hullVertices $ hull+{-# INLINE hullToAabb #-}++circleToAabb :: Circle -> Aabb+circleToAabb (Circle (P2 (V2 x y)) (D# r)) =+  Aabb (Bounds (x -## r) (x +## r)) (Bounds (y -## r) (y +## r))++toAabb :: Shape -> Aabb+toAabb (HullShape hull)     = hullToAabb hull+toAabb (CircleShape circle) = circleToAabb circle++-- | Get the (degenerate) AABB for a single point.+toAabb_ :: P2 -> Aabb+toAabb_ (P2 (V2 a b))= Aabb (Bounds a a) (Bounds b b)+{-# INLINE toAabb_ #-}++-- | Find the AABB of a pair of AABBs.+mergeAabb :: Aabb -> Aabb -> Aabb+mergeAabb (Aabb ax ay) (Aabb bx by) =+  Aabb (mergeRange ax bx) (mergeRange ay by)+{-# INLINE mergeAabb #-}++-- | Find the interval that contains a pair of intervals.+mergeRange :: Bounds -> Bounds -> Bounds+mergeRange (Bounds a b) (Bounds c d) = Bounds minx maxx+  where minx = if isTrue# (a <## c) then a else c+        maxx = if isTrue# (b >## d) then b else d+{-# INLINE mergeRange #-}++{- |+Find the AABB for each object in a world.++Build a vector of these AABBs, each identified by its key in the world.++Objects are ordered using the world's traversal order+-}+toAabbs :: (V.Unbox k, PhysicsWorld k w o) => w -> V.Vector (k, Aabb)+toAabbs = V.fromList . fmap f . itoListOf wObjs+  where f (k, obj) = (k, toAabb $ obj ^. woShape)+{-# INLINE toAabbs #-}++{- |+Given a world:++  *Find the AABB for each object.+  *Extract a tag from each object.+  *Build a vector of these tagged AABBs, each identified by its key in the world.++Objects are ordered using the world's traversal order+-}+toTaggedAabbs :: (V.Unbox k, V.Unbox tag, PhysicsWorld k w o) => (o -> tag) -> w -> V.Vector (k, Aabb, tag)+toTaggedAabbs toTag = V.fromList . fmap f . itoListOf wObjs+  where f (k, obj) = (k, toAabb $ obj ^. woShape, toTag obj)+{-# INLINE toTaggedAabbs #-}++{- |+Called \"unordered\" because (x, y) is equivalent to (y, x)++Given an 'Int' n, find all choices of two different 'Int's [0, n - 1]++These pairs (x, y) are in decreasing order, where x is the most significant value and y is the least significant value.+-}+unorderedPairs :: Int -> [(Int, Int)]+unorderedPairs n+  | n < 2 = []+  | otherwise = f (n - 1) (n - 2)+  where f 1 0 = [(1, 0)]+        f x 0 = (x, 0) : f (x - 1) (x - 2)+        f x y = (x, y) : f x (y - 1)+        {-# INLINE f #-}+{-# INLINE unorderedPairs #-}++-- | Find pairs of objects with overlapping AABBs.+-- Note: Pairs of static objects are excluded.+-- These pairs are in descending order according to 'unorderedPairs', where \"ascending\" is the world's traversal order.+culledKeys :: (V.Unbox k, PhysicsWorld k w o, WorldObj a ~ o) => w -> Descending (k, k)+culledKeys w = Descending . catMaybes $ fmap f ijs+  where taggedAabbs = toTaggedAabbs isStatic w+        ijs = unorderedPairs $ V.length taggedAabbs+        -- NOTE: don't aabbCheck static objects, otherwise the sim explodes+        f (i, j) = if not (isStaticA && isStaticB) && aabbCheck a b then Just (i', j') else Nothing+          where (i', a, isStaticA) = taggedAabbs V.! i+                (j', b, isStaticB) = taggedAabbs V.! j+        {-# INLINE f #-}+        isStatic WorldObj{..} = C.isStatic $ C._physObjInvMass _worldPhysObj+        {-# INLINE isStatic #-}+{-# INLINE culledKeys #-}
+ src/Physics/Broadphase/Grid.hs view
@@ -0,0 +1,139 @@+{-# LANGUAGE DeriveAnyClass         #-}+{-# LANGUAGE DeriveGeneric          #-}+{-# LANGUAGE FlexibleContexts       #-}+{-# LANGUAGE FunctionalDependencies #-}+{-# LANGUAGE MagicHash              #-}+{-# LANGUAGE MultiParamTypeClasses  #-}+{-# LANGUAGE RankNTypes             #-}+{-# LANGUAGE RecordWildCards        #-}+{-# LANGUAGE TemplateHaskell        #-}+{-# LANGUAGE TupleSections          #-}+{-# LANGUAGE TypeFamilies           #-}++module Physics.Broadphase.Grid where++import           GHC.Generics                 (Generic)+import           GHC.Types                    (Double (D#))++import           Control.DeepSeq+import           Control.Lens+import           Data.Foldable                (foldl')+import qualified Data.IntMap.Strict           as IM+import           Data.List                    (sortBy)+import           Data.Maybe                   (mapMaybe)+import qualified Data.Vector.Unboxed          as V+import           Data.Vector.Unboxed.Deriving++import           Physics.Broadphase.Aabb      (Aabb (..), Bounds (..),+                                               aabbCheck, toTaggedAabbs)+import qualified Physics.Constraint           as C+import           Physics.Contact.ConvexHull+import           Physics.World.Class+import           Physics.World.Object+import           Utils.Descending+import           Utils.Utils++{- |+The grid is indexed in row-major order:++3 4 5+0 1 2++(where X is horizontal and Y is vertical)++* The grid is only used for shape queries, so it should only contain AABBs.+* We may want a reverse-lookup from shape ID to grid squares in the future.+-}+data Grid = Grid+  { _gridSquares :: IM.IntMap (IM.IntMap TaggedAabb)+  , _gridX       :: !GridAxis+  , _gridY       :: !GridAxis+  } deriving (Eq, Show, Generic, NFData)++data GridAxis = GridAxis+  { _gridLength :: !Int+  , _gridUnit   :: !Double+  , _gridOrigin :: !Double+  } deriving (Eq, Show, Generic, NFData)++data TaggedAabb = TaggedAabb+  { _taggedStatic :: !Bool+  , _taggedBox    :: !Aabb+  } deriving (Eq, Show, Generic, NFData)++makeLenses ''Grid+makeLenses ''GridAxis++toGrid :: (PhysicsWorld Int w o, WorldObj a ~ o) => (GridAxis, GridAxis) -> w -> Grid+toGrid axes@(xAxis, yAxis) w = Grid (fromTaggedAabbs axes taggedAabbs) xAxis yAxis+  where taggedAabbs = toTaggedAabbs isStatic w+        isStatic WorldObj{..} = C.isStatic $ C._physObjInvMass _worldPhysObj++culledKeys :: Grid -> Descending (Int, Int)+culledKeys Grid{..} = Descending . uniq . sortBy f . concat $ culledKeys' <$> IM.elems _gridSquares+  where f x y = case compare x y of LT -> GT+                                    EQ -> EQ+                                    GT -> LT++culledKeys' :: IM.IntMap TaggedAabb -> [(Int, Int)]+culledKeys' square = mapMaybe colliding $ allPairs $ IM.toDescList square+  where colliding ((_, (TaggedAabb True _)), (_, (TaggedAabb True _))) = Nothing+        -- ^ Don't check two static shapes for collision.+        colliding ((a, (TaggedAabb _ boxA)), (b, (TaggedAabb _ boxB)))+          | aabbCheck boxA boxB = Just (a, b)+          | otherwise = Nothing++allPairs :: [a] -> [(a, a)]+allPairs [] = []+allPairs (x:xs) = f [] x xs+  where f accumPairs first [] = accumPairs+        f accumPairs first remaining@(x:xs) = f (foldl' g accumPairs remaining) x xs+          where g accumPairs x = (first, x):accumPairs++uniq :: Eq a => [a] -> [a]+uniq [] = []+uniq (x:[]) = [x]+uniq (x:y:rest)+  | x == y = uniq (x:rest)+  | otherwise = x : uniq (y:rest)++fromTaggedAabbs :: (GridAxis, GridAxis) -> V.Vector (Int, Aabb, Bool) -> IM.IntMap (IM.IntMap TaggedAabb)+fromTaggedAabbs (x, y) = V.foldl' insertBox IM.empty+  where+    insertBox grid (key, box, isStatic) = foldl' insertBoxAt grid indices+      where+        indices = boxIndices (x, y) box+        insertBoxAt grid index =+          grid & at index . non IM.empty . at key .~ Just taggedBox+        taggedBox = TaggedAabb isStatic box++-- | Flatten a pair of axial indices to a single grid index.+flattenIndex :: Grid -> (Int, Int) -> Int+flattenIndex Grid{..} (x, y) = flattenIndex' _gridX (x, y)++-- | Flatten a pair of axial indices to a single grid index.+flattenIndex' :: GridAxis -> (Int, Int) -> Int+flattenIndex' xAxis@GridAxis{..} (x, y) = x + (y * _gridLength)++-- | Flattened grid index of a given point.+pointIndex :: Grid -> (Double, Double) -> Int+pointIndex grid@Grid{..} (x, y) = flattenIndex' _gridX (i, j)+  where i = axialIndex _gridX x+        j = axialIndex _gridY y++-- | Index along a single axis.+axialIndex :: GridAxis -> Double -> Int+axialIndex GridAxis{..} val =+  floor $ (val - _gridOrigin) / _gridUnit++-- | All flattened grid indices that match a given 'Aabb'.+boxIndices :: (GridAxis, GridAxis) -> Aabb -> [Int]+boxIndices (xAxis, yAxis) Aabb {..} = do+  x <- axisRange _aabbx xAxis+  y <- axisRange _aabby yAxis+  return $ flattenIndex' xAxis (x, y)+  where+    axisRange (Bounds min max) axis = [minIx .. maxIx]+      where+        minIx = axialIndex axis (D# min)+        maxIx = axialIndex axis (D# max)
+ src/Physics/Constraint.hs view
@@ -0,0 +1,233 @@+{-# LANGUAGE BangPatterns           #-}+{-# LANGUAGE DataKinds              #-}+{-# LANGUAGE DeriveAnyClass         #-}+{-# LANGUAGE DeriveGeneric          #-}+{-# LANGUAGE FlexibleInstances      #-}+{-# LANGUAGE FunctionalDependencies #-}+{-# LANGUAGE MagicHash              #-}+{-# LANGUAGE MultiParamTypeClasses  #-}+{-# LANGUAGE RecordWildCards        #-}+{-# LANGUAGE TemplateHaskell        #-}+{-# LANGUAGE TypeFamilies           #-}+{-# OPTIONS_GHC -fno-warn-orphans #-}++{- |+Types for describing the motion of physical objects.+Functions for solving constraints.+-}+module Physics.Constraint ( module Physics.Constraint+                          , module Physics.Constraint.Types+                          ) where++import           GHC.Generics                 (Generic)+import           GHC.Prim                     (Double#, (/##), (==##))+import           GHC.Types                    (Double (D#), isTrue#)++import           Control.DeepSeq+import           Control.Lens                 hiding (transform)+import           Data.Vector.Unboxed.Deriving++import           Physics.Constraint.Types+import           Physics.Linear+import           Physics.Transform+import           Utils.Utils++-- | Multiplicative inverse of linear and rotational mass+data InvMass2 = InvMass2 { _imLin :: Double#+                         , _imRot :: Double#+                         } deriving (Show, Eq)++instance NFData InvMass2 where+  rnf (InvMass2 _ _) = ()+  {-# INLINE rnf #-}++derivingUnbox "InvMass2"+  [t| InvMass2 -> (Double, Double) |]+  [| \InvMass2{..} -> (D# _imLin, D# _imRot) |]+  [| \(D# linMass, D# rotMass) -> InvMass2 linMass rotMass |]++-- | The state of motion for a physical body.+-- Rotation is measured in the Z direction (right-handed coordinates).+data PhysicalObj = PhysicalObj { _physObjVel     :: !V2+                               , _physObjRotVel  :: !Double+                               , _physObjPos     :: !V2+                               , _physObjRotPos  :: !Double+                               , _physObjInvMass :: !InvMass2+                               } deriving (Show, Generic, NFData)+makeLenses ''PhysicalObj++derivingUnbox "PhysicalObj"+  [t| PhysicalObj -> (V2, Double, V2, Double, InvMass2) |]+  [| \PhysicalObj{..} -> (_physObjVel, _physObjRotVel, _physObjPos, _physObjRotPos, _physObjInvMass) |]+  [| \(vel, rotvel, pos, rotPos, invMass) -> PhysicalObj vel rotvel pos rotPos invMass |]++_physObjVel3 :: PhysicalObj -> V3+_physObjVel3 po = _physObjVel po `append2` _physObjRotVel po+{-# INLINE _physObjVel3 #-}++-- | Lens for 3D velocity vector: (v_x, v_y, v_rot)+physObjVel3 :: Functor f => (V3 -> f V3) -> PhysicalObj -> f PhysicalObj+physObjVel3 f po = fmap g (f (_physObjVel3 po))+  where g v3' = po & physObjVel .~ v & physObjRotVel .~ vr+          where !(v, vr) = split3 v3'+        {-# INLINE g #-}+{-# INLINE physObjVel3 #-}++-- | Convert (linear mass, rotational inertia) into 'InvMass2'.+-- Use 0 for infinite mass (non-translating/non-rotating objects).+toInvMass2 :: (Double, Double) -> InvMass2+toInvMass2 (D# ml, D# mr) = InvMass2 (invert ml) (invert mr)+  where invert m = if isTrue# (m ==## 0.0##) then 0.0## else 1.0## /## m+        {-# INLINE invert #-}+{-# INLINE toInvMass2 #-}++-- | A constraint equation between two objects+-- to be solved using the objects' state of motion+data Constraint = Constraint { _constraintJ :: !V6 -- ^ Jacobian - coordinate transform to the constraint space+                             , _constraintB :: !Double -- ^ extra term+                             } deriving Show+-- | Generates a constraint equation from a pair of objects+type Constraint' p = (p, p) -> Constraint+-- | Are these two different motion states?+-- Used to determine whether the constraint solver has converged.+type PhysObjChanged = PhysicalObj -> PhysicalObj -> Bool++derivingUnbox "Constraint"+  [t| Constraint -> (V6, Double) |]+  [| \Constraint{..} -> (_constraintJ, _constraintB) |]+  [| uncurry Constraint |]++instance Flippable Constraint where+  flipp (Constraint j b) = Constraint (flip3v3 j) b+  {-# INLINE flipp #-}++-- | Get a 6D velocity vector for a pair of objects.+-- (a_vx, a_vy, a_vr, b_vx, b_vy, b_vr)+--+-- Called \"constrained\" because it's used with objects constrained together.+_constrainedVel6 :: (PhysicalObj, PhysicalObj) -> V6+_constrainedVel6 cp = uncurry join3v3 (pairMap (view physObjVel3) cp)+{-# INLINE _constrainedVel6 #-}++-- | Lens for 6D velocity vector ('_constrainedVel6')+constrainedVel6 :: (Functor f) => (V6 -> f V6) -> (PhysicalObj, PhysicalObj) -> f (PhysicalObj, PhysicalObj)+constrainedVel6 f cp = fmap g (f (_constrainedVel6 cp))+  where g v6 = pairMap h (split3v3 v6) `pairAp` cp+        h v3 po = po & physObjVel3 .~ v3+{-# INLINE constrainedVel6 #-}++-- | 6x6 diagonal matrix of inverse mass+--+-- > invMassM2 (InvMass2 ma ia) (InvMass2 mb ib) = Diag6 (V6 ma ma ia mb mb ib)+invMassM2 :: InvMass2 -> InvMass2 -> Diag6+invMassM2 (InvMass2 ma ia) (InvMass2 mb ib) = Diag6 (V6 ma ma ia mb mb ib)+{-# INLINE invMassM2 #-}++-- | Is this object completely static (unmoving)?+isStatic :: InvMass2 -> Bool+isStatic = (== InvMass2 0.0## 0.0##)+{-# INLINE isStatic #-}++-- | Is this object non-translating (no center-of-mass movement)?+isStaticLin :: InvMass2 -> Bool+isStaticLin x = isTrue# (0.0## ==## _imLin x)+{-# INLINE isStaticLin #-}++-- | Is this object non-rotating?+isStaticRot :: InvMass2 -> Bool+isStaticRot x = isTrue# (0.0## ==## _imRot x)+{-# INLINE isStaticRot #-}++-- | see 'invMassM2'+_constrainedInvMassM2 :: (PhysicalObj, PhysicalObj) -> Diag6+_constrainedInvMassM2 cp = uncurry invMassM2 (pairMap (view physObjInvMass) cp)+{-# INLINE _constrainedInvMassM2 #-}++-- | Get 'WorldTransform' from origin to the current position+-- (translation & rotation) of an object.+_physObjTransform :: PhysicalObj -> WorldTransform+_physObjTransform obj = toTransform (_physObjPos obj) rot+  where !(D# rot) = _physObjRotPos obj+{-# INLINE _physObjTransform #-}++-- TODO: dedupe this & _constrainedVel6+-- | Get a 6D velocity vector for a pair of objects.+-- Same as '_constrainedVel6'+velocity2 :: PhysicalObj -> PhysicalObj -> V6+velocity2 a b = (va `append2` wa) `join3v3` (vb `append2` wb)+  where va = _physObjVel a+        vb = _physObjVel b+        wa = _physObjRotVel a+        wb = _physObjRotVel b+{-# INLINE velocity2 #-}++-- | Use objects' current state of motion to solve their constraint equation.+--+-- The 'Lagrangian' multiplier is the (signed) magnitude+-- of the constraint impulse along the constraint axis.+lagrangian2 :: (PhysicalObj, PhysicalObj) -> Constraint -> Lagrangian+lagrangian2 (o1, o2) (Constraint j b) =+  Lagrangian $ (-(D# (j `dotV6` v) + b)) / mc+  where v = velocity2 o1 o2+        mc = effMassM2 j o1 o2+{-# INLINE lagrangian2 #-}++-- TODO: rename effMassM2 to invEffMass+-- | The inverse effective mass of a pair of objects along the constraint axis+effMassM2 :: V6 -- ^ Jacobian+          -> PhysicalObj+          -> PhysicalObj+          -> Double -- ^ Inverse of effective mass+effMassM2 j a b = D# ((j `vmulDiag6` im) `dotV6` j)+  where im = curry _constrainedInvMassM2 a b+{-# INLINE effMassM2 #-}++-- | Get the impulse that solves a constraint equation.+constraintImpulse2 :: V6 -- ^ Jacobian+                   -> Lagrangian+                   -> V6 -- ^ 6D constraint impulse vector+constraintImpulse2 j (Lagrangian l) = l `smulV6` j+{-# INLINE constraintImpulse2 #-}++-- | Apply a constraint impulse to two objects.+updateVelocity2_ :: V6 -- ^ 6D velocity for two objects+                 -> Diag6 -- ^ Inverse mass for two objects+                 -> V6 -- ^ 6D constraint impulse+                 -> V6 -- ^ New 6D velocity+updateVelocity2_ v im pc = v `plusV6` (im `vmulDiag6'` pc)+{-# INLINE updateVelocity2_ #-}++-- | Use a Lagrangian multiplier to update a pair of objects.+applyLagrangian2 :: Diag6 -- ^ Inverse mass+                 -> V6 -- ^ Jacobian+                 -> Lagrangian+                 -> (PhysicalObj, PhysicalObj)+                 -> (PhysicalObj, PhysicalObj)+applyLagrangian2 im j lagr = constrainedVel6 %~ f+  where f v6 = updateVelocity2_ v6 im (constraintImpulse2 j lagr)+{-# INLINE applyLagrangian2 #-}++-- | Solve a constraint between two objects.+solveConstraint :: Constraint -- ^ Constraint equation+                -> (PhysicalObj, PhysicalObj)+                -> (PhysicalObj, PhysicalObj) -- ^ Updated state of motion+solveConstraint c ab =+  applyLagrangian (lagrangian2 ab c) c ab+{-# INLINE solveConstraint #-}++-- | Use a Lagrangian multiplier to update a pair of objects.+applyLagrangian :: Lagrangian -- ^ Lagrangian multiplier from solving the constraint+                -> Constraint -- ^ The constraint equation+                -> (PhysicalObj, PhysicalObj)+                -> (PhysicalObj, PhysicalObj) -- ^ Updated state of motion+applyLagrangian lagr (Constraint j _) ab =+  applyLagrangian2 (_constrainedInvMassM2 ab) j lagr ab+{-# INLINE applyLagrangian #-}++-- | Advance the position (translation & rotation) of an object by+-- applying its velocity over a time delta.+advanceObj :: PhysicalObj -> Double -> PhysicalObj+advanceObj obj dt = obj & physObjPos %~ f & physObjRotPos %~ g+  where f pos = (dt `smulV2` (obj ^. physObjVel)) `plusV2` pos+        g ori = (dt * (obj ^. physObjRotVel)) + ori+{-# INLINE advanceObj #-}
+ src/Physics/Constraint/Types.hs view
@@ -0,0 +1,27 @@+{-# LANGUAGE TypeFamilies #-}+{-# LANGUAGE MultiParamTypeClasses #-}+{-# LANGUAGE GeneralizedNewtypeDeriving #-}+{-# LANGUAGE TemplateHaskell #-}++{- |+Just pretend this module is part of Physics.Constraint.+Last time I checked, GeneralizedNewtypeDeriving didn't work on Lagrangian in Physics.Constraint.+-}+module Physics.Constraint.Types where++import Control.DeepSeq+import Control.Lens+import Data.Vector.Unboxed.Deriving++-- TODO: Why doesn't GeneralizedNewtypeDeriving work in Physics.Constraint?+-- | Lagrangian multiplier - the (signed) magnitude of the impulse+-- required to solve a constraint equation.+newtype Lagrangian =+  Lagrangian { _lagrangianVal :: Double } deriving (Show, Num, Eq, Ord, NFData)++makeLenses ''Lagrangian++derivingUnbox "Lagrangian"+  [t| Lagrangian -> Double |]+  [| \(Lagrangian l) -> l |]+  [| Lagrangian |]
+ src/Physics/Constraints/Contact.hs view
@@ -0,0 +1,83 @@+{-# LANGUAGE DeriveAnyClass        #-}+{-# LANGUAGE DeriveGeneric         #-}+{-# LANGUAGE MultiParamTypeClasses #-}+{-# LANGUAGE RankNTypes            #-}+{-# LANGUAGE RecordWildCards       #-}+{-# LANGUAGE TemplateHaskell       #-}+{-# LANGUAGE TypeFamilies          #-}++{- |+Generate and solve all contact constraints for pairs of colliding objects.+-}+module Physics.Constraints.Contact where++import           GHC.Generics                               (Generic)++import           Control.DeepSeq+import           Control.Lens+import qualified Data.Vector.Unboxed                        as V+import           Data.Vector.Unboxed.Deriving++import           Physics.Constraint+import qualified Physics.Constraints.Contact.Friction       as F+import qualified Physics.Constraints.Contact.NonPenetration as NP+import           Physics.Constraints.Types+import           Physics.Contact+import           Physics.Contact.Types+import           Utils.Descending+import           Utils.Utils++{- |+Indicates a specific pair of "features" on a specific pair of objects that are touching.+This is how we check if we can reuse a cached solution from the previous frame.+(We can reuse the cached solution if it has the same 'ObjectFeatureKey')+-}+data ObjectFeatureKey k = ObjectFeatureKey+  { _ofkObjKeys  :: (k, k) -- ^ (first shape's key, second shape's key)+  , _ofkFeatKeys :: (Int, Int) -- ^ (first shape's feature's key, second shape's feature's key)+  } deriving (Generic, Show, NFData, Eq, Ord)+makeLenses ''ObjectFeatureKey+derivingUnbox+  "ObjectFeatureKey"+  [t|forall k. (V.Unbox k) =>+                 ObjectFeatureKey k -> ((k, k), (Int, Int))|]+  [|\ObjectFeatureKey {..} -> (_ofkObjKeys, _ofkFeatKeys)|]+  [|uncurry ObjectFeatureKey|]++-- | Calculate all contacts between a pair of shapes.+keyedContacts ::+     (k, k)+  -> (Shape, Shape)+  -> Descending (ObjectFeatureKey k, Flipping Contact')+keyedContacts ij ab = fmap f contacts+  where contacts = generateContacts ab+        f (featKeys, contact) = (ObjectFeatureKey ij featKeys, contact)+        {-# INLINE f #-}+{-# INLINE keyedContacts #-}++-- | Build a constraint from a pair of shapes and a contact between them.+constraintGen ::+     ContactBehavior+  -> Double+  -> Flipping Contact'+  -> (PhysicalObj, PhysicalObj)+  -> ContactResult Constraint+constraintGen beh dt fContact ab =+  ContactResult { _crNonPen = NP.constraintGen beh dt fContact ab+                , _crFriction = F.constraintGen fContact ab }+{-# INLINE constraintGen #-}++{- |+Given an already-applied Lagrangian and the newly-calculated Lagrangian,+figure out what portion of the newly-calculated Lagrangian should actually be applied.+-}+solutionProcessor ::+     (Double, Double) -- ^ coefficients of friction for a pair of shapes (a, b)+  -> ContactResult Lagrangian -- ^ cached solution+  -> ContactResult Lagrangian -- ^ new incremental solution+  -> Processed (ContactResult Lagrangian) -- ^ 1. incremental solution to actually apply, 2. new cached solution+solutionProcessor mu_ab (ContactResult npCached fCached) (ContactResult npNew fNew) =+  ContactResult <$> npProcessed <*> fProcessed+  where npProcessed = NP.solutionProcessor npCached npNew+        fProcessed = F.solutionProcessor mu_ab (_processedToCache npProcessed) fCached fNew+{-# INLINE solutionProcessor #-}
+ src/Physics/Constraints/Contact/Friction.hs view
@@ -0,0 +1,55 @@+{-# LANGUAGE RecordWildCards #-}++{- |+Generate and solve friction constraints for colliding objects.+-}+module Physics.Constraints.Contact.Friction where++import Control.Lens++import Physics.Constraint+import Physics.Constraints.Types+import Physics.Constraints.SolutionProcessors+import Physics.Contact.Types+import Physics.Linear++import Utils.Utils++constraintGen :: Flipping Contact'+              -> (PhysicalObj, PhysicalObj)+              -> Constraint+constraintGen fContact ab =+  flipExtract $ flipMap toConstraint fContact ab+{-# INLINE constraintGen #-}++toConstraint :: Contact'+             -> (PhysicalObj, PhysicalObj)+             -> Constraint+toConstraint c ab = Constraint (jacobian c ab) 0+{-# INLINE toConstraint #-}++jacobian :: Contact'+         -> (PhysicalObj, PhysicalObj)+         -> V6+jacobian Contact'{..} (a, b) = ja `join3v3` jb+  where ja = ta `append2` ((p' `minusV2` xa) `crossV2` ta)+        jb = tb `append2` ((p' `minusV2` xb) `crossV2` tb)+        xa = _physObjPos a+        xb = _physObjPos b+        (P2 p') = _contactPenetrator'+        ta = negateV2 tb+        tb = clockwiseV2 n+        n = _contactEdgeNormal'+{-# INLINE jacobian #-}++pairMu :: (Double, Double) -> Double+pairMu (ua, ub) = (ua + ub) / 2+{-# INLINE pairMu #-}++solutionProcessor :: (Double, Double)+                  -> Lagrangian+                  -> Lagrangian+                  -> Lagrangian+                  -> Processed Lagrangian+solutionProcessor ab nonpen = clampAbs (nonpen & lagrangianVal *~ pairMu ab)+{-# INLINE solutionProcessor #-}
+ src/Physics/Constraints/Contact/NonPenetration.hs view
@@ -0,0 +1,62 @@+{-# LANGUAGE RecordWildCards #-}++{- |+Generate and solve non-penetration constraints for colliding objects.+-}+module Physics.Constraints.Contact.NonPenetration where++import Physics.Constraint+import Physics.Constraints.Types+import Physics.Constraints.SolutionProcessors+import Physics.Contact.Types+import Physics.Linear++import Utils.Utils++constraintGen :: ContactBehavior+              -> Double+              -> Flipping Contact'+              -> (PhysicalObj, PhysicalObj)+              -> Constraint+constraintGen beh dt fContact ab =+  flipExtract $ flipMap (toConstraint beh dt) fContact ab+{-# INLINE constraintGen #-}++toConstraint :: ContactBehavior+             -> Double+             -> Contact'+             -> (PhysicalObj, PhysicalObj)+             -> Constraint+toConstraint beh dt c ab = Constraint (jacobian c ab) (baumgarte beh dt c)+{-# INLINE toConstraint #-}++-- TODO: comment or name stuff so it's clear that `a` is penetrated by `b`+jacobian :: Contact'+         -> (PhysicalObj, PhysicalObj)+         -> V6+jacobian Contact'{..} (a, b) = ja `join3v3` jb+  where ja = negateV2 n `append2` ((xa `minusV2` p') `crossV2` n)+        jb = n `append2` ((p' `minusV2` xb) `crossV2` n)+        xa = _physObjPos a+        xb = _physObjPos b+        (P2 p') = _contactPenetrator'+        n = _contactEdgeNormal'+{-# INLINE jacobian #-}++-- add extra energy if the penetration exceeds the allowed slop+-- (i.e. subtract from C' = Jv + b in constraint C' <= 0)+baumgarte :: ContactBehavior+          -> Double+          -> Contact'+          -> Double+baumgarte beh dt c = if d > slop then (b / dt) * (slop - d) else 0+  where b = contactBaumgarte beh+        slop = contactPenetrationSlop beh+        d = _contactDepth' c+{-# INLINE baumgarte #-}++solutionProcessor :: Lagrangian+                  -> Lagrangian+                  -> Processed Lagrangian+solutionProcessor = positive+{-# INLINE solutionProcessor #-}
+ src/Physics/Constraints/SolutionProcessors.hs view
@@ -0,0 +1,53 @@+{- |+SolutionProcessors take incremental and accumulated constraint solutions+and use rules to determine what incremental impulse (constraint solution) to apply to an object.++For example, a solution processor might enforce that the total accumulated impulse is nonnegative.+-}+module Physics.Constraints.SolutionProcessors where++import           Physics.Constraint+import           Physics.Constraints.Types++wrapProcessor :: (Lagrangian -> Lagrangian -> Lagrangian)+              -> Lagrangian+              -> Lagrangian+              -> Processed Lagrangian+wrapProcessor f cached_l new_l =+  Processed {_processedToCache = cached_l', _processedToApply = apply_l}+  where+    cached_l' = cached_l + apply_l+    apply_l = f cached_l new_l+{-# INLINE wrapProcessor #-}++-- | Apply the entire newly-calculated Lagrangian.+simple :: Lagrangian -> Lagrangian -> Processed Lagrangian+simple = wrapProcessor (flip const)+{-# INLINE simple #-}++{- |+Ensure that the sum of the applied Lagrangians is always positive.+This is useful if a constraint should only apply impulse in one direction.+e.g. Non-penetration should resist penetration but have no effect on separation.+-}+positive :: Lagrangian -> Lagrangian -> Processed Lagrangian+positive = wrapProcessor (\cached_l new_l -> max new_l (-cached_l))+{-# INLINE positive #-}++{- |+Ensure that the magnitude of the sum of the applied Lagrangians never exceeds a threshold.+This is useful if there's a limit to the force a constraint can apply.+e.g. Friction resists sliding motion, but this force is limited.+-}+clampAbs :: Lagrangian -> Lagrangian -> Lagrangian -> Processed Lagrangian+clampAbs maxThresh cached new =+  Processed {_processedToCache = accum_l', _processedToApply = apply_l}+  where+    accum_l = cached + new+    accum_l'+      | accum_l > maxThresh = maxThresh+      | accum_l < minThresh = minThresh+      | otherwise = accum_l+    apply_l = accum_l' - cached+    minThresh = -maxThresh+{-# INLINE clampAbs #-}
+ src/Physics/Constraints/Types.hs view
@@ -0,0 +1,81 @@+{-# LANGUAGE MultiParamTypeClasses #-}+{-# LANGUAGE RankNTypes            #-}+{-# LANGUAGE RecordWildCards       #-}+{-# LANGUAGE TemplateHaskell       #-}+{-# LANGUAGE TypeFamilies          #-}++{- |+Types used in generating and solving contact constraints.+-}+module Physics.Constraints.Types where++import           Control.Lens+import           Data.Monoid+import           Data.Vector.Unboxed          (Unbox)+import           Data.Vector.Unboxed.Deriving++import           Physics.Constraint+import           Physics.Contact.Types++import           Utils.Utils++type ContactConstraintGen a = Flipping Contact' -> (a, a) -> Constraint++{- |+Used by "solution processors", which take a cached solution and a new solution+and decide what the new incremental solution should be.+-}+data Processed a =+  Processed { _processedToCache :: !a -- ^ the old cached solution + the new incremental solution+            , _processedToApply :: !a -- ^ the new incremental solution to apply+            }++{- |+Some 'SolutionProcessor's use contextual information.+e.g. The 'SolutionProcessor' for friction needs to know the coefficient of friction and the normal force.+(Normal force is the solution to the non-penetration constraint.)+-}+type SolutionProcessor a b = a -> b -> b -> Processed b++{- |+Used in the constraint solver to cache solutions ('ContactResult Lagrangian')+and constraints ('ContactResult Constraint') in unboxed vectors.++Constraints are calculated from contacts, and Lagrangians are calculated from these constraints,+which makes both types "results" of a contact.+-}+data ContactResult a = ContactResult+  { _crNonPen   :: a -- ^ "result" related to the non-penetration constraint+  , _crFriction :: a -- ^ "result" related to the friction constraint+  }++derivingUnbox "ContactResult"+  [t| forall a. (Unbox a) => ContactResult a -> (a, a) |]+  [| \ContactResult{..} -> (_crNonPen, _crFriction) |]+  [| uncurry ContactResult |]++makeLenses ''ContactResult++instance Functor ContactResult where+  fmap f (ContactResult a b) = ContactResult (f a) (f b)+  {-# INLINE fmap #-}++instance Applicative ContactResult where+  pure x = ContactResult x x+  ContactResult f g <*> ContactResult x y = ContactResult (f x) (g y)+  {-# INLINE pure #-}+  {-# INLINE (<*>) #-}++-- do nonpen before friction+instance Foldable ContactResult where+  foldMap f (ContactResult a b) = f a <> f b++instance Functor Processed where+  fmap f (Processed a b) = Processed (f a) (f b)+  {-# INLINE fmap #-}++instance Applicative Processed where+  pure x = Processed x x+  Processed f g <*> Processed x y = Processed (f x) (g y)+  {-# INLINE pure #-}+  {-# INLINE (<*>) #-}
+ src/Physics/Contact.hs view
@@ -0,0 +1,45 @@+{-# LANGUAGE DeriveAnyClass #-}+{-# LANGUAGE DeriveGeneric  #-}++module Physics.Contact where++import           GHC.Generics                   (Generic)++import           Control.DeepSeq+import           Physics.Contact.Circle+import qualified Physics.Contact.CircleVsCircle as CC+import qualified Physics.Contact.CircleVsHull   as CH+import           Physics.Contact.ConvexHull+import qualified Physics.Contact.HullVsHull     as HH+import           Physics.Contact.Types+import           Physics.Linear+import           Utils.Descending+import           Utils.Utils++data Shape = HullShape ConvexHull | CircleShape Circle+  deriving (Show, Generic, NFData)++generateContacts ::+  (Shape, Shape)+  -> Descending ((Int, Int), Flipping Contact')+generateContacts (CircleShape a, CircleShape b) =+  Descending $+  case CC.generateContacts a b of+    Nothing      -> []+    Just contact -> [((0, 0), Same contact)]+generateContacts (CircleShape a, HullShape b) =+  Descending $+  case CH.generateContacts a b of+    Nothing                     -> []+    Just (hullFeature, contact) -> [((0, hullFeature), Same contact)]+generateContacts (HullShape a, CircleShape b) =+  Descending $+  case CH.generateContacts b a of+    Nothing                     -> []+    Just (hullFeature, contact) -> [((hullFeature, 0), Flip contact)]+generateContacts (HullShape a, HullShape b) = HH.generateContacts (a, b)++-- assumes scale-invariant transform from localspace+setShapeTransform :: Shape -> (P2 -> P2) -> Shape+setShapeTransform (HullShape hull) = HullShape . setHullTransform hull+setShapeTransform (CircleShape circle) = CircleShape . setCircleTransform circle
+ src/Physics/Contact/Circle.hs view
@@ -0,0 +1,71 @@+{-# LANGUAGE DeriveAnyClass        #-}+{-# LANGUAGE DeriveGeneric         #-}+{-# LANGUAGE FlexibleContexts      #-}+{-# LANGUAGE FlexibleInstances     #-}+{-# LANGUAGE MagicHash             #-}+{-# LANGUAGE MultiParamTypeClasses #-}+{-# LANGUAGE RecordWildCards       #-}+{-# LANGUAGE ScopedTypeVariables   #-}+{-# LANGUAGE TemplateHaskell       #-}+{-# LANGUAGE TypeFamilies          #-}++module Physics.Contact.Circle where++import           GHC.Generics    (Generic)++import           Control.DeepSeq+import           Physics.Linear++data Circle = Circle+  { _circleCenter :: !P2 -- ^ in world coordinates+  , _circleRadius :: !Double+  } deriving (Show, Generic, NFData)++circleWithRadius :: Double -> Circle+circleWithRadius = Circle (P2 (V2 0.0## 0.0##))++-- TODO: use the same type as Physics.Contact+data Contact = Contact+  { _contactCenter :: !P2+  , _contactDepth  :: !Double+  , _contactNormal :: !V2+  } deriving (Show, Generic, NFData)++{- |+The normal points out of the "penetrated" circle.+-}+contact :: Circle+  -- ^ the penetratee+  -> Circle+  -- ^ the penetrator+  -> Maybe Contact+contact circleA circleB+  | rab * rab >= abSq =+    Just Contact {_contactCenter = center, _contactDepth = depth, _contactNormal = abN}+  | otherwise = Nothing+  where+    a = _circleCenter circleA+    b = _circleCenter circleB+    ab = diffP2 b a+    -- ^ vector from 'a' to 'b'+    ra = _circleRadius circleA+    rb = _circleRadius circleB+    rab = ra + rb+    abSq = sqLengthV2 ab+    -- ^ squared length of 'ab'+    abLength = sqrt abSq+    abN = abLength `sdivV2` ab+    -- ^ normalized 'ab'+    a' = (ra `smulV2` abN) `vplusP2` a+    -- ^ edge of circle A+    b' = ((-rb) `smulV2` abN) `vplusP2` b+    -- ^ edge of circle B+    center = midpointP2 a' b'+    depth = ra + rb - abLength++-- assumes scale-invariant transform from localspace+setCircleTransform :: Circle+  -> (P2 -> P2)+  -> Circle+setCircleTransform Circle {..} fromLocalSpace =+  Circle (fromLocalSpace zeroP2) _circleRadius
+ src/Physics/Contact/CircleVsCircle.hs view
@@ -0,0 +1,19 @@+{-# LANGUAGE MagicHash       #-}+{-# LANGUAGE RecordWildCards #-}+{-# LANGUAGE TupleSections   #-}++module Physics.Contact.CircleVsCircle where++import           GHC.Types              (Double (..))++import           Data.Either+import           Physics.Contact.Circle+import           Physics.Contact.Types+import           Physics.Linear++generateContacts :: Circle -> Circle -> Maybe Contact'+generateContacts circleA circleB =+  case contact circleA circleB of+    Nothing -> Nothing+    Just Contact {..} ->+      Just (Contact' _contactNormal _contactCenter _contactDepth)
+ src/Physics/Contact/CircleVsHull.hs view
@@ -0,0 +1,69 @@+{-# LANGUAGE MagicHash       #-}+{-# LANGUAGE RecordWildCards #-}+{-# LANGUAGE TupleSections   #-}++module Physics.Contact.CircleVsHull where++import           GHC.Types                  (Double (..))++import           Data.Either+import           Physics.Contact.Circle+import           Physics.Contact.ConvexHull+import           Physics.Contact.GJK+import           Physics.Contact.Types+import           Physics.Linear+import           Utils.Utils++-- | There's only one contact point between a circle and a convex hull.+generateContacts :: Circle -> ConvexHull -> Maybe (Int, Contact')+  -- ^ (hull feature index, contact manifold) -- circle is always the penetrator+generateContacts circle@Circle {..} hull = convertSimplex circle simplex+  where+    simplex = closestSimplex hull _circleCenter++-- TODO: handle the "deep overlap" case (3-point simplex)+convertSimplex :: Circle -> Simplex -> Maybe (Int, Contact')+convertSimplex circle (Simplex' simplex) = convertSimplex12 circle simplex+convertSimplex _ (Simplex3' _)           = Nothing++convertSimplex12 :: Circle -> Simplex12 -> Maybe (Int, Contact')+convertSimplex12 circle = either (processSimplex1 circle) (processSimplex2 circle)++processSimplex1 :: Circle -> Simplex1 -> Maybe (Int, Contact')+processSimplex1 circle (Simplex1 aa) =+  (_neighborhoodIndex aa, ) <$> processSimplex_ circle (_neighborhoodCenter aa)++processSimplex2 :: Circle -> Simplex2 -> Maybe (Int, Contact')+processSimplex2 circle@Circle {..} simplex@(Simplex2 feature _) =+  (_neighborhoodIndex feature, ) <$> processSimplex_ circle a+  where+    a = _circleCenter `closestAlong` simplex++processSimplex_ :: Circle -> P2 -> Maybe Contact'+processSimplex_ Circle {..} a+  | sqRadius < abSq = Nothing -- distance greater than circle radius+  | otherwise =+    Just $+    Contact'+    { _contactEdgeNormal' = negateV2 normal+    , _contactPenetrator' = a+    , _contactDepth' = _circleRadius - abLength+    }+  where+    b = _circleCenter+    sqRadius = _circleRadius * _circleRadius+    ab = diffP2 b a+    abSq = sqLengthV2 ab+    abLength = sqrt abSq+    normal = sdivV2 abLength ab++closestAlong :: P2 -- ^ target point+  -> Simplex2 -- ^ line segment+  -> P2+o `closestAlong` (Simplex2 aa bb) = aoAlong `vplusP2` a+  where a = _neighborhoodCenter aa+        b = _neighborhoodCenter bb+        ao = diffP2 o a+        ab = diffP2 b a+        abNorm = normalizeV2 ab+        aoAlong = D# (ao `dotV2` abNorm) `smulV2` abNorm
+ src/Physics/Contact/ConvexHull.hs view
@@ -0,0 +1,238 @@+{-# LANGUAGE TemplateHaskell #-}+{-# LANGUAGE MultiParamTypeClasses #-}+{-# LANGUAGE FlexibleInstances #-}+{-# LANGUAGE FlexibleContexts #-}+{-# LANGUAGE TypeFamilies #-}+{-# LANGUAGE ScopedTypeVariables #-}+{-# LANGUAGE BangPatterns #-}+{-# LANGUAGE RecordWildCards #-}+{-# LANGUAGE MagicHash #-}+{-# LANGUAGE DeriveGeneric #-}+{-# LANGUAGE DeriveAnyClass #-}++{- |+Convex polygons and their vertices and edges.+Functions and types for treating convex polygons as support functions+(axis -> extent along an axis).+-}+module Physics.Contact.ConvexHull where++import GHC.Generics (Generic)+import GHC.Prim (Double#, (/##), negateDouble#)++import Control.DeepSeq+import Control.Lens (makeLenses)+import Data.Array+import Physics.Linear+import Utils.Utils++data Neighborhood = Neighborhood { _neighborhoodCenter :: !P2+                                 , _neighborhoodNext :: Neighborhood+                                 , _neighborhoodPrev :: Neighborhood+                                 , _neighborhoodUnitNormal :: !V2+                                 , _neighborhoodIndex :: !Int+                                 } deriving (Generic)+makeLenses ''Neighborhood++instance NFData Neighborhood where+  rnf (Neighborhood a _ _ b c) = rnf (a, b, c)+  {-# INLINE rnf #-}++data Extent f =+  Extent { _extentMin :: !f+         , _extentMax :: !f+         , _extentProjection :: !(SP Double Double)+         } deriving (Show, Eq, Generic, NFData)+makeLenses ''Extent++instance Functor Extent where+  fmap f (Extent x y p) = Extent (f x) (f y) p+  {-# INLINE fmap #-}++instance Show Neighborhood where+  show Neighborhood{..} =+    "Neighborhood (" +++    show _neighborhoodCenter ++ ") (" +++    show _neighborhoodUnitNormal ++ ") (" +++    show _neighborhoodIndex ++ ")"++type Vertices = [P2]++data ConvexHull =+  ConvexHull { _hullVertexCount :: !Int+             , _hullVertices :: !(Array Int P2)+             , _hullEdgeNormals :: !(Array Int V2)+             , _hullNeighborhoods :: Array Int Neighborhood+             , _hullExtents :: !(Array Int (Int, Int))+             , _hullLocalVertices :: !(Array Int P2)+             } deriving (Show, Generic, NFData)+makeLenses ''ConvexHull++_hullNeighborhood :: Int -> ConvexHull -> Neighborhood+_hullNeighborhood i hull =+  _hullNeighborhoods hull ! i+{-# INLINE _hullNeighborhood #-}++distanceAlong :: Neighborhood -> V2 -> Double+Neighborhood{..} `distanceAlong` dir =+  dir `afdot'` _neighborhoodCenter+{-# INLINE distanceAlong #-}++extentAlong' :: ConvexHull -> V2 -> SP Neighborhood Neighborhood+extentAlong' ConvexHull{..} dir = toSP . pairMap snd . foldl1 g $ fmap f _hullNeighborhoods+  where f neigh =+          ((dist, neigh), (dist, neigh))+          where dist = neigh `distanceAlong` dir+        {-# INLINE f #-}+        g (minA@(minDistA, _), maxA@(maxDistA, _)) (minB@(minDistB, _), maxB@(maxDistB, _)) =+          (minAB, maxAB)+          where minAB = if minDistB < minDistA then minB else minA+                maxAB = if maxDistB > maxDistA then maxB else maxA+        {-# INLINE g #-}+{-# INLINE extentAlong' #-}++extentAlong :: ConvexHull -> V2 -> Extent Neighborhood+extentAlong shape dir =+  Extent minv maxv projectedExtent+  where projectedExtent = spMap (f . _neighborhoodCenter) ext+          where f v = dir `afdot'` v+        ext@(SP minv maxv) = extentAlong' shape dir+{-# INLINE extentAlong #-}++extentIndices :: Extent Neighborhood -> (Int, Int)+extentIndices ext =+  (_neighborhoodIndex . _extentMin $ ext, _neighborhoodIndex . _extentMax $ ext)+{-# INLINE extentIndices #-}++extentAlongSelf' :: ConvexHull -> Int -> (Int, Int)+extentAlongSelf' ConvexHull{..} = (_hullExtents !)+{-# INLINE extentAlongSelf' #-}++extentAlongSelf :: ConvexHull -> (Int, V2) -> Extent Neighborhood+extentAlongSelf hull@ConvexHull{..} (index', dir) =+  Extent { _extentMin = minN+         , _extentMax = maxN+         , _extentProjection = SP (minN `distanceAlong` dir) (maxN `distanceAlong` dir)+         }+  where (minN, maxN) = pairMap (_hullNeighborhoods !) $ extentAlongSelf' hull index'+{-# INLINE extentAlongSelf #-}++neighborhoods :: ConvexHull -> [Neighborhood]+neighborhoods = elems . _hullNeighborhoods+{-# INLINE neighborhoods #-}++support :: ConvexHull -> V2 -> Neighborhood+support ConvexHull{..} dir = snd . foldl1 g $ fmap f _hullNeighborhoods+  where f neigh@Neighborhood{..} = (dir `afdot'` _neighborhoodCenter, neigh)+        g a@(distA, _) b@(distB, _) = if distB > distA then b else a+{-# INLINE support #-}++-- TODO: make ConvexHull a proper WorldTransformable+--instance (Epsilon a, Floating a, Ord a) => WorldTransformable (ConvexHull a) a where+  --transform t = flip transformHull (transform t)+  --untransform t = flip transformHull (untransform t)++rectangleVertices :: Double# -> Double# -> Vertices+rectangleVertices w h =+  [ P2 $ V2 w2 h2+  , P2 $ V2 nw2 h2+  , P2 $ V2 nw2 nh2+  , P2 $ V2 w2 nh2 ]+  where w2 = w /## 2.0##+        h2 = h /## 2.0##+        nw2 = negateDouble# w2+        nh2 = negateDouble# h2+{-# INLINE rectangleVertices #-}++rectangleHull :: Double# -> Double# -> ConvexHull+rectangleHull w h = listToHull $ rectangleVertices w h+{-# INLINE rectangleHull #-}++listToHull :: [P2] -> ConvexHull+listToHull vertices =+  hull+  where vertexCount = length vertices+        vertexBound = vertexCount - 1+        vertexBounds = (0, vertexBound)+        vertices' = listArray vertexBounds vertices+        edgeNormals = ixedMap (unitEdgeNormal vertexBound) vertices'+        extents = fmap (extentIndices . extentAlong hull) edgeNormals+        hull :: ConvexHull+        hull = ConvexHull vertexCount+               vertices'+               edgeNormals+               (makeNeighborhoods hull)+               extents+               vertices'+{-# INLINE listToHull #-}++-- assumes scale-invariant transform in worldspace+transformHull ::  ConvexHull+              -> (P2 -> P2)+              -> ConvexHull+transformHull hull@ConvexHull{..} fInWorldSpace =+  hull'+  where hull' = hull { _hullVertices = vertices+                     , _hullEdgeNormals = edgeNormals+                     , _hullNeighborhoods = makeNeighborhoods hull+                     }+        vertices = fmap fInWorldSpace _hullVertices+        edgeNormals = ixedMap (unitEdgeNormal $ _hullVertexCount - 1) vertices+{-# INLINE transformHull #-}++-- assumes scale-invariant transform from localspace+setHullTransform :: ConvexHull+                 -> (P2 -> P2)+                 -> ConvexHull+setHullTransform hull@ConvexHull{..} fromLocalSpace =+  hull'+  where hull' = hull { _hullVertices = vertices+                     , _hullEdgeNormals = edgeNormals+                     , _hullNeighborhoods = makeNeighborhoods hull'+                     }+        vertices = fmap fromLocalSpace _hullLocalVertices+        edgeNormals = ixedMap (unitEdgeNormal $ _hullVertexCount - 1) vertices+{-# INLINE setHullTransform #-}++makeNeighborhoods :: ConvexHull -> Array Int Neighborhood+makeNeighborhoods hull@ConvexHull{..} =+  listArray (bounds _hullVertices) $+  fmap (makeNeighborhood hull) (indices _hullVertices)+{-# INLINE makeNeighborhoods #-}++makeNeighborhood :: ConvexHull -> Int -> Neighborhood+makeNeighborhood ConvexHull{..} i =+  Neighborhood { _neighborhoodCenter = _hullVertices ! i+               , _neighborhoodNext = _hullNeighborhoods ! nextIndex maxIndex i+               , _neighborhoodPrev = _hullNeighborhoods ! prevIndex maxIndex i+               , _neighborhoodUnitNormal = _hullEdgeNormals ! i+               , _neighborhoodIndex = i+               }+  where maxIndex = arrMaxBound _hullVertices+{-# INLINE makeNeighborhood #-}++ixedMap :: (Ix i) => (Array i e -> i -> x) -> Array i e -> Array i x+ixedMap f arr = listArray (bounds arr) $ fmap (f arr) (indices arr)+{-# INLINE ixedMap #-}++edgeNormal :: Int -> Array Int P2 -> Int -> V2+edgeNormal maxIndex vs i = clockwiseV2 (v' `diffP2` v)+  where v = vs ! i+        v' = vs ! nextIndex maxIndex i+{-# INLINE edgeNormal #-}++unitEdgeNormal :: Int -> Array Int P2 -> Int -> V2+unitEdgeNormal maxIndex vs = normalizeV2 . edgeNormal maxIndex vs+{-# INLINE unitEdgeNormal #-}++arrMaxBound :: Array Int a -> Int+arrMaxBound = snd . bounds+{-# INLINE arrMaxBound #-}++nextIndex :: Int -> Int -> Int+nextIndex max_i i = if i < max_i then i + 1 else 0+{-# INLINE nextIndex #-}++prevIndex :: Int -> Int -> Int+prevIndex max_i i = if i > 0 then i - 1 else max_i+{-# INLINE prevIndex #-}
+ src/Physics/Contact/GJK.hs view
@@ -0,0 +1,146 @@+{-# LANGUAGE MagicHash #-}++{- |+Gilbert-Johnson-Keerthi (GJK) for finding the closest part of a Minkowski space+to the origin.++Choose a simplex using opposite extents along some axis.+Extend the simplex in the direction of the origin.+If the simplex encloses the origin, stop.++based on slides/video by Casey Muratori:+* https://www.youtube.com/watch?v=Qupqu1xe7Io++The loop of GJK is:++- extend the simplex along the search direction+- shift to the closest component of the simplex+- do this until we can't extend the simplex any more+  (search stopped short of the origin)++-}++module Physics.Contact.GJK where++import           GHC.Prim+import           GHC.Types                  (Double (D#), isTrue#)++import           Physics.Contact.ConvexHull+import           Physics.Linear+import           Utils.Utils++-- | 2-simplex. The first element is the most recently added. (like the head of a list)+data Simplex3 =+  Simplex3 !Neighborhood+           !Neighborhood+           !Neighborhood+  deriving (Show)+data Simplex2 =+  Simplex2 !Neighborhood+           !Neighborhood+  deriving (Show)+data Simplex1 =+  Simplex1 !Neighborhood+  deriving (Show)+type Simplex12 = Either Simplex1 Simplex2+type Simplex23 = Either Simplex2 Simplex3+data Simplex+  = Simplex' Simplex12+  | Simplex3' Simplex3+  deriving (Show)++closestSimplex :: ConvexHull -> P2 -> Simplex+closestSimplex hull origin = loop (Left $ Simplex1 a) d+  where+    a = _hullNeighborhood 0 hull+    d = diffP2 origin $ _neighborhoodCenter a+    loop :: Simplex12 -> V2 -> Simplex+    loop simplex d =+      case extendSimplex simplex aa of+        Nothing -> Simplex' simplex -- search failed+        Just simplex ->+          case shiftSimplex simplex origin of+            Right simplex     -> Simplex3' simplex -- enclosed the origin+            Left (simplex, d) -> loop simplex d+      where+        aa = support hull d+        a = _neighborhoodCenter aa+        ao = diffP2 a origin++extendSimplex :: Simplex12 -> Neighborhood -> Maybe Simplex23+extendSimplex (Left simplex) aa =+  Left <$> extendSimplex1 simplex aa+extendSimplex (Right simplex) aa = Right <$> extendSimplex2 simplex aa++extendSimplex1 :: Simplex1 -> Neighborhood -> Maybe Simplex2+extendSimplex1 simplex@(Simplex1 bb) aa+  | _neighborhoodIndex bb == _neighborhoodIndex aa = Nothing -- it's a repeat+  | otherwise = Just $ mkSimplex2 aa simplex++extendSimplex2 :: Simplex2 -> Neighborhood -> Maybe Simplex3+extendSimplex2 simplex@(Simplex2 bb cc) aa+  | bi == ai || ci == ai = Nothing -- it's a repeat+  | otherwise = Just $ mkSimplex3 aa simplex+  where ai = _neighborhoodIndex aa+        bi = _neighborhoodIndex bb+        ci = _neighborhoodIndex cc++shiftSimplex :: Simplex23 -> P2 -> Either (Simplex12, V2) Simplex3+shiftSimplex (Left simplex) origin = Left $ shiftSimplex2 simplex origin+shiftSimplex (Right simplex) origin =+  case shiftSimplex3 simplex origin of+    Nothing     -> Right simplex+    Just result -> Left result++shiftSimplex2 :: Simplex2+  -- ^ 1D simplex of 2 points+  -> P2+  -- ^ origin (the target point)+  -> (Simplex12, V2)+  -- ^ new simplex, new search direction+shiftSimplex2 aabb@(Simplex2 aa bb) origin+  | sameDirection ab ao = (Right aabb, crossV2V2 ab ao ab)+  -- search perpendicular to AB toward the origin.+  | otherwise = (Left $ Simplex1 aa, ao) -- throw out B, search from A toward the origin.+  where+    a = _neighborhoodCenter aa+    b = _neighborhoodCenter bb+    ab = diffP2 b a+    ao = diffP2 origin a++shiftSimplex3 :: Simplex3+  -- ^ 2D simplex of 3 points+  -> P2+  -- ^ origin (the target point)+  -> Maybe (Simplex12, V2)+  -- ^ (new simplex, new search direction) OR simplex encloses the origin!+shiftSimplex3 (Simplex3 aa bb cc) origin+  | sameDirection abcac ao =+    if sameDirection ac ao+      then Just (Right $ Simplex2 aa cc, crossV2V2 ac ao ac)+      else Just star+  | sameDirection ababc ao = Just star+  | otherwise = Nothing -- simplex encloses the origin+  where+    a = _neighborhoodCenter aa+    b = _neighborhoodCenter bb+    c = _neighborhoodCenter cc+    ab = diffP2 b a+    ac = diffP2 c a+    ao = diffP2 origin a+    abc = ab `crossV2` ac+    abcac = abc `zcrossV2` ac+    ababc = ab `crosszV2` abc+    star =+      if sameDirection ab ao+        then (Right $ Simplex2 aa bb, crossV2V2 ab ao ab)+        else (Left $ Simplex1 aa, ao)++sameDirection :: V2 -> V2 -> Bool+sameDirection a b = isTrue# (a `dotV2` b >## 0.0##)++mkSimplex3 :: Neighborhood -> Simplex2 -> Simplex3+mkSimplex3 aa (Simplex2 bb cc) = Simplex3 aa bb cc++mkSimplex2 :: Neighborhood -> Simplex1 -> Simplex2+mkSimplex2 aa (Simplex1 bb) = Simplex2 aa bb
+ src/Physics/Contact/HullVsHull.hs view
@@ -0,0 +1,88 @@+{-# LANGUAGE MultiParamTypeClasses #-}+{-# LANGUAGE RecordWildCards       #-}+{-# LANGUAGE ScopedTypeVariables   #-}+{-# LANGUAGE TemplateHaskell       #-}+{-# LANGUAGE TypeFamilies          #-}++{- |+Finding and describing the contact between two colliding objects.+Also, a type for configuring contact constraint solver behavior.+-}+module Physics.Contact.HullVsHull where++import           Control.Lens+import           Data.Vector.Unboxed.Deriving++import           Physics.Contact.ConvexHull+import           Physics.Contact.SAT+import           Physics.Contact.Types+import           Physics.Linear+import           Utils.Descending+import           Utils.Utils++contactDepth :: Neighborhood -- ^ Penetrated edge+             -> Neighborhood -- ^ Penetrating feature+             -> Double -- ^ Penetration depth+contactDepth edge = contactDepth_ edge . _neighborhoodCenter+{-# INLINE contactDepth #-}++contactDepth_ :: Neighborhood -- ^ Penetrated edge+              -> P2 -- ^ Penetrating feature+              -> Double -- ^ Penetration depth+contactDepth_ neighborhood p = f v - f p+  where f = afdot' n+        n = _neighborhoodUnitNormal neighborhood+        v = _neighborhoodCenter neighborhood+{-# INLINE contactDepth_ #-}++defaultContactBehavior :: ContactBehavior+defaultContactBehavior =+  ContactBehavior { contactBaumgarte = 0+                  , contactPenetrationSlop = 0+                  }+{-# INLINE defaultContactBehavior #-}++-- | Extract the 'Contact' if it exists.+unwrapContactResult :: Maybe (Flipping (Either Neighborhood Contact))+                    -- ^ May contain either a separating axis or a 'Contact'+                    -> Maybe (Flipping Contact)+unwrapContactResult contactInfo = (flipInjectF . fmap eitherToMaybe) =<< contactInfo+{-# INLINE unwrapContactResult #-}++-- TODO: better names for Contact vs Contact'+-- | Flatten a 'Contact' into 'Contact''s.+flattenContactResult :: Maybe (Flipping Contact)+                     -> Descending ((Int, Int), Flipping Contact')+                     -- ^ in decreasing key order, where x is MSV and y is LSV in (x, y)+flattenContactResult Nothing = Descending []+flattenContactResult (Just fContact) =+  fmap f . flipInjectF . fmap flatten $ fContact+  where flatten :: Contact -> Descending ((Int, Int), Contact')+        flatten Contact{..} = g <$> flattenContactPoints _contactPenetrator+          where g :: Neighborhood -> ((Int, Int), Contact')+                g pen =+                  ( (_neighborhoodIndex _contactEdge, _neighborhoodIndex pen)+                  , Contact' { _contactEdgeNormal' = _neighborhoodUnitNormal _contactEdge+                             , _contactPenetrator' = _neighborhoodCenter pen+                             , _contactDepth' = contactDepth _contactEdge pen+                             }+                  )+        {-# INLINE flatten #-}+        f :: Flipping ((Int, Int), Contact') -> ((Int, Int), Flipping Contact')+        f x = (flipExtractPair fst x, snd <$> x)+        {-# INLINE f #-}+{-# INLINE flattenContactResult #-}++-- Find the 'Contact' between a pair of shapes if they overlap.+generateContacts' :: (ConvexHull, ConvexHull)+                  -> Maybe (Flipping Contact)+generateContacts' shapes = unwrapContactResult $ uncurry contact shapes+{-# INLINE generateContacts' #-}++-- Find the 'Contact''s between a pair of shapes if they overlap.+generateContacts :: (ConvexHull, ConvexHull)+                 -> Descending ((Int, Int), Flipping Contact')+                 -- ^ in decreasing key order, where x is MSV and y is LSV in (x, y)+                 --   x is the first hull's feature, y is the second hull's feature+generateContacts = flattenContactResult . generateContacts'+{-# INLINE generateContacts #-}
+ src/Physics/Contact/SAT.hs view
@@ -0,0 +1,267 @@+{-# LANGUAGE FlexibleContexts      #-}+{-# LANGUAGE FlexibleInstances     #-}+{-# LANGUAGE MagicHash             #-}+{-# LANGUAGE MultiParamTypeClasses #-}+{-# LANGUAGE RecordWildCards       #-}+{-# LANGUAGE ScopedTypeVariables   #-}+{-# LANGUAGE TemplateHaskell       #-}+{-# LANGUAGE TypeFamilies          #-}++{- |+Separating Axis Test (SAT).+A separating axis is a direction along which the projections of two shapes do not overlap.+Alternately, a separating axis is a line between two shapes that do not intersect.++If no separating axis is found, use the axis of smallest overlap to determine+which features of the objects are involved in the collision (e.g. calculate contact points and normals).+-}+module Physics.Contact.SAT where++import           GHC.Types                  (Double (D#))++import           Control.Lens               (makeLenses, makePrisms, view, (^.),+                                             _1)+import           Data.Either.Combinators+import           Data.Function              (on)+import           Physics.Contact.ConvexHull+import           Physics.Linear+import           Utils.Descending+import           Utils.Utils++-- | An overlap between two shapes.+data Overlap = Overlap { _overlapEdge       :: !Neighborhood+                       -- ^ the first vertex of the penetrated edge+                       , _overlapDepth      :: !Double+                       , _overlapPenetrator :: !Neighborhood+                       -- ^ the vertex that penetrates the edge+                       } deriving Show+makeLenses ''Overlap++-- | Either the separating axis or the smallest overlap between two shapes.+data SATResult+  = Separated Neighborhood+  -- ^ the edge that forms a separating axis between the two shapes.+  | MinOverlap Overlap+  -- ^ the smallest overlap+  deriving (Show)+makePrisms ''SATResult++{- |+A contact manifold can contain either a single point or a pair of points.+For example, a pair of touching edges can be described by a pair of points.+A vertex touching an edge can be described by a single point.+-}+type ContactPoints = Either Neighborhood (SP Neighborhood Neighborhood)++-- | A contact manifold+data Contact =+  Contact { _contactEdge            :: !Neighborhood+          -- ^ the first vertex of the edge being penetrated+          , _contactPenetrator      :: !ContactPoints+          -- ^ the points of the contact manifold (after clipping the penetrating edge to the penetrated edge)+          , _contactPenetratingEdge :: !(SP Neighborhood Neighborhood)+          -- ^ the edge that penetrates '_contactEdge'+          } deriving Show+makeLenses ''Contact++-- | One side of an isomorphism.+satToEither :: SATResult -> Either Neighborhood Overlap+satToEither (Separated x)  = Left x+satToEither (MinOverlap x) = Right x+{-# INLINE satToEither #-}++-- | assumes pairs are (min, max)+overlapTest ::+     (Ord a)+  => SP a a+  -- ^ an interval+  -> SP a a+  -- ^ another interval+  -> Bool+  -- ^ Do the intervals overlap?+overlapTest (SP a b) (SP c d) = not (c > b || d < a)+{-# INLINE overlapTest #-}++-- | assumes pairs are (min, max)+overlapAmount ::+     (Ord a, Num a)+  => SP a a+  -- ^ an interval (e.g. the projection of a shape onto an axis)+  -> SP a a+  -- ^ another interval+  -> Maybe a+  -- ^ If the intervals overlap, by how much?+overlapAmount x@(SP _ edge) y@(SP penetrator _) = toMaybe (overlapTest x y) (edge - penetrator)+{-# INLINE overlapAmount #-}++-- | get the normal from the overlap+overlapNormal :: Overlap -> V2+overlapNormal = _neighborhoodUnitNormal . _overlapEdge+{-# INLINE overlapNormal #-}++-- | Check for overlap along a single axis (edge normal).+overlap :: ConvexHull+  -- ^ The receiving shape "sEdge".+  -> Neighborhood+  -- ^ An edge normal from the receiving shape.+  -> ConvexHull+  -- ^ The penetrating shape "sPen".+  -> Maybe Overlap+  -- ^ Any overlap from "sPen" into "sEdge".+overlap sEdge edge sPen =+  fmap (\oval' -> Overlap edge oval' penetrator ) oval+  where dir = _neighborhoodUnitNormal edge+        extentS = extentAlongSelf sEdge (edge ^. neighborhoodIndex, dir)+        extentP = extentAlong sPen dir+        penetrator = extentP ^. extentMin+        oval = overlapAmount (extentS ^. extentProjection) (extentP ^. extentProjection)+{-# INLINE overlap #-}++-- | Find the axis (edge normal) with the smallest overlap between the two shapes.+minOverlap :: ConvexHull+           -- ^ The receiving shape "sEdge".+           -> [Neighborhood]+           -- ^ Edge normals from the receiving shape.+           -> ConvexHull+           -- ^ The penetrating shape "sPen".+           -> SATResult+           -- ^ Axis of smallest overlap or separating axis.+minOverlap sEdge edges sPen =+  foldl1 f os -- lazy fold for early exit?+  where os = fmap (\edge -> maybe (Separated edge) MinOverlap $ overlap sEdge edge sPen) edges+        f :: SATResult -> SATResult -> SATResult+        f sep@(Separated _) _ = sep+        f _ sep@(Separated _) = sep+        f mino@(MinOverlap mino') o@(MinOverlap o') =+          if _overlapDepth o' < _overlapDepth mino' then o else mino+        {-# INLINE f #-}+{-# INLINE minOverlap #-}++-- | Wrapper for 'minOverlap'.+minOverlap' :: ConvexHull -> ConvexHull -> SATResult+minOverlap' a = minOverlap a (neighborhoods a)+{-# INLINE minOverlap' #-}++{- |+Choose the best edge to act as a penetrator.+The overlap test yields a penetrating vertex, but this vertex belongs to two edges.++Choose the edge that is closest to perpendicular to the overlap normal vector.+i.e. the edge that is closest to parallel with the penetrated edge+-}+penetratingEdge :: Overlap+  -> SP Neighborhood Neighborhood+  -- ^ the two vertices that define the edge (in order)+penetratingEdge (Overlap edge _ b) =+  if bcn < abn then SP b c+  else SP a b+  where c = _neighborhoodNext b+        a = _neighborhoodPrev b+        cc = _neighborhoodCenter c+        bb = _neighborhoodCenter b+        aa = _neighborhoodCenter a+        abn = abs (D# ((bb `diffP2` aa) `dotV2` n))+        bcn = abs (D# ((cc `diffP2` bb) `dotV2` n))+        n = _neighborhoodUnitNormal edge+{-# INLINE penetratingEdge #-}++-- | Extract the endpoints of the penetrated edge.+penetratedEdge :: Overlap -> SP Neighborhood Neighborhood+penetratedEdge (Overlap edgeStart _ _) = SP edgeStart (_neighborhoodNext edgeStart)+{-# INLINE penetratedEdge #-}++-- | Extract just the point data from 'ContactPoints'.+contactPoints' :: ContactPoints -> Either P2 (SP P2 P2)+contactPoints' = mapBoth f g+  where f = _neighborhoodCenter+        g = spMap f+{-# INLINE contactPoints' #-}++-- | Sort 'ContactPoints' by decreasing feature index.+flattenContactPoints :: ContactPoints -> Descending Neighborhood+flattenContactPoints (Left p) = Descending [p]+flattenContactPoints (Right (SP p1 p2)) =+  if _neighborhoodIndex p1 > _neighborhoodIndex p2+  then Descending [p1, p2]+  else Descending [p2, p1]+{-# INLINE flattenContactPoints #-}++-- | Clip a pair of edges into a contact manifold.+clipEdge ::+     SP Neighborhood Neighborhood+  -- ^ the penetrated edge+  -> V2+  -- ^ the normal vector for the overlap+  -> SP Neighborhood Neighborhood+  -- ^ the penetrating edge ("incident" edge)+  -> Maybe ContactPoints+clipEdge (SP aa bb) n inc_ = do+  -- "a" and "b" are the vertices of the penetrated edge.+  -- We're clipping the incident edge to the bounds of the penetrated edge.+  -- clip the incident edge using the bounding plane at point "a"+  inc' <- lApplyClip' l (clipSegment aBound (SP cd' inc)) inc_+  -- clip the incident edge using the bounding plane at point "b"+  inc'' <- lApplyClip' l (clipSegment bBound (SP cd' (f inc'))) inc'+  applyClip'' (clipSegment abBound (SP cd' (f inc''))) inc''+  where aBound = perpLine2 a b+        -- ^ bounding plane against going past point "a" along the edge+        bBound = perpLine2 b a+        -- ^ bounding plane against going past point "b" along the edge+        abBound = Line2 a (negateV2 n)+        -- ^ bounding plane facing into the penetrated object (against going outside the object)+        cd' = toLine2 c d+        inc@(SP c d) = f inc_+        -- ^ the incident edge+        (SP a b) = f (SP aa bb)+        f = spMap (view neighborhoodCenter)+        l = neighborhoodCenter+{-# INLINE clipEdge #-}++-- | Pull out  the inner 'Maybe'.+convertContactResult :: Flipping (Either Neighborhood (Maybe Contact))+                     -> Maybe (Flipping (Either Neighborhood Contact))+convertContactResult = flipInjectF . fmap liftRightMaybe+{-# INLINE convertContactResult #-}++{- |+'Flipping' indicates the direction of the collision.+'Same' means `a` is penetrated by `b`.+'Flipped' means `b` is penetrated by `a`.++How it works:++1. Find the smallest overlap along the axes of each shape's edges.+2. Clip this overlap to a contact manifold.++The result should probably never be 'Nothing', but I don't know if that's guaranteed.+-}+contactDebug :: ConvexHull+             -> ConvexHull+             -> (Maybe (Flipping (Either Neighborhood Contact)), SATResult, SATResult)+             -- ^ 'Either' of separating axis (the normal at the 'Neighborhood') or a contact manifold+contactDebug a b = (convertContactResult $ fmap (mapRight contact_) ovl, ovlab, ovlba)+  where ovlab = minOverlap' a b+        ovlba = minOverlap' b a+        ovlab' = satToEither ovlab+        ovlba' = satToEither ovlba+        ovl :: Flipping (Either Neighborhood Overlap)+        ovl = eitherBranchBoth ((<) `on` _overlapDepth) ovlab' ovlba'+{-# INLINE contactDebug #-}++contact :: ConvexHull+        -- ^ shape "a"+        -> ConvexHull+        -- ^ shape "b"+        -> Maybe (Flipping (Either Neighborhood Contact))+        -- ^ 'Either' of separating axis (the normal at the 'Neighborhood') or a contact manifold+contact a b = contactDebug a b ^. _1+{-# INLINE contact #-}++-- | Use clipping to calculate the contact manifold for a given overlap.+contact_ :: Overlap -> Maybe Contact+contact_ ovl@Overlap{..} = fmap f (clipEdge edge n pen)+  where edge = penetratedEdge ovl+        pen = penetratingEdge ovl+        n = overlapNormal ovl+        f c = Contact _overlapEdge c pen+{-# INLINE contact_ #-}
+ src/Physics/Contact/Types.hs view
@@ -0,0 +1,36 @@+{-# LANGUAGE TemplateHaskell #-}+{-# LANGUAGE TypeFamilies #-}+{-# LANGUAGE MultiParamTypeClasses #-}+{-# LANGUAGE RecordWildCards #-}+{-# LANGUAGE ScopedTypeVariables #-}++module Physics.Contact.Types where++import Control.Lens+import Data.Vector.Unboxed.Deriving++import Physics.Linear++-- | Configuring contact constraint behavior+-- | TODO: does this belong in a different module?+data ContactBehavior =+  ContactBehavior { contactBaumgarte :: !Double+                  -- ^ Bias factor: 0 <= B <= 1, used to feed positional error back into a constraint+                  , contactPenetrationSlop :: !Double+                  -- ^ Amount objects must overlap before they are considered \"touching\"+                  } deriving Show++-- | A contact between two objects - the source of a single set of contact constraints+data Contact' =+  Contact' { _contactEdgeNormal' :: !V2+           -- ^ Unit normal of penetrated edge (or direction toward center of circle)+           , _contactPenetrator' :: !P2+           -- ^ Coordinates of penetrating feature (or the best estimate of point-of-contact)+           , _contactDepth' :: !Double -- ^ Depth of penetration+           } deriving Show++makeLenses ''Contact'+derivingUnbox "Contact'"+  [t| Contact' -> (V2, P2, Double) |]+  [| \Contact'{..} -> (_contactEdgeNormal', _contactPenetrator', _contactDepth') |]+  [| \(n, p, d) -> Contact' n p d |]
+ src/Physics/Engine.hs view
@@ -0,0 +1,50 @@+{-# LANGUAGE EmptyDataDecls #-}+{-# LANGUAGE MagicHash      #-}+{-# LANGUAGE TypeFamilies   #-}++{- |+Another piece of the sample implementation of a physics engine that uses this library.+-}+module Physics.Engine where++import           Data.Proxy+import           GHC.Types                  (Double (D#))++import           Physics.Constraint         (PhysicalObj (..), toInvMass2)+import           Physics.Contact            (Shape (..))+import           Physics.Contact.Circle     (circleWithRadius)+import           Physics.Contact.ConvexHull (ConvexHull, listToHull,+                                             rectangleHull)+import           Physics.Contact.Types      (ContactBehavior (..))+import           Physics.Engine.Class+import           Physics.Linear             (P2 (..), V2 (..))+import           Physics.World              (World, fromList)+import           Physics.World.External     (constantAccel)+import           Physics.World.Object       (WorldObj)+import qualified Physics.World.Object       as PO++data Engine a++engineP :: Proxy (Engine a)+engineP = Proxy++pairToV2 :: (Double, Double) -> V2+pairToV2 (D# x, D# y) = V2 x y++instance PhysicsEngine (Engine a) where+  type PEWorld (Engine a) = World+  type PEWorldObj (Engine a) = WorldObj+  type PEPhysicalObj (Engine a) = PhysicalObj+  type PEExternalObj (Engine a) = a+  type PEContactBehavior (Engine a) = ContactBehavior+  type PENumber (Engine a) = Double+  type PEShape (Engine a) = Shape+  makePhysicalObj _ vel rotvel pos rotpos =+    PhysicalObj (pairToV2 vel) rotvel (pairToV2 pos) rotpos . toInvMass2+  makeWorldObj _ = PO.makeWorldObj+  makeWorld _ = fromList+  makeContactBehavior _ = ContactBehavior+  makeConstantAccel _ = constantAccel . pairToV2+  makeHull _ = HullShape . listToHull . fmap (P2 . pairToV2)+  makeRectangleHull _ (D# w) (D# h) = HullShape $ rectangleHull w h+  makeCircle _ = CircleShape . circleWithRadius
+ src/Physics/Engine/Class.hs view
@@ -0,0 +1,57 @@+{-# LANGUAGE TypeFamilies #-}+{-# LANGUAGE FlexibleContexts #-}++{- |+This module is a holdover from when I had two (slow and less slow) implementations of the physics engine.+I used this class so I could run the same demos on both engines to compare them.+There's a good chance I remove this in the future.+-}+module Physics.Engine.Class where++import Data.Proxy+import Physics.World.Class++class (Fractional (PENumber e)) => PhysicsEngine e where+  type PEWorld e :: * -> *+  type PEWorldObj e :: * -> *+  type PEExternalObj e+  type PEPhysicalObj e+  type PEContactBehavior e+  type PENumber e+  type PEShape e++  -- | Create a @PEPhysicalObj e@.+  makePhysicalObj :: Proxy e+                  -> (PENumber e, PENumber e)+                  -- ^ Velocity+                  -> PENumber e+                  -- ^ Rotational velocity+                  -> (PENumber e, PENumber e)+                  -- ^ Position+                  -> PENumber e+                  -- ^ Rotation+                  -> (PENumber e, PENumber e)+                  -- ^ Linear mass paired with rotational mass+                  -> PEPhysicalObj e++  -- | Create a @PEWorldObj e@+  makeWorldObj :: Proxy e+               -> PEPhysicalObj e+               -- ^ The physical body of this object.+               -> PENumber e+               -- ^ Coefficient of friction μ (mu).+               -> PEShape e+               -- ^ The shape of the object.+               -> PEExternalObj e+               -- ^ Any userland piece of data from outside the simulation.+               -> PEWorldObj e (PEExternalObj e)++  makeWorld :: Proxy e -> [PEWorldObj e (PEExternalObj e)] -> PEWorld' e+  makeContactBehavior :: Proxy e -> PENumber e -> PENumber e -> PEContactBehavior e+  makeConstantAccel :: Proxy e -> (PENumber e, PENumber e) -> External+  makeHull :: Proxy e -> [(PENumber e, PENumber e)] -> PEShape e+  makeRectangleHull :: Proxy e -> PENumber e -> PENumber e -> PEShape e+  makeCircle :: Proxy e -> PENumber e -> PEShape e++type PEWorldObj' e = PEWorldObj e (PEExternalObj e)+type PEWorld' e = PEWorld e (PEWorldObj' e)
+ src/Physics/Engine/Main.hs view
@@ -0,0 +1,106 @@+{-# LANGUAGE RankNTypes          #-}+{-# LANGUAGE RecordWildCards     #-}+{-# LANGUAGE ScopedTypeVariables #-}++{- |+This is an example of how to use this library to create and simulate a world.+It's more documentation than an API I actually expect people to use.+-}+module Physics.Engine.Main ( module Physics.Engine.Main+                           , module Physics.Engine+                           ) where++import           Control.Lens+import           Control.Monad.Reader+import           Control.Monad.ST+import           Control.Monad.State.Strict+import qualified Data.Vector.Generic.Mutable as MV+import qualified Data.Vector.Unboxed         as V++import           Physics.Broadphase.Aabb+import qualified Physics.Broadphase.Grid     as G+import           Physics.Constraint+import           Physics.Constraints.Contact+import           Physics.Constraints.Types+import           Physics.Contact.Types       (ContactBehavior)+import           Physics.Solvers.Contact+import           Physics.World+import           Physics.World.Class+import           Physics.World.Object++import           Physics.Engine+import           Physics.Scenes.Scene++type World' a = World (WorldObj a)++type EngineCache s = V.MVector s (ObjectFeatureKey Int, ContactResult Lagrangian)+type EngineState a s = (World' a, EngineCache s, [External])+data EngineConfig =+  EngineConfig { _engineTimestep   :: Double+               , _engineContactBeh :: ContactBehavior+               } deriving Show+type EngineST a s = ReaderT EngineConfig (StateT (EngineState a s) (ST s))++gridAxes :: (G.GridAxis, G.GridAxis)+gridAxes = (G.GridAxis 20 1 (-10), G.GridAxis 20 1 (-10))++initEngine :: Scene (Engine a) -> ST s (EngineState a s)+initEngine Scene{..} = do+  cache <- MV.new 0+  return (_scWorld, cache, _scExts)++changeScene :: Scene (Engine a) -> EngineST a s ()+changeScene scene = do+  eState <- lift . lift $ initEngine scene+  put eState++-- TODO: can I do this with _1?+wrapUpdater :: V.Vector (ContactResult Constraint)+            -> (EngineCache s -> V.Vector (ContactResult Constraint) -> World' a -> ST s (World' a))+            -> EngineST a s ()+wrapUpdater constraints f = do+  (world, cache, externals) <- get+  world' <- lift . lift $ f cache constraints world+  put (world', cache, externals)++wrapUpdater' :: (World' a -> ST s (World' a)) -> EngineST a s (World' a)+wrapUpdater' f = do+  (world, cache, externals) <- get+  world' <- lift . lift $ f world+  put (world', cache, externals)+  return world'++wrapInitializer :: (EngineCache s -> (World' a) -> ST s (EngineCache s, V.Vector (ContactResult Constraint), (World' a)))+                -> EngineST a s (V.Vector (ContactResult Constraint))+wrapInitializer f = do+  (world, cache, externals) <- get+  (cache', constraints, world') <- lift . lift $ f cache world+  put (world', cache', externals)+  return constraints++updateWorld :: EngineST a s (World' a)+updateWorld = do+  EngineConfig{..} <- ask+  (world, _, exts) <- get+  let keys = G.culledKeys (G.toGrid gridAxes world)+      kContacts = prepareFrame keys world+  void . wrapUpdater' $ return . wApplyExternals exts _engineTimestep+  constraints <- wrapInitializer $ applyCachedSlns _engineContactBeh _engineTimestep kContacts+  wrapUpdater constraints $ improveWorld solutionProcessor kContacts+  wrapUpdater constraints $ improveWorld solutionProcessor kContacts+  void . wrapUpdater' $ return . wAdvance _engineTimestep+  wrapUpdater' $ return . over worldObjs (fmap woUpdateShape)++stepWorld :: Int -> EngineST a s (World' a)+stepWorld steps = do+  replicateM_ steps updateWorld+  view _1 <$> get++runEngineST :: Double -> Scene (Engine a) -> (forall s. EngineST a s b) -> b+runEngineST dt scene@Scene{..} action = runST $ do+  state' <- initEngine scene+  evalStateT (runReaderT action engineConfig) state'+  where engineConfig = EngineConfig dt _scContactBeh++runWorld :: Double -> Scene (Engine a) -> Int -> (World' a)+runWorld dt scene steps = runEngineST dt scene $ stepWorld steps
+ src/Physics/Linear.hs view
@@ -0,0 +1,391 @@+{-# LANGUAGE TemplateHaskell #-}+{-# LANGUAGE MagicHash #-}+{-# LANGUAGE RankNTypes #-}+{-# LANGUAGE BangPatterns #-}+{-# LANGUAGE DeriveGeneric #-}+{-# LANGUAGE DeriveAnyClass #-}+{-# LANGUAGE TypeFamilies #-}+{-# LANGUAGE MultiParamTypeClasses #-}++{- |+Arithmetic utility functions for vectors and matrices.+-}+module Physics.Linear where++import GHC.Generics (Generic)+import GHC.Prim+import GHC.Types (Double(D#))++import Control.DeepSeq+import Control.Lens+import Data.Vector.Unboxed.Deriving++import Shapes.Linear.Template (makeVectorType, defineJoinSplit)+import Shapes.Linear.MatrixTemplate+import Shapes.Linear.ValueInfos (doubleInfo)++import Utils.Utils++$(makeVectorType doubleInfo 2)+$(makeVectorType doubleInfo 3)+$(makeVectorType doubleInfo 6)+$(makeMatrixType doubleInfo (2, 2))+$(makeMatrixType doubleInfo (3, 3))+$(makeMatrixType doubleInfo (6, 6))+$(defineMatrixMul doubleInfo (2, 2, 2))+$(defineMatrixMul doubleInfo (3, 3, 3))+$(defineJoinSplit doubleInfo (3, 3))++newtype Diag6 = Diag6 V6 deriving Show++instance NFData V2 where+  rnf (V2 _ _) = ()+  {-# INLINE rnf #-}++newtype P2 = P2 V2 deriving (Generic, Show, NFData)++makeLenses ''P2++derivingUnbox "V2"+  [t| V2 -> (Double, Double) |]+  [| \(V2 a b) -> (D# a, D# b) |]+  [| \(D# a, D# b) -> V2 a b |]++derivingUnbox "P2"+  [t| P2 -> V2 |]+  [| \(P2 v) -> v |]+  [| P2 |]++derivingUnbox "V6"+  [t| V6 -> (Double, Double, Double, Double, Double, Double) |]+  [| \(V6 a b c d e f) -> (D# a, D# b, D# c, D# d, D# e, D# f) |]+  [| \(D# a, D# b, D# c, D# d, D# e, D# f) -> V6 a b c d e f |]++append2 :: V2 -> Double -> V3+(V2 a b) `append2` (D# c) = V3 a b c+{-# INLINE append2 #-}++split3 :: V3 -> (V2, Double)+split3 (V3 a b c) = (V2 a b, D# c)+{-# INLINE split3 #-}++smulV2 :: Double -> V2 -> V2+smulV2 (D# s) = liftV2 (*## s)+{-# INLINE smulV2 #-}++smulV2' :: V2 -> Double -> V2+smulV2' = flip smulV2+{-# INLINE smulV2' #-}++sdivV2 :: Double -> V2 -> V2+sdivV2 (D# s) = liftV2 (/## s)+{-# INLINE sdivV2 #-}++smulV6 :: Double -> V6 -> V6+smulV6 (D# s) = liftV6 (*## s)+{-# INLINE smulV6 #-}++smulV6' :: V6 -> Double -> V6+smulV6' = flip smulV6+{-# INLINE smulV6' #-}++smulM2x2 :: Double -> M2x2 -> M2x2+smulM2x2 (D# s) = liftM2x2 (*## s)+{-# INLINE smulM2x2 #-}++smulM2x2' :: M2x2 -> Double -> M2x2+smulM2x2' = flip smulM2x2+{-# INLINE smulM2x2' #-}++plusV2 :: V2 -> V2 -> V2+plusV2 = lift2V2 (+##)+{-# INLINE plusV2 #-}++plusV6 :: V6 -> V6 -> V6+plusV6 = lift2V6 (+##)+{-# INLINE plusV6 #-}++zeroV2 :: V2+zeroV2 = V2 0.0## 0.0##++zeroP2 :: P2+zeroP2 = P2 zeroV2++minusV2 :: V2 -> V2 -> V2+minusV2 = lift2V2 (-##)+{-# INLINE minusV2 #-}++crossV2 :: V2 -> V2 -> Double+crossV2 (V2 ax ay) (V2 bx by) = D# ((ax *## by) -## (ay *## bx))+{-# INLINE crossV2 #-}++crosszV2 :: V2 -> Double -> V2+crosszV2 (V2 ax ay) (D# bz) = V2 x y+  where x = ay *## bz+        y = negateDouble# (ax *## bz)++zcrossV2 :: Double -> V2 -> V2+zcrossV2 (D# az) (V2 bx by) = V2 x y+  where x = negateDouble# (az *## by)+        y = az *## bx++unitV2 :: Double -> V2+unitV2 (D# theta) = V2 (cosDouble# theta) (sinDouble# theta)++crossV2V2 :: V2 -> V2 -> V2 -> V2+crossV2V2 (V2 ax ay) (V2 bx by) (V2 cx cy) = V2 abcx abcy+  where abz = ax *## by -## ay *## bx+        abcx = negateDouble# (abz *## cy)+        abcy = abz *## cx++vmulDiag6 :: V6 -> Diag6 -> V6+vmulDiag6 v (Diag6 m) = lift2V6 (*##) v m+{-# INLINE vmulDiag6 #-}++vmulDiag6' :: Diag6 -> V6 -> V6+vmulDiag6' (Diag6 m) v = lift2V6 (*##) v m+{-# INLINE vmulDiag6' #-}++flip3v3 :: V6 -> V6+flip3v3 (V6 a b c d e f) = V6 d e f a b c+{-# INLINE flip3v3 #-}++afdot :: P2 -> V2 -> Double+afdot (P2 v0) v1 = D# (v0 `dotV2` v1)+{-# INLINE afdot #-}++afdot' :: V2 -> P2 -> Double+afdot' = flip afdot+{-# INLINE afdot' #-}++clockwiseV2 :: V2 -> V2+clockwiseV2 (V2 x y) = V2 y (negateDouble# x)+{-# INLINE clockwiseV2 #-}++normalizeV2 :: V2 -> V2+normalizeV2 (V2 x y) = V2 (x /## n) (y /## n)+  where n = sqrtDouble# ((x *## x) +## (y *## y))+{-# INLINE normalizeV2 #-}++-- | Length of a vector.+lengthV2 :: V2 -> Double+lengthV2 (V2 x y) = D# (sqrtDouble# ((x *## x) +## (y *## y)))++-- | Squared length of a vector.+sqLengthV2 :: V2 -> Double+sqLengthV2 (V2 x y) = D# ((x *## x) +## (y *## y))++diffP2 :: P2 -> P2 -> V2+diffP2 (P2 v0) (P2 v1) = v0 `minusV2` v1+{-# INLINE diffP2 #-}++midpointP2 :: P2 -> P2 -> P2+midpointP2 (P2 v0) (P2 v1) = P2 (2 `sdivV2` (v0 `plusV2` v1))++vplusP2 :: V2 -> P2 -> P2+vplusP2 v0 (P2 v1) = P2 (v0 `plusV2` v1)++pminusV2 :: P2 -> V2 -> P2+pminusV2 (P2 v0) v1 = P2 (v0 `minusV2` v1)++pplusV2 :: P2 -> V2 -> P2+pplusV2 (P2 v0) v1 = P2 (v0 `plusV2` v1)++invM2x2 :: M2x2 -> M2x2+invM2x2 (M2x2 a b c d) =+  D# invDet `smulM2x2` M2x2 d (negateDouble# b) (negateDouble# c) a+  where det = (a *## d) -## (b *## c)+        invDet = 1.0## /## det+{-# INLINE invM2x2 #-}++negateV2 :: V2 -> V2+negateV2 = liftV2 negateDouble#+{-# INLINE negateV2 #-}++identity2x2 :: M2x2+identity2x2 = M2x2 1.0## 0.0## 0.0## 1.0##+{-# INLINE identity2x2 #-}++identity3x3 :: M3x3+identity3x3 =+  M3x3+  1.0## 0.0## 0.0##+  0.0## 1.0## 0.0##+  0.0## 0.0## 1.0##+{-# INLINE identity3x3 #-}++afmul :: M3x3 -> V2 -> V2+afmul t (V2 a b) = V2 x y+  where !(V3 x y _) = t `mul3x3c` V3 a b 1.0##+{-# INLINE afmul #-}++afmul' :: M3x3 -> P2 -> P2+afmul' t (P2 v) = P2 $ t `afmul` v+{-# INLINE afmul' #-}++{-+WORKING WITH LINES+-}++data Line2 = Line2 { linePoint :: !P2+                   , lineNormal :: !V2 }++toLine2 :: P2 -> P2 -> Line2+toLine2 a b = Line2 { linePoint = a+                    , lineNormal = clockwiseV2 (b `diffP2` a) }+{-# INLINE toLine2 #-}++perpLine2 :: P2 -> P2 -> Line2+perpLine2 a b = Line2 { linePoint = a+                      , lineNormal = b `diffP2` a }+{-# INLINE perpLine2 #-}++-- solving some `mx = b` up in here+intersect2 :: Line2 -> Line2 -> P2+intersect2 (Line2 p n@(V2 n0 n1)) (Line2 p' n'@(V2 n2 n3)) =+  P2 (invM2x2 m `mul2x2c` b)+  where b = V2 b0 b1+        !(D# b0) = p `afdot` n+        !(D# b1) = p' `afdot` n'+        m = M2x2 n0 n1 n2 n3+{-# INLINE intersect2 #-}++{-+CLIPPING LINE SEGMENTS+-}++data ClipResult a+  = ClipLeft !a -- ^ clip the left side to this new endpoint+  | ClipRight !a -- ^ clip the right side to this new endpoint+  | ClipBoth !a -- ^ the entire segment was out-of-bounds+  | ClipNone -- ^ the entire segment was in-bounds++{- |+Apply a 'ClipResult' to a line segment. Replaces clipped endpoints.+If both endpoints (entire segment) clipped, use 'Left'ed clip point.++TODO: Delete this function?+-}+applyClip ::+     ClipResult a+  -> SP a a+  -> Either a (SP a a)+applyClip res (SP a b) = case res of+  ClipLeft c -> Right (SP c b)+  ClipRight c -> Right (SP a c)+  ClipBoth c -> Left c+  ClipNone -> Right (SP a b)+{-# INLINE applyClip #-}++-- | Alternate form of 'applyClip'. 'Nothing' if entire segment clipped.+applyClip' :: ClipResult a -> SP a a -> Maybe (SP a a)+applyClip' (ClipBoth _) _ = Nothing -- redundant definition+applyClip' res seg = either (const Nothing) Just (applyClip res seg)+{-# INLINE applyClip' #-}++-- | Alternate form of 'applyClip'. Removes clipped points.+applyClip'' :: ClipResult a -> SP s s -> Maybe (Either s (SP s s))+applyClip'' res (SP a b) = case res of+  ClipLeft _ -> Just $ Left b+  ClipRight _ -> Just $ Left a+  ClipBoth _ -> Nothing+  ClipNone -> Just $ Right (SP a b)+{-# INLINE applyClip'' #-}++{- |+Alternate form of 'applyClip'. Applies clipping using the given lens.++If 'ClipBoth', then use only the 'first' vertex of the line segment+and change it to use the clipping point. (TODO: Why?)++TODO: Delete this function?+-}+lApplyClip :: ASetter' s a+  -- ^ lens to access the "point" data to apply the clipping+  -> ClipResult a+  -- ^ clipping+  -> (SP s s)+  -- ^ line segment with endpoints that contain "point" data+  -> Either s (SP s s)+lApplyClip l res (SP a b) = case res of+  ClipLeft c -> Right (SP (set l c a) b)+  ClipRight c -> Right (SP a (set l c b))+  ClipBoth c -> Left (set l c a) -- use the 'first' vertex by default+  ClipNone -> Right (SP a b)+{-# INLINE lApplyClip #-}++-- | Alternate form of 'lApplyClip'. If the entire segment was behind the bound, use 'Nothing'.+lApplyClip' :: ASetter' s a -> ClipResult a -> (SP s s) -> Maybe (SP s s)+lApplyClip' _ (ClipBoth _) _ = Nothing -- redundant definition+lApplyClip' l res seg = either (const Nothing) Just (lApplyClip l res seg)+{-# INLINE lApplyClip' #-}++{- |+Given a bounding plane (expressed as a point and a normal),+figure out how to clip a line segment so it is on the positive side of the plane.+-}+clipSegment :: Line2+  -- ^ bounding plane+  -> SP Line2 (SP P2 P2)+  -- ^ (plane of the line segment, endpoints of the line segment)+  -> ClipResult P2+  -- ^ which endpoint(s) to clip, and what point to clip to+clipSegment boundary (SP incident (SP a b))+  | a' < c' = if b' < c' then ClipBoth c+              else ClipLeft c+  | b' < c' = ClipRight c+  | otherwise = ClipNone+  where c = intersect2 boundary incident+        n = lineNormal boundary+        a' = a `afdot` n+        b' = b `afdot` n+        c' = c `afdot` n+{-# INLINE clipSegment #-}++{-+TRANSFORMS+-}++rotate22_ :: Double# -> Double# -> M2x2+rotate22_ cosv sinv = M2x2 cosv (negateDouble# sinv) sinv cosv+{-# INLINE rotate22_ #-}++rotate22 :: Double# -> M2x2+rotate22 ori = rotate22_ c s+  where c = cosDouble# ori+        s = sinDouble# ori+{-# INLINE rotate22 #-}++afmat33 :: M2x2 -> M3x3+afmat33 (M2x2 x0 x1 y0 y1) =+  M3x3+  x0 x1 zer+  y0 y1 zer+  zer zer one+  where !one = 1.0##+        !zer = 0.0##+{-# INLINE afmat33 #-}++aftranslate33 :: V2 -> M3x3+aftranslate33 (V2 x y) =+  M3x3+  one zer x+  zer one y+  zer zer one+  where !one = 1.0##+        !zer = 0.0##+{-# INLINE aftranslate33 #-}++afrotate33 :: Double# -> M3x3+afrotate33 ori = afmat33 (rotate22 ori)+{-# INLINE afrotate33 #-}++afscale33 :: V2 -> M3x3+afscale33 (V2 x y) =+  M3x3+  x zer zer+  zer y zer+  zer zer one+  where !one = 1.0##+        !zer = 0.0##+{-# INLINE afscale33 #-}
+ src/Physics/Linear/Convert.hs view
@@ -0,0 +1,22 @@+{-# LANGUAGE MagicHash #-}++{- |+Convert between nice types from linear and unboxed types from shapes-math.+-}+module Physics.Linear.Convert where++import GHC.Types (Double(D#))++import qualified Linear.Affine as L+import qualified Linear.V2 as L++import Physics.Linear++toLV2 :: V2 -> L.V2 Double+toLV2 = (\[x, y] -> L.V2 x y) . toListV2++toLP2 :: P2 -> L.Point L.V2 Double+toLP2 (P2 v) = L.P . toLV2 $ v++fromLV2 :: L.V2 Double -> V2+fromLV2 (L.V2 (D# x) (D# y)) = V2 x y
+ src/Physics/Scenes/Balls.hs view
@@ -0,0 +1,100 @@+module Physics.Scenes.Balls where++import Control.Lens+import Control.Monad+import Data.Proxy+import Physics.Engine.Class+import Physics.Scenes.Scene+import Physics.World.Class+import Physics.Scenes.Stacks (box, boxStack, boxFloor', contactBehavior, externals)+import Physics.Scenes.TwoFlyingBoxes (boxB')++circle' :: (PhysicsEngine e)+     => Proxy e+     -> PENumber e+     -> (PENumber e, PENumber e)+     -> (PENumber e, PENumber e)+     -> PEExternalObj e+     -> PEWorldObj' e+circle' p radius center velocity =+  makeWorldObj p (box p center velocity) 0.2 (makeCircle p radius)++circleStack :: (PhysicsEngine e)+         => Proxy e+         -> PENumber e -- ^ radius+         -> (PENumber e, PENumber e) -- ^ bottom position+         -> (PENumber e, PENumber e) -- ^ velocity+         -> PENumber e -- ^ vertical spacing+         -> Int -- ^ number of objects+         -> PEExternalObj e -- ^ arbitrary user data+         -> [PEWorldObj' e]+circleStack _ _ _ _ _ 0 _ = []+circleStack p diameter bottom vel spacing n ext =+  circle' p (diameter / 2) bottom vel ext : circleStack p diameter bottom' vel spacing (n - 1) ext+  where bottom' = bottom & _2 %~ (+ (diameter + spacing))++stacks_ :: (PhysicsEngine e)+       => (Bool -> Bool)+       -> Proxy e+       -> PENumber e+       -> (PENumber e, PENumber e)+       -> (PENumber e, PENumber e)+       -> PENumber e+       -> (Int, Int)+       -> PEExternalObj e+       -> [PEWorldObj' e]+stacks_ ftype p diameter (center, bottom) vel spacing (n_w, n_h) ext =+  join . fmap f . take n_w $ iterate (\(a, b) -> (a + diameter, ftype b)) (leftmost, True)+  where leftmost = center - (diameter * fromIntegral (n_w - 1) / 2)+        f (left, True) = circleStack p diameter (left, bottom) vel spacing n_h ext+        f (left, False) = boxStack p (diameter, diameter) (left, bottom) vel spacing n_h ext++stacks :: (PhysicsEngine e)+       => Proxy e+       -> PENumber e+       -> (PENumber e, PENumber e)+       -> (PENumber e, PENumber e)+       -> PENumber e+       -> (Int, Int)+       -> PEExternalObj e+       -> [PEWorldObj' e]+stacks = stacks_ not++stacks' :: (PhysicsEngine e)+       => Proxy e+       -> PENumber e+       -> (PENumber e, PENumber e)+       -> (PENumber e, PENumber e)+       -> PENumber e+       -> (Int, Int)+       -> PEExternalObj e+       -> [PEWorldObj' e]+stacks' = stacks_ (const True)++makeScene :: (PhysicsEngine e) => (Int, Int) -> PENumber e -> PENumber e -> Proxy e -> PEExternalObj e -> Scene e+makeScene dims diameter spacing p ext = Scene w (externals p) (contactBehavior p)+  where w = makeWorld p (boxFloor' p ext : stacks p diameter (0, -4.5) (0, 0) spacing dims ext)++makeScene' :: (PhysicsEngine e) => (Int, Int) -> PENumber e -> PENumber e -> Proxy e -> PEExternalObj e -> Scene e+makeScene' dims diameter spacing p ext = Scene w (externals p) (contactBehavior p)+  where w = makeWorld p (boxFloor' p ext : stacks' p diameter (0, -4.5) (0, 0) spacing dims ext)++circleA :: (PhysicsEngine e) => Proxy e -> PEPhysicalObj e+circleA p = makePhysicalObj p (1, 0) 0 (-5, 0) 0 (2, 1)++circleB :: (PhysicsEngine e) => Proxy e -> PEPhysicalObj e+circleB p = makePhysicalObj p (-4, 0) 0 (5, 1.5) 0 (1, 0.5)++circleA' :: (PhysicsEngine e) => Proxy e -> PEExternalObj e -> PEWorldObj' e+circleA' p = makeWorldObj p (circleA p) 0.2 $ makeCircle p 2++circleB' :: (PhysicsEngine e) => Proxy e -> PEExternalObj e -> PEWorldObj' e+circleB' p = makeWorldObj p (circleB p) 0.2 $ makeCircle p 1++twoCircles :: (PhysicsEngine e) => Proxy e -> PEExternalObj e -> PEExternalObj e -> Scene e+twoCircles p a b = Scene world [] (contactBehavior p)+  where world = makeWorld p [circleA' p a, circleB' p b]++circleAndBox :: (PhysicsEngine e) => Proxy e -> PEExternalObj e -> PEExternalObj e -> Scene e+circleAndBox p a b = Scene world [] (contactBehavior p)+  where world = makeWorld p [circleA' p a, boxB' p b]
+ src/Physics/Scenes/FourBoxesTwoStatic.hs view
@@ -0,0 +1,82 @@+module Physics.Scenes.FourBoxesTwoStatic where++import Data.Proxy+import Physics.Engine.Class+import Physics.Scenes.Scene+import Physics.World.Class++boxA :: (PhysicsEngine e) => Proxy e -> PEPhysicalObj e+boxA p = makePhysicalObj p (1, 0) 0 (-5, 0) 0 (2, 1)++boxB :: (PhysicsEngine e) => Proxy e -> PEPhysicalObj e+boxB p = makePhysicalObj p (-4, 0) 0 (5, 2) 0 (1, 0.5)++boxC :: (PhysicsEngine e) => Proxy e -> PEPhysicalObj e+boxC p = makePhysicalObj p (0, 0) 0 (0, -6) 0 (0, 0)++boxD :: (PhysicsEngine e) => Proxy e -> PEPhysicalObj e+boxD p = makePhysicalObj p (0, 0) 0 (-5, -4) 0 (1, 0)++staticBoxD :: (PhysicsEngine e) => Proxy e -> PEPhysicalObj e+staticBoxD p = makePhysicalObj p (0, 0) 0 (-5, -4) 0 (0, 0)++boxA' :: (PhysicsEngine e) => Proxy e -> PEExternalObj e -> PEWorldObj' e+boxA' p = makeWorldObj p (boxA p) 0.2 $ makeRectangleHull p 4 4++boxB' :: (PhysicsEngine e) => Proxy e -> PEExternalObj e -> PEWorldObj' e+boxB' p = makeWorldObj p (boxB p) 0.2 $ makeRectangleHull p 2 2++boxC' :: (PhysicsEngine e) => Proxy e -> PEExternalObj e -> PEWorldObj' e+boxC' p = makeWorldObj p (boxC p) 0.2 $ makeRectangleHull p 18 1++boxD' :: (PhysicsEngine e) => Proxy e -> PEExternalObj e -> PEWorldObj' e+boxD' p = makeWorldObj p (boxD p) 0.2 $ makeRectangleHull p 0.4 3++staticBoxD' :: (PhysicsEngine e) => Proxy e -> PEExternalObj e -> PEWorldObj' e+staticBoxD' p = makeWorldObj p (staticBoxD p) 0.2 $ makeRectangleHull p 0.4 3++world+  :: (PhysicsEngine e)+  => Proxy e+  -> PEExternalObj e+  -> PEExternalObj e+  -> PEExternalObj e+  -> PEExternalObj e+  -> PEWorld' e+world p a b c d = makeWorld p [boxA' p a, boxB' p b, boxC' p c, boxD' p d]++world'+  :: (PhysicsEngine e)+  => Proxy e+  -> PEExternalObj e+  -> PEExternalObj e+  -> PEExternalObj e+  -> PEExternalObj e+  -> PEWorld' e+world' p a b c d = makeWorld p [boxA' p a, boxB' p b, boxC' p c, staticBoxD' p d]++externals :: (PhysicsEngine e) => Proxy e -> [External]+externals p = [makeConstantAccel p (0, -2)]++contactBehavior :: (PhysicsEngine e) => Proxy e -> PEContactBehavior e+contactBehavior p = makeContactBehavior p 0.01 0.02++scene+  :: (PhysicsEngine e)+  => Proxy e+  -> PEExternalObj e+  -> PEExternalObj e+  -> PEExternalObj e+  -> PEExternalObj e+  -> Scene e+scene p a b c d = Scene (world p a b c d) (externals p) (contactBehavior p)++scene'+  :: (PhysicsEngine e)+  => Proxy e+  -> PEExternalObj e+  -> PEExternalObj e+  -> PEExternalObj e+  -> PEExternalObj e+  -> Scene e+scene' p a b c d = Scene (world' p a b c d) (externals p) (contactBehavior p)
+ src/Physics/Scenes/Rolling.hs view
@@ -0,0 +1,51 @@+module Physics.Scenes.Rolling where++import Data.Proxy+import Physics.Engine.Class+import Physics.Scenes.Scene+import Physics.World.Class++shapeA :: (PhysicsEngine e) => Proxy e -> PEPhysicalObj e+shapeA p = makePhysicalObj p (0, 0) 0 (0, -6) 0 (0, 0)++shapeB :: (PhysicsEngine e) => Proxy e -> PEPhysicalObj e+shapeB p = makePhysicalObj p (0, 0) (-3) (-7, 12) 0 (1, 0.5)++shapeA' :: (PhysicsEngine e) => Proxy e -> PEExternalObj e -> PEWorldObj' e+shapeA' p = makeWorldObj p (shapeA p) 0.5 $ makeHull p [ (9, -0.5)+                                                       , (-9, 10)+                                                       , (-9, -0.5)+                                                       ]++shapeB' :: (PhysicsEngine e) => Proxy e -> PEExternalObj e -> PEWorldObj' e+shapeB' p = makeWorldObj p (shapeB p) 0.5 $ makeHull p [ (2, 1)+                                                       , (1, 2)+                                                       , (-1, 2)+                                                       , (-2, 1)+                                                       , (-2, -1)+                                                       , (-1, -2)+                                                       , (1, -2)+                                                       , (2, -1)+                                                       ]++world+  :: (PhysicsEngine e)+  => Proxy e+  -> PEExternalObj e+  -> PEExternalObj e+  -> PEWorld' e+world p a b = makeWorld p [shapeA' p a, shapeB' p b]++externals :: (PhysicsEngine e) => Proxy e -> [External]+externals p = [makeConstantAccel p (0, -4)]++contactBehavior :: (PhysicsEngine e) => Proxy e -> PEContactBehavior e+contactBehavior p = makeContactBehavior p 0.01 0.02++scene+  :: (PhysicsEngine e)+  => Proxy e+  -> PEExternalObj e+  -> PEExternalObj e+  -> Scene e+scene p a b = Scene (world p a b) (externals p) (contactBehavior p)
+ src/Physics/Scenes/Scene.hs view
@@ -0,0 +1,18 @@+{-# LANGUAGE TemplateHaskell #-}++{- |+Demo scenes for sample implementation.+TODO: Figure out a good place to put demo/sample stuff without slowing down my development workflow.+-}+module Physics.Scenes.Scene where++import Control.Lens+import Physics.Engine.Class+import Physics.World.Class++data Scene e =+  Scene { _scWorld :: PEWorld' e+        , _scExts :: [External]+        , _scContactBeh :: PEContactBehavior e+        }+makeLenses ''Scene
+ src/Physics/Scenes/Stacks.hs view
@@ -0,0 +1,104 @@+module Physics.Scenes.Stacks where++import Control.Lens+import Control.Monad+import Data.Proxy+import Physics.Engine.Class+import Physics.Scenes.Scene+import Physics.World.Class++box :: (PhysicsEngine e)+    => Proxy e+    -> (PENumber e, PENumber e)+    -> (PENumber e, PENumber e)+    -> PEPhysicalObj e+box p (x, y) (vx, vy) =+  makePhysicalObj p (vx, vy) 0 (x, y) 0 (2, 1)++boxFloor :: (PhysicsEngine e) => Proxy e -> PEPhysicalObj e+boxFloor p =+  makePhysicalObj p (0, 0) 0 (0, -6) 0 (0, 0)++box' :: (PhysicsEngine e)+     => Proxy e+     -> (PENumber e, PENumber e)+     -> (PENumber e, PENumber e)+     -> (PENumber e, PENumber e)+     -> PEExternalObj e+     -> PEWorldObj' e+box' p (w, h) center velocity =+  makeWorldObj p (box p center velocity) 0.2 (makeRectangleHull p w h)++boxFloor' :: (PhysicsEngine e) => Proxy e -> PEExternalObj e -> PEWorldObj' e+boxFloor' p =+  makeWorldObj p (boxFloor p) 0.2 (makeRectangleHull p 18 1)++boxStack :: (PhysicsEngine e)+         => Proxy e+         -> (PENumber e, PENumber e)+         -> (PENumber e, PENumber e)+         -> (PENumber e, PENumber e)+         -> PENumber e+         -> Int+         -> PEExternalObj e+         -> [PEWorldObj' e]+boxStack _ _ _ _ _ 0 _ = []+boxStack p size@(_, h) bottom vel spacing n ext =+  box' p size bottom vel ext : boxStack p size bottom' vel spacing (n - 1) ext+  where bottom' = bottom & _2 %~ (+ (h + spacing))++stacks :: (PhysicsEngine e)+       => Proxy e+       -> (PENumber e, PENumber e)+       -> (PENumber e, PENumber e)+       -> (PENumber e, PENumber e)+       -> PENumber e+       -> (Int, Int)+       -> PEExternalObj e+       -> [PEWorldObj' e]+stacks p size@(w, _) (center, bottom) vel spacing (n_w, n_h) ext =+  join . fmap f . take n_w $ iterate (+ w) leftmost+  where leftmost = center - (w * fromIntegral (n_w - 1) / 2)+        f left = boxStack p size (left, bottom) vel spacing n_h ext++world :: (PhysicsEngine e) => Proxy e -> PEExternalObj e -> PEWorld' e+world p ext = makeWorld p $ concat [ [boxFloor' p ext]+                                   , boxStack p (2, 2) (8  , -4.5) (-1, 0) 0 5 ext+                                   , boxStack p (2, 2) (5.5, -4.5) (-2, 0) 0 5 ext+                                   ]++world' :: (PhysicsEngine e) => Proxy e -> PEExternalObj e -> PEWorld' e+world' p ext = makeWorld p $ concat [ [boxFloor' p ext]+                                    , boxStack p (2, 2) (0, -4.5) (0, 0) 0 5 ext+                                    , [box' p (2, 2) (8, 0) (-6, 0) ext]+                                    ]++world'' :: (PhysicsEngine e) => Proxy e -> PEExternalObj e -> PEWorld' e+world'' p ext =+  makeWorld p (boxFloor' p ext : stacks p (1, 1) (0, -4.5) (0, 0) 1 (10, 10) ext)++world''' :: (PhysicsEngine e) => Proxy e -> PEExternalObj e -> PEWorld' e+world''' p ext =+  makeWorld p (boxFloor' p ext : stacks p (0.75, 0.75) (0, -4.5) (0, 0) 1 (15, 15) ext)++externals :: (PhysicsEngine e) => Proxy e -> [External]+externals p = [makeConstantAccel p (0, -2)]++contactBehavior :: (PhysicsEngine e) => Proxy e -> PEContactBehavior e+contactBehavior p = makeContactBehavior p 0.01 0.02++scene :: (PhysicsEngine e) => Proxy e -> PEExternalObj e -> Scene e+scene p ext = Scene (world p ext) (externals p) (contactBehavior p)++scene' :: (PhysicsEngine e) => Proxy e -> PEExternalObj e -> Scene e+scene' p ext = Scene (world' p ext) (externals p) (contactBehavior p)++scene'' :: (PhysicsEngine e) => Proxy e -> PEExternalObj e -> Scene e+scene'' p ext = Scene (world'' p ext) (externals p) (contactBehavior p)++scene''' :: (PhysicsEngine e) => Proxy e -> PEExternalObj e -> Scene e+scene''' p ext = Scene (world''' p ext) (externals p) (contactBehavior p)++makeScene :: (PhysicsEngine e) => (Int, Int) -> PENumber e -> Proxy e -> PEExternalObj e -> Scene e+makeScene dims spacing p ext = Scene w (externals p) (contactBehavior p)+  where w = makeWorld p (boxFloor' p ext : stacks p (0.2, 0.2) (0, -4.5) (0, 0) spacing dims ext)
+ src/Physics/Scenes/TwoFlyingBoxes.hs view
@@ -0,0 +1,42 @@+{-# LANGUAGE FlexibleContexts #-}++module Physics.Scenes.TwoFlyingBoxes where++import Data.Proxy+import Physics.Engine.Class+import Physics.Scenes.Scene+import Physics.World.Class++boxA :: (PhysicsEngine e) => Proxy e -> PEPhysicalObj e+boxA p = makePhysicalObj p (1, 0) 0 (-5, 0) 0 (2, 1)++boxB :: (PhysicsEngine e) => Proxy e -> PEPhysicalObj e+boxB p = makePhysicalObj p (-4, 0) 0 (5, 2) 0 (1, 0.5)++boxA' :: (PhysicsEngine e) => Proxy e -> PEExternalObj e -> PEWorldObj' e+boxA' p = makeWorldObj p (boxA p) 0.2 $ makeRectangleHull p 4 4++boxB' :: (PhysicsEngine e) => Proxy e -> PEExternalObj e -> PEWorldObj' e+boxB' p = makeWorldObj p (boxB p) 0.2 $ makeRectangleHull p 2 2++world+  :: (PhysicsEngine e)+  => Proxy e+  -> PEExternalObj e+  -> PEExternalObj e+  -> PEWorld' e+world p a b = makeWorld p [boxA' p a, boxB' p b]++externals :: Proxy e -> [External]+externals _ = []++contactBehavior :: (PhysicsEngine e) => Proxy e -> PEContactBehavior e+contactBehavior p = makeContactBehavior p 0.01 0.02++scene+  :: (PhysicsEngine e)+  => Proxy e+  -> PEExternalObj e+  -> PEExternalObj e+  -> Scene e+scene p a b = Scene (world p a b) (externals p) (contactBehavior p)
+ src/Physics/Solvers/Contact.hs view
@@ -0,0 +1,179 @@+{-# LANGUAGE FlexibleContexts      #-}+{-# LANGUAGE FlexibleInstances     #-}+{-# LANGUAGE MagicHash             #-}+{-# LANGUAGE MultiParamTypeClasses #-}+{-# LANGUAGE RankNTypes            #-}+{-# LANGUAGE RecordWildCards       #-}+{-# LANGUAGE ScopedTypeVariables   #-}++{- |+This is the backbone of the physics engine.+The functions here find contacts between objects and generate and solve constraints for these contacts.+It exploits temporal coherence of the scene by caching constraint solutions between frames.+This way, it can accumulate stability over time instead of requiring many solver iterations each frame.++The functions in this module are designed to be used in this order:++1. 'prepareFrame' - Which contacts are creating constraints for us to solve this frame?+2. 'applyCachedSlns' - Build this frame's Lagrangian and constraint caches. Apply relevant Lagrangians from the previous frame.+3. 'improveWorld' - Iteratively solve the constraints and update the cached Lagrangians. (Can do this step multiple times.)++The cache of Lagrangians should be retained for the next frame's 'applyCachedSlns'.+-}+module Physics.Solvers.Contact where++import           Control.Lens+import           Control.Monad+import           Control.Monad.ST+import           Data.Maybe+import qualified Data.Vector.Generic.Mutable as MV+import qualified Data.Vector.Unboxed         as V++import           Physics.Constraint+import           Physics.Constraints.Contact+import           Physics.Constraints.Types+import           Physics.Contact.Types+import           Physics.World.Class+import           Utils.Descending+import           Utils.Utils++-- | Calculate all contacts for the current frame.+prepareFrame ::+     (PhysicsWorld k w o)+  => Descending (k, k) -- ^ broadphase-filtered pairs of shapes to check for contact+  -> w -- ^ the world+  -> Descending (ObjectFeatureKey k, Flipping Contact') -- ^ list of contacts between shapes (in descending order of 'ObjectFeatureKey' because the caches are ordered)+prepareFrame pairKeys w =+  join $ f <$> pairKeys+  where f pairKey = keyedContacts pairKey shapes+          where shapes = pairMap (view woShape) $ fromJust (w ^? wPair pairKey)+        {-# INLINE f #-}+{-# INLINE prepareFrame #-}++-- | Update a pair of shapes based on the solution to their constraint.+applySln ::+     ContactResult Lagrangian -- ^ the solution+  -> ContactResult Constraint -- ^ the constraint+  -> (PhysicalObj, PhysicalObj)+  -> (PhysicalObj, PhysicalObj)+applySln crL crConstraint ab =+  foldl (flip ($)) ab $ applyLagrangian <$> crL <*> crConstraint+{-# INLINE applySln #-}++{- |+Calculate all new constraints from the contacts.+Apply cached lagrangians using new constraints.+Build new lagrangians cache with either zero or previously cached value.++TODO: reader monad for stuff that's const between frames (beh, dt)+-}+applyCachedSlns ::+     forall s k w o. (V.Unbox k, PhysicsWorld k w o)+  => ContactBehavior+  -> Double -- ^ dt+  -> Descending (ObjectFeatureKey k, Flipping Contact') -- ^ list of contacts between shapes+  -> V.MVector s (ObjectFeatureKey k, ContactResult Lagrangian) -- ^ list of constraint solutions from the previous frame+  -> w -- ^ the world+  -> ST s ( V.MVector s (ObjectFeatureKey k, ContactResult Lagrangian)+          , V.Vector (ContactResult Constraint)+          , w)+                          -- ^ (this frame's constraint solutions, this frame's constraints, the updated world)+applyCachedSlns beh dt kContacts oldLagrangians world0 = do+  lagrangians <- MV.new contactCount+  constraints <- MV.new contactCount+  let newCache ::+           (Int, w) -- ^ (current index in cache, current world)+        -> (ObjectFeatureKey k, Flipping Contact') -- ^ the contact to store at this index in the cache+        -> ST s (Int, w) -- ^ (next index in cache, updated world)+      newCache (cache_i', world) (key@ObjectFeatureKey {..}, fContact) = do+        let ab = fromJust $ iixView (\k -> wObj k . woPhys) _ofkObjKeys world+            -- ^ a pair of shapes (a, b)+            constraint = constraintGen beh dt fContact ab+        -- no previously-cached lagrangian, so start with 0.+        MV.write lagrangians cache_i' (key, pure 0)+        -- save the constraint so we can solve it (calculate/apply lagrangian)+        MV.write constraints cache_i' constraint+        return (cache_i' + 1, world)+      {-# INLINE newCache #-}+      useCache ::+           (Int, w) -- ^ (current index in cache, current world)+        -> (ObjectFeatureKey k, Flipping Contact') -- ^ the contact to store at this index in the cache+        -> (ObjectFeatureKey k, ContactResult Lagrangian) -- ^ the previous frame's solution for the last frame's corresponding contact+        -> ST s (Int, w) -- ^ (next index in cache, updated world)+      useCache (cache_i', world) (ObjectFeatureKey {..}, fContact) kLagr@(_, lagr) = do+        let ab = fromJust $ iixView (\k -> wObj k . woPhys) _ofkObjKeys world+            -- ^ a pair of shapes (a, b)+            constraint = constraintGen beh dt fContact ab+            world' =+              iixOver+                (\k -> wObj k . woPhys)+                (applySln lagr constraint)+                _ofkObjKeys+                world+            -- ^ update the world by applying the cached lagrangian with the newly-calculated constraint+        -- propagate the previously-cached lagrangian to the current frame's cache+        MV.write lagrangians cache_i' kLagr+        -- save the constraint so we can solve it (calculate/apply lagrangian)+        MV.write constraints cache_i' constraint+        return (cache_i' + 1, world')+      {-# INLINE useCache #-}+  -- zip the previous frame's cached solutions into this frame's contacts, applying cached solutions as we go+  (_, world1) <-+    descZipVector fst fst useCache newCache (0, world0) kContacts oldLagrangians+  frozenConstraints <- V.unsafeFreeze constraints+  return (lagrangians, frozenConstraints, world1)+  where+    contactCount = length kContacts+{-# INLINE applyCachedSlns #-}++-- | Solve the constraints for a given contact. (And apply the solution.)+improveSln ::+     (V.Unbox k, Contactable o)+  => SolutionProcessor (Double, Double) (ContactResult Lagrangian)+  -> ObjectFeatureKey k -- ^ identifies the contact: which objects, and which features within the objects+  -> Int -- ^ index in the solution/constraint caches+  -> V.MVector s (ObjectFeatureKey k, ContactResult Lagrangian) -- ^ solution cache+  -> V.Vector (ContactResult Constraint) -- ^ constraint cache+  -> (o, o) -- ^ pair of objects+  -> ST s (o, o) -- ^ updated pair of objects+improveSln slnProc key cache_i lagrangians constraints ab = do+  (_, cached_l) <- MV.read lagrangians cache_i+  let constraint = constraints V.! cache_i+      phys_ab = pairView woPhys ab+      mu_ab = pairView woMu ab+      new_l = lagrangian2 phys_ab <$> constraint+      processed_l = slnProc mu_ab cached_l new_l+      phys_ab' = applySln (_processedToApply processed_l) constraint phys_ab+  MV.write lagrangians cache_i (key, _processedToCache processed_l)+  return $ pairSet woPhys phys_ab' ab+{-# INLINE improveSln #-}++-- | Wraps `improveSln` to operate on the world instead of a pair of objects.+improveWorld' ::+     (V.Unbox k, PhysicsWorld k w o)+  => SolutionProcessor (Double, Double) (ContactResult Lagrangian)+  -> ObjectFeatureKey k+  -> Int+  -> V.MVector s (ObjectFeatureKey k, ContactResult Lagrangian)+  -> V.Vector (ContactResult Constraint)+  -> w+  -> ST s w+improveWorld' slnProc key@ObjectFeatureKey{..} cache_i lagrangians constraints =+  iixOver' wObj f _ofkObjKeys+  where f = improveSln slnProc key cache_i lagrangians constraints+{-# INLINE improveWorld' #-}++-- | Run `improveSln` on every constraint in the world.+improveWorld ::+     (V.Unbox k, PhysicsWorld k w o)+  => SolutionProcessor (Double, Double) (ContactResult Lagrangian)+  -> Descending (ObjectFeatureKey k, Flipping Contact')+  -> V.MVector s (ObjectFeatureKey k, ContactResult Lagrangian)+  -> V.Vector (ContactResult Constraint)+  -> w+  -> ST s w+improveWorld slnProc kContacts lagrangians constraints world0 =+  snd <$> foldM f (0, world0) kContacts+  where f (cache_i, world) (key, _) =+          (,) (cache_i + 1) <$> improveWorld' slnProc key cache_i lagrangians constraints world+{-# INLINE improveWorld #-}
+ src/Physics/Transform.hs view
@@ -0,0 +1,248 @@+{-# Language MagicHash #-}+{-# Language MultiParamTypeClasses #-}+{-# Language FlexibleInstances #-}+{-# Language FlexibleContexts #-}++{- |+Types for keeping track of local spaces (transformed relative to global space).+Also, tools for creating and composing 2D transformations.+-}+module Physics.Transform where++import GHC.Prim (Double#, (/##), negateDouble#)++import Utils.Utils+import Physics.Linear++{- |+A pair of transformation matrices to and from world space, respectively.+See 'transform' and 'untransform'.++The transformation matrices are multiplied with (2D affine) column vectors.+-}+type WorldTransform = SP M3x3 M3x3++{- |+Create a 'WorldTransform' with a given translation and rotation.++Applying the resulting 'WorldTransform' in the forward direction+moves the origin to the first argument (:: 'V2'), and rotates+by the second argument (:: 'Double#').++Applying the result in the reverse direction will revert the transformation.+-}+toTransform :: V2 -- ^ Translation+            -> Double# -- ^ Rotation+            -> WorldTransform+toTransform pos ori = joinTransforms (translateTransform pos) (rotateTransform ori)+{-# INLINE toTransform #-}++{- |+Create a 'WorldTransform' with a given scale.+-}+scaleTransform :: V2 -- ^ Scale+               -> WorldTransform+scaleTransform s@(V2 x y) = SP (afscale33 s) (afscale33 s')+  where s' = V2 (1.0## /## x) (1.0## /## y)+{-# INLINE scaleTransform #-}++{- |+Create a 'WorldTransform' with a given rotation.+-}+rotateTransform :: Double# -- ^ Rotation+                -> WorldTransform+rotateTransform ori = SP rot rot'+  where rot = afrotate33 ori+        rot' = afrotate33 (negateDouble# ori)+{-# INLINE rotateTransform #-}++-- | Create a 'WorldTransform' with a given translation.+translateTransform :: V2 -- ^ Translation+                   -> WorldTransform+translateTransform pos = SP transl transl'+  where transl = aftranslate33 pos+        transl' = aftranslate33 (negateV2 pos)+{-# INLINE translateTransform #-}++-- | Identity 'WorldTransform' does not alter the space.+idTransform :: WorldTransform+idTransform = SP identity3x3 identity3x3+{-# INLINE idTransform #-}++-- | Sequence two 'WorldTransform's to produce a third.+joinTransforms :: WorldTransform -- ^ The outer transform - applied last+               -> WorldTransform -- ^ The inner transform - applied first+               -> WorldTransform -- ^ The composite transform+joinTransforms (SP outer outer') (SP inner inner') = SP (outer `mul3x3x3` inner) (inner' `mul3x3x3` outer')+{-# INLINE joinTransforms #-}++-- | Sequence a list of 'WorldTransform's.+joinTransforms' :: [WorldTransform]+                -- ^ Transforms in order from outermost to innermost+                -> WorldTransform -- ^ The composite transform+joinTransforms' = foldl1 joinTransforms+{-# INLINE joinTransforms' #-}++-- | Reverse the direction of a 'WorldTransform'.+-- Simply swaps the two transformation matrices.+invertTransform :: WorldTransform -> WorldTransform+invertTransform (SP f g) = SP g f+{-# INLINE invertTransform #-}++-- TODO: add another type variable to track values that originated in the same local space+-- see lap, Geometry.overlap+data LocalT b = LocalT !WorldTransform !b deriving Show+type LV2 = LocalT V2+type LP2 = LocalT P2++data WorldT a = WorldT !a deriving (Show, Eq)+type WV2 = WorldT V2+type WP2 = WorldT P2++iExtract :: WorldT a -> a+iExtract (WorldT x) = x+{-# INLINE iExtract #-}++iInject :: a -> WorldT a+iInject = WorldT+{-# INLINE iInject #-}++iInject_ :: b -> LocalT b+iInject_ = LocalT idTransform+{-# INLINE iInject_ #-}++instance Functor LocalT where+  fmap f (LocalT t v) = LocalT t (f v)+  {-# INLINE fmap #-}++instance Functor WorldT where+  fmap f (WorldT v) = WorldT (f v)+  {-# INLINE fmap #-}++-- wExtract and wInject don't change the transform - they only move between types+class WorldTransformable t where+  -- | Apply 'WorldTransform' in the forward direction (local space to world space).+  transform :: WorldTransform -> t -> t+  -- | Apply 'WorldTransform' in the reverse direction (world space to local space).+  untransform :: WorldTransform -> t -> t++  wExtract :: LocalT t -> WorldT t+  wExtract (LocalT t v) = WorldT (transform t v)++  wExtract_ :: LocalT t -> t+  wExtract_ = iExtract . wExtract++  wInject :: WorldTransform -> WorldT t -> LocalT t+  wInject t v = LocalT t (untransform t (iExtract v))++  wInject_ :: WorldTransform -> t -> t -- same as wInject, but throws away type information+  wInject_ = untransform++instance WorldTransformable P2 where+  transform (SP trans _) = afmul' trans+  untransform (SP _ untrans) = afmul' untrans+  {-# INLINE transform #-}+  {-# INLINE untransform #-}++instance WorldTransformable V2 where+  transform (SP trans _) = afmul trans+  untransform (SP _ untrans) = afmul untrans+  {-# INLINE transform #-}+  {-# INLINE untransform #-}++instance (WorldTransformable t) => WorldTransformable (WorldT t) where+  transform t = WorldT . transform t . iExtract+  untransform t = WorldT . untransform t . iExtract+  {-# INLINE transform #-}+  {-# INLINE untransform #-}++instance WorldTransformable (LocalT b) where+  transform t' (LocalT t v) = LocalT (joinTransforms t' t) v+  untransform t' (LocalT t v) = LocalT (joinTransforms (invertTransform t') t) v+  wInject _ = LocalT idTransform . iExtract+  {-# INLINE transform #-}+  {-# INLINE untransform #-}+  {-# INLINE wInject #-}++instance (WorldTransformable b) => WorldTransformable (b, b) where+  transform t = pairMap (transform t)+  untransform t = pairMap (untransform t)+  {-# INLINE transform #-}+  {-# INLINE untransform #-}++instance (WorldTransformable b) => WorldTransformable [b] where+  transform t = map (transform t)+  untransform t = map (untransform t)+  {-# INLINE transform #-}+  {-# INLINE untransform #-}++instance (WorldTransformable b) => WorldTransformable (Maybe b) where+  transform t = fmap (transform t)+  untransform t = fmap (untransform t)+  {-# INLINE transform #-}+  {-# INLINE untransform #-}++data WaL w l = WaL { _wlW :: !(WorldT w)+                   , _wlL :: !(LocalT l)+                   } deriving (Show)+type WaL' t = WaL t t++instance (WorldTransformable w) => WorldTransformable (WaL w l) where+  transform t (WaL w l) =+    WaL (transform t w) (transform t l)+  untransform t (WaL w l) =+    WaL (untransform t w) (untransform t l)+  {-# INLINE transform #-}+  {-# INLINE untransform #-}++wfmap :: (Functor t) => (a -> t b) -> WorldT a -> t (WorldT b)+wfmap f (WorldT v) = fmap WorldT (f v)+{-# INLINE wfmap #-}++wflip :: (Functor t) => WorldT (t a) -> t (WorldT a)+wflip (WorldT v) = fmap WorldT v+{-# INLINE wflip #-}++wmap :: (a -> b) -> WorldT a -> WorldT b+wmap = fmap+{-# INLINE wmap #-}++wlift2 :: (a -> b -> c) -> WorldT a -> WorldT b -> WorldT c+wlift2 f x = wap (wmap f x)+{-# INLINE wlift2 #-}++wlift2_ :: (a -> b -> c) -> WorldT a -> WorldT b -> c+wlift2_ f x y = iExtract (wlift2 f x y)+{-# INLINE wlift2_ #-}++wap :: WorldT (a -> b) -> WorldT a -> WorldT b+wap (WorldT f) = wmap f+{-# INLINE wap #-}++wlap :: (WorldTransformable a) => WorldT (a -> b) -> LocalT a -> WorldT b+wlap f = wap f . wExtract+{-# INLINE wlap #-}++lwap :: (WorldTransformable a) => LocalT (a -> b) -> WorldT a -> LocalT b+lwap (LocalT t f) x = lmap f (wInject t x)+{-# INLINE lwap #-}++lap :: (WorldTransformable a) => LocalT (a -> b) -> LocalT a -> LocalT b+lap f x = lwap f (wExtract x)+{-# INLINE lap #-}++lmap :: (a -> b) -> LocalT a -> LocalT b+lmap = fmap+{-# INLINE lmap #-}++lfmap :: (Functor t) => (a -> t b) -> LocalT a -> t (LocalT b)+lfmap f (LocalT t v) = fmap (LocalT t) (f v)+{-# INLINE lfmap #-}++lunsafe_ :: (a -> b) -> LocalT a -> b+lunsafe_ f (LocalT _ v) = f v+{-# INLINE lunsafe_ #-}++wlens :: (Functor f) => (a -> f a) -> WorldT a -> f (WorldT a)+wlens f = fmap WorldT . f . iExtract+{-# INLINE wlens #-}
+ src/Physics/World.hs view
@@ -0,0 +1,58 @@+{-# LANGUAGE TemplateHaskell #-}+{-# LANGUAGE FlexibleContexts #-}+{-# LANGUAGE FlexibleInstances #-}+{-# LANGUAGE MultiParamTypeClasses #-}+{-# LANGUAGE RankNTypes #-}+{-# LANGUAGE DeriveGeneric #-}+{-# LANGUAGE DeriveAnyClass #-}+{-# LANGUAGE RecordWildCards #-}++{- |+Simple data structure that can act as a physical world to simulate.+I will likely implement more interesting world data structures in the future.+-}+module Physics.World where++import GHC.Generics (Generic)++import Control.DeepSeq+import Control.Lens+import qualified Data.IntMap.Strict as IM++import Physics.World.Class+import Utils.Utils++-- | A simple 'PhysicsWorld' implementation using 'IM.IntMap'+data World a =+  World { _worldObjs :: !(IM.IntMap a) -- ^ Inhabitants by unique 'Int' key+        , _worldNextKey :: !Int -- ^ Key to use for the next new inhabitant+        } deriving (Show, Generic, NFData)+makeLenses ''World++-- | A 'World' without any inhabitants.+emptyWorld :: World a+emptyWorld = World IM.empty 0+{-# INLINE emptyWorld #-}++-- | Add a new inhabitant to the 'World'+addObj :: World a -> a -> World a+addObj w = snd . addObj' w+{-# INLINE addObj #-}++-- | Add a new inhabitant to the 'World'. Also, get inhabitant's 'Int' key.+addObj' :: World a -> a -> (Int, World a)+addObj' w o = (n, w & worldObjs %~ IM.insert n o & worldNextKey .~ n + 1)+  where n = w ^. worldNextKey+{-# INLINE addObj' #-}++-- | Create a 'World' from a list of inhabitants+fromList :: [a] -- ^ Population for the new 'World'+         -> World a+fromList = foldl addObj emptyWorld+{-# INLINE fromList #-}++instance (Contactable a) => PhysicsWorld Int (World a) a where+  wKeys = IM.keys . _worldObjs+  wObj k = worldObjs . ix k+  wPair k = worldObjs . pairix k+  wObjs = worldObjs . itraversed
+ src/Physics/World/Class.hs view
@@ -0,0 +1,74 @@+{-# LANGUAGE TypeFamilies #-}+{-# LANGUAGE FunctionalDependencies #-}+{-# LANGUAGE FlexibleContexts #-}+{-# LANGUAGE RankNTypes #-}++{- |+Classes for data structures that can behave like a \"world\" of physical objects.+-}+module Physics.World.Class where++import Control.Lens hiding (transform)++import Physics.Constraint (PhysicalObj, advanceObj, _physObjTransform)+import Physics.Contact (Shape, setShapeTransform)+import Physics.Transform (transform)++-- | Class for objects with physical properties.+class Physical p where+  -- | Lens for the embedded 'PhysicalObj'+  woPhys :: Functor f => (PhysicalObj -> f PhysicalObj) -> p -> f p++-- | Class for objects that can be in contact with each other.+class (Physical p) => Contactable p where+  -- | Lens for embedded coefficient of friction \"mu\"+  woMu :: Functor f => (Double -> f Double) -> p -> f p+  -- | Lens for embedded contact shape+  woShape :: Functor f => (Shape -> f Shape) -> p -> f p+  -- | Lens for embedded pair of (coefficient of friction, contact shape)+  woMuShape :: Functor f+            => ((Double, Shape) -> f (Double, Shape))+            -> p+            -> f p++-- | Class for worlds (:: w) inhabited by physical objects (:: o)+-- each uniquely identified by a key (:: k)+class (Ord k, Contactable o) => PhysicsWorld k w o | w -> k o where+  -- | Keys of all the world's inhabitants+  wKeys :: w -> [k]+  -- | 'Traversal' of inhabitants with a given key+  wObj :: k -> Traversal' w o+  -- | 'Traversal'' of pairs of inhabitants with a given pair of keys+  wPair :: (k, k) -> Traversal' w (o, o)+  -- | 'IndexedTraversal'' of all inhabitants+  wObjs :: IndexedTraversal' k w o++-- | Advance the physical state of the world by a given time delta+-- using each inhabitant's current velocity.+wAdvance :: (PhysicsWorld k w o) => Double -- ^ Time delta+         -> w+         -> w+wAdvance dt w = w & wObjs.woPhys %~ (`advanceObj` dt)+{-# INLINE wAdvance #-}++-- | Update the shape of an object to match its current physical state.+--+-- By keeping all shapes in world space, we ensure that each shape+-- only needs to be transformed once per frame.+woUpdateShape :: (Contactable o) => o -> o+woUpdateShape obj =+  obj & woShape %~ flip setShapeTransform (transform t)+  where t = _physObjTransform . view woPhys $ obj+{-# INLINE woUpdateShape #-}++-- | An 'External' is a non-constraint effect (e.g. gravity) on physical objects.+type External = Double -> PhysicalObj -> PhysicalObj++-- | Apply 'External' effects to the objects in a world.+--+-- This happens each frame before constraints are created and solved.+wApplyExternals :: (PhysicsWorld k w o) => [External] -> Double -> w -> w+wApplyExternals exts dt w = foldl f w exts+  where f w0 ext = w0 & wObjs.woPhys %~ ext dt+        {-# INLINE f #-}+{-# INLINE wApplyExternals #-}
+ src/Physics/World/External.hs view
@@ -0,0 +1,28 @@+{-# LANGUAGE MagicHash #-}++{- |+The physics engine can also apply impulses that aren't related to constraints.+Gravity is an example.+-}+module Physics.World.External where++import GHC.Types (Double(D#))++import Control.Lens+import Physics.Constraint+import Physics.Linear+import Physics.World.Class++constantForce :: V2 -> External+constantForce f dt o = o & physObjVel %~ f'+  where f' v = v `plusV2` (f `smulV2'` dt) `smulV2'` im+        {-# INLINE f' #-}+        im = o ^. physObjInvMass.to (\x -> D# (_imLin x))+{-# INLINE constantForce #-}++constantAccel :: V2 -> External+constantAccel a dt o = o & physObjVel %~ f+  where f v = if isStaticLin (o ^. physObjInvMass)+              then v else v `plusV2` (a `smulV2'` dt)+        {-# INLINE f #-}+{-# INLINE constantAccel #-}
+ src/Physics/World/Object.hs view
@@ -0,0 +1,47 @@+{-# LANGUAGE TemplateHaskell #-}+{-# LANGUAGE DeriveGeneric #-}+{-# LANGUAGE DeriveAnyClass #-}+{-# LANGUAGE RecordWildCards #-}++{- | A physical object that can inhabit a physical world.+Contains a field to hold a reference to something outside+the physical world.+-}+module Physics.World.Object where++import GHC.Generics (Generic)++import Control.DeepSeq+import Control.Lens (makeLenses)+import Physics.Constraint+import Physics.Contact+import Physics.World.Class++data WorldObj a =+  WorldObj { _worldPhysObj  :: !PhysicalObj+           , _worldObjMu    :: !Double+           , _worldShape    :: !Shape+           , _worldUserData :: !a+           } deriving (Generic, NFData)+makeLenses ''WorldObj++instance Show (WorldObj a) where+  show (WorldObj obj _ _ _) = "WorldObj { " ++ show obj ++ " ... }"++instance Physical (WorldObj a) where+  woPhys = worldPhysObj++instance Contactable (WorldObj a) where+  woMu = worldObjMu+  woShape = worldShape+  woMuShape f obj@WorldObj{..} =+    g <$> f (_worldObjMu, _worldShape)+    where g (mu, shape) =  obj { _worldObjMu = mu+                               , _worldShape = shape+                               }+          {-# INLINE g #-}+  {-# INLINE woMuShape #-}++makeWorldObj :: PhysicalObj -> Double -> Shape -> a -> WorldObj a+makeWorldObj phys mu shape = woUpdateShape . WorldObj phys mu shape+{-# INLINE makeWorldObj #-}
+ src/Utils/Descending.hs view
@@ -0,0 +1,71 @@+{-# LANGUAGE TemplateHaskell #-}+{-# LANGUAGE FlexibleContexts #-}+{-# LANGUAGE FlexibleInstances #-}+{-# LANGUAGE TypeFamilies #-}+{-# LANGUAGE MultiParamTypeClasses #-}+{-# LANGUAGE DeriveGeneric #-}+{-# LANGUAGE DeriveAnyClass #-}+{-# LANGUAGE ScopedTypeVariables #-}++{- |+Keep track of lists that are in some kind of descending order.+Doesn't do anything fancy to actually enforce this, though.+-}+module Utils.Descending where++import GHC.Generics (Generic)++import Control.DeepSeq+import Control.Lens+import Control.Monad+import Control.Monad.ST+import qualified Data.Vector.Unboxed as V+import qualified Data.Vector.Generic.Mutable as MV++newtype Descending a =+  Descending { _descList :: [a] } deriving (Generic, NFData)+makeLenses ''Descending++instance Functor Descending where+  fmap f (Descending xs) = Descending $ fmap f xs+  {-# INLINE fmap #-}++instance Applicative Descending where+  pure = Descending . pure+  (Descending fs) <*> (Descending xs) = Descending (fs <*> xs)+  {-# INLINE pure #-}+  {-# INLINE (<*>) #-}++instance Monad Descending where+  (Descending xs) >>= f = Descending (xs >>= _descList . f)+  {-# INLINE (>>=) #-}++instance Foldable Descending where+  foldMap f (Descending xs) = foldMap f xs+  {-# INLINE foldMap #-}++descZipVector :: forall a b c s k. (Ord k, MV.MVector V.MVector b)+              => (a -> k)+              -> (b -> k)+              -> (c -> a -> b -> ST s c)+              -> (c -> a -> ST s c)+              -> c+              -> Descending a+              -> V.MVector s b+              -> ST s c+descZipVector getThisKey getThatKey accumBoth accumThis accum0 these those =+  let f :: (Int, c) -> a -> ST s (Int, c)+      f (that_i, accum) this+        | that_i < thatCount = do+            that <- MV.read those that_i+            let thatKey = getThatKey that+            if thisKey < thatKey+              then f (that_i + 1, accum) this -- keep looking+              else if thisKey == thatKey+                   then (,) (that_i + 1) <$> accumBoth accum this that+                   else (,) that_i <$> accumThis accum this+        | otherwise = (,) that_i <$> accumThis accum this+        where thatCount = MV.length those+              thisKey = getThisKey this+  in snd <$> foldM f (0, accum0) these+{-# INLINE descZipVector #-}
+ src/Utils/Utils.hs view
@@ -0,0 +1,356 @@+{-# LANGUAGE TemplateHaskell #-}+{-# LANGUAGE RankNTypes #-}+{-# LANGUAGE FlexibleContexts #-}+{-# LANGUAGE FlexibleInstances #-}+{-# LANGUAGE TypeFamilies #-}+{-# LANGUAGE MultiParamTypeClasses #-}+{-# LANGUAGE DeriveGeneric #-}+{-# LANGUAGE DeriveAnyClass #-}+{-# LANGUAGE RecordWildCards #-}++{- |+A bunch of unrelated utility functions and types.+-}+module Utils.Utils where++import GHC.Generics (Generic)++import Control.DeepSeq+import Control.Monad.Trans.Maybe+import Control.Lens+import Data.Maybe+import Data.Tuple+import qualified Data.IntMap.Strict as IM+import qualified Data.Vector.Unboxed as V+import Data.Vector.Unboxed.Deriving++data SP a b = SP { _spFst :: !a+                 , _spSnd :: !b+                 } deriving (Show, Eq, Generic, NFData, Ord)+makeLenses ''SP+derivingUnbox "SP"+  [t| forall a b. (V.Unbox a, V.Unbox b) => SP a b -> (a, b) |]+  [| \SP{..} -> (_spFst, _spSnd) |]+  [| uncurry SP |]++type SP' a = SP a a++toSP :: (a, b) -> SP a b+toSP (x, y) = SP x y+{-# INLINE toSP #-}++fromSP :: SP a b -> (a, b)+fromSP (SP x y) = (x, y)+{-# INLINE fromSP #-}++spMap :: (a -> b) -> SP a a -> SP b b+spMap f (SP x y) = SP (f x) (f y)+{-# INLINE spMap #-}++pairMap :: (a -> b) -> (a, a) -> (b, b)+pairMap f (x, y) = (f x, f y)+{-# INLINE pairMap #-}++pairAp :: (a -> b, c -> d) -> (a, c) -> (b, d)+pairAp (f, g) (x, y) = (f x, g y)+{-# INLINE pairAp #-}++pairFold :: Monoid m => (m, m) -> m+pairFold (a, b) = mappend a b+{-# INLINE pairFold #-}++maybeChange :: a -> (a -> Maybe a) -> a+maybeChange x f = fromMaybe x (f x)+{-# INLINE maybeChange #-}++toMaybe :: Bool -> a -> Maybe a+toMaybe b x = if b then Just x else Nothing+{-# INLINE toMaybe #-}++eitherToMaybe :: Either a b -> Maybe b+eitherToMaybe (Left _) = Nothing+eitherToMaybe (Right x) = Just x+{-# INLINE eitherToMaybe #-}++maybeBranch :: (a -> a -> Bool) -> Maybe a -> Maybe a -> Maybe (Either a a)+maybeBranch _ Nothing Nothing = Nothing+maybeBranch _ Nothing (Just x) = Just $ Right x+maybeBranch _ (Just x) Nothing = Just $ Left x+maybeBranch useLeft (Just x) (Just y) = if useLeft x y then Just $ Left x+                                        else Just $ Right y+{-# INLINE maybeBranch #-}++maybeBranchBoth :: (a -> a -> Bool) -> Maybe a -> Maybe a -> Maybe (Either a a)+maybeBranchBoth _ Nothing _ = Nothing+maybeBranchBoth _ _ Nothing = Nothing+maybeBranchBoth useLeft (Just x) (Just y) =+  if useLeft x y then Just $ Left x+  else Just $ Right y+{-# INLINE maybeBranchBoth #-}++takeIfAll :: (a -> Bool) -> [a] -> Maybe [a]+takeIfAll _ [] = Just []+takeIfAll p (x:xs)+  | p x = fmap (x:) (takeIfAll p xs)+  | otherwise = Nothing+{-# INLINE takeIfAll #-}++cycles :: [a] -> [[a]]+cycles xs = folds tail (cycle xs) xs+{-# INLINE cycles #-}++data Loop a = Loop { loopPrev :: Loop a+                   , loopVal :: !a+                   , loopNext :: Loop a }++-- TODO: for debugging only+instance (Show a, Eq a) => Show (Loop a) where+  show l = "loopify [" ++ f l [] ++ "]"+    where f x seen = if loopVal y `elem` seen' then show val+                     else show val ++ ", " ++ f y seen'+            where y = loopNext x+                  seen' = val:seen+                  val = loopVal x++loopify :: [a] -> Loop a+loopify [] = error "can't have an empty loop"+loopify v = let (first, lst) = f lst v first+            in first+  where f :: Loop a -> [a] -> Loop a -> (Loop a, Loop a)+        f prev [] next = (next, prev)+        f prev (x:xs) next = let (next', last') = f this xs next+                                 this = Loop prev x next'+                             in (this, last')+{-# INLINE loopify #-}++takeNext :: Int -> Loop a -> [Loop a]+takeNext = takeDir loopNext+{-# INLINE takeNext #-}++takePrev :: Int -> Loop a -> [Loop a]+takePrev = takeDir loopPrev+{-# INLINE takePrev #-}++takeDir :: (Loop a -> Loop a) -> Int -> Loop a -> [Loop a]+takeDir dir n x+  | n > 0 = x : takeDir dir (n - 1) (dir x)+  | n == 0 = []+  | otherwise = error "cannot take fewer than 0"+{-# INLINE takeDir #-}+++folds :: (b -> b) -> b -> [a] -> [b]+folds _ _ [] = []+folds f a0 (_:xs) = a0 : folds f (f a0) xs+{-# INLINE folds #-}++data Flipping a = Same !a | Flip !a deriving Show++flipToTuple :: Flipping a -> (Bool, a)+flipToTuple (Same x) = (True, x)+flipToTuple (Flip x) = (False, x)+{-# INLINE flipToTuple #-}++derivingUnbox "Flipping"+  [t| forall a. (V.Unbox a) => Flipping a -> (Bool, a) |]+  [| flipToTuple |]+  [| \(isSame, x) -> if isSame then Same x else Flip x |]++-- TODO: write an iso for Flipping and Either+flipAsEither :: Flipping a -> Either a a+flipAsEither (Same x) = Left x+flipAsEither (Flip x) = Right x+{-# INLINE flipAsEither #-}++flipWrap :: Flipping a -> b -> Flipping b+flipWrap (Same _) = Same+flipWrap (Flip _) = Flip+{-# INLINE flipWrap #-}++flipUnsafe :: (a -> (b, b) -> c) -> Flipping a -> (b, b) -> c+flipUnsafe f (Same x) = f x+flipUnsafe f (Flip x) = f x . swap+{-# INLINE flipUnsafe #-}++flipMap :: (a -> (b, b) -> c) -> Flipping a -> (b, b) -> Flipping c+flipMap f x = flipWrap x . flipUnsafe f x+{-# INLINE flipMap #-}++flipExtractWith :: (a -> b, a -> b) -> Flipping a -> b+flipExtractWith (f, _) (Same x) = f x+flipExtractWith (_, f) (Flip x) = f x+{-# INLINE flipExtractWith #-}++flipExtractPair :: (a -> (b, b)) -> Flipping a -> (b, b)+flipExtractPair f = flipExtractWith (f, swap . f)+{-# INLINE flipExtractPair #-}++flipJoin :: Flipping (Flipping a) -> Flipping a+flipJoin (Same (Same x)) = Same x+flipJoin (Flip (Same x)) = Flip x+flipJoin (Same (Flip x)) = Flip x+flipJoin (Flip (Flip x)) = Same x+{-# INLINE flipJoin #-}++instance Functor Flipping where+  fmap f (Same x) = Same (f x)+  fmap f (Flip x) = Flip (f x)+  {-# INLINE fmap #-}++class Flippable f where+  flipp :: f -> f++instance Flippable (x, x) where+  flipp = swap+  {-# INLINE flipp #-}++instance Flippable (Flipping x) where+  flipp (Same x) = Flip x+  flipp (Flip x) = Same x+  {-# INLINE flipp #-}++flipExtract :: (Flippable a) => Flipping a -> a+flipExtract (Same x) = x+flipExtract (Flip x) = flipp x+{-# INLINE flipExtract #-}++flipExtractUnsafe :: Flipping a -> a+flipExtractUnsafe (Same x) = x+flipExtractUnsafe (Flip x) = x+{-# INLINE flipExtractUnsafe #-}++flipInjectF :: Functor f => Flipping (f a) -> f (Flipping a)+flipInjectF x = fmap (flipWrap x) . flipExtractUnsafe $ x+{-# INLINE flipInjectF #-}++{- |+Combine two 'Either's, using the provided function to choose between two 'Right's.+Always choose the first 'Left'.+-}+eitherBranchBoth :: (b -> b -> Bool) -> Either a b -> Either a b -> Flipping (Either a b)+eitherBranchBoth _ x@(Left _) _ = Same x+eitherBranchBoth _ _ x@(Left _) = Flip x+eitherBranchBoth useFirst x@(Right a) y@(Right b) =+  if useFirst a b then Same x else Flip y+{-# INLINE eitherBranchBoth #-}++liftRightMaybe :: Either a (Maybe b) -> Maybe (Either a b)+liftRightMaybe (Right Nothing) = Nothing+liftRightMaybe (Right (Just x)) = Just $ Right x+liftRightMaybe (Left x) = Just $ Left x+{-# INLINE liftRightMaybe #-}++-- TODO: pull out the stuff that depends on lens.++ixZipWith :: (Ixed s, TraversableWithIndex (Index s) t) => (a -> Maybe (IxValue s) -> b) -> t a -> s -> t b+ixZipWith f xs ys = xs & itraversed %@~ g+  where g i x = f x (ys ^? ix i)+{-# INLINE ixZipWith #-}++overWith :: Lens' s a -> ((a, a) -> (a, a)) -> (s, s) -> (s, s)+overWith l f (x, y) = (x & l .~ a, y & l .~ b)+  where (a, b) = f (x ^. l, y ^. l)+{-# INLINE overWith #-}++findOrInsert :: IM.Key -> a -> IM.IntMap a -> (Maybe a, IM.IntMap a)+findOrInsert = IM.insertLookupWithKey (\_ _ a -> a)+{-# INLINE findOrInsert #-}++findOrInsert' :: IM.Key -> a -> IM.IntMap a -> (a, IM.IntMap a)+findOrInsert' k x t = (fromMaybe x mx, t')+  where (mx, t') = findOrInsert k x t+{-# INLINE findOrInsert' #-}++posMod :: (Integral a) => a -> a -> a+posMod x n = if res < 0 then res + n else res+  where res = x `mod` n+{-# INLINE posMod #-}++pairiix :: (Ixed m) => (Index m, Index m) -> IndexedTraversal' (Index m, Index m) m (IxValue m, IxValue m)+pairiix ij f = pairix ij (indexed f ij)+{-# INLINE pairiix #-}++-- Applicative f => (IxValue m -> f (IxValue m)) -> m -> f m+pairix :: (Ixed m) => (Index m, Index m) -> Traversal' m (IxValue m, IxValue m)+pairix ij@(i, j) f t = maybe (pure t) change pair+  where pair = do+          a <- t ^? ix i+          b <- t ^? ix j+          return (a, b)+        change pair' = uncurry g <$> indexed f ij pair'+          where g a b = set (ix j) b . set (ix i) a $ t+{-# INLINE pairix #-}++pairOver :: (forall f. Functor f => (b -> f b) -> a -> f a)+         -> ((b, b) -> (b, b))+         -> (a, a)+         -> (a, a)+pairOver l f (a, b) = (a', b')+  where x = a ^. l+        y = b ^. l+        (x', y') = f (x, y)+        a' = a & l .~ x'+        b' = b & l .~ y'+{-# INLINE pairOver #-}++pairView :: (forall f. Functor f => (b -> f b) -> a -> f a)+         -> (a, a)+         -> (b, b)+pairView l = pairMap $ view l+{-# INLINE pairView #-}++pairView' :: (forall f. Applicative f => (b -> f b) -> a -> f a)+          -> (a, a)+          -> Maybe (b, b)+pairView' l (a, b) = (,) <$> (a ^? l) <*> (b ^? l)+{-# INLINE pairView' #-}++pairSet :: (forall f. Functor f => (b -> f b) -> a -> f a)+         -> (b, b)+         -> (a, a)+         -> (a, a)+pairSet l (x, y) (a, b) = (a & l .~ x, b & l .~ y)+{-# INLINE pairSet #-}++-- TODO: this is the wrong name+iixOver :: (forall f. Applicative f => k -> (b -> f b) -> a -> f a)+        -> ((b, b) -> (b, b))+        -> (k, k)+        -> a+        -> a+iixOver ixL f (i, j) w =+  fromMaybe w $ do+  a <- w ^? ixL i+  b <- w ^? ixL j+  let (a', b') = f (a, b)+  return $ w & ixL i .~ a' & ixL j .~ b'+{-# INLINE iixOver #-}++-- TODO: this is the wrong name+iixOver' :: (Monad m)+        => (forall f. Applicative f => k -> (b -> f b) -> a -> f a)+        -> ((b, b) -> m (b, b))+        -> (k, k)+        -> a+        -> m a+iixOver' ixL f (i, j) w =+  fmap (fromMaybe w) . runMaybeT $ do+  a <- liftMaybe $ w ^? ixL i+  b <- liftMaybe $ w ^? ixL j+  (a', b') <- liftMaybe' $ f (a, b)+  return $ w & ixL i .~ a' & ixL j .~ b'+{-# INLINE iixOver' #-}++-- TODO: this is the wrong name+iixView :: (forall f. Applicative f => k -> (b -> f b) -> a -> f a)+        -> (k, k)+        -> a+        -> Maybe (b, b)+iixView ixL (i, j) w = (,) <$> (w ^? ixL i) <*> (w ^? ixL j)+{-# INLINE iixView #-}++liftMaybe :: (Monad m) => Maybe a -> MaybeT m a+liftMaybe = MaybeT . return++liftMaybe' :: (Monad m) => m a -> MaybeT m a+liftMaybe' = MaybeT . fmap return
+ test/Physics/Broadphase/AabbSpec.hs view
@@ -0,0 +1,16 @@+module Physics.Broadphase.AabbSpec where++import Test.Hspec+import Test.QuickCheck++import Physics.Broadphase.Aabb++spec :: Spec+spec =+  it "|unorderedPairs n| = n * n-1 / 2" $ property $+    \(ItemCount n) -> length (unorderedPairs n) == (n * (n-1) `quot` 2)++newtype ItemCount = ItemCount Int deriving Show++instance Arbitrary ItemCount where+  arbitrary = ItemCount <$> choose (0, 30)
+ test/Spec.hs view
@@ -0,0 +1,9 @@+module Main where++import Test.Hspec++import qualified Physics.Broadphase.AabbSpec++main :: IO ()+main = hspec $+  describe "TemplateSpec" Physics.Broadphase.AabbSpec.spec