trying attracotr physics

assets/spheres/bigsphere.mtl

@@ -0,0 +1,10 @@
+# Blender MTL File: 'bigsphere.blend'
+# Material Count: 1
+
+newmtl None
+Ns 0
+Ka 0.000000 0.000000 0.000000
+Kd 0.8 0.8 0.8
+Ks 0.8 0.8 0.8
+d 1
+illum 2

assets/spheres/smallsphere.mtl

@@ -0,0 +1,10 @@
+# Blender MTL File: 'smallsphere.blend'
+# Material Count: 1
+
+newmtl None
+Ns 0
+Ka 0.000000 0.000000 0.000000
+Kd 0.8 0.8 0.8
+Ks 0.8 0.8 0.8
+d 1
+illum 2

assets/spheres/smallsphere.obj

@@ -0,0 +1,58 @@
+# Blender v2.79 (sub 0) OBJ File: 'smallsphere.blend'
+# www.blender.org
+mtllib smallsphere.mtl
+o Icosphere
+v 0.000000 -1.000000 0.000000
+v 0.723600 -0.447215 0.525720
+v -0.276385 -0.447215 0.850640
+v -0.894425 -0.447215 0.000000
+v -0.276385 -0.447215 -0.850640
+v 0.723600 -0.447215 -0.525720
+v 0.276385 0.447215 0.850640
+v -0.723600 0.447215 0.525720
+v -0.723600 0.447215 -0.525720
+v 0.276385 0.447215 -0.850640
+v 0.894425 0.447215 0.000000
+v 0.000000 1.000000 0.000000
+vn 0.1876 -0.7947 0.5774
+vn 0.6071 -0.7947 0.0000
+vn -0.4911 -0.7947 0.3568
+vn -0.4911 -0.7947 -0.3568
+vn 0.1876 -0.7947 -0.5774
+vn 0.9822 -0.1876 0.0000
+vn 0.3035 -0.1876 0.9342
+vn -0.7946 -0.1876 0.5774
+vn -0.7946 -0.1876 -0.5774
+vn 0.3035 -0.1876 -0.9342
+vn 0.7946 0.1876 0.5774
+vn -0.3035 0.1876 0.9342
+vn -0.9822 0.1876 0.0000
+vn -0.3035 0.1876 -0.9342
+vn 0.7946 0.1876 -0.5774
+vn 0.4911 0.7947 0.3568
+vn -0.1876 0.7947 0.5774
+vn -0.6071 0.7947 0.0000
+vn -0.1876 0.7947 -0.5774
+vn 0.4911 0.7947 -0.3568
+usemtl None
+s off
+f 1//1 2//1 3//1
+f 2//2 1//2 6//2
+f 1//3 3//3 4//3
+f 1//4 4//4 5//4
+f 1//5 5//5 6//5
+f 2//6 6//6 11//6
+f 3//7 2//7 7//7
+f 4//8 3//8 8//8
+f 5//9 4//9 9//9
+f 6//10 5//10 10//10
+f 2//11 11//11 7//11
+f 3//12 7//12 8//12
+f 4//13 8//13 9//13
+f 5//14 9//14 10//14
+f 6//15 10//15 11//15
+f 7//16 11//16 12//16
+f 8//17 7//17 12//17
+f 9//18 8//18 12//18
+f 10//19 9//19 12//19
+f 11//20 10//20 12//20

examples/example02/Init.hs

