├── .gitignore ├── __init__.py ├── examples ├── rectangle ├── simple ├── misshapen ├── the_sacred_polygon ├── half_iron_cross ├── holey ├── south_africa └── florida ├── README.md ├── demo.py ├── polyskel.py └── euclid.py /.gitignore: -------------------------------------------------------------------------------- 1 | *.py[cod] 2 | -------------------------------------------------------------------------------- /__init__.py: -------------------------------------------------------------------------------- 1 | from polyskel import skeletonize, set_debug, log 2 | -------------------------------------------------------------------------------- /examples/rectangle: -------------------------------------------------------------------------------- 1 | 40, 40 2 | 40, 310 3 | 520, 310 4 | 520, 40 5 | -------------------------------------------------------------------------------- /examples/simple: -------------------------------------------------------------------------------- 1 | 30, 20 2 | 30, 120 3 | 90, 70 4 | 160, 140 5 | 178, 93 6 | 160, 20 7 | -------------------------------------------------------------------------------- /examples/misshapen: -------------------------------------------------------------------------------- 1 | 100, 50 2 | 150, 150 3 | 50, 100 4 | 50, 250 5 | 150, 250 6 | 50, 350 7 | 350, 350 8 | 350, 100 9 | 250, 150 10 | 300, 50 11 | -------------------------------------------------------------------------------- /examples/the_sacred_polygon: -------------------------------------------------------------------------------- 1 | # straight from Aichholzer 2 | 40, 50 3 | 40, 520 4 | 625,425 5 | 500,325 6 | 635,250 7 | 635,10 8 | 250,40 9 | 200,200 10 | 100,50 11 | -------------------------------------------------------------------------------- /examples/half_iron_cross: -------------------------------------------------------------------------------- 1 | # classic example of a degenerate polygon producing vertex events 2 | 100, 50 3 | 150, 150 4 | 50, 100 5 | 50, 350 6 | 350, 350 7 | 350, 100 8 | 250, 150 9 | 300, 50 10 | -------------------------------------------------------------------------------- /examples/holey: -------------------------------------------------------------------------------- 1 | # straight from Felkel 2 | 30,100 3 | 50, 200 4 | 220,240 5 | 440,240 6 | 430,40 7 | 230,30, 8 | 85,40 9 | --- # hole separator, vertices of the hole contour are in clockwise order 10 | 175,85 11 | 245,140 12 | 315,90 13 | 385,160 14 | 330,200 15 | 165,180 16 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | This is an implementation of [Botffy's polyskel](https://github.com/Botffy/polyskel) in Python 3.5. [pyeuclid](https://github.com/ezag/pyeuclid) for geometry computation is also updated for the compatibility and included in this repository. 2 | 3 | See Botffy's polyskel for details. 4 | -------------------------------------------------------------------------------- /examples/south_africa: -------------------------------------------------------------------------------- 1 | # south africa 2 | 162, 387 3 | 186, 401 4 | 337, 368 5 | 434, 267 6 | 442, 241 7 | 416, 250 8 | 402, 237 9 | 429, 200 10 | 417, 144 11 | 377, 139 12 | 301, 212 13 | 254, 203 14 | 225, 236 15 | 203, 236 16 | 193, 195 17 | 193, 269 18 | 176, 281 19 | 140, 276 20 | 134, 262 21 | 125, 273 22 | 144, 315 23 | --- 24 | # lesotho 25 | 345, 316 26 | 336, 291 27 | 358, 275 28 | 372, 290 29 | -------------------------------------------------------------------------------- /examples/florida: -------------------------------------------------------------------------------- 1 | 208, 131 2 | 213, 142 3 | 168, 141 4 | 260, 168 5 | 246, 149 6 | 277, 142 7 | 271, 163 8 | 302, 180 9 | 268, 173 10 | 305, 196 11 | 319, 225 12 | 367, 214 13 | 423, 169 14 | 471, 160 15 | 540, 208 16 | 588, 268 17 | 616, 270 18 | 644, 308 19 | 630, 446 20 | 647, 472 21 | 641, 459 22 | 656, 467 23 | 660, 450 24 | 646, 423 25 | 687, 447 26 | 666, 495 27 | 651, 495 28 | 711, 580 29 | 728, 584 30 | 714, 557 31 | 746, 560 32 | 735, 569 33 | 744, 617 34 | 769, 594 35 | 753, 624 36 | 771, 628 37 | 793, 700 38 | 842, 708 39 | 871, 759 40 | 902, 780 41 | 891, 788 42 | 871, 773 43 | 887, 799 44 | 947, 774 45 | 964, 782 46 | 978, 689 47 | 985, 678 48 | 990, 695 49 | 984, 555 50 | 868, 338 51 | 854, 294 52 | 869, 316 53 | 887, 314 54 | 892, 366 55 | 895, 322 56 | 805, 196 57 | 747, 61 58 | 759, 59 59 | 753, 43 60 | 691, 33 61 | 683, 98 62 | 661, 72 63 | 355, 83 64 | 333, 46 65 | 35, 70 66 | 70, 144 67 | 50, 165 68 | 77, 154 69 | 87, 125 70 | 99, 139 71 | 106, 118 72 | 122, 139 73 | 89, 152 74 | 169, 124 75 | -------------------------------------------------------------------------------- /demo.py: -------------------------------------------------------------------------------- 1 | import logging 2 | import argparse 3 | import re 4 | from PIL import Image, ImageDraw 5 | import polyskel 6 | 7 | if __name__ == "__main__": 8 | logging.basicConfig() 9 | 10 | argparser = argparse.ArgumentParser(description="Construct the straight skeleton of a polygon. The polygon is to be given as a counter-clockwise series of vertices specified by their coordinates: see the example files for the exact format.") 11 | argparser.add_argument('polygon_file', metavar="", type=argparse.FileType('r'), help="text file describing the polygon ('-' for standard input)") 12 | argparser.add_argument('--verbose', '--v', action="store_true", default=False, help="Show construction of the skeleton") 13 | argparser.add_argument('--log', dest="loglevel", choices=['DEBUG', 'INFO', 'WARNING', 'ERROR'], default='WARNING', help="Set log level") 14 | args = argparser.parse_args() 15 | 16 | polyskel.log.setLevel(getattr(logging, args.loglevel)) 17 | polygon_line_pat = re.compile(r"\s*(?P\d+(\.\d+)?)\s*,\s*(?P\d+(\.\d+)?)\s*(#.*)?") 18 | 19 | contours = [] 20 | poly = [] 21 | for line in args.polygon_file: 22 | line = line.strip() 23 | if not line or line.startswith('#'): 24 | continue 25 | 26 | if line.startswith('-'): 27 | contours.append(poly) 28 | poly = [] 29 | continue 30 | 31 | match = polygon_line_pat.match(line) 32 | poly.append((float(match.group("coord_x")), float(match.group("coord_y")))) 33 | 34 | if not args.polygon_file.isatty(): 35 | args.polygon_file.close() 36 | 37 | contours.append(poly) 38 | poly = contours[0] 39 | holes = contours[1:] if len(contours) > 0 else None 40 | bbox_end_x = int(max(poly, key=lambda x: x[0])[0]+20) 41 | bbox_end_y = int(max(poly, key=lambda x: x[1])[1]+20) 42 | 43 | im = Image.new("RGB", (bbox_end_x, bbox_end_y), "white"); 44 | draw = ImageDraw.Draw(im); 45 | if args.verbose: 46 | polyskel.set_debug((im, draw)) 47 | 48 | 49 | for contour in contours: 50 | for point, next in zip(contour, contour[1:]+contour[:1]): 51 | draw.line((point[0], point[1], next[0], next[1]), fill=0) 52 | print(poly) 53 | print(holes) 54 | skeleton = polyskel.skeletonize(poly, holes) 55 | 56 | for res in skeleton: 57 | print(res) 58 | 59 | for arc in skeleton: 60 | for sink in arc.sinks: 61 | draw.line((arc.source.x, arc.source.y, sink.x, sink.y), fill="red") 62 | im.show(); 63 | -------------------------------------------------------------------------------- /polyskel.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | 3 | """ 4 | Implementation of the straight skeleton algorithm as described by Felkel and Obdržálek in their 1998 conference paper Straight skeleton implementation. 5 | """ 6 | 7 | import logging 8 | import heapq 9 | from euclid import * 10 | from itertools import * 11 | from collections import namedtuple 12 | 13 | log = logging.getLogger("__name__") 14 | 15 | class Debug: 16 | def __init__(self, image): 17 | if image is not None: 18 | self.im = image[0] 19 | self.draw = image[1] 20 | self.do = True 21 | else: 22 | self.do = False 23 | 24 | def line(self, *args, **kwargs): 25 | if self.do: 26 | self.draw.line(*args, **kwargs) 27 | 28 | def rectangle(self, *args, **kwargs): 29 | if self.do: 30 | self.draw.rectangle(*args, **kwargs) 31 | 32 | def show(self): 33 | if self.do: 34 | self.im.show() 35 | 36 | _debug = Debug(None) 37 | 38 | def set_debug(image): 39 | global _debug 40 | _debug = Debug(image) 41 | 42 | def _window(lst): 43 | prevs, items, nexts = tee(lst, 3) 44 | prevs = islice(cycle(prevs), len(lst)-1, None) 45 | nexts = islice(cycle(nexts), 1, None) 46 | return zip(prevs, items, nexts) 47 | 48 | def _cross(a, b): 49 | res = a.x*b.y - b.x*a.y 50 | return res 51 | 52 | def _approximately_equals(a, b): 53 | return a==b or (abs(a-b) <= max(abs(a), abs(b)) * 0.001) 54 | 55 | def _approximately_same(point_a, point_b): 56 | return _approximately_equals(point_a.x, point_b.x) and _approximately_equals(point_a.y, point_b.y) 57 | 58 | def _normalize_contour(contour): 59 | contour = [Point2(float(x), float(y)) for (x,y) in contour] 60 | return [point for prev, point, next in _window(contour) if not (point==next or (point-prev).normalized() == (next-point).normalized())] 61 | 62 | 63 | class _SplitEvent(namedtuple("_SplitEvent", "distance, intersection_point, vertex, opposite_edge")): 64 | __slots__ = () 65 | def __str__(self): 66 | return "{} Split event @ {} from {} to {}".format(self.distance, self.intersection_point, self.vertex, self.opposite_edge) 67 | 68 | class _EdgeEvent(namedtuple("_EdgeEvent", "distance intersection_point vertex_a vertex_b")): 69 | __slots__ = () 70 | def __str__(self): 71 | return "{} Edge event @ {} between {} and {}".format(self.distance, self.intersection_point, self.vertex_a, self.vertex_b) 72 | 73 | _OriginalEdge = namedtuple("_OriginalEdge", "edge bisector_left, bisector_right") 74 | 75 | Subtree = namedtuple("Subtree", "source, height, sinks") 76 | 77 | def _side(point, line): 78 | a = line.p.x 79 | b = line.p.y 80 | 81 | class _LAVertex: 82 | def __init__(self, point, edge_left, edge_right, direction_vectors=None): 83 | self.point = point 84 | self.edge_left = edge_left 85 | self.edge_right = edge_right 86 | self.prev = None 87 | self.next = None 88 | self.lav = None 89 | self._valid = True; # this should be handled better. Maybe membership in lav implies validity? 90 | 91 | creator_vectors = (edge_left.v.normalized()*-1, edge_right.v.normalized()) 92 | if direction_vectors is None: 93 | direction_vectors = creator_vectors 94 | 95 | self._is_reflex = (_cross(*direction_vectors)) < 0 96 | self._bisector = Ray2(self.point, operator.add(*creator_vectors) * (-1 if self.is_reflex else 1)) 97 | log.info("Created vertex %s", self.__repr__()) 98 | _debug.line((self.bisector.p.x, self.bisector.p.y, self.bisector.p.x+self.bisector.v.x*100, self.bisector.p.y+self.bisector.v.y*100), fill="blue") 99 | 100 | @property 101 | def bisector(self): 102 | return self._bisector 103 | 104 | @property 105 | def is_reflex(self): 106 | return self._is_reflex 107 | 108 | @property 109 | def original_edges(self): 110 | return self.lav._slav._original_edges 111 | 112 | def next_event(self): 113 | events = [] 114 | if self.is_reflex: 115 | # a reflex vertex may generate a split event 116 | # split events happen when a vertex hits an opposite edge, splitting the polygon in two. 117 | log.debug("looking for split candidates for vertex %s", self) 118 | for edge in self.original_edges: 119 | if edge.edge == self.edge_left or edge.edge == self.edge_right: 120 | continue 121 | 122 | log.debug("\tconsidering EDGE %s", edge) 123 | 124 | # a potential b is at the intersection of between our own bisector and the bisector of the 125 | # angle between the tested edge and any one of our own edges. 126 | 127 | # we choose the "less parallel" edge (in order to exclude a potentially parallel edge) 128 | leftdot = abs(self.edge_left.v.normalized().dot(edge.edge.v.normalized())) 129 | rightdot = abs(self.edge_right.v.normalized().dot(edge.edge.v.normalized())) 130 | selfedge = self.edge_left if leftdot < rightdot else self.edge_right 131 | otheredge = self.edge_left if leftdot > rightdot else self.edge_right 132 | 133 | i = Line2(selfedge).intersect(Line2(edge.edge)) 134 | if i is not None and not _approximately_equals(i, self.point): 135 | #locate candidate b 136 | linvec = (self.point - i).normalized() 137 | edvec = edge.edge.v.normalized() 138 | if linvec.dot(edvec)<0: 139 | edvec = -edvec 140 | 141 | bisecvec = edvec + linvec 142 | if abs(bisecvec) == 0: 143 | continue 144 | bisector = Line2(i, bisecvec) 145 | b = bisector.intersect(self.bisector) 146 | 147 | if b is None: 148 | continue 149 | 150 | #check eligibility of b 151 | # a valid b should lie within the area limited by the edge and the bisectors of its two vertices: 152 | xleft = _cross(edge.bisector_left.v.normalized(), (b - edge.bisector_left.p).normalized()) > 0 153 | xright = _cross(edge.bisector_right.v.normalized(), (b - edge.bisector_right.p).normalized()) < 0 154 | xedge = _cross(edge.edge.v.normalized(), (b - edge.edge.p).normalized()) < 0 155 | 156 | if not (xleft and xright and xedge): 157 | log.debug("\t\tDiscarded candidate %s (%s-%s-%s)", b, xleft, xright, xedge) 158 | continue 159 | 160 | log.debug("\t\tFound valid candidate %s", b) 161 | events.append( _SplitEvent(Line2(edge.edge).distance(b), b, self, edge.edge) ) 162 | 163 | i_prev = self.bisector.intersect(self.prev.bisector) 164 | i_next = self.bisector.intersect(self.next.bisector) 165 | 166 | if i_prev is not None: 167 | events.append(_EdgeEvent(Line2(self.edge_left).distance(i_prev), i_prev, self.prev, self)) 168 | if i_next is not None: 169 | events.append(_EdgeEvent(Line2(self.edge_right).distance(i_next), i_next, self, self.next)) 170 | 171 | if not events: 172 | return None 173 | 174 | ev = min(events, key=lambda event: self.point.distance(event.intersection_point)) 175 | 176 | log.info("Generated new event for %s: %s", self, ev) 177 | return ev 178 | 179 | def invalidate(self): 180 | if self.lav is not None: 181 | self.lav.invalidate(self) 182 | else: 183 | self._valid = False 184 | 185 | @property 186 | def is_valid(self): 187 | return self._valid 188 | 189 | def __str__(self): 190 | return "Vertex ({:.2f};{:.2f})".format(self.point.x, self.point.y) 191 | 192 | def __lt__(self, other): 193 | if isinstance(other, _LAVertex): 194 | return self.point.x < other.point.x 195 | 196 | def __repr__(self): 197 | return "Vertex ({}) ({:.2f};{:.2f}), bisector {}, edges {} {}".format("reflex" if self.is_reflex else "convex", self.point.x, self.point.y, self.bisector, self.edge_left, self.edge_right) 198 | 199 | class _SLAV: 200 | def __init__(self, polygon, holes): 201 | contours = [_normalize_contour(polygon)] 202 | contours.extend([_normalize_contour(hole) for hole in holes]) 203 | 204 | self._lavs = [ _LAV.from_polygon(contour, self) for contour in contours ] 205 | 206 | #store original polygon edges for calculating split events 207 | self._original_edges = [_OriginalEdge(LineSegment2(vertex.prev.point, vertex.point), vertex.prev.bisector, vertex.bisector) for vertex in chain.from_iterable(self._lavs)] 208 | 209 | def __iter__(self): 210 | for lav in self._lavs: 211 | yield lav 212 | 213 | def __len__(self): 214 | return len(self._lavs) 215 | 216 | def empty(self): 217 | return len(self._lavs) == 0 218 | 219 | def handle_edge_event(self, event): 220 | sinks = [] 221 | events = [] 222 | 223 | lav = event.vertex_a.lav 224 | if event.vertex_a.prev == event.vertex_b.next: 225 | log.info("%.2f Peak event at intersection %s from <%s,%s,%s> in %s", event.distance, event.intersection_point, event.vertex_a, event.vertex_b, event.vertex_a.prev, lav) 226 | self._lavs.remove(lav) 227 | for vertex in list(lav): 228 | sinks.append(vertex.point) 229 | vertex.invalidate() 230 | else: 231 | log.info("%.2f Edge event at intersection %s from <%s,%s> in %s", event.distance, event.intersection_point, event.vertex_a, event.vertex_b,lav) 232 | new_vertex = lav.unify(event.vertex_a, event.vertex_b, event.intersection_point) 233 | if lav.head in (event.vertex_a, event.vertex_b): 234 | lav.head = new_vertex 235 | sinks.extend((event.vertex_a.point, event.vertex_b.point)) 236 | next_event = new_vertex.next_event() 237 | if next_event is not None: 238 | events.append(next_event) 239 | 240 | return (Subtree(event.intersection_point, event.distance, sinks), events) 241 | 242 | def handle_split_event(self, event): 243 | lav = event.vertex.lav 244 | log.info("%.2f Split event at intersection %s from vertex %s, for edge %s in %s", event.distance, event.intersection_point, event.vertex, event.opposite_edge, lav) 245 | 246 | sinks = [event.vertex.point] 247 | vertices = [] 248 | x = None # right vertex 249 | y = None # left vertex 250 | norm = event.opposite_edge.v.normalized() 251 | for v in chain.from_iterable(self._lavs): 252 | log.debug("%s in %s", v, v.lav) 253 | if norm == v.edge_left.v.normalized() and event.opposite_edge.p == v.edge_left.p: 254 | x = v 255 | y = x.prev 256 | elif norm == v.edge_right.v.normalized() and event.opposite_edge.p == v.edge_right.p: 257 | y=v 258 | x=y.next 259 | 260 | if x: 261 | xleft = _cross(y.bisector.v.normalized(), (event.intersection_point - y.point).normalized()) >= 0 262 | xright = _cross(x.bisector.v.normalized(), (event.intersection_point - x.point).normalized()) <= 0 263 | log.debug("Vertex %s holds edge as %s edge (%s, %s)", v, ("left" if x==v else "right"), xleft, xright) 264 | 265 | if xleft and xright: 266 | break 267 | else: 268 | x = None 269 | y = None 270 | 271 | if x is None: 272 | log.info("Failed split event %s (equivalent edge event is expected to follow)", event) 273 | return (None,[]) 274 | 275 | v1 = _LAVertex(event.intersection_point, event.vertex.edge_left, event.opposite_edge) 276 | v2 = _LAVertex(event.intersection_point, event.opposite_edge, event.vertex.edge_right) 277 | 278 | v1.prev = event.vertex.prev 279 | v1.next = x 280 | event.vertex.prev.next = v1 281 | x.prev = v1 282 | 283 | v2.prev = y 284 | v2.next = event.vertex.next 285 | event.vertex.next.prev = v2 286 | y.next = v2 287 | 288 | new_lavs = None 289 | self._lavs.remove(lav) 290 | if lav != x.lav: 291 | # the split event actually merges two lavs 292 | self._lavs.remove(x.lav) 293 | new_lavs = [_LAV.from_chain(v1, self)] 294 | else: 295 | new_lavs = [_LAV.from_chain(v1, self), _LAV.from_chain(v2, self)] 296 | 297 | for l in new_lavs: 298 | log.debug(l) 299 | if len(l) > 2: 300 | self._lavs.append(l) 301 | vertices.append(l.head) 302 | else: 303 | log.info("LAV %s has collapsed into the line %s--%s", l, l.head.point, l.head.next.point) 304 | sinks.append(l.head.next.point) 305 | for v in list(l): 306 | v.invalidate() 307 | 308 | events = [] 309 | for vertex in vertices: 310 | next_event = vertex.next_event() 311 | if next_event is not None: 312 | events.append(next_event) 313 | 314 | event.vertex.invalidate() 315 | return (Subtree(event.intersection_point, event.distance, sinks), events) 316 | 317 | class _LAV: 318 | def __init__(self, slav): 319 | self.head = None 320 | self._slav = slav 321 | self._len = 0 322 | log.debug("Created LAV %s", self) 323 | 324 | 325 | @classmethod 326 | def from_polygon(cls, polygon, slav): 327 | lav = cls(slav) 328 | for prev, point, next in _window(polygon): 329 | lav._len += 1 330 | vertex = _LAVertex(point, LineSegment2(prev, point), LineSegment2(point, next)) 331 | vertex.lav = lav 332 | if lav.head == None: 333 | lav.head = vertex 334 | vertex.prev = vertex.next = vertex 335 | else: 336 | vertex.next = lav.head 337 | vertex.prev = lav.head.prev 338 | vertex.prev.next = vertex 339 | lav.head.prev = vertex 340 | return lav 341 | 342 | @classmethod 343 | def from_chain(cls, head, slav): 344 | lav = cls(slav) 345 | lav.head = head 346 | for vertex in lav: 347 | lav._len += 1 348 | vertex.lav = lav 349 | return lav 350 | 351 | def invalidate(self, vertex): 352 | assert vertex.lav is self, "Tried to invalidate a vertex that's not mine" 353 | log.debug("Invalidating %s", vertex) 354 | vertex._valid = False 355 | if self.head == vertex: 356 | self.head = self.head.next 357 | vertex.lav = None 358 | 359 | def unify(self, vertex_a, vertex_b, point): 360 | replacement =_LAVertex(point, vertex_a.edge_left, vertex_b.edge_right, (vertex_b.bisector.v.normalized(), vertex_a.bisector.v.normalized())) 361 | replacement.lav = self 362 | 363 | if self.head in [vertex_a, vertex_b]: 364 | self.head = replacement 365 | 366 | vertex_a.prev.next = replacement 367 | vertex_b.next.prev = replacement 368 | replacement.prev = vertex_a.prev 369 | replacement.next = vertex_b.next 370 | 371 | vertex_a.invalidate() 372 | vertex_b.invalidate() 373 | 374 | self._len -= 1 375 | return replacement 376 | 377 | def __str__(self): 378 | return "LAV {}".format(id(self)) 379 | 380 | def __repr__(self): 381 | return "{} = {}".format(str(self), [vertex for vertex in self]) 382 | 383 | def __len__(self): 384 | return self._len 385 | 386 | def __iter__(self): 387 | cur = self.head 388 | while True: 389 | yield cur 390 | cur = cur.next 391 | if cur == self.head: 392 | raise StopIteration 393 | 394 | def _show(self): 395 | cur = self.head 396 | while True: 397 | print(cur.__repr__()) 398 | cur = cur.next 399 | if cur == self.head: 400 | break 401 | 402 | class _EventQueue: 403 | def __init__(self): 404 | self.__data = [] 405 | 406 | def put(self, item): 407 | if item is not None: 408 | heapq.heappush(self.__data, item) 409 | 410 | def put_all(self, iterable): 411 | for item in iterable: 412 | heapq.heappush(self.__data, item) 413 | 414 | def get(self): 415 | return heapq.heappop(self.__data) 416 | 417 | def empty(self): 418 | return len(self.__data)==0 419 | 420 | def peek(self): 421 | return self.__data[0] 422 | 423 | def show(self): 424 | for item in self.__data: 425 | print(item) 426 | 427 | def skeletonize(polygon, holes=None): 428 | """ 429 | Compute the straight skeleton of a polygon. 430 | 431 | The polygon should be given as a list of vertices in counter-clockwise order. 432 | Holes is a list of the contours of the holes, the vertices of which should be in clockwise order. 433 | 434 | Returns the straight skeleton as a list of "subtrees", which are in the form of (source, height, sinks), 435 | where source is the highest points, height is its height, and sinks are the point connected to the source. 436 | """ 437 | slav = _SLAV(polygon, holes) 438 | output = [] 439 | prioque = _EventQueue() 440 | 441 | for lav in slav: 442 | for vertex in lav: 443 | v = vertex.next_event() 444 | prioque.put(v) 445 | 446 | while not (prioque.empty() or slav.empty()): 447 | log.debug("SLAV is %s", [repr(lav) for lav in slav]) 448 | i = prioque.get() 449 | if isinstance(i, _EdgeEvent): 450 | if not i.vertex_a.is_valid or not i.vertex_b.is_valid: 451 | log.info("%.2f Discarded outdated edge event %s", i.distance, i) 452 | continue 453 | 454 | (arc, events) = slav.handle_edge_event(i) 455 | elif isinstance(i, _SplitEvent): 456 | if not i.vertex.is_valid: 457 | log.info("%.2f Discarded outdated split event %s", i.distance, i) 458 | continue 459 | (arc, events) = slav.handle_split_event(i) 460 | 461 | prioque.put_all(events) 462 | 463 | if arc is not None: 464 | output.append(arc) 465 | for sink in arc.sinks: 466 | _debug.line((arc.source.x, arc.source.y, sink.x, sink.y), fill="red") 467 | 468 | _debug.show() 469 | return output 470 | -------------------------------------------------------------------------------- /euclid.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # 3 | # euclid graphics maths module 4 | # 5 | # Copyright (c) 2006 Alex Holkner 6 | # Copyright (c) 2011 Eugen Zagorodniy 7 | # Copyright (c) 2011 Dov Grobgeld 8 | # Copyright (c) 2012 Lorenzo Riano 9 | # 10 | # This library is free software; you can redistribute it and/or modify it 11 | # under the terms of the GNU Lesser General Public License as published by the 12 | # Free Software Foundation; either version 2.1 of the License, or (at your 13 | # option) any later version. 14 | # 15 | # This library is distributed in the hope that it will be useful, but WITHOUT 16 | # ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 17 | # FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License 18 | # for more details. 19 | # 20 | # You should have received a copy of the GNU Lesser General Public License 21 | # along with this library; if not, write to the Free Software Foundation, 22 | # Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 23 | 24 | '''euclid graphics maths module 25 | 26 | Documentation and tests are included in the file "euclid.rst", or online 27 | at https://github.com/ezag/pyeuclid/blob/master/euclid.rst 28 | ''' 29 | 30 | __docformat__ = 'restructuredtext' 31 | __version__ = '$Id$' 32 | __revision__ = '$Revision$' 33 | 34 | import math 35 | import operator 36 | import types 37 | 38 | try: 39 | long 40 | except NameError: 41 | long = int 42 | 43 | # Some magic here. If _use_slots is True, the classes will derive from 44 | # object and will define a __slots__ class variable. If _use_slots is 45 | # False, classes will be old-style and will not define __slots__. 46 | # 47 | # _use_slots = True: Memory efficient, probably faster in future versions 48 | # of Python, "better". 49 | # _use_slots = False: Ordinary classes, much faster than slots in current 50 | # versions of Python (2.4 and 2.5). 51 | _use_slots = True 52 | 53 | # If True, allows components of Vector2 and Vector3 to be set via swizzling; 54 | # e.g. v.xyz = (1, 2, 3). This is much, much slower than the more verbose 55 | # v.x = 1; v.y = 2; v.z = 3, and slows down ordinary element setting as 56 | # well. Recommended setting is False. 57 | _enable_swizzle_set = False 58 | 59 | # Requires class to derive from object. 60 | if _enable_swizzle_set: 61 | _use_slots = True 62 | 63 | # Implement _use_slots magic. 64 | class _EuclidMetaclass(type): 65 | def __new__(cls, name, bases, dct): 66 | if '__slots__' in dct: 67 | dct['__getstate__'] = cls._create_getstate(dct['__slots__']) 68 | dct['__setstate__'] = cls._create_setstate(dct['__slots__']) 69 | if _use_slots: 70 | return type.__new__(cls, name, bases + (object,), dct) 71 | else: 72 | if '__slots__' in dct: 73 | del dct['__slots__'] 74 | return types.ClassType.__new__(types.ClassType, name, bases, dct) 75 | 76 | @classmethod 77 | def _create_getstate(cls, slots): 78 | def __getstate__(self): 79 | d = {} 80 | for slot in slots: 81 | d[slot] = getattr(self, slot) 82 | return d 83 | return __getstate__ 84 | 85 | @classmethod 86 | def _create_setstate(cls, slots): 87 | def __setstate__(self, state): 88 | for name, value in state.items(): 89 | setattr(self, name, value) 90 | return __setstate__ 91 | 92 | __metaclass__ = _EuclidMetaclass 93 | 94 | class Vector2: 95 | __slots__ = ['x', 'y'] 96 | __hash__ = None 97 | 98 | def __init__(self, x=0, y=0): 99 | self.x = x 100 | self.y = y 101 | 102 | def __copy__(self): 103 | return self.__class__(self.x, self.y) 104 | 105 | copy = __copy__ 106 | 107 | def __repr__(self): 108 | return 'Vector2(%.2f, %.2f)' % (self.x, self.y) 109 | 110 | def __eq__(self, other): 111 | if isinstance(other, Vector2): 112 | return self.x == other.x and \ 113 | self.y == other.y 114 | else: 115 | assert hasattr(other, '__len__') and len(other) == 2 116 | return self.x == other[0] and \ 117 | self.y == other[1] 118 | 119 | def __ne__(self, other): 120 | return not self.__eq__(other) 121 | 122 | def __nonzero__(self): 123 | return bool(self.x != 0 or self.y != 0) 124 | 125 | def __len__(self): 126 | return 2 127 | 128 | def __getitem__(self, key): 129 | return (self.x, self.y)[key] 130 | 131 | def __setitem__(self, key, value): 132 | l = [self.x, self.y] 133 | l[key] = value 134 | self.x, self.y = l 135 | 136 | def __iter__(self): 137 | return iter((self.x, self.y)) 138 | 139 | def __getattr__(self, name): 140 | try: 141 | return tuple([(self.x, self.y)['xy'.index(c)] \ 142 | for c in name]) 143 | except ValueError: 144 | raise AttributeError(name) 145 | 146 | if _enable_swizzle_set: 147 | # This has detrimental performance on ordinary setattr as well 148 | # if enabled 149 | def __setattr__(self, name, value): 150 | if len(name) == 1: 151 | object.__setattr__(self, name, value) 152 | else: 153 | try: 154 | l = [self.x, self.y] 155 | for c, v in map(None, name, value): 156 | l['xy'.index(c)] = v 157 | self.x, self.y = l 158 | except ValueError: 159 | raise AttributeError(name) 160 | 161 | def __add__(self, other): 162 | if isinstance(other, Vector2): 163 | # Vector + Vector -> Vector 164 | # Vector + Point -> Point 165 | # Point + Point -> Vector 166 | if self.__class__ is other.__class__: 167 | _class = Vector2 168 | else: 169 | _class = Point2 170 | return _class(self.x + other.x, 171 | self.y + other.y) 172 | else: 173 | assert hasattr(other, '__len__') and len(other) == 2 174 | return Vector2(self.x + other[0], 175 | self.y + other[1]) 176 | __radd__ = __add__ 177 | 178 | def __iadd__(self, other): 179 | if isinstance(other, Vector2): 180 | self.x += other.x 181 | self.y += other.y 182 | else: 183 | self.x += other[0] 184 | self.y += other[1] 185 | return self 186 | 187 | def __sub__(self, other): 188 | if isinstance(other, Vector2): 189 | # Vector - Vector -> Vector 190 | # Vector - Point -> Point 191 | # Point - Point -> Vector 192 | if self.__class__ is other.__class__: 193 | _class = Vector2 194 | else: 195 | _class = Point2 196 | return _class(self.x - other.x, 197 | self.y - other.y) 198 | else: 199 | assert hasattr(other, '__len__') and len(other) == 2 200 | return Vector2(self.x - other[0], 201 | self.y - other[1]) 202 | 203 | 204 | def __rsub__(self, other): 205 | if isinstance(other, Vector2): 206 | return Vector2(other.x - self.x, 207 | other.y - self.y) 208 | else: 209 | assert hasattr(other, '__len__') and len(other) == 2 210 | return Vector2(other.x - self[0], 211 | other.y - self[1]) 212 | 213 | def __mul__(self, other): 214 | assert type(other) in (int, long, float) 215 | return Vector2(self.x * other, 216 | self.y * other) 217 | 218 | __rmul__ = __mul__ 219 | 220 | def __imul__(self, other): 221 | assert type(other) in (int, long, float) 222 | self.x *= other 223 | self.y *= other 224 | return self 225 | 226 | def __div__(self, other): 227 | assert type(other) in (int, long, float) 228 | return Vector2(operator.div(self.x, other), 229 | operator.div(self.y, other)) 230 | 231 | 232 | def __rdiv__(self, other): 233 | assert type(other) in (int, long, float) 234 | return Vector2(operator.div(other, self.x), 235 | operator.div(other, self.y)) 236 | 237 | def __floordiv__(self, other): 238 | assert type(other) in (int, long, float) 239 | return Vector2(operator.floordiv(self.x, other), 240 | operator.floordiv(self.y, other)) 241 | 242 | 243 | def __rfloordiv__(self, other): 244 | assert type(other) in (int, long, float) 245 | return Vector2(operator.floordiv(other, self.x), 246 | operator.floordiv(other, self.y)) 247 | 248 | def __truediv__(self, other): 249 | assert type(other) in (int, long, float) 250 | return Vector2(operator.truediv(self.x, other), 251 | operator.truediv(self.y, other)) 252 | 253 | 254 | def __rtruediv__(self, other): 255 | assert type(other) in (int, long, float) 256 | return Vector2(operator.truediv(other, self.x), 257 | operator.truediv(other, self.y)) 258 | 259 | def __neg__(self): 260 | return Vector2(-self.x, 261 | -self.y) 262 | 263 | __pos__ = __copy__ 264 | 265 | def __abs__(self): 266 | return math.sqrt(self.x ** 2 + \ 267 | self.y ** 2) 268 | 269 | magnitude = __abs__ 270 | 271 | def magnitude_squared(self): 272 | return self.x ** 2 + \ 273 | self.y ** 2 274 | 275 | def normalize(self): 276 | d = self.magnitude() 277 | if d: 278 | self.x /= d 279 | self.y /= d 280 | return self 281 | 282 | def normalized(self): 283 | d = self.magnitude() 284 | if d: 285 | return Vector2(self.x / d, 286 | self.y / d) 287 | return self.copy() 288 | 289 | def dot(self, other): 290 | assert isinstance(other, Vector2) 291 | return self.x * other.x + \ 292 | self.y * other.y 293 | 294 | def cross(self): 295 | return Vector2(self.y, -self.x) 296 | 297 | def reflect(self, normal): 298 | # assume normal is normalized 299 | assert isinstance(normal, Vector2) 300 | d = 2 * (self.x * normal.x + self.y * normal.y) 301 | return Vector2(self.x - d * normal.x, 302 | self.y - d * normal.y) 303 | 304 | def angle(self, other): 305 | """Return the angle to the vector other""" 306 | return math.acos(self.dot(other) / (self.magnitude()*other.magnitude())) 307 | 308 | def project(self, other): 309 | """Return one vector projected on the vector other""" 310 | n = other.normalized() 311 | return self.dot(n)*n 312 | 313 | class Vector3: 314 | __slots__ = ['x', 'y', 'z'] 315 | __hash__ = None 316 | 317 | def __init__(self, x=0, y=0, z=0): 318 | self.x = x 319 | self.y = y 320 | self.z = z 321 | 322 | def __copy__(self): 323 | return self.__class__(self.x, self.y, self.z) 324 | 325 | copy = __copy__ 326 | 327 | def __repr__(self): 328 | return 'Vector3(%.2f, %.2f, %.2f)' % (self.x, 329 | self.y, 330 | self.z) 331 | 332 | def __eq__(self, other): 333 | if isinstance(other, Vector3): 334 | return self.x == other.x and \ 335 | self.y == other.y and \ 336 | self.z == other.z 337 | else: 338 | assert hasattr(other, '__len__') and len(other) == 3 339 | return self.x == other[0] and \ 340 | self.y == other[1] and \ 341 | self.z == other[2] 342 | 343 | def __ne__(self, other): 344 | return not self.__eq__(other) 345 | 346 | def __nonzero__(self): 347 | return bool(self.x != 0 or self.y != 0 or self.z != 0) 348 | 349 | def __len__(self): 350 | return 3 351 | 352 | def __getitem__(self, key): 353 | return (self.x, self.y, self.z)[key] 354 | 355 | def __setitem__(self, key, value): 356 | l = [self.x, self.y, self.z] 357 | l[key] = value 358 | self.x, self.y, self.z = l 359 | 360 | def __iter__(self): 361 | return iter((self.x, self.y, self.z)) 362 | 363 | def __getattr__(self, name): 364 | try: 365 | return tuple([(self.x, self.y, self.z)['xyz'.index(c)] \ 366 | for c in name]) 367 | except ValueError: 368 | raise AttributeError(name) 369 | 370 | if _enable_swizzle_set: 371 | # This has detrimental performance on ordinary setattr as well 372 | # if enabled 373 | def __setattr__(self, name, value): 374 | if len(name) == 1: 375 | object.__setattr__(self, name, value) 376 | else: 377 | try: 378 | l = [self.x, self.y, self.z] 379 | for c, v in map(None, name, value): 380 | l['xyz'.index(c)] = v 381 | self.x, self.y, self.z = l 382 | except ValueError: 383 | raise AttributeError(name) 384 | 385 | 386 | def __add__(self, other): 387 | if isinstance(other, Vector3): 388 | # Vector + Vector -> Vector 389 | # Vector + Point -> Point 390 | # Point + Point -> Vector 391 | if self.__class__ is other.__class__: 392 | _class = Vector3 393 | else: 394 | _class = Point3 395 | return _class(self.x + other.x, 396 | self.y + other.y, 397 | self.z + other.z) 398 | else: 399 | assert hasattr(other, '__len__') and len(other) == 3 400 | return Vector3(self.x + other[0], 401 | self.y + other[1], 402 | self.z + other[2]) 403 | __radd__ = __add__ 404 | 405 | def __iadd__(self, other): 406 | if isinstance(other, Vector3): 407 | self.x += other.x 408 | self.y += other.y 409 | self.z += other.z 410 | else: 411 | self.x += other[0] 412 | self.y += other[1] 413 | self.z += other[2] 414 | return self 415 | 416 | def __sub__(self, other): 417 | if isinstance(other, Vector3): 418 | # Vector - Vector -> Vector 419 | # Vector - Point -> Point 420 | # Point - Point -> Vector 421 | if self.__class__ is other.__class__: 422 | _class = Vector3 423 | else: 424 | _class = Point3 425 | return Vector3(self.x - other.x, 426 | self.y - other.y, 427 | self.z - other.z) 428 | else: 429 | assert hasattr(other, '__len__') and len(other) == 3 430 | return Vector3(self.x - other[0], 431 | self.y - other[1], 432 | self.z - other[2]) 433 | 434 | 435 | def __rsub__(self, other): 436 | if isinstance(other, Vector3): 437 | return Vector3(other.x - self.x, 438 | other.y - self.y, 439 | other.z - self.z) 440 | else: 441 | assert hasattr(other, '__len__') and len(other) == 3 442 | return Vector3(other.x - self[0], 443 | other.y - self[1], 444 | other.z - self[2]) 445 | 446 | def __mul__(self, other): 447 | if isinstance(other, Vector3): 448 | # TODO component-wise mul/div in-place and on Vector2; docs. 449 | if self.__class__ is Point3 or other.__class__ is Point3: 450 | _class = Point3 451 | else: 452 | _class = Vector3 453 | return _class(self.x * other.x, 454 | self.y * other.y, 455 | self.z * other.z) 456 | else: 457 | assert type(other) in (int, long, float) 458 | return Vector3(self.x * other, 459 | self.y * other, 460 | self.z * other) 461 | 462 | __rmul__ = __mul__ 463 | 464 | def __imul__(self, other): 465 | assert type(other) in (int, long, float) 466 | self.x *= other 467 | self.y *= other 468 | self.z *= other 469 | return self 470 | 471 | def __div__(self, other): 472 | assert type(other) in (int, long, float) 473 | return Vector3(operator.div(self.x, other), 474 | operator.div(self.y, other), 475 | operator.div(self.z, other)) 476 | 477 | 478 | def __rdiv__(self, other): 479 | assert type(other) in (int, long, float) 480 | return Vector3(operator.div(other, self.x), 481 | operator.div(other, self.y), 482 | operator.div(other, self.z)) 483 | 484 | def __floordiv__(self, other): 485 | assert type(other) in (int, long, float) 486 | return Vector3(operator.floordiv(self.x, other), 487 | operator.floordiv(self.y, other), 488 | operator.floordiv(self.z, other)) 489 | 490 | 491 | def __rfloordiv__(self, other): 492 | assert type(other) in (int, long, float) 493 | return Vector3(operator.floordiv(other, self.x), 494 | operator.floordiv(other, self.y), 495 | operator.floordiv(other, self.z)) 496 | 497 | def __truediv__(self, other): 498 | assert type(other) in (int, long, float) 499 | return Vector3(operator.truediv(self.x, other), 500 | operator.truediv(self.y, other), 501 | operator.truediv(self.z, other)) 502 | 503 | 504 | def __rtruediv__(self, other): 505 | assert type(other) in (int, long, float) 506 | return Vector3(operator.truediv(other, self.x), 507 | operator.truediv(other, self.y), 508 | operator.truediv(other, self.z)) 509 | 510 | def __neg__(self): 511 | return Vector3(-self.x, 512 | -self.y, 513 | -self.z) 514 | 515 | __pos__ = __copy__ 516 | 517 | def __abs__(self): 518 | return math.sqrt(self.x ** 2 + \ 519 | self.y ** 2 + \ 520 | self.z ** 2) 521 | 522 | magnitude = __abs__ 523 | 524 | def magnitude_squared(self): 525 | return self.x ** 2 + \ 526 | self.y ** 2 + \ 527 | self.z ** 2 528 | 529 | def normalize(self): 530 | d = self.magnitude() 531 | if d: 532 | self.x /= d 533 | self.y /= d 534 | self.z /= d 535 | return self 536 | 537 | def normalized(self): 538 | d = self.magnitude() 539 | if d: 540 | return Vector3(self.x / d, 541 | self.y / d, 542 | self.z / d) 543 | return self.copy() 544 | 545 | def dot(self, other): 546 | assert isinstance(other, Vector3) 547 | return self.x * other.x + \ 548 | self.y * other.y + \ 549 | self.z * other.z 550 | 551 | def cross(self, other): 552 | assert isinstance(other, Vector3) 553 | return Vector3(self.y * other.z - self.z * other.y, 554 | -self.x * other.z + self.z * other.x, 555 | self.x * other.y - self.y * other.x) 556 | 557 | def reflect(self, normal): 558 | # assume normal is normalized 559 | assert isinstance(normal, Vector3) 560 | d = 2 * (self.x * normal.x + self.y * normal.y + self.z * normal.z) 561 | return Vector3(self.x - d * normal.x, 562 | self.y - d * normal.y, 563 | self.z - d * normal.z) 564 | 565 | def rotate_around(self, axis, theta): 566 | """Return the vector rotated around axis through angle theta. Right hand rule applies""" 567 | 568 | # Adapted from equations published by Glenn Murray. 569 | # http://inside.mines.edu/~gmurray/ArbitraryAxisRotation/ArbitraryAxisRotation.html 570 | x, y, z = self.x, self.y,self.z 571 | u, v, w = axis.x, axis.y, axis.z 572 | 573 | # Extracted common factors for simplicity and efficiency 574 | r2 = u**2 + v**2 + w**2 575 | r = math.sqrt(r2) 576 | ct = math.cos(theta) 577 | st = math.sin(theta) / r 578 | dt = (u*x + v*y + w*z) * (1 - ct) / r2 579 | return Vector3((u * dt + x * ct + (-w * y + v * z) * st), 580 | (v * dt + y * ct + ( w * x - u * z) * st), 581 | (w * dt + z * ct + (-v * x + u * y) * st)) 582 | 583 | def angle(self, other): 584 | """Return the angle to the vector other""" 585 | return math.acos(self.dot(other) / (self.magnitude()*other.magnitude())) 586 | 587 | def project(self, other): 588 | """Return one vector projected on the vector other""" 589 | n = other.normalized() 590 | return self.dot(n)*n 591 | 592 | # a b c 593 | # e f g 594 | # i j k 595 | 596 | class Matrix3: 597 | __slots__ = list('abcefgijk') 598 | 599 | def __init__(self): 600 | self.identity() 601 | 602 | def __copy__(self): 603 | M = Matrix3() 604 | M.a = self.a 605 | M.b = self.b 606 | M.c = self.c 607 | M.e = self.e 608 | M.f = self.f 609 | M.g = self.g 610 | M.i = self.i 611 | M.j = self.j 612 | M.k = self.k 613 | return M 614 | 615 | copy = __copy__ 616 | def __repr__(self): 617 | return ('Matrix3([% 8.2f % 8.2f % 8.2f\n' \ 618 | ' % 8.2f % 8.2f % 8.2f\n' \ 619 | ' % 8.2f % 8.2f % 8.2f])') \ 620 | % (self.a, self.b, self.c, 621 | self.e, self.f, self.g, 622 | self.i, self.j, self.k) 623 | 624 | def __getitem__(self, key): 625 | return [self.a, self.e, self.i, 626 | self.b, self.f, self.j, 627 | self.c, self.g, self.k][key] 628 | 629 | def __setitem__(self, key, value): 630 | L = self[:] 631 | L[key] = value 632 | (self.a, self.e, self.i, 633 | self.b, self.f, self.j, 634 | self.c, self.g, self.k) = L 635 | 636 | def __mul__(self, other): 637 | if isinstance(other, Matrix3): 638 | # Caching repeatedly accessed attributes in local variables 639 | # apparently increases performance by 20%. Attrib: Will McGugan. 640 | Aa = self.a 641 | Ab = self.b 642 | Ac = self.c 643 | Ae = self.e 644 | Af = self.f 645 | Ag = self.g 646 | Ai = self.i 647 | Aj = self.j 648 | Ak = self.k 649 | Ba = other.a 650 | Bb = other.b 651 | Bc = other.c 652 | Be = other.e 653 | Bf = other.f 654 | Bg = other.g 655 | Bi = other.i 656 | Bj = other.j 657 | Bk = other.k 658 | C = Matrix3() 659 | C.a = Aa * Ba + Ab * Be + Ac * Bi 660 | C.b = Aa * Bb + Ab * Bf + Ac * Bj 661 | C.c = Aa * Bc + Ab * Bg + Ac * Bk 662 | C.e = Ae * Ba + Af * Be + Ag * Bi 663 | C.f = Ae * Bb + Af * Bf + Ag * Bj 664 | C.g = Ae * Bc + Af * Bg + Ag * Bk 665 | C.i = Ai * Ba + Aj * Be + Ak * Bi 666 | C.j = Ai * Bb + Aj * Bf + Ak * Bj 667 | C.k = Ai * Bc + Aj * Bg + Ak * Bk 668 | return C 669 | elif isinstance(other, Point2): 670 | A = self 671 | B = other 672 | P = Point2(0, 0) 673 | P.x = A.a * B.x + A.b * B.y + A.c 674 | P.y = A.e * B.x + A.f * B.y + A.g 675 | return P 676 | elif isinstance(other, Vector2): 677 | A = self 678 | B = other 679 | V = Vector2(0, 0) 680 | V.x = A.a * B.x + A.b * B.y 681 | V.y = A.e * B.x + A.f * B.y 682 | return V 683 | else: 684 | other = other.copy() 685 | other._apply_transform(self) 686 | return other 687 | 688 | def __imul__(self, other): 689 | assert isinstance(other, Matrix3) 690 | # Cache attributes in local vars (see Matrix3.__mul__). 691 | Aa = self.a 692 | Ab = self.b 693 | Ac = self.c 694 | Ae = self.e 695 | Af = self.f 696 | Ag = self.g 697 | Ai = self.i 698 | Aj = self.j 699 | Ak = self.k 700 | Ba = other.a 701 | Bb = other.b 702 | Bc = other.c 703 | Be = other.e 704 | Bf = other.f 705 | Bg = other.g 706 | Bi = other.i 707 | Bj = other.j 708 | Bk = other.k 709 | self.a = Aa * Ba + Ab * Be + Ac * Bi 710 | self.b = Aa * Bb + Ab * Bf + Ac * Bj 711 | self.c = Aa * Bc + Ab * Bg + Ac * Bk 712 | self.e = Ae * Ba + Af * Be + Ag * Bi 713 | self.f = Ae * Bb + Af * Bf + Ag * Bj 714 | self.g = Ae * Bc + Af * Bg + Ag * Bk 715 | self.i = Ai * Ba + Aj * Be + Ak * Bi 716 | self.j = Ai * Bb + Aj * Bf + Ak * Bj 717 | self.k = Ai * Bc + Aj * Bg + Ak * Bk 718 | return self 719 | 720 | def identity(self): 721 | self.a = self.f = self.k = 1. 722 | self.b = self.c = self.e = self.g = self.i = self.j = 0 723 | return self 724 | 725 | def scale(self, x, y): 726 | self *= Matrix3.new_scale(x, y) 727 | return self 728 | 729 | def translate(self, x, y): 730 | self *= Matrix3.new_translate(x, y) 731 | return self 732 | 733 | def rotate(self, angle): 734 | self *= Matrix3.new_rotate(angle) 735 | return self 736 | 737 | # Static constructors 738 | def new_identity(cls): 739 | self = cls() 740 | return self 741 | new_identity = classmethod(new_identity) 742 | 743 | def new_scale(cls, x, y): 744 | self = cls() 745 | self.a = x 746 | self.f = y 747 | return self 748 | new_scale = classmethod(new_scale) 749 | 750 | def new_translate(cls, x, y): 751 | self = cls() 752 | self.c = x 753 | self.g = y 754 | return self 755 | new_translate = classmethod(new_translate) 756 | 757 | def new_rotate(cls, angle): 758 | self = cls() 759 | s = math.sin(angle) 760 | c = math.cos(angle) 761 | self.a = self.f = c 762 | self.b = -s 763 | self.e = s 764 | return self 765 | new_rotate = classmethod(new_rotate) 766 | 767 | def determinant(self): 768 | return (self.a*self.f*self.k 769 | + self.b*self.g*self.i 770 | + self.c*self.e*self.j 771 | - self.a*self.g*self.j 772 | - self.b*self.e*self.k 773 | - self.c*self.f*self.i) 774 | 775 | def inverse(self): 776 | tmp = Matrix3() 777 | d = self.determinant() 778 | 779 | if abs(d) < 0.001: 780 | # No inverse, return identity 781 | return tmp 782 | else: 783 | d = 1.0 / d 784 | 785 | tmp.a = d * (self.f*self.k - self.g*self.j) 786 | tmp.b = d * (self.c*self.j - self.b*self.k) 787 | tmp.c = d * (self.b*self.g - self.c*self.f) 788 | tmp.e = d * (self.g*self.i - self.e*self.k) 789 | tmp.f = d * (self.a*self.k - self.c*self.i) 790 | tmp.g = d * (self.c*self.e - self.a*self.g) 791 | tmp.i = d * (self.e*self.j - self.f*self.i) 792 | tmp.j = d * (self.b*self.i - self.a*self.j) 793 | tmp.k = d * (self.a*self.f - self.b*self.e) 794 | 795 | return tmp 796 | 797 | # a b c d 798 | # e f g h 799 | # i j k l 800 | # m n o p 801 | 802 | class Matrix4: 803 | __slots__ = list('abcdefghijklmnop') 804 | 805 | def __init__(self): 806 | self.identity() 807 | 808 | def __copy__(self): 809 | M = Matrix4() 810 | M.a = self.a 811 | M.b = self.b 812 | M.c = self.c 813 | M.d = self.d 814 | M.e = self.e 815 | M.f = self.f 816 | M.g = self.g 817 | M.h = self.h 818 | M.i = self.i 819 | M.j = self.j 820 | M.k = self.k 821 | M.l = self.l 822 | M.m = self.m 823 | M.n = self.n 824 | M.o = self.o 825 | M.p = self.p 826 | return M 827 | 828 | copy = __copy__ 829 | 830 | 831 | def __repr__(self): 832 | return ('Matrix4([% 8.2f % 8.2f % 8.2f % 8.2f\n' \ 833 | ' % 8.2f % 8.2f % 8.2f % 8.2f\n' \ 834 | ' % 8.2f % 8.2f % 8.2f % 8.2f\n' \ 835 | ' % 8.2f % 8.2f % 8.2f % 8.2f])') \ 836 | % (self.a, self.b, self.c, self.d, 837 | self.e, self.f, self.g, self.h, 838 | self.i, self.j, self.k, self.l, 839 | self.m, self.n, self.o, self.p) 840 | 841 | def __getitem__(self, key): 842 | return [self.a, self.e, self.i, self.m, 843 | self.b, self.f, self.j, self.n, 844 | self.c, self.g, self.k, self.o, 845 | self.d, self.h, self.l, self.p][key] 846 | 847 | def __setitem__(self, key, value): 848 | L = self[:] 849 | L[key] = value 850 | (self.a, self.e, self.i, self.m, 851 | self.b, self.f, self.j, self.n, 852 | self.c, self.g, self.k, self.o, 853 | self.d, self.h, self.l, self.p) = L 854 | 855 | def __mul__(self, other): 856 | if isinstance(other, Matrix4): 857 | # Cache attributes in local vars (see Matrix3.__mul__). 858 | Aa = self.a 859 | Ab = self.b 860 | Ac = self.c 861 | Ad = self.d 862 | Ae = self.e 863 | Af = self.f 864 | Ag = self.g 865 | Ah = self.h 866 | Ai = self.i 867 | Aj = self.j 868 | Ak = self.k 869 | Al = self.l 870 | Am = self.m 871 | An = self.n 872 | Ao = self.o 873 | Ap = self.p 874 | Ba = other.a 875 | Bb = other.b 876 | Bc = other.c 877 | Bd = other.d 878 | Be = other.e 879 | Bf = other.f 880 | Bg = other.g 881 | Bh = other.h 882 | Bi = other.i 883 | Bj = other.j 884 | Bk = other.k 885 | Bl = other.l 886 | Bm = other.m 887 | Bn = other.n 888 | Bo = other.o 889 | Bp = other.p 890 | C = Matrix4() 891 | C.a = Aa * Ba + Ab * Be + Ac * Bi + Ad * Bm 892 | C.b = Aa * Bb + Ab * Bf + Ac * Bj + Ad * Bn 893 | C.c = Aa * Bc + Ab * Bg + Ac * Bk + Ad * Bo 894 | C.d = Aa * Bd + Ab * Bh + Ac * Bl + Ad * Bp 895 | C.e = Ae * Ba + Af * Be + Ag * Bi + Ah * Bm 896 | C.f = Ae * Bb + Af * Bf + Ag * Bj + Ah * Bn 897 | C.g = Ae * Bc + Af * Bg + Ag * Bk + Ah * Bo 898 | C.h = Ae * Bd + Af * Bh + Ag * Bl + Ah * Bp 899 | C.i = Ai * Ba + Aj * Be + Ak * Bi + Al * Bm 900 | C.j = Ai * Bb + Aj * Bf + Ak * Bj + Al * Bn 901 | C.k = Ai * Bc + Aj * Bg + Ak * Bk + Al * Bo 902 | C.l = Ai * Bd + Aj * Bh + Ak * Bl + Al * Bp 903 | C.m = Am * Ba + An * Be + Ao * Bi + Ap * Bm 904 | C.n = Am * Bb + An * Bf + Ao * Bj + Ap * Bn 905 | C.o = Am * Bc + An * Bg + Ao * Bk + Ap * Bo 906 | C.p = Am * Bd + An * Bh + Ao * Bl + Ap * Bp 907 | return C 908 | elif isinstance(other, Point3): 909 | A = self 910 | B = other 911 | P = Point3(0, 0, 0) 912 | P.x = A.a * B.x + A.b * B.y + A.c * B.z + A.d 913 | P.y = A.e * B.x + A.f * B.y + A.g * B.z + A.h 914 | P.z = A.i * B.x + A.j * B.y + A.k * B.z + A.l 915 | return P 916 | elif isinstance(other, Vector3): 917 | A = self 918 | B = other 919 | V = Vector3(0, 0, 0) 920 | V.x = A.a * B.x + A.b * B.y + A.c * B.z 921 | V.y = A.e * B.x + A.f * B.y + A.g * B.z 922 | V.z = A.i * B.x + A.j * B.y + A.k * B.z 923 | return V 924 | else: 925 | other = other.copy() 926 | other._apply_transform(self) 927 | return other 928 | 929 | def __imul__(self, other): 930 | assert isinstance(other, Matrix4) 931 | # Cache attributes in local vars (see Matrix3.__mul__). 932 | Aa = self.a 933 | Ab = self.b 934 | Ac = self.c 935 | Ad = self.d 936 | Ae = self.e 937 | Af = self.f 938 | Ag = self.g 939 | Ah = self.h 940 | Ai = self.i 941 | Aj = self.j 942 | Ak = self.k 943 | Al = self.l 944 | Am = self.m 945 | An = self.n 946 | Ao = self.o 947 | Ap = self.p 948 | Ba = other.a 949 | Bb = other.b 950 | Bc = other.c 951 | Bd = other.d 952 | Be = other.e 953 | Bf = other.f 954 | Bg = other.g 955 | Bh = other.h 956 | Bi = other.i 957 | Bj = other.j 958 | Bk = other.k 959 | Bl = other.l 960 | Bm = other.m 961 | Bn = other.n 962 | Bo = other.o 963 | Bp = other.p 964 | self.a = Aa * Ba + Ab * Be + Ac * Bi + Ad * Bm 965 | self.b = Aa * Bb + Ab * Bf + Ac * Bj + Ad * Bn 966 | self.c = Aa * Bc + Ab * Bg + Ac * Bk + Ad * Bo 967 | self.d = Aa * Bd + Ab * Bh + Ac * Bl + Ad * Bp 968 | self.e = Ae * Ba + Af * Be + Ag * Bi + Ah * Bm 969 | self.f = Ae * Bb + Af * Bf + Ag * Bj + Ah * Bn 970 | self.g = Ae * Bc + Af * Bg + Ag * Bk + Ah * Bo 971 | self.h = Ae * Bd + Af * Bh + Ag * Bl + Ah * Bp 972 | self.i = Ai * Ba + Aj * Be + Ak * Bi + Al * Bm 973 | self.j = Ai * Bb + Aj * Bf + Ak * Bj + Al * Bn 974 | self.k = Ai * Bc + Aj * Bg + Ak * Bk + Al * Bo 975 | self.l = Ai * Bd + Aj * Bh + Ak * Bl + Al * Bp 976 | self.m = Am * Ba + An * Be + Ao * Bi + Ap * Bm 977 | self.n = Am * Bb + An * Bf + Ao * Bj + Ap * Bn 978 | self.o = Am * Bc + An * Bg + Ao * Bk + Ap * Bo 979 | self.p = Am * Bd + An * Bh + Ao * Bl + Ap * Bp 980 | return self 981 | 982 | def transform(self, other): 983 | A = self 984 | B = other 985 | P = Point3(0, 0, 0) 986 | P.x = A.a * B.x + A.b * B.y + A.c * B.z + A.d 987 | P.y = A.e * B.x + A.f * B.y + A.g * B.z + A.h 988 | P.z = A.i * B.x + A.j * B.y + A.k * B.z + A.l 989 | w = A.m * B.x + A.n * B.y + A.o * B.z + A.p 990 | if w != 0: 991 | P.x /= w 992 | P.y /= w 993 | P.z /= w 994 | return P 995 | 996 | def identity(self): 997 | self.a = self.f = self.k = self.p = 1. 998 | self.b = self.c = self.d = self.e = self.g = self.h = \ 999 | self.i = self.j = self.l = self.m = self.n = self.o = 0 1000 | return self 1001 | 1002 | def scale(self, x, y, z): 1003 | self *= Matrix4.new_scale(x, y, z) 1004 | return self 1005 | 1006 | def translate(self, x, y, z): 1007 | self *= Matrix4.new_translate(x, y, z) 1008 | return self 1009 | 1010 | def rotatex(self, angle): 1011 | self *= Matrix4.new_rotatex(angle) 1012 | return self 1013 | 1014 | def rotatey(self, angle): 1015 | self *= Matrix4.new_rotatey(angle) 1016 | return self 1017 | 1018 | def rotatez(self, angle): 1019 | self *= Matrix4.new_rotatez(angle) 1020 | return self 1021 | 1022 | def rotate_axis(self, angle, axis): 1023 | self *= Matrix4.new_rotate_axis(angle, axis) 1024 | return self 1025 | 1026 | def rotate_euler(self, heading, attitude, bank): 1027 | self *= Matrix4.new_rotate_euler(heading, attitude, bank) 1028 | return self 1029 | 1030 | def rotate_triple_axis(self, x, y, z): 1031 | self *= Matrix4.new_rotate_triple_axis(x, y, z) 1032 | return self 1033 | 1034 | def transpose(self): 1035 | (self.a, self.e, self.i, self.m, 1036 | self.b, self.f, self.j, self.n, 1037 | self.c, self.g, self.k, self.o, 1038 | self.d, self.h, self.l, self.p) = \ 1039 | (self.a, self.b, self.c, self.d, 1040 | self.e, self.f, self.g, self.h, 1041 | self.i, self.j, self.k, self.l, 1042 | self.m, self.n, self.o, self.p) 1043 | 1044 | def transposed(self): 1045 | M = self.copy() 1046 | M.transpose() 1047 | return M 1048 | 1049 | # Static constructors 1050 | def new(cls, *values): 1051 | M = cls() 1052 | M[:] = values 1053 | return M 1054 | new = classmethod(new) 1055 | 1056 | def new_identity(cls): 1057 | self = cls() 1058 | return self 1059 | new_identity = classmethod(new_identity) 1060 | 1061 | def new_scale(cls, x, y, z): 1062 | self = cls() 1063 | self.a = x 1064 | self.f = y 1065 | self.k = z 1066 | return self 1067 | new_scale = classmethod(new_scale) 1068 | 1069 | def new_translate(cls, x, y, z): 1070 | self = cls() 1071 | self.d = x 1072 | self.h = y 1073 | self.l = z 1074 | return self 1075 | new_translate = classmethod(new_translate) 1076 | 1077 | def new_rotatex(cls, angle): 1078 | self = cls() 1079 | s = math.sin(angle) 1080 | c = math.cos(angle) 1081 | self.f = self.k = c 1082 | self.g = -s 1083 | self.j = s 1084 | return self 1085 | new_rotatex = classmethod(new_rotatex) 1086 | 1087 | def new_rotatey(cls, angle): 1088 | self = cls() 1089 | s = math.sin(angle) 1090 | c = math.cos(angle) 1091 | self.a = self.k = c 1092 | self.c = s 1093 | self.i = -s 1094 | return self 1095 | new_rotatey = classmethod(new_rotatey) 1096 | 1097 | def new_rotatez(cls, angle): 1098 | self = cls() 1099 | s = math.sin(angle) 1100 | c = math.cos(angle) 1101 | self.a = self.f = c 1102 | self.b = -s 1103 | self.e = s 1104 | return self 1105 | new_rotatez = classmethod(new_rotatez) 1106 | 1107 | def new_rotate_axis(cls, angle, axis): 1108 | assert(isinstance(axis, Vector3)) 1109 | vector = axis.normalized() 1110 | x = vector.x 1111 | y = vector.y 1112 | z = vector.z 1113 | 1114 | self = cls() 1115 | s = math.sin(angle) 1116 | c = math.cos(angle) 1117 | c1 = 1. - c 1118 | 1119 | # from the glRotate man page 1120 | self.a = x * x * c1 + c 1121 | self.b = x * y * c1 - z * s 1122 | self.c = x * z * c1 + y * s 1123 | self.e = y * x * c1 + z * s 1124 | self.f = y * y * c1 + c 1125 | self.g = y * z * c1 - x * s 1126 | self.i = x * z * c1 - y * s 1127 | self.j = y * z * c1 + x * s 1128 | self.k = z * z * c1 + c 1129 | return self 1130 | new_rotate_axis = classmethod(new_rotate_axis) 1131 | 1132 | def new_rotate_euler(cls, heading, attitude, bank): 1133 | # from http://www.euclideanspace.com/ 1134 | ch = math.cos(heading) 1135 | sh = math.sin(heading) 1136 | ca = math.cos(attitude) 1137 | sa = math.sin(attitude) 1138 | cb = math.cos(bank) 1139 | sb = math.sin(bank) 1140 | 1141 | self = cls() 1142 | self.a = ch * ca 1143 | self.b = sh * sb - ch * sa * cb 1144 | self.c = ch * sa * sb + sh * cb 1145 | self.e = sa 1146 | self.f = ca * cb 1147 | self.g = -ca * sb 1148 | self.i = -sh * ca 1149 | self.j = sh * sa * cb + ch * sb 1150 | self.k = -sh * sa * sb + ch * cb 1151 | return self 1152 | new_rotate_euler = classmethod(new_rotate_euler) 1153 | 1154 | def new_rotate_triple_axis(cls, x, y, z): 1155 | m = cls() 1156 | 1157 | m.a, m.b, m.c = x.x, y.x, z.x 1158 | m.e, m.f, m.g = x.y, y.y, z.y 1159 | m.i, m.j, m.k = x.z, y.z, z.z 1160 | 1161 | return m 1162 | new_rotate_triple_axis = classmethod(new_rotate_triple_axis) 1163 | 1164 | def new_look_at(cls, eye, at, up): 1165 | z = (eye - at).normalized() 1166 | x = up.cross(z).normalized() 1167 | y = z.cross(x) 1168 | 1169 | m = cls.new_rotate_triple_axis(x, y, z) 1170 | m.transpose() 1171 | m.d, m.h, m.l = -x.dot(eye), -y.dot(eye), -z.dot(eye) 1172 | return m 1173 | new_look_at = classmethod(new_look_at) 1174 | 1175 | def new_perspective(cls, fov_y, aspect, near, far): 1176 | # from the gluPerspective man page 1177 | f = 1 / math.tan(fov_y / 2) 1178 | self = cls() 1179 | assert near != 0.0 and near != far 1180 | self.a = f / aspect 1181 | self.f = f 1182 | self.k = (far + near) / (near - far) 1183 | self.l = 2 * far * near / (near - far) 1184 | self.o = -1 1185 | self.p = 0 1186 | return self 1187 | new_perspective = classmethod(new_perspective) 1188 | 1189 | def determinant(self): 1190 | return ((self.a * self.f - self.e * self.b) 1191 | * (self.k * self.p - self.o * self.l) 1192 | - (self.a * self.j - self.i * self.b) 1193 | * (self.g * self.p - self.o * self.h) 1194 | + (self.a * self.n - self.m * self.b) 1195 | * (self.g * self.l - self.k * self.h) 1196 | + (self.e * self.j - self.i * self.f) 1197 | * (self.c * self.p - self.o * self.d) 1198 | - (self.e * self.n - self.m * self.f) 1199 | * (self.c * self.l - self.k * self.d) 1200 | + (self.i * self.n - self.m * self.j) 1201 | * (self.c * self.h - self.g * self.d)) 1202 | 1203 | def inverse(self): 1204 | tmp = Matrix4() 1205 | d = self.determinant(); 1206 | 1207 | if abs(d) < 0.001: 1208 | # No inverse, return identity 1209 | return tmp 1210 | else: 1211 | d = 1.0 / d; 1212 | 1213 | tmp.a = d * (self.f * (self.k * self.p - self.o * self.l) + self.j * (self.o * self.h - self.g * self.p) + self.n * (self.g * self.l - self.k * self.h)); 1214 | tmp.e = d * (self.g * (self.i * self.p - self.m * self.l) + self.k * (self.m * self.h - self.e * self.p) + self.o * (self.e * self.l - self.i * self.h)); 1215 | tmp.i = d * (self.h * (self.i * self.n - self.m * self.j) + self.l * (self.m * self.f - self.e * self.n) + self.p * (self.e * self.j - self.i * self.f)); 1216 | tmp.m = d * (self.e * (self.n * self.k - self.j * self.o) + self.i * (self.f * self.o - self.n * self.g) + self.m * (self.j * self.g - self.f * self.k)); 1217 | 1218 | tmp.b = d * (self.j * (self.c * self.p - self.o * self.d) + self.n * (self.k * self.d - self.c * self.l) + self.b * (self.o * self.l - self.k * self.p)); 1219 | tmp.f = d * (self.k * (self.a * self.p - self.m * self.d) + self.o * (self.i * self.d - self.a * self.l) + self.c * (self.m * self.l - self.i * self.p)); 1220 | tmp.j = d * (self.l * (self.a * self.n - self.m * self.b) + self.p * (self.i * self.b - self.a * self.j) + self.d * (self.m * self.j - self.i * self.n)); 1221 | tmp.n = d * (self.i * (self.n * self.c - self.b * self.o) + self.m * (self.b * self.k - self.j * self.c) + self.a * (self.j * self.o - self.n * self.k)); 1222 | 1223 | tmp.c = d * (self.n * (self.c * self.h - self.g * self.d) + self.b * (self.g * self.p - self.o * self.h) + self.f * (self.o * self.d - self.c * self.p)); 1224 | tmp.g = d * (self.o * (self.a * self.h - self.e * self.d) + self.c * (self.e * self.p - self.m * self.h) + self.g * (self.m * self.d - self.a * self.p)); 1225 | tmp.k = d * (self.p * (self.a * self.f - self.e * self.b) + self.d * (self.e * self.n - self.m * self.f) + self.h * (self.m * self.b - self.a * self.n)); 1226 | tmp.o = d * (self.m * (self.f * self.c - self.b * self.g) + self.a * (self.n * self.g - self.f * self.o) + self.e * (self.b * self.o - self.n * self.c)); 1227 | 1228 | tmp.d = d * (self.b * (self.k * self.h - self.g * self.l) + self.f * (self.c * self.l - self.k * self.d) + self.j * (self.g * self.d - self.c * self.h)); 1229 | tmp.h = d * (self.c * (self.i * self.h - self.e * self.l) + self.g * (self.a * self.l - self.i * self.d) + self.k * (self.e * self.d - self.a * self.h)); 1230 | tmp.l = d * (self.d * (self.i * self.f - self.e * self.j) + self.h * (self.a * self.j - self.i * self.b) + self.l * (self.e * self.b - self.a * self.f)); 1231 | tmp.p = d * (self.a * (self.f * self.k - self.j * self.g) + self.e * (self.j * self.c - self.b * self.k) + self.i * (self.b * self.g - self.f * self.c)); 1232 | 1233 | return tmp; 1234 | 1235 | def get_quaternion(self): 1236 | """Returns a quaternion representing the rotation part of the matrix. 1237 | Taken from: 1238 | http://web.archive.org/web/20041029003853/http://www.j3d.org/matrix_faq/matrfaq_latest.html#Q55 1239 | """ 1240 | trace = self.a + self.f + self.k 1241 | 1242 | if trace > 0.00000001: #avoid dividing by zero 1243 | s = math.sqrt(1. + trace) * 2 1244 | x = (self.j - self.g) / s 1245 | y = (self.c - self.i) / s 1246 | z = (self.e - self.b) / s 1247 | w = 0.25 * s 1248 | else: 1249 | #this is really convenient to have now 1250 | mat = (self.a, self.b, self.c, self.d, 1251 | self.e, self.f, self.g, self.h, 1252 | self.i, self.j, self.k, self.l, 1253 | self.m, self.n, self.o, self.p 1254 | ) 1255 | if ( mat[0] > mat[5] and mat[0] > mat[10] ): #Column 0 1256 | s = math.sqrt( 1.0 + mat[0] - mat[5] - mat[10] ) * 2 1257 | x = 0.25 * s 1258 | y = (mat[4] + mat[1] ) / s 1259 | z = (mat[2] + mat[8] ) / s 1260 | w = (mat[9] - mat[6] ) / s 1261 | elif ( mat[5] > mat[10] ): # Column 1 1262 | s = math.sqrt( 1.0 + mat[5] - mat[0] - mat[10] ) * 2 1263 | x = (mat[4] + mat[1] ) / s 1264 | y = 0.25 * s 1265 | z = (mat[9] + mat[6] ) / s 1266 | w = (mat[2] - mat[8] ) / s 1267 | else: # Column 2 1268 | s = math.sqrt( 1.0 + mat[10] - mat[0] - mat[5] ) * 2 1269 | x = (mat[2] + mat[8] ) / s 1270 | y = (mat[9] + mat[6] ) / s 1271 | z = 0.25 * s 1272 | w = (mat[4] - mat[1] ) / s 1273 | 1274 | return Quaternion(w, x, y, z) 1275 | 1276 | 1277 | 1278 | class Quaternion: 1279 | # All methods and naming conventions based off 1280 | # http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions 1281 | 1282 | # w is the real part, (x, y, z) are the imaginary parts 1283 | __slots__ = ['w', 'x', 'y', 'z'] 1284 | 1285 | def __init__(self, w=1, x=0, y=0, z=0): 1286 | self.w = w 1287 | self.x = x 1288 | self.y = y 1289 | self.z = z 1290 | 1291 | def __copy__(self): 1292 | Q = Quaternion() 1293 | Q.w = self.w 1294 | Q.x = self.x 1295 | Q.y = self.y 1296 | Q.z = self.z 1297 | return Q 1298 | 1299 | copy = __copy__ 1300 | 1301 | def __repr__(self): 1302 | return 'Quaternion(real=%.2f, imag=<%.2f, %.2f, %.2f>)' % \ 1303 | (self.w, self.x, self.y, self.z) 1304 | 1305 | def __mul__(self, other): 1306 | if isinstance(other, Quaternion): 1307 | Ax = self.x 1308 | Ay = self.y 1309 | Az = self.z 1310 | Aw = self.w 1311 | Bx = other.x 1312 | By = other.y 1313 | Bz = other.z 1314 | Bw = other.w 1315 | Q = Quaternion() 1316 | Q.x = Ax * Bw + Ay * Bz - Az * By + Aw * Bx 1317 | Q.y = -Ax * Bz + Ay * Bw + Az * Bx + Aw * By 1318 | Q.z = Ax * By - Ay * Bx + Az * Bw + Aw * Bz 1319 | Q.w = -Ax * Bx - Ay * By - Az * Bz + Aw * Bw 1320 | return Q 1321 | elif isinstance(other, Vector3): 1322 | w = self.w 1323 | x = self.x 1324 | y = self.y 1325 | z = self.z 1326 | Vx = other.x 1327 | Vy = other.y 1328 | Vz = other.z 1329 | ww = w * w 1330 | w2 = w * 2 1331 | wx2 = w2 * x 1332 | wy2 = w2 * y 1333 | wz2 = w2 * z 1334 | xx = x * x 1335 | x2 = x * 2 1336 | xy2 = x2 * y 1337 | xz2 = x2 * z 1338 | yy = y * y 1339 | yz2 = 2 * y * z 1340 | zz = z * z 1341 | return other.__class__(\ 1342 | ww * Vx + wy2 * Vz - wz2 * Vy + \ 1343 | xx * Vx + xy2 * Vy + xz2 * Vz - \ 1344 | zz * Vx - yy * Vx, 1345 | xy2 * Vx + yy * Vy + yz2 * Vz + \ 1346 | wz2 * Vx - zz * Vy + ww * Vy - \ 1347 | wx2 * Vz - xx * Vy, 1348 | xz2 * Vx + yz2 * Vy + \ 1349 | zz * Vz - wy2 * Vx - yy * Vz + \ 1350 | wx2 * Vy - xx * Vz + ww * Vz) 1351 | else: 1352 | other = other.copy() 1353 | other._apply_transform(self) 1354 | return other 1355 | 1356 | def __imul__(self, other): 1357 | assert isinstance(other, Quaternion) 1358 | Ax = self.x 1359 | Ay = self.y 1360 | Az = self.z 1361 | Aw = self.w 1362 | Bx = other.x 1363 | By = other.y 1364 | Bz = other.z 1365 | Bw = other.w 1366 | self.x = Ax * Bw + Ay * Bz - Az * By + Aw * Bx 1367 | self.y = -Ax * Bz + Ay * Bw + Az * Bx + Aw * By 1368 | self.z = Ax * By - Ay * Bx + Az * Bw + Aw * Bz 1369 | self.w = -Ax * Bx - Ay * By - Az * Bz + Aw * Bw 1370 | return self 1371 | 1372 | def __abs__(self): 1373 | return math.sqrt(self.w ** 2 + \ 1374 | self.x ** 2 + \ 1375 | self.y ** 2 + \ 1376 | self.z ** 2) 1377 | 1378 | magnitude = __abs__ 1379 | 1380 | def magnitude_squared(self): 1381 | return self.w ** 2 + \ 1382 | self.x ** 2 + \ 1383 | self.y ** 2 + \ 1384 | self.z ** 2 1385 | 1386 | def identity(self): 1387 | self.w = 1 1388 | self.x = 0 1389 | self.y = 0 1390 | self.z = 0 1391 | return self 1392 | 1393 | def rotate_axis(self, angle, axis): 1394 | self *= Quaternion.new_rotate_axis(angle, axis) 1395 | return self 1396 | 1397 | def rotate_euler(self, heading, attitude, bank): 1398 | self *= Quaternion.new_rotate_euler(heading, attitude, bank) 1399 | return self 1400 | 1401 | def rotate_matrix(self, m): 1402 | self *= Quaternion.new_rotate_matrix(m) 1403 | return self 1404 | 1405 | def conjugated(self): 1406 | Q = Quaternion() 1407 | Q.w = self.w 1408 | Q.x = -self.x 1409 | Q.y = -self.y 1410 | Q.z = -self.z 1411 | return Q 1412 | 1413 | def normalize(self): 1414 | d = self.magnitude() 1415 | if d != 0: 1416 | self.w /= d 1417 | self.x /= d 1418 | self.y /= d 1419 | self.z /= d 1420 | return self 1421 | 1422 | def normalized(self): 1423 | d = self.magnitude() 1424 | if d != 0: 1425 | Q = Quaternion() 1426 | Q.w = self.w / d 1427 | Q.x = self.x / d 1428 | Q.y = self.y / d 1429 | Q.z = self.z / d 1430 | return Q 1431 | else: 1432 | return self.copy() 1433 | 1434 | def get_angle_axis(self): 1435 | if self.w > 1: 1436 | self = self.normalized() 1437 | angle = 2 * math.acos(self.w) 1438 | s = math.sqrt(1 - self.w ** 2) 1439 | if s < 0.001: 1440 | return angle, Vector3(1, 0, 0) 1441 | else: 1442 | return angle, Vector3(self.x / s, self.y / s, self.z / s) 1443 | 1444 | def get_euler(self): 1445 | t = self.x * self.y + self.z * self.w 1446 | if t > 0.4999: 1447 | heading = 2 * math.atan2(self.x, self.w) 1448 | attitude = math.pi / 2 1449 | bank = 0 1450 | elif t < -0.4999: 1451 | heading = -2 * math.atan2(self.x, self.w) 1452 | attitude = -math.pi / 2 1453 | bank = 0 1454 | else: 1455 | sqx = self.x ** 2 1456 | sqy = self.y ** 2 1457 | sqz = self.z ** 2 1458 | heading = math.atan2(2 * self.y * self.w - 2 * self.x * self.z, 1459 | 1 - 2 * sqy - 2 * sqz) 1460 | attitude = math.asin(2 * t) 1461 | bank = math.atan2(2 * self.x * self.w - 2 * self.y * self.z, 1462 | 1 - 2 * sqx - 2 * sqz) 1463 | return heading, attitude, bank 1464 | 1465 | def get_matrix(self): 1466 | xx = self.x ** 2 1467 | xy = self.x * self.y 1468 | xz = self.x * self.z 1469 | xw = self.x * self.w 1470 | yy = self.y ** 2 1471 | yz = self.y * self.z 1472 | yw = self.y * self.w 1473 | zz = self.z ** 2 1474 | zw = self.z * self.w 1475 | M = Matrix4() 1476 | M.a = 1 - 2 * (yy + zz) 1477 | M.b = 2 * (xy - zw) 1478 | M.c = 2 * (xz + yw) 1479 | M.e = 2 * (xy + zw) 1480 | M.f = 1 - 2 * (xx + zz) 1481 | M.g = 2 * (yz - xw) 1482 | M.i = 2 * (xz - yw) 1483 | M.j = 2 * (yz + xw) 1484 | M.k = 1 - 2 * (xx + yy) 1485 | return M 1486 | 1487 | # Static constructors 1488 | def new_identity(cls): 1489 | return cls() 1490 | new_identity = classmethod(new_identity) 1491 | 1492 | def new_rotate_axis(cls, angle, axis): 1493 | assert(isinstance(axis, Vector3)) 1494 | axis = axis.normalized() 1495 | s = math.sin(angle / 2) 1496 | Q = cls() 1497 | Q.w = math.cos(angle / 2) 1498 | Q.x = axis.x * s 1499 | Q.y = axis.y * s 1500 | Q.z = axis.z * s 1501 | return Q 1502 | new_rotate_axis = classmethod(new_rotate_axis) 1503 | 1504 | def new_rotate_euler(cls, heading, attitude, bank): 1505 | Q = cls() 1506 | c1 = math.cos(heading / 2) 1507 | s1 = math.sin(heading / 2) 1508 | c2 = math.cos(attitude / 2) 1509 | s2 = math.sin(attitude / 2) 1510 | c3 = math.cos(bank / 2) 1511 | s3 = math.sin(bank / 2) 1512 | 1513 | Q.w = c1 * c2 * c3 - s1 * s2 * s3 1514 | Q.x = s1 * s2 * c3 + c1 * c2 * s3 1515 | Q.y = s1 * c2 * c3 + c1 * s2 * s3 1516 | Q.z = c1 * s2 * c3 - s1 * c2 * s3 1517 | return Q 1518 | new_rotate_euler = classmethod(new_rotate_euler) 1519 | 1520 | def new_rotate_matrix(cls, m): 1521 | if m[0*4 + 0] + m[1*4 + 1] + m[2*4 + 2] > 0.00000001: 1522 | t = m[0*4 + 0] + m[1*4 + 1] + m[2*4 + 2] + 1.0 1523 | s = 0.5/math.sqrt(t) 1524 | 1525 | return cls( 1526 | s*t, 1527 | (m[1*4 + 2] - m[2*4 + 1])*s, 1528 | (m[2*4 + 0] - m[0*4 + 2])*s, 1529 | (m[0*4 + 1] - m[1*4 + 0])*s 1530 | ) 1531 | 1532 | elif m[0*4 + 0] > m[1*4 + 1] and m[0*4 + 0] > m[2*4 + 2]: 1533 | t = m[0*4 + 0] - m[1*4 + 1] - m[2*4 + 2] + 1.0 1534 | s = 0.5/math.sqrt(t) 1535 | 1536 | return cls( 1537 | (m[1*4 + 2] - m[2*4 + 1])*s, 1538 | s*t, 1539 | (m[0*4 + 1] + m[1*4 + 0])*s, 1540 | (m[2*4 + 0] + m[0*4 + 2])*s 1541 | ) 1542 | 1543 | elif m[1*4 + 1] > m[2*4 + 2]: 1544 | t = -m[0*4 + 0] + m[1*4 + 1] - m[2*4 + 2] + 1.0 1545 | s = 0.5/math.sqrt(t) 1546 | 1547 | return cls( 1548 | (m[2*4 + 0] - m[0*4 + 2])*s, 1549 | (m[0*4 + 1] + m[1*4 + 0])*s, 1550 | s*t, 1551 | (m[1*4 + 2] + m[2*4 + 1])*s 1552 | ) 1553 | 1554 | else: 1555 | t = -m[0*4 + 0] - m[1*4 + 1] + m[2*4 + 2] + 1.0 1556 | s = 0.5/math.sqrt(t) 1557 | 1558 | return cls( 1559 | (m[0*4 + 1] - m[1*4 + 0])*s, 1560 | (m[2*4 + 0] + m[0*4 + 2])*s, 1561 | (m[1*4 + 2] + m[2*4 + 1])*s, 1562 | s*t 1563 | ) 1564 | new_rotate_matrix = classmethod(new_rotate_matrix) 1565 | 1566 | def new_interpolate(cls, q1, q2, t): 1567 | assert isinstance(q1, Quaternion) and isinstance(q2, Quaternion) 1568 | Q = cls() 1569 | 1570 | costheta = q1.w * q2.w + q1.x * q2.x + q1.y * q2.y + q1.z * q2.z 1571 | if costheta < 0.: 1572 | costheta = -costheta 1573 | q1 = q1.conjugated() 1574 | elif costheta > 1: 1575 | costheta = 1 1576 | 1577 | theta = math.acos(costheta) 1578 | if abs(theta) < 0.01: 1579 | Q.w = q2.w 1580 | Q.x = q2.x 1581 | Q.y = q2.y 1582 | Q.z = q2.z 1583 | return Q 1584 | 1585 | sintheta = math.sqrt(1.0 - costheta * costheta) 1586 | if abs(sintheta) < 0.01: 1587 | Q.w = (q1.w + q2.w) * 0.5 1588 | Q.x = (q1.x + q2.x) * 0.5 1589 | Q.y = (q1.y + q2.y) * 0.5 1590 | Q.z = (q1.z + q2.z) * 0.5 1591 | return Q 1592 | 1593 | ratio1 = math.sin((1 - t) * theta) / sintheta 1594 | ratio2 = math.sin(t * theta) / sintheta 1595 | 1596 | Q.w = q1.w * ratio1 + q2.w * ratio2 1597 | Q.x = q1.x * ratio1 + q2.x * ratio2 1598 | Q.y = q1.y * ratio1 + q2.y * ratio2 1599 | Q.z = q1.z * ratio1 + q2.z * ratio2 1600 | return Q 1601 | new_interpolate = classmethod(new_interpolate) 1602 | 1603 | # Geometry 1604 | # Much maths thanks to Paul Bourke, http://astronomy.swin.edu.au/~pbourke 1605 | # --------------------------------------------------------------------------- 1606 | 1607 | class Geometry: 1608 | def _connect_unimplemented(self, other): 1609 | raise AttributeError('Cannot connect %s to %s' % 1610 | (self.__class__, other.__class__)) 1611 | 1612 | def _intersect_unimplemented(self, other): 1613 | raise AttributeError('Cannot intersect %s and %s' % 1614 | (self.__class__, other.__class__)) 1615 | 1616 | _intersect_point2 = _intersect_unimplemented 1617 | _intersect_line2 = _intersect_unimplemented 1618 | _intersect_circle = _intersect_unimplemented 1619 | _connect_point2 = _connect_unimplemented 1620 | _connect_line2 = _connect_unimplemented 1621 | _connect_circle = _connect_unimplemented 1622 | 1623 | _intersect_point3 = _intersect_unimplemented 1624 | _intersect_line3 = _intersect_unimplemented 1625 | _intersect_sphere = _intersect_unimplemented 1626 | _intersect_plane = _intersect_unimplemented 1627 | _connect_point3 = _connect_unimplemented 1628 | _connect_line3 = _connect_unimplemented 1629 | _connect_sphere = _connect_unimplemented 1630 | _connect_plane = _connect_unimplemented 1631 | 1632 | def intersect(self, other): 1633 | raise NotImplementedError 1634 | 1635 | def connect(self, other): 1636 | raise NotImplementedError 1637 | 1638 | def distance(self, other): 1639 | c = self.connect(other) 1640 | if c: 1641 | return c.length 1642 | return 0.0 1643 | 1644 | def _intersect_point2_circle(P, C): 1645 | return abs(P - C.c) <= C.r 1646 | 1647 | def _intersect_line2_line2(A, B): 1648 | d = B.v.y * A.v.x - B.v.x * A.v.y 1649 | if d == 0: 1650 | return None 1651 | 1652 | dy = A.p.y - B.p.y 1653 | dx = A.p.x - B.p.x 1654 | ua = (B.v.x * dy - B.v.y * dx) / d 1655 | if not A._u_in(ua): 1656 | return None 1657 | ub = (A.v.x * dy - A.v.y * dx) / d 1658 | if not B._u_in(ub): 1659 | return None 1660 | 1661 | return Point2(A.p.x + ua * A.v.x, 1662 | A.p.y + ua * A.v.y) 1663 | 1664 | def _intersect_line2_circle(L, C): 1665 | a = L.v.magnitude_squared() 1666 | b = 2 * (L.v.x * (L.p.x - C.c.x) + \ 1667 | L.v.y * (L.p.y - C.c.y)) 1668 | c = C.c.magnitude_squared() + \ 1669 | L.p.magnitude_squared() - \ 1670 | 2 * C.c.dot(L.p) - \ 1671 | C.r ** 2 1672 | det = b ** 2 - 4 * a * c 1673 | if det < 0: 1674 | return None 1675 | sq = math.sqrt(det) 1676 | u1 = (-b + sq) / (2 * a) 1677 | u2 = (-b - sq) / (2 * a) 1678 | 1679 | if u1 * u2 > 0 and not L._u_in(u1) and not L._u_in(u2): 1680 | return None 1681 | 1682 | if not L._u_in(u1): 1683 | u1 = max(min(u1, 1.0), 0.0) 1684 | if not L._u_in(u2): 1685 | u2 = max(min(u2, 1.0), 0.0) 1686 | 1687 | # Tangent 1688 | if u1 == u2: 1689 | return Point2(L.p.x + u1 * L.v.x, 1690 | L.p.y + u1 * L.v.y) 1691 | 1692 | return LineSegment2(Point2(L.p.x + u1 * L.v.x, 1693 | L.p.y + u1 * L.v.y), 1694 | Point2(L.p.x + u2 * L.v.x, 1695 | L.p.y + u2 * L.v.y)) 1696 | 1697 | def _intersect_circle_circle(A, B): 1698 | d = abs(A.c - B.c) 1699 | s = A.r + B.r 1700 | m = abs(A.r - B.r) 1701 | if d > s or d < m: 1702 | return None 1703 | d2 = d ** 2 1704 | s2 = s ** 2 1705 | m2 = m ** 2 1706 | k = 0.25 * math.sqrt((s2 - d2) * (d2 - m2)) 1707 | dr = (A.r ** 2 - B.r ** 2) / d2 1708 | kd = 2 * k / d2 1709 | return ( 1710 | Point2( 1711 | 0.5 * (A.c.x + B.c.x + (B.c.x - A.c.x) * dr) + (B.c.y - A.c.y) * kd, 1712 | 0.5 * (A.c.y + B.c.y + (B.c.y - A.c.y) * dr) - (B.c.x - A.c.x) * kd), 1713 | Point2( 1714 | 0.5 * (A.c.x + B.c.x + (B.c.x - A.c.x) * dr) - (B.c.y - A.c.y) * kd, 1715 | 0.5 * (A.c.y + B.c.y + (B.c.y - A.c.y) * dr) + (B.c.x - A.c.x) * kd)) 1716 | 1717 | def _connect_point2_line2(P, L): 1718 | d = L.v.magnitude_squared() 1719 | assert d != 0 1720 | u = ((P.x - L.p.x) * L.v.x + \ 1721 | (P.y - L.p.y) * L.v.y) / d 1722 | if not L._u_in(u): 1723 | u = max(min(u, 1.0), 0.0) 1724 | return LineSegment2(P, 1725 | Point2(L.p.x + u * L.v.x, 1726 | L.p.y + u * L.v.y)) 1727 | 1728 | def _connect_point2_circle(P, C): 1729 | v = P - C.c 1730 | v.normalize() 1731 | v *= C.r 1732 | return LineSegment2(P, Point2(C.c.x + v.x, C.c.y + v.y)) 1733 | 1734 | def _connect_line2_line2(A, B): 1735 | d = B.v.y * A.v.x - B.v.x * A.v.y 1736 | if d == 0: 1737 | # Parallel, connect an endpoint with a line 1738 | if isinstance(B, Ray2) or isinstance(B, LineSegment2): 1739 | p1, p2 = _connect_point2_line2(B.p, A) 1740 | return p2, p1 1741 | # No endpoint (or endpoint is on A), possibly choose arbitrary point 1742 | # on line. 1743 | return _connect_point2_line2(A.p, B) 1744 | 1745 | dy = A.p.y - B.p.y 1746 | dx = A.p.x - B.p.x 1747 | ua = (B.v.x * dy - B.v.y * dx) / d 1748 | if not A._u_in(ua): 1749 | ua = max(min(ua, 1.0), 0.0) 1750 | ub = (A.v.x * dy - A.v.y * dx) / d 1751 | if not B._u_in(ub): 1752 | ub = max(min(ub, 1.0), 0.0) 1753 | 1754 | return LineSegment2(Point2(A.p.x + ua * A.v.x, A.p.y + ua * A.v.y), 1755 | Point2(B.p.x + ub * B.v.x, B.p.y + ub * B.v.y)) 1756 | 1757 | def _connect_circle_line2(C, L): 1758 | d = L.v.magnitude_squared() 1759 | assert d != 0 1760 | u = ((C.c.x - L.p.x) * L.v.x + (C.c.y - L.p.y) * L.v.y) / d 1761 | if not L._u_in(u): 1762 | u = max(min(u, 1.0), 0.0) 1763 | point = Point2(L.p.x + u * L.v.x, L.p.y + u * L.v.y) 1764 | v = (point - C.c) 1765 | v.normalize() 1766 | v *= C.r 1767 | return LineSegment2(Point2(C.c.x + v.x, C.c.y + v.y), point) 1768 | 1769 | def _connect_circle_circle(A, B): 1770 | v = B.c - A.c 1771 | d = v.magnitude() 1772 | if A.r >= B.r and d < A.r: 1773 | #centre B inside A 1774 | s1,s2 = +1, +1 1775 | elif B.r > A.r and d < B.r: 1776 | #centre A inside B 1777 | s1,s2 = -1, -1 1778 | elif d >= A.r and d >= B.r: 1779 | s1,s2 = +1, -1 1780 | v.normalize() 1781 | return LineSegment2(Point2(A.c.x + s1 * v.x * A.r, A.c.y + s1 * v.y * A.r), 1782 | Point2(B.c.x + s2 * v.x * B.r, B.c.y + s2 * v.y * B.r)) 1783 | 1784 | 1785 | class Point2(Vector2, Geometry): 1786 | def __repr__(self): 1787 | return 'Point2(%.2f, %.2f)' % (self.x, self.y) 1788 | 1789 | def __lt__(self, other): 1790 | if isinstance(other, Vector2): 1791 | return self.x < other.x 1792 | 1793 | def __eq__(self, other): 1794 | if isinstance(other, Vector2): 1795 | return self.x == other.x and self.y == other.y 1796 | 1797 | def __hash__(self): 1798 | return hash(repr(self)) 1799 | 1800 | def intersect(self, other): 1801 | return other._intersect_point2(self) 1802 | 1803 | def _intersect_circle(self, other): 1804 | return _intersect_point2_circle(self, other) 1805 | 1806 | def connect(self, other): 1807 | return other._connect_point2(self) 1808 | 1809 | def _connect_point2(self, other): 1810 | return LineSegment2(other, self) 1811 | 1812 | def _connect_line2(self, other): 1813 | c = _connect_point2_line2(self, other) 1814 | if c: 1815 | return c._swap() 1816 | 1817 | def _connect_circle(self, other): 1818 | c = _connect_point2_circle(self, other) 1819 | if c: 1820 | return c._swap() 1821 | 1822 | class Line2(Geometry): 1823 | __slots__ = ['p', 'v'] 1824 | 1825 | def __init__(self, *args): 1826 | if len(args) == 3: 1827 | assert isinstance(args[0], Point2) and \ 1828 | isinstance(args[1], Vector2) and \ 1829 | type(args[2]) == float 1830 | self.p = args[0].copy() 1831 | self.v = args[1] * args[2] / abs(args[1]) 1832 | elif len(args) == 2: 1833 | if isinstance(args[0], Point2) and isinstance(args[1], Point2): 1834 | self.p = args[0].copy() 1835 | self.v = args[1] - args[0] 1836 | elif isinstance(args[0], Point2) and isinstance(args[1], Vector2): 1837 | self.p = args[0].copy() 1838 | self.v = args[1].copy() 1839 | else: 1840 | raise AttributeError('%r' % (args,)) 1841 | elif len(args) == 1: 1842 | if isinstance(args[0], Line2): 1843 | self.p = args[0].p.copy() 1844 | self.v = args[0].v.copy() 1845 | else: 1846 | raise AttributeError('%r' % (args,)) 1847 | else: 1848 | raise AttributeError('%r' % (args,)) 1849 | 1850 | if not self.v: 1851 | raise AttributeError('Line has zero-length vector') 1852 | 1853 | def __copy__(self): 1854 | return self.__class__(self.p, self.v) 1855 | 1856 | copy = __copy__ 1857 | 1858 | def __repr__(self): 1859 | return 'Line2(<%.2f, %.2f> + u<%.2f, %.2f>)' % \ 1860 | (self.p.x, self.p.y, self.v.x, self.v.y) 1861 | 1862 | p1 = property(lambda self: self.p) 1863 | p2 = property(lambda self: Point2(self.p.x + self.v.x, 1864 | self.p.y + self.v.y)) 1865 | 1866 | def _apply_transform(self, t): 1867 | self.p = t * self.p 1868 | self.v = t * self.v 1869 | 1870 | def _u_in(self, u): 1871 | return True 1872 | 1873 | def intersect(self, other): 1874 | return other._intersect_line2(self) 1875 | 1876 | def _intersect_line2(self, other): 1877 | return _intersect_line2_line2(self, other) 1878 | 1879 | def _intersect_circle(self, other): 1880 | return _intersect_line2_circle(self, other) 1881 | 1882 | def connect(self, other): 1883 | return other._connect_line2(self) 1884 | 1885 | def _connect_point2(self, other): 1886 | return _connect_point2_line2(other, self) 1887 | 1888 | def _connect_line2(self, other): 1889 | return _connect_line2_line2(other, self) 1890 | 1891 | def _connect_circle(self, other): 1892 | return _connect_circle_line2(other, self) 1893 | 1894 | class Ray2(Line2): 1895 | def __repr__(self): 1896 | return 'Ray2(<%.2f, %.2f> + u<%.2f, %.2f>)' % \ 1897 | (self.p.x, self.p.y, self.v.x, self.v.y) 1898 | 1899 | def _u_in(self, u): 1900 | return u >= 0.0 1901 | 1902 | class LineSegment2(Line2): 1903 | def __repr__(self): 1904 | return 'LineSegment2(<%.2f, %.2f> to <%.2f, %.2f>)' % \ 1905 | (self.p.x, self.p.y, self.p.x + self.v.x, self.p.y + self.v.y) 1906 | 1907 | def _u_in(self, u): 1908 | return u >= 0.0 and u <= 1.0 1909 | 1910 | def __abs__(self): 1911 | return abs(self.v) 1912 | 1913 | def magnitude_squared(self): 1914 | return self.v.magnitude_squared() 1915 | 1916 | def _swap(self): 1917 | # used by connect methods to switch order of points 1918 | self.p = self.p2 1919 | self.v *= -1 1920 | return self 1921 | 1922 | length = property(lambda self: abs(self.v)) 1923 | 1924 | class Circle(Geometry): 1925 | __slots__ = ['c', 'r'] 1926 | 1927 | def __init__(self, center, radius): 1928 | assert isinstance(center, Vector2) and type(radius) == float 1929 | self.c = center.copy() 1930 | self.r = radius 1931 | 1932 | def __copy__(self): 1933 | return self.__class__(self.c, self.r) 1934 | 1935 | copy = __copy__ 1936 | 1937 | def __repr__(self): 1938 | return 'Circle(<%.2f, %.2f>, radius=%.2f)' % \ 1939 | (self.c.x, self.c.y, self.r) 1940 | 1941 | def _apply_transform(self, t): 1942 | self.c = t * self.c 1943 | 1944 | def intersect(self, other): 1945 | return other._intersect_circle(self) 1946 | 1947 | def _intersect_point2(self, other): 1948 | return _intersect_point2_circle(other, self) 1949 | 1950 | def _intersect_line2(self, other): 1951 | return _intersect_line2_circle(other, self) 1952 | 1953 | def _intersect_circle(self, other): 1954 | return _intersect_circle_circle(other, self) 1955 | 1956 | def connect(self, other): 1957 | return other._connect_circle(self) 1958 | 1959 | def _connect_point2(self, other): 1960 | return _connect_point2_circle(other, self) 1961 | 1962 | def _connect_line2(self, other): 1963 | c = _connect_circle_line2(self, other) 1964 | if c: 1965 | return c._swap() 1966 | 1967 | def _connect_circle(self, other): 1968 | return _connect_circle_circle(other, self) 1969 | 1970 | def tangent_points(self, p): 1971 | m = 0.5 * (self.c + p) 1972 | return self.intersect(Circle(m, abs(p - m))) 1973 | 1974 | # 3D Geometry 1975 | # ------------------------------------------------------------------------- 1976 | 1977 | def _connect_point3_line3(P, L): 1978 | d = L.v.magnitude_squared() 1979 | assert d != 0 1980 | u = ((P.x - L.p.x) * L.v.x + \ 1981 | (P.y - L.p.y) * L.v.y + \ 1982 | (P.z - L.p.z) * L.v.z) / d 1983 | if not L._u_in(u): 1984 | u = max(min(u, 1.0), 0.0) 1985 | return LineSegment3(P, Point3(L.p.x + u * L.v.x, 1986 | L.p.y + u * L.v.y, 1987 | L.p.z + u * L.v.z)) 1988 | 1989 | def _connect_point3_sphere(P, S): 1990 | v = P - S.c 1991 | v.normalize() 1992 | v *= S.r 1993 | return LineSegment3(P, Point3(S.c.x + v.x, S.c.y + v.y, S.c.z + v.z)) 1994 | 1995 | def _connect_point3_plane(p, plane): 1996 | n = plane.n.normalized() 1997 | d = p.dot(plane.n) - plane.k 1998 | return LineSegment3(p, Point3(p.x - n.x * d, p.y - n.y * d, p.z - n.z * d)) 1999 | 2000 | def _connect_line3_line3(A, B): 2001 | assert A.v and B.v 2002 | p13 = A.p - B.p 2003 | d1343 = p13.dot(B.v) 2004 | d4321 = B.v.dot(A.v) 2005 | d1321 = p13.dot(A.v) 2006 | d4343 = B.v.magnitude_squared() 2007 | denom = A.v.magnitude_squared() * d4343 - d4321 ** 2 2008 | if denom == 0: 2009 | # Parallel, connect an endpoint with a line 2010 | if isinstance(B, Ray3) or isinstance(B, LineSegment3): 2011 | return _connect_point3_line3(B.p, A)._swap() 2012 | # No endpoint (or endpoint is on A), possibly choose arbitrary 2013 | # point on line. 2014 | return _connect_point3_line3(A.p, B) 2015 | 2016 | ua = (d1343 * d4321 - d1321 * d4343) / denom 2017 | if not A._u_in(ua): 2018 | ua = max(min(ua, 1.0), 0.0) 2019 | ub = (d1343 + d4321 * ua) / d4343 2020 | if not B._u_in(ub): 2021 | ub = max(min(ub, 1.0), 0.0) 2022 | return LineSegment3(Point3(A.p.x + ua * A.v.x, 2023 | A.p.y + ua * A.v.y, 2024 | A.p.z + ua * A.v.z), 2025 | Point3(B.p.x + ub * B.v.x, 2026 | B.p.y + ub * B.v.y, 2027 | B.p.z + ub * B.v.z)) 2028 | 2029 | def _connect_line3_plane(L, P): 2030 | d = P.n.dot(L.v) 2031 | if not d: 2032 | # Parallel, choose an endpoint 2033 | return _connect_point3_plane(L.p, P) 2034 | u = (P.k - P.n.dot(L.p)) / d 2035 | if not L._u_in(u): 2036 | # intersects out of range, choose nearest endpoint 2037 | u = max(min(u, 1.0), 0.0) 2038 | return _connect_point3_plane(Point3(L.p.x + u * L.v.x, 2039 | L.p.y + u * L.v.y, 2040 | L.p.z + u * L.v.z), P) 2041 | # Intersection 2042 | return None 2043 | 2044 | def _connect_sphere_line3(S, L): 2045 | d = L.v.magnitude_squared() 2046 | assert d != 0 2047 | u = ((S.c.x - L.p.x) * L.v.x + \ 2048 | (S.c.y - L.p.y) * L.v.y + \ 2049 | (S.c.z - L.p.z) * L.v.z) / d 2050 | if not L._u_in(u): 2051 | u = max(min(u, 1.0), 0.0) 2052 | point = Point3(L.p.x + u * L.v.x, L.p.y + u * L.v.y, L.p.z + u * L.v.z) 2053 | v = (point - S.c) 2054 | v.normalize() 2055 | v *= S.r 2056 | return LineSegment3(Point3(S.c.x + v.x, S.c.y + v.y, S.c.z + v.z), 2057 | point) 2058 | 2059 | def _connect_sphere_sphere(A, B): 2060 | v = B.c - A.c 2061 | d = v.magnitude() 2062 | if A.r >= B.r and d < A.r: 2063 | #centre B inside A 2064 | s1,s2 = +1, +1 2065 | elif B.r > A.r and d < B.r: 2066 | #centre A inside B 2067 | s1,s2 = -1, -1 2068 | elif d >= A.r and d >= B.r: 2069 | s1,s2 = +1, -1 2070 | 2071 | v.normalize() 2072 | return LineSegment3(Point3(A.c.x + s1* v.x * A.r, 2073 | A.c.y + s1* v.y * A.r, 2074 | A.c.z + s1* v.z * A.r), 2075 | Point3(B.c.x + s2* v.x * B.r, 2076 | B.c.y + s2* v.y * B.r, 2077 | B.c.z + s2* v.z * B.r)) 2078 | 2079 | def _connect_sphere_plane(S, P): 2080 | c = _connect_point3_plane(S.c, P) 2081 | if not c: 2082 | return None 2083 | p2 = c.p2 2084 | v = p2 - S.c 2085 | v.normalize() 2086 | v *= S.r 2087 | return LineSegment3(Point3(S.c.x + v.x, S.c.y + v.y, S.c.z + v.z), 2088 | p2) 2089 | 2090 | def _connect_plane_plane(A, B): 2091 | if A.n.cross(B.n): 2092 | # Planes intersect 2093 | return None 2094 | else: 2095 | # Planes are parallel, connect to arbitrary point 2096 | return _connect_point3_plane(A._get_point(), B) 2097 | 2098 | def _intersect_point3_sphere(P, S): 2099 | return abs(P - S.c) <= S.r 2100 | 2101 | def _intersect_line3_sphere(L, S): 2102 | a = L.v.magnitude_squared() 2103 | b = 2 * (L.v.x * (L.p.x - S.c.x) + \ 2104 | L.v.y * (L.p.y - S.c.y) + \ 2105 | L.v.z * (L.p.z - S.c.z)) 2106 | c = S.c.magnitude_squared() + \ 2107 | L.p.magnitude_squared() - \ 2108 | 2 * S.c.dot(L.p) - \ 2109 | S.r ** 2 2110 | det = b ** 2 - 4 * a * c 2111 | if det < 0: 2112 | return None 2113 | sq = math.sqrt(det) 2114 | u1 = (-b + sq) / (2 * a) 2115 | u2 = (-b - sq) / (2 * a) 2116 | if not L._u_in(u1): 2117 | u1 = max(min(u1, 1.0), 0.0) 2118 | if not L._u_in(u2): 2119 | u2 = max(min(u2, 1.0), 0.0) 2120 | return LineSegment3(Point3(L.p.x + u1 * L.v.x, 2121 | L.p.y + u1 * L.v.y, 2122 | L.p.z + u1 * L.v.z), 2123 | Point3(L.p.x + u2 * L.v.x, 2124 | L.p.y + u2 * L.v.y, 2125 | L.p.z + u2 * L.v.z)) 2126 | 2127 | def _intersect_line3_plane(L, P): 2128 | d = P.n.dot(L.v) 2129 | if not d: 2130 | # Parallel 2131 | return None 2132 | u = (P.k - P.n.dot(L.p)) / d 2133 | if not L._u_in(u): 2134 | return None 2135 | return Point3(L.p.x + u * L.v.x, 2136 | L.p.y + u * L.v.y, 2137 | L.p.z + u * L.v.z) 2138 | 2139 | def _intersect_plane_plane(A, B): 2140 | n1_m = A.n.magnitude_squared() 2141 | n2_m = B.n.magnitude_squared() 2142 | n1d2 = A.n.dot(B.n) 2143 | det = n1_m * n2_m - n1d2 ** 2 2144 | if det == 0: 2145 | # Parallel 2146 | return None 2147 | c1 = (A.k * n2_m - B.k * n1d2) / det 2148 | c2 = (B.k * n1_m - A.k * n1d2) / det 2149 | return Line3(Point3(c1 * A.n.x + c2 * B.n.x, 2150 | c1 * A.n.y + c2 * B.n.y, 2151 | c1 * A.n.z + c2 * B.n.z), 2152 | A.n.cross(B.n)) 2153 | 2154 | class Point3(Vector3, Geometry): 2155 | def __repr__(self): 2156 | return 'Point3(%.2f, %.2f, %.2f)' % (self.x, self.y, self.z) 2157 | 2158 | def intersect(self, other): 2159 | return other._intersect_point3(self) 2160 | 2161 | def _intersect_sphere(self, other): 2162 | return _intersect_point3_sphere(self, other) 2163 | 2164 | def connect(self, other): 2165 | return other._connect_point3(self) 2166 | 2167 | def _connect_point3(self, other): 2168 | if self != other: 2169 | return LineSegment3(other, self) 2170 | return None 2171 | 2172 | def _connect_line3(self, other): 2173 | c = _connect_point3_line3(self, other) 2174 | if c: 2175 | return c._swap() 2176 | 2177 | def _connect_sphere(self, other): 2178 | c = _connect_point3_sphere(self, other) 2179 | if c: 2180 | return c._swap() 2181 | 2182 | def _connect_plane(self, other): 2183 | c = _connect_point3_plane(self, other) 2184 | if c: 2185 | return c._swap() 2186 | 2187 | class Line3: 2188 | __slots__ = ['p', 'v'] 2189 | 2190 | def __init__(self, *args): 2191 | if len(args) == 3: 2192 | assert isinstance(args[0], Point3) and \ 2193 | isinstance(args[1], Vector3) and \ 2194 | type(args[2]) == float 2195 | self.p = args[0].copy() 2196 | self.v = args[1] * args[2] / abs(args[1]) 2197 | elif len(args) == 2: 2198 | if isinstance(args[0], Point3) and isinstance(args[1], Point3): 2199 | self.p = args[0].copy() 2200 | self.v = args[1] - args[0] 2201 | elif isinstance(args[0], Point3) and isinstance(args[1], Vector3): 2202 | self.p = args[0].copy() 2203 | self.v = args[1].copy() 2204 | else: 2205 | raise AttributeError('%r' % (args,)) 2206 | elif len(args) == 1: 2207 | if isinstance(args[0], Line3): 2208 | self.p = args[0].p.copy() 2209 | self.v = args[0].v.copy() 2210 | else: 2211 | raise AttributeError('%r' % (args,)) 2212 | else: 2213 | raise AttributeError('%r' % (args,)) 2214 | 2215 | # XXX This is annoying. 2216 | #if not self.v: 2217 | # raise AttributeError('Line has zero-length vector') 2218 | 2219 | def __copy__(self): 2220 | return self.__class__(self.p, self.v) 2221 | 2222 | copy = __copy__ 2223 | 2224 | def __repr__(self): 2225 | return 'Line3(<%.2f, %.2f, %.2f> + u<%.2f, %.2f, %.2f>)' % \ 2226 | (self.p.x, self.p.y, self.p.z, self.v.x, self.v.y, self.v.z) 2227 | 2228 | p1 = property(lambda self: self.p) 2229 | p2 = property(lambda self: Point3(self.p.x + self.v.x, 2230 | self.p.y + self.v.y, 2231 | self.p.z + self.v.z)) 2232 | 2233 | def _apply_transform(self, t): 2234 | self.p = t * self.p 2235 | self.v = t * self.v 2236 | 2237 | def _u_in(self, u): 2238 | return True 2239 | 2240 | def intersect(self, other): 2241 | return other._intersect_line3(self) 2242 | 2243 | def _intersect_sphere(self, other): 2244 | return _intersect_line3_sphere(self, other) 2245 | 2246 | def _intersect_plane(self, other): 2247 | return _intersect_line3_plane(self, other) 2248 | 2249 | def connect(self, other): 2250 | return other._connect_line3(self) 2251 | 2252 | def _connect_point3(self, other): 2253 | return _connect_point3_line3(other, self) 2254 | 2255 | def _connect_line3(self, other): 2256 | return _connect_line3_line3(other, self) 2257 | 2258 | def _connect_sphere(self, other): 2259 | return _connect_sphere_line3(other, self) 2260 | 2261 | def _connect_plane(self, other): 2262 | c = _connect_line3_plane(self, other) 2263 | if c: 2264 | return c 2265 | 2266 | class Ray3(Line3): 2267 | def __repr__(self): 2268 | return 'Ray3(<%.2f, %.2f, %.2f> + u<%.2f, %.2f, %.2f>)' % \ 2269 | (self.p.x, self.p.y, self.p.z, self.v.x, self.v.y, self.v.z) 2270 | 2271 | def _u_in(self, u): 2272 | return u >= 0.0 2273 | 2274 | class LineSegment3(Line3): 2275 | def __repr__(self): 2276 | return 'LineSegment3(<%.2f, %.2f, %.2f> to <%.2f, %.2f, %.2f>)' % \ 2277 | (self.p.x, self.p.y, self.p.z, 2278 | self.p.x + self.v.x, self.p.y + self.v.y, self.p.z + self.v.z) 2279 | 2280 | def _u_in(self, u): 2281 | return u >= 0.0 and u <= 1.0 2282 | 2283 | def __abs__(self): 2284 | return abs(self.v) 2285 | 2286 | def magnitude_squared(self): 2287 | return self.v.magnitude_squared() 2288 | 2289 | def _swap(self): 2290 | # used by connect methods to switch order of points 2291 | self.p = self.p2 2292 | self.v *= -1 2293 | return self 2294 | 2295 | length = property(lambda self: abs(self.v)) 2296 | 2297 | class Sphere: 2298 | __slots__ = ['c', 'r'] 2299 | 2300 | def __init__(self, center, radius): 2301 | assert isinstance(center, Vector3) and type(radius) == float 2302 | self.c = center.copy() 2303 | self.r = radius 2304 | 2305 | def __copy__(self): 2306 | return self.__class__(self.c, self.r) 2307 | 2308 | copy = __copy__ 2309 | 2310 | def __repr__(self): 2311 | return 'Sphere(<%.2f, %.2f, %.2f>, radius=%.2f)' % \ 2312 | (self.c.x, self.c.y, self.c.z, self.r) 2313 | 2314 | def _apply_transform(self, t): 2315 | self.c = t * self.c 2316 | 2317 | def intersect(self, other): 2318 | return other._intersect_sphere(self) 2319 | 2320 | def _intersect_point3(self, other): 2321 | return _intersect_point3_sphere(other, self) 2322 | 2323 | def _intersect_line3(self, other): 2324 | return _intersect_line3_sphere(other, self) 2325 | 2326 | def connect(self, other): 2327 | return other._connect_sphere(self) 2328 | 2329 | def _connect_point3(self, other): 2330 | return _connect_point3_sphere(other, self) 2331 | 2332 | def _connect_line3(self, other): 2333 | c = _connect_sphere_line3(self, other) 2334 | if c: 2335 | return c._swap() 2336 | 2337 | def _connect_sphere(self, other): 2338 | return _connect_sphere_sphere(other, self) 2339 | 2340 | def _connect_plane(self, other): 2341 | c = _connect_sphere_plane(self, other) 2342 | if c: 2343 | return c 2344 | 2345 | class Plane: 2346 | # n.p = k, where n is normal, p is point on plane, k is constant scalar 2347 | __slots__ = ['n', 'k'] 2348 | 2349 | def __init__(self, *args): 2350 | if len(args) == 3: 2351 | assert isinstance(args[0], Point3) and \ 2352 | isinstance(args[1], Point3) and \ 2353 | isinstance(args[2], Point3) 2354 | self.n = (args[1] - args[0]).cross(args[2] - args[0]) 2355 | self.n.normalize() 2356 | self.k = self.n.dot(args[0]) 2357 | elif len(args) == 2: 2358 | if isinstance(args[0], Point3) and isinstance(args[1], Vector3): 2359 | self.n = args[1].normalized() 2360 | self.k = self.n.dot(args[0]) 2361 | elif isinstance(args[0], Vector3) and type(args[1]) == float: 2362 | self.n = args[0].normalized() 2363 | self.k = args[1] 2364 | else: 2365 | raise AttributeError('%r' % (args,)) 2366 | 2367 | else: 2368 | raise AttributeError('%r' % (args,)) 2369 | 2370 | if not self.n: 2371 | raise AttributeError('Points on plane are colinear') 2372 | 2373 | def __copy__(self): 2374 | return self.__class__(self.n, self.k) 2375 | 2376 | copy = __copy__ 2377 | 2378 | def __repr__(self): 2379 | return 'Plane(<%.2f, %.2f, %.2f>.p = %.2f)' % \ 2380 | (self.n.x, self.n.y, self.n.z, self.k) 2381 | 2382 | def _get_point(self): 2383 | # Return an arbitrary point on the plane 2384 | if self.n.z: 2385 | return Point3(0., 0., self.k / self.n.z) 2386 | elif self.n.y: 2387 | return Point3(0., self.k / self.n.y, 0.) 2388 | else: 2389 | return Point3(self.k / self.n.x, 0., 0.) 2390 | 2391 | def _apply_transform(self, t): 2392 | p = t * self._get_point() 2393 | self.n = t * self.n 2394 | self.k = self.n.dot(p) 2395 | 2396 | def intersect(self, other): 2397 | return other._intersect_plane(self) 2398 | 2399 | def _intersect_line3(self, other): 2400 | return _intersect_line3_plane(other, self) 2401 | 2402 | def _intersect_plane(self, other): 2403 | return _intersect_plane_plane(self, other) 2404 | 2405 | def connect(self, other): 2406 | return other._connect_plane(self) 2407 | 2408 | def _connect_point3(self, other): 2409 | return _connect_point3_plane(other, self) 2410 | 2411 | def _connect_line3(self, other): 2412 | return _connect_line3_plane(other, self) 2413 | 2414 | def _connect_sphere(self, other): 2415 | return _connect_sphere_plane(other, self) 2416 | 2417 | def _connect_plane(self, other): 2418 | return _connect_plane_plane(other, self) 2419 | --------------------------------------------------------------------------------