Add CAD Z-up BarMesh tool-contact visualization

README.md

@@ -34,6 +34,8 @@ its `@tool` script detects rewritten Curve3D/Animation files and reloads those
 two placeholders automatically. Select `ToolpathCurve` to see the Path3D
 editor gizmo. No game rerun or scene reopen is required.
 
+**BarMesh** (`barmesh/`): CAD **Z-up** `Node`/`Bar`/`BuildRectBarMesh`, ball-nose drop along −Z (`tool_contact.gd`). ImmediateMesh in `draw.gd` is the only Y-up conversion. Strategy **BarMesh viz**.
+
 **Units:** metres in-scene; Bake UI in mm. **Play:** MMB orbit, Shift+MMB pan, wheel zoom.
 
 ## Quick start

barmesh/README.md

@@ -0,0 +1,11 @@
+# BarMesh (CAD Z-up)
+
+Julian’s 3-axis tool-surface mesh. **All geometry here is Z-up** (CAD).
+
+| File | Role |
+|------|------|
+| `barmesh.gd` | `Node` / `Bar` / `BuildRectBarMesh` — cutter locations `(x, y, z)` |
+| `tool_contact.gd` | Ball-nose drop along **tool axis −Z** from a point above |
+| `draw.gd` | ImmediateMesh; **only** place that converts CAD Z-up → Godot Y-up `(x, z, y)` |
+
+Do not assign `Node.p` to a `Node3D.transform` without `draw.cad_to_godot`.

barmesh/barmesh.gd

@@ -0,0 +1,140 @@
+class_name BarMesh
+extends RefCounted
+## CAD Z-up BarMesh (vendor/barmesh/barmesh.py subset).
+## Node.p is (x, y, z) with Z up. Do not put these vectors on Node3D without
+## the draw conversion to Godot Y-up.
+
+class Partition1:
+	var lo: float
+	var hi: float
+	var nparts: int
+	var vs: PackedFloat32Array = PackedFloat32Array()
+
+	func _init(p_lo: float, p_hi: float, p_nparts: int) -> void:
+		lo = p_lo
+		hi = p_hi
+		nparts = p_nparts
+		vs.resize(p_nparts + 1)
+		for i in range(p_nparts + 1):
+			var lam := float(i) / float(p_nparts)
+			vs[i] = lo * (1.0 - lam) + hi * lam
+
+
+class BMNode:
+	## Tool pose in contact: cutter location in CAD XYZ (Z up).
+	var p: Vector3
+	var i: int
+	var pointzone = null
+
+	func _init(p_p: Vector3, p_i: int) -> void:
+		p = p_p
+		i = p_i
+
+
+class BMBar:
+	var nodeback: BMNode
+	var nodefore: BMNode
+	var barforeright: BMBar
+	var barbackleft: BMBar
+	var bbardeleted: bool = false
+	var barvecN: Vector3
+
+	func _init(p_back: BMNode, p_fore: BMNode) -> void:
+		assert(p_fore.i > p_back.i)
+		nodeback = p_back
+		nodefore = p_fore
+		barvecN = (p_fore.p - p_back.p).normalized()
+
+
+var nodes: Array = []
+var bars: Array = []
+var xlo: float = 0.0
+var xhi: float = 0.0
+var ylo: float = 0.0
+var yhi: float = 0.0
+var zlo: float = 0.0
+var zhi: float = 0.0
+
+var _xpart: Partition1
+var _ypart: Partition1
+var _z: float = 0.0
+var _nxs: int = 0
+var _xbars: Array = []
+var _ybars: Array = []
+var _next_y: int = 0
+
+
+func nxs() -> int:
+	return _nxs
+
+
+func new_node(p: Vector3) -> BMNode:
+	if nodes.is_empty():
+		xlo = p.x
+		xhi = p.x
+		ylo = p.y
+		yhi = p.y
+		zlo = p.z
+		zhi = p.z
+	else:
+		xlo = minf(xlo, p.x)
+		xhi = maxf(xhi, p.x)
+		ylo = minf(ylo, p.y)
+		yhi = maxf(yhi, p.y)
+		zlo = minf(zlo, p.z)
+		zhi = maxf(zhi, p.z)
+	var node := BMNode.new(p, nodes.size())
+	nodes.append(node)
+	return node
+
+
+func start_rect_bar_mesh(xpart: Partition1, ypart: Partition1, z: float) -> void:
+	nodes.clear()
+	bars.clear()
+	_xpart = xpart
+	_ypart = ypart
+	_z = z
+	_nxs = xpart.nparts + 1
+	_xbars.clear()
+	_ybars.clear()
+	_next_y = 0
+
+
+func add_next_rect_row() -> bool:
+	if _ypart == null or _next_y >= _ypart.vs.size():
+		return false
+	var y: float = _ypart.vs[_next_y]
+	var bfirstrow := nodes.is_empty()
+	for i in range(_nxs):
+		new_node(Vector3(_xpart.vs[i], y, _z))
+		if not bfirstrow:
+			var xbar := BMBar.new(nodes[-_nxs - 1], nodes[-1])
+			_xbars.append(xbar)
+			bars.append(xbar)
+			if i != 0:
+				xbar.barbackleft = _ybars[1 - _nxs]
+				_ybars[1 - _nxs].barbackleft = _xbars[-2]
+		if i != 0:
+			var ybar := BMBar.new(nodes[-2], nodes[-1])
+			_ybars.append(ybar)
+			bars.append(ybar)
+			if not bfirstrow:
+				ybar.barforeright = _xbars[-1]
+				_xbars[-2].barforeright = ybar
+	_next_y += 1
+	return _next_y < _ypart.vs.size()
+
+
+func build_rect_bar_mesh(xpart: Partition1, ypart: Partition1, z: float) -> void:
+	start_rect_bar_mesh(xpart, ypart, z)
+	while add_next_rect_row():
+		pass
+
+
+func live_bars() -> Array:
+	var out: Array = []
+	for bar in bars:
+		var b: BMBar = bar
+		if not b.bbardeleted:
+			out.append(b)
+	return out

