381 lines
14 KiB
Haskell
381 lines
14 KiB
Haskell
{-# LANGUAGE BangPatterns #-}
|
|
module Geometry
|
|
( module Geometry
|
|
, module Geometry.Data
|
|
, module Geometry.Intersect
|
|
, module Geometry.Bezier
|
|
, module Geometry.Vector
|
|
)
|
|
where
|
|
import Geometry.Data
|
|
import Geometry.Intersect
|
|
import Geometry.Bezier
|
|
import Geometry.Vector
|
|
|
|
import Data.Function
|
|
import Data.List
|
|
import Data.Maybe
|
|
|
|
import Control.Applicative
|
|
-- TODO add bang patterns
|
|
|
|
alongLineBy :: Float -> Point2 -> Point2 -> Point2
|
|
alongLineBy x a b = a +.+ y *.* normalizeV (b -.- a)
|
|
where
|
|
y = min x $ dist a b
|
|
|
|
|
|
closestPointOnLine :: Point2 -> Point2 -> Point2 -> Point2
|
|
{-# INLINE closestPointOnLine #-}
|
|
closestPointOnLine a b p
|
|
= a +.+ u *.* (b -.- a)
|
|
where u = closestPointOnLineParam a b p
|
|
|
|
closestPointOnLineParam :: Point2 -> Point2 -> Point2 -> Float
|
|
{-# INLINE closestPointOnLineParam #-}
|
|
closestPointOnLineParam a b p
|
|
= (p -.- a) `dotV` (b -.- a) / (b -.- a) `dotV` (b -.- a)
|
|
|
|
|
|
|
|
-- the following helper draws a rectangle based on maximal N E S W values
|
|
rectNESW :: Float -> Float -> Float -> Float -> [Point2]
|
|
rectNESW a b c d = [(b,a),(b,c),(d,c),(d,a)
|
|
]
|
|
rectNSEW :: Float -> Float -> Float -> Float -> [Point2]
|
|
rectNSEW n s e w = rectNESW n e s w
|
|
|
|
rectNSWE :: Float -> Float -> Float -> Float -> [Point2]
|
|
rectNSWE n s w e = [ (w,n), (w,s), (e,s), (e,n)]
|
|
|
|
-- -- the following filters points in a polygon: supposes the points in the
|
|
-- polygon are listed in anticlockwise order
|
|
pointInOrOnPolygon :: Point2 -> [Point2] -> Bool
|
|
pointInOrOnPolygon p (x:xs) = all (\l -> not (uncurry isRHS l p)) $ zip (x:xs) (xs ++ [x])
|
|
|
|
|
|
pointInPolygon :: Point2 -> [Point2] -> Bool
|
|
pointInPolygon p [] = False
|
|
pointInPolygon p (x:xs) = all (\l -> uncurry (errorIsLHS 1) l p) $ zip (x:xs) (xs ++ [x])
|
|
|
|
errorPointInPolygon :: Int -> Point2 -> [Point2] -> Bool
|
|
errorPointInPolygon i p xs | length xs == 1 = error "one point polygon"
|
|
| length xs == 2 = error "two point polygon"
|
|
| nub xs == xs = pointInPolygon p xs
|
|
| otherwise = error $ "errorPointInPolygon "++ show i
|
|
|
|
errorNormalizeV :: Int -> Point2 -> Point2
|
|
errorNormalizeV i (0,0) = error $ "problem with function: errorNormalizeV "++show i
|
|
errorNormalizeV i p = normalizeV p
|
|
|
|
errorAngleVV :: Int -> Point2 -> Point2 -> Float
|
|
errorAngleVV i (0,0) _ = error $ "problem with function: errorAngleVV "++show i
|
|
errorAngleVV i _ (0,0) = error $ "problem with function: errorAngleVV "++show i
|
|
errorAngleVV i p p' = angleVV p p'
|
|
|
|
errorIsLHS :: Int -> Point2 -> Point2 -> Point2 -> Bool
|
|
errorIsLHS i x y | x == y = error $ "problem with function: errorIsLHS "
|
|
++show i
|
|
| otherwise = isLHS x y
|
|
|
|
errorClosestPointOnLine :: Int -> Point2 -> Point2 -> Point2 -> Point2
|
|
errorClosestPointOnLine i x y | x == y = error $ "problem with function: errorClosestPointOnLine "
|
|
++show i
|
|
| otherwise = closestPointOnLine x y
|
|
|
|
errorClosestPointOnLineParam :: Int -> Point2 -> Point2 -> Point2 -> Float
|
|
errorClosestPointOnLineParam i x y z | x == y = dist x z
|
|
-- error $ "problem with function: errorClosestPointOnLineParam " ++show i
|
|
| otherwise = closestPointOnLineParam x y z
|
|
|
|
safeNormalizeV :: Point2 -> Point2
|
|
safeNormalizeV (0,0) = (0,0)
|
|
safeNormalizeV p = normalizeV p
|
|
|
|
-- tests whether a point is on the LHS of a line
|
|
-- this has been called somewhere with l1 == l2
|
|
isLHS :: Point2 -> Point2 -> Point2 -> Bool
|
|
{-# INLINE isLHS #-}
|
|
isLHS' :: (Float, Float) -> (Float, Float) -> Point2 -> Bool
|
|
isLHS' l1 l2 p | l1 == l2 = False
|
|
| otherwise = closestPointOnLineParam l1 (l1 +.+ vNormal (l2 -.- l1)) p < 0
|
|
|
|
isLHS (x,y) (x',y') (x'',y'')
|
|
| (x,y) == (x',y') = False
|
|
| otherwise = a1 * b2 - a2 * b1 > 0
|
|
where a1 = x' - x
|
|
a2 = y' - y
|
|
b1 = x'' - x
|
|
b2 = y'' - y
|
|
|
|
isRHS :: Point2 -> Point2 -> Point2 -> Bool
|
|
{-# INLINE isRHS #-}
|
|
isRHS (x,y) (x',y') (x'',y'')
|
|
| (x,y) == (x',y') = False
|
|
| otherwise = a1 * b2 - a2 * b1 < 0
|
|
where a1 = x' - x
|
|
a2 = y' - y
|
|
b1 = x'' - x
|
|
b2 = y'' - y
|
|
--isRHS l1 l2 p = closestPointOnLineParam l1 (l1 +.+ vNormal (l2 -.- l1)) p > 0
|
|
|
|
-- reorders points to be anticlockwise around their center
|
|
orderPolygon :: [Point2] -> [Point2]
|
|
orderPolygon [] = []
|
|
orderPolygon ps = sortBy (compare `on` \p -> argV (p -.- cen)) ps
|
|
where cen = 1/ fromIntegral (length ps) *.* foldr1 (+.+) ps
|
|
|
|
dist :: Point2 -> Point2 -> Float
|
|
{-# INLINE dist #-}
|
|
dist p1 p2 = magV (p2 -.- p1)
|
|
|
|
|
|
pHalf :: Point2 -> Point2 -> Point2
|
|
pHalf a b = 0.5 *.* (a +.+ b)
|
|
|
|
circOnLine' :: Point2 -> Point2 -> Point2 -> Float -> Bool
|
|
circOnLine' p1 p2 c rad = isJustTrue (fmap (\p -> magV (p -.- c) < rad) y)
|
|
where y = intersectSegLine' p1 p2 c (c +.+ vNormal (p1 -.- p2))
|
|
isJustTrue (Just True) = True
|
|
isJustTrue _ = False
|
|
|
|
circOnLine :: Point2 -> Point2 -> Point2 -> Float -> Bool
|
|
circOnLine p1 p2 c rad = magV (p1 -.- c) <= rad || magV (p2 -.- c) <= rad
|
|
|| isJustTrue (fmap (\p -> magV (p -.- c) < rad) y)
|
|
where y = intersectSegLine' p1 p2 c (c +.+ vNormal (p1 -.- p2))
|
|
isJustTrue (Just True) = True
|
|
isJustTrue _ = False
|
|
|
|
difference :: (Ord a, Num a) => a -> a -> a
|
|
difference x y | x > y = x - y
|
|
| otherwise = y - x
|
|
|
|
reflectIn :: Point2 -> Point2 -> Point2
|
|
reflectIn line vec = let angle = 2 * angleBetween line vec
|
|
in rotateV angle vec
|
|
|
|
angleBetween :: Point2 -> Point2 -> Float
|
|
angleBetween v1 v2 = argV v1 - argV v2
|
|
|
|
doublePair :: (a,a) -> [(a,a)]
|
|
doublePair (x,y) = [(x,y),(y,x)]
|
|
|
|
polysIntersect :: [Point2] -> [Point2] -> Bool
|
|
polysIntersect (p:ps) (q:qs)
|
|
= any isJust $ (\(a,b) (c,d) -> myIntersectSegSeg a b c d) <$> pairs1 <*> pairs2
|
|
where pairs1 = zip (p:ps) (ps++[p])
|
|
pairs2 = zip (q:qs) (qs++[q])
|
|
polysIntersect [] _ = False
|
|
polysIntersect _ [] = False
|
|
|
|
anyPolyssIntersect :: [[Point2]] -> [[Point2]] -> Bool
|
|
anyPolyssIntersect x y = or $ polysIntersect <$> x <*> y
|
|
|
|
nRays :: Int -> [Point2]
|
|
nRays n = take n $ iterate (rotateV (2*pi/fromIntegral n)) (600,0)
|
|
|
|
nRaysRad :: Int -> Float -> [Point2]
|
|
nRaysRad n x = take n $ iterate (rotateV (2*pi/fromIntegral n)) (x,0)
|
|
|
|
-- angles go from 0 to 2pi, need to work out what is left of another
|
|
|
|
isLeftOfA :: Float -> Float -> Bool
|
|
isLeftOfA angle1 angle2 = (angle1 - angle2 < pi && angle1 > angle2)
|
|
|| (angle2 - angle1 > pi && angle2 > angle1)
|
|
|
|
isLeftOf :: Point2 -> Point2 -> Bool
|
|
isLeftOf x y = isLeftOfA (argV x) (argV y)
|
|
|
|
-- diffAngles has an issue...
|
|
|
|
diffAngles :: Float -> Float -> Float
|
|
diffAngles x y | diff > pi = diffAngles (x - 2*pi) y
|
|
| diff >= 0 = diff
|
|
| diff > -pi = -diff
|
|
| otherwise = diffAngles (x + 2*pi) y
|
|
where diff = x-y
|
|
differenceAngles = diffAngles
|
|
angleDifference = diffAngles
|
|
|
|
-- given a triangle where we know the length of a first side,
|
|
-- the length of a second side, and the angle between the first side and the
|
|
-- third side, finds the length of the third side
|
|
-- not this doesn't necessarily find ALL solutions, asin is a map not a function
|
|
ssaTri :: Float -> Float -> Float -> Float
|
|
ssaTri ab bc a
|
|
| sin a == 0 = 0
|
|
| bc == 0 = ab
|
|
| otherwise = let c = asin ( (ab * sin a)/bc)
|
|
b = pi - (a + c)
|
|
in sin b * bc / sin a
|
|
|
|
-- fix points: we now fix the triangle in the coordinate system, and return a
|
|
-- third unknown point:
|
|
-- the point which lies between pa and pc' on a line from b of length bc
|
|
-- note that there are likely two such points, this seems to return the point
|
|
-- closer to pc'
|
|
|
|
ssaTriPoint :: Point2 -> Point2 -> Point2 -> Float -> Point2
|
|
ssaTriPoint pa pb pc' bc
|
|
= let ab = magV (pa -.- pb)
|
|
a = errorAngleVV 6 (pb -.- pa) (pc' -.- pa)
|
|
ac = ssaTri ab bc a
|
|
in pa +.+ (ac *.* errorNormalizeV 47 (pc' -.- pa))
|
|
|
|
-- the above SHOULD return a Maybe Point...
|
|
ssaTriPoint' :: Point2 -> Point2 -> Point2 -> Float -> Maybe Point2
|
|
ssaTriPoint' pa pb pc' bc
|
|
| dist pb (closestPointOnSeg pa pc' pb) >= bc
|
|
= Nothing
|
|
| otherwise
|
|
= Just $ ssaTriPoint pa pb pc' bc
|
|
ssaTriPointCorrect :: Point2 -> Point2 -> Point2 -> Float -> Maybe Point2
|
|
ssaTriPointCorrect pa pb pc' bc
|
|
| param <= 1 && param >= 0 = Just p
|
|
| otherwise = Nothing
|
|
where p = ssaTriPoint pa pb pc' bc
|
|
param = closestPointOnLineParam pa pc' p
|
|
|
|
|
|
closestPointOnSeg :: Point2 -> Point2 -> Point2 -> Point2
|
|
closestPointOnSeg segP1 segP2 p
|
|
| errorClosestPointOnLineParam 3 segP1 segP2 p <= 0 = segP1
|
|
| errorClosestPointOnLineParam 4 segP1 segP2 p >= 1 = segP2
|
|
| otherwise = errorClosestPointOnLine 2 segP1 segP2 p
|
|
|
|
pointInCircle :: Point2 -> Float -> Point2 -> Maybe Point2
|
|
pointInCircle p r c | p == c = Just p
|
|
| magV (p -.- c) < r = Just p
|
|
| otherwise = Nothing
|
|
|
|
--determines if a moving point intersects with a circle,
|
|
--if so, returns a point on circle that intersects with the line passing
|
|
--throught the circle : HOPEFULLY THE CORRECT OF THE TWO!
|
|
|
|
collidePointCirc :: Point2 -> Point2 -> Float -> Point2 -> Maybe Point2
|
|
collidePointCirc p1 p2 rad c = ssaTriPoint' p2 c p1 rad
|
|
|
|
-- changes the point to a measure of the distance
|
|
collidePointCirc' :: Point2 -> Point2 -> Float -> Point2 -> Maybe Float
|
|
collidePointCirc' p1 p2 rad c = fmap (\x -> magV (x -.- p1))
|
|
(collidePointCirc p1 p2 rad c)
|
|
|
|
--returns both the point and the measure of the distance
|
|
collidePointCirc'' :: Point2 -> Point2 -> Float -> Point2 -> Maybe (Point2,Float)
|
|
collidePointCirc'' p1 p2 rad c = (,) <$> collidePointCirc p1 p2 rad c
|
|
<*> collidePointCirc' p1 p2 rad c
|
|
|
|
collidePointCircCorrect :: Point2 -> Point2 -> Float -> Point2 -> Maybe Point2
|
|
collidePointCircCorrect p1 p2 rad c = ssaTriPointCorrect p2 c p1 rad
|
|
|
|
|
|
-- finds the height of a triangle using herons formula
|
|
-- the base is the line between the first two points
|
|
heron :: Point2 -> Point2 -> Point2 -> Float
|
|
heron x y z | x == y = 0
|
|
| otherwise = let a = magV $ x -.- y
|
|
b = magV $ y -.- z
|
|
c = magV $ z -.- x
|
|
s = (a+b+c)/2
|
|
area = sqrt(s*(s-a)*(s-b)*(s-c))
|
|
in 2*area/a
|
|
-- multiplies reflection in normal by factor
|
|
reflectInParam :: Float -> Point2 -> Point2 -> Point2
|
|
reflectInParam x line vec = let angle = 2 * angleBetween line vec
|
|
rAng = rotateV angle vec
|
|
p = x *.* errorClosestPointOnLine 3 (0,0) (vNormal line) rAng
|
|
in rAng -.- p
|
|
|
|
|
|
reflectIn' :: Point2 -> Point2 -> Point2 -> Point2 -> Point2
|
|
reflectIn' l1 l2 v1 v2 = v1 +.+ reflectIn (l1 -.- l2) (v2 -.- v1)
|
|
|
|
isOnLine :: Point2 -> Point2 -> Point2 -> Bool
|
|
isOnLine l1 l2 p = errorClosestPointOnLineParam 10 l1 (l1 +.+ vNormal (l2 -.- l1)) p == 0
|
|
&& errorClosestPointOnLineParam 11 l1 l2 p <= 1
|
|
&& errorClosestPointOnLineParam 12 l1 l2 p >= 0
|
|
|
|
-- the take 5000 here is a hack, otherwise divideLine seems to sometimes
|
|
-- generate an infinite list, and I don't know why
|
|
divideLine :: Float -> Point2 -> Point2 -> [Point2]
|
|
--divideLine x a b = map (\i -> a +.+ (i / (fromIntegral numPoints)) *.* (b -.- a))
|
|
divideLine x a b = take 5000
|
|
$ map (\i -> a +.+ (fromIntegral i / fromIntegral numPoints *.* (b -.- a)) )
|
|
ns
|
|
where
|
|
d = dist a b
|
|
numPoints = max 1 $ ceiling $ d / x
|
|
ns = [0 .. numPoints]
|
|
|
|
divideLineOddNumPoints :: Float -> Point2 -> Point2 -> [Point2]
|
|
--divideLine x a b = map (\i -> a +.+ (i / (fromIntegral numPoints)) *.* (b -.- a))
|
|
divideLineOddNumPoints x a b = take 5000
|
|
$ map (\i -> a +.+ (fromIntegral i / fromIntegral numPoints *.* (b -.- a)) )
|
|
ns
|
|
where
|
|
d = dist a b
|
|
numPoints' = max 1 $ ceiling $ d / x
|
|
numPoints | even numPoints' = numPoints'
|
|
| otherwise = numPoints' + 1
|
|
ns = [0 .. numPoints]
|
|
|
|
-- pulled the following from the haskell wiki
|
|
-- it seems to produce an infinite loop sometimes
|
|
-- fuck that, don't trust random code on the internet
|
|
bresenham :: (Int,Int) -> (Int,Int) -> [(Int,Int)]
|
|
{-# INLINE bresenham #-}
|
|
bresenham pa@(xa,ya) pb@(xb,yb) = map maySwitch . unfoldr go $ (x1,y1,0)
|
|
where
|
|
steep = abs (yb - ya) > abs (xb - xa)
|
|
maySwitch = if steep then (\(x,y) -> (y,x)) else id
|
|
[(x1,y1),(x2,y2)] = sort [maySwitch pa, maySwitch pb]
|
|
deltax = x2 - x1
|
|
deltay = abs (y2 - y1)
|
|
ystep = if y1 < y2 then 1 else -1
|
|
go (xTemp, yTemp, error)
|
|
| xTemp > x2 = Nothing
|
|
| otherwise = Just ((xTemp, yTemp), (xTemp + 1, newY, newError))
|
|
where
|
|
tempError = error + deltay
|
|
(newY, newError) = if (2*tempError) >= deltax
|
|
then (yTemp+ystep,tempError-deltax)
|
|
else (yTemp,tempError)
|
|
|
|
digitalLine :: (Int,Int) -> (Int,Int) -> [(Int,Int)]
|
|
digitalLine (x1,y1) (x2,y2)
|
|
| abs (x1-x2) > abs (y1-y2) = [ (x,( (y1-y2) * x + x1*y2 - x2*y1) `rdiv` (x1-x2) )
|
|
| x <- intervalList x1 x2 ]
|
|
| otherwise = [ ( ((x1-x2) * y + y1*x2 - y2*x1) `rdiv` (y1-y2) , y)
|
|
| y <- intervalList y1 y2 ]
|
|
where
|
|
rdiv a b = round $ fromIntegral a / fromIntegral b
|
|
|
|
intervalList :: Int -> Int -> [Int]
|
|
intervalList x y
|
|
| y >= x = [x .. y]
|
|
| otherwise = reverse [y..x]
|
|
|
|
divideCircle :: Float -> Point2 -> Float -> [Point2]
|
|
divideCircle x cen rad = map (cen +.+) $ nPointsOnCirc n rad
|
|
where
|
|
n = ceiling $ rad * 2 * pi / x
|
|
|
|
nPointsOnCirc :: Int -> Float -> [Point2]
|
|
nPointsOnCirc n rad = take n $ iterate (rotateV (2*pi/fromIntegral n)) (rad,0)
|
|
|
|
lineInPolygon :: Point2 -> Point2 -> [Point2] -> Bool
|
|
lineInPolygon a b ps = pointInPolygon a ps || pointInPolygon b ps
|
|
|| any (isJust . uncurry (intersectSegSeg' a b)) pss
|
|
where pss = zip ps (tail ps ++ [head ps])
|
|
|
|
makeLoopPairs :: [Point2] -> [(Point2,Point2)]
|
|
makeLoopPairs [] = error "tried to make loop with empty list of points"
|
|
makeLoopPairs [x] = error "tried to make loop with singleton list of points"
|
|
makeLoopPairs (x:xs) = zip (x:xs) (xs ++ [x])
|
|
|
|
-- note the pair is ordered
|
|
-- doesn't work for obtuse angles
|
|
pointIsInCone :: Point2 -> (Point2,Point2) -> Point2 -> Bool
|
|
pointIsInCone c (rightp,leftp) p = isLHS c rightp p && isLHS leftp c p
|
|
|