Module TeachMyAgent.environments.envs.Box2D_dynamics.water_dynamics

Expand source code
import Box2D
from Box2D.b2 import (edgeShape, circleShape, fixtureDef, polygonShape, revoluteJointDef, contactListener)
from copy import copy
from TeachMyAgent.environments.envs.utils.custom_user_data import CustomUserDataObjectTypes
import numpy as np

class WaterDynamics(object):
    '''
        Calculate forces to simulate in a simplified way water physics.
        Most of the forces implemented here are taken from https://www.iforce2d.net/b2dtut/buoyancy except the push force implemented by Clément Romac.
    '''
    def __init__(self, gravity, drag_mod=0.25, lift_mod=0.25, push_mod=0.05, max_drag=2000, max_lift=500, max_push=20):
        self.gravity = gravity
        self.drag_mod = drag_mod
        self.lift_mod = lift_mod
        self.max_drag = max_drag
        self.max_lift = max_lift
        self.push_mod = push_mod
        self.max_push = max_push

    def compute_centroids(self, vectors):
        count = len(vectors)
        assert count >= 3

        c = Box2D.b2Vec2(0, 0)
        area = 0

        ref_point = Box2D.b2Vec2(0, 0)
        inv3 = 1/3

        for i in range(count):
            # Triangle vertices
            p1 = ref_point
            p2 = vectors[i]
            p3 = vectors[i+1] if i+1 < count else vectors[0]

            e1 = p2 - p1
            e2 = p3 - p1

            d = Box2D.b2Cross(e1, e2)

            triangle_area = 0.5 * d
            area += triangle_area

            # Area weighted centroid
            c += triangle_area * inv3 * (p1 + p2 + p3)

        if area > Box2D.b2_epsilon:
            c *= 1/area
        else:
            area = 0

        return c, area

    def inside(self, cp1, cp2, p):
        return (cp2.x-cp1.x)*(p.y-cp1.y) > (cp2.y-cp1.y)*(p.x-cp1.x)

    def intersection(self, cp1, cp2, s, e):
        dc = Box2D.b2Vec2(cp1.x - cp2.x, cp1.y - cp2.y)
        dp = Box2D.b2Vec2(s.x - e.x, s.y - e.y)
        n1 = cp1.x * cp2.y - cp1.y * cp2.x
        n2 = s.x * e.y - s.y * e.x
        n3 = 1.0 / (dc.x * dp.y - dc.y * dp.x)
        return Box2D.b2Vec2((n1*dp.x - n2*dc.x) * n3,
                            (n1*dp.y - n2*dc.y) * n3)

    def find_intersection(self, fixture_A, fixture_B):
        # assert polygons TODO
        output_vertices = []
        polygon_A = fixture_A.shape
        polygon_B = fixture_B.shape

        #fill 'subject polygon' from fixtureA polygon
        for i in range(len(polygon_A.vertices)):
            output_vertices.append(fixture_A.body.GetWorldPoint(polygon_A.vertices[i]))

        #fill 'clip polygon' from fixtureB polygon
        clip_polygon = []
        for i in range(len(polygon_B.vertices)):
            clip_polygon.append(fixture_B.body.GetWorldPoint(polygon_B.vertices[i]))

        cp1 = clip_polygon[len(clip_polygon) - 1]
        for j in range(len(clip_polygon)):
            cp2 = clip_polygon[j]
            if len(output_vertices) == 0:
                break

            input_list = copy(output_vertices)
            output_vertices.clear()

            s = input_list[len(input_list) - 1]
            for i in range(len(input_list)):
                e = input_list[i]
                if self.inside(cp1, cp2, e):
                    if not self.inside(cp1, cp2, s):
                        output_vertices.append(self.intersection(cp1, cp2, s, e))
                    output_vertices.append(e)
                elif self.inside(cp1, cp2, s):
                    output_vertices.append(self.intersection(cp1, cp2, s, e))
                s = e
            cp1 = cp2
        return len(output_vertices) != 0, output_vertices

    def calculate_forces(self, fixture_pairs):
        for pair in fixture_pairs:
            density = pair[0].density

            has_intersection, intersection_points = self.find_intersection(pair[0], pair[1])

            if has_intersection:
                centroid, area = self.compute_centroids(intersection_points)

                # apply buoyancy force
                displaced_mass = pair[0].density * area
                buoyancy_force = displaced_mass * -self.gravity
                pair[1].body.ApplyForce(force=buoyancy_force, point=centroid, wake=True)

                # apply complex drag
                for i in range(len(intersection_points)):
                    v0 = intersection_points[i]
                    v1 = intersection_points[(i+1)%len(intersection_points)]
                    mid_point = 0.5 * (v0 + v1)

                    ##### DRAG
                    # find relative velocity between object and fluid at edge midpoint
                    vel_dir = pair[1].body.GetLinearVelocityFromWorldPoint(mid_point) - \
                              pair[0].body.GetLinearVelocityFromWorldPoint(mid_point)
                    vel = vel_dir.Normalize()

                    edge = v1 - v0
                    edge_length = edge.Normalize()
                    normal = Box2D.b2Cross(-1, edge)
                    drag_dot = Box2D.b2Dot(normal, vel_dir)
                    if drag_dot >= 0: # normal points backwards - this is not a leading edge
                        # apply drag
                        drag_mag = drag_dot * self.drag_mod * edge_length * density * vel * vel
                        drag_mag = min(drag_mag, self.max_drag)
                        drag_force = drag_mag * -vel_dir
                        pair[1].body.ApplyForce(force=drag_force,
                                                point=mid_point,
                                                wake=True)

                        # apply lift
                        lift_dot = Box2D.b2Dot(edge, vel_dir)
                        lift_mag = drag_dot * lift_dot * self.lift_mod * edge_length * density * vel * vel
                        lift_mag = min(lift_mag, self.max_lift)
                        lift_dir = Box2D.b2Cross(1, vel_dir)
                        lift_force = lift_mag * lift_dir
                        pair[1].body.ApplyForce(force=lift_force,
                                                point=mid_point,
                                                wake=True)
                    ##### PUSH
                    # Apply a linear force to an object linked to a rotation joint applying torque.
                    # Torque and angular inertia are used to calculate the magnitude of the linear force

                    body_to_check = pair[1].body
                    # Simplification /!\
                    # joint = pair[1].body.joints[0].joint
                    # joints_to_check = [joint_edge.joint for joint_edge in body_to_check.joints]
                    joints_to_check = [joint_edge.joint for joint_edge in body_to_check.joints if
                                 joint_edge.joint.bodyB == body_to_check]

                    for joint in joints_to_check:
                        if joint.lowerLimit < joint.angle < joint.upperLimit:
                            torque = joint.GetMotorTorque(60)

                            # Calculate angular inertia of the object
                            moment_of_inertia = body_to_check.inertia
                            angular_velocity = body_to_check.angularVelocity
                            angular_inertia = moment_of_inertia * angular_velocity

                            # Calculate the force applied to the object
                            world_center = body_to_check.worldCenter
                            anchor = joint.anchorB
                            lever_vector = world_center - anchor # vector from pivot to point of application of the force
                            force_applied_at_center = Box2D.b2Cross(lever_vector, -torque)

                            push_dot = Box2D.b2Dot(normal, force_applied_at_center)
                            if push_dot > 0:
                                vel = torque + angular_inertia
                                push_mag = push_dot * self.push_mod * edge_length * density * vel * vel # Wrong approximation /!\
                                # push_mag = min(push_mag, self.max_push)
                                push_force = np.clip(push_mag * -force_applied_at_center, -self.max_push, self.max_push)
                                body_to_check.ApplyForce(force=push_force,
                                                        point=joint.anchorB,#body_to_check.worldCenter,
                                                        wake=True)

