From 67e4c9ca809972c57cce54f6e6e35caed6ea41a0 Mon Sep 17 00:00:00 2001
From: nek0 <nek0@nek0.eu>
Date: Wed, 20 Jan 2021 07:13:04 +0100
Subject: [PATCH] add reverse-checking of collision

---
 src/Classes/Physics/Collidible.hs | 89 +++++++++++++++++++++++++------
 1 file changed, 73 insertions(+), 16 deletions(-)

diff --git a/src/Classes/Physics/Collidible.hs b/src/Classes/Physics/Collidible.hs
index 8848b45..7706e32 100644
--- a/src/Classes/Physics/Collidible.hs
+++ b/src/Classes/Physics/Collidible.hs
@@ -8,6 +8,8 @@ import Linear
 
 import Data.String (fromString)
 
+import Data.List (minimumBy)
+
 -- internal imports
 
 import Classes.Physics.Mass
@@ -51,12 +53,18 @@ class (Show c, Mass c) => Collidible c where
         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)) =
+        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)) =
           if d1x == 0 && d1y == 0
           then dt
           else
@@ -65,13 +73,35 @@ class (Show c, Mass c) => Collidible c where
             in
               (cx * (-ry) - (-rx) * cy) /
               (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
               (V2 rx ry) = pt - ps
           in
             (d1x * cy - cx * 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)
               dc = (p2 + m2p3) - (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` da && dm `dot` da < da `dot` da)
     in
-      if any inside quad1
+      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)
           )
       else
-        let res = foldl
+        let res1 = foldl
               (\acc1 gx ->
                 let gt = foldl
                       (\acc2 q ->
-                        let qt = t q gx
-                            qs = s q gx
+                        let qt = t1 q gx
+                            qs = s1 q gx
                         in
                           if (qs > 0 && qs < 1) && (qt > 0 && qt < acc2)
                           then qt
@@ -103,15 +133,42 @@ class (Show c, Mass c) => Collidible c where
                   if gt < fst acc1
                   then
                     ( gt
-                    , if gx == (p2 + fst g1, p2 + snd g2) ||
-                        gx == (p2 + fst g3, p2 + snd g3)
+                    , 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)
-              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
           if fst res < dt
           then Collision `uncurry`
@@ -198,7 +255,7 @@ elasticCollision damping mo1 mo2 (Collision ddt (V2 dirx diry)) =
         (p1 +
           if ddt == 0
           then
-            if diry == 0
+            if abs bx < abs by
             then
               V2 bx 0
             else