Cura/util/polygon.py

   1 __copyright__ = "Copyright (C) 2013 David Braam - Released under terms of the AGPLv3 License"
   2
   3 import numpy
   4
   5 def convexHull(pointList):
   6         def _isRightTurn((p, q, r)):
   7                 sum1 = q[0]*r[1] + p[0]*q[1] + r[0]*p[1]
   8                 sum2 = q[0]*p[1] + r[0]*q[1] + p[0]*r[1]
   9
  10                 if sum1 - sum2 < 0:
  11                         return 1
  12                 else:
  13                         return 0
  14
  15         unique = {}
  16         for p in pointList:
  17                 unique[p[0],p[1]] = 1
  18
  19         points = unique.keys()
  20         points.sort()
  21
  22         # Build upper half of the hull.
  23         upper = [points[0], points[1]]
  24         for p in points[2:]:
  25                 upper.append(p)
  26                 while len(upper) > 2 and not _isRightTurn(upper[-3:]):
  27                         del upper[-2]
  28
  29         # Build lower half of the hull.
  30         points = points[::-1]
  31         lower = [points[0], points[1]]
  32         for p in points[2:]:
  33                 lower.append(p)
  34                 while len(lower) > 2 and not _isRightTurn(lower[-3:]):
  35                         del lower[-2]
  36
  37         # Remove duplicates.
  38         del lower[0]
  39         del lower[-1]
  40
  41         return numpy.array(upper + lower, numpy.float32) - numpy.array([0.0,0.0], numpy.float32)
  42
  43 def minkowskiHull(a, b):
  44         points = numpy.zeros((len(a) * len(b), 2))
  45         for n in xrange(0, len(a)):
  46                 for m in xrange(0, len(b)):
  47                         points[n * len(b) + m] = a[n] + b[m]
  48         return convexHull(points.copy())
  49
  50 def projectPoly(poly, normal):
  51         pMin = numpy.dot(normal, poly[0])
  52         pMax = pMin
  53         for n in xrange(1 , len(poly)):
  54                 p = numpy.dot(normal, poly[n])
  55                 pMin = min(pMin, p)
  56                 pMax = max(pMax, p)
  57         return pMin, pMax
  58
  59 def polygonCollision(polyA, polyB):
  60         for n in xrange(0, len(polyA)):
  61                 p0 = polyA[n-1]
  62                 p1 = polyA[n]
  63                 normal = (p1 - p0)[::-1]
  64                 normal[1] = -normal[1]
  65                 normal /= numpy.linalg.norm(normal)
  66                 aMin, aMax = projectPoly(polyA, normal)
  67                 bMin, bMax = projectPoly(polyB, normal)
  68                 if aMin > bMax:
  69                         return False
  70                 if bMin > aMax:
  71                         return False
  72         for n in xrange(0, len(polyB)):
  73                 p0 = polyB[n-1]
  74                 p1 = polyB[n]
  75                 normal = (p1 - p0)[::-1]
  76                 normal[1] = -normal[1]
  77                 normal /= numpy.linalg.norm(normal)
  78                 aMin, aMax = projectPoly(polyA, normal)
  79                 bMin, bMax = projectPoly(polyB, normal)
  80                 if aMin > bMax:
  81                         return False
  82                 if aMax < bMin:
  83                         return False
  84         return True
  85
  86 def polygonCollisionPushVector(polyA, polyB):
  87         retSize = 10000000.0
  88         ret = False
  89         for n in xrange(0, len(polyA)):
  90                 p0 = polyA[n-1]
  91                 p1 = polyA[n]
  92                 normal = (p1 - p0)[::-1]
  93                 normal[1] = -normal[1]
  94                 normal /= numpy.linalg.norm(normal)
  95                 aMin, aMax = projectPoly(polyA, normal)
  96                 bMin, bMax = projectPoly(polyB, normal)
  97                 if aMin > bMax:
  98                         return False
  99                 if bMin > aMax:
 100                         return False
 101                 size = min(bMax, bMax) - max(aMin, bMin)
 102                 if size < retSize:
 103                         ret = normal * (size + 0.1)
 104                         retSize = size
 105         for n in xrange(0, len(polyB)):
 106                 p0 = polyB[n-1]
 107                 p1 = polyB[n]
 108                 normal = (p1 - p0)[::-1]
 109                 normal[1] = -normal[1]
 110                 normal /= numpy.linalg.norm(normal)
 111                 aMin, aMax = projectPoly(polyA, normal)
 112                 bMin, bMax = projectPoly(polyB, normal)
 113                 if aMin > bMax:
 114                         return False
 115                 if aMax < bMin:
 116                         return False
 117                 size = min(bMax, bMax) - max(aMin, bMin)
 118                 if size < retSize:
 119                         ret = normal * -(size + 0.1)
 120                         retSize = size
 121         return ret
 122
 123 #Check if polyA is fully inside of polyB.
 124 def fullInside(polyA, polyB):
 125         for n in xrange(0, len(polyA)):
 126                 p0 = polyA[n-1]
 127                 p1 = polyA[n]
 128                 normal = (p1 - p0)[::-1]
 129                 normal[1] = -normal[1]
 130                 normal /= numpy.linalg.norm(normal)
 131                 aMin, aMax = projectPoly(polyA, normal)
 132                 bMin, bMax = projectPoly(polyB, normal)
 133                 if aMax > bMax:
 134                         return False
 135                 if aMin < bMin:
 136                         return False
 137         for n in xrange(0, len(polyB)):
 138                 p0 = polyB[n-1]
 139                 p1 = polyB[n]
 140                 normal = (p1 - p0)[::-1]
 141                 normal[1] = -normal[1]
 142                 normal /= numpy.linalg.norm(normal)
 143                 aMin, aMax = projectPoly(polyA, normal)
 144                 bMin, bMax = projectPoly(polyB, normal)
 145                 if aMax > bMax:
 146                         return False
 147                 if aMin < bMin:
 148                         return False
 149         return True
 150
 151 def isLeft(a, b, c):
 152         return ((b[0] - a[0])*(c[1] - a[1]) - (b[1] - a[1])*(c[0] - a[0])) > 0
 153
 154 def lineLineIntersection(p0, p1, p2, p3):
 155         A1 = p1[1] - p0[1]
 156         B1 = p0[0] - p1[0]
 157         C1 = A1*p0[0] + B1*p0[1]
 158
 159         A2 = p3[1] - p2[1]
 160         B2 = p2[0] - p3[0]
 161         C2 = A2 * p2[0] + B2 * p2[1]
 162
 163         det = A1*B2 - A2*B1
 164         if det == 0:
 165                 return p0
 166         return [(B2*C1 - B1*C2)/det, (A1 * C2 - A2 * C1) / det]
 167
 168 def clipConvex(poly0, poly1):
 169         res = poly0
 170         for p1idx in xrange(0, len(poly1)):
 171                 src = res
 172                 res = []
 173                 p0 = poly1[p1idx-1]
 174                 p1 = poly1[p1idx]
 175                 for n in xrange(0, len(src)):
 176                         p = src[n]
 177                         if not isLeft(p0, p1, p):
 178                                 if isLeft(p0, p1, src[n-1]):
 179                                         res.append(lineLineIntersection(p0, p1, src[n-1], p))
 180                                 res.append(p)
 181                         elif not isLeft(p0, p1, src[n-1]):
 182                                 res.append(lineLineIntersection(p0, p1, src[n-1], p))
 183         return numpy.array(res, numpy.float32)