@@ -0,0 +1,157 @@
+{-# LANGUAGE OverloadedStrings #-}
+
+module Init where
+
+import SDL (($=))
+import qualified SDL
+
+import qualified Graphics.Rendering.OpenGL as GL
+import qualified Graphics.GLUtil as GLU
+
+import qualified Data.ByteString as BS
+
+import System.Random (randomRIO)
+
+import Physics.Bullet.Raw
+
+import Codec.Wavefront
+
+import Linear as L
+
+import SpatialMath
+
+import Foreign
+
+import Util
+import Types
+
+genVertBufObject path = do
+  eobj <- fromFile path
+  let obj = case eobj of
+        Right o -> o
+        Left err -> error err
+  let lobj = loadObj obj
+
+  shipBO <- GL.genObjectName
+  GL.bindVertexArrayObject $= Just shipBO
+
+  verts <- GL.genObjectName
+  GL.bindBuffer GL.ArrayBuffer $= Just verts
+  withArray (loTriangles lobj) $ \ptr ->
+    GL.bufferData GL.ArrayBuffer $=
+      ( fromIntegral $ length (loTriangles lobj) * 3 * sizeOf (0 :: Double)
+      , ptr
+      , GL.StaticDraw
+      )
+  GL.vertexAttribPointer (GL.AttribLocation 0) $=
+    ( GL.ToFloat
+    , GL.VertexArrayDescriptor 4 GL.Float 0 (plusPtr nullPtr 0)
+    )
+  GL.vertexAttribArray (GL.AttribLocation 0) $= GL.Enabled
+  return (shipBO, length (loTriangles lobj))
+
+load :: IO StateData
+load = do
+  _ <- SDL.setMouseLocationMode SDL.RelativeLocation
+  GL.depthFunc $= Just GL.Less
+
+  (shipBO, stl) <- genVertBufObject "assets/spheres/smallsphere.obj"
+
+  (planetBO, ptl) <- genVertBufObject "assets/spheres/bigsphere.obj"
+
+  let vertexShader = foldl BS.append BS.empty
+        [ "attribute vec3 coord3d;"
+        , "attribute vec2 texcoord;"
+        , "uniform mat4 mvp;"
+        , "varying vec2 f_texcoord;"
+        , "void main(void) {"
+        , "  gl_Position = mvp * vec4(coord3d, 1.0);"
+        , "  f_texcoord = texcoord;"
+        , "}"
+        ]
+      fragmentShader = foldl BS.append BS.empty
+        [ "varying vec2 f_texcoord;"
+        , "void main(void) {"
+        , "  gl_FragColor = vec4(1,1,1,0.5);"
+        , "}"
+        ]
+  p <- GLU.simpleShaderProgramBS vertexShader fragmentShader
+
+  poss <- mapM (\_ -> do
+    x <- randomRIO (-50, 50)
+    y <- randomRIO (-50, 50)
+    z <- randomRIO (-50, 50)
+    return (V3 x y z)
+    ) [0..2000]
+
+  let shipList = zipWith (Ship shipBO stl)
+        poss
+        (repeat $ Quaternion 1 (V3 0 0 0))
+      planet = Ship planetBO ptl (V3 0 0 0) (Quaternion 1 (V3 0 0 0))
+
+  phys <- initPhysics
+
+  po <- initPhysicsObjects poss
+
+  mapM_ (addRigidBody (pWorld phys) . bodyRigidBody) (poSmallBalls po)
+  addRigidBody (pWorld phys) (bodyRigidBody $ poBigBall po)
+
+  return StateData
+    { ships = shipList
+    , planet = planet
+    , proj = perspective (pi/2) (1600 / 900) 1 (-1)
+    , camera = Camera
+      { cameraFocus = V3 0 0 0
+      , cameraRot = Euler 0 0 0
+      , cameraDist = -50
+      }
+    , program = p
+    , physics = phys
+    , physicsObjects = po
+    }
+
+loadTex :: FilePath -> IO GL.TextureObject
+loadTex f = do
+  t <- either error id <$> GLU.readTexture f
+  GL.textureFilter GL.Texture2D $= ((GL.Linear', Nothing), GL.Linear')
+  GLU.texture2DWrap $= (GL.Repeated, GL.ClampToEdge)
+  return t
+
+initPhysics :: IO Physics
+initPhysics = do
+  bp <- newDbvtBroadphase
+  config <- newDefaultCollisionConfiguration
+  disp <- newCollisionDispatcher config
+  solver <- newSequentialImpulseConstraintSolver
+  world <- newDiscreteDynamicsWorld disp bp solver config
+  setGravity world (V3 0 (-10) 0)
+  return $ Physics bp config disp solver world
+
+initPhysicsObjects :: [V3 Float] -> IO PhysicsObjects
+initPhysicsObjects poss = do
+  -- ground <- newStaticPlaneShape (V3 0 1 0) 1
+  smallBall <- newSphereShape 1
+  bigBall <- newSphereShape 5
+
+  -- groundMotionState <- newDefaultMotionState (Quaternion 1 (V3 0 0 0)) (V3 0 (-51) 0)
+  -- groundBody <- newRigidBody 0 groundMotionState 0.9 0.5 ground (V3 0 0 0)
+
+  smallBallPOs <- mapM (\pos -> do
+    smallBallMotionState <- newDefaultMotionState (Quaternion 1 (V3 0 0 0))
+      (fmap realToFrac pos)
+    localInertia <- calculateLocalInertia smallBall 1 (V3 0 0 0)
+    smallBallBody <- newRigidBody 1 smallBallMotionState 0.9 0.5 smallBall localInertia
+    return $ PhysBody smallBall smallBallMotionState smallBallBody 1
+    ) poss
+
+  bigBallPO <- do
+    bigBallMotionState <- newDefaultMotionState (Quaternion 1 (V3 0 0 0))
+      (V3 0 0 0)
+    localInertia <- calculateLocalInertia bigBall 1 (V3 0 0 0)
+    bigBallBody <- newRigidBody 0 bigBallMotionState 0.9 0.5 bigBall localInertia
+    return $ PhysBody bigBall bigBallMotionState bigBallBody 0
+
+  return PhysicsObjects
+    { poBigBall = bigBallPO
+    , poSmallBalls  = smallBallPOs
+    }

examples/example02/Main.hs

@@ -0,0 +1,195 @@
+{-# LANGUAGE OverloadedStrings, RecordWildCards #-}
+
+module Main where
+
+import Affection
+
+import SDL (($=))
+import qualified SDL
+
+import qualified Graphics.Rendering.OpenGL as GL
+import qualified Graphics.GLUtil as GLU
+
+import Physics.Bullet.Raw
+
+import Control.Monad (when)
+import Control.Monad.IO.Class (liftIO)
+
+import Linear as L
+
+import System.Random (randomRIO)
+
+import SpatialMath
+
+import Init
+import Types
+
+import Debug.Trace as T
+
+main :: IO ()
+main =
+  withAffection AffectionConfig
+    { initComponents = All
+    , windowTitle    = "hw - example 02"
+    , windowConfig   = SDL.defaultWindow
+      { SDL.windowInitialSize = SDL.V2 1600 900
+      , SDL.windowOpenGL = Just SDL.defaultOpenGL
+        { SDL.glProfile = SDL.Core SDL.Normal 3 2
+        }
+      }
+    , initScreenMode = SDL.Fullscreen
+    , preLoop = return ()
+    , eventLoop = handle
+    , updateLoop = update
+    , drawLoop = draw
+    , loadState = load
+    , cleanUp = const (return ())
+    , canvasSize = Nothing
+    }
+
+update :: Double -> Affection StateData ()
+update dt = do
+  sd <- getAffection
+  let phys = physics sd
+      physos = physicsObjects sd
+  -- mapM_ (\smallBall -> do
+  --   ms1 <- liftIO $ getMotionState (bodyRigidBody smallBall)
+  --   ms2 <- liftIO $ getMotionState (bodyRigidBody $ poBigBall physos)
+  --   r1 <- liftIO $ return . fmap realToFrac =<< getPosition ms1
+  --   r2 <- liftIO $ return . fmap realToFrac =<< getPosition ms2
+  --   let g = 0.0000000000667300
+  --       m1 = bodyMass smallBall
+  --       m2 = bodyMass (poBigBall physos)
+  --       eta_sq = 0.1 ^ 2
+  --       force = (g * m1 * m2 *^ (r1 - r2)) ^/
+  --         (((r1 - r2) `dot` (r1 - r2)) + eta_sq)
+  --   liftIO $ applyCentralForce (bodyRigidBody smallBall) force
+  --   ) (poSmallBalls physos)
+  liftIO $ stepSimulation (pWorld phys) dt 10 Nothing
+  posrots <- mapM ((\ball -> do
+    ms <- liftIO $ getMotionState ball
+    npos <- liftIO $ return . fmap realToFrac =<< getPosition ms
+    nrot <- liftIO $ return . fmap realToFrac =<< getRotation ms
+    return (npos, nrot))
+    . bodyRigidBody) (poSmallBalls physos)
+  let nships = map (\(ship, (pos, rot)) ->
+        ship
+          { shipRot = rot
+          , shipPos = pos
+          }
+        ) (zip (ships sd) posrots)
+  putAffection sd
+    { ships = nships
+    }
+
+draw :: Affection StateData ()
+draw = do
+  GL.viewport $= (GL.Position 0 0, GL.Size 1600 900)
+  StateData{..} <- getAffection
+  GL.currentProgram $= (Just . GLU.program $ program)
+  mapM_ (\Ship{..} -> do
+    let view = lookAt
+          (cameraFocus camera +
+            rotVecByEulerB2A
+            (cameraRot camera)
+            (V3 0 0 (-cameraDist camera)))
+          (cameraFocus camera)
+          (V3 0 1 0)
+        model = mkTransformation shipRot shipPos
+        pvm = proj !*! view !*! model
+    liftIO $ GLU.setUniform program "mvp" pvm
+    GL.bindVertexArrayObject $= Just shipVao
+    liftIO $ GL.drawArrays GL.Triangles 0 (fromIntegral shipVaoLen)
+    ) (planet : ships)
+
+handle :: SDL.EventPayload -> Affection StateData ()
+handle (SDL.WindowClosedEvent _) = quit
+
+handle (SDL.KeyboardEvent dat) = do
+  let key = SDL.keysymKeycode (SDL.keyboardEventKeysym dat)
+  when (SDL.keyboardEventKeyMotion dat == SDL.Pressed) $
+    handleKey key
+handle (SDL.MouseMotionEvent dat) = do
+  sd <- getAffection
+  let (V2 rx ry) = fromIntegral <$> SDL.mouseMotionEventRelMotion dat
+      c = camera sd
+  putAffection sd
+    { camera =
+      case SDL.mouseMotionEventState dat of
+        [SDL.ButtonRight] ->
+          let (V3 sx sy sz) = rotVecByEuler (cameraRot c) (V3 (rx / 10) 0 (ry / 10))
+          in  c {cameraFocus = cameraFocus c + V3 sx 0 sy}
+        [] ->
+          let dphi = pi / 4 / 45 / 10
+              (Euler yaw pitch roll) = cameraRot c
+              nangle
+                | nangle' >= qc = qc - mu
+                | nangle' <= -qc = -qc + mu
+                | otherwise     = nangle'
+                where
+                  nangle' = (dphi * ry) + roll
+                  qc = pi / 2
+                  mu = 0.01
+              nrot =
+                Euler
+                  yaw
+                  (pitch + (rx * dphi))
+                  nangle
+          in  c
+            { cameraRot = nrot
+            }
+        _ ->
+          c
+    }
+
+handle _ = return ()
+
+handleKey :: SDL.Keycode -> Affection StateData ()
+handleKey code
+  | code == SDL.KeycodeR =
+    GL.clearColor $= GL.Color4 1 0 0 1
+  | code == SDL.KeycodeG =
+    GL.clearColor $= GL.Color4 0 1 0 1
+  | code == SDL.KeycodeB =
+    GL.clearColor $= GL.Color4 0 0 1 1
+  | code == SDL.KeycodeP = do
+    r <- liftIO $ randomRIO (0, 1)
+    g <- liftIO $ randomRIO (0, 1)
+    b <- liftIO $ randomRIO (0, 1)
+    a <- liftIO $ randomRIO (0, 1)
+    GL.clearColor $= GL.Color4 r g b a
+  | code == SDL.KeycodeEscape =
+    quit
+  | code == SDL.KeycodeF = do
+    dt <- deltaTime <$> get
+    liftIO $ putStrLn $ show (1 / dt) ++ " FPS"
+  | code == SDL.KeycodeT =
+    toggleScreen
+  | code `elem`
+    [ SDL.KeycodeW
+    , SDL.KeycodeS
+    , SDL.KeycodeA
+    , SDL.KeycodeD
+    , SDL.KeycodeQ
+    , SDL.KeycodeE
+    ]
+    = do
+      sd <- getAffection
+      let ship = head (ships sd)
+          rot = shipRot ship
+          dphi = pi / 2 / 45
+          nquat = case code of
+            SDL.KeycodeW -> rot * axisAngle (V3 1 0 0) (-dphi)
+            SDL.KeycodeS -> rot * axisAngle (V3 1 0 0) dphi
+            SDL.KeycodeA -> rot * axisAngle (V3 0 1 0) (-dphi)
+            SDL.KeycodeD -> rot * axisAngle (V3 0 1 0) dphi
+            SDL.KeycodeE -> rot * axisAngle (V3 0 0 1) (-dphi)
+            SDL.KeycodeQ -> rot * axisAngle (V3 0 0 1) dphi
+            _            -> rot
+      putAffection sd
+        { ships = ship
+          { shipRot = nquat
+          } : tail (ships sd)
+        }
+  | otherwise =
+    return ()

examples/example02/Types.hs

@@ -0,0 +1,53 @@
+module Types where
+
+import qualified Graphics.Rendering.OpenGL as GL
+import qualified Graphics.GLUtil as GLU
+
+import Linear as L
+
+import SpatialMath
+
+import Physics.Bullet.Raw as Bullet
+
+data StateData = StateData
+  { ships :: [Ship]
+  , planet :: Ship
+  , camera :: Camera
+  , proj :: M44 Float
+  , program :: GLU.ShaderProgram
+  , physics :: Physics
+  , physicsObjects :: PhysicsObjects
+  }
+
+data Ship = Ship
+  { shipVao    :: GL.VertexArrayObject
+  , shipVaoLen :: Int
+  , shipPos    :: V3 Float
+  , shipRot    :: Quaternion Float
+  }
+
+data Camera = Camera
+  { cameraFocus :: V3 Float
+  , cameraRot :: Euler Float
+  , cameraDist :: Float
+  }
+
+data Physics = Physics
+  { pBroadphase :: DbvtBroadphase
+  , pConfig     :: DefaultCollisionConfiguration
+  , pDispatcher :: CollisionDispatcher
+  , pSolver     :: SequentialImpulseConstraintSolver
+  , pWorld      :: DiscreteDynamicsWorld
+  }
+
+data PhysicsObjects = PhysicsObjects
+  { poBigBall :: PhysBody SphereShape
+  , poSmallBalls  :: [PhysBody SphereShape]
+  }
+
+data PhysBody a = PhysBody
+  { bodyShape       :: a
+  , bodyMotionState :: MotionState
+  , bodyRigidBody   :: RigidBody
+  , bodyMass        :: Double
+  } deriving (Eq)

examples/example02/Util.hs

@@ -0,0 +1,67 @@
+module Util where
+
+import Codec.Wavefront
+
+import Control.Monad (sequence)
+
+import qualified Data.Vector as V
+
+data LoadedObject = LoadedObject
+  { loTriangles :: [Float]
+  , loLines     :: [Float]
+  , loPoints    :: [Float]
+  , loTexTri    :: Maybe [Float]
+  }
+
+loadObj :: WavefrontOBJ -> LoadedObject
+loadObj obj =
+  LoadedObject ts ls ps tritex
+  where
+    inter = objLocations obj
+    interTex = objTexCoords obj
+    faces = map elValue (V.toList $ objFaces obj)
+    lns = map elValue (V.toList $ objLines obj)
+    points = map elValue (V.toList $ objPoints obj)
+    deface (Face a b c []) =
+      map (\i -> inter V.! (faceLocIndex i -1)) [a, b, c]
+    deface _ =
+      error "loadObj: obj with quads encountered"
+    deline (Line a b) =
+      map (\i -> inter V.! (lineLocIndex i -1)) [a, b]
+    depoint (Point i) = inter V.! (i - 1)
+    tsLocs = concatMap deface faces
+    lsLocs = concatMap deline lns
+    psLocs = map depoint points
+    deLoc (Location x y z w) = [x, y, z, w]
+    deTex (TexCoord r s _) = [r, s]
+    ts = concatMap deLoc tsLocs
+    ls = concatMap deLoc lsLocs
+    ps = concatMap deLoc psLocs
+    defaceTex :: Face -> Maybe [TexCoord]
+    defaceTex (Face a b c []) =
+      mapM
+      (fmap (\x -> interTex V.! (x - 1)) . faceTexCoordIndex)
+      [a, b, c]
+    defaceTex _ =
+      error "loadObj: obj with quads encountered"
+    tritex :: Maybe [Float]
+    tritex = concatMap deTex <$> mftxs
+    mftxs :: Maybe [TexCoord]
+    mftxs = fmap concat (mapM defaceTex faces)
+
+-- objLocsToPtr :: WavefrontOBJ -> IO (Ptr Float, Int)
+-- objLocsToPtr obj = do
+--   let ivs = objLocations obj
+--       faces = map elValue $ V.toList $ objFaces obj
+--       vs = concatMap
+--         (\(Face a b c []) ->
+--           map (\i -> ivs V.! ((faceLocIndex i) - 1)) [a, b, c])
+--         faces
+--   ptr <- newArray $ concatMap (\(Location x y z w) -> [x, y, z, w]) vs
+--   return (ptr, length vs)
+-- 
+-- objUVsToPtr :: WavefrontOBJ -> IO (Ptr Float, Int)
+-- objUVsToPtr obj = do
+--   let uvs= V.toList $ objTexCoords obj
+--   ptr <- newArray $ concatMap (\(TexCoord r s t) -> [r, s, t]) uvs
+--   return (ptr, length uvs)