hooke/util/quickhull.py

   1 # Copyright (c) 2009 W. Trevor King and the authors listed at the
   2 # following URL, and/or the authors of referenced articles or
   3 # incorporated external code:
   4 # http://en.literateprograms.org/Quickhull_(Python,_arrays)?action=history&offset=20090125100840
   5 #
   6 # Permission is hereby granted, free of charge, to any person obtaining
   7 # a copy of this software and associated documentation files (the
   8 # "Software"), to deal in the Software without restriction, including
   9 # without limitation the rights to use, copy, modify, merge, publish,
  10 # distribute, sublicense, and/or sell copies of the Software, and to
  11 # permit persons to whom the Software is furnished to do so, subject to
  12 # the following conditions:
  13 #
  14 # The above copyright notice and this permission notice shall be
  15 # included in all copies or substantial portions of the Software.
  16 #
  17 # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
  18 # EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
  19 # MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
  20 # IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
  21 # CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
  22 # TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
  23 # SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
  24 #
  25 # Retrieved from: http://en.literateprograms.org/Quickhull_(Python,_arrays)?oldid=16036
  26 # Modified by W. Trevor King to add Simplex and point_inside_hull
  27
  28 from numpy import *
  29
  30 link = lambda a,b: concatenate((a,b[1:]))
  31 edge = lambda a,b: concatenate(([a],[b]))
  32
  33 def qhull(sample):
  34     def dome(sample,base):
  35         h, t = base
  36         dists = dot(sample-h, dot(((0,-1),(1,0)),(t-h)))
  37         outer = repeat(sample, dists>0, 0)
  38
  39         if len(outer):
  40             pivot = sample[argmax(dists)]
  41             return link(dome(outer, edge(h, pivot)),
  42                         dome(outer, edge(pivot, t)))
  43         else:
  44             return base
  45
  46     if len(sample) > 2:
  47         axis = sample[:,0]
  48         base = take(sample, [argmin(axis), argmax(axis)], 0)
  49         return link(dome(sample, base),
  50                     dome(sample, base[::-1]))
  51     else:
  52         return sample
  53
  54 class Simplex(object): # N+1 points in N dimensions
  55     def __init__(self, vertices):
  56         self.vertices = vertices
  57         self.origin = self.vertices[0]
  58         self.basis = array([v-self.origin for v in self.vertices[1:]],
  59                            dtype=double).transpose()
  60         self.inverse_basis = linalg.inv(self.basis)
  61     def simplex_coordinates(self, points):
  62         rel_coords = (points-self.origin).transpose()
  63         return dot(self.inverse_basis, rel_coords).transpose()
  64     def inside_single_point_simplex_coords(self, point, tol=1e-15):
  65         # check that we're in the positive coordinate
  66         for coordinate in point:
  67             if coordinate < -tol:
  68                 return False
  69         # check that we're under the (1,1,1,...) plane
  70         if point.sum() > 1+tol:
  71             return False
  72         return True
  73     def inside(self, points):
  74         ps = self.simplex_coordinates(points)
  75         if not hasattr(ps[0], "__len__"): # single point
  76             return self.inside_single_point_simplex_coords(ps)
  77         else:
  78             insides = []
  79             for p in ps:
  80                 insides.append(self.inside_single_point_simplex_coords(p))
  81             return insides
  82
  83 def points_inside_hull(hull, points):
  84     if not hasattr(points[0], "__len__"): # single point
  85         points = numpy.array([points])
  86     inside = [False]*len(points)
  87     pivot = hull[0]
  88     for a,b in zip(hull[1:], hull[2:]):
  89         rb = b-pivot
  90         if vdot(rb, rb) == 0: # hull is a closed loop
  91             continue
  92         simplex = Simplex([pivot,a,b])
  93         simplex_inside = simplex.inside(points)
  94         if any(simplex_inside):
  95             for i,value in enumerate(simplex_inside):
  96                 if value == True:
  97                     inside[i] = True
  98     return inside
  99
 100 def print_postscript(sample, hull):
 101     print "%!"
 102     print "100 500 translate 2 2 scale 0 0 moveto"
 103     print "/tick {moveto 0 2 rlineto 0 -4 rlineto 0 2 rlineto"
 104     print "              2 0 rlineto -4 0 rlineto 2 0 rlineto} def"
 105     for (x,y) in sample:
 106         print x, y, "tick"
 107     print "stroke"
 108     print hull[0,0], hull[0,1], "moveto"
 109     for (x,y) in hull[1:]:
 110         print x, y, "lineto"
 111     print "closepath stroke showpage"
 112
 113 if __name__ == "__main__":
 114     #sample = 10*array([(x,y) for x in arange(10) for y in arange(10)])
 115     sample = 100*random.random((32,2))
 116     hull = qhull(sample)
 117     if all(point_inside_hull(hull, sample)) != True:
 118         for point in sample:
 119             assert point_inside_hull(hull, point) == True, \
 120                 "point %s should be in hull\n%s" % (sample, hull)
 121     apoints = hull[0:-1]
 122     bpoints = hull[1:]
 123     if any(point_inside_hull(hull, apoints - 0.1*(bpoints-apoints))) == True:
 124         for a,b in zip(apoints, bpoints):
 125             assert point_inside_hull(hull, a - 0.1*(b-a)) == False, \
 126                 "point %s (a=%s, b=%s) should be outside hull\n%s" \
 127                 % (point, a, b, hull)
 128     print_postscript(sample, hull)