#==================================================================
#                           Physics.activity
#                     Helper classes and functions
#                           By Alex Levenson
#==================================================================
import math
# distance calculator, pt1 and pt2 are ordred pairs
def distance(pt1, pt2):
        return math.sqrt((pt1[0] - pt2[0]) ** 2 + (pt1[1] -pt2[1]) ** 2)

# returns the angle between the line segment from pt1 --> pt2 and the x axis, from -pi to pi
def getAngle(pt1,pt2):
    xcomp = pt2[0] - pt1[0]
    ycomp = pt1[1] - pt2[1]
    return math.atan2(ycomp,xcomp)

# returns a list of ordered pairs that describe an equilteral triangle around the segment from pt1 --> pt2
def constructTriangleFromLine(p1,p2):
    halfHeightVector = (0.57735*(p2[1] - p1[1]), 0.57735*(p2[0] - p1[0]))
    p3 = (p1[0] + halfHeightVector[0], p1[1] - halfHeightVector[1])
    p4 = (p1[0] - halfHeightVector[0], p1[1] + halfHeightVector[1])
    return [p2,p3,p4]

# returns the area of a polygon
def polyArea(vertices):
    n = len(vertices)
    A = 0
    p=n-1
    q=0
    while q<n:
        A+=vertices[p][0]*vertices[q][1] - vertices[q][0]*vertices[p][1]
        p=q
        q += 1
    return A/2.0

#Some polygon magic, thanks to John W. Ratcliff on www.flipcode.com
    
# returns true if pt is in triangle
def insideTriangle(pt,triangle):

    ax = triangle[2][0] - triangle[1][0]
    ay = triangle[2][1] - triangle[1][1]
    bx = triangle[0][0] - triangle[2][0]
    by = triangle[0][1] - triangle[2][1]
    cx = triangle[1][0] - triangle[0][0]
    cy = triangle[1][1] - triangle[0][1]
    apx= pt[0] - triangle[0][0]
    apy= pt[1] - triangle[0][1]
    bpx= pt[0] - triangle[1][0]
    bpy= pt[1] - triangle[1][1]
    cpx= pt[0] - triangle[2][0]
    cpy= pt[1] - triangle[2][1]
    
    aCROSSbp = ax*bpy - ay*bpx
    cCROSSap = cx*apy - cy*apx
    bCROSScp = bx*cpy - by*cpx  
    return aCROSSbp >= 0.0 and bCROSScp >= 0.0 and cCROSSap >= 0.0    

def polySnip(vertices,u,v,w,n):
    EPSILON = 0.0000000001
    
    Ax = vertices[u][0]
    Ay = vertices[u][1]
    
    Bx = vertices[v][0]
    By = vertices[v][1]
    
    Cx = vertices[w][0]
    Cy = vertices[w][1]
    
    if EPSILON > (((Bx-Ax)*(Cy-Ay)) - ((By-Ay)*(Cx-Ax))):  return False
    
    for p in range(0,n):
        if p == u or p == v or p == w: continue
        Px = vertices[p][0];
        Py = vertices[p][1];        
        if insideTriangle((Px,Py),((Ax,Ay),(Bx,By),(Cx,Cy))): return False;
    
    return True;
    
    
# decomposes a polygon into its triangles
def decomposePoly(vertices):
    vertices = list(vertices)
    n = len(vertices)
    result = []
    if(n < 3): return [] # not a poly!
    
    # force a counter-clockwise polygon
    if 0 >= polyArea(vertices):
        vertices.reverse()
    
    # remove nv-2 vertices, creating 1 triangle every time        
    nv = n
    count = 2*nv # error detection
    m=0
    v=nv-1
    while nv>2:
        count -= 1
        if 0>= count:
            return [] # Error -- probably bad polygon
        
        # three consecutive vertices
        u = v 
        if nv<=u: u = 0      # previous
        v = u+1
        if nv<=v: v = 0    # new v
        w = v+1
        if nv<=w: w = 0    # next

        if(polySnip(vertices,u,v,w,nv)):
            
            # record this triangle
            result.append((vertices[u],vertices[v],vertices[w]))
            
            m+=1
            # remove v from remaining polygon
            vertices.pop(v)
            nv -= 1
            # reset error detection
            count = 2*nv
    return result