{-# LANGUAGE BangPatterns #-}-- Author: Gergely Patai-- from sloth2d: https://github.com/cobbpg/sloth2d/blob/master/Physics/Sloth2D/Geometry2D.hs-- based on Garey, Johnson, Preparata, runtime O(n log n)moduleGraphics.Triangulation.GJPTriangulationwhereimportData.ListimportData.OrdimportData.Vector(Vector,(!))importqualifiedData.VectorasVimportqualifiedData.Vector.Algorithms.IntroasVimportGraphics.Formats.Collada.Vector2D3DdataVertexType=TopCap|BottomCap|TopCup|BottomCup|SidederivingShowdataVertex=Vtx{idx::Int,prev::Int,next::Int,vtype::VertexType,px::Float,py::Float}derivingShowtypeMonotoneSegment=([Int],[Int])-- | Descriptor for a pair of features. The ordering stands for the-- following configurations: @LT@ - V to E, @EQ@ - E to E, @GT - E to-- V, where E stands for edge and V stands for vertex (in other words,-- you can think of edges being greater than vertices). The integers-- are the indices of the features: the vertex itself or the first-- vertex (in ccw order) of the edge. For instance, @(LT,2,4)@ means-- the pair formed by vertex 2 of the first body and the edge between-- vertices 4 and 5 of the second body.typeSeparation=(Ordering,Int,Int)-- | Checking whether an angle is within a given interval.between::Angle->(Angle,Angle)->Boola`between`(a1,a2)|a1<=a2=a>=a1&&a<=a2|otherwise=a>=a1||a<=a2infixl6+<-- | The sum of two angles.(+<)::Angle->Angle->Anglea1+<a2=ifa<-pithena+2*pielseifa>pithena-2*pielseawherea=a1+a2-- | Linear interpolation between two angles along the smaller arc.alerp::Angle->Angle->Float->Anglealerpa1a2t=a1+<(a2+<(-a1))*t-- | Applying a binary function to consecutive pairs in a vector with-- wrap-around.pairsWith::(a->a->b)->Vectora->VectorbpairsWithfvs|V.nullvs=V.empty|otherwise=V.zipWithfvs(V.snoc(V.tailvs)(V.headvs))-- | The edge vectors of a polygon given as a list of vertices.edges::VectorV2->VectorV2edgesvs=ifV.lengthvs<2thenV.emptyelsepairsWith(flip(-))vs-- | The absolute angles (with respect to the x axis) of the edges of-- a polygon given as a list of vertices.angles::VectorV2->VectorAngleangles=V.mapdir.edges-- | The signed area of a simple polygon (positive if vertices are in-- counter-clockwise order).area::VectorV2->Floatareavs=0.5*V.sum(pairsWithcrossvs)-- | The centroid of a simple polygon.centroid::VectorV2->V2centroidvs|V.nullvs=V00|otherwise=divsum(V.foldl1'accum(pairsWithgenvs))wheregenv1v2=letc=v1`cross`v2in(c,(v1+v2)*.c)accum(!c1,!v1)(c2,v2)=(c1+c2,v1+v2)divsum(c,v)|c/=0=v*.(recip(3*c))|otherwise=(V.minimumvs+V.maximumvs)*.0.5-- | The moment of inertia of a simple polygon with respect to the origin.moment::VectorV2->Floatmomentvs|V.lengthvs<3=0|otherwise=divsum(V.foldl1'accum(pairsWithgenvs))wheregenv1v2=letc=v2`cross`v1in(c,(v1`dot`(v1+v2)+squarev2)*c)accum(!s1,!s2)(p1,p2)=(s1+p1,s2+p2)divsum(s1,s2)|s1/=0=s2/(6*s1)|otherwise=0-- | The convex hull of a collection of vertices in counter-clockwise-- order. (Andrew's Monotone Chain Algorithm)convexHull::VectorV2->VectorV2convexHullvs=casecompare(V.lengthvs)2ofLT->vsEQ->V.fromList.nub.V.toList$vsGT->V.fromList(avs'++bvs')wheresvs=V.modifyV.sortvsvmin=V.headsvsvmax=V.lastsvsvd=vmax-vmin(avs,bvs)=V.partition(\v->vd`turnNR`v-vmax).V.init.V.tail$svsavs'=ifV.nullavsthen[vmin]elsetail.V.foldl'(flipaddVertex)[V.headavs,vmin]$V.snoc(V.tailavs)vmaxbvs'=ifV.nullbvsthen[vmax]elsetail.V.foldr'addVertex[V.lastbvs,vmax]$V.consvmin(V.initbvs)addVertexv(v1:vs@(v2:_))|v1-v2`turnNR`v-v1=addVertexvvsaddVertexvvs=v:vs-- | Monotone decomposition of a simple polygon.monotoneDecomposition::VectorV2->[MonotoneSegment]monotoneDecompositionvs=(mapgetIndices.snd)(V.foldl'addVertex([],[])scvs)wherecw=areavs<0ovs=ifcwthenvselseV.reversevsgetIndices(l,r)=ifcwthen(mapidxl,mapidxr)else(mapidx'l,mapidx'r)whereidx'v=V.lengthvs-1-idxvaddVertex(mss,out)v=casevtypevof-- open new monotone segment with this sole vertexTopCap->(([v],[v]):mss,out)-- split monotone segment: all vertices are added to left side,-- only last two to right; this is the only case where we need-- to check geometry to find the matching segmentBottomCap->let(mss',(msl,msr):mss'')=breakisContainedmssms'=(msl,v:msr)ms''=([v,headmsr],[headmsr])in(mss'++ms':ms'':mss'',out)-- close the segment on the right side using the join vertex and-- the next vertex on its other sideTopCup->let([(msl1,msr1),(msl2,msr2)],mssr)=partitionisConnectedmss(msl1',msr1',msl2',msr2')=ifidxv==prev(headmsr1)thenleti=prev(headmsr2)v'=cvs!iin(msl1,v{prev=i}:msr1,v':v:msl2,v':msr2)elseleti=prev(headmsr1)v'=cvs!iin(msl2,v{prev=i}:msr2,v':v:msl1,v':msr1)in((msl1',msr1'):mssr,(msl2',msr2'):out)-- close monotone segment (stage for emission, remove from-- active collection)BottomCup->let(mss',(msl,msr):mss'')=breakisConnectedmssin(mss'++mss'',(v:msl,v:msr):out)-- add to the segment the upper neighbour belongs toSide->let(mss',(msl,msr):mss'')=breakisConnectedmssms'=ifidxv==next(headmsl)then(v:msl,msr)else(msl,v:msr)in(mss'++ms':mss'',out)whereisConnected((vl:_),(vr:_))=idxv==nextvl||idxv==prevvrisConnected_=error"isConnected"isContained((vl:_),(vr:_))=pxv>xl&&pxv<=xrwherevl'=cvs!(nextvl)vr'=cvs!(prevvr)xl=pxvl+(pxvl'-pxvl)*(pyv-pyvl)/(pyvl'-pyvl)xr=pxvr+(pxvr'-pxvr)*(pyv-pyvr)/(pyvr'-pyvr)isContained_=error"isContained"scvs=V.modify(V.sortBy(comparingpy))cvscvs=V.imapclassifyovsclassifyi1v1@(Vx1y1)=Vtxi1i0i2vtyx1y1wherevty=case(comparey1y0,comparey1y2,v2-v1`turn`v1-v0)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_->Sidei0=ifi1==0thenV.lengthovs-1elsei1-1i2=ifi1==V.lengthovs-1then0elsei1+1v0@(V_y0)=ovs!i0v2@(V_y2)=ovs!i2-- | Triangulation of a monotone polygon.monotoneTriangulation::VectorV2->MonotoneSegment->[(Int,Int,Int)]monotoneTriangulationvs(msl,msr)=snd(foldl'addVertex([si2,si1],[])sis)whereaddVertex(si@(s,i):sis,ts)si'@(s',i')|s/=s'=([si',si],zipWith(ifs'thentlelsetr)(si:sis)sis++ts)|concave=(si':si:sis,ts)|otherwise=(si':si'':mapsndsi2s'',zipWith(ifs'thentrelsetl)sis'sis''++ts)whereconcave=isConcave(snd(headsis))i(si2s',si2s'')=breakvisible(zip(si:sis)sis)wherevisible((_,i1),(_,i2))=isConcavei2i1(sis',sis'')=unzipsi2s'si''=lastsis''tl(_,i1)(_,i2)=(i',i2,i1)tr(_,i1)(_,i2)=(i',i1,i2)isConcavei0i1=s'==v1-v0`turnL`v2-v1wherev0=vs!i0v1=vs!i1v2=vs!i'addVertex__=error"addVertex"si1:si2:sis=mergemsl(init(tailmsr))merge[]irs=map((,)True)irsmergeils[]=map((,)False)ilsmergeils@(il:ils')irs@(ir:irs')|y1<y2=(True,ir):mergeilsirs'|otherwise=(False,il):mergeils'irswhereV_y1=vs!ilV_y2=vs!ir-- | Triangulation of a simple polygon.triangulation::VectorV2->[(Int,Int,Int)]triangulationvs=[tri|ms<-monotoneDecompositionvs,tri<-monotoneTriangulationvsms]-- | A 5-tuple @(d2,ds,sep,v1,v2)@ that provides distance information-- on two convex polygons, where @d2@ is the square of the distance,-- @ds@ is its sign (negative in case of penetration), @sep@ describes-- the opposing features, while @v1@ and @v2@ are the absolute-- coordinates of the deepest points within the opposite polygon. If-- the third parameter is @True@, only negative distances are-- reported, and the function yields @Nothing@ for non-overlapping-- polygons. This is more efficient if we are only interested in-- collisions, since the computation can be cancelled upon finding the-- first separating axis. If the third parameter is @False@, the-- result cannot be @Nothing@.convexSeparation::VectorV2-- ^ The vertices of the first polygon (vs1)->VectorV2-- ^ The vertices of the second polygon (vs2)->Bool-- ^ Whether we are only interested in overlapping->Maybe(Float,Float,Separation,V2,V2)convexSeparationvs1vs2onlyCollision|onlyCollision=closestPenetratingPairfirstValidPair|otherwise=Just(closestPairfirstValidPair)wherel1=V.lengthvs1l2=V.lengthvs2succ1n=letn'=succninifn'>=l1then0elsen'succ2n=letn'=succninifn'>=l2then0elsen'pred1n=ifn==0thenl1-1elseprednpred2n=ifn==0thenl2-1elseprednfirstValidPair=untilvalidSeparationstepBackwards(GT,0,0)-- Exhaustive search for the closest feature pairclosestPairs=go(l1+l2-1)(stepBackwardss)(s,v12)dstwhere(dst,v12)=separationsgo0_(s,(v1,v2))(sd,sgd)=(sd,-sgd,s,v1,v2)gonssepdst|dst<dst'=gon'(stepBackwardss)sepdst|otherwise=gon'(stepBackwardss)(s,v12)dst'where(dst',v12)=separationsn'=n-1-- Exhaustive search for the closest penetrating feature pairclosestPenetratingPairs=go(l1+l2-1)(stepBackwardss)(s,v12)dstwhere(dst,v12)=separationsgo0_(s,(v1,v2))(sd,sgd)=Just(sd,-sgd,s,v1,v2)gonssepdst@(_,sg)|sg<0=Nothing|dst<dst'=gon'(stepBackwardss)sepdst|otherwise=gon'(stepBackwardss)(s,v12)dst'where(dst',v12)=separationsn'=n-1{-
-- Step towards the next feature pair counter-clockwise
stepForward (rel,i1,i2) = case rel of
LT -> (turn e1 e2',i1 ,i2')
EQ -> (turn e1' e2',i1',i2')
GT -> (turn e1' e2 ,i1',i2 )
where
i1' = succ1 i1
i2' = succ2 i2
e1 = vs1 ! i1' - vs1 ! i1
e2 = vs2 ! i2 - vs2 ! i2'
e1' = vs1 ! succ1 i1' - vs1 ! i1'
e2' = vs2 ! i2' - vs2 ! succ2 i2'
-}-- Step towards the next feature pair clockwisestepBackwards(_,i1,i2)=caseturne2e1ofLT->(LT,i1,i2')EQ->(EQ,i1',i2')GT->(GT,i1',i2)wherei1'=pred1i1i2'=pred2i2e1=vs1!i1-vs1!i1'e2=vs2!i2'-vs2!i2-- Check if the feature pair is valid (i.e. the edge lies within-- the interval defined by the vertex, or the edges are parallel)validSeparation(rel,i1,i2)=caserelofLT->turnNRe11e22&&turnNRe22e12EQ->parve12e22GT->turnNRe21e12&&turnNRe12e22wherev1=vs1!i1v2=vs2!i2e11=v1-vs1!pred1i1e12=vs1!succ1i1-v1e21=vs2!pred2i2-v2e22=v2-vs2!succ2i2-- Distance information for a given feature pairseparation(rel,i1,i2)=caserelofLT->swap(sv2v2'e2sd2v1)GT->sv1v1'e1sd1v2EQ|sd1>sd2->min(sv1v1'e1sd1v2)(sv1v1'e1sd1v2')|otherwise->swap(min(sv2v2'e2sd2v1)(sv2v2'e2sd2v1'))whereswap(d,(v1,v2))=(d,(v2,v1))v1=vs1!i1v2=vs2!i2v1'=vs1!succ1i1v2'=vs2!succ2i2e1=v1'-v1e2=v2'-v2sd1=squaree1sd2=squaree2-- The squared distance of the v1 to v2 segment and the v3 vertexsv1v2e12sd12v3=((sd,signumcp),(v,v3))wheree13=v3-v1e23=v3-v2sd12'=recipsd12dp=e12`dot`e13-- negative: separation, positive: penetrationcp=e12`cross`e13(v,sd)|dp<=0=(v1,squaree13)|dp>=sd12=(v2,squaree23)|otherwise=(v1+e12*.(dp*sd12'),cp*cp*sd12')