class WaterContactDetector(contactListener):
    '''
        Store fixtures having contact with water.
    '''
    def __init__(self):
        super(WaterContactDetector, self).__init__()
        self.fixture_pairs = []
    def BeginContact(self, contact):
        if contact.fixtureA.body.userData.object_type == CustomUserDataObjectTypes.WATER and \
                        contact.fixtureB.body.userData.object_type == CustomUserDataObjectTypes.BODY_OBJECT:
            self.fixture_pairs.append((contact.fixtureA,
                                       contact.fixtureB))
        elif contact.fixtureB.body.userData.object_type == CustomUserDataObjectTypes.WATER and \
                        contact.fixtureA.body.userData.object_type == CustomUserDataObjectTypes.BODY_OBJECT:
            self.fixture_pairs.append((contact.fixtureB,
                                       contact.fixtureA))

    def EndContact(self, contact):
        if contact.fixtureA.body.userData.object_type == CustomUserDataObjectTypes.WATER and \
                        contact.fixtureB.body.userData.object_type == CustomUserDataObjectTypes.BODY_OBJECT:
            self.fixture_pairs.remove((contact.fixtureA,
                                       contact.fixtureB))
        elif contact.fixtureB.body.userData.object_type == CustomUserDataObjectTypes.WATER and \
                        contact.fixtureA.body.userData.object_type == CustomUserDataObjectTypes.BODY_OBJECT:
            self.fixture_pairs.remove((contact.fixtureB,
                                       contact.fixtureA))

    def Reset(self):
        self.fixture_pairs = []

