module Algorithms.Geometry.PolyLineSimplification.ImaiIri
( simplify
, simplifyWith
) where
import Algorithms.Graph.BFS (bfs')
import Control.Lens
import Data.Ext
import qualified Data.Foldable as F
import Data.Geometry.LineSegment
import Data.Geometry.Point
import Data.Geometry.PolyLine
import Data.Geometry.Vector
import qualified Data.LSeq as LSeq
import Data.List.NonEmpty (NonEmpty(..))
import qualified Data.List.NonEmpty as NonEmpty
import qualified Data.Sequence as Seq
import Data.Tree
import qualified Data.Vector as V
import Witherable
simplify :: (Ord r, Fractional r, Arity d)
=> r -> PolyLine d p r -> PolyLine d p r
simplify :: r -> PolyLine d p r -> PolyLine d p r
simplify r
eps = (LineSegment d p r -> PolyLine d p r -> Bool)
-> PolyLine d p r -> PolyLine d p r
forall (d :: Nat) p r.
(LineSegment d p r -> PolyLine d p r -> Bool)
-> PolyLine d p r -> PolyLine d p r
simplifyWith ((LineSegment d p r -> PolyLine d p r -> Bool)
-> PolyLine d p r -> PolyLine d p r)
-> (LineSegment d p r -> PolyLine d p r -> Bool)
-> PolyLine d p r
-> PolyLine d p r
forall a b. (a -> b) -> a -> b
$ \LineSegment d p r
shortcut PolyLine d p r
subPoly -> ((Point d r :+ p) -> Bool) -> LSeq 2 (Point d r :+ p) -> Bool
forall (t :: * -> *) a. Foldable t => (a -> Bool) -> t a -> Bool
all (LineSegment d p r -> (Point d r :+ p) -> Bool
closeTo LineSegment d p r
shortcut) (PolyLine d p r
subPolyPolyLine d p r
-> Getting
(LSeq 2 (Point d r :+ p))
(PolyLine d p r)
(LSeq 2 (Point d r :+ p))
-> LSeq 2 (Point d r :+ p)
forall s a. s -> Getting a s a -> a
^.Getting
(LSeq 2 (Point d r :+ p))
(PolyLine d p r)
(LSeq 2 (Point d r :+ p))
forall (d1 :: Nat) p1 r1 (d2 :: Nat) p2 r2.
Iso
(PolyLine d1 p1 r1)
(PolyLine d2 p2 r2)
(LSeq 2 (Point d1 r1 :+ p1))
(LSeq 2 (Point d2 r2 :+ p2))
points)
where
closeTo :: LineSegment d p r -> (Point d r :+ p) -> Bool
closeTo LineSegment d p r
seg (Point d r
p :+ p
_) = Point d r -> LineSegment d p r -> r
forall (d :: Nat) r p.
(Arity d, Fractional r, Ord r) =>
Point d r -> LineSegment d p r -> r
sqDistanceToSeg Point d r
p LineSegment d p r
seg r -> r -> Bool
forall a. Ord a => a -> a -> Bool
<= r
epsSq
epsSq :: r
epsSq = r
epsr -> r -> r
forall a. Num a => a -> a -> a
*r
eps
simplifyWith :: (LineSegment d p r -> PolyLine d p r -> Bool)
-> PolyLine d p r -> PolyLine d p r
simplifyWith :: (LineSegment d p r -> PolyLine d p r -> Bool)
-> PolyLine d p r -> PolyLine d p r
simplifyWith LineSegment d p r -> PolyLine d p r -> Bool
isValid PolyLine d p r
pl = PolyLine d p r
plPolyLine d p r
-> (PolyLine d p r -> PolyLine d p r) -> PolyLine d p r
forall a b. a -> (a -> b) -> b
&(LSeq 2 (Point d r :+ p) -> Identity (LSeq 2 (Point d r :+ p)))
-> PolyLine d p r -> Identity (PolyLine d p r)
forall (d1 :: Nat) p1 r1 (d2 :: Nat) p2 r2.
Iso
(PolyLine d1 p1 r1)
(PolyLine d2 p2 r2)
(LSeq 2 (Point d1 r1 :+ p1))
(LSeq 2 (Point d2 r2 :+ p2))
points ((LSeq 2 (Point d r :+ p) -> Identity (LSeq 2 (Point d r :+ p)))
-> PolyLine d p r -> Identity (PolyLine d p r))
-> (LSeq 2 (Point d r :+ p) -> LSeq 2 (Point d r :+ p))
-> PolyLine d p r
-> PolyLine d p r
forall s t a b. ASetter s t a b -> (a -> b) -> s -> t
%~ (forall (m :: Nat) a. LSeq m a -> LSeq 2 a
forall (n :: Nat) (m :: Nat) a. LSeq m a -> LSeq n a
LSeq.promise @2 (LSeq 0 (Point d r :+ p) -> LSeq 2 (Point d r :+ p))
-> (LSeq 2 (Point d r :+ p) -> LSeq 0 (Point d r :+ p))
-> LSeq 2 (Point d r :+ p)
-> LSeq 2 (Point d r :+ p)
forall b c a. (b -> c) -> (a -> b) -> a -> c
. NonEmpty Int -> LSeq 2 (Point d r :+ p) -> LSeq 0 (Point d r :+ p)
forall (n :: Nat) a. NonEmpty Int -> LSeq n a -> LSeq 0 a
extract NonEmpty Int
path)
where
g :: Graph
g = (LineSegment d p r -> PolyLine d p r -> Bool)
-> PolyLine d p r -> Graph
forall (d :: Nat) p r.
(LineSegment d p r -> PolyLine d p r -> Bool)
-> PolyLine d p r -> Graph
mkGraph LineSegment d p r -> PolyLine d p r -> Bool
isValid PolyLine d p r
pl
spt :: Tree Int
spt = Int -> Graph -> Tree Int
forall (f :: * -> *).
Foldable f =>
Int -> Vector (f Int) -> Tree Int
bfs' Int
0 Graph
g
path :: NonEmpty Int
path = case Int -> Tree Int -> [NonEmpty Int]
forall a. Eq a => a -> Tree a -> [NonEmpty a]
pathsTo (PolyLine d p r
plPolyLine d p r -> Getting Int (PolyLine d p r) Int -> Int
forall s a. s -> Getting a s a -> a
^.(LSeq 2 (Point d r :+ p) -> Const Int (LSeq 2 (Point d r :+ p)))
-> PolyLine d p r -> Const Int (PolyLine d p r)
forall (d1 :: Nat) p1 r1 (d2 :: Nat) p2 r2.
Iso
(PolyLine d1 p1 r1)
(PolyLine d2 p2 r2)
(LSeq 2 (Point d1 r1 :+ p1))
(LSeq 2 (Point d2 r2 :+ p2))
points((LSeq 2 (Point d r :+ p) -> Const Int (LSeq 2 (Point d r :+ p)))
-> PolyLine d p r -> Const Int (PolyLine d p r))
-> ((Int -> Const Int Int)
-> LSeq 2 (Point d r :+ p) -> Const Int (LSeq 2 (Point d r :+ p)))
-> Getting Int (PolyLine d p r) Int
forall b c a. (b -> c) -> (a -> b) -> a -> c
.(LSeq 2 (Point d r :+ p) -> Int)
-> (Int -> Const Int Int)
-> LSeq 2 (Point d r :+ p)
-> Const Int (LSeq 2 (Point d r :+ p))
forall (p :: * -> * -> *) (f :: * -> *) s a.
(Profunctor p, Contravariant f) =>
(s -> a) -> Optic' p f s a
to LSeq 2 (Point d r :+ p) -> Int
forall (t :: * -> *) a. Foldable t => t a -> Int
F.length Int -> Int -> Int
forall a. Num a => a -> a -> a
- Int
1) Tree Int
spt of
[] -> [Char] -> NonEmpty Int
forall a. HasCallStack => [Char] -> a
error [Char]
"no path found?"
(NonEmpty Int
pth:[NonEmpty Int]
_) -> NonEmpty Int
pth
type Graph = V.Vector [Int]
mkGraph :: (LineSegment d p r -> PolyLine d p r -> Bool) -> PolyLine d p r -> Graph
mkGraph :: (LineSegment d p r -> PolyLine d p r -> Bool)
-> PolyLine d p r -> Graph
mkGraph LineSegment d p r -> PolyLine d p r -> Bool
isValid = (Graph -> [Int] -> Graph) -> [Int] -> Graph -> Graph
forall a b c. (a -> b -> c) -> b -> a -> c
flip Graph -> [Int] -> Graph
forall a. Vector a -> a -> Vector a
V.snoc [] (Graph -> Graph)
-> (PolyLine d p r -> Graph) -> PolyLine d p r -> Graph
forall b c a. (b -> c) -> (a -> b) -> a -> c
. (Int -> PolyLine d p r -> [Int])
-> Vector (PolyLine d p r) -> Graph
forall a b. (Int -> a -> b) -> Vector a -> Vector b
V.imap Int -> PolyLine d p r -> [Int]
f (Vector (PolyLine d p r) -> Graph)
-> (PolyLine d p r -> Vector (PolyLine d p r))
-> PolyLine d p r
-> Graph
forall b c a. (b -> c) -> (a -> b) -> a -> c
. [PolyLine d p r] -> Vector (PolyLine d p r)
forall a. [a] -> Vector a
V.fromList ([PolyLine d p r] -> Vector (PolyLine d p r))
-> (PolyLine d p r -> [PolyLine d p r])
-> PolyLine d p r
-> Vector (PolyLine d p r)
forall b c a. (b -> c) -> (a -> b) -> a -> c
. Seq (PolyLine d p r) -> [PolyLine d p r]
forall (t :: * -> *) a. Foldable t => t a -> [a]
F.toList (Seq (PolyLine d p r) -> [PolyLine d p r])
-> (PolyLine d p r -> Seq (PolyLine d p r))
-> PolyLine d p r
-> [PolyLine d p r]
forall b c a. (b -> c) -> (a -> b) -> a -> c
. PolyLine d p r -> Seq (PolyLine d p r)
forall (d :: Nat) p r. PolyLine d p r -> Seq (PolyLine d p r)
allPrefixes
where
f :: Int -> PolyLine d p r -> [Int]
f Int
i PolyLine d p r
subPl = [Maybe Int] -> [Int]
forall (f :: * -> *) a. Filterable f => f (Maybe a) -> f a
catMaybes
([Maybe Int] -> [Int]) -> [Maybe Int] -> [Int]
forall a b. (a -> b) -> a -> b
$ (Int -> PolyLine d p r -> Maybe Int)
-> [Int] -> [PolyLine d p r] -> [Maybe Int]
forall a b c. (a -> b -> c) -> [a] -> [b] -> [c]
zipWith Int -> PolyLine d p r -> Maybe Int
isValid' [Int
iInt -> Int -> Int
forall a. Num a => a -> a -> a
+Int
1..] ([PolyLine d p r] -> [Maybe Int])
-> (PolyLine d p r -> [PolyLine d p r])
-> PolyLine d p r
-> [Maybe Int]
forall b c a. (b -> c) -> (a -> b) -> a -> c
. Seq (PolyLine d p r) -> [PolyLine d p r]
forall (t :: * -> *) a. Foldable t => t a -> [a]
F.toList (Seq (PolyLine d p r) -> [PolyLine d p r])
-> (PolyLine d p r -> Seq (PolyLine d p r))
-> PolyLine d p r
-> [PolyLine d p r]
forall b c a. (b -> c) -> (a -> b) -> a -> c
. PolyLine d p r -> Seq (PolyLine d p r)
forall (d :: Nat) p r. PolyLine d p r -> Seq (PolyLine d p r)
allSuffixes (PolyLine d p r -> [Maybe Int]) -> PolyLine d p r -> [Maybe Int]
forall a b. (a -> b) -> a -> b
$ PolyLine d p r
subPl
isValid' :: Int -> PolyLine d p r -> Maybe Int
isValid' Int
j PolyLine d p r
subPoly = let shortcut :: LineSegment d p r
shortcut = (Point d r :+ p) -> (Point d r :+ p) -> LineSegment d p r
forall (d :: Nat) r p.
(Point d r :+ p) -> (Point d r :+ p) -> LineSegment d p r
ClosedLineSegment (PolyLine d p r
subPolyPolyLine d p r
-> Getting (Point d r :+ p) (PolyLine d p r) (Point d r :+ p)
-> Point d r :+ p
forall s a. s -> Getting a s a -> a
^.Getting (Point d r :+ p) (PolyLine d p r) (Point d r :+ p)
forall t. HasStart t => Lens' t (StartCore t :+ StartExtra t)
start) (PolyLine d p r
subPolyPolyLine d p r
-> Getting (Point d r :+ p) (PolyLine d p r) (Point d r :+ p)
-> Point d r :+ p
forall s a. s -> Getting a s a -> a
^.Getting (Point d r :+ p) (PolyLine d p r) (Point d r :+ p)
forall t. HasEnd t => Lens' t (EndCore t :+ EndExtra t)
end)
in if LineSegment d p r -> PolyLine d p r -> Bool
isValid LineSegment d p r
shortcut PolyLine d p r
subPoly then Int -> Maybe Int
forall a. a -> Maybe a
Just Int
j else Maybe Int
forall a. Maybe a
Nothing
allPrefixes :: PolyLine d p r -> Seq.Seq (PolyLine d p r)
allPrefixes :: PolyLine d p r -> Seq (PolyLine d p r)
allPrefixes PolyLine d p r
pl = (Seq (Point d r :+ p) -> Maybe (PolyLine d p r))
-> Seq (Seq (Point d r :+ p)) -> Seq (PolyLine d p r)
forall (f :: * -> *) a b.
Filterable f =>
(a -> Maybe b) -> f a -> f b
mapMaybe Seq (Point d r :+ p) -> Maybe (PolyLine d p r)
forall (d :: Nat) r p.
Seq (Point d r :+ p) -> Maybe (PolyLine d p r)
mkPolyLine (Seq (Seq (Point d r :+ p)) -> Seq (PolyLine d p r))
-> (LSeq 2 (Point d r :+ p) -> Seq (Seq (Point d r :+ p)))
-> LSeq 2 (Point d r :+ p)
-> Seq (PolyLine d p r)
forall b c a. (b -> c) -> (a -> b) -> a -> c
. Seq (Point d r :+ p) -> Seq (Seq (Point d r :+ p))
forall a. Seq a -> Seq (Seq a)
Seq.tails (Seq (Point d r :+ p) -> Seq (Seq (Point d r :+ p)))
-> (LSeq 2 (Point d r :+ p) -> Seq (Point d r :+ p))
-> LSeq 2 (Point d r :+ p)
-> Seq (Seq (Point d r :+ p))
forall b c a. (b -> c) -> (a -> b) -> a -> c
. LSeq 2 (Point d r :+ p) -> Seq (Point d r :+ p)
forall (n :: Nat) a. LSeq n a -> Seq a
LSeq.toSeq (LSeq 2 (Point d r :+ p) -> Seq (PolyLine d p r))
-> LSeq 2 (Point d r :+ p) -> Seq (PolyLine d p r)
forall a b. (a -> b) -> a -> b
$ PolyLine d p r
plPolyLine d p r
-> Getting
(LSeq 2 (Point d r :+ p))
(PolyLine d p r)
(LSeq 2 (Point d r :+ p))
-> LSeq 2 (Point d r :+ p)
forall s a. s -> Getting a s a -> a
^.Getting
(LSeq 2 (Point d r :+ p))
(PolyLine d p r)
(LSeq 2 (Point d r :+ p))
forall (d1 :: Nat) p1 r1 (d2 :: Nat) p2 r2.
Iso
(PolyLine d1 p1 r1)
(PolyLine d2 p2 r2)
(LSeq 2 (Point d1 r1 :+ p1))
(LSeq 2 (Point d2 r2 :+ p2))
points
mkPolyLine :: Seq.Seq (Point d r :+ p) -> Maybe (PolyLine d p r)
mkPolyLine :: Seq (Point d r :+ p) -> Maybe (PolyLine d p r)
mkPolyLine = (LSeq 2 (Point d r :+ p) -> PolyLine d p r)
-> Maybe (LSeq 2 (Point d r :+ p)) -> Maybe (PolyLine d p r)
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
fmap LSeq 2 (Point d r :+ p) -> PolyLine d p r
forall (d :: Nat) p r. LSeq 2 (Point d r :+ p) -> PolyLine d p r
PolyLine (Maybe (LSeq 2 (Point d r :+ p)) -> Maybe (PolyLine d p r))
-> (Seq (Point d r :+ p) -> Maybe (LSeq 2 (Point d r :+ p)))
-> Seq (Point d r :+ p)
-> Maybe (PolyLine d p r)
forall b c a. (b -> c) -> (a -> b) -> a -> c
. forall (m :: Nat) a. KnownNat 2 => LSeq m a -> Maybe (LSeq 2 a)
forall (n :: Nat) (m :: Nat) a.
KnownNat n =>
LSeq m a -> Maybe (LSeq n a)
LSeq.eval @2 (LSeq 0 (Point d r :+ p) -> Maybe (LSeq 2 (Point d r :+ p)))
-> (Seq (Point d r :+ p) -> LSeq 0 (Point d r :+ p))
-> Seq (Point d r :+ p)
-> Maybe (LSeq 2 (Point d r :+ p))
forall b c a. (b -> c) -> (a -> b) -> a -> c
. Seq (Point d r :+ p) -> LSeq 0 (Point d r :+ p)
forall a. Seq a -> LSeq 0 a
LSeq.fromSeq
allSuffixes :: PolyLine d p r -> Seq.Seq (PolyLine d p r)
allSuffixes :: PolyLine d p r -> Seq (PolyLine d p r)
allSuffixes PolyLine d p r
pl = (Seq (Point d r :+ p) -> Maybe (PolyLine d p r))
-> Seq (Seq (Point d r :+ p)) -> Seq (PolyLine d p r)
forall (f :: * -> *) a b.
Filterable f =>
(a -> Maybe b) -> f a -> f b
mapMaybe Seq (Point d r :+ p) -> Maybe (PolyLine d p r)
forall (d :: Nat) r p.
Seq (Point d r :+ p) -> Maybe (PolyLine d p r)
mkPolyLine (Seq (Seq (Point d r :+ p)) -> Seq (PolyLine d p r))
-> (LSeq 2 (Point d r :+ p) -> Seq (Seq (Point d r :+ p)))
-> LSeq 2 (Point d r :+ p)
-> Seq (PolyLine d p r)
forall b c a. (b -> c) -> (a -> b) -> a -> c
. Int -> Seq (Seq (Point d r :+ p)) -> Seq (Seq (Point d r :+ p))
forall a. Int -> Seq a -> Seq a
Seq.drop Int
2 (Seq (Seq (Point d r :+ p)) -> Seq (Seq (Point d r :+ p)))
-> (LSeq 2 (Point d r :+ p) -> Seq (Seq (Point d r :+ p)))
-> LSeq 2 (Point d r :+ p)
-> Seq (Seq (Point d r :+ p))
forall b c a. (b -> c) -> (a -> b) -> a -> c
. Seq (Point d r :+ p) -> Seq (Seq (Point d r :+ p))
forall a. Seq a -> Seq (Seq a)
Seq.inits (Seq (Point d r :+ p) -> Seq (Seq (Point d r :+ p)))
-> (LSeq 2 (Point d r :+ p) -> Seq (Point d r :+ p))
-> LSeq 2 (Point d r :+ p)
-> Seq (Seq (Point d r :+ p))
forall b c a. (b -> c) -> (a -> b) -> a -> c
. LSeq 2 (Point d r :+ p) -> Seq (Point d r :+ p)
forall (n :: Nat) a. LSeq n a -> Seq a
LSeq.toSeq (LSeq 2 (Point d r :+ p) -> Seq (PolyLine d p r))
-> LSeq 2 (Point d r :+ p) -> Seq (PolyLine d p r)
forall a b. (a -> b) -> a -> b
$ PolyLine d p r
plPolyLine d p r
-> Getting
(LSeq 2 (Point d r :+ p))
(PolyLine d p r)
(LSeq 2 (Point d r :+ p))
-> LSeq 2 (Point d r :+ p)
forall s a. s -> Getting a s a -> a
^.Getting
(LSeq 2 (Point d r :+ p))
(PolyLine d p r)
(LSeq 2 (Point d r :+ p))
forall (d1 :: Nat) p1 r1 (d2 :: Nat) p2 r2.
Iso
(PolyLine d1 p1 r1)
(PolyLine d2 p2 r2)
(LSeq 2 (Point d1 r1 :+ p1))
(LSeq 2 (Point d2 r2 :+ p2))
points
pathsTo :: Eq a => a -> Tree a -> [NonEmpty a]
pathsTo :: a -> Tree a -> [NonEmpty a]
pathsTo a
x = (a -> Bool) -> Tree a -> [NonEmpty a]
forall a. (a -> Bool) -> Tree a -> [NonEmpty a]
findPaths (a -> a -> Bool
forall a. Eq a => a -> a -> Bool
== a
x)
findPaths :: (a -> Bool) -> Tree a -> [NonEmpty a]
findPaths :: (a -> Bool) -> Tree a -> [NonEmpty a]
findPaths a -> Bool
p = Tree a -> [NonEmpty a]
go
where
go :: Tree a -> [NonEmpty a]
go (Node a
x Forest a
chs) = case (Tree a -> [NonEmpty a]) -> Forest a -> [NonEmpty a]
forall (t :: * -> *) m a.
(Foldable t, Monoid m) =>
(a -> m) -> t a -> m
foldMap Tree a -> [NonEmpty a]
go Forest a
chs of
[] | a -> Bool
p a
x -> [a
xa -> [a] -> NonEmpty a
forall a. a -> [a] -> NonEmpty a
:|[]]
| Bool
otherwise -> []
[NonEmpty a]
paths | a -> Bool
p a
x -> (a
xa -> [a] -> NonEmpty a
forall a. a -> [a] -> NonEmpty a
:|[]) NonEmpty a -> [NonEmpty a] -> [NonEmpty a]
forall a. a -> [a] -> [a]
: (NonEmpty a -> NonEmpty a) -> [NonEmpty a] -> [NonEmpty a]
forall a b. (a -> b) -> [a] -> [b]
map (a
x a -> NonEmpty a -> NonEmpty a
forall a. a -> NonEmpty a -> NonEmpty a
NonEmpty.<|) [NonEmpty a]
paths
| Bool
otherwise -> (NonEmpty a -> NonEmpty a) -> [NonEmpty a] -> [NonEmpty a]
forall a b. (a -> b) -> [a] -> [b]
map (a
x a -> NonEmpty a -> NonEmpty a
forall a. a -> NonEmpty a -> NonEmpty a
NonEmpty.<|) [NonEmpty a]
paths
extract :: NonEmpty Int -> LSeq.LSeq n a -> LSeq.LSeq 0 a
NonEmpty Int
is = [a] -> LSeq 0 a
forall (f :: * -> *) a. Foldable f => f a -> LSeq 0 a
LSeq.fromList ([a] -> LSeq 0 a) -> (LSeq n a -> [a]) -> LSeq n a -> LSeq 0 a
forall b c a. (b -> c) -> (a -> b) -> a -> c
. [Int] -> Int -> [a] -> [a]
forall a. [Int] -> Int -> [a] -> [a]
extract' (NonEmpty Int -> [Int]
forall (t :: * -> *) a. Foldable t => t a -> [a]
F.toList NonEmpty Int
is) Int
0 ([a] -> [a]) -> (LSeq n a -> [a]) -> LSeq n a -> [a]
forall b c a. (b -> c) -> (a -> b) -> a -> c
. LSeq n a -> [a]
forall (t :: * -> *) a. Foldable t => t a -> [a]
F.toList
extract' :: [Int] -> Int -> [a] -> [a]
[] Int
_ [a]
_ = []
extract' (Int
_:[Int]
_) Int
_ [] = []
extract' is' :: [Int]
is'@(Int
i:[Int]
is) Int
j (a
x:[a]
xs) | Int
i Int -> Int -> Bool
forall a. Eq a => a -> a -> Bool
== Int
j = a
x a -> [a] -> [a]
forall a. a -> [a] -> [a]
: [Int] -> Int -> [a] -> [a]
forall a. [Int] -> Int -> [a] -> [a]
extract' [Int]
is (Int
jInt -> Int -> Int
forall a. Num a => a -> a -> a
+Int
1) [a]
xs
| Bool
otherwise = [Int] -> Int -> [a] -> [a]
forall a. [Int] -> Int -> [a] -> [a]
extract' [Int]
is' (Int
jInt -> Int -> Int
forall a. Num a => a -> a -> a
+Int
1) [a]
xs