pituicat/src/Physics/Classes/Collidible.hs

224 lines
8.8 KiB
Haskell
Raw Normal View History

{-# LANGUAGE AllowAmbiguousTypes #-}
2021-01-19 20:46:07 +00:00
{-# LANGUAGE OverloadedStrings #-}
module Physics.Classes.Collidible where
2020-12-27 04:19:51 +00:00
2021-01-19 20:46:07 +00:00
import Affection as A
2020-12-27 04:19:51 +00:00
import Linear
2021-01-19 20:46:07 +00:00
import Data.String (fromString)
-- internal imports
import Physics.Classes.Mass
2021-01-17 19:29:27 +00:00
data CollisionResult time direction
2021-01-17 01:30:45 +00:00
= NoCollision
2021-02-27 15:08:45 +00:00
| Collision
{ collisionTime :: time
, collisionDirection :: direction
}
2021-01-17 01:30:45 +00:00
deriving (Show, Eq)
2020-12-27 04:19:51 +00:00
-- | Typeclass for implementing collision results on objects.
class (Show c, Mass c) => Collidible c where
2020-12-27 04:19:51 +00:00
-- | returns the bottom left and top right corners relative to the objects
-- positional vector of the axis aligned bounding box (AABB) serving here
-- as collision boundaries.
2020-12-27 04:19:51 +00:00
boundary
:: c -- ^ Object
2021-07-09 17:49:09 +00:00
-> ( V2 Double
, V2 Double
) -- ^ Bottom left and top right corner of AABB relative to position
collisionCheck
:: (Collidible other)
2021-01-17 19:29:27 +00:00
=> Double -- ^ Time step length
-> c -- ^ First object
-> other -- ^ second object
-> CollisionResult Double (V2 Int) -- ^ Do the objects collide?
collisionCheck dt m1 m2 =
2021-01-19 20:46:07 +00:00
let d1@(V2 d1x d1y) = velocity m1
d2@(V2 d2x d2y) = velocity m2
2021-07-25 08:11:28 +00:00
p1@(V2 p1x p1y) = position m1
p2@(V2 p2x p2y) = position m2
2021-01-14 22:08:24 +00:00
(m1b1@(V2 m1b1x m1b1y), m1b2@(V2 m1b2x m1b2y)) = boundary m1
(m2b1@(V2 m2b1x m2b1y), m2b2@(V2 m2b2x m2b2y)) = boundary m2
2021-07-25 08:11:28 +00:00
(V2 pm1b1x pm1b1y, V2 pm1b2x pm1b2y) = (p1 +) <$> boundary m1
(V2 pm2b1x pm2b1y, V2 pm2b2x pm2b2y) = (p2 +) <$> boundary m2
2021-02-27 15:08:45 +00:00
m1p1 = p1 + m1b1
m1p2 = p1 + V2 m1b1x m1b2y
m1p3 = p1 + m1b2
2021-07-25 08:11:28 +00:00
m1p4@(V2 m1p4x _) = p1 + V2 m1b2x m1b1y
m2p1@(V2 m2p1x _) = p2 + m2b1
2021-02-27 15:08:45 +00:00
m2p2 = p2 + V2 m2b1x m2b2y
m2p3 = p2 + m2b2
m2p4 = p2 + V2 m2b2x m2b1y
2021-04-17 02:53:21 +00:00
(V2 b1minx b1miny, V2 b1maxx b1maxy) =
2021-02-27 15:08:45 +00:00
( V2
((\(V2 x _) -> x) (if d1x < 0 then m1p1 + ((dt *) <$> d1) else m1p1))
((\(V2 _ y) -> y) (if d1y < 0 then m1p1 + ((dt *) <$> d1) else m1p1))
, V2
((\(V2 x _) -> x) (if d1x < 0 then m1p3 else m1p3 + ((dt *) <$> d1)))
((\(V2 _ y) -> y) (if d1y < 0 then m1p3 else m1p3 + ((dt *) <$> d1)))
)
2021-04-17 02:53:21 +00:00
(V2 b2minx b2miny, V2 b2maxx b2maxy) =
2021-02-27 15:08:45 +00:00
( V2
((\(V2 x _) -> x) (if d2x < 0 then m2p1 + ((dt *) <$> d2) else m2p1))
((\(V2 _ y) -> y) (if d2y < 0 then m2p1 + ((dt *) <$> d2) else m2p1))
, V2
((\(V2 x _) -> x) (if d2x < 0 then m2p3 else m2p3 + ((dt *) <$> d2)))
((\(V2 _ y) -> y) (if d2y < 0 then m2p3 else m2p3 + ((dt *) <$> d2)))
)
broadphaseOverlap =
2021-04-17 02:53:21 +00:00
let in2 =
2021-03-04 00:56:42 +00:00
(b1minx > b2minx && b1minx < b2maxx &&
b1miny > b2miny && b1miny < b2maxy) ||
(b1maxx > b2minx && b1maxx < b2maxx &&
b1miny > b2miny && b1miny < b2maxy) ||
(b1minx > b2minx && b1minx < b2maxx &&
b1maxy > b2miny && b1maxy < b2maxy) ||
(b1maxx > b2minx && b1maxx < b2maxx &&
b1maxy > b2miny && b1maxy < b2maxy)
2021-02-27 15:08:45 +00:00
in1 =
2021-03-04 00:56:42 +00:00
(b2minx > b1minx && b2minx < b1maxx &&
b2miny > b1miny && b2miny < b1maxy) ||
(b2maxx > b1minx && b2maxx < b1maxx &&
b2miny > b1miny && b2miny < b1maxy) ||
(b2minx > b1minx && b2minx < b1maxx &&
b2maxy > b1miny && b2maxy < b1maxy) ||
(b2maxx > b1minx && b2maxx < b1maxx &&
b2maxy > b1miny && b2maxy < b1maxy)
2021-01-20 06:13:04 +00:00
in
2021-02-27 15:08:45 +00:00
in2 || in1
overlap =
let in1 =
2021-07-25 08:11:28 +00:00
(pm1b1x > pm2b1x && pm1b1x < pm2b2x &&
pm1b1y > pm2b1y && pm1b1y < pm2b2y) ||
(pm1b2x > pm2b1x && pm1b2x < pm2b2x &&
pm1b1y > pm2b1y && pm1b1y < pm2b2y) ||
(pm1b1x > pm2b1x && pm1b2x < pm2b2x &&
pm1b2y > pm2b1y && pm1b2y < pm2b2y) ||
(pm1b2x > pm2b1x && pm1b2x < pm2b2x &&
pm1b2y > pm2b1y && pm1b2y < pm2b2y)
in2 =
2021-07-25 08:11:28 +00:00
(pm2b1x > pm1b1x && pm2b1x < pm1b2x &&
pm2b1y > pm1b1y && pm2b1y < pm1b2y) ||
(pm2b2x > pm1b1x && pm2b2x < pm1b2x &&
pm2b1y > pm1b1y && pm2b1y < pm1b2y) ||
(pm2b1x > pm1b1x && pm2b2x < pm1b2x &&
pm2b2y > pm1b1y && pm2b2y < pm1b2y) ||
(pm2b2x > pm1b1x && pm2b2x < pm1b2x &&
pm2b2y > pm1b1y && pm2b2y < pm1b2y)
in
in2 || in1
2021-02-27 15:08:45 +00:00
tx =
2021-04-16 16:46:01 +00:00
let p1x = (\(V2 x _) -> x) (if d1x < 0 then m1p1 else m1p4)
p2x = (\(V2 x _) -> x) (if d1x < 0 then m2p4 else m2p1)
2021-01-20 06:13:04 +00:00
in
2021-03-04 00:56:42 +00:00
if d2x - d1x == 0 then dt else (p1x - p2x) / (d2x - d1x)
2021-02-27 15:08:45 +00:00
ty =
2021-04-16 16:46:01 +00:00
let p1y = (\(V2 _ y) -> y) (if d1y < 0 then m1p1 else m1p2)
p2y = (\(V2 _ y) -> y) (if d1y < 0 then m2p2 else m2p1)
2021-01-14 22:08:24 +00:00
in
2021-03-04 00:56:42 +00:00
if d2y - d1y == 0 then dt else (p1y - p2y) / (d2y - d1y)
in
2021-03-04 00:56:42 +00:00
if broadphaseOverlap
2021-02-27 15:08:45 +00:00
then
2021-07-25 08:11:28 +00:00
let coll xdir =
let (p11, p12, p21, p22)
| xdir =
if d1x < 0
then
(m1p1, m1p2, m2p4, m2p3)
else
(m1p4, m1p3, m2p1, m2p2)
| otherwise =
if d1y < 0
then
(m1p1, m1p4, m2p2, m2p3)
else
(m1p2, m1p3, m2p1, m2p4)
vselector (V2 x y) = if xdir then y else x
tick = if xdir then tx else ty
g1s = vselector $ p11 + ((tick *) <$> d1)
g1e = vselector $ p12 + ((tick *) <$> d1)
g2s = vselector $ p21 + ((tick *) <$> d2)
g2e = vselector $ p22 + ((tick *) <$> d2)
s11 = (g1s - g2s) / (g2e - g2s)
s12 = (g1e - g2s) / (g2e - g2s)
s21 = (g2s - g1s) / (g1e - g1s)
s22 = (g2e - g1s) / (g1e - g1s)
in
any (\x -> x > 0 && x < 1) [s11, s12, s21 ,s22]
res = case (tx < dt, ty < dt, tx < ty, coll True, coll False) of
(True, _, True, True, _) ->
Collision tx (V2 (floor $ signum d1x) 0)
(_, True, False, _, True) ->
Collision ty (V2 0 (floor $ signum d1y))
(True, _, False, True, False) ->
Collision tx (V2 (floor $ signum d1x) 0)
(_, True, True, False, True) ->
Collision ty (V2 0 (floor $ signum d1y))
(_, _, _, False, False) ->
NoCollision
(_, _, True, _, _) ->
A.log A.Debug "CORNER CASE!" NoCollision
(False, False, _, _, _) ->
NoCollision
x -> error $
"Unhandled combination of collision check results: "
<> fromString (show x)
in
case res of
Collision _ _ ->
res
NoCollision ->
if overlap
then Collision 0 (V2 0 0)
else NoCollision
2021-02-27 15:08:45 +00:00
else
NoCollision
-- | This Function is called for every collision on both colliding objects.
2020-12-27 04:19:51 +00:00
collide
:: (Collidible other)
2021-07-24 07:35:41 +00:00
=> c -- ^ Original object
-> [(other, CollisionResult Double (V2 Int))] -- ^ Collision partners and results
-> Double -- ^ Timestep length
-> c -- ^ Updated original object
collide coll1 collrs = elasticCollision 0.9 coll1 (head collrs)
-- | Implementation of a dampened elastic collision used as default collision
-- implementation of the collision reaction
elasticCollision
:: (Collidible c1, Collidible c2)
=> Double
-> c1
2021-07-24 07:35:41 +00:00
-> (c2, CollisionResult Double (V2 Int))
-> Double -- ^ Timestep length
-> c1
2021-07-24 07:35:41 +00:00
elasticCollision _ mo1 (_, NoCollision) _ = mo1
2021-07-25 08:11:28 +00:00
elasticCollision damping mo1 (mo2, Collision ddt (V2 dirx diry)) dt =
2021-01-17 01:30:45 +00:00
let v1@(V2 v1x v1y) = velocity mo1
(V2 v2x v2y) = velocity mo2
2021-04-17 02:53:21 +00:00
p1 = position mo1
m1 = mass mo1
m2 = mass mo2
v1x' = 2 * (m1 * v1x + m2 * v2x) / (m1 + m2) - v1x
v1y' = 2 * (m1 * v1y + m2 * v2y) / (m1 + m2) - v1y
2021-04-17 02:53:21 +00:00
nvel = if m1 == recip 0
then V2 0 0
else (damping *) <$>
if m2 == recip 0
2021-01-14 22:08:24 +00:00
then
if dirx /= 0
2021-07-09 17:49:09 +00:00
then V2 (-v1x) v1y
else V2 v1x (-v1y)
else V2 v1x' v1y'
2021-01-14 22:08:24 +00:00
in
2021-07-09 17:49:09 +00:00
velocityUpdater
2021-07-24 07:35:41 +00:00
mo1
nvel