barmesh/draw.gd

@@ -0,0 +1,83 @@
+extends MeshInstance3D
+## ImmediateMesh draw of CAD Z-up BarMesh. Only this file converts to Godot Y-up.
+
+const BarMeshGD = preload("res://barmesh/barmesh.gd")
+const Contact = preload("res://barmesh/tool_contact.gd")
+
+@export var part_path: NodePath = NodePath("../MeshRoot/Part")
+@export var nparts: int = 16
+@export var row_delay_s: float = 0.06
+@export var pad_m: float = 0.003
+@export var tool_radius_m: float = 0.0015
+
+var _playing: bool = false
+var _run_id: int = 0
+
+
+static func cad_to_godot(p: Vector3) -> Vector3:
+	## CAD Z-up (x, y, z) -> Godot Y-up (x, z, y).
+	return Vector3(p.x, p.z, p.y)
+
+
+func _ready() -> void:
+	var mat := StandardMaterial3D.new()
+	mat.shading_mode = BaseMaterial3D.SHADING_MODE_UNSHADED
+	mat.albedo_color = Color(0.95, 0.25, 0.72)
+	mat.emission_enabled = true
+	mat.emission = Color(0.7, 0.1, 0.5)
+	mat.emission_energy_multiplier = 1.8
+	material_override = mat
+	call_deferred("play_over_part")
+
+
+func play_over_part(p_radius: float = -1.0) -> void:
+	var part := get_node_or_null(part_path) as MeshInstance3D
+	if part == null or part.mesh == null:
+		push_warning("BarMesh draw: no part mesh")
+		return
+	var R: float = tool_radius_m if p_radius <= 0.0 else p_radius
+	_run_id += 1
+	var my_run: int = _run_id
+	_playing = true
+	var aabb: AABB = part.global_transform * part.get_aabb()
+	aabb = aabb.grow(pad_m)
+	var cad: Dictionary = Contact.cad_aabb_from_godot(aabb)
+	var tris: Array = Contact.mesh_triangles_cad(part)
+	var xpart := BarMeshGD.Partition1.new(cad["xmin"], cad["xmax"], nparts)
+	var ypart := BarMeshGD.Partition1.new(cad["ymin"], cad["ymax"], nparts)
+	var z_above: float = cad["zmax"] + R + pad_m
+	var z_plane: float = cad["zmin"]
+	var bm := BarMeshGD.new()
+	bm.start_rect_bar_mesh(xpart, ypart, z_above)
+	var more := true
+	while more:
+		if my_run != _run_id:
+			return
+		more = bm.add_next_rect_row()
+		_drop_last_row(bm, R, tris, z_plane)
+		_draw_barmesh(bm)
+		if row_delay_s > 0.0:
+			await get_tree().create_timer(row_delay_s).timeout
+	if my_run == _run_id:
+		_playing = false
+
+
+func _drop_last_row(bm: BarMesh, R: float, tris: Array, z_plane: float) -> void:
+	var n: int = bm.nxs()
+	if n <= 0:
+		return
+	var start: int = bm.nodes.size() - n
+	for i in range(n):
+		var node: BarMesh.BMNode = bm.nodes[start + i]
+		node.p.z = Contact.drop_tool_z(node.p.x, node.p.y, R, tris, z_plane)
+
+
+func _draw_barmesh(bm: BarMesh) -> void:
+	var im := ImmediateMesh.new()
+	im.surface_begin(Mesh.PRIMITIVE_LINES)
+	for bar in bm.live_bars():
+		var b: BarMesh.BMBar = bar
+		im.surface_add_vertex(cad_to_godot(b.nodeback.p))
+		im.surface_add_vertex(cad_to_godot(b.nodefore.p))
+	im.surface_end()
+	mesh = im

