diff --git a/Data/SG.hs b/Data/SG.hs new file mode 100644 --- /dev/null +++ b/Data/SG.hs @@ -0,0 +1,77 @@ +-- 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. + + +-- | A small geometry library, with vectors, matrices and simple shape +-- collision detection that is intended to be straightforward in two and three +-- dimensions. +-- +-- The basics of vectors are in the "Data.SG.Vector" module, the basics of lines +-- and geometry tests (e.g. testing whether a point is on a line) are in "Data.SG.Geometry", +-- with further specialised tests in "Data.SG.Geometry.TwoDim" and "Data.SG.Geometry.ThreeDim". +-- Matrix transformations are in "Data.SG.Matrix" and shapes (with collision detection) +-- are in "Data.SG.Shape". +-- +-- The names for most of the types in this library end with a prime. This is because +-- it is intended that you specialise these types (usually to Float or Double) +-- in your application as follows: +-- +-- > type Point2 = Point2' Double +-- > type Rel2 = Rel2' Double +-- > type Line2 = Line2' Double +-- > type Matrix22 = Matrix22' Double +-- +-- Much of the use of the types (especially vectors) in this library is made +-- using type-classes such as Num, Functor, Applicative and so on. For more +-- explanation on some of the less well-known type-classes, see either the +-- article Typeclassopedia in The Monad Reader +-- () issue 13 +-- (), or my own notes +-- at . +-- +-- To understand what various functions will actually do, look at the SGdemo project +-- () +-- on Hackage (and its code) which provides a visual demonstration of several of +-- the functions. +module Data.SG + (module Data.SG.Vector + ,module Data.SG.Vector.Basic + ,module Data.SG.Geometry + ,module Data.SG.Geometry.TwoDim + ,module Data.SG.Geometry.ThreeDim + ,module Data.SG.Matrix + ,module Data.SG.Shape + ) where + +import Data.SG.Vector +import Data.SG.Vector.Basic +import Data.SG.Geometry +import Data.SG.Geometry.TwoDim +import Data.SG.Geometry.ThreeDim +import Data.SG.Matrix +import Data.SG.Shape diff --git a/Data/SG/Geometry.hs b/Data/SG/Geometry.hs new file mode 100644 --- /dev/null +++ b/Data/SG/Geometry.hs @@ -0,0 +1,207 @@ +-- 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 the type-class (and associated functions) for dealing with +-- geometric systems of 2 or 3 dimensions. +module Data.SG.Geometry where + +import Control.Arrow +import Data.SG.Vector +import Data.SG.Vector.Basic + +-- | A geometry system, parameterised over points, relative (free) vectors, and +-- lines. There are separate instances for two dimensions and for three dimensions. +-- Each pair of type-class parameters is uniquely determined by the other parameter +-- (i.e. by the dimensionality, and which vector type you are using). +-- +-- Minimal implementation: everything but scaleRel. +class (VectorNum rel, Coord rel, Coord pt, IsomorphicVectors rel pt, IsomorphicVectors + pt rel) => Geometry rel pt ln | rel -> pt ln, pt -> rel ln, ln -> rel pt where + -- | Scales a relative (free) vector by the given amount. + scaleRel :: Num a => a -> rel a -> rel a + scaleRel a = fmapNum1 (*a) + -- | Adds a relative (free) vector to a given point. + plusDir :: ( Num a, Eq a, Show a ) => pt a -> rel a -> pt a + -- | Determines the relative (free) vector /to/ the first parameter /from/ the + -- second parameter. So: + -- + -- > Point2 (1,8) `fromPt` Point2 (3,4) == Point2 (-2,3) + fromPt :: (Num a, Eq a, Show a) => pt a -> pt a -> rel a + -- | Given a line, converts it back into its point and relative vector. It should + -- always be the case that @uncurry makeLine . getLineVecs@ is the identity function. + getLineVecs :: Num a => ln a -> (pt a, rel a) + -- | Given a point and relative vector, creates a line. It should always be + -- the case that @uncurry makeLine . getLineVecs@ is the identity function. + makeLine :: Num a => pt a -> rel a -> ln a + +instance Geometry Pair Pair LinePair where + plusDir = (+) + fromPt = (-) + getLineVecs (LinePair lp) = lp + makeLine = curry LinePair + +instance Geometry Triple Triple LineTriple where + plusDir = (+) + fromPt = (-) + getLineVecs (LineTriple lp) = lp + makeLine = curry LineTriple + + +-- | Adds the negation of the relative (free) vector to the point. +minusDir :: (Num a, Geometry rel pt ln, Eq a, Show a) => pt a -> rel a -> pt a +minusDir p r = p `plusDir` fmapNum1 negate r + +-- | The flipped version of 'fromPt'. +toPt :: (Geometry rel pt ln, Num a, Eq a, Show a) => pt a -> pt a -> rel a +toPt = flip fromPt + +-- | Gets the line /from/ the first point, /to/ the second point. +lineTo :: (Num a, Geometry rel pt ln, Eq a, Show a) => pt a -> pt a -> ln a +lineTo a b = makeLine a (b `fromPt` a) + +-- | The flipped version of 'lineTo'. +lineFrom :: (Num a, Geometry rel pt ln, Eq a, Show a) => pt a -> pt a -> ln a +lineFrom = flip lineTo + +-- | Gets the point at the start of the line. +getLineStart :: (Num a, Geometry rel pt ln) => ln a -> pt a +getLineStart = fst . getLineVecs + +-- | Gets the direction vector of the line. +getLineDir :: (Num a, Geometry rel pt ln) => ln a -> rel a +getLineDir = snd . getLineVecs + +-- | Gets the point at the end of the line. +getLineEnd :: (Geometry rel pt ln, Num a, Eq a, Show a) => ln a -> pt a +getLineEnd = uncurry plusDir . getLineVecs + +-- | Alters the line to the given length, but with the same start point and direction. +makeLength :: (Floating a, Ord a, Geometry rel pt ln) => a -> ln a -> ln a +makeLength x = uncurry makeLine . second (scaleRel x . unitVector) . getLineVecs + +-- | Given a multiple of the /direction vector/ (this is /not/ distance unless +-- the direction vector is a unit vector), calculates that point. +alongLine :: (Num a, Geometry rel pt ln, Eq a, Show a) => a -> ln a -> pt a +alongLine a = uncurry plusDir . second (scaleRel a) . getLineVecs + +-- | Checks if the given point is on the given line (to within a small epsilon-tolerance). +-- If it is, gives back the distance along the line (as a multiple of its direction +-- vector) to the point in a Just wrapper. If the point is not on the line, Nothing +-- is returned. +distAlongLine :: (Geometry rel pt ln, Ord a, Floating a, Show a) => pt a -> ln a -> Maybe a +distAlongLine pt ln + = if sameDirection lnDir fromStart + then Just $ mag fromStart + else Nothing + where + fromStart = pt `fromPt` getLineStart ln + lnDir = getLineDir ln + +-- | Checks if the given point is on the given line (to within a small epsilon-tolerance). +isOnLine :: (Geometry rel pt ln, Ord a, Floating a, Show a) => pt a -> ln a -> Bool +isOnLine pt ln = sameDirection lnDir fromStart + where + fromStart = pt `fromPt` getLineStart ln + lnDir = getLineDir ln + +-- | Finds the nearest point on the line to the given point, and gives back its +-- distance along the line (as a multiple of the direction vector). Since the +-- nearest distance will be at a right-angle to the point, this is the same as +-- projecting the point onto the line. +nearestDistOnLine :: (Geometry rel pt ln, Ord a, Floating a, Eq a, Show a) => + pt a -> ln a -> a +-- The nearest point on the line will be the one forming a right-angle triangle +-- between the line and the point. We can use the dot product to project the point +-- onto the line. We want |a| cos theta / |b| for the distance, which is the same +-- as a . b / |b|^2. +nearestDistOnLine pt ln + | lnDirMagSq == 0 = 0 -- all-zero direction vector + | otherwise = (fromStart `dotProduct` lnDir) / lnDirMagSq + where + fromStart = pt `fromPt` getLineStart ln + lnDir = getLineDir ln + lnDirMagSq = magSq lnDir + +-- | Finds the nearest point on the line to the given point, and gives back the +-- point. +nearestPointOnLine :: (Geometry rel pt ln, Ord a, Floating a, Show a) => + pt a -> ln a -> pt a +nearestPointOnLine pt ln = nearestDistOnLine pt ln `alongLine` ln + +-- | Gives the distance along the line (2D or 3D) at a given X value. Returns Nothing +-- if the line is parallel to the YZ plane (in 2D, if the X component of the line +-- is zero). The value returned is a multiple of the direction vector of the line, +-- which will only be the same as distance if the direction vector is a unit vector. +valueAtX :: (Geometry rel pt ln, Coord2 rel, Coord2 pt, Fractional a, Eq a) + => ln a -> a -> Maybe a +valueAtX l tgt + | xd == 0 = Nothing + | otherwise = let t = (tgt - x) / xd in Just t + where + x = getX $ getLineStart l + xd = getX $ getLineDir l + +-- | Gives the distance along the line (2D or 3D) at a given Y value. Returns Nothing +-- if the line is parallel to the XZ plane (in 2D, if the Y component of the line +-- is zero). The value returned is a multiple of the direction vector of the line, +-- which will only be the same as distance if the direction vector is a unit vector. +valueAtY :: (Geometry rel pt ln, Coord2 rel, Coord2 pt, Fractional a, Eq a) + => ln a -> a -> Maybe a +valueAtY l tgt + | yd == 0 = Nothing + | otherwise = let t = (tgt - y) / yd in Just t + where + y = getY $ getLineStart l + yd = getY $ getLineDir l + +-- | Gives the distance along the 3D line at a given Z value. Returns Nothing +-- if the line is parallel to the XY plane. The value returned is a multiple +-- of the direction vector of the line, which will only be the same as +-- distance if the direction vector is a unit vector. +valueAtZ :: (Geometry rel pt ln, Coord3 rel, Coord3 pt, Fractional a, Eq a) + => ln a -> a -> Maybe a +valueAtZ l tgt + | zd == 0 = Nothing + | otherwise = let t = (tgt - z) / zd in Just t + where + z = getZ $ getLineStart l + zd = getZ $ getLineDir l + +-- | pointAtX (and the Y and Z equivalents) are wrappers around 'valueAtX' (and +-- similar) that give back the point rather than distance along the line. +pointAtX, pointAtY :: (Geometry rel pt ln, Coord2 rel, Coord2 pt, Fractional a, Eq a, Show a) + => ln a -> a -> Maybe (pt a) +pointAtX l = fmap (flip alongLine l) . valueAtX l +pointAtY l = fmap (flip alongLine l) . valueAtY l + +pointAtZ :: (Geometry rel pt ln, Coord3 rel, Coord3 pt, Fractional a, Eq a, Show a) + => ln a -> a -> Maybe (pt a) +pointAtZ l = fmap (flip alongLine l) . valueAtZ l + + diff --git a/Data/SG/Geometry/ThreeDim.hs b/Data/SG/Geometry/ThreeDim.hs new file mode 100644 --- /dev/null +++ b/Data/SG/Geometry/ThreeDim.hs @@ -0,0 +1,159 @@ +-- 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. + + +-- | A module with types to use in a 3D system, and various helper functions. +-- Several more functions are available for use in the "Data.SG.Geometry" module. +module Data.SG.Geometry.ThreeDim where + +import Control.Applicative +import Data.Foldable (Foldable(foldr)) +import Data.Traversable (Traversable(traverse)) + +import Data.SG.Geometry +import Data.SG.Vector +import Data.SG.Vector.Basic + +-- | A point in 3D space. +newtype Point3' a = Point3 (a, a, a) + deriving (Eq, Ord, Show, Read) + +-- | A relative vector (free vector) in 3D space. The triple is the x, y, z components, +-- and the last item is the /squared magnitude/ of the vector, which is stored +-- with it to speed up various operations. It is suggested you use 'makeRel3' +-- to create one of these, unless the magnitude is easily apparent, e.g. @Rel3 +-- (0, 1, 1) 2@ +data Rel3' a = Rel3 (a, a, a) a + deriving (Eq, Ord, Show, Read) + +-- | Constructs a Rel3' vector +makeRel3 :: Num a => (a, a, a) -> Rel3' a +makeRel3 (x, y, z) = Rel3 (x, y, z) (x * x + y * y + z * z) + +instance IsomorphicVectors Rel3' Point3' where + iso (Rel3 p _) = Point3 p +instance IsomorphicVectors Point3' Rel3' where + iso (Point3 p) = makeRel3 p + +instance IsomorphicVectors Rel3' Triple where + iso (Rel3 p _) = Triple p +instance IsomorphicVectors Triple Rel3' where + iso (Triple p) = makeRel3 p + +instance IsomorphicVectors Point3' Triple where + iso (Point3 p) = Triple p +instance IsomorphicVectors Triple Point3' where + iso (Triple p) = Point3 p + +instance VectorNum Rel3' where + fmapNum1 f (Rel3 (x, y, z) _) = makeRel3 (f x, f y, f z) + fmapNum2 f (Rel3 (x, y, z) _) (Rel3 (x', y', z') _) = makeRel3 (f x x', f y y', f z z') + fmapNum1inv f (Rel3 (x, y, z) m) = Rel3 (f x, f y, f z) m + simpleVec a = Rel3 (a, a, a) (3*a*a) + +instance VectorNum Point3' where + fmapNum1 = fmap + fmapNum1inv = fmap + fmapNum2 = liftA2 + simpleVec = pure + +instance (Show a, Eq a, Num a) => Num (Rel3' a) where + (+) = fmapNum2 (+) + (-) = fmapNum2 (-) + (*) = fmapNum2 (*) + abs = fmapNum1inv abs + signum = fmapNum1 signum + negate = fmapNum1inv negate + fromInteger = simpleVec . fromInteger + +instance Functor Point3' where + fmap f (Point3 (x, y, z)) = Point3 (f x, f y, f z) + +instance Applicative Point3' where + pure a = Point3 (a, a, a) + (<*>) (Point3 (fa, fb, fc)) (Point3 (a, b, c)) = Point3 (fa a, fb b, fc c) + +instance Foldable Point3' where + foldr f t (Point3 (x, y, z)) = x `f` (y `f` (z `f` t)) + +instance Foldable Rel3' where + foldr f t (Rel3 (x, y, z) _) = x `f` (y `f` (z `f` t)) + +instance Traversable Point3' where + traverse f (Point3 (x, y, z)) = liftA3 (curry3 Point3) (f x) (f y) (f z) + where + curry3 g a b c = g (a, b, c) + +instance Coord2 Point3' where + getX (Point3 (a,_,_)) = a + getY (Point3 (_,b,_)) = b + +instance Coord3 Point3' where + getZ (Point3 (_,_,c)) = c + +instance Coord2 Rel3' where + getX (Rel3 (a, _, _) _) = a + getY (Rel3 (_, b, _) _) = b + +instance Coord3 Rel3' where + getZ (Rel3 (_, _, c) _) = c + +instance Coord Point3' where + getComponents (Point3 (a, b, c)) = [a, b, c] + fromComponents (a:b:c:_) = Point3 (a, b, c) + fromComponents xs = fromComponents $ xs ++ repeat 0 + +instance Coord Rel3' where + getComponents (Rel3 (a, b, c) _) = [a, b, c] + fromComponents (a:b:c:_) = makeRel3 (a, b, c) + fromComponents xs = fromComponents $ xs ++ repeat 0 + magSq (Rel3 _ msq) = msq + dotProduct (Rel3 (a, b, c) _) (Rel3 (a', b', c') _) + = a * a' + b * b' + c * c' + +instance Geometry Rel3' Point3' Line3' where + -- a*x*a*x + a*y*a*y = a^2 * (x^2 + y^2) + scaleRel a (Rel3 (x, y, z) m) = Rel3 (a*x, a*y, a*z) (a*a*m) + plusDir (Point3 (x, y, z)) (Rel3 (x', y', z') _) + = Point3 (x + x', y + y', z + z') + fromPt (Point3 (x, y, z)) (Point3 (x', y', z')) + = makeRel3 (x - x', y - y', z - z') + getLineVecs (Line3 pt dir) = (pt, dir) + makeLine = Line3 + +------------------------------------------------------------ +-- Line stuff: +------------------------------------------------------------ + +-- | A line in 3D space. A line is a point and a free vector indicating +-- direction. A line may be treated by a function as either finite (taking +-- the magnitude of the free vector as the length) or infinite (ignoring the +-- magnitude of the direction vector). +data Line3' a = Line3 {getLineStart3 :: (Point3' a) , getLineDir3 :: (Rel3' a)} + deriving (Eq, Show, Read) + diff --git a/Data/SG/Geometry/TwoDim.hs b/Data/SG/Geometry/TwoDim.hs new file mode 100644 --- /dev/null +++ b/Data/SG/Geometry/TwoDim.hs @@ -0,0 +1,316 @@ +-- 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. + + +-- | A module with types to use in a 2D system, and various helper functions. +-- Several more functions are available for use in the "Data.SG.Geometry" module. +module Data.SG.Geometry.TwoDim (Point2'(..), Rel2'(..), makeRel2, Line2'(..), + toAngle, perpendicular2, reflectAgainst2, reflectAgainstIfNeeded2, intersectLines2, findAllIntersections2, + intersectLineCircle, point2AtZ) where + +import Control.Applicative +import Control.Arrow ((&&&)) +import Data.Foldable (Foldable(foldr)) +import Data.Traversable (Traversable(traverse)) +import Data.Maybe + +import Data.SG.Vector +import Data.SG.Vector.Basic +import Data.SG.Geometry + +-- | A point in 2D space. +newtype Point2' a = Point2 (a, a) + deriving (Eq, Ord, Show, Read) + +-- | A relative vector (free vector) in 2D space. The pair are the x and y components, +-- and the last item is the /squared magnitude/ of the vector, which is stored +-- with it to speed up various operations. It is suggested you use 'makeRel2' +-- to create one of these, unless the square magnitude is easily apparent, e.g. @Rel2 +-- (0, 2) 4@ +data Rel2' a = Rel2 (a,a) a + deriving (Eq, Ord, Show, Read) + +-- | Constructs a Rel2' vector. +makeRel2 :: Num a => (a, a) -> Rel2' a +makeRel2 (x, y) = Rel2 (x, y) (x * x + y * y) + +instance IsomorphicVectors Rel2' Point2' where + iso (Rel2 p _) = Point2 p +instance IsomorphicVectors Point2' Rel2' where + iso (Point2 p) = makeRel2 p + +instance IsomorphicVectors Rel2' Pair where + iso (Rel2 p _) = Pair p +instance IsomorphicVectors Pair Rel2' where + iso (Pair p) = makeRel2 p + +instance IsomorphicVectors Point2' Pair where + iso (Point2 p) = Pair p +instance IsomorphicVectors Pair Point2' where + iso (Pair p) = Point2 p + +instance VectorNum Rel2' where + fmapNum1 f (Rel2 (x, y) _) = makeRel2 (f x, f y) + fmapNum2 f (Rel2 (x, y) _) (Rel2 (x', y') _) = makeRel2 (f x x', f y y') + fmapNum1inv f (Rel2 (x, y) m) = Rel2 (f x, f y) m + simpleVec a = Rel2 (a, a) (2*a*a) + +instance VectorNum Point2' where + fmapNum1 = fmap + fmapNum1inv = fmap + fmapNum2 = liftA2 + simpleVec = pure + +-- | Multiplication doesn't make much sense, but the rest do! +instance (Show a, Eq a, Num a) => Num (Rel2' a) where + (+) = fmapNum2 (+) + (-) = fmapNum2 (-) + (*) = fmapNum2 (*) + abs = fmapNum1inv abs + signum = fmapNum1 signum + negate = fmapNum1inv negate + fromInteger = simpleVec . fromInteger + +instance Functor Point2' where + fmap f (Point2 (x, y)) = Point2 (f x, f y) + +instance Applicative Point2' where + pure a = Point2 (a, a) + (<*>) (Point2 (fa, fb)) (Point2 (a, b)) = Point2 (fa a, fb b) + +instance Foldable Point2' where + foldr f t (Point2 (x, y)) = x `f` (y `f` t) + +instance Foldable Rel2' where + foldr f t (Rel2 (x, y) _) = x `f` (y `f` t) + +instance Traversable Point2' where + traverse f (Point2 (x, y)) = liftA2 (curry Point2) (f x) (f y) + +instance Coord2 Point2' where + getX (Point2 (a, _)) = a + getY (Point2 (_, b)) = b + +instance Coord Point2' where + getComponents (Point2 (a, b)) = [a, b] + fromComponents (a:b:_) = Point2 (a, b) + fromComponents xs = fromComponents $ xs ++ repeat 0 + +instance Coord2 Rel2' where + getX (Rel2 (a, _) _) = a + getY (Rel2 (_, b) _) = b + +instance Coord Rel2' where + getComponents (Rel2 (a, b) _) = [a, b] + fromComponents (a:b:_) = makeRel2 (a, b) + fromComponents xs = fromComponents $ xs ++ repeat 0 + magSq (Rel2 _ msq) = msq + dotProduct (Rel2 (a, b) _) (Rel2 (a', b') _) + = a * a' + b * b' + +instance Geometry Rel2' Point2' Line2' where + -- a*x*a*x + a*y*a*y = a^2 * (x^2 + y^2) + scaleRel a (Rel2 (x,y) m) = Rel2 (a*x, a*y) (a*a*m) + plusDir (Point2 (x, y)) (Rel2 (x', y') _) = Point2 (x + x', y + y') + fromPt (Point2 (x, y)) (Point2 (x', y')) = makeRel2 (x - x', y - y') + getLineVecs (Line2 pt dir) = (pt, dir) + makeLine = Line2 + +-- | Gets the angle, in /radians/, anti-clockwise from the x-axis. If you pass +-- the all-zero vector, the return value will be zero. +toAngle :: RealFloat a => Rel2' a -> a +toAngle (Rel2 (x, y) _) + | x == 0 && y == 0 = 0 + | otherwise = atan2 y x + +-- | Gets the vector perpendicular to the given 2D vector. If you pass it a vector +-- that is in a clockwise direction around a polygon, the result will always face +-- away from the polygon. +perpendicular2 :: Num a => Rel2' a -> Rel2' a +perpendicular2 (Rel2 (x,y) m) = Rel2 (-y, x) m + +-- | Reflects the first direction vector against the given surface normal. The +-- resulting direction vector should have the same magnitude as the original +-- first parameter. An example: +-- +-- > makeRel2 (-3, -4) `reflectAgainst2` makeRel2 (0,1) == makeRel2 (-3, 4) +reflectAgainst2 :: (Floating a, Ord a, Eq a, Show a) => Rel2' a -> Rel2' a -> Rel2' a +reflectAgainst2 v n = alongNormal + alongSurface + where + n' = unitVector n + alongNormal = fmapNum1 (*(negate (v `projectOnto` n'))) n' + alongSurface = fmapNum1 (*(v `projectOnto` perpendicular2 n')) (perpendicular2 n') + +-- | Reflects the first direction vector against the given surface normal. The +-- resulting direction vector should have the same magnitude as the original first +-- parameter. +-- +-- The reflection is not performed if the given vector points along the same +-- direction as the normal, that is: if once projected onto the normal vector, +-- the component is positive, the original first parameter is returned +-- unmodified. Examples: +-- +-- > makeRel2 (-3, -4) `reflectAgainstIfNeeded2` makeRel2 (0,1) == makeRel2 (-3, 4) +-- > makeRel2 (-3, 4) `reflectAgainstIfNeeded2` makeRel2 (0,1) == makeRel2 (-3, 4) +reflectAgainstIfNeeded2 :: (Floating a, Ord a, Eq a, Show a) => Rel2' a -> Rel2' a -> Rel2' a +reflectAgainstIfNeeded2 v n + | towardsComponent < 0 = alongNormal + alongSurface + | otherwise = v + where + n' = unitVector n + towardsComponent = v `projectOnto` n' + alongNormal = fmapNum1 (*(negate towardsComponent)) n' + alongSurface = fmapNum1 (*(v `projectOnto` perpendicular2 n')) (perpendicular2 n') + +-- | A line in 2D space. A line is a point, and a free vector indicating +-- direction. A line may be treated by a function as either finite (taking +-- the magnitude of the free vector as the length) or infinite (ignoring the +-- magnitude of the direction vector). +data Line2' a = Line2 {getLineStart2 :: (Point2' a) , getLineDir2 :: (Rel2' a)} + deriving (Eq, Show, Read) + +-- Given vectors: (x,y) + t(xd,yd) +-- (x',y') + t'(xd',yd') +-- Intersection is: +-- +-- (x,y) + t(xd,yd) = (x',y') + t'(xd',yd') +-- +-- Split, work with them in pairs: +-- +-- (X1) x + t xd = x' + t' xd' +-- (Y1) y + t yd = y' + t' yd' +-- +-- (X2a) t xd = x' + t' xd' - x +-- (Y2a) t yd = y' + t' yd' - y +-- +-- (X3a) t xd yd = yd (x' + t' xd' - x) +-- (Y3a) t yd xd = xd (y' + t' yd' - y) +-- +-- Now set RHSs equal: +-- +-- (A1) yd (x' + t' xd' - x) = xd (y' + t' yd' - y) +-- (A2) yd (x' - x) + t' xd' yd = xd (y' - y) + t' xd yd' +-- (A3) t' xd' yd - t' xd yd' = xd (y' - y) - yd (x' - x) +-- (A4) t' (xd' yd - xd yd') = xd (y' - y) - yd (x' - x) +-- +-- If (xd' yd - xd yd') /= 0: +-- t' = [xd (y' - y) - yd (x' - x)] / (xd' yd - xd yd') +-- +-- Similarly: +-- (X2b) t' xd' = x + t xd - x' +-- (Y2b) t' yd' = y + t yd - y' +-- +-- (X3b) t' xd' yd' = yd' (x + t xd - x') +-- (Y3b) t' yd' xd' = xd' (y + t yd - y') +-- +-- Now set RHSs equal: +-- +-- (B1) yd' (x + t xd - x') = xd' (y + t yd - y') +-- (B2) yd' (x - x') + t xd yd' = xd' (y - y') + t xd' yd +-- (B3) t xd yd' - t xd' yd = xd' (y - y') - yd' (x - x') +-- (B4) t (xd yd' - xd' yd) = xd' (y - y') - yd' (x - x') +-- +-- If (xd yd' - xd' yd) /= 0 (note: negation of previous item) +-- t = [xd' (y - y') - yd' (x - x')] / (xd yd' - xd' yd) + +-- | Given two 2D lines, finds out their intersection. The first part of the +-- result pair is how much to multiply the direction vector of the first line +-- by (and add it to the start point of the first line) to reach the +-- intersection, and the second part is the corresponding item for the second line. +-- So given @Just (a, b) = intersectLines2 la lb@, it should be the case (minus +-- some possible precision loss) that @alongLine a la == alongLine b lb@. If the +-- lines are parallel, Nothing is returned. +-- +-- Note that this function assumes the lines are infinite. If you want to check +-- for the intersection of two finite lines, check if the two parts of the result +-- pair are both in the range 0 to 1 inclusive. +intersectLines2 :: (Fractional a, Eq a, Show a) => Line2' a -> Line2' a -> Maybe (a, a) +intersectLines2 (Line2 (Point2 (x,y)) (Rel2 (xd,yd) _)) (Line2 (Point2 (x',y')) (Rel2 (xd',yd') _)) + | a == 0 = Nothing + | otherwise = Just $ (t, t') + where + a = (xd' * yd) - (xd * yd') + t' = ((xd * (y' - y)) - (yd * (x' - x))) / a + t = ((xd' * (y - y')) - (yd' * (x - x'))) / (negate a) + +-- | Finds all the intersections between a line from the first list and a line from +-- the second list, and how far along that is each line. That is, this is a bit +-- like mapMaybe composed with intersectLines2 on all pairings of a line from the +-- first list and a line from the second list. +findAllIntersections2 :: (Fractional a, Eq a, Show a) => ([Line2' a], [Line2' a]) -> [((Line2' a, a), (Line2' a, a))] +findAllIntersections2 (as, bs) + = catMaybes [ case intersectLines2 a b of + Just (ad, bd) -> Just ((a,ad), (b,bd)) + Nothing -> Nothing + | a <- as, b <- bs] + +-- Vector: (x,y) = (x',y') + t(xd,yd) +-- Circle: (x-a)^2+(y-b)^2 = r^2 +-- +-- Substitute: +-- (x' + t xd - a)^2 + (y' + t yd - b)^2 = r^2 +-- Define c = x' - a, d = y' - b: +-- (c + t xd)^2 + (d + t yd)^2 = r^2 +-- t^2 (xd^2 + yd^2) + 2 (c xd + d yd) t + c^2 + d^2 - r^2 = 0 +-- Then use quadratic formula! +-- +-- We can take a slight short cut since xd^2 + yd^2 is the magnitude squared of +-- (xd, yd) +-- +-- No ordering is guaranteed about the return values! + + +-- | Given a line, and a circle (defined by a point and a radius), finds the points +-- of intersection. +-- +-- If the line does not intersect the circle, Nothing is returned. If they do +-- intersect, two values are returned that are distances along the line. That +-- is, given @Just (a, b) = intersectLineCircle l c@, the two points of intersection +-- are @(alongLine l a, alongLine l b)@. +-- +-- The ordering of the two items in the pair is arbitrary, and if the line is a +-- tangent to the circle, the values will be the same. +intersectLineCircle :: (Ord a, Floating a) => Line2' a -> (Point2' a, a) -> Maybe (a, a) +intersectLineCircle (Line2 (Point2 (lx, ly)) (Rel2 (xd, yd) m)) + (Point2 (cx, cy), r) + = case b*b - 4*a*c of + z | z < 0 -> Nothing + | a == 0 -> -- all-zero direction vector + if c == 0 -- If c is zero, the start point is on the line + then Just (0,0) + else Nothing + | otherwise -> Just ((-b + sqrt z) / (2*a), (-b - sqrt z) / (2*a)) + where + a = m + b = 2 * ((lx - cx) * xd + (ly - cy) * yd) + c = (lx - cx)*(lx - cx) + (ly - cy)*(ly - cy) - r*r + +-- | Like 'pointAtZ', but returns a 2D vector instead of a 3D vector +point2AtZ :: (Geometry rel pt ln, Coord3 rel, Coord3 pt, Fractional a, Eq a, Show a) + => ln a -> a -> Maybe (Point2' a) +point2AtZ l = fmap (Point2 . (getX &&& getY) . flip alongLine l) . valueAtZ l diff --git a/Data/SG/Matrix.hs b/Data/SG/Matrix.hs new file mode 100644 --- /dev/null +++ b/Data/SG/Matrix.hs @@ -0,0 +1,220 @@ +-- 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. + + +-- | A module with various simple matrix operations to augment the vector stuff. +-- +-- The Num instances implement proper matrix multiplication as you would expect +-- (not element-wise multiplication). +module Data.SG.Matrix (Matrix22', Matrix33', Matrix44', SquareMatrix(..), Matrix(..), + identityMatrix, multMatrix, multMatrixGen, translate2D, translate3D, rotateXaxis, rotateYaxis, rotateZaxis) where + +import Control.Applicative +import Control.Arrow (first) +import Control.Monad.State hiding (mapM) +import Data.Foldable (Foldable, foldr, toList, sum) +import qualified Data.List as List +import Data.Traversable (Traversable, traverse, mapM) +import Prelude hiding (mapM, foldr, sum) + +import Data.SG.Vector +import Data.SG.Vector.Basic + +-- This function will only work for certain types! Most importantly, it will not +-- work with lists... +fromList :: (Applicative c, Traversable c) => [a] -> c a +fromList = evalState $ mapM (const $ getHead) $ pure (error "Matrix.fromList") + where + getHead = do (x:xs) <- get + put xs + return x + +-- | A square matrix. You will almost certainly want to use 'Matrix22'' and similar +-- instead of this directly. It does have a variety of useful instances though, +-- especially 'Functor', 'Num' and 'Matrix'. +-- +-- Its definition is based on a square matrix being, for example, a pair of pairs +-- or a triple of triples. +newtype SquareMatrix c a = SquareMatrix (c (c a)) + +instance Functor c => Functor (SquareMatrix c) where + fmap f (SquareMatrix m) = SquareMatrix $ fmap (fmap f) m + +instance Applicative c => Applicative (SquareMatrix c) where + pure = SquareMatrix . pure . pure + (SquareMatrix f) <*> (SquareMatrix m) = SquareMatrix $ (fmap (<*>) f) <*> m + -- f :: c (c (a -> b)) + -- m :: c (c a) + -- in ??? <*> m, ??? :: c (c a -> c b) + -- fmap (<*>) f :: c (c a -> c b) + +instance (Foldable c, Applicative c, Eq a) => Eq (SquareMatrix c a) where + (==) a b = foldr (&&) True $ liftA2 (==) a b +-- (==) (SquareMatrix a) (SquareMatrix b) = and $ zipWith (==) (list a) (list b) +-- where list = concatMap toList . toList + +instance Foldable c => Foldable (SquareMatrix c) where + foldr f x (SquareMatrix m) = foldr (flip $ foldr f) x m + +instance Traversable c => Traversable (SquareMatrix c) where + traverse f (SquareMatrix m) = liftA SquareMatrix $ traverse (traverse f) m + +instance (Applicative c, Foldable c, Traversable c, Functor c, Show a) => Show (SquareMatrix c a) where + show = show . matrixComponents + +instance (Read a, Num a, Applicative c, Traversable c) => Read (SquareMatrix c a) where + readsPrec n s = map (first fromMatrixComponents) $ readsPrec n s + +-- | A 2x2 matrix. Primarily useful via its instances, such as 'Functor', 'Num', +-- and 'Matrix'. +type Matrix22' a = SquareMatrix Pair a +-- | A 3x3 matrix. Primarily useful via its instances, such as 'Functor', 'Num', +-- and 'Matrix'. +type Matrix33' a = SquareMatrix Triple a +-- | A 4x4 matrix. Primarily useful via its instances, such as 'Functor', 'Num', +-- and 'Matrix'. +type Matrix44' a = SquareMatrix Quad a + +-- | The class that all matrices belong to. +class Matrix m where + -- | Gives back the matrix as a list of rows. + matrixComponents :: m a -> [[a]] + -- | Creates a matrix from a list of rows. Any missing entries are filled + -- in with the relevant entries from the identity matrix, hence the identity + -- matrix is equivalent to @fromMatrixComponents []@. + fromMatrixComponents :: Num a => [[a]] -> m a + + -- | Transposes a matrix + transpose :: m a -> m a + +-- | The identity matrix. +identityMatrix :: (Num a, Matrix m) => m a +identityMatrix = fromMatrixComponents [] + +instance (Applicative c, Foldable c, Traversable c, Functor c) => Matrix (SquareMatrix c) where + matrixComponents (SquareMatrix m) = map toList $ toList m + fromMatrixComponents = SquareMatrix . fmap fromRow . fromList . zip [0..] . addIdentityRows + where + addIdentityRows xs = xs ++ identityRows (length xs) + identityRow n = replicate n 0 ++ [1] ++ repeat 0 + identityRows n = identityRow n : identityRows (n + 1) + fromRow (n, r) = fromList $ r ++ drop (length r) (identityRow n) + + -- TODO make this all-functors: + transpose (SquareMatrix m) = SquareMatrix . fromList . map fromList . List.transpose . map toList . toList $ m + +instance (Num a, Traversable c, Foldable c, Functor c, Applicative c) => Num (SquareMatrix c a) where + (+) = liftA2 (+) + (-) = liftA2 (-) + -- Multiplication: hmmmm. + -- + -- We need to turn each element of the left-hand matrix into an operation on + -- the whole of the right-hand matrix that will yield the right result. Each + -- element needs to operate on its own row from the LHS, and its own column from + -- the RHS. + -- + (*) (SquareMatrix a) (SquareMatrix b) + = SquareMatrix $ fmap perRow a + where +-- sumSetOfRows :: c (c a) -> c a + sumSetOfRows = foldr (liftA2 (+)) (pure 0) + +-- perRow :: c a -> c a + perRow lrow = sumSetOfRows $ liftA2 (\x y -> fmap (*x) y) lrow b + + abs = fmap abs + negate = fmap negate + signum = fmap signum + fromInteger = pure . fromInteger + +-- | Matrix multiplication. There is no requirement that the size of +-- the matrix matches the size of the vector: +-- +-- * If the vector is too small for the matrix (e.g. multiplying a 4x4 matrix by +-- a 3x3 vector), 1 will be used for the missing vector entries. +-- +-- * If the matrix is too small for the vector (e.g. multiplying a 2x2 matrix by +-- a 3x3 vector), the other components of the vector will be left untouched. +-- +-- This allows you to do tricks such as multiplying a 4x4 matrix by a 3D vector, +-- and doing translation (a standard 3D graphics trick). +multMatrixGen :: (Coord p, Matrix m, Num a) => m a -> p a -> p a +multMatrixGen m v = fromComponents $ comps ++ drop (length comps) vc + where + comps = [sum $ zipWith (*) r vc | r <- matrixComponents m] + -- All missing components are 1: + vc = getComponents v ++ repeat 1 + +-- | Matrix multiplication where the size of the vector matches the dimensions +-- of the matrix. The complicated type just means that this function will +-- work for any combination of matrix types and vectors where the width of the +-- square matrix is the same as the number of dimensions in the vector. +multMatrix :: (Foldable c, Applicative c, Num a, IsomorphicVectors c p, IsomorphicVectors p c) => SquareMatrix c a -> p a -> p a +multMatrix (SquareMatrix m) v + = iso $ fmap (sum . liftA2 (*) (iso v)) m + +-- | Given an angle in /radians/, produces a matrix that rotates anti-clockwise +-- by that angle around the Z axis. Note that this can be used to produce a 2x2 +-- (in which case it is a rotation around the origin), 3x3 or 4x4 matrix. +rotateZaxis :: (Floating a, Matrix m) => a -> m a +rotateZaxis t = fromMatrixComponents [[cos t, - sin t], [sin t, cos t]] + +-- | Given an angle in /radians/, produces a matrix that rotates anti-clockwise +-- by that angle around the X axis. Note that this can be used to produce a 2x2, +-- 3x3 or 4x4 matrix, but if you produce a 2x2 matrix, odd things will happen! +rotateXaxis :: (Floating a, Matrix m) => a -> m a +rotateXaxis t = fromMatrixComponents [[1,0,0], [0, cos t, - sin t], [0, sin t, cos t]] + +-- | Given an angle in /radians/, produces a matrix that rotates anti-clockwise +-- by that angle around the Y axis. Note that this can be used to produce a 2x2, +-- 3x3 or 4x4 matrix, but if you produce a 2x2 matrix, odd things will happen! +rotateYaxis :: (Floating a, Matrix m) => a -> m a +rotateYaxis t = fromMatrixComponents [[cos t, 0, - sin t], [0,1,0], [sin t, 0, cos t]] + +-- | Given a 2D relative vector, produces a matrix that will translate by that +-- much (when you multiply a 2D point with it using multMatrixGen) +translate2D :: (Num a, IsomorphicVectors p Pair) => p a -> Matrix33' a +translate2D v = SquareMatrix $ Triple + (Triple (1, 0, x) + ,Triple (0, 1, y) + ,Triple (0, 0, 1) + ) + where + Pair (x, y) = iso v + +-- | Given a 3D relative vector, produces a matrix that will translate by that +-- much (when you multiply a 3D point with it using multMatrixGen) +translate3D :: (Num a, IsomorphicVectors p Triple) => p a -> Matrix44' a +translate3D v = SquareMatrix $ Quad + (Quad (1, 0, 0, x) + ,Quad (0, 1, 0, y) + ,Quad (0, 0, 1, z) + ,Quad (0, 0, 0, 1) + ) + where + Triple (x, y, z) = iso v diff --git a/Data/SG/Shape.hs b/Data/SG/Shape.hs new file mode 100644 --- /dev/null +++ b/Data/SG/Shape.hs @@ -0,0 +1,330 @@ +-- 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, Eq a, Show 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, Eq a, Show 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, Eq a, Show 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, Eq a, Show 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, Eq a, Show 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, Eq a, Show 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, Eq a, Show 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, Eq a, Show 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, Eq a, Show 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 diff --git a/Data/SG/Test.hs b/Data/SG/Test.hs new file mode 100644 --- /dev/null +++ b/Data/SG/Test.hs @@ -0,0 +1,187 @@ +-- 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. + +-- | A test module (run main) +module Data.SG.Test where + +import Control.Arrow +import qualified Data.List as List +import Test.HUnit + +import Data.SG + +newtype Float' = Float' Float deriving (Ord, Enum, Num, Fractional, Floating, Show) +newtype Double' = Double' Double deriving (Ord, Enum, Num, Fractional, Floating, Show) + +instance Eq Float' where + a == b = abs (a - b) < 0.001 + +instance Eq Double' where + a == b = abs (a - b) < 0.001 + +class (Eq a, Floating a, Enum a, Ord a) => TestFloating a where + input :: [a] + input = [-10..10] + + angs :: [a] + angs = map (*(50/pi)) [0..100] + + testItems2 :: ((a,a) -> b) -> [b] + testItems2 f = [f (x, y) | x <- [-10..10], y <- [-10..10]] + + testItems3 :: ((a,a,a) -> b) -> [b] + testItems3 f = [f (x, y, z) | x <- [-10..10], y <- [-10..10], z <- [-10..10]] + + forRot2 :: ((b, b) -> IO ()) -> (((a,a) -> (a,a)) -> [(b,b)]) -> IO () + forRot2 f g = mapM_ f (concatMap g $ map onPair ops) + where + ops :: [Pair a -> Pair a] + ops = [\p -> multMatrix (rotate2D a) p | a <- angs] + + onPair f p = let Pair p' = f (Pair p) in p' + + forRot3 :: ((b, b) -> IO ()) -> (((a,a,a) -> (a,a,a)) -> [(b,b)]) -> IO () + forRot3 f g = mapM_ f (g id) + + test_mag2 :: a -> ((a, a) -> a) -> IO () + test_mag2 _ f = forRot2 (uncurry $ assertEqual "test_mag2") + $ \rot -> [(sqrt $ (x*x) + (y*y), f (rot (x,y))) | x <- input, y <- input] + + test_unit2 :: (Eq (p a), Show (p a), VectorNum p, Coord p) => + a -> ((a, a) -> p a) -> IO () + test_unit2 _ f = forRot2 (uncurry $ assertEqual "test_unit2") + $ \rot -> [let v = f (rot p) in (v, (fmapNum1 (* mag v) $ unitVector v)) | p <- testItems2 id] + + test_unit3 :: (Eq (p a), Show (p a), VectorNum p, Coord p) => + a -> ((a, a, a) -> p a) -> IO () + test_unit3 _ f = forRot3 (uncurry $ assertEqual "test_unit3") + $ \rot -> [let v = f (rot p) in (v, (fmapNum1 (* mag v) $ unitVector v)) | p <- testItems3 id] + + testRotId :: (Eq (p a), Show (p a), IsomorphicVectors Pair p, IsomorphicVectors + p Pair) => a -> ((a, a) -> p a) -> IO () + testRotId _ f = do mapM_ (uncurry $ assertEqual "testRotId") + $ map (id &&& (rotate2D 0 `multMatrix`)) $ testItems2 f + mapM_ (uncurry $ assertEqual "testRotId") + $ map (id &&& (rotate2D (2*pi) `multMatrix`)) $ testItems2 f + mapM_ (uncurry $ assertEqual "testRotId") + $ map (id &&& (rotate2D (4*pi) `multMatrix`)) $ testItems2 f + mapM_ (uncurry $ assertEqual "testRotId") + $ map (id &&& (rotate2D (-2*pi) `multMatrix`)) $ testItems2 f + mapM_ (uncurry $ assertEqual "testRotId-bothways") + [(rotate2D a `multMatrix` v + ,rotate2D (negate (2*pi - a)) `multMatrix` v + ) + | v <- testItems2 f, a <- angs] + mapM_ (uncurry $ assertEqual "testRotId-twice") + [(v,rotate2D a `multMatrix` (rotate2D (2*pi - a) `multMatrix` v)) + | v <- testItems2 f, a <- angs] + + testProject :: (VectorNum p, Coord p) => a -> ((a, a) -> p a) -> IO () + testProject _ f = do + forRot2 (uncurry $ assertEqual "testProject") $ + \rot -> [ let r = f $ rot (0 :: a, 1) + v = f (rot p) + in (snd p, v `projectOnto` r) + | p <- testItems2 id] + forRot2 (uncurry $ assertEqual "testProject") $ + \rot -> [ let r = f $ rot (0 :: a, -1) + v = f (rot p) + in (negate $ snd p, v `projectOnto` r) + | p <- testItems2 id] + + testReflect :: a -> IO () + testReflect _ = do + forRot2 (uncurry $ assertEqual "testReflect0") $ + \rot -> [ let r = makeRel2 $ rot (0 :: a, 1) + v = makeRel2 $ rot p + v' = makeRel2 $ rot $ second negate p + in (v', v `reflectAgainst2` r) + | p <- testItems2 id] + forRot2 (uncurry $ assertEqual "testReflect1") $ + \rot -> [ let r = makeRel2 $ rot (0 :: a, -1) + v = makeRel2 $ rot p + v' = makeRel2 $ rot $ second negate p + in (v', v `reflectAgainst2` r) + | p <- testItems2 id] + forRot2 (uncurry $ assertEqual "testReflect2") $ + \rot -> [ let r = makeRel2 $ rot (0 :: a, 1) + v = makeRel2 $ rot p + v' = makeRel2 $ rot $ second (\x -> if x > 0 then x else negate x) p + in (v', v `reflectAgainstIfNeeded2` r) + | p <- testItems2 id] + forRot2 (uncurry $ assertEqual "testReflect3") $ + \rot -> [ let r = makeRel2 $ rot (0 :: a, -1) + v = makeRel2 $ rot p + v' = makeRel2 $ rot $ second (\x -> if x < 0 then x else negate x) p + in (v', v `reflectAgainstIfNeeded2` r) + | p <- testItems2 id] + + testMatrixId :: (Matrix (SquareMatrix c), Num (SquareMatrix c a)) => SquareMatrix c a -> IO () + testMatrixId x + = do let id = fromMatrixComponents [] `asTypeOf` x + size = length (matrixComponents id) + groupInto n xs = take n xs : groupInto n (drop n xs) + ns = fromMatrixComponents $ groupInto size [1..] + assertEqual "testMatrixId 0" id (transpose id) + assertEqual "testMatrixId 1" id (id*id) + assertEqual "testMatrixId 2" id (id*id*transpose id*id) + assertEqual "testMatrixId 3" ns (transpose $ transpose $ ns) + assertEqual "testMatrixId 4" (transpose ns) + (fromMatrixComponents $ List.transpose $ matrixComponents $ ns) + assertEqual "testMatrixId 5" ns (id * ns) + assertEqual "testMatrixId 6" ns (ns * id) + + testAll :: a -> IO () + testAll x + = do test_mag2 x (mag . makeRel2) + test_mag2 x (mag . Point2) + test_mag2 x (mag . Pair) + test_unit2 x Point2 + test_unit2 x makeRel2 + test_unit2 x Pair + test_unit3 x Point3 + test_unit3 x makeRel3 + test_unit3 x Triple + testRotId x Point2 + testRotId x makeRel2 + testRotId x Pair + testMatrixId (undefined :: Matrix22' a) + testMatrixId (undefined :: Matrix33' a) + testMatrixId (undefined :: Matrix44' a) + testProject x Point2 + testProject x makeRel2 + testProject x Pair + testReflect x + + +instance TestFloating Float' +instance TestFloating Double' + +main :: IO () +main = do testAll (0 :: Float') + testAll (0 :: Double') diff --git a/Data/SG/Vector.hs b/Data/SG/Vector.hs new file mode 100644 --- /dev/null +++ b/Data/SG/Vector.hs @@ -0,0 +1,187 @@ +-- 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. + + +-- | The module with all the different type-classes for vectors. Generally, the +-- main functions you might need from this function are: +-- +-- * 'magSq' and 'mag' (defined for all vectors). +-- +-- * 'getX' and 'getY' (defined for all vectors) as well as 'getZ' (defined for +-- all vectors with 3 or more dimensions). +-- +-- * 'dotProduct', 'unitVector', 'averageVec', 'averageUnitVec', 'sameDirection', +-- 'projectOnto', 'projectPointOnto', 'distFrom' (defined for all vectors). +-- +-- * 'iso', which is defined for all combinations of vectors with the same number +-- of dimensions. +-- +-- The rest of the functions are mainly just wiring necessary for other functions, +-- but must be exported. +-- +-- As to the vector types, there are two methods to use this library. One is to +-- use the types from the "Data.SG.Vector.Basic" library, which support basic vector +-- operations. The other is to use the types from the "Data.SG.Geometry.TwoDim" +-- and "Data.SG.Geometry.ThreeDim" modules, where a position vector is differentiated +-- from a relative vector (to increase clarity of code, and help prevent errors +-- such as adding two points together). Both systems can be used with various +-- useful functions (involving lines too) from "Data.SG.Geometry". +module Data.SG.Vector where + +import Data.Foldable (Foldable, toList) + +-- | An isomorphism amongst vectors. Allows you to convert between two vectors +-- that have the same dimensions. You will notice that all the instances reflect +-- this. +class IsomorphicVectors from to where + iso :: Num a => from a -> to a + +instance IsomorphicVectors v v where + iso = id + + +-- | The class that is implemented by all vectors. +-- +-- Minimal implementation: fromComponents +class Foldable p => Coord p where + -- | Gets the components of the vector, in the order x, y (, z). + getComponents :: Num a => p a -> [a] + getComponents = toList + -- | Re-constructs a vector from the list of coordinates. If there are too few, + -- the rest will be filled with zeroes. If there are too many, the latter ones are + -- ignored. + fromComponents :: Num a => [a] -> p a + -- | Gets the magnitude squared of the vector. This should be fast for + -- repeated calls on 'Data.SG.Geometry.TwoDim.Rel2'' and + -- 'Data.SG.Geometry.ThreeDim.Rel3'', which cache this value. + magSq :: Num a => p a -> a + magSq = sum . map (\x -> x * x) . getComponents + + -- | Computes the dot product of the two vectors. + dotProduct :: Num a => p a -> p a -> a + dotProduct a b = sum $ zipWith (*) (getComponents a) (getComponents b) + +-- | This class is implemented by all 2D and 3D vectors, so 'getX' gets the X co-ordinate +-- of both 2D and 3D vectors. +class Coord p => Coord2 p where + getX :: p a -> a + getY :: p a -> a + +-- | This class is implemented by all 3D vectors. To get the X and Y components, +-- use 'getX' and 'getY' from 'Coord2'. +class Coord2 p => Coord3 p where + getZ :: p a -> a + +-- | The origin\/all-zero vector (can be used with any vector type you like) +origin :: (Coord p, Num a) => p a +origin = fromComponents $ repeat 0 + +-- | Gets the magnitude of the given vector. +mag :: (Coord p, Floating a) => p a -> a +mag = sqrt . magSq + +-- | Scales the vector so that it has length 1. Note that due to floating-point +-- inaccuracies and so on, mag (unitVector v) will not necessarily equal 1, but +-- it should be very close. If an all-zero vector is passed, the same will be +-- returned. +-- +-- This function should be very fast when called on +-- 'Data.SG.Geometry.TwoDim.Rel2'' and 'Data.SG.Geometry.ThreeDim.Rel3''; +-- vectors that are already unit vectors (no processing is done). +unitVector :: (Coord p, VectorNum p, Ord a, Floating a) => p a -> p a +unitVector v + | abs (magSq v - 1) < 0.000001 = v + | magSq v == 0 = v -- Avoid division by zero + | otherwise = fmapNum1 (/ mag v) v + +-- | Gets the average vector of all the given vectors. Essentially it is the +-- sum of the vectors, divided by the length, so @averageVec [Point2 (-3, 0), Point2 +-- (5,0)]@ will give @Point2 (1,0)@. If the list is empty, the +-- all-zero vector is returned. +averageVec :: (Fractional a, VectorNum p, Num (p a)) => [p a] -> p a +averageVec [] = 0 +averageVec vs = fmapNum1 (/ fromInteger (toInteger $ length vs)) (sum vs) + +-- | Like averageVec composed with unitVector -- gets the average of the +-- vectors in the list, and normalises the length. If the list is empty, the all-zero +-- vector is returned (which is therefore not a unit vector). Similarly, +-- if the average of all the vectors is all-zero, the all-zero vector will be returned. +averageUnitVec :: (Floating a, Ord a, Coord p, VectorNum p, Num (p a)) => [p a] -> p a +averageUnitVec [] = 0 +averageUnitVec vs = unitVector $ sum vs + +-- | Works out if the two vectors are in the same direction (to within a small +-- tolerance). +sameDirection :: (VectorNum rel, Coord rel, Ord a, Floating a) => rel a -> rel a -> Bool +sameDirection v w + = all (< 0.000001) diffs + where + diffs = map abs $ zipWith (-) (getComponents $ unitVector v) (getComponents $ unitVector w) + +-- | Gives back the vector (first parameter), translated onto given axis (second +-- parameter). Note that the scale is always distance, /not/ related to the size +-- of the axis vector. +projectOnto :: (Floating a, Ord a, VectorNum rel, Coord rel) => rel a -> rel a -> a +projectOnto v axis = (v `dotProduct` unitVector axis) + +-- | Projects the first parameter onto the given axes (X, Y), returning a point +-- in terms of the new axes. +projectOnto2 :: (Floating a, Ord a, VectorNum rel, Coord rel) => + rel a -> (rel a, rel a) -> rel a +projectOnto2 v (axisX, axisY) + = fromComponents [v `projectOnto` axisX, v `projectOnto` axisY] + +-- | Gives back the point (first parameter), translated onto given axis (second +-- parameter). Note that the scale is always distance, /not/ related to the size +-- of the axis vector. +projectPointOnto :: (Floating a, Ord a, VectorNum rel, Coord rel, IsomorphicVectors pt rel) => pt a -> rel a -> a +projectPointOnto pt = projectOnto (iso pt) + +-- | Projects the point (first parameter) onto the given axes (X, Y), returning a point +-- in terms of the new axes. +projectPointOnto2 :: (Floating a, Ord a, VectorNum rel, Coord rel, IsomorphicVectors + pt rel, Coord pt) => pt a -> (rel a, rel a) -> pt a +projectPointOnto2 v (axisX, axisY) + = fromComponents [v `projectPointOnto` axisX, v `projectPointOnto` axisY] + +-- | Works out the distance between two points. +distFrom :: (VectorNum pt, Coord pt, Floating a) => pt a -> pt a -> a +distFrom v0 v1 = mag $ fmapNum2 (-) v0 v1 + +-- | A modified version of 'Functor' and 'Control.Applicative.Applicative' that adds the 'Num' +-- constraint on the result. You are unlikely to need to use this class much +-- directly. Some vectors have 'Functor' and 'Control.Applicative.Applicative' instances anyway. +class VectorNum f where + -- | Like 'fmap', but with a 'Num' constraint. + fmapNum1 :: Num b => (a -> b) -> f a -> f b + -- | Like 'Control.Applicative.liftA2', but with a 'Num' constraint. + fmapNum2 :: Num c => (a -> b -> c) -> f a -> f b -> f c + -- | Like 'fmapNum1', but can only be used if you won't change the magnitude: + fmapNum1inv :: Num a => (a -> a) -> f a -> f a + -- | Like 'Control.Applicative.pure' (or 'fromInteger') but with a 'Num' constraint. + simpleVec :: Num a => a -> f a diff --git a/Data/SG/Vector/Basic.hs b/Data/SG/Vector/Basic.hs new file mode 100644 --- /dev/null +++ b/Data/SG/Vector/Basic.hs @@ -0,0 +1,192 @@ +-- 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. + + +-- | Some types that are very basic vectors. Most of the use that can be made +-- of the vectors is in their type-class instances, which support a powerful set +-- of operations. For example: +-- +-- > fmap (*3) v -- Scales vector v by 3 +-- > pure 0 -- Creates a vector filled with zeroes +-- > v + w -- Adds two vectors (there is a 'Num' instance, basically) +-- +-- Plus all the instances for the classes in "Data.SG.Vector", which allows you +-- to use 'getX' and so on. +-- +-- You will probably want to create more friendly type synonyms, such as: +-- +-- > type Vector2 = Pair Double +-- > type Vector3 = Triple Double +-- > type Line2 = LinePair Double +-- > type Line3 = LineTriple Double +module Data.SG.Vector.Basic where + +import Control.Applicative +import Data.Foldable +import Data.Traversable + +import Data.SG.Vector + +-- | A pair, which acts as a 2D vector. +newtype Pair a = Pair (a, a) + deriving (Eq, Ord, Show, Read) +-- | A triple, which acts as a 3D vector. +newtype Triple a = Triple (a, a, a) + deriving (Eq, Ord, Show, Read) +-- | A quad, which acts as a 4D vector. +newtype Quad a = Quad (a, a, a, a) + deriving (Eq, Ord, Show, Read) + +-- | A pair of (position vector, direction vector) to be used as a 2D line. +newtype LinePair a = LinePair (Pair a, Pair a) + deriving (Eq, Ord, Show, Read) +-- | A pair of (position vector, direction vector) to be used as a 3D line. +newtype LineTriple a = LineTriple (Triple a, Triple a) + deriving (Eq, Ord, Show, Read) + +instance VectorNum Pair where + fmapNum1 = fmap + fmapNum1inv = fmap + fmapNum2 = liftA2 + simpleVec = pure + +instance VectorNum Triple where + fmapNum1 = fmap + fmapNum1inv = fmap + fmapNum2 = liftA2 + simpleVec = pure + +instance VectorNum Quad where + fmapNum1 = fmap + fmapNum1inv = fmap + fmapNum2 = liftA2 + simpleVec = pure + +instance (Show a, Eq a, Num a) => Num (Pair a) where + (+) = fmapNum2 (+) + (-) = fmapNum2 (-) + (*) = fmapNum2 (*) + abs = fmapNum1inv abs + signum = fmapNum1 signum + negate = fmapNum1inv negate + fromInteger = simpleVec . fromInteger + +instance (Show a, Eq a, Num a) => Num (Triple a) where + (+) = fmapNum2 (+) + (-) = fmapNum2 (-) + (*) = fmapNum2 (*) + abs = fmapNum1inv abs + signum = fmapNum1 signum + negate = fmapNum1inv negate + fromInteger = simpleVec . fromInteger + +instance (Show a, Eq a, Num a) => Num (Quad a) where + (+) = fmapNum2 (+) + (-) = fmapNum2 (-) + (*) = fmapNum2 (*) + abs = fmapNum1inv abs + signum = fmapNum1 signum + negate = fmapNum1inv negate + fromInteger = simpleVec . fromInteger + +instance Applicative Pair where + pure a = Pair (a, a) + (<*>) (Pair (fa, fb)) (Pair (a, b)) = Pair (fa a, fb b) + +instance Foldable Pair where + foldr f t (Pair (x, y)) = x `f` (y `f` t) + +instance Traversable Pair where + traverse f (Pair (x, y)) = Pair <$> liftA2 (,) (f x) (f y) + +instance Applicative Triple where + pure a = Triple (a, a, a) + (<*>) (Triple (fa, fb, fc)) (Triple (a, b, c)) = Triple (fa a, fb b, fc c) + +instance Foldable Triple where + foldr f t (Triple (x, y, z)) = x `f` (y `f` (z `f` t)) + +instance Traversable Triple where + traverse f (Triple (x, y, z)) = Triple <$> liftA3 (,,) (f x) (f y) (f z) + +instance Applicative Quad where + pure a = Quad (a, a, a, a) + (<*>) (Quad (fa, fb, fc, fd)) (Quad (a, b, c, d)) + = Quad (fa a, fb b, fc c, fd d) + +instance Foldable Quad where + foldr f t (Quad (x, y, z, a)) = x `f` (y `f` (z `f` (a `f` t))) + +instance Traversable Quad where + traverse f (Quad (x, y, z, a)) = Quad <$> ((,,,) <$> f x <*> f y <*> f z <*> f a) + + +instance Functor Pair where + fmap = fmapDefault + +instance Functor Triple where + fmap = fmapDefault + +instance Functor Quad where + fmap = fmapDefault + +instance Coord Pair where + getComponents (Pair (a, b)) = [a, b] + fromComponents (a:b:_) = Pair (a, b) + fromComponents xs = fromComponents $ xs ++ repeat 0 + +instance Coord2 Pair where + getX (Pair (a, _)) = a + getY (Pair (_, b)) = b + +instance Coord Triple where + getComponents (Triple (a, b, c)) = [a, b, c] + fromComponents (a:b:c:_) = Triple (a, b, c) + fromComponents xs = fromComponents $ xs ++ repeat 0 + +instance Coord2 Triple where + getX (Triple (a, _, _)) = a + getY (Triple (_, b, _)) = b + +instance Coord3 Triple where + getZ (Triple (_, _, c)) = c + + +instance Coord Quad where + getComponents (Quad (a, b, c, d)) = [a, b, c, d] + fromComponents (a:b:c:d:_) = Quad (a, b, c, d) + fromComponents xs = fromComponents $ xs ++ repeat 0 + +instance Coord2 Quad where + getX (Quad (a, _, _, _)) = a + getY (Quad (_, b, _, _)) = b + +instance Coord3 Quad where + getZ (Quad (_, _, c, _)) = c + + diff --git a/LICENSE b/LICENSE new file mode 100644 --- /dev/null +++ b/LICENSE @@ -0,0 +1,26 @@ +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. diff --git a/SGplus.cabal b/SGplus.cabal new file mode 100644 --- /dev/null +++ b/SGplus.cabal @@ -0,0 +1,35 @@ +Name: SGplus +Version: 1.1 +Synopsis: (updated) Small geometry library for dealing with vectors and collision detection +License: BSD3 +License-file: LICENSE +Author: Neil Brown +Maintainer: jeremy@praeceptamachinae.com +Copyright: Copyright (c) 2009, Neil Brown +Stability: Provisional +Description: Updated original SG to work with modern GHC. + A small geometry library for dealing with + vectors, points, lines, simple shapes, and their + various intersection tests. See also the SGdemo project + () + for an example of using the module. +Tested-with: GHC==8.0.1 +Build-Type: Simple +Category: Data, Math +Cabal-Version: >=1.2 +Extra-source-files: Data/SG/Test.hs + + +Library + Build-Depends: base >=1.0 && < 1000000000.0, mtl + Exposed-modules: Data.SG + Data.SG.Geometry + Data.SG.Geometry.TwoDim + Data.SG.Geometry.ThreeDim + Data.SG.Matrix + Data.SG.Shape + Data.SG.Vector + Data.SG.Vector.Basic + ghc-options: -Wall + Extensions: MultiParamTypeClasses FlexibleInstances FunctionalDependencies + ScopedTypeVariables FlexibleContexts diff --git a/Setup.lhs b/Setup.lhs new file mode 100644 --- /dev/null +++ b/Setup.lhs @@ -0,0 +1,5 @@ +#! /usr/bin/env runhaskell + +> import Distribution.Simple +> main = defaultMain +