Classes

class WaterContactDetector

Store fixtures having contact with water.

Expand source code
class WaterContactDetector(contactListener):
    '''
        Store fixtures having contact with water.
    '''
    def __init__(self):
        super(WaterContactDetector, self).__init__()
        self.fixture_pairs = []
    def BeginContact(self, contact):
        if contact.fixtureA.body.userData.object_type == CustomUserDataObjectTypes.WATER and \
                        contact.fixtureB.body.userData.object_type == CustomUserDataObjectTypes.BODY_OBJECT:
            self.fixture_pairs.append((contact.fixtureA,
                                       contact.fixtureB))
        elif contact.fixtureB.body.userData.object_type == CustomUserDataObjectTypes.WATER and \
                        contact.fixtureA.body.userData.object_type == CustomUserDataObjectTypes.BODY_OBJECT:
            self.fixture_pairs.append((contact.fixtureB,
                                       contact.fixtureA))

    def EndContact(self, contact):
        if contact.fixtureA.body.userData.object_type == CustomUserDataObjectTypes.WATER and \
                        contact.fixtureB.body.userData.object_type == CustomUserDataObjectTypes.BODY_OBJECT:
            self.fixture_pairs.remove((contact.fixtureA,
                                       contact.fixtureB))
        elif contact.fixtureB.body.userData.object_type == CustomUserDataObjectTypes.WATER and \
                        contact.fixtureA.body.userData.object_type == CustomUserDataObjectTypes.BODY_OBJECT:
            self.fixture_pairs.remove((contact.fixtureB,
                                       contact.fixtureA))

    def Reset(self):
        self.fixture_pairs = []

Ancestors

  • Box2D.Box2D.b2ContactListener

Methods

def BeginContact(self, contact)

b2ContactListener_BeginContact(b2ContactListener self, b2Contact contact)

Called when two fixtures begin to touch.

Expand source code
def BeginContact(self, contact):
    if contact.fixtureA.body.userData.object_type == CustomUserDataObjectTypes.WATER and \
                    contact.fixtureB.body.userData.object_type == CustomUserDataObjectTypes.BODY_OBJECT:
        self.fixture_pairs.append((contact.fixtureA,
                                   contact.fixtureB))
    elif contact.fixtureB.body.userData.object_type == CustomUserDataObjectTypes.WATER and \
                    contact.fixtureA.body.userData.object_type == CustomUserDataObjectTypes.BODY_OBJECT:
        self.fixture_pairs.append((contact.fixtureB,
                                   contact.fixtureA))
