206 lines
6.4 KiB
Haskell
206 lines
6.4 KiB
Haskell
{-# LANGUAGE AllowAmbiguousTypes #-}
|
|
{-# LANGUAGE OverloadedStrings #-}
|
|
module Classes.Physics.Collidible where
|
|
|
|
import Affection as A
|
|
|
|
import Linear
|
|
|
|
import Data.String (fromString)
|
|
|
|
-- internal imports
|
|
|
|
import Classes.Physics.Mass
|
|
|
|
data CollisionResult time direction
|
|
= NoCollision
|
|
| Collision time direction
|
|
deriving (Show, Eq)
|
|
|
|
-- | Typeclass for implementing collision results on objects.
|
|
class (Show c, Mass c) => Collidible c where
|
|
|
|
-- | 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.
|
|
boundary
|
|
:: c -- ^ Object
|
|
-> ( V2 Double -- ^ Bottom left corner of AABB relative to position
|
|
, V2 Double -- ^ Top right corner of AABB relative to position
|
|
)
|
|
|
|
collisionCheck
|
|
:: (Collidible other)
|
|
=> Double -- ^ Time step length
|
|
-> c -- ^ First object
|
|
-> other -- ^ second object
|
|
-> CollisionResult Double (V2 Int) -- ^ Do the objects collide?
|
|
collisionCheck dt m1 m2 =
|
|
let d1@(V2 d1x d1y) = velocity m1
|
|
d2@(V2 d2x d2y) = velocity m2
|
|
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]
|
|
g1 = (m2p1, m2p2)
|
|
g2 = (m2p2, m2p3)
|
|
g3 = (m2p3, m2p4)
|
|
g4 = (m2p4, m2p1)
|
|
gs = map (\(s1, s2) -> (p2 + s1, p2 + s2)) [g1, g2, g3, g4]
|
|
t q@(V2 mqpx mqpy) (ps@(V2 mspx mspy), pt@(V2 mtpx mtpy)) =
|
|
if d1x == 0 && d1y == 0
|
|
then dt
|
|
else
|
|
let (V2 cx cy) = ps - q
|
|
(V2 rx ry) = pt - ps
|
|
in
|
|
(cx * (-ry) - (-rx) * cy) /
|
|
(d1x * (-ry) - (-rx) * d1y)
|
|
s q@(V2 mqpx mqpy) (ps@(V2 mspx mspy), pt@(V2 mtpx mtpy)) =
|
|
let (V2 cx cy) = ps - q
|
|
(V2 rx ry) = pt - ps
|
|
in
|
|
(d1x * cy - cx * d1y) /
|
|
(d1x * (-ry) - (-rx) * d1y)
|
|
inside m =
|
|
let dm = m - (p2 + m2p4)
|
|
dc = (p2 + m2p3) - (p2 + m2p4)
|
|
da = (p2 + m2p1) - (p2 + m2p4)
|
|
in
|
|
(0 < dm `dot` dc && dm `dot` dc < dc `dot` dc) &&
|
|
(0 < dm `dot` da && dm `dot` da < da `dot` da)
|
|
in
|
|
if any inside quad1
|
|
then Collision 0 (V2 0 0)
|
|
else
|
|
let res = foldl
|
|
(\acc1 gx ->
|
|
let gt = foldl
|
|
(\acc2 q ->
|
|
let qt = t q gx
|
|
qs = s 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 g1, p2 + snd g2) ||
|
|
gx == (p2 + fst g3, p2 + snd g3)
|
|
then V2 (round $ signum d1x) 0
|
|
else V2 0 (round $ signum d1y)
|
|
)
|
|
else acc1
|
|
)
|
|
(dt, V2 0 0)
|
|
gs
|
|
in
|
|
if fst res < dt
|
|
then Collision `uncurry`
|
|
A.log Debug ("Collision Imminent!!! " <> fromString (show res)) res
|
|
else NoCollision
|
|
|
|
-- | This Function is called for every collision on both colliding objects.
|
|
collide
|
|
:: (Collidible other)
|
|
=> c -- ^ Original object
|
|
-> other -- ^ Collision partner
|
|
-> CollisionResult Double (V2 Int) -- ^ Collision reaction
|
|
-> c -- ^ Updated original object
|
|
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
|
|
-> CollisionResult Double (V2 Int)
|
|
-> c1
|
|
elasticCollision _ mo1 _ NoCollision = mo1
|
|
elasticCollision damping mo1 mo2 (Collision ddt _) =
|
|
let v1@(V2 v1x v1y) = velocity mo1
|
|
(V2 v2x v2y) = velocity mo2
|
|
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
|
|
(V2 dx dy) = p2 - p1
|
|
nvel@(V2 nvx nvy) = if m1 == recip 0
|
|
then V2 0 0
|
|
else (damping *) <$>
|
|
if m2 == recip 0
|
|
then
|
|
if abs dy < abs dx
|
|
then (V2 (-v1x) v1y)
|
|
else (V2 v1x (-v1y))
|
|
else (V2 v1x' v1y')
|
|
bx = if dx < 0
|
|
then
|
|
if abs dx > abs m1x1 + abs m2x2 &&
|
|
(if dy > 0
|
|
then abs dy > abs m1y2 + abs m2y1
|
|
else abs dy > abs m1y1 + abs m2y2
|
|
)
|
|
then 0
|
|
else (abs m1x1 + abs m2x2) - abs dx
|
|
else
|
|
if abs dx > abs m1x2 + abs m2x1 &&
|
|
(if dy > 0
|
|
then abs dy > abs m1y2 + abs m2y1
|
|
else abs dy > abs m1y1 + abs m2y2
|
|
)
|
|
then 0
|
|
else -((abs m1x2 + abs m2x1) - abs dx)
|
|
by = if dy < 0
|
|
then
|
|
if abs dy > abs m1y1 + abs m2y2 &&
|
|
(if dx > 0
|
|
then abs dx > abs m1x2 + abs m2x1
|
|
else abs dx > abs m1x1 + abs m2x2
|
|
)
|
|
then 0
|
|
else (abs m1y1 + abs m2y2) - abs dy
|
|
else
|
|
if abs dy > abs m1y2 + abs m2y1 &&
|
|
(if dx > 0
|
|
then abs dx > abs m1x2 + abs m2x1
|
|
else abs dx > abs m1x1 + abs m2x2
|
|
)
|
|
then 0
|
|
else -((abs m1y2 + abs m2y1) - abs dy)
|
|
in
|
|
(velocityUpdater
|
|
((positionUpdater mo1)
|
|
(p1 +
|
|
if ddt == 0
|
|
then
|
|
if abs by > abs bx
|
|
then
|
|
V2 bx 0
|
|
else
|
|
V2 0 by
|
|
else
|
|
V2 0 0
|
|
)
|
|
)
|
|
) nvel
|