collision detection GJK

def projection(v,u): return ((v*u)/(u*u))*u 

def SupportC(D,p,r): d=(1/abs(D))*D return p+r*d 

def SupportV(D,points): x=vector(points[0]) for i in range(len(points)): if(x*D<vector(points[i])*D): x=vector(points[i]) return x 

def Support(D,p,r): d=(1/abs(D))*D return p+r*d 

def DoSimplex(points,D): O=vector([0,0]) if len(points)==1: D=O-points[0] if len(points)==2: if (points[0]-points[1])*(O-points[1])<0: D=O-points[1] points=[points[1]] else: D=(O-points[1]-projection(O-points[1],points[0]-points[1])) if len(points)==3: V1=points[0]-points[2] V2=points[1]-points[2] if vector([V1[1],-V1[0]])*V2>0: temp=points[0] points[0]=points[1] points[1]=temp V1=points[0]-points[2] V2=points[1]-points[2] V0=O-points[2] if vector([V1[1],-V1[0]])*V0<0: if (vector([-V2[1],V2[0]])*V0)<0: return (true,points,D) if V2*V0>0: points=[points[2],points[1]] D=(V0-projection(V0,V2)) else: if V1*V0>0: points=[points[2],points[0]] D=(V0-projection(V0,V1)) else: points=[points[1]] D=V0 else: if V1*V0>0: points=[points[2],points[0]] D=(V0-projection(V0,V1)) else: if V2*V0>0: points=[points[2],points[1]] D=(V0-projection(V0,V2)) else: points=[points[0]] D=V0 if D==O: return (true,points,D) else: return (false,points,D) 

def GJK(p,r): c=circle(p,r) O=vector([0,0]) D=vector([0,1]) S=n(SupportC(D,p,r)) points=[S] plotlist=[scatter_plot(points)+c] D=-S count=0 while count<1000: A=SupportC(D,p,r) points.append(A) if len(points)==1: plotlist.append(scatter_plot(points)+c) if len(points)==2: plotlist.append(line(points)+c) if len(points)==3: plotlist.append(polygon(points)+c) if A*D<0: return (false,plotlist) k=DoSimplex(points,D) points=k[1] D=n(k[2]) if k[0]: return (true,plotlist) count=count+1 print(count) return (false,plotlist) 

def GJKcc(p1,r1,p2,r2): c=circle(p1-p2,r1+r2) O=vector([0,0]) D=vector([0,1]) S=n(SupportC(D,p1,r1)-SupportC(-D,p2,r2)) points=[S] plotlist=[scatter_plot(points)+c] D=-S count=0 while count<1000: A=n(SupportC(D,p1,r1)-SupportC(-D,p2,r2)) points.append(A) if len(points)==1: plotlist.append(scatter_plot(points)+c) if len(points)==2: plotlist.append(line(points)+c) if len(points)==3: plotlist.append(polygon(points)+c) if A*D<0: return (false,plotlist) k=DoSimplex(points,D) points=k[1] D=n(k[2]) if k[0]: return (true,plotlist) count=count+1 print(count) return (false,plotlist) 

def GJKvv(v1,v2): O=vector([0,0]) D=vector([0,1]) S=n(SupportV(D,v1)-SupportV(-D,v2)) points=[S] plotlist=[scatter_plot(points)] D=-S count=0 while count<1000: A=n(SupportV(D,v1)-SupportV(-D,v2)) points.append(A) if len(points)==1: plotlist.append(scatter_plot(points)) if len(points)==2: plotlist.append(line(points)) if len(points)==3: plotlist.append(polygon(points)) if A*D<0: return (false,plotlist) k=DoSimplex(points,D) points=k[1] D=n(k[2]) if k[0]: return (true,plotlist) count=count+1 print(count) return (false,plotlist) 

def GJKcv(p,r,v): O=vector([0,0]) D=vector([0,1]) S=n(SupportV(D,p,r)-SupportV(-D,v)) points=[S] plotlist=[scatter_plot(points)] D=-S count=0 while count<1000: A=n(SupportV(D,p,r)-SupportV(-D,v)) points.append(A) if len(points)==1: plotlist.append(scatter_plot(points)) if len(points)==2: plotlist.append(line(points)) if len(points)==3: plotlist.append(polygon(points)) if A*D<0: return (false,plotlist) k=DoSimplex(points,D) points=k[1] D=n(k[2]) if k[0]: return (true,plotlist) count=count+1 print(count) return (false,plotlist) 

temp=GJKcc(vector([1,2]),1,vector([-1,2]),1) list=temp[1] i=-1 temp[0] 

True

True

i=i+1 list[i] 

i=i-1 list[i] 

temp=GJKvv([(0,1),(1,1),(1,0),(0,0)],[(0,-1),(1,-1),(1,0),(0,0)]) list=temp[1] i=-1 temp[0] 

True

True

GJKvv([(0,0),(1,1),(1,-1),(2,0)],[(0,0),(1,1),(-1,1),(0,2)]) 

(True,
 [Graphics object consisting of 1 graphics primitive,
  Graphics object consisting of 1 graphics primitive])

(True,
 [Graphics object consisting of 1 graphics primitive,
  Graphics object consisting of 1 graphics primitive])

i=(1,0) 

D=vector((1,0)) 

vector(i)*D 

1

1

vector((1,0)) 

(1, 0)

(1, 0)

vector((1,0))*D 

1

1