Jerry
299 lines
10 KiB
Python
299 lines
10 KiB
Python
from __future__ import annotations
|
|
|
|
import math
|
|
from itertools import combinations
|
|
from pathlib import Path
|
|
from typing import Iterable
|
|
|
|
from ..cad.ocp_interference import (
|
|
InterferenceResult,
|
|
OcpInterferenceError,
|
|
ShapeMetrics,
|
|
_bbox_overlap,
|
|
_import_ocp,
|
|
common_volume,
|
|
read_step_shape,
|
|
shape_bbox,
|
|
shape_valid,
|
|
shape_volume,
|
|
)
|
|
from .models import BBox, ComponentPlacement, StepShapeMetrics
|
|
|
|
|
|
def _normalize(vector: Iterable[float]) -> tuple[float, float, float]:
|
|
values = tuple(float(value) for value in vector)
|
|
length = math.sqrt(sum(value * value for value in values))
|
|
if length <= 1e-12:
|
|
return (0.0, 0.0, 1.0)
|
|
return tuple(value / length for value in values) # type: ignore[return-value]
|
|
|
|
|
|
def dot(a: Iterable[float], b: Iterable[float]) -> float:
|
|
return sum(float(x) * float(y) for x, y in zip(a, b))
|
|
|
|
|
|
def cross(a: Iterable[float], b: Iterable[float]) -> tuple[float, float, float]:
|
|
ax, ay, az = (float(value) for value in a)
|
|
bx, by, bz = (float(value) for value in b)
|
|
return (ay * bz - az * by, az * bx - ax * bz, ax * by - ay * bx)
|
|
|
|
|
|
def vector_add(a: Iterable[float], b: Iterable[float]) -> tuple[float, float, float]:
|
|
return tuple(float(x) + float(y) for x, y in zip(a, b)) # type: ignore[return-value]
|
|
|
|
|
|
def vector_sub(a: Iterable[float], b: Iterable[float]) -> tuple[float, float, float]:
|
|
return tuple(float(x) - float(y) for x, y in zip(a, b)) # type: ignore[return-value]
|
|
|
|
|
|
def vector_scale(a: Iterable[float], scale: float) -> tuple[float, float, float]:
|
|
return tuple(float(x) * scale for x in a) # type: ignore[return-value]
|
|
|
|
|
|
def axis_angle_between(
|
|
source: Iterable[float],
|
|
target: Iterable[float],
|
|
) -> tuple[float, float, float, float]:
|
|
source_n = _normalize(source)
|
|
target_n = _normalize(target)
|
|
cross_value = cross(source_n, target_n)
|
|
cross_norm = math.sqrt(dot(cross_value, cross_value))
|
|
dot_value = max(-1.0, min(1.0, dot(source_n, target_n)))
|
|
if cross_norm <= 1e-12:
|
|
if dot_value >= 0.0:
|
|
return (0.0, 0.0, 1.0, 0.0)
|
|
fallback = cross(source_n, (1.0, 0.0, 0.0))
|
|
if math.sqrt(dot(fallback, fallback)) <= 1e-12:
|
|
fallback = cross(source_n, (0.0, 1.0, 0.0))
|
|
axis = _normalize(fallback)
|
|
return (*axis, 180.0)
|
|
axis = _normalize(cross_value)
|
|
return (*axis, math.degrees(math.atan2(cross_norm, dot_value)))
|
|
|
|
|
|
def rotate_vector(
|
|
vector: Iterable[float],
|
|
rotation_axis_angle_deg: Iterable[float],
|
|
) -> tuple[float, float, float]:
|
|
vx, vy, vz = (float(value) for value in vector)
|
|
ax, ay, az, angle_deg = (float(value) for value in rotation_axis_angle_deg)
|
|
axis = _normalize((ax, ay, az))
|
|
angle = math.radians(angle_deg)
|
|
cos_a = math.cos(angle)
|
|
sin_a = math.sin(angle)
|
|
ux, uy, uz = axis
|
|
cross_part = cross(axis, (vx, vy, vz))
|
|
dot_part = dot(axis, (vx, vy, vz))
|
|
return (
|
|
vx * cos_a + cross_part[0] * sin_a + ux * dot_part * (1.0 - cos_a),
|
|
vy * cos_a + cross_part[1] * sin_a + uy * dot_part * (1.0 - cos_a),
|
|
vz * cos_a + cross_part[2] * sin_a + uz * dot_part * (1.0 - cos_a),
|
|
)
|
|
|
|
|
|
def apply_placement_to_point(
|
|
point: Iterable[float],
|
|
placement: ComponentPlacement,
|
|
) -> tuple[float, float, float]:
|
|
rotated = rotate_vector(point, placement.rotation_axis_angle_deg)
|
|
return vector_add(rotated, placement.translation_mm)
|
|
|
|
|
|
def apply_placement_to_direction(
|
|
direction: Iterable[float],
|
|
placement: ComponentPlacement,
|
|
) -> tuple[float, float, float]:
|
|
return _normalize(rotate_vector(direction, placement.rotation_axis_angle_deg))
|
|
|
|
|
|
def bbox_axis_range(bbox: BBox, axis: Iterable[float]) -> tuple[float, float]:
|
|
xmin, ymin, zmin, xmax, ymax, zmax = bbox
|
|
axis_n = _normalize(axis)
|
|
values = []
|
|
for x in [xmin, xmax]:
|
|
for y in [ymin, ymax]:
|
|
for z in [zmin, zmax]:
|
|
values.append(dot((x, y, z), axis_n))
|
|
return (min(values), max(values))
|
|
|
|
|
|
def bbox_center(bbox: BBox) -> tuple[float, float, float]:
|
|
xmin, ymin, zmin, xmax, ymax, zmax = bbox
|
|
return ((xmin + xmax) / 2.0, (ymin + ymax) / 2.0, (zmin + zmax) / 2.0)
|
|
|
|
|
|
def union_bbox(bboxes: Iterable[BBox]) -> BBox:
|
|
items = list(bboxes)
|
|
if not items:
|
|
raise ValueError("union_bbox requires at least one bbox")
|
|
return (
|
|
min(item[0] for item in items),
|
|
min(item[1] for item in items),
|
|
min(item[2] for item in items),
|
|
max(item[3] for item in items),
|
|
max(item[4] for item in items),
|
|
max(item[5] for item in items),
|
|
)
|
|
|
|
|
|
def subshape_count(shape, kind_name: str) -> int:
|
|
ocp = _import_ocp()
|
|
kind = {
|
|
"solid": ocp["TopAbs_SOLID"],
|
|
"face": ocp["TopAbs_FACE"],
|
|
}[kind_name]
|
|
explorer = ocp["TopExp_Explorer"](shape, kind)
|
|
count = 0
|
|
while explorer.More():
|
|
count += 1
|
|
explorer.Next()
|
|
return count
|
|
|
|
|
|
def face_type_counts(shape) -> tuple[int, int]:
|
|
ocp = _import_ocp()
|
|
explorer = ocp["TopExp_Explorer"](shape, ocp["TopAbs_FACE"])
|
|
plane_count = 0
|
|
cylinder_count = 0
|
|
while explorer.More():
|
|
try:
|
|
face = ocp["TopoDS"].Face_s(explorer.Current())
|
|
surface = ocp["BRepAdaptor_Surface"](face)
|
|
surface_type = surface.GetType()
|
|
if surface_type == ocp["GeomAbs_Plane"]:
|
|
plane_count += 1
|
|
elif surface_type == ocp["GeomAbs_Cylinder"]:
|
|
cylinder_count += 1
|
|
except Exception: # noqa: BLE001
|
|
pass
|
|
explorer.Next()
|
|
return plane_count, cylinder_count
|
|
|
|
|
|
def metrics_for_shape(
|
|
*,
|
|
component_id: str,
|
|
step_path: Path,
|
|
shape,
|
|
) -> StepShapeMetrics:
|
|
plane_count, cylinder_count = face_type_counts(shape)
|
|
return StepShapeMetrics(
|
|
component_id=component_id,
|
|
step_path=str(step_path.resolve()),
|
|
valid=shape_valid(shape),
|
|
volume_mm3=shape_volume(shape),
|
|
bbox=shape_bbox(shape),
|
|
solid_count=subshape_count(shape, "solid"),
|
|
face_count=subshape_count(shape, "face"),
|
|
plane_face_count=plane_count,
|
|
cylinder_face_count=cylinder_count,
|
|
)
|
|
|
|
|
|
def metrics_for_step(component_id: str, path: Path) -> StepShapeMetrics:
|
|
shape = read_step_shape(path)
|
|
return metrics_for_shape(component_id=component_id, step_path=path, shape=shape)
|
|
|
|
|
|
def apply_transform(shape, placement: ComponentPlacement):
|
|
ocp = _import_ocp()
|
|
ax, ay, az, angle_deg = placement.rotation_axis_angle_deg
|
|
transformed = shape
|
|
if abs(angle_deg) > 1e-12:
|
|
rotation = ocp["gp_Trsf"]()
|
|
rotation.SetRotation(
|
|
ocp["gp_Ax1"](
|
|
ocp["gp_Pnt"](0.0, 0.0, 0.0),
|
|
ocp["gp_Dir"](float(ax), float(ay), float(az)),
|
|
),
|
|
math.radians(float(angle_deg)),
|
|
)
|
|
transformed = ocp["BRepBuilderAPI_Transform"](transformed, rotation, True).Shape()
|
|
tx, ty, tz = placement.translation_mm
|
|
if abs(tx) > 1e-12 or abs(ty) > 1e-12 or abs(tz) > 1e-12:
|
|
translation = ocp["gp_Trsf"]()
|
|
translation.SetTranslation(ocp["gp_Vec"](float(tx), float(ty), float(tz)))
|
|
transformed = ocp["BRepBuilderAPI_Transform"](transformed, translation, True).Shape()
|
|
return transformed
|
|
|
|
|
|
def write_compound_step(shapes: dict[str, object], path: Path) -> None:
|
|
if not shapes:
|
|
raise OcpInterferenceError("cannot_write_empty_joint_compound")
|
|
compound = compound_shape(shapes)
|
|
ocp = _import_ocp()
|
|
path.parent.mkdir(parents=True, exist_ok=True)
|
|
writer = ocp["STEPControl_Writer"]()
|
|
writer.Transfer(compound, ocp["STEPControl_AsIs"])
|
|
status = writer.Write(str(path))
|
|
if status != ocp["IFSelect_RetDone"]:
|
|
raise OcpInterferenceError(f"step_write_failed: {path}")
|
|
|
|
|
|
def compound_shape(shapes: dict[str, object]):
|
|
if not shapes:
|
|
raise OcpInterferenceError("cannot_build_empty_joint_compound")
|
|
ocp = _import_ocp()
|
|
compound = ocp["TopoDS_Compound"]()
|
|
builder = ocp["BRep_Builder"]()
|
|
builder.MakeCompound(compound)
|
|
for shape in shapes.values():
|
|
builder.Add(compound, shape)
|
|
return compound
|
|
|
|
|
|
def detect_interferences_from_placed_shapes(
|
|
shapes: dict[str, object],
|
|
*,
|
|
step_paths: dict[str, Path],
|
|
check_pairs: Iterable[tuple[str, str]] | None = None,
|
|
volume_tolerance_mm3: float = 1e-6,
|
|
bbox_tolerance_mm: float = 1e-7,
|
|
) -> tuple[list[ShapeMetrics], list[InterferenceResult]]:
|
|
if len(shapes) < 2:
|
|
raise OcpInterferenceError("need_at_least_two_placed_shapes")
|
|
metrics = [
|
|
ShapeMetrics(
|
|
component_id=component_id,
|
|
valid=shape_valid(shape),
|
|
volume_mm3=shape_volume(shape),
|
|
bbox=shape_bbox(shape),
|
|
)
|
|
for component_id, shape in shapes.items()
|
|
]
|
|
metric_by_id = {metric.component_id: metric for metric in metrics}
|
|
pairs = list(check_pairs) if check_pairs is not None else list(combinations(sorted(shapes), 2))
|
|
results: list[InterferenceResult] = []
|
|
for component_a, component_b in pairs:
|
|
if component_a not in shapes or component_b not in shapes:
|
|
raise OcpInterferenceError(f"unknown_interference_pair: {component_a}, {component_b}")
|
|
bbox_overlap = _bbox_overlap(
|
|
metric_by_id[component_a].bbox,
|
|
metric_by_id[component_b].bbox,
|
|
tolerance_mm=bbox_tolerance_mm,
|
|
)
|
|
if not bbox_overlap:
|
|
results.append(
|
|
InterferenceResult(
|
|
component_a=component_a,
|
|
component_b=component_b,
|
|
common_volume_mm3=0.0,
|
|
bbox_overlap=False,
|
|
checked_boolean=False,
|
|
interfering=False,
|
|
)
|
|
)
|
|
continue
|
|
volume = common_volume(shapes[component_a], shapes[component_b])
|
|
results.append(
|
|
InterferenceResult(
|
|
component_a=component_a,
|
|
component_b=component_b,
|
|
common_volume_mm3=volume,
|
|
bbox_overlap=True,
|
|
checked_boolean=True,
|
|
interfering=volume > volume_tolerance_mm3,
|
|
)
|
|
)
|
|
return metrics, results
|