1344 lines
49 KiB
Python
1344 lines
49 KiB
Python
from __future__ import annotations
|
|
|
|
import json
|
|
import math
|
|
import re
|
|
from collections import deque
|
|
from dataclasses import dataclass, field
|
|
from pathlib import Path
|
|
from typing import Any
|
|
|
|
try:
|
|
from unfolding import MAX_LINES, UnfoldingDescription, UnfoldingLineSummary, clockwise_sort, order_points_clockwise
|
|
except ImportError: # pragma: no cover - supports package imports in tests/tools.
|
|
from .unfolding import MAX_LINES, UnfoldingDescription, UnfoldingLineSummary, clockwise_sort, order_points_clockwise
|
|
|
|
MIN_NEIGHBOR_DISTANCE_MM = 34.0
|
|
MAX_NEIGHBOR_TOLERANCE_MM = 2.0
|
|
METER_LIKE_MAX_SPAN = 10.0
|
|
|
|
|
|
@dataclass
|
|
class LedPoint:
|
|
uid: int
|
|
source_id: int
|
|
x: float
|
|
y: float
|
|
row: int
|
|
col: int
|
|
line_id: int | None = None
|
|
block_id: int | None = None
|
|
line_index: int | None = None
|
|
block_index: int | None = None
|
|
coordinate_unit: str = "auto"
|
|
|
|
|
|
@dataclass
|
|
class LedBlock:
|
|
id: int
|
|
name: str
|
|
point_ids: list[int] = field(default_factory=list)
|
|
|
|
|
|
@dataclass
|
|
class LedLine:
|
|
id: int
|
|
name: str
|
|
color: str
|
|
blocks: list[LedBlock] = field(default_factory=list)
|
|
|
|
|
|
class MappingDocument:
|
|
def __init__(self) -> None:
|
|
self.path: Path | None = None
|
|
self.collection_name = "circles"
|
|
self.points: list[LedPoint] = []
|
|
self.lines: list[LedLine] = []
|
|
self.unfolding = UnfoldingDescription()
|
|
self.coordinate_unit = "auto"
|
|
self._next_line_id = 1
|
|
self._next_block_id = 1
|
|
|
|
def clear(self) -> None:
|
|
self.path = None
|
|
self.collection_name = "circles"
|
|
self.points.clear()
|
|
self.lines.clear()
|
|
self.unfolding = UnfoldingDescription()
|
|
self.coordinate_unit = "auto"
|
|
self._next_line_id = 1
|
|
self._next_block_id = 1
|
|
|
|
def load(self, path: str | Path) -> None:
|
|
self.clear()
|
|
self.path = Path(path)
|
|
data = json.loads(self.path.read_text(encoding="utf-8-sig"))
|
|
self.coordinate_unit = str(data.get("coordinate_unit", data.get("unit", "auto")))
|
|
points_data = data.get("leds")
|
|
self.collection_name = "leds"
|
|
if not isinstance(points_data, list):
|
|
points_data = data.get("circles")
|
|
self.collection_name = "circles"
|
|
if not isinstance(points_data, list):
|
|
raise ValueError("JSON must contain a 'circles' or 'leds' array.")
|
|
|
|
for index, item in enumerate(points_data):
|
|
self.points.append(
|
|
LedPoint(
|
|
uid=index,
|
|
source_id=int(item.get("source_id", item.get("id", index))),
|
|
x=float(item["x"]),
|
|
y=float(item["y"]),
|
|
row=_optional_int(item.get("row")) if item.get("row") is not None else index * 2,
|
|
col=(
|
|
_optional_int(item.get("col"))
|
|
if item.get("col") is not None
|
|
else _optional_int(item.get("column")) if item.get("column") is not None else 0
|
|
),
|
|
line_id=_optional_int(item.get("line_id")),
|
|
block_id=_optional_int(item.get("block_id")),
|
|
line_index=_optional_int(item.get("line_index")),
|
|
block_index=_optional_int(item.get("block_index")),
|
|
coordinate_unit=self.coordinate_unit,
|
|
)
|
|
)
|
|
|
|
self._load_lines(data)
|
|
self.unfolding = UnfoldingDescription.from_payload(data.get("unfolding"))
|
|
self.unfolding.max_lines = MAX_LINES
|
|
if not self.lines and any(point.line_id is not None for point in self.points):
|
|
self._rebuild_lines_from_points()
|
|
self._deduplicate_block_memberships()
|
|
self._validate_line_limit()
|
|
self.renumber_lines()
|
|
self.reindex_all()
|
|
|
|
def _load_lines(self, data: dict[str, Any]) -> None:
|
|
lines_data = data.get("lines", [])
|
|
if not isinstance(lines_data, list):
|
|
return
|
|
source_to_uid = {point.source_id: point.uid for point in self.points}
|
|
for line_item in lines_data:
|
|
line_id = int(line_item.get("id", self._next_line_id))
|
|
line = LedLine(
|
|
id=line_id,
|
|
name=str(line_item.get("name", f"Line {line_id}")),
|
|
color=str(line_item.get("color", default_color(line_id))),
|
|
)
|
|
for block_item in line_item.get("blocks", []):
|
|
block_id = int(block_item.get("id", self._next_block_id))
|
|
line.blocks.append(
|
|
LedBlock(
|
|
id=block_id,
|
|
name=str(block_item.get("name", f"Block {block_id}")),
|
|
point_ids=self._normalize_loaded_point_ids(
|
|
block_item.get("point_ids", []),
|
|
source_to_uid,
|
|
str(line_item.get("point_id_mode", block_item.get("point_id_mode", "auto"))),
|
|
),
|
|
)
|
|
)
|
|
self._next_block_id = max(self._next_block_id, block_id + 1)
|
|
self.lines.append(line)
|
|
self._next_line_id = max(self._next_line_id, line_id + 1)
|
|
|
|
def _rebuild_lines_from_points(self) -> None:
|
|
for line_id in sorted({point.line_id for point in self.points if point.line_id is not None}):
|
|
line = LedLine(int(line_id), f"Line {line_id}", default_color(int(line_id)))
|
|
block_ids = sorted({
|
|
point.block_id
|
|
for point in self.points
|
|
if point.line_id == line_id and point.block_id is not None
|
|
})
|
|
if not block_ids:
|
|
ids = [
|
|
point.uid
|
|
for point in sorted(
|
|
self.points,
|
|
key=lambda point: (
|
|
point.line_index if point.line_index is not None else 10**9,
|
|
point.source_id,
|
|
),
|
|
)
|
|
if point.line_id == line_id
|
|
]
|
|
if ids:
|
|
line.blocks.append(LedBlock(self._next_block_id, "Block 1", ids))
|
|
self._next_block_id += 1
|
|
for block_id in block_ids:
|
|
ids = [
|
|
point.uid
|
|
for point in sorted(
|
|
self.points,
|
|
key=lambda point: (
|
|
point.block_index if point.block_index is not None else 10**9,
|
|
point.source_id,
|
|
),
|
|
)
|
|
if point.line_id == line_id and point.block_id == block_id
|
|
]
|
|
line.blocks.append(LedBlock(int(block_id), f"Block {block_id}", ids))
|
|
self._next_block_id = max(self._next_block_id, int(block_id) + 1)
|
|
self.lines.append(line)
|
|
self._next_line_id = max(self._next_line_id, int(line_id) + 1)
|
|
|
|
def save(self, path: str | Path) -> None:
|
|
path = Path(path)
|
|
self.renumber_lines()
|
|
self.reindex_all()
|
|
point_payload = []
|
|
for point in self.points:
|
|
point_payload.append(
|
|
{
|
|
"id": point.source_id,
|
|
"x": point.x,
|
|
"y": point.y,
|
|
"row": point.row,
|
|
"col": point.col,
|
|
"line_id": point.line_id,
|
|
"block_id": point.block_id,
|
|
"line_index": point.line_index,
|
|
"block_index": point.block_index,
|
|
}
|
|
)
|
|
|
|
payload = {
|
|
self.collection_name: point_payload,
|
|
"unfolding": self.describe_unfolding().to_payload(),
|
|
"coordinate_unit": self._coordinate_unit_for_save(),
|
|
"lines": [
|
|
{
|
|
"id": line.id,
|
|
"name": line.name,
|
|
"color": line.color,
|
|
"blocks": [
|
|
{"id": block.id, "name": block.name, "point_ids": block.point_ids}
|
|
for block in line.blocks
|
|
],
|
|
"point_id_mode": "uid",
|
|
}
|
|
for line in self.lines
|
|
],
|
|
}
|
|
path.write_text(json.dumps(payload, ensure_ascii=False, indent=2), encoding="utf-8")
|
|
self.path = path
|
|
|
|
def add_line(self) -> LedLine:
|
|
line_limit = self._line_limit()
|
|
if len(self.lines) >= line_limit:
|
|
raise ValueError(f"Mapping supports no more than {line_limit} lines.")
|
|
line = LedLine(self._next_line_id, f"Линия {self._next_line_id}", default_color(self._next_line_id))
|
|
self._next_line_id += 1
|
|
self.lines.append(line)
|
|
return line
|
|
|
|
def add_block(self, line_id: int, point_ids: list[int]) -> LedBlock:
|
|
line = self.get_line(line_id)
|
|
clean_point_ids = unique_in_order([
|
|
point_id for point_id in point_ids if 0 <= point_id < len(self.points)
|
|
])
|
|
self._detach_points_from_blocks(clean_point_ids)
|
|
block = LedBlock(self._next_block_id, self._next_block_name(line), clean_point_ids)
|
|
self._next_block_id += 1
|
|
line.blocks.append(block)
|
|
for point_id in block.point_ids:
|
|
point = self.points[point_id]
|
|
point.line_id = line.id
|
|
point.block_id = block.id
|
|
self.reindex_all()
|
|
return block
|
|
|
|
def append_points_to_block(self, line_id: int, block_id: int, point_ids: list[int]) -> int:
|
|
block = self._get_block(line_id, block_id)
|
|
existing = set(block.point_ids)
|
|
clean_point_ids = unique_in_order([
|
|
point_id
|
|
for point_id in point_ids
|
|
if 0 <= point_id < len(self.points) and point_id not in existing
|
|
])
|
|
if not clean_point_ids:
|
|
return 0
|
|
self._detach_points_from_blocks(clean_point_ids)
|
|
block = self._get_block(line_id, block_id)
|
|
block.point_ids.extend(clean_point_ids)
|
|
self.reindex_all()
|
|
return len(clean_point_ids)
|
|
|
|
def get_line(self, line_id: int) -> LedLine:
|
|
for line in self.lines:
|
|
if line.id == line_id:
|
|
return line
|
|
raise KeyError(f"Unknown line id: {line_id}")
|
|
|
|
def get_point(self, point_id: int) -> LedPoint:
|
|
return self.points[point_id]
|
|
|
|
def _get_block(self, line_id: int, block_id: int) -> LedBlock:
|
|
line = self.get_line(line_id)
|
|
for block in line.blocks:
|
|
if block.id == block_id:
|
|
return block
|
|
raise KeyError(block_id)
|
|
|
|
def clear_point_assignments(self, point_ids: list[int]) -> int:
|
|
point_id_set = set(point_ids)
|
|
removed = 0
|
|
for line in self.lines:
|
|
for block in line.blocks:
|
|
before = len(block.point_ids)
|
|
block.point_ids = [point_id for point_id in block.point_ids if point_id not in point_id_set]
|
|
removed += before - len(block.point_ids)
|
|
|
|
for point_id in point_id_set:
|
|
point = self.points[point_id]
|
|
if point.line_id is not None or point.block_id is not None:
|
|
point.line_id = None
|
|
point.block_id = None
|
|
point.line_index = None
|
|
point.block_index = None
|
|
self.reindex_all()
|
|
return removed
|
|
|
|
def remove_block(self, line_id: int, block_id: int) -> int:
|
|
line = self.get_line(line_id)
|
|
removed_ids: list[int] = []
|
|
kept_blocks: list[LedBlock] = []
|
|
for block in line.blocks:
|
|
if block.id == block_id:
|
|
removed_ids.extend(block.point_ids)
|
|
else:
|
|
kept_blocks.append(block)
|
|
line.blocks = kept_blocks
|
|
self._clear_points(removed_ids)
|
|
self.reindex_all()
|
|
return len(removed_ids)
|
|
|
|
def remove_line(self, line_id: int) -> int:
|
|
removed_ids: list[int] = []
|
|
kept_lines: list[LedLine] = []
|
|
for line in self.lines:
|
|
if line.id == line_id:
|
|
for block in line.blocks:
|
|
removed_ids.extend(block.point_ids)
|
|
else:
|
|
kept_lines.append(line)
|
|
self.lines = kept_lines
|
|
self._clear_points(removed_ids)
|
|
self.renumber_lines()
|
|
self.reindex_all()
|
|
return len(removed_ids)
|
|
|
|
def clear_all_assignments(self) -> int:
|
|
assigned = sum(1 for point in self.points if point.line_id is not None or point.block_id is not None)
|
|
self.lines.clear()
|
|
self._clear_points([point.uid for point in self.points])
|
|
self._next_line_id = 1
|
|
self._next_block_id = 1
|
|
return assigned
|
|
|
|
def renumber_lines(self) -> None:
|
|
self.order_lines_clockwise()
|
|
id_map: dict[int, int] = {}
|
|
for new_id, line in enumerate(self.lines, start=1):
|
|
old_id = line.id
|
|
id_map[old_id] = new_id
|
|
line.id = new_id
|
|
line.name = f"Линия {new_id}"
|
|
line.color = default_color(new_id)
|
|
|
|
for point in self.points:
|
|
if point.line_id in id_map:
|
|
point.line_id = id_map[point.line_id]
|
|
|
|
self._next_line_id = len(self.lines) + 1
|
|
|
|
def _validate_line_limit(self) -> None:
|
|
line_limit = self._line_limit()
|
|
if len(self.lines) > line_limit:
|
|
raise ValueError(f"Mapping supports no more than {line_limit} lines.")
|
|
|
|
def _line_limit(self) -> int:
|
|
return MAX_LINES
|
|
|
|
def _coordinate_unit_for_save(self) -> str:
|
|
if self.coordinate_unit in {"m", "meter", "meters"}:
|
|
return "m"
|
|
if self.coordinate_unit in {"mm", "millimeter", "millimeters"}:
|
|
return "mm"
|
|
return "m" if coordinate_display_scale(self.points) == 1000.0 else "mm"
|
|
|
|
def order_lines_clockwise(self) -> None:
|
|
self.lines = clockwise_sort(self.lines, self._points_for_line)
|
|
|
|
def order_blocks_clockwise(self, line_id: int, *, reorder_leds: bool = False) -> None:
|
|
line = self.get_line(line_id)
|
|
if reorder_leds:
|
|
for block in line.blocks:
|
|
start_point_id = block.point_ids[0] if block.point_ids else None
|
|
ordered_ids = [point.uid for point in order_points_like_grid_svg(self._points_for_block(block), start_point_id)]
|
|
if start_point_id is not None:
|
|
ordered_ids = rotate_ids_to_start(ordered_ids, start_point_id)
|
|
block.point_ids = ordered_ids
|
|
line.blocks = clockwise_sort(line.blocks, self._points_for_block)
|
|
|
|
def describe_unfolding(self) -> UnfoldingDescription:
|
|
summary = UnfoldingDescription(
|
|
max_lines=self._line_limit(),
|
|
line_order="clockwise",
|
|
block_order="clockwise",
|
|
led_index_scope="line",
|
|
block_size=self.unfolding.block_size,
|
|
)
|
|
for line in self.lines:
|
|
led_count = sum(len(block.point_ids) for block in line.blocks)
|
|
block_count = len(line.blocks) or summary.expected_block_count(led_count)
|
|
summary.block_counts.append(UnfoldingLineSummary(line.id, led_count, block_count))
|
|
return summary
|
|
|
|
def _points_for_line(self, line: LedLine) -> list[LedPoint]:
|
|
return [point for block in line.blocks for point in self._points_for_block(block)]
|
|
|
|
def _points_for_block(self, block: LedBlock) -> list[LedPoint]:
|
|
return [self.points[point_id] for point_id in block.point_ids if 0 <= point_id < len(self.points)]
|
|
|
|
def _normalize_loaded_point_ids(
|
|
self,
|
|
values: Any,
|
|
source_to_uid: dict[int, int],
|
|
mode: str = "auto",
|
|
) -> list[int]:
|
|
if not isinstance(values, list):
|
|
return []
|
|
raw_ids = [int(value) for value in values]
|
|
if mode in {"source", "source_id"}:
|
|
return unique_in_order([
|
|
source_to_uid[value]
|
|
for value in raw_ids
|
|
if value in source_to_uid
|
|
])
|
|
valid_uid_ids = [point_id for point_id in raw_ids if 0 <= point_id < len(self.points)]
|
|
if mode == "uid":
|
|
return unique_in_order(valid_uid_ids)
|
|
if mode == "auto" and len(valid_uid_ids) == len(raw_ids):
|
|
return unique_in_order(valid_uid_ids)
|
|
return unique_in_order([
|
|
source_to_uid[value]
|
|
for value in raw_ids
|
|
if value in source_to_uid
|
|
])
|
|
|
|
def _deduplicate_block_memberships(self) -> None:
|
|
seen: set[int] = set()
|
|
for line in self.lines:
|
|
for block in line.blocks:
|
|
unique_ids = []
|
|
for point_id in block.point_ids:
|
|
if not 0 <= point_id < len(self.points):
|
|
continue
|
|
if point_id in seen:
|
|
continue
|
|
seen.add(point_id)
|
|
unique_ids.append(point_id)
|
|
block.point_ids = unique_ids
|
|
|
|
def _detach_points_from_blocks(self, point_ids: list[int]) -> int:
|
|
if not point_ids:
|
|
return 0
|
|
point_id_set = set(point_ids)
|
|
removed = 0
|
|
for line in self.lines:
|
|
for block in line.blocks:
|
|
before = len(block.point_ids)
|
|
block.point_ids = [point_id for point_id in block.point_ids if point_id not in point_id_set]
|
|
removed += before - len(block.point_ids)
|
|
self._clear_points(list(point_id_set))
|
|
return removed
|
|
|
|
def _clear_points(self, point_ids: list[int]) -> None:
|
|
for point_id in point_ids:
|
|
point = self.points[point_id]
|
|
point.line_id = None
|
|
point.block_id = None
|
|
point.line_index = None
|
|
point.block_index = None
|
|
|
|
def auto_connect_line(self, line_id: int) -> None:
|
|
line = self.get_line(line_id)
|
|
self.order_blocks_clockwise(line.id, reorder_leds=True)
|
|
self.reindex_line(line.id)
|
|
|
|
def rebuild_line_blocks_by_route(self, line_id: int) -> int:
|
|
line = self.get_line(line_id)
|
|
point_ids = [point_id for block in line.blocks for point_id in block.point_ids]
|
|
if not point_ids:
|
|
return 0
|
|
start_ids = [block.point_ids[0] for block in line.blocks if block.point_ids]
|
|
start_id = start_ids[0] if start_ids else point_ids[0]
|
|
groups = self.route_groups_from_points(point_ids, start_id)
|
|
line.blocks.clear()
|
|
for group in groups:
|
|
line.blocks.append(LedBlock(self._next_block_id, self._next_block_name(line), group))
|
|
self._next_block_id += 1
|
|
self.reindex_all()
|
|
return len(groups)
|
|
|
|
def auto_blocks_from_selection(self, line_id: int, point_ids: list[int], groups: list[list[int]] | None = None) -> None:
|
|
groups = self.auto_block_preview(point_ids) if groups is None else groups
|
|
if not groups:
|
|
return
|
|
self._detach_points_from_blocks([point_id for group in groups for point_id in group])
|
|
line = self.get_line(line_id)
|
|
for group_ids in groups:
|
|
block = LedBlock(self._next_block_id, self._next_block_name(line), unique_in_order(group_ids))
|
|
self._next_block_id += 1
|
|
line.blocks.append(block)
|
|
self.order_blocks_clockwise(line_id, reorder_leds=False)
|
|
self.reindex_all()
|
|
|
|
def route_groups_from_points(self, point_ids: list[int], start_point_id: int | None = None) -> list[list[int]]:
|
|
points = [
|
|
self.points[point_id]
|
|
for point_id in unique_in_order(point_ids)
|
|
if 0 <= point_id < len(self.points)
|
|
]
|
|
return [
|
|
[point.uid for point in route]
|
|
for route in trace_route_paths_like_grid_svg(points, start_point_id)
|
|
]
|
|
|
|
def add_clockwise_block_from_seed(self, line_id: int, start_point_id: int) -> LedBlock:
|
|
if not 0 <= start_point_id < len(self.points):
|
|
raise ValueError(f"Unknown LED point id: {start_point_id}")
|
|
group_ids = self.clockwise_block_from_seed_preview(start_point_id)
|
|
return self.add_clockwise_block_from_group(line_id, start_point_id, group_ids)
|
|
|
|
def add_clockwise_block_from_group(self, line_id: int, start_point_id: int, group_ids: list[int]) -> LedBlock:
|
|
if not 0 <= start_point_id < len(self.points):
|
|
raise ValueError(f"Unknown LED point id: {start_point_id}")
|
|
block = self.add_block(line_id, group_ids)
|
|
block.point_ids = rotate_ids_to_start(block.point_ids, start_point_id)
|
|
self.reindex_line(line_id)
|
|
return block
|
|
|
|
def clockwise_block_from_seed_preview(self, start_point_id: int) -> list[int]:
|
|
if not 0 <= start_point_id < len(self.points):
|
|
return []
|
|
candidate_points = [
|
|
point
|
|
for point in self.points
|
|
if point.uid == start_point_id or (point.line_id is None and point.block_id is None)
|
|
]
|
|
threshold = min_neighbor_distance_for_points(candidate_points)
|
|
adjacency = build_route_adjacency(candidate_points, threshold)
|
|
route = trace_single_route_like_grid_svg(
|
|
candidate_points,
|
|
start_point_id,
|
|
adjacency=adjacency,
|
|
allowed_ids={point.uid for point in candidate_points},
|
|
min_neighbor_distance=threshold,
|
|
)
|
|
if not route:
|
|
return [start_point_id]
|
|
return rotate_ids_to_start(
|
|
[point.uid for point in normalize_route_clockwise(route, threshold)],
|
|
start_point_id,
|
|
)
|
|
|
|
def auto_block_preview(self, point_ids: list[int]) -> list[list[int]]:
|
|
points = [
|
|
self.points[point_id]
|
|
for point_id in sorted_unique(point_ids)
|
|
if 0 <= point_id < len(self.points)
|
|
]
|
|
return [
|
|
[point.uid for point in route]
|
|
for route in trace_route_paths_like_grid_svg(points)
|
|
]
|
|
|
|
def reindex_all(self) -> None:
|
|
for point in self.points:
|
|
point.line_id = None
|
|
point.block_id = None
|
|
point.line_index = None
|
|
point.block_index = None
|
|
for line in self.lines:
|
|
self.reindex_line(line.id)
|
|
|
|
def reindex_line(self, line_id: int) -> None:
|
|
line = self.get_line(line_id)
|
|
line_index = 0
|
|
for block in line.blocks:
|
|
for block_index, point_id in enumerate(block.point_ids):
|
|
point = self.points[point_id]
|
|
point.line_id = line.id
|
|
point.block_id = block.id
|
|
point.line_index = line_index
|
|
point.block_index = block_index
|
|
line_index += 1
|
|
|
|
@staticmethod
|
|
def _next_block_name(line: LedLine) -> str:
|
|
max_number = 0
|
|
for block in line.blocks:
|
|
match = re.search(r"(\d+)$", block.name)
|
|
if match is not None:
|
|
max_number = max(max_number, int(match.group(1)))
|
|
return f"Блок {max_number + 1}"
|
|
|
|
|
|
def sorted_unique(values: list[int]) -> list[int]:
|
|
return sorted(set(values))
|
|
|
|
|
|
def unique_in_order(values: list[int]) -> list[int]:
|
|
seen: set[int] = set()
|
|
result: list[int] = []
|
|
for value in values:
|
|
if value in seen:
|
|
continue
|
|
seen.add(value)
|
|
result.append(value)
|
|
return result
|
|
|
|
|
|
def rotate_ids_to_start(values: list[int], start_value: int) -> list[int]:
|
|
if start_value not in values:
|
|
return values[:]
|
|
start_index = values.index(start_value)
|
|
return [*values[start_index:], *values[:start_index]]
|
|
|
|
|
|
def normalize_route_clockwise(route: list[LedPoint], min_neighbor_distance: float) -> list[LedPoint]:
|
|
if len(route) < 3:
|
|
return route[:]
|
|
if not is_valid_route_neighbor(route[-1], route[0], min_neighbor_distance):
|
|
return route[:]
|
|
return list(reversed(route)) if route_signed_area(route) > 0 else route[:]
|
|
|
|
|
|
def route_signed_area(route: list[LedPoint]) -> float:
|
|
area = 0.0
|
|
for current, next_point in zip(route, [*route[1:], route[0]]):
|
|
area += current.x * next_point.y - next_point.x * current.y
|
|
return area / 2.0
|
|
|
|
|
|
def _optional_int(value: Any) -> int | None:
|
|
return None if value is None else int(value)
|
|
|
|
|
|
def default_color(line_id: int) -> str:
|
|
colors = ["#e11d48", "#2563eb", "#16a34a", "#f59e0b", "#7c3aed", "#0891b2"]
|
|
return colors[(line_id - 1) % len(colors)]
|
|
|
|
|
|
def distance(a: LedPoint, b: LedPoint) -> float:
|
|
return math.hypot(a.x - b.x, a.y - b.y)
|
|
|
|
|
|
def are_neighbors(a: LedPoint, b: LedPoint) -> bool:
|
|
return abs(a.row - b.row) <= 1 and abs(a.col - b.col) <= 1
|
|
|
|
|
|
def connected_groups(points: list[LedPoint]) -> list[list[LedPoint]]:
|
|
remaining = set(point.uid for point in points)
|
|
by_id = {point.uid: point for point in points}
|
|
groups: list[list[LedPoint]] = []
|
|
while remaining:
|
|
start = remaining.pop()
|
|
queue = [start]
|
|
group = [by_id[start]]
|
|
while queue:
|
|
current_id = queue.pop()
|
|
current = by_id[current_id]
|
|
for candidate_id in list(remaining):
|
|
if are_neighbors(current, by_id[candidate_id]):
|
|
remaining.remove(candidate_id)
|
|
queue.append(candidate_id)
|
|
group.append(by_id[candidate_id])
|
|
groups.append(group)
|
|
return sorted(groups, key=lambda group: min(point.uid for point in group))
|
|
|
|
|
|
def contour_groups_like_grid_svg(points: list[LedPoint]) -> list[list[LedPoint]]:
|
|
points_by_cell: dict[tuple[int, int], list[LedPoint]] = {}
|
|
for point in points:
|
|
points_by_cell.setdefault((point.row, point.col), []).append(point)
|
|
|
|
cells = set(points_by_cell)
|
|
seen: set[tuple[int, int]] = set()
|
|
groups: list[list[LedPoint]] = []
|
|
neighbors = ((1, 0), (-1, 0), (0, 1), (0, -1))
|
|
|
|
for start_cell in sorted(cells):
|
|
if start_cell in seen:
|
|
continue
|
|
queue = deque([start_cell])
|
|
seen.add(start_cell)
|
|
group_cells: list[tuple[int, int]] = []
|
|
while queue:
|
|
row, col = queue.popleft()
|
|
group_cells.append((row, col))
|
|
for d_row, d_col in neighbors:
|
|
next_cell = (row + d_row, col + d_col)
|
|
if next_cell in cells and next_cell not in seen:
|
|
seen.add(next_cell)
|
|
queue.append(next_cell)
|
|
groups.append([
|
|
point
|
|
for cell in group_cells
|
|
for point in points_by_cell[cell]
|
|
])
|
|
|
|
return sorted(groups, key=lambda group: min(point_sort_key_for_model(point) for point in group))
|
|
|
|
|
|
def route_groups_like_grid_svg(points: list[LedPoint]) -> list[list[LedPoint]]:
|
|
points_by_cell: dict[tuple[int, int], list[LedPoint]] = {}
|
|
for point in points:
|
|
points_by_cell.setdefault((point.row, point.col), []).append(point)
|
|
|
|
cells = set(points_by_cell)
|
|
seen: set[tuple[int, int]] = set()
|
|
groups: list[list[LedPoint]] = []
|
|
neighbors = ((1, 0), (-1, 0), (0, 1), (0, -1), (1, 1), (-1, 1), (-1, -1), (1, -1))
|
|
threshold = min_neighbor_distance_for_points(points)
|
|
|
|
for start_cell in sorted(cells):
|
|
if start_cell in seen:
|
|
continue
|
|
queue = deque([start_cell])
|
|
seen.add(start_cell)
|
|
group_cells: list[tuple[int, int]] = []
|
|
while queue:
|
|
row, col = queue.popleft()
|
|
group_cells.append((row, col))
|
|
for d_row, d_col in neighbors:
|
|
next_cell = (row + d_row, col + d_col)
|
|
if next_cell not in cells or next_cell in seen:
|
|
continue
|
|
if not cells_have_valid_route_neighbor(points_by_cell[(row, col)], points_by_cell[next_cell], threshold):
|
|
continue
|
|
seen.add(next_cell)
|
|
queue.append(next_cell)
|
|
groups.append([
|
|
point
|
|
for cell in group_cells
|
|
for point in points_by_cell[cell]
|
|
])
|
|
|
|
return sorted(groups, key=lambda group: min(point_sort_key_for_model(point) for point in group))
|
|
|
|
|
|
def cells_have_valid_route_neighbor(
|
|
first_points: list[LedPoint],
|
|
second_points: list[LedPoint],
|
|
threshold: float,
|
|
) -> bool:
|
|
return any(
|
|
distance(first, second) <= max_neighbor_distance(threshold)
|
|
for first in first_points
|
|
for second in second_points
|
|
)
|
|
|
|
|
|
def order_group_nearest(points: list[LedPoint]) -> list[LedPoint]:
|
|
if len(points) <= 1:
|
|
return points[:]
|
|
remaining = points[:]
|
|
ordered = [min(remaining, key=lambda point: (point.row, point.col, point.source_id))]
|
|
remaining.remove(ordered[0])
|
|
while remaining:
|
|
previous = ordered[-1]
|
|
next_point = min(remaining, key=lambda point: (distance(previous, point), point.row, point.col))
|
|
ordered.append(next_point)
|
|
remaining.remove(next_point)
|
|
return ordered
|
|
|
|
|
|
def order_points_like_grid_svg(points: list[LedPoint], start_point_id: int | None = None) -> list[LedPoint]:
|
|
if len(points) <= 1:
|
|
return points[:]
|
|
min_neighbor_distance = min_neighbor_distance_for_points(points)
|
|
remaining = points[:]
|
|
if start_point_id is None:
|
|
start_point = min(remaining, key=point_sort_key_for_model)
|
|
else:
|
|
start_point = next((point for point in remaining if point.uid == start_point_id), None)
|
|
if start_point is None:
|
|
start_point = min(remaining, key=point_sort_key_for_model)
|
|
ordered = [start_point]
|
|
remaining.remove(start_point)
|
|
|
|
while remaining:
|
|
previous = ordered[-1]
|
|
neighbors = [
|
|
point
|
|
for point in remaining
|
|
if is_valid_route_neighbor(previous, point, min_neighbor_distance)
|
|
]
|
|
if neighbors:
|
|
next_point = min(
|
|
neighbors,
|
|
key=lambda point: route_neighbor_preference(previous, point, min_neighbor_distance),
|
|
)
|
|
else:
|
|
next_point = min(
|
|
remaining,
|
|
key=lambda point: (
|
|
distance(previous, point),
|
|
point_sort_key_for_model(point),
|
|
),
|
|
)
|
|
ordered.append(next_point)
|
|
remaining.remove(next_point)
|
|
|
|
return ordered
|
|
|
|
|
|
def trace_route_paths_like_grid_svg(
|
|
points: list[LedPoint],
|
|
start_point_id: int | None = None,
|
|
) -> list[list[LedPoint]]:
|
|
remaining = points[:]
|
|
remaining_ids = {point.uid for point in remaining}
|
|
routes: list[list[LedPoint]] = []
|
|
preferred_start_id = start_point_id
|
|
min_neighbor_distance = min_neighbor_distance_for_points(points)
|
|
while remaining:
|
|
if preferred_start_id is not None:
|
|
start_point = next((point for point in remaining if point.uid == preferred_start_id), None)
|
|
else:
|
|
start_point = None
|
|
if start_point is None:
|
|
start_point = min(remaining, key=point_sort_key_for_model)
|
|
adjacency = build_route_adjacency(remaining, min_neighbor_distance)
|
|
route = trace_single_route_like_grid_svg(
|
|
remaining,
|
|
start_point.uid,
|
|
adjacency=adjacency,
|
|
allowed_ids=remaining_ids,
|
|
min_neighbor_distance=min_neighbor_distance,
|
|
)
|
|
if not route:
|
|
break
|
|
route_ids = {point.uid for point in route}
|
|
routes.append(route)
|
|
remaining_ids -= route_ids
|
|
remaining = [point for point in remaining if point.uid in remaining_ids]
|
|
preferred_start_id = None
|
|
return routes
|
|
|
|
|
|
def trace_single_route_like_grid_svg(
|
|
points: list[LedPoint],
|
|
start_point_id: int,
|
|
adjacency: dict[int, list[LedPoint]] | None = None,
|
|
allowed_ids: set[int] | None = None,
|
|
min_neighbor_distance: float | None = None,
|
|
) -> list[LedPoint]:
|
|
if not points:
|
|
return []
|
|
min_neighbor_distance = min_neighbor_distance or min_neighbor_distance_for_points(points)
|
|
adjacency = adjacency or build_route_adjacency(points, min_neighbor_distance)
|
|
remaining = points[:]
|
|
remaining_ids = set(allowed_ids) if allowed_ids is not None else {point.uid for point in remaining}
|
|
start_point = next((point for point in remaining if point.uid == start_point_id), None)
|
|
if start_point is None:
|
|
start_point = min(remaining, key=point_sort_key_for_model)
|
|
route = [start_point]
|
|
remaining.remove(start_point)
|
|
remaining_ids.remove(start_point.uid)
|
|
|
|
while remaining:
|
|
previous = route[-1]
|
|
neighbors = [
|
|
point
|
|
for point in adjacency.get(previous.uid, [])
|
|
if point.uid in remaining_ids
|
|
]
|
|
if not neighbors:
|
|
neighbors = fallback_route_neighbors(
|
|
route,
|
|
previous,
|
|
remaining,
|
|
remaining_ids,
|
|
min_neighbor_distance,
|
|
)
|
|
if not neighbors:
|
|
break
|
|
next_point = choose_next_route_point(route, neighbors, min_neighbor_distance)
|
|
route.append(next_point)
|
|
remaining.remove(next_point)
|
|
remaining_ids.remove(next_point.uid)
|
|
|
|
return route
|
|
|
|
|
|
def fallback_route_neighbors(
|
|
route: list[LedPoint],
|
|
current: LedPoint,
|
|
points: list[LedPoint],
|
|
allowed_ids: set[int],
|
|
min_neighbor_distance: float,
|
|
) -> list[LedPoint]:
|
|
valid_neighbors = [
|
|
point
|
|
for point in points
|
|
if point.uid in allowed_ids and is_valid_route_neighbor(current, point, min_neighbor_distance)
|
|
]
|
|
if valid_neighbors or len(route) < 2:
|
|
return valid_neighbors
|
|
|
|
trajectory_neighbors = [
|
|
point
|
|
for point in points
|
|
if point.uid in allowed_ids and is_trajectory_continuation(route[-2], current, point, min_neighbor_distance)
|
|
]
|
|
return trajectory_neighbors
|
|
|
|
|
|
def choose_next_route_point(
|
|
route: list[LedPoint],
|
|
neighbors: list[LedPoint],
|
|
min_neighbor_distance: float,
|
|
) -> LedPoint:
|
|
if len(route) < 2:
|
|
return min(neighbors, key=lambda point: route_path_preference(route, point, min_neighbor_distance))
|
|
|
|
neighbors = nearest_neighbor_band(route[-1], neighbors, min_neighbor_distance)
|
|
previous = route[-2]
|
|
current = route[-1]
|
|
same_vector_direction = [
|
|
point
|
|
for point in neighbors
|
|
if same_direction_vector(previous, current, point)
|
|
]
|
|
if same_vector_direction:
|
|
return min(same_vector_direction, key=lambda point: route_path_preference(route, point, min_neighbor_distance))
|
|
|
|
forward_direction = [
|
|
point
|
|
for point in neighbors
|
|
if forward_direction_vector(previous, current, point)
|
|
]
|
|
if forward_direction:
|
|
return min(forward_direction, key=lambda point: route_path_preference(route, point, min_neighbor_distance))
|
|
|
|
return min(neighbors, key=lambda point: route_path_preference(route, point, min_neighbor_distance))
|
|
|
|
|
|
def nearest_neighbor_band(
|
|
current: LedPoint,
|
|
neighbors: list[LedPoint],
|
|
min_neighbor_distance: float,
|
|
) -> list[LedPoint]:
|
|
nearest_distance = min(distance(current, point) for point in neighbors)
|
|
band_limit = min(nearest_distance + neighbor_tolerance(min_neighbor_distance), max_neighbor_distance(min_neighbor_distance))
|
|
band = [point for point in neighbors if distance(current, point) <= band_limit]
|
|
return band or [min(neighbors, key=lambda point: distance(current, point))]
|
|
|
|
|
|
def same_direction_vector(previous: LedPoint, current: LedPoint, candidate: LedPoint) -> bool:
|
|
incoming_x = current.x - previous.x
|
|
incoming_y = current.y - previous.y
|
|
outgoing_x = candidate.x - current.x
|
|
outgoing_y = candidate.y - current.y
|
|
if incoming_x == 0 and incoming_y == 0:
|
|
return False
|
|
if outgoing_x == 0 and outgoing_y == 0:
|
|
return False
|
|
dot = incoming_x * outgoing_x + incoming_y * outgoing_y
|
|
cross = incoming_x * outgoing_y - incoming_y * outgoing_x
|
|
return dot > 0 and abs(cross) <= max(1e-6, 0.1 * math.hypot(incoming_x, incoming_y) * math.hypot(outgoing_x, outgoing_y))
|
|
|
|
|
|
def forward_direction_vector(previous: LedPoint, current: LedPoint, candidate: LedPoint) -> bool:
|
|
incoming_x = current.x - previous.x
|
|
incoming_y = current.y - previous.y
|
|
outgoing_x = candidate.x - current.x
|
|
outgoing_y = candidate.y - current.y
|
|
incoming_length = math.hypot(incoming_x, incoming_y)
|
|
outgoing_length = math.hypot(outgoing_x, outgoing_y)
|
|
if incoming_length == 0 or outgoing_length == 0:
|
|
return False
|
|
dot = incoming_x * outgoing_x + incoming_y * outgoing_y
|
|
cos_angle = dot / (incoming_length * outgoing_length)
|
|
return cos_angle > -0.25
|
|
|
|
|
|
def is_trajectory_continuation(
|
|
previous: LedPoint,
|
|
current: LedPoint,
|
|
candidate: LedPoint,
|
|
min_neighbor_distance: float,
|
|
) -> bool:
|
|
if not forward_direction_vector(previous, current, candidate):
|
|
return False
|
|
return distance(current, candidate) <= route_continuation_distance_limit(current, candidate, min_neighbor_distance)
|
|
|
|
|
|
def route_continuation_distance_limit(current: LedPoint, candidate: LedPoint, min_neighbor_distance: float) -> float:
|
|
return min_neighbor_distance * 2.0 + neighbor_tolerance(min_neighbor_distance)
|
|
|
|
|
|
def build_route_adjacency(
|
|
points: list[LedPoint],
|
|
min_neighbor_distance: float,
|
|
) -> dict[int, list[LedPoint]]:
|
|
search_radius = min_neighbor_distance * math.sqrt(2.0) + neighbor_tolerance(min_neighbor_distance)
|
|
bucket_size = max(search_radius, 1e-9)
|
|
points_by_bucket: dict[tuple[int, int], list[LedPoint]] = {}
|
|
for point in points:
|
|
bucket = (math.floor(point.x / bucket_size), math.floor(point.y / bucket_size))
|
|
points_by_bucket.setdefault(bucket, []).append(point)
|
|
|
|
raw_adjacency: dict[int, list[LedPoint]] = {}
|
|
for point in points:
|
|
candidates: list[LedPoint] = []
|
|
bucket = (math.floor(point.x / bucket_size), math.floor(point.y / bucket_size))
|
|
for d_x in (-1, 0, 1):
|
|
for d_y in (-1, 0, 1):
|
|
for candidate in points_by_bucket.get((bucket[0] + d_x, bucket[1] + d_y), []):
|
|
if candidate.uid == point.uid:
|
|
continue
|
|
if is_valid_route_neighbor(point, candidate, min_neighbor_distance):
|
|
candidates.append(candidate)
|
|
raw_adjacency[point.uid] = candidates
|
|
|
|
adjacency: dict[int, list[LedPoint]] = {}
|
|
for point in points:
|
|
candidates = [
|
|
candidate
|
|
for candidate in raw_adjacency[point.uid]
|
|
if (
|
|
not route_edge_skips_intermediate_point(
|
|
point,
|
|
candidate,
|
|
points_by_bucket,
|
|
bucket_size,
|
|
min_neighbor_distance,
|
|
)
|
|
and not is_parallel_track_jump(point, candidate, raw_adjacency)
|
|
)
|
|
]
|
|
adjacency[point.uid] = sorted(
|
|
candidates,
|
|
key=lambda candidate: route_neighbor_preference(point, candidate, min_neighbor_distance),
|
|
)
|
|
return adjacency
|
|
|
|
|
|
def route_edge_skips_intermediate_point(
|
|
current: LedPoint,
|
|
candidate: LedPoint,
|
|
points_by_bucket: dict[tuple[int, int], list[LedPoint]],
|
|
bucket_size: float,
|
|
min_neighbor_distance: float,
|
|
) -> bool:
|
|
segment_x = candidate.x - current.x
|
|
segment_y = candidate.y - current.y
|
|
segment_length_sq = segment_x * segment_x + segment_y * segment_y
|
|
if segment_length_sq <= 0:
|
|
return False
|
|
|
|
segment_length = math.sqrt(segment_length_sq)
|
|
line_tolerance = max(neighbor_tolerance(min_neighbor_distance), min_neighbor_distance * 0.25)
|
|
end_tolerance = max(1e-6, line_tolerance * 0.25)
|
|
min_bucket_x = math.floor((min(current.x, candidate.x) - line_tolerance) / bucket_size)
|
|
max_bucket_x = math.floor((max(current.x, candidate.x) + line_tolerance) / bucket_size)
|
|
min_bucket_y = math.floor((min(current.y, candidate.y) - line_tolerance) / bucket_size)
|
|
max_bucket_y = math.floor((max(current.y, candidate.y) + line_tolerance) / bucket_size)
|
|
nearby_points = (
|
|
point
|
|
for bucket_x in range(min_bucket_x, max_bucket_x + 1)
|
|
for bucket_y in range(min_bucket_y, max_bucket_y + 1)
|
|
for point in points_by_bucket.get((bucket_x, bucket_y), [])
|
|
)
|
|
for point in nearby_points:
|
|
if point.uid in {current.uid, candidate.uid}:
|
|
continue
|
|
point_x = point.x - current.x
|
|
point_y = point.y - current.y
|
|
projection = (point_x * segment_x + point_y * segment_y) / segment_length_sq
|
|
if projection <= 0.0 or projection >= 1.0:
|
|
continue
|
|
perpendicular = abs(point_x * segment_y - point_y * segment_x) / segment_length
|
|
if perpendicular > line_tolerance:
|
|
continue
|
|
if distance(current, point) < segment_length - end_tolerance:
|
|
return True
|
|
return False
|
|
|
|
|
|
def is_parallel_track_jump(
|
|
current: LedPoint,
|
|
candidate: LedPoint,
|
|
raw_adjacency: dict[int, list[LedPoint]],
|
|
) -> bool:
|
|
d_x = candidate.x - current.x
|
|
d_y = candidate.y - current.y
|
|
if abs(d_x) > abs(d_y):
|
|
return (
|
|
route_neighbor_axis_count(current, raw_adjacency, "vertical") >= 2
|
|
and route_neighbor_axis_count(candidate, raw_adjacency, "vertical") >= 2
|
|
)
|
|
if abs(d_y) > abs(d_x):
|
|
return (
|
|
route_neighbor_axis_count(current, raw_adjacency, "horizontal") >= 2
|
|
and route_neighbor_axis_count(candidate, raw_adjacency, "horizontal") >= 2
|
|
)
|
|
return False
|
|
|
|
|
|
def route_neighbor_axis_count(
|
|
point: LedPoint,
|
|
raw_adjacency: dict[int, list[LedPoint]],
|
|
axis: str,
|
|
) -> int:
|
|
if axis == "horizontal":
|
|
return sum(1 for candidate in raw_adjacency[point.uid] if abs(candidate.x - point.x) > abs(candidate.y - point.y))
|
|
return sum(1 for candidate in raw_adjacency[point.uid] if abs(candidate.y - point.y) > abs(candidate.x - point.x))
|
|
|
|
|
|
def trace_closed_route(
|
|
points: list[LedPoint],
|
|
start_point: LedPoint,
|
|
min_neighbor_distance: float,
|
|
adjacency: dict[int, list[LedPoint]] | None = None,
|
|
allowed_ids: set[int] | None = None,
|
|
) -> list[LedPoint]:
|
|
adjacency = adjacency or build_route_adjacency(points, min_neighbor_distance)
|
|
allowed_ids = set(allowed_ids) if allowed_ids is not None else {point.uid for point in points}
|
|
if len(adjacency.get(start_point.uid, [])) < 2:
|
|
return []
|
|
|
|
route = [start_point]
|
|
visited = {start_point.uid}
|
|
stack: list[tuple[LedPoint, list[LedPoint], int]] = [(
|
|
start_point,
|
|
[point for point in adjacency[start_point.uid] if point.uid in allowed_ids],
|
|
0,
|
|
)]
|
|
attempts = 0
|
|
max_attempts = min(5000, max(1000, len(points) * 25))
|
|
|
|
while stack and attempts < max_attempts:
|
|
current, candidates, next_index = stack[-1]
|
|
attempts += 1
|
|
|
|
meaningful_continuations = [
|
|
point
|
|
for point in adjacency.get(current.uid, [])
|
|
if point.uid in allowed_ids and point.uid not in visited
|
|
and len([next_point for next_point in adjacency.get(point.uid, []) if next_point.uid in allowed_ids]) > 1
|
|
]
|
|
if (
|
|
len(route) >= 4
|
|
and is_valid_route_neighbor(current, start_point, min_neighbor_distance)
|
|
and not meaningful_continuations
|
|
):
|
|
return route[:]
|
|
|
|
if next_index >= len(candidates):
|
|
stack.pop()
|
|
removed = route.pop()
|
|
visited.remove(removed.uid)
|
|
continue
|
|
|
|
candidate = candidates[next_index]
|
|
stack[-1] = (current, candidates, next_index + 1)
|
|
if candidate.uid in visited:
|
|
continue
|
|
|
|
visited.add(candidate.uid)
|
|
route.append(candidate)
|
|
next_candidates = [
|
|
point
|
|
for point in adjacency.get(candidate.uid, [])
|
|
if point.uid in allowed_ids and point.uid not in visited
|
|
]
|
|
stack.append((candidate, next_candidates, 0))
|
|
|
|
return []
|
|
|
|
|
|
def point_sort_key_for_model(point: LedPoint) -> tuple[int, int, float, float, int]:
|
|
return point.row, point.col, point.x, point.y, point.uid
|
|
|
|
|
|
def too_close_preference(current: LedPoint, candidate: LedPoint, min_distance: float) -> int:
|
|
return 1 if distance(current, candidate) < min_distance else 0
|
|
|
|
|
|
def route_neighbor_preference(
|
|
current: LedPoint,
|
|
candidate: LedPoint,
|
|
min_distance: float,
|
|
) -> tuple[int, int, int, float, tuple[int, int, float, float, int]]:
|
|
return (
|
|
too_close_preference(current, candidate, min_distance),
|
|
diagonal_cell_preference(current, candidate),
|
|
cardinal_direction_preference(current, candidate),
|
|
distance(current, candidate),
|
|
point_sort_key_for_model(candidate),
|
|
)
|
|
|
|
|
|
def route_path_preference(
|
|
route: list[LedPoint],
|
|
candidate: LedPoint,
|
|
min_distance: float,
|
|
) -> tuple[int, float, int, int, float, tuple[int, int, float, float, int]]:
|
|
current = route[-1]
|
|
if len(route) < 2:
|
|
return (
|
|
diagonal_cell_preference(current, candidate),
|
|
0.0,
|
|
cardinal_direction_preference(current, candidate),
|
|
too_close_preference(current, candidate, min_distance),
|
|
0,
|
|
distance(current, candidate),
|
|
point_sort_key_for_model(candidate),
|
|
)
|
|
|
|
previous = route[-2]
|
|
turn_class, turn_rank = route_clockwise_turn_preference(previous, current, candidate)
|
|
return (
|
|
turn_class,
|
|
turn_rank,
|
|
diagonal_cell_preference(current, candidate),
|
|
too_close_preference(current, candidate, min_distance),
|
|
cardinal_direction_preference(current, candidate),
|
|
distance(current, candidate),
|
|
point_sort_key_for_model(candidate),
|
|
)
|
|
|
|
|
|
def route_clockwise_turn_preference(previous: LedPoint, current: LedPoint, candidate: LedPoint) -> tuple[int, float]:
|
|
incoming_x = current.x - previous.x
|
|
incoming_y = current.y - previous.y
|
|
outgoing_x = candidate.x - current.x
|
|
outgoing_y = candidate.y - current.y
|
|
cross = incoming_x * outgoing_y - incoming_y * outgoing_x
|
|
angle = route_turn_angle(previous, current, candidate)
|
|
epsilon = 1e-6
|
|
if abs(cross) <= epsilon:
|
|
return 0, 0.0
|
|
if angle < math.radians(35):
|
|
return 0, angle
|
|
if cross < -epsilon:
|
|
return 1, -angle
|
|
return 2, angle
|
|
|
|
|
|
def route_turn_angle(previous: LedPoint, current: LedPoint, candidate: LedPoint) -> float:
|
|
incoming_x = current.x - previous.x
|
|
incoming_y = current.y - previous.y
|
|
outgoing_x = candidate.x - current.x
|
|
outgoing_y = candidate.y - current.y
|
|
incoming_length = math.hypot(incoming_x, incoming_y)
|
|
outgoing_length = math.hypot(outgoing_x, outgoing_y)
|
|
if incoming_length == 0 or outgoing_length == 0:
|
|
return 0.0
|
|
dot = incoming_x * outgoing_x + incoming_y * outgoing_y
|
|
cos_angle = max(-1.0, min(1.0, dot / (incoming_length * outgoing_length)))
|
|
return math.acos(cos_angle)
|
|
|
|
|
|
def diagonal_cell_preference(current: LedPoint, candidate: LedPoint) -> int:
|
|
return 1 if abs(current.row - candidate.row) == 1 and abs(current.col - candidate.col) == 1 else 0
|
|
|
|
|
|
def cardinal_direction_preference(current: LedPoint, candidate: LedPoint) -> int:
|
|
d_x = candidate.x - current.x
|
|
d_y = candidate.y - current.y
|
|
if abs(d_y) >= abs(d_x) and d_y > 0:
|
|
return 0
|
|
if abs(d_x) > abs(d_y) and d_x < 0:
|
|
return 1
|
|
if abs(d_y) >= abs(d_x) and d_y < 0:
|
|
return 2
|
|
if abs(d_x) > abs(d_y) and d_x > 0:
|
|
return 3
|
|
return 4
|
|
|
|
|
|
def is_valid_route_neighbor(current: LedPoint, candidate: LedPoint, nominal_distance: float) -> bool:
|
|
return distance(current, candidate) <= route_neighbor_distance_limit(
|
|
current,
|
|
candidate,
|
|
nominal_distance,
|
|
)
|
|
|
|
|
|
def route_neighbor_distance_limit(current: LedPoint, candidate: LedPoint, nominal_distance: float) -> float:
|
|
if abs(current.row - candidate.row) == 1 and abs(current.col - candidate.col) == 1:
|
|
return nominal_distance * math.sqrt(2.0) + neighbor_tolerance(nominal_distance)
|
|
return max_neighbor_distance(nominal_distance)
|
|
|
|
|
|
def max_neighbor_distance(nominal_distance: float) -> float:
|
|
return nominal_distance + neighbor_tolerance(nominal_distance)
|
|
|
|
|
|
def neighbor_tolerance(nominal_distance: float) -> float:
|
|
return MAX_NEIGHBOR_TOLERANCE_MM / (MIN_NEIGHBOR_DISTANCE_MM / nominal_distance)
|
|
|
|
|
|
def min_neighbor_distance_for_points(points: list[LedPoint]) -> float:
|
|
return MIN_NEIGHBOR_DISTANCE_MM / coordinate_display_scale(points)
|
|
|
|
|
|
def coordinate_display_scale(points: list[LedPoint]) -> float:
|
|
if not points:
|
|
return 1.0
|
|
explicit_units = {point.coordinate_unit for point in points if point.coordinate_unit != "auto"}
|
|
if explicit_units & {"m", "meter", "meters"}:
|
|
return 1000.0
|
|
if explicit_units & {"mm", "millimeter", "millimeters"}:
|
|
return 1.0
|
|
xs = [point.x for point in points]
|
|
ys = [point.y for point in points]
|
|
span = max(max(xs) - min(xs), max(ys) - min(ys))
|
|
return 1000.0 if span <= METER_LIKE_MAX_SPAN else 1.0
|
|
|
|
|
|
def clockwise_preference(current: LedPoint, candidate: LedPoint) -> int:
|
|
d_row = candidate.row - current.row
|
|
d_col = candidate.col - current.col
|
|
clockwise_directions = [
|
|
(1, 0),
|
|
(1, 1),
|
|
(0, 1),
|
|
(-1, 1),
|
|
(-1, 0),
|
|
(-1, -1),
|
|
(0, -1),
|
|
(1, -1),
|
|
]
|
|
try:
|
|
return clockwise_directions.index((sign(d_row), sign(d_col)))
|
|
except ValueError:
|
|
return len(clockwise_directions)
|
|
|
|
|
|
def same_cell_preference(current: LedPoint, candidate: LedPoint) -> int:
|
|
return 0 if current.row == candidate.row and current.col == candidate.col else 1
|
|
|
|
|
|
def upward_left_preference(current: LedPoint, candidate: LedPoint) -> int:
|
|
if candidate.row < current.row and candidate.col <= current.col:
|
|
return 0
|
|
if candidate.row < current.row:
|
|
return 1
|
|
return 2
|
|
|
|
|
|
def sign(value: int) -> int:
|
|
if value > 0:
|
|
return 1
|
|
if value < 0:
|
|
return -1
|
|
return 0
|