module Wumpus.Basic.Geometry.Intersection
(
interLineLine
, interLinesegLineseg
, interLinesegLine
, interCurveLine
, findIntersect
, makePlane
)
where
import Wumpus.Basic.Geometry.Base
import Wumpus.Core
import Data.AffineSpace
interLineLine :: Fractional u
=> (Point2 u, Point2 u) -> (Point2 u, Point2 u)
-> Maybe (Point2 u)
interLineLine (p1,p2) (q1,q2) =
if det_co == 0 then Nothing
else Just $ P2 (det_xm / det_co) (det_ym / det_co)
where
LineEquation a1 b1 c1 = lineEquation p1 p2
LineEquation a2 b2 c2 = lineEquation q1 q2
coeffM = M2'2 a1 b1 a2 b2
det_co = det2'2 coeffM
xM = M2'2 (negate c1) b1 (negate c2) b2
det_xm = det2'2 xM
yM = M2'2 a1 (negate c1) a2 (negate c2)
det_ym = det2'2 yM
interLinesegLineseg :: (Fractional u, Ord u, Tolerance u)
=> LineSegment u -> LineSegment u -> Maybe (Point2 u)
interLinesegLineseg a@(LineSegment p q) b@(LineSegment s t) =
interLineLine (p,q) (s,t) >>= segcheck
where
segcheck = mbCheck (\pt -> withinPoints pt a && withinPoints pt b)
interLinesegLine :: (Fractional u, Ord u, Tolerance u)
=> LineSegment u -> (Point2 u, Point2 u) -> Maybe (Point2 u)
interLinesegLine a@(LineSegment p q) line =
interLineLine (p,q) line >>= segcheck
where
segcheck = mbCheck (\pt -> withinPoints pt a)
mbCheck :: (a -> Bool) -> a -> Maybe a
mbCheck test a = if test a then Just a else Nothing
withinPoints :: (Ord u, Fractional u, Tolerance u)
=> Point2 u -> LineSegment u -> Bool
withinPoints (P2 x y) (LineSegment (P2 x0 y0) (P2 x1 y1)) =
between x (ordpair x0 x1) && between y (ordpair y0 y1)
where
ordpair a b = (min a b, max a b)
between a (s,t) = (s `tGT` a) && (a `tGT` t)
pve :: (Fractional u, Ord u, Tolerance u) => u -> Bool
pve a = a > eq_tolerance
nve :: (Fractional u, Ord u, Tolerance u) => u -> Bool
nve a = a < (negate eq_tolerance)
interCurveLine :: (Floating u , Ord u, Tolerance u)
=> BezierCurve u -> (Point2 u, Point2 u) -> Maybe (Point2 u)
interCurveLine c0 (p,q) = step c0
where
eqline = lineEquation p q
step c = case cut c eqline of
Left pt -> Just pt
Right False -> Nothing
Right True -> let (a,b) = subdivide c
in case step a of
Just pt -> Just pt
Nothing -> step b
cut :: (Floating u , Ord u, Tolerance u)
=> BezierCurve u -> LineEquation u -> Either (Point2 u) Bool
cut (BezierCurve p0 p1 p2 p3) line =
if d0 `tEQ` 0 then Left p0 else
if d3 `tEQ` 0 then Left p3 else
let ds = [d0,d1,d2,d3] in Right $ not $ all pve ds || all nve ds
where
d0 = pointLineDistance p0 line
d1 = pointLineDistance p1 line
d2 = pointLineDistance p2 line
d3 = pointLineDistance p3 line
findIntersect :: (Floating u, Real u, Ord u, Tolerance u)
=> Point2 u -> Radian -> [LineSegment u]
-> Maybe (Point2 u)
findIntersect radial_ogin ang = step
where
plane = makePlane radial_ogin ang
step [] = Nothing
step (x:xs) = case interLinesegLine x plane of
Just pt | quadrantCheck ang radial_ogin pt -> Just pt
_ -> step xs
quadrantCheck :: (Real u, Floating u)
=> Radian -> Point2 u -> Point2 u -> Bool
quadrantCheck theta ctr pt = theta == lineAngle ctr pt
makePlane :: Floating u => Point2 u -> Radian -> (Point2 u, Point2 u)
makePlane radial_ogin ang = (radial_ogin, radial_ogin .+^ avec ang 100)