Add a restitution constant

elliottt · Aug 1, 2010 · 4086342 · 4086342
1 parent b89d6f2
commit 4086342
Show file tree

Hide file tree

Showing 4 changed files with 23 additions and 16 deletions.
diff --git a/Physics/Shape.hs b/Physics/Shape.hs
@@ -203,9 +203,7 @@ checkPolygonPolygon c10 ps10 c20 ps20 =
           p2 <- range (map proj ps2)
           let o = rangeOverlap p1 p2
           guard (o >= 0)
-          if o < z
-             then return (o,axis)
-             else return (z,norm)
+          if o < z then return (o,axis) else return (z,norm)
     (overlap,Point x y) <- foldM step (100000,Point 0 0) (edges ps1)
     let dir = normalize (pointToVector (c1 - c2))
     return Collision

diff --git a/Physics/Vector.hs b/Physics/Vector.hs
@@ -42,7 +42,8 @@ normalVector :: Vector -> Vector
 normalVector (Vector x y) = Vector (-y) x
 
 projAlong :: Vector -> Vector -> Vector
-projAlong u a = scaleVector ((u `dotProduct` a) / (vectorLength a) ^ 2) a
+projAlong u a = scaleVector ((u `dotProduct` a) / (len * len)) a
+  where len = vectorLength a
 
 -- | Turn a point into a vector.
 pointToVector :: Point -> Vector

diff --git a/Physics/World.hs b/Physics/World.hs
@@ -21,18 +21,20 @@ import Debug.Trace
 type Body = PhysicalState Shape
 
 data World = World
-  { worldBox     :: !AABB
-  , worldBodies  :: [PhysicalState Shape]
-  , worldGravity :: Maybe Vector
+  { worldBox         :: !AABB
+  , worldBodies      :: [PhysicalState Shape]
+  , worldGravity     :: Maybe Vector
+  , worldRestitution :: !GLfloat
   }
 
 instance Render World where
   render w = render (worldBodies w)
 
 emptyWorld w h = World
-  { worldBox     = AABB (Point (-w / 2) (h / 2)) (Point w h)
-  , worldBodies  = []
-  , worldGravity = Nothing
+  { worldBox         = AABB (Point (-w / 2) (h / 2)) (Point w h)
+  , worldBodies      = []
+  , worldGravity     = Nothing
+  , worldRestitution = 1
   }
 
 stepWorld :: Interval -> World -> World
@@ -44,7 +46,7 @@ stepWorld dt0 w = w
   (ds,ss)     = collisions w
   ds'         = mapMaybe step ds
   step (p,cs) = do
-    let resolve (c,q) = resolveCollision c p q
+    let resolve (c,q) = resolveCollision w c p q
     case map resolve cs of
 
       [] -> do
@@ -61,8 +63,8 @@ stepWorld dt0 w = w
         return p'
 
 -- | Turn a collision into a displacement vector, and a new velocity.
-resolveCollision :: Collision -> Body -> Body -> (Vector,Vector)
-resolveCollision c p q =
+resolveCollision :: World -> Collision -> Body -> Body -> (Vector,Vector)
+resolveCollision w c p q =
   "resolve" `trace`
   show v `trace`
   show v' `trace`
@@ -75,7 +77,8 @@ resolveCollision c p q =
   n'    = normalVector n
   v     = psVelocity p
   nperp = projAlong v n'
-  v'    = subtractVector nperp (scaleVector (n `dotProduct` v) n)
+  rest  = worldRestitution w
+  v'    = subtractVector nperp (scaleVector (rest * n `dotProduct` v) n)
 
 collisions :: World -> ([(Body,[(Collision,Body)])], [Body])
 collisions w = loop ds [] []

diff --git a/Test.hs b/Test.hs
@@ -18,13 +18,18 @@ main = do
   initGraphics "Test" 800 600
 
   let ground = mkPhysicalState $ fromJust $ rectangle (Point 0 0) 10 0.1
-  let square = mkPhysicalState $ fromJust $ rectangle (Point 1 5) 1 1
+      wall   = mkPhysicalState $ fromJust $ rectangle (Point (-5) 5) 0.1 10
+  let square = mkPhysicalState $ fromJust $ rectangle (Point 0 5) 1 1
+  let s2     = mkPhysicalState $ fromJust $ rectangle (Point 2 5) 1 1
   let world  = (emptyWorld 1000 1000)
-        { worldGravity = Just (Vector 0 (-0.1))
+        { worldGravity     = Just (Vector 0 (-0.1))
+        , worldRestitution = 0.8
         }
 
   ref <- newIORef $ addBody ground { psStatic = True }
+                  $ addBody wall   { psStatic = True }
                   $ addBody (applyImpulse (Vector 0.01 0) square)
+                  $ addBody s2
                     world
 
   withEventManager $ \em -> do