barmesh/tool_contact.gd

@@ -0,0 +1,132 @@
+class_name BarMeshToolContact
+extends RefCounted
+## Accurate 3-axis tool contact in CAD Z-up.
+## Tool axis is +Z. Drop along -Z from a point above (ball-nose cutter location).
+
+static func godot_world_to_cad(p: Vector3) -> Vector3:
+	## Godot Y-up (x, y, z) -> CAD Z-up (x, y_cad, z_up) = (x, z_godot, y_godot).
+	return Vector3(p.x, p.z, p.y)
+
+
+static func cad_aabb_from_godot(aabb: AABB) -> Dictionary:
+	var c0 := godot_world_to_cad(aabb.position)
+	var c1 := godot_world_to_cad(aabb.end)
+	return {
+		"xmin": minf(c0.x, c1.x),
+		"xmax": maxf(c0.x, c1.x),
+		"ymin": minf(c0.y, c1.y),
+		"ymax": maxf(c0.y, c1.y),
+		"zmin": minf(c0.z, c1.z),
+		"zmax": maxf(c0.z, c1.z),
+	}
+
+
+static func mesh_triangles_cad(mesh_inst: MeshInstance3D) -> Array:
+	var tris: Array = []
+	if mesh_inst == null or mesh_inst.mesh == null:
+		return tris
+	var xf := mesh_inst.global_transform
+	var mesh := mesh_inst.mesh
+	for s in mesh.get_surface_count():
+		var arr = mesh.surface_get_arrays(s)
+		if arr.size() <= Mesh.ARRAY_VERTEX or arr[Mesh.ARRAY_VERTEX] == null:
+			continue
+		var verts: PackedVector3Array = arr[Mesh.ARRAY_VERTEX]
+		var indices = arr[Mesh.ARRAY_INDEX] if arr.size() > Mesh.ARRAY_INDEX else null
+		if indices != null and indices.size() >= 3:
+			var idx: PackedInt32Array = indices
+			var i := 0
+			while i + 2 < idx.size():
+				tris.append([
+					godot_world_to_cad(xf * verts[idx[i]]),
+					godot_world_to_cad(xf * verts[idx[i + 1]]),
+					godot_world_to_cad(xf * verts[idx[i + 2]]),
+				])
+				i += 3
+		else:
+			var i2 := 0
+			while i2 + 2 < verts.size():
+				tris.append([
+					godot_world_to_cad(xf * verts[i2]),
+					godot_world_to_cad(xf * verts[i2 + 1]),
+					godot_world_to_cad(xf * verts[i2 + 2]),
+				])
+				i2 += 3
+	return tris
+
+
+static func drop_tool_z(x: float, y: float, radius: float, tris_cad: Array, fallback_z: float) -> float:
+	## Cutter-location Z (sphere centre) for a vertical ball-nose at CAD (x, y).
+	var z_best: float = -1e30
+	var found := false
+	var R2 := radius * radius
+	for tri in tris_cad:
+		var a: Vector3 = tri[0]
+		var b: Vector3 = tri[1]
+		var c: Vector3 = tri[2]
+		for vi in 3:
+			var v: Vector3 = tri[vi]
+			var dx: float = x - v.x
+			var dy: float = y - v.y
+			var d2: float = dx * dx + dy * dy
+			if d2 <= R2:
+				var z: float = v.z + sqrt(R2 - d2)
+				if z > z_best:
+					z_best = z
+					found = true
+		var edges: Array = [[a, b], [b, c], [c, a]]
+		for edge in edges:
+			var z_e: float = _ball_edge_z(x, y, radius, edge[0], edge[1])
+			if not is_nan(z_e) and z_e > z_best:
+				z_best = z_e
+				found = true
+		var z_f: float = _ball_face_z(x, y, radius, a, b, c)
+		if not is_nan(z_f) and z_f > z_best:
+			z_best = z_f
+			found = true
+	if found:
+		return z_best
+	return fallback_z
+
+
+static func _ball_edge_z(x: float, y: float, R: float, p0: Vector3, p1: Vector3) -> float:
+	var ex := p1.x - p0.x
+	var ey := p1.y - p0.y
+	var ez := p1.z - p0.z
+	var len2 := ex * ex + ey * ey
+	if len2 < 1e-18:
+		return NAN
+	var t := ((x - p0.x) * ex + (y - p0.y) * ey) / len2
+	t = clampf(t, 0.0, 1.0)
+	var cx: float = p0.x + ex * t
+	var cy: float = p0.y + ey * t
+	var cz: float = p0.z + ez * t
+	var dx: float = x - cx
+	var dy: float = y - cy
+	var d2: float = dx * dx + dy * dy
+	var R2: float = R * R
+	if d2 > R2:
+		return NAN
+	return cz + sqrt(R2 - d2)
+
+
+static func _ball_face_z(x: float, y: float, R: float, a: Vector3, b: Vector3, c: Vector3) -> float:
+	var den := (b.y - c.y) * (a.x - c.x) + (c.x - b.x) * (a.y - c.y)
+	if absf(den) < 1e-18:
+		return NAN
+	var w1 := ((b.y - c.y) * (x - c.x) + (c.x - b.x) * (y - c.y)) / den
+	var w2 := ((c.y - a.y) * (x - c.x) + (a.x - c.x) * (y - c.y)) / den
+	var w3 := 1.0 - w1 - w2
+	if w1 < 0.0 or w2 < 0.0 or w3 < 0.0:
+		return NAN
+	var n := (b - a).cross(c - a)
+	if absf(n.z) < 1e-10:
+		return NAN
+	var nlen := n.length()
+	if nlen < 1e-12:
+		return NAN
+	n /= nlen
+	var base := n.x * (x - a.x) + n.y * (y - a.y) - n.z * a.z
+	var z1: float = (R - base) / n.z
+	var z2: float = (-R - base) / n.z
+	return maxf(z1, z2)