diff --git a/LICENSE b/LICENSE new file mode 100644 --- /dev/null +++ b/LICENSE @@ -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. + diff --git a/Setup.hs b/Setup.hs new file mode 100644 --- /dev/null +++ b/Setup.hs @@ -0,0 +1,2 @@ +import Distribution.Simple +main = defaultMain diff --git a/bench/Main.hs b/bench/Main.hs new file mode 100644 --- /dev/null +++ b/bench/Main.hs @@ -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 diff --git a/bench/Physics/Broadphase/Benchmark.hs b/bench/Physics/Broadphase/Benchmark.hs new file mode 100644 --- /dev/null +++ b/bench/Physics/Broadphase/Benchmark.hs @@ -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 diff --git a/bench/Physics/Constraint/Benchmark.hs b/bench/Physics/Constraint/Benchmark.hs new file mode 100644 --- /dev/null +++ b/bench/Physics/Constraint/Benchmark.hs @@ -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] diff --git a/bench/Physics/Contact/Benchmark.hs b/bench/Physics/Contact/Benchmark.hs new file mode 100644 --- /dev/null +++ b/bench/Physics/Contact/Benchmark.hs @@ -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'] diff --git a/bench/Physics/Contact/Circle/Benchmark.hs b/bench/Physics/Contact/Circle/Benchmark.hs new file mode 100644 --- /dev/null +++ b/bench/Physics/Contact/Circle/Benchmark.hs @@ -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) diff --git a/bench/Physics/Contact/GJK/Benchmark.hs b/bench/Physics/Contact/GJK/Benchmark.hs new file mode 100644 --- /dev/null +++ b/bench/Physics/Contact/GJK/Benchmark.hs @@ -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##) diff --git a/shapes.cabal b/shapes.cabal new file mode 100644 --- /dev/null +++ b/shapes.cabal @@ -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 +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 diff --git a/src/Physics/Broadphase/Aabb.hs b/src/Physics/Broadphase/Aabb.hs new file mode 100644 --- /dev/null +++ b/src/Physics/Broadphase/Aabb.hs @@ -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 #-} diff --git a/src/Physics/Broadphase/Grid.hs b/src/Physics/Broadphase/Grid.hs new file mode 100644 --- /dev/null +++ b/src/Physics/Broadphase/Grid.hs @@ -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) diff --git a/src/Physics/Constraint.hs b/src/Physics/Constraint.hs new file mode 100644 --- /dev/null +++ b/src/Physics/Constraint.hs @@ -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 #-} diff --git a/src/Physics/Constraint/Types.hs b/src/Physics/Constraint/Types.hs new file mode 100644 --- /dev/null +++ b/src/Physics/Constraint/Types.hs @@ -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 |] diff --git a/src/Physics/Constraints/Contact.hs b/src/Physics/Constraints/Contact.hs new file mode 100644 --- /dev/null +++ b/src/Physics/Constraints/Contact.hs @@ -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 #-} diff --git a/src/Physics/Constraints/Contact/Friction.hs b/src/Physics/Constraints/Contact/Friction.hs new file mode 100644 --- /dev/null +++ b/src/Physics/Constraints/Contact/Friction.hs @@ -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 #-} diff --git a/src/Physics/Constraints/Contact/NonPenetration.hs b/src/Physics/Constraints/Contact/NonPenetration.hs new file mode 100644 --- /dev/null +++ b/src/Physics/Constraints/Contact/NonPenetration.hs @@ -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 #-} diff --git a/src/Physics/Constraints/SolutionProcessors.hs b/src/Physics/Constraints/SolutionProcessors.hs new file mode 100644 --- /dev/null +++ b/src/Physics/Constraints/SolutionProcessors.hs @@ -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 #-} diff --git a/src/Physics/Constraints/Types.hs b/src/Physics/Constraints/Types.hs new file mode 100644 --- /dev/null +++ b/src/Physics/Constraints/Types.hs @@ -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 (<*>) #-} diff --git a/src/Physics/Contact.hs b/src/Physics/Contact.hs new file mode 100644 --- /dev/null +++ b/src/Physics/Contact.hs @@ -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 diff --git a/src/Physics/Contact/Circle.hs b/src/Physics/Contact/Circle.hs new file mode 100644 --- /dev/null +++ b/src/Physics/Contact/Circle.hs @@ -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 diff --git a/src/Physics/Contact/CircleVsCircle.hs b/src/Physics/Contact/CircleVsCircle.hs new file mode 100644 --- /dev/null +++ b/src/Physics/Contact/CircleVsCircle.hs @@ -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) diff --git a/src/Physics/Contact/CircleVsHull.hs b/src/Physics/Contact/CircleVsHull.hs new file mode 100644 --- /dev/null +++ b/src/Physics/Contact/CircleVsHull.hs @@ -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 diff --git a/src/Physics/Contact/ConvexHull.hs b/src/Physics/Contact/ConvexHull.hs new file mode 100644 --- /dev/null +++ b/src/Physics/Contact/ConvexHull.hs @@ -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 #-} diff --git a/src/Physics/Contact/GJK.hs b/src/Physics/Contact/GJK.hs new file mode 100644 --- /dev/null +++ b/src/Physics/Contact/GJK.hs @@ -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 diff --git a/src/Physics/Contact/HullVsHull.hs b/src/Physics/Contact/HullVsHull.hs new file mode 100644 --- /dev/null +++ b/src/Physics/Contact/HullVsHull.hs @@ -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 #-} diff --git a/src/Physics/Contact/SAT.hs b/src/Physics/Contact/SAT.hs new file mode 100644 --- /dev/null +++ b/src/Physics/Contact/SAT.hs @@ -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_ #-} diff --git a/src/Physics/Contact/Types.hs b/src/Physics/Contact/Types.hs new file mode 100644 --- /dev/null +++ b/src/Physics/Contact/Types.hs @@ -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 |] diff --git a/src/Physics/Engine.hs b/src/Physics/Engine.hs new file mode 100644 --- /dev/null +++ b/src/Physics/Engine.hs @@ -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 diff --git a/src/Physics/Engine/Class.hs b/src/Physics/Engine/Class.hs new file mode 100644 --- /dev/null +++ b/src/Physics/Engine/Class.hs @@ -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) diff --git a/src/Physics/Engine/Main.hs b/src/Physics/Engine/Main.hs new file mode 100644 --- /dev/null +++ b/src/Physics/Engine/Main.hs @@ -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 diff --git a/src/Physics/Linear.hs b/src/Physics/Linear.hs new file mode 100644 --- /dev/null +++ b/src/Physics/Linear.hs @@ -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 #-} diff --git a/src/Physics/Linear/Convert.hs b/src/Physics/Linear/Convert.hs new file mode 100644 --- /dev/null +++ b/src/Physics/Linear/Convert.hs @@ -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 diff --git a/src/Physics/Scenes/Balls.hs b/src/Physics/Scenes/Balls.hs new file mode 100644 --- /dev/null +++ b/src/Physics/Scenes/Balls.hs @@ -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] diff --git a/src/Physics/Scenes/FourBoxesTwoStatic.hs b/src/Physics/Scenes/FourBoxesTwoStatic.hs new file mode 100644 --- /dev/null +++ b/src/Physics/Scenes/FourBoxesTwoStatic.hs @@ -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) diff --git a/src/Physics/Scenes/Rolling.hs b/src/Physics/Scenes/Rolling.hs new file mode 100644 --- /dev/null +++ b/src/Physics/Scenes/Rolling.hs @@ -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) diff --git a/src/Physics/Scenes/Scene.hs b/src/Physics/Scenes/Scene.hs new file mode 100644 --- /dev/null +++ b/src/Physics/Scenes/Scene.hs @@ -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 diff --git a/src/Physics/Scenes/Stacks.hs b/src/Physics/Scenes/Stacks.hs new file mode 100644 --- /dev/null +++ b/src/Physics/Scenes/Stacks.hs @@ -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) diff --git a/src/Physics/Scenes/TwoFlyingBoxes.hs b/src/Physics/Scenes/TwoFlyingBoxes.hs new file mode 100644 --- /dev/null +++ b/src/Physics/Scenes/TwoFlyingBoxes.hs @@ -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) diff --git a/src/Physics/Solvers/Contact.hs b/src/Physics/Solvers/Contact.hs new file mode 100644 --- /dev/null +++ b/src/Physics/Solvers/Contact.hs @@ -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 #-} diff --git a/src/Physics/Transform.hs b/src/Physics/Transform.hs new file mode 100644 --- /dev/null +++ b/src/Physics/Transform.hs @@ -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 #-} diff --git a/src/Physics/World.hs b/src/Physics/World.hs new file mode 100644 --- /dev/null +++ b/src/Physics/World.hs @@ -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 diff --git a/src/Physics/World/Class.hs b/src/Physics/World/Class.hs new file mode 100644 --- /dev/null +++ b/src/Physics/World/Class.hs @@ -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 #-} diff --git a/src/Physics/World/External.hs b/src/Physics/World/External.hs new file mode 100644 --- /dev/null +++ b/src/Physics/World/External.hs @@ -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 #-} diff --git a/src/Physics/World/Object.hs b/src/Physics/World/Object.hs new file mode 100644 --- /dev/null +++ b/src/Physics/World/Object.hs @@ -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 #-} diff --git a/src/Utils/Descending.hs b/src/Utils/Descending.hs new file mode 100644 --- /dev/null +++ b/src/Utils/Descending.hs @@ -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 #-} diff --git a/src/Utils/Utils.hs b/src/Utils/Utils.hs new file mode 100644 --- /dev/null +++ b/src/Utils/Utils.hs @@ -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 diff --git a/test/Physics/Broadphase/AabbSpec.hs b/test/Physics/Broadphase/AabbSpec.hs new file mode 100644 --- /dev/null +++ b/test/Physics/Broadphase/AabbSpec.hs @@ -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) diff --git a/test/Spec.hs b/test/Spec.hs new file mode 100644 --- /dev/null +++ b/test/Spec.hs @@ -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