import copy import OpenGL.GL as GL from OpenGL import GLU import hal try: import linuxcnc except Exception: linuxcnc = None import array, itertools from array import array as array_type from math import * import math import time import multiprocessing as mp import ctypes import threading from collections import deque def _worker_tool_surface_z_at_radius(profile, rho): """Worker-side copy of MeshMaterial3D._tool_surface_z_at_radius().""" best_z = None for item in profile: kind = item[0] if kind == "sphere": _, center_z, radius = item radius = float(radius) if rho <= radius: h = math.sqrt(max(0.0, radius * radius - rho * rho)) z = float(center_z) - h if best_z is None or z < best_z: best_z = z elif kind == "cylinder": _, z1, z2, radius = item radius = float(radius) if rho <= radius: z = min(float(z1), float(z2)) if best_z is None or z < best_z: best_z = z elif kind == "frustum": _, z1, z2, r1, r2 = item z1, z2, r1, r2 = float(z1), float(z2), float(r1), float(r2) if z1 > z2: z1, z2 = z2, z1 r1, r2 = r2, r1 if rho > max(r1, r2): continue if rho <= r1: z = z1 elif r2 > r1 and rho <= r2: z = z1 + (rho - r1) * (z2 - z1) / (r2 - r1) else: continue if best_z is None or z < best_z: best_z = z return best_z def _mesh_cut_worker(conn): """Process worker for CPU-only stock cutting. use NumPy for the expensive cutter/height-map operation when it is available. The numerical model is the same as the previous worker: every captured tool position is applied to the same height map and the lowest tool surface wins. NumPy only changes how the arithmetic is executed; it does not drop positions or interpolate them differently. A pure-Python fallback is kept for systems without NumPy. """ import array as _array try: import numpy as _np except Exception: _np = None height = None profile = [] RES_X = RES_Y = 0 dx = dy = 0.0 x1 = y1 = 0.0 tile_size = 25 R_body = 0.0 def _profile_numpy(rho2): """Return the same lowest local tool Z as the old scalar function.""" inf = _np.inf best = _np.full(rho2.shape, inf, dtype=_np.float64) for item in profile: kind = item[0] if kind == "sphere": _, center_z, radius = item radius = float(radius) if radius <= 0.0: continue r2 = radius * radius mask = rho2 <= r2 if _np.any(mask): vals = float(center_z) - _np.sqrt(_np.maximum(0.0, r2 - rho2[mask])) best[mask] = _np.minimum(best[mask], vals) elif kind == "cylinder": _, z1, z2, radius = item radius = float(radius) if radius <= 0.0: continue mask = rho2 <= radius * radius if _np.any(mask): best[mask] = _np.minimum(best[mask], min(float(z1), float(z2))) elif kind == "frustum": _, z1, z2, r1, r2 = item z1, z2 = float(z1), float(z2) r1, r2 = float(r1), float(r2) if z1 > z2: z1, z2 = z2, z1 r1, r2 = r2, r1 if max(r1, r2) <= 0.0: continue mask_outer = rho2 <= max(r1, r2) * max(r1, r2) if not _np.any(mask_outer): continue rho = _np.sqrt(rho2[mask_outer]) vals = _np.full(rho.shape, inf, dtype=_np.float64) m1 = rho <= r1 vals[m1] = z1 if r2 > r1: m2 = (~m1) & (rho <= r2) vals[m2] = z1 + (rho[m2] - r1) * (z2 - z1) / (r2 - r1) valid = _np.isfinite(vals) if _np.any(valid): idx = _np.flatnonzero(mask_outer) best_flat = best.reshape(-1) best_flat[idx[valid]] = _np.minimum(best_flat[idx[valid]], vals[valid]) return best while True: try: msg = conn.recv() except (EOFError, OSError): break if not msg: continue cmd = msg[0] if cmd == "stop": break if cmd == "init": _, res_x, res_y, px1, py1, pdx, pdy, ptiles, profile_data, height_bytes = msg RES_X = int(res_x) RES_Y = int(res_y) x1, y1 = float(px1), float(py1) dx, dy = float(pdx), float(pdy) tile_size = int(ptiles) profile = profile_data for item in profile: if item[0] == "sphere": R_body = max(R_body, float(item[2])) elif item[0] == "cylinder": R_body = max(R_body, float(item[3])) elif item[0] == "frustum": R_body = max(R_body, float(item[3]), float(item[4])) if _np is not None: height = _np.frombuffer(height_bytes, dtype=_np.float64).copy() height = height.reshape((RES_X + 1, RES_Y + 1)) else: height = _array.array("d") height.frombytes(height_bytes) conn.send(("ready", len(height_bytes) // 8)) continue if cmd != "batch" or height is None: continue _, token, positions = msg t0 = time.perf_counter() min_ix, max_ix = RES_X, 0 min_iy, max_iy = RES_Y, 0 grid_points = 0 profile_calls = 0 profile_time = 0.0 sample_count = 0 if _np is not None: # Vectorized path. The center-to-grid geometry is identical to # the scalar path; only the arithmetic is performed in arrays. for cx, cy, depth in positions: if R_body <= 0.0: continue ix0 = max(0, int(floor((cx - R_body - x1) / dx))) ix1 = min(RES_X, int(floor((cx + R_body - x1) / dx)) + 1) iy0 = max(0, int(floor((cy - R_body - y1) / dy))) iy1 = min(RES_Y, int(floor((cy + R_body - y1) / dy)) + 1) if ix1 < ix0 or iy1 < iy0: continue min_ix = min(min_ix, ix0) max_ix = max(max_ix, ix1) min_iy = min(min_iy, iy0) max_iy = max(max_iy, iy1) xs = x1 + _np.arange(ix0, ix1 + 1, dtype=_np.float64) * dx ys = y1 + _np.arange(iy0, iy1 + 1, dtype=_np.float64) * dy ddx = xs[:, None] - float(cx) ddy = ys[None, :] - float(cy) rho2 = ddx * ddx + ddy * ddy tool_z_local = _profile_numpy(rho2) valid = _np.isfinite(tool_z_local) if _np.any(valid): region = height[ix0:ix1 + 1, iy0:iy1 + 1] region[valid] = _np.minimum(region[valid], float(depth) + tool_z_local[valid]) profile_calls += int(valid.sum()) grid_points += int(rho2.size) sample_count += 1 if max_ix < min_ix or max_iy < min_iy: conn.send(("result", token, None)) continue # Reproduce the scalar central-difference normals. We calculate # the requested region directly from the full height array so the # outermost row/column uses the same clamped neighbours as before. nx0 = max(0, min_ix - 1) nx1 = min(RES_X, max_ix + 1) ny0 = max(0, min_iy - 1) ny1 = min(RES_Y, max_iy + 1) ix = _np.arange(nx0, nx1 + 1) iy = _np.arange(ny0, ny1 + 1) ixm = _np.maximum(ix - 1, 0) ixp = _np.minimum(ix + 1, RES_X) iym = _np.maximum(iy - 1, 0) iyp = _np.minimum(iy + 1, RES_Y) hvals_np = height[nx0:nx1 + 1, ny0:ny1 + 1] dzdx = (height[ixp[:, None], iy[None, :]] - height[ixm[:, None], iy[None, :]]) / ((ixp - ixm)[:, None] * dx) dzdy = (height[ix[:, None], iyp[None, :]] - height[ix[:, None], iym[None, :]]) / ((iyp - iym)[None, :] * dy) nnx = -dzdx nny = -dzdy nnz = _np.ones_like(nnx) ll = _np.sqrt(nnx * nnx + nny * nny + nnz * nnz) good = ll > 1.0e-12 nnx[good] /= ll[good] nny[good] /= ll[good] nnz[good] /= ll[good] nnx[~good] = 0.0 nny[~good] = 0.0 nnz[~good] = 1.0 nvals_np = _np.stack((nnx, nny, nnz), axis=-1) elapsed = time.perf_counter() - t0 conn.send(( "result", token, nx0, nx1, ny0, ny1, hvals_np.tobytes(), nvals_np.astype(_np.float64, copy=False).tobytes(), grid_points, profile_calls, profile_time, t0, elapsed, len(positions) )) continue # Original scalar fallback for systems without NumPy. for cx, cy, depth in positions: if R_body <= 0.0: continue ix0 = max(0, int(floor((cx - R_body - x1) / dx))) ix1 = min(RES_X, int(floor((cx + R_body - x1) / dx)) + 1) iy0 = max(0, int(floor((cy - R_body - y1) / dy))) iy1 = min(RES_Y, int(floor((cy + R_body - y1) / dy)) + 1) min_ix = min(min_ix, ix0) max_ix = max(max_ix, ix1) min_iy = min(min_iy, iy0) max_iy = max(max_iy, iy1) for ix in range(ix0, ix1 + 1): x = x1 + ix * dx base = ix * (RES_Y + 1) for iy in range(iy0, iy1 + 1): grid_points += 1 idx = base + iy best_z = height[idx] y = y1 + iy * dy ddx = x - cx ddy = y - cy rho2 = ddx * ddx + ddy * ddy if rho2 <= R_body * R_body: rho = math.sqrt(rho2) tool_z_local = _worker_tool_surface_z_at_radius(profile, rho) profile_calls += 1 if tool_z_local is not None: tool_z = depth + tool_z_local if tool_z < best_z: best_z = tool_z height[idx] = best_z sample_count += 1 if max_ix < min_ix or max_iy < min_iy: conn.send(("result", token, None)) continue nx0 = max(0, min_ix - 1) nx1 = min(RES_X, max_ix + 1) ny0 = max(0, min_iy - 1) ny1 = min(RES_Y, max_iy + 1) hvals = _array.array("d") nvals = _array.array("d") for ix in range(nx0, nx1 + 1): base = ix * (RES_Y + 1) for iy in range(ny0, ny1 + 1): hvals.append(height[base + iy]) ixm = max(ix - 1, 0) ixp = min(ix + 1, RES_X) iym = max(iy - 1, 0) iyp = min(iy + 1, RES_Y) dzdx = (height[ixp * (RES_Y + 1) + iy] - height[ixm * (RES_Y + 1) + iy]) / ((ixp - ixm) * dx) dzdy = (height[base + iyp] - height[base + iym]) / ((iyp - iym) * dy) nnx, nny, nnz = -dzdx, -dzdy, 1.0 ll = math.sqrt(nnx*nnx + nny*nny + nnz*nnz) if ll > 1.0e-12: nnx /= ll; nny /= ll; nnz /= ll else: nnx, nny, nnz = 0.0, 0.0, 1.0 nvals.extend((nnx, nny, nnz)) elapsed = time.perf_counter() - t0 conn.send(( "result", token, nx0, nx1, ny0, ny1, hvals.tobytes(), nvals.tobytes(), grid_points, profile_calls, profile_time, t0, elapsed, len(positions) )) # render diagnostic switch: # Same functional worker path as ; render diagnostics only. # worker diagnostic switch: # worker diagnostic: stock ON so the real GUI/render path is tested. DRAW_STOCK = True DO_CUT = True CUT_WORKER = True CUT_UPDATE_HZ = 10.0 # machine-position sampler is independent of the Vismach draw rate. # CUT_UPDATE_HZ still controls batching to the CPU cut worker; it does NOT # control how often the real machine position is captured. CUT_POSITION_HZ = 100.0 # Optional GPU synchronization used only when explicitly enabled for timing. # This does not change geometry or cutting; it separates CPU call-list dispatch # from time spent waiting for OpenGL to finish the submitted drawing. RENDER_DIAG_GL_FINISH = False # functions copied from glnav.py def use_pango_font(font, start, count, will_call_prepost=False): import gi gi.require_version('Pango','1.0') gi.require_version('PangoCairo','1.0') from gi.repository import Pango from gi.repository import PangoCairo #from gi.repository import Cairo as cairo import cairo fontDesc = Pango.FontDescription(font) a = array.array('b', itertools.repeat(0, 256*256)) surface = cairo.ImageSurface.create_for_data(a, cairo.FORMAT_A8, 256, 256) context = cairo.Context(surface) pango_context = PangoCairo.create_context(context) layout = PangoCairo.create_layout(context) fontmap = PangoCairo.font_map_get_default() font = fontmap.load_font(fontmap.create_context(), fontDesc) layout.set_font_description(fontDesc) metrics = font.get_metrics() descent = metrics.get_descent() d = descent / Pango.SCALE linespace = metrics.get_ascent() + metrics.get_descent() width = metrics.get_approximate_char_width() GL.glPushClientAttrib(GL.GL_CLIENT_PIXEL_STORE_BIT) GL.glPixelStorei(GL.GL_UNPACK_SWAP_BYTES, 0) GL.glPixelStorei(GL.GL_UNPACK_LSB_FIRST, 1) GL.glPixelStorei(GL.GL_UNPACK_ROW_LENGTH, 256) GL.glPixelStorei(GL.GL_UNPACK_IMAGE_HEIGHT, 256) GL.glPixelStorei(GL.GL_UNPACK_SKIP_PIXELS, 0) GL.glPixelStorei(GL.GL_UNPACK_SKIP_ROWS, 0) GL.glPixelStorei(GL.GL_UNPACK_SKIP_IMAGES, 0) GL.glPixelStorei(GL.GL_UNPACK_ALIGNMENT, 1) GL.glPixelZoom(1, -1) base = GL.glGenLists(count) for i in range(count): ch = chr(start+i) layout.set_text(ch, -1) w, h = layout.get_size() context.save() context.new_path() context.rectangle(0, 0, 256, 256) context.set_source_rgba(0., 0., 0., 0.) context.set_operator (cairo.OPERATOR_SOURCE) context.paint() context.restore() context.save() context.set_source_rgba(1., 1., 1., 1.) context.set_operator (cairo.OPERATOR_SOURCE) context.move_to(0, 0) PangoCairo.update_context(context,pango_context) PangoCairo.show_layout(context,layout) context.restore() w, h = int(w / Pango.SCALE), int(h / Pango.SCALE) GL.glNewList(base+i, GL.GL_COMPILE) GL.glBitmap(1, 0, 0, 0, 0, h-d, bytearray([0]*4)) #glDrawPixels(0, 0, 0, 0, 0, h-d, ''); if not will_call_prepost: pango_font_pre() if w and h: try: pass GL.glDrawPixels(w, h, GL.GL_LUMINANCE, GL.GL_UNSIGNED_BYTE, a.tobytes()) except Exception as e: print("glnav Exception ",e) GL.glBitmap(1, 0, 0, 0, w, -h+d, bytearray([0]*4)) if not will_call_prepost: pango_font_post() GL.glEndList() GL.glPopClientAttrib() return base, int(width / Pango.SCALE), int(linespace / Pango.SCALE) def pango_font_pre(rgba=(1., 1., 0., 1.)): GL.glPushAttrib(GL.GL_COLOR_BUFFER_BIT) GL.glEnable(GL.GL_BLEND) GL.glBlendFunc(GL.GL_ONE, GL.GL_ONE) def pango_font_post(): GL.glPopAttrib() ################################################# class Collection(object): def __init__(self, parts): self.parts = parts self.vol = 0 def set_world(self, world): self.world = world for p in self.parts: if hasattr(p, "set_world"): p.set_world(world) def find_capture(self): for p in self.parts: if isinstance(p, Capture): print("FOUND CAPTURE:", p) return p if hasattr(p, "find_capture"): c = p.find_capture() if c is not None: return c return None def traverse(self): for p in self.parts: if hasattr(p, "apply"): p.apply() if hasattr(p, "capture"): p.capture() if hasattr(p, "draw"): p.draw() if hasattr(p, "traverse"): p.traverse() if hasattr(p, "unapply"): p.unapply() def set_world(self, world): self.world = world for p in self.parts: if hasattr(p, "set_world"): p.set_world(world) elif hasattr(p, "parts"): p.set_world(world) def __del__(self): try: self._stop_cut_worker() except Exception: pass def volume(self): if hasattr(self, "vol") and self.vol != 0: vol = self.vol else: vol = sum(part.volume() for part in self.parts) # print "Collection.volume", vol return vol # a collection consisting of overlapping parts will have an incorrect # volume, because overlapping volumes will be counted twice. If the # correct volume is known, it can be set using this method def set_volume(self, vol): self.vol = vol; class HideCollection(Collection): def __init__(self, parts, comp, var): self.parts = parts self.comp = comp self.var = var self.vol = 0 def traverse(self): try: if self.comp is None: v = bool(hal.get_value(self.var)) else: v = bool(self.comp[self.var]) except: v = 0 if v: return super(HideCollection,self).traverse() class Translate(Collection): def __init__(self, parts, x, y, z): self.parts = parts self.where = x, y, z def apply(self): GL.glPushMatrix() GL.glTranslatef(*self.where) def unapply(self): GL.glPopMatrix() class Scale(Collection): def __init__(self, parts, x, y, z): self.parts = parts self.scaleby = x, y, z def apply(self): GL.glPushMatrix() GL.glScalef(*self.scaleby) def unapply(self): GL.glPopMatrix() class Rotate(Collection): def __init__(self, parts, th, x, y, z): self.parts = parts self.where = th, x, y, z def apply(self): th, x, y, z = self.where GL.glPushMatrix() GL.glRotatef(th, x, y, z) def unapply(self): GL.glPopMatrix() class Track(Collection): '''move and rotate an object to point from one capture()'d coordinate system to another. we need "world" to convert coordinates from GL_MODELVIEW coordinates to our coordinate system''' def __init__(self, parts, position, target, world): self.parts = parts self.target = target self.position = position self.world2view = world def angle_to(self, x, y, z): '''returns polar coordinates in degrees to a point from the origin a rotates around the x-axis; b rotates around the y axis; r is the distance''' azimuth = atan2(y, x) * 180 / pi # longitude elevation = atan2(z, sqrt(x ** 2 + y ** 2)) * 180 / pi radius = sqrt(x ** 2 + y ** 2 + z ** 2) return ((azimuth, elevation, radius)) def map_coords(self, tx, ty, tz, transform): # now we have to transform them to the world frame wx = tx*transform[0][0]+ty*transform[1][0]+tz*transform[2][0]+transform[3][0] wy = tx*transform[0][1]+ty*transform[1][1]+tz*transform[2][1]+transform[3][1] wz = tx*transform[0][2]+ty*transform[1][2]+tz*transform[2][2]+transform[3][2] return ([wx, wy, wz]) def apply(self): # make sure we have something to work with first if (self.world2view.t == []): # something's borkled - give up print("vismach.py: Track: why am i here? world is not in the scene yet") GL.glPushMatrix() return view2world = invert(self.world2view.t) px, py, pz = self.position.t[3][:3] px, py, pz = self.map_coords(px, py, pz, view2world) tx, ty, tz = self.target.t[3][:3] tx, ty, tz = self.map_coords(tx, ty, tz, view2world) dx = tx - px; dy = ty - py; dz = tz - pz; (az, el, r) = self.angle_to(dx, dy, dz) if (hasattr(HUD, "debug_track") and HUD.debug_track == 1): HUD.strs = [] HUD.strs += ["current coords: %3.4f %3.4f %3.4f " % (px, py, pz)] HUD.strs += ["target coords: %3.4f %3.4f %3.4f" % (tx, ty, tz)] HUD.strs += ["az,el,r: %3.4f %3.4f %3.4f" % (az, el, r)] GL.glPushMatrix() GL.glTranslatef(px, py, pz) GL.glRotatef(az - 90, 0, 0, 1) GL.glRotatef(el - 90, 1, 0, 0) def unapply(self): GL.glPopMatrix() class CoordsBase(object): def __init__(self, *args): if args and (isinstance(args[0], hal.component) or args[0] is None): self.comp = args[0] args = args[1:] else: self.comp = None self._coords = args self.q = GLU.gluNewQuadric() def coords(self): return list(map(self._coord, self._coords)) def _coord(self, v): if isinstance(v, str): if self.comp is None: return hal.get_value(v) else: return self.comp[v] return v # give endpoint X values and radii # resulting cylinder is on the X axis class CylinderX(CoordsBase): def draw(self): x1, r1, x2, r2 = self.coords() if x1 > x2: tmp = x1 x1 = x2 x2 = tmp tmp = r1 r1 = r2 r2 = tmp GL.glPushMatrix() # GL creates cylinders along Z, so need to rotate z1 = x1 z2 = x2 GL.glRotatef(90, 0, 1, 0) # need to translate the whole thing to z1 GL.glTranslatef(0, 0, z1) # the cylinder starts out at Z=0 GLU.gluCylinder(self.q, r1, r2, z2 - z1, 32, 1) # bottom cap GL.glRotatef(180, 1, 0, 0) GLU.gluDisk(self.q, 0, r1, 32, 1) GL.glRotatef(180, 1, 0, 0) # the top cap needs flipped and translated GL.glPushMatrix() GL.glTranslatef(0, 0, z2 - z1) GLU.gluDisk(self.q, 0, r2, 32, 1) GL.glPopMatrix() GL.glPopMatrix() def volume(self): x1, r1, x2, r2 = self.coords() # actually a frustum of a cone vol = 3.1415927 / 3.0 * abs(x1 - x2) * (r1 * r1 + r1 * r2 + r2 * r2) # print "CylinderX.volume", vol return vol # give endpoint Y values and radii # resulting cylinder is on the Y axis class CylinderY(CoordsBase): def __init__(self, y1, r1, y2, r2): self._coords = y1, r1, y2, r2 self.q = GLU.gluNewQuadric() def draw(self): y1, r1, y2, r2 = self.coords() if y1 > y2: tmp = y1 y1 = y2 y2 = tmp tmp = r1 r1 = r2 r2 = tmp GL.glPushMatrix() # GL creates cylinders along Z, so need to rotate z1 = y1 z2 = y2 GL.glRotatef(-90, 1, 0, 0) # need to translate the whole thing to z1 GL.glTranslatef(0, 0, z1) # the cylinder starts out at Z=0 GLU.gluCylinder(self.q, r1, r2, z2 - z1, 32, 1) # bottom cap GL.glRotatef(180, 1, 0, 0) GLU.gluDisk(self.q, 0, r1, 32, 1) GL.glRotatef(180, 1, 0, 0) # the top cap needs flipped and translated GL.glPushMatrix() GL.glTranslatef(0, 0, z2 - z1) GLU.gluDisk(self.q, 0, r2, 32, 1) GL.glPopMatrix() GL.glPopMatrix() def volume(self): y1, r1, y2, r2 = self.coords() # actually a frustum of a cone vol = 3.1415927 / 3.0 * abs(y1 - y2) * (r1 * r1 + r1 * r2 + r2 * r2) # print "CylinderY.volume", vol return vol class CylinderZ(CoordsBase): def draw(self): z1, r1, z2, r2 = self.coords() if z1 > z2: tmp = z1 z1 = z2 z2 = tmp tmp = r1 r1 = r2 r2 = tmp # need to translate the whole thing to z1 GL.glPushMatrix() GL.glTranslatef(0, 0, z1) # the cylinder starts out at Z=0 GLU.gluCylinder(self.q, r1, r2, z2 - z1, 32, 1) # bottom cap GL.glRotatef(180, 1, 0, 0) GLU.gluDisk(self.q, 0, r1, 32, 1) GL.glRotatef(180, 1, 0, 0) # the top cap needs flipped and translated GL.glPushMatrix() GL.glTranslatef(0, 0, z2 - z1) GLU.gluDisk(self.q, 0, r2, 32, 1) GL.glPopMatrix() GL.glPopMatrix() def volume(self): z1, r1, z2, r2 = self.coords() # actually a frustum of a cone vol = 3.1415927 / 3.0 * abs(z1 - z2) * (r1 * r1 + r1 * r2 + r2 * r2) # print "CylinderZ.volume", vol return vol # give center and radius class Sphere(CoordsBase): def draw(self): x, y, z, r = self.coords() # need to translate the whole thing to x,y,z GL.glPushMatrix() GL.glTranslatef(x, y, z) # the sphere starts out at the origin GLU.gluSphere(self.q, r, 32, 16) GL.glPopMatrix() def volume(self): x, y, z, r = self.coords() vol = 1.3333333 * 3.1415927 * r * r * r # print "Sphere.volume", vol return vol # triangular plate in XY plane # specify the corners Z values for each side class TriangleXY(CoordsBase): def draw(self): x1, y1, x2, y2, x3, y3, z1, z2 = self.coords() x12 = x1 - x2 y12 = y1 - y2 x13 = x1 - x3 y13 = y1 - y3 cross = x12 * y13 - x13 * y12 if cross < 0: tmp = x2 x2 = x3 x3 = tmp tmp = y2 y2 = y3 y3 = tmp if z1 > z2: tmp = z1 z1 = z2 z2 = tmp x12 = x1 - x2 y12 = y1 - y2 x23 = x2 - x3 y23 = y2 - y3 x31 = x3 - x1 y31 = y3 - y1 GL.glBegin(GL.GL_QUADS) # side 1-2 h = hypot(x12, y12) GL.glNormal3f(-y12 / h, x12 / h, 0) GL.glVertex3f(x1, y1, z1) GL.glVertex3f(x2, y2, z1) GL.glVertex3f(x2, y2, z2) GL.glVertex3f(x1, y1, z2) # side 2-3 h = hypot(x23, y23) GL.glNormal3f(-y23 / h, x23 / h, 0) GL.glVertex3f(x2, y2, z1) GL.glVertex3f(x3, y3, z1) GL.glVertex3f(x3, y3, z2) GL.glVertex3f(x2, y2, z2) # side 3-1 h = hypot(x31, y31) GL.glNormal3f(-y31 / h, x31 / h, 0) GL.glVertex3f(x3, y3, z1) GL.glVertex3f(x1, y1, z1) GL.glVertex3f(x1, y1, z2) GL.glVertex3f(x3, y3, z2) GL.glEnd() GL.glBegin(GL.GL_TRIANGLES) # upper face GL.glNormal3f(0, 0, 1) GL.glVertex3f(x1, y1, z2) GL.glVertex3f(x2, y2, z2) GL.glVertex3f(x3, y3, z2) # lower face GL.glNormal3f(0, 0, -1) GL.glVertex3f(x1, y1, z1) GL.glVertex3f(x3, y3, z1) GL.glVertex3f(x2, y2, z1) GL.glEnd() def volume(self): x1, y1, x2, y2, x3, y3, z1, z2 = self.coords() # compute pts 2 and 3 relative to 1 (puts pt1 at origin) x2 = x2 - x1 x3 = x3 - x1 y2 = y2 - y1 y3 = y3 - y1 # compute area of triangle area = 0.5 * abs(x2 * y3 - x3 * y2) thk = abs(z1 - z2) vol = area * thk # print "TriangleXY.volume = area * thickness)",vol, area, thk return vol # triangular plate in XZ plane class TriangleXZ(TriangleXY): def coords(self): x1, z1, x2, z2, x3, z3, y1, y2 = TriangleXY.coords(self) return x1, z1, x2, z2, x3, z3, -y1, -y2 def draw(self): GL.glPushMatrix() GL.glRotatef(90, 1, 0, 0) # create the triangle in XY plane TriangleXY.draw(self) # bottom cap GL.glPopMatrix() def volume(self): vol = TriangleXY.volume(self) # print " TriangleXZ.volume",vol return vol # triangular plate in YZ plane class TriangleYZ(TriangleXY): def coords(self): y1, z1, y2, z2, y3, z3, x1, x2 = TriangleXY.coords(self) return z1, y1, z2, y2, z3, y3, -x1, -x2 def draw(self): GL.glPushMatrix() GL.glRotatef(90, 0, -1, 0) # create the triangle in XY plane TriangleXY.draw(self) # bottom cap GL.glPopMatrix() def volume(self): vol = TriangleXY.volume(self) # print " TriangleYZ.volume",vol return vol class ArcX(CoordsBase): def draw(self): x1, x2, r1, r2, a1, a2, steps = self.coords() if x1 > x2: tmp = x1 x1 = x2 x2 = tmp if r1 > r2: tmp = r1 r1 = r2 r2 = tmp while a1 > a2: a2 = a2 + 360 astep = ((a2 - a1) / steps) * (pi / 180) a1rads = a1 * (pi / 180) # positive X end face GL.glBegin(GL.GL_QUAD_STRIP) GL.glNormal3f(1, 0, 0) n = 0 while n <= steps: angle = a1rads + n * astep s = sin(angle) c = cos(angle) GL.glVertex3f(x2, r1 * s, r1 * c) GL.glVertex3f(x2, r2 * s, r2 * c) n = n + 1 GL.glEnd() # negative X end face GL.glBegin(GL.GL_QUAD_STRIP) GL.glNormal3f(-1, 0, 0) n = 0 while n <= steps: angle = a1rads + n * astep s = sin(angle) c = cos(angle) GL.glVertex3f(x1, r1 * s, r1 * c) GL.glVertex3f(x1, r2 * s, r2 * c) n = n + 1 GL.glEnd() # inner diameter GL.glBegin(GL.GL_QUAD_STRIP) n = 0 while n <= steps: angle = a1rads + n * astep s = sin(angle) c = cos(angle) GL.glNormal3f(0, -s, -c) GL.glVertex3f(x1, r1 * s, r1 * c) GL.glVertex3f(x2, r1 * s, r1 * c) n = n + 1 GL.glEnd() # outer diameter GL.glBegin(GL.GL_QUAD_STRIP) n = 0 while n <= steps: angle = a1rads + n * astep s = sin(angle) c = cos(angle) GL.glNormal3f(0, s, c) GL.glVertex3f(x1, r2 * s, r2 * c) GL.glVertex3f(x2, r2 * s, r2 * c) n = n + 1 GL.glEnd() # end plates GL.glBegin(GL.GL_QUADS) # first end plate angle = a1 * (pi / 180) s = sin(angle) c = cos(angle) GL.glNormal3f(0, -c, s) GL.glVertex3f(x1, r2 * s, r2 * c) GL.glVertex3f(x2, r2 * s, r2 * c) GL.glVertex3f(x2, r1 * s, r1 * c) GL.glVertex3f(x1, r1 * s, r1 * c) # other end angle = a2 * (pi / 180) s = sin(angle) c = cos(angle) GL.glNormal3f(0, c, -s) GL.glVertex3f(x1, r2 * s, r2 * c) GL.glVertex3f(x2, r2 * s, r2 * c) GL.glVertex3f(x2, r1 * s, r1 * c) GL.glVertex3f(x1, r1 * s, r1 * c) GL.glEnd() def volume(self): x1, x2, r1, r2, a1, a2, steps = self.coords() if x1 > x2: tmp = x1 x1 = x2 x2 = tmp if r1 > r2: tmp = r1 r1 = r2 r2 = tmp while a1 > a2: a2 = a2 + 360 height = x2 - x1 angle = a2 - a1 area = (angle / 360.0) * pi * (r2 * r2 - r1 * r1) vol = area * height # print "Arc.volume = angle * area * height",vol, angle, area, height return vol # six coordinate version - specify each side of the box class Box(CoordsBase): print("BOX DRAW") def draw(self): x1, y1, z1, x2, y2, z2 = self.coords() self.x_min = x1 self.y_min = y1 self.width = x2 - x1 self.height_mm = y2 - y1 if x1 > x2: tmp = x1 x1 = x2 x2 = tmp if y1 > y2: tmp = y1 y1 = y2 y2 = tmp if z1 > z2: tmp = z1 z1 = z2 z2 = tmp GL.glBegin(GL.GL_QUADS) # bottom face GL.glNormal3f(0, 0, -1) GL.glVertex3f(x2, y1, z1) GL.glVertex3f(x1, y1, z1) GL.glVertex3f(x1, y2, z1) GL.glVertex3f(x2, y2, z1) # positive X face GL.glNormal3f(1, 0, 0) GL.glVertex3f(x2, y1, z1) GL.glVertex3f(x2, y2, z1) GL.glVertex3f(x2, y2, z2) GL.glVertex3f(x2, y1, z2) # positive Y face GL.glNormal3f(0, 1, 0) GL.glVertex3f(x1, y2, z1) GL.glVertex3f(x1, y2, z2) GL.glVertex3f(x2, y2, z2) GL.glVertex3f(x2, y2, z1) # negative Y face GL.glNormal3f(0, -1, 0) GL.glVertex3f(x2, y1, z2) GL.glVertex3f(x1, y1, z2) GL.glVertex3f(x1, y1, z1) GL.glVertex3f(x2, y1, z1) # negative X face GL.glNormal3f(-1, 0, 0) GL.glVertex3f(x1, y1, z1) GL.glVertex3f(x1, y1, z2) GL.glVertex3f(x1, y2, z2) GL.glVertex3f(x1, y2, z1) # top face GL.glNormal3f(0, 0, 1) GL.glVertex3f(x1, y2, z2) GL.glVertex3f(x1, y1, z2) GL.glVertex3f(x2, y1, z2) GL.glVertex3f(x2, y2, z2) GL.glEnd() def volume(self): x1, y1, z1, x2, y2, z2 = self.coords() vol = abs((x1 - x2) * (y1 - y2) * (z1 - z2)) # print "Box.volume", vol return vol ################################################################################################## ################################################################################################## ################################################################################################## class MeshMaterial3D(CoordsBase): print("DRAW MATERIAL 3D ") def __init__(self, *args): super().__init__(*args) self.height = None # automatic physical resolution, cells/mm. # Small stock -> more points/mm, large stock -> fewer points/mm. # Limits are deliberately kept at 3..7 cells/mm as requested. self.GRID_RES_AUTO = True self.GRID_RES_MM = 5.0 # used only when AUTO is disabled self.GRID_RES_MIN = 3.0 self.GRID_RES_MAX = 7.0 self.GRID_RES_REF_AREA = 2000.0 # 40x50 mm -> 7 cells/mm self.GRID_RES_LOG_SLOPE = 2.0 self.RES_X = 0 self.RES_Y = 0 self.RES = 0 # compatibility/diagnostics: longest grid dimension self.dx = 0 self.dy = 0 self.x1 = 0 self.y1 = 0 self.z2 = 0 self.last_tool = None self.tool_radius = 1 # independent lightweight position capture. Vismach/OpenGL may # stall for hundreds of milliseconds, so the machine path must not be # sampled from draw(). The sampler only reads HAL and appends raw XYZ # positions; all cutting/interpolation remains in the existing GUI + # CPU-worker pipeline. self._position_samples = deque() self._position_samples_lock = threading.Lock() self._position_sampler_stop = threading.Event() self._position_sampler_thread = None self._position_sampler_last = None self._position_sampler_started = False self._position_sampler_samples = 0 self._position_sampler_drained = 0 self._position_sampler_peak = 0 # anomaly-only sampler diagnostics. These are deliberately # separate from the normal performance counters so ordinary motion # produces no console spam. self.normal = None self.mesh_list = None self.mesh_dirty = True self.last_rebuild = 0.0 self.tile_size = 50 # render optimization: surface tiles are submitted through one # master display list; cutting/rebuild logic is otherwise unchanged. # PERFORMANCE DIAGNOSTICS self._perf_t0 = time.perf_counter() self._perf_draw_calls = 0 self._perf_draw_time = 0.0 self._dirty_full_draws = 0 self._simple_clean_draws = 0 # Motion-session: tiles rebuilt during a motion session stay full-resolution # for the entire session. This prevents visible full/sparse flicker # around the cutter when worker updates arrive frame by frame. self._motion_full_tiles = set() self._was_motion = False self._motion_full_draws = 0 self._perf_cut_segment_calls = 0 self._perf_cut_segment_time = 0.0 self._perf_cut_tool_calls = 0 self._perf_cut_tool_time = 0.0 self._perf_cut_sample_count = 0 self._perf_scan_time = 0.0 self._perf_grid_points = 0 self._perf_profile_calls = 0 self._perf_profile_time = 0.0 self._perf_normals_calls = 0 self._perf_normals_time = 0.0 self._perf_rebuild_calls = 0 self._perf_rebuild_time = 0.0 self._perf_build_mesh_calls = 0 self._perf_build_mesh_time = 0.0 self._perf_dirty_rebuilt = 0 self._perf_dirty_peak = 0 self._perf_calllist_min_ms = None self._perf_calllist_max_ms = 0.0 self._perf_calllist_draws = 0 self._perf_calllist_tile_calls = 0 self._perf_calllist_wall_calls = 0 self._perf_calllist_tile_time = 0.0 self._perf_calllist_wall_time = 0.0 self._perf_glfinish_before_time = 0.0 self._perf_glfinish_after_time = 0.0 self._perf_poll_time = 0.0 self._perf_worker_apply_time = 0.0 self._perf_wall_time = 0.0 self._perf_dirty_time = 0.0 self._perf_dirty_last = 0 self._perf_dirty_max_per_draw = 0 self._perf_worker_batches = 0 self._perf_worker_batch_time = 0.0 self._perf_worker_points = 0 self._perf_worker_profile = 0 self._perf_worker_normals_time = 0.0 self._perf_worker_queue_peak = 0 self._perf_dirty_added = 0 self._perf_calllist_calls = 0 self._perf_calllist_time = 0.0 self._perf_calllist_last_ms = 0.0 self._perf_calllist_min_ms = None self._perf_calllist_max_ms = 0.0 self._perf_calllist_draws = 0 self._perf_poll_time = 0.0 self._perf_worker_apply_time = 0.0 self._perf_wall_time = 0.0 self._perf_dirty_time = 0.0 self._perf_dirty_last = 0 self._perf_dirty_max_per_draw = 0 self._perf_mesh_setup_time = 0.0 self._perf_mesh_loop_time = 0.0 self._perf_mesh_finish_time = 0.0 self._perf_mesh_quads = 0 self._perf_dirty_added = 0 self._perf_calllist_calls = 0 self._perf_calllist_time = 0.0 self._perf_calllist_last_ms = 0.0 self._perf_calllist_tile_calls = 0 self._perf_calllist_wall_calls = 0 self._perf_calllist_tile_time = 0.0 self._perf_calllist_wall_time = 0.0 self._perf_glfinish_before_time = 0.0 self._perf_glfinish_after_time = 0.0 self._perf_mesh_setup_time = 0.0 self._perf_mesh_loop_time = 0.0 self._perf_mesh_finish_time = 0.0 self._perf_mesh_quads = 0 self._perf_vbo_upload_time = 0.0 self._perf_vbo_draw_calls = 0 # surface tiles are stored in GPU vertex/index buffers (VBO/EBO). # Cutting data and dirty-tile logic remain unchanged. self.tile_vbos = {} self.tile_ebos = {} self.tile_index_counts = {} self.tile_vertex_counts = {} # Sparse-render: simplified rendering uses the SAME full-resolution vertex # VBO (including the original normals) but a second, sparse EBO. # The authoritative height/normal mesh is untouched. During motion # we draw fewer quads from the same vertex buffer; after motion we # automatically return to the full EBO. self.tile_simple_ebos = {} self.tile_simple_index_counts = {} self._simple_ebo_ready = False self._motion_until = 0.0 self._motion_last_tool = None self._motion_last_view = None # automatic sparse-render step derived from total surface quads. # 1 = full resolution, 2 = every second quad, 3 = every third quad. self.SIMPLE_STEP_AUTO = True self.SIMPLE_STEP_MIN = 1 self.SIMPLE_STEP_MAX = 3 self.SIMPLE_STEP_Q1 = 800_000 self.SIMPLE_STEP_Q2 = 2_500_000 self._simple_step = 2 self._auto_grid_res_mm = None self._auto_simple_step = None self._last_stock_dims = None # complete stock rebuild when the loaded NGC changes the # material dimensions. Position changes caused by G54 are handled # separately and must NOT rebuild the mesh. self._last_stock_size = None self._simple_draw_calls = 0 self._full_draw_calls = 0 self._motion_frames = 0 self._static_frames = 0 self._simple_draw_time = 0.0 self._full_draw_time = 0.0 self._simple_builds = 0 self._simple_build_time = 0.0 # Sparse-render: freshly rebuilt tiles are rendered full-resolution for one frame. self._force_full_once = set() self._dirty_full_draws = 0 self._simple_clean_draws = 0 # Motion-session: tiles rebuilt during a motion session stay full-resolution # for the entire session. This prevents visible full/sparse flicker # around the cutter when worker updates arrive frame by frame. self._motion_full_tiles = set() self._was_motion = False self._motion_full_draws = 0 self._vbo_ready = False self.left_list = None self.right_list = None self.front_list = None self.back_list = None self.bottom_list = None self.dirty_tiles = set() self.last_mat_x1 = None self.last_mat_y1 = None self.last_mat_z2 = None # Geometry of the real Vismach tool. The handler supplies the same # tool Collection that is used to draw the visible cutter. self.tool_geometry = None self.tool_profile = [] self.tool_max_radius = 0.0 # CPU cutting worker diagnostics. The worker performs only numerical # height/profile/normal calculations; OpenGL remains in this process. self._cut_worker = None self._cut_worker_conn = None self._cut_worker_pending = [] self._cut_worker_inflight = False self._cut_worker_token = 0 self._cut_worker_last_send = 0.0 self._cut_worker_last_result = 0.0 self._perf_worker_batches = 0 self._perf_worker_batch_time = 0.0 self._perf_worker_wait_time = 0.0 self._perf_worker_points = 0 self._perf_worker_profile = 0 self._perf_worker_normals_time = 0.0 self._perf_worker_queue_peak = 0 def _position_sampler_loop(self): """Capture a coherent machine XYZ position. read XYZ from one LinuxCNC status snapshot instead of three independent HAL reads. The previous method could theoretically combine X/Y/Z values belonging to two different servo updates. That is especially dangerous for the height map because one bad sample with a lower Z becomes a real vertical-looking pit. joint_actual_position is also the same source family used by the Vismach machine handler for the linear axes, so the captured point remains in machine coordinates and no G54 offset reconstruction is needed here. """ stat = None if linuxcnc is not None: try: stat = linuxcnc.stat() except Exception: stat = None period = 1.0 / max(10.0, float(CUT_POSITION_HZ)) next_t = time.perf_counter() while not self._position_sampler_stop.is_set(): try: if stat is not None: stat.poll() jp = stat.joint_actual_position if len(jp) < 3: raise RuntimeError("joint_actual_position has < 3 axes") pos = (float(jp[0]), float(jp[1]), float(jp[2])) else: # Fallback keeps /behaviour if linuxcnc Python # bindings are unavailable. x = float(hal.get_value("halui.axis.x.pos-relative")) y = float(hal.get_value("halui.axis.y.pos-relative")) z = float(hal.get_value("halui.axis.z.pos-relative")) pos = (x, y, z) last = self._position_sampler_last if last is None or pos != last: with self._position_samples_lock: self._position_samples.append(pos) qlen = len(self._position_samples) if qlen > self._position_sampler_peak: self._position_sampler_peak = qlen self._position_sampler_last = pos self._position_sampler_samples += 1 except Exception: pass next_t += period delay = next_t - time.perf_counter() if delay > 0: self._position_sampler_stop.wait(delay) else: next_t = time.perf_counter() def _start_position_sampler(self): if not CUT_WORKER or not DO_CUT: return t = self._position_sampler_thread if t is not None and t.is_alive(): return self._position_sampler_stop.clear() self._position_sampler_thread = threading.Thread( target=self._position_sampler_loop, name="VismachMeshPositionSampler", daemon=True, ) self._position_sampler_thread.start() self._position_sampler_started = True # normal sampler startup is intentionally silent. def _stop_position_sampler(self): self._position_sampler_stop.set() t = self._position_sampler_thread if t is not None and t.is_alive(): try: t.join(timeout=0.2) except Exception: pass self._position_sampler_thread = None def _drain_position_samples(self): """Move all captured positions into the existing cut queue. Interpolation is still 0.2 mm, exactly as before. The important difference is that interpolation now happens between consecutive *captured machine positions*, not between two Vismach frames. """ if not CUT_WORKER or not DO_CUT: return with self._position_samples_lock: if not self._position_samples: return samples = list(self._position_samples) self._position_samples.clear() self._position_sampler_drained += len(samples) # First captured position establishes the cutting cursor. if self.last_tool is None: self.last_tool = samples[0] samples = samples[1:] if not samples: return x0, y0, z0 = self.last_tool # sampler positions are already machine/joint coordinates. # Do not reconstruct machine coordinates by mixing several HAL reads. off_x = off_y = off_z = 0.0 for x, y, z in samples: if (x, y, z) == (x0, y0, z0): continue dx = x - x0 dy = y - y0 dz = z - z0 length = math.sqrt(dx*dx + dy*dy + dz*dz) step = 0.2 n = max(1, math.ceil(length / step)) self._perf_cut_segment_calls += 1 self._perf_cut_sample_count += n for i in range(1, n + 1): t = i / n sx = x0 + dx * t sy = y0 + dy * t sz = z0 + dz * t self._queue_cut_worker_position( sx, sy, sz ) x0, y0, z0 = x, y, z self.last_tool = (x0, y0, z0) def _stop_cut_worker(self): if self._cut_worker_conn is not None: try: self._cut_worker_conn.send(("stop",)) except Exception: pass if self._cut_worker is not None: try: self._cut_worker.join(timeout=0.5) except Exception: pass if self._cut_worker.is_alive(): try: self._cut_worker.terminate() self._cut_worker.join(timeout=0.2) except Exception: pass self._cut_worker = None self._cut_worker_conn = None self._cut_worker_pending = [] self._cut_worker_inflight = False def _start_cut_worker(self): if not CUT_WORKER or not DO_CUT or self.height is None: return False if not self.tool_profile: return False if self._cut_worker is not None and self._cut_worker.is_alive(): return True import array flat = array.array("d") for ix in range(self.RES_X + 1): flat.extend(self.height[ix]) ctx = mp.get_context("spawn") parent, child = ctx.Pipe() proc = ctx.Process(target=_mesh_cut_worker, args=(child,), daemon=True) proc.start() child.close() parent.send(( "init", self.RES_X, self.RES_Y, self.x1, self.y1, self.dx, self.dy, self.tile_size, self.tool_profile, flat.tobytes() )) self._cut_worker = proc self._cut_worker_conn = parent self._cut_worker_inflight = False self._cut_worker_pending = [] self._cut_worker_last_send = time.perf_counter() return True def _queue_cut_worker_position(self, x, y, z): if not self._start_cut_worker(): return False self._cut_worker_pending.append((float(x), float(y), float(z))) self._perf_worker_queue_peak = max( self._perf_worker_queue_peak, len(self._cut_worker_pending) ) return True def _poll_cut_worker(self): if self._cut_worker_conn is None: return _poll_t0 = time.perf_counter() while self._cut_worker_conn.poll(): try: msg = self._cut_worker_conn.recv() except (EOFError, OSError): self._stop_cut_worker() return if not msg: continue if msg[0] == "ready": continue if msg[0] != "result": continue self._cut_worker_inflight = False if len(msg) == 3 and msg[2] is None: continue ( _, token, ix0, ix1, iy0, iy1, hbytes, nbytes, grid_points, profile_calls, profile_time, normals_t0, worker_elapsed, batch_count ) = msg _apply_t0 = time.perf_counter() import array hvals = array.array("d") hvals.frombytes(hbytes) nvals = array.array("d") nvals.frombytes(nbytes) p = 0 for ix in range(ix0, ix1 + 1): for iy in range(iy0, iy1 + 1): self.height[ix][iy] = hvals[p] p += 1 p = 0 for ix in range(ix0, ix1 + 1): for iy in range(iy0, iy1 + 1): self.normal[ix][iy] = ( nvals[p], nvals[p + 1], nvals[p + 2] ) p += 3 tx0 = max(0, (ix0 - 1) // self.tile_size) tx1 = min((self.RES_X - 1) // self.tile_size, (ix1 + 1) // self.tile_size) ty0 = max(0, (iy0 - 1) // self.tile_size) ty1 = min((self.RES_Y - 1) // self.tile_size, (iy1 + 1) // self.tile_size) for ty in range(ty0, ty1 + 1): for tx in range(tx0, tx1 + 1): self.dirty_tiles.add((tx, ty)) # The synchronous cut_tool() also rebuilds the four stock walls # when the tool reaches a material boundary. The worker path # must do the same in the GUI process: height[] is updated by the # worker, but the OpenGL wall display lists are GUI-owned. # Without this, the top surface changes correctly while the side # walls visually remain at their original height. _wall_t0 = time.perf_counter() if ix0 <= 0: self.rebuild_left_wall() if ix1 >= self.RES_X: self.rebuild_right_wall() if iy0 <= 0: self.rebuild_front_wall() if iy1 >= self.RES_Y: self.rebuild_back_wall() self._perf_wall_time += time.perf_counter() - _wall_t0 self._perf_worker_batches += 1 self._perf_worker_batch_time += worker_elapsed self._perf_worker_points += grid_points self._perf_worker_profile += profile_calls self._perf_worker_normals_time += worker_elapsed - profile_time self._perf_worker_apply_time += time.perf_counter() - _apply_t0 self._cut_worker_last_result = time.perf_counter() self._perf_poll_time += time.perf_counter() - _poll_t0 def _dispatch_cut_worker(self): if self._cut_worker_conn is None or not self._cut_worker.is_alive(): return self._poll_cut_worker() if self._cut_worker_inflight: return if not self._cut_worker_pending: return now = time.perf_counter() interval = 1.0 / max(0.1, CUT_UPDATE_HZ) if now - self._cut_worker_last_send < interval: return batch = self._cut_worker_pending self._cut_worker_pending = [] self._cut_worker_token += 1 self._cut_worker_conn.send(("batch", self._cut_worker_token, batch)) self._cut_worker_inflight = True self._cut_worker_last_send = now def set_tool(self, tool): """Use the Vismach tool geometry as the cutting profile.""" self.tool_geometry = tool self.tool_profile = self.extract_tool_profile(tool) self.tool_max_radius = 0.0 for item in self.tool_profile: kind = item[0] if kind == "sphere": self.tool_max_radius = max(self.tool_max_radius, float(item[2])) elif kind == "cylinder": self.tool_max_radius = max(self.tool_max_radius, float(item[3])) elif kind == "frustum": self.tool_max_radius = max( self.tool_max_radius, float(item[3]), float(item[4]) ) print("=== MESH MATERIAL TOOL PROFILE (FROM VISMACH TOOL) ===") for item in self.tool_profile: print(" ", item) print(" max radius =", self.tool_max_radius) @staticmethod def extract_tool_profile(obj): """Extract the cutter profile directly from the Vismach tool tree. The original handler creates one real Vismach ``tool = Collection(...)`` and may then wrap it in Translate/Color/HalTranslate nodes. We walk through those wrappers and read only the actual Sphere/CylinderZ primitives. This makes the simulator use the same geometry that Vismach draws, without a second manually maintained TOOL_* profile. This helper lives in primitives.py so the machine handler only needs one short call and can stay close to the original Vismach handler. """ profile = [] visited = set() def walk(node): if node is None: return # Protect against an accidental cycle in a custom Vismach tree. ident = id(node) if ident in visited: return visited.add(ident) if isinstance(node, Sphere): try: x, y, z, r = node.coords() x = float(x) y = float(y) z = float(z) r = float(r) except Exception as exc: print("[TOOL PROFILE ] nie można odczytać Sphere:", repr(exc)) return # The cutter profile is axial. Do not silently turn a # sphere placed away from the tool axis into a wrong profile. if abs(x) > 1.0e-9 or abs(y) > 1.0e-9: print("[TOOL PROFILE ] pomijam Sphere poza osią:", x, y) return if r > 0.0: profile.append(("sphere", z, r)) return if isinstance(node, CylinderZ): try: z1, r1, z2, r2 = node.coords() z1 = float(z1) r1 = float(r1) z2 = float(z2) r2 = float(r2) except Exception as exc: print("[TOOL PROFILE ] nie można odczytać CylinderZ:", repr(exc)) return if max(abs(r1), abs(r2)) <= 0.0: return if abs(r1 - r2) < 1.0e-12: profile.append(("cylinder", z1, z2, r1)) else: profile.append(("frustum", z1, z2, r1, r2)) return # Collection and transformation/color/HAL wrapper nodes expose # their children through .parts. Recurse through all of them. parts = getattr(node, "parts", None) if parts is not None: try: for part in parts: walk(part) except TypeError: # Non-iterable .parts is not a cutter primitive. pass walk(obj) # Preserve the order used by the original Vismach Collection. # Do not sort or merge primitives: their order may be intentional. return profile def _tool_surface_z_at_radius(self, rho): """Return the lowest Z where the tool contains this radius.""" best_z = None for item in self.tool_profile: kind = item[0] if kind == "sphere": _, center_z, radius = item center_z = float(center_z) radius = float(radius) if rho <= radius: h = math.sqrt(max(0.0, radius * radius - rho * rho)) z = center_z - h if best_z is None or z < best_z: best_z = z elif kind == "cylinder": _, z1, z2, radius = item z1 = float(z1) z2 = float(z2) radius = float(radius) if rho <= radius: z = min(z1, z2) if best_z is None or z < best_z: best_z = z elif kind == "frustum": _, z1, z2, r1, r2 = item z1 = float(z1) z2 = float(z2) r1 = float(r1) r2 = float(r2) if z1 > z2: z1, z2 = z2, z1 r1, r2 = r2, r1 if rho > max(r1, r2): continue if rho <= r1: z = z1 elif r2 > r1 and rho <= r2: z = z1 + (rho - r1) * (z2 - z1) / (r2 - r1) else: continue if best_z is None or z < best_z: best_z = z return best_z def _motion_active(self): """Return True briefly after real tool motion. This is deliberately independent of material rebuilding: camera/view movement does not alter the material and therefore must not trigger cutting work. Tool motion does trigger the temporary sparse render. """ try: x = hal.get_value("halui.axis.x.pos-relative") y = hal.get_value("halui.axis.y.pos-relative") z = hal.get_value("halui.axis.z.pos-relative") except Exception: return False cur = (x, y, z) last = self._motion_last_tool self._motion_last_tool = cur if last is not None and cur != last: # Hold sparse rendering for a short tail so the expensive full # render cannot immediately interrupt a moving machine. self._motion_until = time.perf_counter() + 0.18 return True return time.perf_counter() < self._motion_until def _choose_grid_res_mm(self, lx, ly): """Choose cells/mm from material area, limited to 3..7. The logarithmic curve changes resolution smoothly instead of making abrupt jumps when the stock dimensions change. At the reference area (2000 mm^2, e.g. 40x50 mm) it gives 7 cells/mm; larger stock gradually moves toward the 3 cells/mm floor. """ if not self.GRID_RES_AUTO: return float(self.GRID_RES_MM) area = max(1.0, float(lx) * float(ly)) value = ( self.GRID_RES_MAX - self.GRID_RES_LOG_SLOPE * math.log10(area / self.GRID_RES_REF_AREA) ) return max(self.GRID_RES_MIN, min(self.GRID_RES_MAX, value)) def _choose_simple_step(self, quads): """Choose motion-render decimation from the number of surface quads.""" if not self.SIMPLE_STEP_AUTO: return int(self._simple_step) q = max(0, int(quads)) if q <= self.SIMPLE_STEP_Q1: return self.SIMPLE_STEP_MIN if q <= self.SIMPLE_STEP_Q2: return min(2, self.SIMPLE_STEP_MAX) return self.SIMPLE_STEP_MAX def _apply_auto_render_settings(self, lx, ly): """Calculate grid density and motion-render step once per mesh.""" res_mm = self._choose_grid_res_mm(lx, ly) res_x = max(1, int(round(lx * res_mm))) res_y = max(1, int(round(ly * res_mm))) quads = res_x * res_y simple_step = self._choose_simple_step(quads) old_step = self._simple_step self.GRID_RES_MM = res_mm self._auto_grid_res_mm = res_mm self._auto_simple_step = simple_step self._simple_step = simple_step # If an already-created sparse EBO used another step, discard only # those sparse index buffers. Full VBO/EBO geometry stays untouched. if old_step != simple_step and hasattr(self, "tile_simple_ebos"): for key, ebo in list(self.tile_simple_ebos.items()): try: GL.glDeleteBuffers(1, [ebo]) except Exception: pass self.tile_simple_ebos.clear() self.tile_simple_index_counts.clear() print( f"[AUTO GRID ] size={lx:.3f}x{ly:.3f} mm " f"area={lx*ly:.1f} mm2 res={res_mm:.3f}/mm " f"grid={res_x}x{res_y} quads={quads:,} " f"simple_step={simple_step}" ) return res_x, res_y def reset_material(self): """Public GUI entry point: rebuild the material from scratch.""" return self._reset_material_mesh() def _reset_material_mesh(self): """Completely reset the material mesh after a stock-size change. This is intentionally similar to starting with a fresh Vismach material. It removes GPU buffers/lists, stops the old cutting worker, clears captured motion and dirty state, and leaves the object ready for init_mesh() on the next draw(). A pure G54 translation is NOT routed here: only a change of physical stock dimensions calls this method. """ print("[MESH RESET ] stock dimensions changed -> rebuilding material") # The old worker contains a copy of the old height field. It must # never be allowed to return results into the new material. try: self._stop_cut_worker() except Exception: pass # Drop captured positions belonging to the previous stock/program. try: with self._position_samples_lock: self._position_samples.clear() except Exception: pass self._position_sampler_last = None self._position_sampler_drained = 0 self.last_tool = None # Delete surface GPU buffers. This is done from draw(), i.e. with # the OpenGL context active. for d in ( self.tile_vbos, self.tile_ebos, self.tile_simple_ebos, ): for buf in list(d.values()): if buf: try: GL.glDeleteBuffers(1, [buf]) except Exception: pass d.clear() self.tile_index_counts.clear() self.tile_vertex_counts.clear() self.tile_simple_index_counts.clear() # Delete the four wall display lists too; their geometry depends on # RES_X/RES_Y and the stock dimensions. for name in ( "left_list", "right_list", "front_list", "back_list", "bottom_list" ): list_id = getattr(self, name, None) if list_id: try: GL.glDeleteLists(list_id, 1) except Exception: pass setattr(self, name, None) # Clear all state which refers to the previous mesh. self.height = None self.normal = None self.mesh_list = None self.mesh_dirty = True self.dirty_tiles.clear() self._vbo_ready = False self._simple_ebo_ready = False self._force_full_once.clear() self._motion_full_tiles.clear() self._was_motion = False self.RES_X = 0 self.RES_Y = 0 self.RES = 0 self.nx = 0 self.ny = 0 self.dx = 0 self.dy = 0 self.last_mat_x1 = None self.last_mat_y1 = None self.last_mat_z2 = None self._last_stock_dims = None self._auto_grid_res_mm = None self._auto_simple_step = None # The next valid draw() will initialise a completely fresh mesh. print("[MESH RESET ] old mesh cleared; next draw will init fresh stock") def draw(self): t0 = time.perf_counter() self._perf_draw_calls += 1 if CUT_WORKER: self._start_position_sampler() self._poll_cut_worker() self._drain_position_samples() self._dispatch_cut_worker() x1, y1, z1, x2, y2, z2 = self.coords() if x1 > x2: tmp = x1 x1 = x2 x2 = tmp if y1 > y2: tmp = y1 y1 = y2 y2 = tmp if z1 > z2: tmp = z1 z1 = z2 z2 = tmp # a MeshMaterial3D can exist before an NGC/material is loaded. # In that state the HAL-linked coordinates may all collapse to zero # (or otherwise produce a zero-size stock). Do not build a 0x0 mesh: # update_normals() would then divide by dx/dy == 0. Simply skip the # material until valid stock dimensions appear; this also allows the # same Vismach session to start first and load NGC later. try: lx = abs(float(x2 - x1)) ly = abs(float(y2 - y1)) except Exception: return if (not math.isfinite(lx) or not math.isfinite(ly) or lx <= 1e-9 or ly <= 1e-9): # Keep the object uninitialised. When a real material is loaded, # the next draw() will come through this path again and initialise # the mesh normally. self.height = None self.normal = None return GL.glNormal3f(0,0,1) # resolution and motion decimation are derived from stock size. # X and Y are resolved independently, so the grid follows the stock # aspect ratio instead of forcing a square RES x RES grid. dims = (round(lx, 9), round(ly, 9)) # detect a genuinely new stock. G54 translations are excluded: # only physical dimensions are compared. Thus changing X0/Y0/Z0 # still moves the existing material, while loading an NGC with a # different stock size starts from a completely fresh height field. try: lz = abs(float(z2 - z1)) except Exception: lz = 0.0 stock_size = (round(lx, 9), round(ly, 9), round(lz, 9)) if (self._last_stock_size is not None and stock_size != self._last_stock_size): self._reset_material_mesh() self._last_stock_size = stock_size if self._last_stock_dims != dims or self.RES_X <= 0 or self.RES_Y <= 0: self.RES_X, self.RES_Y = self._apply_auto_render_settings(lx, ly) self.RES = max(self.RES_X, self.RES_Y) self._last_stock_dims = dims dx = lx / self.RES_X if self.RES_X > 0 else 1.0 dy = ly / self.RES_Y if self.RES_Y > 0 else 1.0 self.x1 = x1 self.y1 = y1 self.z2 = z2 self.dx = dx self.dy = dy # wykrycie zmiany położenia materiału w X/Y if self.last_mat_x1 is not None: dx_mat = x1 - self.last_mat_x1 dy_mat = y1 - self.last_mat_y1 dz_mat = z2 - self.last_mat_z2 if abs(dx_mat) > 1e-9 or abs(dy_mat) > 1e-9 or abs(dz_mat) > 1e-9: print( "ZMIANA POLOZENIA MATERIALU:", "dx =", dx_mat, "dy =", dy_mat, "dz =", dz_mat ) # zmiana Z -> przesuwamy całą powierzchnię if abs(dz_mat) > 1e-9: for ix in range(self.RES_X + 1): for iy in range(self.RES_Y + 1): self.height[ix][iy] += dz_mat # wszystkie kafelki muszą zostać przebudowane ntx = (self.RES_X + self.tile_size - 1) // self.tile_size nty = (self.RES_Y + self.tile_size - 1) // self.tile_size for ty in range(nty): for tx in range(ntx): self.dirty_tiles.add((tx, ty)) # wszystkie ściany również self.rebuild_left_wall() self.rebuild_right_wall() self.rebuild_front_wall() self.rebuild_back_wall() self.last_mat_x1 = x1 self.last_mat_y1 = y1 self.last_mat_z2 = z2 t1 = time.perf_counter() if self.height is None: self.init_mesh(z2) now = time.perf_counter() _dirty_t0 = time.perf_counter() dirty_now = len(self.dirty_tiles) self._perf_dirty_last = dirty_now self._perf_dirty_max_per_draw = max(self._perf_dirty_max_per_draw, dirty_now) if self.dirty_tiles: self._perf_dirty_peak = max(self._perf_dirty_peak, dirty_now) _dirty_rebuild_t0 = time.perf_counter() #print( # f"[DIRTY DIAG ] rebuild_start dirty={dirty_now} " # f"draw={self._perf_draw_calls} height_init={self.height is not None} " # f"tool={self.last_tool}" #) for tx, ty in self.dirty_tiles: self.rebuild_tile(tx, ty) self._perf_dirty_rebuilt += 1 _dirty_rebuild_ms = (time.perf_counter() - _dirty_rebuild_t0) * 1000.0 #print( # f"[DIRTY DIAG ] rebuild_done dirty={dirty_now} " # f"rebuilt={dirty_now} elapsed={_dirty_rebuild_ms:.1f} ms" #) self.dirty_tiles.clear() self._perf_dirty_time += time.perf_counter() - _dirty_t0 # Start timing for the VBO/EBO render section. # (no longer has the old display-list timing initializer.) _calllist_t0 = time.perf_counter() # render surface tiles directly from GPU VBO/EBO buffers. # No glCallList/glBegin/glVertex3f/glNormal3f calls are made here. _tile_t0 = time.perf_counter() v26_motion = self._motion_active() # Motion-session: start a fresh set for each real machine-motion session. # A tile touched by the cutter is kept at full resolution until # motion stops, instead of alternating full/sparse on every worker # update. That removes the brightness/geometry blinking visible on # steep walls. if v26_motion and not self._was_motion: self._motion_full_tiles.clear() elif not v26_motion and self._was_motion: self._motion_full_tiles.clear() self._was_motion = v26_motion if v26_motion: self._motion_frames += 1 else: self._static_frames += 1 if DRAW_STOCK and self.tile_vbos: GL.glEnableClientState(GL.GL_VERTEX_ARRAY) GL.glEnableClientState(GL.GL_NORMAL_ARRAY) try: for key, vbo in self.tile_vbos.items(): full_ebo = self.tile_ebos.get(key) if not full_ebo: continue # Sparse-render: clean tiles use sparse EBO during motion. Freshly # rebuilt tiles use the authoritative full EBO for this frame. force_full = key in self._force_full_once motion_full = key in self._motion_full_tiles use_simple = ( v26_motion and self._simple_step > 1 and not force_full and not motion_full ) if use_simple: tx, ty = key ix0 = tx * self.tile_size iy0 = ty * self.tile_size ix1 = min(ix0 + self.tile_size, self.RES_X) iy1 = min(iy0 + self.tile_size, self.RES_Y) nx = ix1 - ix0 + 1 ny = iy1 - iy0 + 1 self._ensure_simple_ebo(tx, ty, nx, ny) ebo = self.tile_simple_ebos.get(key) count = self.tile_simple_index_counts.get(key, 0) else: ebo = full_ebo count = self.tile_index_counts.get(key, 0) if not ebo or count <= 0: continue GL.glBindBuffer(GL.GL_ARRAY_BUFFER, vbo) GL.glVertexPointer(3, GL.GL_FLOAT, 24, ctypes.c_void_p(0)) GL.glNormalPointer(GL.GL_FLOAT, 24, ctypes.c_void_p(12)) GL.glBindBuffer(GL.GL_ELEMENT_ARRAY_BUFFER, ebo) GL.glDrawElements(GL.GL_QUADS, count, GL.GL_UNSIGNED_SHORT, ctypes.c_void_p(0)) self._perf_calllist_calls += 1 self._perf_calllist_draws += 1 self._perf_calllist_tile_calls += 1 self._perf_vbo_draw_calls += 1 if use_simple: self._simple_draw_calls += 1 self._simple_clean_draws += 1 else: self._full_draw_calls += 1 if force_full: self._dirty_full_draws += 1 if motion_full: self._motion_full_draws += 1 finally: GL.glBindBuffer(GL.GL_ELEMENT_ARRAY_BUFFER, 0) GL.glBindBuffer(GL.GL_ARRAY_BUFFER, 0) GL.glDisableClientState(GL.GL_NORMAL_ARRAY) GL.glDisableClientState(GL.GL_VERTEX_ARRAY) # Only this frame is forced full-resolution for freshly rebuilt tiles. # The next motion frame can use their newly generated sparse EBOs. self._force_full_once.clear() _tile_elapsed = time.perf_counter() - _tile_t0 if v26_motion: self._simple_draw_time += _tile_elapsed else: self._full_draw_time += _tile_elapsed self._perf_calllist_tile_time += _tile_elapsed _wall_t0 = time.perf_counter() if DRAW_STOCK: if self.left_list: GL.glCallList(self.left_list) self._perf_calllist_calls += 1 self._perf_calllist_draws += 1 self._perf_calllist_wall_calls += 1 if self.right_list: GL.glCallList(self.right_list) self._perf_calllist_calls += 1 self._perf_calllist_draws += 1 self._perf_calllist_wall_calls += 1 if self.front_list: GL.glCallList(self.front_list) self._perf_calllist_calls += 1 self._perf_calllist_draws += 1 self._perf_calllist_wall_calls += 1 if self.back_list: GL.glCallList(self.back_list) self._perf_calllist_calls += 1 self._perf_calllist_draws += 1 self._perf_calllist_wall_calls += 1 _wall_elapsed = time.perf_counter() - _wall_t0 self._perf_calllist_wall_time += _wall_elapsed _calllist_elapsed = time.perf_counter() - _calllist_t0 self._perf_calllist_time += _calllist_elapsed self._perf_calllist_last_ms = _calllist_elapsed * 1000.0 if self._perf_calllist_min_ms is None or self._perf_calllist_last_ms < self._perf_calllist_min_ms: self._perf_calllist_min_ms = self._perf_calllist_last_ms if self._perf_calllist_last_ms > self._perf_calllist_max_ms: self._perf_calllist_max_ms = self._perf_calllist_last_ms if RENDER_DIAG_GL_FINISH: _gf1 = time.perf_counter() GL.glFinish() self._perf_glfinish_after_time += time.perf_counter() - _gf1 t2 = time.perf_counter() self._perf_draw_time = getattr(self, "_perf_draw_time", 0.0) + (time.perf_counter() - t0) self._perf_report() def init_mesh(self, z2): # liczba punktów siatki self.nx = self.RES_X + 1 self.ny = self.RES_Y + 1 # wysokość każdego punktu self.height = [ [z2 for _ in range(self.ny)] for _ in range(self.nx) ] self.normal = [] for ix in range(self.RES_X + 1): row = [] for iy in range(self.RES_Y + 1): row.append((0.0, 0.0, 1.0)) self.normal.append(row) self.update_normals() ntx = (self.RES_X + self.tile_size - 1) // self.tile_size nty = (self.RES_Y + self.tile_size - 1) // self.tile_size print( f"[MESH INIT DIAG ] RES_X={self.RES_X} RES_Y={self.RES_Y} " f"grid_res={self.GRID_RES_MM:.3f}/mm tile_size={self.tile_size} " f"tiles={ntx}x{nty} total={ntx*nty}" ) for ty in range(nty): for tx in range(ntx): self.dirty_tiles.add((tx, ty)) self.rebuild_left_wall() self.rebuild_right_wall() self.rebuild_front_wall() self.rebuild_back_wall() print(f"Mesh: {self.nx} x {self.ny} vertices | cells: {self.RES_X} x {self.RES_Y} | spacing: {self.dx:.4f} x {self.dy:.4f} mm") def volume(self): x1, y1, z1, x2, y2, z2 = self.coords() vol = abs((x1 - x2) * (y1 - y2) * (z1 - z2)) # print "Box.volume", vol return vol def _perf_report(self): # Performance counters are kept internally for troubleshooting, but # normal operation does not print periodic diagnostic lines. self._perf_t0 = time.perf_counter() self._perf_draw_calls = 0 self._perf_draw_time = 0.0 self._perf_cut_segment_calls = 0 self._perf_cut_segment_time = 0.0 self._perf_cut_tool_calls = 0 self._perf_cut_tool_time = 0.0 self._perf_cut_sample_count = 0 self._perf_scan_time = 0.0 self._perf_grid_points = 0 self._perf_profile_calls = 0 self._perf_profile_time = 0.0 self._perf_normals_calls = 0 self._perf_normals_time = 0.0 self._perf_rebuild_calls = 0 self._perf_rebuild_time = 0.0 self._perf_build_mesh_calls = 0 self._perf_build_mesh_time = 0.0 self._perf_dirty_rebuilt = 0 self._perf_dirty_peak = 0 self._perf_calllist_min_ms = None self._perf_calllist_max_ms = 0.0 self._perf_calllist_draws = 0 self._perf_poll_time = 0.0 self._perf_worker_apply_time = 0.0 self._perf_wall_time = 0.0 self._perf_dirty_time = 0.0 self._perf_dirty_last = 0 self._perf_dirty_max_per_draw = 0 self._perf_vbo_upload_time = 0.0 self._perf_vbo_draw_calls = 0 def cut_tool_at(self, x, y, z): # halui.axis.*.pos-relative is in the active G54 work frame. # Convert it to the machine/world frame using: # # machine_position = relative_position + # (pos-feedback - pos-relative) # # This makes the material follow the actual G54 origin, regardless # of whether X0/Y0 are at the stock edge, centre, or elsewhere. try: mx = x + ( float(hal.get_value("halui.axis.x.pos-feedback")) - float(hal.get_value("halui.axis.x.pos-relative")) ) my = y + ( float(hal.get_value("halui.axis.y.pos-feedback")) - float(hal.get_value("halui.axis.y.pos-relative")) ) mz = z + ( float(hal.get_value("halui.axis.z.pos-feedback")) - float(hal.get_value("halui.axis.z.pos-relative")) ) except Exception: # Preserve the behaviour if machine-coordinate HAL pins are # unavailable. mx = x + float(hal.get_value("vismach.mat_X_min")) my = y + float(hal.get_value("vismach.mat_Y_min")) mz = z + float(hal.get_value("vismach.mat_Z_max")) self.cut_tool(mx, my, self.tool_radius, mz) def update_from_hal(self): """Compatibility wrapper; captures positions in a sampler thread.""" if CUT_WORKER: self._start_position_sampler() self._drain_position_samples() self._dispatch_cut_worker() else: # Preserve the old synchronous behaviour for CUT_WORKER=False. x = hal.get_value("halui.axis.x.pos-relative") y = hal.get_value("halui.axis.y.pos-relative") z = hal.get_value("halui.axis.z.pos-relative") if self.last_tool is None: self.last_tool = (x, y, z) return x0, y0, z0 = self.last_tool if DO_CUT and (x, y, z) != (x0, y0, z0): self.cut_segment(x0, y0, z0, x, y, z) self.last_tool = (x, y, z) def cut_segment(self, x0, y0, z0, x1, y1, z1): _t0 = time.perf_counter() self._perf_cut_segment_calls += 1 dx = x1 - x0 dy = y1 - y0 dz = z1 - z0 length = math.sqrt(dx*dx + dy*dy + dz*dz) step = 0.2 n = max(1, math.ceil(length / step)) self._perf_cut_sample_count += n for i in range(1, n + 1): t = i / n x = x0 + dx * t y = y0 + dy * t z = z0 + dz * t self.cut_tool_at(x, y, z) self._perf_cut_segment_time += time.perf_counter() - _t0 def cut_tool(self, cx, cy, radius, depth): _t0 = time.perf_counter() self._perf_cut_tool_calls += 1 # Jeśli podłączono narzędzie Vismach, tniemy jego rzeczywistym profilem. if self.tool_profile: R_body = self.tool_max_radius if R_body <= 0.0: return ix0 = max(0, floor((cx - R_body - self.x1) / self.dx)) ix1 = min(self.RES_X, floor((cx + R_body - self.x1) / self.dx) + 1) iy0 = max(0, floor((cy - R_body - self.y1) / self.dy)) iy1 = min(self.RES_Y, floor((cy + R_body - self.y1) / self.dy) + 1) _scan_t0 = time.perf_counter() for ix in range(ix0, ix1 + 1): for iy in range(iy0, iy1 + 1): self._perf_grid_points += 1 best_z = self.height[ix][iy] # Height[] stores values at grid vertices, so evaluate # the tool profile at the exact vertex coordinate. # The previous fractional-cell sampling evaluated the # tool at an offset position and then wrote that result # back to a grid vertex. This could make the cut appear # shifted/asymmetric relative to the visible Vismach tool. x = self.x1 + ix * self.dx y = self.y1 + iy * self.dy dx = x - cx dy = y - cy rho2 = dx * dx + dy * dy if rho2 <= R_body * R_body: rho = math.sqrt(rho2) _pt0 = time.perf_counter() tool_z_local = self._tool_surface_z_at_radius(rho) self._perf_profile_calls += 1 self._perf_profile_time += time.perf_counter() - _pt0 if tool_z_local is not None: tool_z = depth + tool_z_local if tool_z < best_z: best_z = tool_z self.height[ix][iy] = best_z self._perf_scan_time += time.perf_counter() - _scan_t0 self.update_normals( ix0 - 1, iy0 - 1, ix1 + 1, iy1 + 1 ) tx0 = ix0 // self.tile_size tx1 = ix1 // self.tile_size ty0 = iy0 // self.tile_size ty1 = iy1 // self.tile_size for ty in range(ty0, ty1 + 1): for tx in range(tx0, tx1 + 1): self.dirty_tiles.add((tx, ty)) if ix0 <= 0: self.rebuild_left_wall() if ix1 >= self.RES_X: self.rebuild_right_wall() if iy0 <= 0: self.rebuild_front_wall() if iy1 >= self.RES_Y: self.rebuild_back_wall() self._perf_cut_tool_time += time.perf_counter() - _t0 return # geometria części tnącej: # Sphere R1 # następnie przejście R1 -> R4 na długości 46 mm # # Z: # 0 ... 1 : sfera R1 # 1 ... 47 : rozszerzenie R1 -> R4 R_tip = radius R_body = 4.0 Z_tip = 1.0 Z_body = 47.0 ix0 = max(0, floor((cx - R_body - self.x1) / self.dx)) ix1 = min(self.RES_X, floor((cx + R_body - self.x1) / self.dx) + 1) iy0 = max(0, floor((cy - R_body - self.y1) / self.dy)) iy1 = min(self.RES_Y, floor((cy + R_body - self.y1) / self.dy) + 1) for ix in range(ix0, ix1 + 1): for iy in range(iy0, iy1 + 1): best_z = self.height[ix][iy] for sx in (0.25, 0.75): for sy in (0.25, 0.75): x = self.x1 + (ix + sx) * self.dx y = self.y1 + (iy + sy) * self.dy dx = x - cx dy = y - cy rho2 = dx * dx + dy * dy # poza promieniem R4 nic nie tnie if rho2 > R_body * R_body: continue rho = math.sqrt(rho2) # --------------------------------- # 1. końcówka kulista R1 # --------------------------------- if rho <= R_tip: h = math.sqrt( R_tip * R_tip - rho2 ) tool_z = depth + R_tip - h # --------------------------------- # 2. rozszerzająca się część # R1 -> R4 # --------------------------------- else: tool_z = depth + Z_tip + ( (rho - R_tip) * (Z_body - Z_tip) / (R_body - R_tip) ) if tool_z < best_z: best_z = tool_z self.height[ix][iy] = best_z self.update_normals( ix0 - 1, iy0 - 1, ix1 + 1, iy1 + 1 ) tx0 = ix0 // self.tile_size tx1 = ix1 // self.tile_size ty0 = iy0 // self.tile_size ty1 = iy1 // self.tile_size for ty in range(ty0, ty1 + 1): for tx in range(tx0, tx1 + 1): self.dirty_tiles.add((tx, ty)) if ix0 <= 0: self.rebuild_left_wall() if ix1 >= self.RES_X: self.rebuild_right_wall() if iy0 <= 0: self.rebuild_front_wall() if iy1 >= self.RES_Y: self.rebuild_back_wall() def cut_ball(self, cx, cy, radius, depth): ix0 = max(0, floor((cx - radius - self.x1) / self.dx)) ix1 = min(self.RES_X, floor((cx + radius - self.x1) / self.dx) + 1) iy0 = max(0, floor((cy - radius - self.y1) / self.dy)) iy1 = min(self.RES_Y, floor((cy + radius - self.y1) / self.dy) + 1) for ix in range(ix0, ix1 + 1): for iy in range(iy0, iy1 + 1): best_z = self.height[ix][iy] for sx in (0.25, 0.75): for sy in (0.25, 0.75): x = self.x1 + (ix + sx) * self.dx y = self.y1 + (iy + sy) * self.dy dx = x - cx dy = y - cy d2 = dx * dx + dy * dy if d2 > radius * radius: continue h = math.sqrt(radius * radius - d2) new_z = depth + radius - h if new_z < best_z: best_z = new_z self.height[ix][iy] = best_z self.update_normals( ix0 - 1, iy0 - 1, ix1 + 1, iy1 + 1 ) tx0 = max(0, (ix0 - 1) // self.tile_size) tx1 = min( (self.RES_X - 1) // self.tile_size, (ix1 + 1) // self.tile_size ) ty0 = max(0, (iy0 - 1) // self.tile_size) ty1 = min( (self.RES_Y - 1) // self.tile_size, (iy1 + 1) // self.tile_size ) for ty in range(ty0, ty1 + 1): for tx in range(tx0, tx1 + 1): self.dirty_tiles.add((tx, ty)) if ix0 <= 0: self.rebuild_left_wall() if ix1 >= self.RES_X: self.rebuild_right_wall() if iy0 <= 0: self.rebuild_front_wall() if iy1 >= self.RES_Y: self.rebuild_back_wall() #print(f"cut_ball {(time.perf_counter()-t0)*1000:.3f} ms") def cut_flat(self, cx, cy, radius, depth): ix0 = max(0, floor((cx - radius - self.x1) / self.dx)) ix1 = min(self.RES_X, floor((cx + radius - self.x1) / self.dx) + 1) iy0 = max(0, floor((cy - radius - self.y1) / self.dy)) iy1 = min(self.RES_Y, floor((cy + radius - self.y1) / self.dy) + 1) r2 = radius * radius for ix in range(ix0, ix1 + 1): for iy in range(iy0, iy1 + 1): best_z = self.height[ix][iy] # środek komórki x = self.x1 + (ix + 0.5) * self.dx y = self.y1 + (iy + 0.5) * self.dy dx = x - cx dy = y - cy if dx * dx + dy * dy <= r2: if depth < best_z: best_z = depth self.height[ix][iy] = best_z self.update_normals( ix0 - 1, iy0 - 1, ix1 + 1, iy1 + 1 ) tx0 = ix0 // self.tile_size tx1 = ix1 // self.tile_size ty0 = iy0 // self.tile_size ty1 = iy1 // self.tile_size for ty in range(ty0, ty1 + 1): for tx in range(tx0, tx1 + 1): self.dirty_tiles.add((tx, ty)) if ix0 <= 0: self.rebuild_left_wall() if ix1 >= self.RES_X: self.rebuild_right_wall() if iy0 <= 0: self.rebuild_front_wall() if iy1 >= self.RES_Y: self.rebuild_back_wall() def calc_normal(self, p1, p2, p4): ux = p2[0] - p1[0] uy = p2[1] - p1[1] uz = p2[2] - p1[2] vx = p4[0] - p1[0] vy = p4[1] - p1[1] vz = p4[2] - p1[2] nx = uy * vz - uz * vy ny = uz * vx - ux * vz nz = ux * vy - uy * vx l = math.sqrt(nx*nx + ny*ny + nz*nz) if l > 1e-12: nx /= l ny /= l nz /= l else: nx = 0 ny = 0 nz = 1 return nx, ny, nz def update_normals(self, ix0=0, iy0=0, ix1=None, iy1=None): _t0 = time.perf_counter() self._perf_normals_calls += 1 if ix1 is None: ix1 = self.RES_X if iy1 is None: iy1 = self.RES_Y ix0 = max(0, ix0) iy0 = max(0, iy0) ix1 = min(self.RES_X, ix1) iy1 = min(self.RES_Y, iy1) for ix in range(ix0, ix1 + 1): for iy in range(iy0, iy1 + 1): ixm = max(ix - 1, 0) ixp = min(ix + 1, self.RES_X) iym = max(iy - 1, 0) iyp = min(iy + 1, self.RES_Y) dzdx = (self.height[ixp][iy] - self.height[ixm][iy]) / ((ixp - ixm) * self.dx) dzdy = (self.height[ix][iyp] - self.height[ix][iym]) / ((iyp - iym) * self.dy) nx = -dzdx ny = -dzdy nz = 1.0 l = math.sqrt(nx*nx + ny*ny + nz*nz) if l > 1e-12: nx /= l ny /= l nz /= l else: nx = 0 ny = 0 nz = 1 self.normal[ix][iy] = (nx, ny, nz) self._perf_normals_time += time.perf_counter() - _t0 def _ensure_vbo(self): """Create the OpenGL VBO/EBO API objects lazily. LinuxCNC/Vismach normally runs with a compatibility OpenGL context, where ARB_vertex_buffer_object / OpenGL 1.5 is available. We keep this check explicit so a useful error is produced on an unusually old OpenGL implementation. """ if self._vbo_ready: return required = ( "glGenBuffers", "glBindBuffer", "glBufferData", "glDrawElements", "glEnableClientState", "glVertexPointer", "glNormalPointer" ) missing = [name for name in required if not hasattr(GL, name)] if missing: raise RuntimeError( "MeshMaterial3D wymaga VBO OpenGL; brak: " + ", ".join(missing) ) self._vbo_ready = True def _delete_tile_vbo(self, key): vbo = self.tile_vbos.pop(key, None) ebo = self.tile_ebos.pop(key, None) simple_ebo = self.tile_simple_ebos.pop(key, None) self.tile_index_counts.pop(key, None) self.tile_vertex_counts.pop(key, None) self.tile_simple_index_counts.pop(key, None) if vbo: try: GL.glDeleteBuffers(1, [vbo]) except Exception: pass if ebo: try: GL.glDeleteBuffers(1, [ebo]) except Exception: pass if simple_ebo: try: GL.glDeleteBuffers(1, [simple_ebo]) except Exception: pass def _ensure_simple_ebo(self, tx, ty, nx, ny): """Build a sparse EBO from the existing tile vertex VBO. No vertices or normals are regenerated. The sparse index buffer simply selects every second grid quad in both directions. """ key = (tx, ty) if key in self.tile_simple_ebos: return t0 = time.perf_counter() step = self._simple_step indices = array_type("H") for ly in range(0, ny - 1, step): row = ly * nx next_row = row + nx for lx in range(0, nx - 1, step): # Keep the exact same winding as the full-resolution EBO. i = row + lx r = min(lx + step, nx - 1) b = min(ly + step, ny - 1) br = b * nx + r bl = b * nx + lx indices.extend((i, row + r, br, bl)) ebo = GL.glGenBuffers(1) GL.glBindBuffer(GL.GL_ELEMENT_ARRAY_BUFFER, ebo) data = indices.tobytes() GL.glBufferData(GL.GL_ELEMENT_ARRAY_BUFFER, len(data), data, GL.GL_STATIC_DRAW) GL.glBindBuffer(GL.GL_ELEMENT_ARRAY_BUFFER, 0) self.tile_simple_ebos[key] = ebo self.tile_simple_index_counts[key] = len(indices) self._simple_builds += 1 self._simple_build_time += time.perf_counter() - t0 def rebuild_tile(self, tx, ty): """Rebuild one dirty surface tile into a VBO/EBO. Each tile uses local 16-bit indices. With tile_size=50 the largest tile has only 51*51 = 2601 vertices, so GL_UNSIGNED_SHORT is enough. This avoids duplicating the four corners of every quad in the GPU vertex buffer while keeping the exact RES=3200 surface resolution. """ _t0 = time.perf_counter() self._perf_rebuild_calls += 1 self._ensure_vbo() key = (tx, ty) ix0 = tx * self.tile_size iy0 = ty * self.tile_size ix1 = min(ix0 + self.tile_size, self.RES_X) iy1 = min(iy0 + self.tile_size, self.RES_Y) nx = ix1 - ix0 + 1 ny = iy1 - iy0 + 1 vertex_count = nx * ny if vertex_count > 65535: raise RuntimeError( f"MeshMaterial3D tile {(tx, ty)} ma {vertex_count} " "wierzchołków; zwiększenie tile_size przekracza limit uint16." ) # Interleaved: position.xyz + normal.xyz = 24 bytes / vertex. vertices = array_type("f") _t_loop0 = time.perf_counter() for ly in range(ny): iy = iy0 + ly y = self.y1 + iy * self.dy for lx in range(nx): ix = ix0 + lx x = self.x1 + ix * self.dx n = self.normal[ix][iy] vertices.extend(( float(x), float(y), float(self.height[ix][iy]), float(n[0]), float(n[1]), float(n[2]) )) indices = array_type("H") for ly in range(ny - 1): row = ly * nx next_row = row + nx for lx in range(nx - 1): i = row + lx # Same winding as the old GL_QUADS path: p1,p2,p3,p4. indices.extend((i, i + 1, next_row + lx + 1, next_row + lx)) self._perf_mesh_loop_time += time.perf_counter() - _t_loop0 self._perf_mesh_quads += (nx - 1) * (ny - 1) # Re-use the existing buffers when possible. Reallocating a tile's # storage is intentional: dirty tiles can change height/normal data. old_vbo = self.tile_vbos.get(key) old_ebo = self.tile_ebos.get(key) if old_vbo is None: old_vbo = GL.glGenBuffers(1) self.tile_vbos[key] = old_vbo if old_ebo is None: old_ebo = GL.glGenBuffers(1) self.tile_ebos[key] = old_ebo _t_upload0 = time.perf_counter() GL.glBindBuffer(GL.GL_ARRAY_BUFFER, old_vbo) vertex_bytes = vertices.tobytes() GL.glBufferData( GL.GL_ARRAY_BUFFER, len(vertex_bytes), vertex_bytes, GL.GL_STATIC_DRAW ) GL.glBindBuffer(GL.GL_ELEMENT_ARRAY_BUFFER, old_ebo) index_bytes = indices.tobytes() GL.glBufferData( GL.GL_ELEMENT_ARRAY_BUFFER, len(index_bytes), index_bytes, GL.GL_STATIC_DRAW ) GL.glBindBuffer(GL.GL_ARRAY_BUFFER, 0) GL.glBindBuffer(GL.GL_ELEMENT_ARRAY_BUFFER, 0) self.tile_index_counts[key] = len(indices) self.tile_vertex_counts[key] = vertex_count # Sparse-render: invalidate the sparse EBO after a fresh height/normal upload. old_simple_ebo = self.tile_simple_ebos.pop(key, None) self.tile_simple_index_counts.pop(key, None) if old_simple_ebo: try: GL.glDeleteBuffers(1, [old_simple_ebo]) except Exception: pass self._force_full_once.add(key) # Motion-session: remember this tile as one of the cutter-affected tiles for # the current motion session. It will remain full-res until stop. self._motion_full_tiles.add(key) _upload_elapsed = time.perf_counter() - _t_upload0 self._perf_vbo_upload_time += _upload_elapsed self._perf_mesh_finish_time += _upload_elapsed self._perf_rebuild_time += time.perf_counter() - _t0 def build_mesh(self, list_id, ix0, ix1, iy0, iy1): """Compatibility wrapper retained for callers from older versions. no longer uses display lists for the surface. The normal path calls rebuild_tile(), which knows the tile coordinates and creates the corresponding VBO/EBO. """ tx = int(ix0 // self.tile_size) ty = int(iy0 // self.tile_size) self.rebuild_tile(tx, ty) def rebuild_left_wall(self): if self.left_list is None: self.left_list = GL.glGenLists(1) x1, y1, z1, x2, y2, z2 = self.coords() if z1 > z2: z1, z2 = z2, z1 GL.glNewList(self.left_list, GL.GL_COMPILE) GL.glBegin(GL.GL_QUADS) GL.glNormal3f(-1, 0, 0) for iy in range(self.RES_Y): y = self.y1 + iy * self.dy GL.glVertex3f(x1, y, z1) GL.glVertex3f(x1, y + self.dy, z1) GL.glVertex3f( x1, y + self.dy, self.height[0][iy + 1] ) GL.glVertex3f( x1, y, self.height[0][iy] ) GL.glEnd() GL.glEndList() def rebuild_right_wall(self): if self.right_list is None: self.right_list = GL.glGenLists(1) x1, y1, z1, x2, y2, z2 = self.coords() if z1 > z2: z1, z2 = z2, z1 GL.glNewList(self.right_list, GL.GL_COMPILE) GL.glBegin(GL.GL_QUADS) GL.glNormal3f(1, 0, 0) for iy in range(self.RES_Y): y = self.y1 + iy * self.dy GL.glVertex3f(x2, y, self.height[self.RES_X][iy]) GL.glVertex3f(x2, y+self.dy, self.height[self.RES_X][iy+1]) GL.glVertex3f(x2, y+self.dy, z1) GL.glVertex3f(x2, y, z1) GL.glEnd() GL.glEndList() def rebuild_front_wall(self): if self.front_list is None: self.front_list = GL.glGenLists(1) x1, y1, z1, x2, y2, z2 = self.coords() if z1 > z2: z1, z2 = z2, z1 GL.glNewList(self.front_list, GL.GL_COMPILE) GL.glBegin(GL.GL_QUADS) GL.glNormal3f(0, -1, 0) for ix in range(self.RES_X): x = self.x1 + ix * self.dx GL.glVertex3f(x, y1, z1) GL.glVertex3f(x + self.dx, y1, z1) GL.glVertex3f( x + self.dx, y1, self.height[ix + 1][0] ) GL.glVertex3f( x, y1, self.height[ix][0] ) GL.glEnd() GL.glEndList() def rebuild_back_wall(self): if self.back_list is None: self.back_list = GL.glGenLists(1) x1, y1, z1, x2, y2, z2 = self.coords() if z1 > z2: z1, z2 = z2, z1 GL.glNewList(self.back_list, GL.GL_COMPILE) GL.glBegin(GL.GL_QUADS) GL.glNormal3f(0, 1, 0) for ix in range(self.RES_X): x = self.x1 + ix * self.dx GL.glVertex3f( x, y2, self.height[ix][self.RES_Y] ) GL.glVertex3f( x + self.dx, y2, self.height[ix + 1][self.RES_Y] ) GL.glVertex3f( x + self.dx, y2, z1 ) GL.glVertex3f( x, y2, z1 ) GL.glEnd() GL.glEndList() ####################################################################################################################### ####################################################################################################################### ####################################################################################################################### # specify the width in X and Y, and the height in Z # the box is centered on the origin class BoxCentered(Box): def __init__(self, xw, yw, zw): Box.__init__(self, -xw / 2.0, -yw / 2.0, -zw / 2.0, xw / 2.0, yw / 2.0, zw / 2.0) # specify the width in X and Y, and the height in Z # the box is centered in X and Y, and runs from Z=0 up # (or down) to the specified Z value class BoxCenteredXY(Box): def __init__(self, xw, yw, zw): Box.__init__(self, -xw / 2.0, -yw / 2.0, 0, xw / 2.0, yw / 2.0, zw) # capture current transformation matrix # note that this transforms from the current coordinate system # to the viewport system, NOT to the world system class Capture(object): def __init__(self): self.t = [] def capture(self): self.t = GL.glGetDoublev(GL.GL_MODELVIEW_MATRIX) def volume(self): return 0.0 # function to invert a transform matrix # based on http://steve.hollasch.net/cgindex/math/matrix/afforthinv.c # with simplifications since we don't do scaling # This function inverts a 4x4 matrix that is affine and orthogonal. In # other words, the perspective components are [0 0 0 1], and the basis # vectors are orthogonal to each other. In addition, the matrix must # not do scaling def invert(src): # make a copy inv = copy.deepcopy(src[:]) # The inverse of the upper 3x3 is the transpose (since the basis # vectors are orthogonal to each other. inv[0][1], inv[1][0] = inv[1][0], inv[0][1] inv[0][2], inv[2][0] = inv[2][0], inv[0][2] inv[1][2], inv[2][1] = inv[2][1], inv[1][2] # The inverse of the translation component is just the negation # of the translation after dotting with the new upper3x3 rows. */ inv[3][0] = -(src[3][0] * inv[0][0] + src[3][1] * inv[1][0] + src[3][2] * inv[2][0]) inv[3][1] = -(src[3][0] * inv[0][1] + src[3][1] * inv[1][1] + src[3][2] * inv[2][1]) inv[3][2] = -(src[3][0] * inv[0][2] + src[3][1] * inv[1][2] + src[3][2] * inv[2][2]) return inv class Hud(object): '''head up display - draws a semi-transparent text box. use HUD.strs for things that must be updated constantly, and HUD.show("stuff") for one-shot things like error messages''' def __init__(self, showme=1): self.app = [] self.strs = [] self.messages = [] self.showme = 0 self.fontbase = [] self._font = 'monospace bold 16' self._width = 9 self._height = 15 self._displayOnRight = False def display_on_left(self): self._displayOnRight = False def display_on_right(self): self._displayOnRight = True def show(self, string="xyzzy"): self.showme = 1 if string != "xyzzy": self.messages += [str(string)] def hide(self): self.showme = 0 def clear(self): self.messages = [] def draw(self): drawtext = self.strs + self.messages self.lines = len(drawtext) # draw head-up-display # see axis.py for more font/color configurability if ((self.showme == 0) or (self.lines == 0)): return GL.glMatrixMode(GL.GL_PROJECTION) GL.glPushMatrix() GL.glLoadIdentity() if not self.fontbase: self.fontbase, self._width, linespace = use_pango_font(self._font, 0, 128) xmargin, ymargin = 5, 5 ypos = float(self.app.winfo_height()) winWidth = int(self.app.winfo_width()) GL.glOrtho(0.0, winWidth, 0.0, ypos, -1.0, 1.0) GL.glMatrixMode(GL.GL_MODELVIEW) GL.glPushMatrix() GL.glLoadIdentity() # draw the text box maxlen = max([len(p) for p in drawtext]) box_width = maxlen * self._width GL.glDepthFunc(GL.GL_ALWAYS) GL.glDepthMask(GL.GL_FALSE) GL.glDisable(GL.GL_LIGHTING) GL.glEnable(GL.GL_BLEND) GL.glEnable(GL.GL_NORMALIZE) GL.glBlendFunc(GL.GL_ONE, GL.GL_CONSTANT_ALPHA) GL.glColor3f(0.2, 0, 0) GL.glBlendColor(0, 0, 0, 0.5) # rgba # left side or right side display if self._displayOnRight: offset = winWidth-box_width-xmargin-xmargin else: offset = 0 GL.glBegin(GL.GL_QUADS) GL.glVertex3f(offset, ypos, 1) # upper left GL.glVertex3f(offset, ypos - 2 * ymargin - self._height * len(drawtext), 1) # lower left GL.glVertex3f(offset+box_width + 2 * xmargin, ypos - 2 * ymargin - self._height * len(drawtext), 1) # lower right GL.glVertex3f(offset+box_width + 2 * xmargin, ypos, 1) # upper right GL.glEnd() GL.glDisable(GL.GL_BLEND) GL.glEnable(GL.GL_LIGHTING) # fill the box with text maxlen = 0 ypos -= self._height + ymargin i = 0 GL.glDisable(GL.GL_LIGHTING) GL.glColor3f(0.9, 0.9, 0.9) for string in drawtext: maxlen = max(maxlen, len(string)) # if i < len(homed) and homed[i]: # GL.glRasterPos2i(6, ypos) # GL.glBitmap(13, 16, 0, 3, 17, 0, homeicon) GL.glRasterPos2i(offset+xmargin, int(ypos)) for char in string: GL.glCallList(self.fontbase + ord(char)) # if i < len(homed) and limit[i]: # GL.glBitmap(13, 16, -5, 3, 17, 0, limiticon) ypos -= self._height i = i + 1 GL.glDepthFunc(GL.GL_LESS) GL.glDepthMask(GL.GL_TRUE) GL.glEnable(GL.GL_LIGHTING) GL.glPopMatrix() GL.glMatrixMode(GL.GL_PROJECTION) GL.glPopMatrix() GL.glMatrixMode(GL.GL_MODELVIEW) class HalHud(object): '''head up display - draws a semi-transparent text box. use HUD.strs for things that must be updated constantly, and HUD.show("stuff") for one-shot things like error messages''' def __init__(self): self.showme = 0 self._font = 'monospace bold 16' self.strs = [] self.messages = [] self.messages_top = [] self.fontbase = [] self.strings = [] self.formats = [] self.pinnames = [] self.background_color = (0,0.2,0,.5) self.text_color = (0.9,0.9,0.0) self.char_width = 13 self.char_height = 18 self._displayOnRight = False def display_on_left(self): self._displayOnRight = False def display_on_right(self): self._displayOnRight = True def add_pin(self, text,form,pinname): self.strings.append(str(text)) self.formats.append(form) self.pinnames.append(pinname) def show_top(self, string): self.showme = 1 self.messages_top += [str(string)] def show(self, string): self.showme = 1 self.messages += [str(string)] def hide(self): self.showme = 0 def clear(self): self.messages = [] # changes the hud color and transparency def set_background_color(self, r, g, b, a =.7): self.background_color = (r, g, b, 1-a) # changes the hud text color def set_text_color(self, r, g, b): self.text_color = (r, g, b) def set_char_width(self, width): self.char_width = width def set_char_height(self, height): self.char_height = height def set_font(self, font): self._font = font def draw(self): self.strs = [] # create the strings with the updated values using the corresponding list elements for n in range(len(self.strings)): try: value = hal.get_value( self.pinnames[n]) except: value = 0.0 self.strs += [self.strings[n] + str(self.formats[n].format(value))] drawtext = self.messages_top + self.strs + self.messages self.lines = len(drawtext) # draw head-up-display if ((self.showme == 0) or (self.lines == 0)): return GL.glMatrixMode(GL.GL_PROJECTION) GL.glPushMatrix() GL.glLoadIdentity() if not self.fontbase: self.fontbase, self._width, linespace = use_pango_font(self._font, 0, 128) xmargin,ymargin = 5,5 ypos = float(self.app.winfo_height()) winWidth = int(self.app.winfo_width()) GL.glOrtho(0.0, winWidth, 0.0, ypos, -1.0, 1.0) GL.glMatrixMode(GL.GL_MODELVIEW) GL.glPushMatrix() GL.glLoadIdentity() #draw the text box maxlen = max([len(p) for p in drawtext]) box_width = maxlen * self.char_width GL.glDepthFunc(GL.GL_ALWAYS) GL.glDepthMask(GL.GL_FALSE) GL.glDisable(GL.GL_LIGHTING) GL.glEnable(GL.GL_BLEND) GL.glEnable(GL.GL_NORMALIZE) GL.glBlendFunc(GL.GL_ONE, GL.GL_CONSTANT_ALPHA) color = self.background_color GL.glColor3f(color[0],color[1],color[2]) GL.glBlendColor(0,0,0,color[3]) #rgba, sets the transparency of the overlay using the 'a' value # left side or right side display if self._displayOnRight: offset = winWidth-box_width-xmargin-xmargin else: offset = 0 GL.glBegin(GL.GL_QUADS) GL.glVertex3f(offset, ypos, 1) #upper left GL.glVertex3f(offset, ypos - 2*ymargin - self.char_height*len(drawtext), 1) #lower left GL.glVertex3f(offset+box_width+2*xmargin, ypos - 2*ymargin - self.char_height*len(drawtext), 1) #lower right GL.glVertex3f(offset+box_width+2*xmargin, ypos , 1) #upper right GL.glEnd() GL.glDisable(GL.GL_BLEND) GL.glEnable(GL.GL_LIGHTING) #fill the box with text maxlen = 0 ypos -= self.char_height+ymargin i=0 GL.glDisable(GL.GL_LIGHTING) color = self.text_color GL.glColor3f(color[0],color[1],color[2]) for string in drawtext: maxlen = max(maxlen, len(string)) GL.glRasterPos2i(offset+xmargin, int(ypos)) for char in string: GL.glCallList(self.fontbase + ord(char)) ypos -= self.char_height i = i + 1 GL.glDepthFunc(GL.GL_LESS) GL.glDepthMask(GL.GL_TRUE) GL.glEnable(GL.GL_LIGHTING) GL.glPopMatrix() GL.glMatrixMode(GL.GL_PROJECTION) GL.glPopMatrix() GL.glMatrixMode(GL.GL_MODELVIEW) class Color(Collection): def __init__(self, color, parts): self.color = color Collection.__init__(self, parts) def apply(self): GL.glPushAttrib(GL.GL_LIGHTING_BIT) GL.glMaterialfv(GL.GL_FRONT_AND_BACK, GL.GL_AMBIENT_AND_DIFFUSE, self.color) def unapply(self): GL.glPopAttrib() # change the color of an object with a HAL U32 pin class HALColorRGB(Collection): def __init__(self, parts, comp, var, alpha=1.0): self.comp = comp self.var = var self.alpha = float(alpha) Collection.__init__(self, parts) def apply(self): try: if self.comp is None: v = int(hal.get_value(self.var)) else: v = int(self.comp[self.var]) except: v = 0 # split the u32 value into 3 (0-1.0) floats representing RGB r = ((v & 0x000000FF) >> 0)/255 g = ((v & 0x0000FF00) >> 8)/255 b = ((v & 0x00FF0000) >> 16)/255 # add preset alpha into the open GL color variable c = [r,g,b,self.alpha] GL.glPushAttrib(GL.GL_LIGHTING_BIT) GL.glMaterialfv(GL.GL_FRONT_AND_BACK, GL.GL_AMBIENT_AND_DIFFUSE, c) GL.glEnable(GL.GL_BLEND) GL.glBlendFunc(GL.GL_SRC_ALPHA, GL.GL_ONE_MINUS_SRC_ALPHA) def unapply(self): GL.glPopAttrib() GL.glDisable(GL.GL_BLEND) # change between two preset colors with a HAL Bit pin class HALColorFlip(Collection): def __init__(self, color1, color2, parts, comp, var): self.color1 = color1 self.color2 = color2 self.comp = comp self.var = var Collection.__init__(self, parts) def apply(self): try: if self.comp is None: v = bool(hal.get_value(self.var)) else: v = bool(self.comp[self.var]) except: v = False if v : c = self.color2 else: c = self.color1 GL.glPushAttrib(GL.GL_LIGHTING_BIT) GL.glMaterialfv(GL.GL_FRONT_AND_BACK, GL.GL_AMBIENT_AND_DIFFUSE, c) GL.glEnable(GL.GL_BLEND) GL.glBlendFunc(GL.GL_SRC_ALPHA, GL.GL_ONE_MINUS_SRC_ALPHA) def unapply(self): GL.glPopAttrib() GL.glDisable(GL.GL_BLEND) class AsciiSTL: def __init__(self, filename=None, data=None): if data is None: data = open(filename, "r") elif isinstance(data, str): data = data.split("\n") self.list = None t = [] n = [0, 0, 0] self.d = d = [] for line in data: if line.find("normal") != -1: line = line.split() x, y, z = list(map(float, line[-3:])) n = [x, y, z] elif line.find("vertex") != -1: line = line.split() x, y, z = list(map(float, line[-3:])) t.append([x, y, z]) if len(t) == 3: if n == [0, 0, 0]: dx1 = t[1][0] - t[0][0] dy1 = t[1][1] - t[0][1] dz1 = t[1][2] - t[0][2] dx2 = t[2][0] - t[0][0] dy2 = t[2][1] - t[0][1] dz2 = t[2][2] - t[0][2] n = [dy1 * dz2 - dy2 * dz1, dz1 * dx2 - dz2 * dx1, dy1 * dx2 - dy2 * dx1] d.append((n, t)) t = [] n = [0, 0, 0] def draw(self): if self.list is None: # OpenGL isn't ready yet in __init__ so the display list # is created during the first draw self.list = GL.glGenLists(1) GL.glNewList(self.list, GL.GL_COMPILE) GL.glBegin(GL.GL_TRIANGLES) for n, t in self.d: GL.glNormal3f(*n) GL.glVertex3f(*t[0]) GL.glVertex3f(*t[1]) GL.glVertex3f(*t[2]) GL.glEnd() GL.glEndList() del self.d GL.glCallList(self.list) class AsciiOBJ: def __init__(self, filename=None, data=None): if data is None: data = open(filename, "r") elif isinstance(data, str): data = data.split("\n") self.v = v = [] self.vn = vn = [] self.f = f = [] for line in data: if line.startswith("#"): continue if line.startswith("vn"): vn.append([float(w) for w in line.split()[1:]]) elif line.startswith("v"): v.append([float(w) for w in line.split()[1:]]) elif line.startswith("f"): f.append(self.parse_face(line)) # print v[:5] # print vn[:5] # print f[:5] self.list = None def parse_int(self, i): if i == '': return None return int(i) def parse_slash(self, word): return [self.parse_int(i) for i in word.split("/")] def parse_face(self, line): return [self.parse_slash(w) for w in line.split()[1:]] def draw(self): if self.list is None: # OpenGL isn't ready yet in __init__ so the display list # is created during the first draw self.list = GL.glGenLists(1) GL.glNewList(self.list, GL.GL_COMPILE) GL.glDisable(GL.GL_CULL_FACE) GL.glBegin(GL.GL_TRIANGLES) # print "obj", len(self.f) for f in self.f: for v, t, n in f: if n: GL.glNormal3f(*self.vn[n - 1]) GL.glVertex3f(*self.v[v - 1]) GL.glEnd() GL.glEndList() del self.v del self.vn del self.f GL.glCallList(self.list) ################################################################ # animated objects ################################################################ class HalTranslate(Collection): def __init__(self, parts, comp, var, x, y, z): self.parts = parts self.where = x, y, z self.comp = comp self.var = var def apply(self): x, y, z = self.where try: if self.comp is None: v = hal.get_value(self.var) else: v = self.comp[self.var] except: v = 0 GL.glPushMatrix() GL.glTranslatef(x * v, y * v, z * v) def unapply(self): GL.glPopMatrix() class HalRotate(Collection): def __init__(self, parts, comp, var, th, x, y, z): self.parts = parts self.where = th, x, y, z self.comp = comp self.var = var def apply(self): th, x, y, z = self.where GL.glPushMatrix() try: if self.comp is None: v = hal.get_value(self.var) else: v = self.comp[self.var] except: v = 0 angle = th * v # LIGHT TEST: # Keep the normal Vismach rotation for the object itself, but # temporarily disable lighting while the A rotation is applied. # Lighting is restored before the children are traversed. # # This is intentionally only a diagnostic test of the interaction # between HalRotate and OpenGL lighting. GL.glRotatef(angle, x, y, z) def unapply(self): GL.glPopMatrix() # updates tool cylinder shape. class HalToolCylinder(CylinderZ): METRIC = 1 IMPERIAL = 25.4 MODEL_SCALING = IMPERIAL def __init__(self, comp=None, *args): # get machine access so it can # change itself as it runs # specifically tool cylinder in this case. CylinderZ.__init__(self, *args) def coords(self): # get diameter and divide by 2 to get radius. try: dia = (hal.get_value('halui.tool.diameter') * self.MODEL_SCALING) except: dia = 0 rad = dia / 2 # change to rad # this instantly updates tool model but tooltip doesn't move till - # tooltip, the drawing point will NOT move till g43h(tool number) is called, however. # Tool will "crash" if h and tool length does not match. try: leng = hal.get_value('motion.tooloffset.z') * self.MODEL_SCALING except: leng = 0 # Update tool length when g43h(toolnumber) is called, otherwise stays at 0 or previous size. # commented out as I prefer machine to show actual tool size right away. # leng = self.comp["toollength"] return (-leng, rad, 0, rad) def set_tool_scale(self, scale): self.MODEL_SCALING = scale # we use a triangle for the tool # since we need to visualize a lathe tool here class HalToolTriangle(TriangleXZ): def __init__(self, comp=None, *args): # get machine access so it can # change itself as it runs # specifically tool cylinder in this case. TriangleXZ.__init__(self, *args) self.comp = comp def coords(self): try: leng = hal.get_value('motion.tooloffset.z') * self.MODEL_SCALING except: leng = 0 try: xleng = hal.get_value('motion.tooloffset.x') * self.MODEL_SCALING except: xleng = 0 x1 = leng z1 = -xleng x2 = 1#self.comp["tool-x-offset"]+10 z2 = -leng/2 x3 = 2#self.comp["tooldiameter"] z3 = 0 return (x1, z1, x2, z2, x3, z3, 0, 1)