def EndContact(self, contact)

b2ContactListener_EndContact(b2ContactListener self, b2Contact contact)

Called when two fixtures cease to touch.

Expand source code
def EndContact(self, contact):
    if contact.fixtureA.body.userData.object_type == CustomUserDataObjectTypes.WATER and \
                    contact.fixtureB.body.userData.object_type == CustomUserDataObjectTypes.BODY_OBJECT:
        self.fixture_pairs.remove((contact.fixtureA,
                                   contact.fixtureB))
    elif contact.fixtureB.body.userData.object_type == CustomUserDataObjectTypes.WATER and \
                    contact.fixtureA.body.userData.object_type == CustomUserDataObjectTypes.BODY_OBJECT:
        self.fixture_pairs.remove((contact.fixtureB,
                                   contact.fixtureA))
def Reset(self)
Expand source code
def Reset(self):
    self.fixture_pairs = []
class WaterDynamics (gravity, drag_mod=0.25, lift_mod=0.25, push_mod=0.05, max_drag=2000, max_lift=500, max_push=20)

Calculate forces to simulate in a simplified way water physics. Most of the forces implemented here are taken from https://www.iforce2d.net/b2dtut/buoyancy except the push force implemented by Clément Romac.

Expand source code
class WaterDynamics(object):
    '''
        Calculate forces to simulate in a simplified way water physics.
        Most of the forces implemented here are taken from https://www.iforce2d.net/b2dtut/buoyancy except the push force implemented by Clément Romac.
    '''
    def __init__(self, gravity, drag_mod=0.25, lift_mod=0.25, push_mod=0.05, max_drag=2000, max_lift=500, max_push=20):
        self.gravity = gravity
        self.drag_mod = drag_mod
        self.lift_mod = lift_mod
        self.max_drag = max_drag
        self.max_lift = max_lift
        self.push_mod = push_mod
        self.max_push = max_push

    def compute_centroids(self, vectors):
        count = len(vectors)
        assert count >= 3

        c = Box2D.b2Vec2(0, 0)
        area = 0

        ref_point = Box2D.b2Vec2(0, 0)
        inv3 = 1/3

        for i in range(count):
            # Triangle vertices
            p1 = ref_point
            p2 = vectors[i]
            p3 = vectors[i+1] if i+1 < count else vectors[0]

            e1 = p2 - p1
            e2 = p3 - p1

            d = Box2D.b2Cross(e1, e2)

            triangle_area = 0.5 * d
            area += triangle_area

            # Area weighted centroid
            c += triangle_area * inv3 * (p1 + p2 + p3)

        if area > Box2D.b2_epsilon:
            c *= 1/area
        else:
            area = 0

        return c, area

    def inside(self, cp1, cp2, p):
        return (cp2.x-cp1.x)*(p.y-cp1.y) > (cp2.y-cp1.y)*(p.x-cp1.x)

    def intersection(self, cp1, cp2, s, e):
        dc = Box2D.b2Vec2(cp1.x - cp2.x, cp1.y - cp2.y)
        dp = Box2D.b2Vec2(s.x - e.x, s.y - e.y)
        n1 = cp1.x * cp2.y - cp1.y * cp2.x
        n2 = s.x * e.y - s.y * e.x
        n3 = 1.0 / (dc.x * dp.y - dc.y * dp.x)
        return Box2D.b2Vec2((n1*dp.x - n2*dc.x) * n3,
                            (n1*dp.y - n2*dc.y) * n3)

    def find_intersection(self, fixture_A, fixture_B):
        # assert polygons TODO
        output_vertices = []
        polygon_A = fixture_A.shape
        polygon_B = fixture_B.shape

        #fill 'subject polygon' from fixtureA polygon
        for i in range(len(polygon_A.vertices)):
            output_vertices.append(fixture_A.body.GetWorldPoint(polygon_A.vertices[i]))

        #fill 'clip polygon' from fixtureB polygon
        clip_polygon = []
        for i in range(len(polygon_B.vertices)):
            clip_polygon.append(fixture_B.body.GetWorldPoint(polygon_B.vertices[i]))

        cp1 = clip_polygon[len(clip_polygon) - 1]
        for j in range(len(clip_polygon)):
            cp2 = clip_polygon[j]
            if len(output_vertices) == 0:
                break

            input_list = copy(output_vertices)
            output_vertices.clear()

            s = input_list[len(input_list) - 1]
            for i in range(len(input_list)):
                e = input_list[i]
                if self.inside(cp1, cp2, e):
                    if not self.inside(cp1, cp2, s):
                        output_vertices.append(self.intersection(cp1, cp2, s, e))
                    output_vertices.append(e)
                elif self.inside(cp1, cp2, s):
                    output_vertices.append(self.intersection(cp1, cp2, s, e))
                s = e
            cp1 = cp2
        return len(output_vertices) != 0, output_vertices

    def calculate_forces(self, fixture_pairs):
        for pair in fixture_pairs:
            density = pair[0].density

            has_intersection, intersection_points = self.find_intersection(pair[0], pair[1])

            if has_intersection:
                centroid, area = self.compute_centroids(intersection_points)

                # apply buoyancy force
                displaced_mass = pair[0].density * area
                buoyancy_force = displaced_mass * -self.gravity
                pair[1].body.ApplyForce(force=buoyancy_force, point=centroid, wake=True)

                # apply complex drag
                for i in range(len(intersection_points)):
                    v0 = intersection_points[i]
                    v1 = intersection_points[(i+1)%len(intersection_points)]
                    mid_point = 0.5 * (v0 + v1)

                    ##### DRAG
                    # find relative velocity between object and fluid at edge midpoint
                    vel_dir = pair[1].body.GetLinearVelocityFromWorldPoint(mid_point) - \
                              pair[0].body.GetLinearVelocityFromWorldPoint(mid_point)
                    vel = vel_dir.Normalize()

                    edge = v1 - v0
                    edge_length = edge.Normalize()
                    normal = Box2D.b2Cross(-1, edge)
                    drag_dot = Box2D.b2Dot(normal, vel_dir)
                    if drag_dot >= 0: # normal points backwards - this is not a leading edge
                        # apply drag
                        drag_mag = drag_dot * self.drag_mod * edge_length * density * vel * vel
                        drag_mag = min(drag_mag, self.max_drag)
                        drag_force = drag_mag * -vel_dir
                        pair[1].body.ApplyForce(force=drag_force,
                                                point=mid_point,
                                                wake=True)

                        # apply lift
                        lift_dot = Box2D.b2Dot(edge, vel_dir)
                        lift_mag = drag_dot * lift_dot * self.lift_mod * edge_length * density * vel * vel
                        lift_mag = min(lift_mag, self.max_lift)
                        lift_dir = Box2D.b2Cross(1, vel_dir)
                        lift_force = lift_mag * lift_dir
                        pair[1].body.ApplyForce(force=lift_force,
                                                point=mid_point,
                                                wake=True)
                    ##### PUSH
                    # Apply a linear force to an object linked to a rotation joint applying torque.
                    # Torque and angular inertia are used to calculate the magnitude of the linear force

                    body_to_check = pair[1].body
                    # Simplification /!\
                    # joint = pair[1].body.joints[0].joint
                    # joints_to_check = [joint_edge.joint for joint_edge in body_to_check.joints]
                    joints_to_check = [joint_edge.joint for joint_edge in body_to_check.joints if
                                 joint_edge.joint.bodyB == body_to_check]

                    for joint in joints_to_check:
                        if joint.lowerLimit < joint.angle < joint.upperLimit:
                            torque = joint.GetMotorTorque(60)

                            # Calculate angular inertia of the object
                            moment_of_inertia = body_to_check.inertia
                            angular_velocity = body_to_check.angularVelocity
                            angular_inertia = moment_of_inertia * angular_velocity

                            # Calculate the force applied to the object
                            world_center = body_to_check.worldCenter
                            anchor = joint.anchorB
                            lever_vector = world_center - anchor # vector from pivot to point of application of the force
                            force_applied_at_center = Box2D.b2Cross(lever_vector, -torque)

                            push_dot = Box2D.b2Dot(normal, force_applied_at_center)
                            if push_dot > 0:
                                vel = torque + angular_inertia
                                push_mag = push_dot * self.push_mod * edge_length * density * vel * vel # Wrong approximation /!\
                                # push_mag = min(push_mag, self.max_push)
                                push_force = np.clip(push_mag * -force_applied_at_center, -self.max_push, self.max_push)
                                body_to_check.ApplyForce(force=push_force,
                                                        point=joint.anchorB,#body_to_check.worldCenter,
                                                        wake=True)

