-- SG library -- Copyright (c) 2009, Neil Brown. -- All rights reserved. -- -- Redistribution and use in source and binary forms, with or without -- modification, are permitted provided that the following conditions are -- met: -- -- * Redistributions of source code must retain the above copyright -- notice, this list of conditions and the following disclaimer. -- * Redistributions in binary form must reproduce the above copyright -- notice, this list of conditions and the following disclaimer in the -- documentation and/or other materials provided with the distribution. -- * The author's name may not be used to endorse or promote products derived -- from this software without specific prior written permission. -- -- THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS -- IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, -- THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR -- PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR -- CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, -- EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, -- PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR -- PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF -- LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING -- NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS -- SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -- | This module has types and functions for dealing with collision detection on -- simple 2D shapes. module Data.SG.Shape (Shape'(..), moveShape, rotateShape, scaleShape, shapePoints, boundingBox, overlap, intersectLineShape) where import Control.Arrow import Data.List import Data.Maybe import Data.SG.Geometry import Data.SG.Geometry.TwoDim import Data.SG.Matrix import Data.SG.Vector -- | A type for simple 2D convex shapes. It is expected that you will define a -- synonym in your own application such as @type Shape = Shape' Double@, hence -- the funny name. data Shape' a = Rectangle {shapeCentre :: Point2' a, rectSize :: (a, a)} -- ^ A rectangle with a centre, and a width (distance from the centre -- to the left or right side of the rectangle) and a height (distance -- from the centre to the top or bottom side of the rectangle. So the -- rectangle with corners (1,1) and (3,2) is @Rectangle (Point2 (2,1.5)) -- (1, 0.5)@. Technically a rectangle is a polygon, of course, but a -- rectangle (which is axis-aligned) can be processed faster by most algorithms. | Circle {shapeCentre :: Point2' a, circSize :: a} -- ^ A circle with a centre and a radius. | Polygon {shapeCentre :: Point2' a, -- Points are offsets from centre (and join in loop): polyPoints :: [Rel2' a]} -- ^ A polygon with a centre, and a list of points. The points are relative -- vectors from the centre of the polygon, and are expected to be in clockwise -- order. For example, the triangle with corners (1,1) (3,3) and (3,1) -- could be @Polygon (Point2 (2.5, 1.5)) [Rel2 (-1.5,-0.5), Rel2 (0.5,1.5), -- Rel2 (-1.5, 1.5)]@. -- -- Note that whereabouts the centre is inside the polygon is up to you -- (it does not /have to be/ the geometric average of the points), but -- it should at least be inside the polygon, or else some algorithms will -- behave strangely with it. -- -- The list of points should have at least 3 points in it, or else some -- algorithms will behave strangely. -- -- If your points are not in clockwise order (with the X-Y axes being -- how they are in graphs, not on screens), funny things will happen with -- the collision detection. deriving (Show, Read, Eq, Ord) -- | Moves a shape by a given vector (by moving the centre). moveShape :: Num a => Rel2' a -> Shape' a -> Shape' a moveShape x s = s {shapeCentre = shapeCentre s `plusDir` x} -- | Given an angle in /radians/, rotates the shape by that angle in an anti-clockwise -- direction. A circle will remain untouched, a polygon will have its points rotated, -- and a rectangle will become a polygon and get rotated (even if you pass 0 as the angle). rotateShape :: forall a. Floating a => a -> Shape' a -> Shape' a rotateShape _ s@(Circle {}) = s rotateShape a s@(Rectangle c _) = rotateShape a (Polygon c $ polygonPoints s) rotateShape a (Polygon c ps) = Polygon c $ map (multMatrix mat) ps where mat :: Matrix22' a mat = rotateZaxis a -- | Scales the size of the shape (for all edges, from the centre) by the given -- factor. scaleShape :: Num a => a -> Shape' a -> Shape' a scaleShape a (Circle c r) = Circle c (r*a) scaleShape a (Rectangle c (w, h)) = Rectangle c (w*a, h*a) scaleShape a (Polygon c ps) = Polygon c $ map (scaleRel a) ps pts :: Num a => Point2' a -> (a, a) -> (Point2' a, Point2' a) pts (Point2 (x, y)) (adjX, adjY) = (Point2 (x - adjX, y - adjY), Point2 (x + adjX, y + adjY)) -- | Gives back the bounding box of a shape in terms of the minimum X-Y and -- the maximum X-Y corners of the bounding box. boundingBox :: (Num a, Ord a) => Shape' a -> (Point2' a, Point2' a) boundingBox (Circle c r) = pts c (r, r) boundingBox (Rectangle c (w, h)) = pts c (w, h) boundingBox (Polygon p ps) = (p `plusDir` foldl (fmapNum2 min) (simpleVec 0) ps ,p `plusDir` foldl (fmapNum2 max) (simpleVec 0) ps) twoFromList :: [a] -> Maybe (a, a) twoFromList [] = Nothing twoFromList [x] = Just (x, x) twoFromList (x:y:_) = Just (x, y) between :: Ord a => (a, a) -> a -> Bool between (l, h) x = l <= x && x <= h -- | Given a line and a shape, finds all possible intersections of the line -- with the shape. Since the shapes are convex, continuous 2D shapes, there -- will either be no intersections or two (which could be the same point). -- The returned value is distance along the line in multiples of the direction -- vector (the return value is the same idea as 'intersectLineCircle'). intersectLineShape :: forall a. (Floating a, Ord a) => Line2' a -> Shape' a -> Maybe (a, a) -- For circle, use existing function: intersectLineShape l (Circle c r) = intersectLineCircle l (c, r) -- For rectangle, use axis alignment: intersectLineShape l (Rectangle (Point2 (x,y)) (w, h)) = let leftE = fmap (flip alongLine l &&& id) $ valueAtX l (x-w) rightE = fmap (flip alongLine l &&& id) $ valueAtX l (x+w) topE = fmap (flip alongLine l &&& id) $ valueAtY l (y-h) bottomE = fmap (flip alongLine l &&& id) $ valueAtY l (y+h) in twoFromList $ map snd $ (filter (between (y-h, y+h) . getY . fst) $ catMaybes [leftE, rightE]) ++ (filter (between (x-w, x+w) . getX . fst) $ catMaybes [topE, bottomE]) -- For polygons, treat the line as a 0-length item in the perpendicular direction; -- project all the polygon points onto that direction, and any that cross the 0-point -- intersect. intersectLineShape l (Polygon c ps) = twoFromList $ mapMaybe check $ pairsInLoop ps' where -- To translate points to the line, we must add the centre of the polygon, -- and subtract the start of the line: translate = (fmapNum2 (-) c (getLineStart l) `plusDir`) ps' = map (flip projectPointOnto2 $ id &&& perpendicular2 $ getLineDir l) $ map translate ps sc = mag $ getLineDir l check :: (Point2' a, Point2' a) -> Maybe a check (p@(Point2 (_, y)), p'@(Point2 (_, y'))) = if signum y /= signum y' then fmap ((/ sc) . getX) $ pointAtY (p `lineTo` p') 0 else Nothing -- | Checks for overlap between the two shapes. If they do not collide, -- returns Nothing. If they do collide, gives back suggested angles away from -- each other. These are not necessarily the shortest direction to separate -- the two shapes, but should be decent for doing collision resolution (by using -- them as surface normals, or push-away vectors) -- -- The first vector returned is the direction in which the first shape should -- head (or the surface normal to bounce the first shape off), whereas the -- second vector returned is the direction in which the second shape should -- head (or the surface normal to bounce the second shape off). -- -- This function includes an initial quick test, followed by a more detailed test -- if necessary. overlap :: (Floating a, Ord a) => Shape' a -> Shape' a -> Maybe (Rel2' a, Rel2' a) overlap a b | not (possibleOverlap a b) = Nothing | otherwise = detailedOverlap a b -- | A quick test for possible intersection. -- -- If it returns False, there is definitely no overlap. If it returns True, there -- might be some overlap. For two circles, radiuses are checked (and the answer is -- always accurate), for any other combination of shapes it checks bounding boxes. possibleOverlap :: (Floating a, Ord a) => Shape' a -> Shape' a -> Bool possibleOverlap (Circle ca ra) (Circle cb rb) = magSq (ca `fromPt` cb) <= ((ra+rb)*(ra+rb)) possibleOverlap a b = not $ don'tOverlap getX || don'tOverlap getY where (a1, a2) = boundingBox a (b1, b2) = boundingBox b don'tOverlap f = f a2 < f b1 || f a1 > f b2 -- Projects an already-moved shape onto that axis. Returns a list of pairs where -- each item of the pair also has an index for that point (for circles, this is -- always -1). projectShape :: (Ord a, Floating a) => Shape' a -> Rel2' a -> [(Int, a)] projectShape (Circle c r) axis = let a = c `projectPointOnto` axis in [(-1,a - r), (-1, a + r)] -- I am assuming (perhaps incorrectly) that projecting each point onto the axis -- will be sufficient (rather than projecting each side) projectShape (Polygon c ps) axis = zip [0..] $ map (((c `projectPointOnto` axis') +) . (`projectOnto` axis')) ps where axis' = unitVector axis -- A rectangle has four points, all permutations of (+-w, +-h) -- Projection is done using the dot product. We can speed things up by calculating -- the two components of the dot product once, then adding them in different ways -- to achieve the projection. projectShape (Rectangle c (w,h)) axis = zip [0..] $ map ((c `projectPointOnto` axis) +) [-dotx+doty,dotx+doty,dotx-doty,-dotx-doty] where dotx = w * getX (unitVector axis) doty = h * getY (unitVector axis) -- All adjacent pairings, including last-first pairsInLoop :: [a] -> [(a,a)] pairsInLoop [] = [] pairsInLoop [_] = [] pairsInLoop xs = pairs' xs where -- all patterns are taken care of, despite what GHC thinks pairs' [x] = [(x, head xs)] pairs' (x:y:ys) = (x, y) : pairs' (y:ys) pairs' _ = error "Unreachable code in pairsInLoop in Shape module" -- | Collects a list of (unit-vector) axes perpendicular to all the edges of the -- polygon, pointed outwards. The list will be empty for circles. collectAxes :: (Floating a, Ord a) => Shape' a -> [Rel2' a] collectAxes (Circle {}) = [] collectAxes (Polygon _ ps) = map unitVector [perpendicular2 (a + b) | (a,b) <- pairsInLoop ps] collectAxes (Rectangle {}) = map (flip Rel2 1) [(-1,0), (1,0), (0, -1), (0, 1)] -- | Given a shape, gets a list of relative vectors from the centre of the shape -- to the points of the shape. For polygons, this is the points list (unmodified). -- For rectangles, it will be vectors to the four corners, and for circles, the -- list will be empty. polygonPoints :: Num a => Shape' a -> [Rel2' a] polygonPoints (Circle {}) = [] polygonPoints (Rectangle _ (w, h)) = map (flip Rel2 $ w*w + h*h) [(-w,h), (w, h), (w, -h), (-w, -h)] polygonPoints (Polygon _ ps) = ps -- | Given a shape, gets a list of points that make up the vertices of the -- shape. For circles, this list will be empty. shapePoints :: Num a => Shape' a -> [Point2' a] shapePoints s = map (shapeCentre s `plusDir`) (polygonPoints s) -- | Gets a list of lines representing each side of the shape (headed clockwise). -- For circles, the list will be empty. polygonLines :: (Floating a) => Shape' a -> [Line2' a] polygonLines s = map (uncurry lineTo) . pairsInLoop . map (shapeCentre s `plusDir`) . polygonPoints $ s -- Gives back the reflected unit vector for each shape's angle away from the other. -- returns Nothing if there was no collision after all. You should only call this -- if quickOverlap returned True. detailedOverlap :: forall a. (Num a, Ord a, Floating a) => Shape' a -> Shape' a -> Maybe (Rel2' a, Rel2' a) detailedOverlap (Circle pa _) (Circle pb _) -- Rely on quickOverlap having been called: = let a_min_b = pa `fromPt` pb in Just (unitVector a_min_b, unitVector $ negate a_min_b) -- We actually need to handle circle vs something, different than two polygons, -- because a circle and polygon can intersect without points being contained inside -- the other, which screws up our angle of incidence tests and so on. -- -- We test which lines intersect the circle, and use those to form the angle of -- incidence for the circle. For the reverse, we just use the vector from the -- centre of the circle to the average of the line intersections detailedOverlap (Circle pa ra) pb | null intersections = Nothing | otherwise = Just ({- Angle from polygon -} averageUnitVec $ map (perpendicular2 . getLineDir . fst) intersections ,{- Angle from circle -} averageUnitVec $ map (`fromPt` pa) $ map (uncurry $ flip alongLine) intersections ) where intersections = filter (\(_,x) -> 0 <= x && x <= 1) $ concat [if a == b then [(l, a)] else [(l, a),(l, b)] | (l, Just (a,b)) <- map (id &&& flip intersectLineCircle (pa, ra)) $ polygonLines pb] detailedOverlap pa pb@(Circle {}) = fmap (\(x,y) -> (y,x)) $ detailedOverlap pb pa -- Must be no circles now: detailedOverlap pa pb = case foldl1 intersect' (map (uncurry getOverlaps) projected) of (aps, bps) | null aps && null bps -> Nothing | otherwise -> let aLines = getLineIndexes (length $ collectAxes pa) aps bLines = getLineIndexes (length $ collectAxes pb) bps in Just $ averageUnitVec *** averageUnitVec $ unzip $ map (getPerpUnit *** getPerpUnit) $ map (fst *** fst) $ filter inBound $ findAllIntersections2 (map (polygonLines pb !!) bLines, map (polygonLines pa !!) aLines) where axes = collectAxes pa ++ collectAxes pb getPerpUnit = unitVector . perpendicular2 . (\(Line2 _ dir) -> dir) inBound ((_, ad), (_, bd)) = 0 <= ad && ad <= 1 && 0 <= bd && bd <= 1 -- Given number of points, and some point indexes, gets the indexes of all -- the lines adjacent to those points. If an empty list is given for the points, -- all line indexes are returned. getLineIndexes :: Int -> [Int] -> [Int] getLineIndexes total [] = [0 .. total - 1] getLineIndexes total ns = nub $ map (`mod` total) $ concatMap (\n -> [n-1,n]) ns projected :: [([(Int, a)], [(Int, a)])] projected = map (projectShape pa &&& projectShape pb) axes -- We can shortcut if any pair of lists involved is empty: intersect' :: ([Int], [Int]) -> ([Int], [Int]) -> ([Int], [Int]) intersect' (as, bs) (cs, ds) | (null as && null bs) || (null cs && null ds) = ([], []) | otherwise = (as `intersect` cs, bs `intersect` ds) getOverlaps :: [(Int, a)] -> [(Int, a)] -> ([Int], [Int]) getOverlaps as bs | maxa < minb || mina > maxb = ([], []) | otherwise = (map fst $ filter (overlapb . snd) as ,map fst $ filter (overlapa . snd) bs) where getMinMax = minimum &&& maximum (mina, maxa) = getMinMax $ map snd as (minb, maxb) = getMinMax $ map snd bs overlapa x = mina <= x && x <= maxa overlapb x = minb <= x && x <= maxb