pituicat/src/Classes/Physics/Collidible.hs

268 lines
8.6 KiB
Haskell
Raw Normal View History

{-# LANGUAGE AllowAmbiguousTypes #-}
2021-01-19 20:46:07 +00:00
{-# LANGUAGE OverloadedStrings #-}
2020-12-27 04:19:51 +00:00
module Classes.Physics.Collidible where
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)
2021-01-20 06:13:04 +00:00
import Data.List (minimumBy)
-- internal imports
import Classes.Physics.Mass
2021-01-17 19:29:27 +00:00
data CollisionResult time direction
2021-01-17 01:30:45 +00:00
= NoCollision
2021-01-17 19:29:27 +00:00
| Collision time 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
-> ( V2 Double -- ^ Bottom left corner of AABB relative to position
, V2 Double -- ^ Top right corner of AABB relative to position
2020-12-27 04:19:51 +00:00
)
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-01-14 22:08:24 +00:00
p1 = position m1
p2 = position m2
(m1b1@(V2 m1b1x m1b1y), m1b2@(V2 m1b2x m1b2y)) = boundary m1
(m2b1@(V2 m2b1x m2b1y), m2b2@(V2 m2b2x m2b2y)) = boundary m2
m1p1 = m1b1
m1p2 = V2 m1b1x m1b2y
m1p3 = m1b2
m1p4 = V2 m1b2x m1b1y
m2p1 = m2b1
m2p2 = V2 m2b1x m2b2y
m2p3 = m2b2
m2p4 = V2 m2b2x m2b1y
quad1 = map (p1 +) [m1p1, m1p2, m1p3, m1p4]
2021-01-20 06:13:04 +00:00
quad2 = map (p2 +) [m2p1, m2p2, m2p3, m2p4]
g11 = (m1p1, m1p2)
g12 = (m1p2, m1p3)
g13 = (m1p3, m1p4)
g14 = (m1p4, m1p1)
g21 = (m2p1, m2p2)
g22 = (m2p2, m2p3)
g23 = (m2p3, m2p4)
g24 = (m2p4, m2p1)
g1s = map (\(s1, s2) -> (p1 + s1, p1 + s2)) [g11, g12, g13, g14]
g2s = map (\(s1, s2) -> (p2 + s1, p2 + s2)) [g21, g22, g23, g24]
t1 q@(V2 mqpx mqpy) (ps@(V2 mspx mspy), pt@(V2 mtpx mtpy)) =
2021-01-14 22:08:24 +00:00
if d1x == 0 && d1y == 0
then dt
else
2021-01-19 20:46:07 +00:00
let (V2 cx cy) = ps - q
(V2 rx ry) = pt - ps
in
(cx * (-ry) - (-rx) * cy) /
(d1x * (-ry) - (-rx) * d1y)
2021-01-20 06:13:04 +00:00
t2 q@(V2 mqpx mqpy) (ps@(V2 mspx mspy), pt@(V2 mtpx mtpy)) =
if d2x == 0 && d2y == 0
then dt
else
let (V2 cx cy) = ps - q
(V2 rx ry) = pt - ps
in
(cx * (-ry) - (-rx) * cy) /
(d2x * (-ry) - (-rx) * d2y)
s1 q@(V2 mqpx mqpy) (ps@(V2 mspx mspy), pt@(V2 mtpx mtpy)) =
2021-01-19 20:46:07 +00:00
let (V2 cx cy) = ps - q
(V2 rx ry) = pt - ps
in
(d1x * cy - cx * d1y) /
(d1x * (-ry) - (-rx) * d1y)
2021-01-20 06:13:04 +00:00
s2 q@(V2 mqpx mqpy) (ps@(V2 mspx mspy), pt@(V2 mtpx mtpy)) =
let (V2 cx cy) = ps - q
(V2 rx ry) = pt - ps
in
(d2x * cy - cx * d2y) /
(d2x * (-ry) - (-rx) * d2y)
inside1 m =
let dm = m - (p1 + m1p4)
dc = (p1 + m1p3) - (p1 + m1p4)
da = (p1 + m1p1) - (p1 + m1p4)
in
(0 < dm `dot` dc && dm `dot` dc < dc `dot` dc) &&
(0 < dm `dot` da && dm `dot` da < da `dot` da)
inside2 m =
2021-01-17 01:30:45 +00:00
let dm = m - (p2 + m2p4)
dc = (p2 + m2p3) - (p2 + m2p4)
da = (p2 + m2p1) - (p2 + m2p4)
2021-01-14 22:08:24 +00:00
in
2021-01-17 01:30:45 +00:00
(0 < dm `dot` dc && dm `dot` dc < dc `dot` dc) &&
(0 < dm `dot` da && dm `dot` da < da `dot` da)
in
2021-01-20 06:13:04 +00:00
if any inside1 quad2 || any inside2 quad1
then Collision 0
(if abs d1x > abs d1y
then V2 (round $ signum d1x) 0
else V2 0 (round $ signum d1y)
)
2021-01-17 01:30:45 +00:00
else
2021-01-20 06:13:04 +00:00
let res1 = foldl
(\acc1 gx ->
let gt = foldl
(\acc2 q ->
let qt = t1 q gx
qs = s1 q gx
in
if (qs > 0 && qs < 1) && (qt > 0 && qt < acc2)
then qt
else acc2
)
(fst acc1)
quad1
in
if gt < fst acc1
then
( gt
, if gx == (p2 + fst g21, p2 + snd g22) ||
gx == (p2 + fst g23, p2 + snd g23)
then V2 (round $ signum d1x) 0
else V2 0 (round $ signum d1y)
)
else acc1
)
(dt, V2 0 0)
g2s
res2 = foldl
2021-01-17 01:30:45 +00:00
(\acc1 gx ->
let gt = foldl
(\acc2 q ->
2021-01-20 06:13:04 +00:00
let qt = t2 q gx
qs = s2 q gx
2021-01-17 01:30:45 +00:00
in
if (qs > 0 && qs < 1) && (qt > 0 && qt < acc2)
then qt
else acc2
2021-01-17 19:29:27 +00:00
)
(fst acc1)
2021-01-17 01:30:45 +00:00
quad1
in
2021-01-17 19:29:27 +00:00
if gt < fst acc1
then
( gt
2021-01-20 06:13:04 +00:00
, if gx == (p1 + fst g11, p2 + snd g12) ||
gx == (p1 + fst g13, p2 + snd g13)
2021-01-17 19:57:45 +00:00
then V2 (round $ signum d1x) 0
else V2 0 (round $ signum d1y)
2021-01-17 19:29:27 +00:00
)
2021-01-17 01:30:45 +00:00
else acc1
)
2021-01-17 19:29:27 +00:00
(dt, V2 0 0)
2021-01-20 06:13:04 +00:00
g1s
res = minimumBy (\a b -> fst a `compare` fst b) [res1, res2]
2021-01-17 01:30:45 +00:00
in
2021-01-17 19:29:27 +00:00
if fst res < dt
2021-01-19 20:46:07 +00:00
then Collision `uncurry`
A.log Debug ("Collision Imminent!!! " <> fromString (show res)) res
2021-01-17 01:30: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-01-17 19:29:27 +00:00
=> c -- ^ Original object
-> other -- ^ Collision partner
-> CollisionResult Double (V2 Int) -- ^ Collision reaction
-> c -- ^ Updated original object
2021-01-12 02:12:02 +00:00
collide = elasticCollision 1
-- | Implementation of a dampened elastic collision used as default collision
-- implementation of the collision reaction
elasticCollision
:: (Collidible c1, Collidible c2)
=> Double
-> c1
-> c2
2021-01-17 19:29:27 +00:00
-> CollisionResult Double (V2 Int)
-> c1
2021-01-17 01:30:45 +00:00
elasticCollision _ mo1 _ NoCollision = mo1
elasticCollision damping mo1 mo2 (Collision ddt (V2 dirx diry)) =
2021-01-17 01:30:45 +00:00
let v1@(V2 v1x v1y) = velocity mo1
(V2 v2x v2y) = velocity mo2
2021-01-03 00:43:37 +00:00
p1@(V2 p1x p1y) = position mo1
p2 = position mo2
(V2 m1x1 m1y1, V2 m1x2 m1y2) = boundary mo1
(V2 m2x1 m2y1, V2 m2x2 m2y2) = boundary mo2
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-01-03 00:43:37 +00:00
(V2 dx dy) = p2 - p1
2021-01-14 22:08:24 +00:00
nvel@(V2 nvx nvy) = if m1 == recip 0
then V2 0 0
else (damping *) <$>
if m2 == recip 0
2021-01-14 22:08:24 +00:00
then
if abs dy < abs dx
2021-01-12 02:12:02 +00:00
then (V2 (-v1x) v1y)
else (V2 v1x (-v1y))
else (V2 v1x' v1y')
bx = if dx < 0
2021-01-17 01:30:45 +00:00
then
if abs dx > abs m1x1 + abs m2x2 &&
2021-01-17 01:30:45 +00:00
(if dy > 0
then abs dy > abs m1y2 + abs m2y1
else abs dy > abs m1y1 + abs m2y2
2021-01-17 01:30:45 +00:00
)
then 0
else (abs m1x1 + abs m2x2) - abs dx
2021-01-17 01:30:45 +00:00
else
if abs dx > abs m1x2 + abs m2x1 &&
2021-01-17 01:30:45 +00:00
(if dy > 0
then abs dy > abs m1y2 + abs m2y1
else abs dy > abs m1y1 + abs m2y2
2021-01-17 01:30:45 +00:00
)
then 0
else -((abs m1x2 + abs m2x1) - abs dx)
by = if dy < 0
2021-01-17 19:29:27 +00:00
then
if abs dy > abs m1y1 + abs m2y2 &&
2021-01-17 01:30:45 +00:00
(if dx > 0
then abs dx > abs m1x2 + abs m2x1
else abs dx > abs m1x1 + abs m2x2
2021-01-17 01:30:45 +00:00
)
then 0
else (abs m1y1 + abs m2y2) - abs dy
2021-01-17 01:30:45 +00:00
else
if abs dy > abs m1y2 + abs m2y1 &&
2021-01-17 01:30:45 +00:00
(if dx > 0
then abs dx > abs m1x2 + abs m2x1
else abs dx > abs m1x1 + abs m2x2
2021-01-17 01:30:45 +00:00
)
then 0
else -((abs m1y2 + abs m2y1) - abs dy)
2021-01-14 22:08:24 +00:00
in
2021-01-17 01:30:45 +00:00
(velocityUpdater
((positionUpdater mo1)
(p1 +
if ddt == 0
then
2021-01-20 06:13:04 +00:00
if abs bx < abs by
2021-01-17 01:30:45 +00:00
then
V2 bx 0
else
V2 0 by
else
V2 0 0
)
)
) nvel