b2_separator.py

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.')