Skip to content
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
14 changes: 7 additions & 7 deletions src/GJK/Collision.hs
Original file line number Diff line number Diff line change
Expand Up @@ -27,7 +27,7 @@ the case where you are writing your own support functions.

collision 10 (poly1, polySupport) (poly2, polySupport) == Just True
-}
collision :: Int -> Mink a -> Mink b -> Maybe Bool
collision :: (Ord n, Fractional n) => Int -> Mink n a -> Mink n b -> Maybe Bool
collision limit minkA minkB =
let
d1 = (1.0, -1.0)
Expand All @@ -40,7 +40,7 @@ collision limit minkA minkB =
_ -> Nothing

-- | Prepare the input for doSimplex and start the calculation.
collision2 :: Int -> Mink a -> Mink b -> (Double, Double) -> (Double, Double) -> Maybe Bool
collision2 :: (Ord n, Fractional n) => Int -> Mink n a -> Mink n b -> (n, n) -> (n, n) -> Maybe Bool
collision2 limit minkA minkB c b =
let
-- simplex is cb and direction is (cb x c0 x cb)
Expand Down Expand Up @@ -68,7 +68,7 @@ to speak) a is obtained by finding the direction of the origin from the
currently building simplex, and finding the extremal point on the boundry in
that direction.
-}
doSimplex :: Int -> Int -> Mink a -> Mink b -> ([Pt], Pt) -> Maybe (Bool, ([Pt], Pt))
doSimplex :: (Ord n, Fractional n) => Int -> Int -> Mink n a -> Mink n b -> ([Pt n], Pt n) -> Maybe (Bool, ([Pt n], Pt n))
doSimplex limit depth minkA minkB (sim, d) =
let
maybea = calcMinkSupport minkA minkB d
Expand All @@ -79,7 +79,7 @@ doSimplex limit depth minkA minkB (sim, d) =

-- | Actual doSimplex calculation. doSimplex handles the error case of
-- calcMinkSupport and feeds a real a to doSimplex2.
doSimplex2 :: Int -> Int -> Mink a -> Mink b -> ([Pt], Pt) -> (Double, Double) -> Maybe (Bool, ([Pt], Pt))
doSimplex2 :: (Ord n, Fractional n) => Int -> Int -> Mink n a -> Mink n b -> ([Pt n], Pt n) -> (n, n) -> Maybe (Bool, ([Pt n], Pt n))
doSimplex2 limit depth minkA minkB (sim, d) a =
let
notPastOrig = (dot a d < 0)
Expand All @@ -102,7 +102,7 @@ doSimplex2 limit depth minkA minkB (sim, d) a =
- a new search direction based on the simplex's relation to the origin, in the case
- where there is no containment.
-}
enclosesOrigin :: Pt -> [Pt] -> (Bool, ([Pt], Pt))
enclosesOrigin :: (Ord n, Fractional n) => Pt n -> [Pt n] -> (Bool, ([Pt n], Pt n))
enclosesOrigin a sim =
case sim of
-- 0-simplex case
Expand All @@ -113,7 +113,7 @@ enclosesOrigin a sim =

-- | Simplex is a single point, we will be adding a to the simplex one way or
-- another (the new simplex will be either [a,b] or [a])
handle0Simplex :: Pt -> Pt -> (Bool, ([Pt], Pt))
handle0Simplex :: (Ord n, Fractional n) => Pt n -> Pt n -> (Bool, ([Pt n], Pt n))
handle0Simplex a b =
let
-- line given by adding our new point
Expand All @@ -127,7 +127,7 @@ handle0Simplex a b =
-- | Simplex is a line segment [b,c], adding 'a' gives us a 2-Simplex. We now
-- either enclose the origin, or will be replacing the simplex with the closest
-- sub-component to the origin.
handle1Simplex :: Pt -> Pt -> Pt -> (Bool, ([Pt], Pt))
handle1Simplex :: (Ord n, Fractional n) => Pt n -> Pt n -> Pt n -> (Bool, ([Pt n], Pt n))
handle1Simplex a b c =
let
-- a is our new local point of reference
Expand Down
4 changes: 2 additions & 2 deletions src/GJK/Mink.hs
Original file line number Diff line number Diff line change
Expand Up @@ -9,10 +9,10 @@ module GJK.Mink
import GJK.Point (Pt, sub, neg)

