add reverse-checking of collision

This commit is contained in:
nek0 2021-01-20 07:13:04 +01:00
parent 55db04c27f
commit 67e4c9ca80
1 changed files with 73 additions and 16 deletions

View File

@ -8,6 +8,8 @@ import Linear
import Data.String (fromString) import Data.String (fromString)
import Data.List (minimumBy)
-- internal imports -- internal imports
import Classes.Physics.Mass import Classes.Physics.Mass
@ -51,12 +53,18 @@ class (Show c, Mass c) => Collidible c where
m2p3 = m2b2 m2p3 = m2b2
m2p4 = V2 m2b2x m2b1y m2p4 = V2 m2b2x m2b1y
quad1 = map (p1 +) [m1p1, m1p2, m1p3, m1p4] quad1 = map (p1 +) [m1p1, m1p2, m1p3, m1p4]
g1 = (m2p1, m2p2) quad2 = map (p2 +) [m2p1, m2p2, m2p3, m2p4]
g2 = (m2p2, m2p3) g11 = (m1p1, m1p2)
g3 = (m2p3, m2p4) g12 = (m1p2, m1p3)
g4 = (m2p4, m2p1) g13 = (m1p3, m1p4)
gs = map (\(s1, s2) -> (p2 + s1, p2 + s2)) [g1, g2, g3, g4] g14 = (m1p4, m1p1)
t q@(V2 mqpx mqpy) (ps@(V2 mspx mspy), pt@(V2 mtpx mtpy)) = 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)) =
if d1x == 0 && d1y == 0 if d1x == 0 && d1y == 0
then dt then dt
else else
@ -65,13 +73,35 @@ class (Show c, Mass c) => Collidible c where
in in
(cx * (-ry) - (-rx) * cy) / (cx * (-ry) - (-rx) * cy) /
(d1x * (-ry) - (-rx) * d1y) (d1x * (-ry) - (-rx) * d1y)
s q@(V2 mqpx mqpy) (ps@(V2 mspx mspy), pt@(V2 mtpx mtpy)) = 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)) =
let (V2 cx cy) = ps - q let (V2 cx cy) = ps - q
(V2 rx ry) = pt - ps (V2 rx ry) = pt - ps
in in
(d1x * cy - cx * d1y) / (d1x * cy - cx * d1y) /
(d1x * (-ry) - (-rx) * d1y) (d1x * (-ry) - (-rx) * d1y)
inside m = 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 =
let dm = m - (p2 + m2p4) let dm = m - (p2 + m2p4)
dc = (p2 + m2p3) - (p2 + m2p4) dc = (p2 + m2p3) - (p2 + m2p4)
da = (p2 + m2p1) - (p2 + m2p4) da = (p2 + m2p1) - (p2 + m2p4)
@ -79,19 +109,19 @@ class (Show c, Mass c) => Collidible c where
(0 < dm `dot` dc && dm `dot` dc < dc `dot` dc) && (0 < dm `dot` dc && dm `dot` dc < dc `dot` dc) &&
(0 < dm `dot` da && dm `dot` da < da `dot` da) (0 < dm `dot` da && dm `dot` da < da `dot` da)
in in
if any inside quad1 if any inside1 quad2 || any inside2 quad1
then Collision 0 then Collision 0
(if abs d1x > abs d1y (if abs d1x > abs d1y
then V2 (round $ signum d1x) 0 then V2 (round $ signum d1x) 0
else V2 0 (round $ signum d1y) else V2 0 (round $ signum d1y)
) )
else else
let res = foldl let res1 = foldl
(\acc1 gx -> (\acc1 gx ->
let gt = foldl let gt = foldl
(\acc2 q -> (\acc2 q ->
let qt = t q gx let qt = t1 q gx
qs = s q gx qs = s1 q gx
in in
if (qs > 0 && qs < 1) && (qt > 0 && qt < acc2) if (qs > 0 && qs < 1) && (qt > 0 && qt < acc2)
then qt then qt
@ -103,15 +133,42 @@ class (Show c, Mass c) => Collidible c where
if gt < fst acc1 if gt < fst acc1
then then
( gt ( gt
, if gx == (p2 + fst g1, p2 + snd g2) || , if gx == (p2 + fst g21, p2 + snd g22) ||
gx == (p2 + fst g3, p2 + snd g3) gx == (p2 + fst g23, p2 + snd g23)
then V2 (round $ signum d1x) 0 then V2 (round $ signum d1x) 0
else V2 0 (round $ signum d1y) else V2 0 (round $ signum d1y)
) )
else acc1 else acc1
) )
(dt, V2 0 0) (dt, V2 0 0)
gs g2s
res2 = foldl
(\acc1 gx ->
let gt = foldl
(\acc2 q ->
let qt = t2 q gx
qs = s2 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 == (p1 + fst g11, p2 + snd g12) ||
gx == (p1 + fst g13, p2 + snd g13)
then V2 (round $ signum d1x) 0
else V2 0 (round $ signum d1y)
)
else acc1
)
(dt, V2 0 0)
g1s
res = minimumBy (\a b -> fst a `compare` fst b) [res1, res2]
in in
if fst res < dt if fst res < dt
then Collision `uncurry` then Collision `uncurry`
@ -198,7 +255,7 @@ elasticCollision damping mo1 mo2 (Collision ddt (V2 dirx diry)) =
(p1 + (p1 +
if ddt == 0 if ddt == 0
then then
if diry == 0 if abs bx < abs by
then then
V2 bx 0 V2 bx 0
else else