Cura/util/polygon.py

   1 """
   2 The polygon module has functions that assist in working with 2D convex polygons.
   3 """
   4 __copyright__ = "Copyright (C) 2013 David Braam - Released under terms of the AGPLv3 License"
   5
   6 import numpy
   7
   8
   9 def _isLeft(a, b, c):
  10         """ Check if C is left of the infinite line from A to B """
  11         return ((b[0] - a[0])*(c[1] - a[1]) - (b[1] - a[1])*(c[0] - a[0])) > 0
  12
  13
  14 def _isRightTurn((p, q, r)):
  15         sum1 = q[0]*r[1] + p[0]*q[1] + r[0]*p[1]
  16         sum2 = q[0]*p[1] + r[0]*q[1] + p[0]*r[1]
  17
  18         if sum1 - sum2 < 0:
  19                 return 1
  20         else:
  21                 return 0
  22
  23
  24 def convexHull(pointList):
  25         """ Create a convex hull from a list of points. """
  26         unique = {}
  27         for p in pointList:
  28                 unique[p[0], p[1]] = 1
  29
  30         points = unique.keys()
  31         points.sort()
  32         if len(points) < 1:
  33                 return numpy.zeros((0, 2), numpy.float32)
  34         if len(points) < 2:
  35                 return numpy.array(points, numpy.float32)
  36
  37         # Build upper half of the hull.
  38         upper = [points[0], points[1]]
  39         for p in points[2:]:
  40                 upper.append(p)
  41                 while len(upper) > 2 and not _isRightTurn(upper[-3:]):
  42                         del upper[-2]
  43
  44         # Build lower half of the hull.
  45         points = points[::-1]
  46         lower = [points[0], points[1]]
  47         for p in points[2:]:
  48                 lower.append(p)
  49                 while len(lower) > 2 and not _isRightTurn(lower[-3:]):
  50                         del lower[-2]
  51
  52         # Remove duplicates.
  53         del lower[0]
  54         del lower[-1]
  55
  56         return numpy.array(upper + lower, numpy.float32)
  57
  58
  59 def minkowskiHull(a, b):
  60         """Calculate the minkowski hull of 2 convex polygons"""
  61         points = numpy.zeros((len(a) * len(b), 2))
  62         for n in xrange(0, len(a)):
  63                 for m in xrange(0, len(b)):
  64                         points[n * len(b) + m] = a[n] + b[m]
  65         return convexHull(points.copy())
  66
  67
  68 def _projectPoly(poly, normal):
  69         """
  70         Project a convex polygon on a given normal.
  71         A projection of a convex polygon on a infinite line is a finite line.
  72         Give the min and max value on the normal line.
  73         """
  74         pMin = numpy.dot(normal, poly[0])
  75         pMax = pMin
  76         for n in xrange(1 , len(poly)):
  77                 p = numpy.dot(normal, poly[n])
  78                 pMin = min(pMin, p)
  79                 pMax = max(pMax, p)
  80         return pMin, pMax
  81
  82
  83 def polygonCollision(polyA, polyB):
  84         """ Check if convexy polygon A and B collide, return True if this is the case. """
  85         for n in xrange(0, len(polyA)):
  86                 p0 = polyA[n-1]
  87                 p1 = polyA[n]
  88                 normal = (p1 - p0)[::-1]
  89                 normal[1] = -normal[1]
  90                 normal /= numpy.linalg.norm(normal)
  91                 aMin, aMax = _projectPoly(polyA, normal)
  92                 bMin, bMax = _projectPoly(polyB, normal)
  93                 if aMin > bMax:
  94                         return False
  95                 if bMin > aMax:
  96                         return False
  97         for n in xrange(0, len(polyB)):
  98                 p0 = polyB[n-1]
  99                 p1 = polyB[n]
 100                 normal = (p1 - p0)[::-1]
 101                 normal[1] = -normal[1]
 102                 normal /= numpy.linalg.norm(normal)
 103                 aMin, aMax = _projectPoly(polyA, normal)
 104                 bMin, bMax = _projectPoly(polyB, normal)
 105                 if aMin > bMax:
 106                         return False
 107                 if bMin > aMax:
 108                         return False
 109         return True
 110
 111
 112 def polygonCollisionPushVector(polyA, polyB):
 113         """ Check if convex polygon A and B collide, return the vector of penetration if this is the case, else return False. """
 114         retSize = 10000000.0
 115         ret = False
 116         for n in xrange(0, len(polyA)):
 117                 p0 = polyA[n-1]
 118                 p1 = polyA[n]
 119                 normal = (p1 - p0)[::-1]
 120                 normal[1] = -normal[1]
 121                 normal /= numpy.linalg.norm(normal)
 122                 aMin, aMax = _projectPoly(polyA, normal)
 123                 bMin, bMax = _projectPoly(polyB, normal)
 124                 if aMin > bMax:
 125                         return False
 126                 if bMin > aMax:
 127                         return False
 128                 size = min(aMax, bMax) - max(aMin, bMin)
 129                 if size < retSize:
 130                         ret = normal * (size + 0.1)
 131                         retSize = size
 132         for n in xrange(0, len(polyB)):
 133                 p0 = polyB[n-1]
 134                 p1 = polyB[n]
 135                 normal = (p1 - p0)[::-1]
 136                 normal[1] = -normal[1]
 137                 normal /= numpy.linalg.norm(normal)
 138                 aMin, aMax = _projectPoly(polyA, normal)
 139                 bMin, bMax = _projectPoly(polyB, normal)
 140                 if aMin > bMax:
 141                         return False
 142                 if bMin > aMax:
 143                         return False
 144                 size = min(aMax, bMax) - max(aMin, bMin)
 145                 if size < retSize:
 146                         ret = normal * -(size + 0.1)
 147                         retSize = size
 148         return ret
 149
 150
 151 def fullInside(polyA, polyB):
 152         """
 153         Check if convex polygon A is completely inside of convex polygon B.
 154         """
 155         for n in xrange(0, len(polyA)):
 156                 p0 = polyA[n-1]
 157                 p1 = polyA[n]
 158                 normal = (p1 - p0)[::-1]
 159                 normal[1] = -normal[1]
 160                 normal /= numpy.linalg.norm(normal)
 161                 aMin, aMax = _projectPoly(polyA, normal)
 162                 bMin, bMax = _projectPoly(polyB, normal)
 163                 if aMax > bMax:
 164                         return False
 165                 if aMin < bMin:
 166                         return False
 167         for n in xrange(0, len(polyB)):
 168                 p0 = polyB[n-1]
 169                 p1 = polyB[n]
 170                 normal = (p1 - p0)[::-1]
 171                 normal[1] = -normal[1]
 172                 normal /= numpy.linalg.norm(normal)
 173                 aMin, aMax = _projectPoly(polyA, normal)
 174                 bMin, bMax = _projectPoly(polyB, normal)
 175                 if aMax > bMax:
 176                         return False
 177                 if aMin < bMin:
 178                         return False
 179         return True
 180
 181
 182 def lineLineIntersection(p0, p1, p2, p3):
 183         """ Return the intersection of the infinite line trough points p0 and p1 and infinite line trough points p2 and p3. """
 184         A1 = p1[1] - p0[1]
 185         B1 = p0[0] - p1[0]
 186         C1 = A1*p0[0] + B1*p0[1]
 187
 188         A2 = p3[1] - p2[1]
 189         B2 = p2[0] - p3[0]
 190         C2 = A2 * p2[0] + B2 * p2[1]
 191
 192         det = A1*B2 - A2*B1
 193         if det == 0:
 194                 return p0
 195         return [(B2*C1 - B1*C2)/det, (A1 * C2 - A2 * C1) / det]
 196
 197
 198 def clipConvex(poly0, poly1):
 199         """ Cut the convex polygon 0 so that it completely fits in convex polygon 1, any part sticking out of polygon 1 is cut off """
 200         res = poly0
 201         for p1idx in xrange(0, len(poly1)):
 202                 src = res
 203                 res = []
 204                 p0 = poly1[p1idx-1]
 205                 p1 = poly1[p1idx]
 206                 for n in xrange(0, len(src)):
 207                         p = src[n]
 208                         if not _isLeft(p0, p1, p):
 209                                 if _isLeft(p0, p1, src[n-1]):
 210                                         res.append(lineLineIntersection(p0, p1, src[n-1], p))
 211                                 res.append(p)
 212                         elif not _isLeft(p0, p1, src[n-1]):
 213                                 res.append(lineLineIntersection(p0, p1, src[n-1], p))
 214         return numpy.array(res, numpy.float32)