-- | Simple alias for boundary objects bundled with a support function
type Mink a = (a, a -> Pt -> Maybe Pt)
type Mink n a = (a, a -> Pt n -> Maybe (Pt n))

-- | Calculate the support of the Minkowski difference of two Mink's
calcMinkSupport :: Mink a -> Mink b -> (Double, Double) -> Maybe (Double, Double)
calcMinkSupport :: (Ord n, Fractional n) => Mink n a -> Mink n b -> (n, n) -> Maybe (n, n)
calcMinkSupport (objA, suppA) (objB, suppB) d =
let
maybep1 = suppA objA (neg d)
Expand Down
26 changes: 13 additions & 13 deletions src/GJK/Point.hs
Original file line number Diff line number Diff line change
Expand Up @@ -17,38 +17,38 @@ module GJK.Point
) where

-- | Simple alias for a two dimentional point
type Pt = (Double, Double)
type Pt n = (n, n)

-- | Dot product of given points
dot :: Pt -> Pt -> Double
dot :: (Num n) => Pt n -> Pt n -> n
dot (x1,y1) (x2,y2) = (x1*x2) + (y1*y2)

-- | Subtract a from b
sub :: Pt -> Pt -> Pt
sub :: (Eq n, Num n, Fractional n) => Pt n -> Pt n -> Pt n
sub (ax, ay) (bx,by) = (bx-ax, by-ay)

-- | Alias for sub - reads better in some cases - as in "from a to b"
from :: Pt -> Pt -> Pt
from = sub
from :: (Eq n, Fractional n) => Pt n -> Pt n -> Pt n
from = sub

-- | Add a and b
add :: Pt -> Pt -> Pt
add :: (Num n) => Pt n -> Pt n -> Pt n
add (ax, ay) (bx,by) = (bx+ax, by+ay)

-- | Multiply x and y with n
mul :: Double -> Pt -> Pt
mul :: (Num n) => n -> Pt n -> Pt n
mul n (x,y) = (n*x, n*y)

-- | Negation
neg :: Pt -> Pt
neg :: (Num n) => Pt n -> Pt n
neg (x,y) = (-x,-y)

-- | 2D cross product
cross2D :: Pt -> Pt -> Double
cross2D :: (Num n) => Pt n -> Pt n -> n
cross2D (ax, ay) (bx, by) = ax*by - ay*bx

-- | Implements a x (b x c) = b(a.c) - c(a.b)
trip :: Pt -> Pt -> Pt -> Pt
trip :: (Eq n, Fractional n) => Pt n -> Pt n -> Pt n -> Pt n
trip a b c =
let
bac = mul (dot a c) b
Expand All @@ -57,17 +57,17 @@ trip a b c =
sub cab bac

-- | Perpendicular to a in direction of b (2D case)
perp :: Pt -> Pt -> Pt
perp :: (Eq n, Fractional n) => Pt n -> Pt n -> Pt n
perp a b = trip a b a

-- | Check if the dot product of a and by is greater than 0 and the direction
-- is same.
isSameDirection :: Pt -> Pt -> Bool
isSameDirection :: (Ord n, Num n) => Pt n -> Pt n -> Bool
isSameDirection a b = dot a b > 0

-- | Get the direction vector of given points.
-- Pass bc as first parameter.
getDirectionVector :: Pt -> Pt -> Pt
getDirectionVector :: (Eq n, Fractional n) => Pt n -> Pt n -> Pt n
getDirectionVector (x1, y1) (x2, y2) =
let
-- try the triple cross product
Expand Down
4 changes: 2 additions & 2 deletions src/GJK/Support.hs
Original file line number Diff line number Diff line change
Expand Up @@ -11,7 +11,7 @@ import GJK.Collision (collision)
import Data.Maybe (fromMaybe)

-- | Support function
polySupport :: [Pt] -> Pt -> Maybe Pt
polySupport :: (Ord n, Fractional n) => [Pt n] -> Pt n -> Maybe (Pt n)
polySupport list d =
let
dotList = fmap (dot d) list
Expand All @@ -28,6 +28,6 @@ safeMaximum [] = Nothing
safeMaximum list = Just $ maximum list

-- | Check if a and b collide
isCollision :: [Pt] -> [Pt] -> Bool
isCollision :: (Ord n, Fractional n) => [Pt n] -> [Pt n] -> Bool
isCollision a b =
fromMaybe False $ collision 1 (a, polySupport) (b, polySupport)