add first version of collision detection algorithm

4 files changed, 78 insertions, 4 deletions

diff --git a/doc/intersection-line-circle.pdf b/doc/intersection-line-circle.pdf
new file mode 100644
index 0000000..eff28c0
--- /dev/null
+++ b/doc/intersection-line-circle.pdf
Binary files differdiff --git a/grav2ty.cabal b/grav2ty.cabal
index a8b45f3..5dbafd5 100644
--- a/grav2ty.cabal
+++ b/grav2ty.cabal
@@ -13,7 +13,9 @@ author:              sternenseemann
 maintainer:          git@lukasepple.de
 -- copyright:
 category:            Game
-extra-source-files:  CHANGELOG.md, README.md
+extra-source-files:  CHANGELOG.md
+                   , README.md
+                   , doc/intersection-line-circle.pdf
 
 library grav2ty-lib
   exposed-modules:     Grav2ty.Simulation
@@ -23,6 +25,7 @@ library grav2ty-lib
   build-depends:       base ^>=4.12.0.0
                      , containers ^>=0.6.0.1
                      , linear
+                     , lens
   hs-source-dirs:      lib
   default-language:    Haskell2010
 
diff --git a/lib/Grav2ty/Simulation.hs b/lib/Grav2ty/Simulation.hs
index 9ecee21..65ab83d 100644
--- a/lib/Grav2ty/Simulation.hs
+++ b/lib/Grav2ty/Simulation.hs
@@ -1,11 +1,13 @@
 module Grav2ty.Simulation where
 
+import Control.Lens
+import Linear.Matrix
 import Linear.Metric (norm, distance)
 import Linear.V2
 import Linear.Vector
 
 data Hitbox a
-  = HMultiple [Hitbox a]
+  = HCombined [Hitbox a]
   | HLine
   { lineStart  :: V2 a
   , lineEnd    :: V2 a
@@ -16,7 +18,7 @@ data Hitbox a
   } deriving (Eq, Show, Ord)
 
 shipHitbox :: Num a => Hitbox a
-shipHitbox = HMultiple
+shipHitbox = HCombined
   [ HLine (V2 (-10) (-5)) (V2 (-10) 5)
   , HLine (V2 (-10) (-5)) (V2 10 0)
   , HLine (V2 (-10) 5)    (V2 10 0)
@@ -25,6 +27,75 @@ shipHitbox = HMultiple
 centeredCircle :: Num a => a -> Hitbox a
 centeredCircle r = HCircle (V2 0 0) r
 
+-- | Based on <https://de.wikipedia.org/wiki/Cramersche_Regel>
+cramer2 :: (Eq a, Fractional a) => M22 a -> V2 a -> Maybe (a, a)
+cramer2 coeff res = if detA == 0
+                      then Nothing
+                      else Just (det22 a1 / detA, det22 a2 / detA)
+  where detA = det22 coeff
+        a1 = set (column _x) res coeff
+        a2 = set (column _y) res coeff
+
+inRange :: Ord a => (a, a) -> a -> Bool
+inRange (l, u) x = x >= l && x <= u
+
+-- | Determines wether two 'Hitbox'es collide, i. e. any individual
+--   'HCircle' or 'HLine' intersect.
+--
+--   * Intersection of two 'HCircle' hitboxes is achieved by substracting the
+--     sum of their radius from the (absolute) distance of their centers.
+--     If this number is 0 or negative, the circles touch or intersect.
+--   * Intersection of a 'HLine' and a 'HCircle' is calculated by solving the
+--     equation describing the intersection of the infinite line the 'HLine' is
+--     on and the 'HCircle'. After that we check, wether the possible
+--     intersection points are on the 'HLine'. More details on the equation
+--     and the origins of its derivation are found in my notes which can be
+--     found in @doc/intersection-line-circle.pdf@ of the source distribution of
+--     this package.
+--   * If 'HCombined' is involved, all possible intersections are recursively
+--     checked. This might be an area of future bugs.
+--
+--   Note that in calculation we rely on Laziness to prevent DivByZero
+--   Exceptions.
+collision :: (Ord a, Floating a) => Hitbox a -> Hitbox a -> Bool
+collision (HCircle l1 r1) (HCircle l2 r2) = distance l1 l2 - r1 - r2 <= 0
+collision (HLine start end) (HCircle (V2 cx cy) r) =
+  let (V2 dirX dirY) = end - start
+      (V2 ax ay) = start
+      a = dirX ^ 2 + dirY ^ 2
+      b = 2 * dirX * ax + 2 * dirY * ay - 2 * cx * dirX - 2 * cy * dirY
+      c = ax ^ 2 + ay ^ 2 - 2 * cx * ax - 2 * cy * ay + cx ^ 2 + cy ^ 2 - r ^ 2
+      discriminant = b ** 2 - 4 * a * c
+      solution m = (-b `m` (sqrt discriminant)) / (2 * a)
+      solutions = solution (+) :
+        (if discriminant > 0 then [solution (-)] else [])
+   in discriminant >= 0 -- there is a possible intersection
+   && a /= 0 -- HLine is proper line (i.e. has distinct start and end points)
+   && any (inRange (0, 1)) solutions
+   -- at least one solutions is actually on the line. the line is modeled as
+   -- p = start + q * dir where dir = end - start. therefore all points p with
+   -- 0 <= q <= 1 are on the line.
+collision a@(HCircle _ _) b@(HLine _ _) = collision b a
+-- TODO collision not registered if the lines are parallel, but touch each
+-- other (Nothing is returned).
+collision (HLine a1 b1) (HLine a2 b2) =
+  let (V2 xd1 yd1) = b1 - a1
+      (V2 xd2 yd2) = b2 - a2
+      (V2 xa1 ya1) = a1
+      (V2 xa2 ya2) = a2
+      coeff = V2 (V2 xd1 (-xd2)) (V2 yd1 (-yd2))
+      res = V2 (xa2 - xa1) (ya2 - ya1)
+      solutions = cramer2 coeff res
+   in case solutions of
+        Just (s, t) -> inRange (0, 1) s && inRange (0, 1) t
+        Nothing -> False
+collision (HCombined as) b = any (collision b) as
+collision a b@(HCombined _) = collision b a
+
+collisionWithWorld :: (Ord a, Floating a) => World a -> Object a -> Bool
+collisionWithWorld world obj =
+  any (collision (objectHitbox obj) . objectHitbox) world
+
 data Modifier
   = NoMod
   | LocalMod
diff --git a/src/Main.hs b/src/Main.hs
index 2bb9ded..1fe67f5 100644
--- a/src/Main.hs
+++ b/src/Main.hs
@@ -31,7 +31,7 @@ renderHitbox box =  Color white $
   case box of
     HCircle (V2 x' y') r -> translate x' y' $ Circle r
     HLine a b -> Line . map vectorToPoint $ [a, b]
-    HMultiple boxes -> Pictures $ map renderHitbox boxes
+    HCombined boxes -> Pictures $ map renderHitbox boxes
 
 renderObject :: Object Float -> Picture
 renderObject obj = translate x y . rot . renderHitbox . objectHitbox $ obj