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