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