diff --git a/barmesh/draw.gd b/barmesh/draw.gd index f249d51..4a6e20d 100644 --- a/barmesh/draw.gd +++ b/barmesh/draw.gd @@ -182,16 +182,13 @@ func _draw_barmesh(bm: BarMesh) -> void: if show_normals: var nim := ImmediateMesh.new() nim.surface_begin(Mesh.PRIMITIVE_LINES) - var tick: float = 0.003 for node in bm.nodes: var nd: BarMesh.BMNode = node if nd.contact_kind == BarMesh.BMNode.ContactFeature.NONE: continue - if nd.contact_normal.length_squared() < 1e-12: - continue - # Normals start at mesh contact; bars use cutter CL (fixed XY). + # Radius-length: mesh contact ↔ tool centre (CL). Length should be R when tangent. var p0: Vector3 = nd.contact_point - var p1: Vector3 = p0 + nd.contact_normal * tick + var p1: Vector3 = nd.p nim.surface_add_vertex(cad_to_godot(p0)) nim.surface_add_vertex(cad_to_godot(p1)) nim.surface_end() diff --git a/barmesh/tool_contact.gd b/barmesh/tool_contact.gd index 045d69a..6147558 100644 --- a/barmesh/tool_contact.gd +++ b/barmesh/tool_contact.gd @@ -160,22 +160,65 @@ static func _ball_edge_z(x: float, y: float, R: float, p0: Vector3, p1: Vector3) static func _ball_edge_hit(x: float, y: float, R: float, p0: Vector3, p1: Vector3) -> Dictionary: - 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: + ## True ball↔segment contact: (CL − pt) ⊥ edge and |CL − pt| = R. + ## (Old XY-only projection is wrong for tilted edges and causes fall-through.) + var dx := p1.x - p0.x + var dy := p1.y - p0.y + var dz := p1.z - p0.z + var L2 := dx * dx + dy * dy + dz * dz + if L2 < 1e-18: return {} - var t := ((x - p0.x) * ex + (y - p0.y) * ey) / len2 - t = clampf(t, 0.0, 1.0) - var pt := Vector3(p0.x + ex * t, p0.y + ey * t, p0.z + ez * t) - var dx: float = x - pt.x - var dy: float = y - pt.y - var d2: float = dx * dx + dy * dy - var R2: float = R * R - if d2 > R2: + var cx := x - p0.x + var cy := y - p0.y + var A := cx * dx + cy * dy + var H2 := dx * dx + dy * dy + var R2 := R * R + var best_z: float = -1e30 + var best_pt := Vector3.ZERO + var found := false + + if H2 < 1e-18: + # Nearly vertical edge: cylinder test in XY, z from segment extent. + var d2xy := cx * cx + cy * cy + if d2xy > R2: + return {} + var z_off := sqrt(R2 - d2xy) + for t in [0.0, 1.0]: + var pt_v := Vector3(p0.x + dx * t, p0.y + dy * t, p0.z + dz * t) + var z_cl: float = pt_v.z + z_off + if z_cl > best_z: + best_z = z_cl + best_pt = pt_v + found = true + else: + # Quadratic in cz = z - p0.z from |C-P|^2=R^2 and (C-P)·d=0. + var a := H2 + var b := -2.0 * A * dz + var c := L2 * (cx * cx + cy * cy) - A * A - R2 * L2 + var disc := b * b - 4.0 * a * c + if disc < 0.0: + return {} + var sdisc := sqrt(disc) + for sign in [-1.0, 1.0]: + var cz: float = (-b + sign * sdisc) / (2.0 * a) + var z: float = p0.z + cz + var t: float = (A + cz * dz) / L2 + if t < -1e-6 or t > 1.0 + 1e-6: + continue + t = clampf(t, 0.0, 1.0) + var pt := Vector3(p0.x + dx * t, p0.y + dy * t, p0.z + dz * t) + var cl := Vector3(x, y, z) + if absf(cl.distance_to(pt) - R) > 1e-3: + continue + if z > best_z: + best_z = z + best_pt = pt + found = true + + # Endpoints are also covered by vertex tests; keep segment-interior hits here. + if not found: return {} - return {"z": pt.z + sqrt(R2 - d2), "point": pt} + return {"z": best_z, "point": best_pt} static func _ball_face_z(x: float, y: float, R: float, a: Vector3, b: Vector3, c: Vector3) -> float: diff --git a/scenes/main.tscn b/scenes/main.tscn index 74c7bd3..667aceb 100644 --- a/scenes/main.tscn +++ b/scenes/main.tscn @@ -148,6 +148,10 @@ layout_mode = 2 button_pressed = true text = "Orthographic camera" +[node name="DebugFlatMesh" type="CheckBox" parent="ToolpathUI/Panel/VBox"] +layout_mode = 2 +text = "Debug flat mesh (2 tris)" + [node name="Bake" type="Button" parent="ToolpathUI/Panel/VBox"] layout_mode = 2 text = "Build tool surface" diff --git a/scripts/load_obj_mesh.gd b/scripts/load_obj_mesh.gd index f356580..5849acb 100644 --- a/scripts/load_obj_mesh.gd +++ b/scripts/load_obj_mesh.gd @@ -3,14 +3,44 @@ extends MeshInstance3D ## Adds collision for raycasts and applies flat (faceted) normals. ## Mesh is assigned in the editor. Units: Godot metres (STL mm × 0.001). +var _original_mesh: Mesh +var _debug_flat: bool = false + + func _ready() -> void: if mesh == null: push_error("Part mesh missing — assign meshes/eartip.obj in the editor") return + _original_mesh = mesh + _apply_flat_shading() + _rebuild_collision() + + +func set_debug_flat_mesh(enabled: bool) -> void: + ## Same XY/Z AABB as the part, but only two triangles (flat top of the bbox). + if mesh == null and _original_mesh == null: + return + if _original_mesh == null: + _original_mesh = mesh + _debug_flat = enabled + if enabled: + mesh = _make_two_triangle_bbox(_original_mesh) + else: + mesh = _original_mesh.duplicate() if _original_mesh else null _apply_flat_shading() + _rebuild_collision() + + +func is_debug_flat_mesh() -> bool: + return _debug_flat + + +func _rebuild_collision() -> void: for c in get_children(): if c is StaticBody3D: c.queue_free() + if mesh == null: + return var body := StaticBody3D.new() var col := CollisionShape3D.new() var shape := ConcavePolygonShape3D.new() @@ -19,8 +49,30 @@ func _ready() -> void: body.add_child(col) add_child(body) + +func _make_two_triangle_bbox(src: Mesh) -> ArrayMesh: + var aabb: AABB = src.get_aabb() + # Godot Y-up mesh space: flat plate at max Y (CAD Z-up max), covering XY footprint. + var x0 := aabb.position.x + var x1 := aabb.position.x + aabb.size.x + var z0 := aabb.position.z + var z1 := aabb.position.z + aabb.size.z + var y := aabb.position.y + aabb.size.y + var st := SurfaceTool.new() + st.begin(Mesh.PRIMITIVE_TRIANGLES) + # Two tris, CCW when viewed from above (+Y). + st.add_vertex(Vector3(x0, y, z0)) + st.add_vertex(Vector3(x1, y, z0)) + st.add_vertex(Vector3(x1, y, z1)) + st.add_vertex(Vector3(x0, y, z0)) + st.add_vertex(Vector3(x1, y, z1)) + st.add_vertex(Vector3(x0, y, z1)) + st.generate_normals() + return st.commit() + + func _apply_flat_shading() -> void: - if mesh.get_surface_count() < 1: + if mesh == null or mesh.get_surface_count() < 1: return var st := SurfaceTool.new() st.create_from(mesh, 0) diff --git a/scripts/toolpath_ui.gd b/scripts/toolpath_ui.gd index f91a8f3..d72928a 100644 --- a/scripts/toolpath_ui.gd +++ b/scripts/toolpath_ui.gd @@ -70,6 +70,16 @@ func _ready() -> void: if cam and cam.has_method("apply_orthogonal"): cam.apply_orthogonal(v) ) + var flat := $Panel/VBox/DebugFlatMesh as CheckBox + if flat: + flat.button_pressed = false + flat.toggled.connect(func(v: bool) -> void: + var part := get_parent().get_node_or_null("MeshRoot/Part") + if part and part.has_method("set_debug_flat_mesh"): + part.set_debug_flat_mesh(v) + # Rebuild lattice on the substituted mesh. + _on_bake() + ) func _preview() -> Node: diff --git a/tools/test_contact.gd b/tools/test_contact.gd index c799c77..d328ec8 100644 --- a/tools/test_contact.gd +++ b/tools/test_contact.gd @@ -93,5 +93,21 @@ func _run() -> void: _fail("tilt face contact XY unexpected equal to CL") return - print("CONTACT_OK face,vertex,edge,tilt_xy R=", R) + # Tilted edge: |CL−pt|=R and (CL−pt)⊥edge (XY-only drop was wrong here). + var e0 := Vector3(0, 0, 0) + var e1 := Vector3(0.02, 0, 0.01) + var skinny: Array = [[e0, e1, Vector3(0.01, 1e-4, 0.005)]] + var eh: Dictionary = Contact.drop_tool_contact(0.01, 0.0, R, skinny, -1.0) + _assert_cl_xy(eh, 0.01, 0.0) + var cl_e := Vector3(float(eh["x"]), float(eh["y"]), float(eh["z"])) + if absf(cl_e.distance_to(eh["point"]) - R) > 1e-4: + _fail("tilted edge |CL-pt| != R " + str(eh)) + return + var edir: Vector3 = (e1 - e0).normalized() + var radial: Vector3 = cl_e - eh["point"] + if absf(radial.dot(edir)) > 1e-3: + _fail("tilted edge not perpendicular " + str(radial.dot(edir))) + return + + print("CONTACT_OK face,vertex,edge,tilt_xy,tilt_edge R=", R) quit(0)