Methods

def calculate_forces(self, fixture_pairs)
Expand source code
def calculate_forces(self, fixture_pairs):
    for pair in fixture_pairs:
        density = pair[0].density

        has_intersection, intersection_points = self.find_intersection(pair[0], pair[1])

        if has_intersection:
            centroid, area = self.compute_centroids(intersection_points)

            # apply buoyancy force
            displaced_mass = pair[0].density * area
            buoyancy_force = displaced_mass * -self.gravity
            pair[1].body.ApplyForce(force=buoyancy_force, point=centroid, wake=True)

            # apply complex drag
            for i in range(len(intersection_points)):
                v0 = intersection_points[i]
                v1 = intersection_points[(i+1)%len(intersection_points)]
                mid_point = 0.5 * (v0 + v1)

                ##### DRAG
                # find relative velocity between object and fluid at edge midpoint
                vel_dir = pair[1].body.GetLinearVelocityFromWorldPoint(mid_point) - \
                          pair[0].body.GetLinearVelocityFromWorldPoint(mid_point)
                vel = vel_dir.Normalize()

                edge = v1 - v0
                edge_length = edge.Normalize()
                normal = Box2D.b2Cross(-1, edge)
                drag_dot = Box2D.b2Dot(normal, vel_dir)
                if drag_dot >= 0: # normal points backwards - this is not a leading edge
                    # apply drag
                    drag_mag = drag_dot * self.drag_mod * edge_length * density * vel * vel
                    drag_mag = min(drag_mag, self.max_drag)
                    drag_force = drag_mag * -vel_dir
                    pair[1].body.ApplyForce(force=drag_force,
                                            point=mid_point,
                                            wake=True)

                    # apply lift
                    lift_dot = Box2D.b2Dot(edge, vel_dir)
                    lift_mag = drag_dot * lift_dot * self.lift_mod * edge_length * density * vel * vel
                    lift_mag = min(lift_mag, self.max_lift)
                    lift_dir = Box2D.b2Cross(1, vel_dir)
                    lift_force = lift_mag * lift_dir
                    pair[1].body.ApplyForce(force=lift_force,
                                            point=mid_point,
                                            wake=True)
                ##### PUSH
                # Apply a linear force to an object linked to a rotation joint applying torque.
                # Torque and angular inertia are used to calculate the magnitude of the linear force

                body_to_check = pair[1].body
                # Simplification /!\
                # joint = pair[1].body.joints[0].joint
                # joints_to_check = [joint_edge.joint for joint_edge in body_to_check.joints]
                joints_to_check = [joint_edge.joint for joint_edge in body_to_check.joints if
                             joint_edge.joint.bodyB == body_to_check]

                for joint in joints_to_check:
                    if joint.lowerLimit < joint.angle < joint.upperLimit:
                        torque = joint.GetMotorTorque(60)

                        # Calculate angular inertia of the object
                        moment_of_inertia = body_to_check.inertia
                        angular_velocity = body_to_check.angularVelocity
                        angular_inertia = moment_of_inertia * angular_velocity

                        # Calculate the force applied to the object
                        world_center = body_to_check.worldCenter
                        anchor = joint.anchorB
                        lever_vector = world_center - anchor # vector from pivot to point of application of the force
                        force_applied_at_center = Box2D.b2Cross(lever_vector, -torque)

                        push_dot = Box2D.b2Dot(normal, force_applied_at_center)
                        if push_dot > 0:
                            vel = torque + angular_inertia
                            push_mag = push_dot * self.push_mod * edge_length * density * vel * vel # Wrong approximation /!\
                            # push_mag = min(push_mag, self.max_push)
                            push_force = np.clip(push_mag * -force_applied_at_center, -self.max_push, self.max_push)
                            body_to_check.ApplyForce(force=push_force,
                                                    point=joint.anchorB,#body_to_check.worldCenter,
                                                    wake=True)
