From 392a0c5a4d19da8f06b3ccab8b9d7fdcd558e64d Mon Sep 17 00:00:00 2001 From: Brian Jordan Date: Fri, 27 Jun 2008 18:17:45 +0000 Subject: Initial commit --- (limited to 'elements/tools_poly.py') diff --git a/elements/tools_poly.py b/elements/tools_poly.py new file mode 100755 index 0000000..cd6c4c6 --- /dev/null +++ b/elements/tools_poly.py @@ -0,0 +1,440 @@ +""" +This file is part of the 'Elements' Project +Elements is a 2D Physics API for Python (supporting Box2D2) + +Copyright (C) 2008, The Elements Team, + +Home: http://elements.linuxuser.at +IRC: #elements on irc.freenode.org + +Code: http://www.assembla.com/wiki/show/elements + svn co http://svn2.assembla.com/svn/elements + +License: GPLv3 | See LICENSE for the full text +This program is free software: you can redistribute it and/or modify +it under the terms of the GNU General Public License as published by +the Free Software Foundation, either version 3 of the License, or +(at your option) any later version. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. + +You should have received a copy of the GNU General Public License +along with this program. If not, see . +""" +from functools import partial + +from math import fabs +from math import sqrt +from math import atan +from math import atan2 +from math import degrees +from math import acos + +from locals import * +from elements import box2d + +def ComputeCentroid(pd): + count = pd.vertexCount + + if count < 3: + return False + + c = box2d.b2Vec2(0, 0) + area = 0.0 + + # pRef is the reference point for forming triangles. + # It's location doesn't change the result (except for rounding error). + pRef = box2d.b2Vec2(0.0, 0.0) + + inv3 = 1.0 / 3.0 + + for i in range(count): + # Triangle vertices. + p1 = pRef + p2 = pd.getVertex(i) + if i + 1 < count: + p3 = pd.getVertex(i+1) + else: p3 = pd.getVertex(0) + + e1 = p2 - p1 + e2 = p3 - p1 + + D = box2d.b2Cross(e1, e2) + + triangleArea = 0.5 * D + area += triangleArea + + # Area weighted centroid + c += triangleArea * inv3 * (p1 + p2 + p3) + + # Centroid + # if area < FLT_EPSILON: + #raise ValueError, "ComputeCentroid: area < FLT_EPSILON" + + return c / area + +def checkDef(pd): + """Check the polygon definition for invalid vertices, etc. + + Return: True if valid, False if invalid + """ + +# if pd.vertexCount < 3 or pd.vertexCount > box2d.b2_maxPolygonVertices: + #raise ValueError, "Invalid vertexCount" + + threshold = FLT_EPSILON * FLT_EPSILON + verts = pd.getVertices_b2Vec2() + normals = [] + v0 = verts[0] + for i in range(pd.vertexCount): + if i == pd.vertexCount-1: + v1 = verts[0] + else: v1 = verts[i+1] + edge=v1 - v0 +# if edge.LengthSquared() < threshold: +# raise ValueError, "edge.LengthSquared < FLT_EPSILON**2" + normals.append( box2d.b2Cross(edge, 1.0) ) + normals[-1].Normalize() + v0=v1 + + centroid = ComputeCentroid(pd) + + d=box2d.b2Vec2() + for i in range(pd.vertexCount): + i1 = i - 1 + if i1 < 0: i1 = pd.vertexCount - 1 + i2 = i + n1 = normals[i1] + n2 = normals[i2] + v = verts[i] - centroid + + d.x = box2d.b2Dot(n1, v) - box2d.b2_toiSlop + d.y = box2d.b2Dot(n2, v) - box2d.b2_toiSlop + + # Shifting the edge inward by b2_toiSlop should + # not cause the plane to pass the centroid. + + # Your shape has a radius/extent less than b2_toiSlop. +# if d.x < 0.0 or d.y <= 0.0: +# raise ValueError, "Your shape has a radius/extent less than b2_toiSlop." + + A = box2d.b2Mat22() + A.col1.x = n1.x; A.col2.x = n1.y + A.col1.y = n2.x; A.col2.y = n2.y + #coreVertices[i] = A.Solve(d) + m_centroid + + return True + +def calc_center(points): + """ Calculate the center of a polygon + + Return: The center (x,y) + """ + tot_x, tot_y = 0,0 + for p in points: + tot_x += p[0] + tot_y += p[1] + n = len(points) + return (tot_x/n, tot_y/n) + +def poly_center_vertices(pointlist): + """ Rearranges vectors around the center + + Return: pointlist ([(x, y), ...]) + """ + poly_points_center = [] + center = cx, cy = calc_center(pointlist) + + for p in pointlist: + x = p[0] - cx + y = cy - p[1] + poly_points_center.append((x, y)) + + return poly_points_center + +def is_line(vertices, tolerance=25.0): + """ Check if passed vertices are a line. Done by comparing + the angles of all vectors and check tolerance. + + Parameters: + vertices ... a list of vertices (x, y) + tolerance .. how many degrees should be allowed max to be a line + + Returns: True if line, False if no line + """ + if len(vertices) <= 2: + return True + + # Step 1: Points -> Vectors + p_old = vertices[0] + alphas = [] + + + for p in vertices[1:]: + x1, y1 = p_old + x2, y2 = p + p_old = p + + # Create Vector + vx, vy = (x2-x1, y2-y1) + + # Check Length + l = sqrt((vx*vx) + (vy*vy)) + if l == 0.0: continue + + # Normalize vector + vx /= l + vy /= l + + # Append angle + if fabs(vx) < 0.2: alpha = 90.0 + else: alpha = degrees(atan(vy / vx)) + + alphas.append(fabs(alpha)) + + # Sort angles + alphas.sort() + + # Get maximum difference + alpha_diff = fabs(alphas[-1] - alphas[0]) + print "alpha difference:", alpha_diff + + if alpha_diff < tolerance: + return True + else: + return False + +def reduce_poly_by_angle(vertices, tolerance=10.0, minlen=20): + """ This function reduces a poly by the angles of the vectors (detect lines) + If the angle difference from one vector to the last > tolerance: use last point + If the angle is quite the same, it's on the line + + Parameters: + vertices ... a list of vertices (x, y) + tolerance .. how many degrees should be allowed max + + Returns: (1) New Pointlist, (2) Soft reduced pointlist (reduce_poly()) + """ + v_last = vertices[-1] + vertices = vxx = reduce_poly(vertices, minlen) + + p_new = [] + p_new.append(vertices[0]) + + dir = None + is_convex = True + + for i in xrange(len(vertices)-1): + if i == 0: + p_old = vertices[i] + continue + + x1, y1 = p_old + x2, y2 = vertices[i] + x3, y3 = vertices[i+1] + p_old = vertices[i] + + # Create Vectors + v1x = (x2 - x1) * 1.0 + v1y = (y2 - y1) * 1.0 + + v2x = (x3 - x2) * 1.0 + v2y = (y3 - y2) * 1.0 + + # Calculate angle + a = ((v1x * v2x) + (v1y * v2y)) + b = sqrt((v1x*v1x) + (v1y*v1y)) + c = sqrt((v2x*v2x) + (v2y*v2y)) + + # No Division by 0 :) + if (b*c) == 0.0: continue + + # Get the current degrees + # We have a bug here sometimes... + try: + angle = degrees(acos(a / (b*c))) + except: + # cos=1.0 + print "cos=", a/(b*c) + continue + + # Check if inside tolerance + if fabs(angle) > tolerance: + p_new.append(vertices[i]) + # print "x", 180-angle, is_left(vertices[i-1], vertices[i], vertices[i+1]) + + # Check if convex: + if dir == None: + dir = is_left(vertices[i-1], vertices[i], vertices[i+1]) + else: + if dir != is_left(vertices[i-1], vertices[i], vertices[i+1]): + is_convex = False + + # We also want to append the last point :) + p_new.append(v_last) + + # Returns: (1) New Pointlist, (2) Soft reduced pointlist (reduce_poly()) + return p_new, is_convex + + """ OLD FUNCTION: """ + # Wipe all points too close to each other + vxx = vertices = reduce_poly(vertices, minlen) + + # Create Output List + p_new = [] + p_new.append(vertices[0]) + + # Set the starting vertice + p_old = vertices[0] + alpha_old = None + + # For each vector, compare the angle difference to the last one + for i in range(1, len(vertices)): + x1, y1 = p_old + x2, y2 = vertices[i] + p_old = (x2, y2) + + # Make Vector + vx, vy = (x2-x1, y2-y1) + + # Vector length + l = sqrt((vx*vx) + (vy*vy)) + + # normalize + vx /= l + vy /= l + + # Get Angle + if fabs(vx) < 0.2: + alpha = 90 + else: + alpha = degrees(atan(vy * 1.0) / (vx*1.0)) + + if alpha_old == None: + alpha_old = alpha + continue + + # Get difference to previous angle + alpha_diff = fabs(alpha - alpha_old) + alpha_old = alpha + + # If the new vector differs from the old one, we add the old point + # to the output list, as the line changed it's way :) + if alpha_diff > tolerance: + #print ">",alpha_diff, "\t", vx, vy, l + p_new.append(vertices[i-1]) + + # We also want to append the last point :) + p_new.append(vertices[-1]) + + # Returns: (1) New Pointlist, (2) Soft reduced pointlist (reduce_poly()) + return p_new, vxx + + +# The following functions is_left, reduce_poly and convex_hull are +# from the pymunk project (http://code.google.com/p/pymunk/) +def is_left(p0, p1, p2): + """Test if p2 is left, on or right of the (infinite) line (p0,p1). + + :return: > 0 for p2 left of the line through p0 and p1 + = 0 for p2 on the line + < 0 for p2 right of the line + """ + sorting = (p1[0] - p0[0])*(p2[1]-p0[1]) - (p2[0]-p0[0])*(p1[1]-p0[1]) + if sorting > 0: return 1 + elif sorting < 0: return -1 + else: return 0 + +def is_convex(points): + """Test if a polygon (list of (x,y)) is strictly convex or not. + + :return: True if the polygon is convex, False otherwise + """ + #assert len(points) > 2, "not enough points to form a polygon" + + p0 = points[0] + p1 = points[1] + p2 = points[2] + + xc, yc = 0, 0 + is_same_winding = is_left(p0, p1, p2) + for p2 in points[2:] + [p0] + [p1]: + if is_same_winding != is_left(p0, p1, p2): + return False + a = p1[0] - p0[0], p1[1] - p0[1] # p1-p0 + b = p2[0] - p1[0], p2[1] - p1[1] # p2-p1 + if sign(a[0]) != sign(b[0]): xc +=1 + if sign(a[1]) != sign(b[1]): yc +=1 + p0, p1 = p1, p2 + + return xc <= 2 and yc <= 2 + +def sign(x): + if x < 0: return -1 + else: return 1 + + +def reduce_poly(points, tolerance=50): + """Remove close points to simplify a polyline + tolerance is the min distance between two points squared. + + :return: The reduced polygon as a list of (x,y) + """ + curr_p = points[0] + reduced_ps = [points[0]] + + for p in points[1:]: + x1, y1 = curr_p + x2, y2 = p + dx = fabs(x2 - x1) + dy = fabs(y2 - y1) + l = sqrt((dx*dx) + (dy*dy)) + if l > tolerance: + curr_p = p + reduced_ps.append(p) + + return reduced_ps + +def convex_hull(points): + """Create a convex hull from a list of points. + This function uses the Graham Scan Algorithm. + + :return: Convex hull as a list of (x,y) + """ + ### Find lowest rightmost point + p0 = points[0] + for p in points[1:]: + if p[1] < p0[1]: + p0 = p + elif p[1] == p0[1] and p[0] > p0[0]: + p0 = p + points.remove(p0) + + ### Sort the points angularly about p0 as center + f = partial(is_left, p0) + points.sort(cmp = f) + points.reverse() + points.insert(0, p0) + + ### Find the hull points + hull = [p0, points[1]] + + for p in points[2:]: + + pt1 = hull[-1] + pt2 = hull[-2] + l = is_left(pt2, pt1, p) + if l > 0: + hull.append(p) + else: + while l <= 0 and len(hull) > 2: + hull.pop() + pt1 = hull[-1] + pt2 = hull[-2] + l = is_left(pt2, pt1, p) + hull.append(p) + return hull + -- cgit v0.9.1