module Graphics.Triangulation.GJPTriangulation where
import Data.List
import Data.Ord
import Data.Vector (Vector, (!))
import qualified Data.Vector as V
import qualified Data.Vector.Algorithms.Intro as V
import Graphics.Formats.Collada.Vector2D3D
data VertexType = TopCap | BottomCap | TopCup | BottomCup | Side
deriving Show
data Vertex = Vtx
{ idx :: Int
, prev :: Int
, next :: Int
, vtype :: VertexType
, px :: Float
, py :: Float
} deriving Show
type MonotoneSegment = ([Int],[Int])
type Separation = (Ordering, Int, Int)
between :: Angle -> (Angle,Angle) -> Bool
a `between` (a1,a2)
| a1 <= a2 = a >= a1 && a <= a2
| otherwise = a >= a1 || a <= a2
infixl 6 +<
(+<) :: Angle -> Angle -> Angle
a1 +< a2 = if a < pi then a+2*pi
else if a > pi then a2*pi
else a
where
a = a1+a2
alerp :: Angle -> Angle -> Float -> Angle
alerp a1 a2 t = a1+<(a2+<(a1))*t
pairsWith :: (a -> a -> b) -> Vector a -> Vector b
pairsWith f vs
| V.null vs = V.empty
| otherwise = V.zipWith f vs (V.snoc (V.tail vs) (V.head vs))
edges :: Vector V2 -> Vector V2
edges vs = if V.length vs < 2 then V.empty else pairsWith (flip ()) vs
angles :: Vector V2 -> Vector Angle
angles = V.map dir . edges
area :: Vector V2 -> Float
area vs = 0.5 * V.sum (pairsWith cross vs)
centroid :: Vector V2 -> V2
centroid vs
| V.null vs = V 0 0
| otherwise = divsum (V.foldl1' accum (pairsWith gen vs))
where
gen v1 v2 = let c = v1 `cross` v2 in (c,(v1+v2)*.c)
accum (!c1,!v1) (c2,v2) = (c1+c2,v1+v2)
divsum (c,v)
| c /= 0 = v*.(recip (3*c))
| otherwise = (V.minimum vs+V.maximum vs)*.0.5
moment :: Vector V2 -> Float
moment vs
| V.length vs < 3 = 0
| otherwise = divsum (V.foldl1' accum (pairsWith gen vs))
where
gen v1 v2 = let c = v2 `cross` v1 in (c,(v1 `dot` (v1+v2) + square v2)*c)
accum (!s1,!s2) (p1,p2) = (s1+p1,s2+p2)
divsum (s1,s2)
| s1 /= 0 = s2/(6*s1)
| otherwise = 0
convexHull :: Vector V2 -> Vector V2
convexHull vs = case compare (V.length vs) 2 of
LT -> vs
EQ -> V.fromList . nub . V.toList $ vs
GT -> V.fromList (avs' ++ bvs')
where
svs = V.modify V.sort vs
vmin = V.head svs
vmax = V.last svs
vd = vmaxvmin
(avs,bvs) = V.partition (\v -> vd `turnNR` vvmax) . V.init . V.tail $ svs
avs' = if V.null avs then [vmin]
else tail . V.foldl' (flip addVertex) [V.head avs,vmin] $ V.snoc (V.tail avs) vmax
bvs' = if V.null bvs then [vmax]
else tail . V.foldr' addVertex [V.last bvs,vmax] $ V.cons vmin (V.init bvs)
addVertex v (v1:vs@(v2:_)) | v1v2 `turnNR` vv1 = addVertex v vs
addVertex v vs = v:vs
monotoneDecomposition :: Vector V2 -> [MonotoneSegment]
monotoneDecomposition vs = (map getIndices . snd) (V.foldl' addVertex ([], []) scvs)
where
cw = area vs < 0
ovs = if cw then vs else V.reverse vs
getIndices (l,r) = if cw then (map idx l, map idx r)
else (map idx' l, map idx' r)
where
idx' v = V.length vs 1 idx v
addVertex (mss, out) v = case vtype v of
TopCap -> (([v], [v]) : mss, out)
BottomCap -> let (mss',(msl,msr):mss'') = break isContained mss
ms' = (msl, v : msr)
ms'' = ([v, head msr], [head msr])
in (mss' ++ ms':ms'':mss'', out)
TopCup -> let ([(msl1,msr1),(msl2,msr2)], mssr) = partition isConnected mss
(msl1',msr1',msl2',msr2') =
if idx v == prev (head msr1)
then let i = prev (head msr2)
v' = cvs ! i
in (msl1, v { prev = i } : msr1, v':v:msl2, v':msr2)
else let i = prev (head msr1)
v' = cvs ! i
in (msl2, v { prev = i } : msr2, v':v:msl1, v':msr1)
in ((msl1',msr1'):mssr,(msl2',msr2'):out)
BottomCup -> let (mss',(msl,msr):mss'') = break isConnected mss
in (mss' ++ mss'', (v:msl,v:msr):out)
Side -> let (mss',(msl,msr):mss'') = break isConnected mss
ms' = if idx v == next (head msl) then (v:msl, msr) else (msl, v:msr)
in (mss' ++ ms':mss'', out)
where
isConnected ((vl:_), (vr:_)) = idx v == next vl || idx v == prev vr
isConnected _ = error "isConnected"
isContained ((vl:_), (vr:_)) = px v > xl && px v <= xr
where
vl' = cvs ! (next vl)
vr' = cvs ! (prev vr)
xl = px vl + (px vl' px vl) * (py v py vl) / (py vl' py vl)
xr = px vr + (px vr' px vr) * (py v py vr) / (py vr' py vr)
isContained _ = error "isContained"
scvs = V.modify (V.sortBy (comparing py)) cvs
cvs = V.imap classify ovs
classify i1 v1@(V x1 y1) = Vtx i1 i0 i2 vty x1 y1
where
vty = case (compare y1 y0, compare y1 y2, v2v1 `turn` v1v0) of
(LT, LT, LT) -> BottomCap
(EQ, LT, LT) -> BottomCap
(LT, LT, GT) -> TopCap
(LT, EQ, GT) -> TopCap
(GT, GT, GT) -> BottomCup
(EQ, GT, GT) -> BottomCup
(GT, GT, LT) -> TopCup
(GT, EQ, LT) -> TopCup
_ -> Side
i0 = if i1 == 0 then V.length ovs 1 else i11
i2 = if i1 == V.length ovs 1 then 0 else i1+1
v0@(V _ y0) = ovs ! i0
v2@(V _ y2) = ovs ! i2
monotoneTriangulation :: Vector V2 -> MonotoneSegment -> [(Int,Int,Int)]
monotoneTriangulation vs (msl,msr) = snd (foldl' addVertex ([si2,si1],[]) sis)
where
addVertex (si@(s,i):sis,ts) si'@(s',i')
| s /= s' = ([si',si], zipWith (if s' then tl else tr) (si:sis) sis ++ ts)
| concave = (si':si:sis,ts)
| otherwise = (si':si'':map snd si2s'', zipWith (if s' then tr else tl) sis' sis'' ++ ts)
where
concave = isConcave (snd (head sis)) i
(si2s',si2s'') = break visible (zip (si:sis) sis)
where
visible ((_,i1),(_,i2)) = isConcave i2 i1
(sis',sis'') = unzip si2s'
si'' = last sis''
tl (_,i1) (_,i2) = (i',i2,i1)
tr (_,i1) (_,i2) = (i',i1,i2)
isConcave i0 i1 = s' == v1v0 `turnL` v2v1
where
v0 = vs ! i0
v1 = vs ! i1
v2 = vs ! i'
addVertex _ _ = error "addVertex"
si1:si2:sis = merge msl (init (tail msr))
merge [] irs = map ((,) True) irs
merge ils [] = map ((,) False) ils
merge ils@(il:ils') irs@(ir:irs')
| y1 < y2 = (True,ir) : merge ils irs'
| otherwise = (False,il) : merge ils' irs
where
V _ y1 = vs ! il
V _ y2 = vs ! ir
triangulation :: Vector V2 -> [(Int, Int, Int)]
triangulation vs = [tri | ms <- monotoneDecomposition vs, tri <- monotoneTriangulation vs ms]
convexSeparation
:: Vector V2
-> Vector V2
-> Bool
-> Maybe (Float, Float, Separation, V2, V2)
convexSeparation vs1 vs2 onlyCollision
| onlyCollision = closestPenetratingPair firstValidPair
| otherwise = Just (closestPair firstValidPair)
where
l1 = V.length vs1
l2 = V.length vs2
succ1 n = let n' = succ n in if n' >= l1 then 0 else n'
succ2 n = let n' = succ n in if n' >= l2 then 0 else n'
pred1 n = if n == 0 then l11 else pred n
pred2 n = if n == 0 then l21 else pred n
firstValidPair = until validSeparation stepBackwards (GT,0,0)
closestPair s = go (l1+l21) (stepBackwards s) (s,v12) dst
where
(dst,v12) = separation s
go 0 _ (s,(v1,v2)) (sd,sgd) = (sd,sgd,s,v1,v2)
go n s sep dst
| dst < dst' = go n' (stepBackwards s) sep dst
| otherwise = go n' (stepBackwards s) (s,v12) dst'
where
(dst',v12) = separation s
n' = n1
closestPenetratingPair s = go (l1+l21) (stepBackwards s) (s,v12) dst
where
(dst,v12) = separation s
go 0 _ (s,(v1,v2)) (sd,sgd) = Just (sd,sgd,s,v1,v2)
go n s sep dst@(_,sg)
| sg < 0 = Nothing
| dst < dst' = go n' (stepBackwards s) sep dst
| otherwise = go n' (stepBackwards s) (s,v12) dst'
where
(dst',v12) = separation s
n' = n1
stepBackwards (_,i1,i2) = case turn e2 e1 of
LT -> (LT,i1 ,i2')
EQ -> (EQ,i1',i2')
GT -> (GT,i1',i2 )
where
i1' = pred1 i1
i2' = pred2 i2
e1 = vs1 ! i1 vs1 ! i1'
e2 = vs2 ! i2' vs2 ! i2
validSeparation (rel,i1,i2) = case rel of
LT -> turnNR e11 e22 && turnNR e22 e12
EQ -> parv e12 e22
GT -> turnNR e21 e12 && turnNR e12 e22
where
v1 = vs1 ! i1
v2 = vs2 ! i2
e11 = v1 vs1 ! pred1 i1
e12 = vs1 ! succ1 i1 v1
e21 = vs2 ! pred2 i2 v2
e22 = v2 vs2 ! succ2 i2
separation (rel,i1,i2) = case rel of
LT -> swap (s v2 v2' e2 sd2 v1)
GT -> s v1 v1' e1 sd1 v2
EQ | sd1 > sd2 -> min (s v1 v1' e1 sd1 v2) (s v1 v1' e1 sd1 v2')
| otherwise -> swap (min (s v2 v2' e2 sd2 v1) (s v2 v2' e2 sd2 v1'))
where
swap (d,(v1,v2)) = (d,(v2,v1))
v1 = vs1 ! i1
v2 = vs2 ! i2
v1' = vs1 ! succ1 i1
v2' = vs2 ! succ2 i2
e1 = v1'v1
e2 = v2'v2
sd1 = square e1
sd2 = square e2
s v1 v2 e12 sd12 v3 = ((sd,signum cp),(v,v3))
where
e13 = v3v1
e23 = v3v2
sd12' = recip sd12
dp = e12 `dot` e13
cp = e12 `cross` e13
(v,sd) | dp <= 0 = (v1,square e13)
| dp >= sd12 = (v2,square e23)
| otherwise = (v1+e12*.(dp*sd12'),cp*cp*sd12')