{-# OPTIONS_GHC -XBangPatterns -XNoMonomorphismRestriction -XScopedTypeVariables -XFunctionalDependencies -XFlexibleInstances -XMultiParamTypeClasses #-} ------------------------------------------------------------------------------- import GHC.Prim import System.Random import System.Environment import Data.IORef import Data.Maybe import Data.List (minimumBy, deleteBy) import Data.Function (on) import FRP.Yampa.VectorSpace import FRP.Yampa.Vector2 import FRP.Yampa.Point2 import FRP.Yampa.Forceable import qualified Graphics.UI.GLFW as GLFW import qualified Graphics.Rendering.OpenGL as GL import qualified Graphics.Rendering.OpenGL.GLU as GLU import Graphics.Rendering.OpenGL (($=), get, Vertex2(..)) import Control.Applicative import Control.Monad import Debug.Trace import System.IO.Unsafe import Control.Concurrent ------------------------------------------------------------------------------- -- Old school stream based FRP type Time = Double type DTime = Double type SF a b = [a] -> [b] constant a = repeat a integral :: (Show v, VectorSpace v s) => v -> [v] -> [s] -> [v] integral x (v:vs) (dt:dts) = x:(integral (x ^+^ dt *^ v) vs dts) population :: [SF a b] -> SF a [b] population sfs as = transpose (map ($ as) sfs) where transpose :: [[a]] -> [[a]] transpose ss = (map strictHead ss) : transpose (map tail ss) where -- Make sure we force evaluation of all heads of our streams strictHead !((!x):xs) = x ------------------------------------------------------------------------------- windowWidth = 800 -- physical pixels windowHeight = 600 -- physical pixels windowDepth = 16 -- bits ------------------------------------------------------------------------------- worldMinX = -400 -- logical units worldMaxX = 400 -- logical units worldMinY = -300 -- logical units worldMaxY = 300 -- logical units worldMinXY = vector2 worldMinX worldMinY worldMaxXY = vector2 worldMaxX worldMaxY worldWidth = worldMaxX - worldMinX -- logical units worldHeight = worldMaxY - worldMinY -- logical units worldFarZ = 1 worldNearZ = 0 ------------------------------------------------------------------------------- type Scalar = Double type Vector = Vector2 Scalar type UnitVector = Vector type Velocity = Vector type Position = Vector type Speed = Scalar type Radius = Scalar type Length = Scalar type Degrees = Scalar type Radians = Scalar type RGBA = GL.Color4 Scalar orthogonalOn v = let (x,y) = vector2XY v in vector2 (-y) x lerp s lo hi = lo ^+^ s*^(hi^-^lo) prop v lo hi = (v-lo) / (hi-lo) scale sx sy v = vector2 (vx*sx) (vy*sy) where (vx,vy) = vector2XY v translate tx ty v = vector2 tx ty ^+^ v ------------------------------------------------------------------------------- -- IL = list of items that have a unique identifier type IK = Int data IL a = IL { ilNextKey :: !IK , ilItems :: [(IK,a)] } deriving Show deleteFirst :: (a -> Bool) -> [a] -> [a] deleteFirst p (x:xs) = if p x then xs else x:deleteFirst p xs deleteFirst _ [] = [] ilEmpty = IL 0 [] ilFromList = ilAppend ilEmpty ilInsert (IL key items) value = IL (key+1) ((key,value):items) ilRemove (IL key items) id = IL key $ deleteFirst ( (/=id) . fst) items ilAppend il (v:vs) = (il `ilInsert` v) `ilAppend` vs ilAppend il [] = il ilMap f (IL key items) = let (keys, values) = unzip items in IL key $ zip keys (map f values) ilValues (IL key items) = let (keys, values) = unzip items in values instance Functor IL where fmap = ilMap ------------------------------------------------------------------------------- -- The elements in our tiny simulation data RigidObject = Particle { objPosition :: !Position , objVelocity :: !Velocity , objRadius :: !Scalar , objColor :: !RGBA } | Wall -- The wall is actually a (hyper)plane, which in the 2D case is just a line, given as point and normal... { objPosition :: !Position , objNormal :: !UnitVector , objColor :: !RGBA } deriving (Show, Eq) wall p d c = Wall p (d ^/ norm d) c ------------------------------------------------------------------------------- -- Convert OpenGL screen position to a world position screenToWorldPosition :: GL.Position -> Vector screenToWorldPosition (GL.Position x y) = let (nx,ny) = (fromIntegral x, fromIntegral (windowHeight - y - 1)) (wx,wy) = (lerp (prop nx 0 $ fromIntegral windowWidth) worldMinX worldMaxX, lerp (prop ny 0 $ fromIntegral windowHeight) worldMinY worldMaxY) in vector2 wx wy {- screenToWorldPosition :: GL.Position -> IO Vector screenToWorldPosition pos@(GL.Position x y) = do viewport@(_,GL.Size _ height) <- get GL.viewport putStrLn $ show height modelviewMatrix <- getMatrix $ GL.Modelview 0 projectionMatrix <- getMatrix $ GL.Projection let pos3 = GL.Vertex3 (fromIntegral x) (fromIntegral (height-y)) 0 GL.Vertex3 vx vy _ <- GLU.unProject pos3 modelviewMatrix projectionMatrix viewport return $ vector2 vx vy where getMatrix :: GL.MatrixMode -> IO(GL.GLmatrix Scalar) getMatrix mode = get $ GL.matrix $ Just mode -} ------------------------------------------------------------------------------- data Sensors = Sensors { deltaTime :: !DTime, mouseWorldPosition :: !Vector } deriving Show sense :: IORef Time -> IO Sensors sense timeRef = do GLFW.pollEvents lastTime <- readIORef timeRef thisTime <- get GLFW.time writeIORef timeRef thisTime smp <- get GLFW.mousePos let wmp = screenToWorldPosition smp -- wmp <- screenToWorldPosition smp return $ Sensors { deltaTime = min (thisTime-lastTime) 0.05 , mouseWorldPosition = wmp } sinterleave action = unsafeInterleaveIO $ do a <- action as <- sinterleave action return (a:as) makeSensors = do time <- get GLFW.time timeRef <- newIORef time sinterleave (sense timeRef) ------------------------------------------------------------------------------- -- Quick and dirty frames per second counter type FPS = IORef(Scalar,Int) initFPS :: IO FPS initFPS = do t0 <- get GLFW.time newIORef (t0,0) updateFPS :: FPS -> IO() updateFPS fps = do (t1,c1) <- readIORef fps t2 <- get GLFW.time let c2 = c1+1 dt = t2-t1 if dt >= 1 then do let text = show (round $ 1000000*dt / fromIntegral c2) ++ " microsec per frame = " ++ show (round $ fromIntegral c2 / dt) ++ " frames per sec" GLFW.windowTitle $= text --print text writeIORef fps (t2,0) else writeIORef fps (t1,c2) ------------------------------------------------------------------------------- initGfx = do GLFW.initialize GLFW.disableSpecial GLFW.AutoPollEvent GLFW.openWindow (GL.Size windowWidth windowHeight) [GLFW.DisplayDepthBits windowDepth, GLFW.DisplayAlphaBits 8] GLFW.Window -- Set to 0 to render and swap as fast as possible -- Set to 1 to synchronize rendering with screen's vertical refresh rate GLFW.swapInterval $= 1 GLFW.windowTitle $= "Peter Verswyvelen's Elastic Collision Nightmare - move mouse left/right to slowdown/speedup" GLFW.windowSizeCallback $= onWindowResized GL.hint GL.PolygonSmooth $= GL.Nicest GL.hint GL.LineSmooth $= GL.Nicest GL.blend $= GL.Enabled GL.blendFunc $= (GL.SrcAlpha, GL.OneMinusSrcAlpha) GL.shadeModel $= GL.Smooth GL.lineSmooth $= GL.Enabled GL.polygonSmooth $= GL.Enabled GL.pointSmooth $= GL.Enabled GL.pointSize $= 5.0 GL.lineWidth $= 1.5 GL.clearColor $= GL.Color4 0 0 0 1 GL.clearDepth $= worldFarZ GL.depthRange $= (worldNearZ, worldFarZ) GL.depthFunc $= Just GL.Lequal GL.matrixMode $= GL.Projection GL.loadIdentity GL.ortho worldMinX worldMaxX worldMinY worldMaxY worldNearZ worldFarZ GL.matrixMode $= GL.Modelview 0 GL.loadIdentity where onWindowResized windowSize@(GL.Size windowWidth windowHeight) = do let ww = fromIntegral windowWidth wh = fromIntegral windowHeight cw = worldWidth ch = worldHeight sw = wh * cw / ch sh = ww * ch / cw if sw < ww then GL.viewport $= (GL.Position (round $ (ww-sw)/2) 0, GL.Size (round sw) (round wh)) else GL.viewport $= (GL.Position 0 (round $ (wh-sh)/2), GL.Size (round ww) (round sh)) termGfx = do GLFW.terminate ------------------------------------------------------------------------------- exited :: IO Bool exited = do windowOpenStatus <- get $ GLFW.windowParam GLFW.Opened return $ windowOpenStatus == 0 ------------------------------------------------------------------------------- run actions = do loop actions where loop (action:actions) = do viewport@(_,GL.Size _ height) <- get GL.viewport action done <- exited unless done (loop actions) ------------------------------------------------------------------------------- unitCircle :: Int -> [Vector] unitCircle numPoints = [vector2 (cos rads) (sin rads) | i <- [0..numPoints], let rads = fromIntegral i * 2 * pi / fromIntegral numPoints ] vector2vertex v = GL.Vertex2 (vector2X v) (vector2Y v) foreach = flip mapM_ renderObject (Particle p v r c) = do GL.color c GL.preservingMatrix $ do GL.translate $ GL.Vector3 (vector2X p) (vector2Y p) 0 GL.scale r r r GL.renderPrimitive GL.LineLoop $ mapM_ (GL.vertex . vector2vertex) (unitCircle 36) renderObject (Wall p d c) = let (dx,dy) = vector2XY d in do renderRay p (vector2 (-dy) dx) c renderRay p (vector2 dy (-dx)) c renderLine p (p ^+^ 10 *^ d) c renderOverlay (Particle p v r c) = do renderLine p (p ^+^ r *^ normalize v) c renderOverlay _ = return () renderLine :: Vector -> Vector -> RGBA -> IO() renderLine p1 p2 color = do GL.lineWidth $= 3 GL.color color GL.renderPrimitive GL.Lines $ do GL.vertex $ GL.Vertex2 x1 y1 GL.vertex $ GL.Vertex2 x2 y2 where x1 = vector2X p1 y1 = vector2Y p1 x2 = vector2X p2 y2 = vector2Y p2 renderRay :: Position -> Vector -> RGBA -> IO() renderRay p d color = do GL.lineWidth $= 2 GL.color color GL.renderPrimitive GL.Lines $ do GL.vertex $ GL.Vertex4 x1 y1 0 1 GL.vertex $ GL.Vertex4 x2 y2 0 0 where x1 = vector2X p y1 = vector2Y p x2 = vector2X d y2 = vector2Y d renderObjects :: [RigidObject] -> IO () renderObjects obs = do GL.clearColor $= GL.Color4 0 0 0 0 GL.clear [GL.ColorBuffer] -- REALLY SLOW foreach obs renderObject --foreach obs renderOverlay GLFW.swapBuffers ------------------------------------------------------------------------------- unorderedPairs [] = [] unorderedPairs (p:ps) = pairs p ps ++ unorderedPairs ps where pairs p (q:qs) = (p, q):pairs p qs pairs p [] = [] solveQuadratic a b c = let d = b*b - 4*a*c in if d<0 then [] else [((-b)+sqrt(d))/(2*a), ((-b)-sqrt(d))/(2*a)] maybeCollisionAtTime (Particle p1 v1 r1 c1) (Particle p2 v2 r2 c2) dt = let r = r1+r2 rr = r*r dp = p1^-^p2 dv = v1^-^v2 dvdv = dv `dot` dv dvdp = dv `dot` dp dpdp = dp `dot` dp ss = solveQuadratic dvdv (2*dvdp) (dpdp-rr) in if dvdp>=0 then -- Particles are not moving towards eachother Nothing else -- Compute times of future impacts, if any case filter (>=0) $ filter ( Nothing ts -> Just (minimum ts) maybeCollisionAtTime (Wall _ _ _) (Wall _ _ _) dt = Nothing maybeCollisionAtTime p@(Particle _ _ _ _) w@(Wall _ _ _) dt = maybeCollisionAtTime w p dt maybeCollisionAtTime (Wall wp wd _) (Particle pp pv pr _) dt = let n = ((wp ^+^ pr *^ wd) ^-^pp) `dot` wd d = pv `dot` wd epsilon = 1e-9 in if d > 0 || (abs d) < epsilon then Nothing else let t = n / d in if t >= 0 && t <= dt then Just t else Nothing type CollisionPair = ( (IK,Velocity), (IK,Velocity) ) -- A single collision at some time data Collision = COL { colTime :: Time , colPair :: CollisionPair } deriving Show -- Multiple collisions at some time data Collisions = COLS { colsTime :: Time , colsPairs :: [CollisionPair] } deriving Show type IdRigidObject = (IK,RigidObject) collisionsUntil :: Time -> [IdRigidObject] -> (Time, [Collision]) collisionsUntil dt keyObjPairs = (dt,cols) where cols = map fromJust $ filter isJust $ map aux $ unorderedPairs keyObjPairs where aux ((_,Wall _ _ _ ), (_, Wall _ _ _)) = Nothing aux (ko1@(_,Particle _ _ _ _), ko2@(_, Wall _ _ _)) = aux (ko2,ko1) aux ((key1, obj1@(Wall wp wd _)), (key2, obj2@(Particle pp pv pr _))) = fmap impulse $ maybeCollisionAtTime obj1 obj2 dt where impulse t = COL t ((key1,zeroVector),(key2,vel2)) where vn = (pv `dot` wd) *^ wd vt = pv ^-^ vn vel2 = vt ^-^ vn aux ((key1,obj1@(Particle p1 v1 _ _)),(key2,obj2@(Particle p2 v2 _ _))) = fmap impulse $ maybeCollisionAtTime obj1 obj2 dt where impulse t = COL t ((key1,vel1),(key2,vel2)) where v = v2 ^-^ v1 d = p2 ^-^ p1 dd = d `dot` d vn = ((v `dot` d) / dd) *^ d vt = v ^-^ vn vel1 = vn ^+^ v1 vel2 = vt ^+^ v1 earliestCollisions :: (Time,[Collision]) -> Collisions earliestCollisions (dt,cols) = aux cols where aux [] = COLS dt [] aux cs = let ct = minimum $ map colTime cs in COLS ct (map colPair $ filter ((==ct) . colTime) cs) {- COLLISION TEST -} red::RGBA = GL.Color4 1 0 0 1 green::RGBA = GL.Color4 0 1 0 1 blue::RGBA = GL.Color4 0 0 1 1 p1 = Particle (vector2 (-100) 0) (vector2 50 0) 20 red p2 = Particle (vector2 100 0) (vector2 (-50) 0) 20 red p3 = Particle (vector2 0 100) (vector2 0 (-50)) 20 red p4 = Particle (vector2 300 0) (vector2 0 50) 20 red ps = ilFromList [p1,p2,p3,p4] -- Should be COLS {colsTime = 1.434314575050762, colsPairs = [(2,1),(2,0)]} cols = earliestCollisions $ collisionsUntil 100 (ilItems ps) glfwError = (unsafePerformIO GLFW.terminate) `seq` error ------------------------------------------------------------------------------- momentum ros = sum [ v `dot` v | obj <- ilValues ros, let v = velocity obj ] where velocity (Particle p v _ _) = v velocity _ = zeroVector collisionPairsOf k = aux where aux (((k1,v1),(k2,v2)):kvs) | k1==k = ((k1,v1),(k2,v2)):aux kvs | k2==k = ((k2,v2),(k1,v1)):aux kvs | otherwise = aux kvs aux [] = [] simulation :: IL RigidObject -> [DTime] -> [IL RigidObject] simulation ros (dt:dts) = ros2:simulation ros2 dts where ros2 = aux dt ros epsilon = 1e-9 aux dt ros1@(IL key os1) | dt <= epsilon = ros1 | otherwise = aux (dt-ct) ros2 where os1 = ilItems ros1 COLS ct pairs = earliestCollisions $ collisionsUntil dt os1 ros2 = IL key os2 os2 = map (impulse pairs . integrate) os1 where integrate (k, Particle p v r c) = (k, Particle (p ^+^ (ct *^ v)) v r c) integrate s = s -- Assume static object when not a particle impulse pvs kp@(k, Particle p _ r c) = aux (collisionPairsOf k pvs) where aux [((_,v),_)] = (k, Particle p v r c) aux kvs@(kv1:kv2:_) = glfwError $ "Unsupported"++show kvs aux _ = kp impulse _ kp = kp ------------------------------------------------------------------------------- removeOverlappingObjects :: [RigidObject] -> [RigidObject] removeOverlappingObjects rs = aux rs [] where -- If a head object overlaps with any of the tail objects, remove it -- Otherwise add it to the list of non-overlapping objects, and continue checking the remaining objects aux (r:rs) os = if any (overlap r) rs then aux rs os else aux rs (r:os) aux [] os = os overlap (Particle p1 _ r1 _) (Particle p2 _ r2 _) = let dp = p1 ^-^ p2 sr = (r1+r2) in dot dp dp < sr*sr overlap _ _ = False -- TODO ------------------------------------------------------------------------------- -- A list of center point of each rectangle in a n by m grid grid n m = let w = worldWidth/n h = worldHeight/m in [vector2 (worldMinX+w*(i+0.5)) (worldMinY+h*(j+0.5)) | i <- [0..n-1], j <- [0..m-1]] ------------------------------------------------------------------------------- -- HSV to RGB colorspace converter -- h<-[0..360) s<-[0..1] v<-[0..1] hsvToRgb (h,s,v) | s==0 = (0,0,0) | otherwise = let hs = h / 60 i = fromIntegral $ floor hs f = hs - i; p = v*(1-s) q = v*(1-(s*f)) t = v*(1-(s*(1-f))) in case i of 0 -> (v, t, p) 1 -> (q, v, p) 2 -> (p, v, t) 3 -> (p, q, v) 4 -> (t, p, v) _ -> (v, p, q) hsvToGlColor4 hsv = let (r,g,b) = hsvToRgb hsv in GL.Color4 r g b 1 ------------------------------------------------------------------------------- randomParticles ps = do let stdgen = read "1780242961 1"::StdGen --stdgen <- getStdGen let pss = [ Particle p (vector2Polar s a) r (GL.Color4 cr cg cb 1) | (p, (ra,rs,rr,rh)) <- zip ps $ tuples $ randoms stdgen , let a = lerp ra (-pi) pi , let s = lerp rs 50 1000 , let r = lerp rr 10 30 , let (cr,cg,cb) = hsvToRgb (lerp rh 0 359,1,1) ] let nops = removeOverlappingObjects pss return nops where tuples (r1:r2:r3:r4:rs) = (r1,r2,r3,r4):tuples rs ------------------------------------------------------------------------------- printStream s = forkIO $ mapM_ (putStrLn . show) s wallSegment p1 p2 color = wall ((p1^+^p2)^/2) (orthogonalOn (p2^-^p1)) color polyWalls = let numCorners = 11 wallCorners = cycle $ map toCorner (unitCircle numCorners) wallColor = GL.Color4 0 1 0 1 toCorner = translate (-350) (-250) . scale 350 250 . translate 1 1 in take numCorners $ getZipList $ wallSegment <$> ZipList wallCorners <*> ZipList (drop 1 wallCorners) <*> ZipList (repeat wallColor) worldWalls = [wall (vector2 0 (worldMaxY*1)) (vector2 0 (-1) ) wc ,wall (vector2 0 (worldMinY*1)) (vector2 0 1) wc ,wall (vector2 (worldMaxX*1) 0) (vector2 (-1) 0) wc ,wall (vector2 (worldMinX*1) 0) (vector2 1 0) wc ,wall (vector2 (worldMinX+100) (worldMinY+100)) (vector2 1 1) wc] where wc = GL.Color4 0 1 0 1 main = do initGfx fps <- initFPS sensors <- makeSensors let dts = deltaTime <$> sensors let mp = mouseWorldPosition <$> sensors let sdts = getZipList $ ZipList ((\p -> let x = vector2X p in \dt -> min 0.02 (dt * exp (x*0.005))) <$> mp) <*> ZipList dts -- simultan causes 3 particles to collide simultaneously, -- but this is not supported, and the program will exit with a message. -- let objs = simultan -- pendulum makes 1 particle bounce with n touching particles. -- momentum is transfered to the particle at the other end. -- should be used with only worldWalls, no polyWalls. -- let objs = pendulum -- bouncers is a bunch of bouncing particles, moving randomly let objs = bouncers --let walls = worldWalls let walls = worldWalls ++ polyWalls run $ render fps <$> ilValues <$> (simulation (ilFromList $ walls ++ objs) sdts) termGfx where render fps ps = updateFPS fps >> renderObjects ps bouncers = unsafePerformIO $ randomParticles (grid 10 6) simultan = [ Particle (vector2 0 0) (vector2 0 50) 20 red , Particle (vector2 (-30) 100) (vector2 0 0) 20 green , Particle (vector2 30 100) (vector2 0 0) 20 green ] pendulum = let n = 4 r = 20 in Particle (vector2 ((n+1)*r*2) 0) (vector2 100 0) 20 red : [Particle (vector2 (i*2*r) 0) (vector2 0 0) 20 (hsvToGlColor4 (i*360/n,1,1)) | i <- [0..n-1]] dump x = trace (show x) x instance Forceable (Vertex2 Scalar) where force = id instance Forceable (Sensors) where force = id z = main