-
-
Save Hammer2900/3f77d266cb44bff27fd29a94c2ed95b2 to your computer and use it in GitHub Desktop.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
import Box2D | |
import Box2D.b2 as b2 | |
class b2Separator: | |
""" | |
* Convex Separator for Box2D Flash | |
* | |
* This code has been ported to Python by Stephen Tian from the Java port by Chad Palsulich of the Flash original written by Antoan Angelov. | |
* It is designed to work with Erin Catto's Box2D physics library. | |
* | |
* Everybody can use this software for any purpose, under two restrictions: | |
* 1. You cannot claim that you wrote this software. | |
* 2. You can not remove or alter this notice. | |
* | |
""" | |
PPM = 100 | |
@staticmethod | |
def separate(body, fixtureDef, verticesVec, scale=30): | |
n = len(verticesVec) | |
vec = [] | |
for i in range(n): | |
vec.append(Box2D.b2Vec2(verticesVec[i][0]*scale, verticesVec[i][1]*scale)) | |
figsVec = b2Separator.calc_shapes(vec) | |
n = len(figsVec) | |
for i in range(n): | |
vec = figsVec[i] | |
verticesVec = [] | |
m = len(vec) | |
for j in range(m): | |
verticesVec.append(Box2D.b2Vec2(vec[j][0]/(scale * b2Separator.PPM), vec[j][1]/(scale * b2Separator.PPM))) | |
polyShape = Box2D.b2PolygonShape(vertices=verticesVec) | |
fixtureDef.shape = polyShape | |
fixtureDef.restitution = 0.2 | |
fixtureDef.density = 0.5 | |
fixtureDef.friction = 0.3 | |
body.CreateFixture(fixtureDef) | |
@staticmethod | |
def validate(verticesVec): | |
n = len(verticesVec) | |
ret = 0 | |
fl, fl2 = False, False | |
for i in range(n): | |
i2 = i+1 if i<n-1 else 0 | |
i3 = i-1 if i>0 else n-1 | |
fl = False | |
for j in range(n): | |
if j != i and j != i2: | |
if not fl: | |
d = b2Separator.det(verticesVec[i][0], verticesVec[i][1], verticesVec[i2][0], | |
verticesVec[i2][1], verticesVec[j][0], verticesVec[j][1]) | |
if d > 0: | |
fl = True | |
if j != i3: | |
j2 = j+1 if j < n-1 else 0 | |
if b2Separator.hit_segment(verticesVec[i][0], verticesVec[i][1], verticesVec[i2][0], | |
verticesVec[i2][1], verticesVec[j][0], verticesVec[j][1], | |
verticesVec[j2][0], verticesVec[j2][1]) is not None: | |
ret = 1 | |
if not fl: | |
fl2 = True | |
if fl2: | |
if ret == 1: | |
ret = 3 | |
else: | |
ret = 2 | |
return ret | |
@staticmethod | |
def calc_shapes(vertices_vec): | |
vec = [] | |
k = 0 | |
h = -1 | |
vec1, vec2 = [], [] | |
figs_vec = [] | |
queue = [] | |
queue.append(vertices_vec) | |
while len(queue) > 0: | |
vec = queue[0] | |
n = len(vec) | |
isConvex = True | |
for i in range(n): | |
i1 = i | |
i2 = i+1 if i < n-1 else i+1-n | |
i3 = i+2 if i < n-2 else i+2-n | |
p1 = vec[i1] | |
p2 = vec[i2] | |
p3 = vec[i3] | |
d = b2Separator.det(p1[0], p1[1], p2[0], p2[1], p3[0], p3[1]) | |
if d < 0: | |
isConvex = False | |
minLen = 1e30 | |
for j in range(n): | |
if j != i1 and j != i2: | |
j1 = j | |
j2 = j+1 if j < n-1 else 0 | |
v1 = vec[j1] | |
v2 = vec[j2] | |
v = b2Separator.hit_ray(p1[0], p1[1], p2[0], p2[1], v1[0], v1[1], v2[0], v2[1]) | |
if v is not None: | |
dx = p2[0]-v[0] | |
dy = p2[1]-v[1] | |
t = dx*dx + dy*dy | |
if t < minLen: | |
h = j1 | |
k = j2 | |
hitV = v | |
minLen = t | |
if minLen == 1e30: | |
b2Separator.err() | |
vec1, vec2 = [], [] | |
j1 = h | |
j2 = k | |
v1 = vec[j1] | |
v2 = vec[j2] | |
if not b2Separator.points_match(hitV[0], hitV[1], v2[0], v2[1]): | |
vec1.append(hitV) | |
if not b2Separator.points_match(hitV[0], hitV[1], v1[0], v1[1]): | |
vec2.append(hitV) | |
h = -1 | |
k = i1 | |
while True: | |
if k != j2: | |
vec1.append(vec[k]) | |
else: | |
if h < 0 or h >=n: | |
b2Separator.err() | |
if not b2Separator.is_on_segment(v2[0], v2[1], vec[h][0], vec[h][1], p1[0], p1[1]): | |
vec1.append(vec[k]) | |
break | |
h = k | |
if k-1<0: | |
k = n-1 | |
else: | |
k -= 1 | |
vec1 = vec1[::-1] | |
h = -1 | |
k = i2 | |
while True: | |
if k != j2: | |
vec2.append(vec[k]) | |
else: | |
if h < 0 or h >= n: | |
b2Separator.err() | |
if not b2Separator.is_on_segment(v1[0], v1[1], vec[h][0], vec[h][1], p2[0], p2[1]): | |
vec2.append(vec[k]) | |
break | |
h = k | |
if k+1>n-1: | |
k = 0 | |
else: | |
k += 1 | |
queue.append(vec1) | |
queue.append(vec2) | |
queue.pop(0) | |
break | |
if isConvex: | |
figs_vec.append(queue.pop(0)) | |
return figs_vec | |
@staticmethod | |
def hit_ray(x1, y1, x2, y2, x3, y3, x4, y4): | |
t1 = x3-x1 | |
t2 = y3-y1 | |
t3 = x2-x1 | |
t4 = y2-y1 | |
t5 = x4-x3 | |
t6 = y4-y3 | |
t7 = t4*t5 -t3*t6 | |
if t7 != 0: | |
a = (t5*t2 - t6*t1)/t7 | |
else: | |
a = 0 | |
px = x1+a*t3 | |
py = y1+a*t4 | |
b1 = b2Separator.is_on_segment(x2, y2, x1, y1, px, py) | |
b2 = b2Separator.is_on_segment(px, py, x3, y3, x4, y4) | |
if b1 and b2: | |
return Box2D.b2Vec2(px, py) | |
return None | |
@staticmethod | |
def hit_segment(x1, y1, x2, y2, x3, y3, x4, y4): | |
t1 = x3 - x1 | |
t2 = y3 - y1 | |
t3 = x2 - x1 | |
t4 = y2 - y1 | |
t5 = x4 - x3 | |
t6 = y4 - y3 | |
t7 = t4 * t5 - t3 * t6 | |
if t7 == 0: | |
return None | |
a = ((t5 * t2) - t6 * t1) / t7 | |
px = x1 + a * t3 | |
py = y1 + a * t4 | |
b1 = b2Separator.is_on_segment(px, py, x1, y1, x2, y2) | |
b2 = b2Separator.is_on_segment(px, py, x3, y3, x4, y4) | |
if b1 and b2: | |
return Box2D.b2Vec2(px, py) | |
return None | |
@staticmethod | |
def is_on_segment(px, py, x1, y1, x2, y2): | |
b1 = (x1 + 0.1 >= px >= x2 - 0.1) or (x1 - 0.1 <= px <= x2 + 0.1) | |
b2 = (y1 + 0.1 >= py >= y2 - 0.1) or (y1 - 0.1 <= py <= y2 + 0.1) | |
return b1 and b2 and b2Separator.is_on_line(px, py, x1, y1, x2, y2) | |
@staticmethod | |
def points_match(x1, y1, x2, y2): | |
dx = x2-x1 if x2>=x1 else x1-x2 | |
dy = y2-y1 if y2>=y1 else y1-y2 | |
return dx<0.1 and dy < 0.1 | |
@staticmethod | |
def is_on_line(px, py, x1, y1, x2, y2): | |
if x2-x1 > 0.1 or x1-x2 > 0.1: | |
a = (y2-y1)/(x2-x1) | |
possibleY = a*(px-x1)+y1 | |
diff = possibleY-py if possibleY>py else py-possibleY | |
return diff < 0.1 | |
return (px-x1<0.1) or (x1-px < 0.1) | |
@staticmethod | |
def det(x1, y1, x2, y2, x3, y3): | |
return x1*y2+x2*y3+x3*y1-y1*x2-y2*x3-y3*x1 | |
@staticmethod | |
def err(): | |
raise ArithmeticError('A problem has occurred. Use the validate() method to see where the problem is.') |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment