Mercurial > hg-public > lasercut-scripts / file comparison
comparison: printrun-src/printrun/stltool.py
printrun-src/printrun/stltool.py
- changeset 15
- 0bbb006204fc
- child 46
- cce0af6351f0
equal
deleted
inserted
replaced
| 14:51bf56ba3c10 | 15:0bbb006204fc |
|---|---|
| 1 # coding: utf-8 | |
| 2 | |
| 3 # This file is part of the Printrun suite. | |
| 4 # | |
| 5 # Printrun is free software: you can redistribute it and/or modify | |
| 6 # it under the terms of the GNU General Public License as published by | |
| 7 # the Free Software Foundation, either version 3 of the License, or | |
| 8 # (at your option) any later version. | |
| 9 # | |
| 10 # Printrun is distributed in the hope that it will be useful, | |
| 11 # but WITHOUT ANY WARRANTY; without even the implied warranty of | |
| 12 # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the | |
| 13 # GNU General Public License for more details. | |
| 14 # | |
| 15 # You should have received a copy of the GNU General Public License | |
| 16 # along with Printrun. If not, see <http://www.gnu.org/licenses/>. | |
| 17 | |
| 18 import sys | |
| 19 import struct | |
| 20 import math | |
| 21 import logging | |
| 22 | |
| 23 import numpy | |
| 24 import numpy.linalg | |
| 25 | |
| 26 def normalize(v): | |
| 27 return v / numpy.linalg.norm(v) | |
| 28 | |
| 29 def genfacet(v): | |
| 30 veca = v[1] - v[0] | |
| 31 vecb = v[2] - v[1] | |
| 32 vecx = numpy.cross(veca, vecb) | |
| 33 vlen = numpy.linalg.norm(vecx) | |
| 34 if vlen == 0: | |
| 35 vlen = 1 | |
| 36 normal = vecx / vlen | |
| 37 return (normal, v) | |
| 38 | |
| 39 I = numpy.identity(4) | |
| 40 | |
| 41 def homogeneous(v, w = 1): | |
| 42 return numpy.append(v, w) | |
| 43 | |
| 44 def applymatrix(facet, matrix = I): | |
| 45 return genfacet(map(lambda x: matrix.dot(homogeneous(x))[:3], facet[1])) | |
| 46 | |
| 47 def ray_triangle_intersection(ray_near, ray_dir, (v1, v2, v3)): | |
| 48 """ | |
| 49 Möller–Trumbore intersection algorithm in pure python | |
| 50 Based on http://en.wikipedia.org/wiki/M%C3%B6ller%E2%80%93Trumbore_intersection_algorithm | |
| 51 """ | |
| 52 eps = 0.000001 | |
| 53 edge1 = v2 - v1 | |
| 54 edge2 = v3 - v1 | |
| 55 pvec = numpy.cross(ray_dir, edge2) | |
| 56 det = edge1.dot(pvec) | |
| 57 if abs(det) < eps: | |
| 58 return False, None | |
| 59 inv_det = 1. / det | |
| 60 tvec = ray_near - v1 | |
| 61 u = tvec.dot(pvec) * inv_det | |
| 62 if u < 0. or u > 1.: | |
| 63 return False, None | |
| 64 qvec = numpy.cross(tvec, edge1) | |
| 65 v = ray_dir.dot(qvec) * inv_det | |
| 66 if v < 0. or u + v > 1.: | |
| 67 return False, None | |
| 68 | |
| 69 t = edge2.dot(qvec) * inv_det | |
| 70 if t < eps: | |
| 71 return False, None | |
| 72 | |
| 73 return True, t | |
| 74 | |
| 75 def ray_rectangle_intersection(ray_near, ray_dir, p0, p1, p2, p3): | |
| 76 match1, _ = ray_triangle_intersection(ray_near, ray_dir, (p0, p1, p2)) | |
| 77 match2, _ = ray_triangle_intersection(ray_near, ray_dir, (p0, p2, p3)) | |
| 78 return match1 or match2 | |
| 79 | |
| 80 def ray_box_intersection(ray_near, ray_dir, p0, p1): | |
| 81 x0, y0, z0 = p0[:] | |
| 82 x1, y1, z1 = p1[:] | |
| 83 rectangles = [((x0, y0, z0), (x1, y0, z0), (x1, y1, z0), (x0, y1, z0)), | |
| 84 ((x0, y0, z1), (x1, y0, z1), (x1, y1, z1), (x0, y1, z1)), | |
| 85 ((x0, y0, z0), (x1, y0, z0), (x1, y0, z1), (x0, y0, z1)), | |
| 86 ((x0, y1, z0), (x1, y1, z0), (x1, y1, z1), (x0, y1, z1)), | |
| 87 ((x0, y0, z0), (x0, y1, z0), (x0, y1, z1), (x0, y0, z1)), | |
| 88 ((x1, y0, z0), (x1, y1, z0), (x1, y1, z1), (x1, y0, z1)), | |
| 89 ] | |
| 90 rectangles = [(numpy.array(p) for p in rect) | |
| 91 for rect in rectangles] | |
| 92 for rect in rectangles: | |
| 93 if ray_rectangle_intersection(ray_near, ray_dir, *rect): | |
| 94 return True | |
| 95 return False | |
| 96 | |
| 97 def emitstl(filename, facets = [], objname = "stltool_export", binary = True): | |
| 98 if filename is None: | |
| 99 return | |
| 100 if binary: | |
| 101 with open(filename, "wb") as f: | |
| 102 buf = "".join(["\0"] * 80) | |
| 103 buf += struct.pack("<I", len(facets)) | |
| 104 facetformat = struct.Struct("<ffffffffffffH") | |
| 105 for facet in facets: | |
| 106 l = list(facet[0][:]) | |
| 107 for vertex in facet[1]: | |
| 108 l += list(vertex[:]) | |
| 109 l.append(0) | |
| 110 buf += facetformat.pack(*l) | |
| 111 f.write(buf) | |
| 112 else: | |
| 113 with open(filename, "w") as f: | |
| 114 f.write("solid " + objname + "\n") | |
| 115 for i in facets: | |
| 116 f.write(" facet normal " + " ".join(map(str, i[0])) + "\n outer loop\n") | |
| 117 for j in i[1]: | |
| 118 f.write(" vertex " + " ".join(map(str, j)) + "\n") | |
| 119 f.write(" endloop" + "\n") | |
| 120 f.write(" endfacet" + "\n") | |
| 121 f.write("endsolid " + objname + "\n") | |
| 122 | |
| 123 class stl(object): | |
| 124 | |
| 125 _dims = None | |
| 126 | |
| 127 def _get_dims(self): | |
| 128 if self._dims is None: | |
| 129 minx = float("inf") | |
| 130 miny = float("inf") | |
| 131 minz = float("inf") | |
| 132 maxx = float("-inf") | |
| 133 maxy = float("-inf") | |
| 134 maxz = float("-inf") | |
| 135 for normal, facet in self.facets: | |
| 136 for vert in facet: | |
| 137 if vert[0] < minx: | |
| 138 minx = vert[0] | |
| 139 if vert[1] < miny: | |
| 140 miny = vert[1] | |
| 141 if vert[2] < minz: | |
| 142 minz = vert[2] | |
| 143 if vert[0] > maxx: | |
| 144 maxx = vert[0] | |
| 145 if vert[1] > maxy: | |
| 146 maxy = vert[1] | |
| 147 if vert[2] > maxz: | |
| 148 maxz = vert[2] | |
| 149 self._dims = [minx, maxx, miny, maxy, minz, maxz] | |
| 150 return self._dims | |
| 151 dims = property(_get_dims) | |
| 152 | |
| 153 def __init__(self, filename = None): | |
| 154 self.facet = (numpy.zeros(3), (numpy.zeros(3), numpy.zeros(3), numpy.zeros(3))) | |
| 155 self.facets = [] | |
| 156 self.facetsminz = [] | |
| 157 self.facetsmaxz = [] | |
| 158 | |
| 159 self.name = "" | |
| 160 self.insolid = 0 | |
| 161 self.infacet = 0 | |
| 162 self.inloop = 0 | |
| 163 self.facetloc = 0 | |
| 164 if filename is None: | |
| 165 return | |
| 166 with open(filename) as f: | |
| 167 data = f.read() | |
| 168 if "facet normal" in data[1:300] and "outer loop" in data[1:300]: | |
| 169 lines = data.split("\n") | |
| 170 for line in lines: | |
| 171 if not self.parseline(line): | |
| 172 return | |
| 173 else: | |
| 174 logging.warning("Not an ascii stl solid - attempting to parse as binary") | |
| 175 f = open(filename, "rb") | |
| 176 buf = f.read(84) | |
| 177 while len(buf) < 84: | |
| 178 newdata = f.read(84 - len(buf)) | |
| 179 if not len(newdata): | |
| 180 break | |
| 181 buf += newdata | |
| 182 facetcount = struct.unpack_from("<I", buf, 80) | |
| 183 facetformat = struct.Struct("<ffffffffffffH") | |
| 184 for i in xrange(facetcount[0]): | |
| 185 buf = f.read(50) | |
| 186 while len(buf) < 50: | |
| 187 newdata = f.read(50 - len(buf)) | |
| 188 if not len(newdata): | |
| 189 break | |
| 190 buf += newdata | |
| 191 fd = list(facetformat.unpack(buf)) | |
| 192 self.name = "binary soloid" | |
| 193 facet = [fd[:3], [fd[3:6], fd[6:9], fd[9:12]]] | |
| 194 self.facets.append(facet) | |
| 195 self.facetsminz.append((min(map(lambda x: x[2], facet[1])), facet)) | |
| 196 self.facetsmaxz.append((max(map(lambda x: x[2], facet[1])), facet)) | |
| 197 f.close() | |
| 198 return | |
| 199 | |
| 200 def intersect_box(self, ray_near, ray_far): | |
| 201 ray_near = numpy.array(ray_near) | |
| 202 ray_far = numpy.array(ray_far) | |
| 203 ray_dir = normalize(ray_far - ray_near) | |
| 204 x0, x1, y0, y1, z0, z1 = self.dims | |
| 205 p0 = numpy.array([x0, y0, z0]) | |
| 206 p1 = numpy.array([x1, y1, z1]) | |
| 207 return ray_box_intersection(ray_near, ray_dir, p0, p1) | |
| 208 | |
| 209 def intersect(self, ray_near, ray_far): | |
| 210 ray_near = numpy.array(ray_near) | |
| 211 ray_far = numpy.array(ray_far) | |
| 212 ray_dir = normalize(ray_far - ray_near) | |
| 213 best_facet = None | |
| 214 best_dist = float("inf") | |
| 215 for facet_i, (normal, facet) in enumerate(self.facets): | |
| 216 match, dist = ray_triangle_intersection(ray_near, ray_dir, facet) | |
| 217 if match and dist < best_dist: | |
| 218 best_facet = facet_i | |
| 219 best_dist = dist | |
| 220 return best_facet, best_dist | |
| 221 | |
| 222 def rebase(self, facet_i): | |
| 223 normal, facet = self.facets[facet_i] | |
| 224 u1 = facet[1] - facet[0] | |
| 225 v2 = facet[2] - facet[0] | |
| 226 n1 = u1.dot(u1) | |
| 227 e1 = u1 / math.sqrt(n1) | |
| 228 u2 = v2 - u1 * v2.dot(u1) / n1 | |
| 229 e2 = u2 / numpy.linalg.norm(u2) | |
| 230 e3 = numpy.cross(e1, e2) | |
| 231 # Ensure Z direction if opposed to the normal | |
| 232 if normal.dot(e3) > 0: | |
| 233 e2 = - e2 | |
| 234 e3 = - e3 | |
| 235 matrix = [[e1[0], e2[0], e3[0], 0], | |
| 236 [e1[1], e2[1], e3[1], 0], | |
| 237 [e1[2], e2[2], e3[2], 0], | |
| 238 [0, 0, 0, 1]] | |
| 239 matrix = numpy.array(matrix) | |
| 240 # Inverse change of basis matrix | |
| 241 matrix = numpy.linalg.inv(matrix) | |
| 242 # Set first vertex of facet as origin | |
| 243 neworig = matrix.dot(homogeneous(facet[0])) | |
| 244 matrix[:3, 3] = -neworig[:3] | |
| 245 newmodel = self.transform(matrix) | |
| 246 return newmodel | |
| 247 | |
| 248 def cut(self, axis, direction, dist): | |
| 249 s = stl() | |
| 250 s.facets = [] | |
| 251 f = min if direction == 1 else max | |
| 252 for _, facet in self.facets: | |
| 253 minval = f([vertex[axis] for vertex in facet]) | |
| 254 if direction * minval > direction * dist: | |
| 255 continue | |
| 256 vertices = [] | |
| 257 for vertex in facet: | |
| 258 vertex = numpy.copy(vertex) | |
| 259 if direction * (vertex[axis] - dist) > 0: | |
| 260 vertex[axis] = dist | |
| 261 vertices.append(vertex) | |
| 262 s.facets.append(genfacet(vertices)) | |
| 263 s.insolid = 0 | |
| 264 s.infacet = 0 | |
| 265 s.inloop = 0 | |
| 266 s.facetloc = 0 | |
| 267 s.name = self.name | |
| 268 for facet in s.facets: | |
| 269 s.facetsminz += [(min(map(lambda x:x[2], facet[1])), facet)] | |
| 270 s.facetsmaxz += [(max(map(lambda x:x[2], facet[1])), facet)] | |
| 271 return s | |
| 272 | |
| 273 def translation_matrix(self, v): | |
| 274 matrix = [[1, 0, 0, v[0]], | |
| 275 [0, 1, 0, v[1]], | |
| 276 [0, 0, 1, v[2]], | |
| 277 [0, 0, 0, 1] | |
| 278 ] | |
| 279 return numpy.array(matrix) | |
| 280 | |
| 281 def translate(self, v = [0, 0, 0]): | |
| 282 return self.transform(self.translation_matrix(v)) | |
| 283 | |
| 284 def rotation_matrix(self, v): | |
| 285 z = v[2] | |
| 286 matrix1 = [[math.cos(math.radians(z)), -math.sin(math.radians(z)), 0, 0], | |
| 287 [math.sin(math.radians(z)), math.cos(math.radians(z)), 0, 0], | |
| 288 [0, 0, 1, 0], | |
| 289 [0, 0, 0, 1] | |
| 290 ] | |
| 291 matrix1 = numpy.array(matrix1) | |
| 292 y = v[0] | |
| 293 matrix2 = [[1, 0, 0, 0], | |
| 294 [0, math.cos(math.radians(y)), -math.sin(math.radians(y)), 0], | |
| 295 [0, math.sin(math.radians(y)), math.cos(math.radians(y)), 0], | |
| 296 [0, 0, 0, 1] | |
| 297 ] | |
| 298 matrix2 = numpy.array(matrix2) | |
| 299 x = v[1] | |
| 300 matrix3 = [[math.cos(math.radians(x)), 0, -math.sin(math.radians(x)), 0], | |
| 301 [0, 1, 0, 0], | |
| 302 [math.sin(math.radians(x)), 0, math.cos(math.radians(x)), 0], | |
| 303 [0, 0, 0, 1] | |
| 304 ] | |
| 305 matrix3 = numpy.array(matrix3) | |
| 306 return matrix3.dot(matrix2.dot(matrix1)) | |
| 307 | |
| 308 def rotate(self, v = [0, 0, 0]): | |
| 309 return self.transform(self.rotation_matrix(v)) | |
| 310 | |
| 311 def scale_matrix(self, v): | |
| 312 matrix = [[v[0], 0, 0, 0], | |
| 313 [0, v[1], 0, 0], | |
| 314 [0, 0, v[2], 0], | |
| 315 [0, 0, 0, 1] | |
| 316 ] | |
| 317 return numpy.array(matrix) | |
| 318 | |
| 319 def scale(self, v = [0, 0, 0]): | |
| 320 return self.transform(self.scale_matrix(v)) | |
| 321 | |
| 322 def transform(self, m = I): | |
| 323 s = stl() | |
| 324 s.facets = [applymatrix(i, m) for i in self.facets] | |
| 325 s.insolid = 0 | |
| 326 s.infacet = 0 | |
| 327 s.inloop = 0 | |
| 328 s.facetloc = 0 | |
| 329 s.name = self.name | |
| 330 for facet in s.facets: | |
| 331 s.facetsminz += [(min(map(lambda x:x[2], facet[1])), facet)] | |
| 332 s.facetsmaxz += [(max(map(lambda x:x[2], facet[1])), facet)] | |
| 333 return s | |
| 334 | |
| 335 def export(self, f = sys.stdout): | |
| 336 f.write("solid " + self.name + "\n") | |
| 337 for i in self.facets: | |
| 338 f.write(" facet normal " + " ".join(map(str, i[0])) + "\n") | |
| 339 f.write(" outer loop" + "\n") | |
| 340 for j in i[1]: | |
| 341 f.write(" vertex " + " ".join(map(str, j)) + "\n") | |
| 342 f.write(" endloop" + "\n") | |
| 343 f.write(" endfacet" + "\n") | |
| 344 f.write("endsolid " + self.name + "\n") | |
| 345 f.flush() | |
| 346 | |
| 347 def parseline(self, l): | |
| 348 l = l.strip() | |
| 349 if l.startswith("solid"): | |
| 350 self.insolid = 1 | |
| 351 self.name = l[6:] | |
| 352 elif l.startswith("endsolid"): | |
| 353 self.insolid = 0 | |
| 354 return 0 | |
| 355 elif l.startswith("facet normal"): | |
| 356 l = l.replace(", ", ".") | |
| 357 self.infacet = 1 | |
| 358 self.facetloc = 0 | |
| 359 normal = numpy.array(map(float, l.split()[2:])) | |
| 360 self.facet = (normal, (numpy.zeros(3), numpy.zeros(3), numpy.zeros(3))) | |
| 361 elif l.startswith("endfacet"): | |
| 362 self.infacet = 0 | |
| 363 self.facets.append(self.facet) | |
| 364 facet = self.facet | |
| 365 self.facetsminz += [(min(map(lambda x:x[2], facet[1])), facet)] | |
| 366 self.facetsmaxz += [(max(map(lambda x:x[2], facet[1])), facet)] | |
| 367 elif l.startswith("vertex"): | |
| 368 l = l.replace(", ", ".") | |
| 369 self.facet[1][self.facetloc][:] = numpy.array(map(float, l.split()[1:])) | |
| 370 self.facetloc += 1 | |
| 371 return 1 | |
| 372 | |
| 373 if __name__ == "__main__": | |
| 374 s = stl("../../Downloads/frame-vertex-neo-foot-x4.stl") | |
| 375 for i in xrange(11, 11): | |
| 376 working = s.facets[:] | |
| 377 for j in reversed(sorted(s.facetsminz)): | |
| 378 if j[0] > i: | |
| 379 working.remove(j[1]) | |
| 380 else: | |
| 381 break | |
| 382 for j in (sorted(s.facetsmaxz)): | |
| 383 if j[0] < i: | |
| 384 working.remove(j[1]) | |
| 385 else: | |
| 386 break | |
| 387 | |
| 388 print i, len(working) | |
| 389 emitstl("../../Downloads/frame-vertex-neo-foot-x4-a.stl", s.facets, "emitted_object") | |
| 390 # stl("../prusamendel/stl/mendelplate.stl") |