def compute_centroids(self, vectors)
Expand source code
def compute_centroids(self, vectors):
    count = len(vectors)
    assert count >= 3

    c = Box2D.b2Vec2(0, 0)
    area = 0

    ref_point = Box2D.b2Vec2(0, 0)
    inv3 = 1/3

    for i in range(count):
        # Triangle vertices
        p1 = ref_point
        p2 = vectors[i]
        p3 = vectors[i+1] if i+1 < count else vectors[0]

        e1 = p2 - p1
        e2 = p3 - p1

        d = Box2D.b2Cross(e1, e2)

        triangle_area = 0.5 * d
        area += triangle_area

        # Area weighted centroid
        c += triangle_area * inv3 * (p1 + p2 + p3)

    if area > Box2D.b2_epsilon:
        c *= 1/area
    else:
        area = 0

    return c, area
def find_intersection(self, fixture_A, fixture_B)
Expand source code
def find_intersection(self, fixture_A, fixture_B):
    # assert polygons TODO
    output_vertices = []
    polygon_A = fixture_A.shape
    polygon_B = fixture_B.shape

    #fill 'subject polygon' from fixtureA polygon
    for i in range(len(polygon_A.vertices)):
        output_vertices.append(fixture_A.body.GetWorldPoint(polygon_A.vertices[i]))

    #fill 'clip polygon' from fixtureB polygon
    clip_polygon = []
    for i in range(len(polygon_B.vertices)):
        clip_polygon.append(fixture_B.body.GetWorldPoint(polygon_B.vertices[i]))

    cp1 = clip_polygon[len(clip_polygon) - 1]
    for j in range(len(clip_polygon)):
        cp2 = clip_polygon[j]
        if len(output_vertices) == 0:
            break

        input_list = copy(output_vertices)
        output_vertices.clear()

        s = input_list[len(input_list) - 1]
        for i in range(len(input_list)):
            e = input_list[i]
            if self.inside(cp1, cp2, e):
                if not self.inside(cp1, cp2, s):
                    output_vertices.append(self.intersection(cp1, cp2, s, e))
                output_vertices.append(e)
            elif self.inside(cp1, cp2, s):
                output_vertices.append(self.intersection(cp1, cp2, s, e))
            s = e
        cp1 = cp2
    return len(output_vertices) != 0, output_vertices
def inside(self, cp1, cp2, p)
Expand source code
def inside(self, cp1, cp2, p):
    return (cp2.x-cp1.x)*(p.y-cp1.y) > (cp2.y-cp1.y)*(p.x-cp1.x)
def intersection(self, cp1, cp2, s, e)
Expand source code
def intersection(self, cp1, cp2, s, e):
    dc = Box2D.b2Vec2(cp1.x - cp2.x, cp1.y - cp2.y)
    dp = Box2D.b2Vec2(s.x - e.x, s.y - e.y)
    n1 = cp1.x * cp2.y - cp1.y * cp2.x
    n2 = s.x * e.y - s.y * e.x
    n3 = 1.0 / (dc.x * dp.y - dc.y * dp.x)
    return Box2D.b2Vec2((n1*dp.x - n2*dc.x) * n3,
                        (n1*dp.y - n2*dc.y) * n3)